Skip to content
GitLab
Commit 03f773bd authored by Lucile Remigy's avatar Lucile Remigy
Browse files

Merge branch 'devel' into refactoring

parents 6884d050 2d4578cd
Hide whitespace changes
Inline Side-by-side
CMakeLists.txt
View file @ 03f773bd
......@@ -128,13 +128,10 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/math/types.h
include/hpp/fcl/math/transform.h
include/hpp/fcl/data_types.h
include/hpp/fcl/BVH/BV_splitter.h
include/hpp/fcl/BVH/BVH_internal.h
include/hpp/fcl/BVH/BVH_model.h
include/hpp/fcl/BVH/BV_fitter.h
include/hpp/fcl/BVH/BVH_front.h
include/hpp/fcl/BVH/BVH_utility.h
include/hpp/fcl/intersect.h
include/hpp/fcl/collision_object.h
include/hpp/fcl/collision_utility.h
include/hpp/fcl/octree.h
......
include/hpp/fcl/BV/AABB.h
View file @ 03f773bd
......@@ -39,7 +39,7 @@
#define HPP_FCL_AABB_H
#include <stdexcept>
#include <hpp/fcl/math/types.h>
#include <hpp/fcl/data_types.h>
namespace hpp
{
......@@ -81,16 +81,6 @@ public:
{
}
//start API in common test
/// @name Bounding volume API
/// Common API to BVs.
/// @{
......@@ -189,19 +179,6 @@ public:
}
/// @}
//End API in common test
/// @brief Check whether the AABB contains another AABB
inline bool contain(const AABB& other) const
......
include/hpp/fcl/BV/BV.h
View file @ 03f773bd
......@@ -125,7 +125,7 @@ class Converter<RSS, OBB>
public:
static void convert(const RSS& bv1, const Transform3f& tf1, OBB& bv2)
{
bv2.extent.noalias() = Vec3f(bv1.l[0] * 0.5 + bv1.r, bv1.l[1] * 0.5 + bv1.r, bv1.r);
bv2.extent.noalias() = Vec3f(bv1.length[0] * 0.5 + bv1.radius, bv1.length[1] * 0.5 + bv1.radius, bv1.radius);
bv2.To.noalias() = tf1.transform(bv1.Tr);
bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
}
......@@ -168,9 +168,9 @@ public:
bv2.Tr = tf1.transform(bv1.To);
bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
bv2.r = bv1.extent[2];
bv2.l[0] = 2 * (bv1.extent[0] - bv2.r);
bv2.l[1] = 2 * (bv1.extent[1] - bv2.r);
bv2.radius = bv1.extent[2];
bv2.length[0] = 2 * (bv1.extent[0] - bv2.radius);
bv2.length[1] = 2 * (bv1.extent[1] - bv2.radius);
}
};
......@@ -183,9 +183,9 @@ public:
bv2.Tr.noalias() = tf1.transform(bv1.Tr);
bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
bv2.r = bv1.r;
bv2.l[0] = bv1.l[0];
bv2.l[1] = bv1.l[1];
bv2.radius = bv1.radius;
bv2.length[0] = bv1.length[0];
bv2.length[1] = bv1.length[1];
}
};
......@@ -232,9 +232,9 @@ public:
}
Vec3f extent = (bv1.max_ - bv1.min_) * 0.5;
bv2.r = extent[id[2]];
bv2.l[0] = (extent[id[0]] - bv2.r) * 2;
bv2.l[1] = (extent[id[1]] - bv2.r) * 2;
bv2.radius = extent[id[2]];
bv2.length[0] = (extent[id[0]] - bv2.radius) * 2;
bv2.length[1] = (extent[id[1]] - bv2.radius) * 2;
const Matrix3f& R = tf1.getRotation();
bool left_hand = (id[0] == (id[1] + 1) % 3);
......
include/hpp/fcl/BV/BV_node.h
View file @ 03f773bd
......@@ -39,7 +39,7 @@
#ifndef HPP_FCL_BV_NODE_H
#define HPP_FCL_BV_NODE_H
#include <hpp/fcl/math/types.h>
#include <hpp/fcl/data_types.h>
#include <hpp/fcl/BV/BV.h>
#include <iostream>
......
include/hpp/fcl/BV/OBB.h
View file @ 03f773bd
......@@ -38,7 +38,7 @@
#ifndef HPP_FCL_OBB_H
#define HPP_FCL_OBB_H
#include <hpp/fcl/math/types.h>
#include <hpp/fcl/data_types.h>
namespace hpp
{
......@@ -60,9 +60,6 @@ public:
/// @brief Half dimensions of OBB
Vec3f extent;
/// @brief Check whether the OBB contains a point.
bool contain(const Vec3f& p) const;
......
include/hpp/fcl/BV/OBBRSS.h
View file @ 03f773bd
......@@ -59,10 +59,6 @@ public:
/// @brief RSS member, for distance
RSS rss;
/// @brief Check whether the OBBRSS contains a point
inline bool contain(const Vec3f& p) const
{
......@@ -151,11 +147,6 @@ public:
}
};
/// @brief Check collision between two OBBRSS, b1 is in configuration (R0, T0) and b2 is in indentity
inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2)
{
......
include/hpp/fcl/BV/RSS.h
View file @ 03f773bd
......@@ -39,7 +39,7 @@
#define HPP_FCL_RSS_H
#include <stdexcept>
#include <hpp/fcl/math/types.h>
#include <hpp/fcl/data_types.h>
#include <boost/math/constants/constants.hpp>
namespace hpp
......@@ -60,10 +60,10 @@ public:
Vec3f Tr;
/// @brief Side lengths of rectangle
FCL_REAL l[2];
FCL_REAL length[2];
/// @brief Radius of sphere summed with rectangle to form RSS
FCL_REAL r;
FCL_REAL radius;
/// @brief Check whether the RSS contains a point
......@@ -101,7 +101,7 @@ public:
/// @brief Size of the RSS (used in BV_Splitter to order two RSSs)
inline FCL_REAL size() const
{
return (std::sqrt(l[0] * l[0] + l[1] * l[1]) + 2 * r);
return (std::sqrt(length[0] * length[0] + length[1] * length[1]) + 2 * radius);
}
/// @brief The RSS center
......@@ -113,25 +113,25 @@ public:
/// @brief Width of the RSS
inline FCL_REAL width() const
{
return l[0] + 2 * r;
return length[0] + 2 * radius;
}
/// @brief Height of the RSS
inline FCL_REAL height() const
{
return l[1] + 2 * r;
return length[1] + 2 * radius;
}
/// @brief Depth of the RSS
inline FCL_REAL depth() const
{
return 2 * r;
return 2 * radius;
}
/// @brief Volume of the RSS
inline FCL_REAL volume() const
{
return (l[0] * l[1] * 2 * r + 4 * boost::math::constants::pi<FCL_REAL>() * r * r * r);
return (length[0] * length[1] * 2 * radius + 4 * boost::math::constants::pi<FCL_REAL>() * radius * radius * radius);
}
/// @brief Check collision between two RSS and return the overlap part.
......@@ -161,4 +161,4 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2,
} // namespace hpp
#endif
#endif
\ No newline at end of file
include/hpp/fcl/BV/kDOP.h
View file @ 03f773bd
......@@ -39,7 +39,7 @@
#define HPP_FCL_KDOP_H
#include <stdexcept>
#include <hpp/fcl/math/types.h>
#include <hpp/fcl/data_types.h>
namespace hpp
{
......
include/hpp/fcl/BVH/BVH_model.h
View file @ 03f773bd
......@@ -41,8 +41,8 @@
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/BVH/BVH_internal.h>
#include <hpp/fcl/BV/BV_node.h>
#include <hpp/fcl/BVH/BV_splitter.h>
#include <hpp/fcl/BVH/BV_fitter.h>
#include "../../src/BVH/BV_splitter.h"
#include "../../src/BVH/BV_fitter.h"
#include <vector>
#include <boost/shared_ptr.hpp>
#include <boost/noncopyable.hpp>
......
include/hpp/fcl/collision.h
View file @ 03f773bd
......@@ -39,7 +39,7 @@
#ifndef HPP_FCL_COLLISION_H
#define HPP_FCL_COLLISION_H
#include <hpp/fcl/math/types.h>
#include <hpp/fcl/data_types.h>
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/collision_data.h>
......
include/hpp/fcl/collision_data.h
View file @ 03f773bd
......@@ -41,7 +41,7 @@
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/math/types.h>
#include <hpp/fcl/data_types.h>
#include <vector>
#include <set>
#include <limits>
......
include/hpp/fcl/data_types.h
View file @ 03f773bd
......@@ -41,16 +41,37 @@
#include <cstddef>
#include <boost/cstdint.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
namespace hpp
{
#ifdef HPP_FCL_HAVE_OCTOMAP
#define OCTOMAP_VERSION_AT_LEAST(x,y,z) \
(OCTOMAP_MAJOR_VERSION > x || (OCTOMAP_MAJOR_VERSION >= x && \
(OCTOMAP_MINOR_VERSION > y || (OCTOMAP_MINOR_VERSION >= y && \
OCTOMAP_PATCH_VERSION >= z))))
#define OCTOMAP_VERSION_AT_MOST(x,y,z) \
(OCTOMAP_MAJOR_VERSION < x || (OCTOMAP_MAJOR_VERSION <= x && \
(OCTOMAP_MINOR_VERSION < y || (OCTOMAP_MINOR_VERSION <= y && \
OCTOMAP_PATCH_VERSION <= z))))
#endif // HPP_FCL_HAVE_OCTOMAP
}
namespace hpp
{
namespace fcl
{
typedef double FCL_REAL;
typedef boost::uint64_t FCL_INT64;
typedef boost::int64_t FCL_UINT64;
typedef boost::uint32_t FCL_UINT32;
typedef boost::int32_t FCL_INT32;
typedef Eigen::Matrix<FCL_REAL, 3, 1> Vec3f;
typedef Eigen::Matrix<FCL_REAL, 3, 3> Matrix3f;
/// @brief Triangle with 3 indices for points
class Triangle
......@@ -84,4 +105,5 @@ public:
} // namespace hpp
#endif
include/hpp/fcl/math/transform.h
View file @ 03f773bd
......@@ -39,7 +39,7 @@
#ifndef HPP_FCL_TRANSFORM_H
#define HPP_FCL_TRANSFORM_H
#include <hpp/fcl/math/types.h>
#include <hpp/fcl/data_types.h>
#include <boost/thread/mutex.hpp>
namespace hpp
......
include/hpp/fcl/math/types.h
View file @ 03f773bd
......@@ -38,50 +38,9 @@
#ifndef HPP_FCL_MATH_TYPES_H
#define HPP_FCL_MATH_TYPES_H
#include <hpp/fcl/data_types.h>
# warning "This file is deprecated. Include <hpp/fcl/data_types.h> instead."
#include <Eigen/Core>
#include <Eigen/Geometry>
namespace hpp
{
#ifdef HPP_FCL_HAVE_OCTOMAP
#define OCTOMAP_VERSION_AT_LEAST(x,y,z) \
(OCTOMAP_MAJOR_VERSION > x || (OCTOMAP_MAJOR_VERSION >= x && \
(OCTOMAP_MINOR_VERSION > y || (OCTOMAP_MINOR_VERSION >= y && \
OCTOMAP_PATCH_VERSION >= z))))
#define OCTOMAP_VERSION_AT_MOST(x,y,z) \
(OCTOMAP_MAJOR_VERSION < x || (OCTOMAP_MAJOR_VERSION <= x && \
(OCTOMAP_MINOR_VERSION < y || (OCTOMAP_MINOR_VERSION <= y && \
OCTOMAP_PATCH_VERSION <= z))))
#endif // HPP_FCL_HAVE_OCTOMAP
}
namespace hpp
{
namespace fcl
{
typedef Eigen::Matrix<FCL_REAL, 3, 1> Vec3f;
typedef Eigen::Matrix<FCL_REAL, 3, 3> Matrix3f;
/// @brief Class for variance matrix in 3d
class Variance3f
{
public:
/// @brief Variation matrix
Matrix3f Sigma;
/// @brief Variations along the eign axes
Matrix3f::Scalar sigma[3];
/// @brief Eigen axes of the variation matrix
Vec3f axis[3];
};
}
} // namespace hpp
// List of equivalent includes.
# include <hpp/fcl/data_types.h>
#endif
\ No newline at end of file
include/hpp/fcl/mesh_loader/loader.h
View file @ 03f773bd
......@@ -40,7 +40,7 @@
#include <boost/shared_ptr.hpp>
#include <hpp/fcl/fwd.hh>
#include <hpp/fcl/config.hh>
#include <hpp/fcl/math/types.h>
#include <hpp/fcl/data_types.h>
#include <hpp/fcl/collision_object.h>
namespace hpp
......
include/hpp/fcl/shape/geometric_shapes.h
View file @ 03f773bd
......@@ -42,7 +42,7 @@
#include <boost/math/constants/constants.hpp>
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/math/types.h>
#include <hpp/fcl/data_types.h>
#include <string.h>
namespace hpp
......@@ -151,12 +151,17 @@ public:
{
}
// Capsule::Capsule() : HalfLength(lz/2), lz(0){}
/// @brief Radius of capsule
FCL_REAL radius;
/// @brief Length along z axis
FCL_REAL lz;
/// @brief Half Length along z axis
FCL_REAL HalfLength;
/// @brief Compute AABB
void computeLocalAABB();
......@@ -191,12 +196,17 @@ public:
{
}
//Cone::Cone() : HalfLength(lz/2), lz(0){}
/// @brief Radius of the cone
FCL_REAL radius;
/// @brief Length along z axis
FCL_REAL lz;
/// @brief Half Length along z axis
FCL_REAL HalfLength;
/// @brief Compute AABB
void computeLocalAABB();
......@@ -233,6 +243,7 @@ public:
{
}
// Cylinder::Cylinder() : HalfLength(lz/2), lz(0){}
/// @brief Radius of the cylinder
FCL_REAL radius;
......@@ -240,6 +251,9 @@ public:
/// @brief Length along z axis
FCL_REAL lz;
/// @brief Half Length along z axis
FCL_REAL HalfLength;
/// @brief Compute AABB
void computeLocalAABB();
......
src/BV/RSS.cpp
View file @ 03f773bd
......@@ -844,8 +844,8 @@ bool RSS::overlap(const RSS& other) const
/// Now compute R1'R2
Matrix3f R (axes.transpose() * other.axes);
FCL_REAL dist = rectDistance(R, T, l, other.l);
return (dist <= (r + other.r));
FCL_REAL dist = rectDistance(R, T, length, other.length);
return (dist <= (radius + other.radius));
}
bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2)
......@@ -859,8 +859,8 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2)
Vec3f T(b1.axes.transpose() * Ttemp);
Matrix3f R(b1.axes.transpose() * R0 * b2.axes);
FCL_REAL dist = rectDistance(R, T, b1.l, b2.l);
return (dist <= (b1.r + b2.r));
FCL_REAL dist = rectDistance(R, T, b1.length, b2.length);
return (dist <= (b1.radius + b2.radius));
}
bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2,
......@@ -876,7 +876,7 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2,
Vec3f T(b1.axes.transpose() * Ttemp);
Matrix3f R(b1.axes.transpose() * R0 * b2.axes);
FCL_REAL dist = rectDistance(R, T, b1.l, b2.l) - b1.r - b2.r;
FCL_REAL dist = rectDistance(R, T, b1.length, b2.length) - b1.radius - b2.radius;
if (dist <= 0) return true;
sqrDistLowerBound = dist * dist;
return false;
......@@ -893,28 +893,28 @@ bool RSS::contain(const Vec3f& p) const
Vec3f proj(proj0, proj1, proj2);
/// projection is within the rectangle
if((proj0 < l[0]) && (proj0 > 0) && (proj1 < l[1]) && (proj1 > 0))
if((proj0 < length[0]) && (proj0 > 0) && (proj1 < length[1]) && (proj1 > 0))
{
return (abs_proj2 < r);
return (abs_proj2 < radius);
}
else if((proj0 < l[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > l[1])))
else if((proj0 < length[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > length[1])))
{
FCL_REAL y = (proj1 > 0) ? l[1] : 0;
FCL_REAL y = (proj1 > 0) ? length[1] : 0;
Vec3f v(proj0, y, 0);
return ((proj - v).squaredNorm() < r * r);
return ((proj - v).squaredNorm() < radius * radius);
}
else if((proj1 < l[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > l[0])))
else if((proj1 < length[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > length[0])))
{
FCL_REAL x = (proj0 > 0) ? l[0] : 0;
FCL_REAL x = (proj0 > 0) ? length[0] : 0;
Vec3f v(x, proj1, 0);
return ((proj - v).squaredNorm() < r * r);
return ((proj - v).squaredNorm() < radius * radius);
}
else
{
FCL_REAL x = (proj0 > 0) ? l[0] : 0;
FCL_REAL y = (proj1 > 0) ? l[1] : 0;
FCL_REAL x = (proj0 > 0) ? length[0] : 0;
FCL_REAL y = (proj1 > 0) ? length[1] : 0;
Vec3f v(x, y, 0);
return ((proj - v).squaredNorm() < r * r);
return ((proj - v).squaredNorm() < radius * radius);
}
}
......@@ -928,99 +928,99 @@ RSS& RSS::operator += (const Vec3f& p)
Vec3f proj(proj0, proj1, proj2);
// projection is within the rectangle
if((proj0 < l[0]) && (proj0 > 0) && (proj1 < l[1]) && (proj1 > 0))
if((proj0 < length[0]) && (proj0 > 0) && (proj1 < length[1]) && (proj1 > 0))
{
if(abs_proj2 < r)
if(abs_proj2 < radius)
; // do nothing
else
{
r = 0.5 * (r + abs_proj2); // enlarge the r
radius = 0.5 * (radius + abs_proj2); // enlarge the r
// change RSS origin position
if(proj2 > 0)
Tr[2] += 0.5 * (abs_proj2 - r);
Tr[2] += 0.5 * (abs_proj2 - radius);
else
Tr[2] -= 0.5 * (abs_proj2 - r);
Tr[2] -= 0.5 * (abs_proj2 - radius);
}
}
else if((proj0 < l[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > l[1])))
else if((proj0 < length[0]) && (proj0 > 0) && ((proj1 < 0) || (proj1 > length[1])))
{
FCL_REAL y = (proj1 > 0) ? l[1] : 0;
FCL_REAL y = (proj1 > 0) ? length[1] : 0;
Vec3f v(proj0, y, 0);
FCL_REAL new_r_sqr = (proj - v).squaredNorm();
if(new_r_sqr < r * r)
if(new_r_sqr < radius * radius)
; // do nothing
else
{
if(abs_proj2 < r)
if(abs_proj2 < radius)
{
FCL_REAL delta_y = - std::sqrt(r * r - proj2 * proj2) + fabs(proj1 - y);
l[1] += delta_y;
FCL_REAL delta_y = - std::sqrt(radius * radius - proj2 * proj2) + fabs(proj1 - y);
length[1] += delta_y;
if(proj1 < 0)
Tr[1] -= delta_y;
}
else
{
FCL_REAL delta_y = fabs(proj1 - y);
l[1] += delta_y;
length[1] += delta_y;
if(proj1 < 0)
Tr[1] -= delta_y;
if(proj2 > 0)
Tr[2] += 0.5 * (abs_proj2 - r);
Tr[2] += 0.5 * (abs_proj2 - radius);
else
Tr[2] -= 0.5 * (abs_proj2 - r);
Tr[2] -= 0.5 * (abs_proj2 - radius);
}
}
}
else if((proj1 < l[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > l[0])))
else if((proj1 < length[1]) && (proj1 > 0) && ((proj0 < 0) || (proj0 > length[0])))
{
FCL_REAL x = (proj0 > 0) ? l[0] : 0;
FCL_REAL x = (proj0 > 0) ? length[0] : 0;
Vec3f v(x, proj1, 0);
FCL_REAL new_r_sqr = (proj - v).squaredNorm();
if(new_r_sqr < r * r)
if(new_r_sqr < radius * radius)
; // do nothing
else
{
if(abs_proj2 < r)
if(abs_proj2 < radius)
{
FCL_REAL delta_x = - std::sqrt(r * r - proj2 * proj2) + fabs(proj0 - x);
l[0] += delta_x;
FCL_REAL delta_x = - std::sqrt(radius * radius - proj2 * proj2) + fabs(proj0 - x);
length[0] += delta_x;
if(proj0 < 0)
Tr[0] -= delta_x;
}
else
{
FCL_REAL delta_x = fabs(proj0 - x);
l[0] += delta_x;
length[0] += delta_x;
if(proj0 < 0)
Tr[0] -= delta_x;
if(proj2 > 0)
Tr[2] += 0.5 * (abs_proj2 - r);
Tr[2] += 0.5 * (abs_proj2 - radius);