dali-physics/third-party/bullet3/src/BulletDynamics/Dynamics/btRigidBody.cpp

   1 /*
   2 Bullet Continuous Collision Detection and Physics Library
   3 Copyright (c) 2003-2006 Erwin Coumans  https://bulletphysics.org
   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 btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
  30 {
  31         setupRigidBody(constructionInfo);
  32 }
  33
  34 btRigidBody::btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia)
  35 {
  36         btRigidBodyConstructionInfo cinfo(mass, motionState, collisionShape, localInertia);
  37         setupRigidBody(cinfo);
  38 }
  39
  40 void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
  41 {
  42         m_internalType = CO_RIGID_BODY;
  43
  44         m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
  45         m_angularVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
  46         m_angularFactor.setValue(1, 1, 1);
  47         m_linearFactor.setValue(1, 1, 1);
  48         m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
  49         m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
  50         m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
  51         m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
  52                 setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping);
  53
  54         m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold;
  55         m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold;
  56         m_optionalMotionState = constructionInfo.m_motionState;
  57         m_contactSolverType = 0;
  58         m_frictionSolverType = 0;
  59         m_additionalDamping = constructionInfo.m_additionalDamping;
  60         m_additionalDampingFactor = constructionInfo.m_additionalDampingFactor;
  61         m_additionalLinearDampingThresholdSqr = constructionInfo.m_additionalLinearDampingThresholdSqr;
  62         m_additionalAngularDampingThresholdSqr = constructionInfo.m_additionalAngularDampingThresholdSqr;
  63         m_additionalAngularDampingFactor = constructionInfo.m_additionalAngularDampingFactor;
  64
  65         if (m_optionalMotionState)
  66         {
  67                 m_optionalMotionState->getWorldTransform(m_worldTransform);
  68         }
  69         else
  70         {
  71                 m_worldTransform = constructionInfo.m_startWorldTransform;
  72         }
  73
  74         m_interpolationWorldTransform = m_worldTransform;
  75         m_interpolationLinearVelocity.setValue(0, 0, 0);
  76         m_interpolationAngularVelocity.setValue(0, 0, 0);
  77
  78         //moved to btCollisionObject
  79         m_friction = constructionInfo.m_friction;
  80         m_rollingFriction = constructionInfo.m_rollingFriction;
  81         m_spinningFriction = constructionInfo.m_spinningFriction;
  82
  83         m_restitution = constructionInfo.m_restitution;
  84
  85         setCollisionShape(constructionInfo.m_collisionShape);
  86         m_debugBodyId = uniqueId++;
  87
  88         setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia);
  89         updateInertiaTensor();
  90
  91         m_rigidbodyFlags = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY;
  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 void btRigidBody::predictIntegratedTransform(btScalar timeStep, btTransform& predictedTransform)
 101 {
 102         btTransformUtil::integrateTransform(m_worldTransform, m_linearVelocity, m_angularVelocity, timeStep, predictedTransform);
 103 }
 104
 105 void btRigidBody::saveKinematicState(btScalar timeStep)
 106 {
 107         //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
 108         if (timeStep != btScalar(0.))
 109         {
 110                 //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
 111                 if (getMotionState())
 112                         getMotionState()->getWorldTransform(m_worldTransform);
 113                 btVector3 linVel, angVel;
 114
 115                 btTransformUtil::calculateVelocity(m_interpolationWorldTransform, m_worldTransform, timeStep, m_linearVelocity, m_angularVelocity);
 116                 m_interpolationLinearVelocity = m_linearVelocity;
 117                 m_interpolationAngularVelocity = m_angularVelocity;
 118                 m_interpolationWorldTransform = m_worldTransform;
 119                 //printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ());
 120         }
 121 }
 122
 123 void btRigidBody::getAabb(btVector3& aabbMin, btVector3& aabbMax) const
 124 {
 125         getCollisionShape()->getAabb(m_worldTransform, aabbMin, aabbMax);
 126 }
 127
 128 void btRigidBody::setGravity(const btVector3& acceleration)
 129 {
 130         if (m_inverseMass != btScalar(0.0))
 131         {
 132                 m_gravity = acceleration * (btScalar(1.0) / m_inverseMass);
 133         }
 134         m_gravity_acceleration = acceleration;
 135 }
 136
 137 void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
 138 {
 139 #ifdef BT_USE_OLD_DAMPING_METHOD
 140         m_linearDamping = btMax(lin_damping, btScalar(0.0));
 141         m_angularDamping = btMax(ang_damping, btScalar(0.0));
 142 #else
 143         m_linearDamping = btClamped(lin_damping, btScalar(0.0), btScalar(1.0));
 144         m_angularDamping = btClamped(ang_damping, btScalar(0.0), btScalar(1.0));
 145 #endif
 146 }
 147
 148 ///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping
 149 void btRigidBody::applyDamping(btScalar timeStep)
 150 {
 151         //On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
 152         //todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway
 153
 154 #ifdef BT_USE_OLD_DAMPING_METHOD
 155         m_linearVelocity *= btMax((btScalar(1.0) - timeStep * m_linearDamping), btScalar(0.0));
 156         m_angularVelocity *= btMax((btScalar(1.0) - timeStep * m_angularDamping), btScalar(0.0));
 157 #else
 158         m_linearVelocity *= btPow(btScalar(1) - m_linearDamping, timeStep);
 159         m_angularVelocity *= btPow(btScalar(1) - m_angularDamping, timeStep);
 160 #endif
 161
 162         if (m_additionalDamping)
 163         {
 164                 //Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
 165                 //Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
 166                 if ((m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr) &&
 167                         (m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr))
 168                 {
 169                         m_angularVelocity *= m_additionalDampingFactor;
 170                         m_linearVelocity *= m_additionalDampingFactor;
 171                 }
 172
 173                 btScalar speed = m_linearVelocity.length();
 174                 if (speed < m_linearDamping)
 175                 {
 176                         btScalar dampVel = btScalar(0.005);
 177                         if (speed > dampVel)
 178                         {
 179                                 btVector3 dir = m_linearVelocity.normalized();
 180                                 m_linearVelocity -= dir * dampVel;
 181                         }
 182                         else
 183                         {
 184                                 m_linearVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
 185                         }
 186                 }
 187
 188                 btScalar angSpeed = m_angularVelocity.length();
 189                 if (angSpeed < m_angularDamping)
 190                 {
 191                         btScalar angDampVel = btScalar(0.005);
 192                         if (angSpeed > angDampVel)
 193                         {
 194                                 btVector3 dir = m_angularVelocity.normalized();
 195                                 m_angularVelocity -= dir * angDampVel;
 196                         }
 197                         else
 198                         {
 199                                 m_angularVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
 200                         }
 201                 }
 202         }
 203 }
 204
 205 void btRigidBody::applyGravity()
 206 {
 207         if (isStaticOrKinematicObject())
 208                 return;
 209
 210         applyCentralForce(m_gravity);
 211 }
 212
 213 void btRigidBody::clearGravity()
 214 {
 215     if (isStaticOrKinematicObject())
 216         return;
 217
 218     applyCentralForce(-m_gravity);
 219 }
 220
 221 void btRigidBody::proceedToTransform(const btTransform& newTrans)
 222 {
 223         setCenterOfMassTransform(newTrans);
 224 }
 225
 226 void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
 227 {
 228         if (mass == btScalar(0.))
 229         {
 230                 m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT;
 231                 m_inverseMass = btScalar(0.);
 232         }
 233         else
 234         {
 235                 m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
 236                 m_inverseMass = btScalar(1.0) / mass;
 237         }
 238
 239         //Fg = m * a
 240         m_gravity = mass * m_gravity_acceleration;
 241
 242         m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0),
 243                                                            inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0),
 244                                                            inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0));
 245
 246         m_invMass = m_linearFactor * m_inverseMass;
 247 }
 248
 249 void btRigidBody::updateInertiaTensor()
 250 {
 251         m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
 252 }
 253
 254 btVector3 btRigidBody::getLocalInertia() const
 255 {
 256         btVector3 inertiaLocal;
 257         const btVector3 inertia = m_invInertiaLocal;
 258         inertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0),
 259                                                   inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0),
 260                                                   inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0));
 261         return inertiaLocal;
 262 }
 263
 264 inline btVector3 evalEulerEqn(const btVector3& w1, const btVector3& w0, const btVector3& T, const btScalar dt,
 265                                                           const btMatrix3x3& I)
 266 {
 267         const btVector3 w2 = I * w1 + w1.cross(I * w1) * dt - (T * dt + I * w0);
 268         return w2;
 269 }
 270
 271 inline btMatrix3x3 evalEulerEqnDeriv(const btVector3& w1, const btVector3& w0, const btScalar dt,
 272                                                                          const btMatrix3x3& I)
 273 {
 274         btMatrix3x3 w1x, Iw1x;
 275         const btVector3 Iwi = (I * w1);
 276         w1.getSkewSymmetricMatrix(&w1x[0], &w1x[1], &w1x[2]);
 277         Iwi.getSkewSymmetricMatrix(&Iw1x[0], &Iw1x[1], &Iw1x[2]);
 278
 279         const btMatrix3x3 dfw1 = I + (w1x * I - Iw1x) * dt;
 280         return dfw1;
 281 }
 282
 283 btVector3 btRigidBody::computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const
 284 {
 285         btVector3 inertiaLocal = getLocalInertia();
 286         btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose();
 287         btVector3 tmp = inertiaTensorWorld * getAngularVelocity();
 288         btVector3 gf = getAngularVelocity().cross(tmp);
 289         btScalar l2 = gf.length2();
 290         if (l2 > maxGyroscopicForce * maxGyroscopicForce)
 291         {
 292                 gf *= btScalar(1.) / btSqrt(l2) * maxGyroscopicForce;
 293         }
 294         return gf;
 295 }
 296
 297 btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Body(btScalar step) const
 298 {
 299         btVector3 idl = getLocalInertia();
 300         btVector3 omega1 = getAngularVelocity();
 301         btQuaternion q = getWorldTransform().getRotation();
 302
 303         // Convert to body coordinates
 304         btVector3 omegab = quatRotate(q.inverse(), omega1);
 305         btMatrix3x3 Ib;
 306         Ib.setValue(idl.x(), 0, 0,
 307                                 0, idl.y(), 0,
 308                                 0, 0, idl.z());
 309
 310         btVector3 ibo = Ib * omegab;
 311
 312         // Residual vector
 313         btVector3 f = step * omegab.cross(ibo);
 314
 315         btMatrix3x3 skew0;
 316         omegab.getSkewSymmetricMatrix(&skew0[0], &skew0[1], &skew0[2]);
 317         btVector3 om = Ib * omegab;
 318         btMatrix3x3 skew1;
 319         om.getSkewSymmetricMatrix(&skew1[0], &skew1[1], &skew1[2]);
 320
 321         // Jacobian
 322         btMatrix3x3 J = Ib + (skew0 * Ib - skew1) * step;
 323
 324         //      btMatrix3x3 Jinv = J.inverse();
 325         //      btVector3 omega_div = Jinv*f;
 326         btVector3 omega_div = J.solve33(f);
 327
 328         // Single Newton-Raphson update
 329         omegab = omegab - omega_div;  //Solve33(J, f);
 330         // Back to world coordinates
 331         btVector3 omega2 = quatRotate(q, omegab);
 332         btVector3 gf = omega2 - omega1;
 333         return gf;
 334 }
 335
 336 btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) const
 337 {
 338         // use full newton-euler equations.  common practice to drop the wxIw term. want it for better tumbling behavior.
 339         // calculate using implicit euler step so it's stable.
 340
 341         const btVector3 inertiaLocal = getLocalInertia();
 342         const btVector3 w0 = getAngularVelocity();
 343
 344         btMatrix3x3 I;
 345
 346         I = m_worldTransform.getBasis().scaled(inertiaLocal) *
 347                 m_worldTransform.getBasis().transpose();
 348
 349         // use newtons method to find implicit solution for new angular velocity (w')
 350         // f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0
 351         // df/dw' = I + 1xIw'*step + w'xI*step
 352
 353         btVector3 w1 = w0;
 354
 355         // one step of newton's method
 356         {
 357                 const btVector3 fw = evalEulerEqn(w1, w0, btVector3(0, 0, 0), step, I);
 358                 const btMatrix3x3 dfw = evalEulerEqnDeriv(w1, w0, step, I);
 359
 360                 btVector3 dw;
 361                 dw = dfw.solve33(fw);
 362                 //const btMatrix3x3 dfw_inv = dfw.inverse();
 363                 //dw = dfw_inv*fw;
 364
 365                 w1 -= dw;
 366         }
 367
 368         btVector3 gf = (w1 - w0);
 369         return gf;
 370 }
 371
 372 void btRigidBody::integrateVelocities(btScalar step)
 373 {
 374         if (isStaticOrKinematicObject())
 375                 return;
 376
 377         m_linearVelocity += m_totalForce * (m_inverseMass * step);
 378         m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
 379
 380 #define MAX_ANGVEL SIMD_HALF_PI
 381         /// clamp angular velocity. collision calculations will fail on higher angular velocities
 382         btScalar angvel = m_angularVelocity.length();
 383         if (angvel * step > MAX_ANGVEL)
 384         {
 385                 m_angularVelocity *= (MAX_ANGVEL / step) / angvel;
 386         }
 387         #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
 388         clampVelocity(m_angularVelocity);
 389         #endif
 390 }
 391
 392 btQuaternion btRigidBody::getOrientation() const
 393 {
 394         btQuaternion orn;
 395         m_worldTransform.getBasis().getRotation(orn);
 396         return orn;
 397 }
 398
 399 void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
 400 {
 401         if (isKinematicObject())
 402         {
 403                 m_interpolationWorldTransform = m_worldTransform;
 404         }
 405         else
 406         {
 407                 m_interpolationWorldTransform = xform;
 408         }
 409         m_interpolationLinearVelocity = getLinearVelocity();
 410         m_interpolationAngularVelocity = getAngularVelocity();
 411         m_worldTransform = xform;
 412         updateInertiaTensor();
 413 }
 414
 415 void btRigidBody::addConstraintRef(btTypedConstraint* c)
 416 {
 417         ///disable collision with the 'other' body
 418
 419         int index = m_constraintRefs.findLinearSearch(c);
 420         //don't add constraints that are already referenced
 421         //btAssert(index == m_constraintRefs.size());
 422         if (index == m_constraintRefs.size())
 423         {
 424                 m_constraintRefs.push_back(c);
 425                 btCollisionObject* colObjA = &c->getRigidBodyA();
 426                 btCollisionObject* colObjB = &c->getRigidBodyB();
 427                 if (colObjA == this)
 428                 {
 429                         colObjA->setIgnoreCollisionCheck(colObjB, true);
 430                 }
 431                 else
 432                 {
 433                         colObjB->setIgnoreCollisionCheck(colObjA, true);
 434                 }
 435         }
 436 }
 437
 438 void btRigidBody::removeConstraintRef(btTypedConstraint* c)
 439 {
 440         int index = m_constraintRefs.findLinearSearch(c);
 441         //don't remove constraints that are not referenced
 442         if (index < m_constraintRefs.size())
 443         {
 444                 m_constraintRefs.remove(c);
 445                 btCollisionObject* colObjA = &c->getRigidBodyA();
 446                 btCollisionObject* colObjB = &c->getRigidBodyB();
 447                 if (colObjA == this)
 448                 {
 449                         colObjA->setIgnoreCollisionCheck(colObjB, false);
 450                 }
 451                 else
 452                 {
 453                         colObjB->setIgnoreCollisionCheck(colObjA, false);
 454                 }
 455         }
 456 }
 457
 458 int btRigidBody::calculateSerializeBufferSize() const
 459 {
 460         int sz = sizeof(btRigidBodyData);
 461         return sz;
 462 }
 463
 464 ///fills the dataBuffer and returns the struct name (and 0 on failure)
 465 const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const
 466 {
 467         btRigidBodyData* rbd = (btRigidBodyData*)dataBuffer;
 468
 469         btCollisionObject::serialize(&rbd->m_collisionObjectData, serializer);
 470
 471         m_invInertiaTensorWorld.serialize(rbd->m_invInertiaTensorWorld);
 472         m_linearVelocity.serialize(rbd->m_linearVelocity);
 473         m_angularVelocity.serialize(rbd->m_angularVelocity);
 474         rbd->m_inverseMass = m_inverseMass;
 475         m_angularFactor.serialize(rbd->m_angularFactor);
 476         m_linearFactor.serialize(rbd->m_linearFactor);
 477         m_gravity.serialize(rbd->m_gravity);
 478         m_gravity_acceleration.serialize(rbd->m_gravity_acceleration);
 479         m_invInertiaLocal.serialize(rbd->m_invInertiaLocal);
 480         m_totalForce.serialize(rbd->m_totalForce);
 481         m_totalTorque.serialize(rbd->m_totalTorque);
 482         rbd->m_linearDamping = m_linearDamping;
 483         rbd->m_angularDamping = m_angularDamping;
 484         rbd->m_additionalDamping = m_additionalDamping;
 485         rbd->m_additionalDampingFactor = m_additionalDampingFactor;
 486         rbd->m_additionalLinearDampingThresholdSqr = m_additionalLinearDampingThresholdSqr;
 487         rbd->m_additionalAngularDampingThresholdSqr = m_additionalAngularDampingThresholdSqr;
 488         rbd->m_additionalAngularDampingFactor = m_additionalAngularDampingFactor;
 489         rbd->m_linearSleepingThreshold = m_linearSleepingThreshold;
 490         rbd->m_angularSleepingThreshold = m_angularSleepingThreshold;
 491
 492         // Fill padding with zeros to appease msan.
 493 #ifdef BT_USE_DOUBLE_PRECISION
 494         memset(rbd->m_padding, 0, sizeof(rbd->m_padding));
 495 #endif
 496
 497         return btRigidBodyDataName;
 498 }
 499
 500 void btRigidBody::serializeSingleObject(class btSerializer* serializer) const
 501 {
 502         btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(), 1);
 503         const char* structType = serialize(chunk->m_oldPtr, serializer);
 504         serializer->finalizeChunk(chunk, structType, BT_RIGIDBODY_CODE, (void*)this);
 505 }