From 67d385e910b59378e6dd8ac69c1dab8ee2d41c87 Mon Sep 17 00:00:00 2001 From: Jorrit Rouwe Date: Sat, 24 Jan 2026 10:51:17 +0100 Subject: [PATCH] Jolt Physics: Made MoveKinematic more accurate when rotating a body by a very small angle --- thirdparty/jolt_physics/Jolt/Math/Quat.h | 3 +++ thirdparty/jolt_physics/Jolt/Math/Quat.inl | 20 +++++++++++++++++++ .../Jolt/Physics/Body/MotionProperties.inl | 5 +---- 3 files changed, 24 insertions(+), 4 deletions(-) diff --git a/thirdparty/jolt_physics/Jolt/Math/Quat.h b/thirdparty/jolt_physics/Jolt/Math/Quat.h index 6e7e3bef6ce..1971e2ddf11 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 57e1947b306..7c160a5f230 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 474ab15c18c..e6caae61f51 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()