BulletSim: change nonimal physics frame rate to 55 to give same numbers as ODE.

Change character scaling to represent size of capsule (diameter rather than radius)
Modify create capsule call to pass radius and height.
Eliminate errors when calculating shape inertia (should have some type checking).

6 files changed, 57 insertions, 52 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index 8e1171a..e7bff6e 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -89,13 +89,15 @@ public class BSCharacter : BSPhysObject
         _appliedVelocity = OMV.Vector3.Zero;
         _buoyancy = ComputeBuoyancyFromFlying(isFlying);
         _currentFriction = PhysicsScene.Params.avatarStandingFriction;
+        _avatarDensity = PhysicsScene.Params.avatarDensity;
 
         // The dimensions of the avatar capsule are kept in the scale.
         // Physics creates a unit capsule which is scaled by the physics engine.
         ComputeAvatarScale(_size);
-        _avatarDensity = PhysicsScene.Params.avatarDensity;
         // set _avatarVolume and _mass based on capsule size, _density and Scale
         ComputeAvatarVolumeAndMass();
+        DetailLog("{0},BSCharacter.create,call,size={1},scale={2},density={3},volume={4},mass={5}",
+                            LocalID, _size, Scale, _avatarDensity, _avatarVolume, MassRaw);
 
         ShapeData shapeData = new ShapeData();
         shapeData.ID = LocalID;
@@ -103,6 +105,7 @@ public class BSCharacter : BSPhysObject
         shapeData.Position = _position;
         shapeData.Rotation = _orientation;
         shapeData.Velocity = _velocity;
+        shapeData.Size = Scale;
         shapeData.Scale = Scale;
         shapeData.Mass = _mass;
         shapeData.Buoyancy = _buoyancy;
@@ -114,8 +117,9 @@ public class BSCharacter : BSPhysObject
         PhysicsScene.TaintedObject("BSCharacter.create", delegate()
         {
             DetailLog("{0},BSCharacter.create,taint", LocalID);
-            PhysicsScene.Shapes.GetBodyAndShape(true, PhysicsScene.World, this, shapeData, null, null, null);

-

+            // New body and shape into BSBody and BSShape
+            PhysicsScene.Shapes.GetBodyAndShape(true, PhysicsScene.World, this, shapeData, null, null, null);
+
             SetPhysicalProperties();
         });
         return;
@@ -138,7 +142,7 @@ public class BSCharacter : BSPhysObject
 
         ZeroMotion();
 
-        OMV.Vector3 localInertia = BulletSimAPI.CalculateLocalInertia2(BSShape.ptr, MassRaw);

+        OMV.Vector3 localInertia = BulletSimAPI.CalculateLocalInertia2(BSShape.ptr, MassRaw);
         BulletSimAPI.SetMassProps2(BSBody.ptr, MassRaw, localInertia);
 
         ForcePosition = _position;
@@ -151,13 +155,13 @@ public class BSCharacter : BSPhysObject
         {
             BulletSimAPI.SetCcdMotionThreshold2(BSBody.ptr, PhysicsScene.Params.ccdMotionThreshold);
             BulletSimAPI.SetCcdSweepSphereRadius2(BSBody.ptr, PhysicsScene.Params.ccdSweptSphereRadius);
-        }

-

+        }
+
         BulletSimAPI.AddToCollisionFlags2(BSBody.ptr, CollisionFlags.CF_CHARACTER_OBJECT);
 
         BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, BSBody.ptr);
 
-        BulletSimAPI.ForceActivationState2(BSBody.ptr, ActivationState.DISABLE_DEACTIVATION);
+        BulletSimAPI.ForceActivationState2(BSBody.ptr, ActivationState.ACTIVE_TAG);
         BulletSimAPI.UpdateSingleAabb2(PhysicsScene.World.ptr, BSBody.ptr);
 
         // Do this after the object has been added to the world
@@ -176,7 +180,7 @@ public class BSCharacter : BSPhysObject
         get
         {
             // Avatar capsule size is kept in the scale parameter.
-            return new OMV.Vector3(Scale.X * 2, Scale.Y * 2, Scale.Z);
+            return _size;
         }
 
         set {
@@ -184,11 +188,13 @@ public class BSCharacter : BSPhysObject
             _size = value;
             ComputeAvatarScale(_size);
             ComputeAvatarVolumeAndMass();
+            DetailLog("{0},BSCharacter.setSize,call,scale={1},density={2},volume={3},mass={4}",
+                            LocalID, Scale, _avatarDensity, _avatarVolume, MassRaw);
 
             PhysicsScene.TaintedObject("BSCharacter.setSize", delegate()
             {
                 BulletSimAPI.SetLocalScaling2(BSShape.ptr, Scale);
-                OMV.Vector3 localInertia = BulletSimAPI.CalculateLocalInertia2(BSBody.ptr, MassRaw);
+                OMV.Vector3 localInertia = BulletSimAPI.CalculateLocalInertia2(BSShape.ptr, MassRaw);
                 BulletSimAPI.SetMassProps2(BSBody.ptr, MassRaw, localInertia);
             });
 
@@ -578,13 +584,13 @@ public class BSCharacter : BSPhysObject
     private void ComputeAvatarScale(OMV.Vector3 size)
     {
         OMV.Vector3 newScale = OMV.Vector3.Zero;
-        newScale.X = PhysicsScene.Params.avatarCapsuleRadius;
-        newScale.Y = PhysicsScene.Params.avatarCapsuleRadius;
+        // Scale wants the diameter so mult radius by two
+        newScale.X = PhysicsScene.Params.avatarCapsuleRadius * 2f;
+        newScale.Y = PhysicsScene.Params.avatarCapsuleRadius * 2f;
 
-        // The 1.15 came from ODE but it seems to cause the avatar to float off the ground
-        // Scale.Z = (_size.Z * 1.15f) - (Scale.X + Scale.Y);
-        // From the total height, remove the capsule half spheres that are at each end 
-        newScale.Z = (size.Z) - (Math.Min(newScale.X, newScale.Y) * 2f);
+        // 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);
         Scale = newScale;
     }
 
@@ -593,14 +599,14 @@ public class BSCharacter : BSPhysObject
     {
         _avatarVolume = (float)(
                         Math.PI
-                        * Scale.X
-                        * Scale.Y  // the area of capsule cylinder
+                        * (Scale.X / 2f)
+                        * (Scale.Y / 2f)  // the area of capsule cylinder
                         * Scale.Z  // times height of capsule cylinder
                       + 1.33333333f
                         * Math.PI
-                        * Scale.X
-                        * Math.Min(Scale.X, Scale.Y)
-                        * Scale.Y  // plus the volume of the capsule end caps
+                        * (Scale.X / 2f)
+                        * (Math.Min(Scale.X, Scale.Y) / 2f)
+                        * (Scale.Y / 2f)  // 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 34dec26..b8ef338 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -141,18 +141,15 @@ public abstract class BSPhysObject : PhysicsActor
         // if someone has subscribed for collision events....
         if (SubscribedEvents()) {
             CollisionCollection.AddCollider(collidingWith, new ContactPoint(contactPoint, contactNormal, pentrationDepth));
-            // DetailLog("{0},{1}.Collison.AddCollider,call,with={2},point={3},normal={4},depth={5}",
-            //                 LocalID, TypeName, collidingWith, contactPoint, contactNormal, pentrationDepth);
+            DetailLog("{0},{1}.Collison.AddCollider,call,with={2},point={3},normal={4},depth={5}",
+                            LocalID, TypeName, collidingWith, contactPoint, contactNormal, pentrationDepth);
 
             ret = true;
         }
         return ret;
     }
 
-    // Routine to send the collected collisions into the simulator.
-    // Also handles removal of this from the collection of objects with collisions if
-    //      there are no collisions from this object. Mechanism is create one last
-    //      collision event to make collision_end work.
+    // Send the collected collisions into the simulator.
     // Called at taint time from within the Step() function thus no locking problems
     //      with CollisionCollection and ObjectsWithNoMoreCollisions.
     // Return 'true' if there were some actual collisions passed up
@@ -161,10 +158,9 @@ public abstract class BSPhysObject : PhysicsActor
         bool ret = true;
 
         // throttle the collisions to the number of milliseconds specified in the subscription
-        int nowTime = PhysicsScene.SimulationNowTime;
-        if (nowTime >= NextCollisionOkTime)
+        if (PhysicsScene.SimulationNowTime >= NextCollisionOkTime)
         {
-            NextCollisionOkTime = nowTime + SubscribedEventsMs;
+            NextCollisionOkTime = PhysicsScene.SimulationNowTime + SubscribedEventsMs;
 
             // We are called if we previously had collisions. If there are no collisions
             //   this time, send up one last empty event so OpenSim can sense collision end.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index b9e1908..8013e68 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -48,10 +48,10 @@ public sealed class BSPrim : BSPhysObject
 
     private PrimitiveBaseShape _pbs;
 
-    // _size is what the user passed. _scale is what we pass to the physics engine with the mesh.
-    // Often _scale is unity because the meshmerizer will apply _size when creating the mesh.
+    // _size is what the user passed. Scale is what we pass to the physics engine with the mesh.
+    // Often Scale is unity because the meshmerizer will apply _size when creating the mesh.
     private OMV.Vector3 _size;  // the multiplier for each mesh dimension as passed by the user
-    private OMV.Vector3 _scale; // the multiplier for each mesh dimension for the mesh as created by the meshmerizer
+    // private OMV.Vector3 _scale; // the multiplier for each mesh dimension for the mesh as created by the meshmerizer
 
     private bool _grabbed;
     private bool _isSelected;
@@ -98,7 +98,7 @@ public sealed class BSPrim : BSPhysObject
         _physicsActorType = (int)ActorTypes.Prim;
         _position = pos;
         _size = size;
-        _scale = new OMV.Vector3(1f, 1f, 1f);   // the scale will be set by CreateGeom depending on object type
+        Scale = new OMV.Vector3(1f, 1f, 1f);   // the scale will be set by CreateGeom depending on object type
         _orientation = rotation;
         _buoyancy = 1f;
         _velocity = OMV.Vector3.Zero;
@@ -166,7 +166,7 @@ public sealed class BSPrim : BSPhysObject
                 // Since _size changed, the mesh needs to be rebuilt. If rebuilt, all the correct
                 //   scale and margins are set.
                 CreateGeomAndObject(true);
-                // DetailLog("{0},BSPrim.setSize,size={1},scale={2},mass={3},physical={4}", LocalID, _size, _scale, _mass, IsPhysical);
+                // DetailLog("{0},BSPrim.setSize,size={1},scale={2},mass={3},physical={4}", LocalID, _size, Scale, _mass, IsPhysical);
             });
         }
     }
@@ -1224,7 +1224,8 @@ public sealed class BSPrim : BSPhysObject
         shape.Position = _position;
         shape.Rotation = _orientation;
         shape.Velocity = _velocity;
-        shape.Scale = _scale;
+        shape.Size = _size;
+        shape.Scale = Scale;
         shape.Mass = _isPhysical ? _mass : 0f;
         shape.Buoyancy = _buoyancy;
         shape.HullKey = 0;
@@ -1234,7 +1235,6 @@ public sealed class BSPrim : BSPhysObject
         shape.Collidable = (!IsPhantom) ? ShapeData.numericTrue : ShapeData.numericFalse;
         shape.Static = _isPhysical ? ShapeData.numericFalse : ShapeData.numericTrue;
         shape.Solid = IsSolid ? ShapeData.numericFalse : ShapeData.numericTrue;
-        shape.Size = _size;
     }
     // Rebuild the geometry and object.
     // This is called when the shape changes so we need to recreate the mesh/hull.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index c6e8bc4..ab2835c 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -533,7 +533,7 @@ 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
+        //   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)
@@ -577,9 +577,9 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
         // 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 45 to give a recognizable running rate (45 or less).
-        return numSubSteps * m_fixedTimeStep * 1000 * 45;
-        // return timeStep * 1000 * 45;
+        // We multiply by 55 to give a recognizable running rate (55 or less).
+        return numSubSteps * m_fixedTimeStep * 1000 * 55;
+        // return timeStep * 1000 * 55;
     }
 
     // Something has collided
@@ -795,7 +795,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
     delegate void ParamUser(BSScene scene, IConfig conf, string paramName, float val);
     delegate float ParamGet(BSScene scene);
-    delegate void ParamSet(BSScene scene, string paramName, uint localID, float val);

+    delegate void ParamSet(BSScene scene, string paramName, uint localID, float val);
     delegate void SetOnObject(BSScene scene, BSPhysObject obj, float val);
 
     private struct ParameterDefn
@@ -805,7 +805,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         public float defaultValue;  // default value if not specified anywhere else
         public ParamUser userParam; // get the value from the configuration file
         public ParamGet getter;     // return the current value stored for this parameter
-        public ParamSet setter;     // set the current value for this parameter

+        public ParamSet setter;     // set the current value for this parameter
         public SetOnObject onObject;    // set the value on an object in the physical domain
         public ParameterDefn(string n, string d, float v, ParamUser u, ParamGet g, ParamSet s)
         {
@@ -814,7 +814,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             defaultValue = v;
             userParam = u;
             getter = g;
-            setter = s;

+            setter = s;
             onObject = null;
         }
         public ParameterDefn(string n, string d, float v, ParamUser u, ParamGet g, ParamSet s, SetOnObject o)
@@ -824,7 +824,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             defaultValue = v;
             userParam = u;
             getter = g;
-            setter = s;

+            setter = s;
             onObject = o;
         }
     }
@@ -1266,13 +1266,13 @@ public class BSScene : PhysicsScene, IPhysicsParameters
                 objectIDs.Add(TERRAIN_ID);
                 TaintedUpdateParameter(parm, objectIDs, val);
                 break;
-            case PhysParameterEntry.APPLY_TO_ALL:

+            case PhysParameterEntry.APPLY_TO_ALL:
                 defaultLoc = val;  // setting ALL also sets the default value
-                lock (PhysObjects) objectIDs = new List<uint>(PhysObjects.Keys);

+                lock (PhysObjects) objectIDs = new List<uint>(PhysObjects.Keys);
                 TaintedUpdateParameter(parm, objectIDs, val);
                 break;
             default:
-                // setting only one localID

+                // setting only one localID
                 objectIDs.Add(localID);
                 TaintedUpdateParameter(parm, objectIDs, val);
                 break;
@@ -1282,8 +1282,8 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     // schedule the actual updating of the paramter to when the phys engine is not busy
     protected void TaintedUpdateParameter(string parm, List<uint> lIDs, float val)
     {
-        float xval = val;

-        List<uint> xlIDs = lIDs;

+        float xval = val;
+        List<uint> xlIDs = lIDs;
         string xparm = parm;
         TaintedObject("BSScene.UpdateParameterSet", delegate() {
             ParameterDefn thisParam;
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
index bbfdac6..3d15eaa 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
@@ -417,7 +417,10 @@ public class BSShapeCollection : IDisposable
 
         if (shapeType == ShapeData.PhysicsShapeType.SHAPE_AVATAR)
         {
-            newShape = new BulletShape(BulletSimAPI.BuildCapsuleShape2(PhysicsScene.World.ptr, shapeData), shapeType);
+            // 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);
             newShape.shapeKey = (System.UInt64)shapeKey;
             newShape.isNativeShape = true;
         }
@@ -428,7 +431,7 @@ public class BSShapeCollection : IDisposable
             newShape.isNativeShape = true;
         }
 
-        // Don't need to do a 'ReferenceShape()' here because native shapes are not tracked.
+        // Don't need to do a 'ReferenceShape()' here because native shapes are not shared.
         // DetailLog("{0},BSShapeCollection.AddNativeShapeToPrim,create,newshape={1}", shapeData.ID, newShape);
 
         prim.BSShape = newShape;
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
index 30754a7..24d8db6 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
@@ -608,7 +608,7 @@ public static extern IntPtr BuildNativeShape2(IntPtr world, ShapeData shapeData)
 public static extern bool IsNativeShape2(IntPtr shape);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern IntPtr BuildCapsuleShape2(IntPtr world, ShapeData shapeData);
+public static extern IntPtr BuildCapsuleShape2(IntPtr world, float radius, float height, Vector3 scale);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern IntPtr CreateCompoundShape2(IntPtr sim);