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

@@ -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
 {

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:    
-    
 };
 
 

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

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];

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
 {

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);
 
 }
 

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_));

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;
+}
+