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); } }; diff --git a/Engine/source/T3D/rigidShape.cpp b/Engine/source/T3D/rigidShape.cpp index b87bcc056..98aec96e8 100644 --- a/Engine/source/T3D/rigidShape.cpp +++ b/Engine/source/T3D/rigidShape.cpp @@ -1253,6 +1253,9 @@ bool RigidShape::updateCollision(F32 dt) mRigid.getTransform(&mat); cmat = mConvex.getTransform(); + SceneObject* mounted; + for (mounted = getMountList(); mounted; mounted = mounted->getMountLink()) + mounted->disableCollision(); mCollisionList.clear(); CollisionState *state = mConvex.findClosestState(cmat, getScale(), mDataBlock->collisionTol); if (state && state->mDist <= mDataBlock->collisionTol) @@ -1260,9 +1263,12 @@ bool RigidShape::updateCollision(F32 dt) //resolveDisplacement(ns,state,dt); mConvex.getCollisionInfo(cmat, getScale(), &mCollisionList, mDataBlock->collisionTol); } - // Resolve collisions bool collided = resolveCollision(mRigid,mCollisionList, dt); + + for (mounted = getMountList(); mounted; mounted = mounted->getMountLink()) + mounted->enableCollision(); + return collided; } @@ -1682,7 +1688,7 @@ void RigidShape::initPersistFields() docsURL; addField("disableMove", TypeBool, Offset(mDisableMove, RigidShape), "When this flag is set, the vehicle will ignore throttle changes."); - addField("isAtRest", TypeBool, Offset(mRigid.atRest, RigidShape), + addProtectedField("isAtRest", TypeBool, Offset(mRigid.atRest, RigidShape), &defaultProtectedNotSetFn, &defaultProtectedGetFn, "Debug read of the rest state. do not set"); Parent::initPersistFields(); } diff --git a/Engine/source/T3D/vehicles/wheeledVehicle.cpp b/Engine/source/T3D/vehicles/wheeledVehicle.cpp index ec107eedc..44b8b0623 100644 --- a/Engine/source/T3D/vehicles/wheeledVehicle.cpp +++ b/Engine/source/T3D/vehicles/wheeledVehicle.cpp @@ -821,7 +821,7 @@ void WheeledVehicle::advanceTime(F32 dt) // Stick the wheels to the ground. This is purely so they look // good while the vehicle is being interpolated. - //extendWheels(); + extendWheels(isClientObject()); // Update wheel angular position and slip, this is a client visual // feature only, it has no affect on the physics. @@ -1201,6 +1201,7 @@ void WheeledVehicle::extendWheels(bool clientHack) wheel->surface.pos = rInfo.point; wheel->surface.material = rInfo.material; wheel->surface.object = rInfo.object; + wheel->slipping = false; } else { @@ -1475,35 +1476,36 @@ void WheeledVehicle::writePacketData(GameConnection *connection, BitStream *stre { Parent::writePacketData(connection, stream); stream->writeFlag(mBraking); - - Wheel* wend = &mWheel[mDataBlock->wheelCount]; - for (Wheel* wheel = mWheel; wheel < wend; wheel++) - { - stream->write(wheel->avel); - stream->write(wheel->Dy); - stream->write(wheel->Dx); - stream->writeFlag(wheel->slipping); - } + } void WheeledVehicle::readPacketData(GameConnection *connection, BitStream *stream) { Parent::readPacketData(connection, stream); mBraking = stream->readFlag(); - + Wheel* wend = &mWheel[mDataBlock->wheelCount]; + for (Wheel* wheel = mWheel; wheel < wend; wheel++) { - stream->read(&wheel->avel); - stream->read(&wheel->Dy); - stream->read(&wheel->Dx); - wheel->slipping = stream->readFlag(); + if (wheel->tire && wheel->spring) { + // Update angular position + wheel->apos += (wheel->avel) / M_2PI; + wheel->apos -= mFloor(wheel->apos); + if (wheel->apos < 0) + wheel->apos = 1 - wheel->apos; + } } // Rigid state is transmitted by the parent... setPosition(mRigid.linPosition,mRigid.angPosition); mDelta.pos = mRigid.linPosition; mDelta.rot[1] = mRigid.angPosition; + + // Stick the wheels to the ground. This is purely so they look + // good while the vehicle is being interpolated. + extendWheels(isClientObject()); + updateWheelThreads(); }