From 09b6b7e47a5623a22654ab81e013edbbede29d45 Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Wed, 20 Jun 2012 18:06:58 +0000
Subject: [PATCH] The shape collision basic code is finished. Support both
collision and distance. can use gjk in libccd or use independent gjk. tested
on simple examples. remove geometric_shapes_intersect.h/cpp, replace by
gjk_libccd.h/cpp and move them to narrowphase folder
Next step is testing the external interface for shape/mesh
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@103 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/CMakeLists.txt | 2 +-
trunk/fcl/include/fcl/collision_object.h | 2 +-
.../geometric_shapes_intersect.cpp | 7 +-
.../geometric_shapes_intersect.h | 71 +-
trunk/fcl/include/fcl/deprecated/gjk.h | 412 ++++++++
trunk/fcl/include/fcl/geometric_shapes.h | 11 +
.../include/fcl/geometric_shapes_utility.h | 4 +
trunk/fcl/include/fcl/{ => narrowphase}/gjk.h | 101 +-
.../fcl/include/fcl/narrowphase/gjk_libccd.h | 170 ++++
.../fcl/include/fcl/narrowphase/narrowphase.h | 300 ++++++
trunk/fcl/include/fcl/traversal_node_shapes.h | 2 +-
trunk/fcl/src/collision_func_matrix.cpp | 2 +-
trunk/fcl/src/geometric_shapes_utility.cpp | 29 +
trunk/fcl/src/gjk.cpp | 63 +-
trunk/fcl/src/gjk_libccd.cpp | 959 ++++++++++++++++++
15 files changed, 1990 insertions(+), 145 deletions(-)
rename trunk/fcl/{src => include/fcl/deprecated}/geometric_shapes_intersect.cpp (99%)
rename trunk/fcl/include/fcl/{ => deprecated}/geometric_shapes_intersect.h (76%)
create mode 100644 trunk/fcl/include/fcl/deprecated/gjk.h
rename trunk/fcl/include/fcl/{ => narrowphase}/gjk.h (71%)
create mode 100644 trunk/fcl/include/fcl/narrowphase/gjk_libccd.h
create mode 100644 trunk/fcl/include/fcl/narrowphase/narrowphase.h
create mode 100644 trunk/fcl/src/gjk_libccd.cpp
diff --git a/trunk/fcl/CMakeLists.txt b/trunk/fcl/CMakeLists.txt
index 3cdce3a6..780aca93 100644
--- a/trunk/fcl/CMakeLists.txt
+++ b/trunk/fcl/CMakeLists.txt
@@ -37,7 +37,7 @@ link_directories(${CCD_LIBRARY_DIRS})
add_definitions(-DUSE_SVMLIGHT=0)
-add_library(${PROJECT_NAME} SHARED src/BV/AABB.cpp src/BV/OBB.cpp src/BV/RSS.cpp src/BV/kIOS.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/geometric_shapes_intersect.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp src/conservative_advancement.cpp src/matrix_3f.cpp src/interval.cpp src/interval_vector.cpp src/interval_matrix.cpp src/taylor_model.cpp src/taylor_vector.cpp src/taylor_matrix.cpp src/distance_func_matrix.cpp src/distance.cpp src/gjk.cpp)
+add_library(${PROJECT_NAME} SHARED src/BV/AABB.cpp src/BV/OBB.cpp src/BV/RSS.cpp src/BV/kIOS.cpp src/traversal_node_base.cpp src/traversal_node_bvhs.cpp src/intersect.cpp src/motion.cpp src/BV_fitter.cpp src/BV_splitter.cpp src/BVH_model.cpp src/BVH_utility.cpp src/transform.cpp src/simple_setup.cpp src/geometric_shapes.cpp src/geometric_shapes_utility.cpp src/gjk_libccd.cpp src/collision_node.cpp src/traversal_recurse.cpp src/broad_phase_collision.cpp src/collision.cpp src/collision_func_matrix.cpp src/interval_tree.cpp src/conservative_advancement.cpp src/matrix_3f.cpp src/interval.cpp src/interval_vector.cpp src/interval_matrix.cpp src/taylor_model.cpp src/taylor_vector.cpp src/taylor_matrix.cpp src/distance_func_matrix.cpp src/distance.cpp src/gjk.cpp)
target_link_libraries(${PROJECT_NAME} ${FLANN_LIBRARIES} ${CCD_LIBRARIES})
diff --git a/trunk/fcl/include/fcl/collision_object.h b/trunk/fcl/include/fcl/collision_object.h
index 5de9b44c..9a274085 100644
--- a/trunk/fcl/include/fcl/collision_object.h
+++ b/trunk/fcl/include/fcl/collision_object.h
@@ -48,7 +48,7 @@ namespace fcl
enum OBJECT_TYPE {OT_UNKNOWN, OT_BVH, OT_GEOM};
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_BOX, GEOM_SPHERE, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE, GEOM_TRIANGLE};
class CollisionGeometry
{
diff --git a/trunk/fcl/src/geometric_shapes_intersect.cpp b/trunk/fcl/include/fcl/deprecated/geometric_shapes_intersect.cpp
similarity index 99%
rename from trunk/fcl/src/geometric_shapes_intersect.cpp
rename to trunk/fcl/include/fcl/deprecated/geometric_shapes_intersect.cpp
index ecf63767..46dfe608 100644
--- a/trunk/fcl/src/geometric_shapes_intersect.cpp
+++ b/trunk/fcl/include/fcl/deprecated/geometric_shapes_intersect.cpp
@@ -42,6 +42,9 @@
namespace fcl
{
+namespace details
+{
+
struct ccd_obj_t
{
ccd_vec3_t pos;
@@ -951,4 +954,6 @@ void triDeleteGJKObject(void* o_)
delete o;
}
-}
+} // details
+
+} // fcl
diff --git a/trunk/fcl/include/fcl/geometric_shapes_intersect.h b/trunk/fcl/include/fcl/deprecated/geometric_shapes_intersect.h
similarity index 76%
rename from trunk/fcl/include/fcl/geometric_shapes_intersect.h
rename to trunk/fcl/include/fcl/deprecated/geometric_shapes_intersect.h
index 2ff0129c..a6ed5b38 100644
--- a/trunk/fcl/include/fcl/geometric_shapes_intersect.h
+++ b/trunk/fcl/include/fcl/deprecated/geometric_shapes_intersect.h
@@ -47,6 +47,9 @@
namespace fcl
{
+namespace details
+{
+
/** \brief recall function used by GJK algorithm */
typedef void (*GJKSupportFunction)(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v);
typedef void (*GJKCenterFunction)(const void* obj, ccd_vec3_t* c);
@@ -154,6 +157,13 @@ bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
void* obj2, ccd_support_fn supp2, ccd_center_fn cen2,
Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal);
+bool GJKDistance(void* obj1, ccd_support_fn supp1,
+ void* obj2, ccd_support_fn supp2,
+ BVH_REAL* dist);
+
+
+} // details
+
/** intersection checking between two shapes */
template<typename S1, typename S2>
@@ -161,15 +171,15 @@ bool shapeIntersect(const S1& s1, const SimpleTransform& tf1,
const S2& s2, const SimpleTransform& tf2,
Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
{
- void* o1 = GJKInitializer<S1>::createGJKObject(s1, tf1);
- void* o2 = GJKInitializer<S2>::createGJKObject(s2, tf2);
+ void* o1 = details::GJKInitializer<S1>::createGJKObject(s1, tf1);
+ void* o2 = details::GJKInitializer<S2>::createGJKObject(s2, tf2);
- bool res = GJKCollide(o1, GJKInitializer<S1>::getSupportFunction(), GJKInitializer<S1>::getCenterFunction(),
- o2, GJKInitializer<S2>::getSupportFunction(), GJKInitializer<S2>::getCenterFunction(),
- contact_points, penetration_depth, normal);
+ bool res = details::GJKCollide(o1, details::GJKInitializer<S1>::getSupportFunction(), details::GJKInitializer<S1>::getCenterFunction(),
+ o2, details::GJKInitializer<S2>::getSupportFunction(), details::GJKInitializer<S2>::getCenterFunction(),
+ contact_points, penetration_depth, normal);
- GJKInitializer<S1>::deleteGJKObject(o1);
- GJKInitializer<S2>::deleteGJKObject(o2);
+ details::GJKInitializer<S1>::deleteGJKObject(o1);
+ details::GJKInitializer<S2>::deleteGJKObject(o2);
return res;
}
@@ -179,15 +189,15 @@ template<typename S>
bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
{
- void* o1 = GJKInitializer<S>::createGJKObject(s, tf);
- void* o2 = triCreateGJKObject(P1, P2, P3);
+ void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf);
+ void* o2 = details::triCreateGJKObject(P1, P2, P3);
- bool res = GJKCollide(o1, GJKInitializer<S>::getSupportFunction(), GJKInitializer<S>::getCenterFunction(),
- o2, triGetSupportFunction(), triGetCenterFunction(),
- contact_points, penetration_depth, normal);
+ bool res = details::GJKCollide(o1, details::GJKInitializer<S>::getSupportFunction(), details::GJKInitializer<S>::getCenterFunction(),
+ o2, details::triGetSupportFunction(), details::triGetCenterFunction(),
+ contact_points, penetration_depth, normal);
- GJKInitializer<S>::deleteGJKObject(o1);
- triDeleteGJKObject(o2);
+ details::GJKInitializer<S>::deleteGJKObject(o1);
+ details::triDeleteGJKObject(o2);
return res;
}
@@ -198,42 +208,37 @@ bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf,
const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
{
- void* o1 = GJKInitializer<S>::createGJKObject(s, tf);
- void* o2 = triCreateGJKObject(P1, P2, P3, R, T);
+ void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf);
+ void* o2 = details::triCreateGJKObject(P1, P2, P3, R, T);
- bool res = GJKCollide(o1, GJKInitializer<S>::getSupportFunction(), GJKInitializer<S>::getCenterFunction(),
- o2, triGetSupportFunction(), triGetCenterFunction(),
- contact_points, penetration_depth, normal);
+ bool res = details::GJKCollide(o1, details::GJKInitializer<S>::getSupportFunction(), details::GJKInitializer<S>::getCenterFunction(),
+ o2, details::triGetSupportFunction(), details::triGetCenterFunction(),
+ contact_points, penetration_depth, normal);
- GJKInitializer<S>::deleteGJKObject(o1);
- triDeleteGJKObject(o2);
+ details::GJKInitializer<S>::deleteGJKObject(o1);
+ details::triDeleteGJKObject(o2);
return res;
}
-bool GJKDistance(void* obj1, ccd_support_fn supp1,
- void* obj2, ccd_support_fn supp2,
- BVH_REAL* dist);
-
-
/** \brief distance computation between two shapes */
template<typename S1, typename S2>
bool shapeDistance(const S1& s1, const SimpleTransform& tf1,
const S2& s2, const SimpleTransform& tf2,
BVH_REAL* dist)
{
- void* o1 = GJKInitializer<S1>::createGJKObject(s1, tf1);
- void* o2 = GJKInitializer<S2>::createGJKObject(s2, tf2);
+ void* o1 = details::GJKInitializer<S1>::createGJKObject(s1, tf1);
+ void* o2 = details::GJKInitializer<S2>::createGJKObject(s2, tf2);
- bool res = GJKDistance(o1, GJKInitializer<S1>::getSupportFunction(),
- o2, GJKInitializer<S2>::getSupportFunction(),
- dist);
+ bool res = details::GJKDistance(o1, details::GJKInitializer<S1>::getSupportFunction(),
+ o2, details::GJKInitializer<S2>::getSupportFunction(),
+ dist);
if(*dist > 0) *dist = std::sqrt(*dist);
- GJKInitializer<S1>::deleteGJKObject(o1);
- GJKInitializer<S2>::deleteGJKObject(o2);
+ details::GJKInitializer<S1>::deleteGJKObject(o1);
+ details::GJKInitializer<S2>::deleteGJKObject(o2);
return res;
}
diff --git a/trunk/fcl/include/fcl/deprecated/gjk.h b/trunk/fcl/include/fcl/deprecated/gjk.h
new file mode 100644
index 00000000..595f298d
--- /dev/null
+++ b/trunk/fcl/include/fcl/deprecated/gjk.h
@@ -0,0 +1,412 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jia Pan */
+
+#ifndef FCL_GJK_H
+#define FCL_GJK_H
+
+#include "fcl/geometric_shapes.h"
+#include "fcl/matrix_3f.h"
+#include "fcl/transform.h"
+
+namespace fcl
+{
+
+namespace details
+{
+
+Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir);
+
+struct MinkowskiDiff
+{
+ const ShapeBase* shapes[2];
+ Matrix3f toshape1;
+ SimpleTransform toshape0;
+
+ MinkowskiDiff() { }
+
+ inline Vec3f support0(const Vec3f& d) const
+ {
+ return getSupport(shapes[0], d);
+ }
+
+ inline Vec3f support1(const Vec3f& d) const
+ {
+ return toshape0.transform(getSupport(shapes[1], toshape1 * d));
+ }
+
+ inline Vec3f support(const Vec3f& d) const
+ {
+ return support0(d) - support1(-d);
+ }
+
+ inline Vec3f support(const Vec3f& d, size_t index) const
+ {
+ if(index)
+ return support1(d);
+ else
+ return support0(d);
+ }
+
+};
+
+
+BVH_REAL projectOrigin(const Vec3f& a, const Vec3f& b, BVH_REAL* w, size_t& m);
+
+BVH_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, BVH_REAL* w, size_t& m);
+
+BVH_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d, BVH_REAL* w, size_t& m);
+
+
+static const BVH_REAL GJK_EPS = 0.000001;
+static const size_t GJK_MAX_ITERATIONS = 128;
+
+struct GJK
+{
+ struct SimplexV
+ {
+ Vec3f d; // support direction
+ Vec3f w; // support vector
+ };
+
+ struct Simplex
+ {
+ SimplexV* c[4]; // simplex vertex
+ BVH_REAL p[4]; // weight
+ size_t rank; // size of simplex (number of vertices)
+ };
+
+ enum Status {Valid, Inside, Failed};
+
+ MinkowskiDiff shape;
+ Vec3f ray;
+ BVH_REAL distance;
+ Simplex simplices[2];
+
+
+ GJK() { initialize(); }
+
+ void initialize();
+
+ Status evaluate(const MinkowskiDiff& shape_, const Vec3f& guess);
+
+ void getSupport(const Vec3f& d, SimplexV& sv) const;
+
+ void removeVertex(Simplex& simplex);
+
+ void appendVertex(Simplex& simplex, const Vec3f& v);
+
+ bool encloseOrigin();
+
+ inline Simplex* getSimplex() const
+ {
+ return simplex;
+ }
+
+private:
+ SimplexV store_v[4];
+ SimplexV* free_v[4];
+ size_t nfree;
+ size_t current;
+ Simplex* simplex;
+ Status status;
+};
+
+
+static const size_t EPA_MAX_FACES = 128;
+static const size_t EPA_MAX_VERTICES = 64;
+static const BVH_REAL EPA_EPS = 0.000001;
+static const size_t EPA_MAX_ITERATIONS = 255;
+
+struct EPA
+{
+private:
+ typedef GJK::SimplexV SimplexV;
+ struct SimplexF
+ {
+ Vec3f n;
+ BVH_REAL d;
+ SimplexV* c[3]; // a face has three vertices
+ SimplexF* f[3]; // a face has three adjacent faces
+ SimplexF* l[2]; // the pre and post faces in the list
+ size_t e[3];
+ size_t pass;
+ };
+
+ struct SimplexList
+ {
+ SimplexF* root;
+ size_t count;
+ SimplexList() : root(NULL), count(0) {}
+ void append(SimplexF* face)
+ {
+ face->l[0] = NULL;
+ face->l[1] = root;
+ if(root) root->l[0] = face;
+ root = face;
+ ++count;
+ }
+
+ void remove(SimplexF* face)
+ {
+ if(face->l[1]) face->l[1]->l[0] = face->l[0];
+ if(face->l[0]) face->l[0]->l[1] = face->l[1];
+ if(face == root) root = face->l[1];
+ --count;
+ }
+ };
+
+ static inline void bind(SimplexF* fa, size_t ea, SimplexF* fb, size_t eb)
+ {
+ fa->e[ea] = eb; fa->f[ea] = fb;
+ fb->e[eb] = ea; fb->f[eb] = fa;
+ }
+
+ struct SimplexHorizon
+ {
+ SimplexF* cf; // current face in the horizon
+ SimplexF* ff; // first face in the horizon
+ size_t nf; // number of faces in the horizon
+ SimplexHorizon() : cf(NULL), ff(NULL), nf(0) {}
+ };
+
+public:
+
+ enum Status {Valid, Touching, Degenerated, NonConvex, InvalidHull, OutOfFaces, OutOfVertices, AccuracyReached, FallBack, Failed};
+
+ Status status;
+ GJK::Simplex result;
+ Vec3f normal;
+ BVH_REAL depth;
+ SimplexV sv_store[EPA_MAX_VERTICES];
+ SimplexF fc_store[EPA_MAX_FACES];
+ size_t nextsv;
+ SimplexList hull, stock;
+
+ EPA()
+ {
+ initialize();
+ }
+
+ void initialize();
+
+ bool getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, BVH_REAL& dist);
+
+ SimplexF* newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced);
+
+ /** \brief Find the best polytope face to split */
+ SimplexF* findBest();
+
+ Status evaluate(GJK& gjk, const Vec3f& guess);
+
+ /** \brief the goal is to add a face connecting vertex w and face edge f[e] */
+ bool expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon);
+};
+
+
+} // details
+
+
+
+
+
+template<typename S1, typename S2>
+bool shapeDistance2(const S1& s1, const SimpleTransform& tf1,
+ const S2& s2, const SimpleTransform& tf2,
+ BVH_REAL* distance)
+{
+ Vec3f guess(1, 0, 0);
+ details::MinkowskiDiff shape;
+ shape.shapes[0] = &s1;
+ shape.shapes[1] = &s2;
+ shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
+ shape.toshape0 = tf1.inverseTimes(tf2);
+
+ details::GJK gjk;
+ details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
+ if(gjk_status == details::GJK::Valid)
+ {
+ Vec3f w0, w1;
+ for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
+ {
+ BVH_REAL p = gjk.getSimplex()->p[i];
+ w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
+ w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
+ }
+
+ *distance = (w0 - w1).length();
+ return true;
+ }
+ else
+ {
+ *distance = -1;
+ return false;
+ }
+}
+
+template<typename S1, typename S2>
+bool shapeIntersect2(const S1& s1, const SimpleTransform& tf1,
+ const S2& s2, const SimpleTransform& tf2,
+ Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
+{
+ Vec3f guess(1, 0, 0);
+ details::MinkowskiDiff shape;
+ shape.shapes[0] = &s1;
+ shape.shapes[1] = &s2;
+ shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
+ shape.toshape0 = tf1.inverseTimes(tf2);
+
+ details::GJK gjk;
+ details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
+ switch(gjk_status)
+ {
+ case details::GJK::Inside:
+ {
+ details::EPA epa;
+ details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
+ if(epa_status != details::EPA::Failed)
+ {
+ Vec3f w0;
+ for(size_t i = 0; i < epa.result.rank; ++i)
+ {
+ w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
+ }
+ if(penetration_depth) *penetration_depth = -epa.depth;
+ if(normal) *normal = -epa.normal;
+ if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
+ return true;
+ }
+ else return false;
+ }
+ break;
+ default:
+ ;
+ }
+
+ return false;
+}
+
+
+template<typename S>
+bool shapeTriangleIntersect2(const S& s, const SimpleTransform& tf,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
+ Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
+{
+ Triangle2 tri(P1, P2, P3);
+ Vec3f guess(1, 0, 0);
+ details::MinkowskiDiff shape;
+ shape.shapes[0] = &s;
+ shape.shapes[1] = &tri;
+ shape.toshape1 = tf.getRotation();
+ shape.toshape0 = tf.inverse();
+
+ details::GJK gjk;
+ details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
+ switch(gjk_status)
+ {
+ case details::GJK::Inside:
+ {
+ details::EPA epa;
+ details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
+ if(epa_status != details::EPA::Failed)
+ {
+ Vec3f w0;
+ for(size_t i = 0; i < epa.result.rank; ++i)
+ {
+ w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
+ }
+ if(penetration_depth) *penetration_depth = -epa.depth;
+ if(normal) *normal = -epa.normal;
+ if(contact_points) *contact_points = tf.transform(w0 - epa.normal*(epa.depth *0.5));
+ return true;
+ }
+ else return false;
+ }
+ break;
+ default:
+ ;
+ }
+
+ return false;
+}
+
+template<typename S>
+bool shapeTriangleIntersect2(const S& s, const SimpleTransform& tf,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
+ Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
+{
+ Triangle2 tri(P1, P2, P3);
+ SimpleTransform tf2(R, T);
+ Vec3f guess(1, 0, 0);
+ details::MinkowskiDiff shape;
+ shape.shapes[0] = &s;
+ shape.shapes[1] = &tri;
+ shape.toshape1 = tf2.getRotation().transposeTimes(tf.getRotation());
+ shape.toshape0 = tf.inverseTimes(tf2);
+
+ details::GJK gjk;
+ details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
+ switch(gjk_status)
+ {
+ case details::GJK::Inside:
+ {
+ details::EPA epa;
+ details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
+ if(epa_status != details::EPA::Failed)
+ {
+ Vec3f w0;
+ for(size_t i = 0; i < epa.result.rank; ++i)
+ {
+ w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
+ }
+ if(penetration_depth) *penetration_depth = -epa.depth;
+ if(normal) *normal = -epa.normal;
+ if(contact_points) *contact_points = tf.transform(w0 - epa.normal*(epa.depth *0.5));
+ return true;
+ }
+ else return false;
+ }
+ break;
+ default:
+ ;
+ }
+
+ return false;
+}
+
+}
+
+
+#endif
diff --git a/trunk/fcl/include/fcl/geometric_shapes.h b/trunk/fcl/include/fcl/geometric_shapes.h
index 608fe846..0a317ab7 100644
--- a/trunk/fcl/include/fcl/geometric_shapes.h
+++ b/trunk/fcl/include/fcl/geometric_shapes.h
@@ -57,6 +57,17 @@ public:
OBJECT_TYPE getObjectType() const { return OT_GEOM; }
};
+class Triangle2 : public ShapeBase
+{
+public:
+ Triangle2(const Vec3f& a_, const Vec3f& b_, const Vec3f& c_) : a(a_), b(b_), c(c_) {}
+
+ void computeLocalAABB();
+
+ NODE_TYPE getNodeType() const { return GEOM_TRIANGLE; }
+
+ Vec3f a, b, c;
+};
/** Center at zero point, axis aligned box */
class Box : public ShapeBase
diff --git a/trunk/fcl/include/fcl/geometric_shapes_utility.h b/trunk/fcl/include/fcl/geometric_shapes_utility.h
index 208a3b6d..8e1f04aa 100644
--- a/trunk/fcl/include/fcl/geometric_shapes_utility.h
+++ b/trunk/fcl/include/fcl/geometric_shapes_utility.h
@@ -53,6 +53,7 @@ std::vector<Vec3f> getBoundVertices(const Capsule& capsule, const SimpleTransfor
std::vector<Vec3f> getBoundVertices(const Cone& cone, const SimpleTransform& tf);
std::vector<Vec3f> getBoundVertices(const Cylinder& cylinder, const SimpleTransform& tf);
std::vector<Vec3f> getBoundVertices(const Convex& convex, const SimpleTransform& tf);
+std::vector<Vec3f> getBoundVertices(const Triangle2& triangle, const SimpleTransform& tf);
} // end detail
@@ -81,6 +82,9 @@ void computeBV<AABB, Cylinder>(const Cylinder& s, const SimpleTransform& tf, AAB
template<>
void computeBV<AABB, Convex>(const Convex& s, const SimpleTransform& tf, AABB& bv);
+template<>
+void computeBV<AABB, Triangle2>(const Triangle2& s, const SimpleTransform& tf, AABB& bv);
+
/** \brief the bounding volume for half space back of plane for OBB, it is the plane itself */
template<>
diff --git a/trunk/fcl/include/fcl/gjk.h b/trunk/fcl/include/fcl/narrowphase/gjk.h
similarity index 71%
rename from trunk/fcl/include/fcl/gjk.h
rename to trunk/fcl/include/fcl/narrowphase/gjk.h
index ed59d44b..8e67c7b3 100644
--- a/trunk/fcl/include/fcl/gjk.h
+++ b/trunk/fcl/include/fcl/narrowphase/gjk.h
@@ -44,6 +44,8 @@
namespace fcl
{
+namespace details
+{
Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir);
@@ -80,8 +82,6 @@ struct MinkowskiDiff
};
-namespace details
-{
BVH_REAL projectOrigin(const Vec3f& a, const Vec3f& b, BVH_REAL* w, size_t& m);
@@ -89,7 +89,6 @@ BVH_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, BVH_REAL*
BVH_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d, BVH_REAL* w, size_t& m);
-}
static const BVH_REAL GJK_EPS = 0.000001;
static const size_t GJK_MAX_ITERATIONS = 128;
@@ -119,14 +118,7 @@ struct GJK
GJK() { initialize(); }
- void initialize()
- {
- ray = Vec3f();
- nfree = 0;
- status = Failed;
- current = 0;
- distance = 0.0;
- }
+ void initialize();
Status evaluate(const MinkowskiDiff& shape_, const Vec3f& guess);
@@ -160,6 +152,7 @@ static const size_t EPA_MAX_ITERATIONS = 255;
struct EPA
{
+private:
typedef GJK::SimplexV SimplexV;
struct SimplexF
{
@@ -209,6 +202,8 @@ struct EPA
SimplexHorizon() : cf(NULL), ff(NULL), nf(0) {}
};
+public:
+
enum Status {Valid, Touching, Degenerated, NonConvex, InvalidHull, OutOfFaces, OutOfVertices, AccuracyReached, FallBack, Failed};
Status status;
@@ -225,15 +220,7 @@ struct EPA
initialize();
}
- void initialize()
- {
- status = Failed;
- normal = Vec3f(0, 0, 0);
- depth = 0;
- nextsv = 0;
- for(size_t i = 0; i < EPA_MAX_FACES; ++i)
- stock.append(&fc_store[EPA_MAX_FACES-i-1]);
- }
+ void initialize();
bool getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, BVH_REAL& dist);
@@ -248,81 +235,11 @@ struct EPA
bool expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon);
};
-template<typename S1, typename S2>
-bool shapeDistance2(const S1& s1, const SimpleTransform& tf1,
- const S2& s2, const SimpleTransform& tf2,
- BVH_REAL* distance)
-{
- Vec3f guess(1, 0, 0);
- MinkowskiDiff shape;
- shape.shapes[0] = &s1;
- shape.shapes[1] = &s2;
- shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
- shape.toshape0 = tf1.inverseTimes(tf2);
-
- GJK gjk;
- GJK::Status gjk_status = gjk.evaluate(shape, -guess);
- if(gjk_status == GJK::Valid)
- {
- Vec3f w0, w1;
- for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
- {
- BVH_REAL p = gjk.getSimplex()->p[i];
- w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
- w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
- }
- *distance = (w0 - w1).length();
- return true;
- }
- else
- {
- *distance = -1;
- return false;
- }
-}
+} // details
+
-template<typename S1, typename S2>
-bool shapeIntersect2(const S1& s1, const SimpleTransform& tf1,
- const S2& s2, const SimpleTransform& tf2,
- Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
-{
- Vec3f guess(1, 0, 0);
- MinkowskiDiff shape;
- shape.shapes[0] = &s1;
- shape.shapes[1] = &s2;
- shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
- shape.toshape0 = tf1.inverseTimes(tf2);
-
- GJK gjk;
- GJK::Status gjk_status = gjk.evaluate(shape, -guess);
- switch(gjk_status)
- {
- case GJK::Inside:
- {
- EPA epa;
- EPA::Status epa_status = epa.evaluate(gjk, -guess);
- if(epa_status != EPA::Failed)
- {
- Vec3f w0;
- for(size_t i = 0; i < epa.result.rank; ++i)
- {
- w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
- }
- if(penetration_depth) *penetration_depth = -epa.depth;
- if(normal) *normal = -epa.normal;
- if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
- return true;
- }
- else return false;
- }
- break;
- default:
- ;
- }
- return false;
-}
}
diff --git a/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h b/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h
new file mode 100644
index 00000000..b14faa8e
--- /dev/null
+++ b/trunk/fcl/include/fcl/narrowphase/gjk_libccd.h
@@ -0,0 +1,170 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jia Pan */
+
+
+#ifndef FCL_GJK_LIBCCD_H
+#define FCL_GJK_LIBCCD_H
+
+#include "fcl/geometric_shapes.h"
+#include "fcl/transform.h"
+
+#include <ccd/ccd.h>
+#include <ccd/quat.h>
+
+namespace fcl
+{
+
+namespace details
+{
+
+/** \brief recall function used by GJK algorithm */
+typedef void (*GJKSupportFunction)(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v);
+typedef void (*GJKCenterFunction)(const void* obj, ccd_vec3_t* c);
+
+/** \brief initialize GJK stuffs */
+template<typename T>
+class GJKInitializer
+{
+public:
+ /** \brief Get GJK support function */
+ static GJKSupportFunction getSupportFunction() { return NULL; }
+
+ /** \brief Get GJK center function */
+ static GJKCenterFunction getCenterFunction() { return NULL; }
+
+ /** \brief Get GJK object from a shape
+ * Notice that only local transformation is applied.
+ * Gloal transformation are considered later
+ */
+ static void* createGJKObject(const T& s, const SimpleTransform& tf) { return NULL; }
+
+ /** \brief Delete GJK object */
+ static void deleteGJKObject(void* o) {}
+};
+
+/** \brief initialize GJK Cylinder */
+template<>
+class GJKInitializer<Cylinder>
+{
+public:
+ static GJKSupportFunction getSupportFunction();
+ static GJKCenterFunction getCenterFunction();
+ static void* createGJKObject(const Cylinder& s, const SimpleTransform& tf);
+ static void deleteGJKObject(void* o);
+};
+
+/** \brief initialize GJK Sphere */
+template<>
+class GJKInitializer<Sphere>
+{
+public:
+ static GJKSupportFunction getSupportFunction();
+ static GJKCenterFunction getCenterFunction();
+ static void* createGJKObject(const Sphere& s, const SimpleTransform& tf);
+ static void deleteGJKObject(void* o);
+};
+
+/** \brief initialize GJK Box */
+template<>
+class GJKInitializer<Box>
+{
+public:
+ static GJKSupportFunction getSupportFunction();
+ static GJKCenterFunction getCenterFunction();
+ static void* createGJKObject(const Box& s, const SimpleTransform& tf);
+ static void deleteGJKObject(void* o);
+};
+
+/** \brief initialize GJK Capsule */
+template<>
+class GJKInitializer<Capsule>
+{
+public:
+ static GJKSupportFunction getSupportFunction();
+ static GJKCenterFunction getCenterFunction();
+ static void* createGJKObject(const Capsule& s, const SimpleTransform& tf);
+ static void deleteGJKObject(void* o);
+};
+
+/** \brief initialize GJK Cone */
+template<>
+class GJKInitializer<Cone>
+{
+public:
+ static GJKSupportFunction getSupportFunction();
+ static GJKCenterFunction getCenterFunction();
+ static void* createGJKObject(const Cone& s, const SimpleTransform& tf);
+ static void deleteGJKObject(void* o);
+};
+
+/** \brief initialize GJK Convex */
+template<>
+class GJKInitializer<Convex>
+{
+public:
+ static GJKSupportFunction getSupportFunction();
+ static GJKCenterFunction getCenterFunction();
+ static void* createGJKObject(const Convex& s, const SimpleTransform& tf);
+ static void deleteGJKObject(void* o);
+};
+
+/** \brief initialize GJK Triangle */
+GJKSupportFunction triGetSupportFunction();
+
+GJKCenterFunction triGetCenterFunction();
+
+void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3);
+
+void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T);
+
+void triDeleteGJKObject(void* o);
+
+/** \brief GJK collision algorithm */
+bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
+ void* obj2, ccd_support_fn supp2, ccd_center_fn cen2,
+ Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal);
+
+bool GJKDistance(void* obj1, ccd_support_fn supp1,
+ void* obj2, ccd_support_fn supp2,
+ BVH_REAL* dist);
+
+
+} // details
+
+
+}
+
+#endif
diff --git a/trunk/fcl/include/fcl/narrowphase/narrowphase.h b/trunk/fcl/include/fcl/narrowphase/narrowphase.h
new file mode 100644
index 00000000..736ef0c7
--- /dev/null
+++ b/trunk/fcl/include/fcl/narrowphase/narrowphase.h
@@ -0,0 +1,300 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jia Pan */
+
+#ifndef FCL_NARROWPHASE_H
+#define FCL_NARROWPHASE_H
+
+#include "fcl/narrowphase/gjk.h"
+#include "fcl/narrowphase/gjk_libccd.h"
+
+namespace fcl
+{
+
+/** intersection checking between two shapes */
+template<typename S1, typename S2>
+bool shapeIntersect(const S1& s1, const SimpleTransform& tf1,
+ const S2& s2, const SimpleTransform& tf2,
+ Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
+{
+ void* o1 = details::GJKInitializer<S1>::createGJKObject(s1, tf1);
+ void* o2 = details::GJKInitializer<S2>::createGJKObject(s2, tf2);
+
+ bool res = details::GJKCollide(o1, details::GJKInitializer<S1>::getSupportFunction(), details::GJKInitializer<S1>::getCenterFunction(),
+ o2, details::GJKInitializer<S2>::getSupportFunction(), details::GJKInitializer<S2>::getCenterFunction(),
+ contact_points, penetration_depth, normal);
+
+ details::GJKInitializer<S1>::deleteGJKObject(o1);
+ details::GJKInitializer<S2>::deleteGJKObject(o2);
+
+ return res;
+}
+
+/** \brief intersection checking between one shape and a triangle */
+template<typename S>
+bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
+{
+ void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf);
+ void* o2 = details::triCreateGJKObject(P1, P2, P3);
+
+ bool res = details::GJKCollide(o1, details::GJKInitializer<S>::getSupportFunction(), details::GJKInitializer<S>::getCenterFunction(),
+ o2, details::triGetSupportFunction(), details::triGetCenterFunction(),
+ contact_points, penetration_depth, normal);
+
+ details::GJKInitializer<S>::deleteGJKObject(o1);
+ details::triDeleteGJKObject(o2);
+
+ return res;
+}
+
+/** \brief intersection checking between one shape and a triangle with transformation */
+template<typename S>
+bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
+ Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
+{
+ void* o1 = details::GJKInitializer<S>::createGJKObject(s, tf);
+ void* o2 = details::triCreateGJKObject(P1, P2, P3, R, T);
+
+ bool res = details::GJKCollide(o1, details::GJKInitializer<S>::getSupportFunction(), details::GJKInitializer<S>::getCenterFunction(),
+ o2, details::triGetSupportFunction(), details::triGetCenterFunction(),
+ contact_points, penetration_depth, normal);
+
+ details::GJKInitializer<S>::deleteGJKObject(o1);
+ details::triDeleteGJKObject(o2);
+
+ return res;
+}
+
+
+/** \brief distance computation between two shapes */
+template<typename S1, typename S2>
+bool shapeDistance(const S1& s1, const SimpleTransform& tf1,
+ const S2& s2, const SimpleTransform& tf2,
+ BVH_REAL* dist)
+{
+ void* o1 = details::GJKInitializer<S1>::createGJKObject(s1, tf1);
+ void* o2 = details::GJKInitializer<S2>::createGJKObject(s2, tf2);
+
+ bool res = details::GJKDistance(o1, details::GJKInitializer<S1>::getSupportFunction(),
+ o2, details::GJKInitializer<S2>::getSupportFunction(),
+ dist);
+
+ if(*dist > 0) *dist = std::sqrt(*dist);
+
+ details::GJKInitializer<S1>::deleteGJKObject(o1);
+ details::GJKInitializer<S2>::deleteGJKObject(o2);
+
+ return res;
+}
+
+}
+
+
+
+namespace fcl
+{
+
+
+
+template<typename S1, typename S2>
+bool shapeDistance2(const S1& s1, const SimpleTransform& tf1,
+ const S2& s2, const SimpleTransform& tf2,
+ BVH_REAL* distance)
+{
+ Vec3f guess(1, 0, 0);
+ details::MinkowskiDiff shape;
+ shape.shapes[0] = &s1;
+ shape.shapes[1] = &s2;
+ shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
+ shape.toshape0 = tf1.inverseTimes(tf2);
+
+ details::GJK gjk;
+ details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
+ if(gjk_status == details::GJK::Valid)
+ {
+ Vec3f w0, w1;
+ for(size_t i = 0; i < gjk.getSimplex()->rank; ++i)
+ {
+ BVH_REAL p = gjk.getSimplex()->p[i];
+ w0 += shape.support(gjk.getSimplex()->c[i]->d, 0) * p;
+ w1 += shape.support(-gjk.getSimplex()->c[i]->d, 1) * p;
+ }
+
+ *distance = (w0 - w1).length();
+ return true;
+ }
+ else
+ {
+ *distance = -1;
+ return false;
+ }
+}
+
+template<typename S1, typename S2>
+bool shapeIntersect2(const S1& s1, const SimpleTransform& tf1,
+ const S2& s2, const SimpleTransform& tf2,
+ Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
+{
+ Vec3f guess(1, 0, 0);
+ details::MinkowskiDiff shape;
+ shape.shapes[0] = &s1;
+ shape.shapes[1] = &s2;
+ shape.toshape1 = tf2.getRotation().transposeTimes(tf1.getRotation());
+ shape.toshape0 = tf1.inverseTimes(tf2);
+
+ details::GJK gjk;
+ details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
+ switch(gjk_status)
+ {
+ case details::GJK::Inside:
+ {
+ details::EPA epa;
+ details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
+ if(epa_status != details::EPA::Failed)
+ {
+ Vec3f w0;
+ for(size_t i = 0; i < epa.result.rank; ++i)
+ {
+ w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
+ }
+ if(penetration_depth) *penetration_depth = -epa.depth;
+ if(normal) *normal = -epa.normal;
+ if(contact_points) *contact_points = tf1.transform(w0 - epa.normal*(epa.depth *0.5));
+ return true;
+ }
+ else return false;
+ }
+ break;
+ default:
+ ;
+ }
+
+ return false;
+}
+
+
+template<typename S>
+bool shapeTriangleIntersect2(const S& s, const SimpleTransform& tf,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
+ Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
+{
+ Triangle2 tri(P1, P2, P3);
+ Vec3f guess(1, 0, 0);
+ details::MinkowskiDiff shape;
+ shape.shapes[0] = &s;
+ shape.shapes[1] = &tri;
+ shape.toshape1 = tf.getRotation();
+ shape.toshape0 = tf.inverse();
+
+ details::GJK gjk;
+ details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
+ switch(gjk_status)
+ {
+ case details::GJK::Inside:
+ {
+ details::EPA epa;
+ details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
+ if(epa_status != details::EPA::Failed)
+ {
+ Vec3f w0;
+ for(size_t i = 0; i < epa.result.rank; ++i)
+ {
+ w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
+ }
+ if(penetration_depth) *penetration_depth = -epa.depth;
+ if(normal) *normal = -epa.normal;
+ if(contact_points) *contact_points = tf.transform(w0 - epa.normal*(epa.depth *0.5));
+ return true;
+ }
+ else return false;
+ }
+ break;
+ default:
+ ;
+ }
+
+ return false;
+}
+
+template<typename S>
+bool shapeTriangleIntersect2(const S& s, const SimpleTransform& tf,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, const Vec3f& T,
+ Vec3f* contact_points = NULL, BVH_REAL* penetration_depth = NULL, Vec3f* normal = NULL)
+{
+ Triangle2 tri(P1, P2, P3);
+ SimpleTransform tf2(R, T);
+ Vec3f guess(1, 0, 0);
+ details::MinkowskiDiff shape;
+ shape.shapes[0] = &s;
+ shape.shapes[1] = &tri;
+ shape.toshape1 = tf2.getRotation().transposeTimes(tf.getRotation());
+ shape.toshape0 = tf.inverseTimes(tf2);
+
+ details::GJK gjk;
+ details::GJK::Status gjk_status = gjk.evaluate(shape, -guess);
+ switch(gjk_status)
+ {
+ case details::GJK::Inside:
+ {
+ details::EPA epa;
+ details::EPA::Status epa_status = epa.evaluate(gjk, -guess);
+ if(epa_status != details::EPA::Failed)
+ {
+ Vec3f w0;
+ for(size_t i = 0; i < epa.result.rank; ++i)
+ {
+ w0 += shape.support(epa.result.c[i]->d, 0) * epa.result.p[i];
+ }
+ if(penetration_depth) *penetration_depth = -epa.depth;
+ if(normal) *normal = -epa.normal;
+ if(contact_points) *contact_points = tf.transform(w0 - epa.normal*(epa.depth *0.5));
+ return true;
+ }
+ else return false;
+ }
+ break;
+ default:
+ ;
+ }
+
+ return false;
+}
+
+
+}
+
+#endif
diff --git a/trunk/fcl/include/fcl/traversal_node_shapes.h b/trunk/fcl/include/fcl/traversal_node_shapes.h
index 214a25cb..0fde38b9 100644
--- a/trunk/fcl/include/fcl/traversal_node_shapes.h
+++ b/trunk/fcl/include/fcl/traversal_node_shapes.h
@@ -39,7 +39,7 @@
#define FCL_TRAVERSAL_NODE_SHAPES_H
#include "fcl/traversal_node_base.h"
-#include "fcl/geometric_shapes_intersect.h"
+#include "fcl/narrowphase/narrowphase.h"
namespace fcl
{
diff --git a/trunk/fcl/src/collision_func_matrix.cpp b/trunk/fcl/src/collision_func_matrix.cpp
index c06c4f32..001102ea 100644
--- a/trunk/fcl/src/collision_func_matrix.cpp
+++ b/trunk/fcl/src/collision_func_matrix.cpp
@@ -42,7 +42,7 @@
#include "fcl/geometric_shapes.h"
#include "fcl/BVH_model.h"
#include "fcl/collision_node.h"
-#include "fcl/geometric_shapes_intersect.h"
+#include "fcl/narrowphase/narrowphase.h"
namespace fcl
diff --git a/trunk/fcl/src/geometric_shapes_utility.cpp b/trunk/fcl/src/geometric_shapes_utility.cpp
index 89d98a4f..045641a0 100644
--- a/trunk/fcl/src/geometric_shapes_utility.cpp
+++ b/trunk/fcl/src/geometric_shapes_utility.cpp
@@ -203,6 +203,16 @@ std::vector<Vec3f> getBoundVertices(const Convex& convex, const SimpleTransform&
return result;
}
+std::vector<Vec3f> getBoundVertices(const Triangle2& triangle, const SimpleTransform& tf)
+{
+ std::vector<Vec3f> result(3);
+ result[0] = tf.transform(triangle.a);
+ result[1] = tf.transform(triangle.b);
+ result[2] = tf.transform(triangle.c);
+
+ return result;
+}
+
} // end detail
@@ -287,6 +297,17 @@ void computeBV<AABB, Convex>(const Convex& s, const SimpleTransform& tf, AABB& b
bv = bv_;
}
+template<>
+void computeBV<AABB, Triangle2>(const Triangle2& s, const SimpleTransform& tf, AABB& bv)
+{
+ AABB bv_;
+ bv_ += tf.transform(s.a);
+ bv_ += tf.transform(s.b);
+ bv_ += tf.transform(s.c);
+
+ bv = bv_;
+}
+
template<>
void computeBV<AABB, Plane>(const Plane& s, const SimpleTransform& tf, AABB& bv)
{
@@ -509,6 +530,14 @@ void Plane::computeLocalAABB()
aabb_radius = (aabb.min_ - aabb_center).length();
}
+void Triangle2::computeLocalAABB()
+{
+ AABB aabb;
+ computeBV<AABB>(*this, SimpleTransform(), aabb);
+ aabb_center = aabb.center();
+ aabb_radius = (aabb.min_ - aabb_center).length();
+}
+
diff --git a/trunk/fcl/src/gjk.cpp b/trunk/fcl/src/gjk.cpp
index 76b3f206..d19f2f49 100644
--- a/trunk/fcl/src/gjk.cpp
+++ b/trunk/fcl/src/gjk.cpp
@@ -34,17 +34,40 @@
/** \author Jia Pan */
-#include "fcl/gjk.h"
+#include "fcl/narrowphase/gjk.h"
namespace fcl
{
-
+namespace details
+{
Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir)
{
switch(shape->getNodeType())
{
+ case GEOM_TRIANGLE:
+ {
+ const Triangle2* triangle = static_cast<const Triangle2*>(shape);
+ BVH_REAL dota = dir.dot(triangle->a);
+ BVH_REAL dotb = dir.dot(triangle->b);
+ BVH_REAL dotc = dir.dot(triangle->c);
+ if(dota > dotb)
+ {
+ if(dotc > dota)
+ return triangle->c;
+ else
+ return triangle->a;
+ }
+ else
+ {
+ if(dotc > dotb)
+ return triangle->c;
+ else
+ return triangle->b;
+ }
+ }
+ break;
case GEOM_BOX:
{
const Box* box = static_cast<const Box*>(shape);
@@ -115,14 +138,12 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir)
case GEOM_CONVEX:
{
const Convex* convex = static_cast<const Convex*>(shape);
- const Vec3f& center = convex->center;
BVH_REAL maxdot = - std::numeric_limits<BVH_REAL>::max();
Vec3f* curp = convex->points;
Vec3f bestv;
for(size_t i = 0; i < convex->num_points; ++i, curp+=1)
{
- Vec3f p = (*curp) - center;
- BVH_REAL dot = dir.dot(p);
+ BVH_REAL dot = dir.dot(*curp);
if(dot > maxdot)
{
bestv = *curp;
@@ -143,9 +164,6 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir)
}
-namespace details
-{
-
BVH_REAL projectOrigin(const Vec3f& a, const Vec3f& b, BVH_REAL* w, size_t& m)
{
const Vec3f d = b - a;
@@ -255,9 +273,14 @@ BVH_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec
}
-} // detail
-
-
+void GJK::initialize()
+{
+ ray = Vec3f();
+ nfree = 0;
+ status = Failed;
+ current = 0;
+ distance = 0.0;
+}
GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
{
@@ -337,11 +360,11 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
switch(curr_simplex.rank)
{
case 2:
- sqdist = details::projectOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w, weights, mask); break;
+ sqdist = projectOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w, weights, mask); break;
case 3:
- sqdist = details::projectOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w, curr_simplex.c[2]->w, weights, mask); break;
+ sqdist = projectOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w, curr_simplex.c[2]->w, weights, mask); break;
case 4:
- sqdist = details::projectOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w, curr_simplex.c[2]->w, curr_simplex.c[3]->w, weights, mask); break;
+ sqdist = projectOrigin(curr_simplex.c[0]->w, curr_simplex.c[1]->w, curr_simplex.c[2]->w, curr_simplex.c[3]->w, weights, mask); break;
}
if(sqdist >= 0)
@@ -464,6 +487,16 @@ bool GJK::encloseOrigin()
}
+void EPA::initialize()
+{
+ status = Failed;
+ normal = Vec3f(0, 0, 0);
+ depth = 0;
+ nextsv = 0;
+ for(size_t i = 0; i < EPA_MAX_FACES; ++i)
+ stock.append(&fc_store[EPA_MAX_FACES-i-1]);
+}
+
bool EPA::getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, BVH_REAL& dist)
{
Vec3f ba = b->w - a->w;
@@ -723,6 +756,6 @@ bool EPA::expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon
return false;
}
-
+} // details
} // fcl
diff --git a/trunk/fcl/src/gjk_libccd.cpp b/trunk/fcl/src/gjk_libccd.cpp
new file mode 100644
index 00000000..fc8e1f24
--- /dev/null
+++ b/trunk/fcl/src/gjk_libccd.cpp
@@ -0,0 +1,959 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011, Willow Garage, Inc.
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Willow Garage, Inc. nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Jia Pan */
+
+
+#include "fcl/narrowphase/gjk_libccd.h"
+#include <ccd/simplex.h>
+#include <ccd/vec3.h>
+
+namespace fcl
+{
+
+namespace details
+{
+
+struct ccd_obj_t
+{
+ ccd_vec3_t pos;
+ ccd_quat_t rot, rot_inv;
+};
+
+struct ccd_box_t : public ccd_obj_t
+{
+ ccd_real_t dim[3];
+};
+
+struct ccd_cap_t : public ccd_obj_t
+{
+ ccd_real_t radius, height;
+};
+
+struct ccd_cyl_t : public ccd_obj_t
+{
+ ccd_real_t radius, height;
+};
+
+struct ccd_cone_t : public ccd_obj_t
+{
+ ccd_real_t radius, height;
+};
+
+struct ccd_sphere_t : public ccd_obj_t
+{
+ ccd_real_t radius;
+};
+
+struct ccd_convex_t : public ccd_obj_t
+{
+ const Convex* convex;
+};
+
+struct ccd_triangle_t : public ccd_obj_t
+{
+ ccd_vec3_t p[3];
+ ccd_vec3_t c;
+};
+
+static void tripleCross(const ccd_vec3_t* a, const ccd_vec3_t* b,
+ const ccd_vec3_t* c, ccd_vec3_t* d)
+{
+ ccd_vec3_t e;
+ ccdVec3Cross(&e, a, b);
+ ccdVec3Cross(d, &e, c);
+}
+
+static int doSimplex2Dist(ccd_simplex_t *simplex, ccd_vec3_t *dir, ccd_real_t* dist)
+{
+ const ccd_support_t *A, *B;
+ ccd_vec3_t AB, AO, tmp;
+ ccd_real_t dot;
+
+ // get last added as A
+ A = ccdSimplexLast(simplex);
+ // get the other point
+ B = ccdSimplexPoint(simplex, 0);
+ // compute AB oriented segment
+ ccdVec3Sub2(&AB, &B->v, &A->v);
+ // compute AO vector
+ ccdVec3Copy(&AO, &A->v);
+ ccdVec3Scale(&AO, -CCD_ONE);
+
+ // dot product AB . -AO
+ dot = ccdVec3Dot(&AB, &AO);
+
+ // check if origin doesn't lie on AB segment
+ ccdVec3Cross(&tmp, &AB, &AO);
+ if(ccdIsZero(ccdVec3Len2(&tmp)) && dot > CCD_ZERO)
+ {
+ return 1;
+ }
+
+ *dist = ccdVec3PointSegmentDist2(ccd_vec3_origin, &A->v, &B->v, NULL);
+
+ // check if origin is in area where AB segment is
+ if(ccdIsZero(dot) || dot < CCD_ZERO)
+ {
+ // origin is in outside are of A
+ ccdSimplexSet(simplex, 0, A);
+ ccdSimplexSetSize(simplex, 1);
+ ccdVec3Copy(dir, &AO);
+ }
+ else
+ {
+ // origin is in area where AB segment is
+
+ // keep simplex untouched and set direction to
+ // AB x AO x AB
+ tripleCross(&AB, &AO, &AB, dir);
+ }
+
+ return 0;
+}
+
+static int doSimplex3Dist(ccd_simplex_t *simplex, ccd_vec3_t *dir, ccd_real_t* dist)
+{
+ const ccd_support_t *A, *B, *C;
+ ccd_vec3_t AO, AB, AC, ABC, tmp;
+ ccd_real_t dot;
+
+ // get last added as A
+ A = ccdSimplexLast(simplex);
+ // get the other points
+ B = ccdSimplexPoint(simplex, 1);
+ C = ccdSimplexPoint(simplex, 0);
+
+ // check touching contact
+ *dist = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, NULL);
+ if(ccdIsZero(*dist))
+ {
+ return 1;
+ }
+
+ // check if triangle is really triangle (has area > 0)
+ // if not simplex can't be expanded and thus no itersection is found
+ if(ccdVec3Eq(&A->v, &B->v) || ccdVec3Eq(&A->v, &C->v))
+ {
+ return -1;
+ }
+
+ // compute AO vector
+ ccdVec3Copy(&AO, &A->v);
+ ccdVec3Scale(&AO, -CCD_ONE);
+
+ // compute AB and AC segments and ABC vector (perpendircular to triangle)
+ ccdVec3Sub2(&AB, &B->v, &A->v);
+ ccdVec3Sub2(&AC, &C->v, &A->v);
+ ccdVec3Cross(&ABC, &AB, &AC);
+
+ ccdVec3Cross(&tmp, &ABC, &AC);
+ dot = ccdVec3Dot(&tmp, &AO);
+ if(ccdIsZero(dot) || dot > CCD_ZERO)
+ {
+ dot = ccdVec3Dot(&AC, &AO);
+ if(ccdIsZero(dot) || dot > CCD_ZERO)
+ {
+ // C is already in place
+ ccdSimplexSet(simplex, 1, A);
+ ccdSimplexSetSize(simplex, 2);
+ tripleCross(&AC, &AO, &AC, dir);
+ }
+ else
+ {
+ ccd_do_simplex3_45:
+ dot = ccdVec3Dot(&AB, &AO);
+ if(ccdIsZero(dot) || dot > CCD_ZERO)
+ {
+ ccdSimplexSet(simplex, 0, B);
+ ccdSimplexSet(simplex, 1, A);
+ ccdSimplexSetSize(simplex, 2);
+ tripleCross(&AB, &AO, &AB, dir);
+ }
+ else
+ {
+ ccdSimplexSet(simplex, 0, A);
+ ccdSimplexSetSize(simplex, 1);
+ ccdVec3Copy(dir, &AO);
+ }
+ }
+ }
+ else
+ {
+ ccdVec3Cross(&tmp, &AB, &ABC);
+ dot = ccdVec3Dot(&tmp, &AO);
+ if(ccdIsZero(dot) || dot > CCD_ZERO)
+ {
+ goto ccd_do_simplex3_45;
+ }
+ else
+ {
+ dot = ccdVec3Dot(&ABC, &AO);
+ if (ccdIsZero(dot) || dot > CCD_ZERO)
+ {
+ ccdVec3Copy(dir, &ABC);
+ }
+ else
+ {
+ ccd_support_t Ctmp;
+ ccdSupportCopy(&Ctmp, C);
+ ccdSimplexSet(simplex, 0, B);
+ ccdSimplexSet(simplex, 1, &Ctmp);
+
+ ccdVec3Copy(dir, &ABC);
+ ccdVec3Scale(dir, -CCD_ONE);
+ }
+ }
+ }
+
+
+ return 0;
+}
+
+static int doSimplex4Dist(ccd_simplex_t *simplex, ccd_vec3_t *dir, ccd_real_t* dist)
+{
+ const ccd_support_t *A, *B, *C, *D;
+ ccd_vec3_t AO, AB, AC, AD, ABC, ACD, ADB;
+ int B_on_ACD, C_on_ADB, D_on_ABC;
+ int AB_O, AC_O, AD_O;
+
+ // get last added as A
+ A = ccdSimplexLast(simplex);
+ // get the other points
+ B = ccdSimplexPoint(simplex, 2);
+ C = ccdSimplexPoint(simplex, 1);
+ D = ccdSimplexPoint(simplex, 0);
+
+ // check if tetrahedron is really tetrahedron (has volume > 0)
+ // if it is not simplex can't be expanded and thus no intersection is
+ // found
+ ccd_real_t volume = ccdVec3PointTriDist2(&A->v, &B->v, &C->v, &D->v, NULL);
+ if(ccdIsZero(volume))
+ {
+ return -1;
+ }
+
+ // check if origin lies on some of tetrahedron's face - if so objects
+ // intersect
+ volume = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &C->v, NULL);
+ if(ccdIsZero(volume))
+ return 1;
+ volume = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &C->v, &D->v, NULL);
+ if(ccdIsZero(volume))
+ return 1;
+ volume = ccdVec3PointTriDist2(ccd_vec3_origin, &A->v, &B->v, &D->v, NULL);
+ if(ccdIsZero(volume))
+ return 1;
+ volume = ccdVec3PointTriDist2(ccd_vec3_origin, &B->v, &C->v, &D->v, NULL);
+ if(ccdIsZero(volume))
+ return 1;
+
+ // compute AO, AB, AC, AD segments and ABC, ACD, ADB normal vectors
+ ccdVec3Copy(&AO, &A->v);
+ ccdVec3Scale(&AO, -CCD_ONE);
+ ccdVec3Sub2(&AB, &B->v, &A->v);
+ ccdVec3Sub2(&AC, &C->v, &A->v);
+ ccdVec3Sub2(&AD, &D->v, &A->v);
+ ccdVec3Cross(&ABC, &AB, &AC);
+ ccdVec3Cross(&ACD, &AC, &AD);
+ ccdVec3Cross(&ADB, &AD, &AB);
+
+ // side (positive or negative) of B, C, D relative to planes ACD, ADB
+ // and ABC respectively
+ B_on_ACD = ccdSign(ccdVec3Dot(&ACD, &AB));
+ C_on_ADB = ccdSign(ccdVec3Dot(&ADB, &AC));
+ D_on_ABC = ccdSign(ccdVec3Dot(&ABC, &AD));
+
+ // whether origin is on same side of ACD, ADB, ABC as B, C, D
+ // respectively
+ AB_O = ccdSign(ccdVec3Dot(&ACD, &AO)) == B_on_ACD;
+ AC_O = ccdSign(ccdVec3Dot(&ADB, &AO)) == C_on_ADB;
+ AD_O = ccdSign(ccdVec3Dot(&ABC, &AO)) == D_on_ABC;
+
+ if(AB_O && AC_O && AD_O)
+ {
+ // origin is in tetrahedron
+ return 1;
+
+ // rearrange simplex to triangle and call doSimplex3Dist()
+ }
+ else if(!AB_O)
+ {
+ // B is farthest from the origin among all of the tetrahedron's
+ // points, so remove it from the list and go on with the triangle
+ // case
+
+ // D and C are in place
+ ccdSimplexSet(simplex, 2, A);
+ ccdSimplexSetSize(simplex, 3);
+ }
+ else if(!AC_O)
+ {
+ // C is farthest
+ ccdSimplexSet(simplex, 1, D);
+ ccdSimplexSet(simplex, 0, B);
+ ccdSimplexSet(simplex, 2, A);
+ ccdSimplexSetSize(simplex, 3);
+ }
+ else
+ {
+ // (!AD_O)
+ ccdSimplexSet(simplex, 0, C);
+ ccdSimplexSet(simplex, 1, B);
+ ccdSimplexSet(simplex, 2, A);
+ ccdSimplexSetSize(simplex, 3);
+ }
+
+ return doSimplex3Dist(simplex, dir, dist);
+}
+
+static int doSimplexDist(ccd_simplex_t *simplex, ccd_vec3_t *dir, ccd_real_t* dist)
+{
+ if(ccdSimplexSize(simplex) == 2)
+ {
+ // simplex contains segment only one segment
+ return doSimplex2Dist(simplex, dir, dist);
+ }
+ else if(ccdSimplexSize(simplex) == 3)
+ {
+ // simplex contains triangle
+ return doSimplex3Dist(simplex, dir, dist);
+ }
+ else
+ {
+ // ccdSimplexSize(simplex) == 4
+ // tetrahedron - this is the only shape which can encapsule origin
+ // so doSimplex4() also contains test on it
+ return doSimplex4Dist(simplex, dir, dist);
+ }
+}
+
+
+static ccd_real_t __ccdGJKDist(const void *obj1, const void *obj2,
+ const ccd_t *ccd, ccd_simplex_t *simplex)
+{
+ unsigned long iterations;
+ ccd_vec3_t dir; // direction vector
+ ccd_support_t last; // last support point
+ int do_simplex_res;
+ ccd_real_t min_dist = -1;
+
+ // initialize simplex struct
+ ccdSimplexInit(simplex);
+
+ // get first direction
+ ccd->first_dir(obj1, obj2, &dir);
+ // get first support point
+ __ccdSupport(obj1, obj2, &dir, ccd, &last);
+ // and add this point to simplex as last one
+ ccdSimplexAdd(simplex, &last);
+
+ // set up direction vector to as (O - last) which is exactly -last
+ ccdVec3Copy(&dir, &last.v);
+ ccdVec3Scale(&dir, -CCD_ONE);
+
+ // start iterations
+ for (iterations = 0UL; iterations < ccd->max_iterations; ++iterations)
+ {
+ // obtain support point
+ __ccdSupport(obj1, obj2, &dir, ccd, &last);
+
+ // check if farthest point in Minkowski difference in direction dir
+ // isn't somewhere before origin (the test on negative dot product)
+ // - because if it is, objects are not intersecting at all.
+ if(ccdVec3Dot(&last.v, &dir) < CCD_ZERO)
+ {
+ ccd_real_t dist = ccdVec3Len2(&last.v);
+ if(min_dist < 0) min_dist = dist;
+ else
+ min_dist = std::min(min_dist, dist);
+ }
+
+ // add last support vector to simplex
+ ccdSimplexAdd(simplex, &last);
+
+ // if doSimplex returns 1 if objects intersect, -1 if objects don't
+ // intersect and 0 if algorithm should continue
+ ccd_real_t dist;
+ do_simplex_res = doSimplexDist(simplex, &dir, &dist);
+
+ if(do_simplex_res == 1)
+ {
+ return -1; // intersection found
+ }
+ else if(do_simplex_res == -1)
+ {
+ return min_dist;
+ }
+
+ if(ccdIsZero(ccdVec3Len2(&dir)))
+ {
+ return min_dist; // intersection not found
+ }
+
+ if(min_dist > 0)
+ {
+ if(fabs(min_dist - dist) < 0.000001 && iterations > 0)
+ break;
+ else
+ min_dist = std::min(min_dist, dist);
+ }
+ else min_dist = dist;
+ }
+
+ // intersection wasn't found
+ return min_dist;
+}
+
+
+/** Basic shape to ccd shape */
+static void shapeToGJK(const ShapeBase& s, const SimpleTransform& tf, ccd_obj_t* o)
+{
+ const SimpleQuaternion& q = tf.getQuatRotation();
+ const Vec3f& T = tf.getTranslation();
+ ccdVec3Set(&o->pos, T[0], T[1], T[2]);
+ ccdQuatSet(&o->rot, q.getX(), q.getY(), q.getZ(), q.getW());
+ ccdQuatInvert2(&o->rot_inv, &o->rot);
+}
+
+static void boxToGJK(const Box& s, const SimpleTransform& tf, ccd_box_t* box)
+{
+ shapeToGJK(s, tf, box);
+ box->dim[0] = s.side[0] / 2.0;
+ box->dim[1] = s.side[1] / 2.0;
+ box->dim[2] = s.side[2] / 2.0;
+}
+
+static void capToGJK(const Capsule& s, const SimpleTransform& tf, ccd_cap_t* cap)
+{
+ shapeToGJK(s, tf, cap);
+ cap->radius = s.radius;
+ cap->height = s.lz / 2;
+}
+
+static void cylToGJK(const Cylinder& s, const SimpleTransform& tf, ccd_cyl_t* cyl)
+{
+ shapeToGJK(s, tf, cyl);
+ cyl->radius = s.radius;
+ cyl->height = s.lz / 2;
+}
+
+static void coneToGJK(const Cone& s, const SimpleTransform& tf, ccd_cone_t* cone)
+{
+ shapeToGJK(s, tf, cone);
+ cone->radius = s.radius;
+ cone->height = s.lz / 2;
+}
+
+static void sphereToGJK(const Sphere& s, const SimpleTransform& tf, ccd_sphere_t* sph)
+{
+ shapeToGJK(s, tf, sph);
+ sph->radius = s.radius;
+}
+
+static void convexToGJK(const Convex& s, const SimpleTransform& tf, ccd_convex_t* conv)
+{
+ shapeToGJK(s, tf, conv);
+ conv->convex = &s;
+}
+
+/** Support functions */
+static void supportBox(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
+{
+ const ccd_box_t* o = (const ccd_box_t*)obj;
+ ccd_vec3_t dir;
+ ccdVec3Copy(&dir, dir_);
+ ccdQuatRotVec(&dir, &o->rot_inv);
+ ccdVec3Set(v, ccdSign(ccdVec3X(&dir)) * o->dim[0],
+ ccdSign(ccdVec3Y(&dir)) * o->dim[1],
+ ccdSign(ccdVec3Z(&dir)) * o->dim[2]);
+ ccdQuatRotVec(v, &o->rot);
+ ccdVec3Add(v, &o->pos);
+}
+
+static void supportCap(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
+{
+ const ccd_cap_t* o = (const ccd_cap_t*)obj;
+ ccd_vec3_t dir, pos1, pos2;
+
+ ccdVec3Copy(&dir, dir_);
+ ccdQuatRotVec(&dir, &o->rot_inv);
+
+ ccdVec3Set(&pos1, CCD_ZERO, CCD_ZERO, o->height);
+ ccdVec3Set(&pos2, CCD_ZERO, CCD_ZERO, -o->height);
+
+ ccdVec3Copy(v, &dir);
+ ccdVec3Scale(v, o->radius);
+ ccdVec3Add(&pos1, v);
+ ccdVec3Add(&pos2, v);
+
+ if(ccdVec3Dot(&dir, &pos1) > ccdVec3Dot(&dir, &pos2))
+ ccdVec3Copy(v, &pos1);
+ else
+ ccdVec3Copy(v, &pos2);
+
+ // transform support vertex
+ ccdQuatRotVec(v, &o->rot);
+ ccdVec3Add(v, &o->pos);
+}
+
+static void supportCyl(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
+{
+ const ccd_cyl_t* cyl = (const ccd_cyl_t*)obj;
+ ccd_vec3_t dir;
+ double zdist, rad;
+
+ ccdVec3Copy(&dir, dir_);
+ ccdQuatRotVec(&dir, &cyl->rot_inv);
+
+ zdist = dir.v[0] * dir.v[0] + dir.v[1] * dir.v[1];
+ zdist = sqrt(zdist);
+ if(ccdIsZero(zdist))
+ ccdVec3Set(v, 0., 0., ccdSign(ccdVec3Z(&dir)) * cyl->height);
+ else
+ {
+ rad = cyl->radius / zdist;
+
+ ccdVec3Set(v, rad * ccdVec3X(&dir),
+ rad * ccdVec3Y(&dir),
+ ccdSign(ccdVec3Z(&dir)) * cyl->height);
+ }
+
+ // transform support vertex
+ ccdQuatRotVec(v, &cyl->rot);
+ ccdVec3Add(v, &cyl->pos);
+}
+
+static void supportCone(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
+{
+ const ccd_cone_t* cone = (const ccd_cone_t*)obj;
+ ccd_vec3_t dir;
+
+ ccdVec3Copy(&dir, dir_);
+ ccdQuatRotVec(&dir, &cone->rot_inv);
+
+ double zdist, len, rad;
+ zdist = dir.v[0] * dir.v[0] + dir.v[1] * dir.v[1];
+ len = zdist + dir.v[2] * dir.v[2];
+ zdist = sqrt(zdist);
+ len = sqrt(len);
+
+ double sin_a = cone->radius / sqrt(cone->radius * cone->radius + 4 * cone->height * cone->height);
+
+ if(dir.v[2] > len * sin_a)
+ ccdVec3Set(v, 0., 0., cone->height);
+ else if(zdist > 0)
+ {
+ rad = cone->radius / zdist;
+ ccdVec3Set(v, rad * ccdVec3X(&dir), rad * ccdVec3Y(&dir), -cone->height);
+ }
+ else
+ ccdVec3Set(v, 0, 0, -cone->height);
+
+ // transform support vertex
+ ccdQuatRotVec(v, &cone->rot);
+ ccdVec3Add(v, &cone->pos);
+}
+
+static void supportSphere(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
+{
+ const ccd_sphere_t* s = (const ccd_sphere_t*)obj;
+ ccd_vec3_t dir;
+
+ ccdVec3Copy(&dir, dir_);
+ ccdQuatRotVec(&dir, &s->rot_inv);
+
+ ccdVec3Copy(v, &dir);
+ ccdVec3Scale(v, s->radius);
+ ccdVec3Scale(v, CCD_ONE / CCD_SQRT(ccdVec3Len2(&dir)));
+
+ // transform support vertex
+ ccdQuatRotVec(v, &s->rot);
+ ccdVec3Add(v, &s->pos);
+}
+
+static void supportConvex(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
+{
+ const ccd_convex_t* c = (const ccd_convex_t*)obj;
+ ccd_vec3_t dir, p;
+ ccd_real_t maxdot, dot;
+ int i;
+ Vec3f* curp;
+ const Vec3f& center = c->convex->center;
+
+ ccdVec3Copy(&dir, dir_);
+ ccdQuatRotVec(&dir, &c->rot_inv);
+
+ maxdot = -CCD_REAL_MAX;
+ curp = c->convex->points;
+
+ for(i = 0; i < c->convex->num_points; ++i, curp += 1)
+ {
+ ccdVec3Set(&p, (*curp)[0] - center[0], (*curp)[1] - center[1], (*curp)[2] - center[2]);
+ dot = ccdVec3Dot(&dir, &p);
+ if(dot > maxdot)
+ {
+ ccdVec3Set(v, (*curp)[0], (*curp)[1], (*curp)[2]);
+ maxdot = dot;
+ }
+ }
+
+ // transform support vertex
+ ccdQuatRotVec(v, &c->rot);
+ ccdVec3Add(v, &c->pos);
+}
+
+static void supportTriangle(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
+{
+ const ccd_triangle_t* tri = (const ccd_triangle_t*)obj;
+ ccd_vec3_t dir, p;
+ ccd_real_t maxdot, dot;
+ int i;
+
+ ccdVec3Copy(&dir, dir_);
+ ccdQuatRotVec(&dir, &tri->rot_inv);
+
+ maxdot = -CCD_REAL_MAX;
+
+ for(i = 0; i < 3; ++i)
+ {
+ ccdVec3Set(&p, tri->p[i].v[0] - tri->c.v[0], tri->p[i].v[1] - tri->c.v[1], tri->p[i].v[2] - tri->c.v[2]);
+ dot = ccdVec3Dot(&dir, &p);
+ if(dot > maxdot)
+ {
+ ccdVec3Copy(v, &tri->p[i]);
+ maxdot = dot;
+ }
+ }
+
+ // transform support vertex
+ ccdQuatRotVec(v, &tri->rot);
+ ccdVec3Add(v, &tri->pos);
+}
+
+static void centerShape(const void* obj, ccd_vec3_t* c)
+{
+ const ccd_obj_t *o = (const ccd_obj_t*)obj;
+ ccdVec3Copy(c, &o->pos);
+}
+
+static void centerConvex(const void* obj, ccd_vec3_t* c)
+{
+ const ccd_convex_t *o = (const ccd_convex_t*)obj;
+ ccdVec3Set(c, o->convex->center[0], o->convex->center[1], o->convex->center[2]);
+ ccdQuatRotVec(c, &o->rot);
+ ccdVec3Add(c, &o->pos);
+}
+
+static void centerTriangle(const void* obj, ccd_vec3_t* c)
+{
+ const ccd_triangle_t *o = (const ccd_triangle_t*)obj;
+ ccdVec3Copy(c, &o->c);
+ ccdQuatRotVec(c, &o->rot);
+ ccdVec3Add(c, &o->pos);
+}
+
+bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
+ void* obj2, ccd_support_fn supp2, ccd_center_fn cen2,
+ Vec3f* contact_points, BVH_REAL* penetration_depth, Vec3f* normal)
+{
+ ccd_t ccd;
+ int res;
+ ccd_real_t depth;
+ ccd_vec3_t dir, pos;
+
+
+ CCD_INIT(&ccd);
+ ccd.support1 = supp1;
+ ccd.support2 = supp2;
+ ccd.center1 = cen1;
+ ccd.center2 = cen2;
+ ccd.max_iterations = 500;
+ ccd.mpr_tolerance = 1e-6;
+
+ if(!contact_points)
+ {
+ return ccdMPRIntersect(obj1, obj2, &ccd);
+ }
+
+
+ res = ccdMPRPenetration(obj1, obj2, &ccd, &depth, &dir, &pos);
+ if(res == 0)
+ {
+ contact_points->setValue(ccdVec3X(&pos), ccdVec3Y(&pos), ccdVec3Z(&pos));
+ *penetration_depth = depth;
+ normal->setValue(-ccdVec3X(&dir), -ccdVec3Y(&dir), -ccdVec3Z(&dir));
+
+ return true;
+ }
+
+ return false;
+}
+
+bool GJKDistance(void* obj1, ccd_support_fn supp1,
+ void* obj2, ccd_support_fn supp2,
+ BVH_REAL* res)
+{
+ ccd_t ccd;
+ ccd_real_t dist;
+ ccd_vec3_t dir, pos;
+ CCD_INIT(&ccd);
+ ccd.support1 = supp1;
+ ccd.support2 = supp2;
+
+ ccd.max_iterations = 500;
+
+ ccd_simplex_t simplex;
+ dist = __ccdGJKDist(obj1, obj2, &ccd, &simplex);
+ *res = dist;
+ if(dist < 0) return false;
+ else return true;
+}
+
+
+GJKSupportFunction GJKInitializer<Cylinder>::getSupportFunction()
+{
+ return &supportCyl;
+}
+
+
+GJKCenterFunction GJKInitializer<Cylinder>::getCenterFunction()
+{
+ return ¢erShape;
+}
+
+
+void* GJKInitializer<Cylinder>::createGJKObject(const Cylinder& s, const SimpleTransform& tf)
+{
+ ccd_cyl_t* o = new ccd_cyl_t;
+ cylToGJK(s, tf, o);
+ return o;
+}
+
+
+void GJKInitializer<Cylinder>::deleteGJKObject(void* o_)
+{
+ ccd_cyl_t* o = (ccd_cyl_t*)o_;
+ delete o;
+}
+
+
+GJKSupportFunction GJKInitializer<Sphere>::getSupportFunction()
+{
+ return &supportSphere;
+}
+
+
+GJKCenterFunction GJKInitializer<Sphere>::getCenterFunction()
+{
+ return ¢erShape;
+}
+
+
+void* GJKInitializer<Sphere>::createGJKObject(const Sphere& s, const SimpleTransform& tf)
+{
+ ccd_sphere_t* o = new ccd_sphere_t;
+ sphereToGJK(s, tf, o);
+ return o;
+}
+
+void GJKInitializer<Sphere>::deleteGJKObject(void* o_)
+{
+ ccd_sphere_t* o = (ccd_sphere_t*)o_;
+ delete o;
+}
+
+GJKSupportFunction GJKInitializer<Box>::getSupportFunction()
+{
+ return &supportBox;
+}
+
+
+GJKCenterFunction GJKInitializer<Box>::getCenterFunction()
+{
+ return ¢erShape;
+}
+
+
+void* GJKInitializer<Box>::createGJKObject(const Box& s, const SimpleTransform& tf)
+{
+ ccd_box_t* o = new ccd_box_t;
+ boxToGJK(s, tf, o);
+ return o;
+}
+
+
+void GJKInitializer<Box>::deleteGJKObject(void* o_)
+{
+ ccd_box_t* o = (ccd_box_t*)o_;
+ delete o;
+}
+
+
+GJKSupportFunction GJKInitializer<Capsule>::getSupportFunction()
+{
+ return &supportCap;
+}
+
+
+GJKCenterFunction GJKInitializer<Capsule>::getCenterFunction()
+{
+ return ¢erShape;
+}
+
+
+void* GJKInitializer<Capsule>::createGJKObject(const Capsule& s, const SimpleTransform& tf)
+{
+ ccd_cap_t* o = new ccd_cap_t;
+ capToGJK(s, tf, o);
+ return o;
+}
+
+
+void GJKInitializer<Capsule>::deleteGJKObject(void* o_)
+{
+ ccd_cap_t* o = (ccd_cap_t*)o_;
+ delete o;
+}
+
+
+GJKSupportFunction GJKInitializer<Cone>::getSupportFunction()
+{
+ return &supportCone;
+}
+
+
+GJKCenterFunction GJKInitializer<Cone>::getCenterFunction()
+{
+ return ¢erShape;
+}
+
+
+void* GJKInitializer<Cone>::createGJKObject(const Cone& s, const SimpleTransform& tf)
+{
+ ccd_cone_t* o = new ccd_cone_t;
+ coneToGJK(s, tf, o);
+ return o;
+}
+
+
+void GJKInitializer<Cone>::deleteGJKObject(void* o_)
+{
+ ccd_cone_t* o = (ccd_cone_t*)o_;
+ delete o;
+}
+
+
+GJKSupportFunction GJKInitializer<Convex>::getSupportFunction()
+{
+ return &supportConvex;
+}
+
+
+GJKCenterFunction GJKInitializer<Convex>::getCenterFunction()
+{
+ return ¢erConvex;
+}
+
+
+void* GJKInitializer<Convex>::createGJKObject(const Convex& s, const SimpleTransform& tf)
+{
+ ccd_convex_t* o = new ccd_convex_t;
+ convexToGJK(s, tf, o);
+ return o;
+}
+
+
+void GJKInitializer<Convex>::deleteGJKObject(void* o_)
+{
+ ccd_convex_t* o = (ccd_convex_t*)o_;
+ delete o;
+}
+
+
+GJKSupportFunction triGetSupportFunction()
+{
+ return &supportTriangle;
+}
+
+
+GJKCenterFunction triGetCenterFunction()
+{
+ return ¢erTriangle;
+}
+
+
+void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3)
+{
+ ccd_triangle_t* o = new ccd_triangle_t;
+ Vec3f center((P1[0] + P2[0] + P3[0]) / 3, (P1[1] + P2[1] + P3[1]) / 3, (P1[2] + P2[2] + P3[2]) / 3);
+
+ ccdVec3Set(&o->p[0], P1[0], P1[1], P1[2]);
+ ccdVec3Set(&o->p[1], P2[0], P2[1], P2[2]);
+ ccdVec3Set(&o->p[2], P3[0], P3[1], P3[2]);
+ ccdVec3Set(&o->c, center[0], center[1], center[2]);
+ ccdVec3Set(&o->pos, 0., 0., 0.);
+ ccdQuatSet(&o->rot, 0., 0., 0., 1.);
+ ccdQuatInvert2(&o->rot_inv, &o->rot);
+
+ return o;
+}
+
+void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Matrix3f& R, Vec3f const& T)
+{
+ ccd_triangle_t* o = new ccd_triangle_t;
+ Vec3f center((P1[0] + P2[0] + P3[0]) / 3, (P1[1] + P2[1] + P3[1]) / 3, (P1[2] + P2[2] + P3[2]) / 3);
+
+ ccdVec3Set(&o->p[0], P1[0], P1[1], P1[2]);
+ ccdVec3Set(&o->p[1], P2[0], P2[1], P2[2]);
+ ccdVec3Set(&o->p[2], P3[0], P3[1], P3[2]);
+ ccdVec3Set(&o->c, center[0], center[1], center[2]);
+ SimpleQuaternion q;
+ q.fromRotation(R);
+ ccdVec3Set(&o->pos, T[0], T[1], T[2]);
+ ccdQuatSet(&o->rot, q.getX(), q.getY(), q.getZ(), q.getW());
+ ccdQuatInvert2(&o->rot_inv, &o->rot);
+
+ return o;
+}
+
+void triDeleteGJKObject(void* o_)
+{
+ ccd_triangle_t* o = (ccd_triangle_t*)o_;
+ delete o;
+}
+
+} // details
+
+} // fcl
--
GitLab