@Override public String toString() { StringBuffer b = new StringBuffer(); b.append("DQuaternion[ "); b.append(get0()).append(", "); b.append(get1()).append(", "); b.append(get2()).append(", "); b.append(get3()).append(" ]"); return b.toString(); }
@Override public String toString() { StringBuffer b = new StringBuffer(); b.append("DQuaternion[ "); b.append(get0()).append(", "); b.append(get1()).append(", "); b.append(get2()).append(", "); b.append(get3()).append(" ]"); return b.toString(); }
public DQuaternion add(DQuaternion q) { v[0] += q.get0(); v[1] += q.get1(); v[2] +=q.get2(); v[3] += q.get3(); return this; }
public DQuaternion add(DQuaternion q) { v[0] += q.get0(); v[1] += q.get1(); v[2] +=q.get2(); v[3] += q.get3(); return this; }
private void computeInitialRelativeRotation() { if ( node[0].body != null ) { if ( node[1].body != null ) { dQMultiply1 ( qrel, node[0].body._q, node[1].body._q ); } else { // set joint->qrel to the transpose of the first body q qrel.set0( node[0].body._q.get0() ); for ( int i = 1; i < 4; i++ ) qrel.set(i, -node[0].body._q.get(i) ); // WARNING do we need the - in -joint->node[0].body->q[i]; or not } } }
private void computeInitialRelativeRotation() { if ( node[0].body != null ) { if ( node[1].body != null ) { dQMultiply1 ( qrel, node[0].body._q, node[1].body._q ); } else { // set joint->qrel to the transpose of the first body q qrel.set0( node[0].body._q.get0() ); for ( int i = 1; i < 4; i++ ) qrel.set(i, -node[0].body._q.get(i) ); // WARNING do we need the - in -joint->node[0].body->q[i]; or not } } }
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); } }
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.v[0] = node[0].body._q.v[0]; // qrel.v[1] = -node[0].body._q.v[1]; // qrel.v[2] = -node[0].body._q.v[2]; // qrel.v[3] = -node[0].body._q.v[3]; qrel.set( node[0].body._q.get0(), -node[0].body._q.get1(), -node[0].body._q.get2(), -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.v[0] = node[0].body._q.v[0]; // qrel.v[1] = -node[0].body._q.v[1]; // qrel.v[2] = -node[0].body._q.v[2]; // qrel.v[3] = -node[0].body._q.v[3]; qrel.set( node[0].body._q.get0(), -node[0].body._q.get1(), -node[0].body._q.get2(), -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() ); } } }
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() ); } } }