Commit 8754b233 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Vectorize kDOP

parent 2720bbe2
......@@ -116,6 +116,10 @@ struct BVNode : public BVNodeBase
static const Matrix3f id3 = Matrix3f::Identity();
return id3;
}
/// \cond
EIGEN_MAKE_ALIGNED_OPERATOR_NEW
/// \endcond
};
template<>
......
......@@ -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);
}
......
......@@ -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;
......
......@@ -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)
......
......@@ -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
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment