OpenSim
/
OpenSimMirror
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

BulletSim: add locking to constraintCollection and rename some of the public method variables to reduce confusion between a physics scene and the real scene.

integration
Robert Adams 2012-08-15 11:36:21 -07:00
parent 68f112888b
commit 9efe7bf7ba
2 changed files with 198 additions and 148 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

96
OpenSim/Region/Physics/BulletSPlugin/BSConstraintCollection.cs
View File

@ -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<BSConstraint> toRemove = new List<BSConstraint>();
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;
}
}
}

250
OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
View File

@ -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<BSPrim> 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<BSPrim>();
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<BSPrim> toRemove = new List<BSPrim>();
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);
}
}