~robot3d-team/robot3d/trunk

#include "lightSensor.h"

#include <math.h>

#include <osgDB/WriteFile>

 

namespace srCore {

 

LightSensor::LightSensor(dtCore::RefPtr<BodyBase> attachedTo, osg::Vec3 pos, osg::Vec3 orient, float flareAngle, const std::string &name):

        CameraSensorBase(attachedTo, pos, orient, name, "LightSensor", RGB_CAMERA, true)

{

        // 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 = 1;

        this->maxDistance = 100;

        this->resolution  = osg::Vec2(30,30);

        this->cameraType = RGB_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 = "LightSensor";

 

 

        this->sensorReturnValueType = typeid(unsigned long*).name();

 

}

 

void LightSensor::getSensorValue(std::ostream& theStream) const {

 

        theStream << value;

}

 

 

// DO NOT USE THIS FUNCTION DIRECTLY, USE THE STREAM OPERATOR INSTEAD !!!

void LightSensor::getNewSensorValue() {

 

    dynamic_cast<dtCore::Camera*>(sensor.get())->SetClearColor(osg::Vec4(255,255,255,255));

 

        unsigned long lightValue = 0;

 

        // 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++)

                lightValue += image->data()[i];

 

 

        this->value = (int) (lightValue / 2700);

}

 

 

void LightSensor::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);

        */

 

 

        // 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);

 

        *data = value;

        srv.pValue = data;

        // return sensor value

 

        //srv.pValue = &(this->value);

 

        //note: this is a raw sensor value. it might be necessary to normalize the sensor value to fit in a given range.

 

}

 

 

LightSensor::~LightSensor()

{

}

 

}