Defines basic math and gemoetric primitives. dReal is defined in ODE. Any functions not in here like conversions between quaterions and matrices are most likely defined in ODE. Originally from OpenRAVE (http://openrave.programmingvision.com) --------------------------------------------------------------------. More...
Go to the source code of this file.
Classes | |
struct | AABB |
struct | OBB |
class | RaveTransform< T > |
affine transformation parameterized with quaterions More... | |
class | RaveTransformMatrix< T > |
affine transformation parameterized with rotation matrices More... | |
class | RaveVector< T > |
struct | RAY |
struct | TRIANGLE |
Defines | |
#define | _R(i, j) R[(i)*4+(j)] |
#define | dCos(x) (cos(x)) |
#define | distinctRoots 0 |
#define | dRecip(x) ((1.0/(x))) |
#define | dSin(x) (sin(x)) |
#define | dSINGLE |
#define | dSqrt(x) (sqrt(x)) |
#define | floatRoot01 2 |
#define | floatRoot12 4 |
#define | g_fEpsilon 1e-8 |
#define | MULT3(stride) |
#define | PI ((dReal)3.141592654) |
#define | rswap(x, y) *(int*)&(x) ^= *(int*)&(y) ^= *(int*)&(x) ^= *(int*)&(y); |
#define | singleRoot 1 |
#define | tripleRoot 6 |
Typedefs | |
typedef dReal | dMatrix3 [4 *3] |
typedef dReal | dMatrix4 [4 *4] |
typedef dReal | dMatrix6 [8 *6] |
typedef dReal | dQuaternion [4] |
typedef double | dReal |
typedef dReal | dVector3 [4] |
typedef dReal | dVector4 [4] |
typedef RaveTransform< dReal > | Transform |
typedef RaveTransformMatrix < dReal > | TransformMatrix |
typedef RaveVector< dReal > | Vector |
Functions | |
template<class T > | |
T * | _cross3 (T *pfout, const T *pf1, const T *pf2) |
template<class T > | |
T | _dot2 (const T *pf1, const T *pf2) |
template<class T > | |
T | _dot3 (const T *pf1, const T *pf2) |
template<class T > | |
T | _dot4 (const T *pf1, const T *pf2) |
template<class T > | |
T * | _inv3 (const T *pf, T *pfres, T *pfdet, int stride) |
template<class T > | |
T * | _inv4 (const T *pf, T *pfres) |
template<class T > | |
T | _lengthsqr2 (const T *pf) |
template<class T > | |
T | _lengthsqr3 (const T *pf) |
template<class T > | |
T | _lengthsqr4 (const T *pf) |
template<class T > | |
T * | _mult3_s3 (T *pfres, const T *pf1, const T *pf2) |
mult3 with a 3x3 matrix whose row stride is 12 bytes | |
template<class T > | |
T * | _mult3_s4 (T *pfres, const T *pf1, const T *pf2) |
mult3 with a 3x3 matrix whose row stride is 16 bytes | |
template<class T > | |
T * | _mult4 (T *pfres, const T *p1, const T *p2) |
template<class T > | |
T * | _multtrans3 (T *pfres, const T *pf1, const T *pf2) |
template<class T > | |
T * | _multtrans4 (T *pfres, const T *pf1, const T *pf2) |
template<class T > | |
T * | _normalize2 (T *pfout, const T *pf) |
template<class T > | |
T * | _normalize3 (T *pfout, const T *pf) |
template<class T > | |
T * | _normalize4 (T *pfout, const T *pf) |
template<class T > | |
T * | _transnorm3 (T *pfout, const T *pfmat, const T *pf) |
template<class T > | |
T * | _transpose3 (const T *pf, T *pfres) |
template<class T > | |
T * | _transpose4 (const T *pf, T *pfres) |
template<class T > | |
void | add (T *pf1, T *pf2, int r) |
template<class T > | |
RaveVector< T > | AxisAngle2Quat (const RaveVector< T > &rotaxis, T angle) |
double * | cross3 (double *pfout, const double *pf1, const double *pf2) |
float * | cross3 (float *pfout, const float *pf1, const float *pf2) |
int | CubicRoots (double c0, double c1, double c2, double *r0, double *r1, double *r2) |
dReal | DistVertexOBBSq (const Vector &v, const OBB &o) |
template<class T > | |
T | dot (T *pf1, T *pf2, int length) |
double | dot2 (const double *pf1, const double *pf2) |
float | dot2 (const float *pf1, const float *pf2) |
double | dot3 (const double *pf1, const double *pf2) |
float | dot3 (const float *pf1, const float *pf2) |
double | dot4 (const double *pf1, const double *pf2) |
float | dot4 (const float *pf1, const float *pf2) |
void | dQfromR (dQuaternion q, const dMatrix3 R) |
void | dQMultiply0 (dQuaternion qa, const dQuaternion qb, const dQuaternion qc) |
template<class T > | |
RaveVector< T > | dQSlerp (const RaveVector< T > &qa, const RaveVector< T > &qb, T t) |
void | dRfromQ (dMatrix3 R, const dQuaternion q) |
bool | eig2 (const dReal *pfmat, dReal *peigs, dReal &fv1x, dReal &fv1y, dReal &fv2x, dReal &fv2y) |
void | EigenSymmetric3 (dReal *fCovariance, dReal *eval, dReal *fAxes) |
void | GetCovarBasisVectors (dReal fCovariance[3][3], Vector *vRight, Vector *vUp, Vector *vDir) |
Vector | GetRandomQuat (void) |
int | insideQuadrilateral (const Vector *p0, const Vector *p1, const Vector *p2, const Vector *p3) |
int | insideTriangle (const Vector *p0, const Vector *p1, const Vector *p2) |
template<class T > | |
bool | inv2 (T *pf, T *pfres) |
double * | inv3 (const double *pf, double *pfres, double *pfdet, int stride) |
float * | inv3 (const float *pf, float *pfres, float *pfdet, int stride) |
double * | inv4 (const double *pf, double *pfres) |
float * | inv4 (const float *pf, float *pfres) |
template<class T > | |
T | lengthsqr (const T *pf1, const T *pf2, int length) |
double | lengthsqr2 (const double *pf) |
float | lengthsqr2 (const float *pf) |
double | lengthsqr3 (const double *pf) |
float | lengthsqr3 (const float *pf) |
double | lengthsqr4 (const double *pf) |
float | lengthsqr4 (const float *pf) |
template<class T > | |
T | matrixdet3 (const T *pf, int stride) |
calculates the determinant of a 3x3 matrix whose row stride stride elements | |
template<class T > | |
int | Max (T *pts, int stride, int numPts) |
template<class T > | |
int | Min (T *pts, int stride, int numPts) |
template<class T , class R , class S > | |
S * | mult (T *pf1, R *pf2, int r1, int c1, int c2, S *pfres, bool badd=false) |
template<class T > | |
void | mult (T *pf, T fa, int r) |
double * | mult3_s3 (double *pfres, const double *pf1, const double *pf2) |
float * | mult3_s3 (float *pfres, const float *pf1, const float *pf2) |
double * | mult3_s4 (double *pfres, const double *pf1, const double *pf2) |
float * | mult3_s4 (float *pfres, const float *pf1, const float *pf2) |
double * | mult4 (double *pfres, const double *pf1, const double *pf2) |
float * | mult4 (float *pfres, const float *pf1, const float *pf2) |
template<class T > | |
T * | multto1 (T *pf1, T *pf2, int r1, int c1, T *pftemp=NULL) |
template<class T , class S > | |
T * | multto2 (T *pf1, S *pf2, int r2, int c2, S *pftemp=NULL) |
template<class T , class R , class S > | |
S * | multtrans (T *pf1, R *pf2, int r1, int c1, int c2, S *pfres, bool badd=false) |
double * | multtrans3 (double *pfres, const double *pf1, const double *pf2) |
float * | multtrans3 (float *pfres, const float *pf1, const float *pf2) |
double * | multtrans4 (double *pfres, const double *pf1, const double *pf2) |
float * | multtrans4 (float *pfres, const float *pf1, const float *pf2) |
template<class T , class R , class S > | |
S * | multtrans_to2 (T *pf1, R *pf2, int r1, int c1, int r2, S *pfres, bool badd=false) |
double * | normalize2 (double *pfout, const double *pf) |
float * | normalize2 (float *pfout, const float *pf) |
double * | normalize3 (double *pfout, const double *pf) |
float * | normalize3 (float *pfout, const float *pf) |
double * | normalize4 (double *pfout, const double *pf) |
float * | normalize4 (float *pfout, const float *pf) |
template<class T > | |
T | normsqr (const T *pf1, int r) |
template<class T > | |
RaveVector< T > | operator* (double f, const RaveVector< T > &left) |
template<class T > | |
RaveVector< T > | operator* (float f, const RaveVector< T > &left) |
template<class T , class U > | |
std::basic_ostream< T > & | operator<< (std::basic_ostream< T > &O, const RaveTransformMatrix< U > &v) |
template<class T , class U > | |
std::basic_ostream< T > & | operator<< (std::basic_ostream< T > &O, const RaveTransform< U > &v) |
template<class T , class U > | |
std::basic_ostream< T > & | operator<< (std::basic_ostream< T > &O, const RaveVector< U > &v) |
template<class T , class U > | |
std::basic_istream< T > & | operator>> (std::basic_istream< T > &I, RaveTransformMatrix< U > &v) |
template<class T , class U > | |
std::basic_istream< T > & | operator>> (std::basic_istream< T > &I, RaveTransform< U > &v) |
template<class T , class U > | |
std::basic_istream< T > & | operator>> (std::basic_istream< T > &I, RaveVector< U > &v) |
bool | QLAlgorithm3 (double *m_aafEntry, double *afDiag, double *afSubDiag) |
bool | QLAlgorithm3 (float *m_aafEntry, float *afDiag, float *afSubDiag) |
void | QuadraticSolver (dReal *pfQuadratic, dReal *pfRoots) |
template<class T > | |
T | RAD_2_DEG (T radians) |
double | RaveAcos (double f) |
float | RaveAcos (float f) |
double | RaveCos (double f) |
float | RaveCos (float f) |
double | RaveFabs (double f) |
float | RaveFabs (float f) |
double | RaveSin (double f) |
float | RaveSin (float f) |
double | RaveSqrt (double f) |
float | RaveSqrt (float f) |
bool | RayOBBTest (const RAY &r, const OBB &obb) |
template<class T > | |
int | solvequad (T a, T b, T c, T &r1, T &r2) |
template<class T > | |
void | sub (T *pf1, T *pf2, int r) |
template<class T > | |
T | sum (T *pf, int length) |
void | svd3 (const dReal *A, dReal *U, dReal *D, dReal *V) |
template<class T > | |
T * | transcoord3 (T *pfout, const RaveTransformMatrix< T > *pmat, const T *pf) |
dReal * | transcoord3 (dReal *pfout, const TransformMatrix *pmat, const dReal *pf) |
computes (*pmat) * v | |
template<class T > | |
T * | transnorm3 (T *pfout, const RaveTransformMatrix< T > *pmat, const T *pf) |
dReal * | transnorm3 (dReal *pfout, const TransformMatrix *pmat, const dReal *pf) |
uses only 3x3 upper submatrix | |
double * | transnorm3 (double *pfout, const double *pfmat, const double *pf) |
float * | transnorm3 (float *pfout, const float *pfmat, const float *pf) |
double * | transpose3 (const double *pf, double *pfres) |
float * | transpose3 (const float *pf, float *pfres) |
double * | transpose4 (const double *pf, double *pfres) |
float * | transpose4 (const float *pf, float *pfres) |
template<class T , class S > | |
void | Tridiagonal3 (S *mat, T *diag, T *subd) |
bool | TriTriCollision (const RaveVector< double > &u1, const RaveVector< double > &u2, const RaveVector< double > &u3, const RaveVector< double > &v1, const RaveVector< double > &v2, const RaveVector< double > &v3, RaveVector< double > &contactpos, RaveVector< double > &contactnorm) |
bool | TriTriCollision (const RaveVector< float > &u1, const RaveVector< float > &u2, const RaveVector< float > &u3, const RaveVector< float > &v1, const RaveVector< float > &v2, const RaveVector< float > &v3, RaveVector< float > &contactpos, RaveVector< float > &contactnorm) |
Defines basic math and gemoetric primitives. dReal is defined in ODE. Any functions not in here like conversions between quaterions and matrices are most likely defined in ODE. Originally from OpenRAVE (http://openrave.programmingvision.com) --------------------------------------------------------------------.
--------------------------------------------------------------------
Definition in file math.h.
#define MULT3 | ( | stride | ) |
{ \ pfres2[0*stride+0] = pf1[0*stride+0]*pf2[0*stride+0]+pf1[0*stride+1]*pf2[1*stride+0]+pf1[0*stride+2]*pf2[2*stride+0]; \ pfres2[0*stride+1] = pf1[0*stride+0]*pf2[0*stride+1]+pf1[0*stride+1]*pf2[1*stride+1]+pf1[0*stride+2]*pf2[2*stride+1]; \ pfres2[0*stride+2] = pf1[0*stride+0]*pf2[0*stride+2]+pf1[0*stride+1]*pf2[1*stride+2]+pf1[0*stride+2]*pf2[2*stride+2]; \ pfres2[1*stride+0] = pf1[1*stride+0]*pf2[0*stride+0]+pf1[1*stride+1]*pf2[1*stride+0]+pf1[1*stride+2]*pf2[2*stride+0]; \ pfres2[1*stride+1] = pf1[1*stride+0]*pf2[0*stride+1]+pf1[1*stride+1]*pf2[1*stride+1]+pf1[1*stride+2]*pf2[2*stride+1]; \ pfres2[1*stride+2] = pf1[1*stride+0]*pf2[0*stride+2]+pf1[1*stride+1]*pf2[1*stride+2]+pf1[1*stride+2]*pf2[2*stride+2]; \ pfres2[2*stride+0] = pf1[2*stride+0]*pf2[0*stride+0]+pf1[2*stride+1]*pf2[1*stride+0]+pf1[2*stride+2]*pf2[2*stride+0]; \ pfres2[2*stride+1] = pf1[2*stride+0]*pf2[0*stride+1]+pf1[2*stride+1]*pf2[1*stride+1]+pf1[2*stride+2]*pf2[2*stride+1]; \ pfres2[2*stride+2] = pf1[2*stride+0]*pf2[0*stride+2]+pf1[2*stride+1]*pf2[1*stride+2]+pf1[2*stride+2]*pf2[2*stride+2]; \ }
#define rswap | ( | x, | |||
y | ) | *(int*)&(x) ^= *(int*)&(y) ^= *(int*)&(x) ^= *(int*)&(y); |
typedef dReal dQuaternion[4] |
typedef RaveTransform<dReal> Transform |
typedef RaveTransformMatrix<dReal> TransformMatrix |
typedef RaveVector<dReal> Vector |
T* _cross3 | ( | T * | pfout, | |
const T * | pf1, | |||
const T * | pf2 | |||
) | [inline] |
T _dot2 | ( | const T * | pf1, | |
const T * | pf2 | |||
) | [inline] |
T _dot3 | ( | const T * | pf1, | |
const T * | pf2 | |||
) | [inline] |
T _dot4 | ( | const T * | pf1, | |
const T * | pf2 | |||
) | [inline] |
T* _inv3 | ( | const T * | pf, | |
T * | pfres, | |||
T * | pfdet, | |||
int | stride | |||
) | [inline] |
T* _inv4 | ( | const T * | pf, | |
T * | pfres | |||
) | [inline] |
T* _mult3_s3 | ( | T * | pfres, | |
const T * | pf1, | |||
const T * | pf2 | |||
) | [inline] |
T* _mult3_s4 | ( | T * | pfres, | |
const T * | pf1, | |||
const T * | pf2 | |||
) | [inline] |
T* _mult4 | ( | T * | pfres, | |
const T * | p1, | |||
const T * | p2 | |||
) | [inline] |
T* _multtrans3 | ( | T * | pfres, | |
const T * | pf1, | |||
const T * | pf2 | |||
) | [inline] |
T* _multtrans4 | ( | T * | pfres, | |
const T * | pf1, | |||
const T * | pf2 | |||
) | [inline] |
T* _normalize2 | ( | T * | pfout, | |
const T * | pf | |||
) | [inline] |
T* _normalize3 | ( | T * | pfout, | |
const T * | pf | |||
) | [inline] |
T* _normalize4 | ( | T * | pfout, | |
const T * | pf | |||
) | [inline] |
T* _transnorm3 | ( | T * | pfout, | |
const T * | pfmat, | |||
const T * | pf | |||
) | [inline] |
T* _transpose3 | ( | const T * | pf, | |
T * | pfres | |||
) | [inline] |
T* _transpose4 | ( | const T * | pf, | |
T * | pfres | |||
) | [inline] |
void add | ( | T * | pf1, | |
T * | pf2, | |||
int | r | |||
) | [inline] |
RaveVector<T> AxisAngle2Quat | ( | const RaveVector< T > & | rotaxis, | |
T | angle | |||
) | [inline] |
double * cross3 | ( | double * | pfout, | |
const double * | pf1, | |||
const double * | pf2 | |||
) | [inline] |
float * cross3 | ( | float * | pfout, | |
const float * | pf1, | |||
const float * | pf2 | |||
) | [inline] |
int CubicRoots | ( | double | c0, | |
double | c1, | |||
double | c2, | |||
double * | r0, | |||
double * | r1, | |||
double * | r2 | |||
) |
T dot | ( | T * | pf1, | |
T * | pf2, | |||
int | length | |||
) | [inline] |
double dot2 | ( | const double * | pf1, | |
const double * | pf2 | |||
) | [inline] |
float dot2 | ( | const float * | pf1, | |
const float * | pf2 | |||
) | [inline] |
double dot3 | ( | const double * | pf1, | |
const double * | pf2 | |||
) | [inline] |
float dot3 | ( | const float * | pf1, | |
const float * | pf2 | |||
) | [inline] |
double dot4 | ( | const double * | pf1, | |
const double * | pf2 | |||
) | [inline] |
float dot4 | ( | const float * | pf1, | |
const float * | pf2 | |||
) | [inline] |
void dQfromR | ( | dQuaternion | q, | |
const dMatrix3 | R | |||
) | [inline] |
void dQMultiply0 | ( | dQuaternion | qa, | |
const dQuaternion | qb, | |||
const dQuaternion | qc | |||
) | [inline] |
RaveVector<T> dQSlerp | ( | const RaveVector< T > & | qa, | |
const RaveVector< T > & | qb, | |||
T | t | |||
) | [inline] |
void dRfromQ | ( | dMatrix3 | R, | |
const dQuaternion | q | |||
) | [inline] |
int insideQuadrilateral | ( | const Vector * | p0, | |
const Vector * | p1, | |||
const Vector * | p2, | |||
const Vector * | p3 | |||
) |
double * inv3 | ( | const double * | pf, | |
double * | pfres, | |||
double * | pfdet, | |||
int | stride | |||
) | [inline] |
float * inv3 | ( | const float * | pf, | |
float * | pfres, | |||
float * | pfdet, | |||
int | stride | |||
) | [inline] |
double * inv4 | ( | const double * | pf, | |
double * | pfres | |||
) | [inline] |
T lengthsqr | ( | const T * | pf1, | |
const T * | pf2, | |||
int | length | |||
) | [inline] |
T matrixdet3 | ( | const T * | pf, | |
int | stride | |||
) | [inline] |
int Max | ( | T * | pts, | |
int | stride, | |||
int | numPts | |||
) | [inline] |
int Min | ( | T * | pts, | |
int | stride, | |||
int | numPts | |||
) | [inline] |
S * mult | ( | T * | pf1, | |
R * | pf2, | |||
int | r1, | |||
int | c1, | |||
int | c2, | |||
S * | pfres, | |||
bool | badd = false | |||
) | [inline] |
void mult | ( | T * | pf, | |
T | fa, | |||
int | r | |||
) | [inline] |
double * mult3_s3 | ( | double * | pfres, | |
const double * | pf1, | |||
const double * | pf2 | |||
) | [inline] |
float * mult3_s3 | ( | float * | pfres, | |
const float * | pf1, | |||
const float * | pf2 | |||
) | [inline] |
double * mult3_s4 | ( | double * | pfres, | |
const double * | pf1, | |||
const double * | pf2 | |||
) | [inline] |
float * mult3_s4 | ( | float * | pfres, | |
const float * | pf1, | |||
const float * | pf2 | |||
) | [inline] |
double * mult4 | ( | double * | pfres, | |
const double * | pf1, | |||
const double * | pf2 | |||
) | [inline] |
float * mult4 | ( | float * | pfres, | |
const float * | pf1, | |||
const float * | pf2 | |||
) | [inline] |
T * multto1 | ( | T * | pf1, | |
T * | pf2, | |||
int | r1, | |||
int | c1, | |||
T * | pftemp = NULL | |||
) | [inline] |
T * multto2 | ( | T * | pf1, | |
S * | pf2, | |||
int | r2, | |||
int | c2, | |||
S * | pftemp = NULL | |||
) | [inline] |
S * multtrans | ( | T * | pf1, | |
R * | pf2, | |||
int | r1, | |||
int | c1, | |||
int | c2, | |||
S * | pfres, | |||
bool | badd = false | |||
) | [inline] |
double * multtrans3 | ( | double * | pfres, | |
const double * | pf1, | |||
const double * | pf2 | |||
) | [inline] |
float * multtrans3 | ( | float * | pfres, | |
const float * | pf1, | |||
const float * | pf2 | |||
) | [inline] |
double * multtrans4 | ( | double * | pfres, | |
const double * | pf1, | |||
const double * | pf2 | |||
) | [inline] |
float * multtrans4 | ( | float * | pfres, | |
const float * | pf1, | |||
const float * | pf2 | |||
) | [inline] |
S * multtrans_to2 | ( | T * | pf1, | |
R * | pf2, | |||
int | r1, | |||
int | c1, | |||
int | r2, | |||
S * | pfres, | |||
bool | badd = false | |||
) | [inline] |
double * normalize2 | ( | double * | pfout, | |
const double * | pf | |||
) | [inline] |
float * normalize2 | ( | float * | pfout, | |
const float * | pf | |||
) | [inline] |
double * normalize3 | ( | double * | pfout, | |
const double * | pf | |||
) | [inline] |
float * normalize3 | ( | float * | pfout, | |
const float * | pf | |||
) | [inline] |
double * normalize4 | ( | double * | pfout, | |
const double * | pf | |||
) | [inline] |
float * normalize4 | ( | float * | pfout, | |
const float * | pf | |||
) | [inline] |
T normsqr | ( | const T * | pf1, | |
int | r | |||
) | [inline] |
RaveVector<T> operator* | ( | double | f, | |
const RaveVector< T > & | left | |||
) | [inline] |
RaveVector<T> operator* | ( | float | f, | |
const RaveVector< T > & | left | |||
) | [inline] |
std::basic_ostream<T>& operator<< | ( | std::basic_ostream< T > & | O, | |
const RaveTransformMatrix< U > & | v | |||
) | [inline] |
std::basic_ostream<T>& operator<< | ( | std::basic_ostream< T > & | O, | |
const RaveTransform< U > & | v | |||
) | [inline] |
std::basic_ostream<T>& operator<< | ( | std::basic_ostream< T > & | O, | |
const RaveVector< U > & | v | |||
) | [inline] |
std::basic_istream<T>& operator>> | ( | std::basic_istream< T > & | I, | |
RaveTransformMatrix< U > & | v | |||
) | [inline] |
std::basic_istream<T>& operator>> | ( | std::basic_istream< T > & | I, | |
RaveTransform< U > & | v | |||
) | [inline] |
std::basic_istream<T>& operator>> | ( | std::basic_istream< T > & | I, | |
RaveVector< U > & | v | |||
) | [inline] |
bool QLAlgorithm3 | ( | double * | m_aafEntry, | |
double * | afDiag, | |||
double * | afSubDiag | |||
) |
bool QLAlgorithm3 | ( | float * | m_aafEntry, | |
float * | afDiag, | |||
float * | afSubDiag | |||
) |
int solvequad | ( | T | a, | |
T | b, | |||
T | c, | |||
T & | r1, | |||
T & | r2 | |||
) | [inline] |
void sub | ( | T * | pf1, | |
T * | pf2, | |||
int | r | |||
) | [inline] |
SVD of a 3x3 matrix A such that A = U*diag(D)*V' where U is a 3x3 matrix, V is a 3x3 matrix, and D is a 3x1 vector The row stride for all matrices is 9 bytes
T* transcoord3 | ( | T * | pfout, | |
const RaveTransformMatrix< T > * | pmat, | |||
const T * | pf | |||
) | [inline] |
dReal* transcoord3 | ( | dReal * | pfout, | |
const TransformMatrix * | pmat, | |||
const dReal * | pf | |||
) | [inline] |
computes (*pmat) * v
T* transnorm3 | ( | T * | pfout, | |
const RaveTransformMatrix< T > * | pmat, | |||
const T * | pf | |||
) | [inline] |
dReal* transnorm3 | ( | dReal * | pfout, | |
const TransformMatrix * | pmat, | |||
const dReal * | pf | |||
) | [inline] |
uses only 3x3 upper submatrix
double * transnorm3 | ( | double * | pfout, | |
const double * | pfmat, | |||
const double * | pf | |||
) | [inline] |
float * transnorm3 | ( | float * | pfout, | |
const float * | pfmat, | |||
const float * | pf | |||
) | [inline] |
double * transpose3 | ( | const double * | pf, | |
double * | pfres | |||
) | [inline] |
float * transpose3 | ( | const float * | pf, | |
float * | pfres | |||
) | [inline] |
double * transpose4 | ( | const double * | pf, | |
double * | pfres | |||
) | [inline] |
float * transpose4 | ( | const float * | pf, | |
float * | pfres | |||
) | [inline] |
void Tridiagonal3 | ( | S * | mat, | |
T * | diag, | |||
T * | subd | |||
) | [inline] |
bool TriTriCollision | ( | const RaveVector< double > & | u1, | |
const RaveVector< double > & | u2, | |||
const RaveVector< double > & | u3, | |||
const RaveVector< double > & | v1, | |||
const RaveVector< double > & | v2, | |||
const RaveVector< double > & | v3, | |||
RaveVector< double > & | contactpos, | |||
RaveVector< double > & | contactnorm | |||
) |
bool TriTriCollision | ( | const RaveVector< float > & | u1, | |
const RaveVector< float > & | u2, | |||
const RaveVector< float > & | u3, | |||
const RaveVector< float > & | v1, | |||
const RaveVector< float > & | v2, | |||
const RaveVector< float > & | v3, | |||
RaveVector< float > & | contactpos, | |||
RaveVector< float > & | contactnorm | |||
) |