diff --git a/OpenSim/Framework/IClientAPI.cs b/OpenSim/Framework/IClientAPI.cs index 8d9cdaaa01..88cb49e89b 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 7c665996d8..e3b91a5ed2 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(); //Global/static handlers for all clients protected Dictionary m_packetHandlers = new Dictionary(); + protected Dictionary m_genericPacketHandlers = new Dictionary(); //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; + } + /// /// Try to process a packet using registered packet handlers /// @@ -3457,21 +3472,30 @@ namespace OpenSim.Region.ClientStack.LindenUDP public bool HandleGenericMessage(IClientAPI sender, Packet pack) { GenericMessagePacket gmpack = (GenericMessagePacket) pack; - handlerGenericMessage = OnGenericMessage; - - List msg = new List(); - - 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) - { - msg.Add(Util.FieldToString(block.Parameter)); - } + List msg = new List(); - handlerGenericMessage(this, method, msg); + if (handlerGenericMessage != null) + { + foreach (GenericMessagePacket.ParamListBlock block in gmpack.ParamList) + { + msg.Add(Util.FieldToString(block.Parameter)); + } + try + { + handlerGenericMessage(sender, method, msg); + return true; + } + catch (Exception ex) + { + } + } } - 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 0a27086164..b5a5123bc2 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 29c7c3e196..998140f01f 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 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 392a53b1f7..606b1fb846 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 b5ab1ace0d..89b3bf61fe 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 } }