6 files changed, 280 insertions, 86 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index 7f94666..3afd52e 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -70,6 +70,17 @@ public abstract class BSLinkset
         return ret;
     }
 
+    public class BSLinkInfo
+    {
+        public BSPrimLinkable member;
+        public BSLinkInfo(BSPrimLinkable pMember)
+        {
+            member = pMember;
+        }
+    }
+
+    public LinksetImplementation LinksetImpl { get; protected set; }
+
     public BSPrimLinkable LinksetRoot { get; protected set; }
 
     protected BSScene m_physicsScene { get; private set; }
@@ -78,7 +89,8 @@ public abstract class BSLinkset
     public int LinksetID { get; private set; }
 
     // The children under the root in this linkset.
-    protected HashSet<BSPrimLinkable> m_children;
+    // protected HashSet<BSPrimLinkable> m_children;
+    protected Dictionary<BSPrimLinkable, BSLinkInfo> m_children;
 
     // We lock the diddling of linkset classes to prevent any badness.
     // This locks the modification of the instances of this class. Changes
@@ -109,7 +121,7 @@ public abstract class BSLinkset
             m_nextLinksetID = 1;
         m_physicsScene = scene;
         LinksetRoot = parent;
-        m_children = new HashSet<BSPrimLinkable>();
+        m_children = new Dictionary<BSPrimLinkable, BSLinkInfo>();
         LinksetMass = parent.RawMass;
         Rebuilding = false;
 
@@ -170,17 +182,7 @@ public abstract class BSLinkset
         bool ret = false;
         lock (m_linksetActivityLock)
         {
-            ret = m_children.Contains(child);
-            /* Safer version but the above should work
-            foreach (BSPrimLinkable bp in m_children)
-            {
-                if (child.LocalID == bp.LocalID)
-                {
-                    ret = true;
-                    break;
-                }
-            }
-             */
+            ret = m_children.ContainsKey(child);
         }
         return ret;
     }
@@ -194,7 +196,24 @@ public abstract class BSLinkset
         lock (m_linksetActivityLock)
         {
             action(LinksetRoot);
-            foreach (BSPrimLinkable po in m_children)
+            foreach (BSPrimLinkable po in m_children.Keys)
+            {
+                if (action(po))
+                    break;
+            }
+        }
+        return ret;
+    }
+
+    // Perform an action on each member of the linkset including root prim.
+    // Depends on the action on whether this should be done at taint time.
+    public delegate bool ForEachLinkInfoAction(BSLinkInfo obj);
+    public virtual bool ForEachLinkInfo(ForEachLinkInfoAction action)
+    {
+        bool ret = false;
+        lock (m_linksetActivityLock)
+        {
+            foreach (BSLinkInfo po in m_children.Values)
             {
                 if (action(po))
                     break;
@@ -364,7 +383,7 @@ public abstract class BSLinkset
         {
             lock (m_linksetActivityLock)
             {
-                foreach (BSPrimLinkable bp in m_children)
+                foreach (BSPrimLinkable bp in m_children.Keys)
                 {
                     mass += bp.RawMass;
                 }
@@ -382,7 +401,7 @@ public abstract class BSLinkset
             com = LinksetRoot.Position * LinksetRoot.RawMass;
             float totalMass = LinksetRoot.RawMass;
 
-            foreach (BSPrimLinkable bp in m_children)
+            foreach (BSPrimLinkable bp in m_children.Keys)
             {
                 com += bp.Position * bp.RawMass;
                 totalMass += bp.RawMass;
@@ -401,7 +420,7 @@ public abstract class BSLinkset
         {
             com = LinksetRoot.Position;
 
-            foreach (BSPrimLinkable bp in m_children)
+            foreach (BSPrimLinkable bp in m_children.Keys)
             {
                 com += bp.Position;
             }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
index 6359046..085d195 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
@@ -42,6 +42,7 @@ public sealed class BSLinksetCompound : BSLinkset
     public BSLinksetCompound(BSScene scene, BSPrimLinkable parent)
         : base(scene, parent)
     {
+        LinksetImpl = LinksetImplementation.Compound;
     }
 
     // ================================================================
@@ -257,7 +258,7 @@ public sealed class BSLinksetCompound : BSLinkset
     {
         if (!HasChild(child))
         {
-            m_children.Add(child);
+            m_children.Add(child, new BSLinkInfo(child));
 
             DetailLog("{0},BSLinksetCompound.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
 
@@ -353,7 +354,7 @@ public sealed class BSLinksetCompound : BSLinkset
 
             // Add the shapes of all the components of the linkset
             int memberIndex = 1;
-            ForEachMember(delegate(BSPrimLinkable cPrim)
+            ForEachMember((cPrim) =>
             {
                 if (IsRoot(cPrim))
                 {
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
index f17d698..4bac222 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
@@ -36,8 +36,78 @@ public sealed class BSLinksetConstraints : BSLinkset
 {
     // private static string LogHeader = "[BULLETSIM LINKSET CONSTRAINTS]";
 
+    public class BSLinkInfoConstraint : BSLinkInfo
+    {
+        public ConstraintType constraintType;
+        public BSConstraint constraint;
+        public OMV.Vector3 linearLimitLow;
+        public OMV.Vector3 linearLimitHigh;
+        public OMV.Vector3 angularLimitLow;
+        public OMV.Vector3 angularLimitHigh;
+        public bool useFrameOffset;
+        public bool enableTransMotor;
+        public float transMotorMaxVel;
+        public float transMotorMaxForce;
+        public float cfm;
+        public float erp;
+        public float solverIterations;
+
+        public BSLinkInfoConstraint(BSPrimLinkable pMember)
+            : base(pMember)
+        {
+            constraint = null;
+            ResetToFixedConstraint();
+        }
+
+        // Set all the parameters for this constraint to a fixed, non-movable constraint.
+        public void ResetToFixedConstraint()
+        {
+            constraintType = ConstraintType.D6_CONSTRAINT_TYPE;
+            linearLimitLow = OMV.Vector3.Zero;
+            linearLimitHigh = OMV.Vector3.Zero;
+            angularLimitLow = OMV.Vector3.Zero;
+            angularLimitHigh = OMV.Vector3.Zero;
+            useFrameOffset = BSParam.LinkConstraintUseFrameOffset;
+            enableTransMotor = BSParam.LinkConstraintEnableTransMotor;
+            transMotorMaxVel = BSParam.LinkConstraintTransMotorMaxVel;
+            transMotorMaxForce = BSParam.LinkConstraintTransMotorMaxForce;
+            cfm = BSParam.LinkConstraintCFM;
+            erp = BSParam.LinkConstraintERP;
+            solverIterations = BSParam.LinkConstraintSolverIterations;
+        }
+
+        // Given a constraint, apply the current constraint parameters to same.
+        public void SetConstraintParameters(BSConstraint constrain)
+        {
+            switch (constraintType)
+            {
+                case ConstraintType.D6_CONSTRAINT_TYPE:
+                    BSConstraint6Dof constrain6dof = constrain as BSConstraint6Dof;
+                    if (constrain6dof != null)
+                    {
+                        // zero linear and angular limits makes the objects unable to move in relation to each other
+                        constrain6dof.SetLinearLimits(linearLimitLow, linearLimitHigh);
+                        constrain6dof.SetAngularLimits(angularLimitLow, angularLimitHigh);
+
+                        // tweek the constraint to increase stability
+                        constrain6dof.UseFrameOffset(useFrameOffset);
+                        constrain6dof.TranslationalLimitMotor(enableTransMotor, transMotorMaxVel, transMotorMaxForce);
+                        constrain6dof.SetCFMAndERP(cfm, erp);
+                        if (solverIterations != 0f)
+                        {
+                            constrain6dof.SetSolverIterations(solverIterations);
+                        }
+                    }
+                    break;
+                default:
+                    break;
+            }
+        }
+    }
+
     public BSLinksetConstraints(BSScene scene, BSPrimLinkable parent) : base(scene, parent)
     {
+        LinksetImpl = LinksetImplementation.Constraint;
     }
 
     // When physical properties are changed the linkset needs to recalculate
@@ -142,7 +212,7 @@ public sealed class BSLinksetConstraints : BSLinkset
     {
         if (!HasChild(child))
         {
-            m_children.Add(child);
+            m_children.Add(child, new BSLinkInfoConstraint(child));
 
             DetailLog("{0},BSLinksetConstraints.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
 
@@ -190,73 +260,74 @@ public sealed class BSLinksetConstraints : BSLinkset
     }
 
     // Create a static constraint between the two passed objects
-    private BSConstraint BuildConstraint(BSPrimLinkable rootPrim, BSPrimLinkable childPrim)
+    private BSConstraint BuildConstraint(BSPrimLinkable rootPrim, BSLinkInfo li)
     {
+        BSLinkInfoConstraint liConstraint = li as BSLinkInfoConstraint;
+        if (liConstraint == null)
+            return null;
+
         // Zero motion for children so they don't interpolate
-        childPrim.ZeroMotion(true);
-
-        // 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},BSLinksetConstraint.BuildConstraint,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}",
-                                        rootPrim.LocalID,
-                                        rootPrim.LocalID, rootPrim.PhysBody.AddrString,
-                                        childPrim.LocalID, childPrim.PhysBody.AddrString,
-                                        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
-
-        BSConstraint6Dof constrain = new BSConstraint6Dof(
-                            m_physicsScene.World, rootPrim.PhysBody, childPrim.PhysBody, midPoint, true, true );
-                            // PhysicsScene.World, childPrim.BSBody, rootPrim.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 for 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);
-
-        DetailLog("{0},BSLinksetConstraint.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),
-                        true,
-                        true
-                        );
-        // ==================================================================================
-        */
+        li.member.ZeroMotion(true);
 
-        m_physicsScene.Constraints.AddConstraint(constrain);
+        BSConstraint constrain = null;
 
-        // 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(BSParam.LinkConstraintUseFrameOffset);
-        constrain.TranslationalLimitMotor(BSParam.LinkConstraintEnableTransMotor,
-                        BSParam.LinkConstraintTransMotorMaxVel,
-                        BSParam.LinkConstraintTransMotorMaxForce);
-        constrain.SetCFMAndERP(BSParam.LinkConstraintCFM, BSParam.LinkConstraintERP);
-        if (BSParam.LinkConstraintSolverIterations != 0f)
+        switch (liConstraint.constraintType)
         {
-            constrain.SetSolverIterations(BSParam.LinkConstraintSolverIterations);
+            case ConstraintType.D6_CONSTRAINT_TYPE:
+                // Relative position normalized to the root prim
+                // Essentually a vector pointing from center of rootPrim to center of li.member
+                OMV.Vector3 childRelativePosition = liConstraint.member.Position - rootPrim.Position;
+
+                // real world coordinate of midpoint between the two objects
+                OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2);
+
+                DetailLog("{0},BSLinksetConstraint.BuildConstraint,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}",
+                                                rootPrim.LocalID,
+                                                rootPrim.LocalID, rootPrim.PhysBody.AddrString,
+                                                liConstraint.member.LocalID, liConstraint.member.PhysBody.AddrString,
+                                                rootPrim.Position, liConstraint.member.Position, midPoint);
+
+                // create a constraint that allows no freedom of movement between the two objects
+                // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
+
+                constrain = new BSConstraint6Dof(
+                                    m_physicsScene.World, rootPrim.PhysBody, liConstraint.member.PhysBody, 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 for future programmers.
+                // ==================================================================================
+                // relative position normalized to the root prim
+                OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation);
+                OMV.Vector3 childRelativePosition = (liConstraint.member.Position - rootPrim.Position) * invThisOrientation;
+
+                // relative rotation of the child to the parent
+                OMV.Quaternion childRelativeRotation = invThisOrientation * liConstraint.member.Orientation;
+                OMV.Quaternion inverseChildRelativeRotation = OMV.Quaternion.Inverse(childRelativeRotation);
+
+                DetailLog("{0},BSLinksetConstraint.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, liConstraint.member.LocalID);
+                constrain = new BS6DofConstraint(
+                                PhysicsScene.World, rootPrim.Body, liConstraint.member.Body,
+                                OMV.Vector3.Zero,
+                                OMV.Quaternion.Inverse(rootPrim.Orientation),
+                                OMV.Vector3.Zero,
+                                OMV.Quaternion.Inverse(liConstraint.member.Orientation),
+                                true,
+                                true
+                                );
+                // ==================================================================================
+                */
+
+                break;
+            default:
+                break;
         }
+
+        liConstraint.SetConstraintParameters(constrain);
+
+        m_physicsScene.Constraints.AddConstraint(constrain);
+
         return constrain;
     }
 
@@ -317,23 +388,24 @@ public sealed class BSLinksetConstraints : BSLinkset
                 return; // Note the 'finally' clause at the botton which will get executed.
             }
 
-            foreach (BSPrimLinkable child in m_children)
+            ForEachLinkInfo((li) =>
             {
                 // A child in the linkset physically shows the mass of the whole linkset.
                 // This allows Bullet to apply enough force on the child to move the whole linkset.
                 // (Also do the mass stuff before recomputing the constraint so mass is not zero.)
-                child.UpdatePhysicalMassProperties(linksetMass, true);
+                li.member.UpdatePhysicalMassProperties(linksetMass, true);
 
                 BSConstraint constrain;
-                if (!m_physicsScene.Constraints.TryGetConstraint(LinksetRoot.PhysBody, child.PhysBody, out constrain))
+                if (!m_physicsScene.Constraints.TryGetConstraint(LinksetRoot.PhysBody, li.member.PhysBody, out constrain))
                 {
                     // If constraint doesn't exist yet, create it.
-                    constrain = BuildConstraint(LinksetRoot, child);
+                    constrain = BuildConstraint(LinksetRoot, li);
                 }
                 constrain.RecomputeConstraintVariables(linksetMass);
 
                 // PhysicsScene.PE.DumpConstraint(PhysicsScene.World, constrain.Constraint);    // DEBUG DEBUG
-            }
+                return false;   // 'false' says to keep processing other members
+            });
         }
         finally
         {
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index e92a1d2..a0b6abc 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -1541,6 +1541,50 @@ public class BSPrim : BSPhysObject
         PhysicalActors.RemoveDependencies();
     }
 
+    #region Extension
+    public override object Extension(string pFunct, params object[] pParams)
+    {
+        object ret = null;
+        switch (pFunct)
+        {
+            case BSScene.PhysFunctGetLinksetType:
+            {
+                BSPrimLinkable myHandle = this as BSPrimLinkable;
+                if (myHandle != null)
+                {
+                    ret = (object)myHandle.LinksetType;
+                }
+                m_log.DebugFormat("{0} Extension.physGetLinksetType, type={1}", LogHeader, ret);
+                break;
+            }
+            case BSScene.PhysFunctSetLinksetType:
+            {
+                if (pParams.Length > 0)
+                {
+                    BSLinkset.LinksetImplementation linksetType = (BSLinkset.LinksetImplementation)pParams[0];
+                    BSPrimLinkable myHandle = this as BSPrimLinkable;
+                    if (myHandle != null && myHandle.Linkset.IsRoot(myHandle))
+                    {
+                        PhysScene.TaintedObject("BSPrim.PhysFunctSetLinksetType", delegate()
+                        {
+                            // Cause the linkset type to change
+                            m_log.DebugFormat("{0} Extension.physSetLinksetType, oldType={1}, newType={2}",
+                                                LogHeader, myHandle.Linkset.LinksetImpl, linksetType);
+                            myHandle.ConvertLinkset(linksetType);
+                        });
+                    }
+                    ret = (object)(int)linksetType;
+                }
+                break;
+            }
+            default:
+                ret = base.Extension(pFunct, pParams);
+                break;
+        }
+        return ret;
+    }
+    #endregion  // Extension
+
     // 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.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs
index 2f392da..7179a6d 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrimLinkable.cs
@@ -233,5 +233,46 @@ public class BSPrimLinkable : BSPrimDisplaced
             base.HasSomeCollision = value;
         }
     }
+
+    // Convert the existing linkset of this prim into a new type.
+    public bool ConvertLinkset(BSLinkset.LinksetImplementation newType)
+    {
+        bool ret = false;
+        if (LinksetType != newType)
+        {
+            // Set the implementation type first so the call to BSLinkset.Factory gets the new type.
+            this.LinksetType = newType;
+
+            BSLinkset oldLinkset = this.Linkset;
+            BSLinkset newLinkset = BSLinkset.Factory(PhysScene, this);
+
+            this.Linkset = newLinkset;
+
+            // Pick up any physical dependencies this linkset might have in the physics engine.
+            oldLinkset.RemoveDependencies(this);
+
+            // Create a list of the children (mainly because can't interate through a list that's changing)
+            List<BSPrimLinkable> children = new List<BSPrimLinkable>();
+            oldLinkset.ForEachMember((child) =>
+            {
+                if (!oldLinkset.IsRoot(child))
+                    children.Add(child);
+                return false;   // 'false' says to continue to next member
+            });
+
+            // Remove the children from the old linkset and add to the new (will be a new instance from the factory)
+            foreach (BSPrimLinkable child in children)
+            {
+                oldLinkset.RemoveMeFromLinkset(child);
+                newLinkset.AddMeToLinkset(child);
+                child.Linkset = newLinkset;
+            }
+
+            // Force the shape and linkset to get reconstructed
+            newLinkset.Refresh(this);
+            this.ForceBodyShapeRebuild(true /* inTaintTime */);
+        }
+        return ret;
+    }
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index 41aca3b..79ac5a5 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -862,6 +862,23 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
 
     public override bool IsThreaded { get { return false;  } }
 
+    #region Extensions
+    // =============================================================
+    // Per scene functions. See below.
+
+    // Per avatar functions. See BSCharacter.
+
+    // Per prim functions. See BSPrim.
+    public const string PhysFunctGetLinksetType = "BulletSim.GetLinksetType";
+    public const string PhysFunctSetLinksetType = "BulletSim.SetLinksetType";
+    // =============================================================
+
+    public override object Extension(string pFunct, params object[] pParams)
+    {
+        return base.Extension(pFunct, pParams);
+    }
+    #endregion // Extensions
+
     #region Taints
     // The simulation execution order is:
     // Simulate()