1 files changed, 265 insertions, 166 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index 3a92f93..087b9bb 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -32,78 +32,35 @@ using OMV = OpenMetaverse;
 
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
-public abstract class BSLinkset
+public class BSLinkset
 {
-    // private static string LogHeader = "[BULLETSIM LINKSET]";
+    private static string LogHeader = "[BULLETSIM LINKSET]";
 
-    public enum LinksetImplementation
-    {
-        Constraint   = 0,   // linkset tied together with constraints
-        Compound     = 1,   // linkset tied together as a compound object
-        Manual       = 2    // linkset tied together manually (code moves all the pieces)
-    }
-    // Create the correct type of linkset for this child
-    public static BSLinkset Factory(BSScene physScene, BSPhysObject parent)
-    {
-        BSLinkset ret = null;
-
-        switch ((int)physScene.Params.linksetImplementation)
-        {
-            case (int)LinksetImplementation.Compound:
-                ret = new BSLinksetCompound(physScene, parent);
-                break;
-            case (int)LinksetImplementation.Manual:
-                // ret = new BSLinksetManual(physScene, parent);
-                break;
-            default:
-                ret = new BSLinksetConstraints(physScene, parent);
-                break;
-        }
-        return ret;
-    }
-
-    public BSPhysObject LinksetRoot { get; protected set; }
+    private BSPrim m_linksetRoot;
+    public BSPrim LinksetRoot { get { return m_linksetRoot; } }
 
-    public BSScene PhysicsScene { get; private set; }
+    private BSScene m_physicsScene;
+    public BSScene PhysicsScene { get { return m_physicsScene; } }
 
-    static int m_nextLinksetID = 1;
-    public int LinksetID { get; private set; }
-
-    // The children under the root in this linkset.
-    protected HashSet<BSPhysObject> m_children;
+    // The children under the root in this linkset
+    private List<BSPrim> m_children;
 
     // 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.
-    protected object m_linksetActivityLock = new Object();
-
-    // Some linksets have a preferred physical shape.
-    // Returns SHAPE_UNKNOWN if there is no preference. Causes the correct shape to be selected.
-    public virtual ShapeData.PhysicsShapeType PreferredPhysicalShape(BSPhysObject requestor)
-    {
-        return ShapeData.PhysicsShapeType.SHAPE_UNKNOWN;
-    }
+    private object m_linksetActivityLock = new Object();
 
-    // Linksets move around the children so the linkset might need to compute the child position
-    public virtual OMV.Vector3 Position(BSPhysObject member)
-        { return member.RawPosition; }
-    public virtual OMV.Quaternion Orientation(BSPhysObject member)
-        { return member.RawOrientation; }
-    // TODO: does this need to be done for Velocity and RotationalVelocityy?
-    
     // We keep the prim's mass in the linkset structure since it could be dependent on other prims
-    protected float m_mass;
-    public float LinksetMass
-    {
-        get
+    private float m_mass;
+    public float LinksetMass 
+    { 
+        get 
         {
             m_mass = ComputeLinksetMass();
             return m_mass;
         }
     }
 
-    public virtual bool LinksetIsColliding { get { return false; } }
-
     public OMV.Vector3 CenterOfMass
     {
         get { return ComputeLinksetCenterOfMass(); }
@@ -114,30 +71,23 @@ public abstract class BSLinkset
         get { return ComputeLinksetGeometricCenter(); }
     }
 
-    protected void Initialize(BSScene scene, BSPhysObject parent)
+    public BSLinkset(BSScene scene, BSPrim parent)
     {
         // A simple linkset of one (no children)
-        LinksetID = m_nextLinksetID++;
-        // We create LOTS of linksets.
-        if (m_nextLinksetID <= 0)
-            m_nextLinksetID = 1;
-        PhysicsScene = scene;
-        LinksetRoot = parent;
-        m_children = new HashSet<BSPhysObject>();
-        m_mass = parent.RawMass;
+        m_physicsScene = scene;
+        m_linksetRoot = parent;
+        m_children = new List<BSPrim>();
+        m_mass = parent.MassRaw;
     }
 
     // Link to a linkset where the child knows the parent.
     // Parent changing should not happen so do some sanity checking.
     // We return the parent's linkset so the child can track its membership.
-    // Called at runtime.
-    public BSLinkset AddMeToLinkset(BSPhysObject child)
+    public BSLinkset AddMeToLinkset(BSPrim child)
     {
         lock (m_linksetActivityLock)
         {
-            // Don't add the root to its own linkset
-            if (!IsRoot(child))
-                AddChildToLinkset(child);
+            AddChildToLinkset(child);
         }
         return this;
     }
@@ -145,27 +95,36 @@ public abstract class BSLinkset
     // Remove a child from a linkset.
     // Returns a new linkset for the child which is a linkset of one (just the
     //    orphened child).
-    // Called at runtime.
-    public BSLinkset RemoveMeFromLinkset(BSPhysObject child)
+    public BSLinkset RemoveMeFromLinkset(BSPrim child)
     {
         lock (m_linksetActivityLock)
         {
             if (IsRoot(child))
             {
-                // Cannot remove the root from a linkset.
-                return this;
+                // if root of linkset, take the linkset apart
+                while (m_children.Count > 0)
+                {
+                    // Note that we don't do a foreach because the remove routine
+                    //    takes it out of the list.
+                    RemoveChildFromOtherLinkset(m_children[0]);
+                }
+                m_children.Clear(); // just to make sure
+            }
+            else
+            {
+                // Just removing a child from an existing linkset
+                RemoveChildFromLinkset(child);
             }
-            RemoveChildFromLinkset(child);
         }
 
         // The child is down to a linkset of just itself
-        return BSLinkset.Factory(PhysicsScene, child);
+        return new BSLinkset(PhysicsScene, child);
     }
 
     // Return 'true' if the passed object is the root object of this linkset
-    public bool IsRoot(BSPhysObject requestor)
+    public bool IsRoot(BSPrim requestor)
     {
-        return (requestor.LocalID == LinksetRoot.LocalID);
+        return (requestor.LocalID == m_linksetRoot.LocalID);
     }
 
     public int NumberOfChildren { get { return m_children.Count; } }
@@ -174,14 +133,12 @@ public abstract class BSLinkset
     public bool HasAnyChildren { get { return (m_children.Count > 0); } }
 
     // Return 'true' if this child is in this linkset
-    public bool HasChild(BSPhysObject child)
+    public bool HasChild(BSPrim child)
     {
         bool ret = false;
         lock (m_linksetActivityLock)
         {
-            ret = m_children.Contains(child);
-            /* Safer version but the above should work
-            foreach (BSPhysObject bp in m_children)
+            foreach (BSPrim bp in m_children)
             {
                 if (child.LocalID == bp.LocalID)
                 {
@@ -189,132 +146,274 @@ public abstract class BSLinkset
                     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 ForEachMemberAction(BSPhysObject obj);
-    public virtual bool ForEachMember(ForEachMemberAction action)
+    private float ComputeLinksetMass()
     {
-        bool ret = false;
+        float mass = m_linksetRoot.MassRaw;
+        foreach (BSPrim bp in m_children)
+        {
+            mass += bp.MassRaw;
+        }
+        return mass;
+    }
+
+    private OMV.Vector3 ComputeLinksetCenterOfMass()
+    {
+        OMV.Vector3 com = m_linksetRoot.Position * m_linksetRoot.MassRaw;
+        float totalMass = m_linksetRoot.MassRaw;
+
         lock (m_linksetActivityLock)
         {
-            action(LinksetRoot);
-            foreach (BSPhysObject po in m_children)
+            foreach (BSPrim bp in m_children)
             {
-                if (action(po))
-                    break;
+                com += bp.Position * bp.MassRaw;
+                totalMass += bp.MassRaw;
             }
+            if (totalMass != 0f)
+                com /= totalMass;
         }
-        return ret;
+
+        return com;
     }
 
-    // I am the root of a linkset and a new child is being added
-    // Called while LinkActivity is locked.
-    protected abstract void AddChildToLinkset(BSPhysObject child);
+    private OMV.Vector3 ComputeLinksetGeometricCenter()
+    {
+        OMV.Vector3 com = m_linksetRoot.Position;
 
-    // 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 abstract void RemoveChildFromLinkset(BSPhysObject child);
+        lock (m_linksetActivityLock)
+        {
+            foreach (BSPrim bp in m_children)
+            {
+                com += bp.Position * bp.MassRaw;
+            }
+            com /= (m_children.Count + 1);
+        }
+
+        return com;
+    }
 
     // When physical properties are changed the linkset needs to recalculate
     //   its internal properties.
-    // May be called at runtime or taint-time.
-    public abstract void Refresh(BSPhysObject requestor);
-
-    // 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 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 abstract bool MakeStatic(BSPhysObject child);
-
-    // Called when a parameter update comes from the physics engine for any object
-    //      of the linkset is received.
-    // Called at taint-time!!
-    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 was actually removed and would need restoring
-    // Called at taint-time!!
-    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 abstract void RestoreBodyDependencies(BSPrim child);
-
-    // ================================================================
-    protected virtual float ComputeLinksetMass()
+    public void Refresh(BSPrim requestor)
     {
-        float mass = LinksetRoot.RawMass;
-        if (HasAnyChildren)
+        // If there are no children, there aren't any constraints to recompute
+        if (!HasAnyChildren)
+            return;
+
+        // Only the root does the recomputation
+        if (IsRoot(requestor))
+        {
+            PhysicsScene.TaintedObject("BSLinkSet.Refresh", delegate()
+            {
+                RecomputeLinksetConstraintVariables();
+            });
+        }
+    }
+
+    // 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 bool RecomputeLinksetConstraintVariables()
+    {
+        float linksetMass = LinksetMass;
+        lock (m_linksetActivityLock)
         {
-            lock (m_linksetActivityLock)
+            foreach (BSPrim child in m_children)
             {
-                foreach (BSPhysObject bp in m_children)
+                BSConstraint constrain;
+                if (m_physicsScene.Constraints.TryGetConstraint(LinksetRoot.Body, child.Body, 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
                 {
-                    mass += bp.RawMass;
+                    // Non-fatal error that can happen when children are being added to the linkset but
+                    //    their constraints have not been created yet.
+                    // Caused by the fact that m_children is built at run time but building constraints
+                    //    happens at taint time.
+                    // m_physicsScene.Logger.ErrorFormat("[BULLETSIM LINKSET] RecomputeLinksetConstraintVariables: constraint not found for root={0}, child={1}",
+                    //                                 m_linksetRoot.Body.ID, child.Body.ID);
                 }
             }
         }
-        return mass;
+        return false;
     }
 
-    protected virtual OMV.Vector3 ComputeLinksetCenterOfMass()
+    // I am the root of a linkset and a new child is being added
+    // Called while LinkActivity is locked.
+    private void AddChildToLinkset(BSPrim child)
     {
-        OMV.Vector3 com;
-        lock (m_linksetActivityLock)
+        if (!HasChild(child))
         {
-            com = LinksetRoot.Position * LinksetRoot.RawMass;
-            float totalMass = LinksetRoot.RawMass;
+            m_children.Add(child);
 
-            foreach (BSPhysObject bp in m_children)
+            BSPrim rootx = LinksetRoot; // capture the root as of now
+            BSPrim childx = child;
+            m_physicsScene.TaintedObject("AddChildToLinkset", delegate()
             {
-                com += bp.Position * bp.RawMass;
-                totalMass += bp.RawMass;
-            }
-            if (totalMass != 0f)
-                com /= totalMass;
+                // DebugLog("{0}: AddChildToLinkset: adding child {1} to {2}", LogHeader, child.LocalID, m_linksetRoot.LocalID);
+                // DetailLog("{0},AddChildToLinkset,taint,child={1}", m_linksetRoot.LocalID, child.LocalID);
+                PhysicallyLinkAChildToRoot(rootx, childx);     // build the physical binding between me and the child
+            });
         }
+        return;
+    }
 
-        return com;
+    // 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
+    //    has to be updated also (like pointer to prim's parent).
+    private void RemoveChildFromOtherLinkset(BSPrim pchild)
+    {
+        pchild.Linkset = new BSLinkset(m_physicsScene, pchild);
+        RemoveChildFromLinkset(pchild);
     }
 
-    protected virtual OMV.Vector3 ComputeLinksetGeometricCenter()
+    // 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(BSPrim child)
     {
-        OMV.Vector3 com;
-        lock (m_linksetActivityLock)
+        if (m_children.Remove(child))
         {
-            com = LinksetRoot.Position;
-
-            foreach (BSPhysObject bp in m_children)
+            BSPrim rootx = LinksetRoot; // capture the root as of now
+            BSPrim childx = child;
+            m_physicsScene.TaintedObject("RemoveChildFromLinkset", delegate()
             {
-                com += bp.Position * bp.RawMass;
-            }
-            com /= (m_children.Count + 1);
+                // DebugLog("{0}: RemoveChildFromLinkset: Removing constraint to {1}", LogHeader, child.LocalID);
+                // DetailLog("{0},RemoveChildFromLinkset,taint,child={1}", m_linksetRoot.LocalID, child.LocalID);
+
+                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;
+    }
 
-        return com;
+    // Create a constraint between me (root of linkset) and the passed prim (the child).
+    // Called at taint time!
+    private void PhysicallyLinkAChildToRoot(BSPrim rootPrim, BSPrim 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);
+
+        // create a constraint that allows no freedom of movement between the two objects
+        // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
+        // DebugLog("{0}: CreateLinkset: Adding a constraint between root prim {1} and child prim {2}", LogHeader, LocalID, childPrim.LocalID);
+        DetailLog("{0},PhysicallyLinkAChildToRoot,taint,root={1},child={2},rLoc={3},cLoc={4},midLoc={5}", 
+            rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID, rootPrim.Position, childPrim.Position, midPoint);
+        BS6DofConstraint constrain = new BS6DofConstraint(
+                        m_physicsScene.World, rootPrim.Body, childPrim.Body,
+                        midPoint,
+                        true,
+                        true
+                        );
+        /* NOTE: attempt to build constraint with full frame computation, etc.
+         *     Using the midpoint is easier since it lets the Bullet code use the transforms
+         *     of the objects.
+         * Code left here as an example.
+        // ==================================================================================
+        // 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
+        // DebugLog("{0}: CreateLinkset: Adding a constraint between root prim {1} and child prim {2}", LogHeader, LocalID, childPrim.LocalID);
+        DetailLog("{0},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
+                        );
+        // ==================================================================================
+        */
+
+        m_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);
+
+        RecomputeLinksetConstraintVariables();
+    }
+
+    // Remove linkage between myself and a particular child
+    // Called at taint time!
+    private void PhysicallyUnlinkAChildFromRoot(BSPrim rootPrim, BSPrim childPrim)
+    {
+        // DebugLog("{0}: PhysicallyUnlinkAChildFromRoot: RemoveConstraint between root prim {1} and child prim {2}", 
+        //             LogHeader, rootPrim.LocalID, childPrim.LocalID);
+        DetailLog("{0},PhysicallyUnlinkAChildFromRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
+
+        // Find the constraint for this link and get rid of it from the overall collection and from my list
+        m_physicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.Body, childPrim.Body);
+
+        // Make the child refresh its location
+        BulletSimAPI.PushUpdate2(childPrim.Body.Ptr);
+    }
+
+    // Remove linkage between myself and any possible children I might have
+    // Called at taint time!
+    private void PhysicallyUnlinkAllChildrenFromRoot(BSPrim rootPrim)
+    {
+        // DebugLog("{0}: PhysicallyUnlinkAllChildren:", LogHeader);
+        DetailLog("{0},PhysicallyUnlinkAllChildren,taint", rootPrim.LocalID);
+
+        m_physicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.Body);
+    }
+
+    // Invoke the detailed logger and output something if it's enabled.
+    private void DebugLog(string msg, params Object[] args)
+    {
+        if (m_physicsScene.ShouldDebugLog)
+            m_physicsScene.Logger.DebugFormat(msg, args);
     }
 
     // Invoke the detailed logger and output something if it's enabled.
-    protected void DetailLog(string msg, params Object[] args)
+    private void DetailLog(string msg, params Object[] args)
     {
-        if (PhysicsScene.PhysicsLogging.Enabled)
-            PhysicsScene.DetailLog(msg, args);
+        m_physicsScene.PhysicsLogging.Write(msg, args);
     }
 
 }