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
	Wraps glewInit() to separate the glew opengl definitions from the osg opengl definitions only imgui needs glew but we need to call glewInit() from a osg callback, using this call we avoid getting warnings about redefinitions.

Functions
template<class T , int MOpt>
bool	SurgSim::Math::doesIntersectSegmentSegment (const Eigen::Matrix< T, 2, 1, MOpt > &s0v0, const Eigen::Matrix< T, 2, 1, MOpt > &s0v1, const Eigen::Matrix< T, 2, 1, MOpt > &s1v0, const Eigen::Matrix< T, 2, 1, MOpt > &s1v1, T *s, T *t)
	Calculate the intersection between the two 2D segments. 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 > &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 projected into the plane of a triangle, is inside that 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 projected into the plane of a triangle, is inside that 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>
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>
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::distanceLinePlane (const Eigen::Matrix< T, 3, 1, MOpt > &v0, const Eigen::Matrix< T, 3, 1, MOpt > &v1, const Eigen::Matrix< T, 3, 1, MOpt > &n, T d, Eigen::Matrix< T, 3, 1, MOpt > *intersectionPoint)
	Calculate the distance between a line 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, 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::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::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.

Function Documentation

barycentricCoordinates() [1/3]

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  )

inline

Calculate the barycentric coordinates of a point with respect to a line segment.

Template Parameters

T	Floating point type of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

	pt	Vertex of the point.
	sv0,sv1	Vertices of the line segment.
[out]	coordinates	Barycentric coordinates.

Returns

bool true on success, false if two or more if the line segment is considered degenerate

Note

The point need not be on the line segment, in which case, the barycentric coordinate of the projection is calculated.

barycentricCoordinates() [2/3]

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  )

inline

Calculate the barycentric coordinates of a point with respect to a triangle.

Precondition

The normal must be unit length

The triangle vertices must be in counter clockwise order in respect to the normal

Template Parameters

T	Floating point type of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

	pt	Vertex of the point.
	tv0,tv1,tv2	Vertices of the triangle in counter clockwise order in respect to the normal.
	tn	Normal of the triangle (yes must be of norm 1 and a,b,c CCW).
[out]	coordinates	Barycentric coordinates.

Returns

bool true on success, false if two or more if the triangle is considered degenerate

barycentricCoordinates() [3/3]

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  )

inline

Calculate the barycentric coordinates of a point with respect to a triangle.

Please note that each time you use this call the normal of the triangle will be calculated, if you convert more than one coordinate against this triangle, precalculate the normal and use the other version of this function

Template Parameters

T	Floating point type of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

	pt	Vertex of the point.
	tv0,tv1,tv2	Vertices of the triangle.
[out]	coordinates	The Barycentric coordinates.

Returns

bool true on success, false if the triangle is considered degenerate

calculateContactTriangleCapsule() [1/2]

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  )

inline

Calculate the contact between a capsule and a triangle.

If the shapes intersect, the deepest penetration of the capsule along the triangle normal is calculated.

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

	tv0,tv1,tv2	Vertices of the triangle.
	tn	Normal of the triangle, should be normalized.
	cv0,cv1	Ends of the capsule axis.
	cr	Capsule radius.
[out]	penetrationDepth	The depth of penetration.
[out]	penetrationPointTriangle	The contact point on triangle.
[out]	penetrationPointCapsule	The contact point on capsule.
[out]	contactNormal	The contact normal that points from capsule to triangle.
[out]	penetrationPointCapsuleAxis	The point on the capsule axis closest to the triangle.

Returns

True, if intersection is detected.

Note

The [out] params are not modified if there is no intersection.

If penetrationPointTriangle is moved by (contactNormal*penetrationDepth*0.5) and penetrationPointCapsule is moved by -(contactNormal*penetrationDepth*0.5), the shapes will no longer be intersecting.

calculateContactTriangleCapsule() [2/2]

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  )

inline

Calculate the contact between a capsule and a triangle.

If the shapes intersect, the deepest penetration of the capsule along the triangle normal is calculated.

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

	tv0,tv1,tv2	Vertices of the triangle.
	cv0,cv1	Ends of the capsule axis.
	cr	Capsule radius.
[out]	penetrationDepth	The depth of penetration.
[out]	penetrationPointTriangle	The contact point on triangle.
[out]	penetrationPointCapsule	The contact point on capsule.
[out]	contactNormal	The contact normal that points from capsule to triangle.
[out]	penetrationPointCapsuleAxis	The point on the capsule axis closest to the triangle.

Returns

True, if intersection is detected.

Note

The [out] params are not modified if there is no intersection.

If penetrationPointTriangle is moved by (contactNormal*penetrationDepth*0.5) and penetrationPointCapsule is moved by -(contactNormal*penetrationDepth*0.5), the shapes will no longer be intersecting.

calculateContactTriangleTriangle() [1/2]

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  )

inline

Calculate the contact between two triangles.

Algorithm presented in https://docs.google.com/a/simquest.com/document/d/11ajMD7QoTVelT2_szGPpeUEY0wHKKxW1TOgMe8k5Fsc/pub. If the triangle are known to intersect, the deepest penetration of the triangles into each other is calculated. The triangle which penetrates less into the other triangle is chosen as contact.

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

	t0v0,t0v1,t0v2	Vertices of the first triangle.
	t1v0,t1v1,t1v2	Vertices of the second triangle.
	t0n	Unit length normal of the first triangle, should be normalized.
	t1n	Unit length normal of the second triangle, should be normalized.
[out]	penetrationDepth	The depth of penetration.
[out]	penetrationPoint0	The contact point on triangle0 (t0v0,t0v1,t0v2).
[out]	penetrationPoint1	The contact point on triangle1 (t1v0,t1v1,t1v2).
[out]	contactNormal	The contact normal that points from triangle1 to triangle0.

Returns

True, if intersection is detected.

Note

The [out] params are not modified if there is no intersection.

If penetrationPoint0 is moved by (contactNormal*penetrationDepth*0.5) and penetrationPoint1 is moved by -(contactNormal*penetrationDepth*0.5), the triangles will no longer be intersecting.

calculateContactTriangleTriangle() [2/2]

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  )

inline

Calculate the contact between two triangles.

Algorithm presented in https://docs.google.com/a/simquest.com/document/d/11ajMD7QoTVelT2_szGPpeUEY0wHKKxW1TOgMe8k5Fsc/pub. If the triangle are known to intersect, the deepest penetration of the triangles into each other is calculated. The triangle which penetrates less into the other triangle is chosen as contact.

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

	t0v0,t0v1,t0v2	Vertices of the first triangle, should be normalized.
	t1v0,t1v1,t1v2	Vertices of the second triangle, should be normalized.
[out]	penetrationDepth	The depth of penetration.
[out]	penetrationPoint0	The contact point on triangle0 (t0v0,t0v1,t0v2).
[out]	penetrationPoint1	The contact point on triangle1 (t1v0,t1v1,t1v2).
[out]	contactNormal	The contact normal that points from triangle1 to triangle0.

Returns

True, if intersection is detected.

Note

The [out] params are not modified if there is no intersection.

If penetrationPoint0 is moved by (contactNormal*penetrationDepth*0.5) and penetrationPoint1 is moved by -(contactNormal*penetrationDepth*0.5), the triangles will no longer be intersecting.

calculateContactTriangleTriangleSeparatingAxis() [1/2]

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  )

inline

Calculate the contact between two triangles.

Algorithm is implemented from http://fileadmin.cs.lth.se/cs/Personal/Tomas_Akenine-Moller/pubs/tritri.pdf

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

	t0v0,t0v1,t0v2	Vertices of the first triangle.
	t1v0,t1v1,t1v2	Vertices of the second triangle.
	t0n	Normal of the first triangle, should be normalized.
	t1n	Normal of the second triangle, should be normalized.
[out]	penetrationDepth	The depth of penetration.
[out]	penetrationPoint0	The contact point on triangle0 (t0v0,t0v1,t0v2).
[out]	penetrationPoint1	The contact point on triangle1 (t1v0,t1v1,t1v2).
[out]	contactNormal	The contact normal that points from triangle1 to triangle0.

Returns

True, if intersection is detected.

Note

The [out] params are not modified if there is no intersection.

If penetrationPoint0 is moved by (contactNormal*penetrationDepth*0.5) and penetrationPoint1 is moved by -(contactNormal*penetrationDepth*0.5), the triangles will no longer be intersecting.

calculateContactTriangleTriangleSeparatingAxis() [2/2]

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  )

inline

Calculate the contact between two triangles.

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

	t0v0,t0v1,t0v2	Vertices of the first triangle.
	t1v0,t1v1,t1v2	Vertices of the second triangle.
[out]	penetrationDepth	The depth of penetration.
[out]	penetrationPoint0	The contact point on triangle0 (t0v0,t0v1,t0v2).
[out]	penetrationPoint1	The contact point on triangle1 (t1v0,t1v1,t1v2).
[out]	contactNormal	The contact normal that points from triangle1 to triangle0.

Returns

True, if intersection is detected.

Note

The [out] params are not modified if there is no intersection.

If penetrationPoint0 is moved by (contactNormal*penetrationDepth*0.5) and penetrationPoint1 is moved by -(contactNormal*penetrationDepth*0.5), the triangles will no longer be intersecting.

distanceLineLine()

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  )

inline

Determine the distance between two lines.

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

	l0v0,l0v1	Points on Line 0.
	l1v0,l1v1	Points on Line 1.
[out]	pt0	The closest point on line 0.
[out]	pt1	The closest point on line 1.

Returns

The normal distance between the two given lines i.e. (pt0 - pt1).norm()

Note

We are using distancePointSegment for the degenerate cases as opposed to distancePointLine, why is that ??? (HS-2013-apr-26)

distanceLinePlane()

template<class T , int MOpt>

T SurgSim::Math::distanceLinePlane ( const Eigen::Matrix< T, 3, 1, MOpt > &  v0, const Eigen::Matrix< T, 3, 1, MOpt > &  v1, const Eigen::Matrix< T, 3, 1, MOpt > &  n, T  d, Eigen::Matrix< T, 3, 1, MOpt > *  intersectionPoint  )

inline

Calculate the distance between a line and a plane.

Precondition

n should be normalized

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

	v0,v1	Two points on the line.
	n	Normal of the plane n (normalized).
	d	Constant d in n.x + d = 0.
[out]	intersectionPoint	Point of intersection if any. If multiple, then v0.

Returns

The distance between the line and plane if they do not intersect, otherwise 0.

distancePointLine()

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  )

inline

Calculate the normal distance between a point and a line.

Parameters

	pt	The input point.
	v0,v1	Two vertices on the line.
[out]	result	The point projected onto the line.

Returns

The normal distance between the point and the line

distancePointPlane()

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  )

inline

Calculate the distance of a point to a plane.

Precondition

n needs to the normalized

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

	pt	The point to check.
	n	The normal of the plane n (normalized).
	d	Constant d for the plane equation as in n.x + d = 0.
[out]	result	Projection of point p into the plane.

Returns

The distance to the plane (negative if on the backside of the plane).

distancePointSegment()

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  )

inline

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.

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

	pt	The input point
	sv0,sv1	The segment extremities.
[out]	result	Either the projection onto the segment or one of the 2 vertices.

Returns

The distance of the point from the segment.

distancePointTriangle()

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  )

inline

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.

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

	pt	The point that is being measured.
	tv0,tv1,tv2	The vertices of the triangle.
[out]	result	The point on the triangle that is closest to pt, if the projection of pt onto the triangle. plane is not inside the triangle the closest point on the edge will be used.

Returns

The distance between the point and the triangle, i.e (result - pt).norm()

distanceSegmentPlane()

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  )

inline

Calculate the distance between a segment and a plane.

Precondition

n should be normalized

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

	sv0,sv1	Endpoints of the segments.
	n	Normal of the plane n (normalized).
	d	Constant d in n.x + d = 0.
[out]	closestPointSegment	Point closest to the plane, the midpoint of the segment (v0+v1)/2 is being used if the segment is parallel to the plane. If the segment actually intersects the plane segmentIntersectionPoint will be equal to planeIntersectionPoint.
[out]	planeIntersectionPoint	the point on the plane where the line defined by the segment intersects the plane.

Returns

the distance of closest point of the segment to the plane, 0 if the segment intersects the plane, negative if the closest point is on the other side of the plane.

distanceSegmentSegment()

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.

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

	s0v0,s0v1	Segment 0 Extremities.
	s1v0,s1v1	Segment 1 Extremities.
[out]	pt0	Closest point on segment 0
[out]	pt1	Closest point on segment 1
[out]	s0t	Abscissa at the point of intersection on Segment 0 (s0v0 + t * (s0v1 - s0v0)).
[out]	s1t	Abscissa at the point of intersection on Segment 0 (s1v0 + t * (s1v1 - s1v0)).

Returns

Distance between the segments, i.e. (pt0 - pt1).norm()

distanceSegmentTriangle() [1/2]

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  )

inline

Calculate the distance of a line segment to a triangle.

Precondition

n needs to be normalized.

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

	sv0,sv1	Extremities of the line segment.
	tv0,tv1,tv2	Points of the triangle.
	normal	Normal of the triangle (Expected to be normalized)
[out]	segmentPoint	Closest point on the segment.
[out]	trianglePoint	Closest point on the triangle.

Returns

the distance between the two closest points.

distanceSegmentTriangle() [2/2]

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  )

inline

Calculate the distance of a line segment to a triangle.

Note that this version will calculate the normal of the triangle, if the normal is known use the other version of this function.

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

	sv0,sv1	Extremities of the line segment.
	tv0,tv1,tv2	Triangle points.
[out]	segmentPoint	Closest point on the segment.
[out]	trianglePoint	Closest point on the triangle.

Returns

the the distance between the two closest points, i.e. (trianglePoint - segmentPoint).norm().

distanceTrianglePlane()

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  )

inline

Calculate the distance of a triangle to a plane.

Precondition

n should be normalized.

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

tv0,tv1,tv2	Points of the triangle.
n	Normal of the plane n (normalized).
d	Constant d in n.x + d = 0.
closestPointTriangle	Closest point on the triangle, when the triangle is coplanar to the plane (tv0+tv1+tv2)/3 is used, when the triangle intersects the plane the midpoint of the intersection segment is returned.
planeProjectionPoint	Projection of the closest point onto the plane, when the triangle intersects the plane the midpoint of the intersection segment is returned.

Returns

The distance of the triangle to the plane.

distanceTriangleTriangle()

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  )

inline

Calculate the distance between two triangles.

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

	t0v0,t0v1,t0v2	Points of the first triangle.
	t1v0,t1v1,t1v2	Points of the second triangle.
[out]	closestPoint0	Closest point on the first triangle, unless penetrating, in which case it is the point along the edge that allows min separation.
[out]	closestPoint1	Closest point on the second triangle, unless penetrating, in which case it is the point along the edge that allows min separation.

Returns

the distance between the two triangles.

doesCollideSegmentTriangle()

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  )

inline

Calculate the intersection of a line segment with a triangle See http://geomalgorithms.com/a06-_intersect-2.html#intersect_RayTriangle for the algorithm.

Precondition

The normal must be unit length

The triangle vertices must be in counter clockwise order in respect to the normal

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

	sv0,sv1	Extremities of the segment.
	tv0,tv1,tv2	The triangle vertices. CCW around the normal.
	tn	The triangle normal, should be normalized.
[out]	result	The point where the triangle and the line segment intersect, invalid if they don't intersect.

Returns

true if the segment intersects with the triangle, false if it does not

Note

HS-2013-may-07 This is the only function that only checks for intersection rather than returning a distance if necessary this should be rewritten to do the distance calculation, doing so would necessitate to check against all the triangle edges in the non intersection cases.

doesIntersectBoxCapsule()

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.

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

capsuleBottom	Position of the capsule bottom
capsuleTop	Position of the capsule top
capsuleRadius	The capsule radius
box	Axis aligned bounding box

Returns

True, if intersection is detected.

doesIntersectPlanePlane()

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  )

inline

Test if two planes are intersecting, if yes also calculate the intersection line.

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

	pn0,pd0	Normal and constant of the first plane, nx + d = 0.
	pn1,pd1	Normal and constant of the second plane, nx + d = 0.
[out]	pt0,pt1	Two points on the intersection line, not valid if there is no intersection.

Returns

true when a unique line exists, false for disjoint or coinciding.

doesIntersectSegmentSegment()

template<class T , int MOpt>

bool SurgSim::Math::doesIntersectSegmentSegment ( const Eigen::Matrix< T, 2, 1, MOpt > &  s0v0, const Eigen::Matrix< T, 2, 1, MOpt > &  s0v1, const Eigen::Matrix< T, 2, 1, MOpt > &  s1v0, const Eigen::Matrix< T, 2, 1, MOpt > &  s1v1, T *  s, T *  t  )

inline

Calculate the intersection between the two 2D segments.

Template Parameters

T	Floating point type of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

s0v0,s0v1	First segment extremities
s1v0,s1v1	Second segment extremities
s,t	[out] if intersection exists, contains distance on segment where intersection is

doesIntersectTriangleTriangle() [1/2]

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  )

inline

Check if the two triangles intersect using separating axis test.

Algorithm is implemented from http://fileadmin.cs.lth.se/cs/Personal/Tomas_Akenine-Moller/pubs/tritri.pdf

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

t0v0,t0v1,t0v2	Vertices of the first triangle.
t1v0,t1v1,t1v2	Vertices of the second triangle.
t0n	Normal of the first triangle, should be normalized.
t1n	Normal of the second triangle, should be normalized.

Returns

True, if intersection is detected.

doesIntersectTriangleTriangle() [2/2]

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  )

inline

Check if the two triangles intersect using separating axis test.

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

t0v0,t0v1,t0v2	Vertices of the first triangle.
t1v0,t1v1,t1v2	Vertices of the second triangle.

Returns

True, if intersection is detected.

intersectionsSegmentBox()

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.

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

	sv0,sv1	Extremities of the line segment.
	box	Axis aligned bounding box
[out]	intersections	The points of intersection between the segment and the box

isCoplanar()

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  )

inline

Check whether the points are coplanar.

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

a,b,c,d

Points to check for coplanarity.

Returns

true if the points are coplanar.

isPointInsideTriangle() [1/2]

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  )

inline

Check if a point projected into the plane of a triangle, is inside that triangle.

Note

Use barycentricCoordinates() if you need the coordinates

Precondition

The normal must be unit length

The triangle vertices must be in counter clockwise order in respect to the normal

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

pt	Vertex of the point.
tv0,tv1,tv2	Vertices of the triangle, must be in CCW.
tn	Normal of the triangle (yes must be of norm 1 and a,b,c CCW).

Returns

true if pt lies inside the triangle tv0, tv1, tv2, false otherwise.

isPointInsideTriangle() [2/2]

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  )

inline

Check if a point projected into the plane of a triangle, is inside that triangle.

Note

Use barycentricCoordinates() if you need the coordinates. Please note that the normal will be calculated each time you use this call, if you are doing more than one test with the same triangle, precalculate the normal and pass it. Into the other version of this function

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

pt	Vertex of the point.
tv0,tv1,tv2	Vertices of the triangle, must be in CCW.

Returns

true if pt lies inside the triangle tv0, tv1, tv2, false otherwise.

isPointOnTriangleEdge()

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  )

inline

Check if a point is on the edge of a triangle.

Template Parameters

T	Accuracy of the calculation, can usually be inferred.
MOpt	Eigen Matrix options, can usually be inferred.

Parameters

pt	Vertex of the point.
tv0,tv1,tv2	Vertices of the triangle.

Returns

true if pt lies on the edge of the triangle tv0, tv1, tv2, false otherwise.

nearestPointOnLine()

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.

Template Parameters

T	the numeric data type used for the vector argument. Can usually be deduced.
VOpt	the option flags (alignment etc.) used for the vector argument. Can be deduced.

Parameters

point	is the point under consideration.
segment0	one point on the line
segment1	second point on the line

Returns

the closest point on the line through the segment to the point under test

timesOfCoplanarityInRange01()

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.

Template Parameters

T	The scalar type
MOpt	The matrix options

Parameters

	A,B,C,D	the 4 point' motion (each has a pair from -> to)
[out]	timesOfCoplanarity	The normalized times (in [0..1]) at which the 4 points are coplanar

Returns

The number of times the 4 points are coplanar throughout their motion in [0..1]

Let's define the following: A(t) = A0 + t * VA with VA = A1 - A0 Similarily for B(t), C(t) and D(t) Therefore we have AB(t) = B(t) - A(t) = B(0) + t * VB - A(0) - t * VA = AB(0) + t * [VB - VA] = AB(0) + t * VAB

The 4 points ABCD are coplanar are time t if they verify: [AB(t).cross(CD(t))].AC(t) = 0 We develop this equation to clearly formulate the resulting cubic equation:

[AB(0).cross(CD(0)) + t*AB(0).cross(VCD) + t*VAB.cross(CD(0)) + t^2*VAB.cross(VCD)] . [AC(0) + t * VAC] = 0 t^0 * [[AB(0).cross(CD(0))].AC(0)] + t^1 * [[AB(0).cross(CD(0))].VAC + [AB(0).cross(VCD)].AC(0) + [VAB.cross(CD(0))].AC(0)] + t^2 * [[AB(0).cross(VCD)].VAC + [VAB.cross(CD(0))].VAC + [VAB.cross(VCD)].AC(0)] + t^3 * [[VAB.cross(VCD)].VAC] = 0