// © Copyright 2008 Владимир Фролов
#define MGML_MQ_GUARDIAN 


#ifndef MGML_ERROR_GUARDIAN
	#ifndef __CUDACC__
		#include "MGML_ERROR.h"
		//typedef MGML_ERROR::Zero_Division ZeroDivision;
	#else
		
	#endif
#endif


#ifndef MPMG_GUARDIAN
	#include "MPMG.h"
#endif

#ifndef MGML_MATH_GUARDIAN
	#include "MGML_MATH.h"
#endif


using namespace MPMG;



namespace MGML_MATH
{



// обозначим внутреннею структуру матриц.
template<class T>
struct Tmat2x2
{
	union
	{
		struct
		{	
			T  M00,M01,
			   M10,M11;
        };
		T M[2][2];
		T L[4];
	};

	typedef const T CT;
	inline universal_call void set(CT a00,CT a01,
								   CT a10,CT a11)
	{
		M00 = a00;
		M01 = a01;
		M10 = a10;
		M11 = a11;
	}

	inline universal_call void setScale(CT a,CT b)
	{
		M00 = a; 
		M11 = a;
	}
};

template<class T>
struct Tmat3x3
{
	union
	{
		struct
		{	
			T  M00,M01,M02,
			   M10,M11,M12,
			   M20,M21,M22;
        };
		T M[3][3];
		T L[9];
	};

	typedef const T CT;

	inline universal_call void set(CT a00,CT a01,CT a02,
								   CT a10,CT a11,CT a12,
								   CT a20,CT a21,CT a22)
	{
	    M00 = a00;
		M01 = a01;
		M02 = a02;

		M10 = a10;
		M11 = a11;
		M12 = a12;

		M20 = a20;
		M21 = a21;
		M22 = a22;
		
	}

	inline universal_call void setScale(CT a,CT b,CT c)
	{
		M00 = a; 
		M11 = b;
		M22 = c;
	}
};

template<class T>
struct Tmat4x4
{
	union
	{

		T L[16];
		T M[4][4];
		struct
		{	
			T  M00,M01,M02,M03;
			T  M10,M11,M12,M13;
			T  M20,M21,M22,M23;
			T  M30,M31,M32,M33;
        };
	};

	typedef const T CT;

	inline universal_call void set(CT a00,CT a01,CT a02,CT a03,
								   CT a10,CT a11,CT a12,CT a13,
								   CT a20,CT a21,CT a22,CT a23,
								   CT a30,CT a31,CT a32,CT a33)
	{
	    M00 = a00;
		M01 = a01;
		M02 = a02;
		M03 = a03;

		M10 = a10;
		M11 = a11;
		M12 = a12;
		M13 = a13;

		M20 = a20;
		M21 = a21;
		M22 = a22;
		M23 = a23;

		M30 = a30;
		M31 = a31;
		M32 = a32;
		M33 = a33;	
	}

	inline universal_call void setScale(CT a,CT b,CT c)
	{
		M00 = a; 
		M11 = b;
		M22 = c;
	}
};

// матрица для SIMD
template<>
struct SSE_ALIGNED Tmat4x4<float> : public SSE_Aligned_Object
{
 
	union
	{
		struct
		{	
		  float  M00,M01,M02,M03;
		  float	 M10,M11,M12,M13;
		  float	 M20,M21,M22,M23;
		  float  M30,M31,M32,M33;
        };
		 __declspec(align(16)) float M[4][4];
		 __declspec(align(16)) float L[16];

		 __m128 ss[4];
	};

	typedef const float CT;

	inline Tmat4x4<float>& Tmat4x4<float>::operator=(const Tmat4x4<float>& rhs)
	{
	  this->ss[0] = rhs.ss[0];
	  this->ss[1] = rhs.ss[1];
	  this->ss[2] = rhs.ss[2];
	  this->ss[3] = rhs.ss[3];
	  return *this;
	}

  inline universal_call void set(  CT a00,CT a01,CT a02,CT a03,
				    CT a10,CT a11,CT a12,CT a13,
				    CT a20,CT a21,CT a22,CT a23,
					CT a30,CT a31,CT a32,CT a33)
  {
	    M00 = a00;
		M01 = a01;
		M02 = a02;
		M03 = a03;

		M10 = a10;
		M11 = a11;
		M12 = a12;
		M13 = a13;

		M20 = a20;
		M21 = a21;
		M22 = a22;
		M23 = a23;

		M30 = a30;
		M31 = a31;
		M32 = a32;
		M33 = a33;	
  }

  inline universal_call void setScale(CT a,CT b,CT c)
  {
		M00 = a; 
		M11 = b;
		M22 = c;
  }

};


template<class T>
struct Tmat3x4
{
	union
	{
		struct
		{	
			T  M00,M01,M02,M03,
			   M10,M11,M12,M13,
			   M20,M21,M22,M23;
        };
		T M[3][4];
		T L[12];
	};

 typedef const T CT;
 
 inline universal_call void set(CT a00,CT a01,CT a02,CT a03,
				 CT a10,CT a11,CT a12,CT a13,
				 CT a20,CT a21,CT a22,CT a23)
 {
	    M00 = a00;
		M01 = a01;
		M02 = a02;
		M03 = a03;

		M10 = a10;
		M11 = a11;
		M12 = a12;
		M13 = a13;

		M20 = a20;
		M21 = a21;
		M22 = a22;
		M23 = a23;
 }

 inline universal_call void setScale(CT a,CT b,CT c)
 {
		M00 = a; 
		M11 = b;
		M22 = c;
 }

};


template<class T>
struct Tmat4x3
{
	union
	{
		struct
		{	
			T  M00,M01,M02,
			   M10,M11,M12,
			   M20,M21,M22,
			   M30,M31,M32;
        };
		T M[4][3];
		T L[12];
	};

  typedef const T CT;

  inline universal_call void set(CT a00,CT a01,CT a02,
				  CT a10,CT a11,CT a12,
				  CT a20,CT a21,CT a22,
				  CT a30,CT a31,CT a32)
  {
	    M00 = a00;
		M01 = a01;
		M02 = a02;

		M10 = a10;
		M11 = a11;
		M12 = a12;

		M20 = a20;
		M21 = a21;
		M22 = a22;

		M30 = a30;
		M31 = a31;
		M32 = a32;	
  }

  inline universal_call void setScale(CT a,CT b,CT c)
  {
		M00 = a; 
		M11 = b;
		M22 = c;
  }

};


template<int n,int m,class T>
struct DefaultMat
{
	union
	{
		T M[n][m];
		T L[n*m];
	};
};


template<int n,int m,class T>
class RectMatrix: public SWITCH<(n),
						 CASE<2, typename IF<(m==2), Tmat2x2<T>, DefaultMat<n,m,T> >::RET,
						 CASE<3, typename SWITCH<(m),
									  CASE<3,Tmat3x3<T>,
									  CASE<4,Tmat3x4<T>,
									  CASE<DEFAULT,DefaultMat<n,m,T> > > > >::RET,
						 CASE<4, typename SWITCH<(m),
									  CASE<3,Tmat4x3<T>,
									  CASE<4,Tmat4x4<T>,
									  CASE<DEFAULT,DefaultMat<n,m,T> > > > >::RET,
						 CASE<DEFAULT,DefaultMat<n,m,T> > > > >
						 >::RET
{
protected:
 typedef const T CT;

 typedef typename SWITCH<(n),
				CASE<2, typename IF<(m==2), Tmat2x2<T>, DefaultMat<n,m,T> >::RET,
				CASE<3, typename SWITCH<(m),
								CASE<3,Tmat3x3<T>,
								CASE<4,Tmat3x4<T>,
								CASE<DEFAULT,DefaultMat<n,m,T> > > > >::RET,
				CASE<4, typename SWITCH<(m),
								CASE<3,Tmat4x3<T>,
								CASE<4,Tmat4x4<T>,
								CASE<DEFAULT,DefaultMat<n,m,T> > > > >::RET,
				CASE<DEFAULT,DefaultMat<n,m,T> > > > >
				>::RET Base;

 inline RectMatrix& operator=(const RectMatrix& rhs)
 {
	 Base::operator=(rhs);
	 return *this;
 }

 inline universal_call void swapRows(int r1,int r2);
};



template<int n,class T>
class SquareMatrix: public RectMatrix<n,n,T>
{
	// прибавление к to строке which строку * k (от j до n)
    inline universal_call void add_row(int which,int to,int j,T k)
	{ for(register int i=j;i<n;i++) this->M[to][i] += this->M[which][i]*k; }

	typedef RectMatrix<n,n,T> Base;
public:


	inline SquareMatrix& operator=(const SquareMatrix& rhs)
	{
		Base::operator=(rhs);
		return *this;
	}

	inline universal_call void Transpose() // #TEST
	{
		for(int j=1;j<n;j++)
		{
	     for(int  i=0;i<j;i++)
		 {
			 T tmp = this->M[i][j];
			 this->M[i][j] = this->M[j][i];
			 this->M[j][i] = tmp;
			 //MGML_MATH::SWAP(this->M[i][j],this->M[j][i]);
		 }
		}
	}
    universal_call void Inverse();	
	universal_call T det();
};

// внести шум в марицу. Нужно, чтобы избежать делений на ноль.
// при обращении еденичной матрицы
// do_matrix_noise<n*m,T>::exec(L);	
template<int n,class T>
struct do_matrix_noise
{
 static universal_call void exec(T* L)
 {
	
 }

};

template<int n>
struct do_matrix_noise<n,float>
{
 static universal_call void exec(float* L)
 {
#ifndef __CUDACC__
	 for(int i=0;i<n;i++)
		 L[i] += MGML_MATH::rnd(0.00001f,0.00002f);
#else
	 ASSERT(false);
#endif
 }

};


template<int n>
struct do_matrix_noise<n,double>
{
 static universal_call void exec(double* L)
 {
#ifndef __CUDACC__
	 for(int i=0;i<n;i++)
		 L[i] += MGML_MATH::rnd(0.0000001,0.0000002);
#else
	 ASSERT(false);
#endif
 }

};



template<int n,int m,class T>
class MATRIX: public IF<(n==m),
						SquareMatrix<n,T>,
					  RectMatrix<n,m,T> >::RET
{
private:
	typedef typename IF<(n==m),
						SquareMatrix<n,T>,
					 RectMatrix<n,m,T> >::RET MBaseT;
	typedef const T CT;
public:
	inline universal_call MATRIX()
	{
		for(int i=0;i<n;i++)
			for(int j=0;j<m;j++)
				this->M[i][j] = ( (i==j) ? (T)1:(T)0 );
	}

	// ALERT! OPTIMIZATION NEEDED!
	inline universal_call MATRIX(const T* p)    
	{
		for(int i=0;i<n*m;i++)
			this->L[i]=p[i];
	}
	
	// ALERT! OPTIMIZATION NEEDED!
	inline universal_call MATRIX(const MATRIX<n,m,T>& m2) 
	{
		for(int i=0;i<n*m;i++)
			this->L[i]=m2.L[i];
	}
	
	inline universal_call MATRIX(CT a00,CT a01,
				  CT a10,CT a11) { set(a00,a01,a10,a11); }

    inline universal_call MATRIX(CT a00,CT a01,CT a02,
				  CT a10,CT a11,CT a12,
				  CT a20,CT a21,CT a22) { set(a00,a01,a02,
											  a10,a11,a12,
											  a20,a21,a22); }
	inline universal_call MATRIX(CT a00,CT a01,CT a02,CT a03,
				  CT a10,CT a11,CT a12,CT a13,
				  CT a20,CT a21,CT a22,CT a23,
				  CT a30,CT a31,CT a32,CT a33) { set(a00,a01,a02,a03,
													 a10,a11,a12,a13,
													 a20,a21,a22,a23,
													 a30,a31,a32,a33); }

     // для матриц 3x4 и 4x3 (!для обоих)
	inline universal_call MATRIX(CT a00,CT a01,CT a02,CT a03,
								 CT a10,CT a11,CT a12,CT a13,
								 CT a20,CT a21,CT a22,CT a23){ set(a00,a01,a02,a03,
																   a10,a11,a12,a13,
																   a20,a21,a22,a23); }

	inline MATRIX& operator=(const MATRIX& rhs)
	{
		MBaseT::operator=(rhs);
		return *this;
	}

    inline universal_call void Zero()
	 { for(register int i=0;i<n*m;i++) this->L[i] = 0;}

    inline universal_call void Identity()
	{Zero();for(register int i=0;i<n;i++) this->M[i][i] =1; }

	inline universal_call void setRow(const int r,const VECTOR<m,T>& v)
	{
		MovVec<m,T>::exec(this->L + r*m,v.M);
		 //memcpy(L + r*m,v.M,sizeof(T)*m);
	}
    
	inline universal_call void setCol(const int r,const VECTOR<m,T>& v)  // done
	 { for(int i=0;i<m;i++) this->M[i][r] = v.M[i]; }

	inline universal_call VECTOR<m,T> getRow(const int r) const // done
	{
	  VECTOR<m,T> temp;

	  MovVec<m,T>::exec(temp.M,this->L + r*m);
	  //memcpy(temp.M,L + r*m,sizeof(T)*m);
	  return temp;
	}

	inline universal_call const VECTOR<m,T> getCol(const int r) const // done
	{
	  VECTOR<m,T> temp;
	  for(int i=0;i<m;i++) temp.M[i] = this->M[i][r];
	  return temp;
	}

    // WARNING! THIS WAY OF PROGRAMMING MAY CAUSE PROBLEMS DUE TO THE k < n #CHECK_THIS
	template<int k>
	inline universal_call void setScale(const VECTOR<k,T>& vec) // done
	 { for(int i=0;i<k;i++) this->M[i][i] = vec.M[i]; }


	template<int k>
	inline universal_call void setTranslate(const VECTOR<k,T>& v) // done
	{  
		 for(int i=0;i<k;i++) 
			 this->M[i][n-1] = v.M[i];
		 
		 this->M[n-1][n-1] = 1.0f;
	}

	inline universal_call void Transpose() { this->MBaseT::Transpose(); } // done
	inline universal_call void Inverse()   { this->MBaseT::Inverse(); }   // done

	inline universal_call MATRIX<n,n,T> getTranspose() const
	{
		MATRIX<n,n,T> temp(*this);
		temp.Transpose();
		return temp;
	}

	inline universal_call MATRIX<n,n,T> getInverse() const 
	{
		MATRIX<n,n,T> temp(*this);
		temp.Inverse();
		return temp;
	}

	inline universal_call void setRotationX(T ang); 
	inline universal_call void setRotationY(T ang); 
	inline universal_call void setRotationZ(T ang); 

	inline universal_call void setRotationXYZ(T ang_x,T ang_y,T ang_z); 
	inline universal_call void setRotationXYZ(const VECTOR<3,T>& v) { setRotationXYZ(v.x(), v.y(), v.z()); }
	inline universal_call void setRotationXYZ(const T* p)			{ setRotationXYZ(p[0],p[1],p[2]); }

	inline universal_call void setRotationYXZ(T x_ang,T y_ang,T z_ang);
	inline universal_call void setRotationYXZ(const VECTOR<3,T>& v) { setRotationYXZ(v.x(), v.y(), v.z());} 
	inline universal_call void setRotationYXZ(const T* p)			{ setRotationYXZ(p[0],p[1],p[2]);} 

    template <int n1>
	inline universal_call MATRIX<n,m,T> operator*(const MATRIX<m,n1,T>& m2) const
	{
	 MATRIX<n,m,T> ret;
     ret.Zero();
	 for(register int i=0; i < n; i++)
      for(register int j=0; j < m; j++)
        for(register int c=0; c < n1; c++)
	    ret.M[i][j]+=this->M[i][c]*m2.M[c][j];
     return ret;
	}

	inline universal_call MATRIX<n,m,T>& operator+=(const MATRIX<n,m,T>& rhs)
	{
      AddVec<n*m,T>::exec(this->L,rhs.L);
	  return *this;
	}

#ifndef __CUDACC__
	void print(std::ostream& out) const;
#endif

};

#ifndef __CUDACC__
	template<int n,int m,class T>
	std::ostream& operator<<(std::ostream& out,const MATRIX<n,m,T>& m) 
	{ m.print(out); return out; }
#endif



template <class T> class QUATERNION: public VECTOR<4,T>
{
  // Эти функции стали фиктивными для кватерниона.
  T dot(const VECTOR<4,T>& v1) {return 0;}
public:
  inline universal_call QUATERNION(const T a,const T b,const T c,const T d): MGML_MATH::VECTOR<4,T>(a,b,c,d){}
  inline universal_call QUATERNION(): MGML_MATH::VECTOR<4,T>(){}
   
  inline universal_call void set(const VECTOR<3,T>& axis,T theta);      
  inline universal_call void set(const VECTOR<3,T>& from,const VECTOR<3,T>& to); 
  inline universal_call void set(const MATRIX<4,4,T>& m);               
  inline universal_call const MATRIX<4,4,T> toMatrix() const;          

  inline universal_call QUATERNION<T>& operator=(const VECTOR<4,T>& q);
  inline universal_call QUATERNION<T>& operator*=(const QUATERNION<T>& qr);
  inline universal_call QUATERNION<T>  operator*(const QUATERNION<T>& qr) const;  

  friend universal_call const VECTOR<3,T> operator*(const VECTOR<3,T>& vec,const QUATERNION<T>& q)
  {
    // return _vecMultQat(vec,q);
	  ASSERT(false);
	  return 0;
  }

};


#ifndef __CUDACC__

	static MATRIX<4,4,float> MatrixFromString(const std::string in_str)
	{
		MATRIX<4,4,float> r;
		
		sscanf(in_str.c_str(),"%f %f %f %f \
							   %f %f %f %f \
							   %f %f %f %f \
							   %f %f %f %f", 
								&r.L[0], &r.L[1], &r.L[2], &r.L[3],
								&r.L[4], &r.L[5], &r.L[6], &r.L[7],
								&r.L[8], &r.L[9], &r.L[10], &r.L[11],
								&r.L[12], &r.L[13], &r.L[14], &r.L[15]);

		return r;
	}

#endif

};




///////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////  MATRIX   ////////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////// 

#ifndef __CUDACC__
// =================================================================================
//
// =================================================================================
template<int n,int m,class T> 
universal_call void MGML_MATH::MATRIX<n,m,T>::print(std::ostream& out) const
{
 for(int i=0;i<n;i++)
 {
	 for(int j=0;j<m;j++)
		 out << M[i][j] << " ";
	 out << std::endl;
 }
}
#endif
// =================================================================================
//
// =================================================================================
template<int n,int m,class T> 
inline universal_call void MGML_MATH::MATRIX<n,m,T>::setRotationX(T ang)
{
 Identity();
 T sinx = sin(ang);
 T cosx = cos(ang);
 this->M11=cosx; this->M12=sinx;
 this->M21=-sinx;this->M22=cosx;
}
// =================================================================================
//
// =================================================================================
template<int n,int m,class T> 
inline universal_call void MGML_MATH::MATRIX<n,m,T>::setRotationY(T ang)
{
 Identity();
 T siny = sin(ang);
 T cosy = cos(ang);
 this->M00=cosy;this->M02=-siny;
 this->M20=siny;this->M22=cosy;
}
// =================================================================================
//
// =================================================================================
template<int n,int m,class T> 
inline universal_call void MGML_MATH::MATRIX<n,m,T>::setRotationZ(T ang)
{
 Identity();
 T sinz = sin(ang);
 T cosz = cos(ang);
 this->M00=cosz; this->M01=sinz;
 this->M10=-sinz;this->M11=cosz;
}
// =================================================================================
//
// =================================================================================
template<int n,int m,class T> 
inline universal_call void MGML_MATH::MATRIX<n,m,T>::setRotationXYZ(T x_ang,T y_ang,T z_ang)
{
	MATRIX<n,m,T> mx,my,mz;
	mx.setRotationX(x_ang);
	my.setRotationY(y_ang);
	mz.setRotationZ(z_ang);
	*this = mx*my*mz;
}

// =================================================================================
//
// =================================================================================
template<int n,int m,class T> 
inline universal_call void MGML_MATH::MATRIX<n,m,T>::setRotationYXZ(T x_ang,T y_ang,T z_ang)
{
	MATRIX<n,m,T> mx,my,mz;
	mx.setRotationX(x_ang);
	my.setRotationY(y_ang);
	mz.setRotationZ(z_ang);
	*this = my*mx*mz;
}

///////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////  SquareMatrix   //////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////// 
// =================================================================================
//
// =================================================================================
template<int n,int m,class T> 
inline universal_call void MGML_MATH::RectMatrix<n,m,T>::swapRows(int r1,int r2)
{

	// optimization needed
	for(int i=0;i<m;i++)
	{
		T tmp = this->M[r1][i];
		this->M[r1][i] = this->M[r2][i];
		this->M[r2][i] = tmp;
		//MGML_MATH::SWAP(this->M[r1][i],this->M[r2][i]);
	}

}
// =================================================================================
//
// =================================================================================
template<int n,class T> universal_call T MGML_MATH::SquareMatrix<n,T>::det()
{
 // прямой ход метода гаусса с выбором не нулевого элемента в столбце
 // так как особая точность не нужна, выбор главного элемента не делается
 {for(register int i=0;i<n-1;i++)
  {

	 if(this->M[i][i]*this->M[i][i] < EPSILON_E10M)
	 {
		 bool flag = false;
		 for(register int j=i;j<n;j++) 
		  if(this->M[i][j]*this->M[i][i] > EPSILON_E10M) 
		   { this->swapRows(i,j); flag = true; break;}
		 if(!flag)
		 {
			#ifndef __CUDACC__
				throw Zero_Division("Attempt to get determinant of singular matrix"); 
			#else
				return 0;
			#endif
		 }
	 }
	 // сделать нули в столбце
	 for(register int j=i+1;j<n;j++)
	 {
      T w = -this->M[j][i]/this->M[i][i];
	  add_row(i,j,i,w);
	 }

  }}

 T _det = 1;
 for(register int i=0;i<n;i++) _det *= this->M[i][i];
 return _det;
}

// =================================================================================
// 
// =================================================================================
template<int n,class T> 
universal_call void MGML_MATH::SquareMatrix<n,T>::Inverse()
{
 SquareMatrix<n,T> a; // дополнительная матрица.
 for(register int y=0;y<n*n;y++)a.L[y]    = 0;
 for(register int w=0;w<n;w++)  a.M[w][w] = 1;
  
 // прямой ход метода гаусса с выбором не нулевого элемента в столбце
 // так как особая точность не нужна, выбор главного элемента не делается
 for(register int i=0;i<n-1;i++)
  {

	 if(this->M[i][i]*this->M[i][i] < EPSILON_E10M)
	 {
		 bool flag = false;
		 for(register int j=i;j<n;j++) 
		  if(this->M[i][j]*this->M[i][i] > EPSILON_E10M) 
		    { this->swapRows(i,j); flag = true; break; }
	   if(!flag)
	   {
			#ifndef __CUDACC__
				throw Zero_Division("Attempt to inverse singular matrix"); 
		//		for(register int g=0;g<n*n;g++) this->L[g] = 0;
		//		for(register int g=0;g<n;g++) this->M[g][g] =1; 
		//		return;
			#else
				return;
			#endif
	   }
	 }
	 // сделать нули в столбце
	 for(register int j=i+1;j<n;j++)
	 {
      T w = -this->M[j][i]/this->M[i][i];
	  add_row(i,j,i,w);
	  a.add_row(i,j,0,w);
	 }

  }

 // обратный ход метода гаусса
 for(register int i=n-1;i>0;i--)
  {
    for(register int j=i-1;j>=0;j--)
    {
		if(fabs(this->M[i][i]) > EPSILON_E5M)
		{
			T w = -this->M[j][i]/this->M[i][i];
			add_row(i,j,i,w);
			a.add_row(i,j,0,w);
		}
    }
  }

 // делим на диагональные элементы
 // быстрее будет умножать на 1/M[i][j] чем делить на M[i][j]
 for(register int i=0;i<n;i++) 
 {
	 if(fabs(this->M[i][i]) > EPSILON_E5M)
		this->M[i][i] = 1.0/this->M[i][i];
	 else  
		this->M[i][i] = 1.0;
 }

  for(register int i=0;i<n;i++)
    for(register int j=0;j<n;j++)
		a.M[i][j] *= this->M[i][i];

  // need optimisation   

  for(int i=0;i<n*n;i++) //memcpy(M,a.M,sizeof(T)*n*n);
	this->L[i] = a.L[i]; 

}
///////////////////////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////     QUATERNION   //////////////////////////////////////////////
/////////////////////////////////////////////////////////////////////////////////////////////////// 
//=======================================================
// оператор =
//=======================================================
template<class T>
inline universal_call MGML_MATH::QUATERNION<T>& MGML_MATH::QUATERNION<T>::operator=(const VECTOR<4,T>& q)
{
	this->x=q.M[0];
	this->y=q.M[1];
	this->z=q.M[2];
	this->w=q.M[3];
	return *this;
}
//=======================================================
// устанавливает поворот по вектору, во круг которого
// надо повернуть и углу поворота
//=======================================================
template<class T>
inline universal_call void MGML_MATH::QUATERNION<T>::set(const VECTOR<3,T>& axis,T theta)
{
  T sqnorm = axis.lenSquare();

   if (sqnorm <= EPSILON_E5M)
   {
     // axis too small.
     this->x = this->y = this->z = 0.0;
	 this->w = 1;
   } 
   else 
   {
     theta /= 2;
     T sin_theta = (T)(sin(theta));

     if(fabs(sqnorm-1) > EPSILON_E5M) 
        sin_theta /= (T)(sqrt(sqnorm));

     this->x = sin_theta * axis.M[0]; // SIMD
     this->y = sin_theta * axis.M[1];
     this->z = sin_theta * axis.M[2];
     this->w = (T)(cos(theta));
  }
}
// =================================================================================
//
// =================================================================================
template <class T> 
inline universal_call void MGML_MATH::QUATERNION<T>::set(const MATRIX<4,4,T>& m)
{
  T tr, s;
  const int nxt[3] = { 1, 2, 0 };
  tr = m.M[0][0] + m.M[1][1] + m.M[2][2];
  
  if ( tr > 0 )
  {
    s = (T)(sqrt( tr + m.M[3][3] ));
    this->M[3] = (T) ( s * 0.5 );
    s = (T)(0.5) / s;

    this->M[0] = (T)( ( m.M[1][2] - m.M[2][1] ) * s ); // SIMD
    this->M[1] = (T)( ( m.M[2][0] - m.M[0][2] ) * s );
    this->M[2] = (T)( ( m.M[0][1] - m.M[1][0] ) * s );
  }
  else
  {
    register int i = 0,j,k;
    if ( m.M[1][1] > m.M[0][0] )
              i = 1;

    if ( m.M[2][2] > m.M[i][i] )
              i = 2;

     j = nxt[i];
     k = nxt[j];

     s = (T)(sqrt( ( m.M[i][j] - ( m.M[j][j] + m.M[k][k] )) + 1));

     this->M[i] = (T)( s * 0.5 );
     s = (T)(0.5 / s);

     this->M[3] = (T)( ( m.M[j][k] - m.M[k][j] ) * s ); // SIMD
     this->M[j] = (T)( ( m.M[i][j] + m.M[j][i] ) * s );
     this->M[k] = (T)( ( m.M[i][k] + m.M[k][i] ) * s );
  }
}
// =================================================================================
//
// =================================================================================

