// void setAnchors( dxJoint j, double x, double y, double z,
// dVector3 anchor1, dVector3 anchor2 )
final void setAnchors(DVector3C xyz, DVector3 anchor1, DVector3 anchor2) {
if (node[0].body != null) {
/// double[] q = new double[4];
DVector3 q = new DVector3();
// q.v[0] = x - node[0].body._posr.pos.v[0];
// q.v[1] = y - node[0].body._posr.pos.v[1];
// q.v[2] = z - node[0].body._posr.pos.v[2];
// q.v[3] = 0;
q.eqDiff(xyz, node[0].body.posr().pos());
dMultiply1_331(anchor1, node[0].body.posr().R(), q);
if (node[1].body != null) {
// q.v[0] = x - node[1].body._posr.pos.v[0];
// q.v[1] = y - node[1].body._posr.pos.v[1];
// q.v[2] = z - node[1].body._posr.pos.v[2];
// q.v[3] = 0;
q.eqDiff(xyz, node[1].body.posr().pos());
dMultiply1_331(anchor2, node[1].body.posr().R(), q);
} else {
// anchor2.v[0] = x;
// anchor2.v[1] = y;
// anchor2.v[2] = z;
anchor2.set(xyz);
}
}
// anchor1.v[3] = 0;
// anchor2.v[3] = 0;
}
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)));
}
}
