diff --git a/Engine/source/T3D/rigid.cpp b/Engine/source/T3D/rigid.cpp index ffdcda6ff..adb453879 100644 --- a/Engine/source/T3D/rigid.cpp +++ b/Engine/source/T3D/rigid.cpp @@ -46,6 +46,11 @@ Rigid::Rigid() restitution = 0.3f; friction = 0.5f; atRest = false; + + sleepLinearThreshold = 0.0004f; + sleepAngThreshold = 0.0004f; + sleepTimeThreshold = 0.75f; + sleepTimer = 0.0f; } void Rigid::clearForces() @@ -57,9 +62,18 @@ void Rigid::clearForces() //----------------------------------------------------------------------------- void Rigid::integrate(F32 delta) { - // Update Angular position + if (atRest && force.isZero() && torque.isZero()) + return; + + // 1. advance momentum + angMomentum += torque * delta; + linMomentum += force * delta; + linVelocity = linMomentum * oneOverMass; + + // 2. advance orientation if ang vel significant F32 angle = angVelocity.len(); - if (angle != 0.0f) { + if (mFabs(angle)> POINT_EPSILON) + { QuatF dq; F32 sinHalfAngle; mSinCos(angle * delta * -0.5f, sinHalfAngle, dq.w); @@ -73,22 +87,29 @@ void Rigid::integrate(F32 delta) // Rotate the position around the center of mass Point3F lp = linPosition - worldCenterOfMass; - dq.mulP(lp,&linPosition); + dq.mulP(lp, &linPosition); linPosition += worldCenterOfMass; } - // Update angular momentum - angMomentum = angMomentum + torque * delta; + // 3. advance position + linPosition += linVelocity * delta; - // Update linear position, momentum - linPosition = linPosition + linVelocity * delta; - linMomentum = linMomentum + force * delta; - linVelocity = linMomentum * oneOverMass; + // 4. rebuild world inertia + if (mFabs(angle) > POINT_EPSILON) + { + updateInertialTensor(); + } - // Update dependent state variables - updateInertialTensor(); - updateVelocity(); + // 5. refresh ang velocity + updateAngularVelocity(); + + + // 6. CoM update updateCenterOfMass(); + + // 7. check if we can sleep + trySleep(delta); + } void Rigid::updateVelocity() @@ -118,7 +139,7 @@ void Rigid::updateCenterOfMass() void Rigid::applyImpulse(const Point3F &r, const Point3F &impulse) { if (impulse.lenSquared() < mass) return; - atRest = false; + wake(); // Linear momentum and velocity linMomentum += impulse; @@ -278,6 +299,47 @@ void Rigid::translateCenterOfMass(const Point3F &oldPos,const Point3F &newPos) objectInertia(2,1) = objectInertia(1,2); } +void Rigid::trySleep(F32 dt) +{ + // If there is active force/torque, don’t sleep + if (!force.isZero() || !torque.isZero()) + { + sleepTimer = 0.0f; return; + } + + const F32 linV2 = linVelocity.lenSquared(); + const F32 angV2 = angVelocity.lenSquared(); + + if (linV2 < sleepLinearThreshold && angV2 < sleepAngThreshold) + { + sleepTimer += dt; + if (sleepTimer >= sleepTimeThreshold) + { + setAtRest(); + } + } + else + { + sleepTimer = 0.0f; + } +} + +void Rigid::setSleepThresholds(F32 linVel2, F32 angVel2, F32 timeToSleep) +{ + sleepLinearThreshold = linVel2; + sleepAngThreshold = angVel2; + sleepTimeThreshold = timeToSleep; +} + +void Rigid::wake() +{ + if (atRest) + { + atRest = false; + sleepTimer = 0.0f; + } +} + void Rigid::getVelocity(const Point3F& r, Point3F* v) { mCross(angVelocity, r, v); diff --git a/Engine/source/T3D/rigid.h b/Engine/source/T3D/rigid.h index 17e2fe0aa..c9fa9197e 100644 --- a/Engine/source/T3D/rigid.h +++ b/Engine/source/T3D/rigid.h @@ -61,11 +61,18 @@ public: F32 oneOverMass; ///< 1 / mass F32 restitution; ///< Collision restitution F32 friction; ///< Friction coefficient + + // sleep threshold parameters + F32 sleepLinearThreshold; ///< M/S ^ 2 + F32 sleepAngThreshold; ///< R/S ^ 2 + F32 sleepTimeThreshold; ///< Seconds + F32 sleepTimer; + bool atRest; private: void translateCenterOfMass(const Point3F &oldPos,const Point3F &newPos); - + void trySleep(F32 dt); public: // Rigid(); @@ -94,6 +101,11 @@ public: bool checkRestCondition(); void setAtRest(); + + // + void setSleepThresholds(F32 linVel2, F32 angVel2, F32 timeToSleep); + void wake(); + TORQUE_FORCEINLINE void updateAngularVelocity() { invWorldInertia.mulV(angMomentum, &angVelocity); } };