From a61f20ac74836049cbd24397670c2dcd75fb22da Mon Sep 17 00:00:00 2001 From: Robert Adams Date: Sun, 21 Oct 2012 16:12:06 -0700 Subject: BulletSim: Create LinkSet abstract class and sparate constraint based linksets into own subclass. Will eventually add manual movement linkset subclass. --- OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs | 355 ++---------------- .../Physics/BulletSPlugin/BSLinksetConstraints.cs | 396 +++++++++++++++++++++ .../Region/Physics/BulletSPlugin/BSPhysObject.cs | 2 +- 3 files changed, 434 insertions(+), 319 deletions(-) create mode 100755 OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs (limited to 'OpenSim/Region/Physics/BulletSPlugin') 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 m_children; - private HashSet m_taintChildren; + protected HashSet m_children; + protected HashSet 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(); -- cgit v1.1