/** * 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; }
/** * 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 DQuaternion set(DQuaternionC q) { v[0]=q.get0(); v[1]=q.get1(); v[2]=q.get2(); v[3]=q.get3(); return this; }
public DQuaternion set(DQuaternionC q) { v[0]=q.get0(); v[1]=q.get1(); v[2]=q.get2(); v[3]=q.get3(); return this; }
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() ); }
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 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); }
/** 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); }
public static void dRfromQ (DMatrix3 R, final DQuaternionC q) { //dAASSERT (q, R); // q = (s,vx,vy,vz) double q0 = q.get0(); double q1 = q.get1(); double q2 = q.get2(); double q3 = q.get3(); double qq1 = 2*q1*q1; double qq2 = 2*q2*q2; double qq3 = 2*q3*q3; R.set00( 1 - qq2 - qq3 ); R.set01( 2*(q1*q2 - q0*q3) ); R.set02( 2*(q1*q3 + q0*q2) ); //R.v[_R(0,3)] = 0.0 ); R.set10( 2*(q1*q2 + q0*q3) ); R.set11( 1 - qq1 - qq3 ); R.set12( 2*(q2*q3 - q0*q1) ); //R.v[_R(1,3)] = 0.0 ); R.set20( 2*(q1*q3 - q0*q2) ); R.set21( 2*(q2*q3 + q0*q1) ); R.set22( 1 - qq1 - qq2 ); //R.v[_R(2,3)] = 0.0 ); }
public static void dRfromQ (DMatrix3 R, final DQuaternionC q) { //dAASSERT (q, R); // q = (s,vx,vy,vz) double q0 = q.get0(); double q1 = q.get1(); double q2 = q.get2(); double q3 = q.get3(); double qq1 = 2*q1*q1; double qq2 = 2*q2*q2; double qq3 = 2*q3*q3; R.set00( 1 - qq2 - qq3 ); R.set01( 2*(q1*q2 - q0*q3) ); R.set02( 2*(q1*q3 + q0*q2) ); //R.v[_R(0,3)] = 0.0 ); R.set10( 2*(q1*q2 + q0*q3) ); R.set11( 1 - qq1 - qq3 ); R.set12( 2*(q2*q3 - q0*q1) ); //R.v[_R(1,3)] = 0.0 ); R.set20( 2*(q1*q3 - q0*q2) ); R.set21( 2*(q2*q3 + q0*q1) ); R.set22( 1 - qq1 - qq2 ); //R.v[_R(2,3)] = 0.0 ); }