//
//
//  Typical 3d vector math code.
//  By S Melax 1998-2008
//
//

#ifndef SM_VEC_MATH_H
#define SM_VEC_MATH_H

#include <stdio.h>
#include <math.h>
#include <assert.h>
#include <xmmintrin.h>

#define M_PIf (3.1415926535897932384626433832795f)

inline float DegToRad(float angle_degrees) { return angle_degrees * M_PIf / 180.0f; }  // returns Radians.
inline float RadToDeg(float angle_radians) { return angle_radians * 180.0f / M_PIf; }  // returns Degrees.

#define OFFSET(Class, Member) (((char *)(&(((Class *)NULL)->Member))) - ((char *)NULL))

int argmin(const float a[], int n);
int argmax(const float a[], int n);
float squared(float a);
float clamp(float a, const float minval = 0.0f, const float maxval = 1.0f);
int clamp(int a, const int minval, const int maxval);
float Round(float a, float precision);
float Interpolate(const float &f0, const float &f1, float alpha);

template <class T>
void Swap(T &a, T &b)
{
	T tmp = a;
	a = b;
	b = tmp;
}

template <class T>
T Max(const T &a, const T &b)
{
	return (a > b) ? a : b;
}

template <class T>
T Min(const T &a, const T &b)
{
	return (a < b) ? a : b;
}

//for template normalize functions:
inline float squareroot(float a) { return sqrtf(a); }
inline double squareroot(double a) { return sqrt(a); }

//----------------------------------

//-------- 2D --------

template <class T>
class vec2
{
public:
	T x, y;
	inline vec2()
	{
		x = 0;
		y = 0;
	}
	inline vec2(const T &_x, const T &_y)
	{
		x = _x;
		y = _y;
	}
	inline T &operator[](int i) { return ((T *)this)[i]; }
	inline const T &operator[](int i) const { return ((T *)this)[i]; }
};

typedef vec2<int> int2;
typedef vec2<float> float2;

template <class T>
inline int operator==(const vec2<T> &a, const vec2<T> &b)
{
	return (a.x == b.x && a.y == b.y);
}
template <class T>
inline vec2<T> operator-(const vec2<T> &a, const vec2<T> &b)
{
	return vec2<T>(a.x - b.x, a.y - b.y);
}
template <class T>
inline vec2<T> operator+(const vec2<T> &a, const vec2<T> &b)
{
	return float2(a.x + b.x, a.y + b.y);
}

//--------- 3D ---------

template <class T>
class vec3
{
public:
	T x, y, z;
	inline vec3()
	{
		x = 0;
		y = 0;
		z = 0;
	};
	inline vec3(const T &_x, const T &_y, const T &_z)
	{
		x = _x;
		y = _y;
		z = _z;
	};
	inline T &operator[](int i) { return ((T *)this)[i]; }
	inline const T &operator[](int i) const { return ((T *)this)[i]; }
};

typedef vec3<int> int3;
typedef vec3<short> short3;
typedef vec3<float> float3;

// due to ambiguity there is no overloaded operators for v3*v3 use dot,cross,outerprod,cmul
template <class T>
inline int operator==(const vec3<T> &a, const vec3<T> &b)
{
	return (a.x == b.x && a.y == b.y && a.z == b.z);
}
template <class T>
inline int operator!=(const vec3<T> &a, const vec3<T> &b)
{
	return !(a == b);
}
template <class T>
inline vec3<T> operator+(const vec3<T> &a, const vec3<T> &b)
{
	return vec3<T>(a.x + b.x, a.y + b.y, a.z + b.z);
}
template <class T>
inline vec3<T> operator-(const vec3<T> &a, const vec3<T> &b)
{
	return vec3<T>(a.x - b.x, a.y - b.y, a.z - b.z);
}
template <class T>
inline vec3<T> operator-(const vec3<T> &v)
{
	return vec3<T>(-v.x, -v.y, -v.z);
}
template <class T>
inline vec3<T> operator*(const vec3<T> &v, const T &s)
{
	return vec3<T>(v.x * s, v.y * s, v.z * s);
}
template <class T>
inline vec3<T> operator*(T s, const vec3<T> &v)
{
	return v * s;
}
template <class T>
inline vec3<T> operator/(const vec3<T> &v, T s)
{
	return vec3<T>(v.x / s, v.y / s, v.z / s);
}
template <class T>
inline T dot(const vec3<T> &a, const vec3<T> &b)
{
	return a.x * b.x + a.y * b.y + a.z * b.z;
}
template <class T>
inline vec3<T> cmul(const vec3<T> &a, const vec3<T> &b)
{
	return vec3<T>(a.x * b.x, a.y * b.y, a.z * b.z);
}
template <class T>
inline vec3<T> cross(const vec3<T> &a, const vec3<T> &b)
{
	return vec3<T>(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
}
template <class T>
inline T magnitude(const vec3<T> &v)
{
	return squareroot(dot(v, v));
}
template <class T>
inline vec3<T> normalize(const vec3<T> &v)
{
	return v / magnitude(v);
}
template <class T>
inline vec3<T> &operator+=(vec3<T> &a, const vec3<T> &b)
{
	a.x += b.x;
	a.y += b.y;
	a.z += b.z;
	return a;
}
template <class T>
inline vec3<T> &operator-=(vec3<T> &a, const vec3<T> &b)
{
	a.x -= b.x;
	a.y -= b.y;
	a.z -= b.z;
	return a;
}
template <class T>
inline vec3<T> &operator*=(vec3<T> &v, T s)
{
	v.x *= s;
	v.y *= s;
	v.z *= s;
	return v;
}
template <class T>
inline vec3<T> &operator/=(vec3<T> &v, T s)
{
	v.x /= s;
	v.y /= s;
	v.z /= s;
	return v;
}

float3 safenormalize(const float3 &v);
float3 vabs(const float3 &v);
float3 Interpolate(const float3 &v0, const float3 &v1, float alpha);
float3 Round(const float3 &a, float precision);
template <class T>
inline vec3<T> VectorMin(const vec3<T> &a, const vec3<T> &b)
{
	return vec3<T>(Min(a.x, b.x), Min(a.y, b.y), Min(a.z, b.z));
}
template <class T>
inline vec3<T> VectorMax(const vec3<T> &a, const vec3<T> &b)
{
	return vec3<T>(Max(a.x, b.x), Max(a.y, b.y), Max(a.z, b.z));
}
int overlap(const float3 &bmina, const float3 &bmaxa, const float3 &bminb, const float3 &bmaxb);

template <class T>
class mat3x3
{
public:
	vec3<T> x, y, z;  // the 3 rows of the Matrix
	inline mat3x3() {}
	inline mat3x3(const T &xx, const T &xy, const T &xz, const T &yx, const T &yy, const T &yz, const T &zx, const T &zy, const T &zz) : x(xx, xy, xz), y(yx, yy, yz), z(zx, zy, zz) {}
	inline mat3x3(const vec3<T> &_x, const vec3<T> &_y, const vec3<T> &_z) : x(_x), y(_y), z(_z) {}
	inline vec3<T> &operator[](int i) { return (&x)[i]; }
	inline const vec3<T> &operator[](int i) const { return (&x)[i]; }
	inline T &operator()(int r, int c) { return ((&x)[r])[c]; }
	inline const T &operator()(int r, int c) const { return ((&x)[r])[c]; }
};
typedef mat3x3<float> float3x3;

float3x3 Transpose(const float3x3 &m);
template <class T>
vec3<T> operator*(const vec3<T> &v, const mat3x3<T> &m)
{
	return vec3<T>((m.x.x * v.x + m.y.x * v.y + m.z.x * v.z),
				   (m.x.y * v.x + m.y.y * v.y + m.z.y * v.z),
				   (m.x.z * v.x + m.y.z * v.y + m.z.z * v.z));
}

float3 operator*(const float3x3 &m, const float3 &v);
float3x3 operator*(const float3x3 &m, const float &s);
float3x3 operator*(const float3x3 &ma, const float3x3 &mb);
float3x3 operator/(const float3x3 &a, const float &s);
float3x3 operator+(const float3x3 &a, const float3x3 &b);
float3x3 operator-(const float3x3 &a, const float3x3 &b);
float3x3 &operator+=(float3x3 &a, const float3x3 &b);
float3x3 &operator-=(float3x3 &a, const float3x3 &b);
float3x3 &operator*=(float3x3 &a, const float &s);
float Determinant(const float3x3 &m);
float3x3 Inverse(const float3x3 &a);  // its just 3x3 so we simply do that cofactor method
float3x3 outerprod(const float3 &a, const float3 &b);

//-------- 4D Math --------

template <class T>
class vec4
{
public:
	T x, y, z, w;
	inline vec4()
	{
		x = 0;
		y = 0;
		z = 0;
		w = 0;
	};
	inline vec4(const T &_x, const T &_y, const T &_z, const T &_w)
	{
		x = _x;
		y = _y;
		z = _z;
		w = _w;
	}
	inline vec4(const vec3<T> &v, const T &_w)
	{
		x = v.x;
		y = v.y;
		z = v.z;
		w = _w;
	}
	//operator float *() { return &x;};
	T &operator[](int i) { return ((T *)this)[i]; }
	const T &operator[](int i) const { return ((T *)this)[i]; }
	inline const vec3<T> &xyz() const { return *((vec3<T> *)this); }
	inline vec3<T> &xyz() { return *((vec3<T> *)this); }
};

typedef vec4<float> float4;
typedef vec4<int> int4;
typedef vec4<unsigned char> byte4;

template <class T>
inline int operator==(const vec4<T> &a, const vec4<T> &b)
{
	return (a.x == b.x && a.y == b.y && a.z == b.z && a.w == b.w);
}
template <class T>
inline int operator!=(const vec4<T> &a, const vec4<T> &b)
{
	return !(a == b);
}
template <class T>
inline vec4<T> operator+(const vec4<T> &a, const vec4<T> &b)
{
	return vec4<T>(a.x + b.x, a.y + b.y, a.z + b.z, a.w + b.w);
}
template <class T>
inline vec4<T> operator-(const vec4<T> &a, const vec4<T> &b)
{
	return vec4<T>(a.x - b.x, a.y - b.y, a.z - b.z, a.w - b.w);
}
template <class T>
inline vec4<T> operator-(const vec4<T> &v)
{
	return vec4<T>(-v.x, -v.y, -v.z, -v.w);
}
template <class T>
inline vec4<T> operator*(const vec4<T> &v, const T &s)
{
	return vec4<T>(v.x * s, v.y * s, v.z * s, v.w * s);
}
template <class T>
inline vec4<T> operator*(T s, const vec4<T> &v)
{
	return v * s;
}
template <class T>
inline vec4<T> operator/(const vec4<T> &v, T s)
{
	return vec4<T>(v.x / s, v.y / s, v.z / s, v.w / s);
}
template <class T>
inline T dot(const vec4<T> &a, const vec4<T> &b)
{
	return a.x * b.x + a.y * b.y + a.z * b.z + a.w * b.w;
}
template <class T>
inline vec4<T> cmul(const vec4<T> &a, const vec4<T> &b)
{
	return vec4<T>(a.x * b.x, a.y * b.y, a.z * b.z, a.w * b.w);
}
template <class T>
inline vec4<T> &operator+=(vec4<T> &a, const vec4<T> &b)
{
	a.x += b.x;
	a.y += b.y;
	a.z += b.z;
	a.w += b.w;
	return a;
}
template <class T>
inline vec4<T> &operator-=(vec4<T> &a, const vec4<T> &b)
{
	a.x -= b.x;
	a.y -= b.y;
	a.z -= b.z;
	a.w -= b.w;
	return a;
}
template <class T>
inline vec4<T> &operator*=(vec4<T> &v, T s)
{
	v.x *= s;
	v.y *= s;
	v.z *= s;
	v.w *= s;
	return v;
}
template <class T>
inline vec4<T> &operator/=(vec4<T> &v, T s)
{
	v.x /= s;
	v.y /= s;
	v.z /= s;
	v.w /= s;
	return v;
}
template <class T>
inline T magnitude(const vec4<T> &v)
{
	return squareroot(dot(v, v));
}
template <class T>
inline vec4<T> normalize(const vec4<T> &v)
{
	return v / magnitude(v);
}

struct D3DXMATRIX;

template <class T>
class mat4x4
{
public:
	vec4<T> x, y, z, w;  // the 4 rows
	inline mat4x4() {}
	inline mat4x4(const vec4<T> &_x, const vec4<T> &_y, const vec4<T> &_z, const vec4<T> &_w) : x(_x), y(_y), z(_z), w(_w) {}
	inline mat4x4(const T &m00, const T &m01, const T &m02, const T &m03,
				  const T &m10, const T &m11, const T &m12, const T &m13,
				  const T &m20, const T &m21, const T &m22, const T &m23,
				  const T &m30, const T &m31, const T &m32, const T &m33)
		: x(m00, m01, m02, m03), y(m10, m11, m12, m13), z(m20, m21, m22, m23), w(m30, m31, m32, m33) {}
	inline vec4<T> &operator[](int i)
	{
		assert(i >= 0 && i < 4);
		return (&x)[i];
	}
	inline const vec4<T> &operator[](int i) const
	{
		assert(i >= 0 && i < 4);
		return (&x)[i];
	}
	inline T &operator()(int r, int c)
	{
		assert(r >= 0 && r < 4 && c >= 0 && c < 4);
		return ((&x)[r])[c];
	}
	inline const T &operator()(int r, int c) const
	{
		assert(r >= 0 && r < 4 && c >= 0 && c < 4);
		return ((&x)[r])[c];
	}
	inline operator T *() { return &x.x; }
	inline operator const T *() const { return &x.x; }
	operator struct D3DXMATRIX *() { return (struct D3DXMATRIX *)this; }
	operator const struct D3DXMATRIX *() const { return (struct D3DXMATRIX *)this; }
};

typedef mat4x4<float> float4x4;

float4x4 operator*(const float4x4 &a, const float4x4 &b);
float4 operator*(const float4 &v, const float4x4 &m);
float4x4 Inverse(const float4x4 &m);
float4x4 MatrixRigidInverse(const float4x4 &m);
float4x4 MatrixTranspose(const float4x4 &m);
float4x4 MatrixPerspectiveFov(float fovy, float Aspect, float zn, float zf);
float4x4 MatrixTranslation(const float3 &t);
float4x4 MatrixRotationZ(const float angle_radians);
float4x4 MatrixLookAt(const float3 &eye, const float3 &at, const float3 &up);
int operator==(const float4x4 &a, const float4x4 &b);

//-------- Quaternion ------------

template <class T>
class quaternion : public vec4<T>
{
public:
	inline quaternion()
	{
		this->x = this->y = this->z = 0.0f;
		this->w = 1.0f;
	}
	inline quaternion(const T &_x, const T &_y, const T &_z, const T &_w)
	{
		this->x = _x;
		this->y = _y;
		this->z = _z;
		this->w = _w;
	}
	inline explicit quaternion(const vec4<T> &v) : vec4<T>(v) {}
	T angle() const { return acosf(this->w) * 2.0f; }
	vec3<T> axis() const
	{
		vec3<T> a(this->x, this->y, this->z);
		if (fabsf(angle()) < 0.0000001f) return vec3<T>(1, 0, 0);
		return a * (1 / sinf(angle() / 2.0f));
	}
	inline vec3<T> xdir() const { return vec3<T>(1 - 2 * (this->y * this->y + this->z * this->z), 2 * (this->x * this->y + this->w * this->z),
												 2 * (this->x * this->z - this->w * this->y)); }
	inline vec3<T> ydir() const { return vec3<T>(2 * (this->x * this->y - this->w * this->z), 1 - 2 * (this->x * this->x + this->z * this->z), 2 * (this->y * this->z + this->w * this->x)); }
	inline vec3<T> zdir() const { return vec3<T>(2 * (this->x * this->z + this->w * this->y),
												 2 * (this->y * this->z - this->w * this->x), 1 - 2 * (this->x * this->x + this->y * this->y)); }
	inline mat3x3<T> getmatrix() const { return mat3x3<T>(xdir(), ydir(), zdir()); }
	//operator float3x3() { return getmatrix(); }
	void Normalize();
};

template <class T>
inline quaternion<T> quatfrommat(const mat3x3<T> &m)
{
	T magw = m[0][0] + m[1][1] + m[2][2];
	T magxy;
	T magzw;
	vec3<T> pre;
	vec3<T> prexy;
	vec3<T> prezw;
	quaternion<T> postxy;
	quaternion<T> postzw;
	quaternion<T> post;
	int wvsz = (magw > m[2][2]);
	magzw = (wvsz) ? magw : m[2][2];
	prezw = (wvsz) ? vec3<T>(1.0f, 1.0f, 1.0f) : vec3<T>(-1.0f, -1.0f, 1.0f);
	postzw = (wvsz) ? quaternion<T>(0.0f, 0.0f, 0.0f, 1.0f) : quaternion<T>(0.0f, 0.0f, 1.0f, 0.0f);
	int xvsy = (m[0][0] > m[1][1]);
	magxy = (xvsy) ? m[0][0] : m[1][1];
	prexy = (xvsy) ? vec3<T>(1.0f, -1.0f, -1.0f) : vec3<T>(-1.0f, 1.0f, -1.0f);
	postxy = (xvsy) ? quaternion<T>(1.0f, 0.0f, 0.0f, 0.0f) : quaternion<T>(0.0f, 1.0f, 0.0f, 0.0f);
	int zwvsxy = (magzw > magxy);
	pre = (zwvsxy) ? prezw : prexy;
	post = (zwvsxy) ? postzw : postxy;

	T t = pre.x * m[0][0] + pre.y * m[1][1] + pre.z * m[2][2] + 1.0f;
	T s = 1 / sqrt(t) * 0.5f;
	quaternion<T> qp;
	qp.x = (pre.y * m[1][2] - pre.z * m[2][1]) * s;
	qp.y = (pre.z * m[2][0] - pre.x * m[0][2]) * s;
	qp.z = (pre.x * m[0][1] - pre.y * m[1][0]) * s;
	qp.w = t * s;
	return qp * post;
}

typedef quaternion<float> Quaternion;

inline Quaternion QuatFromAxisAngle(const float3 &_v, float angle_radians)
{
	float3 v = normalize(_v) * sinf(angle_radians / 2.0f);
	return Quaternion(v.x, v.y, v.z, cosf(angle_radians / 2.0f));
}

template <class T>
inline quaternion<T> Conjugate(const quaternion<T> &q)
{
	return quaternion<T>(-q.x, -q.y, -q.z, q.w);
}
template <class T>
inline quaternion<T> Inverse(const quaternion<T> &q)
{
	return Conjugate(q);
}
template <class T>
inline quaternion<T> normalize(const quaternion<T> &a)
{
	return quaternion<T>(normalize((vec4<T> &)a));
}
template <class T>
inline quaternion<T> &operator*=(quaternion<T> &a, T s)
{
	return (quaternion<T> &)((vec4<T> &)a *= s);
}
template <class T>
inline quaternion<T> operator*(const quaternion<T> &a, float s)
{
	return quaternion<T>((vec4<T> &)a * s);
}
template <class T>
inline quaternion<T> operator+(const quaternion<T> &a, const quaternion<T> &b)
{
	return quaternion<T>((vec4<T> &)a + (vec4<T> &)b);
}
template <class T>
inline quaternion<T> operator-(const quaternion<T> &a, const quaternion<T> &b)
{
	return quaternion<T>((vec4<T> &)a - (vec4<T> &)b);
}
template <class T>
inline quaternion<T> operator-(const quaternion<T> &b)
{
	return quaternion<T>(-(vec4<T> &)b);
}
template <class T>
inline quaternion<T> operator*(const quaternion<T> &a, const quaternion<T> &b)
{
	return quaternion<T>(
		a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y,   //x
		a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x,   //y
		a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w,   //z
		a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z);  //w
}

float3 rotate(const Quaternion &q, const float3 &v);
//float3		operator*( const Quaternion& q, const float3& v );
//float3		operator*( const float3& v, const Quaternion& q );

Quaternion slerp(const Quaternion &a, const Quaternion &b, float t);
Quaternion Interpolate(const Quaternion &q0, const Quaternion &q1, float t);
Quaternion RotationArc(float3 v0, float3 v1);  // returns quat q where q*v0*q^-1=v1
float4x4 MatrixFromQuatVec(const Quaternion &q, const float3 &v);

inline Quaternion QuatFromMat(const float3 &t, const float3 &b, const float3 &n)
{
	return normalize(quatfrommat<float>(float3x3(t, b, n)));
}

//---------------- Pose ------------------

class Pose
{
public:
	float3 position;
	Quaternion orientation;
	Pose() {}
	Pose(const float3 &p, const Quaternion &q) : position(p), orientation(q) {}
	Pose &pose() { return *this; }
	const Pose &pose() const { return *this; }
};

inline float3 operator*(const Pose &a, const float3 &v)
{
	return a.position + rotate(a.orientation, v);
}

inline Pose operator*(const Pose &a, const Pose &b)
{
	return Pose(a.position + rotate(a.orientation, b.position), a.orientation * b.orientation);
}

inline Pose Inverse(const Pose &a)
{
	Quaternion q = Inverse(a.orientation);
	return Pose(rotate(q, -a.position), q);
}

inline Pose slerp(const Pose &p0, const Pose &p1, float t)
{
	return Pose(p0.position * (1.0f - t) + p1.position * t, slerp(p0.orientation, p1.orientation, t));
}

inline float4x4 MatrixFromPose(const Pose &pose)
{
	return MatrixFromQuatVec(pose.orientation, pose.position);
}

//------ Euler Angle -----

Quaternion YawPitchRoll(float yaw, float pitch, float roll);
float Yaw(const Quaternion &q);
float Pitch(const Quaternion &q);
float Roll(const Quaternion &q);
float Yaw(const float3 &v);
float Pitch(const float3 &v);

//------- Plane ----------
class Plane : public float4
{
public:
	float3 &normal() { return xyz(); }
	const float3 &normal() const { return xyz(); }
	float &dist() { return w; }              // distance below origin - the D from plane equasion Ax+By+Cz+D=0
	const float &dist() const { return w; }  // distance below origin - the D from plane equasion Ax+By+Cz+D=0
	Plane(const float3 &n, float d) : float4(n, d) {}
	Plane() { dist() = 0; }
	explicit Plane(const float4 &v) : float4(v) {}
};

Plane Transform(const Plane &p, const float3 &translation, const Quaternion &rotation);

inline Plane PlaneFlip(const Plane &p) { return Plane(-p.normal(), -p.dist()); }
inline int operator==(const Plane &a, const Plane &b) { return (a.normal() == b.normal() && a.dist() == b.dist()); }
inline int coplanar(const Plane &a, const Plane &b) { return (a == b || a == PlaneFlip(b)); }

float3 PlaneLineIntersection(const Plane &plane, const float3 &p0, const float3 &p1);
float3 PlaneProject(const Plane &plane, const float3 &point);
float3 PlanesIntersection(const Plane &p0, const Plane &p1, const Plane &p2);
float3 PlanesIntersection(const Plane *planes, int planes_count, const float3 &seed = float3(0, 0, 0));

int Clip(const Plane &p, const float3 *verts_in, int count, float *verts_out);                                                            // verts_out must be preallocated with sufficient size >= count+1 or more if concave
int ClipPolyPoly(const float3 &normal, const float3 *clipper, int clipper_count, const float3 *verts_in, int in_count, float3 *scratch);  //scratch must be preallocated

//--------- Utility Functions ------

float3 PlaneLineIntersection(const float3 &normal, const float dist, const float3 &p0, const float3 &p1);
float3 LineProject(const float3 &p0, const float3 &p1, const float3 &a);  // projects a onto infinite line p0p1
float LineProjectTime(const float3 &p0, const float3 &p1, const float3 &a);
int BoxInside(const float3 &p, const float3 &bmin, const float3 &bmax);
int BoxIntersect(const float3 &v0, const float3 &v1, const float3 &bmin, const float3 &bmax, float3 *impact);
float DistanceBetweenLines(const float3 &ustart, const float3 &udir, const float3 &vstart, const float3 &vdir, float3 *upoint = NULL, float3 *vpoint = NULL);
float3 TriNormal(const float3 &v0, const float3 &v1, const float3 &v2);
float3 NormalOf(const float3 *vert, const int n);
Quaternion VirtualTrackBall(const float3 &cop, const float3 &cor, const float3 &dir0, const float3 &dir1);
int Clip(const float3 &plane_normal, float plane_dist, const float3 *verts_in, int count, float *verts_out);                              // verts_out must be preallocated with sufficient size >= count+1 or more if concave
int ClipPolyPoly(const float3 &normal, const float3 *clipper, int clipper_count, const float3 *verts_in, int in_count, float3 *scratch);  //scratch must be preallocated
float3 Diagonal(const float3x3 &M);
Quaternion Diagonalizer(const float3x3 &A);
float3 Orth(const float3 &v);
int SolveQuadratic(float a, float b, float c, float *ta, float *tb);  // if true returns roots ta,tb where ta<=tb
int HitCheckPoly(const float3 *vert, const int n, const float3 &v0, const float3 &v1, float3 *impact = NULL, float3 *normal = NULL);
int HitCheckRaySphere(const float3 &sphereposition, float radius, const float3 &_v0, const float3 &_v1, float3 *impact, float3 *normal);
int HitCheckRayCylinder(const float3 &p0, const float3 &p1, float radius, const float3 &_v0, const float3 &_v1, float3 *impact, float3 *normal);
int HitCheckSweptSphereTri(const float3 &p0, const float3 &p1, const float3 &p2, float radius, const float3 &v0, const float3 &_v1, float3 *impact, float3 *normal);
void BoxLimits(const float3 *verts, int verts_count, float3 &bmin_out, float3 &bmax_out);
void BoxLimits(const float4 *verts, int verts_count, float3 &bmin_out, float3 &bmax_out);

template <class T>
inline int maxdir(const T *p, int count, const T &dir)
{
	assert(count);
	int m = 0;
	for (int i = 1; i < count; i++)
	{
		if (dot(p[i], dir) > dot(p[m], dir)) m = i;
	}
	return m;
}

float3 CenterOfMass(const float3 *vertices, const int3 *tris, const int count);
float3x3 Inertia(const float3 *vertices, const int3 *tris, const int count, const float3 &com = float3(0, 0, 0));
float Volume(const float3 *vertices, const int3 *tris, const int count);
int calchull(float3 *verts, int verts_count, int3 *&tris_out, int &tris_count, int vlimit);  // computes convex hull see hull.cpp

#endif  // VEC_MATH_H