mirror of
https://github.com/TorqueGameEngines/Torque3D.git
synced 2026-03-21 21:31:09 +00:00
Updated SDL, Bullet and OpenAL soft libs
Fixed case sensitivity problem Fixed clang compiler problem with having the class namespace used in an inline for the == operator Tweaked some theme stuff to be more consistent. Added initial test of no-pie for linux test sidestep of getTexCoord in shadergen hlsl feature so we don't assert when getting the terrain's shaderstuffs(which uses float3 instead of normal float2)
This commit is contained in:
Areloch
parent
e87dc787ee
commit
f8750dd8ed
1102 changed files with 205083 additions and 62836 deletions
|
|
@ -3,25 +3,30 @@
|
|||
#include <cmath>
|
||||
#include <limits>
|
||||
|
||||
namespace btInverseDynamics {
|
||||
namespace btInverseDynamics
|
||||
{
|
||||
static const idScalar kIsZero = 5 * std::numeric_limits<idScalar>::epsilon();
|
||||
// requirements for axis length deviation from 1.0
|
||||
// experimentally set from random euler angle rotation matrices
|
||||
static const idScalar kAxisLengthEpsilon = 10 * kIsZero;
|
||||
|
||||
void setZero(vec3 &v) {
|
||||
void setZero(vec3 &v)
|
||||
{
|
||||
v(0) = 0;
|
||||
v(1) = 0;
|
||||
v(2) = 0;
|
||||
}
|
||||
|
||||
void setZero(vecx &v) {
|
||||
for (int i = 0; i < v.size(); i++) {
|
||||
void setZero(vecx &v)
|
||||
{
|
||||
for (int i = 0; i < v.size(); i++)
|
||||
{
|
||||
v(i) = 0;
|
||||
}
|
||||
}
|
||||
|
||||
void setZero(mat33 &m) {
|
||||
void setZero(mat33 &m)
|
||||
{
|
||||
m(0, 0) = 0;
|
||||
m(0, 1) = 0;
|
||||
m(0, 2) = 0;
|
||||
|
|
@ -33,7 +38,8 @@ void setZero(mat33 &m) {
|
|||
m(2, 2) = 0;
|
||||
}
|
||||
|
||||
void skew(vec3& v, mat33* result) {
|
||||
void skew(vec3 &v, mat33 *result)
|
||||
{
|
||||
(*result)(0, 0) = 0.0;
|
||||
(*result)(0, 1) = -v(2);
|
||||
(*result)(0, 2) = v(1);
|
||||
|
|
@ -45,22 +51,28 @@ void skew(vec3& v, mat33* result) {
|
|||
(*result)(2, 2) = 0.0;
|
||||
}
|
||||
|
||||
idScalar maxAbs(const vecx &v) {
|
||||
idScalar maxAbs(const vecx &v)
|
||||
{
|
||||
idScalar result = 0.0;
|
||||
for (int i = 0; i < v.size(); i++) {
|
||||
for (int i = 0; i < v.size(); i++)
|
||||
{
|
||||
const idScalar tmp = BT_ID_FABS(v(i));
|
||||
if (tmp > result) {
|
||||
if (tmp > result)
|
||||
{
|
||||
result = tmp;
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
idScalar maxAbs(const vec3 &v) {
|
||||
idScalar maxAbs(const vec3 &v)
|
||||
{
|
||||
idScalar result = 0.0;
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
const idScalar tmp = BT_ID_FABS(v(i));
|
||||
if (tmp > result) {
|
||||
if (tmp > result)
|
||||
{
|
||||
result = tmp;
|
||||
}
|
||||
}
|
||||
|
|
@ -68,60 +80,75 @@ idScalar maxAbs(const vec3 &v) {
|
|||
}
|
||||
|
||||
#if (defined BT_ID_HAVE_MAT3X)
|
||||
idScalar maxAbsMat3x(const mat3x &m) {
|
||||
// only used for tests -- so just loop here for portability
|
||||
idScalar result = 0.0;
|
||||
for (idArrayIdx col = 0; col < m.cols(); col++) {
|
||||
for (idArrayIdx row = 0; row < 3; row++) {
|
||||
result = BT_ID_MAX(result, std::fabs(m(row, col)));
|
||||
}
|
||||
}
|
||||
return result;
|
||||
idScalar maxAbsMat3x(const mat3x &m)
|
||||
{
|
||||
// only used for tests -- so just loop here for portability
|
||||
idScalar result = 0.0;
|
||||
for (idArrayIdx col = 0; col < m.cols(); col++)
|
||||
{
|
||||
for (idArrayIdx row = 0; row < 3; row++)
|
||||
{
|
||||
result = BT_ID_MAX(result, std::fabs(m(row, col)));
|
||||
}
|
||||
}
|
||||
return result;
|
||||
}
|
||||
|
||||
void mul(const mat33 &a, const mat3x &b, mat3x *result) {
|
||||
if (b.cols() != result->cols()) {
|
||||
error_message("size missmatch. b.cols()= %d, result->cols()= %d\n",
|
||||
static_cast<int>(b.cols()), static_cast<int>(result->cols()));
|
||||
abort();
|
||||
}
|
||||
void mul(const mat33 &a, const mat3x &b, mat3x *result)
|
||||
{
|
||||
if (b.cols() != result->cols())
|
||||
{
|
||||
bt_id_error_message("size missmatch. b.cols()= %d, result->cols()= %d\n",
|
||||
static_cast<int>(b.cols()), static_cast<int>(result->cols()));
|
||||
abort();
|
||||
}
|
||||
|
||||
for (idArrayIdx col = 0; col < b.cols(); col++) {
|
||||
const idScalar x = a(0,0)*b(0,col)+a(0,1)*b(1,col)+a(0,2)*b(2,col);
|
||||
const idScalar y = a(1,0)*b(0,col)+a(1,1)*b(1,col)+a(1,2)*b(2,col);
|
||||
const idScalar z = a(2,0)*b(0,col)+a(2,1)*b(1,col)+a(2,2)*b(2,col);
|
||||
setMat3xElem(0, col, x, result);
|
||||
setMat3xElem(1, col, y, result);
|
||||
setMat3xElem(2, col, z, result);
|
||||
}
|
||||
for (idArrayIdx col = 0; col < b.cols(); col++)
|
||||
{
|
||||
const idScalar x = a(0, 0) * b(0, col) + a(0, 1) * b(1, col) + a(0, 2) * b(2, col);
|
||||
const idScalar y = a(1, 0) * b(0, col) + a(1, 1) * b(1, col) + a(1, 2) * b(2, col);
|
||||
const idScalar z = a(2, 0) * b(0, col) + a(2, 1) * b(1, col) + a(2, 2) * b(2, col);
|
||||
setMat3xElem(0, col, x, result);
|
||||
setMat3xElem(1, col, y, result);
|
||||
setMat3xElem(2, col, z, result);
|
||||
}
|
||||
}
|
||||
void add(const mat3x &a, const mat3x &b, mat3x *result) {
|
||||
if (a.cols() != b.cols()) {
|
||||
error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
|
||||
static_cast<int>(a.cols()), static_cast<int>(b.cols()));
|
||||
abort();
|
||||
}
|
||||
for (idArrayIdx col = 0; col < b.cols(); col++) {
|
||||
for (idArrayIdx row = 0; row < 3; row++) {
|
||||
setMat3xElem(row, col, a(row, col) + b(row, col), result);
|
||||
}
|
||||
}
|
||||
void add(const mat3x &a, const mat3x &b, mat3x *result)
|
||||
{
|
||||
if (a.cols() != b.cols())
|
||||
{
|
||||
bt_id_error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
|
||||
static_cast<int>(a.cols()), static_cast<int>(b.cols()));
|
||||
abort();
|
||||
}
|
||||
for (idArrayIdx col = 0; col < b.cols(); col++)
|
||||
{
|
||||
for (idArrayIdx row = 0; row < 3; row++)
|
||||
{
|
||||
setMat3xElem(row, col, a(row, col) + b(row, col), result);
|
||||
}
|
||||
}
|
||||
}
|
||||
void sub(const mat3x &a, const mat3x &b, mat3x *result) {
|
||||
if (a.cols() != b.cols()) {
|
||||
error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
|
||||
static_cast<int>(a.cols()), static_cast<int>(b.cols()));
|
||||
abort();
|
||||
}
|
||||
for (idArrayIdx col = 0; col < b.cols(); col++) {
|
||||
for (idArrayIdx row = 0; row < 3; row++) {
|
||||
setMat3xElem(row, col, a(row, col) - b(row, col), result);
|
||||
}
|
||||
}
|
||||
void sub(const mat3x &a, const mat3x &b, mat3x *result)
|
||||
{
|
||||
if (a.cols() != b.cols())
|
||||
{
|
||||
bt_id_error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
|
||||
static_cast<int>(a.cols()), static_cast<int>(b.cols()));
|
||||
abort();
|
||||
}
|
||||
for (idArrayIdx col = 0; col < b.cols(); col++)
|
||||
{
|
||||
for (idArrayIdx row = 0; row < 3; row++)
|
||||
{
|
||||
setMat3xElem(row, col, a(row, col) - b(row, col), result);
|
||||
}
|
||||
}
|
||||
}
|
||||
#endif
|
||||
|
||||
mat33 transformX(const idScalar &alpha) {
|
||||
mat33 transformX(const idScalar &alpha)
|
||||
{
|
||||
mat33 T;
|
||||
const idScalar cos_alpha = BT_ID_COS(alpha);
|
||||
const idScalar sin_alpha = BT_ID_SIN(alpha);
|
||||
|
|
@ -143,7 +170,8 @@ mat33 transformX(const idScalar &alpha) {
|
|||
return T;
|
||||
}
|
||||
|
||||
mat33 transformY(const idScalar &beta) {
|
||||
mat33 transformY(const idScalar &beta)
|
||||
{
|
||||
mat33 T;
|
||||
const idScalar cos_beta = BT_ID_COS(beta);
|
||||
const idScalar sin_beta = BT_ID_SIN(beta);
|
||||
|
|
@ -165,7 +193,8 @@ mat33 transformY(const idScalar &beta) {
|
|||
return T;
|
||||
}
|
||||
|
||||
mat33 transformZ(const idScalar &gamma) {
|
||||
mat33 transformZ(const idScalar &gamma)
|
||||
{
|
||||
mat33 T;
|
||||
const idScalar cos_gamma = BT_ID_COS(gamma);
|
||||
const idScalar sin_gamma = BT_ID_SIN(gamma);
|
||||
|
|
@ -187,7 +216,8 @@ mat33 transformZ(const idScalar &gamma) {
|
|||
return T;
|
||||
}
|
||||
|
||||
mat33 tildeOperator(const vec3 &v) {
|
||||
mat33 tildeOperator(const vec3 &v)
|
||||
{
|
||||
mat33 m;
|
||||
m(0, 0) = 0.0;
|
||||
m(0, 1) = -v(2);
|
||||
|
|
@ -201,7 +231,8 @@ mat33 tildeOperator(const vec3 &v) {
|
|||
return m;
|
||||
}
|
||||
|
||||
void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3 *r, mat33 *T) {
|
||||
void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3 *r, mat33 *T)
|
||||
{
|
||||
const idScalar sa = BT_ID_SIN(alpha);
|
||||
const idScalar ca = BT_ID_COS(alpha);
|
||||
const idScalar st = BT_ID_SIN(theta);
|
||||
|
|
@ -224,7 +255,8 @@ void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec
|
|||
(*T)(2, 2) = ca;
|
||||
}
|
||||
|
||||
void bodyTParentFromAxisAngle(const vec3 &axis, const idScalar &angle, mat33 *T) {
|
||||
void bodyTParentFromAxisAngle(const vec3 &axis, const idScalar &angle, mat33 *T)
|
||||
{
|
||||
const idScalar c = BT_ID_COS(angle);
|
||||
const idScalar s = -BT_ID_SIN(angle);
|
||||
const idScalar one_m_c = 1.0 - c;
|
||||
|
|
@ -246,192 +278,233 @@ void bodyTParentFromAxisAngle(const vec3 &axis, const idScalar &angle, mat33 *T)
|
|||
(*T)(2, 2) = z * z * one_m_c + c;
|
||||
}
|
||||
|
||||
bool isPositiveDefinite(const mat33 &m) {
|
||||
bool isPositiveDefinite(const mat33 &m)
|
||||
{
|
||||
// test if all upper left determinants are positive
|
||||
if (m(0, 0) <= 0) { // upper 1x1
|
||||
if (m(0, 0) <= 0)
|
||||
{ // upper 1x1
|
||||
return false;
|
||||
}
|
||||
if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) <= 0) { // upper 2x2
|
||||
if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) <= 0)
|
||||
{ // upper 2x2
|
||||
return false;
|
||||
}
|
||||
if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) -
|
||||
m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
|
||||
m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0) {
|
||||
m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool isPositiveSemiDefinite(const mat33 &m) {
|
||||
bool isPositiveSemiDefinite(const mat33 &m)
|
||||
{
|
||||
// test if all upper left determinants are positive
|
||||
if (m(0, 0) < 0) { // upper 1x1
|
||||
if (m(0, 0) < 0)
|
||||
{ // upper 1x1
|
||||
return false;
|
||||
}
|
||||
if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < 0) { // upper 2x2
|
||||
if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < 0)
|
||||
{ // upper 2x2
|
||||
return false;
|
||||
}
|
||||
if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) -
|
||||
m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
|
||||
m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0) {
|
||||
m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool isPositiveSemiDefiniteFuzzy(const mat33 &m) {
|
||||
bool isPositiveSemiDefiniteFuzzy(const mat33 &m)
|
||||
{
|
||||
// test if all upper left determinants are positive
|
||||
if (m(0, 0) < -kIsZero) { // upper 1x1
|
||||
if (m(0, 0) < -kIsZero)
|
||||
{ // upper 1x1
|
||||
return false;
|
||||
}
|
||||
if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < -kIsZero) { // upper 2x2
|
||||
if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < -kIsZero)
|
||||
{ // upper 2x2
|
||||
return false;
|
||||
}
|
||||
if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) -
|
||||
m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
|
||||
m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < -kIsZero) {
|
||||
m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < -kIsZero)
|
||||
{
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
idScalar determinant(const mat33 &m) {
|
||||
idScalar determinant(const mat33 &m)
|
||||
{
|
||||
return m(0, 0) * m(1, 1) * m(2, 2) + m(0, 1) * m(1, 2) * m(2, 0) + m(0, 2) * m(1, 0) * m(2, 1) -
|
||||
m(0, 2) * m(1, 1) * m(2, 0) - m(0, 0) * m(1, 2) * m(2, 1) - m(0, 1) * m(1, 0) * m(2, 2);
|
||||
}
|
||||
|
||||
bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint) {
|
||||
bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
|
||||
{
|
||||
// TODO(Thomas) do we really want this?
|
||||
// in cases where the inertia tensor about the center of mass is zero,
|
||||
// the determinant of the inertia tensor about the joint axis is almost
|
||||
// zero and can have a very small negative value.
|
||||
if (!isPositiveSemiDefiniteFuzzy(I)) {
|
||||
error_message("invalid inertia matrix for body %d, not positive definite "
|
||||
"(fixed joint)\n",
|
||||
index);
|
||||
error_message("matrix is:\n"
|
||||
"[%.20e %.20e %.20e;\n"
|
||||
"%.20e %.20e %.20e;\n"
|
||||
"%.20e %.20e %.20e]\n",
|
||||
I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
|
||||
I(2, 2));
|
||||
if (!isPositiveSemiDefiniteFuzzy(I))
|
||||
{
|
||||
bt_id_error_message(
|
||||
"invalid inertia matrix for body %d, not positive definite "
|
||||
"(fixed joint)\n",
|
||||
index);
|
||||
bt_id_error_message(
|
||||
"matrix is:\n"
|
||||
"[%.20e %.20e %.20e;\n"
|
||||
"%.20e %.20e %.20e;\n"
|
||||
"%.20e %.20e %.20e]\n",
|
||||
I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
|
||||
I(2, 2));
|
||||
|
||||
return false;
|
||||
}
|
||||
|
||||
// check triangle inequality, must have I(i,i)+I(j,j)>=I(k,k)
|
||||
if (!has_fixed_joint) {
|
||||
if (I(0, 0) + I(1, 1) < I(2, 2)) {
|
||||
error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
|
||||
error_message("matrix is:\n"
|
||||
"[%.20e %.20e %.20e;\n"
|
||||
"%.20e %.20e %.20e;\n"
|
||||
"%.20e %.20e %.20e]\n",
|
||||
I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
|
||||
I(2, 2));
|
||||
if (!has_fixed_joint)
|
||||
{
|
||||
if (I(0, 0) + I(1, 1) < I(2, 2))
|
||||
{
|
||||
bt_id_error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
|
||||
bt_id_error_message(
|
||||
"matrix is:\n"
|
||||
"[%.20e %.20e %.20e;\n"
|
||||
"%.20e %.20e %.20e;\n"
|
||||
"%.20e %.20e %.20e]\n",
|
||||
I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
|
||||
I(2, 2));
|
||||
return false;
|
||||
}
|
||||
if (I(0, 0) + I(1, 1) < I(2, 2)) {
|
||||
error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
|
||||
error_message("matrix is:\n"
|
||||
"[%.20e %.20e %.20e;\n"
|
||||
"%.20e %.20e %.20e;\n"
|
||||
"%.20e %.20e %.20e]\n",
|
||||
I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
|
||||
I(2, 2));
|
||||
if (I(0, 0) + I(1, 1) < I(2, 2))
|
||||
{
|
||||
bt_id_error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
|
||||
bt_id_error_message(
|
||||
"matrix is:\n"
|
||||
"[%.20e %.20e %.20e;\n"
|
||||
"%.20e %.20e %.20e;\n"
|
||||
"%.20e %.20e %.20e]\n",
|
||||
I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
|
||||
I(2, 2));
|
||||
return false;
|
||||
}
|
||||
if (I(1, 1) + I(2, 2) < I(0, 0)) {
|
||||
error_message("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index);
|
||||
error_message("matrix is:\n"
|
||||
"[%.20e %.20e %.20e;\n"
|
||||
"%.20e %.20e %.20e;\n"
|
||||
"%.20e %.20e %.20e]\n",
|
||||
I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
|
||||
I(2, 2));
|
||||
if (I(1, 1) + I(2, 2) < I(0, 0))
|
||||
{
|
||||
bt_id_error_message("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index);
|
||||
bt_id_error_message(
|
||||
"matrix is:\n"
|
||||
"[%.20e %.20e %.20e;\n"
|
||||
"%.20e %.20e %.20e;\n"
|
||||
"%.20e %.20e %.20e]\n",
|
||||
I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
|
||||
I(2, 2));
|
||||
return false;
|
||||
}
|
||||
}
|
||||
// check positive/zero diagonal elements
|
||||
for (int i = 0; i < 3; i++) {
|
||||
if (I(i, i) < 0) { // accept zero
|
||||
error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i));
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
if (I(i, i) < 0)
|
||||
{ // accept zero
|
||||
bt_id_error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i));
|
||||
return false;
|
||||
}
|
||||
}
|
||||
// check symmetry
|
||||
if (BT_ID_FABS(I(1, 0) - I(0, 1)) > kIsZero) {
|
||||
error_message("invalid inertia tensor for body %d I(1,0)!=I(0,1). I(1,0)-I(0,1)= "
|
||||
"%e\n",
|
||||
index, I(1, 0) - I(0, 1));
|
||||
if (BT_ID_FABS(I(1, 0) - I(0, 1)) > kIsZero)
|
||||
{
|
||||
bt_id_error_message(
|
||||
"invalid inertia tensor for body %d I(1,0)!=I(0,1). I(1,0)-I(0,1)= "
|
||||
"%e\n",
|
||||
index, I(1, 0) - I(0, 1));
|
||||
return false;
|
||||
}
|
||||
if (BT_ID_FABS(I(2, 0) - I(0, 2)) > kIsZero) {
|
||||
error_message("invalid inertia tensor for body %d I(2,0)!=I(0,2). I(2,0)-I(0,2)= "
|
||||
"%e\n",
|
||||
index, I(2, 0) - I(0, 2));
|
||||
if (BT_ID_FABS(I(2, 0) - I(0, 2)) > kIsZero)
|
||||
{
|
||||
bt_id_error_message(
|
||||
"invalid inertia tensor for body %d I(2,0)!=I(0,2). I(2,0)-I(0,2)= "
|
||||
"%e\n",
|
||||
index, I(2, 0) - I(0, 2));
|
||||
return false;
|
||||
}
|
||||
if (BT_ID_FABS(I(1, 2) - I(2, 1)) > kIsZero) {
|
||||
error_message("invalid inertia tensor body %d I(1,2)!=I(2,1). I(1,2)-I(2,1)= %e\n", index,
|
||||
I(1, 2) - I(2, 1));
|
||||
if (BT_ID_FABS(I(1, 2) - I(2, 1)) > kIsZero)
|
||||
{
|
||||
bt_id_error_message("invalid inertia tensor body %d I(1,2)!=I(2,1). I(1,2)-I(2,1)= %e\n", index,
|
||||
I(1, 2) - I(2, 1));
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool isValidTransformMatrix(const mat33 &m) {
|
||||
#define print_mat(x) \
|
||||
error_message("matrix is [%e, %e, %e; %e, %e, %e; %e, %e, %e]\n", x(0, 0), x(0, 1), x(0, 2), \
|
||||
x(1, 0), x(1, 1), x(1, 2), x(2, 0), x(2, 1), x(2, 2))
|
||||
bool isValidTransformMatrix(const mat33 &m)
|
||||
{
|
||||
#define print_mat(x) \
|
||||
bt_id_error_message("matrix is [%e, %e, %e; %e, %e, %e; %e, %e, %e]\n", x(0, 0), x(0, 1), x(0, 2), \
|
||||
x(1, 0), x(1, 1), x(1, 2), x(2, 0), x(2, 1), x(2, 2))
|
||||
|
||||
// check for unit length column vectors
|
||||
for (int i = 0; i < 3; i++) {
|
||||
for (int i = 0; i < 3; i++)
|
||||
{
|
||||
const idScalar length_minus_1 =
|
||||
BT_ID_FABS(m(0, i) * m(0, i) + m(1, i) * m(1, i) + m(2, i) * m(2, i) - 1.0);
|
||||
if (length_minus_1 > kAxisLengthEpsilon) {
|
||||
error_message("Not a valid rotation matrix (column %d not unit length)\n"
|
||||
"column = [%.18e %.18e %.18e]\n"
|
||||
"length-1.0= %.18e\n",
|
||||
i, m(0, i), m(1, i), m(2, i), length_minus_1);
|
||||
if (length_minus_1 > kAxisLengthEpsilon)
|
||||
{
|
||||
bt_id_error_message(
|
||||
"Not a valid rotation matrix (column %d not unit length)\n"
|
||||
"column = [%.18e %.18e %.18e]\n"
|
||||
"length-1.0= %.18e\n",
|
||||
i, m(0, i), m(1, i), m(2, i), length_minus_1);
|
||||
print_mat(m);
|
||||
return false;
|
||||
}
|
||||
}
|
||||
// check for orthogonal column vectors
|
||||
if (BT_ID_FABS(m(0, 0) * m(0, 1) + m(1, 0) * m(1, 1) + m(2, 0) * m(2, 1)) > kAxisLengthEpsilon) {
|
||||
error_message("Not a valid rotation matrix (columns 0 and 1 not orthogonal)\n");
|
||||
if (BT_ID_FABS(m(0, 0) * m(0, 1) + m(1, 0) * m(1, 1) + m(2, 0) * m(2, 1)) > kAxisLengthEpsilon)
|
||||
{
|
||||
bt_id_error_message("Not a valid rotation matrix (columns 0 and 1 not orthogonal)\n");
|
||||
print_mat(m);
|
||||
return false;
|
||||
}
|
||||
if (BT_ID_FABS(m(0, 0) * m(0, 2) + m(1, 0) * m(1, 2) + m(2, 0) * m(2, 2)) > kAxisLengthEpsilon) {
|
||||
error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
|
||||
if (BT_ID_FABS(m(0, 0) * m(0, 2) + m(1, 0) * m(1, 2) + m(2, 0) * m(2, 2)) > kAxisLengthEpsilon)
|
||||
{
|
||||
bt_id_error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
|
||||
print_mat(m);
|
||||
return false;
|
||||
}
|
||||
if (BT_ID_FABS(m(0, 1) * m(0, 2) + m(1, 1) * m(1, 2) + m(2, 1) * m(2, 2)) > kAxisLengthEpsilon) {
|
||||
error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
|
||||
if (BT_ID_FABS(m(0, 1) * m(0, 2) + m(1, 1) * m(1, 2) + m(2, 1) * m(2, 2)) > kAxisLengthEpsilon)
|
||||
{
|
||||
bt_id_error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
|
||||
print_mat(m);
|
||||
return false;
|
||||
}
|
||||
// check determinant (rotation not reflection)
|
||||
if (determinant(m) <= 0) {
|
||||
error_message("Not a valid rotation matrix (determinant <=0)\n");
|
||||
if (determinant(m) <= 0)
|
||||
{
|
||||
bt_id_error_message("Not a valid rotation matrix (determinant <=0)\n");
|
||||
print_mat(m);
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
bool isUnitVector(const vec3 &vector) {
|
||||
bool isUnitVector(const vec3 &vector)
|
||||
{
|
||||
return BT_ID_FABS(vector(0) * vector(0) + vector(1) * vector(1) + vector(2) * vector(2) - 1.0) <
|
||||
kIsZero;
|
||||
}
|
||||
|
||||
vec3 rpyFromMatrix(const mat33 &rot) {
|
||||
vec3 rpyFromMatrix(const mat33 &rot)
|
||||
{
|
||||
vec3 rpy;
|
||||
rpy(2) = BT_ID_ATAN2(-rot(1, 0), rot(0, 0));
|
||||
rpy(1) = BT_ID_ATAN2(rot(2, 0), BT_ID_COS(rpy(2)) * rot(0, 0) - BT_ID_SIN(rpy(0)) * rot(1, 0));
|
||||
rpy(0) = BT_ID_ATAN2(-rot(2, 0), rot(2, 2));
|
||||
return rpy;
|
||||
}
|
||||
}
|
||||
} // namespace btInverseDynamics
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue