/*
Fast template math class by M Phillips - 2005.

This code is provided as is with no warranties or guarantees of
any kind.

Please send an email to M Phillips (mbp2@i4free.co.nz)
  - if you use this file in a released product, or
  - if you find any bugs, or
  - if you have any suggestions
*/

#ifndef TEMPLATED_VECTOR_H
#define TEMPLATED_VECTOR_H

#include <cmath>
#include <iostream>
#include <assert.h>
#include <stdlib.h>

#ifndef _DEBUG
  #ifdef _MSC_VER
	#pragma inline_depth( 255 )
	#pragma inline_recursion( on )
	#pragma auto_inline( on )
	#define inline __forceinline
  #endif
	#define EXPR_TMPL
#endif

#ifdef EXPR_TMPL
//Expression Templates
template<class t, class lExpr, class rExpr> class add_expr {
	const lExpr m_a; const rExpr m_b;
public:
	inline add_expr(const lExpr &a, const rExpr &b) : m_a(a), m_b(b) {}
	inline const t operator [] (const ptrdiff_t i) const { return m_a[i] + m_b[i]; }
private:
	add_expr& operator =(const add_expr&);
};
template<class t, class lExpr, class rExpr> class sub_expr {
	const lExpr m_a; const rExpr m_b;
public:
	inline sub_expr(const lExpr &a, const rExpr &b) : m_a(a), m_b(b) {}
	inline const t operator [] (const ptrdiff_t i) const { return m_a[i] - m_b[i]; }
private:
	sub_expr& operator =(const sub_expr&);
};

template<class t, class lExpr, class rExpr> class mul_expr {
	const lExpr m_a; const rExpr m_b;
public:
	inline mul_expr(const lExpr &a, const rExpr &b) : m_a(a), m_b(b) {}
	inline const t operator [] (const ptrdiff_t i) const { return m_a[i] * m_b[i]; }
private:
	mul_expr& operator =(const mul_expr&);
};

template<class t, class lExpr, class rExpr> class div_expr {
	const lExpr m_a; const rExpr m_b;
public:
	inline div_expr(const lExpr &a, const rExpr &b) : m_a(a), m_b(b) {}
	inline const t operator [] (const ptrdiff_t i) const { return m_a[i] / m_b[i]; }
private:
	div_expr& operator =(const div_expr&);
};

template<class t, class Expr> class neg_expr {
	const Expr m_a;
public:
	inline neg_expr(const Expr &a) : m_a(a) {}
	inline const t operator [] (const ptrdiff_t i) const { return -m_a[i]; }
private:
	neg_expr& operator =(const neg_expr&);
};

template<class t, class lExpr> class mul2_expr {
	const lExpr m_a; const t m_b;
public:
	inline mul2_expr(const lExpr &a, const t &b) : m_a(a), m_b(b) {}
	inline const t operator [] (const ptrdiff_t i) const { return m_a[i] * m_b; }
private:
	mul2_expr& operator =(const mul2_expr&);
};

template<class t, class rExpr> class div1_expr {
	const t m_a; const rExpr m_b;
public:
	inline div1_expr(const t &a, const rExpr &b) : m_a(a), m_b(b) {}
	inline const t operator [] (const ptrdiff_t i) const { return m_a / m_b[i]; }
private:
	div1_expr& operator =(const div1_expr&);
};

template<class t, class Expr> class perp_expr {
	const Expr m_a;
public:
	inline perp_expr(const Expr &a) : m_a(a) {}
	inline const t operator [] (const ptrdiff_t i) const { return (i != 0) ? -m_a[0] : m_a[1]; }
private:
	perp_expr& operator =(const perp_expr&);
};

template<class t, class lExpr, class rExpr> class crossprod_expr {
	const lExpr m_a; const rExpr m_b;
public:
	inline crossprod_expr(const lExpr &a, const rExpr &b) : m_a(a), m_b(b) {}
	inline const t operator [] (const ptrdiff_t i) const { const ptrdiff_t i1 = (i+1)%3; const ptrdiff_t i2 = (i+2)%3; return m_a[i1] * m_b[i2] - m_a[i2] * m_b[i1]; }
private:
	crossprod_expr& operator =(const crossprod_expr&);
};

//Loop Unrollers (not required)
/*template <class u, class v, int n, int i> struct unrolled {
	enum {CTR = i+1};
	inline static void assign(u &a, const v &b) { a[i] = b[i]; unrolled<u, v, n, CTR>::assign(a, b); }
	inline static void addAssign(u &a, const v &b) { a[i] += b[i]; unrolled<u, v, n, CTR>::addAssign(a, b); }
	inline static void subAssign(u &a, const v &b) { a[i] -= b[i]; unrolled<u, v, n, CTR>::subAssign(a, b); }
	inline static void mulAssign(u &a, const v &b) { a[i] *= b[i]; unrolled<u, v, n, CTR>::mulAssign(a, b); }
	inline static void divAssign(u &a, const v &b) { a[i] /= b[i]; unrolled<u, v, n, CTR>::divAssign(a, b); }
};
template <class u, class v, int n> struct unrolled<u, v, n, n> {
	inline static void assign(u &a, const v &b) {}
	inline static void addAssign(u &a, const v &b) {}
	inline static void subAssign(u &a, const v &b) {}
	inline static void mulAssign(u &a, const v &b) {}
	inline static void divAssign(u &a, const v &b) {}
};*/
#endif

// Template implementing a "vector space" (in the precise mathematical sense).
// A vector space is a type "vectorSpace", implemented as
// an array of "n" elements of type "t".
template<class t, int n>
class vectorSpace {
private:
	typedef void (vectorSpace::*bool_type)() const;
	void true_type() const {}
protected:
	t x[n];
public:
    operator bool_type() const {
		bool c = false;
		for (int i=0; i<n; ++i)
			c |= (x[i] != 0);
		return c ? &vectorSpace::true_type : NULL;
	}
#ifdef _MSC_VER
	typedef t t;
	enum {n = n};
#endif
	inline t& operator [] (const ptrdiff_t i) {/*assert(i>=0 && i<n);*/ return x[i];}
	inline const t& operator [] (const ptrdiff_t i) const {/*assert(i>=0 && i<n);*/ return x[i];}

	const t* begin() const { return &x[0]; }
	t* begin() { return &x[0]; }
	const t* end() const { return &x[n]; }
	t* end() { return &x[n]; }

	const t* rbegin() const { return &x[n-1]; }
	t* rbegin() { return &x[n-1]; }
	const t* rend() const { return &x[-1]; }
	t* rend() { return &x[-1]; }

	friend bool operator == (const vectorSpace& a, const vectorSpace& b) {bool c = true; for (int i=0; i<n; ++i) c &= (a.x[i] == b.x[i]); return c;}
	friend bool operator != (const vectorSpace& a, const vectorSpace& b) {bool c = false; for (int i=0; i<n; ++i) c |= (a.x[i] != b.x[i]); return c;}

	vectorSpace& operator  = (const vectorSpace& b) {for (int i=0; i<n; ++i) x[i]  = b.x[i]; return *this;}
	vectorSpace& operator += (const vectorSpace& b) {for (int i=0; i<n; ++i) x[i] += b.x[i]; return *this;}
	vectorSpace& operator -= (const vectorSpace& b) {for (int i=0; i<n; ++i) x[i] -= b.x[i]; return *this;}
	vectorSpace& operator *= (const vectorSpace& b) {for (int i=0; i<n; ++i) x[i] *= b.x[i]; return *this;}
	vectorSpace& operator /= (const vectorSpace& b) {for (int i=0; i<n; ++i) x[i] /= b.x[i]; return *this;}

	vectorSpace& operator *= (const t b) {for (int i=0; i<n; ++i) x[i] *= b; return *this;}
	vectorSpace& operator /= (const t b) {t c=t(1)/b; for (int i=0; i<n; ++i) x[i] *= c; return *this;}

	const vectorSpace& operator + () const {return *this;}

	vectorSpace& makeParallelTo(const vectorSpace& theVec) {
		const t c = len2(theVec);
		assert(c != 0);
		const t d = dotProd(*this, theVec) / c;
		for (int i=0; i<n; ++i) x[i] = theVec.x[i] * d;
		return *this;
	}
	vectorSpace& makePerpTo(const vectorSpace& theVec) {
		const t c = len2(theVec);
		assert(c != 0);
		const t d = dotProd(*this, theVec) / c;
		for (int i=0; i<n; ++i) x[i] -= theVec.x[i] * d;
		return *this;
	}
	vectorSpace& reflectOff(const vectorSpace& normal) {
		const t d = dotProd(*this, normal) * t(-2);
		for (int i=0; i<n; ++i) x[i] += normal.x[i] * d;
		return *this;
	}

	const t setLen(const t b) {const t c = len(*this); assert(c != 0); *this *= b/c; return c;}

	void clear() {for (int i=0; i<n; ++i) x[i] = t(0);}
	void reverse() {for (int i=0; i<n; ++i) x[i] = -x[i];}
	const t normalize() {const t c = len(*this); assert(c != 0); *this *= t(1)/c; return c;}
	/*const t vectorSum() const {t c=0; for (int i=0; i<n; ++i) c += x[i]; return c;}
	const t vectorProd() const {t c=1; for (int i=0; i<n; ++i) c *= x[i]; return c;}
	const t len2() const {t c=0; for (int i=0; i<n; ++i) c += x[i] * x[i]; return c;}
	const t len() const {t c=0; for (int i=0; i<n; ++i) c += x[i] * x[i]; return sqrt(c);}
	const t dotProd(const vectorSpace& b) const {t c=0; for (int i=0; i<n; ++i) c += x[i] * b.x[i]; return c;}*/

	friend void clear(vectorSpace& a) {for (int i=0; i<n; ++i) a.x[i] = t(0);}
	friend void reverse(vectorSpace& a) {for (int i=0; i<n; ++i) a.x[i] = -a.x[i];}
	friend const t normalize(vectorSpace& a) {const t c=len(a); assert(c != 0); a *= t(1)/c; return c;}
	friend const t vectorSum(const vectorSpace& a) {t c=0; for (int i=0; i<n; ++i) c += a.x[i]; return c;}
	friend const t vectorProd(const vectorSpace& a) {t c=1; for (int i=0; i<n; ++i) c *= a.x[i]; return c;}
	friend const t len2(const vectorSpace& a) {t c=0; for (int i=0; i<n; ++i) c += a.x[i] * a.x[i]; return c;}
	friend const t len(const vectorSpace& a) {t c=0; for (int i=0; i<n; ++i) c += a.x[i] * a.x[i]; return sqrt(c);}
	friend const t dotProd(const vectorSpace& a, const vectorSpace& b) {t c=0; for (int i=0; i<n; ++i) c += a.x[i] * b.x[i]; return c;}
	friend const t degreesBetweenVectors(const vectorSpace &a, const vectorSpace &b) { return angleBetweenVectors(a, b) * (180/M_PI); }
	friend const t angleBetweenVectors(const vectorSpace &a, const vectorSpace &b) {
		const t lenSqrA = len2(a); if (lenSqrA == 0) return 0;
		const t lenSqrB = len2(b); if (lenSqrB == 0) return 0;
		return acos((a * b)/sqrt(lenSqrA*lenSqrB));
	}

	//Less efficient operators:
	const vectorSpace parallelPart(const vectorSpace& theVec) const {const t c=len2(theVec); assert(c != 0); return theVec * (dotProd(*this, theVec)/c);}
	const vectorSpace perpPart(const vectorSpace& theVec) const {const t c=len2(theVec); assert(c != 0); return *this - theVec * (dotProd(*this, theVec)/c);}
	const vectorSpace reflectedPart(const vectorSpace& normal) const {vectorSpace c; const t dot = dotProd(*this, normal) * t(-2); for (int i=0; i<n; ++i) c.x[i] = x[i] + normal.x[i] * dot; return c;}

	const vectorSpace operator - () const {vectorSpace c; for (int i=0; i<n; ++i) c.x[i] = -x[i]; return c;}

	friend const vectorSpace operator + (const vectorSpace& a, const vectorSpace& b) {vectorSpace c; for (int i=0; i<n; ++i) c.x[i] = a.x[i] + b.x[i]; return c;}
	friend const vectorSpace operator - (const vectorSpace& a, const vectorSpace& b) {vectorSpace c; for (int i=0; i<n; ++i) c.x[i] = a.x[i] - b.x[i]; return c;}
	friend const vectorSpace operator * (const vectorSpace& a, const vectorSpace& b) {vectorSpace c; for (int i=0; i<n; ++i) c.x[i] = a.x[i] * b.x[i]; return c;}
	friend const vectorSpace operator / (const vectorSpace& a, const vectorSpace& b) {vectorSpace c; for (int i=0; i<n; ++i) c.x[i] = a.x[i] / b.x[i]; return c;}

	friend const vectorSpace operator * (const t a, const vectorSpace& b) {vectorSpace c; for (int i=0; i<n; ++i) c.x[i] = a * b.x[i]; return c;}
	friend const vectorSpace operator * (const vectorSpace& a, const t b) {vectorSpace c; for (int i=0; i<n; ++i) c.x[i] = a.x[i] * b; return c;}
	friend const vectorSpace operator / (const vectorSpace& a, const t b) {vectorSpace c; const t d=t(1)/b; for (int i=0; i<n; ++i) c.x[i] = a.x[i] * d; return c;}
	friend const vectorSpace operator / (const t a, const vectorSpace& b) {vectorSpace c; for (int i=0; i<n; ++i) c.x[i] = a / b.x[i]; return c;}

	friend const t distPointToLine(const vectorSpace& pt, const vectorSpace& linePos1, const vectorSpace& linePos2) {
		return len((pt-linePos1).perpPart(linePos2-linePos1));
	}

	const vectorSpace randomVec(const t a) {
		assert(a != 0);
		vectorSpace b;
		t lenSqr;
		do {
			for (int i=0; i<n; ++i)
				b.x[i] = (rand()-RAND_MAX/2) * (const t)(t(1) / t((RAND_MAX+1)/2));
			lenSqr = len2(b);
		} while ((lenSqr > 1) || (lenSqr == 0));
		b *= a/sqrt(lenSqr);
		return b;
	}

	//Stream operators:
	friend std::ostream& operator << (std::ostream &os, const vectorSpace &a) {
		os << '(' << a.x[0];
		for (int i=1; i<n; i++)
			os << ',' << a.x[i];
		return os << ')';
	}
	friend std::istream& operator >> (std::istream &is, vectorSpace &a) {
		char ch;
		is >> ch >> a.x[0];
		for (int i=1; i<n; i++)
			is >> ch >> a.x[i];
		return is >> ch;
	}
};

class vec2;
class vec3;
class vec4;
class mtx22;
class mtx33;
class mtx44;
class quat;

// 2-component floating point vectors.
class vec2: public vectorSpace<float,2> {
public:
	typedef float t;
	enum { n = 2 };

	operator const vectorSpace<float,2>& () { return *this; }
	using vectorSpace<float,2>::operator=;
	vec2(const vectorSpace<float,2> &v) : vectorSpace<float,2>(v) {}
	vec2() {x[0]=x[1]=t(0);}
	vec2(t _x, t _y) {x[0]=_x; x[1]=_y;}
	void vecSet(t _x, t _y) {x[0]=_x; x[1]=_y;}
	friend t operator | (const vec2& a, const vec2& b) {return a[1]*b[0] - a[0]*b[1];} //Perp Prod

	vec2& operator *= (const t b) {for (int i=0; i<n; ++i) x[i] *= b; return *this;}
	vec2& operator /= (const t b) {t c=t(1)/b; for (int i=0; i<n; ++i) x[i] *= c; return *this;}

#ifdef EXPR_TMPL
	template <class Expr> inline vec2(const Expr &b) { for (int i=0; i<n; ++i) x[i] = b[i]; } //{ unrolled<vec2, Expr, n, 0>::assign(*this, b); }
	template <class Expr> inline vec2& operator  = (const Expr &b) { for (int i=0; i<n; ++i) x[i]  = b[i]; return *this; }
	template <class Expr> inline vec2& operator += (const Expr &b) { for (int i=0; i<n; ++i) x[i] += b[i]; return *this; }
	template <class Expr> inline vec2& operator -= (const Expr &b) { for (int i=0; i<n; ++i) x[i] -= b[i]; return *this; }
	template <class Expr> inline vec2& operator *= (const Expr &b) { for (int i=0; i<n; ++i) x[i] *= b[i]; return *this; }
	template <class Expr> inline vec2& operator /= (const Expr &b) { for (int i=0; i<n; ++i) x[i] /= b[i]; return *this; }

	template <class rExpr> vec2& makeParallelTo(const rExpr &theVec) {
		const vec2 v(theVec);
		const t c = len2(v);
		assert(c != 0);
		const t d = (dotProd(*this, v) / c);
		for (int i=0; i<n; ++i) x[i] = v.x[i] * d;
		return *this;
	}
	template <class rExpr> vec2& makePerpTo(const rExpr &theVec) {
		const vec2 v(theVec);
		const t c = len2(v);
		assert(c != 0);
		const t d = (dotProd(*this, v) / c);
		for (int i=0; i<n; ++i) x[i] -= v.x[i] * d;
		return *this;
	}
	template <class rExpr> vec2& reflectOff(const rExpr &normal) {
		const vec2 v(normal);
		const t d = dotProd(*this, v) * t(-2);
		for (int i=0; i<n; ++i) x[i] += v.x[i] * d;
		return *this;
	}
	template <class Expr> mul2_expr<t, Expr> parallelPart(const Expr &theVec) const {
		const vec2 v(theVec);
		const t c=len2(v);
		assert(c != 0);
		return mul2_expr<t, vec2>(v, dotProd(*this, v)/c);
	}
	template <class Expr> sub_expr<t, vec2, mul2_expr<t, vec2> > perpPart(const Expr &theVec) const {
		const vec2 v(theVec);
		const t c=len2(v);
		assert(c != 0);
		return sub_expr<t, vec2, mul2_expr<t, vec2> >(*this, mul2_expr<t, vec2>(v, dotProd(*this, v)/c));
	}
	template <class Expr> add_expr<t, vec2, mul2_expr<t, Expr> > reflectedPart(const Expr& normal) const {
		const vec2 v(normal);
		return add_expr<t, vec2, mul2_expr<t, vec2> >(*this, mul2_expr<t, vec2>(v, dotProd(*this, v) * t(-2)));
	}
	template <class Expr> inline friend const perp_expr<t, Expr> perpProd(const Expr &a) { return perp_expr<t, Expr>(a); }

	template <class Expr> inline friend const perp_expr<t, Expr> operator ~ (const Expr &a) { return perp_expr<t, Expr>(a); }
#else
	friend const vec2 perpProd(const vec2& a) { return vec2(a[1], -a[0]); }

	friend const vec2 operator ~ (const vec2& a) { return vec2(a[1], -a[0]); } //Perp Operator
#endif
};

// 3-component floating point vectors.
class vec3: public vectorSpace<float,3> {
public:
	typedef float t;
	enum { n = 3 };

	operator const vectorSpace<float,3>& () { return *this; }
	using vectorSpace<float,3>::operator=;
	vec3(const vectorSpace<float,3> &v) : vectorSpace<float,3>(v) {}
	vec3() {x[0]=x[1]=x[2]=t(0);}
	vec3(t _x, t _y, t _z) {x[0]=_x; x[1]=_y; x[2]=_z;}
	void vecSet(t _x, t _y, t _z) {x[0]=_x; x[1]=_y; x[2]=_z;}
	void makeOrthogonalVectors(vec3 &v2, vec3 &v3) {
		const t x1 = x[0], y1 = x[1], z1 = x[2];
		const t a = fabsf(x1), b = fabsf(y1), c = fabsf(z1);
		if (a <= b && a <= c) {
			v2.vecSet(0, z1, -y1);
			v3.vecSet(-y1*y1-z1*z1, x1*y1, x1*z1);
		} else if (b <= a && b <= c) {
			v2.vecSet(-z1, 0, x1);
			v3.vecSet(y1*x1, -z1*z1-x1*x1, y1*z1);
		} else {
			v2.vecSet(y1, -x1, 0);
			v3.vecSet(z1*x1, z1*y1, -x1*x1-y1*y1);
		}
	}

	vec3& operator *= (const t b) {for (int i=0; i<n; ++i) x[i] *= b; return *this;}
	vec3& operator /= (const t b) {t c=t(1)/b; for (int i=0; i<n; ++i) x[i] *= c; return *this;}

	vec3& makeParallelTo(const vec3 &v) {
		const t c = len2(v);
		assert(c != 0);
		const t d = (dotProd(*this, v) / c);
		for (int i=0; i<n; ++i) x[i] = v.x[i] * d;
		return *this;
	}
	vec3& makePerpTo(const vec3 &v) {
		const t c = len2(v);
		assert(c != 0);
		const t d = (dotProd(*this, v) / c);
		for (int i=0; i<n; ++i) x[i] -= v.x[i] * d;
		return *this;
	}
	vec3& reflectOff(const vec3 &normal) {
		const t d = dotProd(*this, normal) * t(-2);
		for (int i=0; i<n; ++i) x[i] += normal.x[i] * d;
		return *this;
	}

#ifdef EXPR_TMPL
	template <class lExpr, class rExpr> inline friend bool operator == (const lExpr &a, const rExpr &b) {for (int i=0; i<n; ++i) if (a[i] != b[i]) return false; return true;}
	template <class lExpr, class rExpr> inline friend bool operator != (const lExpr &a, const rExpr &b) {for (int i=0; i<n; ++i) if (a[i] != b[i]) return true; return false;}

	template <class Expr> inline vec3(const Expr &b) { for (int i=0; i<n; ++i) x[i] = b[i]; } //{ unrolled<vec3, Expr, n, 0>::assign(*this, b); }
	template <class Expr> inline vec3& operator  = (const Expr &b) { for (int i=0; i<n; ++i) x[i]  = b[i]; return *this; }
	template <class Expr> inline vec3& operator += (const Expr &b) { for (int i=0; i<n; ++i) x[i] += b[i]; return *this; }
	template <class Expr> inline vec3& operator -= (const Expr &b) { for (int i=0; i<n; ++i) x[i] -= b[i]; return *this; }
	template <class Expr> inline vec3& operator *= (const Expr &b) { for (int i=0; i<n; ++i) x[i] *= b[i]; return *this; }
	template <class Expr> inline vec3& operator /= (const Expr &b) { for (int i=0; i<n; ++i) x[i] /= b[i]; return *this; }

	template <class lExpr, class rExpr> inline friend const add_expr<t, lExpr, rExpr> operator + (const lExpr &a, const rExpr &b)
	{ return add_expr<t, lExpr, rExpr>(a, b); }
	template <class lExpr, class rExpr> inline friend const sub_expr<t, lExpr, rExpr> operator - (const lExpr &a, const rExpr &b)
	{ return sub_expr<t, lExpr, rExpr>(a, b); }
	template <class lExpr, class rExpr> inline friend const mul_expr<t, lExpr, rExpr> operator * (const lExpr &a, const rExpr &b)
	{ return mul_expr<t, lExpr, rExpr>(a, b); }
	template <class lExpr, class rExpr> inline friend const div_expr<t, lExpr, rExpr> operator / (const lExpr &a, const rExpr &b)
	{ return div_expr<t, lExpr, rExpr>(a, b); }
	template <class Expr> inline friend const neg_expr<t, Expr> operator - (const Expr &a) { return neg_expr<t, Expr>(a); }
	template <class Expr> inline friend const Expr& operator + (const Expr &a) { return a; }
	template <class Expr> inline friend const t vectorSum(const Expr& a)
	{t c=0; for (int i=0; i<n; ++i) c += a[i]; return c;}
	template <class Expr> inline friend const t vectorProd(const Expr& a)
	{t c=1; for (int i=0; i<n; ++i) c *= a[i]; return c;}
	template <class Expr> inline friend const t len2(const Expr& a) { t c=0; for (int i=0; i<n; ++i) {t d(a[i]); c += d*d;} return c; }
	template <class Expr> inline friend const t len(const Expr& a) { t c=0; for (int i=0; i<n; ++i) {t d(a[i]); c += d*d;} return sqrt(c); }
	template <class lExpr, class rExpr> inline friend const t dotProd(const lExpr &a, const rExpr &b)
	{t c=0; for (int i=0; i<n; ++i) c += a[i] * b[i]; return c;}
	template <class Expr> mul2_expr<t, Expr> parallelPart(const Expr &theVec) const {
		const vec3 v(theVec);
		const t c=len2(v);
		assert(c != 0);
		return mul2_expr<t, vec3>(v, dotProd(*this, v)/c);
	}
	template <class Expr> sub_expr<t, vec3, mul2_expr<t, vec3> > perpPart(const Expr &theVec) const {
		const vec3 v(theVec);
		const t c=len2(v);
		assert(c != 0);
		return sub_expr<t, vec3, mul2_expr<t, vec3> >(*this, mul2_expr<t, vec3>(v, dotProd(*this, v)/c));
	}
	template <class Expr> add_expr<t, vec3, mul2_expr<t, Expr> > reflectedPart(const Expr& normal) const {
		const vec3 v(normal);
		return add_expr<t, vec3, mul2_expr<t, vec3> >(*this, mul2_expr<t, vec3>(v, dotProd(*this, v) * t(-2)));
	}
	template <class lExpr, class mExpr, class rExpr> friend const t distPointToLine(const lExpr &pt, const mExpr &linePos1, const rExpr &linePos2) {
		const vec3 temp(pt-linePos1);
		return len(temp.perpPart(linePos2-linePos1));
	}
	template <class rExpr> inline friend const mul2_expr<t, rExpr> operator * (const t &a, const rExpr &b) { return mul2_expr<t, rExpr>(b, a); }
	template <class lExpr> inline friend const mul2_expr<t, lExpr> operator * (const lExpr &a, const t &b) { return mul2_expr<t, lExpr>(a, b); }
	template <class rExpr> inline friend const div1_expr<t, rExpr> operator / (const t &a, const rExpr &b) { return div1_expr<t, rExpr>(a, b); }
	template <class lExpr> inline friend const mul2_expr<t, lExpr> operator / (const lExpr &a, const t &b) { t c(t(1)/b); return mul2_expr<t, lExpr>(a, c); }

	template <class rExpr> inline friend const mul2_expr<t, rExpr> operator * <int, rExpr>(const int a, const rExpr &b) { t c(a); return mul2_expr<t, rExpr>(b, c); }
	template <class lExpr> inline friend const mul2_expr<t, lExpr> operator * <lExpr, int>(const lExpr &a, const int b) { t c(b); return mul2_expr<t, lExpr>(a, c); }
	template <class rExpr> inline friend const div1_expr<t, rExpr> operator / <int, rExpr>(const int a, const rExpr &b) { t c(a); return div1_expr<t, rExpr>(c, b); }
	template <class lExpr> inline friend const mul2_expr<t, lExpr> operator / <lExpr, int>(const lExpr &a, const int b) { t c(t(1)/b); return mul2_expr<t, lExpr>(a, c); }

	template <class lExpr, class rExpr> inline friend const crossprod_expr<t, vec3, vec3> operator ^ (const lExpr &a, const rExpr &b)
	{ return crossprod_expr<t, vec3, vec3>(vec3(a), vec3(b)); }
	template <class lExpr, class rExpr> inline friend const crossprod_expr<t, vec3, vec3> crossProd(const lExpr &a, const rExpr &b)
	{ return crossprod_expr<t, vec3, vec3>(vec3(a), vec3(b)); }
#else
	inline friend const vec3 operator ^ (const vec3& a, const vec3& b) {
		return vec3(a.x[1]*b.x[2] - a.x[2]*b.x[1], a.x[2]*b.x[0] - a.x[0]*b.x[2], a.x[0]*b.x[1] - a.x[1]*b.x[0]);
	}
	inline friend const vec3 crossProd (const vec3& a, const vec3& b) {
		return vec3(a.x[1]*b.x[2] - a.x[2]*b.x[1], a.x[2]*b.x[0] - a.x[0]*b.x[2], a.x[0]*b.x[1] - a.x[1]*b.x[0]);
	}
#endif
};

// 4-component floating point vectors.
class vec4: public vectorSpace<float,4> {
public:
	typedef float t;
	enum { n = 4 };

	operator const vectorSpace<float,4>& () { return *this; }
	using vectorSpace<float,4>::operator=;
	vec4(const vectorSpace<float,4> &v) : vectorSpace<float,4>(v) {}
	vec4() {x[0]=x[1]=x[2]=x[3]=t(0);}
	vec4(t _x, t _y, t _z, t _w) {x[0]=_x; x[1]=_y; x[2]=_z; x[3]=_w;}
	void vecSet(t _x, t _y, t _z, t _w) {x[0]=_x; x[1]=_y; x[2]=_z; x[3]=_w;}
	friend const vec4 quatToRotationAxisAngle(const quat& a);
	friend const quat quatFromRotationAxisAngle(const vec4& a);

	vec4& operator *= (const t b) {for (int i=0; i<n; ++i) x[i] *= b; return *this;}
	vec4& operator /= (const t b) {t c=t(1)/b; for (int i=0; i<n; ++i) x[i] *= c; return *this;}
};

// 2x2 matrices.
class mtx22: public vectorSpace<vec2,2> {
public:
	typedef vec2 t;
	enum { n = 2 };

	operator const vectorSpace<vec2,2>& () { return *this; }
	using vectorSpace<vec2,2>::operator=;
	mtx22(const vectorSpace<vec2,2> &v) : vectorSpace<vec2,2>(v) {}
	mtx22() {clear();}
	mtx22(const t& _x, const t& _y) {x[0]=_x; x[1]=_y;}
	friend std::ostream& operator << (std::ostream &os, const mtx22 &a) {
		return os << '{' << a[0] << ',' << a[1] << '}';
	}
	friend std::istream& operator >> (std::istream &is, mtx22 &a) {
		char ch;
		return is >> ch >> a[0] >> ch >> a[1] >> ch;
	}
	void makeIdentity() {x[0]=t(1,0); x[1]=t(0,1);}
	void clear() {for (int i=0; i<n; ++i) x[i].clear();}
	friend const mtx22 transpose(const mtx22 &a) {
		return mtx22(
			t(a[0][0],a[1][0]),
			t(a[0][1],a[1][1])
		);
	}
	friend const mtx22 operator * (const mtx22 &a, const mtx22 &b) {
		mtx22 c;
        for (int k = 0; k < n; ++k)
            for (int j = 0; j < n; ++j)
                for (int i = 0; i < n; ++i)
                    c.x[k][j] += a.x[i][j] * b.x[k][i];
        return c;
	}
	mtx22& operator *= (const t::t b) { for (int i=0; i<n; ++i) x[i] *= b; return *this; }
	mtx22& operator *= (const mtx22 &b) { *this = *this * b; return *this; }
	friend t rotate(const mtx22 &a, const t &b) {
		const t::t x=b[0], y=b[1];
		return t(x*a[0][0]+y*a[0][1], x*a[1][0]+y*a[1][1]);
	}
	friend t transposedRotate(const mtx22 &a, const t &b) {
		const t::t x=b[0], y=b[1];
		return t(x*a[0][0]+y*a[1][0], x*a[0][1]+y*a[1][1]);
	}
	friend const mtx22 operator * (const mtx22 &a, const t::t b) { return mtx22(a[0]*b, a[1]*b); }
	friend const mtx22 operator * (const t::t &a, const mtx22 b) { return mtx22(b[0]*a, b[1]*a); }
	friend t::t Determinant(const mtx22 &m) { return m[0][0]*m[1][1]- m[0][1]*m[1][0]; }
	mtx22 operator ~() {
		t::t det = Determinant(*this); if (det) det = t::t(1)/det;
		return mtx22(
			t((*this)[1][1]*det, -(*this)[1][0]*det),
			t(-(*this)[0][1]*det, (*this)[0][0]*det));
	}
	const mtx22 MakeRotationBetweenTwoVectors(const vec3 &vecFrom, const vec3 &vecTo) {
		const t::t c = (dotProd(vecFrom, vecTo) / (len(vecFrom)*len(vecTo))), s = sqrt(1-c*c);
		return mtx22(t(s, c), t(c, -s));
	}
	void concatRotation(const t::t angle) {
		const t::t c = cos(angle), s = sin(angle);
		const t r = (*this)[1];
		(*this)[1] = (*this)[1]*c + (*this)[0]*s;
		(*this)[0] = (*this)[0]*c - r*s;
	}
#ifdef EXPR_TMPL
	template <class Expr> inline mtx22(const Expr &b) { for (int i=0; i<n; ++i) x[i] = b[i]; } //{ unrolled<mtx22, Expr, n, 0>::assign(*this, b); }
	template <class Expr> inline mtx22& operator  = (const Expr &b) { *this  = mtx22(b); return *this; }
	template <class Expr> inline mtx22& operator += (const Expr &b) { *this += mtx22(b); return *this; }
	template <class Expr> inline mtx22& operator -= (const Expr &b) { *this -= mtx22(b); return *this; }
	template <class Expr> inline mtx22& operator *= (const Expr &b) { *this *= mtx22(b); return *this; }
	template <class Expr> inline mtx22& operator /= (const Expr &b) { *this /= mtx22(b); return *this; }
#endif
};

// 3x3 matrices.
class mtx33: public vectorSpace<vec3,3> {
public:
	typedef vec3 t;
	enum { n = 3 };

	operator const vectorSpace<vec3,3>& () { return *this; }
	using vectorSpace<vec3,3>::operator=;
	mtx33(const vectorSpace<vec3,3> &v) : vectorSpace<vec3,3>(v) {}
	mtx33() {clear();}
	mtx33(const t& _x, const t& _y, const t& _z) {x[0]=_x; x[1]=_y; x[2]=_z;}
	friend std::ostream& operator << (std::ostream &os, const mtx33 &a) {
		os << '{' << a[0];
		for (int i=1; i<n; i++)
			os << ',' << a[i];
		return os << '}';
	}
	friend std::istream& operator >> (std::istream &is, mtx33 &a) {
		char ch;
		is >> ch >> a[0];
		for (int i=1; i<n; i++)
			is >> ch >> a[i];
		return is >> ch;
	}
	void makeIdentity() {x[0]=t(1,0,0); x[1]=t(0,1,0); x[2]=t(0,0,1);}
	void clear() {for (int i=0; i<n; ++i) x[i].clear();}
	friend const mtx33 transpose(const mtx33 &a) {
		return mtx33(
			t(a[0][0],a[1][0],a[2][0]),
			t(a[0][1],a[1][1],a[2][1]),
			t(a[0][2],a[1][2],a[2][2])
		);
	}
	friend const mtx33 operator * (const mtx33 &a, const mtx33 &b) {
		mtx33 c;
        for (int k = 0; k < n; ++k)
            for (int j = 0; j < n; ++j)
                for (int i = 0; i < n; ++i)
                    c.x[k][j] += a.x[i][j] * b.x[k][i];
        return c;
	}
	mtx33& operator *= (const t::t b) { for (int i=0; i<n; ++i) x[i] *= b; return *this; }
	mtx33& operator *= (const mtx33 &b) { *this = *this * b; return *this; }
	friend const vec2 rotate(const mtx33 &a, const vec2 &b) {
		const t::t x=b[0], y=b[1];
		return vec2(
			x*a[0][0]+y*a[0][1],
			x*a[1][0]+y*a[1][1]);
	}
	friend const vec2 transposedRotate(const mtx33 &a, const vec2 &b) {
		const t::t x=b[0], y=b[1];
		return vec2(
			x*a[0][0]+y*a[1][0],
			x*a[0][1]+y*a[1][1]);
	}
	friend const vec2 transform(const mtx33 &a, const vec2 &b) {
		const t::t x=b[0], y=b[1];
		return vec2(
			x*a[0][0]+y*a[0][1]+a[0][2],
			x*a[1][0]+y*a[1][1]+a[1][2]);
	}
	friend const vec2 transposedTransform(const mtx33 &a, const vec2 &b) {
		const t::t x=b[0]-a[2][0], y=b[1]-a[2][1];
		return vec2(
			x*a[0][0]+y*a[1][0],
			x*a[0][1]+y*a[1][1]);
	}
	friend const t rotate(const mtx33 &a, const t &b) {
		const t::t x=b[0], y=b[1], z=b[2];
		return t(
			x*a[0][0]+y*a[0][1]+z*a[0][2],
			x*a[1][0]+y*a[1][1]+z*a[1][2],
			x*a[2][0]+y*a[2][1]+z*a[2][2]);
	}
	friend const t transposedRotate(const mtx33 &a, const t &b) {
		const t::t x=b[0], y=b[1], z=b[2];
		return t(
			x*a[0][0]+y*a[1][0]+z*a[2][0],
			x*a[0][1]+y*a[1][1]+z*a[2][1],
			x*a[0][2]+y*a[1][2]+z*a[2][2]);
	}
	friend const mtx33 operator * (const mtx33 &a, const t::t &b) { return mtx33(a[0]*b, a[1]*b, a[2]*b); }
	friend const mtx33 operator * (const t::t &a, const mtx33 &b) { return mtx33(b[0]*a, b[1]*a, b[2]*a); }
	friend t::t Determinant(const mtx33 &m) {
		return
			m[0][0]*m[1][1]*m[2][2] + m[0][1]*m[1][2]*m[2][0] + m[0][2]*m[1][0]*m[2][1] -
			m[0][0]*m[1][2]*m[2][1] - m[0][1]*m[1][0]*m[2][2] - m[0][2]*m[1][1]*m[2][0];
	}
	const mtx33 operator ~() {
		t::t det = Determinant(*this); if (det) det = t::t(1)/det;
		return mtx33 (
			t(((*this)[1][1] * (*this)[2][2] - (*this)[1][2] * (*this)[2][1])*det,
			  ((*this)[2][1] * (*this)[0][2] - (*this)[0][1] * (*this)[2][2])*det,
			  ((*this)[0][1] * (*this)[1][2] - (*this)[1][1] * (*this)[0][2])*det),
			t(((*this)[2][0] * (*this)[1][2] - (*this)[1][0] * (*this)[2][2])*det,
			  ((*this)[0][0] * (*this)[2][2] - (*this)[0][2] * (*this)[2][0])*det,
			  ((*this)[1][0] * (*this)[0][2] - (*this)[0][0] * (*this)[1][2])*det),
			t(((*this)[1][0] * (*this)[2][1] - (*this)[2][0] * (*this)[1][1])*det,
			  ((*this)[2][0] * (*this)[0][1] - (*this)[0][0] * (*this)[2][1])*det,
			  ((*this)[0][0] * (*this)[1][1] - (*this)[1][0] * (*this)[0][1])*det)
		);
	}
	const mtx33 MakeRotationAroundUnitVector(const t &v, t::t angle) {
		const t::t c = cos(angle), s = sin(angle), r = t::t(1)-c; \
		const t::t x = v[0], y = v[1], z = v[2];
		const t::t rxy = r*x*y, rxz = r*x*z, ryz = r*y*z;
		const t::t sx = s*x, sy = s*y, sz = s*z;
		return mtx33(
			t(r*x*x+c, rxy-sz, rxz+sy),
			t(rxy+sz, r*y*y+c, ryz-sx),
			t(rxz-sy, ryz+sx, r*z*z+c));
	}
	const mtx33 MakeRotationBetweenTwoVectors(const t &vecFrom, const t &vecTo) {
		const t axis(vecFrom ^ vecTo);
		const t::t leng = len2(axis);
		//assert(leng > SMALLERNUMBER);
		const t::t l = t::t(1)/sqrt(leng), x = axis[0]*l, y = axis[1]*l, z = axis[2]*l;
		const t::t c = (dotProd(vecFrom, vecTo) / (len(vecFrom)*len(vecTo))), s = sqrt(1-c*c), r = t::t(1)-c;
		const t::t rxy = r*x*y, rxz = r*x*z, ryz = r*y*z;
		const t::t sx = s*x, sy = s*y, sz = s*z;
		return mtx33(
			t(r*x*x+c, rxy-sz, rxz+sy),
			t(rxy+sz, r*y*y+c, ryz-sx),
			t(rxz-sy, ryz+sx, r*z*z+c));
	}
	void Orthonormalize() {
		(*this)[0].normalize(); // normalize first vector
		// make second vector perpendicular to the first
		(*this)[1] -= (*this)[0] * dotProd((*this)[1], (*this)[0]);
		(*this)[1].normalize(); // normalize second vector
		// third vector is first cross second
		(*this)[2] = (*this)[0] ^ (*this)[1];
		// no need to normalize this third one since the first two are normalized
	}
	void concatRotation(const t::t angle, const int a, const int b) {
		const t::t c = cos(angle), s = sin(angle);
		const t r = (*this)[b];
		(*this)[b] = (*this)[b]*c + (*this)[a]*s;
		(*this)[a] = (*this)[a]*c - r*s;
	}
	void concatXRotation(const t::t angle) { concatRotation(angle, 2, 1); }
	void concatYRotation(const t::t angle) { concatRotation(angle, 2, 0); }
	void concatZRotation(const t::t angle) { concatRotation(angle, 0, 1); }
	void concatTranslation(const t::t x, const t::t y) {
		(*this)[0][2]+=(*this)[0][0]*x+(*this)[0][1]*y;
		(*this)[1][2]+=(*this)[1][0]*x+(*this)[1][1]*y;
	}
	void concatTranslation(const vec2 b) {
		(*this)[0][2]+=(*this)[0][0]*b[0]+(*this)[0][1]*b[1];
		(*this)[1][2]+=(*this)[1][0]*b[0]+(*this)[1][1]*b[1];
	}
	void preConcatTranslation(const vec2 b) {
		(*this)[0][2] += b[0];
		(*this)[1][2] += b[1];
	}
	void preConcatTranslation(const t::t x, const t::t y) {
		(*this)[0][2] += x;
		(*this)[1][2] += y;
	}
	void copyRotationFrom(const mtx33 &m) {
		(*this)[0][2] = (*this)[1][2] =
		(*this)[2][0] = (*this)[2][1] =
		(*this)[2][2] = 0;
		for (int i=0; i<n-1; ++i)
			for (int j=0; j<n-1; ++j)
				(*this)[j][i] = m[j][i];
	}
#ifdef EXPR_TMPL
	template <class Expr> inline mtx33(const Expr &b) { for (int i=0; i<n; ++i) x[i] = b[i]; } //{ unrolled<mtx33, Expr, n, 0>::assign(*this, b); }
	template <class Expr> inline mtx33& operator  = (const Expr &b) { *this  = mtx33(b); return *this; }
	template <class Expr> inline mtx33& operator += (const Expr &b) { *this += mtx33(b); return *this; }
	template <class Expr> inline mtx33& operator -= (const Expr &b) { *this -= mtx33(b); return *this; }
	template <class Expr> inline mtx33& operator *= (const Expr &b) { *this *= mtx33(b); return *this; }
	template <class Expr> inline mtx33& operator /= (const Expr &b) { *this /= mtx33(b); return *this; }
#endif
};

// 4x4 matrices.
class mtx44: public vectorSpace<vec4,4> {
public:
	typedef vec4 t;
	enum { n = 4 };

	operator const vectorSpace<vec4,4>& () { return *this; }
	using vectorSpace<vec4,4>::operator=;
	mtx44(const vectorSpace<vec4,4> &v) : vectorSpace<vec4,4>(v) {}
	mtx44() {clear();}
	mtx44(const t& _x, const t& _y, const t& _z, const t& _w) {x[0]=_x; x[1]=_y; x[2]=_z; x[3]=_w;}
	friend const mtx44 quatToRotationMatrix(const quat& a);
	friend const quat quatFromRotationMatrix(const mtx44& m);
	friend std::ostream& operator << (std::ostream &os, const mtx44 &a) {
		os << '{' << a[0];
		for (int i=1; i<n; i++)
			os << ',' << a[i];
		return os << '}';
	}
	friend std::istream& operator >> (std::istream &is, mtx44 &a) {
		char ch;
		is >> ch >> a[0];
		for (int i=1; i<n; i++)
			is >> ch >> a[i];
		return is >> ch;
	}
	void makeIdentity() {x[0]=t(1,0,0,0); x[1]=t(0,1,0,0); x[2]=t(0,0,1,0); x[3]=t(0,0,0,1);}
	void clear() {for (int i=0; i<n; ++i) x[i].clear();}
	friend const mtx44 transpose(const mtx44 &a) {
		return mtx44(
			t(a[0][0],a[1][0],a[2][0],a[3][0]),
			t(a[0][1],a[1][1],a[2][1],a[3][1]),
			t(a[0][2],a[1][2],a[2][2],a[3][2]),
			t(a[0][3],a[1][3],a[2][3],a[3][3])
		);
	}
	friend const mtx44 operator * (const mtx44 &a, const mtx44 &b) {
		mtx44 c;
        for (int k = 0; k < n; ++k)
            for (int j = 0; j < n; ++j)
                for (int i = 0; i < n; ++i)
                    c.x[k][j] += a.x[i][j] * b.x[k][i];
        return c;
	}
	mtx44& operator *= (const t::t b) { for (int i=0; i<n; ++i) x[i]*= b; return *this; }
	mtx44& operator *= (const mtx44 &b) { *this = *this * b; return *this; }
	friend const t rotate(const mtx44 &a, const t &b) {
		const t::t x=b[0], y=b[1], z=b[2], w=b[3];
		return t(
			x*a[0][0]+y*a[0][1]+z*a[0][2]+w*a[0][3],
			x*a[1][0]+y*a[1][1]+z*a[1][2]+w*a[1][3],
			x*a[2][0]+y*a[2][1]+z*a[2][2]+w*a[2][3],
			x*a[3][0]+y*a[3][1]+z*a[3][2]+w*a[3][3]);
	}
	friend const t transposedRotate(const mtx44 &a, const t &b) {
		const t::t x=b[0], y=b[1], z=b[2], w=b[3];
		return t(
			x*a[0][0]+y*a[1][0]+z*a[2][0]+w*a[3][0],
			x*a[0][1]+y*a[1][1]+z*a[2][1]+w*a[3][1],
			x*a[0][2]+y*a[1][2]+z*a[2][2]+w*a[3][2],
			x*a[0][3]+y*a[1][3]+z*a[2][3]+w*a[3][3]);
	}
	friend const vec3 transform(const mtx44 &a, const vec3 &b) {
		const t::t x=b[0], y=b[1], z=b[2];
		return vec3(
			x*a[0][0]+y*a[0][1]+z*a[0][2]+a[0][3],
			x*a[1][0]+y*a[1][1]+z*a[1][2]+a[1][3],
			x*a[2][0]+y*a[2][1]+z*a[2][2]+a[2][3]);
	}
	friend const vec3 transposedTransform(const mtx44 &a, const vec3 &b) {
		const t::t x=b[0]-a[0][3], y=b[1]-a[1][3], z=b[2]-a[2][3];
		return vec3(
			x*a[0][0]+y*a[1][0]+z*a[2][0],
			x*a[0][1]+y*a[1][1]+z*a[2][1],
			x*a[0][2]+y*a[1][2]+z*a[2][2]);
	}
	friend const vec3 rotate(const mtx44 &a, const vec3 &b) {
		const t::t x=b[0], y=b[1], z=b[2];
		return vec3(
			x*a[0][0]+y*a[0][1]+z*a[0][2],
			x*a[1][0]+y*a[1][1]+z*a[1][2],
			x*a[2][0]+y*a[2][1]+z*a[2][2]);
	}
	friend const vec3 transposedRotate(const mtx44 &a, const vec3 &b) {
		const t::t x=b[0], y=b[1], z=b[2];
		return vec3(
			x*a[0][0]+y*a[1][0]+z*a[2][0],
			x*a[0][1]+y*a[1][1]+z*a[2][1],
			x*a[0][2]+y*a[1][2]+z*a[2][2]);
	}
	friend const mtx44 operator * (const mtx44 &a, const t::t b) { return mtx44(a[0]*b, a[1]*b, a[2]*b, a[3]*b); }
	friend const mtx44 operator * (const t::t &a, const mtx44 b) { return mtx44(b[0]*a, b[1]*a, b[2]*a, b[3]*a); }
	const mtx44 MakeRotationAroundUnitVector(const t &v, t::t angle) {
		const t::t c = cos(angle), s = sin(angle), r = t::t(1)-c; \
		const t::t x = v[0], y = v[1], z = v[2];
		const t::t rxy = r*x*y, rxz = r*x*z, ryz = r*y*z;
		const t::t sx = s*x, sy = s*y, sz = s*z;
		return mtx44(
			t(r*x*x+c, rxy-sz, rxz+sy, 0),
			t(rxy+sz, r*y*y+c, ryz-sx, 0),
			t(rxz-sy, ryz+sx, r*z*z+c, 0),
			t(0, 0, 0, 1));
	}
	const mtx44 MakeRotationBetweenTwoVectors(const vec3 &vecFrom, const vec3 &vecTo) {
		const vec3 axis(crossProd(vecFrom, vecTo));
		const t::t leng = len2(axis);
		//assert(leng > SMALLERNUMBER);
		const t::t l = t::t(1)/sqrt(leng), x = axis[0]*l, y = axis[1]*l, z = axis[2]*l;
		const t::t c = (dotProd(vecFrom, vecTo) / (len(vecFrom)*len(vecTo))), s = sqrt(1-c*c), r = t::t(1)-c;
		const t::t rxy = r*x*y, rxz = r*x*z, ryz = r*y*z;
		const t::t sx = s*x, sy = s*y, sz = s*z;
		return mtx44(
			t(r*x*x+c, rxy-sz, rxz+sy, 0),
			t(rxy+sz, r*y*y+c, ryz-sx, 0),
			t(rxz-sy, ryz+sx, r*z*z+c, 0),
			t(0, 0, 0, 1));
	}
	void concatRotation(const t::t angle, const int a, const int b) {
		const t::t c = cos(angle), s = sin(angle);
		const t::t r[3] = {(*this)[b][0], (*this)[b][1], (*this)[b][2]};
		(*this)[b][0] = (*this)[b][0]*c + (*this)[a][0]*s;
		(*this)[b][1] = (*this)[b][1]*c + (*this)[a][1]*s;
		(*this)[b][2] = (*this)[b][2]*c + (*this)[a][2]*s;
		(*this)[a][0] = (*this)[a][0]*c - r[0]*s;
		(*this)[a][1] = (*this)[a][1]*c - r[1]*s;
		(*this)[a][2] = (*this)[a][2]*c - r[2]*s;
	}
	inline void concatXRotation(const t::t angle) { concatRotation(angle, 2, 1); }
	inline void concatYRotation(const t::t angle) { concatRotation(angle, 2, 0); }
	inline void concatZRotation(const t::t angle) { concatRotation(angle, 0, 1); }
	void concatTranslation(const t::t x, const t::t y, const t::t z) {
		(*this)[0][3]+=(*this)[0][0]*x+(*this)[0][1]*y+(*this)[0][2]*z;
		(*this)[1][3]+=(*this)[1][0]*x+(*this)[1][1]*y+(*this)[1][2]*z;
		(*this)[2][3]+=(*this)[2][0]*x+(*this)[2][1]*y+(*this)[2][2]*z;
	}
	void concatTranslation(const vec3 b) {
		(*this)[0][3]+=(*this)[0][0]*b[0]+(*this)[0][1]*b[1]+(*this)[0][2]*b[2];
		(*this)[1][3]+=(*this)[1][0]*b[0]+(*this)[1][1]*b[1]+(*this)[1][2]*b[2];
		(*this)[2][3]+=(*this)[2][0]*b[0]+(*this)[2][1]*b[1]+(*this)[2][2]*b[2];
	}
	void preConcatTranslation(const vec3 b) {
		(*this)[0][3] += b[0];
		(*this)[1][3] += b[1];
		(*this)[2][3] += b[2];
	}
	void preConcatTranslation(const t::t x, const t::t y, const t::t z) {
		(*this)[0][3] += x;
		(*this)[1][3] += y;
		(*this)[2][3] += z;
	}
	void copyRotationFrom(const mtx44 &m) {
		(*this)[0][3] = (*this)[1][3] = (*this)[2][3] =
		(*this)[3][0] = (*this)[3][1] = (*this)[3][2] =
		(*this)[3][3] = 0;
		for (int i=0; i<n-1; ++i)
			for (int j=0; j<n-1; ++j)
				(*this)[j][i] = m[j][i];
	}
#ifdef EXPR_TMPL
	template <class Expr> inline mtx44(const Expr &b) { for (int i=0; i<n; ++i) x[i] = b[i]; } //{ unrolled<mtx44, Expr, n, 0>::assign(*this, b); }
	template <class Expr> inline mtx44& operator  = (const Expr &b) { *this  = mtx44(b); return *this; }
	template <class Expr> inline mtx44& operator += (const Expr &b) { *this += mtx44(b); return *this; }
	template <class Expr> inline mtx44& operator -= (const Expr &b) { *this -= mtx44(b); return *this; }
	template <class Expr> inline mtx44& operator *= (const Expr &b) { *this *= mtx44(b); return *this; }
	template <class Expr> inline mtx44& operator /= (const Expr &b) { *this /= mtx44(b); return *this; }
#endif
};

template<class t> t sampleLinear(const t& a, const t& b, float s) {
	return (1-s)*a+s*b;
}
template<class t> t sampleHermite(const t& a, const t& b, const t& at, const t& bt, float s) {
	return
		(+2*s*s*s -3*s*s +1)*a+
		(-2*s*s*s +3*s*s )*b+
		(+1*s*s*s -2*s*s +s )*at+
		(+1*s*s*s -1*s*s )*bt;
}
template<class t> t sampleBezier(const t& a, const t& b, const t& ac, const t& bc, float s) {
	return
		(-1*s*s*s +3*s*s -3*s +1)*a+
		(+3*s*s*s -6*s*s +3*s )*b+
		(-3*s*s*s +3*s*s )*ac+
		(+1*s*s*s )*bc;
}
// A 3-dimensional (4-component) quaternion.
class quat: public vectorSpace<float,4> {
public:
	typedef float t;
	enum { n = 4 };

	quat() {x[0]=x[1]=x[2]=x[3]=t(0);}
	quat(t _x, t _y, t _z, t _w) {x[0]=_x; x[1]=_y; x[2]=_z; x[3]=_w;}
	quat(const vectorSpace<float,4> &v) : vectorSpace<float,4>(v) {}
	friend const quat operator * (const quat& a, const quat& b) {
		return quat(
			+a[0]*b[3] +a[1]*b[2] -a[2]*b[1] +a[3]*b[0],
			-a[0]*b[2] +a[1]*b[3] +a[2]*b[0] +a[3]*b[1],
			+a[0]*b[1] -a[1]*b[0] +a[2]*b[3] +a[3]*b[2],
			-a[0]*b[0] -a[1]*b[1] -a[2]*b[2] +a[3]*b[3]
		);
	}
	friend t norm(const quat& a) {return a[0]*a[0] + a[1]*a[1] + a[2]*a[2] + a[3]*a[3];}
	friend t mag(const quat& a) {return sqrt(norm(a));}
	friend const quat normal(const quat& a) {return a/mag(a);}
	friend const quat conj(const quat& a) {return quat(-a[0], -a[1], -a[2], a[3]);}
	friend const quat inv(const quat& a) {return (t(1)/norm(a))*conj(a);}
	friend const quat exp(const quat& a);
	friend const quat ln(const quat& a);
	friend const quat quatFromRotationAxisAngle(const vec4& a);
	friend const quat quatFromRotationMatrix(const mtx44& m);
};
const quat exp(const quat& a) {
	const quat::t r = sqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);
	const quat::t et = expf(a[3]);
	const quat::t s = r>0.00001f ? et*sinf(r)/r : quat::t(0);
	return quat(s*a[0], s*a[1], s*a[2], et*cosf(r));
}
const quat ln(const quat& a) {
	const quat::t r = sqrt(a[0]*a[0]+a[1]*a[1]+a[2]*a[2]);
	const quat::t t = r>0.00001f ? atan2f(r,a[3])/r : quat::t(0);
	return quat(t*a[0], t*a[1], t*a[2], 0.5f*logf(norm(a)));
}
const vec4 quatToRotationAxisAngle(const quat& a) {
	const quat::t t = quat::t(1) - a[3]*a[3];
	const quat::t rsa = t>0.00001f ? quat::t(1)/sqrt(t) : quat::t(1);
	return vec4(a[0]*rsa, a[1]*rsa, a[2]*rsa, acosf(a[3])*quat::t(2));
}
const quat quatFromRotationAxisAngle(const vec4& a) {
	const vec4::t s = sinf(a[3]/vec4::t(2));
	const vec4::t c = cosf(a[3]/vec4::t(2));
	return quat(a[0]*s, a[1]*s, a[2]*s, c);
}
const mtx44 quatToRotationMatrix(const quat& a) {
	const quat::t xx=a[0]*a[0];
	const quat::t xy=a[0]*a[1], yy=a[1]*a[1];
	const quat::t xz=a[0]*a[2], yz=a[1]*a[2], zz=a[2]*a[2];
	const quat::t xw=a[0]*a[3], yw=a[1]*a[3], zw=a[2]*a[3], ww=a[3]*a[3];
	return mtx44(
		vec4(+xx-yy-zz+ww, +xy+zw+xy+zw, +xz-yw+xz-yw, 0),
		vec4(+xy-zw+xy-zw, -xx+yy-zz+ww, +yz+xw+yz+xw, 0),
		vec4(+xz+yw+xz+yw, +yz-xw+yz-xw, -xx-yy+zz+ww, 0),
		vec4(0, 0, 0, 1)
	);
}
const quat quatFromRotationMatrix(const mtx44& m) {
	const quat::t w=0.5f * sqrt(m[0][0]+m[1][1]+m[2][2]+m[3][3]);
	assert(w != 0);
	const quat::t s = 0.25f/w;
	return quat(
		(m[1][2]-m[2][1])*s,
		(m[2][0]-m[0][2])*s,
		(m[0][1]-m[1][0])*s,
		w
	);
}

const mtx44 RotationAxisAngleToRotationMatrix(const vec4 &a) {
	const vec4::t c = cos(a[3]), s = sin(a[3]), r = vec4::t(1)-c; \
	const vec4::t x = a[0], y = a[1], z = a[2];
	const vec4::t rx = r*x, ry = r*y, rz = r*z;
	const vec4::t rxy = rx*y, rxz = rx*z, ryz = ry*z;
	const vec4::t sx = s*x, sy = s*y, sz = s*z;
	return mtx44(
		vec4(rx*x+c, rxy-sz, rxz+sy, 0),
		vec4(rxy+sz, ry*y+c, ryz-sx, 0),
		vec4(rxz-sy, ryz+sx, rz*z+c, 0),
		vec4(0, 0, 0, 1)
	);
}
#endif