From 2f7ddb4d520ddc20bf0a26db323f502cdcc40120 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Tue, 27 Aug 2019 14:10:06 +0200
Subject: [PATCH] Implement AABB::overlap with distance lower bound.
---
include/hpp/fcl/BV/AABB.h | 8 ++------
src/BV/AABB.cpp | 17 ++++++++++++++++-
2 files changed, 18 insertions(+), 7 deletions(-)
diff --git a/include/hpp/fcl/BV/AABB.h b/include/hpp/fcl/BV/AABB.h
index 0b93c48a..a67aeece 100644
--- a/include/hpp/fcl/BV/AABB.h
+++ b/include/hpp/fcl/BV/AABB.h
@@ -97,12 +97,8 @@ public:
}
/// Not implemented
- inline bool overlap(const AABB& other, const CollisionRequest&,
- FCL_REAL& sqrDistLowerBound) const
- {
- sqrDistLowerBound = sqrt (-1);
- return overlap (other);
- }
+ bool overlap(const AABB& other, const CollisionRequest& request,
+ FCL_REAL& sqrDistLowerBound) const;
/// @brief Check whether the AABB contains another AABB
inline bool contain(const AABB& other) const
diff --git a/src/BV/AABB.cpp b/src/BV/AABB.cpp
index 782e0bc1..e9829aae 100644
--- a/src/BV/AABB.cpp
+++ b/src/BV/AABB.cpp
@@ -38,7 +38,7 @@
#include <hpp/fcl/BV/AABB.h>
#include <limits>
-#include <iostream>
+#include <hpp/fcl/collision_data.h>
namespace hpp
{
@@ -50,6 +50,21 @@ AABB::AABB() : min_(Vec3f::Constant(std::numeric_limits<FCL_REAL>::max())),
{
}
+bool AABB::overlap(const AABB& other, const CollisionRequest& request,
+ FCL_REAL& sqrDistLowerBound) const
+{
+ const FCL_REAL breakDistance (request.break_distance + request.security_margin);
+ const FCL_REAL breakDistance2 = breakDistance * breakDistance;
+
+ sqrDistLowerBound = (min_ - other.max_).array().max(0).matrix().squaredNorm();
+ if(sqrDistLowerBound > breakDistance2) return false;
+
+ sqrDistLowerBound = (other.min_ - max_).array().max(0).matrix().squaredNorm();
+ if(sqrDistLowerBound > breakDistance2) return false;
+
+ return true;
+}
+
FCL_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const
{
FCL_REAL result = 0;
--
GitLab