| 1 |
// Copyright 2020, Hesham Wahba. |
| 2 |
// Copyright 2020, Nova King. |
| 3 |
// SPDX-License-Identifier: BSD-3-Clause |
| 4 |
|
| 5 |
#pragma once |
| 6 |
|
| 7 |
#include "math/m_mathinclude.h" |
| 8 |
using namespace std; |
| 9 |
|
| 10 |
// min set of functions needed |
| 11 |
const float kEpsilon = 0.00001f; |
| 12 |
|
| 13 |
|
| 14 |
// Min Port of a Vector3 class for the deformation rendering system. |
| 15 |
class Vector3 |
| 16 |
{ |
| 17 |
public: |
| 18 |
inline Vector3(const Vector3 &in) |
| 19 |
{ |
| 20 |
x = in.x; |
| 21 |
y = in.y; |
| 22 |
z = in.z; |
| 23 |
}; |
| 24 |
|
| 25 |
inline Vector3() |
| 26 |
{ |
| 27 |
x = 0.f; |
| 28 |
y = 0.f; |
| 29 |
z = 0.f; |
| 30 |
}; |
| 31 |
|
| 32 |
inline Vector3(float _x, float _y, float _z) |
| 33 |
{ |
| 34 |
x = _x; |
| 35 |
y = _y; |
| 36 |
z = _z; |
| 37 |
}; |
| 38 |
|
| 39 |
inline Vector3(float _x, float _y) |
| 40 |
{ |
| 41 |
x = _x; |
| 42 |
y = _y; |
| 43 |
z = 0.f; |
| 44 |
}; |
| 45 |
|
| 46 |
constexpr Vector3 & |
| 47 |
operator=(const Vector3 &lhr) |
| 48 |
{ |
| 49 |
this->x = lhr.x; |
| 50 |
this->y = lhr.y; |
| 51 |
this->z = lhr.z; |
| 52 |
return *this; |
| 53 |
} |
| 54 |
|
| 55 |
inline static Vector3 |
| 56 |
Up() |
| 57 |
{ |
| 58 |
return Vector3(0.f, 1.f, 0.f); |
| 59 |
}; |
| 60 |
inline static Vector3 |
| 61 |
Down() |
| 62 |
{ |
| 63 |
return Vector3(0.f, -1.f, 0.f); |
| 64 |
}; |
| 65 |
inline static Vector3 |
| 66 |
Left() |
| 67 |
{ |
| 68 |
return Vector3(-1.f, 0.f, 0.f); |
| 69 |
}; |
| 70 |
inline static Vector3 |
| 71 |
Right() |
| 72 |
{ |
| 73 |
return Vector3(1.f, 0.f, 0.f); |
| 74 |
}; |
| 75 |
inline static Vector3 |
| 76 |
Forward() |
| 77 |
{ |
| 78 |
return Vector3(0.f, 0.f, 1.f); |
| 79 |
}; |
| 80 |
inline static Vector3 |
| 81 |
Backward() |
| 82 |
{ |
| 83 |
return Vector3(0.f, 0.f, -1.f); |
| 84 |
}; |
| 85 |
|
| 86 |
inline Vector3 |
| 87 |
operator-(const Vector3 &rhs) const |
| 88 |
{ |
| 89 |
Vector3 ret; |
| 90 |
ret.x = (x - rhs.x); |
| 91 |
ret.y = (y - rhs.y); |
| 92 |
ret.z = (z - rhs.z); |
| 93 |
return ret; |
| 94 |
} |
| 95 |
|
| 96 |
inline Vector3 |
| 97 |
operator-() const |
| 98 |
{ |
| 99 |
return Vector3(-x, -y, -z); |
| 100 |
} |
| 101 |
|
| 102 |
inline Vector3 |
| 103 |
operator+(const Vector3 &rhs) const |
| 104 |
{ |
| 105 |
Vector3 ret; |
| 106 |
ret.x = (x + rhs.x); |
| 107 |
ret.y = (y + rhs.y); |
| 108 |
ret.z = (z + rhs.z); |
| 109 |
return ret; |
| 110 |
} |
| 111 |
|
| 112 |
inline Vector3 & |
| 113 |
operator+=(const Vector3 &v) |
| 114 |
{ |
| 115 |
this->x += v.x; |
| 116 |
this->y += v.y; |
| 117 |
this->z += v.z; |
| 118 |
return *this; |
| 119 |
} |
| 120 |
|
| 121 |
inline Vector3 |
| 122 |
operator/(const float &d) const |
| 123 |
{ |
| 124 |
Vector3 ret; |
| 125 |
ret.x = (x / d); |
| 126 |
ret.y = (y / d); |
| 127 |
ret.z = (z / d); |
| 128 |
return ret; |
| 129 |
} |
| 130 |
|
| 131 |
inline Vector3 // Until clang-format-11 is on the CI. |
| 132 |
operator*(const float &d) const |
| 133 |
{ |
| 134 |
Vector3 ret; |
| 135 |
ret.x = (x * d); |
| 136 |
ret.y = (y * d); |
| 137 |
ret.z = (z * d); |
| 138 |
return ret; |
| 139 |
} |
| 140 |
|
| 141 |
inline Vector3 |
| 142 |
Inverse() const |
| 143 |
{ |
| 144 |
return {-x, -y, -z}; |
| 145 |
} |
| 146 |
|
| 147 |
inline float static Dot(Vector3 const &lhs, Vector3 const &rhs) |
| 148 |
{ |
| 149 |
float result = (lhs.x * rhs.x) + (lhs.y * rhs.y) + (lhs.z * rhs.z); |
| 150 |
return result; |
| 151 |
} |
| 152 |
|
| 153 |
inline float |
| 154 |
Dot(Vector3 const &rhs) const |
| 155 |
{ |
| 156 |
float result = (x * rhs.x) + (y * rhs.y) + (z * rhs.z); |
| 157 |
return result; |
| 158 |
} |
| 159 |
|
| 160 |
inline static float |
| 161 |
Angle(const Vector3 &v0, const Vector3 &v1) |
| 162 |
{ |
| 163 |
Vector3 dir0 = v0.Normalized(); |
| 164 |
Vector3 dir1 = v1.Normalized(); |
| 165 |
|
| 166 |
float dot = dir0.Dot(dir1); |
| 167 |
dot = (dot < -1.f ? -1.f : (dot > 1.f ? 1.f : dot)); |
| 168 |
|
| 169 |
float angle = acos(dot); |
| 170 |
|
| 171 |
return angle; |
| 172 |
} |
| 173 |
|
| 174 |
inline float |
| 175 |
sqrMagnitude() const |
| 176 |
{ |
| 177 |
return x * x + y * y + z * z; |
| 178 |
} |
| 179 |
|
| 180 |
inline float |
| 181 |
Magnitude() const |
| 182 |
{ |
| 183 |
return sqrt(x * x + y * y + z * z); |
| 184 |
} |
| 185 |
|
| 186 |
inline static Vector3 |
| 187 |
Zero() |
| 188 |
{ |
| 189 |
return {0.f, 0.f, 0.f}; |
| 190 |
} |
| 191 |
|
| 192 |
inline static Vector3 |
| 193 |
One() |
| 194 |
{ |
| 195 |
return {1.f, 1.f, 1.f}; |
| 196 |
} |
| 197 |
|
| 198 |
inline static Vector3 |
| 199 |
Reflect(const Vector3 &inDirection, const Vector3 &inNormal) |
| 200 |
{ |
| 201 |
return inNormal * -2.F * Dot(inNormal, inDirection) + inDirection; |
| 202 |
} |
| 203 |
|
| 204 |
inline void |
| 205 |
Normalize() |
| 206 |
{ |
| 207 |
float mag = Magnitude(); |
| 208 |
if (mag > kEpsilon) { |
| 209 |
x = x / mag; |
| 210 |
y = y / mag; |
| 211 |
z = z / mag; |
| 212 |
} else { |
| 213 |
x = 0.f; |
| 214 |
y = 0.f; |
| 215 |
z = 0.f; |
| 216 |
} |
| 217 |
} |
| 218 |
|
| 219 |
inline Vector3 |
| 220 |
Normalized() const |
| 221 |
{ |
| 222 |
Vector3 ret; |
| 223 |
float mag = Magnitude(); |
| 224 |
if (mag > kEpsilon) { |
| 225 |
ret.x = x / mag; |
| 226 |
ret.y = y / mag; |
| 227 |
ret.z = z / mag; |
| 228 |
} else { |
| 229 |
ret.x = 0.f; |
| 230 |
ret.y = 0.f; |
| 231 |
ret.z = 0.f; |
| 232 |
} |
| 233 |
return ret; |
| 234 |
} |
| 235 |
|
| 236 |
|
| 237 |
inline void |
| 238 |
rotate(const Vector3 &axis, float radians) |
| 239 |
{ |
| 240 |
float cos_theta = cosf(radians); |
| 241 |
float sin_theta = sinf(radians); |
| 242 |
|
| 243 |
x = (x * cos_theta) + (Vector3::Cross(axis) * sin_theta).x + |
| 244 |
(axis.x * Vector3::Dot(axis, *this)) * (1 - cos_theta); |
| 245 |
|
| 246 |
y = (y * cos_theta) + (Vector3::Cross(axis) * sin_theta).y + |
| 247 |
(axis.y * Vector3::Dot(axis, *this)) * (1 - cos_theta); |
| 248 |
|
| 249 |
z = (z * cos_theta) + (Vector3::Cross(axis) * sin_theta).z + |
| 250 |
(axis.z * Vector3::Dot(axis, *this)) * (1 - cos_theta); |
| 251 |
} |
| 252 |
|
| 253 |
inline Vector3 |
| 254 |
Cross(const Vector3 &in) const |
| 255 |
{ |
| 256 |
Vector3 ret; |
| 257 |
ret.x = y * in.z - z * in.y; |
| 258 |
ret.y = z * in.x - x * in.z; |
| 259 |
ret.z = x * in.y - y * in.x; |
| 260 |
return ret; |
| 261 |
} |
| 262 |
|
| 263 |
float x; |
| 264 |
float y; |
| 265 |
float z; |
| 266 |
}; |
| 267 |
|
| 268 |
// Min Port of a Vector2 class for the deformation rendering system. |
| 269 |
class Vector2 |
| 270 |
{ |
| 271 |
public: |
| 272 |
inline Vector2() |
| 273 |
{ |
| 274 |
x = 0.f; |
| 275 |
y = 0.f; |
| 276 |
}; |
| 277 |
|
| 278 |
inline Vector2(const Vector2 &in) |
| 279 |
{ |
| 280 |
x = in.x; |
| 281 |
y = in.y; |
| 282 |
} |
| 283 |
|
| 284 |
inline Vector2(float _x, float _y) |
| 285 |
{ |
| 286 |
x = _x; |
| 287 |
y = _y; |
| 288 |
}; |
| 289 |
|
| 290 |
constexpr Vector2 & |
| 291 |
operator=(const Vector2 &lhr) |
| 292 |
{ |
| 293 |
this->x = lhr.x; |
| 294 |
this->y = lhr.y; |
| 295 |
return *this; |
| 296 |
} |
| 297 |
|
| 298 |
inline static Vector2 |
| 299 |
zero() |
| 300 |
{ |
| 301 |
Vector2 ret; |
| 302 |
ret.x = 0.f; |
| 303 |
ret.y = 0.f; |
| 304 |
return ret; |
| 305 |
} |
| 306 |
|
| 307 |
inline static Vector2 |
| 308 |
One() |
| 309 |
{ |
| 310 |
Vector2 ret; |
| 311 |
ret.x = 1.f; |
| 312 |
ret.y = 1.f; |
| 313 |
return ret; |
| 314 |
} |
| 315 |
|
| 316 |
inline Vector2 |
| 317 |
operator/(float d) const |
| 318 |
{ |
| 319 |
Vector2 ret; |
| 320 |
ret.x = (x / d); |
| 321 |
ret.y = (y / d); |
| 322 |
return ret; |
| 323 |
} |
| 324 |
|
| 325 |
inline Vector2 // Until clang-format-11 is on the CI. |
| 326 |
operator*(float d) const |
| 327 |
{ |
| 328 |
Vector2 ret; |
| 329 |
ret.x = (x * d); |
| 330 |
ret.y = (y * d); |
| 331 |
return ret; |
| 332 |
} |
| 333 |
inline Vector2 |
| 334 |
operator-(const Vector2 &rhs) const |
| 335 |
{ |
| 336 |
Vector2 ret; |
| 337 |
ret.x = (x - rhs.x); |
| 338 |
ret.y = (y - rhs.y); |
| 339 |
return ret; |
| 340 |
} |
| 341 |
|
| 342 |
inline Vector2 |
| 343 |
operator+(const Vector2 &rhs) const |
| 344 |
{ |
| 345 |
Vector2 ret; |
| 346 |
ret.x = (x + rhs.x); |
| 347 |
ret.y = (y + rhs.y); |
| 348 |
return ret; |
| 349 |
} |
| 350 |
|
| 351 |
float x; |
| 352 |
float y; |
| 353 |
}; |
| 354 |
|
| 355 |
class Matrix4x4 |
| 356 |
{ |
| 357 |
public: |
| 358 |
inline Matrix4x4() |
| 359 |
{ |
| 360 |
m00 = 0.0f; |
| 361 |
m01 = 0.0f; |
| 362 |
m02 = 0.0f; |
| 363 |
m03 = 0.0f; |
| 364 |
m10 = 0.0f; |
| 365 |
m11 = 0.0f; |
| 366 |
m12 = 0.0f; |
| 367 |
m13 = 0.0f; |
| 368 |
m20 = 0.0f; |
| 369 |
m21 = 0.0f; |
| 370 |
m22 = 0.0f; |
| 371 |
m23 = 0.0f; |
| 372 |
m30 = 0.0f; |
| 373 |
m31 = 0.0f; |
| 374 |
m32 = 0.0f; |
| 375 |
m33 = 0.0f; |
| 376 |
}; |
| 377 |
|
| 378 |
inline Matrix4x4(const Matrix4x4 &_in) |
| 379 |
{ |
| 380 |
m00 = _in.m00; |
| 381 |
m01 = _in.m01; |
| 382 |
m02 = _in.m02; |
| 383 |
m03 = _in.m03; |
| 384 |
m10 = _in.m10; |
| 385 |
m11 = _in.m11; |
| 386 |
m12 = _in.m12; |
| 387 |
m13 = _in.m13; |
| 388 |
m20 = _in.m20; |
| 389 |
m21 = _in.m21; |
| 390 |
m22 = _in.m22; |
| 391 |
m23 = _in.m23; |
| 392 |
m30 = _in.m30; |
| 393 |
m31 = _in.m31; |
| 394 |
m32 = _in.m32; |
| 395 |
m33 = _in.m33; |
| 396 |
}; |
| 397 |
|
| 398 |
inline Matrix4x4(float in00, |
| 399 |
float in01, |
| 400 |
float in02, |
| 401 |
float in03, |
| 402 |
float in10, |
| 403 |
float in11, |
| 404 |
float in12, |
| 405 |
float in13, |
| 406 |
float in20, |
| 407 |
float in21, |
| 408 |
float in22, |
| 409 |
float in23, |
| 410 |
float in30, |
| 411 |
float in31, |
| 412 |
float in32, |
| 413 |
float in33) |
| 414 |
{ |
| 415 |
m00 = in00; |
| 416 |
m01 = in01; |
| 417 |
m02 = in02; |
| 418 |
m03 = in03; |
| 419 |
m10 = in10; |
| 420 |
m11 = in11; |
| 421 |
m12 = in12; |
| 422 |
m13 = in13; |
| 423 |
m20 = in20; |
| 424 |
m21 = in21; |
| 425 |
m22 = in22; |
| 426 |
m23 = in23; |
| 427 |
m30 = in30; |
| 428 |
m31 = in31; |
| 429 |
m32 = in32; |
| 430 |
m33 = in33; |
| 431 |
}; |
| 432 |
|
| 433 |
constexpr Matrix4x4 & |
| 434 |
operator=(const Matrix4x4 &lhr) |
| 435 |
{ |
| 436 |
this->m00 = lhr.m00; |
| 437 |
this->m01 = lhr.m01; |
| 438 |
this->m02 = lhr.m02; |
| 439 |
this->m03 = lhr.m03; |
| 440 |
this->m10 = lhr.m10; |
| 441 |
this->m11 = lhr.m11; |
| 442 |
this->m12 = lhr.m12; |
| 443 |
this->m13 = lhr.m13; |
| 444 |
this->m20 = lhr.m20; |
| 445 |
this->m21 = lhr.m21; |
| 446 |
this->m22 = lhr.m22; |
| 447 |
this->m23 = lhr.m23; |
| 448 |
this->m30 = lhr.m30; |
| 449 |
this->m31 = lhr.m31; |
| 450 |
this->m32 = lhr.m32; |
| 451 |
this->m33 = lhr.m33; |
| 452 |
return *this; |
| 453 |
}; |
| 454 |
|
| 455 |
inline static Matrix4x4 |
| 456 |
Identity() |
| 457 |
{ |
| 458 |
Matrix4x4 m; |
| 459 |
m.m00 = 1; |
| 460 |
m.m01 = 0; |
| 461 |
m.m02 = 0; |
| 462 |
m.m03 = 0; |
| 463 |
m.m10 = 0; |
| 464 |
m.m11 = 1; |
| 465 |
m.m12 = 0; |
| 466 |
m.m13 = 0; |
| 467 |
m.m20 = 0; |
| 468 |
m.m21 = 0; |
| 469 |
m.m22 = 1; |
| 470 |
m.m23 = 0; |
| 471 |
m.m30 = 0; |
| 472 |
m.m31 = 0; |
| 473 |
m.m32 = 0; |
| 474 |
m.m33 = 1; |
| 475 |
return m; |
| 476 |
} |
| 477 |
|
| 478 |
// Returns a 3x3 rotation matrix (padded to a Matrix4x4). |
| 479 |
inline static Matrix4x4 |
| 480 |
RotationAlign(Vector3 const &fromDir, Vector3 const &toDir) |
| 481 |
{ |
| 482 |
const Vector3 v = fromDir.Cross(toDir); |
| 483 |
const float c = fromDir.Dot(toDir); |
| 484 |
const float k = 1.0f / (1.0f + c); |
| 485 |
|
| 486 |
return Matrix4x4(v.x * v.x * k + c, v.y * v.x * k - v.z, v.z * v.x * k + v.y, 0.f, v.x * v.y * k + v.z, |
| 487 |
v.y * v.y * k + c, v.z * v.y * k - v.x, 0.f, v.x * v.z * k - v.y, v.y * v.z * k + v.x, |
| 488 |
v.z * v.z * k + c, 0.f, 0.f, 0.f, 0.f, 1.f); |
| 489 |
} |
| 490 |
|
| 491 |
inline Matrix4x4 // Until clang-format-11 is on the CI. |
| 492 |
operator*(const Matrix4x4 &_in) const |
| 493 |
{ |
| 494 |
Matrix4x4 ret; |
| 495 |
ret.m00 = (m00 * _in.m00) + (m01 * _in.m10) + (m02 * _in.m20) + (m03 * _in.m30); |
| 496 |
ret.m01 = (m00 * _in.m01) + (m01 * _in.m11) + (m02 * _in.m21) + (m03 * _in.m31); |
| 497 |
ret.m02 = (m00 * _in.m02) + (m01 * _in.m12) + (m02 * _in.m22) + (m03 * _in.m32); |
| 498 |
ret.m03 = (m00 * _in.m03) + (m01 * _in.m13) + (m02 * _in.m23) + (m03 * _in.m33); |
| 499 |
|
| 500 |
|
| 501 |
ret.m10 = (m10 * _in.m00) + (m11 * _in.m10) + (m12 * _in.m20) + (m13 * _in.m30); |
| 502 |
ret.m11 = (m10 * _in.m01) + (m11 * _in.m11) + (m12 * _in.m21) + (m13 * _in.m31); |
| 503 |
ret.m12 = (m10 * _in.m02) + (m11 * _in.m12) + (m12 * _in.m22) + (m13 * _in.m32); |
| 504 |
ret.m13 = (m10 * _in.m03) + (m11 * _in.m13) + (m12 * _in.m23) + (m13 * _in.m33); |
| 505 |
|
| 506 |
|
| 507 |
ret.m20 = (m20 * _in.m00) + (m21 * _in.m10) + (m22 * _in.m20) + (m23 * _in.m30); |
| 508 |
ret.m21 = (m20 * _in.m01) + (m21 * _in.m11) + (m22 * _in.m21) + (m23 * _in.m31); |
| 509 |
ret.m22 = (m20 * _in.m02) + (m21 * _in.m12) + (m22 * _in.m22) + (m23 * _in.m32); |
| 510 |
ret.m23 = (m20 * _in.m03) + (m21 * _in.m13) + (m22 * _in.m23) + (m23 * _in.m33); |
| 511 |
|
| 512 |
ret.m30 = (m30 * _in.m00) + (m31 * _in.m10) + (m32 * _in.m20) + (m33 * _in.m30); |
| 513 |
ret.m31 = (m30 * _in.m01) + (m31 * _in.m11) + (m32 * _in.m21) + (m33 * _in.m31); |
| 514 |
ret.m32 = (m30 * _in.m02) + (m31 * _in.m12) + (m32 * _in.m22) + (m33 * _in.m32); |
| 515 |
ret.m33 = (m30 * _in.m03) + (m31 * _in.m13) + (m32 * _in.m23) + (m33 * _in.m33); |
| 516 |
|
| 517 |
return ret; |
| 518 |
} |
| 519 |
|
| 520 |
inline Vector3 |
| 521 |
MultiplyPoint(Vector3 const &point) const |
| 522 |
{ |
| 523 |
Vector3 res; |
| 524 |
float w; |
| 525 |
res.x = m00 * point.x + m01 * point.y + m02 * point.z + m03; |
| 526 |
res.y = m10 * point.x + m11 * point.y + m12 * point.z + m13; |
| 527 |
res.z = m20 * point.x + m21 * point.y + m22 * point.z + m23; |
| 528 |
|
| 529 |
w = m30 * point.x + m31 * point.y + m32 * point.z + m33; |
| 530 |
|
| 531 |
w = 1.f / w; |
| 532 |
res.x *= w; |
| 533 |
res.y *= w; |
| 534 |
res.z *= w; |
| 535 |
return res; |
| 536 |
} |
| 537 |
|
| 538 |
inline static Matrix4x4 |
| 539 |
Translate(const Vector3 &vector) |
| 540 |
{ |
| 541 |
Matrix4x4 m; |
| 542 |
m.m00 = 1.f; |
| 543 |
m.m01 = 0.f; |
| 544 |
m.m02 = 0.f; |
| 545 |
m.m03 = vector.x; |
| 546 |
m.m10 = 0.f; |
| 547 |
m.m11 = 1.f; |
| 548 |
m.m12 = 0.f; |
| 549 |
m.m13 = vector.y; |
| 550 |
m.m20 = 0.f; |
| 551 |
m.m21 = 0.f; |
| 552 |
m.m22 = 1.f; |
| 553 |
m.m23 = vector.z; |
| 554 |
m.m30 = 0.f; |
| 555 |
m.m31 = 0.f; |
| 556 |
m.m32 = 0.f; |
| 557 |
m.m33 = 1.f; |
| 558 |
return m; |
| 559 |
} |
| 560 |
|
| 561 |
inline Vector3 |
| 562 |
MultiplyVector(const Vector3 &vector) const |
| 563 |
{ |
| 564 |
Vector3 res; |
| 565 |
res.x = m00 * vector.x + m01 * vector.y + m02 * vector.z; |
| 566 |
res.y = m10 * vector.x + m11 * vector.y + m12 * vector.z; |
| 567 |
res.z = m20 * vector.x + m21 * vector.y + m22 * vector.z; |
| 568 |
return res; |
| 569 |
} |
| 570 |
|
| 571 |
inline Vector3 |
| 572 |
MultiplyPoint3x4(const Vector3 &point) const |
| 573 |
{ |
| 574 |
Vector3 res; |
| 575 |
res.x = m00 * point.x + m01 * point.y + m02 * point.z + m03; |
| 576 |
res.y = m10 * point.x + m11 * point.y + m12 * point.z + m13; |
| 577 |
res.z = m20 * point.x + m21 * point.y + m22 * point.z + m23; |
| 578 |
return res; |
| 579 |
} |
| 580 |
|
| 581 |
inline Matrix4x4 |
| 582 |
Transpose() const |
| 583 |
{ |
| 584 |
Matrix4x4 r; |
| 585 |
r.m00 = m00; |
| 586 |
r.m01 = m10; |
| 587 |
r.m02 = m20; |
| 588 |
r.m03 = m30; |
| 589 |
r.m10 = m01; |
| 590 |
r.m11 = m11; |
| 591 |
r.m12 = m21; |
| 592 |
r.m13 = m31; |
| 593 |
r.m20 = m02; |
| 594 |
r.m21 = m12; |
| 595 |
r.m22 = m22; |
| 596 |
r.m23 = m32; |
| 597 |
r.m30 = m03; |
| 598 |
r.m31 = m13; |
| 599 |
r.m32 = m23; |
| 600 |
r.m33 = m33; |
| 601 |
return r; |
| 602 |
} |
| 603 |
|
| 604 |
inline Matrix4x4 |
| 605 |
Inverse() const |
| 606 |
{ |
| 607 |
float A2323 = m22 * m33 - m23 * m32; |
| 608 |
float A1323 = m21 * m33 - m23 * m31; |
| 609 |
float A1223 = m21 * m32 - m22 * m31; |
| 610 |
float A0323 = m20 * m33 - m23 * m30; |
| 611 |
float A0223 = m20 * m32 - m22 * m30; |
| 612 |
float A0123 = m20 * m31 - m21 * m30; |
| 613 |
float A2313 = m12 * m33 - m13 * m32; |
| 614 |
float A1313 = m11 * m33 - m13 * m31; |
| 615 |
float A1213 = m11 * m32 - m12 * m31; |
| 616 |
float A2312 = m12 * m23 - m13 * m22; |
| 617 |
float A1312 = m11 * m23 - m13 * m21; |
| 618 |
float A1212 = m11 * m22 - m12 * m21; |
| 619 |
float A0313 = m10 * m33 - m13 * m30; |
| 620 |
float A0213 = m10 * m32 - m12 * m30; |
| 621 |
float A0312 = m10 * m23 - m13 * m20; |
| 622 |
float A0212 = m10 * m22 - m12 * m20; |
| 623 |
float A0113 = m10 * m31 - m11 * m30; |
| 624 |
float A0112 = m10 * m21 - m11 * m20; |
| 625 |
|
| 626 |
float det = |
| 627 |
m00 * (m11 * A2323 - m12 * A1323 + m13 * A1223) - m01 * (m10 * A2323 - m12 * A0323 + m13 * A0223) + |
| 628 |
m02 * (m10 * A1323 - m11 * A0323 + m13 * A0123) - m03 * (m10 * A1223 - m11 * A0223 + m12 * A0123); |
| 629 |
det = 1 / det; |
| 630 |
|
| 631 |
return Matrix4x4( |
| 632 |
det * (m11 * A2323 - m12 * A1323 + m13 * A1223), det * -(m01 * A2323 - m02 * A1323 + m03 * A1223), |
| 633 |
det * (m01 * A2313 - m02 * A1313 + m03 * A1213), det * -(m01 * A2312 - m02 * A1312 + m03 * A1212), |
| 634 |
det * -(m10 * A2323 - m12 * A0323 + m13 * A0223), det * (m00 * A2323 - m02 * A0323 + m03 * A0223), |
| 635 |
det * -(m00 * A2313 - m02 * A0313 + m03 * A0213), det * (m00 * A2312 - m02 * A0312 + m03 * A0212), |
| 636 |
det * (m10 * A1323 - m11 * A0323 + m13 * A0123), det * -(m00 * A1323 - m01 * A0323 + m03 * A0123), |
| 637 |
det * (m00 * A1313 - m01 * A0313 + m03 * A0113), det * -(m00 * A1312 - m01 * A0312 + m03 * A0112), |
| 638 |
det * -(m10 * A1223 - m11 * A0223 + m12 * A0123), det * (m00 * A1223 - m01 * A0223 + m02 * A0123), |
| 639 |
det * -(m00 * A1213 - m01 * A0213 + m02 * A0113), det * (m00 * A1212 - m01 * A0212 + m02 * A0112)); |
| 640 |
} |
| 641 |
|
| 642 |
float m00; |
| 643 |
float m01; |
| 644 |
float m02; |
| 645 |
float m03; |
| 646 |
|
| 647 |
float m10; |
| 648 |
float m11; |
| 649 |
float m12; |
| 650 |
float m13; |
| 651 |
|
| 652 |
float m20; |
| 653 |
float m21; |
| 654 |
float m22; |
| 655 |
float m23; |
| 656 |
|
| 657 |
float m30; |
| 658 |
float m31; |
| 659 |
float m32; |
| 660 |
float m33; |
| 661 |
}; |
| 662 |
|
| 663 |
// Vector4 min port to get code working... |
| 664 |
class Vector4 |
| 665 |
{ |
| 666 |
public: |
| 667 |
inline Vector4() |
| 668 |
{ |
| 669 |
x = 0.f; |
| 670 |
y = 0.f; |
| 671 |
z = 0.f; |
| 672 |
w = 0.f; |
| 673 |
} |
| 674 |
|
| 675 |
inline Vector4(const Vector4 &in) |
| 676 |
{ |
| 677 |
x = in.x; |
| 678 |
y = in.y; |
| 679 |
z = in.z; |
| 680 |
w = in.w; |
| 681 |
} |
| 682 |
|
| 683 |
constexpr Vector4 & |
| 684 |
operator=(const Vector4 &lhr) |
| 685 |
{ |
| 686 |
if (this == &lhr) { |
| 687 |
// avoid self-assign |
| 688 |
return *this; |
| 689 |
} |
| 690 |
this->x = lhr.x; |
| 691 |
this->y = lhr.y; |
| 692 |
this->z = lhr.z; |
| 693 |
this->w = lhr.w; |
| 694 |
return *this; |
| 695 |
} |
| 696 |
|
| 697 |
inline Matrix4x4 |
| 698 |
ComposeProjection() const |
| 699 |
{ |
| 700 |
const float zNear = 0.07f; |
| 701 |
const float zFar = 1000.f; |
| 702 |
|
| 703 |
float fLeft = x; |
| 704 |
float fRight = y; |
| 705 |
float fTop = z; |
| 706 |
float fBottom = w; |
| 707 |
|
| 708 |
float idx = 1.0f / (fRight - fLeft); |
| 709 |
float idy = 1.0f / (fBottom - fTop); |
| 710 |
// float idz = 1.0f / (zFar - zNear); |
| 711 |
float sx = fRight + fLeft; |
| 712 |
float sy = fBottom + fTop; |
| 713 |
|
| 714 |
float c = -(zFar + zNear) / (zFar - zNear); |
| 715 |
float d = -(2.0F * zFar * zNear) / (zFar - zNear); |
| 716 |
|
| 717 |
Matrix4x4 m; |
| 718 |
m.m00 = 2.f * idx; |
| 719 |
m.m01 = 0.f; |
| 720 |
m.m02 = sx * idx; |
| 721 |
m.m03 = 0.f; |
| 722 |
m.m10 = 0.f; |
| 723 |
m.m11 = 2.f * idy; |
| 724 |
m.m12 = sy * idy; |
| 725 |
m.m13 = 0.f; |
| 726 |
m.m20 = 0.f; |
| 727 |
m.m21 = 0.f; |
| 728 |
m.m22 = c; |
| 729 |
m.m23 = d; |
| 730 |
m.m30 = 0.f; |
| 731 |
m.m31 = 0.f; |
| 732 |
m.m32 = -1.0f; |
| 733 |
m.m33 = 0.f; |
| 734 |
|
| 735 |
return m; |
| 736 |
} |
| 737 |
|
| 738 |
float x; |
| 739 |
float y; |
| 740 |
float z; |
| 741 |
float w; |
| 742 |
}; |
| 743 |
|
| 744 |
class Ray |
| 745 |
{ |
| 746 |
public: |
| 747 |
inline Ray(const Vector3 &origin, Vector3 direction) |
| 748 |
{ |
| 749 |
m_Origin = origin; |
| 750 |
direction.Normalize(); |
| 751 |
m_Direction.x = direction.x; |
| 752 |
m_Direction.y = direction.y; |
| 753 |
m_Direction.z = direction.z; |
| 754 |
} |
| 755 |
|
| 756 |
inline Vector3 |
| 757 |
GetPoint(float distance) const |
| 758 |
{ |
| 759 |
return m_Origin + m_Direction * distance; |
| 760 |
} |
| 761 |
|
| 762 |
Vector3 m_Origin; |
| 763 |
Vector3 m_Direction; |
| 764 |
}; |
| 765 |
|
| 766 |
static inline float |
| 767 |
SIGN(float x) |
| 768 |
{ |
| 769 |
return (x >= 0.0f) ? +1.0f : -1.0f; |
| 770 |
} |
| 771 |
|
| 772 |
static inline float |
| 773 |
NORM(float a, float b, float c, float d) |
| 774 |
{ |
| 775 |
return sqrt(a * a + b * b + c * c + d * d); |
| 776 |
} |
| 777 |
|
| 778 |
class Quaternion |
| 779 |
{ |
| 780 |
public: |
| 781 |
inline Quaternion() |
| 782 |
{ |
| 783 |
x = 0.f; |
| 784 |
y = 0.f; |
| 785 |
z = 0.f; |
| 786 |
w = 1.f; |
| 787 |
}; |
| 788 |
|
| 789 |
inline Quaternion(float _x, float _y, float _z, float _w) |
| 790 |
{ |
| 791 |
x = _x; |
| 792 |
y = _y; |
| 793 |
z = _z; |
| 794 |
w = _w; |
| 795 |
}; |
| 796 |
|
| 797 |
inline Quaternion(const Quaternion &_in) = default; |
| 798 |
|
| 799 |
Quaternion & |
| 800 |
operator=(const Quaternion &lhr) = default; |
| 801 |
|
| 802 |
inline static Quaternion |
| 803 |
Identity() |
| 804 |
{ |
| 805 |
return Quaternion(0, 0, 0, 1); |
| 806 |
} |
| 807 |
|
| 808 |
inline Quaternion |
| 809 |
conjugate() const |
| 810 |
{ |
| 811 |
return Quaternion(-x, -y, -z, w); |
| 812 |
} |
| 813 |
|
| 814 |
inline float |
| 815 |
norm() const |
| 816 |
{ |
| 817 |
return sqrt((x * x) + (y * y) + (z * z) + (w * w)); |
| 818 |
} |
| 819 |
|
| 820 |
inline Quaternion |
| 821 |
scale(float s) const |
| 822 |
{ |
| 823 |
return Quaternion(w * s, x * s, y * s, z * s); |
| 824 |
} |
| 825 |
|
| 826 |
inline Quaternion |
| 827 |
Inverse() const |
| 828 |
{ |
| 829 |
return conjugate() / norm(); |
| 830 |
} |
| 831 |
|
| 832 |
inline Vector3 |
| 833 |
Right() const |
| 834 |
{ |
| 835 |
return *this * Vector3::Right(); |
| 836 |
} |
| 837 |
|
| 838 |
inline Vector3 |
| 839 |
Up() const |
| 840 |
{ |
| 841 |
return *this * Vector3::Up(); |
| 842 |
} |
| 843 |
|
| 844 |
inline Vector3 |
| 845 |
Forward() const |
| 846 |
{ |
| 847 |
return *this * Vector3::Forward(); |
| 848 |
} |
| 849 |
|
| 850 |
inline static void |
| 851 |
ToEulerAngle(const Quaternion &q, float &roll, float &pitch, float &yaw) |
| 852 |
{ |
| 853 |
float sinr = +2.0f * (q.w * q.x + q.y * q.z); |
| 854 |
float cosr = +1.0f - 2.0f * (q.x * q.x + q.y * q.y); |
| 855 |
roll = atan2f(sinr, cosr); |
| 856 |
|
| 857 |
float sinp = +2.0f * (q.w * q.y - q.z * q.x); |
| 858 |
if (fabs(sinp) >= 1.f) { |
| 859 |
pitch = copysignf(M_PI / 2.f, sinp); |
| 860 |
} else { |
| 861 |
pitch = asinf(sinp); |
| 862 |
} |
| 863 |
|
| 864 |
float siny = +2.0f * (q.w * q.z + q.x * q.y); |
| 865 |
float cosy = +1.0f - 2.0f * (q.y * q.y + q.z * q.z); |
| 866 |
yaw = atan2f(siny, cosy); |
| 867 |
} |
| 868 |
|
| 869 |
inline static Vector3 |
| 870 |
ToEulerAngles(const Quaternion &in) |
| 871 |
{ |
| 872 |
Vector3 euler; |
| 873 |
const static float PI_OVER_2 = M_PI * 0.5f; |
| 874 |
const static float EPSILON = 1e-10f; |
| 875 |
float sqw; |
| 876 |
float sqx; |
| 877 |
float sqy; |
| 878 |
float sqz; |
| 879 |
|
| 880 |
// quick conversion to Euler angles to give tilt to user |
| 881 |
sqw = in.w * in.w; |
| 882 |
sqx = in.x * in.x; |
| 883 |
sqy = in.y * in.y; |
| 884 |
sqz = in.z * in.z; |
| 885 |
|
| 886 |
euler.y = asinf(2.0f * (in.w * in.y - in.x * in.z)); |
| 887 |
if (PI_OVER_2 - fabs(euler.y) > EPSILON) { |
| 888 |
euler.z = atan2f(2.0f * (in.x * in.y + in.w * in.z), sqx - sqy - sqz + sqw); |
| 889 |
euler.x = atan2f(2.0f * (in.w * in.x + in.y * in.z), sqw - sqx - sqy + sqz); |
| 890 |
} else { |
| 891 |
// compute heading from local 'down' vector |
| 892 |
euler.z = atan2f(2.f * in.y * in.z - 2.f * in.x * in.w, 2.f * in.x * in.z + 2.f * in.y * in.w); |
| 893 |
euler.x = 0.0f; |
| 894 |
|
| 895 |
// If facing down, reverse yaw |
| 896 |
if (euler.y < 0.f) { |
| 897 |
euler.z = M_PI - euler.z; |
| 898 |
} |
| 899 |
} |
| 900 |
return euler; |
| 901 |
} |
| 902 |
|
| 903 |
inline Vector3 // Until clang-format-11 is on the CI. |
| 904 |
operator*(Vector3 const &vec) const |
| 905 |
{ |
| 906 |
float num = x * 2.f; |
| 907 |
float num2 = y * 2.f; |
| 908 |
float num3 = z * 2.f; |
| 909 |
float num4 = x * num; |
| 910 |
float num5 = y * num2; |
| 911 |
float num6 = z * num3; |
| 912 |
float num7 = x * num2; |
| 913 |
float num8 = x * num3; |
| 914 |
float num9 = y * num3; |
| 915 |
float num10 = w * num; |
| 916 |
float num11 = w * num2; |
| 917 |
float num12 = w * num3; |
| 918 |
Vector3 result; |
| 919 |
result.x = (1.f - (num5 + num6)) * vec.x + (num7 - num12) * vec.y + (num8 + num11) * vec.z; |
| 920 |
result.y = (num7 + num12) * vec.x + (1.f - (num4 + num6)) * vec.y + (num9 - num10) * vec.z; |
| 921 |
result.z = (num8 - num11) * vec.x + (num9 + num10) * vec.y + (1.f - (num4 + num5)) * vec.z; |
| 922 |
return result; |
| 923 |
} |
| 924 |
|
| 925 |
inline static Quaternion |
| 926 |
Euler(const Vector3 &euler) |
| 927 |
{ |
| 928 |
float c1 = cos(euler.z * 0.5f); |
| 929 |
float c2 = cos(euler.y * 0.5f); |
| 930 |
float c3 = cos(euler.x * 0.5f); |
| 931 |
float s1 = sin(euler.z * 0.5f); |
| 932 |
float s2 = sin(euler.y * 0.5f); |
| 933 |
float s3 = sin(euler.x * 0.5f); |
| 934 |
|
| 935 |
Quaternion ret; |
| 936 |
ret.x = c1 * c2 * s3 - s1 * s2 * c3; |
| 937 |
ret.y = c1 * s2 * c3 + s1 * c2 * s3; |
| 938 |
ret.z = s1 * c2 * c3 - c1 * s2 * s3; |
| 939 |
ret.w = c1 * c2 * c3 + s1 * s2 * s3; |
| 940 |
|
| 941 |
return ret; |
| 942 |
} |
| 943 |
|
| 944 |
|
| 945 |
Matrix4x4 |
| 946 |
ToMatrix4x4() const |
| 947 |
{ |
| 948 |
float qw = w; |
| 949 |
float qx = x; |
| 950 |
float qy = y; |
| 951 |
float qz = z; |
| 952 |
|
| 953 |
const float n = 1.0f / sqrt(qx * qx + qy * qy + qz * qz + qw * qw); |
| 954 |
qx *= n; |
| 955 |
qy *= n; |
| 956 |
qz *= n; |
| 957 |
qw *= n; |
| 958 |
|
| 959 |
return Matrix4x4(1.0f - 2.0f * qy * qy - 2.0f * qz * qz, 2.0f * qx * qy - 2.0f * qz * qw, |
| 960 |
2.0f * qx * qz + 2.0f * qy * qw, 0.0f, 2.0f * qx * qy + 2.0f * qz * qw, |
| 961 |
1.0f - 2.0f * qx * qx - 2.0f * qz * qz, 2.0f * qy * qz - 2.0f * qx * qw, 0.0f, |
| 962 |
2.0f * qx * qz - 2.0f * qy * qw, 2.0f * qy * qz + 2.0f * qx * qw, |
| 963 |
1.0f - 2.0f * qx * qx - 2.0f * qy * qy, 0.0f, 0.0f, 0.0f, 0.0f, 1.0f); |
| 964 |
} |
| 965 |
|
| 966 |
inline Quaternion // Until clang-format-11 is on the CI. |
| 967 |
operator*(const Quaternion &q) const |
| 968 |
{ |
| 969 |
return Quaternion(y * q.z - z * q.y + x * q.w + w * q.x, z * q.x - x * q.z + y * q.w + w * q.y, |
| 970 |
x * q.y - y * q.x + z * q.w + w * q.z, w * q.w - x * q.x - y * q.y - z * q.z); |
| 971 |
} |
| 972 |
|
| 973 |
Quaternion |
| 974 |
operator/(const float div) const |
| 975 |
{ |
| 976 |
return Quaternion(x / div, y / div, z / div, w / div); |
| 977 |
} |
| 978 |
|
| 979 |
inline static Quaternion |
| 980 |
AxisAngle(const Vector3 &axis, float angle) |
| 981 |
{ |
| 982 |
float halfAngle = angle * .5f; |
| 983 |
float s = (float)sin(halfAngle); |
| 984 |
Quaternion q; |
| 985 |
q.x = axis.x * s; |
| 986 |
q.y = axis.y * s; |
| 987 |
q.z = axis.z * s; |
| 988 |
q.w = (float)cos(halfAngle); |
| 989 |
return q; |
| 990 |
} |
| 991 |
|
| 992 |
inline static Quaternion |
| 993 |
LookAt(const Vector3 &sourcePoint, const Vector3 &destPoint) |
| 994 |
{ |
| 995 |
Vector3 forwardVector = (destPoint - sourcePoint).Normalized(); |
| 996 |
|
| 997 |
float dot = Vector3::Dot(Vector3::Forward(), forwardVector); |
| 998 |
|
| 999 |
if (fabs(dot - (-1.0f)) < 0.000001f) { |
| 1000 |
return Quaternion(Vector3::Up().x, Vector3::Up().y, Vector3::Up().z, 3.1415926535897932f); |
| 1001 |
} |
| 1002 |
if (fabs(dot - (1.0f)) < 0.000001f) { |
| 1003 |
return Quaternion(); |
| 1004 |
} |
| 1005 |
|
| 1006 |
float rotAngle = cos(dot); |
| 1007 |
Vector3 rotAxis = Vector3::Forward().Cross(forwardVector); |
| 1008 |
rotAxis = rotAxis.Normalized(); |
| 1009 |
return AxisAngle(rotAxis, rotAngle); |
| 1010 |
} |
| 1011 |
|
| 1012 |
inline static Quaternion |
| 1013 |
QuaternionLookRotation(const Vector3 &forward, const Vector3 &Up) |
| 1014 |
{ |
| 1015 |
|
| 1016 |
Vector3 vector1 = forward.Normalized(); |
| 1017 |
Vector3 vector2 = (Up.Cross(vector1)).Normalized(); |
| 1018 |
Vector3 vector3 = vector1.Cross(vector2); |
| 1019 |
|
| 1020 |
float m00 = vector2.x; |
| 1021 |
float m01 = vector2.y; |
| 1022 |
float m02 = vector2.z; |
| 1023 |
float m10 = vector3.x; |
| 1024 |
float m11 = vector3.y; |
| 1025 |
float m12 = vector3.z; |
| 1026 |
float m20 = vector1.x; |
| 1027 |
float m21 = vector1.y; |
| 1028 |
float m22 = vector1.z; |
| 1029 |
|
| 1030 |
float num8 = (m00 + m11) + m22; |
| 1031 |
Quaternion quaternion; |
| 1032 |
if (num8 > 0.f) { |
| 1033 |
float num = (float)sqrtf(num8 + 1.f); |
| 1034 |
quaternion.w = num * 0.5f; |
| 1035 |
num = 0.5f / num; |
| 1036 |
quaternion.x = (m12 - m21) * num; |
| 1037 |
quaternion.y = (m20 - m02) * num; |
| 1038 |
quaternion.z = (m01 - m10) * num; |
| 1039 |
return quaternion; |
| 1040 |
} |
| 1041 |
|
| 1042 |
if ((m00 >= m11) && (m00 >= m22)) { |
| 1043 |
float num7 = (float)sqrtf(((1.f + m00) - m11) - m22); |
| 1044 |
float num4 = 0.5f / num7; |
| 1045 |
quaternion.x = 0.5f * num7; |
| 1046 |
quaternion.y = (m01 + m10) * num4; |
| 1047 |
quaternion.z = (m02 + m20) * num4; |
| 1048 |
quaternion.w = (m12 - m21) * num4; |
| 1049 |
return quaternion; |
| 1050 |
} |
| 1051 |
|
| 1052 |
if (m11 > m22) { |
| 1053 |
float num6 = (float)sqrtf(((1.f + m11) - m00) - m22); |
| 1054 |
float num3 = 0.5f / num6; |
| 1055 |
quaternion.x = (m10 + m01) * num3; |
| 1056 |
quaternion.y = 0.5f * num6; |
| 1057 |
quaternion.z = (m21 + m12) * num3; |
| 1058 |
quaternion.w = (m20 - m02) * num3; |
| 1059 |
return quaternion; |
| 1060 |
} |
| 1061 |
|
| 1062 |
float num5 = (float)sqrtf(((1.f + m22) - m00) - m11); |
| 1063 |
float num2 = 0.5f / num5; |
| 1064 |
quaternion.x = (m20 + m02) * num2; |
| 1065 |
quaternion.y = (m21 + m12) * num2; |
| 1066 |
quaternion.z = 0.5f * num5; |
| 1067 |
quaternion.w = (m01 - m10) * num2; |
| 1068 |
return quaternion; |
| 1069 |
} |
| 1070 |
|
| 1071 |
inline static Quaternion |
| 1072 |
FromMatrix(const Matrix4x4 &m) |
| 1073 |
{ |
| 1074 |
float tr = m.m00 + m.m11 + m.m22; |
| 1075 |
float qx; |
| 1076 |
float qy; |
| 1077 |
float qz; |
| 1078 |
float qw; |
| 1079 |
|
| 1080 |
if (tr > 0) { |
| 1081 |
float S = sqrtf(tr + 1.f) * 2.f; |
| 1082 |
qw = 0.25f * S; |
| 1083 |
qx = (m.m21 - m.m12) / S; |
| 1084 |
qy = (m.m02 - m.m20) / S; |
| 1085 |
qz = (m.m10 - m.m01) / S; |
| 1086 |
} else if ((m.m00 > m.m11) && (m.m00 > m.m22)) { |
| 1087 |
float S = sqrtf(1.f + m.m00 - m.m11 - m.m22) * 2.f; |
| 1088 |
qw = (m.m21 - m.m12) / S; |
| 1089 |
qx = 0.25f * S; |
| 1090 |
qy = (m.m01 + m.m10) / S; |
| 1091 |
qz = (m.m02 + m.m20) / S; |
| 1092 |
} else if (m.m11 > m.m22) { |
| 1093 |
float S = sqrtf(1.f + m.m11 - m.m00 - m.m22) * 2.f; |
| 1094 |
qw = (m.m02 - m.m20) / S; |
| 1095 |
qx = (m.m01 + m.m10) / S; |
| 1096 |
qy = 0.25f * S; |
| 1097 |
qz = (m.m12 + m.m21) / S; |
| 1098 |
} else { |
| 1099 |
float S = sqrtf(1.f + m.m22 - m.m00 - m.m11) * 2.f; |
| 1100 |
qw = (m.m10 - m.m01) / S; |
| 1101 |
qx = (m.m02 + m.m20) / S; |
| 1102 |
qy = (m.m12 + m.m21) / S; |
| 1103 |
qz = 0.25f * S; |
| 1104 |
} |
| 1105 |
|
| 1106 |
return Quaternion(qx, qy, qz, qw); |
| 1107 |
} |
| 1108 |
|
| 1109 |
//// quaternion = [w, x, y, z]' |
| 1110 |
// void mRot2Quat(const Matrix4x4 m) { |
| 1111 |
// float r11 = m.m00; |
| 1112 |
// float r12 = m.m01; |
| 1113 |
// float r13 = m.m02; |
| 1114 |
// float r21 = m.m10; |
| 1115 |
// float r22 = m.m11; |
| 1116 |
// float r23 = m.m12; |
| 1117 |
// float r31 = m.m20; |
| 1118 |
// float r32 = m.m21; |
| 1119 |
// float r33 = m.m22; |
| 1120 |
// float w = (r11 + r22 + r33 + 1.0f) / 4.0f; |
| 1121 |
// float x = (r11 - r22 - r33 + 1.0f) / 4.0f; |
| 1122 |
// float y = (-r11 + r22 - r33 + 1.0f) / 4.0f; |
| 1123 |
// float z = (-r11 - r22 + r33 + 1.0f) / 4.0f; |
| 1124 |
// if (w < 0.0f) { |
| 1125 |
// w = 0.0f; |
| 1126 |
// } |
| 1127 |
// if (x < 0.0f) { |
| 1128 |
// x = 0.0f; |
| 1129 |
// } |
| 1130 |
// if (y < 0.0f) { |
| 1131 |
// y = 0.0f; |
| 1132 |
// } |
| 1133 |
// if (z < 0.0f) { |
| 1134 |
// z = 0.0f; |
| 1135 |
// } |
| 1136 |
// w = sqrt(w); |
| 1137 |
// x = sqrt(x); |
| 1138 |
// y = sqrt(y); |
| 1139 |
// z = sqrt(z); |
| 1140 |
// if (w >= x && w >= y && w >= z) { |
| 1141 |
// w *= +1.0f; |
| 1142 |
// x *= SIGN(r32 - r23); |
| 1143 |
// y *= SIGN(r13 - r31); |
| 1144 |
// z *= SIGN(r21 - r12); |
| 1145 |
// } |
| 1146 |
// else if (x >= w && x >= y && x >= z) { |
| 1147 |
// w *= SIGN(r32 - r23); |
| 1148 |
// x *= +1.0f; |
| 1149 |
// y *= SIGN(r21 + r12); |
| 1150 |
// z *= SIGN(r13 + r31); |
| 1151 |
// } |
| 1152 |
// else if (y >= w && y >= x && y >= z) { |
| 1153 |
// w *= SIGN(r13 - r31); |
| 1154 |
// x *= SIGN(r21 + r12); |
| 1155 |
// y *= +1.0f; |
| 1156 |
// z *= SIGN(r32 + r23); |
| 1157 |
// } |
| 1158 |
// else if (z >= w && z >= x && z >= y) { |
| 1159 |
// w *= SIGN(r21 - r12); |
| 1160 |
// x *= SIGN(r31 + r13); |
| 1161 |
// y *= SIGN(r32 + r23); |
| 1162 |
// z *= +1.0f; |
| 1163 |
// } |
| 1164 |
// else { |
| 1165 |
// //U_LOG_E("coding error"); |
| 1166 |
// } |
| 1167 |
// float r = NORM(w, x, w, z); |
| 1168 |
// w /= r; |
| 1169 |
// x /= r; |
| 1170 |
// y /= r; |
| 1171 |
// z /= r; |
| 1172 |
//} |
| 1173 |
|
| 1174 |
inline static Quaternion |
| 1175 |
FromToRotation(const Vector3 &dir0, const Vector3 &dir1) |
| 1176 |
{ |
| 1177 |
Vector3 axis = dir0.Cross(dir1).Normalized(); |
| 1178 |
float angle = Vector3::Angle(dir0, dir1); |
| 1179 |
return Quaternion::AxisAngle(axis, angle); |
| 1180 |
} |
| 1181 |
|
| 1182 |
float x; |
| 1183 |
float y; |
| 1184 |
float z; |
| 1185 |
float w; |
| 1186 |
}; |