diff --git a/CMakeLists.txt b/CMakeLists.txt index db65d593b..afa51909e 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -209,6 +209,7 @@ SET(${PROJECT_NAME}_HEADERS include/hpp/fcl/shape/details/convex.hxx include/hpp/fcl/shape/geometric_shape_to_BVH_model.h include/hpp/fcl/shape/geometric_shapes.h + include/hpp/fcl/shape/geometric_shapes_traits.h include/hpp/fcl/shape/geometric_shapes_utility.h include/hpp/fcl/distance_func_matrix.h include/hpp/fcl/collision.h @@ -232,6 +233,7 @@ SET(${PROJECT_NAME}_HEADERS include/hpp/fcl/mesh_loader/loader.h include/hpp/fcl/internal/BV_fitter.h include/hpp/fcl/internal/BV_splitter.h + include/hpp/fcl/internal/shape_shape_func.h include/hpp/fcl/internal/intersect.h include/hpp/fcl/internal/tools.h include/hpp/fcl/internal/traversal_node_base.h diff --git a/include/hpp/fcl/BV/kDOP.h b/include/hpp/fcl/BV/kDOP.h index e8e6ba328..92d82ba4f 100644 --- a/include/hpp/fcl/BV/kDOP.h +++ b/include/hpp/fcl/BV/kDOP.h @@ -38,7 +38,7 @@ #ifndef HPP_FCL_KDOP_H #define HPP_FCL_KDOP_H -#include +#include #include namespace hpp { @@ -175,14 +175,14 @@ class HPP_FCL_DLLAPI KDOP { template bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP& /*b1*/, const KDOP& /*b2*/) { - throw std::logic_error("not implemented"); + HPP_FCL_THROW_PRETTY("not implemented", std::logic_error); } template bool overlap(const Matrix3f& /*R0*/, const Vec3f& /*T0*/, const KDOP& /*b1*/, const KDOP& /*b2*/, const CollisionRequest& /*request*/, FCL_REAL& /*sqrDistLowerBound*/) { - throw std::logic_error("not implemented"); + HPP_FCL_THROW_PRETTY("not implemented", std::logic_error); } /// @brief translate the KDOP BV diff --git a/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h b/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h index a078c321d..de51472a2 100644 --- a/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h +++ b/include/hpp/fcl/broadphase/detail/simple_hash_table-inl.h @@ -56,7 +56,8 @@ SimpleHashTable::SimpleHashTable(const HashFnc& h) : h_(h) { template void SimpleHashTable::init(size_t size) { if (size == 0) { - throw std::logic_error("SimpleHashTable must have non-zero size."); + HPP_FCL_THROW_PRETTY("SimpleHashTable must have non-zero size.", + std::logic_error); } table_.resize(size); diff --git a/include/hpp/fcl/broadphase/detail/simple_hash_table.h b/include/hpp/fcl/broadphase/detail/simple_hash_table.h index e136ddeaa..ed1f0fcd8 100644 --- a/include/hpp/fcl/broadphase/detail/simple_hash_table.h +++ b/include/hpp/fcl/broadphase/detail/simple_hash_table.h @@ -38,10 +38,10 @@ #ifndef HPP_FCL_BROADPHASE_SIMPLEHASHTABLE_H #define HPP_FCL_BROADPHASE_SIMPLEHASHTABLE_H -#include #include #include #include + namespace hpp { namespace fcl { diff --git a/include/hpp/fcl/broadphase/detail/sparse_hash_table.h b/include/hpp/fcl/broadphase/detail/sparse_hash_table.h index f0ca992b3..4d8030087 100644 --- a/include/hpp/fcl/broadphase/detail/sparse_hash_table.h +++ b/include/hpp/fcl/broadphase/detail/sparse_hash_table.h @@ -38,7 +38,6 @@ #ifndef HPP_FCL_BROADPHASE_SPARSEHASHTABLE_H #define HPP_FCL_BROADPHASE_SPARSEHASHTABLE_H -#include #include #include #include diff --git a/include/hpp/fcl/collision_data.h b/include/hpp/fcl/collision_data.h index 40e9264a7..bf4357365 100644 --- a/include/hpp/fcl/collision_data.h +++ b/include/hpp/fcl/collision_data.h @@ -534,6 +534,28 @@ struct HPP_FCL_DLLAPI DistanceResult : QueryResult { } }; +namespace internal { +inline void updateDistanceLowerBoundFromBV(const CollisionRequest& /*req*/, + CollisionResult& res, + const FCL_REAL& sqrDistLowerBound) { + // BV cannot find negative distance. + if (res.distance_lower_bound <= 0) return; + FCL_REAL new_dlb = std::sqrt(sqrDistLowerBound); // - req.security_margin; + if (new_dlb < res.distance_lower_bound) res.distance_lower_bound = new_dlb; +} + +inline void updateDistanceLowerBoundFromLeaf(const CollisionRequest&, + CollisionResult& res, + const FCL_REAL& distance, + const Vec3f& p0, const Vec3f& p1) { + if (distance < res.distance_lower_bound) { + res.distance_lower_bound = distance; + res.nearest_points[0] = p0; + res.nearest_points[1] = p1; + } +} +} // namespace internal + inline CollisionRequestFlag operator~(CollisionRequestFlag a) { return static_cast(~static_cast(a)); } diff --git a/include/hpp/fcl/fwd.hh b/include/hpp/fcl/fwd.hh index b1822e18d..0585ce3b4 100644 --- a/include/hpp/fcl/fwd.hh +++ b/include/hpp/fcl/fwd.hh @@ -40,6 +40,7 @@ #include #include +#include #include #include diff --git a/include/hpp/fcl/hfield.h b/include/hpp/fcl/hfield.h index 44a50f0f0..8611faf14 100644 --- a/include/hpp/fcl/hfield.h +++ b/include/hpp/fcl/hfield.h @@ -61,14 +61,22 @@ struct HPP_FCL_DLLAPI HFNodeBase { Eigen::DenseIndex x_id, x_size; Eigen::DenseIndex y_id, y_size; + FCL_REAL max_height; + /// @brief Default constructor - HFNodeBase() : first_child(0), x_id(-1), x_size(0), y_id(-1), y_size(0) {} + HFNodeBase() + : first_child(0), + x_id(-1), + x_size(0), + y_id(-1), + y_size(0), + max_height(std::numeric_limits::lowest()) {} /// @brief Comparison operator bool operator==(const HFNodeBase& other) const { return first_child == other.first_child && x_id == other.x_id && x_size == other.x_size && y_id == other.y_id && - y_size == other.y_size; + y_size == other.y_size && max_height == other.max_height; } /// @brief Difference operator @@ -353,6 +361,8 @@ class HPP_FCL_DLLAPI HeightField : public CollisionGeometry { max_height = (std::max)(max_left_height, max_right_height); } + bv_node.max_height = max_height; + const Vec3f pointA(x_grid[bv_node.x_id], y_grid[bv_node.y_id], min_height); const Vec3f pointB(x_grid[bv_node.x_id + bv_node.x_size], y_grid[bv_node.y_id + bv_node.y_size], max_height); @@ -372,14 +382,14 @@ class HPP_FCL_DLLAPI HeightField : public CollisionGeometry { "x_size or y_size are not of correct value"); assert(bv_id < bvs.size() && "bv_id exceeds the vector dimension"); - HFNode& bvnode = bvs[bv_id]; + HFNode& bv_node = bvs[bv_id]; FCL_REAL max_height; if (x_size == 1 && y_size == 1) // don't build any BV for the current child node { max_height = heights.block<2, 2>(y_id, x_id).maxCoeff(); } else { - bvnode.first_child = num_bvs; + bv_node.first_child = num_bvs; num_bvs += 2; FCL_REAL max_left_height = min_height, max_right_height = min_height; @@ -387,27 +397,28 @@ class HPP_FCL_DLLAPI HeightField : public CollisionGeometry { { Eigen::DenseIndex x_size_half = x_size / 2; if (x_size == 1) x_size_half = 1; - max_left_height = recursiveBuildTree(bvnode.leftChild(), x_id, + max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id, x_size_half, y_id, y_size); max_right_height = - recursiveBuildTree(bvnode.rightChild(), x_id + x_size_half, + recursiveBuildTree(bv_node.rightChild(), x_id + x_size_half, x_size - x_size_half, y_id, y_size); } else // splitting along the Y axis { Eigen::DenseIndex y_size_half = y_size / 2; if (y_size == 1) y_size_half = 1; - max_left_height = recursiveBuildTree(bvnode.leftChild(), x_id, x_size, + max_left_height = recursiveBuildTree(bv_node.leftChild(), x_id, x_size, y_id, y_size_half); max_right_height = - recursiveBuildTree(bvnode.rightChild(), x_id, x_size, + recursiveBuildTree(bv_node.rightChild(), x_id, x_size, y_id + y_size_half, y_size - y_size_half); } max_height = (std::max)(max_left_height, max_right_height); } + bv_node.max_height = max_height; // max_height = std::max(max_height,min_height); const Vec3f pointA(x_grid[x_id], y_grid[y_id], min_height); @@ -416,11 +427,11 @@ class HPP_FCL_DLLAPI HeightField : public CollisionGeometry { const Vec3f pointB(x_grid[x_id + x_size], y_grid[y_id + y_size], max_height); - details::UpdateBoundingVolume::run(pointA, pointB, bvnode.bv); - bvnode.x_id = x_id; - bvnode.y_id = y_id; - bvnode.x_size = x_size; - bvnode.y_size = y_size; + details::UpdateBoundingVolume::run(pointA, pointB, bv_node.bv); + bv_node.x_id = x_id; + bv_node.y_id = y_id; + bv_node.x_size = x_size; + bv_node.y_size = y_size; return max_height; } diff --git a/include/hpp/fcl/internal/shape_shape_func.h b/include/hpp/fcl/internal/shape_shape_func.h index 0e2d8051f..181302ea8 100644 --- a/include/hpp/fcl/internal/shape_shape_func.h +++ b/include/hpp/fcl/internal/shape_shape_func.h @@ -40,10 +40,13 @@ /// @cond INTERNAL #include +#include #include +#include namespace hpp { namespace fcl { + template HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, const Transform3f& tf1, @@ -54,13 +57,56 @@ HPP_FCL_DLLAPI FCL_REAL ShapeShapeDistance(const CollisionGeometry* o1, DistanceResult& result); template -HPP_FCL_DLLAPI std::size_t ShapeShapeCollide(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, - const GJKSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result); +struct ShapeShapeCollider { + static std::size_t run(const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, + const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) { + if (request.isSatisfied(result)) return result.numContacts(); + + DistanceResult distanceResult; + DistanceRequest distanceRequest(request.enable_contact); + FCL_REAL distance = ShapeShapeDistance( + o1, tf1, o2, tf2, nsolver, distanceRequest, distanceResult); + + size_t num_contacts = 0; + const Vec3f& p1 = distanceResult.nearest_points[0]; + const Vec3f& p2 = distanceResult.nearest_points[1]; + FCL_REAL distToCollision = distance - request.security_margin; + + internal::updateDistanceLowerBoundFromLeaf(request, result, distToCollision, + p1, p2); + if (distToCollision <= request.collision_distance_threshold && + result.numContacts() < request.num_max_contacts) { + if (result.numContacts() < request.num_max_contacts) { + const Vec3f& p1 = distanceResult.nearest_points[0]; + const Vec3f& p2 = distanceResult.nearest_points[1]; + + Contact contact( + o1, o2, distanceResult.b1, distanceResult.b2, (p1 + p2) / 2, + (distance <= 0 ? distanceResult.normal : (p2 - p1).normalized()), + -distance); + + result.addContact(contact); + } + num_contacts = result.numContacts(); + } + + return num_contacts; + } +}; + +template +std::size_t ShapeShapeCollide(const CollisionGeometry* o1, + const Transform3f& tf1, + const CollisionGeometry* o2, + const Transform3f& tf2, const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) { + return ShapeShapeCollider::run( + o1, tf1, o2, tf2, nsolver, request, result); +} #define SHAPE_SHAPE_DISTANCE_SPECIALIZATION(T1, T2) \ template <> \ @@ -96,13 +142,17 @@ SHAPE_SHAPE_DISTANCE_SPECIALIZATION(TriangleP, Halfspace); #undef SHAPE_SHAPE_DISTANCE_SPECIALIZATION -#define SHAPE_SHAPE_COLLIDE_SPECIALIZATION(T1, T2) \ - template <> \ - HPP_FCL_DLLAPI std::size_t ShapeShapeCollide( \ - const CollisionGeometry* o1, const Transform3f& tf1, \ - const CollisionGeometry* o2, const Transform3f& tf2, \ - const GJKSolver* nsolver, const CollisionRequest& request, \ - CollisionResult& result) +#define SHAPE_SHAPE_COLLIDE_SPECIALIZATION(T1, T2) \ + template <> \ + struct ShapeShapeCollider { \ + static HPP_FCL_DLLAPI std::size_t run(const CollisionGeometry* o1, \ + const Transform3f& tf1, \ + const CollisionGeometry* o2, \ + const Transform3f& tf2, \ + const GJKSolver* nsolver, \ + const CollisionRequest& request, \ + CollisionResult& result); \ + } SHAPE_SHAPE_COLLIDE_SPECIALIZATION(Sphere, Sphere); diff --git a/include/hpp/fcl/internal/traversal.h b/include/hpp/fcl/internal/traversal.h index dbf032bb6..e999d8c43 100644 --- a/include/hpp/fcl/internal/traversal.h +++ b/include/hpp/fcl/internal/traversal.h @@ -59,10 +59,10 @@ struct HPP_FCL_DLLAPI RelativeTransformation { template <> struct HPP_FCL_DLLAPI RelativeTransformation { static const Matrix3f& _R() { - throw std::logic_error("should never reach this point"); + HPP_FCL_THROW_PRETTY("should never reach this point", std::logic_error); } static const Vec3f& _T() { - throw std::logic_error("should never reach this point"); + HPP_FCL_THROW_PRETTY("should never reach this point", std::logic_error); } }; } // namespace details diff --git a/include/hpp/fcl/internal/traversal_node_base.h b/include/hpp/fcl/internal/traversal_node_base.h index adc58042b..a3c56f8d4 100644 --- a/include/hpp/fcl/internal/traversal_node_base.h +++ b/include/hpp/fcl/internal/traversal_node_base.h @@ -108,9 +108,6 @@ class CollisionTraversalNodeBase : public TraversalNodeBase { virtual ~CollisionTraversalNodeBase() {} - /// @brief BV test between b1 and b2 - virtual bool BVDisjoints(unsigned int b1, unsigned int b2) const = 0; - /// BV test between b1 and b2 /// @param b1, b2 Bounding volumes to test, /// @retval sqrDistLowerBound square of a lower bound of the minimal @@ -167,28 +164,6 @@ class DistanceTraversalNodeBase : public TraversalNodeBase { DistanceResult* result; }; -namespace internal { -inline void updateDistanceLowerBoundFromBV(const CollisionRequest& req, - CollisionResult& res, - const FCL_REAL& sqrDistLowerBound) { - // BV cannot find negative distance. - if (res.distance_lower_bound <= 0) return; - FCL_REAL new_dlb = std::sqrt(sqrDistLowerBound) - req.security_margin; - if (new_dlb < res.distance_lower_bound) res.distance_lower_bound = new_dlb; -} - -inline void updateDistanceLowerBoundFromLeaf(const CollisionRequest&, - CollisionResult& res, - const FCL_REAL& distance, - const Vec3f& p0, const Vec3f& p1) { - if (distance < res.distance_lower_bound) { - res.distance_lower_bound = distance; - res.nearest_points[0] = p0; - res.nearest_points[1] = p1; - } -} -} // namespace internal - ///@} } // namespace fcl diff --git a/include/hpp/fcl/internal/traversal_node_bvh_shape.h b/include/hpp/fcl/internal/traversal_node_bvh_shape.h index 7d0b1cd52..dae9a3783 100644 --- a/include/hpp/fcl/internal/traversal_node_bvh_shape.h +++ b/include/hpp/fcl/internal/traversal_node_bvh_shape.h @@ -152,16 +152,6 @@ class MeshShapeCollisionTraversalNode nsolver = NULL; } - /// @brief BV culling test in one BVTT node - bool BVDisjoints(unsigned int b1, unsigned int /*b2*/) const { - if (this->enable_statistics) this->num_bv_tests++; - if (RTIsIdentity) - return !this->model1->getBV(b1).bv.overlap(this->model2_bv); - else - return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), - this->model2_bv, this->model1->getBV(b1).bv); - } - /// test between BV b1 and shape /// @param b1 BV to test, /// @retval sqrDistLowerBound square of a lower bound of the minimal @@ -263,17 +253,6 @@ class ShapeMeshCollisionTraversalNode nsolver = NULL; } - /// BV test between b1 and b2 - /// @param b2 Bounding volumes to test, - bool BVDisjoints(unsigned int /*b1*/, unsigned int b2) const { - if (this->enable_statistics) this->num_bv_tests++; - if (RTIsIdentity) - return !this->model2->getBV(b2).bv.overlap(this->model1_bv); - else - return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), - this->model1_bv, this->model2->getBV(b2).bv); - } - /// BV test between b1 and b2 /// @param b2 Bounding volumes to test, /// @retval sqrDistLowerBound square of a lower bound of the minimal diff --git a/include/hpp/fcl/internal/traversal_node_bvhs.h b/include/hpp/fcl/internal/traversal_node_bvhs.h index 9bcd9555a..652df4c90 100644 --- a/include/hpp/fcl/internal/traversal_node_bvhs.h +++ b/include/hpp/fcl/internal/traversal_node_bvhs.h @@ -146,16 +146,6 @@ class MeshCollisionTraversalNode : public BVHCollisionTraversalNode { tri_indices2 = NULL; } - /// @brief BV culling test in one BVTT node - bool BVDisjoints(unsigned int b1, unsigned int b2) const { - if (this->enable_statistics) this->num_bv_tests++; - if (RTIsIdentity) - return !this->model1->getBV(b1).overlap(this->model2->getBV(b2)); - else - return !overlap(RT._R(), RT._T(), this->model1->getBV(b1).bv, - this->model2->getBV(b2).bv); - } - /// BV test between b1 and b2 /// @param b1, b2 Bounding volumes to test, /// @retval sqrDistLowerBound square of a lower bound of the minimal diff --git a/include/hpp/fcl/internal/traversal_node_hfield_shape.h b/include/hpp/fcl/internal/traversal_node_hfield_shape.h index 19f3a928e..8d90863bb 100644 --- a/include/hpp/fcl/internal/traversal_node_hfield_shape.h +++ b/include/hpp/fcl/internal/traversal_node_hfield_shape.h @@ -111,9 +111,9 @@ void buildConvexTriangles(const HFNode& node, const HeightField& model, const FCL_REAL x0 = x_grid[node.x_id], x1 = x_grid[node.x_id + 1], y0 = y_grid[node.y_id], y1 = y_grid[node.y_id + 1]; + const FCL_REAL max_height = node.max_height; const Eigen::Block cell = heights.block<2, 2>(node.y_id, node.x_id); - const FCL_REAL max_height = cell.maxCoeff(); assert(max_height > min_height && "max_height is lower than min_height"); // Check whether the geometry @@ -132,8 +132,8 @@ void buildConvexTriangles(const HFNode& node, const HeightField& model, pts[7] = Vec3f(x1, y0, cell(0, 1)); Triangle* triangles = new Triangle[8]; - triangles[0].set(0, 1, 3); - triangles[1].set(4, 5, 7); + triangles[0].set(0, 1, 3); // bottom + triangles[1].set(4, 5, 7); // top triangles[2].set(0, 1, 4); triangles[3].set(4, 1, 5); triangles[4].set(1, 7, 3); @@ -151,18 +151,11 @@ void buildConvexTriangles(const HFNode& node, const HeightField& model, { Vec3f* pts = new Vec3f[8]; - pts[0] = Vec3f(x0, y0, min_height); - pts[1] = Vec3f(x0, y1, min_height); - pts[2] = Vec3f(x1, y1, min_height); - pts[3] = Vec3f(x1, y0, min_height); - pts[4] = Vec3f(x0, y0, cell(0, 0)); - pts[5] = Vec3f(x0, y1, cell(1, 0)); - pts[6] = Vec3f(x1, y1, cell(1, 1)); - pts[7] = Vec3f(x1, y0, cell(0, 1)); + memcpy(pts, convex1.points, 8 * sizeof(Vec3f)); Triangle* triangles = new Triangle[8]; - triangles[0].set(3, 2, 1); - triangles[1].set(5, 6, 7); + triangles[0].set(3, 2, 1); // top + triangles[1].set(5, 6, 7); // bottom triangles[2].set(1, 2, 5); triangles[3].set(5, 2, 6); triangles[4].set(1, 3, 7); @@ -187,6 +180,7 @@ class HeightFieldShapeCollisionTraversalNode : public CollisionTraversalNodeBase { public: typedef CollisionTraversalNodeBase Base; + typedef Eigen::Array Array2d; enum { Options = _Options, @@ -203,6 +197,7 @@ class HeightFieldShapeCollisionTraversalNode query_time_seconds = 0.0; nsolver = NULL; + shape_inflation.setZero(); } /// @brief Whether the BV node in the first BVH tree is leaf @@ -220,21 +215,6 @@ class HeightFieldShapeCollisionTraversalNode return static_cast(model1->getBV(b).rightChild()); } - /// @brief BV culling test in one BVTT node - bool BVDisjoints(unsigned int b1, unsigned int /*b2*/) const { - std::cout << "\t BVDisjoints - 2" << std::endl; - if (this->enable_statistics) this->num_bv_tests++; - if (RTIsIdentity) { - std::cout << "RTIsIdentity" << std::endl; - assert(false && "must never happened"); - return !this->model1->getBV(b1).bv.overlap(this->model2_bv); - } else { - std::cout << "\t call !overlap(" << std::endl; - return !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), - this->model2_bv, this->model1->getBV(b1).bv); - } - } - /// test between BV b1 and shape /// @param b1 BV to test, /// @retval sqrDistLowerBound square of a lower bound of the minimal @@ -243,6 +223,7 @@ class HeightFieldShapeCollisionTraversalNode bool BVDisjoints(unsigned int b1, unsigned int /*b2*/, FCL_REAL& sqrDistLowerBound) const { if (this->enable_statistics) this->num_bv_tests++; + bool disjoint; if (RTIsIdentity) { assert(false && "must never happened"); @@ -250,12 +231,14 @@ class HeightFieldShapeCollisionTraversalNode this->model2_bv, this->request, sqrDistLowerBound); } else { disjoint = !overlap(this->tf1.getRotation(), this->tf1.getTranslation(), - this->model2_bv, this->model1->getBV(b1).bv, + this->model1->getBV(b1).bv, this->model2_bv, this->request, sqrDistLowerBound); } + if (disjoint) internal::updateDistanceLowerBoundFromBV(this->request, *this->result, sqrDistLowerBound); + assert(!disjoint || sqrDistLowerBound > 0); return disjoint; } @@ -305,182 +288,47 @@ class HeightFieldShapeCollisionTraversalNode return false; } - /// @brief Intersection testing between leaves (one Convex and one shape) - void leafCollides(unsigned int b1, unsigned int /*b2*/, - FCL_REAL& sqrDistLowerBound) const { - if (this->enable_statistics) this->num_leaf_tests++; - const HFNode& node = this->model1->getBV(b1); - - // Split quadrilateral primitives into two convex shapes corresponding to - // polyhedron with triangular bases. This is essential to keep the convexity - - // typedef Convex ConvexQuadrilateral; - // const ConvexQuadrilateral convex = - // details::buildConvexQuadrilateral(node,*this->model1); - - typedef Convex ConvexTriangle; - ConvexTriangle convex1, convex2; - details::buildConvexTriangles(node, *this->model1, convex1, convex2); - - FCL_REAL distance; - Vec3f c1, c2, normal; - - bool collision = - this->shapeDistance(convex1, convex2, this->tf1, *(this->model2), - this->tf2, distance, c1, c2, normal); - - FCL_REAL distToCollision = distance - this->request.security_margin; - if (collision) { - sqrDistLowerBound = 0; - if (this->request.num_max_contacts > this->result->numContacts()) { - this->result->addContact(Contact(this->model1, this->model2, (int)b1, - (int)Contact::NONE, c1, normal, - -distance)); - assert(this->result->isCollision()); - } - } else if (distToCollision <= this->request.collision_distance_threshold) { - sqrDistLowerBound = 0; - if (this->request.num_max_contacts > this->result->numContacts()) { - this->result->addContact(Contact(this->model1, this->model2, (int)b1, - (int)Contact::NONE, .5 * (c1 + c2), - (c2 - c1).normalized(), -distance)); - } - } else - sqrDistLowerBound = distToCollision * distToCollision; - - internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result, - distToCollision, c1, c2); - - assert(this->result->isCollision() || sqrDistLowerBound > 0); - } - - const GJKSolver* nsolver; - - const HeightField* model1; - const S* model2; - BV model2_bv; - - mutable int num_bv_tests; - mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; -}; - -/// @brief Traversal node for collision between shape and mesh -template -class ShapeHeightFieldCollisionTraversalNode - : public CollisionTraversalNodeBase { - public: - typedef CollisionTraversalNodeBase Base; - - enum { - Options = _Options, - RTIsIdentity = _Options & RelativeTransformationIsIdentity - }; - - ShapeHeightFieldCollisionTraversalNode(const CollisionRequest& request) - : CollisionTraversalNodeBase(request) { - model1 = NULL; - model2 = NULL; - - num_bv_tests = 0; - num_leaf_tests = 0; - query_time_seconds = 0.0; - - nsolver = NULL; - } - - /// @brief Alway extend the second model, which is a BVH model - bool firstOverSecond(unsigned int, unsigned int) const { return false; } - - /// @brief Whether the BV node in the second BVH tree is leaf - bool isSecondNodeLeaf(unsigned int b) const { - return model2->getBV(b).isLeaf(); - } - - /// @brief Obtain the left child of BV node in the second BVH - int getSecondLeftChild(unsigned int b) const { - return model2->getBV(b).leftChild(); - } - - /// @brief Obtain the right child of BV node in the second BVH - int getSecondRightChild(unsigned int b) const { - return model2->getBV(b).rightChild(); - } - - /// BV test between b1 and b2 - /// @param b2 Bounding volumes to test, - bool BVDisjoints(unsigned int /*b1*/, unsigned int b2) const { - if (this->enable_statistics) this->num_bv_tests++; - if (RTIsIdentity) - return !this->model2->getBV(b2).bv.overlap(this->model1_bv); - else - return !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), - this->model1_bv, this->model2->getBV(b2).bv); - } - - /// BV test between b1 and b2 - /// @param b2 Bounding volumes to test, - /// @retval sqrDistLowerBound square of a lower bound of the minimal - /// distance between bounding volumes. - bool BVDisjoints(unsigned int /*b1*/, unsigned int b2, - FCL_REAL& sqrDistLowerBound) const { - if (this->enable_statistics) this->num_bv_tests++; - bool disjoint; - if (RTIsIdentity) - disjoint = !this->model2->getBV(b2).bv.overlap(this->model1_bv, - sqrDistLowerBound); - else - disjoint = !overlap(this->tf2.getRotation(), this->tf2.getTranslation(), - this->model1_bv, this->model2->getBV(b2).bv, - sqrDistLowerBound); - - if (disjoint) - internal::updateDistanceLowerBoundFromBV(this->request, *this->result, - sqrDistLowerBound); - assert(!disjoint || sqrDistLowerBound > 0); - return disjoint; - } - template - bool shapeDistance(const S& shape, const Transform3f& tf1, - const Convex& convex1, - const Convex& convex2, const Transform3f& tf2, - FCL_REAL& distance, Vec3f& c1, Vec3f& c2, - Vec3f& normal) const { + bool shapeCollision(const Convex& convex1, + const Convex& convex2, const Transform3f& tf1, + const S& shape, const Transform3f& tf2, + FCL_REAL& distance_lower_bound, Vec3f& contact_point, + Vec3f& normal) const { const Transform3f Id; - Vec3f contact2_1, contact2_2, normal2; - FCL_REAL distance2; + Vec3f contact_point2, normal2; + FCL_REAL distance_lower_bound2; bool collision1, collision2; if (RTIsIdentity) - collision1 = !nsolver->shapeDistance(shape, tf1, convex1, Id, distance, - c1, c2, normal); + collision1 = + nsolver->shapeIntersect(convex1, Id, shape, tf2, distance_lower_bound, + true, &contact_point, &normal); else - collision1 = !nsolver->shapeDistance(shape, tf1, convex1, tf2, distance, - c1, c2, normal); + collision1 = nsolver->shapeIntersect(convex1, tf1, shape, tf2, + distance_lower_bound, true, + &contact_point, &normal); if (RTIsIdentity) - collision2 = !nsolver->shapeDistance(shape, tf1, convex2, Id, distance2, - c1, c2, normal); + collision2 = nsolver->shapeIntersect(convex2, Id, shape, tf2, + distance_lower_bound2, true, + &contact_point2, &normal2); else - collision2 = !nsolver->shapeDistance(shape, tf1, convex2, tf2, distance2, - contact2_1, contact2_2, normal2); + collision2 = nsolver->shapeIntersect(convex2, tf1, shape, tf2, + distance_lower_bound2, true, + &contact_point2, &normal2); if (collision1 && collision2) { - if (distance > distance2) // switch values + if (distance_lower_bound > distance_lower_bound2) // switch values { - distance = distance2; - c1 = contact2_1; - c2 = contact2_2; + distance_lower_bound = distance_lower_bound2; + contact_point = contact_point2; normal = normal2; } return true; } else if (collision1) { return true; } else if (collision2) { - distance = distance2; - c1 = contact2_1; - c2 = contact2_2; + distance_lower_bound = distance_lower_bound2; + contact_point = contact_point2; normal = normal2; return true; } @@ -488,48 +336,56 @@ class ShapeHeightFieldCollisionTraversalNode return false; } - /// @brief Intersection testing between leaves (one shape and one triangle) - void leafCollides(unsigned int /*b1*/, unsigned int b2, + /// @brief Intersection testing between leaves (one Convex and one shape) + void leafCollides(unsigned int b1, unsigned int /*b2*/, FCL_REAL& sqrDistLowerBound) const { if (this->enable_statistics) this->num_leaf_tests++; - const HFNode& node = this->model2->getBV(b2); + const HFNode& node = this->model1->getBV(b1); + + // Split quadrilateral primitives into two convex shapes corresponding to + // polyhedron with triangular bases. This is essential to keep the convexity // typedef Convex ConvexQuadrilateral; // const ConvexQuadrilateral convex = - // details::buildConvexQuadrilateral(node,*this->model2); + // details::buildConvexQuadrilateral(node,*this->model1); typedef Convex ConvexTriangle; ConvexTriangle convex1, convex2; details::buildConvexTriangles(node, *this->model1, convex1, convex2); FCL_REAL distance; - Vec3f normal; - Vec3f c1, c2; // closest points + // Vec3f contact_point, normal; + Vec3f c1, c2, normal; bool collision = - this->shapeDistance(*(this->model1), this->tf1, convex1, convex2, + this->shapeDistance(convex1, convex2, this->tf1, *(this->model2), this->tf2, distance, c1, c2, normal); + // this->shapeCollision(convex1, convex2, this->tf1, *(this->model2), + // this->tf2, + // distance, contact_point, normal); + FCL_REAL distToCollision = distance - this->request.security_margin; - if (collision) { + if (distToCollision <= this->request.collision_distance_threshold) { sqrDistLowerBound = 0; if (this->request.num_max_contacts > this->result->numContacts()) { - this->result->addContact(Contact(this->model1, this->model2, - Contact::NONE, b2, c1, normal, - distance)); - assert(this->result->isCollision()); - return; + this->result->addContact(Contact(this->model1, this->model2, (int)b1, + (int)Contact::NONE, .5 * (c1 + c2), + (c2 - c1).normalized(), -distance)); } - } else if (distToCollision < 0) { + } else if (collision && this->request.security_margin >= 0) { sqrDistLowerBound = 0; if (this->request.num_max_contacts > this->result->numContacts()) { - this->result->addContact(Contact(this->model1, this->model2, - Contact::NONE, b2, .5 * (c1 + c2), - (c2 - c1).normalized(), distance)); + this->result->addContact(Contact(this->model1, this->model2, (int)b1, + (int)Contact::NONE, c1, normal, + -distance)); + assert(this->result->isCollision()); } } else sqrDistLowerBound = distToCollision * distToCollision; + // const Vec3f c1 = contact_point - distance * 0.5 * normal; + // const Vec3f c2 = contact_point + distance * 0.5 * normal; internal::updateDistanceLowerBoundFromLeaf(this->request, *this->result, distToCollision, c1, c2); @@ -538,9 +394,11 @@ class ShapeHeightFieldCollisionTraversalNode const GJKSolver* nsolver; - const S* model1; - const HeightField* model2; - BV model1_bv; + const HeightField* model1; + const S* model2; + BV model2_bv; + + Array2d shape_inflation; mutable int num_bv_tests; mutable int num_leaf_tests; @@ -593,7 +451,8 @@ class HeightFieldShapeDistanceTraversalNode : public DistanceTraversalNodeBase { /// @brief BV culling test in one BVTT node FCL_REAL BVDistanceLowerBound(unsigned int b1, unsigned int /*b2*/) const { - return model1->getBV(b1).bv.distance(model2_bv); + return model1->getBV(b1).bv.distance( + model2_bv); // TODO(jcarpent): tf1 is not taken into account here. } /// @brief Distance testing between leaves (one triangle and one shape) @@ -638,89 +497,6 @@ class HeightFieldShapeDistanceTraversalNode : public DistanceTraversalNodeBase { mutable FCL_REAL query_time_seconds; }; -/// @brief Traversal node for distance computation between shape and mesh -template -class ShapeHeightFieldDistanceTraversalNode : public DistanceTraversalNodeBase { - public: - typedef DistanceTraversalNodeBase Base; - - enum { - Options = _Options, - RTIsIdentity = _Options & RelativeTransformationIsIdentity - }; - - ShapeHeightFieldDistanceTraversalNode() { - model1 = NULL; - model2 = NULL; - - num_leaf_tests = 0; - query_time_seconds = 0.0; - - rel_err = 0; - abs_err = 0; - nsolver = NULL; - } - - /// @brief Alway extend the second model, which is a BVH model - bool firstOverSecond(unsigned int, unsigned int) const { return false; } - - /// @brief Whether the BV node in the second BVH tree is leaf - bool isSecondNodeLeaf(unsigned int b) const { - return model2->getBV(b).isLeaf(); - } - - /// @brief Obtain the left child of BV node in the second BVH - int getSecondLeftChild(unsigned int b) const { - return model2->getBV(b).leftChild(); - } - - /// @brief Obtain the right child of BV node in the second BVH - int getSecondRightChild(unsigned int b) const { - return model2->getBV(b).rightChild(); - } - - /// @brief Distance testing between leaves (one triangle and one shape) - void leafComputeDistance(unsigned int /*b1*/, unsigned int b2) const { - if (this->enable_statistics) this->num_leaf_tests++; - - const BVNode& node = this->model2->getBV(b2); - - typedef Convex ConvexQuadrilateral; - const ConvexQuadrilateral convex = - details::buildConvexQuadrilateral(node, *this->model2); - - FCL_REAL d; - Vec3f closest_p1, closest_p2, normal; - - nsolver->shapeDistance(*(this->model1), this->tf1, convex, this->tf2, d, - closest_p1, closest_p2, normal); - - this->result->update(d, this->model1, this->model2, DistanceResult::NONE, - b2, closest_p1, closest_p2, normal); - } - - /// @brief Whether the traversal process can stop early - bool canStop(FCL_REAL c) const { - if ((c >= this->result->min_distance - abs_err) && - (c * (1 + rel_err) >= this->result->min_distance)) - return true; - return false; - } - - FCL_REAL rel_err; - FCL_REAL abs_err; - - const GJKSolver* nsolver; - - const S* model1; - const HeightField* model2; - BV model1_bv; - - mutable int num_leaf_tests; - mutable FCL_REAL query_time_seconds; -}; - /// @} } // namespace fcl diff --git a/include/hpp/fcl/internal/traversal_node_octree.h b/include/hpp/fcl/internal/traversal_node_octree.h index d9ed85e98..7aea1bfa9 100644 --- a/include/hpp/fcl/internal/traversal_node_octree.h +++ b/include/hpp/fcl/internal/traversal_node_octree.h @@ -708,8 +708,6 @@ class HPP_FCL_DLLAPI OcTreeCollisionTraversalNode otsolver = NULL; } - bool BVDisjoints(unsigned int, unsigned) const { return false; } - bool BVDisjoints(unsigned, unsigned, FCL_REAL&) const { return false; } void leafCollides(unsigned, unsigned, FCL_REAL& sqrDistLowerBound) const { @@ -739,8 +737,6 @@ class HPP_FCL_DLLAPI ShapeOcTreeCollisionTraversalNode otsolver = NULL; } - bool BVDisjoints(unsigned int, unsigned int) const { return false; } - bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const { return false; } @@ -774,8 +770,6 @@ class HPP_FCL_DLLAPI OcTreeShapeCollisionTraversalNode otsolver = NULL; } - bool BVDisjoints(unsigned int, unsigned int) const { return false; } - bool BVDisjoints(unsigned int, unsigned int, fcl::FCL_REAL&) const { return false; } @@ -808,8 +802,6 @@ class HPP_FCL_DLLAPI MeshOcTreeCollisionTraversalNode otsolver = NULL; } - bool BVDisjoints(unsigned int, unsigned int) const { return false; } - bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const { return false; } @@ -842,8 +834,6 @@ class HPP_FCL_DLLAPI OcTreeMeshCollisionTraversalNode otsolver = NULL; } - bool BVDisjoints(unsigned int, unsigned int) const { return false; } - bool BVDisjoints(unsigned int, unsigned int, FCL_REAL&) const { return false; } diff --git a/include/hpp/fcl/internal/traversal_node_setup.h b/include/hpp/fcl/internal/traversal_node_setup.h index 41f79dc67..7d64ee149 100644 --- a/include/hpp/fcl/internal/traversal_node_setup.h +++ b/include/hpp/fcl/internal/traversal_node_setup.h @@ -365,37 +365,7 @@ bool initialize(HeightFieldShapeCollisionTraversalNode& node, HeightField& model1, Transform3f& tf1, const S& model2, const Transform3f& tf2, const GJKSolver* nsolver, CollisionResult& result, bool use_refit = false, - bool refit_bottomup = false) { - if (!tf1.isIdentity()) { - std::vector vertices_transformed(model1.num_vertices); - for (unsigned int i = 0; i < model1.num_vertices; ++i) { - const Vec3f& p = model1.vertices[i]; - Vec3f new_v = tf1.transform(p); - vertices_transformed[i] = new_v; - } - - model1.beginReplaceModel(); - model1.replaceSubModel(vertices_transformed); - model1.endReplaceModel(use_refit, refit_bottomup); - - tf1.setIdentity(); - } - - node.model1 = &model1; - node.tf1 = tf1; - node.model2 = &model2; - node.tf2 = tf2; - node.nsolver = nsolver; - - computeBV(model2, tf2, node.model2_bv); - - node.vertices = model1.vertices; - node.tri_indices = model1.tri_indices; - - node.result = &result; - - return true; -} + bool refit_bottomup = false); /// @brief Initialize traversal node for collision between one mesh and one /// shape diff --git a/include/hpp/fcl/internal/traversal_node_shapes.h b/include/hpp/fcl/internal/traversal_node_shapes.h index 9c54bc434..35577af44 100644 --- a/include/hpp/fcl/internal/traversal_node_shapes.h +++ b/include/hpp/fcl/internal/traversal_node_shapes.h @@ -65,12 +65,9 @@ class HPP_FCL_DLLAPI ShapeCollisionTraversalNode nsolver = NULL; } - /// @brief BV culling test in one BVTT node - bool BVDisjoints(int, int) const { return false; } - /// @brief BV culling test in one BVTT node bool BVDisjoints(int, int, FCL_REAL&) const { - throw std::runtime_error("Not implemented"); + HPP_FCL_THROW_PRETTY("Not implemented", std::runtime_error); } /// @brief Intersection testing between leaves (two shapes) diff --git a/include/hpp/fcl/math/transform.h b/include/hpp/fcl/math/transform.h index 11e3e02e5..9b4cec0a6 100644 --- a/include/hpp/fcl/math/transform.h +++ b/include/hpp/fcl/math/transform.h @@ -184,7 +184,11 @@ class HPP_FCL_DLLAPI Transform3f { } /// @brief check whether the transform is identity - inline bool isIdentity() const { return R.isIdentity() && T.isZero(); } + inline bool isIdentity( + const FCL_REAL& prec = + Eigen::NumTraits::dummy_precision()) const { + return R.isIdentity(prec) && T.isZero(prec); + } /// @brief set the transform to be identity transform inline void setIdentity() { diff --git a/include/hpp/fcl/mesh_loader/assimp.h b/include/hpp/fcl/mesh_loader/assimp.h index e78d18d05..115933740 100644 --- a/include/hpp/fcl/mesh_loader/assimp.h +++ b/include/hpp/fcl/mesh_loader/assimp.h @@ -96,9 +96,7 @@ inline void meshFromAssimpScene( int res = mesh->beginModel(); if (res != fcl::BVH_OK) { - std::ostringstream error; - error << "fcl BVHReturnCode = " << res; - throw std::runtime_error(error.str()); + HPP_FCL_THROW_PRETTY("fcl BVHReturnCode = " << res, std::runtime_error); } buildMesh(scale, scene, (unsigned)mesh->num_vertices, tv); diff --git a/include/hpp/fcl/narrowphase/gjk.h b/include/hpp/fcl/narrowphase/gjk.h index c129803c0..fe6a52c3e 100644 --- a/include/hpp/fcl/narrowphase/gjk.h +++ b/include/hpp/fcl/narrowphase/gjk.h @@ -57,6 +57,8 @@ Vec3f getSupport(const ShapeBase* shape, const Vec3f& dir, bool dirIsNormalized, /// /// @note The Minkowski difference is expressed in the frame of the first shape. struct HPP_FCL_DLLAPI MinkowskiDiff { + typedef Eigen::Array Array2d; + /// @brief points to two shapes const ShapeBase* shapes[2]; @@ -77,8 +79,9 @@ struct HPP_FCL_DLLAPI MinkowskiDiff { Vec3f ot1; /// @brief The radius of the sphere swepted volume. - /// The 2 values correspond to the inflation of shape 0 and shape 1. - Eigen::Array inflation; + /// The 2 values correspond to the inflation of shape 0 and shape 1/ + /// These inflation values are used for Sphere and Capsule. + Array2d inflation; /// @brief Number of points in a Convex object from which using a logarithmic /// support function is faster than a linear one. @@ -130,15 +133,6 @@ struct HPP_FCL_DLLAPI MinkowskiDiff { getSupportFunc(*this, d, dIsNormalized, supp0, supp1, hint, const_cast(data)); } - - /// @brief Set wether or not to use the normalization heuristic when computing - /// a support point. Only effective if acceleration version of GJK is used. - /// By default, when MinkowskiDiff::set is called, the normalization heuristic - /// is deduced from the shapes. The user can override this behavior with this - /// function. - inline void setNormalizeSupportDirection(bool normalize) { - normalize_support_direction = normalize; - } }; /// @brief class for GJK algorithm @@ -216,7 +210,7 @@ struct HPP_FCL_DLLAPI GJK { inline void getSupport(const Vec3f& d, bool dIsNormalized, SimplexV& sv, support_func_guess_t& hint) const { shape->support(d, dIsNormalized, sv.w0, sv.w1, hint); - sv.w.noalias() = sv.w0 - sv.w1; + sv.w = sv.w0 - sv.w1; } /// @brief whether the simplex enclose the origin diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h index 4196eca95..f401fbdc1 100644 --- a/include/hpp/fcl/narrowphase/narrowphase.h +++ b/include/hpp/fcl/narrowphase/narrowphase.h @@ -41,6 +41,7 @@ #define HPP_FCL_NARROWPHASE_H #include +#include #include #include @@ -51,6 +52,8 @@ namespace fcl { /// @brief collision and distance solver based on GJK algorithm implemented in /// fcl (rewritten the code from the GJK in bullet) struct HPP_FCL_DLLAPI GJKSolver { + typedef Eigen::Array Array2d; + /// @brief initialize GJK template void initialize_gjk(details::GJK& gjk, const details::MinkowskiDiff& shape, @@ -66,18 +69,19 @@ struct HPP_FCL_DLLAPI GJKSolver { support_hint = support_func_cached_guess; break; case GJKInitialGuess::BoundingVolumeGuess: - if (s1.aabb_radius < 0 || s2.aabb_radius < 0) { - throw std::logic_error( - "computeLocalAABB must have been called on the shapes when using " - "GJKInitialGuess::BoundingVolumeGuess."); + if (s1.aabb_local.volume() < 0 || s2.aabb_local.volume() < 0) { + HPP_FCL_THROW_PRETTY( + "computeLocalAABB must have been called on the shapes before " + "using " + "GJKInitialGuess::BoundingVolumeGuess.", + std::logic_error); } - guess.noalias() = - s1.aabb_center - (shape.oR1 * s2.aabb_center + shape.ot1); - support_hint = - support_func_cached_guess; // we could also put it to (0, 0) + guess.noalias() = s1.aabb_local.center() - + (shape.oR1 * s2.aabb_local.center() + shape.ot1); + support_hint.setZero(); break; default: - throw std::logic_error("Wrong initial guess for GJK."); + HPP_FCL_THROW_PRETTY("Wrong initial guess for GJK.", std::logic_error); } // TODO: use gjk_initial_guess instead HPP_FCL_COMPILER_DIAGNOSTIC_PUSH @@ -393,6 +397,7 @@ struct HPP_FCL_DLLAPI GJKSolver { GJKSolver(const CollisionRequest& request) { cached_guess = Vec3f(1, 0, 0); support_func_cached_guess = support_func_guess_t::Zero(); + distance_upper_bound = (std::numeric_limits::max)(); // EPS settings epa_max_face_num = 128; @@ -431,17 +436,6 @@ struct HPP_FCL_DLLAPI GJKSolver { /// @brief Copy constructor GJKSolver(const GJKSolver& other) = default; - // TODO: (enable/set/get)CachedGuess -> use gjk_initial_guess instead - void enableCachedGuess(bool if_enable) const { - enable_cached_guess = if_enable; - } - - HPP_FCL_COMPILER_DIAGNOSTIC_POP - - void setCachedGuess(const Vec3f& guess) const { cached_guess = guess; } - - Vec3f getCachedGuess() const { return cached_guess; } - HPP_FCL_COMPILER_DIAGNOSTIC_PUSH HPP_FCL_COMPILER_DIAGNOSTIC_IGNORED_DEPRECECATED_DECLARATIONS bool operator==(const GJKSolver& other) const { diff --git a/include/hpp/fcl/serialization/hfield.h b/include/hpp/fcl/serialization/hfield.h index c88f63abd..7ae2433ef 100644 --- a/include/hpp/fcl/serialization/hfield.h +++ b/include/hpp/fcl/serialization/hfield.h @@ -21,6 +21,7 @@ void serialize(Archive &ar, hpp::fcl::HFNodeBase &node, ar &make_nvp("x_size", node.x_size); ar &make_nvp("y_id", node.y_id); ar &make_nvp("y_size", node.y_size); + ar &make_nvp("max_height", node.max_height); } template diff --git a/include/hpp/fcl/shape/details/convex.hxx b/include/hpp/fcl/shape/details/convex.hxx index 8edac251b..c660b858a 100644 --- a/include/hpp/fcl/shape/details/convex.hxx +++ b/include/hpp/fcl/shape/details/convex.hxx @@ -235,7 +235,7 @@ void Convex::fillNeighbors() { for (unsigned int i = 0; i < num_points; ++i) { Neighbors& n = neighbors[i]; if (nneighbors[i].size() >= (std::numeric_limits::max)()) - throw std::logic_error("Too many neighbors."); + HPP_FCL_THROW_PRETTY("Too many neighbors.", std::logic_error); n.count_ = (unsigned char)nneighbors[i].size(); n.n_ = p_nneighbors; p_nneighbors = diff --git a/include/hpp/fcl/shape/geometric_shapes.h b/include/hpp/fcl/shape/geometric_shapes.h index 15b845e9d..151f286bd 100644 --- a/include/hpp/fcl/shape/geometric_shapes.h +++ b/include/hpp/fcl/shape/geometric_shapes.h @@ -55,7 +55,7 @@ class HPP_FCL_DLLAPI ShapeBase : public CollisionGeometry { /// \brief Copy constructor ShapeBase(const ShapeBase& other) : CollisionGeometry(other) {} - ShapeBase& operator=(const ShapeBase& /*other*/) { return *this; } + ShapeBase& operator=(const ShapeBase& other) = default; virtual ~ShapeBase(){}; @@ -84,6 +84,26 @@ class HPP_FCL_DLLAPI TriangleP : public ShapeBase { NODE_TYPE getNodeType() const { return GEOM_TRIANGLE; } + // std::pair inflated(const FCL_REAL value) const { + // if (value == 0) return std::make_pair(new TriangleP(*this), + // Transform3f()); Vec3f AB(b - a), BC(c - b), CA(a - c); AB.normalize(); + // BC.normalize(); + // CA.normalize(); + // + // Vec3f new_a(a + value * Vec3f(-AB + CA).normalized()); + // Vec3f new_b(b + value * Vec3f(-BC + AB).normalized()); + // Vec3f new_c(c + value * Vec3f(-CA + BC).normalized()); + // + // return std::make_pair(new TriangleP(new_a, new_b, new_c), + // Transform3f()); + // } + // + // FCL_REAL minInflationValue() const + // { + // return (std::numeric_limits::max)(); // TODO(jcarpent): + // implement + // } + Vec3f a, b, c; private: @@ -139,6 +159,24 @@ class HPP_FCL_DLLAPI Box : public ShapeBase { return (Vec3f(s[1] + s[2], s[0] + s[2], s[0] + s[1]) / 3).asDiagonal(); } + FCL_REAL minInflationValue() const { return -halfSide.minCoeff(); } + + /// \brief Inflate the box by an amount given by value + /// + /// \param[in] value of the shape inflation. + /// + /// \returns a new inflated box and the related transform to account for the + /// change of shape frame + std::pair inflated(const FCL_REAL value) const { + if (value <= minInflationValue()) + HPP_FCL_THROW_PRETTY("value (" << value << ") " + << "is two small. It should be at least: " + << minInflationValue(), + std::invalid_argument); + return std::make_pair(Box(2 * (halfSide + Vec3f::Constant(value))), + Transform3f()); + } + private: virtual bool isEqual(const CollisionGeometry& _other) const { const Box* other_ptr = dynamic_cast(&_other); @@ -181,6 +219,23 @@ class HPP_FCL_DLLAPI Sphere : public ShapeBase { radius / 3; } + FCL_REAL minInflationValue() const { return -radius; } + + /// \brief Inflate the sphere by an amount given by value + /// + /// \param[in] value of the shape inflation. + /// + /// \returns a new inflated sphere and the related transform to account for + /// the change of shape frame + std::pair inflated(const FCL_REAL value) const { + if (value <= minInflationValue()) + HPP_FCL_THROW_PRETTY( + "value (" << value << ") is two small. It should be at least: " + << minInflationValue(), + std::invalid_argument); + return std::make_pair(Sphere(radius + value), Transform3f()); + } + private: virtual bool isEqual(const CollisionGeometry& _other) const { const Sphere* other_ptr = dynamic_cast(&_other); @@ -200,6 +255,8 @@ class HPP_FCL_DLLAPI Ellipsoid : public ShapeBase { Ellipsoid(FCL_REAL rx, FCL_REAL ry, FCL_REAL rz) : ShapeBase(), radii(rx, ry, rz) {} + Ellipsoid(const Vec3f& radii) : radii(radii) {} + Ellipsoid(const Ellipsoid& other) : ShapeBase(other), radii(other.radii) {} /// @brief Clone *this into a new Ellipsoid @@ -230,6 +287,24 @@ class HPP_FCL_DLLAPI Ellipsoid : public ShapeBase { radii[2] / 3; } + FCL_REAL minInflationValue() const { return -radii.minCoeff(); } + + /// \brief Inflate the ellipsoid by an amount given by value + /// + /// \param[in] value of the shape inflation. + /// + /// \returns a new inflated ellipsoid and the related transform to account for + /// the change of shape frame + std::pair inflated(const FCL_REAL value) const { + if (value <= minInflationValue()) + HPP_FCL_THROW_PRETTY( + "value (" << value << ") is two small. It should be at least: " + << minInflationValue(), + std::invalid_argument); + return std::make_pair(Ellipsoid(radii + Vec3f::Constant(value)), + Transform3f()); + } + private: virtual bool isEqual(const CollisionGeometry& _other) const { const Ellipsoid* other_ptr = dynamic_cast(&_other); @@ -291,6 +366,24 @@ class HPP_FCL_DLLAPI Capsule : public ShapeBase { return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); } + FCL_REAL minInflationValue() const { return -radius; } + + /// \brief Inflate the capsule by an amount given by value + /// + /// \param[in] value of the shape inflation. + /// + /// \returns a new inflated capsule and the related transform to account for + /// the change of shape frame + std::pair inflated(const FCL_REAL value) const { + if (value <= minInflationValue()) + HPP_FCL_THROW_PRETTY( + "value (" << value << ") is two small. It should be at least: " + << minInflationValue(), + std::invalid_argument); + return std::make_pair(Capsule(radius + value, 2 * halfLength), + Transform3f()); + } + private: virtual bool isEqual(const CollisionGeometry& _other) const { const Capsule* other_ptr = dynamic_cast(&_other); @@ -347,6 +440,35 @@ class HPP_FCL_DLLAPI Cone : public ShapeBase { Vec3f computeCOM() const { return Vec3f(0, 0, -0.5 * halfLength); } + FCL_REAL minInflationValue() const { return -(std::min)(radius, halfLength); } + + /// \brief Inflate the cone by an amount given by value + /// + /// \param[in] value of the shape inflation. + /// + /// \returns a new inflated cone and the related transform to account for the + /// change of shape frame + std::pair inflated(const FCL_REAL value) const { + if (value <= minInflationValue()) + HPP_FCL_THROW_PRETTY( + "value (" << value << ") is two small. It should be at least: " + << minInflationValue(), + std::invalid_argument); + + // tan(alpha) = 2*halfLength/radius; + const FCL_REAL tan_alpha = 2 * halfLength / radius; + const FCL_REAL sin_alpha = tan_alpha / std::sqrt(1 + tan_alpha * tan_alpha); + const FCL_REAL top_inflation = value / sin_alpha; + const FCL_REAL bottom_inflation = value; + + const FCL_REAL new_lz = 2 * halfLength + top_inflation + bottom_inflation; + const FCL_REAL new_cz = (top_inflation + bottom_inflation) / 2.; + const FCL_REAL new_radius = new_lz / tan_alpha; + + return std::make_pair(Cone(new_radius, new_lz), + Transform3f(Vec3f(0., 0., new_cz))); + } + private: virtual bool isEqual(const CollisionGeometry& _other) const { const Cone* other_ptr = dynamic_cast(&_other); @@ -406,6 +528,24 @@ class HPP_FCL_DLLAPI Cylinder : public ShapeBase { return (Matrix3f() << ix, 0, 0, 0, ix, 0, 0, 0, iz).finished(); } + FCL_REAL minInflationValue() const { return -(std::min)(radius, halfLength); } + + /// \brief Inflate the cylinder by an amount given by value + /// + /// \param[in] value of the shape inflation. + /// + /// \returns a new inflated cylinder and the related transform to account for + /// the change of shape frame + std::pair inflated(const FCL_REAL value) const { + if (value <= minInflationValue()) + HPP_FCL_THROW_PRETTY( + "value (" << value << ") is two small. It should be at least: " + << minInflationValue(), + std::invalid_argument); + return std::make_pair(Cylinder(radius + value, 2 * (halfLength + value)), + Transform3f()); + } + private: virtual bool isEqual(const CollisionGeometry& _other) const { const Cylinder* other_ptr = dynamic_cast(&_other); @@ -607,6 +747,25 @@ class HPP_FCL_DLLAPI Halfspace : public ShapeBase { /// @brief Get node type: a half space NODE_TYPE getNodeType() const { return GEOM_HALFSPACE; } + FCL_REAL minInflationValue() const { + return std::numeric_limits::lowest(); + } + + /// \brief Inflate the cylinder by an amount given by value + /// + /// \param[in] value of the shape inflation. + /// + /// \returns a new inflated cylinder and the related transform to account for + /// the change of shape frame + std::pair inflated(const FCL_REAL value) const { + if (value <= minInflationValue()) + HPP_FCL_THROW_PRETTY( + "value (" << value << ") is two small. It should be at least: " + << minInflationValue(), + std::invalid_argument); + return std::make_pair(Halfspace(n, d + value), Transform3f()); + } + /// @brief Plane normal Vec3f n; diff --git a/include/hpp/fcl/shape/geometric_shapes_traits.h b/include/hpp/fcl/shape/geometric_shapes_traits.h new file mode 100644 index 000000000..30c5317cd --- /dev/null +++ b/include/hpp/fcl/shape/geometric_shapes_traits.h @@ -0,0 +1,150 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2011-2014, Willow Garage, Inc. + * Copyright (c) 2014-2015, Open Source Robotics Foundation + * Copyright (c) 2015-2022, CNRS, Inria + * 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. + */ + +#ifndef HPP_FCL_GEOMETRIC_SHAPES_TRAITS_H +#define HPP_FCL_GEOMETRIC_SHAPES_TRAITS_H + +#include + +namespace hpp { +namespace fcl { + +struct shape_traits_base { + enum { + NeedNormalizedDir = true, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = false, + HasInflatedSupportFunction = false + }; +}; + +template +struct shape_traits : shape_traits_base {}; + +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = false, + HasInflatedSupportFunction = false + }; +}; + +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = true, + HasInflatedSupportFunction = false + }; +}; + +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = true, + HasInflatedSupportFunction = false + }; +}; + +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = true, + HasInflatedSupportFunction = false + }; +}; + +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = true, + HasInflatedSupportFunction = false + }; +}; + +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = true, + HasInflatedSupportFunction = false + }; +}; + +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = true, + HasInflatedSupportFunction = false + }; +}; + +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = true, + IsInflatable = false, + HasInflatedSupportFunction = true + }; +}; + +template <> +struct shape_traits : shape_traits_base { + enum { + NeedNormalizedDir = false, + NeedNesterovNormalizeHeuristic = false, + IsInflatable = true, + HasInflatedSupportFunction = false + }; +}; + +} // namespace fcl +} // namespace hpp + +#endif // ifndef HPP_FCL_GEOMETRIC_SHAPES_TRAITS_H diff --git a/python/collision-geometries.cc b/python/collision-geometries.cc index 598356d9a..5abc2e872 100644 --- a/python/collision-geometries.cc +++ b/python/collision-geometries.cc @@ -322,6 +322,7 @@ void exposeShapes() { class_, shared_ptr >( "Ellipsoid", doxygen::class_doc(), no_init) .def(dv::init()) + .def(dv::init()) .DEF_RW_CLASS_ATTRIB(Ellipsoid, radii) .def("clone", &Ellipsoid::clone, doxygen::member_func_doc(&Ellipsoid::clone), diff --git a/python/gjk.cc b/python/gjk.cc index 4dd2c72e7..c9e9f856f 100644 --- a/python/gjk.cc +++ b/python/gjk.cc @@ -80,8 +80,8 @@ void exposeGJK() { .DEF_CLASS_FUNC(MinkowskiDiff, support0) .DEF_CLASS_FUNC(MinkowskiDiff, support1) .DEF_CLASS_FUNC(MinkowskiDiff, support) - .DEF_CLASS_FUNC(MinkowskiDiff, setNormalizeSupportDirection) - .DEF_RW_CLASS_ATTRIB(MinkowskiDiff, inflation); + .DEF_RW_CLASS_ATTRIB(MinkowskiDiff, inflation) + .DEF_RW_CLASS_ATTRIB(MinkowskiDiff, normalize_support_direction); } if (!eigenpy::register_symbolic_link_to_registered_type()) { diff --git a/python/math.cc b/python/math.cc index 5308c8c3f..fdd7dd09f 100644 --- a/python/math.cc +++ b/python/math.cc @@ -90,7 +90,10 @@ void exposeMaths() { return_value_policy()) .def("getRotation", &Transform3f::getRotation, return_value_policy()) - .def(dv::member_func("isIdentity", &Transform3f::isIdentity)) + .def("isIdentity", &Transform3f::isIdentity, + (bp::arg("self"), + bp::arg("prec") = Eigen::NumTraits::dummy_precision()), + doxygen::member_func_doc(&Transform3f::getTranslation)) .def(dv::member_func("setQuatRotation", &Transform3f::setQuatRotation)) .def("setTranslation", &Transform3f::setTranslation) diff --git a/src/BV/AABB.cpp b/src/BV/AABB.cpp index b6ed66cbc..3fdc7036c 100644 --- a/src/BV/AABB.cpp +++ b/src/BV/AABB.cpp @@ -49,15 +49,24 @@ AABB::AABB() bool AABB::overlap(const AABB& other, const CollisionRequest& request, FCL_REAL& sqrDistLowerBound) const { - const FCL_REAL breakDistance(request.break_distance + - request.security_margin); - const FCL_REAL breakDistance2 = breakDistance * breakDistance; - - sqrDistLowerBound = (min_ - other.max_).array().max(0).matrix().squaredNorm(); - if (sqrDistLowerBound > breakDistance2) return false; - - sqrDistLowerBound = (other.min_ - max_).array().max(0).matrix().squaredNorm(); - if (sqrDistLowerBound > breakDistance2) return false; + const FCL_REAL break_distance_squared = + request.break_distance * request.break_distance; + + sqrDistLowerBound = + (min_ - other.max_ - Vec3f::Constant(request.security_margin)) + .array() + .max(0) + .matrix() + .squaredNorm(); + if (sqrDistLowerBound > break_distance_squared) return false; + + sqrDistLowerBound = + (other.min_ - max_ - Vec3f::Constant(request.security_margin)) + .array() + .max(0) + .matrix() + .squaredNorm(); + if (sqrDistLowerBound > break_distance_squared) return false; return true; } diff --git a/src/BV/OBB.cpp b/src/BV/OBB.cpp index 4c3d159aa..f56e51cba 100644 --- a/src/BV/OBB.cpp +++ b/src/BV/OBB.cpp @@ -343,14 +343,23 @@ struct HPP_FCL_LOCAL obbDisjoint_check_Ai_cross_Bi { // This function tests whether bounding boxes should be broken down. // bool obbDisjointAndLowerBoundDistance(const Matrix3f& B, const Vec3f& T, - const Vec3f& a, const Vec3f& b, + const Vec3f& a_, const Vec3f& b_, const CollisionRequest& request, FCL_REAL& squaredLowerBoundDistance) { - const FCL_REAL breakDistance(request.break_distance + - request.security_margin); - const FCL_REAL breakDistance2 = breakDistance * breakDistance; + assert(request.security_margin > + -2 * (std::min)(a_.minCoeff(), b_.minCoeff()) - + 10 * Eigen::NumTraits::epsilon() && + "A negative security margin could not be lower than the OBB extent."); + // const FCL_REAL breakDistance(request.break_distance + + // request.security_margin); + const FCL_REAL breakDistance2 = + request.break_distance * request.break_distance; Matrix3f Bf(B.cwiseAbs()); + const Vec3f a( + (a_ + Vec3f::Constant(request.security_margin / 2)).array().max(0)); + const Vec3f b( + (b_ + Vec3f::Constant(request.security_margin / 2)).array().max(0)); // Corner of b axis aligned bounding box the closest to the origin squaredLowerBoundDistance = internal::obbDisjoint_check_A_axis(T, a, b, Bf); diff --git a/src/BV/RSS.cpp b/src/BV/RSS.cpp index b6d91cf77..0144c591d 100644 --- a/src/BV/RSS.cpp +++ b/src/BV/RSS.cpp @@ -37,8 +37,11 @@ #include #include -#include #include +#include + +#include + namespace hpp { namespace fcl { @@ -740,7 +743,7 @@ bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, } bool overlap(const Matrix3f& R0, const Vec3f& T0, const RSS& b1, const RSS& b2, - const CollisionRequest& /*request*/, FCL_REAL& sqrDistLowerBound) { + const CollisionRequest& request, FCL_REAL& sqrDistLowerBound) { // ROb2 = R0 . b2 // where b2 = [ b2.axis [0] | b2.axis [1] | b2.axis [2] ] @@ -750,8 +753,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.length, b2.length) - b1.radius - b2.radius; + FCL_REAL dist = rectDistance(R, T, b1.length, b2.length) - b1.radius - + b2.radius - request.security_margin; if (dist <= 0) return true; sqrDistLowerBound = dist * dist; return false; diff --git a/src/collision_func_matrix.cpp b/src/collision_func_matrix.cpp index c7c921fcb..997c11444 100644 --- a/src/collision_func_matrix.cpp +++ b/src/collision_func_matrix.cpp @@ -41,6 +41,7 @@ #include <../src/collision_node.h> #include #include +#include #include <../src/traits_traversal.h> namespace hpp { @@ -49,12 +50,18 @@ namespace fcl { #ifdef HPP_FCL_HAS_OCTOMAP template -std::size_t Collide(const CollisionGeometry* o1, const Transform3f& tf1, - const CollisionGeometry* o2, const Transform3f& tf2, - const GJKSolver* nsolver, const CollisionRequest& request, - CollisionResult& result) { +std::size_t OctreeCollide(const CollisionGeometry* o1, const Transform3f& tf1, + const CollisionGeometry* o2, const Transform3f& tf2, + const GJKSolver* nsolver, + const CollisionRequest& request, + CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); + if (request.security_margin < 0) + HPP_FCL_THROW_PRETTY( + "Negative security margin are not handled yet for Octree", + std::invalid_argument); + typename TraversalTraitsCollision::CollisionTraversal_t node( request); const TypeA* obj1 = dynamic_cast(o1); @@ -69,46 +76,6 @@ std::size_t Collide(const CollisionGeometry* o1, const Transform3f& tf1, #endif -template -std::size_t ShapeShapeCollide(const CollisionGeometry* o1, - const Transform3f& tf1, - const CollisionGeometry* o2, - const Transform3f& tf2, const GJKSolver* nsolver, - const CollisionRequest& request, - CollisionResult& result) { - if (request.isSatisfied(result)) return result.numContacts(); - - DistanceResult distanceResult; - DistanceRequest distanceRequest(request.enable_contact); - FCL_REAL distance = ShapeShapeDistance( - o1, tf1, o2, tf2, nsolver, distanceRequest, distanceResult); - - size_t num_contacts = 0; - const Vec3f& p1 = distanceResult.nearest_points[0]; - const Vec3f& p2 = distanceResult.nearest_points[1]; - FCL_REAL distToCollision = distance - request.security_margin; - - internal::updateDistanceLowerBoundFromLeaf(request, result, distToCollision, - p1, p2); - if (distToCollision <= request.collision_distance_threshold && - result.numContacts() < request.num_max_contacts) { - if (result.numContacts() < request.num_max_contacts) { - const Vec3f& p1 = distanceResult.nearest_points[0]; - const Vec3f& p2 = distanceResult.nearest_points[1]; - - Contact contact( - o1, o2, distanceResult.b1, distanceResult.b2, (p1 + p2) / 2, - (distance <= 0 ? distanceResult.normal : (p2 - p1).normalized()), - -distance); - - result.addContact(contact); - } - num_contacts = result.numContacts(); - } - - return num_contacts; -} - namespace details { template struct bvh_shape_traits { @@ -141,6 +108,11 @@ struct HPP_FCL_LOCAL BVHShapeCollider { CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); + if (request.security_margin < 0) + HPP_FCL_THROW_PRETTY( + "Negative security margin are not handled yet for BVHModel", + std::invalid_argument); + if (_Options & RelativeTransformationIsIdentity) return aligned(o1, tf1, o2, tf2, nsolver, request, result); else @@ -205,11 +177,12 @@ struct HPP_FCL_LOCAL HeightFieldShapeCollider { CollisionResult& result) { if (request.isSatisfied(result)) return result.numContacts(); + const HF& height_field = static_cast(*o1); + const Shape& shape = static_cast(*o2); + HeightFieldShapeCollisionTraversalNode node(request); - const HF* obj1 = static_cast(o1); - const Shape* obj2 = static_cast(o2); - initialize(node, *obj1, tf1, *obj2, tf2, nsolver, result); + initialize(node, height_field, tf1, shape, tf2, nsolver, result); fcl::collide(&node, request, result); return result.numContacts(); } @@ -640,51 +613,65 @@ CollisionFunctionMatrix::CollisionFunctionMatrix() { collision_matrix[BV_OBBRSS][BV_OBBRSS] = &BVHCollide; #ifdef HPP_FCL_HAS_OCTOMAP - collision_matrix[GEOM_OCTREE][GEOM_BOX] = &Collide; - collision_matrix[GEOM_OCTREE][GEOM_SPHERE] = &Collide; - collision_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &Collide; - collision_matrix[GEOM_OCTREE][GEOM_CONE] = &Collide; - collision_matrix[GEOM_OCTREE][GEOM_CYLINDER] = &Collide; - collision_matrix[GEOM_OCTREE][GEOM_CONVEX] = &Collide; - collision_matrix[GEOM_OCTREE][GEOM_PLANE] = &Collide; - collision_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = &Collide; - collision_matrix[GEOM_OCTREE][GEOM_ELLIPSOID] = &Collide; - - collision_matrix[GEOM_BOX][GEOM_OCTREE] = &Collide; - collision_matrix[GEOM_SPHERE][GEOM_OCTREE] = &Collide; - collision_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &Collide; - collision_matrix[GEOM_CONE][GEOM_OCTREE] = &Collide; - collision_matrix[GEOM_CYLINDER][GEOM_OCTREE] = &Collide; - collision_matrix[GEOM_CONVEX][GEOM_OCTREE] = &Collide; - collision_matrix[GEOM_PLANE][GEOM_OCTREE] = &Collide; - collision_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = &Collide; - - collision_matrix[GEOM_OCTREE][GEOM_OCTREE] = &Collide; - - collision_matrix[GEOM_OCTREE][BV_AABB] = &Collide >; - collision_matrix[GEOM_OCTREE][BV_OBB] = &Collide >; - collision_matrix[GEOM_OCTREE][BV_RSS] = &Collide >; + collision_matrix[GEOM_OCTREE][GEOM_BOX] = &OctreeCollide; + collision_matrix[GEOM_OCTREE][GEOM_SPHERE] = &OctreeCollide; + collision_matrix[GEOM_OCTREE][GEOM_CAPSULE] = &OctreeCollide; + collision_matrix[GEOM_OCTREE][GEOM_CONE] = &OctreeCollide; + collision_matrix[GEOM_OCTREE][GEOM_CYLINDER] = + &OctreeCollide; + collision_matrix[GEOM_OCTREE][GEOM_CONVEX] = + &OctreeCollide; + collision_matrix[GEOM_OCTREE][GEOM_PLANE] = &OctreeCollide; + collision_matrix[GEOM_OCTREE][GEOM_HALFSPACE] = + &OctreeCollide; + collision_matrix[GEOM_OCTREE][GEOM_ELLIPSOID] = + &OctreeCollide; + + collision_matrix[GEOM_BOX][GEOM_OCTREE] = &OctreeCollide; + collision_matrix[GEOM_SPHERE][GEOM_OCTREE] = &OctreeCollide; + collision_matrix[GEOM_CAPSULE][GEOM_OCTREE] = &OctreeCollide; + collision_matrix[GEOM_CONE][GEOM_OCTREE] = &OctreeCollide; + collision_matrix[GEOM_CYLINDER][GEOM_OCTREE] = + &OctreeCollide; + collision_matrix[GEOM_CONVEX][GEOM_OCTREE] = + &OctreeCollide; + collision_matrix[GEOM_PLANE][GEOM_OCTREE] = &OctreeCollide; + collision_matrix[GEOM_HALFSPACE][GEOM_OCTREE] = + &OctreeCollide; + + collision_matrix[GEOM_OCTREE][GEOM_OCTREE] = &OctreeCollide; + + collision_matrix[GEOM_OCTREE][BV_AABB] = + &OctreeCollide >; + collision_matrix[GEOM_OCTREE][BV_OBB] = + &OctreeCollide >; + collision_matrix[GEOM_OCTREE][BV_RSS] = + &OctreeCollide >; collision_matrix[GEOM_OCTREE][BV_OBBRSS] = - &Collide >; - collision_matrix[GEOM_OCTREE][BV_kIOS] = &Collide >; + &OctreeCollide >; + collision_matrix[GEOM_OCTREE][BV_kIOS] = + &OctreeCollide >; collision_matrix[GEOM_OCTREE][BV_KDOP16] = - &Collide > >; + &OctreeCollide > >; collision_matrix[GEOM_OCTREE][BV_KDOP18] = - &Collide > >; + &OctreeCollide > >; collision_matrix[GEOM_OCTREE][BV_KDOP24] = - &Collide > >; - - collision_matrix[BV_AABB][GEOM_OCTREE] = &Collide, OcTree>; - collision_matrix[BV_OBB][GEOM_OCTREE] = &Collide, OcTree>; - collision_matrix[BV_RSS][GEOM_OCTREE] = &Collide, OcTree>; - collision_matrix[BV_OBBRSS][GEOM_OCTREE] = &Collide, OcTree>; - collision_matrix[BV_kIOS][GEOM_OCTREE] = &Collide, OcTree>; + &OctreeCollide > >; + + collision_matrix[BV_AABB][GEOM_OCTREE] = + &OctreeCollide, OcTree>; + collision_matrix[BV_OBB][GEOM_OCTREE] = &OctreeCollide, OcTree>; + collision_matrix[BV_RSS][GEOM_OCTREE] = &OctreeCollide, OcTree>; + collision_matrix[BV_OBBRSS][GEOM_OCTREE] = + &OctreeCollide, OcTree>; + collision_matrix[BV_kIOS][GEOM_OCTREE] = + &OctreeCollide, OcTree>; collision_matrix[BV_KDOP16][GEOM_OCTREE] = - &Collide >, OcTree>; + &OctreeCollide >, OcTree>; collision_matrix[BV_KDOP18][GEOM_OCTREE] = - &Collide >, OcTree>; + &OctreeCollide >, OcTree>; collision_matrix[BV_KDOP24][GEOM_OCTREE] = - &Collide >, OcTree>; + &OctreeCollide >, OcTree>; #endif } // template struct CollisionFunctionMatrix; diff --git a/src/distance/sphere_sphere.cpp b/src/distance/sphere_sphere.cpp index ed1f044cc..13a2b960b 100644 --- a/src/distance/sphere_sphere.cpp +++ b/src/distance/sphere_sphere.cpp @@ -93,8 +93,7 @@ FCL_REAL ShapeShapeDistance( return result.min_distance; } -template <> -std::size_t ShapeShapeCollide( +std::size_t ShapeShapeCollider::run( const CollisionGeometry* o1, const Transform3f& tf1, const CollisionGeometry* o2, const Transform3f& tf2, const GJKSolver*, const CollisionRequest& request, CollisionResult& result) { diff --git a/src/narrowphase/gjk.cpp b/src/narrowphase/gjk.cpp index 4a417b5c8..ba4af779a 100644 --- a/src/narrowphase/gjk.cpp +++ b/src/narrowphase/gjk.cpp @@ -38,59 +38,13 @@ #include #include #include +#include namespace hpp { namespace fcl { namespace details { -struct HPP_FCL_LOCAL shape_traits_base { - enum { NeedNormalizedDir = true, NeedNesterovNormalizeHeuristic = false }; -}; - -template -struct HPP_FCL_LOCAL shape_traits : shape_traits_base {}; - -template <> -struct HPP_FCL_LOCAL shape_traits : shape_traits_base { - enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false }; -}; - -template <> -struct HPP_FCL_LOCAL shape_traits : shape_traits_base { - enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false }; -}; - -template <> -struct HPP_FCL_LOCAL shape_traits : shape_traits_base { - enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false }; -}; - -template <> -struct HPP_FCL_LOCAL shape_traits : shape_traits_base { - enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false }; -}; - -template <> -struct HPP_FCL_LOCAL shape_traits : shape_traits_base { - enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false }; -}; - -template <> -struct HPP_FCL_LOCAL shape_traits : shape_traits_base { - enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false }; -}; - -template <> -struct HPP_FCL_LOCAL shape_traits : shape_traits_base { - enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = false }; -}; - -template <> -struct HPP_FCL_LOCAL shape_traits : shape_traits_base { - enum { NeedNormalizedDir = false, NeedNesterovNormalizeHeuristic = true }; -}; - void getShapeSupport(const TriangleP* triangle, const Vec3f& dir, Vec3f& support, int&, MinkowskiDiff::ShapeData*) { FCL_REAL dota = dir.dot(triangle->a); @@ -538,9 +492,9 @@ void MinkowskiDiff::set(const ShapeBase* shape0, const ShapeBase* shape1, getNormalizeSupportDirectionFromShapes(shape0, shape1, normalize_support_direction); - oR1 = tf0.getRotation().transpose() * tf1.getRotation(); - ot1 = tf0.getRotation().transpose() * - (tf1.getTranslation() - tf0.getTranslation()); + oR1.noalias() = tf0.getRotation().transpose() * tf1.getRotation(); + ot1.noalias() = tf0.getRotation().transpose() * + (tf1.getTranslation() - tf0.getTranslation()); bool identity = (oR1.isIdentity() && ot1.isZero()); @@ -769,8 +723,8 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, FCL_REAL frank_wolfe_duality_gap = 2 * ray.dot(ray - w); if (frank_wolfe_duality_gap - tolerance <= 0) { removeVertex(simplices[current]); - current_gjk_variant = DefaultGJK; - continue; // continue to next iteration + current_gjk_variant = DefaultGJK; // move back to classic GJK + continue; // continue to next iteration } } @@ -783,7 +737,7 @@ GJK::Status GJK::evaluate(const MinkowskiDiff& shape_, const Vec3f& guess, if (iterations > 0 && cv_check_passed) { if (iterations > 0) removeVertex(simplices[current]); if (current_gjk_variant != DefaultGJK) { - current_gjk_variant = DefaultGJK; + current_gjk_variant = DefaultGJK; // move back to classic GJK continue; } distance = rl - inflation; diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp index 1d63d044c..51fa9cddf 100644 --- a/src/narrowphase/narrowphase.cpp +++ b/src/narrowphase/narrowphase.cpp @@ -364,7 +364,7 @@ bool GJKSolver::shapeTriangleInteraction(const Plane& s, const Transform3f& tf1, // +------------+-----+--------+---------+------+----------+-------+------------+----------+ // | box | | O | | | | | | | // +------------+-----+--------+---------+------+----------+-------+------------+----------+ -// | sphere |/////| O | O | | | | | | +// | sphere |/////| O | O | | O | | | | // +------------+-----+--------+---------+------+----------+-------+------------+----------+ // | capsule |/////|////////| O | | | | | | // +------------+-----+--------+---------+------+----------+-------+------------+----------+ diff --git a/src/shape/convex.cpp b/src/shape/convex.cpp index 58fb6d24b..3841f0059 100644 --- a/src/shape/convex.cpp +++ b/src/shape/convex.cpp @@ -57,7 +57,7 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, Qhull qh; const char* command = qhullCommand ? qhullCommand : (keepTriangles ? "Qt" : ""); - qh.runQhull("", 3, num_points, pts[0].data(), command); + qh.runQhull("", 3, static_cast(num_points), pts[0].data(), command); if (qh.qhullStatus() != qh_ERRnone) { if (qh.hasQhullMessage()) std::cerr << qh.qhullMessage() << std::endl; @@ -71,14 +71,14 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, std::vector pts_to_vertices(num_points, -1); // Initialize the vertices - int nvertex = qh.vertexCount(); + int nvertex = (qh.vertexCount()); Vec3f* vertices = new Vec3f[nvertex]; QhullVertexList vertexList(qh.vertexList()); int i_vertex = 0; for (QhullVertexList::const_iterator v = vertexList.begin(); v != vertexList.end(); ++v) { QhullPoint pt((*v).point()); - pts_to_vertices[pt.id()] = i_vertex; + pts_to_vertices[(size_t)pt.id()] = (int)i_vertex; vertices[i_vertex] = Vec3f(pt[0], pt[1], pt[2]); ++i_vertex; } @@ -90,13 +90,13 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, convex = convex_tri = new Convex(); else convex = new ConvexBase; - convex->initialize(true, vertices, nvertex); + convex->initialize(true, vertices, static_cast(nvertex)); // Build the neighbors convex->neighbors = new Neighbors[nvertex]; - std::vector > nneighbors(nvertex); + std::vector > nneighbors(static_cast(nvertex)); if (keepTriangles) { - convex_tri->num_polygons = qh.facetCount(); + convex_tri->num_polygons = static_cast(qh.facetCount()); convex_tri->polygons = new Triangle[convex_tri->num_polygons]; convex_tri->computeCenter(); } @@ -110,18 +110,22 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, if (facet.isSimplicial()) { // In 3D, simplicial faces have 3 vertices. We mark them as neighbors. QhullVertexSet f_vertices(facet.vertices()); - int n = f_vertices.count(); + size_t n = static_cast(f_vertices.count()); assert(n == 3); - Triangle tri(pts_to_vertices[f_vertices[0].point().id()], - pts_to_vertices[f_vertices[1].point().id()], - pts_to_vertices[f_vertices[2].point().id()]); + Triangle tri( + static_cast( + pts_to_vertices[static_cast(f_vertices[0].point().id())]), + static_cast( + pts_to_vertices[static_cast(f_vertices[1].point().id())]), + static_cast(pts_to_vertices[static_cast( + f_vertices[2].point().id())])); if (keepTriangles) { reorderTriangle(convex_tri, tri); convex_tri->polygons[i_polygon++] = tri; } - for (size_type j = 0; j < n; ++j) { - size_type i = (j == 0) ? n - 1 : j - 1; - size_type k = (j == n - 1) ? 0 : j + 1; + for (size_t j = 0; j < n; ++j) { + size_t i = (j == 0) ? n - 1 : j - 1; + size_t k = (j == n - 1) ? 0 : j + 1; // Update neighbors of pj; if (nneighbors[tri[j]].insert(tri[i]).second) c_nneighbors++; if (nneighbors[tri[j]].insert(tri[k]).second) c_nneighbors++; @@ -138,11 +142,19 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, QhullRidgeSet f_ridges(facet.ridges()); for (size_type j = 0; j < f_ridges.count(); ++j) { assert(f_ridges[j].vertices().count() == 2); - index_type pi = pts_to_vertices[f_ridges[j].vertices()[0].point().id()], - pj = pts_to_vertices[f_ridges[j].vertices()[1].point().id()]; + int pi = pts_to_vertices[static_cast( + f_ridges[j].vertices()[0].point().id())], + pj = pts_to_vertices[static_cast( + f_ridges[j].vertices()[1].point().id())]; // Update neighbors of pi and pj; - if (nneighbors[pj].insert(pi).second) c_nneighbors++; - if (nneighbors[pi].insert(pj).second) c_nneighbors++; + if (nneighbors[static_cast(pj)] + .insert(static_cast(pi)) + .second) + c_nneighbors++; + if (nneighbors[static_cast(pi)] + .insert(static_cast(pj)) + .second) + c_nneighbors++; } } } @@ -151,7 +163,7 @@ ConvexBase* ConvexBase::convexHull(const Vec3f* pts, unsigned int num_points, // Fill the neighbor attribute of the returned object. convex->nneighbors_ = new unsigned int[c_nneighbors]; unsigned int* p_nneighbors = convex->nneighbors_; - for (int i = 0; i < nvertex; ++i) { + for (size_t i = 0; i < static_cast(nvertex); ++i) { Neighbors& n = convex->neighbors[i]; if (nneighbors[i].size() >= (std::numeric_limits::max)()) throw std::logic_error("Too many neighbors."); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 087327623..6056713e6 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -37,6 +37,7 @@ add_fcl_test(distance distance.cpp) add_fcl_test(distance_lower_bound distance_lower_bound.cpp) add_fcl_test(security_margin security_margin.cpp) add_fcl_test(geometric_shapes geometric_shapes.cpp) +add_fcl_test(shape_inflation shape_inflation.cpp) #add_fcl_test(shape_mesh_consistency shape_mesh_consistency.cpp) add_fcl_test(frontlist frontlist.cpp) SET_TESTS_PROPERTIES(frontlist PROPERTIES TIMEOUT 7200) diff --git a/test/bvh_models.cpp b/test/bvh_models.cpp index 38bac56cb..bb8403054 100644 --- a/test/bvh_models.cpp +++ b/test/bvh_models.cpp @@ -46,6 +46,7 @@ #include #include #include +#include #include #include #include "utility.h" @@ -370,3 +371,15 @@ BOOST_AUTO_TEST_CASE(load_illformated_mesh) { MeshLoader loader; BOOST_CHECK_NO_THROW(loader.load(filename)); } + +BOOST_AUTO_TEST_CASE(test_convex) { + Box* box_ptr = new hpp::fcl::Box(1, 1, 1); + CollisionGeometryPtr_t b1(box_ptr); + BVHModel box_bvh_model = BVHModel(); + generateBVHModel(box_bvh_model, *box_ptr, Transform3f()); + box_bvh_model.buildConvexRepresentation(false); + + box_bvh_model.convex->computeLocalAABB(); + std::shared_ptr convex_copy(box_bvh_model.convex->clone()); + BOOST_CHECK(*convex_copy.get() == *box_bvh_model.convex.get()); +} diff --git a/test/convex.cpp b/test/convex.cpp index c7e15d217..d55cab48a 100644 --- a/test/convex.cpp +++ b/test/convex.cpp @@ -225,8 +225,8 @@ BOOST_AUTO_TEST_CASE(convex_hull_quad) { Vec3f(0, 0, 1), }); - ConvexBase* convexHull = - ConvexBase::convexHull(points.data(), (int)points.size(), false, NULL); + ConvexBase* convexHull = ConvexBase::convexHull( + points.data(), (unsigned int)points.size(), false, NULL); BOOST_REQUIRE_EQUAL(convexHull->num_points, 4); BOOST_CHECK_EQUAL(convexHull->neighbors[0].count(), 3); @@ -249,8 +249,8 @@ BOOST_AUTO_TEST_CASE(convex_hull_box_like) { Vec3f(0, 0, 0.99), }); - ConvexBase* convexHull = - ConvexBase::convexHull(points.data(), (int)points.size(), false, NULL); + ConvexBase* convexHull = ConvexBase::convexHull( + points.data(), (unsigned int)points.size(), false, NULL); BOOST_REQUIRE_EQUAL(8, convexHull->num_points); for (int i = 0; i < 8; ++i) { @@ -259,8 +259,8 @@ BOOST_AUTO_TEST_CASE(convex_hull_box_like) { } delete convexHull; - convexHull = - ConvexBase::convexHull(points.data(), (int)points.size(), true, NULL); + convexHull = ConvexBase::convexHull(points.data(), + (unsigned int)points.size(), true, NULL); Convex* convex_tri = dynamic_cast*>(convexHull); BOOST_CHECK(convex_tri != NULL); diff --git a/test/gjk_convergence_criterion.cpp b/test/gjk_convergence_criterion.cpp index f1d4d5c8a..92e8c48fe 100644 --- a/test/gjk_convergence_criterion.cpp +++ b/test/gjk_convergence_criterion.cpp @@ -36,7 +36,6 @@ #include "hpp/fcl/data_types.h" #include -#include #define BOOST_TEST_MODULE FCL_NESTEROV_GJK #include diff --git a/test/hfields.cpp b/test/hfields.cpp index b16873570..bc6d8279a 100644 --- a/test/hfields.cpp +++ b/test/hfields.cpp @@ -268,6 +268,148 @@ BOOST_AUTO_TEST_CASE(building_constant_hfields) { test_constant_hfields(100, 100, min_altitude, max_altitude); } +template +void test_negative_security_margin(const Eigen::DenseIndex nx, + const Eigen::DenseIndex ny, + const FCL_REAL min_altitude, + const FCL_REAL max_altitude) { + const FCL_REAL x_dim = 1., y_dim = 2.; + const MatrixXf heights = MatrixXf::Constant(ny, nx, max_altitude); + + HeightField hfield(x_dim, y_dim, heights, min_altitude); + + // Build equivalent object + const Box equivalent_box(x_dim, y_dim, max_altitude - min_altitude); + const Transform3f box_placement( + Matrix3f::Identity(), Vec3f(0., 0., (max_altitude + min_altitude) / 2.)); + + // Test collision + const Sphere sphere(1.); + static const Transform3f IdTransform; + + const Box box(Vec3f::Ones()); + + Transform3f M_sphere, M_box; + + // No collision case + { + const FCL_REAL eps_no_collision = +0.1 * (max_altitude - min_altitude); + M_sphere.setTranslation( + Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); + M_box.setTranslation( + Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); + CollisionRequest request; + + CollisionResult result; + collide(&hfield, IdTransform, &sphere, M_sphere, request, result); + + BOOST_CHECK(!result.isCollision()); + + CollisionResult result_check_sphere; + collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, + request, result_check_sphere); + + BOOST_CHECK(!result_check_sphere.isCollision()); + + CollisionResult result_check_box; + collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, + result_check_box); + + BOOST_CHECK(!result_check_box.isCollision()); + } + + // Collision case - positive security_margin + { + const FCL_REAL eps_no_collision = +0.1 * (max_altitude - min_altitude); + M_sphere.setTranslation( + Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); + M_box.setTranslation( + Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); + CollisionRequest request; + request.security_margin = eps_no_collision + 1e-6; + + CollisionResult result; + collide(&hfield, IdTransform, &sphere, M_sphere, request, result); + + BOOST_CHECK(result.isCollision()); + + CollisionResult result_check_sphere; + collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, + request, result_check_sphere); + + BOOST_CHECK(result_check_sphere.isCollision()); + + CollisionResult result_check_box; + collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, + result_check_box); + + BOOST_CHECK(result_check_box.isCollision()); + } + + // Collision case + { + const FCL_REAL eps_no_collision = -0.1 * (max_altitude - min_altitude); + M_sphere.setTranslation( + Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); + M_box.setTranslation( + Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); + CollisionRequest request; + + CollisionResult result; + collide(&hfield, IdTransform, &sphere, M_sphere, request, result); + + BOOST_CHECK(result.isCollision()); + + CollisionResult result_check_sphere; + collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, + request, result_check_sphere); + + BOOST_CHECK(result_check_sphere.isCollision()); + + CollisionResult result_check_box; + collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, + result_check_box); + + BOOST_CHECK(result_check_box.isCollision()); + } + + // No collision case - negative security_margin + { + const FCL_REAL eps_no_collision = -0.1 * (max_altitude - min_altitude); + M_sphere.setTranslation( + Vec3f(0., 0., max_altitude + sphere.radius + eps_no_collision)); + M_box.setTranslation( + Vec3f(0., 0., max_altitude + box.halfSide[2] + eps_no_collision)); + CollisionRequest request; + request.security_margin = eps_no_collision - 1e-4; + + CollisionResult result; + collide(&hfield, IdTransform, &sphere, M_sphere, request, result); + + BOOST_CHECK(!result.isCollision()); + + CollisionResult result_check_sphere; + collide(&equivalent_box, IdTransform * box_placement, &sphere, M_sphere, + request, result_check_sphere); + + BOOST_CHECK(!result_check_sphere.isCollision()); + + CollisionResult result_check_box; + collide(&equivalent_box, IdTransform * box_placement, &box, M_box, request, + result_check_box); + + BOOST_CHECK(!result_check_box.isCollision()); + } +} + +BOOST_AUTO_TEST_CASE(negative_security_margin) { + const FCL_REAL max_altitude = 1., min_altitude = 0.; + + // test_negative_security_margin(100, 100, min_altitude, + // max_altitude); + test_negative_security_margin(100, 100, min_altitude, max_altitude); +} + BOOST_AUTO_TEST_CASE(hfield_with_square_hole) { const Eigen::DenseIndex nx = 100, ny = 100; @@ -309,3 +451,70 @@ BOOST_AUTO_TEST_CASE(hfield_with_square_hole) { BOOST_CHECK(result.isCollision()); } } + +BOOST_AUTO_TEST_CASE(hfield_with_circular_hole) { + const Eigen::DenseIndex nx = 100, ny = 100; + + // typedef OBBRSS BV; TODO(jcarpent): OBBRSS does not work (compile in Debug + // mode), as the overlap of OBBRSS is not satisfactory yet. + typedef AABB BV; + const MatrixXf X = + Eigen::RowVectorXd::LinSpaced(nx, -1., 1.).replicate(ny, 1); + const MatrixXf Y = Eigen::VectorXd::LinSpaced(ny, 1., -1.).replicate(1, nx); + + const FCL_REAL dim_hole = 1; + + const Eigen::Array hole = + (X.array().square() + Y.array().square() <= dim_hole); + + const MatrixXf heights = + MatrixXf::Ones(ny, nx) - hole.cast().matrix(); + + const HeightField hfield(2., 2., heights, -10.); + + BOOST_CHECK(hfield.getXGrid()[0] == -1.); + BOOST_CHECK(hfield.getXGrid()[nx - 1] == +1.); + + BOOST_CHECK(hfield.getYGrid()[0] == +1.); + BOOST_CHECK(hfield.getYGrid()[ny - 1] == -1.); + + Sphere sphere(0.975); + const Transform3f sphere_pos(Vec3f(0., 0., 1.)); + const Transform3f hfield_pos; + + { + CollisionResult result; + CollisionRequest request; + request.security_margin = 0.; + collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); + + BOOST_CHECK(!result.isCollision()); + } + + { + CollisionResult result; + CollisionRequest request; + request.security_margin = 0.01; + collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); + + BOOST_CHECK(!result.isCollision()); + } + + { + CollisionResult result; + CollisionRequest request; + request.security_margin = 1. - sphere.radius; + collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); + + BOOST_CHECK(result.isCollision()); + } + + { + CollisionResult result; + CollisionRequest request; + request.security_margin = -0.005; + collide(&hfield, hfield_pos, &sphere, sphere_pos, request, result); + + BOOST_CHECK(!result.isCollision()); + } +} diff --git a/test/nesterov_gjk.cpp b/test/nesterov_gjk.cpp index 9f3efa52e..a75aeb157 100644 --- a/test/nesterov_gjk.cpp +++ b/test/nesterov_gjk.cpp @@ -74,11 +74,9 @@ BOOST_AUTO_TEST_CASE(set_gjk_variant) { // Checking set solver.gjk_variant = GJKVariant::NesterovAcceleration; gjk.gjk_variant = GJKVariant::NesterovAcceleration; - shape.setNormalizeSupportDirection(true); BOOST_CHECK(solver.gjk_variant == GJKVariant::NesterovAcceleration); BOOST_CHECK(gjk.gjk_variant == GJKVariant::NesterovAcceleration); - BOOST_CHECK(shape.normalize_support_direction == true); } BOOST_AUTO_TEST_CASE(need_nesterov_normalize_support_direction) { diff --git a/test/python_unit/CMakeLists.txt b/test/python_unit/CMakeLists.txt index 4cd4777a8..6bad8ec8a 100644 --- a/test/python_unit/CMakeLists.txt +++ b/test/python_unit/CMakeLists.txt @@ -4,6 +4,7 @@ SET(${PROJECT_NAME}_PYTHON_TESTS collision ) +ADD_DEPENDENCIES(build_tests hppfcl) FOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS}) ADD_PYTHON_UNIT_TEST("py-${TEST}" "test/python_unit/${TEST}.py" "python") ENDFOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS}) diff --git a/test/security_margin.cpp b/test/security_margin.cpp index 1647a4b7d..64963b469 100644 --- a/test/security_margin.cpp +++ b/test/security_margin.cpp @@ -42,6 +42,8 @@ #include #include #include +#include +#include #include "utility.h" @@ -55,6 +57,129 @@ using hpp::fcl::DistanceResult; using hpp::fcl::Transform3f; using hpp::fcl::Vec3f; +#define MATH_SQUARED(x) (x * x) + +BOOST_AUTO_TEST_CASE(aabb_aabb) { + CollisionGeometryPtr_t b1(new hpp::fcl::Box(1, 1, 1)); + CollisionGeometryPtr_t b2(new hpp::fcl::Box(1, 1, 1)); + + const Transform3f tf1; + const Transform3f tf2_collision(Vec3f(0, 1, 1)); + hpp::fcl::Box s1(1, 1, 1); + hpp::fcl::Box s2(1, 1, 1); + const double tol = 1e-8; + + AABB bv1, bv2; + computeBV(s1, Transform3f(), bv1); + computeBV(s2, Transform3f(), bv2); + + // No security margin - collision + { + CollisionRequest collisionRequest(CONTACT, 1); + AABB bv2_transformed; + computeBV(s2, tf2_collision, bv2_transformed); + FCL_REAL sqrDistLowerBound; + bool res = + bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); + BOOST_CHECK(res); + BOOST_CHECK_CLOSE(sqrDistLowerBound, 0, tol); + } + + // No security margin - no collision + { + CollisionRequest collisionRequest(CONTACT, 1); + const double distance = 0.01; + Transform3f tf2_no_collision( + Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance))); + AABB bv2_transformed; + computeBV(s2, tf2_no_collision, bv2_transformed); + FCL_REAL sqrDistLowerBound; + bool res = + bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); + BOOST_CHECK(!res); + BOOST_CHECK_CLOSE(sqrDistLowerBound, MATH_SQUARED(distance), tol); + } + + // Security margin - collision + { + CollisionRequest collisionRequest(CONTACT, 1); + const double distance = 0.01; + collisionRequest.security_margin = distance; + Transform3f tf2_no_collision( + Vec3f(tf2_collision.getTranslation() + Vec3f(0, 0, distance))); + AABB bv2_transformed; + computeBV(s2, tf2_no_collision, bv2_transformed); + FCL_REAL sqrDistLowerBound; + bool res = + bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); + BOOST_CHECK(res); + BOOST_CHECK_SMALL(sqrDistLowerBound, tol); + } + + // Negative security margin - collion because the two boxes are in contact + { + CollisionRequest collisionRequest(CONTACT, 1); + const double distance = -0.01; + collisionRequest.security_margin = distance; + const Transform3f tf2( + Vec3f(tf2_collision.getTranslation() + Vec3f(0, distance, distance))); + AABB bv2_transformed; + computeBV(s2, tf2, bv2_transformed); + FCL_REAL sqrDistLowerBound; + bool res = + bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); + BOOST_CHECK(res); + BOOST_CHECK_SMALL(sqrDistLowerBound, tol); + } + + // Negative security margin - no collision + { + CollisionRequest collisionRequest(CONTACT, 1); + const double distance = -0.01; + collisionRequest.security_margin = distance; + AABB bv2_transformed; + computeBV(s2, tf2_collision, bv2_transformed); + FCL_REAL sqrDistLowerBound; + bool res = + bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); + BOOST_CHECK(!res); + BOOST_CHECK_CLOSE( + sqrDistLowerBound, + MATH_SQUARED((std::sqrt(2) * collisionRequest.security_margin)), tol); + } +} + +BOOST_AUTO_TEST_CASE(aabb_aabb_degenerated_cases) { + CollisionGeometryPtr_t b1(new hpp::fcl::Box(1, 1, 1)); + CollisionGeometryPtr_t b2(new hpp::fcl::Box(1, 1, 1)); + + const Transform3f tf1; + const Transform3f tf2_collision(Vec3f(0, 0, 0)); + hpp::fcl::Box s1(1, 1, 1); + hpp::fcl::Box s2(1, 1, 1); + const double tol = 1e-8; + + AABB bv1, bv2; + computeBV(s1, Transform3f(), bv1); + computeBV(s2, Transform3f(), bv2); + + // The two AABB are collocated + { + CollisionRequest collisionRequest(CONTACT, 1); + const double distance = -2.; + collisionRequest.security_margin = distance; + AABB bv2_transformed; + computeBV(s2, tf2_collision, bv2_transformed); + FCL_REAL sqrDistLowerBound; + bool res = + bv1.overlap(bv2_transformed, collisionRequest, sqrDistLowerBound); + BOOST_CHECK(!res); + } + + const AABB bv3; + BOOST_CHECK(!bv1.overlap(bv3)); +} + BOOST_AUTO_TEST_CASE(sphere_sphere) { CollisionGeometryPtr_t s1(new hpp::fcl::Sphere(1)); CollisionGeometryPtr_t s2(new hpp::fcl::Sphere(2)); @@ -86,7 +211,7 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, distance, 1e-8); } - // Positive security margin + // Positive security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); CollisionResult collisionResult; @@ -125,7 +250,8 @@ BOOST_AUTO_TEST_CASE(sphere_sphere) { collide(s1.get(), tf1, s2.get(), tf2_collision, collisionRequest, collisionResult); BOOST_CHECK(!collisionResult.isCollision()); - BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, 0.01, 1e-8); + BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, + -collisionRequest.security_margin, 1e-8); } } @@ -208,7 +334,7 @@ BOOST_AUTO_TEST_CASE(box_box) { CollisionGeometryPtr_t b2(new hpp::fcl::Box(1, 1, 1)); const Transform3f tf1; - const Transform3f tf2_collision(Vec3f(0, 0, 1)); + const Transform3f tf2_collision(Vec3f(0, 1, 1)); const double tol = 1e-3; @@ -251,7 +377,7 @@ BOOST_AUTO_TEST_CASE(box_box) { BOOST_CHECK_SMALL(-collisionResult.getContact(0).penetration_depth, 1e-8); } - // Positive security margin - no collision + // Negative security margin - no collision { CollisionRequest collisionRequest(CONTACT, 1); collisionRequest.security_margin = -0.01; @@ -266,16 +392,113 @@ BOOST_AUTO_TEST_CASE(box_box) { // Negative security margin - collision { CollisionRequest collisionRequest(CONTACT, 1); - collisionRequest.security_margin = -0.01; + const FCL_REAL distance = -0.01; + collisionRequest.security_margin = distance; CollisionResult collisionResult; const Transform3f tf2((tf2_collision.getTranslation() + - Vec3f(0, 0, collisionRequest.security_margin)) + Vec3f(0, collisionRequest.security_margin, + collisionRequest.security_margin)) .eval()); collide(b1.get(), tf1, b2.get(), tf2, collisionRequest, collisionResult); BOOST_CHECK(collisionResult.isCollision()); BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, tol); BOOST_CHECK_CLOSE(-collisionResult.getContact(0).penetration_depth, - collisionRequest.security_margin, tol); + distance, tol); + } +} + +template +void test_shape_shape(const ShapeType1& shape1, const Transform3f& tf1, + const ShapeType2& shape2, + const Transform3f& tf2_collision, const FCL_REAL tol) { + // No security margin - collision + { + CollisionRequest collisionRequest(CONTACT, 1); + CollisionResult collisionResult; + collide(&shape1, tf1, &shape2, tf2_collision, collisionRequest, + collisionResult); + BOOST_CHECK(collisionResult.isCollision()); + BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, tol); + BOOST_CHECK_SMALL(-collisionResult.getContact(0).penetration_depth, 1e-8); + } + + // No security margin - no collision + { + CollisionRequest collisionRequest(CONTACT, 1); + const double distance = 0.01; + const Transform3f tf2_no_collision( + (tf2_collision.getTranslation() + Vec3f(0, 0, distance)).eval()); + + CollisionResult collisionResult; + collide(&shape1, tf1, &shape2, tf2_no_collision, collisionRequest, + collisionResult); + BOOST_CHECK(!collisionResult.isCollision()); + BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, distance, tol); + } + + // Positive security margin - collision + { + CollisionRequest collisionRequest(CONTACT, 1); + const double distance = 0.01; + collisionRequest.security_margin = distance; + CollisionResult collisionResult; + collide(&shape1, tf1, &shape2, tf2_collision, collisionRequest, + collisionResult); + BOOST_CHECK(collisionResult.isCollision()); + BOOST_CHECK_CLOSE(collisionResult.distance_lower_bound, + -collisionRequest.security_margin, tol); + BOOST_CHECK_SMALL(-collisionResult.getContact(0).penetration_depth, 1e-8); + } + + // Negative security margin - no collision + { + CollisionRequest collisionRequest(CONTACT, 1); + collisionRequest.security_margin = -0.01; + CollisionResult collisionResult; + collide(&shape1, tf1, &shape2, tf2_collision, collisionRequest, + collisionResult); + BOOST_CHECK(!collisionResult.isCollision()); + BOOST_CHECK_CLOSE( + collisionResult.distance_lower_bound, + (collisionRequest.security_margin * tf2_collision.getTranslation()) + .norm(), + tol); + } + + // Negative security margin - collision + { + CollisionRequest collisionRequest(CONTACT, 1); + const FCL_REAL distance = -0.01; + collisionRequest.security_margin = distance; + CollisionResult collisionResult; + + const Transform3f tf2((tf2_collision.getTranslation() + + Vec3f(0, collisionRequest.security_margin, + collisionRequest.security_margin)) + .eval()); + collide(&shape1, tf1, &shape2, tf2, collisionRequest, collisionResult); + BOOST_CHECK(collisionResult.isCollision()); + BOOST_CHECK_SMALL(collisionResult.distance_lower_bound, tol); + BOOST_CHECK_CLOSE(-collisionResult.getContact(0).penetration_depth, + distance, tol); } } + +BOOST_AUTO_TEST_CASE(sphere_box) { + Box* box_ptr = new hpp::fcl::Box(1, 1, 1); + CollisionGeometryPtr_t b1(box_ptr); + BVHModel box_bvh_model = BVHModel(); + generateBVHModel(box_bvh_model, *box_ptr, Transform3f()); + box_bvh_model.buildConvexRepresentation(false); + ConvexBase& box_convex = *box_bvh_model.convex.get(); + CollisionGeometryPtr_t s2(new hpp::fcl::Sphere(0.5)); + + const Transform3f tf1; + const Transform3f tf2_collision(Vec3f(0, 0, 1)); + + const double tol = 1e-6; + + test_shape_shape(*b1.get(), tf1, *s2.get(), tf2_collision, tol); + test_shape_shape(box_convex, tf1, *s2.get(), tf2_collision, tol); +} diff --git a/test/shape_inflation.cpp b/test/shape_inflation.cpp new file mode 100644 index 000000000..e305906bd --- /dev/null +++ b/test/shape_inflation.cpp @@ -0,0 +1,198 @@ +/* + * Software License Agreement (BSD License) + * + * Copyright (c) 2022, INRIA. + * 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 Willow Garage, Inc. 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. + */ + +#define BOOST_TEST_MODULE FCL_SECURITY_MARGIN +#include + +#include +#include +#include +#include +#include +#include +#include +#include + +#include "utility.h" + +using namespace hpp::fcl; +using hpp::fcl::CollisionGeometryPtr_t; +using hpp::fcl::CollisionObject; +using hpp::fcl::CollisionRequest; +using hpp::fcl::CollisionResult; +using hpp::fcl::DistanceRequest; +using hpp::fcl::DistanceResult; +using hpp::fcl::Transform3f; +using hpp::fcl::Vec3f; + +#define MATH_SQUARED(x) (x * x) + +template +bool isApprox(const Shape &s1, const Shape &s2, const FCL_REAL tol); + +bool isApprox(const FCL_REAL &v1, const FCL_REAL &v2, const FCL_REAL tol) { + typedef Eigen::Matrix ScalarMatrix; + ScalarMatrix m1; + m1 << v1; + ScalarMatrix m2; + m2 << v2; + return m1.isApprox(m2, tol); +} + +bool isApprox(const Box &s1, const Box &s2, const FCL_REAL tol) { + return s1.halfSide.isApprox(s2.halfSide, tol); +} + +bool isApprox(const Sphere &s1, const Sphere &s2, const FCL_REAL tol) { + return isApprox(s1.radius, s2.radius, tol); +} + +bool isApprox(const Ellipsoid &s1, const Ellipsoid &s2, const FCL_REAL tol) { + return s1.radii.isApprox(s2.radii, tol); +} + +bool isApprox(const Capsule &s1, const Capsule &s2, const FCL_REAL tol) { + return isApprox(s1.radius, s2.radius, tol) && + isApprox(s1.halfLength, s2.halfLength, tol); +} + +bool isApprox(const Cylinder &s1, const Cylinder &s2, const FCL_REAL tol) { + return isApprox(s1.radius, s2.radius, tol) && + isApprox(s1.halfLength, s2.halfLength, tol); +} + +bool isApprox(const Cone &s1, const Cone &s2, const FCL_REAL tol) { + return isApprox(s1.radius, s2.radius, tol) && + isApprox(s1.halfLength, s2.halfLength, tol); +} + +bool isApprox(const TriangleP &s1, const TriangleP &s2, const FCL_REAL tol) { + return s1.a.isApprox(s2.a, tol) && s1.b.isApprox(s2.b, tol) && + s1.c.isApprox(s2.c, tol); +} + +bool isApprox(const Halfspace &s1, const Halfspace &s2, const FCL_REAL tol) { + return isApprox(s1.d, s2.d, tol) && s1.n.isApprox(s2.n, tol); +} + +template +void test(const Shape &original_shape, const FCL_REAL inflation, + const FCL_REAL tol = 1e-8) { + // Zero inflation + { + const FCL_REAL inflation = 0.; + const auto &inflation_result = original_shape.inflated(inflation); + const Transform3f &shift = inflation_result.second; + const Shape &inflated_shape = inflation_result.first; + + BOOST_CHECK(isApprox(original_shape, inflated_shape, tol)); + BOOST_CHECK(shift.isIdentity(tol)); + } + + // Positive inflation + { + const auto &inflation_result = original_shape.inflated(inflation); + const Shape &inflated_shape = inflation_result.first; + const Transform3f &inflation_shift = inflation_result.second; + + BOOST_CHECK(!isApprox(original_shape, inflated_shape, tol)); + + const auto &deflation_result = inflated_shape.inflated(-inflation); + const Shape &deflated_shape = deflation_result.first; + const Transform3f &deflation_shift = deflation_result.second; + + BOOST_CHECK(isApprox(original_shape, deflated_shape, tol)); + BOOST_CHECK((inflation_shift * deflation_shift).isIdentity(tol)); + } + + // Negative inflation + { + const auto &inflation_result = original_shape.inflated(-inflation); + const Shape &inflated_shape = inflation_result.first; + const Transform3f &inflation_shift = inflation_result.second; + + BOOST_CHECK(!isApprox(original_shape, inflated_shape, tol)); + + const auto &deflation_result = inflated_shape.inflated(+inflation); + const Shape &deflated_shape = deflation_result.first; + const Transform3f &deflation_shift = deflation_result.second; + + BOOST_CHECK(isApprox(original_shape, deflated_shape, tol)); + BOOST_CHECK((inflation_shift * deflation_shift).isIdentity(tol)); + } +} + +template +void test_throw(const Shape &shape, const FCL_REAL inflation) { + BOOST_REQUIRE_THROW(shape.inflated(inflation), std::invalid_argument); +} + +template +void test_no_throw(const Shape &shape, const FCL_REAL inflation) { + BOOST_REQUIRE_NO_THROW(shape.inflated(inflation)); +} + +BOOST_AUTO_TEST_CASE(test_inflate) { + const hpp::fcl::Sphere sphere(1); + test(sphere, 0.01, 1e-8); + test_throw(sphere, -1.1); + test_no_throw(sphere, 1.); + + const hpp::fcl::Box box(1, 1, 1); + test(box, 0.01, 1e-8); + test_throw(box, -0.6); + + const hpp::fcl::Ellipsoid ellipsoid(1, 2, 3); + test(ellipsoid, 0.01, 1e-8); + test_throw(ellipsoid, -1.1); + + const hpp::fcl::Capsule capsule(1., 2.); + test(capsule, 0.01, 1e-8); + test_throw(capsule, -1.1); + + const hpp::fcl::Cylinder cylinder(1., 2.); + test(cylinder, 0.01, 1e-8); + test_throw(cylinder, -1.1); + + const hpp::fcl::Cone cone(1., 4.); + test(cone, 0.01, 1e-8); + test_throw(cone, -1.1); + + const hpp::fcl::Halfspace halfspace(Vec3f::UnitZ(), 0.); + test(halfspace, 0.01, 1e-8); + + // const hpp::fcl::TriangleP triangle(Vec3f::UnitX(), Vec3f::UnitY(), + // Vec3f::UnitZ()); + // test(triangle, 0.01, 1e-8); +}