diff --git a/trunk/fcl/include/fcl/ccd/interval_matrix.h b/trunk/fcl/include/fcl/ccd/interval_matrix.h
index 657616cb5ec39f62450c0abd2a983e5d0e0c7fbc..2a3195a5500cd3ef7a80ca047f48061012e2cd59 100644
--- a/trunk/fcl/include/fcl/ccd/interval_matrix.h
+++ b/trunk/fcl/include/fcl/ccd/interval_matrix.h
@@ -71,14 +71,14 @@ struct IMatrix3
Matrix3f getLow() const;
Matrix3f getHigh() const;
- inline const IVector3& operator [] (size_t i) const
+ inline const Interval& operator () (size_t i, size_t j) const
{
- return v_[i];
+ return v_[i][j];
}
- inline IVector3& operator [] (size_t i)
+ inline Interval& operator () (size_t i, size_t j)
{
- return v_[i];
+ return v_[i][j];
}
IMatrix3 operator + (const IMatrix3& m) const;
diff --git a/trunk/fcl/include/fcl/ccd/taylor_matrix.h b/trunk/fcl/include/fcl/ccd/taylor_matrix.h
index f3e2ddd896e7843a6cbe8f046d1c48244c81989f..9e513a8b06b01e5d466ff4e84c4b0edd3eb3dc0e 100644
--- a/trunk/fcl/include/fcl/ccd/taylor_matrix.h
+++ b/trunk/fcl/include/fcl/ccd/taylor_matrix.h
@@ -58,8 +58,8 @@ struct TMatrix3
TVector3 getColumn(size_t i) const;
const TVector3& getRow(size_t i) const;
- const TVector3& operator [] (size_t i) const;
- TVector3& operator [] (size_t i);
+ const TaylorModel& operator () (size_t i, size_t j) const;
+ TaylorModel& operator () (size_t i, size_t j);
TVector3 operator * (const Vec3f& v) const;
TVector3 operator * (const TVector3& v) const;
diff --git a/trunk/fcl/include/fcl/collision_func_matrix.h b/trunk/fcl/include/fcl/collision_func_matrix.h
index c8dfa442a1bd0a42ad10db3507753366f350effe..c46c6e0842e1fc3e8166798ab64f4abe9edda757 100644
--- a/trunk/fcl/include/fcl/collision_func_matrix.h
+++ b/trunk/fcl/include/fcl/collision_func_matrix.h
@@ -50,7 +50,7 @@ struct CollisionFunctionMatrix
{
typedef int (*CollisionFunc)(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver, int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts);
- CollisionFunc collision_matrix[16][16];
+ CollisionFunc collision_matrix[NODE_COUNT-1][NODE_COUNT-1];
CollisionFunctionMatrix();
};
diff --git a/trunk/fcl/include/fcl/collision_object.h b/trunk/fcl/include/fcl/collision_object.h
index 5b5229454c77e8ea2262a453f443aa7eae2602c8..49e8ef703aa61e8a99226880fe8c5cb8defbf7df 100644
--- a/trunk/fcl/include/fcl/collision_object.h
+++ b/trunk/fcl/include/fcl/collision_object.h
@@ -45,10 +45,10 @@
namespace fcl
{
-enum OBJECT_TYPE {OT_UNKNOWN, OT_BVH, OT_GEOM};
+enum OBJECT_TYPE {OT_UNKNOWN, OT_BVH, OT_GEOM, OT_OCTREE, OT_COUNT};
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_TRIANGLE};
+ GEOM_BOX, GEOM_SPHERE, GEOM_CAPSULE, GEOM_CONE, GEOM_CYLINDER, GEOM_CONVEX, GEOM_PLANE, GEOM_TRIANGLE, GEOM_OCTREE, NODE_COUNT};
class CollisionGeometry
{
@@ -77,6 +77,9 @@ public:
/** AABB radius */
FCL_REAL aabb_radius;
+ /** AABB in local coordinate, used for tight AABB when only translation transform */
+ AABB aabb_local;
+
/** pointer to user defined data specific to this object */
void *user_data;
};
@@ -125,10 +128,17 @@ public:
inline void computeAABB()
{
- Vec3f center = t.transform(cgeom->aabb_center);
- Vec3f delta(cgeom->aabb_radius, cgeom->aabb_radius, cgeom->aabb_radius);
- aabb.min_ = center - delta;
- aabb.max_ = center + delta;
+ if(t.getQuatRotation().isIdentity())
+ {
+ aabb = cgeom->aabb_local;
+ }
+ else
+ {
+ Vec3f center = t.transform(cgeom->aabb_center);
+ Vec3f delta(cgeom->aabb_radius, cgeom->aabb_radius, cgeom->aabb_radius);
+ aabb.min_ = center - delta;
+ aabb.max_ = center + delta;
+ }
}
void* getUserData() const
diff --git a/trunk/fcl/include/fcl/distance_func_matrix.h b/trunk/fcl/include/fcl/distance_func_matrix.h
index f909505c5abfddcdad8a6ff630e9abc46d351ccf..ea697304a8937b31d89959509eb480010a06fa5b 100644
--- a/trunk/fcl/include/fcl/distance_func_matrix.h
+++ b/trunk/fcl/include/fcl/distance_func_matrix.h
@@ -48,7 +48,7 @@ struct DistanceFunctionMatrix
{
typedef FCL_REAL (*DistanceFunc)(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2, const NarrowPhaseSolver* nsolver);
- DistanceFunc distance_matrix[16][16];
+ DistanceFunc distance_matrix[NODE_COUNT-1][NODE_COUNT-1];
DistanceFunctionMatrix();
};
diff --git a/trunk/fcl/include/fcl/geometric_shapes.h b/trunk/fcl/include/fcl/geometric_shapes.h
index 4681fc8957aee7f1ec46e0a0e7c157a235f075b8..3dfa447cf6dc015274b5c7616e977419e8c12807 100644
--- a/trunk/fcl/include/fcl/geometric_shapes.h
+++ b/trunk/fcl/include/fcl/geometric_shapes.h
@@ -81,6 +81,11 @@ public:
computeLocalAABB();
}
+ Box(const Vec3f& side_) : ShapeBase(), side(side_)
+ {
+ computeLocalAABB();
+ }
+
/** box side length */
Vec3f side;
diff --git a/trunk/fcl/include/fcl/geometric_shapes_utility.h b/trunk/fcl/include/fcl/geometric_shapes_utility.h
index 8e1f04aa0313935e078498de49346e303762c143..4251becdd47de9b44596b4b9caadafc8aae83ea7 100644
--- a/trunk/fcl/include/fcl/geometric_shapes_utility.h
+++ b/trunk/fcl/include/fcl/geometric_shapes_utility.h
@@ -131,6 +131,40 @@ void computeBV<KDOP<18>, Plane>(const Plane& s, const SimpleTransform& tf, KDOP<
template<>
void computeBV<KDOP<24>, Plane>(const Plane& s, const SimpleTransform& tf, KDOP<24>& bv);
+
+void constructBox(const AABB& bv, Box& box, SimpleTransform& tf);
+
+void constructBox(const OBB& bv, Box& box, SimpleTransform& tf);
+
+void constructBox(const OBBRSS& bv, Box& box, SimpleTransform& tf);
+
+void constructBox(const kIOS& bv, Box& box, SimpleTransform& tf);
+
+void constructBox(const RSS& bv, Box& box, SimpleTransform& tf);
+
+void constructBox(const KDOP<16>& bv, Box& box, SimpleTransform& tf);
+
+void constructBox(const KDOP<18>& bv, Box& box, SimpleTransform& tf);
+
+void constructBox(const KDOP<24>& bv, Box& box, SimpleTransform& tf);
+
+void constructBox(const AABB& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf);
+
+void constructBox(const OBB& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf);
+
+void constructBox(const OBBRSS& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf);
+
+void constructBox(const kIOS& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf);
+
+void constructBox(const RSS& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf);
+
+void constructBox(const KDOP<16>& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf);
+
+void constructBox(const KDOP<18>& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf);
+
+void constructBox(const KDOP<24>& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf);
+
+
}
#endif
diff --git a/trunk/fcl/include/fcl/octree.h b/trunk/fcl/include/fcl/octree.h
index 0ffe7a357dcd61e29de79be5fabadc77011a755d..7dec38c302fff29dddd81cc5eea9e3ba26957e9d 100644
--- a/trunk/fcl/include/fcl/octree.h
+++ b/trunk/fcl/include/fcl/octree.h
@@ -38,17 +38,18 @@
#ifndef FCL_OCTREE_H
#define FCL_OCTREE_H
+
#include <boost/shared_ptr.hpp>
#include <boost/array.hpp>
#include <octomap/octomap.h>
#include "fcl/BV/AABB.h"
-#include "fcl/geometric_shapes.h"
+#include "fcl/collision_object.h"
namespace fcl
{
-class OcTree
+class OcTree : public CollisionGeometry
{
private:
boost::shared_ptr<octomap::OcTree> tree;
@@ -106,8 +107,16 @@ public:
return boxes;
}
+ OBJECT_TYPE getObjectType() const { return OT_OCTREE; }
+ NODE_TYPE getNodeType() const { return GEOM_OCTREE; }
+ void computeLocalAABB()
+ {
+ aabb_center = Vec3f();
+ aabb_radius = (1 << tree->getTreeDepth()) * tree->getResolution() / 2;
+ }
};
+
}
#endif
diff --git a/trunk/fcl/src/BVH_model.cpp b/trunk/fcl/src/BVH_model.cpp
index 060995e63f15a035737dc20cae5e5900e0579d6b..607c69d5f074067eb8cb323802d4bcfdb55f5c6d 100644
--- a/trunk/fcl/src/BVH_model.cpp
+++ b/trunk/fcl/src/BVH_model.cpp
@@ -871,6 +871,8 @@ void BVHModel<BV>::computeLocalAABB()
}
aabb_radius = sqrt(aabb_radius);
+
+ aabb_local = aabb_;
}
template<>
diff --git a/trunk/fcl/src/ccd/taylor_matrix.cpp b/trunk/fcl/src/ccd/taylor_matrix.cpp
index d282b979f301f173f1ac2430116af0aab370e5af..2018d3a8aa03c67b079cde2ab257b33bd61e9647 100644
--- a/trunk/fcl/src/ccd/taylor_matrix.cpp
+++ b/trunk/fcl/src/ccd/taylor_matrix.cpp
@@ -96,14 +96,14 @@ const TVector3& TMatrix3::getRow(size_t i) const
return v_[i];
}
-const TVector3& TMatrix3::operator [] (size_t i) const
+const TaylorModel& TMatrix3::operator () (size_t i, size_t j) const
{
- return v_[i];
+ return v_[i][j];
}
-TVector3& TMatrix3::operator [] (size_t i)
+TaylorModel& TMatrix3::operator () (size_t i, size_t j)
{
- return v_[i];
+ return v_[i][j];
}
TMatrix3 TMatrix3::operator * (const Matrix3f& m) const
diff --git a/trunk/fcl/src/collision_func_matrix.cpp b/trunk/fcl/src/collision_func_matrix.cpp
index 669d0101f27aeb4c61dc9dd3a92474f55124c2c0..148b3f70a6d9f2c68f41b5516dfb4df90819cbe0 100644
--- a/trunk/fcl/src/collision_func_matrix.cpp
+++ b/trunk/fcl/src/collision_func_matrix.cpp
@@ -41,6 +41,7 @@
#include "fcl/simple_setup.h"
#include "fcl/geometric_shapes.h"
#include "fcl/BVH_model.h"
+#include "fcl/octree.h"
#include "fcl/collision_node.h"
#include "fcl/narrowphase/narrowphase.h"
@@ -48,6 +49,49 @@
namespace fcl
{
+template<typename T_SH, typename NarrowPhaseSolver>
+int ShapeOcTreeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+ const NarrowPhaseSolver* nsolver,
+ int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+{
+ const T_SH* obj1 = static_cast<const T_SH*>(o1);
+ const OcTree* obj2 = static_cast<const OcTree*>(o2);
+}
+
+template<typename T_SH, typename NarrowPhaseSolver>
+int OcTreeShapeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+ const NarrowPhaseSolver* nsolver,
+ int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+{
+ const OcTree* obj1 = static_cast<const OcTree*>(o1);
+ const T_SH* obj2 = static_cast<const T_SH*>(o2);
+}
+
+int OcTreeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+ int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+{
+ const OcTree* obj1 = static_cast<const OcTree*>(o1);
+ const OcTree* obj2 = static_cast<const OcTree*>(o2);
+
+}
+
+template<typename T_BVH, typename NarrowPhaseSolver>
+int OcTreeBVHCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+ int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+{
+ const OcTree* obj1 = static_cast<const OcTree*>(o1);
+ const BVHModel<T_BVH>* obj2 = static_cast<const BVHModel<T_BVH>*>(o2);
+}
+
+template<typename T_BVH, typename NarrowPhaseSolver>
+int BVHOcTreeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
+ int num_max_contacts, bool exhaustive, bool enable_contact, std::vector<Contact>& contacts)
+{
+ const BVHModel<T_BVH>* obj1 = static_cast<const BVHModel<T_BVH>*>(o1);
+ const OcTree* obj2 = static_cast<const OcTree*>(o2);
+}
+
+
template<typename T_SH1, typename T_SH2, typename NarrowPhaseSolver>
int ShapeShapeCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const CollisionGeometry* o2, const SimpleTransform& tf2,
const NarrowPhaseSolver* nsolver,
@@ -317,9 +361,9 @@ int BVHCollide(const CollisionGeometry* o1, const SimpleTransform& tf1, const Co
template<typename NarrowPhaseSolver>
CollisionFunctionMatrix<NarrowPhaseSolver>::CollisionFunctionMatrix()
{
- for(int i = 0; i < 16; ++i)
+ for(int i = 0; i < NODE_COUNT - 1; ++i)
{
- for(int j = 0; j < 16; ++j)
+ for(int j = 0; j < NODE_COUNT - 1; ++j)
collision_matrix[i][j] = NULL;
}
diff --git a/trunk/fcl/src/distance_func_matrix.cpp b/trunk/fcl/src/distance_func_matrix.cpp
index e79cd523865c100af017c6677116f3cd4b88145c..07973312968d15f078dbcc6367aa826afc8593aa 100644
--- a/trunk/fcl/src/distance_func_matrix.cpp
+++ b/trunk/fcl/src/distance_func_matrix.cpp
@@ -192,9 +192,9 @@ FCL_REAL BVHDistance(const CollisionGeometry* o1, const SimpleTransform& tf1, co
template<typename NarrowPhaseSolver>
DistanceFunctionMatrix<NarrowPhaseSolver>::DistanceFunctionMatrix()
{
- for(int i = 0; i < 16; ++i)
+ for(int i = 0; i < NODE_COUNT-1; ++i)
{
- for(int j = 0; j < 16; ++j)
+ for(int j = 0; j < NODE_COUNT-1; ++j)
distance_matrix[i][j] = NULL;
}
diff --git a/trunk/fcl/src/geometric_shapes.cpp b/trunk/fcl/src/geometric_shapes.cpp
index c594a7f85c71059c9f4fbf730f9210a8b30aec57..ddc5db2d7212eacc94a288d7c8904fc76e384278 100644
--- a/trunk/fcl/src/geometric_shapes.cpp
+++ b/trunk/fcl/src/geometric_shapes.cpp
@@ -116,66 +116,58 @@ void Plane::unitNormalTest()
void Box::computeLocalAABB()
{
- AABB aabb;
- computeBV<AABB>(*this, SimpleTransform(), aabb);
- aabb_center = aabb.center();
- aabb_radius = (aabb.min_ - aabb_center).length();
+ computeBV<AABB>(*this, SimpleTransform(), aabb_local);
+ aabb_center = aabb_local.center();
+ aabb_radius = (aabb_local.min_ - aabb_center).length();
}
void Sphere::computeLocalAABB()
{
- AABB aabb;
- computeBV<AABB>(*this, SimpleTransform(), aabb);
- aabb_center = aabb.center();
+ computeBV<AABB>(*this, SimpleTransform(), aabb_local);
+ aabb_center = aabb_local.center();
aabb_radius = radius;
}
void Capsule::computeLocalAABB()
{
- AABB aabb;
- computeBV<AABB>(*this, SimpleTransform(), aabb);
- aabb_center = aabb.center();
- aabb_radius = (aabb.min_ - aabb_center).length();
+ computeBV<AABB>(*this, SimpleTransform(), aabb_local);
+ aabb_center = aabb_local.center();
+ aabb_radius = (aabb_local.min_ - aabb_center).length();
}
void Cone::computeLocalAABB()
{
- AABB aabb;
- computeBV<AABB>(*this, SimpleTransform(), aabb);
- aabb_center = aabb.center();
- aabb_radius = (aabb.min_ - aabb_center).length();
+ computeBV<AABB>(*this, SimpleTransform(), aabb_local);
+ aabb_center = aabb_local.center();
+ aabb_radius = (aabb_local.min_ - aabb_center).length();
}
void Cylinder::computeLocalAABB()
{
- AABB aabb;
- computeBV<AABB>(*this, SimpleTransform(), aabb);
- aabb_center = aabb.center();
- aabb_radius = (aabb.min_ - aabb_center).length();
+ computeBV<AABB>(*this, SimpleTransform(), aabb_local);
+ aabb_center = aabb_local.center();
+ aabb_radius = (aabb_local.min_ - aabb_center).length();
}
void Convex::computeLocalAABB()
{
- AABB aabb;
- computeBV<AABB>(*this, SimpleTransform(), aabb);
- aabb_center = aabb.center();
- aabb_radius = (aabb.min_ - aabb_center).length();
+ computeBV<AABB>(*this, SimpleTransform(), aabb_local);
+ aabb_center = aabb_local.center();
+ aabb_radius = (aabb_local.min_ - aabb_center).length();
}
void Plane::computeLocalAABB()
{
- AABB aabb;
- computeBV<AABB>(*this, SimpleTransform(), aabb);
- aabb_center = aabb.center();
- aabb_radius = (aabb.min_ - aabb_center).length();
+ computeBV<AABB>(*this, SimpleTransform(), aabb_local);
+ aabb_center = aabb_local.center();
+ aabb_radius = (aabb_local.min_ - aabb_center).length();
}
void Triangle2::computeLocalAABB()
{
- AABB aabb;
- computeBV<AABB>(*this, SimpleTransform(), aabb);
- aabb_center = aabb.center();
- aabb_radius = (aabb.min_ - aabb_center).length();
+ computeBV<AABB>(*this, SimpleTransform(), aabb_local);
+ aabb_center = aabb_local.center();
+ aabb_radius = (aabb_local.min_ - aabb_center).length();
}
diff --git a/trunk/fcl/src/geometric_shapes_utility.cpp b/trunk/fcl/src/geometric_shapes_utility.cpp
index 74cf3767400cf2268c0afcab59f888fc6a587a83..c2f6fc9604c2ecffc2884fb39e2f95c5b9741550 100644
--- a/trunk/fcl/src/geometric_shapes_utility.cpp
+++ b/trunk/fcl/src/geometric_shapes_utility.cpp
@@ -475,6 +475,123 @@ void computeBV<KDOP<24>, Plane>(const Plane& s, const SimpleTransform& tf, KDOP<
}
+void constructBox(const AABB& bv, Box& box, SimpleTransform& tf)
+{
+ box = Box(bv.max_ - bv.min_);
+ tf = SimpleTransform(bv.center());
+}
+
+void constructBox(const OBB& bv, Box& box, SimpleTransform& tf)
+{
+ box = Box(bv.extent * 2);
+ tf = SimpleTransform(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
+ bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
+ bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.To);
+}
+
+void constructBox(const OBBRSS& bv, Box& box, SimpleTransform& tf)
+{
+ box = Box(bv.obb.extent * 2);
+ tf = SimpleTransform(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
+ bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
+ bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
+
+}
+
+void constructBox(const kIOS& bv, Box& box, SimpleTransform& tf)
+{
+ box = Box(bv.obb_bv.extent * 2);
+ tf = SimpleTransform(Matrix3f(bv.obb_bv.axis[0][0], bv.obb_bv.axis[1][0], bv.obb_bv.axis[2][0],
+ bv.obb_bv.axis[0][1], bv.obb_bv.axis[1][1], bv.obb_bv.axis[2][1],
+ bv.obb_bv.axis[0][2], bv.obb_bv.axis[1][2], bv.obb_bv.axis[2][2]), bv.obb_bv.To);
+
+}
+
+void constructBox(const RSS& bv, Box& box, SimpleTransform& tf)
+{
+ box = Box(bv.width(), bv.height(), bv.depth());
+ tf = SimpleTransform(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
+ bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
+ bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.Tr);
+}
+
+void constructBox(const KDOP<16>& bv, Box& box, SimpleTransform& tf)
+{
+ box = Box(bv.width(), bv.height(), bv.depth());
+ tf = SimpleTransform(bv.center());
+}
+
+void constructBox(const KDOP<18>& bv, Box& box, SimpleTransform& tf)
+{
+ box = Box(bv.width(), bv.height(), bv.depth());
+ tf = SimpleTransform(bv.center());
+}
+
+void constructBox(const KDOP<24>& bv, Box& box, SimpleTransform& tf)
+{
+ box = Box(bv.width(), bv.height(), bv.depth());
+ tf = SimpleTransform(bv.center());
+}
+
+
+
+void constructBox(const AABB& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf)
+{
+ box = Box(bv.max_ - bv.min_);
+ tf = tf_bv * SimpleTransform(bv.center());
+}
+
+void constructBox(const OBB& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf)
+{
+ box = Box(bv.extent * 2);
+ tf = tf_bv *SimpleTransform(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
+ bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
+ bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.To);
+}
+
+void constructBox(const OBBRSS& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf)
+{
+ box = Box(bv.obb.extent * 2);
+ tf = tf_bv * SimpleTransform(Matrix3f(bv.obb.axis[0][0], bv.obb.axis[1][0], bv.obb.axis[2][0],
+ bv.obb.axis[0][1], bv.obb.axis[1][1], bv.obb.axis[2][1],
+ bv.obb.axis[0][2], bv.obb.axis[1][2], bv.obb.axis[2][2]), bv.obb.To);
+
+}
+
+void constructBox(const kIOS& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf)
+{
+ box = Box(bv.obb_bv.extent * 2);
+ tf = tf_bv * SimpleTransform(Matrix3f(bv.obb_bv.axis[0][0], bv.obb_bv.axis[1][0], bv.obb_bv.axis[2][0],
+ bv.obb_bv.axis[0][1], bv.obb_bv.axis[1][1], bv.obb_bv.axis[2][1],
+ bv.obb_bv.axis[0][2], bv.obb_bv.axis[1][2], bv.obb_bv.axis[2][2]), bv.obb_bv.To);
+
+}
+
+void constructBox(const RSS& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf)
+{
+ box = Box(bv.width(), bv.height(), bv.depth());
+ tf = tf_bv * SimpleTransform(Matrix3f(bv.axis[0][0], bv.axis[1][0], bv.axis[2][0],
+ bv.axis[0][1], bv.axis[1][1], bv.axis[2][1],
+ bv.axis[0][2], bv.axis[1][2], bv.axis[2][2]), bv.Tr);
+}
+
+void constructBox(const KDOP<16>& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf)
+{
+ box = Box(bv.width(), bv.height(), bv.depth());
+ tf = tf_bv * SimpleTransform(bv.center());
+}
+
+void constructBox(const KDOP<18>& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf)
+{
+ box = Box(bv.width(), bv.height(), bv.depth());
+ tf = tf_bv * SimpleTransform(bv.center());
+}
+
+void constructBox(const KDOP<24>& bv, const SimpleTransform& tf_bv, Box& box, SimpleTransform& tf)
+{
+ box = Box(bv.width(), bv.height(), bv.depth());
+ tf = tf_bv * SimpleTransform(bv.center());
+}
diff --git a/trunk/fcl/src/octomap_extension.cpp b/trunk/fcl/src/octomap_extension.cpp
index 6e4832bd12a4696d79627cdc18745ad5436c4ca3..2437a04d4cfe6c1ad21508ab528a5f8be712026b 100644
--- a/trunk/fcl/src/octomap_extension.cpp
+++ b/trunk/fcl/src/octomap_extension.cpp
@@ -99,9 +99,7 @@ bool collisionRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
{
if(root1->bv.overlap(root2_bv))
{
- Box* box = new Box(root2_bv.max_[0] - root2_bv.min_[0],
- root2_bv.max_[1] - root2_bv.min_[1],
- root2_bv.max_[2] - root2_bv.min_[2]);
+ Box* box = new Box(root2_bv.max_ - root2_bv.min_);
CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), SimpleTransform(root2_bv.center()));
return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata);
}
@@ -157,9 +155,7 @@ bool distanceRecurse(DynamicAABBTreeCollisionManager::DynamicAABBNode* root1,
{
if(tree2->isNodeOccupied(root2))
{
- Box* box = new Box(root2_bv.max_[0] - root2_bv.min_[0],
- root2_bv.max_[1] - root2_bv.min_[1],
- root2_bv.max_[2] - root2_bv.min_[2]);
+ Box* box = new Box(root2_bv.max_ - root2_bv.min_);
CollisionObject obj(boost::shared_ptr<CollisionGeometry>(box), SimpleTransform(root2_bv.center()));
return callback(static_cast<CollisionObject*>(root1->data), &obj, cdata, min_dist);
}