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