Skip to content
GitLab
Commit 029259cc authored by jpan's avatar jpan
Browse files

change quaternion and transform class name 


git-svn-id: https://kforge.ros.org/fcl/fcl_ros@149 253336fb-580f-4252-a368-f3cef5a2a82b
parent 090772b1
Expand all Hide whitespace changes
Inline Side-by-side
trunk/fcl/include/fcl/BV.h
View file @ 029259cc
......@@ -55,7 +55,7 @@ template<typename BV1, typename BV2>
class Converter
{
private:
static void convert(const BV1& bv1, const SimpleTransform& tf1, BV2& bv2)
static void convert(const BV1& bv1, const Transform3f& tf1, BV2& bv2)
{
// should only use the specialized version, so it is private.
}
......@@ -65,7 +65,7 @@ template<>
class Converter<AABB, AABB>
{
public:
static void convert(const AABB& bv1, const SimpleTransform& tf1, AABB& bv2)
static void convert(const AABB& bv1, const Transform3f& tf1, AABB& bv2)
{
const Vec3f& center = bv1.center();
FCL_REAL r = (bv1.max_ - bv1.min_).length() * 0.5;
......@@ -80,7 +80,7 @@ template<>
class Converter<AABB, OBB>
{
public:
static void convert(const AABB& bv1, const SimpleTransform& tf1, OBB& bv2)
static void convert(const AABB& bv1, const Transform3f& tf1, OBB& bv2)
{
/*
bv2.extent = (bv1.max_ - bv1.min_) * 0.5;
......@@ -129,7 +129,7 @@ template<>
class Converter<OBB, OBB>
{
public:
static void convert(const OBB& bv1, const SimpleTransform& tf1, OBB& bv2)
static void convert(const OBB& bv1, const Transform3f& tf1, OBB& bv2)
{
bv2.extent = bv1.extent;
bv2.To = tf1.transform(bv1.To);
......@@ -143,7 +143,7 @@ template<>
class Converter<OBBRSS, OBB>
{
public:
static void convert(const OBBRSS& bv1, const SimpleTransform& tf1, OBB& bv2)
static void convert(const OBBRSS& bv1, const Transform3f& tf1, OBB& bv2)
{
Converter<OBB, OBB>::convert(bv1.obb, tf1, bv2);
}
......@@ -153,7 +153,7 @@ template<>
class Converter<RSS, OBB>
{
public:
static void convert(const RSS& bv1, const SimpleTransform& tf1, OBB& bv2)
static void convert(const RSS& bv1, const Transform3f& tf1, OBB& bv2)
{
bv2.extent = Vec3f(bv1.l[0] * 0.5 + bv1.r, bv1.l[1] * 0.5 + bv1.r, bv1.r);
bv2.To = tf1.transform(bv1.Tr);
......@@ -168,7 +168,7 @@ template<typename BV1>
class Converter<BV1, AABB>
{
public:
static void convert(const BV1& bv1, const SimpleTransform& tf1, AABB& bv2)
static void convert(const BV1& bv1, const Transform3f& tf1, AABB& bv2)
{
const Vec3f& center = bv1.center();
FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).length() * 0.5;
......@@ -183,10 +183,10 @@ template<typename BV1>
class Converter<BV1, OBB>
{
public:
static void convert(const BV1& bv1, const SimpleTransform& tf1, OBB& bv2)
static void convert(const BV1& bv1, const Transform3f& tf1, OBB& bv2)
{
AABB bv;
Converter<BV1, AABB>::convert(bv1, SimpleTransform(), bv);
Converter<BV1, AABB>::convert(bv1, Transform3f(), bv);
Converter<AABB, OBB>::convert(bv, tf1, bv2);
}
};
......@@ -195,7 +195,7 @@ template<>
class Converter<OBB, RSS>
{
public:
static void convert(const OBB& bv1, const SimpleTransform& tf1, RSS& bv2)
static void convert(const OBB& bv1, const Transform3f& tf1, RSS& bv2)
{
bv2.Tr = tf1.transform(bv1.To);
bv2.axis[0] = tf1.getQuatRotation().transform(bv1.axis[0]);
......@@ -212,7 +212,7 @@ template<>
class Converter<RSS, RSS>
{
public:
static void convert(const RSS& bv1, const SimpleTransform& tf1, RSS& bv2)
static void convert(const RSS& bv1, const Transform3f& tf1, RSS& bv2)
{
bv2.Tr = tf1.transform(bv1.Tr);
bv2.axis[0] = tf1.getQuatRotation().transform(bv1.axis[0]);
......@@ -229,7 +229,7 @@ template<>
class Converter<OBBRSS, RSS>
{
public:
static void convert(const OBBRSS& bv1, const SimpleTransform& tf1, RSS& bv2)
static void convert(const OBBRSS& bv1, const Transform3f& tf1, RSS& bv2)
{
Converter<RSS, RSS>::convert(bv1.rss, tf1, bv2);
}
......@@ -239,7 +239,7 @@ template<>
class Converter<AABB, RSS>
{
public:
static void convert(const AABB& bv1, const SimpleTransform& tf1, RSS& bv2)
static void convert(const AABB& bv1, const Transform3f& tf1, RSS& bv2)
{
bv2.Tr = tf1.transform(bv1.center());
FCL_REAL d[3] = {bv1.width(), bv1.height(), bv1.depth() };
......@@ -282,7 +282,7 @@ public:
template<typename BV1, typename BV2>
static inline void convertBV(const BV1& bv1, const SimpleTransform& tf1, BV2& bv2)
static inline void convertBV(const BV1& bv1, const Transform3f& tf1, BV2& bv2)
{
Converter<BV1, BV2>::convert(bv1, tf1, bv2);
}
......
trunk/fcl/include/fcl/broad_phase_collision.h
View file @ 029259cc
......@@ -1454,9 +1454,9 @@ private:
void update_(CollisionObject* updated_obj);
/** \brief special manager-obj collision for octree */
bool collisionRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, CollisionCallBack callback) const;
bool collisionRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback) const;
bool distanceRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
bool distanceRecurse(DynamicAABBNode* root1, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
};
......@@ -1641,9 +1641,9 @@ private:
void update_(CollisionObject* updated_obj);
/** \brief special manager-obj collision for octree */
bool collisionRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, CollisionCallBack callback) const;
bool collisionRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, CollisionCallBack callback) const;
bool distanceRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const SimpleTransform& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
bool distanceRecurse(DynamicAABBNode* nodes1, size_t root1_id, const OcTree* tree2, const OcTree::OcTreeNode* root2, const AABB& root2_bv, const Transform3f& tf2, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const;
};
......
trunk/fcl/include/fcl/collision.h
View file @ 029259cc
......@@ -59,8 +59,8 @@ std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
CollisionResult& result);
template<typename NarrowPhaseSolver>
std::size_t collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
const CollisionGeometry* o2, const SimpleTransform& tf2,
std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const NarrowPhaseSolver* nsolver,
const CollisionRequest& request,
CollisionResult& result);
......@@ -69,8 +69,8 @@ std::size_t collide(const CollisionObject* o1, const CollisionObject* o2,
const CollisionRequest& request,
CollisionResult& result);
std::size_t collide(const CollisionGeometry* o1, const SimpleTransform& tf1,
const CollisionGeometry* o2, const SimpleTransform& tf2,
std::size_t collide(const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const CollisionRequest& request,
CollisionResult& result);
}
......
trunk/fcl/include/fcl/collision_func_matrix.h
View file @ 029259cc
......@@ -48,7 +48,7 @@ namespace fcl
template<typename NarrowPhaseSolver>
struct CollisionFunctionMatrix
{
typedef std::size_t (*CollisionFunc)(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result);
typedef std::size_t (*CollisionFunc)(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver, const CollisionRequest& request, CollisionResult& result);
CollisionFunc collision_matrix[NODE_COUNT][NODE_COUNT];
......
trunk/fcl/include/fcl/collision_object.h
View file @ 029259cc
......@@ -114,14 +114,14 @@ public:
computeAABB();
}
CollisionObject(const boost::shared_ptr<CollisionGeometry> &cgeom_, const SimpleTransform& tf) : cgeom(cgeom_), t(tf)
CollisionObject(const boost::shared_ptr<CollisionGeometry> &cgeom_, const Transform3f& tf) : cgeom(cgeom_), t(tf)
{
cgeom->computeLocalAABB();
computeAABB();
}
CollisionObject(const boost::shared_ptr<CollisionGeometry> &cgeom_, const Matrix3f& R, const Vec3f& T):
cgeom(cgeom_), t(SimpleTransform(R, T))
cgeom(cgeom_), t(Transform3f(R, T))
{
cgeom->computeLocalAABB();
computeAABB();
......@@ -185,12 +185,12 @@ public:
return t.getRotation();
}
inline const SimpleQuaternion& getQuatRotation() const
inline const Quaternion3f& getQuatRotation() const
{
return t.getQuatRotation();
}
inline const SimpleTransform& getTransform() const
inline const Transform3f& getTransform() const
{
return t;
}
......@@ -205,7 +205,7 @@ public:
t.setTranslation(T);
}
void setQuatRotation(const SimpleQuaternion& q)
void setQuatRotation(const Quaternion3f& q)
{
t.setQuatRotation(q);
}
......@@ -215,12 +215,12 @@ public:
t.setTransform(R, T);
}
void setTransform(const SimpleQuaternion& q, const Vec3f& T)
void setTransform(const Quaternion3f& q, const Vec3f& T)
{
t.setTransform(q, T);
}
void setTransform(const SimpleTransform& tf)
void setTransform(const Transform3f& tf)
{
t = tf;
}
......@@ -269,7 +269,7 @@ protected:
boost::shared_ptr<CollisionGeometry> cgeom;
SimpleTransform t;
Transform3f t;
/// AABB in global coordinate
mutable AABB aabb;
......
trunk/fcl/include/fcl/deprecated/geometric_shapes_intersect.h deleted 100644 → 0
View file @ 090772b1
/*
* 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_GEOMETRIC_SHAPES_INTERSECT_H
#define FCL_GEOMETRIC_SHAPES_INTERSECT_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, FCL_REAL* penetration_depth, Vec3f* normal);
bool GJKDistance(void* obj1, ccd_support_fn supp1,
void* obj2, ccd_support_fn supp2,
FCL_REAL* dist);
} // details
/** 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, FCL_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, FCL_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, FCL_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,
FCL_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;
}
}
#endif
trunk/fcl/include/fcl/deprecated/gjk.h deleted 100644 → 0
View file @ 090772b1
/*
* 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);
}
};
FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, FCL_REAL* w, size_t& m);
FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, FCL_REAL* w, size_t& m);
FCL_REAL projectOrigin(const Vec3f& a, const Vec3f& b, const Vec3f& c, const Vec3f& d, FCL_REAL* w, size_t& m);
static const FCL_REAL GJK_EPS = 0.000001;
static const size_t GJK_MAX_ITERATIONS = 128;
struct GJK
{
struct SimplexV
{
Ve