matrix<4 A;
vector<4 col(1,0,0,0);

A[c0] = col;		// set the first column to 1,0,0,0
A[r1] << 0,1,0,0;	// set second row to 0,1,0,0
A[r12,c12] = zero;	// set the center four elements to zero 

matrix<1,4 transvec;
vector<4 vec;
vector<3 vec3;

vec = transpose(transvec);
vec = transvec[tr];	// same as above
vec[tr] = transvec;	// also the same

vec3 = vec[xyw];	// select elements
vec[xyw] = vec3;	// works both ways

vec = vec3[z|x|y|y];	// custom swizzling 

matrix<4 A, B, C; vector<4 u, v, w; // matrix multiplication
(vectors are column matrices) A *= B; C = A * B; u = A * v;

// elementwise operations for vectors
u *= v;
w = v + u;
u /= 5; 

C.MatrixMult(A, B); // not supported, not needed 

#ifndef MATHLIB_BOX_H
#define MATHLIB_BOX_H

#include <mathlib/matrix.h>

namespace mathlib {

// axis aligned bounding box
template<int D>
class box {
public:
	vector<D> min;
	vector<D> max;

	box() {}
	box(const vector<D> &min_, const vector<D> &max_)
		: min(min_), max(max_) {}

	box &operator|=(const vector<D> &v) {
		for (int i=0; i!=D; ++i)
			if (v(i) < min(i))
				min(i) = v(i);
			else if (v(i) > max(i))
				max(i) = v(i);
		return *this;
	}
};

template<int D> inline
box<D> operator|(const box<D> &b, const vector<D> &v) {
	box<D> r = b;
	return r |= v;
}

} // namespace mathlib

#endif
 

#ifndef MATHLIB_COMPLEX_H
#define MATHLIB_COMPLEX_H

#include <complex>

namespace mathlib {

typedef std::complex<float> complex;

} // namespace mathlib

#endif
 

#ifndef MATHLIB_LINE_H
#define MATHLIB_LINE_H

#include <mathlib/matrix.h>

namespace mathlib {

template<int D>
class line {
public:
	vector<D> base;
	vector<D> direction;

	line() {}
	line(const vector<D> &r, const vector<D> &v)
		: base(r), direction(v) {}
};

} // namespace mathlib

#endif
 

#ifndef MATHLIB_MATHCONF_H
#define MATHLIB_MATHCONF_H

// detects defects and features of the compiler

// g++
#if __GNUG__
	// unaliased references are supported in g++, use them
#	define MATHLIB_RESTRICT __restrict
#	if __GNUG__ < 3
#		ifndef MATHLIB_NO_USING
#			define MATHLIB_NO_USING	1
#		endif
#		ifndef MATHLIB_LIBSTDCPP2
#			define MATHLIB_LIBSTDCPP2	1
#		endif
#	endif
#endif

// work around libstdc++-v2 defects
#if MATHLIB_LIBSTDCPP2
#	ifndef MATHLIB_NO_LIMITS
#		define MATHLIB_NO_LIMITS	1
#	endif
#	ifndef MATHLIB_NO_OSTREAM
#		define MATHLIB_NO_OSTREAM	1
#	endif
#endif

// unaliased references does not appear to be supported
#ifndef MATHLIB_RESTRICT
#	define MATHLIB_RESTRICT
#endif

#endif
 

#ifndef MATHLIB_MATHDEFS_H
#define MATHLIB_MATHDEFS_H

// constants

#include <mathlib/mathconf.h>

#if MATHLIB_NO_LIMITS
#	include <float.h>
#else
#	include <limits>
#endif

namespace mathlib {

const float pi = 3.14159265358979323846;
const float ln_2 = 0.693147180559945309417;
#if MATHLIB_NO_LIMITS
const float epsilon = FLT_EPSILON;
#else
const float epsilon = std::numeric_limits<float>::epsilon();
#endif

struct identity_t {};
struct zero_t {
	operator float() const { return 0.f; }
};

// absence of geometry.
struct nothing {};

// typeless multiplicative identity
const identity_t identity = identity_t();

// typeless zero
const zero_t zero = zero_t();

} // namespace mathlib

#endif
 

#ifndef MATHLIB_MATRIX_H
#define MATHLIB_MATRIX_H

// class definitions for matrix and vector

#include <mathlib/mathconf.h>
#include <mathlib/mathdefs.h>
#include <mathlib/simd/config.h>

namespace mathlib {

class float_keeper;
template<class D, class S> struct assign_dispatch;
template<class I> struct matrix_indexer;
template<class Op, class A> struct matrix_unop;
template<class I> struct unop_index;
template<class I> struct unop_cindex;
template<class A> class tmatrix;
template<int R, int C> class matrix;

// representation of matrices
template<int Rows, int Cols>
class matrix_storage {
	float m_data[Cols][Rows];
public:
	float &operator()(int r, int c) { return m_data[c][r]; }
	float operator()(int r, int c) const { return m_data[c][r]; }
	float *data() { return &m_data[0][0]; }
	const float *data() const { return &m_data[0][0]; }
};
template<>
class matrix_storage<3, 3> {
	float m_data[3][3];
public:
	float &operator()(int r, int c) { return m_data[c][r]; }
	float operator()(int r, int c) const { return m_data[c][r]; }
	float *data() { return &m_data[0][0]; }
	const float *data() const { return &m_data[0][0]; }
#if MATHLIB_SIMD4
private:
	float m_filler;
#endif
} MATHLIB_SIMD4_ALIGN;
template<>
class matrix_storage<4, 4> {
	float m_data[4][4];
public:
	float &operator()(int r, int c) { return m_data[c][r]; }
	float operator()(int r, int c) const { return m_data[c][r]; }
	float *data() { return &m_data[0][0]; }
	const float *data() const { return &m_data[0][0]; }
} MATHLIB_SIMD4_ALIGN;

// specializations for vectors
template<int Rows>
class matrix_storage<Rows, 1> {
	float m_data[Rows];
public:
	float &operator()(int r, int) { return m_data[r]; }
	float operator()(int r, int) const { return m_data[r]; }
	float &operator()(int r) { return m_data[r]; }
	float operator()(int r) const { return m_data[r]; }
	float *data() { return m_data; }
	const float *data() const { return m_data; }
};
template<>
class matrix_storage<2, 1> {
public:
	union { float x; float s; };
	union { float y; float t; };

	float &operator()(int r, int) { return (&x)[r]; }
	float operator()(int r, int) const { return (&x)[r]; }
	float &operator()(int r) { return (&x)[r]; }
	float operator()(int r) const { return (&x)[r]; }
	float *data() { return &x; }
	const float *data() const { return &x; }

	void assign(float a, float b) { x = a; y = b; }
};
template<>
class matrix_storage<3, 1> {
public:
 	union { float x; float s; float r; };
 	union { float y; float t; float g; };
 	union { float z; float u; float b; };

	float &operator()(int r, int) { return (&x)[r]; }
	float operator()(int r, int) const { return (&x)[r]; }
	float &operator()(int r) { return (&x)[r]; }
	float operator()(int r) const { return (&x)[r]; }
	float *data() { return &x; }
	const float *data() const { return &x; }

	void assign(float a, float b, float c) { x = a; y = b; z = c; }

#if MATHLIB_SIMD4
	matrix_storage() {}
	matrix_storage(const matrix_storage &);
	matrix_storage &operator=(const matrix_storage &);
private:
	float m_filler;
#endif
} MATHLIB_SIMD4_ALIGN;
template<>
class matrix_storage<4, 1> {
public:
 	union { float x; float s; float r; };
 	union { float y; float t; float g; };
 	union { float z; float u; float b; };
	union { float w; float v; float a; };

	float &operator()(int r, int) { return (&x)[r]; }
	float operator()(int r, int) const { return (&x)[r]; }
	float &operator()(int r) { return (&x)[r]; }
	float operator()(int r) const { return (&x)[r]; }
	float *data() { return &x; }
	const float *data() const { return &x; }

	void assign(float a, float b, float c, float d)
		{ x = a; y = b; z = c; w = d; }

#if MATHLIB_SIMD4
	matrix_storage() {}
	matrix_storage(const matrix_storage &);
	matrix_storage &operator=(const matrix_storage &);
#endif
} MATHLIB_SIMD4_ALIGN;

// the type most matrix operations work with. subclass tmatrix<basic_matrix>
// to make your own matrix type. do not use basic_matrix directly
template<int Rows, int Cols>
class basic_matrix : public matrix_storage<Rows, Cols> {
public:
	static const int rows = Rows;
	static const int columns = Cols;
	static const bool is_storage = true;
	static const bool is_lvalue = true;
	static const bool error = false;
	static const bool element_op = true;
	typedef basic_matrix<Rows, Cols> base_type;

	basic_matrix() {}
	// used for implicit evaluation by the select_* templates in
	// matrix_operators.tpp
	template<class T> basic_matrix(const T &m);

	template<int R, int C> float element() const {
		enum { i = R+C*Rows };
		return data()[i];
	}
	template<int R, int C> float &element() {
		enum { i = R+C*Rows };
		return data()[i];
	}

	mathlib::matrix<Rows, Cols> &matrix() {
		return *static_cast<mathlib::matrix<Rows, Cols> *>(this);
	}
	const mathlib::matrix<Rows, Cols> &matrix() const {
		return *static_cast<const mathlib::matrix<Rows, Cols> *>(this);
	}
};

// should be wrapped around matrix expressions that are strictly for
// initialization to avoid having to write two constructor/assignment
// pairs per initializer
template<class Base>
class tmatrix_ndim : public Base {
public:
	tmatrix_ndim() {}
	template<class A> explicit tmatrix_ndim(const A &a) : Base(a) {}
	template<class A, class B> tmatrix_ndim(const A &a, const B &b)
		: Base(a, b) {}
};

// wrapped around matrix expressions by tmatrix_type to make overloading based
// on size possible. the type most operators accept
template<int Rows, int Cols, class Base>
class tmatrix_dim : public Base {
public:
	tmatrix_dim() {}
	template<class A> explicit tmatrix_dim(A &a) : Base(a) {}
	template<class A> explicit tmatrix_dim(const A &a) : Base(a) {}
	template<class A, class B> tmatrix_dim(const A &a, const B &b)
		: Base(a, b) {}

	tmatrix_dim &operator*=(float);
	tmatrix_dim &operator/=(float);

	tmatrix_dim &operator=(zero_t);
	tmatrix_dim &operator=(identity_t);

	template<class T>
	tmatrix_dim &operator=(const tmatrix_dim<Rows, Cols, T> &);
	template<class T>
	tmatrix_dim &operator=(const tmatrix_ndim<T> &);

	template<class A>
	tmatrix_dim &operator*=(const tmatrix_dim<Rows, 1, A> &);
	template<class A>
	tmatrix_dim &operator/=(const tmatrix_dim<Rows, 1, A> &);

	template<class A>
	tmatrix_dim &operator*=(const tmatrix_dim<Cols, Rows, A> &);
	template<class A>
	tmatrix_dim &operator+=(const tmatrix_dim<Rows, Cols, A> &);
	template<class A>
	tmatrix_dim &operator-=(const tmatrix_dim<Rows, Cols, A> &);

	template<class I> tmatrix<matrix_unop<unop_index<I>, Base> > 
		operator[](matrix_indexer<I>);
	template<class I> const tmatrix<matrix_unop<unop_cindex<I>, Base> >
		operator[](matrix_indexer<I>) const;
};

// wrapped around expressions by tmatrix. used for early detection of
// illegal operations
template<bool Error, bool Is_scalar, bool Lvalue, class Base>
class tmatrix_type {
	// error - cannot be used
};
template<class Base>
class tmatrix_type<false, false, true, Base>
	: public tmatrix_dim<Base::rows, Base::columns, Base> {
	typedef tmatrix_dim<Base::rows, Base::columns, Base> m_base;
public:
	// not scalar, is l-value
	tmatrix_type() {}
	template<class A> explicit tmatrix_type(A &a) : m_base(a) {}
	template<class A> explicit tmatrix_type(const A &a) : m_base(a) {}
	template<class A, class B> tmatrix_type(const A &a, const B &b)
		: m_base(a, b) {}

#if MATHLIB_NO_USING
	template<class A> tmatrix_type &operator=(const A &a)
		{ m_base::operator=(a); return *this; }
#else
	using m_base::operator=;
#endif
};
template<class Base>
class tmatrix_type<false, false, false, Base>
	: public tmatrix_dim<Base::rows, Base::columns, Base> {
	typedef tmatrix_dim<Base::rows, Base::columns, Base> m_base;
public:
	// not scalar, not l-value
	tmatrix_type() {}
	template<class A> explicit tmatrix_type(A &a) : m_base(a) {}
	template<class A> explicit tmatrix_type(const A &a) : m_base(a) {}
	template<class A, class B> tmatrix_type(const A &a, const B &b)
		: m_base(a, b) {}
};
template<class Base>
class tmatrix_type<false, true, true, Base>
	: public Base {
	typedef Base m_base;
public:
	// scalar, l-value. disable matrix operations
	tmatrix_type() {}
	template<class A> explicit tmatrix_type(A &a) : m_base(a) {}
	template<class A> explicit tmatrix_type(const A &a) : m_base(a) {}
	template<class A, class B> tmatrix_type(const A &a, const B &b)
		: m_base(a, b) {}

	float &operator=(float f) { return Base::template element<0,0>() = f; }
#if MATHLIB_NO_USING
	template<class A> float &operator=(const A &f) {
		float t = const_cast<A &>(f);
		return *this = t;
	}
#endif

	operator float &() { return Base::template element<0,0>(); }
};
template<class Base>
class tmatrix_type<false, true, false, Base>
	: public Base {
	typedef Base m_base;
public:
	// scalar, not l-value. disable matrix operations
	tmatrix_type() {}
	template<class A> explicit tmatrix_type(A &a) : m_base(a) {}
	template<class A> explicit tmatrix_type(const A &a) : m_base(a) {}
	template<class A, class B> tmatrix_type(const A &a, const B &b)
		: m_base(a, b) {}

	operator float() const { basic_matrix<1,1> t(*this); return *t.data(); }
};

// selects superclasses to provide (only) the supported operations for
// expressions
template<class Base>
class tmatrix : public tmatrix_type<Base::error,
	Base::rows == 1 && Base::columns == 1, Base::is_lvalue, Base> {
	typedef tmatrix_type<Base::error,
		Base::rows == 1 && Base::columns == 1,
		Base::is_lvalue, Base> m_base;
public:
	tmatrix() {}
	template<class A> explicit tmatrix(A &a) : m_base(a) {}
	template<class A> explicit tmatrix(const A &a) : m_base(a) {}
	template<class A, class B> tmatrix(const A &a, const B &b)
		: m_base(a, b) {}

#if MATHLIB_NO_USING
	template<class A> tmatrix &operator=(const A &a)
		{ m_base::operator=(a); return *this; }
#else
	using m_base::operator=;
#endif
};

// friendly matrix
template<int Rows, int Cols = Rows>
class matrix : public tmatrix<basic_matrix<Rows, Cols> > {
	typedef tmatrix<basic_matrix<Rows, Cols> > m_base;
public:
	matrix() {}
	matrix(zero_t);
	matrix(identity_t);
	template<class T> matrix(const tmatrix_dim<Rows, Cols, T> &);
	template<class T> matrix(const tmatrix_ndim<T> &);

#if MATHLIB_NO_USING
	template<class A> matrix &operator=(const A &a)
		{ m_base::operator=(a); return *this; }
#else
	using m_base::operator=;
#endif
};

// friendly column matrix
template<int Rows>
class vector : public tmatrix<basic_matrix<Rows, 1> > {
	typedef tmatrix<basic_matrix<Rows, 1> > m_base;
public:
	vector() {}
	explicit vector(float filler);
	vector(float x, float y) { assign(x, y); }
	vector(float x, float y, float z) { assign(x, y, z); }
	vector(float x, float y, float z, float w) { assign(x, y, z, w); }
	vector(zero_t);
	template<class T> vector(const tmatrix_dim<Rows, 1, T> &);
	template<class T> vector(const tmatrix_ndim<T> &);

#if MATHLIB_NO_USING
	template<class A> vector &operator=(const A &a)
		{ m_base::operator=(a);	return *this; }
#else
	using m_base::operator=;
#endif
};

} // namespace mathlib

#endif
 

#ifndef MATHLIB_PLANE_H
#define MATHLIB_PLANE_H

#include <mathlib/matrix.h>

namespace mathlib {

template<int ND>
class plane {
public:
	vector<ND> n;
	float D;

	plane() {}
	plane(const vector<ND> &n_, float d) : n(n_), D(d) {}
};

template<int D>
class parameter_plane {
public:
	vector<D> v, u;

	parameter_plane(const vector<D> &v_, const vector<D> &u_)
		: v(v_), u(u_) {}
	parameter_plane(const plane<D> &);

	operator plane<D>() const;
};

template<int ND> inline
float alignment(const plane<ND> &p, const vector<ND> &v)
{
	return dot(p.n, v) + p.D;
}

} // namespace mathlib

#endif
 

#ifndef MATHLIB_QUATERNION_H
#define MATHLIB_QUATERNION_H

#include <mathlib/complex.h>

namespace mathlib {

class quaternion {
public:
	float a, b, c, d;

	quaternion() {}
	quaternion(float a_, float b_ = 0.f, float c_ = 0.f, float d_ = 0.f)
		: a(a_), b(b_), c(c_), d(d_) {}

	float real() const { return a; }
	quaternion unreal() const { return quaternion(0.f, b, c, d); }

	quaternion &operator+=(float f) { a += f; return *this; }
	quaternion &operator-=(float f) { a -= f; return *this; }
	quaternion &operator*=(float f)
		{ a *= f; b *= f; c *= f; d *= f; return *this; }
	quaternion &operator/=(float f)
		{ a /= f; b /= f; c /= f; d /= f; return *this; }

	quaternion &operator+=(complex c)
		{ a += c.real(); b += c.imag(); return *this; }
	quaternion &operator-=(complex c)
		{ a -= c.real(); b -= c.imag(); return *this; }
	quaternion &operator*=(complex);
	quaternion &operator/=(complex);

	quaternion &operator+=(const quaternion &q)
		{ a += q.a; b += q.b; c += q.c; d += q.d; return *this; }
	quaternion &operator-=(const quaternion &q)
		{ a -= q.a; b -= q.b; c -= q.c; d -= q.d; return *this; }
	quaternion &operator*=(const quaternion &q)
		{ return *this = *this * q; }
	quaternion &operator/=(const quaternion &q)
		{ return *this = *this / q; }

	friend quaternion operator*(const quaternion &, const quaternion &);
	friend quaternion operator/(const quaternion &, const quaternion &);

	friend float real(const quaternion &q) { return q.real(); }
	friend quaternion unreal(const quaternion &q) { return q.unreal(); }

	friend float abs(const quaternion &q)
		{ return sqrt(q.a*q.a + q.b*q.b + q.c*q.c + q.d*q.d); }
	friend quaternion normalize(const quaternion &q)
		{ return q / abs(q); }
};

} // namespace mathlib

#endif
 

#ifndef MATHLIB_SEGMENT_H
#define MATHLIB_SEGMENT_H

namespace mathlib {

template<int D>
class segment {
public:
	vector<D> v1, v2;

	segment() {}
	segment(const vector<D> &a, const vector<D> &b) : v1(a), v2(b) {}
};

} // namespace mathlib
 

#ifndef MATHLIB_SIMD_CONFIG_H
#define MATHLIB_SIMD_CONFIG_H

// type definitions and defines for simd instructions

namespace mathlib {

#if defined(__GNUG__) && defined(__SSE__) || 1
	// g++ with sse instructions enabled
#	define MATHLIB_SIMD 1
#	define MATHLIB_SIMD4 1
#	define MATHLIB_GCC_SSE 1

#	define MATHLIB_SIMD4_ALIGN __attribute__((aligned(__alignof__(v4sf))))

// vector of 4 floats
typedef int v4sf __attribute__ ((mode(V4SF)));

#else
#	define MATHLIB_SIMD4_ALIGN
#endif

} // namespace mathlib

#endif
 

#ifndef MATHLIB_SPHERE_H
#define MATHLIB_SPHERE_H

#include <mathlib/matrix.h>

namespace mathlib {

template<int D>
class sphere {
public:
	vector<D> position;
	float radius;

	sphere() {}
	sphere(const vector<D> &pos, float r) : position(pos), radius(r) {}
};

} // namespace mathlib

#endif
 

#ifndef MATHLIB_TRIANGLE_H
#define MATHLIB_TRIANGLE_H

#include <mathlib/matrix.h>

namespace mathlib {

template<int D>
class triangle {
public:
	vector<D> a, b, c;

	triangle() {}
	triangle(const vector<D> &a_, const vector<D> &b_,
		const vector<D> &c_) : a(a_), b(b_), c(c_) {}
};

} // namespace mathlib

#endif
 

#ifndef MATHLIB_UTILITY_H
#define MATHLIB_UTILITY_H

// random mathematics functions

#include <cmath>

#include <mathlib/mathdefs.h>

namespace mathlib {

inline float sqr(float a) { return a*a; }

inline bool close(float a, float b) {
	return std::abs(a - b) < epsilon;
}

inline int sign(float a) {
	if (a > epsilon) return 1;
	if (a < -epsilon) return -1;
	return 0;
}

} // namespace mathlib

#endif
 

#include <mathlib/matrix.tpp>

namespace mathlib {

typedef basic_matrix<2, 2> m2;

void matrix_assign_handler::go(m2 &dest, matrix_unop<unop_inverse, m2> src)
{
	float det_src = det_bm(src.a);
	float d = 1.f / det_src;

	// dest = adjoint(src.a) / det(src.a)
	dest(0,0) = d * +src.a(1,1);
	dest(1,0) = d * -src.a(1,0);

	dest(0,1) = d * -src.a(0,1);
	dest(1,1) = d * +src.a(0,0);
}

} // namespace mathlib
 

#include <mathlib/matrix.tpp>

namespace mathlib {

typedef basic_matrix<3, 3> m3;
typedef basic_matrix<3, 1> v3;

float det_bm(const m3 &m)
{
	return m(0,0) * det(cofactor_bm<0,0>(m))
		- m(1,0) * det(cofactor_bm<1,0>(m))
		+ m(2,0) * det(cofactor_bm<2,0>(m));
}

void matrix_assign_handler::go(m3 &dest, matrix_unop<unop_adjoint, m3> src)
{
	dest(0,0) = +det(cofactor_bm<0,0>(src.a));
	dest(1,0) = -det(cofactor_bm<0,1>(src.a));
	dest(2,0) = +det(cofactor_bm<0,2>(src.a));

	dest(0,1) = -det(cofactor_bm<1,0>(src.a));
	dest(1,1) = +det(cofactor_bm<1,1>(src.a));
	dest(2,1) = -det(cofactor_bm<1,2>(src.a));

	dest(0,2) = +det(cofactor_bm<2,0>(src.a));
	dest(1,2) = -det(cofactor_bm<2,1>(src.a));
	dest(2,2) = +det(cofactor_bm<2,2>(src.a));
}

void matrix_assign_handler::go(m3 &dest, matrix_unop<unop_inverse, m3> src)
{
	float det_src = det_bm(src.a);
	float d = 1.f / det_src;

	// dest = adjoint(src.a) / det(src.a)
	dest(0,0) = d * +det(cofactor_bm<0,0>(src.a));
	dest(1,0) = d * -det(cofactor_bm<0,1>(src.a));
	dest(2,0) = d * +det(cofactor_bm<0,2>(src.a));

	dest(0,1) = d * -det(cofactor_bm<1,0>(src.a));
	dest(1,1) = d * +det(cofactor_bm<1,1>(src.a));
	dest(2,1) = d * -det(cofactor_bm<1,2>(src.a));

	dest(0,2) = d * +det(cofactor_bm<2,0>(src.a));
	dest(1,2) = d * -det(cofactor_bm<2,1>(src.a));
	dest(2,2) = d * +det(cofactor_bm<2,2>(src.a));
}

} // namespace mathlib
 

#include <mathlib/matrix.tpp>

namespace mathlib {

const matrix<4> frustum(float l, float r, float b, float t, float n, float f)
{
	float n2 = 2 * n;
	float rl = r - l;
	float tb = t - b;
	float fn = f - n;

	matrix<4> dest = zero;

	dest(0,0) = n2/rl;
	dest(0,2) = (r+l)/rl;
	dest(1,1) = n2/tb;
	dest(1,2) = (t+b)/tb;
	dest(2,2) = -(f+n)/fn;
	dest(2,3) = -2*f*n/fn;
	dest(3,2) = -1;

	return dest;
}

const matrix<4> ortho(float l, float r, float b, float t, float n, float f)
{
	float rl = r - l;
	float tb = t - b;
	float fn = f - n;

	matrix<4> dest = zero;

	dest(0,0) = 2/rl;
	dest(0,3) = (r+l)/rl;
	dest(1,1) = 2/tb;
	dest(1,3) = -(t+b)/tb;
	dest(2,2) = -2/fn;
	dest(2,3) = (f+n)/fn;
	dest(3,3) = 1;

	return dest;
}

void matrix_assign_handler::go(bm44 &dest, 
	matrix_unop<unop_translation, bm31> m)
{
	dest(0,3) = m.a.x;
	dest(1,3) = m.a.y;
	dest(2,3) = m.a.z;
	dest(0,1) = dest(0,2) = 0;
	dest(1,0) = dest(1,2) = 0;
	dest(2,0) = dest(2,1) = 0;
	dest(3,0) = dest(3,1) = dest(3,2) = 0;
	dest(0,0) = dest(1,1) = dest(2,2) = dest(3,3) = 1;
}

void matrix_assign_handler::go(bm44 &dest, matrix_unop<unop_scaling, 
	bm31> src)
{
	dest(0,0) = src.a.x;
	dest(1,1) = src.a.y;
	dest(2,2) = src.a.z;
	dest(3,3) = 1;
	dest(0,1) = dest(0,2) = dest(0,3) = 0;
	dest(1,0) = dest(1,2) = dest(1,3) = 0;
	dest(2,0) = dest(2,1) = dest(2,3) = 0;
	dest(3,0) = dest(3,1) = dest(3,2) = 0;
}

void matrix_assign_handler::go(bm44 &dest, matrix_unop<unop_scaling, 
	float_keeper> src)
{
	dest(0,0) = dest(1,1) = dest(2,2) = src.a.f;
	dest(3,3) = 1;
	dest(0,1) = dest(0,2) = dest(0,3) = 0;
	dest(1,0) = dest(1,2) = dest(1,3) = 0;
	dest(2,0) = dest(2,1) = dest(2,3) = 0;
	dest(3,0) = dest(3,1) = dest(3,2) = 0;
}

void matrix_assign_handler::go(bm44 &dest, matrix_binop<binop_rotation, 
	bm31, float_keeper> src)
{
	vector<3> u = norm(src.a.matrix());
	matrix<3> S;
	S <<    0, -u.z,  u.y,
	      u.z,    0, -u.x,
	     -u.y,  u.x,    0;

	matrix<3> uut = u*transpose(u);
	matrix<3> I = identity;
	matrix<4> & MATHLIB_RESTRICT R = dest.matrix();
	float a = src.b.f;

	R[r02,c02] = uut + cos(a)*(I - uut) + sin(a)*S;
	R[r3,c02] = zero;
	R[r02,c3] = zero;
	R(3,3) = 1;
}

void matrix_assign_handler::go(bm44 &dest, matrix_binop<binop_rotation, 
	x_axis_type, float_keeper> src)
{
	float c = cos(src.b.f), s = sin(src.b.f);

	dest(1,1) =  c;
	dest(2,2) =  c;
	dest(1,2) = -s;
	dest(2,1) =  s;

	dest(0,1) = dest(0,2) = 0;
	dest(1,0) = dest(2,0) = 0;
	dest(0,0) = 1;

	dest(0,3) = dest(1,3) = dest(2,3) = dest(3,0) = dest(3,1)
		= dest(3,2) = 0;
	dest(3,3) = 1;
}

void matrix_assign_handler::go(bm44 &dest, matrix_binop<binop_rotation, 
	y_axis_type, float_keeper> src)
{
	float c = cos(src.b.f), s = sin(src.b.f);

	dest(0,0) =  c;
	dest(2,2) =  c;
	dest(2,0) = -s;
	dest(0,2) =  s;

	dest(0,1) = dest(1,2) = 0;
	dest(1,0) = dest(2,1) = 0;
	dest(1,1) = 1;

	dest(0,3) = dest(1,3) = dest(2,3) = dest(3,0) = dest(3,1)
		= dest(3,2) = 0;
	dest(3,3) = 1;
}

void matrix_assign_handler::go(bm44 &dest, matrix_binop<binop_rotation, 
	z_axis_type, float_keeper> src)
{
	float c = cos(src.b.f), s = sin(src.b.f);

	dest(0,0) =  c;
	dest(1,1) =  c;
	dest(0,1) = -s;
	dest(1,0) =  s;

	dest(0,2) = dest(1,2) = 0;
	dest(2,0) = dest(2,1) = 0;
	dest(2,2) = 1;

	dest(0,3) = dest(1,3) = dest(2,3) = dest(3,0) = dest(3,1)
		= dest(3,2) = 0;
	dest(3,3) = 1;
}

float det_bm(const bm44 &m)
{
	return m(0,0) * det(cofactor_bm<0,0>(m))
		- m(1,0) * det(cofactor_bm<1,0>(m))
		+ m(2,0) * det(cofactor_bm<2,0>(m))
		- m(3,0) * det(cofactor_bm<3,0>(m));
}

void matrix_assign_handler::go(bm44 &dest, matrix_unop<unop_adjoint, bm44> src)
{
	matrix<4> & MATHLIB_RESTRICT R = dest.matrix();
	const matrix<4> & MATHLIB_RESTRICT S = src.a.matrix();

	R(0,0) =  det(cofactor<0,0>(S));
	R(1,0) = -det(cofactor<0,1>(S));
	R(2,0) =  det(cofactor<0,2>(S));
	R(3,0) = -det(cofactor<0,3>(S));

	R(0,1) = -det(cofactor<1,0>(S));
	R(1,1) =  det(cofactor<1,1>(S));
	R(2,1) = -det(cofactor<1,2>(S));
	R(3,1) =  det(cofactor<1,3>(S));

	R(0,2) =  det(cofactor<2,0>(S));
	R(1,2) = -det(cofactor<2,1>(S));
	R(2,2) =  det(cofactor<2,2>(S));
	R(3,2) = -det(cofactor<2,3>(S));

	R(0,3) = -det(cofactor<3,0>(S));
	R(1,3) =  det(cofactor<3,1>(S));
	R(2,3) = -det(cofactor<3,2>(S));
	R(3,3) =  det(cofactor<3,3>(S));
}

void matrix_assign_handler::go(bm44 &dest, matrix_unop<unop_inverse, bm44> src)
{
	matrix<4> & MATHLIB_RESTRICT R = dest.matrix();
	const matrix<4> & MATHLIB_RESTRICT S = src.a.matrix();

	float det_s = det(S);
	float d = 1 / det_s;

	// dest = adjoint(src.a) / det(src.a)
	R(0,0) = d *  det(cofactor<0,0>(S));
	R(1,0) = d * -det(cofactor<0,1>(S));
	R(2,0) = d *  det(cofactor<0,2>(S));
	R(3,0) = d * -det(cofactor<0,3>(S));

	R(0,1) = d * -det(cofactor<1,0>(S));
	R(1,1) = d *  det(cofactor<1,1>(S));
	R(2,1) = d * -det(cofactor<1,2>(S));
	R(3,1) = d *  det(cofactor<1,3>(S));

	R(0,2) = d *  det(cofactor<2,0>(S));
	R(1,2) = d * -det(cofactor<2,1>(S));
	R(2,2) = d *  det(cofactor<2,2>(S));
	R(3,2) = d * -det(cofactor<2,3>(S));

	R(0,3) = d * -det(cofactor<3,0>(S));
	R(1,3) = d *  det(cofactor<3,1>(S));
	R(2,3) = d * -det(cofactor<3,2>(S));
	R(3,3) = d *  det(cofactor<3,3>(S));
}

} // namespace mathlib
 

#include <mathlib/quaternion.h>

namespace mathlib {

quaternion operator*(const quaternion &a, const quaternion &b) {
	quaternion r;
	r.a = a.a*b.a - a.b*b.b - a.c*b.c - a.d*b.d;
	r.b = a.a*b.b + a.b*b.a + a.c*b.d - a.d*b.c;
	r.c = a.a*b.c - a.b*b.d + a.c*b.a + a.d*b.b;
	r.d = a.a*b.d + a.b*b.c - a.c*b.b + a.d*b.a;
	return r;
}

quaternion operator/(const quaternion &a, const quaternion &b) {
	quaternion r;
	float d = abs(b);
	r.a = (+a.a*b.a + a.b*b.b + a.c*b.c + a.d*b.d) / d;
	r.b = (-a.a*b.b + a.b*b.a - a.c*b.d + a.d*b.c) / d;
	r.c = (-a.a*b.c + a.b*b.d + a.c*b.a - a.d*b.b) / d;
	r.d = (-a.a*b.d - a.b*b.c + a.c*b.b + a.d*b.a) / d;
	return r;
}

} // namespace mathlib
 

#include <mathlib/simd/config.h>

#if MATHLIB_GCC_SSE

#include <mathlib/matrix.tpp>

#include <xmmintrin.h>

namespace mathlib {

void simd_assign_handler<unop_copy>::go(bm44 &dest,
	matrix_binop<binop_mmul<4>, bm44, bm44> src) {

	v4sf v, f1, f2, f3, f4;
	v4sf c1, c2, c3, c4;
	v4sf ac1, ac2, ac3, ac4;
	v4sf s1, s2;

	ac1 = ssev(src.a)[0];
	ac2 = ssev(src.a)[1];
	ac3 = ssev(src.a)[2];
	ac4 = ssev(src.a)[3];

	v = ssev(src.b)[0];
	f1 = __builtin_ia32_shufps(v, v, 0x00);
	f2 = __builtin_ia32_shufps(v, v, 0x55);
	f3 = __builtin_ia32_shufps(v, v, 0xaa);
	f4 = __builtin_ia32_shufps(v, v, 0xff);
	c1 = __builtin_ia32_mulps(f1, ac1);
	c2 = __builtin_ia32_mulps(f2, ac2);
	c3 = __builtin_ia32_mulps(f3, ac3);
	c4 = __builtin_ia32_mulps(f4, ac4);
	s1 = __builtin_ia32_addps(c1, c2);
	s2 = __builtin_ia32_addps(c3, c4);
	ssev(dest)[0] = __builtin_ia32_addps(s1, s2);

	v = ssev(src.b)[1];
	f1 = __builtin_ia32_shufps(v, v, 0x00);
	f2 = __builtin_ia32_shufps(v, v, 0x55);
	f3 = __builtin_ia32_shufps(v, v, 0xaa);
	f4 = __builtin_ia32_shufps(v, v, 0xff);
	c1 = __builtin_ia32_mulps(f1, ac1);
	c2 = __builtin_ia32_mulps(f2, ac2);
	c3 = __builtin_ia32_mulps(f3, ac3);
	c4 = __builtin_ia32_mulps(f4, ac4);
	s1 = __builtin_ia32_addps(c1, c2);
	s2 = __builtin_ia32_addps(c3, c4);
	ssev(dest)[1] = __builtin_ia32_addps(s1, s2);

	v = ssev(src.b)[2];
	f1 = __builtin_ia32_shufps(v, v, 0x00);
	f2 = __builtin_ia32_shufps(v, v, 0x55);
	f3 = __builtin_ia32_shufps(v, v, 0xaa);
	f4 = __builtin_ia32_shufps(v, v, 0xff);
	c1 = __builtin_ia32_mulps(f1, ac1);
	c2 = __builtin_ia32_mulps(f2, ac2);
	c3 = __builtin_ia32_mulps(f3, ac3);
	c4 = __builtin_ia32_mulps(f4, ac4);
	s1 = __builtin_ia32_addps(c1, c2);
	s2 = __builtin_ia32_addps(c3, c4);
	ssev(dest)[2] = __builtin_ia32_addps(s1, s2);

	v = ssev(src.b)[3];
	f1 = __builtin_ia32_shufps(v, v, 0x00);
	f2 = __builtin_ia32_shufps(v, v, 0x55);
	f3 = __builtin_ia32_shufps(v, v, 0xaa);
	f4 = __builtin_ia32_shufps(v, v, 0xff);
	c1 = __builtin_ia32_mulps(f1, ac1);
	c2 = __builtin_ia32_mulps(f2, ac2);
	c3 = __builtin_ia32_mulps(f3, ac3);
	c4 = __builtin_ia32_mulps(f4, ac4);
	s1 = __builtin_ia32_addps(c1, c2);
	s2 = __builtin_ia32_addps(c3, c4);
	ssev(dest)[3] = __builtin_ia32_addps(s1, s2);
}

} // namespace mathlib

#endif // MATHLIB_GCC_SSE