== SoC Bullet - Bullet Upgrade to 2.76 ==
[blender.git] / extern / bullet2 / BulletDynamics / Dynamics / btRigidBody.cpp
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose, 
8 including commercial applications, and to alter it and redistribute it freely, 
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16 #include "btRigidBody.h"
17 #include "BulletCollision/CollisionShapes/btConvexShape.h"
18 #include "LinearMath/btMinMax.h"
19 #include "LinearMath/btTransformUtil.h"
20 #include "LinearMath/btMotionState.h"
21 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
22 #include "LinearMath/btSerializer.h"
23
24 //'temporarily' global variables
25 btScalar        gDeactivationTime = btScalar(2.);
26 bool    gDisableDeactivation = false;
27 static int uniqueId = 0;
28
29
30 btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
31 {
32         setupRigidBody(constructionInfo);
33 }
34
35 btRigidBody::btRigidBody(btScalar mass, btMotionState *motionState, btCollisionShape *collisionShape, const btVector3 &localInertia)
36 {
37         btRigidBodyConstructionInfo cinfo(mass,motionState,collisionShape,localInertia);
38         setupRigidBody(cinfo);
39 }
40
41 void    btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
42 {
43
44         m_internalType=CO_RIGID_BODY;
45
46         m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
47         m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
48         m_angularFactor.setValue(1,1,1);
49         m_linearFactor.setValue(1,1,1);
50         m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
51         m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
52         m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
53         m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
54         m_linearDamping = btScalar(0.);
55         m_angularDamping = btScalar(0.5);
56         m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold;
57         m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold;
58         m_optionalMotionState = constructionInfo.m_motionState;
59         m_contactSolverType = 0;
60         m_frictionSolverType = 0;
61         m_additionalDamping = constructionInfo.m_additionalDamping;
62         m_additionalDampingFactor = constructionInfo.m_additionalDampingFactor;
63         m_additionalLinearDampingThresholdSqr = constructionInfo.m_additionalLinearDampingThresholdSqr;
64         m_additionalAngularDampingThresholdSqr = constructionInfo.m_additionalAngularDampingThresholdSqr;
65         m_additionalAngularDampingFactor = constructionInfo.m_additionalAngularDampingFactor;
66
67         if (m_optionalMotionState)
68         {
69                 m_optionalMotionState->getWorldTransform(m_worldTransform);
70         } else
71         {
72                 m_worldTransform = constructionInfo.m_startWorldTransform;
73         }
74
75         m_interpolationWorldTransform = m_worldTransform;
76         m_interpolationLinearVelocity.setValue(0,0,0);
77         m_interpolationAngularVelocity.setValue(0,0,0);
78         
79         //moved to btCollisionObject
80         m_friction = constructionInfo.m_friction;
81         m_restitution = constructionInfo.m_restitution;
82
83         setCollisionShape( constructionInfo.m_collisionShape );
84         m_debugBodyId = uniqueId++;
85         
86         setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia);
87     setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping);
88         updateInertiaTensor();
89
90         m_rigidbodyFlags = 0;
91
92
93         m_deltaLinearVelocity.setZero();
94         m_deltaAngularVelocity.setZero();
95         m_invMass = m_inverseMass*m_linearFactor;
96         m_pushVelocity.setZero();
97         m_turnVelocity.setZero();
98
99         
100
101 }
102
103
104 void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) 
105 {
106         btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
107 }
108
109 void                    btRigidBody::saveKinematicState(btScalar timeStep)
110 {
111         //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
112         if (timeStep != btScalar(0.))
113         {
114                 //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
115                 if (getMotionState())
116                         getMotionState()->getWorldTransform(m_worldTransform);
117                 btVector3 linVel,angVel;
118                 
119                 btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,timeStep,m_linearVelocity,m_angularVelocity);
120                 m_interpolationLinearVelocity = m_linearVelocity;
121                 m_interpolationAngularVelocity = m_angularVelocity;
122                 m_interpolationWorldTransform = m_worldTransform;
123                 //printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ());
124         }
125 }
126         
127 void    btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const
128 {
129         getCollisionShape()->getAabb(m_worldTransform,aabbMin,aabbMax);
130 }
131
132
133
134
135 void btRigidBody::setGravity(const btVector3& acceleration) 
136 {
137         if (m_inverseMass != btScalar(0.0))
138         {
139                 m_gravity = acceleration * (btScalar(1.0) / m_inverseMass);
140         }
141         m_gravity_acceleration = acceleration;
142 }
143
144
145
146
147
148
149 void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
150 {
151         m_linearDamping = GEN_clamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
152         m_angularDamping = GEN_clamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
153 }
154
155
156
157
158 ///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping
159 void                    btRigidBody::applyDamping(btScalar timeStep)
160 {
161         //On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
162         //todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway
163
164 //#define USE_OLD_DAMPING_METHOD 1
165 #ifdef USE_OLD_DAMPING_METHOD
166         m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
167         m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
168 #else
169         m_linearVelocity *= btPow(btScalar(1)-m_linearDamping, timeStep);
170         m_angularVelocity *= btPow(btScalar(1)-m_angularDamping, timeStep);
171 #endif
172
173         if (m_additionalDamping)
174         {
175                 //Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
176                 //Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
177                 if ((m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr) &&
178                         (m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr))
179                 {
180                         m_angularVelocity *= m_additionalDampingFactor;
181                         m_linearVelocity *= m_additionalDampingFactor;
182                 }
183         
184
185                 btScalar speed = m_linearVelocity.length();
186                 if (speed < m_linearDamping)
187                 {
188                         btScalar dampVel = btScalar(0.005);
189                         if (speed > dampVel)
190                         {
191                                 btVector3 dir = m_linearVelocity.normalized();
192                                 m_linearVelocity -=  dir * dampVel;
193                         } else
194                         {
195                                 m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
196                         }
197                 }
198
199                 btScalar angSpeed = m_angularVelocity.length();
200                 if (angSpeed < m_angularDamping)
201                 {
202                         btScalar angDampVel = btScalar(0.005);
203                         if (angSpeed > angDampVel)
204                         {
205                                 btVector3 dir = m_angularVelocity.normalized();
206                                 m_angularVelocity -=  dir * angDampVel;
207                         } else
208                         {
209                                 m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
210                         }
211                 }
212         }
213 }
214
215
216 void btRigidBody::applyGravity()
217 {
218         if (isStaticOrKinematicObject())
219                 return;
220         
221         applyCentralForce(m_gravity);   
222
223 }
224
225 void btRigidBody::proceedToTransform(const btTransform& newTrans)
226 {
227         setCenterOfMassTransform( newTrans );
228 }
229         
230
231 void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
232 {
233         if (mass == btScalar(0.))
234         {
235                 m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT;
236                 m_inverseMass = btScalar(0.);
237         } else
238         {
239                 m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
240                 m_inverseMass = btScalar(1.0) / mass;
241         }
242         
243         m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0),
244                                    inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0),
245                                    inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0));
246
247         m_invMass = m_linearFactor*m_inverseMass;
248 }
249
250         
251
252 void btRigidBody::updateInertiaTensor() 
253 {
254         m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
255 }
256
257
258 void btRigidBody::integrateVelocities(btScalar step) 
259 {
260         if (isStaticOrKinematicObject())
261                 return;
262
263         m_linearVelocity += m_totalForce * (m_inverseMass * step);
264         m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
265
266 #define MAX_ANGVEL SIMD_HALF_PI
267         /// clamp angular velocity. collision calculations will fail on higher angular velocities       
268         btScalar angvel = m_angularVelocity.length();
269         if (angvel*step > MAX_ANGVEL)
270         {
271                 m_angularVelocity *= (MAX_ANGVEL/step) /angvel;
272         }
273
274 }
275
276 btQuaternion btRigidBody::getOrientation() const
277 {
278                 btQuaternion orn;
279                 m_worldTransform.getBasis().getRotation(orn);
280                 return orn;
281 }
282         
283         
284 void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
285 {
286
287         if (isStaticOrKinematicObject())
288         {
289                 m_interpolationWorldTransform = m_worldTransform;
290         } else
291         {
292                 m_interpolationWorldTransform = xform;
293         }
294         m_interpolationLinearVelocity = getLinearVelocity();
295         m_interpolationAngularVelocity = getAngularVelocity();
296         m_worldTransform = xform;
297         updateInertiaTensor();
298 }
299
300
301 bool btRigidBody::checkCollideWithOverride(btCollisionObject* co)
302 {
303         btRigidBody* otherRb = btRigidBody::upcast(co);
304         if (!otherRb)
305                 return true;
306
307         for (int i = 0; i < m_constraintRefs.size(); ++i)
308         {
309                 btTypedConstraint* c = m_constraintRefs[i];
310                 if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb)
311                         return false;
312         }
313
314         return true;
315 }
316
317 void    btRigidBody::internalWritebackVelocity(btScalar timeStep)
318 {
319     (void) timeStep;
320         if (m_inverseMass)
321         {
322                 setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity);
323                 setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity);
324                 
325                 //correct the position/orientation based on push/turn recovery
326                 btTransform newTransform;
327                 btTransformUtil::integrateTransform(getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
328                 setWorldTransform(newTransform);
329                 //m_originalBody->setCompanionId(-1);
330         }
331         m_deltaLinearVelocity.setZero();
332         m_deltaAngularVelocity .setZero();
333         m_pushVelocity.setZero();
334         m_turnVelocity.setZero();
335 }
336
337
338
339 void btRigidBody::addConstraintRef(btTypedConstraint* c)
340 {
341         int index = m_constraintRefs.findLinearSearch(c);
342         if (index == m_constraintRefs.size())
343                 m_constraintRefs.push_back(c); 
344
345         m_checkCollideWith = true;
346 }
347
348 void btRigidBody::removeConstraintRef(btTypedConstraint* c)
349 {
350         m_constraintRefs.remove(c);
351         m_checkCollideWith = m_constraintRefs.size() > 0;
352 }
353
354 int     btRigidBody::calculateSerializeBufferSize()     const
355 {
356         int sz = sizeof(btRigidBodyData);
357         return sz;
358 }
359
360         ///fills the dataBuffer and returns the struct name (and 0 on failure)
361 const char*     btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const
362 {
363         btRigidBodyData* rbd = (btRigidBodyData*) dataBuffer;
364
365         btCollisionObject::serialize(&rbd->m_collisionObjectData, serializer);
366
367         m_invInertiaTensorWorld.serialize(rbd->m_invInertiaTensorWorld);
368         m_linearVelocity.serialize(rbd->m_linearVelocity);
369         m_angularVelocity.serialize(rbd->m_angularVelocity);
370         rbd->m_inverseMass = m_inverseMass;
371         m_angularFactor.serialize(rbd->m_angularFactor);
372         m_linearFactor.serialize(rbd->m_linearFactor);
373         m_gravity.serialize(rbd->m_gravity);
374         m_gravity_acceleration.serialize(rbd->m_gravity_acceleration);
375         m_invInertiaLocal.serialize(rbd->m_invInertiaLocal);
376         m_totalForce.serialize(rbd->m_totalForce);
377         m_totalTorque.serialize(rbd->m_totalTorque);
378         rbd->m_linearDamping = m_linearDamping;
379         rbd->m_angularDamping = m_angularDamping;
380         rbd->m_additionalDamping = m_additionalDamping;
381         rbd->m_additionalDampingFactor = m_additionalDampingFactor;
382         rbd->m_additionalLinearDampingThresholdSqr = m_additionalLinearDampingThresholdSqr;
383         rbd->m_additionalAngularDampingThresholdSqr = m_additionalAngularDampingThresholdSqr;
384         rbd->m_additionalAngularDampingFactor = m_additionalAngularDampingFactor;
385         rbd->m_linearSleepingThreshold=m_linearSleepingThreshold;
386         rbd->m_angularSleepingThreshold = m_angularSleepingThreshold;
387
388         return btRigidBodyDataName;
389 }
390
391
392
393 void btRigidBody::serializeSingleObject(class btSerializer* serializer) const
394 {
395         btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(),1);
396         const char* structType = serialize(chunk->m_oldPtr, serializer);
397         serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,(void*)this);
398 }
399
400