Fix calc bug, docs and better testing.

Test code changes:
* better coded
* rand and manual numbers
* singularity checking

2 files changed, 33 insertions, 31 deletions

diff --git a/unsupported/Eigen/src/EulerAngles/EulerAngles.h b/unsupported/Eigen/src/EulerAngles/EulerAngles.h
index da86cc13b..8a723d9ee 100644
--- a/unsupported/Eigen/src/EulerAngles/EulerAngles.h
+++ b/unsupported/Eigen/src/EulerAngles/EulerAngles.h
@@ -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()); }
diff --git a/unsupported/Eigen/src/EulerAngles/EulerSystem.h b/unsupported/Eigen/src/EulerAngles/EulerSystem.h
index aa96461f9..0790e612f 100644
--- a/unsupported/Eigen/src/EulerAngles/EulerSystem.h
+++ b/unsupported/Eigen/src/EulerAngles/EulerSystem.h
@@ -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>