Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/SuperOrxoBros_HS18/SuperOrxoBros_HS18/src/external/bullet/BulletDynamics/Dynamics/btRigidBody.cpp @ 12175

Last change on this file since 12175 was 12175, checked in by siramesh, 5 years ago

Super Orxo Bros (Sidharth Ramesh, Nisa Balta, Jeff Ren)

File size: 12.7 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    setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping);
55
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        updateInertiaTensor();
88
89        m_rigidbodyFlags = 0;
90
91
92        m_deltaLinearVelocity.setZero();
93        m_deltaAngularVelocity.setZero();
94        m_invMass = m_inverseMass*m_linearFactor;
95        m_pushVelocity.setZero();
96        m_turnVelocity.setZero();
97
98       
99
100}
101
102
103void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) 
104{
105        btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
106}
107
108void                    btRigidBody::saveKinematicState(btScalar timeStep)
109{
110        //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
111        if (timeStep != btScalar(0.))
112        {
113                //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
114                if (getMotionState())
115                        getMotionState()->getWorldTransform(m_worldTransform);
116                btVector3 linVel,angVel;
117               
118                btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,timeStep,m_linearVelocity,m_angularVelocity);
119                m_interpolationLinearVelocity = m_linearVelocity;
120                m_interpolationAngularVelocity = m_angularVelocity;
121                m_interpolationWorldTransform = m_worldTransform;
122                //printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ());
123        }
124}
125       
126void    btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const
127{
128        getCollisionShape()->getAabb(m_worldTransform,aabbMin,aabbMax);
129}
130
131
132
133
134void btRigidBody::setGravity(const btVector3& acceleration) 
135{
136        if (m_inverseMass != btScalar(0.0))
137        {
138                m_gravity = acceleration * (btScalar(1.0) / m_inverseMass);
139        }
140        m_gravity_acceleration = acceleration;
141}
142
143
144
145
146
147
148void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
149{
150        m_linearDamping = btClamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
151        m_angularDamping = btClamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
152}
153
154
155
156
157///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping
158void                    btRigidBody::applyDamping(btScalar timeStep)
159{
160        //On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
161        //todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway
162
163//#define USE_OLD_DAMPING_METHOD 1
164#ifdef USE_OLD_DAMPING_METHOD
165        m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
166        m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
167#else
168        m_linearVelocity *= btPow(btScalar(1)-m_linearDamping, timeStep);
169        m_angularVelocity *= btPow(btScalar(1)-m_angularDamping, timeStep);
170#endif
171
172        if (m_additionalDamping)
173        {
174                //Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
175                //Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
176                if ((m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr) &&
177                        (m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr))
178                {
179                        m_angularVelocity *= m_additionalDampingFactor;
180                        m_linearVelocity *= m_additionalDampingFactor;
181                }
182       
183
184                btScalar speed = m_linearVelocity.length();
185                if (speed < m_linearDamping)
186                {
187                        btScalar dampVel = btScalar(0.005);
188                        if (speed > dampVel)
189                        {
190                                btVector3 dir = m_linearVelocity.normalized();
191                                m_linearVelocity -=  dir * dampVel;
192                        } else
193                        {
194                                m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
195                        }
196                }
197
198                btScalar angSpeed = m_angularVelocity.length();
199                if (angSpeed < m_angularDamping)
200                {
201                        btScalar angDampVel = btScalar(0.005);
202                        if (angSpeed > angDampVel)
203                        {
204                                btVector3 dir = m_angularVelocity.normalized();
205                                m_angularVelocity -=  dir * angDampVel;
206                        } else
207                        {
208                                m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
209                        }
210                }
211        }
212}
213
214
215void btRigidBody::applyGravity()
216{
217        if (isStaticOrKinematicObject())
218                return;
219       
220        applyCentralForce(m_gravity);   
221
222}
223
224void btRigidBody::proceedToTransform(const btTransform& newTrans)
225{
226        setCenterOfMassTransform( newTrans );
227}
228       
229
230void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
231{
232        if (mass == btScalar(0.))
233        {
234                m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT;
235                m_inverseMass = btScalar(0.);
236        } else
237        {
238                m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
239                m_inverseMass = btScalar(1.0) / mass;
240        }
241
242        //Fg = m * a
243        m_gravity = mass * m_gravity_acceleration;
244       
245        m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0),
246                                   inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0),
247                                   inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0));
248
249        m_invMass = m_linearFactor*m_inverseMass;
250}
251
252       
253
254void btRigidBody::updateInertiaTensor() 
255{
256        m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
257}
258
259
260void btRigidBody::integrateVelocities(btScalar step) 
261{
262        if (isStaticOrKinematicObject())
263                return;
264
265        m_linearVelocity += m_totalForce * (m_inverseMass * step);
266        m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
267
268#define MAX_ANGVEL SIMD_HALF_PI
269        /// clamp angular velocity. collision calculations will fail on higher angular velocities       
270        btScalar angvel = m_angularVelocity.length();
271        if (angvel*step > MAX_ANGVEL)
272        {
273                m_angularVelocity *= (MAX_ANGVEL/step) /angvel;
274        }
275
276}
277
278btQuaternion btRigidBody::getOrientation() const
279{
280                btQuaternion orn;
281                m_worldTransform.getBasis().getRotation(orn);
282                return orn;
283}
284       
285       
286void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
287{
288
289        if (isStaticOrKinematicObject())
290        {
291                m_interpolationWorldTransform = m_worldTransform;
292        } else
293        {
294                m_interpolationWorldTransform = xform;
295        }
296        m_interpolationLinearVelocity = getLinearVelocity();
297        m_interpolationAngularVelocity = getAngularVelocity();
298        m_worldTransform = xform;
299        updateInertiaTensor();
300}
301
302
303bool btRigidBody::checkCollideWithOverride(btCollisionObject* co)
304{
305        btRigidBody* otherRb = btRigidBody::upcast(co);
306        if (!otherRb)
307                return true;
308
309        for (int i = 0; i < m_constraintRefs.size(); ++i)
310        {
311                btTypedConstraint* c = m_constraintRefs[i];
312                if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb)
313                        return false;
314        }
315
316        return true;
317}
318
319void    btRigidBody::internalWritebackVelocity(btScalar timeStep)
320{
321    (void) timeStep;
322        if (m_inverseMass)
323        {
324                setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity);
325                setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity);
326               
327                //correct the position/orientation based on push/turn recovery
328                btTransform newTransform;
329                btTransformUtil::integrateTransform(getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
330                setWorldTransform(newTransform);
331                //m_originalBody->setCompanionId(-1);
332        }
333//      m_deltaLinearVelocity.setZero();
334//      m_deltaAngularVelocity .setZero();
335//      m_pushVelocity.setZero();
336//      m_turnVelocity.setZero();
337}
338
339
340
341void btRigidBody::addConstraintRef(btTypedConstraint* c)
342{
343        int index = m_constraintRefs.findLinearSearch(c);
344        if (index == m_constraintRefs.size())
345                m_constraintRefs.push_back(c); 
346
347        m_checkCollideWith = true;
348}
349
350void btRigidBody::removeConstraintRef(btTypedConstraint* c)
351{
352        m_constraintRefs.remove(c);
353        m_checkCollideWith = m_constraintRefs.size() > 0;
354}
355
356int     btRigidBody::calculateSerializeBufferSize()     const
357{
358        int sz = sizeof(btRigidBodyData);
359        return sz;
360}
361
362        ///fills the dataBuffer and returns the struct name (and 0 on failure)
363const char*     btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const
364{
365        btRigidBodyData* rbd = (btRigidBodyData*) dataBuffer;
366
367        btCollisionObject::serialize(&rbd->m_collisionObjectData, serializer);
368
369        m_invInertiaTensorWorld.serialize(rbd->m_invInertiaTensorWorld);
370        m_linearVelocity.serialize(rbd->m_linearVelocity);
371        m_angularVelocity.serialize(rbd->m_angularVelocity);
372        rbd->m_inverseMass = m_inverseMass;
373        m_angularFactor.serialize(rbd->m_angularFactor);
374        m_linearFactor.serialize(rbd->m_linearFactor);
375        m_gravity.serialize(rbd->m_gravity);
376        m_gravity_acceleration.serialize(rbd->m_gravity_acceleration);
377        m_invInertiaLocal.serialize(rbd->m_invInertiaLocal);
378        m_totalForce.serialize(rbd->m_totalForce);
379        m_totalTorque.serialize(rbd->m_totalTorque);
380        rbd->m_linearDamping = m_linearDamping;
381        rbd->m_angularDamping = m_angularDamping;
382        rbd->m_additionalDamping = m_additionalDamping;
383        rbd->m_additionalDampingFactor = m_additionalDampingFactor;
384        rbd->m_additionalLinearDampingThresholdSqr = m_additionalLinearDampingThresholdSqr;
385        rbd->m_additionalAngularDampingThresholdSqr = m_additionalAngularDampingThresholdSqr;
386        rbd->m_additionalAngularDampingFactor = m_additionalAngularDampingFactor;
387        rbd->m_linearSleepingThreshold=m_linearSleepingThreshold;
388        rbd->m_angularSleepingThreshold = m_angularSleepingThreshold;
389
390        return btRigidBodyDataName;
391}
392
393
394
395void btRigidBody::serializeSingleObject(class btSerializer* serializer) const
396{
397        btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(),1);
398        const char* structType = serialize(chunk->m_oldPtr, serializer);
399        serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,(void*)this);
400}
401
402
Note: See TracBrowser for help on using the repository browser.