From afa5983227236419adcaa04110ab47f9b74689b3 Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Thu, 13 Jun 2019 09:57:01 +0200
Subject: [PATCH] Add missing overlap method.
---
include/hpp/fcl/BV/AABB.h | 8 ++++++++
include/hpp/fcl/BV/RSS.h | 4 ++++
include/hpp/fcl/BV/kDOP.h | 16 +++++++++++++++-
include/hpp/fcl/BV/kIOS.h | 6 ++++++
src/BV/AABB.cpp | 14 ++++++++++++++
src/BV/RSS.cpp | 19 +++++++++++++++++++
src/BV/kIOS.cpp | 39 +++++++++++++++++++++++++++++++++------
7 files changed, 99 insertions(+), 7 deletions(-)
diff --git a/include/hpp/fcl/BV/AABB.h b/include/hpp/fcl/BV/AABB.h
index b875b3c8..0b93c48a 100644
--- a/include/hpp/fcl/BV/AABB.h
+++ b/include/hpp/fcl/BV/AABB.h
@@ -262,6 +262,14 @@ static inline AABB rotate(const AABB& aabb, const Matrix3f& t)
return res;
}
+/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity.
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, const AABB& b2);
+
+/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity.
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
+ const AABB& b2, const CollisionRequest& request,
+ FCL_REAL& sqrDistLowerBound);
+
}
} // namespace hpp
diff --git a/include/hpp/fcl/BV/RSS.h b/include/hpp/fcl/BV/RSS.h
index 3b33b280..6822e97e 100644
--- a/include/hpp/fcl/BV/RSS.h
+++ b/include/hpp/fcl/BV/RSS.h
@@ -155,6 +155,10 @@ FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS&
/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity.
bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2);
+/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity.
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2,
+ const CollisionRequest& request,
+ FCL_REAL& sqrDistLowerBound);
}
diff --git a/include/hpp/fcl/BV/kDOP.h b/include/hpp/fcl/BV/kDOP.h
index 72be7f89..da590162 100644
--- a/include/hpp/fcl/BV/kDOP.h
+++ b/include/hpp/fcl/BV/kDOP.h
@@ -39,7 +39,7 @@
#define HPP_FCL_KDOP_H
#include <stdexcept>
-#include <hpp/fcl/math/vec_3f.h>
+#include <hpp/fcl/math/matrix_3f.h>
namespace hpp
{
@@ -176,6 +176,20 @@ public:
};
+template<size_t N>
+bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/,
+ const KDOP<N>& /*b1*/, const KDOP<N>& /*b2*/)
+{
+ throw std::logic_error ("not implemented");
+}
+
+template<size_t N>
+bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/,
+ const KDOP<N>& /*b1*/, const KDOP<N>& /*b2*/,
+ const CollisionRequest& /*request*/, FCL_REAL& /*sqrDistLowerBound*/)
+{
+ throw std::logic_error ("not implemented");
+}
/// @brief translate the KDOP BV
template<size_t N>
diff --git a/include/hpp/fcl/BV/kIOS.h b/include/hpp/fcl/BV/kIOS.h
index 64550f38..a7228168 100644
--- a/include/hpp/fcl/BV/kIOS.h
+++ b/include/hpp/fcl/BV/kIOS.h
@@ -157,6 +157,12 @@ kIOS translate(const kIOS& bv, const Vec3f& t);
/// @todo Not efficient
bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2);
+/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) and b2 is in identity.
+/// @todo Not efficient
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2,
+ const CollisionRequest& request,
+ FCL_REAL& sqrDistLowerBound);
+
/// @brief Approximate distance between two kIOS bounding volumes
/// @todo P and Q is not returned, need implementation
FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
diff --git a/src/BV/AABB.cpp b/src/BV/AABB.cpp
index e65bee4b..782e0bc1 100644
--- a/src/BV/AABB.cpp
+++ b/src/BV/AABB.cpp
@@ -128,6 +128,20 @@ FCL_REAL AABB::distance(const AABB& other) const
return std::sqrt(result);
}
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, const AABB& b2)
+{
+ AABB bb1 (translate (rotate (b1, R0), T0));
+ return bb1.overlap (b2);
+}
+
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
+ const AABB& b2, const CollisionRequest& request,
+ FCL_REAL& sqrDistLowerBound)
+{
+ AABB bb1 (translate (rotate (b1, R0), T0));
+ return bb1.overlap (b2, request, sqrDistLowerBound);
+}
+
}
} // namespace hpp
diff --git a/src/BV/RSS.cpp b/src/BV/RSS.cpp
index 7ef7cc7b..412b9f00 100644
--- a/src/BV/RSS.cpp
+++ b/src/BV/RSS.cpp
@@ -862,6 +862,25 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2)
return (dist <= (b1.r + b2.r));
}
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2,
+ const CollisionRequest& request,
+ FCL_REAL& sqrDistLowerBound)
+{
+ // ROb2 = R0 . b2
+ // where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ]
+
+ // (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0]
+ // R = b2^T RO^T b1
+ Vec3f Ttemp (R0 * b2.Tr + T0 - b1.Tr);
+ Vec3f T(b1.axes.transpose() * Ttemp);
+ Matrix3f R(b1.axes.transpose() * R0 * b2.axes);
+
+ FCL_REAL dist = rectDistance(R, T, b1.l, b2.l) - b1.r - b2.r;
+ if (dist <= 0) return true;
+ sqrDistLowerBound = dist * dist;
+ return false;
+}
+
bool RSS::contain(const Vec3f& p) const
{
Vec3f local_p = p - Tr;
diff --git a/src/BV/kIOS.cpp b/src/BV/kIOS.cpp
index e6c267a8..010966e3 100644
--- a/src/BV/kIOS.cpp
+++ b/src/BV/kIOS.cpp
@@ -61,17 +61,27 @@ bool kIOS::overlap(const kIOS& other) const
}
return obb.overlap(other.obb);
-
- return true;
}
- bool kIOS::overlap(const kIOS& other, const CollisionRequest&,
- FCL_REAL& sqrDistLowerBound) const
+bool kIOS::overlap(const kIOS& other, const CollisionRequest& request,
+ FCL_REAL& sqrDistLowerBound) const
+{
+ for(unsigned int i = 0; i < num_spheres; ++i)
{
- sqrDistLowerBound = sqrt (-1);
- return overlap (other);
+ for(unsigned int j = 0; j < other.num_spheres; ++j)
+ {
+ FCL_REAL o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm();
+ FCL_REAL sum_r = spheres[i].r + other.spheres[j].r;
+ if(o_dist > sum_r * sum_r) {
+ o_dist = sqrt(o_dist) - sum_r;
+ sqrDistLowerBound = o_dist*o_dist;
+ return false;
+ }
+ }
}
+ return obb.overlap(other.obb, request, sqrDistLowerBound);
+}
bool kIOS::contain(const Vec3f& p) const
{
@@ -192,6 +202,23 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2
return b1.overlap(b2_temp);
}
+bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2,
+ const CollisionRequest& request,
+ FCL_REAL& sqrDistLowerBound)
+{
+ kIOS b2_temp = b2;
+ for(unsigned int i = 0; i < b2_temp.num_spheres; ++i)
+ {
+ b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0;
+ }
+
+
+ b2_temp.obb.To = R0 * b2_temp.obb.To + T0;
+ b2_temp.obb.axes.applyOnTheLeft(R0);
+
+ return b1.overlap(b2_temp, request, sqrDistLowerBound);
+}
+
FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P, Vec3f* Q)
{
kIOS b2_temp = b2;
--
GitLab