1
  2
  3
  4
  5
  6
  7
  8
  9
 10
 11
 12
 13
 14
 15
 16
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
/// @file Math utility functions used in inverse dynamics library.
///	   Defined here as they may not be provided by the math library.

#ifndef IDMATH_HPP_
#define IDMATH_HPP_
#include "IDConfig.hpp"

namespace btInverseDynamics<--- Skipping configuration 'IDCONFIG_HPP_;btInverseDynamics' since the value of 'btInverseDynamics' is unknown. Use -D if you want to check it. You can use -U to skip it explicitly.
{
/// set all elements to zero
void setZero(vec3& v);
/// set all elements to zero
void setZero(vecx& v);
/// set all elements to zero
void setZero(mat33& m);
/// create a skew symmetric matrix from a vector (useful for cross product abstraction, e.g. v x a = V * a)
void skew(vec3& v, mat33* result);
/// return maximum absolute value
idScalar maxAbs(const vecx& v);
#ifndef ID_LINEAR_MATH_USE_EIGEN
/// return maximum absolute value
idScalar maxAbs(const vec3& v);
#endif  //ID_LINEAR_MATH_USE_EIGEN

#if (defined BT_ID_HAVE_MAT3X)
idScalar maxAbsMat3x(const mat3x& m);
void setZero(mat3x& m);
// define math functions on mat3x here to avoid allocations in operators.
void mul(const mat33& a, const mat3x& b, mat3x* result);
void add(const mat3x& a, const mat3x& b, mat3x* result);
void sub(const mat3x& a, const mat3x& b, mat3x* result);
#endif

/// get offset vector & transform matrix from DH parameters
/// TODO: add documentation
void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3* r, mat33* T);

/// Check if a 3x3 matrix is positive definite
/// @param m a 3x3 matrix
/// @return true if m>0, false otherwise
bool isPositiveDefinite(const mat33& m);

/// Check if a 3x3 matrix is positive semi definite
/// @param m a 3x3 matrix
/// @return true if m>=0, false otherwise
bool isPositiveSemiDefinite(const mat33& m);
/// Check if a 3x3 matrix is positive semi definite within numeric limits
/// @param m a 3x3 matrix
/// @return true if m>=-eps, false otherwise
bool isPositiveSemiDefiniteFuzzy(const mat33& m);

/// Determinant of 3x3 matrix
/// NOTE: implemented here for portability, as determinant operation
///	   will be implemented differently for various matrix/vector libraries
/// @param m a 3x3 matrix
/// @return det(m)
idScalar determinant(const mat33& m);

/// Test if a 3x3 matrix satisfies some properties of inertia matrices
/// @param I a 3x3 matrix
/// @param index body index (for error messages)
/// @param has_fixed_joint: if true, positive semi-definite matrices are accepted
/// @return true if I satisfies inertia matrix properties, false otherwise.
bool isValidInertiaMatrix(const mat33& I, int index, bool has_fixed_joint);

/// Check if a 3x3 matrix is a valid transform (rotation) matrix
/// @param m a 3x3 matrix
/// @return true if m is a rotation matrix, false otherwise
bool isValidTransformMatrix(const mat33& m);
/// Transform matrix from parent to child frame,
/// when the child frame is rotated about @param axis by @angle
/// (mathematically positive)
/// @param axis the axis of rotation
/// @param angle rotation angle
/// @param T pointer to transform matrix
void bodyTParentFromAxisAngle(const vec3& axis, const idScalar& angle, mat33* T);

/// Check if this is a unit vector
/// @param vector
/// @return true if |vector|=1 within numeric limits
bool isUnitVector(const vec3& vector);

/// @input a vector in R^3
/// @returns corresponding spin tensor
mat33 tildeOperator(const vec3& v);
/// @param alpha angle in radians
/// @returns transform matrix for ratation with @param alpha about x-axis
mat33 transformX(const idScalar& alpha);
/// @param beta angle in radians
/// @returns transform matrix for ratation with @param beta about y-axis
mat33 transformY(const idScalar& beta);
/// @param gamma angle in radians
/// @returns transform matrix for ratation with @param gamma about z-axis
mat33 transformZ(const idScalar& gamma);
///calculate rpy angles (x-y-z Euler angles) from a given rotation matrix
/// @param rot rotation matrix
/// @returns x-y-z Euler angles
vec3 rpyFromMatrix(const mat33& rot);
}  // namespace btInverseDynamics
#endif  // IDMATH_HPP_