Thanks Micheil Merlin for Mantis #4784: [PATCH] llRotBetween does not return correct rotations for a 180 degree angle between vectors

Signed-off-by: dahlia <dahliaTrimble@gmail.removeme.com>

1 files changed, 71 insertions, 18 deletions

diff --git a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs
index 1feb153..dc34e1c 100644
--- a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs
+++ b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs
@@ -703,24 +703,77 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
 
         public LSL_Rotation llRotBetween(LSL_Vector a, LSL_Vector b)
         {
-            //A and B should both be normalized
-            m_host.AddScriptLPS(1);
-            double dotProduct = LSL_Vector.Dot(a, b);
-            LSL_Vector crossProduct = LSL_Vector.Cross(a, b);
-            double magProduct = LSL_Vector.Mag(a) * LSL_Vector.Mag(b);
-            double angle = Math.Acos(dotProduct / magProduct);
-            LSL_Vector axis = LSL_Vector.Norm(crossProduct);
-            double s = Math.Sin(angle / 2);
-
-            double x = axis.x * s;
-            double y = axis.y * s;
-            double z = axis.z * s;
-            double w = Math.Cos(angle / 2);
-
-            if (Double.IsNaN(x) || Double.IsNaN(y) || Double.IsNaN(z) || Double.IsNaN(w))
-                return new LSL_Rotation(0.0f, 0.0f, 0.0f, 1.0f);
-
-            return new LSL_Rotation((float)x, (float)y, (float)z, (float)w);
+            //A and B should both be normalized

+            m_host.AddScriptLPS(1);

+            LSL_Rotation rotBetween;

+            // Check for zero vectors. If either is zero, return zero rotation. Otherwise,

+            // continue calculation.

+            if (a == new LSL_Vector(0.0f, 0.0f, 0.0f) || b == new LSL_Vector(0.0f, 0.0f, 0.0f))

+            {

+                rotBetween = new LSL_Rotation(0.0f, 0.0f, 0.0f, 1.0f);

+            }

+            else

+            {

+                a = LSL_Vector.Norm(a);

+                b = LSL_Vector.Norm(b);

+                double dotProduct = LSL_Vector.Dot(a, b);

+                // There are two degenerate cases possible. These are for vectors 180 or

+                // 0 degrees apart. These have to be detected and handled individually.

+                //

+                // Check for vectors 180 degrees apart.

+                // A dot product of -1 would mean the angle between vectors is 180 degrees.

+                if (dotProduct < -0.9999999f)

+                {

+                    // First assume X axis is orthogonal to the vectors.

+                    LSL_Vector orthoVector = new LSL_Vector(1.0f, 0.0f, 0.0f);

+                    orthoVector = orthoVector - a * (a.x / LSL_Vector.Dot(a, a));

+                    // Check for near zero vector. A very small non-zero number here will create

+                    // a rotation in an undesired direction.

+                    if (LSL_Vector.Mag(orthoVector) > 0.0001)

+                    {

+                        rotBetween = new LSL_Rotation(orthoVector.x, orthoVector.y, orthoVector.z, 0.0f);

+                    }

+                    // If the magnitude of the vector was near zero, then assume the X axis is not

+                    // orthogonal and use the Z axis instead.

+                    else

+                    {

+                        // Set 180 z rotation.

+                        rotBetween = new LSL_Rotation(0.0f, 0.0f, 1.0f, 0.0f);

+                    }

+                }

+                // Check for parallel vectors.

+                // A dot product of 1 would mean the angle between vectors is 0 degrees.

+                else if (dotProduct > 0.9999999f)

+                {

+                    // Set zero rotation.

+                    rotBetween = new LSL_Rotation(0.0f, 0.0f, 0.0f, 1.0f);

+                }

+                else

+                {

+                    // All special checks have been performed so get the axis of rotation.

+                    LSL_Vector crossProduct = LSL_Vector.Cross(a, b);

+                    // Quarternion s value is the length of the unit vector + dot product.

+                    double qs = 1.0 + dotProduct;

+                    rotBetween = new LSL_Rotation(crossProduct.x, crossProduct.y, crossProduct.z, qs);

+                    // Normalize the rotation.

+                    double mag = LSL_Rotation.Mag(rotBetween);

+                    // We shouldn't have to worry about a divide by zero here. The qs value will be

+                    // non-zero because we already know if we're here, then the dotProduct is not -1 so

+                    // qs will not be zero. Also, we've already handled the input vectors being zero so the

+                    // crossProduct vector should also not be zero.

+                    rotBetween.x = rotBetween.x / mag;

+                    rotBetween.y = rotBetween.y / mag;

+                    rotBetween.z = rotBetween.z / mag;

+                    rotBetween.s = rotBetween.s / mag;

+                    // Check for undefined values and set zero rotation if any found. This code might not actually be required

+                    // any longer since zero vectors are checked for at the top.

+                    if (Double.IsNaN(rotBetween.x) || Double.IsNaN(rotBetween.y) || Double.IsNaN(rotBetween.z) || Double.IsNaN(rotBetween.s))

+                    {

+                        rotBetween = new LSL_Rotation(0.0f, 0.0f, 0.0f, 1.0f);

+                    }

+                }

+            }

+            return rotBetween;
         }
 
         public void llWhisper(int channelID, string text)