3 files changed, 434 insertions, 319 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index 43b1262..2e6b104 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -32,10 +32,27 @@ using OMV = OpenMetaverse;
 
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
-public class BSLinkset
+public abstract class BSLinkset
 {
     // private static string LogHeader = "[BULLETSIM LINKSET]";
 
+    // Create the correct type of linkset for this child
+    public static BSLinkset Factory(BSScene physScene, BSPhysObject parent)
+    {
+        BSLinkset ret = null;
+        /*
+        if (parent.IsPhysical)
+            ret = new BSLinksetConstraints(physScene, parent);
+        else
+            ret = new BSLinksetManual(physScene, parent);
+         */
+
+        // at the moment, there is only one
+        ret = new BSLinksetConstraints(physScene, parent);
+
+        return ret;
+    }
+
     public BSPhysObject LinksetRoot { get; protected set; }
 
     public BSScene PhysicsScene { get; private set; }
@@ -52,16 +69,16 @@ public class BSLinkset
     //    the physical 'taint' children separately.
     // After taint processing and before the simulation step, these
     //    two lists must be the same.
-    private HashSet<BSPhysObject> m_children;
-    private HashSet<BSPhysObject> m_taintChildren;
+    protected HashSet<BSPhysObject> m_children;
+    protected HashSet<BSPhysObject> m_taintChildren;
 
     // We lock the diddling of linkset classes to prevent any badness.
     // This locks the modification of the instances of this class. Changes
     //    to the physical representation is done via the tainting mechenism.
-    private object m_linksetActivityLock = new Object();
+    protected object m_linksetActivityLock = new Object();
 
     // We keep the prim's mass in the linkset structure since it could be dependent on other prims
-    private float m_mass;
+    protected float m_mass;
     public float LinksetMass
     {
         get
@@ -81,7 +98,7 @@ public class BSLinkset
         get { return ComputeLinksetGeometricCenter(); }
     }
 
-    public BSLinkset(BSScene scene, BSPhysObject parent)
+    protected void Initialize(BSScene scene, BSPhysObject parent)
     {
         // A simple linkset of one (no children)
         LinksetID = m_nextLinksetID++;
@@ -128,7 +145,7 @@ public class BSLinkset
         }
 
         // The child is down to a linkset of just itself
-        return new BSLinkset(PhysicsScene, child);
+        return BSLinkset.Factory(PhysicsScene, child);
     }
 
     // Return 'true' if the passed object is the root object of this linkset
@@ -163,24 +180,7 @@ public class BSLinkset
     // When physical properties are changed the linkset needs to recalculate
     //   its internal properties.
     // May be called at runtime or taint-time (just pass the appropriate flag).
-    public void Refresh(BSPhysObject requestor, bool inTaintTime)
-    {
-        // If there are no children, not physical or not root, I am not the one that recomputes the constraints
-        // (For the moment, static linksets do create constraints so remove the test for physical.)
-        if (!HasAnyChildren || /*!requestor.IsPhysical ||*/ !IsRoot(requestor))
-            return;
-
-        BSScene.TaintCallback refreshOperation = delegate()
-            {
-                RecomputeLinksetConstraintVariables();
-                DetailLog("{0},BSLinkset.Refresh,complete,rBody={1}", 
-                                LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"));
-            };
-        if (inTaintTime)
-            refreshOperation();
-        else
-            PhysicsScene.TaintedObject("BSLinkSet.Refresh", refreshOperation);
-    }
+    public abstract void Refresh(BSPhysObject requestor, bool inTaintTime);
 
     // The object is going dynamic (physical). Do any setup necessary
     //     for a dynamic linkset.
@@ -188,102 +188,36 @@ public class BSLinkset
     //     has not yet been fully constructed.
     // Return 'true' if any properties updated on the passed object.
     // Called at taint-time!
-    public bool MakeDynamic(BSPhysObject child)
-    {
-        // What is done for each object in BSPrim is what we want.
-        return false;
-    }
+    public abstract bool MakeDynamic(BSPhysObject child);
 
     // The object is going static (non-physical). Do any setup necessary
     //     for a static linkset.
     // Return 'true' if any properties updated on the passed object.
     // Called at taint-time!
-    public bool MakeStatic(BSPhysObject child)
-    {
-        // What is done for each object in BSPrim is what we want.
-        return false;
-    }
+    public abstract bool MakeStatic(BSPhysObject child);
 
     // If the software is handling the movement of all the objects in a linkset
     // (like if one doesn't use constraints for static linksets), this is called
     // when an update for the root of the linkset is received.
     // Called at taint-time!!
-    public void UpdateProperties(BSPhysObject physObject)
-    {
-        // The root local properties have been updated. Apply to the children if appropriate.
-        if (IsRoot(physObject) && HasAnyChildren)
-        {
-            if (!physObject.IsPhysical)
-            {
-                // TODO: implement software linkset update for static object linksets
-            }
-        }
-    }
+    public abstract void UpdateProperties(BSPhysObject physObject);
 
     // Routine used when rebuilding the body of the root of the linkset
     // Destroy all the constraints have have been made to root.
     // This is called when the root body is changing.
     // Returns 'true' of something eas actually removed and would need restoring
     // Called at taint-time!!
-    public bool RemoveBodyDependencies(BSPrim child)
-    {
-        bool ret = false;
-
-        lock (m_linksetActivityLock)
-        {
-            if (IsRoot(child))
-            {
-                // If the one with the dependency is root, must undo all children
-                DetailLog("{0},BSLinkset.RemoveBodyDependencies,removeChildrenForRoot,rID={1},rBody={2}",
-                                                child.LocalID, LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"));
-
-                ret = PhysicallyUnlinkAllChildrenFromRoot(LinksetRoot);
-            }
-            else
-            {
-                DetailLog("{0},BSLinkset.RemoveBodyDependencies,removeSingleChild,rID={1},rBody={2},cID={3},cBody={4}",
-                                                child.LocalID,
-                                                LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"),
-                                                child.LocalID, child.BSBody.ptr.ToString("X"));
-                // ret = PhysicallyUnlinkAChildFromRoot(LinksetRoot, child);
-                // Despite the function name, this removes any link to the specified object.
-                ret = PhysicallyUnlinkAllChildrenFromRoot(child);
-            }
-        }
-        return ret;
-    }
+    public abstract bool RemoveBodyDependencies(BSPrim child);
 
     // Companion to RemoveBodyDependencies(). If RemoveBodyDependencies() returns 'true',
     // this routine will restore the removed constraints.
     // Called at taint-time!!
-    public void RestoreBodyDependencies(BSPrim child)
-    {
-        lock (m_linksetActivityLock)
-        {
-            if (IsRoot(child))
-            {
-                DetailLog("{0},BSLinkset.RestoreBodyDependencies,restoreChildrenForRoot,rID={1},numChild={2}",
-                                                child.LocalID, LinksetRoot.LocalID, m_taintChildren.Count);
-                foreach (BSPhysObject bpo in m_taintChildren)
-                {
-                    PhysicallyLinkAChildToRoot(LinksetRoot, bpo);
-                }
-            }
-            else
-            {
-                DetailLog("{0},BSLinkset.RestoreBodyDependencies,restoreSingleChild,rID={1},rBody={2},cID={3},cBody={4}",
-                                                LinksetRoot.LocalID,
-                                                LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"),
-                                                child.LocalID, child.BSBody.ptr.ToString("X"));
-                PhysicallyLinkAChildToRoot(LinksetRoot, child);
-            }
-        }
-    }
+    public abstract void RestoreBodyDependencies(BSPrim child);
 
     // ================================================================
     // Below this point is internal magic
 
-    private float ComputeLinksetMass()
+    protected virtual float ComputeLinksetMass()
     {
         float mass;
         lock (m_linksetActivityLock)
@@ -297,7 +231,7 @@ public class BSLinkset
         return mass;
     }
 
-    private OMV.Vector3 ComputeLinksetCenterOfMass()
+    protected virtual OMV.Vector3 ComputeLinksetCenterOfMass()
     {
         OMV.Vector3 com;
         lock (m_linksetActivityLock)
@@ -317,7 +251,7 @@ public class BSLinkset
         return com;
     }
 
-    private OMV.Vector3 ComputeLinksetGeometricCenter()
+    protected virtual OMV.Vector3 ComputeLinksetGeometricCenter()
     {
         OMV.Vector3 com;
         lock (m_linksetActivityLock)
@@ -336,236 +270,21 @@ public class BSLinkset
 
     // I am the root of a linkset and a new child is being added
     // Called while LinkActivity is locked.
-    private void AddChildToLinkset(BSPhysObject child)
-    {
-        if (!HasChild(child))
-        {
-            m_children.Add(child);
-
-            BSPhysObject rootx = LinksetRoot; // capture the root as of now
-            BSPhysObject childx = child;
-
-            DetailLog("{0},AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
-
-            PhysicsScene.TaintedObject("AddChildToLinkset", delegate()
-            {
-                DetailLog("{0},AddChildToLinkset,taint,rID={1},rBody={2},cID={3},cBody={4}", 
-                                rootx.LocalID,
-                                rootx.LocalID, rootx.BSBody.ptr.ToString("X"),
-                                childx.LocalID, childx.BSBody.ptr.ToString("X"));
-                // Since this is taint-time, the body and shape could have changed for the child
-                rootx.ForcePosition = rootx.Position;   // DEBUG
-                childx.ForcePosition = childx.Position;   // DEBUG
-                PhysicallyLinkAChildToRoot(rootx, childx);
-                m_taintChildren.Add(child);
-            });
-        }
-        return;
-    }
+    protected abstract void AddChildToLinkset(BSPhysObject child);
 
     // Forcefully removing a child from a linkset.
     // This is not being called by the child so we have to make sure the child doesn't think
     //    it's still connected to the linkset.
     // Normal OpenSimulator operation will never do this because other SceneObjectPart information
     //    also has to be updated (like pointer to prim's parent).
-    private void RemoveChildFromOtherLinkset(BSPhysObject pchild)
-    {
-        pchild.Linkset = new BSLinkset(PhysicsScene, pchild);
-        RemoveChildFromLinkset(pchild);
-    }
+    protected abstract void RemoveChildFromOtherLinkset(BSPhysObject pchild);
 
     // I am the root of a linkset and one of my children is being removed.
     // Safe to call even if the child is not really in my linkset.
-    private void RemoveChildFromLinkset(BSPhysObject child)
-    {
-        if (m_children.Remove(child))
-        {
-            BSPhysObject rootx = LinksetRoot; // capture the root and body as of now
-            BSPhysObject childx = child;
-
-            DetailLog("{0},RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}", 
-                            childx.LocalID,
-                            rootx.LocalID, rootx.BSBody.ptr.ToString("X"),
-                            childx.LocalID, childx.BSBody.ptr.ToString("X"));
-
-            PhysicsScene.TaintedObject("RemoveChildFromLinkset", delegate()
-            {
-                m_taintChildren.Remove(child);
-                PhysicallyUnlinkAChildFromRoot(rootx, childx);
-                RecomputeLinksetConstraintVariables();
-            });
-
-        }
-        else
-        {
-            // This will happen if we remove the root of the linkset first. Non-fatal occurance.
-            // PhysicsScene.Logger.ErrorFormat("{0}: Asked to remove child from linkset that was not in linkset", LogHeader);
-        }
-        return;
-    }
-
-    // Create a constraint between me (root of linkset) and the passed prim (the child).
-    // Called at taint time!
-    private void PhysicallyLinkAChildToRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
-    {
-        // Zero motion for children so they don't interpolate
-        childPrim.ZeroMotion();
-
-        // Relative position normalized to the root prim
-        // Essentually a vector pointing from center of rootPrim to center of childPrim
-        OMV.Vector3 childRelativePosition = childPrim.Position - rootPrim.Position;
-
-        // real world coordinate of midpoint between the two objects
-        OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2);
-
-        DetailLog("{0},BSLinkset.PhysicallyLinkAChildToRoot,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}",
-                                        rootPrim.LocalID,
-                                        rootPrim.LocalID, rootPrim.BSBody.ptr.ToString("X"),
-                                        childPrim.LocalID, childPrim.BSBody.ptr.ToString("X"),
-                                        rootPrim.Position, childPrim.Position, midPoint);
-
-        // create a constraint that allows no freedom of movement between the two objects
-        // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
-
-        BS6DofConstraint constrain = new BS6DofConstraint(
-                            PhysicsScene.World, rootPrim.BSBody, childPrim.BSBody, midPoint, true, true );
-
-        /* NOTE: below is an attempt to build constraint with full frame computation, etc.
-         *     Using the midpoint is easier since it lets the Bullet code manipulate the transforms
-         *     of the objects.
-         * Code left as a warning to future programmers.
-        // ==================================================================================
-        // relative position normalized to the root prim
-        OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation);
-        OMV.Vector3 childRelativePosition = (childPrim.Position - rootPrim.Position) * invThisOrientation;
-
-        // relative rotation of the child to the parent
-        OMV.Quaternion childRelativeRotation = invThisOrientation * childPrim.Orientation;
-        OMV.Quaternion inverseChildRelativeRotation = OMV.Quaternion.Inverse(childRelativeRotation);
-
-        // create a constraint that allows no freedom of movement between the two objects
-        // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
-        DetailLog("{0},BSLinkset.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
-        BS6DofConstraint constrain = new BS6DofConstraint(
-                        PhysicsScene.World, rootPrim.Body, childPrim.Body,
-                        OMV.Vector3.Zero,
-                        OMV.Quaternion.Inverse(rootPrim.Orientation),
-                        OMV.Vector3.Zero,
-                        OMV.Quaternion.Inverse(childPrim.Orientation),
-                        // A point half way between the parent and child
-                        // childRelativePosition/2,
-                        // childRelativeRotation,
-                        // childRelativePosition/2,
-                        // inverseChildRelativeRotation,
-                        true,
-                        true
-                        );
-        // ==================================================================================
-        */
-
-        PhysicsScene.Constraints.AddConstraint(constrain);
-
-        // zero linear and angular limits makes the objects unable to move in relation to each other
-        constrain.SetLinearLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
-        constrain.SetAngularLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
-
-        // tweek the constraint to increase stability
-        constrain.UseFrameOffset(PhysicsScene.BoolNumeric(PhysicsScene.Params.linkConstraintUseFrameOffset));
-        constrain.TranslationalLimitMotor(PhysicsScene.BoolNumeric(PhysicsScene.Params.linkConstraintEnableTransMotor),
-                        PhysicsScene.Params.linkConstraintTransMotorMaxVel,
-                        PhysicsScene.Params.linkConstraintTransMotorMaxForce);
-        constrain.SetCFMAndERP(PhysicsScene.Params.linkConstraintCFM, PhysicsScene.Params.linkConstraintERP);
-        if (PhysicsScene.Params.linkConstraintSolverIterations != 0f)
-        {
-            constrain.SetSolverIterations(PhysicsScene.Params.linkConstraintSolverIterations);
-        }
-    }
-
-    // Remove linkage between myself and a particular child
-    // The root and child bodies are passed in because we need to remove the constraint between
-    //      the bodies that were at unlink time.
-    // Called at taint time!
-    private bool PhysicallyUnlinkAChildFromRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
-    {
-        bool ret = false;
-        DetailLog("{0},BSLinkset.PhysicallyUnlinkAChildFromRoot,taint,root={1},rBody={2},child={3},cBody={4}",
-                            rootPrim.LocalID,
-                            rootPrim.LocalID, rootPrim.BSBody.ptr.ToString("X"),
-                            childPrim.LocalID, childPrim.BSBody.ptr.ToString("X"));
-
-        // Find the constraint for this link and get rid of it from the overall collection and from my list
-        if (PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.BSBody, childPrim.BSBody))
-        {
-            // Make the child refresh its location
-            BulletSimAPI.PushUpdate2(childPrim.BSBody.ptr);
-            ret = true;
-        }
-
-        return ret;
-    }
-
-    // Remove linkage between myself and any possible children I might have.
-    // Called at taint time!
-    private bool PhysicallyUnlinkAllChildrenFromRoot(BSPhysObject rootPrim)
-    {
-        DetailLog("{0},BSLinkset.PhysicallyUnlinkAllChildren,taint", rootPrim.LocalID);
-        bool ret = false;
-
-        if (PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.BSBody))
-        {
-            ret = true;
-        }
-        return ret;
-    }
-
-    // Call each of the constraints that make up this linkset and recompute the
-    //    various transforms and variables. Used when objects are added or removed
-    //    from a linkset to make sure the constraints know about the new mass and
-    //    geometry.
-    // Must only be called at taint time!!
-    private void RecomputeLinksetConstraintVariables()
-    {
-        float linksetMass = LinksetMass;
-        foreach (BSPhysObject child in m_taintChildren)
-        {
-            BSConstraint constrain;
-            if (PhysicsScene.Constraints.TryGetConstraint(LinksetRoot.BSBody, child.BSBody, out constrain))
-            {
-                // DetailLog("{0},BSLinkset.RecomputeLinksetConstraintVariables,taint,child={1},mass={2},A={3},B={4}",
-                //         LinksetRoot.LocalID, child.LocalID, linksetMass, constrain.Body1.ID, constrain.Body2.ID);
-                constrain.RecomputeConstraintVariables(linksetMass);
-            }
-            else
-            {
-                // Non-fatal error that happens when children are being added to the linkset but
-                //    their constraints have not been created yet.
-                break;
-            }
-        }
-
-        // If the whole linkset is not here, doesn't make sense to recompute linkset wide values
-        if (m_children.Count == m_taintChildren.Count)
-        {
-            // If this is a multiple object linkset, set everybody's center of mass to the set's center of mass
-            OMV.Vector3 centerOfMass = ComputeLinksetCenterOfMass();
-            BulletSimAPI.SetCenterOfMassByPosRot2(LinksetRoot.BSBody.ptr, 
-                                centerOfMass, OMV.Quaternion.Identity);
-            DetailLog("{0},BSLinkset.RecomputeLinksetConstraintVariables,setCenterOfMass,COM={1},rBody={2}",
-                                LinksetRoot.LocalID, centerOfMass, LinksetRoot.BSBody.ptr.ToString("X"));
-            foreach (BSPhysObject child in m_taintChildren)
-            {
-                BulletSimAPI.SetCenterOfMassByPosRot2(child.BSBody.ptr, 
-                                centerOfMass, OMV.Quaternion.Identity);
-            }
-
-            // BulletSimAPI.DumpAllInfo2(PhysicsScene.World.ptr);  // DEBUG DEBUG DEBUG
-        }
-        return;
-    }
-
+    protected abstract void RemoveChildFromLinkset(BSPhysObject child);
 
     // Invoke the detailed logger and output something if it's enabled.
-    private void DetailLog(string msg, params Object[] args)
+    protected void DetailLog(string msg, params Object[] args)
     {
         if (PhysicsScene.PhysicsLogging.Enabled)
             PhysicsScene.DetailLog(msg, args);
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
new file mode 100755
index 0000000..ee53d92
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
@@ -0,0 +1,396 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyrightD
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * 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.
+ */
+using System;
+using System.Collections.Generic;
+using System.Text;
+
+using OMV = OpenMetaverse;
+
+namespace OpenSim.Region.Physics.BulletSPlugin
+{
+public class BSLinksetConstraints : BSLinkset
+{
+    // private static string LogHeader = "[BULLETSIM LINKSET CONSTRAINTS]";
+
+    public BSLinksetConstraints(BSScene scene, BSPhysObject parent)
+    {
+        base.Initialize(scene, parent);
+    }
+
+    // When physical properties are changed the linkset needs to recalculate
+    //   its internal properties.
+    // May be called at runtime or taint-time (just pass the appropriate flag).
+    public override void Refresh(BSPhysObject requestor, bool inTaintTime)
+    {
+        // If there are no children, not physical or not root, I am not the one that recomputes the constraints
+        // (For the moment, static linksets do create constraints so remove the test for physical.)
+        if (!HasAnyChildren || /*!requestor.IsPhysical ||*/ !IsRoot(requestor))
+            return;
+
+        BSScene.TaintCallback refreshOperation = delegate()
+            {
+                RecomputeLinksetConstraintVariables();
+                DetailLog("{0},BSLinkset.Refresh,complete,rBody={1}", 
+                                LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"));
+            };
+        if (inTaintTime)
+            refreshOperation();
+        else
+            PhysicsScene.TaintedObject("BSLinkSet.Refresh", refreshOperation);
+    }
+
+    // The object is going dynamic (physical). Do any setup necessary
+    //     for a dynamic linkset.
+    // Only the state of the passed object can be modified. The rest of the linkset
+    //     has not yet been fully constructed.
+    // Return 'true' if any properties updated on the passed object.
+    // Called at taint-time!
+    public override bool MakeDynamic(BSPhysObject child)
+    {
+        // What is done for each object in BSPrim is what we want.
+        return false;
+    }
+
+    // The object is going static (non-physical). Do any setup necessary
+    //     for a static linkset.
+    // Return 'true' if any properties updated on the passed object.
+    // Called at taint-time!
+    public override bool MakeStatic(BSPhysObject child)
+    {
+        // What is done for each object in BSPrim is what we want.
+        return false;
+    }
+
+    // If the software is handling the movement of all the objects in a linkset
+    // (like if one doesn't use constraints for static linksets), this is called
+    // when an update for the root of the linkset is received.
+    // Called at taint-time!!
+    public override void UpdateProperties(BSPhysObject physObject)
+    {
+        // The root local properties have been updated. Apply to the children if appropriate.
+        if (IsRoot(physObject) && HasAnyChildren)
+        {
+            if (!physObject.IsPhysical)
+            {
+                // TODO: implement software linkset update for static object linksets
+            }
+        }
+    }
+
+    // Routine used when rebuilding the body of the root of the linkset
+    // Destroy all the constraints have have been made to root.
+    // This is called when the root body is changing.
+    // Returns 'true' of something eas actually removed and would need restoring
+    // Called at taint-time!!
+    public override bool RemoveBodyDependencies(BSPrim child)
+    {
+        bool ret = false;
+
+        lock (m_linksetActivityLock)
+        {
+            if (IsRoot(child))
+            {
+                // If the one with the dependency is root, must undo all children
+                DetailLog("{0},BSLinkset.RemoveBodyDependencies,removeChildrenForRoot,rID={1},rBody={2}",
+                                                child.LocalID, LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"));
+
+                ret = PhysicallyUnlinkAllChildrenFromRoot(LinksetRoot);
+            }
+            else
+            {
+                DetailLog("{0},BSLinkset.RemoveBodyDependencies,removeSingleChild,rID={1},rBody={2},cID={3},cBody={4}",
+                                                child.LocalID,
+                                                LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"),
+                                                child.LocalID, child.BSBody.ptr.ToString("X"));
+                // ret = PhysicallyUnlinkAChildFromRoot(LinksetRoot, child);
+                // Despite the function name, this removes any link to the specified object.
+                ret = PhysicallyUnlinkAllChildrenFromRoot(child);
+            }
+        }
+        return ret;
+    }
+
+    // Companion to RemoveBodyDependencies(). If RemoveBodyDependencies() returns 'true',
+    // this routine will restore the removed constraints.
+    // Called at taint-time!!
+    public override void RestoreBodyDependencies(BSPrim child)
+    {
+        lock (m_linksetActivityLock)
+        {
+            if (IsRoot(child))
+            {
+                DetailLog("{0},BSLinkset.RestoreBodyDependencies,restoreChildrenForRoot,rID={1},numChild={2}",
+                                                child.LocalID, LinksetRoot.LocalID, m_taintChildren.Count);
+                foreach (BSPhysObject bpo in m_taintChildren)
+                {
+                    PhysicallyLinkAChildToRoot(LinksetRoot, bpo);
+                }
+            }
+            else
+            {
+                DetailLog("{0},BSLinkset.RestoreBodyDependencies,restoreSingleChild,rID={1},rBody={2},cID={3},cBody={4}",
+                                                LinksetRoot.LocalID,
+                                                LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"),
+                                                child.LocalID, child.BSBody.ptr.ToString("X"));
+                PhysicallyLinkAChildToRoot(LinksetRoot, child);
+            }
+        }
+    }
+
+    // ================================================================
+    // Below this point is internal magic
+
+    // I am the root of a linkset and a new child is being added
+    // Called while LinkActivity is locked.
+    protected override void AddChildToLinkset(BSPhysObject child)
+    {
+        if (!HasChild(child))
+        {
+            m_children.Add(child);
+
+            BSPhysObject rootx = LinksetRoot; // capture the root as of now
+            BSPhysObject childx = child;
+
+            DetailLog("{0},AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
+
+            PhysicsScene.TaintedObject("AddChildToLinkset", delegate()
+            {
+                DetailLog("{0},AddChildToLinkset,taint,rID={1},rBody={2},cID={3},cBody={4}", 
+                                rootx.LocalID,
+                                rootx.LocalID, rootx.BSBody.ptr.ToString("X"),
+                                childx.LocalID, childx.BSBody.ptr.ToString("X"));
+                // Since this is taint-time, the body and shape could have changed for the child
+                rootx.ForcePosition = rootx.Position;   // DEBUG
+                childx.ForcePosition = childx.Position;   // DEBUG
+                PhysicallyLinkAChildToRoot(rootx, childx);
+                m_taintChildren.Add(child);
+            });
+        }
+        return;
+    }
+
+    // Forcefully removing a child from a linkset.
+    // This is not being called by the child so we have to make sure the child doesn't think
+    //    it's still connected to the linkset.
+    // Normal OpenSimulator operation will never do this because other SceneObjectPart information
+    //    also has to be updated (like pointer to prim's parent).
+    protected override void RemoveChildFromOtherLinkset(BSPhysObject pchild)
+    {
+        pchild.Linkset = BSLinkset.Factory(PhysicsScene, pchild);
+        RemoveChildFromLinkset(pchild);
+    }
+
+    // I am the root of a linkset and one of my children is being removed.
+    // Safe to call even if the child is not really in my linkset.
+    protected override void RemoveChildFromLinkset(BSPhysObject child)
+    {
+        if (m_children.Remove(child))
+        {
+            BSPhysObject rootx = LinksetRoot; // capture the root and body as of now
+            BSPhysObject childx = child;
+
+            DetailLog("{0},RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}", 
+                            childx.LocalID,
+                            rootx.LocalID, rootx.BSBody.ptr.ToString("X"),
+                            childx.LocalID, childx.BSBody.ptr.ToString("X"));
+
+            PhysicsScene.TaintedObject("RemoveChildFromLinkset", delegate()
+            {
+                m_taintChildren.Remove(child);
+                PhysicallyUnlinkAChildFromRoot(rootx, childx);
+                RecomputeLinksetConstraintVariables();
+            });
+
+        }
+        else
+        {
+            // This will happen if we remove the root of the linkset first. Non-fatal occurance.
+            // PhysicsScene.Logger.ErrorFormat("{0}: Asked to remove child from linkset that was not in linkset", LogHeader);
+        }
+        return;
+    }
+
+    // Create a constraint between me (root of linkset) and the passed prim (the child).
+    // Called at taint time!
+    private void PhysicallyLinkAChildToRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
+    {
+        // Zero motion for children so they don't interpolate
+        childPrim.ZeroMotion();
+
+        // Relative position normalized to the root prim
+        // Essentually a vector pointing from center of rootPrim to center of childPrim
+        OMV.Vector3 childRelativePosition = childPrim.Position - rootPrim.Position;
+
+        // real world coordinate of midpoint between the two objects
+        OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2);
+
+        DetailLog("{0},BSLinkset.PhysicallyLinkAChildToRoot,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}",
+                                        rootPrim.LocalID,
+                                        rootPrim.LocalID, rootPrim.BSBody.ptr.ToString("X"),
+                                        childPrim.LocalID, childPrim.BSBody.ptr.ToString("X"),
+                                        rootPrim.Position, childPrim.Position, midPoint);
+
+        // create a constraint that allows no freedom of movement between the two objects
+        // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
+
+        BS6DofConstraint constrain = new BS6DofConstraint(
+                            PhysicsScene.World, rootPrim.BSBody, childPrim.BSBody, midPoint, true, true );
+
+        /* NOTE: below is an attempt to build constraint with full frame computation, etc.
+         *     Using the midpoint is easier since it lets the Bullet code manipulate the transforms
+         *     of the objects.
+         * Code left as a warning to future programmers.
+        // ==================================================================================
+        // relative position normalized to the root prim
+        OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation);
+        OMV.Vector3 childRelativePosition = (childPrim.Position - rootPrim.Position) * invThisOrientation;
+
+        // relative rotation of the child to the parent
+        OMV.Quaternion childRelativeRotation = invThisOrientation * childPrim.Orientation;
+        OMV.Quaternion inverseChildRelativeRotation = OMV.Quaternion.Inverse(childRelativeRotation);
+
+        // create a constraint that allows no freedom of movement between the two objects
+        // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
+        DetailLog("{0},BSLinkset.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
+        BS6DofConstraint constrain = new BS6DofConstraint(
+                        PhysicsScene.World, rootPrim.Body, childPrim.Body,
+                        OMV.Vector3.Zero,
+                        OMV.Quaternion.Inverse(rootPrim.Orientation),
+                        OMV.Vector3.Zero,
+                        OMV.Quaternion.Inverse(childPrim.Orientation),
+                        // A point half way between the parent and child
+                        // childRelativePosition/2,
+                        // childRelativeRotation,
+                        // childRelativePosition/2,
+                        // inverseChildRelativeRotation,
+                        true,
+                        true
+                        );
+        // ==================================================================================
+        */
+
+        PhysicsScene.Constraints.AddConstraint(constrain);
+
+        // zero linear and angular limits makes the objects unable to move in relation to each other
+        constrain.SetLinearLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
+        constrain.SetAngularLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
+
+        // tweek the constraint to increase stability
+        constrain.UseFrameOffset(PhysicsScene.BoolNumeric(PhysicsScene.Params.linkConstraintUseFrameOffset));
+        constrain.TranslationalLimitMotor(PhysicsScene.BoolNumeric(PhysicsScene.Params.linkConstraintEnableTransMotor),
+                        PhysicsScene.Params.linkConstraintTransMotorMaxVel,
+                        PhysicsScene.Params.linkConstraintTransMotorMaxForce);
+        constrain.SetCFMAndERP(PhysicsScene.Params.linkConstraintCFM, PhysicsScene.Params.linkConstraintERP);
+        if (PhysicsScene.Params.linkConstraintSolverIterations != 0f)
+        {
+            constrain.SetSolverIterations(PhysicsScene.Params.linkConstraintSolverIterations);
+        }
+    }
+
+    // Remove linkage between myself and a particular child
+    // The root and child bodies are passed in because we need to remove the constraint between
+    //      the bodies that were at unlink time.
+    // Called at taint time!
+    private bool PhysicallyUnlinkAChildFromRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
+    {
+        bool ret = false;
+        DetailLog("{0},BSLinkset.PhysicallyUnlinkAChildFromRoot,taint,root={1},rBody={2},child={3},cBody={4}",
+                            rootPrim.LocalID,
+                            rootPrim.LocalID, rootPrim.BSBody.ptr.ToString("X"),
+                            childPrim.LocalID, childPrim.BSBody.ptr.ToString("X"));
+
+        // Find the constraint for this link and get rid of it from the overall collection and from my list
+        if (PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.BSBody, childPrim.BSBody))
+        {
+            // Make the child refresh its location
+            BulletSimAPI.PushUpdate2(childPrim.BSBody.ptr);
+            ret = true;
+        }
+
+        return ret;
+    }
+
+    // Remove linkage between myself and any possible children I might have.
+    // Called at taint time!
+    private bool PhysicallyUnlinkAllChildrenFromRoot(BSPhysObject rootPrim)
+    {
+        DetailLog("{0},BSLinkset.PhysicallyUnlinkAllChildren,taint", rootPrim.LocalID);
+        bool ret = false;
+
+        if (PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.BSBody))
+        {
+            ret = true;
+        }
+        return ret;
+    }
+
+    // Call each of the constraints that make up this linkset and recompute the
+    //    various transforms and variables. Used when objects are added or removed
+    //    from a linkset to make sure the constraints know about the new mass and
+    //    geometry.
+    // Must only be called at taint time!!
+    private void RecomputeLinksetConstraintVariables()
+    {
+        float linksetMass = LinksetMass;
+        foreach (BSPhysObject child in m_taintChildren)
+        {
+            BSConstraint constrain;
+            if (PhysicsScene.Constraints.TryGetConstraint(LinksetRoot.BSBody, child.BSBody, out constrain))
+            {
+                // DetailLog("{0},BSLinkset.RecomputeLinksetConstraintVariables,taint,child={1},mass={2},A={3},B={4}",
+                //         LinksetRoot.LocalID, child.LocalID, linksetMass, constrain.Body1.ID, constrain.Body2.ID);
+                constrain.RecomputeConstraintVariables(linksetMass);
+            }
+            else
+            {
+                // Non-fatal error that happens when children are being added to the linkset but
+                //    their constraints have not been created yet.
+                break;
+            }
+        }
+
+        // If the whole linkset is not here, doesn't make sense to recompute linkset wide values
+        if (m_children.Count == m_taintChildren.Count)
+        {
+            // If this is a multiple object linkset, set everybody's center of mass to the set's center of mass
+            OMV.Vector3 centerOfMass = ComputeLinksetCenterOfMass();
+            BulletSimAPI.SetCenterOfMassByPosRot2(LinksetRoot.BSBody.ptr, 
+                                centerOfMass, OMV.Quaternion.Identity);
+            DetailLog("{0},BSLinkset.RecomputeLinksetConstraintVariables,setCenterOfMass,COM={1},rBody={2}",
+                                LinksetRoot.LocalID, centerOfMass, LinksetRoot.BSBody.ptr.ToString("X"));
+            foreach (BSPhysObject child in m_taintChildren)
+            {
+                BulletSimAPI.SetCenterOfMassByPosRot2(child.BSBody.ptr, 
+                                centerOfMass, OMV.Quaternion.Identity);
+            }
+
+            // BulletSimAPI.DumpAllInfo2(PhysicsScene.World.ptr);  // DEBUG DEBUG DEBUG
+        }
+        return;
+    }
+}
+}
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index ead6a08..51b9196 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -46,7 +46,7 @@ public abstract class BSPhysObject : PhysicsActor
         PhysObjectName = name;
         TypeName = typeName;
 
-        Linkset = new BSLinkset(PhysicsScene, this);
+        Linkset = BSLinkset.Factory(PhysicsScene, this);
         LastAssetBuildFailed = false;
 
         CollisionCollection = new CollisionEventUpdate();