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 ); } }
public static void dQSetIdentity (DQuaternion q) { q.setIdentity(); }
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 } } }
public DQuaternion(DQuaternion x) { this(); set(x); }
public void setAutoDisableAverageSamplesCount(int samples) { if (samples != autoDisableAverageSamples) { autoDisableAverageSamples = samples; for (DxRagdollBody bone : bones) { bone.pBuffer = new DVector3[samples]; bone.qBuffer = new DQuaternion[samples]; for (int i = 0; i < samples; i++) { bone.pBuffer[i] = new DVector3(); bone.qBuffer[i] = new DQuaternion(); } } } autoDisableBufferIndex = 0; autoDisableBufferReady = false; }
DBody body = bone.body; DVector3 avgPos = new DVector3(); DQuaternion avgQuat = new DQuaternion(); for (int i = 0; i < autoDisableAverageSamples; i++) { DVector3C prevPos = bone.pBuffer[i]; DQuaternion prevQuat = bone.qBuffer[i]; avgPos.add(prevPos); avgQuat.add(prevQuat); avgQuat.safeNormalize4(); avgPos = avgPos.sub(body.getPosition()); double ldiff = avgPos.dot(avgPos); DQuaternion adiff = new DQuaternion(); dQMultiply1(adiff, avgQuat, body.getQuaternion()); if (ldiff > linearThreashold || adiff.get0() < angularThreashold) { allIdle = false; break; for (DxRagdollBody bone : bones) { bone.pBuffer[autoDisableBufferIndex].set(bone.body.getPosition()); bone.qBuffer[autoDisableBufferIndex].set(bone.body.getQuaternion()); if (autoDisabled) { bone.body.disable(); } else { bone.position.set(bone.body.getPosition()); bone.quaternion.set(bone.body.getQuaternion());
public void dJointSetHingeAxisOffset( double x, double y, double z, double dangle ) { setAxes( x, y, z, _axis1, _axis2 ); computeInitialRelativeRotation(); if ( isFlagsReverse() ) dangle = -dangle; DQuaternion qAngle = new DQuaternion(), qOffset = new DQuaternion(); dQFromAxisAndAngle(qAngle, x, y, z, dangle); dQMultiply3(qOffset, qAngle, qrel); // qrel.v[0] = qOffset.v[0]; // qrel.v[1] = qOffset.v[1]; // qrel.v[2] = qOffset.v[2]; // qrel.v[3] = qOffset.v[3]; qrel.set(qOffset); }
private void reset_ball() { float sx=0.0f, sy=3.40f, sz=7.15f; // //#if defined(_MSC_VER) && defined(dDOUBLE) // //sy -= 0.01; // Cheat, to make it score under win32/double // //#endif sy += 0.033; // Windows 64 //TODO //sy += 0.046; //For 'double' on Linux 64bit. //TODO !!! DQuaternion q = new DQuaternion(); q.setIdentity(); sphbody.setPosition (sx, sy, sz); sphbody.setQuaternion(q); sphbody.setLinearVel (0,0,0); sphbody.setAngularVel (0,0,0); }
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.set(0, node[0].body._q.get(0) ); 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 } } }
void dBodyCopyQuaternion (DxBody b, DQuaternion quat) { quat.set(b._q); }
public void setAutoDisableAverageSamplesCount(int samples) { if (samples != autoDisableAverageSamples) { autoDisableAverageSamples = samples; for (DxRagdollBody bone : bones) { bone.pBuffer = new DVector3[samples]; bone.qBuffer = new DQuaternion[samples]; for (int i = 0; i < samples; i++) { bone.pBuffer[i] = new DVector3(); bone.qBuffer[i] = new DQuaternion(); } } } autoDisableBufferIndex = 0; autoDisableBufferReady = false; }
DBody body = bone.body; DVector3 avgPos = new DVector3(); DQuaternion avgQuat = new DQuaternion(); for (int i = 0; i < autoDisableAverageSamples; i++) { DVector3C prevPos = bone.pBuffer[i]; DQuaternion prevQuat = bone.qBuffer[i]; avgPos.add(prevPos); avgQuat.add(prevQuat); avgQuat.safeNormalize4(); avgPos = avgPos.sub(body.getPosition()); double ldiff = avgPos.dot(avgPos); DQuaternion adiff = new DQuaternion(); dQMultiply1(adiff, avgQuat, body.getQuaternion()); if (ldiff > linearThreashold || adiff.get0() < angularThreashold) { allIdle = false; break; for (DxRagdollBody bone : bones) { bone.pBuffer[autoDisableBufferIndex].set(bone.body.getPosition()); bone.qBuffer[autoDisableBufferIndex].set(bone.body.getQuaternion()); if (autoDisabled) { bone.body.disable(); } else { bone.position.set(bone.body.getPosition()); bone.quaternion.set(bone.body.getQuaternion());
public void dJointSetHingeAxisOffset( double x, double y, double z, double dangle ) { setAxes( x, y, z, _axis1, _axis2 ); computeInitialRelativeRotation(); if ( isFlagsReverse() ) dangle = -dangle; DQuaternion qAngle = new DQuaternion(), qOffset = new DQuaternion(); dQFromAxisAndAngle(qAngle, x, y, z, dangle); dQMultiply3(qOffset, qAngle, qrel); // qrel.v[0] = qOffset.v[0]; // qrel.v[1] = qOffset.v[1]; // qrel.v[2] = qOffset.v[2]; // qrel.v[3] = qOffset.v[3]; qrel.set(qOffset); }
private void reset_ball() { float sx=0.0f, sy=2.973f, sz=7.51f; // //#if defined(_MSC_VER) && defined(dDOUBLE) // //sy -= 0.01; // Cheat, to make it score under win32/double // //#endif sy += 0.033; // Windows 64 //TODO //sy += 0.046; //For 'double' on Linux 64bit. //TODO !!! DQuaternion q = new DQuaternion(); q.setIdentity(); sphbody.setPosition (sx, sy, sz); sphbody.setQuaternion(q); sphbody.setLinearVel (0,0,0); sphbody.setAngularVel (0,0,0); }
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.set(0, node[0].body._q.get(0) ); 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 ); } }