libs/splines/math_matrix.h

   1 /*
   2    Copyright (C) 1999-2006 Id Software, Inc. and contributors.
   3    For a list of contributors, see the accompanying CONTRIBUTORS file.
   4
   5    This file is part of GtkRadiant.
   6
   7    GtkRadiant is free software; you can redistribute it and/or modify
   8    it under the terms of the GNU General Public License as published by
   9    the Free Software Foundation; either version 2 of the License, or
  10    (at your option) any later version.
  11
  12    GtkRadiant is distributed in the hope that it will be useful,
  13    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 GtkRadiant; if not, write to the Free Software
  19    Foundation, Inc., 51 Franklin St, Fifth Floor, Boston, MA  02110-1301  USA
  20  */
  21
  22 #ifndef __MATH_MATRIX_H__
  23 #define __MATH_MATRIX_H__
  24
  25 #include <string.h>
  26 #include "math_vector.h"
  27
  28 class quat_t;
  29 class angles_t;
  30
  31 class mat3_t {
  32 public:
  33 idVec3 mat[ 3 ];
  34
  35 mat3_t();
  36 mat3_t( float src[ 3 ][ 3 ] );
  37 mat3_t( idVec3 const &x, idVec3 const &y, idVec3 const &z );
  38 mat3_t( const float xx, const float xy, const float xz, const float yx, const float yy, const float yz, const float zx, const float zy, const float zz );
  39
  40 friend void     toMatrix( quat_t const &src, mat3_t &dst );
  41 friend void     toMatrix( angles_t const &src, mat3_t &dst );
  42 friend void     toMatrix( idVec3 const &src, mat3_t &dst );
  43
  44 idVec3 operator[]( int index ) const;
  45 idVec3          &operator[]( int index );
  46
  47 idVec3 operator*( const idVec3 &vec ) const;
  48 mat3_t operator*( const mat3_t &a ) const;
  49 mat3_t operator*( float a ) const;
  50 mat3_t operator+( mat3_t const &a ) const;
  51 mat3_t operator-( mat3_t const &a ) const;
  52
  53 friend idVec3 operator*( const idVec3 &vec, const mat3_t &mat );
  54 friend mat3_t operator*( float a, mat3_t const &b );
  55
  56 mat3_t          &operator*=( float a );
  57 mat3_t          &operator+=( mat3_t const &a );
  58 mat3_t          &operator-=( mat3_t const &a );
  59
  60 void            Clear( void );
  61
  62 void            ProjectVector( const idVec3 &src, idVec3 &dst ) const;
  63 void            UnprojectVector( const idVec3 &src, idVec3 &dst ) const;
  64
  65 void            OrthoNormalize( void );
  66 void            Transpose( mat3_t &matrix );
  67 void            Transpose( void );
  68 mat3_t          Inverse( void ) const;
  69 void            Identity( void );
  70
  71 friend void     InverseMultiply( const mat3_t &inv, const mat3_t &b, mat3_t &dst );
  72 friend mat3_t   SkewSymmetric( idVec3 const &src );
  73 };
  74
  75 ID_INLINE mat3_t::mat3_t() {
  76 }
  77
  78 ID_INLINE mat3_t::mat3_t( float src[ 3 ][ 3 ] ) {
  79         //memcpy( mat, src, sizeof( src ) );
  80         for( unsigned int i = 0; i < 3; i++ ) {
  81                 mat[i].x = src[i][0];
  82                 mat[i].y = src[i][1];
  83                 mat[i].z = src[i][2];
  84         }
  85 }
  86
  87 ID_INLINE mat3_t::mat3_t( idVec3 const &x, idVec3 const &y, idVec3 const &z ) {
  88         mat[ 0 ].x = x.x; mat[ 0 ].y = x.y; mat[ 0 ].z = x.z;
  89         mat[ 1 ].x = y.x; mat[ 1 ].y = y.y; mat[ 1 ].z = y.z;
  90         mat[ 2 ].x = z.x; mat[ 2 ].y = z.y; mat[ 2 ].z = z.z;
  91 }
  92
  93 ID_INLINE mat3_t::mat3_t( const float xx, const float xy, const float xz, const float yx, const float yy, const float yz, const float zx, const float zy, const float zz ) {
  94         mat[ 0 ].x = xx; mat[ 0 ].y = xy; mat[ 0 ].z = xz;
  95         mat[ 1 ].x = yx; mat[ 1 ].y = yy; mat[ 1 ].z = yz;
  96         mat[ 2 ].x = zx; mat[ 2 ].y = zy; mat[ 2 ].z = zz;
  97 }
  98
  99 ID_INLINE idVec3 mat3_t::operator[]( int index ) const {
 100         assert( ( index >= 0 ) && ( index < 3 ) );
 101         return mat[ index ];
 102 }
 103
 104 ID_INLINE idVec3& mat3_t::operator[]( int index ) {
 105         assert( ( index >= 0 ) && ( index < 3 ) );
 106         return mat[ index ];
 107 }
 108
 109 ID_INLINE idVec3 mat3_t::operator*( const idVec3 &vec ) const {
 110         return idVec3(
 111                            mat[ 0 ].x * vec.x + mat[ 1 ].x * vec.y + mat[ 2 ].x * vec.z,
 112                            mat[ 0 ].y * vec.x + mat[ 1 ].y * vec.y + mat[ 2 ].y * vec.z,
 113                            mat[ 0 ].z * vec.x + mat[ 1 ].z * vec.y + mat[ 2 ].z * vec.z );
 114 }
 115
 116 ID_INLINE mat3_t mat3_t::operator*( const mat3_t &a ) const {
 117         return mat3_t(
 118                            mat[0].x * a[0].x + mat[0].y * a[1].x + mat[0].z * a[2].x,
 119                            mat[0].x * a[0].y + mat[0].y * a[1].y + mat[0].z * a[2].y,
 120                            mat[0].x * a[0].z + mat[0].y * a[1].z + mat[0].z * a[2].z,
 121                            mat[1].x * a[0].x + mat[1].y * a[1].x + mat[1].z * a[2].x,
 122                            mat[1].x * a[0].y + mat[1].y * a[1].y + mat[1].z * a[2].y,
 123                            mat[1].x * a[0].z + mat[1].y * a[1].z + mat[1].z * a[2].z,
 124                            mat[2].x * a[0].x + mat[2].y * a[1].x + mat[2].z * a[2].x,
 125                            mat[2].x * a[0].y + mat[2].y * a[1].y + mat[2].z * a[2].y,
 126                            mat[2].x * a[0].z + mat[2].y * a[1].z + mat[2].z * a[2].z );
 127 }
 128
 129 ID_INLINE mat3_t mat3_t::operator*( float a ) const {
 130         return mat3_t(
 131                            mat[0].x * a, mat[0].y * a, mat[0].z * a,
 132                            mat[1].x * a, mat[1].y * a, mat[1].z * a,
 133                            mat[2].x * a, mat[2].y * a, mat[2].z * a );
 134 }
 135
 136 ID_INLINE mat3_t mat3_t::operator+( mat3_t const &a ) const {
 137         return mat3_t(
 138                            mat[0].x + a[0].x, mat[0].y + a[0].y, mat[0].z + a[0].z,
 139                            mat[1].x + a[1].x, mat[1].y + a[1].y, mat[1].z + a[1].z,
 140                            mat[2].x + a[2].x, mat[2].y + a[2].y, mat[2].z + a[2].z );
 141 }
 142
 143 ID_INLINE mat3_t mat3_t::operator-( mat3_t const &a ) const {
 144         return mat3_t(
 145                            mat[0].x - a[0].x, mat[0].y - a[0].y, mat[0].z - a[0].z,
 146                            mat[1].x - a[1].x, mat[1].y - a[1].y, mat[1].z - a[1].z,
 147                            mat[2].x - a[2].x, mat[2].y - a[2].y, mat[2].z - a[2].z );
 148 }
 149
 150 ID_INLINE idVec3 operator*( const idVec3 &vec, const mat3_t &mat ) {
 151         return idVec3(
 152                            mat[ 0 ].x * vec.x + mat[ 1 ].x * vec.y + mat[ 2 ].x * vec.z,
 153                            mat[ 0 ].y * vec.x + mat[ 1 ].y * vec.y + mat[ 2 ].y * vec.z,
 154                            mat[ 0 ].z * vec.x + mat[ 1 ].z * vec.y + mat[ 2 ].z * vec.z );
 155 }
 156
 157 ID_INLINE mat3_t operator*( float a, mat3_t const &b ) {
 158         return mat3_t(
 159                            b[0].x * a, b[0].y * a, b[0].z * a,
 160                            b[1].x * a, b[1].y * a, b[1].z * a,
 161                            b[2].x * a, b[2].y * a, b[2].z * a );
 162 }
 163
 164 ID_INLINE mat3_t &mat3_t::operator*=( float a ) {
 165         mat[0].x *= a; mat[0].y *= a; mat[0].z *= a;
 166         mat[1].x *= a; mat[1].y *= a; mat[1].z *= a;
 167         mat[2].x *= a; mat[2].y *= a; mat[2].z *= a;
 168
 169         return *this;
 170 }
 171
 172 ID_INLINE mat3_t &mat3_t::operator+=( mat3_t const &a ) {
 173         mat[0].x += a[0].x; mat[0].y += a[0].y; mat[0].z += a[0].z;
 174         mat[1].x += a[1].x; mat[1].y += a[1].y; mat[1].z += a[1].z;
 175         mat[2].x += a[2].x; mat[2].y += a[2].y; mat[2].z += a[2].z;
 176
 177         return *this;
 178 }
 179
 180 ID_INLINE mat3_t &mat3_t::operator-=( mat3_t const &a ) {
 181         mat[0].x -= a[0].x; mat[0].y -= a[0].y; mat[0].z -= a[0].z;
 182         mat[1].x -= a[1].x; mat[1].y -= a[1].y; mat[1].z -= a[1].z;
 183         mat[2].x -= a[2].x; mat[2].y -= a[2].y; mat[2].z -= a[2].z;
 184
 185         return *this;
 186 }
 187
 188 ID_INLINE void mat3_t::OrthoNormalize( void ) {
 189         mat[ 0 ].Normalize();
 190         mat[ 2 ].Cross( mat[ 0 ], mat[ 1 ] );
 191         mat[ 2 ].Normalize();
 192         mat[ 1 ].Cross( mat[ 2 ], mat[ 0 ] );
 193         mat[ 1 ].Normalize();
 194 }
 195
 196 ID_INLINE void mat3_t::Identity( void ) {
 197         mat[ 0 ].x = 1.f; mat[ 0 ].y = 0.f; mat[ 0 ].z = 0.f;
 198         mat[ 1 ].x = 0.f; mat[ 1 ].y = 1.f; mat[ 1 ].z = 0.f;
 199         mat[ 2 ].x = 0.f; mat[ 2 ].y = 0.f; mat[ 2 ].z = 1.f;
 200 }
 201
 202 ID_INLINE void InverseMultiply( const mat3_t &inv, const mat3_t &b, mat3_t &dst ) {
 203         dst[0].x = inv[0].x * b[0].x + inv[1].x * b[1].x + inv[2].x * b[2].x;
 204         dst[0].y = inv[0].x * b[0].y + inv[1].x * b[1].y + inv[2].x * b[2].y;
 205         dst[0].z = inv[0].x * b[0].z + inv[1].x * b[1].z + inv[2].x * b[2].z;
 206         dst[1].x = inv[0].y * b[0].x + inv[1].y * b[1].x + inv[2].y * b[2].x;
 207         dst[1].y = inv[0].y * b[0].y + inv[1].y * b[1].y + inv[2].y * b[2].y;
 208         dst[1].z = inv[0].y * b[0].z + inv[1].y * b[1].z + inv[2].y * b[2].z;
 209         dst[2].x = inv[0].z * b[0].x + inv[1].z * b[1].x + inv[2].z * b[2].x;
 210         dst[2].y = inv[0].z * b[0].y + inv[1].z * b[1].y + inv[2].z * b[2].y;
 211         dst[2].z = inv[0].z * b[0].z + inv[1].z * b[1].z + inv[2].z * b[2].z;
 212 }
 213
 214 ID_INLINE mat3_t SkewSymmetric( idVec3 const &src ) {
 215         return mat3_t( 0.0f, -src.z,  src.y, src.z,   0.0f, -src.x, -src.y,  src.x,   0.0f );
 216 }
 217
 218 extern mat3_t mat3_default;
 219
 220 #endif /* !__MATH_MATRIX_H__ */