wheeledexperimenthelper.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 "wheeledexperimenthelper.h"
#include "logger.h"
#include "phybox.h"
#include "phycylinder.h"
#include "robots.h"
#include "arena.h"

namespace farsa {

// This anonymous namespace contains helper functions used in this file
namespace {
    wVector computeWallVertex(PhyBox* wall, unsigned int vertexID)
    {
        const wVector centerOnPlane = wall->matrix().w_pos - wall->matrix().z_ax.scale(wall->sideZ() / 2.0);
        const wVector halfSide1Vector = wall->matrix().x_ax.scale(wall->sideX() / 2.0);
        const wVector halfSide2Vector = wall->matrix().y_ax.scale(wall->sideY() / 2.0);

        wVector vertex;
        switch(vertexID % 4) {
            case 0:
                vertex = centerOnPlane + halfSide1Vector + halfSide2Vector;
                break;
            case 1:
                vertex = centerOnPlane + halfSide1Vector - halfSide2Vector;
                break;
            case 2:
                vertex = centerOnPlane - halfSide1Vector + halfSide2Vector;
                break;
            case 3:
                vertex = centerOnPlane - halfSide1Vector - halfSide2Vector;
                break;
            default:
                break;
        }

        return vertex;
    }


    double getAngleWithXAxis(const wMatrix& mtr, const wVector& v)
    {
        // Normalizing v
        const wVector vdir = v.scale(1.0 / v.norm());

        // To get the angle (unsigned), computing the acos of the dot product of the two vectors
        const double unsignedAngle = acos(mtr.x_ax % vdir);

        // Now choosing the right sign. To do this we first compute the cross product of the x axis and
        // the vertex direction, then we see if it has the same direction of Z or not
        const double s = mtr.z_ax % (mtr.x_ax * vdir);
        return (s < 0.0) ? -unsignedAngle : unsignedAngle;
    }

    void computeLinearViewFieldOccupiedRangeForCylinder(const wMatrix& cameraMtr, const wMatrix& objMtr, double height, double radius, double& minAngle, double& maxAngle, double& distance)
    {
        // The center of the lower base of the cylinder. The local x axis points towards the ground (cylinders
        // are created this way by the arena...)
        const wVector baseCenter = objMtr.w_pos + objMtr.x_ax.scale(height / 2.0);

        // We have to translate the camera to lie on the same plane of the cylinder base. We translate it along
        // its local upvector (Z axis) until it reaches the  plane containing the base of the cylinder. Of course
        // this only works if the camera Z axis is not paraller to the plane with the base of the cylinder. In
        // that case all computations would be invalid, so we don't do anything
        wMatrix mtr = cameraMtr;
        if (fabs(mtr.z_ax % objMtr.x_ax) < 0.0001) {
            distance = -1.0;
            return;
        }
        mtr.w_pos = mtr.w_pos + mtr.z_ax.scale((baseCenter.z - mtr.w_pos.z) / mtr.z_ax.z);

        // First of all we have to calculate the angle between the vector from the center of the
        // camera to the center of the cylinder and the vector from the center of the camera and
        // tangent to the circumference. We know that the tangent is always perpendicular to the
        // radius to the contact point, so we can simply take the asin(radius/distance to center
        // of cylinder)
        const wVector centerDir = baseCenter - mtr.w_pos;
        distance = centerDir.norm();
        const double deltaAngle = asin(radius / distance);

        // Now computing the angle of the cylinder center
        const double centerAngle = getAngleWithXAxis(mtr, centerDir);

        // Finally we can compute the min and max angle
        minAngle = centerAngle - deltaAngle;
        if (minAngle < -PI_GRECO) {
            minAngle += 2.0 * PI_GRECO;
        }
        maxAngle = centerAngle + deltaAngle;
        if (maxAngle > PI_GRECO) {
            maxAngle -= 2.0 * PI_GRECO;
        }
    }

    void computeDistanceAndOrientationFromRobotToCylinder(wVector robotPosition, double robotOrientation, double robotRadius, double radius, wVector position, double& distance, double& angle)
    {
        // Setting to 0.0 the z coordinate of positions
        robotPosition.z = 0.0;
        position.z = 0.0;

        // Computing the distance. We have to remove both the robot radius and the object radius
        distance = (position - robotPosition).norm() - robotRadius - radius;

        // Now computing the angle between the robot and the object
        angle = atan2(position.y - robotPosition.y, position.x - robotPosition.x) - robotOrientation;
    }
}

PhyObject2DWrapper::PhyObject2DWrapper(Arena *arena) :
    m_arena(arena)
{
    // Nothing to do here
}

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

WObject* PhyObject2DWrapper::wObject()
{
    return phyObject();
}

const WObject* PhyObject2DWrapper::wObject() const
{
    return phyObject();
}

void PhyObject2DWrapper::setStatic(bool s)
{
    if (phyObject() != NULL) {
        phyObject()->setStatic(s);
    }
}

bool PhyObject2DWrapper::getStatic() const
{
    if (phyObject() != NULL) {
        return phyObject()->getStatic();
    } else {
        return true;
    }
}

void PhyObject2DWrapper::setPosition(wVector pos)
{
    setPosition(pos.x, pos.y);
}

wVector PhyObject2DWrapper::position() const
{
    return wObject()->matrix().w_pos;
}

void PhyObject2DWrapper::setTexture(QString textureName)
{
    wObject()->setTexture(textureName);
}

QString PhyObject2DWrapper::texture() const
{
    return wObject()->texture();
}

void PhyObject2DWrapper::setColor(QColor color)
{
    wObject()->setColor(color);
}

QColor PhyObject2DWrapper::color() const
{
    return wObject()->color();
}

void PhyObject2DWrapper::setUseColorTextureOfOwner(bool b)
{
    wObject()->setUseColorTextureOfOwner(b);
}

bool PhyObject2DWrapper::useColorTextureOfOwner() const
{
    return wObject()->useColorTextureOfOwner();
}

Box2DWrapper::Box2DWrapper(Arena* arena, PhyBox* box, Type type) :
    PhyObject2DWrapper(arena),
    m_box(box),
    m_vertexes(QVector<wVector>() << computeWallVertex(m_box, 0) << computeWallVertex(m_box, 1) << computeWallVertex(m_box, 2) << computeWallVertex(m_box, 3)),
    m_centerOnPlane(m_box->matrix().w_pos - m_box->matrix().z_ax.scale(m_box->sideZ() / 2.0)),
    m_type(((type != Plane) && (type != Wall) && (type != RectangularTargetArea)) ? Box : type)
{
    if (m_type != Box) {
        m_box->setStatic(true);
    }
}

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

PhyBox* Box2DWrapper::phyObject()
{
    return m_box;
}

const PhyBox* Box2DWrapper::phyObject() const
{
    return m_box;
}

Box2DWrapper::Type Box2DWrapper::type() const
{
    return m_type;
}

void Box2DWrapper::setStatic(bool s)
{
    // Only Boxes can be made non-static
    if (m_type != Box) {
        return;
    }

    phyObject()->setStatic(s);
}

void Box2DWrapper::setPosition(real x, real y)
{
    // Planes and Walls cannot be moved
    if ((m_type == Plane) || (m_type == Wall)) {
        return;
    }

    wVector pos = phyObject()->matrix().w_pos;

    pos.x = x;
    pos.y = y;

    phyObject()->setPosition(pos);

    // We also have to recompute the vertexes and center
    for (unsigned int i = 0; i < 4; i++) {
        m_vertexes[i] = computeWallVertex(m_box, i);
    }
    m_centerOnPlane = m_box->matrix().w_pos - m_box->matrix().z_ax.scale(m_box->sideZ() / 2.0);
}

void Box2DWrapper::computeLinearViewFieldOccupiedRange(const wMatrix& cameraMtr, double& minAngle, double& maxAngle, double& distance) const
{
    // If this is a Plane or a RectangularTargetArea, we simply return a negative distance (they are not visible with
    // a linear camera)
    if ((m_type == Plane) || (m_type == RectangularTargetArea)) {
        distance = -1.0;
        return;
    }

    // We have to translate the camera to lie on the same plane of the vertex. We translate it along
    // its local upvector (Z axis) until it reaches the  plane containing the base of the wall. Of course
    // this only works if the camera Z axis is not paraller to the plane with the base of the wall. In
    // that case all computations would be invalid, so we don't do anything
    wMatrix mtr = cameraMtr;
    if (fabs(mtr.z_ax % m_box->matrix().z_ax) < 0.0001) {
        distance = -1.0;
        return;
    }
    mtr.w_pos = mtr.w_pos + mtr.z_ax.scale((m_centerOnPlane.z - mtr.w_pos.z) / mtr.z_ax.z);

    // First of all computing the angle for every vertex
    QVector<double> angles(4);

    for (int i = 0; i < 4; i++) {
        // Computing the vector giving the direction to the vertex
        const wVector vdir = m_vertexes[i] - mtr.w_pos;

        // Now computing the angle
        angles[i] = getAngleWithXAxis(mtr, vdir);
    }

    // Now finding the min and max angle (their indexes). We have to take into account the fact that the
    // angle with the minimum value could be the upper limit and viceversa because the object could be
    // behind the camera. However we know that, as the camera is outside the wall, the maximum possible
    // angular sector of the view filed occupied by the wall is 180°. This means that also the angular
    // distance of one vertex with the center of the wall must be less than 180°. So, if we compute this
    // distance and get a value greater than 180°, we have to take (360° - computed_angular_distance)
    // and invert min with max.
    const wVector centerDir = m_centerOnPlane - mtr.w_pos;
    const double centerAngle = getAngleWithXAxis(mtr, centerDir);
    int minAngleID = 0;
    int maxAngleID = 0;

    // These two are the angular distances of the current min and max angles from the center. Their initial
    // value is the lowest possible
    double minDelta = 0.0;
    double maxDelta = 0.0;

    for (int i = 0; i < 4; i++) {
        const double curDelta = fabs(angles[i] - centerAngle);

        // Checking if the vertex and the center are behind the camera
        if (curDelta > PI_GRECO) {
            const double actualDelta = (2.0 * PI_GRECO) - curDelta;
            if (angles[i] > centerAngle) {
                // This is a candidate minimum angle
                if (actualDelta > minDelta) {
                    minAngleID = i;
                    minDelta = actualDelta;
                }
            } else {
                // This is a candidate maximum angle
                if (actualDelta > maxDelta) {
                    maxAngleID = i;
                    maxDelta = actualDelta;
                }
            }
        } else {
            if (angles[i] < centerAngle) {
                // This is a candidate minimum angle
                if (curDelta > minDelta) {
                    minAngleID = i;
                    minDelta = curDelta;
                }
            } else {
                // This is a candidate maximum angle
                if (curDelta > maxDelta) {
                    maxAngleID = i;
                    maxDelta = curDelta;
                }
            }
        }
    }

    // Filling the minAngle and maxAngle parameters
    minAngle = angles[minAngleID];
    maxAngle = angles[maxAngleID];

#ifdef __GNUC__
    #warning QUESTO MODO DI CALCOLARE LA DISTANZA È SBAGLIATO (E NON NE CAPISCO IL SENSO), MA È QUELLO USATO IN EVOROBOT, QUINDI PER IL MOMENTO LO USO (ANCHE PERCHÉ USARE LA DISTANZA PER L OCCLUSIONE NON VA BENE COMUNQUE)
#endif
    // Now computing distance. This way of calculating the distance is plainly wrong (and I can't
    // see why it is written this way), but it is the method used by Evorobot, so for the moment
    // using it (moreover using distance for occlusion is not correct)
    distance = ((mtr.w_pos - m_vertexes[minAngleID]).norm() + (mtr.w_pos - m_vertexes[maxAngleID]).norm()) / 2.0;
}

bool Box2DWrapper::computeDistanceAndOrientationFromRobot(const WheeledRobot2DWrapper& robot, double& distance, double& angle) const
{
#ifdef __GNUC__
    #warning I CALCOLI PER DISTANZA E ORIENTAMENTO IN EVOROBOT SONO A DIR POCO FANTASIOSI, QUI HO CERCATO DI FARE QUALCOSA DI PIÙ SENSATO...
#endif
    // Only doing computations for walls and boxes
    if ((m_type != Wall) && (m_type != Box)) {
        return false;
    }

    // Taking the robot position and setting z to lie on the same plane of the vertexes
    const wVector robotPosition(robot.position().x, robot.position().y, m_vertexes[0].z);

    // Now computing the robot position in the box frame of reference
    const wVector relRobotPosition = m_box->matrix().untransformVector(robotPosition);

    // Now we can find the point in the rectangle that is nearest to the robot position. As we work in the box
    // frame of reference, the vertex are easy to compute. They are (discarding z):
    //  (+m_box->sideX() / 2.0, +m_box->sideY() / 2.0)
    //  (+m_box->sideX() / 2.0, -m_box->sideY() / 2.0)
    //  (-m_box->sideX() / 2.0, +m_box->sideY() / 2.0)
    //  (-m_box->sideX() / 2.0, -m_box->sideY() / 2.0)
    // Finding the nearest point is just a matter of separately computing x and y. Note that if the robot is inside
    // the box, the distance will be 0.0 and will become negative once we subtract the robot radius
    real nearestX;
    if (relRobotPosition.x < -m_box->sideX() / 2.0) {
        nearestX = -m_box->sideX() / 2.0;
    } else if (relRobotPosition.x > +m_box->sideX() / 2.0) {
        nearestX = +m_box->sideX() / 2.0;
    } else {
        nearestX = relRobotPosition.x;
    }
    real nearestY;
    if (relRobotPosition.y < -m_box->sideY() / 2.0) {
        nearestY = -m_box->sideY() / 2.0;
    } else if (relRobotPosition.y > +m_box->sideY() / 2.0) {
        nearestY = +m_box->sideY() / 2.0;
    } else {
        nearestY = relRobotPosition.y;
    }

    // Although distance is independent of the frame of reference, we convert the nearest point to the global frame
    // of reference because we only have the robot orientation in that frame
    const wVector nearestPoint = m_box->matrix().transformVector(wVector(nearestX, nearestY, relRobotPosition.z));

    // Now we can easily compute the distance and orientation. For the distance we have to remove the robot radius
    distance = (nearestPoint - robotPosition).norm() - robot.getRadius();
    const real robotOrientation = (dynamic_cast<const RobotOnPlane*>(robot.wObject()))->orientation(m_arena->getPlane());
    angle = atan2(nearestPoint.y - robotPosition.y, nearestPoint.x - robotPosition.x) - robotOrientation;

    return true;
}

Cylinder2DWrapper::Cylinder2DWrapper(Arena* arena, PhyCylinder* cylinder, Type type) :
    PhyObject2DWrapper(arena),
    m_cylinder(cylinder),
    m_type(((type != SmallCylinder) && (type != BigCylinder) && (type != CircularTargetArea)) ? Cylinder : type)
{
    if (m_type == CircularTargetArea) {
        m_cylinder->setStatic(true);
    }
}

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

PhyCylinder* Cylinder2DWrapper::phyObject()
{
    return m_cylinder;
}

const PhyCylinder* Cylinder2DWrapper::phyObject() const
{
    return m_cylinder;
}

void Cylinder2DWrapper::setStatic(bool s)
{
    // CircularTargetArea cannot be made non-static
    if (m_type == CircularTargetArea) {
        return;
    }

    phyObject()->setStatic(s);
}

Cylinder2DWrapper::Type Cylinder2DWrapper::type() const
{
    return m_type;
}

void Cylinder2DWrapper::setPosition(real x, real y)
{
    wVector pos = phyObject()->matrix().w_pos;

    pos.x = x;
    pos.y = y;

    phyObject()->setPosition(pos);
}

void Cylinder2DWrapper::computeLinearViewFieldOccupiedRange(const wMatrix& cameraMtr, double& minAngle, double& maxAngle, double& distance) const
{
    // If this is a CircularTargetArea, we simply return a negative distance (it is not visible with
    // a linear camera)
    if (m_type == CircularTargetArea) {
        distance = -1.0;
        return;
    }

    computeLinearViewFieldOccupiedRangeForCylinder(cameraMtr, m_cylinder->matrix(), m_cylinder->height(), m_cylinder->radius(), minAngle, maxAngle, distance);
}

bool Cylinder2DWrapper::computeDistanceAndOrientationFromRobot(const WheeledRobot2DWrapper& robot, double& distance, double& angle) const
{
    // If this is a CircularTargetArea, we simply return a negative distance
    if (m_type == CircularTargetArea) {
        return false;
    }

    const double robotOrientation = (dynamic_cast<const RobotOnPlane*>(robot.wObject()))->orientation(m_arena->getPlane());
    computeDistanceAndOrientationFromRobotToCylinder(robot.position(), robotOrientation, robot.getRadius(), m_cylinder->radius(), m_cylinder->matrix().w_pos, distance, angle);

    return true;
}

WheeledRobot2DWrapper::WheeledRobot2DWrapper(Arena* arena, RobotOnPlane* robot, double height, double radius) :
    PhyObject2DWrapper(arena),
    m_robot(robot),
    m_height(height),
    m_radius(radius),
    m_previousMatrix((dynamic_cast<WObject*>(robot))->matrix())
{
}

WheeledRobot2DWrapper::~WheeledRobot2DWrapper()
{
}

RobotOnPlane* WheeledRobot2DWrapper::robotOnPlane()
{
    return m_robot;
}

const RobotOnPlane* WheeledRobot2DWrapper::robotOnPlane() const
{
    return m_robot;
}

WObject* WheeledRobot2DWrapper::wObject()
{
    return dynamic_cast<WObject*>(m_robot);
}

const WObject* WheeledRobot2DWrapper::wObject() const
{
    return dynamic_cast<const WObject*>(m_robot);
}

PhyObject* WheeledRobot2DWrapper::phyObject()
{
    return NULL;
}

const PhyObject* WheeledRobot2DWrapper::phyObject() const
{
    return NULL;
}

WheeledRobot2DWrapper::Type WheeledRobot2DWrapper::type() const
{
    return WheeledRobot;
}

void WheeledRobot2DWrapper::setPosition(real x, real y)
{
    wVector pos = wObject()->matrix().w_pos;

    pos.x = x;
    pos.y = y;

    wObject()->setPosition(pos);
}

void WheeledRobot2DWrapper::computeLinearViewFieldOccupiedRange(const wMatrix& cameraMtr, double& minAngle, double& maxAngle, double& distance) const
{
    wMatrix mtr = wObject()->matrix();

    mtr.w_pos.z += m_height / 2.0;

    computeLinearViewFieldOccupiedRangeForCylinder(cameraMtr, mtr, m_height, m_radius, minAngle, maxAngle, distance);
}

bool WheeledRobot2DWrapper::computeDistanceAndOrientationFromRobot(const WheeledRobot2DWrapper& robot, double& distance, double& angle) const
{
    if (this == &robot) {
        return false;
    }

    const double robotOrientation = (dynamic_cast<const RobotOnPlane*>(robot.wObject()))->orientation(m_arena->getPlane());
    computeDistanceAndOrientationFromRobotToCylinder(robot.position(), robotOrientation, robot.getRadius(), m_radius, position(), distance, angle);

    return true;
}

wVector positionOnPlane(const Box2DWrapper* plane, real x, real y)
{
    wVector pos = plane->position();

    pos.x = x;
    pos.y = y;
    pos.z += plane->phyObject()->sideZ() / 2.0f;

    return pos;
}

void orientationOnPlane(const Box2DWrapper* plane, real angle, wMatrix& mtr)
{
    wMatrix rotatedMtr = plane->phyObject()->matrix();

    // Now rotating the matrix around the Z axis
    rotatedMtr = rotatedMtr.rotateAround(rotatedMtr.z_ax, rotatedMtr.w_pos, angle);

    // Setting the position of the rotated matrix to be the same as the original one
    rotatedMtr.w_pos = mtr.w_pos;

    // Now overwriting the matrix
    mtr = rotatedMtr;
}

real angleBetweenXAxes(const wMatrix& mtr1, const wMatrix& mtr2)
{
    // Taking the two x axes. We can take the x axis of mtr1 as is, while we need to project the X axis
    // of mtr2 onto the XY plane of mtr1. To do so we simply project the x axis of mtr2 on the z axis of
    // mtr1 and subtract this vector from the original x axis of mtr2
    const wVector& x1 = mtr1.x_ax;
    const wVector x2 = mtr2.x_ax - mtr1.z_ax.scale(mtr2.x_ax % mtr1.z_ax);

    // Now normalizing both axes
    const wVector normX1 = x1.scale(1.0 / x1.norm());
    const wVector normX2 = x2.scale(1.0 / x2.norm());

    // To get the angle (unsigned), computing the acos of the dot product of the two vectors
    const double unsignedAngle = acos(normX1 % normX2);

    // Now choosing the right sign. To do this we first compute the cross product of the two x axes and
    // then we see if it has the same direction of the z axis of the first matrix or not
    const double s = mtr1.z_ax % (normX1 * normX2);
    return (s < 0.0) ? -unsignedAngle : unsignedAngle;
}

} // end namespace farsa