Quake-III-Arena

Quake III Arena GPL Source Release
Log | Files | Refs

math_matrix.cpp (3299B)

      1 /*
      2 ===========================================================================
      3 Copyright (C) 1999-2005 Id Software, Inc.
      4 
      5 This file is part of Quake III Arena source code.
      6 
      7 Quake III Arena source code is free software; you can redistribute it
      8 and/or modify it under the terms of the GNU General Public License as
      9 published by the Free Software Foundation; either version 2 of the License,
     10 or (at your option) any later version.
     11 
     12 Quake III Arena source code is distributed in the hope that it will be
     13 useful, but WITHOUT ANY WARRANTY; without even the implied warranty of
     14 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
     15 GNU General Public License for more details.
     16 
     17 You should have received a copy of the GNU General Public License
     18 along with Foobar; if not, write to the Free Software
     19 Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
     20 ===========================================================================
     21 */
     22 #include "q_shared.h"
     23 
     24 mat3_t mat3_default( idVec3_t( 1, 0, 0 ), idVec3_t( 0, 1, 0 ), idVec3_t( 0, 0, 1 ) );
     25 
     26 void toMatrix( quat_t const &src, mat3_t &dst ) {
     27 	float	wx, wy, wz;
     28 	float	xx, yy, yz;
     29 	float	xy, xz, zz;
     30 	float	x2, y2, z2;
     31 
     32 	x2 = src.x + src.x;
     33 	y2 = src.y + src.y;
     34 	z2 = src.z + src.z;
     35 
     36 	xx = src.x * x2;
     37 	xy = src.x * y2;
     38 	xz = src.x * z2;
     39 
     40 	yy = src.y * y2;
     41 	yz = src.y * z2;
     42 	zz = src.z * z2;
     43 
     44 	wx = src.w * x2;
     45 	wy = src.w * y2;
     46 	wz = src.w * z2;
     47 
     48 	dst[ 0 ][ 0 ] = 1.0f - ( yy + zz );
     49 	dst[ 0 ][ 1 ] = xy - wz;
     50 	dst[ 0 ][ 2 ] = xz + wy;
     51 
     52 	dst[ 1 ][ 0 ] = xy + wz;
     53 	dst[ 1 ][ 1 ] = 1.0f - ( xx + zz );
     54 	dst[ 1 ][ 2 ] = yz - wx;
     55 
     56 	dst[ 2 ][ 0 ] = xz - wy;
     57 	dst[ 2 ][ 1 ] = yz + wx;
     58 	dst[ 2 ][ 2 ] = 1.0f - ( xx + yy );
     59 }
     60 
     61 void toMatrix( angles_t const &src, mat3_t &dst ) {
     62 	float			angle;
     63 	static float	sr, sp, sy, cr, cp, cy; // static to help MS compiler fp bugs
     64 		
     65 	angle = src.yaw * ( M_PI * 2.0f / 360.0f );
     66 	sy = sin( angle );
     67 	cy = cos( angle );
     68 
     69 	angle = src.pitch * ( M_PI * 2.0f / 360.0f );
     70 	sp = sin( angle );
     71 	cp = cos( angle );
     72 
     73 	angle = src.roll * ( M_PI * 2.0f / 360.0f );
     74 	sr = sin( angle );
     75 	cr = cos( angle );
     76 
     77 	dst[ 0 ].set( cp * cy, cp * sy, -sp );
     78 	dst[ 1 ].set( sr * sp * cy + cr * -sy, sr * sp * sy + cr * cy, sr * cp );
     79 	dst[ 2 ].set( cr * sp * cy + -sr * -sy, cr * sp * sy + -sr * cy, cr * cp );
     80 }
     81 
     82 void toMatrix( idVec3_t const &src, mat3_t &dst ) {
     83         angles_t sup = src;
     84         toMatrix(sup, dst);
     85 }
     86 
     87 void mat3_t::ProjectVector( const idVec3_t &src, idVec3_t &dst ) const {
     88 	dst.x = src * mat[ 0 ];
     89 	dst.y = src * mat[ 1 ];
     90 	dst.z = src * mat[ 2 ];
     91 }
     92 
     93 void mat3_t::UnprojectVector( const idVec3_t &src, idVec3_t &dst ) const {
     94 	dst = mat[ 0 ] * src.x + mat[ 1 ] * src.y + mat[ 2 ] * src.z;
     95 }
     96 
     97 void mat3_t::Transpose( mat3_t &matrix ) {
     98 	int	i;
     99 	int	j;
    100    
    101 	for( i = 0; i < 3; i++ ) {
    102 		for( j = 0; j < 3; j++ ) {
    103 			matrix[ i ][ j ] = mat[ j ][ i ];
    104         }
    105 	}
    106 }
    107 
    108 void mat3_t::Transpose( void ) {
    109 	float	temp;
    110 	int		i;
    111 	int		j;
    112    
    113 	for( i = 0; i < 3; i++ ) {
    114 		for( j = i + 1; j < 3; j++ ) {
    115 			temp = mat[ i ][ j ];
    116 			mat[ i ][ j ] = mat[ j ][ i ];
    117 			mat[ j ][ i ] = temp;
    118         }
    119 	}
    120 }
    121 
    122 mat3_t mat3_t::Inverse( void ) const {
    123 	mat3_t inv( *this );
    124 
    125 	inv.Transpose();
    126 
    127 	return inv;
    128 }
    129 
    130 void mat3_t::Clear( void ) {
    131 	mat[0].set( 1, 0, 0 );
    132 	mat[1].set( 0, 1, 0 );
    133 	mat[2].set( 0, 0, 1 );
    134 }