|
Sirikata
|
00001 /* Sirikata Utilities -- Math Library 00002 * Matrix4x4.hpp 00003 * 00004 * Copyright (c) 2009, Daniel Reiter Horn 00005 * All rights reserved. 00006 * 00007 * Redistribution and use in source and binary forms, with or without 00008 * modification, are permitted provided that the following conditions are 00009 * met: 00010 * * Redistributions of source code must retain the above copyright 00011 * notice, this list of conditions and the following disclaimer. 00012 * * Redistributions in binary form must reproduce the above copyright 00013 * notice, this list of conditions and the following disclaimer in 00014 * the documentation and/or other materials provided with the 00015 * distribution. 00016 * * Neither the name of Sirikata nor the names of its contributors may 00017 * be used to endorse or promote products derived from this software 00018 * without specific prior written permission. 00019 * 00020 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS 00021 * IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED 00022 * TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A 00023 * PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER 00024 * OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, 00025 * EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, 00026 * PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR 00027 * PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF 00028 * LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING 00029 * NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS 00030 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 00031 */ 00032 #ifndef _SIRIKATA_MATRIX4X4_HPP_ 00033 #define _SIRIKATA_MATRIX4X4_HPP_ 00034 00035 #include "Vector3.hpp" 00036 #include "Vector4.hpp" 00037 #include "Quaternion.hpp" 00038 00039 namespace Sirikata { 00040 00041 template <typename scalar> class Matrix4x4 { 00042 public: 00043 typedef Vector3<scalar> Vector3x; 00044 typedef Vector4<scalar> Vector4x; 00045 00046 class COLUMNS{}; 00047 class ROWS{}; 00048 00049 class COLUMN_MAJOR{}; 00050 class ROW_MAJOR{}; 00051 00052 enum Dimension { 00053 X = 0, 00054 Y = 1, 00055 Z = 2, 00056 W = 3 00057 }; 00058 private: 00059 Vector4x mCol[4]; 00060 public: 00061 typedef scalar real; 00062 Matrix4x4(){ 00063 setCol(0,Vector4x::zero()); 00064 setCol(1,Vector4x::zero()); 00065 setCol(2,Vector4x::zero()); 00066 setCol(3,Vector4x::zero()); 00067 } 00068 static const Matrix4x4& zero() { 00069 static Matrix4x4 zero_( 00070 Vector4x::zero(), 00071 Vector4x::zero(), 00072 Vector4x::zero(), 00073 Vector4x::zero(), 00074 COLUMNS() 00075 ); 00076 return zero_; 00077 } 00078 static const Matrix4x4& identity() { 00079 static Matrix4x4 identity (Vector4x::unitX(), 00080 Vector4x::unitY(), 00081 Vector4x::unitZ(), 00082 Vector4x(0, 0, 0, 1), 00083 COLUMNS()); 00084 return identity; 00085 } 00086 // Generate a matrix for swapping two dimensions. 00087 static Matrix4x4 swapDimensions(Dimension old_up, Dimension new_up) { 00088 assert(old_up != W && new_up != W); 00089 if (old_up == new_up) return identity(); 00090 Vector4x dims[3] = { Vector4x::unitX(), Vector4x::unitY(), Vector4x::unitZ() }; 00091 std::swap(dims[old_up], dims[new_up]); 00092 return Matrix4x4( 00093 dims[0], dims[1], dims[2], 00094 Vector4x(0, 0, 0, 1), 00095 COLUMNS() 00096 ); 00097 } 00098 static Matrix4x4 reflection(Dimension across) { 00099 return Matrix4x4( 00100 (across == X ? -1.f : 1.f) * Vector4x::unitX(), 00101 (across == Y ? -1.f : 1.f) * Vector4x::unitY(), 00102 (across == Z ? -1.f : 1.f) * Vector4x::unitZ(), 00103 Vector4x(0, 0, 0, 1), 00104 COLUMNS()); 00105 } 00106 static Matrix4x4 scale(Dimension dim, scalar s) { 00107 return Matrix4x4( 00108 (dim == X ? s : 1.f) * Vector4x::unitX(), 00109 (dim == Y ? s : 1.f) * Vector4x::unitY(), 00110 (dim == Z ? s : 1.f) * Vector4x::unitZ(), 00111 Vector4x(0, 0, 0, 1), 00112 COLUMNS() 00113 ); 00114 } 00115 static Matrix4x4 scale(scalar s) { 00116 return Matrix4x4( 00117 Vector4x::unitX() * s, 00118 Vector4x::unitY() * s, 00119 Vector4x::unitZ() * s, 00120 Vector4x(0, 0, 0, 1), 00121 COLUMNS() 00122 ); 00123 } 00124 static Matrix4x4 translate(const Vector3x& t) { 00125 return Matrix4x4( 00126 Vector4x(Vector3x::unitX(), 0), 00127 Vector4x(Vector3x::unitY(), 0), 00128 Vector4x(Vector3x::unitZ(), 0), 00129 Vector4x(t.x, t.y, t.z, 1), 00130 COLUMNS() 00131 ); 00132 } 00133 static Matrix4x4 rotate(const Quaternion& r) { 00134 return Matrix4x4( 00135 Vector4x(r.xAxis(), 0), 00136 Vector4x(r.yAxis(), 0), 00137 Vector4x(r.zAxis(), 0), 00138 Vector4x(0, 0, 0, 1), 00139 COLUMNS() 00140 ); 00141 } 00142 Matrix4x4(const Vector4x&col1, const Vector4x&col2, const Vector4x&col3, const Vector4x&col4, COLUMNS c){ 00143 setCol(0,col1); 00144 setCol(1,col2); 00145 setCol(2,col3); 00146 setCol(3,col4); 00147 } 00148 Matrix4x4(const Vector4x&row1, const Vector4x&row2, const Vector4x&row3, const Vector4x&row4, ROWS r){ 00149 setRow(0,row1); 00150 setRow(1,row2); 00151 setRow(2,row3); 00152 setRow(3,row4); 00153 } 00154 00155 template<typename T> 00156 Matrix4x4(const T&other, COLUMN_MAJOR c) { 00157 for(int i = 0; i < 4; i++) 00158 setCol(i, Vector4x(other[i][0], other[i][1], other[i][2], other[i][3])); 00159 } 00160 template<typename T> 00161 Matrix4x4(const T&other, ROW_MAJOR r) { 00162 for(int i = 0; i < 4; i++) 00163 setRow(i, Vector4x(other[i][0], other[i][1], other[i][2], other[i][3])); 00164 } 00165 00166 template<typename T> 00167 Matrix4x4(T* other, COLUMN_MAJOR c) { 00168 for(int i = 0; i < 4; i++) 00169 setCol(i, Vector4x(other[i*4+0], other[i*4+1], other[i*4+2], other[i*4+3])); 00170 } template<typename T> 00171 Matrix4x4(T* other, ROW_MAJOR c) { 00172 for(int i = 0; i < 4; i++) 00173 setRow(i, Vector4x(other[i*4+0], other[i*4+1], other[i*4+2], other[i*4+3])); 00174 } 00175 00176 const Vector4x& getCol(unsigned int which) const { 00177 assert(which<4); 00178 return mCol[which]; 00179 } 00180 void setCol(unsigned int which,const Vector4x &col) { 00181 assert(which<4); 00182 mCol[which]=col; 00183 } 00184 Vector4x getRow(unsigned int which) const { 00185 assert(which<4); 00186 return Vector4x(mCol[0][which], 00187 mCol[1][which], 00188 mCol[2][which], 00189 mCol[3][which]); 00190 } 00191 void setRow(unsigned int which, const Vector4x &row) { 00192 assert(which<4); 00193 mCol[0][which]=row[0]; 00194 mCol[1][which]=row[1]; 00195 mCol[2][which]=row[2]; 00196 mCol[3][which]=row[3]; 00197 } 00198 scalar&operator() (unsigned int row, unsigned int column) { 00199 return mCol[column][row]; 00200 } 00201 scalar operator() (unsigned int row, unsigned int column) const{ 00202 return mCol[column][row]; 00203 } 00204 template <typename T> Vector4<T> operator *(const Vector4<T>&other)const { 00205 return mCol[0]*other.x+mCol[1]*other.y+mCol[2]*other.z+mCol[3]*other.w; 00206 } 00207 template <typename T> Vector3<T> operator *(const Vector3<T>&other)const { 00208 Vector4<T> tmp = mCol[0]*other.x+mCol[1]*other.y+mCol[2]*other.z+mCol[3]*1.f; 00209 return Vector3<T>(tmp.x/tmp.w, tmp.y/tmp.w, tmp.z/tmp.w); 00210 } 00211 Matrix4x4 operator *(scalar other)const { 00212 return Matrix4x4(getCol(0)*other,getCol(1)*other,getCol(2)*other,getCol(3)*other,COLUMNS()); 00213 } 00214 Matrix4x4 operator /(scalar other)const { 00215 return Matrix4x4(getCol(0)/other,getCol(1)/other,getCol(2)/other,getCol(3)/other,COLUMNS()); 00216 } 00217 bool operator == (const Matrix4x4&other)const{ 00218 return getCol(0)==other.getCol(0)&&getCol(1)==other.getCol(1)&&getCol(2)==other.getCol(2)&&getCol(3)==other.getCol(3); 00219 } 00220 bool operator != (const Matrix4x4&other)const{ 00221 return getCol(0)!=other.getCol(0) || getCol(1)!=other.getCol(1) || getCol(2)!=other.getCol(2) || getCol(3)!=other.getCol(3); 00222 } 00223 Matrix4x4 operator+ (const Matrix4x4&other)const { 00224 return Matrix4x4(getCol(0)+other.getCol(0), 00225 getCol(1)+other.getCol(1), 00226 getCol(2)+other.getCol(2), 00227 getCol(3)+other.getCol(3), 00228 COLUMNS()); 00229 } 00230 Matrix4x4 operator- (const Matrix4x4&other)const { 00231 return Matrix4x4(getCol(0)-other.getCol(0), 00232 getCol(1)-other.getCol(1), 00233 getCol(2)-other.getCol(2), 00234 getCol(3)-other.getCol(3), 00235 COLUMNS()); 00236 } 00237 Matrix4x4 operator- ()const { 00238 return Matrix4x4(-getCol(0), 00239 -getCol(1), 00240 -getCol(2), 00241 -getCol(3), 00242 COLUMNS()); 00243 } 00244 Matrix4x4& operator+= (const Matrix4x4&other) { 00245 mCol[0]+=other.getCol(0); 00246 mCol[1]+=other.getCol(1); 00247 mCol[2]+=other.getCol(2); 00248 mCol[3]+=other.getCol(3); 00249 return *this; 00250 } 00251 Matrix4x4& operator-= (const Matrix4x4&other) { 00252 mCol[0]-=other.getCol(0); 00253 mCol[1]-=other.getCol(1); 00254 mCol[2]-=other.getCol(2); 00255 mCol[3]-=other.getCol(3); 00256 return *this; 00257 } 00258 Matrix4x4 operator* (const Matrix4x4&other)const { 00259 Vector4<scalar> ocol0=other.getCol(0); 00260 Vector4<scalar> ocol1=other.getCol(1); 00261 Vector4<scalar> ocol2=other.getCol(2); 00262 Vector4<scalar> ocol3=other.getCol(3); 00263 return Matrix4x4(mCol[0]*ocol0.x+mCol[1]*ocol0.y+mCol[2]*ocol0.z+mCol[3]*ocol0.w, 00264 mCol[0]*ocol1.x+mCol[1]*ocol1.y+mCol[2]*ocol1.z+mCol[3]*ocol1.w, 00265 mCol[0]*ocol2.x+mCol[1]*ocol2.y+mCol[2]*ocol2.z+mCol[3]*ocol2.w, 00266 mCol[0]*ocol3.x+mCol[1]*ocol3.y+mCol[2]*ocol3.z+mCol[3]*ocol3.w, 00267 COLUMNS()); 00268 } 00269 Matrix4x4& operator*= (const Matrix4x4&other) { 00270 Vector4<scalar> ocol0=other.getCol(0); 00271 Vector4<scalar> ocol1=other.getCol(1); 00272 Vector4<scalar> ocol2=other.getCol(2); 00273 Vector4<scalar> ocol3=other.getCol(3); 00274 //cannot do this *= inplace 00275 ocol0=mCol[0]*ocol0.x+mCol[1]*ocol0.y+mCol[2]*ocol0.z+mCol[3]*ocol0.w; 00276 ocol1=mCol[0]*ocol1.x+mCol[1]*ocol1.y+mCol[2]*ocol1.z+mCol[3]*ocol1.w; 00277 ocol2=mCol[0]*ocol2.x+mCol[1]*ocol2.y+mCol[2]*ocol2.z+mCol[3]*ocol2.w; 00278 ocol3=mCol[0]*ocol3.x+mCol[1]*ocol3.y+mCol[2]*ocol3.z+mCol[3]*ocol3.w; 00279 //have to copy back 00280 mCol[0]=ocol0; 00281 mCol[1]=ocol1; 00282 mCol[2]=ocol2; 00283 mCol[3]=ocol3; 00284 return *this; 00285 } 00286 Matrix4x4& operator*= (scalar other) { 00287 mCol[0]*=other; 00288 mCol[1]*=other; 00289 mCol[2]*=other; 00290 mCol[3]*=other; 00291 return *this; 00292 } 00293 Matrix4x4& operator/= (scalar other) { 00294 mCol[0]/=other; 00295 mCol[1]/=other; 00296 mCol[2]/=other; 00297 mCol[3]/=other; 00298 return *this; 00299 } 00300 Matrix4x4 transpose() const { 00301 return Matrix4x4(getCol(0), 00302 getCol(1), 00303 getCol(2), 00304 getCol(3), 00305 ROWS()); 00306 } 00307 00308 scalar invert(Matrix4x4& inv) const { 00309 /* Code adapted from an Intel matrix inversion optimization report 00310 (ftp://download.intel.com/design/pentiumiii/sml/24504301.pdf) */ 00311 scalar mat[16]; 00312 scalar dst[16]; 00313 00314 int counter = 0; 00315 for (int i=0; i<4; i++) { 00316 for (int j=0; j<4; j++) { 00317 mat[counter] = mCol[j][i]; 00318 counter++; 00319 } 00320 } 00321 00322 scalar tmp[12]; /* temp array for pairs */ 00323 scalar src[16]; /* array of transpose source matrix */ 00324 scalar det; /* determinant */ 00325 /* transpose matrix */ 00326 for (int i = 0; i < 4; i++) { 00327 src[i] = mat[i*4]; 00328 src[i + 4] = mat[i*4 + 1]; 00329 src[i + 8] = mat[i*4 + 2]; 00330 src[i + 12] = mat[i*4 + 3]; 00331 } 00332 /* calculate pairs for first 8 elements (cofactors) */ 00333 tmp[0] = src[10] * src[15]; 00334 tmp[1] = src[11] * src[14]; 00335 tmp[2] = src[9] * src[15]; 00336 tmp[3] = src[11] * src[13]; 00337 tmp[4] = src[9] * src[14]; 00338 tmp[5] = src[10] * src[13]; 00339 tmp[6] = src[8] * src[15]; 00340 tmp[7] = src[11] * src[12]; 00341 tmp[8] = src[8] * src[14]; 00342 tmp[9] = src[10] * src[12]; 00343 tmp[10] = src[8] * src[13]; 00344 tmp[11] = src[9] * src[12]; 00345 /* calculate first 8 elements (cofactors) */ 00346 dst[0] = tmp[0]*src[5] + tmp[3]*src[6] + tmp[4]*src[7]; 00347 dst[0] -= tmp[1]*src[5] + tmp[2]*src[6] + tmp[5]*src[7]; 00348 dst[1] = tmp[1]*src[4] + tmp[6]*src[6] + tmp[9]*src[7]; 00349 dst[1] -= tmp[0]*src[4] + tmp[7]*src[6] + tmp[8]*src[7]; 00350 dst[2] = tmp[2]*src[4] + tmp[7]*src[5] + tmp[10]*src[7]; 00351 dst[2] -= tmp[3]*src[4] + tmp[6]*src[5] + tmp[11]*src[7]; 00352 dst[3] = tmp[5]*src[4] + tmp[8]*src[5] + tmp[11]*src[6]; 00353 dst[3] -= tmp[4]*src[4] + tmp[9]*src[5] + tmp[10]*src[6]; 00354 dst[4] = tmp[1]*src[1] + tmp[2]*src[2] + tmp[5]*src[3]; 00355 dst[4] -= tmp[0]*src[1] + tmp[3]*src[2] + tmp[4]*src[3]; 00356 dst[5] = tmp[0]*src[0] + tmp[7]*src[2] + tmp[8]*src[3]; 00357 dst[5] -= tmp[1]*src[0] + tmp[6]*src[2] + tmp[9]*src[3]; 00358 dst[6] = tmp[3]*src[0] + tmp[6]*src[1] + tmp[11]*src[3]; 00359 dst[6] -= tmp[2]*src[0] + tmp[7]*src[1] + tmp[10]*src[3]; 00360 dst[7] = tmp[4]*src[0] + tmp[9]*src[1] + tmp[10]*src[2]; 00361 dst[7] -= tmp[5]*src[0] + tmp[8]*src[1] + tmp[11]*src[2]; 00362 /* calculate pairs for second 8 elements (cofactors) */ 00363 tmp[0] = src[2]*src[7]; 00364 tmp[1] = src[3]*src[6]; 00365 tmp[2] = src[1]*src[7]; 00366 tmp[3] = src[3]*src[5]; 00367 tmp[4] = src[1]*src[6]; 00368 tmp[5] = src[2]*src[5]; 00369 tmp[6] = src[0]*src[7]; 00370 tmp[7] = src[3]*src[4]; 00371 tmp[8] = src[0]*src[6]; 00372 tmp[9] = src[2]*src[4]; 00373 tmp[10] = src[0]*src[5]; 00374 tmp[11] = src[1]*src[4]; 00375 /* calculate second 8 elements (cofactors) */ 00376 dst[8] = tmp[0]*src[13] + tmp[3]*src[14] + tmp[4]*src[15]; 00377 dst[8] -= tmp[1]*src[13] + tmp[2]*src[14] + tmp[5]*src[15]; 00378 dst[9] = tmp[1]*src[12] + tmp[6]*src[14] + tmp[9]*src[15]; 00379 dst[9] -= tmp[0]*src[12] + tmp[7]*src[14] + tmp[8]*src[15]; 00380 dst[10] = tmp[2]*src[12] + tmp[7]*src[13] + tmp[10]*src[15]; 00381 dst[10]-= tmp[3]*src[12] + tmp[6]*src[13] + tmp[11]*src[15]; 00382 dst[11] = tmp[5]*src[12] + tmp[8]*src[13] + tmp[11]*src[14]; 00383 dst[11]-= tmp[4]*src[12] + tmp[9]*src[13] + tmp[10]*src[14]; 00384 dst[12] = tmp[2]*src[10] + tmp[5]*src[11] + tmp[1]*src[9]; 00385 dst[12]-= tmp[4]*src[11] + tmp[0]*src[9] + tmp[3]*src[10]; 00386 dst[13] = tmp[8]*src[11] + tmp[0]*src[8] + tmp[7]*src[10]; 00387 dst[13]-= tmp[6]*src[10] + tmp[9]*src[11] + tmp[1]*src[8]; 00388 dst[14] = tmp[6]*src[9] + tmp[11]*src[11] + tmp[3]*src[8]; 00389 dst[14]-= tmp[10]*src[11] + tmp[2]*src[8] + tmp[7]*src[9]; 00390 dst[15] = tmp[10]*src[10] + tmp[4]*src[8] + tmp[9]*src[9]; 00391 dst[15]-= tmp[8]*src[9] + tmp[11]*src[10] + tmp[5]*src[8]; 00392 /* calculate determinant */ 00393 det=src[0]*dst[0]+src[1]*dst[1]+src[2]*dst[2]+src[3]*dst[3]; 00394 00395 if (det == 0.0) return 0.0; 00396 00397 /* calculate matrix inverse */ 00398 det = 1/det; 00399 for (int j = 0; j < 16; j++) 00400 dst[j] *= det; 00401 00402 counter = 0; 00403 for (int i=0; i<4; i++) { 00404 for (int j=0; j<4; j++) { 00405 inv(i,j) = dst[counter]; 00406 counter++; 00407 } 00408 } 00409 00410 return det; 00411 } 00412 00413 00414 Matrix3x3<scalar> extract3x3() const { 00415 Vector4x c1 = getCol(0); 00416 Vector4x c2 = getCol(1); 00417 Vector4x c3 = getCol(2); 00418 return Matrix3x3<scalar>( 00419 Vector3<scalar>(c1[0], c1[1], c1[2]), 00420 Vector3<scalar>(c2[0], c2[1], c2[2]), 00421 Vector3<scalar>(c3[0], c3[1], c3[2]), 00422 Sirikata::COLUMNS() 00423 ); 00424 } 00425 std::string toString() const { 00426 std::ostringstream os; 00427 os<<"{ col1:"<<mCol[0]<<" col2:"<<mCol[1]<<" col3:"<<mCol[2]<<" col4:"<<mCol[3]<<'}'; 00428 return os.str(); 00429 } 00430 }; 00431 template<typename scalar> inline std::ostream& operator <<(std::ostream& os, const Matrix4x4<scalar> &rhs) { 00432 os<<"{ col1:"<<rhs.getCol(0)<<" col2:"<<rhs.getCol(1)<<" col3:"<<rhs.getCol(2)<<" col4:"<<rhs.getCol(3)<<'}'; 00433 return os; 00434 } 00435 00436 template <typename T,typename S> Vector4<T> operator *(const Vector4<T>&vec, const Matrix4x4<S>&mat) { 00437 return Vector4<T>( 00438 mat.getCol(0).dot(vec), 00439 mat.getCol(1).dot(vec), 00440 mat.getCol(2).dot(vec), 00441 mat.getCol(3).dot(vec) 00442 ); 00443 } 00444 template <typename T> Matrix4x4<T> operator *(T other, const Matrix4x4<T>&mat) { 00445 return Matrix4x4<T>(mat.getCol(0)*other,mat.getCol(1)*other,mat.getCol(2)*other,mat.getCol(3)*other,COLUMNS()); 00446 } 00447 template <typename T> Matrix4x4<T> operator /(T other, const Matrix4x4<T>&mat) { 00448 return Matrix4x4<T>(other/mat.getCol(0),other/mat.getCol(1),other/mat.getCol(2),other/mat.getCol(3),COLUMNS()); 00449 } 00450 00451 typedef Matrix4x4<float32> Matrix4x4f; 00452 typedef Matrix4x4<float64> Matrix4x4d; 00453 00454 } 00455 #endif // _SIRIKATA_MATRIX4x4_HPP_
1.7.4