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

namespace farsa {

PhyCompoundObject::PhyCompoundObject( QVector<PhyObject*> objs, World* w, QString name, const wMatrix& tm )
    : PhyObject( w, name, tm, false ) {
    bodyv = objs;
    for( int i=0; i<bodyv.size(); i++ ) {
        bodyv[i]->setOwner(this, true);
    }
    w->pushObject( this );
    createPrivateObject();
    changedMatrix();
}

PhyCompoundObject::~PhyCompoundObject() {
    // Owned objects are deleted by Ownable destructor
#ifdef WORLDSIM_USE_NEWTON
    /* nothing to do */
#endif
}

#ifdef WORLDSIM_USE_NEWTON
NewtonCollision* copyCollisionHelper( NewtonWorld* world, NewtonCollision* toCopy, const wMatrix& offsetMatrix ) {
    NewtonCollisionInfoRecord collisionInfo;
    NewtonCollisionGetInfo( toCopy, &collisionInfo );
    switch( collisionInfo.m_collisionType ) {
    case SERIALIZE_ID_BOX:
        return NewtonCreateBox( world, collisionInfo.m_box.m_x, collisionInfo.m_box.m_y, collisionInfo.m_box.m_z, 1, &offsetMatrix[0][0] );
    case SERIALIZE_ID_CONE:
        return NewtonCreateCone( world, collisionInfo.m_cone.m_r, collisionInfo.m_cone.m_height, 1, &offsetMatrix[0][0] );
    case SERIALIZE_ID_SPHERE:
        return NewtonCreateSphere( world, collisionInfo.m_sphere.m_r0, collisionInfo.m_sphere.m_r1, collisionInfo.m_sphere.m_r2, 1, &offsetMatrix[0][0] );
    case SERIALIZE_ID_CAPSULE:
        return NewtonCreateCapsule( world, collisionInfo.m_capsule.m_r0, collisionInfo.m_capsule.m_height, 1, &offsetMatrix[0][0] );
    case SERIALIZE_ID_CYLINDER:
        return NewtonCreateCylinder( world, collisionInfo.m_cylinder.m_r0, collisionInfo.m_cylinder.m_height, 1, &offsetMatrix[0][0] );
    default:
        qFatal( "A Primitive not supported by copyCollisionHelper has been used in creating a CompoundObject" );
        return NULL;
    }
    return NULL;
}
#endif

void PhyCompoundObject::createPrivateObject() {
#ifdef WORLDSIM_USE_NEWTON
    int dim = bodyv.size();
    NewtonCollision** temp = new NewtonCollision*[dim];
    real mass = 0.0;
    for( int i=0; i<dim; i++ ) {
        temp[i] = copyCollisionHelper( worldpriv->world, bodyv[i]->priv->collision, bodyv[i]->matrix() );
        mass += bodyv[i]->mass();
        world()->popObject( bodyv[i] );
    }
    NewtonCollision* c = NewtonCreateCompoundCollision( worldpriv->world, dim, temp, 1 );
    wMatrix initialTransformationMatrix = wMatrix::identity(); // The transformation matrix is set in other places
    priv->body = NewtonCreateBody( worldpriv->world, c, &initialTransformationMatrix[0][0] );
    priv->collision = c;
    for( int i=0; i<dim; i++ ) {
        NewtonDestroyBody( worldpriv->world, bodyv[i]->priv->body );
        NewtonReleaseCollision( worldpriv->world, bodyv[i]->priv->collision );
        bodyv[i]->priv->body = NULL;
        bodyv[i]->priv->collision = NULL;
        NewtonReleaseCollision( worldpriv->world, temp[i] );
    }
    delete []temp;
    NewtonBodySetAutoSleep( priv->body, 0 );
    setMass( mass );
    NewtonBodySetUserData( priv->body, this );
    NewtonBodySetLinearDamping( priv->body, 0.0 );
    wVector zero = wVector(0,0,0,0);
    NewtonBodySetAngularDamping( priv->body, &zero[0] );
    NewtonBodySetAutoSleep( priv->body, 0 );
    NewtonBodySetFreezeState( priv->body, 0 );
    // Sets the signal-wrappers callback
    NewtonBodySetTransformCallback( priv->body, (PhyObjectPrivate::setTransformHandler) );
    NewtonBodySetForceAndTorqueCallback( priv->body, (PhyObjectPrivate::applyForceAndTorqueHandler) );
#endif
}

} // end namespace farsa