// rotate.java       compile    javac -cp . rotate.java
//                   execute    java  -cp . rotate > rotate_java.out
//
//   | 1   0     0   |  | cosb  0  sinb |  | cosc  -sinc  0 |  | X |
//   |               |  |               |  |                |  |   |
//   | 0 cosa  -sina |  |   0   1   0   |  | sinc   cosc  0 |  | Y |
//   |               |  |               |  |                |  |   |
//   | 0 sina  cosa  |  | -simb 0  cosb |  |   0     0    1 |  | Z |
//
// rotate about X axis,      Y axis,           Z axis
//             Mx           My                Mz
//
// MXYZ = Mx * My * Mz ????
// need 4 by 4 arrays to do offset, translation:
//            all above would have  1  at  [3][3]
//   | 1  0  0  dx |   |x|
//   | 0  1  0  dy | * |y|
//   | 0  0  1  dz |   |z|
//   | 0  0  0   1 |   |1|  see rotate4.java

public class rotate
{
  double A[][]  = new double[3][3];
  double B[][]  = new double[3][3];
  double C[][]  = new double[3][3];
  double V[]    = new double[3];
  double W[]    = new double[3];
  double P[]    = new double[3];
  double Q[]    = new double[3];
  double Mx[][] = new double[3][3];
  double My[][] = new double[3][3];
  double Mz[][] = new double[3][3];
  double Mt[][] = new double[3][3];
  double Mw[][] = new double[3][3];
  double Mxyz[][] = new double[3][3];
  double Mxy[][] = new double[3][3];
  double Mxz[][] = new double[3][3];

  double X, Y, Z;
  double xx, yx, zx, xy, yy, zy, xz, yz, zz;
  double plength, vlength, angrad, angdeg;
  double dx, dy, dz, angx, angy, angz;
    
  double Pi = 3.14159265358979323846;
  double mPi, dPi;
  double xanga = Pi/4.0;  // try 45 degrees
  double yangb = Pi/4.0;
  double zangc = Pi/4.0;


  public rotate()
  {
    System.out.println("rotate.java running");
    System.out.println("experiment with many methods of rotating a vector");
    System.out.println(" ");
    mPi = Math.PI;
    dPi = Pi-mPi;
    System.out.println("Pi-mPi="+dPi+" radians");
    
    System.out.println("xanga="+xanga+" radians "+degrees(xanga)+" degrees");
    System.out.println("yangb="+yangb+" radians "+degrees(yangb)+" degrees");
    System.out.println("zangc="+zangc+" radians "+degrees(zangc)+" degrees");
    System.out.println("build rotation matrix using these angles");

    identm(Mx);
    Mx[1][1] = Math.cos(xanga);
    Mx[2][2] = Math.cos(xanga);
    Mx[2][1] = Math.sin(xanga);
    Mx[1][2] = - Math.sin(xanga);
    printm(Mx, "rotate about X Mx=");
    System.out.println(" ");

    identm(My);
    My[0][0] = Math.cos(yangb);
    My[2][2] = Math.cos(yangb);
    My[0][2] = Math.sin(yangb);
    My[2][0] = -Math.sin(yangb);
    printm(My, "rotate about Y My=");
    System.out.println(" ");

    identm(Mz);
    Mz[0][0] = Math.cos(zangc);
    Mz[1][1] = Math.cos(zangc);
    Mz[1][0] = Math.sin(zangc);
    Mz[0][1] = -Math.sin(zangc);
    printm(Mz, "rotate about Z Mz=");
    System.out.println(" ");

    V[0]=1.0;
    V[1]=1.0;
    V[2]=1.0;
    matvec(Mx, V, P);
    System.out.println("rotating V(1,1,1) about X is P("+
		       P[0]+", "+P[1]+", "+P[2]+")");
    plength = Math.sqrt(P[0]*P[0]+P[1]*P[1]+P[2]*P[2]);
    vlength = Math.sqrt(V[0]*V[0]+V[1]*V[1]+V[2]*V[2]);
    System.out.println(" ");
    System.out.println("vlength="+vlength+"  plength="+plength);

    // angle V from (0,0,0)
    dx = V[0];
    dy = V[1];
    dz = V[2];
    System.out.println("dx="+dx+"  dy="+dy+"  dz="+dz);
    angx = Math.acos(dx/vlength);
    angy = Math.acos(dy/vlength);
    angz = Math.acos(dz/vlength);
    System.out.println("angle V from (0,0,0)");
    System.out.println("angx="+angx+"  angy="+angy+"  angz="+angz+" rad");
    System.out.println("angx="+degrees(angx)+"  angy="+degrees(angy)+
		       "  angz="+degrees(angz)+" deg");
    System.out.println(" ");

    // angle P from (0,0,0)
    dx = P[0];
    dy = P[1];
    dz = P[2];
    System.out.println("dx="+dx+"  dy="+dy+"  dz="+dz);
    angx = Math.acos(dx/vlength);
    angy = Math.acos(dy/vlength);
    angz = Math.acos(dz/vlength);
    System.out.println("angle P from (0,0,0)");
    System.out.println("angx="+angx+"  angy="+angy+"  angz="+angz+" rad");
    System.out.println("angx="+degrees(angx)+"  angy="+degrees(angy)+
		       "  angz="+degrees(angz)+" deg");
    System.out.println(" ");

    // angle V - P
    dx = V[0]-P[0];
    dy = V[1]-P[1];
    dz = V[2]-P[2];
    System.out.println("dx="+dx+"  dy="+dy+"  dz="+dz);
    angx = Math.acos(dx/vlength);
    angy = Math.acos(dy/vlength);
    angz = Math.acos(dz/vlength);
    System.out.println("angle V - P");
    System.out.println("angx="+angx+"  angy="+angy+"  angz="+angz+" rad");
    System.out.println("angx="+degrees(angx)+"  angy="+degrees(angy)+
		       "  angz="+degrees(angz)+" deg");
    System.out.println(" ");
    System.out.println(" ");

    matvec(My, V, P);
    System.out.println("rotating V(1,1,1) about Y is P("+
		       P[0]+", "+P[1]+", "+P[2]+")");
    plength = Math.sqrt(P[0]*P[0]+P[1]*P[1]+P[2]*P[2]);
    vlength = Math.sqrt(V[0]*V[0]+V[1]*V[1]+V[2]*V[2]);
    System.out.println(" ");
    System.out.println("vlength="+vlength+"  plength="+plength);

    // angle V from (0,0,0)
    dx = V[0];
    dy = V[1];
    dz = V[2];
    System.out.println("dx="+dx+"  dy="+dy+"  dz="+dz);
    angx = Math.acos(dx/vlength);
    angy = Math.acos(dy/vlength);
    angz = Math.acos(dz/vlength);
    System.out.println("angle V from (0,0,0)");
    System.out.println("angx="+angx+"  angy="+angy+"  angz="+angz+" rad");
    System.out.println("angx="+degrees(angx)+"  angy="+degrees(angy)+
		       "  angz="+degrees(angz)+" deg");
    System.out.println(" ");

    // angle P from (0,0,0)
    dx = P[0];
    dy = P[1];
    dz = P[2];
    System.out.println("dx="+dx+"  dy="+dy+"  dz="+dz);
    angx = Math.acos(dx/vlength);
    angy = Math.acos(dy/vlength);
    angz = Math.acos(dz/vlength);
    System.out.println("angle P from (0,0,0)");
    System.out.println("angx="+angx+"  angy="+angy+"  angz="+angz+" rad");
    System.out.println("angx="+degrees(angx)+"  angy="+degrees(angy)+
		       "  angz="+degrees(angz)+" deg");
    System.out.println(" ");

    // angle V - P
    dx = V[0]-P[0];
    dy = V[1]-P[1];
    dz = V[2]-P[2];
    System.out.println("dx="+dx+"  dy="+dy+"  dz="+dz);
    angx = Math.acos(dx/vlength);
    angy = Math.acos(dy/vlength);
    angz = Math.acos(dz/vlength);
    System.out.println("angle V - P");
    System.out.println("angx="+angx+"  angy="+angy+"  angz="+angz+" rad");
    System.out.println("angx="+degrees(angx)+"  angy="+degrees(angy)+
		       "  angz="+degrees(angz)+" deg");
    System.out.println(" ");
    System.out.println(" ");

    matvec(Mz, V, P);
    System.out.println("rotating (1,1,1) about Z is ("+
                       P[0]+", "+P[1]+", "+P[2]+")");
    plength = Math.sqrt(P[0]*P[0]+P[1]*P[1]+P[2]*P[2]);
    vlength = Math.sqrt(V[0]*V[0]+V[1]*V[1]+V[2]*V[2]);
    System.out.println(" ");
    System.out.println("vlength="+vlength+"  plength="+plength);

    // angle V from (0,0,0)
    dx = V[0];
    dy = V[1];
    dz = V[2];
    System.out.println("dx="+dx+"  dy="+dy+"  dz="+dz);
    angx = Math.acos(dx/vlength);
    angy = Math.acos(dy/vlength);
    angz = Math.acos(dz/vlength);
    System.out.println("angle V from (0,0,0)");
    System.out.println("angx="+angx+"  angy="+angy+"  angz="+angz+" rad");
    System.out.println("angx="+degrees(angx)+"  angy="+degrees(angy)+
		       "  angz="+degrees(angz)+" deg");
    System.out.println(" ");

    // angle P from (0,0,0)
    dx = P[0];
    dy = P[1];
    dz = P[2];
    System.out.println("dx="+dx+"  dy="+dy+"  dz="+dz);
    angx = Math.acos(dx/vlength);
    angy = Math.acos(dy/vlength);
    angz = Math.acos(dz/vlength);
    System.out.println("angle P from (0,0,0)");
    System.out.println("angx="+angx+"  angy="+angy+"  angz="+angz+" rad");
    System.out.println("angx="+degrees(angx)+"  angy="+degrees(angy)+
		       "  angz="+degrees(angz)+" deg");
    System.out.println(" ");

    // angle V - P
    dx = V[0]-P[0];
    dy = V[1]-P[1];
    dz = V[2]-P[2];
    System.out.println("dx="+dx+"  dy="+dy+"  dz="+dz);
    angx = Math.acos(dx/vlength);
    angy = Math.acos(dy/vlength);
    angz = Math.acos(dz/vlength);
    System.out.println("angle V - P");
    System.out.println("angx="+angx+"  angy="+angy+"  angz="+angz+" rad");
    System.out.println("angx="+degrees(angx)+"  angy="+degrees(angy)+
		       "  angz="+degrees(angz)+" deg");
    System.out.println(" ");
    System.out.println(" ");
    System.out.println(" ");


    matvec(Mx, V, W);
    matvec(My, W, Q);
    matvec(Mz, Q, P);
    System.out.println("rotating V(1,1,1) about x then y then z is P("+
  		       P[0]+", "+P[1]+", "+P[2]+")");

    plength = Math.sqrt(P[0]*P[0]+P[1]*P[1]+P[2]*P[2]);
    vlength = Math.sqrt(V[0]*V[0]+V[1]*V[1]+V[2]*V[2]);
    System.out.println("vlength="+vlength+"  plength="+plength);
    System.out.println(" ");

    // angle V - P
    dx = V[0]-P[0];
    dy = V[1]-P[1];
    dz = V[2]-P[2];
    System.out.println("dx="+dx+"  dy="+dy+"  dz="+dz);
    angx = Math.acos(dx/vlength);
    angy = Math.acos(dy/vlength);
    angz = Math.acos(dz/vlength);
    System.out.println("angle V - P");
    System.out.println("angx="+angx+"  angy="+angy+"  angz="+angz+" rad");
    System.out.println("angx="+degrees(angx)+"  angy="+degrees(angy)+
		       "  angz="+degrees(angz)+" deg");
    System.out.println(" ");




    System.out.println(" ");
    
      
    System.out.println(" \n");
    System.out.println("simple matrix product, no transpose, then transpose \n");
    matmul(My, Mz, Mt);
    matvec(Mt, V, P);
    System.out.println("rotating V(1,1,1) about Y,Z is P("+
		       P[0]+", "+P[1]+", "+P[2]+")");
    matvec(My, V, W);
    matvec(Mz, W, P);
    System.out.println("rotating V(1,1,1) about Y then Z is P("+
  		       P[0]+", "+P[1]+", "+P[2]+")");

    transp(Mz, Mt);
    matmul(My, Mt, Mw);
    matvec(Mw, V, P);
    System.out.println("rotating V(1,1,1) about Y,Z^t is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");

    System.out.println(" \n");
    matmul(Mx, Mz, Mt);
    matvec(Mt, V, P);
    System.out.println("rotating V(1,1,1) about X,Z is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");
    matvec(Mx, V, W);
    matvec(Mz, W, P);
    System.out.println("rotating V(1,1,1) about X,Z is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");
    transp(Mz, Mt);
    matmul(Mx, Mt, Mw);
    matvec(Mw, V, P);
    System.out.println("rotating V(1,1,1) about X,Z^t is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");

    System.out.println(" ");
    matmul(Mx, My, Mt);
    matvec(Mt, V, P);
    System.out.println("rotating V(1,1,1) about X,Y is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");
    matvec(Mx, V, W);
    matvec(My, W, P);
    System.out.println("rotating V(1,1,1) about X then Y is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");

    transp(My, Mt);
    matmul(Mx, Mt, Mw);
    matvec(Mw, V, P);
    System.out.println("rotating V(1,1,1) about X,Y^t is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");

    System.out.println(" \n");
    matmul(Mx, My, Mt);
    matmul(Mz, Mt, Mw);
    matvec(Mw, V, P);
    printm(Mw, " Mw = Mx * My * Mz");
    System.out.println("using Mw = Mx * My * Mz");
    System.out.println("rotating V(1,1,1) about X,Y,Z is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");
    matvec(Mx, V, W);
    matvec(My, W, Q);
    matvec(Mz, Q, P);
    System.out.println("rotating V(1,1,1) about X,Y,Z is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");

    System.out.println(" \n");
    matmul(Mx, My, Mt);
    printm(Mt, "Mx * My");

    matmul(My, Mx, Mw);
    printm(Mw, "My * Mx");
    transp(My, Mt);

    matmul(Mx, Mt, Mw);
    printm(Mw, "Mx * My^t");

    System.out.println(" \n");
    matmul(Mx, My, Mt);
    matmul(Mt, Mz, Mw);
    printm(Mw, "My * Mx * Mz");

    System.out.println(" \n");
    xanga = Pi/8.0;
    yangb = Pi/8.0;
    zangc = Pi/8.0;
    System.out.println("xanga="+xanga+" radians "+degrees(xanga)+" degrees");
    System.out.println("yangb="+yangb+" radians "+degrees(yangb)+" degrees");
    System.out.println("zangc="+zangc+" radians "+degrees(zangc)+" degrees");
    System.out.println("build rotation matrix using these angles");

    identm(Mx);
    Mx[1][1] = Math.cos(xanga);
    Mx[2][2] = Math.cos(xanga);
    Mx[2][1] = Math.sin(xanga);
    Mx[1][2] = - Math.sin(xanga);
    printm(Mx, "rotate about X Mx=");

    identm(My);
    My[0][0] = Math.cos(yangb);
    My[2][2] = Math.cos(yangb);
    My[0][2] = Math.sin(yangb);
    My[2][0] = -Math.sin(yangb);
    printm(My, "rotate about Y My=");

    identm(Mz);
    Mz[0][0] = Math.cos(zangc);
    Mz[1][1] = Math.cos(zangc);
    Mz[1][0] = Math.sin(zangc);
    Mz[0][1] = -Math.sin(zangc);
    printm(Mz, "rotate about Z Mz=");

    matmul(Mx, My, Mxy);
    printm(Mxy, "Mx * My");

    matmul(Mx, Mz, Mxz);
    printm(Mxz, "Mx * Mz");

    System.out.println("build Mxyz simple product");
    matmul(Mx, My, Mt);
    matmul(Mt, Mz, Mxyz);
    printm(Mxyz, "Mx * My * Mz");
    V[0]=1.0;
    V[1]=1.0;
    V[2]=1.0;
    matvec(Mxyz, V, P);
    System.out.println("rotating V(1,1,1) using Mxyz is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");
    System.out.println(" ");
        
    System.out.println("try simple angle on 1,0,0  0,1,0  0,0,1");
    V[0]=1.0;
    V[1]=0.0;
    V[2]=0.0;
    matvec(Mx, V, P);
    System.out.println("rotating V(1,0,0) using Mx is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");
    System.out.println(" ");

    matvec(My, V, P);
    System.out.println("rotating V(1,0,0) using My is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");
    System.out.println(" ");

    matvec(Mz, V, P);
    System.out.println("rotating V(1,0,0) using Mz is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");
    System.out.println(" ");

    matvec(Mxy, V, P);
    System.out.println("rotating V(1,0,0) using Mxy is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");
    System.out.println(" ");

    matvec(Mxz, V, P);
    System.out.println("rotating V(1,0,0) using Mxz is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");
    System.out.println(" ");
    
    V[0]=0.0;
    V[1]=1.0;
    V[2]=0.0;
    matvec(Mx, V, P);
    System.out.println("rotating V(0,1,0) using Mx is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");
    System.out.println(" ");

    matvec(My, V, P);
    System.out.println("rotating V(0,1,0) using My is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");
    System.out.println(" ");
    
    matvec(Mz, V, P);
    System.out.println("rotating V(0,1,1) using Mz is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");
    System.out.println(" ");

    V[0]=0.0;
    V[1]=0.0;
    V[2]=1.0;
    matvec(Mx, V, P);
    System.out.println("rotating V(0,0,1) using Mx is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");

    System.out.println(" ");

    matvec(My, V, P);
    System.out.println("rotating V(0,0,1) using My is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");
    System.out.println(" ");
    
    matvec(Mz, V, P);
    System.out.println("rotating V(0,0,1) using Mz is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");
    System.out.println(" ");

    matvec(Mxy, V, P);
    System.out.println("rotating V(0,0,1) using Mxy is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");
    System.out.println(" ");

    matvec(Mxz, V, P);
    System.out.println("rotating V(0,0,1) using Mxz is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");
    System.out.println(" ");

    V[0]=0.0;
    V[1]=1.0;
    V[2]=1.0;
    matvec(Mx, V, P);
    System.out.println("rotating V(0,1,1) using Mx is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");
    System.out.println(" ");

    matvec(My, V, P);
    System.out.println("rotating V(0,1,1) using My is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");
    System.out.println(" ");
    
    matvec(Mz, V, P);
    System.out.println("rotating V(0,1,1) using Mz is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");
    System.out.println(" ");

    matvec(Mxy, V, P);
    System.out.println("rotating V(0,1,1) using Mxy is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");
    System.out.println(" ");

    matvec(Mx, V, Q);
    matvec(My, Q, P);
    System.out.println("rotating V(0,1,1) using Mx then My is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");
    System.out.println(" ");
    matvec(Mxz, V, P);
    System.out.println("rotating V(0,1,1) using Mxz is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");
    System.out.println(" ");

    matvec(Mx, V, Q);
    matvec(Mz, Q, P);
    System.out.println("rotating V(0,1,1) using Mx then Mz is P("+
                       P[0]+", "+P[1]+", "+P[2]+")");
    System.out.println(" ");

    System.out.println("rotate.java finished \n");
  } // end rotate

  void zerom(double A[][])
  {
    int i, j;
    for(i=0; i<3; i++)
      for(j=0; j<3; j++)
        A[i][j] = 0.0;
  } // end zerom

  void identm(double A[][])
  {
    int i, j;
    for(i=0; i<3; i++)
    {
      for(j=0; j<3; j++)
        A[i][j] = 0.0;
      A[i][i] = 1.0;
    }
  } // end identm

  void printm(double A[][], String txt)
  {
    int i, j;
    System.out.println(" ");
    System.out.println(txt);
    for(i=0; i<3; i++)
    {
      System.out.print("| ");
      for(j=0; j<3; j++)
	  System.out.print(A[i][j]+"   ");
      System.out.println(" |");
    }
  } // end printm

  void matmul(double A[][], double B[][], double C[][])
  {
    int i, j, k;
    for(i=0; i<3; i++)
    {
      for(j=0; j<3; j++)
      {
        C[i][j] = 0.0;
        for(k=0; k<3; k++)
          C[i][j] += A[i][k]*B[k][j];
      }
    }
  } // end matmul

  void matvec(double A[][], double V[], double W[])
  {
    int i, j;
    for(i=0; i<3; i++)
    {
      W[i] = 0.0;
      for(j=0; j<3; j++)
        W[i] += A[i][j]*V[j];
    } 
  } // end matvec

  void transp(double A[][], double B[][])
  {
    int i, j;
    for(i=0; i<3; i++)
      for(j=0; j<3; j++)
        B[i][j] = A[j][i];
  } // end transp

  double degrees(double a)
  {
    return 180.0*a/Pi;
  }

  public static void main (String[] args)
  {
    new rotate();
  }
} // end rotate.java