BulletSim: add separate runtime and taint-time linkset children lists to keep the creation of constraints separate from runtime.

connector_plugin
Robert Adams 2012-09-27 22:00:02 -07:00
parent 74dea4cfd5
commit 6f89975526
3 changed files with 93 additions and 80 deletions

View File

@ -43,8 +43,17 @@ public class BSLinkset
static int m_nextLinksetID = 1; static int m_nextLinksetID = 1;
public int LinksetID { get; private set; } public int LinksetID { get; private set; }
// The children under the root in this linkset // 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.
private List<BSPhysObject> m_children; private List<BSPhysObject> m_children;
private List<BSPhysObject> m_taintChildren;
// We lock the diddling of linkset classes to prevent any badness. // We lock the diddling of linkset classes to prevent any badness.
// This locks the modification of the instances of this class. Changes // This locks the modification of the instances of this class. Changes
@ -82,6 +91,7 @@ public class BSLinkset
PhysicsScene = scene; PhysicsScene = scene;
LinksetRoot = parent; LinksetRoot = parent;
m_children = new List<BSPhysObject>(); m_children = new List<BSPhysObject>();
m_taintChildren = new List<BSPhysObject>();
m_mass = parent.MassRaw; m_mass = parent.MassRaw;
} }
@ -194,30 +204,40 @@ public class BSLinkset
// Routine used when rebuilding the body of the root of the linkset // Routine used when rebuilding the body of the root of the linkset
// Destroy all the constraints have have been made to root. // Destroy all the constraints have have been made to root.
// This is called when the root body is changing. // This is called when the root body is changing.
// Returns 'true' of something eas actually removed and would need restoring
// Called at taint-time!! // Called at taint-time!!
public void RemoveBodyDependencies(BSPrim child) public bool RemoveBodyDependencies(BSPrim child)
{ {
bool ret = false;
lock (m_linksetActivityLock) lock (m_linksetActivityLock)
{ {
if (IsRoot(child)) if (IsRoot(child))
{ {
// If the one with the dependency is root, must undo all children // If the one with the dependency is root, must undo all children
DetailLog("{0},BSLinkset.RemoveBodyDependencies,removeChildrenForRoot,rID={1},numChild={2}", DetailLog("{0},BSLinkset.RemoveBodyDependencies,removeChildrenForRoot,rID={1},numChild={2}",
LinksetRoot.LocalID, m_children.Count); child.LocalID, LinksetRoot.LocalID, m_taintChildren.Count);
foreach (BSPhysObject bpo in m_children) foreach (BSPhysObject bpo in m_taintChildren)
{ {
PhysicallyUnlinkAChildFromRoot(LinksetRoot, LinksetRoot.BSBody, bpo, bpo.BSBody); PhysicallyUnlinkAChildFromRoot(LinksetRoot, LinksetRoot.BSBody, bpo, bpo.BSBody);
ret = true;
} }
} }
else else
{ {
DetailLog("{0},BSLinkset.RemoveBodyDependencies,removeSingleChild,rID={1},rBody={2},cID={3},cBody={4}", DetailLog("{0},BSLinkset.RemoveBodyDependencies,removeSingleChild,rID={1},rBody={2},cID={3},cBody={4}",
child.LocalID,
LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"), LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"),
child.LocalID, child.BSBody.ptr.ToString("X")); child.LocalID, child.BSBody.ptr.ToString("X"));
// Remove the dependency on the body of this one // Remove the dependency on the body of this one
PhysicallyUnlinkAChildFromRoot(LinksetRoot, LinksetRoot.BSBody, child, child.BSBody); if (m_taintChildren.Contains(child))
{
PhysicallyUnlinkAChildFromRoot(LinksetRoot, LinksetRoot.BSBody, child, child.BSBody);
ret = true;
}
} }
} }
return ret;
} }
// Routine used when rebuilding the body of the root of the linkset // Routine used when rebuilding the body of the root of the linkset
@ -231,8 +251,8 @@ public class BSLinkset
if (IsRoot(child)) if (IsRoot(child))
{ {
DetailLog("{0},BSLinkset.RestoreBodyDependencies,restoreChildrenForRoot,rID={1},numChild={2}", DetailLog("{0},BSLinkset.RestoreBodyDependencies,restoreChildrenForRoot,rID={1},numChild={2}",
LinksetRoot.LocalID, m_children.Count); child.LocalID, LinksetRoot.LocalID, m_taintChildren.Count);
foreach (BSPhysObject bpo in m_children) foreach (BSPhysObject bpo in m_taintChildren)
{ {
PhysicallyLinkAChildToRoot(LinksetRoot, LinksetRoot.BSBody, bpo, bpo.BSBody); PhysicallyLinkAChildToRoot(LinksetRoot, LinksetRoot.BSBody, bpo, bpo.BSBody);
} }
@ -240,6 +260,7 @@ public class BSLinkset
else else
{ {
DetailLog("{0},BSLinkset.RestoreBodyDependencies,restoreSingleChild,rID={1},rBody={2},cID={3},cBody={4}", DetailLog("{0},BSLinkset.RestoreBodyDependencies,restoreSingleChild,rID={1},rBody={2},cID={3},cBody={4}",
LinksetRoot.LocalID,
LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"), LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"),
child.LocalID, child.BSBody.ptr.ToString("X")); child.LocalID, child.BSBody.ptr.ToString("X"));
PhysicallyLinkAChildToRoot(LinksetRoot, LinksetRoot.BSBody, child, child.BSBody); PhysicallyLinkAChildToRoot(LinksetRoot, LinksetRoot.BSBody, child, child.BSBody);
@ -256,7 +277,7 @@ public class BSLinkset
lock (m_linksetActivityLock) lock (m_linksetActivityLock)
{ {
mass = LinksetRoot.MassRaw; mass = LinksetRoot.MassRaw;
foreach (BSPhysObject bp in m_children) foreach (BSPhysObject bp in m_taintChildren)
{ {
mass += bp.MassRaw; mass += bp.MassRaw;
} }
@ -272,7 +293,7 @@ public class BSLinkset
com = LinksetRoot.Position * LinksetRoot.MassRaw; com = LinksetRoot.Position * LinksetRoot.MassRaw;
float totalMass = LinksetRoot.MassRaw; float totalMass = LinksetRoot.MassRaw;
foreach (BSPhysObject bp in m_children) foreach (BSPhysObject bp in m_taintChildren)
{ {
com += bp.Position * bp.MassRaw; com += bp.Position * bp.MassRaw;
totalMass += bp.MassRaw; totalMass += bp.MassRaw;
@ -291,70 +312,16 @@ public class BSLinkset
{ {
com = LinksetRoot.Position; com = LinksetRoot.Position;
foreach (BSPhysObject bp in m_children) foreach (BSPhysObject bp in m_taintChildren)
{ {
com += bp.Position * bp.MassRaw; com += bp.Position * bp.MassRaw;
} }
com /= (m_children.Count + 1); com /= (m_taintChildren.Count + 1);
} }
return com; return com;
} }
// 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;
lock (m_linksetActivityLock)
{
bool somethingMissing = false;
foreach (BSPhysObject child in m_children)
{
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.
// Caused by the fact that m_children is built at run time but building constraints
// happens at taint time.
somethingMissing = true;
break;
}
}
// If the whole linkset is not here, doesn't make sense to recompute linkset wide values
if (!somethingMissing)
{
// 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);
foreach (BSPhysObject child in m_children)
{
BulletSimAPI.SetCenterOfMassByPosRot2(child.BSBody.ptr, centerOfMass, OMV.Quaternion.Identity);
}
/*
// The root prim takes on the weight of the whole linkset
OMV.Vector3 inertia = BulletSimAPI.CalculateLocalInertia2(LinksetRoot.BSShape.Ptr, linksetMass);
BulletSimAPI.SetMassProps2(LinksetRoot.BSBody.Ptr, linksetMass, inertia);
OMV.Vector3 centerOfMass = ComputeLinksetCenterOfMass();
BulletSimAPI.SetCenterOfMassByPosRot2(LinksetRoot.BSBody.Ptr, centerOfMass, OMV.Quaternion.Identity);
BulletSimAPI.UpdateInertiaTensor2(LinksetRoot.BSBody.Ptr);
*/
}
}
return;
}
// I am the root of a linkset and a new child is being added // I am the root of a linkset and a new child is being added
// Called while LinkActivity is locked. // Called while LinkActivity is locked.
private void AddChildToLinkset(BSPhysObject child) private void AddChildToLinkset(BSPhysObject child)
@ -377,6 +344,7 @@ public class BSLinkset
{ {
DetailLog("{0},AddChildToLinkset,taint,child={1}", LinksetRoot.LocalID, child.LocalID); DetailLog("{0},AddChildToLinkset,taint,child={1}", LinksetRoot.LocalID, child.LocalID);
// build the physical binding between me and the child // build the physical binding between me and the child
m_taintChildren.Add(childx);
PhysicallyLinkAChildToRoot(rootx, rootBodyx, childx, childBodyx); PhysicallyLinkAChildToRoot(rootx, rootBodyx, childx, childBodyx);
}); });
} }
@ -405,13 +373,16 @@ public class BSLinkset
BSPhysObject childx = child; BSPhysObject childx = child;
BulletBody childBodyx = child.BSBody; BulletBody childBodyx = child.BSBody;
DetailLog("{0},RemoveChildFromLinkset,call,child={1}", DetailLog("{0},RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}",
rootx.LocalID, childx.LocalID,
rootx.LocalID, rootBodyx.ptr.ToString("X"), rootx.LocalID, rootBodyx.ptr.ToString("X"),
childx.LocalID, childBodyx.ptr.ToString("X")); childx.LocalID, childBodyx.ptr.ToString("X"));
PhysicsScene.TaintedObject("RemoveChildFromLinkset", delegate() PhysicsScene.TaintedObject("RemoveChildFromLinkset", delegate()
{ {
if (m_taintChildren.Contains(childx))
m_taintChildren.Remove(childx);
PhysicallyUnlinkAChildFromRoot(rootx, rootBodyx, childx, childBodyx); PhysicallyUnlinkAChildFromRoot(rootx, rootBodyx, childx, childBodyx);
RecomputeLinksetConstraintVariables(); RecomputeLinksetConstraintVariables();
}); });
@ -551,6 +522,46 @@ public class 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.
// 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);
foreach (BSPhysObject child in m_taintChildren)
{
BulletSimAPI.SetCenterOfMassByPosRot2(child.BSBody.ptr, centerOfMass, OMV.Quaternion.Identity);
}
}
return;
}
// Invoke the detailed logger and output something if it's enabled. // Invoke the detailed logger and output something if it's enabled.
private void DetailLog(string msg, params Object[] args) private void DetailLog(string msg, params Object[] args)
{ {

View File

@ -1120,8 +1120,7 @@ public sealed class BSPrim : BSPhysObject
{ {
// Called if the current prim body is about to be destroyed. // Called if the current prim body is about to be destroyed.
// The problem is the constraints for Linksets which need to be updated for the new body. // The problem is the constraints for Linksets which need to be updated for the new body.
Linkset.RemoveBodyDependencies(this); needToRestoreLinkset = Linkset.RemoveBodyDependencies(this);
needToRestoreLinkset = true;
}); });
if (needToRestoreLinkset) if (needToRestoreLinkset)

View File

@ -144,34 +144,37 @@ public class BSShapeCollection : IDisposable
// Release the usage of a body. // Release the usage of a body.
// Called when releasing use of a BSBody. BSShape is handled separately. // Called when releasing use of a BSBody. BSShape is handled separately.
public void DereferenceBody(BulletBody shape, bool inTaintTime, BodyDestructionCallback bodyCallback ) public void DereferenceBody(BulletBody body, bool inTaintTime, BodyDestructionCallback bodyCallback )
{ {
if (shape.ptr == IntPtr.Zero) if (body.ptr == IntPtr.Zero)
return; return;
lock (m_collectionActivityLock) lock (m_collectionActivityLock)
{ {
BodyDesc bodyDesc; BodyDesc bodyDesc;
if (Bodies.TryGetValue(shape.ID, out bodyDesc)) if (Bodies.TryGetValue(body.ID, out bodyDesc))
{ {
bodyDesc.referenceCount--; bodyDesc.referenceCount--;
bodyDesc.lastReferenced = System.DateTime.Now; bodyDesc.lastReferenced = System.DateTime.Now;
Bodies[shape.ID] = bodyDesc; Bodies[body.ID] = bodyDesc;
DetailLog("{0},BSShapeCollection.DereferenceBody,ref={1}", shape.ID, bodyDesc.referenceCount); DetailLog("{0},BSShapeCollection.DereferenceBody,ref={1}", body.ID, bodyDesc.referenceCount);
// If body is no longer being used, free it -- bodies are never shared. // If body is no longer being used, free it -- bodies are never shared.
if (bodyDesc.referenceCount == 0) if (bodyDesc.referenceCount == 0)
{ {
Bodies.Remove(shape.ID); Bodies.Remove(body.ID);
BSScene.TaintCallback removeOperation = delegate() BSScene.TaintCallback removeOperation = delegate()
{ {
DetailLog("{0},BSShapeCollection.DereferenceBody,DestroyingBody. ptr={1}", DetailLog("{0},BSShapeCollection.DereferenceBody,DestroyingBody. ptr={1}",
shape.ID, shape.ptr.ToString("X")); body.ID, body.ptr.ToString("X"));
// If the caller needs to know, pass the event up.
if (bodyCallback != null) bodyCallback(body);
// Zero any reference to the shape so it is not freed when the body is deleted. // Zero any reference to the shape so it is not freed when the body is deleted.
BulletSimAPI.SetCollisionShape2(PhysicsScene.World.ptr, shape.ptr, IntPtr.Zero); BulletSimAPI.SetCollisionShape2(PhysicsScene.World.ptr, body.ptr, IntPtr.Zero);
// It may have already been removed from the world in which case the next is a NOOP. // It may have already been removed from the world in which case the next is a NOOP.
BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, shape.ptr); BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, body.ptr);
BulletSimAPI.DestroyObject2(PhysicsScene.World.ptr, shape.ptr); BulletSimAPI.DestroyObject2(PhysicsScene.World.ptr, body.ptr);
}; };
// If already in taint-time, do the operations now. Otherwise queue for later. // If already in taint-time, do the operations now. Otherwise queue for later.
if (inTaintTime) if (inTaintTime)
@ -182,7 +185,7 @@ public class BSShapeCollection : IDisposable
} }
else else
{ {
DetailLog("{0},BSShapeCollection.DereferenceBody,DID NOT FIND BODY", shape.ID, bodyDesc.referenceCount); DetailLog("{0},BSShapeCollection.DereferenceBody,DID NOT FIND BODY", body.ID, bodyDesc.referenceCount);
} }
} }
} }