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

Implement AABB::overlap with distance lower bound.

parent b4b7cd61
......@@ -97,12 +97,8 @@ public:
}
/// Not implemented
inline bool overlap(const AABB& other, const CollisionRequest&,
FCL_REAL& sqrDistLowerBound) const
{
sqrDistLowerBound = sqrt (-1);
return overlap (other);
}
bool overlap(const AABB& other, const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound) const;
/// @brief Check whether the AABB contains another AABB
inline bool contain(const AABB& other) const
......
......@@ -38,7 +38,7 @@
#include <hpp/fcl/BV/AABB.h>
#include <limits>
#include <iostream>
#include <hpp/fcl/collision_data.h>
namespace hpp
{
......@@ -50,6 +50,21 @@ AABB::AABB() : min_(Vec3f::Constant(std::numeric_limits<FCL_REAL>::max())),
{
}
bool AABB::overlap(const AABB& other, const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound) const
{
const FCL_REAL breakDistance (request.break_distance + request.security_margin);
const FCL_REAL breakDistance2 = breakDistance * breakDistance;
sqrDistLowerBound = (min_ - other.max_).array().max(0).matrix().squaredNorm();
if(sqrDistLowerBound > breakDistance2) return false;
sqrDistLowerBound = (other.min_ - max_).array().max(0).matrix().squaredNorm();
if(sqrDistLowerBound > breakDistance2) return false;
return true;
}
FCL_REAL AABB::distance(const AABB& other, Vec3f* P, Vec3f* Q) const
{
FCL_REAL result = 0;
......
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