WorldSim - 1.2.3

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