WorldSim - 1.2.3

Utilities Configuration GA
NNFW WorldSim Total 99% Experiments
phycompoundobject.cpp
1 /********************************************************************************
2  * WorldSim -- library for robot simulations *
3  * Copyright (C) 2008-2011 Gianluca Massera <emmegian@yahoo.it> *
4  * *
5  * This program is free software; you can redistribute it and/or modify *
6  * it under the terms of the GNU General Public License as published by *
7  * the Free Software Foundation; either version 2 of the License, or *
8  * (at your option) any later version. *
9  * *
10  * This program is distributed in the hope that it will be useful, *
11  * but WITHOUT ANY WARRANTY; without even the implied warranty of *
12  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the *
13  * GNU General Public License for more details. *
14  * *
15  * You should have received a copy of the GNU General Public License *
16  * along with this program; if not, write to the Free Software *
17  * Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA 02110-1301 USA *
18  ********************************************************************************/
19 
20 #include "phycompoundobject.h"
21 #include "private/phyobjectprivate.h"
22 #include "private/worldprivate.h"
23 #include <QString>
24 
25 namespace farsa {
26 
27 PhyCompoundObject::PhyCompoundObject( QVector<PhyObject*> objs, World* w, QString name, const wMatrix& tm )
28  : PhyObject( w, name, tm, false ) {
29  bodyv = objs;
30  for( int i=0; i<bodyv.size(); i++ ) {
31  bodyv[i]->setOwner(this, true);
32  }
33  w->pushObject( this );
34  createPrivateObject();
35  changedMatrix();
36 }
37 
38 PhyCompoundObject::~PhyCompoundObject() {
39  // Owned objects are deleted by Ownable destructor
40 #ifdef WORLDSIM_USE_NEWTON
41  /* nothing to do */
42 #endif
43 }
44 
45 #ifdef WORLDSIM_USE_NEWTON
46 NewtonCollision* copyCollisionHelper( NewtonWorld* world, NewtonCollision* toCopy, const wMatrix& offsetMatrix ) {
47  NewtonCollisionInfoRecord collisionInfo;
48  NewtonCollisionGetInfo( toCopy, &collisionInfo );
49  switch( collisionInfo.m_collisionType ) {
50  case SERIALIZE_ID_BOX:
51  return NewtonCreateBox( world, collisionInfo.m_box.m_x, collisionInfo.m_box.m_y, collisionInfo.m_box.m_z, 1, &offsetMatrix[0][0] );
52  case SERIALIZE_ID_CONE:
53  return NewtonCreateCone( world, collisionInfo.m_cone.m_r, collisionInfo.m_cone.m_height, 1, &offsetMatrix[0][0] );
54  case SERIALIZE_ID_SPHERE:
55  return NewtonCreateSphere( world, collisionInfo.m_sphere.m_r0, collisionInfo.m_sphere.m_r1, collisionInfo.m_sphere.m_r2, 1, &offsetMatrix[0][0] );
56  case SERIALIZE_ID_CAPSULE:
57  return NewtonCreateCapsule( world, collisionInfo.m_capsule.m_r0, collisionInfo.m_capsule.m_height, 1, &offsetMatrix[0][0] );
58  case SERIALIZE_ID_CYLINDER:
59  return NewtonCreateCylinder( world, collisionInfo.m_cylinder.m_r0, collisionInfo.m_cylinder.m_height, 1, &offsetMatrix[0][0] );
60  default:
61  qFatal( "A Primitive not supported by copyCollisionHelper has been used in creating a CompoundObject" );
62  return NULL;
63  }
64  return NULL;
65 }
66 #endif
67 
68 void PhyCompoundObject::createPrivateObject() {
69 #ifdef WORLDSIM_USE_NEWTON
70  int dim = bodyv.size();
71  NewtonCollision** temp = new NewtonCollision*[dim];
72  real mass = 0.0;
73  for( int i=0; i<dim; i++ ) {
74  temp[i] = copyCollisionHelper( worldpriv->world, bodyv[i]->priv->collision, bodyv[i]->matrix() );
75  mass += bodyv[i]->mass();
76  world()->popObject( bodyv[i] );
77  }
78  NewtonCollision* c = NewtonCreateCompoundCollision( worldpriv->world, dim, temp, 1 );
79  wMatrix initialTransformationMatrix = wMatrix::identity(); // The transformation matrix is set in other places
80  priv->body = NewtonCreateBody( worldpriv->world, c, &initialTransformationMatrix[0][0] );
81  priv->collision = c;
82  for( int i=0; i<dim; i++ ) {
83  NewtonDestroyBody( worldpriv->world, bodyv[i]->priv->body );
84  NewtonReleaseCollision( worldpriv->world, bodyv[i]->priv->collision );
85  bodyv[i]->priv->body = NULL;
86  bodyv[i]->priv->collision = NULL;
87  NewtonReleaseCollision( worldpriv->world, temp[i] );
88  }
89  delete []temp;
90  NewtonBodySetAutoSleep( priv->body, 0 );
91  setMass( mass );
92  NewtonBodySetUserData( priv->body, this );
93  NewtonBodySetLinearDamping( priv->body, 0.0 );
94  wVector zero = wVector(0,0,0,0);
95  NewtonBodySetAngularDamping( priv->body, &zero[0] );
96  NewtonBodySetAutoSleep( priv->body, 0 );
97  NewtonBodySetFreezeState( priv->body, 0 );
98  // Sets the signal-wrappers callback
99  NewtonBodySetTransformCallback( priv->body, (PhyObjectPrivate::setTransformHandler) );
100  NewtonBodySetForceAndTorqueCallback( priv->body, (PhyObjectPrivate::applyForceAndTorqueHandler) );
101 #endif
102 }
103 
104 } // end namespace farsa