Thanks to M. Igarashi and nlin for a patch that improves conformance of the funcion llAxes2Rot()

1 files changed, 49 insertions, 36 deletions

diff --git a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs
index a005b01..1d7dcee 100644
--- a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs
+++ b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs
@@ -535,43 +535,56 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
         public LSL_Rotation llAxes2Rot(LSL_Vector fwd, LSL_Vector left, LSL_Vector up)
         {
             m_host.AddScriptLPS(1);
-            double x, y, z, s;
-            int f = 0;
-            // Important Note: q1=<x,y,z,s> is equal to q2=<-x,-y,-z,-s>
-            // Computing quaternion x,y,z,s values
-            x = ((fwd.x - left.y - up.z + 1) / 4);
-            x *= x;
-            x = Math.Sqrt(Math.Sqrt(x));
-            y = ((1 - up.z) / 2 - x * x);
-            y *= y;
-            y = Math.Sqrt(Math.Sqrt(y));
-            z = ((1 - left.y) / 2 - x * x);
-            z *= z;
-            z = Math.Sqrt(Math.Sqrt(z));
-            s = (1 - x * x - y * y - z * z);
-            s *= s;
-            s = Math.Sqrt(Math.Sqrt(s));
-
-            // Set f for signs detection
-            if (fwd.y + left.x >= 0) { f += 1; }
-            if (fwd.z + up.x >= 0) { f += 2; }
-            if (left.z - up.y >= 0) { f += 4; }
-            // Set correct quaternion signs based on f value
-            if (f == 0) { x = -x; }
-            if (f == 1) { x = -x; y = -y; }
-            if (f == 2) { x = -x; z = -z; }
-            if (f == 3) { s = -s; }
-            if (f == 4) { x = -x; s = -s; }
-            if (f == 5) { z = -z; }
-            if (f == 6) { y = -y; }
-
-            LSL_Rotation result = new LSL_Rotation(x, y, z, s);
-
-            // a hack to correct a few questionable angles :(
-            if (llVecDist(llRot2Fwd(result), fwd) > 0.001 || llVecDist(llRot2Left(result), left) > 0.001)
-                result.s = -s;
+            double s;
+            double tr = fwd.x + left.y + up.z + 1.0;
 
-            return result;
+            if (tr >= 1.0)
+            {
+                s = 0.5 / Math.Sqrt(tr);
+                return new LSL_Rotation(
+                        (left.z - up.y) * s,
+                        (up.x - fwd.z) * s,
+                        (fwd.y - left.x) * s,
+                        0.25 / s);
+            }
+            else
+            {
+                double max = (left.y > up.z) ? left.y : up.z;
+
+                if (max < fwd.x)
+                {
+                    s = Math.Sqrt(fwd.x - (left.y + up.z) + 1.0);
+                    double x = s * 0.5;
+                    s = 0.5 / s;
+                    return new LSL_Rotation(
+                            x,
+                            (fwd.y + left.x) * s,
+                            (up.x + fwd.z) * s,
+                            (left.z - up.y) * s);
+                }
+                else if (max == left.y)
+                {
+                    s = Math.Sqrt(left.y - (up.z + fwd.x) + 1.0);
+                    double y = s * 0.5;
+                    s = 0.5 / s;
+                    return new LSL_Rotation(
+                            (fwd.y + left.x) * s,
+                            y,
+                            (left.z + up.y) * s,
+                            (up.x - fwd.z) * s);
+                }
+                else
+                {
+                    s = Math.Sqrt(up.z - (fwd.x + left.y) + 1.0);
+                    double z = s * 0.5;
+                    s = 0.5 / s;
+                    return new LSL_Rotation(
+                            (up.x + fwd.z) * s,
+                            (left.z + up.y) * s,
+                            z,
+                            (fwd.y - left.x) * s);
+                }
+            }
         }
 
         public LSL_Vector llRot2Fwd(LSL_Rotation r)