/*
 *	clipquat.c
 *
 *	クォータニオンライブラリを実装します。
 *
 *	CLiP - Common Library for P/ECE
 *	Copyright (C) 2006 Naoyuki Sawa
 *
 *	* Mon Sep 11 01:20:26 JST 2006 Naoyuki Sawa
 *	- 作成開始。
 *	* Thu Sep 14 13:34:52 JST 2006 Naoyuki Sawa
 *	- 1st リリース。
 */
#include "clip.h"

/*****************************************************************************
 *	quaternion
 *****************************************************************************/

const quaternion QUATERNION_0 = {
	.x = fild(0),
	.y = fild(0),
	.z = fild(0),
	.w = fild(0),
};
const quaternion QUATERNION_1 = {
	.x = fild(0),
	.y = fild(0),
	.z = fild(0),
	.w = fild(1),
};

quaternion
qadd(quaternion a, quaternion b)
{
	/* Qx = Ax + Bx
	 * Qy = Ay + By
	 * Qz = Az + Bz
	 * Qw = Aw + Bw
	 */
	return (quaternion){
		.x = fadd(a.x, b.x),
		.y = fadd(a.y, b.y),
		.z = fadd(a.z, b.z),
		.w = fadd(a.w, b.w),
	};
}

quaternion
qneg(quaternion a)
{
	/* Qx = -Ax
	 * Qy = -Ay
	 * Qz = -Az
	 * Qw = -Aw
	 */
	return (quaternion){
		.x = fchs(a.x),
		.y = fchs(a.y),
		.z = fchs(a.z),
		.w = fchs(a.w),
	};
}

quaternion
qmul(quaternion a, quaternion b)
{
	/* Qx = Ax * Bw + Aw * Bx + Ay * Bz - Az * By
	 * Qy = Ay * Bw + Aw * By + Az * Bx - Ax * Bz
	 * Qz = Az * Bw + Aw * Bz + Ax * By - Ay * Bx
	 * Qw = Aw * Bw - Ax * Bx - Ay * By - Az * Bz
	 */
	return (quaternion){
		.x = fsub(fadd(fadd(fmul(a.x, b.w), fmul(a.w, b.x)), fmul(a.y, b.z)), fmul(a.z, b.y)),
		.y = fsub(fadd(fadd(fmul(a.y, b.w), fmul(a.w, b.y)), fmul(a.z, b.x)), fmul(a.x, b.z)),
		.z = fsub(fadd(fadd(fmul(a.z, b.w), fmul(a.w, b.z)), fmul(a.x, b.y)), fmul(a.y, b.x)),
		.w = fsub(fsub(fsub(fmul(a.w, b.w), fmul(a.x, b.x)), fmul(a.y, b.y)), fmul(a.z, b.z)),
	};
}

quaternion
qinv(quaternion a)
{
	/* ||A||^2 =        Ax^2+Ay^2+Az^2+Aw^2
	 *         = {0,0,0,Ax^2+Ay^2+Az^2+Aw^2}
	 *         = {-Ax,-Ay,-Az,Aw} * {Ax,Ay,Az,Aw}
	 *         = A*               * A
	 * より、
	 * A^-1    = A* / ||A||^2
	 */
	return qfdiv(qconj(a), qnorm2(a));
}

quaternion
qfmul(quaternion a, fixed b)
{
	/* Qx = Ax * b
	 * Qy = Ay * b
	 * Qz = Az * b
	 * Qw = Aw * b
	 */
	return (quaternion){
		.x = fmul(a.x, b),
		.y = fmul(a.y, b),
		.z = fmul(a.z, b),
		.w = fmul(a.w, b),
	};
}

quaternion
qimul(quaternion a, int b)
{
	/* Qx = Ax * b
	 * Qy = Ay * b
	 * Qz = Az * b
	 * Qw = Aw * b
	 */
	return (quaternion){
		.x = fimul(a.x, b),
		.y = fimul(a.y, b),
		.z = fimul(a.z, b),
		.w = fimul(a.w, b),
	};
}

quaternion
qfdiv(quaternion a, fixed b)
{
	/* Qx = Ax / b
	 * Qy = Ay / b
	 * Qz = Az / b
	 * Qw = Aw / b
	 */
	return (quaternion){
		.x = fdiv(a.x, b),
		.y = fdiv(a.y, b),
		.z = fdiv(a.z, b),
		.w = fdiv(a.w, b),
	};
}

quaternion
qidiv(quaternion a, int b)
{
	/* Qx = Ax / b
	 * Qy = Ay / b
	 * Qz = Az / b
	 * Qw = Aw / b
	 */
	return (quaternion){
		.x = fidiv(a.x, b),
		.y = fidiv(a.y, b),
		.z = fidiv(a.z, b),
		.w = fidiv(a.w, b),
	};
}

quaternion
qconj(quaternion a)
{
	/* Qx = -Ax
	 * Qy = -Ay
	 * Qz = -Az
	 * Qw =  Aw
	 */
	return (quaternion){
		.x = fchs(a.x),
		.y = fchs(a.y),
		.z = fchs(a.z),
		.w =      a.w ,
	};
}

fixed
qnorm(quaternion a)
{
	/* ||A|| = √(||A||^2)
	 */
	return fsqrt(qnorm2(a));
}

fixed
qnorm2(quaternion a)
{
	/* ||A||^2 =        Ax^2+Ay^2+Az^2+Aw^2
	 *         = {0,0,0,Ax^2+Ay^2+Az^2+Aw^2}
	 *         = {-Ax,-Ay,-Az,Aw} * {Ax,Ay,Az,Aw}
	 *         = A*               * A
	 */
	return qdot(a, a);		/* Ax^2+Ay^2+Az^2+Aw^2 */
	//↑↓結果はどちらでも同じです。
	//return qmul(qconj(a), a).w;	/* A* * A */
}

quaternion
qnormal(quaternion a)
{
	/* Q = A / ||A||
	 */
	return qfdiv(a, qnorm(a));
}

fixed
qdot(quaternion a, quaternion b)
{
	/* A・B = Ax*Bx + Ay*By + Az*Bz + Aw*Bw
	 *      = (A * B* + B * A*) / 2
	 */
	return fadd(fadd(fadd(fmul(a.x, b.x), fmul(a.y, b.y)), fmul(a.z, b.z)), fmul(a.w, b.w));	/* Ax*Bx + Ay*By + Az*Bz + Aw*Bw */
	//↑↓結果はどちらでも同じです。
	//return qidiv(qadd(qmul(a, qconj(b)), qmul(b, qconj(a))), 2).w;				/* (A * B* + B * A*) / 2 */
}

quaternion
qrot(fixed x, fixed y, fixed z, fixed d)
{
	/* Qx = Ax * sin(θ/2)
	 * Qy = Ay * sin(θ/2)
	 * Qz = Az * sin(θ/2)
	 * Qw =      cos(θ/2)
	 */
	vector v = vnormal((vector){ x, y, z }); /* 回転軸ベクトルを正規化 */
	fidiv_(d, 2);
	vfmul_(v, fsin(d));
	return (quaternion){
		.x = v.x,
		.y = v.y,
		.z = v.z,
		.w = fcos(d),
	};
}

quaternion
qslerp(quaternion a, quaternion b, fixed t)
{
	fixed c;
	fixed s;
	fixed st;
	fixed s1_t;
	fixed d;
	fixed dt;
	fixed d1_t;

//{{誤り
//	/* 回転を表\すクォータニオンq1,q2をパラメータtで球面線形補間し、回転を表\すクォータニオンqを求めるには、
//	 *       q1 sin((1-t)θ) + q2 sin(tθ)
//	 *   q = -----------------------------   (θ: q1とq2の成す角度 … 以下のコメント参照)
//	 *                   sinθ            
//	 *-------------------------------------------------------------------------------------------------------------------------------
//	 * qslerp()は、クォータニオンa,bが回転を表\すクォータニオンであると仮定して、球面線形補間を行います。
//	 * 従って、||a||=||b||=1 と見なして、正規化を行いません。
//	 * また、回転を表\すクォータニオンはθ/2を元に作成されているので、両者の成す角度を得るには cos^-1(a・b) を二倍する必要があります。
//	 * もし、クォータニオンa,bが位置を表\すクォータニオンあった場合は、cos^-1(a・b) を二倍する必要はありません。
//	 * 従って、この関数は回転を表\すクォータニオンの球面線形補間専用であり、位置を表\すクォータニオンの球面線形補間には利用できません。
//	 *-------------------------------------------------------------------------------------------------------------------------------
//	 */
//	c = qdot(a, b);			/* cosθ/2 = ||a||||b||cosθ/2 = a・b */
//	d = facos(c);			/*    θ/2                            */
//	fimul_(d, 2);			/*    θ                              */
//	s = fsin(d);			/* sinθ                              */
//	if(s) {				/* aとbが同じ方向(=球面線形補間不要)、または、真逆の方向(=球面線形補間不可能\) ならば処理しない */
//		dt   = fmul(d,  t);	/*                            t θ           */
//		d1_t = fsub(d, dt);	/*        (1-t)θ                            */
//		st   = fsin(dt  );	/*                     sin(   t θ)          */
//		s1_t = fsin(d1_t);	/*    sin((1-t)θ)                           */
//		qfmul_(a, s1_t);	/*  a sin((1-t)θ)                           */
//		qfmul_(b, st  );	/*                   b sin(   t θ)          */
//		qadd_(a, b);		/*  a sin((1-t)θ) + b sin(   t θ)          */
//		qfdiv_(a, s);		/* (a sin((1-t)θ) + b sin(   t θ)) / sinθ */
//	}
//}}誤り

	/* 回転を表\すクォータニオンq1,q2をパラメータtで球面線形補間し、回転を表\すクォータニオンqを求めるには、
	 *       q1 sin((1-t)θ) + q2 sin(tθ)
	 *   q = -----------------------------   (θ: q1とq2の成す角度 … 以下のコメント参照)
	 *                   sinθ            
	 *-------------------------------------------------------------------------------------------------------------------------------
	 * q1とq2が回転を表\すクォータニオンである場合、qaとq2の内積は、両者の成す角度の半分(θ/2)となります。
	 * 一見、二倍してθに戻す必要があるように見え、Web上のプログラムでもそうしているものが散見されるのですが、二倍してはいけません。
	 * 二倍せずに、実質θ/2のままで進めると、回転を表\すクォータニオンの正しい球面線形補間結果になります。
	 * なぜこうなるのか、まだいまいち理解できていません。(回転を表\すクォータニオンは終始一貫してθ/2が基準だから、戻してはいけない?)
	 * 結局、回転を表\すクォータニオンの補間も、位置を表\すクォータニオン補間も、正規化の不要/必要の違いを除けば、同じ手順となります。
	 *-------------------------------------------------------------------------------------------------------------------------------
	 */
	c = qdot(a, b);			// cosθ = ||a||||b||cosθ/2 = a・b   ただし、||a||=||b||=1 と仮定します。
	d = facos(c);			//    θ
	s = fsin(d);			// sinθ
	if(s) {				// aとbが同じ方向(=球面線形補間不要)、または、真逆の方向(=球面線形補間不可能\)ならば処理しません。
		dt   = fmul(d,  t);	//                            t θ
		d1_t = fsub(d, dt);	//        (1-t)θ
		st   = fsin(dt  );	//                     sin(   t θ)
		s1_t = fsin(d1_t);	//    sin((1-t)θ)
		qfmul_(a, s1_t);	//  a sin((1-t)θ)
		qfmul_(b, st  );	//                   b sin(   t θ)
		qadd_(a, b);		//  a sin((1-t)θ) + b sin(   t θ)
		qfdiv_(a, s);		// (a sin((1-t)θ) + b sin(   t θ)) / sinθ
	}

	return a;
}

vector
vrotq(vector v, quaternion q)
{
	/* q = 回転を表\すクォータニオン
	 * p = 位置を表\すクォータニオン
	 * のとき、qを使ってpを回転し、回転後の位置を表\すクォータニオンp'を求めるには、
	 * p' = q p q*
	 *-----------------------------------------------------------------------------------------------------------------------------------------------------------------------
	 * * Tue Sep 12 03:39:47 JST 2006 Naoyuki Sawa
	 * - 「p' = q* p q」とする資料が多いですが、「p' = q* p q」では逆回転になってしまいます。
	 *   「p' = q p q*」とすれば、正しい回転になりました。(実験より)
	 * - 「3D-CGプログラマーのためのクォータニオン入門」(工学社)のサンプルプログラムは、「p' = q* p q」となっていますが、乗算(quat_mul())の仕様を変えて結果を合わせています。
	 *   従って、正しい回転になります。
	 *   結果は正しいのですが、やや無理やりな感じがします。
	 * - 「70秒で分る、使える、四元数・4元数・クォータニオン・Quaternionで回転」(http://staff.aist.go.jp/toru-nakata/quaternion.html)のサンプルプログラムは、
	 *   「p' = q* p q」、かつ、乗算(Kakezan())の仕様もそのままです。
	 *   従って、逆回転になってしまいます。
	 * - 「クォータニオンによる座標変換」(http://www.purose.net/~y_kim/QuaternionTransform.pdf)の解説は、「p' = q p q*」としています。
	 *   従って、正しい回転になります。
	 *   この資料のみ、DirectX方式の行列計算(ベクトルの右から行列をかける)を用いているので、他の資料との違いは単にその理由によるものかも知れないのですが、
	 *   実際に試してみると、OpenGL形式の行列計算(ベクトルの左から行列をかける)であっても、「p' = q p q*」の方がつじつまが合い、正しい回転になるように思います。
	 *   そもそも、行列計算の方式(左/右)が違っても、クォータニオンの記述は同じなのですから、クォータニオン同士の乗算の並び順には関係が無いのではないでしょうか？
	 *-----------------------------------------------------------------------------------------------------------------------------------------------------------------------
	 */
	quaternion p = {
		.x = v.x,
		.y = v.y,
		.z = v.z,
		.w = fild(0),
	};
	p = qmul(qmul(q, p), qconj(q));
	return (vector){
		.x = p.x,
		.y = p.y,
		.z = p.z,
	};
}

matrix
mrotq(matrix m, quaternion q)
{
	/* 位置を表\すクォータニオンPと、回転を表\すクォータニオンQがあり、
	 *	Q = { Ax,Ay,Az,Aw }
	 *	P = { Px,Py,Pz, 0 }
	 * のとき、PをQで回転した位置P'を求めるクォータニオン演算：
	 *	P' = Q P Q*
	 * に相当する行列演算は、次の通りです。
	 *	| Px' |   | 1-2*(Qy*Qy+Qz*Qz)   2*(Qx*Qy-Qz*Qw)   2*(Qz*Qx+Qy*Qw) 0 | | Px |
	 *	| Py' | = |   2*(Qx*Qy+Qz*Qw) 1-2*(Qz*Qz+Qx*Qx)   2*(Qy*Qz-Qx*Qw) 0 | | Py |
	 *	| Pz' |   |   2*(Qz*Qx-Qy*Qw)   2*(Qy*Qz+Qx*Qw) 1-2*(Qx*Qx+Qy*Qy) 0 | | Pz |
	 */
	return mxform(m, (matrix){
		fisubr(fimul(fadd(fmul(q.y, q.y), fmul(q.z, q.z)), 2), 1),        fimul(fsub(fmul(q.x, q.y), fmul(q.z, q.w)), 2)    ,        fimul(fadd(fmul(q.z, q.x), fmul(q.y, q.w)), 2)    , fild(0),
		       fimul(fadd(fmul(q.x, q.y), fmul(q.z, q.w)), 2)    , fisubr(fimul(fadd(fmul(q.z, q.z), fmul(q.x, q.x)), 2), 1),        fimul(fsub(fmul(q.y, q.z), fmul(q.x, q.w)), 2)    , fild(0),
		       fimul(fsub(fmul(q.z, q.x), fmul(q.y, q.w)), 2)    ,        fimul(fadd(fmul(q.y, q.z), fmul(q.x, q.w)), 2)    , fisubr(fimul(fadd(fmul(q.x, q.x), fmul(q.y, q.y)), 2), 1), fild(0),
	});
}

