14 files changed, 358 insertions, 77 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs b/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs
index f5b84d4..fdf2cb9 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs
@@ -268,6 +268,25 @@ public override BulletShape BuildHullShapeFromMesh(BulletWorld world, BulletShap
                     BSPhysicsShapeType.SHAPE_HULL);
 }
 
+public override BulletShape BuildConvexHullShapeFromMesh(BulletWorld world, BulletShape meshShape)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    BulletShapeUnman shapeu = meshShape as BulletShapeUnman;
+    return new BulletShapeUnman(
+                    BSAPICPP.BuildConvexHullShapeFromMesh2(worldu.ptr, shapeu.ptr),
+                    BSPhysicsShapeType.SHAPE_CONVEXHULL);
+}
+
+public override BulletShape CreateConvexHullShape(BulletWorld world,
+                int indicesCount, int[] indices,
+                int verticesCount, float[] vertices)
+{
+    BulletWorldUnman worldu = world as BulletWorldUnman;
+    return new BulletShapeUnman(
+                    BSAPICPP.CreateConvexHullShape2(worldu.ptr, indicesCount, indices, verticesCount, vertices),
+                    BSPhysicsShapeType.SHAPE_CONVEXHULL);
+}
+
 public override BulletShape BuildNativeShape(BulletWorld world, ShapeData shapeData)
 {
     BulletWorldUnman worldu = world as BulletWorldUnman;
@@ -1414,6 +1433,14 @@ public static extern IntPtr CreateHullShape2(IntPtr world,
 public static extern IntPtr BuildHullShapeFromMesh2(IntPtr world, IntPtr meshShape, HACDParams parms);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr BuildConvexHullShapeFromMesh2(IntPtr world, IntPtr meshShape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr CreateConvexHullShape2(IntPtr world,
+                int indicesCount, [MarshalAs(UnmanagedType.LPArray)] int[] indices,
+                int verticesCount, [MarshalAs(UnmanagedType.LPArray)] float[] vertices );
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern IntPtr BuildNativeShape2(IntPtr world, ShapeData shapeData);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs b/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
index f6b4359..b37265a 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs
@@ -1778,6 +1778,16 @@ private sealed class BulletConstraintXNA : BulletConstraint
         /* TODO */ return null;
     }
 
+    public override BulletShape BuildConvexHullShapeFromMesh(BulletWorld world, BulletShape meshShape)
+    {
+        /* TODO */ return null;
+    }
+
+    public override BulletShape CreateConvexHullShape(BulletWorld pWorld, int pIndicesCount, int[] indices, int pVerticesCount, float[] verticesAsFloats)
+    {
+        /* TODO */ return null;
+    }
+
     public override BulletShape CreateMeshShape(BulletWorld pWorld, int pIndicesCount, int[] indices, int pVerticesCount, float[] verticesAsFloats)
     {
         //DumpRaw(indices,verticesAsFloats,pIndicesCount,pVerticesCount);
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/BSApiTemplate.cs b/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
index d0d9f34..bfeec24 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
@@ -70,6 +70,7 @@ public enum BSPhysicsShapeType
         SHAPE_COMPOUND  = 22,
         SHAPE_HEIGHTMAP = 23,
     SHAPE_AVATAR    = 24,
+    SHAPE_CONVEXHULL= 25,
 };
 
 // The native shapes have predefined shape hash keys
@@ -325,6 +326,12 @@ public abstract BulletShape CreateHullShape(BulletWorld world,
 
 public abstract BulletShape BuildHullShapeFromMesh(BulletWorld world, BulletShape meshShape, HACDParams parms);
 
+public abstract BulletShape BuildConvexHullShapeFromMesh(BulletWorld world, BulletShape meshShape);
+
+public abstract BulletShape CreateConvexHullShape(BulletWorld world,
+                int indicesCount, int[] indices,
+                int verticesCount, float[] vertices );
+
 public abstract BulletShape BuildNativeShape(BulletWorld world, ShapeData shapeData);
 
 public abstract bool IsNativeShape(BulletShape shape);
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/BSShapeCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
index 1976c42..bc26460 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
@@ -612,7 +612,7 @@ public sealed class BSShapeCollection : IDisposable
 
         newShape = CreatePhysicalMesh(prim, newMeshKey, prim.BaseShape, prim.Size, lod);
         // Take evasive action if the mesh was not constructed.
-        newShape = VerifyMeshCreated(newShape, prim);
+        newShape = VerifyMeshCreated(PhysicsScene, newShape, prim);
 
         ReferenceShape(newShape);
 
@@ -719,7 +719,7 @@ public sealed class BSShapeCollection : IDisposable
 
         newShape = CreatePhysicalHull(prim, newHullKey, prim.BaseShape, prim.Size, lod);
         // It might not have been created if we're waiting for an asset.
-        newShape = VerifyMeshCreated(newShape, prim);
+        newShape = VerifyMeshCreated(PhysicsScene, newShape, prim);
 
         ReferenceShape(newShape);
 
@@ -923,7 +923,7 @@ public sealed class BSShapeCollection : IDisposable
 
     // Create a hash of all the shape parameters to be used as a key
     //    for this particular shape.
-    private System.UInt64 ComputeShapeKey(OMV.Vector3 size, PrimitiveBaseShape pbs, out float retLod)
+    public static System.UInt64 ComputeShapeKey(OMV.Vector3 size, PrimitiveBaseShape pbs, out float retLod)
     {
         // level of detail based on size and type of the object
         float lod = BSParam.MeshLOD;
@@ -944,7 +944,7 @@ public sealed class BSShapeCollection : IDisposable
         return pbs.GetMeshKey(size, lod);
     }
     // For those who don't want the LOD
-    private System.UInt64 ComputeShapeKey(OMV.Vector3 size, PrimitiveBaseShape pbs)
+    public static System.UInt64 ComputeShapeKey(OMV.Vector3 size, PrimitiveBaseShape pbs)
     {
         float lod;
         return ComputeShapeKey(size, pbs, out lod);
@@ -957,7 +957,7 @@ public sealed class BSShapeCollection : IDisposable
     //     us to not loop forever.
     // Called after creating a physical mesh or hull. If the physical shape was created,
     //     just return.
-    private BulletShape VerifyMeshCreated(BulletShape newShape, BSPhysObject prim)
+    public static BulletShape VerifyMeshCreated(BSScene physicsScene, BulletShape newShape, BSPhysObject prim)
     {
         // If the shape was successfully created, nothing more to do
         if (newShape.HasPhysicalShape)
@@ -969,7 +969,7 @@ public sealed class BSShapeCollection : IDisposable
         if (prim.PrimAssetState == BSPhysObject.PrimAssetCondition.Fetched)
         {
             prim.PrimAssetState = BSPhysObject.PrimAssetCondition.Failed;
-            PhysicsScene.Logger.WarnFormat("{0} Fetched asset would not mesh. {1}, texture={2}",
+            physicsScene.Logger.WarnFormat("{0} Fetched asset would not mesh. {1}, texture={2}",
                                             LogHeader, prim.PhysObjectName, prim.BaseShape.SculptTexture);
         }
         else
@@ -981,14 +981,14 @@ public sealed class BSShapeCollection : IDisposable
                 && prim.BaseShape.SculptTexture != OMV.UUID.Zero
                 )
             {
-                DetailLog("{0},BSShapeCollection.VerifyMeshCreated,fetchAsset", prim.LocalID);
+                physicsScene.DetailLog("{0},BSShapeCollection.VerifyMeshCreated,fetchAsset", prim.LocalID);
                 // Multiple requestors will know we're waiting for this asset
                 prim.PrimAssetState = BSPhysObject.PrimAssetCondition.Waiting;
 
                 BSPhysObject xprim = prim;
                 Util.FireAndForget(delegate
                     {
-                        RequestAssetDelegate assetProvider = PhysicsScene.RequestAssetMethod;
+                        RequestAssetDelegate assetProvider = physicsScene.RequestAssetMethod;
                         if (assetProvider != null)
                         {
                             BSPhysObject yprim = xprim; // probably not necessary, but, just in case.
@@ -1016,7 +1016,7 @@ public sealed class BSShapeCollection : IDisposable
                                     yprim.PrimAssetState = BSPhysObject.PrimAssetCondition.Fetched;
                                 else
                                     yprim.PrimAssetState = BSPhysObject.PrimAssetCondition.Failed;
-                                DetailLog("{0},BSShapeCollection,fetchAssetCallback,found={1},isSculpt={2},ids={3}",
+                                physicsScene.DetailLog("{0},BSShapeCollection,fetchAssetCallback,found={1},isSculpt={2},ids={3}",
                                             yprim.LocalID, assetFound, yprim.BaseShape.SculptEntry, mismatchIDs );
 
                             });
@@ -1024,8 +1024,8 @@ public sealed class BSShapeCollection : IDisposable
                         else
                         {
                             xprim.PrimAssetState = BSPhysObject.PrimAssetCondition.Failed;
-                            PhysicsScene.Logger.ErrorFormat("{0} Physical object requires asset but no asset provider. Name={1}",
-                                                        LogHeader, PhysicsScene.Name);
+                            physicsScene.Logger.ErrorFormat("{0} Physical object requires asset but no asset provider. Name={1}",
+                                                        LogHeader, physicsScene.Name);
                         }
                     });
             }
@@ -1033,15 +1033,15 @@ public sealed class BSShapeCollection : IDisposable
             {
                 if (prim.PrimAssetState == BSPhysObject.PrimAssetCondition.Failed)
                 {
-                    PhysicsScene.Logger.WarnFormat("{0} Mesh failed to fetch asset. obj={1}, texture={2}",
+                    physicsScene.Logger.WarnFormat("{0} Mesh failed to fetch asset. obj={1}, texture={2}",
                                                 LogHeader, prim.PhysObjectName, prim.BaseShape.SculptTexture);
                 }
             }
         }
 
         // While we wait for the mesh defining asset to be loaded, stick in a simple box for the object.
-        BulletShape fillinShape = BuildPhysicalNativeShape(prim, BSPhysicsShapeType.SHAPE_BOX, FixedShapeKey.KEY_BOX);
-        DetailLog("{0},BSShapeCollection.VerifyMeshCreated,boxTempShape", prim.LocalID);
+        BulletShape fillinShape = physicsScene.Shapes.BuildPhysicalNativeShape(prim, BSPhysicsShapeType.SHAPE_BOX, FixedShapeKey.KEY_BOX);
+        physicsScene.DetailLog("{0},BSShapeCollection.VerifyMeshCreated,boxTempShape", prim.LocalID);
 
         return fillinShape;
     }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSShapes.cs b/OpenSim/Region/Physics/BulletSPlugin/BSShapes.cs
index ee18379..dd5ae1a 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSShapes.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSShapes.cs
@@ -29,6 +29,9 @@ using System;
 using System.Collections.Generic;
 using System.Text;
 
+using OpenSim.Framework;
+using OpenSim.Region.Physics.Manager;
+
 using OMV = OpenMetaverse;
 
 namespace OpenSim.Region.Physics.BulletSPlugin
@@ -37,11 +40,19 @@ public abstract class BSShape
 {
     public int referenceCount { get; set; }
     public DateTime lastReferenced { get; set; }
+    public BulletShape physShapeInfo { get; set; }
 
     public BSShape()
     {
         referenceCount = 0;
         lastReferenced = DateTime.Now;
+        physShapeInfo = new BulletShape();
+    }
+    public BSShape(BulletShape pShape)
+    {
+        referenceCount = 0;
+        lastReferenced = DateTime.Now;
+        physShapeInfo = pShape;
     }
 
     // Get a reference to a physical shape. Create if it doesn't exist
@@ -79,21 +90,30 @@ public abstract class BSShape
 
         return ret;
     }
-    public static BSShape GetShapeReferenceNonSpecial(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)
+    private static BSShape GetShapeReferenceNonSpecial(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)
     {
+        BSShapeMesh.GetReference(physicsScene, forceRebuild, prim);
+        BSShapeHull.GetReference(physicsScene, forceRebuild, prim);
         return null;
     }
-    public static BSShape GetShapeReferenceNonNative(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)
+
+    // Called when this shape is being used again.
+    public virtual void IncrementReference()
     {
-        return null;
+        referenceCount++;
+        lastReferenced = DateTime.Now;
+    }
+
+    // Called when this shape is being used again.
+    public virtual void DecrementReference()
+    {
+        referenceCount--;
+        lastReferenced = DateTime.Now;
     }
 
     // Release the use of a physical shape.
     public abstract void Dereference(BSScene physicsScene);
 
-    // All shapes have a static call to get a reference to the physical shape
-    // protected abstract static BSShape GetReference();
-
     // Returns a string for debugging that uniquily identifies the memory used by this instance
     public virtual string AddrString
     {
@@ -112,6 +132,7 @@ public abstract class BSShape
     }
 }
 
+// ============================================================================================================
 public class BSShapeNull : BSShape
 {
     public BSShapeNull() : base()
@@ -121,23 +142,39 @@ public class BSShapeNull : BSShape
     public override void Dereference(BSScene physicsScene) { /* The magic of garbage collection will make this go away */ }
 }
 
+// ============================================================================================================
 public class BSShapeNative : BSShape
 {
     private static string LogHeader = "[BULLETSIM SHAPE NATIVE]";
-    public BSShapeNative() : base()
+    public BSShapeNative(BulletShape pShape) : base(pShape)
     {
     }
+
     public static BSShape GetReference(BSScene physicsScene, BSPhysObject prim, 
-                    BSPhysicsShapeType shapeType, FixedShapeKey shapeKey) 
+                                            BSPhysicsShapeType shapeType, FixedShapeKey shapeKey) 
     {
         // Native shapes are not shared and are always built anew.
-        //return new BSShapeNative(physicsScene, prim, shapeType, shapeKey);
-        return null;
+        return new BSShapeNative(CreatePhysicalNativeShape(physicsScene, prim, shapeType, shapeKey));
     }
 
-    private BSShapeNative(BSScene physicsScene, BSPhysObject prim,
-                    BSPhysicsShapeType shapeType, FixedShapeKey shapeKey)
+    // Make this reference to the physical shape go away since native shapes are not shared.
+    public override void Dereference(BSScene physicsScene)
+    {
+        // Native shapes are not tracked and are released immediately
+        if (physShapeInfo.HasPhysicalShape)
+        {
+            physicsScene.DetailLog("{0},BSShapeNative.DereferenceShape,deleteNativeShape,shape={1}", BSScene.DetailLogZero, this);
+            physicsScene.PE.DeleteCollisionShape(physicsScene.World, physShapeInfo);
+        }
+        physShapeInfo.Clear();
+        // Garbage collection will free up this instance.
+    }
+
+    private static BulletShape CreatePhysicalNativeShape(BSScene physicsScene, BSPhysObject prim,
+                                            BSPhysicsShapeType shapeType, FixedShapeKey shapeKey)
     {
+        BulletShape newShape;
+
         ShapeData nativeShapeData = new ShapeData();
         nativeShapeData.Type = shapeType;
         nativeShapeData.ID = prim.LocalID;
@@ -146,63 +183,164 @@ public class BSShapeNative : BSShape
         nativeShapeData.MeshKey = (ulong)shapeKey;
         nativeShapeData.HullKey = (ulong)shapeKey;
 
-       
-        /*
         if (shapeType == BSPhysicsShapeType.SHAPE_CAPSULE)
         {
-            ptr = PhysicsScene.PE.BuildCapsuleShape(physicsScene.World, 1f, 1f, prim.Scale);
-            physicsScene.DetailLog("{0},BSShapeCollection.BuiletPhysicalNativeShape,capsule,scale={1}", prim.LocalID, prim.Scale);
+            newShape = physicsScene.PE.BuildCapsuleShape(physicsScene.World, 1f, 1f, prim.Scale);
+            physicsScene.DetailLog("{0},BSShapeNative,capsule,scale={1}", prim.LocalID, prim.Scale);
         }
         else
         {
-            ptr = PhysicsScene.PE.BuildNativeShape(physicsScene.World, nativeShapeData);
+            newShape = physicsScene.PE.BuildNativeShape(physicsScene.World, nativeShapeData);
         }
-        if (ptr == IntPtr.Zero)
+        if (!newShape.HasPhysicalShape)
         {
             physicsScene.Logger.ErrorFormat("{0} BuildPhysicalNativeShape failed. ID={1}, shape={2}",
                                     LogHeader, prim.LocalID, shapeType);
         }
-        type = shapeType;
-        key = (UInt64)shapeKey;
-         */
-    }
-    // Make this reference to the physical shape go away since native shapes are not shared.
-    public override void Dereference(BSScene physicsScene)
-    {
-        /*
-        // Native shapes are not tracked and are released immediately
-        physicsScene.DetailLog("{0},BSShapeCollection.DereferenceShape,deleteNativeShape,shape={1}", BSScene.DetailLogZero, this);
-        PhysicsScene.PE.DeleteCollisionShape(physicsScene.World, this);
-        ptr = IntPtr.Zero;
-        // Garbage collection will free up this instance.
-         */
+        newShape.type = shapeType;
+        newShape.isNativeShape = true;
+        newShape.shapeKey = (UInt64)shapeKey;
+        return newShape;
     }
+
 }
 
+// ============================================================================================================
 public class BSShapeMesh : BSShape
 {
     private static string LogHeader = "[BULLETSIM SHAPE MESH]";
     private static Dictionary<System.UInt64, BSShapeMesh> Meshes = new Dictionary<System.UInt64, BSShapeMesh>();
 
-    public BSShapeMesh() : base()
+    public BSShapeMesh(BulletShape pShape) : base(pShape)
     {
     }
-    public static BSShape GetReference() { return new BSShapeNull();  }
-    public override void Dereference(BSScene physicsScene) { }
+    public static BSShape GetReference(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)
+    {
+        float lod;
+        System.UInt64 newMeshKey = BSShapeCollection.ComputeShapeKey(prim.Size, prim.BaseShape, out lod);
+
+        physicsScene.DetailLog("{0},BSShapeMesh,create,oldKey={1},newKey={2},size={3},lod={4}",
+                                prim.LocalID, prim.PhysShape.shapeKey.ToString("X"), newMeshKey.ToString("X"), prim.Size, lod);
+
+        BSShapeMesh retMesh;
+        lock (Meshes)
+        {
+            if (Meshes.TryGetValue(newMeshKey, out retMesh))
+            {
+                // The mesh has already been created. Return a new reference to same.
+                retMesh.IncrementReference();
+            }
+            else
+            {
+                // An instance of this mesh has not been created. Build and remember same.
+                BulletShape newShape = CreatePhysicalMesh(physicsScene, prim, newMeshKey, prim.BaseShape, prim.Size, lod);
+                // Take evasive action if the mesh was not constructed.
+                newShape = BSShapeCollection.VerifyMeshCreated(physicsScene, newShape, prim);
+
+                retMesh = new BSShapeMesh(newShape);
+
+                Meshes.Add(newMeshKey, retMesh);
+            }
+        }
+        return retMesh;
+    }
+    public override void Dereference(BSScene physicsScene)
+    {
+        lock (Meshes)
+        {
+            this.DecrementReference();
+            // TODO: schedule aging and destruction of unused meshes.
+        }
+    }
+
+    private static BulletShape CreatePhysicalMesh(BSScene physicsScene, BSPhysObject prim, System.UInt64 newMeshKey,
+                                            PrimitiveBaseShape pbs, OMV.Vector3 size, float lod)
+    {
+        BulletShape newShape = null;
+
+        IMesh meshData = physicsScene.mesher.CreateMesh(prim.PhysObjectName, pbs, size, lod, 
+                                        false,  // say it is not physical so a bounding box is not built
+                                        false   // do not cache the mesh and do not use previously built versions
+                                        );
+
+        if (meshData != null)
+        {
+
+            int[] indices = meshData.getIndexListAsInt();
+            int realIndicesIndex = indices.Length;
+            float[] verticesAsFloats = meshData.getVertexListAsFloat();
+
+            if (BSParam.ShouldRemoveZeroWidthTriangles)
+            {
+                // Remove degenerate triangles. These are triangles with two of the vertices
+                //    are the same. This is complicated by the problem that vertices are not
+                //    made unique in sculpties so we have to compare the values in the vertex.
+                realIndicesIndex = 0;
+                for (int tri = 0; tri < indices.Length; tri += 3)
+                {
+                    // Compute displacements into vertex array for each vertex of the triangle
+                    int v1 = indices[tri + 0] * 3;
+                    int v2 = indices[tri + 1] * 3;
+                    int v3 = indices[tri + 2] * 3;
+                // Check to see if any two of the vertices are the same
+                    if (!( (  verticesAsFloats[v1 + 0] == verticesAsFloats[v2 + 0]
+                           && verticesAsFloats[v1 + 1] == verticesAsFloats[v2 + 1]
+                           && verticesAsFloats[v1 + 2] == verticesAsFloats[v2 + 2])
+                        || (  verticesAsFloats[v2 + 0] == verticesAsFloats[v3 + 0]
+                           && verticesAsFloats[v2 + 1] == verticesAsFloats[v3 + 1]
+                           && verticesAsFloats[v2 + 2] == verticesAsFloats[v3 + 2])
+                        || (  verticesAsFloats[v1 + 0] == verticesAsFloats[v3 + 0]
+                           && verticesAsFloats[v1 + 1] == verticesAsFloats[v3 + 1]
+                           && verticesAsFloats[v1 + 2] == verticesAsFloats[v3 + 2]) )
+                    )
+                    {
+                        // None of the vertices of the triangles are the same. This is a good triangle;
+                        indices[realIndicesIndex + 0] = indices[tri + 0];
+                        indices[realIndicesIndex + 1] = indices[tri + 1];
+                        indices[realIndicesIndex + 2] = indices[tri + 2];
+                        realIndicesIndex += 3;
+                    }
+                }
+            }
+            physicsScene.DetailLog("{0},BSShapeCollection.CreatePhysicalMesh,origTri={1},realTri={2},numVerts={3}",
+                        BSScene.DetailLogZero, indices.Length / 3, realIndicesIndex / 3, verticesAsFloats.Length / 3);
+
+            if (realIndicesIndex != 0)
+            {
+                newShape = physicsScene.PE.CreateMeshShape(physicsScene.World,
+                                    realIndicesIndex, indices, verticesAsFloats.Length / 3, verticesAsFloats);
+            }
+            else
+            {
+                physicsScene.Logger.DebugFormat("{0} All mesh triangles degenerate. Prim {1} at {2} in {3}",
+                                    LogHeader, prim.PhysObjectName, prim.RawPosition, physicsScene.Name);
+            }
+        }
+        newShape.shapeKey = newMeshKey;
+
+        return newShape;
+    }
 }
 
+// ============================================================================================================
 public class BSShapeHull : BSShape
 {
     private static string LogHeader = "[BULLETSIM SHAPE HULL]";
     private static Dictionary<System.UInt64, BSShapeHull> Hulls = new Dictionary<System.UInt64, BSShapeHull>();
 
-    public BSShapeHull() : base()
+    public BSShapeHull(BulletShape pShape) : base(pShape)
+    {
+    }
+    public static BSShape GetReference(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)
+    {
+        return new BSShapeNull();
+    }
+    public override void Dereference(BSScene physicsScene)
     {
     }
-    public static BSShape GetReference() { return new BSShapeNull();  }
-    public override void Dereference(BSScene physicsScene) { }
 }
 
+// ============================================================================================================
 public class BSShapeCompound : BSShape
 {
     private static string LogHeader = "[BULLETSIM SHAPE COMPOUND]";
@@ -216,6 +354,7 @@ public class BSShapeCompound : BSShape
     public override void Dereference(BSScene physicsScene) { }
 }
 
+// ============================================================================================================
 public class BSShapeAvatar : BSShape
 {
     private static string LogHeader = "[BULLETSIM SHAPE AVATAR]";
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
diff --git a/bin/lib32/BulletSim.dll b/bin/lib32/BulletSim.dll
index 618d4bd..1e94d05 100755
--- a/bin/lib32/BulletSim.dll
+++ b/bin/lib32/BulletSim.dll
diff --git a/bin/lib32/libBulletSim.so b/bin/lib32/libBulletSim.so
index e3b40fb..d79896a 100755
--- a/bin/lib32/libBulletSim.so
+++ b/bin/lib32/libBulletSim.so
diff --git a/bin/lib64/BulletSim.dll b/bin/lib64/BulletSim.dll
index 491ac22..68cfcc4 100755
--- a/bin/lib64/BulletSim.dll
+++ b/bin/lib64/BulletSim.dll
diff --git a/bin/lib64/libBulletSim.so b/bin/lib64/libBulletSim.so
index a8ad533..08c00af 100755
--- a/bin/lib64/libBulletSim.so
+++ b/bin/lib64/libBulletSim.so