When libccd reports the nearest feature is an edge, validate the result #388
Conversation
hongkai-dai
force-pushed
the
fix_ccd_nearest_edge_feature
branch
from
295c07a
to
35eae12
Compare
There was a problem hiding this comment.
+@SeanCurtis-TRI for feature review please, thanks!
Reviewable status: 0 of 2 files reviewed, all discussions resolved (waiting on @SeanCurtis-TRI)
|
This fixes #319 - thank you @hongkai-dai :) |
There was a problem hiding this comment.
You've still got some angry CI. Should I review it?
Reviewable status: 0 of 2 files reviewed, all discussions resolved (waiting on @SeanCurtis-TRI)
There was a problem hiding this comment.
@SeanCurtis-TRI , yes I think the code is ready for review. The CI failure are due to the outdated version of libccd. I haven't added the line to enable test only if libccd 2.1 is available. The Mac OSX CI passes (with the newer version of libccd). By the time when we finish the code review, if libccd is still not upgraded on CI, I will then disable the test.
Reviewable status: 0 of 2 files reviewed, all discussions resolved (waiting on @SeanCurtis-TRI)
|
I am running this branch in an extensive computation process - so far it really improved the stability of the process and did solve #319. I just encountered the following though (validateNearestFeatureOfPolytopeBeingEdge): As this statement has been newly introduced in this PR I am posting it here - but it may be better to place it in a new issue? Do you have any insights on this? Thanks, |
|
Thanks @wxmerkt ! Is it OK if you could post the error in a separate issue? My guess is that the problem occurs in the We have an issue on #366 , @SeanCurtis-TRI submitted a PR #367 to fix some of the bugs that cause the issue #366, but we know that we haven't totally fixed the issue yet. |
|
My guess is that there are several stumbling blocks. This PR helped you over the earlier stumbling block so you could hit the later stumbling block. Fortunately, we should be able to reproduce the bug given the error report. |
|
FYI I checked the rotation matrix in X_FS1 -- it is perfectly orthonormal so the 1e-08's in it actually represent that Shape1 was reoriented almost exactly 90 degrees but not quite! |
|
I confirm that in @wxmerkt example, I will look into the |
There was a problem hiding this comment.
First pass done -- mostly just typos, documentation, and passing thoughts.
Reviewed 2 of 2 files at r1.
Reviewable status: all files reviewed, 11 unresolved discussions (waiting on @hongkai-dai)
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 946 at r1 (raw file):
* If A, B, P are coincident, report true. */ static bool is_point_on_line_segment(const ccd_vec3_t& p, const ccd_vec3_t& a, const ccd_vec3_t& b) {
nit: line length
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1561 at r1 (raw file):
* 1. The origin lies exactly on that edge * 2. The two neighbouring faces of that edge are coplanar, and the projection * of the origin onto that plane is on the edge. Inside this function, we will
nit: Put a newline between "...is on that edge. \n Inside this function..."
Separate the condition from what the function does with the conditions.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1564 at r1 (raw file):
* verify if one of these two conditions are true. If not, we will modify the * nearest feature stored inside @p polytope, such that it stores the right * nearest feature and distance.
BTW The "right" nearest feature is predicated on the assumption that the edge is a slight perturbation of the truth -- that the nearest feature really lies on one of the two adjacent faces. If the assumption is incorrect, then this function won't produce the "right" nearest feature. Probably worth documenting that assumption/condition.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1571 at r1 (raw file):
*/ static void validateNearestFeatureOfPolytopeBeingEdge(ccd_pt_t* polytope) { if (polytope->nearest_type == CCD_PT_EDGE) {
btw It might be worth throwing if this isn't the case -- because it will prevent this from being called in the wrong context. Otherwise, it could just be plastered in arbitrary places and the caller won't have to have provided sufficient analysis to make the call meaningful.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1578 at r1 (raw file):
// Find the outward normals on the two neighbouring faces of the edge, if // the origin is on the "inner" side of these two faces, then we regard the // origin to be inside the polytope. Note that these @p face_normals are
BTW the @p is probably overkill inside a function. :)
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1587 at r1 (raw file):
ccdVec3Normalize(&(face_normals[i])); // If the origin is on the "inner" side of the face, then // face_normals[i].dot(origin - edge.vertex) <= 0.
BTW Might be worth carrying the reader a bit further:
// face_normals[i].dot(origin - edge.vertex) <= 0, or, with simplification,
// -face_normals[i].dot(-edge.vertex) <= 0Which makes me think this would read better as:
// n̂ ⋅ (o - vₑ) ≤ 0 or, with simplification, -n̂ ⋅ vₑ ≤ 0.That way the algebra is done when it looks clean, and the reader can map algebra to code. But doing algebra with code is a nuisance.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1589 at r1 (raw file):
// face_normals[i].dot(origin - edge.vertex) <= 0. origin_to_face_distance[i] = -ccdVec3Dot(&(face_normals[i]), &(nearest_edge->vertex[0]->v.v));
BTW This is one of the reasons I so despise libccd. An otherwise simple mathematical expression is rendered HIDEOUSLY!
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1590 at r1 (raw file):
origin_to_face_distance[i] = -ccdVec3Dot(&(face_normals[i]), &(nearest_edge->vertex[0]->v.v)); if (origin_to_face_distance[i] > 0) {
BTW Given this initial test, you could defer normalization until after you know you're actually on the inside. And then, rather than normalizing the whole vector you could simply normalize the result (distance) reducing the number of divisions by 2/3. Or skip the division entirely by doing multiplications -- after all, you only want to know which is smaller.
But this is all optimization for a rare circumstance. So, consider these idle thoughts.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1597 at r1 (raw file):
} // If the two neighbouring faces are co-planar, then their face normals are // the same.
BTW Some acknowledgement that this is an anisotropic test. The magnitude of epsilon gets bigger and bigger the father the vectors move away from the basis vectors.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1606 at r1 (raw file):
} bool is_edge_closest_feature = true; if (!are_neighbouring_faces_coplanar) {
It seems this could be simplified. If the edge is truly the closest feature, than whether the adjacent faces are co-planar or not, the projection of the origin onto either face would lie on the edge. So, what's the value of the co-planarity testing and decomposition into two cases?
Is the concern that the projections may not be equally good witnesses to the incorrectness and you might end up selecting the projection that is the weaker witness?
Thoughts?
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1630 at r1 (raw file):
} if (!is_edge_closest_feature) { // We assume that libccd is not crazily wrong. Although the closest
BTW This is the very idea I had in mind when you defined "right" in the function documentation.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1631 at r1 (raw file):
if (!is_edge_closest_feature) { // We assume that libccd is not crazily wrong. Although the closest // feature should not be the edge, it is near that edge. Hence we pick one
nit: "...feature is not the edge."
(After all, that's what all the previous code set out to prove.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1631 at r1 (raw file):
nit: "we pick one of the neighboring faces" is misleading and doesn't do the code justice. How about:
Hence, we select the neighboring face that is closest to the origin.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1682 at r1 (raw file):
*nearest = ccdPtNearest(polytope); if (polytope->nearest_type == CCD_PT_EDGE) { // When libccd thinks the nearest feature is an edge, that is often
BTW Perhaps a TODO which would explain when this test could be removed?
test/test_fcl_signed_distance.cpp, line 168 at r1 (raw file):
template <typename S> void test_distance_cylinder_sphere1() {
BTW What's the significance of the 1 on the name (and on the next test)?
test/test_fcl_signed_distance.cpp, line 171 at r1 (raw file):
// This is a specific case that has cropped up in the wild that reaches the // unexpected `expandPolytope()` error, where the nearest point and the new // vertiex botih lie on an edge. It turns out that libccd incorrectly thinks
nit s/vertiex/vertex
s/botih/both
test/test_fcl_signed_distance.cpp, line 173 at r1 (raw file):
// vertiex botih lie on an edge. It turns out that libccd incorrectly thinks // that the edge is the nearest feature, while actually one of the // neighbouring face is. This test confirms that the bug is fixed, by the
nit s/face/faces
test/test_fcl_signed_distance.cpp, line 174 at r1 (raw file):
// that the edge is the nearest feature, while actually one of the // neighbouring face is. This test confirms that the bug is fixed, by the // function validateNearestFeatureOfPolytopeBeingEdge.
btw Please end with ().
test/test_fcl_signed_distance.cpp, line 181 at r1 (raw file):
new fcl::Cylinder<S>(cylinder_radius, cylinder_length)); Transform3<S> X_WC = Transform3<S>::Identity(); X_WC.translation() <<0.6, 0,0.325;
nit: space
test/test_fcl_signed_distance.cpp, line 188 at r1 (raw file):
Transform3<S> X_WS = Transform3<S>::Identity(); // clang-format off X_WS.matrix() << -0.9954758066974283004, -0.029586630161622884394, 0.090291470227168560414, 0.54197940181714598928,
FYI On my machine, truncated values also produce failure without your fix:
// clang-format off
X_WS.matrix() << -0.9954758066, -0.0295866301, 0.0902914702, 0.5419794018,
-0.0851034786, -0.1449450565, -0.9857729599, -0.0621175025,
0.0422530022, -0.9889972506, 0.1417713719, 0.6016236576,
0, 0, 0, 1;
// clang-format onAnd these fit in 80 columns and are easier to read.
I used a regular expression to search and replace:
Find: (-?\d*\.\d{10})\d+
Replace: $1
(In Clion -- although, this is pretty standard regex.)
test/test_fcl_signed_distance.cpp, line 197 at r1 (raw file):
fcl::DistanceRequest<S> request; request.gjk_solver_type = GJKSolverType::GST_LIBCCD; request.distance_tolerance = 9.9999999999999995475e-07;
BTW Feel free to make this simply 1e-8. I guarantee it'll still do the job.
test/test_fcl_signed_distance.cpp, line 201 at r1 (raw file):
fcl::DistanceResult<S> result; EXPECT_NO_THROW(fcl::distance(&cylinder, &sphere, request, result));
nit ASSERT_NO_THROW (if it throws, no sense in doing the rest of the test).
test/test_fcl_signed_distance.cpp, line 219 at r1 (raw file):
// unexpected `expandPolytope()` error. This error was reported in // https://github.com/flexible-collision-library/fcl/issues/319 template <typename S>
When I disabled the corrective function, this test did not fail. (I've confirmed that I'm got doubles for ccd_real_t. Thoughts?
test/test_fcl_signed_distance.cpp, line 246 at r1 (raw file):
fcl::DistanceRequest<S> request; request.gjk_solver_type = GJKSolverType::GST_LIBCCD; request.distance_tolerance = 9.9999999999999995475e-07;
FYI Another case of 1e-8 being sufficient.
test/test_fcl_signed_distance.cpp, line 250 at r1 (raw file):
fcl::DistanceResult<S> result; EXPECT_NO_THROW(fcl::distance(&cylinder, &box, request, result));
nit ASSERT_NO_THROW
There was a problem hiding this comment.
Reviewable status: 0 of 2 files reviewed, 2 unresolved discussions (waiting on @SeanCurtis-TRI)
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 946 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
nit: line length
Done.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1561 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
nit: Put a newline between "...is on that edge. \n Inside this function..."
Separate the condition from what the function does with the conditions.
Done.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1564 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW The "right" nearest feature is predicated on the assumption that the edge is a slight perturbation of the truth -- that the nearest feature really lies on one of the two adjacent faces. If the assumption is incorrect, then this function won't produce the "right" nearest feature. Probably worth documenting that assumption/condition.
Done.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1571 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
btw It might be worth throwing if this isn't the case -- because it will prevent this from being called in the wrong context. Otherwise, it could just be plastered in arbitrary places and the caller won't have to have provided sufficient analysis to make the call meaningful.
I add an assertion here, would that be good?
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1578 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW the
@pis probably overkill inside a function. :)
Done.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1587 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW Might be worth carrying the reader a bit further:
// face_normals[i].dot(origin - edge.vertex) <= 0, or, with simplification, // -face_normals[i].dot(-edge.vertex) <= 0Which makes me think this would read better as:
// n̂ ⋅ (o - vₑ) ≤ 0 or, with simplification, -n̂ ⋅ vₑ ≤ 0.That way the algebra is done when it looks clean, and the reader can map algebra to code. But doing algebra with code is a nuisance.
Done, thanks!
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1589 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW This is one of the reasons I so despise libccd. An otherwise simple mathematical expression is rendered HIDEOUSLY!
Agreed
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1590 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW Given this initial test, you could defer normalization until after you know you're actually on the inside. And then, rather than normalizing the whole vector you could simply normalize the result (distance) reducing the number of divisions by 2/3. Or skip the division entirely by doing multiplications -- after all, you only want to know which is smaller.
But this is all optimization for a rare circumstance. So, consider these idle thoughts.
I see. I agree it would save some division. It probably makes the code less straightforward. I will leave the current implementation here.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1597 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW Some acknowledgement that this is an anisotropic test. The magnitude of epsilon gets bigger and bigger the father the vectors move away from the basis vectors.
Done.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1606 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
It seems this could be simplified. If the edge is truly the closest feature, than whether the adjacent faces are co-planar or not, the projection of the origin onto either face would lie on the edge. So, what's the value of the co-planarity testing and decomposition into two cases?
Is the concern that the projections may not be equally good witnesses to the incorrectness and you might end up selecting the projection that is the weaker witness?
Thoughts?
Done. Good call. I simplified the code.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1631 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
nit: "...feature is not the edge."
(After all, that's what all the previous code set out to prove.
Done.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1631 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
nit: "we pick one of the neighboring faces" is misleading and doesn't do the code justice. How about:
Hence, we select the neighboring face that is closest to the origin.
Done.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1682 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW Perhaps a TODO which would explain when this test could be removed?
Done.
test/test_fcl_signed_distance.cpp, line 168 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW What's the significance of the
1on the name (and on the next test)?
I assume we will encounter more errors when people test FCL code, so we will have test_distance_cylinder_sphere2 to capture the user reported error in the future.
test/test_fcl_signed_distance.cpp, line 171 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
nit s/vertiex/vertex
s/botih/both
Done.
test/test_fcl_signed_distance.cpp, line 173 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
nit s/face/faces
Done.
test/test_fcl_signed_distance.cpp, line 174 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
btw Please end with
().
Done.
test/test_fcl_signed_distance.cpp, line 181 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
nit: space
Done.
test/test_fcl_signed_distance.cpp, line 188 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
FYI On my machine, truncated values also produce failure without your fix:
// clang-format off X_WS.matrix() << -0.9954758066, -0.0295866301, 0.0902914702, 0.5419794018, -0.0851034786, -0.1449450565, -0.9857729599, -0.0621175025, 0.0422530022, -0.9889972506, 0.1417713719, 0.6016236576, 0, 0, 0, 1; // clang-format onAnd these fit in 80 columns and are easier to read.
I used a regular expression to search and replace:
Find:
(-?\d*\.\d{10})\d+
Replace:$1
(In Clion -- although, this is pretty standard regex.)
Nice, thanks!
test/test_fcl_signed_distance.cpp, line 197 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW Feel free to make this simply 1e-8. I guarantee it'll still do the job.
Done.
test/test_fcl_signed_distance.cpp, line 201 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
nit ASSERT_NO_THROW (if it throws, no sense in doing the rest of the test).
Done.
test/test_fcl_signed_distance.cpp, line 219 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
When I disabled the corrective function, this test did not fail. (I've confirmed that I'm got
doubles forccd_real_t. Thoughts?
Did you try to compile fcl in debug mode? When I compile it in debug mode, I got the assertion error.
In my branch https://github.com/hongkai-dai/fcl/tree/fix_ccd_nearest_edge_feature_test, it only has the tests, but no fix in gjk_libccd-inl.h. If you compile fcl in debug mode, and run test_fcl_signed_distance, I think you should see an assertion error in test_distance_cylinder_box1.
test/test_fcl_signed_distance.cpp, line 246 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
FYI Another case of 1e-8 being sufficient.
Done.
test/test_fcl_signed_distance.cpp, line 250 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
nit
ASSERT_NO_THROW
Done.
There was a problem hiding this comment.
I'd like to see the modification to the CMake so we can see a successful run through CI.
Reviewed 2 of 2 files at r2.
Reviewable status: all files reviewed, 3 unresolved discussions (waiting on @hongkai-dai)
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1597 at r1 (raw file):
Previously, hongkai-dai (Hongkai Dai) wrote…
Done.
Done in the sense that it just goes away. My favorite kind. :)
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1606 at r1 (raw file):
Previously, hongkai-dai (Hongkai Dai) wrote…
Done. Good call. I simplified the code.
And resolved the uneven quality of the witnesses by asking both.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 396 at r2 (raw file):
// check touching contact // Compute origin_projection as well. Without computing the origin projection, // libccd could give inaccurate result. See
nit: Explicitly state that the value is not used. In fact, you could go ahead and call the variable origin_projection_unused. Same for point_projection_on_triangle. We want it to be clear to readers that this quantity is thrown out.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1577 at r2 (raw file):
* If not, we will modify the nearest feature stored inside @p polytope, such * that it stores the right nearest feature and distance. * @note we assume that even if the edge is not the right nearest feature, the
BTW In this context, probably worth swapping "right" for correct, as one can also think about the adjacent faces being "left" and "right".
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1615 at r2 (raw file):
// If the projection to both faces are on the edge, the the edge is the // closest feature. std::array<ccd_vec3_t, 2> origin_projection_to_planes;
nit: As near as I can tell, these values are only used once, in the body of the for loop and then discarded. As such, they should just be a local in the for loop.
test/test_fcl_signed_distance.cpp, line 168 at r1 (raw file):
Previously, hongkai-dai (Hongkai Dai) wrote…
I assume we will encounter more errors when people test FCL code, so we will have
test_distance_cylinder_sphere2to capture the user reported error in the future.
BTW You might pick the issue number. test_distance_issue319 or some such thing.
test/test_fcl_signed_distance.cpp, line 219 at r1 (raw file):
Previously, hongkai-dai (Hongkai Dai) wrote…
Did you try to compile
fclin debug mode? When I compile it in debug mode, I got the assertion error.In my branch https://github.com/hongkai-dai/fcl/tree/fix_ccd_nearest_edge_feature_test, it only has the tests, but no fix in
gjk_libccd-inl.h. If you compile fcl in debug mode, and run test_fcl_signed_distance, I think you should see an assertion error intest_distance_cylinder_box1.
I hadn't -- but it's worth calling out that property. It only failed in debug mode due to a failed assertion.
test/test_fcl_signed_distance.cpp, line 277 at r2 (raw file):
new fcl::Box<S>(box1_size(0), box1_size(1), box1_size(2))); Transform3<S> X_WB1 = Transform3<S>::Identity(); X_WB1.matrix() << -3.0627937852578681533e-08, -0.99999999999999888978,
BTW Not required, but a truncated version of this that nevertheless still fails would be easier to consume. :)
test/test_fcl_signed_distance.cpp, line 296 at r2 (raw file):
fcl::DistanceRequest<S> request; request.gjk_solver_type = GJKSolverType::GST_LIBCCD; request.distance_tolerance = 9.9999999999999995475e-07;
BTW Same not applies here -- this is actually what the literal 1e-6 turns into.
There was a problem hiding this comment.
Reviewable status: 0 of 3 files reviewed, 1 unresolved discussion (waiting on @SeanCurtis-TRI)
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 396 at r2 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
nit: Explicitly state that the value is not used. In fact, you could go ahead and call the variable
origin_projection_unused. Same forpoint_projection_on_triangle. We want it to be clear to readers that this quantity is thrown out.
Done.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1577 at r2 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW In this context, probably worth swapping "right" for correct, as one can also think about the adjacent faces being "left" and "right".
Done.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1615 at r2 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
nit: As near as I can tell, these values are only used once, in the body of the for loop and then discarded. As such, they should just be a local in the for loop.
Done.
test/test_fcl_signed_distance.cpp, line 168 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW You might pick the issue number.
test_distance_issue319or some such thing.
This actually comes from enabling-anzu. I can't put the enabling anzu issue there, as the repo is in github enterprise.
test/test_fcl_signed_distance.cpp, line 219 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
I hadn't -- but it's worth calling out that property. It only failed in debug mode due to a failed assertion.
Done.
test/test_fcl_signed_distance.cpp, line 277 at r2 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW Not required, but a truncated version of this that nevertheless still fails would be easier to consume. :)
Hmm, I truncated some of the numbers, but for others I don't know where to truncate.
test/test_fcl_signed_distance.cpp, line 296 at r2 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW Same not applies here -- this is actually what the literal 1e-6 turns into.
Done.
There was a problem hiding this comment.
Reviewed 3 of 3 files at r3.
Reviewable status:complete! all files reviewed, all discussions resolved
test/test_fcl_signed_distance.cpp, line 168 at r1 (raw file):
Previously, hongkai-dai (Hongkai Dai) wrote…
This actually comes from enabling-anzu. I can't put the enabling anzu issue there, as the repo is in github enterprise.
I'll mark myself as satisfied; but I'll say two last thoughts.
- We could take the scenario, put it in its own issue where it's described in coherent detail and reference it here. It need not be explicitly attributed to private repos.
- We might benefit from a naming convention where all of the "tests from the wild" have some part of the name that references that.
test/test_fcl_signed_distance.cpp, line 271 at r3 (raw file):
// This is a *specific* case that has cropped up in the wild that reaches the // unexpected `validateNearestFeatureOfPolytopeBeingEdge` error. This error was // reported in https://github.com/flexible-collision-library/fcl/issues/388
BTW This one could've been labeled with an issue number.
There was a problem hiding this comment.
+@sherm1 for platform review please, thanks!
Reviewable status:
There was a problem hiding this comment.
Reviewable status:
test/test_fcl_signed_distance.cpp, line 168 at r1 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
I'll mark myself as satisfied; but I'll say two last thoughts.
- We could take the scenario, put it in its own issue where it's described in coherent detail and reference it here. It need not be explicitly attributed to private repos.
- We might benefit from a naming convention where all of the "tests from the wild" have some part of the name that references that.
I added the github issue, and referred to it.
test/test_fcl_signed_distance.cpp, line 271 at r3 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW This one could've been labeled with an issue number.
I think we refer to issues in two repos (FCL, libccd) in the test. The URL makes it clear that it is for FCL.
There was a problem hiding this comment.
Reviewed 1 of 1 files at r4.
Reviewable status:
test/test_fcl_signed_distance.cpp, line 271 at r3 (raw file):
Previously, hongkai-dai (Hongkai Dai) wrote…
I think we refer to issues in two repos (FCL, libccd) in the test. The URL makes it clear that it is for FCL.
BTW I meant the test name not the comment: test_distance_box_box_issue_388 (kind of thing).
There was a problem hiding this comment.
Reviewable status:
There was a problem hiding this comment.
Checkpoint.
Reviewed 2 of 3 files at r3.
Reviewable status: all files reviewed, 9 unresolved discussions (waiting on @hongkai-dai)
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 399 at r4 (raw file):
// https://github.com/danfis/libccd/issues/55. ccd_vec3_t origin_projection_unused; dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v,
minor: better to write this
const ccd_real_t dist2 = ccdVec3PointTriDist2(...);(at least use dist2 or dist_sq or whatever for this quantity!)
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 401 at r4 (raw file):
dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, &origin_projection_unused); if (isAbsValueLessThanEpsSquared(dist)){
nit missing space before {
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 495 at r4 (raw file):
// https://github.com/danfis/libccd/issues/55 for an explanation. ccd_vec3_t point_projection_on_triangle_unused; dist_squared = ccdVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v,
Yes! Great variable name. For a minor improvement:
ccd_real_t dist_squared = ccdVec3PointTriDist2(...);include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 756 at r4 (raw file):
__ccdSupport(obj1, obj2, &dir, ccd, &d); ccd_vec3_t point_projection_on_triangle_unused; dist = ccdVec3PointTriDist2(&d.v, &a->v, &b->v, &c->v,
Would be good to make this dist_squared also.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 762 at r4 (raw file):
ccdVec3Scale(&dir, -CCD_ONE); __ccdSupport(obj1, obj2, &dir, ccd, &d2); dist2 = ccdVec3PointTriDist2(&d2.v, &a->v, &b->v, &c->v,
Yikes! Here I think the 2 is just intended to mean a second distance (squared). Very confusing since Dist2 in the function name uses the 2 to mean squared. Please consider renaming this while you're here. Something like dist_squared_opposite?
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1487 at r4 (raw file):
// Compute distance of support point in the support direction, so we can // determine whether we expanded a polytope surrounding the origin a bit. dist = ccdVec3Dot(&out->v, &dir);
minor: consider const ccd_real_t dist = ccdVec3Dot(...) here. This is a real distance not a distance squared.
Then declare ccd_real_t dist_squared prior to the "if" blocks below, with the promise that whatever case we fall into it will set dist_squared properly.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1507 at r4 (raw file):
// check if new point can significantly expand polytope ccd_vec3_t point_projection_on_triangle_unused; dist = ccdVec3PointTriDist2(&out->v, a, b, c,
minor: const ccd_real_t dist_squared?
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1578 at r4 (raw file):
* 2. The two neighbouring faces of that edge are coplanar, and the projection * of the origin onto that plane is on the edge. * Inside this function, we will verify if one of these two conditions are true.
minor: for clarification, consider adding "At times libccd incorrectly claims that the nearest feature is an edge. Inside this function, we will ..."
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1593 at r4 (raw file):
// Only verify the feature if the nearest feature is an edge. ccd_pt_edge_t* nearest_edge =
BTW I think this can be const ccd_pt_edge_t* const nearest edge.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1604 at r4 (raw file):
face_normals[i] = faceNormalPointingOutward(polytope, nearest_edge->faces[i]); ccdVec3Normalize(&(face_normals[i]));
BTW can be &face_normals[i] (without the extra parentheses)
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1606 at r4 (raw file):
ccdVec3Normalize(&(face_normals[i])); // If the origin is on the "inner" side of the face, then // n̂ ⋅ (o - vₑ) ≤ 0 or, with simplification, -n̂ ⋅ vₑ ≤ 0.
BTW took me a while to see why you can drop n̂⋅o here, and I'm still not certain. I think it is because "o" in the local frame is (0,0,0) by definition? Please consider spelling that (or the right reason) out here for us dummies.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1608 at r4 (raw file):
// n̂ ⋅ (o - vₑ) ≤ 0 or, with simplification, -n̂ ⋅ vₑ ≤ 0. origin_to_face_distance[i] = -ccdVec3Dot(&(face_normals[i]), &(nearest_edge->vertex[0]->v.v));
BTW two extra pairs of () on this line.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1627 at r4 (raw file):
nearest_edge->vertex[1]->v.v)) { is_edge_closest_feature = false; }
minor: can break; here as soon as it's false, right?
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1641 at r4 (raw file):
// polytope->nearest_dist stores the SQUARED distance. polytope->nearest_dist = origin_to_face_distance[closest_face] * origin_to_face_distance[closest_face];
BTW consider pow(origin_to_face_distance[closest_face], 2); for readability. I checked in Godbolt -- the generated code is the same either way (it doesn't really call the std::pow() function for an integer exponent of 2).
There was a problem hiding this comment.
Platform
Reviewed 1 of 1 files at r4.
Reviewable status: all files reviewed, 9 unresolved discussions (waiting on @hongkai-dai)
test/test_fcl_signed_distance.cpp, line 239 at r4 (raw file):
ASSERT_NO_THROW(fcl::distance(&cylinder, &box, request, result)); EXPECT_NEAR(std::abs(result.min_distance),
FYI would be better to do using std::abs; above, then just abs() here so that this code would work if S==AutoDiff. But OK as-is since it is only instantiated for double.
test/test_fcl_signed_distance.cpp, line 251 at r4 (raw file):
const Vector3<S> p_BPb = X_WB.inverse() * result.nearest_points[1]; EXPECT_TRUE((p_BPb.array().abs() <= box_size.array() / 2 + 10 * std::numeric_limits<S>::epsilon())
BTW consider
const double tol = 10 * std::numeric_limits<S>::epsilon();to make this easier to read (and like you did below).
test/test_fcl_signed_distance.cpp, line 383 at r4 (raw file):
GTEST_TEST(FCL_SIGNED_DISTANCE, box_box1_ccd) { test_distance_box_box1<double>(); }
BTW awesome test cases!
There was a problem hiding this comment.
Reviewable status:
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 399 at r4 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: better to write this
const ccd_real_t dist2 = ccdVec3PointTriDist2(...);(at least use dist2 or dist_sq or whatever for this quantity!)
Done.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 401 at r4 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
nit missing space before
{
Done.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 495 at r4 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
Yes! Great variable name. For a minor improvement:
ccd_real_t dist_squared = ccdVec3PointTriDist2(...);
Done.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 756 at r4 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
Would be good to make this dist_squared also.
Done.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 762 at r4 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
Yikes! Here I think the 2 is just intended to mean a second distance (squared). Very confusing since Dist2 in the function name uses the 2 to mean squared. Please consider renaming this while you're here. Something like dist_squared_opposite?
Done.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1487 at r4 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: consider
const ccd_real_t dist = ccdVec3Dot(...)here. This is a real distance not a distance squared.Then declare
ccd_real_t dist_squaredprior to the "if" blocks below, with the promise that whatever case we fall into it will set dist_squared properly.
Done.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1507 at r4 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: const ccd_real_t dist_squared?
Done.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1578 at r4 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: for clarification, consider adding "At times libccd incorrectly claims that the nearest feature is an edge. Inside this function, we will ..."
Done.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1593 at r4 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
BTW I think this can be
const ccd_pt_edge_t* const nearest edge.
Done.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1604 at r4 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
BTW can be
&face_normals[i](without the extra parentheses)
Done.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1606 at r4 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
BTW took me a while to see why you can drop
n̂⋅ohere, and I'm still not certain. I think it is because "o" in the local frame is (0,0,0) by definition? Please consider spelling that (or the right reason) out here for us dummies.
Done.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1608 at r4 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
BTW two extra pairs of
()on this line.
Done.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1627 at r4 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
minor: can
break;here as soon as it's false, right?
Done. Good call.
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1641 at r4 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
BTW consider
pow(origin_to_face_distance[closest_face], 2);for readability. I checked in Godbolt -- the generated code is the same either way (it doesn't really call thestd::pow()function for an integer exponent of 2).
Done.
test/test_fcl_signed_distance.cpp, line 271 at r3 (raw file):
Previously, SeanCurtis-TRI (Sean Curtis) wrote…
BTW I meant the test name not the comment:
test_distance_box_box_issue_388(kind of thing).
Now this function contains more than one test cases. I think likely in the future we will add more test cases, so it doesn't just correspond to PR 388.
test/test_fcl_signed_distance.cpp, line 239 at r4 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
FYI would be better to do
using std::abs;above, then just abs() here so that this code would work if S==AutoDiff. But OK as-is since it is only instantiated for double.
Done.
test/test_fcl_signed_distance.cpp, line 251 at r4 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
BTW consider
const double tol = 10 * std::numeric_limits<S>::epsilon();to make this easier to read (and like you did below).
Done.
There was a problem hiding this comment.
Reviewed 2 of 2 files at r5.
Reviewable status:
There was a problem hiding this comment.
Great, thanks Hongkai. All good but I added one more comment I'd missed before.
Reviewed 2 of 2 files at r5.
Reviewable status:
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1637 at r5 (raw file):
// Note origin_to_face_distance is the signed distance. const int closest_face = origin_to_face_distance[0] < origin_to_face_distance[1] ? 1 : 0;
BTW (sorry, should have said this before): I thought this was wrong when I first saw it. But now I realize that origin_to_face_distance is always negative if we get to this point. That's very unusual and it would have helped to have that spelled out right here. Consider adding:
// Note origin_to_face_distance is the *signed* distance and it is
// guaranteed to be negative if we are here, hence sense of this
// comparison is reversed.or something like that.
There was a problem hiding this comment.
Reviewable status:
include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h, line 1637 at r5 (raw file):
Previously, sherm1 (Michael Sherman) wrote…
BTW (sorry, should have said this before): I thought this was wrong when I first saw it. But now I realize that origin_to_face_distance is always negative if we get to this point. That's very unusual and it would have helped to have that spelled out right here. Consider adding:
// Note origin_to_face_distance is the *signed* distance and it is // guaranteed to be negative if we are here, hence sense of this // comparison is reversed.or something like that.
Done.
There was a problem hiding this comment.
Reviewed 1 of 1 files at r6.
Reviewable status:
There was a problem hiding this comment.
Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @hongkai-dai)
test/narrowphase/detail/convexity_based_algorithm/CMakeLists.txt, line 12 at r6 (raw file):
endforeach(test) if(CCD_VERSION VERSION_EQUAL 2.0 OR CCD_VERSION VERSION_GREATER 2.0)
The thought just occurred to me. We know this test is meaningless with libccd 1.5. But it's meaningful for the release tagged as v2.1. What happens with v2.0? Unfortunately, from Cmake's perspective, it can't distinguish between 2.0 and 2.1.
There was a problem hiding this comment.
The latest word for the test is to condition it thusly:
if(TARGET ccd)
add_test(...) # fails with libccd < 2.1
endif()
Note: @jamiesnape added the additional further thought:
Note that would break if danfis/libccd#37 were merged, but that has been sitting there for a year and a half (you would need
if(TARGET ccd OR TARGET ccd::ccd)).
Reviewable status: all files reviewed, 1 unresolved discussion (waiting on @hongkai-dai)
…/fcl into fix_ccd_nearest_edge_feature
Also fixes #319.
This change is