Book a Demo!
CoCalc Logo Icon
StoreFeaturesDocsShareSupportNewsAboutPoliciesSign UpSign In
godotengine
GitHub Repository: godotengine/godot
 Path: blob/master/thirdparty/jolt_physics/patches/0004-backport-upstream-commit-e0a6a9a16.patch
45998 views
1
diff --git a/thirdparty/jolt_physics/Jolt/Math/Quat.h b/thirdparty/jolt_physics/Jolt/Math/Quat.h

2
index 6e7e3bef6c..1971e2ddf1 100644

3
--- a/thirdparty/jolt_physics/Jolt/Math/Quat.h

4
+++ b/thirdparty/jolt_physics/Jolt/Math/Quat.h

5
@@ -111,6 +111,9 @@ public:

6
 	/// Get axis and angle that represents this quaternion, outAngle will always be in the range \f$[0, \pi]\f$

7
 	JPH_INLINE void				GetAxisAngle(Vec3 &outAxis, float &outAngle) const;

8
 

9
+	/// Calculate angular velocity given that this quaternion represents the rotation that is reached after inDeltaTime when starting from identity rotation

10
+	JPH_INLINE Vec3				GetAngularVelocity(float inDeltaTime) const;

11
+

12
 	/// Create quaternion that rotates a vector from the direction of inFrom to the direction of inTo along the shortest path

13
 	/// @see https://www.euclideanspace.com/maths/algebra/vectors/angleBetween/index.htm

14
 	JPH_INLINE static Quat		sFromTo(Vec3Arg inFrom, Vec3Arg inTo);

15
diff --git a/thirdparty/jolt_physics/Jolt/Math/Quat.inl b/thirdparty/jolt_physics/Jolt/Math/Quat.inl

16
index 57e1947b30..7c160a5f23 100644

17
--- a/thirdparty/jolt_physics/Jolt/Math/Quat.inl

18
+++ b/thirdparty/jolt_physics/Jolt/Math/Quat.inl

19
@@ -151,6 +151,26 @@ void Quat::GetAxisAngle(Vec3 &outAxis, float &outAngle) const

20
 	}

21
 }

22
 

23
+Vec3 Quat::GetAngularVelocity(float inDeltaTime) const

24
+{

25
+	JPH_ASSERT(IsNormalized());

26
+

27
+	// w = cos(angle / 2), ensure it is positive so that we get an angle in the range [0, PI]

28
+	Quat w_pos = EnsureWPositive();

29
+

30
+	// The imaginary part of the quaternion is axis * sin(angle / 2),

31
+	// if the length is small use the approximation sin(x) = x to calculate angular velocity

32
+	Vec3 xyz = w_pos.GetXYZ();

33
+	float xyz_len_sq = xyz.LengthSq();

34
+	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)

35
+		return (2.0f / inDeltaTime) * xyz;

36
+

37
+	// Otherwise calculate the angle from w = cos(angle / 2) and determine the axis by normalizing the imaginary part

38
+	// 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.

39
+	float angle = 2.0f * ACos(w_pos.GetW());

40
+	return (xyz / (sqrt(xyz_len_sq) * inDeltaTime)) * angle;

41
+}

42
+

43
 Quat Quat::sFromTo(Vec3Arg inFrom, Vec3Arg inTo)

44
 {

45
 	/*

46
diff --git a/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl

47
index 474ab15c18..e6caae61f5 100644

48
--- a/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl

49
+++ b/thirdparty/jolt_physics/Jolt/Physics/Body/MotionProperties.inl

50
@@ -17,10 +17,7 @@ void MotionProperties::MoveKinematic(Vec3Arg inDeltaPosition, QuatArg inDeltaRot

51
 	mLinearVelocity = LockTranslation(inDeltaPosition / inDeltaTime);

52
 

53
 	// Calculate required angular velocity

54
-	Vec3 axis;

55
-	float angle;

56
-	inDeltaRotation.GetAxisAngle(axis, angle);

57
-	mAngularVelocity = LockAngular(axis * (angle / inDeltaTime));

58
+	mAngularVelocity = LockAngular(inDeltaRotation.GetAngularVelocity(inDeltaTime));

59
 }

60
 

61
 void MotionProperties::ClampLinearVelocity()

62


63