Use double precision for angular integration

notgiven688 · Nov 27, 2024 · 16d2306 · 16d2306
1 parent 4849318
commit 16d2306
Showing 1 changed file with 7 additions and 4 deletions.
diff --git a/src/Jitter2/World.Step.cs b/src/Jitter2/World.Step.cs
@@ -588,6 +588,8 @@ private void IntegrateCallback(Parallel.Batch batch)
     {
         var span = memRigidBodies.Active[batch.Start..batch.End];
 
+        double substepDouble = substep_dt;
+
         for (int i = 0; i < span.Length; i++)
         {
             ref RigidBodyData rigidBody = ref span[i];
@@ -599,24 +601,25 @@ private void IntegrateCallback(Parallel.Batch batch)
 
             rigidBody.Position += lvel * substep_dt;
 
-            Real angle = avel.Length();
+            double angle = avel.Length();
+
             JVector axis;
 
             if (angle < (Real)0.001)
             {
                 // use Taylor's expansions of sync function
                 // axis = body.angularVelocity * ((Real)0.5 * timestep - (timestep * timestep * timestep) * ((Real)0.020833333333) * angle * angle);
                 JVector.Multiply(avel,
-                    (Real)0.5 * substep_dt - substep_dt * substep_dt * substep_dt * (Real)0.020833333333 * angle * angle,
+                    (Real)(0.5d * substepDouble - substepDouble * substepDouble * substepDouble * 0.020833333333d * angle * angle),
                     out axis);
             }
             else
             {
                 // sync(fAngle) = sin(c*fAngle)/t
-                JVector.Multiply(avel, MathR.Sin((Real)0.5 * angle * substep_dt) / angle, out axis);
+                JVector.Multiply(avel, (Real)(Math.Sin(0.5d * angle * substepDouble) / angle), out axis);
             }
 
-            JQuaternion dorn = new(axis.X, axis.Y, axis.Z, MathR.Cos(angle * substep_dt * (Real)0.5));
+            JQuaternion dorn = new(axis.X, axis.Y, axis.Z, (Real)Math.Cos(angle * substepDouble * 0.5d));
             //JQuaternion.CreateFromMatrix(rigidBody.Orientation, out JQuaternion ornA);
             JQuaternion ornA = rigidBody.Orientation;