BulletSim: improve avatar stair walking up. Add more parameters to control force

of both position change and up force that move avatars over barrier.
Default parameters are for steps up to 0.5m in height.

5 files changed, 123 insertions, 25 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSActorAvatarMove.cs b/OpenSim/Region/Physics/BulletSPlugin/BSActorAvatarMove.cs
index 8416740..bd5ee0b1 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSActorAvatarMove.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSActorAvatarMove.cs
@@ -40,10 +40,16 @@ public class BSActorAvatarMove : BSActor
 {
     BSVMotor m_velocityMotor;
 
+    // Set to true if we think we're going up stairs.
+    //    This state is remembered because collisions will turn on and off as we go up stairs.
+    int m_walkingUpStairs;
+    float m_lastStepUp;
+
     public BSActorAvatarMove(BSScene physicsScene, BSPhysObject pObj, string actorName)
         : base(physicsScene, pObj, actorName)
     {
         m_velocityMotor = null;
+        m_walkingUpStairs = 0;
         m_physicsScene.DetailLog("{0},BSActorAvatarMove,constructor", m_controllingPrim.LocalID);
     }
 
@@ -119,6 +125,8 @@ public class BSActorAvatarMove : BSActor
             SetVelocityAndTarget(m_controllingPrim.RawVelocity, m_controllingPrim.TargetVelocity, true /* inTaintTime */);
 
             m_physicsScene.BeforeStep += Mover;
+
+            m_walkingUpStairs = 0;
         }
     }
 
@@ -216,8 +224,6 @@ public class BSActorAvatarMove : BSActor
             // 'stepVelocity' is now the speed we'd like the avatar to move in. Turn that into an instantanous force.
             OMV.Vector3 moveForce = (stepVelocity - m_controllingPrim.RawVelocity) * m_controllingPrim.Mass;
 
-            // 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();
 
@@ -233,24 +239,33 @@ public class BSActorAvatarMove : BSActor
     {
         OMV.Vector3 ret = OMV.Vector3.Zero;
 
+        m_physicsScene.DetailLog("{0},BSCharacter.WalkUpStairs,IsColliding={1},flying={2},targSpeed={3},collisions={4},avHeight={5}",
+                        m_controllingPrim.LocalID, m_controllingPrim.IsColliding, m_controllingPrim.Flying,
+                        m_controllingPrim.TargetVelocitySpeed, m_controllingPrim.CollisionsLastTick.Count, m_controllingPrim.Size.Z);
         // 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 (m_controllingPrim.IsColliding && !m_controllingPrim.Flying && m_controllingPrim.TargetVelocitySpeed > 0.1f /* && ForwardSpeed < 0.1f */)
+        // Check for stairs climbing if colliding, not flying and moving forward
+        if ( m_controllingPrim.IsColliding
+                    && !m_controllingPrim.Flying
+                    && m_controllingPrim.TargetVelocitySpeed > 0.1f )
         {
             // The range near the character's feet where we will consider stairs
-            float nearFeetHeightMin = m_controllingPrim.RawPosition.Z - (m_controllingPrim.Size.Z / 2f) + 0.05f;
+            // float nearFeetHeightMin = m_controllingPrim.RawPosition.Z - (m_controllingPrim.Size.Z / 2f) + 0.05f;
+            // Note: there is a problem with the computation of the capsule height. Thus RawPosition is off
+            //    from the height. Revisit size and this computation when height is scaled properly.
+            float nearFeetHeightMin = m_controllingPrim.RawPosition.Z - (m_controllingPrim.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
+            // Look for a collision point that is near the character's feet and is oriented the same as the charactor is.
+            // Find the highest 'good' collision.
+            OMV.Vector3 highestTouchPosition = OMV.Vector3.Zero;
             foreach (KeyValuePair<uint, ContactPoint> kvp in m_controllingPrim.CollisionsLastTick.m_objCollisionList)
             {
                 // Don't care about collisions with the terrain
                 if (kvp.Key > m_physicsScene.TerrainManager.HighestTerrainID)
                 {
                     OMV.Vector3 touchPosition = kvp.Value.Position;
-                    // DetailLog("{0},BSCharacter.WalkUpStairs,min={1},max={2},touch={3}",
-                    //                 LocalID, nearFeetHeightMin, nearFeetHeightMax, touchPosition);
+                    m_physicsScene.DetailLog("{0},BSCharacter.WalkUpStairs,min={1},max={2},touch={3}",
+                                    m_controllingPrim.LocalID, nearFeetHeightMin, nearFeetHeightMax, touchPosition);
                     if (touchPosition.Z >= nearFeetHeightMin && touchPosition.Z <= nearFeetHeightMax)
                     {
                         // This contact is within the 'near the feet' range.
@@ -261,24 +276,76 @@ public class BSActorAvatarMove : BSActor
                         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) * m_controllingPrim.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);
-                            m_controllingPrim.ForcePosition = m_controllingPrim.RawPosition + displacement;
+                            if (highestTouchPosition.Z < touchPosition.Z)
+                                highestTouchPosition = touchPosition;
                         }
-                        m_physicsScene.DetailLog("{0},BSCharacter.WalkUpStairs,touchPos={1},nearFeetMin={2},faceDir={3},norm={4},diff={5},ret={6}",
-                                m_controllingPrim.LocalID, touchPosition, nearFeetHeightMin, directionFacing, touchNormal, diff, ret);
                     }
                 }
             }
+            m_walkingUpStairs = 0;
+            // If there is a good step sensing, move the avatar over the step.
+            if (highestTouchPosition != OMV.Vector3.Zero)
+            {
+                // Remember that we are going up stairs. This is needed because collisions
+                //    will stop when we move up so this smoothes out that effect.
+                m_walkingUpStairs = BSParam.AvatarStepSmoothingSteps;
+
+                m_lastStepUp = highestTouchPosition.Z - nearFeetHeightMin;
+                ret = ComputeStairCorrection(m_lastStepUp);
+                m_physicsScene.DetailLog("{0},BSCharacter.WalkUpStairs,touchPos={1},nearFeetMin={2},ret={3}",
+                        m_controllingPrim.LocalID, highestTouchPosition, nearFeetHeightMin, ret);
+            }
+        }
+        else
+        {
+            // If we used to be going up stairs but are not now, smooth the case where collision goes away while
+            //    we are bouncing up the stairs.
+            if (m_walkingUpStairs > 0)
+            {
+                m_walkingUpStairs--;
+                ret = ComputeStairCorrection(m_lastStepUp);
+            }
         }
 
         return ret;
     }
 
+    private OMV.Vector3 ComputeStairCorrection(float stepUp)
+    {
+        OMV.Vector3 ret = OMV.Vector3.Zero;
+        OMV.Vector3 displacement = OMV.Vector3.Zero;
+
+        if (stepUp > 0f)
+        {
+            // Found the stairs contact point. Push up a little to raise the character.
+            if (BSParam.AvatarStepForceFactor > 0f)
+            {
+                float upForce = stepUp * m_controllingPrim.Mass * BSParam.AvatarStepForceFactor;
+                ret = new OMV.Vector3(0f, 0f, upForce);
+            }
+
+            // Also move the avatar up for the new height
+            if (BSParam.AvatarStepUpCorrectionFactor > 0f)
+            {
+                // Move the avatar up related to the height of the collision
+                displacement = new OMV.Vector3(0f, 0f, stepUp * BSParam.AvatarStepUpCorrectionFactor);
+                m_controllingPrim.ForcePosition = m_controllingPrim.RawPosition + displacement;
+            }
+            else
+            {
+                if (BSParam.AvatarStepUpCorrectionFactor < 0f)
+                {
+                    // Move the avatar up about the specified step height
+                    displacement = new OMV.Vector3(0f, 0f, BSParam.AvatarStepHeight);
+                    m_controllingPrim.ForcePosition = m_controllingPrim.RawPosition + displacement;
+                }
+            }
+            m_physicsScene.DetailLog("{0},BSCharacter.WalkUpStairs.ComputeStairCorrection,disp={1},force={2}",
+                                        m_controllingPrim.LocalID, displacement, ret);
+
+        }
+        return ret;
+    }
 }
 }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
index 06df85e..980d405 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
@@ -128,6 +128,8 @@ public static class BSParam
         public static float AvatarStepHeight { get; private set; }
         public static float AvatarStepApproachFactor { get; private set; }
         public static float AvatarStepForceFactor { get; private set; }
+        public static float AvatarStepUpCorrectionFactor { get; private set; }
+        public static int AvatarStepSmoothingSteps { get; private set; }
 
     // Vehicle parameters
     public static float VehicleMaxLinearVelocity { get; private set; }
@@ -234,6 +236,7 @@ public static class BSParam
             objectSet = pObjSetter;
         }
         /* Wish I could simplify using this definition but CLR doesn't store references so closure around delegates of references won't work
+         * TODO: Maybe use reflection and the name of the variable to create a reference for the getter/setter.
         public ParameterDefn(string pName, string pDesc, T pDefault, ref T loc)
             : base(pName, pDesc)
         {
@@ -561,7 +564,7 @@ public static class BSParam
             (s) => { return AvatarBelowGroundUpCorrectionMeters; },
             (s,v) => { AvatarBelowGroundUpCorrectionMeters = v; } ),
             new ParameterDefn<float>("AvatarStepHeight", "Height of a step obstacle to consider step correction",
-            0.3f,
+            0.6f,
             (s) => { return AvatarStepHeight; },
             (s,v) => { AvatarStepHeight = v; } ),
             new ParameterDefn<float>("AvatarStepApproachFactor", "Factor to control angle of approach to step (0=straight on)",
@@ -569,9 +572,17 @@ public static class BSParam
             (s) => { return AvatarStepApproachFactor; },
             (s,v) => { AvatarStepApproachFactor = v; } ),
             new ParameterDefn<float>("AvatarStepForceFactor", "Controls the amount of force up applied to step up onto a step",
-            2.0f,
+            1.0f,
             (s) => { return AvatarStepForceFactor; },
             (s,v) => { AvatarStepForceFactor = v; } ),
+            new ParameterDefn<float>("AvatarStepUpCorrectionFactor", "Multiplied by height of step collision to create up movement at step",
+            1.0f,
+            (s) => { return AvatarStepUpCorrectionFactor; },
+            (s,v) => { AvatarStepUpCorrectionFactor = v; } ),
+            new ParameterDefn<int>("AvatarStepSmoothingSteps", "Number of frames after a step collision that we continue walking up stairs",
+            2,
+            (s) => { return AvatarStepSmoothingSteps; },
+            (s,v) => { AvatarStepSmoothingSteps = v; } ),
 
         new ParameterDefn<float>("VehicleMaxLinearVelocity", "Maximum velocity magnitude that can be assigned to a vehicle",
             1000.0f,
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index 98eb4ca..309d004 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -96,7 +96,7 @@ public abstract class BSPhysObject : PhysicsActor
         SetMaterial((int)MaterialAttributes.Material.Wood);
 
         CollisionCollection = new CollisionEventUpdate();
-        CollisionsLastTick = CollisionCollection;
+        CollisionsLastReported = CollisionCollection;
         SubscribedEventsMs = 0;
         CollidingStep = 0;
         CollidingGroundStep = 0;
@@ -368,11 +368,14 @@ public abstract class BSPhysObject : PhysicsActor
         }
     }
 
-    // The collisions that have been collected this tick
+    // The collisions that have been collected for the next collision reporting (throttled by subscription)
     protected CollisionEventUpdate CollisionCollection;
-    // Remember collisions from last tick for fancy collision based actions
+    // This is the collision collection last reported to the Simulator.
+    public CollisionEventUpdate CollisionsLastReported;
+    // Remember the collisions recorded in the last tick for fancy collision checking
     //     (like a BSCharacter walking up stairs).
     public CollisionEventUpdate CollisionsLastTick;
+    private long CollisionsLastTickStep = -1;
 
     // The simulation step is telling this object about a collision.
     // Return 'true' if a collision was processed and should be sent up.
@@ -399,6 +402,15 @@ public abstract class BSPhysObject : PhysicsActor
         // For movement tests, remember if we are colliding with an object that is moving.
         ColliderIsMoving = collidee != null ? (collidee.RawVelocity != OMV.Vector3.Zero) : false;
 
+        // Make a collection of the collisions that happened the last simulation tick.
+        // This is different than the collection created for sending up to the simulator as it is cleared every tick.
+        if (CollisionsLastTickStep != PhysicsScene.SimulationStep)
+        {
+            CollisionsLastTick = new CollisionEventUpdate();
+            CollisionsLastTickStep = PhysicsScene.SimulationStep;
+        }
+        CollisionsLastTick.AddCollider(collidingWith, new ContactPoint(contactPoint, contactNormal, pentrationDepth));
+
         // If someone has subscribed for collision events log the collision so it will be reported up
         if (SubscribedEvents()) {
             CollisionCollection.AddCollider(collidingWith, new ContactPoint(contactPoint, contactNormal, pentrationDepth));
@@ -419,7 +431,7 @@ public abstract class BSPhysObject : PhysicsActor
         bool ret = true;
 
         // If the 'no collision' call, force it to happen right now so quick collision_end
-        bool force = (CollisionCollection.Count == 0 && CollisionsLastTick.Count != 0);
+        bool force = (CollisionCollection.Count == 0 && CollisionsLastReported.Count != 0);
 
         // throttle the collisions to the number of milliseconds specified in the subscription
         if (force || (PhysicsScene.SimulationNowTime >= NextCollisionOkTime))
@@ -438,7 +450,7 @@ public abstract class BSPhysObject : PhysicsActor
             base.SendCollisionUpdate(CollisionCollection);
 
             // Remember the collisions from this tick for some collision specific processing.
-            CollisionsLastTick = CollisionCollection;
+            CollisionsLastReported = 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.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 3423d2e..4bc266b 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -69,12 +69,17 @@ public class BSPrim : BSPhysObject
 
     private int CrossingFailures { get; set; }
 
+    // Keep a handle to the vehicle actor so it is easy to set parameters on same.
     public BSDynamics VehicleActor;
     public const string VehicleActorName = "BasicVehicle";
 
+    // Parameters for the hover actor
     public const string HoverActorName = "HoverActor";
+    // Parameters for the axis lock actor
     public const String LockedAxisActorName = "BSPrim.LockedAxis";
+    // Parameters for the move to target actor
     public const string MoveToTargetActorName = "MoveToTargetActor";
+    // Parameters for the setForce and setTorque actors
     public const string SetForceActorName = "SetForceActor";
     public const string SetTorqueActorName = "SetTorqueActor";
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
index a0131c7..1284ae7 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
@@ -42,6 +42,8 @@ One sided meshes? Should terrain be built into a closed shape?
 
 VEHICLES TODO LIST:
 =================================================
+UBit improvements to remove rubber-banding of avatars sitting on vehicle child prims:
+        https://github.com/UbitUmarov/Ubit-opensim
 Border crossing with linked vehicle causes crash
         20121129.1411: editting/moving phys object across region boundries causes crash
             getPos-> btRigidBody::upcast -> getBodyType -> BOOM
@@ -167,6 +169,7 @@ Eliminate collisions between objects in a linkset. (LinksetConstraint)
 
 MORE
 ======================================================
+Compute avatar size and scale correctly. Now it is a bit off from the capsule size.
 Create tests for different interface components
         Have test objects/scripts measure themselves and turn color if correct/bad
         Test functions in SL and calibrate correctness there