Universe.Physics.BulletSPlugin.BSAPIXNA.Create6DofConstraintToPoint C# (CSharp) Method

        public override BulletConstraint Create6DofConstraintToPoint(BulletWorld pWorld, BulletBody pBody1,
            BulletBody pBody2, Vector3 pjoinPoint, bool puseLinearReferenceFrameA,
            bool pdisableCollisionsBetweenLinkedBodies)
        {
            DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
            RigidBody body1 = (pBody1 as BulletBodyXNA).rigidBody;
            RigidBody body2 = (pBody2 as BulletBodyXNA).rigidBody;
            IndexedMatrix frame1 = new IndexedMatrix(IndexedBasisMatrix.Identity, new IndexedVector3(0, 0, 0));
            IndexedMatrix frame2 = new IndexedMatrix(IndexedBasisMatrix.Identity, new IndexedVector3(0, 0, 0));

            IndexedVector3 joinPoint = new IndexedVector3(pjoinPoint.X, pjoinPoint.Y, pjoinPoint.Z);
            IndexedMatrix mat = IndexedMatrix.Identity;
            mat._origin = new IndexedVector3(pjoinPoint.X, pjoinPoint.Y, pjoinPoint.Z);
            frame1._origin = body1.GetWorldTransform().Inverse() * joinPoint;
            frame2._origin = body2.GetWorldTransform().Inverse() * joinPoint;

            Generic6DofConstraint consttr = new Generic6DofConstraint(body1, body2, ref frame1, ref frame2,
                puseLinearReferenceFrameA);
            consttr.CalculateTransforms();
            world.AddConstraint(consttr, pdisableCollisionsBetweenLinkedBodies);

            return new BulletConstraintXNA(consttr);
        }