bug #823: add static method to Quaternion for uniform random rotations.

1 files changed, 22 insertions, 0 deletions

diff --git a/Eigen/src/Geometry/Quaternion.h b/Eigen/src/Geometry/Quaternion.h
index 32d1499c6..babfc86a9 100644
--- a/Eigen/src/Geometry/Quaternion.h
+++ b/Eigen/src/Geometry/Quaternion.h
@@ -271,6 +271,8 @@ public:
   explicit inline Quaternion(const Quaternion<OtherScalar, OtherOptions>& other)
   { m_coeffs = other.coeffs().template cast<Scalar>(); }
 
+  static Quaternion UniformRandom();
+
   template<typename Derived1, typename Derived2>
   static Quaternion FromTwoVectors(const MatrixBase<Derived1>& a, const MatrixBase<Derived2>& b);
 
@@ -609,6 +611,26 @@ inline Derived& QuaternionBase<Derived>::setFromTwoVectors(const MatrixBase<Deri
   return derived();
 }
 
+/** Returns a random quaternion following a uniform distribution law.
+  *
+  * \returns resulting quaternion
+  *
+  * \note The implementation is based on http://planning.cs.uiuc.edu/node198.html
+  */
+template<typename Scalar, int Options>
+Quaternion<Scalar,Options> Quaternion<Scalar,Options>::UniformRandom()
+{
+  const Scalar u1 = internal::random<Scalar>(0,1),
+               u2 = internal::random<Scalar>(0, 2*M_PI),
+               u3 = internal::random<Scalar>(0, 2*M_PI);
+  const Scalar a = std::sqrt (1 - u1),
+               b = std::sqrt (u1);
+  return Quaternion (a * std::sin (u2),
+                     a * std::cos (u2),
+                     b * std::sin (u3),
+                     b * std::cos (u3));
+}
+
 
 /** Returns a quaternion representing a rotation between
   * the two arbitrary vectors \a a and \a b. In other words, the built