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.

Sign up for GitHub

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

Jump to bottom

use the actual distance instead of the squared distance to determine if the origin is on the facet of the simplex. #365

Merged
merged 3 commits into from
Jan 29, 2019
Merged

use the actual distance instead of the squared distance to determine if the origin is on the facet of the simplex. #365

Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
4612a0b
use the actual distance instead of the squared distance to determine …
hongkai-dai Jan 29, 2019
f002080
Address Sean and Sherm's comments.
hongkai-dai Jan 29, 2019
fe383ee
Address Sherm's comment.
hongkai-dai Jan 29, 2019
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
35 changes: 20 additions & 15 deletions include/fcl/narrowphase/detail/convexity_based_algorithm/gjk_libccd-inl.h
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -348,13 +348,18 @@ static int doSimplex3(ccd_simplex_t *simplex, ccd_vec3_t *dir)
return 0;
}

static bool isAbsValueLessThanEpsSquared(ccd_real_t val) {
return std::abs(val) < std::numeric_limits<ccd_real_t>::epsilon() *
std::numeric_limits<ccd_real_t>::epsilon();
}

static int doSimplex4(ccd_simplex_t *simplex, ccd_vec3_t *dir)
{
const ccd_support_t *A, *B, *C, *D;
ccd_vec3_t AO, AB, AC, AD, ABC, ACD, ADB;
int B_on_ACD, C_on_ADB, D_on_ABC;
int AB_O, AC_O, AD_O;
ccd_real_t dist;
ccd_real_t dist_squared;

// get last added as A
A = ccdSimplexLast(simplex);
Expand All @@ -366,25 +371,25 @@ static int doSimplex4(ccd_simplex_t *simplex, ccd_vec3_t *dir)
// check if tetrahedron is really tetrahedron (has volume > 0)
// if it is not simplex can't be expanded and thus no intersection is
// found
dist = ccdVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, nullptr);
if (ccdIsZero(dist)){
dist_squared = ccdVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, nullptr);
if (isAbsValueLessThanEpsSquared(dist_squared)) {
return -1;
}

// check if origin lies on some of tetrahedron's face - if so objects
// intersect
dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, nullptr);
if (ccdIsZero(dist))
return 1;
dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &C->v, &D->v, nullptr);
if (ccdIsZero(dist))
return 1;
dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &D->v, nullptr);
if (ccdIsZero(dist))
return 1;
dist = ccdVec3PointTriDist2(ccd_vec3_origin, &B->v, &C->v, &D->v, nullptr);
if (ccdIsZero(dist))
return 1;
dist_squared =
ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, nullptr);
if (isAbsValueLessThanEpsSquared((dist_squared))) return 1;
dist_squared =
ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &C->v, &D->v, nullptr);
if (isAbsValueLessThanEpsSquared((dist_squared))) return 1;
dist_squared =
ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &D->v, nullptr);
if (isAbsValueLessThanEpsSquared(dist_squared)) return 1;
dist_squared =
ccdVec3PointTriDist2(ccd_vec3_origin, &B->v, &C->v, &D->v, nullptr);
if (isAbsValueLessThanEpsSquared(dist_squared)) return 1;

// compute AO, AB, AC, AD segments and ABC, ACD, ADB normal vectors
ccdVec3Copy(&AO, &A->v);
Expand Down