diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index c31180fde33d969aba79ddec970c05288bbb36ce..23688c2226e9c4d4b05548680edb1bfc43dd9e9e 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -53,6 +53,8 @@ set(${LIBRARY_NAME}_SOURCES
distance_capsule_capsule.cpp
distance_capsule_halfspace.cpp
distance_capsule_plane.cpp
+ distance_cone_halfspace.cpp
+ distance_cone_plane.cpp
distance_cylinder_halfspace.cpp
distance_cylinder_plane.cpp
distance_sphere_sphere.cpp
diff --git a/src/distance_cone_halfspace.cpp b/src/distance_cone_halfspace.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..f895ac0b6f96112310f86d2302ec1bbbadcac9b6
--- /dev/null
+++ b/src/distance_cone_halfspace.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 <Cone, Halfspace, GJKSolver_indep>
+ (const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2,
+ const GJKSolver_indep*, const DistanceRequest& request,
+ DistanceResult& result)
+ {
+ const Cone& s1 = static_cast <const Cone&> (*o1);
+ const Halfspace& s2 = static_cast <const Halfspace&> (*o2);
+ bool col = details::coneHalfspaceIntersect
+ (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 <Halfspace, Cone, GJKSolver_indep>
+ (const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2,
+ const GJKSolver_indep*, const DistanceRequest& request,
+ DistanceResult& result)
+ {
+ const Halfspace& s1 = static_cast <const Halfspace&> (*o1);
+ const Cone& s2 = static_cast <const Cone&> (*o2);
+ bool col = details::coneHalfspaceIntersect
+ (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/distance_cone_plane.cpp b/src/distance_cone_plane.cpp
new file mode 100644
index 0000000000000000000000000000000000000000..6087cda7c467b3ba6bf8dd039a0065d6e3b56934
--- /dev/null
+++ b/src/distance_cone_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 <Cone, Plane, GJKSolver_indep>
+ (const CollisionGeometry* o1, const Transform3f& tf1,
+ const CollisionGeometry* o2, const Transform3f& tf2,
+ const GJKSolver_indep*, const DistanceRequest& request,
+ DistanceResult& result)
+ {
+ const Cone& s1 = static_cast <const Cone&> (*o1);
+ const Plane& s2 = static_cast <const Plane&> (*o2);
+ bool col = details::conePlaneIntersect
+ (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, Cone, 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 Cone& s2 = static_cast <const Cone&> (*o2);
+ bool col = details::conePlaneIntersect
+ (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 817a6b7ab44e8a7f53f6b79f23a032a2e8ebada3..3cc3f63682a5e1838de83bbae1519ffc1542af40 100644
--- a/src/narrowphase/details.h
+++ b/src/narrowphase/details.h
@@ -1651,7 +1651,8 @@ namespace fcl {
inline bool coneHalfspaceIntersect
(const Cone& s1, const Transform3f& tf1,
const Halfspace& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
+ FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
+ Vec3f& normal)
{
Halfspace new_s2 = transform(s2, tf2);
@@ -1664,20 +1665,24 @@ namespace fcl {
if(cosa < halfspaceIntersectTolerance<FCL_REAL>())
{
FCL_REAL signed_dist = new_s2.signedDistance(T);
- FCL_REAL depth = s1.radius - signed_dist;
- if(depth < 0) return false;
+ distance = signed_dist - s1.radius;
+ if(distance > 0) {
+ p1 = p2 = Vec3f (0, 0, 0);
+ return false;
+ }
else
{
- if(penetration_depth) *penetration_depth = depth;
- if(normal) *normal = -new_s2.n;
- if(contact_points) *contact_points = T - dir_z * (s1.lz * 0.5) + new_s2.n * (0.5 * depth - s1.radius);
+ normal = -new_s2.n;
+ p1 = p2 = T - dir_z * (s1.lz * 0.5) -
+ new_s2.n * (0.5 * distance + s1.radius);
return true;
}
}
else
{
Vec3f C = dir_z * cosa - new_s2.n;
- if(std::abs(cosa + 1) < halfspaceIntersectTolerance<FCL_REAL>() || std::abs(cosa - 1) < halfspaceIntersectTolerance<FCL_REAL>())
+ if(std::abs(cosa + 1) < halfspaceIntersectTolerance<FCL_REAL>() ||
+ std::abs(cosa - 1) < halfspaceIntersectTolerance<FCL_REAL>())
C = Vec3f(0, 0, 0);
else
{
@@ -1686,19 +1691,18 @@ namespace fcl {
C *= s;
}
- Vec3f p1 = T + dir_z * (0.5 * s1.lz);
- Vec3f p2 = T - dir_z * (0.5 * s1.lz) + C;
+ Vec3f a1 = T + dir_z * (0.5 * s1.lz);
+ Vec3f a2 = T - dir_z * (0.5 * s1.lz) + C;
- FCL_REAL d1 = new_s2.signedDistance(p1);
- FCL_REAL d2 = new_s2.signedDistance(p2);
+ FCL_REAL d1 = new_s2.signedDistance(a1);
+ FCL_REAL d2 = new_s2.signedDistance(a2);
if(d1 > 0 && d2 > 0) return false;
else
{
- FCL_REAL depth = -std::min(d1, d2);
- if(penetration_depth) *penetration_depth = depth;
- if(normal) *normal = -new_s2.n;
- if(contact_points) *contact_points = ((d1 < d2) ? p1 : p2) + new_s2.n * (0.5 * depth);
+ distance = std::min(d1, d2);
+ normal = -new_s2.n;
+ p1 = p2 = ((d1 < d2) ? a1 : a2) - (0.5 * distance) * new_s2.n;
return true;
}
}
@@ -2215,7 +2219,8 @@ namespace fcl {
inline bool conePlaneIntersect
(const Cone& s1, const Transform3f& tf1,
const Plane& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
+ FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
+ Vec3f& normal)
{
Plane new_s2 = transform(s2, tf2);
@@ -2228,20 +2233,24 @@ namespace fcl {
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;
+ distance = std::abs(d) - s1.radius;
+ if(distance > 0) {
+ p1 = p2 = Vec3f (0,0,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 - dir_z * (0.5 * s1.lz) + dir_z * (0.5 * depth / s1.radius * s1.lz) - new_s2.n * d;
+ if (d < 0) normal = new_s2.n; else normal = -new_s2.n;
+ p1 = p2 = T - dir_z * (0.5 * s1.lz) +
+ dir_z * (-0.5 * distance / s1.radius * s1.lz) - 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>())
+ if(std::abs(cosa + 1) < planeIntersectTolerance<FCL_REAL>() ||
+ std::abs(cosa - 1) < planeIntersectTolerance<FCL_REAL>())
C = Vec3f(0, 0, 0);
else
{
@@ -2260,7 +2269,8 @@ namespace fcl {
d[1] = new_s2.signedDistance(c[1]);
d[2] = new_s2.signedDistance(c[2]);
- if((d[0] >= 0 && d[1] >= 0 && d[2] >= 0) || (d[0] <= 0 && d[1] <= 0 && d[2] <= 0))
+ if((d[0] >= 0 && d[1] >= 0 && d[2] >= 0) ||
+ (d[0] <= 0 && d[1] <= 0 && d[2] <= 0))
return false;
else
{
@@ -2283,43 +2293,41 @@ namespace fcl {
}
}
- if(penetration_depth) *penetration_depth = std::min(d_positive, d_negative);
- if(normal) { if (d_positive > d_negative) *normal = -new_s2.n; else *normal = new_s2.n; }
- if(contact_points)
- {
- Vec3f p[2];
- Vec3f q;
-
- FCL_REAL p_d[2];
- FCL_REAL q_d(0);
+ distance = -std::min(d_positive, d_negative);
+ if (d_positive > d_negative) normal = -new_s2.n;
+ else normal = new_s2.n;
+ Vec3f p[2];
+ Vec3f q;
- if(n_positive == 2)
- {
- for(std::size_t i = 0, j = 0; i < 3; ++i)
- {
- if(positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; }
- else { q = c[i]; q_d = d[i]; }
- }
+ FCL_REAL p_d[2];
+ FCL_REAL q_d(0);
- Vec3f t1 = (-p[0] * q_d + q * p_d[0]) / (-q_d + p_d[0]);
- Vec3f t2 = (-p[1] * q_d + q * p_d[1]) / (-q_d + p_d[1]);
- *contact_points = (t1 + t2) * 0.5;
- }
- else
- {
- for(std::size_t i = 0, j = 0; i < 3; ++i)
- {
- if(!positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; }
- else { q = c[i]; q_d = d[i]; }
- }
+ if(n_positive == 2)
+ {
+ for(std::size_t i = 0, j = 0; i < 3; ++i)
+ {
+ if(positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; }
+ else { q = c[i]; q_d = d[i]; }
+ }
- Vec3f t1 = (p[0] * q_d - q * p_d[0]) / (q_d - p_d[0]);
- Vec3f t2 = (p[1] * q_d - q * p_d[1]) / (q_d - p_d[1]);
- *contact_points = (t1 + t2) * 0.5;
- }
+ Vec3f t1 = (-p[0] * q_d + q * p_d[0]) / (-q_d + p_d[0]);
+ Vec3f t2 = (-p[1] * q_d + q * p_d[1]) / (-q_d + p_d[1]);
+ p1 = p2 = (t1 + t2) * 0.5;
+ }
+ else
+ {
+ for(std::size_t i = 0, j = 0; i < 3; ++i)
+ {
+ if(!positive[i]) { p[j] = c[i]; p_d[j] = d[i]; j++; }
+ else { q = c[i]; q_d = d[i]; }
}
- return true;
+
+ Vec3f t1 = (p[0] * q_d - q * p_d[0]) / (q_d - p_d[0]);
+ Vec3f t2 = (p[1] * q_d - q * p_d[1]) / (q_d - p_d[1]);
+ p1 = p2 = (t1 + t2) * 0.5;
+ }
}
+ return true;
}
}
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index b3b52d6c1f92f2d9a24ffb4d8ce2d64485f50d2a..7f32458d31949009200c8923af05ee21dd022d12 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -266,20 +266,32 @@ bool GJKSolver_indep::shapeIntersect<Halfspace, Cylinder>
}
template<>
-bool GJKSolver_indep::shapeIntersect<Cone, Halfspace>(const Cone& s1, const Transform3f& tf1,
- const Halfspace& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+bool GJKSolver_indep::shapeIntersect<Cone, Halfspace>
+(const Cone& s1, const Transform3f& tf1,
+ const Halfspace& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
- return details::coneHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
+ FCL_REAL distance;
+ Vec3f p1, p2;
+ bool res = details::coneHalfspaceIntersect
+ (s1, tf1, s2, tf2, distance, p1, p2, *normal);
+ *contact_points = p1;
+ *penetration_depth = -distance;
}
template<>
-bool GJKSolver_indep::shapeIntersect<Halfspace, Cone>(const Halfspace& s1, const Transform3f& tf1,
- const Cone& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+bool GJKSolver_indep::shapeIntersect<Halfspace, Cone>
+(const Halfspace& s1, const Transform3f& tf1,
+ const Cone& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
- const bool res = details::coneHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
- if (normal) (*normal) *= -1.0;
+ FCL_REAL distance;
+ Vec3f p1, p2;
+ bool res = details::coneHalfspaceIntersect
+ (s2, tf2, s1, tf1, distance, p1, p2, *normal);
+ *contact_points = p1;
+ *penetration_depth = -distance;
+ (*normal) *= -1.0;
return res;
}
@@ -431,20 +443,33 @@ bool GJKSolver_indep::shapeIntersect<Plane, Cylinder>
}
template<>
-bool GJKSolver_indep::shapeIntersect<Cone, Plane>(const Cone& s1, const Transform3f& tf1,
- const Plane& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+bool GJKSolver_indep::shapeIntersect<Cone, Plane>
+(const Cone& s1, const Transform3f& tf1,
+ const Plane& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
- return details::conePlaneIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
+ FCL_REAL distance;
+ Vec3f p1, p2;
+ bool res = details::conePlaneIntersect
+ (s1, tf1, s2, tf2, distance, p1, p2, *normal);
+ *contact_points = p1;
+ *penetration_depth = -distance;
+ return res;
}
template<>
-bool GJKSolver_indep::shapeIntersect<Plane, Cone>(const Plane& s1, const Transform3f& tf1,
- const Cone& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+bool GJKSolver_indep::shapeIntersect<Plane, Cone>
+(const Plane& s1, const Transform3f& tf1,
+ const Cone& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
- const bool res = details::conePlaneIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
- if (normal) (*normal) *= -1.0;
+ FCL_REAL distance;
+ Vec3f p1, p2;
+ bool res = details::conePlaneIntersect
+ (s2, tf2, s1, tf1, distance, p1, p2, *normal);
+ *contact_points = p1;
+ *penetration_depth = -distance;
+ (*normal) *= -1.0;
return res;
}