2 files changed, 202 insertions, 1 deletions

diff --git a/OpenSim/Region/Framework/Scenes/Scene.cs b/OpenSim/Region/Framework/Scenes/Scene.cs
index 7897564..fc89b2a 100644
--- a/OpenSim/Region/Framework/Scenes/Scene.cs
+++ b/OpenSim/Region/Framework/Scenes/Scene.cs
@@ -896,6 +896,8 @@ namespace OpenSim.Region.Framework.Scenes
 
             if (dm != null)
                 m_eventManager.OnPermissionError += dm.SendAlertToUser;
+
+            m_eventManager.OnSignificantClientMovement += HandleOnSignificantClientMovement;
         }
 
         public override string GetSimulatorVersion()
@@ -5396,7 +5398,7 @@ namespace OpenSim.Region.Framework.Scenes
             return true;
         }
 
-        public void StartTimerWatchdog()
+                public void StartTimerWatchdog()
         {
             m_timerWatchdog.Interval = 1000;
             m_timerWatchdog.Elapsed += TimerWatchdog;
@@ -5407,6 +5409,70 @@ namespace OpenSim.Region.Framework.Scenes
         public void TimerWatchdog(object sender, ElapsedEventArgs e)
         {
             CheckHeartbeat();
+                }
+
+        /// This method deals with movement when an avatar is automatically moving (but this is distinct from the
+        /// autopilot that moves an avatar to a sit target!.
+        /// </summary>
+        /// <remarks>
+        /// This is not intended as a permament location for this method.
+        /// </remarks>
+        /// <param name="presence"></param>
+        private void HandleOnSignificantClientMovement(ScenePresence presence)
+        {
+            if (presence.MovingToTarget)
+            {
+                double distanceToTarget = Util.GetDistanceTo(presence.AbsolutePosition, presence.MoveToPositionTarget);
+//                            m_log.DebugFormat(
+//                                "[SCENE]: Abs pos of {0} is {1}, target {2}, distance {3}",
+//                                presence.Name, presence.AbsolutePosition, presence.MoveToPositionTarget, distanceToTarget);
+
+                // Check the error term of the current position in relation to the target position
+                if (distanceToTarget <= ScenePresence.SIGNIFICANT_MOVEMENT)
+                {
+                    // We are close enough to the target
+//                        m_log.DebugFormat("[SCENEE]: Stopping autopilot of  {0}", presence.Name);
+
+                    presence.Velocity = Vector3.Zero;
+                    presence.AbsolutePosition = presence.MoveToPositionTarget;
+                    presence.ResetMoveToTarget();
+
+                    if (presence.PhysicsActor.Flying)
+                    {
+                        // A horrible hack to stop the avatar dead in its tracks rather than having them overshoot
+                        // the target if flying.
+                        // We really need to be more subtle (slow the avatar as it approaches the target) or at
+                        // least be able to set collision status once, rather than 5 times to give it enough
+                        // weighting so that that PhysicsActor thinks it really is colliding.
+                        for (int i = 0; i < 5; i++)
+                            presence.PhysicsActor.IsColliding = true;
+
+//                            Vector3 targetPos = presence.MoveToPositionTarget;
+//                            if (m_avatars[presence.UUID].LandAtTarget)
+//                                presence.PhysicsActor.Flying = false;
+
+//                            float terrainHeight = (float)presence.Scene.Heightmap[(int)targetPos.X, (int)targetPos.Y];
+//                            if (targetPos.Z - terrainHeight < 0.2)
+//                            {
+//                                presence.PhysicsActor.Flying = false;
+//                            }
+                    }
+
+//                        m_log.DebugFormat(
+//                            "[SCENE]: AgentControlFlags {0}, MovementFlag {1} for {2}",
+//                            presence.AgentControlFlags, presence.MovementFlag, presence.Name);
+                }
+                else
+                {
+//                        m_log.DebugFormat(
+//                            "[SCENE]: Updating npc {0} at {1} for next movement to {2}",
+//                            presence.Name, presence.AbsolutePosition, presence.MoveToPositionTarget);
+
+                    Vector3 agent_control_v3 = new Vector3();
+                    presence.HandleMoveToTargetUpdate(ref agent_control_v3);
+                    presence.AddNewMovement(agent_control_v3);
+                }
+            }
         }
     }
 }
diff --git a/OpenSim/Region/Framework/Scenes/Tests/ScenePresenceAutopilotTests.cs b/OpenSim/Region/Framework/Scenes/Tests/ScenePresenceAutopilotTests.cs
new file mode 100644
index 0000000..5a85d7f
--- /dev/null
+++ b/OpenSim/Region/Framework/Scenes/Tests/ScenePresenceAutopilotTests.cs
@@ -0,0 +1,135 @@
+/*
+ * 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 System.Collections.Generic;
+using System.Reflection;
+using log4net;
+using Nini.Config;
+using NUnit.Framework;
+using OpenMetaverse;
+using OpenSim.Framework;
+using OpenSim.Framework.Communications;
+using OpenSim.Region.Framework.Interfaces;
+using OpenSim.Region.Framework.Scenes;
+using OpenSim.Tests.Common;
+using OpenSim.Tests.Common.Mock;
+
+namespace OpenSim.Region.Framework.Scenes.Tests
+{
+    [TestFixture]
+    public class ScenePresenceAutopilotTests
+    {
+        private TestScene m_scene;
+
+        [TestFixtureSetUp]
+        public void FixtureInit()
+        {
+            // Don't allow tests to be bamboozled by asynchronous events.  Execute everything on the same thread.
+            Util.FireAndForgetMethod = FireAndForgetMethod.None;
+        }
+
+        [TestFixtureTearDown]
+        public void TearDown()
+        {
+            // We must set this back afterwards, otherwise later tests will fail since they're expecting multiple
+            // threads.  Possibly, later tests should be rewritten not to worry about such things.
+            Util.FireAndForgetMethod = Util.DefaultFireAndForgetMethod;
+        }
+
+        [SetUp]
+        public void Init()
+        {
+            m_scene = SceneHelpers.SetupScene();
+        }
+
+        [Test]
+        public void TestMove()
+        {
+            TestHelpers.InMethod();
+//            log4net.Config.XmlConfigurator.Configure();
+
+            ScenePresence sp = SceneHelpers.AddScenePresence(m_scene, TestHelpers.ParseTail(0x1));
+
+            Vector3 startPos = sp.AbsolutePosition;
+//            Vector3 startPos = new Vector3(128, 128, 30);
+
+            // For now, we'll make the scene presence fly to simplify this test, but this needs to change.
+            sp.PhysicsActor.Flying = true;
+
+            m_scene.Update();
+            Assert.That(sp.AbsolutePosition, Is.EqualTo(startPos));
+
+            Vector3 targetPos = startPos + new Vector3(0, 10, 0);
+            sp.MoveToTarget(targetPos, false);
+
+            Assert.That(sp.AbsolutePosition, Is.EqualTo(startPos));
+            Assert.That(
+                sp.Rotation, new QuaternionToleranceConstraint(new Quaternion(0, 0, 0.7071068f, 0.7071068f), 0.000001));
+
+            m_scene.Update();
+
+            // We should really check the exact figure.
+            Assert.That(sp.AbsolutePosition.X, Is.EqualTo(startPos.X));
+            Assert.That(sp.AbsolutePosition.Y, Is.GreaterThan(startPos.Y));
+            Assert.That(sp.AbsolutePosition.Z, Is.EqualTo(startPos.Z));
+            Assert.That(sp.AbsolutePosition.Z, Is.LessThan(targetPos.X));
+
+            for (int i = 0; i < 10; i++)
+                m_scene.Update();
+
+            double distanceToTarget = Util.GetDistanceTo(sp.AbsolutePosition, targetPos);
+            Assert.That(distanceToTarget, Is.LessThan(1), "Avatar not within 1 unit of target position on first move");
+            Assert.That(sp.AbsolutePosition, Is.EqualTo(targetPos));
+            Assert.That(sp.AgentControlFlags, Is.EqualTo((uint)AgentManager.ControlFlags.NONE));
+
+            // Try a second movement
+            startPos = sp.AbsolutePosition;
+            targetPos = startPos + new Vector3(10, 0, 0);
+            sp.MoveToTarget(targetPos, false);
+
+            Assert.That(sp.AbsolutePosition, Is.EqualTo(startPos));
+            Assert.That(
+                sp.Rotation, new QuaternionToleranceConstraint(new Quaternion(0, 0, 0, 1), 0.000001));
+
+            m_scene.Update();
+
+            // We should really check the exact figure.
+            Assert.That(sp.AbsolutePosition.X, Is.GreaterThan(startPos.X));
+            Assert.That(sp.AbsolutePosition.X, Is.LessThan(targetPos.X));
+            Assert.That(sp.AbsolutePosition.Y, Is.EqualTo(startPos.Y));
+            Assert.That(sp.AbsolutePosition.Z, Is.EqualTo(startPos.Z));
+
+            for (int i = 0; i < 10; i++)
+                m_scene.Update();
+
+            distanceToTarget = Util.GetDistanceTo(sp.AbsolutePosition, targetPos);
+            Assert.That(distanceToTarget, Is.LessThan(1), "Avatar not within 1 unit of target position on second move");
+            Assert.That(sp.AbsolutePosition, Is.EqualTo(targetPos));
+        }
+    }
+}
\ No newline at end of file