Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Contact patch: add bvh and hfield #583

Merged
merged 4 commits into from
May 16, 2024
Merged
Show file tree
Hide file tree
Changes from all commits
Commits
File filter

Filter by extension

Filter by extension

Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
31 changes: 31 additions & 0 deletions include/hpp/fcl/collision_data.h
Original file line number Diff line number Diff line change
Expand Up @@ -894,6 +894,20 @@ struct HPP_FCL_DLLAPI ContactPatchResult {
return this->m_contact_patches.back();
}

/// @brief Getter for the i-th contact patch of the result.
ContactPatch& contactPatch(const size_t i) {
if (this->m_contact_patches.empty()) {
HPP_FCL_THROW_PRETTY(
"The number of contact patches is zero. No ContactPatch can be "
"returned.",
std::invalid_argument);
}
if (i < this->m_contact_patches.size()) {
return this->m_contact_patches[i];
}
return this->m_contact_patches.back();
}

/// @brief Clears the contact patch result.
void clear() {
this->m_contact_patches.clear();
Expand Down Expand Up @@ -948,6 +962,23 @@ struct HPP_FCL_DLLAPI ContactPatchResult {

return true;
}

/// @brief Repositions the ContactPatch when they get inverted during their
/// construction.
void swapObjects() {
// Create new transform: it's the reflection of `tf` along the z-axis.
// This corresponds to doing a PI rotation along the y-axis, i.e. the x-axis
// becomes -x-axis, y-axis stays the same, z-axis becomes -z-axis.
for (size_t i = 0; i < this->numContactPatches(); ++i) {
ContactPatch& patch = this->contactPatch(i);
patch.tf.rotation().col(0) *= -1.0;
patch.tf.rotation().col(2) *= -1.0;

for (size_t j = 0; j < patch.size(); ++j) {
patch.point(i)(0) *= -1.0; // only invert the x-axis
}
}
}
};

struct DistanceResult;
Expand Down
1 change: 1 addition & 0 deletions include/hpp/fcl/contact_patch.h
Original file line number Diff line number Diff line change
Expand Up @@ -107,6 +107,7 @@ class HPP_FCL_DLLAPI ComputeContactPatch {
mutable ContactPatchSolver csolver;

ContactPatchFunctionMatrix::ContactPatchFunc func;
bool swap_geoms;

virtual void run(const Transform3f& tf1, const Transform3f& tf2,
const CollisionResult& collision_result,
Expand Down
73 changes: 41 additions & 32 deletions src/contact_patch.cpp
Original file line number Diff line number Diff line change
Expand Up @@ -55,26 +55,34 @@ void computeContactPatch(const CollisionGeometry* o1, const Transform3f& tf1,
return;
}

// Before doing any computation, we initialize and clear the input result.
result.set(request);
ContactPatchSolver csolver(request);

OBJECT_TYPE object_type1 = o1->getObjectType();
OBJECT_TYPE object_type2 = o2->getObjectType();
NODE_TYPE node_type1 = o1->getNodeType();
NODE_TYPE node_type2 = o2->getNodeType();
// TODO(louis): add support for BVH (leaf is a triangle) and Hfield (leaf is a
// convex)
if (object_type1 != OBJECT_TYPE::OT_GEOM ||
object_type2 != OBJECT_TYPE::OT_GEOM) {
HPP_FCL_THROW_PRETTY("Computing contact patches between node type "
<< std::string(get_node_type_name(node_type1))
<< " and node type "
<< std::string(get_node_type_name(node_type2))
<< " is not yet supported. Only primitive shapes "
"are supported for now.",
std::invalid_argument);
return;
}

const ContactPatchFunctionMatrix& looktable =
getContactPatchFunctionLookTable();

if (object_type1 == OT_GEOM &&
(object_type2 == OT_BVH || object_type2 == OT_HFIELD)) {
if (!looktable.contact_patch_matrix[node_type2][node_type1]) {
HPP_FCL_THROW_PRETTY("Computing contact patches between node type "
<< std::string(get_node_type_name(node_type1))
<< " and node type "
<< std::string(get_node_type_name(node_type2))
<< " is not yet supported.",
std::invalid_argument);
}
looktable.contact_patch_matrix[node_type2][node_type1](
o2, tf2, o1, tf1, collision_result, &csolver, request, result);
result.swapObjects();
return;
}

if (!looktable.contact_patch_matrix[node_type1][node_type2]) {
HPP_FCL_THROW_PRETTY("Contact patch computation between node type "
<< std::string(get_node_type_name(node_type1))
Expand All @@ -84,9 +92,6 @@ void computeContactPatch(const CollisionGeometry* o1, const Transform3f& tf1,
std::invalid_argument);
}

// Before doing any computation, we initialize and clear the input result.
result.set(request);
ContactPatchSolver csolver(request);
return looktable.contact_patch_matrix[node_type1][node_type2](
o1, tf1, o2, tf2, collision_result, &csolver, request, result);
}
Expand All @@ -111,27 +116,25 @@ ComputeContactPatch::ComputeContactPatch(const CollisionGeometry* o1,
OBJECT_TYPE object_type2 = this->o2->getObjectType();
NODE_TYPE node_type2 = this->o2->getNodeType();

if (object_type1 != OBJECT_TYPE::OT_GEOM ||
object_type2 != OBJECT_TYPE::OT_GEOM) {
HPP_FCL_THROW_PRETTY("Computing contact patches between node type "
<< std::string(get_node_type_name(node_type1))
<< " and node type "
<< std::string(get_node_type_name(node_type2))
<< " is not yet supported. Only primitive shapes "
"are supported for now.",
std::invalid_argument);
}

if (!looktable.contact_patch_matrix[node_type1][node_type2]) {
HPP_FCL_THROW_PRETTY("Contact patch computation between node type "
this->swap_geoms = object_type1 == OT_GEOM &&
(object_type2 == OT_BVH || object_type2 == OT_HFIELD);
if ((this->swap_geoms &&
!looktable.contact_patch_matrix[node_type2][node_type1]) ||
(!this->swap_geoms &&
!looktable.contact_patch_matrix[node_type1][node_type2])) {
HPP_FCL_THROW_PRETTY("Collision function between node type "
<< std::string(get_node_type_name(node_type1))
<< " and node type "
<< std::string(get_node_type_name(node_type2))
<< " is not yet supported.",
std::invalid_argument);
}

this->func = looktable.contact_patch_matrix[node_type1][node_type2];
if (this->swap_geoms) {
this->func = looktable.contact_patch_matrix[node_type2][node_type1];
} else {
this->func = looktable.contact_patch_matrix[node_type1][node_type2];
}
}

void ComputeContactPatch::run(const Transform3f& tf1, const Transform3f& tf2,
Expand All @@ -145,8 +148,14 @@ void ComputeContactPatch::run(const Transform3f& tf1, const Transform3f& tf2,

// Before doing any computation, we initialize and clear the input result.
result.set(request);
this->func(this->o1, tf1, this->o2, tf2, collision_result, &(this->csolver),
request, result);
if (this->swap_geoms) {
this->func(this->o2, tf2, this->o1, tf1, collision_result, &(this->csolver),
request, result);
result.swapObjects();
} else {
this->func(this->o1, tf1, this->o2, tf2, collision_result, &(this->csolver),
request, result);
}
}

void ComputeContactPatch::operator()(const Transform3f& tf1,
Expand Down
Loading
Loading