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"

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;
    }
}

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

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

void PhyObject2DWrapper::setStatic(bool s)
{
    phyObject()->setStatic(s);
}

bool PhyObject2DWrapper::getStatic() const
{
    return phyObject()->getStatic();
}

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

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

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

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

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

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

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

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

Box2DWrapper::Box2DWrapper(PhyBox* box, Type type) :
    PhyObject2DWrapper(),
    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;
}

Cylinder2DWrapper::Cylinder2DWrapper(PhyCylinder* cylinder, Type type) :
    PhyObject2DWrapper(),
    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;
    }

    // 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 = m_cylinder->matrix().w_pos + m_cylinder->matrix().x_ax.scale(m_cylinder->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 % m_cylinder->matrix().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(m_cylinder->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;
    }
}

} // end namespace farsa