diff --git a/src/distance_capsule_capsule.cpp b/src/distance_capsule_capsule.cpp
index 029c23680cddf7b9771990228e93dd909077f3b4..aa870d7ca640166229e54770aa06d6751d8d1d80 100644
--- a/src/distance_capsule_capsule.cpp
+++ b/src/distance_capsule_capsule.cpp
@@ -1,10 +1,35 @@
-// Geometric Tools, LLC
-// Copyright (c) 1998-2011
-// Distributed under the Boost Software License, Version 1.0.
-// http://www.boost.org/LICENSE_1_0.txt
-// http://www.geometrictools.com/License/Boost/LICENSE_1_0.txt
-//
-// Modified by Florent Lamiraux 2014
+/*
+ * Software License Agreement (BSD License)
+ * Copyright (c) 2015-2019, CNRS - LAAS 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.
+ */
#include <cmath>
#include <limits>
@@ -12,6 +37,8 @@
#include <hpp/fcl/shape/geometric_shapes.h>
#include <../src/distance_func_matrix.h>
+#define CLAMP(value, l, u) fmax(fmin(value, u), l)
+
// Note that partial specialization of template functions is not allowed.
// Therefore, two implementations with the default narrow phase solvers are
// provided. If another narrow phase solver were to be used, the default
@@ -25,299 +52,110 @@ namespace hpp
namespace fcl {
class GJKSolver;
+
+ // Compute the distance between C1 and C2 by computing the distance
+ // between the two segments supporting the capsules.
+ // Match algorithm of Real-Time Collision Detection, Christer Ericson - Closest Point of Two Line Segments
template <>
- FCL_REAL ShapeShapeDistance <Capsule, Capsule>
- (const CollisionGeometry* o1, const Transform3f& tf1,
+ FCL_REAL ShapeShapeDistance <Capsule, Capsule> (const CollisionGeometry* o1, const Transform3f& tf1,
const CollisionGeometry* o2, const Transform3f& tf2,
const GJKSolver*, const DistanceRequest& request,
DistanceResult& result)
{
- const Capsule* c1 = static_cast <const Capsule*> (o1);
- const Capsule* c2 = static_cast <const Capsule*> (o2);
+ const Capsule* capsule1 = static_cast <const Capsule*> (o1);
+ const Capsule* capsule2 = static_cast <const Capsule*> (o2);
+
+ FCL_REAL EPSILON = std::numeric_limits<FCL_REAL>::epsilon () * 100;
// We assume that capsules are centered at the origin.
- const fcl::Vec3f& center1 = tf1.getTranslation ();
- const fcl::Vec3f& center2 = tf2.getTranslation ();
+ const fcl::Vec3f& c1 = tf1.getTranslation ();
+ const fcl::Vec3f& c2 = tf2.getTranslation ();
// We assume that capsules are oriented along z-axis.
- Matrix3f::ConstColXpr direction1 = tf1.getRotation ().col (2);
- Matrix3f::ConstColXpr direction2 = tf2.getRotation ().col (2);
- FCL_REAL halfLength1 = c1->halfLength;
- FCL_REAL halfLength2 = c2->halfLength;
+ FCL_REAL halfLength1 = capsule1->halfLength;
+ FCL_REAL halfLength2 = capsule2->halfLength;
+ FCL_REAL radius1 = capsule1->radius;
+ FCL_REAL radius2 = capsule2->radius;
+ // direction of capsules
+ // ||d1|| = 2 * halfLength1
+ const fcl::Vec3f d1 = 2 * halfLength1 * tf1.getRotation().col(2);
+ const fcl::Vec3f d2 = 2 * halfLength2 * tf2.getRotation().col(2);
- Vec3f diff = center1 - center2;
- FCL_REAL a01 = -direction1.dot (direction2);
- FCL_REAL b0 = diff.dot (direction1);
- FCL_REAL b1 = -diff.dot (direction2);
- FCL_REAL c = diff.squaredNorm ();
- FCL_REAL det = fabs (1.0 - a01*a01);
- FCL_REAL s1, s2, sqrDist, extDet0, extDet1, tmpS0, tmpS1;
- FCL_REAL epsilon = std::numeric_limits<FCL_REAL>::epsilon () * 100;
+ // Starting point of the segments
+ // p1 + d1 is the end point of the segment
+ const fcl::Vec3f p1 = c1 - d1 / 2;
+ const fcl::Vec3f p2 = c2 - d2 / 2;
+ const fcl::Vec3f r = p1-p2;
+ FCL_REAL a = d1.dot(d1);
+ FCL_REAL b = d1.dot(d2);
+ FCL_REAL c = d1.dot(r);
+ FCL_REAL e = d2.dot(d2);
+ FCL_REAL f = d2.dot(r);
+ // S1 is parametrized by the equation p1 + s * d1
+ // S2 is parametrized by the equation p2 + t * d2
+ FCL_REAL s = 0.0;
+ FCL_REAL t = 0.0;
- if (det >= epsilon) {
- // Segments are not parallel.
- s1 = a01*b1 - b0;
- s2 = a01*b0 - b1;
- extDet0 = halfLength1*det;
- extDet1 = halfLength2*det;
+ if (a <= EPSILON && e <= EPSILON)
+ {
+ // Check if the segments degenerate into points
+ s = t = 0.0;
+ }
+ else if (a <= EPSILON)
+ {
+ // First segment is degenerated
+ s = 0.0;
+ t = CLAMP(f / e, 0.0, 1.0);
+ }
+ else if (e <= EPSILON)
+ {
+ // Second segment is degenerated
+ s = CLAMP(-c / a, 0.0, 1.0);
+ t = 0.0;
+ }
+ else
+ {
+ // Always non-negative, equal 0 if the segments are colinear
+ FCL_REAL denom = fmax(a*e-b*b, 0);
- if (s1 >= -extDet0) {
- if (s1 <= extDet0) {
- if (s2 >= -extDet1) {
- if (s2 <= extDet1) { // region 0 (interior)
- // Minimum at interior points of segments.
- FCL_REAL invDet = (1.0)/det;
- s1 *= invDet;
- s2 *= invDet;
- sqrDist = s1*(s1 + a01*s2 + 2.0*b0) +
- s2*(a01*s1 + s2 + 2.0*b1) + c;
- }
- else { // region 3 (side)
- s2 = halfLength2;
- tmpS0 = -(a01*s2 + b0);
- if (tmpS0 < -halfLength1) {
- s1 = -halfLength1;
- sqrDist = s1*(s1 - 2.0*tmpS0) +
- s2*(s2 + 2.0*b1) + c;
- }
- else if (tmpS0 <= halfLength1) {
- s1 = tmpS0;
- sqrDist = -s1*s1 + s2*(s2 + 2.0*b1) + c;
- }
- else {
- s1 = halfLength1;
- sqrDist = s1*(s1 - 2.0*tmpS0) +
- s2*(s2 + 2.0*b1) + c;
- }
- }
- }
- else { // region 7 (side)
- s2 = -halfLength2;
- tmpS0 = -(a01*s2 + b0);
- if (tmpS0 < -halfLength1) {
- s1 = -halfLength1;
- sqrDist = s1*(s1 - 2.0*tmpS0) +
- s2*(s2 + 2.0*b1) + c;
- } else if (tmpS0 <= halfLength1) {
- s1 = tmpS0;
- sqrDist = -s1*s1 + s2*(s2 + 2.0*b1) + c;
- } else {
- s1 = halfLength1;
- sqrDist = s1*(s1 - 2.0*tmpS0) +
- s2*(s2 + 2.0*b1) + c;
- }
- }
- }
- else {
- if (s2 >= -extDet1) {
- if (s2 <= extDet1) { // region 1 (side)
- s1 = halfLength1;
- tmpS1 = -(a01*s1 + b1);
- if (tmpS1 < -halfLength2) {
- s2 = -halfLength2;
- sqrDist = s2*(s2 - 2.0*tmpS1) +
- s1*(s1 + 2.0*b0) + c;
- }
- else if (tmpS1 <= halfLength2) {
- s2 = tmpS1;
- sqrDist = -s2*s2 + s1*(s1 + 2.0*b0) + c;
- }
- else {
- s2 = halfLength2;
- sqrDist = s2*(s2 - 2.0*tmpS1) +
- s1*(s1 + 2.0*b0) + c;
- }
- }
- else { // region 2 (corner)
- s2 = halfLength2;
- tmpS0 = -(a01*s2 + b0);
- if (tmpS0 < -halfLength1) {
- s1 = -halfLength1;
- sqrDist = s1*(s1 - 2.0*tmpS0) +
- s2*(s2 + 2.0*b1) + c;
- }
- else if (tmpS0 <= halfLength1) {
- s1 = tmpS0;
- sqrDist = -s1*s1 + s2*(s2 + 2.0*b1) + c;
- }
- else {
- s1 = halfLength1;
- tmpS1 = -(a01*s1 + b1);
- if (tmpS1 < -halfLength2) {
- s2 = -halfLength2;
- sqrDist = s2*(s2 - 2.0*tmpS1) +
- s1*(s1 + 2.0*b0) + c;
- }
- else if (tmpS1 <= halfLength2) {
- s2 = tmpS1;
- sqrDist = -s2*s2 + s1*(s1 + 2.0*b0) + c;
- }
- else {
- s2 = halfLength2;
- sqrDist = s2*(s2 - 2.0*tmpS1) +
- s1*(s1 + 2.0*b0) + c;
- }
- }
- }
- }
- else { // region 8 (corner)
- s2 = -halfLength2;
- tmpS0 = -(a01*s2 + b0);
- if (tmpS0 < -halfLength1) {
- s1 = -halfLength1;
- sqrDist = s1*(s1 - 2.0*tmpS0) +
- s2*(s2 + 2.0*b1) + c;
- }
- else if (tmpS0 <= halfLength1) {
- s1 = tmpS0;
- sqrDist = -s1*s1 + s2*(s2 + 2.0*b1) + c;
- }
- else {
- s1 = halfLength1;
- tmpS1 = -(a01*s1 + b1);
- if (tmpS1 > halfLength2) {
- s2 = halfLength2;
- sqrDist = s2*(s2 - 2.0*tmpS1) +
- s1*(s1 + 2.0*b0) + c;
- }
- else if (tmpS1 >= -halfLength2) {
- s2 = tmpS1;
- sqrDist = -s2*s2 + s1*(s1 + 2.0*b0) + c;
- }
- else {
- s2 = -halfLength2;
- sqrDist = s2*(s2 - 2.0*tmpS1) +
- s1*(s1 + 2.0*b0) + c;
- }
- }
- }
- }
+ if (denom > EPSILON)
+ {
+ s = CLAMP((b*f-c*e) / denom, 0.0, 1.0);
}
- else {
- if (s2 >= -extDet1) {
- if (s2 <= extDet1) { // region 5 (side)
- s1 = -halfLength1;
- tmpS1 = -(a01*s1 + b1);
- if (tmpS1 < -halfLength2) {
- s2 = -halfLength2;
- sqrDist = s2*(s2 - 2.0*tmpS1) +
- s1*(s1 + 2.0*b0) + c;
- }
- else if (tmpS1 <= halfLength2) {
- s2 = tmpS1;
- sqrDist = -s2*s2 + s1*(s1 + 2.0*b0) + c;
- }
- else {
- s2 = halfLength2;
- sqrDist = s2*(s2 - 2.0*tmpS1) +
- s1*(s1 + 2.0*b0) + c;
- }
- }
- else { // region 4 (corner)
- s2 = halfLength2;
- tmpS0 = -(a01*s2 + b0);
- if (tmpS0 > halfLength1) {
- s1 = halfLength1;
- sqrDist = s1*(s1 - 2.0*tmpS0) +
- s2*(s2 + 2.0*b1) + c;
- }
- else if (tmpS0 >= -halfLength1) {
- s1 = tmpS0;
- sqrDist = -s1*s1 + s2*(s2 + 2.0*b1) + c;
- }
- else {
- s1 = -halfLength1;
- tmpS1 = -(a01*s1 + b1);
- if (tmpS1 < -halfLength2) {
- s2 = -halfLength2;
- sqrDist = s2*(s2 - 2.0*tmpS1) +
- s1*(s1 + 2.0*b0) + c;
- }
- else if (tmpS1 <= halfLength2) {
- s2 = tmpS1;
- sqrDist = -s2*s2 + s1*(s1 + 2.0*b0) + c;
- }
- else {
- s2 = halfLength2;
- sqrDist = s2*(s2 - 2.0*tmpS1) +
- s1*(s1 + 2.0*b0) + c;
- }
- }
- }
- }
- else { // region 6 (corner)
- s2 = -halfLength2;
- tmpS0 = -(a01*s2 + b0);
- if (tmpS0 > halfLength1) {
- s1 = halfLength1;
- sqrDist = s1*(s1 - 2.0*tmpS0) +
- s2*(s2 + 2.0*b1) + c;
- }
- else if (tmpS0 >= -halfLength1) {
- s1 = tmpS0;
- sqrDist = -s1*s1 + s2*(s2 + 2.0*b1) + c;
- }
- else {
- s1 = -halfLength1;
- tmpS1 = -(a01*s1 + b1);
- if (tmpS1 < -halfLength2) {
- s2 = -halfLength2;
- sqrDist = s2*(s2 - 2.0*tmpS1) +
- s1*(s1 + 2.0*b0) + c;
- }
- else if (tmpS1 <= halfLength2) {
- s2 = tmpS1;
- sqrDist = -s2*s2 + s1*(s1 + 2.0*b0) + c;
- }
- else {
- s2 = halfLength2;
- sqrDist = s2*(s2 - 2.0*tmpS1) +
- s1*(s1 + 2.0*b0) + c;
- }
- }
- }
+ else
+ {
+ s = 0.0;
}
- }
- else {
- // The segments are parallel. The average b0 term is designed to
- // ensure symmetry of the function. That is, dist(seg0,seg1) and
- // dist(seg1,seg0) should produce the same number.
- FCL_REAL e0pe1 = halfLength1 + halfLength2;
- FCL_REAL sign = (a01 > 0.0 ? -1.0 : 1.0);
- FCL_REAL b0Avr = (0.5)*(b0 - sign*b1);
- FCL_REAL lambda = -b0Avr;
- if (lambda < -e0pe1) {
- lambda = -e0pe1;
+
+ t = (b*s + f) / e;
+ if (t < 0.0)
+ {
+ t = 0.0;
+ s = CLAMP(-c / a, 0.0, 1.0);
}
- else if (lambda > e0pe1) {
- lambda = e0pe1;
+ else if (t > 1.0)
+ {
+ t = 1.0;
+ s = CLAMP((b - c)/a, 0.0, 1.0);
}
-
- s2 = -sign*lambda*halfLength2/e0pe1;
- s1 = lambda + sign*s2;
- sqrDist = lambda*(lambda + 2.0*b0Avr) + c;
}
- Vec3f closestOnSegment1 = center1 + s1*direction1;
- Vec3f closestOnSegment2 = center2 + s2*direction2;
+ // witness points achieving the distance between the two segments
+ const Vec3f w1 = p1 + s * d1;
+ const Vec3f w2 = p2 + t * d2;
+ FCL_REAL distance = (w1 - w2).norm();
+ Vec3f normal = (w1 - w2) / distance;
+ result.normal = normal;
- Vec3f unitVector = closestOnSegment2 - closestOnSegment1;
- FCL_REAL distance = sqrt (sqrDist);
- if (distance >= epsilon) {
- unitVector /= distance;
- } else {
- unitVector.setZero ();
+ // capsule spcecific distance computation
+ distance = distance - (radius1 + radius2);
+ result.min_distance = distance;
+ // witness points for the capsules
+ if (request.enable_nearest_points)
+ {
+ result.nearest_points[0] = w1 - radius1 * normal;
+ result.nearest_points[1] = w2 + radius2 * normal;
}
- // Update distance result.
- result.min_distance = distance - c1->radius - c2->radius;
- if (request.enable_nearest_points) {
- result.nearest_points [0] = closestOnSegment1 + c1->radius * unitVector;
- result.nearest_points [1] = closestOnSegment2 - c2->radius * unitVector;
- }
- result.o1 = o1;
- result.o2 = o2;
- result.b1 = -1;
- result.b2 = -1;
- return result.min_distance;
+ return distance;
}
template <>
@@ -334,18 +172,22 @@ namespace fcl {
FCL_REAL distance = ShapeShapeDistance <Capsule, Capsule>
(o1, tf1, o2, tf2, unused, distanceRequest, distanceResult);
- if (distance <= 0) {
- Contact contact (o1, o2, distanceResult.b1, distanceResult.b2);
+ if (distance > 0)
+ {
+ return 0;
+ }
+ else
+ {
+ Contact contact (o1, o2, -1, -1);
const Vec3f& p1 = distanceResult.nearest_points [0];
const Vec3f& p2 = distanceResult.nearest_points [1];
- contact.pos = .5*(p1+p2);
- contact.normal = (p2-p1)/(p2-p1).norm ();
- result.addContact (contact);
+ contact.pos = 0.5 * (p1+p2);
+ contact.normal = distanceResult.normal;
+ result.addContact(contact);
return 1;
}
- result.distance_lower_bound = distance;
- return 0;
}
+
} // namespace fcl
} // namespace hpp
diff --git a/test/capsule_capsule.cpp b/test/capsule_capsule.cpp
index 7dda5376b31576e3fefee5dadbe8c2232d810841..ef1effd3fedf30037d3caeb284956193b888db53 100644
--- a/test/capsule_capsule.cpp
+++ b/test/capsule_capsule.cpp
@@ -45,6 +45,7 @@
#include <cmath>
#include <iostream>
#include <hpp/fcl/distance.h>
+#include <hpp/fcl/collision.h>
#include <hpp/fcl/math/transform.h>
#include <hpp/fcl/collision.h>
#include <hpp/fcl/collision_object.h>
@@ -55,6 +56,163 @@
using namespace hpp::fcl;
typedef boost::shared_ptr <CollisionGeometry> CollisionGeometryPtr_t;
+BOOST_AUTO_TEST_CASE(collision_capsule_capsule_trivial)
+{
+ const double radius = 1.;
+
+ CollisionGeometryPtr_t c1 (new Capsule (radius, 0.));
+ CollisionGeometryPtr_t c2 (new Capsule (radius, 0.));
+
+ CollisionGeometryPtr_t s1 (new Sphere (radius));
+ CollisionGeometryPtr_t s2 (new Sphere (radius));
+
+ #ifndef NDEBUG
+ int num_tests = 1e3;
+ #else
+ int num_tests = 1e6;
+ #endif
+
+ Transform3f tf1;
+ Transform3f tf2;
+
+ for(int i = 0; i < num_tests; ++i)
+ {
+ Eigen::Vector3d p1 = Eigen::Vector3d::Random()*(2.*radius);
+ Eigen::Vector3d p2 = Eigen::Vector3d::Random()*(2.*radius);
+
+ Eigen::Matrix3d rot1 = Eigen::Quaterniond(Eigen::Vector4d::Random().normalized()).toRotationMatrix();
+ Eigen::Matrix3d rot2 = Eigen::Quaterniond(Eigen::Vector4d::Random().normalized()).toRotationMatrix();
+
+ tf1.setTranslation(p1); tf1.setRotation(rot1);
+ tf2.setTranslation(p2); tf2.setRotation(rot2);
+
+ CollisionObject capsule_o1 (c1, tf1);
+ CollisionObject capsule_o2 (c2, tf2);
+
+ CollisionObject sphere_o1 (s1, tf1);
+ CollisionObject sphere_o2 (s2, tf2);
+
+ // Enable computation of nearest points
+ CollisionRequest collisionRequest;
+ CollisionResult capsule_collisionResult, sphere_collisionResult;
+
+ size_t sphere_num_collisions = collide(&sphere_o1, &sphere_o2, collisionRequest, sphere_collisionResult);
+ size_t capsule_num_collisions = collide(&capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult);
+
+ BOOST_CHECK(sphere_num_collisions == capsule_num_collisions);
+ }
+}
+
+BOOST_AUTO_TEST_CASE(collision_capsule_capsule_aligned)
+{
+ const double radius = 0.01;
+ const double length = 0.2;
+
+ CollisionGeometryPtr_t c1 (new Capsule (radius, length));
+ CollisionGeometryPtr_t c2 (new Capsule (radius, length));
+#ifndef NDEBUG
+ int num_tests = 1e3;
+#else
+ int num_tests = 1e6;
+#endif
+
+ Transform3f tf1;
+ Transform3f tf2;
+
+ Eigen::Vector3d p1 = Eigen::Vector3d::Zero();
+ Eigen::Vector3d p2_no_collision = Eigen::Vector3d(0.,0.,2*(length/2. + radius) + 1e-3); // because capsule are along the Z axis
+
+ for(int i = 0; i < num_tests; ++i)
+ {
+ Eigen::Matrix3d rot = Eigen::Quaterniond(Eigen::Vector4d::Random().normalized()).toRotationMatrix();
+
+ tf1.setTranslation(p1); tf1.setRotation(rot);
+ tf2.setTranslation(p2_no_collision); tf2.setRotation(rot);
+
+ CollisionObject capsule_o1 (c1, tf1);
+ CollisionObject capsule_o2 (c2, tf2);
+
+ // Enable computation of nearest points
+ CollisionRequest collisionRequest;
+ CollisionResult capsule_collisionResult;
+
+ size_t capsule_num_collisions = collide(&capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult);
+
+ BOOST_CHECK(capsule_num_collisions == 0);
+ }
+
+ Eigen::Vector3d p2_with_collision = Eigen::Vector3d(0.,0.,std::min(length/2.,radius)*(1.-1e-2));
+ for(int i = 0; i < num_tests; ++i)
+ {
+ Eigen::Matrix3d rot = Eigen::Quaterniond(Eigen::Vector4d::Random().normalized()).toRotationMatrix();
+
+ tf1.setTranslation(p1); tf1.setRotation(rot);
+ tf2.setTranslation(p2_with_collision); tf2.setRotation(rot);
+
+ CollisionObject capsule_o1 (c1, tf1);
+ CollisionObject capsule_o2 (c2, tf2);
+
+ // Enable computation of nearest points
+ CollisionRequest collisionRequest;
+ CollisionResult capsule_collisionResult;
+
+ size_t capsule_num_collisions = collide(&capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult);
+
+ BOOST_CHECK(capsule_num_collisions > 0);
+ }
+
+ p2_no_collision = Eigen::Vector3d(0.,0.,2*(length/2. + radius) + 1e-3);
+
+ Transform3f geom1_placement(Eigen::Matrix3d::Identity(),Eigen::Vector3d::Zero());
+ Transform3f geom2_placement(Eigen::Matrix3d::Identity(),p2_no_collision);
+
+ for(int i = 0; i < num_tests; ++i)
+ {
+ Eigen::Matrix3d rot = Eigen::Quaterniond(Eigen::Vector4d::Random().normalized()).toRotationMatrix();
+ Eigen::Vector3d trans = Eigen::Vector3d::Random();
+
+ Transform3f displacement(rot,trans);
+ Transform3f tf1 = displacement * geom1_placement;
+ Transform3f tf2 = displacement * geom2_placement;
+
+ CollisionObject capsule_o1 (c1, tf1);
+ CollisionObject capsule_o2 (c2, tf2);
+
+ // Enable computation of nearest points
+ CollisionRequest collisionRequest;
+ CollisionResult capsule_collisionResult;
+
+ size_t capsule_num_collisions = collide(&capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult);
+
+ BOOST_CHECK(capsule_num_collisions == 0);
+ }
+
+// p2_with_collision = Eigen::Vector3d(0.,0.,std::min(length/2.,radius)*(1.-1e-2));
+ p2_with_collision = Eigen::Vector3d(0.,0.,0.01);
+ geom2_placement.setTranslation(p2_with_collision);
+
+ for(int i = 0; i < num_tests; ++i)
+ {
+ Eigen::Matrix3d rot = Eigen::Quaterniond(Eigen::Vector4d::Random().normalized()).toRotationMatrix();
+ Eigen::Vector3d trans = Eigen::Vector3d::Random();
+
+ Transform3f displacement(rot,trans);
+ Transform3f tf1 = displacement * geom1_placement;
+ Transform3f tf2 = displacement * geom2_placement;
+
+ CollisionObject capsule_o1 (c1, tf1);
+ CollisionObject capsule_o2 (c2, tf2);
+
+ // Enable computation of nearest points
+ CollisionRequest collisionRequest;
+ CollisionResult capsule_collisionResult;
+
+ size_t capsule_num_collisions = collide(&capsule_o1, &capsule_o2, collisionRequest, capsule_collisionResult);
+
+ BOOST_CHECK(capsule_num_collisions > 0);
+ }
+}
+
BOOST_AUTO_TEST_CASE(distance_capsulecapsule_origin)
{
CollisionGeometryPtr_t s1 (new Capsule (5, 10));
@@ -157,13 +315,14 @@ BOOST_AUTO_TEST_CASE(distance_capsulecapsule_transformZ2)
distance (&o1, &o2, distanceRequest, distanceResult);
std::cerr << "Applied rotation and translation on two capsules" << std::endl;
- std::cerr << "R1 = " << tf1.getRotation ()
- << ", T1 = " << tf1.getTranslation() << std::endl
- << "R2 = " << tf2.getRotation ()
- << ", T2 = " << tf2.getTranslation() << std::endl;
- std::cerr << "Closest points: p1 = " << distanceResult.nearest_points [0]
- << ", p2 = " << distanceResult.nearest_points [1]
- << ", distance = " << distanceResult.min_distance << std::endl;
+ std::cerr << "R1 = " << tf1.getRotation () << std::endl
+ << "T1 = " << tf1.getTranslation().transpose() << std::endl
+ << "R2 = " << tf2.getRotation () << std::endl
+ << "T2 = " << tf2.getTranslation().transpose() << std::endl;
+ std::cerr << "Closest points:" << std::endl
+ << "p1 = " << distanceResult.nearest_points[0].transpose() << std::endl
+ << "p2 = " << distanceResult.nearest_points[1].transpose() << std::endl
+ << "distance = " << distanceResult.min_distance << std::endl;
const Vec3f& p1 = distanceResult.nearest_points [0];
const Vec3f& p2 = distanceResult.nearest_points [1];