using System;
using XELF.Framework;

partial class MathSample4 {
	static void Main(string[] args) {
		Conjugate();
		Invert();
		Invert2();
		RotationPosition();
		ToMatrix();
	}
}

partial class MathSample4 {
	static void Conjugate() {
		var position = new Vector3(1, 2, 3);
		var rotation = Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, -1, 0)), MathHelper.Pi / 3);
		var a = DualQuaternion.CreateFromRotationPosition(rotation, position);
		DualQuaternion b;
		DualQuaternion.SwapConjugate(ref a, out b);

        Console.WriteLine("-- 4.5.2項 184ページ"); 
        Console.WriteLine("共役デュアルクォータニオン");
		Console.WriteLine("デュアルクォータニオン = {0}", a);
		Console.WriteLine("共役デュアルクォータニオン = {0}", b);
		Console.WriteLine();
	}
}

partial class MathSample4 {
	static void Invert() {
		var position = new Vector3(1, 2, 3);
		var rotation = Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, -1, 0)), MathHelper.Pi / 3);
		var a = DualQuaternion.CreateFromRotationPosition(rotation, position);
		DualQuaternion b;
		DualQuaternion.Invert(ref a, out b);

        Console.WriteLine("-- 4.5.3項 186ページ");
        Console.WriteLine("逆デュアルクォータニオン");
		Console.WriteLine("デュアルクォータニオン = {0}", a);
		Console.WriteLine("逆デュアルクォータニオン = {0}", b);
		Console.WriteLine();
	}
}

partial class MathSample4 {
	static void Invert2() {
		var a = new DualQuaternion(1, 2, 3, 4, 5, 6, 7, 8);
		DualQuaternion b, c;
		DualQuaternion.Invert(ref a, out b);
		DualQuaternion.Invert(ref b, out c);

        Console.WriteLine("-- 4.5.3項 (追加)");
        Console.WriteLine("逆デュアルクォータニオン(2)");
		Console.WriteLine("デュアルクォータニオンA = {0}", a);
		Console.WriteLine("Aの逆デュアルクォータニオンB = {0}", b);
		Console.WriteLine("Bの逆デュアルクォータニオンC = {0}", c);
		Console.WriteLine();
	}
}

partial class MathSample4 {
	static void RotationPosition() {
		var position = new Vector3(1, 2, 3);
		var rotation = Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, -1, 0)), MathHelper.Pi / 3);
		var a = DualQuaternion.CreateFromRotationPosition(rotation, position);

        Console.WriteLine("-- 4.5.5項 189ページ"); 
        Console.WriteLine("回転と位置からデュアルクォータニオン");
		Console.WriteLine("回転クォータニオン = {0}", rotation);
		Console.WriteLine("位置ベクトル = {0}", position);
		Console.WriteLine("デュアルクォータニオン = {0}", a);
		Console.WriteLine();
	}
}

partial class MathSample4 {
	static void ToMatrix() {
		var position = new Vector3(1, 2, 3);
		var rotation = Quaternion.CreateFromAxisAngle(Vector3.Normalize(new Vector3(1, -1, 0)), MathHelper.Pi / 3);
		var a = DualQuaternion.CreateFromRotationPosition(rotation, position);
		Matrix ma, r, t, mb, md;
		Matrix.CreateFromDualQuaternion(ref a, out ma);
		Matrix.CreateFromQuaternion(ref rotation, out r);
		Matrix.CreateTranslation(ref position, out t);
		Matrix.Multiply(ref r, ref t, out mb);
		Matrix.Subtract(ref ma, ref mb, out md);

        Console.WriteLine("-- 4.5.6項 192ページ"); 
        Console.WriteLine("デュアルクォータニオンから行列と、回転と位置から変換行列の比較");
		Console.WriteLine("回転クォータニオン = {0}", rotation);
		Console.WriteLine("位置ベクトル = {0}", position);
		Console.WriteLine("デュアルクォータニオン = {0}", a);
		Console.WriteLine("デュアルクォータニオンから行列a = {0}", ma);
		Console.WriteLine("回転と位置から変換行列b = {0}", mb);
		Console.WriteLine("a - b = {0}", md);
		Console.WriteLine();
	}
}
