// Bullet Continuous Collision Detection and Physics Library
// Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
//
//
// Matrix.inl
//
// Copyright (c) 2006 Simon Hobbs
//
// This software is provided 'as-is', without any express or implied warranty. In no event will the authors be held liable for any damages arising from the use of this software.
//
// Permission is granted to anyone to use this software for any purpose, including commercial applications, and to alter it and redistribute it freely, subject to the following restrictions:
//
// 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
//
// 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
//
// 3. This notice may not be removed or altered from any source distribution.
#pragma once

#include <assert.h>
#include "math.h"

////////////////////////////////////////////////////////////////////////////////
// Matrix33

inline Matrix33::Matrix33()
{
}

inline Matrix33::Matrix33(const Vector3& x, const Vector3& y, const Vector3& z)
{
	m_rows[0] = x;
	m_rows[1] = y;
	m_rows[2] = z;
}

inline Matrix33::Matrix33(const Maths::ZeroTag&)
{
	m_rows[0] = Maths::Zero;
	m_rows[1] = Maths::Zero;
	m_rows[2] = Maths::Zero;
}

inline Matrix33::Matrix33(const Maths::IdentityTag&)
{
	m_rows[0] = Maths::UnitX;
	m_rows[1] = Maths::UnitY;
	m_rows[2] = Maths::UnitZ;
}

inline Matrix33::Matrix33(const Maths::RotateXTag&, float radians)
{
	float c = cosf(radians);
	float s = sinf(radians);

	m_rows[0] = Maths::UnitX;
	m_rows[1] = Vector3(0.0f, c, s);
	m_rows[2] = Vector3(0.0f, -s, c);
}

inline Matrix33::Matrix33(const Maths::RotateYTag&, float radians)
{
	float c = cosf(radians);
	float s = sinf(radians);

	m_rows[0] = Vector3(c, 0.0f, -s);
	m_rows[1] = Maths::UnitY;
	m_rows[2] = Vector3(s, 0.0f, c);
}

inline Matrix33::Matrix33(const Maths::RotateZTag&, float radians)
{
	float c = cosf(radians);
	float s = sinf(radians);

	m_rows[0] = Vector3(c, s, 0.0f);
	m_rows[1] = Vector3(-s, c, 0.0f);
	m_rows[2] = Maths::UnitZ;
}

inline Matrix33::Matrix33(const Maths::ScaleTag&, const Vector3& scale)
{
	m_rows[0] = scale * Vector3(Maths::UnitX);
	m_rows[1] = scale * Vector3(Maths::UnitY);
	m_rows[2] = scale * Vector3(Maths::UnitZ);
}

inline Matrix33::Matrix33(const Maths::SkewTag&, const Vector3& v)
{
	m_rows[0] = Vector3(0.0f, v[2], -v[1]);
	m_rows[1] = Vector3(-v[2], 0.0f, v[0]);
	m_rows[2] = Vector3(v[1], -v[0], 0.0f);
}

inline const Matrix33& Matrix33::operator=(const Matrix33& m)
{
	m_rows[0] = m.m_rows[0];
	m_rows[1] = m.m_rows[1];
	m_rows[2] = m.m_rows[2];
	return *this;
}

inline const Matrix33& Matrix33::operator=(const Maths::ZeroTag&)
{
	m_rows[0] = m_rows[1] = m_rows[2] = Maths::Zero;
	return *this;
}

inline const Matrix33& Matrix33::operator=(const Maths::IdentityTag&)
{
	m_rows[0] = Maths::UnitX;
	m_rows[1] = Maths::UnitY;
	m_rows[2] = Maths::UnitZ;
	return *this;
}

inline Vector3& Matrix33::operator[](int row)
{
	return m_rows[row];
}

inline const Vector3& Matrix33::operator[](int row) const
{
	return m_rows[row];
}

inline Vector3& Matrix33::GetAxisX()
{
	return m_rows[0];
}

inline const Vector3& Matrix33::GetAxisX() const
{
	return m_rows[0];
}

inline void Matrix33::SetAxisX(const Vector3& v)
{
	m_rows[0] = v;
}

inline Vector3& Matrix33::GetAxisY()
{
	return m_rows[1];
}

inline const Vector3& Matrix33::GetAxisY() const
{
	return m_rows[1];
}

inline void Matrix33::SetAxisY(const Vector3& v)
{
	m_rows[1] = v;
}

inline Vector3& Matrix33::GetAxisZ()
{
	return m_rows[2];
}

inline const Vector3& Matrix33::GetAxisZ() const
{
	return m_rows[2];
}

inline void Matrix33::SetAxisZ(const Vector3& v)
{
	m_rows[2] = v;
}

inline const Vector3 operator*(const Vector3& v, const Matrix33& m)
{
	Scalar xxxx = v.GetX();
	Scalar yyyy = v.GetY();
	Scalar zzzz = v.GetZ();

	return xxxx * m[0] + yyyy * m[1] + zzzz * m[2];
}

inline const Vector3 operator*(const Matrix33& m, const Vector3& vT)
{
	return Vector3(Dot(m[0], vT), Dot(m[1], vT), Dot(m[2], vT));
}

inline void Matrix33::operator*=(const Matrix33& a)
{
	*this = *this * a;
}

inline void Matrix33::operator*=(const Scalar& s)
{
	m_rows[0] *= s;
	m_rows[1] *= s;
	m_rows[2] *= s;
}

inline void Matrix33::operator+=(const Matrix33& a)
{
	m_rows[0] += a[0];
	m_rows[1] += a[1];
	m_rows[2] += a[2];
}

inline void Matrix33::operator-=(const Matrix33& a)
{
	m_rows[0] -= a[0];
	m_rows[1] -= a[1];
	m_rows[2] -= a[2];
}

inline const Matrix33 operator*(const Scalar& s, const Matrix33& m)
{
	Vector3 scale(s);
	return Matrix33(scale * m[0], scale * m[1], scale * m[2]);
}

inline const Matrix33 operator*(const Matrix33& m, const Scalar& s)
{
	Vector3 scale(s);
	return Matrix33(scale * m[0], scale * m[1], scale * m[2]);
}

inline const Matrix33 operator*(const Matrix33& a, const Matrix33& b)
{
	return Matrix33(a[0] * b, a[1] * b, a[2] * b);
}

inline const Matrix33 operator+(const Matrix33& a, const Matrix33& b)
{
	return Matrix33(a[0] + b[0], a[1] + b[1], a[2] + b[2]);
}

inline const Matrix33 operator-(const Matrix33& a, const Matrix33& b)
{
	return Matrix33(a[0] - b[0], a[1] - b[1], a[2] - b[2]);
}

inline const Matrix33 Transpose(const Matrix33& m)
{
	// easiest way is to actually do a 4 * 4 transpose with an implied zero bottom row:

	// a b c d			a e i 0
	// e f g h	 --->	b f j 0
	// i j k l			c g k 0
	// 0 0 0 0

	// shuffle the rows to make 4 quarters
	__m128 abef = _mm_shuffle_ps(m[0].base, m[1].base, _MM_SHUFFLE(1, 0, 1, 0));
	__m128 cdgh = _mm_shuffle_ps(m[0].base, m[1].base, _MM_SHUFFLE(3, 2, 3, 2));
	__m128 ij00 = _mm_shuffle_ps(m[2].base, _mm_setzero_ps(), _MM_SHUFFLE(1, 0, 1, 0));
	__m128 kl00 = _mm_shuffle_ps(m[2].base, _mm_setzero_ps(), _MM_SHUFFLE(3, 2, 3, 2));

	// shuffle the quarters to make new rows
	__m128 aei0 = _mm_shuffle_ps(abef, ij00, _MM_SHUFFLE(2, 0, 2, 0));
	__m128 bfj0 = _mm_shuffle_ps(abef, ij00, _MM_SHUFFLE(3, 1, 3, 1));
	__m128 cgk0 = _mm_shuffle_ps(cdgh, kl00, _MM_SHUFFLE(2, 0, 2, 0));

	return Matrix33(Vector3(aei0), Vector3(bfj0), Vector3(cgk0));
}

inline const Scalar Det(const Matrix33& m)
{
	return Dot(Cross(m.GetAxisX(), m.GetAxisY()),m.GetAxisZ());
}


////////////////////////////////////////////////////////////////////////////////
// Matrix44

inline Matrix44::Matrix44()
{
}

inline Matrix44::Matrix44(const Vector4& x, const Vector4& y, const Vector4& z, const Vector4& w)
{
	m_rows[0] = x;
	m_rows[1] = y;
	m_rows[2] = z;
	m_rows[3] = w;
}

inline Matrix44::Matrix44(const Maths::ZeroTag&)
{
	m_rows[0] = Maths::Zero;
	m_rows[1] = Maths::Zero;
	m_rows[2] = Maths::Zero;
	m_rows[3] = Maths::Zero;
}

inline Matrix44::Matrix44(const Maths::IdentityTag&)
{
	m_rows[0] = Maths::UnitX;
	m_rows[1] = Maths::UnitY;
	m_rows[2] = Maths::UnitZ;
	m_rows[3] = Maths::UnitW;
}

inline Matrix44::Matrix44(const Maths::RotateXTag&, float radians)
{
	float c = cosf(radians);
	float s = sinf(radians);

	m_rows[0] = Maths::UnitX;
	m_rows[1] = Vector4(0.0f, c, s, 0.0f);
	m_rows[2] = Vector4(0.0f, -s, c, 0.0f);
	m_rows[3] = Maths::UnitW;
}

inline Matrix44::Matrix44(const Maths::RotateYTag&, float radians)
{
	float c = cosf(radians);
	float s = sinf(radians);

	m_rows[0] = Vector4(c, 0.0f, -s, 0.0f);
	m_rows[1] = Maths::UnitY;
	m_rows[2] = Vector4(s, 0.0f, c, 0.0f);
	m_rows[3] = Maths::UnitW;
}

inline Matrix44::Matrix44(const Maths::RotateZTag&, float radians)
{
	float c = cosf(radians);
	float s = sinf(radians);

	m_rows[0] = Vector4(c, s, 0.0f, 0.0f);
	m_rows[1] = Vector4(-s, c, 0.0f, 0.0f);
	m_rows[2] = Maths::UnitZ;
	m_rows[3] = Maths::UnitW;
}

inline const Matrix44& Matrix44::operator=(const Matrix44& m)
{
	m_rows[0] = m.m_rows[0];
	m_rows[1] = m.m_rows[1];
	m_rows[2] = m.m_rows[2];
	m_rows[3] = m.m_rows[3];
	return *this;
}

inline const Matrix44& Matrix44::operator=(const Maths::ZeroTag&)
{
	m_rows[0] = m_rows[1] = m_rows[2] = m_rows[3] = Maths::Zero;
	return *this;
}

inline const Matrix44& Matrix44::operator=(const Maths::IdentityTag&)
{
	m_rows[0] = Maths::UnitX;
	m_rows[1] = Maths::UnitY;
	m_rows[2] = Maths::UnitZ;
	m_rows[3] = Maths::UnitW;
	return *this;
}

inline Vector4& Matrix44::operator[](int row)
{
	return m_rows[row];
}

inline const Vector4& Matrix44::operator[](int row) const
{
	return m_rows[row];
}

inline void Matrix44::operator*=(const Matrix44& a)
{
	*this = *this * a;
}

inline void Matrix44::operator*=(const Scalar& s)
{
	m_rows[0] *= s;
	m_rows[1] *= s;
	m_rows[2] *= s;
	m_rows[3] *= s;
}

inline void Matrix44::operator+=(const Matrix44& a)
{
	m_rows[0] += a[0];
	m_rows[1] += a[1];
	m_rows[2] += a[2];
	m_rows[3] += a[3];
}

inline void Matrix44::operator-=(const Matrix44& a)
{
	m_rows[0] -= a[0];
	m_rows[1] -= a[1];
	m_rows[2] -= a[2];
	m_rows[3] -= a[3];
}

inline const Vector3 operator*(const Vector3& v, const Matrix44& m)
{
	Scalar xxxx = v.GetX();
	Scalar yyyy = v.GetY();
	Scalar zzzz = v.GetZ();

	return xxxx * Vector3(m[0]) + yyyy * Vector3(m[1]) + zzzz * Vector3(m[2]);
}

inline const Point3 operator*(const Point3& v, const Matrix44& m)
{
	Scalar xxxx = v.GetX();
	Scalar yyyy = v.GetY();
	Scalar zzzz = v.GetZ();

	return Point3(xxxx * m[0] + yyyy * m[1] + zzzz * m[2] + m[3]);
}

inline const Vector4 operator*(const Vector4& v, const Matrix44& m)
{
	Scalar xxxx = v.GetX();
	Scalar yyyy = v.GetY();
	Scalar zzzz = v.GetZ();
	Scalar wwww = v.GetW();

	return xxxx * m[0] + yyyy * m[1] + zzzz * m[2] + wwww * m[3];
}

inline const Matrix44 operator*(const Matrix44& a, const Matrix44& b)
{
	return Matrix44(a[0] * b, a[1] * b, a[2] * b, a[3] * b);
}

inline const Matrix44 operator*(const Matrix44& m, const Scalar& s)
{
	Vector4 scale(s);
	return Matrix44(scale * m[0], scale * m[1], scale * m[2], scale * m[3]);
}

inline const Matrix44 operator*(const Scalar& s, const Matrix44& m)
{
	Vector4 scale(s);
	return Matrix44(scale * m[0], scale * m[1], scale * m[2], scale * m[3]);
}

inline const Matrix44 operator+(const Matrix44& a, const Matrix44& b)
{
	return Matrix44(a[0] + b[0], a[1] + b[1], a[2] + b[2], a[3] + b[3]);
}

inline const Matrix44 operator-(const Matrix44& a, const Matrix44& b)
{
	return Matrix44(a[0] - b[0], a[1] - b[1], a[2] - b[2], a[3] - b[3]);
}

inline const Matrix44 Transpose(const Matrix44& m)
{
	// a b c d			a e i m
	// e f g h	 --->	b f j n
	// i j k l			c g k o
	// m n o p			d h l p

	// shuffle the rows to make 4 quarters
	__m128 abef = _mm_shuffle_ps(m[0].base, m[1].base, _MM_SHUFFLE(1, 0, 1, 0));
	__m128 cdgh = _mm_shuffle_ps(m[0].base, m[1].base, _MM_SHUFFLE(3, 2, 3, 2));
	__m128 ijmn = _mm_shuffle_ps(m[2].base, m[3].base, _MM_SHUFFLE(1, 0, 1, 0));
	__m128 klop = _mm_shuffle_ps(m[2].base, m[3].base, _MM_SHUFFLE(3, 2, 3, 2));

	// shuffle the quarters to make new rows
	__m128 aeim = _mm_shuffle_ps(abef, ijmn, _MM_SHUFFLE(2, 0, 2, 0));
	__m128 bfjn = _mm_shuffle_ps(abef, ijmn, _MM_SHUFFLE(3, 1, 3, 1));
	__m128 cgko = _mm_shuffle_ps(cdgh, klop, _MM_SHUFFLE(2, 0, 2, 0));
	__m128 dhlp = _mm_shuffle_ps(cdgh, klop, _MM_SHUFFLE(3, 1, 3, 1));

	return Matrix44(Vector4(aeim), Vector4(bfjn), Vector4(cgko), Vector4(dhlp));
}


////////////////////////////////////////////////////////////////////////////////
// Transform

inline Transform::Transform()
{
}

inline Transform::Transform(const Matrix33& xyz, const Point3& w)
{
	m_rotation = xyz;
	m_translation = w;
}

inline Transform::Transform(const Vector3& x, const Vector3& y, const Vector3& z, const Point3& w)
{
	m_rotation[0] = x;
	m_rotation[1] = y;
	m_rotation[2] = z;
	m_translation = w;
}

inline Transform::Transform(const Maths::IdentityTag&)
{
	m_rotation = Maths::Identity;
	m_translation = Maths::Zero;
}

inline const Transform& Transform::operator=(const Transform& m)
{
	m_rotation = m.m_rotation;
	m_translation = m.m_translation;
	return *this;
}

inline const Transform& Transform::operator=(const Maths::IdentityTag&)
{
	m_rotation = Maths::Identity;
	m_translation = Maths::Zero;
	return *this;
}

inline Matrix33& Transform::GetRotation()
{
	return m_rotation;
}

inline const Matrix33& Transform::GetRotation() const
{
	return m_rotation;
}

inline void Transform::SetRotation(const Matrix33& m)
{
	m_rotation = m;
}

inline void Transform::SetRotation(const Quat& q)
{
	m_rotation = Matrix33(q);
}

inline Point3& Transform::GetTranslation()
{
	return m_translation;
}

inline const Point3& Transform::GetTranslation() const
{
	return m_translation;
}

inline void Transform::SetTranslation(const Point3& t)
{
	m_translation = t;
}

inline Vector3& Transform::GetAxisX()
{
	return m_rotation[0];
}

inline const Vector3& Transform::GetAxisX() const
{
	return m_rotation[0];
}

inline Vector3& Transform::GetAxisY()
{
	return m_rotation[1];
}

inline const Vector3& Transform::GetAxisY() const
{
	return m_rotation[1];
}

inline Vector3& Transform::GetAxisZ()
{
	return m_rotation[2];
}

inline const Vector3& Transform::GetAxisZ() const
{
	return m_rotation[2];
}

inline const Vector3 operator*(const Vector3& v, const Transform& m)
{
	Scalar xxxx = v.GetX();
	Scalar yyyy = v.GetY();
	Scalar zzzz = v.GetZ();

	return xxxx * m.GetAxisX() + yyyy * m.GetAxisY() + zzzz * m.GetAxisZ();
}

inline const Point3 operator*(const Point3& v, const Transform& m)
{
	Scalar xxxx = v.GetX();
	Scalar yyyy = v.GetY();
	Scalar zzzz = v.GetZ();

	return xxxx * m.GetAxisX() + yyyy * m.GetAxisY() + zzzz * m.GetAxisZ() + m.GetTranslation();
}

inline const Transform operator*(const Transform& a, const Transform& b)
{
	return Transform(a.GetAxisX() * b, a.GetAxisY() * b, a.GetAxisZ() * b, a.GetTranslation() * b);
}

inline const Transform Inv(const Transform& m)
{
	Matrix33 r(Transpose(m.GetRotation()));
	Point3 t = m.GetTranslation();
	t = Point3(-(t.GetX() * r.GetAxisX() + t.GetY() * r.GetAxisY() + t.GetZ() * r.GetAxisZ()));

	return Transform(r, t); 
}