9 files changed, 304 insertions, 141 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs b/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
index 7ab86d2..3f83ef0 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs
@@ -225,9 +225,10 @@ public enum CollisionFlags : uint
     CF_DISABLE_VISUALIZE_OBJECT      = 1 << 5,
     CF_DISABLE_SPU_COLLISION_PROCESS = 1 << 6,
     // Following used by BulletSim to control collisions and updates
-    BS_SUBSCRIBE_COLLISION_EVENTS    = 1 << 10,
-    BS_FLOATS_ON_WATER               = 1 << 11,
-    BS_VEHICLE_COLLISIONS            = 1 << 12,
+    BS_SUBSCRIBE_COLLISION_EVENTS    = 1 << 10, // return collision events from unmanaged to managed
+    BS_FLOATS_ON_WATER               = 1 << 11, // the object should float at water level
+    BS_VEHICLE_COLLISIONS            = 1 << 12, // return collisions for vehicle ground checking
+    BS_RETURN_ROOT_COMPOUND_SHAPE    = 1 << 13, // return the pos/rot of the root shape in a compound shape
     BS_NONE                          = 0,
     BS_ALL                           = 0xFFFFFFFF
 };
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index f781aea..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
@@ -274,7 +283,7 @@ public sealed class BSCharacter : BSPhysObject
         // 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 (IsColliding && !Flying && TargetSpeed > 0.1f /* && ForwardSpeed < 0.1f */)
+        if (IsColliding && !Flying && TargetVelocitySpeed > 0.1f /* && ForwardSpeed < 0.1f */)
         {
             // The range near the character's feet where we will consider stairs
             float nearFeetHeightMin = RawPosition.Z - (Size.Z / 2f) + 0.05f;
@@ -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/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index e35311f..4ece1eb 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -127,6 +127,8 @@ public abstract class BSLinkset
         m_children = new HashSet<BSPrimLinkable>();
         LinksetMass = parent.RawMass;
         Rebuilding = false;
+
+        parent.ClearDisplacement();
     }
 
     // Link to a linkset where the child knows the parent.
@@ -280,6 +282,7 @@ public abstract class BSLinkset
         return mass;
     }
 
+    // Computes linkset's center of mass in world coordinates.
     protected virtual OMV.Vector3 ComputeLinksetCenterOfMass()
     {
         OMV.Vector3 com;
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
index 36bae9b..4ce58c7 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
@@ -93,7 +93,8 @@ public sealed class BSLinksetCompound : BSLinkset
 {
     private static string LogHeader = "[BULLETSIM LINKSET COMPOUND]";
 
-    public BSLinksetCompound(BSScene scene, BSPrimLinkable parent) : base(scene, parent)
+    public BSLinksetCompound(BSScene scene, BSPrimLinkable parent)
+        : base(scene, parent)
     {
     }
 
@@ -217,59 +218,45 @@ public sealed class BSLinksetCompound : BSLinkset
             //     and that is caused by us updating the object.
             if ((whichUpdated & ~(UpdatedProperties.Position | UpdatedProperties.Orientation)) == 0)
             {
-                // Gather the child info. It might not be there if the linkset is in transition.
-                BSLinksetCompoundInfo lsi = updated.LinksetInfo as BSLinksetCompoundInfo;
-                if (lsi != null)
-                {
-                    // Since the child moved or rotationed, it needs a new relative position within the linkset
-                    BSLinksetCompoundInfo newLsi = new BSLinksetCompoundInfo(lsi.Index, LinksetRoot, updated, OMV.Vector3.Zero);
-                    updated.LinksetInfo = newLsi;
-
                     // Find the physical instance of the child 
-                    if (LinksetRoot.PhysShape.HasPhysicalShape && PhysicsScene.PE.IsCompound(LinksetRoot.PhysShape))
+                if (LinksetRoot.PhysShape.HasPhysicalShape && PhysicsScene.PE.IsCompound(LinksetRoot.PhysShape))
+                {
+                    // It is possible that the linkset is still under construction and the child is not yet
+                    //    inserted into the compound shape. A rebuild of the linkset in a pre-step action will
+                    //    build the whole thing with the new position or rotation.
+                    // The index must be checked because Bullet references the child array but does no validity
+                    //    checking of the child index passed.
+                    int numLinksetChildren = PhysicsScene.PE.GetNumberOfCompoundChildren(LinksetRoot.PhysShape);
+                    if (updated.LinksetChildIndex < numLinksetChildren)
                     {
-                        // It is possible that the linkset is still under construction and the child is not yet
-                        //    inserted into the compound shape. A rebuild of the linkset in a pre-step action will
-                        //    build the whole thing with the new position or rotation.
-                        // The index must be checked because Bullet references the child array but does no validity
-                        //    checking of the child index passed.
-                        int numLinksetChildren = PhysicsScene.PE.GetNumberOfCompoundChildren(LinksetRoot.PhysShape);
-                        if (lsi.Index < numLinksetChildren)
+                        BulletShape linksetChildShape = PhysicsScene.PE.GetChildShapeFromCompoundShapeIndex(LinksetRoot.PhysShape, updated.LinksetChildIndex);
+                        if (linksetChildShape.HasPhysicalShape)
                         {
-                            BulletShape linksetChildShape = PhysicsScene.PE.GetChildShapeFromCompoundShapeIndex(LinksetRoot.PhysShape, lsi.Index);
-                            if (linksetChildShape.HasPhysicalShape)
-                            {
-                                // Found the child shape within the compound shape
-                                PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, lsi.Index,
-                                                                            newLsi.OffsetFromCenterOfMass,
-                                                                            newLsi.OffsetRot,
-                                                                            true /* shouldRecalculateLocalAabb */);
-                                updatedChild = true;
-                                DetailLog("{0},BSLinksetCompound.UpdateProperties,changeChildPosRot,whichUpdated={1},newLsi={2}",
-                                                                            updated.LocalID, whichUpdated, newLsi);
-                            }
-                            else    // DEBUG DEBUG
-                            {       // DEBUG DEBUG
-                                DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,noChildShape,shape={1}",
-                                                                            updated.LocalID, linksetChildShape);
-                            }       // DEBUG DEBUG
+                            // Found the child shape within the compound shape
+                            PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, updated.LinksetChildIndex,
+                                                                        updated.RawPosition - LinksetRoot.RawPosition,
+                                                                        updated.RawOrientation * OMV.Quaternion.Inverse(LinksetRoot.RawOrientation),
+                                                                        true /* shouldRecalculateLocalAabb */);
+                            updatedChild = true;
+                            DetailLog("{0},BSLinksetCompound.UpdateProperties,changeChildPosRot,whichUpdated={1},pos={2},rot={3}",
+                                                        updated.LocalID, whichUpdated, updated.RawPosition, updated.RawOrientation);
                         }
                         else    // DEBUG DEBUG
                         {       // DEBUG DEBUG
-                            // the child is not yet in the compound shape. This is non-fatal.
-                            DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,childNotInCompoundShape,numChildren={1},index={2}",
-                                                                        updated.LocalID, numLinksetChildren, lsi.Index);
+                            DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,noChildShape,shape={1}",
+                                                                        updated.LocalID, linksetChildShape);
                         }       // DEBUG DEBUG
                     }
                     else    // DEBUG DEBUG
                     {       // DEBUG DEBUG
-                        DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,noBodyOrNotCompound", updated.LocalID);
+                        // the child is not yet in the compound shape. This is non-fatal.
+                        DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,childNotInCompoundShape,numChildren={1},index={2}",
+                                                                    updated.LocalID, numLinksetChildren, updated.LinksetChildIndex);
                     }       // DEBUG DEBUG
                 }
                 else    // DEBUG DEBUG
                 {       // DEBUG DEBUG
-                    DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,noLinkSetInfo,rootPhysShape={1}",
-                                                    updated.LocalID, LinksetRoot.PhysShape);
+                    DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,noBodyOrNotCompound", updated.LocalID);
                 }       // DEBUG DEBUG
 
                 if (!updatedChild)
@@ -379,6 +366,8 @@ public sealed class BSLinksetCompound : BSLinkset
     // Safe to call even if the child is not really in the linkset.
     protected override void RemoveChildFromLinkset(BSPrimLinkable child)
     {
+        child.ClearDisplacement();
+
         if (m_children.Remove(child))
         {
             DetailLog("{0},BSLinksetCompound.RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}",
@@ -424,30 +413,31 @@ public sealed class BSLinksetCompound : BSLinkset
             // The center of mass for the linkset is the geometric center of the group.
             // Compute a displacement for each component so it is relative to the center-of-mass.
             // Bullet presumes an object's origin (relative <0,0,0>) is its center-of-mass
-            OMV.Vector3 centerOfMass;
-            OMV.Vector3 centerDisplacement = OMV.Vector3.Zero;
-            if (disableCOM)                             // DEBUG DEBUG
-            {                                           // DEBUG DEBUG
-                centerOfMass = LinksetRoot.RawPosition; // DEBUG DEBUG
-                // LinksetRoot.PositionDisplacement = OMV.Vector3.Zero;
-            }                                           // DEBUG DEBUG
-            else
+            OMV.Vector3 centerOfMassW = LinksetRoot.RawPosition;
+            if (!disableCOM)                             // DEBUG DEBUG
             {
-                centerOfMass = ComputeLinksetCenterOfMass();
-                // 'centerDisplacement' is the value to *add* to all the shape offsets
-                centerDisplacement = LinksetRoot.RawPosition - centerOfMass;
-
-                // Since we're displacing the center of the shape, we need to move the body in the world
-                // LinksetRoot.PositionDisplacement = centerDisplacement;
-
-                // This causes the root prim position to be set properly based on the new PositionDisplacement
-                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, OMV.Quaternion.Identity, false);
-                DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,COM,com={1},rootPos={2},centerDisp={3}",
-                                        LinksetRoot.LocalID, centerOfMass, LinksetRoot.RawPosition, centerDisplacement);
+                // Compute a center-of-mass in world coordinates.
+                centerOfMassW = ComputeLinksetCenterOfMass();
             }
 
+            OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(LinksetRoot.RawOrientation);
+
+            // 'centerDisplacement' is the value to subtract from children to give physical offset position
+            OMV.Vector3 centerDisplacement = (centerOfMassW - LinksetRoot.RawPosition) * invRootOrientation;
+            LinksetRoot.SetEffectiveCenterOfMassW(centerDisplacement);
+
+            // This causes the physical position of the root prim to be offset to accomodate for the displacements
+            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 /* 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);
+
             DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,start,rBody={1},rShape={2},numChildren={3}",
                             LinksetRoot.LocalID, LinksetRoot.PhysBody, LinksetRoot.PhysShape, NumberOfChildren);
 
@@ -455,38 +445,33 @@ public sealed class BSLinksetCompound : BSLinkset
             int memberIndex = 1;
             ForEachMember(delegate(BSPrimLinkable cPrim)
             {
-                if (!IsRoot(cPrim))
+                if (IsRoot(cPrim))
                 {
-                    // Compute the displacement of the child from the root of the linkset.
-                    // This info is saved in the child prim so the relationship does not
-                    //    change over time and the new child position can be computed
-                    //    when the linkset is being disassembled (the linkset may have moved).
-                    BSLinksetCompoundInfo lci = cPrim.LinksetInfo as BSLinksetCompoundInfo;
-                    if (lci == null)
-                    {
-                        lci = new BSLinksetCompoundInfo(memberIndex, LinksetRoot, cPrim, centerDisplacement);
-                        cPrim.LinksetInfo = lci;
-                        DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,creatingRelPos,lci={1}", cPrim.LocalID, lci);
-                    }
-
-                    DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addMemberToShape,mID={1},mShape={2},lci={3}",
-                                                LinksetRoot.LocalID, cPrim.LocalID, cPrim.PhysShape, lci);
+                    cPrim.LinksetChildIndex = 0;
+                }
+                else
+                {
+                    cPrim.LinksetChildIndex = memberIndex;
 
                     if (cPrim.PhysShape.isNativeShape)
                     {
                         // A native shape is turned into a hull collision shape because native
                         //    shapes are not shared so we have to hullify it so it will be tracked
                         //    and freed at the correct time. This also solves the scaling problem
-                        //    (native shapes scaled but hull/meshes are assumed to not be).
+                        //    (native shapes scale but hull/meshes are assumed to not be).
                         // TODO: decide of the native shape can just be used in the compound shape.
                         //    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;
-                        PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, newShape, lci.OffsetFromCenterOfMass, lci.OffsetRot);
+
+                        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
                     {
@@ -498,9 +483,13 @@ public sealed class BSLinksetCompound : BSLinkset
                             PhysicsScene.Logger.ErrorFormat("{0} Rebuilt sharable shape when building linkset! Region={1}, primID={2}, shape={3}",
                                                 LogHeader, PhysicsScene.RegionName, cPrim.LocalID, cPrim.PhysShape);
                         }
-                        PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, cPrim.PhysShape, lci.OffsetFromCenterOfMass, lci.OffsetRot);
+                        OMV.Vector3 offsetPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation - centerDisplacement;
+                        OMV.Quaternion offsetRot = cPrim.RawOrientation * invRootOrientation;
+                        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);
+
                     }
-                    lci.Index = memberIndex;
                     memberIndex++;
                 }
                 return false;   // 'false' says to move onto the next child in the list
@@ -509,12 +498,16 @@ public sealed class BSLinksetCompound : BSLinkset
             // With all of the linkset packed into the root prim, it has the mass of everyone.
             LinksetMass = ComputeLinksetMass();
             LinksetRoot.UpdatePhysicalMassProperties(LinksetMass, true);
+
+            // Enable the physical position updator to return the position and rotation of the root shape
+            PhysicsScene.PE.AddToCollisionFlags(LinksetRoot.PhysBody, CollisionFlags.BS_RETURN_ROOT_COMPOUND_SHAPE);
         }
         finally
         {
             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 de69fa0..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.
@@ -136,6 +139,7 @@ public abstract class BSPhysObject : PhysicsActor
 
     // The objects base shape information. Null if not a prim type shape.
     public PrimitiveBaseShape BaseShape { get; protected set; }
+
     // Some types of objects have preferred physical representations.
     // Returns SHAPE_UNKNOWN if there is no preference.
     public virtual BSPhysicsShapeType PreferredPhysicalShape
@@ -150,15 +154,17 @@ public abstract class BSPhysObject : PhysicsActor
     public EntityProperties LastEntityProperties { get; set; }
 
     public virtual OMV.Vector3 Scale { get; set; }
-    public abstract bool IsSolid { get; }
-    public abstract bool IsStatic { get; }
-    public abstract bool IsSelected { get; }
 
     // It can be confusing for an actor to know if it should move or update an object
     //    depeneding on the setting of 'selected', 'physical, ...
     // This flag is the true test -- if true, the object is being acted on in the physical world
     public abstract bool IsPhysicallyActive { get; }
 
+    // Detailed state of the object.
+    public abstract bool IsSolid { get; }
+    public abstract bool IsStatic { get; }
+    public abstract bool IsSelected { get; }
+
     // Materialness
     public MaterialAttributes.Material Material { get; private set; }
     public override void SetMaterial(int material)
@@ -169,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.
@@ -185,14 +192,6 @@ public abstract class BSPhysObject : PhysicsActor
     public abstract OMV.Quaternion RawOrientation { get; set; }
     public abstract OMV.Quaternion ForceOrientation { get; set; }
 
-    public virtual float TargetSpeed
-    {
-        get
-        {
-            OMV.Vector3 characterOrientedVelocity = TargetVelocity * OMV.Quaternion.Inverse(OMV.Quaternion.Normalize(RawOrientation));
-            return characterOrientedVelocity.X;
-        }
-    }
     public abstract OMV.Vector3 RawVelocity { get; set; }
     public abstract OMV.Vector3 ForceVelocity { get; set; }
 
@@ -202,6 +201,7 @@ public abstract class BSPhysObject : PhysicsActor
 
     public virtual bool ForceBodyShapeRebuild(bool inTaintTime) { return false; }
 
+    // The current velocity forward
     public virtual float ForwardSpeed
     {
         get
@@ -210,6 +210,22 @@ public abstract class BSPhysObject : PhysicsActor
             return characterOrientedVelocity.X;
         }
     }
+    // The forward speed we are trying to achieve (TargetVelocity)
+    public virtual float TargetVelocitySpeed
+    {
+        get
+        {
+            OMV.Vector3 characterOrientedVelocity = TargetVelocity * OMV.Quaternion.Inverse(OMV.Quaternion.Normalize(RawOrientation));
+            return characterOrientedVelocity.X;
+        }
+    }
+
+    // The user can optionally set the center of mass. The user's setting will override any
+    //    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
 
@@ -407,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);
@@ -455,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);
@@ -494,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 cf7aa0f..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);
@@ -442,7 +482,7 @@ public class BSPrim : BSPhysObject
                 RegisterPreStepAction("BSPrim.setForce", LocalID,
                     delegate(float timeStep)
                     {
-                        if (!IsPhysicallyActive)
+                        if (!IsPhysicallyActive || _force == OMV.Vector3.Zero)
                         {
                             UnRegisterPreStepAction("BSPrim.setForce", LocalID);
                             return;
@@ -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; }
@@ -647,7 +689,7 @@ public class BSPrim : BSPhysObject
                 RegisterPreStepAction("BSPrim.setTorque", LocalID,
                     delegate(float timeStep)
                     {
-                        if (!IsPhysicallyActive)
+                        if (!IsPhysicallyActive || _torque == OMV.Vector3.Zero)
                         {
                             UnRegisterPreStepAction("BSPrim.setTorque", LocalID);
                             return;
@@ -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 6401308..f1c3b5c 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
@@ -44,72 +44,107 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 {
 public class BSPrimDisplaced : BSPrim
 {
-    // 'Position' and 'Orientation' is what the simulator thinks the positions of the prim is.
-    // Because Bullet needs the zero coordinate to be the center of mass of the linkset,
-    //     sometimes it is necessary to displace the position the physics engine thinks
-    //     the position is. PositionDisplacement must be added and removed from the
-    //     position as the simulator position is stored and fetched from the physics
-    //     engine. Similar to OrientationDisplacement.
+    // The purpose of this module is to do any mapping between what the simulator thinks
+    //    the prim position and orientation is and what the physical position/orientation.
+    //    This difference happens because Bullet assumes the center-of-mass is the <0,0,0>
+    //    of the prim/linkset. The simulator tracks the location of the prim/linkset by
+    //    the location of the root prim. So, if center-of-mass is anywhere but the origin
+    //    of the root prim, the physical origin is displaced from the simulator origin.
+    //
+    // This routine works by capturing the Force* setting of position/orientation/... and
+    //    adjusting the simulator values (being set) into the physical values.
+    //    The conversion is also done in the opposite direction (physical origin -> simulator origin).
+    //
+    // The updateParameter call is also captured and the values from the physics engine
+    //    are converted into simulator origin values before being passed to the base
+    //    class.
+
     public virtual OMV.Vector3 PositionDisplacement { get; set; }
     public virtual OMV.Quaternion OrientationDisplacement { get; set; }
-    public virtual OMV.Vector3 CenterOfMassLocation { get; set; }
-    public virtual OMV.Vector3 GeometricCenterLocation { get; set; }
 
     public BSPrimDisplaced(uint localID, String primName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size,
                        OMV.Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical)
         : base(localID, primName, parent_scene, pos, size, rotation, pbs, pisPhysical)
     {
-        CenterOfMassLocation = RawPosition;
-        GeometricCenterLocation = RawPosition;
+        ClearDisplacement();
     }
 
-    public override Vector3 ForcePosition
+    public void ClearDisplacement()
+    {
+        PositionDisplacement = OMV.Vector3.Zero;
+        OrientationDisplacement = OMV.Quaternion.Identity;
+    }
+
+    // Set this sets and computes the displacement from the passed prim to the center-of-mass.
+    // A user set value for center-of-mass overrides whatever might be passed in here.
+    // The displacement is in local coordinates (relative to root prim in linkset oriented coordinates).
+    public virtual void SetEffectiveCenterOfMassW(Vector3 centerOfMassDisplacement)
     {
-        get
+        Vector3 comDisp;
+        if (UserSetCenterOfMass.HasValue)
+            comDisp = (OMV.Vector3)UserSetCenterOfMass;
+        else
+            comDisp = centerOfMassDisplacement;
+
+        if (comDisp == Vector3.Zero)
         {
-            return base.ForcePosition;
+            // If there is no diplacement. Things get reset.
+            PositionDisplacement = OMV.Vector3.Zero;
+            OrientationDisplacement = OMV.Quaternion.Identity;
         }
-        set
+        else
         {
-            base.ForcePosition = value;
-            CenterOfMassLocation = RawPosition;
-            GeometricCenterLocation = RawPosition;
+            // Remember the displacement from root as well as the origional rotation of the
+            //    new center-of-mass.
+            PositionDisplacement = comDisp;
+            OrientationDisplacement = OMV.Quaternion.Identity;
         }
     }
 
-    public override Quaternion ForceOrientation
+    public override Vector3 ForcePosition
     {
-        get
+        get { return base.ForcePosition; }
+        set
         {
-            return base.ForceOrientation;
+            if (PositionDisplacement != OMV.Vector3.Zero)
+                base.ForcePosition = value - (PositionDisplacement * RawOrientation);
+            else
+                base.ForcePosition = value;
         }
+    }
+
+    public override Quaternion ForceOrientation
+    {
+        get { return base.ForceOrientation; }
         set
         {
             base.ForceOrientation = value;
         }
     }
 
+    // TODO: decide if this is the right place for these variables.
+    //     Somehow incorporate the optional settability by the user.
     // Is this used?
     public override OMV.Vector3 CenterOfMass
     {
-        get { return CenterOfMassLocation; }
+        get { return RawPosition; }
     }
 
     // Is this used?
     public override OMV.Vector3 GeometricCenter
     {
-        get { return GeometricCenterLocation; }
+        get { return RawPosition; }
     }
 
-
     public override void UpdateProperties(EntityProperties entprop)
     {
         // Undo any center-of-mass displacement that might have been done.
-        if (PositionDisplacement != OMV.Vector3.Zero)
+        if (PositionDisplacement != OMV.Vector3.Zero || OrientationDisplacement != OMV.Quaternion.Identity)
         {
             // Correct for any rotation around the center-of-mass
             // TODO!!!
-            entprop.Position -= PositionDisplacement;
+            entprop.Position = entprop.Position + (PositionDisplacement * entprop.Rotation);
+            // entprop.Rotation = something;
         }
 
         base.UpdateProperties(entprop);
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs
index 9898562..d65d407 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs
@@ -38,6 +38,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 public class BSPrimLinkable : BSPrimDisplaced
 {
     public BSLinkset Linkset { get; set; }
+    // The index of this child prim.
+    public int LinksetChildIndex { get; set; }
+
     public BSLinksetInfo LinksetInfo { get; set; }
 
     public BSPrimLinkable(uint localID, String primName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size,
@@ -90,7 +93,6 @@ public class BSPrimLinkable : BSPrimDisplaced
         DetailLog("{0},BSPrimLinkset.delink,parentBefore={1},childrenBefore={2},parentAfter={3},childrenAfter={4}, ",
             LocalID, parentBefore.LocalID, childrenBefore, Linkset.LinksetRoot.LocalID, Linkset.NumberOfChildren);
         return;
-        base.delink();
     }
 
     // When simulator changes position, this might be moving a child of the linkset.
@@ -133,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)