package org.ntlab.radishforandroidstudio.java3d;


public class Transform3D {
	double[][] mat;
	double[] rot;
	double[] scales = new double[] {1.0,1.0,1.0};
	
	 static final double EPSILON_ABSOLUTE = 1.0e-5;
	
	public Transform3D() {
		mat = new double[][]{{1.0,0.0,0.0,0.0},{0.0,1.0,0.0,0.0},{0.0,0.0,1.0,0.0},{0.0,0.0,0.0,1.0}};
	}
	
	public Transform3D(Matrix4d m1) {
		mat = new double[][]{{m1.m00,m1.m01,m1.m02,m1.m03},{m1.m10,m1.m11,m1.m12,m1.m13},{m1.m20,m1.m21,m1.m22,m1.m23},{m1.m30,m1.m31,m1.m32,m1.m33}};
	}
	
	public Transform3D(Transform3D transform) {
		mat = new double[4][4];
		set(transform);
	}
	
	public void set(Transform3D transform) {
		for (int i = 0;i < 4; i++) {
			for (int j = 0;j < 4; j++) {
				mat[i][j] = transform.mat[i][j];
			}
		}
		scales[0] = transform.scales[0];
		scales[1] = transform.scales[1];
		scales[2] = transform.scales[2];
	}
	
	/**
	 * this = t1 * t2
	 * 
	 * @param t1
	 * @param t2
	 */
	public void mul(Transform3D t1, Transform3D t2) {
		for (int i = 0; i < 4; i++) {
			double[] row = row(i, t1.mat);
			for (int j = 0; j < 4; j++) {
				mat[i][j] = 0.0;
				double[] col = col(j, t2.mat); 
				for (int k = 0; k < 4; k++) {
					mat[i][j] += row[k] * col[k]; 
				}
			}
		}
	}

	/**
	 * this = this * t1
	 * 
	 * @param t1
	 */
	public void mul(Transform3D t1) {
		this.mul(new Transform3D(this), t1);
	}

	/** 4*4行列の行を返す */
	private double[] row(int row, double[][] mat) {
		return new double[] {mat[row][0], mat[row][1], mat[row][2], mat[row][3]};
	}
	
	/** 4*4行列の列を返す */
	private double[] col(int col, double[][] mat) {
		return new double[] {mat[0][col], mat[1][col], mat[2][col], mat[3][col]};
	}

	public void set(Vector3d v) {
		mat[0][3] = v.x;
		mat[1][3] = v.y;
		mat[2][3] = v.z;
	}

	/** 4*4の行列を16の1次元配列で返す */
	public float[] getMatrix() {
		float[] result = new float[16];
		for (int i = 0; i < 4; i++) {
			for (int j = 0; j < 4; j++) {
				result[j + 4 * i] = (float)(mat[j][i]);
			}	
		}
		return result;
	}

	/** 対角行列をスカラ倍する */
	public void setScale(double s) {
		for (int i = 0; i < 3; i++) {
			mat[i][i] = s;
			scales[i] = s;
		}
	}
	
	/** 対角行列をvectorの成分倍する */
	public void setScale(Vector3d vector3d) {
		scales[0] = mat[0][0] = vector3d.x;
		scales[1] = mat[1][1] = vector3d.y;
		scales[2] = mat[2][2] = vector3d.z;
	}
	
	/** 任意の単位ベクトルまわりに回転する */
	public void setRotation(AxisAngle4d t) {
		double vx = t.x;
		double vy = t.y;
		double vz = t.z;
		double sin = Math.sin(t.angle);
		double cos = Math.cos(t.angle);

		mat[0][0] = (Math.pow(vx, 2.0)) * (1 - cos) + cos;
		mat[0][1] = ((vx * vy) * (1 - cos)) - (vz * sin);
		mat[0][2] = ((vz * vx) * (1 - cos)) + (vy * sin);
		mat[1][0] = ((vx * vy) * (1 - cos)) + (vz * sin);
		mat[1][1] = (Math.pow(vy, 2.0)) * (1 - cos) + cos;
		mat[1][2] = ((vy * vz) * (1 - cos)) - (vx * sin);
		mat[2][0] = ((vz * vx) * (1 - cos)) - (vy * sin);
		mat[2][1] = ((vy * vz) * (1 - cos)) + (vx * sin);
		mat[2][2] = (Math.pow(vz, 2.0)) * (1 - cos) + cos;
	}
	
	/** 任意の単位ベクトルまわりに回転する */
	public void setRotation(Quat4d q) {
//		double vx = q.x;
//		double vy = q.y;
//		double vz = q.z;
//		double sin = Math.sin(q.w);
//		double cos = Math.cos(q.w);
//		double cosm = 1 - cos;
//
//		mat[0][0] = (vx * vx) * cosm + cos;
//		mat[0][1] = ((vx * vy) * cosm) - (vz * sin);
//		mat[0][2] = ((vz * vx) * cosm) + (vy * sin);
//		mat[1][0] = ((vx * vy) * cosm) + (vz * sin);
//		mat[1][1] = (vy * vy) * cosm + cos;
//		mat[1][2] = ((vy * vz) * cosm) - (vx * sin);
//		mat[2][0] = ((vz * vx) * cosm) - (vy * sin);
//		mat[2][1] = ((vy * vz) * cosm) + (vx * sin);
//		mat[2][2] = (vz * vz) * cosm + cos;

		mat[0][0] = (1.0 - 2.0 * q.y * q.y - 2.0 * q.z * q.z) * scales[0];
		mat[1][0] = (2.0 * (q.x * q.y + q.w * q.z)) * scales[0];
		mat[2][0] = (2.0 * (q.x * q.z - q.w * q.y)) * scales[0];
		mat[0][1] = (2.0 * (q.x * q.y - q.w * q.z)) * scales[1];
		mat[1][1] = (1.0 - 2.0 * q.x * q.x - 2.0 * q.z * q.z) * scales[1];
		mat[2][1] = (2.0 * (q.y * q.z + q.w * q.x)) * scales[1];
		mat[0][2] = (2.0 * (q.x * q.z + q.w * q.y)) * scales[2];
		mat[1][2] = (2.0 * (q.y * q.z - q.w * q.x)) * scales[2];
		mat[2][2] = (1.0 - 2.0 * q.x * q.x - 2.0 * q.y * q.y) * scales[2];
	}
	
	public void rotX(double angle) {
		double sinAngle = Math.sin(angle);
		double cosAngle = Math.cos(angle);
		
		mat[0][0] = 1.0;
		mat[0][1] = 0.0;
		mat[0][2] = 0.0;
		mat[0][3] = 0.0;

		mat[1][0] = 0.0;
		mat[1][1] = cosAngle;
		mat[1][2] = -sinAngle;
		mat[1][3] = 0.0;
		
		mat[2][0] = 0.0;
		mat[2][1] = sinAngle;
		mat[2][2] = cosAngle;
		mat[2][3] = 0.0;

		mat[3][0] = 0.0;
		mat[3][1] = 0.0;
		mat[3][2] = 0.0;
		mat[3][3] = 1.0;
	}
	
	public void rotY(double angle) {
		double sinAngle = Math.sin(angle);
		double cosAngle = Math.cos(angle);
		
		mat[0][0] = cosAngle;
		mat[0][1] =  0.0;
		mat[0][2] = sinAngle;
		mat[0][3] = 0.0;

		mat[1][0] = 0.0;
		mat[1][1] = 1.0;
		mat[1][2] = 0.0;
		mat[1][3] = 0.0;
		
		mat[2][0] = -sinAngle;
		mat[2][1] = 0.0;
		mat[2][2] = cosAngle;
		mat[2][3] = 0.0;

		mat[3][0] = 0.0;
		mat[3][1] = 0.0;
		mat[3][2] = 0.0;
		mat[3][3] = 1.0;
	}

	public void rotZ(double angle) {
		double sinAngle = Math.sin(angle);
		double cosAngle = Math.cos(angle);
		
		mat[0][0] = cosAngle;
		mat[0][1] = -sinAngle;
		mat[0][2] = 0.0;
		mat[0][3] = 0.0;

		mat[1][0] = sinAngle;
		mat[1][1] = cosAngle;
		mat[1][2] = 0.0;
		mat[1][3] = 0.0;
		
		mat[2][0] = 0.0;
		mat[2][1] = 0.0;
		mat[2][2] = 1.0;
		mat[2][3] = 0.0;

		mat[3][0] = 0.0;
		mat[3][1] = 0.0;
		mat[3][2] = 0.0;
		mat[3][3] = 1.0;
	}

	/** 引数の成分を格納する */
	public void get(Matrix4d mat4d) {
		mat4d.m00 = mat[0][0];
		mat4d.m01 = mat[0][1];
		mat4d.m02 = mat[0][2];
		mat4d.m03 = mat[0][3];
		mat4d.m10 = mat[1][0];
		mat4d.m11 = mat[1][1];
		mat4d.m12 = mat[1][2];
		mat4d.m13 = mat[1][3];
		mat4d.m20 = mat[2][0];
		mat4d.m21 = mat[2][1];
		mat4d.m22 = mat[2][2];
		mat4d.m23 = mat[2][3];
		mat4d.m30 = mat[3][0];
		mat4d.m31 = mat[3][1];
		mat4d.m32 = mat[3][2];
		mat4d.m33 = mat[3][3];
	}
	

	public void set(double[] matrix) {
		mat[0][0] = matrix[0];
		mat[0][1] = matrix[1];
		mat[0][2] = matrix[2];
		mat[0][3] = matrix[3];
		mat[1][0] = matrix[4];
		mat[1][1] = matrix[5];
		mat[1][2] = matrix[6];
		mat[1][3] = matrix[7];
		mat[2][0] = matrix[8];
		mat[2][1] = matrix[9];
		mat[2][2] = matrix[10];
		mat[2][3] = matrix[11];
		mat[3][0] = matrix[12];
		mat[3][1] = matrix[13];
		mat[3][2] = matrix[14];
		mat[3][3] = matrix[15];
		scales[0] = 1.0;
		scales[1] = 1.0;
		scales[2] = 1.0;
	}

	public void set(AxisAngle4d a1) {
		double mag = Math.sqrt( a1.x*a1.x + a1.y*a1.y + a1.z*a1.z);
		if (almostZero(mag)) {
			setIdentity();
		} else {
			mag = 1.0/mag;
			double ax = a1.x*mag;
			double ay = a1.y*mag;
			double az = a1.z*mag;
			double sinTheta = Math.sin((double)a1.angle);
			double cosTheta = Math.cos((double)a1.angle);
			double t = 1.0 - cosTheta;
			double xz = ax * az;
			double xy = ax * ay;
			double yz = ay * az;
			mat[0][0] = t * ax * ax + cosTheta;
			mat[0][1] = t * xy - sinTheta * az;
			mat[0][2] = t * xz + sinTheta * ay;
			mat[0][3] = 0.0;
			mat[1][0] = t * xy + sinTheta * az;
			mat[1][1] = t * ay * ay + cosTheta;
			mat[1][2] = t * yz - sinTheta * ax;
			mat[1][3] = 0.0;
			mat[2][0] = t * xz - sinTheta * ay;
			mat[2][1] = t * yz + sinTheta * ax;
			mat[2][2] = t * az * az + cosTheta;
			mat[2][3] = 0.0;
			mat[3][0] = 0.0;
			mat[3][1] = 0.0;
			mat[3][2] = 0.0;
			mat[3][3] = 1.0;
		}
	}

	public void set(Quat4d q1) {
		mat[0][0] = (1.0f - 2.0f*q1.y*q1.y - 2.0f*q1.z*q1.z);
		mat[1][0] = (2.0f*(q1.x*q1.y + q1.w*q1.z));
		mat[2][0] = (2.0f*(q1.x*q1.z - q1.w*q1.y));
		 
		mat[0][1] = (2.0f*(q1.x*q1.y - q1.w*q1.z));
		mat[1][1] = (1.0f - 2.0f*q1.x*q1.x - 2.0f*q1.z*q1.z);
		mat[2][1] = (2.0f*(q1.y*q1.z + q1.w*q1.x));
		 
		mat[0][2] = (2.0f*(q1.x*q1.z + q1.w*q1.y));
		mat[1][2] = (2.0f*(q1.y*q1.z - q1.w*q1.x));
		mat[2][2] = (1.0f - 2.0f*q1.x*q1.x - 2.0f*q1.y*q1.y);
		 
		mat[0][3] =  0.0;
		mat[1][3] =  0.0;
		mat[2][3] = 0.0;
		 
		mat[3][0] = 0.0;
		mat[3][1] = 0.0;
		mat[3][2] = 0.0;
		mat[3][3] = 1.0;
		 
		// Issue 253: set all dirty bits if input is infinity or NaN
//		if (isInfOrNaN(q1)) {
//			dirtyBits = ALL_DIRTY;
//			return;
//		}
		 
//		dirtyBits = CLASSIFY_BIT | SCALE_BIT | ROTATION_BIT;
//		type = RIGID | CONGRUENT | AFFINE | ORTHO;
	}

	/** Vector3dをTransform3dで変換する */
	public void transform(Vector3d v) {
		double vx =  mat[0][0]*v.x + mat[0][1]*v.y + mat[0][2]*v.z;// + mat[0][3] * 1.0;
		double vy = mat[1][0]*v.x + mat[1][1]*v.y + mat[1][2]*v.z;// + mat[1][3] * 1.0;
		v.z = mat[2][0]*v.x + mat[2][1]*v.y + mat[2][2]*v.z;// + mat[2][3] * 1.0;
		v.x = vx;
		v.y = vy;
	}
		
	private static final boolean almostZero(double a) {
		return ((a < EPSILON_ABSOLUTE) && (a > -EPSILON_ABSOLUTE));
	}
	
	public final void setIdentity() {
		mat[0][0] = 1.0; mat[0][1] = 0.0; mat[0][2] = 0.0; mat[0][3] = 0.0;
		mat[1][0] = 0.0; mat[1][1] = 1.0; mat[1][2] = 0.0; mat[1][3] = 0.0;
		mat[2][0] = 0.0; mat[2][1] = 0.0; mat[2][2] = 1.0; mat[2][3] = 0.0;
		mat[3][0] = 0.0; mat[3][1] = 0.0; mat[3][2] = 0.0; mat[3][3] = 1.0;
		}

	public void invert() {
		Matrix4d m = new Matrix4d();
		m.m00 = mat[0][0]; m.m01 = mat[0][1]; m.m02 = mat[0][2]; m.m03 = mat[0][3];
		m.m10 = mat[1][0]; m.m11 = mat[1][1]; m.m12 = mat[1][2]; m.m13 = mat[1][3];
		m.m20 = mat[2][0]; m.m21 = mat[2][1]; m.m22 = mat[2][2]; m.m23 = mat[2][3];
		m.m30 = mat[3][0]; m.m31 = mat[3][1]; m.m32 = mat[3][2]; m.m33 = mat[3][3];
		m.invert();
		mat = new double[][]{{m.m00,m.m01,m.m02,m.m03},{m.m10,m.m11,m.m12,m.m13},{m.m20,m.m21,m.m22,m.m23},{m.m30,m.m31,m.m32,m.m33}};
	}

	public void transpose() {
		for (int i = 0; i < 3; i++) {
			for (int j = 1 + i; j < 4; j++) {
				double m = mat[j][i];
				mat[j][i] = mat[i][j];
				mat[i][j] = m;
			}
		}
	}

	public void transform(Point3d point) {
		Point3d p = new Point3d();
		p.x = mat[0][0] * point.x + mat[0][1] * point.y + mat[0][2] * point.z;
		p.y = mat[1][0] * point.x + mat[1][1] * point.y + mat[1][2] * point.z;
		p.z = mat[2][0] * point.x + mat[2][1] * point.y + mat[2][2] * point.z;
		point.x = p.x;
		point.y = p.y;
		point.z = p.z;
	}
	
	/**
	 * 平面に対してしか使ってはいけない
	 * @param plane
	 */
	public void transform(Vector4d plane) {
//		double x = mat[0][0] * plane.x + mat[0][1] * plane.y + mat[0][2] * plane.z;
//		double y = mat[1][0] * plane.x + mat[1][1] * plane.y + mat[1][2] * plane.z;
//		double z = mat[2][0] * plane.x + mat[2][1] * plane.y + mat[2][2] * plane.z;
//		
//		double vx = x * plane.w - mat[0][3];
//		double vy = y * plane.w - mat[1][3];
//		double vz = z * plane.w - mat[2][3];
//
//		plane.x = x;
//		plane.y = y;
//		plane.z = z;
//		plane.w = x * vx + y * vy + z * vz;
//		
		double x = (mat[0][0]*plane.x + mat[0][1]*plane.y
				+ mat[0][2]*plane.z + mat[0][3]*plane.w);
		double y = (mat[1][0]*plane.x + mat[1][1]*plane.y
				+ mat[1][2]*plane.z + mat[1][3]*plane.w);
		double z = (mat[2][0]*plane.x + mat[2][1]*plane.y
				+ mat[2][2]*plane.z + mat[2][3]*plane.w);
		plane.w = (mat[3][0]*plane.x + mat[3][1]*plane.y
				+ mat[3][2]*plane.z + mat[3][3]*plane.w);
		plane.x = x;
		plane.y = y;
		plane.z = z;
	}

	public void setTranslation(Vector3d v) {
		mat[3][0] = v.x;
		mat[3][1] = v.y;
		mat[3][2] = v.z;
	}
	
}