Namespaces | Classes | Typedefs | Enumerations | Functions | Variables
SurgSim::Math Namespace Reference

Namespaces

 CardinalSplines
 
 Geometry
 
 TriangleCapsuleContactCalculation
 

Classes

class  BoxShape
 Box shape: box centered on (0 0 0), aligned with the axis with different sizes along X, Y and Z. More...
 
class  CapsuleShape
 Capsule shape: centered on (0, 0, 0), aligned along Y, with length and radius. More...
 
class  clampOperator
 define a custom template unary functor to execute the clamp operation over an Eigen matrix structure. More...
 
class  CompoundShape
 
class  CylinderShape
 Cylinder shape: centered on (0 0 0), aligned along Y, defined with length and radius. More...
 
class  DoubleSidedPlaneShape
 DoubleSidedPlaneShape: The XZ plane (d = 0) with normal pointing along positive Y axis. More...
 
struct  gaussQuadraturePoint
 1D Gauss-Legendre quadrature More...
 
struct  gaussQuadratureTrianglePoint
 2D Gauss-Legendre quadrature on a triangle More...
 
class  Interval
 Interval defines the concept of a mathematical interval and provides operations on it including arithmetic operations, construction, and IO. More...
 
class  IntervalND
 IntervalND defines the concept of a group of mathematical intervals and provides operations on them including arithmetic operations, construction, and IO. More...
 
class  IntervalND< T, 3 >
 IntervalND<T,3> defines the concept of a group of mathematical intervals specialized to 3 intervals and provides operations on them including arithmetic operations, construction, and IO. More...
 
class  KalmanFilter
 Implements a linear Kalman filter, a recursive estimator. More...
 
class  LinearMotion
 LinearMotion is (intentionally) a lot like Interval, but it deals with linear motion where all operands start and end their motion simultaneously. More...
 
class  LinearMotionND
 LinearMotionND<T, N> defines the concept of a group of linear motions and provides operations on them including arithmetic operations, construction, and I/O. More...
 
class  LinearMotionND< T, 3 >
 LinearMotionND<T, 3> specializes the LinearMotionND<T, N> class for 3 dimensions. More...
 
class  LinearSolveAndInverse
 LinearSolveAndInverse aims at performing an efficient linear system resolution and calculating its inverse matrix at the same time. More...
 
class  LinearSolveAndInverseDenseMatrix
 Derivation for dense matrix type. More...
 
class  LinearSolveAndInverseDiagonalMatrix
 Derivation for diagonal matrix type. More...
 
class  LinearSolveAndInverseSymmetricTriDiagonalBlockMatrix
 Derivation for symmetric tri-diagonal block matrix type. More...
 
class  LinearSolveAndInverseTriDiagonalBlockMatrix
 Derivation for tri-diagonal block matrix type. More...
 
class  LinearSparseSolveAndInverse
 LinearSparseSolveAndInverse aims at performing an efficient linear system resolution and calculating its inverse matrix at the same time. More...
 
class  LinearSparseSolveAndInverseCG
 Derivation for sparse CG solver. More...
 
class  LinearSparseSolveAndInverseLU
 Derivation for sparse LU solver. More...
 
class  MeshShape
 Mesh shape: shape made of a triangle mesh The triangle mesh needs to be watertight to produce valid volume, center and second moment of volume. More...
 
class  MlcpGaussSeidelSolver
 A solver for mixed LCP problems using the Gauss-Seidel iterative method. More...
 
struct  MlcpProblem
 A description of an MLCP (mixed linear complementarity problem, or mixed LCP) system to be solved. More...
 
struct  MlcpSolution
 The description of a solution to a mixed linear complementarity problem. More...
 
class  MlcpSolver
 This class provides a solver interface for mixed linear complementarity problems. More...
 
class  OctreeShape
 Octree Shape A defined by an octree data structure. More...
 
class  OdeEquation
 Ode equation of 2nd order of the form \(M(x,v).a = F(x, v)\) with \((x0, v0)\) for initial conditions and a set of boundary conditions. More...
 
class  OdeSolver
 Base class for all solvers of ode equation of order 2 of the form \(M(x(t), v(t)).a(t) = f(t, x(t), v(t))\). More...
 
class  OdeSolverEulerExplicit
 Euler Explicit ode solver solves the following \(2^{nd}\) order ode \(M(x(t), v(t)).a(t) = f(t, x(t), v(t))\). More...
 
class  OdeSolverEulerExplicitModified
 Modified Euler Explicit ode solver solves the following \(2^{nd}\) order ode \(M(x(t), v(t)).a(t) = f(t, x(t), v(t))\). More...
 
class  OdeSolverEulerImplicit
 Euler implicit (a.k.a backward Euler) ode solver (see OdeSolverEulerImplicit.dox for more details). More...
 
class  OdeSolverLinearEulerExplicit
 Linear Version of the Euler Explicit ode solver This solver assumes that the system is linear, ie that Mass, Damping, and Stiffness matrices do not change. More...
 
class  OdeSolverLinearEulerExplicitModified
 Linear Version of the Modified Euler Explicit ode solver This solver assumes that the system is linear, ie that Mass, Damping, and Stiffness matrices do not change. More...
 
class  OdeSolverLinearEulerImplicit
 Linear Version of the Euler Implicit ode solver This solver assumes that the system is linear, ie that Mass, Damping, and Stiffness matrices do not change. More...
 
class  OdeSolverLinearRungeKutta4
 Linear Version of the Runge Kutta 4 ode solver This solver assumes that the system is linear ie that Mass, Damping, and Stiffness matrices do not change. More...
 
class  OdeSolverLinearStatic
 Linear version of the static ode solver This solver assumes that the system is linear, ie that Stiffness matrix does not change. More...
 
class  OdeSolverRungeKutta4
 Runge Kutta 4 ode solver (See http://en.wikipedia.org/wiki/Runge%E2%80%93Kutta_methods) solves the following \(2^{nd}\) order ode \(M(x(t), v(t)).a(t) = f(t, x(t), v(t))\). More...
 
class  OdeSolverStatic
 Static ode solver solves the following \(2^{nd}\) order ode \(M(x(t), v(t)).a(t) = f(t, x(t), v(t))\). More...
 
class  OdeState
 The state \(y\) of an ode of 2nd order of the form \(M(x,v).a = F(x, v)\) with boundary conditions. More...
 
class  Operation
 Helper class to run operation a column/row of a matrix to a chunk of memory where the size is dynamically defined. More...
 
class  Operation< DerivedSub, SparseType, Eigen::ColMajor >
 Specialization for column major storage. More...
 
class  Operation< DerivedSub, SparseType, Eigen::RowMajor >
 Specialization for row major storage. More...
 
class  ParticlesShape
 Particles Shape: A shape consisting of a group of particles of equal radius. More...
 
class  PlaneShape
 The XZ plane (d = 0) with normal pointing along positive Y axis. More...
 
class  Polynomial
 Polynomial<T, N> defines the concept of an N degree polynomial with type T coefficients and provides operations on them including arithmetic operations, construction, and IO. More...
 
class  Polynomial< T, 0 >
 Polynomial<T, 0> specializes the Polynomial class for degree 0 (constant polynomials) More...
 
class  Polynomial< T, 1 >
 Polynomial<T, 1> specializes the Polynomial class for degree 1 (linear polynomials) More...
 
class  Polynomial< T, 2 >
 Polynomial<T, 2> specializes the Polynomial class for degree 2 (quadratic polynomials) More...
 
class  Polynomial< T, 3 >
 Polynomial<T, 3> specializes the Polynomial class for degree 3 (cubic polynomials) More...
 
class  PolynomialRoots
 The (algebraic) roots of a Polynomial<N,T>. More...
 
class  PolynomialRoots< T, 1 >
 PolynomialRoots<T, 1> specializes the PolynomialRoots class for degree 1 (linear polynomials) More...
 
class  PolynomialRoots< T, 2 >
 PolynomialRoots<T, 2> specializes the PolynomialRoots class for degree 2 (quadratic polynomials) More...
 
class  PolynomialRootsCommon
 The common base class for PolynomialRoots specializations for various N. More...
 
class  PolynomialValues
 Class to manage polynomial based calculations of interval boundaries. More...
 
class  PolynomialValues< T, 0 >
 PolynomialValues<T, 0> specializes the PolynomialValues class for degree 0 (constant polynomials) More...
 
class  PolynomialValues< T, 1 >
 PolynomialValues<T, 1> specializes the PolynomialValues class for degree 1 (linear polynomials) More...
 
class  PolynomialValues< T, 2 >
 PolynomialValues<T, 2> specializes the PolynomialValues class for degree 2 (quadratic polynomials) More...
 
class  PolynomialValues< T, 3 >
 PolynomialValues<T, 3> specializes the PolynomialValues class for degree 3 (cubic polynomials) More...
 
struct  PosedShape
 PosedShape is a transformed shape with a record of the pose used to transform it. More...
 
struct  PosedShapeMotion
 PosedShapeMotion is embedding the motion of a PosedShape, providing a posed shape at 2 different instant. More...
 
class  SegmentMeshShape
 SegmentMeshShape defines a shape based on a mesh, like MeshShape. More...
 
class  SegmentMeshShapePlyReaderDelegate
 Implementation of ply reader for segment meshes, enable to read the radius from the ply file. More...
 
class  Shape
 Generic rigid shape class defining a shape. More...
 
class  SphereShape
 Sphere shape: sphere centered on (0 0 0), defined with radius. More...
 
class  SurfaceMeshShape
 SurfaceMeshShape defines a shape based on a mesh, like MeshShape. More...
 
class  TriangleHelper
 A helper class for a triangle, used for the following two purposes: More...
 

Typedefs

typedef Eigen::AlignedBox< float, 3 > Aabbf
 Wrapper around the Eigen type. More...
 
typedef Eigen::AlignedBox< double, 3 > Aabbd
 Wrapper around the Eigen type. More...
 
typedef Eigen::Matrix< float, 2, 2, Eigen::RowMajor > Matrix22f
 A 2x2 matrix of floats. More...
 
typedef Eigen::Matrix< float, 3, 3, Eigen::RowMajor > Matrix33f
 A 3x3 matrix of floats. More...
 
typedef Eigen::Matrix< float, 4, 4, Eigen::RowMajor > Matrix44f
 A 4x4 matrix of floats. More...
 
typedef Eigen::Matrix< double, 2, 2, Eigen::RowMajor > Matrix22d
 A 2x2 matrix of doubles. More...
 
typedef Eigen::Matrix< double, 3, 3, Eigen::RowMajor > Matrix33d
 A 3x3 matrix of doubles. More...
 
typedef Eigen::Matrix< double, 4, 4, Eigen::RowMajor > Matrix44d
 A 4x4 matrix of doubles. More...
 
typedef Eigen::Matrix< double, 6, 6, Eigen::RowMajor > Matrix66d
 A 6x6 matrix of doubles. More...
 
typedef Eigen::DiagonalMatrix< double, Eigen::Dynamic > DiagonalMatrix
 A dynamic size diagonal matrix. More...
 
typedef Eigen::Matrix< double, Eigen::Dynamic, Eigen::Dynamic > Matrix
 A dynamic size matrix. More...
 
typedef Eigen::Quaternion< float > Quaternionf
 A quaternion of floats. More...
 
typedef Eigen::Quaternion< double > Quaterniond
 A quaternion of doubles. More...
 
typedef Eigen::Transform< float, 2, Eigen::Isometry > RigidTransform2f
 A 2D rigid (isometric) transform, represented as floats. More...
 
typedef Eigen::Transform< float, 3, Eigen::Isometry > RigidTransform3f
 A 3D rigid (isometric) transform, represented as floats. More...
 
typedef Eigen::Transform< double, 2, Eigen::Isometry > RigidTransform2d
 A 2D rigid (isometric) transform, represented as doubles. More...
 
typedef Eigen::Transform< double, 3, Eigen::Isometry > RigidTransform3d
 A 3D rigid (isometric) transform, represented as doubles. More...
 
typedef Eigen::SparseMatrix< double > SparseMatrix
 A sparse matrix. More...
 
typedef Eigen::Matrix< float, 2, 1 > Vector2f
 A 2D vector of floats. More...
 
typedef Eigen::Matrix< float, 3, 1 > Vector3f
 A 3D vector of floats. More...
 
typedef Eigen::Matrix< float, 4, 1 > Vector4f
 A 4D vector of floats. More...
 
typedef Eigen::Matrix< float, 6, 1 > Vector6f
 A 6D vector of floats. More...
 
typedef Eigen::Matrix< double, 2, 1 > Vector2d
 A 2D vector of doubles. More...
 
typedef Eigen::Matrix< double, 3, 1 > Vector3d
 A 3D vector of doubles. More...
 
typedef Eigen::Matrix< double, 4, 1 > Vector4d
 A 4D vector of doubles. More...
 
typedef Eigen::Matrix< double, 6, 1 > Vector6d
 A 6D matrix of doubles. More...
 
typedef Eigen::Matrix< double, Eigen::Dynamic, 1 > Vector
 A dynamic size column vector. More...
 

Enumerations

enum  LinearSolver { LINEARSOLVER_LU = 0, LINEARSOLVER_CONJUGATEGRADIENT, MAX_LINEARSOLVER }
 The linear numerical integration scheme supported Each Linear Solver should have its own entry in this enum. More...
 
enum  MlcpConstraintType {
  MLCP_INVALID_CONSTRAINT = -1, MLCP_BILATERAL_1D_CONSTRAINT = 0, MLCP_BILATERAL_2D_CONSTRAINT, MLCP_BILATERAL_3D_CONSTRAINT,
  MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT, MLCP_UNILATERAL_3D_FRICTIONAL_CONSTRAINT, MLCP_BILATERAL_FRICTIONLESS_SLIDING_CONSTRAINT, MLCP_BILATERAL_FRICTIONAL_SLIDING_CONSTRAINT,
  MLCP_NUM_CONSTRAINT_TYPES
}
 
enum  OdeEquationUpdate {
  ODEEQUATIONUPDATE_F = 1<<0, ODEEQUATIONUPDATE_M = 1<<1, ODEEQUATIONUPDATE_D = 1<<2, ODEEQUATIONUPDATE_K = 1<<3,
  ODEEQUATIONUPDATE_FMDK = ODEEQUATIONUPDATE_F | ODEEQUATIONUPDATE_M | ODEEQUATIONUPDATE_D | ODEEQUATIONUPDATE_K
}
 Enum to identify which of the data need to be updated by the OdeEquation::update() More...
 
enum  IntegrationScheme {
  INTEGRATIONSCHEME_STATIC = 0, INTEGRATIONSCHEME_LINEAR_STATIC, INTEGRATIONSCHEME_EULER_EXPLICIT, INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT,
  INTEGRATIONSCHEME_EULER_EXPLICIT_MODIFIED, INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT_MODIFIED, INTEGRATIONSCHEME_EULER_IMPLICIT, INTEGRATIONSCHEME_LINEAR_EULER_IMPLICIT,
  INTEGRATIONSCHEME_RUNGE_KUTTA_4, INTEGRATIONSCHEME_LINEAR_RUNGE_KUTTA_4, MAX_INTEGRATIONSCHEMES
}
 The diverse numerical integration scheme supported Each Ode Solver should have its own entry in this enum. More...
 
enum  ShapeDirection { SHAPE_DIRECTION_AXIS_X = 0, SHAPE_DIRECTION_AXIS_Y = 1, SHAPE_DIRECTION_AXIS_Z = 2 }
 Type defining the shape direction for certain templatized shape (i.e. More...
 
enum  ShapeType {
  SHAPE_TYPE_NONE = -1, SHAPE_TYPE_BOX, SHAPE_TYPE_CAPSULE, SHAPE_TYPE_CYLINDER,
  SHAPE_TYPE_DOUBLESIDEDPLANE, SHAPE_TYPE_MESH, SHAPE_TYPE_OCTREE, SHAPE_TYPE_PARTICLES,
  SHAPE_TYPE_PLANE, SHAPE_TYPE_SPHERE, SHAPE_TYPE_SURFACEMESH, SHAPE_TYPE_SEGMENTMESH,
  SHAPE_TYPE_COMPOUNDSHAPE, SHAPE_TYPE_COUNT
}
 Fixed List of enums for the available Shape types, do not explicitly assign values, ShapeCount is used to determine the number of actual shape types. More...
 

Functions

template<class Scalar , int Dim>
bool doAabbIntersect (const Eigen::AlignedBox< Scalar, Dim > &aabb0, const Eigen::AlignedBox< Scalar, Dim > &aabb1, double tolerance)
 Determine whether two AABBs have an intersection with each other, for the calculation see http://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?page=3. More...
 
template<class Scalar , int Dim>
bool doAabbIntersect (const Eigen::AlignedBox< Scalar, Dim > &a, const Eigen::AlignedBox< Scalar, Dim > &b)
 Determine whether two AABBs overlap, using a minimal set of eigen calls, does not take a tolerance. More...
 
template<class Scalar , int Dim, int MType>
Eigen::AlignedBox< Scalar, Dim > makeAabb (const Eigen::Matrix< Scalar, Dim, 1, MType > &vector0, const Eigen::Matrix< Scalar, Dim, 1, MType > &vector1, const Eigen::Matrix< Scalar, Dim, 1, MType > &vector2)
 Convenience function for creating a bounding box from three vertices (e.g. More...
 
template<class Scalar , int Dim>
Eigen::AlignedBox< Scalar, Dim > transformAabb (const Eigen::Transform< Scalar, Dim, Eigen::Isometry > &transform, const Eigen::AlignedBox< Scalar, Dim > &aabb)
 Rotate the extrema of the aabb, note that that will extend the size of the box. More...
 
template<class T >
int findRootsInRange01 (const Polynomial< T, 3 > &polynomial, std::array< T, 3 > *roots)
 Find all roots in range \([0 \ldotp\ldotp 1]\) of a cubic equation. More...
 
template<class T , int MOpt>
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. More...
 
template<class T , int MOpt>
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)
 Calculate the barycentric coordinates of a point with respect to a triangle. More...
 
template<class T , int MOpt>
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)
 Calculate the barycentric coordinates of a point with respect to a triangle. More...
 
template<class T , int MOpt>
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. More...
 
template<class T , int MOpt>
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)
 Check if a point is inside a triangle. More...
 
template<class T , int MOpt>
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. More...
 
template<class T , int MOpt>
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)
 Check if a point is on the edge of a triangle. More...
 
template<class T , int MOpt>
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. More...
 
template<class T , int MOpt>
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>
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>
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>
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>
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 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>
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>
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>
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 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>
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>
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>
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 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 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 > 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 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 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 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 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 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 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 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 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 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...
 
template<typename T >
Interval< T > operator+ (T v, const Interval< T > &i)
 
template<typename T >
Interval< T > operator* (T v, const Interval< T > &i)
 
template<class T >
void IntervalArithmetic_add (const Interval< T > &a, const Interval< T > &b, Interval< T > *res)
 
template<class T >
void IntervalArithmetic_addadd (const Interval< T > &a, const Interval< T > &b, Interval< T > *res)
 
template<class T >
void IntervalArithmetic_sub (const Interval< T > &a, const Interval< T > &b, Interval< T > *res)
 
template<class T >
void IntervalArithmetic_addsub (const Interval< T > &a, const Interval< T > &b, Interval< T > *res)
 
template<class T >
void IntervalArithmetic_mul (const Interval< T > &a, const Interval< T > &b, Interval< T > *res)
 
template<class T >
void IntervalArithmetic_addmul (const Interval< T > &a, const Interval< T > &b, Interval< T > *res)
 
template<class T >
void IntervalArithmetic_submul (const Interval< T > &a, const Interval< T > &b, Interval< T > *res)
 
template<typename T >
std::ostream & operator<< (std::ostream &o, const Interval< T > &interval)
 Write a textual version of the interval to an output stream. More...
 
template<typename T , int N>
std::ostream & operator<< (std::ostream &o, const IntervalND< T, N > &interval)
 Write a textual version of an interval group to an output stream. More...
 
template<class T >
void IntervalArithmetic_add (const IntervalND< T, 3 > &a, const IntervalND< T, 3 > &b, IntervalND< T, 3 > *res)
 
template<class T >
void IntervalArithmetic_sub (const IntervalND< T, 3 > &a, const IntervalND< T, 3 > &b, IntervalND< T, 3 > *res)
 
template<class T >
void IntervalArithmetic_crossProduct (const IntervalND< T, 3 > &a, const IntervalND< T, 3 > &b, IntervalND< T, 3 > *res)
 
template<class T >
void IntervalArithmetic_dotProduct (const IntervalND< T, 3 > &a, const IntervalND< T, 3 > &b, Interval< T > *res)
 
template<class P >
void IntervalArithmetic_add (const Interval< P > &a, const Interval< P > &b, Interval< P > *res)
 Calculate the sum of two intervals. More...
 
template<class P >
void IntervalArithmetic_addadd (const Interval< P > &a, const Interval< P > &b, Interval< P > *res)
 Calculate the sum of three intervals res + a + b. More...
 
template<class P >
void IntervalArithmetic_sub (const Interval< P > &a, const Interval< P > &b, Interval< P > *res)
 Calculate the difference of two intervals. More...
 
template<class P >
void IntervalArithmetic_addsub (const Interval< P > &a, const Interval< P > &b, Interval< P > *res)
 Add the difference of two intervals to an existing value. More...
 
template<class P >
void IntervalArithmetic_mul (const Interval< P > &a, const Interval< P > &b, Interval< P > *res)
 Calculate the product of two intervals. More...
 
template<class P >
void IntervalArithmetic_addmul (const Interval< P > &a, const Interval< P > &b, Interval< P > *res)
 Add the product of two intervals to an existing value. More...
 
template<class P >
void IntervalArithmetic_submul (const Interval< P > &a, const Interval< P > &b, Interval< P > *res)
 Subtract the product of two intervals from an existing value. More...
 
template<class P >
void IntervalArithmetic_add (const IntervalND< P, 3 > &a, const IntervalND< P, 3 > &b, IntervalND< P, 3 > *res)
 Calculate the sum of two interval groups. More...
 
template<class P >
void IntervalArithmetic_sub (const IntervalND< P, 3 > &a, const IntervalND< P, 3 > &b, IntervalND< P, 3 > *res)
 Calculate the difference of two interval groups. More...
 
template<class P >
void IntervalArithmetic_dotProduct (const IntervalND< P, 3 > &a, const IntervalND< P, 3 > &b, Interval< P > *res)
 Calculate the dot product of two interval groups. More...
 
template<class P >
void IntervalArithmetic_crossProduct (const IntervalND< P, 3 > &a, const IntervalND< P, 3 > &b, IntervalND< P, 3 > *res)
 Calculate the cross product of two interval groups. More...
 
template<typename T >
std::ostream & operator<< (std::ostream &o, const LinearMotion< T > &motion)
 Write a textual version of a linear motion to an output stream. More...
 
template<typename T , int N>
std::ostream & operator<< (std::ostream &o, const LinearMotionND< T, N > &motion)
 Write a textual version of a linear motion group to an output stream. More...
 
template<typename T >
Polynomial< T, 2 > analyticDotProduct (const LinearMotionND< T, 3 > &a, const LinearMotionND< T, 3 > &b)
 Calculate an analytic dot product as a Polynomial. More...
 
template<typename T , int A>
Polynomial< T, 2 > analyticCrossProductAxis (const LinearMotionND< T, 3 > &a, const LinearMotionND< T, 3 > &b)
 Calculate a single axis of an analytic cross product as a Polynomial. More...
 
template<typename T >
Polynomial< T, 2 > analyticCrossProductXAxis (const LinearMotionND< T, 3 > &a, const LinearMotionND< T, 3 > &b)
 Calculate the X axis of an analytic cross product as a Polynomial. More...
 
template<typename T >
Polynomial< T, 2 > analyticCrossProductYAxis (const LinearMotionND< T, 3 > &a, const LinearMotionND< T, 3 > &b)
 Calculate the Y axis of an analytic cross product as a Polynomial. More...
 
template<typename T >
Polynomial< T, 2 > analyticCrossProductZAxis (const LinearMotionND< T, 3 > &a, const LinearMotionND< T, 3 > &b)
 Calculate the Z axis of an analytic cross product as a Polynomial. More...
 
template<typename T >
void analyticCrossProduct (const LinearMotionND< T, 3 > &a, const LinearMotionND< T, 3 > &b, Polynomial< T, 2 > *resultXAxis, Polynomial< T, 2 > *resultYAxis, Polynomial< T, 2 > *resultZAxis)
 Calculate an analytic cross product as a Polynomial. More...
 
template<typename T >
Polynomial< T, 3 > analyticTripleProduct (const LinearMotionND< T, 3 > &a, const LinearMotionND< T, 3 > &b, const LinearMotionND< T, 3 > &c)
 Calculate an analytic cross product as a Polynomial, as a polynomial whose value for t=0..1 is the value of the triple product. More...
 
template<typename T >
Polynomial< T, 2 > analyticMagnitudeSquared (const LinearMotionND< T, 3 > &motion)
 Calculate the magnitude squared of a linear motion 3 group as a polynomial. More...
 
template<class T >
Interval< T > tripleProduct (const LinearMotionND< T, 3 > &a, const LinearMotionND< T, 3 > &b, const LinearMotionND< T, 3 > &c, const Interval< T > &range)
 Calculate the triple product, as an interval. More...
 
template<typename T , int VOpt>
Eigen::Matrix< T, 3, 3 > makeRotationMatrix (const T &angle, const Eigen::Matrix< T, 3, 1, VOpt > &axis)
 Create a rotation matrix corresponding to the specified angle (in radians) and axis. More...
 
template<typename T , int VOpt>
Eigen::Matrix< T, 3, 3 > makeSkewSymmetricMatrix (const Eigen::Matrix< T, 3, 1, VOpt > &vector)
 Create a skew-symmetric matrix corresponding to the specified vector. More...
 
template<typename T , int MOpt>
Eigen::Matrix< T, 3, 1 > skew (const Eigen::Matrix< T, 3, 3, MOpt > &matrix)
 Extract the unique vector from the skew-symmetric part of a given matrix. More...
 
template<typename T , int MOpt, int VOpt>
void computeAngleAndAxis (const Eigen::Matrix< T, 3, 3, MOpt > &matrix, T *angle, Eigen::Matrix< T, 3, 1, VOpt > *axis)
 Get the angle (in radians) and axis corresponding to a rotation matrix. More...
 
template<typename T , int MOpt>
computeAngle (const Eigen::Matrix< T, 3, 3, MOpt > &matrix)
 Get the angle corresponding to a quaternion's rotation, in radians. More...
 
template<class Matrix , class SubMatrix >
void addSubMatrix (const SubMatrix &subMatrix, size_t blockIdRow, size_t blockIdCol, size_t blockSizeRow, size_t blockSizeCol, Matrix *matrix)
 Helper method to add a sub-matrix into a matrix, for the sake of clarity. More...
 
template<class Matrix , class SubMatrix >
void addSubMatrix (const SubMatrix &subMatrix, const std::vector< size_t > blockIds, size_t blockSize, Matrix *matrix)
 Helper method to add a sub-matrix made of squared-blocks into a matrix, for the sake of clarity. More...
 
template<class Matrix , class SubMatrix >
void setSubMatrix (const SubMatrix &subMatrix, size_t blockIdRow, size_t blockIdCol, size_t blockSizeRow, size_t blockSizeCol, Matrix *matrix)
 Helper method to set a sub-matrix into a matrix, for the sake of clarity. More...
 
template<class Matrix >
Eigen::Block< MatrixgetSubMatrix (Matrix &matrix, size_t blockIdRow, size_t blockIdCol, size_t blockSizeRow, size_t blockSizeCol)
 Helper method to access a sub-matrix from a matrix, for the sake of clarity. More...
 
template<class Derived >
void zeroRow (size_t row, Eigen::DenseBase< Derived > *matrix)
 Helper method to zero a row of a matrix. More...
 
template<class Derived >
void zeroColumn (size_t column, Eigen::DenseBase< Derived > *matrix)
 Helper method to zero a column of a matrix. More...
 
template<class T >
void minMax (const T &a1, const T &a2, T *minVal, T *maxVal)
 Calculate the minimum and maximum of two values. More...
 
template<class T >
void minMax (const T &a1, const T &a2, const T &a3, T *minVal, T *maxVal)
 Calculate the minimum and maximum of three values. More...
 
template<class T >
void minMax (const T &a1, const T &a2, const T &a3, const T &a4, T *minVal, T *maxVal)
 Calculate the minimum and maximum of four values. More...
 
template<class T >
void minMax (const T &a1, const T &a2, const T &a3, const T &a4, const T &a5, T *minVal, T *maxVal)
 Calculate the minimum and maximum of five values. More...
 
template<class T >
void minMax (const T *values, int numValues, T *minVal, T *maxVal)
 Calculate the minimum and maximum of numValues values. More...
 
std::string getMlcpConstraintTypeName (MlcpConstraintType constraintType)
 
MlcpConstraintType getMlcpConstraintTypeValue (const std::string &typeName)
 
template<class T , int MOpt>
bool isPointInsideTriangle (T time, const std::pair< Eigen::Matrix< T, 3, 1, MOpt >, Eigen::Matrix< T, 3, 1, MOpt >> &P, 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, Eigen::Matrix< T, 3, 1, MOpt > *barycentricCoordinates)
 Check if a point belongs to a triangle at a given time of their motion. More...
 
template<class T , int MOpt>
bool calculateCcdContactPointTriangle (const std::pair< Eigen::Matrix< T, 3, 1, MOpt >, Eigen::Matrix< T, 3, 1, MOpt >> &P, 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, T *timeOfImpact, T *tv01Param, T *tv02Param)
 Continuous collision detection between a point P and a triangle ABC. More...
 
template<typename T >
bool isNearZero (const T &value, const T &epsilon=static_cast< T >(polynomial_epsilon))
 Define an utility function for comparing individual coefficients to 0. More...
 
template<typename T , int N, int M>
Polynomial< T, N+M > operator* (const Polynomial< T, N > &p, const Polynomial< T, M > &q)
 Multiply two polynomials of arbitrary degree. More...
 
template<typename T >
Polynomial< T, 2 > operator* (const Polynomial< T, 1 > &p, const Polynomial< T, 1 > &q)
 Multiply two polynomials of degree 1. More...
 
template<typename T >
Polynomial< T, 3 > operator* (const Polynomial< T, 2 > &p, const Polynomial< T, 1 > &q)
 Multiply two polynomials of degree 2 and 1 respectively. More...
 
template<typename T >
Polynomial< T, 3 > operator* (const Polynomial< T, 1 > &p, const Polynomial< T, 2 > &q)
 Multiply two polynomials of degree 1 and 2 respectively. More...
 
template<typename T >
Polynomial< T, 0 > square (const Polynomial< T, 0 > &p)
 Square a degree 0 polynomial. More...
 
template<typename T >
Polynomial< T, 2 > square (const Polynomial< T, 1 > &p)
 Square a degree 1 polynomial. More...
 
template<typename T , int N>
std::ostream & operator<< (std::ostream &stream, const Polynomial< T, N > &p)
 Write a textual version of a Polynomial to an output stream. More...
 
template<typename T , int N>
void solve (const T &a, const T &b, const T &epsilon, int *numRoots, std::array< T, N > *roots)
 Specialized solve routine for linear polynomials (2 coefficients) More...
 
template<typename T , int N>
void solve (const T &a, const T &b, const T &c, const T &epsilon, int *numRoots, std::array< T, N > *roots)
 Specialized solve routine for quadratic polynomials (3 coefficients) More...
 
template<class T , int N>
Interval< T > valuesOverInterval (const Polynomial< T, N > &p, const Interval< T > &interval)
 Calculate the minimum and maximum values of the dependent variable over a specified range of the independent variable. More...
 
template<typename T , int VOpt>
Eigen::Quaternion< T > makeRotationQuaternion (const T &angle, const Eigen::Matrix< T, 3, 1, VOpt > &axis)
 Create a quaternion rotation corresponding to the specified angle (in radians) and axis. More...
 
template<typename T , int QOpt>
Eigen::Quaternion< T, QOpt > negate (const Eigen::Quaternion< T, QOpt > &q)
 Quaternion negation (i.e. More...
 
template<typename T , int QOpt, int VOpt>
void computeAngleAndAxis (const Eigen::Quaternion< T, QOpt > &quaternion, T *angle, Eigen::Matrix< T, 3, 1, VOpt > *axis)
 Get the angle (in radians) and axis corresponding to a quaternion's rotation. More...
 
template<typename T , int QOpt>
computeAngle (const Eigen::Quaternion< T, QOpt > &quaternion)
 Get the angle corresponding to a quaternion's rotation, in radians. More...
 
template<typename T , int TOpt, int VOpt>
void computeRotationVector (const Eigen::Transform< T, 3, Eigen::Isometry, TOpt > &end, const Eigen::Transform< T, 3, Eigen::Isometry, TOpt > &start, Eigen::Matrix< T, 3, 1, VOpt > *rotationVector)
 Get the vector corresponding to the rotation between transforms. More...
 
template<typename T , int QOpt>
Eigen::Quaternion< T, QOpt > interpolate (const Eigen::Quaternion< T, QOpt > &q0, const Eigen::Quaternion< T, QOpt > &q1, T t)
 Interpolate (slerp) between 2 quaternions. More...
 
template<typename M , typename V >
Eigen::Transform< typename M::Scalar, M::RowsAtCompileTime, Eigen::Isometry > makeRigidTransform (const Eigen::MatrixBase< M > &rotation, const Eigen::MatrixBase< V > &translation)
 Create a rigid transform using the specified rotation matrix and translation. More...
 
template<typename Q , typename V >
Eigen::Transform< typename Q::Scalar, 3, Eigen::Isometry > makeRigidTransform (const Eigen::QuaternionBase< Q > &rotation, const Eigen::MatrixBase< V > &translation)
 Create a rigid transform using the specified rotation quaternion and translation. More...
 
template<typename T , int VOpt>
Eigen::Transform< T, 3, Eigen::Isometry > makeRigidTransform (const Eigen::Matrix< T, 3, 1, VOpt > &position, const Eigen::Matrix< T, 3, 1, VOpt > &center, const Eigen::Matrix< T, 3, 1, VOpt > &up)
 Make a rigid transform from a eye point a center view point and an up direction, does not check whether up is colinear with eye-center The original formula can be found at http://www.opengl.org/sdk/docs/man2/xhtml/gluLookAt.xml. More...
 
template<typename V >
Eigen::Transform< typename V::Scalar, V::SizeAtCompileTime, Eigen::Isometry > makeRigidTranslation (const Eigen::MatrixBase< V > &translation)
 Create a rigid transform using the identity rotation and the specified translation. More...
 
template<typename T , int TOpt>
Eigen::Transform< T, 3, Eigen::Isometry > interpolate (const Eigen::Transform< T, 3, Eigen::Isometry, TOpt > &t0, const Eigen::Transform< T, 3, Eigen::Isometry, TOpt > &t1, T t)
 Interpolate (slerp) between 2 rigid transformations. More...
 
template<class T >
clamp (T value, T min, T max, T epsilon)
 Clamp any values within an epsilon window to a maximum or minimum value. More...
 
template<class T , int MOpt>
bool areSegmentsIntersecting (T time, 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, Eigen::Matrix< T, 2, 1, MOpt > *barycentricCoordinates)
 Check if 2 segments intersect at a given time of their motion. More...
 
template<class T , int MOpt>
bool calculateCcdContactSegmentSegment (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, T *timeOfImpact, T *s0p1Factor, T *s1p1Factor)
 Continuous collision detection between two segments AB and CD. More...
 
template<int Opt, typename Index >
void blockWithSearch (const Eigen::Ref< const Matrix > &subMatrix, size_t rowStart, size_t columnStart, size_t n, size_t m, Eigen::SparseMatrix< double, Opt, Index > *matrix, void(Operation< Matrix, Eigen::SparseMatrix< double, Opt, Index >>::*op)(double *, size_t, size_t, size_t, const Matrix &, size_t))
 Runs a given operation on a SparseMatrix block(i, j, n, m) from a (n x m) 'subMatrix' with searching for the block 1st element. More...
 
template<int Opt, typename Index >
void blockOperation (const Eigen::Ref< const Matrix > &subMatrix, size_t rowStart, size_t columnStart, Eigen::SparseMatrix< double, Opt, Index > *matrix, void(Operation< Matrix, Eigen::SparseMatrix< double, Opt, Index >>::*op)(double *, const double &))
 Runs a given operation on a SparseMatrix block(i, j, n, m) from a (n x m) 'subMatrix' with searching for the block 1st element. More...
 
template<int Opt, typename Index >
void addSubMatrix (const Eigen::Ref< const Matrix > &subMatrix, size_t blockIdRow, size_t blockIdCol, Eigen::SparseMatrix< double, Opt, Index > *matrix, bool initialize=true)
 Helper method to add a sub-matrix into a matrix, for the sake of clarity. More...
 
template<int Opt, typename Index >
void assignSubMatrix (const Eigen::Ref< const Matrix > &subMatrix, size_t blockIdRow, size_t blockIdCol, Eigen::SparseMatrix< double, Opt, Index > *matrix, bool initialize=true)
 Helper method to assign a sub-matrix into a matrix, for the sake of clarity. More...
 
template<typename T , int Opt, typename Index >
void zeroRow (size_t row, Eigen::SparseMatrix< T, Opt, Index > *matrix)
 Helper method to zero a row of a matrix specialized for Sparse Matrices. More...
 
template<typename T , int Opt, typename Index >
void zeroColumn (size_t column, Eigen::SparseMatrix< T, Opt, Index > *matrix)
 Helper method to zero a column of a matrix specialized for Sparse Matrices. More...
 
template<typename T , int Opt, typename Index >
void clearMatrix (Eigen::SparseMatrix< T, Opt, Index > *matrix)
 Helper method to zero all entries of a matrix specialized for Sparse Matrices. More...
 
template<class T >
void edgeIntersection (T dStart, T dEnd, T pvStart, T pvEnd, T *parametricIntersection, size_t *parametricIntersectionIndex)
 Two ends of the triangle edge are given in terms of the following vertex properties. More...
 
bool isValid (float value)
 Check if a float value is valid. More...
 
bool isValid (double value)
 Check if a double value is valid. More...
 
template<typename T >
bool isValid (const Eigen::DenseBase< T > &value)
 Check if a matrix or a vector is valid. More...
 
template<typename T >
bool isValid (const Eigen::QuaternionBase< T > &value)
 Check if a quaternion is valid. More...
 
template<typename T >
bool isValid (const Eigen::AngleAxis< T > &value)
 Check if an angle/axis 3D rotation is valid. More...
 
template<typename T >
bool isValid (const Eigen::Rotation2D< T > &value)
 Check if a 2D rotation is valid. More...
 
template<typename T , int D, int M, int O>
bool isValid (const Eigen::Transform< T, D, M, O > &value)
 Check if a transform is valid. More...
 
bool isSubnormal (float value)
 Check if a float value is subnormal. More...
 
bool isSubnormal (double value)
 Check if a double value is subnormal. More...
 
template<typename T >
bool isSubnormal (const Eigen::DenseBase< T > &value)
 Check if a matrix or a vector contains any subnormal floating-point values. More...
 
template<typename T >
bool isSubnormal (const Eigen::QuaternionBase< T > &value)
 Check if a quaternion contains any subnormal floating-point values. More...
 
template<typename T >
bool isSubnormal (const Eigen::AngleAxis< T > &value)
 Check if an angle/axis 3D rotation contains any subnormal floating-point values. More...
 
template<typename T >
bool isSubnormal (const Eigen::Rotation2D< T > &value)
 Check if a 2D rotation is described by an angle that is subnormal. More...
 
template<typename T , int D, int M, int O>
bool isSubnormal (const Eigen::Transform< T, D, M, O > &value)
 Check if a transform contains any subnormal floating-point values. More...
 
bool setSubnormalToZero (float *value)
 If the float value is subnormal, set it to zero. More...
 
bool setSubnormalToZero (double *value)
 If the double value is subnormal, set it to zero. More...
 
template<typename T >
bool setSubnormalToZero (Eigen::DenseBase< T > *value)
 Set all subnormal values in a matrix or a vector to zero. More...
 
template<typename T >
bool setSubnormalToZero (Eigen::QuaternionBase< T > *value)
 Set all subnormal values in a quaternion to zero. More...
 
template<typename T >
bool setSubnormalToZero (Eigen::AngleAxis< T > *value)
 Set all subnormal values in an angle/axis 3D rotation to zero. More...
 
template<typename T >
bool setSubnormalToZero (Eigen::Rotation2D< T > *value)
 If the angle of a 2D rotation is subnormal, set it to zero. More...
 
template<typename T , int D, int M, int O>
bool setSubnormalToZero (Eigen::Transform< T, D, M, O > *value)
 Set all subnormal values in a transform to zero. More...
 
template<class Vector , class SubVector >
void addSubVector (const SubVector &subVector, size_t blockId, size_t blockSize, Vector *vector)
 Helper method to add a sub-vector into a vector, for the sake of clarity. More...
 
template<class Vector , class SubVector >
void addSubVector (const SubVector &subVector, const std::vector< size_t > blockIds, size_t blockSize, Vector *vector)
 Helper method to add a sub-vector per block into a vector, for the sake of clarity. More...
 
template<class Vector , class SubVector >
void setSubVector (const SubVector &subVector, size_t blockId, size_t blockSize, Vector *vector)
 Helper method to set a sub-vector into a vector, for the sake of clarity. More...
 
template<class Vector >
Eigen::VectorBlock< VectorgetSubVector (Vector &vector, size_t blockId, size_t blockSize)
 Helper method to access a sub-vector from a vector, for the sake of clarity. More...
 
template<class Vector , class SubVector >
void getSubVector (const Vector &vector, const std::vector< size_t > blockIds, size_t blockSize, SubVector *subVector)
 Helper method to get a sub-vector per block from a vector, for the sake of clarity. More...
 
template<typename T , int size, int TOpt>
Eigen::Matrix< T, size, 1, TOpt > interpolate (const Eigen::Matrix< T, size, 1, TOpt > &previous, const Eigen::Matrix< T, size, 1, TOpt > &next, T t)
 Interpolate (slerp) between 2 vectors. More...
 
template<class T , int VOpt>
bool buildOrthonormalBasis (Eigen::Matrix< T, 3, 1, VOpt > *i, Eigen::Matrix< T, 3, 1, VOpt > *j, Eigen::Matrix< T, 3, 1, VOpt > *k)
 Helper method to construct an orthonormal basis (i, j, k) given the 1st vector direction. More...
 
template<class T , int VOpt>
Eigen::Matrix< T, 3, 1, VOpt > robustCrossProduct (const std::array< Eigen::Matrix< T, 3, 1, VOpt >, 2 > &p, const std::array< Eigen::Matrix< T, 3, 1, VOpt >, 2 > &q, T epsilon)
 Calculate the best unit normal we can find in the direction of pXq for one of the endpoints of q. More...
 

Variables

const std::array< gaussQuadraturePoint, 1 > gaussQuadrature1Point
 1D 1-point Gauss-Legendre quadrature \({<x_1, w_1>}\) More...
 
const std::array< gaussQuadraturePoint, 2 > gaussQuadrature2Points
 1D 2-points Gauss-Legendre quadrature \({<x_1, w_1>, <x_2, w_2>}\) More...
 
const std::array< gaussQuadraturePoint, 3 > gaussQuadrature3Points
 1D 3-points Gauss-Legendre quadrature \({<x_1, w_1>, <x_2, w_2>, <x_3, w_3>}\) More...
 
const std::array< gaussQuadraturePoint, 4 > gaussQuadrature4Points
 1D 4-points Gauss-Legendre quadrature \({<x_1, w_1>, <x_2, w_2>, <x_3, w_3>, <x_4, w_4>}\) More...
 
const std::array< gaussQuadraturePoint, 5 > gaussQuadrature5Points
 1D 5-points Gauss-Legendre quadrature \({<x_1, w_1>, <x_2, w_2>, <x_3, w_3>, <x_4, w_4>, <x_5, w_5>}\) More...
 
const std::array< gaussQuadraturePoint, 100 > gaussQuadrature100Points
 1D 100-points Gauss-Legendre quadrature \({<x_1, w_1>, <x_2, w_2>, <x_3, w_3>, ..., <x_{100}, w_{100}>}\) More...
 
const std::array< gaussQuadratureTrianglePoint, 3 > gaussQuadrature2DTriangle3Points
 2D triangle Gauss-Legendre quadrature 3-points \({<\xi_1, \eta_1, w_1>, ..., <\xi_3, \eta_3, w_3>}\) More...
 
const std::array< gaussQuadratureTrianglePoint, 6 > gaussQuadrature2DTriangle6Points
 2D triangle Gauss-Legendre quadrature 6-points \({<\xi_1, \eta_1, w_1>, ..., <\xi_6, \eta_6, w_6>}\) More...
 
const std::array< gaussQuadratureTrianglePoint, 12 > gaussQuadrature2DTriangle12Points
 2D triangle Gauss-Legendre quadrature 12-points \({<\xi_1, \eta_1, w_1>, ..., <\xi_{12}, \eta_{12}, w_{12}>}\) More...
 
const std::unordered_map< LinearSolver, std::string, std::hash< int > > LinearSolverNames
 
const std::unordered_map< IntegrationScheme, std::string, std::hash< int > > IntegrationSchemeNames
 

Typedef Documentation

§ Aabbd

typedef Eigen::AlignedBox<double, 3> SurgSim::Math::Aabbd

Wrapper around the Eigen type.

§ Aabbf

typedef Eigen::AlignedBox<float, 3> SurgSim::Math::Aabbf

Wrapper around the Eigen type.

§ DiagonalMatrix

typedef Eigen::DiagonalMatrix<double, Eigen::Dynamic> SurgSim::Math::DiagonalMatrix

A dynamic size diagonal matrix.

§ Matrix

typedef Eigen::Matrix<double, Eigen::Dynamic, Eigen::Dynamic> SurgSim::Math::Matrix

A dynamic size matrix.

§ Matrix22d

typedef Eigen::Matrix<double, 2, 2, Eigen::RowMajor> SurgSim::Math::Matrix22d

A 2x2 matrix of doubles.

This type (and any structs that contain it) can be safely allocated via new.

§ Matrix22f

typedef Eigen::Matrix<float, 2, 2, Eigen::RowMajor> SurgSim::Math::Matrix22f

A 2x2 matrix of floats.

This type (and any structs that contain it) can be safely allocated via new.

§ Matrix33d

typedef Eigen::Matrix<double, 3, 3, Eigen::RowMajor> SurgSim::Math::Matrix33d

A 3x3 matrix of doubles.

This type (and any structs that contain it) can be safely allocated via new.

§ Matrix33f

typedef Eigen::Matrix<float, 3, 3, Eigen::RowMajor> SurgSim::Math::Matrix33f

A 3x3 matrix of floats.

This type (and any structs that contain it) can be safely allocated via new.

§ Matrix44d

typedef Eigen::Matrix<double, 4, 4, Eigen::RowMajor> SurgSim::Math::Matrix44d

A 4x4 matrix of doubles.

This type (and any structs that contain it) can be safely allocated via new.

§ Matrix44f

typedef Eigen::Matrix<float, 4, 4, Eigen::RowMajor> SurgSim::Math::Matrix44f

A 4x4 matrix of floats.

This type (and any structs that contain it) can be safely allocated via new.

§ Matrix66d

typedef Eigen::Matrix<double, 6, 6, Eigen::RowMajor> SurgSim::Math::Matrix66d

A 6x6 matrix of doubles.

This type (and any structs that contain it) can be safely allocated via new.

§ Quaterniond

typedef Eigen::Quaternion<double> SurgSim::Math::Quaterniond

A quaternion of doubles.

This type (and any structs that contain it) can be safely allocated via new.

§ Quaternionf

typedef Eigen::Quaternion<float> SurgSim::Math::Quaternionf

A quaternion of floats.

This type (and any structs that contain it) can be safely allocated via new.

§ RigidTransform2d

typedef Eigen::Transform<double, 2, Eigen::Isometry> SurgSim::Math::RigidTransform2d

A 2D rigid (isometric) transform, represented as doubles.

This type (and any struct that contain it) can be safely allocated via new.

§ RigidTransform2f

typedef Eigen::Transform<float, 2, Eigen::Isometry> SurgSim::Math::RigidTransform2f

A 2D rigid (isometric) transform, represented as floats.

This type (and any struct that contain it) can be safely allocated via new.

§ RigidTransform3d

typedef Eigen::Transform<double, 3, Eigen::Isometry> SurgSim::Math::RigidTransform3d

A 3D rigid (isometric) transform, represented as doubles.

This type (and any struct that contain it) can be safely allocated via new.

§ RigidTransform3f

typedef Eigen::Transform<float, 3, Eigen::Isometry> SurgSim::Math::RigidTransform3f

A 3D rigid (isometric) transform, represented as floats.

This type (and any struct that contain it) can be safely allocated via new.

§ SparseMatrix

typedef Eigen::SparseMatrix<double> SurgSim::Math::SparseMatrix

A sparse matrix.

§ Vector

typedef Eigen::Matrix<double, Eigen::Dynamic, 1> SurgSim::Math::Vector

A dynamic size column vector.

§ Vector2d

typedef Eigen::Matrix<double, 2, 1> SurgSim::Math::Vector2d

A 2D vector of doubles.

This type (and any structs that contain it) can be safely allocated via new.

§ Vector2f

typedef Eigen::Matrix<float, 2, 1> SurgSim::Math::Vector2f

A 2D vector of floats.

This type (and any structs that contain it) can be safely allocated via new.

§ Vector3d

typedef Eigen::Matrix<double, 3, 1> SurgSim::Math::Vector3d

A 3D vector of doubles.

This type (and any structs that contain it) can be safely allocated via new.

§ Vector3f

typedef Eigen::Matrix<float, 3, 1> SurgSim::Math::Vector3f

A 3D vector of floats.

This type (and any structs that contain it) can be safely allocated via new.

§ Vector4d

typedef Eigen::Matrix<double, 4, 1> SurgSim::Math::Vector4d

A 4D vector of doubles.

This type (and any structs that contain it) can be safely allocated via new.

§ Vector4f

typedef Eigen::Matrix<float, 4, 1> SurgSim::Math::Vector4f

A 4D vector of floats.

This type (and any structs that contain it) can be safely allocated via new.

§ Vector6d

typedef Eigen::Matrix<double, 6, 1> SurgSim::Math::Vector6d

A 6D matrix of doubles.

This type (and any structs that contain it) can be safely allocated via new.

§ Vector6f

typedef Eigen::Matrix<float, 6, 1> SurgSim::Math::Vector6f

A 6D vector of floats.

This type (and any structs that contain it) can be safely allocated via new.

Enumeration Type Documentation

§ IntegrationScheme

The diverse numerical integration scheme supported Each Ode Solver should have its own entry in this enum.

Enumerator
INTEGRATIONSCHEME_STATIC 
INTEGRATIONSCHEME_LINEAR_STATIC 
INTEGRATIONSCHEME_EULER_EXPLICIT 
INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT 
INTEGRATIONSCHEME_EULER_EXPLICIT_MODIFIED 
INTEGRATIONSCHEME_LINEAR_EULER_EXPLICIT_MODIFIED 
INTEGRATIONSCHEME_EULER_IMPLICIT 
INTEGRATIONSCHEME_LINEAR_EULER_IMPLICIT 
INTEGRATIONSCHEME_RUNGE_KUTTA_4 
INTEGRATIONSCHEME_LINEAR_RUNGE_KUTTA_4 
MAX_INTEGRATIONSCHEMES 

§ LinearSolver

The linear numerical integration scheme supported Each Linear Solver should have its own entry in this enum.

Enumerator
LINEARSOLVER_LU 
LINEARSOLVER_CONJUGATEGRADIENT 
MAX_LINEARSOLVER 

§ MlcpConstraintType

Enumerator
MLCP_INVALID_CONSTRAINT 
MLCP_BILATERAL_1D_CONSTRAINT 
MLCP_BILATERAL_2D_CONSTRAINT 
MLCP_BILATERAL_3D_CONSTRAINT 
MLCP_UNILATERAL_3D_FRICTIONLESS_CONSTRAINT 
MLCP_UNILATERAL_3D_FRICTIONAL_CONSTRAINT 
MLCP_BILATERAL_FRICTIONLESS_SLIDING_CONSTRAINT 
MLCP_BILATERAL_FRICTIONAL_SLIDING_CONSTRAINT 
MLCP_NUM_CONSTRAINT_TYPES 

§ OdeEquationUpdate

Enum to identify which of the data need to be updated by the OdeEquation::update()

Enumerator
ODEEQUATIONUPDATE_F 
ODEEQUATIONUPDATE_M 
ODEEQUATIONUPDATE_D 
ODEEQUATIONUPDATE_K 
ODEEQUATIONUPDATE_FMDK 

§ ShapeDirection

Type defining the shape direction for certain templatized shape (i.e.

CylinderShape and CapsuleShape)

Enumerator
SHAPE_DIRECTION_AXIS_X 
SHAPE_DIRECTION_AXIS_Y 
SHAPE_DIRECTION_AXIS_Z 

§ ShapeType

Fixed List of enums for the available Shape types, do not explicitly assign values, ShapeCount is used to determine the number of actual shape types.

Enumerator
SHAPE_TYPE_NONE 
SHAPE_TYPE_BOX 
SHAPE_TYPE_CAPSULE 
SHAPE_TYPE_CYLINDER 
SHAPE_TYPE_DOUBLESIDEDPLANE 
SHAPE_TYPE_MESH 
SHAPE_TYPE_OCTREE 
SHAPE_TYPE_PARTICLES 
SHAPE_TYPE_PLANE 
SHAPE_TYPE_SPHERE 
SHAPE_TYPE_SURFACEMESH 
SHAPE_TYPE_SEGMENTMESH 
SHAPE_TYPE_COMPOUNDSHAPE 
SHAPE_TYPE_COUNT 

Function Documentation

§ addSubMatrix() [1/3]

template<class Matrix , class SubMatrix >
void SurgSim::Math::addSubMatrix ( const SubMatrix &  subMatrix,
size_t  blockIdRow,
size_t  blockIdCol,
size_t  blockSizeRow,
size_t  blockSizeCol,
Matrix matrix 
)

Helper method to add a sub-matrix into a matrix, for the sake of clarity.

Template Parameters
MatrixThe matrix type
SubMatrixThe sub-matrix type
Parameters
subMatrixThe sub-matrix
blockIdRow,blockIdColThe block indices in matrix
blockSizeRow,blockSizeColThe block size (size of the sub-matrix)
[out]matrixThe matrix to add the sub-matrix into

§ addSubMatrix() [2/3]

template<class Matrix , class SubMatrix >
void SurgSim::Math::addSubMatrix ( const SubMatrix &  subMatrix,
const std::vector< size_t >  blockIds,
size_t  blockSize,
Matrix matrix 
)

Helper method to add a sub-matrix made of squared-blocks into a matrix, for the sake of clarity.

Template Parameters
MatrixThe matrix type
SubMatrixThe sub-matrix type
Parameters
subMatrixThe sub-matrix (containing all the squared-blocks)
blockIdsVector of block indices (for accessing matrix) corresponding to the blocks in sub-matrix
blockSizeThe blocks size
[out]matrixThe matrix to add the sub-matrix blocks into

§ addSubMatrix() [3/3]

template<int Opt, typename Index >
void SurgSim::Math::addSubMatrix ( const Eigen::Ref< const Matrix > &  subMatrix,
size_t  blockIdRow,
size_t  blockIdCol,
Eigen::SparseMatrix< double, Opt, Index > *  matrix,
bool  initialize = true 
)

Helper method to add a sub-matrix into a matrix, for the sake of clarity.

Template Parameters
Opt,IndexType parameters defining the output matrix type SparseMatrix<double, Opt, Index>
Parameters
subMatrixThe sub-matrix
blockIdRow,blockIdColThe block indices in matrix
[in,out]matrixThe matrix to add the sub-matrix into
initializeOption parameter, default=true. If true, the matrix form is assumed to be undefined and is initialized when necessary. If false, the matrix form is assumed to be previously defined.
Note
This is a specialization of addSubMatrix for sparse matrices.

§ addSubVector() [1/2]

template<class Vector , class SubVector >
void SurgSim::Math::addSubVector ( const SubVector &  subVector,
size_t  blockId,
size_t  blockSize,
Vector vector 
)

Helper method to add a sub-vector into a vector, for the sake of clarity.

Template Parameters
VectorThe vector type
SubVectorThe sub-vector type
Parameters
subVectorThe sub-vector
blockIdThe block index in vector
blockSizeThe block size
[out]vectorThe vector to add the sub-vector into

§ addSubVector() [2/2]

template<class Vector , class SubVector >
void SurgSim::Math::addSubVector ( const SubVector &  subVector,
const std::vector< size_t >  blockIds,
size_t  blockSize,
Vector vector 
)

Helper method to add a sub-vector per block into a vector, for the sake of clarity.

Template Parameters
VectorThe vector type
SubVectorThe sub-vector type
Parameters
subVectorThe sub-vector (containing all the blocks)
blockIdsVector of block indices (for accessing vector) corresponding to the blocks in sub-vector
blockSizeThe block size
[out]vectorThe vector to add the sub-vector blocks into

§ analyticCrossProduct()

template<typename T >
void SurgSim::Math::analyticCrossProduct ( const LinearMotionND< T, 3 > &  a,
const LinearMotionND< T, 3 > &  b,
Polynomial< T, 2 > *  resultXAxis,
Polynomial< T, 2 > *  resultYAxis,
Polynomial< T, 2 > *  resultZAxis 
)

Calculate an analytic cross product as a Polynomial.

Template Parameters
Tunderlying type of the linear motion
Parameters
athe first linear motion 3 group
bthe second linear motion 3 group
[out]resultXAxisthe X axis in a polynomial representation
[out]resultYAxisthe Y axis in a polynomial representation
[out]resultZAxisthe Z axis in a polynomial representation

§ analyticCrossProductAxis()

template<typename T , int A>
Polynomial< T, 2 > SurgSim::Math::analyticCrossProductAxis ( const LinearMotionND< T, 3 > &  a,
const LinearMotionND< T, 3 > &  b 
)

Calculate a single axis of an analytic cross product as a Polynomial.

Template Parameters
Tunderlying type of the linear motion
Aaxis to generate: 0 = X, 1=Y, 2=Z
Parameters
athe first linear motion 3 group
bthe second linear motion 3 group
Returns
the selected axis in a polynomial representation

§ analyticCrossProductXAxis()

template<typename T >
Polynomial< T, 2 > SurgSim::Math::analyticCrossProductXAxis ( const LinearMotionND< T, 3 > &  a,
const LinearMotionND< T, 3 > &  b 
)

Calculate the X axis of an analytic cross product as a Polynomial.

Template Parameters
Tunderlying type of the linear motion
Parameters
athe first linear motion 3 group
bthe second linear motion 3 group
Returns
the X axis in a polynomial representation

§ analyticCrossProductYAxis()

template<typename T >
Polynomial< T, 2 > SurgSim::Math::analyticCrossProductYAxis ( const LinearMotionND< T, 3 > &  a,
const LinearMotionND< T, 3 > &  b 
)

Calculate the Y axis of an analytic cross product as a Polynomial.

Template Parameters
Tunderlying type of the linear motion
Parameters
athe first linear motion 3 group
bthe second linear motion 3 group
Returns
the Y axis in a polynomial representation

§ analyticCrossProductZAxis()

template<typename T >
Polynomial< T, 2 > SurgSim::Math::analyticCrossProductZAxis ( const LinearMotionND< T, 3 > &  a,
const LinearMotionND< T, 3 > &  b 
)

Calculate the Z axis of an analytic cross product as a Polynomial.

Template Parameters
Tunderlying type of the linear motion
Parameters
athe first linear motion 3 group
bthe second linear motion 3 group
Returns
the Z axis in a polynomial representation

§ analyticDotProduct()

template<typename T >
Polynomial< T, 2 > SurgSim::Math::analyticDotProduct ( const LinearMotionND< T, 3 > &  a,
const LinearMotionND< T, 3 > &  b 
)

Calculate an analytic dot product as a Polynomial.

Template Parameters
Tunderlying type of the linear motion
Parameters
athe first linear motion 3 group
bthe second linear motion 3 group
Returns
the dot product in a polynomial representation

§ analyticMagnitudeSquared()

template<typename T >
Polynomial< T, 2 > SurgSim::Math::analyticMagnitudeSquared ( const LinearMotionND< T, 3 > &  motion)

Calculate the magnitude squared of a linear motion 3 group as a polynomial.

Template Parameters
Tunderlying type of the linear motion
Parameters
motionthe linear motion 3 group
Returns
the magnitude squared of the linear motion as a polynomial

§ analyticTripleProduct()

template<typename T >
Polynomial< T, 3 > SurgSim::Math::analyticTripleProduct ( const LinearMotionND< T, 3 > &  a,
const LinearMotionND< T, 3 > &  b,
const LinearMotionND< T, 3 > &  c 
)

Calculate an analytic cross product as a Polynomial, as a polynomial whose value for t=0..1 is the value of the triple product.

Template Parameters
Tunderlying type of the linear motion
Parameters
athe first linear motion 3 group
bthe second linear motion 3 group
cthe third linear motion 3 group
Returns
a 3rd order polynomial representation of the triple product

§ areSegmentsIntersecting()

template<class T , int MOpt>
bool SurgSim::Math::areSegmentsIntersecting ( time,
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,
Eigen::Matrix< T, 2, 1, MOpt > *  barycentricCoordinates 
)

Check if 2 segments intersect at a given time of their motion.

Template Parameters
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
timeThe time of coplanarity of the 4 points (A(t), B(t), C(t), D(t) are expected to be coplanar)
A,BThe 1st segment motion (each point's motion is from first to second)
C,DThe 2nd segment motion (each point's motion is from first to second)
[out]barycentricCoordinatesThe barycentric coordinates of the intersection in AB(t) and CD(t) i.e. P(t) = A + barycentricCoordinates[0].AB(t) = C + barycentricCoordinates[1].CD
Returns
True if AB(t) is intersecting CD(t), False otherwise

§ assignSubMatrix()

template<int Opt, typename Index >
void SurgSim::Math::assignSubMatrix ( const Eigen::Ref< const Matrix > &  subMatrix,
size_t  blockIdRow,
size_t  blockIdCol,
Eigen::SparseMatrix< double, Opt, Index > *  matrix,
bool  initialize = true 
)

Helper method to assign a sub-matrix into a matrix, for the sake of clarity.

Template Parameters
Opt,IndexType parameters defining the output matrix type SparseMatrix<double, Opt, Index>
Parameters
subMatrixThe sub-matrix
blockIdRow,blockIdColThe block indices in matrix
[in,out]matrixThe matrix to assign the sub-matrix into
initializeOption parameter, default=true. If true, the matrix form is assumed to be undefined and is initialized when necessary. If false, the matrix form is assumed to be previously defined.

§ 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
TFloating point type of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
ptVertex of the point.
sv0,sv1Vertices of the line segment.
[out]coordinatesBarycentric 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
TFloating point type of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
ptVertex of the point.
tv0,tv1,tv2Vertices of the triangle in counter clockwise order in respect to the normal.
tnNormal of the triangle (yes must be of norm 1 and a,b,c CCW).
[out]coordinatesBarycentric 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
TFloating point type of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
ptVertex of the point.
tv0,tv1,tv2Vertices of the triangle.
[out]coordinatesThe Barycentric coordinates.
Returns
bool true on success, false if the triangle is considered degenerate

§ blockOperation()

template<int Opt, typename Index >
void SurgSim::Math::blockOperation ( const Eigen::Ref< const Matrix > &  subMatrix,
size_t  rowStart,
size_t  columnStart,
Eigen::SparseMatrix< double, Opt, Index > *  matrix,
void(Operation< Matrix, Eigen::SparseMatrix< double, Opt, Index >>::*)(double *, const double &)  op 
)

Runs a given operation on a SparseMatrix block(i, j, n, m) from a (n x m) 'subMatrix' with searching for the block 1st element.


It supposes:

  • that both the SparseMatrix and the 'subMatrix' are using the same Scalar type, double.
    This function can be used to initialize the form of a sparse matrix as it will add entries when appropriate entries do not exist.
    Template Parameters
    Opt,IndexType parameters defining the output matrix type SparseMatrix<double, Opt, Index>
    Parameters
    subMatrixThe sub matrix that will be copied into the SparseMatrix block
    rowStart,columnStartThe row and column indices to indicate where the block in the SparseMatrix starts
    [in,out]matrixThe sparse matrix in which the block needs to be set by 'subMatrix'
    opThe operation to run on the block
    Exceptions
    SurgSim::Framework::AssertionFailureIf one of the following conditions is met:
  • if 'subMatrix' is smaller than (n x m) in any dimension
  • if 'matrix' is nullptr or smaller than (n x m) in any dimension
  • if the requested block is out of range in 'matrix'.
  • if 'matrix' does not fulfill the requirement (i.e. is missing elements within the block).
    Note
    The receiving SparseMatrix must have a structure like the following:
    (xx x0 x)
    (xx 0x x)
    (x0[xx]x) -> The block must already contain all the coefficients but these rows and columns may
    (0x[xx]0) -> contains more coefficients before and after the block.
    (xx 00 x)

§ blockWithSearch()

template<int Opt, typename Index >
void SurgSim::Math::blockWithSearch ( const Eigen::Ref< const Matrix > &  subMatrix,
size_t  rowStart,
size_t  columnStart,
size_t  n,
size_t  m,
Eigen::SparseMatrix< double, Opt, Index > *  matrix,
void(Operation< Matrix, Eigen::SparseMatrix< double, Opt, Index >>::*)(double *, size_t, size_t, size_t, const Matrix &, size_t)  op 
)

Runs a given operation on a SparseMatrix block(i, j, n, m) from a (n x m) 'subMatrix' with searching for the block 1st element.


It supposes:

  • that the SparseMatrix already contains all the elements within the block (no insertion necessary)
  • that both the SparseMatrix and the 'subMatrix' are using the same Scalar type, double.
    This function will not change anything to the structure of the SparseMatrix, only change the values of the corresponding coefficients.
    Template Parameters
    Opt,IndexType parameters defining the output matrix type SparseMatrix<double, Opt, Index>
    Parameters
    subMatrixThe sub matrix that will be copied into the SparseMatrix block
    rowStart,columnStartThe row and column indices to indicate where the block in the SparseMatrix starts
    n,mThe block size (Derived may be bigger but cannot be smaller in both dimension)
    [in,out]matrixThe sparse matrix in which the block needs to be set by 'subMatrix'
    opThe operation to run on the block
    Exceptions
    SurgSim::Framework::AssertionFailureIf one of the following conditions is met:
  • if 'subMatrix' is smaller than (n x m) in any dimension
  • if 'matrix' is nullptr or smaller than (n x m) in any dimension
  • if the requested block is out of range in 'matrix'.
  • if 'matrix' does not fulfill the requirement (i.e. is missing elements within the block).
    Note
    The receiving SparseMatrix must have a structure like the following:
    (xx x0 x)
    (xx 0x x)
    (x0[xx]x) -> The block must already contain all the coefficients but these rows and columns may
    (0x[xx]0) -> contains more coefficients before and after the block.
    (xx 00 x)

§ buildOrthonormalBasis()

template<class T , int VOpt>
bool SurgSim::Math::buildOrthonormalBasis ( Eigen::Matrix< T, 3, 1, VOpt > *  i,
Eigen::Matrix< T, 3, 1, VOpt > *  j,
Eigen::Matrix< T, 3, 1, VOpt > *  k 
)

Helper method to construct an orthonormal basis (i, j, k) given the 1st vector direction.

Template Parameters
Tthe numeric data type used for the vector argument. Can usually be deduced.
VOptthe option flags (alignment etc.) used for the vector argument. Can be deduced.
Parameters
[in,out]iShould provide the 1st direction on input. The 1st vector of the basis (i, j, k) on output.
[out]j,kThe 2nd and 3rd orthonormal vectors of the basis (i, j, k)
Returns
True if (i, j, k) has been built successfully, False if 'i' is a (or close to a) null vector
Note
If any of the parameter is a nullptr, an exception will be raised

§ calculateCcdContactPointTriangle()

template<class T , int MOpt>
bool SurgSim::Math::calculateCcdContactPointTriangle ( const std::pair< Eigen::Matrix< T, 3, 1, MOpt >, Eigen::Matrix< T, 3, 1, MOpt >> &  P,
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,
T *  timeOfImpact,
T *  tv01Param,
T *  tv02Param 
)
inline

Continuous collision detection between a point P and a triangle ABC.

Template Parameters
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
Pthe point motion (from first to second) to calculate the ccd with
A,B,CThe triangle points motion (from first to second) to calculate the ccd with
[out]timeOfImpactThe time of impact within [0..1] if a collision is found
[out]tv01Param,tv02ParamThe barycentric coordinate of the contact point in the triangle i.e. ContactPoint(timeOfImpact) = A(timeOfImpact) + tv01Param.AB(timeOfImpact) + tv02Param.AC(timeOfImpact)
Returns
True if the given point/triangle motions intersect, False otherwise
Note
Simple cubic-solver https://graphics.stanford.edu/courses/cs468-02-winter/Papers/Collisions_vetements.pdf
Optimized method http://www.robotics.sei.ecnu.edu.cn/CCDLY/GMOD15.pdf
Optimized method https://www.cs.ubc.ca/~rbridson/docs/brochu-siggraph2012-ccd.pdf

§ calculateCcdContactSegmentSegment()

template<class T , int MOpt>
bool SurgSim::Math::calculateCcdContactSegmentSegment ( 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,
T *  timeOfImpact,
T *  s0p1Factor,
T *  s1p1Factor 
)
inline

Continuous collision detection between two segments AB and CD.

Template Parameters
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
A,BThe 1st segment motion (each point's motion is from first to second)
C,DThe 2nd segment motion (each point's motion is from first to second)
[out]timeOfImpactThe time of impact within [0..1] if an collision is found
[out]s0p1Factor,s1p1FactorThe barycentric coordinate of the contact point on the 2 segments i.e. ContactPoint(timeOfImpact) = A(timeOfImpact) + s0p1Factor.AB(timeOfImpact) i.e. ContactPoint(timeOfImpact) = C(timeOfImpact) + s1p1Factor.CD(timeOfImpact)
Returns
True if the given segment/segment motions intersect, False otherwise
Note
Simple cubic-solver https://graphics.stanford.edu/courses/cs468-02-winter/Papers/Collisions_vetements.pdf
Optimized method http://www.robotics.sei.ecnu.edu.cn/CCDLY/GMOD15.pdf
Optimized method https://www.cs.ubc.ca/~rbridson/docs/brochu-siggraph2012-ccd.pdf

§ 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
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
tv0,tv1,tv2Vertices of the triangle.
tnNormal of the triangle, should be normalized.
cv0,cv1Ends of the capsule axis.
crCapsule radius.
[out]penetrationDepthThe depth of penetration.
[out]penetrationPointTriangleThe contact point on triangle.
[out]penetrationPointCapsuleThe contact point on capsule.
[out]contactNormalThe contact normal that points from capsule to triangle.
[out]penetrationPointCapsuleAxisThe 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
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
tv0,tv1,tv2Vertices of the triangle.
cv0,cv1Ends of the capsule axis.
crCapsule radius.
[out]penetrationDepthThe depth of penetration.
[out]penetrationPointTriangleThe contact point on triangle.
[out]penetrationPointCapsuleThe contact point on capsule.
[out]contactNormalThe contact normal that points from capsule to triangle.
[out]penetrationPointCapsuleAxisThe 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
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
t0v0,t0v1,t0v2Vertices of the first triangle.
t1v0,t1v1,t1v2Vertices of the second triangle.
t0nUnit length normal of the first triangle, should be normalized.
t1nUnit length normal of the second triangle, should be normalized.
[out]penetrationDepthThe depth of penetration.
[out]penetrationPoint0The contact point on triangle0 (t0v0,t0v1,t0v2).
[out]penetrationPoint1The contact point on triangle1 (t1v0,t1v1,t1v2).
[out]contactNormalThe 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
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
t0v0,t0v1,t0v2Vertices of the first triangle, should be normalized.
t1v0,t1v1,t1v2Vertices of the second triangle, should be normalized.
[out]penetrationDepthThe depth of penetration.
[out]penetrationPoint0The contact point on triangle0 (t0v0,t0v1,t0v2).
[out]penetrationPoint1The contact point on triangle1 (t1v0,t1v1,t1v2).
[out]contactNormalThe 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
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
t0v0,t0v1,t0v2Vertices of the first triangle.
t1v0,t1v1,t1v2Vertices of the second triangle.
t0nNormal of the first triangle, should be normalized.
t1nNormal of the second triangle, should be normalized.
[out]penetrationDepthThe depth of penetration.
[out]penetrationPoint0The contact point on triangle0 (t0v0,t0v1,t0v2).
[out]penetrationPoint1The contact point on triangle1 (t1v0,t1v1,t1v2).
[out]contactNormalThe 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
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
t0v0,t0v1,t0v2Vertices of the first triangle.
t1v0,t1v1,t1v2Vertices of the second triangle.
[out]penetrationDepthThe depth of penetration.
[out]penetrationPoint0The contact point on triangle0 (t0v0,t0v1,t0v2).
[out]penetrationPoint1The contact point on triangle1 (t1v0,t1v1,t1v2).
[out]contactNormalThe 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.

§ clamp()

template<class T >
T SurgSim::Math::clamp ( value,
min,
max,
epsilon 
)

Clamp any values within an epsilon window to a maximum or minimum value.

Note
max is tested first, so if the value < min + epsilon and value > max - epsilon, value will be set to max.
Template Parameters
Tunderlying type
Parameters
value[in/out] the value to be clamped
minthe minimum value for the clamp
maxthe maximum value for the clamp
epsilondefinition of the epsilon window.

§ clearMatrix()

template<typename T , int Opt, typename Index >
void SurgSim::Math::clearMatrix ( Eigen::SparseMatrix< T, Opt, Index > *  matrix)
inline

Helper method to zero all entries of a matrix specialized for Sparse Matrices.

This allows the preservation of the the matrix form while still allowing the reset of the matrix entries to zero.

Parameters
[in,out]matrixThe matrix to set to zero

§ computeAngle() [1/2]

template<typename T , int QOpt>
T SurgSim::Math::computeAngle ( const Eigen::Quaternion< T, QOpt > &  quaternion)
inline

Get the angle corresponding to a quaternion's rotation, in radians.

Template Parameters
Tthe numeric data type used for arguments and the return value. Can usually be deduced.
QOptthe option flags (alignment etc.) used for the quaternion argument. Can be deduced.
Parameters
quaternionthe rotation quaternion to inspect.
Returns
the angle of the rotation within [0 +pi], in radians.

§ computeAngle() [2/2]

template<typename T , int MOpt>
T SurgSim::Math::computeAngle ( const Eigen::Matrix< T, 3, 3, MOpt > &  matrix)
inline

Get the angle corresponding to a quaternion's rotation, in radians.

If you don't care about the rotation axis, this is more efficient than computeAngleAndAxis().

Template Parameters
Tthe numeric data type used for arguments and the return value. Can usually be deduced.
MOptthe option flags (alignment etc.) used for the rotation matrix argument. Can be deduced.
Parameters
matrixthe rotation matrix to inspect.
Returns
the angle of the rotation, in radians.

§ computeAngleAndAxis() [1/2]

template<typename T , int QOpt, int VOpt>
void SurgSim::Math::computeAngleAndAxis ( const Eigen::Quaternion< T, QOpt > &  quaternion,
T *  angle,
Eigen::Matrix< T, 3, 1, VOpt > *  axis 
)
inline

Get the angle (in radians) and axis corresponding to a quaternion's rotation.

Note
Unit quaternions cover the unit sphere twice (q=-q). To make sure that the same rotation (q or -q)
returns the same Axis/Angle, we need a pi range for the angle.
We choose to enforce half the angle in the quadrant [0 +pi/2], which leads to an output angle in [0 +pi].
Template Parameters
Tthe numeric data type used for arguments and the return value. Can usually be deduced.
QOptthe option flags (alignment etc.) used for the quaternion argument. Can be deduced.
VOptthe option flags (alignment etc.) used for the axis vector argument. Can be deduced.
Parameters
quaternionthe rotation quaternion to inspect.
[out]anglethe angle of the rotation in [0 +pi], in radians.
[out]axisthe axis of the rotation.

§ computeAngleAndAxis() [2/2]

template<typename T , int MOpt, int VOpt>
void SurgSim::Math::computeAngleAndAxis ( const Eigen::Matrix< T, 3, 3, MOpt > &  matrix,
T *  angle,
Eigen::Matrix< T, 3, 1, VOpt > *  axis 
)
inline

Get the angle (in radians) and axis corresponding to a rotation matrix.

Template Parameters
Tthe numeric data type used for arguments and the return value. Can usually be deduced.
MOptthe option flags (alignment etc.) used for the rotation matrix argument. Can be deduced.
VOptthe option flags (alignment etc.) used for the axis vector argument. Can be deduced.
Parameters
matrixthe rotation matrix to inspect.
[out]anglethe angle of the rotation, in radians.
[out]axisthe axis of the rotation.

§ computeRotationVector()

template<typename T , int TOpt, int VOpt>
void SurgSim::Math::computeRotationVector ( const Eigen::Transform< T, 3, Eigen::Isometry, TOpt > &  end,
const Eigen::Transform< T, 3, Eigen::Isometry, TOpt > &  start,
Eigen::Matrix< T, 3, 1, VOpt > *  rotationVector 
)

Get the vector corresponding to the rotation between transforms.

Template Parameters
Tthe numeric data type used for arguments and the return value. Can usually be deduced.
TOptthe option flags (alignment etc.) used for the transform arguments. Can be deduced.
VOptthe option flags (alignment etc.) used for the vector argument. Can be deduced.
Parameters
endthe transform after the rotation.
startthe transform before the rotation.
[out]rotationVectora vector describing the rotation.

§ 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
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
l0v0,l0v1Points on Line 0.
l1v0,l1v1Points on Line 1.
[out]pt0The closest point on line 0.
[out]pt1The 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)

§ 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
ptThe input point.
v0,v1Two vertices on the line.
[out]resultThe 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,
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
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
ptThe point to check.
nThe normal of the plane n (normalized).
dConstant d for the plane equation as in n.x + d = 0.
[out]resultProjection 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
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
ptThe input point
sv0,sv1The segment extremities.
[out]resultEither 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
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
ptThe point that is being measured.
tv0,tv1,tv2The vertices of the triangle.
[out]resultThe 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,
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
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
sv0,sv1Endpoints of the segments.
nNormal of the plane n (normalized).
dConstant d in n.x + d = 0.
[out]closestPointSegmentPoint 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]planeIntersectionPointthe 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
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
s0v0,s0v1Segment 0 Extremities.
s1v0,s1v1Segment 1 Extremities.
[out]pt0Closest point on segment 0
[out]pt1Closest point on segment 1
[out]s0tAbscissa at the point of intersection on Segment 0 (s0v0 + t * (s0v1 - s0v0)).
[out]s1tAbscissa 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,
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
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
sv0,sv1Extremities of the line segment.
tv0,tv1,tv2Triangle points.
[out]segmentPointClosest point on the segment.
[out]trianglePointClosest point on the triangle.
Returns
the the distance between the two closest points, i.e. (trianglePoint - segmentPoint).norm().

§ 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,
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
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
sv0,sv1Extremities of the line segment.
tv0,tv1,tv2Points of the triangle.
normalNormal of the triangle (Expected to be normalized)
[out]segmentPointClosest point on the segment.
[out]trianglePointClosest point on the triangle.
Returns
the distance between the two closest points.

§ 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,
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
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
tv0,tv1,tv2Points of the triangle.
nNormal of the plane n (normalized).
dConstant d in n.x + d = 0.
closestPointTriangleClosest 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.
planeProjectionPointProjection 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
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
t0v0,t0v1,t0v2Points of the first triangle.
t1v0,t1v1,t1v2Points of the second triangle.
[out]closestPoint0Closest point on the first triangle, unless penetrating, in which case it is the point along the edge that allows min separation.
[out]closestPoint1Closest 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.

§ doAabbIntersect() [1/2]

template<class Scalar , int Dim>
bool SurgSim::Math::doAabbIntersect ( const Eigen::AlignedBox< Scalar, Dim > &  aabb0,
const Eigen::AlignedBox< Scalar, Dim > &  aabb1,
double  tolerance 
)

Determine whether two AABBs have an intersection with each other, for the calculation see http://www.gamasutra.com/view/feature/131790/simple_intersection_tests_for_games.php?page=3.

Template Parameters
Scalarnumeric type
Dimdimension of the space to be used
Parameters
aabb0first axis aligned bounding box
aabb1second axis aligned bounding box
tolerancethe bounding boxes will be considered bigger by this amount
Returns
true if there is an overlap between the two boxes

§ doAabbIntersect() [2/2]

template<class Scalar , int Dim>
bool SurgSim::Math::doAabbIntersect ( const Eigen::AlignedBox< Scalar, Dim > &  a,
const Eigen::AlignedBox< Scalar, Dim > &  b 
)

Determine whether two AABBs overlap, using a minimal set of eigen calls, does not take a tolerance.

Template Parameters
Scalarnumeric type
Dimdimension of the space to be used
Parameters
afirst axis aligned bounding box
bsecond axis aligned bounding box
Returns
true if there is an overlap between the two boxes

§ 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
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
sv0,sv1Extremities of the segment.
tv0,tv1,tv2The triangle vertices. CCW around the normal.
tnThe triangle normal, should be normalized.
[out]resultThe 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
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
capsuleBottomPosition of the capsule bottom
capsuleTopPosition of the capsule top
capsuleRadiusThe capsule radius
boxAxis 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,
pd0,
const Eigen::Matrix< T, 3, 1, MOpt > &  pn1,
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
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
pn0,pd0Normal and constant of the first plane, nx + d = 0.
pn1,pd1Normal and constant of the second plane, nx + d = 0.
[out]pt0,pt1Two points on the intersection line, not valid if there is no intersection.
Returns
true when a unique line exists, false for disjoint or coinciding.

§ 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
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
t0v0,t0v1,t0v2Vertices of the first triangle.
t1v0,t1v1,t1v2Vertices of the second triangle.
t0nNormal of the first triangle, should be normalized.
t1nNormal 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
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
t0v0,t0v1,t0v2Vertices of the first triangle.
t1v0,t1v1,t1v2Vertices of the second triangle.
Returns
True, if intersection is detected.

§ edgeIntersection()

template<class T >
void SurgSim::Math::edgeIntersection ( dStart,
dEnd,
pvStart,
pvEnd,
T *  parametricIntersection,
size_t *  parametricIntersectionIndex 
)

Two ends of the triangle edge are given in terms of the following vertex properties.

  • Signed distance from the colliding triangle.
  • Projection on the separating axis. Get the intersection of this edge and the plane in terms of the projection on the separating axis.
    Template Parameters
    TAccuracy of the calculation, can usually be inferred.
    Parameters
    dStartSigned distance of the start of edge from the plane of the colliding triangle.
    dEndSigned distance of the end of edge from the plane of the colliding triangle.
    pvStartProjection of the start of edge from the plane of the colliding triangle.
    pvEndProjection of the end of edge from the plane of the colliding triangle.
    parametricIntersectionParametric representation of the intersection between the triangle edge and the plane in terms of the projection on the separating axis.
    parametricIntersectionIndexThe array index of parametricIntersection.

§ findRootsInRange01()

template<class T >
int SurgSim::Math::findRootsInRange01 ( const Polynomial< T, 3 > &  polynomial,
std::array< T, 3 > *  roots 
)

Find all roots in range \([0 \ldotp\ldotp 1]\) of a cubic equation.

Template Parameters
TThe equation coefficient type
Parameters
polynomialThe cubic polynomial \(ax^3 + bx^2 + cx + d\)
[out]rootsAll roots ordered ascendingly in \([0 \ldotp\ldotp 1]\) if any (3 max)
Returns
The number of roots found in \([0 \ldotp\ldotp 1]\) and saved in roots.

\[ \begin{array}{lll} P(x) &=& ax^3 + bx^2 + cx + d \\ \text{} P'(x) &=& 3ax^2 + 2bx + c \Rightarrow \Delta = (2b)^2 - 4(3a)(c) = 4(b^2 - 3ac) \end{array} \\ \text{} \left\{ \begin{array}{ll} \Delta < 0 & \text{P is monotonic, P' is always the same sign, the sign of P'(0) = sign(c)} \\ \text{} \Delta = 0 & \text{P is monotonic with an inflection point, P' is always the same sign, except at P'(root) = 0} \\ \text{} \Delta > 0 & \text{P is monotonic on 3 separate intervals} \end{array} \right. \]

§ getMlcpConstraintTypeName()

std::string SurgSim::Math::getMlcpConstraintTypeName ( MlcpConstraintType  constraintType)
inline

§ getMlcpConstraintTypeValue()

MlcpConstraintType SurgSim::Math::getMlcpConstraintTypeValue ( const std::string &  typeName)
inline

§ getSubMatrix()

template<class Matrix >
Eigen::Block<Matrix> SurgSim::Math::getSubMatrix ( Matrix matrix,
size_t  blockIdRow,
size_t  blockIdCol,
size_t  blockSizeRow,
size_t  blockSizeCol 
)

Helper method to access a sub-matrix from a matrix, for the sake of clarity.

Template Parameters
MatrixThe matrix type to get the sub-matrix from
Parameters
matrixThe matrix to get the sub-matrix from
blockIdRow,blockIdColThe block indices
blockSizeRow,blockSizeColThe block size
Returns
The requested sub-matrix
Note
Disable cpplint warnings for use of non-const reference
Eigen has a specific type for Block that we want to return with read/write access
therefore the Matrix from which the Block is built from must not be const

§ getSubVector() [1/2]

template<class Vector >
Eigen::VectorBlock<Vector> SurgSim::Math::getSubVector ( Vector vector,
size_t  blockId,
size_t  blockSize 
)

Helper method to access a sub-vector from a vector, for the sake of clarity.

Template Parameters
VectorThe vector type to get the sub-vector from
Parameters
vectorThe vector to get the sub-vector from
blockIdThe block index
blockSizeThe block size
Returns
The requested sub-vector
Note
Disable cpplint warnings for use of non-const reference
Eigen has a specific type for VectorBlock that we want to return with read/write access
therefore the Vector from which the VectorBlock is built from must not be const

§ getSubVector() [2/2]

template<class Vector , class SubVector >
void SurgSim::Math::getSubVector ( const Vector vector,
const std::vector< size_t >  blockIds,
size_t  blockSize,
SubVector *  subVector 
)

Helper method to get a sub-vector per block from a vector, for the sake of clarity.

Template Parameters
VectorThe vector type
SubVectorThe sub-vector type
Parameters
vectorThe vector (containing the blocks in a sparse manner)
blockIdsVector of block indices (for accessing vector) corresponding to the blocks in vector
blockSizeThe block size
[out]subVectorThe sub-vector to store the requested blocks (blockIds) from vector into

§ interpolate() [1/3]

template<typename T , int TOpt>
Eigen::Transform<T, 3, Eigen::Isometry> SurgSim::Math::interpolate ( const Eigen::Transform< T, 3, Eigen::Isometry, TOpt > &  t0,
const Eigen::Transform< T, 3, Eigen::Isometry, TOpt > &  t1,
t 
)
inline

Interpolate (slerp) between 2 rigid transformations.

Template Parameters
Tthe numeric data type used for arguments and the return value. Can usually be deduced.
TOptthe option flags (alignment etc.) used for the Transform arguments. Can be deduced.
Parameters
t0The start transform (at time 0.0).
t1The end transform (at time 1.0).
tThe interpolation time requested. Within [0..1].
Returns
the transform resulting in the slerp interpolation at time t, between t0 and t1.
Note
t=0 => returns t0
t=1 => returns t1

§ interpolate() [2/3]

template<typename T , int QOpt>
Eigen::Quaternion<T, QOpt> SurgSim::Math::interpolate ( const Eigen::Quaternion< T, QOpt > &  q0,
const Eigen::Quaternion< T, QOpt > &  q1,
t 
)
inline

Interpolate (slerp) between 2 quaternions.

Template Parameters
Tthe numeric data type used for arguments and the return value. Can usually be deduced.
QOptthe option flags (alignment etc.) used for the quaternion arguments. Can be deduced.
Parameters
q0The start quaternion (at time 0.0).
q1The end quaternion (at time 1.0).
tThe interpolation time requested. Within [0..1].
Returns
the quaternion resulting in the slerp interpolation at time t, between q0 and q1.
Note
t=0 => returns either q0 or -q0
t=1 => returns either q1 or -q1
'Interpolate' has been created because slerp might not be enough in certain cases. This gives room for correction and special future treatment

§ interpolate() [3/3]

template<typename T , int size, int TOpt>
Eigen::Matrix<T, size, 1, TOpt> SurgSim::Math::interpolate ( const Eigen::Matrix< T, size, 1, TOpt > &  previous,
const Eigen::Matrix< T, size, 1, TOpt > &  next,
t 
)

Interpolate (slerp) between 2 vectors.

Template Parameters
Tthe numeric data type used for arguments and the return value. Can usually be deduced.
sizethe size of the vectors. Can be deduced.
TOptthe option flags (alignment etc.) used for the Vector arguments. Can be deduced.
Parameters
previousThe starting vector (at time 0.0).
nextThe ending vector (at time 1.0).
tThe interpolation time requested. Within [0..1], although note bounds are not checked.
Returns
the transform resulting in the slerp interpolation at time t.
Note
t=0 => returns vector 'previous'
t=1 => returns vector 'next'

§ 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
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
sv0,sv1Extremities of the line segment.
boxAxis aligned bounding box
[out]intersectionsThe points of intersection between the segment and the box

§ IntervalArithmetic_add() [1/4]

template<class P >
void SurgSim::Math::IntervalArithmetic_add ( const Interval< P > &  a,
const Interval< P > &  b,
Interval< P > *  res 
)

Calculate the sum of two intervals.

Template Parameters
Punderlying type of the interval
Parameters
athe first interval
bthe second interval
res[out] the result of a + b

§ IntervalArithmetic_add() [2/4]

template<class P >
void SurgSim::Math::IntervalArithmetic_add ( const IntervalND< P, 3 > &  a,
const IntervalND< P, 3 > &  b,
IntervalND< P, 3 > *  res 
)

Calculate the sum of two interval groups.

Template Parameters
Punderlying type of the interval
Parameters
athe first interval group
bthe second interval group
res[out] the result of a + b

§ IntervalArithmetic_add() [3/4]

template<class T >
void SurgSim::Math::IntervalArithmetic_add ( const Interval< T > &  a,
const Interval< T > &  b,
Interval< T > *  res 
)

§ IntervalArithmetic_add() [4/4]

template<class T >
void SurgSim::Math::IntervalArithmetic_add ( const IntervalND< T, 3 > &  a,
const IntervalND< T, 3 > &  b,
IntervalND< T, 3 > *  res 
)

§ IntervalArithmetic_addadd() [1/2]

template<class P >
void SurgSim::Math::IntervalArithmetic_addadd ( const Interval< P > &  a,
const Interval< P > &  b,
Interval< P > *  res 
)

Calculate the sum of three intervals res + a + b.

Template Parameters
Punderlying type of the interval
Parameters
athe first interval
bthe second interval
res[in/out] the result of res + a + b

§ IntervalArithmetic_addadd() [2/2]

template<class T >
void SurgSim::Math::IntervalArithmetic_addadd ( const Interval< T > &  a,
const Interval< T > &  b,
Interval< T > *  res 
)

§ IntervalArithmetic_addmul() [1/2]

template<class P >
void SurgSim::Math::IntervalArithmetic_addmul ( const Interval< P > &  a,
const Interval< P > &  b,
Interval< P > *  res 
)

Add the product of two intervals to an existing value.

Template Parameters
Punderlying type of the interval
Parameters
athe first interval
bthe second interval
res[in/out] the result of res + (a * b)

§ IntervalArithmetic_addmul() [2/2]

template<class T >
void SurgSim::Math::IntervalArithmetic_addmul ( const Interval< T > &  a,
const Interval< T > &  b,
Interval< T > *  res 
)

§ IntervalArithmetic_addsub() [1/2]

template<class P >
void SurgSim::Math::IntervalArithmetic_addsub ( const Interval< P > &  a,
const Interval< P > &  b,
Interval< P > *  res 
)

Add the difference of two intervals to an existing value.

Template Parameters
Punderlying type of the interval
Parameters
athe first interval
bthe second interval
res[in/out] the result of res + (a - b)

§ IntervalArithmetic_addsub() [2/2]

template<class T >
void SurgSim::Math::IntervalArithmetic_addsub ( const Interval< T > &  a,
const Interval< T > &  b,
Interval< T > *  res 
)

§ IntervalArithmetic_crossProduct() [1/2]

template<class P >
void SurgSim::Math::IntervalArithmetic_crossProduct ( const IntervalND< P, 3 > &  a,
const IntervalND< P, 3 > &  b,
IntervalND< P, 3 > *  res 
)

Calculate the cross product of two interval groups.

Template Parameters
Punderlying type of the interval
Parameters
athe first interval group
bthe second interval group
res[out] the cross product of a and b

§ IntervalArithmetic_crossProduct() [2/2]

template<class T >
void SurgSim::Math::IntervalArithmetic_crossProduct ( const IntervalND< T, 3 > &  a,
const IntervalND< T, 3 > &  b,
IntervalND< T, 3 > *  res 
)

§ IntervalArithmetic_dotProduct() [1/2]

template<class P >
void SurgSim::Math::IntervalArithmetic_dotProduct ( const IntervalND< P, 3 > &  a,
const IntervalND< P, 3 > &  b,
Interval< P > *  res 
)

Calculate the dot product of two interval groups.

Template Parameters
Punderlying type of the interval
Parameters
athe first interval group
bthe second interval group
res[out] the dot product of a and b

§ IntervalArithmetic_dotProduct() [2/2]

template<class T >
void SurgSim::Math::IntervalArithmetic_dotProduct ( const IntervalND< T, 3 > &  a,
const IntervalND< T, 3 > &  b,
Interval< T > *  res 
)

§ IntervalArithmetic_mul() [1/2]

template<class P >
void SurgSim::Math::IntervalArithmetic_mul ( const Interval< P > &  a,
const Interval< P > &  b,
Interval< P > *  res 
)

Calculate the product of two intervals.

Template Parameters
Punderlying type of the interval
Parameters
athe first interval
bthe second interval
res[out] the result of a * b

§ IntervalArithmetic_mul() [2/2]

template<class T >
void SurgSim::Math::IntervalArithmetic_mul ( const Interval< T > &  a,
const Interval< T > &  b,
Interval< T > *  res 
)

§ IntervalArithmetic_sub() [1/4]

template<class P >
void SurgSim::Math::IntervalArithmetic_sub ( const Interval< P > &  a,
const Interval< P > &  b,
Interval< P > *  res 
)

Calculate the difference of two intervals.

Template Parameters
Punderlying type of the interval
Parameters
athe first interval
bthe second interval
res[out] the result of a - b

§ IntervalArithmetic_sub() [2/4]

template<class P >
void SurgSim::Math::IntervalArithmetic_sub ( const IntervalND< P, 3 > &  a,
const IntervalND< P, 3 > &  b,
IntervalND< P, 3 > *  res 
)

Calculate the difference of two interval groups.

Template Parameters
Punderlying type of the interval
Parameters
athe first interval group
bthe second interval group
res[out] the result of a - b

§ IntervalArithmetic_sub() [3/4]

template<class T >
void SurgSim::Math::IntervalArithmetic_sub ( const Interval< T > &  a,
const Interval< T > &  b,
Interval< T > *  res 
)

§ IntervalArithmetic_sub() [4/4]

template<class T >
void SurgSim::Math::IntervalArithmetic_sub ( const IntervalND< T, 3 > &  a,
const IntervalND< T, 3 > &  b,
IntervalND< T, 3 > *  res 
)

§ IntervalArithmetic_submul() [1/2]

template<class P >
void SurgSim::Math::IntervalArithmetic_submul ( const Interval< P > &  a,
const Interval< P > &  b,
Interval< P > *  res 
)

Subtract the product of two intervals from an existing value.

Template Parameters
Punderlying type of the interval
Parameters
athe first interval
bthe second interval
res[in/out] the result of res - (a * b)

§ IntervalArithmetic_submul() [2/2]

template<class T >
void SurgSim::Math::IntervalArithmetic_submul ( const Interval< T > &  a,
const Interval< T > &  b,
Interval< T > *  res 
)

§ 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
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
a,b,c,dPoints to check for coplanarity.
Returns
true if the points are coplanar.

§ isNearZero()

template<typename T >
bool SurgSim::Math::isNearZero ( const T &  value,
const T &  epsilon = static_cast< T >(polynomial_epsilon) 
)

Define an utility function for comparing individual coefficients to 0.

Template Parameters
Tunderlying data type used as either the scalar or as the data type for the interval
Parameters
valuethe value to compare
epsilonthe tolerance
Returns
true if the value is within epsilon of 0

§ isPointInsideTriangle() [1/3]

template<class T , int MOpt>
bool SurgSim::Math::isPointInsideTriangle ( time,
const std::pair< Eigen::Matrix< T, 3, 1, MOpt >, Eigen::Matrix< T, 3, 1, MOpt >> &  P,
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,
Eigen::Matrix< T, 3, 1, MOpt > *  barycentricCoordinates 
)

Check if a point belongs to a triangle at a given time of their motion.

Template Parameters
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
timeThe time of coplanarity of the 4 points (P(t), A(t), B(t), C(t) are expected to be coplanar)
Pthe point motion (from first to second)
A,B,CThe triangle points motion (from first to second)
[out]barycentricCoordinatesThe barycentric coordinates of P(t) in ABC(t)
Returns
true if P(t) is inside the triangle ABC(t)

§ isPointInsideTriangle() [2/3]

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 is inside a 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
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
ptVertex of the point.
tv0,tv1,tv2Vertices of the triangle, must be in CCW.
tnNormal 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() [3/3]

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 is inside a 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
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
ptVertex of the point.
tv0,tv1,tv2Vertices of the triangle, must be in CCW.
Returns
true if pt lies inside the triangle tv0, tv1, tv2, false otherwise.

§ isPointOnTriangleEdge() [1/2]

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 
)
inline

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

Note
Use barycentricCoordinates() if you need the coordinates.
Template Parameters
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
ptVertex of the point.
tv0,tv1,tv2Vertices of the triangle, must be in CCW.
tnNormal of the triangle (must be of norm 1 and a,b,c CCW).
Returns
true if pt lies on the edge of the triangle tv0, tv1, tv2, false otherwise.

§ isPointOnTriangleEdge() [2/2]

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.

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
TAccuracy of the calculation, can usually be inferred.
MOptEigen Matrix options, can usually be inferred.
Parameters
ptVertex of the point.
tv0,tv1,tv2Vertices of the triangle, must be in CCW.
Returns
true if pt lies on the edge of the triangle tv0, tv1, tv2, false otherwise.

§ isSubnormal() [1/7]

bool SurgSim::Math::isSubnormal ( float  value)
inline

Check if a float value is subnormal.

Subnormal values have absolute values in the range std::numeric_limits<float>::denorm_min() <= x < std::numeric_limits<float>::min(), and can result in very slow floating point calculations under some conditions.

Parameters
valuethe value to check.
Returns
true if subnormal; false if not (normal, zero, infinite or NaN).

§ isSubnormal() [2/7]

bool SurgSim::Math::isSubnormal ( double  value)
inline

Check if a double value is subnormal.

Subnormal values have absolute values in the range std::numeric_limits<double>::denorm_min() <= x < std::numeric_limits<double>::min(), and can result in very slow floating point calculations under some conditions.

Parameters
valuethe value to check.
Returns
true if subnormal; false if not (normal, zero, infinite or NaN).

§ isSubnormal() [3/7]

template<typename T >
bool SurgSim::Math::isSubnormal ( const Eigen::DenseBase< T > &  value)
inline

Check if a matrix or a vector contains any subnormal floating-point values.

Subnormal values have absolute values in the range std::numeric_limits<T>::denorm_min() <= x < std::numeric_limits<T>::min(), and can result in very slow floating point calculations under some conditions.

Template Parameters
Tthe base type used to describe the matrix or vector. Can usually be deduced.
Parameters
valuethe matrix or vector value to check.
Returns
true if any value is subnormal; false if none are (i.e. each is normal, zero, infinite or NaN).

§ isSubnormal() [4/7]

template<typename T >
bool SurgSim::Math::isSubnormal ( const Eigen::QuaternionBase< T > &  value)
inline

Check if a quaternion contains any subnormal floating-point values.

Subnormal values have absolute values in the range std::numeric_limits<T>::denorm_min() <= x < std::numeric_limits<T>::min(), and can result in very slow floating point calculations under some conditions.

Template Parameters
Tthe base type used to describe the quaternion. Can usually be deduced.
Parameters
valuethe quaternion value to check.
Returns
true if any value is subnormal; false if none are (i.e. each is normal, zero, infinite or NaN).

§ isSubnormal() [5/7]

template<typename T >
bool SurgSim::Math::isSubnormal ( const Eigen::AngleAxis< T > &  value)
inline

Check if an angle/axis 3D rotation contains any subnormal floating-point values.

Subnormal values have absolute values in the range std::numeric_limits<T>::denorm_min() <= x < std::numeric_limits<T>::min(), and can result in very slow floating point calculations under some conditions.

Template Parameters
Tthe scalar type used to describe the rotation. Can usually be deduced.
Parameters
valuethe rotation value to check.
Returns
true if any value is subnormal; false if none are (i.e. each is normal, zero, infinite or NaN).

§ isSubnormal() [6/7]

template<typename T >
bool SurgSim::Math::isSubnormal ( const Eigen::Rotation2D< T > &  value)
inline

Check if a 2D rotation is described by an angle that is subnormal.

Subnormal values have absolute values in the range std::numeric_limits<T>::denorm_min() <= x < std::numeric_limits<T>::min(), and can result in very slow floating point calculations under some conditions.

Template Parameters
Tthe scalar type used to describe the rotation. Can usually be deduced.
Parameters
valuethe 2D rotation value to check.
Returns
true if the angle is subnormal; false if not (normal, zero, infinite or NaN).

§ isSubnormal() [7/7]

template<typename T , int D, int M, int O>
bool SurgSim::Math::isSubnormal ( const Eigen::Transform< T, D, M, O > &  value)
inline

Check if a transform contains any subnormal floating-point values.

Subnormal values have absolute values in the range std::numeric_limits<T>::denorm_min() <= x < std::numeric_limits<T>::min(), and can result in very slow floating point calculations under some conditions.

Template Parameters
Tthe scalar type used to describe the transform. Can usually be deduced.
Dthe dimension used to describe the transform. Can usually be deduced.
Mthe mode value used to describe the transform. Can usually be deduced.
Othe options value used to describe the transform. Can usually be deduced.
Parameters
valuethe transform value to check.
Returns
true if any value is subnormal; false if none are (i.e. each is normal, zero, infinite or NaN).

§ isValid() [1/7]

bool SurgSim::Math::isValid ( float  value)
inline

Check if a float value is valid.

Zero, subnormal and normal numbers are valid; infinities and NaNs are not.

Parameters
valuethe value to check.
Returns
true if valid, false if not.

§ isValid() [2/7]

bool SurgSim::Math::isValid ( double  value)
inline

Check if a double value is valid.

Zero, subnormal and normal numbers are valid; infinities and NaNs are not.

Parameters
valuethe value to check.
Returns
true if valid, false if not.

§ isValid() [3/7]

template<typename T >
bool SurgSim::Math::isValid ( const Eigen::DenseBase< T > &  value)
inline

Check if a matrix or a vector is valid.

These quantities are valid if all of their elements are valid. Zero, subnormal and normal numbers are valid; infinities and NaNs are not.

Template Parameters
Tthe base type used to describe the matrix or vector. Can usually be deduced.
Parameters
valuethe matrix or vector value to check.
Returns
true if valid, false if not.

§ isValid() [4/7]

template<typename T >
bool SurgSim::Math::isValid ( const Eigen::QuaternionBase< T > &  value)
inline

Check if a quaternion is valid.

Quaternions are valid if all of their components are valid. Zero, subnormal and normal numbers are valid; infinities and NaNs are not.

Template Parameters
Tthe base type used to describe the quaternion. Can usually be deduced.
Parameters
valuethe quaternion value to check.
Returns
true if valid, false if not.

§ isValid() [5/7]

template<typename T >
bool SurgSim::Math::isValid ( const Eigen::AngleAxis< T > &  value)
inline

Check if an angle/axis 3D rotation is valid.

Angle/axis rotations are valid if the angle and the axis components are valid. Zero, subnormal and normal numbers are valid; infinities and NaNs are not.

Template Parameters
Tthe scalar type used to describe the rotation. Can usually be deduced.
Parameters
valuethe rotation value to check.
Returns
true if valid, false if not.

§ isValid() [6/7]

template<typename T >
bool SurgSim::Math::isValid ( const Eigen::Rotation2D< T > &  value)
inline

Check if a 2D rotation is valid.

2D rotations are valid if the rotation angle is valid. Zero, subnormal and normal numbers are valid; infinities and NaNs are not.

Template Parameters
Tthe scalar type used to describe the rotation. Can usually be deduced.
Parameters
valuethe rotation value to check.
Returns
true if valid, false if not.

§ isValid() [7/7]

template<typename T , int D, int M, int O>
bool SurgSim::Math::isValid ( const Eigen::Transform< T, D, M, O > &  value)
inline

Check if a transform is valid.

Transforms are valid if all of their components are valid. Zero, subnormal and normal numbers are valid; infinities and NaNs are not.

Template Parameters
Tthe scalar type used to describe the transform. Can usually be deduced.
Dthe dimension used to describe the transform. Can usually be deduced.
Mthe mode value used to describe the transform. Can usually be deduced.
Othe options value used to describe the transform. Can usually be deduced.
Parameters
valuethe transform value to check.
Returns
true if valid, false if not.

§ makeAabb()

template<class Scalar , int Dim, int MType>
Eigen::AlignedBox<Scalar, Dim> SurgSim::Math::makeAabb ( const Eigen::Matrix< Scalar, Dim, 1, MType > &  vector0,
const Eigen::Matrix< Scalar, Dim, 1, MType > &  vector1,
const Eigen::Matrix< Scalar, Dim, 1, MType > &  vector2 
)

Convenience function for creating a bounding box from three vertices (e.g.

the vertices of a triangle)

Template Parameters
Scalarnumeric type
Dimdimension of the space to be used
MTypethe eigen type of the vectors
Returns
an AABB containing all the points passed

§ makeRigidTransform() [1/3]

template<typename M , typename V >
Eigen::Transform<typename M::Scalar, M::RowsAtCompileTime, Eigen::Isometry> SurgSim::Math::makeRigidTransform ( const Eigen::MatrixBase< M > &  rotation,
const Eigen::MatrixBase< V > &  translation 
)
inline

Create a rigid transform using the specified rotation matrix and translation.

Template Parameters
Mthe type used to describe the rotation matrix. Can usually be deduced.
Vthe type used to describe the translation vector. Can usually be deduced.
Parameters
rotationthe rotation matrix.
translationthe translation vector.
Returns
the transform with the specified rotation and translation.

§ makeRigidTransform() [2/3]

template<typename Q , typename V >
Eigen::Transform<typename Q::Scalar, 3, Eigen::Isometry> SurgSim::Math::makeRigidTransform ( const Eigen::QuaternionBase< Q > &  rotation,
const Eigen::MatrixBase< V > &  translation 
)
inline

Create a rigid transform using the specified rotation quaternion and translation.

Template Parameters
Qthe type used to describe the rotation quaternion. Can usually be deduced.
Vthe type used to describe the translation vector. Can usually be deduced.
Parameters
rotationthe rotation quaternion.
translationthe translation vector.
Returns
the transform with the specified rotation and translation.

§ makeRigidTransform() [3/3]

template<typename T , int VOpt>
Eigen::Transform<T, 3, Eigen::Isometry> SurgSim::Math::makeRigidTransform ( const Eigen::Matrix< T, 3, 1, VOpt > &  position,
const Eigen::Matrix< T, 3, 1, VOpt > &  center,
const Eigen::Matrix< T, 3, 1, VOpt > &  up 
)
inline

Make a rigid transform from a eye point a center view point and an up direction, does not check whether up is colinear with eye-center The original formula can be found at http://www.opengl.org/sdk/docs/man2/xhtml/gluLookAt.xml.

Template Parameters
typenameT T the numeric data type used for arguments and the return value. Can usually be deduced.
intVOpt VOpt the option flags (alignment etc.) used for the axis vector argument. Can be deduced.
Parameters
positionThe position of the object.
centerThe point to which the object should point.
upThe up vector to be used for this calculation.
Returns
a RigidTransform that locates the object at position rotated into the direction of center.

§ makeRigidTranslation()

template<typename V >
Eigen::Transform<typename V::Scalar, V::SizeAtCompileTime, Eigen::Isometry> SurgSim::Math::makeRigidTranslation ( const Eigen::MatrixBase< V > &  translation)
inline

Create a rigid transform using the identity rotation and the specified translation.

Template Parameters
Vthe type used to describe the translation vector. Can usually be deduced.
Parameters
translationthe translation vector.
Returns
the transform with the identity rotation and the specified translation.

§ makeRotationMatrix()

template<typename T , int VOpt>
Eigen::Matrix<T, 3, 3> SurgSim::Math::makeRotationMatrix ( const T &  angle,
const Eigen::Matrix< T, 3, 1, VOpt > &  axis 
)
inline

Create a rotation matrix corresponding to the specified angle (in radians) and axis.

Template Parameters
Tthe numeric data type used for arguments and the return value. Can usually be deduced.
VOptthe option flags (alignment etc.) used for the axis vector argument. Can be deduced.
Parameters
anglethe angle of the rotation, in radians.
axisthe axis of the rotation.
Returns
the rotation matrix.

§ makeRotationQuaternion()

template<typename T , int VOpt>
Eigen::Quaternion<T> SurgSim::Math::makeRotationQuaternion ( const T &  angle,
const Eigen::Matrix< T, 3, 1, VOpt > &  axis 
)
inline

Create a quaternion rotation corresponding to the specified angle (in radians) and axis.

Template Parameters
Tthe numeric data type used for arguments and the return value. Can usually be deduced.
VOptthe option flags (alignment etc.) used for the axis vector argument. Can be deduced.
Parameters
anglethe angle of the rotation, in radians.
axisthe axis of the rotation.
Returns
the rotation quaternion.

§ makeSkewSymmetricMatrix()

template<typename T , int VOpt>
Eigen::Matrix<T, 3, 3> SurgSim::Math::makeSkewSymmetricMatrix ( const Eigen::Matrix< T, 3, 1, VOpt > &  vector)
inline

Create a skew-symmetric matrix corresponding to the specified vector.

Skew-symmetric matrices are particularly useful for representing a portion of the vector cross-product.

Template Parameters
Tthe numeric data type used for arguments and the return value. Can usually be deduced.
VOptthe option flags (alignment etc.) used for the vector argument. Can be deduced.
Parameters
vectorthe vector to be transformed.
Returns
the skew-symmetric matrix corresponding with the vector argument.

§ minMax() [1/5]

template<class T >
void SurgSim::Math::minMax ( const T &  a1,
const T &  a2,
T *  minVal,
T *  maxVal 
)

Calculate the minimum and maximum of two values.

Template Parameters
Tunderlying type
Parameters
a1the first value
a2the second value
minVal[out] the minimum of a1 and a2
maxVal[out] the maximum of a1 and a2

§ minMax() [2/5]

template<class T >
void SurgSim::Math::minMax ( const T &  a1,
const T &  a2,
const T &  a3,
T *  minVal,
T *  maxVal 
)

Calculate the minimum and maximum of three values.

Template Parameters
Tunderlying type
Parameters
a1the first value
a2the second value
a3the third value
minVal[out] the minimum of a1, a2 and a3
maxVal[out] the maximum of a1, a2 and a3

§ minMax() [3/5]

template<class T >
void SurgSim::Math::minMax ( const T &  a1,
const T &  a2,
const T &  a3,
const T &  a4,
T *  minVal,
T *  maxVal 
)

Calculate the minimum and maximum of four values.

Template Parameters
Tunderlying type
Parameters
a1the first value
a2the second value
a3the third value
a4the fourth value
minVal[out] the minimum of a1, a2, a3 and a4
maxVal[out] the maximum of a1, a2, a3 and a4

§ minMax() [4/5]

template<class T >
void SurgSim::Math::minMax ( const T &  a1,
const T &  a2,
const T &  a3,
const T &  a4,
const T &  a5,
T *  minVal,
T *  maxVal 
)

Calculate the minimum and maximum of five values.

Template Parameters
Tunderlying type
Parameters
a1the first value
a2the second value
a3the third value
a4the fourth value
a5the fifth value
minVal[out] the minimum of a1, a2, a3, a4 and a5
maxVal[out] the maximum of a1, a2, a3, a4 and a5

§ minMax() [5/5]

template<class T >
void SurgSim::Math::minMax ( const T *  values,
int  numValues,
T *  minVal,
T *  maxVal 
)

Calculate the minimum and maximum of numValues values.

Template Parameters
Tunderlying type
Parameters
valuesa series of numValues values
numValuesthe number of values in the series
minVal[out] the minimum value in values
maxVal[out] the maximum value in values

§ 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
Tthe numeric data type used for the vector argument. Can usually be deduced.
VOptthe option flags (alignment etc.) used for the vector argument. Can be deduced.
Parameters
pointis the point under consideration.
segment0one point on the line
segment1second point on the line
Returns
the closest point on the line through the segment to the point under test

§ negate()

template<typename T , int QOpt>
Eigen::Quaternion<T, QOpt> SurgSim::Math::negate ( const Eigen::Quaternion< T, QOpt > &  q)
inline

Quaternion negation (i.e.

unary operator -)

Template Parameters
Tthe numeric data type used for arguments and the return value. Can usually be deduced.
QOptthe option flags (alignment etc.) used for the quaternion arguments. Can be deduced.
Parameters
qThe quaternion to negate
Returns
the negation of q (i.e. -q)

§ operator*() [1/5]

template<typename T , int N, int M>
Polynomial< T, N+M > SurgSim::Math::operator* ( const Polynomial< T, N > &  p,
const Polynomial< T, M > &  q 
)

Multiply two polynomials of arbitrary degree.

This current implementation is limited to a resulting polynomial of no more than degree 3 (i.e. N + M <= 3) because of limits in the underlying polynomial representations.

Template Parameters
Ndegree of the first polynomial
Mdegree of the second polynomial
Tunderlying data type over which the interval is defined.
Parameters
pfirst polynomial of degree N
qsecond polynomial of degree M
Returns
p * q as a polynomial of degree N + M

§ operator*() [2/5]

template<typename T >
Polynomial< T, 2 > SurgSim::Math::operator* ( const Polynomial< T, 1 > &  p,
const Polynomial< T, 1 > &  q 
)

Multiply two polynomials of degree 1.

Template Parameters
Tunderlying data type over which the interval is defined.
Parameters
pfirst polynomial of degree 1
qsecond polynomial of degree 1
Returns
p * q as a polynomial of degree 2

§ operator*() [3/5]

template<typename T >
Polynomial< T, 3 > SurgSim::Math::operator* ( const Polynomial< T, 2 > &  p,
const Polynomial< T, 1 > &  q 
)

Multiply two polynomials of degree 2 and 1 respectively.

Template Parameters
Tunderlying data type over which the interval is defined.
Parameters
pfirst polynomial of degree 2
qsecond polynomial of degree 1
Returns
p * q as a polynomial of degree 3

§ operator*() [4/5]

template<typename T >
Polynomial< T, 3 > SurgSim::Math::operator* ( const Polynomial< T, 1 > &  p,
const Polynomial< T, 2 > &  q 
)

Multiply two polynomials of degree 1 and 2 respectively.

Template Parameters
Tunderlying data type over which the interval is defined.
Parameters
pfirst polynomial of degree 1
qsecond polynomial of degree 2
Returns
p * q as a polynomial of degree 3

§ operator*() [5/5]

template<typename T >
Interval< T > SurgSim::Math::operator* ( v,
const Interval< T > &  i 
)
Template Parameters
Tunderlying type of the interval
Parameters
vthe scalar
ithe interval
Returns
the product of the scalar v and the interval i

§ operator+()

template<typename T >
Interval< T > SurgSim::Math::operator+ ( v,
const Interval< T > &  i 
)
Template Parameters
Tunderlying type of the interval
Parameters
vthe scalar
ithe interval
Returns
the sum of the scalar v and the interval i

§ operator<<() [1/5]

template<typename T >
std::ostream & SurgSim::Math::operator<< ( std::ostream &  o,
const LinearMotion< T > &  motion 
)

Write a textual version of a linear motion to an output stream.

Template Parameters
Tunderlying type of the linear motion
Parameters
othe ostream to be written to
motionthe motion to write
Returns
the active ostream

§ operator<<() [2/5]

template<typename T , int N>
std::ostream & SurgSim::Math::operator<< ( std::ostream &  o,
const LinearMotionND< T, N > &  motion 
)

Write a textual version of a linear motion group to an output stream.

Template Parameters
Tunderlying type of the linear motion
Nnumber of linear motions in the group
Parameters
othe ostream to be written to
motionthe motion group to write
Returns
the active ostream

§ operator<<() [3/5]

template<typename T , int N>
std::ostream & SurgSim::Math::operator<< ( std::ostream &  stream,
const Polynomial< T, N > &  p 
)
inline

Write a textual version of a Polynomial to an output stream.

Template Parameters
Tunderlying type of the polynomial coefficients
Ndegree of the polynomial
Parameters
streamthe ostream to be written to
pthe polynomial to write
Returns
the active stream

§ operator<<() [4/5]

template<typename T >
std::ostream & SurgSim::Math::operator<< ( std::ostream &  o,
const Interval< T > &  interval 
)

Write a textual version of the interval to an output stream.

Template Parameters
Tunderlying type of the interval
Parameters
othe ostream to be written to
intervalthe interval to write
Returns
the active ostream

§ operator<<() [5/5]

template<typename T , int N>
std::ostream & SurgSim::Math::operator<< ( std::ostream &  o,
const IntervalND< T, N > &  interval 
)

Write a textual version of an interval group to an output stream.

Template Parameters
Tunderlying type of the interval
Nnumber of intervals in the group
Parameters
othe ostream to be written to
intervalthe interval group to write
Returns
the active ostream

§ robustCrossProduct()

template<class T , int VOpt>
Eigen::Matrix<T, 3, 1, VOpt> SurgSim::Math::robustCrossProduct ( const std::array< Eigen::Matrix< T, 3, 1, VOpt >, 2 > &  p,
const std::array< Eigen::Matrix< T, 3, 1, VOpt >, 2 > &  q,
epsilon 
)

Calculate the best unit normal we can find in the direction of pXq for one of the endpoints of q.

Try multiple arrangements of the end points to reduce the artifacts when three of the vertices may be nearly collinear.

Parameters
psegment p
qsegment q
epsilonwhen the norm of p x q is above epsilon, the cross product is assumed to be valid. return the normalized cross product of p x q

§ setSubMatrix()

template<class Matrix , class SubMatrix >
void SurgSim::Math::setSubMatrix ( const SubMatrix &  subMatrix,
size_t  blockIdRow,
size_t  blockIdCol,
size_t  blockSizeRow,
size_t  blockSizeCol,
Matrix matrix 
)

Helper method to set a sub-matrix into a matrix, for the sake of clarity.

Template Parameters
MatrixThe matrix type
SubMatrixThe sub-matrix type
Parameters
subMatrixThe sub-matrix
blockIdRow,blockIdColThe block indices for row and column in matrix
blockSizeRow,blockSizeColThe size of the sub-matrix
[out]matrixThe matrix to set the sub-matrix into

§ setSubnormalToZero() [1/7]

bool SurgSim::Math::setSubnormalToZero ( float *  value)
inline

If the float value is subnormal, set it to zero.

Subnormal values have absolute values in the range std::numeric_limits<float>::denorm_min() <= x < std::numeric_limits<float>::min(), and can result in very slow floating point calculations under some conditions.

Parameters
[in,out]valuethe value to check and possibly modify.
Returns
true if the value was modified.

§ setSubnormalToZero() [2/7]

bool SurgSim::Math::setSubnormalToZero ( double *  value)
inline

If the double value is subnormal, set it to zero.

Subnormal values have absolute values in the range std::numeric_limits<double>::denorm_min() <= x < std::numeric_limits<double>::min(), and can result in very slow floating point calculations under some conditions.

Parameters
[in,out]valuethe value to check and possibly modify.
Returns
true if the value was modified.

§ setSubnormalToZero() [3/7]

template<typename T >
bool SurgSim::Math::setSubnormalToZero ( Eigen::DenseBase< T > *  value)
inline

Set all subnormal values in a matrix or a vector to zero.

Subnormal values have absolute values in the range std::numeric_limits<T>::denorm_min() <= x < std::numeric_limits<T>::min(), and can result in very slow floating point calculations under some conditions.

Template Parameters
Tthe base type used to describe the matrix or vector. Can usually be deduced.
Parameters
[in,out]valuethe matrix or vector value to check and possibly modify.
Returns
true if any value was modified.

§ setSubnormalToZero() [4/7]

template<typename T >
bool SurgSim::Math::setSubnormalToZero ( Eigen::QuaternionBase< T > *  value)
inline

Set all subnormal values in a quaternion to zero.

Subnormal values have absolute values in the range std::numeric_limits<T>::denorm_min() <= x < std::numeric_limits<T>::min(), and can result in very slow floating point calculations under some conditions.

Template Parameters
Tthe base type used to describe the quaternion. Can usually be deduced.
Parameters
[in,out]valuethe quaternion value to check and possibly modify.
Returns
true if any value was modified.

§ setSubnormalToZero() [5/7]

template<typename T >
bool SurgSim::Math::setSubnormalToZero ( Eigen::AngleAxis< T > *  value)
inline

Set all subnormal values in an angle/axis 3D rotation to zero.

Subnormal values have absolute values in the range std::numeric_limits<T>::denorm_min() <= x < std::numeric_limits<T>::min(), and can result in very slow floating point calculations under some conditions.

Template Parameters
Tthe scalar type used to describe the rotation. Can usually be deduced.
Parameters
[in,out]valuethe rotation value to check and possibly modify.
Returns
true if any value was modified.

§ setSubnormalToZero() [6/7]

template<typename T >
bool SurgSim::Math::setSubnormalToZero ( Eigen::Rotation2D< T > *  value)
inline

If the angle of a 2D rotation is subnormal, set it to zero.

Subnormal values have absolute values in the range std::numeric_limits<T>::denorm_min() <= x < std::numeric_limits<T>::min(), and can result in very slow floating point calculations under some conditions.

Template Parameters
Tthe scalar type used to describe the rotation. Can usually be deduced.
Parameters
[in,out]valuethe rotation value to check and possibly modify.
Returns
true if the value was modified.

§ setSubnormalToZero() [7/7]

template<typename T , int D, int M, int O>
bool SurgSim::Math::setSubnormalToZero ( Eigen::Transform< T, D, M, O > *  value)
inline

Set all subnormal values in a transform to zero.

Subnormal values have absolute values in the range std::numeric_limits<T>::denorm_min() <= x < std::numeric_limits<T>::min(), and can result in very slow floating point calculations under some conditions.

Template Parameters
Tthe base type used to describe the transform. Can usually be deduced.
Parameters
[in,out]valuethe transform value to check and possibly modify.
Returns
true if any value was modified.

§ setSubVector()

template<class Vector , class SubVector >
void SurgSim::Math::setSubVector ( const SubVector &  subVector,
size_t  blockId,
size_t  blockSize,
Vector vector 
)

Helper method to set a sub-vector into a vector, for the sake of clarity.

Template Parameters
VectorThe vector type
SubVectorThe sub-vector type
Parameters
subVectorThe sub-vector
blockIdThe block index in vector
blockSizeThe size of the sub-vector
[out]vectorThe vector to set the sub-vector into

§ skew()

template<typename T , int MOpt>
Eigen::Matrix<T, 3, 1> SurgSim::Math::skew ( const Eigen::Matrix< T, 3, 3, MOpt > &  matrix)
inline

Extract the unique vector from the skew-symmetric part of a given matrix.

Template Parameters
Tthe numeric data type used for arguments and the return value. Can usually be deduced.
MOptthe option flags (alignment etc.) used for the matrix argument. Can be deduced.
Parameters
matrixthe matrix to compute the skew symmetric part from.
Returns
the unique vector defining the skew-symmetric part of the matrix.
Note
For any vector u, skew(makeSkewSymmetricMatrix(u)) = u
In general, returns the vector of the skew symmetric part of matrix: (matrix - matrix^T)/2

§ solve() [1/2]

template<typename T , int N>
void SurgSim::Math::solve ( const T &  a,
const T &  b,
const T &  epsilon,
int *  numRoots,
std::array< T, N > *  roots 
)

Specialized solve routine for linear polynomials (2 coefficients)

Template Parameters
Nmaximum number of roots that can be stored
Ttype of the coefficients and computations
Parameters
acoefficient of the linear term
bcoefficient of the constant term
epsilontolerance parameter for determining the number of valid, unique roots
numRoots[out] number of roots calculated, or DEGENERATE if there are infinitely many
roots[out] array containing the calculated roots ordered ascendingly
Exceptions
ifthere are more than N roots

§ solve() [2/2]

template<typename T , int N>
void SurgSim::Math::solve ( const T &  a,
const T &  b,
const T &  c,
const T &  epsilon,
int *  numRoots,
std::array< T, N > *  roots 
)

Specialized solve routine for quadratic polynomials (3 coefficients)

Template Parameters
Nmaximum number of roots that can be stored
Ttype of the coefficients and computations
Parameters
acoefficient of the square term
bcoefficient of the linear term
ccoefficient of the constant term
epsilontolerance parameter for determining the number of valid, unique roots
numRoots[out] number of roots calculated, or DEGENERATE if there are infinitely many
roots[out] array containing the calculated roots ordered ascendingly
Exceptions
ifthere are more than N roots

§ square() [1/2]

template<typename T >
Polynomial< T, 0 > SurgSim::Math::square ( const Polynomial< T, 0 > &  p)

Square a degree 0 polynomial.

Template Parameters
Tunderlying data type over which the interval is defined.
Parameters
ppolynomial of degree 0
Returns
p^2 as a polynomial of degree 0

§ square() [2/2]

template<typename T >
Polynomial< T, 2 > SurgSim::Math::square ( const Polynomial< T, 1 > &  p)

Square a degree 1 polynomial.

Template Parameters
Tunderlying data type over which the interval is defined.
Parameters
ppolynomial of degree 1
Returns
p^2 as a polynomial of degree 2

§ 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
TThe scalar type
MOptThe matrix options
Parameters
A,B,C,Dthe 4 point' motion (each has a pair from -> to)
[out]timesOfCoplanarityThe 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

§ transformAabb()

template<class Scalar , int Dim>
Eigen::AlignedBox<Scalar, Dim> SurgSim::Math::transformAabb ( const Eigen::Transform< Scalar, Dim, Eigen::Isometry > &  transform,
const Eigen::AlignedBox< Scalar, Dim > &  aabb 
)

Rotate the extrema of the aabb, note that that will extend the size of the box.

Template Parameters
Scalarnumeric type
Dimdimension of the space to be used
Parameters
transformThe Rigidtransform to use
aabbthe aabb to transform
Returns
the transformed aabb

§ tripleProduct()

template<class T >
Interval<T> SurgSim::Math::tripleProduct ( const LinearMotionND< T, 3 > &  a,
const LinearMotionND< T, 3 > &  b,
const LinearMotionND< T, 3 > &  c,
const Interval< T > &  range 
)

Calculate the triple product, as an interval.

Template Parameters
Tunderlying type of the linear motion
Parameters
athe first linear motion 3 group
bthe second linear motion 3 group
cthe third linear motion 3 group
rangethe range over which the triple product is to be evaluated
Returns
an interval representation of the triple product

§ valuesOverInterval()

template<class T , int N>
Interval< T > SurgSim::Math::valuesOverInterval ( const Polynomial< T, N > &  p,
const Interval< T > &  interval 
)

Calculate the minimum and maximum values of the dependent variable over a specified range of the independent variable.

Template Parameters
Ndegree of the polynomial being managed
Ttype of the coefficients and computations
Parameters
ppolynomial on which the value calculations are based
intervalan interval on the independent variable over which the values are to be calculated
Returns
the minimum and maximum polynomial values over interval

§ zeroColumn() [1/2]

template<class Derived >
void SurgSim::Math::zeroColumn ( size_t  column,
Eigen::DenseBase< Derived > *  matrix 
)

Helper method to zero a column of a matrix.

Template Parameters
MatrixThe matrix type
Parameters
columnThe column to set to zero
[in,out]matrixThe matrix to set the zero column on.

§ zeroColumn() [2/2]

template<typename T , int Opt, typename Index >
void SurgSim::Math::zeroColumn ( size_t  column,
Eigen::SparseMatrix< T, Opt, Index > *  matrix 
)
inline

Helper method to zero a column of a matrix specialized for Sparse Matrices.

Parameters
columnThe column to set to zero
[in,out]matrixThe matrix to set the zero column on.

§ zeroRow() [1/2]

template<class Derived >
void SurgSim::Math::zeroRow ( size_t  row,
Eigen::DenseBase< Derived > *  matrix 
)

Helper method to zero a row of a matrix.

Template Parameters
MatrixThe matrix type
Parameters
rowThe row to set to zero
[in,out]matrixThe matrix to set the zero row on.

§ zeroRow() [2/2]

template<typename T , int Opt, typename Index >
void SurgSim::Math::zeroRow ( size_t  row,
Eigen::SparseMatrix< T, Opt, Index > *  matrix 
)

Helper method to zero a row of a matrix specialized for Sparse Matrices.

Parameters
rowThe row to set to zero
[in,out]matrixThe matrix to set the zero row on.

Variable Documentation

§ gaussQuadrature100Points

const std::array< gaussQuadraturePoint, 100 > SurgSim::Math::gaussQuadrature100Points

1D 100-points Gauss-Legendre quadrature \({<x_1, w_1>, <x_2, w_2>, <x_3, w_3>, ..., <x_{100}, w_{100}>}\)

Note
Gauss-Legendre quadrature numerically evaluates the integral of a function \(f\) with a finite sum using some weights and specific points of evaluation of the function \(f\):
\(\int_{-1}^{+1} f(x) dx = \sum_{i=1}^n w_i f(x_i)\)
n is the number of points used to discretized the integral
\(x_i\) is the point to evaluate the function \(f\) with
\(w_i\) is the weight to assign to the function evaluation at the given point \(x_i\)

§ gaussQuadrature1Point

const std::array< gaussQuadraturePoint, 1 > SurgSim::Math::gaussQuadrature1Point

1D 1-point Gauss-Legendre quadrature \({<x_1, w_1>}\)

Note
Gauss-Legendre quadrature numerically evaluates the integral of a function \(f\) with a finite sum using some weights and specific points of evaluation of the function \(f\):
\(\int_{-1}^{+1} f(x) dx = \sum_{i=1}^n w_i f(x_i)\)
n is the number of points used to discretized the integral
\(x_i\) is the point to evaluate the function \(f\) with
\(w_i\) is the weight to assign to the function evaluation at the given point \(x_i\)

§ gaussQuadrature2DTriangle12Points

const std::array< gaussQuadratureTrianglePoint, 12 > SurgSim::Math::gaussQuadrature2DTriangle12Points

2D triangle Gauss-Legendre quadrature 12-points \({<\xi_1, \eta_1, w_1>, ..., <\xi_{12}, \eta_{12}, w_{12}>}\)

Note
Gauss-Legendre quadrature numerically evaluates the integral of a function \(f(\xi, \eta)\) with a finite sum using some weights and specific points on the triangle.
\(\int_{0}^{1} \int_{0}^{1-\eta} f(\xi, \eta) d\xi d\eta = \sum_{i=1}^n w_i f(\xi_i, \eta_i)\)
n is the number of points used to discretized the integral
\((\xi_i, \eta_i)\) is the parametrized location of the triangle point to evaluate the function with
\(w_i\) is the weight to assign to the function evaluation at the given point
A 12-points Gauss-Legendre quadrature on the triangle is exact for polynomial functions of degree 6 or less.

§ gaussQuadrature2DTriangle3Points

const std::array< gaussQuadratureTrianglePoint, 3 > SurgSim::Math::gaussQuadrature2DTriangle3Points

2D triangle Gauss-Legendre quadrature 3-points \({<\xi_1, \eta_1, w_1>, ..., <\xi_3, \eta_3, w_3>}\)

Note
Gauss-Legendre quadrature numerically evaluates the integral of a function \(f(\xi, \eta)\) with a finite sum using some weights and specific points on the triangle.
\(\int_{0}^{1} \int_{0}^{1-\eta} f(\xi, \eta) d\xi d\eta = \sum_{i=1}^n w_i f(\xi_i, \eta_i)\)
n is the number of points used to discretized the integral
\((\xi_i, \eta_i)\) is the parametrized location of the triangle point to evaluate the function with
\(w_i\) is the weight to assign to the function evaluation at the given point
A 3-points Gauss-Legendre quadrature on the triangle is exact for polynomial functions of degree 2 or less.

§ gaussQuadrature2DTriangle6Points

const std::array< gaussQuadratureTrianglePoint, 6 > SurgSim::Math::gaussQuadrature2DTriangle6Points

2D triangle Gauss-Legendre quadrature 6-points \({<\xi_1, \eta_1, w_1>, ..., <\xi_6, \eta_6, w_6>}\)

Note
Gauss-Legendre quadrature numerically evaluates the integral of a function \(f(\xi, \eta)\) with a finite sum using some weights and specific points on the triangle.
\(\int_{0}^{1} \int_{0}^{1-\eta} f(\xi, \eta) d\xi d\eta = \sum_{i=1}^n w_i f(\xi_i, \eta_i)\)
n is the number of points used to discretized the integral
\((\xi_i, \eta_i)\) is the parametrized location of the triangle point to evaluate the function with
\(w_i\) is the weight to assign to the function evaluation at the given point
A 6-points Gauss-Legendre quadrature on the triangle is exact for polynomial functions of degree 4 or less.

§ gaussQuadrature2Points

const std::array< gaussQuadraturePoint, 2 > SurgSim::Math::gaussQuadrature2Points

1D 2-points Gauss-Legendre quadrature \({<x_1, w_1>, <x_2, w_2>}\)

Note
Gauss-Legendre quadrature numerically evaluates the integral of a function \(f\) with a finite sum using some weights and specific points of evaluation of the function \(f\):
\(\int_{-1}^{+1} f(x) dx = \sum_{i=1}^n w_i f(x_i)\)
n is the number of points used to discretized the integral
\(x_i\) is the point to evaluate the function \(f\) with
\(w_i\) is the weight to assign to the function evaluation at the given point \(x_i\)

§ gaussQuadrature3Points

const std::array< gaussQuadraturePoint, 3 > SurgSim::Math::gaussQuadrature3Points

1D 3-points Gauss-Legendre quadrature \({<x_1, w_1>, <x_2, w_2>, <x_3, w_3>}\)

Note
Gauss-Legendre quadrature numerically evaluates the integral of a function \(f\) with a finite sum using some weights and specific points of evaluation of the function \(f\):
\(\int_{-1}^{+1} f(x) dx = \sum_{i=1}^n w_i f(x_i)\)
n is the number of points used to discretized the integral
\(x_i\) is the point to evaluate the function \(f\) with
\(w_i\) is the weight to assign to the function evaluation at the given point \(x_i\)

§ gaussQuadrature4Points

const std::array< gaussQuadraturePoint, 4 > SurgSim::Math::gaussQuadrature4Points

1D 4-points Gauss-Legendre quadrature \({<x_1, w_1>, <x_2, w_2>, <x_3, w_3>, <x_4, w_4>}\)

Note
Gauss-Legendre quadrature numerically evaluates the integral of a function \(f\) with a finite sum using some weights and specific points of evaluation of the function \(f\):
\(\int_{-1}^{+1} f(x) dx = \sum_{i=1}^n w_i f(x_i)\)
n is the number of points used to discretized the integral
\(x_i\) is the point to evaluate the function \(f\) with
\(w_i\) is the weight to assign to the function evaluation at the given point \(x_i\)

§ gaussQuadrature5Points

const std::array< gaussQuadraturePoint, 5 > SurgSim::Math::gaussQuadrature5Points

1D 5-points Gauss-Legendre quadrature \({<x_1, w_1>, <x_2, w_2>, <x_3, w_3>, <x_4, w_4>, <x_5, w_5>}\)

Note
Gauss-Legendre quadrature numerically evaluates the integral of a function \(f\) with a finite sum using some weights and specific points of evaluation of the function \(f\):
\(\int_{-1}^{+1} f(x) dx = \sum_{i=1}^n w_i f(x_i)\)
n is the number of points used to discretized the integral
\(x_i\) is the point to evaluate the function \(f\) with
\(w_i\) is the weight to assign to the function evaluation at the given point \(x_i\)

§ IntegrationSchemeNames

const std::unordered_map<IntegrationScheme, std::string, std::hash<int> > SurgSim::Math::IntegrationSchemeNames

§ LinearSolverNames

const std::unordered_map<LinearSolver, std::string, std::hash<int> > SurgSim::Math::LinearSolverNames