Geometry.h File Reference

a collection of functions that calculation geometric properties of various basic geometric shapes. More...

`#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"`
`#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"`

Go to the source code of this file.

## Namespaces

| SurgSim
| SurgSim::Math
| SurgSim::Math::Geometry

## Functions

template<class T , int MOpt>
bool | SurgSim::Math::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. More...
template<class T , int MOpt>
bool | SurgSim::Math::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)
| Calculate the barycentric coordinates of a point with respect to a triangle. More...
template<class T , int MOpt>
bool | SurgSim::Math::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)
| Calculate the barycentric coordinates of a point with respect to a triangle. More...
template<class T , int MOpt>
bool | SurgSim::Math::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. More...
template<class T , int MOpt>
bool | SurgSim::Math::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)
| Check if a point is inside a triangle. More...
template<class T , int MOpt>
bool | SurgSim::Math::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. More...
template<class T , int MOpt>
bool | SurgSim::Math::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)
| Check if a point is on the edge of a triangle. More...
template<class T , int MOpt>
bool | SurgSim::Math::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. More...
template<class T , int MOpt>
T | SurgSim::Math::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. More...
template<class T , int MOpt>
T | SurgSim::Math::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. More...
template<class T , int MOpt>
T | SurgSim::Math::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. More...
template<class T , int MOpt>
T | SurgSim::Math::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, the segment endpoints will be used for the distance calculation. More...
template<class T , int MOpt>
T | SurgSim::Math::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 the triangle if the projection of the point is not inside the triangle. More...
template<class T , int MOpt>
bool | SurgSim::Math::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. More...
template<class T , int MOpt>
T | SurgSim::Math::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. More...
template<class T , int MOpt>
T | SurgSim::Math::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. More...
template<class T , int MOpt>
T | SurgSim::Math::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. More...
template<class T , int MOpt>
bool | SurgSim::Math::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. More...
template<class T , int MOpt>
T | SurgSim::Math::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. More...
template<class T , int MOpt>
T | SurgSim::Math::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)
| Calculate the distance of a line segment to a triangle. More...
template<class T , int MOpt>

T

SurgSim::Math::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. More...
template<class T , int MOpt>
void | SurgSim::Math::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. More...
template<class T , int MOpt>
bool | SurgSim::Math::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. More...
template<class T , int VOpt>
Eigen::Matrix< T, 3, 1, VOpt > | SurgSim::Math::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. More...
template<class T , int MOpt>

bool

SurgSim::Math::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. More...
template<class T , int MOpt>
bool | SurgSim::Math::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)
| Check if the two triangles intersect using separating axis test. More...
template<class T , int MOpt>

bool

SurgSim::Math::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. More...
template<class T , int MOpt>

bool

SurgSim::Math::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)

| Calculate the contact between two triangles. More...
template<class T , int MOpt>

bool

SurgSim::Math::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. More...
template<class T , int MOpt>

bool

SurgSim::Math::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)

| Calculate the contact between two triangles. More...
template<class T , int MOpt>

bool

SurgSim::Math::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. More...
template<class T , int MOpt>

bool

SurgSim::Math::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)

| Calculate the contact between a capsule and a triangle. More...
template<class T , int MOpt>

int

SurgSim::Math::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. More...

## Detailed Description

a collection of functions that calculation geometric properties of various basic geometric shapes.

Point, LineSegment, Plane, Triangle. All functions are templated for the accuracy of the calculation (float/double). There are also three kinds of epsilon defined that are used on a case by case basis. In general all function here will return a floating point or boolean value and take a series of output parameters. When those outputs cannot be calculated their values will be set to NAN. This functions are meant as a basic layer that will be wrapped with calls from structures mainting more state information about the primitives they are handling. As a convention we are using a plane equation in the form nx + d = 0

Note
  HS-2013-may-07 Even though some of the names in this file do not agree with the coding standards in regard to the use of verbs for functions it was determined that other phrasing would not necessarily improve the readability or expressiveness of the function names.