BulletSim: reorder avatar collision checking to eliminate double collision_end.

Various tweekings to avatar shape/mass/inertia/etc.
Remove change from avatar radius to diameter. But still the avatar sinks.
Collision_end now happens immediately rather than at the next subscription time.

6 files changed, 43 insertions, 35 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index e7bff6e..fc4526b 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -141,10 +141,6 @@ public class BSCharacter : BSPhysObject
         BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, BSBody.ptr);
 
         ZeroMotion();
-
-        OMV.Vector3 localInertia = BulletSimAPI.CalculateLocalInertia2(BSShape.ptr, MassRaw);
-        BulletSimAPI.SetMassProps2(BSBody.ptr, MassRaw, localInertia);
-
         ForcePosition = _position;
         // Set the velocity and compute the proper friction
         ForceVelocity = _velocity;
@@ -157,6 +153,9 @@ public class BSCharacter : BSPhysObject
             BulletSimAPI.SetCcdSweepSphereRadius2(BSBody.ptr, PhysicsScene.Params.ccdSweptSphereRadius);
         }
 
+        OMV.Vector3 localInertia = BulletSimAPI.CalculateLocalInertia2(BSShape.ptr, MassRaw);
+        BulletSimAPI.SetMassProps2(BSBody.ptr, MassRaw, localInertia);
+
         BulletSimAPI.AddToCollisionFlags2(BSBody.ptr, CollisionFlags.CF_CHARACTER_OBJECT);
 
         BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, BSBody.ptr);
@@ -583,14 +582,16 @@ public class BSCharacter : BSPhysObject
 
     private void ComputeAvatarScale(OMV.Vector3 size)
     {
+        // The 'size' given by the simulator is the mid-point of the avatar
+        //    and X and Y are unspecified.
+
         OMV.Vector3 newScale = OMV.Vector3.Zero;
-        // Scale wants the diameter so mult radius by two
-        newScale.X = PhysicsScene.Params.avatarCapsuleRadius * 2f;
-        newScale.Y = PhysicsScene.Params.avatarCapsuleRadius * 2f;
+        newScale.X = PhysicsScene.Params.avatarCapsuleRadius;
+        newScale.Y = PhysicsScene.Params.avatarCapsuleRadius;
 
-        // From the total height, add the capsule half spheres that are at each end 
-        // newScale.Z = (size.Z) - Math.Min(newScale.X, newScale.Y);
-        newScale.Z = (size.Z * 2f);
+        // From the total height, remote the capsule half spheres that are at each end 
+        newScale.Z = (size.Z * 2f) - Math.Min(newScale.X, newScale.Y);
+        // newScale.Z = (size.Z * 2f);
         Scale = newScale;
     }
 
@@ -599,14 +600,14 @@ public class BSCharacter : BSPhysObject
     {
         _avatarVolume = (float)(
                         Math.PI
-                        * (Scale.X / 2f)
-                        * (Scale.Y / 2f)  // the area of capsule cylinder
-                        * Scale.Z  // times height of capsule cylinder
+                        * Scale.X
+                        * Scale.Y    // the area of capsule cylinder
+                        * Scale.Z    // times height of capsule cylinder
                       + 1.33333333f
                         * Math.PI
-                        * (Scale.X / 2f)
-                        * (Math.Min(Scale.X, Scale.Y) / 2f)
-                        * (Scale.Y / 2f)  // plus the volume of the capsule end caps
+                        * Scale.X
+                        * Math.Min(Scale.X, Scale.Y)
+                        * Scale.Y    // plus the volume of the capsule end caps
                         );
         _mass = _avatarDensity * _avatarVolume;
     }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index b8ef338..202052d 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -156,9 +156,11 @@ public abstract class BSPhysObject : PhysicsActor
     public virtual bool SendCollisions()
     {
         bool ret = true;
+        // If the 'no collision' call, force it to happen right now so quick collision_end
+        bool force = CollisionCollection.Count == 0;
 
         // throttle the collisions to the number of milliseconds specified in the subscription
-        if (PhysicsScene.SimulationNowTime >= NextCollisionOkTime)
+        if (force || (PhysicsScene.SimulationNowTime >= NextCollisionOkTime))
         {
             NextCollisionOkTime = PhysicsScene.SimulationNowTime + SubscribedEventsMs;
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index ab2835c..6621d39 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -532,26 +532,26 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             }
         }
 
-        // This is a kludge to get avatar movement updates.
-        //   The simulator expects collisions for avatars even if there are have been no collisions. This updates
-        //   avatar animations and stuff.
-        // If you fix avatar animation updates, remove this overhead and let normal collision processing happen.
-        foreach (BSPhysObject bsp in m_avatars)
-            bsp.SendCollisions();
-
         // The above SendCollision's batch up the collisions on the objects.
         //      Now push the collisions into the simulator.
         if (ObjectsWithCollisions.Count > 0)
         {
             foreach (BSPhysObject bsp in ObjectsWithCollisions)
-                if (!m_avatars.Contains(bsp))   // don't call avatars twice
-                    if (!bsp.SendCollisions())
-                    {
-                        // If the object is done colliding, see that it's removed from the colliding list
-                        ObjectsWithNoMoreCollisions.Add(bsp);
-                    }
+                if (!bsp.SendCollisions())
+                {
+                    // If the object is done colliding, see that it's removed from the colliding list
+                    ObjectsWithNoMoreCollisions.Add(bsp);
+                }
         }
 
+        // This is a kludge to get avatar movement updates.
+        // The simulator expects collisions for avatars even if there are have been no collisions. 
+        //    The event updates avatar animations and stuff.
+        // If you fix avatar animation updates, remove this overhead and let normal collision processing happen.
+        foreach (BSPhysObject bsp in m_avatars)
+            if (!ObjectsWithCollisions.Contains(bsp))   // don't call avatars twice
+                bsp.SendCollisions();
+
         // Objects that are done colliding are removed from the ObjectsWithCollisions list.
         // Not done above because it is inside an iteration of ObjectWithCollisions.
         if (ObjectsWithNoMoreCollisions.Count > 0)
@@ -575,6 +575,10 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             }
         }
 
+        // This causes the unmanaged code to output ALL the values found in ALL the objects in the world.
+        // Only enable this in a limited test world with few objects.
+        // BulletSimAPI.DumpAllInfo2(World.ptr);    // DEBUG DEBUG DEBUG
+
         // The physics engine returns the number of milliseconds it simulated this call.
         // These are summed and normalized to one second and divided by 1000 to give the reported physics FPS.
         // We multiply by 55 to give a recognizable running rate (55 or less).
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
index 3d15eaa..861ffe7 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
@@ -417,10 +417,9 @@ public class BSShapeCollection : IDisposable
 
         if (shapeType == ShapeData.PhysicsShapeType.SHAPE_AVATAR)
         {
-            // The radius is scaled by 1/2 because we scale by the diameter.
             newShape = new BulletShape(
-                    BulletSimAPI.BuildCapsuleShape2(PhysicsScene.World.ptr, 0.5f, 1.0f, shapeData.Scale), 
-                    shapeType);
+                        BulletSimAPI.BuildCapsuleShape2(PhysicsScene.World.ptr, 1.0f, 1.0f, shapeData.Scale), 
+                        shapeType);
             newShape.shapeKey = (System.UInt64)shapeKey;
             newShape.isNativeShape = true;
         }
@@ -432,7 +431,8 @@ public class BSShapeCollection : IDisposable
         }
 
         // Don't need to do a 'ReferenceShape()' here because native shapes are not shared.
-        // DetailLog("{0},BSShapeCollection.AddNativeShapeToPrim,create,newshape={1}", shapeData.ID, newShape);
+        DetailLog("{0},BSShapeCollection.AddNativeShapeToPrim,create,newshape={1},scale={2}", 
+                                shapeData.ID, newShape, shapeData.Scale);
 
         prim.BSShape = newShape;
         return true;
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
index 9743d94..4106534 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
@@ -114,6 +114,7 @@ public class BSTerrainManager
                         BulletSimAPI.CreateBodyWithDefaultMotionState2(groundPlaneShape.ptr, BSScene.GROUNDPLANE_ID,
                                                             Vector3.Zero, Quaternion.Identity));
         BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, m_groundPlane.ptr);
+        BulletSimAPI.UpdateSingleAabb2(PhysicsScene.World.ptr, m_groundPlane.ptr);
         // Ground plane does not move
         BulletSimAPI.ForceActivationState2(m_groundPlane.ptr, ActivationState.DISABLE_SIMULATION);
         // Everything collides with the ground plane.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
index 24d8db6..5ffd591 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
@@ -390,7 +390,7 @@ public enum CollisionFilterGroups : uint
     VolumeDetectMask        = ~BSensorTrigger,
     TerrainFilter           = BTerrainFilter,
     TerrainMask             = BAllFilter & ~BStaticFilter,
-    GroundPlaneFilter       = BAllFilter,
+    GroundPlaneFilter       = BGroundPlaneFilter,
     GroundPlaneMask         = BAllFilter
 
 };