00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024 #ifndef VCG_USE_EIGEN
00025 #include "deprecated_matrix33.h"
00026 #else
00027
00028 #ifndef __VCGLIB_MATRIX33_H
00029 #define __VCGLIB_MATRIX33_H
00030
00031 #include "eigen.h"
00032 #include "matrix44.h"
00033
00034 namespace vcg{
00035 template<class Scalar> class Matrix33;
00036 }
00037
00038 namespace Eigen{
00039 template<typename Scalar>
00040 struct ei_traits<vcg::Matrix33<Scalar> > : ei_traits<Eigen::Matrix<Scalar,3,3,RowMajor> > {};
00041 template<typename XprType> struct ei_to_vcgtype<XprType,3,3,RowMajor,3,3>
00042 { typedef vcg::Matrix33<typename XprType::Scalar> type; };
00043 }
00044
00045 namespace vcg {
00046
00053 template<class _Scalar>
00054 class Matrix33 : public Eigen::Matrix<_Scalar,3,3,Eigen::RowMajor>
00055 {
00056
00057 typedef Eigen::Matrix<_Scalar,3,3,Eigen::RowMajor> _Base;
00058 public:
00059
00060 using _Base::coeff;
00061 using _Base::coeffRef;
00062 using _Base::setZero;
00063
00064 _EIGEN_GENERIC_PUBLIC_INTERFACE(Matrix33,_Base);
00065 typedef _Scalar ScalarType;
00066
00067 VCG_EIGEN_INHERIT_ASSIGNMENT_OPERATORS(Matrix33)
00068
00069
00070 inline Matrix33() : Base() {}
00071
00073 Matrix33(const Matrix33& m ) : Base(m) {}
00074
00076 Matrix33(const Scalar * v ) : Base(Eigen::Map<Eigen::Matrix<Scalar,3,3,Eigen::RowMajor> >(v)) {}
00077
00079 Matrix33(const Matrix44<Scalar> & m, const int & k) : Base(m.minor(k,k)) {}
00080
00081 template<typename OtherDerived>
00082 Matrix33(const Eigen::MatrixBase<OtherDerived>& other) : Base(other) {}
00083
00085 inline typename Base::RowXpr operator[](const unsigned int i)
00086 { return Base::row(i); }
00087
00089 inline const typename Base::RowXpr operator[](const unsigned int i) const
00090 { return Base::row(i); }
00091
00093 Matrix33 & SetRotateRad(Scalar angle, const Point3<Scalar> & axis )
00094 {
00095 *this = Eigen::AngleAxis<Scalar>(angle,axis).toRotationMatrix();
00096 return (*this);
00097 }
00099 Matrix33 & SetRotateDeg(Scalar angle, const Point3<Scalar> & axis ){
00100 return SetRotateRad(math::ToRad(angle),axis);
00101 }
00102
00103
00104
00106 Matrix33 & FastInvert() { return *this = Base::inverse(); }
00107
00108 void show(FILE * fp)
00109 {
00110 for(int i=0;i<3;++i)
00111 printf("| %g \t%g \t%g |\n",coeff(i,0),coeff(i,1),coeff(i,2));
00112 }
00113
00117
00118 void ExternalProduct(const Point3<Scalar> &a, const Point3<Scalar> &b) { *this = a * b.transpose(); }
00119
00121 Scalar Norm() { return Base::cwise().abs2().sum(); }
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00134
00135 template <class STLPOINTCONTAINER >
00136 void Covariance(const STLPOINTCONTAINER &points, Point3<Scalar> &bp) {
00137 assert(!points.empty());
00138 typedef typename STLPOINTCONTAINER::const_iterator PointIte;
00139
00140 bp.setZero();
00141 for( PointIte pi = points.begin(); pi != points.end(); ++pi) bp+= (*pi);
00142 bp/=points.size();
00143
00144 this->setZero();
00145 vcg::Matrix33<ScalarType> A;
00146 for( PointIte pi = points.begin(); pi != points.end(); ++pi) {
00147 Point3<Scalar> p = (*pi)-bp;
00148 A.OuterProduct(p,p);
00149 (*this)+= A;
00150 }
00151 }
00152
00163
00164 template <class STLPOINTCONTAINER >
00165 void CrossCovariance(const STLPOINTCONTAINER &P, const STLPOINTCONTAINER &X,
00166 Point3<Scalar> &bp, Point3<Scalar> &bx)
00167 {
00168 setZero();
00169 assert(P.size()==X.size());
00170 bx.setZero();
00171 bp.setZero();
00172 Matrix33<Scalar> tmp;
00173 typename std::vector <Point3<Scalar> >::const_iterator pi,xi;
00174 for(pi=P.begin(),xi=X.begin();pi!=P.end();++pi,++xi){
00175 bp+=*pi;
00176 bx+=*xi;
00177 tmp.ExternalProduct(*pi,*xi);
00178 (*this)+=tmp;
00179 }
00180 bp/=P.size();
00181 bx/=X.size();
00182 (*this)/=P.size();
00183 tmp.ExternalProduct(bp,bx);
00184 (*this)-=tmp;
00185 }
00186
00187 template <class STLPOINTCONTAINER, class STLREALCONTAINER>
00188 void WeightedCrossCovariance(const STLREALCONTAINER & weights,
00189 const STLPOINTCONTAINER &P,
00190 const STLPOINTCONTAINER &X,
00191 Point3<Scalar> &bp,
00192 Point3<Scalar> &bx)
00193 {
00194 setZero();
00195 assert(P.size()==X.size());
00196 bx.SetZero();
00197 bp.SetZero();
00198 Matrix33<Scalar> tmp;
00199 typename std::vector <Point3<Scalar> >::const_iterator pi,xi;
00200 typename STLREALCONTAINER::const_iterator pw;
00201
00202 for(pi=P.begin(),xi=X.begin();pi!=P.end();++pi,++xi){
00203 bp+=(*pi);
00204 bx+=(*xi);
00205 }
00206 bp/=P.size();
00207 bx/=X.size();
00208
00209 for(pi=P.begin(),xi=X.begin(),pw = weights.begin();pi!=P.end();++pi,++xi,++pw){
00210
00211 tmp.ExternalProduct(((*pi)-(bp)),((*xi)-(bp)));
00212
00213 (*this)+=tmp*(*pw);
00214 }
00215 }
00216 };
00217
00218 template <class S>
00219 void Invert(Matrix33<S> &m) { m = m.lu().inverse(); }
00220
00221 template <class S>
00222 Matrix33<S> Inverse(const Matrix33<S>&m) { return m.lu().inverse(); }
00223
00225 template <class S>
00226 Matrix33<S> RotationMatrix(vcg::Point3<S> v0,vcg::Point3<S> v1,bool normalized=true)
00227 {
00228 typedef typename vcg::Point3<S> CoordType;
00229 Matrix33<S> rotM;
00230 const S epsilon=0.00001;
00231 if (!normalized)
00232 {
00233 v0.Normalize();
00234 v1.Normalize();
00235 }
00236 S dot=v0.dot(v1);
00238 if (dot>((S)1-epsilon))
00239 {
00240 rotM.SetIdentity();
00241 return rotM;
00242 }
00243
00245 CoordType axis;
00246 axis=v0^v1;
00247 axis.Normalize();
00248
00250 S u=axis.X();
00251 S v=axis.Y();
00252 S w=axis.Z();
00253 S phi=acos(dot);
00254 S rcos = cos(phi);
00255 S rsin = sin(phi);
00256
00257 rotM[0][0] = rcos + u*u*(1-rcos);
00258 rotM[1][0] = w * rsin + v*u*(1-rcos);
00259 rotM[2][0] = -v * rsin + w*u*(1-rcos);
00260 rotM[0][1] = -w * rsin + u*v*(1-rcos);
00261 rotM[1][1] = rcos + v*v*(1-rcos);
00262 rotM[2][1] = u * rsin + w*v*(1-rcos);
00263 rotM[0][2] = v * rsin + u*w*(1-rcos);
00264 rotM[1][2] = -u * rsin + v*w*(1-rcos);
00265 rotM[2][2] = rcos + w*w*(1-rcos);
00266
00267 return rotM;
00268 }
00269
00271 template <class S>
00272 Matrix33<S> RotationMatrix(const vcg::Point3<S> &axis,
00273 const float &angleRad)
00274 {
00275 vcg::Matrix44<S> matr44;
00276 vcg::Matrix33<S> matr33;
00277 matr44.SetRotate(angleRad,axis);
00278 for (int i=0;i<3;i++)
00279 for (int j=0;j<3;j++)
00280 matr33[i][j]=matr44[i][j];
00281 return matr33;
00282 }
00283
00287 template <class S>
00288 Matrix33<S> RandomRotation(){
00289 S x1,x2,x3;
00290 Matrix33<S> R,H,M,vv;
00291 Point3<S> v;
00292 R.SetIdentity();
00293 H.SetIdentity();
00294 x1 = rand()/S(RAND_MAX);
00295 x2 = rand()/S(RAND_MAX);
00296 x3 = rand()/S(RAND_MAX);
00297
00298 R[0][0] = cos(S(2)*M_PI*x1);
00299 R[0][1] = sin(S(2)*M_PI*x1);
00300 R[1][0] = - R[0][1];
00301 R[1][1] = R[0][0];
00302
00303 v[0] = cos(2.0 * M_PI * x2)*sqrt(x3);
00304 v[1] = sin(2.0 * M_PI * x2)*sqrt(x3);
00305 v[2] = sqrt(1-x3);
00306
00307 vv.OuterProduct(v,v);
00308 H -= vv*S(2);
00309 M = H*R*S(-1);
00310 return M;
00311 }
00312
00314 typedef Matrix33<short> Matrix33s;
00315 typedef Matrix33<int> Matrix33i;
00316 typedef Matrix33<float> Matrix33f;
00317 typedef Matrix33<double> Matrix33d;
00318
00319 }
00320
00321 #endif
00322
00323 #endif
