Unverified Commit 0441937a authored by Joseph Mirabel's avatar Joseph Mirabel Committed by GitHub
Browse files

Merge pull request #74 from LucileRemigy/devel

Clean include  BV/BVH/math/mesh_loader
parents c83a604f 8fbd0cb6
...@@ -128,6 +128,7 @@ SET(${PROJECT_NAME}_HEADERS ...@@ -128,6 +128,7 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/distance.h include/hpp/fcl/distance.h
include/hpp/fcl/math/matrix_3f.h include/hpp/fcl/math/matrix_3f.h
include/hpp/fcl/math/vec_3f.h include/hpp/fcl/math/vec_3f.h
include/hpp/fcl/math/types.h
include/hpp/fcl/math/tools.h include/hpp/fcl/math/tools.h
include/hpp/fcl/math/transform.h include/hpp/fcl/math/transform.h
include/hpp/fcl/traversal/details/traversal.h include/hpp/fcl/traversal/details/traversal.h
......
...@@ -39,8 +39,7 @@ ...@@ -39,8 +39,7 @@
#define HPP_FCL_AABB_H #define HPP_FCL_AABB_H
#include <stdexcept> #include <stdexcept>
#include <hpp/fcl/math/vec_3f.h> #include <hpp/fcl/math/types.h>
#include <hpp/fcl/math/matrix_3f.h>
namespace hpp namespace hpp
{ {
...@@ -104,23 +103,12 @@ public: ...@@ -104,23 +103,12 @@ public:
return overlap (other); return overlap (other);
} }
/// @brief Check whether the AABB contains another AABB /// @brief Check whether the AABB contains another AABB
inline bool contain(const AABB& other) const inline bool contain(const AABB& other) const
{ {
return (other.min_[0] >= min_[0]) && (other.max_[0] <= max_[0]) && (other.min_[1] >= min_[1]) && (other.max_[1] <= max_[1]) && (other.min_[2] >= min_[2]) && (other.max_[2] <= max_[2]); return (other.min_[0] >= min_[0]) && (other.max_[0] <= max_[0]) && (other.min_[1] >= min_[1]) && (other.max_[1] <= max_[1]) && (other.min_[2] >= min_[2]) && (other.max_[2] <= max_[2]);
} }
/// @brief Check whether two AABB are overlapped along specific axis
inline bool axisOverlap(const AABB& other, int axis_id) const
{
if(min_[axis_id] > other.max_[axis_id]) return false;
if(max_[axis_id] < other.min_[axis_id]) return false;
return true;
}
/// @brief Check whether two AABB are overlap and return the overlap part /// @brief Check whether two AABB are overlap and return the overlap part
inline bool overlap(const AABB& other, AABB& overlap_part) const inline bool overlap(const AABB& other, AABB& overlap_part) const
{ {
...@@ -186,7 +174,7 @@ public: ...@@ -186,7 +174,7 @@ public:
return max_[2] - min_[2]; return max_[2] - min_[2];
} }
/// @brief Volume of the AABB /// @brief Volume of the AABB
inline FCL_REAL volume() const inline FCL_REAL volume() const
{ {
return width() * height() * depth(); return width() * height() * depth();
...@@ -198,12 +186,6 @@ public: ...@@ -198,12 +186,6 @@ public:
return (max_ - min_).squaredNorm(); return (max_ - min_).squaredNorm();
} }
/// @brief Radius of the AABB
inline FCL_REAL radius() const
{
return (max_ - min_).norm() / 2;
}
/// @brief Center of the AABB /// @brief Center of the AABB
inline Vec3f center() const inline Vec3f center() const
{ {
...@@ -216,12 +198,6 @@ public: ...@@ -216,12 +198,6 @@ public:
/// @brief Distance between two AABBs /// @brief Distance between two AABBs
FCL_REAL distance(const AABB& other) const; FCL_REAL distance(const AABB& other) const;
/// @brief whether two AABB are equal
inline bool equal(const AABB& other) const
{
return isEqual(min_, other.min_) && isEqual(max_, other.max_);
}
/// @brief expand the half size of the AABB by delta, and keep the center unchanged. /// @brief expand the half size of the AABB by delta, and keep the center unchanged.
inline AABB& expand(const Vec3f& delta) inline AABB& expand(const Vec3f& delta)
{ {
......
...@@ -90,43 +90,7 @@ class Converter<AABB, OBB> ...@@ -90,43 +90,7 @@ class Converter<AABB, OBB>
{ {
public: public:
static void convert(const AABB& bv1, const Transform3f& tf1, OBB& bv2) static void convert(const AABB& bv1, const Transform3f& tf1, OBB& bv2)
{ {
/*
bv2.To = tf1.transform(bv1.center());
/// Sort the AABB edges so that AABB extents are ordered.
FCL_REAL d[3] = {bv1.width(), bv1.height(), bv1.depth() };
std::size_t id[3] = {0, 1, 2};
for(std::size_t i = 1; i < 3; ++i)
{
for(std::size_t j = i; j > 0; --j)
{
if(d[j] > d[j-1])
{
{
FCL_REAL tmp = d[j];
d[j] = d[j-1];
d[j-1] = tmp;
}
{
std::size_t tmp = id[j];
id[j] = id[j-1];
id[j-1] = tmp;
}
}
}
}
Vec3f extent = (bv1.max_ - bv1.min_) * 0.5;
bv2.extent = Vec3f(extent[id[0]], extent[id[1]], extent[id[2]]);
const Matrix3f& R = tf1.getRotation();
bool left_hand = (id[0] == (id[1] + 1) % 3);
bv2.axis[0] = left_hand ? -R.col(id[0]) : R.col(id[0]);
bv2.axis[1] = R.col(id[1]);
bv2.axis[2] = R.col(id[2]);
*/
bv2.To.noalias() = tf1.transform(bv1.center()); bv2.To.noalias() = tf1.transform(bv1.center());
bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5; bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
bv2.axes.noalias() = tf1.getRotation(); bv2.axes.noalias() = tf1.getRotation();
......
...@@ -39,8 +39,7 @@ ...@@ -39,8 +39,7 @@
#ifndef HPP_FCL_BV_NODE_H #ifndef HPP_FCL_BV_NODE_H
#define HPP_FCL_BV_NODE_H #define HPP_FCL_BV_NODE_H
#include <hpp/fcl/math/vec_3f.h> #include <hpp/fcl/math/types.h>
#include <hpp/fcl/math/matrix_3f.h>
#include <hpp/fcl/BV/BV.h> #include <hpp/fcl/BV/BV.h>
#include <iostream> #include <iostream>
......
...@@ -38,9 +38,7 @@ ...@@ -38,9 +38,7 @@
#ifndef HPP_FCL_OBB_H #ifndef HPP_FCL_OBB_H
#define HPP_FCL_OBB_H #define HPP_FCL_OBB_H
#include <hpp/fcl/math/types.h>
#include <hpp/fcl/math/vec_3f.h>
#include <hpp/fcl/math/matrix_3f.h>
namespace hpp namespace hpp
{ {
...@@ -73,13 +71,6 @@ public: ...@@ -73,13 +71,6 @@ public:
bool overlap(const OBB& other, const CollisionRequest& request, bool overlap(const OBB& other, const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound) const; FCL_REAL& sqrDistLowerBound) const;
/// @brief Check collision between two OBB and return the overlap part. For OBB, the overlap_part return value is NOT used as the overlap part of two obbs usually is not an obb.
bool overlap(const OBB& other, OBB& /*overlap_part*/) const
{
return overlap(other);
}
/// @brief Check whether the OBB contains a point. /// @brief Check whether the OBB contains a point.
bool contain(const Vec3f& p) const; bool contain(const Vec3f& p) const;
......
...@@ -74,13 +74,7 @@ public: ...@@ -74,13 +74,7 @@ public:
return obb.overlap(other.obb, request, sqrDistLowerBound); return obb.overlap(other.obb, request, sqrDistLowerBound);
} }
/// @brief Check collision between two OBBRSS and return the overlap part. /// @brief Check whether the OBBRSS contains a point
bool overlap(const OBBRSS& other, OBBRSS& /*overlap_part*/) const
{
return overlap(other);
}
/// @brief Check whether the OBBRSS contains a point
inline bool contain(const Vec3f& p) const inline bool contain(const Vec3f& p) const
{ {
return obb.contain(p); return obb.contain(p);
...@@ -153,9 +147,6 @@ public: ...@@ -153,9 +147,6 @@ public:
} }
}; };
/// @brief Translate the OBBRSS bv
OBBRSS translate(const OBBRSS& bv, const Vec3f& t);
/// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) and b2 is in indentity /// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) and b2 is in indentity
bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2); bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2);
......
...@@ -39,8 +39,7 @@ ...@@ -39,8 +39,7 @@
#define HPP_FCL_RSS_H #define HPP_FCL_RSS_H
#include <stdexcept> #include <stdexcept>
#include <hpp/fcl/math/vec_3f.h> #include <hpp/fcl/math/types.h>
#include <hpp/fcl/math/matrix_3f.h>
#include <boost/math/constants/constants.hpp> #include <boost/math/constants/constants.hpp>
namespace hpp namespace hpp
...@@ -142,10 +141,6 @@ public: ...@@ -142,10 +141,6 @@ public:
}; };
/// @brief Translate the RSS bv
RSS translate(const RSS& bv, const Vec3f& t);
/// @brief distance between two RSS bounding volumes /// @brief distance between two RSS bounding volumes
/// P and Q (optional return values) are the closest points in the rectangles, not the RSS. But the direction P - Q is the correct direction for cloest points /// P and Q (optional return values) are the closest points in the rectangles, not the RSS. But the direction P - Q is the correct direction for cloest points
/// Notice that P and Q are both in the local frame of the first RSS (not global frame and not even the local frame of object 1) /// Notice that P and Q are both in the local frame of the first RSS (not global frame and not even the local frame of object 1)
......
...@@ -39,7 +39,7 @@ ...@@ -39,7 +39,7 @@
#define HPP_FCL_KDOP_H #define HPP_FCL_KDOP_H
#include <stdexcept> #include <stdexcept>
#include <hpp/fcl/math/matrix_3f.h> #include <hpp/fcl/math/types.h>
namespace hpp namespace hpp
{ {
...@@ -85,6 +85,10 @@ namespace fcl ...@@ -85,6 +85,10 @@ namespace fcl
template<size_t N> template<size_t N>
class KDOP class KDOP
{ {
private:
/// @brief Origin's distances to N KDOP planes
FCL_REAL dist_[N];
public: public:
/// @brief Creating kDOP containing nothing /// @brief Creating kDOP containing nothing
...@@ -158,11 +162,6 @@ public: ...@@ -158,11 +162,6 @@ public:
/// @brief The distance between two KDOP<N>. Not implemented. /// @brief The distance between two KDOP<N>. Not implemented.
FCL_REAL distance(const KDOP<N>& other, Vec3f* P = NULL, Vec3f* Q = NULL) const; FCL_REAL distance(const KDOP<N>& other, Vec3f* P = NULL, Vec3f* Q = NULL) const;
private:
/// @brief Origin's distances to N KDOP planes
FCL_REAL dist_[N];
public:
inline FCL_REAL dist(std::size_t i) const inline FCL_REAL dist(std::size_t i) const
{ {
return dist_[i]; return dist_[i];
......
...@@ -100,14 +100,6 @@ public: ...@@ -100,14 +100,6 @@ public:
bool overlap(const kIOS& other, const CollisionRequest&, bool overlap(const kIOS& other, const CollisionRequest&,
FCL_REAL& sqrDistLowerBound) const; FCL_REAL& sqrDistLowerBound) const;
/// @brief Check collision between two kIOS and return the overlap part.
/// For kIOS, we return nothing, as the overlappart of two kIOS usually is not an kIOS
/// @todo Not efficient. It first checks the sphere collisions and then use OBB for further culling.
bool overlap(const kIOS& other, kIOS& /*overlap_part*/) const
{
return overlap(other);
}
/// @brief Check whether the kIOS contains a point /// @brief Check whether the kIOS contains a point
inline bool contain(const Vec3f& p) const; inline bool contain(const Vec3f& p) const;
......
...@@ -59,6 +59,30 @@ class BVHModel : public CollisionGeometry, ...@@ -59,6 +59,30 @@ class BVHModel : public CollisionGeometry,
{ {
public: public:
/// @brief Geometry point data
Vec3f* vertices;
/// @brief Geometry triangle index data, will be NULL for point clouds
Triangle* tri_indices;
/// @brief Geometry point data in previous frame
Vec3f* prev_vertices;
/// @brief Number of triangles
int num_tris;
/// @brief Number of points
int num_vertices;
/// @brief The state of BVH building process
BVHBuildState build_state;
/// @brief Split rule to split one BV node into two children
boost::shared_ptr<BVSplitterBase<BV> > bv_splitter;
/// @brief Fitting rule to fit a BV node to a set of geometry primitives
boost::shared_ptr<BVFitterBase<BV> > bv_fitter;
/// @brief Model type described by the instance /// @brief Model type described by the instance
BVHModelType getModelType() const BVHModelType getModelType() const
{ {
...@@ -150,7 +174,6 @@ public: ...@@ -150,7 +174,6 @@ public:
/// @brief End BVH model construction, will build the bounding volume hierarchy /// @brief End BVH model construction, will build the bounding volume hierarchy
int endModel(); int endModel();
/// @brief Replace the geometry information of current frame (i.e. should have the same mesh topology with the previous frame) /// @brief Replace the geometry information of current frame (i.e. should have the same mesh topology with the previous frame)
int beginReplaceModel(); int beginReplaceModel();
...@@ -166,7 +189,6 @@ public: ...@@ -166,7 +189,6 @@ public:
/// @brief End BVH model replacement, will also refit or rebuild the bounding volume hierarchy /// @brief End BVH model replacement, will also refit or rebuild the bounding volume hierarchy
int endReplaceModel(bool refit = true, bool bottomup = true); int endReplaceModel(bool refit = true, bool bottomup = true);
/// @brief Replace the geometry information of current frame (i.e. should have the same mesh topology with the previous frame). /// @brief Replace the geometry information of current frame (i.e. should have the same mesh topology with the previous frame).
/// The current frame will be saved as the previous frame in prev_vertices. /// The current frame will be saved as the previous frame in prev_vertices.
int beginUpdateModel(); int beginUpdateModel();
...@@ -186,7 +208,7 @@ public: ...@@ -186,7 +208,7 @@ public:
/// @brief Check the number of memory used /// @brief Check the number of memory used
int memUsage(int msg) const; int memUsage(int msg) const;
/// @brief This is a special acceleration: BVH_model default stores the BV's transform in world coordinate. However, we can also store each BV's transform related to its parent /// @brief This is a special acceleration: BVH_model default stores the BV's transform in world coordinate. However, we can also store each BV's transform related to its parent
/// BV node. When traversing the BVH, this can save one matrix transformation. /// BV node. When traversing the BVH, this can save one matrix transformation.
void makeParentRelative() void makeParentRelative()
{ {
...@@ -244,32 +266,6 @@ public: ...@@ -244,32 +266,6 @@ public:
return C.trace() * Matrix3f::Identity() - C; return C.trace() * Matrix3f::Identity() - C;
} }
public:
/// @brief Geometry point data
Vec3f* vertices;
/// @brief Geometry triangle index data, will be NULL for point clouds
Triangle* tri_indices;
/// @brief Geometry point data in previous frame
Vec3f* prev_vertices;
/// @brief Number of triangles
int num_tris;
/// @brief Number of points
int num_vertices;
/// @brief The state of BVH building process
BVHBuildState build_state;
/// @brief Split rule to split one BV node into two children
boost::shared_ptr<BVSplitterBase<BV> > bv_splitter;
/// @brief Fitting rule to fit a BV node to a set of geometry primitives
boost::shared_ptr<BVFitterBase<BV> > bv_fitter;
private: private:
int num_tris_allocated; int num_tris_allocated;
......
...@@ -46,39 +46,6 @@ namespace hpp ...@@ -46,39 +46,6 @@ namespace hpp
{ {
namespace fcl namespace fcl
{ {
/// @brief Expand the BVH bounding boxes according to the variance matrix corresponding to the data stored within each BV node
template<typename BV>
void BVHExpand(BVHModel<BV>& model, const Variance3f* ucs, FCL_REAL r)
{
for(int i = 0; i < model.num_bvs; ++i)
{
BVNode<BV>& bvnode = model.getBV(i);
BV bv;
for(int j = 0; j < bvnode.num_primitives; ++j)
{
int v_id = bvnode.first_primitive + j;
const Variance3f& uc = ucs[v_id];
Vec3f& v = model.vertices[bvnode.first_primitive + j];
for(int k = 0; k < 3; ++k)
{
bv += (v + uc.axis[k] * (r * uc.sigma[k]));
bv += (v - uc.axis[k] * (r * uc.sigma[k]));
}
}
bvnode.bv = bv;
}
}
/// @brief Expand the BVH bounding boxes according to the corresponding variance information, for OBB
void BVHExpand(BVHModel<OBB>& model, const Variance3f* ucs, FCL_REAL r);
/// @brief Expand the BVH bounding boxes according to the corresponding variance information, for RSS
void BVHExpand(BVHModel<RSS>& model, const Variance3f* ucs, FCL_REAL r);
/// @brief Extract the part of the BVHModel that is inside an AABB. /// @brief Extract the part of the BVHModel that is inside an AABB.
/// A triangle in collision with the AABB is considered inside. /// A triangle in collision with the AABB is considered inside.
template<typename BV> template<typename BV>
...@@ -99,7 +66,6 @@ void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec ...@@ -99,7 +66,6 @@ void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec
/// @brief Compute the maximum distance from a given center point to a point cloud /// @brief Compute the maximum distance from a given center point to a point cloud
FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Vec3f& query); FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Vec3f& query);
} }
} // namespace hpp } // namespace hpp
......
...@@ -39,7 +39,7 @@ ...@@ -39,7 +39,7 @@
#ifndef HPP_FCL_COLLISION_H #ifndef HPP_FCL_COLLISION_H
#define HPP_FCL_COLLISION_H #define HPP_FCL_COLLISION_H
#include <hpp/fcl/math/vec_3f.h> #include <hpp/fcl/math/types.h>
#include <hpp/fcl/collision_object.h> #include <hpp/fcl/collision_object.h>
#include <hpp/fcl/collision_data.h> #include <hpp/fcl/collision_data.h>
......
...@@ -41,7 +41,7 @@ ...@@ -41,7 +41,7 @@
#include <hpp/fcl/collision_object.h> #include <hpp/fcl/collision_object.h>
#include <hpp/fcl/math/vec_3f.h> #include <hpp/fcl/math/types.h>
#include <vector> #include <vector>
#include <set> #include <set>
#include <limits> #include <limits>
......
...@@ -49,7 +49,7 @@ namespace fcl ...@@ -49,7 +49,7 @@ namespace fcl
{ {
/// @brief collision matrix stores the functions for collision between different types of objects and provides a uniform call interface /// @brief collision matrix stores the functions for collision between different types of objects and provides a uniform call interface
template<typename NarrowPhaseSolver> template<typename GJKSolver>
struct CollisionFunctionMatrix struct CollisionFunctionMatrix
{ {
/// @brief the uniform call interface for collision: for collision, we need know /// @brief the uniform call interface for collision: for collision, we need know
...@@ -57,7 +57,7 @@ struct CollisionFunctionMatrix ...@@ -57,7 +57,7 @@ struct CollisionFunctionMatrix
/// 2. the solver for narrow phase collision, this is for the collision between geometric shapes; /// 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); /// 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 /// 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); typedef std::size_t (*CollisionFunc)(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* 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 /// @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]; CollisionFunc collision_matrix[NODE_COUNT][NODE_COUNT];
......
...@@ -240,11 +240,7 @@ public: ...@@ -240,11 +240,7 @@ public:
return t.getRotation(); return t.getRotation();
} }
/// @brief get quaternion rotation of the object
inline const Quaternion3f& getQuatRotation() const
{
return t.getQuatRotation();
}
/// @brief get object's transform /// @brief get object's transform
inline const Transform3f& getTransform() const inline const Transform3f& getTransform() const
...@@ -264,11 +260,7 @@ public: ...@@ -264,11 +260,7 @@ public:
t.setTranslation(T); t.setTranslation(T);
} }
/// @brief set object's quatenrion rotation
void setQuatRotation(const Quaternion3f& q)
{
t.setQuatRotation(q);
}
/// @brief set object's transform /// @brief set object's transform
void setTransform(const Matrix3f& R, const Vec3f& T) void setTransform(const Matrix3f& R, const Vec3f& T)
...@@ -276,11 +268,7 @@ public: ...@@ -276,11 +268,7 @@ public:
t.setTransform(R, T); t.setTransform(R, T);
} }
/// @brief set object's transform
void setTransform(const Quaternion3f& q, const Vec3f& T)
{
t.setTransform(q, T);
}
/// @brief set object's transform /// @brief set object's transform