[core] Add Perlin ground generators.

core/include/jiminy/core/robot/model.h

@@ -1,6 +1,8 @@
 #ifndef JIMINY_MODEL_H
 #define JIMINY_MODEL_H
 
+#include <optional>
+
 #include "pinocchio/spatial/fwd.hpp"         // `pinocchio::SE3`
 #include "pinocchio/multibody/model.hpp"     // `pinocchio::Model`
 #include "pinocchio/multibody/data.hpp"      // `pinocchio::Data`

core/include/jiminy/core/utilities/geometry.h

@@ -48,6 +48,21 @@ namespace jiminy
     /// \param[in] orientation Orientation of the staircases in the XY plane.
     HeightmapFunction JIMINY_DLLAPI periodicStairs(
         double stepWidth, double stepHeight, uint32_t stepNumber, double orientation);
+
+    HeightmapFunction JIMINY_DLLAPI unidirectionalRandomPerlinGround(
+        double wavelength, std::size_t numOctaves, double orientation, uint32_t seed);
+
+    HeightmapFunction JIMINY_DLLAPI unidirectionalPeriodicPerlinGround(double wavelength,
+                                                                       double period,
+                                                                       std::size_t numOctaves,
+                                                                       double orientation,
+                                                                       uint32_t seed);
+
+    HeightmapFunction JIMINY_DLLAPI randomPerlinGround(
+        double wavelength, std::size_t numOctaves, uint32_t seed);
+
+    HeightmapFunction JIMINY_DLLAPI periodicPerlinGround(
+        double wavelength, double period, std::size_t numOctaves, uint32_t seed);
 }
 
 #endif  // JIMINY_GEOMETRY_H

core/include/jiminy/core/utilities/random.h

@@ -331,7 +331,7 @@ namespace jiminy
         const double wavelength_;
         const double period_;
         const Eigen::Index numTimes_{static_cast<int>(std::ceil(period_ / (0.1 * wavelength_)))};
-        const double dt_{period_ / numTimes_};
+        const double dt_{period_ / static_cast<double>(numTimes_)};
 
         Eigen::VectorXd values_{numTimes_};
         Eigen::VectorXd grads_{numTimes_};

core/include/jiminy/core/utilities/random.hxx

@@ -418,7 +418,7 @@ namespace jiminy
         {
             // Generate a uniformly distributed random vector on n-sphere
             VectorN<double> dir;
-            for (uint_fast8_t i = 0; i < N; i += 2)
+            for (uint32_t i = 0; i < N; i += 2)
             {
                 // Generate 2 uniformly distributed random variables
                 const float u1 = static_cast<float>(hash) / fHashMax;

core/src/utilities/geometry.cc

@@ -1,7 +1,9 @@
 #include "hpp/fcl/BVH/BVH_model.h"  // `hpp::fcl::CollisionGeometry`, `hpp::fcl::BVHModel`, `hpp::fcl::OBBRSS`
 #include "hpp/fcl/shape/geometric_shapes.h"  // `hpp::fcl::Halfspace`
 #include "hpp/fcl/hfield.h"                  // `hpp::fcl::HeightField`
+
 #include "jiminy/core/utilities/geometry.h"
+#include "jiminy/core/utilities/random.h"
 
 
 namespace jiminy
@@ -803,4 +805,88 @@ namespace jiminy
             }
         };
     }
+
+    template<template<unsigned int> class AnyPerlinProcess>
+    static HeightmapFunction generateHeightmapFromPerlinProcess1D(AnyPerlinProcess<1> && fun,
+                                                                  double orientation)
+    {
+        // Define the projection axis
+        const Eigen::Vector2d axis{std::cos(orientation), std::sin(orientation)};
+
+        auto tmp = [fun = std::move(fun), axis](const Eigen::Vector2d & pos,
+                                                double & height,
+                                                Eigen::Ref<Eigen::Vector3d> normal) mutable
+        {
+            // Compute the position along axis
+            const Vector1<double> posAxis = Vector1<double>{axis.dot(pos)};
+
+            // Compute the height
+            height = fun(posAxis);
+
+            // Compute the gradient of the Perlin Proces
+            const double slope = fun.grad(posAxis)[0];
+
+            // Compute the inverse of the normal's Euclidean norm
+            const double normInv = 1.0 / std::sqrt(1.0 + std::pow(slope, 2));
+
+            // Update normal vector
+            normal << -slope * normInv * axis, normInv;
+        };
+        return tmp;
+    }
+
+    template<template<unsigned int> class AnyPerlinProcess>
+    static HeightmapFunction generateHeightmapFromPerlinProcess2D(AnyPerlinProcess<2> && fun)
+    {
+        return [fun = std::move(fun)](const Eigen::Vector2d & pos,
+                                      double & height,
+                                      Eigen::Ref<Eigen::Vector3d> normal) mutable
+        {
+            // Compute the height
+            height = fun(pos);
+
+            // Compute the gradient of the Perlin Proces
+            const auto grad = fun.grad(pos);
+
+            // Compute the inverse of the normal's Euclidean norm
+            const double normInv = 1.0 / grad.norm();
+
+            // Update normal vector
+            normal << -normInv * grad.template head<2>(), normInv;
+        };
+    }
+
+    HeightmapFunction unidirectionalRandomPerlinGround(
+        double wavelength, std::size_t numOctaves, double orientation, uint32_t seed)
+    {
+        auto fun = RandomPerlinProcess<1>(wavelength, numOctaves);
+        fun.reset(PCG32(seed));
+        return generateHeightmapFromPerlinProcess1D(std::move(fun), orientation);
+    }
+
+    HeightmapFunction randomPerlinGround(double wavelength, std::size_t numOctaves, uint32_t seed)
+    {
+        auto fun = RandomPerlinProcess<2>(wavelength, numOctaves);
+        fun.reset(PCG32(seed));
+        return generateHeightmapFromPerlinProcess2D(std::move(fun));
+    }
+
+    HeightmapFunction periodicPerlinGround(
+        double wavelength, double period, std::size_t numOctaves, uint32_t seed)
+    {
+        auto fun = PeriodicPerlinProcess<2>(wavelength, period, numOctaves);
+        fun.reset(PCG32(seed));
+        return generateHeightmapFromPerlinProcess2D(std::move(fun));
+    }
+
+    HeightmapFunction unidirectionalPeriodicPerlinGround(double wavelength,
+                                                         double period,
+                                                         std::size_t numOctaves,
+                                                         double orientation,
+                                                         uint32_t seed)
+    {
+        auto fun = PeriodicPerlinProcess<1>(wavelength, period, numOctaves);
+        fun.reset(PCG32(seed));
+        return generateHeightmapFromPerlinProcess1D(std::move(fun), orientation);
+    }
 }

core/src/utilities/random.cc

@@ -253,7 +253,7 @@ namespace jiminy
         Eigen::Index indexRight = indexLeft + 1;
 
         // Compute the time ratio
-        const double ratio = quot - indexLeft;
+        const double ratio = quot - static_cast<double>(indexLeft);
 
         return {indexLeft, indexRight, ratio};
     }
@@ -262,7 +262,7 @@ namespace jiminy
         double value, double delta, Eigen::Index numTimes)
     {
         // Wrap value in period interval
-        const double period = numTimes * delta;
+        const double period = static_cast<double>(numTimes) * delta;
         value = std::fmod(value, period);
         if (value < 0.0)
         {
@@ -403,7 +403,8 @@ namespace jiminy
         values_.noalias() += scale * cosMat_ * normalVec2;
 
         const auto diff =
-            2 * M_PI / period_ * Eigen::VectorXd::LinSpaced(numHarmonics_, 1, numHarmonics_);
+            2 * M_PI / period_ * Eigen::VectorXd::LinSpaced(
+                numHarmonics_, 1, static_cast<double>(numHarmonics_));
         grads_ = scale * cosMat_ * normalVec1.cwiseProduct(diff);
         grads_.noalias() -= scale * sinMat_ * normalVec2.cwiseProduct(diff);
     }

python/jiminy_pywrap/src/generators.cc

@@ -256,6 +256,18 @@ namespace jiminy::python
         bp::def("periodic_stairs_ground",
                 &periodicStairs,
                 (bp::arg("step_width"), "step_height", "step_number", "orientation"));
+        bp::def("unidirectional_random_perlin_ground",
+                &unidirectionalRandomPerlinGround,
+                (bp::arg("wavelength"), "num_octaves", "orientation", "seed"));
+        bp::def("unidirectional_periodic_perlin_ground",
+                &unidirectionalPeriodicPerlinGround,
+                (bp::arg("wavelength"), "period", "num_octaves", "orientation", "seed"));
+        bp::def("random_perlin_ground",
+                &randomPerlinGround,
+                (bp::arg("wavelength"), "num_octaves", "seed"));
+        bp::def("periodic_perlin_ground",
+                &periodicPerlinGround,
+                (bp::arg("wavelength"), "period", "num_octaves", "seed"));
         bp::def("sum_heightmaps", &sumHeightmaps, (bp::arg("heightmaps")));
         bp::def("merge_heightmaps", &mergeHeightmaps, (bp::arg("heightmaps")));