Partially fix autopilot/go here

This now works again except that it requires a click or avatar mvmt to get going
This is because the ScenePresence.HandleAgentUpdate() method doesn't trigger until the client does something significant, at which point autopilot takes over.
Even clicking is enough to trigger.
This will be improved presently.

7 files changed, 99 insertions, 119 deletions

diff --git a/OpenSim/ApplicationPlugins/Rest/Inventory/tests/Remote.cs b/OpenSim/ApplicationPlugins/Rest/Inventory/tests/Remote.cs
index 4333ef1..6ec11ef 100644
--- a/OpenSim/ApplicationPlugins/Rest/Inventory/tests/Remote.cs
+++ b/OpenSim/ApplicationPlugins/Rest/Inventory/tests/Remote.cs
@@ -169,7 +169,7 @@ namespace OpenSim.ApplicationPlugins.Rest.Inventory.Tests
                         float y = Convert.ToSingle(rdata.Parameters[PARM_MOVE_Y]);
                         float z = Convert.ToSingle(rdata.Parameters[PARM_MOVE_Z]);
                         Vector3 vector = new Vector3(x,y,z);
-                        presence.DoAutoPilot(0,vector,presence.ControllingClient);
+                        presence.DoMoveToPosition(0, vector, presence.ControllingClient);
                     }
                     catch (Exception e)
                     {
diff --git a/OpenSim/Region/ClientStack/Linden/UDP/LLClientView.cs b/OpenSim/Region/ClientStack/Linden/UDP/LLClientView.cs
index 60f0075..bb491a1 100644
--- a/OpenSim/Region/ClientStack/Linden/UDP/LLClientView.cs
+++ b/OpenSim/Region/ClientStack/Linden/UDP/LLClientView.cs
@@ -5266,6 +5266,8 @@ namespace OpenSim.Region.ClientStack.LindenUDP
             AddLocalPacketHandler(PacketType.GroupVoteHistoryRequest, HandleGroupVoteHistoryRequest);
             AddLocalPacketHandler(PacketType.SimWideDeletes, HandleSimWideDeletes);
             AddLocalPacketHandler(PacketType.SendPostcard, HandleSendPostcard);
+
+            AddGenericPacketHandler("autopilot", HandleAutopilot);
         }
 
         #region Packet Handlers
@@ -5308,7 +5310,9 @@ namespace OpenSim.Region.ClientStack.LindenUDP
                        );
                 }
                 else
+                {
                     update = true;
+                }
 
                 // These should be ordered from most-likely to
                 // least likely to change. I've made an initial
@@ -5316,6 +5320,8 @@ namespace OpenSim.Region.ClientStack.LindenUDP
 
                 if (update)
                 {
+//                    m_log.DebugFormat("[LLCLIENTVIEW]: Triggered AgentUpdate for {0}", sener.Name);
+
                     AgentUpdateArgs arg = new AgentUpdateArgs();
                     arg.AgentID = x.AgentID;
                     arg.BodyRotation = x.BodyRotation;
@@ -11609,54 +11615,37 @@ namespace OpenSim.Region.ClientStack.LindenUDP
             return false;
         }
 
-        /// <summary>
-        /// Breaks down the genericMessagePacket into specific events
-        /// </summary>
-        /// <param name="gmMethod"></param>
-        /// <param name="gmInvoice"></param>
-        /// <param name="gmParams"></param>
-        public void DecipherGenericMessage(string gmMethod, UUID gmInvoice, GenericMessagePacket.ParamListBlock[] gmParams)
+        protected void HandleAutopilot(Object sender, string method, List<String> args)
         {
-            switch (gmMethod)
+            try
             {
-                case "autopilot":
-                    float locx;
-                    float locy;
-                    float locz;
-
-                    try
-                    {
-                        uint regionX;
-                        uint regionY;
-                        Utils.LongToUInts(Scene.RegionInfo.RegionHandle, out regionX, out regionY);
-                        locx = Convert.ToSingle(Utils.BytesToString(gmParams[0].Parameter)) - regionX;
-                        locy = Convert.ToSingle(Utils.BytesToString(gmParams[1].Parameter)) - regionY;
-                        locz = Convert.ToSingle(Utils.BytesToString(gmParams[2].Parameter));
-                    }
-                    catch (InvalidCastException)
-                    {
-                        m_log.Error("[CLIENT]: Invalid autopilot request");
-                        return;
-                    }
-
-                    UpdateVector handlerAutoPilotGo = OnAutoPilotGo;
-                    if (handlerAutoPilotGo != null)
-                    {
-                        handlerAutoPilotGo(0, new Vector3(locx, locy, locz), this);
-                    }
-                    m_log.InfoFormat("[CLIENT]: Client Requests autopilot to position <{0},{1},{2}>", locx, locy, locz);
-
-
-                    break;
-                default:
-                    m_log.Debug("[CLIENT]: Unknown Generic Message, Method: " + gmMethod + ". Invoice: " + gmInvoice + ".  Dumping Params:");
-                    for (int hi = 0; hi < gmParams.Length; hi++)
-                    {
-                        Console.WriteLine(gmParams[hi].ToString());
-                    }
-                    //gmpack.MethodData.
-                    break;
+                float locx = 0f;
+                float locy = 0f;
+                float locz = 0f;
+                uint regionX = 0;
+                uint regionY = 0;
+                try
+                {
+                    Utils.LongToUInts(m_scene.RegionInfo.RegionHandle, out regionX, out regionY);
+                    locx = Convert.ToSingle(args[0]) - (float)regionX;
+                    locy = Convert.ToSingle(args[1]) - (float)regionY;
+                    locz = Convert.ToSingle(args[2]);
+                }
+                catch (InvalidCastException)
+                {
+                    m_log.Error("[CLIENT]: Invalid autopilot request");
+                    return;
+                }
 
+                UpdateVector handlerAutoPilotGo = OnAutoPilotGo;
+                if (handlerAutoPilotGo != null)
+                {
+                    handlerAutoPilotGo(0, new Vector3(locx, locy, locz), this);
+                }
+            }
+            catch (Exception e)
+            {
+                m_log.ErrorFormat("[LLCLIENTVIEW]: HandleAutopilot exception {0} {1}", e.Message, e.StackTrace);
             }
         }
 
diff --git a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs
index b6fb5a4..1417efb 100644
--- a/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs
+++ b/OpenSim/Region/Framework/Scenes/SceneObjectGroup.cs
@@ -1650,16 +1650,7 @@ namespace OpenSim.Region.Framework.Scenes
                     ScenePresence avatar = m_scene.GetScenePresence(rootpart.AttachedAvatar);
                     if (avatar != null)
                     {
-                        List<string> coords = new List<string>();
-                        uint regionX = 0;
-                        uint regionY = 0;
-                        Utils.LongToUInts(Scene.RegionInfo.RegionHandle, out regionX, out regionY);
-                        target.X += regionX;
-                        target.Y += regionY;
-                        coords.Add(target.X.ToString());
-                        coords.Add(target.Y.ToString());
-                        coords.Add(target.Z.ToString());
-                        avatar.DoMoveToPosition(avatar, "", coords);
+                        avatar.DoMoveToPosition(0, target, null);
                     }
                 }
                 else
diff --git a/OpenSim/Region/Framework/Scenes/ScenePresence.cs b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
index 4739f5b..a508813 100644
--- a/OpenSim/Region/Framework/Scenes/ScenePresence.cs
+++ b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
@@ -116,7 +116,7 @@ namespace OpenSim.Region.Framework.Scenes
         private ScriptControlled IgnoredControls = ScriptControlled.CONTROL_ZERO;
         private ScriptControlled LastCommands = ScriptControlled.CONTROL_ZERO;
         private bool MouseDown = false;
-        private SceneObjectGroup proxyObjectGroup;
+//        private SceneObjectGroup proxyObjectGroup;
         //private SceneObjectPart proxyObjectPart = null;
         public Vector3 lastKnownAllowedPosition;
         public bool sentMessageAboutRestrictedParcelFlyingDown;
@@ -779,8 +779,7 @@ namespace OpenSim.Region.Framework.Scenes
             m_controllingClient.OnStartAnim += HandleStartAnim;
             m_controllingClient.OnStopAnim += HandleStopAnim;
             m_controllingClient.OnForceReleaseControls += HandleForceReleaseControls;
-            m_controllingClient.OnAutoPilotGo += DoAutoPilot;
-            m_controllingClient.AddGenericPacketHandler("autopilot", DoMoveToPosition);
+            m_controllingClient.OnAutoPilotGo += DoMoveToPosition;
 
             // ControllingClient.OnChildAgentStatus += new StatusChange(this.ChildStatusChange);
             // ControllingClient.OnStopMovement += new GenericCall2(this.StopMovement);
@@ -1480,6 +1479,7 @@ namespace OpenSim.Region.Framework.Scenes
                                 bAllowUpdateMoveToPosition = true;
                             }
                         }
+
                         i++;
                     }
 
@@ -1492,12 +1492,21 @@ namespace OpenSim.Region.Framework.Scenes
                         bAllowUpdateMoveToPosition = false;
                     }
 
+                    m_log.DebugFormat(
+                        "[SCENE PRESENCE]: bAllowUpdateMoveToPosition {0}, m_moveToPositionInProgress {1}, m_autopilotMoving {2}",
+                        bAllowUpdateMoveToPosition, m_moveToPositionInProgress, m_autopilotMoving);
+
                     if (bAllowUpdateMoveToPosition && (m_moveToPositionInProgress && !m_autopilotMoving))
                     {
-                        //Check the error term of the current position in relation to the target position
-                        if (Util.GetDistanceTo(AbsolutePosition, m_moveToPositionTarget) <= 0.5f)
+                        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);
+
+                        // Check the error term of the current position in relation to the target position
+                        if (distanceToTarget <= 1)
                         {
-                            // we are close enough to the target
+                            // We are close enough to the target
                             m_moveToPositionTarget = Vector3.Zero;
                             m_moveToPositionInProgress = false;
                             update_movementflag = true;
@@ -1608,8 +1617,6 @@ namespace OpenSim.Region.Framework.Scenes
                     //                        "In {0} adding velocity to {1} of {2}", m_scene.RegionInfo.RegionName, Name, agent_control_v3);
 
                     AddNewMovement(agent_control_v3, q);
-
-                    
                 }
             }
 
@@ -1621,61 +1628,44 @@ namespace OpenSim.Region.Framework.Scenes
             m_scene.StatsReporter.AddAgentTime(Util.EnvironmentTickCountSubtract(m_perfMonMS));
         }
 
-        public void DoAutoPilot(uint not_used, Vector3 Pos, IClientAPI remote_client)
-        {
-            m_autopilotMoving = true;
-            m_autoPilotTarget = Pos;
-            m_sitAtAutoTarget = false;
-            PrimitiveBaseShape proxy = PrimitiveBaseShape.Default;
-            //proxy.PCode = (byte)PCode.ParticleSystem;
+//        public void DoAutoPilot(uint not_used, Vector3 Pos, IClientAPI remote_client)
+//        {
+//            m_autopilotMoving = true;
+//            m_autoPilotTarget = Pos;
+//            m_sitAtAutoTarget = false;
+//            PrimitiveBaseShape proxy = PrimitiveBaseShape.Default;
+//            //proxy.PCode = (byte)PCode.ParticleSystem;
+//
+//            proxyObjectGroup = new SceneObjectGroup(UUID, Pos, Rotation, proxy);
+//            proxyObjectGroup.AttachToScene(m_scene);
+//            
+//            // Commented out this code since it could never have executed, but might still be informative.
+////            if (proxyObjectGroup != null)
+////            {
+//                proxyObjectGroup.SendGroupFullUpdate();
+//                remote_client.SendSitResponse(proxyObjectGroup.UUID, Vector3.Zero, Quaternion.Identity, true, Vector3.Zero, Vector3.Zero, false);
+//                m_scene.DeleteSceneObject(proxyObjectGroup, false);
+////            }
+////            else
+////            {
+////                m_autopilotMoving = false;
+////                m_autoPilotTarget = Vector3.Zero;
+////                ControllingClient.SendAlertMessage("Autopilot cancelled");
+////            }
+//        }
 
-            proxyObjectGroup = new SceneObjectGroup(UUID, Pos, Rotation, proxy);
-            proxyObjectGroup.AttachToScene(m_scene);
-            
-            // Commented out this code since it could never have executed, but might still be informative.
-//            if (proxyObjectGroup != null)
-//            {
-                proxyObjectGroup.SendGroupFullUpdate();
-                remote_client.SendSitResponse(proxyObjectGroup.UUID, Vector3.Zero, Quaternion.Identity, true, Vector3.Zero, Vector3.Zero, false);
-                m_scene.DeleteSceneObject(proxyObjectGroup, false);
-//            }
-//            else
-//            {
-//                m_autopilotMoving = false;
-//                m_autoPilotTarget = Vector3.Zero;
-//                ControllingClient.SendAlertMessage("Autopilot cancelled");
-//            }
-        }
-
-        public void DoMoveToPosition(Object sender, string method, List<String> args)
+        /// <summary>
+        /// Move this presence to the given position over time.
+        /// </summary>
+        /// <param name="pos"></param>
+        public void DoMoveToPosition(uint not_used, Vector3 pos, IClientAPI remote_client)
         {
-            try
-            {
-                float locx = 0f;
-                float locy = 0f;
-                float locz = 0f;
-                uint regionX = 0;
-                uint regionY = 0;
-                try
-                {
-                    Utils.LongToUInts(Scene.RegionInfo.RegionHandle, out regionX, out regionY);
-                    locx = Convert.ToSingle(args[0]) - (float)regionX;
-                    locy = Convert.ToSingle(args[1]) - (float)regionY;
-                    locz = Convert.ToSingle(args[2]);
-                }
-                catch (InvalidCastException)
-                {
-                    m_log.Error("[CLIENT]: Invalid autopilot request");
-                    return;
-                }
-                m_moveToPositionInProgress = true;
-                m_moveToPositionTarget = new Vector3(locx, locy, locz);
-            }
-            catch (Exception ex)
-            {
-                //Why did I get this error?
-               m_log.Error("[SCENEPRESENCE]: DoMoveToPosition" + ex);
-            }
+            m_log.DebugFormat(
+                "[SCENE PRESENCE]: Avatar {0} received request to move to position {1} in {2}",
+                Name, pos, m_scene.RegionInfo.RegionName);
+
+            m_moveToPositionInProgress = true;
+            m_moveToPositionTarget = pos;
         }
 
         private void CheckAtSitTarget()
diff --git a/OpenSim/Region/Framework/Scenes/UndoState.cs b/OpenSim/Region/Framework/Scenes/UndoState.cs
index 393f42d..d34d8e5 100644
--- a/OpenSim/Region/Framework/Scenes/UndoState.cs
+++ b/OpenSim/Region/Framework/Scenes/UndoState.cs
@@ -35,7 +35,7 @@ namespace OpenSim.Region.Framework.Scenes
 {
     public class UndoState
     {
-        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
+//        private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
 
         public Vector3 Position = Vector3.Zero;
         public Vector3 Scale = Vector3.Zero;
diff --git a/OpenSim/Region/OptionalModules/World/NPC/NPCAvatar.cs b/OpenSim/Region/OptionalModules/World/NPC/NPCAvatar.cs
index 3afcc8d..e87993a 100644
--- a/OpenSim/Region/OptionalModules/World/NPC/NPCAvatar.cs
+++ b/OpenSim/Region/OptionalModules/World/NPC/NPCAvatar.cs
@@ -99,6 +99,7 @@ namespace OpenSim.Region.OptionalModules.World.NPC
         {
 
         }
+        
         public void SendSitResponse(UUID TargetID, Vector3 OffsetPos, Quaternion SitOrientation, bool autopilot,
                                         Vector3 CameraAtOffset, Vector3 CameraEyeOffset, bool ForceMouseLook)
         {
diff --git a/OpenSim/Region/OptionalModules/World/NPC/NPCModule.cs b/OpenSim/Region/OptionalModules/World/NPC/NPCModule.cs
index 6286dc8..8cb8318 100644
--- a/OpenSim/Region/OptionalModules/World/NPC/NPCModule.cs
+++ b/OpenSim/Region/OptionalModules/World/NPC/NPCModule.cs
@@ -140,7 +140,16 @@ namespace OpenSim.Region.OptionalModules.World.NPC
                 {
                     ScenePresence sp;
                     scene.TryGetScenePresence(agentID, out sp);
-                    sp.DoAutoPilot(0, pos, m_avatars[agentID]);
+
+//                    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]);
                 }
             }
         }