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)