13 files changed, 242 insertions, 170 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index aa247dd..c27d3f0 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -774,7 +774,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
         // Since the computation of terrain height can be a little involved, this routine
         //    is used to fetch the height only once for each vehicle simulation step.
-        Vector3 lastRememberedHeightPos;
+        Vector3 lastRememberedHeightPos = new Vector3(-1, -1, -1);
         private float GetTerrainHeight(Vector3 pos)
         {
             if ((m_knownHas & m_knownChangedTerrainHeight) == 0 || pos != lastRememberedHeightPos)
@@ -788,14 +788,16 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
         // Since the computation of water level can be a little involved, this routine
         //    is used ot fetch the level only once for each vehicle simulation step.
+        Vector3 lastRememberedWaterHeightPos = new Vector3(-1, -1, -1);
         private float GetWaterLevel(Vector3 pos)
         {
-            if ((m_knownHas & m_knownChangedWaterLevel) == 0)
+            if ((m_knownHas & m_knownChangedWaterLevel) == 0 || pos != lastRememberedWaterHeightPos)
             {
+                lastRememberedWaterHeightPos = pos;
                 m_knownWaterLevel = ControllingPrim.PhysScene.TerrainManager.GetWaterLevelAtXYZ(pos);
                 m_knownHas |= m_knownChangedWaterLevel;
             }
-            return (float)m_knownWaterLevel;
+            return m_knownWaterLevel;
         }
 
         private Vector3 VehiclePosition
@@ -991,11 +993,17 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             {
                 Vector3 vel = VehicleVelocity;
                 if ((m_flags & (VehicleFlag.NO_X)) != 0)
+                {
                     vel.X = 0;
+                }
                 if ((m_flags & (VehicleFlag.NO_Y)) != 0)
+                {
                     vel.Y = 0;
+                }
                 if ((m_flags & (VehicleFlag.NO_Z)) != 0)
+                {
                     vel.Z = 0;
+                }
                 VehicleVelocity = vel;
             }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
index 308cf13..1d94142 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
@@ -35,62 +35,6 @@ using OMV = OpenMetaverse;
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
 
-    /*
-// When a child is linked, the relationship position of the child to the parent
-//    is remembered so the child's world position can be recomputed when it is
-//    removed from the linkset.
-sealed class BSLinksetCompoundInfo : BSLinksetInfo
-{
-    public int Index;
-    public OMV.Vector3 OffsetFromRoot;
-    public OMV.Vector3 OffsetFromCenterOfMass;
-    public OMV.Quaternion OffsetRot;
-    public BSLinksetCompoundInfo(int indx, OMV.Vector3 p, OMV.Quaternion r)
-    {
-        Index = indx;
-        OffsetFromRoot = p;
-        OffsetFromCenterOfMass = p;
-        OffsetRot = r;
-    }
-    // 'centerDisplacement' is the distance from the root the the center-of-mass (Bullet 'zero' of the shape)
-    public BSLinksetCompoundInfo(int indx, BSPrimLinkable root, BSPrimLinkable child, OMV.Vector3 centerDisplacement)
-    {
-        // Each child position and rotation is given relative to the center-of-mass.
-        OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(root.RawOrientation);
-        OMV.Vector3 displacementFromRoot = (child.RawPosition - root.RawPosition) * invRootOrientation;
-        OMV.Vector3 displacementFromCOM = displacementFromRoot - centerDisplacement;
-        OMV.Quaternion displacementRot = child.RawOrientation * invRootOrientation;
-
-        // Save relative position for recomputing child's world position after moving linkset.
-        Index = indx;
-        OffsetFromRoot = displacementFromRoot;
-        OffsetFromCenterOfMass = displacementFromCOM;
-        OffsetRot = displacementRot;
-    }
-    public override void Clear()
-    {
-        Index = 0;
-        OffsetFromRoot = OMV.Vector3.Zero;
-        OffsetFromCenterOfMass = OMV.Vector3.Zero;
-        OffsetRot = OMV.Quaternion.Identity;
-    }
-    public override string ToString()
-    {
-        StringBuilder buff = new StringBuilder();
-        buff.Append("<i=");
-        buff.Append(Index.ToString());
-        buff.Append(",p=");
-        buff.Append(OffsetFromRoot.ToString());
-        buff.Append(",m=");
-        buff.Append(OffsetFromCenterOfMass.ToString());
-        buff.Append(",r=");
-        buff.Append(OffsetRot.ToString());
-        buff.Append(">");
-        return buff.ToString();
-    }
-};
-     */
-
 public sealed class BSLinksetCompound : BSLinkset
 {
     private static string LogHeader = "[BULLETSIM LINKSET COMPOUND]";
@@ -151,7 +95,9 @@ public sealed class BSLinksetCompound : BSLinkset
     public override bool MakeStatic(BSPrimLinkable child)
     {
         bool ret = false;
+
         DetailLog("{0},BSLinksetCompound.MakeStatic,call,IsRoot={1}", child.LocalID, IsRoot(child));
+        child.ClearDisplacement();
         if (IsRoot(child))
         {
             // Schedule a rebuild to verify that the root shape is set to the real shape.
@@ -315,7 +261,6 @@ public sealed class BSLinksetCompound : BSLinkset
     // Note that this works for rebuilding just the root after a linkset is taken apart.
     // Called at taint time!!
     private bool UseBulletSimRootOffsetHack = false;    // Attempt to have Bullet track the coords of root compound shape
-    private bool disableCOM = true; // For basic linkset debugging, turn off the center-of-mass setting
     private void RecomputeLinksetCompound()
     {
         try
@@ -326,55 +271,70 @@ public sealed class BSLinksetCompound : BSLinkset
             //     to what they should be as if the root was not in a linkset.
             // Not that bad since we only get into this routine if there are children in the linkset and
             //     something has been updated/changed.
+            // Have to do the rebuild before checking for physical because this might be a linkset
+            //     being destructed and going non-physical.
             LinksetRoot.ForceBodyShapeRebuild(true);
 
             // There is no reason to build all this physical stuff for a non-physical linkset.
             if (!LinksetRoot.IsPhysicallyActive)
             {
-                // Clean up any old linkset shape and make sure the root shape is set to the root object.
                 DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,notPhysical", LinksetRoot.LocalID);
-
                 return; // Note the 'finally' clause at the botton which will get executed.
             }
 
             // Get a new compound shape to build the linkset shape in.
             BSShape linksetShape = BSShapeCompound.GetReference(m_physicsScene);
 
-            // 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 centerOfMassW = ComputeLinksetCenterOfMass();
 
             OMV.Quaternion invRootOrientation = OMV.Quaternion.Normalize(OMV.Quaternion.Inverse(LinksetRoot.RawOrientation));
+            OMV.Vector3 origRootPosition = LinksetRoot.RawPosition;
 
-            // 'centerDisplacement' is the value to subtract from children to give physical offset position
+            // 'centerDisplacementV' is the vehicle relative distance from the simulator root position to the center-of-mass
             OMV.Vector3 centerDisplacementV = (centerOfMassW - LinksetRoot.RawPosition) * invRootOrientation;
-            if (UseBulletSimRootOffsetHack || disableCOM)
+            if (UseBulletSimRootOffsetHack || !BSParam.LinksetOffsetCenterOfMass)
             {
+                // Zero everything if center-of-mass displacement is not being done.
                 centerDisplacementV = OMV.Vector3.Zero;
                 LinksetRoot.ClearDisplacement();
             }
             else
             {
-                LinksetRoot.SetEffectiveCenterOfMassDisplacement(centerDisplacementV);
+                // The actual center-of-mass could have been set by the user.
+                centerDisplacementV = LinksetRoot.SetEffectiveCenterOfMassDisplacement(centerDisplacementV);
             }
+
             DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,COM,rootPos={1},com={2},comDisp={3}",
-                                LinksetRoot.LocalID, LinksetRoot.RawPosition, centerOfMassW, centerDisplacementV);
+                                LinksetRoot.LocalID, origRootPosition, centerOfMassW, centerDisplacementV);
 
             // Add the shapes of all the components of the linkset
             int memberIndex = 1;
             ForEachMember(delegate(BSPrimLinkable cPrim)
             {
-                // Root shape is always index zero.
-                cPrim.LinksetChildIndex = IsRoot(cPrim) ? 0 : memberIndex;
+                if (IsRoot(cPrim))
+                {
+                    // Root shape is always index zero.
+                    cPrim.LinksetChildIndex = 0;
+                }
+                else
+                {
+                    cPrim.LinksetChildIndex = memberIndex;
+                    memberIndex++;
+                }
 
-                // Get a reference to the shape of the child and add that shape to the linkset compound shape
+                // Get a reference to the shape of the child for adding of that shape to the linkset compound shape
                 BSShape childShape = cPrim.PhysShape.GetReference(m_physicsScene, cPrim);
-                OMV.Vector3 offsetPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation - centerDisplacementV;
+
+                // Offset the child shape from the center-of-mass and rotate it to vehicle relative.
+                OMV.Vector3 offsetPos = (cPrim.RawPosition - origRootPosition) * invRootOrientation - centerDisplacementV;
                 OMV.Quaternion offsetRot = OMV.Quaternion.Normalize(cPrim.RawOrientation) * invRootOrientation;
+
+                // Add the child shape to the compound shape being built
                 m_physicsScene.PE.AddChildShapeToCompoundShape(linksetShape.physShapeInfo, childShape.physShapeInfo, offsetPos, offsetRot);
                 DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addChild,indx={1},cShape={2},offPos={3},offRot={4}",
-                                    LinksetRoot.LocalID, memberIndex, childShape, offsetPos, offsetRot);
+                                    LinksetRoot.LocalID, cPrim.LinksetChildIndex, childShape, offsetPos, offsetRot);
 
                 // Since we are borrowing the shape of the child, disable the origional child body
                 if (!IsRoot(cPrim))
@@ -386,8 +346,6 @@ public sealed class BSLinksetCompound : BSLinkset
                     cPrim.PhysBody.collisionType = CollisionType.LinksetChild;
                 }
 
-                memberIndex++;
-
                 return false;   // 'false' says to move onto the next child in the list
             });
 
@@ -409,8 +367,9 @@ public sealed class BSLinksetCompound : BSLinkset
             {
                 // Enable the physical position updator to return the position and rotation of the root shape.
                 // This enables a feature in the C++ code to return the world coordinates of the first shape in the
-                //     compound shape. This eleviates the need to offset the returned physical position by the
+                //     compound shape. This aleviates the need to offset the returned physical position by the
                 //     center-of-mass offset.
+                // TODO: either debug this feature or remove it.
                 m_physicsScene.PE.AddToCollisionFlags(LinksetRoot.PhysBody, CollisionFlags.BS_RETURN_ROOT_COMPOUND_SHAPE);
             }
         }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
index 1214703..7693195 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
@@ -188,6 +188,8 @@ public class BSVMotor : BSMotor
         CurrentValue = current;
         return Step(timeStep);
     }
+    // Given and error, computer a correction for this step.
+    // Simple scaling of the error by the timestep.
     public virtual Vector3 StepError(float timeStep, Vector3 error)
     {
         if (!Enabled) return Vector3.Zero;
@@ -221,7 +223,7 @@ public class BSVMotor : BSMotor
                                 CurrentValue, TargetValue);
 
         LastError = BSMotor.InfiniteVector;
-        while (maxOutput-- > 0 && !LastError.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
+        while (maxOutput-- > 0 && !ErrorIsZero())
         {
             Vector3 lastStep = Step(timeStep);
             MDetailLog("{0},BSVMotor.Test,{1},cur={2},tgt={3},lastError={4},lastStep={5}",
@@ -375,7 +377,6 @@ public class BSPIDVMotor : BSVMotor
     // The factors are vectors for the three dimensions. This is the proportional of each
     //    that is applied. This could be multiplied through the actual factors but it
     //    is sometimes easier to manipulate the factors and their mix separately.
-    //      to
     public Vector3 FactorMix;
 
     // Arbritrary factor range.
@@ -413,14 +414,14 @@ public class BSPIDVMotor : BSVMotor
             // If efficiency is high (1f), use a factor value that moves the error value to zero with little overshoot.
             // If efficiency is low (0f), use a factor value that overcorrects.
             // TODO: might want to vary contribution of different factor depending on efficiency.
-            float factor = ((1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow) / 3f;
-            // float factor = (1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow;
+            // float factor = ((1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow) / 3f;
+            float factor = (1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow;
 
             proportionFactor = new Vector3(factor, factor, factor);
             integralFactor = new Vector3(factor, factor, factor);
             derivFactor = new Vector3(factor, factor, factor);
 
-            MDetailLog("{0},BSPIDVMotor.setEfficiency,eff={1},factor={2}", BSScene.DetailLogZero, Efficiency, factor);
+            MDetailLog("{0},  BSPIDVMotor.setEfficiency,eff={1},factor={2}", BSScene.DetailLogZero, Efficiency, factor);
         }
     }
 
@@ -441,8 +442,8 @@ public class BSPIDVMotor : BSVMotor
                         + derivitive / TimeScale         * derivFactor      * FactorMix.Z
                         ;
 
-        MDetailLog("{0},  BSPIDVMotor.step,ts={1},err={2},runnInt={3},deriv={4},ret={5}",
-                        BSScene.DetailLogZero, timeStep, error, RunningIntegration, derivitive, ret);
+        MDetailLog("{0},  BSPIDVMotor.step,ts={1},err={2},lerr={3},runnInt={4},deriv={5},ret={6}",
+                        BSScene.DetailLogZero, timeStep, error, LastError, RunningIntegration, derivitive, ret);
 
         return ret;
     }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
index d17c8e7..0f84bf7 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
@@ -176,6 +176,7 @@ public static class BSParam
 
     // Linkset implementation parameters
     public static float LinksetImplementation { get; private set; }
+    public static bool LinksetOffsetCenterOfMass { get; private set; }
     public static bool LinkConstraintUseFrameOffset { get; private set; }
     public static bool LinkConstraintEnableTransMotor { get; private set; }
     public static float LinkConstraintTransMotorMaxVel { get; private set; }
@@ -684,6 +685,8 @@ public static class BSParam
 
             new ParameterDefn<float>("LinksetImplementation", "Type of linkset implementation (0=Constraint, 1=Compound, 2=Manual)",
             (float)BSLinkset.LinksetImplementation.Compound ),
+            new ParameterDefn<bool>("LinksetOffsetCenterOfMass", "If 'true', compute linkset center-of-mass and offset linkset position to account for same",
+            false ),
             new ParameterDefn<bool>("LinkConstraintUseFrameOffset", "For linksets built with constraints, enable frame offsetFor linksets built with constraints, enable frame offset.",
             false ),
             new ParameterDefn<bool>("LinkConstraintEnableTransMotor", "Whether to enable translational motor on linkset constraints",
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index a0d5c42..738e2d0 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -90,6 +90,8 @@ public abstract class BSPhysObject : PhysicsActor
         PhysBody = new BulletBody(localID);
         PhysShape = new BSShapeNull();
 
+        UserSetCenterOfMassDisplacement = null;
+
         PrimAssetState = PrimAssetCondition.Unknown;
 
         // Default material type. Also sets Friction, Restitution and Density.
@@ -180,6 +182,7 @@ public abstract class BSPhysObject : PhysicsActor
         Material = (MaterialAttributes.Material)material;
 
         // Setting the material sets the material attributes also.
+        // TODO: decide if this is necessary -- the simulator does this.
         MaterialAttributes matAttrib = BSMaterials.GetAttributes(Material, false);
         Friction = matAttrib.friction;
         Restitution = matAttrib.restitution;
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index b2947c6..ce4c3da 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -802,6 +802,7 @@ public class BSPrim : BSPhysObject
     //     isSolid: other objects bounce off of this object
     //     isVolumeDetect: other objects pass through but can generate collisions
     //     collisionEvents: whether this object returns collision events
+    // NOTE: overloaded by BSPrimLinkable to also update linkset physical parameters.
     public virtual void UpdatePhysicalParameters()
     {
         if (!PhysBody.HasPhysicalBody)
@@ -1125,7 +1126,9 @@ public class BSPrim : BSPhysObject
                 OMV.Vector3 addForce = force;
                 PhysScene.TaintedObject(inTaintTime, "BSPrim.AddForce", delegate()
                 {
-                    // Bullet adds this central force to the total force for this tick
+                    // Bullet adds this central force to the total force for this tick.
+                    // Deep down in Bullet:
+                    //      linearVelocity += totalForce / mass * timeStep;
                     DetailLog("{0},BSPrim.addForce,taint,force={1}", LocalID, addForce);
                     if (PhysBody.HasPhysicalBody)
                     {
@@ -1493,6 +1496,8 @@ public class BSPrim : BSPhysObject
 
         returnMass = Util.Clamp(returnMass, BSParam.MinimumObjectMass, BSParam.MaximumObjectMass);
         // DetailLog("{0},BSPrim.CalculateMass,den={1},vol={2},mass={3}", LocalID, Density, volume, returnMass);
+        DetailLog("{0},BSPrim.CalculateMass,den={1},vol={2},mass={3},pathB={4},pathE={5},profB={6},profE={7},siz={8}",
+                            LocalID, Density, volume, returnMass, pathBegin, pathEnd, profileBegin, profileEnd, _size);
 
         return returnMass;
     }// end CalculateMass
@@ -1528,6 +1533,8 @@ public class BSPrim : BSPhysObject
 
     // The physics engine says that properties have updated. Update same and inform
     // the world that things have changed.
+    // NOTE: BSPrim.UpdateProperties is overloaded by BSPrimLinkable which modifies updates from root and children prims.
+    // NOTE: BSPrim.UpdateProperties is overloaded by BSPrimDisplaced which handles mapping physical position to simulator position.
     public override void UpdateProperties(EntityProperties entprop)
     {
         // Let anyone (like the actors) modify the updated properties before they are pushed into the object and the simulator.
@@ -1563,8 +1570,6 @@ public class BSPrim : BSPhysObject
         LastEntityProperties = CurrentEntityProperties;
         CurrentEntityProperties = entprop;
 
-        // Note that BSPrim can be overloaded by BSPrimLinkable which controls updates from root and children prims.
-        
         PhysScene.PostUpdate(this);
     }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
index f5ee671..35d5a08 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
@@ -44,14 +44,14 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 {
 public class BSPrimDisplaced : BSPrim
 {
-    // The purpose of this module is to do any mapping between what the simulator thinks
+    // The purpose of this subclass 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.
+    //    of the prim/linkset. The simulator, on the other hand, 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
+    // This routine works by capturing ForcePosition and
     //    adjusting the simulator values (being set) into the physical values.
     //    The conversion is also done in the opposite direction (physical origin -> simulator origin).
     //
@@ -59,8 +59,8 @@ public class BSPrimDisplaced : BSPrim
     //    are converted into simulator origin values before being passed to the base
     //    class.
 
+    // PositionDisplacement is the vehicle relative distance from the root prim position to the center-of-mass.
     public virtual OMV.Vector3 PositionDisplacement { get; set; }
-    public virtual OMV.Quaternion OrientationDisplacement { get; set; }
 
     public BSPrimDisplaced(uint localID, String primName, BSScene parent_scene, OMV.Vector3 pos, OMV.Vector3 size,
                        OMV.Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical)
@@ -69,50 +69,81 @@ public class BSPrimDisplaced : BSPrim
         ClearDisplacement();
     }
 
+    // Clears any center-of-mass displacement introduced by linksets, etc.
+    // Does not clear the displacement set by the user.
     public void ClearDisplacement()
     {
-        PositionDisplacement = OMV.Vector3.Zero;
-        OrientationDisplacement = OMV.Quaternion.Identity;
+        if (UserSetCenterOfMassDisplacement.HasValue)
+            PositionDisplacement = (OMV.Vector3)UserSetCenterOfMassDisplacement;
+        else
+            PositionDisplacement = OMV.Vector3.Zero;
     }
 
     // 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 SetEffectiveCenterOfMassDisplacement(Vector3 centerOfMassDisplacement)
+    // Returns the relative offset from the root position to the center-of-mass.
+    // Called at taint time.
+    public virtual Vector3 SetEffectiveCenterOfMassDisplacement(Vector3 centerOfMassDisplacement)
     {
+        PhysScene.AssertInTaintTime("BSPrimDisplaced.SetEffectiveCenterOfMassDisplacement");
         Vector3 comDisp;
         if (UserSetCenterOfMassDisplacement.HasValue)
             comDisp = (OMV.Vector3)UserSetCenterOfMassDisplacement;
         else
             comDisp = centerOfMassDisplacement;
 
+        // Eliminate any jitter caused be very slight differences in masses and positions
+        if (comDisp.ApproxEquals(Vector3.Zero, 0.01f) )
+            comDisp = Vector3.Zero;
+
         DetailLog("{0},BSPrimDisplaced.SetEffectiveCenterOfMassDisplacement,userSet={1},comDisp={2}",
                                     LocalID, UserSetCenterOfMassDisplacement.HasValue, comDisp);
-        if (comDisp == Vector3.Zero)
+        if ( !comDisp.ApproxEquals(PositionDisplacement, 0.01f) )
         {
-            // If there is no diplacement. Things get reset.
-            PositionDisplacement = OMV.Vector3.Zero;
-            OrientationDisplacement = OMV.Quaternion.Identity;
-        }
-        else
-        {
-            // Remember the displacement from root as well as the origional rotation of the
-            //    new center-of-mass.
+            // Displacement setting is changing.
+            // The relationship between the physical object and simulated object must be aligned.
             PositionDisplacement = comDisp;
-            OrientationDisplacement = OMV.Quaternion.Identity;
+            this.ForcePosition = RawPosition;
         }
+
+        return PositionDisplacement;
     }
 
+    // 'ForcePosition' is the one way to set the physical position of the body in the physics engine.
+    // Displace the simulator idea of position (center of root prim) to the physical position.
     public override Vector3 ForcePosition
     {
-        get { return base.ForcePosition; }
+        get {
+            OMV.Vector3 physPosition = base.ForcePosition;
+            if (PositionDisplacement != OMV.Vector3.Zero)
+            {
+                // If there is some displacement, return the physical position (center-of-mass)
+                //     location minus the displacement to give the center of the root prim.
+                OMV.Vector3 displacement = PositionDisplacement * ForceOrientation;
+                DetailLog("{0},BSPrimDisplaced.ForcePosition,get,physPos={1},disp={2},simPos={3}",
+                                LocalID, physPosition, displacement, physPosition - displacement);
+                physPosition -= displacement;
+            }
+            return physPosition;
+        }
         set
         {
             if (PositionDisplacement != OMV.Vector3.Zero)
             {
-                OMV.Vector3 displacedPos = value - (PositionDisplacement * RawOrientation);
-                DetailLog("{0},BSPrimDisplaced.ForcePosition,val={1},disp={2},newPos={3}", LocalID, value, PositionDisplacement, displacedPos);
-                base.ForcePosition = displacedPos;
+                // This value is the simulator's idea of where the prim is: the center of the root prim
+                RawPosition = value;
+
+                // Move the passed root prim postion to the center-of-mass position and set in the physics engine.
+                OMV.Vector3 displacement = PositionDisplacement * RawOrientation;
+                OMV.Vector3 displacedPos = RawPosition + displacement;
+                DetailLog("{0},BSPrimDisplaced.ForcePosition,set,simPos={1},disp={2},physPos={3}",
+                                        LocalID, RawPosition, displacement, displacedPos);
+                if (PhysBody.HasPhysicalBody)
+                {
+                    PhysScene.PE.SetTranslation(PhysBody, displacedPos, RawOrientation);
+                    ActivateIfPhysical(false);
+                }
             }
             else
             {
@@ -121,25 +152,12 @@ public class BSPrimDisplaced : BSPrim
         }
     }
 
-    public override Quaternion ForceOrientation
-    {
-        get { return base.ForceOrientation; }
-        set
-        {
-            // TODO:
-            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?
+    // These are also overridden by BSPrimLinkable if the prim can be part of a linkset
     public override OMV.Vector3 CenterOfMass
     {
         get { return RawPosition; }
     }
 
-    // Is this used?
     public override OMV.Vector3 GeometricCenter
     {
         get { return RawPosition; }
@@ -148,15 +166,18 @@ public class BSPrimDisplaced : BSPrim
     public override void UpdateProperties(EntityProperties entprop)
     {
         // Undo any center-of-mass displacement that might have been done.
-        if (PositionDisplacement != OMV.Vector3.Zero || OrientationDisplacement != OMV.Quaternion.Identity)
+        if (PositionDisplacement != OMV.Vector3.Zero)
         {
-            // Correct for any rotation around the center-of-mass
-            // TODO!!!
-            
-            OMV.Vector3 displacedPos = entprop.Position + (PositionDisplacement * entprop.Rotation);
-            DetailLog("{0},BSPrimDisplaced.ForcePosition,physPos={1},disp={2},newPos={3}", LocalID, entprop.Position, PositionDisplacement, displacedPos);
+            // The origional shape was offset from 'zero' by PositionDisplacement.
+            // These physical location must be back converted to be centered around the displaced
+            //     root shape.
+
+            // Move the returned center-of-mass location to the root prim location.
+            OMV.Vector3 displacement = PositionDisplacement * entprop.Rotation;
+            OMV.Vector3 displacedPos = entprop.Position - displacement;
+            DetailLog("{0},BSPrimDisplaced.UpdateProperties,physPos={1},disp={2},simPos={3}",
+                                    LocalID, entprop.Position, displacement, displacedPos);
             entprop.Position = displacedPos;
-            // entprop.Rotation = something;
         }
 
         base.UpdateProperties(entprop);
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs
index 87eed98..1fbcfcc 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs
@@ -37,6 +37,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 {
 public class BSPrimLinkable : BSPrimDisplaced
 {
+    // The purpose of this subclass is to add linkset functionality to the prim. This overrides
+    //    operations necessary for keeping the linkset created and, additionally, this
+    //    calls the linkset implementation for its creation and management.
+
+    // This adds the overrides for link() and delink() so the prim is linkable.
+
     public BSLinkset Linkset { get; set; }
     // The index of this child prim.
     public int LinksetChildIndex { get; set; }
@@ -69,8 +75,8 @@ public class BSPrimLinkable : BSPrimDisplaced
         BSPrimLinkable parent = obj as BSPrimLinkable;
         if (parent != null)
         {
-            BSPhysObject parentBefore = Linkset.LinksetRoot;
-            int childrenBefore = Linkset.NumberOfChildren;
+            BSPhysObject parentBefore = Linkset.LinksetRoot;    // DEBUG
+            int childrenBefore = Linkset.NumberOfChildren;      // DEBUG
 
             Linkset = parent.Linkset.AddMeToLinkset(this);
 
@@ -85,8 +91,8 @@ public class BSPrimLinkable : BSPrimDisplaced
         // TODO: decide if this parent checking needs to happen at taint time
         // Race condition here: if link() and delink() in same simulation tick, the delink will not happen
 
-        BSPhysObject parentBefore = Linkset.LinksetRoot;
-        int childrenBefore = Linkset.NumberOfChildren;
+        BSPhysObject parentBefore = Linkset.LinksetRoot;        // DEBUG
+        int childrenBefore = Linkset.NumberOfChildren;          // DEBUG
 
         Linkset = Linkset.RemoveMeFromLinkset(this);
 
@@ -128,6 +134,17 @@ public class BSPrimLinkable : BSPrimDisplaced
         get { return Linkset.LinksetMass; }
     }
 
+    public override OMV.Vector3 CenterOfMass
+    {
+        get { return Linkset.CenterOfMass; }
+    }
+
+    public override OMV.Vector3 GeometricCenter
+    {
+        get { return Linkset.GeometricCenter; }
+    }
+
+    // Refresh the linkset structure and parameters when the prim's physical parameters are changed.
     public override void UpdatePhysicalParameters()
     {
         base.UpdatePhysicalParameters();
@@ -139,13 +156,17 @@ public class BSPrimLinkable : BSPrimDisplaced
             Linkset.Refresh(this);
     }
 
+    // When the prim is made dynamic or static, the linkset needs to change.
     protected override void MakeDynamic(bool makeStatic)
     {
         base.MakeDynamic(makeStatic);
-        if (makeStatic)
-            Linkset.MakeStatic(this);
-        else
-            Linkset.MakeDynamic(this);
+        if (Linkset != null)    // null can happen during initialization
+        {
+            if (makeStatic)
+                Linkset.MakeStatic(this);
+            else
+                Linkset.MakeDynamic(this);
+        }
     }
 
     // Body is being taken apart. Remove physical dependencies and schedule a rebuild.
@@ -155,6 +176,8 @@ public class BSPrimLinkable : BSPrimDisplaced
         base.RemoveDependencies();
     }
 
+    // Called after a simulation step for the changes in physical object properties.
+    // Do any filtering/modification needed for linksets.
     public override void UpdateProperties(EntityProperties entprop)
     {
         if (Linkset.IsRoot(this))
@@ -176,6 +199,7 @@ public class BSPrimLinkable : BSPrimDisplaced
         Linkset.UpdateProperties(UpdatedProperties.EntPropUpdates, this);
     }
 
+    // Called after a simulation step to post a collision with this object.
     public override bool Collide(uint collidingWith, BSPhysObject collidee,
                     OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth)
     {
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index 1645c98..e56a6f6 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -785,7 +785,6 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
             {
                 // The simulation of the time interval took less than realtime.
                 // Do a sleep for the rest of realtime.
-                DetailLog("{0},BulletSPluginPhysicsThread,sleeping={1}", BSScene.DetailLogZero, simulationTimeVsRealtimeDifferenceMS);
                 Thread.Sleep(simulationTimeVsRealtimeDifferenceMS);
             }
             else
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
index 4357ef1..0453376 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt
@@ -3,25 +3,21 @@ CURRENT PROBLEMS TO FIX AND/OR LOOK AT
 Vehicle buoyancy. Computed correctly? Possibly creating very large effective mass.
     Interaction of llSetBuoyancy and vehicle buoyancy. Should be additive?
     Negative buoyancy computed correctly
+Center-of-gravity
 Computation of mesh mass. How done? How should it be done?
-Script changing rotation of child prim while vehicle moving (eg turning wheel) causes
-        the wheel to appear to jump back. Looks like sending position from previous update.
 Enable vehicle border crossings (at least as poorly as ODE)
         Terrain skirts
         Avatar created in previous region and not new region when crossing border
         Vehicle recreated in new sim at small Z value (offset from root value?) (DONE)
+User settable terrain mesh
+        Allow specifying as convex or concave and use different getHeight functions depending
+Boats, when turning nose down into the water
+        Acts like rotation around Z is also effecting rotation around X and Y
 Deleting a linkset while standing on the root will leave the physical shape of the root behind.
         Not sure if it is because standing on it. Done with large prim linksets.
 Linkset child rotations.
         Nebadon spiral tube has middle sections which are rotated wrong.
         Select linked spiral tube. Delink and note where the middle section ends up.
-Refarb compound linkset creation to create a pseudo-root for center-of-mass
-        Let children change their shape to physical indendently and just add shapes to compound
-Vehicle angular vertical attraction
-vehicle angular banking
-Center-of-gravity
-Vehicle angular deflection
-        Preferred orientation angular correction fix
 Teravus llMoveToTarget script debug
         Mixing of hover, buoyancy/gravity, moveToTarget, into one force
         Setting hover height to zero disables hover even if hover flags are on (from SL wiki)
@@ -33,10 +29,16 @@ Vehicle script tuning/debugging
         Avanti speed script
         Weapon shooter script
 Move material definitions (friction, ...) into simulator.
+osGetPhysicsEngineVerion() and create a version code for the C++ DLL
 One sided meshes? Should terrain be built into a closed shape?
         When meshes get partially wedged into the terrain, they cannot push themselves out.
         It is possible that Bullet processes collisions whether entering or leaving a mesh.
         Ref: http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4869
+Small physical objects do not interact correctly
+    Create chain of .5x.5x.1 torui and make all but top physical so to hang.
+        The chain will fall apart and pairs will dance around on ground
+    Chains of 1x1x.2 will stay connected but will dance.
+    Chains above 2x2x.4 are more stable and get stablier as torui get larger.
 
 VEHICLES TODO LIST:
 =================================================
@@ -45,14 +47,12 @@ LINEAR_MOTOR_DIRECTION values should be clamped to reasonable numbers.
         Same for other velocity settings.
 UBit improvements to remove rubber-banding of avatars sitting on vehicle child prims:
         https://github.com/UbitUmarov/Ubit-opensim
-Vehicles (Move smoothly)
 Some vehicles should not be able to turn if no speed or off ground.
 Cannot edit/move a vehicle being ridden: it jumps back to the origional position.
 Neb car jiggling left and right
         Happens on terrain and any other mesh object. Flat cubes are much smoother.
         This has been reduced but not eliminated.
 Implement referenceFrame for all the motion routines.
-For limitMotorUp, use raycast down to find if vehicle is in the air.
 Verify llGetVel() is returning a smooth and good value for vehicle movement.
 llGetVel() should return the root's velocity if requested in a child prim.
 Implement function efficiency for lineaar and angular motion.
@@ -93,29 +93,15 @@ Revisit CollisionMargin. Builders notice the 0.04 spacing between prims.
 Duplicating a physical prim causes old prim to jump away
         Dup a phys prim and the original become unselected and thus interacts w/ selected prim.
 Scenes with hundred of thousands of static objects take a lot of physics CPU time.
-BSPrim.Force should set a continious force on the prim. The force should be
-        applied each tick.  Some limits?
 Gun sending shooter flying.
 Collision margin (gap between physical objects lying on each other)
 Boundry checking (crashes related to crossing boundry)
     Add check for border edge position for avatars and objects.
     Verify the events are created for border crossings.
-Avatar rotation (check out changes to ScenePresence for physical rotation)
-Avatar running (what does phys engine need to do?)
-Small physical objects do not interact correctly
-    Create chain of .5x.5x.1 torui and make all but top physical so to hang.
-        The chain will fall apart and pairs will dance around on ground
-    Chains of 1x1x.2 will stay connected but will dance.
-    Chains above 2x2x.4 are more stable and get stablier as torui get larger.
-Add PID motor for avatar movement (slow to stop, ...)
-setForce should set a constant force. Different than AddImpulse.
-Implement raycast.
 Implement ShapeCollection.Dispose()
-Implement water as a plain so raycasting and collisions can happen with same.
+Implement water as a plain or mesh so raycasting and collisions can happen with same.
 Add collision penetration return
         Add field passed back by BulletSim.dll and fill with info in ManifoldConstact.GetDistance()
-Add osGetPhysicsEngineName() so scripters can tell whether BulletSim or ODE
-        Also osGetPhysicsEngineVerion() maybe.
 Linkset.Position and Linkset.Orientation requre rewrite to properly return
         child position. LinksetConstraint acts like it's at taint time!!
 Implement LockAngularMotion -- implements llSetStatus(ROTATE_AXIS_*, T/F)
@@ -127,9 +113,6 @@ Selecting and deselecting physical objects causes CPU processing time to jump
 Re-implement buoyancy as a separate force on the object rather than diddling gravity.
         Register a pre-step event to add the force.
 More efficient memory usage when passing hull information from BSPrim to BulletSim
-Avatar movement motor check for zero or small movement. Somehow suppress small movements
-        when avatar has stopped and is just standing. Simple test for near zero has
-        the problem of preventing starting up (increase from zero) especially when falling.
 Physical and phantom will drop through the terrain
 
 
@@ -172,7 +155,6 @@ Do we need to do convex hulls all the time? Can complex meshes be left meshes?
         There is some problem with meshes and collisions
         Hulls are not as detailed as meshes. Hulled vehicles insides are different shape.
 Debounce avatar contact so legs don't keep folding up when standing.
-Implement LSL physics controls. Like STATUS_ROTATE_X.
 Add border extensions to terrain to help region crossings and objects leaving region.
 Use a different capsule shape for avatar when sitting
         LL uses a pyrimidal shape scaled by the avatar's bounding box
@@ -205,8 +187,6 @@ Keep avatar scaling correct. http://pennycow.blogspot.fr/2011/07/matter-of-scale
 
 INTERNAL IMPROVEMENT/CLEANUP
 =================================================
-Can the 'inTaintTime' flag be cleaned up and used? For instance, a call to
-        BSScene.TaintedObject() could immediately execute the callback if already in taint time.
 Create the physical wrapper classes (BulletBody, BulletShape) by methods on
         BSAPITemplate and make their actual implementation Bullet engine specific.
         For the short term, just call the existing functions in ShapeCollection.
@@ -365,4 +345,35 @@ After getting off a vehicle, the root prim is phantom (can be walked through)
 Explore btGImpactMeshShape as alternative to convex hulls for simplified physical objects.
         Regular triangle meshes don't do physical collisions.
         (DONE: discovered GImpact is VERY CPU intensive)
+Script changing rotation of child prim while vehicle moving (eg turning wheel) causes
+        the wheel to appear to jump back. Looks like sending position from previous update.
+        (DONE: redo of compound linksets fixed problem)
+Refarb compound linkset creation to create a pseudo-root for center-of-mass
+        Let children change their shape to physical indendently and just add shapes to compound
+        (DONE: redo of compound linkset fixed problem)
+Vehicle angular vertical attraction     (DONE: vegaslon code)
+vehicle angular banking (DONE: vegaslon code)
+Vehicle angular deflection      (DONE: vegaslon code)
+        Preferred orientation angular correction fix
+Vehicles (Move smoothly)
+For limitMotorUp, use raycast down to find if vehicle is in the air.
+        (WILL NOT BE DONE: gravity does the job well enough)
+BSPrim.Force should set a continious force on the prim. The force should be
+        applied each tick.  Some limits?
+        (DONE: added physical actors. Implemented SetForce, SetTorque, ...)
+Implement LSL physics controls. Like STATUS_ROTATE_X. (DONE)
+Add osGetPhysicsEngineName() so scripters can tell whether BulletSim or ODE
+Avatar rotation (check out changes to ScenePresence for physical rotation) (DONE)
+Avatar running (what does phys engine need to do?) (DONE: multiplies run factor by walking force)
+setForce should set a constant force. Different than AddImpulse. (DONE)
+Add PID motor for avatar movement (slow to stop, ...) (WNBD: current works ok)
+Avatar movement motor check for zero or small movement. Somehow suppress small movements
+        when avatar has stopped and is just standing. Simple test for near zero has
+        the problem of preventing starting up (increase from zero) especially when falling.
+        (DONE: avatar movement actor knows if standing on stationary object and zeros motion)
+Can the 'inTaintTime' flag be cleaned up and used? For instance, a call to
+        BSScene.TaintedObject() could immediately execute the callback if already in taint time.
+        (DONE)
+
+
 
diff --git a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs
index c0b8373..34e2b4d 100644
--- a/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs
+++ b/OpenSim/Region/ScriptEngine/Shared/Api/Implementation/LSL_Api.cs
@@ -2382,6 +2382,32 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api
             return force;
         }
 
+        public void llSetVelocity(LSL_Vector velocity, int local)
+        {
+            m_host.AddScriptLPS(1);
+
+            if (!m_host.ParentGroup.IsDeleted)
+            {
+                if (local != 0)
+                    velocity *= llGetRot();
+
+                m_host.ParentGroup.RootPart.Velocity = velocity;
+            }
+        }
+
+        public void llSetAngularVelocity(LSL_Vector angularVelocity, int local)
+        {
+            m_host.AddScriptLPS(1);
+
+            if (!m_host.ParentGroup.IsDeleted)
+            {
+                if (local != 0)
+                    angularVelocity *= llGetRot();
+
+                m_host.ParentGroup.RootPart.AngularVelocity = angularVelocity;
+            }
+        }
+
         public LSL_Integer llTarget(LSL_Vector position, double range)
         {
             m_host.AddScriptLPS(1);
diff --git a/OpenSim/Region/ScriptEngine/Shared/Api/Interface/ILSL_Api.cs b/OpenSim/Region/ScriptEngine/Shared/Api/Interface/ILSL_Api.cs
index ff13ee6..340edb3 100644
--- a/OpenSim/Region/ScriptEngine/Shared/Api/Interface/ILSL_Api.cs
+++ b/OpenSim/Region/ScriptEngine/Shared/Api/Interface/ILSL_Api.cs
@@ -342,6 +342,8 @@ namespace OpenSim.Region.ScriptEngine.Shared.Api.Interfaces
               void llSetDamage(double damage);
               void llSetForce(LSL_Vector force, int local);
               void llSetForceAndTorque(LSL_Vector force, LSL_Vector torque, int local);
+              void llSetVelocity(LSL_Vector velocity, int local);
+              void llSetAngularVelocity(LSL_Vector angularVelocity, int local);
               void llSetHoverHeight(double height, int water, double tau);
               void llSetInventoryPermMask(string item, int mask, int value);
               void llSetLinkAlpha(int linknumber, double alpha, int face);
diff --git a/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/LSL_Stub.cs b/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/LSL_Stub.cs
index 87cc342..7cd17e7 100644
--- a/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/LSL_Stub.cs
+++ b/OpenSim/Region/ScriptEngine/Shared/Api/Runtime/LSL_Stub.cs
@@ -1548,6 +1548,16 @@ namespace OpenSim.Region.ScriptEngine.Shared.ScriptBase
             m_LSL_Functions.llSetForceAndTorque(force, torque, local);
         }
 
+        public void llSetVelocity(LSL_Vector force, int local)
+        {
+            m_LSL_Functions.llSetVelocity(force, local);
+        }
+
+        public void llSetAngularVelocity(LSL_Vector force, int local)
+        {
+            m_LSL_Functions.llSetAngularVelocity(force, local);
+        }
+
         public void llSetHoverHeight(double height, int water, double tau)
         {
             m_LSL_Functions.llSetHoverHeight(height, water, tau);