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()