@@ -260,14 +260,15 @@ class HPP_FCL_DLLAPI OcTreeSolver {
260
260
box.computeLocalAABB ();
261
261
}
262
262
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);
268
267
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 );
271
272
272
273
return drequest->isSatisfied (*dresult);
273
274
} else
@@ -512,8 +513,9 @@ class HPP_FCL_DLLAPI OcTreeSolver {
512
513
513
514
// When reaching this point, `this->solver` has already been set up
514
515
// 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.
517
519
const bool compute_penetration = this ->crequest ->enable_contact ||
518
520
(this ->crequest ->security_margin < 0 );
519
521
const DistanceRequest distanceRequest (compute_penetration,
@@ -524,14 +526,14 @@ class HPP_FCL_DLLAPI OcTreeSolver {
524
526
&box, box_tf, &tri, tf2, this ->solver , distanceRequest,
525
527
distanceResult);
526
528
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 ];
529
531
const Vec3f& normal = distanceResult.normal ;
530
532
const FCL_REAL distToCollision =
531
533
distance - this ->crequest ->security_margin ;
532
534
533
535
internal::updateDistanceLowerBoundFromLeaf (
534
- *crequest, *cresult, distToCollision, c1, c2, normal );
536
+ *( this -> crequest ) , *( this -> cresult ) , distToCollision, c1, c2, normal );
535
537
536
538
if (cresult->numContacts () < crequest->num_max_contacts ) {
537
539
if (distToCollision <= crequest->collision_distance_threshold ) {
@@ -828,16 +830,15 @@ class HPP_FCL_DLLAPI OcTreeSolver {
828
830
box2.computeLocalAABB ();
829
831
}
830
832
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);
837
837
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 );
841
842
842
843
return drequest->isSatisfied (*dresult);
843
844
} else
@@ -945,28 +946,43 @@ class HPP_FCL_DLLAPI OcTreeSolver {
945
946
constructBox (bv1, tf1, box1, box1_tf);
946
947
constructBox (bv2, tf2, box2, box2_tf);
947
948
948
- if (solver->gjk_initial_guess == GJKInitialGuess::BoundingVolumeGuess) {
949
+ if (this ->solver ->gjk_initial_guess ==
950
+ GJKInitialGuess::BoundingVolumeGuess) {
949
951
box1.computeLocalAABB ();
950
952
box2.computeLocalAABB ();
951
953
}
952
954
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;
960
965
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 (
964
982
Contact (tree1, tree2, static_cast <int >(root1 - tree1->getRoot ()),
965
983
static_cast <int >(root2 - tree2->getRoot ()), c1, c2,
966
984
normal , distance));
967
985
}
968
- internal::updateDistanceLowerBoundFromLeaf (
969
- *crequest, *cresult, distToCollision, c1, c2, normal );
970
986
971
987
return crequest->isSatisfied (*cresult);
972
988
}
0 commit comments