From 12e69ec89660166759ea817f35fe95fb6fea9cc1 Mon Sep 17 00:00:00 2001
From: Hauke Heibel <hauke.heibel@gmail.com>
Date: Wed, 15 May 2013 12:05:01 +0200
Subject: [PATCH] Added asserts to AngleAxis class which verify that the
 initial axis is normalized.

---
 Eigen/src/Eigen2Support/Geometry/AngleAxis.h | 16 +++++++++++++++-
 1 file changed, 15 insertions(+), 1 deletion(-)

diff --git a/Eigen/src/Eigen2Support/Geometry/AngleAxis.h b/Eigen/src/Eigen2Support/Geometry/AngleAxis.h
index af598a403..9f48c460b 100644
--- a/Eigen/src/Eigen2Support/Geometry/AngleAxis.h
+++ b/Eigen/src/Eigen2Support/Geometry/AngleAxis.h
@@ -67,12 +67,21 @@ public:
 
   /** Default constructor without initialization. */
   AngleAxis() {}
+
   /** Constructs and initialize the angle-axis rotation from an \a angle in radian
     * and an \a axis which must be normalized. */
   template<typename Derived>
-  inline AngleAxis(Scalar angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle) {}
+  inline AngleAxis(Scalar angle, const MatrixBase<Derived>& axis) : m_axis(axis), m_angle(angle)
+  {
+    using std::sqrt;
+    using std::abs;
+    // since we compare against 1, this is equal to computing the relative error
+    eigen_assert( abs(m_axis.derived().squaredNorm() - 1) < sqrt( dummy_precision<Scalar>() ) );
+  }
+
   /** Constructs and initialize the angle-axis rotation from a quaternion \a q. */
   inline AngleAxis(const QuaternionType& q) { *this = q; }
+
   /** Constructs and initialize the angle-axis rotation from a 3x3 rotation matrix. */
   template<typename Derived>
   inline explicit AngleAxis(const MatrixBase<Derived>& m) { *this = m; }
@@ -167,6 +176,11 @@ AngleAxis<Scalar>& AngleAxis<Scalar>::operator=(const QuaternionType& q)
   {
     m_angle = 2*std::acos(q.w());
     m_axis = q.vec() / ei_sqrt(n2);
+
+    using std::sqrt;
+    using std::abs;
+    // since we compare against 1, this is equal to computing the relative error
+    eigen_assert( abs(m_axis.derived().squaredNorm() - 1) < sqrt( dummy_precision<Scalar>() ) );
   }
   return *this;
 }