1 files changed, 54 insertions, 0 deletions

diff --git a/OpenSim/Framework/Util.cs b/OpenSim/Framework/Util.cs
index 57c1601..28c818a 100644
--- a/OpenSim/Framework/Util.cs
+++ b/OpenSim/Framework/Util.cs
@@ -116,6 +116,60 @@ namespace OpenSim.Framework
 
         # endregion
 
+        public static Quaternion Axes2Rot(Vector3 fwd, Vector3 left, Vector3 up)
+        {
+            float s;
+            float tr = (float) (fwd.X + left.Y + up.Z + 1.0);
+
+            if (tr >= 1.0)
+            {
+                s = (float) (0.5 / Math.Sqrt(tr));
+                return new Quaternion(
+                        (left.Z - up.Y) * s,
+                        (up.X - fwd.Z) * s,
+                        (fwd.Y - left.X) * s,
+                        (float) 0.25 / s);
+            }
+            else
+            {
+                float max = (left.Y > up.Z) ? left.Y : up.Z;
+
+                if (max < fwd.X)
+                {
+                    s = (float) (Math.Sqrt(fwd.X - (left.Y + up.Z) + 1.0));
+                    float x = (float) (s * 0.5);
+                    s = (float) (0.5 / s);
+                    return new Quaternion(
+                            x,
+                            (fwd.Y + left.X) * s,
+                            (up.X + fwd.Z) * s,
+                            (left.Z - up.Y) * s);
+                }
+                else if (max == left.Y)
+                {
+                    s = (float) (Math.Sqrt(left.Y - (up.Z + fwd.X) + 1.0));
+                    float y = (float) (s * 0.5);
+                    s = (float) (0.5 / s);
+                    return new Quaternion(
+                            (fwd.Y + left.X) * s,
+                            y,
+                            (left.Z + up.Y) * s,
+                            (up.X - fwd.Z) * s);
+                }
+                else
+                {
+                    s = (float) (Math.Sqrt(up.Z - (fwd.X + left.Y) + 1.0));
+                    float z = (float) (s * 0.5);
+                    s = (float) (0.5 / s);
+                    return new Quaternion(
+                            (up.X + fwd.Z) * s,
+                            (left.Z + up.Y) * s,
+                            z,
+                            (fwd.Y - left.X) * s);
+                }
+            }
+        }
+
         public static Random RandomClass
         {
             get { return randomClass; }