From 8add7f84944b208de41bafc7b172088ea8c91aa1 Mon Sep 17 00:00:00 2001
From: jpan <jpan@253336fb-580f-4252-a368-f3cef5a2a82b>
Date: Wed, 11 Jul 2012 00:47:18 +0000
Subject: [PATCH] change to new libccd distance. need to use the new version of
libccd.
git-svn-id: https://kforge.ros.org/fcl/fcl_ros@125 253336fb-580f-4252-a368-f3cef5a2a82b
---
trunk/fcl/include/fcl/broad_phase_collision.h | 26 ++++++--
trunk/fcl/include/fcl/hierarchy_tree.h | 15 +++++
.../fcl/include/fcl/narrowphase/narrowphase.h | 8 +--
trunk/fcl/src/broad_phase_collision.cpp | 62 ++++++++++++++-----
trunk/fcl/src/gjk_libccd.cpp | 61 ++++++++++++------
5 files changed, 129 insertions(+), 43 deletions(-)
diff --git a/trunk/fcl/include/fcl/broad_phase_collision.h b/trunk/fcl/include/fcl/broad_phase_collision.h
index d07c2b45..e1e05634 100644
--- a/trunk/fcl/include/fcl/broad_phase_collision.h
+++ b/trunk/fcl/include/fcl/broad_phase_collision.h
@@ -490,12 +490,14 @@ void SpatialHashingCollisionManager<HashTable>::getObjects(std::vector<Collision
template<typename HashTable>
void SpatialHashingCollisionManager<HashTable>::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
{
+ if(size() == 0) return;
collide_(obj, cdata, callback);
}
template<typename HashTable>
void SpatialHashingCollisionManager<HashTable>::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
{
+ if(size() == 0) return;
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
distance_(obj, cdata, callback, min_dist);
}
@@ -503,8 +505,6 @@ void SpatialHashingCollisionManager<HashTable>::distance(CollisionObject* obj, v
template<typename HashTable>
bool SpatialHashingCollisionManager<HashTable>::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
{
- if(size() == 0) return false;
-
const AABB& obj_aabb = obj->getAABB();
AABB overlap_aabb;
@@ -543,8 +543,6 @@ bool SpatialHashingCollisionManager<HashTable>::collide_(CollisionObject* obj, v
template<typename HashTable>
bool SpatialHashingCollisionManager<HashTable>::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
{
- if(size() == 0) return false;
-
Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
AABB aabb = obj->getAABB();
if(min_dist < std::numeric_limits<FCL_REAL>::max())
@@ -700,6 +698,8 @@ void SpatialHashingCollisionManager<HashTable>::collide(void* cdata, CollisionCa
template<typename HashTable>
void SpatialHashingCollisionManager<HashTable>::distance(void* cdata, DistanceCallBack callback) const
{
+ if(size() == 0) return;
+
enable_tested_set_ = true;
tested_set.clear();
@@ -716,6 +716,9 @@ template<typename HashTable>
void SpatialHashingCollisionManager<HashTable>::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
{
SpatialHashingCollisionManager<HashTable>* other_manager = static_cast<SpatialHashingCollisionManager<HashTable>* >(other_manager_);
+
+ if((size() == 0) || (other_manager->size() == 0)) return;
+
if(this == other_manager)
{
collide(cdata, callback);
@@ -738,6 +741,9 @@ template<typename HashTable>
void SpatialHashingCollisionManager<HashTable>::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
{
SpatialHashingCollisionManager<HashTable>* other_manager = static_cast<SpatialHashingCollisionManager<HashTable>* >(other_manager_);
+
+ if((size() == 0) || (other_manager->size() == 0)) return;
+
if(this == other_manager)
{
distance(cdata, callback);
@@ -1328,12 +1334,14 @@ public:
/** \brief perform collision test between one object and all the objects belonging to the manager */
void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
{
+ if(size() == 0) return;
collisionRecurse(dtree.getRoot(), obj, cdata, callback);
}
/** \brief perform distance computation between one object and all the objects belonging to the manager */
void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
{
+ if(size() == 0) return;
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
distanceRecurse(dtree.getRoot(), obj, cdata, callback, min_dist);
}
@@ -1341,12 +1349,14 @@ public:
/** \brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) */
void collide(void* cdata, CollisionCallBack callback) const
{
+ if(size() == 0) return;
selfCollisionRecurse(dtree.getRoot(), cdata, callback);
}
/** \brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) */
void distance(void* cdata, DistanceCallBack callback) const
{
+ if(size() == 0) return;
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
selfDistanceRecurse(dtree.getRoot(), cdata, callback, min_dist);
}
@@ -1355,6 +1365,7 @@ public:
void collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
{
DynamicAABBTreeCollisionManager* other_manager = static_cast<DynamicAABBTreeCollisionManager*>(other_manager_);
+ if((size() == 0) || (other_manager->size() == 0)) return;
collisionRecurse(dtree.getRoot(), other_manager->dtree.getRoot(), cdata, callback);
}
@@ -1362,6 +1373,7 @@ public:
void distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
{
DynamicAABBTreeCollisionManager* other_manager = static_cast<DynamicAABBTreeCollisionManager*>(other_manager_);
+ if((size() == 0) || (other_manager->size() == 0)) return;
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
distanceRecurse(dtree.getRoot(), other_manager->dtree.getRoot(), cdata, callback, min_dist);
}
@@ -1464,12 +1476,14 @@ public:
/** \brief perform collision test between one object and all the objects belonging to the manager */
void collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
{
+ if(size() == 0) return;
collisionRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback);
}
/** \brief perform distance computation between one object and all the objects belonging to the manager */
void distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
{
+ if(size() == 0) return;
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
distanceRecurse(dtree.getNodes(), dtree.getRoot(), obj, cdata, callback, min_dist);
}
@@ -1477,12 +1491,14 @@ public:
/** \brief perform collision test for the objects belonging to the manager (i.e., N^2 self collision) */
void collide(void* cdata, CollisionCallBack callback) const
{
+ if(size() == 0) return;
selfCollisionRecurse(dtree.getNodes(), dtree.getRoot(), cdata, callback);
}
/** \brief perform distance test for the objects belonging to the manager (i.e., N^2 self distance) */
void distance(void* cdata, DistanceCallBack callback) const
{
+ if(size() == 0) return;
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
selfDistanceRecurse(dtree.getNodes(), dtree.getRoot(), cdata, callback, min_dist);
}
@@ -1491,6 +1507,7 @@ public:
void collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
{
DynamicAABBTreeCollisionManager2* other_manager = static_cast<DynamicAABBTreeCollisionManager2*>(other_manager_);
+ if((size() == 0) || (other_manager->size() == 0)) return;
collisionRecurse(dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), cdata, callback);
}
@@ -1498,6 +1515,7 @@ public:
void distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
{
DynamicAABBTreeCollisionManager2* other_manager = static_cast<DynamicAABBTreeCollisionManager2*>(other_manager_);
+ if((size() == 0) || (other_manager->size() == 0)) return;
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
distanceRecurse(dtree.getNodes(), dtree.getRoot(), other_manager->dtree.getNodes(), other_manager->dtree.getRoot(), cdata, callback, min_dist);
}
diff --git a/trunk/fcl/include/fcl/hierarchy_tree.h b/trunk/fcl/include/fcl/hierarchy_tree.h
index 49df8dda..254ed180 100644
--- a/trunk/fcl/include/fcl/hierarchy_tree.h
+++ b/trunk/fcl/include/fcl/hierarchy_tree.h
@@ -63,6 +63,13 @@ struct NodeBase
};
FCL_UINT32 code;
+
+ NodeBase()
+ {
+ parent = NULL;
+ childs[0] = NULL;
+ childs[1] = NULL;
+ }
};
template<typename BV>
@@ -223,11 +230,15 @@ public:
size_t getMaxHeight() const
{
+ if(!root_node)
+ return 0;
return getMaxHeight(root_node);
}
size_t getMaxDepth() const
{
+ if(!root_node) return 0;
+
size_t max_depth;
getMaxDepth(root_node, 0, max_depth);
return max_depth;
@@ -1366,11 +1377,15 @@ public:
size_t getMaxHeight() const
{
+ if(root_node == NULL_NODE) return 0;
+
return getMaxHeight(root_node);
}
size_t getMaxDepth() const
{
+ if(root_node == NULL_NODE) return 0;
+
size_t max_depth;
getMaxDepth(root_node, 0, max_depth);
return max_depth;
diff --git a/trunk/fcl/include/fcl/narrowphase/narrowphase.h b/trunk/fcl/include/fcl/narrowphase/narrowphase.h
index 6091684a..7582b0c6 100644
--- a/trunk/fcl/include/fcl/narrowphase/narrowphase.h
+++ b/trunk/fcl/include/fcl/narrowphase/narrowphase.h
@@ -121,8 +121,6 @@ struct GJKSolver_libccd
max_distance_iterations, distance_tolerance,
dist);
- if(*dist > 0) *dist = std::sqrt(*dist);
-
details::GJKInitializer<S1>::deleteGJKObject(o1);
details::GJKInitializer<S2>::deleteGJKObject(o2);
@@ -143,8 +141,6 @@ struct GJKSolver_libccd
o2, details::triGetSupportFunction(),
max_distance_iterations, distance_tolerance,
dist);
-
- if(*dist > 0) *dist = std::sqrt(*dist);
details::GJKInitializer<S>::deleteGJKObject(o1);
details::triDeleteGJKObject(o2);
@@ -165,8 +161,6 @@ struct GJKSolver_libccd
o2, details::triGetSupportFunction(),
max_distance_iterations, distance_tolerance,
dist);
-
- if(*dist > 0) *dist = std::sqrt(*dist);
details::GJKInitializer<S>::deleteGJKObject(o1);
details::triDeleteGJKObject(o2);
@@ -178,7 +172,7 @@ struct GJKSolver_libccd
GJKSolver_libccd()
{
max_collision_iterations = 500;
- max_distance_iterations = 500;
+ max_distance_iterations = 1000;
collision_tolerance = 1e-6;
distance_tolerance = 1e-6;
}
diff --git a/trunk/fcl/src/broad_phase_collision.cpp b/trunk/fcl/src/broad_phase_collision.cpp
index 2eeffb1d..5e8fbb8a 100644
--- a/trunk/fcl/src/broad_phase_collision.cpp
+++ b/trunk/fcl/src/broad_phase_collision.cpp
@@ -191,6 +191,8 @@ void NaiveCollisionManager::collide(BroadPhaseCollisionManager* other_manager_,
{
NaiveCollisionManager* other_manager = static_cast<NaiveCollisionManager*>(other_manager_);
+ if((size() == 0) || (other_manager->size() == 0)) return;
+
if(this == other_manager)
{
collide(cdata, callback);
@@ -214,6 +216,8 @@ void NaiveCollisionManager::distance(BroadPhaseCollisionManager* other_manager_,
{
NaiveCollisionManager* other_manager = static_cast<NaiveCollisionManager*>(other_manager_);
+ if((size() == 0) || (other_manager->size() == 0)) return;
+
if(this == other_manager)
{
distance(cdata, callback);
@@ -414,13 +418,13 @@ bool SSaPCollisionManager::checkDis(std::vector<CollisionObject*>::const_iterato
void SSaPCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
{
+ if(size() == 0) return;
+
collide_(obj, cdata, callback);
}
bool SSaPCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
{
- if(size() == 0) return false;
-
static const unsigned int CUTOFF = 100;
DummyCollisionObject dummyHigh(AABB(obj->getAABB().max_));
@@ -469,14 +473,14 @@ bool SSaPCollisionManager::collide_(CollisionObject* obj, void* cdata, Collision
void SSaPCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
{
+ if(size() == 0) return;
+
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
distance_(obj, cdata, callback, min_dist);
}
bool SSaPCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
{
- if(size() == 0) return false;
-
static const unsigned int CUTOFF = 100;
Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
Vec3f dummy_vector = obj->getAABB().max_;
@@ -574,8 +578,7 @@ bool SSaPCollisionManager::distance_(CollisionObject* obj, void* cdata, Distance
void SSaPCollisionManager::collide(void* cdata, CollisionCallBack callback) const
{
- if(size() == 0)
- return;
+ if(size() == 0) return;
std::vector<CollisionObject*>::const_iterator pos, run_pos, pos_end;
size_t axis = selectOptimalAxis(objs_x, objs_y, objs_z,
@@ -631,8 +634,7 @@ void SSaPCollisionManager::collide(void* cdata, CollisionCallBack callback) cons
void SSaPCollisionManager::distance(void* cdata, DistanceCallBack callback) const
{
- if(size() == 0)
- return;
+ if(size() == 0) return;
std::vector<CollisionObject*>::const_iterator it, it_end;
size_t axis = selectOptimalAxis(objs_x, objs_y, objs_z, it, it_end);
@@ -648,6 +650,9 @@ void SSaPCollisionManager::distance(void* cdata, DistanceCallBack callback) cons
void SSaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
{
SSaPCollisionManager* other_manager = static_cast<SSaPCollisionManager*>(other_manager_);
+
+ if((size() == 0) || (other_manager->size() == 0)) return;
+
if(this == other_manager)
{
collide(cdata, callback);
@@ -671,6 +676,9 @@ void SSaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, v
void SSaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
{
SSaPCollisionManager* other_manager = static_cast<SSaPCollisionManager*>(other_manager_);
+
+ if((size() == 0) || (other_manager->size() == 0)) return;
+
if(this == other_manager)
{
distance(cdata, callback);
@@ -1348,6 +1356,9 @@ void SaPCollisionManager::distance(void* cdata, DistanceCallBack callback) const
void SaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
{
SaPCollisionManager* other_manager = static_cast<SaPCollisionManager*>(other_manager_);
+
+ if((size() == 0) || (other_manager->size() == 0)) return;
+
if(this == other_manager)
{
collide(cdata, callback);
@@ -1375,6 +1386,9 @@ void SaPCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, vo
void SaPCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
{
SaPCollisionManager* other_manager = static_cast<SaPCollisionManager*>(other_manager_);
+
+ if((size() == 0) || (other_manager->size() == 0)) return;
+
if(this == other_manager)
{
distance(cdata, callback);
@@ -1701,13 +1715,12 @@ void IntervalTreeCollisionManager::getObjects(std::vector<CollisionObject*>& obj
void IntervalTreeCollisionManager::collide(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
{
+ if(size() == 0) return;
collide_(obj, cdata, callback);
}
bool IntervalTreeCollisionManager::collide_(CollisionObject* obj, void* cdata, CollisionCallBack callback) const
{
- if(size() == 0) return false;
-
static const unsigned int CUTOFF = 100;
std::deque<SimpleInterval*> results0, results1, results2;
@@ -1744,14 +1757,13 @@ bool IntervalTreeCollisionManager::collide_(CollisionObject* obj, void* cdata, C
void IntervalTreeCollisionManager::distance(CollisionObject* obj, void* cdata, DistanceCallBack callback) const
{
+ if(size() == 0) return;
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
distance_(obj, cdata, callback, min_dist);
}
bool IntervalTreeCollisionManager::distance_(CollisionObject* obj, void* cdata, DistanceCallBack callback, FCL_REAL& min_dist) const
{
- if(size() == 0) return false;
-
static const unsigned int CUTOFF = 100;
Vec3f delta = (obj->getAABB().max_ - obj->getAABB().min_) * 0.5;
@@ -1895,6 +1907,8 @@ void IntervalTreeCollisionManager::collide(void* cdata, CollisionCallBack callba
void IntervalTreeCollisionManager::distance(void* cdata, DistanceCallBack callback) const
{
+ if(size() == 0) return;
+
enable_tested_set_ = true;
tested_set.clear();
FCL_REAL min_dist = std::numeric_limits<FCL_REAL>::max();
@@ -1909,6 +1923,9 @@ void IntervalTreeCollisionManager::distance(void* cdata, DistanceCallBack callba
void IntervalTreeCollisionManager::collide(BroadPhaseCollisionManager* other_manager_, void* cdata, CollisionCallBack callback) const
{
IntervalTreeCollisionManager* other_manager = static_cast<IntervalTreeCollisionManager*>(other_manager_);
+
+ if((size() == 0) || (other_manager->size() == 0)) return;
+
if(this == other_manager)
{
collide(cdata, callback);
@@ -1930,6 +1947,9 @@ void IntervalTreeCollisionManager::collide(BroadPhaseCollisionManager* other_man
void IntervalTreeCollisionManager::distance(BroadPhaseCollisionManager* other_manager_, void* cdata, DistanceCallBack callback) const
{
IntervalTreeCollisionManager* other_manager = static_cast<IntervalTreeCollisionManager*>(other_manager_);
+
+ if((size() == 0) || (other_manager->size() == 0)) return;
+
if(this == other_manager)
{
distance(cdata, callback);
@@ -2055,8 +2075,15 @@ void DynamicAABBTreeCollisionManager::setup()
{
if(!setup_)
{
- int height = dtree.getMaxHeight();
int num = dtree.size();
+ if(num == 0)
+ {
+ setup_ = true;
+ return;
+ }
+
+ int height = dtree.getMaxHeight();
+
if(height - std::log((FCL_REAL)num) / std::log(2.0) < max_tree_nonbalanced_level)
dtree.balanceIncremental(tree_incremental_balance_pass);
@@ -2365,8 +2392,15 @@ void DynamicAABBTreeCollisionManager2::setup()
{
if(!setup_)
{
- int height = dtree.getMaxHeight();
int num = dtree.size();
+ if(num == 0)
+ {
+ setup_ = true;
+ return;
+ }
+
+ int height = dtree.getMaxHeight();
+
if(height - std::log((FCL_REAL)num) / std::log(2.0) < max_tree_nonbalanced_level)
dtree.balanceIncremental(tree_incremental_balance_pass);
diff --git a/trunk/fcl/src/gjk_libccd.cpp b/trunk/fcl/src/gjk_libccd.cpp
index c445f592..a3bfaa30 100644
--- a/trunk/fcl/src/gjk_libccd.cpp
+++ b/trunk/fcl/src/gjk_libccd.cpp
@@ -491,7 +491,7 @@ static void convexToGJK(const Convex& s, const SimpleTransform& tf, ccd_convex_t
/** 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;
+ const ccd_box_t* o = static_cast<const ccd_box_t*>(obj);
ccd_vec3_t dir;
ccdVec3Copy(&dir, dir_);
ccdQuatRotVec(&dir, &o->rot_inv);
@@ -504,7 +504,7 @@ static void supportBox(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
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;
+ const ccd_cap_t* o = static_cast<const ccd_cap_t*>(obj);
ccd_vec3_t dir, pos1, pos2;
ccdVec3Copy(&dir, dir_);
@@ -530,7 +530,7 @@ static void supportCap(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
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;
+ const ccd_cyl_t* cyl = static_cast<const ccd_cyl_t*>(obj);
ccd_vec3_t dir;
double zdist, rad;
@@ -557,7 +557,7 @@ static void supportCyl(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
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;
+ const ccd_cone_t* cone = static_cast<const ccd_cone_t*>(obj);
ccd_vec3_t dir;
ccdVec3Copy(&dir, dir_);
@@ -588,7 +588,7 @@ static void supportCone(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v)
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;
+ const ccd_sphere_t* s = static_cast<const ccd_sphere_t*>(obj);
ccd_vec3_t dir;
ccdVec3Copy(&dir, dir_);
@@ -636,7 +636,7 @@ static void supportConvex(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t* v
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;
+ const ccd_triangle_t* tri = static_cast<const ccd_triangle_t*>(obj);
ccd_vec3_t dir, p;
ccd_real_t maxdot, dot;
int i;
@@ -664,13 +664,13 @@ static void supportTriangle(const void* obj, const ccd_vec3_t* dir_, ccd_vec3_t*
static void centerShape(const void* obj, ccd_vec3_t* c)
{
- const ccd_obj_t *o = (const ccd_obj_t*)obj;
+ const ccd_obj_t *o = static_cast<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;
+ const ccd_convex_t *o = static_cast<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);
@@ -678,7 +678,7 @@ static void centerConvex(const void* obj, ccd_vec3_t* c)
static void centerTriangle(const void* obj, ccd_vec3_t* c)
{
- const ccd_triangle_t *o = (const ccd_triangle_t*)obj;
+ const ccd_triangle_t *o = static_cast<const ccd_triangle_t*>(obj);
ccdVec3Copy(c, &o->c);
ccdQuatRotVec(c, &o->rot);
ccdVec3Add(c, &o->pos);
@@ -722,6 +722,7 @@ bool GJKCollide(void* obj1, ccd_support_fn supp1, ccd_center_fn cen1,
return false;
}
+/*
bool GJKDistance(void* obj1, ccd_support_fn supp1,
void* obj2, ccd_support_fn supp2,
unsigned int max_iterations, FCL_REAL tolerance_,
@@ -729,7 +730,6 @@ bool GJKDistance(void* obj1, ccd_support_fn supp1,
{
ccd_t ccd;
ccd_real_t dist;
- ccd_vec3_t dir, pos;
CCD_INIT(&ccd);
ccd.support1 = supp1;
ccd.support2 = supp2;
@@ -740,12 +740,37 @@ bool GJKDistance(void* obj1, ccd_support_fn supp1,
ccd_simplex_t simplex;
dist = __ccdGJKDist(obj1, obj2, &ccd, &simplex, tolerance);
+
+ if(dist > 0)
+ {
+ *res = std::sqrt(dist);
+ return true;
+ }
+ else
+ return false;
+}
+*/
+
+bool GJKDistance(void* obj1, ccd_support_fn supp1,
+ void* obj2, ccd_support_fn supp2,
+ unsigned int max_iterations, FCL_REAL tolerance,
+ FCL_REAL* res)
+{
+ ccd_t ccd;
+ ccd_real_t dist;
+ CCD_INIT(&ccd);
+ ccd.support1 = supp1;
+ ccd.support2 = supp2;
+
+ ccd.max_iterations = max_iterations;
+ ccd.dist_tolerance = tolerance;
+
+ dist = ccdGJKDist(obj1, obj2, &ccd);
*res = dist;
if(dist < 0) return false;
else return true;
}
-
GJKSupportFunction GJKInitializer<Cylinder>::getSupportFunction()
{
return &supportCyl;
@@ -768,7 +793,7 @@ void* GJKInitializer<Cylinder>::createGJKObject(const Cylinder& s, const SimpleT
void GJKInitializer<Cylinder>::deleteGJKObject(void* o_)
{
- ccd_cyl_t* o = (ccd_cyl_t*)o_;
+ ccd_cyl_t* o = static_cast<ccd_cyl_t*>(o_);
delete o;
}
@@ -794,7 +819,7 @@ void* GJKInitializer<Sphere>::createGJKObject(const Sphere& s, const SimpleTrans
void GJKInitializer<Sphere>::deleteGJKObject(void* o_)
{
- ccd_sphere_t* o = (ccd_sphere_t*)o_;
+ ccd_sphere_t* o = static_cast<ccd_sphere_t*>(o_);
delete o;
}
@@ -820,7 +845,7 @@ void* GJKInitializer<Box>::createGJKObject(const Box& s, const SimpleTransform&
void GJKInitializer<Box>::deleteGJKObject(void* o_)
{
- ccd_box_t* o = (ccd_box_t*)o_;
+ ccd_box_t* o = static_cast<ccd_box_t*>(o_);
delete o;
}
@@ -847,7 +872,7 @@ void* GJKInitializer<Capsule>::createGJKObject(const Capsule& s, const SimpleTra
void GJKInitializer<Capsule>::deleteGJKObject(void* o_)
{
- ccd_cap_t* o = (ccd_cap_t*)o_;
+ ccd_cap_t* o = static_cast<ccd_cap_t*>(o_);
delete o;
}
@@ -874,7 +899,7 @@ void* GJKInitializer<Cone>::createGJKObject(const Cone& s, const SimpleTransform
void GJKInitializer<Cone>::deleteGJKObject(void* o_)
{
- ccd_cone_t* o = (ccd_cone_t*)o_;
+ ccd_cone_t* o = static_cast<ccd_cone_t*>(o_);
delete o;
}
@@ -901,7 +926,7 @@ void* GJKInitializer<Convex>::createGJKObject(const Convex& s, const SimpleTrans
void GJKInitializer<Convex>::deleteGJKObject(void* o_)
{
- ccd_convex_t* o = (ccd_convex_t*)o_;
+ ccd_convex_t* o = static_cast<ccd_convex_t*>(o_);
delete o;
}
@@ -954,7 +979,7 @@ void* triCreateGJKObject(const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, cons
void triDeleteGJKObject(void* o_)
{
- ccd_triangle_t* o = (ccd_triangle_t*)o_;
+ ccd_triangle_t* o = static_cast<ccd_triangle_t*>(o_);
delete o;
}
--
GitLab