change avatar physics and motion control. Still not that good :(

2 files changed, 173 insertions, 143 deletions

diff --git a/OpenSim/Region/Framework/Scenes/Animation/ScenePresenceAnimator.cs b/OpenSim/Region/Framework/Scenes/Animation/ScenePresenceAnimator.cs
index db3b834..6003e92 100644
--- a/OpenSim/Region/Framework/Scenes/Animation/ScenePresenceAnimator.cs
+++ b/OpenSim/Region/Framework/Scenes/Animation/ScenePresenceAnimator.cs
@@ -56,11 +56,13 @@ namespace OpenSim.Region.Framework.Scenes.Animation
         /// The current movement animation
         /// </value>
         public string CurrentMovementAnimation { get; private set; }
-        
+
         private int m_animTickFall;
-        public int m_animTickJump;              // ScenePresence has to see this to control +Z force
+        private int m_animTickLand;
+        private int m_animTickJump;
+
         public bool m_jumping = false; 
-        public float m_jumpVelocity = 0f;
+
 //        private int m_landing = 0;
 
         /// <summary>
@@ -68,7 +70,7 @@ namespace OpenSim.Region.Framework.Scenes.Animation
         /// </summary>
         public bool Falling { get; private set; }
 
-        private float m_fallHeight;
+        private float m_lastFallVelocity;
 
         /// <value>
         /// The scene presence that this animator applies to
@@ -205,7 +207,7 @@ namespace OpenSim.Region.Framework.Scenes.Animation
 
                 if (aoSitGndAnim != UUID.Zero)
                 {
-                    avnChangeAnim(aoSitGndAnim, false, false);
+                    avnChangeAnim(aoSitGndAnim, false, true);
                     aoSitGndAnim = UUID.Zero;
                 }
 
@@ -258,20 +260,38 @@ namespace OpenSim.Region.Framework.Scenes.Animation
             return ret;
         }
 
+        public enum motionControlStates : byte
+        {
+            sitted = 0,
+            flying,
+            falling,
+            jumping,
+            landing,
+            onsurface
+        }
+
+        public motionControlStates currentControlState = motionControlStates.onsurface;
+
         /// <summary>
         /// This method determines the proper movement related animation
         /// </summary>
         private string DetermineMovementAnimation()
         {
-            const float FALL_DELAY = 800f;
-            const float PREJUMP_DELAY = 200f;
-            const float JUMP_PERIOD = 800f;
+            const int FALL_DELAY = 800;
+            const int PREJUMP_DELAY = 200;
+            const int JUMP_PERIOD = 800;
             #region Inputs
 
             if (m_scenePresence.SitGround)
+            {
+                currentControlState = motionControlStates.sitted;
                 return "SITGROUND";
+            }
             if (m_scenePresence.ParentID != 0 || m_scenePresence.ParentUUID != UUID.Zero)
+            {
+                currentControlState = motionControlStates.sitted;
                 return "SIT";
+            }
 
             AgentManager.ControlFlags controlFlags = (AgentManager.ControlFlags)m_scenePresence.AgentControlFlags;
             PhysicsActor actor = m_scenePresence.PhysicsActor;
@@ -311,17 +331,31 @@ namespace OpenSim.Region.Framework.Scenes.Animation
 //            bool moving = (move != Vector3.Zero);
             #endregion Inputs
 
+            // no physics actor case
+            if (actor == null)
+            {
+                // well what to do?
+
+                currentControlState = motionControlStates.onsurface;
+                if (move.X != 0f || move.Y != 0f)
+                    return "WALK";
+
+                return "STAND";
+            }
+
+
             #region Flying
 
-            if (actor != null && actor.Flying)
+            bool isColliding = actor.IsColliding;
+
+            if (actor.Flying)
             {
                 m_animTickFall = 0;
                 m_animTickJump = 0;
                 m_jumping = false;
                 Falling = false;
-                m_jumpVelocity = 0f;
-                actor.Selected = false;
-                m_fallHeight = actor.Position.Z;    // save latest flying height
+
+                currentControlState = motionControlStates.flying;
 
                 if (move.X != 0f || move.Y != 0f)
                 {
@@ -333,8 +367,13 @@ namespace OpenSim.Region.Framework.Scenes.Animation
                 }
                 else if (move.Z < 0f)
                 {
-                    if (actor != null && actor.IsColliding)
+                    if (isColliding)
+                    {
+                        actor.Flying = false;
+                        currentControlState = motionControlStates.landing;
+                        m_animTickLand = Environment.TickCount;
                         return "LAND";
+                    }
                     else
                         return "HOVER_DOWN";
                 }
@@ -343,29 +382,41 @@ namespace OpenSim.Region.Framework.Scenes.Animation
                     return "HOVER";
                 }
             }
+            else
+            {
+                if (isColliding && currentControlState == motionControlStates.flying)
+                {
+                    currentControlState = motionControlStates.landing;
+                    m_animTickLand = Environment.TickCount;
+                    return "LAND";
+                }
+            }
 
             #endregion Flying
 
             #region Falling/Floating/Landing
 
-            if ((actor == null || !actor.IsColliding) && !m_jumping)
+            if (!isColliding && currentControlState != motionControlStates.jumping)
             {
-                float fallElapsed = (float)(Environment.TickCount - m_animTickFall);
-                float fallVelocity = (actor != null) ? actor.Velocity.Z : 0.0f;
+                float fallVelocity = actor.Velocity.Z;
 
-                if (!m_jumping && (fallVelocity < -3.0f))
+                if (fallVelocity < -2.5f)
                     Falling = true;
 
-                if (m_animTickFall == 0 || (fallVelocity >= 0.0f))
+                if (m_animTickFall == 0 || (fallVelocity >= -0.5f))
                 {
-                    // not falling yet, or going up         
-                    // reset start of fall time
                     m_animTickFall = Environment.TickCount;
                 }
-                else if (!m_jumping && (fallElapsed > FALL_DELAY) && (fallVelocity < -3.0f) && (m_scenePresence.WasFlying))
+                else
                 {
-                    // Falling long enough to trigger the animation
-                    return "FALLDOWN";
+                    int fallElapsed = (Environment.TickCount - m_animTickFall);
+                    if ((fallElapsed > FALL_DELAY) && (fallVelocity < -3.0f))
+                    {
+                        currentControlState = motionControlStates.falling;
+                        m_lastFallVelocity = fallVelocity;
+                        // Falling long enough to trigger the animation
+                        return "FALLDOWN";
+                    }
                 }
 
                 // Check if the user has stopped walking just now
@@ -375,49 +426,44 @@ namespace OpenSim.Region.Framework.Scenes.Animation
                 return CurrentMovementAnimation;
             }
 
-            #endregion Falling/Floating/Landing
+            m_animTickFall = 0;
 
+            #endregion Falling/Floating/Landing
 
             #region Jumping     // section added for jumping...
 
-            int jumptime;
-            jumptime = Environment.TickCount - m_animTickJump;
-
-            if ((move.Z > 0f) && (!m_jumping))
+            if (isColliding &&  move.Z > 0f && currentControlState != motionControlStates.jumping)
             {
                 // Start jumping, prejump
-                m_animTickFall = 0;
+                currentControlState = motionControlStates.jumping;
                 m_jumping = true;
                 Falling = false;
-                actor.Selected = true;      // borrowed for jumping flag
                 m_animTickJump = Environment.TickCount;
-                m_jumpVelocity = 0.35f;
                 return "PREJUMP";
             }
 
-            if (m_jumping)
+            if (currentControlState == motionControlStates.jumping)
             {
+                int jumptime = Environment.TickCount - m_animTickJump;
                 if ((jumptime > (JUMP_PERIOD * 1.5f)) && actor.IsColliding)
                 {
                     // end jumping
                     m_jumping = false;
                     Falling = false;
                     actor.Selected = false;      // borrowed for jumping flag
-                    m_jumpVelocity = 0f;
-                    m_animTickFall = Environment.TickCount;
+                    m_animTickLand = Environment.TickCount;
+                    currentControlState = motionControlStates.landing;
                     return "LAND";
                 }
                 else if (jumptime > JUMP_PERIOD)
                 {
                     // jump down
-                    m_jumpVelocity = 0f;
                     return "JUMP";
                 }
                 else if (jumptime > PREJUMP_DELAY)
                 {
                     // jump up
                     m_jumping = true;
-                    m_jumpVelocity = 10f;
                     return "JUMP";
                 }
             }
@@ -426,42 +472,48 @@ namespace OpenSim.Region.Framework.Scenes.Animation
 
             #region Ground Movement
 
-            if (CurrentMovementAnimation == "FALLDOWN")
+            if (currentControlState == motionControlStates.falling)
             {
                 Falling = false;
-                m_animTickFall = Environment.TickCount;
+                currentControlState = motionControlStates.landing;
+                m_animTickLand = Environment.TickCount;
                 // TODO: SOFT_LAND support
-                float fallHeight = m_fallHeight - actor.Position.Z;
-                if (fallHeight > 15.0f)
+                float fallVsq =m_lastFallVelocity*m_lastFallVelocity;
+                if (fallVsq > 300f) // aprox 20*h 
                     return "STANDUP";
-                else if (fallHeight > 8.0f)
+                else if (fallVsq > 160f)
                     return "SOFT_LAND";
                 else
                     return "LAND";
             }
-            else if ((CurrentMovementAnimation == "LAND") || (CurrentMovementAnimation == "SOFT_LAND") || (CurrentMovementAnimation == "STANDUP"))
+
+
+            if (currentControlState == motionControlStates.landing)
             {
-                int landElapsed = Environment.TickCount - m_animTickFall;
+                Falling = false;
+                int landElapsed = Environment.TickCount - m_animTickLand;
                 int limit = 1000;
                 if (CurrentMovementAnimation == "LAND")
                     limit = 350;
                 // NB if the above is set too long a weird anim reset from some place prevents STAND from being sent to client
 
-                if ((m_animTickFall != 0) && (landElapsed <= limit))
+                if ((m_animTickLand != 0) && (landElapsed <= limit))
                 {
                     return CurrentMovementAnimation;
                 }
                 else
                 {
-                    m_fallHeight = actor.Position.Z;    // save latest flying height
+                    currentControlState = motionControlStates.onsurface;
+                    m_animTickLand = 0;
                     return "STAND";
                 }
             }
 
+
             // next section moved outside paren. and realigned for jumping
             if (move.X != 0f || move.Y != 0f)
             {
-                m_fallHeight = actor.Position.Z;    // save latest flying height
+                currentControlState = motionControlStates.onsurface;
                 Falling = false;
                 // Walking / crouchwalking / running
                 if (move.Z < 0f)
@@ -480,6 +532,7 @@ namespace OpenSim.Region.Framework.Scenes.Animation
             }
             else if (!m_jumping)
             {
+                currentControlState = motionControlStates.onsurface;
                 Falling = false;
                 // Not walking
                 if (move.Z < 0)
@@ -493,8 +546,6 @@ namespace OpenSim.Region.Framework.Scenes.Animation
             }
             #endregion Ground Movement
 
-            Falling = false;
-
             return CurrentMovementAnimation;
         }
 
diff --git a/OpenSim/Region/Framework/Scenes/ScenePresence.cs b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
index 5946979..e6a366d 100644
--- a/OpenSim/Region/Framework/Scenes/ScenePresence.cs
+++ b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
@@ -287,14 +287,7 @@ namespace OpenSim.Region.Framework.Scenes
             set { PhysicsActor.Flying = value; }
         }
 
-        // add for fly velocity control
-        private bool FlyingOld {get; set;}
-        public bool WasFlying
-        {
-            get; private set;
-        }
-
-        public bool IsColliding
+         public bool IsColliding
         {
             get { return PhysicsActor != null && PhysicsActor.IsColliding; }
             // We would expect setting IsColliding to be private but it's used by a hack in Scene
@@ -936,7 +929,7 @@ namespace OpenSim.Region.Framework.Scenes
         /// <remarks>
         /// AGENT_CONTRL_STOP comes about if user holds down space key on viewers.
         /// </remarks>
-        private float AgentControlStopSlowWhilstMoving = 0.5f;
+        private float AgentControlStopSlowWhilstMoving = 0.2f;
 
         private bool m_forceFly;
 
@@ -2174,7 +2167,7 @@ namespace OpenSim.Region.Framework.Scenes
                 bool DCFlagKeyPressed = false;
                 Vector3 agent_control_v3 = Vector3.Zero;
 
-                bool newFlying = actor.Flying;
+                bool newFlying = false;
 
                 if (ForceFly)
                     newFlying = true;
@@ -2286,11 +2279,11 @@ namespace OpenSim.Region.Framework.Scenes
                 if (Flying && !ForceFly)
                 {
                     // Need to stop in mid air if user holds down AGENT_CONTROL_STOP
-                    if (AgentControlStopActive)
-                    {
-                        agent_control_v3 = Vector3.Zero;
-                    }
-                    else
+                    //                    if (AgentControlStopActive)
+                    //                    {
+                    //                        agent_control_v3 = Vector3.Zero;
+                    //                    }
+                    //                    else
                     {
                         // Landing detection code
 
@@ -2298,38 +2291,44 @@ namespace OpenSim.Region.Framework.Scenes
                         bool controlland = (((flags & AgentManager.ControlFlags.AGENT_CONTROL_UP_NEG) != 0) ||
                                             ((flags & AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_UP_NEG) != 0));
 
-                       //m_log.Debug("[CONTROL]: " +flags);
+                        //m_log.Debug("[CONTROL]: " +flags);
                         // Applies a satisfying roll effect to the avatar when flying.
                         if ((flags & AgentManager.ControlFlags.AGENT_CONTROL_TURN_LEFT) != 0 && (flags & AgentManager.ControlFlags.AGENT_CONTROL_YAW_POS) != 0)
                         {
                             ApplyFlyingRoll(
-                                FLY_ROLL_RADIANS_PER_UPDATE, 
-                                (flags & AgentManager.ControlFlags.AGENT_CONTROL_UP_POS) != 0, 
+                                FLY_ROLL_RADIANS_PER_UPDATE,
+                                (flags & AgentManager.ControlFlags.AGENT_CONTROL_UP_POS) != 0,
                                 (flags & AgentManager.ControlFlags.AGENT_CONTROL_UP_NEG) != 0);
-                        } 
+                        }
                         else if ((flags & AgentManager.ControlFlags.AGENT_CONTROL_TURN_RIGHT) != 0 &&
                                  (flags & AgentManager.ControlFlags.AGENT_CONTROL_YAW_NEG) != 0)
                         {
                             ApplyFlyingRoll(
-                                -FLY_ROLL_RADIANS_PER_UPDATE, 
-                                (flags & AgentManager.ControlFlags.AGENT_CONTROL_UP_POS) != 0, 
-                                (flags & AgentManager.ControlFlags.AGENT_CONTROL_UP_NEG) != 0);                      
+                                -FLY_ROLL_RADIANS_PER_UPDATE,
+                                (flags & AgentManager.ControlFlags.AGENT_CONTROL_UP_POS) != 0,
+                                (flags & AgentManager.ControlFlags.AGENT_CONTROL_UP_NEG) != 0);
                         }
                         else
                         {
                             if (m_AngularVelocity.Z != 0)
-                                m_AngularVelocity.Z += CalculateFlyingRollResetToZero(FLY_ROLL_RESET_RADIANS_PER_UPDATE);                        
-                        }                  
-
-                        if (Flying && IsColliding && controlland)
-                        {
-                            // nesting this check because LengthSquared() is expensive and we don't 
-                            // want to do it every step when flying.
-                            if ((Velocity.LengthSquared() <= LAND_VELOCITYMAG_MAX))
-                                StopFlying();
+                                m_AngularVelocity.Z += CalculateFlyingRollResetToZero(FLY_ROLL_RESET_RADIANS_PER_UPDATE);
                         }
+                        
+                        /*
+                                                if (Flying && IsColliding && controlland)
+                                                {
+                                                    // nesting this check because LengthSquared() is expensive and we don't 
+                                                    // want to do it every step when flying.
+                                                    if ((Velocity.LengthSquared() <= LAND_VELOCITYMAG_MAX))
+                                                        StopFlying();
+                                                }
+                         */
                     }
                 }
+                else if (IsColliding && agent_control_v3.Z < 0f)
+                    agent_control_v3.Z = 0;
+//                else if(AgentControlStopActive %% Velocity.Z <0.01f)
+                    
 
 //                m_log.DebugFormat("[SCENE PRESENCE]: MovementFlag {0} for {1}", MovementFlag, Name);
 
@@ -2342,32 +2341,22 @@ namespace OpenSim.Region.Framework.Scenes
                 if (update_movementflag 
                     || (update_rotation && DCFlagKeyPressed && (!AgentControlStopActive || MovementFlag != 0)))
                 {
-//                    if (update_movementflag || !AgentControlStopActive || MovementFlag != 0)
-//                    {
-//                        m_log.DebugFormat(
-//                            "[SCENE PRESENCE]: In {0} adding velocity of {1} to {2}, umf = {3}, mf = {4}, ur = {5}",
-//                            m_scene.RegionInfo.RegionName, agent_control_v3, Name, 
-//                            update_movementflag, MovementFlag, update_rotation);
-
-                        float speedModifier;
 
-                        if (AgentControlStopActive)
-                            speedModifier = AgentControlStopSlowWhilstMoving;
+                    if (AgentControlStopActive)
+                    {
+//                        if (MovementFlag == 0 && Animator.Falling)
+                        if (MovementFlag == 0 && Animator.currentControlState == ScenePresenceAnimator.motionControlStates.falling)
+                        {
+                            AddNewMovement(agent_control_v3, AgentControlStopSlowWhilstMoving, true);
+                        }
                         else
-                            speedModifier = 1;
+                            AddNewMovement(agent_control_v3, AgentControlStopSlowWhilstMoving);
+                    }
+
+                    else
+                        AddNewMovement(agent_control_v3);
 
-                        AddNewMovement(agent_control_v3, speedModifier);
-//                    }
                 }
-//                else
-//                {
-//                    if (!update_movementflag)
-//                    {
-//                        m_log.DebugFormat(
-//                            "[SCENE PRESENCE]: In {0} ignoring requested update of {1} for {2} as update_movementflag = false",
-//                            m_scene.RegionInfo.RegionName, agent_control_v3, Name);
-//                    }
-//                }
 
                 if (update_movementflag && ParentID == 0)
                 {
@@ -3246,68 +3235,58 @@ namespace OpenSim.Region.Framework.Scenes
         /// <param name="vec">The vector in which to move.  This is relative to the rotation argument</param>
         /// <param name="thisAddSpeedModifier">
         /// Optional additional speed modifier for this particular add.  Default is 1</param>
-        public void AddNewMovement(Vector3 vec, float thisAddSpeedModifier = 1)
+        public void AddNewMovement(Vector3 vec, float thisAddSpeedModifier = 1, bool breaking = false)
         {
-//            m_log.DebugFormat(
-//                "[SCENE PRESENCE]: Adding new movement {0} with rotation {1}, thisAddSpeedModifier {2} for {3}", 
-//                vec, Rotation, thisAddSpeedModifier, Name);
+            //            m_log.DebugFormat(
+            //                "[SCENE PRESENCE]: Adding new movement {0} with rotation {1}, thisAddSpeedModifier {2} for {3}", 
+            //                vec, Rotation, thisAddSpeedModifier, Name);
 
             Vector3 direc = vec * Rotation;
             direc.Normalize();
 
-            if (Flying != FlyingOld)                // add for fly velocity control
-            {
-                FlyingOld = Flying;                 // add for fly velocity control
-                if (!Flying)
-                    WasFlying = true;      // add for fly velocity control
-            }
-
-            if (IsColliding)
-                WasFlying = false;        // add for fly velocity control
-
             if ((vec.Z == 0f) && !Flying)
                 direc.Z = 0f; // Prevent camera WASD up.
 
             direc *= 0.03f * 128f * SpeedModifier * thisAddSpeedModifier;
 
-//            m_log.DebugFormat("[SCENE PRESENCE]: Force to apply before modification was {0} for {1}", direc, Name);
+            //            m_log.DebugFormat("[SCENE PRESENCE]: Force to apply before modification was {0} for {1}", direc, Name);
 
-            if (PhysicsActor != null)
+            if (Animator.currentControlState == ScenePresenceAnimator.motionControlStates.falling)
             {
-                if (Flying)
-                {
+                if (breaking)
+                    direc.Z = -9999f; //hack
+                else
+                    direc = Vector3.Zero;
+            }
+            else if (Flying)
+            {
+                if(IsColliding)
+                    direc = Vector3.Zero;
+                else
                     direc *= 4.0f;
-                    //bool controlland = (((m_AgentControlFlags & (uint)AgentManager.ControlFlags.AGENT_CONTROL_UP_NEG) != 0) || ((m_AgentControlFlags & (uint)AgentManager.ControlFlags.AGENT_CONTROL_NUDGE_UP_NEG) != 0));
-                    //if (controlland)
-                    //    m_log.Info("[AGENT]: landCommand");
-                    //if (IsColliding)
-                    //    m_log.Info("[AGENT]: colliding");
-                    //if (Flying && IsColliding && controlland)
-                    //{
-                    //    StopFlying();
-                    //    m_log.Info("[AGENT]: Stop Flying");
-                    //}
-                }
-                if (Animator.Falling && WasFlying)    // if falling from flying, disable motion add
+            }
+            else if (IsColliding)
+            {
+                if (direc.Z > 2.0f)
                 {
-                    direc *= 0.0f;
+                    direc.Z *= 2.6f;
                 }
-                else if (!Flying && IsColliding)
+                else if (direc.Z < 0)
+                    direc.Z = 0;
+/*                
+                float c = CollisionPlane.Z;
+                if (c > 0.2f && c < 0.94f && (direc.X != 0 || direc.Y != 0))
                 {
-                    if (direc.Z > 2.0f)
-                    {
-                        direc.Z *= 2.6f;
-
-                        // TODO: PreJump and jump happen too quickly.  Many times prejump gets ignored.
-//                        Animator.TrySetMovementAnimation("PREJUMP");
-//                        Animator.TrySetMovementAnimation("JUMP");
-                    }
+                    float p = direc.X * CollisionPlane.X + direc.Y * CollisionPlane.Y;
+                    direc.X -= p * CollisionPlane.X;
+                    direc.Y -= p * CollisionPlane.Y;
+                    direc.Z -= p * c;
                 }
+ */
             }
 
-//            m_log.DebugFormat("[SCENE PRESENCE]: Setting force to apply to {0} for {1}", direc, Name);
+            //            m_log.DebugFormat("[SCENE PRESENCE]: Setting force to apply to {0} for {1}", direc, Name);
 
-            // TODO: Add the force instead of only setting it to support multiple forces per frame?
             m_forceToApply = direc;
             Animator.UpdateMovementAnimations();
         }