61 lines
3.1 KiB
Diff
61 lines
3.1 KiB
Diff
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()
|