diff --git a/core/src/io/serialization.cc b/core/src/io/serialization.cc index 3b95deb0f..7436fdb03 100644 --- a/core/src/io/serialization.cc +++ b/core/src/io/serialization.cc @@ -250,10 +250,13 @@ namespace boost::serialization { fun = [](const Eigen::Vector2d & /* xy */, double & height, - Eigen::Ref normal) + std::optional> normal) { height = 0.0; - normal = Eigen::Vector3d::UnitZ(); + if (normal.has_value()) + { + normal->setUnit(2); + } }; } diff --git a/core/src/utilities/geometry.cc b/core/src/utilities/geometry.cc index 4ff39f270..65d7677cd 100644 --- a/core/src/utilities/geometry.cc +++ b/core/src/utilities/geometry.cc @@ -623,8 +623,7 @@ namespace jiminy for (Eigen::Index i = 0; i < vertices.rows(); ++i) { auto vertex = vertices.row(i); - Eigen::Vector3d normal; - heightmap(vertex.head<2>(), vertex[2], normal); + heightmap(vertex.head<2>(), vertex[2], std::nullopt); } // Check if the heightmap is flat @@ -694,62 +693,103 @@ namespace jiminy HeightmapFunction sumHeightmaps(const std::vector & heightmaps) { + // Make sure that at least one heightmap has been specified + if (heightmaps.empty()) + { + JIMINY_THROW(bad_control_flow, "At least one heightmap must be specified."); + } + + // Early return if a single heightmap has been specified. Nothing to do. if (heightmaps.size() == 1) { return heightmaps[0]; } + return [heightmaps](const Eigen::Vector2d & pos, double & height, - Eigen::Ref normal) -> void + std::optional> normal) -> void { - thread_local static double height_i; - thread_local static Eigen::Vector3d normal_i; - height = 0.0; - normal.setZero(); - for (const HeightmapFunction & heightmap : heightmaps) + if (normal.has_value()) { - heightmap(pos, height_i, normal_i); - height += height_i; - normal += normal_i; + normal->setZero(); + for (const HeightmapFunction & heightmap : heightmaps) + { + double height_i; + Eigen::Vector3d normal_i; + heightmap(pos, height_i, normal_i); + height += height_i; + normal.value() += normal_i; + } + normal->normalize(); + } + else + { + for (const HeightmapFunction & heightmap : heightmaps) + { + double height_i; + heightmap(pos, height_i, std::nullopt); + height += height_i; + } } - normal.normalize(); }; } HeightmapFunction mergeHeightmaps(const std::vector & heightmaps) { + // Make sure that at least one heightmap has been specified + if (heightmaps.empty()) + { + JIMINY_THROW(bad_control_flow, "At least one heightmap must be specified."); + } + + // Early return if a single heightmap has been specified. Nothing to do. if (heightmaps.size() == 1) { return heightmaps[0]; } + return [heightmaps](const Eigen::Vector2d & pos, double & height, - Eigen::Ref normal) -> void + std::optional> normal) -> void { - thread_local static double height_i; - thread_local static Eigen::Vector3d normal_i; - height = -INF; - bool is_dirty = false; - for (const HeightmapFunction & heightmap : heightmaps) + if (normal.has_value()) { - heightmap(pos, height_i, normal_i); - if (std::abs(height_i - height) < EPS) + bool is_dirty = false; + for (const HeightmapFunction & heightmap : heightmaps) { - normal += normal_i; - is_dirty = true; + double height_i; + Eigen::Vector3d normal_i; + heightmap(pos, height_i, normal_i); + if (std::abs(height_i - height) < EPS) + { + normal.value() += normal_i; + is_dirty = true; + } + else if (height_i > height) + { + height = height_i; + normal = normal_i; + is_dirty = false; + } } - else if (height_i > height) + if (is_dirty) { - height = height_i; - normal = normal_i; - is_dirty = false; + normal->normalize(); } } - if (is_dirty) + else { - normal.normalize(); + for (const HeightmapFunction & heightmap : heightmaps) + { + double height_i; + heightmap(pos, height_i, std::nullopt); + if (height_i > height) + { + height = height_i; + } + } } }; } @@ -765,14 +805,14 @@ namespace jiminy return [stepWidth, stepHeight, stepNumber, axis, interpDelta]( const Eigen::Vector2d & pos, double & height, - Eigen::Ref normal) -> void + std::optional> normal) -> void { // Compute position in stairs reference frame // Eigen::Vector2d posRel = rotMat.inverse() * pos; const double posRel = axis.dot(pos); const double modPos = std::fmod(std::abs(posRel), stepWidth * stepNumber * 2); - // Compute the default height and normal + // Compute the default height uint32_t stairIndex = static_cast(modPos / stepWidth); int8_t staircaseSlopeSign = 1; if (stairIndex >= stepNumber) @@ -781,7 +821,6 @@ namespace jiminy staircaseSlopeSign = -1; } height = stairIndex * stepHeight; - normal = Eigen::Vector3d::UnitZ(); // Avoid unsupported vertical edge const double posRelOnStep = @@ -801,7 +840,14 @@ namespace jiminy // step 2. Rotate normal vector in world plane reference frame: // normal.head<2>() = rotMat * normal.head<2>(); // Or simply in a single operation: - normal << -slope * normInv * axis, normInv; + if (normal.has_value()) + { + normal.value() << -slope * normInv * axis, normInv; + } + } + else if (normal.has_value()) + { + normal->setUnit(2); } }; } @@ -813,9 +859,10 @@ namespace jiminy // 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 normal) mutable + return + [fun = std::move(fun), axis](const Eigen::Vector2d & pos, + double & height, + std::optional> normal) mutable { // Compute the position along axis const Vector1 posAxis = Vector1{axis.dot(pos)}; @@ -823,16 +870,18 @@ namespace jiminy // Compute the height height = fun(posAxis); - // Compute the gradient of the Perlin Proces - const double slope = fun.grad(posAxis)[0]; + if (normal.has_value()) + { + // 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)); + // 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; + // Update normal vector + normal.value() << -slope * normInv * axis, normInv; + } }; - return tmp; } template class AnyPerlinProcess> @@ -840,19 +889,22 @@ namespace jiminy { return [fun = std::move(fun)](const Eigen::Vector2d & pos, double & height, - Eigen::Ref normal) mutable + std::optional> normal) mutable { // Compute the height height = fun(pos); - // Compute the gradient of the Perlin Proces - const auto grad = fun.grad(pos); + if (normal.has_value()) + { + // 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(); + // 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; + // Update normal vector + normal.value() << -normInv * grad.template head<2>(), normInv; + } }; } diff --git a/core/src/utilities/json.cc b/core/src/utilities/json.cc index b29281b58..235ed123b 100644 --- a/core/src/utilities/json.cc +++ b/core/src/utilities/json.cc @@ -147,10 +147,13 @@ namespace jiminy { return {[](const Eigen::Vector2d & /* xy */, double & height, - Eigen::Ref normal) -> void + std::optional> normal) -> void { height = 0.0; - normal = Eigen::Vector3d::UnitZ(); + if (normal.has_value()) + { + normal->setUnit(2); + } }}; } diff --git a/core/src/utilities/random.cc b/core/src/utilities/random.cc index 5cf02dbaf..75bebee3d 100644 --- a/core/src/utilities/random.cc +++ b/core/src/utilities/random.cc @@ -572,7 +572,7 @@ namespace jiminy return [size, heightMax, interpDelta, rotMat, sparsity, interpThr, offset, seed]( const Eigen::Vector2d & pos, double & height, - Eigen::Ref normal) -> void + std::optional> normal) -> void { // Compute the tile index and relative coordinate Eigen::Vector2d posRel = (rotMat * (pos + offset)).array() / size.array(); @@ -636,14 +636,20 @@ namespace jiminy const double dheight_x = dheight_x_0 + (dheight_x_p - dheight_x_0) * ratio; const double dheight_y = (height_p - height_0) / (2.0 * size[1] * interpThr[1]); - normal << -dheight_x, -dheight_y, 1.0; - normal.normalize(); + if (normal.has_value()) + { + normal.value() << -dheight_x, -dheight_y, 1.0; + normal->normalize(); + } } } else { height = heightMax * uniformSparseFromState(posIndices, sparsity, seed); - normal = Eigen::Vector3d::UnitZ(); + if (normal.has_value()) + { + normal->setUnit(2); + } } }; } diff --git a/python/jiminy_pywrap/include/jiminy/python/functors.h b/python/jiminy_pywrap/include/jiminy/python/functors.h index d79558563..230d7df1e 100644 --- a/python/jiminy_pywrap/include/jiminy/python/functors.h +++ b/python/jiminy_pywrap/include/jiminy/python/functors.h @@ -220,18 +220,25 @@ namespace jiminy::python { } - void operator()( - const Eigen::Vector2d & posFrame, double & height, Eigen::Ref normal) + void operator()(const Eigen::Vector2d & posFrame, + double & height, + std::optional> normal) { switch (heightmapType_) { case HeightmapType::CONSTANT: height = bp::extract(handlePyPtr_); - normal = Eigen::Vector3d::UnitZ(); + if (normal.has_value()) + { + normal->setUnit(2); + } break; case HeightmapType::STAIRS: handlePyPtr_(posFrame, convertToPython(height, false)); - normal = Eigen::Vector3d::UnitZ(); + if (normal.has_value()) + { + normal->setUnit(2); + } break; case HeightmapType::GENERIC: default: diff --git a/python/jiminy_pywrap/src/functors.cc b/python/jiminy_pywrap/src/functors.cc index ada1629c8..8f5dcebdd 100644 --- a/python/jiminy_pywrap/src/functors.cc +++ b/python/jiminy_pywrap/src/functors.cc @@ -41,8 +41,7 @@ namespace jiminy::python // Loop over all query points sequentially for (Eigen::Index i = 0; i < positions.cols(); ++i) { - Eigen::Vector3d normal; - heightmap(positions.col(i), heights[i], normal); + heightmap(positions.col(i), heights[i], std::nullopt); } }