/*
 * $Id: 3dmath.h 4747 2017-10-30 13:39:56Z antonin.misek-skoda-auto.cz $
 *
 * Description  : One line description of file.
 * Author       : Vaclav Kyba <mail/Jabber: vaseo1@gmail.com>  <ICQ: 98576293>
 *
 * Purpose      :
 *     Long description of what the file is for.
 */


#ifndef __3DMATH__H__
#define __3DMATH__H__

#include "VrutApi.h"

#include <wx/string.h>
//#include <sstream>
//#include <memory.h>
#include <cmath>
#include <float.h>
#include <vector>
#include <algorithm>
#include <iostream>
#include <iomanip>
#include "common.h"
#include "textwriter.h"
#include "decompose.h"
#include <smmintrin.h>

// TODO Why are not these structures in namespace VRUT?

///Degrees to radians coefficient
const float DEG2RAD = 0.017453293f;
///Radians to degrees coefficient
const float RAD2DEG = 57.29577951f;
#ifndef M_PI
#define M_PI        3.14159265358979323846f
#endif

inline float DegToRad(float deg)	{	return deg * DEG2RAD;		}
inline float RadToDeg(float rad)	{	return rad * RAD2DEG;		}

template<class T> inline T clamp(const T &value, const T &min_value, const T &max_value)
{
	return (value < min_value) ? min_value : ((max_value < value) ? max_value : value);
}

struct Vector2;
struct Vector3;
struct Vector4;

///2 component vector
struct Vector2
{
	union
	{
		struct
		{
			///Components
			float x, y;
		};
		///Components
		float	_v[2];
	};

	///Default constructor
	Vector2() : x(0.0f), y(0.0f) {}
	///Constructor
	Vector2(const float _x, const float _y) : x(_x), y(_y)  {}
	///Constructor
	explicit Vector2(const float * v) : x(v[0]), y(v[1])         {}
	///Copy constructor
	Vector2(const Vector2 & v) : x(v.x), y(v.y)         {}

	// Vec3 to Vec2 constructor, drops z value
	explicit inline Vector2(const Vector3 &xy);
	// Vec4 to Vec2 constructor, drops z and w value
	explicit inline Vector2(const Vector4 &xy);

	///Add operator
	Vector2 operator+(const Vector2 & v) const
	{
		return Vector2(x+v.x, y+v.y);
	}
	///Subtract operator
	Vector2 operator-(const Vector2 & v) const
	{
		return Vector2(x-v.x, y-v.y);
	}
	///Scalar multiply operator
	Vector2 operator*(const float f) const
	{
		return Vector2(f*x, f*y);
	}
	///Scalar multiply operator
	friend Vector2 operator*(const float f, const Vector2 & v)
	{
		return Vector2(f*v.x, f*v.y);
	}
	///Scalar divide operator
	Vector2 operator/(const float f) const
	{
		float div = 1.0f/f;
		return operator*(div);
	}

	///Unary operator
	Vector2 operator-() const
	{
		return Vector2(-x, -y);
	}

	Vector2 &operator+=(const Vector2 &v)	{	x += v.x;	y += v.y;	return *this;	}
	Vector2 &operator-=(const Vector2 &v)	{	x -= v.x;	y -= v.y;	return *this;	}
	Vector2 &operator*=(const float f) 		{	x *= f;		y *= f;		return *this;	}
	Vector2 &operator/=(const float f)		{	x /= f;		y /= f;		return *this;	}

	///Equal operator
	bool operator==(const Vector2 & v) const
	{
		return (x == v.x) && (y == v.y);
	}
	///Not Equal operator
	bool operator!=(const Vector2 & v) const
	{
		return (x != v.x) || ( y != v.y);
	}
	///Less then operator
	bool operator<(const Vector2 & v) const
	{
		return (x < v.x) || ((x == v.x) && ( y < v.y));
	}
	///Get squared length
	float LengthSq() const
	{
		return x*x + y*y;
	}
	///Get length
	float Length() const
	{
		return sqrtf(x*x + y*y);
	}
	///Get dot product
	float Dot(const Vector2 & v) const
	{
		return x * v.x + y * v.y;
	}
	///Get determinant
	float Determinant(const Vector2 & v) const
	{
		return x * v.y - y * v.x;
	}
	///Get normalized vector
	Vector2 Normalize() const
	{
		float a = 1.0f / Length();
		return operator*(a);
	}
	///Get string
	wxString ToString() const
	{
		return wxString(wxT("("))+TextWriter::WriteToWxString(_v, 2, wxT(", "))+wxT(")");
	}

	// Per component multiply and divide
	static inline Vector2 MultiplyComp(const Vector2 &a, const Vector2 &b)
	{
		return Vector2(a.x*b.x, a.y*b.y);
	}
	static inline Vector2 DivideComp(const Vector2 &a, const Vector2 &b)
	{
		return Vector2(a.x/b.x, a.y/b.y);
	}
	
	// Per component max and min
	static inline Vector2 MaxComp(const Vector2 &a, const Vector2 &b)
	{
		return Vector2(__max(a.x, b.x), __max(a.y, b.y));
	}
	static inline Vector2 MinComp(const Vector2 &a, const Vector2 &b)
	{
		return Vector2(__min(a.x, b.x), __min(a.y, b.y));
	}

	// Returns Vector2(x,x)
	static inline Vector2 Replicate(const float x)	{	return Vector2(x, x);		}
};

struct Vector2i
{
    int x;
    int y;
    Vector2i() : x(0), y(0) {}
    Vector2i(int x, int y) : x(x), y(y) {}
};

///3 component vector
struct Vector3
{
	union
	{
		struct
		{
			///Components
			float x, y, z;
		};
		///Components
		float	_v[3];
	};

	/// Default constructor
	Vector3() : x(0.0f), y(0.0f), z(0.0f)	{}
	///Constructor
	Vector3(const float _x, const float _y, const float _z) : x(_x), y(_y), z(_z)	{}
	///Constructor
	explicit Vector3(const float * v) : x(v[0]), y(v[1]), z(v[2])	{}
	///Copy constructor
	Vector3(const Vector3 & v) : x(v.x), y(v.y), z(v.z)		{}

	// Vec2 to Vec3 constructor
	explicit inline Vector3(const Vector2 &xy, const float z);
	explicit inline Vector3(const float x, const Vector2 &yz);
	// Vec4 to Vec3 constructor, drops w value
	explicit inline Vector3(const Vector4 &xyz);

	///Assignment operator
	Vector3 & operator=(const Vector3 & v)
	{
		memcpy(_v, v._v, 12);
		return *this;
	}

	///Get component
	float operator[](unsigned i) const
	{
		return _v[i];
	}

	///Get component ref
	float & operator[](unsigned i)
	{
		return _v[i];
	}

	///Subtract operator
	Vector3 operator-(const Vector3 & v) const
	{
		return Vector3(x-v.x, y-v.y, z-v.z);
	}

	///Unary operator
	Vector3 operator-() const
	{
		return Vector3(-x, -y, -z);
	}

	///Add operator
	Vector3 operator+(const Vector3 & v) const
	{
		return Vector3(x+v.x, y+v.y, z+v.z);
	}

	///Scalar multiply operator
	Vector3 operator*(const float f) const
	{
		return Vector3(f*x, f*y, f*z);
	}

	///Scalar multiply operator
	Vector3 operator*(const double f) const
	{
		return Vector3((float)(f*x), (float)(f*y), (float)(f*z));
	}

	///Scalar multiply operator
	friend Vector3 operator*(const float f, const Vector3 & v)
	{
		return Vector3(f*v.x, f*v.y, f*v.z);
	}

	///Scalar divide operator
	Vector3 operator/(const float f) const
	{
		float div = 1.0f/f;
		return operator*(div);
	}

	///Add assignment operator
	Vector3 & operator+=(const Vector3 & v)
	{
		x += v.x;
		y += v.y;
		z += v.z;
		return *this;
	}

	///Subtract assignment operator
	Vector3 & operator-=(const Vector3 & v)
	{
		x -= v.x;
		y -= v.y;
		z -= v.z;
		return *this;
	}

	///Multiply assignment operator
	Vector3 & operator*=(const float f)
	{
		x *= f;
		y *= f;
		z *= f;
		return *this;
	}

	///Divide assignment operator
	Vector3 & operator/=(const float f)
	{
		x /= f;
		y /= f;
		z /= f;
		return *this;
	}

	///Equal operator
	bool operator==(const Vector3 & v) const
	{
		return (x == v.x) && ( y == v.y) && (z == v.z);
	}

	///Equal with tolerance operator
	bool IsSameAs(const Vector3 & v, const float eps=0) const
	{
		return (fabs(x-v.x)<=eps) && (fabs(y-v.y)<=eps) && (fabs(z-v.z)<=eps);
	}

	///Not Equal operator
	bool operator!=(const Vector3 & v) const
	{
		return (x != v.x) || ( y != v.y) || (z != v.z);
	}

	///Less then operator
	bool operator<(const Vector3 & v) const
	{
		return (x < v.x) || ((x == v.x) && ( y < v.y)) || ((x == v.x) && ( y == v.y) && (z < v.z));
	}

	///Get dot product
	float Dot(const Vector3 & v) const
	{
		return x * v.x + y * v.y + z * v.z;
	}

	///Get cross product
	Vector3 Cross(const Vector3 & v) const
	{
		return Vector3(y*v.z-z*v.y,z*v.x-x*v.z,x*v.y-y*v.x);
	}

	///Get normalized vector
	Vector3 Normalize() const
	{
		float a = 1.0f/Length();
		return operator*(a);
	}

	///Get squared length
	float LengthSq() const
	{
		return x*x + y*y + z*z;
	}

	///Get length
	float Length() const
	{
		return sqrtf(x*x + y*y + z*z);
	}

	///Compute triangle surface area
	//numerically stable version of Heron's formula is used
	float TriangleSurfaceArea(const Vector3 &p2, const Vector3 &p3)
	{
		// TODO Try this: return (p2 - *this).Cross(p3 - *this).Length() * 0.5f;
		std::vector<double> coefs;
		coefs.push_back((p2-*this).Length());
		coefs.push_back((p3-p2).Length());
		coefs.push_back((*this-p3).Length());
		std::sort(coefs.begin(), coefs.end());
		return sqrtf((coefs[0]+(coefs[1]+coefs[2]))*(coefs[2]-(coefs[0]-coefs[1]))*(coefs[2]+(coefs[0]-coefs[1]))*(coefs[0]+(coefs[1]-coefs[2])))/4.0f;
	}

	///Get string
	wxString ToString() const
	{
		return wxString(wxT("("))+TextWriter::WriteToWxString(_v, 3, wxT(", "))+wxT(")");
	}

	///Output stream operator
	friend std::ostream & operator<<(std::ostream & os, const Vector3 & v)
	{
		return os << "(" << v.x << ", " << v.y << ", " << v.z << ")";
	}

	///Get copy of instance
	Vector3 * Clone() const
	{
		return new Vector3(*this);
	}

	friend float Distance(const Vector3 &a, const Vector3 &b) {
		return (a-b).Length();
	}
	friend float SqrDistance(const Vector3 &a, const Vector3 &b) {
		return (a-b).LengthSq();
	}

	// Per component multiply and divide
	static inline Vector3 MultiplyComp(const Vector3 &a, const Vector3 &b)
	{
		return Vector3(a.x*b.x, a.y*b.y, a.z*b.z);
	}
	static inline Vector3 DivideComp(const Vector3 &a, const Vector3 &b)
	{
		return Vector3(a.x/b.x, a.y/b.y, a.z/b.z);
	}
	
	// Per component max and min
	static inline Vector3 MaxComp(const Vector3 &a, const Vector3 &b)
	{
		return Vector3(__max(a.x, b.x), __max(a.y, b.y), __max(a.z, b.z));
	}
	static inline Vector3 MinComp(const Vector3 &a, const Vector3 &b)
	{
		return Vector3(__min(a.x, b.x), __min(a.y, b.y), __min(a.z, b.z));
	}

	// Returns Vector3(x,x,x)
	static inline Vector3 Replicate(const float x)	{	return Vector3(x, x, x);		}


	inline friend Vector3 ArbitraryNormal(const Vector3 &N) {
		float dist2 = N.x * N.x + N.y * N.y;
		if (dist2 > 0.0001f) {
			float inv_size = 1.0f/sqrtf(dist2);
			return Vector3(N.y * inv_size, -N.x * inv_size, 0); // N x (0,0,1)
		}
		float inv_size = 1.0f/sqrtf(N.z * N.z + N.x * N.x);
		return Vector3(-N.z * inv_size, 0, N.x * inv_size); // N x (0,1,0)
	}

	
	void RightHandedBase(Vector3& U, Vector3& V) const {
		V = ArbitraryNormal(*this);
		U = V.Cross(*this);
	}
	
	inline friend Vector3 CosineRandomVector(const float r1, const float r2, const Vector3 &normal)
	{
		float theta = 2.0f*M_PI*r2;
		float radius = sqrt(r1);
		float x = radius*sin(theta);
		float z = radius*cos(theta);
		float y = sqrt(1.0f - x*x - z*z);
		//Vector3 dir(x, y, z);
		Vector3 u,v;
		normal.RightHandedBase(u, v);
		return x*u + z*v + y*normal;
	}
};


///4 component vector
struct Vector4
{
	union
	{
		struct
		{
			///Components
			float x, y, z, w;
		};
		///Components
		float	_v[4];
	};

	///Default constructor
	Vector4() : x(0.0f), y(0.0f), z(0.0f), w(1.0f)	{}
	///Constructor
	Vector4(float _x, float _y, float _z, float _w) : x(_x), y(_y), z(_z), w(_w)	{}
	///Constructor
	explicit Vector4(const float * v) : x(v[0]), y(v[1]), z(v[2]), w(v[3])	{}
	///Copy constructor
	Vector4(const Vector4 & v) : x(v.x), y(v.y), z(v.z), w(v.w)		{}

	// Vec2 to Vec4, Vec3 to Vec4 constructors
	explicit inline Vector4(const Vector2 &xy, float z, float w);
	explicit inline Vector4(float x, const Vector2 &yz, float w);
	explicit inline Vector4(float x, float y, const Vector2 &zw);
	explicit inline Vector4(const Vector2 &xy, const Vector2 &zw);
	explicit inline Vector4(const Vector3 &xyz, float w);
	explicit inline Vector4(float x, const Vector3 &yzw);

	///Assignment operator
	Vector4 & operator=(const Vector4 & v)
	{
		x = v.x;
		y = v.y;
		z = v.z;
		w = v.w;
		return *this;
	}

	///Get component
	float operator[](unsigned i) const
	{
		return _v[i];
	}

	///Scalar multiply operator
	Vector4 operator*(float f) const
	{
		return Vector4(f*x, f*y, f*z, f*w);
	}

	///Subtract operator
	Vector4 operator-(const Vector4 & v) const
	{
		return Vector4(x-v.x, y-v.y, z-v.z, w-v.w);
	}

	///Unary operator
	Vector4 operator-() const
	{
		return Vector4(-x, -y, -z, -w);
	}

	///Add operator
	Vector4 operator+(const Vector4 & v) const
	{
		return Vector4(x+v.x, y+v.y, z+v.z, w + v.w);
	}

	///Scalar multiply operator
	friend Vector4 operator*(float f, const Vector4 & v)
	{
		return Vector4(f*v.x, f*v.y, f*v.z, f*v.w);
	}

	///Multiply operator
	// TODO Why Vector4 has per-component operator* and Vector3 doesn't?
	Vector4 operator*(const Vector4 & v) const
	{
		return Vector4(x*v.x, y*v.y, z*v.z, w*v.w);
	}

	///Scalar divide operator
	Vector4 operator/(float f) const
	{
		float div = 1.0f/f;
		return operator*(div);
	}

	///Add assignment operator
	Vector4 & operator+=(const Vector4 & v)
	{
		x += v.x;
		y += v.y;
		z += v.z;
		w += v.w;
		return *this;
	}
	///Multiply assignment operator
	Vector4 & operator*=(float f)
	{
		x *= f;
		y *= f;
		z *= f;
		w *= f;
		return *this;
	}

	///Multiply assignment operator
	Vector4 & operator*=(const Vector4 & v)
	{
		x *= v.x;
		y *= v.y;
		z *= v.z;
		w *= v.w;
		return *this;
	}

	///Divide assignment operator
	Vector4 & operator/=(float f)
	{
		x /= f;
		y /= f;
		z /= f;
		w /= f;
		return *this;
	}

	///Equal operator
	bool operator==(const Vector4 & v) const
	{
		return (x == v.x) && ( y == v.y) && (z == v.z) && (w == v.w);
	}

	///Notequal operator
	bool operator!=(const Vector4 & v) const
	{
		return (x != v.x) || ( y != v.y) || (z != v.z) || (w != v.w);
	}

	///Get dot product
	float Dot(const Vector4 & v) const
	{
		return x * v.x + y * v.y + z * v.z + w * v.w;
	}

	///Get normalized vector
	Vector4 Normalize() const
	{
		float a = 1.0f/Length();
		return operator*(a);
	}

	///Get squared length
	float LengthSq() const
	{
		return x*x + y*y + z*z + w*w;
	}

	///Get length
	float Length() const
	{
		return sqrtf(x*x + y*y + z*z + w*w);
	}

	///quaternion to axis-angle
	Vector4 QuaternionToAxisAngle() const
	{
		Vector4 result;
		double omega, s, l;
		l = Length();
		if (l == 0.0) {
			result.w = 1.0;	
			result.x = result.y = result.z = 0.0;
		}
		else {
			result = *this*(1.0f/l);
		}
		omega = acos(result.w);
		result.w = (float)(2.0*omega);
		s = sin(omega);	
		if (fabs(s) > 0.000001f)
		{
			result.x /= s;
			result.y /= s;
			result.z /= s;
		}
		else
			result.x = result.y = result.z = 0.0;
		return result;
	}

    void QuaternionNormalize()
    {
        float n = sqrt(x*x + y*y + z*z + w*w);
        x /= n;
        y /= n;
        z /= n;
        w /= n;
    }

    void QuaternionAxisAngleToQuat(const Vector3 &axis, float angleRad)
    {
        x = axis.x * sinf(angleRad / 2);
        y = axis.y * sinf(angleRad / 2);
        z = axis.z * sinf(angleRad / 2);

        w = cosf(angleRad / 2);
    }

	///Get normalized plane
	Vector4 PlaneNormalize() const
	{
		float imag = 1.0f / sqrtf(x*x + y*y + z*z);
		return Vector4(x * imag, y * imag, z * imag, w * imag);
	}

	///Compute Plane-Plane intersection
	bool PlanePlaneIntersection(const Vector4 &plane, Vector3 &norm, Vector3 &point) const
	{
		if (x*plane.x+y*plane.y+z*plane.z==0.0f)
		{
			norm=point=Vector3(FLT_MAX, FLT_MAX, FLT_MAX);//planes are parallel, what should be the result?
			return false;
		}
		norm=Vector3(y*plane.z-z*plane.y, z*plane.x-x*plane.z, x*plane.y-y*plane.x);
		if (((norm.x==0) && (norm.y==0) && (norm.z==0)) || _isnan(norm.x) || _isnan(norm.y) || _isnan(norm.z))
		{
			norm=point=Vector3(FLT_MAX, FLT_MAX, FLT_MAX);//planes are parallel, what should be the result?
			return false;
		}
		if ((fabs(norm.x)>fabs(norm.y)) && (fabs(norm.x)>fabs(norm.z))) //largest is X
		{
			float jm=y*plane.z-plane.y*z;
			point=Vector3(0.0f, (z*plane.w-plane.z*w)/jm, (plane.y*w-y*plane.w)/jm);
		}
		else if (fabs(norm.y)>fabs(norm.z)) //largest is Y
		{
			float jm=x*plane.z-plane.x*z;
			point=Vector3((z*plane.w-plane.z*w)/jm, 0.0f, (plane.x*w-x*plane.w)/jm);
		}
		else //largest is Z
		{
			float jm=x*plane.y-plane.x*y;
			point=Vector3((y*plane.w-plane.y*w)/jm, (plane.x*w-x*plane.w)/jm, 0.0f);
		}
		return true;
	}

	//Compute plane-line intersection, it return dot product of plane norm with line norm
	float PlaneLineIntersection(const Vector3 &lnorm, const Vector3 &lpoint, Vector3 &point) const
	{
		Vector3 planeNorm(x, y, z);
		float dot=planeNorm.Dot(lnorm);
		if (dot==0.0f) //the line is parallel to the plane
		{
			if (PlaneDotCoord(lpoint)==0)
				point=lpoint;//the line is on plane, so any point from line can be the result
			else
				point=Vector3(FLT_MAX, FLT_MAX, FLT_MAX);//what should be the result?
			return dot;
		}
		float s = -(planeNorm.Dot(lpoint)+w)/dot;
		point=lpoint+s*lnorm;
		return dot;
	}
	//TriangleIntersection
	//http://stackoverflow.com/questions/3142469/determining-the-intersection-of-a-triangle-and-a-plane

	/// Computes the plane-line intersection in homogeneous coordinates. Line is defined by two its points, v1 and v2.
	Vector4 PlaneLineIntersectionHomogeneous(const Vector4 &v1, const Vector4 &v2) const
	{
		// We can derive that given d1 = dot(p,v1) and d2 = dot(p,v2)
		// result = v1 + (v2-v1) * d1/(d1-d2) = ... = (v2d1 - v1d2) / (d1-d2) = ... (in homogeneous coordinates) ... = v2d1 - v1d2
		// Fascinating, isn't it? No division, no epsilon, and always correct :-).
		return v2*Dot(v1) - v1*Dot(v2);
	}

	///Get plane from point and normal
	static Vector4 PlaneFromPointNorm(const Vector3 & point, const Vector3 & normal)
	{
		return Vector4(normal.x, normal.y, normal.z, -normal.Dot(point));
	}

	///Get plane from three points
	static Vector4 PlaneFromPoints(const Vector3 & p1, const Vector3 & p2, const Vector3 & p3)
	{
		return PlaneFromPointNorm(p1, ((p2 - p1).Cross(p3 - p1)).Normalize());
	}

	///Get point distance from plane, the plane must be normalized to get really distance, the distance is signed - it is positive on the same side like is normal vector and negative on the other side
	float PlaneDotCoord(const Vector3 & p) const
	{
		return x * p.x + y * p.y + z * p.z + w;
	}

	/// Clips given polygon using Sutherland-Hodgman algorithm (https://en.wikipedia.org/wiki/Sutherland-Hodgman_algorithm)
	/// Works with homogeneous coordinates.
	void PlaneClipPolygonHomogeneous(std::vector<Vector4> &out_polygon, const std::vector<Vector4> &in_polygon) const;

	/// Clips given polygon with multiple planes, using Sutherland-Hodgman algorithm (https://en.wikipedia.org/wiki/Sutherland-Hodgman_algorithm)
	/// Works with homogeneous coordinates.
	static void PlaneClipPolygonHomogeneous(std::vector<Vector4> &polygon, std::vector<Vector4> &tmp_array, const std::vector<Vector4> &planes);

	///Get string
	wxString ToString() const
	{
		return wxString(wxT("("))+TextWriter::WriteToWxString(_v, 4, wxT(", "))+wxT(")");
	}

	///Output stream operator
	friend std::ostream & operator<<(std::ostream & os, const Vector4 & v)
	{
		return os << "(" << v.x << ", " << v.y << ", " << v.z << ", " << v.w << ")";
	}

	///Get copy of instance
	Vector4 * Clone() const
	{
		return new Vector4(*this);
	}

	// Per component multiply and divide
	static inline Vector4 MultiplyComp(const Vector4 &a, const Vector4 &b)
	{
		return Vector4(a.x*b.x, a.y*b.y, a.z*b.z, a.w*b.w);
	}
	static inline Vector4 DivideComp(const Vector4 &a, const Vector4 &b)
	{
		return Vector4(a.x/b.x, a.y/b.y, a.z/b.z, a.w/b.w);
	}
	// Per component max and min
	static inline Vector4 MaxComp(const Vector4 &a, const Vector4 &b)
	{
		return Vector4(__max(a.x, b.x), __max(a.y, b.y), __max(a.z, b.z), __max(a.w, b.w));
	}
	static inline Vector4 MinComp(const Vector4 &a, const Vector4 &b)
	{
		return Vector4(__min(a.x, b.x), __min(a.y, b.y), __min(a.z, b.z), __min(a.w, b.w));
	}

	// Returns Vector4(x,x,x,x)
	static inline Vector4 Replicate(float x)	{	return Vector4(x, x, x, x);		}
};

inline Vector2::Vector2(const Vector3 &xy): x(xy.x), y(xy.y) {}
inline Vector2::Vector2(const Vector4 &xy): x(xy.x), y(xy.y) {}
inline Vector3::Vector3(const Vector2 &xy, float z): x(xy.x), y(xy.y), z(z) {}
inline Vector3::Vector3(float x, const Vector2 &yz): x(x), y(yz.x), z(yz.y) {}
inline Vector3::Vector3(const Vector4 &xyz): x(xyz.x), y(xyz.y), z(xyz.z) {}
inline Vector4::Vector4(const Vector2 &xy, float z, float w): x(xy.x), y(xy.y), z(z), w(w) {}
inline Vector4::Vector4(float x, const Vector2 &yz, float w): x(x), y(yz.x), z(yz.y), w(w) {}
inline Vector4::Vector4(float x, float y, const Vector2 &zw): x(x), y(y), z(zw.x), w(zw.y) {}
inline Vector4::Vector4(const Vector2 &xy, const Vector2 &zw): x(xy.x), y(xy.y), z(zw.x), w(zw.y) {}
inline Vector4::Vector4(const Vector3 &xyz, float w): x(xyz.x), y(xyz.y), z(xyz.z), w(w) {}
inline Vector4::Vector4(float x, const Vector3 &yzw): x(x), y(yzw.x), z(yzw.y), w(yzw.z) {}



///4x4 matrix
/// JC: This is how matrix computation works in VRUT
///   To apply a transformation on a vector V_in, think about it as:
///     V_after_transformation = V_before_transtormation * Matrix.
///   This means the last four components (i.e. _m41, _m42, _m43; or _m[12], _m[13], _m[14]) contains translation
///   part of the matrix.
///   To obtain vector Vp after all transformations (including projection) from V in model coordinates, compute:
///     Vp = V * my_local * parent_local * his_parent_local * ... * view * projection
///   which is the same as:
///     Vp = V * world * view * projection
///   Matrix::TransformCoord(V) computes:
///     result = V * Matrix
///   Beware of Multiply(Matrix d, Matrix c), which actually computes:
///     result = c * d
///   instead of d * c, which can be confusing. I personnaly recommend to avoid using Multiply function.
struct VRUT_CORE_API Matrix
{
	///some constants
	static const Matrix Unit;

	union
	{
		struct
		{
			///Components
			float	_m11, _m12, _m13, _m14,
					_m21, _m22, _m23, _m24,
					_m31, _m32, _m33, _m34,
					_m41, _m42, _m43, _m44;
		};
		///Components
		float _m[16];
	};

	///Constructor
	Matrix()
	{
		memset(_m, 0, sizeof(_m));
		_m11 = _m22 = _m33 = _m44 = 1;
	}

	///Constructor
	Matrix(float _11, float _12, float _13, float _14,
		float _21, float _22, float _23, float _24,
		float _31, float _32, float _33, float _34,
		float _41, float _42, float _43, float _44)
	:_m11(_11), _m12(_12), _m13(_13), _m14(_14),
	_m21(_21), _m22(_22), _m23(_23), _m24(_24),
	_m31(_31), _m32(_32), _m33(_33), _m34(_34),
	_m41(_41), _m42(_42), _m43(_43), _m44(_44)	{}

	///Constructor
	explicit Matrix(const float * m)
	{
		memcpy(_m, m, sizeof(float)*16);
		/*for (int i = 0; i < 16; i++)
			_m[i] = m[i];*/
	}

	///Copy constructor
	Matrix(const Matrix & m)
	{
		memcpy(_m, m._m, sizeof(float)*16);
		/*for (int i = 0; i < 16; i++)
			_m[i] = m._m[i];*/
	}

	///Assignment operator
	Matrix & operator=(const Matrix & mat)
	{
		memcpy(_m, mat._m, sizeof(float)*16);
		/*for (int i = 0; i < 16; i++)
			_m[i] = mat._m[i];*/
		return *this;
	}

	///Add operator
	Matrix operator+(const Matrix & c) const
	{
		Matrix ret;
		float * r = ret._m;
		const float * m1 = _m;
		const float * m2 = c._m;
		for (int i = 0; i < 16; i++)
			*r++ = *m1++ + *m2++;
		return ret;
	}

	///Add assignment operator
	Matrix & operator+=(const Matrix & c)
	{
		float * m1 = _m;
		const float * m2 = c._m;
		for (int i = 0; i < 16; i++)
			*m1++ += *m2++;
		return *this;
	}

	///Subtract operator
	Matrix operator-(const Matrix & c) const
	{
		Matrix ret;
		float * r = ret._m;
		const float * m1 = _m;
		const float * m2 = c._m;
		for (int i = 0; i < 16; i++)
			*r++ = *m1++ - *m2++;
		return ret;
	}

	///Subtract assignment operator
	Matrix & operator-=(const Matrix & c)
	{
		float * m1 = _m;
		const float * m2 = c._m;
		for (int i = 0; i < 16; i++)
			*m1++ -= *m2++;
		return *this;
	}

	///Multiply operator
	Matrix operator*(const Matrix & c) const
	{
		Matrix ret;
		ret._m11 = _m11*c._m11+_m12*c._m21+_m13*c._m31+_m14*c._m41;
		ret._m12 = _m11*c._m12+_m12*c._m22+_m13*c._m32+_m14*c._m42;
		ret._m13 = _m11*c._m13+_m12*c._m23+_m13*c._m33+_m14*c._m43;
		ret._m14 = _m11*c._m14+_m12*c._m24+_m13*c._m34+_m14*c._m44;
		ret._m21 = _m21*c._m11+_m22*c._m21+_m23*c._m31+_m24*c._m41;
		ret._m22 = _m21*c._m12+_m22*c._m22+_m23*c._m32+_m24*c._m42;
		ret._m23 = _m21*c._m13+_m22*c._m23+_m23*c._m33+_m24*c._m43;
		ret._m24 = _m21*c._m14+_m22*c._m24+_m23*c._m34+_m24*c._m44;
		ret._m31 = _m31*c._m11+_m32*c._m21+_m33*c._m31+_m34*c._m41;
		ret._m32 = _m31*c._m12+_m32*c._m22+_m33*c._m32+_m34*c._m42;
		ret._m33 = _m31*c._m13+_m32*c._m23+_m33*c._m33+_m34*c._m43;
		ret._m34 = _m31*c._m14+_m32*c._m24+_m33*c._m34+_m34*c._m44;
		ret._m41 = _m41*c._m11+_m42*c._m21+_m43*c._m31+_m44*c._m41;
		ret._m42 = _m41*c._m12+_m42*c._m22+_m43*c._m32+_m44*c._m42;
		ret._m43 = _m41*c._m13+_m42*c._m23+_m43*c._m33+_m44*c._m43;
		ret._m44 = _m41*c._m14+_m42*c._m24+_m43*c._m34+_m44*c._m44;
		return ret;
	}

	///Multiply operator
	/// JC: Removing, the order is opposite to the one in "operator *", which is used more frequently.
	///		Use "operator *" instead, or change the order of the parameters (and update all places where
	///		this function is used - probably nowhere).
	/*
	friend Matrix Multiply(const Matrix & d, const Matrix & c) 
	{
		Matrix ret;
		ret._m11 = c._m11*d._m11+c._m12*d._m21+c._m13*d._m31+c._m14*d._m41;
		ret._m12 = c._m11*d._m12+c._m12*d._m22+c._m13*d._m32+c._m14*d._m42;
		ret._m13 = c._m11*d._m13+c._m12*d._m23+c._m13*d._m33+c._m14*d._m43;
		ret._m14 = c._m11*d._m14+c._m12*d._m24+c._m13*d._m34+c._m14*d._m44;
		ret._m21 = c._m21*d._m11+c._m22*d._m21+c._m23*d._m31+c._m24*d._m41;
		ret._m22 = c._m21*d._m12+c._m22*d._m22+c._m23*d._m32+c._m24*d._m42;
		ret._m23 = c._m21*d._m13+c._m22*d._m23+c._m23*d._m33+c._m24*d._m43;
		ret._m24 = c._m21*d._m14+c._m22*d._m24+c._m23*d._m34+c._m24*d._m44;
		ret._m31 = c._m31*d._m11+c._m32*d._m21+c._m33*d._m31+c._m34*d._m41;
		ret._m32 = c._m31*d._m12+c._m32*d._m22+c._m33*d._m32+c._m34*d._m42;
		ret._m33 = c._m31*d._m13+c._m32*d._m23+c._m33*d._m33+c._m34*d._m43;
		ret._m34 = c._m31*d._m14+c._m32*d._m24+c._m33*d._m34+c._m34*d._m44;
		ret._m41 = c._m41*d._m11+c._m42*d._m21+c._m43*d._m31+c._m44*d._m41;
		ret._m42 = c._m41*d._m12+c._m42*d._m22+c._m43*d._m32+c._m44*d._m42;
		ret._m43 = c._m41*d._m13+c._m42*d._m23+c._m43*d._m33+c._m44*d._m43;
		ret._m44 = c._m41*d._m14+c._m42*d._m24+c._m43*d._m34+c._m44*d._m44;
		return ret;
	}*/

	///Scalar multiply operator
	Matrix operator*(float f) const
	{
		return Matrix(	f*_m11, f*_m12, f*_m13, f*_m14,
						f*_m21, f*_m22, f*_m23, f*_m24,
						f*_m31, f*_m32, f*_m33, f*_m34,
						f*_m41, f*_m42, f*_m43, f*_m44 );
	}

	///Scalar multiply operator
	friend Matrix operator*(float f, const Matrix & m)
	{
		return Matrix(	f*m._m11, f*m._m12, f*m._m13, f*m._m14,
						f*m._m21, f*m._m22, f*m._m23, f*m._m24,
						f*m._m31, f*m._m32, f*m._m33, f*m._m34,
						f*m._m41, f*m._m42, f*m._m43, f*m._m44 );
	}

	///Assignment multiply operator
	Matrix & operator*=(const Matrix & m)
	{
		operator=(*this * m);
		return *this;
	}

	///Assignment scalar multiply operator
	Matrix & operator*=(float f)
	{
		for (int i = 0; i < 16; i++)
			_m[i] *= f;
		return *this;
	}

	///Comparison operator
	bool operator==(const Matrix & m) const
	{
		for (int i=0; i<16; i++)
			if (_m[i]!=m._m[i])
				return false;
		return true;
	}

	///Comparison operator
	bool operator!=(const Matrix & m) const
	{
		for (int i=0; i<16; i++)
			if (_m[i]!=m._m[i])
				return true;
		return false;
	}

	void SetRow(int row, const Vector4 & v)
	{
		memcpy(_m+row*4, v._v, sizeof(float)*4);
	}

	void SetRow(int row, const Vector3 & v, float f = 0.0f)
	{
		memcpy(_m+row*4, v._v, sizeof(float)*3);
		_m[row*4+3]=f;
	}

	void SetColumn(int col, const Vector4 & v)
	{
		for (int i = 0; i < 4; ++i)
			_m[i*4+col] = v[i];
	}

	void SetColumn(int col, float f1, float f2, float f3, float f4 = 0.0f)
	{
		_m[col] = f1;
		_m[4+col] = f2;
		_m[8+col] = f3;
		_m[12+col] = f4;
	}

	void SetColumn(int col, const Vector3 & v, float f = 0.0f)
	{
		for (int i = 0; i < 3; ++i)
			_m[i*4+col] = v[i];
		_m[12+col] = f;
	}

	///Test if it is unit matrix
	bool IsUnit() const
	{
		for (int i=0; i<4; i++)
			for (int j=0; j<4; j++)
				if (_m[i*4+j]!=((i==j)?1:0))
					return false;
		return true;
	}

	bool IsMirror() const
	{
		return (Vector3(_m11, _m12, _m13).Cross(Vector3(_m21, _m22, _m23)).Dot(Vector3(_m31, _m32, _m33))<0.0);
	}

	///Extract position from world transformation matrix
	Vector3 ExtractTranslation() const
	{
		return Vector3(_m41, _m42, _m43);
	}

	///Get matrix preserving rotation (and scale) but with no translation
	Matrix ExtractRotation() const
	{
		Matrix mat(_m);
		mat._m41 = mat._m42 = mat._m43 = mat._m14 = mat._m24 = mat._m34 = 0;
		mat._m44 = 1;
		return mat;
	}

	///Transform 2D coordinate (assuming w = 1) with transformation matrix, usefull for texture coordinates
	Vector2 TransformCoord(const Vector2 &v) const
	{
		Vector2 ret;
		ret.x = v.x * _m11 + v.y * _m21 + _m41;
		ret.y = v.x * _m12 + v.y * _m22 + _m42;
		return ret;
	}

	///Transform 3D coordinate (assuming w = 1) with transformation matrix
	Vector3 TransformCoord(const Vector3 & v) const
	{
		Vector3 ret;
		ret.x = v.x * _m11 + v.y * _m21 + v.z * _m31 + _m41;
		ret.y = v.x * _m12 + v.y * _m22 + v.z * _m32 + _m42;
		ret.z = v.x * _m13 + v.y * _m23 + v.z * _m33 + _m43;
//		float w = v.x * _m14 + v.y * _m24 + v.z * _m34 + _m44;
//		if (w != 1)
//			return ret;
		return ret;
	}

	///Transforms 4D coordinate
	Vector4 TransformCoord(const Vector4 & v) const
	{
		Vector4 ret;
		ret.x = v.x * _m11 + v.y * _m21 + v.z * _m31 + v.w * _m41;
		ret.y = v.x * _m12 + v.y * _m22 + v.z * _m32 + v.w * _m42;
		ret.z = v.x * _m13 + v.y * _m23 + v.z * _m33 + v.w * _m43;
		ret.w = v.x * _m14 + v.y * _m24 + v.z * _m34 + v.w * _m44;
		return ret;
	}

	///Transform 3D coordinate with transformation matrix
	Vector3 TransformCoordHom(const Vector3 & v) const {
		Vector3 ret;
		ret.x = v.x * _m11 + v.y * _m21 + v.z * _m31 + _m41;
		ret.y = v.x * _m12 + v.y * _m22 + v.z * _m32 + _m42;
		ret.z = v.x * _m13 + v.y * _m23 + v.z * _m33 + _m43;
		float w = v.x * _m14 + v.y * _m24 + v.z * _m34 + _m44;
		return ret/w;
	}

	///Transform 3D direction with transformation matrix
	Vector3 TransformNormal(const Vector3 & v) const
	{
		Vector3 ret;
		ret.x = v.x * _m11 + v.y * _m21 + v.z * _m31;
		ret.y = v.x * _m12 + v.y * _m22 + v.z * _m32;
		ret.z = v.x * _m13 + v.y * _m23 + v.z * _m33;
		return ret;
	}

	///Transform 4D direction with transformation matrix (assumes w = 0)
	Vector3 TransformNormal(const Vector4 & v) const
	{
		Vector3 ret;
		ret.x = v.x * _m11 + v.y * _m21 + v.z * _m31;
		ret.y = v.x * _m12 + v.y * _m22 + v.z * _m32;
		ret.z = v.x * _m13 + v.y * _m23 + v.z * _m33;
		return ret;
	}

	/// Array versions of Transform methods. Usage:
	///
	/// Matrix mat;
	/// vector<Vector3> input, output, inout;
	/// mat.TransformCoordArray<Vector3>(input.begin(), input.end(), output.begin());	// Input and output are different
	/// mat.TransformCoordArray<Vector3>(inout.begin(), inout.end(), inout.begin());	// Input and output are the same (in-place transformation)
	template <class T, class InputIter, class OutputIter>
	void TransformCoordArray(InputIter in_begin, InputIter in_end, OutputIter out_begin) const
	{
		std::transform(in_begin, in_end, out_begin, [this] (const T &v) { return TransformCoord(v); } );
	}
	template <class T, class InputIter, class OutputIter>
	void TransformCoordHomArray(InputIter in_begin, InputIter in_end, OutputIter out_begin) const
	{
		std::transform(in_begin, in_end, out_begin, [this] (const T &v) { return TransformCoordHom(v); } );
	}
	template <class T, class InputIter, class OutputIter>
	void TransformNormalArray(InputIter in_begin, InputIter in_end, OutputIter out_begin) const
	{
		std::transform(in_begin, in_end, out_begin, [this] (const T &v) { return TransformNormal(v); } );
	}

	///Get inverted matrix (it works only for matrix that have orthogonal 3x3 upper left part and arbitrary translation part, i.e. only rotation, reflection, transposition and their combinations)
	Matrix InverseOrto() const
	{
		Matrix ret;

		ret._m[0] = _m[0];
		ret._m[1] = _m[4];
		ret._m[2] = _m[8];
		ret._m[4] = _m[1];
		ret._m[5] = _m[5];
		ret._m[6] = _m[9];
		ret._m[8] = _m[2];
		ret._m[9] = _m[6];
		ret._m[10] = _m[10];

#define INVERSE_ORTHO_DOUBLE 0

#if INVERSE_ORTHO_DOUBLE
		// try to compute the translation part in higher precision
		ret._m[12] = (float)(((double)ret._m[0])*(-(double)_m[12]) + ((double)ret._m[4])*-((double)_m[13]) + ((double)ret._m[8])*-((double)_m[14]));
		ret._m[13] = (float)(((double)ret._m[1])*(-(double)_m[12]) + ((double)ret._m[5])*-((double)_m[13]) + ((double)ret._m[9])*-((double)_m[14]));
		ret._m[14] = (float)(((double)ret._m[2])*(-(double)_m[12]) + ((double)ret._m[6])*-((double)_m[13]) + ((double)ret._m[10])*-((double)_m[14]));

#else
		ret._m[12] = ret._m[0]*-_m[12]+ret._m[4]*-_m[13]+ret._m[8]*-_m[14];
		ret._m[13] = ret._m[1]*-_m[12]+ret._m[5]*-_m[13]+ret._m[9]*-_m[14];
		ret._m[14] = ret._m[2]*-_m[12]+ret._m[6]*-_m[13]+ret._m[10]*-_m[14];
#endif
		
		ret._m[3] = 0.0f;
		ret._m[7] = 0.0f;
		ret._m[11] = 0.0f;
		ret._m[15] = 1.0f;

		return ret;
	}

	///InverseOrto matrix computation gauss_jacobiho method .. from N.R. in C
	///Attention: In case of singular matrix the result is undefined
	Matrix Inverse() const
	{
		Matrix ret(*this);
		ret.InvertThisPrecise();
		return ret;
	}

	///InverseOrto matrix computation gauss_jacobiho method .. from N.R. in C
	///If matrix is regular = computation successful = returns true
	///In case of singular matrix returns false
	bool InvertThisPrecise()
	{
		int indxc[4],indxr[4],ipiv[4];
		int i,icol,irow,j,k,l,ll,n;
		float dum, temp;
		// satisfy the compiler
		icol = irow = 0;
		// the size of the matrix
		n = 4;

		for ( j = 0 ; j < n ; j++) /* zero pivots */
			ipiv[j] = 0;
		for (i = 0; i < n; i++)
		{
			float big = 0.0;
			for (j = 0; j < n; j++)
			{
				if (ipiv[j] != 1)
				{
					for (k = 0; k < n; k++)
					{
						if (ipiv[k] == 0)
						{
							if (fabs(_m[4 * k + j]) >= big)
							{
								big = fabs(_m[4 * k + j]);
								irow = j;
								icol = k;
							}
						}
						else if (ipiv[k] > 1)
							return false; /* singular matrix */
					}
				}
			}
			++(ipiv[icol]);
			if (irow != icol)
			{
				for (l = 0; l < n; l++)
				{
					temp = _m[4 * l + icol];
					_m[4 * l + icol] = _m[4 * l + irow];
					_m[4 * l + irow] = temp;
				}
			}
			indxr[i] = irow;
			indxc[i] = icol;
			if (_m[4 * icol + icol] == 0.0)
				return false; /* singular matrix */

			float pivinv = 1.0f / _m[4 * icol + icol];
			_m[4 * icol + icol] = 1.0;
			for (l = 0; l < n; l++)
				_m[4 * l + icol] = _m[4 * l + icol] * pivinv;
			for (ll = 0; ll < n; ll++)
			{
				if (ll != icol)
				{
					dum = _m[4 * icol + ll];
					_m[4 * icol + ll] = 0.0;
					for (l = 0; l < n; l++)
						_m[4 * l + ll] = _m[4 * l + ll] - _m[4 * l + icol] * dum;
				}
			}
		}
		for ( l = n; l--; )
		{
			if (indxr[l] != indxc[l])
				for ( k = 0; k<n ; k++)
				{
					temp = _m[4*indxr[l]+k];
					_m[4*indxr[l]+k] = _m[4*indxc[l]+k];
					_m[4*indxc[l]+k] = temp;
				}
		}
		return true; // matrix is regular .. inversion was successful
	}

	///Get the normal transformation matrix (equivalent to gl_NormalMatrix in GLSL)
	///This is calculated as the transpose of the inverse of the upper left 3x3 submatrix
	///Author: ML
	Matrix NormalMatrix() const{
		//we are computing the upper left 3x3 submatrix of a column-major matrix, so we'll represent it as three columnt vectors (starting at the top)
		Vector3 v0(this->_m), v1(this->_m+4), v2(this->_m+8);

		//get a fresh matrix
		Matrix ret;

		//a vector to store the intermediate results
		Vector3 x;

		//determinant of the upper 3x3 submatrix, obtained as a triple product
		float det=v0.Dot(v1.Cross(v2));

		//cross products will give us the individual rows of the inverted matrix.
		//the result of a cross product of two column vectors is a column vector that we should transpose to get a row of the inverted matrix (after division by the determinant)
		//however, to transform normals, this has to be transposed again, so we will forego transpositions altogether
		x=v1.Cross(v2) /det;

		//this will now be stored in the resulting matrix
		for (int i=0; i<3; i++)
			ret._m[i]=x._v[i];

		//rinse and repeat
		x=v2.Cross(v0) /det;

		for (int i=0; i<3; i++)
			ret._m[i+4]=x._v[i];

		x=v0.Cross(v1) /det;

		for (int i=0; i<3; i++)
			ret._m[i+8]=x._v[i];

		//return the result
		return ret;
	}

	///Get 3x3 determinant
	float Det3x3() const
	{
		return _m11*_m22*_m33 + _m12*_m23*_m31
			+ _m13*_m21*_m32 - _m13*_m22*_m31
			- _m11*_m23*_m32 - _m12*_m21*_m33;
	}

	///Get transposed matrix
	Matrix Transpose() const
	{
		Matrix mat;
		for (int i = 0; i < 4; i++)
			for (int j = 0; j < 4; j++)
				mat._m[4*j+i] = _m[4*i+j];
		return mat;
	}

	///Translation matrix
	///TODO: check multiply order
	Matrix & Translate(const Vector3 & v)
	{
		*this *= Translation(v);
		return *this;
	}

	///Scale matrix
	///TODO: check multiply order
	Matrix & Scale(const Vector3 & v)
	{
		*this *= ScaleMat(v);
		return *this;
	}

	///Convert matrix to Euler angles
	///It works only for pure rotation matrix!
	//TODO: check correctness in all special cases
	//TODO: update to handle arbitrary matrix 
	
	void ToEuler(float &x, float &y, float &z) const
	{
		//https://github.com/jzrake/glm/commit/d3313421c664db5bd1b672d39ba3faec0d430117
		float T1 = atan2(_m32, _m33);
		float C2 = sqrt(_m11*_m11 + _m21*_m21);
		float T2 = atan2(-_m31, C2);
		float S1 = sin(T1);
		float C1 = cos(T1);
		float T3 = atan2(S1*_m13 - C1*_m12, C1*_m22 - S1*_m23);
		x = -T1;
		y = -T2;
		z = -T3;
		/*
		//http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToEuler/index.htm
		if((_m12 > 0.998))
		{
			x = - atan2( - _m31, _m33);
			z = 0.0;
			y = 3.14159265358979f * 0.5f;
		}
		else if((_m12 < -0.998))
		{
			x = - atan2( - _m31, _m33);
			z = 0.0;
			y = - 3.14159265358979f * 0.5f;
		}
		else
		{
			x = (atan2( - _m13, _m11));			
			z = (atan2( - _m32, _m22));
			y = asin(_m12);
		}*/
		//http://www.staff.city.ac.uk/~sbbh653/publications/euler.pdf
	}

	///Convert matrix to Axis-Angle angles, used algorithm from http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToAngle/index.htm
	///It works only for pure rotation matrix!
	//TODO: check correctness in all special cases
	//TODO: update to handle arbitrary matrix 
	void ToAxisAngle(Vector3 &axis, float &angle) const
	{
		float epsilon = 0.01f; // margin to allow for rounding errors
		float epsilon2 = 0.1f; // margin to distinguish between 0 and 180 degrees
		// optional check that input is pure rotation, 'isRotationMatrix' is defined at:
		// http://www.euclideanspace.com/maths/algebra/matrix/orthogonal/rotation/
		//assert isRotationMatrix(m) : "not valid rotation matrix" ;// for debugging
		if ((fabs(_m12-_m21)<EPSILON) && (fabs(_m13-_m31)<EPSILON) && (fabs(_m23-_m32)<EPSILON)) {
				// singularity found
				// first check for identity matrix which must have +1 for all terms
				//  in leading diagonal and zero in other terms
				if ((fabs(_m12+_m21) < epsilon2) && (fabs(_m13+_m31) < epsilon2) && (fabs(_m23+_m32) < epsilon2) && (fabs(_m11+_m22+_m33-3) < epsilon2)) {
						// this singularity is identity matrix so angle = 0 and arbitrary axis
						axis=Vector3(0, 1, 0);
						angle=0;
						return;
				}
				// otherwise this singularity is angle = 180
				angle = 3.14159f;
				angle = -angle;			// Matrices at www.euclideanspace.com are transposed, this is actually an inverse rotation
				float xx = (_m11+1)/2;
				float yy = (_m22+1)/2;
				float zz = (_m33+1)/2;
				float xy = (_m12+_m21)/4;
				float xz = (_m13+_m31)/4;
				float yz = (_m23+_m32)/4;
				if ((xx > yy) && (xx > zz)) { // _m11 is the largest diagonal term
					if (xx< epsilon) {
						axis=Vector3(0.0f, 0.7071f, 0.7071f);
					} else {
						xx=sqrt(xx);
						axis=Vector3(xx, xy/xx, xz/xx);
					}
				} else if (yy > zz) { // _m22 is the largest diagonal term
					if (yy< epsilon) {
						axis=Vector3(0.7071f, 0.0f, 0.7071f);
					} else {
						yy=sqrt(yy);
						axis=Vector3(xy/yy, yy, yz/yy);
					}	
				} else { // _m33 is the largest diagonal term so base result on this
					if (zz< epsilon) {
						axis=Vector3(0.7071f, 0.7071f, 0.0f);
					} else {
						zz = sqrt(zz);
						axis=Vector3(xz/zz, yz/zz, zz);
					}
				}
				return;// return 180 deg rotation
		}
		// as we have reached here there are no singularities so we can handle normally
//		float s = sqrt((_m32 - _m23)*(_m32 - _m23) + (_m13 - _m31)*(_m13 - _m31) + (_m21 - _m12)*(_m21 - _m12)); // used to normalise
//		if (fabs(s) < 0.001) s=1; 
		// prevent divide by zero, should not happen if matrix is orthogonal and should be
		// caught by singularity test above, but I've left it in just in case
		float tmp=(_m11 + _m22 + _m33 - 1)/2;
		if (tmp>=1.0f)
			angle=0.0f;
		else if (tmp<=-1.0f)
			angle=3.14159265358979323846f;
		else
			angle = acos(tmp);
		angle = -angle;			// Matrices at www.euclideanspace.com are transposed, this is actually an inverse rotation
		axis = Vector3(_m32 - _m23, _m13 - _m31, _m21 - _m12).Normalize();
	}

	///Get scale matrix
	static Matrix ScaleMat(const Vector3 & v)
	{
		Matrix ret;
		ret._m11 = v.x;
		ret._m22 = v.y;
		ret._m33 = v.z;
		return ret;
	}

	///Get translation matrix
	static Matrix Translation(const Vector3 & v)
	{
		Matrix mat;
		mat._m41 = v.x;
		mat._m42 = v.y;
		mat._m43 = v.z;
		return mat;
	}

	///Get rotation matrix around the X axis
	static Matrix RotationX(float angle)
	{
		Matrix M;
		float Cosine = cos(angle);
		float Sine = sin(angle);
		M._m22 = Cosine;
		M._m23 = Sine;
		M._m32 = -Sine;
		M._m33 = Cosine;
		return M;
	}

	///Get rotation matrix around the Y axis
	static Matrix RotationY(float angle)
	{
		Matrix M;
		float Cosine = cos(angle);
		float Sine = sin(angle);
		M._m11 = Cosine;
		M._m13 = -Sine;
		M._m31 = Sine;
		M._m33 = Cosine;
		return M;
	}

	///Get rotation matrix around the Z axis
	static Matrix RotationZ(float angle)
	{
		Matrix M;
		float Cosine = cos(angle);
		float Sine = sin(angle);
		M._m11 = Cosine;
		M._m12 = Sine;
		M._m21 = -Sine;
		M._m22 = Cosine;
		return M;
	}

	///A method to create a view matrix for camera so that it corresponds to a displaying surface in the
	///"flying carpet" model.
	///
	///Effectively creates a tangent-bitangent-normal matrix, that will convert given parent camera coordinates
	///into tangential coordinates of the viewing surface, with the tangent (x axis) pointing to the right and bitangent (y axis)
	///pointing upwards, as is conventional in the standard viewing surface (device coordinates).
	///
	///After this transformation, combined with the parent camera transformation, the scene should be in this camera's view space.
	///
	/// NOTE: It is conventional in VRUT to store not the camera view matrix (equivalent to gluLookAt) in the node, but rather a
	/// regular transformation matrix representing the camera's position and orientation. Render modules will always invert the camera world
	/// transformation matrix before rendering, so we have to invert it beforehand to make it work.
	///
	///\param[in] min The tracking coordinates of the lower left-hand corner of the viewing surface
	///\param[in] max The tracking coordinates of the upper right-hand corner of the viewing surface
	///\param[in] up A vector (in tracking coordinates) pointing upwards on the viewing plane
	static Matrix SetupTrackedCamera(const Vector3 &min, const Vector3 &max, Vector3 up) {
		Vector3 origin=(min+max)/2;
		Vector3 cross=max-min;
		cross=cross.Normalize();

		up=up.Normalize();

		Vector3 normal=cross.Cross(up);
		normal=normal.Normalize();

		Vector3 tangent=up.Cross(normal);
		Vector3 bitangent=normal.Cross(tangent);

		Matrix M;

		M._m11=tangent.x;
		M._m12=tangent.y;
		M._m13=tangent.z;

		M._m21=bitangent.x;
		M._m22=bitangent.y;
		M._m23=bitangent.z;

		M._m31=normal.x;
		M._m32=normal.y;
		M._m33=normal.z;

		M._m44=1;

		M*=(Matrix::Translation(origin));

		return M;
	}

	///Construct a yaw-pitch-roll rotation matrix.	Rotate Yaw
	///radians about the XY axis, rotate Pitch radians in the
	///plane defined by the Yaw rotation, and rotate Roll radians
	///about the axis defined by the previous two angles.
	static Matrix RotationXYZ(float x, float y, float z)
	{
		Matrix M;
		//https://github.com/jzrake/glm/commit/d3313421c664db5bd1b672d39ba3faec0d430117
		float c1 = cos(-x);
		float c2 = cos(-y);
		float c3 = cos(-z);
		float s1 = sin(-x);
		float s2 = sin(-y);
		float s3 = sin(-z);
		
		M._m11 = c2 * c3;
		M._m12 = -c1 * s3 + s1 * s2 * c3;
		M._m13 = s1 * s3 + c1 * s2 * c3;
		M._m21 = c2 * s3;
		M._m22 = c1 * c3 + s1 * s2 * s3;
		M._m23 = -s1 * c3 + c1 * s2 * s3;
		M._m31 = -s2;
		M._m32 = s1 * c2;
		M._m33 = c1 * c2;
		/*
		//http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToEuler/index.htm
		float ch = cos(Yaw);
		float sh = sin(Yaw);
		float ca = cos(Pitch);
		float sa = sin(Pitch);
		float cb = cos(Roll);
		float sb = sin(Roll);

		M._m11 = ch * ca;
		M._m12 = sa;
		M._m13 = - sh * ca;
		M._m21 = - ch * sa * cb + sh * sb;
		M._m22 = ca * cb;
		M._m23 = sh * sa * cb + ch * sb;
		M._m31 = ch * sa * sb + sh * cb;
		M._m32 = - ca * sb;
		M._m33 = - sh * sa * sb + ch * cb;
		*/
		return M;
	}

	/// Constructs a yaw-pitch-roll rotation matrix with data from vector
	static Matrix RotationXYZ(const Vector3 &ypr)
	{
		return RotationXYZ(ypr.x, ypr.y, ypr.z);
	}

	///Construct a rotation of a given angle about a given axis.
	///Derived from Eric Haines's SPD (Standard Procedural
	///Database).
	static Matrix RotationAxis(const Vector3 & axis, float angle)
	{
		Matrix M;
		Vector3 _axis = axis;
		if ((_axis.x != 0.0f) || (_axis.y != 0.0f) || (_axis.z != 0.0f))
			_axis = _axis.Normalize();		// Axis must be normalized

		float cosine = cos(angle);
		float sine = sin(angle);
		float one_minus_cosine = 1.0f - cosine;

		M._m11 = _axis.x * _axis.x + (1.0f - _axis.x * _axis.x) * cosine;
		M._m12 = _axis.x * _axis.y * one_minus_cosine + _axis.z * sine;
		M._m13 = _axis.x * _axis.z * one_minus_cosine - _axis.y * sine;
		M._m14 = 0;

		M._m21 = _axis.y * _axis.x * one_minus_cosine - _axis.z * sine;
		M._m22 = _axis.y * _axis.y + (1.0f - _axis.y * _axis.y) * cosine;
		M._m23 = _axis.y * _axis.z * one_minus_cosine + _axis.x * sine;
		M._m24 = 0;

		M._m31 = _axis.x * _axis.z * one_minus_cosine + _axis.y * sine;
		M._m32 = _axis.y * _axis.z * one_minus_cosine - _axis.x * sine;
		M._m33 = _axis.z * _axis.z + (1.0f - _axis.z * _axis.z) * cosine;
		M._m34 = 0;

		M._m41 = 0;
		M._m42 = 0;
		M._m43 = 0;
		M._m44 = 1;

		return M;
	}


	///Construct the rotation matrix that rotates 'vec1' to 'vec2'
	static Matrix RotationVectors(const Vector3 & vecStart, const Vector3 & vecTo)
	{
		Vector3 vec(vecStart.Cross(vecTo));

		if (vec.Length() > 1e-6f)
		{
			// vector exist, compute angle
			float angle = -acos(vecStart.Dot(vecTo));
			// normalize for sure
			vec = vec.Normalize();
			return RotationAxis(vec, angle);
		}

		Matrix ret;
		// opposite or collinear vectors
		//TODO: solve properly opposite vectors
		if (vecStart.Dot(vecTo) < 0.0f)
			//ret *= -1.0f; // original totaly wrong slution
			//return RotationAxis((vecTo-vecStart).Normalize(), -3.14159f);
		{
			//hack for SeatStitcher
			ret._m11=-1.0f;
			ret._m33=-1.0f;
		}
		return ret;
		/*Matrix mat;
		float angle = acos(vecStart.Dot(vecTo));
		if (angle<1e-6f)
			return mat;
		Vector3 n1;
		if (fabsf(angle-M_PI)<1e-6f)
		{
			Matrix ret;
			ret._m11=-1.0f;
			ret._m33=-1.0f;
			return ret;
			//solution 2
			//float tmp=vecStart.y;
			//vecStart.y=vecStart.z;
			//vecStart.z=-tmp;
			//solution 3
			//n1=Matrix::RotationX(-0.5f).TransformNormal(vecStart);
		}
		return RotationAxis(vecStart.Cross(vecTo).Normalize(), -angle);*/
	}

	///Construct the matrix for planar mirror defined by 'point' and 'normal'
	static Matrix MirrorReflect(const Vector3 & point, const Vector3 & normal)
	{
		float dot=point.Dot(normal);
		Matrix ret;
		ret._m11 = 1-2*normal.x*normal.x;
		ret._m21 =  -2*normal.x*normal.y;
		ret._m31 =  -2*normal.x*normal.z;
		ret._m41 = 2*dot*normal.x;

		ret._m12 =  -2*normal.y*normal.x;
		ret._m22 = 1-2*normal.y*normal.y;
		ret._m32 =  -2*normal.y*normal.z;
		ret._m42 = 2*dot*normal.y;

		ret._m13 =  -2*normal.z*normal.x;
		ret._m23 =  -2*normal.z*normal.y;
		ret._m33 = 1-2*normal.z*normal.z;
		ret._m43 = 2*dot*normal.z;

		ret._m14 = 0;
		ret._m24 = 0;
		ret._m34 = 0;
		ret._m44 = 1;
		return ret;
	}

	static Matrix LookAt(const Vector3& eye, const Vector3& target, const Vector3& up)
	{
		//OpenGL implementation. This lookAt implementation can be verified against this.
		/*
		glPushMatrix();
		float debug[16];
		glLoadIdentity();
		gluLookAt(eye.x, eye.y, eye.z, target.x, target.y, target.z, up.x, up.y, up.z);
		glGetFloatv(GL_PROJECTION_MATRIX, debug);
		glPopMatrix();
		memcpy(localTransMatrix._m, debug, 16 * sizeof(float));
		localTransMatrix.InvertThisPrecise();
		*/

		Vector3 viewDirection(target - eye);
		viewDirection = viewDirection.Normalize();
		Vector3 sideDirection(viewDirection.Cross(up).Normalize());
		Vector3 newUpDirection(sideDirection.Cross(viewDirection).Normalize());
		Matrix localTransMatrix = Matrix(
			sideDirection.x, newUpDirection.x, -viewDirection.x, 0,
			sideDirection.y, newUpDirection.y, -viewDirection.y, 0,
			sideDirection.z, newUpDirection.z, -viewDirection.z, 0,
			0, 0, 0, 1
			);
		Matrix pos;
		pos._m41 = -eye.x;
		pos._m42 = -eye.y;
		pos._m43 = -eye.z;
		localTransMatrix = pos * localTransMatrix;
		localTransMatrix.InvertThisPrecise(); //TODO: Find out why is there an InverseOrto while loading a viewing matrix into the OpenGL (glLoadMatrixf(camera->GetWorldTransMatrix()->InverseOrto()._m); in RenderGlModule.cpp)
		return localTransMatrix;
		/*LOG(wxT("localTransMatrix"));
		LOG(localTransMatrix.ToString());*/
	}

	///the rotation and scaleOrientation are in form of axis-angle
	///it returns sign of determinant
	float Decompose(Vector3 &translation, Vector4 &rotation, Vector3 &scale, Vector4 &scaleOrientation) const
	{
		AffineParts parts;	
		float matToDecomp[4][4];
		for (int i=0,j=0; i<4; i++)
		{
			matToDecomp[0][i] = _m[j++];
			matToDecomp[1][i] = _m[j++];
			matToDecomp[2][i] = _m[j++];
			matToDecomp[3][i] = _m[j++];
		}

		decomp_affine(matToDecomp, &parts);

		translation=Vector3(parts.t.x, parts.t.y, parts.t.z);
		rotation = Vector4(parts.q.x, parts.q.y, parts.q.z, parts.q.w).QuaternionToAxisAngle();

		scale = Vector3(parts.k.x, parts.k.y, parts.k.z);
		scaleOrientation = Vector4(parts.u.x, parts.u.y, parts.u.z, parts.u.w).QuaternionToAxisAngle();
		return parts.f;
	}

	///Get string
	wxString ToString() const
	{
		wxString str;
		for (int i = 0; i < 4; i++)
			str+=TextWriter::WriteToWxString(_m+4*i, 4, wxT(" "))+wxT("\n");
		return str;
	}

	///Output stream operator
	friend std::ostream & operator<<(std::ostream & os, const Matrix & M)
	{
		for (int i = 0; i < 4; i++)
		{ // y
			for (int j = 0; j < 4; j++)
			{ // x
				os << std::setprecision(4) << std::setw(10) << M._m[4*i+j] << " ";
			}
			os << '\n';
		}
		return os;
	}

	///Get copy of instance
	Matrix * Clone() const
	{
		return new Matrix(*this);
	}

	/// Creates projection matrix of generic projection. Produces the same matrix as glFrustum.
	static Matrix PerspectiveGeneric(float left, float right, float bottom, float top, float near_z, float far_z)
	{
		Matrix result;

		result._m11 = (2.0f * near_z) / (right - left);
		result._m31 = (right + left) / (right - left);
		result._m22 = (2.0f * near_z) / (top - bottom);
		result._m32 = (top + bottom) / (top - bottom);
		result._m33 = -(far_z + near_z) / (far_z - near_z);
		result._m43 = -(2.0f * far_z * near_z) / (far_z - near_z);
		result._m34 = -1.0f;
		result._m44 = 0.0f;

		return result;
	}

	/// Creates projection matrix of centered projection. Produces the same matrix as gluPerspective.
	static Matrix PerspectiveCentered(float fovy_deg, float aspect, float near_z, float far_z)
	{
		float top = near_z * tanf(DegToRad(fovy_deg) * 0.5f);
		float bottom = -top;
		float left = bottom * aspect;
		float right = top * aspect;
		return Matrix::PerspectiveGeneric(left, right, bottom, top, near_z, far_z);
	}

	// Per component multiply and divide
	static inline Matrix MultiplyComp(const Matrix &a, const Matrix &b)
	{
		Matrix res;
		for (int i = 0; i < 16; i++)	res._m[i] = a._m[i] * b._m[i];
		return res;
	}
	static inline Matrix DivideComp(const Matrix &a, const Matrix &b)
	{
		Matrix res;
		for (int i = 0; i < 16; i++)	res._m[i] = a._m[i] / b._m[i];
		return res;
	}

};


inline __m128 CrossProductSSE(const __m128 &a, const __m128 &b)
{
	return _mm_sub_ps(
		_mm_mul_ps(_mm_shuffle_ps(a, a, _MM_SHUFFLE(3, 0, 2, 1)), _mm_shuffle_ps(b, b, _MM_SHUFFLE(3, 1, 0, 2))),
		_mm_mul_ps(_mm_shuffle_ps(a, a, _MM_SHUFFLE(3, 1, 0, 2)), _mm_shuffle_ps(b, b, _MM_SHUFFLE(3, 0, 2, 1)))   );
}


///Structure with ray data
struct Ray
{
public:
	///Ray origin
	Vector3 origin;
	///Offset - it is used by RayTracer
	unsigned int offset;
	///Ray direction
	Vector3 direction;
	///t
	float t;

	///Constructor
	Ray() : offset(0), t(0.0f) {}
	///Constructor
	Ray(const Vector3 & orig, const Vector3 & dir, const float t = FLT_MAX) : origin(orig), offset(0), direction(dir), t(t)
	{
	}
	///Copy constructor
	Ray(const Ray & ray) : origin(ray.origin), offset(ray.offset), direction(ray.direction), t(ray.t)
	{
	}

	///Get intersection distance from origin to plane
	float GetIntersectionDist(const Vector4 & pln) const
	{
		return -pln.PlaneDotCoord(origin) / direction.Dot(Vector3(pln._v));
	}

	///Get intersection point between plane and ray
	Vector3 GetIntersection(const Vector4 & pln) const
	{
		return origin + direction * GetIntersectionDist(pln);
	}

	///Check if ray intersects given triangle
	//TODO: not working in some cases
	bool IntersectsTri(const Vector3 & v0, const Vector3 & v1, const Vector3 & v2, float * dist = (float *)NULL) const
	{
#define MOLLER_RAY_TRI_INTERSECTION 1
#if !MOLLER_RAY_TRI_INTERSECTION

		const Vector3 edge1(v1 - v0);
		const Vector3 edge2(v2 - v0);

		/* begin calculating determinant - also used to calculate U parameter */
		Vector3 pvec(direction.Cross(edge2));

		/* if determinant is near zero, ray lies in plane of triangle */
		const float det = fabsf( edge1.Dot(pvec) ); // = vector3U::TripleProduct(dir,edge1,edge2);

		/* calculate distance from vert0 to ray origin */
		const Vector3 tvec(origin - v0);

		/* calculate U parameter and test bounds */
		const float u = tvec.Dot(pvec); // = vector3U::TripleProduct(dir,edge2,tvec);
		if ( u < 0 || u > det )
			return false;

		/* prepare to test V parameter */
		/* calculate V parameter and test bounds */
		const float v = direction.Dot( tvec.Cross(edge1) ); // = dir * (tvec ^ edge1)
		if ( v < 0 || u + v > det )
			return false;

		float intDist = GetIntersectionDist(Plane::FromPoints(v0, v1, v2));
		if (intDist < 0)
			return false;
		else if (dist)
			*dist = intDist;

		return true;
#else
		const float threshold = 1e-6f;
		const Vector3 e1(v0 - v1);
		const Vector3 e2(v2 - v1);
		const Vector3 p(direction.Cross(e2));
		float det = e1.Dot(p);
		const Vector3 w0(origin - v1);
		const float u = w0.Dot(p);
		const Vector3 q(w0.Cross(e1));
		const float v = direction.Dot(q);

		if (det > threshold) {
			if (u < 0.0f || u > det)
				return false;
			if (v < 0.0f || u + v > det)
				return false;
		}
		else
		{
			if (det < -threshold)
			{
				if (u > 0.0 || u < det)
					return false;
				if (v > 0.0 || u + v < det)
					return false;
			} else
				return false;
		}

		float t = e2.Dot(q) / det;

		if (t < 0.0f)
			return false;

		if (dist)
			*dist = t;
		return true;
#endif
	}

	///Compute intersection with triangle with uv coordinates, but it must be shorter then maxDist 
	bool IntersectsTriSSE(const __m128 & v1, const __m128 & e1, const __m128 & e2, const float &maxDist, float & u, float & v, float &dist) const
	{
		static const float threshold = 1e-6f;
		__m128 sdirection=_mm_setr_ps(direction.x, direction.y, direction.z, 0);
		__m128 sp=CrossProductSSE(sdirection, e2);
		float det;
		_mm_store_ss(&det, _mm_dp_ps(e1, sp, 0x71));
		__m128 sorigin=_mm_setr_ps(origin.x, origin.y, origin.z, 0);
		__m128 sw0=_mm_sub_ps(sorigin, v1);

		if (det > threshold) {
			_mm_store_ss(&u, _mm_dp_ps(sw0, sp, 0x71));
			if (u < 0.0f || u > det)
				return false;

			__m128 sq=CrossProductSSE(sw0, e1);
			_mm_store_ss(&v, _mm_dp_ps(sdirection, sq, 0x71));
			if (v < 0.0f || u + v > det)
				return false;

			float t;
			_mm_store_ss(&t, _mm_dp_ps(e2, sq, 0x71));
			t /= det;

			if ((t < 0.0f) || (t>=maxDist))
				return false;
			dist=t;
		}
		else
		{
			if (det < -threshold)
			{
				_mm_store_ss(&u, _mm_dp_ps(sw0, sp, 0x71));

				if (u > 0.0 || u < det)
					return false;

				__m128 sq=CrossProductSSE(sw0, e1);
				_mm_store_ss(&v, _mm_dp_ps(sdirection, sq, 0x71));
				if (v > 0.0 || u + v < det)
					return false;

				float t;
				_mm_store_ss(&t, _mm_dp_ps(e2, sq, 0x71));
				t /= det;

				if ((t < 0.0f) || (t>=maxDist))
					return false;
				dist=t;
			} else
				return false;
		}

		u = u / det;
		v = v / det;
		return true;//*/
	}
	///Compute intersection with triangle with uv coordinates, but it must be shorter then maxDist 
	bool IntersectsTri(const Vector3 & v1, const Vector3 & e1, const Vector3 & e2, const float &maxDist, float & u, float & v, float &dist) const
	{
		/*static const float threshold = 1e-6f;
		__m128 sdirection=_mm_setr_ps(direction.x, direction.y, direction.z, 0);
		__m128 se2=_mm_setr_ps(e2.x, e2.y, e2.z, 0);
		__m128 sp=CrossProductSSE(sdirection, se2);
		__m128 se1=_mm_setr_ps(e1.x, e1.y, e1.z, 0);
		float det=_mm_dp_ps(se1, sp, 0x71).m128_f32[0];
		__m128 sorigin=_mm_setr_ps(origin.x, origin.y, origin.z, 0);
		__m128 sv1=_mm_setr_ps(v1.x, v1.y, v1.z, 0);
		__m128 sw0=_mm_sub_ps(sorigin, sv1);

		if (det > threshold) {
			u=_mm_dp_ps(sw0, sp, 0x71).m128_f32[0];
			if (u < 0.0f || u > det)
				return false;

			__m128 sq=CrossProductSSE(sw0, se1);
			v=_mm_dp_ps(sdirection, sq, 0x71).m128_f32[0];
			if (v < 0.0f || u + v > det)
				return false;

			float t = _mm_dp_ps(se2, sq, 0x71).m128_f32[0] / det;
			if ((t < 0.0f) || (t>=maxDist))
				return false;
			dist=t;
		}
		else
		{
			if (det < -threshold)
			{
				u=_mm_dp_ps(sw0, sp, 0x71).m128_f32[0];
				if (u > 0.0 || u < det)
					return false;

				__m128 sq=CrossProductSSE(sw0, se1);
				v=_mm_dp_ps(sdirection, sq, 0x71).m128_f32[0];
				if (v > 0.0 || u + v < det)
					return false;

				float t = _mm_dp_ps(se2, sq, 0x71).m128_f32[0] / det;
				if ((t < 0.0f) || (t>=maxDist))
					return false;
				dist=t;
			} else
				return false;
		}

		u = u / det;
		v = v / det;
		return true;//*/
		const float threshold = 1e-6f;
		const Vector3 p(direction.Cross(e2));
		float det = e1.Dot(p);
		const Vector3 w0(origin - v1);

		if (det > threshold) {
			u = w0.Dot(p);
			if (u < 0.0f || u > det)
				return false;

			const Vector3 q(w0.Cross(e1));
			v = direction.Dot(q);
			if (v < 0.0f || u + v > det)
				return false;

			float t = e2.Dot(q) / det;
			if ((t < 0.0f) || (t>=maxDist))
				return false;
			dist=t;
		}
		else
		{
			if (det < -threshold)
			{
				u = w0.Dot(p);
				if (u > 0.0 || u < det)
					return false;

				const Vector3 q(w0.Cross(e1));
				v = direction.Dot(q);
				if (v > 0.0 || u + v < det)
					return false;

				float t = e2.Dot(q) / det;
				if ((t < 0.0f) || (t>=maxDist))
					return false;
				dist=t;
			} else
				return false;
		}

		u = u / det;
		v = v / det;
		return true;//*/
	}
	bool IntersectsTriWireframe(const Vector3 & v1, const Vector3 & e1, const Vector3 & e2, const float &maxDist, float & u, float & v, float &dist, float maxD/* = 0.001f*/) const
	{
		const float threshold = 1e-6f;
		const Vector3 p(direction.Cross(e2));
		float det = e1.Dot(p);
		const Vector3 w0(origin - v1);

		if (det > threshold) {
			u = w0.Dot(p);
			if (u < 0.0f || u > det)
				return false;

			const Vector3 q(w0.Cross(e1));
			v = direction.Dot(q);
			if (v < 0.0f || u + v > det)
				return false;

			float t = e2.Dot(q) / det;
			if ((t < 0.0f) || (t>=maxDist))
				return false;
			dist=t;
		}
		else
		{
			if (det < -threshold)
			{
				u = w0.Dot(p);
				if (u > 0.0 || u < det)
					return false;

				const Vector3 q(w0.Cross(e1));
				v = direction.Dot(q);
				if (v > 0.0 || u + v < det)
					return false;

				float t = e2.Dot(q) / det;
				if ((t < 0.0f) || (t>=maxDist))
					return false;
				dist=t;
			} else
				return false;
		}

		u = u / det;
		v = v / det;
		
		//for distance point to line see:
		//http://dagles.klenot.cz/rihova/vzdalenosti.pdf
		Vector3 A = origin + direction * dist; // point

		float pdist = e1.Cross(A - v1).Length() / e1.Length();
		if (pdist <= maxD)
			return true;

		pdist =  e2.Cross(A - v1).Length() / e2.Length();
		if (pdist <= maxD)
			return true;
		
		Vector3 MN3 = e2 - e1;
		pdist = MN3.Cross(A - v1 - e1).Length() / MN3.Length();
		if (pdist <= maxD)
			return true;
		return false;
	}

	///Get copy of instance
	Ray * Clone() const
	{
		return new Ray(*this);
	}
};

struct Triangle
{
	Vector3 v1;
	Vector3 v2;
	Vector3 v3;

	///Constructor
	Triangle() {}

	///Constructor
	Triangle(const Vector3 &_v1, const Vector3 &_v2, const Vector3 &_v3) : v1(_v1), v2(_v2), v3(_v3) {}

	///Assignment operator
	Triangle & operator=(const Triangle & tr)
	{
		v1 = tr.v1;
		v2 = tr.v2;
		v3 = tr.v3;
		return *this;
	}

	///Get component
	Vector3 operator[](unsigned i) const
	{
		if (i == 0)
			return v1;
		else if (i == 1)
			return v2;
		else if (i == 2)
			return v3;

		return Vector3();
	}

	Vector3 GetCenter() const
	{
		return (v1 + v2 + v3) / 3.0f;
	}

	Vector3 GetNormal() const
	{
		if ((v1!=v2) && (v2!=v3) && (v3!=v1))
			return ((v2-v1).Cross(v3-v1)).Normalize();
		return Vector3(0,0,0); //TODO: decide what to do in such case
	}
};

/// structure holds Triangle inf plus Normals and texture coordinates
struct TriangleData : public Triangle{
	// normals per Vertex
	bool hasNormals;
	Vector3 n1;
	Vector3 n2;
	Vector3 n3;

	// texture coordinates
	Vector2 texCoord1;
	Vector2 texCoord2;
	Vector2 texCoord3;

	//intersection accelerators
	Vector3 e1;
	Vector3 e2;

	// constructor
	TriangleData() : hasNormals(false) {}

	///Constructor
	TriangleData(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3, const Vector3 &n1, const Vector3 &n2, const Vector3 &n3, const Vector2 &tc1, const Vector2 &tc2, const Vector2 &tc3)
		: Triangle(v1, v2, v3), hasNormals(true), n1(n1), n2(n2), n3(n3), texCoord1(tc1), texCoord2(tc2), texCoord3(tc3), e1(v1 - v2), e2(v3 - v2) {}
	
	///Constructor
	TriangleData(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3) : Triangle(v1, v2, v3), hasNormals(false), e1(v1 - v2), e2(v3 - v2) {}

	///Constructor
	TriangleData(const Vector3 &v1, const Vector3 &v2, const Vector3 &v3, const Vector3 &n1, const Vector3 &n2, const Vector3 &n3) 
		: Triangle(v1, v2, v3), hasNormals(true), n1(n1), n2(n2), n3(n3), e1(v1 - v2), e2(v3 - v2) {}

	///Assignment operator
	TriangleData & operator=(const TriangleData & tr)
	{
		v1 = tr.v1;
		v2 = tr.v2;
		v3 = tr.v3;
		n1 = tr.n1;
		n2 = tr.n2;
		n3 = tr.n3;
		texCoord1 = tr.texCoord1;
		texCoord2 = tr.texCoord2;
		texCoord3 = tr.texCoord3;
		e1 = tr.e1;
		e2 = tr.e2;
		hasNormals = tr.hasNormals;
		return *this;
	}

	void SetNormals(const Vector3 &n1, const Vector3 &n2, const Vector3 &n3)
	{
		hasNormals=true;
		this->n1=n1;
		this->n2=n2;
		this->n3=n3;
	}

	void SetTexCoords(const Vector2 &tc1, const Vector2 &tc2, const Vector2 &tc3)
	{
		this->texCoord1=tc1;
		this->texCoord2=tc2;
		this->texCoord3=tc3;
	}
};

///Get cross product
inline Vector3 CrossProd(const Vector3 & a, const Vector3 & b)
{
	return a.Cross(b);
}

///Get dot product
inline float DotProd(const Vector3 & a, const Vector3 & b)
{
	return a.Dot(b);
}

inline Vector3 Normalize(const Vector3 &a)
{
	Vector3 b(a);
	return b.Normalize();
}

inline bool AreParallel(const Vector3 &a, const Vector3 &b, float tolerance ) {
	Vector3 aUnit = a.Normalize();
	Vector3 bUnit = b.Normalize();
	return fabs(CrossProd(aUnit, bUnit).Length()) < tolerance;
}

/// returns 2D angle between two 3D vectors on X Y plane including sign
inline float GetAngle2D(const Vector3 &a, const Vector3 &b) {
	Vector3 a_(a);
	Vector3 b_(b);
	a_.z=0;
	b_.z=0;
	float magnitude = acos(a_.Dot(b_));
	Vector3 cross = a_.Cross(b_);
	Vector3 axis(0.0f,0.0f,-1.0f);
	float dir = axis.Dot(cross);
    if (dir < 0.0f)
        magnitude = -magnitude;
    return magnitude;
}

#endif
