From 8201402e3495f00e4d98cc0218e7f78523a1c2e2 Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Tue, 19 Jun 2012 08:57:58 +0000
Subject: [PATCH] shape functions
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@100 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/CMakeLists.txt | 2 +-
.../include/fcl/geometric_shapes_intersect.h | 9 +
.../include/fcl/geometric_shapes_utility.h | 2 +-
trunk/fcl/include/fcl/gjk.h | 325 ++++++++
trunk/fcl/include/fcl/transform.h | 12 +
trunk/fcl/include/fcl/vec_3f.h | 2 +-
trunk/fcl/src/geometric_shapes_intersect.cpp | 62 ++
trunk/fcl/src/gjk.cpp | 728 ++++++++++++++++++
8 files changed, 1139 insertions(+), 3 deletions(-)
create mode 100644 trunk/fcl/include/fcl/gjk.h
create mode 100644 trunk/fcl/src/gjk.cpp
diff --git a/trunk/fcl/CMakeLists.txt b/trunk/fcl/CMakeLists.txt
index 9f334478..3cdce3a6 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)
+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)
target_link_libraries(${PROJECT_NAME} ${FLANN_LIBRARIES} ${CCD_LIBRARIES})
diff --git a/trunk/fcl/include/fcl/geometric_shapes_intersect.h b/trunk/fcl/include/fcl/geometric_shapes_intersect.h
index 30d49f0c..b09d1bc2 100644
--- a/trunk/fcl/include/fcl/geometric_shapes_intersect.h
+++ b/trunk/fcl/include/fcl/geometric_shapes_intersect.h
@@ -205,6 +205,15 @@ bool shapeTriangleIntersect(const S& s, const SimpleTransform& tf,
triDeleteGJKObject(o2);
}
+
+/** \brief distance computation between two shapes */
+template<typename S1, typename S2>
+BVH_REAL shapeDistance(const S1& s1, const SimpleTransform& tf1,
+ const S2& s2, const SimpleTransform& tf2)
+{
+ return 0.0;
+}
+
}
#endif
diff --git a/trunk/fcl/include/fcl/geometric_shapes_utility.h b/trunk/fcl/include/fcl/geometric_shapes_utility.h
index aa86a102..208a3b6d 100644
--- a/trunk/fcl/include/fcl/geometric_shapes_utility.h
+++ b/trunk/fcl/include/fcl/geometric_shapes_utility.h
@@ -82,7 +82,7 @@ template<>
void computeBV<AABB, Convex>(const Convex& s, const SimpleTransform& tf, AABB& bv);
-/** the bounding volume for half space back of plane for OBB, it is the plane itself */
+/** \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 SimpleTransform& tf, AABB& bv);
diff --git a/trunk/fcl/include/fcl/gjk.h b/trunk/fcl/include/fcl/gjk.h
new file mode 100644
index 00000000..6e50c03f
--- /dev/null
+++ b/trunk/fcl/include/fcl/gjk.h
@@ -0,0 +1,325 @@
+/*
+ * 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
+{
+
+
+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);
+ }
+
+};
+
+namespace details
+{
+
+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.0001;
+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()
+ {
+ ray = Vec3f();
+ nfree = 0;
+ status = Failed;
+ current = 0;
+ distance = 0.0;
+ }
+
+ 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.0001;
+static const size_t EPA_MAX_ITERATIONS = 255;
+
+struct EPA
+{
+ 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) {}
+ };
+
+ 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()
+ {
+ 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 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);
+};
+
+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
+ return false;
+}
+
+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)
+{
+ 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;
+ }
+
+ return false;
+}
+
+}
+
+
+#endif
diff --git a/trunk/fcl/include/fcl/transform.h b/trunk/fcl/include/fcl/transform.h
index 0fb56cf8..f72038ff 100644
--- a/trunk/fcl/include/fcl/transform.h
+++ b/trunk/fcl/include/fcl/transform.h
@@ -221,6 +221,18 @@ public:
return q.transform(v) + T;
}
+ SimpleTransform inverse() const
+ {
+ Matrix3f Rinv = R.transpose();
+ return SimpleTransform(Rinv, Rinv * (-T));
+ }
+
+ SimpleTransform inverseTimes(const SimpleTransform& other) const
+ {
+ Vec3f v = other.T - T;
+ return SimpleTransform(R.transposeTimes(other.R), R.transposeTimes(v));
+ }
+
const SimpleTransform& operator *= (const SimpleTransform& other)
{
T = q.transform(other.T) + T;
diff --git a/trunk/fcl/include/fcl/vec_3f.h b/trunk/fcl/include/fcl/vec_3f.h
index 7360e5f9..002f6104 100644
--- a/trunk/fcl/include/fcl/vec_3f.h
+++ b/trunk/fcl/include/fcl/vec_3f.h
@@ -78,7 +78,7 @@ public:
inline Vec3fX& operator -= (U t) { data -= t; return *this; }
inline Vec3fX& operator *= (U t) { data *= t; return *this; }
inline Vec3fX& operator /= (U t) { data /= t; return *this; }
- inline Vec3fX operator - () { return Vec3fX(-data); }
+ inline Vec3fX operator - () const { return Vec3fX(-data); }
inline Vec3fX cross(const Vec3fX& other) const { return Vec3fX(details::cross_prod(data, other.data)); }
inline U dot(const Vec3fX& other) const { return details::dot_prod3(data, other.data); }
inline bool normalize()
diff --git a/trunk/fcl/src/geometric_shapes_intersect.cpp b/trunk/fcl/src/geometric_shapes_intersect.cpp
index ea139967..2e56b4de 100644
--- a/trunk/fcl/src/geometric_shapes_intersect.cpp
+++ b/trunk/fcl/src/geometric_shapes_intersect.cpp
@@ -82,6 +82,68 @@ struct ccd_triangle_t : public ccd_obj_t
ccd_vec3_t c;
};
+/*
+static int __ccdGJK(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 a, b, c;
+ int do_simplex_res;
+
+ // initialize simplex struct
+ ccdSimplexInit(simplex);
+
+ ccd->first_dir(obj1, obj2, &dir);
+ __ccdSupport(obj1, obj2, &dir, ccd, &a);
+ ccdSimplexAdd(simplex, &a);
+
+ // set up direction vector to as (O - last) which is exactly -last
+ ccdVec3Copy(&dir, &a.v);
+ ccdVec3Scale(&dir, -CCD_ONE);
+ __ccdSupport(obj1, obj2, &dir, ccd, &b);
+ ccdSimplexAdd(simplex, &b);
+
+ // start iterations
+ for(iterations = 0UL; iterations < ccd->max_iterations; ++iterations)
+ {
+ ccd_vec3_t ba, bo, tmp;
+ ccdVec3Sub2(&ba, &a.v, &b.v);
+ ccdVec3Copy(&bo, &b.v);
+ ccdVec3Scale(&bo, -CCD_ONE);
+
+ ccd_real_t dot = ccdVec3Dot(&ba, &bo);
+ ccdVec3Cross(&tmp, &ba, &bo);
+ if(ccdIsZero(dot) || dot < CCD_ZERO)
+ {
+ ccdSimplexSet(simplex, 0, &b);
+ ccdSimpleSetSize(simplex, 1);
+ ccdVec3Copy(&dir, &bo);
+ }
+ else
+ {
+ tripleCross(&ba, &bo, &ba, &dir);
+ }
+ __ccdSupport(obj1, obj2, &dir, ccd, &c);
+ ccdSimplexAdd(simplex, &c);
+
+ if(ccdSimplexSize(simplex) == 2)
+ {
+
+ }
+ else if(ccdSimplexSize(simplex) == 3)
+ {
+
+ }
+
+
+
+ }
+}
+
+*/
+
+
/** Basic shape to ccd shape */
static void shapeToGJK(const ShapeBase& s, const SimpleTransform& tf, ccd_obj_t* o)
{
diff --git a/trunk/fcl/src/gjk.cpp b/trunk/fcl/src/gjk.cpp
new file mode 100644
index 00000000..8f497023
--- /dev/null
+++ b/trunk/fcl/src/gjk.cpp
@@ -0,0 +1,728 @@
+/*
+ * 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/gjk.h"
+
+namespace fcl
+{
+
+
+
+Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir)
+{
+ switch(shape->getObjectType())
+ {
+ case GEOM_BOX:
+ {
+ const Box* box = static_cast<const Box*>(shape);
+ return Vec3f((dir[0]>0)?(box->side[0]/2):(-box->side[0]/2),
+ (dir[1]>0)?(box->side[1]/2):(-box->side[1]/2),
+ (dir[2]>0)?(box->side[2]/2):(-box->side[2]/2));
+ }
+ break;
+ case GEOM_SPHERE:
+ {
+ const Sphere* sphere = static_cast<const Sphere*>(shape);
+ return dir * sphere->radius;
+ }
+ break;
+ case GEOM_CAPSULE:
+ {
+ const Capsule* capsule = static_cast<const Capsule*>(shape);
+ BVH_REAL half_h = capsule->lz * 0.5;
+ Vec3f pos1(0, 0, half_h);
+ Vec3f pos2(0, 0, -half_h);
+ Vec3f v = dir * capsule->radius;
+ pos1 += v;
+ pos2 += v;
+ if(dir.dot(pos1) > dir.dot(pos2))
+ return pos1;
+ else return pos2;
+ }
+ break;
+ case GEOM_CONE:
+ {
+ const Cone* cone = static_cast<const Cone*>(shape);
+ BVH_REAL zdist = dir[0] * dir[0] + dir[1] * dir[1];
+ BVH_REAL len = zdist + dir[2] * dir[2];
+ zdist = std::sqrt(zdist);
+ len = std::sqrt(len);
+ BVH_REAL half_h = cone->lz * 0.5;
+ BVH_REAL radius = cone->radius;
+
+ BVH_REAL sin_a = radius / std::sqrt(radius * radius + 4 * half_h * half_h);
+
+ if(dir[2] > len * sin_a)
+ return Vec3f(0, 0, half_h);
+ else if(zdist > 0)
+ {
+ BVH_REAL rad = radius / zdist;
+ return Vec3f(rad * dir[0], rad * dir[1], -half_h);
+ }
+ else
+ return Vec3f(0, 0, -half_h);
+ }
+ break;
+ case GEOM_CYLINDER:
+ {
+ const Cylinder* cylinder = static_cast<const Cylinder*>(shape);
+ BVH_REAL zdist = std::sqrt(dir[0] * dir[0] + dir[1] * dir[1]);
+ BVH_REAL half_h = cylinder->lz * 0.5;
+ if(zdist == 0.0)
+ {
+ return Vec3f(0, 0, (dir[2]>0)? half_h:-half_h);
+ }
+ else
+ {
+ BVH_REAL d = cylinder->radius / zdist;
+ return Vec3f(d * dir[0], d * dir[1], (dir[2]>0)?half_h:-half_h);
+ }
+ }
+ break;
+ 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);
+ if(dot > maxdot)
+ {
+ bestv = *curp;
+ maxdot = dot;
+ }
+ }
+ return bestv;
+ }
+ break;
+ case GEOM_PLANE:
+ {
+ return Vec3f(0, 0, 0);
+ }
+ break;
+ }
+
+ return Vec3f(0, 0, 0);
+}
+
+
+namespace details
+{
+
+BVH_REAL projectOrigin(const Vec3f& a, const Vec3f& b, BVH_REAL* w, size_t& m)
+{
+ const Vec3f d = b - a;
+ const BVH_REAL l = d.sqrLength();
+ if(l > 0)
+ {
+ const BVH_REAL t(l > 0 ? - a.dot(d) / l : 0);
+ if(t >= 1) { w[0] = 0; w[1] = 1; m = 2; return b.sqrLength(); } // m = 0x10
+ else if(t <= 0) { w[0] = 1; w[1] = 0; m = 1; return a.sqrLength(); } // m = 0x01
+ else { w[0] = 1 - (w[1] = t); m = 3; return (a + d * t).sqrLength(); } // m = 0x11
+ }
+
+ return -1;
+}
+
+BVH_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, BVH_REAL* w, size_t& m)
+{
+ static const size_t nexti[3] = {1, 2, 0};
+ const Vec3f* vt[] = {&a, &b, &c};
+ const Vec3f dl[] = {a - b, b - c, c - a};
+ const Vec3f& n = dl[0].cross(dl[1]);
+ const BVH_REAL l = n.sqrLength();
+
+ if(l > 0)
+ {
+ BVH_REAL mindist = -1;
+ BVH_REAL subw[2] = {0, 0};
+ size_t subm = 0;
+ for(size_t i = 0; i < 3; ++i)
+ {
+ if(vt[i]->dot(dl[i].cross(n)) > 0) // origin is to the outside part of the triangle edge, then the optimal can only be on the edge
+ {
+ size_t j = nexti[i];
+ BVH_REAL subd = projectOrigin(*vt[i], *vt[j], subw, subm);
+ if(mindist < 0 || subd < mindist)
+ {
+ mindist = subd;
+ m = static_cast<size_t>(((subm&1)?1<<i:0) + ((subm&2)?1<<j:0));
+ w[i] = subw[0];
+ w[j] = subw[1];
+ w[nexti[j]] = 0;
+ }
+ }
+ }
+
+ if(mindist < 0) // the origin project is within the triangle
+ {
+ BVH_REAL d = a.dot(n);
+ BVH_REAL s = sqrt(l);
+ Vec3f p = n * (d / l);
+ mindist = p.sqrLength();
+ m = 7; // m = 0x111
+ w[0] = dl[1].cross(b-p).length() / s;
+ w[1] = dl[2].cross(c-p).length() / s;
+ w[2] = 1 - (w[0] + w[1]);
+ }
+
+ return mindist;
+ }
+ return -1;
+}
+
+BVH_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d, BVH_REAL* w, size_t& m)
+{
+ static const size_t nexti[] = {1, 2, 0};
+ const Vec3f* vt[] = {&a, &b, &c, &d};
+ const Vec3f dl[3] = {a-d, b-d, c-d};
+ BVH_REAL vl = triple(dl[0], dl[1], dl[2]);
+ bool ng = (vl * a.dot((b-c).cross(a-b))) <= 0;
+ if(ng && std::abs(vl) > 0) // abs(vl) == 0, the tetrahedron is degenerated; if ng is false, then the last vertex in the tetrahedron does not grow toward the origin (in fact origin is on the other side of the abc face)
+ {
+ BVH_REAL mindist = -1;
+ BVH_REAL subw[3] = {0, 0, 0};
+ size_t subm = 0;
+ for(size_t i = 0; i < 3; ++i)
+ {
+ size_t j = nexti[i];
+ BVH_REAL s = vl * d.dot(dl[i].cross(dl[j]));
+ if(s > 0) // the origin is to the outside part of a triangle face, then the optimal can only be on the triangle face
+ {
+ BVH_REAL subd = projectOrigin(*vt[i], *vt[j], d, subw, subm);
+ if(mindist < 0 || subd < mindist)
+ {
+ mindist = subd;
+ m = static_cast<size_t>( (subm&1?1<<i:0) + (subm&2?1<<j:0) + (subm&4?8:0) );
+ w[i] = subw[0];
+ w[j] = subw[1];
+ w[nexti[j]] = 0;
+ w[3] = subw[2];
+ }
+ }
+ }
+
+ if(mindist < 0)
+ {
+ mindist = 0;
+ m = 15;
+ w[0] = triple(c, b, d) / vl;
+ w[1] = triple(a, c, d) / vl;
+ w[2] = triple(b, a, d) / vl;
+ w[3] = 1 - (w[0] + w[1] + w[2]);
+ }
+
+ return mindist;
+ }
+ return -1;
+}
+
+
+} // detail
+
+
+
+GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess)
+{
+ size_t iterations = 0;
+ BVH_REAL sqdist = 0;
+ BVH_REAL alpha = 0;
+ Vec3f lastw[4];
+ size_t clastw = 0;
+
+ free_v[0] = &store_v[0];
+ free_v[1] = &store_v[1];
+ free_v[2] = &store_v[2];
+ free_v[3] = &store_v[3];
+
+ nfree = 4;
+ current = 0;
+ status = Valid;
+ shape = shape_;
+ distance = 0.0;
+ simplices[0].rank = 0;
+ ray = guess;
+
+ BVH_REAL sqrl = ray.sqrLength();
+ appendVertex(simplices[0], (sqrl>0) ? -ray : Vec3f(1, 0, 0));
+ simplices[0].p[0] = 1;
+ ray = simplices[0].c[0]->w;
+ sqdist = sqrl;
+ lastw[0] = lastw[1] = lastw[2] = lastw[3] = ray;
+
+ do
+ {
+ size_t next = 1 - current;
+ Simplex& curr_simplex = simplices[current];
+ Simplex& next_simplex = simplices[next];
+
+ BVH_REAL rl = ray.sqrLength();
+ if(rl < GJK_EPS) // means origin is near the face of original simplex, return touch
+ {
+ status = Inside;
+ break;
+ }
+
+ appendVertex(curr_simplex, -ray); // see below, ray points away from origin
+
+ Vec3f& w = curr_simplex.c[curr_simplex.rank - 1]->w;
+ bool found = false;
+ for(size_t i = 0; i < 4; ++i)
+ {
+ if((w - lastw[i]).sqrLength() < GJK_EPS)
+ {
+ found = true; break;
+ }
+ }
+
+ if(found)
+ {
+ removeVertex(simplices[current]);
+ break;
+ }
+ else
+ {
+ lastw[clastw = (clastw+1)&3] = w;
+ }
+
+ // check for termination, from bullet
+ BVH_REAL omega = ray.dot(w) / rl;
+ alpha = std::max(alpha, omega);
+ if((rl - alpha) - GJK_EPS * rl <= 0)
+ {
+ removeVertex(simplices[current]);
+ break;
+ }
+
+ // reduce simplex and decide the extend direction
+ BVH_REAL weights[4];
+ size_t mask = 0; // decide the simplex vertices that compose the minimal distance
+ switch(curr_simplex.rank)
+ {
+ case 2:
+ sqdist = details::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;
+ 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;
+ }
+
+ if(sqdist >= 0)
+ {
+ next_simplex.rank = 0;
+ ray = Vec3f();
+ current = next;
+ for(size_t i = 0; i < curr_simplex.rank; ++i)
+ {
+ if(mask & (1 << i))
+ {
+ next_simplex.c[next_simplex.rank] = curr_simplex.c[i];
+ next_simplex.p[next_simplex.rank++] = weights[i];
+ ray += curr_simplex.c[i]->w * weights[i];
+ }
+ else
+ free_v[nfree++] = curr_simplex.c[i];
+ }
+ if(mask == 15) status = Inside; // the origin is within the 4-simplex, collision
+ }
+ else
+ {
+ removeVertex(simplices[current]);
+ break;
+ }
+
+ status = ((++iterations) < GJK_MAX_ITERATIONS) ? status : Failed;
+
+ } while(status == Valid);
+
+ simplex = &simplices[current];
+ switch(status)
+ {
+ case Valid: distance = ray.length(); break;
+ case Inside: distance = 0; break;
+ }
+ return status;
+}
+
+void GJK::getSupport(const Vec3f& d, SimplexV& sv) const
+{
+ sv.d = d.normalized();
+ sv.w = shape.support(sv.d);
+}
+
+void GJK::removeVertex(Simplex& simplex)
+{
+ free_v[nfree++] = simplex.c[--simplex.rank];
+}
+
+void GJK::appendVertex(Simplex& simplex, const Vec3f& v)
+{
+ simplex.p[simplex.rank] = 0; // initial weight 0
+ simplex.c[simplex.rank] = free_v[--nfree]; // set the memory
+ getSupport(v, *simplex.c[simplex.rank++]);
+}
+
+bool GJK::encloseOrigin()
+{
+ switch(simplex->rank)
+ {
+ case 1:
+ {
+ for(size_t i = 0; i < 3; ++i)
+ {
+ Vec3f axis;
+ axis[i] = 1;
+ appendVertex(*simplex, axis);
+ if(encloseOrigin()) return true;
+ removeVertex(*simplex);
+ appendVertex(*simplex, -axis);
+ if(encloseOrigin()) return true;
+ removeVertex(*simplex);
+ }
+ }
+ break;
+ case 2:
+ {
+ Vec3f d = simplex->c[1]->w - simplex->c[0]->w;
+ for(size_t i = 0; i < 3; ++i)
+ {
+ Vec3f axis;
+ axis[i] = 1;
+ Vec3f p = d.cross(axis);
+ if(p.sqrLength() > 0)
+ {
+ appendVertex(*simplex, p);
+ if(encloseOrigin()) return true;
+ removeVertex(*simplex);
+ appendVertex(*simplex, -p);
+ if(encloseOrigin()) return true;
+ removeVertex(*simplex);
+ }
+ }
+ }
+ break;
+ case 3:
+ {
+ Vec3f n = (simplex->c[1]->w - simplex->c[0]->w).cross(simplex->c[2]->w - simplex->c[0]->w);
+ if(n.sqrLength() > 0)
+ {
+ appendVertex(*simplex, n);
+ if(encloseOrigin()) return true;
+ removeVertex(*simplex);
+ appendVertex(*simplex, -n);
+ if(encloseOrigin()) return true;
+ removeVertex(*simplex);
+ }
+ }
+ break;
+ case 4:
+ {
+ if(std::abs(triple(simplex->c[0]->w - simplex->c[3]->w, simplex->c[1]->w - simplex->c[3]->w, simplex->c[2]->w - simplex->c[3]->w)) > 0)
+ return true;
+ }
+ break;
+ }
+
+ return false;
+}
+
+
+bool EPA::getEdgeDist(SimplexF* face, SimplexV* a, SimplexV* b, BVH_REAL& dist)
+{
+ Vec3f ba = b->w - a->w;
+ Vec3f n_ab = ba.cross(face->n);
+ BVH_REAL a_dot_nab = a->w.dot(n_ab);
+
+ if(a_dot_nab < 0) // the origin is on the outside part of ab
+ {
+ BVH_REAL ba_l2 = ba.sqrLength();
+
+ // following is similar to projectOrigin for two points
+ // however, as we dont need to compute the parameterization, dont need to compute 0 or 1
+ BVH_REAL a_dot_ba = a->w.dot(ba);
+ BVH_REAL b_dot_ba = b->w.dot(ba);
+
+ if(a_dot_ba > 0)
+ dist = a->w.length();
+ else if(b_dot_ba < 0)
+ dist = b->w.length();
+ else
+ {
+ BVH_REAL a_dot_b = a->w.dot(b->w);
+ dist = std::sqrt(std::max(a->w.sqrLength() * b->w.sqrLength() - a_dot_b * a_dot_b, (BVH_REAL)0));
+ }
+
+ return true;
+ }
+
+ return false;
+}
+
+EPA::SimplexF* EPA::newFace(SimplexV* a, SimplexV* b, SimplexV* c, bool forced)
+{
+ if(stock.root)
+ {
+ SimplexF* face = stock.root;
+ stock.remove(face);
+ hull.append(face);
+ face->pass = 0;
+ face->c[0] = a;
+ face->c[1] = b;
+ face->c[2] = c;
+ face->n = (b->w - a->w).cross(c->w - a->w);
+ BVH_REAL l = face->n.length();
+
+ if(l > EPA_EPS)
+ {
+ if(!(getEdgeDist(face, a, b, face->d) ||
+ getEdgeDist(face, b, c, face->d) ||
+ getEdgeDist(face, c, a, face->d)))
+ {
+ face->d = a->w.dot(face->n) / l;
+ }
+
+ face->n /= l;
+ if(forced || face->d >= -EPA_EPS)
+ return face;
+ else
+ status = NonConvex;
+ }
+ else
+ status = Degenerated;
+
+ hull.remove(face);
+ stock.append(face);
+ return NULL;
+ }
+
+ status = stock.root ? OutOfVertices : OutOfFaces;
+ return NULL;
+}
+
+/** \brief Find the best polytope face to split */
+EPA::SimplexF* EPA::findBest()
+{
+ SimplexF* minf = hull.root;
+ BVH_REAL mind = minf->d * minf->d;
+ for(SimplexF* f = minf->l[1]; f; f = f->l[1])
+ {
+ BVH_REAL sqd = f->d * f->d;
+ if(sqd < mind)
+ {
+ minf = f;
+ mind = sqd;
+ }
+ }
+ return minf;
+}
+
+EPA::Status EPA::evaluate(GJK& gjk, const Vec3f& guess)
+{
+ GJK::Simplex& simplex = *gjk.getSimplex();
+ if((simplex.rank > 1) && gjk.encloseOrigin())
+ {
+ while(hull.root)
+ {
+ SimplexF* f = hull.root;
+ hull.remove(f);
+ stock.append(f);
+ }
+
+ status = Valid;
+ nextsv = 0;
+
+ if((simplex.c[0]->w - simplex.c[3]->w).dot((simplex.c[1]->w - simplex.c[3]->w).cross(simplex.c[2]->w - simplex.c[3]->w)) < 0)
+ {
+ SimplexV* tmp = simplex.c[0];
+ simplex.c[0] = simplex.c[1];
+ simplex.c[1] = tmp;
+
+ BVH_REAL tmpv = simplex.p[0];
+ simplex.p[0] = simplex.p[1];
+ simplex.p[1] = tmpv;
+ }
+
+ SimplexF* tetrahedron[] = {newFace(simplex.c[0], simplex.c[1], simplex.c[2], true),
+ newFace(simplex.c[1], simplex.c[0], simplex.c[3], true),
+ newFace(simplex.c[2], simplex.c[1], simplex.c[3], true),
+ newFace(simplex.c[0], simplex.c[2], simplex.c[3], true) };
+
+ if(hull.count == 4)
+ {
+ SimplexF* best = findBest(); // find the best face (the face with the minimum distance to origin) to split
+ SimplexF outer = *best;
+ size_t pass = 0;
+ size_t iterations = 0;
+
+ // set the face connectivity
+ bind(tetrahedron[0], 0, tetrahedron[1], 0);
+ bind(tetrahedron[0], 1, tetrahedron[2], 0);
+ bind(tetrahedron[0], 2, tetrahedron[3], 0);
+ bind(tetrahedron[1], 1, tetrahedron[3], 2);
+ bind(tetrahedron[1], 2, tetrahedron[2], 1);
+ bind(tetrahedron[2], 2, tetrahedron[3], 1);
+
+ status = Valid;
+ for(; iterations < EPA_MAX_ITERATIONS; ++iterations)
+ {
+ if(nextsv < EPA_MAX_VERTICES)
+ {
+ SimplexHorizon horizon;
+ SimplexV* w = &sv_store[nextsv++];
+ bool valid = true;
+ best->pass = ++pass;
+ gjk.getSupport(best->n, *w);
+ BVH_REAL wdist = best->n.dot(w->w) - best->d;
+ if(wdist > EPA_EPS)
+ {
+ for(size_t j = 0; (j < 3) && valid; ++j)
+ {
+ valid &= expand(pass, w, best->f[j], best->e[j], horizon);
+ }
+
+
+ if(valid && horizon.nf >= 3)
+ {
+ // need to add the edge connectivity between first and last faces
+ bind(horizon.ff, 2, horizon.cf, 1);
+ hull.remove(best);
+ stock.append(best);
+ best = findBest();
+ outer = *best;
+ }
+ else
+ {
+ status = InvalidHull; break;
+ }
+ }
+ else
+ {
+ status = AccuracyReached; break;
+ }
+ }
+ else
+ {
+ status = OutOfVertices; break;
+ }
+ }
+
+ Vec3f projection = outer.n * outer.d;
+ normal = outer.n;
+ depth = outer.d;
+ result.rank = 3;
+ result.c[0] = outer.c[0];
+ result.c[1] = outer.c[1];
+ result.c[2] = outer.c[2];
+ result.p[0] = ((outer.c[1]->w - projection).cross(outer.c[2]->w - projection)).length();
+ result.p[1] = ((outer.c[2]->w - projection).cross(outer.c[0]->w - projection)).length();
+ result.p[2] = ((outer.c[0]->w - projection).cross(outer.c[1]->w - projection)).length();
+
+ BVH_REAL sum = result.p[0] + result.p[1] + result.p[2];
+ result.p[0] /= sum;
+ result.p[1] /= sum;
+ result.p[2] /= sum;
+ return status;
+ }
+ }
+
+ status = FallBack;
+ normal = -guess;
+ BVH_REAL nl = normal.length();
+ if(nl > 0) normal /= nl;
+ else normal = Vec3f(1, 0, 0);
+ depth = 0;
+ result.rank = 1;
+ result.c[0] = simplex.c[0];
+ result.p[0] = 1;
+ return status;
+}
+
+
+/** \brief the goal is to add a face connecting vertex w and face edge f[e] */
+bool EPA::expand(size_t pass, SimplexV* w, SimplexF* f, size_t e, SimplexHorizon& horizon)
+{
+ static const size_t nexti[] = {1, 2, 0};
+ static const size_t previ[] = {2, 0, 1};
+
+ if(f->pass != pass)
+ {
+ const size_t e1 = nexti[e];
+
+ // case 1: the new face is not degenerated, i.e., the new face is not coplanar with the old face f.
+ if(f->n.dot(w->w) - f->d < -EPA_EPS)
+ {
+ SimplexF* nf = newFace(f->c[e1], f->c[e], w, false);
+ if(nf)
+ {
+ // add face-face connectivity
+ bind(nf, 0, f, e);
+
+ // if there is last face in the horizon, then need to add another connectivity, i.e. the edge connecting the current new add edge and the last new add edge.
+ // This does not finish all the connectivities because the final face need to connect with the first face, this will be handled in the evaluate function.
+ // Notice the face is anti-clockwise, so the edges are 0 (bottom), 1 (right), 2 (left)
+ if(horizon.cf)
+ bind(nf, 2, horizon.cf, 1);
+ else
+ horizon.ff = nf;
+
+ horizon.cf = nf;
+ ++horizon.nf;
+ return true;
+ }
+ }
+ else // case 2: the new face is coplanar with the old face f. We need to add two faces and delete the old face
+ {
+ const size_t e2 = previ[e];
+ f->pass = pass;
+ if(expand(pass, w, f->f[e1], f->e[e1], horizon) && expand(pass, w, f->f[e2], f->e[e2], horizon))
+ {
+ hull.remove(f);
+ stock.append(f);
+ return true;
+ }
+ }
+ }
+
+ return false;
+}
+
+
+
+} // fcl
--
GitLab