Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/kicklib/src/external/bullet/BulletDynamics/Dynamics/btRigidBody.cpp @ 8474

Last change on this file since 8474 was 7983, checked in by rgrieder, 14 years ago

Updated Bullet Physics Engine from v2.74 to v2.77
  • Property svn:eol-style set to native
File size: 12.8 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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
25btScalar        gDeactivationTime = btScalar(2.);
26bool    gDisableDeactivation = false;
27static int uniqueId = 0;
28
29
30btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
31{
32        setupRigidBody(constructionInfo);
33}
34
35btRigidBody::btRigidBody(btScalar mass, btMotionState *motionState, btCollisionShape *collisionShape, const btVector3 &localInertia)
36{
37        btRigidBodyConstructionInfo cinfo(mass,motionState,collisionShape,localInertia);
38        setupRigidBody(cinfo);
39}
40
41void    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
104void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) 
105{
106        btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
107}
108
109void                    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       
127void    btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const
128{
129        getCollisionShape()->getAabb(m_worldTransform,aabbMin,aabbMax);
130}
131
132
133
134
135void 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
149void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
150{
151        m_linearDamping = btClamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
152        m_angularDamping = btClamped(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
159void                    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
216void btRigidBody::applyGravity()
217{
218        if (isStaticOrKinematicObject())
219                return;
220       
221        applyCentralForce(m_gravity);   
222
223}
224
225void btRigidBody::proceedToTransform(const btTransform& newTrans)
226{
227        setCenterOfMassTransform( newTrans );
228}
229       
230
231void 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        //Fg = m * a
244        m_gravity = mass * m_gravity_acceleration;
245       
246        m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0),
247                                   inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0),
248                                   inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0));
249
250        m_invMass = m_linearFactor*m_inverseMass;
251}
252
253       
254
255void btRigidBody::updateInertiaTensor() 
256{
257        m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
258}
259
260
261void btRigidBody::integrateVelocities(btScalar step) 
262{
263        if (isStaticOrKinematicObject())
264                return;
265
266        m_linearVelocity += m_totalForce * (m_inverseMass * step);
267        m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
268
269#define MAX_ANGVEL SIMD_HALF_PI
270        /// clamp angular velocity. collision calculations will fail on higher angular velocities       
271        btScalar angvel = m_angularVelocity.length();
272        if (angvel*step > MAX_ANGVEL)
273        {
274                m_angularVelocity *= (MAX_ANGVEL/step) /angvel;
275        }
276
277}
278
279btQuaternion btRigidBody::getOrientation() const
280{
281                btQuaternion orn;
282                m_worldTransform.getBasis().getRotation(orn);
283                return orn;
284}
285       
286       
287void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
288{
289
290        if (isStaticOrKinematicObject())
291        {
292                m_interpolationWorldTransform = m_worldTransform;
293        } else
294        {
295                m_interpolationWorldTransform = xform;
296        }
297        m_interpolationLinearVelocity = getLinearVelocity();
298        m_interpolationAngularVelocity = getAngularVelocity();
299        m_worldTransform = xform;
300        updateInertiaTensor();
301}
302
303
304bool btRigidBody::checkCollideWithOverride(btCollisionObject* co)
305{
306        btRigidBody* otherRb = btRigidBody::upcast(co);
307        if (!otherRb)
308                return true;
309
310        for (int i = 0; i < m_constraintRefs.size(); ++i)
311        {
312                btTypedConstraint* c = m_constraintRefs[i];
313                if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb)
314                        return false;
315        }
316
317        return true;
318}
319
320void    btRigidBody::internalWritebackVelocity(btScalar timeStep)
321{
322    (void) timeStep;
323        if (m_inverseMass)
324        {
325                setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity);
326                setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity);
327               
328                //correct the position/orientation based on push/turn recovery
329                btTransform newTransform;
330                btTransformUtil::integrateTransform(getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
331                setWorldTransform(newTransform);
332                //m_originalBody->setCompanionId(-1);
333        }
334//      m_deltaLinearVelocity.setZero();
335//      m_deltaAngularVelocity .setZero();
336//      m_pushVelocity.setZero();
337//      m_turnVelocity.setZero();
338}
339
340
341
342void btRigidBody::addConstraintRef(btTypedConstraint* c)
343{
344        int index = m_constraintRefs.findLinearSearch(c);
345        if (index == m_constraintRefs.size())
346                m_constraintRefs.push_back(c); 
347
348        m_checkCollideWith = true;
349}
350
351void btRigidBody::removeConstraintRef(btTypedConstraint* c)
352{
353        m_constraintRefs.remove(c);
354        m_checkCollideWith = m_constraintRefs.size() > 0;
355}
356
357int     btRigidBody::calculateSerializeBufferSize()     const
358{
359        int sz = sizeof(btRigidBodyData);
360        return sz;
361}
362
363        ///fills the dataBuffer and returns the struct name (and 0 on failure)
364const char*     btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const
365{
366        btRigidBodyData* rbd = (btRigidBodyData*) dataBuffer;
367
368        btCollisionObject::serialize(&rbd->m_collisionObjectData, serializer);
369
370        m_invInertiaTensorWorld.serialize(rbd->m_invInertiaTensorWorld);
371        m_linearVelocity.serialize(rbd->m_linearVelocity);
372        m_angularVelocity.serialize(rbd->m_angularVelocity);
373        rbd->m_inverseMass = m_inverseMass;
374        m_angularFactor.serialize(rbd->m_angularFactor);
375        m_linearFactor.serialize(rbd->m_linearFactor);
376        m_gravity.serialize(rbd->m_gravity);
377        m_gravity_acceleration.serialize(rbd->m_gravity_acceleration);
378        m_invInertiaLocal.serialize(rbd->m_invInertiaLocal);
379        m_totalForce.serialize(rbd->m_totalForce);
380        m_totalTorque.serialize(rbd->m_totalTorque);
381        rbd->m_linearDamping = m_linearDamping;
382        rbd->m_angularDamping = m_angularDamping;
383        rbd->m_additionalDamping = m_additionalDamping;
384        rbd->m_additionalDampingFactor = m_additionalDampingFactor;
385        rbd->m_additionalLinearDampingThresholdSqr = m_additionalLinearDampingThresholdSqr;
386        rbd->m_additionalAngularDampingThresholdSqr = m_additionalAngularDampingThresholdSqr;
387        rbd->m_additionalAngularDampingFactor = m_additionalAngularDampingFactor;
388        rbd->m_linearSleepingThreshold=m_linearSleepingThreshold;
389        rbd->m_angularSleepingThreshold = m_angularSleepingThreshold;
390
391        return btRigidBodyDataName;
392}
393
394
395
396void btRigidBody::serializeSingleObject(class btSerializer* serializer) const
397{
398        btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(),1);
399        const char* structType = serialize(chunk->m_oldPtr, serializer);
400        serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,(void*)this);
401}
402
403
Note: See TracBrowser for help on using the repository browser.