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 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 dSubtractVectorCross3(DVector3 a, DVector3C b, DMatrix3C c) { a.add0( - (b.get1()*c.get02() - b.get2()*c.get01()) ); a.add1( - (b.get2()*c.get00() - b.get0()*c.get02()) ); a.add2( - (b.get0()*c.get01() - b.get1()*c.get00()) ); } // public static void dCROSS(DVector3 a, OP op, DVector3C b, DMatrix3C c) {
public DMatrix3 add(DMatrix3C m) { set00( get00() + m.get00() ); set01( get01() + m.get01() ); set02( get02() + m.get02() ); set10( get10() + m.get10() ); set11( get11() + m.get11() ); set12( get12() + m.get12() ); set20( get20() + m.get20() ); set21( get21() + m.get21() ); set22( get22() + m.get22() ); // DMatrix3 M = (DMatrix3) M2; // for (int i = 0; i < v.length; i++) { // v[i] += M.v[i]; // } return this; }
/** * Cross product, set a -= b x c. * @param a a * @param b b * @param c c */ public static void dSubtractVectorCross3(DVector3 a, DVector3C b, DVector3C c) { a.add0( -b.get1()*c.get2() + b.get2()*c.get1() ); a.add1( -b.get2()*c.get0() + b.get0()*c.get2() ); a.add2( -b.get0()*c.get1() + b.get1()*c.get0() ); }
public static void dAddScaledVectors3(DVector3 res, DVector3C a, DVector3C b, double a_scale, double b_scale) { double res_0, res_1, res_2; res_0 = a_scale * a.get0() + b_scale * b.get0(); res_1 = a_scale * a.get1() + b_scale * b.get1(); res_2 = a_scale * a.get2() + b_scale * b.get2(); // Only assign after all the calculations are over to avoid incurring memory aliasing res.set( res_0, res_1, res_2 ); }
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 dSubtractVectorCross3(DVector3 a, DVector3C b, DMatrix3C c) { a.add0( - (b.get1()*c.get02() - b.get2()*c.get01()) ); a.add1( - (b.get2()*c.get00() - b.get0()*c.get02()) ); a.add2( - (b.get0()*c.get01() - b.get1()*c.get00()) ); } // public static void dCROSS(DVector3 a, OP op, DVector3C b, DMatrix3C c) {
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 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 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() ); }