public final SimpleVector matrixToRad(Matrix m) { float[] mDump = m.getDump(); float heading,attitude,bank; if (mDump[4] > 0.998) { // singularity at north pole heading = (float) Math.atan2(mDump[2],mDump[10]); attitude = (float) (Math.PI/2); bank = 0; } else if (mDump[4] < -0.998) { // singularity at south pole heading = (float) Math.atan2(mDump[2],mDump[10]); attitude = (float) (-Math.PI/2); bank = 0; } else{ heading = (float) Math.atan2(-mDump[8],mDump[0]); bank = (float) Math.atan2(-mDump[6],mDump[5]); attitude = (float) Math.asin(mDump[4]); } return new SimpleVector(bank,heading,attitude); }