//	Darkstar Engine  (c) UnderWorld Interactive Studio (all coding by Svarog    2007)
//
//      Данный файл является частью Darkstar Engine. Любое использование кода файла
//      без согласия компании, является нарушением закона об авторском праве (в слу-
//      чае если не оговорено обратное).
//
//      Имя файла: quat.inl
//      Краткое описание: работа с кватернионами
//

namespace math
{
	inline Quat::Quat()
	{

	};

	inline Quat::Quat(const Quat& q)
	{
		a = q.a;
		v = q.v;
	};

	inline Quat::Quat(const float a, const float b, const float c, const float d)
	{
		this->a = a;
		v = Vect3(b, c, d);
	};

	inline Quat::Quat(const float* abcd)
	{
		a = abcd[0];
		v = Vect3(abcd[1], abcd[2], abcd[2]);
	};

	inline Quat::Quat(const float a, const Vect3& v)
	{
		this->a = a;
		this->v = v;
	};

	inline Quat::Quat(const Vect4& v4)
	{
		a = v4.x;
		v.x = v4.y;
		v.y = v4.z;
		v.z = v4.w;
	};

	inline Quat::Quat(const Matr3& m)
	{
		setRotation(m);
	};

	inline Quat::Quat(const Matr4& m)
	{
		setRotation(m);
	};

	inline Quat::Quat(const Matr34& m)
	{
		setRotation(m);
	};

	inline Quat::~Quat()
	{

	};

	inline Quat& Quat::operator =(const Quat& q)
	{
		a = q.a;
		v = q.v;
		return *this;
	};

	inline Quat Quat::operator +(const Quat& q) const
	{
		return Quat(a + q.a, v + q.v);
	};

	inline const Quat& Quat::operator +=(const Quat& q)
	{
		a += q.a;
		v += q.v;
		return *this;
	};

	inline Quat Quat::operator -(const Quat& q) const
	{
		return Quat(a - q.a, v - q.v);
	};

	inline const Quat& Quat::operator -=(const Quat& q)
	{
		a -= q.a;
		v -= q.v;
		return *this;
	};

	inline Quat Quat::operator *(const Quat& q) const
	{
		float A, B, C, D, E, F, G, H;

		A = (a + v.x) * (q.a + q.v.x);
		B = (v.z - v.y) * (q.v.y - q.v.z);
		C = (v.x - a) * (q.v.y + q.v.z);
		D = (v.y + v.z) * (q.v.x - q.a);
		E = (v.x + v.z) * (q.v.x + q.v.y);
		F = (v.x - v.z) * (q.v.x - q.v.y);
		G = (a + v.y) * (q.a - q.v.z);
		H = (a - v.y) * (q.a + q.v.z);

		return Quat(B + (-E - F + G + H) * 0.5f, A - ( E + F + G + H) * 0.5f, -C + ( E - F + G - H) * 0.5f, -D + ( E - F - G + H) * 0.5f);
	};

	inline const Quat& Quat::operator *=(const Quat& q)
	{
		float A, B, C, D, E, F, G, H;

		A = (a + v.x) * (q.a + q.v.x);
		B = (v.z - v.y) * (q.v.y - q.v.z);
		C = (v.x - a) * (q.v.y + q.v.z);
		D = (v.y + v.z) * (q.v.x - q.a);
		E = (v.x + v.z) * (q.v.x + q.v.y);
		F = (v.x - v.z) * (q.v.x - q.v.y);
		G = (a + v.y) * (q.a - q.v.z);
		H = (a - v.y) * (q.a + q.v.z);

		a = B + (-E - F + G + H) * 0.5f;
		v.x = A - ( E + F + G + H) * 0.5f;
		v.y = -C + ( E - F + G - H) * 0.5f;
		v.z = -D + ( E - F - G + H) * 0.5f;
		return *this;
	};

	inline Quat Quat::operator -() const
	{
		return Quat(-a, -v);
	};

	inline bool Quat::operator ==(const Quat& q) const
	{
		return (float_equal(a, q.a) && v == q.v);
	};

	inline bool Quat::operator !=(const Quat& q) const
	{
		return (float_notequal(a, q.a) || v != q.v);
	};

	inline float Quat::norm() const
	{
		return sqrtf(a*a + v.x*v.x + v.y*v.y + v.z*v.z);
	};

	inline float Quat::sqNorm() const
	{
		return a*a + v.x*v.x + v.y*v.y + v.z*v.z;
	};

	inline const Quat& Quat::invert()
	{
		float l = 1/sqNorm();
		a *= l;
		v *= -l;
		return *this;
	};

	inline Quat Quat::getInvert() const
	{
		float l = 1/sqNorm();
		return Quat(a*l, -v.x*l, -v.y*l, -v.z*l);
	};

	inline const Quat& Quat::fastInvert()
	{
		this->conj();
		return *this;
	};

	inline Quat Quat::getFastInvert() const
	{
		return Quat(a, -v.x, -v.y, -v.z);
	};

	inline const Quat& Quat::normalize()
	{
		float l = 1/norm();

		a *= l;
		v *= l;
		return *this;
	};

	inline Quat Quat::getNormalize() const
	{
		float l = 1/norm();
		return Quat(a*l, v*l);
	};

	inline const Quat& Quat::conj()
	{
		v = -v;
		return *this;
	};

	inline Quat Quat::getConj() const
	{
		return Quat(a, -v);
	};

	inline Matr3 Quat::getAsMatrix3() const
	{
		Matr3 m;

		float x2 = this->v.x + this->v.x;
		float y2 = this->v.y + this->v.y;
		float z2 = this->v.z + this->v.z;
		float xx = v.x * x2;
		float xy = v.x * y2;
		float xz = v.x * z2;
		float yy = v.y * y2;
		float yz = v.y * z2;
		float zz = v.z * z2;
		float wx = a * x2;
		float wy = a * y2;
		float wz = a * z2;

		m[0][0] = 1.0f-(yy+zz); 
		m[0][1] = xy-wz; 
		m[0][2] = xz+wy;
		m[1][0] = xy+wz; 
		m[1][1] = 1.0f-(xx+zz);
		m[1][2] = yz-wx;
		m[2][0] = xz-wy;
		m[2][1] = yz+wx;
		m[2][2] = 1.0f-(xx+yy);
		return m;
	};

	inline Matr4 Quat::getAsMatrix4() const
	{
		Matr4 m;	
		float x2 = this->v.x + this->v.x;
		float y2 = this->v.y + this->v.y;
		float z2 = this->v.z + this->v.z;
		float xx = v.x * x2;
		float xy = v.x * y2;
		float xz = v.x * z2;
		float yy = v.y * y2;
		float yz = v.y * z2;
		float zz = v.z * z2;
		float wx = a * x2;
		float wy = a * y2;
		float wz = a * z2;

		m[0][0] = 1.0f-(yy+zz); 
		m[0][1] = xy-wz; 
		m[0][2] = xz+wy;
		m[1][0] = xy+wz; 
		m[1][1] = 1.0f-(xx+zz);
		m[1][2] = yz-wx;
		m[2][0] = xz-wy;
		m[2][1] = yz+wx;
		m[2][2] = 1.0f-(xx+yy);

		m[0][3] = m[1][3] = m[2][3] = 0;
		m[3][0] = m[3][1] = m[3][2] = 0;
		m[3][3] = 1;
		return m;
	};

	inline Matr34 Quat::getAsMatrix34() const
	{
		Matr34 m;	
		float x2 = this->v.x + this->v.x;
		float y2 = this->v.y + this->v.y;
		float z2 = this->v.z + this->v.z;
		float xx = v.x * x2;
		float xy = v.x * y2;
		float xz = v.x * z2;
		float yy = v.y * y2;
		float yz = v.y * z2;
		float zz = v.z * z2;
		float wx = a * x2;
		float wy = a * y2;
		float wz = a * z2;

		m[0][0] = 1.0f-(yy+zz); 
		m[0][1] = xy-wz; 
		m[0][2] = xz+wy;
		m[1][0] = xy+wz; 
		m[1][1] = 1.0f-(xx+zz);
		m[1][2] = yz-wx;
		m[2][0] = xz-wy;
		m[2][1] = yz+wx;
		m[2][2] = 1.0f-(xx+yy);

		m[0][3] = m[1][3] = m[2][3] = 0;
		return m;
	};

	inline const Quat& Quat::setRotation(const float angle, const Vect3& axe)
	{
		float hang = angle/2;
		a = cosf(hang);
		v = axe.getNormalize()*sinf(hang);
		return *this;
	};

	inline const Quat& Quat::setRotation(const Matr3& m)
	{
		float tr, s, q[4];
		int i, j, k;
		int nxt[3] = {1, 2, 0};
		tr = m[0][0] + m[1][1] + m[2][2];
		if (tr > 0.0f)
		{
			s = sqrtf(tr + 1.0f);
			this->a = s / 2.0f;
			s = 0.5f / s;
			this->v.x = (m[2][1] - m[1][2]) * s;
			this->v.y = (m[0][2] - m[2][0]) * s;
			this->v.z = (m[1][0] - m[0][1]) * s;
		}
		else
		{
			i = 0;
			if (m[1][1] > m[0][0]) i = 1;
			if (m[2][2] > m[i][i]) i = 2;
			j = nxt[i];
			k = nxt[j];

			s = sqrtf((m[i][i] - (m[j][j] + m[k][k])) + 1.0f);

			q[i] = s * 0.5f;

			if (s != 0.0f) s = 0.5f / s;

			q[3] = (m[k][j] - m[j][k]) * s;
			q[j] = (m[j][i] + m[i][j]) * s;
			q[k] = (m[k][i] + m[i][k]) * s;

			this->v.x = q[0];
			this->v.y = q[1];
			this->v.z = q[2];
			this->a = q[3];
		};
		return *this;
	};

	inline const Quat& Quat::setRotation(const Matr4& m)
	{
		float tr, s, q[4];
		int i, j, k;
		int nxt[3] = {1, 2, 0};
		tr = m[0][0] + m[1][1] + m[2][2];
		if (tr > 0.0f)
		{
			s = sqrtf(tr + 1.0f);
			this->a = s / 2.0f;
			s = 0.5f / s;
			this->v.x = (m[2][1] - m[1][2]) * s;
			this->v.y = (m[0][2] - m[2][0]) * s;
			this->v.z = (m[1][0] - m[0][1]) * s;
		}
		else
		{
			i = 0;
			if (m[1][1] > m[0][0]) i = 1;
			if (m[2][2] > m[i][i]) i = 2;
			j = nxt[i];
			k = nxt[j];

			s = sqrtf((m[i][i] - (m[j][j] + m[k][k])) + 1.0f);

			q[i] = s * 0.5f;

			if (s != 0.0f) s = 0.5f / s;

			q[3] = (m[k][j] - m[j][k]) * s;
			q[j] = (m[j][i] + m[i][j]) * s;
			q[k] = (m[k][i] + m[i][k]) * s;

			this->v.x = q[0];
			this->v.y = q[1];
			this->v.z = q[2];
			this->a = q[3];
		};
		return *this;
	};

	inline const Quat& Quat::setRotation(const Matr34& m)
	{
		float  tr, s, q[4];
		int    i, j, k;
		int nxt[3] = {1, 2, 0};
		tr = m[0][0] + m[1][1] + m[2][2];
		if (tr > 0.0f)
		{
			s = sqrtf(tr + 1.0f);
			this->a = s / 2.0f;
			s = 0.5f / s;
			this->v.x = (m[2][1] - m[1][2]) * s;
			this->v.y = (m[0][2] - m[2][0]) * s;
			this->v.z = (m[1][0] - m[0][1]) * s;
		}
		else
		{
			i = 0;
			if (m[1][1] > m[0][0]) i = 1;
			if (m[2][2] > m[i][i]) i = 2;
			j = nxt[i];
			k = nxt[j];

			s = sqrtf((m[i][i] - (m[j][j] + m[k][k])) + 1.0f);

			q[i] = s * 0.5f;

			if (s != 0.0f) s = 0.5f / s;

			q[3] = (m[k][j] - m[j][k]) * s;
			q[j] = (m[j][i] + m[i][j]) * s;
			q[k] = (m[k][i] + m[i][k]) * s;

			this->v.x = q[0];
			this->v.y = q[1];
			this->v.z = q[2];
			this->a = q[3];
		};
		return *this;
	};
	
	inline Vect3 Quat::rotate(const Vect3& vec) const
	{
		Vect3 result;
		float x2 = v.x * v.x;
		float y2 = v.y * v.y;
		float z2 = v.z * v.z;
		float w2 = a * a;
		float xa = v.x * vec.x;
		float yb = v.y * vec.y;
		float zc = v.z * vec.z;

		result.x = vec.x * (x2 - y2 - z2 + w2) + 2 * (a * (v.y * vec.z - v.z * vec.y) + v.x * (yb + zc));
		result.y = vec.y * (-x2 + y2 - z2 + w2) + 2 * (a * (v.z * vec.x - v.x * vec.z) + v.y * (xa + zc));
		result.z = vec.z * (-x2 - y2 + z2 + w2) + 2 * (a * (v.x * vec.y - v.y * vec.x) + v.z * (xa + yb));
		return result;
	};

	inline Vect4 Quat::rotate(const Vect4& vec) const
	{
		Vect4 result;
		float x2 = v.x * v.x;
		float y2 = v.y * v.y;
		float z2 = v.z * v.z;
		float w2 = a * a;
		float xa = v.x * vec.x;
		float yb = v.y * vec.y;
		float zc = v.z * vec.z;

		result.x = vec.x * (x2 - y2 - z2 + w2) + 2 * (a * (v.y * vec.z - v.z * vec.y) + v.x * (yb + zc));
		result.y = vec.y * (-x2 + y2 - z2 + w2) + 2 * (a * (v.z * vec.y - v.x * vec.z) + v.y * (xa + zc));
		result.z = vec.z * (-x2 - y2 + z2 + w2) + 2 * (a * (v.x * vec.y - v.y * vec.x) + v.z * (xa + yb));
		result.w = 1;
	};

	inline Quat slerp(const Quat& q, const Quat& p, const float t)
	{
		float p1[4];
		float omega, cosom, sinom, scale0, scale1;
		cosom = q.v.x*p.v.x + q.v.y*p.v.y + q.v.z*p.v.z + q.a*p.a;
		if (cosom < 0.0f)
		{ 
			cosom = -cosom;
			p1[0] = -p.v.x;  p1[1] = -p.v.y;
			p1[2] = -p.v.z;  p1[3] = -p.a;
		}
		else
		{
			p1[0] = p.v.x;    p1[1] = p.v.y;
			p1[2] = p.v.z;    p1[3] = p.a;
		}
		if ((1.0f - cosom) > 0.001f)
		{
			omega = acosf(cosom);
			sinom = 1/sinf(omega);
			scale0 = sinf((1.0f - t)*omega)*sinom;
			scale1 = sinf(t * omega)*sinom;
		}
		else
		{        
			scale0 = 1.0f - t;
			scale1 = t;
		}
		return Quat(scale0*q.a + scale1*p1[3], scale0*q.v.x + scale1*p1[0], scale0*q.v.y + scale1*p1[1], scale0*q.v.z + scale1*p1[2]);
	};

	inline std::ostream& operator<<(std::ostream& os, const Quat& q)
	{
		os << "(" << q.a << ", " << q.v.x << ", " << q.v.y << ", " << q.v.z << ")";
		return os;
	};
};

