Commit afa59832 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Add missing overlap method.

parent c3edc377
......@@ -262,6 +262,14 @@ static inline AABB rotate(const AABB& aabb, const Matrix3f& t)
return res;
}
/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity.
bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, const AABB& b2);
/// @brief Check collision between two aabbs, b1 is in configuration (R0, T0) and b2 is in identity.
bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
const AABB& b2, const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound);
}
} // namespace hpp
......
......@@ -155,6 +155,10 @@ FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS&
/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity.
bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2);
/// @brief Check collision between two RSSs, b1 is in configuration (R0, T0) and b2 is in identity.
bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2,
const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound);
}
......
......@@ -39,7 +39,7 @@
#define HPP_FCL_KDOP_H
#include <stdexcept>
#include <hpp/fcl/math/vec_3f.h>
#include <hpp/fcl/math/matrix_3f.h>
namespace hpp
{
......@@ -176,6 +176,20 @@ public:
};
template<size_t 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>
bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/,
const KDOP<N>& /*b1*/, const KDOP<N>& /*b2*/,
const CollisionRequest& /*request*/, FCL_REAL& /*sqrDistLowerBound*/)
{
throw std::logic_error ("not implemented");
}
/// @brief translate the KDOP BV
template<size_t N>
......
......@@ -157,6 +157,12 @@ kIOS translate(const kIOS& bv, const Vec3f& t);
/// @todo Not efficient
bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2);
/// @brief Check collision between two kIOSs, b1 is in configuration (R0, T0) and b2 is in identity.
/// @todo Not efficient
bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2,
const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound);
/// @brief Approximate distance between two kIOS bounding volumes
/// @todo P and Q is not returned, need implementation
FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P = NULL, Vec3f* Q = NULL);
......
......@@ -128,6 +128,20 @@ FCL_REAL AABB::distance(const AABB& other) const
return std::sqrt(result);
}
bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1, const AABB& b2)
{
AABB bb1 (translate (rotate (b1, R0), T0));
return bb1.overlap (b2);
}
bool overlap(const Matrix3f& R0, const Vec3f& T0, const AABB& b1,
const AABB& b2, const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound)
{
AABB bb1 (translate (rotate (b1, R0), T0));
return bb1.overlap (b2, request, sqrDistLowerBound);
}
}
} // namespace hpp
......@@ -862,6 +862,25 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2)
return (dist <= (b1.r + b2.r));
}
bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2,
const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound)
{
// ROb2 = R0 . b2
// where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ]
// (1 0 0)^T R0b2^T axis [0] = (1 0 0)^T b2^T R0^T axis [0]
// R = b2^T RO^T b1
Vec3f Ttemp (R0 * b2.Tr + T0 - b1.Tr);
Vec3f T(b1.axes.transpose() * Ttemp);
Matrix3f R(b1.axes.transpose() * R0 * b2.axes);
FCL_REAL dist = rectDistance(R, T, b1.l, b2.l) - b1.r - b2.r;
if (dist <= 0) return true;
sqrDistLowerBound = dist * dist;
return false;
}
bool RSS::contain(const Vec3f& p) const
{
Vec3f local_p = p - Tr;
......
......@@ -61,17 +61,27 @@ bool kIOS::overlap(const kIOS& other) const
}
return obb.overlap(other.obb);
return true;
}
bool kIOS::overlap(const kIOS& other, const CollisionRequest&,
FCL_REAL& sqrDistLowerBound) const
bool kIOS::overlap(const kIOS& other, const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound) const
{
for(unsigned int i = 0; i < num_spheres; ++i)
{
sqrDistLowerBound = sqrt (-1);
return overlap (other);
for(unsigned int j = 0; j < other.num_spheres; ++j)
{
FCL_REAL o_dist = (spheres[i].o - other.spheres[j].o).squaredNorm();
FCL_REAL sum_r = spheres[i].r + other.spheres[j].r;
if(o_dist > sum_r * sum_r) {
o_dist = sqrt(o_dist) - sum_r;
sqrDistLowerBound = o_dist*o_dist;
return false;
}
}
}
return obb.overlap(other.obb, request, sqrDistLowerBound);
}
bool kIOS::contain(const Vec3f& p) const
{
......@@ -192,6 +202,23 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2
return b1.overlap(b2_temp);
}
bool overlap(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2,
const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound)
{
kIOS b2_temp = b2;
for(unsigned int i = 0; i < b2_temp.num_spheres; ++i)
{
b2_temp.spheres[i].o = R0 * b2_temp.spheres[i].o + T0;
}
b2_temp.obb.To = R0 * b2_temp.obb.To + T0;
b2_temp.obb.axes.applyOnTheLeft(R0);
return b1.overlap(b2_temp, request, sqrDistLowerBound);
}
FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const kIOS& b1, const kIOS& b2, Vec3f* P, Vec3f* Q)
{
kIOS b2_temp = b2;
......
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