Commit 0c86f23e authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Merge remote-tracking branch 'origin/refactoring' into jmirabel/devel

parents c5bf1bf6 0441937a
Pipeline #5441 failed with stage
in 29 minutes and 7 seconds
......@@ -125,6 +125,7 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/distance.h
include/hpp/fcl/math/matrix_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/transform.h
include/hpp/fcl/traversal/details/traversal.h
......
......@@ -39,8 +39,7 @@
#define HPP_FCL_AABB_H
#include <stdexcept>
#include <hpp/fcl/math/vec_3f.h>
#include <hpp/fcl/math/matrix_3f.h>
#include <hpp/fcl/math/types.h>
namespace hpp
{
......@@ -100,23 +99,12 @@ public:
bool overlap(const AABB& other, const CollisionRequest& request,
FCL_REAL& sqrDistLowerBound) const;
/// @brief Check whether the AABB contains another AABB
/// @brief Check whether the AABB contains another AABB
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]);
}
/// @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
inline bool overlap(const AABB& other, AABB& overlap_part) const
{
......@@ -182,7 +170,7 @@ public:
return max_[2] - min_[2];
}
/// @brief Volume of the AABB
/// @brief Volume of the AABB
inline FCL_REAL volume() const
{
return width() * height() * depth();
......@@ -194,12 +182,6 @@ public:
return (max_ - min_).squaredNorm();
}
/// @brief Radius of the AABB
inline FCL_REAL radius() const
{
return (max_ - min_).norm() / 2;
}
/// @brief Center of the AABB
inline Vec3f center() const
{
......@@ -212,12 +194,6 @@ public:
/// @brief Distance between two AABBs
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.
inline AABB& expand(const Vec3f& delta)
{
......
......@@ -90,43 +90,7 @@ class Converter<AABB, OBB>
{
public:
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.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
bv2.axes.noalias() = tf1.getRotation();
......
......@@ -39,8 +39,7 @@
#ifndef HPP_FCL_BV_NODE_H
#define HPP_FCL_BV_NODE_H
#include <hpp/fcl/math/vec_3f.h>
#include <hpp/fcl/math/matrix_3f.h>
#include <hpp/fcl/math/types.h>
#include <hpp/fcl/BV/BV.h>
#include <iostream>
......
......@@ -38,9 +38,7 @@
#ifndef HPP_FCL_OBB_H
#define HPP_FCL_OBB_H
#include <hpp/fcl/math/vec_3f.h>
#include <hpp/fcl/math/matrix_3f.h>
#include <hpp/fcl/math/types.h>
namespace hpp
{
......@@ -73,13 +71,6 @@ public:
bool overlap(const OBB& other, const CollisionRequest& request,
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.
bool contain(const Vec3f& p) const;
......
......@@ -74,13 +74,7 @@ public:
return obb.overlap(other.obb, request, sqrDistLowerBound);
}
/// @brief Check collision between two OBBRSS and return the overlap part.
bool overlap(const OBBRSS& other, OBBRSS& /*overlap_part*/) const
{
return overlap(other);
}
/// @brief Check whether the OBBRSS contains a point
/// @brief Check whether the OBBRSS contains a point
inline bool contain(const Vec3f& p) const
{
return obb.contain(p);
......@@ -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
inline bool overlap(const Matrix3f& R0, const Vec3f& T0, const OBBRSS& b1, const OBBRSS& b2)
{
......
......@@ -39,8 +39,7 @@
#define HPP_FCL_RSS_H
#include <stdexcept>
#include <hpp/fcl/math/vec_3f.h>
#include <hpp/fcl/math/matrix_3f.h>
#include <hpp/fcl/math/types.h>
#include <boost/math/constants/constants.hpp>
namespace hpp
......@@ -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
/// 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)
......
......@@ -39,7 +39,7 @@
#define HPP_FCL_KDOP_H
#include <stdexcept>
#include <hpp/fcl/math/matrix_3f.h>
#include <hpp/fcl/math/types.h>
namespace hpp
{
......@@ -85,6 +85,10 @@ namespace fcl
template<size_t N>
class KDOP
{
private:
/// @brief Origin's distances to N KDOP planes
FCL_REAL dist_[N];
public:
/// @brief Creating kDOP containing nothing
......@@ -158,11 +162,6 @@ public:
/// @brief The distance between two KDOP<N>. Not implemented.
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
{
return dist_[i];
......
......@@ -100,14 +100,6 @@ public:
bool overlap(const kIOS& other, const CollisionRequest&,
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
inline bool contain(const Vec3f& p) const;
......
......@@ -58,6 +58,30 @@ class BVHModel : public CollisionGeometry
{
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
BVHModelType getModelType() const
{
......@@ -151,7 +175,6 @@ public:
/// @brief End BVH model construction, will build the bounding volume hierarchy
int endModel();
/// @brief Replace the geometry information of current frame (i.e. should have the same mesh topology with the previous frame)
int beginReplaceModel();
......@@ -167,7 +190,6 @@ public:
/// @brief End BVH model replacement, will also refit or rebuild the bounding volume hierarchy
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).
/// The current frame will be saved as the previous frame in prev_vertices.
int beginUpdateModel();
......@@ -187,7 +209,7 @@ public:
/// @brief Check the number of memory used
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.
void makeParentRelative()
{
......@@ -245,32 +267,6 @@ public:
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:
int num_tris_allocated;
......
......@@ -46,39 +46,6 @@ namespace hpp
{
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.
/// A triangle in collision with the AABB is considered inside.
template<typename BV>
......@@ -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
FCL_REAL maximumDistance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Vec3f& query);
}
} // namespace hpp
......
......@@ -39,7 +39,7 @@
#ifndef 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_data.h>
......
......@@ -41,7 +41,7 @@
#include <hpp/fcl/collision_object.h>
#include <hpp/fcl/math/vec_3f.h>
#include <hpp/fcl/math/types.h>
#include <vector>
#include <set>
#include <limits>
......
......@@ -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
template<typename NarrowPhaseSolver>
template<typename GJKSolver>
struct CollisionFunctionMatrix
{
/// @brief the uniform call interface for collision: for collision, we need know
......@@ -57,7 +57,7 @@ struct CollisionFunctionMatrix
/// 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);
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
CollisionFunc collision_matrix[NODE_COUNT][NODE_COUNT];
......
......@@ -240,11 +240,7 @@ public:
return t.getRotation();
}
/// @brief get quaternion rotation of the object
inline const Quaternion3f& getQuatRotation() const
{
return t.getQuatRotation();
}
/// @brief get object's transform
inline const Transform3f& getTransform() const
......@@ -264,11 +260,7 @@ public:
t.setTranslation(T);
}
/// @brief set object's quatenrion rotation
void setQuatRotation(const Quaternion3f& q)
{
t.setQuatRotation(q);
}
/// @brief set object's transform
void setTransform(const Matrix3f& R, const Vec3f& T)
......@@ -276,11 +268,7 @@ public:
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
void setTransform(const Transform3f& tf)
......
......@@ -47,14 +47,14 @@ namespace fcl
{
/// @brief distance matrix stores the functions for distance between different types of objects and provides a uniform call interface
template<typename NarrowPhaseSolver>
template<typename GJKSolver>
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,
typedef FCL_REAL (*DistanceFunc)(const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver* 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
......
......@@ -38,70 +38,9 @@
#ifndef HPP_FCL_MATRIX_3F_H
#define HPP_FCL_MATRIX_3F_H
#include <hpp/fcl/math/vec_3f.h>
# warning "This file is deprecated. Include <hpp/fcl/math/types.h> instead."
namespace hpp
{
namespace fcl
{
// List of equivalent includes.
# include <hpp/fcl/math/types.h>
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];
Variance3f() {}
Variance3f(const Matrix3f& S) : Sigma(S)
{
init();
}
/// @brief init the Variance
void init()
{
eigen(Sigma, sigma, axis);
}
/// @brief Compute the sqrt of Sigma matrix based on the eigen decomposition result, this is useful when the uncertainty matrix is initialized as a square variation matrix
Variance3f& sqrt()
{
for(std::size_t i = 0; i < 3; ++i)
{
if(sigma[i] < 0) sigma[i] = 0;
sigma[i] = std::sqrt(sigma[i]);
}
Sigma.setZero();
for(std::size_t i = 0; i < 3; ++i)
{
for(std::size_t j = 0; j < 3; ++j)
{
Sigma(i, j) += sigma[0] * axis[0][i] * axis[0][j];
Sigma(i, j) += sigma[1] * axis[1][i] * axis[1][j];
Sigma(i, j) += sigma[2] * axis[2][i] * axis[2][j];
}
}
return *this;
}
};
}
} // namespace hpp
#endif
#endif
\ No newline at end of file
......@@ -98,14 +98,6 @@ void generateCoordinateSystem(
}
/* ----- Start Matrices ------ */
template<typename Derived, typename OtherDerived>
void hat(const Eigen::MatrixBase<Derived>& mat, const Eigen::MatrixBase<OtherDerived>& vec)
{
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(Derived, 3, 3);
EIGEN_STATIC_ASSERT_MATRIX_SPECIFIC_SIZE(OtherDerived, 3, 1);
const_cast< Eigen::MatrixBase<Derived>& >(mat) << 0, -vec[2], vec[1], vec[2], 0, -vec[0], -vec[1], vec[0], 0;
}
template<typename Derived, typename OtherDerived>
void relativeTransform(const Eigen::MatrixBase<Derived>& R1, const Eigen::MatrixBase<OtherDerived>& t1,
const Eigen::MatrixBase<Derived>& R2, const Eigen::MatrixBase<OtherDerived>& t2,
......@@ -205,38 +197,13 @@ void eigen(const Eigen::MatrixBase<Derived>& m, typename Derived::Scalar dout[3]
return;
}
template<typename Derived, typename OtherDerived>
typename Derived::Scalar quadraticForm(const Eigen::MatrixBase<Derived>& R, const Eigen::MatrixBase<OtherDerived>& v)
{
return v.dot(R * v);
}
template<typename Derived, typename OtherDerived>
bool isEqual(const Eigen::MatrixBase<Derived>& lhs, const Eigen::MatrixBase<OtherDerived>& rhs, const FCL_REAL tol = std::numeric_limits<FCL_REAL>::epsilon()*100)
{
return ((lhs - rhs).array().abs() < tol).all();
}
template <typename Derived>
inline Derived& setEulerZYX(const Eigen::MatrixBase<Derived>& R, FCL_REAL eulerX, FCL_REAL eulerY, FCL_REAL eulerZ)
{
const_cast< Eigen::MatrixBase<Derived>& >(R).noalias() = (
Eigen::AngleAxisd (eulerZ, Eigen::Vector3d::UnitZ()) *
Eigen::AngleAxisd (eulerY, Eigen::Vector3d::UnitY()) *
Eigen::AngleAxisd (eulerX, Eigen::Vector3d::UnitX())
).toRotationMatrix();
return const_cast< Eigen::MatrixBase<Derived>& >(R).derived();
}
template <typename Derived>
inline Derived& setEulerYPR(const Eigen::MatrixBase<Derived>& R, FCL_REAL yaw, FCL_REAL pitch, FCL_REAL roll)
{
return setEulerZYX(R, roll, pitch, yaw);
}
}
} // namespace hpp
#endif
#endif
\ No newline at end of file
......@@ -39,7 +39,7 @@
#ifndef HPP_FCL_TRANSFORM_H
#define HPP_FCL_TRANSFORM_H