diff --git a/Engine/source/T3D/rigidShape.cpp b/Engine/source/T3D/rigidShape.cpp index 2fb3eee8d..c3e5f22dd 100644 --- a/Engine/source/T3D/rigidShape.cpp +++ b/Engine/source/T3D/rigidShape.cpp @@ -900,9 +900,11 @@ bool RigidShape::onNewDataBlock(GameBaseData* dptr, bool reload) mRigid.restitution = mDataBlock->body.restitution; mRigid.setCenterOfMass(mDataBlock->massCenter); - // Ignores massBox, just set sphere for now. Derived objects - // can set what they want. - mRigid.setObjectInertia(); + // Set inertial tensor, default for the RigidShape is sphere + if (mDataBlock->massBox.x > 0 && mDataBlock->massBox.y > 0 && mDataBlock->massBox.z > 0) + mRigid.setObjectInertia(mDataBlock->massBox); + else + mRigid.setObjectInertia(mObjBox.maxExtents - mObjBox.minExtents); scriptOnNewDataBlock(); @@ -1115,9 +1117,9 @@ void RigidShape::updatePos(F32 dt) if (mCollisionList.getCount()) { F32 k = mRigid.getKineticEnergy(); - F32 G = mNetGravity * dt; + F32 G = mNetGravity; F32 Kg = 0.5 * mRigid.mass * G * G; - if (k < sRestTol * Kg && ++restCount > sRestCount) + if (k < sRestTol * Kg* dt && ++restCount > sRestCount) mRigid.setAtRest(); } else @@ -1222,10 +1224,6 @@ void RigidShape::updateForces(F32 dt) mRigid.force = force; mRigid.torque = torque; - - // If we're still atRest, make sure we're not accumulating anything - if ((force.lenSquared() < mDataBlock->contactTol)&& (force.lenSquared() < mDataBlock->contactTol)) - mRigid.setAtRest(); } @@ -1260,8 +1258,8 @@ bool RigidShape::updateCollision(F32 dt) } // Resolve collisions - bool collided = resolveCollision(mRigid,mCollisionList); - resolveContacts(mRigid,mCollisionList,dt); + bool collided = resolveCollision(mRigid,mCollisionList, dt); + //resolveContacts(mRigid,mCollisionList,dt); return collided; } @@ -1271,11 +1269,12 @@ bool RigidShape::updateCollision(F32 dt) Handle collision impacts, as opposed to contacts. Impulses are calculated based on standard collision resolution formulas. */ -bool RigidShape::resolveCollision(Rigid& ns,CollisionList& cList) +bool RigidShape::resolveCollision(Rigid& ns,CollisionList& cList, F32 dt) { PROFILE_SCOPE(RigidShape_resolveCollision); // Apply impulses to resolve collision bool collided = false; + Point3F t, p(0, 0, 0), l(0, 0, 0); for (S32 i = 0; i < cList.getCount(); i++) { Collision& c = cList[i]; @@ -1288,9 +1287,7 @@ bool RigidShape::resolveCollision(Rigid& ns,CollisionList& cList) F32 vn = mDot(v, c.normal); // Only interested in velocities greater than sContactTol, - // velocities less than that will be dealt with as contacts - // "constraints". - if (vn < -mDataBlock->contactTol) + if (mFabs(vn) > mDataBlock->contactTol) { // Apply impulses to the rigid body to keep it from @@ -1313,8 +1310,44 @@ bool RigidShape::resolveCollision(Rigid& ns,CollisionList& cList) queueCollision(col, v - col->getVelocity()); } } + // velocities less than that will be dealt with as contacts + // "constraints". + else + { + // Penetration force. This is actually a spring which + // will seperate the body from the collision surface. + F32 zi = 2 * mFabs(mRigid.getZeroImpulse(r, c.normal) / dt); + F32 s = mMax((mDataBlock->collisionTol - c.distance) * zi - ((vn / 2.0) * zi),0.0f); + Point3F f = c.normal * s; + + // Friction impulse, calculated as a function of the + // amount of force it would take to stop the motion + // perpendicular to the normal. + Point3F uv = v - (c.normal * vn); + F32 ul = uv.len(); + if (s > 0 && ul) + { + uv /= -ul; + F32 u = ul * ns.getZeroImpulse(r, uv) / dt; + s *= mRigid.friction; + if (u > s) + u = s; + f += uv * u; + } + + // Accumulate forces + p += f; + mCross(r, f, &t); + l += t; + + } } } + // Contact constraint forces act over time... + ns.linMomentum += p * dt; + ns.angMomentum += l * dt; + ns.updateVelocity(); + return collided; } @@ -1350,7 +1383,7 @@ bool RigidShape::resolveContacts(Rigid& ns,CollisionList& cList,F32 dt) // Penetration force. This is actually a spring which // will seperate the body from the collision surface. - F32 zi = 2 * mFabs(mRigid.getZeroImpulse(r,c.normal)); + F32 zi = 2 * mFabs(mRigid.getZeroImpulse(r,c.normal) / dt); F32 s = (mDataBlock->collisionTol - c.distance) * zi - ((vn / mDataBlock->contactTol) * zi); Point3F f = c.normal * s; @@ -1362,7 +1395,7 @@ bool RigidShape::resolveContacts(Rigid& ns,CollisionList& cList,F32 dt) if (s > 0 && ul) { uv /= -ul; - F32 u = ul * ns.getZeroImpulse(r,uv); + F32 u = ul * ns.getZeroImpulse(r,uv) / dt; s *= mRigid.friction; if (u > s) u = s; @@ -1693,6 +1726,8 @@ 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), + "Debug read of the rest state. do not set"); Parent::initPersistFields(); } diff --git a/Engine/source/T3D/rigidShape.h b/Engine/source/T3D/rigidShape.h index 1e465557c..5eb470455 100644 --- a/Engine/source/T3D/rigidShape.h +++ b/Engine/source/T3D/rigidShape.h @@ -216,8 +216,8 @@ class RigidShape: public ShapeBase bool onNewDataBlock( GameBaseData *dptr, bool reload ); void updatePos(F32 dt); bool updateCollision(F32 dt); - bool resolveCollision(Rigid& ns,CollisionList& cList); - bool resolveContacts(Rigid& ns,CollisionList& cList,F32 dt); + bool resolveCollision(Rigid& ns,CollisionList& cList, F32 dt); + bool resolveContacts(Rigid& ns,CollisionList& cList, F32 dt); bool resolveDisplacement(Rigid& ns,CollisionState *state,F32 dt); void checkTriggers(); static void findCallback(SceneObject* obj,void * key); diff --git a/Engine/source/T3D/shapeBase.h b/Engine/source/T3D/shapeBase.h index 576c23e7f..ae823b6b9 100644 --- a/Engine/source/T3D/shapeBase.h +++ b/Engine/source/T3D/shapeBase.h @@ -91,7 +91,7 @@ class SFXProfile; typedef void* Light; -const F32 gGravity = -20; +const F32 gGravity = -9.8f; //-------------------------------------------------------------------------- diff --git a/Engine/source/T3D/vehicles/vehicle.cpp b/Engine/source/T3D/vehicles/vehicle.cpp index 27d1ef922..fcddd5cc0 100644 --- a/Engine/source/T3D/vehicles/vehicle.cpp +++ b/Engine/source/T3D/vehicles/vehicle.cpp @@ -807,9 +807,9 @@ void Vehicle::updatePos(F32 dt) if (mCollisionList.getCount()) { F32 k = mRigid.getKineticEnergy(); - F32 G = mNetGravity * dt; + F32 G = mNetGravity; F32 Kg = 0.5 * mRigid.mass * G * G; - if (k < sRestTol * Kg && ++restCount > sRestCount) + if (k < sRestTol * Kg * dt && ++restCount > sRestCount) mRigid.setAtRest(); } else