fmat Namespace Reference

fixed-size matrix routines for high performance with small allocations More...

Namespaces
namespace	fmat_internal
Classes
struct	Length_mismatch_or_non_vector
struct	NonSquare_Matrix
struct	SubVector_is_longer_than_source
struct	SubMatrix_has_more_rows_than_source
struct	SubMatrix_has_more_columns_than_source
struct	Rotation_requires_larger_dimensions
struct	Matrix_multiplication_left_width_must_match_right_height
class	SubVector
class	SubMatrix
class	Matrix
class	Column
class	Row
class	QuaternionT
	Quaternions can be more efficient and more stable for a series of rotations than a corresponding 3x3 matrix, also more compact storage. More...
class	TransformT
	Efficient computation of affine transform operations. More...
Typedefs
typedef float	fmatReal
typedef QuaternionT	Quaternion
typedef TransformT	Transform
Functions
const Column< 4 >	ZEROH (fmat::pack(0, 0, 0, 1))
const Column< 2 >	UNIT2_X (fmat::pack(1, 0))
const Column< 2 >	UNIT2_Y (fmat::pack(0, 1))
const Column< 3 >	UNIT3_X (fmat::pack(1, 0, 0))
const Column< 3 >	UNIT3_Y (fmat::pack(0, 1, 0))
const Column< 3 >	UNIT3_Z (fmat::pack(0, 0, 1))
template<typename R >
Column< 2, R >	packT (R x, R y)
	templated version to pack non fmatReal type, using a different name 'packT' so it needs to be explicitly called, e.g. pack(0,0,1) is generally intended as fmatReal, not int
template<typename R >
Column< 3, R >	packT (R x, R y, R z)
template<typename R >
Column< 4, R >	packT (R x, R y, R z, R d)
Column< 2, fmatReal >	pack (fmatReal x, fmatReal y)
	generic packing of N values into a Column<N> (note assumes fmatReal, see packT()
Column< 3, fmatReal >	pack (fmatReal x, fmatReal y, fmatReal z)
Column< 4, fmatReal >	pack (fmatReal x, fmatReal y, fmatReal z, fmatReal d)
template<typename R >
std::string	fullfmt (const R *const data, size_t H, size_t W, std::string const &numberFormat, std::string const &firstLineStart, std::string const &nextLineStart, std::string const &lastLineEnd, std::string const &rowBegin, std::string const &elementSep, std::string const &rowEnd, std::string const &rowSep)
template<template< size_t N, typename R > class T, size_t N, typename R1 , typename R2 >
Column< N+1, typename fmat_internal::unconst< R1 > ::type >	pack (const T< N, R1 > &x, R2 y)
	packing columns: appending an element or concatenating two columns
template<template< size_t N, typename R > class T, size_t N, typename R1 , typename R2 >
Column< N+1, typename fmat_internal::unconst< R2 > ::type >	pack (R1 x, const T< N, R2 > &y)
template<template< size_t N, typename R > class T1, template< size_t N, typename R > class T2, size_t N1, size_t N2, typename R1 , typename R2 >
Column< N1+N2, typename fmat_internal::promotion_trait < R1, R2 >::type >	pack (const T1< N1, R1 > &x, const T2< N2, R2 > &y)
template<size_t N, typename R1 , typename R2 >
Column< N+1, typename fmat_internal::unconst< R1 > ::type >	pack (const Column< N, R1 > &x, R2 y)
template<size_t N, typename R1 , typename R2 >
Column< N+1, typename fmat_internal::unconst< R2 > ::type >	pack (R1 x, const Column< N, R2 > &y)
template<size_t N1, size_t N2, typename R >
Column< N1+N2, typename fmat_internal::unconst< R > ::type >	pack (const Column< N1, R > &x, const Column< N2, R > &y)
template<size_t N, typename R1 , typename R2 >
Row< N+1, typename fmat_internal::unconst< R1 > ::type >	pack (const Row< N, R1 > &x, R2 y)
template<size_t N, typename R1 , typename R2 >
Row< N+1, typename fmat_internal::unconst< R2 > ::type >	pack (R1 x, const Row< N, R2 > &y)
template<size_t N1, size_t N2, typename R >
Row< N1+N2, typename fmat_internal::unconst< R > ::type >	pack (const Row< N1, R > &x, const Row< N2, R > &y)
template<size_t N, typename R >
std::ostream &	operator<< (std::ostream &os, const SubVector< N, R > &x)
template<size_t H, size_t W, typename R >
std::ostream &	operator<< (std::ostream &os, const SubMatrix< H, W, R > &x)
template<size_t H, size_t W, typename R >
std::ostream &	operator<< (std::ostream &os, const Matrix< H, W, R > &x)
template<size_t N, typename R >
std::ostream &	operator<< (std::ostream &os, const Column< N, R > &x)
template<size_t N, typename R >
std::ostream &	operator<< (std::ostream &os, const Row< N, R > &x)
template<template< size_t H, size_t W, typename R > class T1, template< size_t H, size_t W, typename R > class T2, size_t H, size_t N, size_t W, typename R1 , typename R2 >
Matrix< H, W, typename fmat_internal::promotion_trait < R1, R2 >::type >	operator* (const T1< H, N, R1 > &a, const T2< N, W, R2 > &b)
template<template< size_t H, size_t W, typename R > class T1, template< size_t H, size_t W, typename R > class T2, size_t D, typename R1 , typename R2 >
T1< D, D, R1 > &	operator*= (T1< D, D, R1 > &a, const T2< D, D, R2 > &b)
template<template< size_t H, size_t W, typename R > class T2, size_t D, typename R1 , typename R2 >
const SubMatrix< D, D, R1 > &	operator*= (const SubMatrix< D, D, R1 > &a, const T2< D, D, R2 > &b)
template<template< size_t H, size_t W, typename R > class T1, template< size_t N, typename R > class T2, size_t H, size_t N, typename R1 , typename R2 >
Column< H, typename fmat_internal::promotion_trait < R1, R2 >::type >	operator* (const T1< H, N, R1 > &a, const T2< N, R2 > &b)
template<template< size_t W, typename R > class T1, template< size_t H, size_t W, typename R > class T2, size_t N, size_t W, typename R1 , typename R2 >
Row< W, typename fmat_internal::promotion_trait < R1, R2 >::type >	operator* (const T1< N, R1 > &a, const T2< N, W, R2 > &b)
template<template< size_t N, typename R > class T, size_t N, typename R >
fmat::Column< N, typename fmat_internal::unconst< R > ::type >	abs (const T< N, R > &v)
template<template< size_t H, size_t W, typename R > class T, size_t H, size_t W, typename R >
fmat::Matrix< H, W, typename fmat_internal::unconst< R > ::type >	abs (const T< H, W, R > &m)
template<typename R >
R	atan (const Column< 2, R > &v)
template<typename R >
R	atan (const Row< 2, R > &v)
template<typename R >
R	atan (const SubVector< 2, R > &v)
template<template< size_t N, typename R > class T1, template< size_t N, typename R > class T2, size_t N, typename R1 , typename R2 >
fmat_internal::promotion_trait < R1, R2 >::type	dotProduct (const T1< N, R1 > &a, const T2< N, R2 > &b)
template<class Ta , class Tb >
fmat_internal::promotion_trait < typename Ta::storage_t, typename Tb::storage_t >::type	dotProduct (const Ta &a, const Tb &b)
template<class Ta , class Tb >
Column< 3, typename fmat_internal::promotion_trait < typename Ta::storage_t, typename Tb::storage_t >::type >	crossProduct (const Ta &a, const Tb &b)
template<template< size_t H, size_t W, typename R > class M, typename R >
fmat_internal::unconst< R >::type	determinant (const M< 2, 2, R > &m)
template<template< size_t H, size_t W, typename R > class M, typename R >
fmat_internal::unconst< R >::type	determinant (const M< 3, 3, R > &m)
template<typename M >
Matrix< M::HEIGHT, M::WIDTH, typename fmat_internal::unconst < typename M::storage_t > ::type >	invert (const M &m)
	Computes and returns the inverse of a square matrix using Gauss-Jordan elimination.
template<template< size_t H, size_t W, typename R > class T, typename R >
R	det (const T< 2, 2, R > &m)
	returns the determinant for 2x2 matrices
template<template< size_t H, size_t W, typename R > class T, typename R >
R	det (const T< 3, 3, R > &m)
	returns the determinant for 3x3 matrices
template<template< size_t N, typename R > class V1, template< size_t N, typename R > class V2, typename R1 , typename R2 >
fmat_internal::promotion_trait < R1, R2 >::type	dotProduct (const V1< 2, R1 > &a, const V2< 2, R2 > &b)
template<template< size_t N, typename R > class V1, template< size_t N, typename R > class V2, typename R1 , typename R2 >
fmat_internal::promotion_trait < R1, R2 >::type	dotProduct (const V1< 3, R1 > &a, const V2< 3, R2 > &b)
template<template< size_t N, typename R > class V1, template< size_t N, typename R > class V2, typename R1 , typename R2 >
fmat_internal::promotion_trait < R1, R2 >::type	dotProduct (const V1< 4, R1 > &a, const V2< 4, R2 > &b)
template<typename R >
Matrix< 2, 2, R >	rotation2DT (R rad)
	returns 2x2 rotation matrix (i.e. implied rotation about Z), for angle rad in radians
Matrix< 2, 2, fmatReal >	rotation2D (fmatReal rad)
	returns 2x2 rotation matrix for angle rad (in radians) about Z axis
template<size_t N, typename R >
Matrix< N, N, R >	rotationXN (R rad)
	returns NxN rotation matrix for angle rad (in radians) about X axis (only uses upper 3x3)
Matrix< 3, 3, fmatReal >	rotationX (fmatReal rad)
	returns 3x3 rotation matrix for angle rad (in radians) about X axis
template<size_t N, typename R >
Matrix< N, N, R >	rotationYN (R rad)
	returns NxN rotation matrix for angle rad (in radians) about Y axis (only uses upper 3x3)
Matrix< 3, 3, fmatReal >	rotationY (fmatReal rad)
	returns 3x3 rotation matrix for angle rad (in radians) about Z axis
template<size_t N, typename R >
Matrix< N, N, R >	rotationZN (R rad)
	returns NxN rotation matrix for angle rad (in radians) about Z axis (only uses upper 2x2)
Matrix< 3, 3, fmatReal >	rotationZ (fmatReal rad)
	returns 3x3 rotation matrix for angle rad (in radians) about Z axis
template<class Ra , class Rb >
QuaternionT< typename fmat_internal::promotion_trait < Ra, Rb >::type >	crossProduct (const QuaternionT< Ra > &p, const QuaternionT< Rb > &q)
	returns the rotation needed to transform p into q (this is the core of a 'slerp' implementation)
template<typename R >
QuaternionT< R >	invert (const QuaternionT< R > &q)
template<typename R >
Matrix< 4, 4, R >	invertTransform (const Matrix< 4, 4, R > &t)
	invert the matrix, taking advantage of known structure:
template<typename R >
TransformT< R >	invert (const TransformT< R > &m)
template<typename R >
Column< 3, R >	ypr (const QuaternionT< R > &q)
	Returns yaw-pitch-roll aka heading-elevation-bank conversion, where roll-pitch-yaw correspond to compounding rotations about the global x, y, and z axis respectively (in that order).
template<typename R >
Column< 3, R >	ypr (const TransformT< R > &m)
	Returns yaw-pitch-roll aka heading-elevation-bank conversion, where roll-pitch-yaw correspond to compounding rotations about the global x, y, and z axis respectively (in that order).
template<template< size_t, size_t, typename R > class M, typename R >
Column< 3, typename fmat_internal::unconst< R > ::type >	ypr (const M< 3, 3, R > &m)
	Returns yaw-pitch-roll aka heading-elevation-bank conversion, where roll-pitch-yaw correspond to compounding rotations about the global x, y, and z axis respectively (in that order).
template<typename R >
Column< 2, R >	segmentIntersection (const Column< 2, R > &p1, const Column< 2, R > &d1, const Column< 2, R > &p2, const Column< 2, R > &d2)
	Returns the scaling factors for d1 and d2 through p1 and p2 respectively to reach common intersection.
template<typename R >
R	rayIntersection (const Column< 2, R > &p1, const Column< 2, R > &d1, const Column< 2, R > &p2, const Column< 2, R > &d2)
	Returns the scaling factor of d1 from p1 to reach intersection of d2 through p2.
template<typename R >
Column< 2, R >	lineIntersection (const Column< 2, R > &p1, const Column< 2, R > &d1, const Column< 2, R > &p2, const Column< 2, R > &d2)
	Returns the point of intersection of d1 through p1 and d2 through p2.
template<template< size_t, typename R > class V, typename R >
Column< 2, typename V< 2, R > ::out_t >	normalLeft (const V< 2, R > &v)
	Returns the orthogonal left vector (rotate by 90°).
template<template< size_t, typename R > class V, typename R >
Column< 2, typename V< 2, R > ::out_t >	normalRight (const V< 2, R > &v)
	Returns the orthogonal right vector (rotate by -90°).
Variables
std::string	defaultNumberFormat = "%g"
const Column< 2 >	ZERO2
	a length 2 column vector with all zeros (could also just use Column<2>(), but this is more semantic)
const Column< 3 >	ZERO3
	a length 3 column vector with all zeros (could also just use Column<3>(), but this is more semantic)
const Column< 4 >	ZEROH
	a length 4 column vector representing zero in homogenous coordinates (0,0,0,1)
const Column< 2 >	UNIT2_X
	a length 2 column with '1' in the first dimension
const Column< 2 >	UNIT2_Y
	a length 2 column with '1' in the second dimension
const Column< 3 >	UNIT3_X
	a length 3 column with '1' in the first dimension
const Column< 3 >	UNIT3_Y
	a length 3 column with '1' in the second dimension
const Column< 3 >	UNIT3_Z
	a length 3 column with '1' in the third dimension

Detailed Description

fixed-size matrix routines for high performance with small allocations

Typedef Documentation

typedef float fmat::fmatReal

Definition at line 23 of file fmatCore.h.

typedef QuaternionT fmat::Quaternion

Definition at line 399 of file fmatSpatial.h.

typedef TransformT fmat::Transform

Definition at line 608 of file fmatSpatial.h.

Function Documentation

template<template< size_t H, size_t W, typename R > class T, size_t H, size_t W, typename R >

fmat::Matrix<H,W,typename fmat_internal::unconst<R>::type> fmat::abs ( const T< H, W, R > & m )

Definition at line 1198 of file fmatCore.h.

template<template< size_t N, typename R > class T, size_t N, typename R >

fmat::Column<N,typename fmat_internal::unconst<R>::type> fmat::abs ( const T< N, R > & v )

Definition at line 1187 of file fmatCore.h.

Referenced by abs(), Grasper::MoveArm::advTime(), HingePositionInterface::applyResponse(), Kinematics::baseToLocal(), HeadPointerMC::clipAngularRange(), ArmMC::clipAngularRange(), CylindricalObstacle::collides(), BoxObstacle::collides(), LinkComponent::computeBB2D(), XWalkMC::computeCurrentBodyOffset(), XWalkMC::computeCurrentPosition(), IKThreeLink::computeFirstLinkRevolute(), WaypointEngine::computeIdeal(), LinkComponent::computeOwnAABB(), IKThreeLink::computeSecondLinkRevolute(), IKThreeLink::computeThirdLinkPrismatic(), IKThreeLink::computeThirdLinkRevolute(), RecordMotionNode::doEvent(), DualCoding::Pilot::PushObjectMachine::ChoosePathObjToDest::doStart(), DualCoding::Pilot::PushObjectMachine::ChoosePathInitToObj::doStart(), ArmController::doStart(), DRanNormalZig32(), DRanNormalZig32Vec(), Graphics::drawLine(), Graphics::drawQuarterEllipse(), AprilTags::Edge::edgeCost(), DualCoding::MapBuilder::filterGroundShapes(), fmat::QuaternionT< R >::fromAxisAngle(), plist::Angle::get(), KDTree::getBestNKeypointMatch(), DualCoding::MapBuilder::getCamCrosses(), XWalkParameters::getMaxAVel(), LinkComponent::getObstacle(), hashHoughKey(), GridWorld::heuristic(), invert(), XWalkMC::isDirty(), MotionSequenceEngine::makeSafe(), AprilTags::MathUtil::mod2pi(), BallDetectionGenerator::pct_from_mean(), TorqueCalibrate::TakeMeasurementControl::processEvent(), Kinematics::projectToPlane(), EllipticalObstacle::reset(), XWalkMC::resetPeriod(), HeadController::runCommand(), RRTNodeXYTheta::safeTurn(), plist::Angle::saveXML(), XWalkMC::setTargetDisplacement(), XWalkMC::setTargetVelocity(), IKThreeLink::solve(), IKGradientSolver::solve(), IKCalliope::solve(), IKGradientSolver::step(), and XWalkMC::updateOutputsWalking().

template<typename R >

R fmat::atan ( const SubVector< 2, R > & v )

Definition at line 1212 of file fmatCore.h.

template<typename R >

R fmat::atan ( const Row< 2, R > & v )

Definition at line 1211 of file fmatCore.h.

template<typename R >

R fmat::atan ( const Column< 2, R > & v )

Definition at line 1210 of file fmatCore.h.

Referenced by EllipticalObstacle::getBoundingBox(), IKCalliope::IKCalliope(), IKCalliope::pointToward(), IKCalliope::solve(), and IKCalliope::twoLinkIK().

template<class Ra , class Rb >

QuaternionT<typename fmat_internal::promotion_trait<Ra,Rb>::type> fmat::crossProduct	(	const QuaternionT< Ra > &	p,
		const QuaternionT< Rb > &	q
	)

returns the rotation needed to transform p into q (this is the core of a 'slerp' implementation)

Definition at line 404 of file fmatSpatial.h.

template<class Ta , class Tb >

Column<3,typename fmat_internal::promotion_trait<typename Ta::storage_t,typename Tb::storage_t>::type> fmat::crossProduct	(	const Ta &	a,
		const Tb &	b
	)

Definition at line 1237 of file fmatCore.h.

Referenced by IKSolver::Parallel::computeErrorGradient(), IKSolver::Rotation::computeErrorGradient(), GaitedFootsteps::expand(), KinematicJoint::getJointJacobian(), AprilTags::TagDetection::getRotMatrix(), ConvexPolyObstacle::hull(), GJK::processTetrahedron(), GJK::processTriangle(), and IKGradientSolver::step().

template<template< size_t H, size_t W, typename R > class T, typename R >

R fmat::det ( const T< 3, 3, R > & m )

returns the determinant for 3x3 matrices

Definition at line 1324 of file fmatCore.h.

template<template< size_t H, size_t W, typename R > class T, typename R >

R fmat::det ( const T< 2, 2, R > & m )

returns the determinant for 2x2 matrices

Definition at line 1318 of file fmatCore.h.

Referenced by RawImage::imageScore(), AprilTags::GLine2D::intersectionWith(), and OpticalFlow::iterativeLucasKanade().

template<template< size_t H, size_t W, typename R > class M, typename R >

fmat_internal::unconst<R>::type fmat::determinant ( const M< 3, 3, R > & m )

Definition at line 1253 of file fmatCore.h.

template<template< size_t H, size_t W, typename R > class M, typename R >

fmat_internal::unconst<R>::type fmat::determinant ( const M< 2, 2, R > & m )

Definition at line 1247 of file fmatCore.h.

template<template< size_t N, typename R > class V1, template< size_t N, typename R > class V2, typename R1 , typename R2 >

fmat_internal::promotion_trait<R1,R2>::type fmat::dotProduct	(	const V1< 4, R1 > &	a,
		const V2< 4, R2 > &	b
	)

Definition at line 1339 of file fmatCore.h.

template<template< size_t N, typename R > class V1, template< size_t N, typename R > class V2, typename R1 , typename R2 >

fmat_internal::promotion_trait<R1,R2>::type fmat::dotProduct	(	const V1< 3, R1 > &	a,
		const V2< 3, R2 > &	b
	)

Definition at line 1337 of file fmatCore.h.

template<template< size_t N, typename R > class V1, template< size_t N, typename R > class V2, typename R1 , typename R2 >

fmat_internal::promotion_trait<R1,R2>::type fmat::dotProduct	(	const V1< 2, R1 > &	a,
		const V2< 2, R2 > &	b
	)

Definition at line 1335 of file fmatCore.h.

template<class Ta , class Tb >

fmat_internal::promotion_trait<typename Ta::storage_t,typename Tb::storage_t>::type fmat::dotProduct	(	const Ta &	a,
		const Tb &	b
	)

Definition at line 1226 of file fmatCore.h.

template<template< size_t N, typename R > class T1, template< size_t N, typename R > class T2, size_t N, typename R1 , typename R2 >

fmat_internal::promotion_trait<R1,R2>::type fmat::dotProduct	(	const T1< N, R1 > &	a,
		const T2< N, R2 > &	b
	)

template<typename R >

std::string fmat::fullfmt	(	const R *const	data,
		size_t	H,
		size_t	W,
		std::string const &	numberFormat,
		std::string const &	firstLineStart,
		std::string const &	nextLineStart,
		std::string const &	lastLineEnd,
		std::string const &	rowBegin,
		std::string const &	elementSep,
		std::string const &	rowEnd,
		std::string const &	rowSep
	)

Definition at line 49 of file fmatCore.h.

Referenced by fmat::Row< N, R >::fmt(), fmat::Column< 2, float >::fmt(), fmat::Matrix< 4, 2 >::fmt(), fmat::SubMatrix< H, W, R >::fmt(), and fmat::SubVector< H, R >::fmt().

template<typename R >

TransformT<R> fmat::invert ( const TransformT< R > & m )

Definition at line 639 of file fmatSpatial.h.

template<typename R >

QuaternionT<R> fmat::invert ( const QuaternionT< R > & q )

Definition at line 407 of file fmatSpatial.h.

template<typename M >

Matrix<M::HEIGHT,M::WIDTH,typename fmat_internal::unconst<typename M::storage_t>::type > fmat::invert ( const M & m )

Computes and returns the inverse of a square matrix using Gauss-Jordan elimination.

The computation is done with column-wise operations for computational efficiency. You can think of this as doing the inverse in transposed space: (mᵀ)⁻¹ == (m⁻¹)ᵀ

Definition at line 1263 of file fmatCore.h.

Referenced by Kinematics::calculateGroundPlane(), AprilTags::GrayModel::compute(), Config::vision_config::computePixelCorrected(), Homography::getMatrix(), fmat::TransformT< R >::inverse(), OpticalFlow::iterativeLucasKanade(), and segmentIntersection().

template<typename R >

Matrix<4,4,R> fmat::invertTransform ( const Matrix< 4, 4, R > & t )

invert the matrix, taking advantage of known structure:

Definition at line 612 of file fmatSpatial.h.

template<typename R >

Column<2,R> fmat::lineIntersection	(	const Column< 2, R > &	p1,
		const Column< 2, R > &	d1,
		const Column< 2, R > &	p2,
		const Column< 2, R > &	d2
	)

Returns the point of intersection of d1 through p1 and d2 through p2.

May return point at infinity if d1 and d2 are parallel (including collinear), such a point will be in the correct quadrant.

Definition at line 721 of file fmatSpatial.h.

template<template< size_t, typename R > class V, typename R >

Column<2,typename V<2,R>::out_t> fmat::normalLeft ( const V< 2, R > & v )

Returns the orthogonal left vector (rotate by 90°).

Definition at line 728 of file fmatSpatial.h.

Referenced by LinkComponent::computeBB2D().

template<template< size_t, typename R > class V, typename R >

Column<2,typename V<2,R>::out_t> fmat::normalRight ( const V< 2, R > & v )

Returns the orthogonal right vector (rotate by -90°).

Definition at line 733 of file fmatSpatial.h.

Referenced by LinkComponent::computeBB2D().

template<template< size_t W, typename R > class T1, template< size_t H, size_t W, typename R > class T2, size_t N, size_t W, typename R1 , typename R2 >

Row<W,typename fmat_internal::promotion_trait<R1,R2>::type> fmat::operator*	(	const T1< N, R1 > &	a,
		const T2< N, W, R2 > &	b
	)

Definition at line 1181 of file fmatCore.h.

template<template< size_t H, size_t W, typename R > class T1, template< size_t N, typename R > class T2, size_t H, size_t N, typename R1 , typename R2 >

Column<H,typename fmat_internal::promotion_trait<R1,R2>::type> fmat::operator*	(	const T1< H, N, R1 > &	a,
		const T2< N, R2 > &	b
	)

Definition at line 1175 of file fmatCore.h.

template<template< size_t H, size_t W, typename R > class T1, template< size_t H, size_t W, typename R > class T2, size_t H, size_t N, size_t W, typename R1 , typename R2 >

Matrix<H,W,typename fmat_internal::promotion_trait<R1,R2>::type> fmat::operator*	(	const T1< H, N, R1 > &	a,
		const T2< N, W, R2 > &	b
	)

Definition at line 1149 of file fmatCore.h.

template<template< size_t H, size_t W, typename R > class T2, size_t D, typename R1 , typename R2 >

const SubMatrix<D,D,R1>& fmat::operator*=	(	const SubMatrix< D, D, R1 > &	a,
		const T2< D, D, R2 > &	b
	)

Definition at line 1168 of file fmatCore.h.

template<template< size_t H, size_t W, typename R > class T1, template< size_t H, size_t W, typename R > class T2, size_t D, typename R1 , typename R2 >

T1<D,D,R1>& fmat::operator*=	(	T1< D, D, R1 > &	a,
		const T2< D, D, R2 > &	b
	)

Definition at line 1161 of file fmatCore.h.

template<size_t N, typename R >

std::ostream& fmat::operator<<	(	std::ostream &	os,
		const Row< N, R > &	x
	)

Definition at line 1142 of file fmatCore.h.

template<size_t N, typename R >

std::ostream& fmat::operator<<	(	std::ostream &	os,
		const Column< N, R > &	x
	)

Definition at line 1139 of file fmatCore.h.

template<size_t H, size_t W, typename R >

std::ostream& fmat::operator<<	(	std::ostream &	os,
		const Matrix< H, W, R > &	x
	)

Definition at line 1136 of file fmatCore.h.

template<size_t H, size_t W, typename R >

std::ostream& fmat::operator<<	(	std::ostream &	os,
		const SubMatrix< H, W, R > &	x
	)

Definition at line 1133 of file fmatCore.h.

template<size_t N, typename R >

std::ostream& fmat::operator<<	(	std::ostream &	os,
		const SubVector< N, R > &	x
	)

Definition at line 1130 of file fmatCore.h.

template<size_t N1, size_t N2, typename R >

Row<N1+N2,typename fmat_internal::unconst<R>::type> fmat::pack	(	const Row< N1, R > &	x,
		const Row< N2, R > &	y
	)

Definition at line 1125 of file fmatCore.h.

template<size_t N, typename R1 , typename R2 >

Row<N+1,typename fmat_internal::unconst<R2>::type> fmat::pack	(	R1	x,
		const Row< N, R2 > &	y
	)

Definition at line 1121 of file fmatCore.h.

template<size_t N, typename R1 , typename R2 >

Row<N+1,typename fmat_internal::unconst<R1>::type> fmat::pack	(	const Row< N, R1 > &	x,
		R2	y
	)

Definition at line 1117 of file fmatCore.h.

template<size_t N1, size_t N2, typename R >

Column<N1+N2,typename fmat_internal::unconst<R>::type> fmat::pack	(	const Column< N1, R > &	x,
		const Column< N2, R > &	y
	)

Definition at line 1111 of file fmatCore.h.

template<size_t N, typename R1 , typename R2 >

Column<N+1,typename fmat_internal::unconst<R2>::type> fmat::pack	(	R1	x,
		const Column< N, R2 > &	y
	)

Definition at line 1107 of file fmatCore.h.

template<size_t N, typename R1 , typename R2 >

Column<N+1,typename fmat_internal::unconst<R1>::type> fmat::pack	(	const Column< N, R1 > &	x,
		R2	y
	)

Definition at line 1103 of file fmatCore.h.

template<template< size_t N, typename R > class T1, template< size_t N, typename R > class T2, size_t N1, size_t N2, typename R1 , typename R2 >

Column<N1+N2,typename fmat_internal::promotion_trait<R1,R2>::type> fmat::pack	(	const T1< N1, R1 > &	x,
		const T2< N2, R2 > &	y
	)

Definition at line 1097 of file fmatCore.h.

template<template< size_t N, typename R > class T, size_t N, typename R1 , typename R2 >

Column<N+1,typename fmat_internal::unconst<R2>::type> fmat::pack	(	R1	x,
		const T< N, R2 > &	y
	)

Definition at line 1092 of file fmatCore.h.

template<template< size_t N, typename R > class T, size_t N, typename R1 , typename R2 >

Column<N+1,typename fmat_internal::unconst<R1>::type> fmat::pack	(	const T< N, R1 > &	x,
		R2	y
	)

packing columns: appending an element or concatenating two columns

Definition at line 1087 of file fmatCore.h.

Column< 4 > fmat::pack	(	fmatReal	x,
		fmatReal	y,
		fmatReal	z,
		fmatReal	d
	)

Definition at line 1072 of file fmatCore.h.

Column< 3 > fmat::pack	(	fmatReal	x,
		fmatReal	y,
		fmatReal	z
	)

Definition at line 1071 of file fmatCore.h.

Column< 2 > fmat::pack	(	fmatReal	x,
		fmatReal	y
	)

generic packing of N values into a Column<N> (note assumes fmatReal, see packT()

Definition at line 1070 of file fmatCore.h.

Referenced by GaitedFootsteps::addCandidate(), ShapeSpaceCollisionCheckerBase< 2 >::addDisplayRobotObstacles(), ConvexPolyObstacle::addPoint(), GaitedFootsteps::addRotation(), CBracketGrasperPredicate< N >::admissible(), Kinematics::calculateGroundPlane(), ConvexPolyObstacle::close(), RRTNodeXYTheta::CollisionChecker::colliders(), RRTNodeXYTheta::CollisionChecker::collides(), RectangularObstacle::collides(), RRTNode3DR< N >::CollisionChecker::CollisionChecker(), LinkComponent::computeBB2D(), KoduInterpreter::MotionActionRunner::ExecuteMotionAction::ExecuteGoToShapeRequest::doStart(), Grasper::ArmRaise::doStart(), Grasper::ArmNudge::doStart(), Grasper::DoBodyApproach2::doStart(), Grasper::PlanBodyApproach::doStart(), GaitedFootsteps::expand(), DualCoding::MapBuilder::filterGroundShapes(), EllipsoidObstacle::get2Support(), EllipticalObstacle::getBoundingBox(), DualCoding::MapBuilder::getCamCrosses(), DualCoding::MapBuilder::getCamDominoes(), BoxObstacle::getCorner(), RectangularObstacle::getCorner(), PhysicsBody::getInertia(), LinkComponent::getMassVector(), XWalkParameters::getMaxAVel(), KinematicJoint::getParentPosition(), EllipticalObstacle::getPointOnEdge(), AprilTags::TagDetection::getRotMatrix(), EllipsoidObstacle::getSupport(), CylindricalObstacle::getSupport(), ConvexPolyObstacle::getSupport(), EllipticalObstacle::getSupport(), RawImage::gradient(), SphericalObstacle::gradient(), EllipticalObstacle::gradient(), CircularObstacle::gradient(), ConvexPolyObstacle::hull(), ShapeSpacePlannerXYTheta::initialize(), OpticalFlow::initializePositions(), DualCoding::MapBuilder::isLineVisible(), DualCoding::MapBuilder::isPointVisible(), OpticalFlow::iterativeLucasKanade(), BoxObstacle::loadXML(), RectangularObstacle::loadXML(), ArmMC::moveToPoint(), normalLeft(), normalRight(), ShapeSpacePlanner3DR< N >::plotPath(), ShapeSpacePlanner2DR< N >::plotPath(), Kinematics::projectToPlane(), AprilTags::Quad::Quad(), BoxObstacle::reset(), EllipticalObstacle::reset(), RectangularObstacle::reset(), XWalkMC::resetPeriod(), Draw::setOrigin(), XWalkMC::setTargetDisplacement(), XWalkMC::setTargetVelocity(), IKCalliope::solve(), PostureEngine::solveLinkPosition(), and ShapeSpacePlannerXYTheta::tangentHeading().

template<typename R >

Column< 4, R > fmat::packT	(	R	x,
		R	y,
		R	z,
		R	d
	)

Definition at line 1082 of file fmatCore.h.

template<typename R >

Column< 3, R > fmat::packT	(	R	x,
		R	y,
		R	z
	)

Definition at line 1081 of file fmatCore.h.

template<typename R >

Column< 2, R > fmat::packT	(	R	x,
		R	y
	)

templated version to pack non fmatReal type, using a different name 'packT' so it needs to be explicitly called, e.g. pack(0,0,1) is generally intended as fmatReal, not int

Definition at line 1080 of file fmatCore.h.

template<typename R >

R fmat::rayIntersection	(	const Column< 2, R > &	p1,
		const Column< 2, R > &	d1,
		const Column< 2, R > &	p2,
		const Column< 2, R > &	d2
	)

Returns the scaling factor of d1 from p1 to reach intersection of d2 through p2.

May return infinity if d1 and d2 are parallel (including collinear).

Definition at line 712 of file fmatSpatial.h.

Referenced by lineIntersection().

Matrix<2,2,fmatReal> fmat::rotation2D ( fmatReal rad )

returns 2x2 rotation matrix for angle rad (in radians) about Z axis

Definition at line 21 of file fmatSpatial.h.

Referenced by GaitedFootsteps::addRotation(), IKCalliope::closestThreeLinkIK(), RRTNodeXYTheta::CollisionChecker::colliders(), RRTNodeXYTheta::CollisionChecker::collides(), XWalkMC::computeCurrentPosition(), RectangularObstacle::reset(), XWalkMC::updateOutputsInitial(), and XWalkMC::updateOutputsWalking().

template<typename R >

Matrix<2,2,R> fmat::rotation2DT ( R rad )

returns 2x2 rotation matrix (i.e. implied rotation about Z), for angle rad in radians

Definition at line 15 of file fmatSpatial.h.

Matrix<3,3,fmatReal> fmat::rotationX ( fmatReal rad )

returns 3x3 rotation matrix for angle rad (in radians) about X axis

Definition at line 33 of file fmatSpatial.h.

Referenced by Grasper::computeGoalStates().

template<size_t N, typename R >

Matrix<N,N,R> fmat::rotationXN ( R rad )

returns NxN rotation matrix for angle rad (in radians) about X axis (only uses upper 3x3)

Definition at line 25 of file fmatSpatial.h.

Matrix<3,3,fmatReal> fmat::rotationY ( fmatReal rad )

returns 3x3 rotation matrix for angle rad (in radians) about Z axis

Definition at line 45 of file fmatSpatial.h.

Referenced by Grasper::computeGoalStates().

template<size_t N, typename R >

Matrix<N,N,R> fmat::rotationYN ( R rad )

returns NxN rotation matrix for angle rad (in radians) about Y axis (only uses upper 3x3)

Definition at line 37 of file fmatSpatial.h.

Matrix<3,3,fmatReal> fmat::rotationZ ( fmatReal rad )

returns 3x3 rotation matrix for angle rad (in radians) about Z axis

Definition at line 63 of file fmatSpatial.h.

Referenced by ShapeSpaceCollisionCheckerBase< 2 >::addDisplayRobotObstacles(), Grasper::computeGoalStates(), Grasper::PathPlanToRest::doStart(), Grasper::PathPlanConstrained::doStart(), Grasper::PlanArmApproach::doStart(), Grasper::PlanBodyApproach::doStart(), DualCoding::MapBuilder::getCamDominoes(), ShapeSpacePlannerXYTheta::initialize(), ShapeSpacePlanner3DR< N >::plotPath(), and ShapeSpacePlanner2DR< N >::plotPath().

template<size_t N, typename R >

Matrix<N,N,R> fmat::rotationZN ( R rad )

returns NxN rotation matrix for angle rad (in radians) about Z axis (only uses upper 2x2)

Definition at line 49 of file fmatSpatial.h.

template<typename R >

Column<2,R> fmat::segmentIntersection	(	const Column< 2, R > &	p1,
		const Column< 2, R > &	d1,
		const Column< 2, R > &	p2,
		const Column< 2, R > &	d2
	)

Returns the scaling factors for d1 and d2 through p1 and p2 respectively to reach common intersection.

May return infinity if d1 and d2 are parallel (including collinear).

Definition at line 694 of file fmatSpatial.h.

Referenced by rayIntersection().

const Column<2> fmat::UNIT2_X ( fmat:: pack1, 0 )

const Column<2> fmat::UNIT2_Y ( fmat:: pack0, 1 )

const Column<3> fmat::UNIT3_X ( fmat:: pack1, 0, 0 )

const Column<3> fmat::UNIT3_Y ( fmat:: pack0, 1, 0 )

const Column<3> fmat::UNIT3_Z ( fmat:: pack0, 0, 1 )

template<template< size_t, size_t, typename R > class M, typename R >

Column<3,typename fmat_internal::unconst<R>::type> fmat::ypr ( const M< 3, 3, R > & m )

Returns yaw-pitch-roll aka heading-elevation-bank conversion, where roll-pitch-yaw correspond to compounding rotations about the global x, y, and z axis respectively (in that order).

From the "driver's seat" positive heading is turning to the left (z is up, using right hand rule, not compass heading), positive pitch is looking down, and positive roll is spinning clockwise. Within this frame-oriented view, we apply rotation axes in the 'reverse' order: first z, then y, then x.

You can reconstruct a rotation from these values by: q · v = rotationZ(yaw) * rotationY(pitch) * rotationX(roll) · v Note we right-multiply v to apply the rotation, so x is applied to an incoming vector first, then y, then z.

Definition at line 675 of file fmatSpatial.h.

template<typename R >

Column<3,R> fmat::ypr ( const TransformT< R > & m )

Returns yaw-pitch-roll aka heading-elevation-bank conversion, where roll-pitch-yaw correspond to compounding rotations about the global x, y, and z axis respectively (in that order).

From the "driver's seat" positive heading is turning to the left (z is up, using right hand rule, not compass heading), positive pitch is looking down, and positive roll is spinning clockwise. Within this frame-oriented view, we apply rotation axes in the 'reverse' order: first z, then y, then x.

You can reconstruct a rotation from these values by: q · v = rotationZ(yaw) * rotationY(pitch) * rotationX(roll) · v Note we right-multiply v to apply the rotation, so x is applied to an incoming vector first, then y, then z.

Definition at line 664 of file fmatSpatial.h.

template<typename R >

Column<3,R> fmat::ypr ( const QuaternionT< R > & q )

Returns yaw-pitch-roll aka heading-elevation-bank conversion, where roll-pitch-yaw correspond to compounding rotations about the global x, y, and z axis respectively (in that order).

From the "driver's seat" positive heading is turning to the left (z is up, using right hand rule, not compass heading), positive pitch is looking down, and positive roll is spinning clockwise. Within this frame-oriented view, we apply rotation axes in the 'reverse' order: first z, then y, then x.

You can reconstruct a Quaternion from these values by: q · v = aboutZ(yaw) * aboutY(pitch) * aboutX(roll) · v Note we right-multiply v to apply the rotation, so x is applied to an incoming vector first, then y, then z.

With thanks to http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToEuler/ (note that page uses different axis mapping however!)

Definition at line 653 of file fmatSpatial.h.

Referenced by MoCapLogger::dump(), and MoCapLogger::gotMoCapGUI().

const Column<4> fmat::ZEROH ( fmat:: pack0, 0, 0, 1 )