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