import Vertex3D;

public class Matrix3D {
private float m[];

public Matrix3D()      // null constructor allows for extension
  {
    m = new float[16];
    loadIdentity();
  }

public Matrix3D(ZRaster r)
  {
    m = new float[16];
    float w = r.width / 2;
    float h = r.height / 2;
    float d = ZRaster.MAXZ / 2;
    m[ 0] = w;  m[ 1] = 0;  m[ 2] = 0;  m[ 3] = w;
    m[ 4] = 0;  m[ 5] = h;  m[ 6] = 0;  m[ 7] = h;
    m[ 8] = 0;  m[ 9] = 0;  m[10] = d;  m[11] = d;
    m[12] = 0;  m[13] = 0;  m[14] = 0;  m[15] = 1;
  }

public Matrix3D(Matrix3D copy)    // makes a copy of the matrix
  {
    m = new float[16];
    copy(copy);
  }


  /*
    ... Methods for setting and getting matrix elements ...
    */
public void set(int j, int i, float val)
  {
    m[4*j+i] = val;
  }

public float get(int j, int i)
  {
    return m[4*j+i];
  }

protected void set(int i, float val)
  {
    m[i] = val;
  }
    
protected float get(int i)
  {
    return m[i];
  }


public final void copy(Matrix3D src)
  {
    //System.arraycopy(src, 0, m, 0, 16);
    for (int i = 0; i < 16 ; i++)
      m[i]=src.get(i);
  }

public void transform(Vertex3D in[], Vertex3D out[], int vertices)
  {
    Matrix3D at = new Matrix3D();
    at.copy(this);
    at.invert3x3submatrix();
    at.transpose();
    
    for (int i = 0; i < vertices; i++) {
      out[i].x = m[0]*in[i].x + m[1]*in[i].y + m[2]*in[i].z + m[3]*in[i].w;
      out[i].y = m[4]*in[i].x + m[5]*in[i].y + m[6]*in[i].z + m[7]*in[i].w;
      out[i].z = m[8]*in[i].x + m[9]*in[i].y + m[10]*in[i].z + m[11]*in[i].w;
      out[i].w = m[12]*in[i].x + m[13]*in[i].y + m[14]*in[i].z + m[15]*in[i].w;

      if (in[i].hasNormal) {
	// transform normals here
	out[i].nx = at.get(0)*in[i].nx + at.get(1)*in[i].nx + at.get( 2)*in[i].nx ;
	out[i].ny = at.get(4)*in[i].ny + at.get(5)*in[i].ny + at.get( 6)*in[i].ny ;
	out[i].nz = at.get(8)*in[i].nz + at.get(9)*in[i].nz + at.get(10)*in[i].nz ;
      }
    }
  }

public void invert3x3submatrix()
  {
    float det = m[0]*m[5]*m[10] + m[1]*m[6]*m[8] + m[2]*m[4]*m[9] - m[8]*m[5]*m[2] - m[9]*m[6]*m[0] - m[10]*m[4]*m[1];
    det = 1;///det;

    float detfor00 = m[5]*m[10] - m[9]*m[6];
    float detfor01 = m[1]*m[10] - m[9]*m[2];
    float detfor02 = m[1]*m[6] -  m[5]*m[2];
    
    float detfor10 = m[4]*m[10] - m[8]*m[6];
    float detfor11 = m[0]*m[10] - m[8]*m[2];
    float detfor12 = m[0]*m[6] -  m[4]*m[2];
    
    float detfor20 = m[4]*m[9] - m[8]*m[5];
    float detfor21 = m[0]*m[9] - m[8]*m[1];
    float detfor22 = m[0]*m[5] - m[4]*m[1];
      
    m[0] = 1*detfor00 *det;
    m[1] = -1*detfor01 *det;
    m[2] = 1*detfor02 *det;
    m[3] = 0;

    m[4] = -1*detfor10 *det;
    m[5] = 1*detfor11 *det;
    m[6] = -1*detfor12 *det;
    m[7] = 0;

    m[8] = 1*detfor20 *det;
    m[9] = -1*detfor21 *det;
    m[10] = 1*detfor22 *det;
    m[11] = 0;
    
    m[12] = 0;
    m[13] = 0;
    m[14] = 0;
    m[15] = 1;
  }
public void transpose()
  {
    Matrix3D temp = new Matrix3D();
    temp.set(0,m[ 0]);
    temp.set(1,m[ 4]);
    temp.set(2,m[ 8]);
    temp.set(3,m[12]);
    
    temp.set(4,m[1]);
    temp.set(5,m[5]);
    temp.set(6,m[9]);
    temp.set(7,m[13]);

    temp.set(8,m[2]);
    temp.set(9,m[6]);
    temp.set(10,m[10]);
    temp.set(11,m[14]);

    temp.set(12,m[3]);
    temp.set(13,m[7]);
    temp.set(14,m[11]);
    temp.set(15,m[15]);

    copy(temp);
  }

public Vertex3D transform(Vertex3D v)
  {
    float x, y, z, w;
    x = m[0]*v.x + m[1]*v.y + m[2]*v.z + m[3]*v.w;
    y = m[4]*v.x + m[5]*v.y + m[6]*v.z + m[7]*v.w;
    z = m[8]*v.x + m[9]*v.y + m[10]*v.z + m[11]*v.w;
    w = m[12]*v.x + m[13]*v.y + m[14]*v.z + m[15]*v.w;

    w = 1 / w;
    Vertex3D result = new Vertex3D(x*w, y*w, z*w);
 
    if (v.hasNormal) {
      // transform normals here
    }
        
    return result;
  }

public final void compose(Matrix3D s)
  {
    float t0, t1, t2, t3;
    for (int i = 0; i < 16; i += 4) {
      t0 = m[i  ];
      t1 = m[i+1];
      t2 = m[i+2];
      t3 = m[i+3];
      m[i  ] = t0*s.get(0) + t1*s.get(4) + t2*s.get( 8) + t3*s.get(12);
      m[i+1] = t0*s.get(1) + t1*s.get(5) + t2*s.get( 9) + t3*s.get(13);
      m[i+2] = t0*s.get(2) + t1*s.get(6) + t2*s.get(10) + t3*s.get(14);
      m[i+3] = t0*s.get(3) + t1*s.get(7) + t2*s.get(11) + t3*s.get(15);
    }
  }

public void loadIdentity()
  {
    for (int i = 0; i < 16; i++)
      if ((i >> 2) == (i & 3))
	m[i] = 1;
      else
	m[i] = 0;
  }

public void translate(float tx, float ty, float tz)
  {
    m[ 3] += m[ 0]*tx + m[ 1]*ty + m[ 2]*tz;
    m[ 7] += m[ 4]*tx + m[ 5]*ty + m[ 6]*tz;
    m[11] += m[ 8]*tx + m[ 9]*ty + m[10]*tz;
    m[15] += m[12]*tx + m[13]*ty + m[14]*tz;
  }

public void scale(float sx, float sy, float sz)
  {
    m[ 0] *= sx; m[ 1] *= sy; m[ 2] *= sz;
    m[ 4] *= sx; m[ 5] *= sy; m[ 6] *= sz;
    m[ 8] *= sx; m[ 9] *= sy; m[10] *= sz;
    m[12] *= sx; m[13] *= sy; m[14] *= sz;
  }

public void rotate(float ax, float ay, float az, float angle)
  {
    float t0, t1, t2;

    if (angle == 0) return;          // return with m unmodified

    t0 = ax*ax + ay*ay + az*az;
    if (t0 == 0) return;

    float cosx = (float) Math.cos(angle);
    float sinx = (float) Math.sin(angle);
    t0 = 1f / ((float) Math.sqrt(t0));
    ax *= t0;
    ay *= t0;
    az *= t0;
    t0 = 1f - cosx;

    float r11 = ax*ax*t0 + cosx;
    float r22 = ay*ay*t0 + cosx;
    float r33 = az*az*t0 + cosx;

    t1 = ax*ay*t0;
    t2 = az*sinx;
    float r12 = t1 - t2;
    float r21 = t1 + t2;

    t1 = ax*az*t0;
    t2 = ay*sinx;
    float r13 = t1 + t2;
    float r31 = t1 - t2;

    t1 = ay*az*t0;
    t2 = ax*sinx;
    float r23 = t1 - t2;
    float r32 = t1 + t2;

    for (int i = 0; i < 16; i += 4) {
      t0 = m[i];
      t1 = m[i+1];
      t2 = m[i+2];
      m[i  ] = t0*r11 + t1*r21 + t2*r31;
      m[i+1] = t0*r12 + t1*r22 + t2*r32;
      m[i+2] = t0*r13 + t1*r23 + t2*r33;
    }
  }

public void lookAt(float eyex, float eyey, float eyez,
		   float atx,  float aty,  float atz,
		   float upx,  float upy,  float upz)
  {
    float t0, t1, t2;

    /*
      .... a unit vector along the line of sight ....
      */
    atx -= eyex;
    aty -= eyey;
    atz -= eyez;

    t0 = atx*atx + aty*aty + atz*atz;
    if (t0 == 0) return;                // at and eye at same point
    t0 = (float) (1 / Math.sqrt(t0));
    atx *= t0;
    aty *= t0;
    atz *= t0;

    /*
      .... a unit vector to the right ....
      */
    float rightx, righty, rightz;
    rightx = aty*upz - atz*upy;
    righty = atz*upx - atx*upz;
    rightz = atx*upy - aty*upx;
    t0 = rightx*rightx + righty*righty + rightz*rightz;
    if (t0 == 0) return;                // up is the same as at
    t0 = (float) (1 / Math.sqrt(t0));
    rightx *= t0;
    righty *= t0;
    rightz *= t0;


    /*
      .... a unit up vector ....
      */
    upx = righty*atz - rightz*aty;
    upy = rightz*atx - rightx*atz;
    upz = rightx*aty - righty*atx;


    /*
      .... find camera translation ....
      */
    float tx, ty, tz;
    tx = rightx*eyex + righty*eyey + rightz*eyez;
    ty = upx*eyex + upy*eyey + upz*eyez;
    tz = atx*eyex + aty*eyey + atz*eyez;

    /*
      .... do transform ....
      */
    for (int i = 0; i < 16; i += 4) {
      t0 = m[i];
      t1 = m[i+1];
      t2 = m[i+2];
      m[i  ] = t0*rightx + t1*upx - t2*atx;
      m[i+1] = t0*righty + t1*upy - t2*aty;
      m[i+2] = t0*rightz + t1*upz - t2*atz;
      m[i+3] -= t0*tx + t1*ty - t2*tz;
    }
  }

public void perspective(float left, float right,
			float bottom, float top,
			float near, float far)
  {
    float t0, t1, t2, t3;

    t0 = 1f / (right - left);
    t1 = 1f / (bottom - top);
    t2 = 1f / (far - near);

    float m13 = -t0*(right + left);
    float m23 = -t1*(bottom + top);
    float m33 = t2*(far + near);

    near *= 2;
    float m11 = t0*near;
    float m22 = t1*near;
    float m34 = -t2*far*near;

    for (int i = 0; i < 16; i += 4) {
      t0 = m[i];
      t1 = m[i+1];
      t2 = m[i+2];
      m[i  ] = t0*m11;
      m[i+1] = t1*m22;
      m[i+2] = t0*m13 + t1*m23 + t2*m33 + m[i+3];
      m[i+3] = t2*m34;
    }
  }
    
public void orthographic(float left, float right,
			 float bottom, float top,
			 float near, float far)
  {
    float t0, t1, t2, t3;

    t0 = 1f / (right - left);
    t1 = 1f / (bottom - top);
    t2 = 1f / (far - near);

    float m11 = 2*t0;
    float m22 = 2*t1;
    float m33 = 2*t2;
    float m14 = -t0*(right + left);
    float m24 = -t1*(bottom + top);
    float m34 = -t2*(far + near);

    for (int i = 0; i < 16; i += 4) {
      t0 = m[i];
      t1 = m[i+1];
      t2 = m[i+2];
      m[i  ] = t0*m11;
      m[i+1] = t1*m22;
      m[i+2] = t2*m33;
      m[i+3] = t0*m14 + t1*m24 + t2*m34 + m[i+3];
    }
  }

public String toString()
  {
    return ("[ ["+m[ 0]+", "+m[ 1]+", "+m[ 2]+", "+m[ 3]+" ], ["+
	    m[ 4]+", "+m[ 5]+", "+m[ 6]+", "+m[ 7]+" ], ["+
	    m[ 8]+", "+m[ 9]+", "+m[10]+", "+m[11]+" ], ["+
	    m[12]+", "+m[13]+", "+m[14]+", "+m[15]+" ] ]");
  }
}