Commit a2268fe0 authored by jpan's avatar jpan
Browse files

collision for plane and halfspace, not finished yet.


git-svn-id: https://kforge.ros.org/fcl/fcl_ros@161 253336fb-580f-4252-a368-f3cef5a2a82b
parent d7d66c14
......@@ -46,6 +46,15 @@ namespace fcl
/// @brief KDOP class describes the KDOP collision structures. K is set as the template parameter, which should be 16, 18, or 24
/// The KDOP structure is defined by some pairs of parallel planes defined by some axes.
/// For K = 16, the planes are 6 AABB planes and 10 diagonal planes that cut off some space of the edges:
/// (-1,0,0) and (1,0,0) -> indices 0 and 8
/// (0,-1,0) and (0,1,0) -> indices 1 and 9
/// (0,0,-1) and (0,0,1) -> indices 2 and 10
/// (-1,-1,0) and (1,1,0) -> indices 3 and 11
/// (-1,0,-1) and (1,0,1) -> indices 4 and 12
/// (0,-1,-1) and (0,1,1) -> indices 5 and 13
/// (-1,1,0) and (1,-1,0) -> indices 6 and 14
/// (-1,0,1) and (1,0,-1) -> indices 7 and 15
/// For K = 18, the planes are 6 AABB planes and 12 diagonal planes that cut off some space of the edges:
/// (-1,0,0) and (1,0,0) -> indices 0 and 9
/// (0,-1,0) and (0,1,0) -> indices 1 and 10
......@@ -56,6 +65,19 @@ namespace fcl
/// (-1,1,0) and (1,-1,0) -> indices 6 and 15
/// (-1,0,1) and (1,0,-1) -> indices 7 and 16
/// (0,-1,1) and (0,1,-1) -> indices 8 and 17
/// For K = 18, the planes are 6 AABB planes and 18 diagonal planes that cut off some space of the edges:
/// (-1,0,0) and (1,0,0) -> indices 0 and 12
/// (0,-1,0) and (0,1,0) -> indices 1 and 13
/// (0,0,-1) and (0,0,1) -> indices 2 and 14
/// (-1,-1,0) and (1,1,0) -> indices 3 and 15
/// (-1,0,-1) and (1,0,1) -> indices 4 and 16
/// (0,-1,-1) and (0,1,1) -> indices 5 and 17
/// (-1,1,0) and (1,-1,0) -> indices 6 and 18
/// (-1,0,1) and (1,0,-1) -> indices 7 and 19
/// (0,-1,1) and (0,1,-1) -> indices 8 and 20
/// (-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>
class KDOP
{
......
......@@ -139,9 +139,6 @@ public:
/// @brief The distance between two kIOS
FCL_REAL distance(const kIOS& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
private:
};
......
......@@ -50,7 +50,7 @@ enum OBJECT_TYPE {OT_UNKNOWN, OT_BVH, OT_GEOM, OT_OCTREE, OT_COUNT};
/// @brief traversal node type: bounding volume (AABB, OBB, RSS, kIOS, OBBRSS, KDOP16, KDOP18, kDOP24), basic shape (box, sphere, capsule, cone, cylinder, convex, plane, triangle), and octree
enum NODE_TYPE {BV_UNKNOWN, BV_AABB, BV_OBB, BV_RSS, BV_kIOS, BV_OBBRSS, BV_KDOP16, BV_KDOP18, BV_KDOP24,
GEOM_BOX, GEOM_SPHERE, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE, GEOM_TRIANGLE, GEOM_OCTREE, NODE_COUNT};
GEOM_BOX, GEOM_SPHERE, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE, GEOM_HALFSPACE, GEOM_TRIANGLE, GEOM_OCTREE, NODE_COUNT};
/// @brief The geometry for the object for collision or distance computation
class CollisionGeometry
......
......@@ -130,6 +130,10 @@ public:
inline FCL_REAL& getY() { return data[2]; }
inline FCL_REAL& getZ() { return data[3]; }
Vec3f getColumn(std::size_t i) const;
Vec3f getRow(std::size_t i) const;
private:
FCL_REAL data[4];
......
......@@ -258,6 +258,43 @@ protected:
void fillEdges();
};
/// @brief Half Space: this is equivalent to the Plane in ODE. The separation plane is defined as n * x = d;
/// Points in the negative side of the separation plane (i.e. {x | n * x < d}) are inside the half space and points
/// in the positive side of the separation plane (i.e. {x | n * x > d}) are outside the half space
class Halfspace : public ShapeBase
{
public:
/// @brief Construct a half space with normal direction and offset
Halfspace(const Vec3f& n_, FCL_REAL d_) : ShapeBase(), n(n_), d(d_)
{
unitNormalTest();
}
/// @brief Construct a plane with normal direction and offset
Halfspace(FCL_REAL a, FCL_REAL b, FCL_REAL c, FCL_REAL d_) : n(a, b, c), d(d_)
{
unitNormalTest();
}
/// @brief Compute AABB
void computeLocalAABB();
/// @brief Get node type: a half space
NODE_TYPE getNodeType() const { return GEOM_HALFSPACE; }
/// @brief Plane normal
Vec3f n;
/// @brief Plane offset
FCL_REAL d;
protected:
/// @brief Turn non-unit normal into unit
void unitNormalTest();
};
/// @brief Infinite plane
class Plane : public ShapeBase
{
......
......@@ -89,8 +89,9 @@ void computeBV<AABB, Convex>(const Convex& s, const Transform3f& tf, AABB& bv);
template<>
void computeBV<AABB, Triangle2>(const Triangle2& s, const Transform3f& tf, AABB& bv);
template<>
void computeBV<AABB, Halfspace>(const Halfspace& s, const Transform3f& tf, AABB& bv);
/// @brief the bounding volume for half space back of plane for OBB, it is the plane itself
template<>
void computeBV<AABB, Plane>(const Plane& s, const Transform3f& tf, AABB& bv);
......@@ -114,9 +115,13 @@ void computeBV<OBB, Cylinder>(const Cylinder& s, const Transform3f& tf, OBB& bv)
template<>
void computeBV<OBB, Convex>(const Convex& s, const Transform3f& tf, OBB& bv);
template<>
void computeBV<OBB, Halfspace>(const Halfspace& s, const Transform3f& tf, OBB& bv);
template<>
void computeBV<OBB, Plane>(const Plane& s, const Transform3f& tf, OBB& bv);
template<>
void computeBV<RSS, Plane>(const Plane& s, const Transform3f& tf, RSS& bv);
......@@ -169,6 +174,9 @@ void constructBox(const KDOP<18>& bv, const Transform3f& tf_bv, Box& box, Transf
void constructBox(const KDOP<24>& bv, const Transform3f& tf_bv, Box& box, Transform3f& tf);
Halfspace transform(const Halfspace& a, const Transform3f& tf);
Plane transform(const Plane& a, const Transform3f& tf);
}
......
......@@ -327,6 +327,49 @@ Quaternion3f inverse(const Quaternion3f& q)
return res.inverse();
}
Vec3f Quaternion3f::getColumn(std::size_t i) const
{
switch(i)
{
case 0:
return Vec3f(data[0] * data[0] + data[1] * data[1] - data[2] * data[2] - data[3] * data[3],
2 * (- data[0] * data[3] + data[1] * data[2]),
2 * (data[1] * data[3] + data[0] * data[2]));
case 1:
return Vec3f(2 * (data[1] * data[2] + data[0] * data[3]),
data[0] * data[0] - data[1] * data[1] + data[2] * data[2] - data[3] * data[3],
2 * (data[2] * data[3] - data[0] * data[1]));
case 2:
return Vec3f(2 * (data[1] * data[3] - data[0] * data[2]),
2 * (data[2] * data[3] + data[0] * data[1]),
data[0] * data[0] - data[1] * data[1] - data[2] * data[2] + data[3] * data[3]);
default:
return Vec3f();
}
}
Vec3f Quaternion3f::getRow(std::size_t i) const
{
switch(i)
{
case 0:
return Vec3f(data[0] * data[0] + data[1] * data[1] - data[2] * data[2] - data[3] * data[3],
2 * (data[0] * data[3] + data[1] * data[2]),
2 * (data[1] * data[3] - data[0] * data[2]));
case 1:
return Vec3f(2 * (data[1] * data[2] - data[0] * data[3]),
data[0] * data[0] - data[1] * data[1] + data[2] * data[2] - data[3] * data[3],
2 * (data[2] * data[3] + data[0] * data[1]));
case 2:
return Vec3f(2 * (data[1] * data[3] + data[0] * data[2]),
2 * (data[2] * data[3] - data[0] * data[1]),
data[0] * data[0] - data[1] * data[1] - data[2] * data[2] + data[3] * data[3]);
default:
return Vec3f();
}
}
const Matrix3f& Transform3f::getRotationInternal() const
{
boost::mutex::scoped_lock slock(const_cast<boost::mutex&>(lock_));
......
......@@ -35,6 +35,7 @@
/** \author Jia Pan */
#include "fcl/narrowphase/narrowphase.h"
#include "fcl/shape/geometric_shapes_utility.h"
#include <boost/math/constants/constants.hpp>
#include <vector>
......@@ -1242,6 +1243,543 @@ bool boxBoxIntersect(const Box& s1, const Transform3f& tf1,
bool sphereHalfspaceIntersect(const Sphere& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
{
Halfspace new_s2 = transform(s2, tf2);
FCL_REAL k = tf1.getTranslation().dot(new_s2.n);
FCL_REAL depth = new_s2.d - k + s1.radius;
if(depth >= 0)
{
if(normal) *normal = -new_s2.n; // pointing from s1 to s2
if(penetration_depth) *penetration_depth = depth;
if(contact_points) *contact_points = tf1.getTranslation() - new_s2.n * s1.radius + new_s2.n * (depth * 0.5);
return true;
}
else
return false;
}
bool boxHalfspaceIntersect(const Box& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
{
Halfspace new_s2 = transform(s2, tf2);
/// project sides lengths along normal vector, get absolue values
const Matrix3f& R = tf1.getRotation();
const Vec3f& T = tf1.getTranslation();
const Quaternion3f& q = tf1.getQuatRotation();
Vec3f Q = conj(q).transform(new_s2.n);
Vec3f A(Q[0] * s1.side[0], Q[1] * s1.side[1], Q[2] * s1.side[2]);
Vec3f B = abs(A);
FCL_REAL depth = new_s2.d + 0.5 * (B[0] + B[1] + B[2]) - new_s2.n.dot(tf1.getTranslation());
if(depth < 0) return false;
/// find deepest point
Vec3f p = tf1.getTranslation();
for(std::size_t i = 0; i < 3; ++i)
{
if(A[i] > 0)
p -= R.getColumn(i) * (0.5 * s1.side[i]);
else
p += R.getColumn(i) * (0.5 * s1.side[i]);
}
/// compute the contact point from the deepest point
if(penetration_depth) *penetration_depth = depth;
if(normal) *normal = -new_s2.n;
if(contact_points) *contact_points = p + new_s2.n * (depth * 0.5);
return true;
}
bool capsuleHalfspaceIntersect(const Capsule& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
{
Halfspace new_s2 = transform(s2, tf2);
const Matrix3f& R = tf1.getRotation();
const Vec3f& T = tf1.getTranslation();
Vec3f dir_z = R.getColumn(2);
Vec3f Q = R.transposeTimes(new_s2.n);
FCL_REAL sign = (Q[2] > 0) ? -1 : 1;
Vec3f p = tf1.getTranslation() + dir_z * (s1.lz * 0.5 * sign);
FCL_REAL k = p.dot(new_s2.n);
FCL_REAL depth = new_s2.d - k + s1.radius;
if(depth < 0) return false;
if(penetration_depth) *penetration_depth = depth;
if(normal) *normal = -new_s2.n;
if(contact_points)
{
Vec3f c = p - new_s2.n * s1.radius; // deepest
c += dir_z * (0.5 * depth / Q[2]);
*contact_points = c;
}
return true;
}
template<typename T>
T cylinderHalfspaceIntersectTolerance()
{
}
template<>
double cylinderHalfspaceIntersectTolerance()
{
return 0.0000001;
}
template<>
float cylinderHalfspaceIntersectTolerance()
{
return 0.0001;
}
bool cylinderHalfspaceIntersect(const Cylinder& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
{
Halfspace new_s2 = transform(s2, tf2);
const Matrix3f& R = tf1.getRotation();
const Vec3f& T = tf1.getTranslation();
Vec3f dir_z = R.getColumn(2);
Vec3f p1 = tf1.getTranslation() + dir_z * (0.5 * s1.lz);
Vec3f p2 = tf1.getTranslation() - dir_z * (0.5 * s1.lz);
FCL_REAL s = dir_z.dot(new_s2.n);
if(s < 0)
s += 1;
else
s -= 1;
if(std::abs(s) < cylinderHalfspaceIntersectTolerance<FCL_REAL>())
{
FCL_REAL d1 = new_s2.d - (new_s2.n).dot(p1);
FCL_REAL d2 = new_s2.d - (new_s2.n).dot(p2);
if(d1 >= d2)
{
if(d1 >= 0)
{
if(contact_points) *contact_points = p1 + new_s2.n * (0.5 * d1);
if(penetration_depth) *penetration_depth = d1;
if(normal) *normal = -new_s2.n;
return true;
}
else
return false;
}
else
{
if(d2 >= 0)
{
if(contact_points) *contact_points = p2 + new_s2.n * (0.5 * d2);
if(penetration_depth) *penetration_depth = d2;
if(normal) *normal = -new_s2.n;
return true;
}
else
return false;
}
}
else
{
FCL_REAL t = (new_s2.n).dot(dir_z);
Vec3f C = dir_z * t - new_s2.n;
FCL_REAL s = C.length();
s = s1.radius / s;
C *= s;
// deepest point of disc1
Vec3f c1 = C + p1;
FCL_REAL depth1 = new_s2.d - (new_s2.n).dot(c1);
Vec3f c2 = C + p2;
FCL_REAL depth2 = new_s2.d - (new_s2.n).dot(c2);
if(depth1 >= 0 && depth2 >= 0)
{
if(depth1 > depth2)
{
if(penetration_depth) *penetration_depth = depth1;
if(normal) *normal = -new_s2.n;
if(contact_points) *contact_points = c1 + dir_z * (0.5 * depth1 / s);
}
else
{
if(penetration_depth) *penetration_depth = depth2;
if(normal) *normal = -new_s2.n;
if(contact_points) *contact_points = c2 + dir_z * (0.5 * depth2 / s);
}
return true;
}
else if(depth1 >= 0)
{
if(penetration_depth) *penetration_depth = depth1;
if(normal) *normal = -new_s2.n;
if(contact_points) *contact_points = c1 + dir_z * (0.5 * depth1 / s);
return true;
}
else if(depth2 >= 0)
{
if(penetration_depth) *penetration_depth = depth2;
if(normal) *normal = -new_s2.n;
if(contact_points) *contact_points = c2 + dir_z * (0.5 * depth2 / s);
return true;
}
else
return false;
}
}
bool coneHalfspaceIntersect(const Cone& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
{
Halfspace new_s2 = transform(s2, tf2);
const Matrix3f& R = tf1.getRotation();
const Vec3f& T = tf1.getTranslation();
Vec3f dir_z = R.getColumn(2);
Vec3f p1 = tf1.getTranslation() + dir_z * (0.5 * s1.lz);
Vec3f p2 = tf1.getTranslation() - dir_z * (0.5 * s1.lz);
FCL_REAL s = dir_z.dot(new_s2.n);
if(s < 0)
s += 1;
else
s -= 1;
if(std::abs(s) < cylinderHalfspaceIntersectTolerance<FCL_REAL>())
{
FCL_REAL d1 = new_s2.d - (new_s2.n).dot(p1);
FCL_REAL d2 = new_s2.d - (new_s2.n).dot(p2);
if(d1 >= d2)
{
if(d1 >= 0)
{
if(contact_points) *contact_points = p1 + new_s2.n * (0.5 * d1);
if(penetration_depth) *penetration_depth = d1;
if(normal) *normal = -new_s2.n;
return true;
}
else
return false;
}
else
{
if(d2 >= 0)
{
if(contact_points) *contact_points = p2 + new_s2.n * (0.5 * d2);
if(penetration_depth) *penetration_depth = d2;
if(normal) *normal = -new_s2.n;
return true;
}
else
return false;
}
}
else
{
FCL_REAL t = (new_s2.n).dot(dir_z);
Vec3f C = dir_z * t - new_s2.n;
FCL_REAL s = C.length();
s = s1.radius / s;
C *= s;
// deepest point of disc1
Vec3f c1 = p1;
FCL_REAL depth1 = new_s2.d - (new_s2.n).dot(c1);
Vec3f c2 = C + p2;
FCL_REAL depth2 = new_s2.d - (new_s2.n).dot(c2);
if(depth1 >= 0 && depth2 >= 0)
{
if(depth1 > depth2)
{
if(penetration_depth) *penetration_depth = depth1;
if(normal) *normal = -new_s2.n;
if(contact_points) *contact_points = c1 + dir_z * (0.5 * depth1 / s);
}
else
{
if(penetration_depth) *penetration_depth = depth2;
if(normal) *normal = -new_s2.n;
if(contact_points) *contact_points = c2 + dir_z * (0.5 * depth2 / s);
}
return true;
}
else if(depth1 >= 0)
{
if(penetration_depth) *penetration_depth = depth1;
if(normal) *normal = -new_s2.n;
if(contact_points) *contact_points = c1 + dir_z * (0.5 * depth1 / s);
return true;
}
else if(depth2 >= 0)
{
if(penetration_depth) *penetration_depth = depth2;
if(normal) *normal = -new_s2.n;
if(contact_points) *contact_points = c2 + dir_z * (0.5 * depth2 / s);
return true;
}
else
return false;
}
}
bool convexHalfspaceIntersect(const Convex& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
{
Halfspace new_s2 = transform(s2, tf2);
Vec3f v;
FCL_REAL depth = 1;
for(std::size_t i = 0; i < s1.num_points; ++i)
{
Vec3f p = s1.points[i];
p = tf1.transform(p);
FCL_REAL d = (new_s2.n).dot(p) - new_s2.d;
if(d < depth)
{
depth = d;
v = p;
}
}
if(depth <= 0)
{
if(contact_points) *contact_points = v;
if(penetration_depth) *penetration_depth = depth;
if(normal) *normal = -new_s2.n;
return true;
}
else
return false;
}
bool triangleHalfspaceIntersect(const Triangle2& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
{
Halfspace new_s2 = transform(s2, tf2);
Vec3f v = tf1.transform(s1.a);
FCL_REAL depth = (new_s2.n).dot(v) - new_s2.d;
Vec3f p = tf1.transform(s1.b);
FCL_REAL d = (new_s2.n).dot(p) - new_s2.d;
if(d < depth)
{
depth = d;
v = p;
}
p = tf1.transform(s1.c);
d = (new_s2.n).dot(p) - new_s2.d;
if(d < depth)
{
depth = d;
v = p;
}
if(depth <= 0)
{
if(contact_points) *contact_points = v;
if(penetration_depth) *penetration_depth = depth;
if(normal) *normal = -new_s2.n;
return true;
}
else
return false;
}
/// @brief return whether plane collides with halfspace