3 files changed, 115 insertions, 6 deletions

diff --git a/OpenSim/Region/Framework/Scenes/ScenePresence.cs b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
index 9358a4a..cd3cb60 100644
--- a/OpenSim/Region/Framework/Scenes/ScenePresence.cs
+++ b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
@@ -585,7 +585,11 @@ namespace OpenSim.Region.Framework.Scenes
         public Quaternion Rotation
         {
             get { return m_bodyRot; }
-            set { m_bodyRot = value; }
+            set
+            {
+                m_bodyRot = value;
+                m_log.DebugFormat("[SCENE PRESENCE]: Body rot for {0} set to {1}", Name, m_bodyRot);
+            }
         }
 
         public Quaternion PreviousRotation
@@ -1711,7 +1715,7 @@ namespace OpenSim.Region.Framework.Scenes
 
             // Fudge factor.  It appears that if one clicks "go here" on a piece of ground, the go here request is
             // always slightly higher than the actual terrain height.
-            // FIXME: This constrains NOC movements as well, so should be somewhere else.
+            // FIXME: This constrains NPC movements as well, so should be somewhere else.
             if (pos.Z - terrainHeight < 0.2)
                 pos.Z = terrainHeight;
 
@@ -1727,6 +1731,21 @@ namespace OpenSim.Region.Framework.Scenes
             MovingToTarget = true;
             MoveToPositionTarget = pos;
 
+            // Rotate presence around the z-axis to point in same direction as movement.
+            // Ignore z component of vector
+            Vector3 localVectorToTarget3D = pos - AbsolutePosition;
+            Vector3 localVectorToTarget2D = new Vector3((float)(localVectorToTarget3D.X), (float)(localVectorToTarget3D.Y), 0f);
+
+//            m_log.DebugFormat("[SCENE PRESENCE]: Local vector to target is {0}", localVectorToTarget2D);
+
+            // Calculate the yaw.
+            Vector3 angle = new Vector3(0, 0, (float)(Math.Atan2(localVectorToTarget2D.Y, localVectorToTarget2D.X)));
+
+//            m_log.DebugFormat("[SCENE PRESENCE]: Angle is {0}", angle);
+
+            Rotation = Quaternion.CreateFromEulers(angle);
+//            m_log.DebugFormat("[SCENE PRESENCE]: Body rot for {0} set to {1}", Name, Rotation);
+
             Vector3 agent_control_v3 = new Vector3();
             HandleMoveToTargetUpdate(ref agent_control_v3);
             AddNewMovement(agent_control_v3);
diff --git a/OpenSim/Region/OptionalModules/World/NPC/Tests/NPCModuleTests.cs b/OpenSim/Region/OptionalModules/World/NPC/Tests/NPCModuleTests.cs
index 246bc34..1a0d0c7 100644
--- a/OpenSim/Region/OptionalModules/World/NPC/Tests/NPCModuleTests.cs
+++ b/OpenSim/Region/OptionalModules/World/NPC/Tests/NPCModuleTests.cs
@@ -182,18 +182,21 @@ namespace OpenSim.Region.OptionalModules.World.NPC.Tests
             scene.Update();
             Assert.That(npc.AbsolutePosition, Is.EqualTo(startPos));
 
-            Vector3 targetPos = startPos + new Vector3(0, 0, 10);
+            Vector3 targetPos = startPos + new Vector3(0, 10, 0);
             npcModule.MoveToTarget(npc.UUID, scene, targetPos, false, false);
 
             Assert.That(npc.AbsolutePosition, Is.EqualTo(startPos));
+            //Assert.That(npc.Rotation, Is.EqualTo(new Quaternion(0, 0, 0.7071068f, 0.7071068f)));
+            Assert.That(
+                npc.Rotation, new QuaternionToleranceConstraint(new Quaternion(0, 0, 0.7071068f, 0.7071068f), 0.000001));
 
             scene.Update();
 
             // We should really check the exact figure.
             Assert.That(npc.AbsolutePosition.X, Is.EqualTo(startPos.X));
-            Assert.That(npc.AbsolutePosition.Y, Is.EqualTo(startPos.Y));
-            Assert.That(npc.AbsolutePosition.Z, Is.GreaterThan(startPos.Z));
-            Assert.That(npc.AbsolutePosition.Z, Is.LessThan(targetPos.Z));
+            Assert.That(npc.AbsolutePosition.Y, Is.GreaterThan(startPos.Y));
+            Assert.That(npc.AbsolutePosition.Z, Is.EqualTo(startPos.Z));
+            Assert.That(npc.AbsolutePosition.Z, Is.LessThan(targetPos.X));
 
             for (int i = 0; i < 10; i++)
                 scene.Update();
@@ -208,6 +211,11 @@ namespace OpenSim.Region.OptionalModules.World.NPC.Tests
             targetPos = startPos + new Vector3(10, 0, 0);
             npcModule.MoveToTarget(npc.UUID, scene, targetPos, false, false);
 
+            Assert.That(npc.AbsolutePosition, Is.EqualTo(startPos));
+//            Assert.That(npc.Rotation, Is.EqualTo(new Quaternion(0, 0, 0, 1)));
+            Assert.That(
+                npc.Rotation, new QuaternionToleranceConstraint(new Quaternion(0, 0, 0, 1), 0.000001));
+
             scene.Update();
 
             // We should really check the exact figure.
diff --git a/OpenSim/Tests/Common/QuaternionToleranceConstraint.cs b/OpenSim/Tests/Common/QuaternionToleranceConstraint.cs
new file mode 100644
index 0000000..b38c382
--- /dev/null
+++ b/OpenSim/Tests/Common/QuaternionToleranceConstraint.cs
@@ -0,0 +1,82 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+
+using System;
+using OpenMetaverse;
+using NUnit.Framework;
+using NUnit.Framework.Constraints;
+
+namespace OpenSim.Tests.Common
+{
+    public class QuaternionToleranceConstraint : ANumericalToleranceConstraint
+    {
+        private Quaternion _baseValue;
+        private Quaternion _valueToBeTested;
+
+        public QuaternionToleranceConstraint(Quaternion baseValue, double tolerance) : base(tolerance)
+        {
+            _baseValue = baseValue;
+        }
+
+        /// <summary>
+        /// Test whether the constraint is satisfied by a given value
+        /// </summary>
+        /// <param name="valueToBeTested">The value to be tested</param>
+        /// <returns>
+        /// True for success, false for failure
+        /// </returns>
+        public override bool Matches(object valueToBeTested)
+        {
+            if (valueToBeTested == null)
+            {
+                throw new ArgumentException("Constraint cannot be used upon null values.");
+            }
+            if (valueToBeTested.GetType() != typeof (Quaternion))
+            {
+                throw new ArgumentException("Constraint cannot be used upon non quaternion values.");
+            }
+
+            _valueToBeTested = (Quaternion)valueToBeTested;
+
+            return (IsWithinDoubleConstraint(_valueToBeTested.X, _baseValue.X) &&
+                    IsWithinDoubleConstraint(_valueToBeTested.Y, _baseValue.Y) &&
+                    IsWithinDoubleConstraint(_valueToBeTested.Z, _baseValue.Z) &&
+                    IsWithinDoubleConstraint(_valueToBeTested.W, _baseValue.W));
+        }
+
+        public override void WriteDescriptionTo(MessageWriter writer)
+        {
+            writer.WriteExpectedValue(
+                string.Format("A value {0} within tolerance of plus or minus {1}", _baseValue, _tolerance));
+        }
+
+        public override void WriteActualValueTo(MessageWriter writer)
+        {
+            writer.WriteActualValue(_valueToBeTested);
+        }
+    }
+}
\ No newline at end of file