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

Nan value in the nearest points when using distance() #293

Closed
tongtybj opened this issue May 28, 2018 · 2 comments
Closed

Nan value in the nearest points when using distance() #293

tongtybj opened this issue May 28, 2018 · 2 comments

Comments

@tongtybj
Copy link

While doing distance check for two boxes, I found nan values in the nearest points after using distance().
One of the sample is as follows, and two boxes do not collide.

   DistanceResult<double> result;
   result.clear();
   DistanceRequest<double> request(true);

   std::shared_ptr< CollisionGeometry<double> > cgeomBox1(new Box<double>(2.750000, 6.000000, 0.050000));
   std::shared_ptr< CollisionGeometry<double> > cgeomBox2(new Box<double>(0.424000, 0.150000, 0.168600));
   CollisionObject<double> objBox1(cgeomBox1, Eigen::Quaterniond(1,0,0,0).matrix(), Eigen::Vector3d(1.625000, 0.000000, 0.500000));
   CollisionObject<double> objBox2(cgeomBox2, Eigen::Quaterniond(0.672811, 0.340674, 0.155066, 0.638138).matrix(), Eigen::Vector3d(0.192074, -0.277870, 0.273546));

   fcl::distance(&objBox1, &objBox2, request, result);
   std::cout << "distance = " << result.min_distance << std::endl;
   std::cout << " point on obj1: x = " << result.nearest_points[0](0) << " y = " << result.nearest_points[0](1) << " z = " << result.nearest_points[0](2) << std::endl;
   std::cout << " point on obj2: x = " << result.nearest_points[1](0) << " y = " << result.nearest_points[1](1) << " z = " << result.nearest_points[1](2) << std::endl;

Can someone help me to figure out the reason, or give me some hint about the algorithm of distance calculation between two boxes?

@SeanCurtis-TRI
Copy link
Contributor

Thanks for the detailed reproduction info. I'll look into this and get back to you.

@tongtybj
Copy link
Author

@SeanCurtis-TRI
Thank you for your response.

What I have found is that the normal n is zero, whiling processing the function extractClosestPoints, and this is caused since vector AB and AC are parallel. Right now, I am studying about the algorithm to figure out why triangle ABC is corrupt.

SeanCurtis-TRI added a commit to SeanCurtis-TRI/fcl that referenced this issue Jun 1, 2018
If the simplex is degenerate, it can lead to divide-by-zero errors. This
test is drawn from the real world and exposes such a problem - nearest
points are returned as NaN. See
flexible-collision-library#293
for the discussion
SeanCurtis-TRI added a commit to SeanCurtis-TRI/fcl that referenced this issue Jun 8, 2018
If the simplex is degenerate, it can lead to divide-by-zero errors. This
test is drawn from the real world and exposes such a problem - nearest
points are returned as NaN. See
flexible-collision-library#293
for the discussion
sherm1 pushed a commit that referenced this issue Jun 19, 2018
…mplex (#296)

* Expose numerical errors in gjk_libccd-inl.h extractClosestPoints()

If the simplex is degenerate, it can lead to divide-by-zero errors. This
test is drawn from the real world and exposes such a problem - nearest
points are returned as NaN. See
#293
for the discussion

* Make extractClosestPoints more robust

For simplex of size n, if the simplex is degenerate, extracts from a
simplex of size n - 1 (to prevent returning NaN).

1. Refactor point extraction into independently callable methods.
2. At each level, add method for downgrading the simplex.
3. Add unit tests on the local methods.
4. Add integration test with motivating example.
5. Update documentation of this implementation.
nicovanduijn pushed a commit to nicovanduijn/fcl that referenced this issue Jul 5, 2018
…mplex (flexible-collision-library#296)

* Expose numerical errors in gjk_libccd-inl.h extractClosestPoints()

If the simplex is degenerate, it can lead to divide-by-zero errors. This
test is drawn from the real world and exposes such a problem - nearest
points are returned as NaN. See
flexible-collision-library#293
for the discussion

* Make extractClosestPoints more robust

For simplex of size n, if the simplex is degenerate, extracts from a
simplex of size n - 1 (to prevent returning NaN).

1. Refactor point extraction into independently callable methods.
2. At each level, add method for downgrading the simplex.
3. Add unit tests on the local methods.
4. Add integration test with motivating example.
5. Update documentation of this implementation.
Sign up for free to join this conversation on GitHub. Already have an account? Sign in to comment
Labels
None yet
Projects
None yet
Development

No branches or pull requests

2 participants