GitHub-Proxy/eigen
1
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2025-08-10 18:59:01 +08:00
Code Issues Packages Projects Releases Wiki Activity

fix unitOrthogonal() for size > 3

Browse Source
This commit is contained in:
Gael Guennebaud 2009-02-25 09:23:09 +00:00
parent 2d6d14a3d3
commit 91af389a9a
1 changed files with 24 additions and 4 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

28
Eigen/src/Geometry/OrthoMethods.h
View File

@ -92,9 +92,32 @@ struct ei_unitOrthogonal_selector
typedef typename ei_plain_matrix_type<Derived>::type VectorType; typedef typename ei_plain_matrix_type<Derived>::type VectorType;
typedef typename ei_traits<Derived>::Scalar Scalar; typedef typename ei_traits<Derived>::Scalar Scalar;
typedef typename NumTraits<Scalar>::Real RealScalar; typedef typename NumTraits<Scalar>::Real RealScalar;
typedef Matrix<Scalar,2,1> Vector2;
inline static VectorType run(const Derived& src) inline static VectorType run(const Derived& src)
{ {
VectorType perp(src.size()); VectorType perp = VectorType::Zero(src.size());
int maxi = 0;
int sndi = 0;
src.cwise().abs().maxCoeff(&maxi);
if (maxi==0)
sndi = 1;
RealScalar invnm = RealScalar(1)/Vector2(src.coeff(sndi),src.coeff(maxi)).norm();
perp.coeffRef(maxi) = -ei_conj(src.coeff(sndi)) * invnm;
perp.coeffRef(sndi) = ei_conj(src.coeff(maxi)) * invnm;
return perp;
}
};
template<typename Derived>
struct ei_unitOrthogonal_selector<Derived,3>
{
typedef typename ei_plain_matrix_type<Derived>::type VectorType;
typedef typename ei_traits<Derived>::Scalar Scalar;
typedef typename NumTraits<Scalar>::Real RealScalar;
inline static VectorType run(const Derived& src)
{
VectorType perp;
/* Let us compute the crossed product of *this with a vector /* Let us compute the crossed product of *this with a vector
* that is not too close to being colinear to *this. * that is not too close to being colinear to *this.
*/ */
@ -121,9 +144,6 @@ struct ei_unitOrthogonal_selector
perp.coeffRef(1) = -ei_conj(src.z())*invnm; perp.coeffRef(1) = -ei_conj(src.z())*invnm;
perp.coeffRef(2) = ei_conj(src.y())*invnm; perp.coeffRef(2) = ei_conj(src.y())*invnm;
} }
if( (Derived::SizeAtCompileTime!=Dynamic && Derived::SizeAtCompileTime>3)
|| (Derived::SizeAtCompileTime==Dynamic && src.size()>3) )
perp.end(src.size()-3).setZero();
return perp; return perp;
} }