package dynsim;
import java.lang.Math;
import dynsim.Mat;

// A class to do conversions between axis-angle and 3x3 matrix rotations. 
public class Rotation {
  private float AA[][];
  private float A[][];
  private float AT[][];
  private float temp1[][];
  private float temp2[][];
  private float skew[][];
  private float R[][];
  private boolean synced;

  // Initialize all the arrays
  public Rotation() {
    AA = new float[4][1];
    A = new float[3][1];
    AT = new float[1][3];
    temp1 = new float[3][3];
    temp2 = new float[3][3];
    R = new float[3][3];
    synced = false;
  }

  // Set the rotation with an Axis-Angle argument (should be a 4x1 matrix)
  public void setAA(float AAin[][]) {
    A[0][0] = AAin[0][0];
    A[1][0] = AAin[1][0];
    A[2][0] = AAin[2][0];
    AA[0][0] = AAin[0][0];
    AA[1][0] = AAin[1][0];
    AA[2][0] = AAin[2][0];
    AA[3][0] = AAin[3][0];
    float theta = AAin[3][0];
    Mat.Scale(1.0f/Mat.Norm(A), A, A);
    Mat.Transpose(A,AT);
    Mat.Mult(A,AT,temp1);
    Mat.Ident(temp2);
    Mat.Wadd((1.0f-(float)Math.cos(theta)),temp1, (float)Math.cos(theta), temp2, temp1);
    Mat.Skew(A,temp2);
    Mat.Wadd(1.0f, temp1, (float)Math.sin(theta), temp2, R);
    synced = true;
  }

  // Get an axis-angle representation of the rotation. Return as a 4x1 matrix
  public float[][] getAA() {
    if (! synced) {
      Mat.Transpose(R, temp1);
      Mat.Wadd(1.0f, R, -1.0f, temp1, temp1);
      Mat.Unskew(temp1, A);
      float n = Mat.Norm(A);
      if (Math.abs(n) > 1.0e-6)
	Mat.Scale(1.0f/n, A, A);
      float c =  Mat.Trace(R) - 1.0f;
      float angle = (float)Math.atan2(n, c);
      AA[0][0] = A[0][0];
      AA[1][0] = A[1][0];
      AA[2][0] = A[2][0];
      AA[3][0] = angle;
      synced = true;
    }
    return AA;
  }

  // Same as the last function but copy it into the Aout argument matrix. 
  public void getAA(float Aout[][]) {
    getAA();
    Aout[0][0] = AA[0][0];
    Aout[1][0] = AA[1][0];
    Aout[2][0] = AA[2][0];
    Aout[3][0] = AA[3][0];
  }

  // Get the 3x3 matrix representation of the rotation and return it.
  public float[][] getR() {
    return R;
  }

  // Same but copy it into the Rout argument array.
  public void getR(float Rout[][]) {
    Rout[0][0] = R[0][0];
    Rout[1][0] = R[1][0];
    Rout[2][0] = R[2][0];
    Rout[0][1] = R[0][1];
    Rout[1][1] = R[1][1];
    Rout[2][1] = R[2][1];
    Rout[0][2] = R[0][2];
    Rout[1][2] = R[1][2];
    Rout[2][2] = R[2][2];
  }

  // Set the rotation using a 3x3 input matrix
  public void setR(float Rin[][]) {
    synced = false;
    R[0][0] = Rin[0][0];
    R[1][0] = Rin[1][0];
    R[2][0] = Rin[2][0];
    R[0][1] = Rin[0][1];
    R[1][1] = Rin[1][1];
    R[2][1] = Rin[2][1];
    R[0][2] = Rin[0][2];
    R[1][2] = Rin[1][2];
    R[2][2] = Rin[2][2];
  }
}