From 93fe384cce42e91337f446fd658ef29ca3d9f733 Mon Sep 17 00:00:00 2001 From: Robert Adams Date: Mon, 29 Oct 2012 14:33:31 -0700 Subject: BulletSim: Use the PostTaints operation to build the linkset once before the next simulation step. This eliminates the management of children vs taintChildren and simplifies the constratin creation code. --- OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs | 52 +++----- .../Physics/BulletSPlugin/BSLinksetConstraints.cs | 140 +++++++-------------- 2 files changed, 68 insertions(+), 124 deletions(-) diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs index 569d2e7..187951e 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs @@ -61,16 +61,7 @@ public abstract class BSLinkset public int LinksetID { get; private set; } // The children under the root in this linkset. - // There are two lists of children: the current children at runtime - // and the children at taint-time. For instance, if you delink a - // child from the linkset, the child is removed from m_children - // but the constraint won't be removed until taint time. - // Two lists lets this track the 'current' children and - // the physical 'taint' children separately. - // After taint processing and before the simulation step, these - // two lists must be the same. 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 @@ -110,7 +101,6 @@ public abstract class BSLinkset PhysicsScene = scene; LinksetRoot = parent; m_children = new HashSet(); - m_taintChildren = new HashSet(); m_mass = parent.MassRaw; } @@ -192,7 +182,7 @@ public abstract class BSLinkset lock (m_linksetActivityLock) { action(LinksetRoot); - foreach (BSPhysObject po in m_taintChildren) + foreach (BSPhysObject po in m_children) { if (action(po)) break; @@ -201,9 +191,24 @@ public abstract class BSLinkset return ret; } + // 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); + + // 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 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. + protected abstract void RemoveChildFromLinkset(BSPhysObject child); + // 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). + // May be called at runtime or taint-time. public abstract void Refresh(BSPhysObject requestor); // The object is going dynamic (physical). Do any setup necessary @@ -238,8 +243,6 @@ public abstract class BSLinkset public abstract void RestoreBodyDependencies(BSPrim child); // ================================================================ - // Below this point is internal magic - protected virtual float ComputeLinksetMass() { float mass = LinksetRoot.MassRaw; @@ -264,7 +267,7 @@ public abstract class BSLinkset com = LinksetRoot.Position * LinksetRoot.MassRaw; float totalMass = LinksetRoot.MassRaw; - foreach (BSPhysObject bp in m_taintChildren) + foreach (BSPhysObject bp in m_children) { com += bp.Position * bp.MassRaw; totalMass += bp.MassRaw; @@ -283,31 +286,16 @@ public abstract class BSLinkset { com = LinksetRoot.Position; - foreach (BSPhysObject bp in m_taintChildren) + foreach (BSPhysObject bp in m_children) { com += bp.Position * bp.MassRaw; } - com /= (m_taintChildren.Count + 1); + com /= (m_children.Count + 1); } 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); - - // 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 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. - protected abstract void RemoveChildFromLinkset(BSPhysObject child); - // Invoke the detailed logger and output something if it's enabled. protected void DetailLog(string msg, params Object[] args) { diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs index 086aa12..6c1fa2a 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs @@ -54,7 +54,7 @@ public sealed class BSLinksetConstraints : BSLinkset // Queue to happen after all the other taint processing PhysicsScene.PostTaintObject("BSLinksetContraints.Refresh", requestor.LocalID, delegate() { - RecomputeLinksetConstraintVariables(); + RecomputeLinksetConstraints(); }); } @@ -98,24 +98,13 @@ public sealed class BSLinksetConstraints : BSLinkset lock (m_linksetActivityLock) { - if (IsRoot(child)) - { - // If the one with the dependency is root, must undo all children - DetailLog("{0},BSLinksetConstraint.RemoveBodyDependencies,removeChildrenForRoot,rID={1},rBody={2}", - child.LocalID, LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X")); + // Just undo all the constraints for this linkset. Rebuild at the end of the step. + DetailLog("{0},BSLinksetConstraint.RemoveBodyDependencies,removeChildrenForRoot,rID={1},rBody={2}", + child.LocalID, LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X")); - ret = PhysicallyUnlinkAllChildrenFromRoot(LinksetRoot); - } - else - { - DetailLog("{0},BSLinksetConstraint.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); - } + ret = PhysicallyUnlinkAllChildrenFromRoot(LinksetRoot); + // Cause the constraints, et al to be rebuilt before the next simulation step. + Refresh(LinksetRoot); } return ret; } @@ -125,26 +114,7 @@ public sealed class BSLinksetConstraints : BSLinkset // Called at taint-time!! public override void RestoreBodyDependencies(BSPrim child) { - lock (m_linksetActivityLock) - { - if (IsRoot(child)) - { - DetailLog("{0},BSLinksetConstraint.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},BSLinksetConstraint.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); - } - } + // The Refresh operation will build any missing constraints. } // ================================================================ @@ -163,18 +133,7 @@ public sealed class BSLinksetConstraints : BSLinkset 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); - }); + // Cause constraints and assorted properties to be recomputed before the next simulation step. Refresh(LinksetRoot); } return; @@ -207,7 +166,6 @@ public sealed class BSLinksetConstraints : BSLinkset PhysicsScene.TaintedObject("RemoveChildFromLinkset", delegate() { - m_taintChildren.Remove(child); PhysicallyUnlinkAChildFromRoot(rootx, childx); }); // See that the linkset parameters are recomputed at the end of the taint time. @@ -225,6 +183,12 @@ public sealed class BSLinksetConstraints : BSLinkset // Called at taint time! private void PhysicallyLinkAChildToRoot(BSPhysObject rootPrim, BSPhysObject childPrim) { + // Don't build the constraint when asked. Put it off until just before the simulation step. + Refresh(rootPrim); + } + + private BSConstraint BuildConstraint(BSPhysObject rootPrim, BSPhysObject childPrim) + { // Zero motion for children so they don't interpolate childPrim.ZeroMotion(); @@ -235,7 +199,7 @@ public sealed class BSLinksetConstraints : BSLinkset // real world coordinate of midpoint between the two objects OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2); - DetailLog("{0},BSLinksetConstraint.PhysicallyLinkAChildToRoot,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}", + DetailLog("{0},BSLinksetConstraint.BuildConstraint,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"), @@ -297,6 +261,7 @@ public sealed class BSLinksetConstraints : BSLinkset { constrain.SetSolverIterations(PhysicsScene.Params.linkConstraintSolverIterations); } + return constrain; } // Remove linkage between myself and a particular child @@ -337,56 +302,47 @@ public sealed class BSLinksetConstraints : BSLinkset } // 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. + // various transforms and variables. Create constraints of not created yet. + // Called before the simulation step to make sure the constraint based linkset + // is all initialized. // Must only be called at taint time!! - private void RecomputeLinksetConstraintVariables() + private void RecomputeLinksetConstraints() { float linksetMass = LinksetMass; - foreach (BSPhysObject child in m_taintChildren) + LinksetRoot.UpdatePhysicalMassProperties(linksetMass); + + // For 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); + + // BulletSimAPI.SetCollisionFilterMask2(LinksetRoot.BSBody.ptr, + // (uint)CollisionFilterGroups.LinksetFilter, (uint)CollisionFilterGroups.LinksetMask); + DetailLog("{0},BSLinksetConstraint.RecomputeLinksetConstraints,setCenterOfMass,COM={1},rBody={2},linksetMass={3}", + LinksetRoot.LocalID, centerOfMass, LinksetRoot.BSBody.ptr.ToString("X"), linksetMass); + + foreach (BSPhysObject child in m_children) { + // 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.) + BulletSimAPI.SetCenterOfMassByPosRot2(child.BSBody.ptr, centerOfMass, OMV.Quaternion.Identity); + child.UpdatePhysicalMassProperties(linksetMass); + BSConstraint constrain; - if (PhysicsScene.Constraints.TryGetConstraint(LinksetRoot.BSBody, child.BSBody, out constrain)) - { - // DetailLog("{0},BSLinksetConstraint.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 + if (!PhysicsScene.Constraints.TryGetConstraint(LinksetRoot.BSBody, child.BSBody, out constrain)) { - // Non-fatal error that happens when children are being added to the linkset but - // their constraints have not been created yet. - break; + // If constraint doesn't exist yet, create it. + constrain = BuildConstraint(LinksetRoot, child); } - } + constrain.RecomputeConstraintVariables(linksetMass); - // 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); - // BulletSimAPI.SetCollisionFilterMask2(LinksetRoot.BSBody.ptr, - // (uint)CollisionFilterGroups.LinksetFilter, (uint)CollisionFilterGroups.LinksetMask); - DetailLog("{0},BSLinksetConstraint.RecomputeLinksetConstraintVariables,setCenterOfMass,COM={1},rBody={2},linksetMass={3}", - LinksetRoot.LocalID, centerOfMass, LinksetRoot.BSBody.ptr.ToString("X"), linksetMass); - foreach (BSPhysObject child in m_taintChildren) - { - BulletSimAPI.SetCenterOfMassByPosRot2(child.BSBody.ptr, centerOfMass, OMV.Quaternion.Identity); - // 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. - child.UpdatePhysicalMassProperties(linksetMass); - // DEBUG: see of inter-linkset collisions are causing problems - // BulletSimAPI.SetCollisionFilterMask2(child.BSBody.ptr, - // (uint)CollisionFilterGroups.LinksetFilter, (uint)CollisionFilterGroups.LinksetMask); - } - // Also update the root's physical mass to the whole linkset - LinksetRoot.UpdatePhysicalMassProperties(linksetMass); + // DEBUG: see of inter-linkset collisions are causing problems + // BulletSimAPI.SetCollisionFilterMask2(child.BSBody.ptr, + // (uint)CollisionFilterGroups.LinksetFilter, (uint)CollisionFilterGroups.LinksetMask); - // BulletSimAPI.DumpAllInfo2(PhysicsScene.World.ptr); // DEBUG DEBUG DEBUG + // BulletSimAPI.DumpConstraint2(PhysicsScene.World.ptr, constrain.Constraint.ptr); // DEBUG DEBUG } - return; + } } } -- cgit v1.1