~robot3d-team/robot3d/trunk

/*

 * 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()

{

}

 

}