8 files changed, 74 insertions, 79 deletions

diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSActorAvatarMove.cs b/OpenSim/Region/PhysicsModules/BulletS/BSActorAvatarMove.cs
index 12ffacb..79ee00f 100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSActorAvatarMove.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSActorAvatarMove.cs
@@ -47,10 +47,9 @@ public class BSActorAvatarMove : BSActor
     // The amount the step up is applying. Used to smooth stair walking.
     float m_lastStepUp;
 
-    // Jumping happens over several frames. If use applies up force while colliding, start the
-    //    jump and allow the jump to continue for this number of frames.
-    int m_jumpFrames = 0;
-    float m_jumpVelocity = 0f;
+    // There are times the velocity is set but we don't want to inforce stationary until the
+    //    real velocity drops.
+    bool m_waitingForLowVelocityForStationary = false;
 
     public BSActorAvatarMove(BSScene physicsScene, BSPhysObject pObj, string actorName)
         : base(physicsScene, pObj, actorName)
@@ -117,11 +116,19 @@ public class BSActorAvatarMove : BSActor
                 m_velocityMotor.SetTarget(targ);
                 m_velocityMotor.SetCurrent(vel);
                 m_velocityMotor.Enabled = true;
+                m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,SetVelocityAndTarget,vel={1}, targ={2}",
+                            m_controllingPrim.LocalID, vel, targ);
+                m_waitingForLowVelocityForStationary = false;
             }
         });
     }
 
-    // If a hover motor has not been created, create one and start the hovering.
+    public void SuppressStationayCheckUntilLowVelocity()
+    {
+        m_waitingForLowVelocityForStationary = true;
+    }
+
+    // If a movement motor has not been created, create one and start the hovering.
     private void ActivateAvatarMove()
     {
         if (m_velocityMotor == null)
@@ -133,13 +140,14 @@ public class BSActorAvatarMove : BSActor
                                                 1f                          // efficiency
             );
             m_velocityMotor.ErrorZeroThreshold = BSParam.AvatarStopZeroThreshold;
-            // _velocityMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG so motor will output detail log messages.
+            // m_velocityMotor.PhysicsScene = m_controllingPrim.PhysScene; // DEBUG DEBUG so motor will output detail log messages.
             SetVelocityAndTarget(m_controllingPrim.RawVelocity, m_controllingPrim.TargetVelocity, true /* inTaintTime */);
 
             m_physicsScene.BeforeStep += Mover;
             m_controllingPrim.OnPreUpdateProperty += Process_OnPreUpdateProperty;
 
             m_walkingUpStairs = 0;
+            m_waitingForLowVelocityForStationary = false;
         }
     }
 
@@ -190,12 +198,15 @@ public class BSActorAvatarMove : BSActor
                 // if colliding with something stationary and we're not doing volume detect .
                 if (!m_controllingPrim.ColliderIsMoving && !m_controllingPrim.ColliderIsVolumeDetect)
                 {
-                    m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,collidingWithStationary,totalForce={1}, vel={2}",   /* DEBUG */
-                            m_controllingPrim.LocalID, m_physicsScene.PE.GetTotalForce(m_controllingPrim.PhysBody), m_controllingPrim.Velocity);    /* DEBUG */
-                    // If velocity is very small, assume it is movement creep and suppress it.
-                    // Applying push forces (Character.AddForce) should move the avatar and that is only seen here as velocity.
-                    if ( (m_controllingPrim.Velocity.LengthSquared() < BSParam.AvatarStopZeroThresholdSquared)
-                            && (m_physicsScene.PE.GetTotalForce(m_controllingPrim.PhysBody).LengthSquared() < BSParam.AvatarStopZeroThresholdSquared) )
+                    if (m_waitingForLowVelocityForStationary)
+                    {
+                        // if waiting for velocity to drop and it has finally dropped, we can be stationary
+                        if (m_controllingPrim.RawVelocity.LengthSquared() < BSParam.AvatarStopZeroThresholdSquared)
+                        {
+                            m_waitingForLowVelocityForStationary = false;
+                        }
+                    }
+                    if (!m_waitingForLowVelocityForStationary)
                     {
                         m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,collidingWithStationary,zeroingMotion", m_controllingPrim.LocalID);
                         m_controllingPrim.IsStationary = true;
@@ -203,8 +214,8 @@ public class BSActorAvatarMove : BSActor
                     }
                     else
                     {
-                        m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,collidingWithStationary,not zeroing because velocity={1}",
-                                        m_controllingPrim.LocalID, m_controllingPrim.Velocity);
+                        m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,waitingForLowVel,rawvel={1}",
+                                    m_controllingPrim.LocalID, m_controllingPrim.RawVelocity.Length());
                     }
                 }
 
@@ -250,50 +261,24 @@ public class BSActorAvatarMove : BSActor
                 m_physicsScene.PE.SetFriction(m_controllingPrim.PhysBody, m_controllingPrim.Friction);
             }
 
-            // If not flying and not colliding, assume falling and keep the downward motion component.
-            //    This check is done here for the next jump test.
-            if (!m_controllingPrim.Flying && !m_controllingPrim.IsColliding)
-            {
-                stepVelocity.Z = m_controllingPrim.RawVelocity.Z;
-            }
+            // 'm_velocityMotor is used for walking, flying, and jumping and will thus have the correct values
+            //    for Z. But in come cases it must be over-ridden. Like when falling or jumping.
 
-            // Colliding and not flying with an upward force. The avatar must be trying to jump.
-            if (!m_controllingPrim.Flying && m_controllingPrim.IsColliding && stepVelocity.Z > 0)
-            {
-                // We allow the upward force to happen for this many frames.
-                m_jumpFrames = BSParam.AvatarJumpFrames;
-                m_jumpVelocity = stepVelocity.Z;
-            }
+            float realVelocityZ = m_controllingPrim.RawVelocity.Z;
 
-            // The case where the avatar is not colliding and is not flying is special.
-            // The avatar is either falling or jumping and the user can be applying force to the avatar
-            //     (force in some direction or force up or down).
-            // If the avatar has negative Z velocity and is not colliding, presume we're falling and keep the velocity.
-            // If the user is trying to apply upward force but we're not colliding, assume the avatar
-            //     is trying to jump and don't apply the upward force if not touching the ground any more.
-            if (!m_controllingPrim.Flying && !m_controllingPrim.IsColliding)
+            // If not flying and falling, we over-ride the stepping motor so we can fall to the ground
+            if (!m_controllingPrim.Flying && realVelocityZ < 0)
             {
-                // If upward velocity is being applied, this must be a jump and only allow that to go on so long
-                if (m_jumpFrames > 0)
+                // Can't fall faster than this
+                if (realVelocityZ < BSParam.AvatarTerminalVelocity)
                 {
-                    // Since not touching the ground, only apply upward force for so long.
-                    m_jumpFrames--;
-                    stepVelocity.Z = m_jumpVelocity;
+                    realVelocityZ = BSParam.AvatarTerminalVelocity;
                 }
-                else
-                {
-                    // Since we're not affected by anything, the avatar must be falling and we do not want that to be too fast.
-                    if (m_controllingPrim.RawVelocity.Z < BSParam.AvatarTerminalVelocity)
-                    {
-                        stepVelocity.Z = BSParam.AvatarTerminalVelocity;
-                    }
-                    else
-                    {
-                        stepVelocity.Z = m_controllingPrim.RawVelocity.Z;
-                    }
-                }
-                // DetailLog("{0},BSCharacter.MoveMotor,taint,overrideStepZWithWorldZ,stepVel={1}", LocalID, stepVelocity);
+
+                stepVelocity.Z = realVelocityZ;
             }
+            // m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,DEBUG,motorCurrent={1},realZ={2},flying={3},collid={4},jFrames={5}",
+            //     m_controllingPrim.LocalID, m_velocityMotor.CurrentValue, realVelocityZ, m_controllingPrim.Flying, m_controllingPrim.IsColliding, m_jumpFrames);
 
             //Alicia: Maintain minimum height when flying.
             // SL has a flying effect that keeps the avatar flying above the ground by some margin
@@ -304,6 +289,8 @@ public class BSActorAvatarMove : BSActor
 
                 if( m_controllingPrim.Position.Z < hover_height)
                 {
+                    m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,addingUpforceForGroundMargin,height={1},hoverHeight={2}",
+                                m_controllingPrim.LocalID, m_controllingPrim.Position.Z, hover_height);
                     stepVelocity.Z += BSParam.AvatarFlyingGroundUpForce;
                 }
             }
@@ -328,11 +315,7 @@ public class BSActorAvatarMove : BSActor
         if (m_controllingPrim.IsStationary)
         {
             entprop.Position = m_controllingPrim.RawPosition;
-            // Suppress small movement velocity
-            if (entprop.Velocity.LengthSquared() < BSParam.AvatarStopZeroThresholdSquared) {
-                m_physicsScene.DetailLog("{0},BSCharacter.MoveMotor,OnPreUpdate,zeroing velocity={1}", m_controllingPrim.LocalID, entprop.Velocity);
-                entprop.Velocity = OMV.Vector3.Zero;
-            }
+            entprop.Velocity = OMV.Vector3.Zero;
             m_physicsScene.PE.SetTranslation(m_controllingPrim.PhysBody, entprop.Position, entprop.Rotation);
         }
 
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSActorMoveToTarget.cs b/OpenSim/Region/PhysicsModules/BulletS/BSActorMoveToTarget.cs
index 3db8f2c..87cf972 100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSActorMoveToTarget.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSActorMoveToTarget.cs
@@ -211,7 +211,7 @@ public class BSActorMoveToTarget : BSActor
             // Add enough force to overcome the mass of the object
             addedForce *= m_controllingPrim.Mass;
 
-            m_controllingPrim.AddForce(addedForce, false /* pushForce */, true /* inTaintTime */);
+            m_controllingPrim.AddForce(true /* inTaintTime */, addedForce);
         }
         m_physicsScene.DetailLog("{0},BSActorMoveToTarget.Mover3,move,fromPos={1},addedForce={2}",
                                         m_controllingPrim.LocalID, origPosition, addedForce);
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSActorSetTorque.cs b/OpenSim/Region/PhysicsModules/BulletS/BSActorSetTorque.cs
index a1cf4db..0261bcb 100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSActorSetTorque.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSActorSetTorque.cs
@@ -127,7 +127,7 @@ public class BSActorSetTorque : BSActor
         m_physicsScene.DetailLog("{0},BSActorSetTorque,preStep,force={1}", m_controllingPrim.LocalID, m_controllingPrim.RawTorque);
         if (m_controllingPrim.PhysBody.HasPhysicalBody)
         {
-            m_controllingPrim.AddAngularForce(m_controllingPrim.RawTorque, false, true);
+            m_controllingPrim.AddAngularForce(true /* inTaintTime */, m_controllingPrim.RawTorque);
             m_controllingPrim.ActivateIfPhysical(false);
         }
 
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSCharacter.cs b/OpenSim/Region/PhysicsModules/BulletS/BSCharacter.cs
index ab9cc27..6d5589f 100644
--- a/OpenSim/Region/PhysicsModules/BulletS/BSCharacter.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSCharacter.cs
@@ -89,6 +89,7 @@ public sealed class BSCharacter : BSPhysObject
         _buoyancy = ComputeBuoyancyFromFlying(isFlying);
         Friction = BSParam.AvatarStandingFriction;
         Density = BSParam.AvatarDensity;
+        _isPhysical = true;
 
         // Old versions of ScenePresence passed only the height. If width and/or depth are zero,
         //     replace with the default values.
@@ -457,7 +458,7 @@ public sealed class BSCharacter : BSPhysObject
         get { return RawVelocity; }
         set {
             RawVelocity = value;
-                OMV.Vector3 vel = RawVelocity;
+            OMV.Vector3 vel = RawVelocity;
 
             DetailLog("{0}: set Velocity = {1}", LocalID, value);
 
@@ -662,10 +663,10 @@ public sealed class BSCharacter : BSPhysObject
         addForce *= Mass * BSParam.AvatarAddForcePushFactor;
 
         DetailLog("{0},BSCharacter.addForce,call,force={1},addForce={2},push={3},mass={4}", LocalID, force, addForce, pushforce, Mass);
-        AddForce(addForce, pushforce, false);
+        AddForce(false, addForce);
     }
 
-    public override void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) {
+    public override void AddForce(bool inTaintTime, OMV.Vector3 force) {
         if (force.IsFinite())
         {
             OMV.Vector3 addForce = Util.ClampV(force, BSParam.MaxAddForceMagnitude);
@@ -683,6 +684,10 @@ public sealed class BSCharacter : BSPhysObject
                     PhysScene.PE.ApplyCentralForce(PhysBody, addForce);
                     PhysScene.PE.Activate(PhysBody, true);
                 }
+                if (m_moveActor != null)
+                {
+                    m_moveActor.SuppressStationayCheckUntilLowVelocity();
+                }
             });
         }
         else
@@ -692,7 +697,7 @@ public sealed class BSCharacter : BSPhysObject
         }
     }
 
-    public override void AddAngularForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) {
+    public override void AddAngularForce(bool inTaintTime, OMV.Vector3 force) {
     }
     public override void SetMomentum(OMV.Vector3 momentum) {
     }
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSDynamics.cs b/OpenSim/Region/PhysicsModules/BulletS/BSDynamics.cs
index 0fc5577..313c961 100644
--- a/OpenSim/Region/PhysicsModules/BulletS/BSDynamics.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSDynamics.cs
@@ -768,7 +768,7 @@ namespace OpenSim.Region.PhysicsModule.BulletS
                 }
 
                 if ((m_knownChanged & m_knownChangedForce) != 0)
-                    ControllingPrim.AddForce((Vector3)m_knownForce, false /*pushForce*/, true /*inTaintTime*/);
+                    ControllingPrim.AddForce(false /* inTaintTime */, (Vector3)m_knownForce);
 
                 if ((m_knownChanged & m_knownChangedForceImpulse) != 0)
                     ControllingPrim.AddForceImpulse((Vector3)m_knownForceImpulse, false /*pushforce*/, true /*inTaintTime*/);
@@ -784,7 +784,7 @@ namespace OpenSim.Region.PhysicsModule.BulletS
 
                 if ((m_knownChanged & m_knownChangedRotationalForce) != 0)
                 {
-                    ControllingPrim.AddAngularForce((Vector3)m_knownRotationalForce, false /*pushForce*/, true /*inTaintTime*/);
+                    ControllingPrim.AddAngularForce(true /* inTaintTime */, (Vector3)m_knownRotationalForce);
                 }
 
                 // If we set one of the values (ie, the physics engine didn't do it) we must force
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSParam.cs b/OpenSim/Region/PhysicsModules/BulletS/BSParam.cs
index aea69d7..389a441 100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSParam.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSParam.cs
@@ -633,7 +633,7 @@ public static class BSParam
             new ParameterDefn<float>("AvatarAddForcePushFactor", "BSCharacter.AddForce is multiplied by this and mass to be like other physics engines",
             0.315f ),
             new ParameterDefn<float>("AvatarStopZeroThreshold", "Movement velocity below which avatar is assumed to be stopped",
-            0.1f,
+            0.4f,
             (s) => { return (float)AvatarStopZeroThreshold; },
             (s,v) => { AvatarStopZeroThreshold = v; AvatarStopZeroThresholdSquared = v * v; } ),
             new ParameterDefn<float>("AvatarBelowGroundUpCorrectionMeters", "Meters to move avatar up if it seems to be below ground",
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSPhysObject.cs b/OpenSim/Region/PhysicsModules/BulletS/BSPhysObject.cs
index b50e4cc..a70d1b8 100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSPhysObject.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSPhysObject.cs
@@ -245,10 +245,10 @@ public abstract class BSPhysObject : PhysicsActor
 
     public override void AddAngularForce(OMV.Vector3 force, bool pushforce)
     {
-        AddAngularForce(force, pushforce, false);
+        AddAngularForce(false, force);
     }
-    public abstract void AddAngularForce(OMV.Vector3 force, bool pushforce, bool inTaintTime);
-    public abstract void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime);
+    public abstract void AddAngularForce(bool inTaintTime, OMV.Vector3 force);
+    public abstract void AddForce(bool inTaintTime, OMV.Vector3 force);
 
     public abstract OMV.Vector3 ForceRotationalVelocity { get; set; }
 
@@ -505,17 +505,20 @@ public abstract class BSPhysObject : PhysicsActor
             // Collision sound requires a velocity to know it should happen. This is a lot of computation for a little used feature.
             OMV.Vector3 relvel = OMV.Vector3.Zero;
             if (IsPhysical)
-                relvel = Velocity;
+                relvel = RawVelocity;
             if (collidee != null && collidee.IsPhysical)
-                relvel -= collidee.Velocity;
+                relvel -= collidee.RawVelocity;
             newContact.RelativeSpeed = OMV.Vector3.Dot(relvel, contactNormal);
+            // DetailLog("{0},{1}.Collision.AddCollider,vel={2},contee.vel={3},relvel={4},relspeed={5}",
+            //         LocalID, TypeName, RawVelocity, (collidee == null ? OMV.Vector3.Zero : collidee.RawVelocity), relvel, newContact.RelativeSpeed);
                     
             lock (PhysScene.CollisionLock)
             {
                 CollisionCollection.AddCollider(collideeLocalID, newContact);
             }
-            DetailLog("{0},{1}.Collision.AddCollider,call,with={2},point={3},normal={4},depth={5},colliderMoving={6}",
-                            LocalID, TypeName, collideeLocalID, contactPoint, contactNormal, pentrationDepth, ColliderIsMoving);
+            DetailLog("{0},{1}.Collision.AddCollider,call,with={2},point={3},normal={4},depth={5},speed={6},colliderMoving={7}",
+                            LocalID, TypeName, collideeLocalID, contactPoint, contactNormal, pentrationDepth,
+                            newContact.RelativeSpeed, ColliderIsMoving);
 
             ret = true;
         }
diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSPrim.cs b/OpenSim/Region/PhysicsModules/BulletS/BSPrim.cs
index 1d552eb..fd9b834 100644
--- a/OpenSim/Region/PhysicsModules/BulletS/BSPrim.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSPrim.cs
@@ -394,7 +394,7 @@ public class BSPrim : BSPhysObject
                 // Apply upforce and overcome gravity.
                 OMV.Vector3 correctionForce = upForce - PhysScene.DefaultGravity;
                 DetailLog("{0},BSPrim.PositionSanityCheck,applyForce,pos={1},upForce={2},correctionForce={3}", LocalID, RawPosition, upForce, correctionForce);
-                AddForce(correctionForce, false, inTaintTime);
+                AddForce(inTaintTime, correctionForce);
                 ret = true;
             }
         }
@@ -1249,14 +1249,18 @@ public class BSPrim : BSPhysObject
         // Per documentation, max force is limited.
         OMV.Vector3 addForce = Util.ClampV(force, BSParam.MaxAddForceMagnitude);
 
-        // Since this force is being applied in only one step, make this a force per second.
-        addForce /= PhysScene.LastTimeStep;
-        AddForce(addForce, pushforce, false /* inTaintTime */);
+        // Push forces seem to be scaled differently (follow pattern in ubODE)
+        if (!pushforce) {
+            // Since this force is being applied in only one step, make this a force per second.
+            addForce /= PhysScene.LastTimeStep;
+        }
+
+        AddForce(false /* inTaintTime */, addForce);
     }
 
     // Applying a force just adds this to the total force on the object.
     // This added force will only last the next simulation tick.
-    public override void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) {
+    public override void AddForce(bool inTaintTime, OMV.Vector3 force) {
         // for an object, doesn't matter if force is a pushforce or not
         if (IsPhysicallyActive)
         {
@@ -1315,7 +1319,7 @@ public class BSPrim : BSPhysObject
     }
 
     // BSPhysObject.AddAngularForce()
-    public override void AddAngularForce(OMV.Vector3 force, bool pushforce, bool inTaintTime)
+    public override void AddAngularForce(bool inTaintTime, OMV.Vector3 force)
     {
         if (force.IsFinite())
         {