// Call each of the constraints that make up this linkset and recompute the
// various transforms and variables. Create constraints of not created yet.
// Called before the simulation step to make sure the constraint based linkset
// is all initialized.
// Called at taint time!!
private void RecomputeLinksetConstraints()
{
float linksetMass = LinksetMass;
LinksetRoot.UpdatePhysicalMassProperties(linksetMass, true);
// DEBUG: see of inter-linkset collisions are causing problems
// BulletSimAPI.SetCollisionFilterMask2(LinksetRoot.BSBody.ptr,
// (uint)CollisionFilterGroups.LinksetFilter, (uint)CollisionFilterGroups.LinksetMask);
DetailLog("{0},BSLinksetConstraint.RecomputeLinksetConstraints,set,rBody={1},linksetMass={2}",
LinksetRoot.LocalID, LinksetRoot.PhysBody.ptr.ToString(), linksetMass);
foreach (BSPhysObject child in m_children)
{
// A child in the linkset physically shows the mass of the whole linkset.
// This allows Bullet to apply enough force on the child to move the whole linkset.
// (Also do the mass stuff before recomputing the constraint so mass is not zero.)
child.UpdatePhysicalMassProperties(linksetMass, true);
BSConstraint constrain;
if (!PhysicsScene.Constraints.TryGetConstraint(LinksetRoot.PhysBody, child.PhysBody, out constrain))
{
// If constraint doesn't exist yet, create it.
constrain = BuildConstraint(LinksetRoot, child);
}
constrain.RecomputeConstraintVariables(linksetMass);
// DEBUG: see of inter-linkset collisions are causing problems
// BulletSimAPI.SetCollisionFilterMask2(child.BSBody.ptr,
// (uint)CollisionFilterGroups.LinksetFilter, (uint)CollisionFilterGroups.LinksetMask);
// BulletSimAPI.DumpConstraint2(PhysicsScene.World.ptr, constrain.Constraint.ptr); // DEBUG DEBUG
}
}
// Call each of the constraints that make up this linkset and recompute the
// various transforms and variables. Create constraints of not created yet.
// Called before the simulation step to make sure the constraint based linkset
// is all initialized.
// Called at taint time!!
private void RecomputeLinksetConstraints()
{
float linksetMass = LinksetMass;
LinksetRoot.UpdatePhysicalMassProperties(linksetMass, true);
DetailLog("{0},BSLinksetConstraint.RecomputeLinksetConstraints,set,rBody={1},linksetMass={2}",
LinksetRoot.LocalID, LinksetRoot.PhysBody.AddrString, linksetMass);
foreach (BSPrimLinkable child in m_children)
{
// A child in the linkset physically shows the mass of the whole linkset.
// This allows Bullet to apply enough force on the child to move the whole linkset.
// (Also do the mass stuff before recomputing the constraint so mass is not zero.)
child.UpdatePhysicalMassProperties(linksetMass, true);
BSConstraint constrain;
if (!PhysicsScene.Constraints.TryGetConstraint(LinksetRoot.PhysBody, child.PhysBody, out constrain))
{
// If constraint doesn't exist yet, create it.
constrain = BuildConstraint(LinksetRoot, child);
}
constrain.RecomputeConstraintVariables(linksetMass);
// PhysicsScene.PE.DumpConstraint(PhysicsScene.World, constrain.Constraint); // DEBUG DEBUG
}
}
// 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;
}
// Call each of the constraints that make up this linkset and recompute the
// various transforms and variables. Create constraints of not created yet.
// Called before the simulation step to make sure the constraint based linkset
// is all initialized.
// Called at taint time!!
void RecomputeLinksetConstraints()
{
float linksetMass = LinksetMass;
LinksetRoot.UpdatePhysicalMassProperties(linksetMass, true);
DetailLog("{0},BSLinksetConstraint.RecomputeLinksetConstraints,set,rBody={1},linksetMass={2}",
LinksetRoot.LocalID, LinksetRoot.PhysBody.AddrString, linksetMass);
try
{
Rebuilding = true;
// There is no reason to build all this physical stuff for a non-physical linkset.
if (!LinksetRoot.IsPhysicallyActive || !HasAnyChildren)
{
DetailLog("{0},BSLinksetConstraint.RecomputeLinksetCompound,notPhysicalOrNoChildren",
LinksetRoot.LocalID);
return; // Note the 'finally' clause at the botton which will get executed.
}
ForEachLinkInfo((li) =>
{
// A child in the linkset physically shows the mass of the whole linkset.
// This allows Bullet to apply enough force on the child to move the whole linkset.
// (Also do the mass stuff before recomputing the constraint so mass is not zero.)
li.member.UpdatePhysicalMassProperties(linksetMass, true);
BSConstraint constrain;
if (
!PhysicsScene.Constraints.TryGetConstraint(LinksetRoot.PhysBody, li.member.PhysBody,
out constrain))
{
// If constraint doesn't exist yet, create it.
constrain = BuildConstraint(LinksetRoot, li);
}
li.SetLinkParameters(constrain);
constrain.RecomputeConstraintVariables(linksetMass);
// PhysicScene.PE.DumpConstraint(PhysicsScene.World, constrain.Constraint);
return(false); // 'false' says to keep processing other members
});
}
finally
{
Rebuilding = false;
}
}
private void RecomputeLinksetCompound()
{
try
{
// Suppress rebuilding while rebuilding. (We know rebuilding is on only one thread.)
Rebuilding = true;
if (LinksetRoot.IsPhysical)
{
// Cause the root shape to be rebuilt as a compound object with just the root in it
LinksetRoot.ForceBodyShapeRebuild(true /* inTaintTime */);
// The center of mass for the linkset is the geometric center of the group.
// Compute a displacement for each component so it is relative to the center-of-mass.
// Bullet presumes an object's origin (relative <0,0,0>) is its center-of-mass
OMV.Vector3 centerOfMassW = LinksetRoot.RawPosition;
if (!disableCOM)
{
// Compute a center-of-mass in world coordinates.
centerOfMassW = ComputeLinksetCenterOfMass();
}
OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(LinksetRoot.RawOrientation);
// 'centerDisplacement' is the value to subtract from children to give physical offset position
OMV.Vector3 centerDisplacement = (centerOfMassW - LinksetRoot.RawPosition) * invRootOrientation;
//Set the COM in bullet
PhysicsScene.PE.SetCenterOfMassByPosRot(LinksetRoot.PhysBody, centerDisplacement,
OMV.Quaternion.Identity);
// This causes the physical position of the root prim to be offset to accomodate for the displacements
LinksetRoot.ForcePosition = LinksetRoot.RawPosition;
// Update the local transform for the root child shape so it is offset from the <0,0,0> which is COM
//Not necessary, as the COM is set in bullet now, and this was always Vector3.Zero beforehand, which is of no use
//PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, 0 /* childIndex */,
// -centerDisplacement,
// OMV.Quaternion.Identity, // LinksetRoot.RawOrientation,
// false /* shouldRecalculateLocalAabb (is done later after linkset built) */);
LinksetRoot.LinksetChildIndex = 0;
DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,COM,com={1},rootPos={2},centerDisp={3}",
LinksetRoot.LocalID, centerOfMassW, LinksetRoot.RawPosition, centerDisplacement);
DetailLog(
"{0},BSLinksetCompound.RecomputeLinksetCompound,start,rBody={1},rShape={2},numChildren={3}",
LinksetRoot.LocalID, LinksetRoot.PhysBody, LinksetRoot.PhysShape, NumberOfChildren);
// Add a shape for each of the other children in the linkset
int memberIndex = 1;
lock (m_linksetActivityLock)
{
foreach (BSPrimLinkable cPrim in m_children)
{
cPrim.LinksetChildIndex = memberIndex;
if (cPrim.PhysBody.AddrString == "unknown")
{
if (!PhysicsScene.Shapes.GetBodyAndShape(true, PhysicsScene.World, cPrim))
{
}
}
if (cPrim.PhysShape.isNativeShape)
{
// A native shape is turned into a hull collision shape because native
// shapes are not shared so we have to hullify it so it will be tracked
// and freed at the correct time. This also solves the scaling problem
// (native shapes scale but hull/meshes are assumed to not be).
// TODO: decide of the native shape can just be used in the compound shape.
// Use call to CreateGeomNonSpecial().
BulletShape saveShape = cPrim.PhysShape;
cPrim.PhysShape.Clear(); // Don't let the create free the child's shape
PhysicsScene.Shapes.CreateGeomMeshOrHull(cPrim, null);
BulletShape newShape = cPrim.PhysShape;
cPrim.PhysShape = saveShape;
OMV.Vector3 offsetPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation;
OMV.Quaternion offsetRot = cPrim.RawOrientation * invRootOrientation;
PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, newShape, offsetPos,
offsetRot);
DetailLog(
"{0},BSLinksetCompound.RecomputeLinksetCompound,addNative,indx={1},rShape={2},cShape={3},offPos={4},offRot={5}",
LinksetRoot.LocalID, memberIndex, LinksetRoot.PhysShape, newShape, offsetPos,
offsetRot);
}
else
{
// For the shared shapes (meshes and hulls), just use the shape in the child.
// The reference count added here will be decremented when the compound shape
// is destroyed in BSShapeCollection (the child shapes are looped over and dereferenced).
if (PhysicsScene.Shapes.ReferenceShape(cPrim.PhysShape))
{
PhysicsScene.Logger.ErrorFormat(
"{0} Rebuilt sharable shape when building linkset! Region={1}, primID={2}, shape={3}",
LogHeader, PhysicsScene.RegionName, cPrim.LocalID, cPrim.PhysShape);
}
OMV.Vector3 offsetPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation;
OMV.Quaternion offsetRot = cPrim.RawOrientation * invRootOrientation;
PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, cPrim.PhysShape,
offsetPos, offsetRot);
DetailLog(
"{0},BSLinksetCompound.RecomputeLinksetCompound,addNonNative,indx={1},rShape={2},cShape={3},offPos={4},offRot={5}",
LinksetRoot.LocalID, memberIndex, LinksetRoot.PhysShape, cPrim.PhysShape, offsetPos,
offsetRot);
}
PhysicsScene.PE.AddToCollisionFlags(cPrim.PhysBody, CollisionFlags.CF_NO_CONTACT_RESPONSE);
PhysicsScene.PE.ForceActivationState(cPrim.PhysBody, ActivationState.DISABLE_SIMULATION);
// We don't want collisions from the old linkset children.
PhysicsScene.PE.RemoveFromCollisionFlags(cPrim.PhysBody,
CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
cPrim.PhysBody.collisionType = CollisionType.LinksetChild;
memberIndex++;
}
}
// With all of the linkset packed into the root prim, it has the mass of everyone.
LinksetMass = ComputeLinksetMass();
LinksetRoot.UpdatePhysicalMassProperties(LinksetMass, true);
// Enable the physical position updator to return the position and rotation of the root shape
PhysicsScene.PE.AddToCollisionFlags(LinksetRoot.PhysBody,
CollisionFlags.BS_RETURN_ROOT_COMPOUND_SHAPE);
// See that the Aabb surrounds the new shape
PhysicsScene.PE.RecalculateCompoundShapeLocalAabb(LinksetRoot.PhysShape);
}
else
{
// Cause the root shape to be rebuilt as a compound object with just the root in it
LinksetRoot.ForceBodyShapeRebuild(true /* inTaintTime */);
//Update the AABB so that things will collide properly
PhysicsScene.PE.UpdateSingleAabb(PhysicsScene.World, LinksetRoot.PhysBody);
// 20131224 not used OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(LinksetRoot.RawOrientation);
// This causes the physical position of the root prim to be offset to accomodate for the displacements
LinksetRoot.ForcePosition = LinksetRoot.RawPosition;
int memberIndex = 0;
LinksetRoot.LinksetChildIndex = memberIndex++;
lock (m_linksetActivityLock)
{
foreach (BSPrimLinkable cPrim in m_children)
{
// 20131224 not used OMV.Vector3 offsetPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation;
// 20131224 not used OMV.Quaternion offsetRot = cPrim.RawOrientation * invRootOrientation;
// Build a simple shape for this child prim (given that we aren't allowing it to be dynamic
cPrim.ForceBodyShapeRebuild(true);
// This causes the physical position of the root prim to be offset to accomodate for the displacements
cPrim.ForcePosition = cPrim.RawPosition;
cPrim.LinksetChildIndex = memberIndex++;
//Update AABB for collisions
PhysicsScene.PE.UpdateSingleAabb(PhysicsScene.World, cPrim.PhysBody);
}
}
}
}
finally
{
Rebuilding = false;
}
}
private bool disableCOM = true; // DEBUG DEBUG: disable until we get this debugged
private void RecomputeLinksetCompound()
{
try
{
// Suppress rebuilding while rebuilding. (We know rebuilding is on only one thread.)
Rebuilding = true;
// Cause the root shape to be rebuilt as a compound object with just the root in it
LinksetRoot.ForceBodyShapeRebuild(true /* inTaintTime */);
// The center of mass for the linkset is the geometric center of the group.
// Compute a displacement for each component so it is relative to the center-of-mass.
// Bullet presumes an object's origin (relative <0,0,0>) is its center-of-mass
OMV.Vector3 centerOfMassW = LinksetRoot.RawPosition;
if (!disableCOM) // DEBUG DEBUG
{
// Compute a center-of-mass in world coordinates.
centerOfMassW = ComputeLinksetCenterOfMass();
}
OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(LinksetRoot.RawOrientation);
// 'centerDisplacement' is the value to subtract from children to give physical offset position
OMV.Vector3 centerDisplacement = (centerOfMassW - LinksetRoot.RawPosition) * invRootOrientation;
LinksetRoot.SetEffectiveCenterOfMassW(centerDisplacement);
// This causes the physical position of the root prim to be offset to accomodate for the displacements
LinksetRoot.ForcePosition = LinksetRoot.RawPosition;
// Update the local transform for the root child shape so it is offset from the <0,0,0> which is COM
PhysicsScene.PE.UpdateChildTransform(LinksetRoot.PhysShape, 0 /* childIndex */,
-centerDisplacement,
OMV.Quaternion.Identity, // LinksetRoot.RawOrientation,
false /* shouldRecalculateLocalAabb (is done later after linkset built) */);
DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,COM,com={1},rootPos={2},centerDisp={3}",
LinksetRoot.LocalID, centerOfMassW, LinksetRoot.RawPosition, centerDisplacement);
DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,start,rBody={1},rShape={2},numChildren={3}",
LinksetRoot.LocalID, LinksetRoot.PhysBody, LinksetRoot.PhysShape, NumberOfChildren);
// Add a shape for each of the other children in the linkset
int memberIndex = 1;
ForEachMember(delegate(BSPrimLinkable cPrim)
{
if (IsRoot(cPrim))
{
cPrim.LinksetChildIndex = 0;
}
else
{
cPrim.LinksetChildIndex = memberIndex;
if (cPrim.PhysShape.isNativeShape)
{
// A native shape is turned into a hull collision shape because native
// shapes are not shared so we have to hullify it so it will be tracked
// and freed at the correct time. This also solves the scaling problem
// (native shapes scale but hull/meshes are assumed to not be).
// TODO: decide of the native shape can just be used in the compound shape.
// Use call to CreateGeomNonSpecial().
BulletShape saveShape = cPrim.PhysShape;
cPrim.PhysShape.Clear(); // Don't let the create free the child's shape
PhysicsScene.Shapes.CreateGeomMeshOrHull(cPrim, null);
BulletShape newShape = cPrim.PhysShape;
cPrim.PhysShape = saveShape;
OMV.Vector3 offsetPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation - centerDisplacement;
OMV.Quaternion offsetRot = cPrim.RawOrientation * invRootOrientation;
PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, newShape, offsetPos, offsetRot);
DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addNative,indx={1},rShape={2},cShape={3},offPos={4},offRot={5}",
LinksetRoot.LocalID, memberIndex, LinksetRoot.PhysShape, newShape, offsetPos, offsetRot);
}
else
{
// For the shared shapes (meshes and hulls), just use the shape in the child.
// The reference count added here will be decremented when the compound shape
// is destroyed in BSShapeCollection (the child shapes are looped over and dereferenced).
if (PhysicsScene.Shapes.ReferenceShape(cPrim.PhysShape))
{
PhysicsScene.Logger.ErrorFormat("{0} Rebuilt sharable shape when building linkset! Region={1}, primID={2}, shape={3}",
LogHeader, PhysicsScene.RegionName, cPrim.LocalID, cPrim.PhysShape);
}
OMV.Vector3 offsetPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation - centerDisplacement;
OMV.Quaternion offsetRot = cPrim.RawOrientation * invRootOrientation;
PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, cPrim.PhysShape, offsetPos, offsetRot);
DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addNonNative,indx={1},rShape={2},cShape={3},offPos={4},offRot={5}",
LinksetRoot.LocalID, memberIndex, LinksetRoot.PhysShape, cPrim.PhysShape, offsetPos, offsetRot);
}
memberIndex++;
}
return(false); // 'false' says to move onto the next child in the list
});
// With all of the linkset packed into the root prim, it has the mass of everyone.
LinksetMass = ComputeLinksetMass();
LinksetRoot.UpdatePhysicalMassProperties(LinksetMass, true);
// Enable the physical position updator to return the position and rotation of the root shape
PhysicsScene.PE.AddToCollisionFlags(LinksetRoot.PhysBody, CollisionFlags.BS_RETURN_ROOT_COMPOUND_SHAPE);
}
finally
{
Rebuilding = false;
}
// See that the Aabb surrounds the new shape
PhysicsScene.PE.RecalculateCompoundShapeLocalAabb(LinksetRoot.PhysShape);
}
private bool UseBulletSimRootOffsetHack = false; // Attempt to have Bullet track the coords of root compound shape
private void RecomputeLinksetCompound()
{
try
{
Rebuilding = true;
// No matter what is being done, force the root prim's PhysBody and PhysShape to get set
// to what they should be as if the root was not in a linkset.
// Not that bad since we only get into this routine if there are children in the linkset and
// something has been updated/changed.
// Have to do the rebuild before checking for physical because this might be a linkset
// being destructed and going non-physical.
LinksetRoot.ForceBodyShapeRebuild(true);
// There is no reason to build all this physical stuff for a non-physical linkset.
if (!LinksetRoot.IsPhysicallyActive)
{
DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,notPhysical", LinksetRoot.LocalID);
return; // Note the 'finally' clause at the botton which will get executed.
}
// Get a new compound shape to build the linkset shape in.
BSShape linksetShape = BSShapeCompound.GetReference(m_physicsScene);
// Compute a displacement for each component so it is relative to the center-of-mass.
// Bullet presumes an object's origin (relative <0,0,0>) is its center-of-mass
OMV.Vector3 centerOfMassW = ComputeLinksetCenterOfMass();
OMV.Quaternion invRootOrientation = OMV.Quaternion.Normalize(OMV.Quaternion.Inverse(LinksetRoot.RawOrientation));
OMV.Vector3 origRootPosition = LinksetRoot.RawPosition;
// 'centerDisplacementV' is the vehicle relative distance from the simulator root position to the center-of-mass
OMV.Vector3 centerDisplacementV = (centerOfMassW - LinksetRoot.RawPosition) * invRootOrientation;
if (UseBulletSimRootOffsetHack || !BSParam.LinksetOffsetCenterOfMass)
{
// Zero everything if center-of-mass displacement is not being done.
centerDisplacementV = OMV.Vector3.Zero;
LinksetRoot.ClearDisplacement();
}
else
{
// The actual center-of-mass could have been set by the user.
centerDisplacementV = LinksetRoot.SetEffectiveCenterOfMassDisplacement(centerDisplacementV);
}
DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,COM,rootPos={1},com={2},comDisp={3}",
LinksetRoot.LocalID, origRootPosition, centerOfMassW, centerDisplacementV);
// Add the shapes of all the components of the linkset
int memberIndex = 1;
ForEachMember((cPrim) =>
{
if (IsRoot(cPrim))
{
// Root shape is always index zero.
cPrim.LinksetChildIndex = 0;
}
else
{
cPrim.LinksetChildIndex = memberIndex;
memberIndex++;
}
// Get a reference to the shape of the child for adding of that shape to the linkset compound shape
BSShape childShape = cPrim.PhysShape.GetReference(m_physicsScene, cPrim);
// Offset the child shape from the center-of-mass and rotate it to vehicle relative.
OMV.Vector3 offsetPos = (cPrim.RawPosition - origRootPosition) * invRootOrientation - centerDisplacementV;
OMV.Quaternion offsetRot = OMV.Quaternion.Normalize(cPrim.RawOrientation) * invRootOrientation;
// Add the child shape to the compound shape being built
m_physicsScene.PE.AddChildShapeToCompoundShape(linksetShape.physShapeInfo, childShape.physShapeInfo, offsetPos, offsetRot);
DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addChild,indx={1},cShape={2},offPos={3},offRot={4}",
LinksetRoot.LocalID, cPrim.LinksetChildIndex, childShape, offsetPos, offsetRot);
// Since we are borrowing the shape of the child, disable the origional child body
if (!IsRoot(cPrim))
{
m_physicsScene.PE.AddToCollisionFlags(cPrim.PhysBody, CollisionFlags.CF_NO_CONTACT_RESPONSE);
m_physicsScene.PE.ForceActivationState(cPrim.PhysBody, ActivationState.DISABLE_SIMULATION);
// We don't want collisions from the old linkset children.
m_physicsScene.PE.RemoveFromCollisionFlags(cPrim.PhysBody, CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
cPrim.PhysBody.collisionType = CollisionType.LinksetChild;
}
return(false); // 'false' says to move onto the next child in the list
});
// Replace the root shape with the built compound shape.
// Object removed and added to world to get collision cache rebuilt for new shape.
LinksetRoot.PhysShape.Dereference(m_physicsScene);
LinksetRoot.PhysShape = linksetShape;
m_physicsScene.PE.RemoveObjectFromWorld(m_physicsScene.World, LinksetRoot.PhysBody);
m_physicsScene.PE.SetCollisionShape(m_physicsScene.World, LinksetRoot.PhysBody, linksetShape.physShapeInfo);
m_physicsScene.PE.AddObjectToWorld(m_physicsScene.World, LinksetRoot.PhysBody);
DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addBody,body={1},shape={2}",
LinksetRoot.LocalID, LinksetRoot.PhysBody, linksetShape);
// With all of the linkset packed into the root prim, it has the mass of everyone.
LinksetMass = ComputeLinksetMass();
LinksetRoot.UpdatePhysicalMassProperties(LinksetMass, true);
if (UseBulletSimRootOffsetHack)
{
// Enable the physical position updator to return the position and rotation of the root shape.
// This enables a feature in the C++ code to return the world coordinates of the first shape in the
// compound shape. This aleviates the need to offset the returned physical position by the
// center-of-mass offset.
// TODO: either debug this feature or remove it.
m_physicsScene.PE.AddToCollisionFlags(LinksetRoot.PhysBody, CollisionFlags.BS_RETURN_ROOT_COMPOUND_SHAPE);
}
}
finally
{
Rebuilding = false;
}
// See that the Aabb surrounds the new shape
m_physicsScene.PE.RecalculateCompoundShapeLocalAabb(LinksetRoot.PhysShape.physShapeInfo);
}
// Attempt to have Bullet track the coords of root compound shape
void RecomputeLinksetCompound()
{
try
{
// Suppress rebuilding while rebuilding. (We know rebuilding is on only one thread.)
Rebuilding = true;
// No matter what is being done, force the root prim's PhysBody and PhysShape to get set
// to what they should be as if the root was not in a linkset.
// Not that bad since we only get into this routine if there are children in the linkset and
// something has been updated/changed.
// Have to do the rebuild before checking for physical because this might be a linkset
// being destructed and going non-physical.
LinksetRoot.ForceBodyShapeRebuild(true);
// There is no reason to build all this physical stuff for a non-physical or empty linkset.
if (!LinksetRoot.IsPhysicallyActive || !HasAnyChildren)
{
DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,notPhysicalOrNoChildren",
LinksetRoot.LocalID);
return; // Note the 'finally' clause at the botton which will get executed.
}
// Get a new compound shape to build the linkset shape in.
BSShape linksetShape = BSShapeCompound.GetReference(PhysicsScene);
// Compute a displacement for each component so it is relative to the center-of-mass.
// Bullet presumes an object's origin (relative <0,0,0> is its center-of-mass
OMV.Vector3 centerOfMassW = ComputeLinksetCenterOfMass();
OMV.Quaternion invRootOrientation =
OMV.Quaternion.Normalize(OMV.Quaternion.Inverse(LinksetRoot.RawOrientation));
OMV.Vector3 origRootPosition = LinksetRoot.RawPosition;
// 'centerDisplacementV' is the vehicle relative distance from the simulator root position to the center-of-mass
OMV.Vector3 centerDisplacementV = (centerOfMassW - LinksetRoot.RawPosition) * invRootOrientation;
if (UseBulletSimRootOffsetHack || !BSParam.LinksetOffsetCenterOfMass)
{
// Zero everything if center-of-mass displacement is not being done.
centerDisplacementV = OMV.Vector3.Zero;
LinksetRoot.ClearDisplacement();
}
else
{
// The actual center-of-mass could have been set by the user.
centerDisplacementV = LinksetRoot.SetEffectiveCenterOfMassDisplacement(centerDisplacementV);
}
DetailLog("{0},BSLinksetCompound.RecumputeLinksetCompound,COM,rootPos={1},com={2},comDisp={3}",
LinksetRoot.LocalID, origRootPosition, centerOfMassW, centerDisplacementV);
// Add the shapes of all the components of the linkset
int memberIndex = 1;
ForEachMember((cPrim) =>
{
if (IsRoot(cPrim))
{
// Root shape is always index zero.
cPrim.LinksetChildIndex = 0;
}
else
{
cPrim.LinksetChildIndex = memberIndex;
memberIndex++;
}
// Get a reference to the shape of the child for adding of that shape to the linkset compound shape
BSShape childShape = cPrim.PhysShape.GetReference(PhysicsScene, cPrim);
// Offset the child shape from the center-of-mass and rotate it to root relative.
OMV.Vector3 offsetPos = (cPrim.RawPosition - origRootPosition) * invRootOrientation -
centerDisplacementV;
OMV.Quaternion offsetRot = OMV.Quaternion.Normalize(cPrim.RawOrientation) * invRootOrientation;
// Add the child shape to the compound shape being build
if (childShape.physShapeInfo.HasPhysicalShape)
{
PhysicsScene.PE.AddChildShapeToCompoundShape(linksetShape.physShapeInfo,
childShape.physShapeInfo, offsetPos, offsetRot);
DetailLog(
"{0},BSLinksetCompound.RecomputeLinksetCompound,addChild,indx={1},cShape={2},offPos={3},offRot={4}",
LinksetRoot.LocalID, cPrim.LinksetChildIndex, childShape, offsetPos, offsetRot);
// Since we are borrowing the shape of the child, disable the original child body
if (!IsRoot(cPrim))
{
PhysicsScene.PE.AddToCollisionFlags(cPrim.PhysBody, CollisionFlags.CF_NO_CONTACT_RESPONSE);
PhysicsScene.PE.ForceActivationState(cPrim.PhysBody, ActivationState.DISABLE_SIMULATION);
// We don't want collision from the old linkset children.
PhysicsScene.PE.RemoveFromCollisionFlags(cPrim.PhysBody,
CollisionFlags.BS_SUBSCRIBE_COLLISION_EVENTS);
cPrim.PhysBody.collisionType = CollisionType.LinksetChild;
}
}
else
{
// The linkset must be in an intermediate state where all the children have not yet
// been constructed. This sometimes happens on startup when everything is getting
// built and some shapes have to wait for assets to be read in.
// Just skip this linkset for the moment and cause the shape to be rebuilt next tick.
// One problem might be that the shape is broken somehow and it never becomes completely
// available. This might cause the rebuild to happen over and over.
InternalScheduleRebuild(LinksetRoot);
DetailLog(
"{0},BSLinksetCompound.RecomputeLinksetCompound,addChildWithNoShape,indx={1},cShape={2},offPos={3},offRot={4}",
LinksetRoot.LocalID, cPrim.LinksetChildIndex, childShape, offsetPos, offsetRot);
// Output an annoying warning. It should only happen once but if it keeps coming out,
// the user knows there is something wrong and will report it.
PhysicsScene.Logger.WarnFormat(
"{0} Linkset rebuild warning. If this happens more than one or two times, please report in the issue tracker",
LogHeader);
PhysicsScene.Logger.WarnFormat("{0} pName={1}, childIdx={2}, shape={3}",
LogHeader, LinksetRoot.Name, cPrim.LinksetChildIndex, childShape);
// This causes the loop to bail on building the rest of this linkset.
// The rebuild operation will fix it up next tick or declare the object unbuildable.
return(true);
}
return(false); // 'false' says to move anto the nex child in the list
});
// Replace the root shape with the built compound shape.
// Object removed and added to world to get collision cache rebuilt for new shape.
LinksetRoot.PhysShape.Dereference(PhysicsScene);
LinksetRoot.PhysShape = linksetShape;
PhysicsScene.PE.RemoveObjectFromWorld(PhysicsScene.World, LinksetRoot.PhysBody);
PhysicsScene.PE.SetCollisionShape(PhysicsScene.World, LinksetRoot.PhysBody, linksetShape.physShapeInfo);
PhysicsScene.PE.AddObjectToWorld(PhysicsScene.World, LinksetRoot.PhysBody);
DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addBody,body={1},shape={2}",
LinksetRoot.LocalID, LinksetRoot.PhysBody, linksetShape);
// With all of the linkset packed into the root prim, it has the mass of everyone.
LinksetMass = ComputeLinksetMass();
LinksetRoot.UpdatePhysicalMassProperties(LinksetMass, true);
if (UseBulletSimRootOffsetHack)
{
// Enable the physical position updator to return the position and rotation of the root shape.
// This enables a feature in the C++ code to return the world coordinates of the first shape in the
// compound shape. This aleviates the need to offset the returned physical position by the
// center-of-mass offset.
// TODO: either debug this feature or remove it.
PhysicsScene.PE.AddToCollisionFlags(LinksetRoot.PhysBody,
CollisionFlags.BS_RETURN_ROOT_COMPOUND_SHAPE);
}
}
finally
{
Rebuilding = false;
}
// See that the Aabb surround the new shape
PhysicsScene.PE.RecalculateCompoundShapeLocalAabb(LinksetRoot.PhysShape.physShapeInfo);
}
// Called before the simulation step to make sure the compound based linkset
// is all initialized.
// Constraint linksets are rebuilt every time.
// Note that this works for rebuilding just the root after a linkset is taken apart.
// Called at taint time!!
private void RecomputeLinksetCompound()
{
try
{
// Suppress rebuilding while rebuilding
Rebuilding = true;
// Cause the root shape to be rebuilt as a compound object with just the root in it
LinksetRoot.ForceBodyShapeRebuild(true);
DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,start,rBody={1},rShape={2},numChildren={3}",
LinksetRoot.LocalID, LinksetRoot.PhysBody, LinksetRoot.PhysShape, NumberOfChildren);
// Add a shape for each of the other children in the linkset
ForEachMember(delegate(BSPhysObject cPrim)
{
if (!IsRoot(cPrim))
{
// Compute the displacement of the child from the root of the linkset.
// This info is saved in the child prim so the relationship does not
// change over time and the new child position can be computed
// when the linkset is being disassembled (the linkset may have moved).
BSLinksetCompoundInfo lci = cPrim.LinksetInfo as BSLinksetCompoundInfo;
if (lci == null)
{
// Each child position and rotation is given relative to the root.
OMV.Quaternion invRootOrientation = OMV.Quaternion.Inverse(LinksetRoot.RawOrientation);
OMV.Vector3 displacementPos = (cPrim.RawPosition - LinksetRoot.RawPosition) * invRootOrientation;
OMV.Quaternion displacementRot = cPrim.RawOrientation * invRootOrientation;
// Save relative position for recomputing child's world position after moving linkset.
lci = new BSLinksetCompoundInfo(displacementPos, displacementRot);
cPrim.LinksetInfo = lci;
DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,creatingRelPos,lci={1}", cPrim.LocalID, lci);
}
DetailLog("{0},BSLinksetCompound.RecomputeLinksetCompound,addMemberToShape,mID={1},mShape={2},dispPos={3},dispRot={4}",
LinksetRoot.LocalID, cPrim.LocalID, cPrim.PhysShape, lci.OffsetPos, lci.OffsetRot);
if (cPrim.PhysShape.isNativeShape)
{
// A native shape is turning into a hull collision shape because native
// shapes are not shared so we have to hullify it so it will be tracked
// and freed at the correct time. This also solves the scaling problem
// (native shapes scaled but hull/meshes are assumed to not be).
// TODO: decide of the native shape can just be used in the compound shape.
// Use call to CreateGeomNonSpecial().
BulletShape saveShape = cPrim.PhysShape;
cPrim.PhysShape.Clear(); // Don't let the create free the child's shape
// PhysicsScene.Shapes.CreateGeomNonSpecial(true, cPrim, null);
PhysicsScene.Shapes.CreateGeomMeshOrHull(cPrim, null);
BulletShape newShape = cPrim.PhysShape;
cPrim.PhysShape = saveShape;
PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, newShape, lci.OffsetPos, lci.OffsetRot);
}
else
{
// For the shared shapes (meshes and hulls), just use the shape in the child.
// The reference count added here will be decremented when the compound shape
// is destroyed in BSShapeCollection (the child shapes are looped over and dereferenced).
if (PhysicsScene.Shapes.ReferenceShape(cPrim.PhysShape))
{
PhysicsScene.Logger.ErrorFormat("{0} Rebuilt sharable shape when building linkset! Region={1}, primID={2}, shape={3}",
LogHeader, PhysicsScene.RegionName, cPrim.LocalID, cPrim.PhysShape);
}
PhysicsScene.PE.AddChildShapeToCompoundShape(LinksetRoot.PhysShape, cPrim.PhysShape, lci.OffsetPos, lci.OffsetRot);
}
}
return(false); // 'false' says to move onto the next child in the list
});
// With all of the linkset packed into the root prim, it has the mass of everyone.
LinksetMass = ComputeLinksetMass();
LinksetRoot.UpdatePhysicalMassProperties(LinksetMass, true);
}
finally
{
Rebuilding = false;
}
PhysicsScene.PE.RecalculateCompoundShapeLocalAabb(LinksetRoot.PhysShape);
}
