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.

4 files changed, 109 insertions, 67 deletions

diff --git a/OpenSim/Region/CoreModules/World/Land/LandManagementModule.cs b/OpenSim/Region/CoreModules/World/Land/LandManagementModule.cs
index 63dec15..7554e12 100644
--- a/OpenSim/Region/CoreModules/World/Land/LandManagementModule.cs
+++ b/OpenSim/Region/CoreModules/World/Land/LandManagementModule.cs
@@ -470,53 +470,48 @@ namespace OpenSim.Region.CoreModules.World.Land
             SendLandUpdate(avatar, false);
         }
 
-        public void EventManagerOnSignificantClientMovement(IClientAPI remote_client)
+        public void EventManagerOnSignificantClientMovement(ScenePresence clientAvatar)
         {
-            ScenePresence clientAvatar = m_scene.GetScenePresence(remote_client.AgentId);
-
-            if (clientAvatar != null)
+            SendLandUpdate(clientAvatar);
+            SendOutNearestBanLine(clientAvatar.ControllingClient);
+            ILandObject parcel = GetLandObject(clientAvatar.AbsolutePosition.X, clientAvatar.AbsolutePosition.Y);
+            if (parcel != null)
             {
-                SendLandUpdate(clientAvatar);
-                SendOutNearestBanLine(remote_client);
-                ILandObject parcel = GetLandObject(clientAvatar.AbsolutePosition.X, clientAvatar.AbsolutePosition.Y);
-                if (parcel != null)
+                if (clientAvatar.AbsolutePosition.Z < LandChannel.BAN_LINE_SAFETY_HIEGHT &&
+                    clientAvatar.sentMessageAboutRestrictedParcelFlyingDown)
                 {
-                    if (clientAvatar.AbsolutePosition.Z < LandChannel.BAN_LINE_SAFETY_HIEGHT &&
-                        clientAvatar.sentMessageAboutRestrictedParcelFlyingDown)
+                    EventManagerOnAvatarEnteringNewParcel(clientAvatar, parcel.LandData.LocalID,
+                                                          m_scene.RegionInfo.RegionID);
+                    //They are going under the safety line!
+                    if (!parcel.IsBannedFromLand(clientAvatar.UUID))
                     {
-                        EventManagerOnAvatarEnteringNewParcel(clientAvatar, parcel.LandData.LocalID,
-                                                              m_scene.RegionInfo.RegionID);
-                        //They are going under the safety line!
-                        if (!parcel.IsBannedFromLand(clientAvatar.UUID))
-                        {
-                            clientAvatar.sentMessageAboutRestrictedParcelFlyingDown = false;
-                        }
+                        clientAvatar.sentMessageAboutRestrictedParcelFlyingDown = false;
                     }
-                    else if (clientAvatar.AbsolutePosition.Z < LandChannel.BAN_LINE_SAFETY_HIEGHT &&
-                             parcel.IsBannedFromLand(clientAvatar.UUID))
+                }
+                else if (clientAvatar.AbsolutePosition.Z < LandChannel.BAN_LINE_SAFETY_HIEGHT &&
+                         parcel.IsBannedFromLand(clientAvatar.UUID))
+                {
+                    //once we've sent the message once, keep going toward the target until we are done
+                    if (forcedPosition.ContainsKey(clientAvatar.ControllingClient.AgentId))
                     {
-                        //once we've sent the message once, keep going toward the target until we are done
-                        if (forcedPosition.ContainsKey(clientAvatar.ControllingClient.AgentId))
-                        {
-                            SendYouAreBannedNotice(clientAvatar);
-                            ForceAvatarToPosition(clientAvatar, m_scene.GetNearestAllowedPosition(clientAvatar));
-                        }
+                        SendYouAreBannedNotice(clientAvatar);
+                        ForceAvatarToPosition(clientAvatar, m_scene.GetNearestAllowedPosition(clientAvatar));
                     }
-                    else if (parcel.IsRestrictedFromLand(clientAvatar.UUID))
-                    {
-                        //once we've sent the message once, keep going toward the target until we are done
-                        if (forcedPosition.ContainsKey(clientAvatar.ControllingClient.AgentId))
-                        {
-                            SendYouAreRestrictedNotice(clientAvatar);
-                            ForceAvatarToPosition(clientAvatar, m_scene.GetNearestAllowedPosition(clientAvatar));
-                        }
-                    }
-                    else
+                }
+                else if (parcel.IsRestrictedFromLand(clientAvatar.UUID))
+                {
+                    //once we've sent the message once, keep going toward the target until we are done
+                    if (forcedPosition.ContainsKey(clientAvatar.ControllingClient.AgentId))
                     {
-                        //when we are finally in a safe place, lets release the forced position lock
-                        forcedPosition.Remove(clientAvatar.ControllingClient.AgentId);
+                        SendYouAreRestrictedNotice(clientAvatar);
+                        ForceAvatarToPosition(clientAvatar, m_scene.GetNearestAllowedPosition(clientAvatar));
                     }
                 }
+                else
+                {
+                    //when we are finally in a safe place, lets release the forced position lock
+                    forcedPosition.Remove(clientAvatar.ControllingClient.AgentId);
+                }
             }
         }
 
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
diff --git a/OpenSim/Region/OptionalModules/World/NPC/NPCModule.cs b/OpenSim/Region/OptionalModules/World/NPC/NPCModule.cs
index 8cb8318..fcfacc6 100644
--- a/OpenSim/Region/OptionalModules/World/NPC/NPCModule.cs
+++ b/OpenSim/Region/OptionalModules/World/NPC/NPCModule.cs
@@ -46,12 +46,54 @@ namespace OpenSim.Region.OptionalModules.World.NPC
 
         // private const bool m_enabled = false;
 
-        private Dictionary<UUID,NPCAvatar> m_avatars = new Dictionary<UUID, NPCAvatar>();
-        private Dictionary<UUID,AvatarAppearance> m_appearanceCache = new Dictionary<UUID, AvatarAppearance>();
+        private Dictionary<UUID, NPCAvatar> m_avatars = new Dictionary<UUID, NPCAvatar>();
+        private Dictionary<UUID, AvatarAppearance> m_appearanceCache = new Dictionary<UUID, AvatarAppearance>();
 
         public void Initialise(Scene scene, IConfigSource source)
         {
             scene.RegisterModuleInterface<INPCModule>(this);
+            scene.EventManager.OnSignificantClientMovement += HandleOnSignificantClientMovement;
+        }
+
+        public void HandleOnSignificantClientMovement(ScenePresence presence)
+        {
+            lock (m_avatars)
+            {
+                if (m_avatars.ContainsKey(presence.UUID))
+                {
+                    double distanceToTarget = Util.GetDistanceTo(presence.AbsolutePosition, presence.MoveToPositionTarget);
+//                            m_log.DebugFormat(
+//                                "[NPC MODULE]: 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 <= 1)
+                    {
+//                        m_log.DebugFormat("[NPC MODULE]: Stopping movement of npc {0} {1}", presence.Name, presence.UUID);
+                        // We are close enough to the target for now
+                        presence.ResetMoveToPosition();
+                        presence.Velocity = Vector3.Zero;
+
+                        // FIXME: This doesn't work
+                        if (presence.PhysicsActor.Flying)
+                            presence.Animator.TrySetMovementAnimation("HOVER");
+                        else
+                            presence.Animator.TrySetMovementAnimation("STAND");
+                    }
+                    else
+                    {
+                        Vector3 agent_control_v3 = new Vector3();
+                        presence.DoMoveToPositionUpdate(ref agent_control_v3, presence.Rotation, false, true);
+                        presence.AddNewMovement(agent_control_v3, presence.Rotation);
+                    }
+//
+////                    presence.DoMoveToPositionUpdate((0, presence.MoveToPositionTarget, null);
+
+//
+//
+
+                }
+            }
         }
 
         private AvatarAppearance GetAppearance(UUID target, Scene scene)
@@ -141,15 +183,10 @@ namespace OpenSim.Region.OptionalModules.World.NPC
                     ScenePresence sp;
                     scene.TryGetScenePresence(agentID, out sp);
 
-//                    m_log.DebugFormat(
-//                        "[NPC MODULE]: Moving {0} to {1} in {2}", sp.Name, pos, scene.RegionInfo.RegionName);
-//
-//                    List<string> targetArgs = new List<string>();
-//                    targetArgs.Add(pos.X);
-//                    targetArgs.Add(pos.Y);
-//                    targetArgs.Add(pos.Z);
-//                    sp.DoMoveToPosition(null, "NPC", targetArgs);
-//                    sp.DoMoveToPosition(0, pos, m_avatars[agentID]);
+                    m_log.DebugFormat(
+                        "[NPC MODULE]: Moving {0} to {1} in {2}", sp.Name, pos, scene.RegionInfo.RegionName);
+
+                    sp.DoMoveToPosition(0, pos, m_avatars[agentID]);
                 }
             }
         }