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;
|
||||
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_taintChildren;
|
||||
|
||||
// We lock the diddling of linkset classes to prevent any badness.
|
||||
// This locks the modification of the instances of this class. Changes
|
||||
|
@ -82,6 +91,7 @@ public class BSLinkset
|
|||
PhysicsScene = scene;
|
||||
LinksetRoot = parent;
|
||||
m_children = new List<BSPhysObject>();
|
||||
m_taintChildren = new List<BSPhysObject>();
|
||||
m_mass = parent.MassRaw;
|
||||
}
|
||||
|
||||
|
@ -194,30 +204,40 @@ public class BSLinkset
|
|||
// 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 void RemoveBodyDependencies(BSPrim child)
|
||||
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},numChild={2}",
|
||||
LinksetRoot.LocalID, m_children.Count);
|
||||
foreach (BSPhysObject bpo in m_children)
|
||||
child.LocalID, LinksetRoot.LocalID, m_taintChildren.Count);
|
||||
foreach (BSPhysObject bpo in m_taintChildren)
|
||||
{
|
||||
PhysicallyUnlinkAChildFromRoot(LinksetRoot, LinksetRoot.BSBody, bpo, bpo.BSBody);
|
||||
ret = true;
|
||||
}
|
||||
}
|
||||
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"));
|
||||
// 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
|
||||
|
@ -231,8 +251,8 @@ public class BSLinkset
|
|||
if (IsRoot(child))
|
||||
{
|
||||
DetailLog("{0},BSLinkset.RestoreBodyDependencies,restoreChildrenForRoot,rID={1},numChild={2}",
|
||||
LinksetRoot.LocalID, m_children.Count);
|
||||
foreach (BSPhysObject bpo in m_children)
|
||||
child.LocalID, LinksetRoot.LocalID, m_taintChildren.Count);
|
||||
foreach (BSPhysObject bpo in m_taintChildren)
|
||||
{
|
||||
PhysicallyLinkAChildToRoot(LinksetRoot, LinksetRoot.BSBody, bpo, bpo.BSBody);
|
||||
}
|
||||
|
@ -240,6 +260,7 @@ public class BSLinkset
|
|||
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, LinksetRoot.BSBody, child, child.BSBody);
|
||||
|
@ -256,7 +277,7 @@ public class BSLinkset
|
|||
lock (m_linksetActivityLock)
|
||||
{
|
||||
mass = LinksetRoot.MassRaw;
|
||||
foreach (BSPhysObject bp in m_children)
|
||||
foreach (BSPhysObject bp in m_taintChildren)
|
||||
{
|
||||
mass += bp.MassRaw;
|
||||
}
|
||||
|
@ -272,7 +293,7 @@ public class BSLinkset
|
|||
com = LinksetRoot.Position * LinksetRoot.MassRaw;
|
||||
float totalMass = LinksetRoot.MassRaw;
|
||||
|
||||
foreach (BSPhysObject bp in m_children)
|
||||
foreach (BSPhysObject bp in m_taintChildren)
|
||||
{
|
||||
com += bp.Position * bp.MassRaw;
|
||||
totalMass += bp.MassRaw;
|
||||
|
@ -291,70 +312,16 @@ public class BSLinkset
|
|||
{
|
||||
com = LinksetRoot.Position;
|
||||
|
||||
foreach (BSPhysObject bp in m_children)
|
||||
foreach (BSPhysObject bp in m_taintChildren)
|
||||
{
|
||||
com += bp.Position * bp.MassRaw;
|
||||
}
|
||||
com /= (m_children.Count + 1);
|
||||
com /= (m_taintChildren.Count + 1);
|
||||
}
|
||||
|
||||
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
|
||||
// Called while LinkActivity is locked.
|
||||
private void AddChildToLinkset(BSPhysObject child)
|
||||
|
@ -377,6 +344,7 @@ public class BSLinkset
|
|||
{
|
||||
DetailLog("{0},AddChildToLinkset,taint,child={1}", LinksetRoot.LocalID, child.LocalID);
|
||||
// build the physical binding between me and the child
|
||||
m_taintChildren.Add(childx);
|
||||
PhysicallyLinkAChildToRoot(rootx, rootBodyx, childx, childBodyx);
|
||||
});
|
||||
}
|
||||
|
@ -405,13 +373,16 @@ public class BSLinkset
|
|||
BSPhysObject childx = child;
|
||||
BulletBody childBodyx = child.BSBody;
|
||||
|
||||
DetailLog("{0},RemoveChildFromLinkset,call,child={1}",
|
||||
rootx.LocalID,
|
||||
DetailLog("{0},RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}",
|
||||
childx.LocalID,
|
||||
rootx.LocalID, rootBodyx.ptr.ToString("X"),
|
||||
childx.LocalID, childBodyx.ptr.ToString("X"));
|
||||
|
||||
PhysicsScene.TaintedObject("RemoveChildFromLinkset", delegate()
|
||||
{
|
||||
if (m_taintChildren.Contains(childx))
|
||||
m_taintChildren.Remove(childx);
|
||||
|
||||
PhysicallyUnlinkAChildFromRoot(rootx, rootBodyx, childx, childBodyx);
|
||||
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.
|
||||
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.
|
||||
// The problem is the constraints for Linksets which need to be updated for the new body.
|
||||
Linkset.RemoveBodyDependencies(this);
|
||||
needToRestoreLinkset = true;
|
||||
needToRestoreLinkset = Linkset.RemoveBodyDependencies(this);
|
||||
});
|
||||
|
||||
if (needToRestoreLinkset)
|
||||
|
|
|
@ -144,34 +144,37 @@ public class BSShapeCollection : IDisposable
|
|||
|
||||
// Release the usage of a body.
|
||||
// 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;
|
||||
|
||||
lock (m_collectionActivityLock)
|
||||
{
|
||||
BodyDesc bodyDesc;
|
||||
if (Bodies.TryGetValue(shape.ID, out bodyDesc))
|
||||
if (Bodies.TryGetValue(body.ID, out bodyDesc))
|
||||
{
|
||||
bodyDesc.referenceCount--;
|
||||
bodyDesc.lastReferenced = System.DateTime.Now;
|
||||
Bodies[shape.ID] = bodyDesc;
|
||||
DetailLog("{0},BSShapeCollection.DereferenceBody,ref={1}", shape.ID, bodyDesc.referenceCount);
|
||||
Bodies[body.ID] = bodyDesc;
|
||||
DetailLog("{0},BSShapeCollection.DereferenceBody,ref={1}", body.ID, bodyDesc.referenceCount);
|
||||
|
||||
// If body is no longer being used, free it -- bodies are never shared.
|
||||
if (bodyDesc.referenceCount == 0)
|
||||
{
|
||||
Bodies.Remove(shape.ID);
|
||||
Bodies.Remove(body.ID);
|
||||
BSScene.TaintCallback removeOperation = delegate()
|
||||
{
|
||||
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.
|
||||
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.
|
||||
BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, shape.ptr);
|
||||
BulletSimAPI.DestroyObject2(PhysicsScene.World.ptr, shape.ptr);
|
||||
BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, body.ptr);
|
||||
BulletSimAPI.DestroyObject2(PhysicsScene.World.ptr, body.ptr);
|
||||
};
|
||||
// If already in taint-time, do the operations now. Otherwise queue for later.
|
||||
if (inTaintTime)
|
||||
|
@ -182,7 +185,7 @@ public class BSShapeCollection : IDisposable
|
|||
}
|
||||
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