Skip to content

Commit

Permalink
[core] Make gradient computation optional for heightmap functions.
Browse files Browse the repository at this point in the history
  • Loading branch information
@duburcqa
duburcqa committed Jul 13, 2024
1 parent 3d818a9 commit df4ee42
Show file tree
Hide file tree
Showing 7 changed files with 134 additions and 64 deletions.
2 changes: 1 addition & 1 deletion core/include/jiminy/core/engine/engine.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -297,7 +297,7 @@ namespace jiminy
height = 0.0;
if (normal.has_value())
{
normal->setUnit(2);
normal.value() = Eigen::Vector3d::UnitZ();
}
});

Expand Down
7 changes: 5 additions & 2 deletions core/src/io/serialization.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -250,10 +250,13 @@ namespace boost::serialization
{
fun = [](const Eigen::Vector2d & /* xy */,
double & height,
Eigen::Ref<Eigen::Vector3d> normal)
std::optional<Eigen::Ref<Eigen::Vector3d>> normal)
{
height = 0.0;
normal = Eigen::Vector3d::UnitZ();
if (normal.has_value())
{
normal.value() = Eigen::Vector3d::UnitZ();
}
};
}

Expand Down
150 changes: 101 additions & 49 deletions core/src/utilities/geometry.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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
Expand Down Expand Up @@ -694,62 +693,103 @@ namespace jiminy

HeightmapFunction sumHeightmaps(const std::vector<HeightmapFunction> & 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<Eigen::Vector3d> normal) -> void
std::optional<Eigen::Ref<Eigen::Vector3d>> 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<HeightmapFunction> & 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<Eigen::Vector3d> normal) -> void
std::optional<Eigen::Ref<Eigen::Vector3d>> 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;
}
}
}
};
}
Expand All @@ -765,14 +805,14 @@ namespace jiminy
return [stepWidth, stepHeight, stepNumber, axis, interpDelta](
const Eigen::Vector2d & pos,
double & height,
Eigen::Ref<Eigen::Vector3d> normal) -> void
std::optional<Eigen::Ref<Eigen::Vector3d>> 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<uint32_t>(modPos / stepWidth);
int8_t staircaseSlopeSign = 1;
if (stairIndex >= stepNumber)
Expand All @@ -781,7 +821,6 @@ namespace jiminy
staircaseSlopeSign = -1;
}
height = stairIndex * stepHeight;
normal = Eigen::Vector3d::UnitZ();

// Avoid unsupported vertical edge
const double posRelOnStep =
Expand All @@ -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.value() = Eigen::Vector3d::UnitZ();
}
};
}
Expand All @@ -813,46 +859,52 @@ 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<Eigen::Vector3d> normal) mutable
return
[fun = std::move(fun), axis](const Eigen::Vector2d & pos,
double & height,
std::optional<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];
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<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
std::optional<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);
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;
}
};
}

Expand Down
7 changes: 5 additions & 2 deletions core/src/utilities/json.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -147,10 +147,13 @@ namespace jiminy
{
return {[](const Eigen::Vector2d & /* xy */,
double & height,
Eigen::Ref<Eigen::Vector3d> normal) -> void
std::optional<Eigen::Ref<Eigen::Vector3d>> normal) -> void
{
height = 0.0;
normal = Eigen::Vector3d::UnitZ();
if (normal.has_value())
{
normal.value() = Eigen::Vector3d::UnitZ();
}
}};
}

Expand Down
14 changes: 10 additions & 4 deletions core/src/utilities/random.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -572,7 +572,7 @@ namespace jiminy
return [size, heightMax, interpDelta, rotMat, sparsity, interpThr, offset, seed](
const Eigen::Vector2d & pos,
double & height,
Eigen::Ref<Eigen::Vector3d> normal) -> void
std::optional<Eigen::Ref<Eigen::Vector3d>> normal) -> void
{
// Compute the tile index and relative coordinate
Eigen::Vector2d posRel = (rotMat * (pos + offset)).array() / size.array();
Expand Down Expand Up @@ -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.value() = Eigen::Vector3d::UnitZ();
}
}
};
}
Expand Down
15 changes: 11 additions & 4 deletions python/jiminy_pywrap/include/jiminy/python/functors.h
View file
Original file line number Diff line number Diff line change
Expand Up @@ -220,18 +220,25 @@ namespace jiminy::python
{
}

void operator()(
const Eigen::Vector2d & posFrame, double & height, Eigen::Ref<Eigen::Vector3d> normal)
void operator()(const Eigen::Vector2d & posFrame,
double & height,
std::optional<Eigen::Ref<Eigen::Vector3d>> normal)
{
switch (heightmapType_)
{
case HeightmapType::CONSTANT:
height = bp::extract<double>(handlePyPtr_);
normal = Eigen::Vector3d::UnitZ();
if (normal.has_value())
{
normal.value() = Eigen::Vector3d::UnitZ();
}
break;
case HeightmapType::STAIRS:
handlePyPtr_(posFrame, convertToPython(height, false));
normal = Eigen::Vector3d::UnitZ();
if (normal.has_value())
{
normal.value() = Eigen::Vector3d::UnitZ();
}
break;
case HeightmapType::GENERIC:
default:
Expand Down
3 changes: 1 addition & 2 deletions python/jiminy_pywrap/src/functors.cc
View file
Original file line number Diff line number Diff line change
Expand Up @@ -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);
}
}

Expand Down

0 comments on commit df4ee42

Please sign in to comment.