From ca40f3329dcb423b4a9dfc769eb9726e3b94f73d Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Sun, 16 Dec 2018 11:07:24 +0100
Subject: [PATCH] Specialize ShapeShapeDistance <Cylinder, Plane,
GJKSolver_indep>.
---
src/CMakeLists.txt | 1 +
src/distance_cylinder_plane.cpp | 81 +++++++++++++++++++++++++++++++++
src/narrowphase/narrowphase.cpp | 31 +++++++++----
3 files changed, 104 insertions(+), 9 deletions(-)
create mode 100644 src/distance_cylinder_plane.cpp
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 98098bb5..c31180fd 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -54,6 +54,7 @@ set(${LIBRARY_NAME}_SOURCES
distance_capsule_halfspace.cpp
distance_capsule_plane.cpp
distance_cylinder_halfspace.cpp
+ distance_cylinder_plane.cpp
distance_sphere_sphere.cpp
distance_sphere_halfspace.cpp
distance_sphere_plane.cpp
diff --git a/src/distance_cylinder_plane.cpp b/src/distance_cylinder_plane.cpp
new file mode 100644
index 00000000..0987cfd9
--- /dev/null
+++ b/src/distance_cylinder_plane.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 <Cylinder, Plane, GJKSolver_indep>
+ (const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2,
+ const GJKSolver_indep*, const DistanceRequest& request,
+ DistanceResult& result)
+ {
+ const Cylinder& s1 = static_cast <const Cylinder&> (*o1);
+ const Plane& s2 = static_cast <const Plane&> (*o2);
+ bool col = details::cylinderPlaneIntersect
+ (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 <Plane, Cylinder, GJKSolver_indep>
+ (const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2,
+ const GJKSolver_indep*, const DistanceRequest& request,
+ DistanceResult& result)
+ {
+ const Plane& s1 = static_cast <const Plane&> (*o1);
+ const Cylinder& s2 = static_cast <const Cylinder&> (*o2);
+ bool col = details::cylinderPlaneIntersect
+ (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/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index 5c8912f3..b3b52d6c 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -400,20 +400,33 @@ bool GJKSolver_indep::shapeIntersect<Plane, Capsule>
}
template<>
-bool GJKSolver_indep::shapeIntersect<Cylinder, Plane>(const Cylinder& s1, const Transform3f& tf1,
- const Plane& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+bool GJKSolver_indep::shapeIntersect<Cylinder, Plane>
+(const Cylinder& s1, const Transform3f& tf1,
+ const Plane& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
- return details::cylinderPlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
+ FCL_REAL distance;
+ Vec3f p1, p2;
+ bool res = details::cylinderPlaneIntersect
+ (s1, tf1, s2, tf2, distance, p1, p2, *normal);
+ *contact_points = p1;
+ *penetration_depth = -distance;
+ return res;
}
template<>
-bool GJKSolver_indep::shapeIntersect<Plane, Cylinder>(const Plane& s1, const Transform3f& tf1,
- const Cylinder& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+bool GJKSolver_indep::shapeIntersect<Plane, Cylinder>
+(const Plane& s1, const Transform3f& tf1,
+ const Cylinder& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
- const bool res = details::cylinderPlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
- if (normal) (*normal) *= -1.0;
+ FCL_REAL distance;
+ Vec3f p1, p2;
+ bool res = details::cylinderPlaneIntersect
+ (s2, tf2, s1, tf1, distance, p1, p2, *normal);
+ *contact_points = p1;
+ *penetration_depth = -distance;
+ (*normal) *= -1.0;
return res;
}
--
GitLab