Geometry.h
Go to the documentation of this file.
// This file is a part of the OpenSurgSim project.
// Copyright 2013-2015, SimQuest Solutions Inc.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
// http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
#ifndef SURGSIM_MATH_GEOMETRY_H
#define SURGSIM_MATH_GEOMETRY_H
#include <boost/container/static_vector.hpp>
#include <Eigen/Core>
#include <Eigen/Geometry>
#include "SurgSim/Framework/Log.h"
#include "SurgSim/Math/Polynomial.h"
#include "SurgSim/Math/Vector.h"
namespace SurgSim
{
namespace Math
{
namespace Geometry
{
static const double DistanceEpsilon = 1e-10;
static const double SquaredDistanceEpsilon = 1e-10;
static const double AngularEpsilon = 1e-10;
static const double ScalarEpsilon = 1e-10;
}
template <class T, int MOpt> inline
bool barycentricCoordinates(const Eigen::Matrix<T, 3, 1, MOpt>& pt,
const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
Eigen::Matrix<T, 2, 1, MOpt>* coordinates)
{
const Eigen::Matrix<T, 3, 1, MOpt> line = sv1 - sv0;
const T length2 = line.squaredNorm();
if (length2 < Geometry::SquaredDistanceEpsilon)
{
coordinates->setConstant((std::numeric_limits<double>::quiet_NaN()));
return false;
}
(*coordinates)[1] = (pt - sv0).dot(line) / length2;
(*coordinates)[0] = static_cast<T>(1) - (*coordinates)[1];
return true;
}
template <class T, int MOpt> inline
bool barycentricCoordinates(const Eigen::Matrix<T, 3, 1, MOpt>& pt,
const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
const Eigen::Matrix<T, 3, 1, MOpt>& tn,
Eigen::Matrix<T, 3, 1, MOpt>* coordinates)
{
const T signedTriAreaX2 = ((tv1 - tv0).cross(tv2 - tv0)).dot(tn);
if (signedTriAreaX2 < Geometry::SquaredDistanceEpsilon)
{
// SQ_ASSERT_WARNING(false, "Cannot compute barycentric coords (degenetrate triangle), assigning center");
coordinates->setConstant((std::numeric_limits<double>::quiet_NaN()));
return false;
}
(*coordinates)[0] = ((tv1 - pt).cross(tv2 - pt)).dot(tn) / signedTriAreaX2;
(*coordinates)[1] = ((tv2 - pt).cross(tv0 - pt)).dot(tn) / signedTriAreaX2;
(*coordinates)[2] = 1 - (*coordinates)[0] - (*coordinates)[1];
return true;
}
template <class T, int MOpt> inline
bool barycentricCoordinates(
const Eigen::Matrix<T, 3, 1, MOpt>& pt,
const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
Eigen::Matrix<T, 3, 1, MOpt>* coordinates)
{
Eigen::Matrix<T, 3, 1, MOpt> tn = (tv1 - tv0).cross(tv2 - tv0);
double norm = tn.norm();
if (norm < Geometry::DistanceEpsilon)
{
coordinates->setConstant((std::numeric_limits<double>::quiet_NaN()));
return false;
}
tn /= norm;
return barycentricCoordinates(pt, tv0, tv1, tv2, tn, coordinates);
}
template <class T, int MOpt> inline
bool isPointInsideTriangle(
const Eigen::Matrix<T, 3, 1, MOpt>& pt,
const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
const Eigen::Matrix<T, 3, 1, MOpt>& tn)
{
Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
bool result = barycentricCoordinates(pt, tv0, tv1, tv2, tn, &baryCoords);
return (result &&
baryCoords[0] >= -Geometry::ScalarEpsilon &&
baryCoords[1] >= -Geometry::ScalarEpsilon &&
baryCoords[2] >= -Geometry::ScalarEpsilon);
}
template <class T, int MOpt> inline
bool isPointInsideTriangle(
const Eigen::Matrix<T, 3, 1, MOpt>& pt,
const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
const Eigen::Matrix<T, 3, 1, MOpt>& tv2)
{
Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
bool result = barycentricCoordinates(pt, tv0, tv1, tv2, &baryCoords);
return (result && baryCoords[0] >= -Geometry::ScalarEpsilon &&
baryCoords[1] >= -Geometry::ScalarEpsilon &&
baryCoords[2] >= -Geometry::ScalarEpsilon);
}
template <class T, int MOpt> inline
bool isPointOnTriangleEdge(
const Eigen::Matrix<T, 3, 1, MOpt>& pt,
const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
const Eigen::Matrix<T, 3, 1, MOpt>& tn)
{
Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
bool result = barycentricCoordinates(pt, tv0, tv1, tv2, tn, &baryCoords);
return (result && baryCoords[0] >= -Geometry::ScalarEpsilon &&
baryCoords[1] >= -Geometry::ScalarEpsilon &&
baryCoords[2] >= -Geometry::ScalarEpsilon &&
baryCoords.minCoeff() <= Geometry::ScalarEpsilon);
}
template <class T, int MOpt> inline
bool isPointOnTriangleEdge(
const Eigen::Matrix<T, 3, 1, MOpt>& pt,
const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
const Eigen::Matrix<T, 3, 1, MOpt>& tv2)
{
Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
bool result = barycentricCoordinates(pt, tv0, tv1, tv2, &baryCoords);
return (result && baryCoords[0] >= -Geometry::ScalarEpsilon &&
baryCoords[1] >= -Geometry::ScalarEpsilon &&
baryCoords[2] >= -Geometry::ScalarEpsilon &&
baryCoords.minCoeff() <= Geometry::ScalarEpsilon);
}
template <class T, int MOpt> inline
bool isCoplanar(
const Eigen::Matrix<T, 3, 1, MOpt>& a,
const Eigen::Matrix<T, 3, 1, MOpt>& b,
const Eigen::Matrix<T, 3, 1, MOpt>& c,
const Eigen::Matrix<T, 3, 1, MOpt>& d)
{
return std::abs((c - a).dot((b - a).cross(d - c))) < Geometry::ScalarEpsilon;
}
template <class T, int MOpt> inline
T distancePointLine(
const Eigen::Matrix<T, 3, 1, MOpt>& pt,
const Eigen::Matrix<T, 3, 1, MOpt>& v0,
const Eigen::Matrix<T, 3, 1, MOpt>& v1,
Eigen::Matrix<T, 3, 1, MOpt>* result)
{
// The lines is parametrized by:
// q = v0 + lambda0 * (v1-v0)
// and we solve for pq.v01 = 0;
Eigen::Matrix<T, 3, 1, MOpt> v01 = v1 - v0;
T v01_norm2 = v01.squaredNorm();
if (v01_norm2 <= Geometry::SquaredDistanceEpsilon)
{
*result = v0; // closest point is either
T pv_norm2 = (pt - v0).squaredNorm();
return sqrt(pv_norm2);
}
T lambda = (v01).dot(pt - v0);
*result = v0 + lambda * v01 / v01_norm2;
return (*result - pt).norm();
}
template <class T, int MOpt> inline
T distancePointSegment(
const Eigen::Matrix<T, 3, 1, MOpt>& pt,
const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
Eigen::Matrix<T, 3, 1, MOpt>* result)
{
Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1 - sv0;
T v01Norm2 = v01.squaredNorm();
if (v01Norm2 <= Geometry::SquaredDistanceEpsilon)
{
*result = sv0; // closest point is either
return (pt - sv0).norm();
}
T lambda = v01.dot(pt - sv0);
if (lambda <= 0)
{
*result = sv0;
}
else if (lambda >= v01Norm2)
{
*result = sv1;
}
else
{
*result = sv0 + lambda * v01 / v01Norm2;
}
return (*result - pt).norm();
}
template <class T, int MOpt> inline
T distanceLineLine(
const Eigen::Matrix<T, 3, 1, MOpt>& l0v0,
const Eigen::Matrix<T, 3, 1, MOpt>& l0v1,
const Eigen::Matrix<T, 3, 1, MOpt>& l1v0,
const Eigen::Matrix<T, 3, 1, MOpt>& l1v1,
Eigen::Matrix<T, 3, 1, MOpt>* pt0,
Eigen::Matrix<T, 3, 1, MOpt>* pt1)
{
// Based on the outline of http://www.geometrictools.com/Distance.html, also refer to
// http://geomalgorithms.com/a07-_distance.html for a geometric interpretation
// The lines are parametrized by:
// p0 = l0v0 + lambda0 * (l0v1-l0v0)
// p1 = l1v0 + lambda1 * (l1v1-l1v0)
// and we solve for p0p1 perpendicular to both lines
T lambda0, lambda1;
Eigen::Matrix<T, 3, 1, MOpt> l0v01 = l0v1 - l0v0;
T a = l0v01.squaredNorm();
if (a <= Geometry::SquaredDistanceEpsilon)
{
// Degenerate line 0
*pt0 = l0v0;
return distancePointSegment(l0v0, l1v0, l1v1, pt1);
}
Eigen::Matrix<T, 3, 1, MOpt> l1v01 = l1v1 - l1v0;
T b = -l0v01.dot(l1v01);
T c = l1v01.squaredNorm();
if (c <= Geometry::SquaredDistanceEpsilon)
{
// Degenerate line 1
*pt1 = l1v0;
return distancePointSegment(l1v0, l0v0, l0v1, pt0);
}
Eigen::Matrix<T, 3, 1, MOpt> l0v0_l1v0 = l0v0 - l1v0;
T d = l0v01.dot(l0v0_l1v0);
T e = -l1v01.dot(l0v0_l1v0);
T ratio = a * c - b * b;
if (std::abs(ratio) <= Geometry::ScalarEpsilon)
{
// parallel case
lambda0 = 0;
lambda1 = e / c;
}
else
{
// non-parallel case
T inv_ratio = T(1) / ratio;
lambda0 = (b * e - c * d) * inv_ratio;
lambda1 = (b * d - a * e) * inv_ratio;
}
*pt0 = l0v0 + lambda0 * l0v01;
*pt1 = l1v0 + lambda1 * l1v01;
return ((*pt0) - (*pt1)).norm();
}
template <class T, int MOpt>
T distanceSegmentSegment(
const Eigen::Matrix<T, 3, 1, MOpt>& s0v0,
const Eigen::Matrix<T, 3, 1, MOpt>& s0v1,
const Eigen::Matrix<T, 3, 1, MOpt>& s1v0,
const Eigen::Matrix<T, 3, 1, MOpt>& s1v1,
Eigen::Matrix<T, 3, 1, MOpt>* pt0,
Eigen::Matrix<T, 3, 1, MOpt>* pt1,
T* s0t = nullptr,
T* s1t = nullptr)
{
// Based on the outline of http://www.geometrictools.com/Documentation/DistanceLine3Line3.pdf, also refer to
// http://geomalgorithms.com/a07-_distance.html for a geometric interpretation
// The segments are parametrized by:
// p0 = l0v0 + s * (l0v1-l0v0), with s between 0 and 1
// p1 = l1v0 + t * (l1v1-l1v0), with t between 0 and 1
// We are minimizing Q(s, t) = as*as + 2bst + ct*ct + 2ds + 2et + f,
Eigen::Matrix<T, 3, 1, MOpt> s0v01 = s0v1 - s0v0;
T a = s0v01.squaredNorm();
if (a <= Geometry::SquaredDistanceEpsilon)
{
// Degenerate segment 0
*pt0 = s0v0;
return distancePointSegment<T>(s0v0, s1v0, s1v1, pt1);
}
Eigen::Matrix<T, 3, 1, MOpt> s1v01 = s1v1 - s1v0;
T b = -s0v01.dot(s1v01);
T c = s1v01.squaredNorm();
if (c <= Geometry::SquaredDistanceEpsilon)
{
// Degenerate segment 1
*pt1 = s1v1;
return distancePointSegment<T>(s1v0, s0v0, s0v1, pt0);
}
Eigen::Matrix<T, 3, 1, MOpt> tempLine = s0v0 - s1v0;
T d = s0v01.dot(tempLine);
T e = -s1v01.dot(tempLine);
T ratio = a * c - b * b;
T s, t; // parametrization variables (do not initialize)
int region = -1;
T tmp;
// Non-parallel case
if (1.0 - std::abs(s0v01.normalized().dot(s1v01.normalized())) >= Geometry::SquaredDistanceEpsilon)
{
// Get the region of the global minimum in the s-t space based on the line-line solution
// s=0 s=1
// ^
// | |
// 4 | 3 | 2
// ----|-------|------- t=1
// | |
// 5 | 0 | 1
// | |
// ----|-------|-------> t=0
// | |
// 6 | 7 | 8
// | |
//
s = b * e - c * d;
t = b * d - a * e;
if (s >= 0)
{
if (s <= ratio)
{
if (t >= 0)
{
if (t <= ratio)
{
region = 0;
}
else
{
region = 3;
}
}
else
{
region = 7;
}
}
else
{
if (t >= 0)
{
if (t <= ratio)
{
region = 1;
}
else
{
region = 2;
}
}
else
{
region = 8;
}
}
}
else
{
if (t >= 0)
{
if (t <= ratio)
{
region = 5;
}
else
{
region = 4;
}
}
else
{
region = 6;
}
}
enum edge_type { s0, s1, t0, t1, edge_skip, edge_invalid };
edge_type edge = edge_invalid;
switch (region)
{
case 0:
// Global minimum inside [0,1] [0,1]
s /= ratio;
t /= ratio;
edge = edge_skip;
break;
case 1:
edge = s1;
break;
case 2:
// Q_s(1,1)/2 = a+b+d
if (a + b + d > 0)
{
edge = t1;
}
else
{
edge = s1;
}
break;
case 3:
edge = t1;
break;
case 4:
// Q_s(0,1)/2 = b+d
if (b + d > 0)
{
edge = s0;
}
else
{
edge = t1;
}
break;
case 5:
edge = s0;
break;
case 6:
// Q_s(0,0)/2 = d
if (d > 0)
{
edge = s0;
}
else
{
edge = t0;
}
break;
case 7:
edge = t0;
break;
case 8:
// Q_s(1,0)/2 = a+d
if (a + d > 0)
{
edge = t0;
}
else
{
edge = s1;
}
break;
default:
break;
}
switch (edge)
{
case s0:
// F(t) = Q(0,t), F?(t) = 2*(e+c*t)
// F?(T) = 0 when T = -e/c, then clamp between 0 and 1 (c always >= 0)
s = 0;
tmp = e;
if (tmp > 0)
{
t = 0;
}
else if (-tmp > c)
{
t = 1;
}
else
{
t = -tmp / c;
}
break;
case s1:
// F(t) = Q(1,t), F?(t) = 2*((b+e)+c*t)
// F?(T) = 0 when T = -(b+e)/c, then clamp between 0 and 1 (c always >= 0)
s = 1;
tmp = b + e;
if (tmp > 0)
{
t = 0;
}
else if (-tmp > c)
{
t = 1;
}
else
{
t = -tmp / c;
}
break;
case t0:
// F(s) = Q(s,0), F?(s) = 2*(d+a*s) =>
// F?(S) = 0 when S = -d/a, then clamp between 0 and 1 (a always >= 0)
t = 0;
tmp = d;
if (tmp > 0)
{
s = 0;
}
else if (-tmp > a)
{
s = 1;
}
else
{
s = -tmp / a;
}
break;
case t1:
// F(s) = Q(s,1), F?(s) = 2*(b+d+a*s) =>
// F?(S) = 0 when S = -(b+d)/a, then clamp between 0 and 1 (a always >= 0)
t = 1;
tmp = b + d;
if (tmp > 0)
{
s = 0;
}
else if (-tmp > a)
{
s = 1;
}
else
{
s = -tmp / a;
}
break;
case edge_skip:
break;
default:
break;
}
}
else
// Parallel case
{
if (b > 0)
{
// Segments have different directions
if (d >= 0)
{
// 0-0 end points since s-segment 0 less than t-segment 0
s = 0;
t = 0;
}
else if (-d <= a)
{
// s-segment 0 end-point in the middle of the t 0-1 segment, get distance
s = -d / a;
t = 0;
}
else
{
// s-segment 1 is definitely closer
s = 1;
tmp = a + d;
if (-tmp >= b)
{
t = 1;
}
else
{
t = -tmp / b;
}
}
}
else
{
// Both segments have the same dir
if (-d >= a)
{
// 1-0
s = 1;
t = 0;
}
else if (d <= 0)
{
// mid-0
s = -d / a;
t = 0;
}
else
{
s = 0;
// 1-mid
if (d >= -b)
{
t = 1;
}
else
{
t = -d / b;
}
}
}
}
*pt0 = s0v0 + s * (s0v01);
*pt1 = s1v0 + t * (s1v01);
if (s0t != nullptr && s1t != nullptr)
{
*s0t = s;
*s1t = t;
}
return ((*pt1) - (*pt0)).norm();
}
template <class T, int MOpt> inline
T distancePointTriangle(
const Eigen::Matrix<T, 3, 1, MOpt>& pt,
const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
Eigen::Matrix<T, 3, 1, MOpt>* result)
{
// Based on the outline of http://www.geometrictools.com/Distance.html, also refer to
// http://softsurfer.com/Archive/algorithm_0106 for a geometric interpretation
// The triangle is parametrized by:
// t: tv0 + s * (tv1-tv0) + t * (tv2-tv0) , with s and t between 0 and 1
// We are minimizing Q(s, t) = as*as + 2bst + ct*ct + 2ds + 2et + f,
Eigen::Matrix<T, 3, 1, MOpt> tv01 = tv1 - tv0;
Eigen::Matrix<T, 3, 1, MOpt> tv02 = tv2 - tv0;
T a = tv01.squaredNorm();
if (a <= Geometry::SquaredDistanceEpsilon)
{
// Degenerate edge 1
return distancePointSegment<T>(pt, tv0, tv2, result);
}
T b = tv01.dot(tv02);
T tCross = tv01.cross(tv02).squaredNorm();
if (tCross <= Geometry::SquaredDistanceEpsilon)
{
// Degenerate edge 2
return distancePointSegment<T>(pt, tv0, tv1, result);
}
T c = tv02.squaredNorm();
if (c <= Geometry::SquaredDistanceEpsilon)
{
// Degenerate edge 3
return distancePointSegment<T>(pt, tv0, tv1, result);
}
Eigen::Matrix<T, 3, 1, MOpt> tv0pv0 = tv0 - pt;
T d = tv01.dot(tv0pv0);
T e = tv02.dot(tv0pv0);
T ratio = a * c - b * b;
T s = b * e - c * d;
T t = b * d - a * e;
// Determine region (inside-outside triangle)
int region = -1;
if (s + t <= ratio)
{
if (s < 0)
{
if (t < 0)
{
region = 4;
}
else
{
region = 3;
}
}
else if (t < 0)
{
region = 5;
}
else
{
region = 0;
}
}
else
{
if (s < 0)
{
region = 2;
}
else if (t < 0)
{
region = 6;
}
else
{
region = 1;
}
}
// Regions: /
// ^ t=0 /
// \ 2| /
// \ | /
// \| /
// \ /
// |\ /
// | \ 1 /
// 3 | \ /
// | 0 \ /
// ----|----\-------> s=0 /
// | \ /
// 4 | 5 \ 6 /
// | \ /
// /
T numer, denom, tmp0, tmp1;
enum edge_type { s0, t0, s1t1, edge_skip, edge_invalid };
edge_type edge = edge_invalid;
switch (region)
{
case 0:
// Global minimum inside [0,1] [0,1]
numer = T(1) / ratio;
s *= numer;
t *= numer;
edge = edge_skip;
break;
case 1:
edge = s1t1;
break;
case 2:
// Grad(Q(0,1)).(0,-1)/2 = -c-e
// Grad(Q(0,1)).(1,-1)/2 = b=d-c-e
tmp0 = b + d;
tmp1 = c + e;
if (tmp1 > tmp0)
{
edge = s1t1;
}
else
{
edge = s0;
}
break;
case 3:
edge = s0;
break;
case 4:
// Grad(Q(0,0)).(0,1)/2 = e
// Grad(Q(0,0)).(1,0)/2 = d
if (e >= d)
{
edge = t0;
}
else
{
edge = s0;
}
break;
case 5:
edge = t0;
break;
case 6:
// Grad(Q(1,0)).(-1,0)/2 = -a-d
// Grad(Q(1,0)).(-1,1)/2 = -a-d+b+e
tmp0 = -a - d;
tmp1 = -a - d + b + e;
if (tmp1 > tmp0)
{
edge = t0;
}
else
{
edge = s1t1;
}
break;
default:
break;
}
switch (edge)
{
case s0:
// F(t) = Q(0, t), F'(t)=0 when -e/c = 0
s = 0;
if (e >= 0)
{
t = 0;
}
else
{
t = (-e >= c ? 1 : -e / c);
}
break;
case t0:
// F(s) = Q(s, 0), F'(s)=0 when -d/a = 0
t = 0;
if (d >= 0)
{
s = 0;
}
else
{
s = (-d >= a ? 1 : -d / a);
}
break;
case s1t1:
// F(s) = Q(s, 1-s), F'(s) = 0 when (c+e-b-d)/(a-2b+c) = 0 (denom = || tv01-tv02 ||^2 always > 0)
numer = c + e - b - d;
if (numer <= 0)
{
s = 0;
}
else
{
denom = a - 2 * b + c;
s = (numer >= denom ? 1 : numer / denom);
}
t = 1 - s;
break;
case edge_skip:
break;
default:
break;
}
*result = tv0 + s * tv01 + t * tv02;
return ((*result) - pt).norm();
}
template <class T, int MOpt> inline
bool doesCollideSegmentTriangle(
const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
const Eigen::Matrix<T, 3, 1, MOpt>& tn,
Eigen::Matrix<T, 3, 1, MOpt>* result)
{
// Triangle edges vectors
Eigen::Matrix<T, 3, 1, MOpt> u = tv1 - tv0;
Eigen::Matrix<T, 3, 1, MOpt> v = tv2 - tv0;
// Ray direction vector
Eigen::Matrix<T, 3, 1, MOpt> dir = sv1 - sv0;
Eigen::Matrix<T, 3, 1, MOpt> w0 = sv0 - tv0;
T a = -tn.dot(w0);
T b = tn.dot(dir);
result->setConstant((std::numeric_limits<double>::quiet_NaN()));
// Ray is parallel to triangle plane
if (std::abs(b) <= Geometry::AngularEpsilon)
{
if (std::abs(a) <= Geometry::AngularEpsilon)
{
// Ray lies in triangle plane
Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
for (int i = 0; i < 2; ++i)
{
barycentricCoordinates((i == 0 ? sv0 : sv1), tv0, tv1, tv2, tn, &baryCoords);
if (baryCoords[0] >= 0 && baryCoords[1] >= 0 && baryCoords[2] >= 0)
{
*result = (i == 0) ? sv0 : sv1;
return true;
}
}
// All segment endpoints outside of triangle
return false;
}
else
{
// Segment parallel to triangle but not in same plane
return false;
}
}
// Get intersect point of ray with triangle plane
T r = a / b;
// Ray goes away from triangle
if (r < -Geometry::DistanceEpsilon)
{
return false;
}
//Ray comes toward triangle but isn't long enough to reach it
if (r > 1 + Geometry::DistanceEpsilon)
{
return false;
}
// Intersect point of ray and plane
Eigen::Matrix<T, 3, 1, MOpt> presumedIntersection = sv0 + r * dir;
// Collision point inside T?
T uu = u.dot(u);
T uv = u.dot(v);
T vv = v.dot(v);
Eigen::Matrix<T, 3, 1, MOpt> w = presumedIntersection - tv0;
T wu = w.dot(u);
T wv = w.dot(v);
T D = uv * uv - uu * vv;
// Get and test parametric coords
T s = (uv * wv - vv * wu) / D;
// I is outside T
if (s < -Geometry::DistanceEpsilon || s > 1 + Geometry::DistanceEpsilon)
{
return false;
}
T t = (uv * wu - uu * wv) / D;
// I is outside T
if (t < -Geometry::DistanceEpsilon || (s + t) > 1 + Geometry::DistanceEpsilon)
{
return false;
}
// I is in T
*result = sv0 + r * dir;
return true;
}
template <class T, int MOpt> inline
T distancePointPlane(
const Eigen::Matrix<T, 3, 1, MOpt>& pt,
const Eigen::Matrix<T, 3, 1, MOpt>& n,
T d,
Eigen::Matrix<T, 3, 1, MOpt>* result)
{
T dist = n.dot(pt) + d;
*result = pt - n * dist;
return dist;
}
template <class T, int MOpt> inline
T distanceSegmentPlane(
const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
const Eigen::Matrix<T, 3, 1, MOpt>& n,
T d,
Eigen::Matrix<T, 3, 1, MOpt>* closestPointSegment,
Eigen::Matrix<T, 3, 1, MOpt>* planeIntersectionPoint)
{
T dist0 = n.dot(sv0) + d;
T dist1 = n.dot(sv1) + d;
// Parallel case
Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1 - sv0;
if (std::abs(n.dot(v01)) <= Geometry::AngularEpsilon)
{
*closestPointSegment = (sv0 + sv1) * T(0.5);
dist0 = n.dot(*closestPointSegment) + d;
*planeIntersectionPoint = *closestPointSegment - dist0 * n;
return (std::abs(dist0) < Geometry::DistanceEpsilon ? 0 : dist0);
}
// Both on the same side
if ((dist0 > 0 && dist1 > 0) || (dist0 < 0 && dist1 < 0))
{
if (std::abs(dist0) < std::abs(dist1))
{
*closestPointSegment = sv0;
*planeIntersectionPoint = sv0 - dist0 * n;
return dist0;
}
else
{
*closestPointSegment = sv1;
*planeIntersectionPoint = sv1 - dist1 * n;
return dist1;
}
}
// Segment cutting through
else
{
Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1 - sv0;
T lambda = (-d - sv0.dot(n)) / v01.dot(n);
*planeIntersectionPoint = sv0 + lambda * v01;
*closestPointSegment = *planeIntersectionPoint;
return 0;
}
}
template <class T, int MOpt> inline
T distanceTrianglePlane(
const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
const Eigen::Matrix<T, 3, 1, MOpt>& n,
T d,
Eigen::Matrix<T, 3, 1, MOpt>* closestPointTriangle,
Eigen::Matrix<T, 3, 1, MOpt>* planeProjectionPoint)
{
Eigen::Matrix<T, 3, 1, MOpt> distances(n.dot(tv0) + d, n.dot(tv1) + d, n.dot(tv2) + d);
Eigen::Matrix<T, 3, 1, MOpt> t01 = tv1 - tv0;
Eigen::Matrix<T, 3, 1, MOpt> t02 = tv2 - tv0;
Eigen::Matrix<T, 3, 1, MOpt> t12 = tv2 - tv1;
closestPointTriangle->setConstant((std::numeric_limits<double>::quiet_NaN()));
planeProjectionPoint->setConstant((std::numeric_limits<double>::quiet_NaN()));
// HS-2013-may-09 Could there be a case where we fall into the wrong tree because of the checks against
// the various epsilon values all going against us ???
// Parallel case (including Coplanar)
if (std::abs(n.dot(t01)) <= Geometry::AngularEpsilon && std::abs(n.dot(t02)) <= Geometry::AngularEpsilon)
{
*closestPointTriangle = (tv0 + tv1 + tv2) / T(3);
*planeProjectionPoint = *closestPointTriangle - n * distances[0];
return distances[0];
}
// Is there an intersection
if ((distances.array() < -Geometry::DistanceEpsilon).any() &&
(distances.array() > Geometry::DistanceEpsilon).any())
{
if (distances[0] * distances[1] < 0)
{
*closestPointTriangle = tv0 + (-d - n.dot(tv0)) / n.dot(t01) * t01;
if (distances[0] * distances[2] < 0)
{
*planeProjectionPoint = tv0 + (-d - n.dot(tv0)) / n.dot(t02) * t02;
}
else
{
Eigen::Matrix<T, 3, 1, MOpt> t12 = tv2 - tv1;
*planeProjectionPoint = tv1 + (-d - n.dot(tv1)) / n.dot(t12) * t12;
}
}
else
{
*closestPointTriangle = tv0 + (-d - n.dot(tv0)) / n.dot(t02) * t02;
*planeProjectionPoint = tv1 + (-d - n.dot(tv1)) / n.dot(t12) * t12;
}
// Find the midpoint, take this out to return the segment endpoints
*closestPointTriangle = *planeProjectionPoint = (*closestPointTriangle + *planeProjectionPoint) * T(0.5);
return 0;
}
int index;
distances.cwiseAbs().minCoeff(&index);
switch (index)
{
case 0: //distances[0] is closest
*closestPointTriangle = tv0;
*planeProjectionPoint = tv0 - n * distances[0];
return distances[0];
case 1: //distances[1] is closest
*closestPointTriangle = tv1;
*planeProjectionPoint = tv1 - n * distances[1];
return distances[1];
case 2: //distances[2] is closest
*closestPointTriangle = tv2;
*planeProjectionPoint = tv2 - n * distances[2];
return distances[2];
}
return std::numeric_limits<T>::quiet_NaN();
}
template <class T, int MOpt> inline
bool doesIntersectPlanePlane(
const Eigen::Matrix<T, 3, 1, MOpt>& pn0, T pd0,
const Eigen::Matrix<T, 3, 1, MOpt>& pn1, T pd1,
Eigen::Matrix<T, 3, 1, MOpt>* pt0,
Eigen::Matrix<T, 3, 1, MOpt>* pt1)
{
// Algorithm from real time collision detection - optimized version page 210 (with extra checks)
const Eigen::Matrix<T, 3, 1, MOpt> lineDir = pn0.cross(pn1);
const T lineDirNorm2 = lineDir.squaredNorm();
pt0->setConstant((std::numeric_limits<double>::quiet_NaN()));
pt1->setConstant((std::numeric_limits<double>::quiet_NaN()));
// Test if the two planes are parallel
if (lineDirNorm2 <= Geometry::SquaredDistanceEpsilon)
{
return false; // planes disjoint
}
// Compute common point
*pt0 = (pd1 * pn0 - pd0 * pn1).cross(lineDir) / lineDirNorm2;
*pt1 = *pt0 + lineDir;
return true;
}
template <class T, int MOpt> inline
T distanceSegmentTriangle(
const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
Eigen::Matrix<T, 3, 1, MOpt>* segmentPoint,
Eigen::Matrix<T, 3, 1, MOpt>* trianglePoint)
{
Eigen::Matrix<T, 3, 1, MOpt> n = (tv1 - tv0).cross(tv2 - tv1);
n.normalize();
return distanceSegmentTriangle(sv0, sv1, tv0, tv1, tv2, n, segmentPoint, trianglePoint);
}
template <class T, int MOpt> inline
T distanceSegmentTriangle(
const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
const Eigen::Matrix<T, 3, 1, MOpt>& normal,
Eigen::Matrix<T, 3, 1, MOpt>* segmentPoint,
Eigen::Matrix<T, 3, 1, MOpt>* trianglePoint)
{
segmentPoint->setConstant((std::numeric_limits<double>::quiet_NaN()));
trianglePoint->setConstant((std::numeric_limits<double>::quiet_NaN()));
// Setting up the plane that the triangle is in
const Eigen::Matrix<T, 3, 1, MOpt>& n = normal;
T d = -n.dot(tv0);
Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
// Degenerate case: Line and triangle plane parallel
const Eigen::Matrix<T, 3, 1, MOpt> v01 = sv1 - sv0;
const T v01DotTn = n.dot(v01);
if (std::abs(v01DotTn) <= Geometry::AngularEpsilon)
{
// Check if any of the points project onto the tri
// otherwise normal (non-parallel) processing will get the right result
T dst = std::abs(distancePointPlane(sv0, n, d, trianglePoint));
Eigen::Matrix<T, 3, 1, MOpt> baryCoords;
barycentricCoordinates(*trianglePoint, tv0, tv1, tv2, normal, &baryCoords);
if (baryCoords[0] >= 0 && baryCoords[1] >= 0 && baryCoords[2] >= 0)
{
*segmentPoint = sv0;
return dst;
}
dst = std::abs(distancePointPlane(sv1, n, d, trianglePoint));
barycentricCoordinates(*trianglePoint, tv0, tv1, tv2, normal, &baryCoords);
if (baryCoords[0] >= 0 && baryCoords[1] >= 0 && baryCoords[2] >= 0)
{
*segmentPoint = sv1;
return dst;
}
}
// Line and triangle plane *not* parallel: check cut through case only, the rest will be check later
else
{
T lambda = -n.dot(sv0 - tv0) / v01DotTn;
if (lambda >= 0 && lambda <= 1)
{
*segmentPoint = *trianglePoint = sv0 + lambda * v01;
barycentricCoordinates(*trianglePoint, tv0, tv1, tv2, normal, &baryCoords);
if (baryCoords[0] >= 0 && baryCoords[1] >= 0 && baryCoords[2] >= 0)
{
// Segment goes through the triangle
return 0;
}
}
}
// At this point the segment is nearest point to one of the triangle edges or one of the end points
Eigen::Matrix<T, 3, 1, MOpt> segColPt01, segColPt02, segColPt12, triColPt01, triColPt02, triColPt12;
T dst01 = distanceSegmentSegment(sv0, sv1, tv0, tv1, &segColPt01, &triColPt01);
T dst02 = distanceSegmentSegment(sv0, sv1, tv0, tv2, &segColPt02, &triColPt02);
T dst12 = distanceSegmentSegment(sv0, sv1, tv1, tv2, &segColPt12, &triColPt12);
Eigen::Matrix<T, 3, 1, MOpt> ptTriCol0, ptTriCol1;
T dstPtTri0 = std::abs(distancePointPlane(sv0, n, d, &ptTriCol0));
barycentricCoordinates(ptTriCol0, tv0, tv1, tv2, normal, &baryCoords);
if (baryCoords[0] < 0 || baryCoords[1] < 0 || baryCoords[2] < 0)
{
dstPtTri0 = std::numeric_limits<T>::max();
}
T dstPtTri1 = std::abs(distancePointPlane(sv1, n, d, &ptTriCol1));
barycentricCoordinates(ptTriCol1, tv0, tv1, tv2, normal, &baryCoords);
if (baryCoords[0] < 0 || baryCoords[1] < 0 || baryCoords[2] < 0)
{
dstPtTri1 = std::numeric_limits<T>::max();
}
int minIndex;
Eigen::Matrix<double, 5, 1> distances;
(distances << dst01, dst02, dst12, dstPtTri0, dstPtTri1).finished().minCoeff(&minIndex);
switch (minIndex)
{
case 0:
*segmentPoint = segColPt01;
*trianglePoint = triColPt01;
return dst01;
case 1:
*segmentPoint = segColPt02;
*trianglePoint = triColPt02;
return dst02;
case 2:
*segmentPoint = segColPt12;
*trianglePoint = triColPt12;
return dst12;
case 3:
*segmentPoint = sv0;
*trianglePoint = ptTriCol0;
return dstPtTri0;
case 4:
*segmentPoint = sv1;
*trianglePoint = ptTriCol1;
return dstPtTri1;
}
// Invalid ...
return std::numeric_limits<T>::quiet_NaN();
}
template <class T, int MOpt> inline
T distanceTriangleTriangle(
const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
Eigen::Matrix<T, 3, 1, MOpt>* closestPoint0,
Eigen::Matrix<T, 3, 1, MOpt>* closestPoint1)
{
// Check the segments of t0 against t1
T minDst = std::numeric_limits<T>::max();
T currDst = 0;
Eigen::Matrix<T, 3, 1, MOpt> segPt, triPt;
Eigen::Matrix<T, 3, 1, MOpt> n0 = (t0v1 - t0v0).cross(t0v2 - t0v0);
n0.normalize();
Eigen::Matrix<T, 3, 1, MOpt> n1 = (t1v1 - t1v0).cross(t1v2 - t1v0);
n1.normalize();
currDst = distanceSegmentTriangle(t0v0, t0v1, t1v0, t1v1, t1v2, n1, &segPt, &triPt);
if (currDst < minDst)
{
minDst = currDst;
*closestPoint0 = segPt;
*closestPoint1 = triPt;
}
currDst = distanceSegmentTriangle(t0v1, t0v2, t1v0, t1v1, t1v2, n1, &segPt, &triPt);
if (currDst < minDst)
{
minDst = currDst;
*closestPoint0 = segPt;
*closestPoint1 = triPt;
}
currDst = distanceSegmentTriangle(t0v2, t0v0, t1v0, t1v1, t1v2, n1, &segPt, &triPt);
if (currDst < minDst)
{
minDst = currDst;
*closestPoint0 = segPt;
*closestPoint1 = triPt;
}
// Check the segments of t1 against t0
currDst = distanceSegmentTriangle(t1v0, t1v1, t0v0, t0v1, t0v2, n0, &segPt, &triPt);
if (currDst < minDst)
{
minDst = currDst;
*closestPoint1 = segPt;
*closestPoint0 = triPt;
}
currDst = distanceSegmentTriangle(t1v1, t1v2, t0v0, t0v1, t0v2, n0, &segPt, &triPt);
if (currDst < minDst)
{
minDst = currDst;
*closestPoint1 = segPt;
*closestPoint0 = triPt;
}
currDst = distanceSegmentTriangle(t1v2, t1v0, t0v0, t0v1, t0v2, n0, &segPt, &triPt);
if (currDst < minDst)
{
minDst = currDst;
*closestPoint1 = segPt;
*closestPoint0 = triPt;
}
return (minDst);
}
template <class T, int MOpt>
void intersectionsSegmentBox(
const Eigen::Matrix<T, 3, 1, MOpt>& sv0,
const Eigen::Matrix<T, 3, 1, MOpt>& sv1,
const Eigen::AlignedBox<T, 3>& box,
std::vector<Eigen::Matrix<T, 3, 1, MOpt>>* intersections)
{
Eigen::Array<T, 3, 1, MOpt> v01 = sv1 - sv0;
Eigen::Array<bool, 3, 1, MOpt> parallelToPlane = (v01.cwiseAbs().array() < Geometry::DistanceEpsilon);
if (parallelToPlane.any())
{
Eigen::Array<bool, 3, 1, MOpt> beyondMinCorner = (sv0.array() < box.min().array());
Eigen::Array<bool, 3, 1, MOpt> beyondMaxCorner = (sv0.array() > box.max().array());
if ((parallelToPlane && (beyondMinCorner || beyondMaxCorner)).any())
{
return;
}
}
// Calculate the intersection of the segment with each of the 6 box planes.
// The intersection is calculated as the distance along the segment (abscissa)
// scaled from 0 to 1.
Eigen::Array<T, 3, 2, MOpt> planeIntersectionAbscissas;
planeIntersectionAbscissas.col(0) = (box.min().array() - sv0.array());
planeIntersectionAbscissas.col(1) = (box.max().array() - sv0.array());
// While we could be dividing by zero here, INF values are
// correctly handled by the rest of the function.
planeIntersectionAbscissas.colwise() /= v01;
T entranceAbscissa = planeIntersectionAbscissas.rowwise().minCoeff().maxCoeff();
T exitAbscissa = planeIntersectionAbscissas.rowwise().maxCoeff().minCoeff();
if (entranceAbscissa < exitAbscissa && exitAbscissa > T(0.0))
{
if (entranceAbscissa >= T(0.0) && entranceAbscissa <= T(1.0))
{
intersections->push_back(sv0 + v01.matrix() * entranceAbscissa);
}
if (exitAbscissa >= T(0.0) && exitAbscissa <= T(1.0))
{
intersections->push_back(sv0 + v01.matrix() * exitAbscissa);
}
}
}
template <class T, int MOpt>
bool doesIntersectBoxCapsule(
const Eigen::Matrix<T, 3, 1, MOpt>& capsuleBottom,
const Eigen::Matrix<T, 3, 1, MOpt>& capsuleTop,
const T capsuleRadius,
const Eigen::AlignedBox<T, 3>& box)
{
Eigen::AlignedBox<double, 3> dilatedBox(box.min().array() - capsuleRadius, box.max().array() + capsuleRadius);
std::vector<Vector3d> candidates;
intersectionsSegmentBox(capsuleBottom, capsuleTop, dilatedBox, &candidates);
if (dilatedBox.contains(capsuleBottom))
{
candidates.push_back(capsuleBottom);
}
if (dilatedBox.contains(capsuleTop))
{
candidates.push_back(capsuleTop);
}
bool doesIntersect = false;
ptrdiff_t dimensionsOutsideBox;
Vector3d clampedPosition, segmentPoint;
for (auto candidate = candidates.cbegin(); candidate != candidates.cend(); ++candidate)
{
// Collisions between a capsule and a box are the same as a segment and a dilated
// box with rounded corners. If the intersection occurs outside the original box
// in two dimensions (collision with an edge of the dilated box) or three
// dimensions (collision with the corner of the dilated box) dimensions, we need
// to check if it is inside these rounded corners.
dimensionsOutsideBox = (candidate->array() > box.max().array()).count();
dimensionsOutsideBox += (candidate->array() < box.min().array()).count();
if (dimensionsOutsideBox >= 2)
{
clampedPosition = (*candidate).array().min(box.max().array()).max(box.min().array());
if (distancePointSegment(clampedPosition, capsuleBottom, capsuleTop, &segmentPoint) > capsuleRadius)
{
// Doesn't intersect, try the next candidate.
continue;
}
}
doesIntersect = true;
break;
}
return doesIntersect;
}
template <class T, int VOpt>
Eigen::Matrix<T, 3, 1, VOpt> nearestPointOnLine(const Eigen::Matrix<T, 3, 1, VOpt>& point,
const Eigen::Matrix<T, 3, 1, VOpt>& segment0, const Eigen::Matrix<T, 3, 1, VOpt>& segment1)
{
auto pointToSegmentStart = segment0 - point;
auto segmentDirection = segment1 - segment0;
auto squaredNorm = segmentDirection.squaredNorm();
SURGSIM_ASSERT(squaredNorm != 0.0) << "Line is defined by two collocated points.";
auto distance = -pointToSegmentStart.dot(segmentDirection) / squaredNorm;
auto p0Proj = segment0 + distance * segmentDirection;
return p0Proj;
}
template <class T, int MOpt> inline
bool doesIntersectTriangleTriangle(
const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
const Eigen::Matrix<T, 3, 1, MOpt>& t0n,
const Eigen::Matrix<T, 3, 1, MOpt>& t1n);
template <class T, int MOpt> inline
bool doesIntersectTriangleTriangle(
const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
const Eigen::Matrix<T, 3, 1, MOpt>& t1v2);
template <class T, int MOpt> inline
bool calculateContactTriangleTriangle(
const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
const Eigen::Matrix<T, 3, 1, MOpt>& t0n,
const Eigen::Matrix<T, 3, 1, MOpt>& t1n,
T* penetrationDepth,
Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
Eigen::Matrix<T, 3, 1, MOpt>* contactNormal);
template <class T, int MOpt> inline
bool calculateContactTriangleTriangle(
const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
T* penetrationDepth,
Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
Eigen::Matrix<T, 3, 1, MOpt>* contactNormal);
template <class T, int MOpt> inline
bool calculateContactTriangleTriangleSeparatingAxis(
const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
const Eigen::Matrix<T, 3, 1, MOpt>& t0n,
const Eigen::Matrix<T, 3, 1, MOpt>& t1n,
T* penetrationDepth,
Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
Eigen::Matrix<T, 3, 1, MOpt>* contactNormal);
template <class T, int MOpt> inline
bool calculateContactTriangleTriangleSeparatingAxis(
const Eigen::Matrix<T, 3, 1, MOpt>& t0v0,
const Eigen::Matrix<T, 3, 1, MOpt>& t0v1,
const Eigen::Matrix<T, 3, 1, MOpt>& t0v2,
const Eigen::Matrix<T, 3, 1, MOpt>& t1v0,
const Eigen::Matrix<T, 3, 1, MOpt>& t1v1,
const Eigen::Matrix<T, 3, 1, MOpt>& t1v2,
T* penetrationDepth,
Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint0,
Eigen::Matrix<T, 3, 1, MOpt>* penetrationPoint1,
Eigen::Matrix<T, 3, 1, MOpt>* contactNormal);
template <class T, int MOpt> inline
bool calculateContactTriangleCapsule(
const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
const Eigen::Matrix<T, 3, 1, MOpt>& tn,
const Eigen::Matrix<T, 3, 1, MOpt>& cv0,
const Eigen::Matrix<T, 3, 1, MOpt>& cv1,
double cr,
T* penetrationDepth,
Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointTriangle,
Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointCapsule,
Eigen::Matrix<T, 3, 1, MOpt>* contactNormal,
Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointCapsuleAxis);
template <class T, int MOpt> inline
bool calculateContactTriangleCapsule(
const Eigen::Matrix<T, 3, 1, MOpt>& tv0,
const Eigen::Matrix<T, 3, 1, MOpt>& tv1,
const Eigen::Matrix<T, 3, 1, MOpt>& tv2,
const Eigen::Matrix<T, 3, 1, MOpt>& cv0,
const Eigen::Matrix<T, 3, 1, MOpt>& cv1,
double cr,
T* penetrationDepth,
Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointTriangle,
Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointCapsule,
Eigen::Matrix<T, 3, 1, MOpt>* contactNormal,
Eigen::Matrix<T, 3, 1, MOpt>* penetrationPointCapsuleAxis);
template <class T, int MOpt>
int timesOfCoplanarityInRange01(
const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& A,
const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& B,
const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& C,
const std::pair<Eigen::Matrix<T, 3, 1, MOpt>, Eigen::Matrix<T, 3, 1, MOpt>>& D,
std::array<T, 3>* timesOfCoplanarity)
{
Eigen::Matrix<T, 3, 1, MOpt> AB0 = B.first - A.first;
Eigen::Matrix<T, 3, 1, MOpt> AC0 = C.first - A.first;
Eigen::Matrix<T, 3, 1, MOpt> CD0 = D.first - C.first;
Eigen::Matrix<T, 3, 1, MOpt> VA = (A.second - A.first);
Eigen::Matrix<T, 3, 1, MOpt> VC = (C.second - C.first);
Eigen::Matrix<T, 3, 1, MOpt> VAB = (B.second - B.first) - VA;
Eigen::Matrix<T, 3, 1, MOpt> VAC = VC - VA;
Eigen::Matrix<T, 3, 1, MOpt> VCD = (D.second - D.first) - VC;
T a0 = AB0.cross(CD0).dot(AC0);
T a1 = AB0.cross(CD0).dot(VAC) + (AB0.cross(VCD) + VAB.cross(CD0)).dot(AC0);
T a2 = (AB0.cross(VCD) + VAB.cross(CD0)).dot(VAC) + VAB.cross(VCD).dot(AC0);
T a3 = VAB.cross(VCD).dot(VAC);
return findRootsInRange01(Polynomial<T, 3>(a0, a1, a2, a3), timesOfCoplanarity);
}
}; // namespace Math
}; // namespace SurgSim
#include "SurgSim/Math/PointTriangleCcdContactCalculation-inl.h"
#include "SurgSim/Math/SegmentSegmentCcdContactCalculation-inl.h"
#include "SurgSim/Math/TriangleCapsuleContactCalculation-inl.h"
#include "SurgSim/Math/TriangleTriangleIntersection-inl.h"
#include "SurgSim/Math/TriangleTriangleContactCalculation-inl.h"
#endif
SurgSim::Math::distanceTrianglePlane
T distanceTrianglePlane(const Eigen::Matrix< T, 3, 1, MOpt > &tv0, const Eigen::Matrix< T, 3, 1, MOpt > &tv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv2, const Eigen::Matrix< T, 3, 1, MOpt > &n, T d, Eigen::Matrix< T, 3, 1, MOpt > *closestPointTriangle, Eigen::Matrix< T, 3, 1, MOpt > *planeProjectionPoint)
Calculate the distance of a triangle to a plane.
Definition: Geometry.h:1160
SurgSim
Definition: CompoundShapeToGraphics.cpp:29
SurgSim::Math::calculateContactTriangleCapsule
bool calculateContactTriangleCapsule(const Eigen::Matrix< T, 3, 1, MOpt > &tv0, const Eigen::Matrix< T, 3, 1, MOpt > &tv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv2, const Eigen::Matrix< T, 3, 1, MOpt > &tn, const Eigen::Matrix< T, 3, 1, MOpt > &cv0, const Eigen::Matrix< T, 3, 1, MOpt > &cv1, double cr, T *penetrationDepth, Eigen::Matrix< T, 3, 1, MOpt > *penetrationPointTriangle, Eigen::Matrix< T, 3, 1, MOpt > *penetrationPointCapsule, Eigen::Matrix< T, 3, 1, MOpt > *contactNormal, Eigen::Matrix< T, 3, 1, MOpt > *penetrationPointCapsuleAxis)
Calculate the contact between a capsule and a triangle.
Definition: TriangleCapsuleContactCalculation-inl.h:521
SurgSim::Math::isPointOnTriangleEdge
bool isPointOnTriangleEdge(const Eigen::Matrix< T, 3, 1, MOpt > &pt, const Eigen::Matrix< T, 3, 1, MOpt > &tv0, const Eigen::Matrix< T, 3, 1, MOpt > &tv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv2, const Eigen::Matrix< T, 3, 1, MOpt > &tn)
Check if a point is on the edge of a triangle.
Definition: Geometry.h:206
SurgSim::Math::distancePointTriangle
T distancePointTriangle(const Eigen::Matrix< T, 3, 1, MOpt > &pt, const Eigen::Matrix< T, 3, 1, MOpt > &tv0, const Eigen::Matrix< T, 3, 1, MOpt > &tv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv2, Eigen::Matrix< T, 3, 1, MOpt > *result)
Calculate the normal distance of a point from a triangle, the resulting point will be on the edge of ...
Definition: Geometry.h:752
TriangleTriangleIntersection-inl.h
SurgSim::Math::distancePointSegment
T distancePointSegment(const Eigen::Matrix< T, 3, 1, MOpt > &pt, const Eigen::Matrix< T, 3, 1, MOpt > &sv0, const Eigen::Matrix< T, 3, 1, MOpt > &sv1, Eigen::Matrix< T, 3, 1, MOpt > *result)
Point segment distance, if the projection of the closest point is not within the segments, the closest segment point is used for the distance calculation.
Definition: Geometry.h:297
SurgSim::Math::distanceSegmentTriangle
T distanceSegmentTriangle(const Eigen::Matrix< T, 3, 1, MOpt > &sv0, const Eigen::Matrix< T, 3, 1, MOpt > &sv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv0, const Eigen::Matrix< T, 3, 1, MOpt > &tv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv2, Eigen::Matrix< T, 3, 1, MOpt > *segmentPoint, Eigen::Matrix< T, 3, 1, MOpt > *trianglePoint)
Calculate the distance of a line segment to a triangle.
Definition: Geometry.h:1280
SurgSim::Math::findRootsInRange01
int findRootsInRange01(const Polynomial< T, 3 > &p, std::array< T, 3 > *roots)
Find all roots in range of a cubic equation.
Definition: CubicSolver-inl.h:35
SurgSim::Math::doesIntersectPlanePlane
bool doesIntersectPlanePlane(const Eigen::Matrix< T, 3, 1, MOpt > &pn0, T pd0, const Eigen::Matrix< T, 3, 1, MOpt > &pn1, T pd1, Eigen::Matrix< T, 3, 1, MOpt > *pt0, Eigen::Matrix< T, 3, 1, MOpt > *pt1)
Test if two planes are intersecting, if yes also calculate the intersection line. ...
Definition: Geometry.h:1244
SurgSim::Math::nearestPointOnLine
Eigen::Matrix< T, 3, 1, VOpt > nearestPointOnLine(const Eigen::Matrix< T, 3, 1, VOpt > &point, const Eigen::Matrix< T, 3, 1, VOpt > &segment0, const Eigen::Matrix< T, 3, 1, VOpt > &segment1)
Helper method to determine the nearest point between a point and a line.
Definition: Geometry.h:1601
SurgSim::Math::intersectionsSegmentBox
void intersectionsSegmentBox(const Eigen::Matrix< T, 3, 1, MOpt > &sv0, const Eigen::Matrix< T, 3, 1, MOpt > &sv1, const Eigen::AlignedBox< T, 3 > &box, std::vector< Eigen::Matrix< T, 3, 1, MOpt >> *intersections)
Calculate the intersections between a line segment and an axis aligned box.
Definition: Geometry.h:1494
SURGSIM_ASSERT
#define SURGSIM_ASSERT(condition)
Assert that condition is true.
Definition: Assert.h:77
SurgSim::Math::distanceTriangleTriangle
T distanceTriangleTriangle(const Eigen::Matrix< T, 3, 1, MOpt > &t0v0, const Eigen::Matrix< T, 3, 1, MOpt > &t0v1, const Eigen::Matrix< T, 3, 1, MOpt > &t0v2, const Eigen::Matrix< T, 3, 1, MOpt > &t1v0, const Eigen::Matrix< T, 3, 1, MOpt > &t1v1, const Eigen::Matrix< T, 3, 1, MOpt > &t1v2, Eigen::Matrix< T, 3, 1, MOpt > *closestPoint0, Eigen::Matrix< T, 3, 1, MOpt > *closestPoint1)
Calculate the distance between two triangles.
Definition: Geometry.h:1423
PointTriangleCcdContactCalculation-inl.h
SurgSim::Math::distancePointPlane
T distancePointPlane(const Eigen::Matrix< T, 3, 1, MOpt > &pt, const Eigen::Matrix< T, 3, 1, MOpt > &n, T d, Eigen::Matrix< T, 3, 1, MOpt > *result)
Calculate the distance of a point to a plane.
Definition: Geometry.h:1072
Log.h
The convenience header that provides the entirety of the logging API.
SurgSim::Math::doesIntersectBoxCapsule
bool doesIntersectBoxCapsule(const Eigen::Matrix< T, 3, 1, MOpt > &capsuleBottom, const Eigen::Matrix< T, 3, 1, MOpt > &capsuleTop, const T capsuleRadius, const Eigen::AlignedBox< T, 3 > &box)
Test if an axis aligned box intersects with a capsule.
Definition: Geometry.h:1548
SurgSim::Math::Polynomial< T, 3 >
Polynomial<T, 3> specializes the Polynomial class for degree 3 (cubic polynomials) ...
Definition: Polynomial.h:255
SurgSim::Math::distancePointLine
T distancePointLine(const Eigen::Matrix< T, 3, 1, MOpt > &pt, const Eigen::Matrix< T, 3, 1, MOpt > &v0, const Eigen::Matrix< T, 3, 1, MOpt > &v1, Eigen::Matrix< T, 3, 1, MOpt > *result)
Calculate the normal distance between a point and a line.
Definition: Geometry.h:266
Polynomial.h
SurgSim::Math::distanceSegmentSegment
T distanceSegmentSegment(const Eigen::Matrix< T, 3, 1, MOpt > &s0v0, const Eigen::Matrix< T, 3, 1, MOpt > &s0v1, const Eigen::Matrix< T, 3, 1, MOpt > &s1v0, const Eigen::Matrix< T, 3, 1, MOpt > &s1v1, Eigen::Matrix< T, 3, 1, MOpt > *pt0, Eigen::Matrix< T, 3, 1, MOpt > *pt1, T *s0t=nullptr, T *s1t=nullptr)
Distance between two segments, if the project of the closest point is not on the opposing segment...
Definition: Geometry.h:404
SurgSim::Math::calculateContactTriangleTriangleSeparatingAxis
bool calculateContactTriangleTriangleSeparatingAxis(const Eigen::Matrix< T, 3, 1, MOpt > &t0v0, const Eigen::Matrix< T, 3, 1, MOpt > &t0v1, const Eigen::Matrix< T, 3, 1, MOpt > &t0v2, const Eigen::Matrix< T, 3, 1, MOpt > &t1v0, const Eigen::Matrix< T, 3, 1, MOpt > &t1v1, const Eigen::Matrix< T, 3, 1, MOpt > &t1v2, const Eigen::Matrix< T, 3, 1, MOpt > &t0n, const Eigen::Matrix< T, 3, 1, MOpt > &t1n, T *penetrationDepth, Eigen::Matrix< T, 3, 1, MOpt > *penetrationPoint0, Eigen::Matrix< T, 3, 1, MOpt > *penetrationPoint1, Eigen::Matrix< T, 3, 1, MOpt > *contactNormal)
Calculate the contact between two triangles.
Definition: TriangleTriangleContactCalculation-inl.h:302
SurgSim::Math::isCoplanar
bool isCoplanar(const Eigen::Matrix< T, 3, 1, MOpt > &a, const Eigen::Matrix< T, 3, 1, MOpt > &b, const Eigen::Matrix< T, 3, 1, MOpt > &c, const Eigen::Matrix< T, 3, 1, MOpt > &d)
Check whether the points are coplanar.
Definition: Geometry.h:251
SurgSim::Math::timesOfCoplanarityInRange01
int timesOfCoplanarityInRange01(const std::pair< Eigen::Matrix< T, 3, 1, MOpt >, Eigen::Matrix< T, 3, 1, MOpt >> &A, const std::pair< Eigen::Matrix< T, 3, 1, MOpt >, Eigen::Matrix< T, 3, 1, MOpt >> &B, const std::pair< Eigen::Matrix< T, 3, 1, MOpt >, Eigen::Matrix< T, 3, 1, MOpt >> &C, const std::pair< Eigen::Matrix< T, 3, 1, MOpt >, Eigen::Matrix< T, 3, 1, MOpt >> &D, std::array< T, 3 > *timesOfCoplanarity)
Test when 4 points are coplanar in the range [0..1] given their linear motion.
Definition: Geometry.h:1840
SurgSim::Math::distanceSegmentPlane
T distanceSegmentPlane(const Eigen::Matrix< T, 3, 1, MOpt > &sv0, const Eigen::Matrix< T, 3, 1, MOpt > &sv1, const Eigen::Matrix< T, 3, 1, MOpt > &n, T d, Eigen::Matrix< T, 3, 1, MOpt > *closestPointSegment, Eigen::Matrix< T, 3, 1, MOpt > *planeIntersectionPoint)
Calculate the distance between a segment and a plane.
Definition: Geometry.h:1099
SegmentSegmentCcdContactCalculation-inl.h
SurgSim::Math::barycentricCoordinates
bool barycentricCoordinates(const Eigen::Matrix< T, 3, 1, MOpt > &pt, const Eigen::Matrix< T, 3, 1, MOpt > &sv0, const Eigen::Matrix< T, 3, 1, MOpt > &sv1, Eigen::Matrix< T, 2, 1, MOpt > *coordinates)
Calculate the barycentric coordinates of a point with respect to a line segment.
Definition: Geometry.h:71
SurgSim::Math::distanceLineLine
T distanceLineLine(const Eigen::Matrix< T, 3, 1, MOpt > &l0v0, const Eigen::Matrix< T, 3, 1, MOpt > &l0v1, const Eigen::Matrix< T, 3, 1, MOpt > &l1v0, const Eigen::Matrix< T, 3, 1, MOpt > &l1v1, Eigen::Matrix< T, 3, 1, MOpt > *pt0, Eigen::Matrix< T, 3, 1, MOpt > *pt1)
Determine the distance between two lines.
Definition: Geometry.h:337
Vector.h
Definitions of small fixed-size vector types.
TriangleTriangleContactCalculation-inl.h
SurgSim::Math::calculateContactTriangleTriangle
bool calculateContactTriangleTriangle(const Eigen::Matrix< T, 3, 1, MOpt > &t0v0, const Eigen::Matrix< T, 3, 1, MOpt > &t0v1, const Eigen::Matrix< T, 3, 1, MOpt > &t0v2, const Eigen::Matrix< T, 3, 1, MOpt > &t1v0, const Eigen::Matrix< T, 3, 1, MOpt > &t1v1, const Eigen::Matrix< T, 3, 1, MOpt > &t1v2, const Eigen::Matrix< T, 3, 1, MOpt > &t0n, const Eigen::Matrix< T, 3, 1, MOpt > &t1n, T *penetrationDepth, Eigen::Matrix< T, 3, 1, MOpt > *penetrationPoint0, Eigen::Matrix< T, 3, 1, MOpt > *penetrationPoint1, Eigen::Matrix< T, 3, 1, MOpt > *contactNormal)
Calculate the contact between two triangles.
Definition: TriangleTriangleContactCalculation-inl.h:216
SurgSim::Math::isPointInsideTriangle
bool isPointInsideTriangle(const Eigen::Matrix< T, 3, 1, MOpt > &pt, const Eigen::Matrix< T, 3, 1, MOpt > &tv0, const Eigen::Matrix< T, 3, 1, MOpt > &tv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv2, const Eigen::Matrix< T, 3, 1, MOpt > &tn)
Check if a point is inside a triangle.
Definition: Geometry.h:159
SurgSim::Math::doesCollideSegmentTriangle
bool doesCollideSegmentTriangle(const Eigen::Matrix< T, 3, 1, MOpt > &sv0, const Eigen::Matrix< T, 3, 1, MOpt > &sv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv0, const Eigen::Matrix< T, 3, 1, MOpt > &tv1, const Eigen::Matrix< T, 3, 1, MOpt > &tv2, const Eigen::Matrix< T, 3, 1, MOpt > &tn, Eigen::Matrix< T, 3, 1, MOpt > *result)
Calculate the intersection of a line segment with a triangle See http://geomalgorithms.com/a06-_intersect-2.html#intersect_RayTriangle for the algorithm.
Definition: Geometry.h:973
SurgSim::Math::doesIntersectTriangleTriangle
bool doesIntersectTriangleTriangle(const Eigen::Matrix< T, 3, 1, MOpt > &t0v0, const Eigen::Matrix< T, 3, 1, MOpt > &t0v1, const Eigen::Matrix< T, 3, 1, MOpt > &t0v2, const Eigen::Matrix< T, 3, 1, MOpt > &t1v0, const Eigen::Matrix< T, 3, 1, MOpt > &t1v1, const Eigen::Matrix< T, 3, 1, MOpt > &t1v2, const Eigen::Matrix< T, 3, 1, MOpt > &t0n, const Eigen::Matrix< T, 3, 1, MOpt > &t1n)
Check if the two triangles intersect using separating axis test.
Definition: TriangleTriangleIntersection-inl.h:70
SurgSim::Math::Vector3d
Eigen::Matrix< double, 3, 1 > Vector3d
A 3D vector of doubles.
Definition: Vector.h:57
TriangleCapsuleContactCalculation-inl.h