
#ifndef LinearAlgebraH
#define LinearAlgebraH

#include <math.h>


#define PI		3.1415926535897932384626433832795

#define fptr(qqq) ((float*)&qqq)

class	vec3
{
public:
	float x,y,z,w;
	inline float norm();
	inline float sqrnorm();
    inline void  normalize();
	//inline void operator = ( vec3 & fp);
	inline void operator = (const vec3 & fp);
	inline void operator = (const float* fvectorp);
	inline vec3(float xin=0.0f, float yin=0.0f, float zin=0.0f);
	inline float *operator()()
	{
		return (float *)this;
	}
       /*	String ToStr()
	{
	    return  String("     X= ")+ FloatToStrF (x,ffGeneral,4,4)+
                    String("     Y= ")+ FloatToStrF (y,ffGeneral,4,4)+
                    String("     Z= ")+ FloatToStrF (z,ffGeneral,4,4);
        }*/
};

class	mat3x3
{
public:

	float m11,m12,m13,m21,m22,m23,m31,m32,m33;
	inline void transpose();
	inline vec3 v1();
	inline vec3 v2();
	inline vec3 v3();
	inline void  sete();
	inline mat3x3(const vec3& v1=vec3(1.0f,0.0f,0.0f),
                      const vec3& v2=vec3(0.0f,1.0f,0.0f),
                      const vec3& v3=vec3(0.0f,0.0f,1.0f));




	inline void operator = (const mat3x3& m);
	inline void operator = (const float*	mp);
	inline float *operator()()
	{
		return (float *)this;
	}
    
	typedef float glmat[4][4];
    inline void  InitGLMatrix(glmat &mat)
    {
          mat[0][0]=m11,mat[0][1]=m12,mat[0][2]=m13,mat[0][3]=0.0f;
          mat[1][0]=m21,mat[1][1]=m22,mat[1][2]=m23,mat[1][3]=0.0f;
          mat[2][0]=m31,mat[2][1]=m32,mat[2][2]=m33,mat[2][3]=0.0f;
          mat[3][0]=0.0,mat[3][1]=0.0,mat[3][2]=0.0,mat[3][3]=1.0f;

    }
       /*	String ToStr()
	{
              return v1().ToStr()+" \n"+
                     v2().ToStr()+" \n"+
                     v3().ToStr()+" \n";


	}*/
};





inline void mat3x3::transpose()
{
		float m;

		m=m12;
		m12=m21;
		m21=m;

		m=m13;
		m13=m31;
		m31=m;

		m=m23;
		m23=m32;
		m32=m;
}

mat3x3 inline setrx(const float cosphi,const float sinphi)
{
	        mat3x3 m;
		m.m11=1.0f;
		m.m12=0.0f;
		m.m13=0.0f;

		m.m21=0.0f;
		m.m22=cosphi;
		m.m23=sinphi;

		m.m31=0.0f;
		m.m32=-sinphi;
		m.m33=cosphi;
		return m;
}

inline mat3x3  setry(const float cosphi,const float sinphi)
{
	        mat3x3 m;
		m.m11=cosphi;
		m.m12=0;
		m.m13=-sinphi;

		m.m21=0;
		m.m22=1;
		m.m23=0;

		m.m31=sinphi;
		m.m32=0;
		m.m33=cosphi;
		return m;
}

inline mat3x3 setrz(const float cosphi,const float sinphi)
{
	        mat3x3 m;
		m.m11=cosphi;
		m.m12=sinphi;
		m.m13=0.0f;

		m.m21=-sinphi;
		m.m22=cosphi;
		m.m23=0.0f;

		m.m31=0.0f;
		m.m32=0.0f;
		m.m33=1.0f;
		return m;
}



inline mat3x3  setrx(const float phi)
{
	        mat3x3 m;
		float cosphi=(float)cos(phi),sinphi=(float)sin(phi);
		m.m11=1.0f;
		m.m12=0.0f;
		m.m13=0.0f;

		m.m21=0.0f;
		m.m22=cosphi;
		m.m23=sinphi;

		m.m31=0.0f;
		m.m32=-sinphi;
		m.m33=cosphi;
		return m;
}

inline mat3x3  setry(const float phi)
{
	        mat3x3 m;
		float cosphi=(float)cos(phi),sinphi=(float)sin(phi);
		m.m11=cosphi;
		m.m12=0;
		m.m13=-sinphi;

		m.m21=0;
		m.m22=1;
		m.m23=0;

		m.m31=sinphi;
		m.m32=0;
		m.m33=cosphi;
		return m;
}

inline mat3x3 setrz(const float phi)
{

	        mat3x3 m;
		float cosphi=(float)cos(phi),sinphi=(float)sin(phi);
		m.m11=cosphi;
		m.m12=sinphi;
		m.m13=0.0f;

		m.m21=-sinphi;
		m.m22=cosphi;
		m.m23=0.0f;

		m.m31=0.0f;
		m.m32=0.0f;
		m.m33=1.0f;
		return m;
}





inline vec3 mat3x3::v1()
{
		return vec3(m11,m12,m13);
}

inline vec3 mat3x3::v2()
{
		return vec3(m21,m22,m23);
}

inline vec3 mat3x3::v3()
{
		return vec3(m31,m32,m33);
}


inline void mat3x3::sete()
{
		m11=1.0f;
		m12=0.0f;
		m13=0.0f;

		m21=0.0f;
		m22=1.0f;
		m23=0.0f;

		m31=0.0f;
		m32=0.0f;
		m33=1.0f;
}






inline void mat3x3::operator = (const mat3x3& m)
{
		m11=m.m11;
		m12=m.m12;
		m13=m.m13;

		m21=m.m21;
		m22=m.m22;
		m23=m.m23;

		m31=m.m31;
		m32=m.m32;
		m33=m.m33;
}
inline void mat3x3::operator = (const float* mp)
{
		m11=mp[0];
		m12=mp[1];
		m13=mp[2];

		m21=mp[3];
		m22=mp[4];
		m23=mp[5];

		m31=mp[6];
		m32=mp[7];
		m33=mp[8];
}

inline mat3x3::mat3x3(const vec3& v1,const vec3& v2,const vec3& v3)
{
		m11=v1.x;
		m12=v1.y;
		m13=v1.z;

		m21=v2.x;
		m22=v2.y;
		m23=v2.z;

		m31=v3.x;
		m32=v3.y;
		m33=v3.z;
}

inline	float root(const float x)
{
  return (float)sqrt(x);
}

inline float sqr(const float x)
{
 	return x*x;
}

inline float	invnum(const float x)
{
 	return 1.0f/x;
}


inline float vec3::norm()
{
		return root(x*x+y*y+z*z);
}

inline float vec3::sqrnorm()
{
		return x*x+y*y+z*z;
}

inline void vec3::normalize()
{
		float invnorm=invnum(root(x*x+y*y+z*z));
		x*=invnorm;
		y*=invnorm;
		z*=invnorm;
}


inline vec3::vec3(const float xin,const float yin,const float zin)
{
		x=xin;
		y=yin;
		z=zin;
		w=1.0f;
}


inline bool operator == (const vec3& v1,const vec3& v2)
{
		return ((v1.x==v2.x)&&(v1.y==v2.y)&&(v1.z==v2.z));
}

inline bool operator != (const vec3& v1,const vec3& v2)
{
		return ((v1.x!=v2.x)||(v1.y!=v2.y)||(v1.z!=v2.z));
}

inline bool equality(const vec3& v1,const vec3& v2,const float e)
{
		return ((fabs(v1.x-v2.x)<e)&&(fabs(v1.y-v2.y)<e)&&(fabs(v1.z-v2.z)<e));
}



inline vec3 operator +	(const vec3& v1,const vec3& v2)
{
		return	vec3(v1.x+v2.x,v1.y+v2.y,v1.z+v2.z);
}

inline vec3 operator -	(const vec3& v1,const vec3& v2)
{
		return	vec3(v1.x-v2.x,v1.y-v2.y,v1.z-v2.z);
}

inline vec3 operator *	(const vec3& v,const float t)
{
		return	vec3(v.x*t,v.y*t,v.z*t);
}

inline vec3 operator *	(const float t,const vec3& v)
{
		return	vec3(v.x*t,v.y*t,v.z*t);
}

inline vec3& operator *=	( vec3& v,const float t)
{
		v.x*=t;
		v.y*=t;
		v.z*=t;

		return v;
}

inline vec3 operator /	(const vec3& v,const float t)
{
                float it=invnum(t);
		return	vec3(v.x*it,v.y*it,v.z*it);
}

inline vec3& operator /=(vec3& v,const float t)
{
		float it=invnum(t);

		v.x*=it;
		v.y*=it;
		v.z*=it;

		return	v;
}

inline vec3& operator +=	( vec3& v1,const vec3& v2)
{
		v1.x+=v2.x;
		v1.y+=v2.y;
		v1.z+=v2.z;

		return	v1;
}

inline vec3& operator -=	( vec3& v1,const vec3& v2)
{
		v1.x-=v2.x;
		v1.y-=v2.y;
		v1.z-=v2.z;

		return	v1;
}

inline vec3& operator -	( vec3& v)
{
		v.x=-v.x;
		v.y=-v.y;
		v.z=-v.z;

		return	v;
}


inline	float operator  * (const vec3& v1,const vec3& v2)
{
		return (v1.x*v2.x+v1.y*v2.y+v1.z*v2.z);
}

inline vec3 operator & (const vec3& v1,const vec3& v2)
{
		vec3 v3;

		v3.x=v1.y*v2.z-v1.z*v2.y;
		v3.y=-(v1.x*v2.z-v1.z*v2.x);
		v3.z=v1.x*v2.y-v1.y*v2.x;

		return v3;
}

inline void vec3::operator = (const float*	fvectorp)
{
		x=fvectorp[0];
		y=fvectorp[1];
		z=fvectorp[2];
		w=1.0f;
}




inline void vec3::operator = (const vec3&	fv)
{
		x=fv.x;
		y=fv.y;
		z=fv.z;
		w=1.0f;
}

inline float norm(const vec3 &v)
{
 	     return (float)sqrt(sqr(v.x)+sqr(v.y)+sqr(v.z));
}

inline float sqrnorm(const vec3 &v)
{
 	     return sqr(v.x)+sqr(v.y)+sqr(v.z);
}


inline vec3 normalize(vec3 &v)
{
		float invnorm=invnum(v.norm());
		return vec3(v.x*invnorm,v.y*invnorm,v.z*invnorm);
}


inline float dist(const vec3 &v1,const vec3 &v2)
{
		return root(sqr(v1.x-v2.x)+sqr(v1.y-v2.y)+sqr(v1.z-v2.z));
}

inline float sqrdist(const vec3 &v1,const vec3 &v2)
{
		return sqr(v1.x-v2.x)+sqr(v1.y-v2.y)+sqr(v1.z-v2.z);
}

inline vec3 operator * (const mat3x3& m,const vec3& v)
{
		vec3 newv;

		newv.x=m.m11*v.x+m.m12*v.y+m.m13*v.z;
		newv.y=m.m21*v.x+m.m22*v.y+m.m23*v.z;
		newv.z=m.m31*v.x+m.m32*v.y+m.m33*v.z;

		return newv;
}

inline vec3 operator * (const vec3& v,const mat3x3& m)
{
		vec3 newv;

		newv.x=m.m11*v.x+m.m21*v.y+m.m31*v.z;
		newv.y=m.m12*v.x+m.m22*v.y+m.m32*v.z;
		newv.z=m.m13*v.x+m.m23*v.y+m.m33*v.z;

		return newv;
}

inline mat3x3 operator * (const mat3x3& m,const float c)
{
		mat3x3 newm;

		newm.m11=m.m11*c;
		newm.m12=m.m12*c;
		newm.m13=m.m13*c;

		newm.m21=m.m21*c;
		newm.m22=m.m22*c;
		newm.m23=m.m23*c;

		newm.m31=m.m31*c;
		newm.m32=m.m32*c;
		newm.m33=m.m33*c;

		return newm;
}




inline mat3x3 operator * (const float &c,const mat3x3& m)
{
		mat3x3 newm;

		newm.m11=m.m11*c;
		newm.m12=m.m12*c;
		newm.m13=m.m13*c;

		newm.m21=m.m21*c;
		newm.m22=m.m22*c;
		newm.m23=m.m23*c;

		newm.m31=m.m31*c;
		newm.m32=m.m32*c;
		newm.m33=m.m33*c;

		return newm;
}

inline mat3x3& operator - ( mat3x3& a)
{
		a.m11=-a.m11;
		a.m12=-a.m11;
		a.m13=-a.m11;

		a.m21=-a.m21;
		a.m22=-a.m22;
		a.m23=-a.m23;

		a.m31=-a.m31;
		a.m32=-a.m32;
		a.m33=-a.m33;

		return a;
}


inline mat3x3 operator + (const mat3x3& a,const mat3x3& b)
{
		mat3x3 c;

		c.m11=a.m11+b.m11;
		c.m12=a.m12+b.m12;
		c.m13=a.m13+b.m13;

		c.m21=a.m21+b.m21;
		c.m22=a.m22+b.m22;
		c.m23=a.m23+b.m23;

		c.m31=a.m31+b.m31;
		c.m32=a.m32+b.m32;
		c.m33=a.m33+b.m33;

		return c;
}

inline mat3x3& operator += ( mat3x3& a,const mat3x3& b)
{
		a.m11+=b.m11;
		a.m12+=b.m11;
		a.m13+=b.m11;

		a.m21+=b.m21;
		a.m22+=b.m22;
		a.m23+=b.m23;

		a.m31+=b.m31;
		a.m32+=b.m32;
		a.m33+=b.m33;

		return a;
}

inline mat3x3 operator - (const mat3x3& a,const mat3x3& b)
{
		mat3x3 c;

		c.m11=a.m11-b.m11;
		c.m12=a.m12-b.m12;
		c.m13=a.m13-b.m13;

		c.m21=a.m21-b.m21;
		c.m22=a.m22-b.m22;
		c.m23=a.m23-b.m23;

		c.m31=a.m31-b.m31;
		c.m32=a.m32-b.m32;
		c.m33=a.m33-b.m33;

		return c;
}

inline mat3x3& operator -= ( mat3x3& a,const mat3x3& b)
{
		a.m11-=b.m11;
		a.m12-=b.m11;
		a.m13-=b.m11;

		a.m21-=b.m21;
		a.m22-=b.m22;
		a.m23-=b.m23;

		a.m31-=b.m31;
		a.m32-=b.m32;
		a.m33-=b.m33;

		return a;
}


inline mat3x3 operator *	(const mat3x3& a,const mat3x3& b)
{
		mat3x3 m;

		m.m11=a.m11*b.m11+a.m12*b.m21+a.m13*b.m31;
		m.m21=a.m21*b.m11+a.m22*b.m21+a.m23*b.m31;
		m.m31=a.m31*b.m11+a.m32*b.m21+a.m33*b.m31;

		m.m12=a.m11*b.m12+a.m12*b.m22+a.m13*b.m32;
		m.m22=a.m21*b.m12+a.m22*b.m22+a.m23*b.m32;
		m.m32=a.m31*b.m12+a.m32*b.m22+a.m33*b.m32;

		m.m13=a.m11*b.m13+a.m12*b.m23+a.m13*b.m33;
		m.m23=a.m21*b.m13+a.m22*b.m23+a.m23*b.m33;
		m.m33=a.m31*b.m13+a.m32*b.m23+a.m33*b.m33;

		return m;
}




typedef mat3x3 mat;
typedef mat3x3 mat3;
typedef mat3x3 matrix;
typedef vec3 vec;

#endif



