Thank you kindly, Nlin for a patch that:

Adds a new method to IClientAPI to allow adding message handlers 
for GenericMessages (of which "autopilot" is one). Part 2 adds a 
specific autopilot handler in ScenePresence.cs.
2) Removing unused variables and functions.
3) Simplifying the navigation logic in ScenePresence.cs. The 
original patch was somewhat complex because it included orientation
logic for a future enhancement of orienting the avatar to point 
towards the direction being walked. Currently this isn't working, 
though, so I removed the orientation code, which leaves just the 
smaller and hopefully simpler-to-understand movement code.

6 files changed, 199 insertions, 15 deletions

diff --git a/OpenSim/Framework/IClientAPI.cs b/OpenSim/Framework/IClientAPI.cs
index 8d9cdaa..88cb49e 100644
--- a/OpenSim/Framework/IClientAPI.cs
+++ b/OpenSim/Framework/IClientAPI.cs
@@ -1045,5 +1045,7 @@ namespace OpenSim.Framework
         void SendTerminateFriend(UUID exFriendID);
 
         void KillEndDone();
+
+        bool AddGenericPacketHandler(string MethodName, GenericMessage handler);
     }
 }
diff --git a/OpenSim/Region/ClientStack/LindenUDP/LLClientView.cs b/OpenSim/Region/ClientStack/LindenUDP/LLClientView.cs
index 7c66599..e3b91a5 100644
--- a/OpenSim/Region/ClientStack/LindenUDP/LLClientView.cs
+++ b/OpenSim/Region/ClientStack/LindenUDP/LLClientView.cs
@@ -93,6 +93,7 @@ namespace OpenSim.Region.ClientStack.LindenUDP
             new Dictionary<PacketType, PacketMethod>(); //Global/static handlers for all clients
 
         protected Dictionary<PacketType, PacketMethod> m_packetHandlers = new Dictionary<PacketType, PacketMethod>();
+        protected Dictionary<string, GenericMessage> m_genericPacketHandlers = new Dictionary<string, GenericMessage>(); //PauPaw:Local Generic Message handlers
 
         protected IScene m_scene;
 
@@ -608,6 +609,20 @@ namespace OpenSim.Region.ClientStack.LindenUDP
             return result;
         }
 
+        public bool AddGenericPacketHandler(string MethodName, GenericMessage handler)
+        {
+            bool result = false;
+            lock (m_genericPacketHandlers)
+            {
+                if (!m_genericPacketHandlers.ContainsKey(MethodName))
+                {
+                    m_genericPacketHandlers.Add(MethodName, handler);
+                    result = true;
+                }
+            }
+            return result;
+        }
+
         /// <summary>
         /// Try to process a packet using registered packet handlers
         /// </summary>
@@ -3457,21 +3472,30 @@ namespace OpenSim.Region.ClientStack.LindenUDP
         public bool HandleGenericMessage(IClientAPI sender, Packet pack)
         {
             GenericMessagePacket gmpack = (GenericMessagePacket) pack;
-            handlerGenericMessage = OnGenericMessage;
-
-            List<string> msg = new List<string>();
-
-            if (handlerGenericMessage != null)
+            if (m_genericPacketHandlers.Count == 0) return false;
+            handlerGenericMessage = null;
+            string method = Util.FieldToString(gmpack.MethodData.Method).ToLower().Trim();
+            if(m_genericPacketHandlers.TryGetValue(method, out handlerGenericMessage))
             {
-                string method = Util.FieldToString(gmpack.MethodData.Method);
-                foreach (GenericMessagePacket.ParamListBlock block in gmpack.ParamList)
+                List<string> msg = new List<string>();
+
+                if (handlerGenericMessage != null)
                 {
-                    msg.Add(Util.FieldToString(block.Parameter));
+                    foreach (GenericMessagePacket.ParamListBlock block in gmpack.ParamList)
+                    {
+                        msg.Add(Util.FieldToString(block.Parameter));
+                    }
+                    try
+                    {
+                        handlerGenericMessage(sender, method, msg);
+                        return true;
+                    }
+                    catch (Exception ex)
+                    {
+                    }
                 }
-
-                handlerGenericMessage(this, method, msg);
             }
-            return true;
+            return false;
         }
 
         public bool HandleObjectGroupRequest(IClientAPI sender, Packet Pack)
@@ -3965,12 +3989,12 @@ namespace OpenSim.Region.ClientStack.LindenUDP
             {
                     #region Scene/Avatar
 
-                case PacketType.GenericMessage:
-                    GenericMessagePacket gmpack = (GenericMessagePacket)Pack;
+                      //                case PacketType.GenericMessage:
+                      //                    GenericMessagePacket gmpack = (GenericMessagePacket)Pack;
 
-                    DecipherGenericMessage(Utils.BytesToString(gmpack.MethodData.Method), gmpack.MethodData.Invoice, gmpack.ParamList);
+                      //                    DecipherGenericMessage(Utils.BytesToString(gmpack.MethodData.Method), gmpack.MethodData.Invoice, gmpack.ParamList);
 
-                    break;
+                      //                    break;
                 case PacketType.AvatarPropertiesRequest:
                     AvatarPropertiesRequestPacket avatarProperties = (AvatarPropertiesRequestPacket)Pack;
 
diff --git a/OpenSim/Region/Environment/Modules/World/NPC/NPCAvatar.cs b/OpenSim/Region/Environment/Modules/World/NPC/NPCAvatar.cs
index 0a27086..b5a5123 100644
--- a/OpenSim/Region/Environment/Modules/World/NPC/NPCAvatar.cs
+++ b/OpenSim/Region/Environment/Modules/World/NPC/NPCAvatar.cs
@@ -991,5 +991,15 @@ namespace OpenSim.Region.Environment.Modules.World.NPC
         public void SendTerminateFriend(UUID exFriendID)
         {
         }
+
+        #region IClientAPI Members
+
+
+        public bool AddGenericPacketHandler(string MethodName, GenericMessage handler)
+        {
+            throw new NotImplementedException();
+        }
+
+        #endregion
     }
 }
diff --git a/OpenSim/Region/Environment/Scenes/ScenePresence.cs b/OpenSim/Region/Environment/Scenes/ScenePresence.cs
index 29c7c3e..998140f 100644
--- a/OpenSim/Region/Environment/Scenes/ScenePresence.cs
+++ b/OpenSim/Region/Environment/Scenes/ScenePresence.cs
@@ -175,6 +175,12 @@ namespace OpenSim.Region.Environment.Scenes
 
         private string m_nextSitAnimation = String.Empty;
 
+        //PauPaw:Proper PID Controler for autopilot************
+        private bool m_moveToPositionInProgress = false;
+        private Vector3 m_moveToPositionTarget = Vector3.Zero;
+        private int m_moveToPositionStateStatus = 0;
+        //*****************************************************
+
         // Agent's Draw distance.
         protected float m_DrawDistance = 0f;
 
@@ -550,6 +556,7 @@ namespace OpenSim.Region.Environment.Scenes
             m_controllingClient.OnStopAnim += HandleStopAnim;
             m_controllingClient.OnForceReleaseControls += HandleForceReleaseControls;
             m_controllingClient.OnAutoPilotGo += DoAutoPilot;
+            m_controllingClient.AddGenericPacketHandler("autopilot", DoMoveToPosition);
 
             // ControllingClient.OnChildAgentStatus += new StatusChange(this.ChildStatusChange);
             // ControllingClient.OnStopMovement += new GenericCall2(this.StopMovement);
@@ -1063,10 +1070,13 @@ namespace OpenSim.Region.Environment.Scenes
 
                 if (m_parentID == 0)
                 {
+                    bool bAllowUpdateMoveToPosition = false;
+                    bool bResetMoveToPosition = false;
                     foreach (Dir_ControlFlags DCF in Enum.GetValues(typeof (Dir_ControlFlags)))
                     {
                         if ((flags & (uint) DCF) != 0)
                         {
+                            bResetMoveToPosition = true;
                             DCFlagKeyPressed = true;
                             try
                             {
@@ -1090,9 +1100,100 @@ namespace OpenSim.Region.Environment.Scenes
                                 m_movementflag -= (byte) (uint) DCF;
                                 update_movementflag = true;
                             }
+                            else
+                            {
+                                bAllowUpdateMoveToPosition = true;
+                            }
                         }
                         i++;
                     }
+
+                    //Paupaw:Do Proper PID for Autopilot here
+                    if (bResetMoveToPosition)
+                    {
+                        m_moveToPositionTarget = Vector3.Zero;
+                        m_moveToPositionInProgress = false;
+                        update_movementflag = true;
+                        bAllowUpdateMoveToPosition = false;
+                    }
+
+                    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) <= 1.5)
+                        {
+                            // we are close enough to the target
+                            m_moveToPositionTarget = Vector3.Zero;
+                            m_moveToPositionInProgress = false;
+                            update_movementflag = true;
+                        }
+                        else
+                        {
+                            try
+                            {
+                                // move avatar in 2D at one meter/second towards target, in avatar coordinate frame.
+                                // This movement vector gets added to the velocity through AddNewMovement().
+                                // Theoretically we might need a more complex PID approach here if other 
+                                // 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
+                                    * 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);
+                                LocalVectorToTarget2D.Normalize();
+                                agent_control_v3 += LocalVectorToTarget2D;
+
+                                // update avatar movement flags. the avatar coordinate system is as follows:
+                                //
+                                //                        +X (forward)
+                                //
+                                //                        ^
+                                //                        |
+                                //                        |
+                                //                        |
+                                //                        |
+                                //     (left) +Y <--------o--------> -Y
+                                //                       avatar
+                                //                        |
+                                //                        |
+                                //                        |
+                                //                        |
+                                //                        v
+                                //                        -X
+                                //
+
+                                // based on the above avatar coordinate system, classify the movement into 
+                                // one of left/right/back/forward.
+                                if (LocalVectorToTarget2D.Y > 0)//MoveLeft
+                                {
+                                    m_movementflag += (byte)(uint)Dir_ControlFlags.DIR_CONTROL_FLAG_LEFT;
+                                    update_movementflag = true;
+                                }
+                                else if (LocalVectorToTarget2D.Y < 0) //MoveRight
+                                {
+                                    m_movementflag += (byte)(uint)Dir_ControlFlags.DIR_CONTROL_FLAG_RIGHT;
+                                    update_movementflag = true;
+                                }
+                                if (LocalVectorToTarget2D.X < 0) //MoveBack
+                                {
+                                    m_movementflag += (byte)(uint)Dir_ControlFlags.DIR_CONTROL_FLAG_BACK;
+                                    update_movementflag = true;
+                                }
+                                else if (LocalVectorToTarget2D.X > 0) //Move Forward
+                                {
+                                    m_movementflag += (byte)(uint)Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD;
+                                    update_movementflag = true;
+                                }
+                            }
+                            catch (Exception)
+                            {
+
+                                //Avoid system crash, can be slower but...
+                            }
+
+                        }
+                    }
                 }
                 
                 // Cause the avatar to stop flying if it's colliding
@@ -1158,6 +1259,37 @@ namespace OpenSim.Region.Environment.Scenes
 //            }
         }
 
+        public void DoMoveToPosition(Object sender, string method, List<String> args)
+        {
+            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?
+                System.Diagnostics.Debug.WriteLine(ex.ToString());
+            }
+        }
+
         private void CheckAtSitTarget()
         {
             //m_log.Debug("[AUTOPILOT]: " + Util.GetDistanceTo(AbsolutePosition, m_autoPilotTarget).ToString());
diff --git a/OpenSim/Region/Environment/Scenes/Tests/TestClient.cs b/OpenSim/Region/Environment/Scenes/Tests/TestClient.cs
index 392a53b..606b1fb 100644
--- a/OpenSim/Region/Environment/Scenes/Tests/TestClient.cs
+++ b/OpenSim/Region/Environment/Scenes/Tests/TestClient.cs
@@ -939,5 +939,11 @@ namespace OpenSim.Region.Environment.Scenes.Tests
         public void SendTerminateFriend(UUID exFriendID)
         {
         }
+
+        public bool AddGenericPacketHandler(string MethodName, GenericMessage handler)
+        {
+            throw new NotImplementedException();
+        }
+
     }
 }
diff --git a/OpenSim/Region/Examples/SimpleModule/MyNpcCharacter.cs b/OpenSim/Region/Examples/SimpleModule/MyNpcCharacter.cs
index b5ab1ac..89b3bf6 100644
--- a/OpenSim/Region/Examples/SimpleModule/MyNpcCharacter.cs
+++ b/OpenSim/Region/Examples/SimpleModule/MyNpcCharacter.cs
@@ -991,5 +991,15 @@ namespace OpenSim.Region.Examples.SimpleModule
         public void SendTerminateFriend(UUID exFriendID)
         {
         }
+
+        #region IClientAPI Members
+
+
+        public bool AddGenericPacketHandler(string MethodName, GenericMessage handler)
+        {
+            throw new NotImplementedException();
+        }
+
+        #endregion
     }
 }