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 normalaized 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(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 * 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
);
// ==================================================================================
*/
break;
case ConstraintType.D6_SPRING_CONSTRAINT_TYPE:
constrain = new BSConstraintSpring(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;
}
if (constrain != null) {
linkInfo.SetLinkParameters (constrain);
PhysicsScene.Constraints.AddConstraint (constrain);
}
return constrain;
}
