module gedo.math.direction;



public import gedo.math.vector;
private import gedo.math.quaternion;
private import std.math;
private import std.cstream;



public class Direction
{
public:
	static const Vector UP    = [0.0, 0.0, 1.0];
	static const Vector FRONT = [0.0, 1.0, 0.0];
	static const Vector RIGHT = [1.0, 0.0, 0.0];


	this()
	{
		identity(_rotation);
	}


	void homing(Vector p, float v, bool isRollEnable = false)
	{
		Vector front;

		getFront(front);

		Quaternion qa;

		if(!fromVector(qa, front, p))
		{
			Vector up;

			getUp(up);

			halfRotation(qa, up);
		}

		slerpR(qa, v);

		Quaternion qr = _rotation[];

		.gedo.math.quaternion.multiply(_rotation, qa, qr);

		if(!isRollEnable)
		{
			getFront(front);

			Vector[2] r;

			r[1][0] = front[1];
			r[1][1] = -front[0];
			r[1][2] = 0.0;

			auto r1l = magnitude(r[1]);
			if(r1l > 1 / 1024.0)
			{
				getRight(r[0]);

				.gedo.math.vector.multiply(r[1], 1.0/r1l);

				if(!fromVector(qa, r[0], r[1]))
				{
					halfRotation(qa, front);
				}

				qr[] = _rotation[];

				.gedo.math.quaternion.multiply(_rotation, qa, qr);
			}
		}

		.gedo.math.quaternion.normalize(_rotation);
	}


	void getFront(Vector front)
	{
		front[] = FRONT[];

		rotate(_rotation, front);
	}


	void getUp(Vector up)
	{
		up[] = UP[];

		rotate(_rotation, up);
	}


	void getRight(Vector right)
	{
		right[] = RIGHT[];

		rotate(_rotation, right);
	}


	void toVector(float x, float y, float z, Vector v)
	{
		v[] = 0.0;

		Vector a;

		if(x != 0.0)
		{
			getRight(a);

			.gedo.math.vector.multiply(a, x);

			.gedo.math.vector.add(v, a);
		}

		if(y != 0.0)
		{
			getFront(a);

			.gedo.math.vector.multiply(a, y);

			.gedo.math.vector.add(v, a);
		}

		if(z != 0.0)
		{
			getUp(a);

			.gedo.math.vector.multiply(a, z);

			.gedo.math.vector.add(v, a);
		}
	}


	void transform(Vector p)
	{
		.gedo.math.quaternion.transform(_rotation, p);
	}


	void restoreUp()
	{
		Vector u;

		getUp(u);

		if(u[2] < 255.0 / 256.0)
		{
			Quaternion qa;

			if(!fromVector(qa, u, UP))
			{
				Vector f;

				getFront(f);

				halfRotation(qa, f);
			}

			Quaternion qr = _rotation[];

			.gedo.math.quaternion.multiply(_rotation, qa, qr);
		}
	}


	void copyFrom(Direction d)
	{
		_rotation[] = d._rotation[];
	}


private:
	Quaternion _rotation;
}

