Implemented approximate ACos function (#1059)

This speeds up the dihedral bend constraints by approximately 10%

Jolt/Math/Trigonometry.h

@@ -44,6 +44,25 @@ JPH_INLINE float ACos(float inX)
 	return Vec4::sReplicate(inX).ACos().GetX();
 }
 
+/// An approximation of ACos, max error is 4.2e-3 over the entire range [-1, 1], is approximately 2.5x faster than ACos
+JPH_INLINE float ACosApproximate(float inX)
+{
+	// See: https://www.johndcook.com/blog/2022/09/06/inverse-cosine-near-1/
+	// Taylor of cos(x) = 1 - x^2 / 2 + ...
+	// Substitute x = sqrt(2 y) we get: cos(sqrt(2 y)) = 1 - y
+	// Substitute z = 1 - y we get: cos(sqrt(2 (1 - z))) = z <=> acos(z) = sqrt(2 (1 - z))
+	// To avoid the discontinuity at 1, instead of using the Taylor expansion of acos(x) we use acos(x) / sqrt(2 (1 - x)) = 1 + (1 - x) / 12 + ...
+	// Since the approximation was made at 1, it has quite a large error at 0 meaning that if we want to extend to the
+	// range [-1, 1] by mirroring the range [0, 1], the value at 0+ is not the same as 0-.
+	// So we observe that the form of the Taylor expansion is f(x) = sqrt(1 - x) * (a + b x) and we fit the function so that f(0) = pi / 2
+	// this gives us a = pi / 2. f(1) = 0 regardless of b. We search for a constant b that minimizes the error in the range [0, 1].
+	float abs_x = min(abs(inX), 1.0f); // Ensure that we don't get a value larger than 1
+	float val = sqrt(1.0f - abs_x) * (JPH_PI / 2 - 0.175394f * abs_x);
+
+	// Our approximation is valid in the range [0, 1], extend it to the range [-1, 1]
+	return inX < 0? JPH_PI - val : val;
+}
+
 /// Arc tangent of x (returns value in the range [-PI / 2, PI / 2])
 JPH_INLINE float ATan(float inX)
 {

Jolt/Physics/SoftBody/SoftBodyMotionProperties.cpp

@@ -299,7 +299,7 @@ void SoftBodyMotionProperties::ApplyDihedralBendConstraints(const SoftBodyUpdate
 		// As per "Strain Based Dynamics" Appendix A we need to negate the gradients when (n1 x n2) . e > 0, instead we make sure that the sign of the constraint equation is correct
 		float sign = Sign(n2.Cross(n1).Dot(e));
 		float d = n1.Dot(n2) / sqrt(n1_len_sq_n2_len_sq);
-		float c = sign * ACos(d) - b->mInitialAngle;
+		float c = sign * ACosApproximate(d) - b->mInitialAngle;
 
 		// Ensure the range is -PI to PI
 		if (c > JPH_PI)

Jolt/Physics/SoftBody/SoftBodySharedSettings.cpp

@@ -391,7 +391,7 @@ void SoftBodySharedSettings::CalculateBendConstraintConstants()
 		else
 		{
 			float sign = Sign(n2.Cross(n1).Dot(e0));
-			b.mInitialAngle = sign * ACos(n1.Dot(n2) / denom);
+			b.mInitialAngle = sign * ACosApproximate(n1.Dot(n2) / denom); // Runtime uses the approximation too
 		}
 	}
 }

UnitTests/Math/TrigonometryTests.cpp

@@ -0,0 +1,27 @@
+// Jolt Physics Library (https://github.com/jrouwe/JoltPhysics)
+// SPDX-FileCopyrightText: 2024 Jorrit Rouwe
+// SPDX-License-Identifier: MIT
+
+#include "UnitTestFramework.h"
+#include <Jolt/Math/Trigonometry.h>
+
+TEST_SUITE("TrigonometryTests")
+{
+	TEST_CASE("TestACosApproximate")
+	{
+		// Check error over entire range [-1, 1]
+		for (int i = -1000; i <= 1000; i++)
+		{
+			float x = float(i) / 1000.0f;
+			float acos1 = acos(x);
+			float acos2 = ACosApproximate(x);
+			CHECK_APPROX_EQUAL(acos1, acos2, 4.3e-3f);
+		}
+
+		// Check edge cases for exact matches
+		CHECK(ACosApproximate(1.0f) == 0.0f);
+		CHECK(ACosApproximate(1.0e-12f) == JPH_PI / 2);
+		CHECK(ACosApproximate(-1.0e-12f) == JPH_PI / 2);
+		CHECK(ACosApproximate(-1.0f) == JPH_PI);
+	}
+}

UnitTests/UnitTests.cmake

@@ -29,6 +29,7 @@ set(UNIT_TESTS_SRC_FILES
 	${UNIT_TESTS_ROOT}/Math/MatrixTests.cpp
 	${UNIT_TESTS_ROOT}/Math/QuatTests.cpp
 	${UNIT_TESTS_ROOT}/Math/UVec4Tests.cpp
+	${UNIT_TESTS_ROOT}/Math/TrigonometryTests.cpp
 	${UNIT_TESTS_ROOT}/Math/Vec3Tests.cpp
 	${UNIT_TESTS_ROOT}/Math/Vec4Tests.cpp
 	${UNIT_TESTS_ROOT}/Math/VectorTests.cpp