world.cpp

/********************************************************************************
 *  WorldSim -- library for robot simulations                                   *
 *  Copyright (C) 2008-2011 Gianluca Massera <emmegian@yahoo.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 "world.h"
#include "phyobject.h"
#include "phyjoint.h"
#include "private/phyobjectprivate.h"
#include "private/worldprivate.h"
#include "motorcontrollers.h"
#include <QMutex>
#include <QMutexLocker>
#include <QThread>

#ifdef __GNUC__
#pragma GCC diagnostic ignored "-Wunused-parameter"
#endif
#ifdef FARSA_USE_YARP_AND_ICUB
    #include "yarp/os/Network.h"
#endif
#ifdef __GNUC__
#pragma GCC diagnostic warning "-Wunused-parameter"
#endif

#include <QPair>

namespace farsa {

MaterialDB::MaterialDB( World* w ) : mats(), pmap() {
    this->worldv = w;
    createMaterial( "nonCollidable" );
    createMaterial( "default" );
}

bool MaterialDB::createMaterial( QString name ) {
    if ( mats.contains( name ) ) {
        return false;
    }
#ifdef WORLDSIM_USE_NEWTON
    NewtonWorld* ngdWorld = worldv->priv->world;
    int newm;
    if ( name == "default" ) {
        newm = NewtonMaterialGetDefaultGroupID( ngdWorld );
    } else {
        newm = NewtonMaterialCreateGroupID( ngdWorld );
    }
    worldv->priv->matIDs[name] = newm;
    NewtonMaterialSetCollisionCallback( ngdWorld, newm, newm, (void*)worldv, NULL, (WorldPrivate::processCollisionHandler) );
    // --- setting callbacks
    foreach( QString k, mats.values() ) {
        int kid = worldv->priv->matIDs[k];
        NewtonMaterialSetCollisionCallback( ngdWorld, newm, kid, (void*)worldv, NULL, (WorldPrivate::processCollisionHandler) );
    }
    // --- setting nonCollidable material
    NewtonMaterialSetDefaultCollidable( ngdWorld, worldv->priv->matIDs["nonCollidable"], newm, 0 );
#endif
    mats.insert( name );
    return true;
}

void MaterialDB::setFrictions( QString mat1, QString mat2, real st, real kn ) {
    if ( !mats.contains( mat1 ) ) return;
    if ( !mats.contains( mat2 ) ) return;
    QString pkey = createKey( mat1, mat2 );
    pmap[pkey].staticFriction = st;
    pmap[pkey].dynamicFriction = kn;
#ifdef WORLDSIM_USE_NEWTON
    int id1 = worldv->priv->matIDs[mat1];
    int id2 = worldv->priv->matIDs[mat2];
    NewtonMaterialSetDefaultFriction( worldv->priv->world, id1, id2, st, kn );
#endif
    return;
}

void MaterialDB::setElasticity( QString mat1, QString mat2, real el ) {
    if ( !mats.contains( mat1 ) ) return;
    if ( !mats.contains( mat2 ) ) return;
    QString pkey = createKey( mat1, mat2 );
    pmap[pkey].elasticity = el;
#ifdef WORLDSIM_USE_NEWTON
    int id1 = worldv->priv->matIDs[mat1];
    int id2 = worldv->priv->matIDs[mat2];
    NewtonMaterialSetDefaultElasticity( worldv->priv->world, id1, id2, el );
#endif
    return;
}

void MaterialDB::setSoftness( QString mat1, QString mat2, real sf ) {
    if ( !mats.contains( mat1 ) ) return;
    if ( !mats.contains( mat2 ) ) return;
    QString pkey = createKey( mat1, mat2 );
    pmap[pkey].softness = sf;
#ifdef WORLDSIM_USE_NEWTON
    int id1 = worldv->priv->matIDs[mat1];
    int id2 = worldv->priv->matIDs[mat2];
    NewtonMaterialSetDefaultSoftness( worldv->priv->world, id1, id2, sf );
#endif
    return;
}

void MaterialDB::setGravityForce( QString mat, real force ) {
    gravities[mat] = force;
}

real MaterialDB::gravityForce( QString mat ) {
    if ( gravities.contains( mat ) ) {
        return gravities[mat];
    } else {
        return worldv->gravityForce();
    }
}


void MaterialDB::enableCollision( QString mat1, QString mat2, bool enable ) {
    if ( !mats.contains( mat1 ) ) return;
    if ( !mats.contains( mat2 ) ) return;
    QString pkey = createKey( mat1, mat2 );
    pmap[pkey].collisions = enable;
#ifdef WORLDSIM_USE_NEWTON
    int id1 = worldv->priv->matIDs[mat1];
    int id2 = worldv->priv->matIDs[mat2];
    NewtonMaterialSetDefaultCollidable( worldv->priv->world, id1, id2, enable );
#endif
}

void MaterialDB::setProperties( QString mat1, QString mat2, real fs, real fk, real el, real sf, bool en ) {
    setFrictions( mat1, mat2, fs, fk );
    setElasticity( mat1, mat2, el );
    setSoftness( mat1, mat2, sf );
    enableCollision( mat1, mat2, en );
}

QString MaterialDB::createKey( QString mat1, QString mat2 ) {
    if ( mat1 < mat2 ) {
        return mat1 + ":" + mat2;
    } else {
        return mat1 + ":" + mat2;
    }
}

World::World( QString name, bool LocalYarpPorts )
    : QObject(), minP(-100,-100,-100), maxP(100,100,100), objs(), jointsv(), mapObjJoints(),
      cmap(), nobjs() {
#ifdef FARSA_USE_YARP_AND_ICUB
    static QMutex yarpStuffsMutex; // This is needed because networkYarp is static and so there could be concurrent accesses
    static yarp::os::Network networkYarp;
    if ( LocalYarpPorts ) {
        QMutexLocker locker(&yarpStuffsMutex);

        networkYarp.setLocalMode( LocalYarpPorts );
        networkYarp.setVerbosity( -1 );
    }
#else
    // This is to avoid the warning regarding the unsed parameter LocalYarpPorts
    Q_UNUSED( LocalYarpPorts )
#endif
    timerId = 0;
    state = stoppedS;
    namev = name;
    time = 0.0;
    timestepv = 0.0150f; //--- about 66.66666 frame per second
    isrealtimev = false;
    isInit = false;
    isFinish = true;
    gforce = -9.8f;
    priv = new WorldPrivate();

#ifdef WORLDSIM_USE_NEWTON
    priv->world = NewtonCreate();
    NewtonInvalidateCache( priv->world );
    NewtonSetWorldSize( priv->world, &minP[0], &maxP[0] );
     // keep at least to 100 for stability
    NewtonSetMinimumFrameRate( priv->world, 100 );
    // exact model as default
    NewtonSetSolverModel( priv->world, 0 );
    NewtonSetFrictionModel( priv->world, 0 );
    // enable multi-thread
    //--- for works we before check that all callbacks implementation are thread-safe
    //--- REMEMBER: connect, signal-slot of non-QT data needs to register the type with
    //---           qRegisterMetaType
    //NewtonSetThreadsCount( priv->world, 2 );
#endif

    mats = new MaterialDB( this );
    return;
}

World::~World() {
    // Here we can't use foreach because lists could be modified while we are iterating on them
    // by the popObject(), popJoint() or popMotorController() functions. See the comment in the
    // declaration of objs for more information
    while ( !jointsv.isEmpty() ) {
        PhyJoint* js = jointsv.takeFirst();
        if ( js->owner() == NULL ) {
            delete js;
        }
    }
    while ( !objs.isEmpty() ) {
        WObject* obj = objs.takeFirst();
        if ( obj->owner() == NULL ) {
            delete obj;
        }
    }
#ifdef WORLDSIM_USE_NEWTON
    NewtonDestroy( priv->world );
#endif
    delete priv;
}

QString World::name() const {
    return namev;
}

void World::initialize() {
    if ( isInit ) return;
    if ( !isFinish ) {
        finalize();
    }
    time = 0.0;
    timer.tic();
    state = stoppedS;
    isInit = true;
    isFinish = false;
    emit initialized();
}

void World::advance() {
    if ( !isInit ) {
        initialize();
    }
    while( isrealtimev && timer.tac()/1000000.0 < timestepv ) { };

    timer.tic();
    //--- preUpdate Objects
    for( QLinkedList<WObject*>::iterator it = objs.begin(); it != objs.end(); it++ ) {
        (*it)->preUpdate();
    }
    //--- preUpdate Joints
    for( QLinkedList<PhyJoint*>::iterator it = jointsv.begin(); it != jointsv.end(); it++ ) {
        (*it)->preUpdate();
    }
    // Simulate physic. Before doing the actual simulation step, clearing the map of contacts
    // (we do it here so that calls to preUpdate() can access the old map of contacts)
    cmap.clear();
#ifdef WORLDSIM_USE_NEWTON
    NewtonUpdate( priv->world, timestepv );
#endif
    //--- postUpdate Objects
    for( QLinkedList<WObject*>::iterator it = objs.begin(); it != objs.end(); it++ ) {
        (*it)->postUpdate();
    }
    //--- postUpdate Joints
    for( QLinkedList<PhyJoint*>::iterator it = jointsv.begin(); it != jointsv.end(); it++ ) {
        (*it)->postUpdate();
    }
    time += timestepv;

    emit advanced();
}

void World::finalize() {
    if ( isFinish ) return;
    /* nothing else to do ?!?! */
    isFinish = true;
    isInit = false;
    emit finished();
}

int World::play() {
    if ( state == playingS ) return timerId;
    if ( timerId != 0 ) return timerId;
    state = playingS;
    timerId = startTimer( 0 );
    return timerId;
}

void World::pause() {
    state = pausedS;
    if ( timerId != 0 ) killTimer( timerId );
    timerId = 0;
    emit paused();
    return;
}

void World::stop() {
    finalize();
    state = stoppedS;
    if ( timerId != 0 ) killTimer( timerId );
    timerId = 0;
    emit stopped();
    return;
}

void World::timerEvent( QTimerEvent* ) {
    advance();
    return;
}

void World::customEvent( QEvent* e ) {
    switch( e->type() ) {
    case E_Advance:
        advance();
        e->accept();
        break;
    case E_Play:
        play();
        e->accept();
        break;
    case E_Stop:
        stop();
        e->accept();
        break;
    case E_Pause:
        pause();
        e->accept();
        break;
    default:
        e->ignore();
        break;
    }
    return;
}

real World::elapsedTime() const {
    return time;
}

void World::resetElapsedTime() {
    time = 0.0;
}

void World::setIsRealTime( bool b ) {
    isrealtimev = b;
}

bool World::isRealTime() const {
    return isrealtimev;
}

void World::setTimeStep( real ts ) {
    timestepv = ts;
}

void World::setMinimumFrameRate( unsigned int frames ) {
#ifdef WORLDSIM_USE_NEWTON
    NewtonSetMinimumFrameRate( priv->world, frames );
#endif
}

real World::timeStep() const {
    return timestepv;
}

void World::setGravityForce( real g ) {
    gforce = g;
}

real World::gravityForce() const {
    return gforce;
}

void World::advanceUntil( real t ) {
    blockSignals( true );
    bool b = isrealtimev;
    isrealtimev = false;
    while ( time < t ) {
        advance();
    }
    isrealtimev = b;
    blockSignals( false );
}

const QLinkedList<WObject*> World::objects() {
    return objs;
}

WObject* World::getObject( QString name ) {
    //--- not very efficient, FIX adding a QMap
    foreach( WObject* obj, objs ) {
        if ( obj->name() == name ) return obj;
    }
    return NULL;
}

const QLinkedList<PhyJoint*> World::joints() {
    return jointsv;
}

const QHash<WObject*, QList<PhyJoint*> > World::mapObjectsToJoints() {
    return mapObjJoints;
}

void World::disableCollision( PhyObject* obj1, PhyObject* obj2 ) {
    nobjs.insert( qMakePair( obj1, obj2 ) );
    nobjs.insert( qMakePair( obj2, obj1 ) );
}

void World::enableCollision( PhyObject* obj1, PhyObject* obj2 ) {
    nobjs.remove( qMakePair( obj1, obj2 ) );
    nobjs.remove( qMakePair( obj2, obj1 ) );
}

void World::setSolverModel( QString model ) {
#ifdef WORLDSIM_USE_NEWTON
    if ( model =="exact" ) {
        NewtonSetSolverModel( priv->world, 0 );
    } else if ( model == "linear" ) {
        NewtonSetSolverModel( priv->world, 1 );
    }
#endif
}

void World::setFrictionModel( QString model ) {
#ifdef WORLDSIM_USE_NEWTON
    if ( model =="exact" ) {
        NewtonSetFrictionModel( priv->world, 0 );
    } else if ( model == "linear" ) {
        NewtonSetFrictionModel( priv->world, 1 );
    }
#endif
}

void World::setMultiThread( int numThreads ) {
#ifdef WORLDSIM_USE_NEWTON
    numThreads = max( 1, numThreads );
    NewtonSetThreadsCount( priv->world, numThreads );
#endif
}

void World::setSize( const wVector& minPoint, const wVector& maxPoint ) {
    minP = minPoint;
    maxP = maxPoint;
#ifdef WORLDSIM_USE_NEWTON
    NewtonSetWorldSize( priv->world, &minP[0], &maxP[0] );
#endif
    emit resized();
}

void World::size( wVector &minPoint, wVector &maxPoint ) {
    minPoint = minP;
    maxPoint = maxP;
}

void World::pushObject( WObject* phy ) {
    objs.push_back( phy );
    emit addedObject( phy );
}

void World::popObject( WObject* phy ) {
    const int numRemoved = objs.removeAll( phy );
    if ( numRemoved == 1 ) {
        // Also removing all collisions for the phy (if it is a PhyObject) object. We also have to manipulate the list
        // of all objects colliding with phy
        PhyObject* const phyObject = dynamic_cast<PhyObject*>(phy);
        if ( ( phyObject != NULL ) && ( cmap.contains( phyObject ) ) ) {
            const contactVec &cvec = cmap[ phyObject ];
            foreach ( Contact c, cvec ) {
                contactVec &otherCVec = cmap[ c.collide ];
                contactVec::iterator it = otherCVec.begin();
                while ( it != otherCVec.end() ) {
                    if ( it->collide == phyObject ) {
                        it = otherCVec.erase( it );
                    } else {
                        ++it;
                    }
                }
                if ( otherCVec.isEmpty() ) {
                    // Removing collisions if the list is empty
                    cmap.remove( c.collide );
                }
            }
            cmap.remove( phyObject );
        }
        emit removedObject( phy );
    }
#ifdef FARSA_DEBUG
    else if (numRemoved > 1) {
        qDebug( "INTERNAL ERROR: more than one object with the same address found in the World::objs list" );
    }
#endif
}

void World::pushJoint( PhyJoint* phy ) {
    jointsv.push_back( phy );
    mapObjJoints[ phy->child() ].push_back( phy );
    if ( phy->parent() ) {
        mapObjJoints[ phy->parent() ].push_back( phy );
    }
    emit addedJoint( phy );
}

void World::popJoint( PhyJoint* phy ) {
    const int numRemoved = jointsv.removeAll( phy );
    if ( numRemoved == 1 ) {
        mapObjJoints[ phy->child() ].removeAll( phy );
        mapObjJoints.remove( phy->child() );
        if ( phy->parent() ) {
            mapObjJoints[ phy->parent() ].removeAll( phy );
            mapObjJoints.remove( phy->parent() );
        }
        emit removedJoint( phy );
    }
#ifdef FARSA_DEBUG
    else if (numRemoved > 1) {
        qDebug( "INTERNAL ERROR: more than one joint with the same address found in the World::jointsv list" );
    }
#endif
}

void World::cleanUpMemory() {
#ifdef WORLDSIM_USE_NEWTON
    //qDebug() << "Memory Used by" << name() << NewtonGetMemoryUsed();
    NewtonInvalidateCache( priv->world );
    cmap.clear();
    /*
    qDebug() << "Objects: " << objs.size();
    qDebug() << "Joints: " << jointsv.size();
    qDebug() << "Map Obj-Joints: " << mapObjJoints.size();
    qDebug() << "Contacts: " << cmap.size();
    qDebug() << "Non-Collidable Objs: " << nobjs.size();
    */
    /*
    qDebug() << "Materials: " << mats->mats.size();
    qDebug() << "Gravities: " << mats->gravities.size();
    qDebug() << "Material Infos: " << mats->pmap.size();
    */
#endif
}

const contactMap& World::contacts() {
    return cmap;
}

bool World::checkContacts( PhyObject* obj1, PhyObject* obj2, int maxContacts, QVector<wVector>* contacts, QVector<wVector>* normals, QVector<real>* penetra ) {
#ifdef WORLDSIM_USE_NEWTON
    // Allocating the vector we use to get data from NewtonCollisionCollide
    real *const tmp_data = new real[2 * 3 * maxContacts + maxContacts];
    real *const tmp_contacts = &tmp_data[0 * 3 * maxContacts];
    real *const tmp_normals = &tmp_data[1 * 3 * maxContacts];
    real *const tmp_penetra = &tmp_data[2 * 3 * maxContacts];

    // Computing contacts
    wMatrix t1 = obj1->matrix();
    wMatrix t2 = obj2->matrix();
    const int numContacts = NewtonCollisionCollide( priv->world, maxContacts, obj1->priv->collision, &t1[0][0], obj2->priv->collision, &t2[0][0], tmp_contacts, tmp_normals, tmp_penetra, 0 );

    // Now copying contacts information into user vectors
    if (contacts != NULL) {
        contacts->resize(numContacts);
        for (int i = 0; i < numContacts; i++) {
            (*contacts)[i].x = tmp_contacts[0 + i * 3];
            (*contacts)[i].y = tmp_contacts[1 + i * 3];
            (*contacts)[i].z = tmp_contacts[2 + i * 3];
        }
    }
    if (normals != NULL) {
        normals->resize(numContacts);
        for (int i = 0; i < numContacts; i++) {
            (*normals)[i].x = tmp_normals[0 + i * 3];
            (*normals)[i].y = tmp_normals[1 + i * 3];
            (*normals)[i].z = tmp_normals[2 + i * 3];
        }
    }
    if (penetra != NULL) {
        penetra->resize(numContacts);
        for (int i = 0; i < numContacts; i++) {
            (*penetra)[i] = tmp_penetra[i];
        }
    }

    delete[] tmp_data;
    return (numContacts != 0);
#endif
}

bool World::smartCheckContacts( PhyObject* obj1, PhyObject* obj2, int maxContacts, QVector<wVector>* contacts )
{
    if (obj1->getKinematic() || obj2->getKinematic() || (obj1->getStatic() && obj2->getStatic())) {
        return checkContacts( obj1, obj2, maxContacts, contacts, NULL, NULL);
    } else {
        if (!cmap.contains(obj1)) {
            return false;
        }

        // Taking the vector of contacts
        const contactVec& c = cmap[obj1];
        bool collision = false;
        if (contacts != NULL) {
            contacts->clear();
        }

        // Now extracting all collisions between obj1 and obj2
        for (int i = 0; i < c.size(); i++) {
            if (c[i].collide == obj2) {
                collision = true;
                if ((contacts != NULL) && (contacts->size() < maxContacts)) {
                    // Adding contact point
                    contacts->append(c[i].worldPos);
                }
            }
        }

        return collision;
    }
}

real World::collisionRayCast( PhyObject* obj, wVector start, wVector end, wVector* normal )
{
#ifdef WORLDSIM_USE_NEWTON
    // The vector storing the normal to the contact point and the attribute (unused)
    dFloat n[3];
    int attribute;
    wVector localStart = obj->matrix().inverse().transformVector(start);
    wVector localEnd = obj->matrix().inverse().transformVector(end);

    // Computing the contact
    const real contact = NewtonCollisionRayCast(obj->priv->collision, &localStart[0], &localEnd[0], n, &attribute);

    if (normal != NULL) {
        (*normal)[0] = n[0];
        (*normal)[1] = n[1];
        (*normal)[2] = n[2];
    }

    return contact;
#else
    return 2.0;
#endif
}

rayCastHitVector World::worldRayCast( wVector start, wVector end, bool onlyClosest, const QSet<PhyObject*>& ignoredObjs )
{
#ifdef WORLDSIM_USE_NEWTON
    WorldPrivate::WorldRayCastCallbackUserData data(start, end, onlyClosest, ignoredObjs);

    // Casting the ray
    NewtonWorldRayCast(priv->world, &start[0], &end[0], WorldPrivate::worldRayFilterCallback, &data, NULL);

    return data.vector;
#else
    return rayCastHitVector();
#endif
}

// bool World::checkContacts( PhyObject* obj1, PhyObject* obj2 ) {
// #ifdef WORLDSIM_USE_NEWTON
//  const int maxsize = 4;
//  real contacts[3*maxsize];
//  real normals[3*maxsize];
//  real penetra[3*maxsize];
//  wMatrix t1 = obj1->matrix();
//  wMatrix t2 = obj2->matrix();
//  bool ret = ( NewtonCollisionCollide( priv->world, maxsize, obj1->priv->collision, &t1[0][0], obj2->priv->collision, &t2[0][0], contacts, normals, penetra, 0 ) != 0 );
//  return ret;
// #endif
// }

bool World::closestPoints( PhyObject* objA, PhyObject* objB, wVector& pointA, wVector& pointB ) {
#ifdef WORLDSIM_USE_NEWTON
    wVector normal;
    wMatrix t1 = objA->matrix();
    wMatrix t2 = objB->matrix();
    int ret = NewtonCollisionClosestPoint( priv->world,
        objA->priv->collision, &t1[0][0],
        objB->priv->collision, &t2[0][0],
        &pointA[0], &pointB[0], &normal[0], 0 );
    if ( ret == 0 ) {
        pointA = wVector(0,0,0);
        pointB = wVector(0,0,0);
        return false;
    }
    return true;
#endif
}

} // end namespace farsa