Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/physics/src/bullet/BulletDynamics/Dynamics/btRigidBody.cpp @ 1963

Last change on this file since 1963 was 1963, checked in by rgrieder, 15 years ago

Added Bullet physics engine.

  • Property svn:eol-style set to native
File size: 9.9 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
23//'temporarily' global variables
24btScalar        gDeactivationTime = btScalar(2.);
25bool    gDisableDeactivation = false;
26static int uniqueId = 0;
27
28
29btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
30{
31        setupRigidBody(constructionInfo);
32}
33
34btRigidBody::btRigidBody(btScalar mass, btMotionState *motionState, btCollisionShape *collisionShape, const btVector3 &localInertia)
35{
36        btRigidBodyConstructionInfo cinfo(mass,motionState,collisionShape,localInertia);
37        setupRigidBody(cinfo);
38}
39
40void    btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
41{
42
43        m_internalType=CO_RIGID_BODY;
44
45        m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
46        m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
47        m_angularFactor = btScalar(1.);
48        m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
49        m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
50        m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
51        m_linearDamping = btScalar(0.);
52        m_angularDamping = btScalar(0.5);
53        m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold;
54        m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold;
55        m_optionalMotionState = constructionInfo.m_motionState;
56        m_contactSolverType = 0;
57        m_frictionSolverType = 0;
58        m_additionalDamping = constructionInfo.m_additionalDamping;
59        m_additionalDampingFactor = constructionInfo.m_additionalDampingFactor;
60        m_additionalLinearDampingThresholdSqr = constructionInfo.m_additionalLinearDampingThresholdSqr;
61        m_additionalAngularDampingThresholdSqr = constructionInfo.m_additionalAngularDampingThresholdSqr;
62        m_additionalAngularDampingFactor = constructionInfo.m_additionalAngularDampingFactor;
63
64        if (m_optionalMotionState)
65        {
66                m_optionalMotionState->getWorldTransform(m_worldTransform);
67        } else
68        {
69                m_worldTransform = constructionInfo.m_startWorldTransform;
70        }
71
72        m_interpolationWorldTransform = m_worldTransform;
73        m_interpolationLinearVelocity.setValue(0,0,0);
74        m_interpolationAngularVelocity.setValue(0,0,0);
75       
76        //moved to btCollisionObject
77        m_friction = constructionInfo.m_friction;
78        m_restitution = constructionInfo.m_restitution;
79
80        setCollisionShape( constructionInfo.m_collisionShape );
81        m_debugBodyId = uniqueId++;
82       
83        setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia);
84    setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping);
85        updateInertiaTensor();
86
87}
88
89
90void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) 
91{
92        btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
93}
94
95void                    btRigidBody::saveKinematicState(btScalar timeStep)
96{
97        //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
98        if (timeStep != btScalar(0.))
99        {
100                //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
101                if (getMotionState())
102                        getMotionState()->getWorldTransform(m_worldTransform);
103                btVector3 linVel,angVel;
104               
105                btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,timeStep,m_linearVelocity,m_angularVelocity);
106                m_interpolationLinearVelocity = m_linearVelocity;
107                m_interpolationAngularVelocity = m_angularVelocity;
108                m_interpolationWorldTransform = m_worldTransform;
109                //printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ());
110        }
111}
112       
113void    btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const
114{
115        getCollisionShape()->getAabb(m_worldTransform,aabbMin,aabbMax);
116}
117
118
119
120
121void btRigidBody::setGravity(const btVector3& acceleration) 
122{
123        if (m_inverseMass != btScalar(0.0))
124        {
125                m_gravity = acceleration * (btScalar(1.0) / m_inverseMass);
126        }
127}
128
129
130
131
132
133
134void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
135{
136        m_linearDamping = GEN_clamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
137        m_angularDamping = GEN_clamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
138}
139
140
141
142
143///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping
144void                    btRigidBody::applyDamping(btScalar timeStep)
145{
146        //On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
147        //todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway
148
149//#define USE_OLD_DAMPING_METHOD 1
150#ifdef USE_OLD_DAMPING_METHOD
151        m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
152        m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
153#else
154        m_linearVelocity *= btPow(btScalar(1)-m_linearDamping, timeStep);
155        m_angularVelocity *= btPow(btScalar(1)-m_angularDamping, timeStep);
156#endif
157
158        if (m_additionalDamping)
159        {
160                //Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
161                //Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
162                if ((m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr) &&
163                        (m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr))
164                {
165                        m_angularVelocity *= m_additionalDampingFactor;
166                        m_linearVelocity *= m_additionalDampingFactor;
167                }
168       
169
170                btScalar speed = m_linearVelocity.length();
171                if (speed < m_linearDamping)
172                {
173                        btScalar dampVel = btScalar(0.005);
174                        if (speed > dampVel)
175                        {
176                                btVector3 dir = m_linearVelocity.normalized();
177                                m_linearVelocity -=  dir * dampVel;
178                        } else
179                        {
180                                m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
181                        }
182                }
183
184                btScalar angSpeed = m_angularVelocity.length();
185                if (angSpeed < m_angularDamping)
186                {
187                        btScalar angDampVel = btScalar(0.005);
188                        if (angSpeed > angDampVel)
189                        {
190                                btVector3 dir = m_angularVelocity.normalized();
191                                m_angularVelocity -=  dir * angDampVel;
192                        } else
193                        {
194                                m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
195                        }
196                }
197        }
198}
199
200
201void btRigidBody::applyGravity()
202{
203        if (isStaticOrKinematicObject())
204                return;
205       
206        applyCentralForce(m_gravity);   
207
208}
209
210void btRigidBody::proceedToTransform(const btTransform& newTrans)
211{
212        setCenterOfMassTransform( newTrans );
213}
214       
215
216void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
217{
218        if (mass == btScalar(0.))
219        {
220                m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT;
221                m_inverseMass = btScalar(0.);
222        } else
223        {
224                m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
225                m_inverseMass = btScalar(1.0) / mass;
226        }
227       
228        m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0),
229                                   inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0),
230                                   inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0));
231
232}
233
234       
235
236void btRigidBody::updateInertiaTensor() 
237{
238        m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
239}
240
241
242void btRigidBody::integrateVelocities(btScalar step) 
243{
244        if (isStaticOrKinematicObject())
245                return;
246
247        m_linearVelocity += m_totalForce * (m_inverseMass * step);
248        m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
249
250#define MAX_ANGVEL SIMD_HALF_PI
251        /// clamp angular velocity. collision calculations will fail on higher angular velocities       
252        btScalar angvel = m_angularVelocity.length();
253        if (angvel*step > MAX_ANGVEL)
254        {
255                m_angularVelocity *= (MAX_ANGVEL/step) /angvel;
256        }
257
258}
259
260btQuaternion btRigidBody::getOrientation() const
261{
262                btQuaternion orn;
263                m_worldTransform.getBasis().getRotation(orn);
264                return orn;
265}
266       
267       
268void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
269{
270
271        if (isStaticOrKinematicObject())
272        {
273                m_interpolationWorldTransform = m_worldTransform;
274        } else
275        {
276                m_interpolationWorldTransform = xform;
277        }
278        m_interpolationLinearVelocity = getLinearVelocity();
279        m_interpolationAngularVelocity = getAngularVelocity();
280        m_worldTransform = xform;
281        updateInertiaTensor();
282}
283
284
285bool btRigidBody::checkCollideWithOverride(btCollisionObject* co)
286{
287        btRigidBody* otherRb = btRigidBody::upcast(co);
288        if (!otherRb)
289                return true;
290
291        for (int i = 0; i < m_constraintRefs.size(); ++i)
292        {
293                btTypedConstraint* c = m_constraintRefs[i];
294                if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb)
295                        return false;
296        }
297
298        return true;
299}
300
301void btRigidBody::addConstraintRef(btTypedConstraint* c)
302{
303        int index = m_constraintRefs.findLinearSearch(c);
304        if (index == m_constraintRefs.size())
305                m_constraintRefs.push_back(c); 
306
307        m_checkCollideWith = true;
308}
309
310void btRigidBody::removeConstraintRef(btTypedConstraint* c)
311{
312        m_constraintRefs.remove(c);
313        m_checkCollideWith = m_constraintRefs.size() > 0;
314}
Note: See TracBrowser for help on using the repository browser.