// 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)
{
base.Refresh(requestor);
if (HasAnyChildren && IsRoot(requestor))
{
// Queue to happen after all the other taint processing
PhysicsScene.PostTaintObject("BSLinksetContraints.Refresh", requestor.LocalID, delegate()
{
if (HasAnyChildren && IsRoot(requestor))
RecomputeLinksetConstraints();
});
}
}
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;
PhysicsScene = scene;
LinksetRoot = parent;
m_children = new HashSet<BSPrimLinkable>();
LinksetMass = parent.RawMass;
Rebuilding = false;
parent.ClearDisplacement();
}
// 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},BSLinksetCompound.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.
ScheduleRebuild(LinksetRoot);
}
/*else
{
// The origional prims are removed from the world as the shape of the root compound
// shape takes over.
PhysicsScene.PE.AddToCollisionFlags(child.PhysBody, CollisionFlags.CF_NO_CONTACT_RESPONSE);
PhysicsScene.PE.ForceActivationState(child.PhysBody, ActivationState.DISABLE_SIMULATION);
// We don't want collisions from the old linkset children.
PhysicsScene.PE.RemoveFromCollisionFlags(child.PhysBody, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
child.PhysBody.collisionType = CollisionType.LinksetChild;
ret = true;
}*/
return ret;
}
// 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)
{
// What is done for each object in BSPrim is what we want.
return false;
}
// 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)
{
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);
PhysicsScene.TaintedObject("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;
}
public BSLinksetGroupCompound(BSScene scene, BSPrimLinkable parent)
: base(scene, parent)
{
}
// 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);
// ================================================================
// Add a new child to the linkset.
// Called while LinkActivity is locked.
protected override void AddChildToLinkset(BSPrimLinkable child, bool scheduleRebuild)
{
if (!HasChild(child))
{
m_children.Add(child);
DetailLog("{0},BSLinksetCompound.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
// Rebuild the compound shape with the new child shape included
if (scheduleRebuild)
ScheduleRebuild(child);
}
return;
}
// Schedule a refresh to happen after all the other taint processing.
protected override void ScheduleRebuild(BSPrimLinkable requestor)
{
DetailLog("{0},BSLinksetCompound.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.
if (!Rebuilding && HasAnyChildren)
{
PhysicsScene.PostTaintObject("BSLinksetCompound.ScheduleRebuild", LinksetRoot.LocalID, delegate()
{
if (HasAnyChildren)
RecomputeLinksetCompound();
});
}
}
// For compound implimented linksets, if there are children, use compound shape for the root.
public override BSPhysicsShapeType PreferredPhysicalShape(BSPrimLinkable requestor)
{
// Returning 'unknown' means we don't have a preference.
BSPhysicsShapeType ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
if (IsRoot(requestor) && HasAnyChildren && requestor.IsPhysical)
{
ret = BSPhysicsShapeType.SHAPE_COMPOUND;
}
// DetailLog("{0},BSLinksetCompound.PreferredPhysicalShape,call,shape={1}", LinksetRoot.LocalID, ret);
return ret;
}
// Routine called when rebuilding the body of some member of the linkset.
// Since we don't keep in world relationships, do nothing unless it's a child changing.
// Returns 'true' of something was actually removed and would need restoring
// Called at taint-time!!
public override bool RemoveBodyDependencies(BSPrimLinkable child)
{
bool ret = false;
DetailLog("{0},BSLinksetCompound.RemoveBodyDependencies,refreshIfChild,rID={1},rBody={2},isRoot={3}",
child.LocalID, LinksetRoot.LocalID, LinksetRoot.PhysBody, IsRoot(child));
if (!IsRoot(child))
{
// Because it is a convenient time, recompute child world position and rotation based on
// its position in the linkset.
RecomputeChildWorldPosition(child, true /* inTaintTime */);
child.LinksetInfo = null;
}
// Cannot schedule a refresh/rebuild here because this routine is called when
// the linkset is being rebuilt.
// InternalRefresh(LinksetRoot);
return ret;
}
// ================================================================
// Add a new child to the linkset.
// Called while LinkActivity is locked.
protected override void AddChildToLinkset(BSPrimLinkable child, bool scheduleRebuild)
{
if (!this.LinksetRoot.IsPhysical)
return;
if (!HasChild(child))
{
m_children.Add(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.
if(scheduleRebuild)
Refresh(LinksetRoot);
}
return;
}
// 'centerDisplacement' is the distance from the root the the center-of-mass (Bullet 'zero' of the shape)
public BSLinksetCompoundInfo(int indx, BSPrimLinkable root, BSPrimLinkable child, OMV.Vector3 centerDisplacement)
{
// Each child position and rotation is given relative to the center-of-mass.
OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(root.RawOrientation);
OMV.Vector3 displacementFromRoot = (child.RawPosition - root.RawPosition) * invRootOrientation;
OMV.Vector3 displacementFromCOM = displacementFromRoot - centerDisplacement;
OMV.Quaternion displacementRot = child.RawOrientation * invRootOrientation;
// Save relative position for recomputing child's world position after moving linkset.
Index = indx;
OffsetFromRoot = displacementFromRoot;
OffsetFromCenterOfMass = displacementFromCOM;
OffsetRot = displacementRot;
}
// Called at taint-time!!
public override void UpdateProperties(UpdatedProperties whichUpdated, BSPrimLinkable pObj)
{
// Nothing to do for constraints on property updates
}
// 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 RemoveBodyDependencies(BSPrimLinkable child)
{
bool ret = false;
DetailLog("{0},BSLinksetConstraint.RemoveBodyDependencies,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 a new child is being added // Called while LinkActivity is locked. protected abstract void AddChildToLinkset(BSPrimLinkable child, bool scheduleRebuild);
protected abstract void ScheduleRebuild(BSPrimLinkable requestor);
// 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);
// 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)
{
if (!LinksetRoot.IsPhysical) return false;
bool ret = false;
DetailLog("{0},BSLinksetCompound.MakeStatic,call,IsRoot={1}", child.LocalID, IsRoot(child));
if (IsRoot(child))
{
ScheduleRebuild(LinksetRoot);
}
/*else
{
// The non-physical children can come back to life.
PhysicsScene.PE.RemoveFromCollisionFlags(child.PhysBody, CollisionFlags.CF_NO_CONTACT_RESPONSE);
child.PhysBody.collisionType = CollisionType.LinksetChild;
// Don't force activation so setting of DISABLE_SIMULATION can stay if used.
PhysicsScene.PE.Activate(child.PhysBody, false);
ret = true;
}*/
return ret;
}
protected override void ScheduleRebuild(BSPrimLinkable requestor)
{
Refresh(requestor);
}
// When physical properties are changed the linkset needs to recalculate
// its internal properties.
public override void Refresh(BSPrimLinkable requestor)
{
base.Refresh(requestor);
// Something changed so do the rebuilding thing
// ScheduleRebuild();
}
private BSConstraint BuildConstraint(BSPrimLinkable rootPrim, BSPrimLinkable childPrim)
{
// Zero motion for children so they don't interpolate
childPrim.ZeroMotion(true);
// 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);
DetailLog("{0},BSLinksetConstraint.BuildConstraint,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}",
rootPrim.LocalID,
rootPrim.LocalID, rootPrim.PhysBody.AddrString,
childPrim.LocalID, childPrim.PhysBody.AddrString,
rootPrim.Position, childPrim.Position, midPoint);
// create a constraint that allows no freedom of movement between the two objects
// http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
BSConstraint6Dof constrain = new BSConstraint6Dof(
PhysicsScene.World, rootPrim.PhysBody, childPrim.PhysBody, midPoint, true, true );
// PhysicsScene.World, childPrim.BSBody, rootPrim.BSBody, 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 = (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);
DetailLog("{0},BSLinksetConstraint.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
BS6DofConstraint constrain = new BS6DofConstraint(
PhysicsScene.World, rootPrim.Body, childPrim.Body,
OMV.Vector3.Zero,
OMV.Quaternion.Inverse(rootPrim.Orientation),
OMV.Vector3.Zero,
OMV.Quaternion.Inverse(childPrim.Orientation),
true,
true
);
// ==================================================================================
*/
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(BSParam.LinkConstraintUseFrameOffset);
constrain.TranslationalLimitMotor(BSParam.LinkConstraintEnableTransMotor,
BSParam.LinkConstraintTransMotorMaxVel,
BSParam.LinkConstraintTransMotorMaxForce);
constrain.SetCFMAndERP(BSParam.LinkConstraintCFM, BSParam.LinkConstraintERP);
if (BSParam.LinkConstraintSolverIterations != 0f)
{
constrain.SetSolverIterations(BSParam.LinkConstraintSolverIterations);
}
return constrain;
}
// 'physicalUpdate' is true if these changes came directly from the physics engine. Don't need to rebuild then.
// Called at taint-time.
public override void UpdateProperties(UpdatedProperties whichUpdated, BSPrimLinkable updated)
{
if (!LinksetRoot.IsPhysical) return;
// The user moving a child around requires the rebuilding of the linkset compound shape
// One problem is this happens when a border is crossed -- the simulator implementation
// stores the position into the group which causes the move of the object
// but it also means all the child positions get updated.
// What would cause an unnecessary rebuild so we make sure the linkset is in a
// region before bothering to do a rebuild.
if (!IsRoot(updated) && PhysicsScene.TerrainManager.IsWithinKnownTerrain(LinksetRoot.RawPosition))
{
// If a child of the linkset is updating only the position or rotation, that can be done
// without rebuilding the linkset.
// If a handle for the child can be fetch, we update the child here. If a rebuild was
// scheduled by someone else, the rebuild will just replace this setting.
bool updatedChild = false;
// Anything other than updating position or orientation usually means a physical update
// and that is caused by us updating the object.
if ((whichUpdated & ~(UpdatedProperties.Position | UpdatedProperties.Orientation)) == 0)
{
// Find the physical instance of the child
if (LinksetRoot.PhysShape.HasPhysicalShape && PhysicsScene.PE.IsCompound(LinksetRoot.PhysShape))
{
// It is possible that the linkset is still under construction and the child is not yet
// inserted into the compound shape. A rebuild of the linkset in a pre-step action will
// build the whole thing with the new position or rotation.
// The index must be checked because Bullet references the child array but does no validity
// checking of the child index passed.
int numLinksetChildren = PhysicsScene.PE.GetNumberOfCompoundChildren(LinksetRoot.PhysShape);
if (updated.LinksetChildIndex < numLinksetChildren)
{
BulletShape linksetChildShape = PhysicsScene.PE.GetChildShapeFromCompoundShapeIndex(LinksetRoot.PhysShape, updated.LinksetChildIndex);
if (linksetChildShape.HasPhysicalShape)
{
// Found the child shape within the compound shape
PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, updated.LinksetChildIndex,
updated.RawPosition - LinksetRoot.RawPosition,
updated.RawOrientation * OMV.Quaternion.Inverse(LinksetRoot.RawOrientation),
true /* shouldRecalculateLocalAabb */);
updatedChild = true;
DetailLog("{0},BSLinksetCompound.UpdateProperties,changeChildPosRot,whichUpdated={1},pos={2},rot={3}",
updated.LocalID, whichUpdated, updated.RawPosition, updated.RawOrientation);
}
else // DEBUG DEBUG
{ // DEBUG DEBUG
DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,noChildShape,shape={1}",
updated.LocalID, linksetChildShape);
} // DEBUG DEBUG
}
else // DEBUG DEBUG
{ // DEBUG DEBUG
// the child is not yet in the compound shape. This is non-fatal.
DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,childNotInCompoundShape,numChildren={1},index={2}",
updated.LocalID, numLinksetChildren, updated.LinksetChildIndex);
} // DEBUG DEBUG
}
else // DEBUG DEBUG
{ // DEBUG DEBUG
DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild,noBodyOrNotCompound", updated.LocalID);
} // DEBUG DEBUG
if (!updatedChild)
{
// If couldn't do the individual child, the linkset needs a rebuild to incorporate the new child info.
// Note: there are several ways through this code that will not update the child if
// the linkset is being rebuilt. In this case, scheduling a rebuild is a NOOP since
// there will already be a rebuild scheduled.
DetailLog("{0},BSLinksetCompound.UpdateProperties,couldNotUpdateChild.schedulingRebuild,whichUpdated={1}",
updated.LocalID, whichUpdated);
updated.LinksetInfo = null; // setting to 'null' causes relative position to be recomputed.
ScheduleRebuild(updated);
}
}
}
}
// 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);
}
// Remove the specified child from the linkset.
// Safe to call even if the child is not really in the linkset.
protected override void RemoveChildFromLinkset(BSPrimLinkable child)
{
child.ClearDisplacement();
if (m_children.Remove(child))
{
DetailLog("{0},BSLinksetCompound.RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}",
child.LocalID,
LinksetRoot.LocalID, LinksetRoot.PhysBody.AddrString,
child.LocalID, child.PhysBody.AddrString);
// Cause the child's body to be rebuilt and thus restored to normal operation
RecomputeChildWorldPosition(child, false);
child.LinksetInfo = null;
child.ForceBodyShapeRebuild(false);
if (!HasAnyChildren)
{
// The linkset is now empty. The root needs rebuilding.
LinksetRoot.ForceBodyShapeRebuild(false);
}
else
{
// Rebuild the compound shape with the child removed
ScheduleRebuild(LinksetRoot);
}
}
return;
}
// 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);
// Find the constraint for this link and get rid of it from the overall collection and from my list
if (PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.PhysBody, childPrim.PhysBody))
{
// Make the child refresh its location
PhysicsScene.PE.PushUpdate(childPrim.PhysBody);
ret = true;
}
return ret;
}
// When the linkset is built, the child shape is added to the compound shape relative to the
// root shape. The linkset then moves around but this does not move the actual child
// prim. The child prim's location must be recomputed based on the location of the root shape.
private void RecomputeChildWorldPosition(BSPrimLinkable child, bool inTaintTime)
{
// For the moment (20130201), disable this computation (converting the child physical addr back to
// a region address) until we have a good handle on center-of-mass offsets and what the physics
// engine moving a child actually means.
// The simulator keeps track of where children should be as the linkset moves. Setting
// the pos/rot here does not effect that knowledge as there is no good way for the
// physics engine to send the simulator an update for a child.
/*
BSLinksetCompoundInfo lci = child.LinksetInfo as BSLinksetCompoundInfo;
if (lci != null)
{
if (inTaintTime)
{
OMV.Vector3 oldPos = child.RawPosition;
child.ForcePosition = LinksetRoot.RawPosition + lci.OffsetFromRoot;
child.ForceOrientation = LinksetRoot.RawOrientation * lci.OffsetRot;
DetailLog("{0},BSLinksetCompound.RecomputeChildWorldPosition,oldPos={1},lci={2},newPos={3}",
child.LocalID, oldPos, lci, child.RawPosition);
}
else
{
// TaintedObject is not used here so the raw position is set now and not at taint-time.
child.Position = LinksetRoot.RawPosition + lci.OffsetFromRoot;
child.Orientation = LinksetRoot.RawOrientation * lci.OffsetRot;
}
}
else
{
// This happens when children have been added to the linkset but the linkset
// has not been constructed yet. So like, at taint time, adding children to a linkset
// and then changing properties of the children (makePhysical, for instance)
// but the post-print action of actually rebuilding the linkset has not yet happened.
// PhysicsScene.Logger.WarnFormat("{0} Restoring linkset child position failed because of no relative position computed. ID={1}",
// LogHeader, child.LocalID);
DetailLog("{0},BSLinksetCompound.recomputeChildWorldPosition,noRelativePositonInfo", child.LocalID);
}
*/
}
// 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 PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.PhysBody);
}
// private static string LogHeader = "[BULLETSIM LINKSET CONSTRAINTS]";
public BSLinksetConstraints(BSScene scene, BSPrimLinkable parent)
: base(scene, parent)
{
}
// 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)
{
lock (m_linksetActivityLock)
{
if (IsRoot(child))
{
// Cannot remove the root from a linkset.
return this;
}
RemoveChildFromLinkset(child);
LinksetMass = ComputeLinksetMass();
}
// The child is down to a linkset of just itself
return BSLinkset.Factory(PhysicsScene, child);
}
