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()));
}
}
