From 921692a15f7b793da55cd4ddc4d85eae4393964e Mon Sep 17 00:00:00 2001 From: Charles Krinke Date: Thu, 27 Nov 2008 05:16:47 +0000 Subject: 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. --- OpenSim/Region/Environment/Scenes/ScenePresence.cs | 132 +++++++++++++++++++++ 1 file changed, 132 insertions(+) (limited to 'OpenSim/Region/Environment/Scenes/ScenePresence.cs') 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 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()); -- cgit v1.1