From e8ea40df14eb9a5ad6c01682132caa242159840a Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Sun, 16 Dec 2018 08:17:43 +0100
Subject: [PATCH] Specialize ShapeShapeDistance <Capsule, Plane,
GJKSolver_indep>.
---
src/CMakeLists.txt | 1 +
src/distance_capsule_plane.cpp | 81 +++++++++
src/narrowphase/details.h | 305 ++++++++++++++------------------
src/narrowphase/narrowphase.cpp | 31 +++-
4 files changed, 236 insertions(+), 182 deletions(-)
create mode 100644 src/distance_capsule_plane.cpp
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 80fa1207..bdb5345c 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -52,6 +52,7 @@ set(${LIBRARY_NAME}_SOURCES
distance_box_halfspace.cpp
distance_capsule_capsule.cpp
distance_capsule_halfspace.cpp
+ distance_capsule_plane.cpp
distance_sphere_sphere.cpp
distance_sphere_halfspace.cpp
distance_sphere_plane.cpp
diff --git a/src/distance_capsule_plane.cpp b/src/distance_capsule_plane.cpp
new file mode 100644
index 00000000..aabad2ae
--- /dev/null
+++ b/src/distance_capsule_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 <Capsule, Plane, GJKSolver_indep>
+ (const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2,
+ const GJKSolver_indep*, const DistanceRequest& request,
+ DistanceResult& result)
+ {
+ const Capsule& s1 = static_cast <const Capsule&> (*o1);
+ const Plane& s2 = static_cast <const Plane&> (*o2);
+ bool col = details::capsulePlaneIntersect
+ (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, Capsule, 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 Capsule& s2 = static_cast <const Capsule&> (*o2);
+ bool col = details::capsulePlaneIntersect
+ (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 626beee4..18a97a1c 100644
--- a/src/narrowphase/details.h
+++ b/src/narrowphase/details.h
@@ -2031,8 +2031,11 @@ namespace fcl {
}
- inline bool capsulePlaneIntersect(const Capsule& s1, const Transform3f& tf1,
- const Plane& s2, const Transform3f& tf2)
+ inline bool capsulePlaneIntersect
+ (const Capsule& s1, const Transform3f& tf1,
+ const Plane& s2, const Transform3f& tf2,
+ FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
+ Vec3f& normal)
{
Plane new_s2 = transform(s2, tf2);
@@ -2040,99 +2043,78 @@ namespace fcl {
const Vec3f& T = tf1.getTranslation();
Vec3f dir_z = R.col(2);
- Vec3f p1 = T + dir_z * (0.5 * s1.lz);
- Vec3f p2 = T - dir_z * (0.5 * s1.lz);
- FCL_REAL d1 = new_s2.signedDistance(p1);
- FCL_REAL d2 = new_s2.signedDistance(p2);
- // two end points on different side of the plane
- if(d1 * d2 <= 0)
- return true;
+ Vec3f a1 = T + dir_z * (0.5 * s1.lz);
+ Vec3f a2 = T - dir_z * (0.5 * s1.lz);
- // two end points on the same side of the plane, but the end point spheres might intersect the plane
- return (std::abs(d1) <= s1.radius) || (std::abs(d2) <= s1.radius);
- }
+ FCL_REAL d1 = new_s2.signedDistance(a1);
+ FCL_REAL d2 = new_s2.signedDistance(a2);
- inline bool capsulePlaneIntersect
- (const Capsule& s1, const Transform3f& tf1,
- const Plane& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
- {
- Plane new_s2 = transform(s2, tf2);
+ FCL_REAL abs_d1 = std::abs(d1);
+ FCL_REAL abs_d2 = std::abs(d2);
- if(!contact_points && !penetration_depth && !normal)
- return capsulePlaneIntersect(s1, tf1, s2, tf2);
- else
+ // two end points on different side of the plane
+ // the contact point is the intersect of axis with the plane
+ // the normal is the direction to avoid intersection
+ // the depth is the minimum distance to resolve the collision
+ if(d1 * d2 < -planeIntersectTolerance<FCL_REAL>())
+ {
+ if(abs_d1 < abs_d2)
{
- Plane new_s2 = transform(s2, tf2);
-
- const Matrix3f& R = tf1.getRotation();
- const Vec3f& T = tf1.getTranslation();
-
- Vec3f dir_z = R.col(2);
-
-
- Vec3f p1 = T + dir_z * (0.5 * s1.lz);
- Vec3f p2 = T - dir_z * (0.5 * s1.lz);
-
- FCL_REAL d1 = new_s2.signedDistance(p1);
- FCL_REAL d2 = new_s2.signedDistance(p2);
-
- FCL_REAL abs_d1 = std::abs(d1);
- FCL_REAL abs_d2 = std::abs(d2);
-
- // two end points on different side of the plane
- // the contact point is the intersect of axis with the plane
- // the normal is the direction to avoid intersection
- // the depth is the minimum distance to resolve the collision
- if(d1 * d2 < -planeIntersectTolerance<FCL_REAL>())
- {
- if(abs_d1 < abs_d2)
- {
- if(penetration_depth) *penetration_depth = abs_d1 + s1.radius;
- if(contact_points) *contact_points = p1 * (abs_d2 / (abs_d1 + abs_d2)) + p2 * (abs_d1 / (abs_d1 + abs_d2));
- if(normal) { if (d1 < 0) *normal = -new_s2.n; else *normal = new_s2.n; }
- }
- else
- {
- if(penetration_depth) *penetration_depth = abs_d2 + s1.radius;
- if(contact_points) *contact_points = p1 * (abs_d2 / (abs_d1 + abs_d2)) + p2 * (abs_d1 / (abs_d1 + abs_d2));
- if(normal) { if (d2 < 0) *normal = -new_s2.n; else *normal = new_s2.n; }
- }
- return true;
- }
-
- if(abs_d1 > s1.radius && abs_d2 > s1.radius)
- return false;
- else
- {
- if(penetration_depth) *penetration_depth = s1.radius - std::min(abs_d1, abs_d2);
+ distance = -abs_d1 - s1.radius;
+ p1 = p2 = a1 * (abs_d2 / (abs_d1 + abs_d2)) +
+ a2 * (abs_d1 / (abs_d1 + abs_d2));
+ if (d1 < 0) normal = -new_s2.n; else normal = new_s2.n;
+ }
+ else
+ {
+ distance = -abs_d2 - s1.radius;
+ p1 = p2 = a1 * (abs_d2 / (abs_d1 + abs_d2)) +
+ a2 * (abs_d1 / (abs_d1 + abs_d2));
+ if (d2 < 0) normal = -new_s2.n; else normal = new_s2.n;
+ }
+ return true;
+ }
- if(contact_points)
- {
- if(abs_d1 <= s1.radius && abs_d2 <= s1.radius)
- {
- Vec3f c1 = p1 - new_s2.n * d2;
- Vec3f c2 = p2 - new_s2.n * d1;
- *contact_points = (c1 + c2) * 0.5;
- }
- else if(abs_d1 <= s1.radius)
- {
- Vec3f c = p1 - new_s2.n * d1;
- *contact_points = c;
- }
- else if(abs_d2 <= s1.radius)
- {
- Vec3f c = p2 - new_s2.n * d2;
- *contact_points = c;
- }
- }
+ if(abs_d1 > s1.radius && abs_d2 > s1.radius) {
+ // Here both capsule ends are on the same side of the plane
+ if (d1 > 0) normal = new_s2.n; else normal = -new_s2.n;
+ if (abs_d1 < abs_d2) {
+ distance = abs_d1 - s1.radius;
+ p1 = a1 - s1.radius * normal;
+ p2 = p1 - distance * normal;
+ } else {
+ distance = abs_d2 - s1.radius;
+ p1 = a2 - s1.radius * normal;
+ p2 = p1 - distance * normal;
+ }
+ return false;
+ }
+ else
+ {
+ distance = std::min(abs_d1, abs_d2) - s1.radius;
- if(normal) { if (d1 < 0) *normal = new_s2.n; else *normal = -new_s2.n; }
- return true;
- }
+ if(abs_d1 <= s1.radius && abs_d2 <= s1.radius)
+ {
+ Vec3f c1 = a1 - new_s2.n * d2;
+ Vec3f c2 = p2 - new_s2.n * d1;
+ p1 = p2 = (c1 + c2) * 0.5;
+ }
+ else if(abs_d1 <= s1.radius)
+ {
+ Vec3f c = a1 - new_s2.n * d1;
+ p1 = p2 = c;
+ }
+ else if(abs_d2 <= s1.radius)
+ {
+ Vec3f c = p2 - new_s2.n * d2;
+ p1 = p2 = c;
}
+
+ if (d1 < 0) normal = new_s2.n; else normal = -new_s2.n;
+ return true;
+ }
}
/// @brief cylinder-plane intersect
@@ -2142,108 +2124,85 @@ namespace fcl {
/// (n^T * v3) * h + r * (cosa * (n^T * R * v1) + sina * (n^T * R * v2)) + n * T - d ~ 0
inline bool cylinderPlaneIntersect
(const Cylinder& s1, const Transform3f& tf1,
- const Plane& s2, const Transform3f& tf2)
+ const Plane& s2, const Transform3f& tf2,
+ FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
+ Vec3f& normal)
{
Plane new_s2 = transform(s2, tf2);
const Matrix3f& R = tf1.getRotation();
const Vec3f& T = tf1.getTranslation();
- Vec3f Q = R.transpose() * new_s2.n;
-
- FCL_REAL term = std::abs(Q[2]) * s1.lz + s1.radius * std::sqrt(Q[0] * Q[0] + Q[1] * Q[1]);
- FCL_REAL dist = new_s2.distance(T);
- FCL_REAL depth = term - dist;
-
- if(depth < 0)
- return false;
- else
- return true;
- }
+ Vec3f dir_z = R.col(2);
+ FCL_REAL cosa = dir_z.dot(new_s2.n);
- inline bool cylinderPlaneIntersect
- (const Cylinder& s1, const Transform3f& tf1,
- const Plane& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
- {
- if(!contact_points && !penetration_depth && !normal)
- return cylinderPlaneIntersect(s1, tf1, s2, tf2);
+ if(std::abs(cosa) < planeIntersectTolerance<FCL_REAL>())
+ {
+ FCL_REAL d = new_s2.signedDistance(T);
+ distance = std::abs(d) - s1.radius;
+ if(distance > 0) return false;
+ else
+ {
+ if (d < 0) normal = new_s2.n;
+ else normal = -new_s2.n;
+ p1 = p2 = T - new_s2.n * d;
+ return true;
+ }
+ }
else
+ {
+ Vec3f C = dir_z * cosa - new_s2.n;
+ if(std::abs(cosa + 1) < planeIntersectTolerance<FCL_REAL>() ||
+ std::abs(cosa - 1) < planeIntersectTolerance<FCL_REAL>())
+ C = Vec3f(0, 0, 0);
+ else
{
- Plane new_s2 = transform(s2, tf2);
-
- const Matrix3f& R = tf1.getRotation();
- const Vec3f& T = tf1.getTranslation();
-
- Vec3f dir_z = R.col(2);
- FCL_REAL cosa = dir_z.dot(new_s2.n);
-
- if(std::abs(cosa) < planeIntersectTolerance<FCL_REAL>())
- {
- FCL_REAL d = new_s2.signedDistance(T);
- FCL_REAL depth = s1.radius - std::abs(d);
- if(depth < 0) return false;
- else
- {
- if(penetration_depth) *penetration_depth = depth;
- if(normal) { if (d < 0) *normal = new_s2.n; else *normal = -new_s2.n; }
- if(contact_points) *contact_points = T - new_s2.n * d;
- return true;
- }
- }
- else
- {
- Vec3f C = dir_z * cosa - new_s2.n;
- if(std::abs(cosa + 1) < planeIntersectTolerance<FCL_REAL>() || std::abs(cosa - 1) < planeIntersectTolerance<FCL_REAL>())
- C = Vec3f(0, 0, 0);
- else
- {
- FCL_REAL s = C.norm();
- s = s1.radius / s;
- C *= s;
- }
+ FCL_REAL s = C.norm();
+ s = s1.radius / s;
+ C *= s;
+ }
- Vec3f p1 = T + dir_z * (0.5 * s1.lz);
- Vec3f p2 = T - dir_z * (0.5 * s1.lz);
+ Vec3f a1 = T + dir_z * (0.5 * s1.lz);
+ Vec3f a2 = T - dir_z * (0.5 * s1.lz);
- Vec3f c1, c2;
- if(cosa > 0)
- {
- c1 = p1 - C;
- c2 = p2 + C;
- }
- else
- {
- c1 = p1 + C;
- c2 = p2 - C;
- }
+ Vec3f c1, c2;
+ if(cosa > 0)
+ {
+ c1 = a1 - C;
+ c2 = a2 + C;
+ }
+ else
+ {
+ c1 = a1 + C;
+ c2 = a2 - C;
+ }
- FCL_REAL d1 = new_s2.signedDistance(c1);
- FCL_REAL d2 = new_s2.signedDistance(c2);
+ FCL_REAL d1 = new_s2.signedDistance(c1);
+ FCL_REAL d2 = new_s2.signedDistance(c2);
- if(d1 * d2 <= 0)
- {
- FCL_REAL abs_d1 = std::abs(d1);
- FCL_REAL abs_d2 = std::abs(d2);
+ if(d1 * d2 <= 0)
+ {
+ FCL_REAL abs_d1 = std::abs(d1);
+ FCL_REAL abs_d2 = std::abs(d2);
- if(abs_d1 > abs_d2)
- {
- if(penetration_depth) *penetration_depth = abs_d2;
- if(contact_points) *contact_points = c2 - new_s2.n * d2;
- if(normal) { if (d2 < 0) *normal = -new_s2.n; else *normal = new_s2.n; }
- }
- else
- {
- if(penetration_depth) *penetration_depth = abs_d1;
- if(contact_points) *contact_points = c1 - new_s2.n * d1;
- if(normal) { if (d1 < 0) *normal = -new_s2.n; else *normal = new_s2.n; }
- }
- return true;
- }
- else
- return false;
- }
+ if(abs_d1 > abs_d2)
+ {
+ distance = -abs_d2;
+ if(contact_points) *contact_points = c2 - new_s2.n * d2;
+ if (d2 < 0) *normal = -new_s2.n; else *normal = new_s2.n;
+ }
+ else
+ {
+ distance = -abs_d1;
+ p1 = p2 = c1 - new_s2.n * d1;
+ if (d1 < 0) normal = -new_s2.n;
+ else normal = new_s2.n;
+ }
+ return true;
}
+ else
+ return false;
+ }
}
inline bool conePlaneIntersect
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index 24c9c9ed..8bde198b 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -357,20 +357,33 @@ bool GJKSolver_indep::shapeIntersect<Plane, Box>(const Plane& s1, const Transfor
}
template<>
-bool GJKSolver_indep::shapeIntersect<Capsule, Plane>(const Capsule& s1, const Transform3f& tf1,
- const Plane& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+bool GJKSolver_indep::shapeIntersect<Capsule, Plane>
+(const Capsule& s1, const Transform3f& tf1,
+ const Plane& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
- return details::capsulePlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
+ FCL_REAL distance;
+ Vec3f p1, p2;
+ bool res = details::capsulePlaneIntersect(s1, tf1, s2, tf2, distance, p1,
+ p2, *normal);
+ *contact_points = p1;
+ *penetration_depth = -distance;
+ return res;
}
template<>
-bool GJKSolver_indep::shapeIntersect<Plane, Capsule>(const Plane& s1, const Transform3f& tf1,
- const Capsule& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+bool GJKSolver_indep::shapeIntersect<Plane, Capsule>
+(const Plane& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
- const bool res = details::capsulePlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
- if (normal) (*normal) *= -1.0;
+ FCL_REAL distance;
+ Vec3f p1, p2;
+ bool res = details::capsulePlaneIntersect(s2, tf2, s1, tf1, distance, p1,
+ p2, *normal);
+ *contact_points = p1;
+ *penetration_depth = -distance;
+ (*normal) *= -1.0;
return res;
}
--
GitLab