From 2d8d1be484ecfc8fee76c5c77f12b413a58aba19 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Thu, 6 Feb 2020 00:32:51 +0100
Subject: [PATCH] Implement kDOP::overlap with lower bound.
---
include/hpp/fcl/BV/kDOP.h | 15 +++++++--------
src/BV/kDOP.cpp | 24 ++++++++++++++++++++++++
2 files changed, 31 insertions(+), 8 deletions(-)
diff --git a/include/hpp/fcl/BV/kDOP.h b/include/hpp/fcl/BV/kDOP.h
index 7febbac7..2cb8ea52 100644
--- a/include/hpp/fcl/BV/kDOP.h
+++ b/include/hpp/fcl/BV/kDOP.h
@@ -103,16 +103,15 @@ public:
/// @brief Creating kDOP containing two points
KDOP(const Vec3f& a, const Vec3f& b);
- /// @brief Check whether two KDOPs are overlapped
+ /// @brief Check whether two KDOPs overlap.
bool overlap(const KDOP<N>& other) const;
- /// Not implemented
- bool overlap(const KDOP<N>& other, const CollisionRequest&,
- FCL_REAL& sqrDistLowerBound) const
- {
- sqrDistLowerBound = sqrt (-1);
- return overlap (other);
- }
+ /// @brief Check whether two KDOPs overlap.
+ /// @return true if collision happens.
+ /// @retval sqrDistLowerBound squared lower bound on distance between boxes if
+ /// they do not overlap.
+ bool overlap(const KDOP<N>& other, const CollisionRequest& request,
+ FCL_REAL& sqrDistLowerBound) const;
/// @brief The distance between two KDOP<N>. Not implemented.
FCL_REAL distance(const KDOP<N>& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
diff --git a/src/BV/kDOP.cpp b/src/BV/kDOP.cpp
index e3ece6a7..138292f5 100644
--- a/src/BV/kDOP.cpp
+++ b/src/BV/kDOP.cpp
@@ -39,6 +39,8 @@
#include <limits>
#include <iostream>
+#include <hpp/fcl/collision_data.h>
+
namespace hpp
{
namespace fcl
@@ -155,6 +157,28 @@ bool KDOP<N>::overlap(const KDOP<N>& other) const
return true;
}
+template<short N>
+bool KDOP<N>::overlap(const KDOP<N>& other, const CollisionRequest& request,
+ FCL_REAL& sqrDistLowerBound) const
+{
+ const FCL_REAL breakDistance (request.break_distance + request.security_margin);
+
+ FCL_REAL a = (dist_.template head<N/2>() - other.dist_.template tail<N/2>()).minCoeff();
+ if (a > breakDistance) {
+ sqrDistLowerBound = a*a;
+ return false;
+ }
+
+ FCL_REAL b = (other.dist_.template head<N/2>() - dist_.template tail<N/2>()).minCoeff();
+ if (b > breakDistance) {
+ sqrDistLowerBound = b*b;
+ return false;
+ }
+
+ sqrDistLowerBound = std::min(a, b);
+ return true;
+}
+
template<short N>
bool KDOP<N>::inside(const Vec3f& p) const
{
--
GitLab