From a2268fe0ca1053b2979d5021bb48e01d220becad Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Fri, 10 Aug 2012 02:54:14 +0000
Subject: [PATCH] collision for plane and halfspace, not finished yet.
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@161 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/include/fcl/BV/kDOP.h | 22 +
trunk/fcl/include/fcl/BV/kIOS.h | 3 -
trunk/fcl/include/fcl/collision_object.h | 2 +-
trunk/fcl/include/fcl/math/transform.h | 4 +
.../fcl/include/fcl/shape/geometric_shapes.h | 37 ++
.../fcl/shape/geometric_shapes_utility.h | 10 +-
trunk/fcl/src/math/transform.cpp | 43 ++
trunk/fcl/src/narrowphase/narrowphase.cpp | 538 ++++++++++++++++++
trunk/fcl/src/shape/geometric_shapes.cpp | 23 +
.../src/shape/geometric_shapes_utility.cpp | 504 +++++++++++++++-
10 files changed, 1159 insertions(+), 27 deletions(-)
diff --git a/trunk/fcl/include/fcl/BV/kDOP.h b/trunk/fcl/include/fcl/BV/kDOP.h
index 4560270c..8be4aa4b 100644
--- a/trunk/fcl/include/fcl/BV/kDOP.h
+++ b/trunk/fcl/include/fcl/BV/kDOP.h
@@ -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
{
diff --git a/trunk/fcl/include/fcl/BV/kIOS.h b/trunk/fcl/include/fcl/BV/kIOS.h
index 8f81dfe3..93383eb6 100644
--- a/trunk/fcl/include/fcl/BV/kIOS.h
+++ b/trunk/fcl/include/fcl/BV/kIOS.h
@@ -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:
-
};
diff --git a/trunk/fcl/include/fcl/collision_object.h b/trunk/fcl/include/fcl/collision_object.h
index 952de096..33e51318 100644
--- a/trunk/fcl/include/fcl/collision_object.h
+++ b/trunk/fcl/include/fcl/collision_object.h
@@ -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
diff --git a/trunk/fcl/include/fcl/math/transform.h b/trunk/fcl/include/fcl/math/transform.h
index 461f4d49..5082e74d 100644
--- a/trunk/fcl/include/fcl/math/transform.h
+++ b/trunk/fcl/include/fcl/math/transform.h
@@ -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];
diff --git a/trunk/fcl/include/fcl/shape/geometric_shapes.h b/trunk/fcl/include/fcl/shape/geometric_shapes.h
index 468d3892..e0e9d797 100644
--- a/trunk/fcl/include/fcl/shape/geometric_shapes.h
+++ b/trunk/fcl/include/fcl/shape/geometric_shapes.h
@@ -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
{
diff --git a/trunk/fcl/include/fcl/shape/geometric_shapes_utility.h b/trunk/fcl/include/fcl/shape/geometric_shapes_utility.h
index 018a0187..15acae9f 100644
--- a/trunk/fcl/include/fcl/shape/geometric_shapes_utility.h
+++ b/trunk/fcl/include/fcl/shape/geometric_shapes_utility.h
@@ -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);
}
diff --git a/trunk/fcl/src/math/transform.cpp b/trunk/fcl/src/math/transform.cpp
index cb2e1562..665fe7cc 100644
--- a/trunk/fcl/src/math/transform.cpp
+++ b/trunk/fcl/src/math/transform.cpp
@@ -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_));
diff --git a/trunk/fcl/src/narrowphase/narrowphase.cpp b/trunk/fcl/src/narrowphase/narrowphase.cpp
index d44cf48f..051cd142 100644
--- a/trunk/fcl/src/narrowphase/narrowphase.cpp
+++ b/trunk/fcl/src/narrowphase/narrowphase.cpp
@@ -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
+/// if the separation plane of the halfspace is parallel with the plane
+/// return code 1, if the plane's normal is the same with halfspace's normal and plane is inside halfspace, also return plane in pl
+/// return code 2, if the plane's normal is oppositie to the halfspace's normal and plane is inside halfspace, also return plane in pl
+/// plane is outside halfspace, collision-free
+/// if not parallel
+/// return the intersection ray, return code 3. ray origin is p and direction is d
+bool planeHalfspaceIntersect(const Plane& s1, const Transform3f& tf1,
+ const Halfspace& s2, const Transform3f& tf2,
+ Plane& pl,
+ Vec3f& p, Vec3f& d,
+ FCL_REAL& penetration_depth,
+ int& ret)
+{
+ Plane new_s1 = transform(s1, tf1);
+ Halfspace new_s2 = transform(s2, tf2);
+
+ ret = 0;
+
+ Vec3f dir = (new_s1.n).cross(new_s2.n);
+ FCL_REAL dir_norm = dir.sqrLength();
+ if(dir_norm < std::numeric_limits<FCL_REAL>::epsilon()) // parallel
+ {
+ if((new_s1.n).dot(new_s2.n) > 0)
+ {
+ if(new_s1.d < new_s2.d)
+ {
+ penetration_depth = new_s2.d - new_s1.d;
+ ret = 1;
+ pl = new_s1;
+ return true;
+ }
+ else
+ return false;
+ }
+ else
+ {
+ if(new_s1.d + new_s2.d > 0)
+ return false;
+ else
+ {
+ penetration_depth = -(new_s1.d + new_s2.d);
+ ret = 2;
+ pl = new_s1;
+ return true;
+ }
+ }
+ }
+
+ Vec3f n = new_s2.n * new_s1.d - new_s1.n * new_s2.d;
+ Vec3f origin = n.cross(dir);
+ origin *= (1.0 / dir_norm);
+
+ p = origin;
+ d = dir;
+ ret = 3;
+ penetration_depth = std::numeric_limits<FCL_REAL>::max();
+
+ return true;
+}
+
+///@ brief return whether two halfspace intersect
+/// if the separation planes of the two halfspaces are parallel
+/// return code 1, if two halfspaces' normal are same and s1 is in s2, also return s1 in s;
+/// return code 2, if two halfspaces' normal are same and s2 is in s1, also return s2 in s;
+/// return code 3, if two halfspaces' normal are opposite and s1 and s2 are into each other;
+/// collision free, if two halfspaces' are separate;
+/// if the separation planes of the two halfspaces are not parallel, return intersection ray, return code 4. ray origin is p and direction is d
+/// collision free return code 0
+bool halfspaceIntersect(const Halfspace& s1, const Transform3f& tf1,
+ const Halfspace& s2, const Transform3f& tf2,
+ Vec3f& p, Vec3f& d,
+ Halfspace& s,
+ FCL_REAL& penetration_depth,
+ int& ret)
+{
+ Halfspace new_s1 = transform(s1, tf1);
+ Halfspace new_s2 = transform(s2, tf2);
+
+ ret = 0;
+
+ Vec3f dir = (new_s1.n).cross(new_s2.n);
+ FCL_REAL dir_norm = dir.sqrLength();
+ if(dir_norm < std::numeric_limits<FCL_REAL>::epsilon()) // parallel
+ {
+ if((new_s1.n).dot(new_s2.n) > 0)
+ {
+ if(new_s1.d < new_s2.d) /// s1 is inside s2
+ {
+ ret = 1;
+ penetration_depth = std::numeric_limits<FCL_REAL>::max();
+ s = new_s1;
+ }
+ else /// s2 is inside s1
+ {
+ ret = 2;
+ penetration_depth = std::numeric_limits<FCL_REAL>::max();
+ s = new_s2;
+ }
+ return true;
+ }
+ else
+ {
+ if(new_s1.d + new_s2.d > 0) /// not collision
+ return false;
+ else /// in each other
+ {
+ ret = 3;
+ penetration_depth = -(new_s1.d + new_s2.d);
+ return true;
+ }
+ }
+ }
+
+ Vec3f n = new_s2.n * new_s1.d - new_s1.n * new_s2.d;
+ Vec3f origin = n.cross(dir);
+ origin *= (1.0 / dir_norm);
+
+ p = origin;
+ d = dir;
+ ret = 4;
+ penetration_depth = std::numeric_limits<FCL_REAL>::max();
+
+ return true;
+}
+
+bool spherePlaneIntersect(const Sphere& s1, const Transform3f& tf1,
+ const Plane& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
+{
+ Plane new_s2 = transform(s2, tf2);
+
+ FCL_REAL k = tf1.getTranslation().dot(new_s2.n) - new_s2.d;
+ FCL_REAL depth = - std::abs(k) + s1.radius;
+ if(depth >= 0)
+ {
+ if(normal) *normal = -new_s2.n;
+ if(penetration_depth) *penetration_depth = depth;
+ if(contact_points)
+ {
+ if(k < 0)
+ *contact_points = tf1.getTranslation() + new_s2.n * depth;
+ else
+ *contact_points = tf1.getTranslation() - new_s2.n * depth;
+ }
+
+ return true;
+ }
+ else
+ return false;
+}
+
+/*
+bool boxPlaneIntersect(const Box& s1, const Transform3f& tf1,
+ const Plane& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
+{
+ Plane new_s2 = transform(s2, tf2);
+
+
+}
+*/
+
+
} // details
diff --git a/trunk/fcl/src/shape/geometric_shapes.cpp b/trunk/fcl/src/shape/geometric_shapes.cpp
index 3d74f0ed..3450ce57 100644
--- a/trunk/fcl/src/shape/geometric_shapes.cpp
+++ b/trunk/fcl/src/shape/geometric_shapes.cpp
@@ -97,6 +97,22 @@ void Convex::fillEdges()
}
}
+void Halfspace::unitNormalTest()
+{
+ FCL_REAL l = n.length();
+ if(l > 0)
+ {
+ FCL_REAL inv_l = 1.0 / l;
+ n *= inv_l;
+ d *= inv_l;
+ }
+ else
+ {
+ n.setValue(1, 0, 0);
+ d = 0;
+ }
+}
+
void Plane::unitNormalTest()
{
FCL_REAL l = n.length();
@@ -156,6 +172,13 @@ void Convex::computeLocalAABB()
aabb_radius = (aabb_local.min_ - aabb_center).length();
}
+void Halfspace::computeLocalAABB()
+{
+ computeBV<AABB>(*this, Transform3f(), aabb_local);
+ aabb_center = aabb_local.center();
+ aabb_radius = (aabb_local.min_ - aabb_center).length();
+}
+
void Plane::computeLocalAABB()
{
computeBV<AABB>(*this, Transform3f(), aabb_local);
diff --git a/trunk/fcl/src/shape/geometric_shapes_utility.cpp b/trunk/fcl/src/shape/geometric_shapes_utility.cpp
index b12aa52b..bbe7423d 100644
--- a/trunk/fcl/src/shape/geometric_shapes_utility.cpp
+++ b/trunk/fcl/src/shape/geometric_shapes_utility.cpp
@@ -215,6 +215,36 @@ std::vector<Vec3f> getBoundVertices(const Triangle2& triangle, const Transform3f
} // end detail
+Halfspace transform(const Halfspace& a, const Transform3f& tf)
+{
+ /// suppose the initial halfspace is n * x <= d
+ /// after transform (R, T), x --> x' = R x + T
+ /// and the new half space becomes n' * x' <= d'
+ /// where n' = R * n
+ /// and d' = d + n' * T
+
+ Vec3f n = tf.getQuatRotation().transform(a.n);
+ FCL_REAL d = a.d + n.dot(tf.getTranslation());
+
+ return Halfspace(n, d);
+}
+
+
+Plane transform(const Plane& a, const Transform3f& tf)
+{
+ /// suppose the initial halfspace is n * x <= d
+ /// after transform (R, T), x --> x' = R x + T
+ /// and the new half space becomes n' * x' <= d'
+ /// where n' = R * n
+ /// and d' = d + n' * T
+
+ Vec3f n = tf.getQuatRotation().transform(a.n);
+ FCL_REAL d = a.d + n.dot(tf.getTranslation());
+
+ return Plane(n, d);
+}
+
+
template<>
void computeBV<AABB, Box>(const Box& s, const Transform3f& tf, AABB& bv)
@@ -308,31 +338,66 @@ void computeBV<AABB, Triangle2>(const Triangle2& s, const Transform3f& tf, AABB&
bv = AABB(tf.transform(s.a), tf.transform(s.b), tf.transform(s.c));
}
+
template<>
-void computeBV<AABB, Plane>(const Plane& s, const Transform3f& tf, AABB& bv)
+void computeBV<AABB, Halfspace>(const Halfspace& s, const Transform3f& tf, AABB& bv)
{
- const Matrix3f& R = tf.getRotation();
+ Halfspace new_s = transform(s, tf);
+ const Vec3f& n = new_s.n;
+ const FCL_REAL& d = new_s.d;
+
+ AABB bv_;
+ bv_.min_ = Vec3f(-std::numeric_limits<FCL_REAL>::max());
+ bv_.max_ = Vec3f(std::numeric_limits<FCL_REAL>::max());
+ if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
+ {
+ // normal aligned with x axis
+ if(n[0] < 0) bv_.min_[0] = -d;
+ else if(n[0] > 0) bv_.max_[0] = d;
+ }
+ else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
+ {
+ // normal aligned with y axis
+ if(n[1] < 0) bv_.min_[1] = -d;
+ else if(n[1] > 0) bv_.max_[1] = d;
+ }
+ else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
+ {
+ // normal aligned with z axis
+ if(n[2] < 0) bv_.min_[2] = -d;
+ else if(n[2] > 0) bv_.max_[2] = d;
+ }
- Vec3f n = R * s.n;
+ bv = bv_;
+}
+
+template<>
+void computeBV<AABB, Plane>(const Plane& s, const Transform3f& tf, AABB& bv)
+{
+ Plane new_s = transform(s, tf);
+ const Vec3f& n = new_s.n;
+ const FCL_REAL& d = new_s.d;
AABB bv_;
+ bv_.min_ = Vec3f(-std::numeric_limits<FCL_REAL>::max());
+ bv_.max_ = Vec3f(std::numeric_limits<FCL_REAL>::max());
if(n[1] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
{
// normal aligned with x axis
- if(n[0] < 0) bv_.min_[0] = -s.d;
- else if(n[0] > 0) bv_.max_[0] = s.d;
+ if(n[0] < 0) { bv_.min_[0] = bv_.max_[0] = -d; }
+ else if(n[0] > 0) { bv_.min_[0] = bv_.max_[0] = d; }
}
else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
{
// normal aligned with y axis
- if(n[1] < 0) bv_.min_[1] = -s.d;
- else if(n[1] > 0) bv_.max_[1] = s.d;
+ if(n[1] < 0) { bv_.min_[1] = bv_.max_[1] = -d; }
+ else if(n[1] > 0) { bv_.min_[1] = bv_.max_[1] = d; }
}
else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
{
// normal aligned with z axis
- if(n[2] < 0) bv_.min_[2] = -s.d;
- else if(n[2] > 0) bv_.max_[2] = s.d;
+ if(n[2] < 0) { bv_.min_[2] = bv_.max_[2] = -d; }
+ else if(n[2] > 0) { bv_.min_[2] = bv_.max_[2] = d; }
}
bv = bv_;
@@ -419,59 +484,454 @@ void computeBV<OBB, Convex>(const Convex& s, const Transform3f& tf, OBB& bv)
}
template<>
-void computeBV<OBB, Plane>(const Plane& s, const Transform3f& tf, OBB& bv)
+void computeBV<OBB, Halfspace>(const Halfspace& s, const Transform3f& tf, OBB& bv)
{
- const Matrix3f& R = tf.getRotation();
- const Vec3f& T = tf.getTranslation();
+ /// Half space can only have very rough OBB
+ bv.axis[0] = Vec3f(1, 0, 0);
+ bv.axis[1] = Vec3f(0, 1, 0);
+ bv.axis[2] = Vec3f(0, 0, 1);
+ bv.To = Vec3f(0, 0, 0);
+ bv.extent.setValue(std::numeric_limits<FCL_REAL>::max());
+}
+
+template<>
+void computeBV<RSS, Halfspace>(const Halfspace& s, const Transform3f& tf, RSS& bv)
+{
+ /// Half space can only have very rough RSS
+ bv.axis[0] = Vec3f(1, 0, 0);
+ bv.axis[1] = Vec3f(0, 1, 0);
+ bv.axis[2] = Vec3f(0, 0, 1);
+ bv.Tr = Vec3f(0, 0, 0);
+ bv.l[0] = bv.l[1] = bv.r = std::numeric_limits<FCL_REAL>::max();
+}
+
+template<>
+void computeBV<OBBRSS, Halfspace>(const Halfspace& s, const Transform3f& tf, OBBRSS& bv)
+{
+ computeBV<OBB, Halfspace>(s, tf, bv.obb);
+ computeBV<RSS, Halfspace>(s, tf, bv.rss);
+}
+
+template<>
+void computeBV<kIOS, Halfspace>(const Halfspace& s, const Transform3f& tf, kIOS& bv)
+{
+ bv.num_spheres = 1;
+ computeBV<OBB, Halfspace>(s, tf, bv.obb);
+ bv.spheres[0].o = Vec3f();
+ bv.spheres[0].r = std::numeric_limits<FCL_REAL>::max();
+}
+
+template<>
+void computeBV<KDOP<16>, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<16>& bv)
+{
+ Halfspace new_s = transform(s, tf);
+ 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)
+ bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
+ for(std::size_t 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)
+ {
+ if(n[0] > 0) bv.dist(D) = d;
+ else bv.dist(0) = -d;
+ }
+ else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
+ {
+ if(n[1] > 0) bv.dist(D + 1) = d;
+ else bv.dist(1) = -d;
+ }
+ else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
+ {
+ if(n[2] > 0) bv.dist(D + 2) = d;
+ else bv.dist(2) = -d;
+ }
+ else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1])
+ {
+ if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2;
+ else bv.dist(3) = n[0] * d * 2;
+ }
+ else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2])
+ {
+ if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2;
+ else bv.dist(4) = n[0] * d * 2;
+ }
+ else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2])
+ {
+ if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2;
+ else bv.dist(5) = n[1] * d * 2;
+ }
+ else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
+ {
+ if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2;
+ else bv.dist(6) = n[0] * d * 2;
+ }
+ else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
+ {
+ if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2;
+ else bv.dist(7) = n[0] * d * 2;
+ }
+}
+
+template<>
+void computeBV<KDOP<18>, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<18>& bv)
+{
+ Halfspace new_s = transform(s, tf);
+ const Vec3f& n = new_s.n;
+ const FCL_REAL& d = new_s.d;
+
+ const std::size_t D = 9;
+
+ for(std::size_t i = 0; i < D; ++i)
+ bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
+ for(std::size_t 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)
+ {
+ if(n[0] > 0) bv.dist(D) = d;
+ else bv.dist(0) = -d;
+ }
+ else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
+ {
+ if(n[1] > 0) bv.dist(D + 1) = d;
+ else bv.dist(1) = -d;
+ }
+ else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
+ {
+ if(n[2] > 0) bv.dist(D + 2) = d;
+ else bv.dist(2) = -d;
+ }
+ else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1])
+ {
+ if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2;
+ else bv.dist(3) = n[0] * d * 2;
+ }
+ else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2])
+ {
+ if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2;
+ else bv.dist(4) = n[0] * d * 2;
+ }
+ else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2])
+ {
+ if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2;
+ else bv.dist(5) = n[1] * d * 2;
+ }
+ else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
+ {
+ if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2;
+ else bv.dist(6) = n[0] * d * 2;
+ }
+ else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
+ {
+ if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2;
+ else bv.dist(7) = n[0] * d * 2;
+ }
+ else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0)
+ {
+ if(n[1] > 0) bv.dist(D + 8) = n[1] * d * 2;
+ else bv.dist(8) = n[1] * d * 2;
+ }
+}
+
+template<>
+void computeBV<KDOP<24>, Halfspace>(const Halfspace& s, const Transform3f& tf, KDOP<24>& bv)
+{
+ Halfspace new_s = transform(s, tf);
+ const Vec3f& n = new_s.n;
+ const FCL_REAL& d = new_s.d;
+
+ const std::size_t D = 12;
+
+ for(std::size_t i = 0; i < D; ++i)
+ bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
+ for(std::size_t 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)
+ {
+ if(n[0] > 0) bv.dist(D) = d;
+ else bv.dist(0) = -d;
+ }
+ else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
+ {
+ if(n[1] > 0) bv.dist(D + 1) = d;
+ else bv.dist(1) = -d;
+ }
+ else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
+ {
+ if(n[2] > 0) bv.dist(D + 2) = d;
+ else bv.dist(2) = -d;
+ }
+ else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1])
+ {
+ if(n[0] > 0) bv.dist(D + 3) = n[0] * d * 2;
+ else bv.dist(3) = n[0] * d * 2;
+ }
+ else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2])
+ {
+ if(n[1] > 0) bv.dist(D + 4) = n[0] * d * 2;
+ else bv.dist(4) = n[0] * d * 2;
+ }
+ else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2])
+ {
+ if(n[1] > 0) bv.dist(D + 5) = n[1] * d * 2;
+ else bv.dist(5) = n[1] * d * 2;
+ }
+ else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
+ {
+ if(n[0] > 0) bv.dist(D + 6) = n[0] * d * 2;
+ else bv.dist(6) = n[0] * d * 2;
+ }
+ else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
+ {
+ if(n[0] > 0) bv.dist(D + 7) = n[0] * d * 2;
+ else bv.dist(7) = n[0] * d * 2;
+ }
+ else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0)
+ {
+ if(n[1] > 0) bv.dist(D + 8) = n[1] * d * 2;
+ else bv.dist(8) = n[1] * d * 2;
+ }
+ else if(n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
+ {
+ if(n[0] > 0) bv.dist(D + 9) = n[0] * d * 3;
+ else bv.dist(9) = n[0] * d * 3;
+ }
+ else if(n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0)
+ {
+ if(n[0] > 0) bv.dist(D + 10) = n[0] * d * 3;
+ else bv.dist(10) = n[0] * d * 3;
+ }
+ else if(n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
+ {
+ if(n[1] > 0) bv.dist(D + 11) = n[1] * d * 3;
+ else bv.dist(11) = n[1] * d * 3;
+ }
+}
- // generate other two axes orthonormal to plane normal
- generateCoordinateSystem(s.n, bv.axis[1], bv.axis[2]);
- bv.axis[0] = s.n;
+
+
+template<>
+void computeBV<OBB, Plane>(const Plane& s, const Transform3f& tf, OBB& bv)
+{
+ Vec3f n = tf.getQuatRotation().transform(s.n);
+ generateCoordinateSystem(n, bv.axis[1], bv.axis[2]);
+ bv.axis[0] = n;
bv.extent.setValue(0, std::numeric_limits<FCL_REAL>::max(), std::numeric_limits<FCL_REAL>::max());
- Vec3f p = s.n * s.d;
- bv.To = R * p + T;
+ Vec3f p = s.n * s.d;
+ bv.To = tf.transform(p); /// n'd' = R * n * (d + (R * n) * T) = R * (n * d) + T
}
template<>
void computeBV<RSS, Plane>(const Plane& s, const Transform3f& tf, RSS& bv)
{
- const Matrix3f& R = tf.getRotation();
- const Vec3f& T = tf.getTranslation();
+ Vec3f n = tf.getQuatRotation().transform(s.n);
- generateCoordinateSystem(s.n, bv.axis[1], bv.axis[2]);
- bv.axis[0] = s.n;
+ generateCoordinateSystem(n, bv.axis[1], bv.axis[2]);
+ bv.axis[0] = n;
bv.l[0] = std::numeric_limits<FCL_REAL>::max();
bv.l[1] = std::numeric_limits<FCL_REAL>::max();
- bv.r = std::numeric_limits<FCL_REAL>::max();
+ bv.r = 0;
+
+ Vec3f p = s.n * s.d;
+ bv.Tr = tf.transform(p);
}
template<>
void computeBV<OBBRSS, Plane>(const Plane& s, const Transform3f& tf, OBBRSS& bv)
{
+ computeBV<OBB, Plane>(s, tf, bv.obb);
+ computeBV<RSS, Plane>(s, tf, bv.rss);
}
template<>
void computeBV<kIOS, Plane>(const Plane& s, const Transform3f& tf, kIOS& bv)
{
+ bv.num_spheres = 1;
+ computeBV<OBB, Plane>(s, tf, bv.obb);
+ bv.spheres[0].o = Vec3f();
+ bv.spheres[0].r = std::numeric_limits<FCL_REAL>::max();
}
template<>
void computeBV<KDOP<16>, Plane>(const Plane& s, const Transform3f& tf, KDOP<16>& bv)
{
+ Plane new_s = transform(s, tf);
+ 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)
+ bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
+ for(std::size_t 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)
+ {
+ if(n[0] > 0) bv.dist(0) = bv.dist(D) = d;
+ else bv.dist(0) = bv.dist(D) = -d;
+ }
+ else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
+ {
+ if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d;
+ else bv.dist(1) = bv.dist(D + 1) = -d;
+ }
+ else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
+ {
+ if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d;
+ else bv.dist(2) = bv.dist(D + 2) = -d;
+ }
+ else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1])
+ {
+ bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
+ }
+ else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2])
+ {
+ bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
+ }
+ else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2])
+ {
+ bv.dist(6) = bv.dist(D + 5) = n[1] * d * 2;
+ }
+ else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
+ {
+ bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
+ }
+ else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
+ {
+ bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
+ }
}
template<>
void computeBV<KDOP<18>, Plane>(const Plane& s, const Transform3f& tf, KDOP<18>& bv)
{
+ Plane new_s = transform(s, tf);
+ const Vec3f& n = new_s.n;
+ const FCL_REAL& d = new_s.d;
+
+ const std::size_t D = 9;
+
+ for(std::size_t i = 0; i < D; ++i)
+ bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
+ for(std::size_t 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)
+ {
+ if(n[0] > 0) bv.dist(0) = bv.dist(D) = d;
+ else bv.dist(0) = bv.dist(D) = -d;
+ }
+ else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
+ {
+ if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d;
+ else bv.dist(1) = bv.dist(D + 1) = -d;
+ }
+ else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
+ {
+ if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d;
+ else bv.dist(2) = bv.dist(D + 2) = -d;
+ }
+ else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1])
+ {
+ bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
+ }
+ else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2])
+ {
+ bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
+ }
+ else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2])
+ {
+ bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2;
+ }
+ else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
+ {
+ bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
+ }
+ else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
+ {
+ bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
+ }
+ else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0)
+ {
+ bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2;
+ }
}
template<>
void computeBV<KDOP<24>, Plane>(const Plane& s, const Transform3f& tf, KDOP<24>& bv)
{
+ Plane new_s = transform(s, tf);
+ const Vec3f& n = new_s.n;
+ const FCL_REAL& d = new_s.d;
+
+ const std::size_t D = 12;
+
+ for(std::size_t i = 0; i < D; ++i)
+ bv.dist(i) = -std::numeric_limits<FCL_REAL>::max();
+ for(std::size_t 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)
+ {
+ if(n[0] > 0) bv.dist(0) = bv.dist(D) = d;
+ else bv.dist(0) = bv.dist(D) = -d;
+ }
+ else if(n[0] == (FCL_REAL)0.0 && n[2] == (FCL_REAL)0.0)
+ {
+ if(n[1] > 0) bv.dist(1) = bv.dist(D + 1) = d;
+ else bv.dist(1) = bv.dist(D + 1) = -d;
+ }
+ else if(n[0] == (FCL_REAL)0.0 && n[1] == (FCL_REAL)0.0)
+ {
+ if(n[2] > 0) bv.dist(2) = bv.dist(D + 2) = d;
+ else bv.dist(2) = bv.dist(D + 2) = -d;
+ }
+ else if(n[2] == (FCL_REAL)0.0 && n[0] == n[1])
+ {
+ bv.dist(3) = bv.dist(D + 3) = n[0] * d * 2;
+ }
+ else if(n[1] == (FCL_REAL)0.0 && n[0] == n[2])
+ {
+ bv.dist(4) = bv.dist(D + 4) = n[0] * d * 2;
+ }
+ else if(n[0] == (FCL_REAL)0.0 && n[1] == n[2])
+ {
+ bv.dist(5) = bv.dist(D + 5) = n[1] * d * 2;
+ }
+ else if(n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
+ {
+ bv.dist(6) = bv.dist(D + 6) = n[0] * d * 2;
+ }
+ else if(n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
+ {
+ bv.dist(7) = bv.dist(D + 7) = n[0] * d * 2;
+ }
+ else if(n[0] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0)
+ {
+ bv.dist(8) = bv.dist(D + 8) = n[1] * d * 2;
+ }
+ else if(n[0] + n[2] == (FCL_REAL)0.0 && n[0] + n[1] == (FCL_REAL)0.0)
+ {
+ bv.dist(9) = bv.dist(D + 9) = n[0] * d * 3;
+ }
+ else if(n[0] + n[1] == (FCL_REAL)0.0 && n[1] + n[2] == (FCL_REAL)0.0)
+ {
+ bv.dist(10) = bv.dist(D + 10) = n[0] * d * 3;
+ }
+ else if(n[0] + n[1] == (FCL_REAL)0.0 && n[0] + n[2] == (FCL_REAL)0.0)
+ {
+ bv.dist(11) = bv.dist(D + 11) = n[1] * d * 3;
+ }
}
--
GitLab