Made MoveKinematic more accurate when rotating a body by a very small angle (#1893)

See https://github.com/godotengine/godot/issues/115180

Docs/ReleaseNotes.md

@@ -23,6 +23,7 @@ For breaking API changes see [this document](https://github.com/jrouwe/JoltPhysi
 * Fixed an issue where a character could get stuck. If the character was teleported inside an area surrounded by slopes that are steeper than `mMaxSlopeAngle`, the code to stop the constraint solver from ping ponging between two planes didn't work properly.
 * Fixed an issue where collide/cast shape against a triangle would return a hit result with `mShape2Face` in incorrect winding order. This caused an incorrect normal in the enhanced internal edge removal algorithm. This in turn resulted in objects not settling properly on dense triangle grids.
 * When using `Body::AddForce` to apply gravity, bodies could gain extra energy during elastic collisions. We now cancel added forces in the direction of the contact normal if the body starts in collision to negate this energy gain.
+* Made `MoveKinematic` more accurate when rotating a body by a very small angle.
 
 ## v5.5.0
 

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

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)
 {
 	/*

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

UnitTests/Math/QuatTests.cpp

@@ -298,6 +298,51 @@ TEST_SUITE("QuatTests")
 		}
 	}
 
+	TEST_CASE("TestQuatGetAngularVelocity")
+	{
+		constexpr float cDeltaTime = 1.0f / 60.0f;
+
+		UnitTestRandom random;
+		uniform_real_distribution<float> angle_range(-JPH_PI, JPH_PI);
+		for (int i = 0; i < 1000; ++i)
+		{
+			Vec3 axis = Vec3::sRandom(random);
+			float angle = angle_range(random);
+			Vec3 expected_angular_velocity = axis * angle / cDeltaTime;
+
+			Quat q = Quat::sRotation(axis, angle);
+
+			float max_error = 1.0e-4f * expected_angular_velocity.Length();
+
+			Vec3 angular_velocity = q.GetAngularVelocity(cDeltaTime);
+			CHECK_APPROX_EQUAL(angular_velocity, expected_angular_velocity, max_error);
+
+			angular_velocity = (-q).GetAngularVelocity(cDeltaTime);
+			CHECK_APPROX_EQUAL(angular_velocity, expected_angular_velocity, max_error);
+		}
+
+		constexpr float cSmallAngle = 1.0e-6f;
+
+		// Test very small angles
+		{
+			Quat q = Quat::sRotation(Vec3::sAxisX(), cSmallAngle);
+			Vec3 angular_velocity = q.GetAngularVelocity(cDeltaTime);
+			CHECK_APPROX_EQUAL(angular_velocity, Vec3(cSmallAngle / cDeltaTime, 0, 0), 1.0e-5f * cSmallAngle);
+		}
+
+		{
+			Quat q = Quat::sRotation(-Vec3::sAxisY(), cSmallAngle);
+			Vec3 angular_velocity = q.GetAngularVelocity(cDeltaTime);
+			CHECK_APPROX_EQUAL(angular_velocity, Vec3(0, -cSmallAngle / cDeltaTime, 0), 1.0e-5f * cSmallAngle);
+		}
+
+		{
+			Quat q = -Quat::sRotation(Vec3::sAxisZ(), -cSmallAngle);
+			Vec3 angular_velocity = q.GetAngularVelocity(cDeltaTime);
+			CHECK_APPROX_EQUAL(angular_velocity, Vec3(0, 0, -cSmallAngle / cDeltaTime), 1.0e-5f * cSmallAngle);
+		}
+	}
+
 	TEST_CASE("TestQuatInverse")
 	{
 		UnitTestRandom random;

UnitTests/Physics/PhysicsTests.cpp

@@ -777,6 +777,60 @@ TEST_SUITE("PhysicsTests")
 		}
 	}
 
+
+	// Test a box moving kinematically
+	static void TestPhysicsMoveKinematic(PhysicsTestContext &ioContext)
+	{
+		const int cNumSteps = 60;
+
+		// Create box
+		Body &body = ioContext.CreateBox(RVec3::sZero(), Quat::sIdentity(), EMotionType::Kinematic, EMotionQuality::Discrete, Layers::MOVING, Vec3(1, 1, 1));
+
+		UnitTestRandom random;
+		for (int i = 0; i < 10; ++i)
+		{
+			// Remember old position
+			Quat old_rotation = body.GetRotation();
+			RVec3 old_position = body.GetPosition();
+
+			// Move to new spot
+			RVec3 position = RVec3(Vec3::sRandom(random) * 5.0f);
+			Quat rotation = Quat::sRandom(random);
+			ioContext.GetBodyInterface().MoveKinematic(body.GetID(), position, rotation, ioContext.GetDeltaTime() * cNumSteps);
+
+			// Simulate cNumSteps
+			for (int j = 0; j < cNumSteps; ++j)
+			{
+				ioContext.SimulateSingleStep();
+
+				// Check intermediate position
+				float fraction = float(j + 1) / float(cNumSteps);
+				RVec3 expected_pos = old_position + (position - old_position) * fraction;
+				CHECK_APPROX_EQUAL(body.GetPosition(), expected_pos, 1.0e-4f);
+
+				// Check intermediate rotation
+				Quat expected_rot = old_rotation.SLERP(rotation, fraction);
+				CHECK_APPROX_EQUAL(body.GetRotation(), expected_rot, 1.0e-5f);
+			}
+
+			// Check arrived
+			CHECK_APPROX_EQUAL(body.GetPosition(), position, 1.0e-4f);
+			CHECK_APPROX_EQUAL(body.GetRotation(), rotation, 1.0e-5f);
+		}
+	}
+
+	TEST_CASE("TestPhysicsMoveKinematic")
+	{
+		PhysicsTestContext c1(1.0f / 60.0f, 1);
+		TestPhysicsMoveKinematic(c1);
+
+		PhysicsTestContext c2(2.0f / 60.0f, 2);
+		TestPhysicsMoveKinematic(c2);
+
+		PhysicsTestContext c4(4.0f / 60.0f, 4);
+		TestPhysicsMoveKinematic(c4);
+	}
+
 	// Let box intersect with floor by cPenetrationSlop. It should not move, this is the maximum penetration allowed.
 	static void TestPhysicsPenetrationSlop1(PhysicsTestContext &ioContext)
 	{