diff --git a/CMakeLists.txt b/CMakeLists.txt
index 6fdcca6bbeba4a671815c115860fcb2fd11efc4f..27ed88487bc5802f030782b5eda741802ce2eb21 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -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
diff --git a/include/hpp/fcl/BV/AABB.h b/include/hpp/fcl/BV/AABB.h
index 05859d25b1f359ebe000ecfa9d3ac9768af4e932..72954a64436a08f7d2bf41a19e7d6b3ba6664a67 100644
--- a/include/hpp/fcl/BV/AABB.h
+++ b/include/hpp/fcl/BV/AABB.h
@@ -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
diff --git a/include/hpp/fcl/BV/BV.h b/include/hpp/fcl/BV/BV.h
index bb42f7fbcd6999b6d8c1bdbd3e4d7bcc834728da..3f03df66c96bf4e68105ecf237e00bc1aff3b66a 100644
--- a/include/hpp/fcl/BV/BV.h
+++ b/include/hpp/fcl/BV/BV.h
@@ -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);
diff --git a/include/hpp/fcl/BV/BV_node.h b/include/hpp/fcl/BV/BV_node.h
index 08dbbf8bd1ba82f1f0ed1208d7e9bcbc277c1912..e7fc637418af63b040efc47c7ff1427c77dda8ed 100644
--- a/include/hpp/fcl/BV/BV_node.h
+++ b/include/hpp/fcl/BV/BV_node.h
@@ -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>
diff --git a/include/hpp/fcl/BV/OBB.h b/include/hpp/fcl/BV/OBB.h
index 3b1cb3f5dc09785c116e233256760c6234a50cbb..5dee5cb1824ab8dd648b07fd13a6062523c884af 100644
--- a/include/hpp/fcl/BV/OBB.h
+++ b/include/hpp/fcl/BV/OBB.h
@@ -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;
diff --git a/include/hpp/fcl/BV/OBBRSS.h b/include/hpp/fcl/BV/OBBRSS.h
index cb6ed77bd9307edb6b5173546feff655c6e0b516..36c97e6143d73c530f31e31f124ebdf7b27d6277 100644
--- a/include/hpp/fcl/BV/OBBRSS.h
+++ b/include/hpp/fcl/BV/OBBRSS.h
@@ -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)
{
diff --git a/include/hpp/fcl/BV/RSS.h b/include/hpp/fcl/BV/RSS.h
index e158d5cc7fd80cb7a482d4b579959101f9652cb7..b626e27edf8561a1c14211d2bffbc9be73d1a62c 100644
--- a/include/hpp/fcl/BV/RSS.h
+++ b/include/hpp/fcl/BV/RSS.h
@@ -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
diff --git a/include/hpp/fcl/BV/kDOP.h b/include/hpp/fcl/BV/kDOP.h
index 00af0f813c477e9780c23c49be5b0061bd6062ab..61e43d90244dec4fab97de79c62992af18364ee6 100644
--- a/include/hpp/fcl/BV/kDOP.h
+++ b/include/hpp/fcl/BV/kDOP.h
@@ -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
{
diff --git a/include/hpp/fcl/BVH/BVH_model.h b/include/hpp/fcl/BVH/BVH_model.h
index 7d7f00c8f041dd63d2d6b91352e84eccb4559cf4..7612f9f8969d9057fdbd24e38dce5079dbefc212 100644
--- a/include/hpp/fcl/BVH/BVH_model.h
+++ b/include/hpp/fcl/BVH/BVH_model.h
@@ -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>
diff --git a/include/hpp/fcl/collision.h b/include/hpp/fcl/collision.h
index 700e3234233199340514c8e2a90de0d5396ad8b8..cc68c88bd9371631ba1ca93d210d9a92be494da0 100644
--- a/include/hpp/fcl/collision.h
+++ b/include/hpp/fcl/collision.h
@@ -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>
diff --git a/include/hpp/fcl/collision_data.h b/include/hpp/fcl/collision_data.h
index ded180506448b294d203d18664626df094d95b79..d6278c2e07acd5e531eb9611e83834b0395922c9 100644
--- a/include/hpp/fcl/collision_data.h
+++ b/include/hpp/fcl/collision_data.h
@@ -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>
diff --git a/include/hpp/fcl/data_types.h b/include/hpp/fcl/data_types.h
index fb234d919bacb5ba90718929719462f91be494c3..bec19192f909623e19b5cd9972d840edf320384f 100644
--- a/include/hpp/fcl/data_types.h
+++ b/include/hpp/fcl/data_types.h
@@ -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
diff --git a/include/hpp/fcl/math/transform.h b/include/hpp/fcl/math/transform.h
index c51d50576376e574a239612191a8714dc1565fdd..7abdb7e7d6848f536e48f1c4e4c9fa10abe163cb 100644
--- a/include/hpp/fcl/math/transform.h
+++ b/include/hpp/fcl/math/transform.h
@@ -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
diff --git a/include/hpp/fcl/math/types.h b/include/hpp/fcl/math/types.h
index f6788e46aa0dd0df8dcfba057d3201c74ceb01a8..ec431e7a92b59d788dc4582b7b84d25e3815696f 100644
--- a/include/hpp/fcl/math/types.h
+++ b/include/hpp/fcl/math/types.h
@@ -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
diff --git a/include/hpp/fcl/mesh_loader/loader.h b/include/hpp/fcl/mesh_loader/loader.h
index 4739b753d9d3c4d71011693b2e52633c6fa010cc..43d4c703074860861063b97f9fe1bb708faf10a0 100644
--- a/include/hpp/fcl/mesh_loader/loader.h
+++ b/include/hpp/fcl/mesh_loader/loader.h
@@ -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
diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h
index 7d00796f5b3b2861c9cb6f07c099cf2de0cb979d..fa0f2e4a9d2b3ef609bf4b36e2885d8d171a10c3 100644
--- a/include/hpp/fcl/shape/geometric_shapes.h
+++ b/include/hpp/fcl/shape/geometric_shapes.h
@@ -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();
diff --git a/src/BV/RSS.cpp b/src/BV/RSS.cpp
index ff3563cc29bf3ab591cbcdd65d5bfedd508a56d8..ccd4ba8734e6160860d8f7b97387ac957c6d711e 100644
--- a/src/BV/RSS.cpp
+++ b/src/BV/RSS.cpp
@@ -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);
else
- Tr[2] -= 0.5 * (abs_proj2 - r);
+ Tr[2] -= 0.5 * (abs_proj2 - 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);
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 diag = std::sqrt(new_r_sqr - proj2 * proj2);
- FCL_REAL delta_diag = - std::sqrt(r * r - proj2 * proj2) + diag;
+ FCL_REAL delta_diag = - std::sqrt(radius * radius - proj2 * proj2) + diag;
FCL_REAL delta_x = delta_diag / diag * fabs(proj0 - x);
FCL_REAL delta_y = delta_diag / diag * fabs(proj1 - y);
- l[0] += delta_x;
- l[1] += delta_y;
+ length[0] += delta_x;
+ length[1] += delta_y;
if(proj0 < 0 && proj1 < 0)
{
@@ -1033,8 +1033,8 @@ RSS& RSS::operator += (const Vec3f& p)
FCL_REAL delta_x = fabs(proj0 - x);
FCL_REAL delta_y = fabs(proj1 - y);
- l[0] += delta_x;
- l[1] += delta_y;
+ length[0] += delta_x;
+ length[1] += delta_y;
if(proj0 < 0 && proj1 < 0)
{
@@ -1043,9 +1043,9 @@ RSS& RSS::operator += (const Vec3f& p)
}
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);
}
}
}
@@ -1058,12 +1058,12 @@ RSS RSS::operator + (const RSS& other) const
RSS bv;
Vec3f v[16];
- Vec3f d0_pos (other.axes.col(0) * (other.l[0] + other.r));
- Vec3f d1_pos (other.axes.col(1) * (other.l[1] + other.r));
- Vec3f d0_neg (other.axes.col(0) * (-other.r));
- Vec3f d1_neg (other.axes.col(1) * (-other.r));
- Vec3f d2_pos (other.axes.col(2) * other.r);
- Vec3f d2_neg (other.axes.col(2) * (-other.r));
+ Vec3f d0_pos (other.axes.col(0) * (other.length[0] + other.radius));
+ Vec3f d1_pos (other.axes.col(1) * (other.length[1] + other.radius));
+ Vec3f d0_neg (other.axes.col(0) * (-other.radius));
+ Vec3f d1_neg (other.axes.col(1) * (-other.radius));
+ Vec3f d2_pos (other.axes.col(2) * other.radius);
+ Vec3f d2_neg (other.axes.col(2) * (-other.radius));
v[0].noalias() = other.Tr + d0_pos + d1_pos + d2_pos;
v[1].noalias() = other.Tr + d0_pos + d1_pos + d2_neg;
@@ -1074,12 +1074,12 @@ RSS RSS::operator + (const RSS& other) const
v[6].noalias() = other.Tr + d0_neg + d1_neg + d2_pos;
v[7].noalias() = other.Tr + d0_neg + d1_neg + d2_neg;
- d0_pos.noalias() = axes.col(0) * (l[0] + r);
- d1_pos.noalias() = axes.col(1) * (l[1] + r);
- d0_neg.noalias() = axes.col(0) * (-r);
- d1_neg.noalias() = axes.col(1) * (-r);
- d2_pos.noalias() = axes.col(2) * r;
- d2_neg.noalias() = axes.col(2) * (-r);
+ d0_pos.noalias() = axes.col(0) * (length[0] + radius);
+ d1_pos.noalias() = axes.col(1) * (length[1] + radius);
+ d0_neg.noalias() = axes.col(0) * (-radius);
+ d1_neg.noalias() = axes.col(1) * (-radius);
+ d2_pos.noalias() = axes.col(2) * radius;
+ d2_neg.noalias() = axes.col(2) * (-radius);
v[ 8].noalias() = Tr + d0_pos + d1_pos + d2_pos;
v[ 9].noalias() = Tr + d0_pos + d1_pos + d2_neg;
@@ -1113,7 +1113,7 @@ RSS RSS::operator + (const RSS& other) const
E[0][max]*E[1][mid] - E[0][mid]*E[1][max];
// set rss origin, rectangle size and radius
- getRadiusAndOriginAndRectangleSize(v, NULL, NULL, NULL, 16, bv.axes, bv.Tr, bv.l, bv.r);
+ getRadiusAndOriginAndRectangleSize(v, NULL, NULL, NULL, 16, bv.axes, bv.Tr, bv.length, bv.radius);
return bv;
}
@@ -1126,8 +1126,8 @@ FCL_REAL RSS::distance(const RSS& other, Vec3f* P, Vec3f* Q) const
Matrix3f R (axes.transpose() * other.axes);
Vec3f T (axes.transpose() * (other.Tr - Tr));
- FCL_REAL dist = rectDistance(R, T, l, other.l, P, Q);
- dist -= (r + other.r);
+ FCL_REAL dist = rectDistance(R, T, length, other.length, P, Q);
+ dist -= (radius + other.radius);
return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist;
}
@@ -1138,8 +1138,8 @@ FCL_REAL distance(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS&
Vec3f T(b1.axes.transpose() * Ttemp);
- FCL_REAL dist = rectDistance(R, T, b1.l, b2.l, P, Q);
- dist -= (b1.r + b2.r);
+ FCL_REAL dist = rectDistance(R, T, b1.length, b2.length, P, Q);
+ dist -= (b1.radius + b2.radius);
return (dist < (FCL_REAL)0.0) ? (FCL_REAL)0.0 : dist;
}
diff --git a/src/BVH/BVH_utility.cpp b/src/BVH/BVH_utility.cpp
index ae7b05425f73e09e4d95e167099ef2793d0511a8..e846635e507d51e2848bd9a3eb75a97d642ff2d4 100644
--- a/src/BVH/BVH_utility.cpp
+++ b/src/BVH/BVH_utility.cpp
@@ -45,68 +45,6 @@ namespace hpp
namespace fcl
{
-void BVHExpand(BVHModel<OBB>& model, const Variance3f* ucs, FCL_REAL r = 1.0)
-{
- for(int i = 0; i < model.getNumBVs(); ++i)
- {
- BVNode<OBB>& bvnode = model.getBV(i);
-
- Vec3f* vs = new Vec3f[bvnode.num_primitives * 6];
-
- 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)
- {
- vs[6 * j + 2 * k] = v + uc.axis[k] * (r * uc.sigma[k]);
- vs[6 * j + 2 * k + 1] = v - uc.axis[k] * (r * uc.sigma[k]);
- }
- }
-
- OBB bv;
- fit(vs, bvnode.num_primitives * 6, bv);
-
- delete [] vs;
-
- bvnode.bv = bv;
- }
-}
-
-void BVHExpand(BVHModel<RSS>& model, const Variance3f* ucs, FCL_REAL r = 1.0)
-{
- for(int i = 0; i < model.getNumBVs(); ++i)
- {
- BVNode<RSS>& bvnode = model.getBV(i);
-
- Vec3f* vs = new Vec3f[bvnode.num_primitives * 6];
-
- 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)
- {
- vs[6 * j + 2 * k] = v + uc.axis[k] * (r * uc.sigma[k]);
- vs[6 * j + 2 * k + 1] = v - uc.axis[k] * (r * uc.sigma[k]);
- }
- }
-
- RSS bv;
- fit(vs, bvnode.num_primitives * 6, bv);
-
- delete [] vs;
-
- bvnode.bv = bv;
- }
-}
-
template<typename BV>
BVHModel<BV>* BVHExtract(const BVHModel<BV>& model, const Transform3f& pose, const AABB& _aabb)
{
diff --git a/src/BVH/BV_fitter.cpp b/src/BVH/BV_fitter.cpp
index d04481fb84f451b065881f26c5a4d9060fc83249..a9179268a779fa239db1431e8b09ac4fcabd8315 100644
--- a/src/BVH/BV_fitter.cpp
+++ b/src/BVH/BV_fitter.cpp
@@ -35,7 +35,7 @@
/** \author Jia Pan */
-#include <hpp/fcl/BVH/BV_fitter.h>
+#include "BV_fitter.h"
#include <hpp/fcl/BVH/BVH_utility.h>
#include <limits>
#include "../math/tools.h"
@@ -150,9 +150,9 @@ void fit1(Vec3f* ps, RSS& bv)
{
bv.Tr.noalias() = ps[0];
bv.axes.setIdentity();
- bv.l[0] = 0;
- bv.l[1] = 0;
- bv.r = 0;
+ bv.length[0] = 0;
+ bv.length[1] = 0;
+ bv.radius = 0;
}
void fit2(Vec3f* ps, RSS& bv)
@@ -164,11 +164,11 @@ void fit2(Vec3f* ps, RSS& bv)
bv.axes.col(0) /= len_p1p2;
generateCoordinateSystem(bv.axes.col(0), bv.axes.col(1), bv.axes.col(2));
- bv.l[0] = len_p1p2;
- bv.l[1] = 0;
+ bv.length[0] = len_p1p2;
+ bv.length[1] = 0;
bv.Tr = p2;
- bv.r = 0;
+ bv.radius = 0;
}
void fit3(Vec3f* ps, RSS& bv)
@@ -193,7 +193,7 @@ void fit3(Vec3f* ps, RSS& bv)
bv.axes.col(0).noalias() = e[imax].normalized();
bv.axes.col(1).noalias() = bv.axes.col(2).cross(bv.axes.col(0));
- getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, 3, bv.axes, bv.Tr, bv.l, bv.r);
+ getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, 3, bv.axes, bv.Tr, bv.length, bv.radius);
}
void fit6(Vec3f* ps, RSS& bv)
@@ -215,7 +215,7 @@ void fitn(Vec3f* ps, int n, RSS& bv)
axisFromEigen(E, s, bv.axes);
// set rss origin, rectangle size and radius
- getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, n, bv.axes, bv.Tr, bv.l, bv.r);
+ getRadiusAndOriginAndRectangleSize(ps, NULL, NULL, NULL, n, bv.axes, bv.Tr, bv.length, bv.radius);
}
}
@@ -542,9 +542,9 @@ OBBRSS BVFitter<OBBRSS>::fit(unsigned int* primitive_indices, int num_primitives
getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.rss.axes, origin, l, r);
bv.rss.Tr = origin;
- bv.rss.l[0] = l[0];
- bv.rss.l[1] = l[1];
- bv.rss.r = r;
+ bv.rss.length[0] = l[0];
+ bv.rss.length[1] = l[1];
+ bv.rss.radius = r;
return bv;
}
@@ -568,9 +568,9 @@ RSS BVFitter<RSS>::fit(unsigned int* primitive_indices, int num_primitives)
getRadiusAndOriginAndRectangleSize(vertices, prev_vertices, tri_indices, primitive_indices, num_primitives, bv.axes, origin, l, r);
bv.Tr = origin;
- bv.l[0] = l[0];
- bv.l[1] = l[1];
- bv.r = r;
+ bv.length[0] = l[0];
+ bv.length[1] = l[1];
+ bv.radius = r;
return bv;
diff --git a/include/hpp/fcl/BVH/BV_fitter.h b/src/BVH/BV_fitter.h
similarity index 100%
rename from include/hpp/fcl/BVH/BV_fitter.h
rename to src/BVH/BV_fitter.h
diff --git a/src/BVH/BV_splitter.cpp b/src/BVH/BV_splitter.cpp
index 1674cef4a88b52ba85bb5c64f166efa20063f112..c709ed63862df7d09469cf063258de1f2a798669 100644
--- a/src/BVH/BV_splitter.cpp
+++ b/src/BVH/BV_splitter.cpp
@@ -35,7 +35,7 @@
/** \author Jia Pan */
-#include <hpp/fcl/BVH/BV_splitter.h>
+#include "BV_splitter.h"
namespace hpp
{
diff --git a/include/hpp/fcl/BVH/BV_splitter.h b/src/BVH/BV_splitter.h
similarity index 100%
rename from include/hpp/fcl/BVH/BV_splitter.h
rename to src/BVH/BV_splitter.h
diff --git a/src/intersect.cpp b/src/intersect.cpp
index 15df43fc3432f1245e75ac21d84e5291b4bd72fa..21c8c4b6851b4e0ecfc78ff945f0286014e040fd 100644
--- a/src/intersect.cpp
+++ b/src/intersect.cpp
@@ -35,7 +35,7 @@
/** \author Jia Pan */
-#include <hpp/fcl/intersect.h>
+#include "intersect.h"
#include <iostream>
#include <limits>
#include <vector>
@@ -46,134 +46,6 @@ namespace hpp
{
namespace fcl
{
-const FCL_REAL PolySolver::NEAR_ZERO_THRESHOLD = 1e-9;
-
-
-bool PolySolver::isZero(FCL_REAL v)
-{
- return (v < NEAR_ZERO_THRESHOLD) && (v > -NEAR_ZERO_THRESHOLD);
-}
-
-bool PolySolver::cbrt(FCL_REAL v)
-{
- return powf((float) v, (float) (1.0 / 3.0));
-}
-
-int PolySolver::solveLinear(FCL_REAL c[2], FCL_REAL s[1])
-{
- if(isZero(c[1]))
- return 0;
- s[0] = - c[0] / c[1];
- return 1;
-}
-
-int PolySolver::solveQuadric(FCL_REAL c[3], FCL_REAL s[2])
-{
- FCL_REAL p, q, D;
-
- // make sure we have a d2 equation
-
- if(isZero(c[2]))
- return solveLinear(c, s);
-
- // normal for: x^2 + px + q
- p = c[1] / (2.0 * c[2]);
- q = c[0] / c[2];
- D = p * p - q;
-
- if(isZero(D))
- {
- // one FCL_REAL root
- s[0] = s[1] = -p;
- return 1;
- }
-
- if(D < 0.0)
- // no real root
- return 0;
- else
- {
- // two real roots
- FCL_REAL sqrt_D = sqrt(D);
- s[0] = sqrt_D - p;
- s[1] = -sqrt_D - p;
- return 2;
- }
-}
-
-int PolySolver::solveCubic(FCL_REAL c[4], FCL_REAL s[3])
-{
- int i, num;
- FCL_REAL sub, A, B, C, sq_A, p, q, cb_p, D;
- const FCL_REAL ONE_OVER_THREE = 1 / 3.0;
- const FCL_REAL PI = 3.14159265358979323846;
-
- // make sure we have a d2 equation
- if(isZero(c[3]))
- return solveQuadric(c, s);
-
- // normalize the equation:x ^ 3 + Ax ^ 2 + Bx + C = 0
- A = c[2] / c[3];
- B = c[1] / c[3];
- C = c[0] / c[3];
-
- // substitute x = y - A / 3 to eliminate the quadratic term: x^3 + px + q = 0
- sq_A = A * A;
- p = (-ONE_OVER_THREE * sq_A + B) * ONE_OVER_THREE;
- q = 0.5 * (2.0 / 27.0 * A * sq_A - ONE_OVER_THREE * A * B + C);
-
- // use Cardano's formula
- cb_p = p * p * p;
- D = q * q + cb_p;
-
- if(isZero(D))
- {
- if(isZero(q))
- {
- // one triple solution
- s[0] = 0.0;
- num = 1;
- }
- else
- {
- // one single and one FCL_REAL solution
- FCL_REAL u = cbrt(-q);
- s[0] = 2.0 * u;
- s[1] = -u;
- num = 2;
- }
- }
- else
- {
- if(D < 0.0)
- {
- // three real solutions
- FCL_REAL phi = ONE_OVER_THREE * acos(-q / sqrt(-cb_p));
- FCL_REAL t = 2.0 * sqrt(-p);
- s[0] = t * cos(phi);
- s[1] = -t * cos(phi + PI / 3.0);
- s[2] = -t * cos(phi - PI / 3.0);
- num = 3;
- }
- else
- {
- // one real solution
- FCL_REAL sqrt_D = sqrt(D);
- FCL_REAL u = cbrt(sqrt_D + fabs(q));
- if(q > 0.0)
- s[0] = - u + p / u ;
- else
- s[0] = u - p / u;
- num = 1;
- }
- }
-
- // re-substitute
- sub = ONE_OVER_THREE * A;
- for(i = 0; i < num; i++)
- s[i] -= sub;
- return num;
-}
bool Intersect::buildTrianglePlane
(const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t)
diff --git a/include/hpp/fcl/intersect.h b/src/intersect.h
similarity index 89%
rename from include/hpp/fcl/intersect.h
rename to src/intersect.h
index b56a6c626389fd6628ea5ef31b79a9f9a9fff38b..fd8dfd559040d88bde32a6871dc1a385554b8a99 100644
--- a/include/hpp/fcl/intersect.h
+++ b/src/intersect.h
@@ -41,42 +41,19 @@
#include <hpp/fcl/math/transform.h>
#include <boost/math/special_functions/erf.hpp>
-namespace hpp
-{
+namespace hpp
+{
namespace fcl
{
-/// @brief A class solves polynomial degree (1,2,3) equations
-class PolySolver
+/// @brief CCD intersect kernel among primitives
+class Intersect
{
public:
- /// @brief Solve a linear equation with coefficients c, return roots s and number of roots
- static int solveLinear(FCL_REAL c[2], FCL_REAL s[1]);
-
- /// @brief Solve a quadratic function with coefficients c, return roots s and number of roots
- static int solveQuadric(FCL_REAL c[3], FCL_REAL s[2]);
-
- /// @brief Solve a cubic function with coefficients c, return roots s and number of roots
- static int solveCubic(FCL_REAL c[4], FCL_REAL s[3]);
-
-private:
- /// @brief Check whether v is zero
- static inline bool isZero(FCL_REAL v);
+ static bool buildTrianglePlane
+ (const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t);
+}; // class Intersect
- /// @brief Compute v^{1/3}
- static inline bool cbrt(FCL_REAL v);
-
- static const FCL_REAL NEAR_ZERO_THRESHOLD;
-};
-
-/// @brief CCD intersect kernel among primitives
-class Intersect
-{
-public:
- static bool buildTrianglePlane
- (const Vec3f& v1, const Vec3f& v2, const Vec3f& v3, Vec3f* n, FCL_REAL* t);
-}; // class Intersect
-
/// @brief Project functions
class Project
{
@@ -213,6 +190,6 @@ public:
}
-} // namespace hpp
-
+} // namespace hpp
+
#endif
diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp
index 4d167e985d0dec64ad339a7422444d9a0f5dff59..c7bd666eb769afeb77c8a6dd4148c2b097f14ca9 100644
--- a/src/narrowphase/gjk.cpp
+++ b/src/narrowphase/gjk.cpp
@@ -36,7 +36,7 @@
/** \author Jia Pan */
#include <hpp/fcl/narrowphase/gjk.h>
-#include <hpp/fcl/intersect.h>
+#include "../intersect.h"
#include "../math/tools.h"
namespace hpp
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index 619c77c95f2c458e191e5b9c00ed9770815a0726..f0b29a7270df353702081fcd2ae819fb332fcc34 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -37,7 +37,7 @@
#include <hpp/fcl/narrowphase/narrowphase.h>
#include <hpp/fcl/shape/geometric_shapes_utility.h>
-#include <hpp/fcl/intersect.h>
+#include "../intersect.h"
#include <boost/math/constants/constants.hpp>
#include <vector>
#include "../src/narrowphase/details.h"
diff --git a/src/shape/geometric_shapes_utility.cpp b/src/shape/geometric_shapes_utility.cpp
index 70f26152604a39f76e543f1aa4f8fb0b1900201f..5c776ca07ca76826448810d445a1af5f29bfb652 100644
--- a/src/shape/geometric_shapes_utility.cpp
+++ b/src/shape/geometric_shapes_utility.cpp
@@ -37,7 +37,7 @@
#include <hpp/fcl/shape/geometric_shapes_utility.h>
-#include <hpp/fcl/BVH/BV_fitter.h>
+#include "../BVH/BV_fitter.h"
#include "../math/tools.h"
namespace hpp
@@ -482,7 +482,7 @@ void computeBV<RSS, Halfspace>(const Halfspace&, const Transform3f&, RSS& bv)
/// Half space can only have very rough RSS
bv.axes.setIdentity();
bv.Tr.setZero();
- bv.l[0] = bv.l[1] = bv.r = std::numeric_limits<FCL_REAL>::max();
+ bv.length[0] = bv.length[1] = bv.radius = std::numeric_limits<FCL_REAL>::max();
}
template<>
@@ -716,10 +716,10 @@ void computeBV<RSS, Plane>(const Plane& s, const Transform3f& tf, RSS& bv)
generateCoordinateSystem(n, bv.axes.col(1), bv.axes.col(2));
bv.axes.col(0).noalias() = n;
- bv.l[0] = std::numeric_limits<FCL_REAL>::max();
- bv.l[1] = std::numeric_limits<FCL_REAL>::max();
+ bv.length[0] = std::numeric_limits<FCL_REAL>::max();
+ bv.length[1] = std::numeric_limits<FCL_REAL>::max();
- bv.r = 0;
+ bv.radius = 0;
Vec3f p = s.n * s.d;
bv.Tr = tf.transform(p);
diff --git a/src/traversal/traversal_node_bvhs.h b/src/traversal/traversal_node_bvhs.h
index f0ca5d16bcfa147f61cf389698eef3a20ab356e9..b35e9783846d6618b792e8f6a3dd6828adc1d3e6 100644
--- a/src/traversal/traversal_node_bvhs.h
+++ b/src/traversal/traversal_node_bvhs.h
@@ -44,7 +44,7 @@
#include <hpp/fcl/BV/BV_node.h>
#include <hpp/fcl/BV/BV.h>
#include <hpp/fcl/BVH/BVH_model.h>
-#include <hpp/fcl/intersect.h>
+#include "../intersect.h"
#include <hpp/fcl/shape/geometric_shapes.h>
#include <hpp/fcl/narrowphase/narrowphase.h>
#include "details/traversal.h"
diff --git a/src/traversal/traversal_recurse.cpp b/src/traversal/traversal_recurse.cpp
index dd2e8d2857a93d271297fea05e939b92b1e29c81..5aa55c448893b536e1efc23988af5fda1597ed03 100644
--- a/src/traversal/traversal_recurse.cpp
+++ b/src/traversal/traversal_recurse.cpp
@@ -54,7 +54,7 @@ void collisionRecurse(CollisionTraversalNodeBase* node, int b1, int b2,
{
updateFrontList(front_list, b1, b2);
- if(node->BVDisjoints(b1, b2, sqrDistLowerBound)) return;
+ // if(node->BVDisjoints(b1, b2, sqrDistLowerBound)) return;
node->leafCollides(b1, b2, sqrDistLowerBound);
return;
}
diff --git a/test/math.cpp b/test/math.cpp
index 33845b0a73196805496f3e4591e3ad6dc84752b4..87062cb9e864194d55c639086e2db6305809a9de 100644
--- a/test/math.cpp
+++ b/test/math.cpp
@@ -39,10 +39,10 @@
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
-#include <hpp/fcl/math/types.h>
+#include <hpp/fcl/data_types.h>
#include <hpp/fcl/math/transform.h>
-#include <hpp/fcl/intersect.h>
+#include "../src/intersect.h"
#include "../src/math/tools.h"
using namespace hpp::fcl;
diff --git a/test/simple.cpp b/test/simple.cpp
index 725f6ac657d43743e8a668afdc36f9428d962979..67ccd8699ec559fabc498f58ff6b3da59368c207 100644
--- a/test/simple.cpp
+++ b/test/simple.cpp
@@ -3,7 +3,7 @@
#include <boost/test/unit_test.hpp>
#include <boost/utility/binary.hpp>
-#include <hpp/fcl/intersect.h>
+#include "../src/intersect.h"
#include <hpp/fcl/collision.h>
#include <hpp/fcl/BVH/BVH_model.h>
#include "fcl_resources/config.h"