math.h

这是VCF框架的代码

		return *(&x+i);
	}
};

inline vrReal length(const Vector4 &vec){
	return (vrReal)sqrt((vrReal)(vec.x*vec.x+vec.y*vec.y+vec.z*vec.z+vec.w*vec.w));
}

// TODO: return void?
inline Vector4 normalize(Vector4 &vec){
	vrReal l=1/length(vec);
	vec.x*=l;
	vec.y*=l;
	vec.z*=l;
	vec.w*=l;
	return vec;
}

inline Vector4 normalize(const Vector4 &vec){
	vrReal l=1/length(vec);
	Vector4 vec2;
	vec2.x=vec.x*l;
	vec2.y=vec.y*l;
	vec2.z=vec.z*l;
	vec2.w=vec.w*l;
	return vec2;
}

inline Vector4 operator*(vrReal scalar,const Vector4 &vector){
	return Vector4(vector.x*scalar,vector.y*scalar,vector.z*scalar,vector.w*scalar);
}

inline vrReal dot(const Vector4 &vec1,const Vector4 &vec2){
	return vec1.x*vec2.x+vec1.y*vec2.y+vec1.z*vec2.z+vec1.w*vec2.w;
}

//Matrix3x3
class Matrix3x3{
public:
	vrReal data[9];

	class RowAccessor{
	public:
		inline RowAccessor(Matrix3x3 *m,int r){
			matrix=m;
			row=r;
		}

		inline vrReal &operator[](int col){
			return matrix->at(row,col);
		}

		inline vrReal operator[](int col) const{
			return matrix->at(row,col);
		}

		Matrix3x3 *matrix;
		int row;
	};

	inline Matrix3x3(){
		data[0]=1;data[3]=0;data[6]=0;
		data[1]=0;data[4]=1;data[7]=0;
		data[2]=0;data[5]=0;data[8]=1;
	}

	inline Matrix3x3(vrReal x1,vrReal x2,vrReal x3,vrReal y1,vrReal y2,vrReal y3,vrReal z1,vrReal z2,vrReal z3){
		data[0]=x1;data[3]=x2;data[6]=x3;
		data[1]=y1;data[4]=y2;data[7]=y3;
		data[2]=z1;data[5]=z2;data[8]=z3;
	}

	inline RowAccessor operator[](int row){
		return RowAccessor(this,row);
	}

	inline vrReal at(int row,int col) const{
		return data[col*3+row];
	}

	inline vrReal &at(int row,int col){
		return data[col*3+row];
	}

	inline vrReal *getData(){return (vrReal*)data;}
	inline const vrReal *getData() const{return (vrReal*)data;}

	inline bool operator==(const Matrix3x3 &m2) const{
		return	data[0]==m2.data[0] && data[1]==m2.data[1] && data[2]==m2.data[2] &&
				data[3]==m2.data[3] && data[4]==m2.data[4] && data[5]==m2.data[5] &&
				data[6]==m2.data[6] && data[7]==m2.data[7] && data[8]==m2.data[8];
	}

	inline bool operator!=(const Matrix3x3 &m2) const{
		return	!(*this==m2);
	}

	Matrix3x3 operator*(const Matrix3x3 &m2) const{
		Matrix3x3 dest;

#define A(row,col)  at(row,col)
#define B(row,col)  m2.at(row,col)
#define D(row,col)  dest.at(row,col)

		D(0,0)=A(0,0)*B(0,0)+A(0,1)*B(1,0)+A(0,2)*B(2,0);
		D(0,1)=A(0,0)*B(0,1)+A(0,1)*B(1,1)+A(0,2)*B(2,1);
		D(0,2)=A(0,0)*B(0,2)+A(0,1)*B(1,2)+A(0,2)*B(2,2);

		D(1,0)=A(1,0)*B(0,0)+A(1,1)*B(1,0)+A(1,2)*B(2,0);
		D(1,1)=A(1,0)*B(0,1)+A(1,1)*B(1,1)+A(1,2)*B(2,1);
		D(1,2)=A(1,0)*B(0,2)+A(1,1)*B(1,2)+A(1,2)*B(2,2);

		D(2,0)=A(2,0)*B(0,0)+A(2,1)*B(1,0)+A(2,2)*B(2,0);
		D(2,1)=A(2,0)*B(0,1)+A(2,1)*B(1,1)+A(2,2)*B(2,1);
		D(2,2)=A(2,0)*B(0,2)+A(2,1)*B(1,2)+A(2,2)*B(2,2);

#undef A
#undef B
#undef D

		return dest;
	}

	inline Vector3 operator*(const Vector3 &vec) const{
		return Vector3(	at(0,0)*vec.x+at(0,1)*vec.y+at(0,2)*vec.z,
						at(1,0)*vec.x+at(1,1)*vec.y+at(1,2)*vec.z,
						at(2,0)*vec.x+at(2,1)*vec.y+at(2,2)*vec.z);
	}

	inline void mult(const Vector3 &vec,Vector3 &dest) const{
		dest.x=at(0,0)*vec.x+at(0,1)*vec.y+at(0,2)*vec.z;
		dest.y=at(1,0)*vec.x+at(1,1)*vec.y+at(1,2)*vec.z;
		dest.z=at(2,0)*vec.x+at(2,1)*vec.y+at(2,2)*vec.z;
	}
};

inline void setTranspose(Matrix3x3 &res,const Matrix3x3 &src){
	res.at(0,0)=src.at(0,0);
	res.at(0,1)=src.at(1,0);
	res.at(0,2)=src.at(2,0);
	res.at(1,0)=src.at(0,1);
	res.at(1,1)=src.at(1,1);
	res.at(1,2)=src.at(2,1);
	res.at(2,0)=src.at(0,2);
	res.at(2,1)=src.at(1,2);
	res.at(2,2)=src.at(2,2);
}

inline Matrix3x3 makeTranspose(const Matrix3x3 &src){
	Matrix3x3 res;
	setTranspose(res,src);
	return res;
}

inline double determinant(const Matrix3x3 &src){
	return -src.at(0,0)*src.at(1,1)*src.at(2,2)+src.at(0,0)*src.at(2,1)*src.at(1,2)+src.at(0,1)*src.at(1,0)*src.at(2,2)-src.at(0,1)*src.at(2,0)*src.at(1,2)-src.at(0,2)*src.at(1,0)*src.at(2,1)+src.at(0,2)*src.at(2,0)*src.at(1,1);
}

inline bool setInverse(Matrix3x3 &res,const Matrix3x3 src){
	double det=1.0/determinant(src);
	if(det==0){
		return false;
	}
	else{
		res.at(0,0)=-(src.at(1,1)*src.at(2,2)-src.at(2,1)*src.at(1,2))*det;
		res.at(1,0)=(src.at(1,0)*src.at(2,2)-src.at(2,0)*src.at(1,2))*det;
		res.at(2,0)=-(src.at(1,0)*src.at(2,1)-src.at(2,0)*src.at(1,1))*det;

		res.at(0,1)=(src.at(0,1)*src.at(2,2)-src.at(2,1)*src.at(0,2))*det;
		res.at(1,1)=-(src.at(0,0)*src.at(2,2)-src.at(2,0)*src.at(0,2))*det;
		res.at(2,1)=(src.at(0,0)*src.at(2,1)-src.at(2,0)*src.at(0,1))*det;
		
		res.at(0,2)=-(src.at(0,1)*src.at(1,2)-src.at(1,1)*src.at(0,2))*det;
		res.at(1,2)=(src.at(0,0)*src.at(1,2)-src.at(1,0)*src.at(0,2))*det;
		res.at(2,2)=-(src.at(0,0)*src.at(1,1)-src.at(1,0)*src.at(0,1))*det;
		
		return true;
	}
}

inline Matrix3x3 makeInverse(const Matrix3x3 &src){
	Matrix3x3 res;
	setInverse(res,src);
	return res;
}

inline EulerAngle makeEulerAngle(const Matrix3x3 &mat){
	EulerAngle euler;

	euler.y=asin(-mat.at(2,0));

	if(euler.y<vrPi/2){
		if(euler.y>-vrPi/2){
			euler.z=atan2(mat.at(1,0),mat.at(0,0));
			euler.x=atan2(mat.at(2,1),mat.at(2,2));
		}
		else{
			euler.z=-atan2(-mat.at(0,1),mat.at(0,2));
			euler.x=0;
		}
	}
	else{
		euler.z=atan2(-mat.at(0,1),mat.at(0,2));
		euler.x=0;
	}

	return euler;
}

inline Matrix3x3 makeMatrix3x3FromX(vrReal z){
	return Matrix3x3(	1,0,0,
						0,(vrReal)cos(z),-(vrReal)sin(z),
						0,(vrReal)sin(z),(vrReal)cos(z));
}

inline Matrix3x3 makeMatrix3x3FromY(vrReal y){
	return Matrix3x3(	(vrReal)cos(y),0,(vrReal)sin(y),
						0,1,0,
						-(vrReal)sin(y),0,(vrReal)cos(y));
}

inline Matrix3x3 makeMatrix3x3FromZ(vrReal x){
	return Matrix3x3(	(vrReal)cos(x),-(vrReal)sin(x),0,
						(vrReal)sin(x),(vrReal)cos(x),0,
						0,0,1);
}

inline Matrix3x3 makeMatrix3x3(const EulerAngle &angles){
	return makeMatrix3x3FromX(angles.x)*makeMatrix3x3FromY(angles.y)*makeMatrix3x3FromZ(angles.z);
}

inline void setMatrix3x3FromDirCos(Matrix3x3 &res,const Vector3 &xDest,const Vector3 &yDest,const Vector3 &zDest,const Vector3 &xSrc,const Vector3 &ySrc,const Vector3 &zSrc){
	vrReal Xa,Xb,Xc;
	vrReal Ya,Yb,Yc;
	vrReal Za,Zb,Zc;

	Xa=dot(xDest,xSrc);Xb=dot(xDest,ySrc);Xc=dot(xDest,zSrc);
	Ya=dot(yDest,xSrc);Yb=dot(yDest,ySrc);Yc=dot(yDest,zSrc);
	Za=dot(zDest,xSrc);Zb=dot(zDest,ySrc);Zc=dot(zDest,zSrc);

	res.at(0,0)=Xa;res.at(0,1)=Ya;res.at(0,2)=Za;
	res.at(1,0)=Xb;res.at(1,1)=Yb;res.at(1,2)=Zb;
	res.at(2,0)=Xc;res.at(2,1)=Yc;res.at(2,2)=Zc;
}

inline Matrix3x3 makeMatrix3x3FromDirCos(const Vector3 &xDest,const Vector3 &yDest,const Vector3 &zDest,const Vector3 &xSrc,const Vector3 &ySrc,const Vector3 &zSrc){
	Matrix3x3 res;
	setMatrix3x3FromDirCos(res,xDest,yDest,zDest,xSrc,ySrc,zSrc);
	return res;
}

inline void setMatrix3x3FromAxis(Matrix3x3 &res,const Vector3 &xAxis,const Vector3 &yAxis,const Vector3 &zAxis){
	res.at(0,0)=xAxis[0];
	res.at(1,0)=xAxis[1];
	res.at(2,0)=xAxis[2];

	res.at(0,1)=yAxis[0];
	res.at(1,1)=yAxis[1];
	res.at(2,1)=yAxis[2];

	res.at(0,2)=zAxis[0];
	res.at(1,2)=zAxis[1];
	res.at(2,2)=zAxis[2];
}

inline Matrix3x3 makeMatrix3x3FromAxis(const Vector3 &xAxis,const Vector3 &yAxis,const Vector3 &zAxis){
	Matrix3x3 res;
	setMatrix3x3FromAxis(res,xAxis,yAxis,zAxis);
	return res;
}

class Matrix4x4{
public:
	vrReal data[16];

	class RowAccessor{
	public:
		inline RowAccessor(Matrix4x4 *m,int r){
			matrix=m;
			row=r;
		}

		inline vrReal &operator[](int col){
			return matrix->at(row,col);
		}

		inline vrReal operator[](int col) const{
			return matrix->at(row,col);
		}

		Matrix4x4 *matrix;
		int row;
	};

	inline Matrix4x4(){
		data[0]=1;data[4]=0;data[8]=0;data[12]=0;
		data[1]=0;data[5]=1;data[9]=0;data[13]=0;
		data[2]=0;data[6]=0;data[10]=1;data[14]=0;
		data[3]=0;data[7]=0;data[11]=0;data[15]=1;
	}

	inline Matrix4x4(vrReal x1,vrReal x2,vrReal x3,vrReal x4,vrReal y1,vrReal y2,vrReal y3,vrReal y4,vrReal z1,vrReal z2,vrReal z3,vrReal z4,vrReal w1,vrReal w2,vrReal w3,vrReal w4){
		data[0]=x1;data[4]=x2;data[8]=x3;data[12]=x4;
		data[1]=y1;data[5]=y2;data[9]=y3;data[13]=y4;
		data[2]=z1;data[6]=z2;data[10]=z3;data[14]=z4;
		data[3]=w1;data[7]=w2;data[11]=w3;data[15]=w4;
	}

	inline RowAccessor operator[](int row){
		return RowAccessor(this,row);
	}

	inline vrReal at(int row,int col) const{
		return data[col*4+row];
	}

	inline vrReal &at(int row,int col){
		return data[col*4+row];
	}

	inline vrReal *getData(){return (vrReal*)data;}
	inline const vrReal *getData() const{return (vrReal*)data;}

	Matrix4x4 operator*(const Matrix4x4 &m2) const{
		Matrix4x4 dest;

#define A(row,col)  at(row,col)
#define B(row,col)  m2.at(row,col)
#define D(row,col)  dest.at(row,col)

	D(0,0)=A(0,0)*B(0,0)+A(0,1)*B(1,0)+A(0,2)*B(2,0)+A(0,3)*B(3,0);
	D(0,1)=A(0,0)*B(0,1)+A(0,1)*B(1,1)+A(0,2)*B(2,1)+A(0,3)*B(3,1);
	D(0,2)=A(0,0)*B(0,2)+A(0,1)*B(1,2)+A(0,2)*B(2,2)+A(0,3)*B(3,2);
	D(0,3)=A(0,0)*B(0,3)+A(0,1)*B(1,3)+A(0,2)*B(2,3)+A(0,3)*B(3,3);

	D(1,0)=A(1,0)*B(0,0)+A(1,1)*B(1,0)+A(1,2)*B(2,0)+A(1,3)*B(3,0);
	D(1,1)=A(1,0)*B(0,1)+A(1,1)*B(1,1)+A(1,2)*B(2,1)+A(1,3)*B(3,1);
	D(1,2)=A(1,0)*B(0,2)+A(1,1)*B(1,2)+A(1,2)*B(2,2)+A(1,3)*B(3,2);
	D(1,3)=A(1,0)*B(0,3)+A(1,1)*B(1,3)+A(1,2)*B(2,3)+A(1,3)*B(3,3);

	D(2,0)=A(2,0)*B(0,0)+A(2,1)*B(1,0)+A(2,2)*B(2,0)+A(2,3)*B(3,0);
	D(2,1)=A(2,0)*B(0,1)+A(2,1)*B(1,1)+A(2,2)*B(2,1)+A(2,3)*B(3,1);
	D(2,2)=A(2,0)*B(0,2)+A(2,1)*B(1,2)+A(2,2)*B(2,2)+A(2,3)*B(3,2);
	D(2,3)=A(2,0)*B(0,3)+A(2,1)*B(1,3)+A(2,2)*B(2,3)+A(2,3)*B(3,3);

	D(3,0)=A(3,0)*B(0,0)+A(3,1)*B(1,0)+A(3,2)*B(2,0)+A(3,3)*B(3,0);
	D(3,1)=A(3,0)*B(0,1)+A(3,1)*B(1,1)+A(3,2)*B(2,1)+A(3,3)*B(3,1);
	D(3,2)=A(3,0)*B(0,2)+A(3,1)*B(1,2)+A(3,2)*B(2,2)+A(3,3)*B(3,2);
	D(3,3)=A(3,0)*B(0,3)+A(3,1)*B(1,3)+A(3,2)*B(2,3)+A(3,3)*B(3,3);

#undef D
#undef B
#undef A

		return dest;
	}

	inline Vector4 operator*(const Vector4 &vec) const{
		return Vector4(	at(0,0)*vec.x+at(0,1)*vec.y+at(0,2)*vec.z+at(0,3)*vec.w,
						at(1,0)*vec.x+at(1,1)*vec.y+at(1,2)*vec.z+at(1,3)*vec.w,
						at(2,0)*vec.x+at(2,1)*vec.y+at(2,2)*vec.z+at(2,3)*vec.w,
						at(3,0)*vec.x+at(3,1)*vec.y+at(3,2)*vec.z+at(3,3)*vec.w);
	}

	inline void mult(const Vector4 &vec,Vector4 &dest) const{
		dest.x=at(0,0)*vec.x+at(0,1)*vec.y+at(0,2)*vec.z+at(0,3)*vec.w;
		dest.y=at(1,0)*vec.x+at(1,1)*vec.y+at(1,2)*vec.z+at(1,3)*vec.w;
		dest.z=at(2,0)*vec.x+at(2,1)*vec.y+at(2,2)*vec.z+at(2,3)*vec.w;
		dest.z=at(3,0)*vec.x+at(3,1)*vec.y+at(3,2)*vec.z+at(3,3)*vec.w;
	}
};

inline void setMatrix3x3(Matrix3x3 &res,const Matrix4x4 &src){
	for(int i=0;i<3;i++){
		res.at(0,i)=src.at(0,i);
		res.at(1,i)=src.at(1,i);
		res.at(2,i)=src.at(2,i);
	}
}

inline Matrix3x3 makeMatrix3x3(const Matrix4x4 &src){
	Matrix3x3 res;
	setMatrix3x3(res,src);
	return res;
}

inline void setVector3(Vector3 &res,const Matrix4x4 &src){
	res.x=src.at(0,3);
	res.y=src.at(1,3);
	res.z=src.at(2,3);
}

inline Vector3 makeVector3(const Matrix4x4 &src){
	Vector3 res;
	setVector3(res,src);
	return res;
}

inline void setTranspose(Matrix4x4 &res,const Matrix4x4 &src){
	res.at(0,0)=src.at(0,0);
	res.at(0,1)=src.at(1,0);
	res.at(0,2)=src.at(2,0);
	res.at(0,3)=src.at(3,0);

	res.at(1,0)=src.at(0,1);
	res.at(1,1)=src.at(1,1);
	res.at(1,2)=src.at(2,1);
	res.at(1,3)=src.at(3,1);

	res.at(2,0)=src.at(0,2);
	res.at(2,1)=src.at(1,2);
	res.at(2,2)=src.at(2,2);
	res.at(2,3)=src.at(3,2);

	res.at(3,0)=src.at(0,3);
	res.at(3,1)=src.at(1,3);
	res.at(3,2)=src.at(2,3);
	res.at(3,3)=src.at(3,3);
}

inline Matrix4x4 makeTranspose(const Matrix4x4 &src){
	Matrix4x4 res;
	setTranspose(res,src);
	return res;
}

inline double determinant(const Matrix4x4 &src){
	double det1 = src.at(1,2) * src.at(2,3) - src.at(2,2) * src.at(1,3);
	double det2 = src.at(1,1) * src.at(2,3) - src.at(2,1) * src.at(1,3);
	double det3 = src.at(1,1) * src.at(2,2) - src.at(2,1) * src.at(1,2);
	double det4 = src.at(1,0) * src.at(2,3) - src.at(2,0) * src.at(1,3);
	double det5 = src.at(1,0) * src.at(2,2) - src.at(2,0) * src.at(1,2);
	double det6 = src.at(1,0) * src.at(2,1) - src.at(2,0) * src.at(1,1);

	return -src.at(3,0) * (src.at(0,1) * det1 - src.at(0,2) * det2 + src.at(0,3) * det3) +
			src.at(3,1) * (src.at(0,0) * det1 - src.at(0,2) * det4 + src.at(0,3) * det5) -
			src.at(3,2) * (src.at(0,0) * det2 - src.at(0,1) * det4 + src.at(0,3) * det6) +
			src.at(3,3) * (src.at(0,0) * det3 - src.at(0,1) * det5 + src.at(0,2) * det6);
}

inline bool setInverse(Matrix4x4 &res,const Matrix4x4 &src){
	double det=determinant(src);

	if(fabs(det)<vrEpsilon){
		return false;
	}
	else{
		// calculate (transposed cofactor matrix / determinant of given matrix)
		det=(double)1.0/det;
		res.at(0,0) = det * (src.at(1,1) * (src.at(2,2) * src.at(3,3) - src.at(2,3) * src.at(3,2)) + src.at(1,2) * (src.at(2,3) * src.at(3,1) - src.at(2,1) * src.at(3,3)) + src.at(1,3) * (src.at(2,1) * src.at(3,2) - src.at(2,2) * src.at(3,1)));
		res.at(0,1) = det * (src.at(2,1) * (src.at(0,2) * src.at(3,3) - src.at(0,3) * src.at(3,2)) + src.at(2,2) * (src.at(0,3) * src.at(3,1) - src.at(0,1) * src.at(3,3)) + src.at(2,3) * (src.at(0,1) * src.at(3,2) - src.at(0,2) * src.at(3,1)));
		res.at(0,2) = det * (src.at(3,1) * (src.at(0,2) * src.at(1,3) - src.at(0,3) * src.at(1,2)) + src.at(3,2) * (src.at(0,3) * src.at(1,1) - src.at(0,1) * src.at(1,3)) + src.at(3,3) * (src.at(0,1) * src.at(1,2) - src.at(0,2) * src.at(1,1)));
		res.at(0,3) = det * (src.at(0,1) * (src.at(1,3) * src.at(2,2) - src.at(1,2) * src.at(2,3)) + src.at(0,2) * (src.at(1,1) * src.at(2,3) - src.at(1,3) * src.at(2,1)) + src.at(0,3) * (src.at(1,2) * src.at(2,1) - src.at(1,1) * src.at(2,2)));

		res.at(1,0) = det * (src.at(1,2) * (src.at(2,0) * src.at(3,3) - src.at(2,3) * src.at(3,0)) + src.at(1,3) * (src.at(2,2) * src.at(3,0) - src.at(2,0) * src.at(3,2)) + src.at(1,0) * (src.at(2,3) * src.at(3,2) - src.at(2,2) * src.at(3,3)));
		res.at(1,1) = det * (src.at(2,2) * (src.at(0,0) * src.at(3,3) - src.at(0,3) * src.at(3,0)) + src.at(2,3) * (src.at(0,2) * src.at(3,0) - src.at(0,0) * src.at(3,2)) + src.at(2,0) * (src.at(0,3) * src.at(3,2) - src.at(0,2) * src.at(3,3)));
		res.at(1,2) = det * (src.at(3,2) * (src.at(0,0) * src.at(1,3) - src.at(0,3) * src.at(1,0)) + src.at(3,3) * (src.at(0,2) * src.at(1,0) - src.at(0,0) * src.at(1,2)) + src.at(3,0) * (src.at(0,3) * src.at(1,2) - src.at(0,2) * src.at(1,3)));
		res.at(1,3) = det * (src.at(0,2) * (src.at(1,3) * src.at(2,0) - src.at(1,0) * src.at(2,3)) + src.at(0,3) * (src.at(1,0) * src.at(2,2) - src.at(1,2) * src.at(2,0)) + src.at(0,0) * (src.at(1,2) * src.at(2,3) - src.at(1,3) * src.at(2,2)));
		
		res.at(2,0) = det * (src.at(1,3) * (src.at(2,0) * src.at(3,1) - src.at(2,1) * src.at(3,0)) + src.at(1,0) * (src.at(2,1) * src.at(3,3) - src.at(2,3) * src.at(3,1)) + src.at(1,1) * (src.at(2,3) * src.at(3,0) - src.at(2,0) * src.at(3,3)));
		res.at(2,1) = det * (src.at(2,3) * (src.at(0,0) * src.at(3,1) - src.at(0,1) * src.at(3,0)) + src.at(2,0) * (src.at(0,1) * src.at(3,3) - src.at(0,3) * src.at(3,1)) + src.at(2,1) * (src.at(0,3) * src.at(3,0) - src.at(0,0) * src.at(3,3)));
		res.at(2,2) = det * (src.at(3,3) * (src.at(0,0) * src.at(1,1) - src.at(0,1) * src.at(1,0)) + src.at(3,0) * (src.at(0,1) * src.at(1,3) - src.at(0,3) * src.at(1,1)) + src.at(3,1) * (src.at(0,3) * src.at(1,0) - src.at(0,0) * src.at(1,3)));
		res.at(2,3) = det * (src.at(0,3) * (src.at(1,1) * src.at(2,0) - src.at(1,0) * src.at(2,1)) + src.at(0,0) * (src.at(1,3) * src.at(2,1) - src.at(1,1) * src.at(2,3)) + src.at(0,1) * (src.at(1,0) * src.at(2,3) - src.at(1,3) * src.at(2,0)));
		
		res.at(3,0) = det * (src.at(1,0) * (src.at(2,2) * src.at(3,1) - src.at(2,1) * src.at(3,2)) + src.at(1,1) * (src.at(2,0) * src.at(3,2) - src.at(2,2) * src.at(3,0)) + src.at(1,2) * (src.at(2,1) * src.at(3,0) - src.at(2,0) * src.at(3,1)));
		res.at(3,1) = det * (src.at(2,0) * (src.at(0,2) * src.at(3,1) - src.at(0,1) * src.at(3,2)) + src.at(2,1) * (src.at(0,0) * src.at(3,2) - src.at(0,2) * src.at(3,0)) + src.at(2,2) * (src.at(0,1) * src.at(3,0) - src.at(0,0) * src.at(3,1)));
		res.at(3,2) = det * (src.at(3,0) * (src.at(0,2) * src.at(1,1) - src.at(0,1) * src.at(1,2)) + src.at(3,1) * (src.at(0,0) * src.at(1,2) - src.at(0,2) * src.at(1,0)) + src.at(3,2) * (src.at(0,1) * src.at(1,0) - src.at(0,0) * src.at(1,1)));
		res.at(3,3) = det * (src.at(0,0) * (src.at(1,1) * src.at(2,2) - src.at(1,2) * src.at(2,1)) + src.at(0,1) * (src.at(1,2) * src.at(2,0) - src.at(1,0) * src.at(2,2)) + src.at(0,2) * (src.at(1,0) * src.at(2,1) - src.at(1,1) * src.at(2,0)));

		return true;
	}
}

inline Matrix4x4 makeInverse(const Matrix4x4 &src){
	Matrix4x4 res;
	setInverse(res,src);
	return res;
}

inline void setMatrix4x4(Matrix4x4 &res,const Matrix3x3 &src){
	for(int i=0;i<3;i++){
		res.at(0,i)=src.at(0,i);
		res.at(1,i)=src.at(1,i);
		res.at(2,i)=src.at(2,i);
	}
}

inline Matrix4x4 makeMatrix4x4(const Matrix3x3 &src){
	Matrix4x4 res;
	setMatrix4x4(res,src);
	return res;
}

inline void setMatrix4x4(Matrix4x4 &res,const Vector3 &src){
	res.at(0,3)=src[0];
	res.at(1,3)=src[1];
	res.at(2,3)=src[2];
}

inline Matrix4x4 makeMatrix4x4(const Vector3 &src){
	Matrix4x4 res;
	setMatrix4x4(res,src);
	return res;
}

inline void setMatrix4x4(Matrix4x4 &res,const Matrix3x3 &rotate,const Vector3 &trans){
	setMatrix4x4(res,rotate);
	setMatrix4x4(res,trans);
}	

inline Matrix4x4 makeMatrix4x4(const Matrix3x3 &rotate,const Vector3 &trans){
	Matrix4x4 res;
	setMatrix4x4(res,rotate,trans);
	return res;
}

inline void setMatrix4x4FromScale(Matrix4x4 &mat,const Vector3 &scale){
	mat.data[0]=scale[0];	mat.data[4]=0;			mat.data[8]=0;			mat.data[12]=0;
	mat.data[1]=0;			mat.data[5]=scale[1];	mat.data[9]=0;			mat.data[13]=0;
	mat.data[2]=0;			mat.data[6]=0;			mat.data[10]=scale[2];	mat.data[14]=0;
	mat.data[3]=0;			mat.data[7]=0;			mat.data[11]=0;			mat.data[15]=1;
}

inline Matrix4x4 makeMatrix4x4FromScale(const Vector3 &scale){
	Matrix4x4 res;
	setMatrix4x4FromScale(res,scale);
	return res;
}

// Quaternion
class Quaternion{
public:
	vrReal x,y,z,w;

	inline Quaternion():x(0),y(0),z(0),w(0){}

	inline Quaternion(vrReal x1,vrReal y1,vrReal z1,vrReal w1):x(x1),y(y1),z(z1),w(w1){}

	inline vrReal *getData(){return (vrReal*)&x;}
	inline const vrReal *getData() const{return (vrReal*)&x;}

	inline Quaternion operator*(const Quaternion &q) const{
		return Quaternion(
			+x*q.w+y*q.z-z*q.y+w*q.x,