package org.b2ox.pv3d
{
	import org.papervision3d.core.geom.renderables.Vertex3D;
	import org.papervision3d.core.math.Number3D;
	import org.papervision3d.core.math.Quaternion;

	public class Utils
	{
		/**
		 * Number3DからVertex3Dへ値をコピーする
		 *
		 * @param	v3
		 * @param	n3
		 */
		public static function copyVertex3DfromNumber3D(v3:Vertex3D, n3:Number3D):void { v3.x = n3.x; v3.y = n3.y; v3.z = n3.z; }

		/**
		 * Vertex3DからNumber3Dへ値をコピーする
		 *
		 * @param	n3
		 * @param	v3
		 */
		public static function copyNumber3DfromVertex3D(n3:Number3D, v3:Vertex3D):void { n3.x = v3.x; n3.y = v3.y; n3.z = v3.z; }

		/**
		 * 各座標ごとに線形補間
		 *
		 * @param	v0
		 * @param	v1
		 * @param	x
		 * @param	y
		 * @param	z
		 * @return
		 */
		public static function lerpNumber3D(v0:Number3D, v1:Number3D, x:Number, y:Number, z:Number):Number3D
		{
			return new Number3D((1-x) * v0.x + x * v1.x, (1-y) * v0.y + y * v1.y, (1-z) * v0.z + z * v1.z);
		}

		/**
		 * クォータニオンの値をコピーする
		 *
		 * @param	outQ
		 * @param	q
		 */
		public static function copyQuaternion(outQ:Quaternion, q:Quaternion):void
		{
			outQ.x = q.x; outQ.y = q.y; outQ.z = q.z; outQ.w = q.w;
		}

		/**
		 * クォータニオンの成分値を代入する
		 *
		 * @param	outQ
		 * @param	x
		 * @param	y
		 * @param	z
		 * @param	w
		 */
		public static function resetQuaternion(outQ:Quaternion, x:Number=0, y:Number=0, z:Number=0, w:Number=0):void
		{
			outQ.x = x; outQ.y = y; outQ.z = z; outQ.w = w;
		}

		private static var tmpV:Number3D = new Number3D();
		/**
		 * v0からv1への回転クォータニオンを計算する
		 *
		 * @param	outQ
		 * @param	v0 単位ベクトル
		 * @param	v1 単位ベクトル
		 */
		public static function calcRotation(outQ:Quaternion, v0:Number3D, v1:Number3D):void
		{
			Number3D.cross(v0, v1, tmpV);
			tmpV.normalize();
			outQ.setFromAxisAngle(tmpV.x, tmpV.y, tmpV.z, Math.acos(Number3D.dot(v0, v1)));
		}

		/**
		 * v := Q v P where P := Q.conj, |Q|=1
		 */
		public static function applyQuaternionLeft(v:Number3D, q:Quaternion):void
		{
			/*
			q = (qx,qy,qz)\mu + qw, v = (x,y,z) のとき u\mu = q v\mu q^* とすると u=(ux,uy,uz)
			(記法は http://cid-9da0fa00ac5a8258.skydrive.live.com/self.aspx/.Public/Document/DualQuaternion.pdf を参照)

			注: 以下 qAB=qA*qB と表記する(例: qxy = qx*qy)
			また、Q = qw^2 + qx^2 + qy^2 + qz^2

			ux = (Q - 2(qy^2 + qz^2))x + 2(qxy - qwz)y + 2(qxz + qwy)z
			uy = 2(qxy + qwz)x + (Q - 2(qx^2 + qz^2))y + 2(qyz - qwx)z
			uz = 2(qxz - qwy)x + 2(qyz + qwx)y + (Q - 2(qx^2 + qy^2))z

			Q = 1に限定し、Qab=2*qa*qbと表記すると

			ux = (1 - Qyy - Qzz)x + (Qxy - Qwz)y + (Qxz + Qwy)z
			uy = (Qxy + Qwz)x + (1 - Qxx - Qzz)y + (Qyz - Qwx)z
			uz = (Qxz - Qwy)x + (Qyz + Qwx)y + (1 - Qxx - Qyy)z
			*/
			var tmp:Number = 2 * q.w;
			var Qwx:Number = tmp * q.x;
			var Qwy:Number = tmp * q.y;
			var Qwz:Number = tmp * q.z;
			tmp = 2 * q.x;
			var Qxx:Number = tmp * q.x;
			var Qxy:Number = tmp * q.y;
			var Qxz:Number = tmp * q.z;
			tmp = 2 * q.y;
			var Qyy:Number = tmp * q.y;
			var Qyz:Number = tmp * q.z;
			var Qzz:Number = 2 * q.z * q.z;
			var x:Number = v.x;
			var y:Number = v.y;
			var z:Number = v.z;
			v.x = (1 - Qyy - Qzz) * x + (Qxy - Qwz) * y + (Qxz + Qwy) * z;
			v.y = (Qxy + Qwz) * x + (1 - Qxx - Qzz) * y + (Qyz - Qwx) * z;
			v.z = (Qxz - Qwy) * x + (Qyz + Qwx) * y + (1 - Qxx - Qyy) * z;
		}

		/**
		 * v := P v Q where P := Q.conj, |Q|=1
		 */
		public static function applyQuaternionRight(v:Number3D, q:Quaternion):void
		{
			/* applyQuaternionLeftの計算でqx,qy,qzを-qx,-qy,-qzに置き換えればOK
			* 最終的にはQwx,Qwy,Qwzの符号を入れ替えればOK
			*/
			var tmp:Number = 2 * q.w;
			var Qwx:Number = tmp * q.x;
			var Qwy:Number = tmp * q.y;
			var Qwz:Number = tmp * q.z;
			tmp = 2 * q.x;
			var Qxx:Number = tmp * q.x;
			var Qxy:Number = tmp * q.y;
			var Qxz:Number = tmp * q.z;
			tmp = 2 * q.y;
			var Qyy:Number = tmp * q.y;
			var Qyz:Number = tmp * q.z;
			var Qzz:Number = 2 * q.z * q.z;
			var x:Number = v.x;
			var y:Number = v.y;
			var z:Number = v.z;
			v.x = (1 - Qyy - Qzz) * x + (Qxy + Qwz) * y + (Qxz - Qwy) * z;
			v.y = (Qxy - Qwz) * x + (1 - Qxx - Qzz) * y + (Qyz + Qwx) * z;
			v.z = (Qxz + Qwy) * x + (Qyz - Qwx) * y + (1 - Qxx - Qyy) * z;
		}

		/**
		 * v := k*(P(v-v0)Q + v0') where P := Q.conj, |Q|=1
		 */
		public static function transformVector(v:Number3D, v0:Number3D, v0_:Number3D, q:Quaternion, k:Number=1.0):void
		{
			v.minusEq(v0); // v := v-v0
			applyQuaternionRight(v, q); // v := P v Q
			v.x = k*(v.x + v0_.x); // v := k*(v + v0')
			v.y = k*(v.y + v0_.y);
			v.z = k*(v.z + v0_.z);
		}

		/**
		 * v := k*(P(v-v0)Q + v0') where P := Q.conj, |Q|=1 の計算用
		 *
		 * f = getVectorTransformer(v0, v0', Q);
		 * f(v,k); で上記の計算ができる
		 */
		public static function getVectorTransformer(v0:Number3D, v0_:Number3D, q:Quaternion):Function
		{
			var tmp:Number = 2 * q.w;
			var Qwx:Number = tmp * q.x;
			var Qwy:Number = tmp * q.y;
			var Qwz:Number = tmp * q.z;
			tmp = 2 * q.x;
			var Qxx:Number = tmp * q.x;
			var Qxy:Number = tmp * q.y;
			var Qxz:Number = tmp * q.z;
			tmp = 2 * q.y;
			var Qyy:Number = tmp * q.y;
			var Qyz:Number = tmp * q.z;
			var Qzz:Number = 2 * q.z * q.z;
			var pxx:Number = 1 - Qyy - Qzz;
			var pxy:Number = Qxy + Qwz;
			var pxz:Number = Qxz - Qwy;
			var pyx:Number = Qxy - Qwz;
			var pyy:Number = 1 - Qxx - Qzz;
			var pyz:Number = Qyz + Qwx;
			var pzx:Number = Qxz + Qwy;
			var pzy:Number = Qyz - Qwx;
			var pzz:Number = 1 - Qxx - Qyy;
			return function (tv:Number3D, sv:Vertex3D, k:Number = 1.0):void
			{
				// tv: ターゲット
				// sv: ソース
				var x:Number = sv.x - v0.x;
				var y:Number = sv.y - v0.y;
				var z:Number = sv.z - v0.z;
				tv.x += k*(pxx * x + pxy * y + pxz * z + v0_.x);
				tv.y += k*(pyx * x + pyy * y + pyz * z + v0_.y);
				tv.z += k*(pzx * x + pzy * y + pzz * z + v0_.z);
			}
		}

		/**
		 * クォータニオンからオイラー角を求める
		 * @param	outV
		 * @param	q
		 */
		public static function eulerOfQuaternion(outV:Number3D, q:Quaternion):void
		{
			var v:Number3D = q.toEuler();
			outV.copyFrom(v);
			v = null;
		}

		/**
		 * a x^2 + b x + c = 0の実根をansに格納し、相異なる実根の個数を返す。
		 * @param	a
		 * @param	b
		 * @param	c
		 * @param	ans 事前に2個分の容量を確保しておくこと
		 * @return
		 */
		public static function solveEquation2D(a:Number, b:Number, c:Number, ans:Vector.<Number>):int
		{
			if (a == 0) {
				if (b == 0) return 0;
				ans[0] = -c / b;
				return 1;
			}
			b /= 2*a;
			c /= a;
			var D:Number = b * b - c;
			if (D == 0) {
				ans[0] = -b;
				return 1;
			}
			if (D < 0) return 0;
			// D > 0
			D = Math.sqrt(D);
			ans[0] = -D - b;
			ans[1] = D - b;
			return 2;
		}

		/**
		 * a x^3 + b x^2 + c x + d = 0の実根をansに格納し、相異なる実根の個数を返す。
		 * @param	a
		 * @param	b
		 * @param	c
		 * @param	d
		 * @param	ans 事前に3個分の容量を確保しておくこと
		 * @return
		 */
		public static function solveEquation3D(a:Number, b:Number, c:Number, d:Number, ans:Vector.<Number>):int
		{
			if (a == 0) return solveEquation2D(b, c, d, ans);

			b /= 3*a;
			c /= a;
			d /= a;
			var b2:Number = b * b;
			var n:int = solveEquation3Dsub(c - 3 * b2, b * (2 * b2 - c) + d, ans);
			for (var i:int = 0; i < n; i++) ans[i] -= b;
			return n;
		}

		/**
		 * x^3 + p x + q = 0の実根をansに格納し、相異なる実根の個数を返す。
		 * @param	p
		 * @param	q
		 * @param	ans 事前に3個分の容量を確保しておくこと
		 * @return
		 */
		private static function solveEquation3Dsub(p:Number, q:Number, ans:Vector.<Number>):int
		{
			var q_2:Number = q / 2;
			var q27_2:Number = 27 * q_2;
			var D:Number = q27_2 * q27_2 + 3 * p;
			if (D == 0) {
				if (q == 0) {
					ans[0] = 0;
					return 1;
				} else {
					q_2 = Math.pow(q_2, 1 / 3);
					ans[0] = -2 * q_2;
					ans[1] = q_2;
					return 2;
				}
			}
			if (D > 0) {
				D = Math.sqrt(D);
				ans[0] = (Math.pow(D - q27_2, 1 / 3) + Math.pow( -D - q27_2, 1 / 3)) / 3;
				return 1;
			}
			// D < 0
			var r:Number = 2 * Math.pow( -3 * p, 1 / 6 ) / 3;
			var theta:Number = Math.atan2( -q27_2, Math.sqrt( -D ) ) / 3;
			var pi2_3:Number = Math.PI * 2 / 3;
			ans[0] = r * Math.cos(theta);
			ans[1] = r * Math.cos(theta + pi2_3);
			ans[2] = r * Math.cos(theta - pi2_3);
			return 3;
		}
	}
}

