From 8754b2337394c9173009816bd239aa8d7e07cdee Mon Sep 17 00:00:00 2001
From: Joseph Mirabel <jmirabel@laas.fr>
Date: Wed, 5 Feb 2020 22:50:58 +0100
Subject: [PATCH] Vectorize kDOP
---
include/hpp/fcl/BV/BV_node.h | 4 ++
include/hpp/fcl/BV/kDOP.h | 20 ++++---
src/BV/kDOP.cpp | 83 +++++++++++---------------
src/shape/geometric_shapes_utility.cpp | 36 +++++------
test/collision.cpp | 2 +-
5 files changed, 70 insertions(+), 75 deletions(-)
diff --git a/include/hpp/fcl/BV/BV_node.h b/include/hpp/fcl/BV/BV_node.h
index 13e68127..5f83add9 100644
--- a/include/hpp/fcl/BV/BV_node.h
+++ b/include/hpp/fcl/BV/BV_node.h
@@ -116,6 +116,10 @@ struct BVNode : public BVNodeBase
static const Matrix3f id3 = Matrix3f::Identity();
return id3;
}
+
+ /// \cond
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ /// \endcond
};
template<>
diff --git a/include/hpp/fcl/BV/kDOP.h b/include/hpp/fcl/BV/kDOP.h
index 3293be71..7febbac7 100644
--- a/include/hpp/fcl/BV/kDOP.h
+++ b/include/hpp/fcl/BV/kDOP.h
@@ -85,12 +85,12 @@ struct CollisionRequest;
/// (-1, -1, 1) and (1, 1, -1) --> indices 9 and 21
/// (-1, 1, -1) and (1, -1, 1) --> indices 10 and 22
/// (1, -1, -1) and (-1, 1, 1) --> indices 11 and 23
-template<size_t N>
+template<short N>
class KDOP
{
private:
/// @brief Origin's distances to N KDOP planes
- FCL_REAL dist_[N];
+ Eigen::Array<FCL_REAL, N, 1> dist_;
public:
@@ -135,7 +135,7 @@ public:
/// @brief The (AABB) center
inline Vec3f center() const
{
- return Vec3f(dist_[0] + dist_[N / 2], dist_[1] + dist_[N / 2 + 1], dist_[2] + dist_[N / 2 + 2]) * 0.5;
+ return (dist_.template head<3>() + dist_.template segment<3>(N/2)) / 2;
}
@@ -163,12 +163,12 @@ public:
return width() * height() * depth();
}
- inline FCL_REAL dist(std::size_t i) const
+ inline FCL_REAL dist(short i) const
{
return dist_[i];
}
- inline FCL_REAL& dist(std::size_t i)
+ inline FCL_REAL& dist(short i)
{
return dist_[i];
}
@@ -176,16 +176,20 @@ public:
//// @brief Check whether one point is inside the KDOP
bool inside(const Vec3f& p) const;
+ public:
+ /// \cond
+ EIGEN_MAKE_ALIGNED_OPERATOR_NEW
+ /// \endcond
};
-template<size_t N>
+template<short 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>
+template<short N>
bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/,
const KDOP<N>& /*b1*/, const KDOP<N>& /*b2*/,
const CollisionRequest& /*request*/, FCL_REAL& /*sqrDistLowerBound*/)
@@ -194,7 +198,7 @@ bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/,
}
/// @brief translate the KDOP BV
-template<size_t N>
+template<short N>
KDOP<N> translate(const KDOP<N>& bv, const Vec3f& t);
}
diff --git a/src/BV/kDOP.cpp b/src/BV/kDOP.cpp
index 7959bff0..e3ece6a7 100644
--- a/src/BV/kDOP.cpp
+++ b/src/BV/kDOP.cpp
@@ -67,7 +67,7 @@ inline void minmax(FCL_REAL p, FCL_REAL& minv, FCL_REAL& maxv)
/// @brief Compute the distances to planes with normals from KDOP vectors except those of AABB face planes
-template<std::size_t N>
+template<short N>
void getDistances(const Vec3f& p, FCL_REAL* d) {}
/// @brief Specification of getDistances
@@ -106,39 +106,34 @@ inline void getDistances<9>(const Vec3f& p, FCL_REAL* d)
d[8] = p[1] + p[2] - p[0];
}
-
-
-template<size_t N>
+template<short N>
KDOP<N>::KDOP()
{
FCL_REAL real_max = std::numeric_limits<FCL_REAL>::max();
- for(size_t i = 0; i < N / 2; ++i)
- {
- dist_[i] = real_max;
- dist_[i + N / 2] = -real_max;
- }
+ dist_.template head<N/2>() = real_max;
+ dist_.template tail<N/2>() = -real_max;
}
-template<size_t N>
+template<short N>
KDOP<N>::KDOP(const Vec3f& v)
{
- for(size_t i = 0; i < 3; ++i)
+ for(short i = 0; i < 3; ++i)
{
dist_[i] = dist_[N / 2 + i] = v[i];
}
FCL_REAL d[(N - 6) / 2];
getDistances<(N - 6) / 2>(v, d);
- for(size_t i = 0; i < (N - 6) / 2; ++i)
+ for(short i = 0; i < (N - 6) / 2; ++i)
{
dist_[3 + i] = dist_[3 + i + N / 2] = d[i];
}
}
-template<size_t N>
+template<short N>
KDOP<N>::KDOP(const Vec3f& a, const Vec3f& b)
{
- for(size_t i = 0; i < 3; ++i)
+ for(short i = 0; i < 3; ++i)
{
minmax(a[i], b[i], dist_[i], dist_[i + N / 2]);
}
@@ -146,55 +141,47 @@ KDOP<N>::KDOP(const Vec3f& a, const Vec3f& b)
FCL_REAL ad[(N - 6) / 2], bd[(N - 6) / 2];
getDistances<(N - 6) / 2>(a, ad);
getDistances<(N - 6) / 2>(b, bd);
- for(size_t i = 0; i < (N - 6) / 2; ++i)
+ for(short i = 0; i < (N - 6) / 2; ++i)
{
minmax(ad[i], bd[i], dist_[3 + i], dist_[3 + i + N / 2]);
}
}
-template<size_t N>
+template<short N>
bool KDOP<N>::overlap(const KDOP<N>& other) const
{
- for(size_t i = 0; i < N / 2; ++i)
- {
- if(dist_[i] > other.dist_[i + N / 2]) return false;
- if(dist_[i + N / 2] < other.dist_[i]) return false;
- }
-
+ if ((dist_.template head<N/2>() > other.dist_.template tail<N/2>()).any()) return false;
+ if ((dist_.template tail<N/2>() < other.dist_.template head<N/2>()).any()) return false;
return true;
}
-template<size_t N>
+template<short N>
bool KDOP<N>::inside(const Vec3f& p) const
{
- for(size_t i = 0; i < 3; ++i)
- {
- if(p[i] < dist_[i] || p[i] > dist_[i + N / 2])
- return false;
- }
+ if ((p.array() < dist_.template head<3>()).any()) return false;
+ if ((p.array() > dist_.template segment<3>(N/2)).any()) return false;
- FCL_REAL d[(N - 6) / 2];
- getDistances<(N - 6) / 2>(p, d);
- for(size_t i = 0; i < (N - 6) / 2; ++i)
- {
- if(d[i] < dist_[3 + i] || d[i] > dist_[i + 3 + N / 2])
- return false;
- }
+ enum { P = ((N-6)/2) };
+ Eigen::Array<FCL_REAL, P, 1> d;
+ getDistances<P>(p, d.data());
+
+ if ((d < dist_.template segment<P>(3 )).any()) return false;
+ if ((d > dist_.template segment<P>(3+N/2)).any()) return false;
return true;
}
-template<size_t N>
+template<short N>
KDOP<N>& KDOP<N>::operator += (const Vec3f& p)
{
- for(size_t i = 0; i < 3; ++i)
+ for(short i = 0; i < 3; ++i)
{
minmax(p[i], dist_[i], dist_[N / 2 + i]);
}
FCL_REAL pd[(N - 6) / 2];
getDistances<(N - 6) / 2>(p, pd);
- for(size_t i = 0; i < (N - 6) / 2; ++i)
+ for(short i = 0; i < (N - 6) / 2; ++i)
{
minmax(pd[i], dist_[3 + i], dist_[3 + N / 2 + i]);
}
@@ -202,10 +189,10 @@ KDOP<N>& KDOP<N>::operator += (const Vec3f& p)
return *this;
}
-template<size_t N>
+template<short N>
KDOP<N>& KDOP<N>::operator += (const KDOP<N>& other)
{
- for(size_t i = 0; i < N / 2; ++i)
+ for(short i = 0; i < N / 2; ++i)
{
dist_[i] = std::min(other.dist_[i], dist_[i]);
dist_[i + N / 2] = std::max(other.dist_[i + N / 2], dist_[i + N / 2]);
@@ -213,7 +200,7 @@ KDOP<N>& KDOP<N>::operator += (const KDOP<N>& other)
return *this;
}
-template<size_t N>
+template<short N>
KDOP<N> KDOP<N>::operator + (const KDOP<N>& other) const
{
KDOP<N> res(*this);
@@ -221,7 +208,7 @@ KDOP<N> KDOP<N>::operator + (const KDOP<N>& other) const
}
-template<size_t N>
+template<short N>
FCL_REAL KDOP<N>::distance(const KDOP<N>& /*other*/, Vec3f* /*P*/, Vec3f* /*Q*/) const
{
std::cerr << "KDOP distance not implemented!" << std::endl;
@@ -229,22 +216,22 @@ FCL_REAL KDOP<N>::distance(const KDOP<N>& /*other*/, Vec3f* /*P*/, Vec3f* /*Q*/)
}
-template<size_t N>
+template<short N>
KDOP<N> translate(const KDOP<N>& bv, const Vec3f& t)
{
KDOP<N> res(bv);
- for(size_t i = 0; i < 3; ++i)
+ for(short i = 0; i < 3; ++i)
{
res.dist(i) += t[i];
- res.dist(N / 2 + i) += t[i];
+ res.dist(short(N / 2 + i)) += t[i];
}
FCL_REAL d[(N - 6) / 2];
getDistances<(N - 6) / 2>(t, d);
- for(size_t i = 0; i < (N - 6) / 2; ++i)
+ for(short i = 0; i < (N - 6) / 2; ++i)
{
- res.dist(3 + i) += d[i];
- res.dist(3 + i + N / 2) += d[i];
+ res.dist(short(3 + i)) += d[i];
+ res.dist(short(3 + i + N / 2)) += d[i];
}
return res;
diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp
index c0aba492..bea9e11e 100644
--- a/src/shape/geometric_shapes_utility.cpp
+++ b/src/shape/geometric_shapes_utility.cpp
@@ -508,10 +508,10 @@ void computeBV<KDOP<16>, Halfspace>(const Halfspace& s, const Transform3f& tf, K
const Vec3f& n = new_s.n;
const FCL_REAL& d = new_s.d;
- const std::size_t D = 8;
- for(std::size_t i = 0; i < D; ++i)
+ const short D = 8;
+ for(short i = 0; i < D; ++i)
bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
- for(std::size_t i = D; i < 2 * D; ++i)
+ for(short i = D; i < 2 * D; ++i)
bv.dist(i) = std::numeric_limits<FCL_REAL>::max();
if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
@@ -563,11 +563,11 @@ void computeBV<KDOP<18>, Halfspace>(const Halfspace& s, const Transform3f& tf, K
const Vec3f& n = new_s.n;
const FCL_REAL& d = new_s.d;
- const std::size_t D = 9;
+ const short D = 9;
- for(std::size_t i = 0; i < D; ++i)
+ for(short i = 0; i < D; ++i)
bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
- for(std::size_t i = D; i < 2 * D; ++i)
+ for(short i = D; i < 2 * D; ++i)
bv.dist(i) = std::numeric_limits<FCL_REAL>::max();
if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
@@ -624,11 +624,11 @@ void computeBV<KDOP<24>, Halfspace>(const Halfspace& s, const Transform3f& tf, K
const Vec3f& n = new_s.n;
const FCL_REAL& d = new_s.d;
- const std::size_t D = 12;
+ const short D = 12;
- for(std::size_t i = 0; i < D; ++i)
+ for(short i = 0; i < D; ++i)
bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
- for(std::size_t i = D; i < 2 * D; ++i)
+ for(short i = D; i < 2 * D; ++i)
bv.dist(i) = std::numeric_limits<FCL_REAL>::max();
if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
@@ -748,11 +748,11 @@ void computeBV<KDOP<16>, Plane>(const Plane& s, const Transform3f& tf, KDOP<16>&
const Vec3f& n = new_s.n;
const FCL_REAL& d = new_s.d;
- const std::size_t D = 8;
+ const short D = 8;
- for(std::size_t i = 0; i < D; ++i)
+ for(short i = 0; i < D; ++i)
bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
- for(std::size_t i = D; i < 2 * D; ++i)
+ for(short i = D; i < 2 * D; ++i)
bv.dist(i) = std::numeric_limits<FCL_REAL>::max();
if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
@@ -799,11 +799,11 @@ void computeBV<KDOP<18>, Plane>(const Plane& s, const Transform3f& tf, KDOP<18>&
const Vec3f& n = new_s.n;
const FCL_REAL& d = new_s.d;
- const std::size_t D = 9;
+ const short D = 9;
- for(std::size_t i = 0; i < D; ++i)
+ for(short i = 0; i < D; ++i)
bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
- for(std::size_t i = D; i < 2 * D; ++i)
+ for(short i = D; i < 2 * D; ++i)
bv.dist(i) = std::numeric_limits<FCL_REAL>::max();
if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
@@ -854,11 +854,11 @@ void computeBV<KDOP<24>, Plane>(const Plane& s, const Transform3f& tf, KDOP<24>&
const Vec3f& n = new_s.n;
const FCL_REAL& d = new_s.d;
- const std::size_t D = 12;
+ const short D = 12;
- for(std::size_t i = 0; i < D; ++i)
+ for(short i = 0; i < D; ++i)
bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
- for(std::size_t i = D; i < 2 * D; ++i)
+ for(short i = D; i < 2 * D; ++i)
bv.dist(i) = std::numeric_limits<FCL_REAL>::max();
if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
diff --git a/test/collision.cpp b/test/collision.cpp
index da4655f6..dd6cc97d 100644
--- a/test/collision.cpp
+++ b/test/collision.cpp
@@ -263,7 +263,7 @@ template<typename BV, bool Oriented, bool recursive>
struct traits : base_traits
{};
-template<size_t N, bool recursive>
+template<short N, bool recursive>
struct traits<KDOP<N>, Oriented, recursive> : base_traits
{
enum { IS_IMPLEMENTED = false
--
GitLab