WorldSim - 1.4.5

Utilities Configuration GA
NNFW WorldSim Total 99% Experiments
phyobject.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 "phyobject.h"
21 #include "private/phyobjectprivate.h"
22 #include "private/worldprivate.h"
23 
24 namespace farsa {
25 
26 PhyObject::PhyObject( World* w, QString name, const wMatrix& tm, bool createPrivate )
27  : WObject( w, name, tm, false ), materialv("default") {
28 
29  priv = new PhyObjectPrivate();
30  worldpriv = w->priv;
31  if ( createPrivate ) {
32  w->pushObject( this );
33  createPrivateObject();
34  changedMatrix();
35  }
36 
37  forceAcc = wVector(0,0,0);
38  torqueAcc = wVector(0,0,0);
39  isKinematic = false;
40  isKinematicCollidable = false;
41  isStatic = false;
42  objInertiaVec = wVector(0.0, 1.0, 1.0, 1.0);
43  objInvInertiaVec = wVector(0.0, 1.0, 1.0, 1.0);
44 }
45 
46 PhyObject::~PhyObject() {
47 #ifdef WORLDSIM_USE_NEWTON
48  if ( priv->body != NULL ) {
49  NewtonDestroyBody( worldpriv->world, priv->body );
50  NewtonReleaseCollision( worldpriv->world, priv->collision );
51  }
52 #endif
53  delete priv;
54 }
55 
56 void PhyObject::setKinematic(bool b, bool c)
57 {
58  //if (isKinematic == b) return;
59  isKinematic = b;
60 #ifdef WORLDSIM_USE_NEWTON
61  if (b) {
62  // Unsets the signal-wrappers callback and collision shape
63  NewtonBodySetTransformCallback( priv->body, 0 );
64  NewtonBodySetForceAndTorqueCallback( priv->body, 0 );
65  //NewtonBodySetFreezeState( priv->body, 1 ); <== this will block also any body jointed to this one
66 // //--- Queste tre righe mettono un NullCollision all'oggetto fisico,
67 // // e quindi tutte le collisioni vengono totalmente ignorarte da NGD
68 // //--- Un alternativa e' commentare queste righe, ed impostare la massa a 0
69 // // in questo modo, l'oggetto risultera' statico per NGD, e generera' collisioni
70 // // con gli altri oggetti. Cosi' facendo anche muovendo l'oggetto in modo
71 // // cinematico potra' cmq collidere e spostare oggetti dinamici.
72 // // Pero', cmq, gli passera' attraverso come se fossero trasparenti, quindi
73 // // movimenti veloci risulteranno 'sbagliati' dal punto di vista fisico
74 // NewtonCollision* nullC = NewtonCreateNull( worldpriv->world );
75 // NewtonBodySetCollision( priv->body, nullC );
76 // NewtonReleaseCollision( worldpriv->world, nullC );
77 // //--- soluzione alternativa: NewtonBodySetMassMatrix( priv->body, 0, 1, 1, 1 );
78  isKinematicCollidable = c;
79  if (isKinematicCollidable) {
80  // Here we set mass to 0. This way the object will be static for NGD and will
81  // collide with other solids, influencing them without being influenced.
82  NewtonBodySetMassMatrix( priv->body, 0, 1, 1, 1 );
83  } else {
84  // The following lines set a NullCollision for the physical object, so that
85  // all collisions are completely ignored by Newton Game Dynamics
86  NewtonCollision* nullC = NewtonCreateNull( worldpriv->world );
87  NewtonBodySetCollision( priv->body, nullC );
88  NewtonReleaseCollision( worldpriv->world, nullC );
89  }
90  } else {
91  // Sets the signal-wrappers callback and collision shape
92  NewtonBodySetTransformCallback( priv->body, (PhyObjectPrivate::setTransformHandler) );
93  NewtonBodySetForceAndTorqueCallback( priv->body, (PhyObjectPrivate::applyForceAndTorqueHandler) );
94  NewtonBodySetFreezeState( priv->body, 0 );
95  NewtonBodySetCollision( priv->body, priv->collision );
96  NewtonBodySetMatrix( priv->body, &tm[0][0] );
97  // Restoring mass and inertia
98  if (isStatic) {
99  NewtonBodySetMassMatrix( priv->body, 0.0, 1.0, 1.0, 1.0 );
100  } else {
101  NewtonBodySetMassMatrix( priv->body, objInertiaVec[0], objInertiaVec[1], objInertiaVec[2], objInertiaVec[3] );
102  }
103  }
104 #endif
105 }
106 
107 void PhyObject::setStatic(bool b)
108 {
109  if (isStatic == b) return;
110  isStatic = b;
111 #ifdef WORLDSIM_USE_NEWTON
112  if (b) {
113  NewtonBodySetMassMatrix( priv->body, 0.0, 1.0, 1.0, 1.0 );
114  } else {
115  NewtonBodySetMassMatrix( priv->body, objInertiaVec[0], objInertiaVec[1], objInertiaVec[2], objInertiaVec[3] );
116  }
117 #endif
118 }
119 
120 void PhyObject::reset() {
121 #ifdef WORLDSIM_USE_NEWTON
122  wVector zero = wVector(0,0,0);
123  forceAcc = zero;
124  torqueAcc= zero;
125  NewtonBodySetForce( priv->body, &forceAcc[0] );
126  NewtonBodySetTorque( priv->body, &torqueAcc[0] );
127  NewtonBodySetOmega( priv->body, &zero[0] );
128  NewtonBodySetVelocity( priv->body, &zero[0] );
129  //--- remove any contact involving this object
130  NewtonWorld* nworld = NewtonBodyGetWorld( priv->body );
131  NewtonWorldCriticalSectionLock( nworld );
132  for( NewtonJoint* contactList = NewtonBodyGetFirstContactJoint( priv->body ); contactList;
133  contactList = NewtonBodyGetNextContactJoint( priv->body, contactList ) ) {
134  void* nextContact;
135  for( void* contact = NewtonContactJointGetFirstContact(contactList); contact; contact = nextContact ) {
136  //--- before removing contact, It get the nextContact in the list
137  nextContact = NewtonContactJointGetNextContact( contactList, contact );
138  NewtonContactJointRemoveContact( contactList, contact );
139  }
140  }
141  NewtonWorldCriticalSectionUnlock( nworld );
142 #endif
143 }
144 
145 void PhyObject::changedMatrix() {
146 #ifdef WORLDSIM_USE_NEWTON
147  //qDebug() << "SYNC POSITION" << tm[3][0] << tm[3][1] << tm[3][2];
148  NewtonBodySetMatrix( priv->body, &tm[0][0] );
149 #endif
150 }
151 
152 void PhyObject::addForce( const wVector& force ) {
153  forceAcc += force;
154 }
155 
156 void PhyObject::setForce( const wVector& force) {
157  forceAcc = force;
158 }
159 
160 wVector PhyObject::force() {
161  wVector f;
162 #ifdef WORLDSIM_USE_NEWTON
163  NewtonBodyGetForce( priv->body, &f[0] );
164 #endif
165  return f;
166 }
167 
168 void PhyObject::addTorque( const wVector& torque ) {
169  torqueAcc += torque;
170 }
171 
172 void PhyObject::setTorque( const wVector& torque ) {
173  torqueAcc = torque;
174 }
175 
176 wVector PhyObject::torque() {
177  wVector t;
178 #ifdef WORLDSIM_USE_NEWTON
179  NewtonBodyGetTorque( priv->body, &t[0] );
180 #endif
181  return t;
182 }
183 
184 void PhyObject::setMassInertiaVec( const wVector& mi ) {
185  objInertiaVec = mi;
186 #ifdef WORLDSIM_USE_NEWTON
187  // Setting mass and getting the inverse inertia matrix
188  NewtonBodySetMassMatrix( priv->body, mi[0], mi[1], mi[2], mi[3] );
189  NewtonBodyGetInvMass( priv->body, &objInvInertiaVec[0], &objInvInertiaVec[1], &objInvInertiaVec[2], &objInvInertiaVec[3]);
190 #endif
191 }
192 
193 wVector PhyObject::massInertiaVec() const {
194  return objInertiaVec;
195 }
196 
197 wVector PhyObject::inertiaVec() const {
198  wVector mi;
199  mi[0] = objInertiaVec[1];
200  mi[1] = objInertiaVec[2];
201  mi[2] = objInertiaVec[3];
202  mi[3] = 0.0;
203  return mi;
204 }
205 
206 wVector PhyObject::invMassInertiaVec() const {
207  return objInvInertiaVec;
208 }
209 
210 wVector PhyObject::invInertiaVec() const {
211  wVector mi;
212  mi[0] = objInvInertiaVec[1];
213  mi[1] = objInvInertiaVec[2];
214  mi[2] = objInvInertiaVec[3];
215  mi[3] = 0.0;
216  return mi;
217 }
218 
219 void PhyObject::setMass( real newmass ) {
220 #ifdef WORLDSIM_USE_NEWTON
221  if ( newmass < 0.0001 ) {
222  objInertiaVec = wVector(0.0, 1.0, 1.0, 1.0);
223  objInvInertiaVec = wVector(0.0, 1.0, 1.0, 1.0);
224  isStatic = true;
225  NewtonBodySetMassMatrix( priv->body, 0, 1, 1, 1 );
226  } else {
227  real inertia[3];
228  real centre[3] = { 0, 0, 0 };
229  NewtonConvexCollisionCalculateInertialMatrix( priv->collision, inertia, centre );
230 
231  objInertiaVec = wVector(newmass, newmass*inertia[0], newmass*inertia[1], newmass*inertia[2]);
232  NewtonBodySetMassMatrix( priv->body, objInertiaVec[0], objInertiaVec[1], objInertiaVec[2], objInertiaVec[3] );
233  // Saving the inverse inertia matrix
234  NewtonBodyGetInvMass( priv->body, &objInvInertiaVec[0], &objInvInertiaVec[1], &objInvInertiaVec[2], &objInvInertiaVec[3]);
235  // If the object is static we reset its mass to 0
236  if (isStatic) {
237  NewtonBodySetMassMatrix( priv->body, 0, 1, 1, 1 );
238  }
239  }
240 #endif
241 }
242 
243 real PhyObject::mass() {
244  return objInertiaVec[0];
245 }
246 
247 void PhyObject::setOmega( const wVector& omega ) {
248 #ifdef WORLDSIM_USE_NEWTON
249  NewtonBodySetOmega( priv->body, &omega[0] );
250 #endif
251 }
252 
253 wVector PhyObject::omega() {
254  wVector t;
255 #ifdef WORLDSIM_USE_NEWTON
256  NewtonBodyGetOmega( priv->body, &t[0] );
257 #endif
258  return t;
259 }
260 
261 void PhyObject::setVelocity( const wVector& velocity ) {
262 #ifdef WORLDSIM_USE_NEWTON
263  NewtonBodySetVelocity( priv->body, &velocity[0] );
264 #endif
265 }
266 
267 wVector PhyObject::velocity() {
268  wVector t;
269 #ifdef WORLDSIM_USE_NEWTON
270  NewtonBodyGetVelocity( priv->body, &t[0] );
271 #endif
272  return t;
273 }
274 
275 void PhyObject::addImpulse( const wVector& pointDeltaVeloc, const wVector& pointPosit ) {
276 #ifdef WORLDSIM_USE_NEWTON
277  NewtonBodyAddImpulse( priv->body, &pointDeltaVeloc[0], &pointPosit[0] );
278 #endif
279 }
280 
281 void PhyObject::setMaterial( QString material ) {
282  this->materialv = material;
283 #ifdef WORLDSIM_USE_NEWTON
284  if ( worldv->priv->matIDs.contains( material ) ) {
285  NewtonBodySetMaterialGroupID( priv->body, worldv->priv->matIDs[material] );
286  }
287 #endif
288 }
289 
290 QString PhyObject::material() const {
291  return materialv;
292 }
293 
294 void PhyObject::createPrivateObject() {
295 #ifdef WORLDSIM_USE_NEWTON
296  NewtonCollision* c = NewtonCreateNull( worldpriv->world );
297  wMatrix initialTransformationMatrix = wMatrix::identity(); // The transformation matrix is set in other places
298  priv->body = NewtonCreateBody( worldpriv->world, c, &initialTransformationMatrix[0][0] );
299  priv->collision = NewtonBodyGetCollision( priv->body );
300  //NewtonReleaseCollision( worldpriv->world, c );
301  NewtonBodySetAutoSleep( priv->body, 0 );
302  NewtonBodySetMassMatrix( priv->body, 1.0, 1.0, 1.0, 1.0 );
303  NewtonBodySetUserData( priv->body, this );
304  // Sets the signal-wrappers callback
305  NewtonBodySetTransformCallback( priv->body, (PhyObjectPrivate::setTransformHandler) );
306  NewtonBodySetForceAndTorqueCallback( priv->body, (PhyObjectPrivate::applyForceAndTorqueHandler) );
307 #endif
308 }
309 
310 } // end namespace farsa