diff --git a/thirdparty/README.md b/thirdparty/README.md index e872d1a8505d..2db3bdc95c19 100644 --- a/thirdparty/README.md +++ b/thirdparty/README.md @@ -501,6 +501,16 @@ Files extracted from upstream source: - All files in `Jolt/`, except `Jolt/Jolt.cmake` and any files dependent on `ENABLE_OBJECT_STREAM`, as seen in `Jolt/Jolt.cmake` - `LICENSE` +Patches: + +- `0001-backport-upstream-commit-b385bc3d7.patch` ([GH-111087](https://github.com/godotengine/godot/pull/111087)) +- `0002-backport-upstream-commit-ccfe0a0df.patch` ([GH-111408](https://github.com/godotengine/godot/pull/111408)) +- `0003-backport-upstream-commit-9e48d59be.patch` ([GH-111767](https://github.com/godotengine/godot/pull/111767)) +- `0004-backport-upstream-commit-ee3725250.patch` ([GH-115089](https://github.com/godotengine/godot/pull/115089)) +- `0005-backport-upstream-commit-bc7f1fb8c.patch` ([GH-115305](https://github.com/godotengine/godot/pull/115305)) +- `0006-backport-upstream-commit-365a15367.patch` ([GH-115305](https://github.com/godotengine/godot/pull/115305)) +- `0007-backport-upstream-commit-e0a6a9a16.patch` ([GH-115327](https://github.com/godotengine/godot/pull/115327)) + ## libbacktrace diff --git a/thirdparty/jolt_physics/patches/0001-backport-upstream-commit-b385bc3d7.patch b/thirdparty/jolt_physics/patches/0001-backport-upstream-commit-b385bc3d7.patch new file mode 100644 index 000000000000..bb616412e859 --- /dev/null +++ b/thirdparty/jolt_physics/patches/0001-backport-upstream-commit-b385bc3d7.patch @@ -0,0 +1,28 @@ +diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h +index ec84776a46..387c0d9737 100644 +--- a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h ++++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ConstraintPart/RotationEulerConstraintPart.h +@@ -143,8 +143,21 @@ public: + mInvI2 = inBody2.IsDynamic()? inBody2.GetMotionProperties()->GetInverseInertiaForRotation(inRotation2) : Mat44::sZero(); + + // Calculate effective mass: K^-1 = (J M^-1 J^T)^-1 +- if (!mEffectiveMass.SetInversed3x3(mInvI1 + mInvI2)) +- Deactivate(); ++ Mat44 inertia_sum = mInvI1 + mInvI2; ++ if (!mEffectiveMass.SetInversed3x3(inertia_sum)) ++ { ++ // If a column is zero, the axis is locked and we set the column to identity. ++ // This does not matter because any impulse will always be multiplied with mInvI1 or mInvI2 which will result in zero for the locked coordinate. ++ Vec4 zero = Vec4::sZero(); ++ if (inertia_sum.GetColumn4(0) == zero) ++ inertia_sum.SetColumn4(0, Vec4(1, 0, 0, 0)); ++ if (inertia_sum.GetColumn4(1) == zero) ++ inertia_sum.SetColumn4(1, Vec4(0, 1, 0, 0)); ++ if (inertia_sum.GetColumn4(2) == zero) ++ inertia_sum.SetColumn4(2, Vec4(0, 0, 1, 0)); ++ if (!mEffectiveMass.SetInversed3x3(inertia_sum)) ++ Deactivate(); ++ } + } + + /// Deactivate this constraint diff --git a/thirdparty/jolt_physics/patches/0002-backport-upstream-commit-ccfe0a0df.patch b/thirdparty/jolt_physics/patches/0002-backport-upstream-commit-ccfe0a0df.patch new file mode 100644 index 000000000000..b6bfd8ca0efd --- /dev/null +++ b/thirdparty/jolt_physics/patches/0002-backport-upstream-commit-ccfe0a0df.patch @@ -0,0 +1,12 @@ +diff --git a/thirdparty/jolt_physics/Jolt/Core/Core.h b/thirdparty/jolt_physics/Jolt/Core/Core.h +index b306f5a686..b433a77946 100644 +--- a/thirdparty/jolt_physics/Jolt/Core/Core.h ++++ b/thirdparty/jolt_physics/Jolt/Core/Core.h +@@ -453,6 +453,7 @@ JPH_SUPPRESS_WARNINGS_STD_BEGIN + #include + #include + #include ++#include + #if defined(JPH_COMPILER_MSVC) || (defined(JPH_COMPILER_CLANG) && defined(_MSC_VER)) // MSVC or clang-cl + #include // for alloca + #endif diff --git a/thirdparty/jolt_physics/patches/0003-backport-upstream-commit-9e48d59be.patch b/thirdparty/jolt_physics/patches/0003-backport-upstream-commit-9e48d59be.patch new file mode 100644 index 000000000000..9d73732f4e15 --- /dev/null +++ b/thirdparty/jolt_physics/patches/0003-backport-upstream-commit-9e48d59be.patch @@ -0,0 +1,15 @@ +diff --git a/thirdparty/jolt_physics/Jolt/RegisterTypes.cpp b/thirdparty/jolt_physics/Jolt/RegisterTypes.cpp +index ccfcc1bbe0..e343dd2fa7 100644 +--- a/thirdparty/jolt_physics/Jolt/RegisterTypes.cpp ++++ b/thirdparty/jolt_physics/Jolt/RegisterTypes.cpp +@@ -74,6 +74,10 @@ void RegisterTypesInternal(uint64 inVersionID) + { + Trace("Version mismatch, make sure you compile the client code with the same Jolt version and compiler definitions!"); + uint64 mismatch = JPH_VERSION_ID ^ inVersionID; ++ if (mismatch & 0xffffff) ++ Trace("Client reported version %d.%d.%d, library version is %d.%d.%d.", ++ (inVersionID >> 16) & 0xff, (inVersionID >> 8) & 0xff, inVersionID & 0xff, ++ JPH_VERSION_MAJOR, JPH_VERSION_MINOR, JPH_VERSION_PATCH); + auto check_bit = [mismatch](int inBit, const char *inLabel) { if (mismatch & (uint64(1) << (inBit + 23))) Trace("Mismatching define %s.", inLabel); }; + check_bit(1, "JPH_DOUBLE_PRECISION"); + check_bit(2, "JPH_CROSS_PLATFORM_DETERMINISTIC"); diff --git a/thirdparty/jolt_physics/patches/0004-backport-upstream-commit-ee3725250.patch b/thirdparty/jolt_physics/patches/0004-backport-upstream-commit-ee3725250.patch new file mode 100644 index 000000000000..466ca0970825 --- /dev/null +++ b/thirdparty/jolt_physics/patches/0004-backport-upstream-commit-ee3725250.patch @@ -0,0 +1,53 @@ +diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.cpp +index e8a8c21459..4fb6c58c18 100644 +--- a/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.cpp ++++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CastConvexVsTriangles.cpp +@@ -92,11 +92,14 @@ void CastConvexVsTriangles::Cast(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, uint8 + static_cast(mShapeCast.mShape)->GetSupportingFace(SubShapeID(), transform_1_to_2.Multiply3x3Transposed(-contact_normal), mShapeCast.mScale, mCenterOfMassTransform2 * transform_1_to_2, result.mShape1Face); + + // Get face of the triangle +- triangle.GetSupportingFace(contact_normal, result.mShape2Face); ++ result.mShape2Face.resize(3); ++ result.mShape2Face[0] = mCenterOfMassTransform2 * v0; ++ result.mShape2Face[1] = mCenterOfMassTransform2 * v1; ++ result.mShape2Face[2] = mCenterOfMassTransform2 * v2; + +- // Convert to world space +- for (Vec3 &p : result.mShape2Face) +- p = mCenterOfMassTransform2 * p; ++ // When inside out, we need to swap the triangle winding ++ if (mScaleSign < 0.0f) ++ std::swap(result.mShape2Face[1], result.mShape2Face[2]); + } + + JPH_IF_TRACK_NARROWPHASE_STATS(TrackNarrowPhaseCollector track;) +diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp +index e03659454d..05c21eebfa 100644 +--- a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp ++++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideConvexVsTriangles.cpp +@@ -145,6 +145,10 @@ void CollideConvexVsTriangles::Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, + result.mShape2Face[0] = mTransform1 * v0; + result.mShape2Face[1] = mTransform1 * v1; + result.mShape2Face[2] = mTransform1 * v2; ++ ++ // When inside out, we need to swap the triangle winding ++ if (mScaleSign2 < 0.0f) ++ std::swap(result.mShape2Face[1], result.mShape2Face[2]); + } + + // Notify the collector +diff --git a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.cpp b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.cpp +index 18441ea73b..566efb38ae 100644 +--- a/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.cpp ++++ b/thirdparty/jolt_physics/Jolt/Physics/Collision/CollideSphereVsTriangles.cpp +@@ -111,6 +111,10 @@ void CollideSphereVsTriangles::Collide(Vec3Arg inV0, Vec3Arg inV1, Vec3Arg inV2, + result.mShape2Face[0] = mTransform2 * (mSphereCenterIn2 + v0); + result.mShape2Face[1] = mTransform2 * (mSphereCenterIn2 + v1); + result.mShape2Face[2] = mTransform2 * (mSphereCenterIn2 + v2); ++ ++ // When inside out, we need to swap the triangle winding ++ if (mScaleSign2 < 0.0f) ++ std::swap(result.mShape2Face[1], result.mShape2Face[2]); + } + + // Notify the collector diff --git a/thirdparty/jolt_physics/patches/0005-backport-upstream-commit-bc7f1fb8c.patch b/thirdparty/jolt_physics/patches/0005-backport-upstream-commit-bc7f1fb8c.patch new file mode 100644 index 000000000000..61b81e808337 --- /dev/null +++ b/thirdparty/jolt_physics/patches/0005-backport-upstream-commit-bc7f1fb8c.patch @@ -0,0 +1,236 @@ +diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp +index 9e572dd829..7bb3e58ee0 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,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 +-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(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 +765,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 +784,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 +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()); +@@ -1136,11 +1140,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 +1216,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 diff --git a/thirdparty/jolt_physics/patches/0006-backport-upstream-commit-365a15367.patch b/thirdparty/jolt_physics/patches/0006-backport-upstream-commit-365a15367.patch new file mode 100644 index 000000000000..ee3a1e92ffcc --- /dev/null +++ b/thirdparty/jolt_physics/patches/0006-backport-upstream-commit-365a15367.patch @@ -0,0 +1,28 @@ +diff --git a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp +index 7bb3e58ee0..306ae64ff3 100644 +--- a/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp ++++ b/thirdparty/jolt_physics/Jolt/Physics/Constraints/ContactConstraintManager.cpp +@@ -70,7 +70,10 @@ JPH_INLINE void ContactConstraintManager::WorldContactPoint::TemplatedCalculateF + else if constexpr (Type2 != EMotionType::Static) + relative_velocity = mp2->GetPointVelocityCOM(r2); + else +- static_assert(false, "Static vs static makes no sense"); ++ { ++ JPH_ASSERT(false, "Static vs static makes no sense"); ++ relative_velocity = Vec3::sZero(); ++ } + float normal_velocity = relative_velocity.Dot(inWorldSpaceNormal); + + // How much the shapes are penetrating (> 0 if penetrating, < 0 if separated) +@@ -109,7 +112,10 @@ JPH_INLINE void ContactConstraintManager::WorldContactPoint::TemplatedCalculateF + else if constexpr (Type2 != EMotionType::Static) + relative_acceleration = inGravity * mp2->GetGravityFactor(); + else +- static_assert(false, "Static vs static makes no sense"); ++ { ++ JPH_ASSERT(false, "Static vs static makes no sense"); ++ relative_acceleration = Vec3::sZero(); ++ } + + // Calculate effect of accumulated forces + if constexpr (Type1 == EMotionType::Dynamic) diff --git a/thirdparty/jolt_physics/patches/0007-backport-upstream-commit-e0a6a9a16.patch b/thirdparty/jolt_physics/patches/0007-backport-upstream-commit-e0a6a9a16.patch new file mode 100644 index 000000000000..72788af482b6 --- /dev/null +++ b/thirdparty/jolt_physics/patches/0007-backport-upstream-commit-e0a6a9a16.patch @@ -0,0 +1,61 @@ +diff --git a/thirdparty/jolt_physics/Jolt/Math/Quat.h b/thirdparty/jolt_physics/Jolt/Math/Quat.h +index 6e7e3bef6c..1971e2ddf1 100644 +--- a/thirdparty/jolt_physics/Jolt/Math/Quat.h ++++ b/thirdparty/jolt_physics/Jolt/Math/Quat.h +@@ -111,6 +111,9 @@ public: + /// Get axis and angle that represents this quaternion, outAngle will always be in the range \f$[0, \pi]\f$ + JPH_INLINE void GetAxisAngle(Vec3 &outAxis, float &outAngle) const; + ++ /// Calculate angular velocity given that this quaternion represents the rotation that is reached after inDeltaTime when starting from identity rotation ++ JPH_INLINE Vec3 GetAngularVelocity(float inDeltaTime) const; ++ + /// Create quaternion that rotates a vector from the direction of inFrom to the direction of inTo along the shortest path + /// @see https://www.euclideanspace.com/maths/algebra/vectors/angleBetween/index.htm + JPH_INLINE static Quat sFromTo(Vec3Arg inFrom, Vec3Arg inTo); +diff --git a/thirdparty/jolt_physics/Jolt/Math/Quat.inl b/thirdparty/jolt_physics/Jolt/Math/Quat.inl +index 57e1947b30..7c160a5f23 100644 +--- a/thirdparty/jolt_physics/Jolt/Math/Quat.inl ++++ b/thirdparty/jolt_physics/Jolt/Math/Quat.inl +@@ -151,6 +151,26 @@ void Quat::GetAxisAngle(Vec3 &outAxis, float &outAngle) const + } + } + ++Vec3 Quat::GetAngularVelocity(float inDeltaTime) const ++{ ++ JPH_ASSERT(IsNormalized()); ++ ++ // w = cos(angle / 2), ensure it is positive so that we get an angle in the range [0, PI] ++ Quat w_pos = EnsureWPositive(); ++ ++ // The imaginary part of the quaternion is axis * sin(angle / 2), ++ // if the length is small use the approximation sin(x) = x to calculate angular velocity ++ Vec3 xyz = w_pos.GetXYZ(); ++ float xyz_len_sq = xyz.LengthSq(); ++ if (xyz_len_sq < 4.0e-4f) // Max error introduced is sin(0.02) - 0.02 = 7e-5 (when w is near 1 the angle becomes more inaccurate in the code below, so don't make this number too small) ++ return (2.0f / inDeltaTime) * xyz; ++ ++ // Otherwise calculate the angle from w = cos(angle / 2) and determine the axis by normalizing the imaginary part ++ // Note that it is also possible to calculate the angle through angle = 2 * atan2(|xyz|, w). This is more accurate but also 2x as expensive. ++ float angle = 2.0f * ACos(w_pos.GetW()); ++ return (xyz / (sqrt(xyz_len_sq) * inDeltaTime)) * angle; ++} ++ + Quat Quat::sFromTo(Vec3Arg inFrom, Vec3Arg inTo) + { + /* +diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl +index 474ab15c18..e6caae61f5 100644 +--- a/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl ++++ b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl +@@ -17,10 +17,7 @@ void MotionProperties::MoveKinematic(Vec3Arg inDeltaPosition, QuatArg inDeltaRot + mLinearVelocity = LockTranslation(inDeltaPosition / inDeltaTime); + + // Calculate required angular velocity +- Vec3 axis; +- float angle; +- inDeltaRotation.GetAxisAngle(axis, angle); +- mAngularVelocity = LockAngular(axis * (angle / inDeltaTime)); ++ mAngularVelocity = LockAngular(inDeltaRotation.GetAngularVelocity(inDeltaTime)); + } + + void MotionProperties::ClampLinearVelocity()