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

Fix calc bug, docs and better testing.

Browse Source 
Test code changes:
* better coded
* rand and manual numbers
* singularity checking
This commit is contained in:
Tal Hadad
2016-10-16 14:39:26 +03:00
parent 078a202621
commit 58f5d7d058
3 changed files with 164 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files

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

@@ -36,7 +36,7 @@ namespace Eigen
* ### Rotation representation and conversions ###
*
* It has been proved(see Wikipedia link below) that every rotation can be represented
* by Euler angles, but there is no singular representation (e.g. unlike rotation matrices).
* by Euler angles, but there is no single representation (e.g. unlike rotation matrices).
* Therefore, you can convert from Eigen rotation and to them
* (including rotation matrices, which is not called "rotations" by Eigen design).
*
@@ -55,10 +55,12 @@ namespace Eigen
* Additionally, some axes related computation is done in compile time.
*
* #### Euler angles ranges in conversions ####
* Rotations representation as EulerAngles are not singular (unlike matrices), and even have infinite EulerAngles representations.<BR>
* Rotations representation as EulerAngles are not single (unlike matrices),
* and even have infinite EulerAngles representations.<BR>
* For example, add or subtract 2*PI from either angle of EulerAngles
* and you'll get the same rotation.
* This is the reason for infinite representation, but it's not the only reason for non-singularity.
* This is the general reason for infinite representation,
* but it's not the only general reason for not having a single representation.
*
* When converting rotation to EulerAngles, this class convert it to specific ranges
* When converting some rotation to EulerAngles, the rules for ranges are as follow:
@@ -66,10 +68,10 @@ namespace Eigen
* (even when it represented as RotationBase explicitly), angles ranges are __undefined__.
* - otherwise, Alpha and Gamma angles will be in the range [-PI, PI].<BR>
* As for Beta angle:
* - If the system is Tait-Bryan, the beta angle will be in the range [-PI, PI].
* - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2].
* - otherwise:
* - If the beta axis is positive, the beta angle will be in the range [0, 2*PI]
* - If the beta axis is negative, the beta angle will be in the range [-2*PI, 0]
* - If the beta axis is positive, the beta angle will be in the range [0, PI]
* - If the beta axis is negative, the beta angle will be in the range [-PI, 0]
*
* \sa EulerAngles(const MatrixBase<Derived>&)
* \sa EulerAngles(const RotationBase<Derived, 3>&)
@@ -95,7 +97,7 @@ namespace Eigen
*
* More information about Euler angles: https://en.wikipedia.org/wiki/Euler_angles
*
* \tparam _Scalar the scalar type, i.e., the type of the angles.
* \tparam _Scalar the scalar type, i.e. the type of the angles.
*
* \tparam _System the EulerSystem to use, which represents the axes of rotation.
*/
@@ -146,10 +148,10 @@ namespace Eigen
*
* \note Alpha and Gamma angles will be in the range [-PI, PI].<BR>
* As for Beta angle:
* - If the system is Tait-Bryan, the beta angle will be in the range [-PI, PI].
* - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2].
* - otherwise:
* - If the beta axis is positive, the beta angle will be in the range [0, 2*PI]
* - If the beta axis is negative, the beta angle will be in the range [-2*PI, 0]
* - If the beta axis is positive, the beta angle will be in the range [0, PI]
* - If the beta axis is negative, the beta angle will be in the range [-PI, 0]
*/
template<typename Derived>
EulerAngles(const MatrixBase<Derived>& m) { System::CalcEulerAngles(*this, m); }
@@ -160,10 +162,10 @@ namespace Eigen
* angles ranges are __undefined__.
* Otherwise, Alpha and Gamma angles will be in the range [-PI, PI].<BR>
* As for Beta angle:
* - If the system is Tait-Bryan, the beta angle will be in the range [-PI, PI].
* - If the system is Tait-Bryan, the beta angle will be in the range [-PI/2, PI/2].
* - otherwise:
* - If the beta axis is positive, the beta angle will be in the range [0, 2*PI]
* - If the beta axis is negative, the beta angle will be in the range [-2*PI, 0]
* - If the beta axis is positive, the beta angle will be in the range [0, PI]
* - If the beta axis is negative, the beta angle will be in the range [-PI, 0]
*/
template<typename Derived>
EulerAngles(const RotationBase<Derived, 3>& rot) { System::CalcEulerAngles(*this, rot.toRotationMatrix()); }

36
unsupported/Eigen/src/EulerAngles/EulerSystem.h
View File

@@ -18,7 +18,7 @@ namespace Eigen
namespace internal
{
// TODO: Check if already exists on the rest API
// TODO: Add this trait to the Eigen internal API?
template <int Num, bool IsPositive = (Num > 0)>
struct Abs
{
@@ -186,25 +186,25 @@ namespace Eigen
typedef typename Derived::Scalar Scalar;
Scalar plusMinus = IsEven? 1 : -1;
Scalar minusPlus = IsOdd? 1 : -1;
const Scalar plusMinus = IsEven? 1 : -1;
const Scalar minusPlus = IsOdd? 1 : -1;
Scalar Rsum = sqrt((mat(I,I) * mat(I,I) + mat(I,J) * mat(I,J) + mat(J,K) * mat(J,K) + mat(K,K) * mat(K,K))/2);
const Scalar Rsum = sqrt((mat(I,I) * mat(I,I) + mat(I,J) * mat(I,J) + mat(J,K) * mat(J,K) + mat(K,K) * mat(K,K))/2);
res[1] = atan2(plusMinus * mat(I,K), Rsum);
// There is a singularity when cos(beta) = 0
if(Rsum > 4 * NumTraits<Scalar>::epsilon()) {
// There is a singularity when cos(beta) == 0
if(Rsum > 4 * NumTraits<Scalar>::epsilon()) {// cos(beta) != 0
res[0] = atan2(minusPlus * mat(J, K), mat(K, K));
res[2] = atan2(minusPlus * mat(I, J), mat(I, I));
}
else if(plusMinus * mat(I, K) > 0) {
Scalar spos = mat(J, I) + plusMinus * mat(K, J); // 2*sin(alpha + plusMinus * gamma)
Scalar cpos = mat(J, J) + minusPlus * mat(K, I); // 2*cos(alpha + plusMinus * gamma);
else if(plusMinus * mat(I, K) > 0) {// cos(beta) == 0 and sin(beta) == 1
Scalar spos = mat(J, I) + plusMinus * mat(K, J); // 2*sin(alpha + plusMinus * gamma
Scalar cpos = mat(J, J) + minusPlus * mat(K, I); // 2*cos(alpha + plusMinus * gamma)
Scalar alphaPlusMinusGamma = atan2(spos, cpos);
res[0] = alphaPlusMinusGamma;
res[2] = 0;
}
else {
else {// cos(beta) == 0 and sin(beta) == -1
Scalar sneg = plusMinus * (mat(K, J) + minusPlus * mat(J, I)); // 2*sin(alpha + minusPlus*gamma)
Scalar cneg = mat(J, J) + plusMinus * mat(K, I); // 2*cos(alpha + minusPlus*gamma)
Scalar alphaMinusPlusBeta = atan2(sneg, cneg);
@@ -222,30 +222,30 @@ namespace Eigen
typedef typename Derived::Scalar Scalar;
Scalar plusMinus = IsEven? 1 : -1;
Scalar minusPlus = IsOdd? 1 : -1;
const Scalar plusMinus = IsEven? 1 : -1;
const Scalar minusPlus = IsOdd? 1 : -1;
Scalar Rsum = sqrt((mat(I, J) * mat(I, J) + mat(I, K) * mat(I, K) + mat(J, I) * mat(J, I) + mat(K, I) * mat(K, I)) / 2);
const Scalar Rsum = sqrt((mat(I, J) * mat(I, J) + mat(I, K) * mat(I, K) + mat(J, I) * mat(J, I) + mat(K, I) * mat(K, I)) / 2);
res[1] = atan2(Rsum, mat(I, I));
if(Rsum > 4 * NumTraits<Scalar>::epsilon()) {
// There is a singularity when sin(beta) == 0
if(Rsum > 4 * NumTraits<Scalar>::epsilon()) {// sin(beta) != 0
res[0] = atan2(mat(J, I), minusPlus * mat(K, I));
res[2] = atan2(mat(I, J), plusMinus * mat(I, K));
}
else if( mat(I, I) > 0) {
else if(mat(I, I) > 0) {// sin(beta) == 0 and cos(beta) == 1
Scalar spos = plusMinus * mat(K, J) + minusPlus * mat(J, K); // 2*sin(alpha + gamma)
Scalar cpos = mat(J, J) + mat(K, K); // 2*cos(alpha + gamma)
res[0] = atan2(spos, cpos);
res[2] = 0;
}
else {
else {// sin(beta) == 0 and cos(beta) == -1
Scalar sneg = plusMinus * mat(K, J) + plusMinus * mat(J, K); // 2*sin(alpha - gamma)
Scalar cneg = mat(J, J) - mat(K, K); // 2*cos(alpha - gamma)
res[0] = atan2(sneg, cneg);
res[1] = 0;
res[2] = 0;
}
}
template<typename Scalar>