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 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 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() ); }
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() ); }
/** * return the maximum element difference between the two 4*1 matrices. * @param A A * @param B B * @param n n * @param m m * @return difference */ public static double dMaxDifference (final DQuaternionC A, final DQuaternionC B, int n, int m) { double max = Math.abs(A.get0() - B.get0()); double diff = Math.abs(A.get1() - B.get1()); if (diff > max) max = diff; diff = Math.abs(A.get2() - B.get2()); if (diff > max) max = diff; diff = Math.abs(A.get3() - B.get3()); if (diff > max) max = diff; return max; }
public DxRagdollBody(DBody body, double length, double radius, DVector3C pinitial, DQuaternionC qinitial) { this.body = body; this.length = length; this.radius = radius; this.position = new DVector3(pinitial); this.quaternion = new DQuaternion(qinitial.get0(), qinitial.get1(), qinitial.get2(), qinitial.get3()); }
/** * return the maximum element difference between the two 4*1 matrices. * @param A A * @param B B * @param n n * @param m m * @return difference */ public static double dMaxDifference (final DQuaternionC A, final DQuaternionC B, int n, int m) { double max = Math.abs(A.get0() - B.get0()); double diff = Math.abs(A.get1() - B.get1()); if (diff > max) max = diff; diff = Math.abs(A.get2() - B.get2()); if (diff > max) max = diff; diff = Math.abs(A.get3() - B.get3()); if (diff > max) max = diff; return max; }
public DxRagdollBody(DBody body, double length, double radius, DVector3C pinitial, DQuaternionC qinitial) { this.body = body; this.length = length; this.radius = radius; this.position = new DVector3(pinitial); this.quaternion = new DQuaternion(qinitial.get0(), qinitial.get1(), qinitial.get2(), qinitial.get3()); }
public static void dDQfromW (DQuaternion dq, final DVector3C w, final DQuaternionC q) { //dAASSERT (w, q, dq); dq.set(0, 0.5*(- w.get0()*q.get1() - w.get1()*q.get2() - w.get2()*q.get3()) ); dq.set(1, 0.5*( w.get0()*q.get0() + w.get1()*q.get3() - w.get2()*q.get2()) ); dq.set(2, 0.5*(- w.get0()*q.get3() + w.get1()*q.get0() + w.get2()*q.get1()) ); dq.set(3, 0.5*( w.get0()*q.get2() - w.get1()*q.get1() + w.get2()*q.get0()) ); } }
public static void dDQfromW (DQuaternion dq, final DVector3C w, final DQuaternionC q) { //dAASSERT (w, q, dq); dq.set(0, 0.5*(- w.get0()*q.get1() - w.get1()*q.get2() - w.get2()*q.get3()) ); dq.set(1, 0.5*( w.get0()*q.get0() + w.get1()*q.get3() - w.get2()*q.get2()) ); dq.set(2, 0.5*(- w.get0()*q.get3() + w.get1()*q.get0() + w.get2()*q.get1()) ); dq.set(3, 0.5*( w.get0()*q.get2() - w.get1()*q.get1() + w.get2()*q.get0()) ); } }
/** General collide function */ // static int ccdCollide(dGeomID o1, dGeomID o2, int flags, // dContactGeom *contact, int skip, // void *obj1, ccd_support_fn supp1, ccd_center_fn cen1, // void *obj2, ccd_support_fn supp2, ccd_center_fn cen2); static void ccdGeomToObj(final DGeom g, ccd_obj_t o) { DVector3C ode_pos; DQuaternionC ode_rot; ode_pos = g.getPosition(); ode_rot = g.getQuaternion(); ccdVec3Set(o.pos, ode_pos); ccdQuatSet(o.rot, ode_rot.get1(), ode_rot.get2(), ode_rot.get3(), ode_rot.get0()); ccdQuatInvert2(o.rot_inv, o.rot); }
double cost2 = qrel.get0(); double sint2 = dSqrt( qrel.get1() * qrel.get1() + qrel.get2() * qrel.get2() + qrel.get3() * qrel.get3() ); double theta = ( (qrel.get1()*axis.get0() + qrel.get2()*axis.get1() + qrel.get3()*axis.get2() ) >= 0 ) ? // @@@ padding assumptions ( 2 * dAtan2( sint2, cost2 ) ) : // if u points in direction of axis ( 2 * dAtan2( sint2, -cost2 ) ); // if u points in opposite direction