public void dJointAttach(DBody b1, DBody b2) {
DxBody body1 = (DxBody) b1;
DxBody body2 = (DxBody) b2;
// check arguments
// dUASSERT (joint,"bad joint argument");
dUASSERT(body1 == null || body1 != body2, "can't have body1==body2");
DxWorld world = this.world;
dUASSERT(
(body1 == null || body1.getWorld() == world)
&& (body2 == null || body2.getWorld() == world),
"joint and bodies must be in same world");
// check if the joint can not be attached to just one body
dUASSERT(
!((flags & dJOINT_TWOBODIES) != 0 && ((body1 != null) ^ (body2 != null))),
"joint can not be attached to just one body");
// remove any existing body attachments
if (node[0].body != null || node[1].body != null) {
removeJointReferencesFromAttachedBodies();
}
// if a body is zero, make sure that it is body2, so 0 --> node[1].body
if (body1 == null) {
body1 = body2;
body2 = null;
flags |= dJOINT_REVERSE;
} else {
flags &= (~dJOINT_REVERSE);
}
// attach to new bodies
node[0].body = body1;
node[1].body = body2;
if (body1 != null) {
node[1].next = body1.firstjoint.get();
body1.firstjoint.set(node[1]);
} else node[1].next = null;
if (body2 != null) {
node[0].next = body2.firstjoint.get();
body2.firstjoint.set(node[0]);
} else {
node[0].next = null;
}
// Since the bodies are now set.
// Calculate the values depending on the bodies.
// Only need to calculate relative value if a body exist
if (body1 != null || body2 != null) setRelativeValues();
}
void setBall(DxJoint joint, Info2 info, DVector3 anchor1, DVector3 anchor2) {
// anchor points in global coordinates with respect to body PORs.
DVector3 a1 = new DVector3(), a2 = new DVector3();
int s = info.rowskip();
// set jacobian
info._J[info.J1lp + 0] = 1;
info._J[info.J1lp + s + 1] = 1;
info._J[info.J1lp + 2 * s + 2] = 1;
dMultiply0_331(a1, joint.node[0].body.posr().R(), anchor1);
// dCROSSMAT( info.J1a, a1, s, -, + );
dSetCrossMatrixMinus(info._J, info.J1ap, a1, s);
if (joint.node[1].body != null) {
info._J[info.J2lp + 0] = -1;
info._J[info.J2lp + s + 1] = -1;
info._J[info.J2lp + 2 * s + 2] = -1;
dMultiply0_331(a2, joint.node[1].body.posr().R(), anchor2);
dSetCrossMatrixPlus(info._J, info.J2ap, a2, s);
}
DxBody b0 = joint.node[0].body;
DxBody b1 = joint.node[1].body;
// set right hand side
double k = info.fps * info.erp;
if (joint.node[1].body != null) {
// for ( int j = 0; j < 3; j++ )
// {
// info.setC(j, k * ( a2.v[j] + b1.posr().pos().v[j] -
// a1.v[j] - b0._posr.pos.v[j] ));
// }
info.setC(0, k * (a2.get0() + b1.posr().pos().get0() - a1.get0() - b0.posr().pos().get0()));
info.setC(1, k * (a2.get1() + b1.posr().pos().get1() - a1.get1() - b0.posr().pos().get1()));
info.setC(2, k * (a2.get2() + b1.posr().pos().get2() - a1.get2() - b0.posr().pos().get2()));
} else {
// for ( int j = 0; j < 3; j++ )
// {
// info.setC(j, k * ( anchor2.v[j] - a1.v[j] -
// b0._posr.pos.v[j] ));
// }
info.setC(0, k * (anchor2.get0() - a1.get0() - b0.posr().pos().get0()));
info.setC(1, k * (anchor2.get1() - a1.get1() - b0.posr().pos().get1()));
info.setC(2, k * (anchor2.get2() - a1.get2() - b0.posr().pos().get2()));
}
}