devtools/eigen
1
0
Fork 0
mirror of https://gitlab.com/libeigen/eigen.git synced 2026-04-10 11:34:33 +08:00
Code Issues Packages Projects Releases Wiki Activity

Geometry/EulerAngles: make sure that returned solution has canonical ranges

Browse Source
This commit is contained in:
Juraj Oršulić
2023-04-19 19:12:24 +00:00
committed by Rasmus Munk Larsen
parent a347dbbab2
commit 7f06bcae2c
3 changed files with 70 additions and 21 deletions
Download Patch File Download Diff File Expand all files Collapse all files

2
Eigen/src/Core/MatrixBase.h
View File

@@ -400,7 +400,7 @@ template<typename Derived> class MatrixBase
inline PlainObject unitOrthogonal(void) const;
EIGEN_DEVICE_FUNC
inline Matrix<Scalar,3,1> eulerAngles(Index a0, Index a1, Index a2) const;
inline Matrix<Scalar,3,1> eulerAngles(Index a0, Index a1, Index a2, bool canonical = true) const;
// put this as separate enum value to work around possible GCC 4.3 bug (?)
enum { HomogeneousReturnTypeDirection = ColsAtCompileTime==1&&RowsAtCompileTime==1 ? ((internal::traits<Derived>::Flags&RowMajorBit)==RowMajorBit ? Horizontal : Vertical)

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

@@ -30,13 +30,19 @@ namespace Eigen {
* * AngleAxisf(ea[2], Vector3f::UnitZ()); \endcode
* This corresponds to the right-multiply conventions (with right hand side frames).
*
* The returned angles are in the ranges [0:pi]x[-pi:pi]x[-pi:pi].
* When canonical == true (the default):
* For Tait-Bryan angle configurations (a0 != a2), the returned angles are in the ranges [-pi:pi]x[-pi/2:pi/2]x[-pi:pi].
* For proper Euler angle configurations (a0 == a2), the returned angles are in the ranges [-pi:pi]x[0:pi]x[-pi:pi].
*
* When canonical == false:
* The returned angles follow a non-standard range convention used by legacy versions of Eigen, [0:pi]x[-pi:pi]x[-pi:pi].
* Set canonical to false to retain legacy behaviour.
*
* \sa class AngleAxis
*/
template<typename Derived>
EIGEN_DEVICE_FUNC inline Matrix<typename MatrixBase<Derived>::Scalar,3,1>
MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const
MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2, bool canonical) const
{
EIGEN_USING_STD(atan2)
EIGEN_USING_STD(sin)
@@ -107,6 +113,24 @@ MatrixBase<Derived>::eulerAngles(Index a0, Index a1, Index a2) const
}
if (!odd)
res = -res;
if (canonical)
{
// If Tait-Bryan angles, make sure that the result is in the canonical range (middle axis angle in [-pi/2, pi/2]).
if (a0 != a2 && res.cwiseAbs()[1] > Scalar(EIGEN_PI / 2))
{
res -= Scalar(EIGEN_PI) * res.cwiseSign();
res[1] = -res[1];
}
// If proper Euler angles, make sure that the result is in the canonical range (middle axis angle in [0, pi]).
if (a0 == a2 && res[1] < Scalar(0))
{
res[0] -= Scalar(EIGEN_PI) * res.cwiseSign()[0];
res[2] -= Scalar(EIGEN_PI) * res.cwiseSign()[2];
res[1] = -res[1];
}
}
return res;
}