From 3c7d2b1b6ae1d48e9c5c7ea011f91bc9270591a5 Mon Sep 17 00:00:00 2001 From: AzaezelX Date: Thu, 28 Dec 2023 21:04:16 -0600 Subject: [PATCH 1/5] physics notes based on https://github.com/TorqueGameEngines/Torque3D/pull/1165 and after further talks with @AtomicWalrus: use the massbox or bounds box based mRigid.setObjectInertia method to reduce recirulating, combine resolvecollision and resolvecontacts clamp seperation force for contact resolution gravity normalized to earth standard (9.8,not 20) take delta-time into account *once* for kinetic energy vs gravity rest checks and for debug purposes, expose mRigid.atRest to the inspector to see if it's truly at reast or grinding calcs to minimal effect --- Engine/source/T3D/rigidShape.cpp | 69 +++++++++++++++++++------- Engine/source/T3D/rigidShape.h | 4 +- Engine/source/T3D/shapeBase.h | 2 +- Engine/source/T3D/vehicles/vehicle.cpp | 4 +- 4 files changed, 57 insertions(+), 22 deletions(-) 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 From 783a757427facc19b6e0589b43ea9cca81e3ec70 Mon Sep 17 00:00:00 2001 From: AzaezelX Date: Thu, 28 Dec 2023 21:24:34 -0600 Subject: [PATCH 2/5] filter out self-collisions --- Engine/source/T3D/rigidShape.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/Engine/source/T3D/rigidShape.cpp b/Engine/source/T3D/rigidShape.cpp index c3e5f22dd..837738d8c 100644 --- a/Engine/source/T3D/rigidShape.cpp +++ b/Engine/source/T3D/rigidShape.cpp @@ -1278,6 +1278,11 @@ bool RigidShape::resolveCollision(Rigid& ns,CollisionList& cList, F32 dt) for (S32 i = 0; i < cList.getCount(); i++) { Collision& c = cList[i]; + if (c.object == this) + { + //Con::printf("IMPOSSIBLE!!!!--------------------------------> Self-collision event?"); + continue; + } if (c.distance < mDataBlock->collisionTol) { // Velocity into surface From f3af564dee2c1d486a883f3cf8be229b1a63806f Mon Sep 17 00:00:00 2001 From: AzaezelX Date: Thu, 28 Dec 2023 21:28:23 -0600 Subject: [PATCH 3/5] clean out resolvecontacts entirely --- Engine/source/T3D/rigidShape.cpp | 67 -------------------------------- Engine/source/T3D/rigidShape.h | 1 - 2 files changed, 68 deletions(-) diff --git a/Engine/source/T3D/rigidShape.cpp b/Engine/source/T3D/rigidShape.cpp index 837738d8c..3b2d9f0db 100644 --- a/Engine/source/T3D/rigidShape.cpp +++ b/Engine/source/T3D/rigidShape.cpp @@ -1259,7 +1259,6 @@ bool RigidShape::updateCollision(F32 dt) // Resolve collisions bool collided = resolveCollision(mRigid,mCollisionList, dt); - //resolveContacts(mRigid,mCollisionList,dt); return collided; } @@ -1357,72 +1356,6 @@ bool RigidShape::resolveCollision(Rigid& ns,CollisionList& cList, F32 dt) return collided; } -//---------------------------------------------------------------------------- -/** Resolve contact forces -Resolve contact forces using the "penalty" method. Forces are generated based -on the depth of penetration and the moment of inertia at the point of contact. -*/ -bool RigidShape::resolveContacts(Rigid& ns,CollisionList& cList,F32 dt) -{ - PROFILE_SCOPE(RigidShape_resolveContacts); - // Use spring forces to manage contact constraints. - bool collided = false; - Point3F t,p(0,0,0),l(0,0,0); - for (S32 i = 0; i < cList.getCount(); i++) - { - const Collision& c = cList[i]; - if (c.distance < mDataBlock->collisionTol) - { - - // Velocity into the surface - Point3F v,r; - ns.getOriginVector(c.point,&r); - ns.getVelocity(r,&v); - F32 vn = mDot(v,c.normal); - - // Only interested in velocities less than mDataBlock->contactTol, - // velocities greater than that are dealt with as collisions. - if (mFabs(vn) < mDataBlock->contactTol) - { - collided = true; - - // 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 = (mDataBlock->collisionTol - c.distance) * zi - ((vn / mDataBlock->contactTol) * zi); - 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 true; -} - - //---------------------------------------------------------------------------- bool RigidShape::resolveDisplacement(Rigid& ns,CollisionState *state, F32 dt) diff --git a/Engine/source/T3D/rigidShape.h b/Engine/source/T3D/rigidShape.h index 5eb470455..90d960162 100644 --- a/Engine/source/T3D/rigidShape.h +++ b/Engine/source/T3D/rigidShape.h @@ -217,7 +217,6 @@ class RigidShape: public ShapeBase void updatePos(F32 dt); bool updateCollision(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); From e7b3a95f894ed2790c9fec57696717958095ac83 Mon Sep 17 00:00:00 2001 From: AzaezelX Date: Fri, 29 Dec 2023 00:26:26 -0600 Subject: [PATCH 4/5] restitution sumation review: average, don't multiply --- Engine/source/T3D/rigid.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Engine/source/T3D/rigid.cpp b/Engine/source/T3D/rigid.cpp index 31f4d411a..3dea66a6a 100644 --- a/Engine/source/T3D/rigid.cpp +++ b/Engine/source/T3D/rigid.cpp @@ -156,7 +156,7 @@ bool Rigid::resolveCollision(const Point3F& p, const Point3F &normal, Rigid* rig return false; // Compute impulse - F32 d, n = -nv * (2.0f + restitution * rigid->restitution); + F32 d, n = -nv * (1.0+(restitution + rigid->restitution)*0.5); Point3F a1,b1,c1; mCross(r1,normal,&a1); invWorldInertia.mulV(a1,&b1); From 93abe5a49cf75d05ea4de07996f9ca486bff702c Mon Sep 17 00:00:00 2001 From: AzaezelX Date: Sat, 30 Dec 2023 16:16:29 -0600 Subject: [PATCH 5/5] cancel out momentums during updateforces if disablemove increment forces on a per-contact basis, not a batch basis --- Engine/source/T3D/rigidShape.cpp | 20 ++++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/Engine/source/T3D/rigidShape.cpp b/Engine/source/T3D/rigidShape.cpp index 3b2d9f0db..faa704ba0 100644 --- a/Engine/source/T3D/rigidShape.cpp +++ b/Engine/source/T3D/rigidShape.cpp @@ -1207,8 +1207,12 @@ void RigidShape::updatePos(F32 dt) void RigidShape::updateForces(F32 dt) { - if (mDisableMove) return; - + if (mDisableMove) + { + mRigid.linVelocity = Point3F::Zero; + mRigid.angMomentum = Point3F::Zero; + return; + } Point3F torque(0, 0, 0); Point3F force(0, 0, mRigid.mass * mNetGravity); @@ -1273,7 +1277,6 @@ 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]; @@ -1318,6 +1321,7 @@ bool RigidShape::resolveCollision(Rigid& ns,CollisionList& cList, F32 dt) // "constraints". else { + Point3F t; // 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); @@ -1340,17 +1344,13 @@ bool RigidShape::resolveCollision(Rigid& ns,CollisionList& cList, F32 dt) } // Accumulate forces - p += f; mCross(r, f, &t); - l += t; - + ns.linMomentum += f * dt; + ns.angMomentum += t * dt; + ns.updateVelocity(); } } } - // Contact constraint forces act over time... - ns.linMomentum += p * dt; - ns.angMomentum += l * dt; - ns.updateVelocity(); return collided;