Commit 0932ea44 authored by jpan's avatar jpan
Browse files

octomap operations with shape and mesh


git-svn-id: https://kforge.ros.org/fcl/fcl_ros@133 253336fb-580f-4252-a368-f3cef5a2a82b
parent 53272a8c
......@@ -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;
......
......@@ -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;
......
......@@ -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();
};
......
......@@ -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
......
......@@ -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();
};
......
......@@ -81,6 +81,11 @@ public:
computeLocalAABB();
}
Box(const Vec3f& side_) : ShapeBase(), side(side_)
{
computeLocalAABB();
}
/** box side length */
Vec3f side;
......
......@@ -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
......@@ -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
......@@ -871,6 +871,8 @@ void BVHModel<BV>::computeLocalAABB()
}
aabb_radius = sqrt(aabb_radius);
aabb_local = aabb_;
}
template<>
......
......@@ -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
......
......@@ -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;
}
......
......@@ -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;
}
......
......@@ -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();
}
......
......@@ -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());
}
......
......@@ -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);
}
......
Markdown is supported
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