OpenRAVE::mathextra Namespace Reference

Extra math routines that are useful to have but don't really belong anywhere. More...

## Functions

float * mult4 (float *pfres, const float *pf1, const float *pf2)

float * multtrans3 (float *pfres, const float *pf1, const float *pf2)

float * multtrans4 (float *pfres, const float *pf1, const float *pf2)

float * transnorm3 (float *pfout, const float *pfmat, const float *pf)

float * transpose3 (const float *pf, float *pfres)

float * transpose4 (const float *pf, float *pfres)

float dot2 (const float *pf1, const float *pf2)

float dot3 (const float *pf1, const float *pf2)

float dot4 (const float *pf1, const float *pf2)

float lengthsqr2 (const float *pf)

float lengthsqr3 (const float *pf)

float lengthsqr4 (const float *pf)

float * normalize2 (float *pfout, const float *pf)

float * normalize3 (float *pfout, const float *pf)

float * normalize4 (float *pfout, const float *pf)

float * cross3 (float *pfout, const float *pf1, const float *pf2)

float * mult3_s4 (float *pfres, const float *pf1, const float *pf2)

float * mult3_s3 (float *pfres, const float *pf1, const float *pf2)

float * inv3 (const float *pf, float *pfres, float *pfdet, int stride)

float * inv4 (const float *pf, float *pfres)

double * mult4 (double *pfres, const double *pf1, const double *pf2)

double * multtrans3 (double *pfres, const double *pf1, const double *pf2)

double * multtrans4 (double *pfres, const double *pf1, const double *pf2)

double * transnorm3 (double *pfout, const double *pfmat, const double *pf)

double * transpose3 (const double *pf, double *pfres)

double * transpose4 (const double *pf, double *pfres)

double dot2 (const double *pf1, const double *pf2)

double dot3 (const double *pf1, const double *pf2)

double dot4 (const double *pf1, const double *pf2)

double lengthsqr2 (const double *pf)

double lengthsqr3 (const double *pf)

double lengthsqr4 (const double *pf)

double * normalize2 (double *pfout, const double *pf)

double * normalize3 (double *pfout, const double *pf)

double * normalize4 (double *pfout, const double *pf)

double * cross3 (double *pfout, const double *pf1, const double *pf2)

double * mult3_s4 (double *pfres, const double *pf1, const double *pf2)

double * mult3_s3 (double *pfres, const double *pf1, const double *pf2)

double * inv3 (const double *pf, double *pfres, double *pfdet, int stride)

double * inv4 (const double *pf, double *pfres)

template<typename T >
bool eig2 (const T *pfmat, T *peigs, T &fv1x, T &fv1y, T &fv2x, T &fv2y)

OPENRAVE_API int CubicRoots (double c0, double c1, double c2, double *r0, double *r1, double *r2)

template<typename T , typename S >
void Tridiagonal3 (S *mat, T *diag, T *subd)

OPENRAVE_API bool QLAlgorithm3 (float *m_aafEntry, float *afDiag, float *afSubDiag)

OPENRAVE_API bool QLAlgorithm3 (double *m_aafEntry, double *afDiag, double *afSubDiag)

OPENRAVE_API void EigenSymmetric3 (const double *fCovariance, double *eval, double *fAxes)

template<typename T >
void GetCovarBasisVectors (const T fCovariance[3][3], T vbasis[3][3])

template<typename T >
void svd3 (const T *A, T *U, T *D, T *V)

template<typename T >
void mult (T *pf, T fa, int r)

template<typename T >
int Min (T *pts, int stride, int numPts)

template<typename T >
int Max (T *pts, int stride, int numPts)

template<typename T , typename R , typename S >
S * mult (T *pf1, R *pf2, int r1, int c1, int c2, S *pfres, bool badd=false)

template<typename T , typename R , typename S >
S * multtrans (T *pf1, R *pf2, int r1, int c1, int c2, S *pfres, bool badd=false)

template<typename T , typename R , typename S >
S * multtrans_to2 (T *pf1, R *pf2, int r1, int c1, int r2, S *pfres, bool badd=false)

template<typename T >
T * multto1 (T *pf1, T *pf2, int r1, int c1, T *pftemp=NULL)

template<typename T , typename S >
T * multto2 (T *pf1, S *pf2, int r2, int c2, S *pftemp=NULL)

template<typename T >
void sub (T *pf1, T *pf2, int r)

template<typename T >
normsqr (const T *pf1, int r)

template<typename T >
lengthsqr (const T *pf1, const T *pf2, int length)

template<typename T >
dot (T *pf1, T *pf2, int length)

template<typename T >
sum (T *pf, int length)

template<typename T >
bool inv2 (T *pf, T *pfres)
takes the inverse of the 2x2 matrix pf and stores it into pfres, returns true if matrix is invertible

template<typename T >
int solvequad (T a, T b, T c, T &r1, T &r2)

template<typename T >
T * _mult3_s4 (T *pfres, const T *pf1, const T *pf2)
mult3 with a 3x3 matrix whose row stride is 16 bytes

template<typename T >
T * _mult3_s3 (T *pfres, const T *pf1, const T *pf2)
mult3 with a 3x3 matrix whose row stride is 12 bytes

template<typename T >
T * _mult4 (T *pfres, const T *p1, const T *p2)

template<typename T >
T * _multtrans3 (T *pfres, const T *pf1, const T *pf2)

template<typename T >
T * _multtrans4 (T *pfres, const T *pf1, const T *pf2)

template<typename T >
matrixdet3 (const T *pf, int stride)
Compute the determinant of a 3x3 matrix whose row stride stride elements.

template<typename T >
T * _inv3 (const T *pf, T *pfres, T *pfdet, int stride)
3x3 matrix inverse.

template<typename T >
T * _inv4 (const T *pf, T *pfres)
4x4 matrix inverse.

template<typename T >
T * _transpose3 (const T *pf, T *pfres)
Transpose a 3x3 matrix.

template<typename T >
T * _transpose4 (const T *pf, T *pfres)
Transpose a 4x4 matrix.

template<typename T >
_dot2 (const T *pf1, const T *pf2)

template<typename T >
_dot3 (const T *pf1, const T *pf2)

template<typename T >
_dot4 (const T *pf1, const T *pf2)

template<typename T >
_lengthsqr2 (const T *pf)

template<typename T >
_lengthsqr3 (const T *pf)

template<typename T >
_lengthsqr4 (const T *pf)

template<typename T >
T * _normalize2 (T *pfout, const T *pf)

template<typename T >
T * _normalize3 (T *pfout, const T *pf)

template<typename T >
T * _normalize4 (T *pfout, const T *pf)

template<typename T >
T * _cross3 (T *pfout, const T *pf1, const T *pf2)

template<typename T >
T * _transnorm3 (T *pfout, const T *pfmat, const T *pf)

template<typename T >
void add (T *pf1, T *pf2, int r)

template<class T >
bool _QLAlgorithm3 (T *m_aafEntry, T *afDiag, T *afSubDiag)

## Detailed Description

Extra math routines that are useful to have but don't really belong anywhere.

## Function Documentation

template<typename T >
 T* OpenRAVE::mathextra::_cross3 ( T * pfout, const T * pf1, const T * pf2 )
inline

Definition at line 668 of file mathextra.h.

template<typename T >
 T OpenRAVE::mathextra::_dot2 ( const T * pf1, const T * pf2 )
inline

Definition at line 582 of file mathextra.h.

template<typename T >
 T OpenRAVE::mathextra::_dot3 ( const T * pf1, const T * pf2 )
inline

Definition at line 589 of file mathextra.h.

template<typename T >
 T OpenRAVE::mathextra::_dot4 ( const T * pf1, const T * pf2 )
inline

Definition at line 596 of file mathextra.h.

template<typename T >
 T* OpenRAVE::mathextra::_inv3 ( const T * pf, T * pfres, T * pfdet, int stride )
inline

3x3 matrix inverse.

Parameters
 [in] pf the input 3x3 matrix [out] pf the result of the operation, can be the same matrix as pf [out] pfdet if not NULL, fills it with the determinant of the source matrix [in] stride the stride in elements between elements.

Definition at line 409 of file mathextra.h.

template<typename T >
 T* OpenRAVE::mathextra::_inv4 ( const T * pf, T * pfres )
inline

4x4 matrix inverse.

Definition at line 462 of file mathextra.h.

template<typename T >
 T OpenRAVE::mathextra::_lengthsqr2 ( const T * pf )
inline

Definition at line 603 of file mathextra.h.

template<typename T >
 T OpenRAVE::mathextra::_lengthsqr3 ( const T * pf )
inline

Definition at line 610 of file mathextra.h.

template<typename T >
 T OpenRAVE::mathextra::_lengthsqr4 ( const T * pf )
inline

Definition at line 617 of file mathextra.h.

template<typename T >
 T* OpenRAVE::mathextra::_mult3_s3 ( T * pfres, const T * pf1, const T * pf2 )
inline

mult3 with a 3x3 matrix whose row stride is 12 bytes

Definition at line 302 of file mathextra.h.

template<typename T >
 T* OpenRAVE::mathextra::_mult3_s4 ( T * pfres, const T * pf1, const T * pf2 )
inline

mult3 with a 3x3 matrix whose row stride is 16 bytes

Definition at line 287 of file mathextra.h.

template<typename T >
 T* OpenRAVE::mathextra::_mult4 ( T * pfres, const T * p1, const T * p2 )
inline

Definition at line 319 of file mathextra.h.

template<typename T >
 T* OpenRAVE::mathextra::_multtrans3 ( T * pfres, const T * pf1, const T * pf2 )
inline

Definition at line 352 of file mathextra.h.

template<typename T >
 T* OpenRAVE::mathextra::_multtrans4 ( T * pfres, const T * pf1, const T * pf2 )
inline

Definition at line 376 of file mathextra.h.

template<typename T >
 T* OpenRAVE::mathextra::_normalize2 ( T * pfout, const T * pf )
inline

Definition at line 624 of file mathextra.h.

template<typename T >
 T* OpenRAVE::mathextra::_normalize3 ( T * pfout, const T * pf )
inline

Definition at line 637 of file mathextra.h.

template<typename T >
 T* OpenRAVE::mathextra::_normalize4 ( T * pfout, const T * pf )
inline

Definition at line 652 of file mathextra.h.

template<class T >
 bool OpenRAVE::mathextra::_QLAlgorithm3 ( T * m_aafEntry, T * afDiag, T * afSubDiag )

Definition at line 142 of file math.cpp.

template<typename T >
 T* OpenRAVE::mathextra::_transnorm3 ( T * pfout, const T * pfmat, const T * pf )
inline

Definition at line 682 of file mathextra.h.

template<typename T >
 T* OpenRAVE::mathextra::_transpose3 ( const T * pf, T * pfres )
inline

Transpose a 3x3 matrix.

Definition at line 540 of file mathextra.h.

template<typename T >
 T* OpenRAVE::mathextra::_transpose4 ( const T * pf, T * pfres )
inline

Transpose a 4x4 matrix.

Definition at line 560 of file mathextra.h.

template<typename T >
 void OpenRAVE::mathextra::add ( T * pf1, T * pf2, int r )
inline

Definition at line 1009 of file mathextra.h.

 float * OpenRAVE::mathextra::cross3 ( float * pfout, const float * pf1, const float * pf2 )
inline

Definition at line 753 of file mathextra.h.

 double * OpenRAVE::mathextra::cross3 ( double * pfout, const double * pf1, const double * pf2 )
inline

Definition at line 824 of file mathextra.h.

 int OpenRAVE::mathextra::CubicRoots ( double c0, double c1, double c2, double * r0, double * r1, double * r2 )

Definition at line 27 of file math.cpp.

template<typename T >
 T OpenRAVE::mathextra::dot ( T * pf1, T * pf2, int length )
inline

Definition at line 1054 of file mathextra.h.

 float OpenRAVE::mathextra::dot2 ( const float * pf1, const float * pf2 )
inline

Definition at line 723 of file mathextra.h.

 double OpenRAVE::mathextra::dot2 ( const double * pf1, const double * pf2 )
inline

Definition at line 794 of file mathextra.h.

 float OpenRAVE::mathextra::dot3 ( const float * pf1, const float * pf2 )
inline

Definition at line 726 of file mathextra.h.

 double OpenRAVE::mathextra::dot3 ( const double * pf1, const double * pf2 )
inline

Definition at line 797 of file mathextra.h.

 float OpenRAVE::mathextra::dot4 ( const float * pf1, const float * pf2 )
inline

Definition at line 729 of file mathextra.h.

 double OpenRAVE::mathextra::dot4 ( const double * pf1, const double * pf2 )
inline

Definition at line 800 of file mathextra.h.

template<typename T >
 bool OpenRAVE::mathextra::eig2 ( const T * pfmat, T * peigs, T & fv1x, T & fv1y, T & fv2x, T & fv2y )
inline

extract eigen values and vectors from a 2x2 matrix and returns true if all values are real returned eigen vectors are normalized

Definition at line 212 of file mathextra.h.

 void OpenRAVE::mathextra::EigenSymmetric3 ( const double * fCovariance, double * eval, double * fAxes )

Definition at line 236 of file math.cpp.

template<typename T >
 void OpenRAVE::mathextra::GetCovarBasisVectors ( const T fCovariance[3][3], T vbasis[3][3] )
inline

Computes the eigenvectors of the covariance matrix and forms a basis

Parameters
 [in] fCovariance a symmetric 3x3 matrix. [out] vbasis the basis vectors extracted (form a right hand coordinate system).

Definition at line 134 of file mathextra.h.

template<typename T >
 bool OpenRAVE::mathextra::inv2 ( T * pf, T * pfres )
inline

takes the inverse of the 2x2 matrix pf and stores it into pfres, returns true if matrix is invertible

Definition at line 1076 of file mathextra.h.

 float * OpenRAVE::mathextra::inv3 ( const float * pf, float * pfres, float * pfdet, int stride )
inline

Definition at line 765 of file mathextra.h.

 double * OpenRAVE::mathextra::inv3 ( const double * pf, double * pfres, double * pfdet, int stride )
inline

Definition at line 836 of file mathextra.h.

 float * OpenRAVE::mathextra::inv4 ( const float * pf, float * pfres )
inline

Definition at line 768 of file mathextra.h.

 double * OpenRAVE::mathextra::inv4 ( const double * pf, double * pfres )
inline

Definition at line 839 of file mathextra.h.

template<typename T >
 T OpenRAVE::mathextra::lengthsqr ( const T * pf1, const T * pf2, int length )
inline

Definition at line 1042 of file mathextra.h.

 float OpenRAVE::mathextra::lengthsqr2 ( const float * pf )
inline

Definition at line 733 of file mathextra.h.

 double OpenRAVE::mathextra::lengthsqr2 ( const double * pf )
inline

Definition at line 804 of file mathextra.h.

 float OpenRAVE::mathextra::lengthsqr3 ( const float * pf )
inline

Definition at line 736 of file mathextra.h.

 double OpenRAVE::mathextra::lengthsqr3 ( const double * pf )
inline

Definition at line 807 of file mathextra.h.

 float OpenRAVE::mathextra::lengthsqr4 ( const float * pf )
inline
Examples:
orplanning_door.cpp.

Definition at line 739 of file mathextra.h.

 double OpenRAVE::mathextra::lengthsqr4 ( const double * pf )
inline

Definition at line 810 of file mathextra.h.

template<typename T >
 T OpenRAVE::mathextra::matrixdet3 ( const T * pf, int stride )
inline

Compute the determinant of a 3x3 matrix whose row stride stride elements.

Definition at line 394 of file mathextra.h.

template<typename T >
 int OpenRAVE::mathextra::Max ( T * pts, int stride, int numPts )

Definition at line 1211 of file mathextra.h.

template<typename T >
 int OpenRAVE::mathextra::Min ( T * pts, int stride, int numPts )

Definition at line 1197 of file mathextra.h.

template<typename T >
 void OpenRAVE::mathextra::mult ( T * pf, T fa, int r )
inline

Definition at line 154 of file mathextra.h.

template<typename T , typename R , typename S >
 S * OpenRAVE::mathextra::mult ( T * pf1, R * pf2, int r1, int c1, int c2, S * pfres, bool badd = `false` )
inline

Definition at line 844 of file mathextra.h.

 float * OpenRAVE::mathextra::mult3_s3 ( float * pfres, const float * pf1, const float * pf2 )
inline

Definition at line 761 of file mathextra.h.

 double * OpenRAVE::mathextra::mult3_s3 ( double * pfres, const double * pf1, const double * pf2 )
inline

Definition at line 832 of file mathextra.h.

 float * OpenRAVE::mathextra::mult3_s4 ( float * pfres, const float * pf1, const float * pf2 )
inline

Definition at line 758 of file mathextra.h.

 double * OpenRAVE::mathextra::mult3_s4 ( double * pfres, const double * pf1, const double * pf2 )
inline

Definition at line 829 of file mathextra.h.

 float * OpenRAVE::mathextra::mult4 ( float * pfres, const float * pf1, const float * pf2 )
inline

Definition at line 702 of file mathextra.h.

 double * OpenRAVE::mathextra::mult4 ( double * pfres, const double * pf1, const double * pf2 )
inline

Definition at line 773 of file mathextra.h.

template<typename T >
 T * OpenRAVE::mathextra::multto1 ( T * pf1, T * pf2, int r1, int c1, T * pftemp = `NULL` )
inline

Definition at line 930 of file mathextra.h.

template<typename T , typename S >
 T * OpenRAVE::mathextra::multto2 ( T * pf1, S * pf2, int r2, int c2, S * pftemp = `NULL` )
inline

Definition at line 967 of file mathextra.h.

template<typename T , typename R , typename S >
 S * OpenRAVE::mathextra::multtrans ( T * pf1, R * pf2, int r1, int c1, int c2, S * pfres, bool badd = `false` )
inline

Definition at line 872 of file mathextra.h.

 float * OpenRAVE::mathextra::multtrans3 ( float * pfres, const float * pf1, const float * pf2 )
inline

Definition at line 706 of file mathextra.h.

 double * OpenRAVE::mathextra::multtrans3 ( double * pfres, const double * pf1, const double * pf2 )
inline

Definition at line 777 of file mathextra.h.

 float * OpenRAVE::mathextra::multtrans4 ( float * pfres, const float * pf1, const float * pf2 )
inline

Definition at line 709 of file mathextra.h.

 double * OpenRAVE::mathextra::multtrans4 ( double * pfres, const double * pf1, const double * pf2 )
inline

Definition at line 780 of file mathextra.h.

template<typename T , typename R , typename S >
 S * OpenRAVE::mathextra::multtrans_to2 ( T * pf1, R * pf2, int r1, int c1, int r2, S * pfres, bool badd = `false` )
inline

Definition at line 903 of file mathextra.h.

 float * OpenRAVE::mathextra::normalize2 ( float * pfout, const float * pf )
inline

Definition at line 743 of file mathextra.h.

 double * OpenRAVE::mathextra::normalize2 ( double * pfout, const double * pf )
inline

Definition at line 814 of file mathextra.h.

 float * OpenRAVE::mathextra::normalize3 ( float * pfout, const float * pf )
inline

Definition at line 746 of file mathextra.h.

 double * OpenRAVE::mathextra::normalize3 ( double * pfout, const double * pf )
inline

Definition at line 817 of file mathextra.h.

 float * OpenRAVE::mathextra::normalize4 ( float * pfout, const float * pf )
inline

Definition at line 749 of file mathextra.h.

 double * OpenRAVE::mathextra::normalize4 ( double * pfout, const double * pf )
inline

Definition at line 820 of file mathextra.h.

template<typename T >
 T OpenRAVE::mathextra::normsqr ( const T * pf1, int r )
inline

Definition at line 1029 of file mathextra.h.

 bool OpenRAVE::mathextra::QLAlgorithm3 ( float * m_aafEntry, float * afDiag, float * afSubDiag )

Definition at line 226 of file math.cpp.

 bool OpenRAVE::mathextra::QLAlgorithm3 ( double * m_aafEntry, double * afDiag, double * afSubDiag )

Definition at line 231 of file math.cpp.

template<typename T >
 int OpenRAVE::mathextra::solvequad ( T a, T b, T c, T & r1, T & r2 )
inline

Definition at line 256 of file mathextra.h.

template<typename T >
 void OpenRAVE::mathextra::sub ( T * pf1, T * pf2, int r )
inline

Definition at line 1019 of file mathextra.h.

template<typename T >
 T OpenRAVE::mathextra::sum ( T * pf, int length )
inline

Definition at line 1065 of file mathextra.h.

template<typename T >
 void OpenRAVE::mathextra::svd3 ( const T * A, T * U, T * D, T * V )
inline

SVD of a 3x3 matrix A such that A = U*diag(D)*V' The row stride for all matrices is 3*sizeof(T) bytes

Parameters
 [in] A 3x3 matrix [out] U 3x3 matrix [out] D 3x1 matrix [out] V 3x3 matrix

Definition at line 1139 of file mathextra.h.

 float * OpenRAVE::mathextra::transnorm3 ( float * pfout, const float * pfmat, const float * pf )
inline

Definition at line 712 of file mathextra.h.

 double * OpenRAVE::mathextra::transnorm3 ( double * pfout, const double * pfmat, const double * pf )
inline

Definition at line 783 of file mathextra.h.

 float * OpenRAVE::mathextra::transpose3 ( const float * pf, float * pfres )
inline

Definition at line 716 of file mathextra.h.

 double * OpenRAVE::mathextra::transpose3 ( const double * pf, double * pfres )
inline

Definition at line 787 of file mathextra.h.

 float * OpenRAVE::mathextra::transpose4 ( const float * pf, float * pfres )
inline

Definition at line 719 of file mathextra.h.

 double * OpenRAVE::mathextra::transpose4 ( const double * pf, double * pfres )
inline

Definition at line 790 of file mathextra.h.

template<typename T , typename S >
 void OpenRAVE::mathextra::Tridiagonal3 ( S * mat, T * diag, T * subd )

Definition at line 1101 of file mathextra.h.