diff --git a/include/hpp/fcl/BV/BV_node.h b/include/hpp/fcl/BV/BV_node.h
index 13e681278872efd0fa6459c8ccfc7c7ef7d3e7c8..5f83add9ba1b95f5e443bbd05dbda83fe9f3511d 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 3293be7117921de97cbc5497c478881673a75676..2cb8ea52c07308f5a6c2c1d348cc4ceab4da2527 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:
@@ -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;
@@ -135,7 +134,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 +162,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 +175,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 +197,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/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h
index 6e7d25ffc487085b533802b52cf1fb94c67c93cb..508480099c974ae6b0b9ee3f8007f2890d260ffe 100644
--- a/include/hpp/fcl/narrowphase/gjk.h
+++ b/include/hpp/fcl/narrowphase/gjk.h
@@ -218,7 +218,6 @@ static const size_t EPA_MAX_ITERATIONS = 255;
/// @brief class for EPA algorithm
struct EPA
{
-private:
typedef GJK::SimplexV SimplexV;
struct SimplexF
{
diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp
index 98a45f4668fbd81d847f37745575fd166bcedb77..9fb2d1502615f633f2c763db84a7eff72f428c62 100644
--- a/src/BV/OBB.cpp
+++ b/src/BV/OBB.cpp
@@ -331,6 +331,9 @@ namespace internal
const Matrix3f& Bf,
const FCL_REAL& breakDistance2, FCL_REAL& squaredLowerBoundDistance)
{
+ FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
+ if (sinus2 < 1e-6) return false;
+
const FCL_REAL s = T[ka] * B(ja, ib) - T[ja] * B(ka, ib);
const FCL_REAL diff = fabs(s) - (a[ja] * Bf(ka, ib) + a[ka] * Bf(ja, ib) +
@@ -338,21 +341,10 @@ namespace internal
// We need to divide by the norm || Aia x Bib ||
// As ||Aia|| = ||Bib|| = 1, (Aia | Bib)^2 = cosine^2
if (diff > 0) {
- FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
- if (sinus2 > 1e-6) {
- squaredLowerBoundDistance = diff * diff / sinus2;
- if (squaredLowerBoundDistance > breakDistance2) {
- return true;
- }
+ squaredLowerBoundDistance = diff * diff / sinus2;
+ if (squaredLowerBoundDistance > breakDistance2) {
+ return true;
}
- /* // or
- FCL_REAL sinus2 = 1 - Bf (ia,ib) * Bf (ia,ib);
- squaredLowerBoundDistance = diff * diff;
- if (squaredLowerBoundDistance > breakDistance2 * sinus2) {
- squaredLowerBoundDistance /= sinus2;
- return true;
- }
- // */
}
return false;
}
diff --git a/src/BV/kDOP.cpp b/src/BV/kDOP.cpp
index 7959bff0d141f61d060c04b64a4a01c229228eb7..ffd99604272d694a1e2ddc77272c7918394e55f4 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
@@ -67,7 +69,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 +108,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>().setConstant( real_max);
+ dist_.template tail<N/2>().setConstant(-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 +143,69 @@ 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<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<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 +213,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 +224,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 +232,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 +240,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/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index 9f0b94db7e2a185edb8bda396a9c2bf7b60bfe5a..329d9537c7e79a337c1506be7334cdf930bb4371 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -1058,25 +1058,26 @@ void EPA::initialize()
bool EPA::getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, FCL_REAL& dist)
{
- Vec3f ba = b->w - a->w;
- Vec3f n_ab = ba.cross(face->n);
+ Vec3f ab = b->w - a->w;
+ Vec3f n_ab = ab.cross(face->n);
FCL_REAL a_dot_nab = a->w.dot(n_ab);
if(a_dot_nab < 0) // the origin is on the outside part of ab
{
// following is similar to projectOrigin for two points
// however, as we dont need to compute the parameterization, dont need to compute 0 or 1
- FCL_REAL a_dot_ba = a->w.dot(ba);
- FCL_REAL b_dot_ba = b->w.dot(ba);
+ FCL_REAL a_dot_ab = a->w.dot(ab);
+ FCL_REAL b_dot_ab = b->w.dot(ab);
- if(a_dot_ba > 0)
+ if(a_dot_ab > 0)
dist = a->w.norm();
- else if(b_dot_ba < 0)
+ else if(b_dot_ab < 0)
dist = b->w.norm();
else
{
- FCL_REAL a_dot_b = a->w.dot(b->w);
- dist = std::sqrt(std::max(a->w.squaredNorm() * b->w.squaredNorm() - a_dot_b * a_dot_b, (FCL_REAL)0));
+ dist = std::sqrt(std::max(
+ a->w.squaredNorm() - a_dot_ab * a_dot_ab / ab.squaredNorm(),
+ 0.));
}
return true;
@@ -1099,16 +1100,17 @@ EPA::SimplexF* EPA::newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced)
face->n = (b->w - a->w).cross(c->w - a->w);
FCL_REAL l = face->n.norm();
- if(l > tolerance)
+ if(l > Eigen::NumTraits<FCL_REAL>::epsilon())
{
+ face->n /= l;
+
if(!(getEdgeDist(face, a, b, face->d) ||
getEdgeDist(face, b, c, face->d) ||
getEdgeDist(face, c, a, face->d)))
{
- face->d = a->w.dot(face->n) / l;
+ face->d = a->w.dot(face->n);
}
- face->n /= l;
if(forced || face->d >= -tolerance)
return face;
else
@@ -1194,46 +1196,35 @@ EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess)
status = Valid;
for(; iterations < max_iterations; ++iterations)
{
- if(nextsv < max_vertex_num)
- {
- SimplexHorizon horizon;
- SimplexV* w = &sv_store[nextsv++];
- bool valid = true;
- best->pass = ++pass;
- // At the moment, SimplexF.n is always normalized. This could be revised in the future...
- gjk.getSupport(best->n, true, *w);
- FCL_REAL wdist = best->n.dot(w->w) - best->d;
- if(wdist > tolerance)
- {
- for(size_t j = 0; (j < 3) && valid; ++j)
- {
- valid &= expand(pass, w, best->f[j], best->e[j], horizon);
- }
-
-
- if(valid && horizon.nf >= 3)
- {
- // need to add the edge connectivity between first and last faces
- bind(horizon.ff, 2, horizon.cf, 1);
- hull.remove(best);
- stock.append(best);
- best = findBest();
- outer = *best;
- }
- else
- {
- status = InvalidHull; break;
- }
- }
- else
- {
- status = AccuracyReached; break;
- }
+ if (nextsv >= max_vertex_num) {
+ status = OutOfVertices;
+ break;
}
- else
- {
- status = OutOfVertices; break;
+
+ SimplexHorizon horizon;
+ SimplexV* w = &sv_store[nextsv++];
+ bool valid = true;
+ best->pass = ++pass;
+ // At the moment, SimplexF.n is always normalized. This could be revised in the future...
+ gjk.getSupport(best->n, true, *w);
+ FCL_REAL wdist = best->n.dot(w->w) - best->d;
+ if(wdist <= tolerance) {
+ status = AccuracyReached;
+ break;
+ }
+ for(size_t j = 0; (j < 3) && valid; ++j)
+ valid &= expand(pass, w, best->f[j], best->e[j], horizon);
+
+ if(!valid || horizon.nf < 3) {
+ status = InvalidHull;
+ break;
}
+ // need to add the edge connectivity between first and last faces
+ bind(horizon.ff, 2, horizon.cf, 1);
+ hull.remove(best);
+ stock.append(best);
+ best = findBest();
+ outer = *best;
}
normal = outer.n;
@@ -1264,45 +1255,44 @@ bool EPA::expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon
static const size_t nexti[] = {1, 2, 0};
static const size_t previ[] = {2, 0, 1};
- if(f->pass != pass)
+ if(f->pass == pass)
+ return false;
+
+ const size_t e1 = nexti[e];
+
+ // case 1: the new face is not degenerated, i.e., the new face is not coplanar with the old face f.
+ if(f->n.dot(w->w - f->vertex[e]->w) < -tolerance)
{
- const size_t e1 = nexti[e];
-
- // case 1: the new face is not degenerated, i.e., the new face is not coplanar with the old face f.
- if(f->n.dot(w->w) - f->d < -tolerance)
+ SimplexF* nf = newFace(f->vertex[e1], f->vertex[e], w, false);
+ if(nf)
{
- SimplexF* nf = newFace(f->vertex[e1], f->vertex[e], w, false);
- if(nf)
- {
- // add face-face connectivity
- bind(nf, 0, f, e);
-
- // if there is last face in the horizon, then need to add another connectivity, i.e. the edge connecting the current new add edge and the last new add edge.
- // This does not finish all the connectivities because the final face need to connect with the first face, this will be handled in the evaluate function.
- // Notice the face is anti-clockwise, so the edges are 0 (bottom), 1 (right), 2 (left)
- if(horizon.cf)
- bind(nf, 2, horizon.cf, 1);
- else
- horizon.ff = nf;
-
- horizon.cf = nf;
- ++horizon.nf;
- return true;
- }
- }
- else // case 2: the new face is coplanar with the old face f. We need to add two faces and delete the old face
- {
- const size_t e2 = previ[e];
- f->pass = pass;
- if(expand(pass, w, f->f[e1], f->e[e1], horizon) && expand(pass, w, f->f[e2], f->e[e2], horizon))
- {
- hull.remove(f);
- stock.append(f);
- return true;
- }
+ // add face-face connectivity
+ bind(nf, 0, f, e);
+
+ // if there is last face in the horizon, then need to add another connectivity, i.e. the edge connecting the current new add edge and the last new add edge.
+ // This does not finish all the connectivities because the final face need to connect with the first face, this will be handled in the evaluate function.
+ // Notice the face is anti-clockwise, so the edges are 0 (bottom), 1 (right), 2 (left)
+ if(horizon.cf)
+ bind(nf, 2, horizon.cf, 1);
+ else
+ horizon.ff = nf;
+
+ horizon.cf = nf;
+ ++horizon.nf;
+ return true;
}
+ return false;
}
+ // case 2: the new face is coplanar with the old face f. We need to add two faces and delete the old face
+ const size_t e2 = previ[e];
+ f->pass = pass;
+ if(expand(pass, w, f->f[e1], f->e[e1], horizon) && expand(pass, w, f->f[e2], f->e[e2], horizon))
+ {
+ hull.remove(f);
+ stock.append(f);
+ return true;
+ }
return false;
}
diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp
index c0aba4929503b4948ed397f71a74ff2ceb51c460..bea9e11ee801a8795708f6dd32a55a2dd2c864c4 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 da4655f681ee2541dfb674958e9c5d6381d2fd66..dd6cc97d0f01d8180eb5b9274e40b14dcd972d42 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
diff --git a/travis_custom/custom_before_install_osx.sh b/travis_custom/custom_before_install_osx.sh
index 0ee7c787bb68f695b252ccd4552813042351cdee..1b33ed2e550534c2a499507b2271a9f2d54dea02 100755
--- a/travis_custom/custom_before_install_osx.sh
+++ b/travis_custom/custom_before_install_osx.sh
@@ -9,5 +9,5 @@ brew install boost assimp eigen octomap
brew uninstall numpy
pip uninstall numpy -y
-brew install eigenpy
+brew install eigenpy@2
brew link --overwrite numpy@1.16