/**
* set three orientation rows in the constraint equation, and the corresponding right hand side.
*
* @param joint
* @param info
* @param qrel
* @param start_row
*/
void setFixedOrientation(DxJoint joint, Info2 info, DQuaternion qrel, int start_row) {
int s = info.rowskip();
int start_index = start_row * s;
// 3 rows to make body rotations equal
info._J[info.J1ap + start_index] = 1;
info._J[info.J1ap + start_index + s + 1] = 1;
info._J[info.J1ap + start_index + s * 2 + 2] = 1;
if (joint.node[1].body != null) {
info._J[info.J2ap + start_index] = -1;
info._J[info.J2ap + start_index + s + 1] = -1;
info._J[info.J2ap + start_index + s * 2 + 2] = -1;
}
// compute the right hand side. the first three elements will result in
// relative angular velocity of the two bodies - this is set to bring them
// back into alignment. the correcting angular velocity is
// |angular_velocity| = angle/time = erp*theta / stepsize
// = (erp*fps) * theta
// angular_velocity = |angular_velocity| * u
// = (erp*fps) * theta * u
// where rotation along unit length axis u by theta brings body 2's frame
// to qrel with respect to body 1's frame. using a small angle approximation
// for sin(), this gives
// angular_velocity = (erp*fps) * 2 * v
// where the quaternion of the relative rotation between the two bodies is
// q = [cos(theta/2) sin(theta/2)*u] = [s v]
// get qerr = relative rotation (rotation error) between two bodies
DQuaternion qerr = new DQuaternion();
DVector3 e = new DVector3();
if (joint.node[1].body != null) {
DQuaternion qq = new DQuaternion();
dQMultiply1(qq, joint.node[0].body._q, joint.node[1].body._q);
dQMultiply2(qerr, qq, qrel);
} else {
dQMultiply3(qerr, joint.node[0].body._q, qrel);
}
if (qerr.get0() < 0) {
qerr.set1(-qerr.get1()); // adjust sign of qerr to make theta small
qerr.set2(-qerr.get2());
qerr.set3(-qerr.get3());
}
// TZ:
// dMULTIPLY0_331( e, joint.node[0].body.posr.R, qerr + 1 ); // @@@ bad SIMD padding!
DVector3 qerr2 = new DVector3();
qerr2.set0(qerr.get1());
qerr2.set1(qerr.get2());
qerr2.set2(qerr.get3());
dMultiply0_331(e, joint.node[0].body.posr().R(), qerr2); // @@@ bad SIMD padding!
double k = info.fps * info.erp;
info.setC(start_row, 2 * k * e.get0());
info.setC(start_row + 1, 2 * k * e.get1());
info.setC(start_row + 2, 2 * k * e.get2());
}
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()));
}
}
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)));
}
}
