Skip to content

Commit 8cec4df

Browse files
committed
Octree: use ShapeShapeDistance instead of GJKSolver
1 parent bb1d826 commit 8cec4df

File tree

2 files changed

+53
-36
lines changed

2 files changed

+53
-36
lines changed

include/hpp/fcl/internal/traversal_node_bvh_shape.h

+3-2
Original file line numberDiff line numberDiff line change
@@ -149,8 +149,9 @@ class MeshShapeCollisionTraversalNode
149149

150150
// When reaching this point, `this->solver` has already been set up
151151
// by the CollisionRequest `this->request`.
152-
// The only thing we need to pass to `ShapeShapeDistance` is whether or
153-
// not penetration information is should be computed in case of collision.
152+
// The only thing we need to (and can) pass to `ShapeShapeDistance` is
153+
// whether or not penetration information is should be computed in case of
154+
// collision.
154155
const bool compute_penetration =
155156
this->request.enable_contact || (this->request.security_margin < 0);
156157
const DistanceRequest distanceRequest(compute_penetration,

include/hpp/fcl/internal/traversal_node_octree.h

+50-34
Original file line numberDiff line numberDiff line change
@@ -260,14 +260,15 @@ class HPP_FCL_DLLAPI OcTreeSolver {
260260
box.computeLocalAABB();
261261
}
262262

263-
FCL_REAL dist;
264-
Vec3f closest_p1, closest_p2, normal;
265-
bool compute_penetration = drequest->enable_signed_distance;
266-
solver->shapeDistance(box, box_tf, s, tf2, dist, compute_penetration,
267-
closest_p1, closest_p2, normal);
263+
DistanceResult distanceResult;
264+
const FCL_REAL distance =
265+
ShapeShapeDistance<Box, S>(&box, box_tf, &s, tf2, this->solver,
266+
*(this->drequest), distanceResult);
268267

269-
dresult->update(dist, tree1, &s, (int)(root1 - tree1->getRoot()),
270-
DistanceResult::NONE, closest_p1, closest_p2, normal);
268+
this->dresult->update(
269+
distance, tree1, &s, (int)(root1 - tree1->getRoot()),
270+
DistanceResult::NONE, distanceResult.nearest_points[0],
271+
distanceResult.nearest_points[1], distanceResult.normal);
271272

272273
return drequest->isSatisfied(*dresult);
273274
} else
@@ -512,8 +513,9 @@ class HPP_FCL_DLLAPI OcTreeSolver {
512513

513514
// When reaching this point, `this->solver` has already been set up
514515
// by the CollisionRequest `this->crequest`.
515-
// The only thing we need to pass to `ShapeShapeDistance` is whether or
516-
// not penetration information is should be computed in case of collision.
516+
// The only thing we need to (and can) pass to `ShapeShapeDistance` is
517+
// whether or not penetration information is should be computed in case of
518+
// collision.
517519
const bool compute_penetration = this->crequest->enable_contact ||
518520
(this->crequest->security_margin < 0);
519521
const DistanceRequest distanceRequest(compute_penetration,
@@ -524,14 +526,14 @@ class HPP_FCL_DLLAPI OcTreeSolver {
524526
&box, box_tf, &tri, tf2, this->solver, distanceRequest,
525527
distanceResult);
526528

527-
const Vec3f& c1 = distanceResult.nearest_points[0]; // cp of triangle
528-
const Vec3f& c2 = distanceResult.nearest_points[1]; // cp of shape
529+
const Vec3f& c1 = distanceResult.nearest_points[0];
530+
const Vec3f& c2 = distanceResult.nearest_points[1];
529531
const Vec3f& normal = distanceResult.normal;
530532
const FCL_REAL distToCollision =
531533
distance - this->crequest->security_margin;
532534

533535
internal::updateDistanceLowerBoundFromLeaf(
534-
*crequest, *cresult, distToCollision, c1, c2, normal);
536+
*(this->crequest), *(this->cresult), distToCollision, c1, c2, normal);
535537

536538
if (cresult->numContacts() < crequest->num_max_contacts) {
537539
if (distToCollision <= crequest->collision_distance_threshold) {
@@ -828,16 +830,15 @@ class HPP_FCL_DLLAPI OcTreeSolver {
828830
box2.computeLocalAABB();
829831
}
830832

831-
FCL_REAL dist;
832-
Vec3f closest_p1, closest_p2, normal;
833-
bool compute_penetration = drequest->enable_signed_distance;
834-
solver->shapeDistance(box1, box1_tf, box2, box2_tf, dist,
835-
compute_penetration, closest_p1, closest_p2,
836-
normal);
833+
DistanceResult distanceResult;
834+
const FCL_REAL distance = ShapeShapeDistance<Box, Box>(
835+
&box1, box1_tf, &box2, box2_tf, this->solver, *(this->drequest),
836+
distanceResult);
837837

838-
dresult->update(dist, tree1, tree2, (int)(root1 - tree1->getRoot()),
839-
(int)(root2 - tree2->getRoot()), closest_p1, closest_p2,
840-
normal);
838+
this->dresult->update(
839+
distance, tree1, tree2, (int)(root1 - tree1->getRoot()),
840+
(int)(root2 - tree2->getRoot()), distanceResult.nearest_points[0],
841+
distanceResult.nearest_points[1], distanceResult.normal);
841842

842843
return drequest->isSatisfied(*dresult);
843844
} else
@@ -945,28 +946,43 @@ class HPP_FCL_DLLAPI OcTreeSolver {
945946
constructBox(bv1, tf1, box1, box1_tf);
946947
constructBox(bv2, tf2, box2, box2_tf);
947948

948-
if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
949+
if (this->solver->gjk_initial_guess ==
950+
GJKInitialGuess::BoundingVolumeGuess) {
949951
box1.computeLocalAABB();
950952
box2.computeLocalAABB();
951953
}
952954

953-
FCL_REAL distance;
954-
Vec3f c1, c2, normal;
955-
bool compute_penetration =
956-
(crequest->enable_contact || crequest->security_margin < 0);
957-
solver->shapeDistance(box1, box1_tf, box2, box2_tf, distance,
958-
compute_penetration, c1, c2, normal);
959-
FCL_REAL distToCollision = distance - crequest->security_margin;
955+
// When reaching this point, `this->solver` has already been set up
956+
// by the CollisionRequest `this->request`.
957+
// The only thing we need to (and can) pass to `ShapeShapeDistance` is
958+
// whether or not penetration information is should be computed in case of
959+
// collision.
960+
const bool compute_penetration = (this->crequest->enable_contact ||
961+
(this->crequest->security_margin < 0));
962+
const DistanceRequest distanceRequest(compute_penetration,
963+
compute_penetration);
964+
DistanceResult distanceResult;
960965

961-
if (cresult->numContacts() < crequest->num_max_contacts) {
962-
if (distToCollision <= crequest->collision_distance_threshold)
963-
cresult->addContact(
966+
FCL_REAL distance = ShapeShapeDistance<Box, Box>(
967+
&box1, box1_tf, &box2, box2_tf, this->solver, distanceRequest,
968+
distanceResult);
969+
970+
const Vec3f& c1 = distanceResult.nearest_points[0];
971+
const Vec3f& c2 = distanceResult.nearest_points[1];
972+
const Vec3f& normal = distanceResult.normal;
973+
const FCL_REAL distToCollision =
974+
distance - this->crequest->security_margin;
975+
976+
internal::updateDistanceLowerBoundFromLeaf(
977+
*(this->crequest), *(this->cresult), distToCollision, c1, c2, normal);
978+
979+
if (this->cresult->numContacts() < this->crequest->num_max_contacts) {
980+
if (distToCollision <= this->crequest->collision_distance_threshold)
981+
this->cresult->addContact(
964982
Contact(tree1, tree2, static_cast<int>(root1 - tree1->getRoot()),
965983
static_cast<int>(root2 - tree2->getRoot()), c1, c2,
966984
normal, distance));
967985
}
968-
internal::updateDistanceLowerBoundFromLeaf(
969-
*crequest, *cresult, distToCollision, c1, c2, normal);
970986

971987
return crequest->isSatisfied(*cresult);
972988
}

0 commit comments

Comments
 (0)