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
namespace {
    const real defaultHeight = 0.3f;

    const real planeThickness = 0.1;

    const real defaultSmallCylinderRadius = 0.0125;

    const real defaultBigCylinderRadius = 0.027;

    const real targetAreasHeight = 0.01;

    const real targetAreasProtrusion = 0.005;
}

Arena::Arena(ConfigurationParameters& params, QString prefix) :
    ParameterSettableInConstructor(params, prefix),
    ConcurrentResourcesUser(),
    m_z(ConfigurationHelper::getDouble(params, prefix + "z", 0.0)),
    m_smallCylinderRadius(ConfigurationHelper::getDouble(params, prefix + "smallCylinderRadius", defaultSmallCylinderRadius)),
    m_bigCylinderRadius(ConfigurationHelper::getDouble(params, prefix + "bigCylinderRadius", defaultBigCylinderRadius)),
    m_objects2DList(QVector<PhyObject2DWrapper*>()),
    m_plane(createPlane(params, prefix, m_z, this)),
    m_robotResourceWrappers(),
    m_kinematicRobotCollisions(),
    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("smallCylinderRadius").def(defaultSmallCylinderRadius).help("The radius of small cylinders", "This is the value to use for the radius of small cylinders. The default value is the one used in evorobot. If you change this, be careful which sample files you use.");
    d.describeReal("bigCylinderRadius").def(defaultBigCylinderRadius).help("The radius of big cylinders", "This is the value to use for the radius of big cylinders. The default value is the one used in evorobot. If you change this, be careful which sample files you use.");
    d.describeReal("planeWidth").def(2.5).limits(0.01, +Infinity).help("The width of the arena");
    d.describeReal("planeHeight").def(2.5).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;
}

const Box2DWrapper* Arena::getPlane() const
{
    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, m_smallCylinderRadius, height, Cylinder2DWrapper::SmallCylinder);
}

Cylinder2DWrapper* Arena::createBigCylinder(QColor color, real height)
{
    return createCylinder(color, m_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 physical object
    delete obj->phyObject();

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

    // Clearing the list of collisions for each robot
    m_kinematicRobotCollisions.clear();
}

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;

                m_kinematicRobotCollisions[robot].append(obj);
            }
        }

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

QVector<PhyObject2DWrapper*> Arena::getKinematicRobotCollisions(WheeledRobot2DWrapper* robot) const
{
    return m_kinematicRobotCollisions.value(robot);
}

QVector<PhyObject2DWrapper*> Arena::getKinematicRobotCollisions(QString robotResourceName) const
{
    if (m_robotResourceWrappers.contains(robotResourceName)) {
        return getKinematicRobotCollisions(m_robotResourceWrappers.value(robotResourceName));
    } else {
        throw ArenaException(QString("The arena doesn't contain the robot named %1 (perhaps you used \"robot\"? You must use the complete name, e.g. agent[0]:robot)").arg(robotResourceName).toAscii().data());
    }
}

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 if it is smaller than necessary
    wVector worldMinPoint;
    wVector worldMaxPoint;
    world->size(worldMinPoint, worldMaxPoint);
    worldMinPoint.x = min(-planeWidth, worldMinPoint.x);
    worldMinPoint.y = min(-planeHeight, worldMinPoint.y);
    worldMinPoint.z = min(-planeThickness, worldMinPoint.z);
    worldMaxPoint.x = max(planeWidth, worldMaxPoint.x);
    worldMaxPoint.y = max(planeHeight, worldMaxPoint.y);
    worldMaxPoint.z = max(+6.0, worldMaxPoint.z);
    world->setSize(worldMinPoint, worldMaxPoint);

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