void setBall2( DxJoint joint, Info2 info, DVector3 anchor1, DVector3 anchor2, DVector3 axis, double erp1) { // anchor points in global coordinates with respect to body PORs. DVector3 a1 = new DVector3(), a2 = new DVector3(); int s = info.rowskip(); // get vectors normal to the axis. in setBall() axis,q1,q2 is [1 0 0], // [0 1 0] and [0 0 1], which makes everything much easier. DVector3 q1 = new DVector3(), q2 = new DVector3(); dPlaneSpace(axis, q1, q2); // set jacobian // for ( i = 0; i < 3; i++ ) info._J[info.J1lp+i] = axis.get(i); // for ( i = 0; i < 3; i++ ) info._J[info.J1lp+s+i] = q1.get(i); // for ( i = 0; i < 3; i++ ) info._J[info.J1lp+2*s+i] = q2.get(i); axis.wrapSet(info._J, info.J1lp); q1.wrapSet(info._J, info.J1lp + s); q2.wrapSet(info._J, info.J1lp + 2 * s); dMultiply0_331(a1, joint.node[0].body.posr().R(), anchor1); dCalcVectorCross3(info._J, info.J1ap, a1, axis); dCalcVectorCross3(info._J, info.J1ap + s, a1, q1); dCalcVectorCross3(info._J, info.J1ap + 2 * s, a1, q2); if (joint.node[1].body != null) { // for ( i = 0; i < 3; i++ ) info._J[info.J2lp+i] = -axis.v[i]; // for ( i = 0; i < 3; i++ ) info._J[info.J2lp+s+i] = -q1.v[i]; // for ( i = 0; i < 3; i++ ) info._J[info.J2lp+2*s+i] = -q2.v[i]; axis.wrapSub(info._J, info.J2lp); q1.wrapSub(info._J, info.J2lp + s); q2.wrapSub(info._J, info.J2lp + 2 * s); // dMultiply0_331( a2, joint->node[1].body->posr.R, anchor2 ); // dReal *J2a = info->J2a; // dCalcVectorCross3( J2a, a2, axis ); // dNegateVector3( J2a ); // dReal *J2a_plus_s = J2a + s; // dCalcVectorCross3( J2a_plus_s, a2, q1 ); // dNegateVector3( J2a_plus_s ); // dReal *J2a_plus_2s = J2a_plus_s + s; // dCalcVectorCross3( J2a_plus_2s, a2, q2 ); // dNegateVector3( J2a_plus_2s ); dMultiply0_331(a2, joint.node[1].body._posr.R, anchor2); double[] J = info._J; // TZ int J2ap = info.J2ap; // TZ // dReal *J2a = info->J2a; dCalcVectorCross3(J, J2ap, a2, axis); dNegateVector3(J, J2ap); // dReal *J2a_plus_s = J2a + s; dCalcVectorCross3(J, J2ap + s, a2, q1); dNegateVector3(J, J2ap + s); // dReal *J2a_plus_2s = J2a_plus_s + s; dCalcVectorCross3(J, J2ap + 2 * s, a2, q2); dNegateVector3(J, J2ap + 2 * s); } // set right hand side - measure error along (axis,q1,q2) double k1 = info.fps * erp1; double k = info.fps * info.erp; // for ( i = 0; i < 3; i++ ) a1.v[i] += joint.node[0].body._posr.pos.v[i]; a1.add(joint.node[0].body.posr().pos()); if (joint.node[1].body != null) { // for ( i = 0; i < 3; i++ ) a2.v[i] += joint.node[1].body._posr.pos.v[i]; a2.add(joint.node[1].body.posr().pos()); DVector3 a2_minus_a1 = new DVector3(); a2_minus_a1.eqDiff(a2, a1); info.setC(0, k1 * (dCalcVectorDot3(axis, a2_minus_a1))); info.setC(1, k * (dCalcVectorDot3(q1, a2_minus_a1))); info.setC(2, k * (dCalcVectorDot3(q2, a2_minus_a1))); } else { DVector3 anchor2_minus_a1 = new DVector3(); anchor2_minus_a1.eqDiff(anchor2, a1); info.setC(0, k1 * (dCalcVectorDot3(axis, anchor2_minus_a1))); info.setC(1, k * (dCalcVectorDot3(q1, anchor2_minus_a1))); info.setC(2, k * (dCalcVectorDot3(q2, anchor2_minus_a1))); } }