public override BulletConstraint CreateConeTwistConstraint(BulletWorld pWorld, BulletBody pBody1,
BulletBody pBody2,
Vector3 pframe1, Quaternion pframe1rot,
Vector3 pframe2, Quaternion pframe2rot,
bool pdisableCollisionsBetweenLinkedBodies)
{
ConeTwistConstraint constrain = null;
DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
RigidBody rb1 = (pBody1 as BulletBodyXNA).rigidBody;
RigidBody rb2 = (pBody2 as BulletBodyXNA).rigidBody;
if (rb1 != null && rb2 != null)
{
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;
constrain = new ConeTwistConstraint(rb1, rb2, ref frame1, ref frame2);
world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
}
return new BulletConstraintXNA(constrain);
}