typename PointMatcher<T>::TransformationParameters TransformationsImpl<T>::RigidTransformation::correctParameters(const TransformationParameters& parameters) const { TransformationParameters ortho = parameters; if(ortho.cols() == 4) { const Eigen::Matrix<T, 3, 1> col0 = parameters.block(0, 0, 3, 1).normalized(); const Eigen::Matrix<T, 3, 1> col1 = parameters.block(0, 1, 3, 1).normalized(); const Eigen::Matrix<T, 3, 1> col2 = parameters.block(0, 2, 3, 1).normalized(); ortho.block(0, 0, 3, 1) = col1.cross(col2); ortho.block(0, 1, 3, 1) = col2.cross(col0); ortho.block(0, 2, 3, 1) = col2; } else if(ortho.cols() == 3) { // R = [ a b] // [-b a] // mean of a and b T a = (parameters(0,0) + parameters(1,1))/2; T b = (-parameters(1,0) + parameters(0,1))/2; T sum = sqrt(pow(a,2) + pow(b,2)); a = a/sum; b = b/sum; ortho(0,0) = a; ortho(0,1) = b; ortho(1,0) = -b; ortho(1,1) = a; } return ortho; }
IGL_INLINE void igl::per_face_normals( const Eigen::MatrixBase<DerivedV>& V, const Eigen::MatrixBase<DerivedF>& F, const Eigen::MatrixBase<DerivedZ> & Z, Eigen::PlainObjectBase<DerivedN> & N) { N.resize(F.rows(),3); // loop over faces int Frows = F.rows(); #pragma omp parallel for if (Frows>10000) for(int i = 0; i < Frows;i++) { const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v1 = V.row(F(i,1)) - V.row(F(i,0)); const Eigen::Matrix<typename DerivedV::Scalar, 1, 3> v2 = V.row(F(i,2)) - V.row(F(i,0)); N.row(i) = v1.cross(v2);//.normalized(); typename DerivedV::Scalar r = N.row(i).norm(); if(r == 0) { N.row(i) = Z; }else { N.row(i) /= r; } } }
IGL_INLINE void igl::grad(const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &V, const Eigen::Matrix<S, Eigen::Dynamic, Eigen::Dynamic> &F, const Eigen::Matrix<T, Eigen::Dynamic, 1>&X, Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> &G ) { G = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>::Zero(F.rows(),3); for (int i = 0; i<F.rows(); ++i) { // renaming indices of vertices of triangles for convenience int i1 = F(i,0); int i2 = F(i,1); int i3 = F(i,2); // #F x 3 matrices of triangle edge vectors, named after opposite vertices Eigen::Matrix<T, 1, 3> v32 = V.row(i3) - V.row(i2); Eigen::Matrix<T, 1, 3> v13 = V.row(i1) - V.row(i3); Eigen::Matrix<T, 1, 3> v21 = V.row(i2) - V.row(i1); // area of parallelogram is twice area of triangle // area of parallelogram is || v1 x v2 || Eigen::Matrix<T, 1, 3> n = v32.cross(v13); // This does correct l2 norm of rows, so that it contains #F list of twice // triangle areas double dblA = std::sqrt(n.dot(n)); // now normalize normals to get unit normals Eigen::Matrix<T, 1, 3> u = n / dblA; // rotate each vector 90 degrees around normal double norm21 = std::sqrt(v21.dot(v21)); double norm13 = std::sqrt(v13.dot(v13)); Eigen::Matrix<T, 1, 3> eperp21 = u.cross(v21); eperp21 = eperp21 / std::sqrt(eperp21.dot(eperp21)); eperp21 *= norm21; Eigen::Matrix<T, 1, 3> eperp13 = u.cross(v13); eperp13 = eperp13 / std::sqrt(eperp13.dot(eperp13)); eperp13 *= norm13; G.row(i) = ((X[i2] -X[i1]) *eperp13 + (X[i3] - X[i1]) *eperp21) / dblA; }; }
Eigen::Matrix<double, 3, 3> getRotationFromA2B(Eigen::Matrix<double, 3, 1> A, Eigen::Matrix<double, 3, 1> B) { Eigen::Matrix<double, 3, 1> v = A.cross(B); double s = v.norm(); double c = A.dot(B); Eigen::Matrix<double, 3, 3> vx; vx << 0, -v[2], v[1], v[2], 0, -v[0], -v[1], v[0], 0; return Eigen::Matrix<double, 3, 3>::Identity() + vx + ((1.0-c)/(s*s))*vx*vx; }
// returns the 90 deg rotation of a (around n) most similar to target b /// a and b should be in the same plane orthogonal to N static inline Eigen::Matrix<typename DerivedV::Scalar, 3, 1> K_PI_new(const Eigen::Matrix<typename DerivedV::Scalar, 3, 1>& a, const Eigen::Matrix<typename DerivedV::Scalar, 3, 1>& b, const Eigen::Matrix<typename DerivedV::Scalar, 3, 1>& n) { Eigen::Matrix<typename DerivedV::Scalar, 3, 1> c = (a.cross(n)).normalized(); typename DerivedV::Scalar scorea = a.dot(b); typename DerivedV::Scalar scorec = c.dot(b); if (fabs(scorea)>=fabs(scorec)) return a*Sign(scorea); else return c*Sign(scorec); }
/// /// @par Detailed description /// ... /// @param [in, out] (param1) ... /// @return ... /// @note ... float sasmath:: signed_angle(Eigen::Matrix<float,3,1> &coor1, Eigen::Matrix<float,3,1> &coor2, Eigen::Matrix<float,3,1> &coor3) { /* This method calcultes the sign of the angle which is used in the calculation of a dihedral angle. As such, this method is not validated for other uses. It will fail if the basis atoms for the dihedral (atom 2 and atom 3) overlap. */ float sa = 180.0 ; float angle ; float pi = acos(-1.0) ; float ada = coor1.dot(coor1) ; float bdb = coor2.dot(coor2) ; if(ada*bdb <= 0.0) { std::cout << "; " ; return sa ; } else { float adb = coor1.dot(coor2) ; float argument = adb/sqrt(ada*bdb) ; angle = (180.0/pi) * acos(argument) ; } Eigen::Matrix<float,3,1> cp ; cp = coor1.cross(coor2) ; float dp = coor3.dot(cp) ; float sign = 0.0 ; if(dp < 0.0) { sign = -1.0 ; } else if(dp > 0.0) { sign = 1.0 ; } sa = sign*angle ; return sa ; }
IGL_INLINE Eigen::Matrix<Scalar, 3, 3> igl::rotation_matrix_from_directions( const Eigen::Matrix<Scalar, 3, 1> v0, const Eigen::Matrix<Scalar, 3, 1> v1) { Eigen::Matrix<Scalar, 3, 3> rotM; const double epsilon=1e-8; Scalar dot=v0.normalized().dot(v1.normalized()); ///control if there is no rotation if ((v0-v1).norm()<epsilon) { rotM = Eigen::Matrix<Scalar, 3, 3>::Identity(); return rotM; } if ((v0+v1).norm()<epsilon) { rotM = -Eigen::Matrix<Scalar, 3, 3>::Identity(); rotM(0,0) = 1.; std::cerr<<"igl::rotation_matrix_from_directions: rotating around x axis by 180o"<<std::endl; return rotM; } ///find the axis of rotation Eigen::Matrix<Scalar, 3, 1> axis; axis=v0.cross(v1); axis.normalize(); ///construct rotation matrix Scalar u=axis(0); Scalar v=axis(1); Scalar w=axis(2); Scalar phi=acos(dot); Scalar rcos = cos(phi); Scalar rsin = sin(phi); rotM(0,0) = rcos + u*u*(1-rcos); rotM(1,0) = w * rsin + v*u*(1-rcos); rotM(2,0) = -v * rsin + w*u*(1-rcos); rotM(0,1) = -w * rsin + u*v*(1-rcos); rotM(1,1) = rcos + v*v*(1-rcos); rotM(2,1) = u * rsin + w*v*(1-rcos); rotM(0,2) = v * rsin + u*w*(1-rcos); rotM(1,2) = -u * rsin + v*w*(1-rcos); rotM(2,2) = rcos + w*w*(1-rcos); return rotM; }