00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
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
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
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
00112 if (timeStep != btScalar(0.))
00113 {
00114
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
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
00162
00163
00164
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
00176
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
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