Commit 2d8d1be4 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Implement kDOP::overlap with lower bound.

parent 8754b233
......@@ -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;
......
......@@ -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
{
......
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