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 ); }
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 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); }
static void MakeMatrix(DVector3C Position, DMatrix3C Rotation, mat4f m) { m.f[0] = (float) Rotation.get00(); m.f[1] = (float) Rotation.get01(); m.f[2] = (float) Rotation.get02(); m.f[4] = (float) Rotation.get10(); m.f[5] = (float) Rotation.get11(); m.f[6] = (float) Rotation.get12(); m.f[8] = (float) Rotation.get20();//[8]; m.f[9] = (float) Rotation.get21();//[9]; m.f[10] = (float) Rotation.get22();//[10]; m.f[3] = (float) Position.get0();//[0]; m.f[7] = (float) Position.get1();//[1]; m.f[11] = (float) Position.get2();//[2]; }
void Decompose(DMatrix3C Matrix, DVector3 Right, DVector3 Up, DVector3 Direction){ // Right[0] = Matrix[0 * 4 + 0]; // Right[1] = Matrix[1 * 4 + 0]; // Right[2] = Matrix[2 * 4 + 0]; // Right[3] = REAL(0.0); Right.set(Matrix.get00(), Matrix.get10(), Matrix.get20()); // Up[0] = Matrix[0 * 4 + 1]; // Up[1] = Matrix[1 * 4 + 1]; // Up[2] = Matrix[2 * 4 + 1]; // Up[3] = REAL(0.0); Up.set(Matrix.get01(), Matrix.get11(), Matrix.get21()); // Direction[0] = Matrix[0 * 4 + 2]; // Direction[1] = Matrix[1 * 4 + 2]; // Direction[2] = Matrix[2 * 4 + 2]; // Direction[3] = REAL(0.0); Direction.set(Matrix.get02(), Matrix.get12(), Matrix.get22()); }
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; }
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_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; }