BulletSim: add separate runtime and taint-time linkset children lists to keep the creation of constraints separate from runtime.
parent
74dea4cfd5
commit
6f89975526
|
@ -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,31 +204,41 @@ 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
|
||||||
|
if (m_taintChildren.Contains(child))
|
||||||
|
{
|
||||||
PhysicallyUnlinkAChildFromRoot(LinksetRoot, LinksetRoot.BSBody, child, child.BSBody);
|
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
|
||||||
// This is called after RemoveAllLinksToRoot() to restore all the constraints.
|
// This is called after RemoveAllLinksToRoot() to restore all the constraints.
|
||||||
|
@ -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)
|
||||||
{
|
{
|
||||||
|
|
|
@ -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)
|
||||||
|
|
|
@ -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);
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
}
|
}
|
||||||
|
|
Loading…
Reference in New Issue