/*******************************************************************************
 * Copyright (c) 2012, 2013 Association for Decentralized Information Management in
 * Industry THTH ry.
 * All rights reserved. This program and the accompanying materials
 * are made available under the terms of the Eclipse Public License v1.0
 * which accompanies this distribution, and is available at
 * http://www.eclipse.org/legal/epl-v10.html
 *
 * Contributors:
 *     VTT Technical Research Centre of Finland - initial API and implementation
 *******************************************************************************/
package org.simantics.g3d.math;

import javax.vecmath.AxisAngle4d;
import javax.vecmath.Matrix3d;
import javax.vecmath.Matrix4d;
import javax.vecmath.Quat4d;
import javax.vecmath.Tuple2d;
import javax.vecmath.Tuple3d;
import javax.vecmath.Tuple4d;
import javax.vecmath.Vector2d;
import javax.vecmath.Vector2f;
import javax.vecmath.Vector3d;

import org.simantics.g3d.math.EulerTools.Order;


/**
 * Some useful geometry related math functions. Beware, methods may modify their input parameters!
 * 
 * @author Marko Luukkainen
 *
 */
public class MathTools {
	
	public static double NEAR_ZERO = 0.000000001;
	public static double NEAR_HALF = 0.499999999;
	
	public static final Vector3d Z_AXIS = new Vector3d(0.0,0.0,1.0);
	public static final Vector3d Y_AXIS = new Vector3d(0.0,1.0,0.0);
	public static final Vector3d X_AXIS = new Vector3d(1.0,0.0,0.0);
	public static final Vector3d ORIGIN = new Vector3d(0.0,0.0,0.0);
	
	final static double EPS = 1.0e-12;
	
	
	public static boolean equals(double d1, double d2) {
		return Math.abs(d1-d2) < EPS;
	}
	
	public static boolean equals(Tuple3d p1, Tuple3d p2) {
		return distanceSquared(p1, p2) < NEAR_ZERO;
	}
	
	public static boolean equals(Tuple4d p1, Tuple4d p2) {
		return distanceSquared(p1, p2) < NEAR_ZERO;
	}
	
	public static double distance(Tuple3d p1, Tuple3d p2) {
		double dx, dy, dz;

		dx = p2.x - p1.x;
		dy = p2.y - p1.y;
		dz = p2.z - p1.z;
		return Math.sqrt(dx*dx+dy*dy+dz*dz);
	}
	
	public static double distance(Tuple4d p1, Tuple4d p2) {
		double dx, dy, dz, dw;

		dx = p2.x - p1.x;
		dy = p2.y - p1.y;
		dz = p2.z - p1.z;
		dw = p2.w - p1.w;
		return Math.sqrt(dx*dx+dy*dy+dz*dz+dw*dw);
	}
	
	public static double distanceSquared(Tuple3d p1, Tuple3d p2) {
		double dx, dy, dz;

		dx = p2.x - p1.x;
		dy = p2.y - p1.y;
		dz = p2.z - p1.z;
		return dx*dx+dy*dy+dz*dz;
	}
	
	public static double distanceSquared(Tuple4d p1, Tuple4d p2) {
		double dx, dy, dz, dw;

		dx = p2.x - p1.x;
		dy = p2.y - p1.y;
		dz = p2.z - p1.z;
		dw = p2.w - p1.w;
		return dx*dx+dy*dy+dz*dz+dw*dw;
	}
	
	public static boolean isValid(Tuple3d t) {
		return !(Double.isInfinite(t.x) || Double.isNaN(t.x) ||
		         Double.isInfinite(t.y) || Double.isNaN(t.y) ||
		         Double.isInfinite(t.z) || Double.isNaN(t.z));
	}
	
	public static Vector3d closestPointOnEdge(Vector3d point, Vector3d edgePoint1, Vector3d edgePoint2) {
		point.sub(edgePoint1);
		Vector3d v = new Vector3d(edgePoint2);
		v.sub(edgePoint1);
		double t = v.dot(point);
		t /= v.lengthSquared();
		if (t <= 0.0f)
			return edgePoint1;
		if (t >= 1.0f)
			return edgePoint2;
		v.scale(t);
		v.add(edgePoint1);
		return v;   
	}
	
	public static Vector3d closestPointOnStraight(Tuple3d point, Tuple3d straightPoint, Vector3d straightDir) {
		Vector3d v = new Vector3d(point);
		v.sub(straightPoint);
		double t = straightDir.dot(v);
		t /= straightDir.lengthSquared();
		v.set(straightDir);
		v.scale(t);
		v.add(straightPoint);
		return v;   
	}
	
	public static Vector3d closestPointOnStraight(Tuple3d point, Tuple3d straightPoint, Vector3d straightDir, double u[]) {
		Vector3d v = new Vector3d(point);
		v.sub(straightPoint);
		u[0] = straightDir.dot(v);
		u[0] /= straightDir.lengthSquared();
		v.set(straightDir);
		v.scale(u[0]);
		v.add(straightPoint);
		return v;   
	}
	
	public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, Tuple3d planePoint) {
		point.sub(planePoint);
		
		return planeNormal.dot(point);
	}
	
	public static double distanceFromPlane(Vector3d point, Vector3d planeNormal, float d) {
		return (planeNormal.dot(point) + d);
	}
	
	public static Vector3d projectToPlane(Vector3d v, Vector3d planeNormal) {
	    //v.normalize();
	    //planeNormal.normalize();
	    Vector3d t = new Vector3d();
	    t.cross(v,planeNormal);
	    t.cross(planeNormal, t);
	    return t;
	    
	}
	
	public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Tuple3d intersectPoint) {
		intersectPoint.set(planePoint);
		intersectPoint.sub(linePoint);
		double u = planeNormal.dot(new Vector3d(intersectPoint));
		double v = planeNormal.dot(lineDir);
		if (Math.abs(v) < NEAR_ZERO)
			return false;
		u /= v;
		intersectPoint.set(lineDir);
		intersectPoint.scale(u);
		intersectPoint.add(linePoint);
		return true;
	}
	
	public static boolean intersectStraightPlane(Tuple3d linePoint, Vector3d lineDir, Tuple3d planePoint, Vector3d planeNormal, Vector3d intersectPoint, double[] u) {
		intersectPoint.set(planePoint);
		intersectPoint.sub(linePoint);
		u[0] = planeNormal.dot(intersectPoint);
		double v = planeNormal.dot(lineDir);
		if (Math.abs(v) < NEAR_ZERO)
			return false;
		u[0] /= v;
		intersectPoint.set(lineDir);
		intersectPoint.scale(u[0]);
		intersectPoint.add(linePoint);
		return true;
	}
	
	public static boolean intersectLineLine(Tuple3d l1_start,Tuple3d l1_end,Tuple3d l2_start,Tuple3d l2_end,Tuple3d l1_pos, Tuple3d l2_pos) {
			Vector3d p13 = new Vector3d();
			Vector3d p43 = new Vector3d();
			Vector3d p21 = new Vector3d();
			double d1343,d4321,d1321,d4343,d2121;
			double numer,denom;
			p13.sub(l1_start, l2_start);
			p43.sub(l2_end,l2_start);
			if (Math.abs(p43.x)  < NEAR_ZERO && Math.abs(p43.y)  < NEAR_ZERO && Math.abs(p43.z)  < NEAR_ZERO)
				return false;
			p21.sub(l1_end,l1_start);
			if (Math.abs(p21.x)  < NEAR_ZERO && Math.abs(p21.y)  < NEAR_ZERO && Math.abs(p21.z)  < NEAR_ZERO)
				return false;

			d1343 = p13.dot(p43);
			d4321 = p43.dot(p21);
			d1321 = p13.dot(p21);
			d4343 = p43.lengthSquared();
			d2121 = p21.lengthSquared();

			denom = d2121 * d4343 - d4321 * d4321;
			if (Math.abs(denom) < NEAR_ZERO)
				return false;
			numer = d1343 * d4321 - d1321 * d4343;

			double mua = numer / denom;
			double mub = (d1343 + d4321 * mua) / d4343;

			l1_pos.x = l1_start.x + mua * p21.x;
			l1_pos.y = l1_start.y + mua * p21.y;
			l1_pos.z = l1_start.z + mua * p21.z;
			l2_pos.x = l2_start.x + mub * p43.x;
			l2_pos.y = l2_start.y + mub * p43.y;
			l2_pos.z = l2_start.z + mub * p43.z;

			return true;
	}
	
	public static boolean intersectStraightStraight(Tuple3d p1,Vector3d p21,Tuple3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb) {
		Vector3d p13 = new Vector3d();

		double d1343,d4321,d1321,d4343,d2121;
		double numer,denom;
		
		p13.sub(p1, p3);
		if (Math.abs(p43.x)  < NEAR_ZERO && Math.abs(p43.y)  < NEAR_ZERO && Math.abs(p43.z)  < NEAR_ZERO)
			return false;
		if (Math.abs(p21.x)  < NEAR_ZERO && Math.abs(p21.y)  < NEAR_ZERO && Math.abs(p21.z)  < NEAR_ZERO)
			return false;

		d1343 = p13.dot(p43);
		d4321 = p43.dot(p21);
		d1321 = p13.dot(p21);
		d4343 = p43.lengthSquared();
		d2121 = p21.lengthSquared();

		denom = d2121 * d4343 - d4321 * d4321;
		if (Math.abs(denom) < NEAR_ZERO)
			return false;
		numer = d1343 * d4321 - d1321 * d4343;

		double mua = numer / denom;
		double mub = (d1343 + d4321 * mua) / d4343;

		pa.x = p1.x + mua * p21.x;
		pa.y = p1.y + mua * p21.y;
		pa.z = p1.z + mua * p21.z;
		pb.x = p3.x + mub * p43.x;
		pb.y = p3.y + mub * p43.y;
		pb.z = p3.z + mub * p43.z;

		return true;
	}
	
	/**
	 * Calculate the line segment PaPb that is the shortest route between
	 *  two lines P1P2 and P3P4. Calculate also the values of mua and mub where
	 *  Pa = P1 + mua (P2 - P1)
	 *  Pb = P3 + mub (P4 - P3)
	 * @param p1
	 * @param p21
	 * @param p3
	 * @param p43
	 * @param pa
	 * @param pb
	 * @param mu
	 * @return
	 */
	public static boolean intersectStraightStraight(Tuple3d p1,Vector3d p21,Tuple3d p3,Vector3d p43,Tuple3d pa,Tuple3d pb, double mu[]) {
		Vector3d p13 = new Vector3d();

		double d1343,d4321,d1321,d4343,d2121;
		double numer,denom;
		double EPS = 0.001;
		p13.sub(p1, p3);
		if (Math.abs(p43.x)  < EPS && Math.abs(p43.y)  < EPS && Math.abs(p43.z)  < EPS)
			return false;
		if (Math.abs(p21.x)  < EPS && Math.abs(p21.y)  < EPS && Math.abs(p21.z)  < EPS)
			return false;

		d1343 = p13.dot(p43);
		d4321 = p43.dot(p21);
		d1321 = p13.dot(p21);
		d4343 = p43.lengthSquared();
		d2121 = p21.lengthSquared();

		denom = d2121 * d4343 - d4321 * d4321;
		if (Math.abs(denom) < EPS)
			return false;
		numer = d1343 * d4321 - d1321 * d4343;

		mu[0] = numer / denom;
		mu[1] = (d1343 + d4321 * mu[0]) / d4343;

		pa.x = p1.x + mu[0] * p21.x;
		pa.y = p1.y + mu[0] * p21.y;
		pa.z = p1.z + mu[0] * p21.z;
		pb.x = p3.x + mu[1] * p43.x;
		pb.y = p3.y + mu[1] * p43.y;
		pb.z = p3.z + mu[1] * p43.z;

		return true;
	}
	

	
	public static void rotate(Quat4d q, Tuple3d in, Tuple3d out) {
		// p' = q * p * q'
		double tw =	        - q.x*in.x - q.y*in.y - q.z*in.z;
		double tx =  q.w*in.x            + q.y*in.z - q.z*in.y;
		double ty =  q.w*in.y - q.x*in.z            + q.z*in.x;
		double tz =  q.w*in.z + q.x*in.y - q.y*in.x           ;
		
		//temp * q' -> x = -x, y = -y z = -z
		//out.w = tw*q.w + tx*q.x + ty*q.y + tz*q.z;
		out.x =  -tw*q.x + tx*q.w - ty*q.z + tz*q.y;
		out.y =  -tw*q.y + tx*q.z + ty*q.w - tz*q.x;
		out.z =  -tw*q.z - tx*q.y + ty*q.x + tz*q.w;  
	}
	
	public static void getMatrix(Quat4d quat, Matrix3d m) {
			m.m00 = 1.0f - 2.0 * (quat.y * quat.y + quat.z * quat.z);
			m.m01 = 2.0 * (quat.x * quat.y + quat.w * quat.z);
			m.m02 = 2.0 * (quat.x * quat.z - quat.w * quat.y);
			m.m10 = 2.0 * (quat.x * quat.y - quat.w * quat.z);
			m.m11 = 1.0 - 2.0f * (quat.x * quat.x + quat.z * quat.z);
			m.m12 = 2.0 * (quat.y * quat.z + quat.w * quat.x);
			m.m20 = 2.0 * (quat.x * quat.z + quat.w * quat.y);
			m.m21 = 2.0 * (quat.y * quat.z - quat.w * quat.x);
			m.m22 = 1.0 - 2.0f * (quat.x * quat.x + quat.y * quat.y);

	}
	
	public static void getMatrix(Quat4d quat, Matrix4d m) {
		m.setZero();
		m.m00 = 1.0f - 2.0 * (quat.y * quat.y + quat.z * quat.z);
		m.m01 = 2.0 * (quat.x * quat.y + quat.w * quat.z);
		m.m02 = 2.0 * (quat.x * quat.z - quat.w * quat.y);
		m.m10 = 2.0 * (quat.x * quat.y - quat.w * quat.z);
		m.m11 = 1.0 - 2.0f * (quat.x * quat.x + quat.z * quat.z);
		m.m12 = 2.0 * (quat.y * quat.z + quat.w * quat.x);
		m.m20 = 2.0 * (quat.x * quat.z + quat.w * quat.y);
		m.m21 = 2.0 * (quat.y * quat.z - quat.w * quat.x);
		m.m22 = 1.0 - 2.0f * (quat.x * quat.x + quat.y * quat.y);
		m.m33 = 1.0;
	}
	
	private static double q[] = new double[3];
	private static int nxt[] = { 1, 2, 0 };
	/**
	 * Converts Matrix to Quaternion
	 * 
	 * Note: non-thread safe.
	 * 
	 * @param mat
	 * @param quat
	 */
	public static void getQuat(Matrix3d mat, Quat4d quat) {
		double tr = mat.m00 + mat.m11 + mat.m22;
		if (tr > 0.0) {
			double s = Math.sqrt(tr + 1.0);
			quat.w = 0.5 * s;
			s = 0.5 / s;
			quat.x = (mat.m21 - mat.m12) * s;
			quat.y = (mat.m02 - mat.m20) * s;
			quat.z = (mat.m10 - mat.m01) * s;
		} else {
			int i = 0, j, k;
			if (mat.m11 > mat.m00)
				i = 1;
			if (mat.m22 > mat.getElement(i, i))
				i = 2;
			

			j = nxt[i];
			k = nxt[j];

			double s = Math.sqrt((mat.getElement(i, i) - (mat.getElement(j, j) + mat.getElement(k, k))) + 1.0);

			q[i] = s * 0.5;

			if (Math.abs(s) > 0.001)
				s = 0.5 / s;

			quat.w = (mat.getElement(k, j) - mat.getElement(j, k)) * s;
			q[j] = (mat.getElement(j, i) + mat.getElement(i, j)) * s;
			q[k] = (mat.getElement(k, i) + mat.getElement(i, k)) * s;

			quat.x = q[0];
			quat.y = q[1];
			quat.z = q[2];
		}
	}
	
	public static Quat4d getQuat(Matrix3d mat) {
		Quat4d q = new Quat4d();
		getQuat(mat, q);
		return q;
	}
	
	public static AxisAngle4d getFromPseudoEuler(Vector3d euler) {
		AxisAngle4d aa = new AxisAngle4d();
		aa.angle = euler.length();
		Vector3d normal = new Vector3d(euler);
		if (aa.angle > NEAR_ZERO) {
			normal.normalize();
			aa.x = normal.x;
			aa.y = normal.y;
			aa.z = normal.z;
		} else {
			aa.x = 1.0;
			aa.y = 0.0;
			aa.z = 0.0;
		}
		
		return aa;
	}
	
	public static Vector3d getPseudoEuler(AxisAngle4d aa) {
		Vector3d euler = new Vector3d(aa.x,aa.y,aa.z);
		euler.scale(aa.angle);
		return euler;
	}
	
	
	public static void getQuat(Vector3d euler, Quat4d quat)  {
		Quat4d q = EulerTools.getQuatFromEuler(Order.YXZ, euler.y,euler.x,euler.z);
		quat.set(q);
	// http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Conversion_formulae_between_formalisms
	// Using the x-convention, the 3-1-3 Euler angles phi, theta and psi (around the Z, X and again the Z-axis)   
//	   quat.x = -Math.cos((euler.x - euler.z)*0.5)*Math.sin(euler.y*0.5);
//	   quat.y = -Math.sin((euler.x - euler.z)*0.5)*Math.sin(euler.y*0.5);
//	   quat.z = -Math.sin((euler.x + euler.z)*0.5)*Math.cos(euler.y*0.5);
//	   quat.w = Math.sin((euler.x + euler.z)*0.5)*Math.cos(euler.y*0.5);
		
		// http://www.euclideanspace.com/maths/geometry/rotations/conversions/eulerToQuaternion/index.htm
		// Y, Z, X order
//	    double c1 = Math.cos(euler.y*0.5);
//	    double s1 = Math.sin(euler.y*0.5);
//	    double c2 = Math.cos(euler.z*0.5);
//	    double s2 = Math.sin(euler.z*0.5);
//	    double c3 = Math.cos(euler.x*0.5);
//	    double s3 = Math.sin(euler.x*0.5);
//	    double c1c2 = c1*c2;
//	    double s1s2 = s1*s2;
//	    quat.w =c1c2*c3 - s1s2*s3;
//	    quat.x =c1c2*s3 + s1s2*c3;
//	    quat.y =s1*c2*c3 + c1*s2*s3;
//	    quat.z =c1*s2*c3 - s1*c2*s3;

//	    Quat4d q2 = EulerTools.getQuatFromEuler(Order.YZX, euler.y,euler.z,euler.x);
//	    System.out.println("Q " + quat + " Q2 " + q2);
//	   double c1 = Math.cos(euler.y);
//	    double s1 = Math.sin(euler.y);
//	    double c2 = Math.cos(euler.z);
//	    double s2 = Math.sin(euler.z);
//	    double c3 = Math.cos(euler.x);
//	    double s3 = Math.sin(euler.x);
//	    quat.w = Math.sqrt(1.0 + c1 * c2 + c1*c3 - s1 * s2 * s3 + c2*c3) / 2.0;
//	    double w4 = (4.0 * quat.w);
//	    quat.x = (c2 * s3 + c1 * s3 + s1 * s2 * c3) / w4 ;
//	    quat.y = (s1 * c2 + s1 * c3 + c1 * s2 * s3) / w4 ;
//	    quat.z = (-s1 * s3 + c1 * s2 * c3 +s2) / w4 ;
	}
	
	

	
	public static void getEuler(Quat4d quat,Vector3d euler)  {
		Vector3d e = EulerTools.getEulerFromQuat(Order.YXZ, quat);
		euler.x = e.y;
		euler.y = e.x;
		euler.z = e.z;
		
		// http://en.wikipedia.org/wiki/Rotation_formalisms_in_three_dimensions#Conversion_formulae_between_formalisms
//	   euler.x = Math.atan2(quat.x * quat.z + quat.y* quat.w, quat.y*quat.z - quat.x * quat.w);
//	   euler.y = Math.acos(-square(quat.x) - square(quat.y) + square(quat.z) + square(quat.w));
//	   euler.z = -Math.atan2(quat.x * quat.z - quat.y* quat.w, quat.y*quat.z + quat.x * quat.w);
		
		// http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/index.htm
	// Y, Z, X order
//	   double test = quat.x * quat.y + quat.z * quat.w;
//	   if (test > NEAR_HALF) {
//		   euler.y = 2.0 * Math.atan2(quat.x,quat.w);
//		   euler.z = Math.PI * 0.5;
//		   euler.x = 0.0;
//	   } else if (test < -NEAR_HALF) {
//		   euler.y = -2.0 * Math.atan2(quat.x,quat.w);
//		   euler.z = -Math.PI * 0.5;
//		   euler.x = 0.0;
//	   } else {
//		   double sqx = square(quat.x);
//		   double sqy = square(quat.y);
//		   double sqz = square(quat.z);
//		   euler.y = Math.atan2(2.0*(quat.y*quat.w-quat.x*quat.z), 1.0 - 2.0*(sqy-sqz));
//		   euler.z = Math.asin(2.0*test);
//		   euler.x = Math.atan2(2.0*(quat.x*quat.w-quat.y*quat.z), 1.0 - 2.0*(sqx-sqz));
//		   System.out.println(euler + " " + EulerTools.getEulerFromQuat(Order.YXZ, quat) +  " " + quat);
//	   }
//	   double sqw = quat.w*quat.w;
//	    double sqx = quat.x*quat.x;
//	    double sqy = quat.y*quat.y;
//	    double sqz = quat.z*quat.z;
//		double unit = sqx + sqy + sqz + sqw; // if normalised is one, otherwise is correction factor
//		double test = quat.x*quat.y + quat.z*quat.w;
//		if (test > 0.499*unit) { // singularity at north pole
//			euler.y = 2 * Math.atan2(quat.x,quat.w);
//			euler.z = Math.PI/2;
//			euler.x = 0;
//			return;
//		}
//		if (test < -0.499*unit) { // singularity at south pole
//			euler.y = -2 * Math.atan2(quat.x,quat.w);
//			euler.z = -Math.PI/2;
//			euler.x = 0;
//			return;
//		}
//		euler.y = Math.atan2(2*quat.y*quat.w-2*quat.x*quat.z , sqx - sqy - sqz + sqw);
//		euler.z = Math.asin(2*test/unit);
//		euler.x = Math.atan2(2*quat.x*quat.w-2*quat.y*quat.z , -sqx + sqy - sqz + sqw);
	}
	
	public static Quat4d getQuat(Vector3d euler) {
		Quat4d q = new Quat4d();
		getQuat(euler,q);
		return q;
	}
	
	
	public static Vector3d getEuler(Quat4d quat) {
		Vector3d v = new Vector3d();
		getEuler(quat, v);
		return v;
	}
	
	public static Quat4d getQuat(AxisAngle4d aa) {
		Quat4d q = new Quat4d();
		getQuat(aa, q);
		return q;
	}
	
	public static AxisAngle4d getAxisAngle(Quat4d q) {
		AxisAngle4d aa = new AxisAngle4d();
		double mag = q.x * q.x + q.y * q.y + q.z * q.z;

		if (mag > EPS) {
			mag = Math.sqrt(mag);
			aa.angle = 2.0 * Math.atan2(mag, q.w);
			mag = 1.0 / mag;
			aa.x = q.x * mag;
			aa.y = q.y * mag;
			aa.z = q.z * mag;
			
		} else {
			aa.x = 0.0;
			aa.y = 1.0;
			aa.z = 0.0;
			aa.angle = 0.0;
		}
		// aa.set(q);
		return aa;
	}
	
	public static Quat4d getIdentityQuat() {
		return new Quat4d(0, 0, 0, 1);
	}
	
	public static void getQuat(AxisAngle4d aa, Quat4d q) {
		double mag,amag;
		// Quat = cos(theta/2) + sin(theta/2)(roation_axis) 
		
		amag = Math.sqrt( aa.x*aa.x + aa.y*aa.y + aa.z*aa.z);
		if( amag < NEAR_ZERO ) {
			q.w = 1.0;
			q.x = 0.0;
			q.y = 0.0;
			q.z = 0.0;
		} else {  
			amag = 1.0/amag; 
			double a2 = aa.angle * 0.5;
			mag = Math.sin(a2);
			q.w = Math.cos(a2);
			q.x = aa.x*amag*mag;
			q.y = aa.y*amag*mag;
			q.z = aa.z*amag*mag;
		}
	}
	
	
	/*
	 * Cohen-Sutherland
	 */
	
	private static final int IN = 0;
	private static final int LEFT = 1;
	private static final int RIGHT = 2;
	private static final int BOTTOM = 4;
	private static final int TOP = 8;
	
	
	private static int bitcode(Vector2f p1, Vector2f min, Vector2f max) {
		int code = IN;
		if (p1.x < min.x)
			code |= LEFT;
		else if (p1.x > max.x)
			code |= RIGHT;
		if (p1.y < min.y)
			code |= BOTTOM;
		else if (p1.y > max.y)
			code |= TOP;
		return code;
	}
	
	public static boolean clipLineRectangle(Vector2f p1,Vector2f p2, Vector2f min, Vector2f max, Vector2f r1, Vector2f r2) {
		while (true) {
			int o1 = bitcode(p1, min, max);
			int o2 = bitcode(p2, min, max);
			int and = o1 & o2;
			int or = o1 | o2;
			if (and != IN) {
				return false;
			}
			if (or == IN) {
				r1.set(p1);
				r2.set(p2);
				return true;
			}
			if (o1 == IN) {
				Vector2f t = p1;
				p1 = p2;
				p2 = t;
				int t2 = o1;
				o1 = o2;
				o2 = t2;
			}
			if ((o1 & TOP) != IN) {
				float t = (max.y - p1.y) / (p2.y - p1.y);
				p1.x += t * (p2.x - p1.x);
				p1.y = max.y;
			} else if ((o1 & BOTTOM) != IN) {
				float t = (min.y - p1.y) / (p2.y - p1.y);
				p1.x += t * (p2.x - p1.x);
				p1.y = min.y;
			} else if ((o1 & LEFT) != IN) {
				float t = (min.x - p1.x) / (p2.x - p1.x);
				p1.y += t * (p2.y - p1.y);
				p1.x = min.x;
			} else if ((o1 & RIGHT) != IN) {
				float t = (max.x - p1.x) / (p2.x - p1.x);
				p1.y += t * (p2.y - p1.y);
				p1.x = max.x;
			} else {
				throw new RuntimeException("Error in clipping code");
			}
		}
		
	}
	
	public static double square(double d) {
		return d * d;
	}
	
	
	public static void multiplyOrientation(AxisAngle4d aa, AxisAngle4d rot)  {
		Quat4d q1 = new Quat4d();
		getQuat(aa, q1);
		Quat4d q2 = new Quat4d();
		getQuat(rot, q2);
		q2.mul(q1);
		rot.set(q2);
	}
	
	public static double radToDeg(double rad) {
		return (rad / Math.PI) * 180.0;
	}
	
	public static double degToRad(double deg) {
		return (deg / 180.0) * Math.PI;
	}
	
	public static double clamp(double min, double max,double v) {
		if (v < min)
			return min;
		if (v > max)
			return max;
		return v;
	}
	
	public static AxisAngle4d createRotation(Vector3d original, Vector3d rotated) { 
		AxisAngle4d result = new AxisAngle4d();
		if (createRotation(original, rotated, result))
			return result;
		return null;
	}
	
	
	public static void setIdentity(Quat4d q) {
		q.w = 1.0;
		q.x = 0.0;
		q.y = 0.0;
		q.z = 0.0;
	}
	
	public static void setIdentity(AxisAngle4d aa) {
		aa.angle = 0.0;
		aa.x = 0.0;
		aa.y = 1.0;
		aa.z = 0.0;
	}
	
	public static void set(Matrix3d mat, double m00, double m01, double m02,
			double m10, double m11, double m12, double m20, double m21,
			double m22) {
		mat.m00 = m00;
		mat.m01 = m01;
		mat.m02 = m02;

		mat.m10 = m10;
		mat.m11 = m11;
		mat.m12 = m12;

		mat.m20 = m20;
		mat.m21 = m21;
		mat.m22 = m22;
	}
	
	public static void set(Matrix4d mat, double[] v) {
		mat.m00 = v[0];
		mat.m01 = v[1];
		mat.m02 = v[2];
		mat.m03 = v[3];

		mat.m10 = v[4];
		mat.m11 = v[5];
		mat.m12 = v[6];
		mat.m13 = v[7];

		mat.m20 = v[8];
		mat.m21 = v[9];
		mat.m22 = v[10];
		mat.m23 = v[11];

		mat.m30 = v[12];
		mat.m31 = v[13];
		mat.m32 = v[14];
		mat.m33 = v[15];

	}
	
	public static boolean createRotation(Vector3d original, Vector3d rotated, AxisAngle4d result) {
		
		if (rotated.lengthSquared() > 0.01)
			rotated.normalize();
		else
			return false;
		double d = original.dot(rotated);
		if (d > 0.9999) {
			// original and rotated are parallel, pointing at the same direction
			result.angle = 0.0;
			result.x = 0.0;
			result.y = 1.0;
			result.z = 0.0;
		} else if (d < -0.9999) {
			// original and rotated are parallel, pointing at the opposite direction 
			Vector3d a = Z_AXIS;
			if (Math.abs(a.dot(original)) > 0.8 )
				a = Y_AXIS;
			result.set(a, Math.PI);
		} else {
			double angle = original.angle(rotated);
			Vector3d axis = new Vector3d();
			axis.cross(original, rotated);
			result.set(axis,angle);
		}
		return true;
	}
	
	public static boolean createRotation(Vector3d original, Vector3d rotated, Quat4d result) {
		
		if (rotated.lengthSquared() > 0.01)
			rotated.normalize();
		else
			return false;
		double d = original.dot(rotated);
		if (d > 0.9999) {
			// original and rotated are parallel, pointing at the same direction
			result.w = 1.0;
			result.x = 0.0;
			result.y = 0.0;
			result.z = 0.0;
		} else if (d < -0.9999) {
			// original and rotated are parallel, pointing at the opposite direction 
			Vector3d a = Z_AXIS;
			if (Math.abs(a.dot(original)) > 0.8 )
				a = Y_AXIS;
			getQuat(a, Math.PI, result);
			
		} else {
			double angle = original.angle(rotated);
			Vector3d axis = new Vector3d();
			axis.cross(original, rotated);
			getQuat(axis, angle, result);
		}
		return true;
	}
	
	public static boolean createRotation(Vector3d original, Vector3d rotated, Vector3d axis, AxisAngle4d result) {
        
        if (rotated.lengthSquared() > 0.01)
            rotated.normalize();
        else
            return false;
        if (original.lengthSquared() > 0.01)
            original.normalize();
        else
            return false;
        if (axis.lengthSquared() > 0.01)
            axis.normalize();
        else
            return false;
        double d = original.dot(rotated);
        if (d > 0.9999) {
            // original and rotated are parallel, pointing at the same direction
            result.angle = 0.0;
            result.x = axis.x;
            result.y = axis.y;
            result.z = axis.z;
        } else if (d < -0.9999) {
            // original and rotated are parallel, pointing at the opposite direction 
            result.angle = Math.PI;
            result.x = axis.x;
            result.y = axis.y;
            result.z = axis.z;
        } else {
            // Project vectors to Axis plane
            Vector3d p1 = projectToPlane(original, axis);
            Vector3d p2 = projectToPlane(rotated, axis);
            // Create vectors where z-axis is plane normal
            Quat4d q = getQuat(createRotation(axis, Z_AXIS));
            Vector3d t1 = new Vector3d();
            Vector3d t2 = new Vector3d();
            rotate(q, p1, t1);
            rotate(q, p2, t2);
            // Calculate angles on z-axis plane.
            double a1 = Math.atan2(t1.y, t1.x);
            double a2 = Math.atan2(t2.y, t2.x);
            result.set(axis,a2-a1);
            
        }
        return true;
    }
	
	public static void getQuat(Vector3d axis, double angle, Quat4d q)
	{
		double mag,amag;
		// Quat = cos(theta/2) + sin(theta/2)(roation_axis) 
		
		amag = Math.sqrt( axis.x*axis.x + axis.y*axis.y + axis.z*axis.z);
		if( amag < EPS ) {
			q.w = 1.0;
			q.x = 0.0;
			q.y = 0.0;
			q.z = 0.0;
		} else {  
			amag = 1.0/amag; 
			double a2 = angle*0.5;
			mag = Math.sin(a2);
			q.w = Math.cos(a2);
			q.x = axis.x*amag*mag;
			q.y = axis.y*amag*mag;
			q.z = axis.z*amag*mag;
		}
	
	}
	
	/**
	 * Linear interpolation of quaternions. Result IS set to q1.
	 * @param q1
	 * @param q2
	 * @param alpha
	 */
	public static void lip(Quat4d q1, Quat4d q2, double alpha) {
		double s1 = 1.0 - alpha;
		double s2 = alpha;
		q1.scale(s1);
		mad(q1,q2,s2);
		q1.normalize();
	}
	
	public static double dot(Quat4d q1, Quat4d q2) {
		return q1.x * q2.x + q1.y * q2.y + q1.z * q2.z + q1.w * q2.w;
	}
	
	public static void mad(Tuple3d q1, Tuple3d q2, double s2) {
		q1.x += q2.x * s2;
		q1.y += q2.y * s2;
		q1.z += q2.z * s2;
	}
	
	public static void mad(Quat4d q1, Quat4d q2, double s2) {
		q1.x += q2.x * s2;
		q1.y += q2.y * s2;
		q1.z += q2.z * s2;
		q1.w += q2.w * s2;
	}
	
	/**
	 * Slerp
	 * 
	 * Sets results to q1. Modifies q2.
	 * 
	 * @param q1
	 * @param q2
	 * @param alpha
	 */
	public static void sip(Quat4d q1, Quat4d q2, double alpha) {
		double cosom = dot(q1,q2);
		if (cosom < 0.0) {
			cosom = -cosom;
			q2.negate();
		}
		
		if (cosom > 0.9999) {
			q2.sub(q1);
			q2.scale(alpha);
			q1.add(q2);
			q1.normalize();
			return;
		}
		double theta_0 = Math.acos(cosom);
		double theta = theta_0 * alpha;
		Quat4d t = new Quat4d(q1);
		t.scale(-cosom);
		t.add(q2);
		t.normalize();
		t.scale(Math.sin(theta));
		q1.scale(Math.cos(theta));
		q1.add(t);
	}
	
	
	public static void rotate(double angle, Tuple2d v1, Tuple2d v2) {
		// TODO : verify implementation
		double sin = Math.sin(angle);
		if (sin == 1.0) {
			v2.x = v1.y;
			v2.y = -v1.x;
		} else if (sin == -1.0) {
			v2.x = -v1.y;
			v2.y = v1.x;
		} else {
			double cos = Math.cos(angle);
			if (cos == -1.0) {
				v2.x = -v1.x;
				v2.y = -v1.y;
			} else if (cos != 1.0) {
				v2.x= v1.x * cos + v1.y * -sin;
				v2.y= v1.x* sin + v1.y *cos;
			}
		}	
	}
	
	public static Tuple3d getPosRot(double m3x2[]) {
		Vector3d t = new Vector3d();
		t.x = m3x2[4];
		t.y = m3x2[5];
		
		
		Vector2d v2 = new Vector2d(1,0);
		Vector2d v = new Vector2d();
		// use rotation of (1,0) to calculate the rotation component
		v.x  =  m3x2[0];
		v.y  =  m3x2[2];
		double a1 = v2.angle(v);
		if (v.y < 0) {
			t.z = a1;
		} else {
			t.z = Math.PI*2.0 - a1;
		}
		return t;
	}
	
	public static Matrix4d glFrustum(double l, double r, double b, double t, double n, double f) {
		Matrix4d mat = new Matrix4d();
		mat.m00 = 2.0 * n / (r - l);
		mat.m11 = 2.0 * n / (t - b);
		mat.m02 = (r+l) / (r-l);
		mat.m12 = (t+b) / (t-b);
		mat.m22 = -(f+n) / (f-n);
		mat.m23 = -(2.0 *f * n) / (f-n);
		mat.m32 = -1.0;
		return mat;
	}
	
	public static Matrix4d glOrtho(double l, double r, double b, double t, double n, double f) {
		Matrix4d mat = new Matrix4d();
		mat.m00 = 2.0 / (r - l);
		mat.m11 = 2.0 / (t - b);
		mat.m22 = -2.0 / (f-n);
		mat.m33 =  1.0;
		mat.m03 = -(r+l)/(r-l);
		mat.m13 = -(t+b)/(t-b);
		mat.m23 = -(f+n)/(f-n);
		return mat;
	}

	/**
	 * Round the number to a given number of decimals, if the rounded result contains at least three
	 * zero trailing decimal places within the rounded result.
	 */
	public static double round(double value, int nsig) {
		if (Math.abs(value) < NEAR_ZERO)
			return 0.0;
		
		int decimals = (int) Math.round(Math.log10(value));
		int shift = nsig - decimals;
		double multiplier = Math.pow(10.0, shift);
		long roundedValue = Math.round(value * multiplier);
		if (roundedValue % 1000 == 0) {
			// Rounding results in at least three zeroes at the end
			return roundedValue / multiplier;
		} else {
			// Number was not close to a shorter decimal representation, return it as is
			return value;
		}
	}
}