public override BulletConstraint Create6DofConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot,
bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
{
DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
RigidBody body1 = (pBody1 as BulletBodyXNA).rigidBody;
RigidBody body2 = (pBody2 as BulletBodyXNA).rigidBody;
IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
frame1._origin = frame1v;
// 20131224 not used IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
frame2._origin = frame1v;
Generic6DofConstraint consttr = new Generic6DofConstraint(body1, body2, ref frame1, ref frame2,
puseLinearReferenceFrameA);
consttr.CalculateTransforms();
world.AddConstraint(consttr, pdisableCollisionsBetweenLinkedBodies);
return new BulletConstraintXNA(consttr);
}