From f9689f5cc96a937ba592487102e3a04535ca4739 Mon Sep 17 00:00:00 2001
From: Justin Clark-Casey (justincc)
Date: Wed, 3 Aug 2011 00:46:46 +0100
Subject: 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.
---
OpenSim/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 a508813..06b27ca 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;
--
cgit v1.1