Implement move to/autopilot for z axis movement as well.

This is jerky (an artifact of the way it's being done, I think), but it's better than on implementation.

2 files changed, 40 insertions, 17 deletions

diff --git a/OpenSim/Region/Framework/Scenes/Animation/ScenePresenceAnimator.cs b/OpenSim/Region/Framework/Scenes/Animation/ScenePresenceAnimator.cs
index 4865481..1334230 100644
--- a/OpenSim/Region/Framework/Scenes/Animation/ScenePresenceAnimator.cs
+++ b/OpenSim/Region/Framework/Scenes/Animation/ScenePresenceAnimator.cs
@@ -295,7 +295,7 @@ namespace OpenSim.Region.Framework.Scenes.Animation
                 if (move.X != 0f || move.Y != 0f)
                 {
                     // Walking / crouchwalking / running
-                    if (move.Z < 0f)
+                    if (move.Z < 0)
                         return "CROUCHWALK";
                     else if (m_scenePresence.SetAlwaysRun)
                         return "RUN";
@@ -305,7 +305,7 @@ namespace OpenSim.Region.Framework.Scenes.Animation
                 else
                 {
                     // Not walking
-                    if (move.Z < 0f)
+                    if (move.Z < 0)
                         return "CROUCH";
                     else
                         return "STAND";
diff --git a/OpenSim/Region/Framework/Scenes/ScenePresence.cs b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
index 9c71492..7b228c6 100644
--- a/OpenSim/Region/Framework/Scenes/ScenePresence.cs
+++ b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
@@ -1559,9 +1559,9 @@ namespace OpenSim.Region.Framework.Scenes
             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);
+                        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)
@@ -1575,7 +1575,7 @@ namespace OpenSim.Region.Framework.Scenes
                 {
                     try
                     {
-                        // move avatar in 2D at one meter/second towards target, in avatar coordinate frame.
+                        // move avatar in 3D 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
@@ -1584,9 +1584,8 @@ namespace OpenSim.Region.Framework.Scenes
                             (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;
+//                        Vector3 LocalVectorToTarget2D = new Vector3((float)(LocalVectorToTarget3D.X), (float)(LocalVectorToTarget3D.Y), 0f);
+                        LocalVectorToTarget3D.Normalize();
 
                         // update avatar movement flags. the avatar coordinate system is as follows:
                         //
@@ -1609,31 +1608,48 @@ namespace OpenSim.Region.Framework.Scenes
 
                         // based on the above avatar coordinate system, classify the movement into
                         // one of left/right/back/forward.
-                        if (LocalVectorToTarget2D.Y > 0)//MoveLeft
+                        if (LocalVectorToTarget3D.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 (LocalVectorToTarget3D.X > 0) //Move Forward
+                        {
+                            m_movementflag += (byte)(uint)Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD;
+                            AgentControlFlags |= (uint)Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD;
+                            updated = true;
+                        }
+
+                        if (LocalVectorToTarget3D.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
+                        else if (LocalVectorToTarget3D.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
+
+                        if (LocalVectorToTarget3D.Z > 0) //Up
                         {
-                            m_movementflag += (byte)(uint)Dir_ControlFlags.DIR_CONTROL_FLAG_BACK;
-                            AgentControlFlags |= (uint)Dir_ControlFlags.DIR_CONTROL_FLAG_BACK;
+                            m_movementflag += (byte)(uint)Dir_ControlFlags.DIR_CONTROL_FLAG_UP;
+                            //AgentControlFlags
+                            AgentControlFlags |= (uint)Dir_ControlFlags.DIR_CONTROL_FLAG_UP;
                             updated = true;
                         }
-                        else if (LocalVectorToTarget2D.X > 0) //Move Forward
+                        else if (LocalVectorToTarget3D.Z < 0) //Down
                         {
-                            m_movementflag += (byte)(uint)Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD;
-                            AgentControlFlags |= (uint)Dir_ControlFlags.DIR_CONTROL_FLAG_FORWARD;
+                            m_movementflag += (byte)(uint)Dir_ControlFlags.DIR_CONTROL_FLAG_DOWN;
+                            AgentControlFlags |= (uint)Dir_ControlFlags.DIR_CONTROL_FLAG_DOWN;
                             updated = true;
                         }
+
+                        agent_control_v3 += LocalVectorToTarget3D;
                     }
                     catch (Exception e)
                     {
@@ -1682,6 +1698,13 @@ namespace OpenSim.Region.Framework.Scenes
                 "[SCENE PRESENCE]: Avatar {0} received request to move to position {1} in {2}",
                 Name, pos, m_scene.RegionInfo.RegionName);
 
+            Vector3 heightAdjust = new Vector3(0, 0, Appearance.AvatarHeight / 2);
+            pos += heightAdjust;
+
+            // Anti duck-walking measure
+            if (Math.Abs(pos.Z - AbsolutePosition.Z) < 0.2f)
+                pos.Z = AbsolutePosition.Z;
+
             m_moveToPositionInProgress = true;
             m_moveToPositionTarget = pos;