BulletSim: move center of gravity of linkset to its geometric center.

Necessitated allowing simulator and physical position of a body to
get out of sync since Bullet assumes that <0,0,0> is the center of mass.
Update DLLs and SOs for the UpdateChildTransform so positions of
  individual prim in a linkset can be implemented.

OpenSim/Region/Physics/BulletSPlugin/BSAPIUnman.cs

@@ -327,6 +327,12 @@ public override void RemoveChildShapeFromCompoundShape(BulletShape shape, Bullet
     BSAPICPP.RemoveChildShapeFromCompoundShape2(shapeu.ptr, removeShapeu.ptr);
 }
 
+public override void UpdateChildTransform(BulletShape pShape, int childIndex, Vector3 pos, Quaternion rot, bool shouldRecalculateLocalAabb)
+{
+    BulletShapeUnman shapeu = pShape as BulletShapeUnman;
+    BSAPICPP.UpdateChildTransform2(shapeu.ptr, childIndex, pos, rot, shouldRecalculateLocalAabb);
+}
+
 public override void RecalculateCompoundShapeLocalAabb(BulletShape shape)
 {
     BulletShapeUnman shapeu = shape as BulletShapeUnman;
@@ -1356,6 +1362,9 @@ public static extern IntPtr RemoveChildShapeFromCompoundShapeIndex2(IntPtr cShap
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern void RemoveChildShapeFromCompoundShape2(IntPtr cShape, IntPtr removeShape);
 
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void UpdateChildTransform2(IntPtr pShape, int childIndex, Vector3 pos, Quaternion rot, bool shouldRecalculateLocalAabb);
+
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern void RecalculateCompoundShapeLocalAabb2(IntPtr cShape);
 

OpenSim/Region/Physics/BulletSPlugin/BSAPIXNA.cs

@@ -1212,6 +1212,7 @@ private sealed class BulletConstraintXNA : BulletConstraint
 
     public override BulletShape GetChildShapeFromCompoundShapeIndex(BulletShape cShape, int indx) { /* TODO */ return null; }
     public override void RemoveChildShapeFromCompoundShape(BulletShape cShape, BulletShape removeShape) { /* TODO */ }
+    public override void UpdateChildTransform(BulletShape pShape, int childIndex, Vector3 pos, Quaternion rot, bool shouldRecalculateLocalAabb) { /* TODO */ }
 
     public override BulletShape CreateGroundPlaneShape(uint pLocalId, float pheight, float pcollisionMargin)
     {

OpenSim/Region/Physics/BulletSPlugin/BSApiTemplate.cs

@@ -342,6 +342,8 @@ public abstract BulletShape RemoveChildShapeFromCompoundShapeIndex(BulletShape c
 
 public abstract void RemoveChildShapeFromCompoundShape(BulletShape cShape, BulletShape removeShape);
 
+public abstract void UpdateChildTransform(BulletShape pShape, int childIndex, Vector3 pos, Quaternion rot, bool shouldRecalculateLocalAabb);
+
 public abstract void RecalculateCompoundShapeLocalAabb(BulletShape cShape);
 
 public abstract BulletShape DuplicateCollisionShape(BulletWorld sim, BulletShape srcShape, uint id);

OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs

@@ -148,7 +148,6 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 enableAngularVerticalAttraction = false;
                 enableAngularDeflection = false;
                 enableAngularBanking = false;
-                VDetailLog("{0},BSDynamics.SetupVehicleDebugging,settingDebugMode", Prim.LocalID);
             }
         }
 
@@ -690,10 +689,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 }
 
                 if ((m_knownChanged & m_knownChangedForce) != 0)
-                    Prim.AddForce((Vector3)m_knownForce, false, true);
+                    Prim.AddForce((Vector3)m_knownForce, false /*pushForce*/, true /*inTaintTime*/);
 
                 if ((m_knownChanged & m_knownChangedForceImpulse) != 0)
-                    Prim.AddForceImpulse((Vector3)m_knownForceImpulse, false, true);
+                    Prim.AddForceImpulse((Vector3)m_knownForceImpulse, false /*pushforce*/, true /*inTaintTime*/);
 
                 if ((m_knownChanged & m_knownChangedRotationalVelocity) != 0)
                 {
@@ -703,7 +702,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
                 if ((m_knownChanged & m_knownChangedRotationalForce) != 0)
                 {
-                    Prim.AddAngularForce((Vector3)m_knownRotationalForce, false, true);
+                    Prim.AddAngularForce((Vector3)m_knownRotationalForce, false /*pushForce*/, true /*inTaintTime*/);
                 }
 
                 // If we set one of the values (ie, the physics engine didn't do it) we must force
@@ -970,7 +969,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // Act against the inertia of the vehicle
             linearMotorForce *= m_vehicleMass;
 
-            VehicleAddForceImpulse(linearMotorForce);
+            VehicleAddForceImpulse(linearMotorForce * pTimestep);
 
             VDetailLog("{0},  MoveLinear,velocity,vehVel={1},step={2},stepVel={3},mix={4},force={5}",
                         Prim.LocalID, VehicleVelocity, linearMotorStep, linearMotorVelocity, mixFactor, linearMotorForce);
@@ -1033,7 +1032,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 {
                     // Error is positive if below the target and negative if above.
                     float verticalError = m_VhoverTargetHeight - VehiclePosition.Z;
-                    float verticalCorrectionVelocity = verticalError / m_VhoverTimescale;
+                    float verticalCorrectionVelocity = verticalError / m_VhoverTimescale * pTimestep;
 
                     // TODO: implement m_VhoverEfficiency correctly
                     VehicleAddForceImpulse(new Vector3(0f, 0f, verticalCorrectionVelocity));
@@ -1323,7 +1322,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             //     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 && VehicleVelocity != Vector3.Zero)
+            if (enableAngularDeflection && m_angularDeflectionEfficiency != 0 && VehicleForwardSpeed > 0.2)
             {
                 // The direction the vehicle is moving
                 Vector3 movingDirection = VehicleVelocity;

OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs

@@ -152,6 +152,7 @@ public abstract class BSLinkset
             if (IsRoot(child))
             {
                 // Cannot remove the root from a linkset.
+                child.PositionDisplacement = OMV.Vector3.Zero;
                 return this;
             }
             RemoveChildFromLinkset(child);
@@ -159,6 +160,7 @@ public abstract class BSLinkset
         }
 
         // The child is down to a linkset of just itself
+        child.PositionDisplacement = OMV.Vector3.Zero;
         return BSLinkset.Factory(PhysicsScene, child);
     }
 
@@ -310,7 +312,7 @@ public abstract class BSLinkset
 
             foreach (BSPhysObject bp in m_children)
             {
-                com += bp.Position * bp.RawMass;
+                com += bp.Position;
             }
             com /= (m_children.Count + 1);
         }

OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs

@@ -40,23 +40,33 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 //    removed from the linkset.
 sealed class BSLinksetCompoundInfo : BSLinksetInfo
 {
-    public OMV.Vector3 OffsetPos;
+    public int Index;
+    public OMV.Vector3 OffsetFromRoot;
+    public OMV.Vector3 OffsetFromCenterOfMass;
     public OMV.Quaternion OffsetRot;
-    public BSLinksetCompoundInfo(OMV.Vector3 p, OMV.Quaternion r)
+    public BSLinksetCompoundInfo(int indx, OMV.Vector3 p, OMV.Quaternion r)
     {
-        OffsetPos = p;
+        Index = indx;
+        OffsetFromRoot = p;
+        OffsetFromCenterOfMass = p;
         OffsetRot = r;
     }
     public override void Clear()
     {
-        OffsetPos = OMV.Vector3.Zero;
+        Index = 0;
+        OffsetFromRoot = OMV.Vector3.Zero;
+        OffsetFromCenterOfMass = OMV.Vector3.Zero;
         OffsetRot = OMV.Quaternion.Identity;
     }
     public override string ToString()
     {
         StringBuilder buff = new StringBuilder();
-        buff.Append("<p=");
+        buff.Append("<i=");
-        buff.Append(OffsetPos.ToString());
+        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(">");
@@ -170,6 +180,8 @@ public sealed class BSLinksetCompound : BSLinkset
         return ret;
     }
 
+    // 'physicalUpdate' is true if these changes came directly from the physics engine. Don't need to rebuild then.
+    // Called at taint-time.
     public override void UpdateProperties(BSPhysObject updated, bool physicalUpdate)
     {
         // The user moving a child around requires the rebuilding of the linkset compound shape
@@ -182,6 +194,7 @@ public sealed class BSLinksetCompound : BSLinkset
                 && !physicalUpdate 
                 && PhysicsScene.TerrainManager.IsWithinKnownTerrain(LinksetRoot.RawPosition))
         {
+            // TODO: replace this with are calculation of the child prim's orientation and pos.
             updated.LinksetInfo = null;
             ScheduleRebuild(updated);
         }
@@ -230,7 +243,7 @@ public sealed class BSLinksetCompound : BSLinkset
             if (inTaintTime)
             {
                 OMV.Vector3 oldPos = child.RawPosition;
-                child.ForcePosition = LinksetRoot.RawPosition + lci.OffsetPos;
+                child.ForcePosition = LinksetRoot.RawPosition + lci.OffsetFromRoot;
                 child.ForceOrientation = LinksetRoot.RawOrientation * lci.OffsetRot;
                 DetailLog("{0},BSLinksetCompound.RecomputeChildWorldPosition,oldPos={1},lci={2},newPos={3}",
                                             child.LocalID, oldPos, lci, child.RawPosition);
@@ -238,7 +251,7 @@ public sealed class BSLinksetCompound : BSLinkset
             else
             {
                 // TaintedObject is not used here so the raw position is set now and not at taint-time.
-                child.Position = LinksetRoot.RawPosition + lci.OffsetPos;
+                child.Position = LinksetRoot.RawPosition + lci.OffsetFromRoot;
                 child.Orientation = LinksetRoot.RawOrientation * lci.OffsetRot;
             }
         }
@@ -316,10 +329,23 @@ public sealed class BSLinksetCompound : BSLinkset
             // Cause the root shape to be rebuilt as a compound object with just the root in it
             LinksetRoot.ForceBodyShapeRebuild(true);
 
+            // 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.
+            OMV.Vector3 centerOfMass = ComputeLinksetGeometricCenter();
+            OMV.Vector3 centerDisplacement = centerOfMass - LinksetRoot.RawPosition;
+
+            // Since we're displacing the center of the shape, we need to move the body in the world
+            LinksetRoot.PositionDisplacement = centerDisplacement * LinksetRoot.RawOrientation;
+
+            PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, 0, -centerDisplacement, OMV.Quaternion.Identity, false);
+            DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,COM,com={1},rootPos={2},centerDisp={3}",
+                                    LinksetRoot.LocalID, centerOfMass, LinksetRoot.RawPosition, centerDisplacement);
+
             DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,start,rBody={1},rShape={2},numChildren={3}",
                             LinksetRoot.LocalID, LinksetRoot.PhysBody, LinksetRoot.PhysShape, NumberOfChildren);
 
             // Add a shape for each of the other children in the linkset
+            int memberIndex = 1;
             ForEachMember(delegate(BSPhysObject cPrim)
             {
                 if (!IsRoot(cPrim))
@@ -337,13 +363,14 @@ public sealed class BSLinksetCompound : BSLinkset
                         OMV.Quaternion displacementRot = cPrim.RawOrientation * invRootOrientation;
 
                         // Save relative position for recomputing child's world position after moving linkset.
-                        lci = new BSLinksetCompoundInfo(displacementPos, displacementRot);
+                        lci = new BSLinksetCompoundInfo(memberIndex, displacementPos, displacementRot);
+                        lci.OffsetFromCenterOfMass = displacementPos - centerDisplacement;
                         cPrim.LinksetInfo = lci;
                         DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,creatingRelPos,lci={1}", cPrim.LocalID, lci);
                     }
 
-                    DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addMemberToShape,mID={1},mShape={2},dispPos={3},dispRot={4}",
+                    DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addMemberToShape,mID={1},mShape={2},lci={3}",
-                        LinksetRoot.LocalID, cPrim.LocalID, cPrim.PhysShape, lci.OffsetPos, lci.OffsetRot);
+                                                LinksetRoot.LocalID, cPrim.LocalID, cPrim.PhysShape, lci);
 
                     if (cPrim.PhysShape.isNativeShape)
                     {
@@ -359,7 +386,7 @@ public sealed class BSLinksetCompound : BSLinkset
                         PhysicsScene.Shapes.CreateGeomMeshOrHull(cPrim, null);
                         BulletShape newShape = cPrim.PhysShape;
                         cPrim.PhysShape = saveShape;
-                        PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, newShape, lci.OffsetPos, lci.OffsetRot);
+                        PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, newShape, lci.OffsetFromCenterOfMass, lci.OffsetRot);
                     }
                     else
                     {
@@ -371,8 +398,10 @@ public sealed class BSLinksetCompound : BSLinkset
                             PhysicsScene.Logger.ErrorFormat("{0} Rebuilt sharable shape when building linkset! Region={1}, primID={2}, shape={3}",
                                                 LogHeader, PhysicsScene.RegionName, cPrim.LocalID, cPrim.PhysShape);
                         }
-                        PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, cPrim.PhysShape, lci.OffsetPos, lci.OffsetRot);
+                        PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, cPrim.PhysShape, lci.OffsetFromCenterOfMass, lci.OffsetRot);
                     }
+                    lci.Index = memberIndex;
+                    memberIndex++;
                 }
                 return false;   // 'false' says to move onto the next child in the list
             });

OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs

@@ -73,6 +73,8 @@ public abstract class BSPhysObject : PhysicsActor
 
         // A linkset of just me
         Linkset = BSLinkset.Factory(PhysicsScene, this);
+        PositionDisplacement = OMV.Vector3.Zero;
+
         LastAssetBuildFailed = false;
 
         // Default material type
@@ -157,6 +159,14 @@ public abstract class BSPhysObject : PhysicsActor
     public abstract OMV.Vector3 RawPosition { get; set; }
     public abstract OMV.Vector3 ForcePosition { get; set; }
 
+    // Position is what the simulator thinks the positions of the prim is.
+    // Because Bullet needs the zero coordinate to be the center of mass of the linkset,
+    //     sometimes it is necessary to displace the position the physics engine thinks
+    //     the position is. PositionDisplacement must be added and removed from the
+    //     position as the simulator position is stored and fetched from the physics
+    //     engine. 
+    public virtual OMV.Vector3 PositionDisplacement { get; set; }
+
     public abstract OMV.Quaternion RawOrientation { get; set; }
     public abstract OMV.Quaternion ForceOrientation { get; set; }
 

OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs

@@ -50,7 +50,10 @@ public sealed class BSPrim : BSPhysObject
     private bool _grabbed;
     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 float _density;
     private OMV.Vector3 _force;
@@ -320,18 +323,37 @@ public sealed class BSPrim : BSPhysObject
     }
     public override OMV.Vector3 ForcePosition {
         get {
-            _position = PhysicsScene.PE.GetPosition(PhysBody);
+            _position = PhysicsScene.PE.GetPosition(PhysBody) - PositionDisplacement;
             return _position;
         }
         set {
             _position = value;
             if (PhysBody.HasPhysicalBody)
             {
-                PhysicsScene.PE.SetTranslation(PhysBody, _position, _orientation);
+                PhysicsScene.PE.SetTranslation(PhysBody, _position + PositionDisplacement, _orientation);
                 ActivateIfPhysical(false);
             }
         }
     }
+    // Override to have position displacement immediately update the physical position.
+    // A feeble attempt to keep the sim and physical positions in sync
+    // Must be called at taint time.
+    public override OMV.Vector3 PositionDisplacement
+    {
+        get
+        {
+            return base.PositionDisplacement;
+        }
+        set
+        {
+            base.PositionDisplacement = value;
+            PhysicsScene.TaintedObject(PhysicsScene.InTaintTime, "BSPrim.setPosition", delegate()
+            {
+                if (PhysBody.HasPhysicalBody)
+                    PhysicsScene.PE.SetTranslation(PhysBody, _position + base.PositionDisplacement, _orientation);
+            });
+        }
+    }
 
     // Check that the current position is sane and, if not, modify the position to make it so.
     // Check for being below terrain and being out of bounds.
@@ -590,6 +612,7 @@ public sealed class BSPrim : BSPhysObject
             _velocity = value;
             if (PhysBody.HasPhysicalBody)
             {
+                DetailLog("{0},BSPrim.ForceVelocity,taint,vel={1}", LocalID, _velocity);
                 PhysicsScene.PE.SetLinearVelocity(PhysBody, _velocity);
                 ActivateIfPhysical(false);
             }
@@ -654,12 +677,7 @@ public sealed class BSPrim : BSPhysObject
 
             PhysicsScene.TaintedObject("BSPrim.setOrientation", delegate()
             {
-                if (PhysBody.HasPhysicalBody)
+                ForceOrientation = _orientation;
-                {
-                    // _position = PhysicsScene.PE.GetObjectPosition(PhysicsScene.World, BSBody);
-                    // DetailLog("{0},BSPrim.setOrientation,taint,pos={1},orient={2}", LocalID, _position, _orientation);
-                    PhysicsScene.PE.SetTranslation(PhysBody, _position, _orientation);
-                }
             });
         }
     }
@@ -674,7 +692,8 @@ public sealed class BSPrim : BSPhysObject
         set
         {
             _orientation = value;
-            PhysicsScene.PE.SetTranslation(PhysBody, _position, _orientation);
+            if (PhysBody.HasPhysicalBody)
+                PhysicsScene.PE.SetTranslation(PhysBody, _position + PositionDisplacement, _orientation);
         }
     }
     public override int PhysicsActorType {
@@ -813,7 +832,7 @@ public sealed class BSPrim : BSPhysObject
             // PhysicsScene.PE.ClearAllForces(BSBody);
 
             // For good measure, make sure the transform is set through to the motion state
-            PhysicsScene.PE.SetTranslation(PhysBody, _position, _orientation);
+            PhysicsScene.PE.SetTranslation(PhysBody, _position + PositionDisplacement, _orientation);
 
             // Center of mass is at the center of the object
             // DEBUG DEBUG PhysicsScene.PE.SetCenterOfMassByPosRot(Linkset.LinksetRoot.PhysBody, _position, _orientation);
@@ -1615,6 +1634,7 @@ public sealed class BSPrim : BSPhysObject
             }
 
             // Assign directly to the local variables so the normal set actions do not happen
+            entprop.Position -= PositionDisplacement;
             _position = entprop.Position;
             _orientation = entprop.Rotation;
             _velocity = entprop.Velocity;

OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs

@@ -906,7 +906,7 @@ public sealed class BSShapeCollection : IDisposable
             }
         }
 
-        // While we figure out the real problem, stick a simple native shape on the object.
+        // While we figure out the real problem, stick in a simple box for the object.
         BulletShape fillinShape =
             BuildPhysicalNativeShape(prim, BSPhysicsShapeType.SHAPE_BOX, FixedShapeKey.KEY_BOX);
 

OpenSim/Region/Physics/BulletSPlugin/BulletSimTODO.txt

@@ -170,6 +170,8 @@ Avatar attachments have no mass? http://forums-archive.secondlife.com/54/f0/3179
 
 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.