diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp
index 7de3a4483e..972fc13583 100644
--- a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp
@@ -46,7 +46,7 @@ void ContactConstraintManager::WorldContactPoint::CalculateNonPenetrationConstra
}
template <EMotionType Type1, EMotionType Type2>
-JPH_INLINE void ContactConstraintManager::WorldContactPoint::TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, float inGravityDeltaTimeDotNormal, const Body &inBody1, const Body &inBody2, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal, Vec3Arg inWorldSpaceTangent1, Vec3Arg inWorldSpaceTangent2, const ContactSettings &inSettings, float inMinVelocityForRestitution)
+JPH_INLINE void ContactConstraintManager::WorldContactPoint::TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, Vec3Arg inGravity, const Body &inBody1, const Body &inBody2, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal, Vec3Arg inWorldSpaceTangent1, Vec3Arg inWorldSpaceTangent2, const ContactSettings &inSettings, float inMinVelocityForRestitution)
{
JPH_DET_LOG("TemplatedCalculateFrictionAndNonPenetrationConstraintProperties: p1: " << inWorldSpacePosition1 << " p2: " << inWorldSpacePosition2
<< " normal: " << inWorldSpaceNormal << " tangent1: " << inWorldSpaceTangent1 << " tangent2: " << inWorldSpaceTangent2
@@ -58,39 +58,19 @@ JPH_INLINE void ContactConstraintManager::WorldContactPoint::TemplatedCalculateF
Vec3 r1 = Vec3(p - inBody1.GetCenterOfMassPosition());
Vec3 r2 = Vec3(p - inBody2.GetCenterOfMassPosition());
- // The gravity is applied in the beginning of the time step. If we get here, there was a collision
- // at the beginning of the time step, so we've applied too much gravity. This means that our
- // calculated restitution can be too high, so when we apply restitution, we cancel the added
- // velocity due to gravity.
- float gravity_dt_dot_normal;
+ const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked();
+ const MotionProperties *mp2 = inBody2.GetMotionPropertiesUnchecked();
// Calculate velocity of collision points
Vec3 relative_velocity;
if constexpr (Type1 != EMotionType::Static && Type2 != EMotionType::Static)
- {
- const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked();
- const MotionProperties *mp2 = inBody2.GetMotionPropertiesUnchecked();
relative_velocity = mp2->GetPointVelocityCOM(r2) - mp1->GetPointVelocityCOM(r1);
- gravity_dt_dot_normal = inGravityDeltaTimeDotNormal * (mp2->GetGravityFactor() - mp1->GetGravityFactor());
- }
else if constexpr (Type1 != EMotionType::Static)
- {
- const MotionProperties *mp1 = inBody1.GetMotionPropertiesUnchecked();
relative_velocity = -mp1->GetPointVelocityCOM(r1);
- gravity_dt_dot_normal = inGravityDeltaTimeDotNormal * mp1->GetGravityFactor();
- }
else if constexpr (Type2 != EMotionType::Static)
- {
- const MotionProperties *mp2 = inBody2.GetMotionPropertiesUnchecked();
relative_velocity = mp2->GetPointVelocityCOM(r2);
- gravity_dt_dot_normal = inGravityDeltaTimeDotNormal * mp2->GetGravityFactor();
- }
else
- {
- JPH_ASSERT(false); // Static vs static makes no sense
- relative_velocity = Vec3::sZero();
- gravity_dt_dot_normal = 0.0f;
- }
+ static_assert(false, "Static vs static makes no sense");
float normal_velocity = relative_velocity.Dot(inWorldSpaceNormal);
// How much the shapes are penetrating (> 0 if penetrating, < 0 if separated)
@@ -114,11 +94,40 @@ JPH_INLINE void ContactConstraintManager::WorldContactPoint::TemplatedCalculateF
// position rather than from a position where it is touching the other object. This causes the object
// to appear to move faster for 1 frame (the opposite of time stealing).
if (normal_velocity < -speculative_contact_velocity_bias)
- normal_velocity_bias = inSettings.mCombinedRestitution * (normal_velocity - gravity_dt_dot_normal);
+ {
+ // The gravity / constant forces are applied in the beginning of the time step.
+ // If we get here, there was a collision at the beginning of the time step, so we've applied too much force.
+ // This means that our calculated restitution can be too high resulting in an increase in energy.
+ // So, when we apply restitution, we cancel the added velocity due to these forces.
+ Vec3 relative_acceleration;
+
+ // Calculate effect of gravity
+ if constexpr (Type1 != EMotionType::Static && Type2 != EMotionType::Static)
+ relative_acceleration = inGravity * (mp2->GetGravityFactor() - mp1->GetGravityFactor());
+ else if constexpr (Type1 != EMotionType::Static)
+ relative_acceleration = -inGravity * mp1->GetGravityFactor();
+ else if constexpr (Type2 != EMotionType::Static)
+ relative_acceleration = inGravity * mp2->GetGravityFactor();
+ else
+ static_assert(false, "Static vs static makes no sense");
+
+ // Calculate effect of accumulated forces
+ if constexpr (Type1 == EMotionType::Dynamic)
+ relative_acceleration -= mp1->GetAccumulatedForce() * mp1->GetInverseMass();
+ if constexpr (Type2 == EMotionType::Dynamic)
+ relative_acceleration += mp2->GetAccumulatedForce() * mp2->GetInverseMass();
+
+ // We only compensate forces towards the contact normal.
+ float force_delta_velocity = min(0.0f, relative_acceleration.Dot(inWorldSpaceNormal) * inDeltaTime);
+
+ normal_velocity_bias = inSettings.mCombinedRestitution * (normal_velocity - force_delta_velocity);
+ }
else
+ {
// In this case we have predicted that we don't hit the other object, but if we do (due to other constraints changing velocities)
// the speculative contact will prevent penetration but will not apply restitution leading to another artifact.
normal_velocity_bias = speculative_contact_velocity_bias;
+ }
}
else
{
@@ -706,7 +715,7 @@ void ContactConstraintManager::PrepareConstraintBuffer(PhysicsUpdateContext *inC
}
template <EMotionType Type1, EMotionType Type2>
-JPH_INLINE void ContactConstraintManager::TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2)
+JPH_INLINE void ContactConstraintManager::TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravity, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2)
{
// Calculate scaled mass and inertia
Mat44 inv_i1;
@@ -737,20 +746,17 @@ JPH_INLINE void ContactConstraintManager::TemplatedCalculateFrictionAndNonPenetr
Vec3 ws_normal = ioConstraint.GetWorldSpaceNormal();
- // Calculate value for restitution correction
- float gravity_dt_dot_normal = inGravityDeltaTime.Dot(ws_normal);
-
// Setup velocity constraint properties
float min_velocity_for_restitution = mPhysicsSettings.mMinVelocityForRestitution;
for (WorldContactPoint &wcp : ioConstraint.mContactPoints)
{
RVec3 p1 = inTransformBody1 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition1);
RVec3 p2 = inTransformBody2 * Vec3::sLoadFloat3Unsafe(wcp.mContactPoint->mPosition2);
- wcp.TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<Type1, Type2>(inDeltaTime, gravity_dt_dot_normal, inBody1, inBody2, ioConstraint.mInvMass1, ioConstraint.mInvMass2, inv_i1, inv_i2, p1, p2, ws_normal, t1, t2, inSettings, min_velocity_for_restitution);
+ wcp.TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<Type1, Type2>(inDeltaTime, inGravity, inBody1, inBody2, ioConstraint.mInvMass1, ioConstraint.mInvMass2, inv_i1, inv_i2, p1, p2, ws_normal, t1, t2, inSettings, min_velocity_for_restitution);
}
}
-inline void ContactConstraintManager::CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2)
+inline void ContactConstraintManager::CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravity, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2)
{
// Dispatch to the correct templated form
switch (inBody1.GetMotionType())
@@ -759,15 +765,15 @@ inline void ContactConstraintManager::CalculateFrictionAndNonPenetrationConstrai
switch (inBody2.GetMotionType())
{
case EMotionType::Dynamic:
- TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+ TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inGravity, inTransformBody1, inTransformBody2, inBody1, inBody2);
break;
case EMotionType::Kinematic:
- TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Kinematic>(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+ TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Kinematic>(ioConstraint, inSettings, inDeltaTime, inGravity, inTransformBody1, inTransformBody2, inBody1, inBody2);
break;
case EMotionType::Static:
- TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Static>(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+ TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Dynamic, EMotionType::Static>(ioConstraint, inSettings, inDeltaTime, inGravity, inTransformBody1, inTransformBody2, inBody1, inBody2);
break;
default:
@@ -778,12 +784,12 @@ inline void ContactConstraintManager::CalculateFrictionAndNonPenetrationConstrai
case EMotionType::Kinematic:
JPH_ASSERT(inBody2.IsDynamic());
- TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Kinematic, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+ TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Kinematic, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inGravity, inTransformBody1, inTransformBody2, inBody1, inBody2);
break;
case EMotionType::Static:
JPH_ASSERT(inBody2.IsDynamic());
- TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Static, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2);
+ TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<EMotionType::Static, EMotionType::Dynamic>(ioConstraint, inSettings, inDeltaTime, inGravity, inTransformBody1, inTransformBody2, inBody1, inBody2);
break;
default:
@@ -863,11 +869,9 @@ void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactA
RMat44 transform_body1 = body1->GetCenterOfMassTransform();
RMat44 transform_body2 = body2->GetCenterOfMassTransform();
- // Get time step
+ // Get time step and gravity
float delta_time = mUpdateContext->mStepDeltaTime;
-
- // Calculate value for restitution correction
- Vec3 gravity_dt = mUpdateContext->mPhysicsSystem->GetGravity() * delta_time;
+ Vec3 gravity = mUpdateContext->mPhysicsSystem->GetGravity();
// Copy manifolds
uint32 output_handle = ManifoldMap::cInvalidHandle;
@@ -970,7 +974,7 @@ void ContactConstraintManager::GetContactsFromCache(ContactAllocator &ioContactA
JPH_DET_LOG("GetContactsFromCache: id1: " << constraint.mBody1->GetID() << " id2: " << constraint.mBody2->GetID() << " key: " << constraint.mSortKey);
// Calculate friction and non-penetration constraint properties for all contact points
- CalculateFrictionAndNonPenetrationConstraintProperties(constraint, settings, delta_time, gravity_dt, transform_body1, transform_body2, *body1, *body2);
+ CalculateFrictionAndNonPenetrationConstraintProperties(constraint, settings, delta_time, gravity, transform_body1, transform_body2, *body1, *body2);
// Notify island builder
mUpdateContext->mIslandBuilder->LinkContact(constraint_idx, body1->GetIndexInActiveBodiesInternal(), body2->GetIndexInActiveBodiesInternal());
@@ -1144,11 +1148,9 @@ bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
// Notify island builder
mUpdateContext->mIslandBuilder->LinkContact(constraint_idx, inBody1.GetIndexInActiveBodiesInternal(), inBody2.GetIndexInActiveBodiesInternal());
- // Get time step
+ // Get time step and gravity
float delta_time = mUpdateContext->mStepDeltaTime;
-
- // Calculate value for restitution correction
- float gravity_dt_dot_normal = inManifold.mWorldSpaceNormal.Dot(mUpdateContext->mPhysicsSystem->GetGravity() * delta_time);
+ Vec3 gravity = mUpdateContext->mPhysicsSystem->GetGravity();
// Calculate scaled mass and inertia
float inv_m1;
@@ -1222,7 +1224,7 @@ bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i
wcp.mContactPoint = &cp;
// Setup velocity constraint
- wcp.TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<Type1, Type2>(delta_time, gravity_dt_dot_normal, inBody1, inBody2, inv_m1, inv_m2, inv_i1, inv_i2, p1_ws, p2_ws, inManifold.mWorldSpaceNormal, t1, t2, settings, mPhysicsSettings.mMinVelocityForRestitution);
+ wcp.TemplatedCalculateFrictionAndNonPenetrationConstraintProperties<Type1, Type2>(delta_time, gravity, inBody1, inBody2, inv_m1, inv_m2, inv_i1, inv_i2, p1_ws, p2_ws, inManifold.mWorldSpaceNormal, t1, t2, settings, mPhysicsSettings.mMinVelocityForRestitution);
}
#ifdef JPH_DEBUG_RENDERER
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h
index 0b5938b77e..dfe5fc8c38 100644
--- a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h
+++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h
@@ -427,7 +427,7 @@ private:
void CalculateNonPenetrationConstraintProperties(const Body &inBody1, float inInvMass1, float inInvInertiaScale1, const Body &inBody2, float inInvMass2, float inInvInertiaScale2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal);
template <EMotionType Type1, EMotionType Type2>
- JPH_INLINE void TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, float inGravityDeltaTimeDotNormal, const Body &inBody1, const Body &inBody2, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal, Vec3Arg inWorldSpaceTangent1, Vec3Arg inWorldSpaceTangent2, const ContactSettings &inSettings, float inMinVelocityForRestitution);
+ JPH_INLINE void TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(float inDeltaTime, Vec3Arg inGravity, const Body &inBody1, const Body &inBody2, float inInvM1, float inInvM2, Mat44Arg inInvI1, Mat44Arg inInvI2, RVec3Arg inWorldSpacePosition1, RVec3Arg inWorldSpacePosition2, Vec3Arg inWorldSpaceNormal, Vec3Arg inWorldSpaceTangent1, Vec3Arg inWorldSpaceTangent2, const ContactSettings &inSettings, float inMinVelocityForRestitution);
/// The constraint parts
AxisConstraintPart mNonPenetrationConstraint;
@@ -485,10 +485,10 @@ public:
private:
/// Internal helper function to calculate the friction and non-penetration constraint properties. Templated to the motion type to reduce the amount of branches and calculations.
template <EMotionType Type1, EMotionType Type2>
- JPH_INLINE void TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2);
+ JPH_INLINE void TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravity, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2);
/// Internal helper function to calculate the friction and non-penetration constraint properties.
- inline void CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravityDeltaTime, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2);
+ inline void CalculateFrictionAndNonPenetrationConstraintProperties(ContactConstraint &ioConstraint, const ContactSettings &inSettings, float inDeltaTime, Vec3Arg inGravity, RMat44Arg inTransformBody1, RMat44Arg inTransformBody2, const Body &inBody1, const Body &inBody2);
/// Internal helper function to add a contact constraint. Templated to the motion type to reduce the amount of branches and calculations.
template <EMotionType Type1, EMotionType Type2>