53 PONCA_MULTIARCH_STD_MATH(sqrt);
59 VectorType barycenter = Base::barycenterLocal();
60 VectorArray dBarycenter = Base::barycenterDerivatives();
63 Scalar epsilon =
Scalar(2) * Eigen::NumTraits<Scalar>::epsilon();
64 Scalar consider_as_zero =
Scalar(2) * std::numeric_limits<Scalar>::denorm_min();
68 Eigen::Matrix<Scalar,2,1> shifted_eivals = Base::m_solver.eigenvalues().template tail<2>().array() - Base::m_solver.eigenvalues()(0);
69 if(shifted_eivals(0) < consider_as_zero || shifted_eivals(0) < epsilon * shifted_eivals(1)) shifted_eivals(0) = 0;
70 if(shifted_eivals(1) < consider_as_zero) shifted_eivals(1) = 0;
73 for(
int k=0; k<Base::NbDerivatives; ++k)
84 Eigen::Matrix<Scalar,2,1> z = - Base::m_solver.eigenvectors().template rightCols<2>().transpose() * (Base::m_dCov[k] * normal);
85 if(shifted_eivals(0)>0) z(0) /= shifted_eivals(0);
86 if(shifted_eivals(1)>0) z(1) /= shifted_eivals(1);
87 m_dNormal.col(k) = Base::m_solver.eigenvectors().template rightCols<2>() * z;
90 if(k>0 || !Base::isScaleDer())
91 dDiff(Base::isScaleDer() ? k-1 : k) += 1;
92 m_dDist(k) = m_dNormal.col(k).dot(barycenter) + normal.dot(dDiff);
99 return Base::m_eCurrentState;