btRigidBody.cpp

Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 #include "btRigidBody.h"
00017 #include "BulletCollision/CollisionShapes/btConvexShape.h"
00018 #include "LinearMath/btMinMax.h"
00019 #include "LinearMath/btTransformUtil.h"
00020 #include "LinearMath/btMotionState.h"
00021 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
00022 #include "LinearMath/btSerializer.h"
00023 
00024 //'temporarily' global variables
00025 btScalar        gDeactivationTime = btScalar(2.);
00026 bool    gDisableDeactivation = false;
00027 static int uniqueId = 0;
00028 
00029 
00030 btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
00031 {
00032         setupRigidBody(constructionInfo);
00033 }
00034 
00035 btRigidBody::btRigidBody(btScalar mass, btMotionState *motionState, btCollisionShape *collisionShape, const btVector3 &localInertia)
00036 {
00037         btRigidBodyConstructionInfo cinfo(mass,motionState,collisionShape,localInertia);
00038         setupRigidBody(cinfo);
00039 }
00040 
00041 void    btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
00042 {
00043 
00044         m_internalType=CO_RIGID_BODY;
00045 
00046         m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
00047         m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
00048         m_angularFactor.setValue(1,1,1);
00049         m_linearFactor.setValue(1,1,1);
00050         m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
00051         m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
00052         m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
00053         m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
00054     setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping);
00055 
00056         m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold;
00057         m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold;
00058         m_optionalMotionState = constructionInfo.m_motionState;
00059         m_contactSolverType = 0;
00060         m_frictionSolverType = 0;
00061         m_additionalDamping = constructionInfo.m_additionalDamping;
00062         m_additionalDampingFactor = constructionInfo.m_additionalDampingFactor;
00063         m_additionalLinearDampingThresholdSqr = constructionInfo.m_additionalLinearDampingThresholdSqr;
00064         m_additionalAngularDampingThresholdSqr = constructionInfo.m_additionalAngularDampingThresholdSqr;
00065         m_additionalAngularDampingFactor = constructionInfo.m_additionalAngularDampingFactor;
00066 
00067         if (m_optionalMotionState)
00068         {
00069                 m_optionalMotionState->getWorldTransform(m_worldTransform);
00070         } else
00071         {
00072                 m_worldTransform = constructionInfo.m_startWorldTransform;
00073         }
00074 
00075         m_interpolationWorldTransform = m_worldTransform;
00076         m_interpolationLinearVelocity.setValue(0,0,0);
00077         m_interpolationAngularVelocity.setValue(0,0,0);
00078         
00079         //moved to btCollisionObject
00080         m_friction = constructionInfo.m_friction;
00081         m_rollingFriction = constructionInfo.m_rollingFriction;
00082         m_restitution = constructionInfo.m_restitution;
00083 
00084         setCollisionShape( constructionInfo.m_collisionShape );
00085         m_debugBodyId = uniqueId++;
00086         
00087         setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia);
00088         updateInertiaTensor();
00089 
00090         m_rigidbodyFlags = 0;
00091 
00092 
00093         m_deltaLinearVelocity.setZero();
00094         m_deltaAngularVelocity.setZero();
00095         m_invMass = m_inverseMass*m_linearFactor;
00096         m_pushVelocity.setZero();
00097         m_turnVelocity.setZero();
00098 
00099         
00100 
00101 }
00102 
00103 
00104 void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) 
00105 {
00106         btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
00107 }
00108 
00109 void                    btRigidBody::saveKinematicState(btScalar timeStep)
00110 {
00111         //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
00112         if (timeStep != btScalar(0.))
00113         {
00114                 //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
00115                 if (getMotionState())
00116                         getMotionState()->getWorldTransform(m_worldTransform);
00117                 btVector3 linVel,angVel;
00118                 
00119                 btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,timeStep,m_linearVelocity,m_angularVelocity);
00120                 m_interpolationLinearVelocity = m_linearVelocity;
00121                 m_interpolationAngularVelocity = m_angularVelocity;
00122                 m_interpolationWorldTransform = m_worldTransform;
00123                 //printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ());
00124         }
00125 }
00126         
00127 void    btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const
00128 {
00129         getCollisionShape()->getAabb(m_worldTransform,aabbMin,aabbMax);
00130 }
00131 
00132 
00133 
00134 
00135 void btRigidBody::setGravity(const btVector3& acceleration) 
00136 {
00137         if (m_inverseMass != btScalar(0.0))
00138         {
00139                 m_gravity = acceleration * (btScalar(1.0) / m_inverseMass);
00140         }
00141         m_gravity_acceleration = acceleration;
00142 }
00143 
00144 
00145 
00146 
00147 
00148 
00149 void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
00150 {
00151         m_linearDamping = btClamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
00152         m_angularDamping = btClamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
00153 }
00154 
00155 
00156 
00157 
00159 void                    btRigidBody::applyDamping(btScalar timeStep)
00160 {
00161         //On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
00162         //todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway
00163 
00164 //#define USE_OLD_DAMPING_METHOD 1
00165 #ifdef USE_OLD_DAMPING_METHOD
00166         m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
00167         m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
00168 #else
00169         m_linearVelocity *= btPow(btScalar(1)-m_linearDamping, timeStep);
00170         m_angularVelocity *= btPow(btScalar(1)-m_angularDamping, timeStep);
00171 #endif
00172 
00173         if (m_additionalDamping)
00174         {
00175                 //Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
00176                 //Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
00177                 if ((m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr) &&
00178                         (m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr))
00179                 {
00180                         m_angularVelocity *= m_additionalDampingFactor;
00181                         m_linearVelocity *= m_additionalDampingFactor;
00182                 }
00183         
00184 
00185                 btScalar speed = m_linearVelocity.length();
00186                 if (speed < m_linearDamping)
00187                 {
00188                         btScalar dampVel = btScalar(0.005);
00189                         if (speed > dampVel)
00190                         {
00191                                 btVector3 dir = m_linearVelocity.normalized();
00192                                 m_linearVelocity -=  dir * dampVel;
00193                         } else
00194                         {
00195                                 m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
00196                         }
00197                 }
00198 
00199                 btScalar angSpeed = m_angularVelocity.length();
00200                 if (angSpeed < m_angularDamping)
00201                 {
00202                         btScalar angDampVel = btScalar(0.005);
00203                         if (angSpeed > angDampVel)
00204                         {
00205                                 btVector3 dir = m_angularVelocity.normalized();
00206                                 m_angularVelocity -=  dir * angDampVel;
00207                         } else
00208                         {
00209                                 m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
00210                         }
00211                 }
00212         }
00213 }
00214 
00215 
00216 void btRigidBody::applyGravity()
00217 {
00218         if (isStaticOrKinematicObject())
00219                 return;
00220         
00221         applyCentralForce(m_gravity);   
00222 
00223 }
00224 
00225 void btRigidBody::proceedToTransform(const btTransform& newTrans)
00226 {
00227         setCenterOfMassTransform( newTrans );
00228 }
00229         
00230 
00231 void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
00232 {
00233         if (mass == btScalar(0.))
00234         {
00235                 m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT;
00236                 m_inverseMass = btScalar(0.);
00237         } else
00238         {
00239                 m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
00240                 m_inverseMass = btScalar(1.0) / mass;
00241         }
00242 
00243         //Fg = m * a
00244         m_gravity = mass * m_gravity_acceleration;
00245         
00246         m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0),
00247                                    inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0),
00248                                    inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0));
00249 
00250         m_invMass = m_linearFactor*m_inverseMass;
00251 }
00252 
00253         
00254 void btRigidBody::updateInertiaTensor() 
00255 {
00256         m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
00257 }
00258 
00259 
00260 btVector3 btRigidBody::computeGyroscopicForce(btScalar maxGyroscopicForce) const
00261 {
00262         btVector3 inertiaLocal;
00263         inertiaLocal[0] = 1.f/getInvInertiaDiagLocal()[0];
00264         inertiaLocal[1] = 1.f/getInvInertiaDiagLocal()[1];
00265         inertiaLocal[2] = 1.f/getInvInertiaDiagLocal()[2];
00266         btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose();
00267         btVector3 tmp = inertiaTensorWorld*getAngularVelocity();
00268         btVector3 gf = getAngularVelocity().cross(tmp);
00269         btScalar l2 = gf.length2();
00270         if (l2>maxGyroscopicForce*maxGyroscopicForce)
00271         {
00272                 gf *= btScalar(1.)/btSqrt(l2)*maxGyroscopicForce;
00273         }
00274         return gf;
00275 }
00276 
00277 void btRigidBody::integrateVelocities(btScalar step) 
00278 {
00279         if (isStaticOrKinematicObject())
00280                 return;
00281 
00282         m_linearVelocity += m_totalForce * (m_inverseMass * step);
00283         m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
00284 
00285 #define MAX_ANGVEL SIMD_HALF_PI
00286 
00287         btScalar angvel = m_angularVelocity.length();
00288         if (angvel*step > MAX_ANGVEL)
00289         {
00290                 m_angularVelocity *= (MAX_ANGVEL/step) /angvel;
00291         }
00292 
00293 }
00294 
00295 btQuaternion btRigidBody::getOrientation() const
00296 {
00297                 btQuaternion orn;
00298                 m_worldTransform.getBasis().getRotation(orn);
00299                 return orn;
00300 }
00301         
00302         
00303 void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
00304 {
00305 
00306         if (isKinematicObject())
00307         {
00308                 m_interpolationWorldTransform = m_worldTransform;
00309         } else
00310         {
00311                 m_interpolationWorldTransform = xform;
00312         }
00313         m_interpolationLinearVelocity = getLinearVelocity();
00314         m_interpolationAngularVelocity = getAngularVelocity();
00315         m_worldTransform = xform;
00316         updateInertiaTensor();
00317 }
00318 
00319 
00320 bool btRigidBody::checkCollideWithOverride(const  btCollisionObject* co) const
00321 {
00322         const btRigidBody* otherRb = btRigidBody::upcast(co);
00323         if (!otherRb)
00324                 return true;
00325 
00326         for (int i = 0; i < m_constraintRefs.size(); ++i)
00327         {
00328                 const btTypedConstraint* c = m_constraintRefs[i];
00329                 if (c->isEnabled())
00330                         if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb)
00331                                 return false;
00332         }
00333 
00334         return true;
00335 }
00336 
00337 
00338 
00339 void btRigidBody::addConstraintRef(btTypedConstraint* c)
00340 {
00341         int index = m_constraintRefs.findLinearSearch(c);
00342         if (index == m_constraintRefs.size())
00343                 m_constraintRefs.push_back(c); 
00344 
00345         m_checkCollideWith = true;
00346 }
00347 
00348 void btRigidBody::removeConstraintRef(btTypedConstraint* c)
00349 {
00350         m_constraintRefs.remove(c);
00351         m_checkCollideWith = m_constraintRefs.size() > 0;
00352 }
00353 
00354 int     btRigidBody::calculateSerializeBufferSize()     const
00355 {
00356         int sz = sizeof(btRigidBodyData);
00357         return sz;
00358 }
00359 
00361 const char*     btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const
00362 {
00363         btRigidBodyData* rbd = (btRigidBodyData*) dataBuffer;
00364 
00365         btCollisionObject::serialize(&rbd->m_collisionObjectData, serializer);
00366 
00367         m_invInertiaTensorWorld.serialize(rbd->m_invInertiaTensorWorld);
00368         m_linearVelocity.serialize(rbd->m_linearVelocity);
00369         m_angularVelocity.serialize(rbd->m_angularVelocity);
00370         rbd->m_inverseMass = m_inverseMass;
00371         m_angularFactor.serialize(rbd->m_angularFactor);
00372         m_linearFactor.serialize(rbd->m_linearFactor);
00373         m_gravity.serialize(rbd->m_gravity);
00374         m_gravity_acceleration.serialize(rbd->m_gravity_acceleration);
00375         m_invInertiaLocal.serialize(rbd->m_invInertiaLocal);
00376         m_totalForce.serialize(rbd->m_totalForce);
00377         m_totalTorque.serialize(rbd->m_totalTorque);
00378         rbd->m_linearDamping = m_linearDamping;
00379         rbd->m_angularDamping = m_angularDamping;
00380         rbd->m_additionalDamping = m_additionalDamping;
00381         rbd->m_additionalDampingFactor = m_additionalDampingFactor;
00382         rbd->m_additionalLinearDampingThresholdSqr = m_additionalLinearDampingThresholdSqr;
00383         rbd->m_additionalAngularDampingThresholdSqr = m_additionalAngularDampingThresholdSqr;
00384         rbd->m_additionalAngularDampingFactor = m_additionalAngularDampingFactor;
00385         rbd->m_linearSleepingThreshold=m_linearSleepingThreshold;
00386         rbd->m_angularSleepingThreshold = m_angularSleepingThreshold;
00387 
00388         return btRigidBodyDataName;
00389 }
00390 
00391 
00392 
00393 void btRigidBody::serializeSingleObject(class btSerializer* serializer) const
00394 {
00395         btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(),1);
00396         const char* structType = serialize(chunk->m_oldPtr, serializer);
00397         serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,(void*)this);
00398 }
00399 
00400