Merge branch 'master' of ssh://opensimulator.org/var/git/opensim

11 files changed, 292 insertions, 285 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index 5ef6992..c9e3ca0 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -43,12 +43,10 @@ public sealed class BSCharacter : BSPhysObject
     private OMV.Vector3 _size;
     private bool _grabbed;
     private bool _selected;
-    private OMV.Vector3 _position;
     private float _mass;
     private float _avatarVolume;
     private float _collisionScore;
     private OMV.Vector3 _acceleration;
-    private OMV.Quaternion _orientation;
     private int _physicsActorType;
     private bool _isPhysical;
     private bool _flying;
@@ -70,10 +68,10 @@ public sealed class BSCharacter : BSPhysObject
             : base(parent_scene, localID, avName, "BSCharacter")
     {
         _physicsActorType = (int)ActorTypes.Agent;
-        _position = pos;
+        RawPosition = pos;
 
         _flying = isFlying;
-        _orientation = OMV.Quaternion.Identity;
+        RawOrientation = OMV.Quaternion.Identity;
         RawVelocity = OMV.Vector3.Zero;
         _buoyancy = ComputeBuoyancyFromFlying(isFlying);
         Friction = BSParam.AvatarStandingFriction;
@@ -133,7 +131,7 @@ public sealed class BSCharacter : BSPhysObject
         PhysScene.PE.RemoveObjectFromWorld(PhysScene.World, PhysBody);
 
         ZeroMotion(true);
-        ForcePosition = _position;
+        ForcePosition = RawPosition;
 
         // Set the velocity
         if (m_moveActor != null)
@@ -272,38 +270,33 @@ public sealed class BSCharacter : BSPhysObject
 
     public override void LockAngularMotion(OMV.Vector3 axis) { return; }
 
-    public override OMV.Vector3 RawPosition
-    {
-        get { return _position; }
-        set { _position = value; }
-    }
     public override OMV.Vector3 Position {
         get {
             // Don't refetch the position because this function is called a zillion times
-            // _position = PhysicsScene.PE.GetObjectPosition(Scene.World, LocalID);
-            return _position;
+            // RawPosition = PhysicsScene.PE.GetObjectPosition(Scene.World, LocalID);
+            return RawPosition;
         }
         set {
-            _position = value;
+            RawPosition = value;
 
             PhysScene.TaintedObject("BSCharacter.setPosition", delegate()
             {
-                DetailLog("{0},BSCharacter.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
+                DetailLog("{0},BSCharacter.SetPosition,taint,pos={1},orient={2}", LocalID, RawPosition, RawOrientation);
                 PositionSanityCheck();
-                ForcePosition = _position;
+                ForcePosition = RawPosition;
             });
         }
     }
     public override OMV.Vector3 ForcePosition {
         get {
-            _position = PhysScene.PE.GetPosition(PhysBody);
-            return _position;
+            RawPosition = PhysScene.PE.GetPosition(PhysBody);
+            return RawPosition;
         }
         set {
-            _position = value;
+            RawPosition = value;
             if (PhysBody.HasPhysicalBody)
             {
-                PhysScene.PE.SetTranslation(PhysBody, _position, _orientation);
+                PhysScene.PE.SetTranslation(PhysBody, RawPosition, RawOrientation);
             }
         }
     }
@@ -331,16 +324,16 @@ public sealed class BSCharacter : BSPhysObject
         float terrainHeight = PhysScene.TerrainManager.GetTerrainHeightAtXYZ(RawPosition);
         if (Position.Z < terrainHeight)
         {
-            DetailLog("{0},BSCharacter.PositionSanityCheck,adjustForUnderGround,pos={1},terrain={2}", LocalID, _position, terrainHeight);
-            _position.Z = terrainHeight + BSParam.AvatarBelowGroundUpCorrectionMeters;
+            DetailLog("{0},BSCharacter.PositionSanityCheck,adjustForUnderGround,pos={1},terrain={2}", LocalID, RawPosition, terrainHeight);
+            RawPosition = new OMV.Vector3(RawPosition.X, RawPosition.Y, terrainHeight + BSParam.AvatarBelowGroundUpCorrectionMeters);
             ret = true;
         }
         if ((CurrentCollisionFlags & CollisionFlags.BS_FLOATS_ON_WATER) != 0)
         {
-            float waterHeight = PhysScene.TerrainManager.GetWaterLevelAtXYZ(_position);
+            float waterHeight = PhysScene.TerrainManager.GetWaterLevelAtXYZ(RawPosition);
             if (Position.Z < waterHeight)
             {
-                _position.Z = waterHeight;
+                RawPosition = new OMV.Vector3(RawPosition.X, RawPosition.Y, waterHeight);
                 ret = true;
             }
         }
@@ -360,8 +353,8 @@ public sealed class BSCharacter : BSPhysObject
             //    just assign to "Position" because of potential call loops.
             PhysScene.TaintedObject(inTaintTime, "BSCharacter.PositionSanityCheck", delegate()
             {
-                DetailLog("{0},BSCharacter.PositionSanityCheck,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                ForcePosition = _position;
+                DetailLog("{0},BSCharacter.PositionSanityCheck,taint,pos={1},orient={2}", LocalID, RawPosition, RawOrientation);
+                ForcePosition = RawPosition;
             });
             ret = true;
         }
@@ -466,19 +459,14 @@ public sealed class BSCharacter : BSPhysObject
         get { return _acceleration; }
         set { _acceleration = value; }
     }
-    public override OMV.Quaternion RawOrientation
-    {
-        get { return _orientation; }
-        set { _orientation = value; }
-    }
     public override OMV.Quaternion Orientation {
-        get { return _orientation; }
+        get { return RawOrientation; }
         set {
             // Orientation is set zillions of times when an avatar is walking. It's like
             //      the viewer doesn't trust us.
-            if (_orientation != value)
+            if (RawOrientation != value)
             {
-                _orientation = value;
+                RawOrientation = value;
                 PhysScene.TaintedObject("BSCharacter.setOrientation", delegate()
                 {
                     // Bullet assumes we know what we are doing when forcing orientation
@@ -486,10 +474,10 @@ public sealed class BSCharacter : BSPhysObject
                     // This forces rotation to be only around the Z axis and doesn't change any of the other axis.
                     // This keeps us from flipping the capsule over which the veiwer does not understand.
                     float oRoll, oPitch, oYaw;
-                    _orientation.GetEulerAngles(out oRoll, out oPitch, out oYaw);
+                    RawOrientation.GetEulerAngles(out oRoll, out oPitch, out oYaw);
                     OMV.Quaternion trimmedOrientation = OMV.Quaternion.CreateFromEulers(0f, 0f, oYaw);
                     // DetailLog("{0},BSCharacter.setOrientation,taint,val={1},valDir={2},conv={3},convDir={4}",
-                    //                 LocalID, _orientation, OMV.Vector3.UnitX * _orientation,
+                    //                 LocalID, RawOrientation, OMV.Vector3.UnitX * RawOrientation,
                     //                 trimmedOrientation, OMV.Vector3.UnitX * trimmedOrientation);
                     ForceOrientation = trimmedOrientation;
                 });
@@ -501,16 +489,16 @@ public sealed class BSCharacter : BSPhysObject
     {
         get
         {
-            _orientation = PhysScene.PE.GetOrientation(PhysBody);
-            return _orientation;
+            RawOrientation = PhysScene.PE.GetOrientation(PhysBody);
+            return RawOrientation;
         }
         set
         {
-            _orientation = value;
+            RawOrientation = value;
             if (PhysBody.HasPhysicalBody)
             {
-                // _position = PhysicsScene.PE.GetPosition(BSBody);
-                PhysScene.PE.SetTranslation(PhysBody, _position, _orientation);
+                // RawPosition = PhysicsScene.PE.GetPosition(BSBody);
+                PhysScene.PE.SetTranslation(PhysBody, RawPosition, RawOrientation);
             }
         }
     }
@@ -723,9 +711,9 @@ public sealed class BSCharacter : BSPhysObject
     {
         // Don't change position if standing on a stationary object.
         if (!IsStationary)
-            _position = entprop.Position;
+            RawPosition = entprop.Position;
 
-        _orientation = entprop.Rotation;
+        RawOrientation = entprop.Rotation;
 
         // Smooth velocity. OpenSimulator is VERY sensitive to changes in velocity of the avatar
         //    and will send agent updates to the clients if velocity changes by more than
@@ -740,8 +728,8 @@ public sealed class BSCharacter : BSPhysObject
         // Do some sanity checking for the avatar. Make sure it's above ground and inbounds.
         if (PositionSanityCheck(true))
         {
-            DetailLog("{0},BSCharacter.UpdateProperties,updatePosForSanity,pos={1}", LocalID, _position);
-            entprop.Position = _position;
+            DetailLog("{0},BSCharacter.UpdateProperties,updatePosForSanity,pos={1}", LocalID, RawPosition);
+            entprop.Position = RawPosition;
         }
 
         // remember the current and last set values
@@ -755,7 +743,7 @@ public sealed class BSCharacter : BSPhysObject
         // base.RequestPhysicsterseUpdate();
 
         DetailLog("{0},BSCharacter.UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
-                LocalID, _position, _orientation, RawVelocity, _acceleration, _rotationalVelocity);
+                LocalID, RawPosition, RawOrientation, RawVelocity, _acceleration, _rotationalVelocity);
     }
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index aa247dd..82fe267 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -144,7 +144,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         public void SetupVehicleDebugging()
         {
             enableAngularVerticalAttraction = true;
-            enableAngularDeflection = false;
+            enableAngularDeflection = true;
             enableAngularBanking = true;
             if (BSParam.VehicleDebuggingEnable)
             {
@@ -173,7 +173,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             switch (pParam)
             {
                 case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
-                    m_angularDeflectionEfficiency = Math.Max(pValue, 0.01f);
+                    m_angularDeflectionEfficiency = ClampInRange(0f, pValue, 1f);
                     break;
                 case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
                     m_angularDeflectionTimescale = Math.Max(pValue, 0.01f);
@@ -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;
             }
 
@@ -1504,11 +1512,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         //     in that direction.
         // TODO: implement reference frame.
         public void ComputeAngularDeflection()
-        {
-            // Since angularMotorUp and angularDeflection are computed independently, they will calculate
-            //     approximately the same X or Y correction. When added together (when contributions are combined)
-            //     this creates an over-correction and then wabbling as the target is overshot.
-            // TODO: rethink how the different correction computations inter-relate.
+        {   
 
             if (enableAngularDeflection && m_angularDeflectionEfficiency != 0 && VehicleForwardSpeed > 0.2)
             {
@@ -1523,10 +1527,14 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
                 // The direction the vehicle is pointing
                 Vector3 pointingDirection = Vector3.UnitX * VehicleOrientation;
-                pointingDirection.Normalize();
+                //Predict where the Vehicle will be pointing after AngularVelocity change is applied. This will keep
+                //   from overshooting and allow this correction to merge with the Vertical Attraction peacefully.
+                Vector3 predictedPointingDirection = pointingDirection * Quaternion.CreateFromAxisAngle(VehicleRotationalVelocity, 0f);
+                predictedPointingDirection.Normalize();
 
                 // The difference between what is and what should be.
-                Vector3 deflectionError = movingDirection - pointingDirection;
+               // Vector3 deflectionError = movingDirection - predictedPointingDirection;
+                Vector3 deflectionError = Vector3.Cross(movingDirection, predictedPointingDirection);
 
                 // Don't try to correct very large errors (not our job)
                 // if (Math.Abs(deflectionError.X) > PIOverFour) deflectionError.X = PIOverTwo * Math.Sign(deflectionError.X);
@@ -1539,15 +1547,16 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 // ret = m_angularDeflectionCorrectionMotor(1f, deflectionError);
 
                 // Scale the correction by recovery timescale and efficiency
-                deflectContributionV = (-deflectionError) * m_angularDeflectionEfficiency;
-                deflectContributionV /= m_angularDeflectionTimescale;
-
-                VehicleRotationalVelocity += deflectContributionV * VehicleOrientation;
+                //    Not modeling a spring so clamp the scale to no more then the arc
+                deflectContributionV = (-deflectionError) * ClampInRange(0, m_angularDeflectionEfficiency/m_angularDeflectionTimescale,1f);
+                //deflectContributionV /= m_angularDeflectionTimescale;
 
+               // VehicleRotationalVelocity += deflectContributionV * VehicleOrientation;
+                VehicleRotationalVelocity += deflectContributionV;
                 VDetailLog("{0},  MoveAngular,Deflection,movingDir={1},pointingDir={2},deflectError={3},ret={4}",
                     ControllingPrim.LocalID, movingDirection, pointingDirection, deflectionError, deflectContributionV);
-                VDetailLog("{0},  MoveAngular,Deflection,fwdSpd={1},defEff={2},defTS={3}",
-                    ControllingPrim.LocalID, VehicleForwardSpeed, m_angularDeflectionEfficiency, m_angularDeflectionTimescale);
+                VDetailLog("{0},  MoveAngular,Deflection,fwdSpd={1},defEff={2},defTS={3},PredictedPointingDir={4}",
+                    ControllingPrim.LocalID, VehicleForwardSpeed, m_angularDeflectionEfficiency, m_angularDeflectionTimescale, predictedPointingDirection);
             }
         }
 
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..a41eaf8 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;
@@ -194,10 +197,10 @@ public abstract class BSPhysObject : PhysicsActor
     // Update the physical location and motion of the object. Called with data from Bullet.
     public abstract void UpdateProperties(EntityProperties entprop);
 
-    public abstract OMV.Vector3 RawPosition { get; set; }
+    public virtual OMV.Vector3 RawPosition { get; set; }
     public abstract OMV.Vector3 ForcePosition { get; set; }
 
-    public abstract OMV.Quaternion RawOrientation { get; set; }
+    public virtual OMV.Quaternion RawOrientation { get; set; }
     public abstract OMV.Quaternion ForceOrientation { get; set; }
 
     public OMV.Vector3 RawVelocity { get; set; }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index b2947c6..d43448e 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -51,12 +51,8 @@ public class BSPrim : BSPhysObject
     private bool _isSelected;
     private bool _isVolumeDetect;
 
-    // _position is what the simulator thinks the positions of the prim is.
-    private OMV.Vector3 _position;
-
     private float _mass;    // the mass of this object
     private OMV.Vector3 _acceleration;
-    private OMV.Quaternion _orientation;
     private int _physicsActorType;
     private bool _isPhysical;
     private bool _flying;
@@ -88,10 +84,10 @@ public class BSPrim : BSPhysObject
     {
         // m_log.DebugFormat("{0}: BSPrim creation of {1}, id={2}", LogHeader, primName, localID);
         _physicsActorType = (int)ActorTypes.Prim;
-        _position = pos;
+        RawPosition = pos;
         _size = size;
         Scale = size;   // prims are the size the user wants them to be (different for BSCharactes).
-        _orientation = rotation;
+        RawOrientation = rotation;
         _buoyancy = 0f;
         RawVelocity = OMV.Vector3.Zero;
         _rotationalVelocity = OMV.Vector3.Zero;
@@ -270,46 +266,42 @@ public class BSPrim : BSPhysObject
         return;
     }
 
-    public override OMV.Vector3 RawPosition
-    {
-        get { return _position; }
-        set { _position = value; }
-    }
     public override OMV.Vector3 Position {
         get {
             // don't do the GetObjectPosition for root elements because this function is called a zillion times.
-            // _position = ForcePosition;
-            return _position;
+            // RawPosition = ForcePosition;
+            return RawPosition;
         }
         set {
             // If the position must be forced into the physics engine, use ForcePosition.
             // All positions are given in world positions.
-            if (_position == value)
+            if (RawPosition == value)
             {
-                DetailLog("{0},BSPrim.setPosition,call,positionNotChanging,pos={1},orient={2}", LocalID, _position, _orientation);
+                DetailLog("{0},BSPrim.setPosition,call,positionNotChanging,pos={1},orient={2}", LocalID, RawPosition, RawOrientation);
                 return;
             }
-            _position = value;
+            RawPosition = value;
             PositionSanityCheck(false);
 
             PhysScene.TaintedObject("BSPrim.setPosition", delegate()
             {
-                DetailLog("{0},BSPrim.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                ForcePosition = _position;
+                DetailLog("{0},BSPrim.SetPosition,taint,pos={1},orient={2}", LocalID, RawPosition, RawOrientation);
+                ForcePosition = RawPosition;
             });
         }
     }
 
+    // NOTE: overloaded by BSPrimDisplaced to handle offset for center-of-gravity.
     public override OMV.Vector3 ForcePosition {
         get {
-            _position = PhysScene.PE.GetPosition(PhysBody);
-            return _position;
+            RawPosition = PhysScene.PE.GetPosition(PhysBody);
+            return RawPosition;
         }
         set {
-            _position = value;
+            RawPosition = value;
             if (PhysBody.HasPhysicalBody)
             {
-                PhysScene.PE.SetTranslation(PhysBody, _position, _orientation);
+                PhysScene.PE.SetTranslation(PhysBody, RawPosition, RawOrientation);
                 ActivateIfPhysical(false);
             }
         }
@@ -343,10 +335,10 @@ public class BSPrim : BSPhysObject
             float targetHeight = terrainHeight + (Size.Z / 2f);
             // If the object is below ground it just has to be moved up because pushing will
             //     not get it through the terrain
-            _position.Z = targetHeight;
+            RawPosition = new OMV.Vector3(RawPosition.X, RawPosition.Y, targetHeight);
             if (inTaintTime)
             {
-                ForcePosition = _position;
+                ForcePosition = RawPosition;
             }
             // If we are throwing the object around, zero its other forces
             ZeroMotion(inTaintTime);
@@ -355,7 +347,7 @@ public class BSPrim : BSPhysObject
 
         if ((CurrentCollisionFlags & CollisionFlags.BS_FLOATS_ON_WATER) != 0)
         {
-            float waterHeight = PhysScene.TerrainManager.GetWaterLevelAtXYZ(_position);
+            float waterHeight = PhysScene.TerrainManager.GetWaterLevelAtXYZ(RawPosition);
             // TODO: a floating motor so object will bob in the water
             if (Math.Abs(RawPosition.Z - waterHeight) > 0.1f)
             {
@@ -364,7 +356,7 @@ public class BSPrim : BSPhysObject
 
                 // Apply upforce and overcome gravity.
                 OMV.Vector3 correctionForce = upForce - PhysScene.DefaultGravity;
-                DetailLog("{0},BSPrim.PositionSanityCheck,applyForce,pos={1},upForce={2},correctionForce={3}", LocalID, _position, upForce, correctionForce);
+                DetailLog("{0},BSPrim.PositionSanityCheck,applyForce,pos={1},upForce={2},correctionForce={3}", LocalID, RawPosition, upForce, correctionForce);
                 AddForce(correctionForce, false, inTaintTime);
                 ret = true;
             }
@@ -383,11 +375,11 @@ public class BSPrim : BSPhysObject
         uint wayOutThere = Constants.RegionSize * Constants.RegionSize;
         // There have been instances of objects getting thrown way out of bounds and crashing
         //    the border crossing code.
-        if (   _position.X < -Constants.RegionSize || _position.X > wayOutThere
-            || _position.Y < -Constants.RegionSize || _position.Y > wayOutThere
-            || _position.Z < -Constants.RegionSize || _position.Z > wayOutThere)
+        if (   RawPosition.X < -Constants.RegionSize || RawPosition.X > wayOutThere
+            || RawPosition.Y < -Constants.RegionSize || RawPosition.Y > wayOutThere
+            || RawPosition.Z < -Constants.RegionSize || RawPosition.Z > wayOutThere)
         {
-            _position = new OMV.Vector3(10, 10, 50);
+            RawPosition = new OMV.Vector3(10, 10, 50);
             ZeroMotion(inTaintTime);
             ret = true;
         }
@@ -713,23 +705,19 @@ public class BSPrim : BSPhysObject
         get { return _acceleration; }
         set { _acceleration = value; }
     }
-    public override OMV.Quaternion RawOrientation
-    {
-        get { return _orientation; }
-        set { _orientation = value; }
-    }
+
     public override OMV.Quaternion Orientation {
         get {
-            return _orientation;
+            return RawOrientation;
         }
         set {
-            if (_orientation == value)
+            if (RawOrientation == value)
                 return;
-            _orientation = value;
+            RawOrientation = value;
 
             PhysScene.TaintedObject("BSPrim.setOrientation", delegate()
             {
-                ForceOrientation = _orientation;
+                ForceOrientation = RawOrientation;
             });
         }
     }
@@ -738,14 +726,14 @@ public class BSPrim : BSPhysObject
     {
         get
         {
-            _orientation = PhysScene.PE.GetOrientation(PhysBody);
-            return _orientation;
+            RawOrientation = PhysScene.PE.GetOrientation(PhysBody);
+            return RawOrientation;
         }
         set
         {
-            _orientation = value;
+            RawOrientation = value;
             if (PhysBody.HasPhysicalBody)
-                PhysScene.PE.SetTranslation(PhysBody, _position, _orientation);
+                PhysScene.PE.SetTranslation(PhysBody, RawPosition, RawOrientation);
         }
     }
     public override int PhysicsActorType {
@@ -802,6 +790,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)
@@ -888,7 +877,7 @@ public class BSPrim : BSPhysObject
             // PhysicsScene.PE.ClearAllForces(BSBody);
 
             // For good measure, make sure the transform is set through to the motion state
-            ForcePosition = _position;
+            ForcePosition = RawPosition;
             ForceVelocity = RawVelocity;
             ForceRotationalVelocity = _rotationalVelocity;
 
@@ -1125,7 +1114,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 +1484,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 +1521,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.
@@ -1536,8 +1531,8 @@ public class BSPrim : BSPhysObject
         // DetailLog("{0},BSPrim.UpdateProperties,entry,entprop={1}", LocalID, entprop);   // DEBUG DEBUG
 
         // Assign directly to the local variables so the normal set actions do not happen
-        _position = entprop.Position;
-        _orientation = entprop.Rotation;
+        RawPosition = entprop.Position;
+        RawOrientation = entprop.Rotation;
         // DEBUG DEBUG DEBUG -- smooth velocity changes a bit. The simulator seems to be
         //    very sensitive to velocity changes.
         if (entprop.Velocity == OMV.Vector3.Zero || !entprop.Velocity.ApproxEquals(RawVelocity, BSParam.UpdateVelocityChangeThreshold))
@@ -1550,21 +1545,19 @@ public class BSPrim : BSPhysObject
         // The sanity check can change the velocity and/or position.
         if (PositionSanityCheck(true /* inTaintTime */ ))
         {
-            entprop.Position = _position;
+            entprop.Position = RawPosition;
             entprop.Velocity = RawVelocity;
             entprop.RotationalVelocity = _rotationalVelocity;
             entprop.Acceleration = _acceleration;
         }
 
-        OMV.Vector3 direction = OMV.Vector3.UnitX * _orientation;   // DEBUG DEBUG DEBUG
+        OMV.Vector3 direction = OMV.Vector3.UnitX * RawOrientation;   // DEBUG DEBUG DEBUG
         DetailLog("{0},BSPrim.UpdateProperties,call,entProp={1},dir={2}", LocalID, entprop, direction);
 
         // remember the current and last set values
         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..2eb1440 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimDisplaced.cs
@@ -23,11 +23,6 @@
  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
- *
- * The quotations from http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial
- * are Copyright (c) 2009 Linden Research, Inc and are used under their license
- * of Creative Commons Attribution-Share Alike 3.0
- * (http://creativecommons.org/licenses/by-sa/3.0/).
  */
 
 using System;
@@ -44,14 +39,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 +54,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 +64,82 @@ 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 = PhysScene.PE.GetPosition(PhysBody);
+            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;
+            }
+            RawPosition = physPosition;
+            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 +148,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 +162,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)
+
+