From 1f11ac4bd3fd03e5394611742b759f1501fc3f2d Mon Sep 17 00:00:00 2001
From: Florent Lamiraux <florent@laas.fr>
Date: Sat, 15 Dec 2018 17:01:14 +0100
Subject: [PATCH] Migrate functions computing interactions between geometric
shapes
in a local header so that they can be called from another file.
---
src/CMakeLists.txt | 1 +
src/narrowphase/details.h | 2550 +++++++++++++++++++++++++++++++
src/narrowphase/narrowphase.cpp | 2498 +-----------------------------
3 files changed, 2571 insertions(+), 2478 deletions(-)
create mode 100644 src/narrowphase/details.h
diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt
index 1e1605e5..bad2cde1 100644
--- a/src/CMakeLists.txt
+++ b/src/CMakeLists.txt
@@ -46,6 +46,7 @@ set(${LIBRARY_NAME}_SOURCES
BV/OBB.cpp
narrowphase/narrowphase.cpp
narrowphase/gjk.cpp
+ narrowphase/details.h
shape/geometric_shapes.cpp
shape/geometric_shapes_utility.cpp
distance_capsule_capsule.cpp
diff --git a/src/narrowphase/details.h b/src/narrowphase/details.h
new file mode 100644
index 00000000..7c50b733
--- /dev/null
+++ b/src/narrowphase/details.h
@@ -0,0 +1,2550 @@
+/*
+ * Software License Agreement (BSD License)
+ *
+ * Copyright (c) 2011-2014, Willow Garage, Inc.
+ * Copyright (c) 2014-2015, Open Source Robotics Foundation
+ * Copyright (c) 2018-2019, Centre 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 Jia Pan, Florent Lamiraux */
+
+#ifndef HPP_FCL_SRC_NARROWPHASE_DETAILS_H
+# define HPP_FCL_SRC_NARROWPHASE_DETAILS_H
+
+#include <hpp/fcl/collision_node.h>
+#include <hpp/fcl/traversal/traversal_node_setup.h>
+#include <hpp/fcl/narrowphase/narrowphase.h>
+
+namespace fcl {
+ namespace details
+ {
+ // Compute the point on a line segment that is the closest point on the
+ // segment to to another point. The code is inspired by the explanation
+ // given by Dan Sunday's page:
+ // http://geomalgorithms.com/a02-_lines.html
+ static inline void lineSegmentPointClosestToPoint (const Vec3f &p, const Vec3f &s1, const Vec3f &s2, Vec3f &sp) {
+ Vec3f v = s2 - s1;
+ Vec3f w = p - s1;
+
+ FCL_REAL c1 = w.dot(v);
+ FCL_REAL c2 = v.dot(v);
+
+ if (c1 <= 0) {
+ sp = s1;
+ } else if (c2 <= c1) {
+ sp = s2;
+ } else {
+ FCL_REAL b = c1/c2;
+ Vec3f Pb = s1 + v * b;
+ sp = Pb;
+ }
+ }
+
+ inline bool sphereCapsuleIntersect
+ (const Sphere& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal_)
+ {
+ Transform3f tf2_inv (tf2);
+ tf2_inv.inverse();
+
+ Vec3f pos1 (tf2.transform (Vec3f (0., 0., 0.5 * s2.lz))); // from distance function
+ Vec3f pos2 (tf2.transform (Vec3f (0., 0., -0.5 * s2.lz)));
+ Vec3f s_c = tf1.getTranslation ();
+
+ Vec3f segment_point;
+
+ lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point);
+ Vec3f diff = s_c - segment_point;
+
+ FCL_REAL diffN = diff.norm();
+ FCL_REAL distance = diffN - s1.radius - s2.radius;
+
+ if (distance > 0)
+ return false;
+
+ // Vec3f normal (-diff.normalized());
+
+ if (distance < 0 && penetration_depth)
+ *penetration_depth = -distance;
+
+ if (normal_)
+ *normal_ = -diff / diffN;
+
+ if (contact_points) {
+ *contact_points = segment_point + diff * s2.radius;
+ }
+
+ return true;
+ }
+
+ inline bool sphereCapsuleDistance
+ (const Sphere& s1, const Transform3f& tf1,
+ const Capsule& s2, const Transform3f& tf2,
+ FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal)
+ {
+ Transform3f tf2_inv(tf2);
+ tf2_inv.inverse();
+
+ Vec3f pos1 (tf2.transform (Vec3f (0., 0., 0.5 * s2.lz)));
+ Vec3f pos2 (tf2.transform (Vec3f (0., 0., -0.5 * s2.lz)));
+ Vec3f s_c = tf1.getTranslation ();
+
+ Vec3f segment_point;
+
+ lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point);
+ normal = segment_point - s_c;
+ FCL_REAL norm (normal.norm());
+ dist = norm - s1.radius - s2.radius;
+
+ FCL_REAL eps (std::numeric_limits<FCL_REAL>::epsilon());
+ if (norm > eps) {
+ normal.normalize();
+ } else {
+ normal << 1,0,0;
+ }
+ p1 = s_c + normal * s1.radius;
+ p2 = segment_point - normal * s2.radius;
+
+ if(dist <= 0) {
+ p1 = p2 = .5 * (p1+p2);
+ return false;
+ }
+ return true;
+ }
+
+ inline bool sphereSphereIntersect
+ (const Sphere& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
+ {
+ Vec3f diff = tf2.getTranslation() - tf1.getTranslation();
+ FCL_REAL len = diff.norm();
+ if(len > s1.radius + s2.radius)
+ return false;
+
+ if(penetration_depth)
+ *penetration_depth = s1.radius + s2.radius - len;
+
+ // If the centers of two sphere are at the same position, the normal is (0, 0, 0).
+ // Otherwise, normal is pointing from center of object 1 to center of object 2
+ if(normal)
+ {
+ if(len > 0)
+ *normal = diff / len;
+ else
+ *normal = diff;
+ }
+
+ if(contact_points)
+ *contact_points = tf1.getTranslation() + diff * s1.radius / (s1.radius + s2.radius);
+
+ return true;
+ }
+
+
+ inline bool sphereSphereDistance(const Sphere& s1, const Transform3f& tf1,
+ const Sphere& s2, const Transform3f& tf2,
+ FCL_REAL& dist, Vec3f& p1, Vec3f& p2,
+ Vec3f& normal)
+ {
+ Vec3f o1 = tf1.getTranslation();
+ Vec3f o2 = tf2.getTranslation();
+ Vec3f diff = o1 - o2;
+ FCL_REAL len = diff.norm();
+ normal = -diff/len;
+ dist = len - s1.radius - s2.radius;
+
+ p1 = o1 - diff * (s1.radius / len);
+ p2 = o2 + diff * (s2.radius / len);
+
+ return (dist >=0);
+ }
+
+ /** \brief the minimum distance from a point to a line */
+ inline FCL_REAL segmentSqrDistance
+ (const Vec3f& from, const Vec3f& to,const Vec3f& p, Vec3f& nearest)
+ {
+ Vec3f diff = p - from;
+ Vec3f v = to - from;
+ FCL_REAL t = v.dot(diff);
+
+ if(t > 0)
+ {
+ FCL_REAL dotVV = v.dot(v);
+ if(t < dotVV)
+ {
+ t /= dotVV;
+ diff -= v * t;
+ }
+ else
+ {
+ t = 1;
+ diff -= v;
+ }
+ }
+ else
+ t = 0;
+
+ nearest = from + v * t;
+ return diff.dot(diff);
+ }
+
+ /// @brief Whether a point's projection is in a triangle
+ inline bool projectInTriangle (const Vec3f& p1, const Vec3f& p2,
+ const Vec3f& p3, const Vec3f& normal,
+ const Vec3f& p)
+ {
+ Vec3f edge1(p2 - p1);
+ Vec3f edge2(p3 - p2);
+ Vec3f edge3(p1 - p3);
+
+ Vec3f p1_to_p(p - p1);
+ Vec3f p2_to_p(p - p2);
+ Vec3f p3_to_p(p - p3);
+
+ Vec3f edge1_normal(edge1.cross(normal));
+ Vec3f edge2_normal(edge2.cross(normal));
+ Vec3f edge3_normal(edge3.cross(normal));
+
+ FCL_REAL r1, r2, r3;
+ r1 = edge1_normal.dot(p1_to_p);
+ r2 = edge2_normal.dot(p2_to_p);
+ r3 = edge3_normal.dot(p3_to_p);
+ if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
+ ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
+ return true;
+ return false;
+ }
+
+ // Intersection between sphere and triangle
+ // Sphere is in position tf1, Triangle is expressed in global frame
+ inline bool sphereTriangleIntersect
+ (const Sphere& s, const Transform3f& tf1,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
+ FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
+ Vec3f& normal_)
+ {
+ Vec3f normal = (P2 - P1).cross(P3 - P1);
+ normal.normalize();
+ const Vec3f& center = tf1.getTranslation();
+ const FCL_REAL& radius = s.radius;
+ assert (radius >= 0);
+ FCL_REAL eps (std::numeric_limits<FCL_REAL>::epsilon());
+ FCL_REAL radius_with_threshold = radius + eps;
+ Vec3f p1_to_center = center - P1;
+ FCL_REAL distance_from_plane = p1_to_center.dot(normal);
+
+ if(distance_from_plane < 0)
+ {
+ distance_from_plane *= -1;
+ normal *= -1;
+ }
+
+ bool is_inside_contact_plane = (distance_from_plane < radius_with_threshold);
+
+ bool has_contact = false;
+ Vec3f contact_point;
+ if(is_inside_contact_plane)
+ {
+ if(projectInTriangle(P1, P2, P3, normal, center))
+ {
+ has_contact = true;
+ contact_point = center - normal * distance_from_plane;
+ }
+ else
+ {
+ FCL_REAL contact_capsule_radius_sqr = radius_with_threshold * radius_with_threshold;
+ Vec3f nearest_on_edge;
+ FCL_REAL distance_sqr;
+ distance_sqr = segmentSqrDistance(P1, P2, center, nearest_on_edge);
+ if(distance_sqr < contact_capsule_radius_sqr)
+ {
+ has_contact = true;
+ contact_point = nearest_on_edge;
+ }
+
+ distance_sqr = segmentSqrDistance(P2, P3, center, nearest_on_edge);
+ if(distance_sqr < contact_capsule_radius_sqr)
+ {
+ has_contact = true;
+ contact_point = nearest_on_edge;
+ }
+
+ distance_sqr = segmentSqrDistance(P3, P1, center, nearest_on_edge);
+ if(distance_sqr < contact_capsule_radius_sqr)
+ {
+ has_contact = true;
+ contact_point = nearest_on_edge;
+ }
+ }
+ }
+
+ Vec3f center_to_contact = contact_point - center;
+ if(has_contact)
+ {
+ FCL_REAL distance_sqr = center_to_contact.squaredNorm();
+
+ if(distance_sqr < radius_with_threshold * radius_with_threshold)
+ {
+ if(distance_sqr > eps)
+ {
+ FCL_REAL dist = std::sqrt(distance_sqr);
+ normal_ = center_to_contact.normalized();
+ p1 = p2 = contact_point;
+ distance = dist - radius;
+ }
+ else
+ {
+ normal_ = -normal;
+ p1 = p2 = contact_point;
+ distance = - radius;
+ }
+ }
+ }
+ else
+ {
+ Vec3f unit (center_to_contact.normalized ());
+ p1 = center + radius * unit;
+ p2 = contact_point;
+ }
+ return has_contact;
+ }
+
+
+ inline bool sphereTriangleDistance
+ (const Sphere& sp, const Transform3f& tf,
+ const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, FCL_REAL* dist)
+ {
+ // from geometric tools, very different from the collision code.
+
+ const Vec3f& center = tf.getTranslation();
+ FCL_REAL radius = sp.radius;
+ Vec3f diff = P1 - center;
+ Vec3f edge0 = P2 - P1;
+ Vec3f edge1 = P3 - P1;
+ FCL_REAL a00 = edge0.squaredNorm();
+ FCL_REAL a01 = edge0.dot(edge1);
+ FCL_REAL a11 = edge1.squaredNorm();
+ FCL_REAL b0 = diff.dot(edge0);
+ FCL_REAL b1 = diff.dot(edge1);
+ FCL_REAL c = diff.squaredNorm();
+ FCL_REAL det = fabs(a00*a11 - a01*a01);
+ FCL_REAL s = a01*b1 - a11*b0;
+ FCL_REAL t = a01*b0 - a00*b1;
+
+ FCL_REAL sqr_dist;
+
+ if(s + t <= det)
+ {
+ if(s < 0)
+ {
+ if(t < 0) // region 4
+ {
+ if(b0 < 0)
+ {
+ t = 0;
+ if(-b0 >= a00)
+ {
+ s = 1;
+ sqr_dist = a00 + 2*b0 + c;
+ }
+ else
+ {
+ s = -b0/a00;
+ sqr_dist = b0*s + c;
+ }
+ }
+ else
+ {
+ s = 0;
+ if(b1 >= 0)
+ {
+ t = 0;
+ sqr_dist = c;
+ }
+ else if(-b1 >= a11)
+ {
+ t = 1;
+ sqr_dist = a11 + 2*b1 + c;
+ }
+ else
+ {
+ t = -b1/a11;
+ sqr_dist = b1*t + c;
+ }
+ }
+ }
+ else // region 3
+ {
+ s = 0;
+ if(b1 >= 0)
+ {
+ t = 0;
+ sqr_dist = c;
+ }
+ else if(-b1 >= a11)
+ {
+ t = 1;
+ sqr_dist = a11 + 2*b1 + c;
+ }
+ else
+ {
+ t = -b1/a11;
+ sqr_dist = b1*t + c;
+ }
+ }
+ }
+ else if(t < 0) // region 5
+ {
+ t = 0;
+ if(b0 >= 0)
+ {
+ s = 0;
+ sqr_dist = c;
+ }
+ else if(-b0 >= a00)
+ {
+ s = 1;
+ sqr_dist = a00 + 2*b0 + c;
+ }
+ else
+ {
+ s = -b0/a00;
+ sqr_dist = b0*s + c;
+ }
+ }
+ else // region 0
+ {
+ // minimum at interior point
+ FCL_REAL inv_det = (1)/det;
+ s *= inv_det;
+ t *= inv_det;
+ sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
+ }
+ }
+ else
+ {
+ FCL_REAL tmp0, tmp1, numer, denom;
+
+ if(s < 0) // region 2
+ {
+ tmp0 = a01 + b0;
+ tmp1 = a11 + b1;
+ if(tmp1 > tmp0)
+ {
+ numer = tmp1 - tmp0;
+ denom = a00 - 2*a01 + a11;
+ if(numer >= denom)
+ {
+ s = 1;
+ t = 0;
+ sqr_dist = a00 + 2*b0 + c;
+ }
+ else
+ {
+ s = numer/denom;
+ t = 1 - s;
+ sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
+ }
+ }
+ else
+ {
+ s = 0;
+ if(tmp1 <= 0)
+ {
+ t = 1;
+ sqr_dist = a11 + 2*b1 + c;
+ }
+ else if(b1 >= 0)
+ {
+ t = 0;
+ sqr_dist = c;
+ }
+ else
+ {
+ t = -b1/a11;
+ sqr_dist = b1*t + c;
+ }
+ }
+ }
+ else if(t < 0) // region 6
+ {
+ tmp0 = a01 + b1;
+ tmp1 = a00 + b0;
+ if(tmp1 > tmp0)
+ {
+ numer = tmp1 - tmp0;
+ denom = a00 - 2*a01 + a11;
+ if(numer >= denom)
+ {
+ t = 1;
+ s = 0;
+ sqr_dist = a11 + 2*b1 + c;
+ }
+ else
+ {
+ t = numer/denom;
+ s = 1 - t;
+ sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
+ }
+ }
+ else
+ {
+ t = 0;
+ if(tmp1 <= 0)
+ {
+ s = 1;
+ sqr_dist = a00 + 2*b0 + c;
+ }
+ else if(b0 >= 0)
+ {
+ s = 0;
+ sqr_dist = c;
+ }
+ else
+ {
+ s = -b0/a00;
+ sqr_dist = b0*s + c;
+ }
+ }
+ }
+ else // region 1
+ {
+ numer = a11 + b1 - a01 - b0;
+ if(numer <= 0)
+ {
+ s = 0;
+ t = 1;
+ sqr_dist = a11 + 2*b1 + c;
+ }
+ else
+ {
+ denom = a00 - 2*a01 + a11;
+ if(numer >= denom)
+ {
+ s = 1;
+ t = 0;
+ sqr_dist = a00 + 2*b0 + c;
+ }
+ else
+ {
+ s = numer/denom;
+ t = 1 - s;
+ sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
+ }
+ }
+ }
+ }
+
+ // Account for numerical round-off error.
+ if(sqr_dist < 0)
+ sqr_dist = 0;
+
+ if(sqr_dist > radius * radius)
+ {
+ if(dist) *dist = std::sqrt(sqr_dist) - radius;
+ return true;
+ }
+ else
+ {
+ if(dist) *dist = -1;
+ return false;
+ }
+ }
+
+
+ inline bool sphereTriangleDistance
+ (const Sphere& sp, const Transform3f& tf, const Vec3f& P1,
+ const Vec3f& P2, const Vec3f& P3, FCL_REAL* dist, Vec3f* p1, Vec3f* p2)
+ {
+ if(p1 || p2)
+ {
+ Vec3f o = tf.getTranslation();
+ Project::ProjectResult result;
+ result = Project::projectTriangle(P1, P2, P3, o);
+ if(result.sqr_distance > sp.radius * sp.radius)
+ {
+ if(dist) *dist = std::sqrt(result.sqr_distance) - sp.radius;
+ Vec3f project_p = P1 * result.parameterization[0] + P2 * result.parameterization[1] + P3 * result.parameterization[2];
+ Vec3f dir = o - project_p;
+ dir.normalize();
+ if(p1) { *p1 = o - dir * sp.radius; }
+ if(p2) *p2 = project_p;
+ return true;
+ }
+ else
+ return false;
+ }
+ else
+ {
+ return sphereTriangleDistance(sp, tf, P1, P2, P3, dist);
+ }
+ }
+
+
+ inline bool sphereTriangleDistance
+ (const Sphere& sp, const Transform3f& tf1, const Vec3f& P1,
+ const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
+ FCL_REAL* dist, Vec3f* p1, Vec3f* p2)
+ {
+ bool res = details::sphereTriangleDistance(sp, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), dist, p1, p2);
+ return res;
+ }
+
+
+
+ struct ContactPoint
+ {
+ Vec3f normal;
+ Vec3f point;
+ FCL_REAL depth;
+ ContactPoint(const Vec3f& n, const Vec3f& p, FCL_REAL d) : normal(n), point(p), depth(d) {}
+ };
+
+
+ static inline void lineClosestApproach(const Vec3f& pa, const Vec3f& ua,
+ const Vec3f& pb, const Vec3f& ub,
+ FCL_REAL* alpha, FCL_REAL* beta)
+ {
+ Vec3f p = pb - pa;
+ FCL_REAL uaub = ua.dot(ub);
+ FCL_REAL q1 = ua.dot(p);
+ FCL_REAL q2 = -ub.dot(p);
+ FCL_REAL d = 1 - uaub * uaub;
+ if(d <= (FCL_REAL)(0.0001f))
+ {
+ *alpha = 0;
+ *beta = 0;
+ }
+ else
+ {
+ d = 1 / d;
+ *alpha = (q1 + uaub * q2) * d;
+ *beta = (uaub * q1 + q2) * d;
+ }
+ }
+
+ // find all the intersection points between the 2D rectangle with vertices
+ // at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]),
+ // (p[2],p[3]),(p[4],p[5]),(p[6],p[7]).
+ //
+ // the intersection points are returned as x,y pairs in the 'ret' array.
+ // the number of intersection points is returned by the function (this will
+ // be in the range 0 to 8).
+ static int intersectRectQuad2(FCL_REAL h[2], FCL_REAL p[8], FCL_REAL ret[16])
+ {
+ // q (and r) contain nq (and nr) coordinate points for the current (and
+ // chopped) polygons
+ int nq = 4, nr = 0;
+ FCL_REAL buffer[16];
+ FCL_REAL* q = p;
+ FCL_REAL* r = ret;
+ for(int dir = 0; dir <= 1; ++dir)
+ {
+ // direction notation: xy[0] = x axis, xy[1] = y axis
+ for(int sign = -1; sign <= 1; sign += 2)
+ {
+ // chop q along the line xy[dir] = sign*h[dir]
+ FCL_REAL* pq = q;
+ FCL_REAL* pr = r;
+ nr = 0;
+ for(int i = nq; i > 0; --i)
+ {
+ // go through all points in q and all lines between adjacent points
+ if(sign * pq[dir] < h[dir])
+ {
+ // this point is inside the chopping line
+ pr[0] = pq[0];
+ pr[1] = pq[1];
+ pr += 2;
+ nr++;
+ if(nr & 8)
+ {
+ q = r;
+ goto done;
+ }
+ }
+ FCL_REAL* nextq = (i > 1) ? pq+2 : q;
+ if((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir]))
+ {
+ // this line crosses the chopping line
+ pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) /
+ (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]);
+ pr[dir] = sign*h[dir];
+ pr += 2;
+ nr++;
+ if(nr & 8)
+ {
+ q = r;
+ goto done;
+ }
+ }
+ pq += 2;
+ }
+ q = r;
+ r = (q == ret) ? buffer : ret;
+ nq = nr;
+ }
+ }
+
+ done:
+ if(q != ret) memcpy(ret, q, nr*2*sizeof(FCL_REAL));
+ return nr;
+ }
+
+ // given n points in the plane (array p, of size 2*n), generate m points that
+ // best represent the whole set. the definition of 'best' here is not
+ // predetermined - the idea is to select points that give good box-box
+ // collision detection behavior. the chosen point indexes are returned in the
+ // array iret (of size m). 'i0' is always the first entry in the array.
+ // n must be in the range [1..8]. m must be in the range [1..n]. i0 must be
+ // in the range [0..n-1].
+ static inline void cullPoints2(int n, FCL_REAL p[], int m, int i0, int iret[])
+ {
+ // compute the centroid of the polygon in cx,cy
+ FCL_REAL a, cx, cy, q;
+ switch(n)
+ {
+ case 1:
+ cx = p[0];
+ cy = p[1];
+ break;
+ case 2:
+ cx = 0.5 * (p[0] + p[2]);
+ cy = 0.5 * (p[1] + p[3]);
+ break;
+ default:
+ a = 0;
+ cx = 0;
+ cy = 0;
+ for(int i = 0; i < n-1; ++i)
+ {
+ q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
+ a += q;
+ cx += q*(p[i*2]+p[i*2+2]);
+ cy += q*(p[i*2+1]+p[i*2+3]);
+ }
+ q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
+ if(std::abs(a+q) > std::numeric_limits<FCL_REAL>::epsilon())
+ a = 1/(3*(a+q));
+ else
+ a= 1e18f;
+
+ cx = a*(cx + q*(p[n*2-2]+p[0]));
+ cy = a*(cy + q*(p[n*2-1]+p[1]));
+ }
+
+
+ // compute the angle of each point w.r.t. the centroid
+ FCL_REAL A[8];
+ for(int i = 0; i < n; ++i)
+ A[i] = atan2(p[i*2+1]-cy,p[i*2]-cx);
+
+ // search for points that have angles closest to A[i0] + i*(2*pi/m).
+ int avail[8];
+ for(int i = 0; i < n; ++i) avail[i] = 1;
+ avail[i0] = 0;
+ iret[0] = i0;
+ iret++;
+ const double pi = boost::math::constants::pi<FCL_REAL>();
+ for(int j = 1; j < m; ++j)
+ {
+ a = j*(2*pi/m) + A[i0];
+ if (a > pi) a -= 2*pi;
+ FCL_REAL maxdiff= 1e9, diff;
+
+ *iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0
+ for(int i = 0; i < n; ++i)
+ {
+ if(avail[i])
+ {
+ diff = std::abs(A[i]-a);
+ if(diff > pi) diff = 2*pi - diff;
+ if(diff < maxdiff)
+ {
+ maxdiff = diff;
+ *iret = i;
+ }
+ }
+ }
+ avail[*iret] = 0;
+ iret++;
+ }
+ }
+
+
+
+ inline int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
+ const Vec3f& side2, const Matrix3f& R2, const Vec3f& T2,
+ Vec3f& normal, FCL_REAL* depth, int* return_code,
+ int maxc, std::vector<ContactPoint>& contacts)
+ {
+ const FCL_REAL fudge_factor = FCL_REAL(1.05);
+ Vec3f normalC;
+ FCL_REAL s, s2, l;
+ int invert_normal, code;
+
+ Vec3f p (T2 - T1); // get vector from centers of box 1 to box 2, relative to box 1
+ Vec3f pp (R1.transpose() * p); // get pp = p relative to body 1
+
+ // get side lengths / 2
+ Vec3f A (side1 * 0.5);
+ Vec3f B (side2 * 0.5);
+
+ // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
+ Matrix3f R (R1.transpose() * R2);
+ Matrix3f Q (R.cwiseAbs());
+
+
+ // for all 15 possible separating axes:
+ // * see if the axis separates the boxes. if so, return 0.
+ // * find the depth of the penetration along the separating axis (s2)
+ // * if this is the largest depth so far, record it.
+ // the normal vector will be set to the separating axis with the smallest
+ // depth. note: normalR is set to point to a column of R1 or R2 if that is
+ // the smallest depth normal so far. otherwise normalR is 0 and normalC is
+ // set to a vector relative to body 1. invert_normal is 1 if the sign of
+ // the normal should be flipped.
+
+ int best_col_id = -1;
+ const Matrix3f* normalR = 0;
+ FCL_REAL tmp = 0;
+
+ s = - std::numeric_limits<FCL_REAL>::max();
+ invert_normal = 0;
+ code = 0;
+
+ // separating axis = u1, u2, u3
+ tmp = pp[0];
+ s2 = std::abs(tmp) - (Q.row(0).dot(B) + A[0]);
+ if(s2 > 0) { *return_code = 0; return 0; }
+ if(s2 > s)
+ {
+ s = s2;
+ best_col_id = 0;
+ normalR = &R1;
+ invert_normal = (tmp < 0);
+ code = 1;
+ }
+
+ tmp = pp[1];
+ s2 = std::abs(tmp) - (Q.row(1).dot(B) + A[1]);
+ if(s2 > 0) { *return_code = 0; return 0; }
+ if(s2 > s)
+ {
+ s = s2;
+ best_col_id = 1;
+ normalR = &R1;
+ invert_normal = (tmp < 0);
+ code = 2;
+ }
+
+ tmp = pp[2];
+ s2 = std::abs(tmp) - (Q.row(2).dot(B) + A[2]);
+ if(s2 > 0) { *return_code = 0; return 0; }
+ if(s2 > s)
+ {
+ s = s2;
+ best_col_id = 2;
+ normalR = &R1;
+ invert_normal = (tmp < 0);
+ code = 3;
+ }
+
+ // separating axis = v1, v2, v3
+ tmp = R2.col(0).dot(p);
+ s2 = std::abs(tmp) - (Q.col(0).dot(A) + B[0]);
+ if(s2 > 0) { *return_code = 0; return 0; }
+ if(s2 > s)
+ {
+ s = s2;
+ best_col_id = 0;
+ normalR = &R2;
+ invert_normal = (tmp < 0);
+ code = 4;
+ }
+
+ tmp = R2.col(1).dot(p);
+ s2 = std::abs(tmp) - (Q.col(1).dot(A) + B[1]);
+ if(s2 > 0) { *return_code = 0; return 0; }
+ if(s2 > s)
+ {
+ s = s2;
+ best_col_id = 1;
+ normalR = &R2;
+ invert_normal = (tmp < 0);
+ code = 5;
+ }
+
+ tmp = R2.col(2).dot(p);
+ s2 = std::abs(tmp) - (Q.col(2).dot(A) + B[2]);
+ if(s2 > 0) { *return_code = 0; return 0; }
+ if(s2 > s)
+ {
+ s = s2;
+ best_col_id = 2;
+ normalR = &R2;
+ invert_normal = (tmp < 0);
+ code = 6;
+ }
+
+
+ FCL_REAL fudge2(1.0e-6);
+ Q.array() += fudge2;
+
+ Vec3f n;
+ FCL_REAL eps = std::numeric_limits<FCL_REAL>::epsilon();
+
+ // separating axis = u1 x (v1,v2,v3)
+ tmp = pp[2] * R(1, 0) - pp[1] * R(2, 0);
+ s2 = std::abs(tmp) - (A[1] * Q(2, 0) + A[2] * Q(1, 0) + B[1] * Q(0, 2) + B[2] * Q(0, 1));
+ if(s2 > 0) { *return_code = 0; return 0; }
+ n = Vec3f(0, -R(2, 0), R(1, 0));
+ l = n.norm();
+ if(l > eps)
+ {
+ s2 /= l;
+ if(s2 * fudge_factor > s)
+ {
+ s = s2;
+ best_col_id = -1;
+ normalC = n / l;
+ invert_normal = (tmp < 0);
+ code = 7;
+ }
+ }
+
+ tmp = pp[2] * R(1, 1) - pp[1] * R(2, 1);
+ s2 = std::abs(tmp) - (A[1] * Q(2, 1) + A[2] * Q(1, 1) + B[0] * Q(0, 2) + B[2] * Q(0, 0));
+ if(s2 > 0) { *return_code = 0; return 0; }
+ n = Vec3f(0, -R(2, 1), R(1, 1));
+ l = n.norm();
+ if(l > eps)
+ {
+ s2 /= l;
+ if(s2 * fudge_factor > s)
+ {
+ s = s2;
+ best_col_id = -1;
+ normalC = n / l;
+ invert_normal = (tmp < 0);
+ code = 8;
+ }
+ }
+
+ tmp = pp[2] * R(1, 2) - pp[1] * R(2, 2);
+ s2 = std::abs(tmp) - (A[1] * Q(2, 2) + A[2] * Q(1, 2) + B[0] * Q(0, 1) + B[1] * Q(0, 0));
+ if(s2 > 0) { *return_code = 0; return 0; }
+ n = Vec3f(0, -R(2, 2), R(1, 2));
+ l = n.norm();
+ if(l > eps)
+ {
+ s2 /= l;
+ if(s2 * fudge_factor > s)
+ {
+ s = s2;
+ best_col_id = -1;
+ normalC = n / l;
+ invert_normal = (tmp < 0);
+ code = 9;
+ }
+ }
+
+ // separating axis = u2 x (v1,v2,v3)
+ tmp = pp[0] * R(2, 0) - pp[2] * R(0, 0);
+ s2 = std::abs(tmp) - (A[0] * Q(2, 0) + A[2] * Q(0, 0) + B[1] * Q(1, 2) + B[2] * Q(1, 1));
+ if(s2 > 0) { *return_code = 0; return 0; }
+ n = Vec3f(R(2, 0), 0, -R(0, 0));
+ l = n.norm();
+ if(l > eps)
+ {
+ s2 /= l;
+ if(s2 * fudge_factor > s)
+ {
+ s = s2;
+ best_col_id = -1;
+ normalC = n / l;
+ invert_normal = (tmp < 0);
+ code = 10;
+ }
+ }
+
+ tmp = pp[0] * R(2, 1) - pp[2] * R(0, 1);
+ s2 = std::abs(tmp) - (A[0] * Q(2, 1) + A[2] * Q(0, 1) + B[0] * Q(1, 2) + B[2] * Q(1, 0));
+ if(s2 > 0) { *return_code = 0; return 0; }
+ n = Vec3f(R(2, 1), 0, -R(0, 1));
+ l = n.norm();
+ if(l > eps)
+ {
+ s2 /= l;
+ if(s2 * fudge_factor > s)
+ {
+ s = s2;
+ best_col_id = -1;
+ normalC = n / l;
+ invert_normal = (tmp < 0);
+ code = 11;
+ }
+ }
+
+ tmp = pp[0] * R(2, 2) - pp[2] * R(0, 2);
+ s2 = std::abs(tmp) - (A[0] * Q(2, 2) + A[2] * Q(0, 2) + B[0] * Q(1, 1) + B[1] * Q(1, 0));
+ if(s2 > 0) { *return_code = 0; return 0; }
+ n = Vec3f(R(2, 2), 0, -R(0, 2));
+ l = n.norm();
+ if(l > eps)
+ {
+ s2 /= l;
+ if(s2 * fudge_factor > s)
+ {
+ s = s2;
+ best_col_id = -1;
+ normalC = n / l;
+ invert_normal = (tmp < 0);
+ code = 12;
+ }
+ }
+
+ // separating axis = u3 x (v1,v2,v3)
+ tmp = pp[1] * R(0, 0) - pp[0] * R(1, 0);
+ s2 = std::abs(tmp) - (A[0] * Q(1, 0) + A[1] * Q(0, 0) + B[1] * Q(2, 2) + B[2] * Q(2, 1));
+ if(s2 > 0) { *return_code = 0; return 0; }
+ n = Vec3f(-R(1, 0), R(0, 0), 0);
+ l = n.norm();
+ if(l > eps)
+ {
+ s2 /= l;
+ if(s2 * fudge_factor > s)
+ {
+ s = s2;
+ best_col_id = -1;
+ normalC = n / l;
+ invert_normal = (tmp < 0);
+ code = 13;
+ }
+ }
+
+ tmp = pp[1] * R(0, 1) - pp[0] * R(1, 1);
+ s2 = std::abs(tmp) - (A[0] * Q(1, 1) + A[1] * Q(0, 1) + B[0] * Q(2, 2) + B[2] * Q(2, 0));
+ if(s2 > 0) { *return_code = 0; return 0; }
+ n = Vec3f(-R(1, 1), R(0, 1), 0);
+ l = n.norm();
+ if(l > eps)
+ {
+ s2 /= l;
+ if(s2 * fudge_factor > s)
+ {
+ s = s2;
+ best_col_id = -1;
+ normalC = n / l;
+ invert_normal = (tmp < 0);
+ code = 14;
+ }
+ }
+
+ tmp = pp[1] * R(0, 2) - pp[0] * R(1, 2);
+ s2 = std::abs(tmp) - (A[0] * Q(1, 2) + A[1] * Q(0, 2) + B[0] * Q(2, 1) + B[1] * Q(2, 0));
+ if(s2 > 0) { *return_code = 0; return 0; }
+ n = Vec3f(-R(1, 2), R(0, 2), 0);
+ l = n.norm();
+ if(l > eps)
+ {
+ s2 /= l;
+ if(s2 * fudge_factor > s)
+ {
+ s = s2;
+ best_col_id = -1;
+ normalC = n / l;
+ invert_normal = (tmp < 0);
+ code = 15;
+ }
+ }
+
+
+
+ if (!code) { *return_code = code; return 0; }
+
+ // if we get to this point, the boxes interpenetrate. compute the normal
+ // in global coordinates.
+ if(best_col_id != -1)
+ normal = normalR->col(best_col_id);
+ else
+ normal = R1 * normalC;
+
+ if(invert_normal)
+ normal = -normal;
+
+ *depth = -s; // s is negative when the boxes are in collision
+
+ // compute contact point(s)
+
+ if(code > 6)
+ {
+ // an edge from box 1 touches an edge from box 2.
+ // find a point pa on the intersecting edge of box 1
+ Vec3f pa(T1);
+ FCL_REAL sign;
+
+ for(int j = 0; j < 3; ++j)
+ {
+ sign = (R1.col(j).dot(normal) > 0) ? 1 : -1;
+ pa += R1.col(j) * (A[j] * sign);
+ }
+
+ // find a point pb on the intersecting edge of box 2
+ Vec3f pb(T2);
+
+ for(int j = 0; j < 3; ++j)
+ {
+ sign = (R2.col(j).dot(normal) > 0) ? -1 : 1;
+ pb += R2.col(j) * (B[j] * sign);
+ }
+
+ FCL_REAL alpha, beta;
+ Vec3f ua(R1.col((code-7)/3));
+ Vec3f ub(R2.col((code-7)%3));
+
+ lineClosestApproach(pa, ua, pb, ub, &alpha, &beta);
+ pa += ua * alpha;
+ pb += ub * beta;
+
+
+ // Vec3f pointInWorld((pa + pb) * 0.5);
+ // contacts.push_back(ContactPoint(-normal, pointInWorld, -*depth));
+ contacts.push_back(ContactPoint(-normal,pb,-*depth));
+ *return_code = code;
+
+ return 1;
+ }
+
+ // okay, we have a face-something intersection (because the separating
+ // axis is perpendicular to a face). define face 'a' to be the reference
+ // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
+ // the incident face (the closest face of the other box).
+
+ const Matrix3f *Ra, *Rb;
+ const Vec3f *pa, *pb, *Sa, *Sb;
+
+ if(code <= 3)
+ {
+ Ra = &R1;
+ Rb = &R2;
+ pa = &T1;
+ pb = &T2;
+ Sa = &A;
+ Sb = &B;
+ }
+ else
+ {
+ Ra = &R2;
+ Rb = &R1;
+ pa = &T2;
+ pb = &T1;
+ Sa = &B;
+ Sb = &A;
+ }
+
+ // nr = normal vector of reference face dotted with axes of incident box.
+ // anr = absolute values of nr.
+ Vec3f normal2, nr, anr;
+ if(code <= 3)
+ normal2 = normal;
+ else
+ normal2 = -normal;
+
+ nr = Rb->transpose() * normal2;
+ anr = nr.cwiseAbs();
+
+ // find the largest compontent of anr: this corresponds to the normal
+ // for the indident face. the other axis numbers of the indicent face
+ // are stored in a1,a2.
+ int lanr, a1, a2;
+ if(anr[1] > anr[0])
+ {
+ if(anr[1] > anr[2])
+ {
+ a1 = 0;
+ lanr = 1;
+ a2 = 2;
+ }
+ else
+ {
+ a1 = 0;
+ a2 = 1;
+ lanr = 2;
+ }
+ }
+ else
+ {
+ if(anr[0] > anr[2])
+ {
+ lanr = 0;
+ a1 = 1;
+ a2 = 2;
+ }
+ else
+ {
+ a1 = 0;
+ a2 = 1;
+ lanr = 2;
+ }
+ }
+
+ // compute center point of incident face, in reference-face coordinates
+ Vec3f center;
+ if(nr[lanr] < 0)
+ center = (*pb) - (*pa) + Rb->col(lanr) * ((*Sb)[lanr]);
+ else
+ center = (*pb) - (*pa) - Rb->col(lanr) * ((*Sb)[lanr]);
+
+ // find the normal and non-normal axis numbers of the reference box
+ int codeN, code1, code2;
+ if(code <= 3)
+ codeN = code-1;
+ else codeN = code-4;
+
+ if(codeN == 0)
+ {
+ code1 = 1;
+ code2 = 2;
+ }
+ else if(codeN == 1)
+ {
+ code1 = 0;
+ code2 = 2;
+ }
+ else
+ {
+ code1 = 0;
+ code2 = 1;
+ }
+
+ // find the four corners of the incident face, in reference-face coordinates
+ FCL_REAL quad[8]; // 2D coordinate of incident face (x,y pairs)
+ FCL_REAL c1, c2, m11, m12, m21, m22;
+ c1 = Ra->col(code1).dot(center);
+ c2 = Ra->col(code2).dot(center);
+ // optimize this? - we have already computed this data above, but it is not
+ // stored in an easy-to-index format. for now it's quicker just to recompute
+ // the four dot products.
+ Vec3f tempRac = Ra->col(code1);
+ m11 = Rb->col(a1).dot(tempRac);
+ m12 = Rb->col(a2).dot(tempRac);
+ tempRac = Ra->col(code2);
+ m21 = Rb->col(a1).dot(tempRac);
+ m22 = Rb->col(a2).dot(tempRac);
+
+ FCL_REAL k1 = m11 * (*Sb)[a1];
+ FCL_REAL k2 = m21 * (*Sb)[a1];
+ FCL_REAL k3 = m12 * (*Sb)[a2];
+ FCL_REAL k4 = m22 * (*Sb)[a2];
+ quad[0] = c1 - k1 - k3;
+ quad[1] = c2 - k2 - k4;
+ quad[2] = c1 - k1 + k3;
+ quad[3] = c2 - k2 + k4;
+ quad[4] = c1 + k1 + k3;
+ quad[5] = c2 + k2 + k4;
+ quad[6] = c1 + k1 - k3;
+ quad[7] = c2 + k2 - k4;
+
+ // find the size of the reference face
+ FCL_REAL rect[2];
+ rect[0] = (*Sa)[code1];
+ rect[1] = (*Sa)[code2];
+
+ // intersect the incident and reference faces
+ FCL_REAL ret[16];
+ int n_intersect = intersectRectQuad2(rect, quad, ret);
+ if(n_intersect < 1) { *return_code = code; return 0; } // this should never happen
+
+ // convert the intersection points into reference-face coordinates,
+ // and compute the contact position and depth for each point. only keep
+ // those points that have a positive (penetrating) depth. delete points in
+ // the 'ret' array as necessary so that 'point' and 'ret' correspond.
+ Vec3f points[8]; // penetrating contact points
+ FCL_REAL dep[8]; // depths for those points
+ FCL_REAL det1 = 1.f/(m11*m22 - m12*m21);
+ m11 *= det1;
+ m12 *= det1;
+ m21 *= det1;
+ m22 *= det1;
+ int cnum = 0; // number of penetrating contact points found
+ for(int j = 0; j < n_intersect; ++j)
+ {
+ FCL_REAL k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
+ FCL_REAL k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
+ points[cnum] = center + Rb->col(a1) * k1 + Rb->col(a2) * k2;
+ dep[cnum] = (*Sa)[codeN] - normal2.dot(points[cnum]);
+ if(dep[cnum] >= 0)
+ {
+ ret[cnum*2] = ret[j*2];
+ ret[cnum*2+1] = ret[j*2+1];
+ cnum++;
+ }
+ }
+ if(cnum < 1) { *return_code = code; return 0; } // this should never happen
+
+ // we can't generate more contacts than we actually have
+ if(maxc > cnum) maxc = cnum;
+ if(maxc < 1) maxc = 1;
+
+ if(cnum <= maxc)
+ {
+ if(code<4)
+ {
+ // we have less contacts than we need, so we use them all
+ for(int j = 0; j < cnum; ++j)
+ {
+ Vec3f pointInWorld = points[j] + (*pa);
+ contacts.push_back(ContactPoint(-normal, pointInWorld, -dep[j]));
+ }
+ }
+ else
+ {
+ // we have less contacts than we need, so we use them all
+ for(int j = 0; j < cnum; ++j)
+ {
+ Vec3f pointInWorld = points[j] + (*pa) - normal * dep[j];
+ contacts.push_back(ContactPoint(-normal, pointInWorld, -dep[j]));
+ }
+ }
+ }
+ else
+ {
+ // we have more contacts than are wanted, some of them must be culled.
+ // find the deepest point, it is always the first contact.
+ int i1 = 0;
+ FCL_REAL maxdepth = dep[0];
+ for(int i = 1; i < cnum; ++i)
+ {
+ if(dep[i] > maxdepth)
+ {
+ maxdepth = dep[i];
+ i1 = i;
+ }
+ }
+
+ int iret[8];
+ cullPoints2(cnum, ret, maxc, i1, iret);
+
+ for(int j = 0; j < maxc; ++j)
+ {
+ Vec3f posInWorld = points[iret[j]] + (*pa);
+ if(code < 4)
+ contacts.push_back(ContactPoint(-normal, posInWorld, -dep[iret[j]]));
+ else
+ contacts.push_back(ContactPoint(-normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]]));
+ }
+ cnum = maxc;
+ }
+
+ *return_code = code;
+ return cnum;
+ }
+
+ inline bool compareContactPoints
+ (const ContactPoint& c1,const ContactPoint& c2)
+ {
+ return c1.depth < c2.depth;
+ } // Accending order, assuming penetration depth is a negative number!
+
+ inline bool boxBoxIntersect(const Box& s1, const Transform3f& tf1,
+ const Box& s2, const Transform3f& tf2,
+ Vec3f* contact_points,
+ FCL_REAL* penetration_depth_, Vec3f* normal_)
+ {
+ std::vector<ContactPoint> contacts;
+ int return_code;
+ Vec3f normal;
+ FCL_REAL depth;
+ /* int cnum = */ boxBox2(s1.side, tf1.getRotation(), tf1.getTranslation(),
+ s2.side, tf2.getRotation(), tf2.getTranslation(),
+ normal, &depth, &return_code,
+ 4, contacts);
+
+ if(normal_) *normal_ = normal;
+ if(penetration_depth_) *penetration_depth_ = depth;
+
+ if(contact_points)
+ {
+ if(contacts.size() > 0)
+ {
+ std::sort(contacts.begin(), contacts.end(), compareContactPoints);
+ *contact_points = contacts[0].point;
+ if(penetration_depth_) *penetration_depth_ = -contacts[0].depth;
+ }
+ }
+
+ return return_code != 0;
+ }
+
+ template<typename T>
+ inline T halfspaceIntersectTolerance()
+ {
+ return 0;
+ }
+
+ template<>
+ inline float halfspaceIntersectTolerance()
+ {
+ return 0.0001f;
+ }
+
+ template<>
+ inline double halfspaceIntersectTolerance()
+ {
+ return 0.0000001;
+ }
+
+ inline bool sphereHalfspaceIntersect
+ (const Sphere& s1, const Transform3f& tf1,
+ const Halfspace& s2, const Transform3f& tf2,
+ FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
+ Vec3f& normal)
+ {
+ Halfspace new_s2 = transform(s2, tf2);
+ const Vec3f& center = tf1.getTranslation();
+ distance = new_s2.signedDistance(center) - s1.radius;
+ if(distance <= 0)
+ {
+ normal = -new_s2.n; // pointing from s1 to s2
+ p1 = p2 = center - new_s2.n * s1.radius - (distance * 0.5) * new_s2.n;
+ return true;
+ }
+ else {
+ p1 = center - s1.radius * new_s2.n;
+ p2 = p1 - distance * new_s2.n;
+ return false;
+ }
+ }
+
+ /// @brief box half space, a, b, c = +/- edge size
+ /// n^T * (R(o + a v1 + b v2 + c v3) + T) <= d
+ /// so (R^T n) (a v1 + b v2 + c v3) + n * T <= d
+ /// check whether d - n * T - (R^T n) (a v1 + b v2 + c v3) >= 0 for some a, b, c
+ /// the max value of left side is d - n * T + |(R^T n) (a v1 + b v2 + c v3)|, check that is enough
+ inline bool boxHalfspaceIntersect
+ (const Box& s1, const Transform3f& tf1,
+ const Halfspace& s2, const Transform3f& tf2)
+ {
+ Halfspace new_s2 = transform(s2, tf2);
+
+ const Matrix3f& R = tf1.getRotation();
+ const Vec3f& T = tf1.getTranslation();
+
+ Vec3f Q = R.transpose() * new_s2.n;
+ Vec3f A(Q[0] * s1.side[0], Q[1] * s1.side[1], Q[2] * s1.side[2]);
+ Vec3f B = A.cwiseAbs();
+
+ FCL_REAL depth = 0.5 * (B[0] + B[1] + B[2]) - new_s2.signedDistance(T);
+ return (depth >= 0);
+ }
+
+
+ inline bool boxHalfspaceIntersect
+ (const Box& s1, const Transform3f& tf1,
+ const Halfspace& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
+ {
+ if(!contact_points && !penetration_depth && !normal)
+ return boxHalfspaceIntersect(s1, tf1, s2, tf2);
+ else
+ {
+ Halfspace new_s2 = transform(s2, tf2);
+
+ const Matrix3f& R = tf1.getRotation();
+ const Vec3f& T = tf1.getTranslation();
+
+ Vec3f Q = R.transpose() * new_s2.n;
+ Vec3f A(Q[0] * s1.side[0], Q[1] * s1.side[1], Q[2] * s1.side[2]);
+ Vec3f B = A.cwiseAbs();
+
+ FCL_REAL depth = 0.5 * (B[0] + B[1] + B[2]) - new_s2.signedDistance(T);
+ if(depth < 0) return false;
+
+ Vec3f axis[3];
+ axis[0] = R.col(0);
+ axis[1] = R.col(1);
+ axis[2] = R.col(2);
+
+ /// find deepest point
+ Vec3f p(T);
+ int sign = 0;
+
+ if(std::abs(Q[0] - 1) < halfspaceIntersectTolerance<FCL_REAL>() || std::abs(Q[0] + 1) < halfspaceIntersectTolerance<FCL_REAL>())
+ {
+ sign = (A[0] > 0) ? -1 : 1;
+ p += axis[0] * (0.5 * s1.side[0] * sign);
+ }
+ else if(std::abs(Q[1] - 1) < halfspaceIntersectTolerance<FCL_REAL>() || std::abs(Q[1] + 1) < halfspaceIntersectTolerance<FCL_REAL>())
+ {
+ sign = (A[1] > 0) ? -1 : 1;
+ p += axis[1] * (0.5 * s1.side[1] * sign);
+ }
+ else if(std::abs(Q[2] - 1) < halfspaceIntersectTolerance<FCL_REAL>() || std::abs(Q[2] + 1) < halfspaceIntersectTolerance<FCL_REAL>())
+ {
+ sign = (A[2] > 0) ? -1 : 1;
+ p += axis[2] * (0.5 * s1.side[2] * sign);
+ }
+ else
+ {
+ for(std::size_t i = 0; i < 3; ++i)
+ {
+ sign = (A[i] > 0) ? -1 : 1;
+ p += axis[i] * (0.5 * s1.side[i] * sign);
+ }
+ }
+
+ /// compute the contact point from the deepest point
+ if(penetration_depth) *penetration_depth = depth;
+ if(normal) *normal = -new_s2.n;
+ if(contact_points) *contact_points = p + new_s2.n * (depth * 0.5);
+
+ return true;
+ }
+ }
+
+ inline bool capsuleHalfspaceIntersect
+ (const Capsule& s1, const Transform3f& tf1,
+ const Halfspace& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
+ {
+ Halfspace 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) < halfspaceIntersectTolerance<FCL_REAL>())
+ {
+ FCL_REAL signed_dist = new_s2.signedDistance(T);
+ FCL_REAL depth = s1.radius - signed_dist;
+ if(depth < 0) return false;
+
+ if(penetration_depth) *penetration_depth = depth;
+ if(normal) *normal = -new_s2.n;
+ if(contact_points) *contact_points = T + new_s2.n * (0.5 * depth - s1.radius);
+
+ return true;
+ }
+ else
+ {
+ int sign = (cosa > 0) ? -1 : 1;
+ Vec3f p = T + dir_z * (s1.lz * 0.5 * sign);
+
+ FCL_REAL signed_dist = new_s2.signedDistance(p);
+ FCL_REAL depth = s1.radius - signed_dist;
+ if(depth < 0) return false;
+
+ if(penetration_depth) *penetration_depth = depth;
+ if(normal) *normal = -new_s2.n;
+ if(contact_points)
+ {
+ // deepest point
+ Vec3f c = p - new_s2.n * s1.radius;
+ *contact_points = c + new_s2.n * (0.5 * depth);
+ }
+
+ return true;
+ }
+ }
+
+
+ inline bool cylinderHalfspaceIntersect
+ (const Cylinder& s1, const Transform3f& tf1,
+ const Halfspace& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
+ {
+ Halfspace 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(cosa < halfspaceIntersectTolerance<FCL_REAL>())
+ {
+ FCL_REAL signed_dist = new_s2.signedDistance(T);
+ FCL_REAL depth = s1.radius - signed_dist;
+ if(depth < 0) return false;
+
+ if(penetration_depth) *penetration_depth = depth;
+ if(normal) *normal = -new_s2.n;
+ if(contact_points) *contact_points = T + new_s2.n * (0.5 * depth - 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>())
+ C = Vec3f(0, 0, 0);
+ else
+ {
+ FCL_REAL s = C.norm();
+ s = s1.radius / s;
+ C *= s;
+ }
+
+ int sign = (cosa > 0) ? -1 : 1;
+ // deepest point
+ Vec3f p = T + dir_z * (s1.lz * 0.5 * sign) + C;
+ FCL_REAL depth = -new_s2.signedDistance(p);
+ if(depth < 0) return false;
+ else
+ {
+ if(penetration_depth) *penetration_depth = depth;
+ if(normal) *normal = -new_s2.n;
+ if(contact_points) *contact_points = p + new_s2.n * (0.5 * depth);
+ return true;
+ }
+ }
+ }
+
+
+ inline bool coneHalfspaceIntersect
+ (const Cone& s1, const Transform3f& tf1,
+ const Halfspace& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
+ {
+ Halfspace 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(cosa < halfspaceIntersectTolerance<FCL_REAL>())
+ {
+ FCL_REAL signed_dist = new_s2.signedDistance(T);
+ FCL_REAL depth = s1.radius - signed_dist;
+ if(depth < 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);
+ 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>())
+ C = Vec3f(0, 0, 0);
+ else
+ {
+ 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) + C;
+
+ FCL_REAL d1 = new_s2.signedDistance(p1);
+ FCL_REAL d2 = new_s2.signedDistance(p2);
+
+ 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);
+ return true;
+ }
+ }
+ }
+
+ inline bool convexHalfspaceIntersect
+ (const Convex& s1, const Transform3f& tf1,
+ const Halfspace& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
+ {
+ Halfspace new_s2 = transform(s2, tf2);
+
+ Vec3f v;
+ FCL_REAL depth = std::numeric_limits<FCL_REAL>::max();
+
+ for(int i = 0; i < s1.num_points; ++i)
+ {
+ Vec3f p = tf1.transform(s1.points[i]);
+
+ FCL_REAL d = new_s2.signedDistance(p);
+ if(d < depth)
+ {
+ depth = d;
+ v = p;
+ }
+ }
+
+ if(depth <= 0)
+ {
+ if(contact_points) *contact_points = v - new_s2.n * (0.5 * depth);
+ if(penetration_depth) *penetration_depth = depth;
+ if(normal) *normal = -new_s2.n;
+ return true;
+ }
+ else
+ return false;
+ }
+
+ // Intersection between half-space and triangle
+ // Half-space is in pose tf1,
+ // Triangle is in pose tf2
+ // Computes distance and closest points (p1, p2) if no collision,
+ // contact point (p1 = p2), normal and penetration depth (-distance)
+ // if collision
+ inline bool halfspaceTriangleIntersect
+ (const Halfspace& s1, const Transform3f& tf1, const Vec3f& P1,
+ const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
+ FCL_REAL& distance, Vec3f& p1, Vec3f& p2, Vec3f& normal)
+ {
+ Halfspace new_s1 = transform(s1, tf1);
+
+ Vec3f v = tf2.transform(P1);
+ FCL_REAL depth = new_s1.signedDistance(v);
+
+ Vec3f p = tf2.transform(P2);
+ FCL_REAL d = new_s1.signedDistance(p);
+ if(d < depth)
+ {
+ depth = d;
+ v = p;
+ }
+
+ p = tf2.transform(P3);
+ d = new_s1.signedDistance(p);
+ if(d < depth)
+ {
+ depth = d;
+ v = p;
+ }
+ // v is the vertex with minimal projection abscissa (depth) on normal to
+ //plane,
+ distance = depth;
+ if(depth <= 0)
+ {
+ normal = new_s1.n;
+ p1 = p2 = v - (0.5 * depth) * new_s1.n;
+ return true;
+ }
+ else
+ {
+ p1 = v - depth * new_s1.n;
+ p2 = v;
+ return false;
+ }
+ }
+
+
+ /// @brief return whether plane collides with halfspace
+ /// if the separation plane of the halfspace is parallel with the plane
+ /// return code 1, if the plane's normal is the same with halfspace's normal and plane is inside halfspace, also return plane in pl
+ /// return code 2, if the plane's normal is oppositie to the halfspace's normal and plane is inside halfspace, also return plane in pl
+ /// plane is outside halfspace, collision-free
+ /// if not parallel
+ /// return the intersection ray, return code 3. ray origin is p and direction is d
+ inline bool planeHalfspaceIntersect
+ (const Plane& s1, const Transform3f& tf1,
+ const Halfspace& s2, const Transform3f& tf2, Plane& pl, Vec3f& p,
+ Vec3f& d, FCL_REAL& penetration_depth, int& ret)
+ {
+ Plane new_s1 = transform(s1, tf1);
+ Halfspace new_s2 = transform(s2, tf2);
+
+ ret = 0;
+
+ Vec3f dir = (new_s1.n).cross(new_s2.n);
+ FCL_REAL dir_norm = dir.squaredNorm();
+ if(dir_norm < std::numeric_limits<FCL_REAL>::epsilon()) // parallel
+ {
+ if((new_s1.n).dot(new_s2.n) > 0)
+ {
+ if(new_s1.d < new_s2.d)
+ {
+ penetration_depth = new_s2.d - new_s1.d;
+ ret = 1;
+ pl = new_s1;
+ return true;
+ }
+ else
+ return false;
+ }
+ else
+ {
+ if(new_s1.d + new_s2.d > 0)
+ return false;
+ else
+ {
+ penetration_depth = -(new_s1.d + new_s2.d);
+ ret = 2;
+ pl = new_s1;
+ return true;
+ }
+ }
+ }
+
+ Vec3f n = new_s2.n * new_s1.d - new_s1.n * new_s2.d;
+ Vec3f origin = n.cross(dir);
+ origin *= (1.0 / dir_norm);
+
+ p = origin;
+ d = dir;
+ ret = 3;
+ penetration_depth = std::numeric_limits<FCL_REAL>::max();
+
+ return true;
+ }
+
+ ///@ brief return whether two halfspace intersect
+ /// if the separation planes of the two halfspaces are parallel
+ /// return code 1, if two halfspaces' normal are same and s1 is in s2, also return s1 in s;
+ /// return code 2, if two halfspaces' normal are same and s2 is in s1, also return s2 in s;
+ /// return code 3, if two halfspaces' normal are opposite and s1 and s2 are into each other;
+ /// collision free, if two halfspaces' are separate;
+ /// if the separation planes of the two halfspaces are not parallel, return intersection ray, return code 4. ray origin is p and direction is d
+ /// collision free return code 0
+ inline bool halfspaceIntersect(const Halfspace& s1, const Transform3f& tf1,
+ const Halfspace& s2, const Transform3f& tf2,
+ Vec3f& p, Vec3f& d,
+ Halfspace& s,
+ FCL_REAL& penetration_depth, int& ret)
+ {
+ Halfspace new_s1 = transform(s1, tf1);
+ Halfspace new_s2 = transform(s2, tf2);
+
+ ret = 0;
+
+ Vec3f dir = (new_s1.n).cross(new_s2.n);
+ FCL_REAL dir_norm = dir.squaredNorm();
+ if(dir_norm < std::numeric_limits<FCL_REAL>::epsilon()) // parallel
+ {
+ if((new_s1.n).dot(new_s2.n) > 0)
+ {
+ if(new_s1.d < new_s2.d) // s1 is inside s2
+ {
+ ret = 1;
+ penetration_depth = std::numeric_limits<FCL_REAL>::max();
+ s = new_s1;
+ }
+ else // s2 is inside s1
+ {
+ ret = 2;
+ penetration_depth = std::numeric_limits<FCL_REAL>::max();
+ s = new_s2;
+ }
+ return true;
+ }
+ else
+ {
+ if(new_s1.d + new_s2.d > 0) // not collision
+ return false;
+ else // in each other
+ {
+ ret = 3;
+ penetration_depth = -(new_s1.d + new_s2.d);
+ return true;
+ }
+ }
+ }
+
+ Vec3f n = new_s2.n * new_s1.d - new_s1.n * new_s2.d;
+ Vec3f origin = n.cross(dir);
+ origin *= (1.0 / dir_norm);
+
+ p = origin;
+ d = dir;
+ ret = 4;
+ penetration_depth = std::numeric_limits<FCL_REAL>::max();
+
+ return true;
+ }
+
+ template<typename T>
+ inline T planeIntersectTolerance()
+ {
+ return 0;
+ }
+
+ template<>
+ inline double planeIntersectTolerance<double>()
+ {
+ return 0.0000001;
+ }
+
+ template<>
+ float inline planeIntersectTolerance<float>()
+ {
+ return 0.0001f;
+ }
+
+ inline bool spherePlaneIntersect
+ (const Sphere& 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);
+
+ const Vec3f& center = tf1.getTranslation();
+ FCL_REAL signed_dist = new_s2.signedDistance(center);
+ FCL_REAL depth = - std::abs(signed_dist) + s1.radius;
+ if(depth >= 0)
+ {
+ if(normal) { if (signed_dist > 0) *normal = -new_s2.n; else *normal = new_s2.n; }
+ if(penetration_depth) *penetration_depth = depth;
+ if(contact_points) *contact_points = center - new_s2.n * signed_dist;
+
+ return true;
+ }
+ else
+ return false;
+ }
+
+ /// @brief box half space, a, b, c = +/- edge size
+ /// n^T * (R(o + a v1 + b v2 + c v3) + T) ~ d
+ /// so (R^T n) (a v1 + b v2 + c v3) + n * T ~ d
+ /// check whether d - n * T - (R^T n) (a v1 + b v2 + c v3) >= 0 for some a, b, c and <=0 for some a, b, c
+ /// so need to check whether |d - n * T| <= |(R^T n)(a v1 + b v2 + c v3)|, the reason is as follows:
+ /// (R^T n) (a v1 + b v2 + c v3) can get |(R^T n) (a v1 + b v2 + c v3)| for one a, b, c.
+ /// if |d - n * T| <= |(R^T n)(a v1 + b v2 + c v3)| then can get both positive and negative value on the right side.
+ inline bool boxPlaneIntersect(const Box& 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);
+
+ const Matrix3f& R = tf1.getRotation();
+ const Vec3f& T = tf1.getTranslation();
+
+ Vec3f Q = R.transpose() * new_s2.n;
+ Vec3f A(Q[0] * s1.side[0], Q[1] * s1.side[1], Q[2] * s1.side[2]);
+ Vec3f B = A.cwiseAbs();
+
+ FCL_REAL signed_dist = new_s2.signedDistance(T);
+ FCL_REAL depth = 0.5 * (B[0] + B[1] + B[2]) - std::abs(signed_dist);
+ if(depth < 0) return false;
+
+ Vec3f axis[3];
+ axis[0] = R.col(0);
+ axis[1] = R.col(1);
+ axis[2] = R.col(2);
+
+ // find the deepest point
+ Vec3f p = T;
+
+ // when center is on the positive side of the plane, use a, b, c make (R^T n) (a v1 + b v2 + c v3) the minimum
+ // otherwise, use a, b, c make (R^T n) (a v1 + b v2 + c v3) the maximum
+ int sign = (signed_dist > 0) ? 1 : -1;
+
+ if(std::abs(Q[0] - 1) < planeIntersectTolerance<FCL_REAL>() || std::abs(Q[0] + 1) < planeIntersectTolerance<FCL_REAL>())
+ {
+ int sign2 = (A[0] > 0) ? -1 : 1;
+ sign2 *= sign;
+ p += axis[0] * (0.5 * s1.side[0] * sign2);
+ }
+ else if(std::abs(Q[1] - 1) < planeIntersectTolerance<FCL_REAL>() || std::abs(Q[1] + 1) < planeIntersectTolerance<FCL_REAL>())
+ {
+ int sign2 = (A[1] > 0) ? -1 : 1;
+ sign2 *= sign;
+ p += axis[1] * (0.5 * s1.side[1] * sign2);
+ }
+ else if(std::abs(Q[2] - 1) < planeIntersectTolerance<FCL_REAL>() || std::abs(Q[2] + 1) < planeIntersectTolerance<FCL_REAL>())
+ {
+ int sign2 = (A[2] > 0) ? -1 : 1;
+ sign2 *= sign;
+ p += axis[2] * (0.5 * s1.side[2] * sign2);
+ }
+ else
+ {
+ for(std::size_t i = 0; i < 3; ++i)
+ {
+ int sign2 = (A[i] > 0) ? -1 : 1;
+ sign2 *= sign;
+ p += axis[i] * (0.5 * s1.side[i] * sign2);
+ }
+ }
+
+ // compute the contact point by project the deepest point onto the plane
+ if(penetration_depth) *penetration_depth = depth;
+ if(normal) { if (signed_dist > 0) *normal = -new_s2.n; else *normal = new_s2.n; }
+ if(contact_points) *contact_points = p - new_s2.n * new_s2.signedDistance(p);
+
+ return true;
+ }
+
+
+ inline bool capsulePlaneIntersect(const Capsule& s1, const Transform3f& tf1,
+ const Plane& s2, const Transform3f& tf2)
+ {
+ 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);
+
+ // two end points on different side of the plane
+ if(d1 * d2 <= 0)
+ return true;
+
+ // 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);
+ }
+
+ 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);
+
+ if(!contact_points && !penetration_depth && !normal)
+ return capsulePlaneIntersect(s1, tf1, s2, tf2);
+ else
+ {
+ 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);
+
+ 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(normal) { if (d1 < 0) *normal = new_s2.n; else *normal = -new_s2.n; }
+ return true;
+ }
+ }
+ }
+
+ /// @brief cylinder-plane intersect
+ /// n^T (R (r * cosa * v1 + r * sina * v2 + h * v3) + T) ~ d
+ /// need one point to be positive and one to be negative
+ /// (n^T * v3) * h + n * T -d + r * (cosa * (n^T * R * v1) + sina * (n^T * R * v2)) ~ 0
+ /// (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)
+ {
+ 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;
+ }
+
+ 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);
+ 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;
+ }
+
+ Vec3f p1 = T + dir_z * (0.5 * s1.lz);
+ Vec3f p2 = 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;
+ }
+
+ 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(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;
+ }
+ }
+ }
+
+ inline bool conePlaneIntersect
+ (const Cone& 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);
+
+ 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 - dir_z * (0.5 * s1.lz) + dir_z * (0.5 * depth / 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>())
+ C = Vec3f(0, 0, 0);
+ else
+ {
+ FCL_REAL s = C.norm();
+ s = s1.radius / s;
+ C *= s;
+ }
+
+ Vec3f c[3];
+ c[0] = T + dir_z * (0.5 * s1.lz);
+ c[1] = T - dir_z * (0.5 * s1.lz) + C;
+ c[2] = T - dir_z * (0.5 * s1.lz) - C;
+
+ FCL_REAL d[3];
+ d[0] = new_s2.signedDistance(c[0]);
+ 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))
+ return false;
+ else
+ {
+ bool positive[3];
+ for(std::size_t i = 0; i < 3; ++i)
+ positive[i] = (d[i] >= 0);
+
+ int n_positive = 0;
+ FCL_REAL d_positive = 0, d_negative = 0;
+ for(std::size_t i = 0; i < 3; ++i)
+ {
+ if(positive[i])
+ {
+ n_positive++;
+ if(d_positive <= d[i]) d_positive = d[i];
+ }
+ else
+ {
+ if(d_negative <= -d[i]) d_negative = -d[i];
+ }
+ }
+
+ 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);
+
+ 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;
+ }
+ 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]; }
+ }
+
+ 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;
+ }
+ }
+ return true;
+ }
+ }
+ }
+
+ inline bool convexPlaneIntersect
+ (const Convex& 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);
+
+ Vec3f v_min, v_max;
+ FCL_REAL d_min = std::numeric_limits<FCL_REAL>::max(), d_max = -std::numeric_limits<FCL_REAL>::max();
+
+ for(int i = 0; i < s1.num_points; ++i)
+ {
+ Vec3f p = tf1.transform(s1.points[i]);
+
+ FCL_REAL d = new_s2.signedDistance(p);
+
+ if(d < d_min) { d_min = d; v_min = p; }
+ if(d > d_max) { d_max = d; v_max = p; }
+ }
+
+ if(d_min * d_max > 0) return false;
+ else
+ {
+ if(d_min + d_max > 0)
+ {
+ if(penetration_depth) *penetration_depth = -d_min;
+ if(normal) *normal = -new_s2.n;
+ if(contact_points) *contact_points = v_min - new_s2.n * d_min;
+ }
+ else
+ {
+ if(penetration_depth) *penetration_depth = d_max;
+ if(normal) *normal = new_s2.n;
+ if(contact_points) *contact_points = v_max - new_s2.n * d_max;
+ }
+ return true;
+ }
+ }
+
+
+
+ inline bool planeTriangleIntersect
+ (const Plane& s1, const Transform3f& tf1, const Vec3f& P1,
+ const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
+ FCL_REAL distance, Vec3f& p1, Vec3f& p2, Vec3f& normal)
+ {
+ Plane new_s1 = transform(s1, tf1);
+
+ Vec3f c[3];
+ c[0] = tf2.transform(P1);
+ c[1] = tf2.transform(P2);
+ c[2] = tf2.transform(P3);
+
+ FCL_REAL d[3];
+ d[0] = new_s1.signedDistance(c[0]);
+ d[1] = new_s1.signedDistance(c[1]);
+ d[2] = new_s1.signedDistance(c[2]);
+ int imin;
+ if (d[0] >= 0 && d[1] >= 0 && d[2] >= 0)
+ {
+ if (d[0] < d[1])
+ if (d[0] < d[2]) {
+ imin = 0;
+ }
+ else { // d [2] <= d[0] < d [1]
+ imin = 2;
+ }
+ else { // d[1] <= d[0]
+ if (d[2] < d[1]) {
+ imin = 2;
+ } else { // d[1] <= d[2]
+ imin = 1;
+ }
+ }
+ distance = d[imin];
+ p2 = c[imin];
+ p1 = c[imin] - d[imin] * new_s1.n;
+ return false;
+ }
+ if (d[0] <= 0 && d[1] <= 0 && d[2] <= 0)
+ {
+ if (d[0] > d[1])
+ if (d[0] > d[2]) {
+ imin = 0;
+ }
+ else { // d [2] >= d[0] > d [1]
+ imin = 2;
+ }
+ else { // d[1] >= d[0]
+ if (d[2] > d[1]) {
+ imin = 2;
+ } else { // d[1] >= d[2]
+ imin = 1;
+ }
+ }
+ distance = -d[imin];
+ p2 = c[imin];
+ p1 = c[imin] - d[imin] * new_s1.n;
+ return false;
+ }
+ bool positive[3];
+ for(std::size_t i = 0; i < 3; ++i)
+ positive[i] = (d[i] > 0);
+
+ int n_positive = 0;
+ FCL_REAL d_positive = 0, d_negative = 0;
+ for(std::size_t i = 0; i < 3; ++i)
+ {
+ if(positive[i])
+ {
+ n_positive++;
+ if(d_positive <= d[i]) d_positive = d[i];
+ }
+ else
+ {
+ if(d_negative <= -d[i]) d_negative = -d[i];
+ }
+ }
+
+ distance = -std::min(d_positive, d_negative);
+ if (d_positive > d_negative)
+ {
+ normal = new_s1.n;
+ } else
+ {
+ normal = -new_s1.n;
+ }
+ Vec3f p[2];
+ Vec3f q;
+
+ FCL_REAL p_d[2];
+ FCL_REAL q_d(0);
+
+ 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]);
+ 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]; }
+ }
+
+ 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;
+ }
+
+ inline bool halfspacePlaneIntersect
+ (const Halfspace& s1, const Transform3f& tf1,
+ const Plane& s2, const Transform3f& tf2,
+ Plane& pl, Vec3f& p, Vec3f& d, FCL_REAL& penetration_depth, int& ret)
+ {
+ return planeHalfspaceIntersect(s2, tf2, s1, tf1, pl, p, d, penetration_depth, ret);
+ }
+
+ inline bool planeIntersect
+ (const Plane& s1, const Transform3f& tf1,
+ const Plane& s2, const Transform3f& tf2,
+ Vec3f* /*contact_points*/, FCL_REAL* /*penetration_depth*/, Vec3f* /*normal*/)
+ {
+ Plane new_s1 = transform(s1, tf1);
+ Plane new_s2 = transform(s2, tf2);
+
+ FCL_REAL a = (new_s1.n).dot(new_s2.n);
+ if(a == 1 && new_s1.d != new_s2.d)
+ return false;
+ if(a == -1 && new_s1.d != -new_s2.d)
+ return false;
+
+ return true;
+ }
+ // Compute penetration distance and normal of two triangles in collision
+ // Normal is normal of triangle 1 (P1, P2, P3), penetration depth is the
+ // minimal distance (Q1, Q2, Q3) should be translated along the normal so
+ // that the triangles are collision free.
+ //
+ // Note that we compute here an upper bound of the penetration distance,
+ // not the exact value.
+ static FCL_REAL computePenetration
+ (const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
+ const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3, const Vec3f& p1,
+ const Transform3f& tf1, const Transform3f& tf2, Vec3f& normal)
+ {
+ Vec3f globalP1 (tf1.transform (P1));
+ Vec3f globalP2 (tf1.transform (P2));
+ Vec3f globalP3 (tf1.transform (P3));
+ Vec3f globalQ1 (tf2.transform (Q1));
+ Vec3f globalQ2 (tf2.transform (Q2));
+ Vec3f globalQ3 (tf2.transform (Q3));
+ Vec3f u ((globalP2-globalP1).cross (globalP3-globalP1));
+ normal = u.normalized ();
+ FCL_REAL depth;
+ FCL_REAL depth1 ((globalP1-globalQ1).dot (normal));
+ FCL_REAL depth2 ((globalP1-globalQ2).dot (normal));
+ FCL_REAL depth3 ((globalP1-globalQ3).dot (normal));
+ depth = std::max (depth1, std::max (depth2, depth3));
+ return depth;
+ }
+ } // details
+} // namespace fcl
+#endif // HPP_FCL_SRC_NARROWPHASE_DETAILS_H
diff --git a/src/narrowphase/narrowphase.cpp b/src/narrowphase/narrowphase.cpp
index d7fdea15..8fe4bb7c 100644
--- a/src/narrowphase/narrowphase.cpp
+++ b/src/narrowphase/narrowphase.cpp
@@ -40,2480 +40,10 @@
#include <hpp/fcl/intersect.h>
#include <boost/math/constants/constants.hpp>
#include <vector>
+#include "../src/narrowphase/details.h"
namespace fcl
{
-
-namespace details
-{
-// Compute the point on a line segment that is the closest point on the
-// segment to to another point. The code is inspired by the explanation
-// given by Dan Sunday's page:
-// http://geomalgorithms.com/a02-_lines.html
-static inline void lineSegmentPointClosestToPoint (const Vec3f &p, const Vec3f &s1, const Vec3f &s2, Vec3f &sp) {
- Vec3f v = s2 - s1;
- Vec3f w = p - s1;
-
- FCL_REAL c1 = w.dot(v);
- FCL_REAL c2 = v.dot(v);
-
- if (c1 <= 0) {
- sp = s1;
- } else if (c2 <= c1) {
- sp = s2;
- } else {
- FCL_REAL b = c1/c2;
- Vec3f Pb = s1 + v * b;
- sp = Pb;
- }
-}
-
-bool sphereCapsuleIntersect(const Sphere& s1, const Transform3f& tf1,
- const Capsule& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal_)
-{
- Transform3f tf2_inv (tf2);
- tf2_inv.inverse();
-
- Vec3f pos1 (tf2.transform (Vec3f (0., 0., 0.5 * s2.lz))); // from distance function
- Vec3f pos2 (tf2.transform (Vec3f (0., 0., -0.5 * s2.lz)));
- Vec3f s_c = tf1.getTranslation ();
-
- Vec3f segment_point;
-
- lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point);
- Vec3f diff = s_c - segment_point;
-
- FCL_REAL diffN = diff.norm();
- FCL_REAL distance = diffN - s1.radius - s2.radius;
-
- if (distance > 0)
- return false;
-
- // Vec3f normal (-diff.normalized());
-
- if (distance < 0 && penetration_depth)
- *penetration_depth = -distance;
-
- if (normal_)
- *normal_ = -diff / diffN;
-
- if (contact_points) {
- *contact_points = segment_point + diff * s2.radius;
- }
-
- return true;
-}
-
-bool sphereCapsuleDistance(const Sphere& s1, const Transform3f& tf1,
- const Capsule& s2, const Transform3f& tf2,
- FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal)
-{
- Transform3f tf2_inv(tf2);
- tf2_inv.inverse();
-
- Vec3f pos1 (tf2.transform (Vec3f (0., 0., 0.5 * s2.lz)));
- Vec3f pos2 (tf2.transform (Vec3f (0., 0., -0.5 * s2.lz)));
- Vec3f s_c = tf1.getTranslation ();
-
- Vec3f segment_point;
-
- lineSegmentPointClosestToPoint (s_c, pos1, pos2, segment_point);
- normal = s_c - segment_point;
-
- dist = normal.norm() - s1.radius - s2.radius;
-
- normal.normalize();
- p1 = s_c - normal * s1.radius;
- p2 = segment_point + normal * s1.radius;
-
- if(dist <= 0)
- return false;
-
- return true;
-}
-
-bool sphereSphereIntersect(const Sphere& s1, const Transform3f& tf1,
- const Sphere& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
-{
- Vec3f diff = tf2.getTranslation() - tf1.getTranslation();
- FCL_REAL len = diff.norm();
- if(len > s1.radius + s2.radius)
- return false;
-
- if(penetration_depth)
- *penetration_depth = s1.radius + s2.radius - len;
-
- // If the centers of two sphere are at the same position, the normal is (0, 0, 0).
- // Otherwise, normal is pointing from center of object 1 to center of object 2
- if(normal)
- {
- if(len > 0)
- *normal = diff / len;
- else
- *normal = diff;
- }
-
- if(contact_points)
- *contact_points = tf1.getTranslation() + diff * s1.radius / (s1.radius + s2.radius);
-
- return true;
-}
-
-
-bool sphereSphereDistance(const Sphere& s1, const Transform3f& tf1,
- const Sphere& s2, const Transform3f& tf2,
- FCL_REAL& dist, Vec3f& p1, Vec3f& p2, Vec3f& normal)
-{
- Vec3f o1 = tf1.getTranslation();
- Vec3f o2 = tf2.getTranslation();
- Vec3f diff = o1 - o2;
- FCL_REAL len = diff.norm();
- normal = -diff/len;
- dist = len - s1.radius - s2.radius;
-
- p1 = o1 - diff * (s1.radius / len);
- p2 = o2 + diff * (s2.radius / len);
-
- return (dist >=0);
-}
-
-/** \brief the minimum distance from a point to a line */
-FCL_REAL segmentSqrDistance(const Vec3f& from, const Vec3f& to,const Vec3f& p, Vec3f& nearest)
-{
- Vec3f diff = p - from;
- Vec3f v = to - from;
- FCL_REAL t = v.dot(diff);
-
- if(t > 0)
- {
- FCL_REAL dotVV = v.dot(v);
- if(t < dotVV)
- {
- t /= dotVV;
- diff -= v * t;
- }
- else
- {
- t = 1;
- diff -= v;
- }
- }
- else
- t = 0;
-
- nearest = from + v * t;
- return diff.dot(diff);
-}
-
-/// @brief Whether a point's projection is in a triangle
-bool projectInTriangle(const Vec3f& p1, const Vec3f& p2, const Vec3f& p3, const Vec3f& normal, const Vec3f& p)
-{
- Vec3f edge1(p2 - p1);
- Vec3f edge2(p3 - p2);
- Vec3f edge3(p1 - p3);
-
- Vec3f p1_to_p(p - p1);
- Vec3f p2_to_p(p - p2);
- Vec3f p3_to_p(p - p3);
-
- Vec3f edge1_normal(edge1.cross(normal));
- Vec3f edge2_normal(edge2.cross(normal));
- Vec3f edge3_normal(edge3.cross(normal));
-
- FCL_REAL r1, r2, r3;
- r1 = edge1_normal.dot(p1_to_p);
- r2 = edge2_normal.dot(p2_to_p);
- r3 = edge3_normal.dot(p3_to_p);
- if ( ( r1 > 0 && r2 > 0 && r3 > 0 ) ||
- ( r1 <= 0 && r2 <= 0 && r3 <= 0 ) )
- return true;
- return false;
-}
-
-// Intersection between sphere and triangle
-// Sphere is in position tf1, Triangle is expressed in global frame
-bool sphereTriangleIntersect(const Sphere& s, const Transform3f& tf1,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
- Vec3f& normal_)
-{
- Vec3f normal = (P2 - P1).cross(P3 - P1);
- normal.normalize();
- const Vec3f& center = tf1.getTranslation();
- const FCL_REAL& radius = s.radius;
- assert (radius >= 0);
- FCL_REAL eps (std::numeric_limits<FCL_REAL>::epsilon());
- FCL_REAL radius_with_threshold = radius + eps;
- Vec3f p1_to_center = center - P1;
- FCL_REAL distance_from_plane = p1_to_center.dot(normal);
-
- if(distance_from_plane < 0)
- {
- distance_from_plane *= -1;
- normal *= -1;
- }
-
- bool is_inside_contact_plane = (distance_from_plane < radius_with_threshold);
-
- bool has_contact = false;
- Vec3f contact_point;
- if(is_inside_contact_plane)
- {
- if(projectInTriangle(P1, P2, P3, normal, center))
- {
- has_contact = true;
- contact_point = center - normal * distance_from_plane;
- }
- else
- {
- FCL_REAL contact_capsule_radius_sqr = radius_with_threshold * radius_with_threshold;
- Vec3f nearest_on_edge;
- FCL_REAL distance_sqr;
- distance_sqr = segmentSqrDistance(P1, P2, center, nearest_on_edge);
- if(distance_sqr < contact_capsule_radius_sqr)
- {
- has_contact = true;
- contact_point = nearest_on_edge;
- }
-
- distance_sqr = segmentSqrDistance(P2, P3, center, nearest_on_edge);
- if(distance_sqr < contact_capsule_radius_sqr)
- {
- has_contact = true;
- contact_point = nearest_on_edge;
- }
-
- distance_sqr = segmentSqrDistance(P3, P1, center, nearest_on_edge);
- if(distance_sqr < contact_capsule_radius_sqr)
- {
- has_contact = true;
- contact_point = nearest_on_edge;
- }
- }
- }
-
- Vec3f center_to_contact = contact_point - center;
- if(has_contact)
- {
- FCL_REAL distance_sqr = center_to_contact.squaredNorm();
-
- if(distance_sqr < radius_with_threshold * radius_with_threshold)
- {
- if(distance_sqr > eps)
- {
- FCL_REAL dist = std::sqrt(distance_sqr);
- normal_ = center_to_contact.normalized();
- p1 = p2 = contact_point;
- distance = dist - radius;
- }
- else
- {
- normal_ = -normal;
- p1 = p2 = contact_point;
- distance = - radius;
- }
- }
- }
- else
- {
- Vec3f unit (center_to_contact.normalized ());
- p1 = center + radius * unit;
- p2 = contact_point;
- }
- return has_contact;
-}
-
-
-bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- FCL_REAL* dist)
-{
- // from geometric tools, very different from the collision code.
-
- const Vec3f& center = tf.getTranslation();
- FCL_REAL radius = sp.radius;
- Vec3f diff = P1 - center;
- Vec3f edge0 = P2 - P1;
- Vec3f edge1 = P3 - P1;
- FCL_REAL a00 = edge0.squaredNorm();
- FCL_REAL a01 = edge0.dot(edge1);
- FCL_REAL a11 = edge1.squaredNorm();
- FCL_REAL b0 = diff.dot(edge0);
- FCL_REAL b1 = diff.dot(edge1);
- FCL_REAL c = diff.squaredNorm();
- FCL_REAL det = fabs(a00*a11 - a01*a01);
- FCL_REAL s = a01*b1 - a11*b0;
- FCL_REAL t = a01*b0 - a00*b1;
-
- FCL_REAL sqr_dist;
-
- if(s + t <= det)
- {
- if(s < 0)
- {
- if(t < 0) // region 4
- {
- if(b0 < 0)
- {
- t = 0;
- if(-b0 >= a00)
- {
- s = 1;
- sqr_dist = a00 + 2*b0 + c;
- }
- else
- {
- s = -b0/a00;
- sqr_dist = b0*s + c;
- }
- }
- else
- {
- s = 0;
- if(b1 >= 0)
- {
- t = 0;
- sqr_dist = c;
- }
- else if(-b1 >= a11)
- {
- t = 1;
- sqr_dist = a11 + 2*b1 + c;
- }
- else
- {
- t = -b1/a11;
- sqr_dist = b1*t + c;
- }
- }
- }
- else // region 3
- {
- s = 0;
- if(b1 >= 0)
- {
- t = 0;
- sqr_dist = c;
- }
- else if(-b1 >= a11)
- {
- t = 1;
- sqr_dist = a11 + 2*b1 + c;
- }
- else
- {
- t = -b1/a11;
- sqr_dist = b1*t + c;
- }
- }
- }
- else if(t < 0) // region 5
- {
- t = 0;
- if(b0 >= 0)
- {
- s = 0;
- sqr_dist = c;
- }
- else if(-b0 >= a00)
- {
- s = 1;
- sqr_dist = a00 + 2*b0 + c;
- }
- else
- {
- s = -b0/a00;
- sqr_dist = b0*s + c;
- }
- }
- else // region 0
- {
- // minimum at interior point
- FCL_REAL inv_det = (1)/det;
- s *= inv_det;
- t *= inv_det;
- sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
- }
- }
- else
- {
- FCL_REAL tmp0, tmp1, numer, denom;
-
- if(s < 0) // region 2
- {
- tmp0 = a01 + b0;
- tmp1 = a11 + b1;
- if(tmp1 > tmp0)
- {
- numer = tmp1 - tmp0;
- denom = a00 - 2*a01 + a11;
- if(numer >= denom)
- {
- s = 1;
- t = 0;
- sqr_dist = a00 + 2*b0 + c;
- }
- else
- {
- s = numer/denom;
- t = 1 - s;
- sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
- }
- }
- else
- {
- s = 0;
- if(tmp1 <= 0)
- {
- t = 1;
- sqr_dist = a11 + 2*b1 + c;
- }
- else if(b1 >= 0)
- {
- t = 0;
- sqr_dist = c;
- }
- else
- {
- t = -b1/a11;
- sqr_dist = b1*t + c;
- }
- }
- }
- else if(t < 0) // region 6
- {
- tmp0 = a01 + b1;
- tmp1 = a00 + b0;
- if(tmp1 > tmp0)
- {
- numer = tmp1 - tmp0;
- denom = a00 - 2*a01 + a11;
- if(numer >= denom)
- {
- t = 1;
- s = 0;
- sqr_dist = a11 + 2*b1 + c;
- }
- else
- {
- t = numer/denom;
- s = 1 - t;
- sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
- }
- }
- else
- {
- t = 0;
- if(tmp1 <= 0)
- {
- s = 1;
- sqr_dist = a00 + 2*b0 + c;
- }
- else if(b0 >= 0)
- {
- s = 0;
- sqr_dist = c;
- }
- else
- {
- s = -b0/a00;
- sqr_dist = b0*s + c;
- }
- }
- }
- else // region 1
- {
- numer = a11 + b1 - a01 - b0;
- if(numer <= 0)
- {
- s = 0;
- t = 1;
- sqr_dist = a11 + 2*b1 + c;
- }
- else
- {
- denom = a00 - 2*a01 + a11;
- if(numer >= denom)
- {
- s = 1;
- t = 0;
- sqr_dist = a00 + 2*b0 + c;
- }
- else
- {
- s = numer/denom;
- t = 1 - s;
- sqr_dist = s*(a00*s + a01*t + 2*b0) + t*(a01*s + a11*t + 2*b1) + c;
- }
- }
- }
- }
-
- // Account for numerical round-off error.
- if(sqr_dist < 0)
- sqr_dist = 0;
-
- if(sqr_dist > radius * radius)
- {
- if(dist) *dist = std::sqrt(sqr_dist) - radius;
- return true;
- }
- else
- {
- if(dist) *dist = -1;
- return false;
- }
-}
-
-
-bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- FCL_REAL* dist, Vec3f* p1, Vec3f* p2)
-{
- if(p1 || p2)
- {
- Vec3f o = tf.getTranslation();
- Project::ProjectResult result;
- result = Project::projectTriangle(P1, P2, P3, o);
- if(result.sqr_distance > sp.radius * sp.radius)
- {
- if(dist) *dist = std::sqrt(result.sqr_distance) - sp.radius;
- Vec3f project_p = P1 * result.parameterization[0] + P2 * result.parameterization[1] + P3 * result.parameterization[2];
- Vec3f dir = o - project_p;
- dir.normalize();
- if(p1) { *p1 = o - dir * sp.radius; }
- if(p2) *p2 = project_p;
- return true;
- }
- else
- return false;
- }
- else
- {
- return sphereTriangleDistance(sp, tf, P1, P2, P3, dist);
- }
-}
-
-
-bool sphereTriangleDistance(const Sphere& sp, const Transform3f& tf1,
- const Vec3f& P1, const Vec3f& P2, const Vec3f& P3, const Transform3f& tf2,
- FCL_REAL* dist, Vec3f* p1, Vec3f* p2)
-{
- bool res = details::sphereTriangleDistance(sp, tf1, tf2.transform(P1), tf2.transform(P2), tf2.transform(P3), dist, p1, p2);
- return res;
-}
-
-
-
-struct ContactPoint
-{
- Vec3f normal;
- Vec3f point;
- FCL_REAL depth;
- ContactPoint(const Vec3f& n, const Vec3f& p, FCL_REAL d) : normal(n), point(p), depth(d) {}
-};
-
-
-static inline void lineClosestApproach(const Vec3f& pa, const Vec3f& ua,
- const Vec3f& pb, const Vec3f& ub,
- FCL_REAL* alpha, FCL_REAL* beta)
-{
- Vec3f p = pb - pa;
- FCL_REAL uaub = ua.dot(ub);
- FCL_REAL q1 = ua.dot(p);
- FCL_REAL q2 = -ub.dot(p);
- FCL_REAL d = 1 - uaub * uaub;
- if(d <= (FCL_REAL)(0.0001f))
- {
- *alpha = 0;
- *beta = 0;
- }
- else
- {
- d = 1 / d;
- *alpha = (q1 + uaub * q2) * d;
- *beta = (uaub * q1 + q2) * d;
- }
-}
-
-// find all the intersection points between the 2D rectangle with vertices
-// at (+/-h[0],+/-h[1]) and the 2D quadrilateral with vertices (p[0],p[1]),
-// (p[2],p[3]),(p[4],p[5]),(p[6],p[7]).
-//
-// the intersection points are returned as x,y pairs in the 'ret' array.
-// the number of intersection points is returned by the function (this will
-// be in the range 0 to 8).
-static int intersectRectQuad2(FCL_REAL h[2], FCL_REAL p[8], FCL_REAL ret[16])
-{
- // q (and r) contain nq (and nr) coordinate points for the current (and
- // chopped) polygons
- int nq = 4, nr = 0;
- FCL_REAL buffer[16];
- FCL_REAL* q = p;
- FCL_REAL* r = ret;
- for(int dir = 0; dir <= 1; ++dir)
- {
- // direction notation: xy[0] = x axis, xy[1] = y axis
- for(int sign = -1; sign <= 1; sign += 2)
- {
- // chop q along the line xy[dir] = sign*h[dir]
- FCL_REAL* pq = q;
- FCL_REAL* pr = r;
- nr = 0;
- for(int i = nq; i > 0; --i)
- {
- // go through all points in q and all lines between adjacent points
- if(sign * pq[dir] < h[dir])
- {
- // this point is inside the chopping line
- pr[0] = pq[0];
- pr[1] = pq[1];
- pr += 2;
- nr++;
- if(nr & 8)
- {
- q = r;
- goto done;
- }
- }
- FCL_REAL* nextq = (i > 1) ? pq+2 : q;
- if((sign*pq[dir] < h[dir]) ^ (sign*nextq[dir] < h[dir]))
- {
- // this line crosses the chopping line
- pr[1-dir] = pq[1-dir] + (nextq[1-dir]-pq[1-dir]) /
- (nextq[dir]-pq[dir]) * (sign*h[dir]-pq[dir]);
- pr[dir] = sign*h[dir];
- pr += 2;
- nr++;
- if(nr & 8)
- {
- q = r;
- goto done;
- }
- }
- pq += 2;
- }
- q = r;
- r = (q == ret) ? buffer : ret;
- nq = nr;
- }
- }
-
- done:
- if(q != ret) memcpy(ret, q, nr*2*sizeof(FCL_REAL));
- return nr;
-}
-
-// given n points in the plane (array p, of size 2*n), generate m points that
-// best represent the whole set. the definition of 'best' here is not
-// predetermined - the idea is to select points that give good box-box
-// collision detection behavior. the chosen point indexes are returned in the
-// array iret (of size m). 'i0' is always the first entry in the array.
-// n must be in the range [1..8]. m must be in the range [1..n]. i0 must be
-// in the range [0..n-1].
-static inline void cullPoints2(int n, FCL_REAL p[], int m, int i0, int iret[])
-{
- // compute the centroid of the polygon in cx,cy
- FCL_REAL a, cx, cy, q;
- switch(n)
- {
- case 1:
- cx = p[0];
- cy = p[1];
- break;
- case 2:
- cx = 0.5 * (p[0] + p[2]);
- cy = 0.5 * (p[1] + p[3]);
- break;
- default:
- a = 0;
- cx = 0;
- cy = 0;
- for(int i = 0; i < n-1; ++i)
- {
- q = p[i*2]*p[i*2+3] - p[i*2+2]*p[i*2+1];
- a += q;
- cx += q*(p[i*2]+p[i*2+2]);
- cy += q*(p[i*2+1]+p[i*2+3]);
- }
- q = p[n*2-2]*p[1] - p[0]*p[n*2-1];
- if(std::abs(a+q) > std::numeric_limits<FCL_REAL>::epsilon())
- a = 1/(3*(a+q));
- else
- a= 1e18f;
-
- cx = a*(cx + q*(p[n*2-2]+p[0]));
- cy = a*(cy + q*(p[n*2-1]+p[1]));
- }
-
-
- // compute the angle of each point w.r.t. the centroid
- FCL_REAL A[8];
- for(int i = 0; i < n; ++i)
- A[i] = atan2(p[i*2+1]-cy,p[i*2]-cx);
-
- // search for points that have angles closest to A[i0] + i*(2*pi/m).
- int avail[8];
- for(int i = 0; i < n; ++i) avail[i] = 1;
- avail[i0] = 0;
- iret[0] = i0;
- iret++;
- const double pi = boost::math::constants::pi<FCL_REAL>();
- for(int j = 1; j < m; ++j)
- {
- a = j*(2*pi/m) + A[i0];
- if (a > pi) a -= 2*pi;
- FCL_REAL maxdiff= 1e9, diff;
-
- *iret = i0; // iret is not allowed to keep this value, but it sometimes does, when diff=#QNAN0
- for(int i = 0; i < n; ++i)
- {
- if(avail[i])
- {
- diff = std::abs(A[i]-a);
- if(diff > pi) diff = 2*pi - diff;
- if(diff < maxdiff)
- {
- maxdiff = diff;
- *iret = i;
- }
- }
- }
- avail[*iret] = 0;
- iret++;
- }
-}
-
-
-
-int boxBox2(const Vec3f& side1, const Matrix3f& R1, const Vec3f& T1,
- const Vec3f& side2, const Matrix3f& R2, const Vec3f& T2,
- Vec3f& normal, FCL_REAL* depth, int* return_code,
- int maxc, std::vector<ContactPoint>& contacts)
-{
- const FCL_REAL fudge_factor = FCL_REAL(1.05);
- Vec3f normalC;
- FCL_REAL s, s2, l;
- int invert_normal, code;
-
- Vec3f p (T2 - T1); // get vector from centers of box 1 to box 2, relative to box 1
- Vec3f pp (R1.transpose() * p); // get pp = p relative to body 1
-
- // get side lengths / 2
- Vec3f A (side1 * 0.5);
- Vec3f B (side2 * 0.5);
-
- // Rij is R1'*R2, i.e. the relative rotation between R1 and R2
- Matrix3f R (R1.transpose() * R2);
- Matrix3f Q (R.cwiseAbs());
-
-
- // for all 15 possible separating axes:
- // * see if the axis separates the boxes. if so, return 0.
- // * find the depth of the penetration along the separating axis (s2)
- // * if this is the largest depth so far, record it.
- // the normal vector will be set to the separating axis with the smallest
- // depth. note: normalR is set to point to a column of R1 or R2 if that is
- // the smallest depth normal so far. otherwise normalR is 0 and normalC is
- // set to a vector relative to body 1. invert_normal is 1 if the sign of
- // the normal should be flipped.
-
- int best_col_id = -1;
- const Matrix3f* normalR = 0;
- FCL_REAL tmp = 0;
-
- s = - std::numeric_limits<FCL_REAL>::max();
- invert_normal = 0;
- code = 0;
-
- // separating axis = u1, u2, u3
- tmp = pp[0];
- s2 = std::abs(tmp) - (Q.row(0).dot(B) + A[0]);
- if(s2 > 0) { *return_code = 0; return 0; }
- if(s2 > s)
- {
- s = s2;
- best_col_id = 0;
- normalR = &R1;
- invert_normal = (tmp < 0);
- code = 1;
- }
-
- tmp = pp[1];
- s2 = std::abs(tmp) - (Q.row(1).dot(B) + A[1]);
- if(s2 > 0) { *return_code = 0; return 0; }
- if(s2 > s)
- {
- s = s2;
- best_col_id = 1;
- normalR = &R1;
- invert_normal = (tmp < 0);
- code = 2;
- }
-
- tmp = pp[2];
- s2 = std::abs(tmp) - (Q.row(2).dot(B) + A[2]);
- if(s2 > 0) { *return_code = 0; return 0; }
- if(s2 > s)
- {
- s = s2;
- best_col_id = 2;
- normalR = &R1;
- invert_normal = (tmp < 0);
- code = 3;
- }
-
- // separating axis = v1, v2, v3
- tmp = R2.col(0).dot(p);
- s2 = std::abs(tmp) - (Q.col(0).dot(A) + B[0]);
- if(s2 > 0) { *return_code = 0; return 0; }
- if(s2 > s)
- {
- s = s2;
- best_col_id = 0;
- normalR = &R2;
- invert_normal = (tmp < 0);
- code = 4;
- }
-
- tmp = R2.col(1).dot(p);
- s2 = std::abs(tmp) - (Q.col(1).dot(A) + B[1]);
- if(s2 > 0) { *return_code = 0; return 0; }
- if(s2 > s)
- {
- s = s2;
- best_col_id = 1;
- normalR = &R2;
- invert_normal = (tmp < 0);
- code = 5;
- }
-
- tmp = R2.col(2).dot(p);
- s2 = std::abs(tmp) - (Q.col(2).dot(A) + B[2]);
- if(s2 > 0) { *return_code = 0; return 0; }
- if(s2 > s)
- {
- s = s2;
- best_col_id = 2;
- normalR = &R2;
- invert_normal = (tmp < 0);
- code = 6;
- }
-
-
- FCL_REAL fudge2(1.0e-6);
- Q.array() += fudge2;
-
- Vec3f n;
- FCL_REAL eps = std::numeric_limits<FCL_REAL>::epsilon();
-
- // separating axis = u1 x (v1,v2,v3)
- tmp = pp[2] * R(1, 0) - pp[1] * R(2, 0);
- s2 = std::abs(tmp) - (A[1] * Q(2, 0) + A[2] * Q(1, 0) + B[1] * Q(0, 2) + B[2] * Q(0, 1));
- if(s2 > 0) { *return_code = 0; return 0; }
- n = Vec3f(0, -R(2, 0), R(1, 0));
- l = n.norm();
- if(l > eps)
- {
- s2 /= l;
- if(s2 * fudge_factor > s)
- {
- s = s2;
- best_col_id = -1;
- normalC = n / l;
- invert_normal = (tmp < 0);
- code = 7;
- }
- }
-
- tmp = pp[2] * R(1, 1) - pp[1] * R(2, 1);
- s2 = std::abs(tmp) - (A[1] * Q(2, 1) + A[2] * Q(1, 1) + B[0] * Q(0, 2) + B[2] * Q(0, 0));
- if(s2 > 0) { *return_code = 0; return 0; }
- n = Vec3f(0, -R(2, 1), R(1, 1));
- l = n.norm();
- if(l > eps)
- {
- s2 /= l;
- if(s2 * fudge_factor > s)
- {
- s = s2;
- best_col_id = -1;
- normalC = n / l;
- invert_normal = (tmp < 0);
- code = 8;
- }
- }
-
- tmp = pp[2] * R(1, 2) - pp[1] * R(2, 2);
- s2 = std::abs(tmp) - (A[1] * Q(2, 2) + A[2] * Q(1, 2) + B[0] * Q(0, 1) + B[1] * Q(0, 0));
- if(s2 > 0) { *return_code = 0; return 0; }
- n = Vec3f(0, -R(2, 2), R(1, 2));
- l = n.norm();
- if(l > eps)
- {
- s2 /= l;
- if(s2 * fudge_factor > s)
- {
- s = s2;
- best_col_id = -1;
- normalC = n / l;
- invert_normal = (tmp < 0);
- code = 9;
- }
- }
-
- // separating axis = u2 x (v1,v2,v3)
- tmp = pp[0] * R(2, 0) - pp[2] * R(0, 0);
- s2 = std::abs(tmp) - (A[0] * Q(2, 0) + A[2] * Q(0, 0) + B[1] * Q(1, 2) + B[2] * Q(1, 1));
- if(s2 > 0) { *return_code = 0; return 0; }
- n = Vec3f(R(2, 0), 0, -R(0, 0));
- l = n.norm();
- if(l > eps)
- {
- s2 /= l;
- if(s2 * fudge_factor > s)
- {
- s = s2;
- best_col_id = -1;
- normalC = n / l;
- invert_normal = (tmp < 0);
- code = 10;
- }
- }
-
- tmp = pp[0] * R(2, 1) - pp[2] * R(0, 1);
- s2 = std::abs(tmp) - (A[0] * Q(2, 1) + A[2] * Q(0, 1) + B[0] * Q(1, 2) + B[2] * Q(1, 0));
- if(s2 > 0) { *return_code = 0; return 0; }
- n = Vec3f(R(2, 1), 0, -R(0, 1));
- l = n.norm();
- if(l > eps)
- {
- s2 /= l;
- if(s2 * fudge_factor > s)
- {
- s = s2;
- best_col_id = -1;
- normalC = n / l;
- invert_normal = (tmp < 0);
- code = 11;
- }
- }
-
- tmp = pp[0] * R(2, 2) - pp[2] * R(0, 2);
- s2 = std::abs(tmp) - (A[0] * Q(2, 2) + A[2] * Q(0, 2) + B[0] * Q(1, 1) + B[1] * Q(1, 0));
- if(s2 > 0) { *return_code = 0; return 0; }
- n = Vec3f(R(2, 2), 0, -R(0, 2));
- l = n.norm();
- if(l > eps)
- {
- s2 /= l;
- if(s2 * fudge_factor > s)
- {
- s = s2;
- best_col_id = -1;
- normalC = n / l;
- invert_normal = (tmp < 0);
- code = 12;
- }
- }
-
- // separating axis = u3 x (v1,v2,v3)
- tmp = pp[1] * R(0, 0) - pp[0] * R(1, 0);
- s2 = std::abs(tmp) - (A[0] * Q(1, 0) + A[1] * Q(0, 0) + B[1] * Q(2, 2) + B[2] * Q(2, 1));
- if(s2 > 0) { *return_code = 0; return 0; }
- n = Vec3f(-R(1, 0), R(0, 0), 0);
- l = n.norm();
- if(l > eps)
- {
- s2 /= l;
- if(s2 * fudge_factor > s)
- {
- s = s2;
- best_col_id = -1;
- normalC = n / l;
- invert_normal = (tmp < 0);
- code = 13;
- }
- }
-
- tmp = pp[1] * R(0, 1) - pp[0] * R(1, 1);
- s2 = std::abs(tmp) - (A[0] * Q(1, 1) + A[1] * Q(0, 1) + B[0] * Q(2, 2) + B[2] * Q(2, 0));
- if(s2 > 0) { *return_code = 0; return 0; }
- n = Vec3f(-R(1, 1), R(0, 1), 0);
- l = n.norm();
- if(l > eps)
- {
- s2 /= l;
- if(s2 * fudge_factor > s)
- {
- s = s2;
- best_col_id = -1;
- normalC = n / l;
- invert_normal = (tmp < 0);
- code = 14;
- }
- }
-
- tmp = pp[1] * R(0, 2) - pp[0] * R(1, 2);
- s2 = std::abs(tmp) - (A[0] * Q(1, 2) + A[1] * Q(0, 2) + B[0] * Q(2, 1) + B[1] * Q(2, 0));
- if(s2 > 0) { *return_code = 0; return 0; }
- n = Vec3f(-R(1, 2), R(0, 2), 0);
- l = n.norm();
- if(l > eps)
- {
- s2 /= l;
- if(s2 * fudge_factor > s)
- {
- s = s2;
- best_col_id = -1;
- normalC = n / l;
- invert_normal = (tmp < 0);
- code = 15;
- }
- }
-
-
-
- if (!code) { *return_code = code; return 0; }
-
- // if we get to this point, the boxes interpenetrate. compute the normal
- // in global coordinates.
- if(best_col_id != -1)
- normal = normalR->col(best_col_id);
- else
- normal = R1 * normalC;
-
- if(invert_normal)
- normal = -normal;
-
- *depth = -s; // s is negative when the boxes are in collision
-
- // compute contact point(s)
-
- if(code > 6)
- {
- // an edge from box 1 touches an edge from box 2.
- // find a point pa on the intersecting edge of box 1
- Vec3f pa(T1);
- FCL_REAL sign;
-
- for(int j = 0; j < 3; ++j)
- {
- sign = (R1.col(j).dot(normal) > 0) ? 1 : -1;
- pa += R1.col(j) * (A[j] * sign);
- }
-
- // find a point pb on the intersecting edge of box 2
- Vec3f pb(T2);
-
- for(int j = 0; j < 3; ++j)
- {
- sign = (R2.col(j).dot(normal) > 0) ? -1 : 1;
- pb += R2.col(j) * (B[j] * sign);
- }
-
- FCL_REAL alpha, beta;
- Vec3f ua(R1.col((code-7)/3));
- Vec3f ub(R2.col((code-7)%3));
-
- lineClosestApproach(pa, ua, pb, ub, &alpha, &beta);
- pa += ua * alpha;
- pb += ub * beta;
-
-
- // Vec3f pointInWorld((pa + pb) * 0.5);
- // contacts.push_back(ContactPoint(-normal, pointInWorld, -*depth));
- contacts.push_back(ContactPoint(-normal,pb,-*depth));
- *return_code = code;
-
- return 1;
- }
-
- // okay, we have a face-something intersection (because the separating
- // axis is perpendicular to a face). define face 'a' to be the reference
- // face (i.e. the normal vector is perpendicular to this) and face 'b' to be
- // the incident face (the closest face of the other box).
-
- const Matrix3f *Ra, *Rb;
- const Vec3f *pa, *pb, *Sa, *Sb;
-
- if(code <= 3)
- {
- Ra = &R1;
- Rb = &R2;
- pa = &T1;
- pb = &T2;
- Sa = &A;
- Sb = &B;
- }
- else
- {
- Ra = &R2;
- Rb = &R1;
- pa = &T2;
- pb = &T1;
- Sa = &B;
- Sb = &A;
- }
-
- // nr = normal vector of reference face dotted with axes of incident box.
- // anr = absolute values of nr.
- Vec3f normal2, nr, anr;
- if(code <= 3)
- normal2 = normal;
- else
- normal2 = -normal;
-
- nr = Rb->transpose() * normal2;
- anr = nr.cwiseAbs();
-
- // find the largest compontent of anr: this corresponds to the normal
- // for the indident face. the other axis numbers of the indicent face
- // are stored in a1,a2.
- int lanr, a1, a2;
- if(anr[1] > anr[0])
- {
- if(anr[1] > anr[2])
- {
- a1 = 0;
- lanr = 1;
- a2 = 2;
- }
- else
- {
- a1 = 0;
- a2 = 1;
- lanr = 2;
- }
- }
- else
- {
- if(anr[0] > anr[2])
- {
- lanr = 0;
- a1 = 1;
- a2 = 2;
- }
- else
- {
- a1 = 0;
- a2 = 1;
- lanr = 2;
- }
- }
-
- // compute center point of incident face, in reference-face coordinates
- Vec3f center;
- if(nr[lanr] < 0)
- center = (*pb) - (*pa) + Rb->col(lanr) * ((*Sb)[lanr]);
- else
- center = (*pb) - (*pa) - Rb->col(lanr) * ((*Sb)[lanr]);
-
- // find the normal and non-normal axis numbers of the reference box
- int codeN, code1, code2;
- if(code <= 3)
- codeN = code-1;
- else codeN = code-4;
-
- if(codeN == 0)
- {
- code1 = 1;
- code2 = 2;
- }
- else if(codeN == 1)
- {
- code1 = 0;
- code2 = 2;
- }
- else
- {
- code1 = 0;
- code2 = 1;
- }
-
- // find the four corners of the incident face, in reference-face coordinates
- FCL_REAL quad[8]; // 2D coordinate of incident face (x,y pairs)
- FCL_REAL c1, c2, m11, m12, m21, m22;
- c1 = Ra->col(code1).dot(center);
- c2 = Ra->col(code2).dot(center);
- // optimize this? - we have already computed this data above, but it is not
- // stored in an easy-to-index format. for now it's quicker just to recompute
- // the four dot products.
- Vec3f tempRac = Ra->col(code1);
- m11 = Rb->col(a1).dot(tempRac);
- m12 = Rb->col(a2).dot(tempRac);
- tempRac = Ra->col(code2);
- m21 = Rb->col(a1).dot(tempRac);
- m22 = Rb->col(a2).dot(tempRac);
-
- FCL_REAL k1 = m11 * (*Sb)[a1];
- FCL_REAL k2 = m21 * (*Sb)[a1];
- FCL_REAL k3 = m12 * (*Sb)[a2];
- FCL_REAL k4 = m22 * (*Sb)[a2];
- quad[0] = c1 - k1 - k3;
- quad[1] = c2 - k2 - k4;
- quad[2] = c1 - k1 + k3;
- quad[3] = c2 - k2 + k4;
- quad[4] = c1 + k1 + k3;
- quad[5] = c2 + k2 + k4;
- quad[6] = c1 + k1 - k3;
- quad[7] = c2 + k2 - k4;
-
- // find the size of the reference face
- FCL_REAL rect[2];
- rect[0] = (*Sa)[code1];
- rect[1] = (*Sa)[code2];
-
- // intersect the incident and reference faces
- FCL_REAL ret[16];
- int n_intersect = intersectRectQuad2(rect, quad, ret);
- if(n_intersect < 1) { *return_code = code; return 0; } // this should never happen
-
- // convert the intersection points into reference-face coordinates,
- // and compute the contact position and depth for each point. only keep
- // those points that have a positive (penetrating) depth. delete points in
- // the 'ret' array as necessary so that 'point' and 'ret' correspond.
- Vec3f points[8]; // penetrating contact points
- FCL_REAL dep[8]; // depths for those points
- FCL_REAL det1 = 1.f/(m11*m22 - m12*m21);
- m11 *= det1;
- m12 *= det1;
- m21 *= det1;
- m22 *= det1;
- int cnum = 0; // number of penetrating contact points found
- for(int j = 0; j < n_intersect; ++j)
- {
- FCL_REAL k1 = m22*(ret[j*2]-c1) - m12*(ret[j*2+1]-c2);
- FCL_REAL k2 = -m21*(ret[j*2]-c1) + m11*(ret[j*2+1]-c2);
- points[cnum] = center + Rb->col(a1) * k1 + Rb->col(a2) * k2;
- dep[cnum] = (*Sa)[codeN] - normal2.dot(points[cnum]);
- if(dep[cnum] >= 0)
- {
- ret[cnum*2] = ret[j*2];
- ret[cnum*2+1] = ret[j*2+1];
- cnum++;
- }
- }
- if(cnum < 1) { *return_code = code; return 0; } // this should never happen
-
- // we can't generate more contacts than we actually have
- if(maxc > cnum) maxc = cnum;
- if(maxc < 1) maxc = 1;
-
- if(cnum <= maxc)
- {
- if(code<4)
- {
- // we have less contacts than we need, so we use them all
- for(int j = 0; j < cnum; ++j)
- {
- Vec3f pointInWorld = points[j] + (*pa);
- contacts.push_back(ContactPoint(-normal, pointInWorld, -dep[j]));
- }
- }
- else
- {
- // we have less contacts than we need, so we use them all
- for(int j = 0; j < cnum; ++j)
- {
- Vec3f pointInWorld = points[j] + (*pa) - normal * dep[j];
- contacts.push_back(ContactPoint(-normal, pointInWorld, -dep[j]));
- }
- }
- }
- else
- {
- // we have more contacts than are wanted, some of them must be culled.
- // find the deepest point, it is always the first contact.
- int i1 = 0;
- FCL_REAL maxdepth = dep[0];
- for(int i = 1; i < cnum; ++i)
- {
- if(dep[i] > maxdepth)
- {
- maxdepth = dep[i];
- i1 = i;
- }
- }
-
- int iret[8];
- cullPoints2(cnum, ret, maxc, i1, iret);
-
- for(int j = 0; j < maxc; ++j)
- {
- Vec3f posInWorld = points[iret[j]] + (*pa);
- if(code < 4)
- contacts.push_back(ContactPoint(-normal, posInWorld, -dep[iret[j]]));
- else
- contacts.push_back(ContactPoint(-normal, posInWorld - normal * dep[iret[j]], -dep[iret[j]]));
- }
- cnum = maxc;
- }
-
- *return_code = code;
- return cnum;
-}
-
-bool compareContactPoints(const ContactPoint& c1,const ContactPoint& c2) { return c1.depth < c2.depth; } // Accending order, assuming penetration depth is a negative number!
-
-bool boxBoxIntersect(const Box& s1, const Transform3f& tf1,
- const Box& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth_, Vec3f* normal_)
-{
- std::vector<ContactPoint> contacts;
- int return_code;
- Vec3f normal;
- FCL_REAL depth;
- /* int cnum = */ boxBox2(s1.side, tf1.getRotation(), tf1.getTranslation(),
- s2.side, tf2.getRotation(), tf2.getTranslation(),
- normal, &depth, &return_code,
- 4, contacts);
-
- if(normal_) *normal_ = normal;
- if(penetration_depth_) *penetration_depth_ = depth;
-
- if(contact_points)
- {
- if(contacts.size() > 0)
- {
- std::sort(contacts.begin(), contacts.end(), compareContactPoints);
- *contact_points = contacts[0].point;
- if(penetration_depth_) *penetration_depth_ = -contacts[0].depth;
- }
- }
-
- return return_code != 0;
-}
-
-template<typename T>
-T halfspaceIntersectTolerance()
-{
- return 0;
-}
-
-template<>
-float halfspaceIntersectTolerance()
-{
- return 0.0001f;
-}
-
-template<>
-double halfspaceIntersectTolerance()
-{
- return 0.0000001;
-}
-
-bool sphereHalfspaceIntersect(const Sphere& s1, const Transform3f& tf1,
- const Halfspace& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
-{
- Halfspace new_s2 = transform(s2, tf2);
- const Vec3f& center = tf1.getTranslation();
- FCL_REAL depth = s1.radius - new_s2.signedDistance(center);
- if(depth >= 0)
- {
- if(normal) *normal = -new_s2.n; // pointing from s1 to s2
- if(penetration_depth) *penetration_depth = depth;
- if(contact_points) *contact_points = center - new_s2.n * s1.radius + new_s2.n * (depth * 0.5);
- return true;
- }
- else
- return false;
-}
-
-/// @brief box half space, a, b, c = +/- edge size
-/// n^T * (R(o + a v1 + b v2 + c v3) + T) <= d
-/// so (R^T n) (a v1 + b v2 + c v3) + n * T <= d
-/// check whether d - n * T - (R^T n) (a v1 + b v2 + c v3) >= 0 for some a, b, c
-/// the max value of left side is d - n * T + |(R^T n) (a v1 + b v2 + c v3)|, check that is enough
-bool boxHalfspaceIntersect(const Box& s1, const Transform3f& tf1,
- const Halfspace& s2, const Transform3f& tf2)
-{
- Halfspace new_s2 = transform(s2, tf2);
-
- const Matrix3f& R = tf1.getRotation();
- const Vec3f& T = tf1.getTranslation();
-
- Vec3f Q = R.transpose() * new_s2.n;
- Vec3f A(Q[0] * s1.side[0], Q[1] * s1.side[1], Q[2] * s1.side[2]);
- Vec3f B = A.cwiseAbs();
-
- FCL_REAL depth = 0.5 * (B[0] + B[1] + B[2]) - new_s2.signedDistance(T);
- return (depth >= 0);
-}
-
-
-bool boxHalfspaceIntersect(const Box& s1, const Transform3f& tf1,
- const Halfspace& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
-{
- if(!contact_points && !penetration_depth && !normal)
- return boxHalfspaceIntersect(s1, tf1, s2, tf2);
- else
- {
- Halfspace new_s2 = transform(s2, tf2);
-
- const Matrix3f& R = tf1.getRotation();
- const Vec3f& T = tf1.getTranslation();
-
- Vec3f Q = R.transpose() * new_s2.n;
- Vec3f A(Q[0] * s1.side[0], Q[1] * s1.side[1], Q[2] * s1.side[2]);
- Vec3f B = A.cwiseAbs();
-
- FCL_REAL depth = 0.5 * (B[0] + B[1] + B[2]) - new_s2.signedDistance(T);
- if(depth < 0) return false;
-
- Vec3f axis[3];
- axis[0] = R.col(0);
- axis[1] = R.col(1);
- axis[2] = R.col(2);
-
- /// find deepest point
- Vec3f p(T);
- int sign = 0;
-
- if(std::abs(Q[0] - 1) < halfspaceIntersectTolerance<FCL_REAL>() || std::abs(Q[0] + 1) < halfspaceIntersectTolerance<FCL_REAL>())
- {
- sign = (A[0] > 0) ? -1 : 1;
- p += axis[0] * (0.5 * s1.side[0] * sign);
- }
- else if(std::abs(Q[1] - 1) < halfspaceIntersectTolerance<FCL_REAL>() || std::abs(Q[1] + 1) < halfspaceIntersectTolerance<FCL_REAL>())
- {
- sign = (A[1] > 0) ? -1 : 1;
- p += axis[1] * (0.5 * s1.side[1] * sign);
- }
- else if(std::abs(Q[2] - 1) < halfspaceIntersectTolerance<FCL_REAL>() || std::abs(Q[2] + 1) < halfspaceIntersectTolerance<FCL_REAL>())
- {
- sign = (A[2] > 0) ? -1 : 1;
- p += axis[2] * (0.5 * s1.side[2] * sign);
- }
- else
- {
- for(std::size_t i = 0; i < 3; ++i)
- {
- sign = (A[i] > 0) ? -1 : 1;
- p += axis[i] * (0.5 * s1.side[i] * sign);
- }
- }
-
- /// compute the contact point from the deepest point
- if(penetration_depth) *penetration_depth = depth;
- if(normal) *normal = -new_s2.n;
- if(contact_points) *contact_points = p + new_s2.n * (depth * 0.5);
-
- return true;
- }
-}
-
-bool capsuleHalfspaceIntersect(const Capsule& s1, const Transform3f& tf1,
- const Halfspace& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
-{
- Halfspace 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) < halfspaceIntersectTolerance<FCL_REAL>())
- {
- FCL_REAL signed_dist = new_s2.signedDistance(T);
- FCL_REAL depth = s1.radius - signed_dist;
- if(depth < 0) return false;
-
- if(penetration_depth) *penetration_depth = depth;
- if(normal) *normal = -new_s2.n;
- if(contact_points) *contact_points = T + new_s2.n * (0.5 * depth - s1.radius);
-
- return true;
- }
- else
- {
- int sign = (cosa > 0) ? -1 : 1;
- Vec3f p = T + dir_z * (s1.lz * 0.5 * sign);
-
- FCL_REAL signed_dist = new_s2.signedDistance(p);
- FCL_REAL depth = s1.radius - signed_dist;
- if(depth < 0) return false;
-
- if(penetration_depth) *penetration_depth = depth;
- if(normal) *normal = -new_s2.n;
- if(contact_points)
- {
- // deepest point
- Vec3f c = p - new_s2.n * s1.radius;
- *contact_points = c + new_s2.n * (0.5 * depth);
- }
-
- return true;
- }
-}
-
-
-bool cylinderHalfspaceIntersect(const Cylinder& s1, const Transform3f& tf1,
- const Halfspace& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
-{
- Halfspace 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(cosa < halfspaceIntersectTolerance<FCL_REAL>())
- {
- FCL_REAL signed_dist = new_s2.signedDistance(T);
- FCL_REAL depth = s1.radius - signed_dist;
- if(depth < 0) return false;
-
- if(penetration_depth) *penetration_depth = depth;
- if(normal) *normal = -new_s2.n;
- if(contact_points) *contact_points = T + new_s2.n * (0.5 * depth - 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>())
- C = Vec3f(0, 0, 0);
- else
- {
- FCL_REAL s = C.norm();
- s = s1.radius / s;
- C *= s;
- }
-
- int sign = (cosa > 0) ? -1 : 1;
- // deepest point
- Vec3f p = T + dir_z * (s1.lz * 0.5 * sign) + C;
- FCL_REAL depth = -new_s2.signedDistance(p);
- if(depth < 0) return false;
- else
- {
- if(penetration_depth) *penetration_depth = depth;
- if(normal) *normal = -new_s2.n;
- if(contact_points) *contact_points = p + new_s2.n * (0.5 * depth);
- return true;
- }
- }
-}
-
-
-bool coneHalfspaceIntersect(const Cone& s1, const Transform3f& tf1,
- const Halfspace& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
-{
- Halfspace 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(cosa < halfspaceIntersectTolerance<FCL_REAL>())
- {
- FCL_REAL signed_dist = new_s2.signedDistance(T);
- FCL_REAL depth = s1.radius - signed_dist;
- if(depth < 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);
- 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>())
- C = Vec3f(0, 0, 0);
- else
- {
- 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) + C;
-
- FCL_REAL d1 = new_s2.signedDistance(p1);
- FCL_REAL d2 = new_s2.signedDistance(p2);
-
- 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);
- return true;
- }
- }
-}
-
-bool convexHalfspaceIntersect(const Convex& s1, const Transform3f& tf1,
- const Halfspace& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal)
-{
- Halfspace new_s2 = transform(s2, tf2);
-
- Vec3f v;
- FCL_REAL depth = std::numeric_limits<FCL_REAL>::max();
-
- for(int i = 0; i < s1.num_points; ++i)
- {
- Vec3f p = tf1.transform(s1.points[i]);
-
- FCL_REAL d = new_s2.signedDistance(p);
- if(d < depth)
- {
- depth = d;
- v = p;
- }
- }
-
- if(depth <= 0)
- {
- if(contact_points) *contact_points = v - new_s2.n * (0.5 * depth);
- if(penetration_depth) *penetration_depth = depth;
- if(normal) *normal = -new_s2.n;
- return true;
- }
- else
- return false;
-}
-
-// Intersection between half-space and triangle
-// Half-space is in pose tf1,
-// Triangle is in pose tf2
-// Computes distance and closest points (p1, p2) if no collision,
-// contact point (p1 = p2), normal and penetration depth (-distance)
-// if collision
-bool halfspaceTriangleIntersect
-(const Halfspace& s1, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
- const Vec3f& P3, const Transform3f& tf2, FCL_REAL& distance, Vec3f& p1,
- Vec3f& p2, Vec3f& normal)
-{
- Halfspace new_s1 = transform(s1, tf1);
-
- Vec3f v = tf2.transform(P1);
- FCL_REAL depth = new_s1.signedDistance(v);
-
- Vec3f p = tf2.transform(P2);
- FCL_REAL d = new_s1.signedDistance(p);
- if(d < depth)
- {
- depth = d;
- v = p;
- }
-
- p = tf2.transform(P3);
- d = new_s1.signedDistance(p);
- if(d < depth)
- {
- depth = d;
- v = p;
- }
- // v is the vertex with minimal projection abscissa (depth) on normal to
- //plane,
- distance = depth;
- if(depth <= 0)
- {
- normal = new_s1.n;
- p1 = p2 = v - (0.5 * depth) * new_s1.n;
- return true;
- }
- else
- {
- p1 = v - depth * new_s1.n;
- p2 = v;
- return false;
- }
-}
-
-
-/// @brief return whether plane collides with halfspace
-/// if the separation plane of the halfspace is parallel with the plane
-/// return code 1, if the plane's normal is the same with halfspace's normal and plane is inside halfspace, also return plane in pl
-/// return code 2, if the plane's normal is oppositie to the halfspace's normal and plane is inside halfspace, also return plane in pl
-/// plane is outside halfspace, collision-free
-/// if not parallel
-/// return the intersection ray, return code 3. ray origin is p and direction is d
-bool planeHalfspaceIntersect(const Plane& s1, const Transform3f& tf1,
- const Halfspace& s2, const Transform3f& tf2,
- Plane& pl,
- Vec3f& p, Vec3f& d,
- FCL_REAL& penetration_depth,
- int& ret)
-{
- Plane new_s1 = transform(s1, tf1);
- Halfspace new_s2 = transform(s2, tf2);
-
- ret = 0;
-
- Vec3f dir = (new_s1.n).cross(new_s2.n);
- FCL_REAL dir_norm = dir.squaredNorm();
- if(dir_norm < std::numeric_limits<FCL_REAL>::epsilon()) // parallel
- {
- if((new_s1.n).dot(new_s2.n) > 0)
- {
- if(new_s1.d < new_s2.d)
- {
- penetration_depth = new_s2.d - new_s1.d;
- ret = 1;
- pl = new_s1;
- return true;
- }
- else
- return false;
- }
- else
- {
- if(new_s1.d + new_s2.d > 0)
- return false;
- else
- {
- penetration_depth = -(new_s1.d + new_s2.d);
- ret = 2;
- pl = new_s1;
- return true;
- }
- }
- }
-
- Vec3f n = new_s2.n * new_s1.d - new_s1.n * new_s2.d;
- Vec3f origin = n.cross(dir);
- origin *= (1.0 / dir_norm);
-
- p = origin;
- d = dir;
- ret = 3;
- penetration_depth = std::numeric_limits<FCL_REAL>::max();
-
- return true;
-}
-
-///@ brief return whether two halfspace intersect
-/// if the separation planes of the two halfspaces are parallel
-/// return code 1, if two halfspaces' normal are same and s1 is in s2, also return s1 in s;
-/// return code 2, if two halfspaces' normal are same and s2 is in s1, also return s2 in s;
-/// return code 3, if two halfspaces' normal are opposite and s1 and s2 are into each other;
-/// collision free, if two halfspaces' are separate;
-/// if the separation planes of the two halfspaces are not parallel, return intersection ray, return code 4. ray origin is p and direction is d
-/// collision free return code 0
-bool halfspaceIntersect(const Halfspace& s1, const Transform3f& tf1,
- const Halfspace& s2, const Transform3f& tf2,
- Vec3f& p, Vec3f& d,
- Halfspace& s,
- FCL_REAL& penetration_depth,
- int& ret)
-{
- Halfspace new_s1 = transform(s1, tf1);
- Halfspace new_s2 = transform(s2, tf2);
-
- ret = 0;
-
- Vec3f dir = (new_s1.n).cross(new_s2.n);
- FCL_REAL dir_norm = dir.squaredNorm();
- if(dir_norm < std::numeric_limits<FCL_REAL>::epsilon()) // parallel
- {
- if((new_s1.n).dot(new_s2.n) > 0)
- {
- if(new_s1.d < new_s2.d) // s1 is inside s2
- {
- ret = 1;
- penetration_depth = std::numeric_limits<FCL_REAL>::max();
- s = new_s1;
- }
- else // s2 is inside s1
- {
- ret = 2;
- penetration_depth = std::numeric_limits<FCL_REAL>::max();
- s = new_s2;
- }
- return true;
- }
- else
- {
- if(new_s1.d + new_s2.d > 0) // not collision
- return false;
- else // in each other
- {
- ret = 3;
- penetration_depth = -(new_s1.d + new_s2.d);
- return true;
- }
- }
- }
-
- Vec3f n = new_s2.n * new_s1.d - new_s1.n * new_s2.d;
- Vec3f origin = n.cross(dir);
- origin *= (1.0 / dir_norm);
-
- p = origin;
- d = dir;
- ret = 4;
- penetration_depth = std::numeric_limits<FCL_REAL>::max();
-
- return true;
-}
-
-template<typename T>
-T planeIntersectTolerance()
-{
- return 0;
-}
-
-template<>
-double planeIntersectTolerance<double>()
-{
- return 0.0000001;
-}
-
-template<>
-float planeIntersectTolerance<float>()
-{
- return 0.0001f;
-}
-
-bool spherePlaneIntersect(const Sphere& 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);
-
- const Vec3f& center = tf1.getTranslation();
- FCL_REAL signed_dist = new_s2.signedDistance(center);
- FCL_REAL depth = - std::abs(signed_dist) + s1.radius;
- if(depth >= 0)
- {
- if(normal) { if (signed_dist > 0) *normal = -new_s2.n; else *normal = new_s2.n; }
- if(penetration_depth) *penetration_depth = depth;
- if(contact_points) *contact_points = center - new_s2.n * signed_dist;
-
- return true;
- }
- else
- return false;
-}
-
-/// @brief box half space, a, b, c = +/- edge size
-/// n^T * (R(o + a v1 + b v2 + c v3) + T) ~ d
-/// so (R^T n) (a v1 + b v2 + c v3) + n * T ~ d
-/// check whether d - n * T - (R^T n) (a v1 + b v2 + c v3) >= 0 for some a, b, c and <=0 for some a, b, c
-/// so need to check whether |d - n * T| <= |(R^T n)(a v1 + b v2 + c v3)|, the reason is as follows:
-/// (R^T n) (a v1 + b v2 + c v3) can get |(R^T n) (a v1 + b v2 + c v3)| for one a, b, c.
-/// if |d - n * T| <= |(R^T n)(a v1 + b v2 + c v3)| then can get both positive and negative value on the right side.
-bool boxPlaneIntersect(const Box& 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);
-
- const Matrix3f& R = tf1.getRotation();
- const Vec3f& T = tf1.getTranslation();
-
- Vec3f Q = R.transpose() * new_s2.n;
- Vec3f A(Q[0] * s1.side[0], Q[1] * s1.side[1], Q[2] * s1.side[2]);
- Vec3f B = A.cwiseAbs();
-
- FCL_REAL signed_dist = new_s2.signedDistance(T);
- FCL_REAL depth = 0.5 * (B[0] + B[1] + B[2]) - std::abs(signed_dist);
- if(depth < 0) return false;
-
- Vec3f axis[3];
- axis[0] = R.col(0);
- axis[1] = R.col(1);
- axis[2] = R.col(2);
-
- // find the deepest point
- Vec3f p = T;
-
- // when center is on the positive side of the plane, use a, b, c make (R^T n) (a v1 + b v2 + c v3) the minimum
- // otherwise, use a, b, c make (R^T n) (a v1 + b v2 + c v3) the maximum
- int sign = (signed_dist > 0) ? 1 : -1;
-
- if(std::abs(Q[0] - 1) < planeIntersectTolerance<FCL_REAL>() || std::abs(Q[0] + 1) < planeIntersectTolerance<FCL_REAL>())
- {
- int sign2 = (A[0] > 0) ? -1 : 1;
- sign2 *= sign;
- p += axis[0] * (0.5 * s1.side[0] * sign2);
- }
- else if(std::abs(Q[1] - 1) < planeIntersectTolerance<FCL_REAL>() || std::abs(Q[1] + 1) < planeIntersectTolerance<FCL_REAL>())
- {
- int sign2 = (A[1] > 0) ? -1 : 1;
- sign2 *= sign;
- p += axis[1] * (0.5 * s1.side[1] * sign2);
- }
- else if(std::abs(Q[2] - 1) < planeIntersectTolerance<FCL_REAL>() || std::abs(Q[2] + 1) < planeIntersectTolerance<FCL_REAL>())
- {
- int sign2 = (A[2] > 0) ? -1 : 1;
- sign2 *= sign;
- p += axis[2] * (0.5 * s1.side[2] * sign2);
- }
- else
- {
- for(std::size_t i = 0; i < 3; ++i)
- {
- int sign2 = (A[i] > 0) ? -1 : 1;
- sign2 *= sign;
- p += axis[i] * (0.5 * s1.side[i] * sign2);
- }
- }
-
- // compute the contact point by project the deepest point onto the plane
- if(penetration_depth) *penetration_depth = depth;
- if(normal) { if (signed_dist > 0) *normal = -new_s2.n; else *normal = new_s2.n; }
- if(contact_points) *contact_points = p - new_s2.n * new_s2.signedDistance(p);
-
- return true;
-}
-
-
-bool capsulePlaneIntersect(const Capsule& s1, const Transform3f& tf1,
- const Plane& s2, const Transform3f& tf2)
-{
- 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);
-
- // two end points on different side of the plane
- if(d1 * d2 <= 0)
- return true;
-
- // 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);
-}
-
-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);
-
- if(!contact_points && !penetration_depth && !normal)
- return capsulePlaneIntersect(s1, tf1, s2, tf2);
- else
- {
- 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);
-
- 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(normal) { if (d1 < 0) *normal = new_s2.n; else *normal = -new_s2.n; }
- return true;
- }
- }
-}
-
-/// @brief cylinder-plane intersect
-/// n^T (R (r * cosa * v1 + r * sina * v2 + h * v3) + T) ~ d
-/// need one point to be positive and one to be negative
-/// (n^T * v3) * h + n * T -d + r * (cosa * (n^T * R * v1) + sina * (n^T * R * v2)) ~ 0
-/// (n^T * v3) * h + r * (cosa * (n^T * R * v1) + sina * (n^T * R * v2)) + n * T - d ~ 0
-bool cylinderPlaneIntersect(const Cylinder& s1, const Transform3f& tf1,
- const Plane& s2, const Transform3f& tf2)
-{
- 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;
-}
-
-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);
- 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;
- }
-
- Vec3f p1 = T + dir_z * (0.5 * s1.lz);
- Vec3f p2 = 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;
- }
-
- 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(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;
- }
- }
-}
-
-bool conePlaneIntersect(const Cone& 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);
-
- 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 - dir_z * (0.5 * s1.lz) + dir_z * (0.5 * depth / 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>())
- C = Vec3f(0, 0, 0);
- else
- {
- FCL_REAL s = C.norm();
- s = s1.radius / s;
- C *= s;
- }
-
- Vec3f c[3];
- c[0] = T + dir_z * (0.5 * s1.lz);
- c[1] = T - dir_z * (0.5 * s1.lz) + C;
- c[2] = T - dir_z * (0.5 * s1.lz) - C;
-
- FCL_REAL d[3];
- d[0] = new_s2.signedDistance(c[0]);
- 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))
- return false;
- else
- {
- bool positive[3];
- for(std::size_t i = 0; i < 3; ++i)
- positive[i] = (d[i] >= 0);
-
- int n_positive = 0;
- FCL_REAL d_positive = 0, d_negative = 0;
- for(std::size_t i = 0; i < 3; ++i)
- {
- if(positive[i])
- {
- n_positive++;
- if(d_positive <= d[i]) d_positive = d[i];
- }
- else
- {
- if(d_negative <= -d[i]) d_negative = -d[i];
- }
- }
-
- 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);
-
- 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;
- }
- 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]; }
- }
-
- 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;
- }
- }
- return true;
- }
- }
-}
-
-bool convexPlaneIntersect(const Convex& 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);
-
- Vec3f v_min, v_max;
- FCL_REAL d_min = std::numeric_limits<FCL_REAL>::max(), d_max = -std::numeric_limits<FCL_REAL>::max();
-
- for(int i = 0; i < s1.num_points; ++i)
- {
- Vec3f p = tf1.transform(s1.points[i]);
-
- FCL_REAL d = new_s2.signedDistance(p);
-
- if(d < d_min) { d_min = d; v_min = p; }
- if(d > d_max) { d_max = d; v_max = p; }
- }
-
- if(d_min * d_max > 0) return false;
- else
- {
- if(d_min + d_max > 0)
- {
- if(penetration_depth) *penetration_depth = -d_min;
- if(normal) *normal = -new_s2.n;
- if(contact_points) *contact_points = v_min - new_s2.n * d_min;
- }
- else
- {
- if(penetration_depth) *penetration_depth = d_max;
- if(normal) *normal = new_s2.n;
- if(contact_points) *contact_points = v_max - new_s2.n * d_max;
- }
- return true;
- }
-}
-
-
-
-bool planeTriangleIntersect
-(const Plane& s1, const Transform3f& tf1, const Vec3f& P1, const Vec3f& P2,
- const Vec3f& P3, const Transform3f& tf2, FCL_REAL distance,
- Vec3f& p1, Vec3f& p2, Vec3f& normal)
-{
- Plane new_s1 = transform(s1, tf1);
-
- Vec3f c[3];
- c[0] = tf2.transform(P1);
- c[1] = tf2.transform(P2);
- c[2] = tf2.transform(P3);
-
- FCL_REAL d[3];
- d[0] = new_s1.signedDistance(c[0]);
- d[1] = new_s1.signedDistance(c[1]);
- d[2] = new_s1.signedDistance(c[2]);
- int imin;
- if (d[0] >= 0 && d[1] >= 0 && d[2] >= 0)
- {
- if (d[0] < d[1])
- if (d[0] < d[2]) {
- imin = 0;
- }
- else { // d [2] <= d[0] < d [1]
- imin = 2;
- }
- else { // d[1] <= d[0]
- if (d[2] < d[1]) {
- imin = 2;
- } else { // d[1] <= d[2]
- imin = 1;
- }
- }
- distance = d[imin];
- p2 = c[imin];
- p1 = c[imin] - d[imin] * new_s1.n;
- return false;
- }
- if (d[0] <= 0 && d[1] <= 0 && d[2] <= 0)
- {
- if (d[0] > d[1])
- if (d[0] > d[2]) {
- imin = 0;
- }
- else { // d [2] >= d[0] > d [1]
- imin = 2;
- }
- else { // d[1] >= d[0]
- if (d[2] > d[1]) {
- imin = 2;
- } else { // d[1] >= d[2]
- imin = 1;
- }
- }
- distance = -d[imin];
- p2 = c[imin];
- p1 = c[imin] - d[imin] * new_s1.n;
- return false;
- }
- bool positive[3];
- for(std::size_t i = 0; i < 3; ++i)
- positive[i] = (d[i] > 0);
-
- int n_positive = 0;
- FCL_REAL d_positive = 0, d_negative = 0;
- for(std::size_t i = 0; i < 3; ++i)
- {
- if(positive[i])
- {
- n_positive++;
- if(d_positive <= d[i]) d_positive = d[i];
- }
- else
- {
- if(d_negative <= -d[i]) d_negative = -d[i];
- }
- }
-
- distance = -std::min(d_positive, d_negative);
- if (d_positive > d_negative)
- {
- normal = new_s1.n;
- } else
- {
- normal = -new_s1.n;
- }
- Vec3f p[2];
- Vec3f q;
-
- FCL_REAL p_d[2];
- FCL_REAL q_d(0);
-
- 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]);
- 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]; }
- }
-
- 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;
-}
-
-bool halfspacePlaneIntersect(const Halfspace& s1, const Transform3f& tf1,
- const Plane& s2, const Transform3f& tf2,
- Plane& pl, Vec3f& p, Vec3f& d,
- FCL_REAL& penetration_depth,
- int& ret)
-{
- return planeHalfspaceIntersect(s2, tf2, s1, tf1, pl, p, d, penetration_depth, ret);
-}
-
-bool planeIntersect(const Plane& s1, const Transform3f& tf1,
- const Plane& s2, const Transform3f& tf2,
- Vec3f* /*contact_points*/, FCL_REAL* /*penetration_depth*/, Vec3f* /*normal*/)
-{
- Plane new_s1 = transform(s1, tf1);
- Plane new_s2 = transform(s2, tf2);
-
- FCL_REAL a = (new_s1.n).dot(new_s2.n);
- if(a == 1 && new_s1.d != new_s2.d)
- return false;
- if(a == -1 && new_s1.d != -new_s2.d)
- return false;
-
- return true;
-}
- // Compute penetration distance and normal of two triangles in collision
- // Normal is normal of triangle 1 (P1, P2, P3), penetration depth is the
- // minimal distance (Q1, Q2, Q3) should be translated along the normal so
- // that the triangles are collision free.
- //
- // Note that we compute here an upper bound of the penetration distance,
- // not the exact value.
- static FCL_REAL computePenetration
- (const Vec3f& P1, const Vec3f& P2, const Vec3f& P3,
- const Vec3f& Q1, const Vec3f& Q2, const Vec3f& Q3, const Vec3f& p1,
- const Transform3f& tf1, const Transform3f& tf2, Vec3f& normal)
- {
- Vec3f globalP1 (tf1.transform (P1));
- Vec3f globalP2 (tf1.transform (P2));
- Vec3f globalP3 (tf1.transform (P3));
- Vec3f globalQ1 (tf2.transform (Q1));
- Vec3f globalQ2 (tf2.transform (Q2));
- Vec3f globalQ3 (tf2.transform (Q3));
- Vec3f u ((globalP2-globalP1).cross (globalP3-globalP1));
- normal = u.normalized ();
- FCL_REAL depth;
- FCL_REAL depth1 ((globalP1-globalQ1).dot (normal));
- FCL_REAL depth2 ((globalP1-globalQ2).dot (normal));
- FCL_REAL depth3 ((globalP1-globalQ3).dot (normal));
- depth = std::max (depth1, std::max (depth2, depth3));
- return depth;
- }
-} // details
-
// Shape intersect algorithms not using libccd
//
// +------------+-----+--------+---------+------+----------+-------+------------+----------+
@@ -2615,11 +145,18 @@ bool GJKSolver_indep::shapeIntersect<Box, Box>(const Box& s1, const Transform3f&
}
template<>
-bool GJKSolver_indep::shapeIntersect<Sphere, Halfspace>(const Sphere& s1, const Transform3f& tf1,
- const Halfspace& s2, const Transform3f& tf2,
- Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
+bool GJKSolver_indep::shapeIntersect<Sphere, Halfspace>
+(const Sphere& s1, const Transform3f& tf1,
+ const Halfspace& s2, const Transform3f& tf2,
+ Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
- return details::sphereHalfspaceIntersect(s1, tf1, s2, tf2, contact_points, penetration_depth, normal);
+ FCL_REAL distance;
+ Vec3f p1, p2;
+ bool res = details::sphereHalfspaceIntersect(s1, tf1, s2, tf2, distance, p1,
+ p2, *normal);
+ *contact_points = p1;
+ *penetration_depth = -distance;
+ return res;
}
template<>
@@ -2627,9 +164,14 @@ bool GJKSolver_indep::shapeIntersect<Halfspace, Sphere>(const Halfspace& s1, con
const Sphere& s2, const Transform3f& tf2,
Vec3f* contact_points, FCL_REAL* penetration_depth, Vec3f* normal) const
{
- const bool res = details::sphereHalfspaceIntersect(s2, tf2, s1, tf1, contact_points, penetration_depth, normal);
- if (normal) (*normal) *= -1.0;
- return res;
+ FCL_REAL distance;
+ Vec3f p1, p2;
+ bool res = details::sphereHalfspaceIntersect(s2, tf2, s1, tf1, distance, p1,
+ p2, *normal);
+ *contact_points = p1;
+ *penetration_depth = -distance;
+ (*normal) *= -1.0;
+ return res;
}
template<>
--
GitLab