nick/godot
1
0
Fork 0
mirror of https://github.com/godotengine/godot.git synced 2026-02-07 19:32:36 +00:00
Code Issues Packages Projects Releases Wiki Activity

Merge pull request #115305 from jrouwe/115169_v3

Browse Source 
Jolt Physics: Rework how gravity is applied to dynamic bodies to prevent energy increase on elastic collisions
This commit is contained in:
Thaddeus Crews
2026-01-26 15:01:56 -06:00
parent d97000d7d1 be27d23116
commit 657d06a8f3
3 changed files with 60 additions and 56 deletions
Download Patch File Download Diff File Expand all files Collapse all files

16
modules/jolt_physics/objects/jolt_body_3d.cpp
View File

@@ -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));
}
}

94
thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp vendored
View File

@@ -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,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 <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 +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<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 +771,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 +790,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 +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<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

6
thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.h vendored
View File

@@ -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 <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;
@@ -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 <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>