details.h

Go to the documentation of this file.

/*
 * 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
 * Copyright (c) 2021-2024, INRIA
 * 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.
 */

#ifndef COAL_SRC_NARROWPHASE_DETAILS_H
#define COAL_SRC_NARROWPHASE_DETAILS_H

#include "coal/internal/traversal_node_setup.h"
#include "coal/narrowphase/narrowphase.h"

namespace coal {
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 Vec3s& p,
 const Vec3s& s1,
 const Vec3s& s2, Vec3s& sp) {
 Vec3s v = s2 - s1;
 Vec3s w = p - s1;

 CoalScalar c1 = w.dot(v);
 CoalScalar c2 = v.dot(v);

 if (c1 <= 0) {
 sp = s1;
 } else if (c2 <= c1) {
 sp = s2;
 } else {
 CoalScalar b = c1 / c2;
 Vec3s Pb = s1 + v * b;
 sp = Pb;
 }
}

inline CoalScalar sphereCapsuleDistance(const Sphere& s1,
 const Transform3s& tf1,
 const Capsule& s2,
 const Transform3s& tf2, Vec3s& p1,
 Vec3s& p2, Vec3s& normal) {
 Vec3s pos1(tf2.transform(Vec3s(0., 0., s2.halfLength)));
 Vec3s pos2(tf2.transform(Vec3s(0., 0., -s2.halfLength)));
 Vec3s s_c = tf1.getTranslation();

 Vec3s segment_point;

 lineSegmentPointClosestToPoint(s_c, pos1, pos2, segment_point);
 normal = segment_point - s_c;
 CoalScalar norm(normal.norm());
 CoalScalar r1 = s1.radius + s1.getSweptSphereRadius();
 CoalScalar r2 = s2.radius + s2.getSweptSphereRadius();
 CoalScalar dist = norm - r1 - r2;

 static const CoalScalar eps(std::numeric_limits<CoalScalar>::epsilon());
 if (norm > eps) {
 normal.normalize();
 } else {
 normal << 1, 0, 0;
 }
 p1 = s_c + normal * r1;
 p2 = segment_point - normal * r2;
 return dist;
}

inline CoalScalar sphereCylinderDistance(const Sphere& s1,
 const Transform3s& tf1,
 const Cylinder& s2,
 const Transform3s& tf2, Vec3s& p1,
 Vec3s& p2, Vec3s& normal) {
 static const CoalScalar eps(sqrt(std::numeric_limits<CoalScalar>::epsilon()));
 CoalScalar r1(s1.radius);
 CoalScalar r2(s2.radius);
 CoalScalar lz2(s2.halfLength);
 // boundaries of the cylinder axis
 Vec3s A(tf2.transform(Vec3s(0, 0, -lz2)));
 Vec3s B(tf2.transform(Vec3s(0, 0, lz2)));
 // Position of the center of the sphere
 Vec3s S(tf1.getTranslation());
 // axis of the cylinder
 Vec3s u(tf2.getRotation().col(2));
 assert((B - A - (s2.halfLength * 2) * u).norm() < eps);
 Vec3s AS(S - A);
 // abscissa of S on cylinder axis with A as the origin
 CoalScalar s(u.dot(AS));
 Vec3s P(A + s * u);
 Vec3s PS(S - P);
 CoalScalar dPS = PS.norm();
 // Normal to cylinder axis such that plane (A, u, v) contains sphere
 // center
 Vec3s v(0, 0, 0);
 CoalScalar dist;
 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;
 Vec3s Sp2(p2 - S);
 CoalScalar 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 = p2 - .5 * (A + B);
 assert(u.dot(normal) >= 0);
 normal.normalize();
 dist = -r1;
 p1 = S + r1 * normal;
 }
 }
 } else if (s <= (s2.halfLength * 2)) {
 // 0 < s <= s2.lz
 normal = -v;
 dist = dPS - r1 - r2;
 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;
 Vec3s Sp2(p2 - S);
 CoalScalar 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 = p2 - .5 * (A + B);
 normal.normalize();
 p1 = S + r1 * normal;
 dist = -r1;
 }
 }
 }

 // Take swept-sphere radius into account
 const CoalScalar ssr1 = s1.getSweptSphereRadius();
 const CoalScalar ssr2 = s2.getSweptSphereRadius();
 if (ssr1 > 0 || ssr2 > 0) {
 p1 += ssr1 * normal;
 p2 -= ssr2 * normal;
 dist -= (ssr1 + ssr2);
 }

 return dist;
}

inline CoalScalar sphereSphereDistance(const Sphere& s1, const Transform3s& tf1,
 const Sphere& s2, const Transform3s& tf2,
 Vec3s& p1, Vec3s& p2, Vec3s& normal) {
 const coal::Vec3s& center1 = tf1.getTranslation();
 const coal::Vec3s& center2 = tf2.getTranslation();
 CoalScalar r1 = (s1.radius + s1.getSweptSphereRadius());
 CoalScalar r2 = (s2.radius + s2.getSweptSphereRadius());

 Vec3s c1c2 = center2 - center1;
 CoalScalar cdist = c1c2.norm();
 Vec3s unit(1, 0, 0);
 if (cdist > Eigen::NumTraits<CoalScalar>::epsilon()) unit = c1c2 / cdist;
 CoalScalar dist = cdist - r1 - r2;
 normal = unit;
 p1.noalias() = center1 + r1 * unit;
 p2.noalias() = center2 - r2 * unit;
 return dist;
}

inline CoalScalar segmentSqrDistance(const Vec3s& from, const Vec3s& to,
 const Vec3s& p, Vec3s& nearest) {
 Vec3s diff = p - from;
 Vec3s v = to - from;
 CoalScalar t = v.dot(diff);

 if (t > 0) {
 CoalScalar 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();
}

inline bool projectInTriangle(const Vec3s& p1, const Vec3s& p2, const Vec3s& p3,
 const Vec3s& normal, const Vec3s& p) {
 Vec3s edge1(p2 - p1);
 Vec3s edge2(p3 - p2);
 Vec3s edge3(p1 - p3);

 Vec3s p1_to_p(p - p1);
 Vec3s p2_to_p(p - p2);
 Vec3s p3_to_p(p - p3);

 Vec3s edge1_normal(edge1.cross(normal));
 Vec3s edge2_normal(edge2.cross(normal));
 Vec3s edge3_normal(edge3.cross(normal));

 CoalScalar 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;
}

inline CoalScalar sphereTriangleDistance(const Sphere& s,
 const Transform3s& tf1,
 const TriangleP& tri,
 const Transform3s& tf2, Vec3s& p1,
 Vec3s& p2, Vec3s& normal) {
 const Vec3s& P1 = tf2.transform(tri.a);
 const Vec3s& P2 = tf2.transform(tri.b);
 const Vec3s& P3 = tf2.transform(tri.c);

 Vec3s tri_normal = (P2 - P1).cross(P3 - P1);
 tri_normal.normalize();
 const Vec3s& center = tf1.getTranslation();
 // Note: comparing an object with a swept-sphere radius of r1 against another
 // object with a swept-sphere radius of r2 is equivalent to comparing the
 // first object with a swept-sphere radius of r1 + r2 against the second
 // object with a swept-sphere radius of 0.
 const CoalScalar& radius =
 s.radius + s.getSweptSphereRadius() + tri.getSweptSphereRadius();
 assert(radius >= 0);
 assert(s.radius >= 0);
 Vec3s p1_to_center = center - P1;
 CoalScalar distance_from_plane = p1_to_center.dot(tri_normal);
 Vec3s closest_point(
 Vec3s::Constant(std::numeric_limits<CoalScalar>::quiet_NaN()));
 CoalScalar min_distance_sqr, distance_sqr;

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

 if (projectInTriangle(P1, P2, P3, tri_normal, center)) {
 closest_point = center - tri_normal * distance_from_plane;
 min_distance_sqr = distance_from_plane * distance_from_plane;
 } else {
 // Compute distance to each edge and take minimal distance
 Vec3s 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;
 }
 }

 normal = (closest_point - center).normalized();
 p1 = center + normal * (s.radius + s.getSweptSphereRadius());
 p2 = closest_point - normal * tri.getSweptSphereRadius();
 const CoalScalar distance = std::sqrt(min_distance_sqr) - radius;
 return distance;
}

inline CoalScalar halfspaceDistance(const Halfspace& h, const Transform3s& tf1,
 const ShapeBase& s, const Transform3s& tf2,
 Vec3s& p1, Vec3s& p2, Vec3s& normal) {
 // TODO(louis): handle multiple contact points when the halfspace normal is
 // parallel to the shape's surface (every primitive except sphere and
 // ellipsoid).

 // Express halfspace in world frame
 Halfspace new_h = transform(h, tf1);

 // Express halfspace normal in shape frame
 Vec3s n_2(tf2.getRotation().transpose() * new_h.n);

 // Compute support of shape in direction of halfspace normal
 int hint = 0;
 p2.noalias() =
 getSupport<details::SupportOptions::WithSweptSphere>(&s, -n_2, hint);
 p2 = tf2.transform(p2);

 const CoalScalar dist = new_h.signedDistance(p2);
 p1.noalias() = p2 - dist * new_h.n;
 normal.noalias() = new_h.n;

 const CoalScalar dummy_precision =
 std::sqrt(Eigen::NumTraits<CoalScalar>::dummy_precision());
 COAL_UNUSED_VARIABLE(dummy_precision);
 assert(new_h.distance(p1) <= dummy_precision);
 return dist;
}

inline CoalScalar planeDistance(const Plane& plane, const Transform3s& tf1,
 const ShapeBase& s, const Transform3s& tf2,
 Vec3s& p1, Vec3s& p2, Vec3s& normal) {
 // TODO(louis): handle multiple contact points when the plane normal is
 // parallel to the shape's surface (every primitive except sphere and
 // ellipsoid).

 // Express plane as two halfspaces in world frame
 std::array<Halfspace, 2> new_h = transformToHalfspaces(plane, tf1);

 // Express halfspace normals in shape frame
 Vec3s n_h1(tf2.getRotation().transpose() * new_h[0].n);
 Vec3s n_h2(tf2.getRotation().transpose() * new_h[1].n);

 // Compute support of shape in direction of halfspace normal and its opposite
 int hint = 0;
 Vec3s p2h1 =
 getSupport<details::SupportOptions::WithSweptSphere>(&s, -n_h1, hint);
 p2h1 = tf2.transform(p2h1);

 hint = 0;
 Vec3s p2h2 =
 getSupport<details::SupportOptions::WithSweptSphere>(&s, -n_h2, hint);
 p2h2 = tf2.transform(p2h2);

 CoalScalar dist1 = new_h[0].signedDistance(p2h1);
 CoalScalar dist2 = new_h[1].signedDistance(p2h2);

 const CoalScalar dummy_precision =
 std::sqrt(Eigen::NumTraits<CoalScalar>::dummy_precision());
 COAL_UNUSED_VARIABLE(dummy_precision);

 CoalScalar dist;
 if (dist1 >= dist2) {
 dist = dist1;
 p2.noalias() = p2h1;
 p1.noalias() = p2 - dist * new_h[0].n;
 normal.noalias() = new_h[0].n;
 assert(new_h[0].distance(p1) <= dummy_precision);
 } else {
 dist = dist2;
 p2.noalias() = p2h2;
 p1.noalias() = p2 - dist * new_h[1].n;
 normal.noalias() = new_h[1].n;
 assert(new_h[1].distance(p1) <= dummy_precision);
 }
 return dist;
}

inline CoalScalar boxSphereDistance(const Box& b, const Transform3s& tfb,
 const Sphere& s, const Transform3s& tfs,
 Vec3s& pb, Vec3s& ps, Vec3s& normal) {
 const Vec3s& os = tfs.getTranslation();
 const Vec3s& ob = tfb.getTranslation();
 const Matrix3s& Rb = tfb.getRotation();

 pb = ob;

 bool outside = false;
 const Vec3s os_in_b_frame(Rb.transpose() * (os - ob));
 int axis = -1;
 CoalScalar min_d = (std::numeric_limits<CoalScalar>::max)();
 for (int i = 0; i < 3; ++i) {
 CoalScalar 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;
 CoalScalar pdist = normal.norm();
 CoalScalar dist; // distance between sphere and box
 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;
 }
 ps = os - s.radius * normal;
 if (!outside || dist <= 0) {
 // project point pb onto the box's surface
 pb = ps - dist * normal;
 }

 // Take swept-sphere radius into account
 const CoalScalar ssrb = b.getSweptSphereRadius();
 const CoalScalar ssrs = s.getSweptSphereRadius();
 if (ssrb > 0 || ssrs > 0) {
 pb += ssrb * normal;
 ps -= ssrs * normal;
 dist -= (ssrb + ssrs);
 }

 return dist;
}

inline CoalScalar halfspaceHalfspaceDistance(const Halfspace& s1,
 const Transform3s& tf1,
 const Halfspace& s2,
 const Transform3s& tf2, Vec3s& p1,
 Vec3s& p2, Vec3s& normal) {
 Halfspace new_s1 = transform(s1, tf1);
 Halfspace new_s2 = transform(s2, tf2);

 CoalScalar distance;
 Vec3s dir = (new_s1.n).cross(new_s2.n);
 CoalScalar dir_sq_norm = dir.squaredNorm();

 if (dir_sq_norm < std::numeric_limits<CoalScalar>::epsilon()) // parallel
 {
 if (new_s1.n.dot(new_s2.n) > 0) {
 // If the two halfspaces have the same normal, one is inside the other
 // and they can't be separated. They have inifinte penetration depth.
 distance = -(std::numeric_limits<CoalScalar>::max)();
 if (new_s1.d <= new_s2.d) {
 normal = new_s1.n;
 p1 = normal * distance;
 p2 = new_s2.n * new_s2.d;
 assert(new_s2.distance(p2) <=
 Eigen::NumTraits<CoalScalar>::dummy_precision());
 } else {
 normal = -new_s1.n;
 p1 << new_s1.n * new_s1.d;
 p2 = -(normal * distance);
 assert(new_s1.distance(p1) <=
 Eigen::NumTraits<CoalScalar>::dummy_precision());
 }
 } else {
 distance = -(new_s1.d + new_s2.d);
 normal = new_s1.n;
 p1 = new_s1.n * new_s1.d;
 p2 = new_s2.n * new_s2.d;
 }
 } else {
 // If the halfspaces are not parallel, they are in collision.
 // Their distance, in the sens of the norm of separation vector, is infinite
 // (it's impossible to find a translation which separates them)
 distance = -(std::numeric_limits<CoalScalar>::max)();
 // p1 and p2 are the same point, corresponding to a point on the
 // intersection line between the two objects. Normal is the direction of
 // that line.
 normal = dir;
 p1 = p2 =
 ((new_s2.n * new_s1.d - new_s1.n * new_s2.d).cross(dir)) / dir_sq_norm;
 // Sources: https://en.wikipedia.org/wiki/Plane%E2%80%93plane_intersection
 // and https://en.wikipedia.org/wiki/Cross_product
 }

 // Take swept-sphere radius into account
 const CoalScalar ssr1 = s1.getSweptSphereRadius();
 const CoalScalar ssr2 = s2.getSweptSphereRadius();
 if (ssr1 > 0 || ssr2 > 0) {
 p1 += ssr1 * normal;
 p2 -= ssr2 * normal;
 distance -= (ssr1 + ssr2);
 }

 return distance;
}

inline CoalScalar halfspacePlaneDistance(const Halfspace& s1,
 const Transform3s& tf1,
 const Plane& s2,
 const Transform3s& tf2, Vec3s& p1,
 Vec3s& p2, Vec3s& normal) {
 Halfspace new_s1 = transform(s1, tf1);
 Plane new_s2 = transform(s2, tf2);

 CoalScalar distance;
 Vec3s dir = (new_s1.n).cross(new_s2.n);
 CoalScalar dir_sq_norm = dir.squaredNorm();

 if (dir_sq_norm < std::numeric_limits<CoalScalar>::epsilon()) // parallel
 {
 normal = new_s1.n;
 distance = new_s1.n.dot(new_s2.n) > 0 ? (new_s2.d - new_s1.d)
 : -(new_s1.d + new_s2.d);
 p1 = new_s1.n * new_s1.d;
 p2 = new_s2.n * new_s2.d;
 assert(new_s1.distance(p1) <=
 Eigen::NumTraits<CoalScalar>::dummy_precision());
 assert(new_s2.distance(p2) <=
 Eigen::NumTraits<CoalScalar>::dummy_precision());
 } else {
 // If the halfspace and plane are not parallel, they are in collision.
 // Their distance, in the sens of the norm of separation vector, is infinite
 // (it's impossible to find a translation which separates them)
 distance = -(std::numeric_limits<CoalScalar>::max)();
 // p1 and p2 are the same point, corresponding to a point on the
 // intersection line between the two objects. Normal is the direction of
 // that line.
 normal = dir;
 p1 = p2 =
 ((new_s2.n * new_s1.d - new_s1.n * new_s2.d).cross(dir)) / dir_sq_norm;
 // Sources: https://en.wikipedia.org/wiki/Plane%E2%80%93plane_intersection
 // and https://en.wikipedia.org/wiki/Cross_product
 }

 // Take swept-sphere radius into account
 const CoalScalar ssr1 = s1.getSweptSphereRadius();
 const CoalScalar ssr2 = s2.getSweptSphereRadius();
 if (ssr1 > 0 || ssr2 > 0) {
 p1 += ssr1 * normal;
 p2 -= ssr2 * normal;
 distance -= (ssr1 + ssr2);
 }

 return distance;
}

inline CoalScalar planePlaneDistance(const Plane& s1, const Transform3s& tf1,
 const Plane& s2, const Transform3s& tf2,
 Vec3s& p1, Vec3s& p2, Vec3s& normal) {
 Plane new_s1 = transform(s1, tf1);
 Plane new_s2 = transform(s2, tf2);

 CoalScalar distance;
 Vec3s dir = (new_s1.n).cross(new_s2.n);
 CoalScalar dir_sq_norm = dir.squaredNorm();

 if (dir_sq_norm < std::numeric_limits<CoalScalar>::epsilon()) // parallel
 {
 p1 = new_s1.n * new_s1.d;
 p2 = new_s2.n * new_s2.d;
 assert(new_s1.distance(p1) <=
 Eigen::NumTraits<CoalScalar>::dummy_precision());
 assert(new_s2.distance(p2) <=
 Eigen::NumTraits<CoalScalar>::dummy_precision());
 distance = (p1 - p2).norm();

 if (distance > Eigen::NumTraits<CoalScalar>::dummy_precision()) {
 normal = (p2 - p1).normalized();
 } else {
 normal = new_s1.n;
 }
 } else {
 // If the planes are not parallel, they are in collision.
 // Their distance, in the sens of the norm of separation vector, is infinite
 // (it's impossible to find a translation which separates them)
 distance = -(std::numeric_limits<CoalScalar>::max)();
 // p1 and p2 are the same point, corresponding to a point on the
 // intersection line between the two objects. Normal is the direction of
 // that line.
 normal = dir;
 p1 = p2 =
 ((new_s2.n * new_s1.d - new_s1.n * new_s2.d).cross(dir)) / dir_sq_norm;
 // Sources: https://en.wikipedia.org/wiki/Plane%E2%80%93plane_intersection
 // and https://en.wikipedia.org/wiki/Cross_product
 }

 // Take swept-sphere radius into account
 const CoalScalar ssr1 = s1.getSweptSphereRadius();
 const CoalScalar ssr2 = s2.getSweptSphereRadius();
 if (ssr1 > 0 || ssr2 > 0) {
 p1 += ssr1 * normal;
 p2 -= ssr2 * normal;
 distance -= (ssr1 + ssr2);
 }

 return distance;
}

inline CoalScalar computePenetration(const Vec3s& P1, const Vec3s& P2,
 const Vec3s& P3, const Vec3s& Q1,
 const Vec3s& Q2, const Vec3s& Q3,
 Vec3s& normal) {
 Vec3s u((P2 - P1).cross(P3 - P1));
 normal = u.normalized();
 CoalScalar depth1((P1 - Q1).dot(normal));
 CoalScalar depth2((P1 - Q2).dot(normal));
 CoalScalar 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 CoalScalar computePenetration(const Vec3s& P1, const Vec3s& P2,
 const Vec3s& P3, const Vec3s& Q1,
 const Vec3s& Q2, const Vec3s& Q3,
 const Transform3s& tf1,
 const Transform3s& tf2, Vec3s& normal) {
 Vec3s globalP1(tf1.transform(P1));
 Vec3s globalP2(tf1.transform(P2));
 Vec3s globalP3(tf1.transform(P3));
 Vec3s globalQ1(tf2.transform(Q1));
 Vec3s globalQ2(tf2.transform(Q2));
 Vec3s globalQ3(tf2.transform(Q3));
 return computePenetration(globalP1, globalP2, globalP3, globalQ1, globalQ2,
 globalQ3, normal);
}

} // namespace details
} // namespace coal

#endif // COAL_SRC_NARROWPHASE_DETAILS_H