1 /** 2 3x3 matrix datatype. 3 4 Copyright: 5 Copyright (c) 2007 Juan Linietsky, Ariel Manzur. 6 Copyright (c) 2014 Godot Engine contributors (cf. AUTHORS.md) 7 Copyright (c) 2017 Godot-D contributors 8 Copyright (c) 2022 Godot-DLang contributors 9 10 License: $(LINK2 https://opensource.org/licenses/MIT, MIT License) 11 12 13 */ 14 module godot.basis; 15 16 import godot.api.types; 17 import godot.vector3; 18 import godot.quat; 19 20 import std.math; 21 import std.algorithm.mutation : swap; 22 23 /** 24 3x3 matrix used for 3D rotation and scale. Contains 3 vector fields x, y, and z as its columns, which can be interpreted as the local basis vectors of a transformation. Can also be accessed as array of 3D vectors. These vectors are orthogonal to each other, but are not necessarily normalized. Almost always used as orthogonal basis for a $(D Transform). 25 26 For such use, it is composed of a scaling and a rotation matrix, in that order (M = R.S). 27 */ 28 struct Basis { 29 @nogc nothrow: 30 31 Vector3[3] elements = 32 [ 33 Vector3(1.0, 0.0, 0.0), 34 Vector3(0.0, 1.0, 0.0), 35 Vector3(0.0, 0.0, 1.0), 36 ]; 37 alias rows = elements; 38 39 this(in Vector3 row0, in Vector3 row1, in Vector3 row2) { 40 elements[0] = row0; 41 elements[1] = row1; 42 elements[2] = row2; 43 } 44 45 this(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz) { 46 set(xx, xy, xz, yx, yy, yz, zx, zy, zz); 47 } 48 49 const(Vector3) opIndex(int axis) const { 50 return elements[axis]; 51 } 52 53 ref Vector3 opIndex(int axis) return { 54 return elements[axis]; 55 } 56 57 private pragma(inline, true) 58 real_t cofac(int row1, int col1, int row2, int col2) const { 59 return ( 60 elements[row1][col1] * elements[row2][col2] - 61 elements[row1][col2] * elements[row2][col1]); 62 } 63 64 void invert() { 65 real_t[3] co = [ 66 cofac(1, 1, 2, 2), cofac(1, 2, 2, 0), cofac(1, 0, 2, 1) 67 ]; 68 real_t det = elements[0][0] * co[0] + 69 elements[0][1] * co[1] + 70 elements[0][2] * co[2]; 71 72 /// ERR_FAIL_COND(det != 0); 73 /// TODO: implement errors; D assert/exceptions won't work! 74 75 real_t s = 1.0 / det; 76 77 set(co[0] * s, cofac(0, 2, 2, 1) * s, cofac(0, 1, 1, 2) * s, 78 co[1] * s, cofac(0, 0, 2, 2) * s, cofac(0, 2, 1, 0) * s, 79 co[2] * s, cofac(0, 1, 2, 0) * s, cofac(0, 0, 1, 1) * s); 80 } 81 82 bool isEqualApprox(in Basis a, in Basis b) const { 83 for (int i = 0; i < 3; i++) { 84 for (int j = 0; j < 3; j++) { 85 if ((fabs(a.elements[i][j] - b.elements[i][j]) < CMP_EPSILON) == false) 86 return false; 87 } 88 } 89 90 return true; 91 } 92 93 bool isOrthogonal() const { 94 Basis id; 95 Basis m = (this) * transposed(); 96 97 return isEqualApprox(id, m); 98 } 99 100 bool isRotation() const { 101 return fabs(determinant() - 1) < CMP_EPSILON && isOrthogonal(); 102 } 103 104 void transpose() { 105 swap(elements[0][1], elements[1][0]); 106 swap(elements[0][2], elements[2][0]); 107 swap(elements[1][2], elements[2][1]); 108 } 109 110 Basis inverse() const { 111 Basis b = this; 112 b.invert(); 113 return b; 114 } 115 116 Basis transposed() const { 117 Basis b = this; 118 b.transpose(); 119 return b; 120 } 121 122 real_t determinant() const { 123 return elements[0][0] * (elements[1][1] * elements[2][2] - elements[2][1] * elements[1][2]) - 124 elements[1][0] * ( 125 elements[0][1] * elements[2][2] - elements[2][1] * elements[0][2]) + 126 elements[2][0] * ( 127 elements[0][1] * elements[1][2] - elements[1][1] * elements[0][2]); 128 } 129 130 Vector3 getAxis(int p_axis) const { 131 // get actual basis axis (elements is transposed for performance) 132 return Vector3(elements[0][p_axis], elements[1][p_axis], elements[2][p_axis]); 133 } 134 135 void setAxis(int p_axis, in Vector3 p_value) { 136 // get actual basis axis (elements is transposed for performance) 137 elements[0][p_axis] = p_value.x; 138 elements[1][p_axis] = p_value.y; 139 elements[2][p_axis] = p_value.z; 140 } 141 142 void rotate(in Vector3 p_axis, real_t p_phi) { 143 this = rotated(p_axis, p_phi); 144 } 145 146 Basis rotated(in Vector3 p_axis, real_t p_phi) const { 147 return Basis(p_axis, p_phi) * (this); 148 } 149 150 void scale(in Vector3 p_scale) { 151 elements[0][0] *= p_scale.x; 152 elements[0][1] *= p_scale.x; 153 elements[0][2] *= p_scale.x; 154 elements[1][0] *= p_scale.y; 155 elements[1][1] *= p_scale.y; 156 elements[1][2] *= p_scale.y; 157 elements[2][0] *= p_scale.z; 158 elements[2][1] *= p_scale.z; 159 elements[2][2] *= p_scale.z; 160 } 161 162 Basis scaled(in Vector3 p_scale) const { 163 Basis b = this; 164 b.scale(p_scale); 165 return b; 166 } 167 168 Vector3 getScale() const { 169 // We are assuming M = R.S, and performing a polar decomposition to extract R and S. 170 // FIXME: We eventually need a proper polar decomposition. 171 // As a cheap workaround until then, to ensure that R is a proper rotation matrix with determinant +1 172 // (such that it can be represented by a Quaternion or Euler angles), we absorb the sign flip into the scaling matrix. 173 // As such, it works in conjuction with get_rotation(). 174 real_t det_sign = determinant() > 0 ? 1 : -1; 175 return det_sign * Vector3( 176 Vector3(elements[0][0], elements[1][0], elements[2][0]).length, 177 Vector3(elements[0][1], elements[1][1], elements[2][1]).length, 178 Vector3(elements[0][2], elements[1][2], elements[2][2]).length 179 ); 180 } 181 182 Vector3 getEuler() const { 183 // Euler angles in XYZ convention. 184 // See https://en.wikipedia.org/wiki/Euler_angles#Rotation_matrix 185 // 186 // rot = cy*cz -cy*sz sy 187 // cz*sx*sy+cx*sz cx*cz-sx*sy*sz -cy*sx 188 // -cx*cz*sy+sx*sz cz*sx+cx*sy*sz cx*cy 189 190 Vector3 euler; 191 192 if (isRotation() == false) 193 return euler; 194 195 euler.y = asin(elements[0][2]); 196 if (euler.y < PI * 0.5) { 197 if (euler.y > -PI * 0.5) { 198 euler.x = atan2(-elements[1][2], elements[2][2]); 199 euler.z = atan2(-elements[0][1], elements[0][0]); 200 } else { 201 real_t r = atan2(elements[1][0], elements[1][1]); 202 euler.z = 0.0; 203 euler.x = euler.z - r; 204 } 205 } else { 206 real_t r = atan2(elements[0][1], elements[1][1]); 207 euler.z = 0; 208 euler.x = r - euler.z; 209 } 210 return euler; 211 } 212 213 void setEuler(in Vector3 p_euler) { 214 real_t c, s; 215 216 c = cos(p_euler.x); 217 s = sin(p_euler.x); 218 Basis xmat = Basis(1.0, 0.0, 0.0, 0.0, c, -s, 0.0, s, c); 219 220 c = cos(p_euler.y); 221 s = sin(p_euler.y); 222 Basis ymat = Basis(c, 0.0, s, 0.0, 1.0, 0.0, -s, 0.0, c); 223 224 c = cos(p_euler.z); 225 s = sin(p_euler.z); 226 Basis zmat = Basis(c, -s, 0.0, s, c, 0.0, 0.0, 0.0, 1.0); 227 228 //optimizer will optimize away all this anyway 229 this = xmat * (ymat * zmat); 230 } 231 232 // transposed dot products 233 real_t tdotx(in Vector3 v) const { 234 return elements[0][0] * v[0] + elements[1][0] * v[1] + elements[2][0] * v[2]; 235 } 236 237 real_t tdoty(in Vector3 v) const { 238 return elements[0][1] * v[0] + elements[1][1] * v[1] + elements[2][1] * v[2]; 239 } 240 241 real_t tdotz(in Vector3 v) const { 242 return elements[0][2] * v[0] + elements[1][2] * v[1] + elements[2][2] * v[2]; 243 } 244 245 int opCmp(in Basis other) const { 246 for (int i = 0; i < 3; i++) { 247 for (int j = 0; j < 3; j++) { 248 if (elements[i][j] != other.elements[i][j]) 249 return (elements[i][j] > other.elements[i][j]) ? 1 : -1; 250 } 251 } 252 return 0; 253 } 254 255 Vector3 xform(in Vector3 p_vector) const { 256 return Vector3( 257 elements[0].dot(p_vector), 258 elements[1].dot(p_vector), 259 elements[2].dot(p_vector) 260 ); 261 } 262 263 Vector3 xformInv(in Vector3 p_vector) const { 264 return Vector3( 265 (elements[0][0] * p_vector.x) + (elements[1][0] * p_vector.y) + ( 266 elements[2][0] * p_vector.z), 267 (elements[0][1] * p_vector.x) + (elements[1][1] * p_vector.y) + ( 268 elements[2][1] * p_vector.z), 269 (elements[0][2] * p_vector.x) + (elements[1][2] * p_vector.y) + ( 270 elements[2][2] * p_vector.z) 271 ); 272 } 273 274 void opOpAssign(string op : "*")(in Basis p_matrix) { 275 set( 276 p_matrix.tdotx(elements[0]), p_matrix.tdoty(elements[0]), p_matrix.tdotz(elements[0]), 277 p_matrix.tdotx(elements[1]), p_matrix.tdoty(elements[1]), p_matrix.tdotz(elements[1]), 278 p_matrix.tdotx(elements[2]), p_matrix.tdoty(elements[2]), p_matrix.tdotz(elements[2])); 279 280 } 281 282 Basis opBinary(string op : "*")(in Basis p_matrix) const { 283 return Basis( 284 p_matrix.tdotx(elements[0]), p_matrix.tdoty(elements[0]), p_matrix.tdotz(elements[0]), 285 p_matrix.tdotx(elements[1]), p_matrix.tdoty(elements[1]), p_matrix.tdotz(elements[1]), 286 p_matrix.tdotx(elements[2]), p_matrix.tdoty(elements[2]), p_matrix.tdotz(elements[2])); 287 288 } 289 290 void opOpAssign(string op : "+")(in Basis p_matrix) { 291 elements[0] += p_matrix.elements[0]; 292 elements[1] += p_matrix.elements[1]; 293 elements[2] += p_matrix.elements[2]; 294 } 295 296 Basis opBinary(string op : "+")(in Basis p_matrix) const { 297 Basis ret = this; 298 ret += p_matrix; 299 return ret; 300 } 301 302 void opOpAssign(string op : "-")(in Basis p_matrix) { 303 elements[0] -= p_matrix.elements[0]; 304 elements[1] -= p_matrix.elements[1]; 305 elements[2] -= p_matrix.elements[2]; 306 } 307 308 Basis opBinary(string op : "-")(in Basis p_matrix) const { 309 Basis ret = this; 310 ret -= p_matrix; 311 return ret; 312 } 313 314 void opOpAssign(string op : "*")(real_t p_val) { 315 316 elements[0] *= p_val; 317 elements[1] *= p_val; 318 elements[2] *= p_val; 319 } 320 321 Basis opBinary(string op : "*")(real_t p_val) const { 322 323 Basis ret = this; 324 ret *= p_val; 325 return ret; 326 } 327 328 /++String opCast(T : String)() const 329 { 330 String s; 331 // @Todo 332 return s; 333 }+/ 334 335 /* create / set */ 336 337 void set(real_t xx, real_t xy, real_t xz, real_t yx, real_t yy, real_t yz, real_t zx, real_t zy, real_t zz) { 338 elements[0][0] = xx; 339 elements[0][1] = xy; 340 elements[0][2] = xz; 341 elements[1][0] = yx; 342 elements[1][1] = yy; 343 elements[1][2] = yz; 344 elements[2][0] = zx; 345 elements[2][1] = zy; 346 elements[2][2] = zz; 347 } 348 349 Vector3 getColumn(int i) const { 350 return Vector3(elements[0][i], elements[1][i], elements[2][i]); 351 } 352 353 Vector3 getRow(int i) const { 354 return Vector3(elements[i][0], elements[i][1], elements[i][2]); 355 } 356 357 Vector3 getMainDiagonal() const { 358 return Vector3(elements[0][0], elements[1][1], elements[2][2]); 359 } 360 361 void setRow(int i, in Vector3 p_row) { 362 elements[i][0] = p_row.x; 363 elements[i][1] = p_row.y; 364 elements[i][2] = p_row.z; 365 } 366 367 Basis transposeXform(in Basis m) const { 368 return Basis( 369 elements[0].x * m[0].x + elements[1].x * m[1].x + elements[2].x * m[2].x, 370 elements[0].x * m[0].y + elements[1].x * m[1].y + elements[2].x * m[2].y, 371 elements[0].x * m[0].z + elements[1].x * m[1].z + elements[2].x * m[2].z, 372 elements[0].y * m[0].x + elements[1].y * m[1].x + elements[2].y * m[2].x, 373 elements[0].y * m[0].y + elements[1].y * m[1].y + elements[2].y * m[2].y, 374 elements[0].y * m[0].z + elements[1].y * m[1].z + elements[2].y * m[2].z, 375 elements[0].z * m[0].x + elements[1].z * m[1].x + elements[2].z * m[2].x, 376 elements[0].z * m[0].y + elements[1].z * m[1].y + elements[2].z * m[2].y, 377 elements[0].z * m[0].z + elements[1].z * m[1].z + elements[2].z * m[2].z); 378 } 379 380 void orthonormalize() { 381 ///ERR_FAIL_COND(determinant() != 0); 382 383 // Gram-Schmidt Process 384 385 Vector3 x = getAxis(0); 386 Vector3 y = getAxis(1); 387 Vector3 z = getAxis(2); 388 389 x.normalize(); 390 y = (y - x * (x.dot(y))); 391 y.normalize(); 392 z = (z - x * (x.dot(z)) - y * (y.dot(z))); 393 z.normalize(); 394 395 setAxis(0, x); 396 setAxis(1, y); 397 setAxis(2, z); 398 } 399 400 Basis orthonormalized() const { 401 Basis b = this; 402 b.orthonormalize(); 403 return b; 404 } 405 406 bool isSymmetric() const { 407 if (fabs(elements[0][1] - elements[1][0]) > CMP_EPSILON) 408 return false; 409 if (fabs(elements[0][2] - elements[2][0]) > CMP_EPSILON) 410 return false; 411 if (fabs(elements[1][2] - elements[2][1]) > CMP_EPSILON) 412 return false; 413 414 return true; 415 } 416 417 Basis diagonalize() { 418 // much copy paste, WOW 419 if (!isSymmetric()) 420 return Basis(); 421 422 const int ite_max = 1024; 423 424 real_t off_matrix_norm_2 = elements[0][1] * elements[0][1] + elements[0][2] * elements[0][2] + elements[1][2] * elements[1][2]; 425 426 int ite = 0; 427 Basis acc_rot; 428 while (off_matrix_norm_2 > CMP_EPSILON2 && ite++ < ite_max) { 429 real_t el01_2 = elements[0][1] * elements[0][1]; 430 real_t el02_2 = elements[0][2] * elements[0][2]; 431 real_t el12_2 = elements[1][2] * elements[1][2]; 432 // Find the pivot element 433 int i, j; 434 if (el01_2 > el02_2) { 435 if (el12_2 > el01_2) { 436 i = 1; 437 j = 2; 438 } else { 439 i = 0; 440 j = 1; 441 } 442 } else { 443 if (el12_2 > el02_2) { 444 i = 1; 445 j = 2; 446 } else { 447 i = 0; 448 j = 2; 449 } 450 } 451 452 // Compute the rotation angle 453 real_t angle; 454 if (fabs(elements[j][j] - elements[i][i]) < CMP_EPSILON) { 455 angle = PI / 4; 456 } else { 457 angle = 0.5 * atan(2 * elements[i][j] / (elements[j][j] - elements[i][i])); 458 } 459 460 // Compute the rotation matrix 461 Basis rot; 462 rot.elements[i][i] = rot.elements[j][j] = cos(angle); 463 rot.elements[i][j] = -(rot.elements[j][i] = sin(angle)); 464 465 // Update the off matrix norm 466 off_matrix_norm_2 -= elements[i][j] * elements[i][j]; 467 468 // Apply the rotation 469 this = rot * this * rot.transposed(); 470 acc_rot = rot * acc_rot; 471 } 472 473 return acc_rot; 474 } 475 476 static immutable Basis[24] _ortho_bases = 477 [ 478 Basis(1, 0, 0, 0, 1, 0, 0, 0, 1), 479 Basis(0, -1, 0, 1, 0, 0, 0, 0, 1), 480 Basis(-1, 0, 0, 0, -1, 0, 0, 0, 1), 481 Basis(0, 1, 0, -1, 0, 0, 0, 0, 1), 482 Basis(1, 0, 0, 0, 0, -1, 0, 1, 0), 483 Basis(0, 0, 1, 1, 0, 0, 0, 1, 0), 484 Basis(-1, 0, 0, 0, 0, 1, 0, 1, 0), 485 Basis(0, 0, -1, -1, 0, 0, 0, 1, 0), 486 Basis(1, 0, 0, 0, -1, 0, 0, 0, -1), 487 Basis(0, 1, 0, 1, 0, 0, 0, 0, -1), 488 Basis(-1, 0, 0, 0, 1, 0, 0, 0, -1), 489 Basis(0, -1, 0, -1, 0, 0, 0, 0, -1), 490 Basis(1, 0, 0, 0, 0, 1, 0, -1, 0), 491 Basis(0, 0, -1, 1, 0, 0, 0, -1, 0), 492 Basis(-1, 0, 0, 0, 0, -1, 0, -1, 0), 493 Basis(0, 0, 1, -1, 0, 0, 0, -1, 0), 494 Basis(0, 0, 1, 0, 1, 0, -1, 0, 0), 495 Basis(0, -1, 0, 0, 0, 1, -1, 0, 0), 496 Basis(0, 0, -1, 0, -1, 0, -1, 0, 0), 497 Basis(0, 1, 0, 0, 0, -1, -1, 0, 0), 498 Basis(0, 0, 1, 0, -1, 0, 1, 0, 0), 499 Basis(0, 1, 0, 0, 0, 1, 1, 0, 0), 500 Basis(0, 0, -1, 0, 1, 0, 1, 0, 0), 501 Basis(0, -1, 0, 0, 0, -1, 1, 0, 0) 502 ]; 503 504 int getOrthogonalIndex() const { 505 //could be sped up if i come up with a way 506 Basis orth = this; 507 for (int i = 0; i < 3; i++) { 508 for (int j = 0; j < 3; j++) { 509 510 real_t v = orth[i][j]; 511 if (v > 0.5) 512 v = 1.0; 513 else if (v < -0.5) 514 v = -1.0; 515 else 516 v = 0; 517 518 orth[i][j] = v; 519 } 520 } 521 522 for (int i = 0; i < 24; i++) { 523 524 if (_ortho_bases[i] == orth) 525 return i; 526 527 } 528 529 return 0; 530 } 531 532 void setOrthogonalIndex(int p_index) { 533 //there only exist 24 orthogonal bases in r3 534 ///ERR_FAIL_COND(p_index >= 24); 535 this = _ortho_bases[p_index]; 536 } 537 538 this(in Vector3 p_euler) { 539 setEuler(p_euler); 540 } 541 542 this(in Quaternion p_quat) { 543 544 real_t d = p_quat.lengthSquared(); 545 real_t s = 2.0 / d; 546 real_t xs = p_quat.x * s, ys = p_quat.y * s, zs = p_quat.z * s; 547 real_t wx = p_quat.w * xs, wy = p_quat.w * ys, wz = p_quat.w * zs; 548 real_t xx = p_quat.x * xs, xy = p_quat.x * ys, xz = p_quat.x * zs; 549 real_t yy = p_quat.y * ys, yz = p_quat.y * zs, zz = p_quat.z * zs; 550 set(1.0 - (yy + zz), xy - wz, xz + wy, 551 xy + wz, 1.0 - (xx + zz), yz - wx, 552 xz - wy, yz + wx, 1.0 - (xx + yy)); 553 554 } 555 556 this(in Vector3 p_axis, real_t p_phi) { 557 // Rotation matrix from axis and angle, see https://en.wikipedia.org/wiki/Rotation_matrix#Rotation_matrix_from_axis_and_angle 558 559 Vector3 axis_sq = Vector3(p_axis.x * p_axis.x, p_axis.y * p_axis.y, p_axis.z * p_axis.z); 560 561 real_t cosine = cos(p_phi); 562 real_t sine = sin(p_phi); 563 564 elements[0][0] = axis_sq.x + cosine * (1.0 - axis_sq.x); 565 elements[0][1] = p_axis.x * p_axis.y * (1.0 - cosine) - p_axis.z * sine; 566 elements[0][2] = p_axis.z * p_axis.x * (1.0 - cosine) + p_axis.y * sine; 567 568 elements[1][0] = p_axis.x * p_axis.y * (1.0 - cosine) + p_axis.z * sine; 569 elements[1][1] = axis_sq.y + cosine * (1.0 - axis_sq.y); 570 elements[1][2] = p_axis.y * p_axis.z * (1.0 - cosine) - p_axis.x * sine; 571 572 elements[2][0] = p_axis.z * p_axis.x * (1.0 - cosine) - p_axis.y * sine; 573 elements[2][1] = p_axis.y * p_axis.z * (1.0 - cosine) + p_axis.x * sine; 574 elements[2][2] = axis_sq.z + cosine * (1.0 - axis_sq.z); 575 576 } 577 578 Quaternion quat() const { 579 ///ERR_FAIL_COND_V(is_rotation() == false, Quaternion()); 580 581 real_t trace = elements[0][0] + elements[1][1] + elements[2][2]; 582 real_t[4] temp; 583 584 if (trace > 0.0) { 585 real_t s = sqrt(trace + 1.0); 586 temp[3] = (s * 0.5); 587 s = 0.5 / s; 588 589 temp[0] = ((elements[2][1] - elements[1][2]) * s); 590 temp[1] = ((elements[0][2] - elements[2][0]) * s); 591 temp[2] = ((elements[1][0] - elements[0][1]) * s); 592 } else { 593 int i = elements[0][0] < elements[1][1] ? 594 (elements[1][1] < elements[2][2] ? 2 : 1) : (elements[0][0] < elements[2][2] ? 2 : 0); 595 int j = (i + 1) % 3; 596 int k = (i + 2) % 3; 597 598 real_t s = sqrt(elements[i][i] - elements[j][j] - elements[k][k] + 1.0); 599 temp[i] = s * 0.5; 600 s = 0.5 / s; 601 602 temp[3] = (elements[k][j] - elements[j][k]) * s; 603 temp[j] = (elements[j][i] + elements[i][j]) * s; 604 temp[k] = (elements[k][i] + elements[i][k]) * s; 605 } 606 return Quaternion(temp[0], temp[1], temp[2], temp[3]); 607 } 608 }