BulletSim: fix density since the simulator/viewer track density in a

funny unit that is 100 times real density (default 1000).
Fix avatar drifting slowly when stationary flying.
Fix for physical prims getting corrected for being under terrain when it was
just its geometric center that was below terrain.
Add PreUpdatePropertyAction allowing plugable modifiction of phys
parameters returned from Bullet.
Fix an exception setting GravityMultiplier on initialization.
Update DLLs and SOs for good measure (no functional change).

7 files changed, 150 insertions, 25 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index 04fb05b..8dca7c6 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -83,7 +83,7 @@ public sealed class BSCharacter : BSPhysObject
         _velocity = OMV.Vector3.Zero;
         _buoyancy = ComputeBuoyancyFromFlying(isFlying);
         Friction = BSParam.AvatarStandingFriction;
-        Density = BSParam.AvatarDensity;
+        Density = BSParam.AvatarDensity / BSParam.DensityScaleFactor;
 
         // Old versions of ScenePresence passed only the height. If width and/or depth are zero,
         //     replace with the default values.
@@ -231,6 +231,15 @@ public sealed class BSCharacter : BSPhysObject
                         PhysicsScene.PE.SetFriction(PhysBody, Friction);
                     }
                 }
+                else
+                {
+                    if (Flying)
+                    {
+                        // Flying and not collising and velocity nearly zero.
+                        ZeroMotion(true /* inTaintTime */);
+                    }
+                }
+
                 DetailLog("{0},BSCharacter.MoveMotor,taint,stopping,target={1},colliding={2}", LocalID, _velocityMotor.TargetValue, IsColliding);
             }
             else
@@ -869,7 +878,7 @@ public sealed class BSCharacter : BSPhysObject
                         * Math.Min(Size.X, Size.Y) / 2
                         * Size.Y / 2f    // plus the volume of the capsule end caps
                         );
-        _mass = Density * _avatarVolume;
+        _mass = Density * BSParam.DensityScaleFactor * _avatarVolume;
     }
 
     // The physics engine says that properties have updated. Update same and inform
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
index 1f66b56..4ce58c7 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
@@ -399,7 +399,7 @@ public sealed class BSLinksetCompound : BSLinkset
     // Constraint linksets are rebuilt every time.
     // Note that this works for rebuilding just the root after a linkset is taken apart.
     // Called at taint time!!
-    private bool disableCOM = false;     // DEBUG DEBUG: disable until we get this debugged
+    private bool disableCOM = true;     // DEBUG DEBUG: disable until we get this debugged
     private void RecomputeLinksetCompound()
     {
         try
@@ -430,10 +430,10 @@ public sealed class BSLinksetCompound : BSLinkset
             LinksetRoot.ForcePosition = LinksetRoot.RawPosition;
 
             // Update the local transform for the root child shape so it is offset from the <0,0,0> which is COM
-            PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, 0,
-                                        -centerDisplacement, 
-                                        LinksetRoot.RawOrientation,
-                                        false /* shouldRecalculateLocalAabb */);
+            PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, 0 /* childIndex */,
+                                                -centerDisplacement,
+                                                OMV.Quaternion.Identity, // LinksetRoot.RawOrientation,
+                                                false /* shouldRecalculateLocalAabb (is done later after linkset built) */);
 
             DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,COM,com={1},rootPos={2},centerDisp={3}",
                                     LinksetRoot.LocalID, centerOfMassW, LinksetRoot.RawPosition, centerDisplacement);
@@ -463,7 +463,6 @@ public sealed class BSLinksetCompound : BSLinkset
                         //    Use call to CreateGeomNonSpecial().
                         BulletShape saveShape = cPrim.PhysShape;
                         cPrim.PhysShape.Clear();        // Don't let the create free the child's shape
-                        // PhysicsScene.Shapes.CreateGeomNonSpecial(true, cPrim, null);
                         PhysicsScene.Shapes.CreateGeomMeshOrHull(cPrim, null);
                         BulletShape newShape = cPrim.PhysShape;
                         cPrim.PhysShape = saveShape;
@@ -471,6 +470,8 @@ public sealed class BSLinksetCompound : BSLinkset
                         OMV.Vector3 offsetPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation - centerDisplacement;
                         OMV.Quaternion offsetRot = cPrim.RawOrientation * invRootOrientation;
                         PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, newShape, offsetPos, offsetRot);
+                        DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addNative,indx={1},rShape={2},cShape={3},offPos={4},offRot={5}",
+                                    LinksetRoot.LocalID, memberIndex, LinksetRoot.PhysShape, newShape, offsetPos, offsetRot);
                     }
                     else
                     {
@@ -484,7 +485,10 @@ public sealed class BSLinksetCompound : BSLinkset
                         }
                         OMV.Vector3 offsetPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation - centerDisplacement;
                         OMV.Quaternion offsetRot = cPrim.RawOrientation * invRootOrientation;
-                        PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, cPrim.PhysShape, offsetPos, offsetRot);
+                        PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, cPrim.PhysShape, offsetPos, offsetRot);

+                        DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addNonNative,indx={1},rShape={2},cShape={3},offPos={4},offRot={5}",

+                                    LinksetRoot.LocalID, memberIndex, LinksetRoot.PhysShape, cPrim.PhysShape, offsetPos, offsetRot);
+
                     }
                     memberIndex++;
                 }
@@ -503,6 +507,7 @@ public sealed class BSLinksetCompound : BSLinkset
             Rebuilding = false;
         }
 
+        // See that the Aabb surrounds the new shape
         PhysicsScene.PE.RecalculateCompoundShapeLocalAabb(LinksetRoot.PhysShape);
     }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
index 3e0b4bc..329169f 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
@@ -49,6 +49,7 @@ public static class BSParam
     public static float MaxLinearVelocity { get; private set; }
     public static float MaxAngularVelocity { get; private set; }
     public static float MaxAddForceMagnitude { get; private set; }
+    public static float DensityScaleFactor { get; private set; }
 
     public static float LinearDamping { get; private set; }
     public static float AngularDamping { get; private set; }
@@ -281,29 +282,35 @@ public static class BSParam
         new ParameterDefn("MinObjectMass", "Minimum object mass (0.0001)",
             0.0001f,
             (s,cf,p,v) => { MinimumObjectMass = cf.GetFloat(p, v); },
-            (s) => { return (float)MinimumObjectMass; },
+            (s) => { return MinimumObjectMass; },
             (s,p,l,v) => { MinimumObjectMass = v; } ),
         new ParameterDefn("MaxObjectMass", "Maximum object mass (10000.01)",
             10000.01f,
             (s,cf,p,v) => { MaximumObjectMass = cf.GetFloat(p, v); },
-            (s) => { return (float)MaximumObjectMass; },
+            (s) => { return MaximumObjectMass; },
             (s,p,l,v) => { MaximumObjectMass = v; } ),
         new ParameterDefn("MaxLinearVelocity", "Maximum velocity magnitude that can be assigned to an object",
             1000.0f,
             (s,cf,p,v) => { MaxLinearVelocity = cf.GetFloat(p, v); },
-            (s) => { return (float)MaxLinearVelocity; },
+            (s) => { return MaxLinearVelocity; },
             (s,p,l,v) => { MaxLinearVelocity = v; } ),
         new ParameterDefn("MaxAngularVelocity", "Maximum rotational velocity magnitude that can be assigned to an object",
             1000.0f,
             (s,cf,p,v) => { MaxAngularVelocity = cf.GetFloat(p, v); },
-            (s) => { return (float)MaxAngularVelocity; },
+            (s) => { return MaxAngularVelocity; },
             (s,p,l,v) => { MaxAngularVelocity = v; } ),
         // LL documentation says thie number should be 20f for llApplyImpulse and 200f for llRezObject
         new ParameterDefn("MaxAddForceMagnitude", "Maximum force that can be applied by llApplyImpulse (SL says 20f)",
             20000.0f,
             (s,cf,p,v) => { MaxAddForceMagnitude = cf.GetFloat(p, v); },
-            (s) => { return (float)MaxAddForceMagnitude; },
+            (s) => { return MaxAddForceMagnitude; },
             (s,p,l,v) => { MaxAddForceMagnitude = v; } ),
+        // Density is passed around as 100kg/m3. This scales that to 1kg/m3.
+        new ParameterDefn("DensityScaleFactor", "Conversion for simulator/viewer density (100kg/m3) to physical density (1kg/m3)",
+            0.01f,
+            (s,cf,p,v) => { DensityScaleFactor = cf.GetFloat(p, v); },
+            (s) => { return DensityScaleFactor; },
+            (s,p,l,v) => { DensityScaleFactor = v; } ),
 
         new ParameterDefn("PID_D", "Derivitive factor for motion smoothing",
             2200f,
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index 8ebb532..f953c1e 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -99,6 +99,9 @@ public abstract class BSPhysObject : PhysicsActor
         CollisionAccumulation = 0;
         ColliderIsMoving = false;
         CollisionScore = 0;
+
+        // All axis free.
+        LockedAxis = LockedAxisFree;
     }
 
     // Tell the object to clean up.
@@ -172,7 +175,8 @@ public abstract class BSPhysObject : PhysicsActor
         MaterialAttributes matAttrib = BSMaterials.GetAttributes(Material, false);
         Friction = matAttrib.friction;
         Restitution = matAttrib.restitution;
-        Density = matAttrib.density;
+        Density = matAttrib.density / BSParam.DensityScaleFactor;
+        DetailLog("{0},{1}.SetMaterial,Mat={2},frict={3},rest={4},den={5}", LocalID, TypeName, Material, Friction, Restitution, Density);
     }
 
     // Stop all physical motion.
@@ -220,6 +224,9 @@ public abstract class BSPhysObject : PhysicsActor
     //    computed center-of-mass (like in linksets).
     public OMV.Vector3? UserSetCenterOfMass { get; set; }
 
+    public OMV.Vector3 LockedAxis { get; set; } // zero means locked. one means free.
+    public readonly OMV.Vector3 LockedAxisFree = new OMV.Vector3(1f, 1f, 1f);  // All axis are free
+
     #region Collisions
 
     // Requested number of milliseconds between collision events. Zero means disabled.
@@ -416,9 +423,7 @@ public abstract class BSPhysObject : PhysicsActor
         {
             // Clean out any existing action
             UnRegisterPreStepAction(op, id);
-
             RegisteredPrestepActions[identifier] = actn;
-
             PhysicsScene.BeforeStep += actn;
         }
         DetailLog("{0},BSPhysObject.RegisterPreStepAction,id={1}", LocalID, identifier);
@@ -464,9 +469,7 @@ public abstract class BSPhysObject : PhysicsActor
         {
             // Clean out any existing action
             UnRegisterPostStepAction(op, id);
-
             RegisteredPoststepActions[identifier] = actn;
-
             PhysicsScene.AfterStep += actn;
         }
         DetailLog("{0},BSPhysObject.RegisterPostStepAction,id={1}", LocalID, identifier);
@@ -503,7 +506,58 @@ public abstract class BSPhysObject : PhysicsActor
         }
         DetailLog("{0},BSPhysObject.UnRegisterAllPostStepActions,", LocalID);
     }
-    
+
+    // When an update to the physical properties happens, this event is fired to let
+    //    different actors to modify the update before it is passed around
+    public delegate void PreUpdatePropertyAction(ref EntityProperties entprop);
+    public event PreUpdatePropertyAction OnPreUpdateProperty;
+    protected void TriggerPreUpdatePropertyAction(ref EntityProperties entprop)
+    {
+        PreUpdatePropertyAction actions = OnPreUpdateProperty;
+        if (actions != null)
+            actions(ref entprop);
+    }
+
+    private Dictionary<string, PreUpdatePropertyAction> RegisteredPreUpdatePropertyActions = new Dictionary<string, PreUpdatePropertyAction>();
+    public void RegisterPreUpdatePropertyAction(string identifier, PreUpdatePropertyAction actn)
+    {
+        lock (RegisteredPreUpdatePropertyActions)
+        {
+            // Clean out any existing action
+            UnRegisterPreUpdatePropertyAction(identifier);
+            RegisteredPreUpdatePropertyActions[identifier] = actn;
+            OnPreUpdateProperty += actn;
+        }
+        DetailLog("{0},BSPhysObject.RegisterPreUpdatePropertyAction,id={1}", LocalID, identifier);
+    }
+    public bool UnRegisterPreUpdatePropertyAction(string identifier)
+    {
+        bool removed = false;
+        lock (RegisteredPreUpdatePropertyActions)
+        {
+            if (RegisteredPreUpdatePropertyActions.ContainsKey(identifier))
+            {
+                OnPreUpdateProperty -= RegisteredPreUpdatePropertyActions[identifier];
+                RegisteredPreUpdatePropertyActions.Remove(identifier);
+                removed = true;
+            }
+        }
+        DetailLog("{0},BSPhysObject.UnRegisterPreUpdatePropertyAction,id={1},removed={2}", LocalID, identifier, removed);
+        return removed;
+    }
+    public void UnRegisterAllPreUpdatePropertyActions()
+    {
+        lock (RegisteredPreUpdatePropertyActions)
+        {
+            foreach (KeyValuePair<string, PreUpdatePropertyAction> kvp in RegisteredPreUpdatePropertyActions)
+            {
+                OnPreUpdateProperty -= kvp.Value;
+            }
+            RegisteredPreUpdatePropertyActions.Clear();
+        }
+        DetailLog("{0},BSPhysObject.UnRegisterAllPreUpdatePropertyAction,", LocalID);
+    }
+
     #endregion // Per Simulation Step actions
 
     // High performance detailed logging routine used by the physical objects.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index a76f8b9..0323b0d 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -242,6 +242,45 @@ public class BSPrim : BSPhysObject
     public override void LockAngularMotion(OMV.Vector3 axis)
     {
         DetailLog("{0},BSPrim.LockAngularMotion,call,axis={1}", LocalID, axis);
+
+        OMV.Vector3 locking = new OMV.Vector3(1f, 1f, 1f);
+        if (axis.X != 1) locking.X = 0f;
+        if (axis.Y != 1) locking.Y = 0f;
+        if (axis.Z != 1) locking.Z = 0f;
+        LockedAxis = locking;
+
+        /*  Not implemented yet
+        if (LockedAxis != LockedAxisFree)
+        {
+            // Something is locked so start the thingy that keeps that axis from changing
+            RegisterPreUpdatePropertyAction("BSPrim.LockAngularMotion", delegate(ref EntityProperties entprop)
+            {
+                if (LockedAxis != LockedAxisFree)
+                {
+                    if (IsPhysicallyActive)
+                    {
+                        // Bullet can lock axis but it only works for global axis.
+                        // Check if this prim is aligned on global axis and use Bullet's
+                        //    system if so.
+
+                        ForceOrientation = entprop.Rotation;
+                        ForceRotationalVelocity = entprop.RotationalVelocity;
+                    }
+                }
+                else
+                {
+                    UnRegisterPreUpdatePropertyAction("BSPrim.LockAngularMotion");
+                }
+
+            });
+        }
+        else
+        {
+            // Everything seems unlocked
+            UnRegisterPreUpdatePropertyAction("BSPrim.LockAngularMotion");
+        }
+         */
+
         return;
     }
 
@@ -311,7 +350,8 @@ public class BSPrim : BSPhysObject
 
         float terrainHeight = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(RawPosition);
         OMV.Vector3 upForce = OMV.Vector3.Zero;
-        if (RawPosition.Z < terrainHeight)
+        float approxSize = Math.Max(Size.X, Math.Max(Size.Y, Size.Z));
+        if ((RawPosition.Z + approxSize / 2f) < terrainHeight)
         {
             DetailLog("{0},BSPrim.PositionAdjustUnderGround,call,pos={1},terrain={2}", LocalID, RawPosition, terrainHeight);
             float targetHeight = terrainHeight + (Size.Z / 2f);
@@ -576,6 +616,8 @@ public class BSPrim : BSPhysObject
             }
         }
     }
+    // The simulator/viewer keep density as 100kg/m3.
+    // Remember to use BSParam.DensityScaleFactor to create the physical density.
     public override float Density
     {
         get { return base.Density; }
@@ -1569,7 +1611,8 @@ public class BSPrim : BSPhysObject
         profileEnd = 1.0f - (float)BaseShape.ProfileEnd * 2.0e-5f;
         volume *= (profileEnd - profileBegin);
 
-        returnMass = Density * volume;
+        returnMass = Density * BSParam.DensityScaleFactor * volume;
+        DetailLog("{0},BSPrim.CalculateMass,den={1},vol={2},mass={3}", LocalID, Density, volume, returnMass);
 
         returnMass = Util.Clamp(returnMass, BSParam.MinimumObjectMass, BSParam.MaximumObjectMass);
 
@@ -1607,6 +1650,8 @@ public class BSPrim : BSPhysObject
     // the world that things have changed.
     public override void UpdateProperties(EntityProperties entprop)
     {
+        TriggerPreUpdatePropertyAction(ref entprop);
+
         // A temporary kludge to suppress the rotational effects introduced on vehicles by Bullet
         // TODO: handle physics introduced by Bullet with computed vehicle physics.
         if (VehicleController.IsActive)
@@ -1619,7 +1664,11 @@ public class BSPrim : BSPhysObject
         // Assign directly to the local variables so the normal set actions do not happen
         _position = entprop.Position;
         _orientation = entprop.Rotation;
-        _velocity = entprop.Velocity;
+        // _velocity = entprop.Velocity;
+        // DEBUG DEBUG DEBUG -- smooth velocity changes a bit. The simulator seems to be
+        //    very sensitive to velocity changes.
+        if (!entprop.Velocity.ApproxEquals(_velocity, 0.1f))
+            _velocity = entprop.Velocity;
         _acceleration = entprop.Acceleration;
         _rotationalVelocity = entprop.RotationalVelocity;
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
index b9f2cca..f1c3b5c 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
@@ -144,7 +144,7 @@ public class BSPrimDisplaced : BSPrim
             // Correct for any rotation around the center-of-mass
             // TODO!!!
             entprop.Position = entprop.Position + (PositionDisplacement * entprop.Rotation);
-            entprop.Rotation = something;
+            // entprop.Rotation = something;
         }
 
         base.UpdateProperties(entprop);
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs
index 96f9762..d65d407 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs
@@ -135,7 +135,8 @@ public class BSPrimLinkable : BSPrimDisplaced
         // When going from non-physical to physical, this re-enables the constraints that
         //     had been automatically disabled when the mass was set to zero.
         // For compound based linksets, this enables and disables interactions of the children.
-        Linkset.Refresh(this);
+        if (Linkset != null)    // null can happen during initialization
+            Linkset.Refresh(this);
     }
 
     protected override void MakeDynamic(bool makeStatic)