101 lines
3.8 KiB
C++
101 lines
3.8 KiB
C++
/// @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
|
|
{
|
|
/// 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_
|