Head

GCC Code Coverage Report
Directory:	./		Exec	Total	Coverage
File:	src/narrowphase/details.h	Lines:	607	807	75.2 %
Date:	2024-02-09 12:57:42	Branches:	977	2148	45.5 %

/*
 * 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/internal/traversal_node_setup.h>
#include <hpp/fcl/narrowphase/narrowphase.h>

namespace hpp {
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,
                                   FCL_REAL& distance, Vec3f* contact_points,
                                   Vec3f* normal_) {
  Vec3f pos1(
      tf2.transform(Vec3f(0., 0., s2.halfLength)));  // from distance function
  Vec3f pos2(tf2.transform(Vec3f(0., 0., -s2.halfLength)));
  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();
  distance = diffN - s1.radius - s2.radius;

  if (distance > 0) return false;

  // Vec3f normal (-diff.normalized());

  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) {
  Vec3f pos1(tf2.transform(Vec3f(0., 0., s2.halfLength)));
  Vec3f pos2(tf2.transform(Vec3f(0., 0., -s2.halfLength)));
  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;

  static const 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 sphereCylinderDistance(const Sphere& s1, const Transform3f& tf1,
                                   const Cylinder& s2, const Transform3f& tf2,
                                   FCL_REAL& dist, Vec3f& p1, Vec3f& p2,
                                   Vec3f& normal) {
  static const FCL_REAL eps(sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
  FCL_REAL r1(s1.radius);
  FCL_REAL r2(s2.radius);
  FCL_REAL lz2(s2.halfLength);
  // boundaries of the cylinder axis
  Vec3f A(tf2.transform(Vec3f(0, 0, -lz2)));
  Vec3f B(tf2.transform(Vec3f(0, 0, lz2)));
  // Position of the center of the sphere
  Vec3f S(tf1.getTranslation());
  // axis of the cylinder
  Vec3f u(tf2.getRotation().col(2));
  /// @todo a tiny performance improvement could be achieved using the abscissa
  /// with S as the origin
  assert((B - A - (s2.halfLength * 2) * u).norm() < eps);
  Vec3f AS(S - A);
  // abscissa of S on cylinder axis with A as the origin
  FCL_REAL s(u.dot(AS));
  Vec3f P(A + s * u);
  Vec3f PS(S - P);
  FCL_REAL dPS = PS.norm();
  // Normal to cylinder axis such that plane (A, u, v) contains sphere
  // center
  Vec3f v(0, 0, 0);
  if (dPS > eps) {
    // S is not on cylinder axis
    v = (1 / dPS) * PS;
  }
  if (s <= 0) {
    if (dPS <= r2) {
      // closest point on cylinder is on cylinder disc basis
      dist = -s - r1;
      p1 = S + r1 * u;
      p2 = A + dPS * v;
      normal = u;
    } else {
      // closest point on cylinder is on cylinder circle basis
      p2 = A + r2 * v;
      Vec3f Sp2(p2 - S);
      FCL_REAL dSp2 = Sp2.norm();
      if (dSp2 > eps) {
        normal = (1 / dSp2) * Sp2;
        p1 = S + r1 * normal;
        dist = dSp2 - r1;
        assert(fabs(dist) - (p1 - p2).norm() < eps);
      } else {
        // Center of sphere is on cylinder boundary
        normal = .5 * (A + B) - p2;
        normal.normalize();
        p1 = p2;
        dist = -r1;
      }
    }
  } else if (s <= (s2.halfLength * 2)) {
    // 0 < s <= s2.lz
    normal = -v;
    dist = dPS - r1 - r2;
    if (dPS <= r2) {
      // Sphere center is inside cylinder
      p1 = p2 = S;
    } else {
      p2 = P + r2 * v;
      p1 = S - r1 * v;
    }
  } else {
    // lz < s
    if (dPS <= r2) {
      // closest point on cylinder is on cylinder disc basis
      dist = s - (s2.halfLength * 2) - r1;
      p1 = S - r1 * u;
      p2 = B + dPS * v;
      normal = -u;
    } else {
      // closest point on cylinder is on cylinder circle basis
      p2 = B + r2 * v;
      Vec3f Sp2(p2 - S);
      FCL_REAL dSp2 = Sp2.norm();
      if (dSp2 > eps) {
        normal = (1 / dSp2) * Sp2;
        p1 = S + r1 * normal;
        dist = dSp2 - r1;
        assert(fabs(dist) - (p1 - p2).norm() < eps);
      } else {
        // Center of sphere is on cylinder boundary
        normal = .5 * (A + B) - p2;
        normal.normalize();
        p1 = p2;
        dist = -r1;
      }
    }
  }
  if (dist < 0) {
    p1 = p2 = .5 * (p1 + p2);
  }
  return (dist > 0);
}

inline bool sphereSphereIntersect(const Sphere& s1, const Transform3f& tf1,
                                  const Sphere& s2, const Transform3f& tf2,
                                  FCL_REAL& distance, Vec3f* contact_points,
                                  Vec3f* normal) {
  const Vec3f diff = tf2.getTranslation() - tf1.getTranslation();
  FCL_REAL len = diff.norm();
  distance = len - s1.radius - s2.radius;
  if (distance > 0) return false;

  // 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) {
  const Vec3f& o1 = tf1.getTranslation();
  const Vec3f& o2 = tf2.getTranslation();
  Vec3f diff = o1 - o2;
  FCL_REAL len = diff.norm();
  normal = -diff / len;
  dist = len - s1.radius - s2.radius;

  p1.noalias() = o1 + normal * s1.radius;
  p2.noalias() = o2 - normal * s2.radius;

  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.squaredNorm();
    if (t < dotVV) {
      t /= dotVV;
      diff -= v * t;
    } else {
      t = 1;
      diff -= v;
    }
  } else
    t = 0;

  nearest.noalias() = from + v * t;
  return diff.squaredNorm();
}

/// @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);
  Vec3f p1_to_center = center - P1;
  FCL_REAL distance_from_plane = p1_to_center.dot(normal);
  Vec3f closest_point(
      Vec3f::Constant(std::numeric_limits<FCL_REAL>::quiet_NaN()));
  FCL_REAL min_distance_sqr, distance_sqr;

  if (distance_from_plane < 0) {
    distance_from_plane *= -1;
    normal *= -1;
  }

  if (projectInTriangle(P1, P2, P3, normal, center)) {
    closest_point = center - normal * distance_from_plane;
    min_distance_sqr = distance_from_plane;
  } else {
    // Compute distance to each each and take minimal distance
    Vec3f nearest_on_edge;
    min_distance_sqr = segmentSqrDistance(P1, P2, center, closest_point);

    distance_sqr = segmentSqrDistance(P2, P3, center, nearest_on_edge);
    if (distance_sqr < min_distance_sqr) {
      min_distance_sqr = distance_sqr;
      closest_point = nearest_on_edge;
    }
    distance_sqr = segmentSqrDistance(P3, P1, center, nearest_on_edge);
    if (distance_sqr < min_distance_sqr) {
      min_distance_sqr = distance_sqr;
      closest_point = nearest_on_edge;
    }
  }

  if (min_distance_sqr < radius * radius) {
    // Collision
    normal_ = (closest_point - center).normalized();
    p1 = p2 = closest_point;
    distance = sqrt(min_distance_sqr) - radius;
    assert(distance < 0);
    return true;
  } else {
    normal_ = (closest_point - center).normalized();
    p1 = center + normal_ * radius;
    p2 = closest_point;
    distance = sqrt(min_distance_sqr) - radius;
    assert(distance >= 0);
    return false;
  }
}

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) {
    const 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 HPP_FCL_LOCAL 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 unsigned 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
  unsigned 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 (unsigned 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(unsigned int n, FCL_REAL p[], unsigned int m,
                               unsigned int i0, unsigned 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;
      assert(n > 0 && "n should be positive");
      for (int i = 0; i < (int)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 (unsigned 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[] = {1, 1, 1, 1, 1, 1, 1, 1};
  avail[i0] = 0;
  iret[0] = i0;
  iret++;
  const double pi = boost::math::constants::pi<FCL_REAL>();
  for (unsigned 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 (unsigned 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 unsigned int boxBox2(const Vec3f& halfSide1, const Matrix3f& R1,
                            const Vec3f& T1, const Vec3f& halfSide2,
                            const Matrix3f& R2, const Vec3f& T2, Vec3f& normal,
                            FCL_REAL* depth, int* return_code,
                            unsigned 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
  const Vec3f& A(halfSide1);
  const Vec3f& B(halfSide2);

  // 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;
  static const 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.noalias() = 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.noalias() = 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];
  const unsigned 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;
  unsigned int cnum = 0;  // number of penetrating contact points found
  for (unsigned 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 (unsigned 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 (unsigned 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.
    unsigned int i1 = 0;
    FCL_REAL maxdepth = dep[0];
    for (unsigned int i = 1; i < cnum; ++i) {
      if (dep[i] > maxdepth) {
        maxdepth = dep[i];
        i1 = i;
      }
    }

    unsigned int iret[8];
    cullPoints2(cnum, ret, maxc, i1, iret);

    for (unsigned 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.halfSide, tf1.getRotation(), tf1.getTranslation(),
                           s2.halfSide, 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);

  FCL_REAL depth =
      Q.cwiseProduct(s1.halfSide).lpNorm<1>() - new_s2.signedDistance(T);
  return (depth >= 0);
}

inline bool boxHalfspaceIntersect(const Box& 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 Matrix3f& R = tf1.getRotation();
  const Vec3f& T = tf1.getTranslation();

  // Q: plane normal expressed in box frame
  const Vec3f Q(R.transpose() * new_s2.n);
  // A: scalar products of each side with normal
  const Vec3f A(Q.cwiseProduct(s1.halfSide));

  distance = new_s2.signedDistance(T) - A.lpNorm<1>();
  if (distance > 0) {
    p1.noalias() = T + R * (A.array() > 0).select(s1.halfSide, -s1.halfSide);
    p2.noalias() = p1 - distance * new_s2.n;
    return false;
  }

  /// 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 += R.col(0) * (s1.halfSide[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 += R.col(1) * (s1.halfSide[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 += R.col(2) * (s1.halfSide[2] * sign);
  } else {
    p.noalias() += R * (A.array() > 0).select(-s1.halfSide, s1.halfSide);
  }

  /// compute the contact point from the deepest point
  normal = -new_s2.n;
  p1 = p2 = p - new_s2.n * (distance * 0.5);

  return true;
}

inline bool capsuleHalfspaceIntersect(const Capsule& 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 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>()) {
    // Capsule parallel to plane
    FCL_REAL signed_dist = new_s2.signedDistance(T);
    distance = signed_dist - s1.radius;
    if (distance > 0) {
      p1 = T - s1.radius * new_s2.n;
      p2 = p1 - distance * new_s2.n;
      return false;
    }

    normal = -new_s2.n;
    p1 = p2 = T + new_s2.n * (-0.5 * distance - s1.radius);
    return true;
  } else {
    int sign = (cosa > 0) ? -1 : 1;
    // closest capsule vertex to halfspace if no collision,
    // or deeper inside halspace if collision
    Vec3f p = T + dir_z * (s1.halfLength * sign);

    FCL_REAL signed_dist = new_s2.signedDistance(p);
    distance = signed_dist - s1.radius;
    if (distance > 0) {
      p1 = T - s1.radius * new_s2.n;
      p2 = p1 - distance * new_s2.n;
      return false;
    }
    normal = -new_s2.n;
    // deepest point
    Vec3f c = p - new_s2.n * s1.radius;
    p1 = p2 = c - (0.5 * distance) * new_s2.n;
    return true;
  }
}

inline bool cylinderHalfspaceIntersect(const Cylinder& 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 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);
    distance = signed_dist - s1.radius;
    if (distance > 0) {
      // TODO: compute closest points
      p1 = p2 = Vec3f(0, 0, 0);
      return false;
    }

    normal = -new_s2.n;
    p1 = p2 = T - (0.5 * distance + s1.radius) * new_s2.n;
    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.halfLength * sign) + C;
    distance = new_s2.signedDistance(p);
    if (distance > 0) {
      // TODO: compute closest points
      p1 = p2 = Vec3f(0, 0, 0);
      return false;
    } else {
      normal = -new_s2.n;
      p1 = p2 = p - (0.5 * distance) * new_s2.n;
      return true;
    }
  }
}

inline bool coneHalfspaceIntersect(const Cone& 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 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);
    distance = signed_dist - s1.radius;
    if (distance > 0) {
      p1 = p2 = Vec3f(0, 0, 0);
      return false;
    } else {
      normal = -new_s2.n;
      p1 = p2 =
          T - dir_z * (s1.halfLength) - new_s2.n * (0.5 * distance + s1.radius);
      return true;
    }
  } else {
    Vec3f C = dir_z * cosa - new_s2.n;
    if (std::abs(cosa + 1) < halfspaceIntersectTolerance<FCL_REAL>() ||
        std::abs(cosa - 1) < halfspaceIntersectTolerance<FCL_REAL>())
      C = Vec3f(0, 0, 0);
    else {
      FCL_REAL s = C.norm();
      s = s1.radius / s;
      C *= s;
    }

    Vec3f a1 = T + dir_z * (s1.halfLength);
    Vec3f a2 = T - dir_z * (s1.halfLength) + C;

    FCL_REAL d1 = new_s2.signedDistance(a1);
    FCL_REAL d2 = new_s2.signedDistance(a2);

    if (d1 > 0 && d2 > 0)
      return false;
    else {
      distance = std::min(d1, d2);
      normal = -new_s2.n;
      p1 = p2 = ((d1 < d2) ? a1 : a2) - (0.5 * distance) * new_s2.n;
      return true;
    }
  }
}

inline bool convexHalfspaceIntersect(
    const ConvexBase& 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 (unsigned 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;

  penetration_depth = (std::numeric_limits<FCL_REAL>::max)();
  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) {
      penetration_depth = new_s2.d - new_s1.d;
      if (penetration_depth < 0)
        return false;
      else {
        ret = 1;
        pl = new_s1;
        return true;
      }
    } else {
      penetration_depth = -(new_s1.d + new_s2.d);
      if (penetration_depth < 0)
        return false;
      else {
        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;

  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;

  penetration_depth = (std::numeric_limits<FCL_REAL>::max)();
  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;
        s = new_s1;
      } else  // s2 is inside s1
      {
        ret = 2;
        s = new_s2;
      }
      return true;
    } else {
      penetration_depth = -(new_s1.d + new_s2.d);
      if (penetration_depth < 0)  // not collision
        return false;
      else  // in each other
      {
        ret = 3;
        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;

  return true;
}

/// @param p1 closest (or most penetrating) point on the Halfspace,
/// @param p2 closest (or most penetrating) point on the shape,
/// @param normal the halfspace normal.
/// @return true if the distance is negative (the shape overlaps).
inline bool halfspaceDistance(const Halfspace& h, const Transform3f& tf1,
                              const ShapeBase& s, const Transform3f& tf2,
                              FCL_REAL& dist, Vec3f& p1, Vec3f& p2,
                              Vec3f& normal) {
  Vec3f n_w = tf1.getRotation() * h.n;
  Vec3f n_2(tf2.getRotation().transpose() * n_w);
  int hint = 0;
  p2 = getSupport(&s, -n_2, true, hint);
  p2 = tf2.transform(p2);

  dist = (p2 - tf1.getTranslation()).dot(n_w) - h.d;
  p1 = p2 - dist * n_w;
  normal = n_w;

  return dist <= 0;
}

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,
                                 FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
                                 Vec3f& normal)
//  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);
  distance = std::abs(signed_dist) - s1.radius;
  if (distance <= 0) {
    if (signed_dist > 0)
      normal = -new_s2.n;
    else
      normal = new_s2.n;
    p1 = p2 = center - new_s2.n * signed_dist;
    return true;
  } else {
    if (signed_dist > 0) {
      p1 = center - s1.radius * new_s2.n;
      p2 = center - signed_dist * new_s2.n;
    } else {
      p1 = center + s1.radius * new_s2.n;
      p2 = center + signed_dist * 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
/// 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,
                              FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
                              Vec3f& normal)
//                            Vec3f* contact_points,
//                            FCL_REAL* penetration_depth, Vec3f* normal)
{
  static const FCL_REAL eps(sqrt(std::numeric_limits<FCL_REAL>::epsilon()));
  const Plane new_s2 = transform(s2, tf2);

  const Matrix3f& R = tf1.getRotation();
  const Vec3f& T = tf1.getTranslation();

  const Vec3f Q(R.transpose() * new_s2.n);
  const Vec3f A(Q.cwiseProduct(s1.halfSide));

  const FCL_REAL signed_dist = new_s2.signedDistance(T);
  distance = std::abs(signed_dist) - A.lpNorm<1>();
  if (distance > 0) {
    // Is the box above or below the plane
    const bool positive = signed_dist > 0;
    // Set p1 at the center of the box
    p1 = T;
    for (Vec3f::Index i = 0; i < 3; ++i) {
      // scalar product between box axis and plane normal
      FCL_REAL alpha((positive ? 1 : -1) * R.col(i).dot(new_s2.n));
      if (alpha > eps) {
        p1 -= R.col(i) * s1.halfSide[i];
      } else if (alpha < -eps) {
        p1 += R.col(i) * s1.halfSide[i];
      }
    }
    p2 = p1 - (positive ? distance : -distance) * new_s2.n;
    assert(new_s2.distance(p2) < 3 * eps);
    return false;
  }

  // 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) ? -sign : sign;
    p.noalias() += R.col(0) * (s1.halfSide[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) ? -sign : sign;
    p.noalias() += R.col(1) * (s1.halfSide[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) ? -sign : sign;
    p.noalias() += R.col(2) * (s1.halfSide[2] * sign2);
  } else {
    Vec3f tmp(sign * R * s1.halfSide);
    p.noalias() += (A.array() > 0).select(-tmp, tmp);
  }

  // compute the contact point by project the deepest point onto the plane
  if (signed_dist > 0)
    normal = -new_s2.n;
  else
    normal = new_s2.n;
  p1 = p2.noalias() = p - new_s2.n * new_s2.signedDistance(p);

  return true;
}

/// Taken from book Real Time Collision Detection, from Christer Ericson
/// @param pb the closest point to the sphere center on the box surface
/// @param ps when colliding, matches pb, which is inside the sphere.
///           when not colliding, the closest point on the sphere
/// @param normal direction of motion of the box
/// @return true if the distance is negative (the shape overlaps).
inline bool boxSphereDistance(const Box& b, const Transform3f& tfb,
                              const Sphere& s, const Transform3f& tfs,
                              FCL_REAL& dist, Vec3f& pb, Vec3f& ps,
                              Vec3f& normal) {
  const Vec3f& os = tfs.getTranslation();
  const Vec3f& ob = tfb.getTranslation();
  const Matrix3f& Rb = tfb.getRotation();

  pb = ob;

  bool outside = false;
  const Vec3f os_in_b_frame(Rb.transpose() * (os - ob));
  int axis = -1;
  FCL_REAL min_d = (std::numeric_limits<FCL_REAL>::max)();
  for (int i = 0; i < 3; ++i) {
    FCL_REAL facedist;
    if (os_in_b_frame(i) < -b.halfSide(i)) {  // outside
      pb.noalias() -= b.halfSide(i) * Rb.col(i);
      outside = true;
    } else if (os_in_b_frame(i) > b.halfSide(i)) {  // outside
      pb.noalias() += b.halfSide(i) * Rb.col(i);
      outside = true;
    } else {
      pb.noalias() += os_in_b_frame(i) * Rb.col(i);
      if (!outside &&
          (facedist = b.halfSide(i) - std::fabs(os_in_b_frame(i))) < min_d) {
        axis = i;
        min_d = facedist;
      }
    }
  }
  normal = pb - os;
  FCL_REAL pdist = normal.norm();
  if (outside) {  // pb is on the box
    dist = pdist - s.radius;
    normal /= -pdist;
  } else {  // pb is inside the box
    if (os_in_b_frame(axis) >= 0)
      normal = Rb.col(axis);
    else
      normal = -Rb.col(axis);
    dist = -min_d - s.radius;
  }
  if (!outside || dist <= 0) {
    ps = pb;
    return true;
  } else {
    ps = os - s.radius * normal;
    return false;
  }
}

inline bool capsulePlaneIntersect(const Capsule& s1, const Transform3f& tf1,
                                  const Plane& s2, const Transform3f& tf2,
                                  FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
                                  Vec3f& normal) {
  Plane new_s2 = transform(s2, tf2);

  // position orientation of capsule
  const Matrix3f& R1 = tf1.getRotation();
  const Vec3f& T1 = tf1.getTranslation();

  Vec3f dir_z = R1.col(2);

  // ends of capsule inner segment
  Vec3f a1 = T1 + dir_z * s1.halfLength;
  Vec3f a2 = T1 - dir_z * s1.halfLength;

  FCL_REAL d1 = new_s2.signedDistance(a1);
  FCL_REAL d2 = new_s2.signedDistance(a2);

  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) {
      distance = -abs_d1 - s1.radius;
      p1 = p2 =
          a1 * (abs_d2 / (abs_d1 + abs_d2)) + a2 * (abs_d1 / (abs_d1 + abs_d2));
      if (d1 < 0)
        normal = -new_s2.n;
      else
        normal = new_s2.n;
    } else {
      distance = -abs_d2 - s1.radius;
      p1 = p2 =
          a1 * (abs_d2 / (abs_d1 + abs_d2)) + a2 * (abs_d1 / (abs_d1 + abs_d2));
      if (d2 < 0)
        normal = -new_s2.n;
      else
        normal = new_s2.n;
    }
    assert(!p1.hasNaN() && !p2.hasNaN());
    return true;
  }

  if (abs_d1 > s1.radius && abs_d2 > s1.radius) {
    // Here both capsule ends are on the same side of the plane
    if (d1 > 0)
      normal = new_s2.n;
    else
      normal = -new_s2.n;
    if (abs_d1 < abs_d2) {
      distance = abs_d1 - s1.radius;
      p1 = a1 - s1.radius * normal;
      p2 = p1 - distance * normal;
    } else {
      distance = abs_d2 - s1.radius;
      p1 = a2 - s1.radius * normal;
      p2 = p1 - distance * normal;
    }
    assert(!p1.hasNaN() && !p2.hasNaN());
    return false;
  } else {
    // Both capsule ends are on the same side of the plane, but one
    // is closer than the capsule radius, hence collision
    distance = std::min(abs_d1, abs_d2) - s1.radius;

    if (abs_d1 <= s1.radius && abs_d2 <= s1.radius) {
      Vec3f c1 = a1 - new_s2.n * d1;
      Vec3f c2 = a2 - new_s2.n * d2;
      p1 = p2 = (c1 + c2) * 0.5;
    } else if (abs_d1 <= s1.radius) {
      Vec3f c = a1 - new_s2.n * d1;
      p1 = p2 = c;
    } else if (abs_d2 <= s1.radius) {
      Vec3f c = a2 - new_s2.n * d2;
      p1 = p2 = c;
    } else {
      assert(false);
    }

    if (d1 < 0)
      normal = new_s2.n;
    else
      normal = -new_s2.n;
    assert(!p1.hasNaN() && !p2.hasNaN());
    return true;
  }
  assert(false);
}

/// @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,
                                   FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
                                   Vec3f& normal) {
  Plane new_s2 = transform(s2, tf2);

  const Matrix3f& R = tf1.getRotation();
  const Vec3f& T = tf1.getTranslation();

  Vec3f 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);
    distance = std::abs(d) - s1.radius;
    if (distance > 0)
      return false;
    else {
      if (d < 0)
        normal = new_s2.n;
      else
        normal = -new_s2.n;
      p1 = p2 = T - new_s2.n * d;
      return true;
    }
  } else {
    Vec3f C = dir_z * cosa - new_s2.n;
    if (std::abs(cosa + 1) < planeIntersectTolerance<FCL_REAL>() ||
        std::abs(cosa - 1) < planeIntersectTolerance<FCL_REAL>())
      C = Vec3f(0, 0, 0);
    else {
      FCL_REAL s = C.norm();
      s = s1.radius / s;
      C *= s;
    }

    Vec3f a1 = T + dir_z * (s1.halfLength);
    Vec3f a2 = T - dir_z * (s1.halfLength);

    Vec3f c1, c2;
    if (cosa > 0) {
      c1 = a1 - C;
      c2 = a2 + C;
    } else {
      c1 = a1 + C;
      c2 = a2 - C;
    }

    FCL_REAL d1 = new_s2.signedDistance(c1);
    FCL_REAL d2 = new_s2.signedDistance(c2);

    if (d1 * d2 <= 0) {
      FCL_REAL abs_d1 = std::abs(d1);
      FCL_REAL abs_d2 = std::abs(d2);

      if (abs_d1 > abs_d2) {
        distance = -abs_d2;
        p1 = p2 = c2 - new_s2.n * d2;
        if (d2 < 0)
          normal = -new_s2.n;
        else
          normal = new_s2.n;
      } else {
        distance = -abs_d1;
        p1 = p2 = c1 - new_s2.n * d1;
        if (d1 < 0)
          normal = -new_s2.n;
        else
          normal = new_s2.n;
      }
      return true;
    } else
      return false;
  }
}

inline bool conePlaneIntersect(const Cone& s1, const Transform3f& tf1,
                               const Plane& s2, const Transform3f& tf2,
                               FCL_REAL& distance, Vec3f& p1, Vec3f& p2,
                               Vec3f& normal) {
  Plane new_s2 = transform(s2, tf2);

  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);
    distance = std::abs(d) - s1.radius;
    if (distance > 0) {
      p1 = p2 = Vec3f(0, 0, 0);
      return false;
    } else {
      if (d < 0)
        normal = new_s2.n;
      else
        normal = -new_s2.n;
      p1 = p2 = T - dir_z * (s1.halfLength) +
                dir_z * (-distance / s1.radius * s1.halfLength) - 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 * (s1.halfLength);
    c[1] = T - dir_z * (s1.halfLength) + C;
    c[2] = T - dir_z * (s1.halfLength) - 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];
        }
      }

      distance = -std::min(d_positive, d_negative);
      if (d_positive > d_negative)
        normal = -new_s2.n;
      else
        normal = new_s2.n;
      Vec3f p[2];
      Vec3f q;

      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 convexPlaneIntersect(const ConvexBase& 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 (unsigned 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;
}

/// See the prototype below
inline FCL_REAL computePenetration(const Vec3f& P1, const Vec3f& P2,
                                   const Vec3f& P3, const Vec3f& Q1,
                                   const Vec3f& Q2, const Vec3f& Q3,
                                   Vec3f& normal) {
  Vec3f u((P2 - P1).cross(P3 - P1));
  normal = u.normalized();
  FCL_REAL depth1((P1 - Q1).dot(normal));
  FCL_REAL depth2((P1 - Q2).dot(normal));
  FCL_REAL depth3((P1 - Q3).dot(normal));
  return std::max(depth1, std::max(depth2, depth3));
}

// 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.
inline FCL_REAL computePenetration(const Vec3f& P1, const Vec3f& P2,
                                   const Vec3f& P3, const Vec3f& Q1,
                                   const Vec3f& Q2, const Vec3f& Q3,
                                   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));
  return computePenetration(globalP1, globalP2, globalP3, globalQ1, globalQ2,
                            globalQ3, normal);
}
}  // namespace details
}  // namespace fcl
}  // namespace hpp

#endif  // HPP_FCL_SRC_NARROWPHASE_DETAILS_H

Generated by: GCOVR (Version 4.2)