Commit c45c79a7 authored by jpan's avatar jpan
Browse files

add necessary documentation and change according to code review


git-svn-id: https://kforge.ros.org/fcl/fcl_ros@152 253336fb-580f-4252-a368-f3cef5a2a82b
parent 3ee66493
......@@ -38,7 +38,7 @@
#ifndef FCL_COLLISION_H
#define FCL_COLLISION_H
#include "fcl/vec_3f.h"
#include "fcl/math/vec_3f.h"
#include "fcl/collision_object.h"
#include "fcl/collision_data.h"
......
/*
* 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_COLLISION_DATA_H
#define FCL_COLLISION_DATA_H
#include "fcl/collision_object.h"
#include "fcl/vec_3f.h"
#include "fcl/math/vec_3f.h"
#include <vector>
#include <set>
#include <limits>
......@@ -108,6 +145,8 @@ struct CostSource
}
};
struct CollisionResult;
/// @brief request to the collision algorithm
struct CollisionRequest
{
......@@ -133,6 +172,7 @@ struct CollisionRequest
{
}
bool isSatisfied(const CollisionResult& result) const;
};
/// @brief collision result
......@@ -145,18 +185,11 @@ private:
/// @brief cost sources
std::set<CostSource> cost_sources;
const CollisionRequest* request;
public:
CollisionResult()
{
request = NULL;
}
void setRequest(const CollisionRequest& request_)
{
request = &request_;
}
/// @brief add one contact into result structure
inline void addContact(const Contact& c)
......@@ -165,18 +198,11 @@ public:
}
/// @brief add one cost source into result structure
inline void addCostSource(const CostSource& c)
inline void addCostSource(const CostSource& c, std::size_t num_max_cost_sources)
{
if(request)
{
cost_sources.insert(c);
if(cost_sources.size() > request->num_max_cost_sources)
cost_sources.erase(cost_sources.begin());
}
else
{
cost_sources.insert(c);
}
cost_sources.insert(c);
if(cost_sources.size() > num_max_cost_sources)
cost_sources.erase(cost_sources.begin());
}
/// @brief return binary collision result
......@@ -228,7 +254,7 @@ public:
}
};
struct DistanceResult;
/// @brief request to the distance computation
struct DistanceRequest
......@@ -239,17 +265,17 @@ struct DistanceRequest
DistanceRequest(bool enable_nearest_points_ = false) : enable_nearest_points(enable_nearest_points_)
{
}
bool isSatisfied(const DistanceResult& result) const;
};
/// @brief distance result
struct DistanceResult
{
private:
const DistanceRequest* request;
public:
/// @brief minimum distance between two objects
/// @brief minimum distance between two objects. if two objects are in collision, min_distance <= 0.
FCL_REAL min_distance;
/// @brief nearest points
......@@ -282,13 +308,8 @@ public:
b1(NONE),
b2(NONE)
{
request = NULL;
}
void setRequest(const DistanceRequest& request_)
{
request = &request_;
}
/// @brief add distance information into the result
void update(FCL_REAL distance, const CollisionGeometry* o1_, const CollisionGeometry* o2_, int b1_, int b2_)
......
......@@ -45,11 +45,18 @@
namespace fcl
{
/// @brief collision matrix stores the functions for collision between different types of objects and provides a uniform call interface
template<typename NarrowPhaseSolver>
struct CollisionFunctionMatrix
{
/// @brief the uniform call interface for collision: for collision, we need know
/// 1. two objects o1 and o2 and their configuration in world coordinate tf1 and tf2;
/// 2. the solver for narrow phase collision, this is for the collision between geometric shapes;
/// 3. the request setting for collision (e.g., whether need to return normal information, whether need to compute cost);
/// 4. the structure to return collision 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);
/// @brief each item in the collision matrix is a function to handle collision between objects of type1 and type2
CollisionFunc collision_matrix[NODE_COUNT][NODE_COUNT];
CollisionFunctionMatrix();
......
......@@ -44,7 +44,7 @@
/// @brief collision and distance function on traversal nodes. these functions provide a higher level abstraction for collision functions provided in collision_func_matrix
namespace fcl
{
......
......@@ -39,7 +39,7 @@
#define FCL_COLLISION_OBJECT_BASE_H
#include "fcl/BV/AABB.h"
#include "fcl/transform.h"
#include "fcl/math/transform.h"
#include <boost/shared_ptr.hpp>
namespace fcl
......
......@@ -43,6 +43,8 @@
namespace fcl
{
/// @brief Main distance interface: given two collision objects, and the requirements for contacts, including whether return the nearest points, this function performs the distance between them.
/// Return value is the minimum distance generated between the two objects.
template<typename NarrowPhaseSolver>
FCL_REAL distance(const CollisionObject* o1, const CollisionObject* o2, const NarrowPhaseSolver* nsolver,
......
......@@ -43,12 +43,18 @@
namespace fcl
{
/// @brief distance matrix stores the functions for distance between different types of objects and provides a uniform call interface
template<typename NarrowPhaseSolver>
struct DistanceFunctionMatrix
{
/// @brief the uniform call interface for distance: for distance, we need know
/// 1. two objects o1 and o2 and their configuration in world coordinate tf1 and tf2;
/// 2. the solver for narrow phase collision, this is for distance computation between geometric shapes;
/// 3. the request setting for distance (e.g., whether need to return nearest points);
typedef FCL_REAL (*DistanceFunc)(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const NarrowPhaseSolver* nsolver,
const DistanceRequest& request, DistanceResult& result);
/// @brief each item in the distance matrix is a function to handle distance between objects of type1 and type2
DistanceFunc distance_matrix[NODE_COUNT][NODE_COUNT];
DistanceFunctionMatrix();
......
......@@ -42,13 +42,12 @@
#include "fcl/BVH_model.h"
#include <boost/math/constants/constants.hpp>
/** \brief Main namespace */
namespace fcl
{
/** \brief Generate BVH model from box */
/// @brief Generate BVH model from box
template<typename BV>
void generateBVHModel(BVHModel<BV>& model, const Box& shape, const Transform3f& pose = Transform3f())
void generateBVHModel(BVHModel<BV>& model, const Box& shape, const Transform3f& pose)
{
double a = shape.side[0];
double b = shape.side[1];
......@@ -88,9 +87,9 @@ void generateBVHModel(BVHModel<BV>& model, const Box& shape, const Transform3f&
model.computeLocalAABB();
}
/** Generate BVH model from sphere */
/// @brief Generate BVH model from sphere, given the number of segments along longitude and number of rings along latitude.
template<typename BV>
void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const Transform3f& pose = Transform3f(), unsigned int seg = 16, unsigned int ring = 16)
void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const Transform3f& pose, unsigned int seg, unsigned int ring)
{
std::vector<Vec3f> points;
std::vector<Triangle> tri_indices;
......@@ -153,83 +152,24 @@ void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const Transform3
model.computeLocalAABB();
}
/** \brief Generate BVH model from sphere
* The difference between generateBVHModel is that it gives the number of triangles faces N for a sphere with unit radius. For sphere of radius r,
* then the number of triangles is r * r * N so that the area represented by a single triangle is approximately the same.s
*/
/// @brief Generate BVH model from sphere
/// The difference between generateBVHModel is that it gives the number of triangles faces N for a sphere with unit radius. For sphere of radius r,
/// then the number of triangles is r * r * N so that the area represented by a single triangle is approximately the same.s
template<typename BV>
void generateBVHModel2(BVHModel<BV>& model, const Sphere& shape, const Transform3f& pose = Transform3f(), unsigned int n_faces_for_unit_sphere = 100)
void generateBVHModel(BVHModel<BV>& model, const Sphere& shape, const Transform3f& pose, unsigned int n_faces_for_unit_sphere)
{
std::vector<Vec3f> points;
std::vector<Triangle> tri_indices;
double r = shape.radius;
double n_low_bound = sqrtf(n_faces_for_unit_sphere / 2.0) * r * r;
unsigned int ring = ceil(n_low_bound);
unsigned int seg = ceil(n_low_bound);
double phi, phid;
const double pi = boost::math::constants::pi<double>();
phid = pi * 2 / seg;
phi = 0;
double theta, thetad;
thetad = pi / (ring + 1);
theta = 0;
for(unsigned int i = 0; i < ring; ++i)
{
double theta_ = theta + thetad * (i + 1);
for(unsigned int j = 0; j < seg; ++j)
{
points.push_back(Vec3f(r * sin(theta_) * cos(phi + j * phid), r * sin(theta_) * sin(phi + j * phid), r * cos(theta_)));
}
}
points.push_back(Vec3f(0, 0, r));
points.push_back(Vec3f(0, 0, -r));
for(unsigned int i = 0; i < ring - 1; ++i)
{
for(unsigned int j = 0; j < seg; ++j)
{
unsigned int a, b, c, d;
a = i * seg + j;
b = (j == seg - 1) ? (i * seg) : (i * seg + j + 1);
c = (i + 1) * seg + j;
d = (j == seg - 1) ? ((i + 1) * seg) : ((i + 1) * seg + j + 1);
tri_indices.push_back(Triangle(a, c, b));
tri_indices.push_back(Triangle(b, c, d));
}
}
for(unsigned int j = 0; j < seg; ++j)
{
unsigned int a, b;
a = j;
b = (j == seg - 1) ? 0 : (j + 1);
tri_indices.push_back(Triangle(ring * seg, a, b));
a = (ring - 1) * seg + j;
b = (j == seg - 1) ? (ring - 1) * seg : ((ring - 1) * seg + j + 1);
tri_indices.push_back(Triangle(a, ring * seg + 1, b));
}
for(unsigned int i = 0; i < points.size(); ++i)
{
points[i] = pose.transform(points[i]);
}
model.beginModel();
model.addSubModel(points, tri_indices);
model.endModel();
model.computeLocalAABB();
generateBVHModel(model, shape, pose, seg, ring);
}
/** \brief Generate BVH model from cylinder */
/// @brief Generate BVH model from cylinder, given the number of segments along circle and the number of segments along axis.
template<typename BV>
void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const Transform3f& pose = Transform3f(), unsigned int tot = 16)
void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const Transform3f& pose, unsigned int tot, unsigned int h_num)
{
std::vector<Vec3f> points;
std::vector<Triangle> tri_indices;
......@@ -241,8 +181,6 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const Transfor
phid = pi * 2 / tot;
phi = 0;
double circle_edge = phid * r;
unsigned int h_num = ceil(h / circle_edge);
double hd = h / h_num;
for(unsigned int i = 0; i < tot; ++i)
......@@ -301,88 +239,29 @@ void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const Transfor
model.computeLocalAABB();
}
/** \brief Generate BVH model from cylinder
* Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cylinder with
* larger radius, the number of circle split number is r * tot.
*/
/// @brief Generate BVH model from cylinder
/// Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cylinder with
/// larger radius, the number of circle split number is r * tot.
template<typename BV>
void generateBVHModel2(BVHModel<BV>& model, const Cylinder& shape, const Transform3f& pose = Transform3f(), unsigned int tot_for_unit_cylinder = 100)
void generateBVHModel(BVHModel<BV>& model, const Cylinder& shape, const Transform3f& pose, unsigned int tot_for_unit_cylinder)
{
std::vector<Vec3f> points;
std::vector<Triangle> tri_indices;
double r = shape.radius;
double h = shape.lz;
double phi, phid;
const double pi = boost::math::constants::pi<double>();
unsigned int tot = tot_for_unit_cylinder * r;
phid = pi * 2 / tot;
phi = 0;
double phid = pi * 2 / tot;
double circle_edge = phid * r;
unsigned int h_num = ceil(h / circle_edge);
double hd = h / h_num;
for(unsigned int i = 0; i < tot; ++i)
points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), h / 2));
for(unsigned int i = 0; i < h_num - 1; ++i)
{
for(unsigned int j = 0; j < tot; ++j)
{
points.push_back(Vec3f(r * cos(phi + phid * j), r * sin(phi + phid * j), h / 2 - (i + 1) * hd));
}
}
for(unsigned int i = 0; i < tot; ++i)
points.push_back(Vec3f(r * cos(phi + phid * i), r * sin(phi + phid * i), - h / 2));
points.push_back(Vec3f(0, 0, h / 2));
points.push_back(Vec3f(0, 0, -h / 2));
for(unsigned int i = 0; i < tot; ++i)
{
Triangle tmp((h_num + 1) * tot, i, ((i == tot - 1) ? 0 : (i + 1)));
tri_indices.push_back(tmp);
}
for(unsigned int i = 0; i < tot; ++i)
{
Triangle tmp((h_num + 1) * tot + 1, h_num * tot + ((i == tot - 1) ? 0 : (i + 1)), h_num * tot + i);
tri_indices.push_back(tmp);
}
for(unsigned int i = 0; i < h_num; ++i)
{
for(unsigned int j = 0; j < tot; ++j)
{
int a, b, c, d;
a = j;
b = (j == tot - 1) ? 0 : (j + 1);
c = j + tot;
d = (j == tot - 1) ? tot : (j + 1 + tot);
int start = i * tot;
tri_indices.push_back(Triangle(start + b, start + a, start + c));
tri_indices.push_back(Triangle(start + b, start + c, start + d));
}
}
for(unsigned int i = 0; i < points.size(); ++i)
{
points[i] = pose.transform(points[i]);
}
model.beginModel();
model.addSubModel(points, tri_indices);
model.endModel();
model.computeLocalAABB();
generateBVHModel(model, shape, pose, tot, h_num);
}
/** \brief Generate BVH model from cone */
/// @brief Generate BVH model from cone, given the number of segments along circle and the number of segments along axis.
template<typename BV>
void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const Transform3f& pose = Transform3f(), unsigned int tot = 16)
void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const Transform3f& pose, unsigned int tot, unsigned int h_num)
{
std::vector<Vec3f> points;
std::vector<Triangle> tri_indices;
......@@ -395,8 +274,6 @@ void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const Transform3f&
phid = pi * 2 / tot;
phi = 0;
double circle_edge = phid * r;
unsigned int h_num = ceil(h / circle_edge);
double hd = h / h_num;
for(unsigned int i = 0; i < h_num - 1; ++i)
......@@ -452,6 +329,25 @@ void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const Transform3f&
model.computeLocalAABB();
}
/// @brief Generate BVH model from cone
/// Difference from generateBVHModel: is that it gives the circle split number tot for a cylinder with unit radius. For cone with
/// larger radius, the number of circle split number is r * tot.
template<typename BV>
void generateBVHModel(BVHModel<BV>& model, const Cone& shape, const Transform3f& pose, unsigned int tot_for_unit_cone)
{
double r = shape.radius;
double h = shape.lz;
const double pi = boost::math::constants::pi<double>();
unsigned int tot = tot_for_unit_cone * r;
double phid = pi * 2 / tot;
double circle_edge = phid * r;
unsigned int h_num = ceil(h / circle_edge);
generateBVHModel(model, shape, pose, tot, h_num);
}
}
#endif
......@@ -39,24 +39,23 @@
#define FCL_GEOMETRIC_SHAPES_H
#include "fcl/collision_object.h"
#include "fcl/vec_3f.h"
#include "fcl/math/vec_3f.h"
#include <string.h>
/** \brief Main namespace */
namespace fcl
{
/** \brief Base class for all basic geometric shapes */
/// @brief Base class for all basic geometric shapes
class ShapeBase : public CollisionGeometry
{
public:
/** \brief Default Constructor */
ShapeBase() {}
/** \brief Get object type: a geometric shape */
/// @brief Get object type: a geometric shape
OBJECT_TYPE getObjectType() const { return OT_GEOM; }
};
/// @brief Triangle stores the points instead of only indices of points
class Triangle2 : public ShapeBase
{
public:
......@@ -64,6 +63,7 @@ public:
{
}
/// @brief virtual function of compute AABB in local coordinate
void computeLocalAABB();
NODE_TYPE getNodeType() const { return GEOM_TRIANGLE; }
......@@ -71,7 +71,7 @@ public:
Vec3f a, b, c;
};
/** Center at zero point, axis aligned box */
/// @brief Center at zero point, axis aligned box
class Box : public ShapeBase
{
public:
......@@ -85,17 +85,17 @@ public:
Box() {}
/** box side length */
/// @brief box side length
Vec3f side;
/** \brief Compute AABB */
/// @brief Compute AABB
void computeLocalAABB();
/** \brief Get node type: a box */
/// @brief Get node type: a box
NODE_TYPE getNodeType() const { return GEOM_BOX; }
};
/** Center at zero point sphere */
/// @brief Center at zero point sphere
class Sphere : public ShapeBase
{
public:
......@@ -103,17 +103,17 @@ public:
{
}
/** \brief Radius of the sphere */
/// @brief Radius of the sphere
FCL_REAL radius;
/** \brief Compute AABB */
/// @brief Compute AABB
void computeLocalAABB();
/** \brief Get node type: a sphere */
/// @brief Get node type: a sphere
NODE_TYPE getNodeType() const { return GEOM_SPHERE; }
};
/** Center at zero point capsule */
/// @brief Center at zero point capsule
class Capsule : public ShapeBase
{
public:
......@@ -121,20 +121,20 @@ public:
{
}
/** \brief Radius of capsule */
/// @brief Radius of capsule
FCL_REAL radius;
/** \brief Length along z axis */
/// @brief Length along z axis
FCL_REAL lz;
/** \brief Compute AABB */
/// @brief Compute AABB
void computeLocalAABB();
/** \brief Get node type: a capsule */
/// @brief Get node type: a capsule
NODE_TYPE getNodeType() const { return GEOM_CAPSULE; }
};
/** Center at zero cone */
/// @brief Center at zero cone
class Cone : public ShapeBase
{
public:
......@@ -142,21 +142,20 @@ public: