#ifndef K3DSDK_ALGEBRA_H
#define K3DSDK_ALGEBRA_H

// K-3D
// Copyright (c) 1995-2004, Timothy M. Shead
//
// Contact: tshead@k-3d.com
//
// This library is free software; you can redistribute it and/or
// modify it under the terms of the GNU General Public
// License as published by the Free Software Foundation; either
// version 2 of the License, or (at your option) any later version.
//
// This library is distributed in the hope that it will be useful,
// but WITHOUT ANY WARRANTY; without even the implied warranty of
// MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
// General Public License for more details.
//
// You should have received a copy of the GNU General Public
// License along with this library; if not, write to the Free Software
// Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA

/** \file
		\brief Algebra classes
		\author Tim Shead (tshead@k-3d.com)
*/

#include "basic_math.h"
#include "vectors.h"

#ifndef FLT_EPSILON
#define FLT_EPSILON 0.000001
#endif

/****************************************************************
*
* C++ Vector and Matrix Algebra routines
* Author: Jean-Francois DOUE
* Version 3.1 --- October 1993
*
****************************************************************/

//
//	From "Graphics Gems IV / Edited by Paul S. Heckbert
//	Academic Press, 1994, ISBN 0-12-336156-9
//	"You are free to use and modify this code in any way
//	you like." (p. xv)
//
//	Modified by J. Nagle, March 1997
//	-	All functions are inline.
//	-	All functions are const-correct.
//	-	All checking is via the standard "assert" macro.
//

// Modified by Tim Shead for use with K-3D, January 1998

namespace k3d
{

class matrix3;
class matrix4;
class quaternion;
class angle_axis;
class euler_angles;

/////////////////////////////////////////////////////////////////////////////
// vector2

/// Multiplication by a matrix
vector2 operator * (const matrix3& a, const vector2& v);
/// Multiplication by a matrix
vector2 operator * (const vector2& v, const matrix3& a);

/////////////////////////////////////////////////////////////////////////////
// vector3

/// Matrix multiplication
vector3 operator * (const matrix4& a, const vector3& v);
/// Matrix multiplication
vector3 operator * (const vector3& v, const matrix4& a);
/// Returns a linear transformation
vector2 operator * (const matrix3& a, const vector2& v);
/// Returns a matrix product
matrix3 operator * (const matrix3& a, const matrix3& b);

/////////////////////////////////////////////////////////////////////////////
// vector4

/// Matrix multiplication
vector4 operator * (const matrix4& a, const vector4& v);
/// Matrix multiplication
vector4 operator * (const vector4& v, const matrix4& a);
/// Linear transformation
vector3 operator * (const matrix4& a, const vector3& v);
/// Matrix multiplication
matrix4 operator * (const matrix4& a, const matrix4& b);

/////////////////////////////////////////////////////////////////////////////
// matrix3

/// A 3x3 matrix
class matrix3
{
public:
	/// Stores the matrix data
	vector3 v[3];
	// Constructors
	matrix3();
	/// Creates the matrix from three vectors
	matrix3(const vector3& v0, const vector3& v1, const vector3& v2);
	/// Creates a matrix of identical elements
	matrix3(const double d);
	/// Copy constructor
	matrix3(const matrix3& m);
	/// Assignment
	matrix3& operator = (const matrix3& m);
	/// Addition
	matrix3& operator += (const matrix3& m);
	/// Subtraction
	matrix3& operator -= (const matrix3& m);
	/// Multiplication by a constant
	matrix3& operator *= (const double d);
	/// Division by a constant
	matrix3& operator /= (const double d);
	/// Returns a vector reference by index
	vector3& operator [] (int i);
	/// Returns a vector reference by index
	const vector3& operator [] (int i) const;
	/// Returns the matrix transposition
	matrix3 Transpose() const;
	/// Returns the matrix inverse
	matrix3 Inverse() const;
	/// Decomposes the matrix returning rotations around X, Y and Z axes
	void Decompose(double& x, double& y, double& z) const;
};

/// Negation
matrix3 operator - (const matrix3& a);
/// Addition
matrix3 operator + (const matrix3& a, const matrix3& b);
/// Subtraction
matrix3 operator - (const matrix3& a, const matrix3& b);
/// Matrix multiplication
matrix3 operator * (const matrix3& a, const matrix3& b);
/// Multiplication by a constant
matrix3 operator * (const matrix3& a, const double d);
/// Multiplication by a constant
matrix3 operator * (const double d, const matrix3& a);
/// Division by a constant
matrix3 operator / (const matrix3& a, const double d);
/// Equality
bool operator == (const matrix3& a, const matrix3& b);
/// Non-equality
bool operator != (const matrix3& a, const matrix3& b);
/// Linear transformation
vector3 operator * (const matrix3& a, const vector3& v);
/// Linear transformation
vector2 operator * (const matrix3& a, const vector2& v);

/// Output inserter
std::ostream& operator<<(std::ostream& Stream, const matrix3& Arg);
/// Input extractor
std::istream& operator>>(std::istream& Stream, matrix3& Arg);

/////////////////////////////////////////////////////////////////////////////
// matrix4

/// A 4x4 matrix
class matrix4
{
public:
	/// Stores the matrix elements
	vector4 v[4];
	// Constructors
	matrix4();
	matrix4(const vector4& v0, const vector4& v1, const vector4& v2, const vector4& v3);
	matrix4(const double d);
	matrix4(euler_angles Angles);
	/// Copy constructor
	matrix4(const matrix4& m);
	/// Assignment of an matrix4
	matrix4& operator	= ( const matrix4& m );
	/// Assignment of a C/C++ array
	matrix4& operator  = ( const double d[4][4]);
	/// Assignment of a C/C++ array
	matrix4& operator  = ( const double d[16]);
	/// Addition
	matrix4& operator += ( const matrix4& m );
	/// Subtraction
	matrix4& operator -= ( const matrix4& m );
	/// Multiplication by a constant
	matrix4& operator *= ( const double d );
	/// Division by a constant
	matrix4& operator /= ( const double d );
	/// Returns a vector by index
	vector4& operator [] ( int i);
	/// Returns a vector by index
	const vector4& operator [] ( int i) const;
	/// Returns the matrix transposition
	matrix4 Transpose() const;
	/// Returns the matrix inverse
	matrix4 Inverse() const;
	/// Copies the matrix contents into a C/C++ style array
	void CopyArray(float m[16]) const;
	/// Copies the matrix contents into a C/C++ style array
	void CopyArray(double m[16]) const;
	/// Casts the matrix to a C/C++ style array
	operator double*() { return &v[0][0]; }
};

/// Negation
matrix4 operator - (const matrix4& a);
/// Addition
matrix4 operator + (const matrix4& a, const matrix4& b);
/// Subtraction
matrix4 operator - (const matrix4& a, const matrix4& b);
/// Matrix multiplication
matrix4 operator * (const matrix4& a, const matrix4& b);
/// Multiplication by a constant
matrix4 operator * (const matrix4& a, const double d);
/// Multiplication by a constant
matrix4 operator * (const double d, const matrix4& a);
/// Division by a constant
matrix4 operator / (const matrix4& a, const double d);
/// Equality
bool operator == (const matrix4& a, const matrix4& b);
/// Non-equality
bool operator != (const matrix4& a, const matrix4& b);
/// Linear transformation
vector4 operator * (const matrix4& a, const vector4& v);
/// Linear transformation
vector3 operator * (const matrix4& a, const vector3& v);

/// Output inserter
std::ostream& operator<<(std::ostream& Stream, const matrix4& Arg);
/// Input extractor
std::istream& operator>>(std::istream& Stream, matrix4& Arg);

/////////////////////////////////////////////////////////////////////////////
// quaternion

/// Encapsulates a quaternion
class quaternion
{
public:
	// The scalar+vector representation is used
	double w;
	vector3 v;

	// Constructors
	quaternion();
	quaternion(const double u, const vector3& t);
	quaternion(const double u, const double x, const double y, const double z);
	quaternion(const angle_axis& AngleAxis);
	quaternion(euler_angles Angles);
	/// Assignment of a quaternion
	quaternion& operator = (const quaternion& q);
	/// Assignment of an axis/angle
	quaternion& operator = (const angle_axis& AngleAxis);
	/// Multiplication by a constant
	quaternion& operator *= (const double f);
	/// Division by a constant
	quaternion& operator /= (const double f);
	/// Addition
	quaternion& operator += (const quaternion& q);
	/// Substraction
	quaternion& operator -= (const quaternion& q);
	/// Multiplication
	quaternion& operator *= (const quaternion& q);
	/// Division
	quaternion& operator /= (const quaternion& q);
	/// Magnitude
	double Magnitude() const;
	/// Normalization
	quaternion& Normalize();
	/// Conjugate
	quaternion& Conjugate();
	/// Inversion in place
	quaternion& Inverse();
	/// Squares the quaternion in place
	quaternion& Square();
	/// Axis rotation
	quaternion& AxisRotation(const double phi);
};

/// Negation
quaternion operator - (quaternion& q);
/// Addition
quaternion operator + (const quaternion& q, const quaternion& r);
/// Substraction
quaternion operator - (const quaternion& q, const quaternion& r);
/// Multiplication by a quaternion
quaternion operator * (const quaternion& q, const quaternion& r);
/// Multiplication by a constant
quaternion operator * (const quaternion& q, const double f);
/// Multiplication by a constant
quaternion operator * (double f, const quaternion& q);
/// Division by a quaternion
quaternion operator / (const quaternion& q, const quaternion& r);
/// Division by a constant
quaternion operator / (const quaternion& q, const double f);
/// Equality test
bool operator == (const quaternion& q, const quaternion& r);
/// Non-equality test
bool operator != (const quaternion& q, const quaternion& r);

/// Output inserter
std::ostream& operator<<(std::ostream& Stream, const quaternion& Arg);
/// Input extractor
std::istream& operator>>(std::istream& Stream, quaternion& Arg);

/////////////////////////////////////////////////////////////////////////////
// angle_axis

/// Encapsulates an angle/axis representation of an object's orientation
class angle_axis
{
public:
	double angle;
	vector3 axis;

	// Constructors ...
	angle_axis();
	angle_axis(const double Angle, const vector3& Axis);
	angle_axis(const double Angle, const double X, const double Y, const double Z);
	angle_axis(const double AngleAxis[4]);
	angle_axis(const quaternion& Quaternion);
	/// Copy constructor
	angle_axis(const angle_axis& AngleAxis);
	/// Assignment of an angle_axis
	angle_axis& operator = ( const angle_axis& AngleAxis );
	/// Assignment of a quaternion
	angle_axis& operator = ( const quaternion& Quaternion );
	/// Assignment of a C/C++ style array
	angle_axis& operator = ( const double AngleAxis[4]);
	/// Casting to a C/C++ style array pointer
	operator double*() { return &angle; }
	/// Copies the vector into a C/C++ style array
	void CopyArray(float AngleAxis[4]) const;
	/// Copies the vector into a C/C++ style array
	void CopyArray(double AngleAxis[4]) const;
};

/// Equality test
bool operator == (const angle_axis& a, const angle_axis& b);
/// Non-equality test
bool operator != (const angle_axis& a, const angle_axis& b);

/// Output inserter
std::ostream& operator<<(std::ostream& Stream, const angle_axis& Arg);
/// Input extractor
std::istream& operator>>(std::istream& Stream, angle_axis& Arg);

#define SDPEULERANGLEORDER(initialaxis, parity, repetition, frame) ((((((initialaxis << 1) + parity) << 1) + repetition) << 1) + frame)

/// Encapsulates a set of Euler angles, including the order in which they should be applied
class euler_angles
{
public:

	/// Enumerates what gets rotated - the axes themselves, or the frame they represent
	typedef enum
	{
		StaticFrame = 0,
		RotatingFrame = 1,
	} OrderFrame;

	/// Enumerates whether the last axis will be the same as the first
	typedef enum
	{
		NoRepetition = 0,
		Repeats = 1,
	} OrderRepetition;

	/// Enumerates which axes will become the second axis
	typedef enum
	{
		EvenParity = 0,
		OddParity = 1,
	} OrderParity;

	/// Enumerates all possible axis order permutations
	typedef enum
	{
		XYZstatic = SDPEULERANGLEORDER(0, EvenParity, NoRepetition, StaticFrame),
		XYXstatic = SDPEULERANGLEORDER(0, EvenParity, Repeats, StaticFrame),
		XZYstatic = SDPEULERANGLEORDER(0, OddParity, NoRepetition, StaticFrame),
		XZXstatic = SDPEULERANGLEORDER(0, OddParity, Repeats, StaticFrame),
		YZXstatic = SDPEULERANGLEORDER(1, EvenParity, NoRepetition, StaticFrame),
		YZYstatic = SDPEULERANGLEORDER(1, EvenParity, Repeats, StaticFrame),
		YXZstatic = SDPEULERANGLEORDER(1, OddParity, NoRepetition, StaticFrame),
		YXYstatic = SDPEULERANGLEORDER(1, OddParity, Repeats, StaticFrame),
		ZXYstatic = SDPEULERANGLEORDER(2, EvenParity, NoRepetition, StaticFrame),
		ZXZstatic = SDPEULERANGLEORDER(2, EvenParity, Repeats, StaticFrame),
		ZYXstatic = SDPEULERANGLEORDER(2, OddParity, NoRepetition, StaticFrame),
		ZYZstatic = SDPEULERANGLEORDER(2, OddParity, Repeats, StaticFrame),
		ZYXrotating = SDPEULERANGLEORDER(0, EvenParity, NoRepetition, RotatingFrame),
		XYXrotating = SDPEULERANGLEORDER(0, EvenParity, Repeats, RotatingFrame),
		YZXrotating = SDPEULERANGLEORDER(0, OddParity, NoRepetition, RotatingFrame),
		XZXrotating = SDPEULERANGLEORDER(0, OddParity, Repeats, RotatingFrame),
		XZYrotating = SDPEULERANGLEORDER(1, EvenParity, NoRepetition, RotatingFrame),
		YZYrotating = SDPEULERANGLEORDER(1, EvenParity, Repeats, RotatingFrame),
		ZXYrotating = SDPEULERANGLEORDER(1, OddParity, NoRepetition, RotatingFrame),
		YXYrotating = SDPEULERANGLEORDER(1, OddParity, Repeats, RotatingFrame),
		YXZrotating = SDPEULERANGLEORDER(2, EvenParity, NoRepetition, RotatingFrame),
		ZXZrotating = SDPEULERANGLEORDER(2, EvenParity, Repeats, RotatingFrame),
		XYZrotating = SDPEULERANGLEORDER(2, OddParity, NoRepetition, RotatingFrame),
		ZYZrotating = SDPEULERANGLEORDER(2, OddParity, Repeats, RotatingFrame),
	} AngleOrder;

	/// Stores the three axis angles
	double n[3];
	/// Stores the axis order in packed format
	AngleOrder order;

	euler_angles();
	euler_angles(vector3 Angles, AngleOrder Order);
	euler_angles(double X, double Y, double Z, AngleOrder Order);
	/// Conversion from a quaternion
	euler_angles(const quaternion& Quaternion, AngleOrder Order);
	/// Conversion from a 4x4 matrix
	euler_angles(const matrix4& Matrix, AngleOrder Order);

	/// Returns the frame type from a packed order representation
	static OrderFrame Frame(AngleOrder& Order) { return OrderFrame(Order & 1); }
	/// Returns the repetition type from a packed order representation
	static OrderRepetition Repetition(AngleOrder& Order) { return OrderRepetition((Order >> 1) & 1); }
	/// Returns the parity type from a packed order representation
	static OrderParity Parity(AngleOrder& Order) { return OrderParity((Order >> 2) & 1); }
	/// Returns the axes in order, from a packed order representation
	static void Axes(AngleOrder& Order, int& i, int& j, int& k);

	/// Returns the frame type of this instance
	OrderFrame Frame() { return Frame(order); }
	/// Returns the repetition type of this instance
	OrderRepetition Repetition() { return Repetition(order); }
	/// Returns the parity type of this instance
	OrderParity Parity() { return Parity(order); }
	/// Returns the axes in order, for this instance
	void Axes(int& i, int& j, int& k) { Axes(order, i, j, k); }

	/// Returns the given angle by reference
	double& operator [] ( int i);
	/// Returns the given angle by value
	double operator[] (int i) const;
};

/// Output inserter
std::ostream& operator<<(std::ostream& Stream, const euler_angles& Arg);
/// Input extractor
std::istream& operator>>(std::istream& Stream, euler_angles& Arg);

/////////////////////////////////////////////////////////////////////////////
// 2D and 3D functions

/// Returns a two-dimensional identity matrix
matrix3 identity2D();
/// Returns a three-dimensional identity matrix
matrix4 identity3D();
/// Returns a two-dimensional translation matrix
matrix3 translation2D(const vector2& v);
/// Returns a three-dimensional translation matrix
matrix4 translation3D(const vector3& v);
/// Returns a two-dimensional rotation matrix
matrix3 rotation2D(const vector2& Center, const double Angle);
/// Returns a three-dimensional rotation matrix about an arbitrary axis
matrix4 rotation3D(const double Angle, vector3 Axis);
/// Returns a three-dimensional rotation matrix about an arbitrary axis
matrix4 rotation3D(const angle_axis& AngleAxis);
/// Returns a three-dimensional rotation matrix based on a set of Euler angles
matrix4 rotation3D(const vector3& EulerAngles);
/// Returns a three-dimensional rotation matrix based on a quaternion
matrix4 rotation3D(const quaternion& Quaternion);
/// Returns a quaternion based on a three-dimensional rotation matrix
quaternion rotation3D(const matrix4& m);
/// Converts a 3x3 rotation matrix into 4x4
matrix4 rotation3D(const matrix3& m);
/// Returns a two-dimensional scaling matrix
matrix3 scaling2D(const vector2& scaleVector);
/// Returns a three-dimensional scaling matrix
matrix4 scaling3D(const vector3& scaleVector);
/// Returns a three-dimensional perspective matrix
matrix4 perspective3D(const double d);
/// Extracts rotation from a three-dimensional matrix
matrix3 extractRotation(const matrix4& pose);
/// Extracts translation from a three-dimensional matrix
vector3 extractTranslation(const matrix4& pose);
/// Extracts scaling from a three-dimensional matrix
vector3 extractScaling(const matrix4& pose);

/////////////////////////////////////////////////////////////////////////////
// vector2 implementation

inline vector2 operator * (const matrix3& a, const vector2& v) {
	vector3 av;

	av.n[VX] = a.v[0].n[VX]*v.n[VX] + a.v[0].n[VY]*v.n[VY] + a.v[0].n[VZ];
	av.n[VY] = a.v[1].n[VX]*v.n[VX] + a.v[1].n[VY]*v.n[VY] + a.v[1].n[VZ];
	av.n[VZ] = a.v[2].n[VX]*v.n[VX] + a.v[2].n[VY]*v.n[VY] + a.v[2].n[VZ];
	return av;
}

inline vector2 operator * (const vector2& v, const matrix3& a)
{ return a.Transpose() * v; }

/////////////////////////////////////////////////////////////////////////////
// vector3 implementation

inline vector3 operator * (const matrix4& a, const vector3& v)
{ return a * vector4(v); }

inline vector3 operator * (const vector3& v, const matrix4& a)
{ return a.Transpose() * v; }

/////////////////////////////////////////////////////////////////////////////
// vector4 implementation

inline vector4 operator * (const matrix4& a, const vector4& v)
{
#define ROWCOL(i) a.v[i].n[0]*v.n[VX] + a.v[i].n[1]*v.n[VY] + a.v[i].n[2]*v.n[VZ] + a.v[i].n[3]*v.n[VW]
	return vector4(ROWCOL(0), ROWCOL(1), ROWCOL(2), ROWCOL(3));
#undef ROWCOL // (i)
}

inline vector4 operator * (const vector4& v, const matrix4& a)
{ return a.Transpose() * v; }

/////////////////////////////////////////////////////////////////////////////
// matrix3 implementation

inline matrix3::matrix3() {}

inline matrix3::matrix3(const vector3& v0, const vector3& v1, const vector3& v2)
{ v[0] = v0; v[1] = v1; v[2] = v2; }

inline matrix3::matrix3(const double d)
{ v[0] = v[1] = v[2] = vector3(d); }

inline matrix3::matrix3(const matrix3& m)
{ v[0] = m.v[0]; v[1] = m.v[1]; v[2] = m.v[2]; }

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

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

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

inline matrix3& matrix3::operator *= ( const double d )
{ v[0] *= d; v[1] *= d; v[2] *= d; return *this; }

inline matrix3& matrix3::operator /= ( const double d )
{ v[0] /= d; v[1] /= d; v[2] /= d; return *this; }

inline vector3& matrix3::operator [] ( int i) {
	assert(! (i < VX || i > VZ));
	return v[i];
}

inline const vector3& matrix3::operator [] ( int i) const {
	assert(!(i < VX || i > VZ));
	return v[i];
}

inline matrix3 matrix3::Transpose() const {
	return matrix3(vector3(v[0][0], v[1][0], v[2][0]),
		vector3(v[0][1], v[1][1], v[2][1]),
		vector3(v[0][2], v[1][2], v[2][2]));
}

inline matrix3 matrix3::Inverse()	const // Gauss-Jordan elimination with partial pivoting
{
	matrix3 a(*this),	 // As a evolves from original mat into identity
	b(identity2D()); // b evolves from identity into Inverse(a)

	// Loop over cols of a from left to right, eliminating above and below diag
	for (int j=0; j<3; ++j) { // Find largest pivot in column j among rows j..2
		int i1 = j;		 // Row with largest pivot candidate
		for (int i=j+1; i<3; ++i) {
			if (fabs(a.v[i].n[j]) > fabs(a.v[i1].n[j]))
				i1 = i;
		}

		// Swap rows i1 and j in a and b to put pivot on diagonal
		std::swap(a.v[i1], a.v[j]);
		std::swap(b.v[i1], b.v[j]);

		// Scale row j to have a unit diagonal
		if(!a.v[j].n[j])
			{
				// Singular matrix - can't invert
				std::cerr << __PRETTY_FUNCTION__ << " : singular matrix, can't invert!" << std::endl;
				return b;
			}

		b.v[j] /= a.v[j].n[j];
		a.v[j] /= a.v[j].n[j];

		// Eliminate off-diagonal elems in col j of a, doing identical ops to b
		for (int i=0; i<3; ++i) {
			if (i!=j) {
				b.v[i] -= a.v[i].n[j]*b.v[j];
				a.v[i] -= a.v[i].n[j]*a.v[j];
			}
		}
	}
	return b;
}

inline void matrix3::Decompose(double& x, double& y, double& z)	const
{
	y = atan2(-v[2][0], sqrt(v[0][0]*v[0][0]) + v[1][0]*v[1][0]);
	double t = cos(y);
	if(t != 0.0) {
		t = 1.0/t;
		z = atan2(v[1][0]*t, v[0][0]*t);
		x = atan2(v[2][1]*t, v[2][2]*t);
	} else {
		z = atan2(v[1][0], v[0][0]);
		x = atan2(v[2][1], v[2][2]);
	}
}

inline matrix3 operator - (const matrix3& a)
{ return matrix3(-a.v[0], -a.v[1], -a.v[2]); }

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

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

inline matrix3 operator * (const matrix3& a, const matrix3& b)
{
#define ROWCOL(i, j) a.v[i].n[0]*b.v[0][j] + a.v[i].n[1]*b.v[1][j] + a.v[i].n[2]*b.v[2][j]
	return matrix3(vector3(ROWCOL(0,0), ROWCOL(0,1), ROWCOL(0,2)),
					vector3(ROWCOL(1,0), ROWCOL(1,1), ROWCOL(1,2)),
					vector3(ROWCOL(2,0), ROWCOL(2,1), ROWCOL(2,2)));
#undef ROWCOL // (i, j)
}

inline matrix3 operator * (const matrix3& a, const double d)
{ return matrix3(a.v[0] * d, a.v[1] * d, a.v[2] * d); }

inline matrix3 operator * (const double d, const matrix3& a)
{ return a*d; }

inline matrix3 operator / (const matrix3& a, const double d)
{ return matrix3(a.v[0] / d, a.v[1] / d, a.v[2] / d); }

inline bool operator == (const matrix3& a, const matrix3& b)
{ return (a.v[0] == b.v[0]) && (a.v[1] == b.v[1]) && (a.v[2] == b.v[2]); }

inline bool operator != (const matrix3& a, const matrix3& b)
{ return !(a == b); }

inline std::ostream& operator<<(std::ostream& Stream, const matrix3& Arg)
{
	Stream << Arg[0] << " " << Arg[1] << " " << Arg[2];
	return Stream;
}

inline std::istream& operator>>(std::istream& Stream, matrix3& Arg)
{
	Stream >> Arg[0] >> Arg[1] >> Arg[2];
	return Stream;
}

/////////////////////////////////////////////////////////////////////////////
// matrix4 implementation

inline matrix4::matrix4() {}

inline matrix4::matrix4(const vector4& v0, const vector4& v1, const vector4& v2, const vector4& v3)
{ v[0] = v0; v[1] = v1; v[2] = v2; v[3] = v3; }

inline matrix4::matrix4(const double d)
{ v[0] = v[1] = v[2] = v[3] = vector4(d); }

inline matrix4::matrix4(const matrix4& m)
{ v[0] = m.v[0]; v[1] = m.v[1]; v[2] = m.v[2]; v[3] = m.v[3]; }

inline matrix4::matrix4(euler_angles Angles)
{
	const euler_angles::OrderFrame frame = Angles.Frame();
	const euler_angles::OrderParity parity = Angles.Parity();
	const euler_angles::OrderRepetition repetition = Angles.Repetition();
	int i, j, k;
	Angles.Axes(i, j, k);

	if(frame == euler_angles::RotatingFrame)
		std::swap(Angles.n[0], Angles.n[2]);

	if(parity == euler_angles::OddParity)
	{
		Angles.n[0] = -Angles.n[0];
		Angles.n[1] = -Angles.n[1];
		Angles.n[2] = -Angles.n[2];
	}

	const double ti = Angles.n[0], tj = Angles.n[1], th = Angles.n[2];
	const double ci = cos(ti), cj = cos(tj), ch = cos(th);
	const double si = sin(ti), sj = sin(tj), sh = sin(th);
	const double cc = ci*ch, cs = ci*sh;
	const double sc = si*ch, ss = si*sh;

	matrix3 m;
	if(repetition == euler_angles::Repeats)
	{
		m[i][i] =  cj;		m[i][j] =  sj*si;	m[i][k] =  sj*ci;
		m[j][i] =  sj*sh;	m[j][j] = -cj*ss+cc;	m[j][k] = -cj*cs-sc;
		m[k][i] = -sj*ch;	m[k][j] =  cj*sc+cs;	m[k][k] =  cj*cc-ss;
	}
	else
	{
		m[i][i] =  cj*ch;	m[i][j] = sj*sc-cs;	m[i][k] = sj*cc+ss;
		m[j][i] =  cj*sh;	m[j][j] = sj*ss+cc;	m[j][k] = sj*cs-sc;
		m[k][i] = -sj;		m[k][j] = cj*si;	m[k][k] = cj*ci;
	}

	*this = rotation3D(m);
}

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

inline matrix4& matrix4::operator = (const double d[4][4])
{ v[0] = d[0]; v[1] = d[1]; v[2] = d[2]; v[3] = d[3]; return *this; }

inline matrix4& matrix4::operator = (const double d[16])
{ memcpy(&v[0][0], &d[0], 16 * sizeof(double)); return *this; }

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

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

inline matrix4& matrix4::operator *= ( const double d )
{ v[0] *= d; v[1] *= d; v[2] *= d; v[3] *= d; return *this; }

inline matrix4& matrix4::operator /= ( const double d )
{ v[0] /= d; v[1] /= d; v[2] /= d; v[3] /= d; return *this; }

inline vector4& matrix4::operator [] ( int i) {
	assert(! (i < VX || i > VW));
	return v[i];
}

inline const vector4& matrix4::operator [] ( int i) const {
	assert(! (i < VX || i > VW));
	return v[i];
}

inline matrix4 matrix4::Transpose() const{
	return matrix4(vector4(v[0][0], v[1][0], v[2][0], v[3][0]),
		vector4(v[0][1], v[1][1], v[2][1], v[3][1]),
		vector4(v[0][2], v[1][2], v[2][2], v[3][2]),
		vector4(v[0][3], v[1][3], v[2][3], v[3][3]));
}

inline matrix4 matrix4::Inverse()	const // Gauss-Jordan elimination with partial pivoting
{
	matrix4 a(*this),	 // As a evolves from original mat into identity
	b(identity3D()); // b evolves from identity into Inverse(a)

	// Loop over cols of a from left to right, eliminating above and below diag
	for (int j=0; j<4; ++j) { // Find largest pivot in column j among rows j..3
		int i1 = j;		 // Row with largest pivot candidate

		for (int i=j+1; i<4; ++i) {
			if (fabs(a.v[i].n[j]) > fabs(a.v[i1].n[j]))
				i1 = i;
		}

		// Swap rows i1 and j in a and b to put pivot on diagonal
		std::swap(a.v[i1], a.v[j]);
		std::swap(b.v[i1], b.v[j]);

		// Scale row j to have a unit diagonal
		if(!a.v[j].n[j])
			{
				// Singular matrix - can't invert
				std::cerr << __PRETTY_FUNCTION__ << " : singular matrix, can't invert!" << std::endl;
				return b;
			}

		b.v[j] /= a.v[j].n[j];
		a.v[j] /= a.v[j].n[j];

		// Eliminate off-diagonal elems in col j of a, doing identical ops to b
		for (int i=0; i<4; ++i) {
			if (i!=j) {
				b.v[i] -= a.v[i].n[j]*b.v[j];
				a.v[i] -= a.v[i].n[j]*a.v[j];
			}
		}
	}
	return b;
}

inline void matrix4::CopyArray(float m[16]) const
{
	unsigned long index = 0;
	for(unsigned long i = 0; i < 4; ++i)
		for(unsigned long j = 0; j < 4; ++j)
			m[index++] = float(v[i][j]);
}

inline void matrix4::CopyArray(double m[16]) const
{
	unsigned long index = 0;
	for(unsigned long i = 0; i < 4; ++i)
		for(unsigned long j = 0; j < 4; ++j)
			m[index++] = v[i][j];
}

inline matrix4 operator - (const matrix4& a)
{ return matrix4(-a.v[0], -a.v[1], -a.v[2], -a.v[3]); }

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

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

inline matrix4 operator * (const matrix4& a, const matrix4& b)
{
#define ROWCOL(i, j) a.v[i].n[0]*b.v[0][j] + a.v[i].n[1]*b.v[1][j] + a.v[i].n[2]*b.v[2][j] + a.v[i].n[3]*b.v[3][j]
	return matrix4(vector4(ROWCOL(0,0), ROWCOL(0,1), ROWCOL(0,2), ROWCOL(0,3)),
									vector4(ROWCOL(1,0), ROWCOL(1,1), ROWCOL(1,2), ROWCOL(1,3)),
									vector4(ROWCOL(2,0), ROWCOL(2,1), ROWCOL(2,2), ROWCOL(2,3)),
									vector4(ROWCOL(3,0), ROWCOL(3,1), ROWCOL(3,2), ROWCOL(3,3)));
}

inline matrix4 operator * (const matrix4& a, const double d)
{ return matrix4(a.v[0] * d, a.v[1] * d, a.v[2] * d, a.v[3] * d); }

inline matrix4 operator * (const double d, const matrix4& a)
{ return a*d; }

inline matrix4 operator / (const matrix4& a, const double d)
{ return matrix4(a.v[0] / d, a.v[1] / d, a.v[2] / d, a.v[3] / d); }

inline bool operator == (const matrix4& a, const matrix4& b)
{ return ((a.v[0] == b.v[0]) && (a.v[1] == b.v[1]) && (a.v[2] == b.v[2]) && (a.v[3] == b.v[3])); }

inline bool operator != (const matrix4& a, const matrix4& b)
{ return !(a == b); }

inline std::ostream& operator<<(std::ostream& Stream, const matrix4& Arg)
{
	Stream << Arg[0] << " " << Arg[1] << " " << Arg[2] << " " << Arg[3];
	return Stream;
}

inline std::istream& operator>>(std::istream& Stream, matrix4& Arg)
{
	Stream >> Arg[0] >> Arg[1] >> Arg[2] >> Arg[3];
	return Stream;
}

/////////////////////////////////////////////////////////////////////////////
// quaternion implementation

inline quaternion::quaternion()
{ w = 1.0; v = vector3(0.0, 0.0, 0.0); }

inline quaternion::quaternion(const double u, const vector3& t)
{ w = u; v = t; }

inline quaternion::quaternion(const double u, const double x, const double y, const double z)
{ w = u; v = vector3(x, y, z); }

inline quaternion::quaternion(const angle_axis& AngleAxis)
{ w = cos(AngleAxis.angle * 0.5); vector3 axis = AngleAxis.axis; axis.Normalize(); v = sin(AngleAxis.angle * 0.5) * axis; }

inline quaternion::quaternion(euler_angles Angles)
{
	const euler_angles::OrderFrame frame = Angles.Frame();
	const euler_angles::OrderParity parity = Angles.Parity();
	const euler_angles::OrderRepetition repetition = Angles.Repetition();
	int i, j, k;
	Angles.Axes(i, j, k);

	if(frame == euler_angles::RotatingFrame)
		std::swap(Angles.n[0], Angles.n[2]);

	if(parity == euler_angles::OddParity)
		Angles.n[1] = -Angles.n[1];

	const double ti = Angles.n[0]*0.5, tj = Angles.n[1]*0.5, th = Angles.n[2]*0.5;
	const double ci = cos(ti), cj = cos(tj), ch = cos(th);
	const double si = sin(ti), sj = sin(tj), sh = sin(th);
	const double cc = ci*ch, cs = ci*sh;
	const double sc = si*ch, ss = si*sh;

	if(repetition == euler_angles::Repeats)
	{
		v[i] = cj*(cs + sc);
		v[j] = sj*(cc + ss);
		v[k] = sj*(cs - sc);
		w = cj*(cc - ss);
	}
	else
	{
		v[i] = cj*sc - sj*cs;
		v[j] = cj*ss + sj*cc;
		v[k] = cj*cs - sj*sc;
		w = cj*cc + sj*ss;
	}

	if(parity == euler_angles::OddParity)
		v[j] = -v[j];
}

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

inline quaternion& quaternion::operator = (const angle_axis& AngleAxis)
{ w = cos(AngleAxis.angle * 0.5); vector3 axis = AngleAxis.axis; axis.Normalize(); v = sin(AngleAxis.angle * 0.5) * axis; return *this; }

inline quaternion& quaternion::operator *= (const double f)
{ w *= f; v *= f; return *this; }

inline quaternion& quaternion::operator /= (const double f)
{ if(f != 0.0) { w /= f; v /= f; } return *this; }

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

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

inline quaternion& quaternion::operator *= (const quaternion& q)
{ w = w*q.w - (v*q.v); v = q.w*v + w*q.v + (v^q.v); return *this; }

inline quaternion& quaternion::operator /= (const quaternion& q)
{ quaternion t = q; (*this) *= t.Inverse(); return *this; }

inline double quaternion::Magnitude() const
{ return std::sqrt(w*w + v.Length2()); }

inline quaternion& quaternion::Normalize()
{
	if(const double magnitude = Magnitude())
		*this /= magnitude;

	return *this;
}

inline quaternion& quaternion::Conjugate()
{ v = -v; return *this; }

inline quaternion& quaternion::Inverse()
{ if(Magnitude() != 0.0) { const double i = 1.0 / Magnitude(); w *= i; v *= -i; } return *this; }

inline quaternion& quaternion::Square()
{ const double t = 2*w; w = w*w - v.Length2(); v *= t; return *this; }

inline quaternion& quaternion::AxisRotation(const double phi)
{ v.Normalize(); v *= sin(phi/2.0); w = cos(phi/2); return *this; }

inline quaternion operator - (const quaternion& q)
{ return quaternion(-q.w,-q.v); }

inline quaternion operator + (const quaternion& q, const quaternion& r)
{ return quaternion(q.w + r.w, q.v + r.v); }

inline quaternion operator - (const quaternion& q, const quaternion& r)
{ return quaternion(q.w - r.w, q.v - r.v); }

inline quaternion operator * (const quaternion& q, const quaternion& r)
{ return quaternion(q.w*r.w - (q.v*r.v), q.w*r.v + r.w*q.v + (q.v^r.v)); }

inline quaternion operator * (const quaternion& q, const double f)
{ return quaternion(q.w*f, q.v*f); }

inline quaternion operator * (const double f, const quaternion& q)
{ return quaternion(q.w*f, q.v*f); }

inline quaternion operator / (const quaternion& q, const quaternion& r)
{ quaternion t = r; return q*t.Inverse(); }

inline quaternion operator / (const quaternion& q, const double f)
{ if( f == 0.0 ) return quaternion(); else return quaternion(q.w/f, q.v/f); }

inline bool operator == (const quaternion& q, const quaternion& r)
{ return ((q.w == r.w) && (q.v == r.v)); }

inline bool operator != (const quaternion& q, const quaternion& r)
{ return !(q == r); }

inline std::ostream& operator<<(std::ostream& Stream, const quaternion& Arg)
{
	Stream << Arg.w << " " << Arg.v[0] << " " << Arg.v[1] << " " << Arg.v[2];
	return Stream;
}

inline std::istream& operator>>(std::istream& Stream, quaternion& Arg)
{
	Stream >> Arg.w >> Arg.v[0] >> Arg.v[1] >> Arg.v[2];
	return Stream;
}

/////////////////////////////////////////////////////////////////////////////
// angle_axis implementation ...

inline angle_axis::angle_axis()
{ angle = 0.0; axis = vector3(0.0, 0.0, 1.0); }

inline angle_axis::angle_axis(const double Angle, const vector3& Axis)
{ angle = Angle; axis = Axis; axis.Normalize(); }

inline angle_axis::angle_axis(const double Angle, const double X, const double Y, const double Z)
{ angle = Angle; axis[0] = X; axis[1] = Y; axis[2] = Z; axis.Normalize(); }

inline angle_axis::angle_axis(const double AngleAxis[4])
{ angle = AngleAxis[0]; axis[0] = AngleAxis[1]; axis[1] = AngleAxis[2]; axis[2] = AngleAxis[3]; axis.Normalize(); }

inline angle_axis::angle_axis(const quaternion& Quaternion)
{
	quaternion q = Quaternion;
	q.Normalize();
	const double halftheta = acos(q.w);
	const double sinhalftheta = sin(halftheta);

	angle = 2.0 * halftheta;
	if(sinhalftheta != 0.0)
		axis = q.v / sinhalftheta;
	else
		axis = vector3(0.0, 0.0, 1.0);
}

inline angle_axis::angle_axis(const angle_axis& AngleAxis)
{ angle = AngleAxis.angle; axis = AngleAxis.axis; }

inline angle_axis& angle_axis::operator = (const angle_axis& AngleAxis)
{ angle = AngleAxis.angle; axis = AngleAxis.axis; return *this; }

inline angle_axis& angle_axis::operator = (const quaternion& Quaternion)
{
	const double halftheta = acos(Quaternion.w);
	const double sinhalftheta = sin(halftheta);

	angle = 2.0 * halftheta;
	if(sinhalftheta != 0.0)
		axis = Quaternion.v / sinhalftheta;
	else
		axis = vector3(0.0, 0.0, 1.0);

	return *this;
}

inline angle_axis& angle_axis::operator = (const double AngleAxis[4])
{ angle = AngleAxis[0]; axis[0] = AngleAxis[1]; axis[1] = AngleAxis[2]; axis[2] = AngleAxis[3]; return *this; }

inline void angle_axis::CopyArray(float AngleAxis[4]) const
{	AngleAxis[0] = float(angle); AngleAxis[1] = float(axis[0]); AngleAxis[2] = float(axis[1]); AngleAxis[3] = float(axis[2]); }

inline void angle_axis::CopyArray(double AngleAxis[4]) const
{	AngleAxis[0] = angle; AngleAxis[1] = axis[0]; AngleAxis[2] = axis[1]; AngleAxis[3] = axis[2]; }

inline bool operator == (const angle_axis& a, const angle_axis& b)
{	return ((a.angle == b.angle) && (a.axis == b.axis)); }
inline bool operator != (const angle_axis& a, const angle_axis& b)
{	return !(a == b); }

inline std::ostream& operator<<(std::ostream& Stream, const angle_axis& Arg)
{
	Stream << Arg.angle << " " << Arg.axis[0] << " " << Arg.axis[1] << " " << Arg.axis[2];
	return Stream;
}

inline std::istream& operator>>(std::istream& Stream, angle_axis& Arg)
{
	Stream >> Arg.angle >> Arg.axis[0] >> Arg.axis[1] >> Arg.axis[2];
	return Stream;
}


/////////////////////////////////////////////////////////////////////////////
// 2D and 3D function implementations

inline matrix3 identity2D()
{
	return matrix3(vector3(1.0, 0.0, 0.0), vector3(0.0, 1.0, 0.0), vector3(0.0, 0.0, 1.0));
}

inline matrix3 translation2D(const vector2& v)
{
	return matrix3(vector3(1.0, 0.0, v[VX]), vector3(0.0, 1.0, v[VY]), vector3(0.0, 0.0, 1.0));
}

inline matrix3 rotation2D(const vector2& Center, const double Angle)
{
	const double c = cos(Angle);
	const double s = sin(Angle);

	return matrix3(vector3(c, -s, Center[VX] * (1.0-c) + Center[VY] * s),
		vector3(s, c, Center[VY] * (1.0-c) - Center[VX] * s),
		vector3(0.0, 0.0, 1.0));
}

inline matrix3 scaling2D(const vector2& scaleVector)
{
	return matrix3(vector3(scaleVector[VX], 0.0, 0.0), vector3(0.0, scaleVector[VY], 0.0), vector3(0.0, 0.0, 1.0));
}

inline matrix4 identity3D()
{
	return matrix4(vector4(1.0, 0.0, 0.0, 0.0),
		vector4(0.0, 1.0, 0.0, 0.0),
		vector4(0.0, 0.0, 1.0, 0.0),
		vector4(0.0, 0.0, 0.0, 1.0));
}

inline matrix4 translation3D(const vector3& v)
{
	return matrix4(vector4(1.0, 0.0, 0.0, v[VX]),
		vector4(0.0, 1.0, 0.0, v[VY]),
		vector4(0.0, 0.0, 1.0, v[VZ]),
		vector4(0.0, 0.0, 0.0, 1.0));
}

inline matrix4 shearing3D(const double XY, const double XZ, const double YX, const double YZ, const double ZX, const double ZY)
{
	return matrix4(
		vector4(1, XY, XZ, 0),
		vector4(YX, 1, YZ, 0),
		vector4(ZX, ZY, 1, 0),
		vector4(0, 0, 0, 1));
}

inline matrix4 rotation3D(const double Angle, vector3 Axis)
{
	const double c = cos(Angle), s = sin(Angle), t = 1.0 - c;

	Axis.Normalize();
	return matrix4(vector4(t * Axis[VX] * Axis[VX] + c, t * Axis[VX] * Axis[VY] - s * Axis[VZ], t * Axis[VX] * Axis[VZ] + s * Axis[VY], 0.0),
		vector4(t * Axis[VX] * Axis[VY] + s * Axis[VZ], t * Axis[VY] * Axis[VY] + c, t * Axis[VY] * Axis[VZ] - s * Axis[VX], 0.0),
		vector4(t * Axis[VX] * Axis[VZ] - s * Axis[VY], t * Axis[VY] * Axis[VZ] + s * Axis[VX], t * Axis[VZ] * Axis[VZ] + c, 0.0),
		vector4(0.0, 0.0, 0.0, 1.0));
}

inline matrix4 rotation3D(const angle_axis& AngleAxis)
{
	return rotation3D(AngleAxis.angle, AngleAxis.axis);
}

inline matrix4 rotation3D(const vector3& EulerAngles)
{
	matrix4 matrix = identity3D();
	matrix = matrix * rotation3D(EulerAngles[1], vector3(0.0, 1.0, 0.0));
	matrix = matrix * rotation3D(EulerAngles[0], vector3(1.0, 0.0, 0.0));
	matrix = matrix * rotation3D(EulerAngles[2], vector3(0.0, 0.0, 1.0));

	return matrix;
}

inline matrix4 rotation3D(const quaternion& Quaternion)
{
	return rotation3D(angle_axis(Quaternion));
}

inline quaternion rotation3D(const matrix4& m)
{
	double d0 = m[0][0];
	double d1 = m[1][1];
	double d2 = m[2][2];

	double trace = d0 + d1 + d2 + 1;
	if(trace > 0.0)
		{
			double s = 0.5 / std::sqrt(trace);
			return quaternion(0.25 / s, s * vector3(m[2][1] - m[1][2], m[0][2] - m[2][0], m[1][0] - m[0][1]));
		}

	// Identify the major diagonal element with greatest value
	if(d0 >= d1 && d0 >= d2)
		{
			double s = std::sqrt(1.0 + d0 - d1 - d2) * 2.0;
			return quaternion(m[1][2] - m[2][1], vector3(0.5, m[0][1] - m[1][0], m[0][2] - m[2][0])) / s;
		}
	else if(d1 >= d0 && d1 >= d2)
		{
			double s = std::sqrt(1.0 + d1 - d0 - d2) * 2.0;
			return quaternion(m[0][2] - m[2][0], vector3(m[0][1] - m[1][0], 0.5, m[1][2] - m[2][1])) / s;
		}
	else
		{
			double s = std::sqrt(1.0 + d2 - d0 - d1) * 2.0;
			return quaternion(m[0][1] - m[1][0], vector3(m[0][2] - m[2][0], m[1][2] - m[2][1], 0.5)) / s;
		}
}

inline matrix4 scaling3D(const vector3& scaleVector)
{
	return matrix4(vector4(scaleVector[VX], 0.0, 0.0, 0.0),
		vector4(0.0, scaleVector[VY], 0.0, 0.0),
		vector4(0.0, 0.0, scaleVector[VZ], 0.0),
		vector4(0.0, 0.0, 0.0, 1.0));
}

inline matrix4 perspective3D(const double d)
{
	return matrix4(vector4(1.0, 0.0, 0.0, 0.0),
		vector4(0.0, 1.0, 0.0, 0.0),
		vector4(0.0, 0.0, 1.0, 0.0),
		vector4(0.0, 0.0, 1.0/d, 0.0));
}

inline matrix4 rotation3D(const matrix3& m)
{
//	return(matrix4(vector4(m[0][0], m[0][1], m[0][2], 0.0),
//									vector4(m[1][0], m[1][1], m[1][2], 0.0),
//									vector4(m[2][0], m[2][1], m[2][2], 0.0),
//									vector4(0.0, 0.0, 0.0, 1.0)));

	// This slightly-more-verbose version eliminates a strange parse error in egcs ...
	vector4 a(m[0][0], m[0][1], m[0][2], 0.0);
	vector4 b(m[1][0], m[1][1], m[1][2], 0.0);
	vector4 c(m[2][0], m[2][1], m[2][2], 0.0);
	vector4 d(0.0, 0.0, 0.0, 1.0);

	return matrix4(a, b, c, d);
}

//
//	Matrix utilities for affine matrices.
//
//	The input matrix must be affine, but need not be orthogonal.
//	In other words, it may contain scaling, rotation, and translation,
//	but not perspective projection.
//

//	Ref. Foley and Van Dam, 2nd ed, p. 217.
/// Extracts the translation vector from a matrix
inline vector3 extractTranslation(const matrix4& m)
{
	return(vector3(m[0][3], m[1][3], m[2][3]));
}

//	Per Valerie Demers, Softimage.
//	Transposed for matrix4 matrices
/// Extracts the scaling vector from a matrix
inline vector3 extractScaling(const matrix4& m)
{
	return(vector3(sqrt(m[0][0] * m[0][0] + m[1][0] * m[1][0] + m[2][0] * m[2][0]),
		sqrt(m[0][1] * m[0][1] + m[1][1] * m[1][1] + m[2][1] * m[2][1]),
		sqrt(m[0][2] * m[0][2] + m[1][2] * m[1][2] + m[2][2] * m[2][2])));
}

/// Extracts a rotation matrix from a matrix
inline matrix3 extractRotation(const matrix4& m)
{
	// Get scaling ...
	vector3 scale(extractScaling(m));
	// Compute the inverse of scaling ...
	vector3 invscale(1.0 / scale[0], 1.0 / scale[1], 1.0 / scale[2]);
	// Apply Inverse of scaling as a transformation, to get unit scale ...
	matrix4 unscaled(m * scaling3D(invscale));

	// Piece-together the rotation matrix ...
	vector3 a(unscaled[0], VW);
	vector3 b(unscaled[1], VW);
	vector3 c(unscaled[2], VW);

	return matrix3(a, b, c);
}

//
//	Unit quaternion utilities.
//
//	Those special functions deal with rotations, thus require unit quaternions.
//

//	Euler/Matrix/Quaternion conversion functions are based on Ken Shoemake code,
//	from Graphic Gems IV

/// Returns the spherical linear interpolation between two quaternions
inline quaternion Slerp(const quaternion& q1, const quaternion& q2, double t)
{
	// Calculate the angle between the two quaternions ...
	double cosangle = q1.w * q2.w + q1.v * q2.v;

	if(cosangle < -1.0)
		cosangle = -1.0;
	if(cosangle > 1.0)
		cosangle = 1.0;

	// If the angle is reasonably large, do the spherical interpolation
	if(1.0 - cosangle > 16 * FLT_EPSILON)
		{
			const double angle = acos(cosangle);
			return (sin((1.0 - t)*angle)*q1 + sin(t*angle)*q2) / sin(angle);
		}

	// The angle is too close to zero - do a linear interpolation
	return k3d::mix(q1, q2, t);
}

/////////////////////////////////////////////////////////////////////////////
// euler_angles implementation

inline euler_angles::euler_angles()
{
	n[0] = n[1] = n[2] = 0.0;
	order = YXZstatic;
}

inline euler_angles::euler_angles(vector3 Angles, AngleOrder Order)
{
	n[0] = Angles[0];
	n[1] = Angles[1];
	n[2] = Angles[2];
	order = Order;
}

inline euler_angles::euler_angles(double X, double Y, double Z, AngleOrder Order)
{
	n[0] = X;
	n[1] = Y;
	n[2] = Z;
	order = Order;
}

inline euler_angles::euler_angles(const quaternion& Quaternion, AngleOrder Order)
{
	const double NQuaternion = Quaternion.Magnitude();
	const double s = (NQuaternion > 0.0) ? (2.0 / NQuaternion) : 0.0;

	const double xs = Quaternion.v[0]*s, ys = Quaternion.v[1]*s, zs = Quaternion.v[2]*s;
	const double wx = Quaternion.w*xs, wy = Quaternion.w*ys, wz = Quaternion.w*zs;
	const double xx = Quaternion.v[0]*xs, xy = Quaternion.v[0]*ys, xz = Quaternion.v[0]*zs;
	const double yy = Quaternion.v[1]*ys, yz = Quaternion.v[1]*zs, zz = Quaternion.v[2]*zs;

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

	matrix4 rotmatrix = rotation3D(matrix);
	*this = euler_angles(rotmatrix, Order);
}

inline euler_angles::euler_angles(const matrix4& Matrix, AngleOrder Order)
{
	OrderRepetition repetition = Repetition(Order);
	OrderParity parity = Parity(Order);
	OrderFrame frame = Frame(Order);
	int i, j, k;
	Axes(Order, i, j, k);


	if(repetition == euler_angles::Repeats )
	{
		const double sy = sqrt(Matrix[i][j]*Matrix[i][j] + Matrix[i][k]*Matrix[i][k]);
		if( sy > 16*FLT_EPSILON )
		{
			n[0] = atan2(Matrix[i][j], Matrix[i][k]);
			n[1] = atan2(sy, Matrix[i][i]);
			n[2] = atan2(Matrix[j][i], -Matrix[k][i]);
		}
		else
		{
			n[0] = atan2(-Matrix[j][k], Matrix[j][j]);
			n[1] = atan2(sy, Matrix[i][i]);
			n[2] = 0;
		}
	}
	else
	{
		const double cy = sqrt(Matrix[i][i]*Matrix[i][i] + Matrix[j][i]*Matrix[j][i]);
		if( cy > 16*FLT_EPSILON )
		{
			n[0] = atan2(Matrix[k][j], Matrix[k][k]);
			n[1] = atan2(-Matrix[k][i], cy);
			n[2] = atan2(Matrix[j][i], Matrix[i][i]);
		}
		else
		{
			n[0] = atan2(-Matrix[j][k], Matrix[j][j]);
			n[1] = atan2(-Matrix[k][i], cy);
			n[2] = 0;
		}
	}

	if(parity == euler_angles::OddParity )
	{
		n[0] = -n[0];
		n[1] = -n[1];
		n[2] = -n[2];
	}

	if(frame == euler_angles::RotatingFrame )
		std::swap(n[0], n[2]);

	order = Order;
}

inline void euler_angles::Axes(AngleOrder& Order, int& i, int& j, int& k)
{
	const int safe[4] = {0, 1, 2, 0};
	const int next[4] = {1, 2, 0, 1};

	i = safe[(Order >> 3) & 3];
	j = next[i + Parity(Order)];
	k = next[i + 1 - Parity(Order)];
}

inline double& euler_angles::operator [] ( int i)
{
	assert(! (i < 0 || i > 2));
	return n[i];
}

inline double euler_angles::operator [] ( int i) const
{
	assert(! (i < 0 || i > 2));
	return n[i];
}

inline std::ostream& operator<<(std::ostream& Stream, const euler_angles& Arg)
{
	Stream << Arg.n[0] << " " << Arg.n[1] << " " << Arg.n[2] << " " << int(Arg.order);
	return Stream;
}

inline std::istream& operator>>(std::istream& Stream, euler_angles& Arg)
{
	int order;
	Stream >> Arg.n[0] >> Arg.n[1] >> Arg.n[2] >> order;

	Arg.order = euler_angles::AngleOrder(order);

	return Stream;
}

} // namespace k3d

#endif // K3DSDK_ALGEBRA_H


