BulletSim: add function to push avatar up when hitting stairs.

It looks like BulletSim and ODE rely on penetration correction to cause
the avatar to move up and thus allowing walking up stairs. Object
penetration was minimized for walking and flying (so one doesn't go
through walls) and this stopped stairs from working. This commit
introduces avatar movement code to check for collisions at the
feet while walking and attempts to raise the avatar for the steps.
Not yet perfect but movement is better.

5 files changed, 119 insertions, 27 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index fe48166..939d38a 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -184,10 +184,6 @@ public sealed class BSCharacter : BSPhysObject
     //    standing as well as moving. Destruction of the avatar will destroy the pre-step action.
     private void SetupMovementMotor()
     {
-
-        // Someday, use a PID motor for asymmetric speed up and slow down
-        // _velocityMotor = new BSPIDVMotor("BSCharacter.Velocity", 3f, 5f, BSMotor.InfiniteVector, 1f);
-
         // Infinite decay and timescale values so motor only changes current to target values.
         _velocityMotor = new BSVMotor("BSCharacter.Velocity", 
                                             0.2f,                       // time scale
@@ -214,25 +210,68 @@ public sealed class BSCharacter : BSPhysObject
             // 'stepVelocity' is now the speed we'd like the avatar to move in. Turn that into an instantanous force.
             OMV.Vector3 moveForce = (stepVelocity - _velocity) * Mass;
 
-            /*
-            // If moveForce is very small, zero things so we don't keep sending microscopic updates to the user
-            float moveForceMagnitudeSquared = moveForce.LengthSquared();
-            if (moveForceMagnitudeSquared < 0.0001)
-            {
-                DetailLog("{0},BSCharacter.MoveMotor,zeroMovement,stepVel={1},vel={2},mass={3},magSq={4},moveForce={5}",
-                                        LocalID, stepVelocity, _velocity, Mass, moveForceMagnitudeSquared, moveForce);
-                ForceVelocity = OMV.Vector3.Zero;
-            }
-            else
-            {
-                AddForce(moveForce, false, true);
-            }
-            */
-            // DetailLog("{0},BSCharacter.MoveMotor,move,stepVel={1},vel={2},mass={3},moveForce={4}", LocalID, stepVelocity, _velocity, Mass, moveForce);
+            // Should we check for move force being small and forcing velocity to zero?
+
+            // Add special movement force to allow avatars to walk up stepped surfaces.
+            moveForce += WalkUpStairs();
+
+            DetailLog("{0},BSCharacter.MoveMotor,move,stepVel={1},vel={2},mass={3},moveForce={4}", LocalID, stepVelocity, _velocity, Mass, moveForce);
             PhysicsScene.PE.ApplyCentralImpulse(PhysBody, moveForce);
         });
     }
 
+    // Decide of the character is colliding with a low object and compute a force to pop the
+    //    avatar up so it has a chance of walking up and over the low object.
+    private OMV.Vector3 WalkUpStairs()
+    {
+        OMV.Vector3 ret = OMV.Vector3.Zero;
+
+        // This test is done if moving forward, not flying and is colliding with something.
+        // DetailLog("{0},BSCharacter.WalkUpStairs,IsColliding={1},flying={2},targSpeed={3},collisions={4}",
+        //                 LocalID, IsColliding, Flying, TargetSpeed, CollisionsLastTick.Count);
+        if (IsColliding && !Flying && TargetSpeed > 0.1f /* && ForwardSpeed < 0.1f */)
+        {
+            // The range near the character's feet where we will consider stairs
+            float nearFeetHeightMin = RawPosition.Z - (Size.Z / 2f) + 0.05f;
+            float nearFeetHeightMax = nearFeetHeightMin + BSParam.AvatarStepHeight;
+
+            // Look for a collision point that is near the character's feet and is oriented the same as the charactor is
+            foreach (KeyValuePair<uint, ContactPoint> kvp in CollisionsLastTick.m_objCollisionList)
+            {
+                // Don't care about collisions with the terrain
+                if (kvp.Key > PhysicsScene.TerrainManager.HighestTerrainID)
+                {
+                    OMV.Vector3 touchPosition = kvp.Value.Position;
+                    // DetailLog("{0},BSCharacter.WalkUpStairs,min={1},max={2},touch={3}",
+                    //                 LocalID, nearFeetHeightMin, nearFeetHeightMax, touchPosition);
+                    if (touchPosition.Z >= nearFeetHeightMin && touchPosition.Z <= nearFeetHeightMax)
+                    {
+                        // This contact is within the 'near the feet' range.
+                        // The normal should be our contact point to the object so it is pointing away
+                        //    thus the difference between our facing orientation and the normal should be small.
+                        OMV.Vector3 directionFacing = OMV.Vector3.UnitX * RawOrientation;
+                        OMV.Vector3 touchNormal = OMV.Vector3.Normalize(kvp.Value.SurfaceNormal);
+                        float diff = Math.Abs(OMV.Vector3.Distance(directionFacing, touchNormal));
+                        if (diff < BSParam.AvatarStepApproachFactor)
+                        {
+                            // Found the stairs contact point. Push up a little to raise the character.
+                            float upForce = (touchPosition.Z - nearFeetHeightMin) * Mass * BSParam.AvatarStepForceFactor;
+                            ret = new OMV.Vector3(0f, 0f, upForce);
+
+                            // Also move the avatar up for the new height
+                            OMV.Vector3 displacement = new OMV.Vector3(0f, 0f, BSParam.AvatarStepHeight / 2f);
+                            ForcePosition = RawPosition + displacement;
+                        }
+                        DetailLog("{0},BSCharacter.WalkUpStairs,touchPos={1},nearFeetMin={2},faceDir={3},norm={4},diff={5},ret={6}",
+                                LocalID, touchPosition, nearFeetHeightMin, directionFacing, touchNormal, diff, ret);
+                    }
+                }
+            }
+        }
+
+        return ret;
+    }
+
     public override void RequestPhysicsterseUpdate()
     {
         base.RequestPhysicsterseUpdate();
@@ -342,13 +381,11 @@ public sealed class BSCharacter : BSPhysObject
         }
         set {
             _position = value;
-            PositionSanityCheck();
 
             PhysicsScene.TaintedObject("BSCharacter.setPosition", delegate()
             {
                 DetailLog("{0},BSCharacter.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                if (PhysBody.HasPhysicalBody)
-                    PhysicsScene.PE.SetTranslation(PhysBody, _position, _orientation);
+                ForcePosition = _position;
             });
         }
     }
@@ -359,8 +396,11 @@ public sealed class BSCharacter : BSPhysObject
         }
         set {
             _position = value;
-            PositionSanityCheck();
-            PhysicsScene.PE.SetTranslation(PhysBody, _position, _orientation);
+            if (PhysBody.HasPhysicalBody)
+            {
+                PositionSanityCheck();
+                PhysicsScene.PE.SetTranslation(PhysBody, _position, _orientation);
+            }
         }
     }
 
@@ -373,7 +413,7 @@ public sealed class BSCharacter : BSPhysObject
         bool ret = false;
 
         // TODO: check for out of bounds
-        if (!PhysicsScene.TerrainManager.IsWithinKnownTerrain(_position))
+        if (!PhysicsScene.TerrainManager.IsWithinKnownTerrain(RawPosition))
         {
             // The character is out of the known/simulated area.
             // Upper levels of code will handle the transition to other areas so, for
@@ -382,7 +422,7 @@ public sealed class BSCharacter : BSPhysObject
         }
 
         // If below the ground, move the avatar up
-        float terrainHeight = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(_position);
+        float terrainHeight = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(RawPosition);
         if (Position.Z < terrainHeight)
         {
             DetailLog("{0},BSCharacter.PositionAdjustUnderGround,call,pos={1},terrain={2}", LocalID, _position, terrainHeight);
@@ -485,6 +525,11 @@ public sealed class BSCharacter : BSPhysObject
             });
         }
     }
+    public override OMV.Vector3 RawVelocity
+    {
+        get { return _velocity; }
+        set { _velocity = value; }
+    }
     // Directly setting velocity means this is what the user really wants now.
     public override OMV.Vector3 Velocity {
         get { return _velocity; }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
index b9bd0bf..23d573f 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
@@ -75,6 +75,9 @@ public static class BSParam
     public static float AvatarCapsuleDepth { get; private set; }
     public static float AvatarCapsuleHeight { get; private set; }
         public static float AvatarContactProcessingThreshold { get; private set; }
+        public static float AvatarStepHeight { get; private set; }
+        public static float AvatarStepApproachFactor { get; private set; }
+        public static float AvatarStepForceFactor { get; private set; }
 
     public static float VehicleAngularDamping { get; private set; }
 
@@ -403,6 +406,21 @@ public static class BSParam
             (s,cf,p,v) => { AvatarContactProcessingThreshold = cf.GetFloat(p, v); },
             (s) => { return AvatarContactProcessingThreshold; },
             (s,p,l,v) => { s.UpdateParameterObject((x)=>{AvatarContactProcessingThreshold=x;}, p, l, v); } ),
+            new ParameterDefn("AvatarStepHeight", "Height of a step obstacle to consider step correction",
+            0.3f,
+            (s,cf,p,v) => { AvatarStepHeight = cf.GetFloat(p, v); },
+            (s) => { return AvatarStepHeight; },
+            (s,p,l,v) => { AvatarStepHeight = v; } ),
+            new ParameterDefn("AvatarStepApproachFactor", "Factor to control angle of approach to step (0=straight on)",
+            0.6f,
+            (s,cf,p,v) => { AvatarStepApproachFactor = cf.GetFloat(p, v); },
+            (s) => { return AvatarStepApproachFactor; },
+            (s,p,l,v) => { AvatarStepApproachFactor = v; } ),
+            new ParameterDefn("AvatarStepForceFactor", "Controls the amount of force up applied to step up onto a step",
+            2.0f,
+            (s,cf,p,v) => { AvatarStepForceFactor = cf.GetFloat(p, v); },
+            (s) => { return AvatarStepForceFactor; },
+            (s,p,l,v) => { AvatarStepForceFactor = v; } ),
 
         new ParameterDefn("VehicleAngularDamping", "Factor to damp vehicle angular movement per second (0.0 - 1.0)",
             0.95f,
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index 534f929..e8575f6 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -79,6 +79,7 @@ public abstract class BSPhysObject : PhysicsActor
         Material = MaterialAttributes.Material.Wood;
 
         CollisionCollection = new CollisionEventUpdate();
+        CollisionsLastTick = CollisionCollection;
         SubscribedEventsMs = 0;
         CollidingStep = 0;
         CollidingGroundStep = 0;
@@ -159,6 +160,7 @@ public abstract class BSPhysObject : PhysicsActor
     public abstract OMV.Quaternion ForceOrientation { get; set; }
 
     // The system is telling us the velocity it wants to move at.
+    // Velocity in world coordinates.
     // protected OMV.Vector3 m_targetVelocity;  // use the definition in PhysicsActor
     public override OMV.Vector3 TargetVelocity
     {
@@ -169,6 +171,15 @@ public abstract class BSPhysObject : PhysicsActor
             Velocity = value;
         }
     }
+    public virtual float TargetSpeed
+    {
+        get
+        {
+            OMV.Vector3 characterOrientedVelocity = TargetVelocity * OMV.Quaternion.Inverse(OMV.Quaternion.Normalize(RawOrientation));
+            return characterOrientedVelocity.X;
+        }
+    }
+    public abstract OMV.Vector3 RawVelocity { get; set; }
     public abstract OMV.Vector3 ForceVelocity { get; set; }
 
     public abstract OMV.Vector3 ForceRotationalVelocity { get; set; }
@@ -177,6 +188,15 @@ public abstract class BSPhysObject : PhysicsActor
 
     public virtual bool ForceBodyShapeRebuild(bool inTaintTime) { return false; }
 
+    public virtual float ForwardSpeed
+    {
+        get
+        {
+            OMV.Vector3 characterOrientedVelocity = RawVelocity * OMV.Quaternion.Inverse(OMV.Quaternion.Normalize(RawOrientation));
+            return characterOrientedVelocity.X;
+        }
+    }
+
     #region Collisions
 
     // Requested number of milliseconds between collision events. Zero means disabled.
@@ -223,9 +243,13 @@ public abstract class BSPhysObject : PhysicsActor
 
     // The collisions that have been collected this tick
     protected CollisionEventUpdate CollisionCollection;
+    // Remember collisions from last tick for fancy collision based actions
+    //     (like a BSCharacter walking up stairs).
+    protected CollisionEventUpdate CollisionsLastTick;
 
     // The simulation step is telling this object about a collision.
     // Return 'true' if a collision was processed and should be sent up.
+    // Return 'false' if this object is not enabled/subscribed/appropriate for or has already seen this collision.
     // Called at taint time from within the Step() function
     public virtual bool Collide(uint collidingWith, BSPhysObject collidee,
                     OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth)
@@ -286,6 +310,9 @@ public abstract class BSPhysObject : PhysicsActor
             // DetailLog("{0},{1}.SendCollisionUpdate,call,numCollisions={2}", LocalID, TypeName, CollisionCollection.Count);
             base.SendCollisionUpdate(CollisionCollection);
 
+            // Remember the collisions from this tick for some collision specific processing.
+            CollisionsLastTick = CollisionCollection;
+
             // The CollisionCollection instance is passed around in the simulator.
             // Make sure we don't have a handle to that one and that a new one is used for next time.
             //    This fixes an interesting 'gotcha'. If we call CollisionCollection.Clear() here, 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 94b63e5..400d5d6 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -562,7 +562,7 @@ public sealed class BSPrim : BSPhysObject
         }
         return;
     }
-    public OMV.Vector3 RawVelocity
+    public override OMV.Vector3 RawVelocity
     {
         get { return _velocity; }
         set { _velocity = value; }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
index 7b59e60..794a6af 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
@@ -88,6 +88,8 @@ setForce should set a constant force. Different than AddImpulse.
 Implement raycast.
 Implement ShapeCollection.Dispose()
 Implement water as a plain so raycasting and collisions can happen with same.
+Add collision penetration return
+        Add field passed back by BulletSim.dll and fill with info in ManifoldConstact.GetDistance()
 Add osGetPhysicsEngineName() so scripters can tell whether BulletSim or ODE
         Also osGetPhysicsEngineVerion() maybe.
 Linkset.Position and Linkset.Orientation requre rewrite to properly return