From 4b72cd62ec9592294e68389362ef747815ad04ba Mon Sep 17 00:00:00 2001
From: Ross Nordby <norbo@bepuentertainment.com>
Date: Wed, 30 Dec 2020 22:32:20 -0600
Subject: [PATCH] Increased MathHelper.ApproximateAcos accuracy.

---
 BepuPhysics/BepuPhysics.csproj     |  2 +-
 BepuUtilities/BepuUtilities.csproj |  2 +-
 BepuUtilities/MathHelper.cs        | 21 ++++++++++++++-------
 3 files changed, 16 insertions(+), 9 deletions(-)

diff --git a/BepuPhysics/BepuPhysics.csproj b/BepuPhysics/BepuPhysics.csproj
index fb8eb4e59..9f23ce1a4 100644
--- a/BepuPhysics/BepuPhysics.csproj
+++ b/BepuPhysics/BepuPhysics.csproj
@@ -1,7 +1,7 @@
 <Project Sdk="Microsoft.NET.Sdk">
   <PropertyGroup>
     <TargetFramework>netstandard2.0</TargetFramework>
-    <Version>2.3.0-beta10</Version>
+    <Version>2.3.0-beta11</Version>
     <Company>Bepu Entertainment LLC</Company>
     <Authors>Ross Nordby</Authors>
     <Description>Speedy real time physics simulation library.</Description>
diff --git a/BepuUtilities/BepuUtilities.csproj b/BepuUtilities/BepuUtilities.csproj
index 4262a8a06..f8aa3c46c 100644
--- a/BepuUtilities/BepuUtilities.csproj
+++ b/BepuUtilities/BepuUtilities.csproj
@@ -3,7 +3,7 @@
     <AssemblyName>BepuUtilities</AssemblyName>
     <RootNamespace>BepuUtilities</RootNamespace>
     <TargetFramework>netstandard2.0</TargetFramework>
-    <Version>2.3.0-beta10</Version>
+    <Version>2.3.0-beta11</Version>
     <Company>Bepu Entertainment LLC</Company>
     <Authors>Ross Nordby</Authors>
     <Description>Supporting utilities library for BEPUphysics v2.</Description>
diff --git a/BepuUtilities/MathHelper.cs b/BepuUtilities/MathHelper.cs
index 743475b99..e8c588fe8 100644
--- a/BepuUtilities/MathHelper.cs
+++ b/BepuUtilities/MathHelper.cs
@@ -308,15 +308,22 @@ public static void Sin(in Vector<float> x, out Vector<float> result)
         }
 
 
+        /// <summary>
+        /// Computes an approximation of arccos. Maximum error less than 6.8e-5.
+        /// </summary>
+        /// <param name="x">Input value to the arccos function.</param>
+        /// <param name="acos">Result of the arccos function.</param>
         [MethodImpl(MethodImplOptions.AggressiveInlining)]
-        public static void ApproximateAcos(in Vector<float> x, out Vector<float> acos)
+        public static void ApproximateAcos(Vector<float> x, out Vector<float> acos)
         {
-            //TODO: Could probably do better than this. Definitely don't need to use a square root.
-            //acos(x) ~= (pi / (2 * sqrt(2))) * sqrt(2 - 2 * x), for 0<=x<=1
-            //acos(x) ~= pi - (pi / (2 * sqrt(2))) * sqrt(2 + 2 * x), for -1<=x<=0
-            var two = new Vector<float>(2f);
-            acos = new Vector<float>(1.11072073454f) * Vector.SquareRoot(Vector.Max(Vector<float>.Zero, two - two * Vector.Abs(x)));
-            acos = Vector.ConditionalSelect(Vector.LessThan(x, Vector<float>.Zero), new Vector<float>(Pi) - acos, acos);
+            //Adapted from Handbook of Mathematical Functions by Milton Abramowitz and Irene A. Stegun.
+            var negate = Vector.ConditionalSelect(Vector.LessThan(x, Vector<float>.Zero), Vector<float>.One, Vector<float>.Zero);
+            x = Vector.Abs(x);
+            acos = new Vector<float>(-0.0187293f) * x + new Vector<float>(0.0742610f);
+            acos = (acos * x - new Vector<float>(0.2121144f)) * x + new Vector<float>(1.5707288f);
+            acos *= Vector.SquareRoot(Vector.Max(Vector<float>.Zero, Vector<float>.One - x));
+            acos -= new Vector<float>(2) * negate * acos;
+            acos = negate * new Vector<float>(3.14159265358979f) + acos;
         }