// Create the correct type of linkset for this child
public static BSLinkset Factory(BSScene physScene, BSPrimLinkable parent)
{
BSLinkset ret = null;
switch (parent.LinksetType)
{
case LinksetImplementation.Constraint:
ret = new BSLinksetConstraints(physScene, parent);
break;
case LinksetImplementation.Compound:
ret = new BSLinksetCompound(physScene, parent);
break;
case LinksetImplementation.Manual:
// ret = new BSLinksetManual(physScene, parent);
break;
default:
ret = new BSLinksetCompound(physScene, parent);
break;
}
if (ret == null)
{
physScene.Logger.ErrorFormat("[BULLETSIM LINKSET] Factory could not create linkset. Parent name={1}, ID={2}", parent.Name, parent.LocalID);
}
return ret;
}
public BSLinkInfoConstraint(BSPrimLinkable pMember)
: base(pMember)
{
constraint = null;
ResetLink();
member.PhysScene.DetailLog("{0},BSLinkInfoConstraint.creation", member.LocalID);
}
// Routine called when rebuilding the body of some member of the linkset.
// If one of the bodies is being changed, the linkset needs rebuilding.
// For instance, a linkset is built and then a mesh asset is read in and the mesh is recreated.
// Returns 'true' of something was actually removed and would need restoring
// Called at taint-time!!
public override bool RemoveDependencies(BSPrimLinkable child)
{
bool ret = false;
DetailLog("{0},BSLinksetCompound.RemoveDependencies,refreshIfChild,rID={1},rBody={2},isRoot={3}",
child.LocalID, LinksetRoot.LocalID, LinksetRoot.PhysBody, IsRoot(child));
Refresh(child);
return(ret);
}
// The object is going dynamic (physical). Do any setup necessary
// for a dynamic linkset.
// Only the state of the passed object can be modified. The rest of the linkset
// has not yet been fully constructed.
// Return 'true' if any properties updated on the passed object.
// Called at taint-time!
public override bool MakeDynamic(BSPrimLinkable child)
{
bool ret = false;
DetailLog("{0},BSLinksetConstraints.MakeDynamic,call,IsRoot={1}", child.LocalID, IsRoot(child));
if (IsRoot(child))
{
// The root is going dynamic. Rebuild the linkset so parts and mass get computed properly.
Refresh(LinksetRoot);
}
return(ret);
}
public bool TryGetLinkInfo(BSPrimLinkable child, out BSLinkInfo foundInfo)
{
bool ret = false;
BSLinkInfo found = null;
lock (m_linksetActivityLock)
{
ret = m_children.TryGetValue(child, out found);
}
foundInfo = found;
return(ret);
}
// ================================================================
// Add a new child to the linkset.
// Called while LinkActivity is locked.
protected override void AddChildToLinkset(BSPrimLinkable child)
{
if (!HasChild(child))
{
m_children.Add(child, new BSLinkInfo(child));
DetailLog("{0},BSLinksetCompound.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
// Rebuild the compound shape with the new child shape included
Refresh(child);
}
return;
}
// The object is going static (non-physical). Do any setup necessary for a static linkset.
// Return 'true' if any properties updated on the passed object.
// This doesn't normally happen -- OpenSim removes the objects from the physical
// world if it is a static linkset.
// Called at taint-time!
public override bool MakeStatic(BSPrimLinkable child)
{
bool ret = false;
DetailLog("{0},BSLinksetConstraint.MakeStatic,call,IsRoot={1}", child.LocalID, IsRoot(child));
child.ClearDisplacement();
if (IsRoot(child))
{
// Schedule a rebuild to verify that the root shape is set to the real shape.
Refresh(LinksetRoot);
}
return(ret);
}
// ================================================================
// Add a new child to the linkset.
// Called while LinkActivity is locked.
protected override void AddChildToLinkset(BSPrimLinkable child)
{
if (!HasChild(child))
{
m_children.Add(child, new BSLinkInfoConstraint(child));
DetailLog("{0},BSLinksetConstraints.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
// Cause constraints and assorted properties to be recomputed before the next simulation step.
Refresh(LinksetRoot);
}
return;
}
// Link to a linkset where the child knows the parent.
// Parent changing should not happen so do some sanity checking.
// We return the parent's linkset so the child can track its membership.
// Called at runtime.
public BSLinkset AddMeToLinkset(BSPrimLinkable child)
{
lock (m_linksetActivityLock)
{
// Don't add the root to its own linkset
if (!IsRoot(child))
{
AddChildToLinkset(child);
}
LinksetMass = ComputeLinksetMass();
}
return(this);
}
// Check the type of the link and return 'true' if the link is flexible and the
// updates from the child should be sent to the simulator so things change.
public virtual bool ShouldReportPropertyUpdates(BSPrimLinkable child)
{
bool ret = false;
BSLinkInfo linkInfo;
if (m_children.TryGetValue(child, out linkInfo))
{
ret = linkInfo.ShouldUpdateChildProperties();
}
return(ret);
}
// Schedule a refresh to happen after all the other taint processing.
private void ScheduleRebuild(BSPrimLinkable requestor)
{
// When rebuilding, it is possible to set properties that would normally require a rebuild.
// If already rebuilding, don't request another rebuild.
// If a linkset with just a root prim (simple non-linked prim) don't bother rebuilding.
lock (m_linksetActivityLock)
{
if (!RebuildScheduled && !Rebuilding && HasAnyChildren)
{
InternalScheduleRebuild(requestor);
}
}
}
// Routine called when rebuilding the body of some member of the linkset.
// Destroy all the constraints have have been made to root and set
// up to rebuild the constraints before the next simulation step.
// Returns 'true' of something was actually removed and would need restoring
// Called at taint-time!!
public override bool RemoveDependencies(BSPrimLinkable child)
{
bool ret = false;
DetailLog("{0},BSLinksetConstraint.RemoveDependencies,removeChildrenForRoot,rID={1},rBody={2}",
child.LocalID, LinksetRoot.LocalID, LinksetRoot.PhysBody.AddrString);
lock (m_linksetActivityLock)
{
// Just undo all the constraints for this linkset. Rebuild at the end of the step.
ret = PhysicallyUnlinkAllChildrenFromRoot(LinksetRoot);
// Cause the constraints, et al to be rebuilt before the next simulation step.
Refresh(LinksetRoot);
}
return(ret);
}
// Remove a child from a linkset.
// Returns a new linkset for the child which is a linkset of one (just the
// orphened child).
// Called at runtime.
public BSLinkset RemoveMeFromLinkset(BSPrimLinkable child, bool inTaintTime)
{
lock (m_linksetActivityLock)
{
if (IsRoot(child))
{
// Cannot remove the root from a linkset.
return(this);
}
RemoveChildFromLinkset(child, inTaintTime);
LinksetMass = ComputeLinksetMass();
}
// The child is down to a linkset of just itself
return(BSLinkset.Factory(m_physicsScene, child));
}
public override void link(OpenSim.Region.PhysicsModules.SharedBase.PhysicsActor obj)
{
BSPrimLinkable parent = obj as BSPrimLinkable;
if (parent != null)
{
BSPhysObject parentBefore = Linkset.LinksetRoot; // DEBUG
int childrenBefore = Linkset.NumberOfChildren; // DEBUG
Linkset = parent.Linkset.AddMeToLinkset(this);
DetailLog("{0},BSPrimLinkable.link,call,parentBefore={1}, childrenBefore=={2}, parentAfter={3}, childrenAfter={4}",
LocalID, parentBefore.LocalID, childrenBefore, Linkset.LinksetRoot.LocalID, Linkset.NumberOfChildren);
}
return;
}
protected BSLinkset(BSScene scene, BSPrimLinkable parent)
{
// A simple linkset of one (no children)
LinksetID = m_nextLinksetID++;
// We create LOTS of linksets.
if (m_nextLinksetID <= 0)
{
m_nextLinksetID = 1;
}
m_physicsScene = scene;
LinksetRoot = parent;
m_children = new Dictionary <BSPrimLinkable, BSLinkInfo>();
LinksetMass = parent.RawMass;
Rebuilding = false;
RebuildScheduled = false;
parent.ClearDisplacement();
}
// Must be called with m_linksetActivityLock or race conditions will haunt you.
private void InternalScheduleRebuild(BSPrimLinkable requestor)
{
DetailLog("{0},BSLinksetCompound.InternalScheduleRebuild,,rebuilding={1},hasChildren={2}",
requestor.LocalID, Rebuilding, HasAnyChildren);
RebuildScheduled = true;
m_physicsScene.PostTaintObject("BSLinksetCompound.ScheduleRebuild", LinksetRoot.LocalID, delegate()
{
if (HasAnyChildren)
{
if (this.AllPartsComplete)
{
RecomputeLinksetCompound();
}
else
{
DetailLog("{0},BSLinksetCompound.InternalScheduleRebuild,,rescheduling because not all children complete",
requestor.LocalID);
InternalScheduleRebuild(requestor);
}
}
RebuildScheduled = false;
});
}
// Called after a simulation step to post a collision with this object.
// Return 'true' if linkset processed the collision. 'false' says the linkset didn't have
// anything to add for the collision and it should be passed through normal processing.
// Default processing for a linkset.
public virtual bool HandleCollide(BSPhysObject collider, BSPhysObject collidee,
OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth)
{
bool ret = false;
// prims in the same linkset cannot collide with each other
BSPrimLinkable convCollidee = collidee as BSPrimLinkable;
if (convCollidee != null && (LinksetID == convCollidee.Linkset.LinksetID))
{
// By returning 'true', we tell the caller the collision has been 'handled' so it won't
// do anything about this collision and thus, effectivily, ignoring the collision.
ret = true;
}
else
{
// Not a collision between members of the linkset. Must be a real collision.
// So the linkset root can know if there is a collision anywhere in the linkset.
LinksetRoot.SomeCollisionSimulationStep = m_physicsScene.SimulationStep;
}
return(ret);
}
public BSLinksetConstraints(BSScene scene, BSPrimLinkable parent) : base(scene, parent)
{
LinksetImpl = LinksetImplementation.Constraint;
}
private void ScheduleRebuild(BSPrimLinkable requestor)
{
DetailLog("{0},BSLinksetConstraint.ScheduleRebuild,,rebuilding={1},hasChildren={2},actuallyScheduling={3}",
requestor.LocalID, Rebuilding, HasAnyChildren, (!Rebuilding && HasAnyChildren));
// When rebuilding, it is possible to set properties that would normally require a rebuild.
// If already rebuilding, don't request another rebuild.
// If a linkset with just a root prim (simple non-linked prim) don't bother rebuilding.
lock (this)
{
if (!RebuildScheduled)
{
if (!Rebuilding && HasAnyChildren)
{
RebuildScheduled = true;
// Queue to happen after all the other taint processing
m_physicsScene.PostTaintObject("BSLinksetContraints.Refresh", requestor.LocalID, delegate()
{
if (HasAnyChildren)
{
// Constraints that have not been changed are not rebuild but make sure
// the constraint of the requestor is rebuilt.
PhysicallyUnlinkAChildFromRoot(LinksetRoot, requestor);
// Rebuild the linkset and all its constraints.
RecomputeLinksetConstraints();
}
RebuildScheduled = false;
});
}
}
}
}
// Called when a parameter update comes from the physics engine for any object // of the linkset is received. // Passed flag is update came from physics engine (true) or the user (false). // Called at taint-time!! public abstract void UpdateProperties(UpdatedProperties whichUpdated, BSPrimLinkable physObject);
public BSLinkInfo(BSPrimLinkable pMember)
{
member = pMember;
}
// Remove linkage between myself and any possible children I might have.
// Returns 'true' of any constraints were destroyed.
// Called at taint time!
private bool PhysicallyUnlinkAllChildrenFromRoot(BSPrimLinkable rootPrim)
{
DetailLog("{0},BSLinksetConstraint.PhysicallyUnlinkAllChildren,taint", rootPrim.LocalID);
return m_physicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.PhysBody);
}
// Called when a parameter update comes from the physics engine for any object // of the linkset is received. // Passed flag is update came from physics engine (true) or the user (false). // Called at taint-time!! public abstract void UpdateProperties(UpdatedProperties whichUpdated, BSPrimLinkable physObject);
// 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(BSPrimLinkable child, bool inTaintTime);
// Remove the specified child from the linkset.
// Safe to call even if the child is not really in my linkset.
protected override void RemoveChildFromLinkset(BSPrimLinkable child, bool inTaintTime)
{
if (m_children.Remove(child))
{
BSPrimLinkable rootx = LinksetRoot; // capture the root and body as of now
BSPrimLinkable childx = child;
DetailLog("{0},BSLinksetConstraints.RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}",
childx.LocalID,
rootx.LocalID, rootx.PhysBody.AddrString,
childx.LocalID, childx.PhysBody.AddrString);
m_physicsScene.TaintedObject(inTaintTime, childx.LocalID, "BSLinksetConstraints.RemoveChildFromLinkset", delegate()
{
PhysicallyUnlinkAChildFromRoot(rootx, childx);
});
// See that the linkset parameters are recomputed at the end of the taint time.
Refresh(LinksetRoot);
}
else
{
// Non-fatal occurance.
// PhysicsScene.Logger.ErrorFormat("{0}: Asked to remove child from linkset that was not in linkset", LogHeader);
}
return;
}
// Called at taint-time!!
public override void UpdateProperties(UpdatedProperties whichUpdated, BSPrimLinkable pObj)
{
// Nothing to do for constraints on property updates
}
// When physical properties are changed the linkset needs to recalculate
// its internal properties.
// May be called at runtime or taint-time.
public virtual void Refresh(BSPrimLinkable requestor)
{
LinksetMass = ComputeLinksetMass();
}
// The object is going dynamic (physical). Do any setup necessary // for a dynamic linkset. // Only the state of the passed object can be modified. The rest of the linkset // has not yet been fully constructed. // Return 'true' if any properties updated on the passed object. // Called at taint-time! public abstract bool MakeDynamic(BSPrimLinkable child);
// The object is going static (non-physical). Do any setup necessary // for a static linkset. // Return 'true' if any properties updated on the passed object. // Called at taint-time! public abstract bool MakeStatic(BSPrimLinkable child);
// When physical properties are changed the linkset needs to recalculate
// its internal properties.
// This is queued in the 'post taint' queue so the
// refresh will happen once after all the other taints are applied.
public override void Refresh(BSPrimLinkable requestor)
{
ScheduleRebuild(requestor);
base.Refresh(requestor);
}
// The object is going static (non-physical). Do any setup necessary for a static linkset.
// Return 'true' if any properties updated on the passed object.
// This doesn't normally happen -- OpenSim removes the objects from the physical
// world if it is a static linkset.
// Called at taint-time!
public override bool MakeStatic(BSPrimLinkable child)
{
bool ret = false;
DetailLog("{0},BSLinksetConstraint.MakeStatic,call,IsRoot={1}", child.LocalID, IsRoot(child));
child.ClearDisplacement();
if (IsRoot(child))
{
// Schedule a rebuild to verify that the root shape is set to the real shape.
Refresh(LinksetRoot);
}
return ret;
}
// Create a constraint between me (root of linkset) and the passed prim (the child).
// Called at taint time!
private void PhysicallyLinkAChildToRoot(BSPrimLinkable rootPrim, BSPrimLinkable childPrim)
{
// Don't build the constraint when asked. Put it off until just before the simulation step.
Refresh(rootPrim);
}
// Routine called when rebuilding the body of some member of the linkset.
// Destroy all the constraints have have been made to root and set
// up to rebuild the constraints before the next simulation step.
// Returns 'true' of something was actually removed and would need restoring
// Called at taint-time!!
public override bool RemoveDependencies(BSPrimLinkable child)
{
bool ret = false;
DetailLog("{0},BSLinksetConstraint.RemoveDependencies,removeChildrenForRoot,rID={1},rBody={2}",
child.LocalID, LinksetRoot.LocalID, LinksetRoot.PhysBody.AddrString);
lock (m_linksetActivityLock)
{
// Just undo all the constraints for this linkset. Rebuild at the end of the step.
ret = PhysicallyUnlinkAllChildrenFromRoot(LinksetRoot);
// Cause the constraints, et al to be rebuilt before the next simulation step.
Refresh(LinksetRoot);
}
return ret;
}
// 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(BSPrimLinkable child, bool inTaintTime);
// Create a static constraint between the two passed objects
private BSConstraint BuildConstraint(BSPrimLinkable rootPrim, BSLinkInfo li)
{
BSLinkInfoConstraint linkInfo = li as BSLinkInfoConstraint;
if (linkInfo == null)
return null;
// Zero motion for children so they don't interpolate
li.member.ZeroMotion(true);
BSConstraint constrain = null;
switch (linkInfo.constraintType)
{
case ConstraintType.BS_FIXED_CONSTRAINT_TYPE:
case ConstraintType.D6_CONSTRAINT_TYPE:
// Relative position normalized to the root prim
// Essentually a vector pointing from center of rootPrim to center of li.member
OMV.Vector3 childRelativePosition = linkInfo.member.Position - rootPrim.Position;
// real world coordinate of midpoint between the two objects
OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2);
DetailLog("{0},BSLinksetConstraint.BuildConstraint,6Dof,rBody={1},cBody={2},rLoc={3},cLoc={4},midLoc={5}",
rootPrim.LocalID, rootPrim.PhysBody, linkInfo.member.PhysBody,
rootPrim.Position, linkInfo.member.Position, midPoint);
// create a constraint that allows no freedom of movement between the two objects
// http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
constrain = new BSConstraint6Dof(
m_physicsScene.World, rootPrim.PhysBody, linkInfo.member.PhysBody, midPoint, true, true );
/* NOTE: below is an attempt to build constraint with full frame computation, etc.
* Using the midpoint is easier since it lets the Bullet code manipulate the transforms
* of the objects.
* Code left for future programmers.
// ==================================================================================
// relative position normalized to the root prim
OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation);
OMV.Vector3 childRelativePosition = (liConstraint.member.Position - rootPrim.Position) * invThisOrientation;
// relative rotation of the child to the parent
OMV.Quaternion childRelativeRotation = invThisOrientation * liConstraint.member.Orientation;
OMV.Quaternion inverseChildRelativeRotation = OMV.Quaternion.Inverse(childRelativeRotation);
DetailLog("{0},BSLinksetConstraint.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, liConstraint.member.LocalID);
constrain = new BS6DofConstraint(
PhysicsScene.World, rootPrim.Body, liConstraint.member.Body,
OMV.Vector3.Zero,
OMV.Quaternion.Inverse(rootPrim.Orientation),
OMV.Vector3.Zero,
OMV.Quaternion.Inverse(liConstraint.member.Orientation),
true,
true
);
// ==================================================================================
*/
break;
case ConstraintType.D6_SPRING_CONSTRAINT_TYPE:
constrain = new BSConstraintSpring(m_physicsScene.World, rootPrim.PhysBody, linkInfo.member.PhysBody,
linkInfo.frameInAloc, linkInfo.frameInArot, linkInfo.frameInBloc, linkInfo.frameInBrot,
linkInfo.useLinearReferenceFrameA,
true /*disableCollisionsBetweenLinkedBodies*/);
DetailLog("{0},BSLinksetConstraint.BuildConstraint,spring,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6}",
rootPrim.LocalID,
rootPrim.LocalID, rootPrim.PhysBody.AddrString,
linkInfo.member.LocalID, linkInfo.member.PhysBody.AddrString,
rootPrim.Position, linkInfo.member.Position);
break;
default:
break;
}
linkInfo.SetLinkParameters(constrain);
m_physicsScene.Constraints.AddConstraint(constrain);
return constrain;
}
// The object is going dynamic (physical). Do any setup necessary // for a dynamic linkset. // Only the state of the passed object can be modified. The rest of the linkset // has not yet been fully constructed. // Return 'true' if any properties updated on the passed object. // Called at taint-time! public abstract bool MakeDynamic(BSPrimLinkable child);
// Create a static constraint between the two passed objects
private BSConstraint BuildConstraint(BSPrimLinkable rootPrim, BSLinkInfo li)
{
BSLinkInfoConstraint linkInfo = li as BSLinkInfoConstraint;
if (linkInfo == null)
{
return(null);
}
// Zero motion for children so they don't interpolate
li.member.ZeroMotion(true);
BSConstraint constrain = null;
switch (linkInfo.constraintType)
{
case ConstraintType.BS_FIXED_CONSTRAINT_TYPE:
case ConstraintType.D6_CONSTRAINT_TYPE:
// Relative position normalized to the root prim
// Essentually a vector pointing from center of rootPrim to center of li.member
OMV.Vector3 childRelativePosition = linkInfo.member.Position - rootPrim.Position;
// real world coordinate of midpoint between the two objects
OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2);
DetailLog("{0},BSLinksetConstraint.BuildConstraint,6Dof,rBody={1},cBody={2},rLoc={3},cLoc={4},midLoc={5}",
rootPrim.LocalID, rootPrim.PhysBody, linkInfo.member.PhysBody,
rootPrim.Position, linkInfo.member.Position, midPoint);
// create a constraint that allows no freedom of movement between the two objects
// http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
constrain = new BSConstraint6Dof(
m_physicsScene.World, rootPrim.PhysBody, linkInfo.member.PhysBody, midPoint, true, true);
/* NOTE: below is an attempt to build constraint with full frame computation, etc.
* Using the midpoint is easier since it lets the Bullet code manipulate the transforms
* of the objects.
* Code left for future programmers.
* // ==================================================================================
* // relative position normalized to the root prim
* OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation);
* OMV.Vector3 childRelativePosition = (liConstraint.member.Position - rootPrim.Position) * invThisOrientation;
*
* // relative rotation of the child to the parent
* OMV.Quaternion childRelativeRotation = invThisOrientation * liConstraint.member.Orientation;
* OMV.Quaternion inverseChildRelativeRotation = OMV.Quaternion.Inverse(childRelativeRotation);
*
* DetailLog("{0},BSLinksetConstraint.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, liConstraint.member.LocalID);
* constrain = new BS6DofConstraint(
* PhysicsScene.World, rootPrim.Body, liConstraint.member.Body,
* OMV.Vector3.Zero,
* OMV.Quaternion.Inverse(rootPrim.Orientation),
* OMV.Vector3.Zero,
* OMV.Quaternion.Inverse(liConstraint.member.Orientation),
* true,
* true
* );
* // ==================================================================================
*/
break;
case ConstraintType.D6_SPRING_CONSTRAINT_TYPE:
constrain = new BSConstraintSpring(m_physicsScene.World, rootPrim.PhysBody, linkInfo.member.PhysBody,
linkInfo.frameInAloc, linkInfo.frameInArot, linkInfo.frameInBloc, linkInfo.frameInBrot,
linkInfo.useLinearReferenceFrameA,
true /*disableCollisionsBetweenLinkedBodies*/);
DetailLog("{0},BSLinksetConstraint.BuildConstraint,spring,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6}",
rootPrim.LocalID,
rootPrim.LocalID, rootPrim.PhysBody.AddrString,
linkInfo.member.LocalID, linkInfo.member.PhysBody.AddrString,
rootPrim.Position, linkInfo.member.Position);
break;
default:
break;
}
linkInfo.SetLinkParameters(constrain);
m_physicsScene.Constraints.AddConstraint(constrain);
return(constrain);
}
// 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 was actually removed and would need restoring // Called at taint-time!! public abstract bool RemoveDependencies(BSPrimLinkable child);
// When physical properties are changed the linkset needs to recalculate
// its internal properties.
// May be called at runtime or taint-time.
public virtual void Refresh(BSPrimLinkable requestor)
{
LinksetMass = ComputeLinksetMass();
}
// Remove linkage between myself and any possible children I might have.
// Returns 'true' of any constraints were destroyed.
// Called at taint time!
private bool PhysicallyUnlinkAllChildrenFromRoot(BSPrimLinkable rootPrim)
{
DetailLog("{0},BSLinksetConstraint.PhysicallyUnlinkAllChildren,taint", rootPrim.LocalID);
return(m_physicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.PhysBody));
}
// The object is going static (non-physical). Do any setup necessary // for a static linkset. // Return 'true' if any properties updated on the passed object. // Called at taint-time! public abstract bool MakeStatic(BSPrimLinkable child);
public override object Extension(string pFunct, params object[] pParams)
{
object ret = null;
switch (pFunct)
{
// pParams = [ BSPhysObject root, BSPhysObject child, integer linkType ]
case ExtendedPhysics.PhysFunctChangeLinkType:
if (pParams.Length > 2)
{
int requestedType = (int)pParams[2];
DetailLog("{0},BSLinksetConstraint.ChangeLinkType,requestedType={1}", LinksetRoot.LocalID, requestedType);
if (requestedType == (int)ConstraintType.BS_FIXED_CONSTRAINT_TYPE ||
requestedType == (int)ConstraintType.D6_CONSTRAINT_TYPE ||
requestedType == (int)ConstraintType.D6_SPRING_CONSTRAINT_TYPE ||
requestedType == (int)ConstraintType.HINGE_CONSTRAINT_TYPE ||
requestedType == (int)ConstraintType.CONETWIST_CONSTRAINT_TYPE ||
requestedType == (int)ConstraintType.SLIDER_CONSTRAINT_TYPE)
{
BSPrimLinkable child = pParams[1] as BSPrimLinkable;
if (child != null)
{
DetailLog("{0},BSLinksetConstraint.ChangeLinkType,rootID={1},childID={2},type={3}",
LinksetRoot.LocalID, LinksetRoot.LocalID, child.LocalID, requestedType);
m_physicsScene.TaintedObject(child.LocalID, "BSLinksetConstraint.PhysFunctChangeLinkType", delegate()
{
// Pick up all the constraints currently created.
RemoveDependencies(child);
BSLinkInfo linkInfo = null;
if (TryGetLinkInfo(child, out linkInfo))
{
BSLinkInfoConstraint linkInfoC = linkInfo as BSLinkInfoConstraint;
if (linkInfoC != null)
{
linkInfoC.constraintType = (ConstraintType)requestedType;
ret = (object)true;
DetailLog("{0},BSLinksetConstraint.ChangeLinkType,link={1},type={2}",
linkInfo.member.LocalID, linkInfo.member.LocalID, linkInfoC.constraintType);
}
else
{
DetailLog("{0},BSLinksetConstraint.ChangeLinkType,linkInfoNotConstraint,childID={1}", LinksetRoot.LocalID, child.LocalID);
}
}
else
{
DetailLog("{0},BSLinksetConstraint.ChangeLinkType,noLinkInfoForChild,childID={1}", LinksetRoot.LocalID, child.LocalID);
}
// Cause the whole linkset to be rebuilt in post-taint time.
Refresh(child);
});
}
else
{
DetailLog("{0},BSLinksetConstraint.SetLinkType,childNotBSPrimLinkable", LinksetRoot.LocalID);
}
}
else
{
DetailLog("{0},BSLinksetConstraint.SetLinkType,illegalRequestedType,reqested={1},spring={2}",
LinksetRoot.LocalID, requestedType, ((int)ConstraintType.D6_SPRING_CONSTRAINT_TYPE));
}
}
break;
// pParams = [ BSPhysObject root, BSPhysObject child ]
case ExtendedPhysics.PhysFunctGetLinkType:
if (pParams.Length > 0)
{
BSPrimLinkable child = pParams[1] as BSPrimLinkable;
if (child != null)
{
BSLinkInfo linkInfo = null;
if (TryGetLinkInfo(child, out linkInfo))
{
BSLinkInfoConstraint linkInfoC = linkInfo as BSLinkInfoConstraint;
if (linkInfoC != null)
{
ret = (object)(int)linkInfoC.constraintType;
DetailLog("{0},BSLinksetConstraint.GetLinkType,link={1},type={2}",
linkInfo.member.LocalID, linkInfo.member.LocalID, linkInfoC.constraintType);
}
}
}
}
break;
// pParams = [ BSPhysObject root, BSPhysObject child, int op, object opParams, int op, object opParams, ... ]
case ExtendedPhysics.PhysFunctChangeLinkParams:
// There should be two parameters: the childActor and a list of parameters to set
if (pParams.Length > 2)
{
BSPrimLinkable child = pParams[1] as BSPrimLinkable;
BSLinkInfo baseLinkInfo = null;
if (TryGetLinkInfo(child, out baseLinkInfo))
{
BSLinkInfoConstraint linkInfo = baseLinkInfo as BSLinkInfoConstraint;
if (linkInfo != null)
{
int valueInt;
float valueFloat;
bool valueBool;
OMV.Vector3 valueVector;
OMV.Vector3 valueVector2;
OMV.Quaternion valueQuaternion;
int axisLow, axisHigh;
int opIndex = 2;
while (opIndex < pParams.Length)
{
int thisOp = 0;
string errMsg = "";
try
{
thisOp = (int)pParams[opIndex];
DetailLog("{0},BSLinksetConstraint.ChangeLinkParams2,op={1},val={2}",
linkInfo.member.LocalID, thisOp, pParams[opIndex + 1]);
switch (thisOp)
{
case ExtendedPhysics.PHYS_PARAM_LINK_TYPE:
valueInt = (int)pParams[opIndex + 1];
ConstraintType valueType = (ConstraintType)valueInt;
if (valueType == ConstraintType.BS_FIXED_CONSTRAINT_TYPE ||
valueType == ConstraintType.D6_CONSTRAINT_TYPE ||
valueType == ConstraintType.D6_SPRING_CONSTRAINT_TYPE ||
valueType == ConstraintType.HINGE_CONSTRAINT_TYPE ||
valueType == ConstraintType.CONETWIST_CONSTRAINT_TYPE ||
valueType == ConstraintType.SLIDER_CONSTRAINT_TYPE)
{
linkInfo.constraintType = valueType;
}
opIndex += 2;
break;
case ExtendedPhysics.PHYS_PARAM_FRAMEINA_LOC:
errMsg = "PHYS_PARAM_FRAMEINA_LOC takes one parameter of type vector";
valueVector = (OMV.Vector3)pParams[opIndex + 1];
linkInfo.frameInAloc = valueVector;
opIndex += 2;
break;
case ExtendedPhysics.PHYS_PARAM_FRAMEINA_ROT:
errMsg = "PHYS_PARAM_FRAMEINA_ROT takes one parameter of type rotation";
valueQuaternion = (OMV.Quaternion)pParams[opIndex + 1];
linkInfo.frameInArot = valueQuaternion;
opIndex += 2;
break;
case ExtendedPhysics.PHYS_PARAM_FRAMEINB_LOC:
errMsg = "PHYS_PARAM_FRAMEINB_LOC takes one parameter of type vector";
valueVector = (OMV.Vector3)pParams[opIndex + 1];
linkInfo.frameInBloc = valueVector;
opIndex += 2;
break;
case ExtendedPhysics.PHYS_PARAM_FRAMEINB_ROT:
errMsg = "PHYS_PARAM_FRAMEINB_ROT takes one parameter of type rotation";
valueQuaternion = (OMV.Quaternion)pParams[opIndex + 1];
linkInfo.frameInBrot = valueQuaternion;
opIndex += 2;
break;
case ExtendedPhysics.PHYS_PARAM_LINEAR_LIMIT_LOW:
errMsg = "PHYS_PARAM_LINEAR_LIMIT_LOW takes one parameter of type vector";
valueVector = (OMV.Vector3)pParams[opIndex + 1];
linkInfo.linearLimitLow = valueVector;
opIndex += 2;
break;
case ExtendedPhysics.PHYS_PARAM_LINEAR_LIMIT_HIGH:
errMsg = "PHYS_PARAM_LINEAR_LIMIT_HIGH takes one parameter of type vector";
valueVector = (OMV.Vector3)pParams[opIndex + 1];
linkInfo.linearLimitHigh = valueVector;
opIndex += 2;
break;
case ExtendedPhysics.PHYS_PARAM_ANGULAR_LIMIT_LOW:
errMsg = "PHYS_PARAM_ANGULAR_LIMIT_LOW takes one parameter of type vector";
valueVector = (OMV.Vector3)pParams[opIndex + 1];
linkInfo.angularLimitLow = valueVector;
opIndex += 2;
break;
case ExtendedPhysics.PHYS_PARAM_ANGULAR_LIMIT_HIGH:
errMsg = "PHYS_PARAM_ANGULAR_LIMIT_HIGH takes one parameter of type vector";
valueVector = (OMV.Vector3)pParams[opIndex + 1];
linkInfo.angularLimitHigh = valueVector;
opIndex += 2;
break;
case ExtendedPhysics.PHYS_PARAM_USE_FRAME_OFFSET:
errMsg = "PHYS_PARAM_USE_FRAME_OFFSET takes one parameter of type integer (bool)";
valueBool = ((int)pParams[opIndex + 1]) != 0;
linkInfo.useFrameOffset = valueBool;
opIndex += 2;
break;
case ExtendedPhysics.PHYS_PARAM_ENABLE_TRANSMOTOR:
errMsg = "PHYS_PARAM_ENABLE_TRANSMOTOR takes one parameter of type integer (bool)";
valueBool = ((int)pParams[opIndex + 1]) != 0;
linkInfo.enableTransMotor = valueBool;
opIndex += 2;
break;
case ExtendedPhysics.PHYS_PARAM_TRANSMOTOR_MAXVEL:
errMsg = "PHYS_PARAM_TRANSMOTOR_MAXVEL takes one parameter of type float";
valueFloat = (float)pParams[opIndex + 1];
linkInfo.transMotorMaxVel = valueFloat;
opIndex += 2;
break;
case ExtendedPhysics.PHYS_PARAM_TRANSMOTOR_MAXFORCE:
errMsg = "PHYS_PARAM_TRANSMOTOR_MAXFORCE takes one parameter of type float";
valueFloat = (float)pParams[opIndex + 1];
linkInfo.transMotorMaxForce = valueFloat;
opIndex += 2;
break;
case ExtendedPhysics.PHYS_PARAM_CFM:
errMsg = "PHYS_PARAM_CFM takes one parameter of type float";
valueFloat = (float)pParams[opIndex + 1];
linkInfo.cfm = valueFloat;
opIndex += 2;
break;
case ExtendedPhysics.PHYS_PARAM_ERP:
errMsg = "PHYS_PARAM_ERP takes one parameter of type float";
valueFloat = (float)pParams[opIndex + 1];
linkInfo.erp = valueFloat;
opIndex += 2;
break;
case ExtendedPhysics.PHYS_PARAM_SOLVER_ITERATIONS:
errMsg = "PHYS_PARAM_SOLVER_ITERATIONS takes one parameter of type float";
valueFloat = (float)pParams[opIndex + 1];
linkInfo.solverIterations = valueFloat;
opIndex += 2;
break;
case ExtendedPhysics.PHYS_PARAM_SPRING_AXIS_ENABLE:
errMsg = "PHYS_PARAM_SPRING_AXIS_ENABLE takes two parameters of types integer and integer (bool)";
valueInt = (int)pParams[opIndex + 1];
valueBool = ((int)pParams[opIndex + 2]) != 0;
GetAxisRange(valueInt, out axisLow, out axisHigh);
for (int ii = axisLow; ii <= axisHigh; ii++)
{
linkInfo.springAxisEnable[ii] = valueBool;
}
opIndex += 3;
break;
case ExtendedPhysics.PHYS_PARAM_SPRING_DAMPING:
errMsg = "PHYS_PARAM_SPRING_DAMPING takes two parameters of types integer and float";
valueInt = (int)pParams[opIndex + 1];
valueFloat = (float)pParams[opIndex + 2];
GetAxisRange(valueInt, out axisLow, out axisHigh);
for (int ii = axisLow; ii <= axisHigh; ii++)
{
linkInfo.springDamping[ii] = valueFloat;
}
opIndex += 3;
break;
case ExtendedPhysics.PHYS_PARAM_SPRING_STIFFNESS:
errMsg = "PHYS_PARAM_SPRING_STIFFNESS takes two parameters of types integer and float";
valueInt = (int)pParams[opIndex + 1];
valueFloat = (float)pParams[opIndex + 2];
GetAxisRange(valueInt, out axisLow, out axisHigh);
for (int ii = axisLow; ii <= axisHigh; ii++)
{
linkInfo.springStiffness[ii] = valueFloat;
}
opIndex += 3;
break;
case ExtendedPhysics.PHYS_PARAM_SPRING_EQUILIBRIUM_POINT:
errMsg = "PHYS_PARAM_SPRING_EQUILIBRIUM_POINT takes two parameters of type vector";
valueVector = (OMV.Vector3)pParams[opIndex + 1];
valueVector2 = (OMV.Vector3)pParams[opIndex + 2];
linkInfo.springLinearEquilibriumPoint = valueVector;
linkInfo.springAngularEquilibriumPoint = valueVector2;
opIndex += 3;
break;
case ExtendedPhysics.PHYS_PARAM_USE_LINEAR_FRAMEA:
errMsg = "PHYS_PARAM_USE_LINEAR_FRAMEA takes one parameter of type integer (bool)";
valueBool = ((int)pParams[opIndex + 1]) != 0;
linkInfo.useLinearReferenceFrameA = valueBool;
opIndex += 2;
break;
default:
break;
}
}
catch (InvalidCastException e)
{
m_physicsScene.Logger.WarnFormat("{0} value of wrong type in physSetLinksetParams: {1}, err={2}",
LogHeader, errMsg, e);
}
catch (Exception e)
{
m_physicsScene.Logger.WarnFormat("{0} bad parameters in physSetLinksetParams: {1}", LogHeader, e);
}
}
}
// Something changed so a rebuild is in order
Refresh(child);
}
}
break;
default:
ret = base.Extension(pFunct, pParams);
break;
}
return(ret);
}
// 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 was actually removed and would need restoring // Called at taint-time!! public abstract bool RemoveDependencies(BSPrimLinkable child);
// Return 'true' if the passed object is the root object of this linkset
public bool IsRoot(BSPrimLinkable requestor)
{
return (requestor.LocalID == LinksetRoot.LocalID);
}
// Remove a child from a linkset.
// Returns a new linkset for the child which is a linkset of one (just the
// orphened child).
// Called at runtime.
public BSLinkset RemoveMeFromLinkset(BSPrimLinkable child, bool inTaintTime)
{
lock (m_linksetActivityLock)
{
if (IsRoot(child))
{
// Cannot remove the root from a linkset.
return this;
}
RemoveChildFromLinkset(child, inTaintTime);
LinksetMass = ComputeLinksetMass();
}
// The child is down to a linkset of just itself
return BSLinkset.Factory(m_physicsScene, child);
}
// When physical properties are changed the linkset needs to recalculate
// its internal properties.
// This is queued in the 'post taint' queue so the
// refresh will happen once after all the other taints are applied.
public override void Refresh(BSPrimLinkable requestor)
{
ScheduleRebuild(requestor);
base.Refresh(requestor);
}
// I am the root of a linkset and a new child is being added // Called while LinkActivity is locked. protected abstract void AddChildToLinkset(BSPrimLinkable child);
// The object is going dynamic (physical). Do any setup necessary
// for a dynamic linkset.
// Only the state of the passed object can be modified. The rest of the linkset
// has not yet been fully constructed.
// Return 'true' if any properties updated on the passed object.
// Called at taint-time!
public override bool MakeDynamic(BSPrimLinkable child)
{
bool ret = false;
DetailLog("{0},BSLinksetConstraints.MakeDynamic,call,IsRoot={1}", child.LocalID, IsRoot(child));
if (IsRoot(child))
{
// The root is going dynamic. Rebuild the linkset so parts and mass get computed properly.
Refresh(LinksetRoot);
}
return ret;
}
// Check the type of the link and return 'true' if the link is flexible and the
// updates from the child should be sent to the simulator so things change.
public virtual bool ShouldReportPropertyUpdates(BSPrimLinkable child)
{
bool ret = false;
BSLinkInfo linkInfo;
if (m_children.TryGetValue(child, out linkInfo))
{
ret = linkInfo.ShouldUpdateChildProperties();
}
return ret;
}
// Called at taint-time!!
public override void UpdateProperties(UpdatedProperties whichUpdated, BSPrimLinkable pObj)
{
// Nothing to do for constraints on property updates
}
// Return 'true' if the passed object is the root object of this linkset
public bool IsRoot(BSPrimLinkable requestor)
{
return(requestor.LocalID == LinksetRoot.LocalID);
}
// ================================================================
// Add a new child to the linkset.
// Called while LinkActivity is locked.
protected override void AddChildToLinkset(BSPrimLinkable child)
{
if (!HasChild(child))
{
m_children.Add(child, new BSLinkInfoConstraint(child));
DetailLog("{0},BSLinksetConstraints.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
// Cause constraints and assorted properties to be recomputed before the next simulation step.
Refresh(LinksetRoot);
}
return;
}
public bool TryGetLinkInfo(BSPrimLinkable child, out BSLinkInfo foundInfo)
{
bool ret = false;
BSLinkInfo found = null;
lock (m_linksetActivityLock)
{
ret = m_children.TryGetValue(child, out found);
}
foundInfo = found;
return ret;
}
// Create a constraint between me (root of linkset) and the passed prim (the child).
// Called at taint time!
private void PhysicallyLinkAChildToRoot(BSPrimLinkable rootPrim, BSPrimLinkable childPrim)
{
// Don't build the constraint when asked. Put it off until just before the simulation step.
Refresh(rootPrim);
}
// Return 'true' if this child is in this linkset
public bool HasChild(BSPrimLinkable child)
{
bool ret = false;
lock (m_linksetActivityLock)
{
ret = m_children.ContainsKey(child);
}
return ret;
}
// Remove linkage between the linkset root and a particular child
// The root and child bodies are passed in because we need to remove the constraint between
// the bodies that were present at unlink time.
// Called at taint time!
private bool PhysicallyUnlinkAChildFromRoot(BSPrimLinkable rootPrim, BSPrimLinkable childPrim)
{
bool ret = false;
DetailLog("{0},BSLinksetConstraint.PhysicallyUnlinkAChildFromRoot,taint,root={1},rBody={2},child={3},cBody={4}",
rootPrim.LocalID,
rootPrim.LocalID, rootPrim.PhysBody.AddrString,
childPrim.LocalID, childPrim.PhysBody.AddrString);
// If asked to unlink root from root, just remove all the constraints
if (rootPrim == childPrim || childPrim == LinksetRoot)
{
PhysicallyUnlinkAllChildrenFromRoot(LinksetRoot);
ret = true;
}
else
{
// Find the constraint for this link and get rid of it from the overall collection and from my list
if (m_physicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.PhysBody, childPrim.PhysBody))
{
// Make the child refresh its location
m_physicsScene.PE.PushUpdate(childPrim.PhysBody);
ret = true;
}
}
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(BSPrimLinkable child);
// Link to a linkset where the child knows the parent.
// Parent changing should not happen so do some sanity checking.
// We return the parent's linkset so the child can track its membership.
// Called at runtime.
public BSLinkset AddMeToLinkset(BSPrimLinkable child)
{
lock (m_linksetActivityLock)
{
// Don't add the root to its own linkset
if (!IsRoot(child))
AddChildToLinkset(child);
LinksetMass = ComputeLinksetMass();
}
return this;
}
public BSLinkInfo(BSPrimLinkable pMember)
{
member = pMember;
}
