已知旋转矩阵求轴角度

public void rotate(Matrix m) {

    float[] mf = new float[9];

    m.getValues(mf);

        // Assuming the angles are in radians.

    if (mf[1] > 0.998) { // singularity at north pole

    rotateY = (float)(Math.atan2(mf[6],mf[8])* 180/3.1415926f);

    rotateZ = (float)(Math.PI/2* 180/3.1415926f);

    rotateX = 0;

    return;

    }

    if (mf[1] < -0.998) { // singularity at south pole

    rotateY = (float)(Math.atan2(mf[6],mf[8])* 180/3.1415926f);

    rotateZ = (float)(-Math.PI/2* 180/3.1415926f);

    rotateX = 0;

    return;

    }

    rotateY = (float)(Math.atan2(-mf[2],mf[0])* 180/3.1415926f);

    rotateX = (float)(Math.atan2(-mf[7],mf[4]) * 180/3.1415926f);

    rotateZ = (float)(Math.asin(mf[1])* 180/3.1415926f);

    }

  

  public float toAxisAngle(Matrix mm) {

float[] m = new float[9];

  mm.getValues(m);

  double angle = 0,x,y,z; // variables for result

double epsilon = 0.01; // margin to allow for rounding errors

double epsilon2 = 0.1; // margin to distinguish between 0 and 180 degrees

// optional check that input is pure rotation, 'isRotationMatrix' is defined at:

// http://www.euclideanspace.com/maths/algebra/matrix/orthogonal/rotation/

//assert isRotationMatrix(m) : "not valid rotation matrix" ;// for debugging

if ((Math.abs(m[2]-m[1])< epsilon)

  && (Math.abs(m[6]-m[2])< epsilon)

  && (Math.abs(m[7]-m[5])< epsilon)) {

// singularity found

// first check for identity matrix which must have +1 for all terms

//  in leading diagonaland zero in other terms

if ((Math.abs(m[3]+m[1]) < epsilon2)

  && (Math.abs(m[6]+m[2]) < epsilon2)

  && (Math.abs(m[7]+m[5]) < epsilon2)

  && (Math.abs(m[0]+m[4]+m[8]-3) < epsilon2)) {

// this singularity is identity matrix so angle = 0

rotate = (float) (angle * 180/3.1415926f);

Log.e("webview", "angle"+angle);

return rotate;//new axisAngle(0,1,0,0); // zero angle, arbitrary axis

}

// otherwise this singularity is angle = 180

angle = Math.PI;

double xx = (m[0]+1)/2;

double yy = (m[4]+1)/2;

double zz = (m[8]+1)/2;

double xy = (m[3]+m[1])/4;

double xz = (m[6]+m[2])/4;

double yz = (m[7]+m[5])/4;

if ((xx > yy) && (xx > zz)) { // m[0][0] is the largest diagonal term

if (xx< epsilon) {

x = 0;

y = 0.7071;

z = 0.7071;

} else {

x = Math.sqrt(xx);

y = xy/x;

z = xz/x;

}

} else if (yy > zz) { // m[1][1] is the largest diagonal term

if (yy< epsilon) {

x = 0.7071;

y = 0;

z = 0.7071;

} else {

y = Math.sqrt(yy);

x = xy/y;

z = yz/y;

}

} else { // m[2][2] is the largest diagonal term so base result on this

if (zz< epsilon) {

x = 0.7071;

y = 0.7071;

z = 0;

} else {

z = Math.sqrt(zz);

x = xz/z;

y = yz/z;

}

}

rotate = (float) (angle * 180/3.1415926f);

Log.e("webview", "angle"+angle);

return rotate;//new axisAngle(angle,x,y,z); // return 180 deg rotation

}

// as we have reached here there are no singularities so we can handle normally

double s = Math.sqrt((m[5] - m[7])*(m[5] - m[7])

+(m[6] - m[2])*(m[6] - m[2])

+(m[1] - m[3])*(m[1] - m[3])); // used to normalise

if (Math.abs(s) < 0.001) s=1; 

// prevent divide by zero, should not happen if matrix is orthogonal and should be

// caught by singularity test above, but I've left it in just in case

angle = Math.acos(( m[0] + m[4] + m[8] - 1)/2);

x = (m[5] - m[7])/s;

y = (m[6] - m[2])/s;

z = (m[1] - m[3])/s;

rotate = (float) (angle * 180/3.1415926f);

Log.e("webview", "angle"+angle);

  return rotate;//new axisAngle(angle,x,y,z);

}

  

  public void fn()

  {

  float m[] = new float[9];

 

  double ang = 45.0f/180*3.1415926f;

  m[0] = (float) Math.cos(ang);

  m[1] = (float) Math.sin(ang);

  m[2] = 0;

  m[3] = -(float) Math.sin(ang);

  m[4] = (float) Math.cos(ang);

  m[5] = 0;

  m[6] = 0;

  m[7] = 0;

  m[8] = 1;

 

  mMatrix.setValues(m);

  }

你可能感兴趣的:(Android)