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
00033
00034
00035
00036
00037
00038
00039
00040
00041
00042
00043
00044
00045
00046
00047
00048
00049
00050
00051
00052
00053
00054
00055
00056
00057
00058
00059
00060
00061
00062
00063
00064
00065
00066
00067
00068
00069
00070
00071
00072
00073
00074
00075
00076
00077
00078
00079
00080
00081
00082
00083
00084
00085
00086
00087
00088
00089
00090
00091
00092 #ifndef __VCGLIB_MATRIX33_H
00093 #define __VCGLIB_MATRIX33_H
00094
00095 #include <stdio.h>
00096 #include <vcg/math/lin_algebra.h>
00097 #include <vcg/math/matrix44.h>
00098 #include <vcg/space/point3.h>
00099 #include <vector>
00100
00101 namespace vcg {
00102
00103 template <class S>
00104 class Matrix33Diag:public Point3<S>{
00105 public:
00106
00112 Matrix33Diag(const S & p0,const S & p1,const S & p2):Point3<S>(p0,p1,p2){};
00113 Matrix33Diag(const Point3<S>&p ):Point3<S>(p){};
00114 };
00115
00116 template<class S>
00122 class Matrix33
00123 {
00124 public:
00125 typedef S ScalarType;
00126
00128 inline Matrix33() {}
00129
00131 Matrix33( const Matrix33 & m )
00132 {
00133 for(int i=0;i<9;++i)
00134 a[i] = m.a[i];
00135 }
00136
00138 Matrix33( const S * v )
00139 {
00140 for(int i=0;i<9;++i) a[i] = v[i];
00141 }
00142
00144 Matrix33 (const Matrix44<S> & m, const int & k)
00145 {
00146 int i,j, r=0, c=0;
00147 for(i = 0; i< 4;++i)if(i!=k){c=0;
00148 for(j=0; j < 4;++j)if(j!=k)
00149 { (*this)[r][c] = m[i][j]; ++c;}
00150 ++r;
00151 }
00152 }
00153
00155 inline unsigned int ColumnsNumber() const
00156 {
00157 return 3;
00158 };
00159
00161 inline unsigned int RowsNumber() const
00162 {
00163 return 3;
00164 };
00165
00167 Matrix33 & operator = ( const Matrix33 & m )
00168 {
00169 for(int i=0;i<9;++i)
00170 a[i] = m.a[i];
00171 return *this;
00172 }
00173
00174
00175
00177 inline S * operator [] ( const int i )
00178 {
00179 return a+i*3;
00180 }
00182 inline const S * operator [] ( const int i ) const
00183 {
00184 return a+i*3;
00185 }
00186
00187
00189 Matrix33 & operator += ( const Matrix33 &m )
00190 {
00191 for(int i=0;i<9;++i)
00192 a[i] += m.a[i];
00193 return *this;
00194 }
00195
00197 Matrix33 & operator += ( const Matrix33Diag<S> &p )
00198 {
00199 a[0] += p[0];
00200 a[4] += p[1];
00201 a[8] += p[2];
00202 return *this;
00203 }
00204
00206 Matrix33 & operator -= ( const Matrix33 &m )
00207 {
00208 for(int i=0;i<9;++i)
00209 a[i] -= m.a[i];
00210 return *this;
00211 }
00212
00214 Matrix33 & operator -= ( const Matrix33Diag<S> &p )
00215 {
00216 a[0] -= p[0];
00217 a[4] -= p[1];
00218 a[8] -= p[2];
00219 return *this;
00220 }
00221
00223 Matrix33 & operator /= ( const S &s )
00224 {
00225 for(int i=0;i<9;++i)
00226 a[i] /= s;
00227 return *this;
00228 }
00229
00230
00232 Matrix33 operator * ( const Matrix33< S> & t ) const
00233 {
00234 Matrix33<S> r;
00235
00236 int i,j;
00237 for(i=0;i<3;++i)
00238 for(j=0;j<3;++j)
00239 r[i][j] = (*this)[i][0]*t[0][j] + (*this)[i][1]*t[1][j] + (*this)[i][2]*t[2][j];
00240
00241 return r;
00242 }
00243
00245 void operator *=( const Matrix33< S> & t )
00246 {
00247 int i,j;
00248 for(i=0;i<3;++i)
00249 for(j=0;j<3;++j)
00250 (*this)[i][j] = (*this)[i][0]*t[0][j] + (*this)[i][1]*t[1][j] + (*this)[i][2]*t[2][j];
00251
00252 }
00253
00255 Matrix33 operator * ( const Matrix33Diag< S> & t ) const
00256 {
00257 Matrix33<S> r;
00258
00259 int i,j;
00260 for(i=0;i<3;++i)
00261 for(j=0;j<3;++j)
00262 r[i][j] = (*this)[i][j]*t[j];
00263
00264 return r;
00265 }
00266
00268 void operator *=( const Matrix33Diag< S> & t )
00269 {
00270 int i,j;
00271 for(i=0;i<3;++i)
00272 for(j=0;j<3;++j)
00273 (*this)[i][j] = (*this)[i][j]*t[j];
00274 }
00275
00277 Matrix33 & operator *= ( const S t )
00278 {
00279 for(int i=0;i<9;++i)
00280 a[i] *= t;
00281 return *this;
00282 }
00283
00285 Matrix33 operator * ( const S t ) const
00286 {
00287 Matrix33<S> r;
00288 for(int i=0;i<9;++i)
00289 r.a[i] = a[i]* t;
00290
00291 return r;
00292 }
00293
00295 Matrix33 operator - ( const Matrix33 &m ) const
00296 {
00297 Matrix33<S> r;
00298 for(int i=0;i<9;++i)
00299 r.a[i] = a[i] - m.a[i];
00300
00301 return r;
00302 }
00303
00305 Matrix33 operator - ( const Matrix33Diag<S> &p ) const
00306 {
00307 Matrix33<S> r=a;
00308 r[0][0] -= p[0];
00309 r[1][1] -= p[1];
00310 r[2][2] -= p[2];
00311 return r;
00312 }
00313
00315 Matrix33 operator + ( const Matrix33 &m ) const
00316 {
00317 Matrix33<S> r;
00318 for(int i=0;i<9;++i)
00319 r.a[i] = a[i] + m.a[i];
00320
00321 return r;
00322 }
00323
00325 Matrix33 operator + ( const Matrix33Diag<S> &p ) const
00326 {
00327 Matrix33<S> r=(*this);
00328 r[0][0] += p[0];
00329 r[1][1] += p[1];
00330 r[2][2] += p[2];
00331 return r;
00332 }
00333
00338 Point3<S> operator * ( const Point3<S> & v ) const
00339 {
00340 Point3<S> t;
00341
00342 t[0] = a[0]*v[0] + a[1]*v[1] + a[2]*v[2];
00343 t[1] = a[3]*v[0] + a[4]*v[1] + a[5]*v[2];
00344 t[2] = a[6]*v[0] + a[7]*v[1] + a[8]*v[2];
00345 return t;
00346 }
00347
00348 void OuterProduct(Point3<S> const &p0, Point3<S> const &p1) {
00349 Point3<S> row;
00350 row = p1*p0[0];
00351 a[0] = row[0];a[1] = row[1];a[2] = row[2];
00352 row = p1*p0[1];
00353 a[3] = row[0]; a[4] = row[1]; a[5] = row[2];
00354 row = p1*p0[2];
00355 a[6] = row[0];a[7] = row[1];a[8] = row[2];
00356 }
00357
00358 Matrix33 & SetZero() {
00359 for(int i=0;i<9;++i) a[i] =0;
00360 return (*this);
00361 }
00362 Matrix33 & SetIdentity() {
00363 for(int i=0;i<9;++i) a[i] =0;
00364 a[0]=a[4]=a[8]=1.0;
00365 return (*this);
00366 }
00367
00368 Matrix33 & SetRotateRad(S angle, const Point3<S> & axis )
00369 {
00370 S c = cos(angle);
00371 S s = sin(angle);
00372 S q = 1-c;
00373 Point3<S> t = axis;
00374 t.Normalize();
00375 a[0] = t[0]*t[0]*q + c;
00376 a[1] = t[0]*t[1]*q - t[2]*s;
00377 a[2] = t[0]*t[2]*q + t[1]*s;
00378 a[3] = t[1]*t[0]*q + t[2]*s;
00379 a[4] = t[1]*t[1]*q + c;
00380 a[5] = t[1]*t[2]*q - t[0]*s;
00381 a[6] = t[2]*t[0]*q -t[1]*s;
00382 a[7] = t[2]*t[1]*q +t[0]*s;
00383 a[8] = t[2]*t[2]*q +c;
00384 return (*this);
00385 }
00386 Matrix33 & SetRotateDeg(S angle, const Point3<S> & axis ){
00387 return SetRotateRad(math::ToRad(angle),axis);
00388 }
00389
00391 Matrix33 & Transpose()
00392 {
00393 math::Swap(a[1],a[3]);
00394 math::Swap(a[2],a[6]);
00395 math::Swap(a[5],a[7]);
00396 return *this;
00397 }
00398
00399
00400 Matrix33 transpose() const
00401 {
00402 Matrix33 res = *this;
00403 res.Transpose();
00404 return res;
00405 }
00406
00407 void transposeInPlace() { this->Transpose(); }
00408
00410 Matrix33 & SetDiagonal(S *v)
00411 {int i,j;
00412 for(i=0;i<3;i++)
00413 for(j=0;j<3;j++)
00414 if(i==j) (*this)[i][j] = v[i];
00415 else (*this)[i][j] = 0;
00416 return *this;
00417 }
00418
00419
00421 void SetColumn(const int n, S* v){
00422 assert( (n>=0) && (n<3) );
00423 a[n]=v[0]; a[n+3]=v[1]; a[n+6]=v[2];
00424 };
00425
00427 void SetRow(const int n, S* v){
00428 assert( (n>=0) && (n<3) );
00429 int m=n*3;
00430 a[m]=v[0]; a[m+1]=v[1]; a[m+2]=v[2];
00431 };
00432
00434 void SetColumn(const int n, const Point3<S> v){
00435 assert( (n>=0) && (n<3) );
00436 a[n]=v[0]; a[n+3]=v[1]; a[n+6]=v[2];
00437 };
00438
00440 void SetRow(const int n, const Point3<S> v){
00441 assert( (n>=0) && (n<3) );
00442 int m=n*3;
00443 a[m]=v[0]; a[m+1]=v[1]; a[m+2]=v[2];
00444 };
00445
00447 Point3<S> GetColumn(const int n) const {
00448 assert( (n>=0) && (n<3) );
00449 Point3<S> t;
00450 t[0]=a[n]; t[1]=a[n+3]; t[2]=a[n+6];
00451 return t;
00452 };
00453
00455 Point3<S> GetRow(const int n) const {
00456 assert( (n>=0) && (n<3) );
00457 Point3<S> t;
00458 int m=n*3;
00459 t[0]=a[m]; t[1]=a[m+1]; t[2]=a[m+2];
00460 return t;
00461 };
00462
00463
00464
00466 S Determinant() const
00467 {
00468 return a[0]*(a[4]*a[8]-a[5]*a[7]) -
00469 a[1]*(a[3]*a[8]-a[5]*a[6]) +
00470 a[2]*(a[3]*a[7]-a[4]*a[6]) ;
00471 }
00472
00473
00474
00475
00476 Matrix33 & FastInvert()
00477 {
00478
00479 S t4 = a[0]*a[4];
00480 S t6 = a[0]*a[5];
00481 S t8 = a[1]*a[3];
00482 S t10 = a[2]*a[3];
00483 S t12 = a[1]*a[6];
00484 S t14 = a[2]*a[6];
00485 S t17 = 1/(t4*a[8]-t6*a[7]-t8*a[8]+t10*a[7]+t12*a[5]-t14*a[4]);
00486 S a0 = a[0];
00487 S a1 = a[1];
00488 S a3 = a[3];
00489 S a4 = a[4];
00490 a[0] = (a[4]*a[8]-a[5]*a[7])*t17;
00491 a[1] = -(a[1]*a[8]-a[2]*a[7])*t17;
00492 a[2] = (a1 *a[5]-a[2]*a[4])*t17;
00493 a[3] = -(a[3]*a[8]-a[5]*a[6])*t17;
00494 a[4] = (a0 *a[8]-t14 )*t17;
00495 a[5] = -(t6 - t10)*t17;
00496 a[6] = (a3 *a[7]-a[4]*a[6])*t17;
00497 a[7] = -(a[0]*a[7]-t12)*t17;
00498 a[8] = (t4-t8)*t17;
00499
00500 return *this;
00501 }
00502
00503 void show(FILE * fp)
00504 {
00505 for(int i=0;i<3;++i)
00506 printf("| %g \t%g \t%g |\n",a[3*i+0],a[3*i+1],a[3*i+2]);
00507 }
00508
00509
00510 S Trace() const
00511 {
00512 return a[0]+a[4]+a[8];
00513 }
00514
00515
00516
00517
00518 void ExternalProduct(const Point3<S> &a, const Point3<S> &b)
00519 {
00520 for(int i=0;i<3;++i)
00521 for(int j=0;j<3;++j)
00522 (*this)[i][j] = a[i]*b[j];
00523 }
00524
00525
00526
00527 ScalarType Norm()
00528 {
00529 ScalarType SQsum=0;
00530 for(int i=0;i<3;++i)
00531 for(int j=0;j<3;++j)
00532 SQsum += a[i]*a[i];
00533 return (math::Sqrt(SQsum));
00534 }
00535
00536
00537
00538
00539
00540 template <class STLPOINTCONTAINER >
00541 void Covariance(const STLPOINTCONTAINER &points, Point3<S> &bp) {
00542 assert(!points.empty());
00543 typedef typename STLPOINTCONTAINER::const_iterator PointIte;
00544
00545 bp.SetZero();
00546 for( PointIte pi = points.begin(); pi != points.end(); ++pi) bp+= (*pi);
00547 bp/=points.size();
00548
00549 this->SetZero();
00550 vcg::Matrix33<ScalarType> A;
00551 for( PointIte pi = points.begin(); pi != points.end(); ++pi) {
00552 Point3<S> p = (*pi)-bp;
00553 A.OuterProduct(p,p);
00554 (*this)+= A;
00555 }
00556 }
00557
00558
00559
00560
00561
00562
00563
00564
00565
00566
00567
00568
00569
00570 template <class STLPOINTCONTAINER >
00571 void CrossCovariance(const STLPOINTCONTAINER &P, const STLPOINTCONTAINER &X,
00572 Point3<S> &bp, Point3<S> &bx)
00573 {
00574 SetZero();
00575 assert(P.size()==X.size());
00576 bx.SetZero();
00577 bp.SetZero();
00578 Matrix33<S> tmp;
00579 typename std::vector <Point3<S> >::const_iterator pi,xi;
00580 for(pi=P.begin(),xi=X.begin();pi!=P.end();++pi,++xi){
00581 bp+=*pi;
00582 bx+=*xi;
00583 tmp.ExternalProduct(*pi,*xi);
00584 (*this)+=tmp;
00585 }
00586 bp/=P.size();
00587 bx/=X.size();
00588 (*this)/=P.size();
00589 tmp.ExternalProduct(bp,bx);
00590 (*this)-=tmp;
00591 }
00592
00593 template <class STLPOINTCONTAINER, class STLREALCONTAINER>
00594 void WeightedCrossCovariance(const STLREALCONTAINER & weights,
00595 const STLPOINTCONTAINER &P,
00596 const STLPOINTCONTAINER &X,
00597 Point3<S> &bp,
00598 Point3<S> &bx)
00599 {
00600 SetZero();
00601 assert(P.size()==X.size());
00602 bx.SetZero();
00603 bp.SetZero();
00604 Matrix33<S> tmp;
00605 typename std::vector <Point3<S> >::const_iterator pi,xi;
00606 typename STLREALCONTAINER::const_iterator pw;
00607
00608 for(pi=P.begin(),xi=X.begin();pi!=P.end();++pi,++xi){
00609 bp+=(*pi);
00610 bx+=(*xi);
00611 }
00612 bp/=P.size();
00613 bx/=X.size();
00614
00615 for(pi=P.begin(),xi=X.begin(),pw = weights.begin();pi!=P.end();++pi,++xi,++pw){
00616
00617 tmp.ExternalProduct(((*pi)-(bp)),((*xi)-(bp)));
00618
00619 (*this)+=tmp*(*pw);
00620 }
00621 }
00622
00623 private:
00624 S a[9];
00625 };
00626
00627 template <class S>
00628 void Invert(Matrix33<S> &m)
00629 {
00630 Matrix33<S> v;
00631 Point3<typename Matrix33<S>::ScalarType> e;
00632 SingularValueDecomposition(m,&e[0],v);
00633 e[0]=1/e[0];e[1]=1/e[1];e[2]=1/e[2];
00634 m.Transpose();
00635 m = v * Matrix33Diag<S>(e) * m;
00636 }
00637
00638 template <class S>
00639 Matrix33<S> Inverse(const Matrix33<S>&m)
00640 {
00641 Matrix33<S> v,m_copy=m;
00642 Point3<S> e;
00643 SingularValueDecomposition(m_copy,&e[0],v);
00644 m_copy.Transpose();
00645 e[0]=1/e[0];e[1]=1/e[1];e[2]=1/e[2];
00646 return v * Matrix33Diag<S>(e) * m_copy;
00647 }
00648
00650 template <class S>
00651 Matrix33<S> RotationMatrix(vcg::Point3<S> v0,vcg::Point3<S> v1,bool normalized=true)
00652 {
00653 typedef typename vcg::Point3<S> CoordType;
00654 Matrix33<S> rotM;
00655 const S epsilon=0.00001;
00656 if (!normalized)
00657 {
00658 v0.Normalize();
00659 v1.Normalize();
00660 }
00661 S dot=(v0*v1);
00663 if (dot>((S)1-epsilon))
00664 {
00665 rotM.SetIdentity();
00666 return rotM;
00667 }
00668
00670 CoordType axis;
00671 axis=v0^v1;
00672 axis.Normalize();
00673
00675 S u=axis.X();
00676 S v=axis.Y();
00677 S w=axis.Z();
00678 S phi=acos(dot);
00679 S rcos = cos(phi);
00680 S rsin = sin(phi);
00681
00682 rotM[0][0] = rcos + u*u*(1-rcos);
00683 rotM[1][0] = w * rsin + v*u*(1-rcos);
00684 rotM[2][0] = -v * rsin + w*u*(1-rcos);
00685 rotM[0][1] = -w * rsin + u*v*(1-rcos);
00686 rotM[1][1] = rcos + v*v*(1-rcos);
00687 rotM[2][1] = u * rsin + w*v*(1-rcos);
00688 rotM[0][2] = v * rsin + u*w*(1-rcos);
00689 rotM[1][2] = -u * rsin + v*w*(1-rcos);
00690 rotM[2][2] = rcos + w*w*(1-rcos);
00691
00692 return rotM;
00693 }
00694
00696 template <class S>
00697 Matrix33<S> RotationMatrix(const vcg::Point3<S> &axis,
00698 const float &angleRad)
00699 {
00700 vcg::Matrix44<S> matr44;
00701 vcg::Matrix33<S> matr33;
00702 matr44.SetRotate(angleRad,axis);
00703 for (int i=0;i<3;i++)
00704 for (int j=0;j<3;j++)
00705 matr33[i][j]=matr44[i][j];
00706 return matr33;
00707 }
00708
00712 template <class S>
00713 Matrix33<S> RandomRotation(){
00714 S x1,x2,x3;
00715 Matrix33<S> R,H,M,vv;
00716 Point3<S> v;
00717 R.SetIdentity();
00718 H.SetIdentity();
00719 x1 = rand()/S(RAND_MAX);
00720 x2 = rand()/S(RAND_MAX);
00721 x3 = rand()/S(RAND_MAX);
00722
00723 R[0][0] = cos(S(2)*M_PI*x1);
00724 R[0][1] = sin(S(2)*M_PI*x1);
00725 R[1][0] = - R[0][1];
00726 R[1][1] = R[0][0];
00727
00728 v[0] = cos(2.0 * M_PI * x2)*sqrt(x3);
00729 v[1] = sin(2.0 * M_PI * x2)*sqrt(x3);
00730 v[2] = sqrt(1-x3);
00731
00732 vv.OuterProduct(v,v);
00733 H -= vv*S(2);
00734 M = H*R*S(-1);
00735 return M;
00736 }
00737
00739 typedef Matrix33<short> Matrix33s;
00740 typedef Matrix33<int> Matrix33i;
00741 typedef Matrix33<float> Matrix33f;
00742 typedef Matrix33<double> Matrix33d;
00743
00744 }
00745
00746 #endif
