diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs index 862b744598..22ea3671fc 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs @@ -56,21 +56,25 @@ public class BSConstraintCollection : IDisposable public void Clear() { - foreach (BSConstraint cons in m_constraints) + lock (m_constraints) { - cons.Dispose(); + foreach (BSConstraint cons in m_constraints) + { + cons.Dispose(); + } + m_constraints.Clear(); } - m_constraints.Clear(); } public bool AddConstraint(BSConstraint cons) { - // There is only one constraint between any bodies. Remove any old just to make sure. - RemoveAndDestroyConstraint(cons.Body1, cons.Body2); + lock (m_constraints) + { + // There is only one constraint between any bodies. Remove any old just to make sure. + RemoveAndDestroyConstraint(cons.Body1, cons.Body2); - m_world.scene.DetailLog("{0},BSConstraintCollection.AddConstraint,call,body1={1},body2={2}", BSScene.DetailLogZero, cons.Body1.ID, cons.Body2.ID); - - m_constraints.Add(cons); + m_constraints.Add(cons); + } return true; } @@ -84,16 +88,19 @@ public class BSConstraintCollection : IDisposable uint lookingID1 = body1.ID; uint lookingID2 = body2.ID; - ForEachConstraint(delegate(BSConstraint constrain) + lock (m_constraints) { - if ((constrain.Body1.ID == lookingID1 && constrain.Body2.ID == lookingID2) - || (constrain.Body1.ID == lookingID2 && constrain.Body2.ID == lookingID1)) + foreach (BSConstraint constrain in m_constraints) { - foundConstraint = constrain; - found = true; + if ((constrain.Body1.ID == lookingID1 && constrain.Body2.ID == lookingID2) + || (constrain.Body1.ID == lookingID2 && constrain.Body2.ID == lookingID1)) + { + foundConstraint = constrain; + found = true; + break; + } } - return found; - }); + } returnConstraint = foundConstraint; return found; } @@ -103,17 +110,16 @@ public class BSConstraintCollection : IDisposable // Return 'true' if a constraint was found and destroyed. public bool RemoveAndDestroyConstraint(BulletBody body1, BulletBody body2) { - // return BulletSimAPI.RemoveConstraint(m_world.ID, obj1.ID, obj2.ID); - bool ret = false; - BSConstraint constrain; - - if (this.TryGetConstraint(body1, body2, out constrain)) + lock (m_constraints) { - m_world.scene.DetailLog("{0},BSConstraintCollection.RemoveAndDestroyConstraint,taint,body1={1},body2={2}", BSScene.DetailLogZero, body1.ID, body2.ID); - // remove the constraint from our collection - RemoveAndDestroyConstraint(constrain); - ret = true; + BSConstraint constrain; + if (this.TryGetConstraint(body1, body2, out constrain)) + { + // remove the constraint from our collection + RemoveAndDestroyConstraint(constrain); + ret = true; + } } return ret; @@ -122,8 +128,11 @@ public class BSConstraintCollection : IDisposable // The constraint MUST exist in the collection public bool RemoveAndDestroyConstraint(BSConstraint constrain) { - // remove the constraint from our collection - m_constraints.Remove(constrain); + lock (m_constraints) + { + // remove the constraint from our collection + m_constraints.Remove(constrain); + } // tell the engine that all its structures need to be freed constrain.Dispose(); // we destroyed something @@ -138,16 +147,15 @@ public class BSConstraintCollection : IDisposable List toRemove = new List(); uint lookingID = body1.ID; - ForEachConstraint(delegate(BSConstraint constrain) - { - if (constrain.Body1.ID == lookingID || constrain.Body2.ID == lookingID) - { - toRemove.Add(constrain); - } - return false; - }); lock (m_constraints) { + foreach (BSConstraint constrain in m_constraints) + { + if (constrain.Body1.ID == lookingID || constrain.Body2.ID == lookingID) + { + toRemove.Add(constrain); + } + } foreach (BSConstraint constrain in toRemove) { m_constraints.Remove(constrain); @@ -159,28 +167,16 @@ public class BSConstraintCollection : IDisposable public bool RecalculateAllConstraints() { - ForEachConstraint(delegate(BSConstraint constrain) - { - constrain.CalculateTransforms(); - return false; - }); - return true; - } - - // Lock the constraint list and loop through it. - // The constraint action returns 'true' if it wants the loop aborted. - private void ForEachConstraint(ConstraintAction action) - { + bool ret = false; lock (m_constraints) { foreach (BSConstraint constrain in m_constraints) { - if (action(constrain)) - break; + constrain.CalculateTransforms(); + ret = true; } } + return ret; } - - } } diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs index 4a71612f1a..087b9bbce2 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs @@ -37,11 +37,12 @@ public class BSLinkset private static string LogHeader = "[BULLETSIM LINKSET]"; private BSPrim m_linksetRoot; - public BSPrim Root { get { return m_linksetRoot; } } + public BSPrim LinksetRoot { get { return m_linksetRoot; } } - private BSScene m_scene; - public BSScene Scene { get { return m_scene; } } + private BSScene m_physicsScene; + public BSScene PhysicsScene { get { return m_physicsScene; } } + // The children under the root in this linkset private List m_children; // We lock the diddling of linkset classes to prevent any badness. @@ -73,7 +74,7 @@ public class BSLinkset public BSLinkset(BSScene scene, BSPrim parent) { // A simple linkset of one (no children) - m_scene = scene; + m_physicsScene = scene; m_linksetRoot = parent; m_children = new List(); m_mass = parent.MassRaw; @@ -91,6 +92,9 @@ public class BSLinkset return this; } + // Remove a child from a linkset. + // Returns a new linkset for the child which is a linkset of one (just the + // orphened child). public BSLinkset RemoveMeFromLinkset(BSPrim child) { lock (m_linksetActivityLock) @@ -114,60 +118,9 @@ public class BSLinkset } // The child is down to a linkset of just itself - return new BSLinkset(Scene, child); + return new BSLinkset(PhysicsScene, child); } - /* DEPRECATED: this is really bad in that it trys to unlink other prims. - // An existing linkset had one of its members rebuilt or something. - // Go through the linkset and rebuild the pointers to the bodies of the linkset members. - public BSLinkset RefreshLinkset(BSPrim requestor) - { - BSLinkset ret = requestor.Linkset; - - lock (m_linksetActivityLock) - { - // The body pointer is refetched in case anything has moved. - System.IntPtr aPtr = BulletSimAPI.GetBodyHandle2(m_scene.World.Ptr, m_linksetRoot.LocalID); - if (aPtr == System.IntPtr.Zero) - { - // That's odd. We can't find the root of the linkset. - // The linkset is somehow dead. The requestor is now a member of a linkset of one. - DetailLog("{0},RefreshLinkset.RemoveRoot,child={1}", m_linksetRoot.LocalID, m_linksetRoot.LocalID); - ret = RemoveMeFromLinkset(m_linksetRoot); - } - else - { - // Reconstruct the pointer to the body of the linkset root. - DetailLog("{0},RefreshLinkset.RebuildRoot,rootID={1},ptr={2}", m_linksetRoot.LocalID, m_linksetRoot.LocalID, aPtr); - m_linksetRoot.Body = new BulletBody(m_linksetRoot.LocalID, aPtr); - - List toRemove = new List(); - foreach (BSPrim bsp in m_children) - { - aPtr = BulletSimAPI.GetBodyHandle2(m_scene.World.Ptr, bsp.LocalID); - if (aPtr == System.IntPtr.Zero) - { - toRemove.Add(bsp); - } - else - { - // Reconstruct the pointer to the body of the linkset root. - DetailLog("{0},RefreshLinkset.RebuildChild,rootID={1},ptr={2}", bsp.LocalID, m_linksetRoot.LocalID, aPtr); - bsp.Body = new BulletBody(bsp.LocalID, aPtr); - } - } - foreach (BSPrim bsp in toRemove) - { - RemoveChildFromOtherLinkset(bsp); - } - } - } - - return ret; - } - */ - - // Return 'true' if the passed object is the root object of this linkset public bool IsRoot(BSPrim requestor) { @@ -183,12 +136,15 @@ public class BSLinkset public bool HasChild(BSPrim child) { bool ret = false; - foreach (BSPrim bp in m_children) + lock (m_linksetActivityLock) { - if (child.LocalID == bp.LocalID) + foreach (BSPrim bp in m_children) { - ret = true; - break; + if (child.LocalID == bp.LocalID) + { + ret = true; + break; + } } } return ret; @@ -209,13 +165,16 @@ public class BSLinkset OMV.Vector3 com = m_linksetRoot.Position * m_linksetRoot.MassRaw; float totalMass = m_linksetRoot.MassRaw; - foreach (BSPrim bp in m_children) + lock (m_linksetActivityLock) { - com += bp.Position * bp.MassRaw; - totalMass += bp.MassRaw; + foreach (BSPrim bp in m_children) + { + com += bp.Position * bp.MassRaw; + totalMass += bp.MassRaw; + } + if (totalMass != 0f) + com /= totalMass; } - if (totalMass != 0f) - com /= totalMass; return com; } @@ -224,29 +183,84 @@ public class BSLinkset { OMV.Vector3 com = m_linksetRoot.Position; - foreach (BSPrim bp in m_children) + lock (m_linksetActivityLock) { - com += bp.Position * bp.MassRaw; + foreach (BSPrim bp in m_children) + { + com += bp.Position * bp.MassRaw; + } + com /= (m_children.Count + 1); } - com /= (m_children.Count + 1); return com; } + // When physical properties are changed the linkset needs to recalculate + // its internal properties. + public void Refresh(BSPrim requestor) + { + // 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) + { + foreach (BSPrim child 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 + { + // 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 false; + } + // I am the root of a linkset and a new child is being added // Called while LinkActivity is locked. - public void AddChildToLinkset(BSPrim child) + private void AddChildToLinkset(BSPrim child) { if (!HasChild(child)) { m_children.Add(child); - BSPrim root = Root; // capture the root as of now - m_scene.TaintedObject("AddChildToLinkset", delegate() + BSPrim rootx = LinksetRoot; // capture the root as of now + BSPrim childx = child; + m_physicsScene.TaintedObject("AddChildToLinkset", delegate() { - 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(root, child); // build the physical binding between me and the child + // 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; @@ -257,31 +271,34 @@ public class BSLinkset // 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). - public void RemoveChildFromOtherLinkset(BSPrim pchild) + private void RemoveChildFromOtherLinkset(BSPrim pchild) { - pchild.Linkset = new BSLinkset(m_scene, pchild); + pchild.Linkset = new BSLinkset(m_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. - public void RemoveChildFromLinkset(BSPrim child) + private void RemoveChildFromLinkset(BSPrim child) { if (m_children.Remove(child)) { - BSPrim root = Root; // capture the root as of now - m_scene.TaintedObject("RemoveChildFromLinkset", delegate() + BSPrim rootx = LinksetRoot; // capture the root as of now + BSPrim childx = child; + m_physicsScene.TaintedObject("RemoveChildFromLinkset", delegate() { - DebugLog("{0}: RemoveChildFromLinkset: Removing constraint to {1}", LogHeader, child.LocalID); - DetailLog("{0},RemoveChildFromLinkset,taint,child={1}", m_linksetRoot.LocalID, child.LocalID); + // DebugLog("{0}: RemoveChildFromLinkset: Removing constraint to {1}", LogHeader, child.LocalID); + // DetailLog("{0},RemoveChildFromLinkset,taint,child={1}", m_linksetRoot.LocalID, child.LocalID); - PhysicallyUnlinkAChildFromRoot(root, child); + PhysicallyUnlinkAChildFromRoot(rootx, childx); }); + + RecomputeLinksetConstraintVariables(); } else { // This will happen if we remove the root of the linkset first. Non-fatal occurance. - // m_scene.Logger.ErrorFormat("{0}: Asked to remove child from linkset that was not in linkset", LogHeader); + // PhysicsScene.Logger.ErrorFormat("{0}: Asked to remove child from linkset that was not in linkset", LogHeader); } return; } @@ -293,37 +310,72 @@ public class BSLinkset // 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( - m_scene.World, rootPrim.Body, childPrim.Body, - childRelativePosition, - childRelativeRotation, + PhysicsScene.World, rootPrim.Body, childPrim.Body, OMV.Vector3.Zero, - -childRelativeRotation + 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_scene.Constraints.AddConstraint(constrain); + // ================================================================================== + */ + + 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(m_scene.BoolNumeric(m_scene.Params.linkConstraintUseFrameOffset)); - constrain.TranslationalLimitMotor(m_scene.BoolNumeric(m_scene.Params.linkConstraintEnableTransMotor), - m_scene.Params.linkConstraintTransMotorMaxVel, - m_scene.Params.linkConstraintTransMotorMaxForce); - constrain.SetCFMAndERP(m_scene.Params.linkConstraintCFM, m_scene.Params.linkConstraintERP); + 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 @@ -334,7 +386,9 @@ public class BSLinkset // LogHeader, rootPrim.LocalID, childPrim.LocalID); DetailLog("{0},PhysicallyUnlinkAChildFromRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID); - m_scene.Constraints.RemoveAndDestroyConstraint(rootPrim.Body, childPrim.Body); + // 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); } @@ -346,20 +400,20 @@ public class BSLinkset // DebugLog("{0}: PhysicallyUnlinkAllChildren:", LogHeader); DetailLog("{0},PhysicallyUnlinkAllChildren,taint", rootPrim.LocalID); - m_scene.Constraints.RemoveAndDestroyConstraint(rootPrim.Body); + 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_scene.ShouldDebugLog) - m_scene.Logger.DebugFormat(msg, args); + if (m_physicsScene.ShouldDebugLog) + m_physicsScene.Logger.DebugFormat(msg, args); } // Invoke the detailed logger and output something if it's enabled. private void DetailLog(string msg, params Object[] args) { - m_scene.PhysicsLogging.Write(msg, args); + m_physicsScene.PhysicsLogging.Write(msg, args); } }