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