add normalize and normalized overloads in AlignedVector3

2 files changed, 16 insertions, 1 deletions

diff --git a/unsupported/Eigen/AlignedVector3 b/unsupported/Eigen/AlignedVector3
index f9f364eba..854f0601f 100644
--- a/unsupported/Eigen/AlignedVector3
+++ b/unsupported/Eigen/AlignedVector3
@@ -152,8 +152,19 @@ template<typename _Scalar> class AlignedVector3
     {
       ei_assert(m_coeffs.w()==Scalar(0));
       ei_assert(other.m_coeffs.w()==Scalar(0));
+      Scalar r = m_coeffs.dot(other.m_coeffs);
       return m_coeffs.dot(other.m_coeffs);
     }
+
+    inline void normalize()
+    {
+      m_coeffs /= norm();
+    }
+
+    inline AlignedVector3 normalized()
+    {
+      return AlignedVector3(m_coeffs / norm());
+    }
     
     inline Scalar sum() const
     {
diff --git a/unsupported/test/alignedvector3.cpp b/unsupported/test/alignedvector3.cpp
index cf33dcceb..52d39ce50 100644
--- a/unsupported/test/alignedvector3.cpp
+++ b/unsupported/test/alignedvector3.cpp
@@ -40,7 +40,7 @@ void alignedvector3()
   
   VERIFY_IS_APPROX(f1,r1);
   VERIFY_IS_APPROX(f4,r4);
-  
+
   VERIFY_IS_APPROX(f4+f1,r4+r1);
   VERIFY_IS_APPROX(f4-f1,r4-r1);
   VERIFY_IS_APPROX(f4+f1-f2,r4+r1-r2);
@@ -56,6 +56,10 @@ void alignedvector3()
   VERIFY_IS_APPROX(f2.dot(f3),r2.dot(r3));
   VERIFY_IS_APPROX(f2.cross(f3),r2.cross(r3));
   VERIFY_IS_APPROX(f2.norm(),r2.norm());
+
+  VERIFY_IS_APPROX(f2.normalized(),r2.normalized());
+
+  VERIFY_IS_APPROX((f2+f1).normalized(),(r2+r1).normalized());
   
   f2.normalize();
   r2.normalize();