dali-physics/third-party/bullet3/src/BulletInverseDynamics/IDMath.hpp

   1 /// @file Math utility functions used in inverse dynamics library.
   2 ///        Defined here as they may not be provided by the math library.
   3
   4 #ifndef IDMATH_HPP_
   5 #define IDMATH_HPP_
   6 #include "IDConfig.hpp"
   7
   8 namespace btInverseDynamics
   9 {
  10 /// set all elements to zero
  11 void setZero(vec3& v);
  12 /// set all elements to zero
  13 void setZero(vecx& v);
  14 /// set all elements to zero
  15 void setZero(mat33& m);
  16 /// create a skew symmetric matrix from a vector (useful for cross product abstraction, e.g. v x a = V * a)
  17 void skew(vec3& v, mat33* result);
  18 /// return maximum absolute value
  19 idScalar maxAbs(const vecx& v);
  20 #ifndef ID_LINEAR_MATH_USE_EIGEN
  21 /// return maximum absolute value
  22 idScalar maxAbs(const vec3& v);
  23 #endif  //ID_LINEAR_MATH_USE_EIGEN
  24
  25 #if (defined BT_ID_HAVE_MAT3X)
  26 idScalar maxAbsMat3x(const mat3x& m);
  27 void setZero(mat3x& m);
  28 // define math functions on mat3x here to avoid allocations in operators.
  29 void mul(const mat33& a, const mat3x& b, mat3x* result);
  30 void add(const mat3x& a, const mat3x& b, mat3x* result);
  31 void sub(const mat3x& a, const mat3x& b, mat3x* result);
  32 #endif
  33
  34 /// get offset vector & transform matrix from DH parameters
  35 /// TODO: add documentation
  36 void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3* r, mat33* T);
  37
  38 /// Check if a 3x3 matrix is positive definite
  39 /// @param m a 3x3 matrix
  40 /// @return true if m>0, false otherwise
  41 bool isPositiveDefinite(const mat33& m);
  42
  43 /// Check if a 3x3 matrix is positive semi definite
  44 /// @param m a 3x3 matrix
  45 /// @return true if m>=0, false otherwise
  46 bool isPositiveSemiDefinite(const mat33& m);
  47 /// Check if a 3x3 matrix is positive semi definite within numeric limits
  48 /// @param m a 3x3 matrix
  49 /// @return true if m>=-eps, false otherwise
  50 bool isPositiveSemiDefiniteFuzzy(const mat33& m);
  51
  52 /// Determinant of 3x3 matrix
  53 /// NOTE: implemented here for portability, as determinant operation
  54 ///        will be implemented differently for various matrix/vector libraries
  55 /// @param m a 3x3 matrix
  56 /// @return det(m)
  57 idScalar determinant(const mat33& m);
  58
  59 /// Test if a 3x3 matrix satisfies some properties of inertia matrices
  60 /// @param I a 3x3 matrix
  61 /// @param index body index (for error messages)
  62 /// @param has_fixed_joint: if true, positive semi-definite matrices are accepted
  63 /// @return true if I satisfies inertia matrix properties, false otherwise.
  64 bool isValidInertiaMatrix(const mat33& I, int index, bool has_fixed_joint);
  65
  66 /// Check if a 3x3 matrix is a valid transform (rotation) matrix
  67 /// @param m a 3x3 matrix
  68 /// @return true if m is a rotation matrix, false otherwise
  69 bool isValidTransformMatrix(const mat33& m);
  70 /// Transform matrix from parent to child frame,
  71 /// when the child frame is rotated about @param axis by @angle
  72 /// (mathematically positive)
  73 /// @param axis the axis of rotation
  74 /// @param angle rotation angle
  75 /// @param T pointer to transform matrix
  76 void bodyTParentFromAxisAngle(const vec3& axis, const idScalar& angle, mat33* T);
  77
  78 /// Check if this is a unit vector
  79 /// @param vector
  80 /// @return true if |vector|=1 within numeric limits
  81 bool isUnitVector(const vec3& vector);
  82
  83 /// @input a vector in R^3
  84 /// @returns corresponding spin tensor
  85 mat33 tildeOperator(const vec3& v);
  86 /// @param alpha angle in radians
  87 /// @returns transform matrix for ratation with @param alpha about x-axis
  88 mat33 transformX(const idScalar& alpha);
  89 /// @param beta angle in radians
  90 /// @returns transform matrix for ratation with @param beta about y-axis
  91 mat33 transformY(const idScalar& beta);
  92 /// @param gamma angle in radians
  93 /// @returns transform matrix for ratation with @param gamma about z-axis
  94 mat33 transformZ(const idScalar& gamma);
  95 ///calculate rpy angles (x-y-z Euler angles) from a given rotation matrix
  96 /// @param rot rotation matrix
  97 /// @returns x-y-z Euler angles
  98 vec3 rpyFromMatrix(const mat33& rot);
  99 }  // namespace btInverseDynamics
 100 #endif  // IDMATH_HPP_