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
00125
00126
00127
00138
00139 template <class STLPOINTCONTAINER >
00140 void CrossCovariance(const STLPOINTCONTAINER &P, const STLPOINTCONTAINER &X,
00141 Point3<Scalar> &bp, Point3<Scalar> &bx)
00142 {
00143 setZero();
00144 assert(P.size()==X.size());
00145 bx.setZero();
00146 bp.setZero();
00147 Matrix33<Scalar> tmp;
00148 typename std::vector <Point3<Scalar> >::const_iterator pi,xi;
00149 for(pi=P.begin(),xi=X.begin();pi!=P.end();++pi,++xi){
00150 bp+=*pi;
00151 bx+=*xi;
00152 tmp.ExternalProduct(*pi,*xi);
00153 (*this)+=tmp;
00154 }
00155 bp/=P.size();
00156 bx/=X.size();
00157 (*this)/=P.size();
00158 tmp.ExternalProduct(bp,bx);
00159 (*this)-=tmp;
00160 }
00161
00162 template <class STLPOINTCONTAINER, class STLREALCONTAINER>
00163 void WeightedCrossCovariance(const STLREALCONTAINER & weights,
00164 const STLPOINTCONTAINER &P,
00165 const STLPOINTCONTAINER &X,
00166 Point3<Scalar> &bp,
00167 Point3<Scalar> &bx)
00168 {
00169 setZero();
00170 assert(P.size()==X.size());
00171 bx.SetZero();
00172 bp.SetZero();
00173 Matrix33<Scalar> tmp;
00174 typename std::vector <Point3<Scalar> >::const_iterator pi,xi;
00175 typename STLREALCONTAINER::const_iterator pw;
00176
00177 for(pi=P.begin(),xi=X.begin();pi!=P.end();++pi,++xi){
00178 bp+=(*pi);
00179 bx+=(*xi);
00180 }
00181 bp/=P.size();
00182 bx/=X.size();
00183
00184 for(pi=P.begin(),xi=X.begin(),pw = weights.begin();pi!=P.end();++pi,++xi,++pw){
00185
00186 tmp.ExternalProduct(((*pi)-(bp)),((*xi)-(bp)));
00187
00188 (*this)+=tmp*(*pw);
00189 }
00190 }
00191 };
00192
00193 template <class S>
00194 void Invert(Matrix33<S> &m) { m = m.lu().inverse(); }
00195
00196 template <class S>
00197 Matrix33<S> Inverse(const Matrix33<S>&m) { return m.lu().inverse(); }
00198
00200 template <class S>
00201 Matrix33<S> RotationMatrix(vcg::Point3<S> v0,vcg::Point3<S> v1,bool normalized=true)
00202 {
00203 typedef typename vcg::Point3<S> CoordType;
00204 Matrix33<S> rotM;
00205 const S epsilon=0.00001;
00206 if (!normalized)
00207 {
00208 v0.Normalize();
00209 v1.Normalize();
00210 }
00211 S dot=v0.dot(v1);
00213 if (dot>((S)1-epsilon))
00214 {
00215 rotM.SetIdentity();
00216 return rotM;
00217 }
00218
00220 CoordType axis;
00221 axis=v0^v1;
00222 axis.Normalize();
00223
00225 S u=axis.X();
00226 S v=axis.Y();
00227 S w=axis.Z();
00228 S phi=acos(dot);
00229 S rcos = cos(phi);
00230 S rsin = sin(phi);
00231
00232 rotM[0][0] = rcos + u*u*(1-rcos);
00233 rotM[1][0] = w * rsin + v*u*(1-rcos);
00234 rotM[2][0] = -v * rsin + w*u*(1-rcos);
00235 rotM[0][1] = -w * rsin + u*v*(1-rcos);
00236 rotM[1][1] = rcos + v*v*(1-rcos);
00237 rotM[2][1] = u * rsin + w*v*(1-rcos);
00238 rotM[0][2] = v * rsin + u*w*(1-rcos);
00239 rotM[1][2] = -u * rsin + v*w*(1-rcos);
00240 rotM[2][2] = rcos + w*w*(1-rcos);
00241
00242 return rotM;
00243 }
00244
00246 template <class S>
00247 Matrix33<S> RotationMatrix(const vcg::Point3<S> &axis,
00248 const float &angleRad)
00249 {
00250 vcg::Matrix44<S> matr44;
00251 vcg::Matrix33<S> matr33;
00252 matr44.SetRotate(angleRad,axis);
00253 for (int i=0;i<3;i++)
00254 for (int j=0;j<3;j++)
00255 matr33[i][j]=matr44[i][j];
00256 return matr33;
00257 }
00258
00262 template <class S>
00263 Matrix33<S> RandomRotation(){
00264 S x1,x2,x3;
00265 Matrix33<S> R,H,M,vv;
00266 Point3<S> v;
00267 R.SetIdentity();
00268 H.SetIdentity();
00269 x1 = rand()/S(RAND_MAX);
00270 x2 = rand()/S(RAND_MAX);
00271 x3 = rand()/S(RAND_MAX);
00272
00273 R[0][0] = cos(S(2)*M_PI*x1);
00274 R[0][1] = sin(S(2)*M_PI*x1);
00275 R[1][0] = - R[0][1];
00276 R[1][1] = R[0][0];
00277
00278 v[0] = cos(2.0 * M_PI * x2)*sqrt(x3);
00279 v[1] = sin(2.0 * M_PI * x2)*sqrt(x3);
00280 v[2] = sqrt(1-x3);
00281
00282 vv.OuterProduct(v,v);
00283 H -= vv*S(2);
00284 M = H*R*S(-1);
00285 return M;
00286 }
00287
00289 typedef Matrix33<short> Matrix33s;
00290 typedef Matrix33<int> Matrix33i;
00291 typedef Matrix33<float> Matrix33f;
00292 typedef Matrix33<double> Matrix33d;
00293
00294 }
00295
00296 #endif
00297
00298 #endif