00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032 #ifndef Matrix3_INCLUDE_ONCE
00033 #define Matrix3_INCLUDE_ONCE
00034
00035 #include <vlCore/Vector3.hpp>
00036 #include <vlCore/Matrix2.hpp>
00037
00038 namespace vl
00039 {
00040
00041
00042
00047 template<typename T_Scalar>
00048 class Matrix3
00049 {
00050 public:
00051 typedef T_Scalar scalar_type;
00052
00053 template<typename T>
00054 explicit Matrix3(const Matrix3<T>& m)
00055 {
00056 e(0,0) = (T_Scalar)m.e(0,0); e(1,0) = (T_Scalar)m.e(1,0); e(2,0) = (T_Scalar)m.e(2,0);
00057 e(0,1) = (T_Scalar)m.e(0,1); e(1,1) = (T_Scalar)m.e(1,1); e(2,1) = (T_Scalar)m.e(2,1);
00058 e(0,2) = (T_Scalar)m.e(0,2); e(1,2) = (T_Scalar)m.e(1,2); e(2,2) = (T_Scalar)m.e(2,2);
00059 }
00060
00061 Matrix3()
00062 {
00063 setIdentity();
00064 }
00065
00066 explicit Matrix3(T_Scalar n)
00067 {
00068 setIdentity();
00069 e(0,0) = e(1,1) = e(2,2) = n;
00070 }
00071
00072 explicit Matrix3(T_Scalar e00, T_Scalar e01, T_Scalar e02,
00073 T_Scalar e10, T_Scalar e11, T_Scalar e12,
00074 T_Scalar e20, T_Scalar e21, T_Scalar e22)
00075 {
00076 e(0,0) = e00; e(0,1) = e01; e(0,2) = e02;
00077 e(1,0) = e10; e(1,1) = e11; e(1,2) = e12;
00078 e(2,0) = e20; e(2,1) = e21; e(2,2) = e22;
00079 }
00080
00081 Matrix3& fill(T_Scalar val)
00082 {
00083 e(0,0) = e(1,0) = e(2,0) =
00084 e(0,1) = e(1,1) = e(2,1) =
00085 e(0,2) = e(1,2) = e(2,2) = val;
00086 return *this;
00087 }
00088
00089 T_Scalar diff(const Matrix3& other) const
00090 {
00091 T_Scalar err = 0;
00092 for(int i=0; i<3; ++i)
00093 for(int j=0; j<3; ++j)
00094 if (e(j,i) > other.e(j,i))
00095 err += e(j,i) - other.e(j,i);
00096 else
00097 err += other.e(j,i) - e(j,i);
00098 return err;
00099 }
00100
00101 Vector2<T_Scalar> getX() const
00102 {
00103 Vector2<T_Scalar> v;
00104 v.x() = e(0,0);
00105 v.y() = e(1,0);
00106 return v;
00107 }
00108
00109 Vector2<T_Scalar> getY() const
00110 {
00111 Vector2<T_Scalar> v;
00112 v.x() = e(0,1);
00113 v.y() = e(1,1);
00114 return v;
00115 }
00116
00117 Vector2<T_Scalar> getT() const
00118 {
00119 Vector2<T_Scalar> v;
00120 v.x() = e(0,2);
00121 v.y() = e(1,2);
00122 return v;
00123 }
00124
00125 Matrix3& setX(const Vector2<T_Scalar>& v)
00126 {
00127 e(0,0) = v.x();
00128 e(1,0) = v.y();
00129 return *this;
00130 }
00131
00132 Matrix3& setY(const Vector2<T_Scalar>& v)
00133 {
00134 e(0,1) = v.x();
00135 e(1,1) = v.y();
00136 return *this;
00137 }
00138
00139 Matrix3& setT(const Vector2<T_Scalar>& v)
00140 {
00141 e(0,2) = v.x();
00142 e(1,2) = v.y();
00143 return *this;
00144 }
00145
00146 bool operator==(const Matrix3& m) const
00147 {
00148 return memcmp(m.mVec, mVec, sizeof(T_Scalar)*9) == 0;
00149 }
00150
00151 bool operator!=(const Matrix3& m) const
00152 {
00153 return !operator==(m);
00154 }
00155
00156 Matrix3& operator=(const Matrix3& m)
00157 {
00158 memcpy(mVec, m.mVec, sizeof(T_Scalar)*9);
00159 return *this;
00160 }
00161
00162 Matrix3 operator+(const Matrix3& m) const
00163 {
00164 Matrix3 t;
00165 for(int i=0; i<3; ++i)
00166 for(int j=0; j<3; ++j)
00167 t.e(j,i) = e(j,i) + m.e(j,i);
00168 return t;
00169 }
00170
00171 Matrix3& operator+=(const Matrix3& m)
00172 {
00173 for(int i=0; i<3; ++i)
00174 for(int j=0; j<3; ++j)
00175 e(j,i) += m.e(j,i);
00176 return *this;
00177 }
00178
00179 Matrix3 operator-(const Matrix3& m) const
00180 {
00181 Matrix3 t;
00182 for(int i=0; i<3; ++i)
00183 for(int j=0; j<3; ++j)
00184 t.e(j,i) = e(j,i) - m.e(j,i);
00185 return t;
00186 }
00187
00188 Matrix3& operator-=(const Matrix3& m)
00189 {
00190 for(int i=0; i<3; ++i)
00191 for(int j=0; j<3; ++j)
00192 e(j,i) -= m.e(j,i);
00193 return *this;
00194 }
00195
00196 Matrix3& operator*=(const Matrix3& m)
00197 {
00198 return postMultiply(m);
00199 }
00200
00201 Matrix3 operator-() const
00202 {
00203 Matrix3 t;
00204 for(int i=0; i<3; ++i)
00205 for(int j=0; j<3; ++j)
00206 t.e(j,i) = -e(j,i);
00207 return t;
00208 }
00209
00210 Matrix3 operator+(T_Scalar d) const
00211 {
00212 Matrix3 t;
00213 for(int i=0; i<3; ++i)
00214 for(int j=0; j<3; ++j)
00215 t.e(j,i) = e(j,i) + d;
00216 return t;
00217 }
00218
00219 Matrix3& operator+=(T_Scalar d)
00220 {
00221 for(int i=0; i<3; ++i)
00222 for(int j=0; j<3; ++j)
00223 e(j,i) += d;
00224 return *this;
00225 }
00226
00227 Matrix3 operator-(T_Scalar d) const
00228 {
00229 Matrix3 t;
00230 for(int i=0; i<3; ++i)
00231 for(int j=0; j<3; ++j)
00232 t.e(j,i) = e(j,i) - d;
00233 return t;
00234 }
00235
00236 Matrix3& operator-=(T_Scalar d)
00237 {
00238 for(int i=0; i<3; ++i)
00239 for(int j=0; j<3; ++j)
00240 e(j,i) -= d;
00241 return *this;
00242 }
00243
00244 Matrix3 operator*(T_Scalar d) const
00245 {
00246 Matrix3 t;
00247 for(int i=0; i<3; ++i)
00248 for(int j=0; j<3; ++j)
00249 t.e(j,i) = e(j,i) * d;
00250 return t;
00251 }
00252
00253 Matrix3& operator*=(T_Scalar d)
00254 {
00255 for(int i=0; i<3; ++i)
00256 for(int j=0; j<3; ++j)
00257 e(j,i) *= d;
00258 return *this;
00259 }
00260
00261 Matrix3 operator/(T_Scalar d) const
00262 {
00263 d = (T_Scalar)1 / d;
00264 Matrix3 t;
00265 for(int i=0; i<3; ++i)
00266 for(int j=0; j<3; ++j)
00267 t.e(j,i) = e(j,i) * d;
00268 return t;
00269 }
00270
00271 Matrix3& operator/=(T_Scalar d)
00272 {
00273 d = (T_Scalar)1 / d;
00274 for(int i=0; i<3; ++i)
00275 for(int j=0; j<3; ++j)
00276 e(j,i) *= d;
00277 return *this;
00278 }
00279
00280 bool isIdentity() const
00281 {
00282 Matrix3 i;
00283 return memcmp(ptr(), i.ptr(), sizeof(T_Scalar)*9) == 0;
00284 }
00285
00286 Matrix2<T_Scalar> get2x2() const
00287 {
00288 Matrix2<T_Scalar> t;
00289 t.e(0,0) = e(0,0); t.e(1,0) = e(1,0);
00290 t.e(0,1) = e(0,1); t.e(1,1) = e(1,1);
00291 return t;
00292 }
00293
00295 void set2x2(const Matrix2<T_Scalar>& m)
00296 {
00297 e(0,0) = m.e(0,0); e(1,0) = m.e(1,0);
00298 e(0,1) = m.e(0,1); e(1,1) = m.e(1,1);
00299 }
00300
00301 T_Scalar* ptr()
00302 {
00303 return &e(0,0);
00304 }
00305
00306 const T_Scalar* ptr() const
00307 {
00308 return &e(0,0);
00309 }
00310
00311 Matrix3& transpose()
00312 {
00313 T_Scalar tmp;
00314 for(int i=0; i<3; ++i)
00315 for(int j=i; j<3; ++j)
00316 {
00317 tmp = e(j,i);
00318 e(j,i) = e(i,j);
00319 e(i,j) = tmp;
00320 }
00321 return *this;
00322 }
00323
00324 Matrix3 getTransposed() const
00325 {
00326 Matrix3 m;
00327 for(int i=0; i<3; ++i)
00328 for(int j=0; j<3; ++j)
00329 m.e(j,i) = e(i,j);
00330 return m;
00331 }
00332
00333 Matrix3& getTransposed(Matrix3& dest) const
00334 {
00335 for(int i=0; i<3; ++i)
00336 for(int j=0; j<3; ++j)
00337 dest.e(j,i) = e(i,j);
00338 return dest;
00339 }
00340
00341 bool isNull() const
00342 {
00343 for(int i=0; i<3; ++i)
00344 for(int j=0; j<3; ++j)
00345 if(mVec[j][i] != 0)
00346 return false;
00347 return true;
00348 }
00349
00350 Matrix3& setNull()
00351 {
00352 fill(0);
00353 return *this;
00354 }
00355
00356 static Matrix3& getNull(Matrix3& out)
00357 {
00358 out.fill(0);
00359 return out;
00360 }
00361
00362 static Matrix3 getNull()
00363 {
00364 return Matrix3().fill(0);
00365 }
00366
00367 Matrix3& setIdentity()
00368 {
00369 static const T_Scalar I3d[] =
00370 {
00371 (T_Scalar)1, (T_Scalar)0, (T_Scalar)0,
00372 (T_Scalar)0, (T_Scalar)1, (T_Scalar)0,
00373 (T_Scalar)0, (T_Scalar)0, (T_Scalar)1,
00374 };
00375 memcpy(mVec, I3d, sizeof(T_Scalar)*9);
00376 return *this;
00377 }
00378
00379 static Matrix3 getIdentity()
00380 {
00381 return Matrix3();
00382 }
00383
00384 static Matrix3& getIdentity(Matrix3& out)
00385 {
00386 out.setIdentity();
00387 return out;
00388 }
00389
00390 T_Scalar getInverse(Matrix3& dest) const;
00391
00392 Matrix3 getInverse(T_Scalar *determinant=NULL) const
00393 {
00394 Matrix3 tmp;
00395 T_Scalar det = getInverse(tmp);
00396 if (determinant)
00397 *determinant = det;
00398 return tmp;
00399 }
00400
00401 Matrix3& invert(T_Scalar *determinant=NULL)
00402 {
00403 T_Scalar det = getInverse(*this);
00404 if (determinant)
00405 *determinant = det;
00406 return *this;
00407 }
00408
00409 static Matrix3 getRotation(T_Scalar degrees);
00410
00411 Matrix3& rotate(T_Scalar degrees)
00412 {
00413 return preMultiply(getRotation(degrees));
00414 }
00415
00416 static Matrix3& getTranslation(Matrix3& out, const Vector2<T_Scalar>& v)
00417 {
00418 return getTranslation(out, v.x(), v.y());
00419 }
00420
00421 static Matrix3 getTranslation(const Vector2<T_Scalar>& v)
00422 {
00423 return getTranslation(v.x(), v.y());
00424 }
00425
00426 static Matrix3 getTranslation(T_Scalar x, T_Scalar y)
00427 {
00428 Matrix3 m;
00429 return getTranslation(m, x, y);
00430 }
00431
00432 static Matrix3& getTranslation(Matrix3& out, T_Scalar x, T_Scalar y)
00433 {
00434 out.setIdentity();
00435 out.e(0,2) = x;
00436 out.e(1,2) = y;
00437 return out;
00438 }
00439
00440 Matrix3& translate(T_Scalar x, T_Scalar y)
00441 {
00442 return preMultiply(getTranslation(x,y));
00443 }
00444
00445 Matrix3& translate(const Vector2<T_Scalar>& v)
00446 {
00447 return preMultiply(getTranslation(v));
00448 }
00449
00450 static Matrix3& getScaling(Matrix3& out, const Vector2<T_Scalar>& v)
00451 {
00452 return getScaling(out, v.x(), v.y());
00453 }
00454
00455 static Matrix3 getScaling(const Vector2<T_Scalar>& v)
00456 {
00457 Matrix3 m;
00458 return getScaling(m, v.x(), v.y());
00459 }
00460
00461 static Matrix3 getScaling(T_Scalar x, T_Scalar y)
00462 {
00463 Matrix3 m;
00464 return getScaling(m, x, y);
00465 }
00466
00467 static Matrix3& getScaling(Matrix3& out, T_Scalar x, T_Scalar y)
00468 {
00469 out.setIdentity();
00470 out.e(0,0) = x;
00471 out.e(1,1) = y;
00472 return out;
00473 }
00474
00475 Matrix3& scale(T_Scalar x, T_Scalar y)
00476 {
00477 return preMultiply(getScaling(x,y));
00478 }
00479
00480 Matrix3& scale(const Vector2<T_Scalar>& v)
00481 {
00482 return preMultiply(getScaling(v.x(),v.y()));
00483 }
00484
00485 static Matrix3& multiply(Matrix3& out, const Matrix3& p, const Matrix3& q)
00486 {
00487 VL_CHECK(out.ptr() != p.ptr() && out.ptr() != q.ptr());
00488
00489 out.e(0,0) = q.e(0,0)*p.e(0,0) + q.e(1,0)*p.e(0,1) + q.e(2,0)*p.e(0,2);
00490 out.e(0,1) = q.e(0,1)*p.e(0,0) + q.e(1,1)*p.e(0,1) + q.e(2,1)*p.e(0,2);
00491 out.e(0,2) = q.e(0,2)*p.e(0,0) + q.e(1,2)*p.e(0,1) + q.e(2,2)*p.e(0,2);
00492
00493 out.e(1,0) = q.e(0,0)*p.e(1,0) + q.e(1,0)*p.e(1,1) + q.e(2,0)*p.e(1,2);
00494 out.e(1,1) = q.e(0,1)*p.e(1,0) + q.e(1,1)*p.e(1,1) + q.e(2,1)*p.e(1,2);
00495 out.e(1,2) = q.e(0,2)*p.e(1,0) + q.e(1,2)*p.e(1,1) + q.e(2,2)*p.e(1,2);
00496
00497 out.e(2,0) = q.e(0,0)*p.e(2,0) + q.e(1,0)*p.e(2,1) + q.e(2,0)*p.e(2,2);
00498 out.e(2,1) = q.e(0,1)*p.e(2,0) + q.e(1,1)*p.e(2,1) + q.e(2,1)*p.e(2,2);
00499 out.e(2,2) = q.e(0,2)*p.e(2,0) + q.e(1,2)*p.e(2,1) + q.e(2,2)*p.e(2,2);
00500
00501 return out;
00502 }
00503
00504 Matrix3& postMultiply(const Matrix3& m)
00505 {
00506 Matrix3<T_Scalar> t;
00507 return *this = multiply(t, *this, m);
00508 }
00509
00510 Matrix3& preMultiply(const Matrix3& m)
00511 {
00512 Matrix3<T_Scalar> t;
00513 return *this = multiply(t, m, *this);
00514 }
00515
00516
00517 const T_Scalar& e(int i, int j) const { return mVec[j][i]; }
00518 T_Scalar& e(int i, int j) { return mVec[j][i]; }
00519
00520 private:
00521 const Vector3<T_Scalar>& operator[](unsigned int i) const { VL_CHECK(i<3); return mVec[i]; }
00522 Vector3<T_Scalar>& operator[](unsigned int i) { VL_CHECK(i<3); return mVec[i]; }
00523
00524 protected:
00525 Vector3<T_Scalar> mVec[3];
00526 };
00527
00528
00529
00530
00531 template<typename T_Scalar>
00532 inline Matrix3<T_Scalar> operator*(const Matrix3<T_Scalar>& p, const Matrix3<T_Scalar>& q)
00533 {
00534 Matrix3<T_Scalar> t;
00535 Matrix3<T_Scalar>::multiply(t, p, q);
00536 return t;
00537 }
00538
00539 template<typename T_Scalar>
00540 inline Matrix3<T_Scalar> operator+(T_Scalar d, const Matrix3<T_Scalar>& m)
00541 {
00542 return m + d;
00543 }
00544
00545 template<typename T_Scalar>
00546 inline Matrix3<T_Scalar> operator*(T_Scalar d, const Matrix3<T_Scalar>& m)
00547 {
00548 return m * d;
00549 }
00550
00552 template<typename T_Scalar>
00553 inline Vector3<T_Scalar> operator*(const Matrix3<T_Scalar>& m, const Vector3<T_Scalar>& v)
00554 {
00555 Vector3<T_Scalar> t;
00556 t.x() = v.x()*m.e(0,0) + v.y()*m.e(0,1) + v.z()*m.e(0,2);
00557 t.y() = v.x()*m.e(1,0) + v.y()*m.e(1,1) + v.z()*m.e(1,2);
00558 t.z() = v.x()*m.e(2,0) + v.y()*m.e(2,1) + v.z()*m.e(2,2);
00559 return t;
00560 }
00561
00564 template<typename T_Scalar>
00565 inline Vector2<T_Scalar> operator*(const Matrix3<T_Scalar>& m, const Vector2<T_Scalar>& v)
00566 {
00567 Vector2<T_Scalar> t;
00568 t.x() = v.x()*m.e(0,0) + v.y()*m.e(0,1) ;
00569 t.y() = v.x()*m.e(1,0) + v.y()*m.e(1,1) ;
00570 return t;
00571 }
00572
00574 template<typename T_Scalar>
00575 inline Vector3<T_Scalar> operator*(const Vector3<T_Scalar>& v, const Matrix3<T_Scalar>& m)
00576 {
00577 Vector3<T_Scalar> t;
00578 t.x() = v.x()*m.e(0,0) + v.y()*m.e(1,0) + v.z()*m.e(2,0);
00579 t.y() = v.x()*m.e(0,1) + v.y()*m.e(1,1) + v.z()*m.e(2,1);
00580 t.z() = v.x()*m.e(0,2) + v.y()*m.e(1,2) + v.z()*m.e(2,2);
00581 return t;
00582 }
00583
00586 template<typename T_Scalar>
00587 inline Vector2<T_Scalar> operator*(const Vector2<T_Scalar>& v, const Matrix3<T_Scalar>& m)
00588 {
00589 Vector2<T_Scalar> t;
00590 t.x() = v.x()*m.e(0,0) + v.y()*m.e(1,0) ;
00591 t.y() = v.x()*m.e(0,1) + v.y()*m.e(1,1) ;
00592 return t;
00593 }
00594
00595 template<typename T_Scalar>
00596 Matrix3<T_Scalar> Matrix3<T_Scalar>::getRotation(T_Scalar degrees)
00597 {
00598 Matrix3<T_Scalar> rot;
00599 degrees = degrees * (T_Scalar)dDEG_TO_RAD;
00600 T_Scalar s = (T_Scalar) sin(degrees);
00601 T_Scalar c = (T_Scalar) cos(degrees);
00602 rot.e(0,0) = (T_Scalar)c;
00603 rot.e(1,1) = (T_Scalar)c;
00604 rot.e(1,0) = (T_Scalar)s;
00605 rot.e(0,1) = -(T_Scalar)s;
00606 return rot;
00607 }
00608
00609 template<typename T_Scalar>
00610 T_Scalar Matrix3<T_Scalar>::getInverse(Matrix3<T_Scalar>& dest) const
00611 {
00612 if (&dest == this)
00613 {
00614 Matrix3<T_Scalar> tmp;
00615 T_Scalar det = getInverse(tmp);
00616 dest = tmp;
00617 return det;
00618 }
00619 else
00620 {
00621 const T_Scalar& a11 = e(0,0);
00622 const T_Scalar& a21 = e(1,0);
00623 const T_Scalar& a31 = e(2,0);
00624 const T_Scalar& a12 = e(0,1);
00625 const T_Scalar& a22 = e(1,1);
00626 const T_Scalar& a32 = e(2,1);
00627 const T_Scalar& a13 = e(0,2);
00628 const T_Scalar& a23 = e(1,2);
00629 const T_Scalar& a33 = e(2,2);
00630
00631 T_Scalar A = a22*a33 - a32*a23;
00632 T_Scalar B = a23*a31 - a33*a21;
00633 T_Scalar C = a21*a32 - a31*a22;
00634
00635 T_Scalar det = a11*A + a12*B + a13*C;
00636
00637 if (det == 0)
00638 dest.fill(0);
00639 else
00640 dest = Matrix3<T_Scalar>(A, a13*a32 - a33*a12, a12*a23 - a22*a13,
00641 B, a11*a33 - a31*a13, a13*a21 - a23*a11,
00642 C, a12*a31 - a32*a11, a11*a22 - a21*a12) / det;
00643 return det;
00644 }
00645 }
00646
00647
00649 typedef Matrix3<double> dmat3;
00651 typedef Matrix3<float> fmat3;
00653 typedef Matrix3<int> imat3;
00655 typedef Matrix3<unsigned int> umat3;
00656
00657 #if VL_PIPELINE_PRECISION == 2
00658
00659 typedef dmat3 mat3;
00660 #else
00661
00662 typedef fmat3 mat3;
00663 #endif
00664 }
00665
00666 #endif