openrave.org

 All Classes Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Groups Pages
Functions
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]pfthe input 3x3 matrix
[out]pfthe result of the operation, can be the same matrix as pf
[out]pfdetif not NULL, fills it with the determinant of the source matrix
[in]stridethe 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],
vbasis[3][3] 
)
inline

Computes the eigenvectors of the covariance matrix and forms a basis

Parameters
[in]fCovariancea symmetric 3x3 matrix.
[out]vbasisthe 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,
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 ( a,
b,
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]A3x3 matrix
[out]U3x3 matrix
[out]D3x1 matrix
[out]V3x3 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.