scene/rotation.cpp
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | // SPDX-License-Identifier: BSD-3-Clause | ||
| 2 | #include "chrono_batch_render/scene/rotation.h" | ||
| 3 | |||
| 4 | #include <algorithm> | ||
| 5 | #include <cmath> | ||
| 6 | #include <numbers> | ||
| 7 | |||
| 8 | #include "chrono_batch_render/util/error.h" | ||
| 9 | |||
| 10 | namespace chrono::batch_render { | ||
| 11 | |||
| 12 | namespace { | ||
| 13 | |||
| 14 | 612 | double Dot(Double3 a, Double3 b) { | |
| 15 | 612 | return a.x * b.x + a.y * b.y + a.z * b.z; | |
| 16 | } | ||
| 17 | |||
| 18 | 376 | Double3 Cross(Double3 a, Double3 b) { | |
| 19 | 376 | return {a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x}; | |
| 20 | } | ||
| 21 | |||
| 22 | 470 | Double3 Unit(Double3 v) { | |
| 23 | 470 | const double length = std::sqrt(Dot(v, v)); | |
| 24 | 470 | if (length == 0) { | |
| 25 | 10 | throw Error("A direction of zero length has no rotation"); | |
| 26 | } | ||
| 27 | 465 | return {v.x / length, v.y / length, v.z / length}; | |
| 28 | } | ||
| 29 | |||
| 30 | 85 | double Radians(double degrees) { | |
| 31 | 85 | return degrees * std::numbers::pi / 180; | |
| 32 | } | ||
| 33 | |||
| 34 | 12 | double Degrees(double radians) { | |
| 35 | 12 | return radians * 180 / std::numbers::pi; | |
| 36 | } | ||
| 37 | |||
| 38 | } | ||
| 39 | |||
| 40 | 146 | Quatd LookAlong(Double3 direction, Double3 up) { | |
| 41 | 146 | const Double3 z = Unit({-direction.x, -direction.y, -direction.z}); | |
| 42 | 143 | Double3 axis = Unit(up); | |
| 43 | 142 | if (std::abs(Dot(z, axis)) > 0.999) { | |
| 44 | 5 | axis = std::abs(axis.y) < 0.999 ? Double3{0, 1, 0} : Double3{0, 0, 1}; | |
| 45 | } | ||
| 46 | 142 | const Double3 x = Unit(Cross(axis, z)); | |
| 47 | 142 | return FromAxes(x, Cross(z, x), z); | |
| 48 | } | ||
| 49 | |||
| 50 | 110 | Quatd LookAt(Double3 eye, Double3 target, Double3 up) { | |
| 51 | 110 | return LookAlong({target.x - eye.x, target.y - eye.y, target.z - eye.z}, up); | |
| 52 | } | ||
| 53 | |||
| 54 | 39 | Quatd FromAxisAngle(Double3 axis, double degrees) { | |
| 55 | 39 | const Double3 n = Unit(axis); | |
| 56 | 38 | const double half = Radians(degrees) / 2; | |
| 57 | 38 | const double s = std::sin(half); | |
| 58 | 38 | return {n.x * s, n.y * s, n.z * s, std::cos(half)}; | |
| 59 | } | ||
| 60 | |||
| 61 | 10 | Quatd FromAzimuthElevation(double azimuth, double elevation) { | |
| 62 | 10 | const double a = Radians(azimuth); | |
| 63 | 10 | const double e = Radians(elevation); | |
| 64 | 10 | return LookAlong({-std::sin(a) * std::cos(e), -std::cos(a) * std::cos(e), -std::sin(e)}); | |
| 65 | } | ||
| 66 | |||
| 67 | 148 | Quatd FromAxes(Double3 x, Double3 y, Double3 z) { | |
| 68 | 148 | const double trace = x.x + y.y + z.z; | |
| 69 | 148 | if (trace > 0) { | |
| 70 | 128 | const double s = std::sqrt(trace + 1) * 2; | |
| 71 | 128 | return {(y.z - z.y) / s, (z.x - x.z) / s, (x.y - y.x) / s, s / 4}; | |
| 72 | } | ||
| 73 | 20 | if (x.x > y.y && x.x > z.z) { | |
| 74 | 2 | const double s = std::sqrt(1 + x.x - y.y - z.z) * 2; | |
| 75 | 2 | return {s / 4, (y.x + x.y) / s, (z.x + x.z) / s, (y.z - z.y) / s}; | |
| 76 | } | ||
| 77 | 18 | if (y.y > z.z) { | |
| 78 | 5 | const double s = std::sqrt(1 + y.y - x.x - z.z) * 2; | |
| 79 | 5 | return {(y.x + x.y) / s, s / 4, (z.y + y.z) / s, (z.x - x.z) / s}; | |
| 80 | } | ||
| 81 | 13 | const double s = std::sqrt(1 + z.z - x.x - y.y) * 2; | |
| 82 | 13 | return {(z.x + x.z) / s, (z.y + y.z) / s, s / 4, (x.y - y.x) / s}; | |
| 83 | } | ||
| 84 | |||
| 85 | 9 | Quatd FromEulerDegrees(Double3 degrees) { | |
| 86 | 9 | const double cx = std::cos(Radians(degrees.x) / 2); | |
| 87 | 9 | const double sx = std::sin(Radians(degrees.x) / 2); | |
| 88 | 9 | const double cy = std::cos(Radians(degrees.y) / 2); | |
| 89 | 9 | const double sy = std::sin(Radians(degrees.y) / 2); | |
| 90 | 9 | const double cz = std::cos(Radians(degrees.z) / 2); | |
| 91 | 9 | const double sz = std::sin(Radians(degrees.z) / 2); | |
| 92 | 9 | return {sx * cy * cz - cx * sy * sz, cx * sy * cz + sx * cy * sz, cx * cy * sz - sx * sy * cz, | |
| 93 | 9 | cx * cy * cz + sx * sy * sz}; | |
| 94 | } | ||
| 95 | |||
| 96 | 4 | Double3 ToEulerDegrees(Quatd q) { | |
| 97 | 4 | const double sine = std::clamp(2 * (q.w * q.y - q.z * q.x), -1.0, 1.0); | |
| 98 | 4 | return {Degrees(std::atan2(2 * (q.w * q.x + q.y * q.z), 1 - 2 * (q.x * q.x + q.y * q.y))), | |
| 99 | 4 | Degrees(std::asin(sine)), | |
| 100 | 4 | Degrees(std::atan2(2 * (q.w * q.z + q.x * q.y), 1 - 2 * (q.y * q.y + q.z * q.z)))}; | |
| 101 | } | ||
| 102 | |||
| 103 | 4 | Quatd Multiply(Quatd a, Quatd b) { | |
| 104 | 4 | return {a.w * b.x + a.x * b.w + a.y * b.z - a.z * b.y, | |
| 105 | 4 | a.w * b.y - a.x * b.z + a.y * b.w + a.z * b.x, | |
| 106 | 4 | a.w * b.z + a.x * b.y - a.y * b.x + a.z * b.w, | |
| 107 | 4 | a.w * b.w - a.x * b.x - a.y * b.y - a.z * b.z}; | |
| 108 | } | ||
| 109 | |||
| 110 | 2 | Quatd Conjugate(Quatd rotation) { | |
| 111 | 2 | return {-rotation.x, -rotation.y, -rotation.z, rotation.w}; | |
| 112 | } | ||
| 113 | |||
| 114 | 2 | Quatd Normalize(Quatd q) { | |
| 115 | 2 | const double length = std::sqrt(q.x * q.x + q.y * q.y + q.z * q.z + q.w * q.w); | |
| 116 | 2 | return length > 0 ? Quatd{q.x / length, q.y / length, q.z / length, q.w / length} : Quatd{}; | |
| 117 | } | ||
| 118 | |||
| 119 | 46 | Double3 Rotate(Quatd rotation, Double3 vector) { | |
| 120 | 46 | const Double3 axis{rotation.x, rotation.y, rotation.z}; | |
| 121 | 46 | const Double3 cross = Cross(axis, vector); | |
| 122 | 46 | const Double3 twice = Cross(axis, cross); | |
| 123 | 46 | return {vector.x + 2 * (rotation.w * cross.x + twice.x), | |
| 124 | 46 | vector.y + 2 * (rotation.w * cross.y + twice.y), | |
| 125 | 46 | vector.z + 2 * (rotation.w * cross.z + twice.z)}; | |
| 126 | } | ||
| 127 | |||
| 128 | } | ||
| 129 |