00001 #ifndef AI__MATRIX33_H
00002 #define AI__MATRIX33_H
00003
00012 #include "AI_TVector3.h"
00013 #include "AI_Quaternion.h"
00014 #include "euler.h"
00015 #include <memory.h>
00016
00017 static float ai_matrix33_ident[9] =
00018 {
00019 1.0f, 0.0f, 0.0f,
00020 0.0f, 1.0f, 0.0f,
00021 0.0f, 0.0f, 1.0f,
00022 };
00023
00024 class AI_TMatrix33
00025 {
00026 public:
00028 AI_TMatrix33();
00030 AI_TMatrix33(const AI_TVector3& v0, const AI_TVector3& v1, const AI_TVector3& v2);
00032 AI_TMatrix33(const AI_TMatrix33& mx);
00034 AI_TMatrix33(float _m11, float _m12, float _m13, float _m21, float _m22, float _m23, float _m31, float _m32, float _m33);
00036 AI_TMatrix33(const AI_Quaternion& q);
00038 AI_Quaternion get_quaternion() const;
00040 AI_TVector3 to_euler() const;
00042 void from_euler(const AI_TVector3& ea);
00044 void lookat(const AI_TVector3& from, const AI_TVector3& to, const AI_TVector3& up);
00046 void billboard(const AI_TVector3& from, const AI_TVector3& to, const AI_TVector3& up);
00048 void set(float m11, float m12, float m13, float m21, float m22, float m23, float m31, float m32, float m33);
00050 void set(const AI_TVector3& v0, const AI_TVector3& v1, const AI_TVector3& v2);
00052 void set(const AI_TMatrix33& m1);
00054 void ident();
00056 void transpose();
00058 bool orthonorm(float limit);
00060 void scale(const AI_TVector3& s);
00062 void rotate_x(const float a);
00064 void rotate_y(const float a);
00066 void rotate_z(const float a);
00068 void rotate_local_x(const float a);
00070 void rotate_local_y(const float a);
00072 void rotate_local_z(const float a);
00074 void rotate(const AI_TVector3& vec, float a);
00076 AI_TVector3 x_component(void) const;
00078 AI_TVector3 y_component(void) const;
00080 AI_TVector3 z_component(void) const;
00081
00082 void operator *= (const AI_TMatrix33& m1);
00084 void mult(const AI_TVector3& src, AI_TVector3& dst) const;
00086 void translate(const AI_TVector2& t);
00087
00088 float m[3][3];
00089 };
00090
00091
00094 static
00095 inline
00096 AI_TMatrix33
00097 operator * (const AI_TMatrix33& m0, const AI_TMatrix33& m1)
00098 {
00099 AI_TMatrix33 m2(
00100 m0.m[0][0]*m1.m[0][0] + m0.m[0][1]*m1.m[1][0] + m0.m[0][2]*m1.m[2][0],
00101 m0.m[0][0]*m1.m[0][1] + m0.m[0][1]*m1.m[1][1] + m0.m[0][2]*m1.m[2][1],
00102 m0.m[0][0]*m1.m[0][2] + m0.m[0][1]*m1.m[1][2] + m0.m[0][2]*m1.m[2][2],
00103
00104 m0.m[1][0]*m1.m[0][0] + m0.m[1][1]*m1.m[1][0] + m0.m[1][2]*m1.m[2][0],
00105 m0.m[1][0]*m1.m[0][1] + m0.m[1][1]*m1.m[1][1] + m0.m[1][2]*m1.m[2][1],
00106 m0.m[1][0]*m1.m[0][2] + m0.m[1][1]*m1.m[1][2] + m0.m[1][2]*m1.m[2][2],
00107
00108 m0.m[2][0]*m1.m[0][0] + m0.m[2][1]*m1.m[1][0] + m0.m[2][2]*m1.m[2][0],
00109 m0.m[2][0]*m1.m[0][1] + m0.m[2][1]*m1.m[1][1] + m0.m[2][2]*m1.m[2][1],
00110 m0.m[2][0]*m1.m[0][2] + m0.m[2][1]*m1.m[1][2] + m0.m[2][2]*m1.m[2][2]
00111 );
00112 return m2;
00113 }
00114
00115
00118 static
00119 inline
00120 AI_TVector3 operator * (const AI_TMatrix33& m, const AI_TVector3& v)
00121 {
00122 return AI_TVector3(
00123 m.m[0][0]*v.x + m.m[1][0]*v.y + m.m[2][0]*v.z,
00124 m.m[0][1]*v.x + m.m[1][1]*v.y + m.m[2][1]*v.z,
00125 m.m[0][2]*v.x + m.m[1][2]*v.y + m.m[2][2]*v.z);
00126 };
00127
00128
00131 inline
00132 AI_TMatrix33::AI_TMatrix33()
00133 {
00134 memcpy(&(m[0][0]), ai_matrix33_ident, sizeof(ai_matrix33_ident));
00135 }
00136
00137
00140 inline
00141 AI_TMatrix33::AI_TMatrix33(const AI_TVector3& v0, const AI_TVector3& v1, const AI_TVector3& v2)
00142 {
00143 m[0][0]=v0.x; m[0][1]=v0.y; m[0][2]=v0.z;
00144 m[1][0]=v1.x; m[1][1]=v1.y; m[1][2]=v1.z;
00145 m[2][0]=v2.x; m[2][1]=v2.y; m[2][2]=v2.z;
00146 }
00147
00148
00151 inline
00152 AI_TMatrix33::AI_TMatrix33(const AI_TMatrix33& m1)
00153 {
00154 memcpy(m, &(m1.m[0][0]), 9*sizeof(float));
00155 }
00156
00157
00160 inline
00161 AI_TMatrix33::AI_TMatrix33(float _m11, float _m12, float _m13,
00162 float _m21, float _m22, float _m23,
00163 float _m31, float _m32, float _m33)
00164 {
00165 m[0][0]=_m11; m[0][1]=_m12; m[0][2]=_m13;
00166 m[1][0]=_m21; m[1][1]=_m22; m[1][2]=_m23;
00167 m[2][0]=_m31; m[2][1]=_m32; m[2][2]=_m33;
00168 }
00169
00170
00173 inline
00174 AI_TMatrix33::AI_TMatrix33(const AI_Quaternion& q)
00175 {
00176 float xx = q.x*q.x; float yy = q.y*q.y; float zz = q.z*q.z;
00177 float xy = q.x*q.y; float xz = q.x*q.z; float yz = q.y*q.z;
00178 float wx = q.w*q.x; float wy = q.w*q.y; float wz = q.w*q.z;
00179
00180 m[0][0] = 1.0f - 2.0f * (yy + zz);
00181 m[1][0] = 2.0f * (xy - wz);
00182 m[2][0] = 2.0f * (xz + wy);
00183
00184 m[0][1] = 2.0f * (xy + wz);
00185 m[1][1] = 1.0f - 2.0f * (xx + zz);
00186 m[2][1] = 2.0f * (yz - wx);
00187
00188 m[0][2] = 2.0f * (xz - wy);
00189 m[1][2] = 2.0f * (yz + wx);
00190 m[2][2] = 1.0f - 2.0f * (xx + yy);
00191 }
00192
00193
00196 inline
00197 AI_Quaternion
00198 AI_TMatrix33::get_quaternion() const
00199 {
00200 float qa[4];
00201 float tr = m[0][0] + m[1][1] + m[2][2];
00202 if (tr > 0.0f)
00203 {
00204 float s = ai_sqrt (tr + 1.0f);
00205 qa[3] = s * 0.5f;
00206 s = 0.5f / s;
00207 qa[0] = (m[1][2] - m[2][1]) * s;
00208 qa[1] = (m[2][0] - m[0][2]) * s;
00209 qa[2] = (m[0][1] - m[1][0]) * s;
00210 }
00211 else
00212 {
00213 int i, j, k, nxt[3] = {1,2,0};
00214 i = 0;
00215 if (m[1][1] > m[0][0]) i=1;
00216 if (m[2][2] > m[i][i]) i=2;
00217 j = nxt[i];
00218 k = nxt[j];
00219 float s = ai_sqrt((m[i][i] - (m[j][j] + m[k][k])) + 1.0f);
00220 qa[i] = s * 0.5f;
00221 s = 0.5f / s;
00222 qa[3] = (m[j][k] - m[k][j])* s;
00223 qa[j] = (m[i][j] + m[j][i]) * s;
00224 qa[k] = (m[i][k] + m[k][i]) * s;
00225 }
00226 AI_Quaternion q(qa[0],qa[1],qa[2],qa[3]);
00227 return q;
00228 }
00229
00230
00233 inline
00234 AI_TVector3
00235 AI_TMatrix33::to_euler() const
00236 {
00237 AI_TVector3 ea;
00238
00239
00240 AI_TMatrix33 tmp(*this);
00241 tmp.transpose();
00242
00243 int i,j,k,h,n,s,f;
00244 EulGetOrd(EulOrdXYZs,i,j,k,h,n,s,f);
00245 if (s==EulRepYes)
00246 {
00247 double sy = (float) sqrt(tmp.m[0][1] * tmp.m[0][1] + tmp.m[0][2] * tmp.m[0][2]);
00248 if (sy > 16*FLT_EPSILON)
00249 {
00250 ea.x = (float) atan2(tmp.m[0][1], tmp.m[0][2]);
00251 ea.y = (float) atan2((float)sy, tmp.m[0][0]);
00252 ea.z = (float) atan2(tmp.m[1][0], -tmp.m[2][0]);
00253 } else {
00254 ea.x = (float) atan2(-tmp.m[1][2], tmp.m[1][1]);
00255 ea.y = (float) atan2((float)sy, tmp.m[0][0]);
00256 ea.z = 0;
00257 }
00258 }
00259 else
00260 {
00261 double cy = sqrt(tmp.m[0][0] * tmp.m[0][0] + tmp.m[1][0] * tmp.m[1][0]);
00262 if (cy > 16*FLT_EPSILON)
00263 {
00264 ea.x = (float) atan2(tmp.m[2][1], tmp.m[2][2]);
00265 ea.y = (float) atan2(-tmp.m[2][0], (float)cy);
00266 ea.z = (float) atan2(tmp.m[1][0], tmp.m[0][0]);
00267 }
00268 else
00269 {
00270 ea.x = (float) atan2(-tmp.m[1][2], tmp.m[1][1]);
00271 ea.y = (float) atan2(-tmp.m[2][0], (float)cy);
00272 ea.z = 0;
00273 }
00274 }
00275 if (n==EulParOdd) {ea.x = -ea.x; ea.y = - ea.y; ea.z = -ea.z;}
00276 if (f==EulFrmR) {float t = ea.x; ea.x = ea.z; ea.z = t;}
00277
00278 return ea;
00279 }
00280
00281
00284 inline
00285 void
00286 AI_TMatrix33::from_euler(const AI_TVector3& ea)
00287 {
00288 AI_TVector3 tea = ea;
00289 double ti, tj, th, ci, cj, ch, si, sj, sh, cc, cs, sc, ss;
00290 int i,j,k,h,n,s,f;
00291 EulGetOrd(EulOrdXYZs,i,j,k,h,n,s,f);
00292 if (f==EulFrmR) {float t = ea.x; tea.x = ea.z; tea.z = t;}
00293 if (n==EulParOdd) {tea.x = -ea.x; tea.y = -ea.y; tea.z = -ea.z;}
00294 ti = tea.x; tj = tea.y; th = tea.z;
00295 ci = cos(ti); cj = cos(tj); ch = cos(th);
00296 si = sin(ti); sj = sin(tj); sh = sin(th);
00297 cc = ci*ch; cs = ci*sh; sc = si*ch; ss = si*sh;
00298 if (s==EulRepYes)
00299 {
00300 m[0][0] = (float)(cj); m[0][1] = (float)(sj*si); m[0][2] = (float)(sj*ci);
00301 m[1][0] = (float)(sj*sh); m[1][1] = (float)(-cj*ss+cc); m[1][2] = (float)(-cj*cs-sc);
00302 m[2][0] = (float)(-sj*ch); m[1][2] = (float)( cj*sc+cs); m[2][2] = (float)( cj*cc-ss);
00303 }
00304 else
00305 {
00306 m[0][0] = (float)(cj*ch); m[0][1] = (float)(sj*sc-cs); m[0][2] = (float)(sj*cc+ss);
00307 m[1][0] = (float)(cj*sh); m[1][1] = (float)(sj*ss+cc); m[1][2] = (float)(sj*cs-sc);
00308 m[2][0] = (float)(-sj); m[2][1] = (float)(cj*si); m[2][2] = (float)(cj*ci);
00309 }
00310
00311
00312 this->transpose();
00313 }
00314
00315
00318 inline
00319 void
00320 AI_TMatrix33::lookat(const AI_TVector3& from, const AI_TVector3& to, const AI_TVector3& up)
00321 {
00322 AI_TVector3 z(from - to);
00323 z.norm();
00324 AI_TVector3 x(up * z);
00325 x.norm();
00326 AI_TVector3 y = z * x;
00327
00328 m[0][0]=x.x; m[0][1]=x.y; m[0][2]=x.z;
00329 m[1][0]=y.x; m[1][1]=y.y; m[1][2]=y.z;
00330 m[2][0]=z.x; m[2][1]=z.y; m[2][2]=z.z;
00331 }
00332
00333
00336 inline
00337 void
00338 AI_TMatrix33::billboard(const AI_TVector3& from, const AI_TVector3& to, const AI_TVector3& up)
00339 {
00340 AI_TVector3 z(from - to);
00341 z.norm();
00342 AI_TVector3 y(up);
00343 y.norm();
00344 AI_TVector3 x(y * z);
00345 z = x * y;
00346
00347 m[0][0]=x.x; m[0][1]=x.y; m[0][2]=x.z;
00348 m[1][0]=y.x; m[1][1]=y.y; m[1][2]=y.z;
00349 m[2][0]=z.x; m[2][1]=z.y; m[2][2]=z.z;
00350 }
00351
00352
00355 inline
00356 void
00357 AI_TMatrix33::set(float m11, float m12, float m13,
00358 float m21, float m22, float m23,
00359 float m31, float m32, float m33)
00360 {
00361 m[0][0]=m11; m[0][1]=m12; m[0][2]=m13;
00362 m[1][0]=m21; m[1][1]=m22; m[1][2]=m23;
00363 m[2][0]=m31; m[2][1]=m32; m[2][2]=m33;
00364 }
00365
00366
00369 inline
00370 void
00371 AI_TMatrix33::set(const AI_TVector3& v0, const AI_TVector3& v1, const AI_TVector3& v2)
00372 {
00373 m[0][0]=v0.x; m[0][1]=v0.y; m[0][2]=v0.z;
00374 m[1][0]=v1.x; m[1][1]=v1.y; m[1][2]=v1.z;
00375 m[2][0]=v2.x; m[2][1]=v2.y; m[2][2]=v2.z;
00376 }
00377
00378
00381 inline
00382 void
00383 AI_TMatrix33::set(const AI_TMatrix33& m1)
00384 {
00385 memcpy(m, &(m1.m), 9*sizeof(float));
00386 }
00387
00388
00391 inline
00392 void
00393 AI_TMatrix33::ident()
00394 {
00395 memcpy(&(m[0][0]), ai_matrix33_ident, sizeof(ai_matrix33_ident));
00396 }
00397
00398
00401 inline
00402 void
00403 AI_TMatrix33::transpose()
00404 {
00405 #undef n_swap
00406 #define n_swap(x,y) { float t=x; x=y; y=t; }
00407 n_swap(m[0][1],m[1][0]);
00408 n_swap(m[0][2],m[2][0]);
00409 n_swap(m[1][2],m[2][1]);
00410 }
00411
00412
00415 inline
00416 bool
00417 AI_TMatrix33::orthonorm(float limit)
00418 {
00419 if (((m[0][0]*m[1][0]+m[0][1]*m[1][1]+m[0][2]*m[1][2])<limit) &&
00420 ((m[0][0]*m[2][0]+m[0][1]*m[2][1]+m[0][2]*m[2][2])<limit) &&
00421 ((m[2][0]*m[1][0]+m[2][1]*m[1][1]+m[2][2]*m[1][2])<limit) &&
00422 ((m[0][0]*m[0][0]+m[0][1]*m[0][1]+m[0][2]*m[0][2])>(1.0-limit)) &&
00423 ((m[0][0]*m[0][0]+m[0][1]*m[0][1]+m[0][2]*m[0][2])<(1.0+limit)) &&
00424 ((m[1][0]*m[1][0]+m[1][1]*m[1][1]+m[1][2]*m[1][2])>(1.0-limit)) &&
00425 ((m[1][0]*m[1][0]+m[1][1]*m[1][1]+m[1][2]*m[1][2])<(1.0+limit)) &&
00426 ((m[2][0]*m[2][0]+m[2][1]*m[2][1]+m[2][2]*m[2][2])>(1.0-limit)) &&
00427 ((m[2][0]*m[2][0]+m[2][1]*m[2][1]+m[2][2]*m[2][2])<(1.0+limit)))
00428 return true;
00429 else
00430 return false;
00431 }
00432
00433
00436 inline
00437 void
00438 AI_TMatrix33::scale(const AI_TVector3& s)
00439 {
00440 int i;
00441 for (i=0; i<3; i++) {
00442 m[i][0] *= s.x;
00443 m[i][1] *= s.y;
00444 m[i][2] *= s.z;
00445 }
00446 }
00447
00448
00451 inline
00452 void
00453 AI_TMatrix33::rotate_x(const float a)
00454 {
00455 float c = ai_cos(a);
00456 float s = ai_sin(a);
00457 int i;
00458 for (i=0; i<3; i++)
00459 {
00460 float mi1 = m[i][1];
00461 float mi2 = m[i][2];
00462 m[i][1] = mi1*c + mi2*-s;
00463 m[i][2] = mi1*s + mi2*c;
00464 }
00465 }
00466
00467
00470 inline
00471 void
00472 AI_TMatrix33::rotate_y(const float a)
00473 {
00474 float c = ai_cos(a);
00475 float s = ai_sin(a);
00476 int i;
00477 for (i=0; i<3; i++)
00478 {
00479 float mi0 = m[i][0];
00480 float mi2 = m[i][2];
00481 m[i][0] = mi0*c + mi2*s;
00482 m[i][2] = mi0*-s + mi2*c;
00483 }
00484 }
00485
00486
00489 inline
00490 void
00491 AI_TMatrix33::rotate_z(const float a)
00492 {
00493 float c = ai_cos(a);
00494 float s = ai_sin(a);
00495 int i;
00496 for (i=0; i<3; i++)
00497 {
00498 float mi0 = m[i][0];
00499 float mi1 = m[i][1];
00500 m[i][0] = mi0*c + mi1*-s;
00501 m[i][1] = mi0*s + mi1*c;
00502 }
00503 }
00504
00505
00508 inline
00509 void
00510 AI_TMatrix33::rotate_local_x(const float a)
00511 {
00512 AI_TMatrix33 rotM;
00513 rotM.m[1][1] = (float) cos(a); rotM.m[1][2] = -(float) sin(a);
00514 rotM.m[2][1] = (float) sin(a); rotM.m[2][2] = (float) cos(a);
00515
00516 (*this) = rotM * (*this);
00517 }
00518
00519
00522 inline
00523 void
00524 AI_TMatrix33::rotate_local_y(const float a)
00525 {
00526 AI_TMatrix33 rotM;
00527 rotM.m[0][0] = (float) cos(a); rotM.m[0][2] = (float) sin(a);
00528 rotM.m[2][0] = -(float) sin(a); rotM.m[2][2] = (float) cos(a);
00529
00530 (*this) = rotM * (*this);
00531 }
00532
00533
00536 inline
00537 void
00538 AI_TMatrix33::rotate_local_z(const float a)
00539 {
00540 AI_TMatrix33 rotM;
00541 rotM.m[0][0] = (float) cos(a); rotM.m[0][1] = -(float) sin(a);
00542 rotM.m[1][0] = (float) sin(a); rotM.m[1][1] = (float) cos(a);
00543
00544 (*this) = rotM * (*this);
00545 }
00546
00547
00550 inline
00551 void
00552 AI_TMatrix33::rotate(const AI_TVector3& vec, float a)
00553 {
00554 AI_TVector3 v(vec);
00555 v.norm();
00556 float sa = (float) ai_sin(a);
00557 float ca = (float) ai_cos(a);
00558
00559 AI_TMatrix33 rotM;
00560 rotM.m[0][0] = ca + (1.0f - ca) * v.x * v.x;
00561 rotM.m[0][1] = (1.0f - ca) * v.x * v.y - sa * v.z;
00562 rotM.m[0][2] = (1.0f - ca) * v.z * v.x + sa * v.y;
00563 rotM.m[1][0] = (1.0f - ca) * v.x * v.y + sa * v.z;
00564 rotM.m[1][1] = ca + (1.0f - ca) * v.y * v.y;
00565 rotM.m[1][2] = (1.0f - ca) * v.y * v.z - sa * v.x;
00566 rotM.m[2][0] = (1.0f - ca) * v.z * v.x - sa * v.y;
00567 rotM.m[2][1] = (1.0f - ca) * v.y * v.z + sa * v.x;
00568 rotM.m[2][2] = ca + (1.0f - ca) * v.z * v.z;
00569
00570 (*this) = (*this) * rotM;
00571 }
00572
00573
00576 inline
00577 AI_TVector3
00578 AI_TMatrix33::x_component() const
00579 {
00580 AI_TVector3 v(m[0][0],m[0][1],m[0][2]);
00581 return v;
00582 }
00583
00584
00587 inline
00588 AI_TVector3
00589 AI_TMatrix33::y_component(void) const
00590 {
00591 AI_TVector3 v(m[1][0],m[1][1],m[1][2]);
00592 return v;
00593 }
00594
00595
00598 inline
00599 AI_TVector3
00600 AI_TMatrix33::z_component(void) const
00601 {
00602 AI_TVector3 v(m[2][0],m[2][1],m[2][2]);
00603 return v;
00604 };
00605
00606
00609 inline
00610 void
00611 AI_TMatrix33::operator *= (const AI_TMatrix33& m1)
00612 {
00613 int i;
00614 for (i=0; i<3; i++) {
00615 float mi0 = m[i][0];
00616 float mi1 = m[i][1];
00617 float mi2 = m[i][2];
00618 m[i][0] = mi0*m1.m[0][0] + mi1*m1.m[1][0] + mi2*m1.m[2][0];
00619 m[i][1] = mi0*m1.m[0][1] + mi1*m1.m[1][1] + mi2*m1.m[2][1];
00620 m[i][2] = mi0*m1.m[0][2] + mi1*m1.m[1][2] + mi2*m1.m[2][2];
00621 };
00622 }
00623
00624
00629 inline
00630 void
00631 AI_TMatrix33::mult(const AI_TVector3& src, AI_TVector3& dst) const
00632 {
00633 dst.x = m[0][0]*src.x + m[1][0]*src.y + m[2][0]*src.z;
00634 dst.y = m[0][1]*src.x + m[1][1]*src.y + m[2][1]*src.z;
00635 dst.z = m[0][2]*src.x + m[1][2]*src.y + m[2][2]*src.z;
00636 }
00637
00638
00641 inline
00642 void
00643 AI_TMatrix33::translate(const AI_TVector2& t)
00644 {
00645 m[2][0] += t.x;
00646 m[2][1] += t.y;
00647 }
00648
00649
00650 #endif