From f9689f5cc96a937ba592487102e3a04535ca4739 Mon Sep 17 00:00:00 2001 From: "Justin Clark-Casey (justincc)" Date: Wed, 3 Aug 2011 00:46:46 +0100 Subject: [PATCH] refactor: move out code from HandleAgentUpdate() which processes updates to move to a set position Also comment out the really spammy log message I accidentally left in on the last commit. --- .../Region/Framework/Scenes/ScenePresence.cs | 214 ++++++++++-------- 1 file changed, 116 insertions(+), 98 deletions(-) diff --git a/OpenSim/Region/Framework/Scenes/ScenePresence.cs b/OpenSim/Region/Framework/Scenes/ScenePresence.cs index a508813979..06b27ca539 100644 --- a/OpenSim/Region/Framework/Scenes/ScenePresence.cs +++ b/OpenSim/Region/Framework/Scenes/ScenePresence.cs @@ -1483,105 +1483,8 @@ namespace OpenSim.Region.Framework.Scenes i++; } - //Paupaw:Do Proper PID for Autopilot here - if (bResetMoveToPosition) - { - m_moveToPositionTarget = Vector3.Zero; - m_moveToPositionInProgress = false; + if (DoMoveToPositionUpdate(ref agent_control_v3, bodyRotation, bResetMoveToPosition, bAllowUpdateMoveToPosition)) update_movementflag = true; - 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)) - { - 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 - 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; - //AgentControlFlags - AgentControlFlags |= (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; - AgentControlFlags |= (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; - AgentControlFlags |= (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; - AgentControlFlags |= (uint)Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD; - update_movementflag = true; - } - } - catch (Exception e) - { - //Avoid system crash, can be slower but... - m_log.DebugFormat("Crash! {0}", e.ToString()); - } - } - } } // Cause the avatar to stop flying if it's colliding @@ -1628,6 +1531,121 @@ namespace OpenSim.Region.Framework.Scenes m_scene.StatsReporter.AddAgentTime(Util.EnvironmentTickCountSubtract(m_perfMonMS)); } + /// + /// Process moving the avatar if a position has been set. + /// + /// Cumulative agent movement that this method will update. + /// New body rotation of the avatar. + /// If true, clear the move to position + /// If true, allow the update in principle. + /// True if movement has been updated in some way. False otherwise. + protected bool DoMoveToPositionUpdate( + ref Vector3 agent_control_v3, Quaternion bodyRotation, bool reset, bool allowUpdate) + { + bool updated = false; + + //Paupaw:Do Proper PID for Autopilot here + if (reset) + { + m_moveToPositionTarget = Vector3.Zero; + m_moveToPositionInProgress = false; + updated = true; + } + + m_log.DebugFormat( + "[SCENE PRESENCE]: bAllowUpdateMoveToPosition {0}, m_moveToPositionInProgress {1}, m_autopilotMoving {2}", + allowUpdate, m_moveToPositionInProgress, m_autopilotMoving); + + 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); + + // 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; + m_moveToPositionInProgress = false; + updated = 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; + //AgentControlFlags + AgentControlFlags |= (uint)Dir_ControlFlags.DIR_CONTROL_FLAG_LEFT; + updated = true; + } + else if (LocalVectorToTarget2D.Y < 0) //MoveRight + { + m_movementflag += (byte)(uint)Dir_ControlFlags.DIR_CONTROL_FLAG_RIGHT; + AgentControlFlags |= (uint)Dir_ControlFlags.DIR_CONTROL_FLAG_RIGHT; + updated = true; + } + if (LocalVectorToTarget2D.X < 0) //MoveBack + { + m_movementflag += (byte)(uint)Dir_ControlFlags.DIR_CONTROL_FLAG_BACK; + AgentControlFlags |= (uint)Dir_ControlFlags.DIR_CONTROL_FLAG_BACK; + updated = true; + } + else if (LocalVectorToTarget2D.X > 0) //Move Forward + { + m_movementflag += (byte)(uint)Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD; + AgentControlFlags |= (uint)Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD; + updated = true; + } + } + catch (Exception e) + { + //Avoid system crash, can be slower but... + m_log.DebugFormat("Crash! {0}", e.ToString()); + } + } + } + + return updated; + } + // public void DoAutoPilot(uint not_used, Vector3 Pos, IClientAPI remote_client) // { // m_autopilotMoving = true;