Transform: add Random and setRandom methods

CHANGELOG.md

@@ -5,6 +5,7 @@ All notable changes to this project will be documented in this file.
 The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
 ## [Unreleased]
+- Added `Transform3f::Random` and `Transform3f::setRandom` ([#584](https://github.com/humanoid-path-planner/hpp-fcl/pull/584))
 - New feature: computation of contact surfaces for any pair of primitive shapes (triangle, sphere, ellipsoid, plane, halfspace, cone, capsule, cylinder, convex) ([#574](https://github.com/humanoid-path-planner/hpp-fcl/pull/574)).
 - Enhance Broadphase DynamicAABBTree to better handle planes and halfspace ([#570](https://github.com/humanoid-path-planner/hpp-fcl/pull/570))
 - [#558](https://github.com/humanoid-path-planner/hpp-fcl/pull/558):

include/hpp/fcl/math/transform.h

@@ -202,6 +202,12 @@ class HPP_FCL_DLLAPI Transform3f {
     T.setZero();
   }
 
+  /// @brief return a random transform
+  static Transform3f Random() { return Transform3f().setRandom(); }
+
+  /// @brief set the transform to a random transform
+  Transform3f& setRandom();
+
   bool operator==(const Transform3f& other) const {
     return (R == other.getRotation()) && (T == other.getTranslation());
   }
@@ -217,6 +223,39 @@ inline Quatf fromAxisAngle(const Eigen::MatrixBase<Derived>& axis,
   return Quatf(Eigen::AngleAxis<FCL_REAL>(angle, axis));
 }
 
+/// @brief Uniformly random quaternion sphere.
+/// Code taken from Pinocchio (https://github.com/stack-of-tasks/pinocchio).
+inline Quatf uniformRandomQuaternion() {
+  // Rotational part
+  const FCL_REAL u1 = (FCL_REAL)rand() / RAND_MAX;
+  const FCL_REAL u2 = (FCL_REAL)rand() / RAND_MAX;
+  const FCL_REAL u3 = (FCL_REAL)rand() / RAND_MAX;
+
+  const FCL_REAL mult1 = std::sqrt(FCL_REAL(1.0) - u1);
+  const FCL_REAL mult2 = std::sqrt(u1);
+
+  static const FCL_REAL PI_value = EIGEN_PI;
+  FCL_REAL s2 = std::sin(2 * PI_value * u2);
+  FCL_REAL c2 = std::cos(2 * PI_value * u2);
+  FCL_REAL s3 = std::sin(2 * PI_value * u3);
+  FCL_REAL c3 = std::cos(2 * PI_value * u3);
+
+  Quatf q;
+  q.w() = mult1 * s2;
+  q.x() = mult1 * c2;
+  q.y() = mult2 * s3;
+  q.z() = mult2 * c3;
+  return q;
+}
+
+inline Transform3f& Transform3f::setRandom() {
+  const Quatf q = uniformRandomQuaternion();
+  this->rotation() = q.matrix();
+  this->translation().setRandom();
+
+  return *this;
+}
+
 /// @brief Construct othonormal basis from vector.
 /// The z-axis is the normalized input vector.
 inline Matrix3f constructOrthonormalBasisFromVector(const Vec3f& vec) {

test/math.cpp

@@ -139,3 +139,10 @@ BOOST_AUTO_TEST_CASE(transform) {
   // std::cout << output.lhs() << std::endl;
   BOOST_CHECK(isEqual(rv + T, tf.transform(v)));
 }
+
+BOOST_AUTO_TEST_CASE(random_transform) {
+  for (size_t i = 0; i < 100; ++i) {
+    const Transform3f tf = Transform3f::Random();
+    BOOST_CHECK((tf.inverseTimes(tf)).isIdentity());
+  }
+}