From f3a5f0d3085c9eec4b3eb713a2e40164e5704cc0 Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Sun, 23 Dec 2018 17:54:40 +0100
Subject: [PATCH] Specialize ShapeShapeDistance <Sphere, Cylinder,
GJKSolver_indep>.
---
include/hpp/fcl/narrowphase/narrowphase.h | 13 ++++
src/CMakeLists.txt | 1 +
src/distance_sphere_cylinder.cpp | 81 ++++++++++++++++++++
src/narrowphase/details.h | 90 +++++++++++++++++++++++
src/narrowphase/narrowphase.cpp | 20 +++++
5 files changed, 205 insertions(+)
create mode 100644 src/distance_sphere_cylinder.cpp
diff --git a/include/hpp/fcl/narrowphase/narrowphase.h b/include/hpp/fcl/narrowphase/narrowphase.h
index 44414f59..ae5de50c 100644
--- a/include/hpp/fcl/narrowphase/narrowphase.h
+++ b/include/hpp/fcl/narrowphase/narrowphase.h
@@ -471,6 +471,19 @@ namespace fcl
const Sphere& s2, const Transform3f& tf2,
FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const;
+ /// @brief Fast implementation for sphere-cylinder distance
+ template<>
+ bool GJKSolver_indep::shapeDistance<Sphere, Cylinder>
+ (const Sphere& s1, const Transform3f& tf1,
+ const Cylinder& s2, const Transform3f& tf2,
+ FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const;
+
+ template<>
+ bool GJKSolver_indep::shapeDistance<Cylinder, Sphere>
+ (const Cylinder& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const;
+
/// @brief Fast implementation for sphere-sphere distance
template<>
bool GJKSolver_indep::shapeDistance<Sphere, Sphere>
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index e5bc88d7..fb9bd5ef 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -60,6 +60,7 @@ set(${LIBRARY_NAME}_SOURCES
distance_cylinder_halfspace.cpp
distance_cylinder_plane.cpp
distance_sphere_sphere.cpp
+ distance_sphere_cylinder.cpp
distance_sphere_halfspace.cpp
distance_sphere_plane.cpp
intersect.cpp
diff --git a/src/distance_sphere_cylinder.cpp b/src/distance_sphere_cylinder.cpp
new file mode 100644
index 00000000..1bc5a37b
--- /dev/null
+++ b/src/distance_sphere_cylinder.cpp
@@ -0,0 +1,81 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
+ * Copyright (c) 2018-2019, Center National de la Recherche Scientifique
+ * All rights reserved.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions
+ * are met:
+ *
+ * * Redistributions of source code must retain the above copyright
+ * notice, this list of conditions and the following disclaimer.
+ * * Redistributions in binary form must reproduce the above
+ * copyright notice, this list of conditions and the following
+ * disclaimer in the documentation and/or other materials provided
+ * with the distribution.
+ * * Neither the name of Open Source Robotics Foundation nor the names of its
+ * contributors may be used to endorse or promote products derived
+ * from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+ * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+ * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+ * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+ * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+ * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+ * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+ * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+ * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+ * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+ * POSSIBILITY OF SUCH DAMAGE.
+ */
+
+/** \author Florent Lamiraux */
+
+#include <cmath>
+#include <limits>
+#include <hpp/fcl/math/transform.h>
+#include <hpp/fcl/shape/geometric_shapes.h>
+#include "distance_func_matrix.h"
+#include "../src/narrowphase/details.h"
+
+namespace fcl {
+ class GJKSolver_indep;
+
+ template <>
+ FCL_REAL ShapeShapeDistance <Sphere, Cylinder, GJKSolver_indep>
+ (const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2,
+ const GJKSolver_indep*, const DistanceRequest&,
+ DistanceResult& result)
+ {
+ const Sphere& s1 = static_cast <const Sphere&> (*o1);
+ const Cylinder& s2 = static_cast <const Cylinder&> (*o2);
+ details::sphereCylinderDistance
+ (s1, tf1, s2, tf2, result.min_distance, result.nearest_points [0],
+ result.nearest_points [1], result.normal);
+ result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1;
+ return result.min_distance;
+ }
+
+ template <>
+ FCL_REAL ShapeShapeDistance <Cylinder, Sphere, GJKSolver_indep>
+ (const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2,
+ const GJKSolver_indep*, const DistanceRequest&,
+ DistanceResult& result)
+ {
+ const Cylinder& s1 = static_cast <const Cylinder&> (*o1);
+ const Sphere& s2 = static_cast <const Sphere&> (*o2);
+ details::sphereCylinderDistance
+ (s2, tf2, s1, tf1, result.min_distance, result.nearest_points [1],
+ result.nearest_points [0], result.normal);
+ result.o1 = o1; result.o2 = o2; result.b1 = -1; result.b2 = -1;
+ result.normal = -result.normal;
+ return result.min_distance;
+ }
+} // namespace fcl
diff --git a/src/narrowphase/details.h b/src/narrowphase/details.h
index 9c041e9f..a933b82a 100644
--- a/src/narrowphase/details.h
+++ b/src/narrowphase/details.h
@@ -140,6 +140,96 @@ namespace fcl {
return true;
}
+ inline bool sphereCylinderDistance
+ (const Sphere& s1, const Transform3f& tf1,
+ const Cylinder& s2, const Transform3f& tf2,
+ FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal)
+ {
+ FCL_REAL eps (sqrt (std::numeric_limits <FCL_REAL>::epsilon ()));
+ FCL_REAL r1 (s1.radius);
+ FCL_REAL r2 (s2.radius);
+ FCL_REAL lz2 (.5*s2.lz);
+ // boundaries of the cylinder axis
+ Vec3f A (tf2.transform (Vec3f (0, 0, -lz2)));
+ Vec3f B (tf2.transform (Vec3f (0, 0, lz2)));
+ // Position of the center of the sphere
+ Vec3f S (tf1.getTranslation ());
+ // axis of the cylinder
+ Vec3f u (tf2.getRotation ().col (2));
+ assert ((B - A - s2.lz * u).norm () < eps);
+ Vec3f AS (S - A);
+ // abscissa of S on cylinder axis with A as the origin
+ FCL_REAL s (u.dot (AS));
+ Vec3f P (A + s*u);
+ Vec3f PS (S - P);
+ FCL_REAL dPS = PS.norm ();
+ // Normal to cylinder axis such that plane (A, u, v) contains sphere
+ // center
+ Vec3f v (0, 0, 0);
+ if (dPS > eps) {
+ // S is not on cylinder axis
+ v = (1/dPS) * PS;
+ }
+ if (s <= 0) {
+ if (dPS <= r2) {
+ // closest point on cylinder is on cylinder disc basis
+ dist = -s - r1; p1 = S + r1 * u; p2 = A + dPS * v; normal = u;
+ } else {
+ // closest point on cylinder is on cylinder circle basis
+ p2 = A + r2 * v;
+ Vec3f Sp2 (p2 - S);
+ FCL_REAL dSp2 = Sp2.norm ();
+ if (dSp2 > eps) {
+ normal = (1/dSp2) * Sp2;
+ p1 = S + r1 * normal;
+ dist = dSp2 - r1;
+ assert (fabs (dist) - (p1-p2).norm () < eps);
+ } else {
+ // Center of sphere is on cylinder boundary
+ normal = .5 * (A + B) - p2; normal.normalize ();
+ p1 = p2;
+ dist = -r1;
+ }
+ }
+ } else if (s <= s2.lz) {
+ // 0 < s <= s2.lz
+ normal = -v;
+ dist = dPS - r1 - r2;
+ if (dPS <= r2) {
+ // Sphere center is inside cylinder
+ p1 = p2 = S;
+ } else {
+ p2 = P + r2*v; p1 = S - r1*v;
+ }
+ } else {
+ // lz < s
+ if (dPS <= r2) {
+ // closest point on cylinder is on cylinder disc basis
+ dist = s - s2.lz - r1; p1 = S - r1 * u; p2 = B + dPS * v; normal = -u;
+ } else {
+ // closest point on cylinder is on cylinder circle basis
+ p2 = B + r2 * v;
+ Vec3f Sp2 (p2 - S);
+ FCL_REAL dSp2 = Sp2.norm ();
+ if (dSp2 > eps) {
+ normal = (1/dSp2) * Sp2;
+ p1 = S + r1 * normal;
+ dist = dSp2 - r1;
+ assert (fabs (dist) - (p1-p2).norm () < eps);
+ } else {
+ // Center of sphere is on cylinder boundary
+ normal = .5 * (A + B) - p2; normal.normalize ();
+ p1 = p2;
+ dist = -r1;
+ }
+ }
+ }
+ if (dist < 0) {
+ p1 = p2 = .5 * (p1 + p2);
+ }
+ return (dist > 0);
+ }
+
inline bool sphereSphereIntersect
(const Sphere& s1, const Transform3f& tf1,
const Sphere& s2, const Transform3f& tf2,
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index 0ba4164e..e2d88de4 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -568,6 +568,26 @@ bool GJKSolver_indep::shapeDistance<Capsule, Sphere>
return details::sphereCapsuleDistance(s2, tf2, s1, tf1, dist, p2, p1, normal);
}
+template<>
+bool GJKSolver_indep::shapeDistance<Sphere, Cylinder>
+(const Sphere& s1, const Transform3f& tf1,
+ const Cylinder& s2, const Transform3f& tf2,
+ FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const
+{
+ return details::sphereCylinderDistance
+ (s1, tf1, s2, tf2, dist, p1, p2, normal);
+}
+
+template<>
+bool GJKSolver_indep::shapeDistance<Cylinder, Sphere>
+(const Cylinder& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal) const
+{
+ return details::sphereCylinderDistance
+ (s2, tf2, s1, tf1, dist, p2, p1, normal);
+}
+
template<>
bool GJKSolver_indep::shapeDistance<Sphere, Sphere>
(const Sphere& s1, const Transform3f& tf1,
--
GitLab