sensorcontrollers.cpp

/********************************************************************************
 *  FARSA Experiments Library                                                   *
 *  Copyright (C) 2007-2012                                                     *
 *  Gianluca Massera <emmegian@yahoo.it>                                        *
 *  Stefano Nolfi <stefano.nolfi@istc.cnr.it>                                   *
 *  Tomassino Ferrauto <tomassino.ferrauto@istc.cnr.it>                         *
 *  Onofrio Gigliotta <onofrio.gigliotta@istc.cnr.it>                           *
 *                                                                              *
 *  This program is free software; you can redistribute it and/or modify        *
 *  it under the terms of the GNU General Public License as published by        *
 *  the Free Software Foundation; either version 2 of the License, or           *
 *  (at your option) any later version.                                         *
 *                                                                              *
 *  This program is distributed in the hope that it will be useful,             *
 *  but WITHOUT ANY WARRANTY; without even the implied warranty of              *
 *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the               *
 *  GNU General Public License for more details.                                *
 *                                                                              *
 *  You should have received a copy of the GNU General Public License           *
 *  along with this program; if not, write to the Free Software                 *
 *  Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA  *
 ********************************************************************************/

#include "sensorcontrollers.h"
#include "mathutils.h"
#include "phyobject.h"
#include "graphicalwobject.h"

namespace farsa {

SensorController::SensorController(World* world) :
    Ownable(),
    m_world(world),
    m_enabled(true)
{
}

SensorController::~SensorController()
{
    // Nothing to do here
}

IRSensorController::IRSensorController(World* world, unsigned int numSensors) :
    SensorController(world),
    m_activations(numSensors),
    m_activeSensor(numSensors, true)
{
}

IRSensorController::~IRSensorController()
{
    // Nothing to do here
}

void IRSensorController::setSensorActive(int i, bool active)
{
    m_activeSensor[i] = active;
}

namespace __SingleIR_internal {
    #ifndef GLMultMatrix
    #define GLMultMatrix glMultMatrixf
    // for double glMultMatrixd
    #endif

    const float sensorCubeSide = 0.005f;

    class SingleIRGraphic : public GraphicalWObject
    {
    public:
        SingleIRGraphic(WObject *object, const wMatrix& offset, const QVector<wVector>& startingRayPoints, const QVector<wVector>& endingRayPoints, bool drawRay, bool drawRealRay, QString name = "unamed") :
            GraphicalWObject(object->world(), name),
            m_object(object),
            m_offset(offset),
            m_startingRayPoints(startingRayPoints),
            m_endingRayPoints(endingRayPoints),
            m_drawRay(drawRay),
            m_drawRealRay(drawRealRay)
        {
            // Attaching to handPiece (which also becomes our owner). We don't
            // pass the offset matrix to attachToObject() because we need to draw stuffs
            // both in our frame of reference and in the frame of reference of the object
            // to which we are attached (we manage the offset ourself)
            attachToObject(m_object, true);

            // We also use our own color and texture
            setUseColorTextureOfOwner(false);
            setTexture("");
            setColor(Qt::black);
        }

        ~SingleIRGraphic()
        {
        }

    protected:
        virtual void render(RenderWObject* renderer, QGLContext* gw)
        {
            // Pushing the transformation matrices separately because the box needs both of them
            // while rays doesn't need the offset (they are already in the solid frame of reference)
            // Bringing the coordinate system on the sensor
            wMatrix mtr = tm * m_offset;

            glPushMatrix();
            GLMultMatrix(&tm[0][0]);

            glPushMatrix();
            GLMultMatrix(&m_offset[0][0]);

            renderer->container()->setupColorTexture(gw, renderer);

            // First drawing the cube representing the sensor. The cube will just be drawn as
            // six quads for the sake of simplicity. For each face, we specify the quad normal
            // (for lighting), then specify the quad's 4 vertices. The top part of the front
            // face is drawn in green to understand if the sensor is mounted upside-down
            glBegin(GL_QUADS);
            const float hside = sensorCubeSide / 2.0;

            // front (top part)
            glColor3f(0.0, 1.0, 0.0);
            glNormal3f(0.0, 0.0, 1.0);
            glVertex3f(-hside,    0.0,  hside);
            glVertex3f( hside,    0.0,  hside);
            glVertex3f( hside,  hside,  hside);
            glVertex3f(-hside,  hside,  hside);

            // front (bottom part)
            glColor3f(0.0, 0.0, 0.0);
            glNormal3f(0.0, 0.0, 1.0);
            glVertex3f(-hside, -hside,  hside);
            glVertex3f( hside, -hside,  hside);
            glVertex3f( hside,    0.0,  hside);
            glVertex3f(-hside,    0.0,  hside);

            // back
            glNormal3f(0.0, 0.0, -1.0);
            glVertex3f( hside, -hside, -hside);
            glVertex3f(-hside, -hside, -hside);
            glVertex3f(-hside,  hside, -hside);
            glVertex3f( hside,  hside, -hside);

            // top
            glNormal3f(0.0, 1.0, 0.0);
            glVertex3f(-hside,  hside,  hside);
            glVertex3f( hside,  hside,  hside);
            glVertex3f( hside,  hside, -hside);
            glVertex3f(-hside,  hside, -hside);

            // bottom
            glNormal3f(0.0, -1.0, 0.0);
            glVertex3f(-hside, -hside, -hside);
            glVertex3f( hside, -hside, -hside);
            glVertex3f( hside, -hside,  hside);
            glVertex3f(-hside, -hside,  hside);

            // right
            glNormal3f(-1.0, 0.0, 0.0);
            glVertex3f(-hside, -hside, -hside);
            glVertex3f(-hside, -hside,  hside);
            glVertex3f(-hside,  hside,  hside);
            glVertex3f(-hside,  hside, -hside);

            // left
            glNormal3f(1.0, 0.0, 0.0);
            glVertex3f( hside, -hside,  hside);
            glVertex3f( hside, -hside, -hside);
            glVertex3f( hside,  hside, -hside);
            glVertex3f( hside,  hside,  hside);

            glEnd();

            // Popping only one matrix because ray need the other one
            glPopMatrix();

            // Disabling lighting here (we want pure red lines no matter from where we look at them)
            glPushAttrib(GL_LIGHTING_BIT);
            glDisable(GL_LIGHTING);

            // Now drawing the ray if we have to
            if (m_drawRay) {
                glLineWidth(2.5);
                glColor3f(1.0, 0.0, 0.0);

                glBegin(GL_LINES);
                for (int i = 0; i < m_startingRayPoints.size(); i++) {
                    const wVector& s = m_startingRayPoints[i];
                    const wVector& e = m_endingRayPoints[i];
                    if (m_drawRealRay) {
                        glVertex3f(s.x, s.y, s.z);
                        glVertex3f(e.x, e.y, e.z);
                    } else {
                        const wVector d = (e - s).normalize().scale(sensorCubeSide * 5.0);
                        glVertex3f(m_offset.w_pos.x, m_offset.w_pos.y, m_offset.w_pos.z);
                        glVertex3f(m_offset.w_pos.x + d.x, m_offset.w_pos.y + d.y, m_offset.w_pos.z + d.z);
                    }
                }
                glEnd();
            }

            // Restoring lighting status
            glPopAttrib();

            glPopMatrix();
        }

        WObject* const m_object;

        const wMatrix m_offset;

        const QVector<wVector> m_startingRayPoints;

        const QVector<wVector> m_endingRayPoints;

        const bool m_drawRay;

        const bool m_drawRealRay;
    };
}

using namespace __SingleIR_internal;

SingleIR::SingleIR() :
    m_object(NULL),
    m_transformation(wMatrix::identity()),
    m_minDist(0.0),
    m_maxDist(0.0),
    m_aperture(0.0),
    m_numRays(0),
    m_drawSensor(false),
    m_drawRay(false),
    m_drawRealRay(false),
    m_startingRayPoints(),
    m_endingRayPoints(),
    m_rayCastHit(),
    m_sensorGraphics(NULL)
{
    m_rayCastHit.object = NULL;
    m_rayCastHit.distance = 1.0;
}

SingleIR::SingleIR(WObject* obj, wMatrix mtr, double minDist, double maxDist, double aperture, unsigned int numRays) :
    m_object(obj),
    m_transformation(mtr),
    m_minDist(minDist),
    m_maxDist(maxDist),
    m_aperture(aperture),
    m_numRays(numRays),
    m_drawSensor(false),
    m_drawRay(false),
    m_drawRealRay(false),
    m_startingRayPoints(),
    m_endingRayPoints(),
    m_rayCastHit(),
    m_sensorGraphics(NULL)
{
    m_rayCastHit.object = NULL;
    m_rayCastHit.distance = 1.0;

    if (m_numRays != 0) {
        m_startingRayPoints.resize(m_numRays);
        m_endingRayPoints.resize(m_numRays);
    }

    // computing the starting and ending point of rays and updating the graphical representation
    computeRayPoints();
    updateGraphicalRepresentation();
}

SingleIR::SingleIR(const SingleIR& other) :
    m_object(other.m_object),
    m_transformation(other.m_transformation),
    m_minDist(other.m_minDist),
    m_maxDist(other.m_maxDist),
    m_aperture(other.m_aperture),
    m_numRays(other.m_numRays),
    m_drawSensor(other.m_drawSensor),
    m_drawRay(other.m_drawRay),
    m_drawRealRay(other.m_drawRealRay),
    m_startingRayPoints(other.m_startingRayPoints),
    m_endingRayPoints(other.m_endingRayPoints),
    m_rayCastHit(other.m_rayCastHit),
    m_sensorGraphics(NULL)
{
    updateGraphicalRepresentation();
}

SingleIR& SingleIR::operator=(const SingleIR& other)
{
    if (&other == this) {
        return *this;
    }

    // Copying data members
    m_object = other.m_object;
    m_transformation = other.m_transformation;
    m_minDist = other.m_minDist;
    m_maxDist = other.m_maxDist;
    m_aperture = other.m_aperture;
    m_numRays = other.m_numRays;
    m_drawSensor = other.m_drawSensor;
    m_drawRay = other.m_drawRay;
    m_drawRealRay = other.m_drawRealRay;
    m_startingRayPoints = other.m_startingRayPoints;
    m_endingRayPoints = other.m_endingRayPoints;
    m_rayCastHit = other.m_rayCastHit;

    updateGraphicalRepresentation();

    return *this;
}

SingleIR::~SingleIR()
{
    // Nothing to do here
}

void SingleIR::update()
{
    // Checking if the sensor is valid
    if (!isValid()) {
        return;
    }

    // Resetting the current RayCastHit object
    m_rayCastHit.object = NULL;
    m_rayCastHit.distance = 1.0;

    // If we are attached to a phyobject, we ignore it in collisions
    QSet<PhyObject*> ignoredObjs;
    PhyObject* phyObj = dynamic_cast<PhyObject*>(m_object);
    if (phyObj != NULL) {
        ignoredObjs.insert(phyObj);
    }

    // The minimum distance (distances range from 0.0 to 1.0)
    double minDist = 2.0;
    for (unsigned int i = 0; i < m_numRays; i++) {
        // Computing the start and end point of the ray in the global frame of reference
        const wVector start = m_object->matrix().transformVector(m_startingRayPoints[i]);
        const wVector end = m_object->matrix().transformVector(m_endingRayPoints[i]);

        // Casting the ray
        const rayCastHitVector v = m_object->world()->worldRayCast(start, end, true, ignoredObjs);

        // Taking the lowest distance: this sensor only reports the distance of the closest
        // object
        if ((v.size() != 0) && (v[0].distance < minDist)) {
            minDist = v[0].distance;
            m_rayCastHit = v[0];
        }
    }
}

void SingleIR::set(WObject* obj, wMatrix mtr, double minDist, double maxDist, double aperture, unsigned int numRays)
{
    // First of all saving the parameters
    m_object = obj;
    m_transformation = mtr;
    m_minDist = minDist;
    m_maxDist = maxDist;
    m_aperture = aperture;
    m_numRays = numRays;
    if (m_numRays != 0) {
        m_startingRayPoints.resize(m_numRays);
        m_endingRayPoints.resize(m_numRays);
    } else {
        m_startingRayPoints.clear();
        m_endingRayPoints.clear();
    }
    m_rayCastHit.object = NULL;
    m_rayCastHit.distance = 1.0;

    computeRayPoints();
    updateGraphicalRepresentation();
}

void SingleIR::setGraphicalProperties(bool drawSensor, bool drawRay, bool drawRealRay)
{
    m_drawSensor = drawSensor;
    m_drawRay = drawRay;
    m_drawRealRay = drawRealRay;

    updateGraphicalRepresentation();
}

void SingleIR::computeRayPoints()
{
    // Checking the sensor is valid
    if (!isValid()) {
        return;
    }

    // First of all we need to compute the starting angle of the rays and the angular distance
    // between them. If there is only one ray it is at angle 0.0 (i.e. along the local Z axis).
    // The angles computed here are in radiants.
    const double startAngle = (m_numRays == 1) ? 0.0 : -(deg2rad(m_aperture) / 2.0);
    const double angularIncrement = (m_numRays == 1) ? 0.0 : (deg2rad(m_aperture) / double(m_numRays - 1));

    // Now computing the angles in the object fame of reference
    for (unsigned int i = 0; i < m_numRays; i++) {
        const double curAngle = startAngle + i * angularIncrement;

        // To get the ray in the sensor frame of reference we have to rotate the Z axis
        // around the Y axis. Then we compute the starting and ending point
        const wVector localRay = wVector::Z().rotateAround(wVector::Y(), curAngle);
        const wVector localStart = localRay.scale(m_minDist);
        const wVector localEnd = localRay.scale(m_maxDist);

        // Finally we can compute the start and end in the object frame of reference
        m_startingRayPoints[i] = m_transformation.transformVector(localStart);
        m_endingRayPoints[i] = m_transformation.transformVector(localEnd);
    }
}

void SingleIR::updateGraphicalRepresentation()
{
    delete m_sensorGraphics;
    m_sensorGraphics = NULL;

    // Checking the sensor is valid
    if (m_drawSensor && isValid()) {
        // creating the graphical representation of the sensor
        m_sensorGraphics = new SingleIRGraphic(m_object, m_transformation, m_startingRayPoints, m_endingRayPoints, m_drawRay, m_drawRealRay);
    }
}

SimulatedIRProximitySensorController::SimulatedIRProximitySensorController(World* world, const QVector<SingleIR>& sensors) :
    IRSensorController(world, sensors.size()),
    m_sensors(sensors),
    m_drawSensor(false),
    m_drawRay(false),
    m_drawRealRay(false)
{
    setGraphicalProperties(m_drawSensor, m_drawRay, m_drawRealRay);
}

SimulatedIRProximitySensorController::~SimulatedIRProximitySensorController()
{
    // Nothing to do here
}

void SimulatedIRProximitySensorController::update()
{
    for (int i = 0; i < m_sensors.size(); i++) {
        if (m_activeSensor[i]) {
            m_sensors[i].update();
            // If there was no hit, distance is 1.0 and activation 0.0
            m_activations[i] = 1.0 - m_sensors[i].getRayCastHit().distance;
        } else {
            m_activations[i] = 0.0;
        }
    }
}

void SimulatedIRProximitySensorController::setSensorActive(int i, bool active)
{
    // Calling parent function
    IRSensorController::setSensorActive(i, active);

    if (active) {
        m_sensors[i].setGraphicalProperties(m_drawSensor, m_drawRay, m_drawRealRay);
    } else {
        m_sensors[i].setGraphicalProperties(false, m_drawRay, m_drawRealRay);
    }
}

void SimulatedIRProximitySensorController::setGraphicalProperties(bool drawSensor, bool drawRay, bool drawRealRay)
{
    m_drawSensor = drawSensor;
    m_drawRay = drawRay;
    m_drawRealRay = drawRealRay;

    for (int i = 0; i < m_sensors.size(); i++) {
        m_sensors[i].setGraphicalProperties(m_activeSensor[i] && drawSensor, drawRay, drawRealRay);
    }
}

SimulatedIRGroundSensorController::SimulatedIRGroundSensorController(World* world, const QVector<SingleIR>& sensors) :
    IRSensorController(world, sensors.size()),
    m_sensors(sensors),
    m_drawSensor(false),
    m_drawRay(false),
    m_drawRealRay(false)
{
    setGraphicalProperties(m_drawSensor, m_drawRay, m_drawRealRay);
}

SimulatedIRGroundSensorController::~SimulatedIRGroundSensorController()
{
    // Nothing to do here
}

void SimulatedIRGroundSensorController::update()
{
    for (int i = 0; i < m_sensors.size(); i++) {
        if (m_activeSensor[i]) {
            m_sensors[i].update();

            // Now taking the color of the nearest solid, converting to HSL and taking
            // normalized lightness as activation
            if (m_sensors[i].getRayCastHit().object != NULL) {
                m_activations[i] = m_sensors[i].getRayCastHit().object->color().lightnessF();
            } else {
                m_activations[i] = 0.0;
            }
        } else {
            m_activations[i] = 0.0;
        }
    }
}

void SimulatedIRGroundSensorController::setSensorActive(int i, bool active)
{
    // Calling parent function
    IRSensorController::setSensorActive(i, active);

    if (active) {
        m_sensors[i].setGraphicalProperties(m_drawSensor, m_drawRay, m_drawRealRay);
    } else {
        m_sensors[i].setGraphicalProperties(false, m_drawRay, m_drawRealRay);
    }
}

void SimulatedIRGroundSensorController::setGraphicalProperties(bool drawSensor, bool drawRay, bool drawRealRay)
{
    m_drawSensor = drawSensor;
    m_drawRay = drawRay;
    m_drawRealRay = drawRealRay;

    for (int i = 0; i < m_sensors.size(); i++) {
        m_sensors[i].setGraphicalProperties(m_activeSensor[i] && drawSensor, drawRay, drawRealRay);
    }
}

TractionSensorController::TractionSensorController(World* world, const PhyJoint& j) :
    SensorController(world),
    m_joint(j)
{
}

TractionSensorController::~TractionSensorController()
{
}

void TractionSensorController::update()
{
    // Nothing to do here, we read the traction force directly in the traction() function
}

wVector TractionSensorController::traction() const
{
    return -m_joint.getForceOnJoint();
}

} // end namespace farsa