Commit 62c66fbb authored by Joseph Mirabel's avatar Joseph Mirabel Committed by Joseph Mirabel
Browse files

Merge remote-tracking branch 'jmirabel/devel' into merge_jmirabel

Conflicts:
	CMakeLists.txt
	include/hpp/fcl/ccd/interval_matrix.h
	include/hpp/fcl/ccd/interval_vector.h
	include/hpp/fcl/ccd/motion.h
	include/hpp/fcl/ccd/taylor_matrix.h
	include/hpp/fcl/ccd/taylor_vector.h
	include/hpp/fcl/collision_data.h
	include/hpp/fcl/math/vec_3fx.h
	include/hpp/fcl/traversal/traversal_node_bvhs.h
	src/CMakeLists.txt
	src/ccd/interval_matrix.cpp
	src/ccd/motion.cpp
	src/ccd/taylor_matrix.cpp
	src/narrowphase/gjk_libccd.cpp
	src/traversal/traversal_node_bvhs.cpp
	test/CMakeLists.txt
	test/test_fcl_geometric_shapes.cpp
parents 0b8f544b 574a3a57
......@@ -46,15 +46,9 @@ set(PROJECT_URL "http://github.com/humanoid-path-planner/hpp-fcl")
setup_project()
set(FCL_HAVE_SSE FALSE CACHE BOOL "Enable SSE vectorization")
add_optional_dependency("eigen3 >= 3.0.0")
set(FCL_HAVE_EIGEN ${EIGEN3_FOUND} CACHE BOOL "Use eigen matrix type when possible")
if (EIGEN3_FOUND)
if (FCL_HAVE_EIGEN)
include_directories(${EIGEN3_INCLUDE_DIRS})
endif (FCL_HAVE_EIGEN)
endif (EIGEN3_FOUND)
add_required_dependency("eigen3 >= 3.0.0")
set(FCL_HAVE_EIGEN TRUE)
include_directories(${EIGEN3_INCLUDE_DIRS})
# Add a cache variable to allow not compiling and running tests
set (RUN_TESTS TRUE CACHE BOOL "compile and run unit tests")
......@@ -77,11 +71,6 @@ if (OCTOMAP_INCLUDE_DIRS AND OCTOMAP_LIBRARY_DIRS)
else()
message(STATUS "FCL does not use Octomap")
endif()
# flann package ill defined: comment.
# add_optional_dependency("flann >= 1.7")
# if (${FLANN_FOUND})
# add_definitions(-DFCL_HAVE_FLANN=1)
# endif()
ADD_REQUIRED_DEPENDENCY("assimp >= 2.0")
if(ASSIMP_FOUND)
if (NOT ${ASSIMP_VERSION} VERSION_LESS "2.0.1150")
......@@ -120,17 +109,15 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/broadphase/broadphase_bruteforce.h
include/hpp/fcl/broadphase/morton.h
include/hpp/fcl/broadphase/hash.h
include/hpp/fcl/learning/classifier.h
include/hpp/fcl/shape/geometric_shapes_utility.h
include/hpp/fcl/shape/geometric_shape_to_BVH_model.h
include/hpp/fcl/shape/geometric_shapes.h
include/hpp/fcl/simd/simd_intersect.h
include/hpp/fcl/simd/math_simd_details.h
include/hpp/fcl/distance_func_matrix.h
include/hpp/fcl/collision.h
include/hpp/fcl/collision_node.h
include/hpp/fcl/collision_func_matrix.h
include/hpp/fcl/distance.h
<<<<<<< HEAD
include/hpp/fcl/knn/greedy_kcenters.h
include/hpp/fcl/knn/nearest_neighbors_GNAT.h
include/hpp/fcl/knn/nearest_neighbors_flann.h
......@@ -138,12 +125,12 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/knn/nearest_neighbors_linear.h
include/hpp/fcl/knn/nearest_neighbors.h
include/hpp/fcl/math/vec_nf.h
=======
include/hpp/fcl/continuous_collision.h
>>>>>>> jmirabel/devel
include/hpp/fcl/math/matrix_3f.h
include/hpp/fcl/math/matrix_3fx.h
include/hpp/fcl/math/vec_3f.h
include/hpp/fcl/math/vec_3fx.h
include/hpp/fcl/math/sampling.h
include/hpp/fcl/math/math_details.h
include/hpp/fcl/math/tools.h
include/hpp/fcl/math/transform.h
include/hpp/fcl/traversal/traversal_node_shapes.h
include/hpp/fcl/traversal/traversal_node_setup.h
......@@ -153,11 +140,6 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/traversal/traversal_node_bvh_shape.h
include/hpp/fcl/traversal/traversal_node_base.h
include/hpp/fcl/data_types.h
include/hpp/fcl/articulated_model/model.h
include/hpp/fcl/articulated_model/joint_config.h
include/hpp/fcl/articulated_model/model_config.h
include/hpp/fcl/articulated_model/joint.h
include/hpp/fcl/articulated_model/link.h
include/hpp/fcl/BVH/BV_splitter.h
include/hpp/fcl/BVH/BVH_internal.h
include/hpp/fcl/BVH/BVH_model.h
......@@ -168,6 +150,7 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/collision_object.h
include/hpp/fcl/octree.h
include/hpp/fcl/fwd.hh
<<<<<<< HEAD
include/hpp/fcl/eigen/math_details.h
include/hpp/fcl/eigen/vec_3fx.h
include/hpp/fcl/eigen/product.h
......@@ -175,6 +158,8 @@ SET(${PROJECT_NAME}_HEADERS
include/hpp/fcl/eigen/plugins/ccd/interval-matrix.h
include/hpp/fcl/eigen/plugins/ccd/interval-vector.h
include/hpp/fcl/mesh_loader/assimp.h
=======
>>>>>>> jmirabel/devel
)
add_subdirectory(src)
......
# Find FLANN, a Fast Library for Approximate Nearest Neighbors
include(FindPackageHandleStandardArgs)
find_path(FLANN_INCLUDE_DIR flann.hpp PATH_SUFFIXES flann)
if (FLANN_INCLUDE_DIR)
file(READ "${FLANN_INCLUDE_DIR}/config.h" FLANN_CONFIG)
string(REGEX REPLACE ".*FLANN_VERSION_ \"([0-9.]+)\".*" "\\1" FLANN_VERSION ${FLANN_CONFIG})
if(NOT FLANN_VERSION VERSION_LESS flann_FIND_VERSION)
string(REGEX REPLACE "/flann$" "" FLANN_INCLUDE_DIRS ${FLANN_INCLUDE_DIR})
endif()
endif()
find_package_handle_standard_args(flann DEFAULT_MSG FLANN_INCLUDE_DIRS)
# this module was taken from http://trac.evemu.org/browser/trunk/cmake/FindTinyXML.cmake
# - Find TinyXML
# Find the native TinyXML includes and library
#
# TINYXML_FOUND - True if TinyXML found.
# TINYXML_INCLUDE_DIR - where to find tinyxml.h, etc.
# TINYXML_LIBRARIES - List of libraries when using TinyXML.
#
INCLUDE( "FindPackageHandleStandardArgs" )
FIND_PATH( TINYXML_INCLUDE_DIRS "tinyxml.h"
PATH_SUFFIXES "tinyxml" )
FIND_LIBRARY( TINYXML_LIBRARY_DIRS
NAMES "tinyxml"
PATH_SUFFIXES "tinyxml" )
# handle the QUIETLY and REQUIRED arguments and set TINYXML_FOUND to TRUE if
# all listed variables are TRUE
FIND_PACKAGE_HANDLE_STANDARD_ARGS( "TinyXML" DEFAULT_MSG TINYXML_INCLUDE_DIRS TINYXML_LIBRARY_DIRS )
......@@ -62,8 +62,8 @@ public:
}
/// @brief Creating an AABB with two endpoints a and b
AABB(const Vec3f& a, const Vec3f&b) : min_(min(a, b)),
max_(max(a, b))
AABB(const Vec3f& a, const Vec3f&b) : min_(a.cwiseMin(b)),
max_(a.cwiseMax(b))
{
}
......@@ -74,8 +74,8 @@ public:
}
/// @brief Creating an AABB contains three points
AABB(const Vec3f& a, const Vec3f& b, const Vec3f& c) : min_(min(min(a, b), c)),
max_(max(max(a, b), c))
AABB(const Vec3f& a, const Vec3f& b, const Vec3f& c) : min_(a.cwiseMin(b).cwiseMin(c)),
max_(a.cwiseMax(b).cwiseMax(c))
{
}
......@@ -124,8 +124,8 @@ public:
return false;
}
overlap_part.min_ = max(min_, other.min_);
overlap_part.max_ = min(max_, other.max_);
overlap_part.min_ = min_.cwiseMax(other.min_);
overlap_part.max_ = max_.cwiseMin(other.max_);
return true;
}
......@@ -143,16 +143,16 @@ public:
/// @brief Merge the AABB and a point
inline AABB& operator += (const Vec3f& p)
{
min_.ubound(p);
max_.lbound(p);
min_ = min_.cwiseMin(p);
max_ = max_.cwiseMax(p);
return *this;
}
/// @brief Merge the AABB and another AABB
inline AABB& operator += (const AABB& other)
{
min_.ubound(other.min_);
max_.lbound(other.max_);
min_ = min_.cwiseMin(other.min_);
max_ = max_.cwiseMax(other.max_);
return *this;
}
......@@ -190,13 +190,13 @@ public:
/// @brief Size of the AABB (used in BV_Splitter to order two AABBs)
inline FCL_REAL size() const
{
return (max_ - min_).sqrLength();
return (max_ - min_).squaredNorm();
}
/// @brief Radius of the AABB
inline FCL_REAL radius() const
{
return (max_ - min_).length() / 2;
return (max_ - min_).norm() / 2;
}
/// @brief Center of the AABB
......@@ -214,7 +214,7 @@ public:
/// @brief whether two AABB are equal
inline bool equal(const AABB& other) const
{
return min_.equal(other.min_) && max_.equal(other.max_);
return isEqual(min_, other.min_) && isEqual(max_, other.max_);
}
/// @brief expand the half size of the AABB by delta, and keep the center unchanged.
......
......@@ -75,7 +75,7 @@ public:
static void convert(const AABB& bv1, const Transform3f& tf1, AABB& bv2)
{
const Vec3f& center = bv1.center();
FCL_REAL r = (bv1.max_ - bv1.min_).length() * 0.5;
FCL_REAL r = (bv1.max_ - bv1.min_).norm() * 0.5;
Vec3f center2 = tf1.transform(center);
Vec3f delta(r, r, r);
bv2.min_ = center2 - delta;
......@@ -120,17 +120,14 @@ public:
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.getColumn(id[0]) : R.getColumn(id[0]);
bv2.axis[1] = R.getColumn(id[1]);
bv2.axis[2] = R.getColumn(id[2]);
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 = tf1.transform(bv1.center());
bv2.extent = (bv1.max_ - bv1.min_) * 0.5;
const Matrix3f& R = tf1.getRotation();
bv2.axis[0] = R.getColumn(0);
bv2.axis[1] = R.getColumn(1);
bv2.axis[2] = R.getColumn(2);
bv2.To.noalias() = tf1.transform(bv1.center());
bv2.extent.noalias() = (bv1.max_ - bv1.min_) * 0.5;
bv2.axes.noalias() = tf1.getRotation();
}
};
......@@ -140,11 +137,9 @@ class Converter<OBB, OBB>
public:
static void convert(const OBB& bv1, const Transform3f& tf1, OBB& bv2)
{
bv2.extent = bv1.extent;
bv2.To = tf1.transform(bv1.To);
bv2.axis[0] = tf1.getQuatRotation().transform(bv1.axis[0]);
bv2.axis[1] = tf1.getQuatRotation().transform(bv1.axis[1]);
bv2.axis[2] = tf1.getQuatRotation().transform(bv1.axis[2]);
bv2.extent.noalias() = bv1.extent;
bv2.To.noalias() = tf1.transform(bv1.To);
bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
}
};
......@@ -164,11 +159,9 @@ class Converter<RSS, OBB>
public:
static void convert(const RSS& bv1, const Transform3f& tf1, OBB& bv2)
{
bv2.extent = Vec3f(bv1.l[0] * 0.5 + bv1.r, bv1.l[1] * 0.5 + bv1.r, bv1.r);
bv2.To = tf1.transform(bv1.Tr);
bv2.axis[0] = tf1.getQuatRotation().transform(bv1.axis[0]);
bv2.axis[1] = tf1.getQuatRotation().transform(bv1.axis[1]);
bv2.axis[2] = tf1.getQuatRotation().transform(bv1.axis[2]);
bv2.extent.noalias() = Vec3f(bv1.l[0] * 0.5 + bv1.r, bv1.l[1] * 0.5 + bv1.r, bv1.r);
bv2.To.noalias() = tf1.transform(bv1.Tr);
bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
}
};
......@@ -180,7 +173,7 @@ public:
static void convert(const BV1& bv1, const Transform3f& tf1, AABB& bv2)
{
const Vec3f& center = bv1.center();
FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).length() * 0.5;
FCL_REAL r = Vec3f(bv1.width(), bv1.height(), bv1.depth()).norm() * 0.5;
Vec3f delta(r, r, r);
Vec3f center2 = tf1.transform(center);
bv2.min_ = center2 - delta;
......@@ -207,9 +200,7 @@ public:
static void convert(const OBB& bv1, const Transform3f& tf1, RSS& bv2)
{
bv2.Tr = tf1.transform(bv1.To);
bv2.axis[0] = tf1.getQuatRotation().transform(bv1.axis[0]);
bv2.axis[1] = tf1.getQuatRotation().transform(bv1.axis[1]);
bv2.axis[2] = tf1.getQuatRotation().transform(bv1.axis[2]);
bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
bv2.r = bv1.extent[2];
bv2.l[0] = 2 * (bv1.extent[0] - bv2.r);
......@@ -223,10 +214,8 @@ class Converter<RSS, RSS>
public:
static void convert(const RSS& bv1, const Transform3f& tf1, RSS& bv2)
{
bv2.Tr = tf1.transform(bv1.Tr);
bv2.axis[0] = tf1.getQuatRotation().transform(bv1.axis[0]);
bv2.axis[1] = tf1.getQuatRotation().transform(bv1.axis[1]);
bv2.axis[2] = tf1.getQuatRotation().transform(bv1.axis[2]);
bv2.Tr.noalias() = tf1.transform(bv1.Tr);
bv2.axes.noalias() = tf1.getRotation() * bv1.axes;
bv2.r = bv1.r;
bv2.l[0] = bv1.l[0];
......@@ -283,9 +272,10 @@ public:
const Matrix3f& R = tf1.getRotation();
bool left_hand = (id[0] == (id[1] + 1) % 3);
if (left_hand) bv2.axis[0] = -R.getColumn(id[0]); else bv2.axis[0] = R.getColumn(id[0]);
bv2.axis[1] = R.getColumn(id[1]);
bv2.axis[2] = R.getColumn(id[2]);
if (left_hand) bv2.axes.col(0).noalias() = -R.col(id[0]);
else bv2.axes.col(0).noalias() = R.col(id[0]);
bv2.axes.col(1).noalias() = R.col(id[1]);
bv2.axes.col(2).noalias() = R.col(id[2]);
}
};
......
......@@ -105,31 +105,25 @@ struct BVNode : public BVNodeBase
Vec3f getCenter() const { return bv.center(); }
/// @brief Access the orientation of the BV
Matrix3f getOrientation() const { return Matrix3f::getIdentity(); }
const Matrix3f& getOrientation() const { return Matrix3f::Identity(); }
};
template<>
inline Matrix3f BVNode<OBB>::getOrientation() const
inline const Matrix3f& BVNode<OBB>::getOrientation() const
{
return 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]);
return bv.axes;
}
template<>
inline Matrix3f BVNode<RSS>::getOrientation() const
inline const Matrix3f& BVNode<RSS>::getOrientation() const
{
return 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]);
return bv.axes;
}
template<>
inline Matrix3f BVNode<OBBRSS>::getOrientation() const
inline const Matrix3f& BVNode<OBBRSS>::getOrientation() const
{
return 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]);
return bv.obb.axes;
}
......
......@@ -51,7 +51,7 @@ class OBB
public:
/// @brief Orientation of OBB. axis[i] is the ith column of the orientation matrix for the box; it is also the i-th principle direction of the box.
/// We assume that axis[0] corresponds to the axis with the longest box edge, axis[1] corresponds to the shorter one and axis[2] corresponds to the shortest one.
Vec3f axis[3];
Matrix3f axes;
/// @brief Center of OBB
Vec3f To;
......@@ -119,7 +119,7 @@ public:
/// @brief Size of the OBB (used in BV_Splitter to order two OBBs)
inline FCL_REAL size() const
{
return extent.sqrLength();
return extent.squaredNorm();
}
/// @brief Center of the OBB
......
......@@ -52,7 +52,7 @@ class RSS
public:
/// @brief Orientation of RSS. axis[i] is the ith column of the orientation matrix for the RSS; it is also the i-th principle direction of the RSS.
/// We assume that axis[0] corresponds to the axis with the longest length, axis[1] corresponds to the shorter one and axis[2] corresponds to the shortest one.
Vec3f axis[3];
Matrix3f axes;
/// @brief Origin of the rectangle in RSS
Vec3f Tr;
......
......@@ -58,7 +58,7 @@ class kIOS
static kIOS_Sphere encloseSphere(const kIOS_Sphere& s0, const kIOS_Sphere& s1)
{
Vec3f d = s1.o - s0.o;
FCL_REAL dist2 = d.sqrLength();
FCL_REAL dist2 = d.squaredNorm();
FCL_REAL diff_r = s1.r - s0.r;
/** The sphere with the larger radius encloses the other */
......
......@@ -188,14 +188,14 @@ public:
/// BV node. When traversing the BVH, this can save one matrix transformation.
void makeParentRelative()
{
Vec3f I[3] = {Vec3f(1, 0, 0), Vec3f(0, 1, 0), Vec3f(0, 0, 1)};
Matrix3f I (Matrix3f::Identity());
makeParentRelativeRecurse(0, I, Vec3f());
}
Vec3f computeCOM() const
{
FCL_REAL vol = 0;
Vec3f com;
Vec3f com(0,0,0);
for(int i = 0; i < num_tris; ++i)
{
const Triangle& tri = tri_indices[i];
......@@ -222,13 +222,12 @@ public:
Matrix3f computeMomentofInertia() const
{
Matrix3f C(0, 0, 0,
0, 0, 0,
0, 0, 0);
Matrix3f C = Matrix3f::Zero();
Matrix3f C_canonical(1/60.0, 1/120.0, 1/120.0,
1/120.0, 1/60.0, 1/120.0,
1/120.0, 1/120.0, 1/60.0);
Matrix3f C_canonical;
C_canonical << 1/60.0, 1/120.0, 1/120.0,
1/120.0, 1/60.0, 1/120.0,
1/120.0, 1/120.0, 1/60.0;
for(int i = 0; i < num_tris; ++i)
{
......@@ -236,16 +235,11 @@ public:
const Vec3f& v1 = vertices[tri[0]];
const Vec3f& v2 = vertices[tri[1]];
const Vec3f& v3 = vertices[tri[2]];
FCL_REAL d_six_vol = (v1.cross(v2)).dot(v3);
Matrix3f A(v1, v2, v3);
C += transpose(A) * C_canonical * A * d_six_vol;
Matrix3f A; A << v1.transpose(), v2.transpose(), v3.transpose();
C += A.derived().transpose() * C_canonical * A * (v1.cross(v2)).dot(v3);
}
FCL_REAL trace_C = C(0, 0) + C(1, 1) + C(2, 2);
return Matrix3f(trace_C - C(0, 0), -C(0, 1), -C(0, 2),
-C(1, 0), trace_C - C(1, 1), -C(1, 2),
-C(2, 0), -C(2, 1), trace_C - C(2, 2));
return C.trace() * Matrix3f::Identity() - C;
}
public:
......@@ -309,13 +303,13 @@ private:
/// @recursively compute each bv's transform related to its parent. For default BV, only the translation works.
/// For oriented BV (OBB, RSS, OBBRSS), special implementation is provided.
void makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c)
void makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c)
{
if(!bvs[bv_id].isLeaf())
{
makeParentRelativeRecurse(bvs[bv_id].first_child, parent_axis, bvs[bv_id].getCenter());
makeParentRelativeRecurse(bvs[bv_id].first_child, parent_axes, bvs[bv_id].getCenter());
makeParentRelativeRecurse(bvs[bv_id].first_child + 1, parent_axis, bvs[bv_id].getCenter());
makeParentRelativeRecurse(bvs[bv_id].first_child + 1, parent_axes, bvs[bv_id].getCenter());
}
bvs[bv_id].bv = translate(bvs[bv_id].bv, -parent_c);
......@@ -324,13 +318,13 @@ private:
template<>
void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c);
void BVHModel<OBB>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c);
template<>
void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c);
void BVHModel<RSS>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c);
template<>
void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id, Vec3f parent_axis[], const Vec3f& parent_c);
void BVHModel<OBBRSS>::makeParentRelativeRecurse(int bv_id, Matrix3f& parent_axes, const Vec3f& parent_c);
/// @brief Specialization of getNodeType() for BVHModel with different BV types
......
......@@ -81,10 +81,10 @@ void BVHExpand(BVHModel<RSS>& model, const Variance3f* ucs, FCL_REAL r);
void getCovariance(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Matrix3f& M);
/// @brief Compute the RSS bounding volume parameters: radius, rectangle size and the origin, given the BV axises.
void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& origin, FCL_REAL l[2], FCL_REAL& r);
void getRadiusAndOriginAndRectangleSize(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, const Matrix3f& axes, Vec3f& origin, FCL_REAL l[2], FCL_REAL& r);
/// @brief Compute the bounding volume extent and center for a set or subset of points, given the BV axises.
void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Vec3f axis[3], Vec3f& center, Vec3f& extent);
void getExtentAndCenter(Vec3f* ps, Vec3f* ps2, Triangle* ts, unsigned int* indices, int n, Matrix3f& axes, Vec3f& center, Vec3f& extent);
/// @brief Compute the center and radius for a triangle's circumcircle
void circumCircleComputation(const Vec3f& a, const Vec3f& b, const Vec3f& c, Vec3f& center, FCL_REAL& radius);
......
......@@ -76,7 +76,7 @@ class BVSplitter : public BVSplitterBase<BV>
{
public:
BVSplitter(SplitMethodType method) : split_method(method)
BVSplitter(SplitMethodType method) : split_vector(0,0,0), split_method(method)
{
}
......
/*
* Software License Agreement (BSD License)
*
* Copyright (c) 2011-2014, Willow Garage, Inc.
* Copyright (c) 2014-2015, Open Source Robotics Foundation
* 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 Open Source Robotics Foundation 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 Dalibor Matura, Jia Pan */
#ifndef FCL_ARTICULATED_MODEL_JOINT_H
#define FCL_ARTICULATED_MODEL_JOINT_H
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/data_types.h>
#include <string>
#include <vector>
#include <map>
#include <limits>
#include <boost/shared_ptr.hpp>
#include <boost/weak_ptr.hpp>
namespace fcl
{
class JointConfig;
class Link;
enum JointType {JT_UNKNOWN, JT_PRISMATIC, JT_REVOLUTE, JT_BALLEULER};
/// @brief Base Joint
class Joint
{
public:
Joint(const boost::shared_ptr<Link>& link_parent, const boost::shared_ptr<Link>& link_child,
const Transform3f& transform_to_parent,
const std::string& name);
Joint(const std::string& name);
virtual ~Joint() {}
const std::string& getName() const;
void setName(const std::string& name);
virtual Transform3f getLocalTransform() const = 0;
virtual std::size_t getNumDofs() const = 0;
boost::shared_ptr<JointConfig> getJointConfig() const;
void setJointConfig(const boost::shared_ptr<JointConfig>& joint_cfg);
boost::shared_ptr<Link> getParentLink() const;
boost::shared_ptr<Link> getChildLink() const;
void setParentLink(const boost::shared_ptr<Link>& link);
void setChildLink(const boost::shared_ptr<Link>& link);