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

   1 #include "IDMath.hpp"
   2
   3 #include <cmath>
   4 #include <limits>
   5
   6 namespace btInverseDynamics
   7 {
   8 static const idScalar kIsZero = 5 * std::numeric_limits<idScalar>::epsilon();
   9 // requirements for axis length deviation from 1.0
  10 // experimentally set from random euler angle rotation matrices
  11 static const idScalar kAxisLengthEpsilon = 10 * kIsZero;
  12
  13 void setZero(vec3 &v)
  14 {
  15         v(0) = 0;
  16         v(1) = 0;
  17         v(2) = 0;
  18 }
  19
  20 void setZero(vecx &v)
  21 {
  22         for (int i = 0; i < v.size(); i++)
  23         {
  24                 v(i) = 0;
  25         }
  26 }
  27
  28 void setZero(mat33 &m)
  29 {
  30         m(0, 0) = 0;
  31         m(0, 1) = 0;
  32         m(0, 2) = 0;
  33         m(1, 0) = 0;
  34         m(1, 1) = 0;
  35         m(1, 2) = 0;
  36         m(2, 0) = 0;
  37         m(2, 1) = 0;
  38         m(2, 2) = 0;
  39 }
  40
  41 void skew(vec3 &v, mat33 *result)
  42 {
  43         (*result)(0, 0) = 0.0;
  44         (*result)(0, 1) = -v(2);
  45         (*result)(0, 2) = v(1);
  46         (*result)(1, 0) = v(2);
  47         (*result)(1, 1) = 0.0;
  48         (*result)(1, 2) = -v(0);
  49         (*result)(2, 0) = -v(1);
  50         (*result)(2, 1) = v(0);
  51         (*result)(2, 2) = 0.0;
  52 }
  53
  54 idScalar maxAbs(const vecx &v)
  55 {
  56         idScalar result = 0.0;
  57         for (int i = 0; i < v.size(); i++)
  58         {
  59                 const idScalar tmp = BT_ID_FABS(v(i));
  60                 if (tmp > result)
  61                 {
  62                         result = tmp;
  63                 }
  64         }
  65         return result;
  66 }
  67
  68 idScalar maxAbs(const vec3 &v)
  69 {
  70         idScalar result = 0.0;
  71         for (int i = 0; i < 3; i++)
  72         {
  73                 const idScalar tmp = BT_ID_FABS(v(i));
  74                 if (tmp > result)
  75                 {
  76                         result = tmp;
  77                 }
  78         }
  79         return result;
  80 }
  81
  82 #if (defined BT_ID_HAVE_MAT3X)
  83 idScalar maxAbsMat3x(const mat3x &m)
  84 {
  85         // only used for tests -- so just loop here for portability
  86         idScalar result = 0.0;
  87         for (idArrayIdx col = 0; col < m.cols(); col++)
  88         {
  89                 for (idArrayIdx row = 0; row < 3; row++)
  90                 {
  91                         result = BT_ID_MAX(result, std::fabs(m(row, col)));
  92                 }
  93         }
  94         return result;
  95 }
  96
  97 void mul(const mat33 &a, const mat3x &b, mat3x *result)
  98 {
  99         if (b.cols() != result->cols())
 100         {
 101                 bt_id_error_message("size missmatch. b.cols()= %d, result->cols()= %d\n",
 102                                                         static_cast<int>(b.cols()), static_cast<int>(result->cols()));
 103                 abort();
 104         }
 105
 106         for (idArrayIdx col = 0; col < b.cols(); col++)
 107         {
 108                 const idScalar x = a(0, 0) * b(0, col) + a(0, 1) * b(1, col) + a(0, 2) * b(2, col);
 109                 const idScalar y = a(1, 0) * b(0, col) + a(1, 1) * b(1, col) + a(1, 2) * b(2, col);
 110                 const idScalar z = a(2, 0) * b(0, col) + a(2, 1) * b(1, col) + a(2, 2) * b(2, col);
 111                 setMat3xElem(0, col, x, result);
 112                 setMat3xElem(1, col, y, result);
 113                 setMat3xElem(2, col, z, result);
 114         }
 115 }
 116 void add(const mat3x &a, const mat3x &b, mat3x *result)
 117 {
 118         if (a.cols() != b.cols())
 119         {
 120                 bt_id_error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
 121                                                         static_cast<int>(a.cols()), static_cast<int>(b.cols()));
 122                 abort();
 123         }
 124         for (idArrayIdx col = 0; col < b.cols(); col++)
 125         {
 126                 for (idArrayIdx row = 0; row < 3; row++)
 127                 {
 128                         setMat3xElem(row, col, a(row, col) + b(row, col), result);
 129                 }
 130         }
 131 }
 132 void sub(const mat3x &a, const mat3x &b, mat3x *result)
 133 {
 134         if (a.cols() != b.cols())
 135         {
 136                 bt_id_error_message("size missmatch. a.cols()= %d, b.cols()= %d\n",
 137                                                         static_cast<int>(a.cols()), static_cast<int>(b.cols()));
 138                 abort();
 139         }
 140         for (idArrayIdx col = 0; col < b.cols(); col++)
 141         {
 142                 for (idArrayIdx row = 0; row < 3; row++)
 143                 {
 144                         setMat3xElem(row, col, a(row, col) - b(row, col), result);
 145                 }
 146         }
 147 }
 148 #endif
 149
 150 mat33 transformX(const idScalar &alpha)
 151 {
 152         mat33 T;
 153         const idScalar cos_alpha = BT_ID_COS(alpha);
 154         const idScalar sin_alpha = BT_ID_SIN(alpha);
 155         // [1  0 0]
 156         // [0  c s]
 157         // [0 -s c]
 158         T(0, 0) = 1.0;
 159         T(0, 1) = 0.0;
 160         T(0, 2) = 0.0;
 161
 162         T(1, 0) = 0.0;
 163         T(1, 1) = cos_alpha;
 164         T(1, 2) = sin_alpha;
 165
 166         T(2, 0) = 0.0;
 167         T(2, 1) = -sin_alpha;
 168         T(2, 2) = cos_alpha;
 169
 170         return T;
 171 }
 172
 173 mat33 transformY(const idScalar &beta)
 174 {
 175         mat33 T;
 176         const idScalar cos_beta = BT_ID_COS(beta);
 177         const idScalar sin_beta = BT_ID_SIN(beta);
 178         // [c 0 -s]
 179         // [0 1  0]
 180         // [s 0  c]
 181         T(0, 0) = cos_beta;
 182         T(0, 1) = 0.0;
 183         T(0, 2) = -sin_beta;
 184
 185         T(1, 0) = 0.0;
 186         T(1, 1) = 1.0;
 187         T(1, 2) = 0.0;
 188
 189         T(2, 0) = sin_beta;
 190         T(2, 1) = 0.0;
 191         T(2, 2) = cos_beta;
 192
 193         return T;
 194 }
 195
 196 mat33 transformZ(const idScalar &gamma)
 197 {
 198         mat33 T;
 199         const idScalar cos_gamma = BT_ID_COS(gamma);
 200         const idScalar sin_gamma = BT_ID_SIN(gamma);
 201         // [ c s 0]
 202         // [-s c 0]
 203         // [ 0 0 1]
 204         T(0, 0) = cos_gamma;
 205         T(0, 1) = sin_gamma;
 206         T(0, 2) = 0.0;
 207
 208         T(1, 0) = -sin_gamma;
 209         T(1, 1) = cos_gamma;
 210         T(1, 2) = 0.0;
 211
 212         T(2, 0) = 0.0;
 213         T(2, 1) = 0.0;
 214         T(2, 2) = 1.0;
 215
 216         return T;
 217 }
 218
 219 mat33 tildeOperator(const vec3 &v)
 220 {
 221         mat33 m;
 222         m(0, 0) = 0.0;
 223         m(0, 1) = -v(2);
 224         m(0, 2) = v(1);
 225         m(1, 0) = v(2);
 226         m(1, 1) = 0.0;
 227         m(1, 2) = -v(0);
 228         m(2, 0) = -v(1);
 229         m(2, 1) = v(0);
 230         m(2, 2) = 0.0;
 231         return m;
 232 }
 233
 234 void getVecMatFromDH(idScalar theta, idScalar d, idScalar a, idScalar alpha, vec3 *r, mat33 *T)
 235 {
 236         const idScalar sa = BT_ID_SIN(alpha);
 237         const idScalar ca = BT_ID_COS(alpha);
 238         const idScalar st = BT_ID_SIN(theta);
 239         const idScalar ct = BT_ID_COS(theta);
 240
 241         (*r)(0) = a;
 242         (*r)(1) = -sa * d;
 243         (*r)(2) = ca * d;
 244
 245         (*T)(0, 0) = ct;
 246         (*T)(0, 1) = -st;
 247         (*T)(0, 2) = 0.0;
 248
 249         (*T)(1, 0) = st * ca;
 250         (*T)(1, 1) = ct * ca;
 251         (*T)(1, 2) = -sa;
 252
 253         (*T)(2, 0) = st * sa;
 254         (*T)(2, 1) = ct * sa;
 255         (*T)(2, 2) = ca;
 256 }
 257
 258 void bodyTParentFromAxisAngle(const vec3 &axis, const idScalar &angle, mat33 *T)
 259 {
 260         const idScalar c = BT_ID_COS(angle);
 261         const idScalar s = -BT_ID_SIN(angle);
 262         const idScalar one_m_c = 1.0 - c;
 263
 264         const idScalar &x = axis(0);
 265         const idScalar &y = axis(1);
 266         const idScalar &z = axis(2);
 267
 268         (*T)(0, 0) = x * x * one_m_c + c;
 269         (*T)(0, 1) = x * y * one_m_c - z * s;
 270         (*T)(0, 2) = x * z * one_m_c + y * s;
 271
 272         (*T)(1, 0) = x * y * one_m_c + z * s;
 273         (*T)(1, 1) = y * y * one_m_c + c;
 274         (*T)(1, 2) = y * z * one_m_c - x * s;
 275
 276         (*T)(2, 0) = x * z * one_m_c - y * s;
 277         (*T)(2, 1) = y * z * one_m_c + x * s;
 278         (*T)(2, 2) = z * z * one_m_c + c;
 279 }
 280
 281 bool isPositiveDefinite(const mat33 &m)
 282 {
 283         // test if all upper left determinants are positive
 284         if (m(0, 0) <= 0)
 285         {  // upper 1x1
 286                 return false;
 287         }
 288         if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) <= 0)
 289         {  // upper 2x2
 290                 return false;
 291         }
 292         if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) -
 293                  m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
 294                  m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0)
 295         {
 296                 return false;
 297         }
 298         return true;
 299 }
 300
 301 bool isPositiveSemiDefinite(const mat33 &m)
 302 {
 303         // test if all upper left determinants are positive
 304         if (m(0, 0) < 0)
 305         {  // upper 1x1
 306                 return false;
 307         }
 308         if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < 0)
 309         {  // upper 2x2
 310                 return false;
 311         }
 312         if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) -
 313                  m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
 314                  m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < 0)
 315         {
 316                 return false;
 317         }
 318         return true;
 319 }
 320
 321 bool isPositiveSemiDefiniteFuzzy(const mat33 &m)
 322 {
 323         // test if all upper left determinants are positive
 324         if (m(0, 0) < -kIsZero)
 325         {  // upper 1x1
 326                 return false;
 327         }
 328         if (m(0, 0) * m(1, 1) - m(0, 1) * m(1, 0) < -kIsZero)
 329         {  // upper 2x2
 330                 return false;
 331         }
 332         if ((m(0, 0) * (m(1, 1) * m(2, 2) - m(1, 2) * m(2, 1)) -
 333                  m(0, 1) * (m(1, 0) * m(2, 2) - m(1, 2) * m(2, 0)) +
 334                  m(0, 2) * (m(1, 0) * m(2, 1) - m(1, 1) * m(2, 0))) < -kIsZero)
 335         {
 336                 return false;
 337         }
 338         return true;
 339 }
 340
 341 idScalar determinant(const mat33 &m)
 342 {
 343         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) -
 344                    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);
 345 }
 346
 347 bool isValidInertiaMatrix(const mat33 &I, const int index, bool has_fixed_joint)
 348 {
 349         // TODO(Thomas) do we really want this?
 350         //                        in cases where the inertia tensor about the center of mass is zero,
 351         //                        the determinant of the inertia tensor about the joint axis is almost
 352         //                        zero and can have a very small negative value.
 353         if (!isPositiveSemiDefiniteFuzzy(I))
 354         {
 355                 bt_id_error_message(
 356                         "invalid inertia matrix for body %d, not positive definite "
 357                         "(fixed joint)\n",
 358                         index);
 359                 bt_id_error_message(
 360                         "matrix is:\n"
 361                         "[%.20e %.20e %.20e;\n"
 362                         "%.20e %.20e %.20e;\n"
 363                         "%.20e %.20e %.20e]\n",
 364                         I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
 365                         I(2, 2));
 366
 367                 return false;
 368         }
 369
 370         // check triangle inequality, must have I(i,i)+I(j,j)>=I(k,k)
 371         if (!has_fixed_joint)
 372         {
 373                 if (I(0, 0) + I(1, 1) < I(2, 2))
 374                 {
 375                         bt_id_error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
 376                         bt_id_error_message(
 377                                 "matrix is:\n"
 378                                 "[%.20e %.20e %.20e;\n"
 379                                 "%.20e %.20e %.20e;\n"
 380                                 "%.20e %.20e %.20e]\n",
 381                                 I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
 382                                 I(2, 2));
 383                         return false;
 384                 }
 385                 if (I(0, 0) + I(1, 1) < I(2, 2))
 386                 {
 387                         bt_id_error_message("invalid inertia tensor for body %d, I(0,0) + I(1,1) < I(2,2)\n", index);
 388                         bt_id_error_message(
 389                                 "matrix is:\n"
 390                                 "[%.20e %.20e %.20e;\n"
 391                                 "%.20e %.20e %.20e;\n"
 392                                 "%.20e %.20e %.20e]\n",
 393                                 I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
 394                                 I(2, 2));
 395                         return false;
 396                 }
 397                 if (I(1, 1) + I(2, 2) < I(0, 0))
 398                 {
 399                         bt_id_error_message("invalid inertia tensor for body %d, I(1,1) + I(2,2) < I(0,0)\n", index);
 400                         bt_id_error_message(
 401                                 "matrix is:\n"
 402                                 "[%.20e %.20e %.20e;\n"
 403                                 "%.20e %.20e %.20e;\n"
 404                                 "%.20e %.20e %.20e]\n",
 405                                 I(0, 0), I(0, 1), I(0, 2), I(1, 0), I(1, 1), I(1, 2), I(2, 0), I(2, 1),
 406                                 I(2, 2));
 407                         return false;
 408                 }
 409         }
 410         // check positive/zero diagonal elements
 411         for (int i = 0; i < 3; i++)
 412         {
 413                 if (I(i, i) < 0)
 414                 {  // accept zero
 415                         bt_id_error_message("invalid inertia tensor, I(%d,%d)= %e <0\n", i, i, I(i, i));
 416                         return false;
 417                 }
 418         }
 419         // check symmetry
 420         if (BT_ID_FABS(I(1, 0) - I(0, 1)) > kIsZero)
 421         {
 422                 bt_id_error_message(
 423                         "invalid inertia tensor for body %d I(1,0)!=I(0,1). I(1,0)-I(0,1)= "
 424                         "%e\n",
 425                         index, I(1, 0) - I(0, 1));
 426                 return false;
 427         }
 428         if (BT_ID_FABS(I(2, 0) - I(0, 2)) > kIsZero)
 429         {
 430                 bt_id_error_message(
 431                         "invalid inertia tensor for body %d I(2,0)!=I(0,2). I(2,0)-I(0,2)= "
 432                         "%e\n",
 433                         index, I(2, 0) - I(0, 2));
 434                 return false;
 435         }
 436         if (BT_ID_FABS(I(1, 2) - I(2, 1)) > kIsZero)
 437         {
 438                 bt_id_error_message("invalid inertia tensor body %d I(1,2)!=I(2,1). I(1,2)-I(2,1)= %e\n", index,
 439                                                         I(1, 2) - I(2, 1));
 440                 return false;
 441         }
 442         return true;
 443 }
 444
 445 bool isValidTransformMatrix(const mat33 &m)
 446 {
 447 #define print_mat(x)                                                                                   \
 448         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), \
 449                                                 x(1, 0), x(1, 1), x(1, 2), x(2, 0), x(2, 1), x(2, 2))
 450
 451         // check for unit length column vectors
 452         for (int i = 0; i < 3; i++)
 453         {
 454                 const idScalar length_minus_1 =
 455                         BT_ID_FABS(m(0, i) * m(0, i) + m(1, i) * m(1, i) + m(2, i) * m(2, i) - 1.0);
 456                 if (length_minus_1 > kAxisLengthEpsilon)
 457                 {
 458                         bt_id_error_message(
 459                                 "Not a valid rotation matrix (column %d not unit length)\n"
 460                                 "column = [%.18e %.18e %.18e]\n"
 461                                 "length-1.0= %.18e\n",
 462                                 i, m(0, i), m(1, i), m(2, i), length_minus_1);
 463                         print_mat(m);
 464                         return false;
 465                 }
 466         }
 467         // check for orthogonal column vectors
 468         if (BT_ID_FABS(m(0, 0) * m(0, 1) + m(1, 0) * m(1, 1) + m(2, 0) * m(2, 1)) > kAxisLengthEpsilon)
 469         {
 470                 bt_id_error_message("Not a valid rotation matrix (columns 0 and 1 not orthogonal)\n");
 471                 print_mat(m);
 472                 return false;
 473         }
 474         if (BT_ID_FABS(m(0, 0) * m(0, 2) + m(1, 0) * m(1, 2) + m(2, 0) * m(2, 2)) > kAxisLengthEpsilon)
 475         {
 476                 bt_id_error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
 477                 print_mat(m);
 478                 return false;
 479         }
 480         if (BT_ID_FABS(m(0, 1) * m(0, 2) + m(1, 1) * m(1, 2) + m(2, 1) * m(2, 2)) > kAxisLengthEpsilon)
 481         {
 482                 bt_id_error_message("Not a valid rotation matrix (columns 0 and 2 not orthogonal)\n");
 483                 print_mat(m);
 484                 return false;
 485         }
 486         // check determinant (rotation not reflection)
 487         if (determinant(m) <= 0)
 488         {
 489                 bt_id_error_message("Not a valid rotation matrix (determinant <=0)\n");
 490                 print_mat(m);
 491                 return false;
 492         }
 493         return true;
 494 }
 495
 496 bool isUnitVector(const vec3 &vector)
 497 {
 498         return BT_ID_FABS(vector(0) * vector(0) + vector(1) * vector(1) + vector(2) * vector(2) - 1.0) <
 499                    kIsZero;
 500 }
 501
 502 vec3 rpyFromMatrix(const mat33 &rot)
 503 {
 504         vec3 rpy;
 505         rpy(2) = BT_ID_ATAN2(-rot(1, 0), rot(0, 0));
 506         rpy(0) = BT_ID_ATAN2(-rot(2, 0), rot(2, 2));
 507         rpy(1) = BT_ID_ATAN2(rot(2, 0), BT_ID_COS(rpy(2)) * rot(0, 0) - BT_ID_SIN(rpy(0)) * rot(1, 0));
 508         return rpy;
 509 }
 510 }  // namespace btInverseDynamics