mirror of
https://github.com/TorqueGameEngines/Torque3D.git
synced 2026-02-19 06:33:49 +00:00
Revert "Updated SDL, Bullet and OpenAL soft libs"
This reverts commit 370161cfb1.
This commit is contained in:
AzaezelX
parent
160dc00c07
commit
e7ee94428e
1102 changed files with 62741 additions and 204988 deletions
|
|
@ -3,30 +3,25 @@
|
|||
#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;
|
||||
|
|
@ -38,8 +33,7 @@ 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);
|
||||
|
|
@ -51,28 +45,22 @@ 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;
|
||||
}
|
||||
}
|
||||
|
|
@ -80,75 +68,60 @@ 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())
|
||||
{
|
||||
bt_id_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()) {
|
||||
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())
|
||||
{
|
||||
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 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 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);
|
||||
}
|
||||
}
|
||||
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);
|
||||
}
|
||||
}
|
||||
}
|
||||
#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);
|
||||
|
|
@ -170,8 +143,7 @@ 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);
|
||||
|
|
@ -193,8 +165,7 @@ 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);
|
||||
|
|
@ -216,8 +187,7 @@ 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);
|
||||
|
|
@ -231,8 +201,7 @@ 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);
|
||||
|
|
@ -255,8 +224,7 @@ 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;
|
||||
|
|
@ -278,233 +246,192 @@ 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))
|
||||
{
|
||||
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));
|
||||
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));
|
||||
|
||||
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))
|
||||
{
|
||||
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));
|
||||
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));
|
||||
return false;
|
||||
}
|
||||
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));
|
||||
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));
|
||||
return false;
|
||||
}
|
||||
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));
|
||||
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));
|
||||
return false;
|
||||
}
|
||||
}
|
||||
// check positive/zero diagonal elements
|
||||
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));
|
||||
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));
|
||||
return false;
|
||||
}
|
||||
}
|
||||
// check symmetry
|
||||
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));
|
||||
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));
|
||||
return false;
|
||||
}
|
||||
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));
|
||||
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));
|
||||
return false;
|
||||
}
|
||||
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));
|
||||
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));
|
||||
return false;
|
||||
}
|
||||
return true;
|
||||
}
|
||||
|
||||
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))
|
||||
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))
|
||||
|
||||
// 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)
|
||||
{
|
||||
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);
|
||||
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);
|
||||
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)
|
||||
{
|
||||
bt_id_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) {
|
||||
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)
|
||||
{
|
||||
bt_id_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) {
|
||||
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)
|
||||
{
|
||||
bt_id_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) {
|
||||
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)
|
||||
{
|
||||
bt_id_error_message("Not a valid rotation matrix (determinant <=0)\n");
|
||||
if (determinant(m) <= 0) {
|
||||
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