// 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 } }
// 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!! 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); }