00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053 #ifndef NV_MATRIX_H
00054 #define NV_MATRIX_H
00055
00056 #include <NvFoundation.h>
00057
00060
00061 namespace nv {
00062
00063 template <class T> class vec2;
00064 template <class T> class vec3;
00065 template <class T> class vec4;
00066
00068
00069
00070
00072 template<class T>
00073 class matrix4
00074 {
00075
00076 public:
00077
00078 matrix4() { make_identity(); }
00079
00080 matrix4( T t )
00081 { set_value(t); }
00082
00083 matrix4( const T * m )
00084 { set_value(m); }
00085
00086 matrix4( T a00, T a01, T a02, T a03,
00087 T a10, T a11, T a12, T a13,
00088 T a20, T a21, T a22, T a23,
00089 T a30, T a31, T a32, T a33 ) :
00090 _11(a00), _12(a01), _13(a02), _14(a03),
00091 _21(a10), _22(a11), _23(a12), _24(a13),
00092 _31(a20), _32(a21), _33(a22), _34(a23),
00093 _41(a30), _42(a31), _43(a32), _44(a33)
00094 {}
00095
00096
00097 void get_value( T * mp ) const {
00098 int32_t c = 0;
00099 for(int32_t j=0; j < 4; j++)
00100 for(int32_t i=0; i < 4; i++)
00101 mp[c++] = element(i,j);
00102 }
00103
00104 const T * get_value() const {
00105 return _array;
00106 }
00107
00108 void set_value( T * mp) {
00109 int32_t c = 0;
00110 for(int32_t j=0; j < 4; j++)
00111 for(int32_t i=0; i < 4; i++)
00112 element(i,j) = mp[c++];
00113 }
00114
00115 void set_value( T r ) {
00116 for(int32_t i=0; i < 4; i++)
00117 for(int32_t j=0; j < 4; j++)
00118 element(i,j) = r;
00119 }
00120
00121 void make_identity() {
00122 element(0,0) = 1.0;
00123 element(0,1) = 0.0;
00124 element(0,2) = 0.0;
00125 element(0,3) = 0.0;
00126
00127 element(1,0) = 0.0;
00128 element(1,1) = 1.0;
00129 element(1,2) = 0.0;
00130 element(1,3) = 0.0;
00131
00132 element(2,0) = 0.0;
00133 element(2,1) = 0.0;
00134 element(2,2) = 1.0;
00135 element(2,3) = 0.0;
00136
00137 element(3,0) = 0.0;
00138 element(3,1) = 0.0;
00139 element(3,2) = 0.0;
00140 element(3,3) = 1.0;
00141 }
00142
00143
00144 void set_scale( T s ) {
00145 element(0,0) = s;
00146 element(1,1) = s;
00147 element(2,2) = s;
00148 }
00149
00150 void set_scale( const vec3<T> & s ) {
00151 for (int32_t i = 0; i < 3; i++) element(i,i) = s[i];
00152 }
00153
00154
00155 void set_translate( const vec3<T> & t ) {
00156 for (int32_t i = 0; i < 3; i++) element(i,3) = t[i];
00157 }
00158
00159 void set_row(int32_t r, const vec4<T> & t) {
00160 for (int32_t i = 0; i < 4; i++) element(r,i) = t[i];
00161 }
00162
00163 void set_column(int32_t c, const vec4<T> & t) {
00164 for (int32_t i = 0; i < 4; i++) element(i,c) = t[i];
00165 }
00166
00167 vec4<T> get_row(int32_t r) const {
00168 vec4<T> v;
00169 for (int32_t i = 0; i < 4; i++) v[i] = element(r,i);
00170 return v;
00171 }
00172
00173 vec4<T> get_column(int32_t c) const {
00174 vec4<T> v;
00175 for (int32_t i = 0; i < 4; i++) v[i] = element(i,c);
00176 return v;
00177 }
00178
00179 matrix4 & operator *= ( const matrix4 & rhs ) {
00180 matrix4 mt(*this);
00181 set_value(T(0));
00182
00183 for(int32_t i=0; i < 4; i++)
00184 for(int32_t j=0; j < 4; j++)
00185 for(int32_t c=0; c < 4; c++)
00186 element(i,j) += mt(i,c) * rhs(c,j);
00187 return *this;
00188 }
00189
00190 friend matrix4 operator * ( const matrix4 & lhs, const matrix4 & rhs ) {
00191 matrix4 r(T(0));
00192
00193 for(int32_t i=0; i < 4; i++)
00194 for(int32_t j=0; j < 4; j++)
00195 for(int32_t c=0; c < 4; c++)
00196 r.element(i,j) += lhs(i,c) * rhs(c,j);
00197 return r;
00198 }
00199
00200
00201 vec4<T> operator *( const vec4<T> &src) const {
00202 vec4<T> r;
00203 for ( int32_t i = 0; i < 4; i++)
00204 r[i] = ( src[0] * element(i,0) + src[1] * element(i,1) +
00205 src[2] * element(i,2) + src[3] * element(i,3));
00206 return r;
00207 }
00208
00209
00210 friend vec4<T> operator *( const vec4<T> &lhs, const matrix4 &rhs) {
00211 vec4<T> r;
00212 for ( int32_t i = 0; i < 4; i++)
00213 r[i] = ( lhs[0] * rhs.element(0,i) + lhs[1] * rhs.element(1,i) +
00214 lhs[2] * rhs.element(2,i) + lhs[3] * rhs.element(3,i));
00215 return r;
00216 }
00217
00218 T & operator () (int32_t row, int32_t col) {
00219 return element(row,col);
00220 }
00221
00222 const T & operator () (int32_t row, int32_t col) const {
00223 return element(row,col);
00224 }
00225
00226 T & element (int32_t row, int32_t col) {
00227 return _array[row | (col<<2)];
00228 }
00229
00230 const T & element (int32_t row, int32_t col) const {
00231 return _array[row | (col<<2)];
00232 }
00233
00234 matrix4 & operator *= ( const T & r ) {
00235 for (int32_t i = 0; i < 4; ++i) {
00236 element(0,i) *= r;
00237 element(1,i) *= r;
00238 element(2,i) *= r;
00239 element(3,i) *= r;
00240 }
00241 return *this;
00242 }
00243
00244 matrix4 & operator += ( const matrix4 & mat ) {
00245 for (int32_t i = 0; i < 4; ++i) {
00246 element(0,i) += mat.element(0,i);
00247 element(1,i) += mat.element(1,i);
00248 element(2,i) += mat.element(2,i);
00249 element(3,i) += mat.element(3,i);
00250 }
00251 return *this;
00252 }
00253
00254
00255 friend bool operator == ( const matrix4 & lhs, const matrix4 & rhs ) {
00256 bool r = true;
00257 for (int32_t i = 0; i < 16; i++)
00258 r &= lhs._array[i] == rhs._array[i];
00259 return r;
00260 }
00261
00262 friend bool operator != ( const matrix4 & lhs, const matrix4 & rhs ) {
00263 bool r = true;
00264 for (int32_t i = 0; i < 16; i++)
00265 r &= lhs._array[i] != rhs._array[i];
00266 return r;
00267 }
00268
00269 union {
00270 struct {
00271 T _11, _12, _13, _14;
00272 T _21, _22, _23, _24;
00273 T _31, _32, _33, _34;
00274 T _41, _42, _43, _44;
00275 };
00276 T _array[16];
00277 };
00278 };
00279
00280
00282
00283
00284
00285
00286
00288 template<class T>
00289 matrix4<T> inverse( const matrix4<T> & m) {
00290 matrix4<T> minv;
00291
00292 T r1[8], r2[8], r3[8], r4[8];
00293 T *s[4], *tmprow;
00294
00295 s[0] = &r1[0];
00296 s[1] = &r2[0];
00297 s[2] = &r3[0];
00298 s[3] = &r4[0];
00299
00300 register int32_t i,j,p,jj;
00301 for(i=0;i<4;i++) {
00302 for(j=0;j<4;j++) {
00303 s[i][j] = m.element(i,j);
00304 if(i==j) s[i][j+4] = 1.0;
00305 else s[i][j+4] = 0.0;
00306 }
00307 }
00308 T scp[4];
00309 for(i=0;i<4;i++) {
00310 scp[i] = T(fabs(s[i][0]));
00311 for(j=1;j<4;j++)
00312 if(T(fabs(s[i][j])) > scp[i]) scp[i] = T(fabs(s[i][j]));
00313 if(scp[i] == 0.0) return minv;
00314 }
00315
00316 int32_t pivot_to;
00317 T scp_max;
00318 for(i=0;i<4;i++) {
00319
00320 pivot_to = i;
00321 scp_max = T(fabs(s[i][i]/scp[i]));
00322
00323 for(p=i+1;p<4;p++)
00324 if (T(fabs(s[p][i]/scp[p])) > scp_max) {
00325 scp_max = T(fabs(s[p][i]/scp[p]));
00326 pivot_to = p;
00327 }
00328
00329 if(pivot_to != i) {
00330 tmprow = s[i];
00331 s[i] = s[pivot_to];
00332 s[pivot_to] = tmprow;
00333 T tmpscp;
00334 tmpscp = scp[i];
00335 scp[i] = scp[pivot_to];
00336 scp[pivot_to] = tmpscp;
00337 }
00338
00339 T mji;
00340
00341 for(j=i+1;j<4;j++) {
00342 mji = s[j][i]/s[i][i];
00343 s[j][i] = 0.0;
00344 for(jj=i+1;jj<8;jj++)
00345 s[j][jj] -= mji*s[i][jj];
00346 }
00347 }
00348 if(s[3][3] == 0.0) return minv;
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358
00359
00360
00361
00362
00363
00364
00365
00366 T mij;
00367 for(i=3;i>0;i--) {
00368 for(j=i-1;j > -1; j--) {
00369 mij = s[j][i]/s[i][i];
00370 for(jj=j+1;jj<8;jj++)
00371 s[j][jj] -= mij*s[i][jj];
00372 }
00373 }
00374
00375 for(i=0;i<4;i++)
00376 for(j=0;j<4;j++)
00377 minv(i,j) = s[i][j+4] / s[i][i];
00378
00379 return minv;
00380 }
00381
00382
00383
00384
00385
00386
00388 template<class T>
00389 matrix4<T> transpose( const matrix4<T> & m) {
00390 matrix4<T> mtrans;
00391
00392 for(int32_t i=0;i<4;i++)
00393 for(int32_t j=0;j<4;j++)
00394 mtrans(i,j) = m.element(j,i);
00395 return mtrans;
00396 }
00397
00398
00399
00400
00401
00402
00404 template<class T>
00405 matrix4<T>& rotationX( matrix4<T> & M, const T angle )
00406 {
00407 T cosa = cos(angle);
00408 T sina = sin(angle);
00409
00410 M.element(0,0) = 1.0;
00411 M.element(0,1) = 0.0;
00412 M.element(0,2) = 0.0;
00413 M.element(0,3) = 0.0;
00414
00415 M.element(1,0) = 0.0;
00416 M.element(1,1) = cosa;
00417 M.element(1,2) = -sina;
00418 M.element(1,3) = 0.0;
00419
00420 M.element(2,0) = 0.0;
00421 M.element(2,1) = sina;
00422 M.element(2,2) = cosa;
00423 M.element(2,3) = 0.0;
00424
00425 M.element(3,0) = 0.0;
00426 M.element(3,1) = 0.0;
00427 M.element(3,2) = 0.0;
00428 M.element(3,3) = 1.0;
00429
00430 return M;
00431 }
00432
00433
00434
00435
00436
00437
00439 template<class T>
00440 matrix4<T>& rotationY( matrix4<T> & M, const T angle )
00441 {
00442 T cosa = cos(angle);
00443 T sina = sin(angle);
00444
00445 M.element(0,0) = cosa;
00446 M.element(0,1) = 0.0;
00447 M.element(0,2) = sina;
00448 M.element(0,3) = 0.0;
00449
00450 M.element(1,0) = 0.0;
00451 M.element(1,1) = 1.0;
00452 M.element(1,2) = 0.0;
00453 M.element(1,3) = 0.0;
00454
00455 M.element(2,0) = -sina;
00456 M.element(2,1) = 0.0;
00457 M.element(2,2) = cosa;
00458 M.element(2,3) = 0.0;
00459
00460 M.element(3,0) = 0.0;
00461 M.element(3,1) = 0.0;
00462 M.element(3,2) = 0.0;
00463 M.element(3,3) = 1.0;
00464
00465 return M;
00466 }
00467
00468
00469
00470
00471
00472
00474 template<class T>
00475 matrix4<T>& rotationZ( matrix4<T> & M, const T angle )
00476 {
00477 T cosa = cos(angle);
00478 T sina = sin(angle);
00479
00480 M.element(0,0) = cosa;
00481 M.element(0,1) = -sina;
00482 M.element(0,2) = 0.0;
00483 M.element(0,3) = 0.0;
00484
00485 M.element(1,0) = sina;
00486 M.element(1,1) = cosa;
00487 M.element(1,2) = 0.0;
00488 M.element(1,3) = 0.0;
00489
00490 M.element(2,0) = 0.0;
00491 M.element(2,1) = 0.0;
00492 M.element(2,2) = 1.0;
00493 M.element(2,3) = 0.0;
00494
00495 M.element(3,0) = 0.0;
00496 M.element(3,1) = 0.0;
00497 M.element(3,2) = 0.0;
00498 M.element(3,3) = 1.0;
00499
00500 return M;
00501 }
00502
00503
00504
00505
00506
00507
00508
00509
00510
00512 template<class T>
00513 matrix4<T>& rotationYawPitchRoll( matrix4<T> & M, const T yaw , const T pitch , const T roll )
00514 {
00515 M.make_identity();
00516 matrix4<T> rot;
00517
00518 if (roll)
00519 {
00520 nv::rotationZ(M, roll );
00521 }
00522 if (pitch)
00523 {
00524 M *= nv::rotationX(rot, pitch );
00525 }
00526 if (yaw)
00527 {
00528 M *= nv::rotationY(rot, yaw );
00529 }
00530
00531 return M;
00532 }
00533
00534
00535
00536
00537
00538
00540 template<class T>
00541 matrix4<T>& translation( matrix4<T> & M, const T tx , const T ty , const T tz )
00542 {
00543 M.element(0,0) = 1.0;
00544 M.element(1,0) = 0.0;
00545 M.element(2,0) = 0.0;
00546 M.element(3,0) = 0.0;
00547
00548 M.element(0,1) = 0.0;
00549 M.element(1,1) = 1.0;
00550 M.element(2,1) = 0.0;
00551 M.element(3,1) = 0.0;
00552
00553 M.element(0,2) = 0.0;
00554 M.element(1,2) = 0.0;
00555 M.element(2,2) = 1.0;
00556 M.element(3,2) = 0.0;
00557
00558 M.element(0,3) = tx;
00559 M.element(1,3) = ty;
00560 M.element(2,3) = tz;
00561 M.element(3,3) = 1.0;
00562
00563 return M;
00564 }
00565
00566
00567
00568
00569
00571 template<class T>
00572 matrix4<T> & lookAt( matrix4<T>& M, const vec3<T>& eye, const vec3<T>& center, const vec3<T>& up)
00573 {
00574 vec3<T> x, y, z;
00575
00576
00577
00578
00579 z.x = eye.x - center.x;
00580 z.y = eye.y - center.y;
00581 z.z = eye.z - center.z;
00582 z = normalize(z);
00583
00584
00585 y.x = up.x;
00586 y.y = up.y;
00587 y.z = up.z;
00588
00589
00590 x = cross(y,z);
00591
00592
00593 y = cross(z,x);
00594
00595
00596
00597 x = normalize(x);
00598 y = normalize(y);
00599
00600 M._11 = x.x; M._21 = x.y; M._31 = x.z; M._41 = -x.x * eye.x - x.y * eye.y - x.z*eye.z;
00601 M._12 = y.x; M._22 = y.y; M._32 = y.z; M._42 = -y.x * eye.x - y.y * eye.y - y.z*eye.z;
00602 M._13 = z.x; M._23 = z.y; M._33 = z.z; M._43 = -z.x * eye.x - z.y * eye.y - z.z*eye.z;
00603 M._14 = 0.0; M._24 = 0.0; M._34 = 0.0; M._44 = 1.0;
00604 return M;
00605 }
00606
00607
00608
00609
00610
00611
00613 template<class T>
00614 matrix4<T>& frustum( matrix4<T> & M, const T l, const T r, const T b, const T t, const T n, const T f)
00615 {
00616 M.element(0,0) = ((T)(2.0))*n / (r-l);
00617 M.element(1,0) = 0.0;
00618 M.element(2,0) = 0.0;
00619 M.element(3,0) = 0.0;
00620
00621 M.element(0,1) = 0.0;
00622 M.element(1,1) = ((T)(2.0))*n / (t-b);
00623 M.element(2,1) = 0.0;
00624 M.element(3,1) = 0.0;
00625
00626 M.element(0,2) = (r+l) / (r-l);
00627 M.element(1,2) = (t+b) / (t-b);
00628 M.element(2,2) = -(f+n) / (f-n);
00629 M.element(3,2) = -1.0;
00630
00631 M.element(0,3) = 0.0;
00632 M.element(1,3) = 0.0;
00633 M.element(2,3) = -(((T)(2.0))*f*n) / (f-n);
00634 M.element(3,3) = 0.0;
00635
00636 return M;
00637 }
00638
00639
00640
00641
00642
00643
00645 template<class T>
00646 matrix4<T>& perspective( matrix4<T> & M, const T fovy, const T aspect, const T n, const T f)
00647 {
00648 T xmin, xmax, ymin, ymax;
00649
00650 ymax = n * (T)tan(fovy * 0.5);
00651 ymin = -ymax;
00652
00653 xmin = ymin * aspect;
00654 xmax = ymax * aspect;
00655
00656 return frustum(M, xmin, xmax, ymin, ymax, n, f);
00657 }
00658
00659 template<class T>
00660 matrix4<T>& perspectivex( matrix4<T> & M, const T fovx, const T aspect, const T near, const T far)
00661 {
00662 float e = 1.0f / tanf(fovx / 2.0f);
00663 float aspectInv = 1.0f / aspect;
00664 float fovy = 2.0f * atanf(aspectInv / e);
00665 float xScale = 1.0f / tanf(0.5f * fovy);
00666 float yScale = xScale / aspectInv;
00667
00668 M._array[0] = xScale;
00669 M._array[1] = 0.0f;
00670 M._array[2] = 0.0f;
00671 M._array[3] = 0.0f;
00672
00673 M._array[4] = 0.0f;
00674 M._array[5] = yScale;
00675 M._array[6] = 0.0f;
00676 M._array[7] = 0.0f;
00677
00678 M._array[8] = 0.0f;
00679 M._array[9] = 0.0f;
00680 M._array[10] = (far + near) / (near - far);
00681 M._array[11] = -1.0f;
00682
00683 M._array[12] = 0.0f;
00684 M._array[13] = 0.0f;
00685 M._array[14] = (2.0f * far * near) / (near - far);
00686 M._array[15] = 0.0f;
00687
00688 return M;
00689 }
00690
00691
00692 template<class T>
00693 matrix4<T>& ortho2D(matrix4<T> & M, T left, T right, T bottom, T top)
00694 {
00695 float zNear=-1.0; float zFar=1.0;
00696
00697 float sx = 2.0f / (right - left);
00698 float sy = 2.0f / (top - bottom);
00699 float sz = 2.0f / (zFar - zNear);
00700
00701 float tx = -(right + left) / (right - left);
00702 float ty = -(top + bottom) / (top - bottom);
00703 float tz = -(zFar + zNear) / (zFar - zNear);
00704
00705
00706 M._array[0] = sx, M._array[4] = 0.0f, M._array[ 8] = 0.0f, M._array[12] = tx;
00707 M._array[1] = 0.0f, M._array[5] = sy, M._array[ 9] = 0.0f, M._array[13] = ty;
00708 M._array[2] = 0.0f, M._array[6] = 0.0f, M._array[10] = sz, M._array[14] = tz;
00709 M._array[3] = 0.0f, M._array[7] = 0.0f, M._array[11] = 0.0f, M._array[15] = 1.0f;
00710
00711 return M;
00712 }
00713
00714 template<class T>
00715 matrix4<T>& ortho3D(matrix4<T> & M, T left, T right, T bottom, T top, T zNear, T zFar)
00716 {
00717 float sx = 2.0f / (right - left);
00718 float sy = 2.0f / (top - bottom);
00719 float sz = 2.0f / (zFar - zNear);
00720
00721 float tx = -(right + left) / (right - left);
00722 float ty = -(top + bottom) / (top - bottom);
00723 float tz = -(zFar + zNear) / (zFar - zNear);
00724
00725
00726 M._array[0] = sx, M._array[4] = 0.0f, M._array[ 8] = 0.0f, M._array[12] = tx;
00727 M._array[1] = 0.0f, M._array[5] = sy, M._array[ 9] = 0.0f, M._array[13] = ty;
00728 M._array[2] = 0.0f, M._array[6] = 0.0f, M._array[10] = sz, M._array[14] = tz;
00729 M._array[3] = 0.0f, M._array[7] = 0.0f, M._array[11] = 0.0f, M._array[15] = 1.0f;
00730
00731 return M;
00732 }
00733
00734 };
00735
00736 #endif