1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
|
/*
* distanceSensor.cpp
*
* Created on: Dec 29, 2008
* Author: winkler
*/
#include "distanceSensor.h"
#include <math.h>
#include <osgDB/WriteFile>
namespace srCore {
DistanceSensor::DistanceSensor(dtCore::RefPtr<BodyBase> attachedTo, osg::Vec3 pos, osg::Vec3 orient, float flareAngle, const std::string &name):
CameraSensorBase(attachedTo, pos, orient, name, "DistanceSensor", DEPTH_CAMERA, true),
value (0)
{
// setup light sensor
// light sensor actually consists of a grid of 30x30 pixels. These values
// will be added to get a more accurate sensor value.
this->minDistance = 0.3;
this->maxDistance = 100;
this->resolution = osg::Vec2(30,30);
this->cameraType = DEPTH_CAMERA;
if ((flareAngle>0) && (flareAngle != M_PI/4) ) {
this->frustumDown = tan(flareAngle/2) * this->minDistance;
this->frustumUp = tan(flareAngle/2) * this->minDistance;
this->frustumRight = tan(flareAngle/2) * this->minDistance;
this->frustumLeft = tan(flareAngle/2) * this->minDistance;
}
//this->init(attachedTo, pos, orient);
this->setup();
this->type = "DistanceSensor";
this->sensorReturnValueType = typeid(unsigned long*).name();
}
void DistanceSensor::getSensorValue(std::ostream& theStream) const {
theStream << value;
}
//std::ostream& operator<< (std::ostream& theStream, DistanceSensor& distSensor)
//{
// distSensor.blub();
// std::cout << " blub " << std::endl;
// theStream << distSensor.value;//__getSensorValue__();
// return theStream;
//
//}
void DistanceSensor::evaluate(ParameterReturnValue srv)
{
/*
osg::Image* image = new osg::Image;
int x = static_cast<int>(this->camera->GetOSGCamera()->getViewport()->x());
int y = static_cast<int>(this->camera->GetOSGCamera()->getViewport()->y());
unsigned int width = static_cast<unsigned int>(this->camera->GetOSGCamera()->getViewport()->width());
unsigned int height = static_cast<unsigned int>(this->camera->GetOSGCamera()->getViewport()->height());
printf("%s: Viewport x: %d Viewport y: %d Höhe: %d, Breite: %d \n", this->camera->GetName().c_str(), x, y, height, width);
image->allocateImage(width, height, 1, GL_RGB, GL_UNSIGNED_BYTE);
image->readPixels(x, y, width, height, GL_RGB, GL_UNSIGNED_BYTE);
*/
//this->value = __getSensorValue__();
// sensor value type check
if (srv.type != typeid(unsigned long*).name())
printf("Parameter type needs to be unsigned long*. Casting could lead to errors. \n");
// cast void* to unsigned long int*
unsigned long* data = reinterpret_cast<unsigned long*> (srv.pValue);
// return sensor value
*data = this->value;
srv.pValue = data;
//note: this is a raw sensor value. it might be necessary to normalize the sensor value to fit in a given range.
}
void DistanceSensor::getNewSensorValue()
{
dynamic_cast<dtCore::Camera*>(sensor.get())->SetClearColor(osg::Vec4(255,255,255,255));
unsigned long distValue = 0xFFFFFF;
// get raw image data
osg::ref_ptr<osg::Image> image = getRawData();
// add red, green and blue values for all pixels
//for (unsigned int i=0; i<image->getImageSizeInBytes(); i += 4) {
if (image->valid()) {
unsigned int i = ((image->getImageSizeInBytes()) >> 1) & ~(0x3);
unsigned long depthValue = (image->data()[i+2] << 16) | (image->data()[i+1] << 8) | (image->data()[i]);
//unsigned int depthValue = 5;
if (distValue > depthValue)
distValue = depthValue;
//}
value = distValue;
}
}
DistanceSensor::~DistanceSensor()
{
}
}
|