Skip to content

Commit 5f46db8

Browse files
committed
CPatchSolver: properly handle segment edge cases
1 parent d1c0eb3 commit 5f46db8

File tree

3 files changed

+159
-156
lines changed

3 files changed

+159
-156
lines changed

include/hpp/fcl/contact_patch/contact_patch_solver.h

+18-47
Original file line numberDiff line numberDiff line change
@@ -58,7 +58,6 @@ namespace fcl {
5858
/// TODO(louis): algo improvement:
5959
/// - The clipping algo is currently n1 * n2; it can be done in n1 + n2.
6060
struct HPP_FCL_DLLAPI ContactPatchSolver {
61-
public:
6261
// Note: `ContactPatch` is an alias for `SupportSet`.
6362
// The two can be used interchangeably.
6463
using ShapeSupportData = details::ShapeSupportData;
@@ -118,29 +117,23 @@ struct HPP_FCL_DLLAPI ContactPatchSolver {
118117
/// @brief Guess for the support sets computation.
119118
mutable support_func_guess_t support_guess;
120119

121-
protected:
122-
/// @brief Support sets used for internal computation.
123-
/// @note The `computePatch` algorithm starts by constructing two 2D
124-
/// convex-hulls (the convex-hulls of the `m_projected_shapes_supports`). It
125-
/// then uses the first convex-hull to clip the second one, effectively
126-
/// computing the intersection between the two convex-hulls.
127-
/// Why have 3 support sets then? Because the algorithm works by
128-
/// successively clipping the first conve-hull. So the first two support
129-
/// sets represent the current and previous iteration of the algorithm and
130-
/// the third set represents the convex-hull of the second shape's support
131-
/// set.
132-
mutable std::array<SupportSet, 3> m_clipping_sets;
133-
134-
/// @brief Tracks the current iterate of the algorithm.
135-
mutable size_t m_id_current{0};
120+
/// @brief Holder for support set of shape 1, used for internal computation.
121+
/// After `computePatch` has been called, this support set is no longer valid.
122+
mutable SupportSet support_set_shape1;
123+
124+
/// @brief Holder for support set of shape 2, used for internal computation.
125+
/// After `computePatch` has been called, this support set is no longer valid.
126+
mutable SupportSet support_set_shape2;
127+
128+
/// @brief Temporary support set used for the Sutherland-Hodgman algorithm.
129+
mutable SupportSet support_set_buffer;
136130

137131
/// @brief Tracks which point of the Sutherland-Hodgman result have been added
138132
/// to the contact patch. Only used if the post-processing step occurs, i.e.
139133
/// if the result of Sutherland-Hodgman has a size bigger than
140134
/// `max_patch_size`.
141-
mutable std::vector<bool> m_added_to_patch;
135+
mutable std::vector<bool> added_to_patch;
142136

143-
public:
144137
/// @brief Default constructor.
145138
explicit ContactPatchSolver() {
146139
const size_t num_contact_patch = 1;
@@ -183,32 +176,9 @@ struct HPP_FCL_DLLAPI ContactPatchSolver {
183176
const ShapeType2& shape2, const Transform3f& tf2,
184177
const ContactPatch& contact_patch) const;
185178

186-
/// @brief Getter for current iterate.
187-
SupportSet& current() { return this->m_clipping_sets[this->m_id_current]; }
188-
189-
/// @brief Const getter for current iterate.
190-
const SupportSet& current() const {
191-
return this->m_clipping_sets[this->m_id_current];
192-
}
193-
194-
/// @brief Getter for previous iterate.
195-
SupportSet& previous() {
196-
return this->m_clipping_sets[1 - this->m_id_current];
197-
}
198-
199-
/// @brief Const getter for previous iterate.
200-
const SupportSet& previous() const {
201-
return this->m_clipping_sets[1 - this->m_id_current];
202-
}
203-
204-
/// @brief Getter for the set used to clip the other one.
205-
SupportSet& clipper() { return this->m_clipping_sets[2]; }
206-
207-
/// @brief Const getter for the set used to clip the other one.
208-
const SupportSet& clipper() const { return this->m_clipping_sets[2]; }
209-
210-
/// @brief Retrieve result from `this->current()`.
211-
void getResult(ContactPatch& contact_patch) const;
179+
/// @brief Retrieve result, adds a post-processing step if result has bigger
180+
/// size than `this->max_patch_size`.
181+
void getResult(const ContactPatch* result, ContactPatch& contact_patch) const;
212182

213183
/// @return true if p inside a clipping region defined by a and b, false
214184
/// otherwise.
@@ -237,9 +207,10 @@ struct HPP_FCL_DLLAPI ContactPatchSolver {
237207
this->num_samples_curved_shapes == other.num_samples_curved_shapes &&
238208
this->patch_tolerance == other.patch_tolerance &&
239209
this->support_guess == other.support_guess &&
240-
this->current() == other.current() &&
241-
this->previous() == other.previous() &&
242-
this->clipper() == other.clipper() &&
210+
this->support_set_shape1 == other.support_set_shape1 &&
211+
this->support_set_shape2 == other.support_set_shape2 &&
212+
this->support_set_buffer == other.support_set_buffer &&
213+
this->added_to_patch == other.added_to_patch &&
243214
this->supportFuncShape1 == other.supportFuncShape1 &&
244215
this->supportFuncShape2 == other.supportFuncShape2;
245216
}

0 commit comments

Comments
 (0)