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