Commit 8201402e authored by jpan's avatar jpan
Browse files

shape functions


git-svn-id: https://kforge.ros.org/fcl/fcl_ros@100 253336fb-580f-4252-a368-f3cef5a2a82b
parent b984df81
......@@ -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})
......
......@@ -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
......@@ -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);
......
/*
* 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
......@@ -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;
......
......@@ -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()
......
......@@ -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)
{
......
This diff is collapsed.
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment