/** * Make the matrix an identity matrix. * Same as setIdenity(). */ public final void eqIdentity() { eqZero(); set00(1); set11(1); set22(1); }
/** * Make the matrix an identity matrix. * Same as setIdenity(). */ public final void eqIdentity() { eqZero(); set00(1); set11(1); set22(1); }
public void dMassSetSphereTotal (double total_mass, double radius) { //dAASSERT (m); dMassSetZero (); _mass = total_mass; double II = 0.4 * total_mass * radius*radius; _I.set00( II ); _I.set11( II ); _I.set22( II ); if (!dNODEBUG) {//# ifndef dNODEBUG dMassCheck (); }//# endif }
public void dMassSetBoxTotal (double total_mass, double lx, double ly, double lz) { // dAASSERT (m); dMassSetZero (); _mass = total_mass; _I.set00( total_mass/12.0 * (ly*ly + lz*lz) ); _I.set11( total_mass/12.0 * (lx*lx + lz*lz) ); _I.set22( total_mass/12.0 * (lx*lx + ly*ly) ); if (!dNODEBUG) {//# ifndef dNODEBUG dMassCheck (); }//# endif }
public void dMassSetSphereTotal (double total_mass, double radius) { //dAASSERT (m); dMassSetZero (); _mass = total_mass; double II = 0.4 * total_mass * radius*radius; _I.set00( II ); _I.set11( II ); _I.set22( II ); if (!dNODEBUG) {//# ifndef dNODEBUG dMassCheck (); }//# endif }
public void dMassSetBoxTotal (double total_mass, double lx, double ly, double lz) { // dAASSERT (m); dMassSetZero (); _mass = total_mass; _I.set00( total_mass/12.0 * (ly*ly + lz*lz) ); _I.set11( total_mass/12.0 * (lx*lx + lz*lz) ); _I.set22( total_mass/12.0 * (lx*lx + ly*ly) ); if (!dNODEBUG) {//# ifndef dNODEBUG dMassCheck (); }//# endif }
public void dMassSetCapsule (double density, int direction, double radius, double length) { double M1,M2,Ia,Ib; // dAASSERT (m); dUASSERT (direction >= 1 && direction <= 3,"bad direction number"); dMassSetZero (); M1 = M_PI*radius*radius*length*density; // cylinder mass M2 = (4.0/3.0)*M_PI*radius*radius*radius*density; // total cap mass _mass = M1+M2; Ia = M1*(0.25*radius*radius + (1.0/12.0)*length*length) + M2*(0.4*radius*radius + 0.375*radius*length + 0.25*length*length); Ib = (M1*0.5 + M2*0.4)*radius*radius; _I.set00( Ia ); _I.set11( Ia ); _I.set22( Ia ); _I.set( direction-1, direction-1, Ib ); if (!dNODEBUG) {//# ifndef dNODEBUG dMassCheck (); }//# endif }
public void dMassSetCylinderTotal (double total_mass, int direction, double radius, double length) { double r2,I; //dAASSERT (m); dUASSERT (direction >= 1 && direction <= 3,"bad direction number"); dMassSetZero (); r2 = radius*radius; _mass = total_mass; I = total_mass*(0.25*r2 + (1.0/12.0)*length*length); _I.set00( I ); _I.set11( I ); _I.set22( I ); _I.set( direction-1, direction-1, total_mass*0.5*r2 ); if (!dNODEBUG) {//# ifndef dNODEBUG dMassCheck (); }//# endif }
public void dMassSetCylinderTotal (double total_mass, int direction, double radius, double length) { double r2,I; //dAASSERT (m); dUASSERT (direction >= 1 && direction <= 3,"bad direction number"); dMassSetZero (); r2 = radius*radius; _mass = total_mass; I = total_mass*(0.25*r2 + (1.0/12.0)*length*length); _I.set00( I ); _I.set11( I ); _I.set22( I ); _I.set( direction-1, direction-1, total_mass*0.5*r2 ); if (!dNODEBUG) {//# ifndef dNODEBUG dMassCheck (); }//# endif }
public void set(double i, double j, double k, double l, double m, double n, double o, double p, double q) { // v[0] = i; v[1] = j; v[2] = k; // v[4] = l; v[5] = m; v[6] = n; // v[8] = o; v[9] = p; v[10] = q; set00( i ); set01( j ); set02( k ); set10( l ); set11( m ); set12( n ); set20( o ); set21( p ); set22( q ); } /**
public void set(double i, double j, double k, double l, double m, double n, double o, double p, double q) { // v[0] = i; v[1] = j; v[2] = k; // v[4] = l; v[5] = m; v[6] = n; // v[8] = o; v[9] = p; v[10] = q; set00( i ); set01( j ); set02( k ); set10( l ); set11( m ); set12( n ); set20( o ); set21( p ); set22( q ); } /**
private static void dMULTIPLYOP1_333(DMatrix3 A, DMatrix3C B, DMatrix3C C) { A.set00( dCalcVectorDot3_44(B,0,C,0) ); A.set01( dCalcVectorDot3_44(B,0,C,1) ); A.set02( dCalcVectorDot3_44(B,0,C,2) ); A.set10( dCalcVectorDot3_44(B,1,C,0) ); A.set11( dCalcVectorDot3_44(B,1,C,1) ); A.set12( dCalcVectorDot3_44(B,1,C,2) ); A.set20( dCalcVectorDot3_44(B,2,C,0) ); A.set21( dCalcVectorDot3_44(B,2,C,1) ); A.set22( dCalcVectorDot3_44(B,2,C,2) ); } private static void dMULTIPLYOP2_333(DMatrix3 A, DMatrix3C B, DMatrix3C C) {
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; }
private static void dMULTIPLYOP0_333(DMatrix3 A, DMatrix3C B, DMatrix3C C) { A.set00( B.dotRowCol(0, C, 0) );//(A).v[0] = dDOT14(B.v,0,C.v,0); A.set01( B.dotRowCol(0, C, 1) );//(A).v[1] = dDOT14(B.v,0,C.v,1); A.set02( B.dotRowCol(0, C, 2) );//(A).v[2] = dDOT14(B.v,0,C.v,2); A.set10( B.dotRowCol(1, C, 0) );//(A).v[4] = dDOT14(B.v,4,C.v,0); A.set11( B.dotRowCol(1, C, 1) );//(A).v[5] = dDOT14(B.v,4,C.v,1); A.set12( B.dotRowCol(1, C, 2) );//(A).v[6] = dDOT14(B.v,4,C.v,2); A.set20( B.dotRowCol(2, C, 0) );//(A).v[8] = dDOT14(B.v,8,C.v,0); A.set21( B.dotRowCol(2, C, 1) );//(A).v[9] = dDOT14(B.v,8,C.v,1); A.set22( B.dotRowCol(2, C, 2) );//(A).v[10] = dDOT14(B.v,8,C.v,2); } private static void dMULTIPLYOP1_333(DMatrix3 A, DMatrix3C B, DMatrix3C C) {
private static void dMULTIPLYOP1_333(DMatrix3 A, DMatrix3C B, DMatrix3C C) { A.set00( dCalcVectorDot3_44(B,0,C,0) ); A.set01( dCalcVectorDot3_44(B,0,C,1) ); A.set02( dCalcVectorDot3_44(B,0,C,2) ); A.set10( dCalcVectorDot3_44(B,1,C,0) ); A.set11( dCalcVectorDot3_44(B,1,C,1) ); A.set12( dCalcVectorDot3_44(B,1,C,2) ); A.set20( dCalcVectorDot3_44(B,2,C,0) ); A.set21( dCalcVectorDot3_44(B,2,C,1) ); A.set22( dCalcVectorDot3_44(B,2,C,2) ); } private static void dMULTIPLYOP2_333(DMatrix3 A, DMatrix3C B, DMatrix3C C) {
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; }
private static void dMULTIPLYOP0_333(DMatrix3 A, DMatrix3C B, DMatrix3C C) { A.set00( B.dotRowCol(0, C, 0) );//(A).v[0] = dDOT14(B.v,0,C.v,0); A.set01( B.dotRowCol(0, C, 1) );//(A).v[1] = dDOT14(B.v,0,C.v,1); A.set02( B.dotRowCol(0, C, 2) );//(A).v[2] = dDOT14(B.v,0,C.v,2); A.set10( B.dotRowCol(1, C, 0) );//(A).v[4] = dDOT14(B.v,4,C.v,0); A.set11( B.dotRowCol(1, C, 1) );//(A).v[5] = dDOT14(B.v,4,C.v,1); A.set12( B.dotRowCol(1, C, 2) );//(A).v[6] = dDOT14(B.v,4,C.v,2); A.set20( B.dotRowCol(2, C, 0) );//(A).v[8] = dDOT14(B.v,8,C.v,0); A.set21( B.dotRowCol(2, C, 1) );//(A).v[9] = dDOT14(B.v,8,C.v,1); A.set22( B.dotRowCol(2, C, 2) );//(A).v[10] = dDOT14(B.v,8,C.v,2); } private static void dMULTIPLYOP1_333(DMatrix3 A, DMatrix3C 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; }
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; }
void dMatrix3Inv( final DMatrix3 ma, DMatrix3 dst ) { double det = dMatrix3Det( ma ); if ( dFabs( det ) < (0.0005) ) { dst.setIdentity(); return; } double detRecip = 1.0 / det; dst.set00( (ma.get11()*ma.get22() - ma.get12()*ma.get21()) * detRecip ); dst.set01( (ma.get21()*ma.get02() - ma.get01()*ma.get22()) * detRecip ); dst.set02( (ma.get01()*ma.get12() - ma.get11()*ma.get02()) * detRecip ); dst.set10( (ma.get12()*ma.get20() - ma.get10()*ma.get22()) * detRecip ); dst.set11( (ma.get00()*ma.get22() - ma.get20()*ma.get02()) * detRecip ); dst.set12( (ma.get10()*ma.get02() - ma.get00()*ma.get12()) * detRecip ); dst.set20( (ma.get10()*ma.get21() - ma.get20()*ma.get11()) * detRecip ); dst.set21( (ma.get20()*ma.get01() - ma.get00()*ma.get21()) * detRecip ); dst.set22( (ma.get00()*ma.get11() - ma.get01()*ma.get10()) * detRecip ); }