Commit 09b6b7e4 authored by jpan's avatar jpan
Browse files

The shape collision basic code is finished. Support both collision and...

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
parent 3802bdd4
......@@ -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})
......
......@@ -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
{
......
......@@ -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
......@@ -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;
}
......
/*
* 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
......@@ -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();