Лабы / 5 / ROMA / MATRIX

#include "vector.cpp"
class MATRIX {
	public :
		REAL v [4][4];
		MATRIX ( void );
		MATRIX ( MATRIX& m1, MATRIX& m2 );
		void Identity ( void );
		void Pitch ( REAL p );
		void Roll ( REAL r );
		void Yaw ( REAL y );
		void Translate ( POINT& Point );
		inline MATRIX& operator = ( MATRIX& m );
		void Split ( VECTOR& vRight, VECTOR& vUp, VECTOR& vNormal );
		void Compose ( VECTOR& vRight, VECTOR& vUp, VECTOR& vNormal );
		void Multiply ( int n, POINT *In, POINT *Out, POINT& Move );
		void Multiply ( int n, POINT *In, POINT *Out );
	};

MATRIX :: MATRIX ( void ){
	Identity ();
}


MATRIX :: MATRIX ( MATRIX& m1, MATRIX& m2 ){
	int i, j;

	for ( i = 0; i < 4; i++ ) {
		for ( j = 0; j < 4; j++ ) {
			v [i][j] =	( m1.v [i][0] * m2.v [0][j] ) +
					( m1.v [i][1] * m2.v [1][j] ) +
					( m1.v [i][2] * m2.v [2][j] ) +
					( m1.v [i][3] * m2.v [3][j] );
		}
	}
}


void MATRIX :: Identity ( void ){
	v [0][0] = 1.0;
	v [0][1] = 0.0;
	v [0][2] = 0.0;
	v [0][3] = 0.0;

	v [1][0] = 0.0;
	v [1][1] = 1.0;
	v [1][2] = 0.0;
	v [1][3] = 0.0;

	v [2][0] = 0.0;
	v [2][1] = 0.0;
	v [2][2] = 1.0;
	v [2][3] = 0.0;

	v [3][0] = 0.0;
	v [3][1] = 0.0;
	v [3][2] = 0.0;
	v [3][3] = 1.0;
}

void MATRIX :: Pitch ( REAL p ){
	REAL Sin, Cos;

	Sin = sin ( p );
	Cos = cos ( p );
	v [1][1] = Cos;
	v [1][2] = Sin;
	v [2][1] = -Sin;
	v [2][2] = Cos;
}

void MATRIX :: Roll ( REAL r ){
	REAL Sin, Cos;

	Sin = sin ( r );
	Cos = cos ( r );
	v [0][0] = Cos;
	v [0][1] = Sin;
	v [1][0] = -Sin;
	v [1][1] = Cos;
}

void MATRIX :: Yaw ( REAL y ){
	REAL Sin, Cos;

	Sin = sin ( y );
	Cos = cos ( y );
	v [0][0] = Cos;
	v [0][2] = -Sin;
	v [2][0] = Sin;
	v [2][2] = Cos;
}

void MATRIX :: Translate ( POINT& Point ){
	v [0][3] = Point.v [0];
	v [1][3] = Point.v [1];
	v [2][3] = Point.v [2];
}

MATRIX& MATRIX :: operator = ( MATRIX& m ){
	v [0][0] = m.v [0][0];
	v [0][1] = m.v [0][1];
	v [0][2] = m.v [0][2];
	v [0][3] = m.v [0][3];

	v [1][0] = m.v [1][0];
	v [1][1] = m.v [1][1];
	v [1][2] = m.v [1][2];
	v [1][3] = m.v [1][3];

	v [2][0] = m.v [2][0];
	v [2][1] = m.v [2][1];
	v [2][2] = m.v [2][2];
	v [2][3] = m.v [2][3];

	v [3][0] = m.v [3][0];
	v [3][1] = m.v [3][1];
	v [3][2] = m.v [3][2];
	v [3][3] = m.v [3][3];

	return ( *this );
}

void MATRIX :: Split ( VECTOR& vRight, VECTOR& vUp, VECTOR& vNormal ){

	vRight.v  [0] = v [0][0];
	vRight.v  [1] = v [0][1];
	vRight.v  [2] = v [0][2];

	vUp.v	  [0] = v [1][0];
	vUp.v	  [1] = v [1][1];
	vUp.v	  [2] = v [1][2];

	vNormal.v [0] = v [2][0];
	vNormal.v [1] = v [2][1];
	vNormal.v [2] = v [2][2];
}


void MATRIX :: Compose ( VECTOR& vRight, VECTOR& vUp, VECTOR& vNormal ){

	v [0][0] = vRight.v [0];
	v [0][1] = vRight.v [1];
	v [0][2] = vRight.v [2];

	v [1][0] = vUp.v [0];
	v [1][1] = vUp.v [1];
	v [1][2] = vUp.v [2];

	v [2][0] = vNormal.v [0];
	v [2][1] = vNormal.v [1];
	v [2][2] = vNormal.v [2];
}

void MATRIX :: Multiply ( int n, POINT *In, POINT *Out, POINT& Move ){
	REAL	x1, y1, z1, x2, y2, z2;

	while ( --n >= 0 ) {
		x1 = In [n].v [0] - Move.v [0];
		y1 = In [n].v [1] - Move.v [1];
		z1 = In [n].v [2] - Move.v [2];
		x2 = ( x1 * v [0][0] ) + ( y1 * v [0][1] ) +
				( z1 * v [0][2] );
		y2 = ( x1 * v [1][0] ) + ( y1 * v [1][1] ) +
				( z1 * v [1][2] );
		z2 = ( x1 * v [2][0] ) + ( y1 * v [2][1] ) +
				( z1 * v [2][2] );

		Out [n].v [0] = x2;
		Out [n].v [1] = y2;
		Out [n].v [2] = z2;
	}
}

void MATRIX :: Multiply ( int n, POINT *In, POINT *Out ){
	REAL	x, y, z;
	while ( --n >= 0 ) {

		x = In [n].v [0];
		y = In [n].v [1];
		z = In [n].v [2];

		Out [n].v [0] = ( x * v [0][0] ) + ( y * v [0][1] ) +
				( z * v [0][2] );
		Out [n].v [1] = ( x * v [1][0] ) + ( y * v [1][1] ) +
				( z * v [1][2] );
		Out [n].v [2] = ( x * v [2][0] ) + ( y * v [2][1] ) +
				( z * v [2][2] );
	}
}