void computeInitialRelativeRotation() { if (node[0].body != null) { if (node[1].body != null) { dQMultiply1 (qrel, node[0].body._q, node[1].body._q ); } else { // set qrel to the transpose of the first body q qrel.set0( node[0].body._q.get0() ); qrel.set1( -node[0].body._q.get1() ); qrel.set2( -node[0].body._q.get2() ); qrel.set3( -node[0].body._q.get3() ); } } }
void computeInitialRelativeRotation() { if (node[0].body != null) { if (node[1].body != null) { dQMultiply1 (qrel, node[0].body._q, node[1].body._q ); } else { // set qrel to the transpose of the first body q qrel.set0( node[0].body._q.get0() ); qrel.set1( -node[0].body._q.get1() ); qrel.set2( -node[0].body._q.get2() ); qrel.set3( -node[0].body._q.get3() ); } } }
private static void DQFR_case_0(DQuaternion q, final DMatrix3C R) { //case_0: double s = dSqrt((R.get00() - (R.get11() + R.get22())) + 1.); q.set1( 0.5 * s ); s = 0.5 * dRecip(s); q.set2( (R.get01() + R.get10()) * s ); q.set3( (R.get20() + R.get02()) * s ); q.set0( (R.get21() - R.get12()) * s ); return; }
private static void DQFR_case_2(DQuaternion q, final DMatrix3C R) { //case_2: double s = dSqrt((R.get22() - (R.get00() + R.get11())) + 1.); q.set3( 0.5 * s ); s = 0.5 * dRecip(s); q.set1( (R.get20() + R.get02()) * s ); q.set2( (R.get12() + R.get21()) * s ); q.set0( (R.get10() - R.get01()) * s ); return; }
private static void DQFR_case_0(DQuaternion q, final DMatrix3C R) { //case_0: double s = dSqrt((R.get00() - (R.get11() + R.get22())) + 1.); q.set1( 0.5 * s ); s = 0.5 * dRecip(s); q.set2( (R.get01() + R.get10()) * s ); q.set3( (R.get20() + R.get02()) * s ); q.set0( (R.get21() - R.get12()) * s ); return; }
private static void DQFR_case_1(DQuaternion q, final DMatrix3C R) { //case_1: double s = dSqrt((R.get11() - (R.get22() + R.get00())) + 1.); q.set2( 0.5 * s ); s = 0.5 * dRecip(s); q.set3( (R.get12() + R.get21()) * s ); q.set1( (R.get01() + R.get10()) * s ); q.set0( (R.get02() - R.get20()) * s ); return; }
private static void DQFR_case_1(DQuaternion q, final DMatrix3C R) { //case_1: double s = dSqrt((R.get11() - (R.get22() + R.get00())) + 1.); q.set2( 0.5 * s ); s = 0.5 * dRecip(s); q.set3( (R.get12() + R.get21()) * s ); q.set1( (R.get01() + R.get10()) * s ); q.set0( (R.get02() - R.get20()) * s ); return; }
private static void DQFR_case_2(DQuaternion q, final DMatrix3C R) { //case_2: double s = dSqrt((R.get22() - (R.get00() + R.get11())) + 1.); q.set3( 0.5 * s ); s = 0.5 * dRecip(s); q.set1( (R.get20() + R.get02()) * s ); q.set2( (R.get12() + R.get21()) * s ); q.set0( (R.get10() - R.get01()) * s ); return; }
static void dQuatInv(final DQuaternion source, DQuaternion dest) { double norm = source.get0()*source.get0() + source.get1()*source.get1() + source.get2()*source.get2() + source.get3()*source.get3(); if (norm > 0.0f) { double invNorm = 1.0/norm; dest.set0( source.get0() * invNorm ); dest.set1( -source.get1() * invNorm ); dest.set2( -source.get2() * invNorm ); dest.set3( -source.get3() * invNorm); } else { // Singular -> return identity // dest.v[0] = (1.0); // dest.v[1] = (0.0); // dest.v[2] = (0.0); // dest.v[3] = (0.0); dest.set( 1, 0, 0, 0 ); } }
static void dQuatInv(final DQuaternion source, DQuaternion dest) { double norm = source.get0()*source.get0() + source.get1()*source.get1() + source.get2()*source.get2() + source.get3()*source.get3(); if (norm > 0.0f) { double invNorm = 1.0/norm; dest.set0( source.get0() * invNorm ); dest.set1( -source.get1() * invNorm ); dest.set2( -source.get2() * invNorm ); dest.set3( -source.get3() * invNorm); } else { // Singular -> return identity // dest.v[0] = (1.0); // dest.v[1] = (0.0); // dest.v[2] = (0.0); // dest.v[3] = (0.0); dest.set( 1, 0, 0, 0 ); } }
void dQuatInvTransform(final DQuaternion quat,final DVector3 source,DVector3 dest) { double norm = quat.get0()*quat.get0() + quat.get1()*quat.get1() + quat.get2()*quat.get2() + quat.get3()*quat.get3(); if (norm > (0.0)) { double invNorm = 1.0/norm; DQuaternion invQuat = new DQuaternion(); invQuat.set0( quat.get0() * invNorm ); invQuat.set1( -quat.get1() * invNorm ); invQuat.set2( -quat.get2() * invNorm ); invQuat.set3( -quat.get3() * invNorm ); dQuatTransform(invQuat,source,dest); } else { // Singular -> return identity dVector3Copy(source,dest); } }
void dQuatInvTransform(final DQuaternion quat,final DVector3 source,DVector3 dest) { double norm = quat.get0()*quat.get0() + quat.get1()*quat.get1() + quat.get2()*quat.get2() + quat.get3()*quat.get3(); if (norm > (0.0)) { double invNorm = 1.0/norm; DQuaternion invQuat = new DQuaternion(); invQuat.set0( quat.get0() * invNorm ); invQuat.set1( -quat.get1() * invNorm ); invQuat.set2( -quat.get2() * invNorm ); invQuat.set3( -quat.get3() * invNorm ); dQuatTransform(invQuat,source,dest); } else { // Singular -> return identity dVector3Copy(source,dest); } }
public static void dQMultiply2 (DQuaternion qa, final DQuaternionC qb, final DQuaternionC qc) { //dAASSERT (qa, qb, qc); qa.set0( qb.get0()*qc.get0() + qb.get1()*qc.get1() + qb.get2()*qc.get2() + qb.get3()*qc.get3() ); qa.set1( -qb.get0()*qc.get1() + qb.get1()*qc.get0() - qb.get2()*qc.get3() + qb.get3()*qc.get2() ); qa.set2( -qb.get0()*qc.get2() + qb.get2()*qc.get0() - qb.get3()*qc.get1() + qb.get1()*qc.get3() ); qa.set3( -qb.get0()*qc.get3() + qb.get3()*qc.get0() - qb.get1()*qc.get2() + qb.get2()*qc.get1() ); }
public static void dQMultiply1 (DQuaternion qa, final DQuaternionC qb, final DQuaternionC qc) { //dAASSERT (qa, qb, qc); qa.set0( qb.get0()*qc.get0() + qb.get1()*qc.get1() + qb.get2()*qc.get2() + qb.get3()*qc.get3() ); qa.set1( qb.get0()*qc.get1() - qb.get1()*qc.get0() - qb.get2()*qc.get3() + qb.get3()*qc.get2() ); qa.set2( qb.get0()*qc.get2() - qb.get2()*qc.get0() - qb.get3()*qc.get1() + qb.get1()*qc.get3() ); qa.set3( qb.get0()*qc.get3() - qb.get3()*qc.get0() - qb.get1()*qc.get2() + qb.get2()*qc.get1() ); }
public static void dQMultiply3 (DQuaternion qa, final DQuaternionC qb, final DQuaternionC qc) { //dAASSERT (qa, qb, qc); qa.set0( qb.get0()*qc.get0() - qb.get1()*qc.get1() - qb.get2()*qc.get2() - qb.get3()*qc.get3() ); qa.set1( -qb.get0()*qc.get1() - qb.get1()*qc.get0() + qb.get2()*qc.get3() - qb.get3()*qc.get2() ); qa.set2( -qb.get0()*qc.get2() - qb.get2()*qc.get0() + qb.get3()*qc.get1() - qb.get1()*qc.get3() ); qa.set3( -qb.get0()*qc.get3() - qb.get3()*qc.get0() + qb.get1()*qc.get2() - qb.get2()*qc.get1() ); }
public static void dQMultiply0 (DQuaternion qa, final DQuaternionC qb, final DQuaternionC qc) { //dAASSERT (qa, qb, qc); qa.set0( qb.get0()*qc.get0() - qb.get1()*qc.get1() - qb.get2()*qc.get2() - qb.get3()*qc.get3() ); qa.set1( qb.get0()*qc.get1() + qb.get1()*qc.get0() + qb.get2()*qc.get3() - qb.get3()*qc.get2() ); qa.set2( qb.get0()*qc.get2() + qb.get2()*qc.get0() + qb.get3()*qc.get1() - qb.get1()*qc.get3() ); qa.set3( qb.get0()*qc.get3() + qb.get3()*qc.get0() + qb.get1()*qc.get2() - qb.get2()*qc.get1() ); }
public static void dQMultiply0 (DQuaternion qa, final DQuaternionC qb, final DQuaternionC qc) { //dAASSERT (qa, qb, qc); qa.set0( qb.get0()*qc.get0() - qb.get1()*qc.get1() - qb.get2()*qc.get2() - qb.get3()*qc.get3() ); qa.set1( qb.get0()*qc.get1() + qb.get1()*qc.get0() + qb.get2()*qc.get3() - qb.get3()*qc.get2() ); qa.set2( qb.get0()*qc.get2() + qb.get2()*qc.get0() + qb.get3()*qc.get1() - qb.get1()*qc.get3() ); qa.set3( qb.get0()*qc.get3() + qb.get3()*qc.get0() + qb.get1()*qc.get2() - qb.get2()*qc.get1() ); }
public static void dQMultiply1 (DQuaternion qa, final DQuaternionC qb, final DQuaternionC qc) { //dAASSERT (qa, qb, qc); qa.set0( qb.get0()*qc.get0() + qb.get1()*qc.get1() + qb.get2()*qc.get2() + qb.get3()*qc.get3() ); qa.set1( qb.get0()*qc.get1() - qb.get1()*qc.get0() - qb.get2()*qc.get3() + qb.get3()*qc.get2() ); qa.set2( qb.get0()*qc.get2() - qb.get2()*qc.get0() - qb.get3()*qc.get1() + qb.get1()*qc.get3() ); qa.set3( qb.get0()*qc.get3() - qb.get3()*qc.get0() - qb.get1()*qc.get2() + qb.get2()*qc.get1() ); }
public static void dQMultiply2 (DQuaternion qa, final DQuaternionC qb, final DQuaternionC qc) { //dAASSERT (qa, qb, qc); qa.set0( qb.get0()*qc.get0() + qb.get1()*qc.get1() + qb.get2()*qc.get2() + qb.get3()*qc.get3() ); qa.set1( -qb.get0()*qc.get1() + qb.get1()*qc.get0() - qb.get2()*qc.get3() + qb.get3()*qc.get2() ); qa.set2( -qb.get0()*qc.get2() + qb.get2()*qc.get0() - qb.get3()*qc.get1() + qb.get1()*qc.get3() ); qa.set3( -qb.get0()*qc.get3() + qb.get3()*qc.get0() - qb.get1()*qc.get2() + qb.get2()*qc.get1() ); }
public static void dQMultiply3 (DQuaternion qa, final DQuaternionC qb, final DQuaternionC qc) { //dAASSERT (qa, qb, qc); qa.set0( qb.get0()*qc.get0() - qb.get1()*qc.get1() - qb.get2()*qc.get2() - qb.get3()*qc.get3() ); qa.set1( -qb.get0()*qc.get1() - qb.get1()*qc.get0() + qb.get2()*qc.get3() - qb.get3()*qc.get2() ); qa.set2( -qb.get0()*qc.get2() - qb.get2()*qc.get0() + qb.get3()*qc.get1() - qb.get1()*qc.get3() ); qa.set3( -qb.get0()*qc.get3() - qb.get3()*qc.get0() + qb.get1()*qc.get2() - qb.get2()*qc.get1() ); }