From be27d2311637d9306bb33421db1ce506e2668b17 Mon Sep 17 00:00:00 2001 From: Jorrit Rouwe Date: Thu, 22 Jan 2026 21:53:42 +0100 Subject: [PATCH] Jolt Physics: Reworked how gravity is applied to dynamic bodies to prevent energy increase on elastic collisions --- modules/jolt_physics/objects/jolt_body_3d.cpp | 16 ++-- .../Constraints/ContactConstraintManager.cpp | 94 ++++++++++--------- .../Constraints/ContactConstraintManager.h | 6 +- 3 files changed, 60 insertions(+), 56 deletions(-) diff --git a/modules/jolt_physics/objects/jolt_body_3d.cpp b/modules/jolt_physics/objects/jolt_body_3d.cpp index d43b6e17a6..b4ec9aeb95 100644 --- a/modules/jolt_physics/objects/jolt_body_3d.cpp +++ b/modules/jolt_physics/objects/jolt_body_3d.cpp @@ -180,23 +180,19 @@ void JoltBody3D::_integrate_forces(float p_step, JPH::Body &p_jolt_body) { if (!custom_integrator) { JPH::MotionProperties &motion_properties = *p_jolt_body.GetMotionPropertiesUnchecked(); - JPH::Vec3 linear_velocity = motion_properties.GetLinearVelocity(); - JPH::Vec3 angular_velocity = motion_properties.GetAngularVelocity(); - // Jolt applies damping differently from Godot Physics, where Godot Physics applies damping before integrating // forces whereas Jolt does it after integrating forces. The way Godot Physics does it seems to yield more // consistent results across different update frequencies when using high (>1) damping values, so we apply the // damping ourselves instead, before any force integration happens. - + JPH::Vec3 linear_velocity = motion_properties.GetLinearVelocity(); linear_velocity *= MAX(1.0f - total_linear_damp * p_step, 0.0f); + motion_properties.SetLinearVelocity(linear_velocity); + + JPH::Vec3 angular_velocity = motion_properties.GetAngularVelocity(); angular_velocity *= MAX(1.0f - total_angular_damp * p_step, 0.0f); + motion_properties.SetAngularVelocity(angular_velocity); - linear_velocity += to_jolt(gravity) * p_step; - - motion_properties.SetLinearVelocityClamped(linear_velocity); - motion_properties.SetAngularVelocityClamped(angular_velocity); - - p_jolt_body.AddForce(to_jolt(constant_force)); + p_jolt_body.AddForce(to_jolt(gravity / motion_properties.GetInverseMass() + constant_force)); p_jolt_body.AddTorque(to_jolt(constant_torque)); } } diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp index 9e572dd829..306ae64ff3 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 -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,38 +58,21 @@ 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 + JPH_ASSERT(false, "Static vs static makes no sense"); relative_velocity = Vec3::sZero(); - gravity_dt_dot_normal = 0.0f; } float normal_velocity = relative_velocity.Dot(inWorldSpaceNormal); @@ -114,11 +97,43 @@ 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 + { + JPH_ASSERT(false, "Static vs static makes no sense"); + relative_acceleration = Vec3::sZero(); + } + + // 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 +721,7 @@ void ContactConstraintManager::PrepareConstraintBuffer(PhysicsUpdateContext *inC } template -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 +752,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(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(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 +771,15 @@ inline void ContactConstraintManager::CalculateFrictionAndNonPenetrationConstrai switch (inBody2.GetMotionType()) { case EMotionType::Dynamic: - TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2); + TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravity, inTransformBody1, inTransformBody2, inBody1, inBody2); break; case EMotionType::Kinematic: - TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2); + TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravity, inTransformBody1, inTransformBody2, inBody1, inBody2); break; case EMotionType::Static: - TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2); + TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravity, inTransformBody1, inTransformBody2, inBody1, inBody2); break; default: @@ -778,12 +790,12 @@ inline void ContactConstraintManager::CalculateFrictionAndNonPenetrationConstrai case EMotionType::Kinematic: JPH_ASSERT(inBody2.IsDynamic()); - TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2); + TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravity, inTransformBody1, inTransformBody2, inBody1, inBody2); break; case EMotionType::Static: JPH_ASSERT(inBody2.IsDynamic()); - TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravityDeltaTime, inTransformBody1, inTransformBody2, inBody1, inBody2); + TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(ioConstraint, inSettings, inDeltaTime, inGravity, inTransformBody1, inTransformBody2, inBody1, inBody2); break; default: @@ -863,11 +875,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 +980,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()); @@ -1136,11 +1146,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; @@ -1214,7 +1222,7 @@ bool ContactConstraintManager::TemplatedAddContactConstraint(ContactAllocator &i wcp.mContactPoint = &cp; // Setup velocity constraint - wcp.TemplatedCalculateFrictionAndNonPenetrationConstraintProperties(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(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 635ce435fe..73e5d749b0 100644 --- a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h +++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h @@ -424,7 +424,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 - 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; @@ -482,10 +482,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 - 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