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 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) {
private void GETCOL(DMatrix3C m, int a, DVector3 v) { if (a==0) { v.set(m.get00(), m.get10(), m.get20()); } else if (a==1) { v.set(m.get01(), m.get11(), m.get21()); } else if (a==2) { v.set(m.get02(), m.get12(), m.get22()); } else throw new IllegalArgumentException("col=" + a); }
private void GETCOL(DMatrix3C m, int a, DVector3 v) { if (a==0) { v.set(m.get00(), m.get10(), m.get20()); } else if (a==1) { v.set(m.get01(), m.get11(), m.get21()); } else if (a==2) { v.set(m.get02(), m.get12(), m.get22()); } else throw new IllegalArgumentException("col=" + a); }
private void GETCOL(DMatrix3C m, int a, DVector3 v) { if (a==0) { v.set(m.get00(), m.get10(), m.get20()); } else if (a==1) { v.set(m.get01(), m.get11(), m.get21()); } else if (a==2) { v.set(m.get02(), m.get12(), m.get22()); } else throw new IllegalArgumentException("col=" + a); }
private void GETCOL(DMatrix3C m, int a, DVector3 v) { if (a==0) { v.set(m.get00(), m.get10(), m.get20()); } else if (a==1) { v.set(m.get01(), m.get11(), m.get21()); } else if (a==2) { v.set(m.get02(), m.get12(), m.get22()); } else throw new IllegalArgumentException("col=" + a); }
private static void DQFR_case_0(DQuaternion q, final DMatrix3C R) { //case_0: double s = dSqrt((R.get00() - (R.get11() + R.get22())) + 1.); q.set1( 0.5 * s ); s = 0.5 * dRecip(s); q.set2( (R.get01() + R.get10()) * s ); q.set3( (R.get20() + R.get02()) * s ); q.set0( (R.get21() - R.get12()) * s ); return; }
private static void DQFR_case_2(DQuaternion q, final DMatrix3C R) { //case_2: double s = dSqrt((R.get22() - (R.get00() + R.get11())) + 1.); q.set3( 0.5 * s ); s = 0.5 * dRecip(s); q.set1( (R.get20() + R.get02()) * s ); q.set2( (R.get12() + R.get21()) * s ); q.set0( (R.get10() - R.get01()) * s ); return; }
private static void DQFR_case_0(DQuaternion q, final DMatrix3C R) { //case_0: double s = dSqrt((R.get00() - (R.get11() + R.get22())) + 1.); q.set1( 0.5 * s ); s = 0.5 * dRecip(s); q.set2( (R.get01() + R.get10()) * s ); q.set3( (R.get20() + R.get02()) * s ); q.set0( (R.get21() - R.get12()) * s ); return; }
public static double dCalcMatrix3Det( final DMatrix3C mat ) { double det; // det = mat[0] * ( mat[5]*mat[10] - mat[9]*mat[6] ) // - mat[1] * ( mat[4]*mat[10] - mat[8]*mat[6] ) // + mat[2] * ( mat[4]*mat[9] - mat[8]*mat[5] ); det = mat.get00() * ( mat.get11()*mat.get22() - mat.get21()*mat.get12() ) - mat.get01() * ( mat.get10()*mat.get22() - mat.get20()*mat.get12() ) + mat.get02() * ( mat.get10()*mat.get21() - mat.get20()*mat.get11() ); return( det ); }
private static void DQFR_case_1(DQuaternion q, final DMatrix3C R) { //case_1: double s = dSqrt((R.get11() - (R.get22() + R.get00())) + 1.); q.set2( 0.5 * s ); s = 0.5 * dRecip(s); q.set3( (R.get12() + R.get21()) * s ); q.set1( (R.get01() + R.get10()) * s ); q.set0( (R.get02() - R.get20()) * s ); return; }
public static double dCalcMatrix3Det( final DMatrix3C mat ) { double det; // det = mat[0] * ( mat[5]*mat[10] - mat[9]*mat[6] ) // - mat[1] * ( mat[4]*mat[10] - mat[8]*mat[6] ) // + mat[2] * ( mat[4]*mat[9] - mat[8]*mat[5] ); det = mat.get00() * ( mat.get11()*mat.get22() - mat.get21()*mat.get12() ) - mat.get01() * ( mat.get10()*mat.get22() - mat.get20()*mat.get12() ) + mat.get02() * ( mat.get10()*mat.get21() - mat.get20()*mat.get11() ); return( det ); }
private static void DQFR_case_1(DQuaternion q, final DMatrix3C R) { //case_1: double s = dSqrt((R.get11() - (R.get22() + R.get00())) + 1.); q.set2( 0.5 * s ); s = 0.5 * dRecip(s); q.set3( (R.get12() + R.get21()) * s ); q.set1( (R.get01() + R.get10()) * s ); q.set0( (R.get02() - R.get20()) * s ); return; }
private static void DQFR_case_2(DQuaternion q, final DMatrix3C R) { //case_2: double s = dSqrt((R.get22() - (R.get00() + R.get11())) + 1.); q.set3( 0.5 * s ); s = 0.5 * dRecip(s); q.set1( (R.get20() + R.get02()) * s ); q.set2( (R.get12() + R.get21()) * s ); q.set0( (R.get10() - R.get01()) * s ); return; }
public DMatrix3 set(DMatrix3C m) { // System.arraycopy(((DMatrix3)m3).v, 0, v, 0, v.length); // //v[0] = v3.v[0]; v[1] = v3.v[1]; v[2] = v3.v[2]; v[3] = v3.v[3]; set00( m.get00() ); set01( m.get01() ); set02( m.get02() ); set10( m.get10() ); set11( m.get11() ); set12( m.get12() ); set20( m.get20() ); set21( m.get21() ); set22( m.get22() ); return this; }
public DMatrix3 set(DMatrix3C m) { // System.arraycopy(((DMatrix3)m3).v, 0, v, 0, v.length); // //v[0] = v3.v[0]; v[1] = v3.v[1]; v[2] = v3.v[2]; v[3] = v3.v[3]; set00( m.get00() ); set01( m.get01() ); set02( m.get02() ); set10( m.get10() ); set11( m.get11() ); set12( m.get12() ); set20( m.get20() ); set21( m.get21() ); set22( m.get22() ); return this; }
void printMassParams (DMass m) { printf ("mass = %.4f\n",m.getMass()); printf ("com = (%.4f,%.4f,%.4f)\n",m.getC().get0(),m.getC().get1(),m.getC().get2()); DMatrix3C I = m.getI(); printf ("I = [ %10.4f %10.4f %10.4f ]\n" + " [ %10.4f %10.4f %10.4f ]\n" + " [ %10.4f %10.4f %10.4f ]\n", I.get00(),I.get01(),I.get02(), I.get10(),I.get11(),I.get12(), I.get20(),I.get21(),I.get22()); }
/** * Calculates the ordinary matrix product for a 3x3 Matrix and a 3-Vector. * <pre> * a0 = m00*v0 + m01*v1 + m02*v2 * a1 = m10*v0 + m11*v1 + m12*v2 * a2 = m20*v0 + m21*v1 + m22*v2 * </pre> * @param m matrix m * @param v2 vector v */ public final void eqProd(DMatrix3C m, DVector3C v2) { set0( m.get00()*v2.get0()+ m.get01()*v2.get1()+ m.get02()*v2.get2() ); set1( m.get10()*v2.get0()+ m.get11()*v2.get1()+ m.get12()*v2.get2() ); set2( m.get20()*v2.get0()+ m.get21()*v2.get1()+ m.get22()*v2.get2() ); }
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; }
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; }