//------------------------------------------------------------------------------
// Lamp : Open source game middleware
// Copyright (C) 2004  Junpei Ohtani ( Email : junpee@users.sourceforge.jp )
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU Lesser General Public
// License as published by the Free Software Foundation; either
// version 2.1 of the License, or (at your option) any later version.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// Lesser General Public License for more details.
//
// You should have received a copy of the GNU Lesser General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
//------------------------------------------------------------------------------

/** @file
 * 四元数ヘッダ
 * @author Junpee
 */

#ifndef QUATERNION_H_
#define QUATERNION_H_

#include <Core/Primitive/Vector3.h>

namespace Lamp{

//------------------------------------------------------------------------------
/**
 * 四元数
 *
 * このクラスは継承しないで下さい。
 */
class Quaternion{
public:
	//--------------------------------------------------------------------------
	// メンバ変数
	//--------------------------------------------------------------------------
	/// メンバ変数
	union{
		/// 各要素
		struct{
			/// X値
			float x;
			/// Y値
			float y;
			/// Z値
			float z;
			/// W値
			float w;
		};

		/// 配列
		float array[4];
	};

	//--------------------------------------------------------------------------
	// 定数
	//--------------------------------------------------------------------------
	/// ゼロ四元数
	static const Quaternion zero;

	/// 恒等四元数
	static const Quaternion identity;

	//--------------------------------------------------------------------------
	// コンストラクタ
	//--------------------------------------------------------------------------
	/**
	 * コンストラクタ
	 *
	 * このコンストラクタは初期値の設定を行わないため値は不定です。
	 */
	inline Quaternion(){}

	/**
	 * コンストラクタ
	 * @param sourceX Xの初期値
	 * @param sourceY Yの初期値
	 * @param sourceZ Zの初期値
	 * @param sourceW Wの初期値
	 */
	inline Quaternion(
		float sourceX, float sourceY, float sourceZ, float sourceW) :
		x(sourceX), y(sourceY), z(sourceZ), w(sourceW){
	}

	/**
	 * コンストラクタ
	 * @param source 初期値配列
	 */
	inline explicit Quaternion(const float* const source) :
		x(source[0]), y(source[1]), z(source[2]), w(source[3]){
	}

	//--------------------------------------------------------------------------
	// 値の設定
	//--------------------------------------------------------------------------
	/**
	 * 値の設定
	 * @param sourceX Xの設定値
	 * @param sourceY Yの設定値
	 * @param sourceZ Zの設定値
	 * @param sourceW Wの設定値
	 */
	inline void set(float sourceX, float sourceY, float sourceZ, float sourceW){
		x = sourceX;
		y = sourceY;
		z = sourceZ;
		w = sourceW;
	}

	/**
	 * 値の設定
	 * @param source 設定値配列
	 */
	inline void set(const float* const source){
		x = source[0];
		y = source[1];
		z = source[2];
		w = source[3];
	}

	/**
	 * ゼロ四元数の設定
	 */
	inline void setZero(){
		set(0.f, 0.f, 0.f, 0.f);
	}

	/**
	 * 恒等四元数の設定
	 */
	inline void setIdentity(){
		set(0.f, 0.f, 0.f, 1.f);
	}

	//--------------------------------------------------------------------------
	// 回転
	//--------------------------------------------------------------------------
	/**
	 * 軸指定回転の設定
	 * @param axis 回転軸
	 * @param radian ラジアン単位での回転角度
	 */
	inline void setRotationAxis(const Vector3& axis, float radian){
		Assert(axis.isUnit());
		float halfRadian = radian * 0.5f;
		float sin = Math::sin(halfRadian);
		x = axis.x * sin;
		y = axis.y * sin;
		z = axis.z * sin;
		w = Math::cos(halfRadian);
	}

	/**
	 * 軸指定回転の追加
	 * @param axis 回転軸
	 * @param radian ラジアン単位での回転角度
	 */
	inline void addRotationAxis(const Vector3& axis, float radian){
		Quaternion quaternion;
		quaternion.setRotationAxis(axis, radian);
		(*this) = quaternion * (*this);
	}

	/**
	 * 軸指定回転の取得
	 * @param axis [out]回転軸を格納するベクトルへのポインタ。
	 *	回転していないときはゼロベクトル
	 * @param radian [out]ラジアン単位での回転角度を格納するfloatへのポインタ
	 */
	inline void getRotationAxis(Vector3* axis, float* radian) const{
		Assert(isUnit());
		axis->set(array);
		float halfRadian = Math::acos(w);
		*radian = 2.f * halfRadian;
		(*axis) *= 1.f / Math::sin(halfRadian);
	}

	//--------------------------------------------------------------------------
	// 三軸オイラー回転
	//--------------------------------------------------------------------------
	/**
	 * XYZ軸回転の設定
	 * @param radian 各軸におけるラジアン単位での回転角度
	 */
	inline void setRotationXYZ(const Vector3& radian){
		float xRadian = radian.x * 0.5f;
		float yRadian = radian.y * 0.5f;
		float zRadian = radian.z * 0.5f;
		float sinX = Math::sin(xRadian);
		float cosX = Math::cos(xRadian);
		float sinY = Math::sin(yRadian);
		float cosY = Math::cos(yRadian);
		float sinZ = Math::sin(zRadian);
		float cosZ = Math::cos(zRadian);
		x =  sinX * cosY * cosZ - cosX * sinY * sinZ;
		y =  cosX * sinY * cosZ + sinX * cosY * sinZ;
		z =  cosX * cosY * sinZ - sinX * sinY * cosZ;
		w =  cosX * cosY * cosZ + sinX * sinY * sinZ;
	}

	/**
	 * XYZ軸回転の追加
	 * @param radian 各軸におけるラジアン単位での回転角度
	 */
	inline void addRotationXYZ(const Vector3& radian){
		Quaternion quaternion;
		quaternion.setRotationXYZ(radian);
		(*this) = quaternion * (*this);
	}

	/**
	 * XYZ軸回転の取得
	 * @param radian [out] 各軸におけるラジアン単位での回転角度
	 * @return 答えが単一であればtrue
	 */
	inline bool getRotationXYZ(Vector3* radian) const{
		// Y回転を求める
		float x2 = x + x;
		float y2 = y + y;
		float z2 = z + z;
		float xz2 = x * z2;
		float wy2 = w * y2;
		float temp = -(xz2 - wy2);
		// 誤差対策
		if(temp >= 1.f){ temp = 1.f; }
		else if(temp <= -1.f){ temp = -1.f; }
		float yRadian = Math::asin(temp);
		radian->y = yRadian;
		// 他の回転を求める
		float xx2 = x * x2;
		float xy2 = x * y2;
		float zz2 = z * z2;
		float wz2 = w * z2;
		if(yRadian < Math::halfPI){
			if(yRadian > -Math::halfPI){
				float yz2 = y * z2;
				float wx2 = w * x2;
				float yy2 = y * y2;
				radian->x = Math::atan2((yz2 + wx2), (1.f - (xx2 + yy2)));
				radian->z = Math::atan2((xy2 + wz2), (1.f - (yy2 + zz2)));
				return true;
			}else{
				radian->x = -Math::atan2((xy2 - wz2), (1.f - (xx2 + zz2)));
				radian->z = 0.f;
				return false;
			}
		}else{
			radian->x = Math::atan2((xy2 - wz2), (1.f - (xx2 + zz2)));
			radian->z = 0.f;
			return false;
		}
	}

	//--------------------------------------------------------------------------
	/**
	 * ZYX軸回転の設定
	 * @param radian 各軸におけるラジアン単位での回転角度
	 */
	inline void setRotationZYX(const Vector3& radian){
		float xRadian = radian.x * 0.5f;
		float yRadian = radian.y * 0.5f;
		float zRadian = radian.z * 0.5f;
		float sinX = Math::sin(xRadian);
		float cosX = Math::cos(xRadian);
		float sinY = Math::sin(yRadian);
		float cosY = Math::cos(yRadian);
		float sinZ = Math::sin(zRadian);
		float cosZ = Math::cos(zRadian);
		x =  sinX * cosY * cosZ + cosX * sinY * sinZ;
		y =  cosX * sinY * cosZ - sinX * cosY * sinZ;
		z =  cosX * cosY * sinZ + sinX * sinY * cosZ;
		w =  cosX * cosY * cosZ - sinX * sinY * sinZ;
	}

	/**
	 * ZYX軸回転の追加
	 * @param radian 各軸におけるラジアン単位での回転角度
	 */
	inline void addRotationZYX(const Vector3& radian){
		Quaternion quaternion;
		quaternion.setRotationZYX(radian);
		(*this) = quaternion * (*this);
	}

	/**
	 * ZYX軸回転の取得
	 * @param radian [out] 各軸におけるラジアン単位での回転角度
	 * @return 答えが単一であればtrue
	 */
	inline bool getRotationZYX(Vector3* radian) const{
		// Y回転を求める
		float x2 = x + x;
		float y2 = y + y;
		float z2 = z + z;
		float xz2 = x * z2;
		float wy2 = w * y2;
		float temp = (xz2 + wy2);
		// 誤差対策
		if(temp >= 1.f){ temp = 1.f; }
		else if(temp <= -1.f){ temp = -1.f; }
		float yRadian = Math::asin(temp);
		radian->y = yRadian;
		// 他の回転を求める
		float xy2 = x * y2;
		float wz2 = w * z2;
		if(yRadian < Math::halfPI){
			if(yRadian > -Math::halfPI){
				float yy2 = y * y2;
				float zz2 = z * z2;
				float yz2 = y * z2;
				float wx2 = w * x2;
				float xx2 = x * x2;
				radian->z = Math::atan2(-(xy2 - wz2), (1.f - (yy2 + zz2)));
				radian->x = Math::atan2(-(yz2 - wx2), (1.f - (xx2 + yy2)));
				return true;
			}else{
				radian->z = -Math::atan2(-(xy2 + wz2), (xz2 - wy2));
				radian->x = 0.f;
				return false;
			}
		}else{
			radian->z = Math::atan2((xy2 + wz2), -(xz2 - wy2));
			radian->x = 0.f;
			return false;
		}
	}

	//--------------------------------------------------------------------------
	// 演算
	//--------------------------------------------------------------------------
	/**
	 * 加算
	 * @param addQuaternion 加算する四元数
	 * @return 加算された四元数
	 */
	inline Quaternion operator +(const Quaternion& addQuaternion) const{
		return Quaternion(
			x + addQuaternion.x,
			y + addQuaternion.y,
			z + addQuaternion.z,
			w + addQuaternion.w);
	}

	/**
	 * 減算
	 * @param subQuaternion 減算する四元数
	 * @return 減算された四元数
	 */
	inline Quaternion operator -(const Quaternion& subQuaternion) const{
		return Quaternion(
			x - subQuaternion.x,
			y - subQuaternion.y,
			z - subQuaternion.z,
			w - subQuaternion.w);
	}

	/**
	 * 乗算
	 * @param mulQuat 乗算する四元数
	 * @return 乗算された四元数
	 */
	inline Quaternion operator *(const Quaternion& mulQuat) const{
		// xyz = (q0.xyz * q1.w) + (q1.xyz * q0.w) + cross(q0.xyz, q1.xyz)
		// w = q0.w * q1.w - dot(q0.xyz, q1.xyz)
		return Quaternion(
			y * mulQuat.z - z * mulQuat.y + x * mulQuat.w + w * mulQuat.x,
			z * mulQuat.x - x * mulQuat.z + y * mulQuat.w + w * mulQuat.y,
			x * mulQuat.y - y * mulQuat.x + z * mulQuat.w + w * mulQuat.z,
			w * mulQuat.w - x * mulQuat.x - y * mulQuat.y - z * mulQuat.z);
	}

	/**
	 * 乗算
	 * @param mulValue 乗算する値
	 * @return 乗算された四元数
	 */
	inline Quaternion operator *(float mulValue) const{
		return Quaternion(
			x * mulValue, y * mulValue, z * mulValue, w * mulValue);
	}

	/**
	 * 乗算
	 * @param mulValue 乗算する値
	 * @param mulQuaternion 乗算する四元数
	 * @return 乗算された四元数
	 */
	inline friend Quaternion operator *(
		float mulValue, const Quaternion& mulQuaternion){
		return Quaternion(
			mulQuaternion.x * mulValue,
			mulQuaternion.y * mulValue,
			mulQuaternion.z * mulValue,
			mulQuaternion.w * mulValue);
	}

	/**
	 * ベクトル乗算
	 * @param mulVector 乗算するベクトル
	 * @return 乗算されたベクトル
	 */
	inline Vector3 operator *(const Vector3& mulVector) const{
		Vector3 quaternion(x, y, z);
		Vector3 cross1 = quaternion.crossProduct(mulVector);
		Vector3 cross2 = quaternion.crossProduct(cross1);
		cross1 *= (w * 2.f);
		cross1 += mulVector;
		cross2 *= 2.f;
		cross1 += cross2;
		return cross1;
	}

	/**
	 * +演算子
	 * @return 四元数のコピー
	 */
	inline Quaternion operator +() const{ return *this; }

	/**
	 * -演算子
	 * @return 値の符号が反転した四元数
	 */
	inline Quaternion operator -() const{ return Quaternion(-x, -y, -z, -w); }

	//--------------------------------------------------------------------------
	// 代入演算
	//--------------------------------------------------------------------------
	/**
	 * 代入加算
	 * @param addQuaternion 加算する四元数
	 * @return 加算された四元数
	 */
	inline Quaternion& operator +=(const Quaternion& addQuaternion){
		x += addQuaternion.x;
		y += addQuaternion.y;
		z += addQuaternion.z;
		w += addQuaternion.w;
		return *this;
	}

	/**
	 * 代入減算
	 * @param subQuaternion 減算する四元数
	 * @return 減算された四元数
	 */
	inline Quaternion& operator -=(const Quaternion& subQuaternion){
		x -= subQuaternion.x;
		y -= subQuaternion.y;
		z -= subQuaternion.z;
		w -= subQuaternion.w;
		return *this;
	}

	/**
	 * 代入乗算
	 * @param mulQuat 乗算する四元数
	 * @return 乗算された四元数
	 */
	inline Quaternion& operator *=(const Quaternion& mulQuat){
		// xyz = (q0.xyz * q1.w) + (q1.xyz * q0.w) + cross(q0.xyz, q1.xyz)
		// w = q0.w * q1.w - dot(q0.xyz, q1.xyz)
		set(y * mulQuat.z - z * mulQuat.y + x * mulQuat.w + w * mulQuat.x,
			z * mulQuat.x - x * mulQuat.z + y * mulQuat.w + w * mulQuat.y,
			x * mulQuat.y - y * mulQuat.x + z * mulQuat.w + w * mulQuat.z,
			w * mulQuat.w - x * mulQuat.x - y * mulQuat.y - z * mulQuat.z);
		return *this;
	}

	/**
	 * 代入乗算
	 * @param mulValue 乗算する値
	 * @return 乗算された四元数
	 */
	inline Quaternion& operator *=(float mulValue){
		x *= mulValue;
		y *= mulValue;
		z *= mulValue;
		w *= mulValue;
		return *this;
	}

	//--------------------------------------------------------------------------
	// 長さ関連
	//--------------------------------------------------------------------------
	/**
	 * 長さの取得
	 * @return 長さ
	 */
	inline float getLength() const{
		return Math::sqrt(x * x + y * y + z * z + w * w);
	}

	/**
	 * ゼロ四元数かどうか
	 * @return ゼロ四元数ならtrueを返す
	 */
	inline bool isZero() const{
		return (getLength() <= Math::epsilon);
	}

	/**
	 * 単位四元数かどうか
	 * @return 単位四元数ならtrueを返す
	 */
	inline bool isUnit() const{
		return (Math::abs(getLength() - 1.f) <= Math::epsilon);
	}

	/**
	 * 正規化
	 * @return 正規化された四元数
	 */
	inline Quaternion& normalize(){
		float legnth = getLength();
		Assert(legnth >= Math::epsilon);
		float inverseLength = 1.f / legnth;
		(*this) *= inverseLength;
		return *this;
	}

	//--------------------------------------------------------------------------
	// 四元数演算
	//--------------------------------------------------------------------------
	/**
	 * ノルムの取得
	 * @return ノルム
	 */
	inline float getNorm() const{
		return x * x + y * y + z * z + w * w;
	}

	/**
	 * 共役
	 * @return 共役化された四元数
	 */
	inline Quaternion& conjugate(){
		x = -x;
		y = -y;
		z = -z;
		return *this;
	}

	/**
	 * 内積
	 * @param dotQuaternion 内積をとる四元数
	 * @return 内積値
	 */
	inline float dotProduct(const Quaternion& dotQuaternion) const{
		return x * dotQuaternion.x + y * dotQuaternion.y +
			z * dotQuaternion.z + w * dotQuaternion.w;
	}

	/**
	 * 逆四元数
	 * @return 逆四元数
	 */
	inline Quaternion& invert(){
		Assert(!isZero());
		float inverseNorm = 1.f / getNorm();
		x = -x * inverseNorm;
		y = -y * inverseNorm;
		z = -z * inverseNorm;
		w =  w * inverseNorm;
		return *this;
	}

	/**
	 * 単位逆四元数
	 * @return 逆四元数
	 */
	inline Quaternion& unitInvert(){
		Assert(isUnit());
		return conjugate();
	}

	//--------------------------------------------------------------------------
	// 補間
	//--------------------------------------------------------------------------
	/**
	 * 球面線形補間
	 * @param source 補間開始四元数
	 * @param target 補間終了四元数
	 * @param alpha 補間係数
	 * @return 補間済み四元数
	 */
	static Quaternion slerp(
		const Quaternion& source, const Quaternion& target, float alpha){
		Assert(source.isUnit());
		Assert(target.isUnit());
		float dot = source.dotProduct(target);
		// 誤差対策
		if(dot >= 1.f){
			dot = 1.f;
		}else if(dot <= -1.f){
			dot = -1.f;
		}
		float radian = Math::acos(dot);
		if(Math::abs(radian) < Math::epsilon){ return target; }
		float inverseSin = 1.f / Math::sin(radian);
		float leftScale = Math::sin((1.f - alpha) * radian) * inverseSin;
		float rightScale = Math::sin(alpha * radian) * inverseSin;
		return source * leftScale + target * rightScale;
	}

	/**
	 * 補正付球面線形補間
	 * @param source 補間開始四元数
	 * @param target 補間終了四元数
	 * @param alpha 補間係数
	 * @return 補間済み四元数
	 */
	static Quaternion correctSlerp(
		const Quaternion& source, const Quaternion& target, float alpha){
		Assert(source.isUnit());
		Assert(target.isUnit());
		float dot = source.dotProduct(target);
		// 反転処理
		Quaternion correctTarget;
		if(dot < 0.f){
			correctTarget = -target;
			dot = -dot;
		}else{
			correctTarget = target;
		}
		// 誤差対策
		if(dot >= 1.f){ dot = 1.f; }
		float radian = Math::acos(dot);
		if(Math::abs(radian) < Math::epsilon){ return correctTarget; }
		float inverseSin = 1.f / Math::sin(radian);
		float leftScale = Math::sin((1.f - alpha) * radian) * inverseSin;
		float rightScale = Math::sin(alpha * radian) * inverseSin;
		return source * leftScale + correctTarget * rightScale;
	}

	/**
	 * 球面二次補間
	 * @param source 補間開始四元数
	 * @param sourceHandle 補間開始四元数ハンドル
	 * @param targetHandle 補間終了四元数ハンドル
	 * @param target 補間終了四元数
	 * @param alpha 補間係数
	 * @return 補間済み四元数
	 */
	static Quaternion squad(
		const Quaternion& source, const Quaternion& sourceHandle,
		const Quaternion& targetHandle, const Quaternion& target, float alpha){
		Quaternion slerpOriginal = slerp(source, target, alpha);
		Quaternion slerpHandle = slerp(sourceHandle, targetHandle, alpha);
		return slerp(slerpOriginal, slerpHandle, 2.f * alpha * (1.f - alpha));
	}

	/**
	 * 補正付球面二次補間
	 * @param source 補間開始四元数
	 * @param sourceHandle 補間開始四元数ハンドル
	 * @param targetHandle 補間終了四元数ハンドル
	 * @param target 補間終了四元数
	 * @param alpha 補間係数
	 * @return 補間済み四元数
	 */
	static Quaternion correctSquad(
		const Quaternion& source, const Quaternion& sourceHandle,
		const Quaternion& targetHandle, const Quaternion& target, float alpha){
		Quaternion slerpOriginal =
			correctSlerp(source, target, alpha);
		Quaternion slerpHandle =
			correctSlerp(sourceHandle, targetHandle, alpha);
		return correctSlerp(
			slerpOriginal, slerpHandle, 2.f * alpha * (1.f - alpha));
	}

	//--------------------------------------------------------------------------
	// 論理演算
	//--------------------------------------------------------------------------
	/**
	 * 四元数が同じかどうか
	 * @param target 比較する四元数
	 * @return 同じ値であればtrueを返す
	 */
	inline bool operator ==(const Quaternion& target) const{
		return ((x == target.x) && (y == target.y) &&
			(z == target.z) && (w == target.w));
	}

	/**
	 * 四元数が同じかどうか
	 * @param target 比較する四元数
	 * @param epsilon 誤差
	 * @return 誤差の範囲内で同じ値であればtrueを返す
	 */
	inline bool epsilonEquals(const Quaternion& target, float epsilon) const{
		Assert(epsilon >= 0.f);
		return (
			(Math::abs(x - target.x) <= epsilon) &&
			(Math::abs(y - target.y) <= epsilon) &&
			(Math::abs(z - target.z) <= epsilon) &&
			(Math::abs(w - target.w) <= epsilon));
	}

	/**
	 * 四元数が同じでないかどうか
	 * @param target 比較する四元数
	 * @return 同じでない値であればtrueを返す
	 */
	inline bool operator !=(const Quaternion& target) const{
		return ((x != target.x) || (y != target.y) ||
			(z != target.z) || (w != target.w));
	}

	/**
	 * 四元数が同じでないかどうか
	 * @param target 比較する四元数
	 * @param epsilon 誤差
	 * @return 誤差の範囲内で同じでない値であればtrueを返す
	 */
	inline bool notEpsilonEquals(const Quaternion& target, float epsilon) const{
		Assert(epsilon >= 0.f);
		return (
			(Math::abs(x - target.x) > epsilon) ||
			(Math::abs(y - target.y) > epsilon) ||
			(Math::abs(z - target.z) > epsilon) ||
			(Math::abs(w - target.w) > epsilon));
	}

	//--------------------------------------------------------------------------
	/**
	 * 四元数が同じ回転をあらわすかどうか
	 * @param target 比較する四元数
	 * @return 同じ回転をあらわせばtrueを返す
	 */
	inline bool rotationEquals(const Quaternion& target) const{
		if((*this) == target){ return true; }
		if((*this) == -target){ return true; }
		return false;
	}

	/**
	 * 四元数が同じ回転をあらわすかどうか
	 * @param target 比較する四元数
	 * @param epsilon 誤差
	 * @return 誤差の範囲内で同じ回転をあらわせばtrueを返す
	 */
	inline bool epsilonRotationEquals(
		const Quaternion& target, float epsilon) const{
		if(epsilonEquals(target, epsilon)){ return true; }
		if(epsilonEquals(-target, epsilon)){ return true; }
		return false;
	}

	//--------------------------------------------------------------------------
	// その他
	//--------------------------------------------------------------------------
	/**
	 * 文字列化
	 * @return ベクトルの文字列表記
	 */
	inline String toString() const{
		String returnString;
		returnString.format("( < %.8f, %.8f, %.8f > %.8f )", x, y, z, w);
		return returnString;
	}

	//--------------------------------------------------------------------------
private:

};

//------------------------------------------------------------------------------
} // End of namespace Lamp
#endif // End of QUATERNION_H_
//------------------------------------------------------------------------------
