arena.cpp

/********************************************************************************
 *  FARSA Experiments Library                                                   *
 *  Copyright (C) 2007-2012                                                     *
 *  Stefano Nolfi <stefano.nolfi@istc.cnr.it>                                   *
 *  Onofrio Gigliotta <onofrio.gigliotta@istc.cnr.it>                           *
 *  Gianluca Massera <emmegian@yahoo.it>                                        *
 *  Tomassino Ferrauto <tomassino.ferrauto@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 "arena.h"
#include "configurationhelper.h"
#include "phybox.h"
#include "logger.h"
#include "robots.h"

namespace farsa {

// This anonymous namespace contains some constants used throughout the code
#ifdef __GNUC__
    #warning QUESTE COSTANTI DEVONO PROBABILMENTE DIVENTARE DEI PARAMETRI
#endif
namespace {
    const real defaultHeight = 0.3f;

    const real planeThickness = 0.1;

    const real smallCylinderRadius = 0.03;

    const real bigCylinderRadius = 0.06;

    const real targetAreasHeight = 0.001;

    const real targetAreasProtrusion = 0.0005;
}

Arena::Arena(ConfigurationParameters& params, QString prefix) :
    ParameterSettableInConstructor(params, prefix),
    ConcurrentResourcesUser(),
    m_z(ConfigurationHelper::getDouble(params, prefix + "z", 0.0)),
    m_objects2DList(QVector<PhyObject2DWrapper*>()),
    m_plane(createPlane(params, prefix, m_z, this)),
    m_robotResourceWrappers(),
    m_world(NULL)
{
    // The list of resources we observe here
    usableResources(QStringList() << "world");
}

Arena::~Arena()
{
    // Removing all wrappers
    for (int i = 0; i < m_objects2DList.size(); i++) {
        delete m_objects2DList[i];
    }
}

void Arena::save(ConfigurationParameters&, QString)
{
    Logger::error("NOT IMPLEMENTED (Arena::save)");
    abort();
}

void Arena::describe(QString type)
{
    // The parent class is ParameterSettableInConstructor, no need to call
    // its describe function (which does nothing)

    Descriptor d = addTypeDescription(type, "The class modelling an arena in which robots can live");
    d.describeReal("z").def(0.0).help("The z coordinate of the plane in the world frame of reference", "This is the z coordinate of the upper part of the plane in the world frame of reference");
    d.describeReal("planeWidth").def(2.5).limits(0.01, +Infinity).help("The width of the arena");
    d.describeReal("planeHeight").def(0.0).limits(0.01, +Infinity).help("The height of the arena");
}

void Arena::addRobots(QStringList robots)
{
    // First of all adding robots to the list of usable resources. This will call the resourceChanged callback
    // because robots already exist, however they are not in the m_robotResourceWrappers map so the callback will
    // do nothing (we take care of creating the wrappers here)
    addUsableResources(robots);

    // Now taking the lock and generating the wrappers for all robots
    ResourcesLocker locker(this);

    foreach(QString robot, robots) {
        bool robotExists;
        RobotOnPlane* r = getResource<RobotOnPlane>(robot, &robotExists);

        // The first thing to do is to check if the robot existed: in that case we have to first delete
        // the old wrapper
        if (m_robotResourceWrappers.contains(robot)) {
            // This will probably crash if the robot is not in the list (this would be a bug anyway)
            m_objects2DList.remove(m_objects2DList.indexOf(m_robotResourceWrappers[robot]));

            // Deleting the wrapper
            delete m_robotResourceWrappers[robot];
        }

        // If the robot actually exists, adding a wrapper, otherwise we simply add the robot to the map to
        // remember the association
        if (robotExists) {
            // Creating the wrapper
            WheeledRobot2DWrapper* wrapper = new WheeledRobot2DWrapper(this, r, r->robotHeight(), r->robotRadius());

            // Adding the wrapper to the list
            m_objects2DList.push_back(wrapper);
            m_robotResourceWrappers.insert(robot, wrapper);
        } else {
            // Adding the robot to the map with a NULL wrapper
            m_robotResourceWrappers.insert(robot, NULL);
        }
    }
}

const WheeledRobot2DWrapper* Arena::getRobotWrapper(QString robotName) const
{
    if (m_robotResourceWrappers.contains(robotName)) {
        return m_robotResourceWrappers[robotName];
    } else {
        return NULL;
    }
}

Box2DWrapper* Arena::getPlane()
{
    return m_plane;
}

Box2DWrapper* Arena::createWall(QColor color, wVector start, wVector end, real thickness, real height)
{
    ResourcesLocker locker(this);

    // Changing parameters
    start.z = m_z;
    end.z = m_z;

    // Creating the box, then we have to change its position and make it static
    Box2DWrapper* b = createBox(color, (end - start).norm(), thickness, height, Box2DWrapper::Wall);

    // Moving the wall and setting the texture. We have to compute the rotation around the Z axis of the
    // wall and the position of its center to build its transformation matrix
    const real angle = atan2(end.y - start.y, end.x - start.x);
    const wVector centerPosition = start + (end - start).scale(0.5);
    wMatrix mtr = wMatrix::roll(angle);
    mtr.w_pos = centerPosition + wVector(0.0, 0.0, b->phyObject()->matrix().w_pos.z);
    b->phyObject()->setMatrix(mtr);
    b->setTexture("tile2");

    return b;
}

Cylinder2DWrapper* Arena::createSmallCylinder(QColor color, real height)
{
    return createCylinder(color, smallCylinderRadius, height, Cylinder2DWrapper::SmallCylinder);
}

Cylinder2DWrapper* Arena::createBigCylinder(QColor color, real height)
{
    return createCylinder(color, bigCylinderRadius, height, Cylinder2DWrapper::BigCylinder);
}

Cylinder2DWrapper* Arena::createCircularTargetArea(real radius, QColor color)
{
    ResourcesLocker locker(this);

    // Creating the cylinder, then we only have to change its z position and set the material to nonCollidable
    Cylinder2DWrapper* c = createCylinder(color, radius, targetAreasHeight, Cylinder2DWrapper::CircularTargetArea);

    wMatrix mtr = c->phyObject()->matrix();
    mtr.w_pos = wVector(0.0, 0.0, m_z - (targetAreasHeight / 2.0) + targetAreasProtrusion);
    c->phyObject()->setMatrix(mtr);
    c->phyObject()->setMaterial("nonCollidable");

    return c;
}

Box2DWrapper* Arena::createRectangularTargetArea(real width, real depth, QColor color)
{
    ResourcesLocker locker(this);

    // Creating the box, then we only have to change its z position and set the material to nonCollidable
    Box2DWrapper* b = createBox(color, width, depth, targetAreasHeight, Box2DWrapper::RectangularTargetArea);

    wMatrix mtr = b->phyObject()->matrix();
    mtr.w_pos = wVector(0.0, 0.0, m_z - (targetAreasHeight / 2.0) + targetAreasProtrusion);
    b->phyObject()->setMatrix(mtr);
    b->phyObject()->setMaterial("nonCollidable");

    return b;
}

bool Arena::delete2DObject(PhyObject2DWrapper* obj)
{
    int index = m_objects2DList.indexOf(obj);

    if ((index == -1) || (dynamic_cast<farsa::WheeledRobot2DWrapper*>(obj) != NULL)) {
        return false;
    }

    // Deleting the object
    delete m_objects2DList[index];
    m_objects2DList.remove(index);

    return true;
}

void Arena::prepareToHandleKinematicRobotCollisions()
{
    ResourcesLocker locker(this);

    // Cycling through the list of robots and storing the matrix
    foreach (WheeledRobot2DWrapper* robot, m_robotResourceWrappers) {
        // We only act on kinematic robots
        if (!robot->robotOnPlane()->isKinematic()) {
            continue;
        }
        robot->storePreviousMatrix();
    }
}

void Arena::handleKinematicRobotCollisions()
{
    ResourcesLocker locker(this);

    // Here we use a naive approach: we check collision of every object with every robot. This is not
    // the best possible algorithm (complexity is O(n^2)...), but it's the way things are done in evorobot

    // Cycling through the list of robots
    foreach (WheeledRobot2DWrapper* robot, m_robotResourceWrappers) {
        // We only act on kinematic robots
        if (!robot->robotOnPlane()->isKinematic()) {
            continue;
        }

        bool collides = false;
        // Checking if the robot collides
        foreach (PhyObject2DWrapper* obj, m_objects2DList) {
            // Not checking the collision of the robot with itself (obviously...)
            if (obj == robot) {
                continue;
            }

            double distance;
            double angle;

            // Getting the distance between the robot and the object. If computeDistanceAndOrientationFromRobot
            // returns false, we have to discard the object
            if (!obj->computeDistanceAndOrientationFromRobot(*robot, distance, angle)) {
                continue;
            }

            // If the distance is negative, we had a collision
            if (distance < 0.0) {
                collides = true;
                break;
            }
        }

        // If the robot collides with something else, moving it back to its previous position
        if (collides) {
            robot->wObject()->setMatrix(robot->previousMatrix());
        }
    }
}

Cylinder2DWrapper* Arena::createCylinder(QColor color, real radius, real height, Cylinder2DWrapper::Type type)
{
    ResourcesLocker locker(this);

    // Changing parameters
    if (height < 0.0) {
        height = defaultHeight;
    }

    PhyCylinder* cylinder = new PhyCylinder(radius, height, m_world, "cylinder");
    wMatrix mtr = wMatrix::yaw(PI_GRECO / 2.0);
    mtr.w_pos = wVector(0.0, 0.0, m_z + (height / 2.0));
    cylinder->setMatrix(mtr);
    cylinder->setUseColorTextureOfOwner(false);
    cylinder->setColor(color);
    cylinder->setTexture("");

    // Creating the wrapper
    Cylinder2DWrapper* wrapper = new Cylinder2DWrapper(this, cylinder, type);

    // Appending the wrapper to the list
    m_objects2DList.append(wrapper);

    return wrapper;
}

Box2DWrapper* Arena::createBox(QColor color, real width, real depth, real height, Box2DWrapper::Type type)
{
    ResourcesLocker locker(this);

    // Changing parameters
    if (height < 0.0) {
        height = defaultHeight;
    }

    PhyBox* box = new PhyBox(width, depth, height, m_world, "box");

    // Moving the box so that it lies on the plane
    wMatrix mtr = wMatrix::identity();
    mtr.w_pos = wVector(0.0, 0.0, m_z + (height / 2.0));
    box->setMatrix(mtr);
    box->setUseColorTextureOfOwner(false);
    box->setColor(color);
    box->setTexture("");

    // Creating the wrapper
    Box2DWrapper* wrapper = new Box2DWrapper(this, box, type);

    // Appending the wrapper to the list
    m_objects2DList.append(wrapper);

    return wrapper;
}

void Arena::resourceChanged(QString resourceName, ResourceChangeType changeType)
{
    if (resourceName == "world") {
        switch (changeType) {
            case Created:
            case Modified:
                m_world = getResource<World>();
                break;
            case Deleted:
                m_world = NULL;
                break;
        }
    } else if (m_robotResourceWrappers.contains(resourceName)) {
        // The resource for a robot has changed status
        switch (changeType) {
            case Created:
            case Modified:
                // We have to delete the previous wrapper and add a new one
                {
                    // Deleting the old wrapper if it exists
                    if (m_robotResourceWrappers[resourceName] != NULL) {
                        m_objects2DList.remove(m_objects2DList.indexOf(m_robotResourceWrappers[resourceName]));

                        // Deleting the wrapper
                        delete m_robotResourceWrappers[resourceName];
                    }

                    // Creating a new wrapper
                    RobotOnPlane* r = getResource<RobotOnPlane>();
                    WheeledRobot2DWrapper* wrapper = new WheeledRobot2DWrapper(this, r, r->robotHeight(), r->robotRadius());

                    // Adding the wrapper to the list
                    m_objects2DList.push_back(wrapper);
                    m_robotResourceWrappers.insert(resourceName, wrapper);
                }
                break;
            case Deleted:
                // Simply removing the old wrapper
                {
                    m_objects2DList.remove(m_objects2DList.indexOf(m_robotResourceWrappers[resourceName]));

                    // Deleting the wrapper
                    delete m_robotResourceWrappers[resourceName];
                    m_robotResourceWrappers[resourceName] = NULL;
                }
                break;
        }
    }
}

Box2DWrapper* Arena::createPlane(ConfigurationParameters& params, QString prefix, real z, Arena* arena)
{
    // First of all getting the dimensions of the plane
    const real planeWidth = ConfigurationHelper::getDouble(params, prefix + "planeWidth", 2.5);
    const real planeHeight = ConfigurationHelper::getDouble(params, prefix + "planeHeight", 2.5);

    // Also getting the world from the ResourcesUser associated with param
    SimpleResourcesUser* r = params.getResourcesUserForResource("world");
    if (r == NULL) {
        ConfigurationHelper::throwUserMissingResourceError("world", "We need a world to create the arena!");
    }
    World* world = r->getResource<World>("world");

    PhyBox* plane = new PhyBox(planeWidth, planeHeight, planeThickness, world, "plane");
    plane->setPosition(wVector(0.0, 0.0, z - (planeThickness / 2.0)));
    plane->setStatic(true);
    plane->setColor(Qt::white);
    plane->setTexture("tile2");

    // Also resizing world to comfortably fit the arena
    world->setSize(wVector(-planeWidth, -planeHeight, -planeThickness), wVector(planeWidth, planeHeight, +6.0));

    // Creating the wrapper and returning it
    Box2DWrapper* ret = new Box2DWrapper(arena, plane, Box2DWrapper::Plane);
    arena->m_objects2DList.append( ret );
    return ret;
}

Box2DWrapper* Arena::createPlane2(ConfigurationParameters& params, QString prefix, real z, Arena* arena)
{
    // First of all getting the dimensions of the plane
    const real planeWidth = ConfigurationHelper::getDouble(params, prefix + "planeWidth", 2.5);
    const real planeHeight = ConfigurationHelper::getDouble(params, prefix + "planeHeight", 2.5);
    
    // the plane will be composed by many boxes near to each other
    const real cellSize = 0.20f;
    int numCellW = ceil(planeWidth/cellSize);
    int numCellH = ceil(planeHeight/cellSize);

    // Also getting the world from the ResourcesUser associated with param
    SimpleResourcesUser* r = params.getResourcesUserForResource("world");
    if (r == NULL) {
        ConfigurationHelper::throwUserMissingResourceError("world", "We need a world to create the arena!");
    }
    World* world = r->getResource<World>("world");

    Box2DWrapper* ret;
    for( int w=0; w<numCellW; w++ ) {
        for( int h=0; h<numCellH; h++ ) {
            PhyBox* plane = new PhyBox(cellSize, cellSize, planeThickness, world, "cellPlane");
            plane->setPosition(wVector( w*cellSize-planeWidth/2.0, h*cellSize-planeHeight/2.0, z - (planeThickness / 2.0)));
            plane->setStatic(true);
            plane->setColor(Qt::white);
            plane->setTexture("tile2");
            
            if ( w==0 || h==0 ) {
                // use this as reference for plane
                ret = new Box2DWrapper(arena, plane, Box2DWrapper::Plane);
            }
            // Appending the wrapper to the list
            arena->m_objects2DList.append( new Box2DWrapper(arena, plane, Box2DWrapper::Plane) );
        }
    }
    // Also resizing world to comfortably fit the arena
    world->setSize(wVector(-planeWidth, -planeHeight, -planeThickness), wVector(planeWidth, planeHeight, +6.0));
    // return one of the cell created as reference for the plane
    return ret;
}

} // end namespace farsa