worldsim/src/phyobject.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 "phyobject.h"
#include "private/phyobjectprivate.h"
#include "private/worldprivate.h"

namespace farsa {

PhyObject::PhyObject( World* w, QString name, const wMatrix& tm, bool createPrivate )
    : WObject( w, name, tm, false ), materialv("default") {

    priv = new PhyObjectPrivate();
    worldpriv = w->priv;
    if ( createPrivate ) {
        w->pushObject( this );
        createPrivateObject();
        changedMatrix();
    }

    forceAcc = wVector(0,0,0);
    torqueAcc = wVector(0,0,0);
    isKinematic = false;
    isStatic = false;
    objInertiaVec = wVector(0.0, 1.0, 1.0, 1.0);
    objInvInertiaVec = wVector(0.0, 1.0, 1.0, 1.0);
}

PhyObject::~PhyObject() {
#ifdef WORLDSIM_USE_NEWTON
    if ( priv->body != NULL ) {
        NewtonDestroyBody( worldpriv->world, priv->body );
        NewtonReleaseCollision( worldpriv->world, priv->collision );
    }
#endif
    delete priv;
}

void PhyObject::setKinematic(bool b, bool c)
{
    //if (isKinematic == b) return;
    isKinematic = b;
#ifdef WORLDSIM_USE_NEWTON
    if (b) {
        // Unsets the signal-wrappers callback and collision shape
        NewtonBodySetTransformCallback( priv->body, 0 );
        NewtonBodySetForceAndTorqueCallback( priv->body, 0 );
        //NewtonBodySetFreezeState( priv->body, 1 ); <== this will block also any body jointed to this one
//      //--- Queste tre righe mettono un NullCollision all'oggetto fisico,
//      //    e quindi tutte le collisioni vengono totalmente ignorarte da NGD
//      //--- Un alternativa e' commentare queste righe, ed impostare la massa a 0
//      //    in questo modo, l'oggetto risultera' statico per NGD, e generera' collisioni
//      //    con gli altri oggetti. Cosi' facendo anche muovendo l'oggetto in modo
//      //    cinematico potra' cmq collidere e spostare oggetti dinamici.
//      //    Pero', cmq, gli passera' attraverso come se fossero trasparenti, quindi
//      //    movimenti veloci risulteranno 'sbagliati' dal punto di vista fisico
//      NewtonCollision* nullC = NewtonCreateNull( worldpriv->world );
//      NewtonBodySetCollision( priv->body, nullC );
//      NewtonReleaseCollision( worldpriv->world, nullC );
//      //--- soluzione alternativa: NewtonBodySetMassMatrix( priv->body, 0, 1, 1, 1 );
        if (c) {
            // Here we set mass to 0. This way the object will be static for NGD and will
            // collide with other solids, influencing them without being influenced. 
            NewtonBodySetMassMatrix( priv->body, 0, 1, 1, 1 );
        } else {
            // The following lines set a NullCollision for the physical object, so that
            // all collisions are completely ignored by Newton Game Dynamics
            NewtonCollision* nullC = NewtonCreateNull( worldpriv->world );
            NewtonBodySetCollision( priv->body, nullC );
            NewtonReleaseCollision( worldpriv->world, nullC );
        }
    } else {
        // Sets the signal-wrappers callback and collision shape
        NewtonBodySetTransformCallback( priv->body, (PhyObjectPrivate::setTransformHandler) );
        NewtonBodySetForceAndTorqueCallback( priv->body, (PhyObjectPrivate::applyForceAndTorqueHandler) );
        NewtonBodySetFreezeState( priv->body, 0 );
        NewtonBodySetCollision( priv->body, priv->collision );
        NewtonBodySetMatrix( priv->body, &tm[0][0] );
        // Restoring mass and inertia
        if (isStatic) {
            NewtonBodySetMassMatrix( priv->body, 0.0, 1.0, 1.0, 1.0 );
        } else {
            NewtonBodySetMassMatrix( priv->body, objInertiaVec[0], objInertiaVec[1], objInertiaVec[2], objInertiaVec[3] );
        }
    }
#endif
}

void PhyObject::setStatic(bool b)
{
    if (isStatic == b) return;
    isStatic = b;
#ifdef WORLDSIM_USE_NEWTON
    if (b) {
        NewtonBodySetMassMatrix( priv->body, 0.0, 1.0, 1.0, 1.0 );
    } else {
        NewtonBodySetMassMatrix( priv->body, objInertiaVec[0], objInertiaVec[1], objInertiaVec[2], objInertiaVec[3] );
    }
#endif
}

void PhyObject::reset() {
#ifdef WORLDSIM_USE_NEWTON
    wVector zero = wVector(0,0,0);
    forceAcc = zero;
    torqueAcc= zero;
    NewtonBodySetForce( priv->body, &forceAcc[0] );
    NewtonBodySetTorque( priv->body, &torqueAcc[0] );
    NewtonBodySetOmega( priv->body, &zero[0] );
    NewtonBodySetVelocity( priv->body, &zero[0] );
    //--- remove any contact involving this object
    NewtonWorld* nworld = NewtonBodyGetWorld( priv->body );
    NewtonWorldCriticalSectionLock( nworld );
    for( NewtonJoint* contactList = NewtonBodyGetFirstContactJoint( priv->body ); contactList;
         contactList = NewtonBodyGetNextContactJoint( priv->body, contactList ) ) {
        void* nextContact;
        for( void* contact = NewtonContactJointGetFirstContact(contactList); contact; contact = nextContact ) {
            //--- before removing contact, It get the nextContact in the list
            nextContact = NewtonContactJointGetNextContact( contactList, contact );
            NewtonContactJointRemoveContact( contactList, contact );
        }
    }
    NewtonWorldCriticalSectionUnlock( nworld );
#endif
}

void PhyObject::changedMatrix() {
#ifdef WORLDSIM_USE_NEWTON
    //qDebug() << "SYNC POSITION" << tm[3][0] << tm[3][1] << tm[3][2];
    NewtonBodySetMatrix( priv->body, &tm[0][0] );
#endif
}

void PhyObject::addForce( const wVector& force ) {
    forceAcc += force;
}

void PhyObject::setForce( const wVector& force) {
    forceAcc = force;
}

wVector PhyObject::force() {
    wVector f;
#ifdef WORLDSIM_USE_NEWTON
    NewtonBodyGetForce( priv->body, &f[0] );
#endif
    return f;
}

void PhyObject::addTorque( const wVector& torque ) {
    torqueAcc += torque;
}

void PhyObject::setTorque( const wVector& torque ) {
    torqueAcc = torque;
}

wVector PhyObject::torque() {
    wVector t;
#ifdef WORLDSIM_USE_NEWTON
    NewtonBodyGetTorque( priv->body, &t[0] );
#endif
    return t;
}

void PhyObject::setMassInertiaVec( const wVector& mi ) {
    objInertiaVec = mi;
#ifdef WORLDSIM_USE_NEWTON
    // Setting mass and getting the inverse inertia matrix
    NewtonBodySetMassMatrix( priv->body, mi[0], mi[1], mi[2], mi[3] );
    NewtonBodyGetInvMass( priv->body, &objInvInertiaVec[0], &objInvInertiaVec[1], &objInvInertiaVec[2], &objInvInertiaVec[3]);
#endif
}

wVector PhyObject::massInertiaVec() const {
    return objInertiaVec;
}

wVector PhyObject::invMassInertiaVec() const {
    return objInvInertiaVec;
}

wVector PhyObject::invInertiaVec() const {
    wVector mi;
    mi[0] = objInvInertiaVec[1];
    mi[1] = objInvInertiaVec[2];
    mi[2] = objInvInertiaVec[3];
    mi[3] = 0.0;
    return mi;
}

void PhyObject::setMass( real newmass ) {
#ifdef WORLDSIM_USE_NEWTON
    if ( newmass < 0.0001 ) {
        objInertiaVec = wVector(0.0, 1.0, 1.0, 1.0);
        objInvInertiaVec = wVector(0.0, 1.0, 1.0, 1.0);
        isStatic = true;
        NewtonBodySetMassMatrix( priv->body, 0, 1, 1, 1 );
    } else {
        real inertia[3];
        real centre[3] = { 0, 0, 0 };
        NewtonConvexCollisionCalculateInertialMatrix( priv->collision, inertia, centre );

        objInertiaVec = wVector(newmass, newmass*inertia[0], newmass*inertia[1], newmass*inertia[2]);
        NewtonBodySetMassMatrix( priv->body, objInertiaVec[0], objInertiaVec[1], objInertiaVec[2], objInertiaVec[3] );
        // Saving the inverse inertia matrix
        NewtonBodyGetInvMass( priv->body, &objInvInertiaVec[0], &objInvInertiaVec[1], &objInvInertiaVec[2], &objInvInertiaVec[3]);
        // If the object is static we reset its mass to 0
        if (isStatic) {
            NewtonBodySetMassMatrix( priv->body, 0, 1, 1, 1 );
        }
    }
#endif
}

real PhyObject::mass() {
    return objInertiaVec[0];
}

void PhyObject::setOmega( const wVector& omega ) {
#ifdef WORLDSIM_USE_NEWTON
    NewtonBodySetOmega( priv->body, &omega[0] );
#endif
}

wVector PhyObject::omega() {
    wVector t;
#ifdef WORLDSIM_USE_NEWTON
    NewtonBodyGetOmega( priv->body, &t[0] );
#endif
    return t;
}

void PhyObject::setVelocity( const wVector& velocity ) {
#ifdef WORLDSIM_USE_NEWTON
    NewtonBodySetVelocity( priv->body, &velocity[0] );
#endif
}

wVector PhyObject::velocity() {
    wVector t;
#ifdef WORLDSIM_USE_NEWTON
    NewtonBodyGetVelocity( priv->body, &t[0] );
#endif
    return t;
}

void PhyObject::addImpulse( const wVector& pointDeltaVeloc, const wVector& pointPosit ) {
#ifdef WORLDSIM_USE_NEWTON
    NewtonBodyAddImpulse( priv->body, &pointDeltaVeloc[0], &pointPosit[0] );
#endif
}

void PhyObject::setMaterial( QString material ) {
    this->materialv = material;
#ifdef WORLDSIM_USE_NEWTON
    if ( worldv->priv->matIDs.contains( material ) ) {
        NewtonBodySetMaterialGroupID( priv->body, worldv->priv->matIDs[material] );
    }
#endif
}

QString PhyObject::material() const {
    return materialv;
}

void PhyObject::createPrivateObject() {
#ifdef WORLDSIM_USE_NEWTON
    NewtonCollision* c = NewtonCreateNull( worldpriv->world );
    wMatrix initialTransformationMatrix = wMatrix::identity(); // The transformation matrix is set in other places
    priv->body = NewtonCreateBody( worldpriv->world, c, &initialTransformationMatrix[0][0] );
    priv->collision = NewtonBodyGetCollision( priv->body );
    //NewtonReleaseCollision( worldpriv->world, c );
    NewtonBodySetAutoSleep( priv->body, 0 );
    NewtonBodySetMassMatrix( priv->body, 1.0, 1.0, 1.0, 1.0 );
    NewtonBodySetUserData( priv->body, this );
    // Sets the signal-wrappers callback
    NewtonBodySetTransformCallback( priv->body, (PhyObjectPrivate::setTransformHandler) );
    NewtonBodySetForceAndTorqueCallback( priv->body, (PhyObjectPrivate::applyForceAndTorqueHandler) );
#endif
}

} // end namespace farsa