//------------------------------------------------------------------------------
// 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
 */

#include "Test/stdafx.h"
#include "Core/Primitive/QuaternionTest.h"
#include "Core/Primitive/Quaternion.h"
#include "Core/Utility/Random.h"

//------------------------------------------------------------------------------
// コンストラクタ
QuaternionTest::QuaternionTest(String name) : TestCase(name){
}
//------------------------------------------------------------------------------
// テストスィートの取得
Test* QuaternionTest::suite(){
	TestSuite* suite = new TestSuite("QuaternionTest");
	suite->addTest(new TestCaller(QuaternionTest, testConstructor));
	suite->addTest(new TestCaller(QuaternionTest, testFixedNumber));
	suite->addTest(new TestCaller(QuaternionTest, testSetValue));
	suite->addTest(new TestCaller(QuaternionTest, testRotation));
	suite->addTest(new TestCaller(QuaternionTest, testEulerRotationXYZ));
	suite->addTest(new TestCaller(QuaternionTest, testEulerRotationZYX));
	suite->addTest(new TestCaller(QuaternionTest, testArithmetic));
	suite->addTest(new TestCaller(QuaternionTest, testSubstitutionArithmetic));
	suite->addTest(new TestCaller(QuaternionTest, testLength));
	suite->addTest(new TestCaller(QuaternionTest, testQuaternion));
	suite->addTest(new TestCaller(QuaternionTest, testInterpolate));
	suite->addTest(new TestCaller(QuaternionTest, testLogicalOperation));
	suite->addTest(new TestCaller(QuaternionTest, testToString));
	return suite;
}
//------------------------------------------------------------------------------
// コンストラクタテスト
void QuaternionTest::testConstructor(){
	TestEquals(16, (int)sizeof(Quaternion));
	Quaternion quat0(1.f, 2.f, 3.f, 4.f), quat1, quat2;
	quat1.x = 1.f;
	quat1.y = 2.f;
	quat1.z = 3.f;
	quat1.w = 4.f;
	TestAssert(quat0 == quat1);
	quat2.array[0] = 1.f;
	quat2.array[1] = 2.f;
	quat2.array[2] = 3.f;
	quat2.array[3] = 4.f;
	TestAssert(quat0 == quat2);
	float array[] = { 1.f, 2.f, 3.f, 4.f };
	Quaternion quat3(array);
	TestAssert(quat0 == quat3);
}
//------------------------------------------------------------------------------
// 定数テスト
void QuaternionTest::testFixedNumber(){
	TestAssert(Quaternion(0.f, 0.f, 0.f, 0.f) == Quaternion::zero);
	TestAssert(Quaternion(0.f, 0.f, 0.f, 1.f) == Quaternion::identity);
}
//------------------------------------------------------------------------------
// 値の設定テスト
void QuaternionTest::testSetValue(){
	Quaternion quat0(1.f, 2.f, 3.f, 4.f);
	Quaternion quat1;
	quat1.set(1.f, 2.f, 3.f, 4.f);
	TestAssert(quat0 == quat1);
	float array[] = { 1.f, 2.f, 3.f, 4.f };
	quat1.set(array);
	TestAssert(quat0 == quat1);
	quat1.setZero();
	TestAssert(Quaternion::zero == quat1);
	quat1.setIdentity();
	TestAssert(Quaternion::identity == quat1);
}
//------------------------------------------------------------------------------
// 回転テスト
void QuaternionTest::testRotation(){
	// 回転の設定
	D3DXQUATERNION d3dQuat0;
	D3DXVECTOR3 d3dAxis(1.f, 2.f, 3.f);
	D3DXQuaternionRotationAxis(&d3dQuat0, &d3dAxis, 4.f);
	Quaternion quat0;
	Vector3 vec0(1.f, 2.f, 3.f);
	vec0.normalize();
	quat0.setRotationAxis(vec0, 4.f);
	TestAssert(d3dEpsilonEquals(d3dQuat0, quat0, Math::epsilon));
	// 回転の追加
	D3DXQUATERNION d3dQuat1;
	D3DXQuaternionRotationAxis(&d3dQuat1, &d3dAxis, -3.f);
	D3DXQuaternionMultiply(&d3dQuat0, &d3dQuat0, &d3dQuat1);
	quat0.addRotationAxis(vec0, -3.f);
	TestAssert(d3dEpsilonEquals(d3dQuat0, quat0, Math::epsilon));
	// 回転の取得
	float d3dRadius;
	D3DXQuaternionToAxisAngle(&d3dQuat0, &d3dAxis, &d3dRadius);
	D3DXVec3Normalize(&d3dAxis, &d3dAxis);
	float radius;
	quat0.getRotationAxis(&vec0, &radius);
	TestEpsilonEquals(d3dAxis.x, vec0.x, Math::epsilon);
	TestEpsilonEquals(d3dAxis.y, vec0.y, Math::epsilon);
	TestEpsilonEquals(d3dAxis.z, vec0.z, Math::epsilon);
	TestEpsilonEquals(d3dRadius, radius, Math::epsilon);
	Quaternion::identity.getRotationAxis(&vec0, &radius);
}
//------------------------------------------------------------------------------
// XYZオイラー回転テスト
void QuaternionTest::testEulerRotationXYZ(){
	Quaternion quat0, quat1;
	Matrix33 quatMtx, eulerMtx;
	Vector3 rotation, addRotation, quatRot, eulerRot;
	Random rand;
	float scale = 100.f;
	for(int i = 0; i < 100; i++){
		rotation.set(rand.getSignedFloat() * scale,
			rand.getSignedFloat() * scale, rand.getSignedFloat() * scale);
		addRotation.set(rand.getSignedFloat() * scale,
			rand.getSignedFloat() * scale, rand.getSignedFloat() * scale);
		// オイラー回転の設定
		quat0.setRotationXYZ(rotation);
		quatMtx.setRotationQuaternion(quat0);
		eulerMtx.setRotationXYZ(rotation);
		TestAssert(quatMtx.epsilonEquals(eulerMtx, Math::epsilon));
		quat1 = eulerMtx.getRotationQuaternion();
		TestAssert(quat0.epsilonRotationEquals(quat1, Math::epsilon));
		// オイラー回転の追加
		quat0.addRotationXYZ(addRotation);
		quatMtx.setRotationQuaternion(quat0);
		eulerMtx.addRotationXYZ(addRotation);
		TestAssert(quatMtx.epsilonEquals(eulerMtx, Math::epsilon));
		quat1 = eulerMtx.getRotationQuaternion();
		TestAssert(quat0.epsilonRotationEquals(quat1, Math::epsilon));
		// オイラー回転の取得
		TestAssert(quat0.getRotationXYZ(&quatRot));
		TestAssert(eulerMtx.getRotationXYZ(&eulerRot));
		TestAssert(quatRot.epsilonEquals(eulerRot, 0.0001f));
	}
}
//------------------------------------------------------------------------------
// ZYXオイラー回転テスト
void QuaternionTest::testEulerRotationZYX(){
	Quaternion quat0, quat1;
	Matrix33 quatMtx, eulerMtx;
	Vector3 rotation, addRotation, quatRot, eulerRot;
	Random rand;
	float scale = 100.f;
	for(int i = 0; i < 100; i++){
		rotation.set(rand.getSignedFloat() * scale,
			rand.getSignedFloat() * scale, rand.getSignedFloat() * scale);
		addRotation.set(rand.getSignedFloat() * scale,
			rand.getSignedFloat() * scale, rand.getSignedFloat() * scale);
		// オイラー回転の設定
		quat0.setRotationZYX(rotation);
		quatMtx.setRotationQuaternion(quat0);
		eulerMtx.setRotationZYX(rotation);
		TestAssert(quatMtx.epsilonEquals(eulerMtx, Math::epsilon));
		quat1 = eulerMtx.getRotationQuaternion();
		TestAssert(quat0.epsilonRotationEquals(quat1, Math::epsilon));
		// オイラー回転の追加
		quat0.addRotationZYX(addRotation);
		quatMtx.setRotationQuaternion(quat0);
		eulerMtx.addRotationZYX(addRotation);
		TestAssert(quatMtx.epsilonEquals(eulerMtx, Math::epsilon));
		quat1 = eulerMtx.getRotationQuaternion();
		TestAssert(quat0.epsilonRotationEquals(quat1, Math::epsilon));
		// オイラー回転の取得
		TestAssert(quat0.getRotationZYX(&quatRot));
		TestAssert(eulerMtx.getRotationZYX(&eulerRot));
		TestAssert(quatRot.epsilonEquals(eulerRot, 0.0001f));
	}
}
//------------------------------------------------------------------------------
// 演算テスト
void QuaternionTest::testArithmetic(){
	D3DXQUATERNION d3dQuat0(1.f, 2.f, 3.f, 4.f), d3dQuat1(2.f, 3.f, 4.f, 7.f);
	Quaternion quat0(1.f, 2.f, 3.f, 4.f), quat1(2.f, 3.f, 4.f, 7.f);
	// 加算
	d3dQuat0 = d3dQuat0 + d3dQuat1;
	quat0 = quat0 + quat1;
	TestAssert(d3dEquals(d3dQuat0, quat0));
	// 減算
	d3dQuat0 = d3dQuat0 - d3dQuat1;
	quat0 = quat0 - quat1;
	TestAssert(d3dEquals(d3dQuat0, quat0));
	// 四元数乗算
	d3dQuat0 = d3dQuat0 * d3dQuat1;
	quat0 = quat1 * quat0;
	TestAssert(d3dEquals(d3dQuat0, quat0));
	// float乗算
	d3dQuat0 = d3dQuat0 * 2.f;
	quat0 = quat0 * 2.f;
	TestAssert(d3dEquals(d3dQuat0, quat0));
	d3dQuat0 = 0.5f * d3dQuat0;
	quat0 = 0.5f * quat0;
	TestAssert(d3dEquals(d3dQuat0, quat0));
	// ベクトル乗算
	D3DXQuaternionNormalize(&d3dQuat0, &d3dQuat0);
	D3DXMATRIX __declspec(align(16)) d3dMtx;
	D3DXMatrixRotationQuaternion(&d3dMtx, &d3dQuat0);
	D3DXVECTOR3 d3dPosition(1.f, 2.f, 3.f);
	D3DXVECTOR4 d3dResult;
	D3DXVec3Transform(&d3dResult, &d3dPosition, &d3dMtx);
	quat0.normalize();
	Vector3 result = quat0 * Vector3(1.f, 2.f, 3.f);
	TestEpsilonEquals(d3dResult.x, result.x, Math::epsilon);
	TestEpsilonEquals(d3dResult.y, result.y, Math::epsilon);
	TestEpsilonEquals(d3dResult.z, result.z, Math::epsilon);
	// +-演算子
	TestAssert(d3dEpsilonEquals(+d3dQuat0, +quat0, Math::epsilon));
	TestAssert(d3dEpsilonEquals(-d3dQuat0, -quat0, Math::epsilon));
}
//------------------------------------------------------------------------------
// 代入演算テスト
void QuaternionTest::testSubstitutionArithmetic(){
	D3DXQUATERNION d3dQuat0(1.f, 2.f, 3.f, 4.f), d3dQuat1(2.f, 3.f, 4.f, 5.f);
	Quaternion quat0(1.f, 2.f, 3.f, 4.f), quat1(2.f, 3.f, 4.f, 5.f);
	// 加算
	d3dQuat0 += d3dQuat1;
	quat0 += quat1;
	TestAssert(d3dEquals(d3dQuat0, quat0));
	// 減算
	d3dQuat0 -= d3dQuat1;
	quat0 -= quat1;
	TestAssert(d3dEquals(d3dQuat0, quat0));
	// 四元数乗算
	d3dQuat0 *= d3dQuat1;
	quat1 *= quat0;
	TestAssert(d3dEquals(d3dQuat0, quat1));
	// float乗算
	d3dQuat0 *= 2.f;
	quat1 *= 2.f;
	TestAssert(d3dEquals(d3dQuat0, quat1));
}
//------------------------------------------------------------------------------
// 長さテスト
void QuaternionTest::testLength(){
	// 正規化
	D3DXQUATERNION d3dQuat(1.f, 2.f, 3.f, 4.f);
	D3DXQuaternionNormalize(&d3dQuat, &d3dQuat);
	Quaternion quat0(1.f, 2.f, 3.f, 4.f);
	quat0.normalize();
	TestAssert(d3dEpsilonEquals(d3dQuat, quat0, Math::epsilon));
	// ゼロ四元数、単位四元数
	Quaternion quat1(1e-07f, 0.f, 0.f, 0.f);
	TestAssert(quat1.isZero());
	quat1.set(2e-06f, 0.f, 0.f, 0.f);
	TestAssert(!quat1.isZero());
	TestAssert(!quat1.isUnit());
	quat1.normalize();
	TestAssert(quat1.isUnit());
}
//------------------------------------------------------------------------------
// 四元数演算テスト
void QuaternionTest::testQuaternion(){
	// ノルム
	Quaternion quat0(1.f, 2.f, 3.f, 4.f);
	TestEquals(30.f, quat0.getNorm());
	// 共役
	D3DXQUATERNION d3dQuat(1.f, 2.f, 3.f, 4.f);
	D3DXQuaternionConjugate(&d3dQuat, &d3dQuat);
	quat0.conjugate();
	TestAssert(d3dEquals(d3dQuat, quat0));
	// 内積
	D3DXQUATERNION d3dQuat1(2.f, 3.f, 4.f, 5.f);
	Quaternion quat1(2.f, 3.f, 4.f, 5.f);
	TestEquals(quat0.dotProduct(quat1), D3DXQuaternionDot(&d3dQuat, &d3dQuat1));
	// 逆四元数
	D3DXQuaternionInverse(&d3dQuat, &d3dQuat);
	quat0.invert();
	TestAssert(d3dEpsilonEquals(d3dQuat, quat0, Math::epsilon));
	D3DXQuaternionNormalize(&d3dQuat, &d3dQuat);
	quat0.normalize();
	TestAssert(d3dEpsilonEquals(d3dQuat, quat0, Math::epsilon));
	D3DXQuaternionInverse(&d3dQuat, &d3dQuat);
	quat0.unitInvert();
	TestAssert(d3dEpsilonEquals(d3dQuat, quat0, Math::epsilon));

}
//------------------------------------------------------------------------------
// 補間テスト
void QuaternionTest::testInterpolate(){
	D3DXQUATERNION d3dQuat0(1.f, 2.f, 3.f, 4.f), d3dQuat1(4.f, 3.f, 2.f, 1.f);
	D3DXQuaternionNormalize(&d3dQuat0, &d3dQuat0);
	D3DXQuaternionNormalize(&d3dQuat1, &d3dQuat1);
	D3DXQUATERNION d3dResult;
	Quaternion quat0(1.f, 2.f, 3.f, 4.f), quat1(4.f, 3.f, 2.f, 1.f), result;
	quat0.normalize();
	quat1.normalize();
	for(int i = 0; i < 11; i++){
		float alpha = i * 0.1f;
		D3DXQuaternionSlerp(&d3dResult, &d3dQuat0, &d3dQuat1, alpha);
		result = Quaternion::slerp(quat0, quat1, alpha);
		TestAssert(d3dEpsilonEquals(d3dResult, result, Math::epsilon));
	}
	D3DXQUATERNION d3dQuat2(5.f, 4.f, 3.f, 2.f), d3dQuat3(2.f, 3.f, 4.f, 5.f);
	D3DXQuaternionNormalize(&d3dQuat2, &d3dQuat2);
	D3DXQuaternionNormalize(&d3dQuat3, &d3dQuat3);
	Quaternion quat2(5.f, 4.f, 3.f, 2.f), quat3(2.f, 3.f, 4.f, 5.f);
	quat2.normalize();
	quat3.normalize();
	for(int i = 0; i < 11; i++){
		float alpha = i * 0.1f;
		D3DXQuaternionSquad(&d3dResult,
			&d3dQuat0, &d3dQuat2, &d3dQuat3, &d3dQuat1, alpha);
		result = Quaternion::squad(quat0, quat2, quat3, quat1, alpha);
		TestAssert(d3dEpsilonEquals(d3dResult, result, Math::epsilon));
	}
}
//------------------------------------------------------------------------------
// 論理演算テスト
void QuaternionTest::testLogicalOperation(){
	Quaternion quat0(1.f, 2.f, 3.f, 4.f), quat1(1.f, 2.f, 3.f, 4.f);
	TestAssert(quat0 == quat1);
	TestAssert(!(quat0 != quat1));
	quat0.x += Math::epsilon;
	TestAssert(!(quat0 == quat1));
	TestAssert(quat0 != quat1);
	TestAssert(quat0.epsilonEquals(quat1, Math::epsilon));
	TestAssert(!quat0.notEpsilonEquals(quat1, Math::epsilon));
	quat0.x += Math::epsilon;
	TestAssert(!quat0.epsilonEquals(quat1, Math::epsilon));
	TestAssert(quat0.notEpsilonEquals(quat1, Math::epsilon));
}
//------------------------------------------------------------------------------
// 文字列テスト
void QuaternionTest::testToString(){
	Quaternion quat0(12345678.f, 1234.5678f, 0.12345678f, 12.34f);
	TestEquals(
		"( < 12345678.00000000, 1234.56774902, 0.12345678 > 12.34000015 )",
		quat0.toString());
}
//------------------------------------------------------------------------------
// Direct3D四元数との比較
bool QuaternionTest::d3dEquals(D3DXQUATERNION& d3d, Quaternion& quat){
	return (
		(d3d.x == quat.x) &&
		(d3d.y == quat.y) &&
		(d3d.z == quat.z) &&
		(d3d.w == quat.w));
}
//------------------------------------------------------------------------------
// Direct3D四元数との比較
bool QuaternionTest::d3dEpsilonEquals(
	D3DXQUATERNION& d3d, Quaternion& quat, float epsilon){
	return (
		(Math::abs(d3d.x - quat.x) <= epsilon) &&
		(Math::abs(d3d.y - quat.y) <= epsilon) &&
		(Math::abs(d3d.z - quat.z) <= epsilon) &&
		(Math::abs(d3d.w - quat.w) <= epsilon));
}
//------------------------------------------------------------------------------
