-
Notifications
You must be signed in to change notification settings - Fork 0
Commit
This commit does not belong to any branch on this repository, and may belong to a fork outside of the repository.
Zephyr: Scene: simplify rotation system
- Loading branch information
Showing
4 changed files
with
76 additions
and
194 deletions.
There are no files selected for viewing
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -1,195 +1,77 @@ | ||
|
|
||
| #pragma once | ||
|
|
||
| #include <zephyr/math/matrix4.hpp> | ||
| #include <zephyr/math/quaternion.hpp> | ||
| #include <zephyr/math/vector.hpp> | ||
| #include <zephyr/event.hpp> | ||
| #include <zephyr/float.hpp> | ||
|
|
||
| namespace zephyr { | ||
|
|
||
| /** | ||
| * Represents a rotation around an arbitrary axis in 3D space. | ||
| * Underlying to the rotation is a quaternion encoding the current axis and angle. | ||
| * Convert a unit quaternion to extrinsic x-y-z (intrinsic z-y'-x'') Tait-Bryan angles. | ||
| * @param quaternion The unit quaternion | ||
| * @returns a {@link #Vector3} storing the X, Y and Z angles. | ||
| */ | ||
| class Rotation { | ||
| public: | ||
| /** | ||
| * Default constructor. By default the Rotation is initialized to not rotate. | ||
| */ | ||
| Rotation() { | ||
| SetFromQuaternion(1.0f, 0.0f, 0.0f, 0.0f); | ||
| } | ||
|
|
||
| /** | ||
| * Construct a Rotation from extrinsic x-y-z (intrinsic z-y'-x'') Tait-Bryan angles. | ||
| * @param x X angle | ||
| * @param y Y angle | ||
| * @param z Z angle | ||
| */ | ||
| Rotation(f32 x, f32 y, f32 z) { | ||
| SetFromEuler(x, y, z); | ||
| } | ||
|
|
||
| /** | ||
| * Construct a Rotation from a quaternion. | ||
| * @param quaternion the Quaternion | ||
| */ | ||
| explicit Rotation(const Quaternion& quaternion) { | ||
| SetFromQuaternion(quaternion); | ||
| } | ||
|
|
||
| /// @returns a reference to an event that is emitted whenever the rotation changes. | ||
| [[nodiscard]] VoidEvent& OnChange() const { | ||
| return m_event_on_change; | ||
| } | ||
|
|
||
| /// @returns the rotation in quaternion form | ||
| [[nodiscard]] const Quaternion& GetAsQuaternion() const { | ||
| return m_quaternion; | ||
| } | ||
|
|
||
| /** | ||
| * Set the rotation from a quaternion. | ||
| * @param quaternion the Quaternion | ||
| */ | ||
| void SetFromQuaternion(const Quaternion& quaternion) { | ||
| m_quaternion = quaternion; | ||
| MarkQuaternionAsChanged(); | ||
| } | ||
|
|
||
| /** | ||
| * Set the rotation from a quaternion. | ||
| * @param w the w-component of the quaternion | ||
| * @param x the x-component of the quaternion | ||
| * @param y the y-component of the quaternion | ||
| * @param z the z-component of the quaternion | ||
| */ | ||
| void SetFromQuaternion(f32 w, f32 x, f32 y, f32 z) { | ||
| SetFromQuaternion({w, x, y, z}); | ||
| } | ||
|
|
||
| /// @returns the rotation in 4x4 matrix form | ||
| [[nodiscard]] const Matrix4& GetAsMatrix4() const { | ||
| UpdateMatrixFromQuaternion(); | ||
| return m_matrix; | ||
| } | ||
|
|
||
| /** | ||
| * Set the rotation from a 4x4 matrix | ||
| * @param matrix the 4x4 matrix | ||
| */ | ||
| void SetFromMatrix4(const Matrix4& matrix) { | ||
| m_quaternion = Quaternion::FromRotationMatrix(matrix); | ||
| // The 4x4 matrix is likely to be read and copying it now is faster than reconstructing it later. | ||
| m_matrix = matrix; | ||
| m_needs_euler_refresh = true; | ||
| m_event_on_change.Emit(); | ||
| } | ||
|
|
||
| /** | ||
| * Returns the rotation in extrinsic x-y-z (intrinsic z-y'-x'') Tait-Bryan angles form. | ||
| * This is an expensive operation because the angles may need to be reconstructed. | ||
| * Due to the reconstruction, there also is no guarantee that calling | ||
| * {@link #GetAsEuler} after {@link #SetFromEuler} will return the original angles. | ||
| * @returns the Tait-Bryan angles | ||
| */ | ||
| const Vector3& GetAsEuler() { | ||
| UpdateEulerFromQuaternion(); | ||
| return m_euler; | ||
| } | ||
|
|
||
| /** | ||
| * Set the rotation from extrinsic x-y-z (intrinsic z-y'-x'') Tait-Bryan angles. | ||
| * @param x X angle | ||
| * @param y Y angle | ||
| * @param z Z angle | ||
| */ | ||
| void SetFromEuler(f32 x, f32 y, f32 z) { | ||
| const f32 half_x = x * 0.5f; | ||
| const f32 cos_x = std::cos(half_x); | ||
| const f32 sin_x = std::sin(half_x); | ||
|
|
||
| const f32 half_y = y * 0.5f; | ||
| const f32 cos_y = std::cos(half_y); | ||
| const f32 sin_y = std::sin(half_y); | ||
|
|
||
| const f32 half_z = z * 0.5f; | ||
| const f32 cos_z = std::cos(half_z); | ||
| const f32 sin_z = std::sin(half_z); | ||
|
|
||
| const f32 cos_z_cos_y = cos_z * cos_y; | ||
| const f32 sin_z_cos_y = sin_z * cos_y; | ||
| const f32 sin_z_sin_y = sin_z * sin_y; | ||
| const f32 cos_z_sin_y = cos_z * sin_y; | ||
|
|
||
| SetFromQuaternion( | ||
| cos_z_cos_y * cos_x + sin_z_sin_y * sin_x, | ||
| cos_z_cos_y * sin_x - sin_z_sin_y * cos_x, | ||
| sin_z_cos_y * sin_x + cos_z_sin_y * cos_x, | ||
| sin_z_cos_y * cos_x - cos_z_sin_y * sin_x | ||
| ); | ||
| } | ||
|
|
||
| /** | ||
| * Set the rotation from extrinsic x-y-z (intrinsic z-y'-x'') Tait-Bryan angles. | ||
| * @param euler the Tait-Bryan euler angles | ||
| */ | ||
| void SetFromEuler(Vector3 euler) { | ||
| SetFromEuler(euler.X(), euler.Y(), euler.Z()); | ||
| } | ||
|
|
||
| private: | ||
| /// Signal internally that the quaternion has changed | ||
| void MarkQuaternionAsChanged() { | ||
| m_needs_matrix_refresh = true; | ||
| m_needs_euler_refresh = true; | ||
| m_event_on_change.Emit(); | ||
| } | ||
|
|
||
| /// Update the 4x4 matrix from the quaternion. | ||
| void UpdateMatrixFromQuaternion() const { | ||
| if(m_needs_matrix_refresh) { | ||
| m_matrix = m_quaternion.ToRotationMatrix(); | ||
| m_needs_matrix_refresh = false; | ||
| } | ||
| } | ||
|
|
||
| /// Update the euler angles from the quaternion. | ||
| void UpdateEulerFromQuaternion() const { | ||
| if(!m_needs_euler_refresh) { | ||
| return; | ||
| } | ||
|
|
||
| // @todo: reconstruct the angles from the quaternion instead of the 4x4 matrix (which possibly needs to be updated first). | ||
| static constexpr f32 k_cos0_threshold = 1.0f - 1e-6f; | ||
|
|
||
| UpdateMatrixFromQuaternion(); | ||
|
|
||
| const f32 sin_y = -m_matrix[0][2]; | ||
|
|
||
| m_euler.Y() = std::asin(std::clamp(sin_y, -1.0f, +1.0f)); | ||
|
|
||
| // Guard against gimbal lock when Y=-90°/+90° (X and Z rotate around the same axis). | ||
| if(std::abs(sin_y) <= k_cos0_threshold) { | ||
| m_euler.X() = std::atan2(m_matrix[1][2], m_matrix[2][2]); | ||
| m_euler.Z() = std::atan2(m_matrix[0][1], m_matrix[0][0]); | ||
| } else { | ||
| m_euler.X() = std::atan2(m_matrix[1][0], m_matrix[2][0]); | ||
| m_euler.Z() = 0.0f; | ||
| } | ||
|
|
||
| m_needs_euler_refresh = false; | ||
| } | ||
|
|
||
| Quaternion m_quaternion{}; ///< the underlying rotation encoding the current axis and angle | ||
|
|
||
| mutable Matrix4 m_matrix{}; ///< a 4x4 rotation matrix which is updated from the quaternion on demand. | ||
| mutable Vector3 m_euler{}; ///< a vector of euler angles which is updated from the quaternion on demand. | ||
| mutable bool m_needs_matrix_refresh{true}; ///< true when the 4x4 matrix (#{@link m_matrix}) is outdated and false otherwise. | ||
| mutable bool m_needs_euler_refresh{true}; ///< true whe euler angles (#{@link m_euler}) are outdated and false otherwise. | ||
| mutable VoidEvent m_event_on_change{}; ///< An event that is emitted when the rotation has changed. | ||
| }; | ||
| inline Vector3 quaternion_to_extrinsic_xyz_angles(const Quaternion& quaternion) { | ||
| Vector3 euler; | ||
|
|
||
| const f32 wy = quaternion.W() * quaternion.Y(); | ||
| const f32 wz = quaternion.W() * quaternion.Z(); | ||
| const f32 xz = quaternion.X() * quaternion.Z(); | ||
| const f32 xy = quaternion.X() * quaternion.Y(); | ||
| const f32 yy = quaternion.Y() * quaternion.Y(); | ||
| const f32 zz = quaternion.Z() * quaternion.Z(); | ||
|
|
||
| const f32 m00 = 1 - 2 * (zz + yy); | ||
| const f32 m01 = 2 * (xy + wz); | ||
| const bool gimbal_lock = std::sqrt(m00 * m00 + m01 * m01) < 1e-6; | ||
|
|
||
| euler.Y() = std::asin(std::clamp(-2.0f * (xz - wy), -1.0f, +1.0f)); | ||
|
|
||
| // Guard against gimbal lock when Y=-90°/+90° (X and Z rotate around the same axis). | ||
| if(!gimbal_lock) { | ||
| const f32 wx = quaternion.W() * quaternion.X(); | ||
| const f32 xx = quaternion.X() * quaternion.X(); | ||
| const f32 yz = quaternion.Y() * quaternion.Z(); | ||
|
|
||
| euler.X() = std::atan2(yz + wx, 0.5f - (xx + yy)); | ||
| euler.Z() = std::atan2(xy + wz, 0.5f - (zz + yy)); | ||
| } else { | ||
| euler.X() = std::atan2(xy - wz, xz + wy); | ||
| euler.Z() = 0.0f; | ||
| } | ||
|
|
||
| return euler; | ||
| } | ||
|
|
||
| /** | ||
| * Convert extrinsic x-y-z (intrinsic z-y'-x'') Tait-Bryan angles into an unit quaternion. | ||
| * @param euler A {@link #Vector3} storing the extrinsic x-y-z Tait-Bryan angles | ||
| * @returns a {@link #Quaternion} | ||
| */ | ||
| inline Quaternion extrinsic_xyz_angles_to_quaternion(const Vector3& euler) { | ||
| const f32 half_x = euler.X() * 0.5f; | ||
| const f32 cos_x = std::cos(half_x); | ||
| const f32 sin_x = std::sin(half_x); | ||
|
|
||
| const f32 half_y = euler.Y() * 0.5f; | ||
| const f32 cos_y = std::cos(half_y); | ||
| const f32 sin_y = std::sin(half_y); | ||
|
|
||
| const f32 half_z = euler.Z() * 0.5f; | ||
| const f32 cos_z = std::cos(half_z); | ||
| const f32 sin_z = std::sin(half_z); | ||
|
|
||
| const f32 cos_z_cos_y = cos_z * cos_y; | ||
| const f32 sin_z_cos_y = sin_z * cos_y; | ||
| const f32 sin_z_sin_y = sin_z * sin_y; | ||
| const f32 cos_z_sin_y = cos_z * sin_y; | ||
|
|
||
| return { | ||
| cos_z_cos_y * cos_x + sin_z_sin_y * sin_x, | ||
| cos_z_cos_y * sin_x - sin_z_sin_y * cos_x, | ||
| sin_z_cos_y * sin_x + cos_z_sin_y * cos_x, | ||
| sin_z_cos_y * cos_x - cos_z_sin_y * sin_x | ||
| }; | ||
| } | ||
|
|
||
| } // namespace zephyr |
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters