1 /* 2 Bullet Continuous Collision Detection and Physics Library 3 Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ 4 5 This software is provided 'as-is', without any express or implied warranty. 6 In no event will the authors be held liable for any damages arising from the use of this software. 7 Permission is granted to anyone to use this software for any purpose, 8 including commercial applications, and to alter it and redistribute it freely, 9 subject to the following restrictions: 10 11 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. 12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. 13 3. 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 25 btScalar gDeactivationTime = btScalar(2.); 26 bool gDisableDeactivation = false; 27 static int uniqueId = 0; 28 29 30 btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo) 31 { 32 setupRigidBody(constructionInfo); 33 } 34 35 btRigidBody::btRigidBody(btScalar mass, btMotionState *motionState, btCollisionShape *collisionShape, const btVector3 &localInertia) 36 { 37 btRigidBodyConstructionInfo cinfo(mass,motionState,collisionShape,localInertia); 38 setupRigidBody(cinfo); 39 } 40 41 void 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_rollingFriction = constructionInfo.m_rollingFriction; 82 m_restitution = constructionInfo.m_restitution; 83 84 setCollisionShape( constructionInfo.m_collisionShape ); 85 m_debugBodyId = uniqueId++; 86 87 setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia); 88 updateInertiaTensor(); 89 90 m_rigidbodyFlags = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY; 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 104 void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) 105 { 106 btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform); 107 } 108 109 void 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 127 void btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const 128 { 129 getCollisionShape()->getAabb(m_worldTransform,aabbMin,aabbMax); 130 } 131 132 133 134 135 void 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 149 void 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 159 void 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 216 void btRigidBody::applyGravity() 217 { 218 if (isStaticOrKinematicObject()) 219 return; 220 221 applyCentralForce(m_gravity); 222 223 } 224 225 void btRigidBody::proceedToTransform(const btTransform& newTrans) 226 { 227 setCenterOfMassTransform( newTrans ); 228 } 229 230 231 void 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 void btRigidBody::updateInertiaTensor() 255 { 256 m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose(); 257 } 258 259 260 261 btVector3 btRigidBody::getLocalInertia() const 262 { 263 264 btVector3 inertiaLocal; 265 const btVector3 inertia = m_invInertiaLocal; 266 inertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0), 267 inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0), 268 inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0)); 269 return inertiaLocal; 270 } 271 272 inline btVector3 evalEulerEqn(const btVector3& w1, const btVector3& w0, const btVector3& T, const btScalar dt, 273 const btMatrix3x3 &I) 274 { 275 const btVector3 w2 = I*w1 + w1.cross(I*w1)*dt - (T*dt + I*w0); 276 return w2; 277 } 278 279 inline btMatrix3x3 evalEulerEqnDeriv(const btVector3& w1, const btVector3& w0, const btScalar dt, 280 const btMatrix3x3 &I) 281 { 282 283 btMatrix3x3 w1x, Iw1x; 284 const btVector3 Iwi = (I*w1); 285 w1.getSkewSymmetricMatrix(&w1x[0], &w1x[1], &w1x[2]); 286 Iwi.getSkewSymmetricMatrix(&Iw1x[0], &Iw1x[1], &Iw1x[2]); 287 288 const btMatrix3x3 dfw1 = I + (w1x*I - Iw1x)*dt; 289 return dfw1; 290 } 291 292 btVector3 btRigidBody::computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const 293 { 294 btVector3 inertiaLocal = getLocalInertia(); 295 btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose(); 296 btVector3 tmp = inertiaTensorWorld*getAngularVelocity(); 297 btVector3 gf = getAngularVelocity().cross(tmp); 298 btScalar l2 = gf.length2(); 299 if (l2>maxGyroscopicForce*maxGyroscopicForce) 300 { 301 gf *= btScalar(1.)/btSqrt(l2)*maxGyroscopicForce; 302 } 303 return gf; 304 } 305 306 void btSetCrossMatrixMinus(btMatrix3x3& res, const btVector3& a) 307 { 308 const btScalar a_0 = a[0], a_1 = a[1], a_2 = a[2]; 309 res.setValue(0, +a_2, -a_1, 310 -a_2, 0, +a_0, 311 +a_1, -a_0, 0); 312 } 313 314 btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Body(btScalar step) const 315 { 316 btVector3 idl = getLocalInertia(); 317 btVector3 omega1 = getAngularVelocity(); 318 btQuaternion q = getWorldTransform().getRotation(); 319 320 // Convert to body coordinates 321 btVector3 omegab = quatRotate(q.inverse(), omega1); 322 btMatrix3x3 Ib; 323 Ib.setValue(idl.x(),0,0, 324 0,idl.y(),0, 325 0,0,idl.z()); 326 327 btVector3 ibo = Ib*omegab; 328 329 // Residual vector 330 btVector3 f = step * omegab.cross(ibo); 331 332 btMatrix3x3 skew0; 333 omegab.getSkewSymmetricMatrix(&skew0[0], &skew0[1], &skew0[2]); 334 btVector3 om = Ib*omegab; 335 btMatrix3x3 skew1; 336 om.getSkewSymmetricMatrix(&skew1[0],&skew1[1],&skew1[2]); 337 338 // Jacobian 339 btMatrix3x3 J = Ib + (skew0*Ib - skew1)*step; 340 341 // btMatrix3x3 Jinv = J.inverse(); 342 // btVector3 omega_div = Jinv*f; 343 btVector3 omega_div = J.solve33(f); 344 345 // Single Newton-Raphson update 346 omegab = omegab - omega_div;//Solve33(J, f); 347 // Back to world coordinates 348 btVector3 omega2 = quatRotate(q,omegab); 349 btVector3 gf = omega2-omega1; 350 return gf; 351 } 352 353 354 355 btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) const 356 { 357 // use full newton-euler equations. common practice to drop the wxIw term. want it for better tumbling behavior. 358 // calculate using implicit euler step so it's stable. 359 360 const btVector3 inertiaLocal = getLocalInertia(); 361 const btVector3 w0 = getAngularVelocity(); 362 363 btMatrix3x3 I; 364 365 I = m_worldTransform.getBasis().scaled(inertiaLocal) * 366 m_worldTransform.getBasis().transpose(); 367 368 // use newtons method to find implicit solution for new angular velocity (w') 369 // f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0 370 // df/dw' = I + 1xIw'*step + w'xI*step 371 372 btVector3 w1 = w0; 373 374 // one step of newton's method 375 { 376 const btVector3 fw = evalEulerEqn(w1, w0, btVector3(0, 0, 0), step, I); 377 const btMatrix3x3 dfw = evalEulerEqnDeriv(w1, w0, step, I); 378 379 btVector3 dw; 380 dw = dfw.solve33(fw); 381 //const btMatrix3x3 dfw_inv = dfw.inverse(); 382 //dw = dfw_inv*fw; 383 384 w1 -= dw; 385 } 386 387 btVector3 gf = (w1 - w0); 388 return gf; 389 } 390 391 392 void btRigidBody::integrateVelocities(btScalar step) 393 { 394 if (isStaticOrKinematicObject()) 395 return; 396 397 m_linearVelocity += m_totalForce * (m_inverseMass * step); 398 m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step; 399 400 #define MAX_ANGVEL SIMD_HALF_PI 401 /// clamp angular velocity. collision calculations will fail on higher angular velocities 402 btScalar angvel = m_angularVelocity.length(); 403 if (angvel*step > MAX_ANGVEL) 404 { 405 m_angularVelocity *= (MAX_ANGVEL/step) /angvel; 406 } 407 408 } 409 410 btQuaternion btRigidBody::getOrientation() const 411 { 412 btQuaternion orn; 413 m_worldTransform.getBasis().getRotation(orn); 414 return orn; 415 } 416 417 418 void btRigidBody::setCenterOfMassTransform(const btTransform& xform) 419 { 420 421 if (isKinematicObject()) 422 { 423 m_interpolationWorldTransform = m_worldTransform; 424 } else 425 { 426 m_interpolationWorldTransform = xform; 427 } 428 m_interpolationLinearVelocity = getLinearVelocity(); 429 m_interpolationAngularVelocity = getAngularVelocity(); 430 m_worldTransform = xform; 431 updateInertiaTensor(); 432 } 433 434 435 436 437 438 void btRigidBody::addConstraintRef(btTypedConstraint* c) 439 { 440 ///disable collision with the 'other' body 441 442 int index = m_constraintRefs.findLinearSearch(c); 443 //don't add constraints that are already referenced 444 //btAssert(index == m_constraintRefs.size()); 445 if (index == m_constraintRefs.size()) 446 { 447 m_constraintRefs.push_back(c); 448 btCollisionObject* colObjA = &c->getRigidBodyA(); 449 btCollisionObject* colObjB = &c->getRigidBodyB(); 450 if (colObjA == this) 451 { 452 colObjA->setIgnoreCollisionCheck(colObjB, true); 453 } 454 else 455 { 456 colObjB->setIgnoreCollisionCheck(colObjA, true); 457 } 458 } 459 } 460 461 void btRigidBody::removeConstraintRef(btTypedConstraint* c) 462 { 463 int index = m_constraintRefs.findLinearSearch(c); 464 //don't remove constraints that are not referenced 465 if(index < m_constraintRefs.size()) 466 { 467 m_constraintRefs.remove(c); 468 btCollisionObject* colObjA = &c->getRigidBodyA(); 469 btCollisionObject* colObjB = &c->getRigidBodyB(); 470 if (colObjA == this) 471 { 472 colObjA->setIgnoreCollisionCheck(colObjB, false); 473 } 474 else 475 { 476 colObjB->setIgnoreCollisionCheck(colObjA, false); 477 } 478 } 479 } 480 481 int btRigidBody::calculateSerializeBufferSize() const 482 { 483 int sz = sizeof(btRigidBodyData); 484 return sz; 485 } 486 487 ///fills the dataBuffer and returns the struct name (and 0 on failure) 488 const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const 489 { 490 btRigidBodyData* rbd = (btRigidBodyData*) dataBuffer; 491 492 btCollisionObject::serialize(&rbd->m_collisionObjectData, serializer); 493 494 m_invInertiaTensorWorld.serialize(rbd->m_invInertiaTensorWorld); 495 m_linearVelocity.serialize(rbd->m_linearVelocity); 496 m_angularVelocity.serialize(rbd->m_angularVelocity); 497 rbd->m_inverseMass = m_inverseMass; 498 m_angularFactor.serialize(rbd->m_angularFactor); 499 m_linearFactor.serialize(rbd->m_linearFactor); 500 m_gravity.serialize(rbd->m_gravity); 501 m_gravity_acceleration.serialize(rbd->m_gravity_acceleration); 502 m_invInertiaLocal.serialize(rbd->m_invInertiaLocal); 503 m_totalForce.serialize(rbd->m_totalForce); 504 m_totalTorque.serialize(rbd->m_totalTorque); 505 rbd->m_linearDamping = m_linearDamping; 506 rbd->m_angularDamping = m_angularDamping; 507 rbd->m_additionalDamping = m_additionalDamping; 508 rbd->m_additionalDampingFactor = m_additionalDampingFactor; 509 rbd->m_additionalLinearDampingThresholdSqr = m_additionalLinearDampingThresholdSqr; 510 rbd->m_additionalAngularDampingThresholdSqr = m_additionalAngularDampingThresholdSqr; 511 rbd->m_additionalAngularDampingFactor = m_additionalAngularDampingFactor; 512 rbd->m_linearSleepingThreshold=m_linearSleepingThreshold; 513 rbd->m_angularSleepingThreshold = m_angularSleepingThreshold; 514 515 return btRigidBodyDataName; 516 } 517 518 519 520 void btRigidBody::serializeSingleObject(class btSerializer* serializer) const 521 { 522 btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(),1); 523 const char* structType = serialize(chunk->m_oldPtr, serializer); 524 serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,(void*)this); 525 } 526 527 528
