Do a partial fix/implementation of OSSL osNpcMoveTo()

Avatar moves and stops.  However, will stop in mid stride.
And if the move to position is in the air, avatar will continue to make vain and quite hilarious attempts to take off (but never doing so).
Clearly more work is needed.

2 files changed, 29 insertions, 19 deletions

diff --git a/OpenSim/Region/Framework/Scenes/EventManager.cs b/OpenSim/Region/Framework/Scenes/EventManager.cs
index b67937d..96da2c3 100644
--- a/OpenSim/Region/Framework/Scenes/EventManager.cs
+++ b/OpenSim/Region/Framework/Scenes/EventManager.cs
@@ -165,8 +165,7 @@ namespace OpenSim.Region.Framework.Scenes
         public delegate void AvatarEnteringNewParcel(ScenePresence avatar, int localLandID, UUID regionID);
         public event AvatarEnteringNewParcel OnAvatarEnteringNewParcel;
 
-        public delegate void SignificantClientMovement(IClientAPI remote_client);
-        public event SignificantClientMovement OnSignificantClientMovement;
+        public event Action<ScenePresence> OnSignificantClientMovement;
 
         public delegate void IncomingInstantMessage(GridInstantMessage message);
         public event IncomingInstantMessage OnIncomingInstantMessage;
@@ -1592,16 +1591,16 @@ namespace OpenSim.Region.Framework.Scenes
             }
         }
 
-        public void TriggerSignificantClientMovement(IClientAPI client)
+        public void TriggerSignificantClientMovement(ScenePresence presence)
         {
-            SignificantClientMovement handlerSignificantClientMovement = OnSignificantClientMovement;
+            Action<ScenePresence> handlerSignificantClientMovement = OnSignificantClientMovement;
             if (handlerSignificantClientMovement != null)
             {
-                foreach (SignificantClientMovement d in handlerSignificantClientMovement.GetInvocationList())
+                foreach (Action<ScenePresence> d in handlerSignificantClientMovement.GetInvocationList())
                 {
                     try
                     {
-                        d(client);
+                        d(presence);
                     }
                     catch (Exception e)
                     {
diff --git a/OpenSim/Region/Framework/Scenes/ScenePresence.cs b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
index a610b6b..9558258 100644
--- a/OpenSim/Region/Framework/Scenes/ScenePresence.cs
+++ b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
@@ -211,7 +211,8 @@ namespace OpenSim.Region.Framework.Scenes
 
         //PauPaw:Proper PID Controler for autopilot************
         private bool m_moveToPositionInProgress;
-        private Vector3 m_moveToPositionTarget;
+
+        public Vector3 MoveToPositionTarget { get; private set; }
 
         private bool m_followCamAuto;
 
@@ -1385,7 +1386,7 @@ namespace OpenSim.Region.Framework.Scenes
                 if (agentData.UseClientAgentPosition)
                 {
                     m_moveToPositionInProgress = (agentData.ClientAgentPosition - AbsolutePosition).Length() > 0.2f;
-                    m_moveToPositionTarget = agentData.ClientAgentPosition;
+                    MoveToPositionTarget = agentData.ClientAgentPosition;
                 }
 
                 int i = 0;
@@ -1543,16 +1544,17 @@ namespace OpenSim.Region.Framework.Scenes
         /// <param value="reset">If true, clear the move to position</param>
         /// <param value="allowUpdate">If true, allow the update in principle.</param>
         /// <returns>True if movement has been updated in some way.  False otherwise.</returns>
-        protected bool DoMoveToPositionUpdate(
+        public bool DoMoveToPositionUpdate(
             ref Vector3 agent_control_v3, Quaternion bodyRotation, bool reset, bool allowUpdate)
         {
+//            m_log.DebugFormat("[SCENE PRESENCE]: Called DoMoveToPositionUpdate() for {0}", Name);
+
             bool updated = false;
 
             //Paupaw:Do Proper PID for Autopilot here
             if (reset)
             {
-                m_moveToPositionTarget = Vector3.Zero;
-                m_moveToPositionInProgress = false;
+                ResetMoveToPosition();
                 updated = true;
             }
 
@@ -1562,16 +1564,16 @@ namespace OpenSim.Region.Framework.Scenes
 
             if (allowUpdate && (m_moveToPositionInProgress && !m_autopilotMoving))
             {
-                double distanceToTarget = Util.GetDistanceTo(AbsolutePosition, m_moveToPositionTarget);
-//                        m_log.DebugFormat(
-//                            "[SCENE PRESENCE]: Abs pos of {0} is {1}, target {2}, distance {3}",
-//                            Name, AbsolutePosition, m_moveToPositionTarget, distanceToTarget);
+                double distanceToTarget = Util.GetDistanceTo(AbsolutePosition, MoveToPositionTarget);
+                        m_log.DebugFormat(
+                            "[SCENE PRESENCE]: Abs pos of {0} is {1}, target {2}, distance {3}",
+                            Name, AbsolutePosition, MoveToPositionTarget, distanceToTarget);
 
                 // Check the error term of the current position in relation to the target position
                 if (distanceToTarget <= 1)
                 {
                     // We are close enough to the target
-                    m_moveToPositionTarget = Vector3.Zero;
+                    MoveToPositionTarget = Vector3.Zero;
                     m_moveToPositionInProgress = false;
                     updated = true;
                 }
@@ -1585,7 +1587,7 @@ namespace OpenSim.Region.Framework.Scenes
                         // unknown forces are acting on the avatar and we need to adaptively respond
                         // to such forces, but the following simple approach seems to works fine.
                         Vector3 LocalVectorToTarget3D =
-                            (m_moveToPositionTarget - AbsolutePosition) // vector from cur. pos to target in global coords
+                            (MoveToPositionTarget - AbsolutePosition) // vector from cur. pos to target in global coords
                             * Matrix4.CreateFromQuaternion(Quaternion.Inverse(bodyRotation)); // change to avatar coords
                         // Ignore z component of vector
 //                        Vector3 LocalVectorToTarget2D = new Vector3((float)(LocalVectorToTarget3D.X), (float)(LocalVectorToTarget3D.Y), 0f);
@@ -1718,13 +1720,22 @@ namespace OpenSim.Region.Framework.Scenes
             }
 
             m_moveToPositionInProgress = true;
-            m_moveToPositionTarget = pos;
+            MoveToPositionTarget = pos;
 
             Vector3 agent_control_v3 = new Vector3();
             DoMoveToPositionUpdate(ref agent_control_v3, Rotation, false, true);
             AddNewMovement(agent_control_v3, Rotation);
         }
 
+        /// <summary>
+        /// Reset the move to position.
+        /// </summary>
+        public void ResetMoveToPosition()
+        {
+            MoveToPositionTarget = Vector3.Zero;
+            m_moveToPositionInProgress = false;
+        }
+
         private void CheckAtSitTarget()
         {
             //m_log.Debug("[AUTOPILOT]: " + Util.GetDistanceTo(AbsolutePosition, m_autoPilotTarget).ToString());
@@ -2731,7 +2742,7 @@ namespace OpenSim.Region.Framework.Scenes
             if (Util.GetDistanceTo(AbsolutePosition, posLastSignificantMove) > SIGNIFICANT_MOVEMENT)
             {
                 posLastSignificantMove = AbsolutePosition;
-                m_scene.EventManager.TriggerSignificantClientMovement(m_controllingClient);
+                m_scene.EventManager.TriggerSignificantClientMovement(this);
             }
 
             // Minimum Draw distance is 64 meters, the Radius of the draw distance sphere is 32m