vmath.h

/* -*- C++ -*- */
/*
 * vmath, set of classes for computer graphics mathemtics.
 * Copyright (c) 2005-2009, Jan Bartipan < barzto at gmail dot com >
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without 
 * modification, are permitted provided that the following conditions 
 * are met:
 *
 * - Redistributions of source code must retain the above copyright 
 *   notice, this list of conditions and the following disclaimer.
 * - Redistributions in binary form must reproduce the above copyright 
 *   notice, this list of conditions and the following disclaimer in 
 *   the documentation and/or other materials provided with the 
 *   distribution.
 * - Neither the names of its contributors may be used to endorse or 
 *   promote products derived from this software without specific 
 *   prior written permission.
 * 
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 
 * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
 * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 
 * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 
 * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
 * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 
 * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY
 * WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
 * POSSIBILITY OF SUCH DAMAGE.
 */

/**
 * @mainpage Vector mathemtics for computer graphics
 *
 * @section Features
 * <ul>
 *    <li> basic aritemetic operations - using operators </li>
 *    <li> basic linear algebra operations - such as transpose, dot product, etc. </li>
 *    <li> aliasis for vertex coordinates - it means:
 *    <pre>
 *  Vector3f v;
 *  // use vertex coordinates
 *  v.x = 1; v.y = 2; v.z = -1;
 *
 *  // use texture coordinates
 *  v.s = 0; v.t = 1; v.u = 0.5;
 *  // use color coordinates
 *  v.r = 1; v.g = 0.5; v.b = 0;
 *    </pre>
 *    </li>
 *    <li> conversion constructor and assign operators - so you can assign a value of Vector3&lt;T1&gt; type 
 *    to a variable of Vector3&lt;T2&gt; type for any convertable T1, T2 type pairs. In other words, you can do this:
 *    <pre>
 *
 *  Vector3f f3; Vector3d d3 = f3;
 *  ...
 *  f3 = d3;
 *    </pre>
 *    </li>
 * </ul>
 */



#ifndef __vmath_Header_File__
#define __vmath_Header_File__

#include <cmath>
#include <cstring>
#include <iostream>
#include <cassert>
#include "boost.h"


#ifdef VMATH_NAMESPACE
namespace VMATH_NAMESPACE
{
#endif

#ifndef M_PI
#define M_PI           3.14159265358979323846  /* pi */
#endif
   
#define DEG2RAD(x) ((x * M_PI) / 180.0)
	//#define EPSILON (4.37114e-07)
   
	const double epsilon = 4.37114e-05;
#define EPSILON epsilon
   
   
	/**
	 * Class for two dimensional vector.
	 */
	template <class T>
	class Vector2 
	{
	   	// Serialization ------------------------------------------
		friend class boost::serialization::access;
		template<class Archive>
		void serialize(Archive & ar, const unsigned int version )
		{
			// Add all class variables here:
			ar & x;
			ar & s;
			ar & y;
			ar & t;
		}
	   	// ---------------------------------------------------------
	   
	public:
		union 
		{
			/**
			 * First element of vector, alias for X-coordinate.
			 */
			T x;
		
			/**
			 * First element of vector, alias for S-coordinate.
			 * For textures notation.
			 */
			T s; 
		};
	
		union 
		{
			/**
			 * Second element of vector, alias for Y-coordinate.
			 */
			T y; 
		
			/**
			 * Second element of vector, alias for T-coordinate.
			 * For textures notation.
			 */
			T t; 
		};

		//----------------[ constructors ]--------------------------
		/**
		 * Creates and sets to (0,0)
		 */
		Vector2() : x(0),y(0)
		{	}
	
	

		/**
		 * Creates and sets to (x,y)
		 * @param nx intial x-coordinate value
		 * @param ny intial y-coordinate value
		 */
		Vector2(T nx, T ny) : x(nx), y(ny)
		{	}


		/**
		 * Copy constructor.
		 * @param src Source of data for new created instace.
		 */
		Vector2(const Vector2<T>& src)
			: x(src.x), y(src.y) 
		{	 }


		/**
		 * Copy casting constructor.
		 * @param src Source of data for new created instace.
		 */
		template <class FromT>
		Vector2(const Vector2<FromT>& src)
			: x(static_cast<T>(src.x)),
			  y(static_cast<T>(src.y))
		{	 }
	
	
		//----------------[ access operators ]-------------------
		/**
		 * Copy casting operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		template <class FromT>
		Vector2<T>& operator=(const Vector2<FromT>& rhs)
		{
			x = static_cast<T>(rhs.x);
			y = static_cast<T>(rhs.y);
			return * this;
		}

		/**
		 * Copy operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector2<T>& operator=(const Vector2<T>& rhs)
		{
			x = rhs.x;
			y = rhs.y;
			return * this;
		}


		/**
		 * Array access operator
		 * @param n Array index
		 * @return For n = 0, reference to x coordinate, else reference to y 
		 * y coordinate.
		 */
		T& operator[](int n)
		{
			assert(n >= 0 && n <= 1);
			if (0 == n) return x;
			else
				return y;
		}
	
	
		//---------------[ vector aritmetic operator ]--------------
		/**
		 * Addition operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector2<T> operator+(const Vector2<T>& rhs) const
		{
			return Vector2<T> (x + rhs.x, y + rhs.y);
		}

		/**
		 * Substraction operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector2<T> operator-(const Vector2<T>& rhs) const
		{
			return Vector2<T> (x - rhs.x, y - rhs.y);
		}

		/**
		 * Multiplication operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector2<T> operator*(const Vector2<T>& rhs) const
		{
			return Vector2<T> (x * rhs.x, y * rhs.y);
		}

		/**
		 * Division operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector2<T> operator/(const Vector2<T>& rhs) const 
		{
			return Vector2<T> (x / rhs.x, y / rhs.y);
		}

		/**
		 * Addition operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector2<T>& operator+=(const Vector2<T>& rhs)
		{
			x += rhs.x;
			y += rhs.y;
			return * this;
		}

		/**
		 * Substraction operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector2<T>& operator-=(const Vector2<T>& rhs)
		{
			x -= rhs.x;
			y -= rhs.y;
			return * this;
		}
	
		/**
		 * Multiplication operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector2<T>& operator*=(const Vector2<T>& rhs)
		{
			x *= rhs.x;
			y *= rhs.y;
			return * this;
		}
	
		/**
		 * Division operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector2<T>& operator/=(const Vector2<T>& rhs)
		{
			x /= rhs.x;
			y /= rhs.y;
			return * this;
		}

		//--------------[ scalar vector operator ]--------------------
		/**
		 * Addition operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector2<T> operator+(T rhs) const
		{
			return Vector2<T> (x + rhs, y + rhs);
		}
	
		/**
		 * Substraction operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector2<T> operator-(T rhs) const
		{
			return Vector2<T> (x - rhs, y - rhs);
		}
	
		/**
		 * Multiplication operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector2<T> operator*(T rhs) const
		{
			return Vector2<T> (x * rhs, y * rhs);
		}
	
		/**
		 * Division operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector2<T> operator/(T rhs) const
		{
			return Vector2<T> (x / rhs, y / rhs);
		}

		/**
		 * Addition operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector2<T>& operator+=(T rhs)
		{
			x += rhs;
			y += rhs;
			return * this;
		}

		/**
		 * Substraction operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector2<T>& operator-=(T rhs) 
		{
			x -= rhs;
			y -= rhs;
			return * this;
		}

		/**
		 * Multiplication operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector2<T>& operator*=(T rhs)
		{
			x *= rhs;
			y *= rhs;
			return * this;
		}

		/**
		 * Division operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector2<T>& operator/=(T rhs) 
		{
			x /= rhs;
			y /= rhs;
			return * this;
		}

		//--------------[ equality operator ]------------------------
		/**
		 * Equality test operator
		 * @param rhs Right hand side argument of binary operator.
		 * @note Test of equality is based of threshold EPSILON value. To be two
		 * values equal, must satisfy this condition | lws.x - rhs.y | < EPSILON,
		 * same for y-coordinate.
		 */
		bool operator==(const Vector2<T>& rhs) const 
		{
			return (std::abs(x - rhs.x) < EPSILON) && (std::abs(y - rhs.y) < EPSILON);
		}

	
		/**
		 * Inequality test operator
		 * @param rhs Right hand side argument of binary operator.
		 * @return not (lhs == rhs) :-P
		 */
		bool operator!=(const Vector2<T>& rhs) const { return ! (*this == rhs); }

		//-------------[ unary operations ]--------------------------
		/**
		 * Unary negate operator
		 * @return negated vector
		 */
		Vector2<T> operator-() const
		{
			return Vector2<T>(-x, -y);
		}

		//-------------[ size operations ]---------------------------
		/**
		 * Get lenght of vector.
		 * @return lenght of vector
		 */
		T length() const
		{
			return (T)std::sqrt(x * x + y * y);
		}

		/**
		 * Normalize vector
		 */
		void normalize()
		{
			T s = length();
			x /= s;
			y /= s;
		}

		/**
		 * Return square of length.
		 * @return lenght ^ 2
		 * @note This method is faster then length(). For comparsion
		 * of length of two vector can be used just this value, instead
		 * of computionaly more expensive length() method.
		 */
		T lengthSq() const
		{
			return x * x + y * y;
		}
	
		//--------------[ misc. operations ]-----------------------
		/**
		 * Linear interpolation of two vectors
		 * @param fact Factor of interpolation. For translation from positon
		 * of this vector to vector r, values of factor goes from 0.0 to 1.0.
		 * @param r Second Vector for interpolation 
		 * @note Hovewer values of fact parameter are reasonable only in interval
		 * [0.0 , 1.0], you can pass also values outside of this interval and you
		 * can get result (extrapolation?)
		 */
		Vector2<T> lerp(T fact, const Vector2<T>& r) const
		{
			return (*this) + (r - (*this)) * fact;	
		}
	
		//added functions
		float get_angle()
		{
			if(x==0 && y==0) return 0;
			return atan2(y, x);
		}
	
		//rotate this angle by a counterclockwise
		void rotate(float a)
		{
			float c= cos(a);
			float s= sin(a);
			float x2= x*c - y*s;
			y= x*s + y*c;
			x= x2;
		}

		//return angle this angle makes with the other one
		float angle_between(const Vector2<T>& other){
			float cross= x*other.y - y*other.x;
			float dot= x*other.x + y*other.y;
			return atan2(cross,dot);
		}
    
		//-------------[ conversion ]-----------------------------
		/**
		 * Conversion to pointer operator
		 * @return Pointer to internaly stored (in managment of class Vector2<T>)
		 * used for passing Vector2<T> values to gl*2[fd] functions.
		 */
		operator T*(){ return (T*) this; }
		/**
		 * Conversion to pointer operator
		 * @return Constant Pointer to internaly stored (in managment of class Vector2<T>)
		 * used for passing Vector2<T> values to gl*2[fd] functions.
		 */
		operator const T*() const { return (const T*) this; }

		//-------------[ output operator ]------------------------
		/**
		 * Output to stream operator
		 * @param lhs Left hand side argument of operator (commonly ostream instance). 
		 * @param rhs Right hand side argument of operator.
		 * @return Left hand side argument - the ostream object passed to operator.
		 */
		friend std::ostream& operator<<(std::ostream& lhs, const Vector2<T>& rhs) 
		{
			lhs << "[" << rhs.x << "," << rhs.y << "]";
			return lhs;
		}


	};


	//--------------------------------------
	// Typedef shortcuts for 2D vector
	//-------------------------------------
	/// Two dimensional Vector of floats
	typedef class Vector2 <float> Vector2f;
	/// Two dimensional Vector of doubles
	typedef class Vector2 <double> Vector2d;


	/**
	 * Class for three dimensional vector.
	 */
	template <class T>
	class Vector3 
	{
	public:
		//T x, y, z;
		union
		{
			/**
			 * First element of vector, alias for X-coordinate.
			 */
			T x;
		
			/**
			 * First element of vector, alias for S-coordinate.
			 * For textures notation.
			 */
			T s;
		
			/**
			 * First element of vector, alias for R-coordinate.
			 * For color notation.
			 */
			T r; 
		};
	
		union
		{
			/**
			 * Second element of vector, alias for Y-coordinate.
			 */
			T y; 
			/**
			 * Second element of vector, alias for T-coordinate.
			 * For textures notation.
			 */
			T t; 
			/**
			 * Second element of vector, alias for G-coordinate.
			 * For color notation.
			 */
			T g; 
		};
	
		union
		{
			/**
			 * Third element of vector, alias for Z-coordinate.
			 */
			T z;
		
			/**
			 * Third element of vector, alias for U-coordinate.
			 * For textures notation.
			 */
			T u; 
			/**
			 * Third element of vector, alias for B-coordinate.
			 * For color notation.
			 */
			T b; 
		};
	
		//----------------[ constructors ]--------------------------
		/**
		 * Creates and sets to (0,0,0)
		 */
		Vector3() : x(0),y(0),z(0)
		{	}

		/**
		 * Creates and sets to (x,y,z)
		 * @param nx intial x-coordinate value
		 * @param ny intial y-coordinate value
		 * @param nz intial z-coordinate value
		 */
		Vector3(T nx, T ny, T nz) : x(nx),y(ny),z(nz)
		{	}

		/**
		 * Copy constructor.
		 * @param src Source of data for new created Vector3 instance.
		 */
		Vector3(const Vector3<T>& src)
			: x(src.x), y(src.y), z(src.z)
		{}
	 
		/**
		 * Copy casting constructor.
		 * @param src Source of data for new created Vector3 instance.
		 */
		template <class FromT>
		Vector3(const Vector3<FromT>& src)
			: x(static_cast<T>(src.x)),
			  y(static_cast<T>(src.y)),
			  z(static_cast<T>(src.z))
		{}
	
	
		//----------------[ access operators ]-------------------
		/**
		 * Copy operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector3<T> operator=(const Vector3<T>& rhs) 
		{
			x = rhs.x;
			y = rhs.y;
			z = rhs.z;
			return * this;
		}

		/**
		 * Copy casting operator.
		 * @param rhs Right hand side argument of binary operator.
		 */
		template <class FromT>
		Vector3<T> operator=(const Vector3<FromT>& rhs)
		{
			x = static_cast<T>(rhs.x);
			y = static_cast<T>(rhs.y);
			z = static_cast<T>(rhs.z);
			return * this;
		}

		/**
		 * Array access operator
		 * @param n Array index
		 * @return For n = 0, reference to x coordinate, n = 1
		 * reference to y, else reference to z 
		 * y coordinate.
		 */
		T & operator[](int n)
		{
			assert(n >= 0 && n <= 2);
			if (0 == n) return x;
			else if (1 == n) return y;
			else
				return z;
		}


		//---------------[ vector aritmetic operator ]--------------
		/**
		 * Addition operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector3<T> operator+(const Vector3<T>& rhs) const 
		{
			return Vector3<T> (x + rhs.x, y + rhs.y, z + rhs.z);
		}
	
		/**
		 * Substraction operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector3<T> operator-(const Vector3<T>& rhs) const 
		{
			return Vector3<T> (x - rhs.x, y - rhs.y, z - rhs.z);
		}
	
		/**
		 * Multiplication operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector3<T> operator*(const Vector3<T>& rhs) const 
		{
			return Vector3<T> (x * rhs.x, y * rhs.y, z * rhs.z);
		}

		/**
		 * Division operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector3<T> operator/(const Vector3<T>& rhs) const 
		{
			return Vector3<T> (x / rhs.x, y / rhs.y, z / rhs.z);
		}

		/**
		 * Addition operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector3<T>& operator+=(const Vector3<T>& rhs) 
		{
			x += rhs.x;
			y += rhs.y;
			z += rhs.z;
			return * this;
		}

		/**
		 * Substraction operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector3<T>& operator-=(const Vector3<T>& rhs) 
		{
			x -= rhs.x;
			y -= rhs.y;
			z -= rhs.z;
			return * this;
		}

		/**
		 * Multiplication operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector3<T>& operator*=(const Vector3<T>& rhs) 
		{
			x *= rhs.x;
			y *= rhs.y;
			z *= rhs.z;
			return * this;
		}

		/**
		 * Division operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector3<T>& operator/=(const Vector3<T>& rhs) 
		{
			x /= rhs.x;
			y /= rhs.y;
			z /= rhs.z;
			return * this;
		}

		/**
		 * Dot product of two vectors.
		 * @param rhs Right hand side argument of binary operator.
		 */
		T dotProduct(const Vector3<T>& rhs) const 
		{
			return x * rhs.x + y * rhs.y + z * rhs.z;
		}

		/**
		 * Cross product opertor
		 * @param rhs Right hand side argument of binary operator.
		 */	
		Vector3<T> crossProduct(const Vector3<T>& rhs) const 
		{
			return Vector3<T> (y * rhs.z - rhs.y * z, z * rhs.x - rhs.z * x, x * rhs.y - rhs.x * y);
		}
	

		//--------------[ scalar vector operator ]--------------------
		/**
		 * Addition operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector3<T> operator+(T rhs) const 
		{
			return Vector3<T> (x + rhs, y + rhs, z + rhs);
		}

		/**
		 * Substraction operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector3<T> operator-(T rhs) const 
		{
			return Vector3<T> (x - rhs, y - rhs, z - rhs);
		}

		/**
		 * Multiplication operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector3<T> operator*(T rhs) const 
		{
			return Vector3<T> (x * rhs, y * rhs, z * rhs);
		}
	
		/**
		 * Division operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector3<T> operator/(T rhs) const 
		{
			return Vector3<T> (x / rhs, y / rhs, z / rhs);
		}
	
		/**
		 * Addition operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector3<T>& operator+=(T rhs) 
		{
			x += rhs;
			y += rhs;
			z += rhs;
			return * this;
		}

		/**
		 * Substraction operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector3<T>& operator-=(T rhs) 
		{
			x -= rhs;
			y -= rhs;
			z -= rhs;
			return * this;
		}

		/**
		 * Multiplication operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector3<T>& operator*=(T rhs) 
		{
			x *= rhs;
			y *= rhs;
			z *= rhs;
			return * this;
		}

		/**
		 * Division operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector3<T>& operator/=(T rhs) 
		{
			x /= rhs;
			y /= rhs;
			z /= rhs;
			return * this;
		}

		//--------------[ equiality operator ]------------------------
		/**
		 * Equality test operator
		 * @param rhs Right hand side argument of binary operator.
		 * @note Test of equality is based of threshold EPSILON value. To be two
		 * values equal, must satisfy this condition | lws.x - rhs.y | < EPSILON,
		 * same for y-coordinate, and z-coordinate.
		 */
		bool operator==(const Vector3<T>& rhs) const
		{
			return std::fabs(x - rhs.x) < EPSILON 
										  && std::fabs(y - rhs.y) < EPSILON 
																	&& std::fabs(z - rhs.z) < EPSILON;
		}

		/**
		 * Inequality test operator
		 * @param rhs Right hand side argument of binary operator.
		 * @return not (lhs == rhs) :-P
		 */
		bool operator!=(const Vector3<T>& rhs) const { return !(*this == rhs); }

		//-------------[ unary operations ]--------------------------
		/**
		 * Unary negate operator
		 * @return negated vector
		 */
		Vector3<T> operator-() const
		{
			return Vector3<T>(-x, -y, -z);
		}

		//-------------[ size operations ]---------------------------
		/**
		 * Get lenght of vector.
		 * @return lenght of vector
		 */
		T length() const 
		{
			return (T)std::sqrt(x * x + y * y + z * z);
		}
	
		/**
		 * Return square of length.
		 * @return lenght ^ 2
		 * @note This method is faster then length(). For comparsion
		 * of length of two vector can be used just this value, instead
		 * of computionaly more expensive length() method.
		 */
		T lengthSq() const 
		{
			return x * x + y * y + z * z;
		}
	
		/**
		 * Normalize vector
		 */
		void normalize() 
		{
			T s = length();
			x /= s;
			y /= s;
			z /= s;
		}

		//------------[ other operations ]---------------------------
		/**
		 * Rotate vector around three axis.
		 * @param ax Angle (in degrees) to be rotated around X-axis.
		 * @param ay Angle (in degrees) to be rotated around Y-axis.
		 * @param az Angle (in degrees) to be rotated around Z-axis.
		 */
		void rotate(T ax, T ay, T az) 
		{
			T a = cos(DEG2RAD(ax));
			T b = sin(DEG2RAD(ax));
			T c = cos(DEG2RAD(ay));
			T d = sin(DEG2RAD(ay));
			T e = cos(DEG2RAD(az));
			T f = sin(DEG2RAD(az));
			T nx = c * e * x - c * f * y + d * z;
			T ny = (a * f + b * d * e) * x + (a * e - b * d * f) * y - b * c * z;
			T nz = (b * f - a * d * e) * x + (a * d * f + b * e) * y + a * c * z;
			x = nx; y = ny; z = nz;
		
		
		}
	
		/**
		 * Linear interpolation of two vectors
		 * @param fact Factor of interpolation. For translation from positon
		 * of this vector to vector r, values of factor goes from 0.0 to 1.0.
		 * @param r Second Vector for interpolation 
		 * @note Hovewer values of fact parameter are reasonable only in interval
		 * [0.0 , 1.0], you can pass also values outside of this interval and you
		 * can get result (extrapolation?)
		 */
		Vector3<T> lerp(T fact, const Vector3<T>& r) const
		{
			return (*this) + (r - (*this)) * fact;	
		}
	
    



		//-------------[ conversion ]-----------------------------
	
		/**
		 * Conversion to pointer operator
		 * @return Pointer to internaly stored (in managment of class Vector3<T>)
		 * used for passing Vector3<T> values to gl*3[fd] functions.
		 */
		operator T*(){ return (T*) this; }

		/**
		 * Conversion to pointer operator
		 * @return Constant Pointer to internaly stored (in managment of class Vector3<T>)
		 * used for passing Vector3<T> values to gl*3[fd] functions.
		 */	
		operator const T*() const { return (const T*) this; }

		//-------------[ output operator ]------------------------
		/**
		 * Output to stream operator
		 * @param lhs Left hand side argument of operator (commonly ostream instance). 
		 * @param rhs Right hand side argument of operator.
		 * @return Left hand side argument - the ostream object passed to operator.
		 */
		friend std::ostream& operator<<(std::ostream& lhs, const Vector3<T> rhs) 
		{
			lhs << "[" << rhs.x << "," << rhs.y << "," << rhs.z  << "]";
			return lhs;
		}
	
	};


	/// Three dimensional Vector of floats
	typedef Vector3 <float> Vector3f;
	/// Three dimensional Vector of doubles
	typedef Vector3 <double> Vector3d;

	/**
	 * Class for four dimensional vector.
	 */
	template <class T>
	class Vector4{
	public:

		union 
		{ 
			/**
			 * First element of vector, alias for R-coordinate.
			 * For color notation.
			 */
			T r
			/**
			 * First element of vector, alias for X-coordinate.
			 */; 
			T x; 
		};
	
		union 
		{ 
			/**
			 * Second element of vector, alias for G-coordinate.
			 * For color notation.
			 */
			T g; 
			/**
			 * Second element of vector, alias for Y-coordinate.
			 */
			T y; 
		};
	
		union 
		{
			/**
			 * Third element of vector, alias for B-coordinate.
			 * For color notation.
			 */
			T b; 
			/**
			 * Third element of vector, alias for Z-coordinate.
			 */
			T z; 
		};
	
		union 
		{
			/**
			 * Fourth element of vector, alias for A-coordinate.
			 * For color notation. This represnt aplha chanell
			 */
			T a; 
			/**
			 * First element of vector, alias for W-coordinate.
			 * @note For vectors (such as normals) should be set to 0.0
			 * For vertices should be set to 1.0
			 */
			T w; 
		};
	
		//----------------[ constructors ]--------------------------
		/**
		 * Creates and sets to (0,0,0,0)
		 */
		Vector4() : x(0),y(0),z(0),w(0)
		{ }


		/**
		 * Creates and sets to (x,y,z,z)
		 * @param nx intial x-coordinate value (R)
		 * @param ny intial y-coordinate value (G)
		 * @param nz intial z-coordinate value (B)
		 * @param nw intial w-coordinate value (Aplha)
		 */
		Vector4(T nx, T ny, T nz, T nw) : x(nx), y(ny), z(nz), w(nw)
		{ }

		/**
		 * Copy constructor.
		 * @param src Source of data for new created Vector4 instance.
		 */
		Vector4(const Vector4<T>& src)
			: x(src.x), y(src.y), z(src.z), w(src.w)
		{}

		/**
		 * Copy casting constructor.
		 * @param src Source of data for new created Vector4 instance.
		 */
		template <class FromT>
		Vector4(const Vector4<FromT>& src)
			: x(static_cast<T>(src.x)),
			  y(static_cast<T>(src.y)),
			  z(static_cast<T>(src.z)),
			  w(static_cast<T>(src.w))
		{}


		//----------------[ access operators ]-------------------
		/**
		 * Copy operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector4<T> operator=(const Vector4<T>& rhs) 
		{
			x = rhs.x;
			y = rhs.y;
			z = rhs.z;
			w = rhs.w;
			return * this;
		}

		/**
		 * Copy casting operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		template<class FromT>
		Vector4<T> operator=(const Vector4<FromT>& rhs) 
		{
			x = static_cast<T>(rhs.x);
			y = static_cast<T>(rhs.y);
			z = static_cast<T>(rhs.z);
			w = static_cast<T>(rhs.w);
			return * this;
		}


		/**
		 * Array access operator
		 * @param n Array index
		 * @return For n = 0, reference to x coordinate, n = 1
		 * reference to y coordinate, n = 2 reference to z,  
		 * else reference to w coordinate.
		 */
		T & operator[](int n) 
		{
			assert(n >= 0 && n <= 3);
			if (0 == n) return x;
			else if (1 == n) return y;
			else if (2 == n) return z;
			else return w;
		}


		//---------------[ vector aritmetic operator ]--------------
		/**
		 * Addition operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector4<T> operator+(const Vector4<T>& rhs) const 
		{
			return Vector4<T> (x + rhs.x, y + rhs.y, z + rhs.z, w + rhs.w);
		}

		/**
		 * Substraction operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector4<T> operator-(const Vector4<T>& rhs) const 
		{
			return Vector4<T> (x - rhs.x, y - rhs.y, z - rhs.z, w - rhs.w);
		}

		/**
		 * Multiplication operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector4<T> operator*(const Vector4<T> rhs) const 
		{
			return Vector4<T> (x * rhs.x, y * rhs.y, z * rhs.z, w * rhs.w);
		}

		/**
		 * Division operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector4<T> operator/(const Vector4<T>& rhs) const 
		{
			return Vector4<T> (x / rhs.x, y / rhs.y, z / rhs.z, w / rhs.w);
		}

		/**
		 * Addition operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector4<T>& operator+=(const Vector4<T>& rhs) 
		{
			x += rhs.x;
			y += rhs.y;
			z += rhs.z;
			w += rhs.w;
			return * this;
		}

		/**
		 * Substraction operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector4<T>& operator-=(const Vector4<T>& rhs) 
		{
			x -= rhs.x;
			y -= rhs.y;
			z -= rhs.z;
			w -= rhs.w;
			return * this;
		}

		/**
		 * Multiplication operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector4<T>& operator*=(const Vector4<T>& rhs) 
		{
			x *= rhs.x;
			y *= rhs.y;
			z *= rhs.z;
			w *= rhs.w;
			return * this;
		}

		/**
		 * Division operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector4<T>& operator/=(const Vector4<T>& rhs) 
		{
			x /= rhs.x;
			y /= rhs.y;
			z /= rhs.z;
			w /= rhs.w;
			return * this;
		}

		//--------------[ equiality operator ]------------------------
		/**
		 * Equality test operator
		 * @param rhs Right hand side argument of binary operator.
		 * @note Test of equality is based of threshold EPSILON value. To be two
		 * values equal, must satisfy this condition | lws.x - rhs.y | < EPSILON,
		 * same for y-coordinate, z-coordinate, and w-coordinate.
		 */
		bool operator==(const Vector4<T>& rhs) const 
		{
			return std::fabs(x - rhs.x) < EPSILON 
										  && std::fabs(y - rhs.y) < EPSILON 
																	&& std::fabs(z - rhs.z) < EPSILON 
																							  && std::fabs(w - rhs.w) < EPSILON;
		}

	
		/**
		 * Inequality test operator
		 * @param rhs Right hand side argument of binary operator.
		 * @return not (lhs == rhs) :-P
		 */
		bool operator!=(const Vector4<T>& rhs) const { return ! (*this == rhs); }

		//-------------[ unary operations ]--------------------------
		/**
		 * Unary negate operator
		 * @return negated vector
		 */
		Vector4<T> operator-() const
		{
			return Vector4<T>(-x, -y, -z, -w);
		}

		//--------------[ scalar vector operator ]--------------------
	
		/**
		 * Addition operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector4<T> operator+(T rhs) const 
		{
			return Vector4<T> (x + rhs, y + rhs, z + rhs, w + rhs);
		}

		/**
		 * Substraction operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector4<T> operator-(T rhs) const 
		{
			return Vector4<T> (x - rhs, y - rhs, z - rhs, w - rhs);
		}

		/**
		 * Multiplication operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector4<T> operator*(T rhs) const 
		{
			return Vector4<T> (x * rhs, y * rhs, z * rhs, w * rhs);
		}

		/**
		 * Division operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector4<T> operator/(T rhs) const 
		{
			return Vector4<T> (x / rhs, y / rhs, z / rhs, w / rhs);
		}

		/**
		 * Addition operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector4<T>& operator+=(T rhs) 
		{
			x += rhs;
			y += rhs;
			z += rhs;
			w += rhs;
			return * this;
		}

		/**
		 * Substraction operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector4<T>& operator-=(T rhs) 
		{
			x -= rhs;
			y -= rhs;
			z -= rhs;
			w -= rhs;
			return * this;
		}

		/**
		 * Multiplication operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector4<T>& operator*=(T rhs) 
		{
			x *= rhs;
			y *= rhs;
			z *= rhs;
			w *= rhs;
			return * this;
		}

		/**
		 * Division operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector4<T>& operator/=(T rhs) 
		{
			x /= rhs;
			y /= rhs;
			z /= rhs;
			w /= rhs;
			return * this;
		}

		//-------------[ size operations ]---------------------------
		/**
		 * Get lenght of vector.
		 * @return lenght of vector
		 */
		T length() const
		{
			return (T)std::sqrt(x * x + y * y + z * z + w * w);
		}

		/**
		 * Normalize vector
		 */
		void normalize() 
		{
			T s = length();
			x /= s;
			y /= s;
			z /= s;
			w /= s;
		}
	
		/**
		 * Return square of length.
		 * @return lenght ^ 2
		 * @note This method is faster then length(). For comparsion
		 * of length of two vector can be used just this value, instead
		 * of computionaly more expensive length() method.
		 */
		T lengthSq() const 
		{
			return x * x + y * y + z * z + w * w;
		}
	
		//--------------[ misc. operations ]-----------------------
		/**
		 * Linear interpolation of two vectors
		 * @param fact Factor of interpolation. For translation from positon
		 * of this vector to vector r, values of factor goes from 0.0 to 1.0.
		 * @param r Second Vector for interpolation 
		 * @note Hovewer values of fact parameter are reasonable only in interval
		 * [0.0 , 1.0], you can pass also values outside of this interval and you
		 * can get result (extrapolation?)
		 */
		Vector4<T> lerp(T fact, const Vector4<T>& r) const
		{
			return (*this) + (r - (*this)) * fact;	
		}
	
    
	

		//-------------[ conversion ]-----------------------------
	
		/**
		 * Conversion to pointer operator
		 * @return Pointer to internaly stored (in managment of class Vector4<T>)
		 * used for passing Vector4<T> values to gl*4[fd] functions.
		 */
		operator T*(){ return (T*) this; }
	
	
		/**
		 * Conversion to pointer operator
		 * @return Constant Pointer to internaly stored (in managment of class Vector4<T>)
		 * used for passing Vector4<T> values to gl*4[fd] functions.
		 */
		operator const T*() const { return (const T*) this; }

		//-------------[ output operator ]------------------------
		/**
		 * Output to stream operator
		 * @param lhs Left hand side argument of operator (commonly ostream instance). 
		 * @param rhs Right hand side argument of operator.
		 * @return Left hand side argument - the ostream object passed to operator.
		 */
		friend std::ostream& operator<<(std::ostream& lhs, const Vector4<T>& rhs) 
		{
			lhs << "[" << rhs.x << "," << rhs.y << "," << rhs.z << "," << rhs.w << "]";
			return lhs;
		}
	};

	/// Three dimensional Vector of floats
	typedef Vector4<float> Vector4f;
	/// Three dimensional Vector of doubles
	typedef Vector4<double> Vector4d;





	/**
	 * Class for matrix 3x3. 
	 * @note Data stored in this matrix are in column major order.
	 */
	template <class T>
	class Matrix3 
	{
	public:
		/// Data stored in column major order
		T data[9];
	
		//--------------------------[ constructors ]-------------------------------
		/**
		 * Creates identity matrix
		 */
		Matrix3() 
		{
			for (int i = 0; i < 9; i++)
				data[i] = (i % 4) ? 0 : 1;
		}

		/**
		 * Copy matrix values from array (these data must be in column
		 * major order!)
		 */
		Matrix3(const T * dt)
		{
			std::memcpy(data, dt, sizeof(T) * 9); 
		}

		/**
		 * Copy constructor.
		 * @param src Data source for new created instance of Matrix3
		 */
		Matrix3(const Matrix3<T>& src)
		{
			std::memcpy(data, src.data, sizeof(T) * 9);
		}

		/**
		 * Copy casting constructor.
		 * @param src Data source for new created instance of Matrix3
		 */
		template<class FromT>
		Matrix3(const Matrix3<FromT>& src)
		{
			for (int i = 0; i < 9; i++)
			{
				data[i] = static_cast<T>(src.data[i]);
			}
		}
	
		/**
		 * Resets matrix to be identity matrix
		 */
		void identity()
		{
			for (int i = 0; i < 9; i++)
				data[i] = (i % 4) ? 0 : 1;
		}
	
	

		/**
		 * Creates rotation matrix by rotation around axis.
		 * @param a Angle (in radians) of rotation around axis X.
		 * @param b Angle (in radians) of rotation around axis Y.
		 * @param c Angle (in radians) of rotation around axis Z.
		 */
		static Matrix3<T> createRotationAroundAxis(T a, T b, T c)
		{
			Matrix3<T> ma, mb, mc;
			float ac = cos(a);
			float as = sin(a);
			float bc = cos(b);
			float bs = sin(b);
			float cc = cos(c);
			float cs = sin(c);

			ma.at(1,1) = ac;
			ma.at(2,1) = as;
			ma.at(1,2) = -as;
			ma.at(2,2) = ac;

			mb.at(0,0) = bc;
			mb.at(2,0) = -bs;
			mb.at(0,2) = bs;
			mb.at(2,2) = bc;

			mc.at(0,0) = cc;
			mc.at(1,0) = cs;
			mc.at(0,1) = -cs;
			mc.at(1,1) = cc;

			Matrix3<T> ret = ma * mb * mc;
			return ret;
		}

		/**
		 * Creates roation matrix from ODE Matrix.
		 */
		template <class It>
		static Matrix3<T> fromOde(const It* mat)
		{
			Matrix3<T> ret;
			for (int i = 0; i < 3; i++)
			{
				for (int j = 0; j < 3; j++)
				{
					ret.at(i,j) = static_cast<T>(mat[j * 4 + i]);
				}
			}
			return ret;
		}


		//---------------------[ equiality operators ]------------------------------
		/**
		 * Equality test operator
		 * @param rhs Right hand side argument of binary operator.
		 * @note Test of equality is based of threshold EPSILON value. To be two
		 * values equal, must satisfy this condition all elements of matrix 
		 * | lws[i] - rhs[i] | < EPSILON,
		 * same for y-coordinate, z-coordinate, and w-coordinate.
		 */	
		bool operator==(const Matrix3<T>& rhs) const
		{
			for (int i = 0; i < 9; i++)
			{
				if (std::fabs(data[i] - rhs.data[i]) >= EPSILON)
					return false;
			}
			return true;
		}
	
	
		/**
		 * Inequality test operator
		 * @param rhs Right hand side argument of binary operator.
		 * @return not (lhs == rhs) :-P
		 */
		bool operator!=(const Matrix3<T>& rhs) const
		{
			return !(*this == rhs);
		}
	

		//---------------------[ access operators ]---------------------------------
		/**
		 * Get reference to element at postion (x,y).
		 * @param x Number of column (0..2)
		 * @param y Number of row (0..2)
		 */
		T& at(int x, int y) 
		{
			assert(x >= 0 && x < 3);
			assert(y >= 0 && y < 3);
			return data[x * 3 + y];
		}

		/**
		 * Get constant reference to element at postion (x,y).
		 * @param x Number of column (0..2)
		 * @param y Number of row (0..2)
		 */
		const T& at(int x, int y) const 
		{
			assert(x >= 0 && x < 3);
			assert(y >= 0 && y < 3);
			return data[x * 3 + y];
		}

		/**
		 * Copy operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Matrix3<T>& operator=(const Matrix3<T>& rhs) 
		{
			std::memcpy(data, rhs.data, sizeof(T) * 9);
			return * this;
		}

		/**
		 * Copy casting operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		template<class FromT>
		Matrix3<T>& operator=(const Matrix3<FromT>& rhs) 
		{
			for (int i = 0; i < 9; i++)
			{
				data[i] = static_cast<T>(rhs.data[i]);
			}
			return * this;
		}

		/**
		 * Copy operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Matrix3<T>& operator=(const T* rhs) 
		{
			std::memcpy(data, rhs, sizeof(T) * 9);
			return * this;
		}


		/*Matrix3<T> & operator=(const double* m)
		  {
		  for (int i = 0; i < 9; i++) data[i] = (T)m[i];
		  return * this;
		  }*/
	
		//--------------------[ matrix with matrix operations ]---------------------
		/**
		 * Addition operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Matrix3<T> operator+(const Matrix3<T>& rhs) const
		{
			Matrix3<T> ret;
			for (int i = 0; i < 9; i++)
				ret.data[i] = data[i] + rhs.data[i];
			return ret;
		}
	
		/**
		 * Substraction operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Matrix3<T> operator-(const Matrix3<T>& rhs) const
		{
			Matrix3<T> ret;
			for (int i = 0; i < 9; i++)
				ret.data[i] = data[i] - rhs.data[i];
			return ret;
		}
	
		//--------------------[ matrix with scalar operations ]---------------------
		/**
		 * Addition operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Matrix3<T> operator+(T rhs) const
		{
			Matrix3<T> ret;
			for (int i = 0; i < 9; i++)
				ret.data[i] = data[i] + rhs;
			return ret;
		}
	
		/**
		 * Substraction operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Matrix3<T> operator-(T rhs) const
		{
			Matrix3<T> ret;
			for (int i = 0; i < 9; i++)
				ret.data[i] = data[i] - rhs;
			return ret;
		}
	
		/**
		 * Multiplication operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Matrix3<T> operator*(T rhs) const
		{
			Matrix3<T> ret;
			for (int i = 0; i < 9; i++)
				ret.data[i] = data[i] * rhs;
			return ret;
		}
	
		/**
		 * Division operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Matrix3<T> operator/(T rhs) const
		{
			Matrix3<T> ret;
			for (int i = 0; i < 9; i++)
				ret.data[i] = data[i] / rhs;
			return ret;
		}
	

		//--------------------[ multiply operators ]--------------------------------
		/**
		 * Multiplication operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector3<T> operator*(const Vector3<T>& rhs) const
		{
			return Vector3<T>(
							  data[0] * rhs.x + data[3] * rhs.y + data[6] * rhs.z,
							  data[1] * rhs.x + data[4] * rhs.y + data[7] * rhs.z,
							  data[2] * rhs.x + data[5] * rhs.y + data[8] * rhs.z
							  );
		}

		/**
		 * Multiplication operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Matrix3<T> operator*(Matrix3<T> rhs) const 
		{
			static Matrix3<T> w;
			for (int i = 0; i < 3; i++) 
			{
				for (int j = 0; j < 3; j++) 
				{
					T n = 0;
					for (int k = 0; k < 3; k++) n += rhs.at(i, k) * at(k, j);
					w.at(i, j) = n;
				}
			}
			return w;
		
		}
	
	
		//---------------------------[ misc operations ]----------------------------
		/**
		 * Transpose matrix.
		 */
		Matrix3<T> transpose()
		{
			Matrix3<T> ret;
			for (int i = 0; i < 3; i++)
			{
				for (int j = 0; j < 3; j++)
				{
					ret.at(i,j) = at(j,i);
				}
			}
			return ret;
		}
	
		/**
		 * Linear interpolation of two vectors
		 * @param fact Factor of interpolation. For translation from positon
		 * of this matrix (lhs) to matrix rhs, values of factor goes from 0.0 to 1.0.
		 * @param rhs Second Matrix for interpolation 
		 * @note Hovewer values of fact parameter are reasonable only in interval
		 * [0.0 , 1.0], you can pass also values outside of this interval and you
		 * can get result (extrapolation?)
		 */
		Matrix3<T> lerp(T fact, const Matrix3<T>& rhs) const
		{
			Matrix3<T> ret = (*this) + (rhs - (*this)) * fact;
			return ret;		
		}

    
		//-------------[ conversion ]-----------------------------
	
		/**
		 * Conversion to pointer operator
		 * @return Pointer to internaly stored (in managment of class Matrix3<T>)
		 * used for passing Matrix3<T> values to gl*[fd]v functions.
		 */
		operator T*(){ return (T*) data; }
	
		/**
		 * Conversion to pointer operator
		 * @return Constant Pointer to internaly stored (in managment of class Matrix3<T>)
		 * used for passing Matrix3<T> values to gl*[fd]v functions.
		 */
		operator const T*() const { return (const T*) data; }

		//----------[ output operator ]----------------------------
		/**
		 * Output to stream operator
		 * @param lhs Left hand side argument of operator (commonly ostream instance). 
		 * @param rhs Right hand side argument of operator.
		 * @return Left hand side argument - the ostream object passed to operator.
		 */
		friend std::ostream& operator << (std::ostream& lhs, const Matrix3<T>& rhs) 
		{
			for (int i = 0; i < 3; i++)
			{
				lhs << "|\t";
				for (int j = 0; j < 3; j++)
				{
					lhs << rhs.at(j,i) << "\t";
				}
				lhs << "|" << std::endl;
			}
			return lhs;
		}
    
	};

	/// Matrix 3x3 of floats
	typedef Matrix3<float> Matrix3f;
	/// Matrix 3x3 of doubles
	typedef Matrix3<double> Matrix3d;


	/**
	 * Class for matrix 4x4 
	 * @note Data stored in this matrix are in column major order.
	 */
	template <class T>
	class Matrix4 
	{
	public:
		/// Data stored in column major order
		T data[16];
	
		//--------------------------[ constructors ]-------------------------------
		/**
		 *Creates identity matrix
		 */
		Matrix4() 
		{
			for (int i = 0; i < 16; i++)
				data[i] = (i % 5) ? 0 : 1;
		}

		/**
		 * Copy matrix values from array (these data must be in column
		 * major order!)
		 */
		Matrix4(const T * dt) 
		{
			std::memcpy(data, dt, sizeof(T) * 16); 
		}

		/**
		 * Copy constructor.
		 * @param src Data source for new created instance of Matrix4.
		 */
		Matrix4(const Matrix4<T>& src)
		{
			std::memcpy(data, src.data, sizeof(T) * 16);
		}
	
		/**
		 * Copy casting constructor.
		 * @param src Data source for new created instance of Matrix4.
		 */
		template <class FromT>
		Matrix4(const Matrix4<FromT>& src)
		{
			for (int i = 0; i < 16; i++)
			{
				data[i] = static_cast<T>(src.data[i]);
			}
		}

		/**
		 * Resets matrix to be identity matrix
		 */
		void identity()
		{
			for (int i = 0; i < 16; i++)
				data[i] = (i % 5) ? 0 : 1;
		}
	
	

		/**
		 * Creates rotation matrix by rotation around axis.
		 * @param a Angle (in radians) of rotation around axis X.
		 * @param b Angle (in radians) of rotation around axis Y.
		 * @param c Angle (in radians) of rotation around axis Z.
		 */
		static Matrix4<T> createRotationAroundAxis(T a, T b, T c)
		{
			Matrix4<T> ma, mb, mc;
			float ac = cos(a);
			float as = sin(a);
			float bc = cos(b);
			float bs = sin(b);
			float cc = cos(c);
			float cs = sin(c);

			ma.at(1,1) = ac;
			ma.at(2,1) = as;
			ma.at(1,2) = -as;
			ma.at(2,2) = ac;

			mb.at(0,0) = bc;
			mb.at(2,0) = -bs;
			mb.at(0,2) = bs;
			mb.at(2,2) = bc;

			mc.at(0,0) = cc;
			mc.at(1,0) = cs;
			mc.at(0,1) = -cs;
			mc.at(1,1) = cc;

			/*std::cout << "RotVec = " << a << "," << b << "," << c << std::endl;
			  std::cout << "Rx = " << std::endl << ma;
			  std::cout << "Ry = " << std::endl << mb;
			  std::cout << "Rz = " << std::endl << mc;*/
		
			Matrix4<T> ret = ma * mb * mc;
			//std::cout << "Result = " << std::endl << ma * (mb * mc);
		
			return ret;
		}

		/// Creates translation matrix
		/**
		 * Creates translation matrix.
		 * @param x X-direction translation
		 * @param y Y-direction translation
		 * @param z Z-direction translation
		 * @param w for W-coordinate translation (impictily set to 1)
		 */
		static Matrix4<T> createTranslation(T x, T y, T z, T w = 1)
		{
			Matrix4 ret;
			ret.at(3,0) = x;
			ret.at(3,1) = y;
			ret.at(3,2) = z;
			ret.at(3,3) = w;
		
			return ret;
		}


		//---------------------[ equiality operators ]------------------------------	
		/**
		 * Equality test operator
		 * @param rhs Right hand side argument of binary operator.
		 * @note Test of equality is based of threshold EPSILON value. To be two
		 * values equal, must satisfy this condition all elements of matrix 
		 * | lws[i] - rhs[i] | < EPSILON,
		 * same for y-coordinate, z-coordinate, and w-coordinate.
		 */	
		bool operator==(const Matrix4<T>& rhs) const
		{
			for (int i = 0; i < 16; i++)
			{
				if (std::fabs(data[i] - rhs.data[i]) >= EPSILON)
					return false;
			}
			return true;
		}
	
		/**
		 * Inequality test operator
		 * @param rhs Right hand side argument of binary operator.
		 * @return not (lhs == rhs) :-P
		 */
		bool operator!=(const Matrix4<T>& rhs) const
		{
			return !(*this == rhs);
		}
	

		//---------------------[ access operators ]---------------------------------
		/**
		 * Get reference to element at postion (x,y).
		 * @param x Number of column (0..3)
		 * @param y Number of row (0..3)
		 */
		T& at(int x, int y) 
		{
			assert(x >= 0 && x < 4);
			assert(y >= 0 && y < 4);
			return data[x * 4 + y];
		}

		/**
		 * Get constant reference to element at postion (x,y).
		 * @param x Number of column (0..3)
		 * @param y Number of row (0..3)
		 */
		const T& at(int x, int y) const 
		{
			assert(x >= 0 && x < 4);
			assert(y >= 0 && y < 4);
			return data[x * 4 + y];
		}


		/**
		 * Sets translation part of matrix.
		 *
		 * @param v Vector of translation to be set.
		 */
		void setTranslation(const Vector3<T>& v)
		{
			at(3,0) = v.x;
			at(3,1) = v.y;
			at(3,2) = v.z;
			at(3,3) = 1;
		}

		Vector3<T> getTranslation()
		{ return Vector3<T>(at(3,0),at(3,1),at(3,2)); }

		/**
		 * Sets roation part (matrix 3x3) of matrix.
		 *
		 * @param m Rotation part of matrix
		 */
		void setRotation(const Matrix3<T>& m)
		{
			for (int i = 0; i < 3; i++)
			{
				for (int j = 0; j < 3; j++)
				{
					at(i,j) = m.at(i,j);
				}
			}
		}
				 

		/**
		 * Copy operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Matrix4<T>& operator=(const Matrix4<T>& rhs) 
		{
			std::memcpy(data, rhs.data, sizeof(T) * 16);
			return * this;
		}

		/**
		 * Copy casting operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		template <class FromT>
		Matrix4<T>& operator=(const Matrix4<FromT>& rhs) 
		{
			for (int i = 0; i < 16; i++)
			{
				data[i] = static_cast<T>(rhs.data[i]);
			}
			return * this;
		}

		/**
		 * Copy operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Matrix4<T>& operator=(const T* rhs) 
		{
			std::memcpy(data, rhs, sizeof(T) * 16);
			return * this;
		}

		/*Matrix4<T> & operator=(const double* m)
		  {
		  for (int i = 0; i < 16; i++) data[i] = (T)m[i];
		  return * this;
		  }*/
	
		//--------------------[ matrix with matrix operations ]---------------------
		/**
		 * Addition operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Matrix4<T> operator+(const Matrix4<T>& rhs) const
		{
			Matrix4<T> ret;
			for (int i = 0; i < 16; i++)
				ret.data[i] = data[i] + rhs.data[i];
			return ret;
		}
	
		/**
		 * Substraction operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Matrix4<T> operator-(const Matrix4<T>& rhs) const
		{
			Matrix4<T> ret;
			for (int i = 0; i < 16; i++)
				ret.data[i] = data[i] - rhs.data[i];
			return ret;
		}
	
		//--------------------[ matrix with scalar operations ]---------------------
		/**
		 * Addition operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Matrix4<T> operator+(T rhs) const
		{
			Matrix4<T> ret;
			for (int i = 0; i < 16; i++)
				ret.data[i] = data[i] + rhs;
			return ret;
		}
	
		/**
		 * Substraction operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Matrix4<T> operator-(T rhs) const
		{
			Matrix4<T> ret;
			for (int i = 0; i < 16; i++)
				ret.data[i] = data[i] - rhs;
			return ret;
		}
	
		/**
		 * Multiplication operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Matrix4<T> operator*(T rhs) const
		{
			Matrix4<T> ret;
			for (int i = 0; i < 16; i++)
				ret.data[i] = data[i] * rhs;
			return ret;
		}
	
		/**
		 * Division operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Matrix4<T> operator/(T rhs) const
		{
			Matrix4<T> ret;
			for (int i = 0; i < 16; i++)
				ret.data[i] = data[i] / rhs;
			return ret;
		}
	

		//--------------------[ multiply operators ]--------------------------------
		/**
		 * Multiplication operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector4<T> operator*(const Vector4<T>& rhs) const 
		{
			return Vector4<T>(
							  data[0] * rhs.x + data[4] * rhs.y + data[8]  * rhs.z + data[12] * rhs.w,
							  data[1] * rhs.x + data[5] * rhs.y + data[9]  * rhs.z + data[13] * rhs.w,
							  data[2] * rhs.x + data[6] * rhs.y + data[10] * rhs.z + data[14] * rhs.w,
							  data[3] * rhs.x + data[7] * rhs.y + data[11] * rhs.z + data[15] * rhs.w
							  );

		}
	
		/**
		 * Multiplication operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Vector3<T> operator*(const Vector3<T>& rhs) const
		{
			return Vector3<T>(
							  data[0] * rhs.x + data[4] * rhs.y + data[8] * rhs.z,
							  data[1] * rhs.x + data[5] * rhs.y + data[9] * rhs.z,
							  data[2] * rhs.x + data[6] * rhs.y + data[10] * rhs.z
							  );
		}

		/**
		 * Multiplication operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Matrix4<T> operator*(Matrix4<T> rhs) const 
		{
			static Matrix4<T> w;
			for (int i = 0; i < 4; i++) 
			{
				for (int j = 0; j < 4; j++) 
				{
					T n = 0;
					for (int k = 0; k < 4; k++) n += rhs.at(i, k) * at(k, j);
					w.at(i, j) = n;
				}
			}
			return w;
		
		}
	
	
		//---------------------------[ misc operations ]----------------------------
 
		/**
		 * Computes determinant of matrix
		 * @return Determinant of matrix
		 * @note This function does 3 * 4 * 6 mul, 3 * 6 add.
		 */
		T det()
		{

			return
				+ at(3,0) * at(2,1) * at(1,2) * at(0,3)
				- at(2,0) * at(3,1) * at(1,2) * at(0,3)
				- at(3,0) * at(1,1) * at(2,2) * at(0,3)
				+ at(1,0) * at(3,1) * at(2,2) * at(0,3)

				+ at(2,0) * at(1,1) * at(3,2) * at(0,3)
				- at(1,0) * at(2,1) * at(3,2) * at(0,3)
				- at(3,0) * at(2,1) * at(0,2) * at(1,3)
				+ at(2,0) * at(3,1) * at(0,2) * at(1,3)

				+ at(3,0) * at(0,1) * at(2,2) * at(1,3)
				- at(0,0) * at(3,1) * at(2,2) * at(1,3)
				- at(2,0) * at(0,1) * at(3,2) * at(1,3)
				+ at(0,0) * at(2,1) * at(3,2) * at(1,3)

				+ at(3,0) * at(1,1) * at(0,2) * at(2,3)
				- at(1,0) * at(3,1) * at(0,2) * at(2,3)
				- at(3,0) * at(0,1) * at(1,2) * at(2,3)
				+ at(0,0) * at(3,1) * at(1,2) * at(2,3)

				+ at(1,0) * at(0,1) * at(3,2) * at(2,3)
				- at(0,0) * at(1,1) * at(3,2) * at(2,3)
				- at(2,0) * at(1,1) * at(0,2) * at(3,3)
				+ at(1,0) * at(2,1) * at(0,2) * at(3,3)

				+ at(2,0) * at(0,1) * at(1,2) * at(3,3)
				- at(0,0) * at(2,1) * at(1,2) * at(3,3)
				- at(1,0) * at(0,1) * at(2,2) * at(3,3)
				+ at(0,0) * at(1,1) * at(2,2) * at(3,3);


		}


		/**
		 * Computes inverse matrix
		 * @return Inverse matrix of this matrix.
		 * @note This is a little bit time consuming operation
		 * (16 * 6 * 3 mul, 16 * 5 add + det() + mul() functions)
		 */
		Matrix4<T> inverse()
		{
			Matrix4<T> ret;

			ret.at(0,0) =  
				+ at(2,1) * at(3,2) * at(1,3) 
				- at(3,1) * at(2,2) * at(1,3) 
				+ at(3,1) * at(1,2) * at(2,3) 
				- at(1,1) * at(3,2) * at(2,3) 
				- at(2,1) * at(1,2) * at(3,3) 
				+ at(1,1) * at(2,2) * at(3,3);

			ret.at(1,0) =
				+ at(3,0) * at(2,2) * at(1,3) 
				- at(2,0) * at(3,2) * at(1,3) 
				- at(3,0) * at(1,2) * at(2,3) 
				+ at(1,0) * at(3,2) * at(2,3) 
				+ at(2,0) * at(1,2) * at(3,3) 
				- at(1,0) * at(2,2) * at(3,3);

			ret.at(2,0) = 
				+ at(2,0) * at(3,1) * at(1,3) 
				- at(3,0) * at(2,1) * at(1,3) 
				+ at(3,0) * at(1,1) * at(2,3) 
				- at(1,0) * at(3,1) * at(2,3) 
				- at(2,0) * at(1,1) * at(3,3) 
				+ at(1,0) * at(2,1) * at(3,3);

			ret.at(3,0) = 
				+ at(3,0) * at(2,1) * at(1,2) 
				- at(2,0) * at(3,1) * at(1,2) 
				- at(3,0) * at(1,1) * at(2,2) 
				+ at(1,0) * at(3,1) * at(2,2) 
				+ at(2,0) * at(1,1) * at(3,2) 
				- at(1,0) * at(2,1) * at(3,2);

			ret.at(0,1) = 
				+ at(3,1) * at(2,2) * at(0,3) 
				- at(2,1) * at(3,2) * at(0,3) 
				- at(3,1) * at(0,2) * at(2,3) 
				+ at(0,1) * at(3,2) * at(2,3) 
				+ at(2,1) * at(0,2) * at(3,3) 
				- at(0,1) * at(2,2) * at(3,3);

			ret.at(1,1) = 
				+ at(2,0) * at(3,2) * at(0,3) 
				- at(3,0) * at(2,2) * at(0,3) 
				+ at(3,0) * at(0,2) * at(2,3) 
				- at(0,0) * at(3,2) * at(2,3) 
				- at(2,0) * at(0,2) * at(3,3) 
				+ at(0,0) * at(2,2) * at(3,3);

			ret.at(2,1) =
				+ at(3,0) * at(2,1) * at(0,3) 
				- at(2,0) * at(3,1) * at(0,3) 
				- at(3,0) * at(0,1) * at(2,3) 
				+ at(0,0) * at(3,1) * at(2,3) 
				+ at(2,0) * at(0,1) * at(3,3) 
				- at(0,0) * at(2,1) * at(3,3);

			ret.at(3,1) = 
				+ at(2,0) * at(3,1) * at(0,2) 
				- at(3,0) * at(2,1) * at(0,2) 
				+ at(3,0) * at(0,1) * at(2,2) 
				- at(0,0) * at(3,1) * at(2,2) 
				- at(2,0) * at(0,1) * at(3,2) 
				+ at(0,0) * at(2,1) * at(3,2);

			ret.at(0,2) =  
				+ at(1,1) * at(3,2) * at(0,3) 
				- at(3,1) * at(1,2) * at(0,3) 
				+ at(3,1) * at(0,2) * at(1,3) 
				- at(0,1) * at(3,2) * at(1,3) 
				- at(1,1) * at(0,2) * at(3,3) 
				+ at(0,1) * at(1,2) * at(3,3);

			ret.at(1,2) =
				+ at(3,0) * at(1,2) * at(0,3) 
				- at(1,0) * at(3,2) * at(0,3) 
				- at(3,0) * at(0,2) * at(1,3) 
				+ at(0,0) * at(3,2) * at(1,3) 
				+ at(1,0) * at(0,2) * at(3,3) 
				- at(0,0) * at(1,2) * at(3,3);

			ret.at(2,2) = 
				+ at(1,0) * at(3,1) * at(0,3) 
				- at(3,0) * at(1,1) * at(0,3) 
				+ at(3,0) * at(0,1) * at(1,3) 
				- at(0,0) * at(3,1) * at(1,3) 
				- at(1,0) * at(0,1) * at(3,3) 
				+ at(0,0) * at(1,1) * at(3,3);

			ret.at(3,2) = 
				+ at(3,0) * at(1,1) * at(0,2) 
				- at(1,0) * at(3,1) * at(0,2) 
				- at(3,0) * at(0,1) * at(1,2) 
				+ at(0,0) * at(3,1) * at(1,2) 
				+ at(1,0) * at(0,1) * at(3,2) 
				- at(0,0) * at(1,1) * at(3,2);

			ret.at(0,3) =
				+ at(2,1) * at(1,2) * at(0,3) 
				- at(1,1) * at(2,2) * at(0,3) 
				- at(2,1) * at(0,2) * at(1,3) 
				+ at(0,1) * at(2,2) * at(1,3) 
				+ at(1,1) * at(0,2) * at(2,3) 
				- at(0,1) * at(1,2) * at(2,3);

			ret.at(1,3) = 
				+ at(1,0) * at(2,2) * at(0,3) 
				- at(2,0) * at(1,2) * at(0,3) 
				+ at(2,0) * at(0,2) * at(1,3) 
				- at(0,0) * at(2,2) * at(1,3) 
				- at(1,0) * at(0,2) * at(2,3) 
				+ at(0,0) * at(1,2) * at(2,3);

			ret.at(2,3) =  
				+ at(2,0) * at(1,1) * at(0,3) 
				- at(1,0) * at(2,1) * at(0,3) 
				- at(2,0) * at(0,1) * at(1,3) 
				+ at(0,0) * at(2,1) * at(1,3) 
				+ at(1,0) * at(0,1) * at(2,3) 
				- at(0,0) * at(1,1) * at(2,3);

			ret.at(3,3) = 
				+ at(1,0) * at(2,1) * at(0,2) 
				- at(2,0) * at(1,1) * at(0,2) 
				+ at(2,0) * at(0,1) * at(1,2) 
				- at(0,0) * at(2,1) * at(1,2) 
				- at(1,0) * at(0,1) * at(2,2) 
				+ at(0,0) * at(1,1) * at(2,2);
  
			return ret * det();
		}



		/**
		 * Transpose matrix.
		 */
		Matrix4<T> transpose()
		{
			Matrix4<T> ret;
			for (int i = 0; i < 4; i++)
			{
				for (int j = 0; j < 4; j++)
				{
					ret.at(i,j) = at(j,i);
				}
			}
			return ret;
		}
	
		/**
		 * Linear interpolation of two vectors
		 * @param fact Factor of interpolation. For translation from positon
		 * of this matrix (lhs) to matrix rhs, values of factor goes from 0.0 to 1.0.
		 * @param rhs Second Matrix for interpolation 
		 * @note Hovewer values of fact parameter are reasonable only in interval
		 * [0.0 , 1.0], you can pass also values outside of this interval and you
		 * can get result (extrapolation?)
		 */
		Matrix4<T> lerp(T fact, const Matrix4<T>& rhs) const
		{
			Matrix4<T> ret = (*this) + (rhs - (*this)) * fact;
			return ret;		
		}

    
		//-------------[ conversion ]-----------------------------
		/**
		 * Conversion to pointer operator
		 * @return Pointer to internaly stored (in managment of class Matrix4<T>)
		 * used for passing Matrix4<T> values to gl*[fd]v functions.
		 */
		operator T*(){ return (T*) data; }
	
		/**
		 * Conversion to pointer operator
		 * @return Constant Pointer to internaly stored (in managment of class Matrix4<T>)
		 * used for passing Matrix4<T> values to gl*[fd]v functions.
		 */
		operator const T*() const { return (const T*) data; }

		//----------[ output operator ]----------------------------
		/**
		 * Output to stream operator
		 * @param lhs Left hand side argument of operator (commonly ostream instance). 
		 * @param rhs Right hand side argument of operator.
		 * @return Left hand side argument - the ostream object passed to operator.
		 */
		friend std::ostream& operator << (std::ostream& lhs, const Matrix4<T>& rhs) 
		{
			for (int i = 0; i < 4; i++)
			{
				lhs << "|\t";
				for (int j = 0; j < 4; j++)
				{
					lhs << rhs.at(j,i) << "\t";
				}
				lhs << "|" << std::endl;
			}
			return lhs;
		}
    
	};


	/// Matrix 3x3 of floats
	typedef Matrix4<float> Matrix4f;
	/// Matrix 3x3 of doubles
	typedef Matrix4<double> Matrix4d;


	/**
	 * Quaternion class implementing some quaternion algebra operations.
	 * Quaternion is kind of complex number it consists of its real part (w)
	 * and its complex part v. This complex part has three elements, so we
	 * can express it as xi + yj + zk . Note that coordinates of (x,y,z) are
	 * hold inside v field.
	 */
	template <class T>
	class Quaternion
	{
	public:
		/**
		 * Real part of quaternion.
		 */
		T w;
		/**
		 * Complex part of quaternion.
		 */
		Vector3<T> v;
	
		/**
		 * Quaternion constructor, sets quaternion to (0 + 0i + 0j + 0k).
		 */
		Quaternion(): w(0), v(0,0,0){}
	
		/**
		 * Copy constructor.
		 */
		Quaternion(const Quaternion<T>& q): w(q.w), v(q.v){	}

		/**
		 * Copy casting constructor.
		 */
		template <class FromT>
		Quaternion(const Quaternion<FromT>& q)
			: w(static_cast<T>(q.w)), 
			  v(q.v){	}

	
		/**
		 * Creates quaternion object from real part w_ and complex part v_.
		 * @param w_ Real part of quaternion.
		 * @param v_ Complex part of quaternion (xi + yj + zk).
		 */
		Quaternion(T w_, const Vector3<T>& v_): w(w_), v(v_){}
	
		/**
		 * Creates quaternion object from value (w_ + xi + yj + zk).
		 * @param w_ Real part of quaternion.
		 * @param x Complex cooeficinet for i complex constant.
		 * @param y Complex cooeficinet for j complex constant.
		 * @param z Complex cooeficinet for k complex constant.
		 */
		Quaternion(T w_, T x, T y, T z): w(w_), v(x,y,z){}
	
		/**
		 * Copy operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Quaternion<T>& operator= (const Quaternion<T>& rhs)
		{
			v = rhs.v;
			w = rhs.w;
			return *this;
		}

		/**
		 * Copy convert operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		template<class FromT>
		Quaternion<T>& operator= (const Quaternion<FromT>& rhs)
		{
			v = rhs.v;
			w = static_cast<T>(rhs.w);
			return *this;
		}
	
		/**
		 * Addition operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Quaternion<T> operator+ (const Quaternion<T>& rhs) const
		{
			const Quaternion<T>& lhs = *this;
			return Quaternion<T>(lhs.w + rhs.w, lhs.v + rhs.v);  
		}
	
		/**
		 * Multiplication operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Quaternion<T> operator* (const Quaternion<T>& rhs) const
		{
			const Quaternion<T>& lhs = *this;
			return Quaternion<T>(
								 lhs.w * rhs.w 	- lhs.v.x * rhs.v.x 	- lhs.v.y * rhs.v.y 	- lhs.v.z * rhs.v.z,
								 lhs.w * rhs.v.x 	+ lhs.v.x * rhs.w 	+ lhs.v.y * rhs.v.z	- lhs.v.z * rhs.v.y,
								 lhs.w * rhs.v.y 	- lhs.v.x * rhs.v.z	+ lhs.v.y * rhs.w	+ lhs.v.z * rhs.v.x,
								 lhs.w * rhs.v.z	+ lhs.v.x * rhs.v.y	- lhs.v.y * rhs.v.x	+ lhs.v.z * rhs.w
								 );
		}
	
		/**
		 * Multiplication operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Quaternion<T> operator* (T rhs) const 
		{ 
			return Quaternion<T>(w*rhs, v*rhs);
		}
	
		/**
		 * Substraction operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Quaternion<T> operator- (const Quaternion<T>& rhs) const
		{
			const Quaternion<T>& lhs = *this;
			return Quaternion<T>(lhs.w - rhs.w, lhs.v - rhs.v);
		}
	
		/**
		 * Addition operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Quaternion<T>& operator+= (const Quaternion<T>& rhs)
		{
			w += rhs.w;
			v += rhs.v;
			return *this;
		}
	
		/**
		 * Substraction operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Quaternion<T>& operator-= (const Quaternion<T>& rhs)
		{
			w -= rhs.w;
			v -= rhs.v;
			return *this;
		}

		/**
		 * Multiplication operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Quaternion<T>& operator*= (const Quaternion<T>& rhs)
		{
			Quaternion q = (*this) * rhs;
			v = q.v;
			w = q.w;
			return *this;
		}
	
		/**
		 * Multiplication operator
		 * @param rhs Right hand side argument of binary operator.
		 */
		Quaternion<T>& operator*= (T rhs)
		{
			w *= rhs;
			v *= rhs;
			return *this;
		}

		/**
		 * Equality test operator
		 * @param rhs Right hand side argument of binary operator.
		 * @note Test of equality is based of threshold EPSILON value. To be two
		 * values equal, must satisfy this condition | lws - rhs | < EPSILON,
		 * for all quaternion coordinates.
		 */
		bool operator==(const Quaternion<T>& rhs) const
		{
			const Quaternion<T>& lhs = *this;
			return (std::fabs(lhs.w - rhs.w) < EPSILON) && lhs.v == rhs.v;
		}
	
		/**
		 * Inequality test operator
		 * @param rhs Right hand side argument of binary operator.
		 * @return not (lhs == rhs) :-P
		 */
		bool operator!=(const Quaternion<T>& rhs) const { return ! (*this == rhs); }

		//-------------[ unary operations ]--------------------------
		/**
		 * Unary negate operator
		 * @return negated quaternion
		 */
		Quaternion<T> operator-() const
		{
			return Quaternion<T>(-w, -v);
		}

		/**
		 * Unary conjugate operator
		 * @return conjugated quaternion
		 */
		Quaternion<T> operator~() const
		{
			return Quaternion<T>(w, -v);
		}

		/**
		 * Get lenght of quaternion.
		 * @return Length of quaternion.
		 */
		T length() const
		{
			return (T)std::sqrt(w*w + v.lengthSq());	
		}
	
		/**
		 * Return square of length.
		 * @return lenght ^ 2
		 * @note This method is faster then length(). For comparsion
		 * of length of two quaternion can be used just this value, instead
		 * of computionaly more expensive length() method.
		 */
		T lengthSq() const
		{
			return w * w + v.lengthSq();	
		}
	
	
		/**
		 * Normalize quaternion
		 */
		void normalize()
		{
			T len = length();
			w /= len;
			v /= len;
		}
	
		/**
		 * Creates quaternion for eulers angles.
		 * @param x Rotation around x axis (in degrees).
		 * @param y Rotation around y axis (in degrees).
		 * @param z Rotation around z axis (in degrees).
		 * @return Quaternion object representing transoformation.
		 */
		static Quaternion<T> fromEulerAngles(T x, T y, T z)
		{
			Quaternion<T> ret = fromAxisRot(Vector3<T>(1,0,0), x)
				* fromAxisRot(Vector3<T>(0,1,0),y) * fromAxisRot(Vector3<T>(0,0,1),z);
			return ret;
		}
	
		/**
		 * Creates quaternion as rotation around axis.
		 * @param axis Unit vector expressing axis of rotation.
		 * @param angleDeg Angle of rotation around axis (in degrees).
		 */
		static Quaternion<T> fromAxisRot(Vector3<T> axis, float angleDeg)
		{
			double angleRad = DEG2RAD(angleDeg);
			double sa2 = std::sin(angleRad/2);
			double ca2 = std::cos(angleRad/2);
			return Quaternion<T>( ca2, axis * sa2);
		}
	
		/**
		 * Converts quaternion into rotation matrix.
		 * @return Rotation matrix expresing this quaternion.
		 */
		Matrix3<T> rotMatrix()
		{
			Matrix3<T> ret;
		
			/*ret.at(0,0) = 1 - 2*v.y*v.y - 2*v.z*v.z;
			  ret.at(1,0) = 2*v.x*v.y - 2*w*v.z;
			  ret.at(2,0) = 2*v.x*v.z - 2*w*v.y;
		
			  ret.at(0,1) = 2*v.x*v.y + 2*w*v.z;
			  ret.at(1,1) = 1 - 2*v.x*v.x - 2*v.z*v.z;
			  ret.at(2,1) = 2*v.y*v.z - 2*w*v.x;
		
			  ret.at(0,2) = 2*v.x*v.z - 2*w*v.y;
			  ret.at(1,2) = 2*v.y*v.z + 2*w*v.x;
			  ret.at(2,2) = 1 - 2*v.x*v.x - 2*v.y*v.y;*/
		
			T xx = v.x * v.x;
			T xy = v.x * v.y;
			T xz = v.x * v.z;
			T xw = v.x * w;
		
			T yy = v.y * v.y;
			T yz = v.y * v.z;
			T yw = v.y * w;
		
			T zz = v.z * v.z;
			T zw = v.z * w;
		
			ret.at(0,0) = 1 - 2 * (yy + zz);
			ret.at(1,0) = 2 * (xy - zw);
			ret.at(2,0) = 2 * (xz + yw);
		
			ret.at(0,1) = 2 * (xy + zw);
			ret.at(1,1) = 1 - 2 * (xx + zz);
			ret.at(2,1) = 2 * (yz - xw);
		
			ret.at(0,2) = 2 * (xz - yw);
			ret.at(1,2) = 2 * (yz + xw);
			ret.at(2,2) = 1 - 2 * (xx + yy);
		
		
			return ret;
		}
	
		/**
		 * Converts quaternion into transformation matrix.
		 * @note This method performs same operation as rotMatrix() 
		 * conversion method. But returns Matrix of 4x4 elements.
		 * @return Transformation matrix expressing this quaternion.
		 */
		Matrix4<T> transform()
		{
			Matrix4<T> ret;
		
			T xx = v.x * v.x;
			T xy = v.x * v.y;
			T xz = v.x * v.z;
			T xw = v.x * w;
		
			T yy = v.y * v.y;
			T yz = v.y * v.z;
			T yw = v.y * w;
		
			T zz = v.z * v.z;
			T zw = v.z * w;
		
			ret.at(0,0) = 1 - 2 * (yy + zz);
			ret.at(1,0) = 2 * (xy - zw);
			ret.at(2,0) = 2 * (xz + yw);
			ret.at(3,0) = 0;
		
			ret.at(0,1) = 2 * (xy + zw);
			ret.at(1,1) = 1 - 2 * (xx + zz);
			ret.at(2,1) = 2 * (yz - xw);
			ret.at(3,1) = 0;
		
			ret.at(0,2) = 2 * (xz - yw);
			ret.at(1,2) = 2 * (yz + xw);
			ret.at(2,2) = 1 - 2 * (xx + yy);
			ret.at(3,2) = 0;

			ret.at(0,3) = 0;
			ret.at(1,3) = 0;
			ret.at(2,3) = 0;
			ret.at(3,3) = 1;
		
			return ret;
		
		}

		/**
		 * Linear interpolation of two quaternions
		 * @param fact Factor of interpolation. For translation from positon
		 * of this vector to quaternion rhs, values of factor goes from 0.0 to 1.0.
		 * @param rhs Second Quaternion for interpolation 
		 * @note Hovewer values of fact parameter are reasonable only in interval
		 * [0.0 , 1.0], you can pass also values outside of this interval and you
		 * can get result (extrapolation?)
		 */
		Quaternion<T> lerp(T fact, const Quaternion<T>& rhs) const
		{
			return Quaternion<T>((1-fact) * w + fact * rhs.w, v.lerp(fact, rhs.v));
		}

		/**
		 * Provides output to standard output stream.
		 */
		friend std::ostream& operator << (std::ostream& oss, const Quaternion<T>& q)
		{
			oss << "Re: " << q.w << " Im: " << q.v;
			return oss;
		}

		/**
		 * Creates quaternion from transform matrix.
		 *
		 * @param m Transfrom matrix used to compute quaternion.
		 * @return Quaternion representing rotation of matrix m.
		 */
		static Quaternion<T> fromMatrix(const Matrix4<T>& m)
		{
			Quaternion<T> q;
						
			T tr,s;
			tr = m.at(0,0) + m.at(1,1) + m.at(2,2);
			if (tr >= epsilon)
			{
				s = (T)sqrt(tr + 1);
				q.w = 0.5 * s;
				s = 0.5 / s;
							 
				q.v.x = (m.at(1,2) - m.at(2,1)) * s;
				q.v.y = (m.at(2,0) - m.at(0,2)) * s;
				q.v.z = (m.at(0,1) - m.at(1,0)) * s;
			}
			else
			{
				T d0 = m.at(0,0);
				T d1 = m.at(1,1);
				T d2 = m.at(2,2);
							 
				char bigIdx = (d0 > d1) ? ((d0 > d2)? 0 : 2) : ((d1 > d2) ? 1 : 2);
							 
				if (bigIdx == 0)
				{
					s = (T)sqrt((d0 - (d1 + d2)) + 1);
									
					q.v.x = 0.5 * s;
					s = 0.5 / s;
					q.v.y = (m.at(1,0) + m.at(0,1)) * s;
					q.v.z = (m.at(2,0) + m.at(0,2)) * s;
					q.w = (m.at(1,2) - m.at(2,1)) * s;
				}
				else if (bigIdx == 1)
				{
					s = (T)sqrt(1 + d1 - (d0 + d2));
					q.v.y = 0.5 * s;
					s = 0.5 / s;
					q.v.z = (m.at(2,1) + m.at(1,2)) / s;
					q.w = (m.at(2,0) - m.at(0,2)) / s;
					q.v.x = (m.at(1,0) + m.at(0,1)) / s;
				}
				else
				{
					s = (T)sqrt(1 + d2 - (d0 + d1));
					q.v.z = 0.5 * s;
					s = 0.5 / s;
					q.w = (m.at(0,1) - m.at(1,0)) / s;
					q.v.x = (m.at(2,0) + m.at(0,2)) / s;
					q.v.y = (m.at(2,1) + m.at(1,2)) / s;
				}
			}
						
			return q;
		}
				 
		/**
		 * Creates quaternion from rotation matrix.
		 *
		 * @param m Rotation matrix used to compute quaternion.
		 * @return Quaternion representing rotation of matrix m.
		 */
		static Quaternion<T> fromMatrix(const Matrix3<T>& m)
		{
			Quaternion<T> q;
						
			T tr,s;
			tr = m.at(0,0) + m.at(1,1) + m.at(2,2);
			// if trace is greater or equal then zero
			if (tr >= epsilon)
			{
				s = (T)sqrt(tr + 1);
				q.w = 0.5 * s;
				s = 0.5 / s;
							 
				q.v.x = (m.at(1,2) - m.at(2,1)) * s;
				q.v.y = (m.at(2,0) - m.at(0,2)) * s;
				q.v.z = (m.at(0,1) - m.at(1,0)) * s;
			}
			else
			{
				T d0 = m.at(0,0);
				T d1 = m.at(1,1);
				T d2 = m.at(2,2);
							 
				// find greates diagonal number
				char bigIdx = (d0 > d1) ? ((d0 > d2)? 0 : 2) : ((d1 > d2) ? 1 : 2);
							 
				if (bigIdx == 0)
				{
					s = (T)sqrt((d0 - (d1 + d2)) + 1);
									
					q.v.x = 0.5 * s;
					s = 0.5 / s;
					q.v.y = (m.at(1,0) + m.at(0,1)) * s;
					q.v.z = (m.at(2,0) + m.at(0,2)) * s;
					q.w = (m.at(1,2) - m.at(2,1)) * s;
				}
				else if (bigIdx == 1)
				{
					s = (T)sqrt(1 + d1 - (d0 + d2));
					q.v.y = 0.5 * s;
					s = 0.5 / s;
					q.v.z = (m.at(2,1) + m.at(1,2)) / s;
					q.w = (m.at(2,0) - m.at(0,2)) / s;
					q.v.x = (m.at(1,0) + m.at(0,1)) / s;
				}
				else
				{
					s = (T)sqrt(1 + d2 - (d0 + d1));
					q.v.z = 0.5 * s;
					s = 0.5 / s;
					q.w = (m.at(0,1) - m.at(1,0)) / s;
					q.v.x = (m.at(2,0) + m.at(0,2)) / s;
					q.v.y = (m.at(2,1) + m.at(1,2)) / s;
				}
			}
						
			return q;
		}

		/**
		 * Computes spherical interpolation between quaternions (this, q2)
		 * using coeficient of interpolation r (in [0, 1]).
		 *
		 * @param r The ratio of interpolation form this (r = 0) to q2 (r = 1).
		 * @param q2 Second quaternion for interpolation.
		 * @return Result of interpolation.
		 */
		Quaternion<T> slerp(T r, const Quaternion<T>& q2) const 
		{
			Quaternion<T> ret;
			T cosTheta = w * q2.w + v.x * q2.v.x + v.y *q2.v.y + v.z * q2.v.z;
			T theta = (T) acos(cosTheta);
			if (fabs(theta) < epsilon)
			{
				ret = *this;
			}
			else
			{
				T sinTheta = (T)sqrt(1.0 - cosTheta * cosTheta);
				if (fabs(sinTheta) < epsilon)
				{
					ret.w = 0.5 * w + 0.5 * q2.w;
					ret.v = v.lerp(0.5, q2.v);
				}
				else
				{
					T rA = (T)sin((1.0 - r) * theta) / sinTheta;
					T rB = (T)sin(r * theta) / sinTheta;
									
					ret.w = w * rA + q2.w * rB;
					ret.v.x = v.x * rA + q2.v.x * rB;
					ret.v.y = v.y * rA + q2.v.y * rB;
					ret.v.z = v.z * rA + q2.v.z * rB;
				}
			}
			return ret;
		}
				 
							 
	
	};

	typedef Quaternion<float> Quatf;
	typedef Quaternion<double> Quatd;
   
   
   
#ifdef VMATH_NAMESPACE
}
#endif

#endif // __vmath_Header_File__