From 8d13ae8175cfcaa7fe7e672ced8ba17081c14ede Mon Sep 17 00:00:00 2001 From: Nicholas Frechette Date: Wed, 13 Nov 2024 22:17:21 -0500 Subject: [PATCH] feat(vqm): implement VQM group --- includes/rtm/experimental/impl/vqm_common.h | 85 +++++ includes/rtm/experimental/types.h | 60 ++++ includes/rtm/experimental/vqmd.h | 370 +++++++++++++++++++ includes/rtm/experimental/vqmf.h | 370 +++++++++++++++++++ tests/sources/test_vqm.cpp | 373 ++++++++++++++++++++ 5 files changed, 1258 insertions(+) create mode 100644 includes/rtm/experimental/impl/vqm_common.h create mode 100644 includes/rtm/experimental/types.h create mode 100644 includes/rtm/experimental/vqmd.h create mode 100644 includes/rtm/experimental/vqmf.h create mode 100644 tests/sources/test_vqm.cpp diff --git a/includes/rtm/experimental/impl/vqm_common.h b/includes/rtm/experimental/impl/vqm_common.h new file mode 100644 index 0000000..9447e60 --- /dev/null +++ b/includes/rtm/experimental/impl/vqm_common.h @@ -0,0 +1,85 @@ +#pragma once + +//////////////////////////////////////////////////////////////////////////////// +// The MIT License (MIT) +// +// Copyright (c) 2024 Nicholas Frechette & Realtime Math contributors +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +//////////////////////////////////////////////////////////////////////////////// + +#include "rtm/math.h" +#include "rtm/version.h" +#include "rtm/experimental/types.h" +#include "rtm/impl/compiler_utils.h" + +RTM_IMPL_FILE_PRAGMA_PUSH + +namespace rtm +{ + RTM_IMPL_VERSION_NAMESPACE_BEGIN + + namespace rtm_impl + { + ////////////////////////////////////////////////////////////////////////// + // This is a helper struct to allow a single consistent API between + // various VQM transform types when the semantics are identical but the return + // type differs. Implicit coercion is used to return the desired value + // at the call site. + ////////////////////////////////////////////////////////////////////////// + struct vqm_identity_impl + { + RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE RTM_SIMD_CALL operator vqmd() const RTM_NO_EXCEPT + { + vqmd result; + result.rotation = quat_identity(); + result.translation = vector_zero(); + result.x_axis = vector_set(1.0, 0.0, 0.0, 0.0); + result.y_axis = vector_set(0.0, 1.0, 0.0, 0.0); + result.z_axis = vector_set(0.0, 0.0, 1.0, 0.0); + + return result; + } + + RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE RTM_SIMD_CALL operator vqmf() const RTM_NO_EXCEPT + { + vqmf result; + result.rotation = quat_identity(); + result.translation = vector_zero(); + result.x_axis = vector_set(1.0F, 0.0F, 0.0F, 0.0F); + result.y_axis = vector_set(0.0F, 1.0F, 0.0F, 0.0F); + result.z_axis = vector_set(0.0F, 0.0F, 1.0F, 0.0F); + + return result; + } + }; + } + + ////////////////////////////////////////////////////////////////////////// + // Returns the identity VQM transform. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE constexpr rtm_impl::vqm_identity_impl RTM_SIMD_CALL vqm_identity() RTM_NO_EXCEPT + { + return rtm_impl::vqm_identity_impl(); + } + + RTM_IMPL_VERSION_NAMESPACE_END +} + +RTM_IMPL_FILE_PRAGMA_POP diff --git a/includes/rtm/experimental/types.h b/includes/rtm/experimental/types.h new file mode 100644 index 0000000..c58a2b6 --- /dev/null +++ b/includes/rtm/experimental/types.h @@ -0,0 +1,60 @@ +#pragma once + +//////////////////////////////////////////////////////////////////////////////// +// The MIT License (MIT) +// +// Copyright (c) 2024 Nicholas Frechette & Realtime Math contributors +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +//////////////////////////////////////////////////////////////////////////////// + +#include "rtm/math.h" +#include "rtm/version.h" +#include "rtm/impl/compiler_utils.h" + +RTM_IMPL_FILE_PRAGMA_PUSH + +namespace rtm +{ + RTM_IMPL_VERSION_NAMESPACE_BEGIN + + struct vqmd + { + // The internal format is meant to be opaque, use vqm_* accessors + quatd rotation; + vector4d translation; + vector4d x_axis; + vector4d y_axis; + vector4d z_axis; + }; + + struct vqmf + { + // The internal format is meant to be opaque, use vqm_* accessors + quatf rotation; + vector4f translation; + vector4f x_axis; + vector4f y_axis; + vector4f z_axis; + }; + + RTM_IMPL_VERSION_NAMESPACE_END +} + +RTM_IMPL_FILE_PRAGMA_POP diff --git a/includes/rtm/experimental/vqmd.h b/includes/rtm/experimental/vqmd.h new file mode 100644 index 0000000..16584ed --- /dev/null +++ b/includes/rtm/experimental/vqmd.h @@ -0,0 +1,370 @@ +#pragma once + +//////////////////////////////////////////////////////////////////////////////// +// The MIT License (MIT) +// +// Copyright (c) 2024 Nicholas Frechette & Realtime Math contributors +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +//////////////////////////////////////////////////////////////////////////////// + +#include "rtm/math.h" +#include "rtm/matrix3x3d.h" +#include "rtm/quatd.h" +#include "rtm/vector4d.h" +#include "rtm/version.h" +#include "rtm/impl/compiler_utils.h" +#include "rtm/experimental/impl/vqm_common.h" + +RTM_IMPL_FILE_PRAGMA_PUSH + +namespace rtm +{ + RTM_IMPL_VERSION_NAMESPACE_BEGIN + + ////////////////////////////////////////////////////////////////////////// + // See here for details: The VQM-Group and its Applications + // By Michael Aristidou and Xin Li + // + // Key insights: + // - We wish to maintain rotation, translation, and scale/shear separately as they do not + // interpolate the same way. In particular, scale/shear and rotation mix together in the + // upper 3x3 part of affine matrices and it is difficult to manage them correctly as there + // is no unique way to decompose them. + // - Let us define [T1, R1, S1] and [T2, R2, S2] as two transforms with 3 affine matrices each + // We will ignore translation since it mostly lives in its own dimension (4th) and does not + // interfere with scale/shear/rotation. + // - We define multiplication as follow: + // (R2 * S2) * (R1 * S1) = (R3 * S3) + // By construction, we wish R3 = R2 * R1, we substitute + // (R2 * S2) * (R1 * S1) = (R2 * R1 * S3) + // We solve for S3, which is the scale/shear matrix we are looking for + // (R2^-1 * R2) * S2 * (R1 * S1) = R1 * S3 + // R1^-1 * (R2^-1 * R2) * S2 * (R1 * S1) = S3 + // R2^-1 * R2 cancel out and we get + // R1^-1 * S2 * R1 * S1 = S3 + // In plain english, to compute our scale/shear matrix, we rotate S1 into the space of S2 + // by multiplying with R1, then we scale/shear the result, and return into the space of S1 + // by applying the inverse R1 rotation. A sensible result. + // - This is all well and good with matrices, but we wish to retain rotation as a quaternion + // for its numerical stability, compact nature, and superior interpolation. How do we multiply + // a matrix with a quaternion? + // - A key insight is that if we apply a rotation matrix onto any other affine matrix (e.g. a + // scale/shear or other pure rotation matrix), what occurs under the matrix multiplication is + // that each column of the affine matrix is rotated by our rotation matrix. This is something + // we can easily achieve as well with a quaternion: by using the sandwich product. From this + // key insight, the various identities in the paper follow as matrix multiplication with a pure + // rotation matrix is equivalent to the sandwich product of each column of the other matrix. + // + // Some VQM identities: + // - If we treat M as a homogenous quaternion matrix, q a quaternion, and r a pure quaternion, + // then: q * (M * r) * q^-1 = (q * M * q^-1) * r + // In plain english, applying scale/shear to a point and rotating that point is equivalent to + // rotating the scale/shear matrix and applying the result to that point. + // + // - If M and N are homogenous quaternion matrices, and q1 and q2 quaternions then: + // (q2 * N * q2^-1) * (q1 * M * q1^-1) = q2 * (N * (q1 * M * q1^-1)) * q2^-1 + // In plain english, the product of two rotated scale/shear matrices is equivalent to the + // rotated product of one scale/shear matrix with another rotated scale/shear matrix meaning + // rotation can occur before the multiplication or after due to assossiativity. This is + // straightforward to see if the rotations are expressed in matrix form. + ////////////////////////////////////////////////////////////////////////// + + ////////////////////////////////////////////////////////////////////////// + // A VQM transform represents a 3D rotation (quaternion), 3D translation (vector3), + // and 3D non-uniform scale and shear (matrix 3x3). + // VQM forms a group with a well defined multiplication and inverse. Its + // multiplication is assossiative but not commutative (like quaternions/matrices). + // Rotations are assumed to represent a single turn (normalized quaternion). + ////////////////////////////////////////////////////////////////////////// +#if 0 // defined in types.h + struct vqmd + { + // The internal format is meant to be opaque, use vqm_* accessors + quatd rotation; + vector4d translation; + vector4d x_axis; + vector4d y_axis; + vector4d z_axis; + }; +#endif + + template<> struct related_types : related_types {}; + + ////////////////////////////////////////////////////////////////////////// + // Creates a VQM transform from a rotation quaternion, a translation, and a 3D scale. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE vqmd RTM_SIMD_CALL vqm_set(vector4d_arg0 translation, quatd_arg1 rotation, vector4d_arg2 scale) RTM_NO_EXCEPT + { + vector4d zero = vector_zero(); + + vqmd result; + result.rotation = rotation; + result.translation = translation; + result.x_axis = vector_set_x(zero, vector_get_x(scale)); + result.y_axis = vector_set_y(zero, vector_get_y(scale)); + result.z_axis = vector_set_z(zero, vector_get_z(scale)); + + return result; + } + + ////////////////////////////////////////////////////////////////////////// + // Returns the rotation part of a VQM transform. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE quatd RTM_SIMD_CALL vqm_get_rotation(const vqmd& input) RTM_NO_EXCEPT + { + return input.rotation; + } + + ////////////////////////////////////////////////////////////////////////// + // Sets the rotation part of a VQM and returns the new value. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE vqmd RTM_SIMD_CALL vqm_set_rotation(const vqmd& qvm, quatd_arg0 rotation) RTM_NO_EXCEPT + { + vqmd result = qvm; + result.rotation = rotation; + return result; + } + + ////////////////////////////////////////////////////////////////////////// + // Returns the translation part of a VQM transform. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE vector4d RTM_SIMD_CALL vqm_get_translation(const vqmd& input) RTM_NO_EXCEPT + { + return input.translation; + } + + ////////////////////////////////////////////////////////////////////////// + // Sets the translation part of a VQM and returns the new value. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE vqmd RTM_SIMD_CALL vqm_set_translation(const vqmd& qvm, vector4d_arg0 translation) RTM_NO_EXCEPT + { + vqmd result = qvm; + result.translation = translation; + return result; + } + + ////////////////////////////////////////////////////////////////////////// + // Returns the scale part of a VQM transform. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE vector4d RTM_SIMD_CALL vqm_get_scale(const vqmd& input) RTM_NO_EXCEPT + { + vector4d xyxy = vector_mix(input.x_axis, input.y_axis); + return vector_mix(xyxy, input.z_axis); + } + + ////////////////////////////////////////////////////////////////////////// + // Sets the scale part of a VQM and returns the new value. + // This preserves existing shear. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE vqmd RTM_SIMD_CALL vqm_set_scale(const vqmd& qvm, vector4d_arg0 scale) RTM_NO_EXCEPT + { + vqmd result = qvm; + result.x_axis = vector_set_x(qvm.x_axis, vector_get_x(scale)); + result.y_axis = vector_set_y(qvm.y_axis, vector_get_y(scale)); + result.z_axis = vector_set_z(qvm.z_axis, vector_get_z(scale)); + return result; + } + + ////////////////////////////////////////////////////////////////////////// + // Adds two VQM transforms. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK inline vqmd RTM_SIMD_CALL vqm_add(const vqmd& lhs, const vqmd& rhs) RTM_NO_EXCEPT + { + // T2 + T1 = [v2, q2, M2] + [v1, q1, M1] = [v2 + v1, q2 + q1, M2 + M1] + vqmd result; + result.rotation = quat_add(lhs.rotation, rhs.rotation); + result.translation = vector_add(lhs.translation, rhs.translation); + result.x_axis = vector_add(lhs.x_axis, rhs.x_axis); + result.y_axis = vector_add(lhs.y_axis, rhs.y_axis); + result.z_axis = vector_add(lhs.z_axis, rhs.z_axis); + + return result; + } + + ////////////////////////////////////////////////////////////////////////// + // Multiplies two VQM transforms. + // Multiplication order is as follow: local_to_world = vqm_mul(local_to_object, object_to_world) + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK inline vqmd RTM_SIMD_CALL vqm_mul(const vqmd& lhs, const vqmd& rhs) RTM_NO_EXCEPT + { + // T2 * T1 = [v2, q2, M2] * [v1, q1, M1] = [q2 * (M2 * v1) * q2^-1 + v2, q2 * q1, (q1^-1 * M2 * q1)(q1 * M1 * q1^-1)] + + const quatd inv_lhs_rotation = quat_conjugate(lhs.rotation); + + matrix3x3d lhs_scale_shear = matrix_set(lhs.x_axis, lhs.y_axis, lhs.z_axis); + matrix3x3d rhs_scale_shear = matrix_set(rhs.x_axis, rhs.y_axis, rhs.z_axis); + + vqmd result; + result.rotation = quat_mul(lhs.rotation, rhs.rotation); + result.translation = vector_add(quat_mul_vector3(matrix_mul_vector3(lhs.translation, rhs_scale_shear), rhs.rotation), rhs.translation); + + rhs_scale_shear.x_axis = quat_mul_vector3(rhs_scale_shear.x_axis, inv_lhs_rotation); + rhs_scale_shear.y_axis = quat_mul_vector3(rhs_scale_shear.y_axis, inv_lhs_rotation); + rhs_scale_shear.z_axis = quat_mul_vector3(rhs_scale_shear.z_axis, inv_lhs_rotation); + + lhs_scale_shear.x_axis = quat_mul_vector3(lhs_scale_shear.x_axis, lhs.rotation); + lhs_scale_shear.y_axis = quat_mul_vector3(lhs_scale_shear.y_axis, lhs.rotation); + lhs_scale_shear.z_axis = quat_mul_vector3(lhs_scale_shear.z_axis, lhs.rotation); + + matrix3x3d scale_shear = matrix_mul(rhs_scale_shear, lhs_scale_shear); + result.x_axis = scale_shear.x_axis; + result.y_axis = scale_shear.y_axis; + result.z_axis = scale_shear.z_axis; + + return result; + } + + ////////////////////////////////////////////////////////////////////////// + // Multiplies a VQM transform with a scalar. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE vqmd RTM_SIMD_CALL vqm_mul(const vqmd& vqm, double scalar) RTM_NO_EXCEPT + { + // s * T = s * [v, q, M] = [s * v, s * q, s * M] + vector4d scalar_v = vector_set(scalar); + + vqmd result; + result.rotation = quat_mul(vqm.rotation, scalar); + result.translation = vector_mul(vqm.translation, scalar_v); + result.x_axis = vector_mul(vqm.x_axis, scalar_v); + result.y_axis = vector_mul(vqm.y_axis, scalar_v); + result.z_axis = vector_mul(vqm.z_axis, scalar_v); + + return result; + } + + ////////////////////////////////////////////////////////////////////////// + // Multiplies a VQM transform and a 3D point. + // Multiplication order is as follow: world_position = vqm_mul_point3(local_position, local_to_world) + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK inline vector4d RTM_SIMD_CALL vqm_mul_point3(vector4d_arg0 point, const vqmd& vqm) RTM_NO_EXCEPT + { + // T * p = [v, q, M] * p = (q * (M * p) * q^-1) + v + + const matrix3x3d scale_shear = matrix_set(vqm.x_axis, vqm.y_axis, vqm.z_axis); + return vector_add(quat_mul_vector3(matrix_mul_vector3(point, scale_shear), vqm.rotation), vqm.translation); + } + + ////////////////////////////////////////////////////////////////////////// + // Multiplies a VQM transform and a 3D vector. + // Multiplication order is as follow: world_position = vqm_mul_point3(local_position, local_to_world) + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK inline vector4d RTM_SIMD_CALL vqm_mul_vector3(vector4d_arg0 vec3, const vqmd& vqm) RTM_NO_EXCEPT + { + // T * vec3 = [v, q, M] * p = (q * (M * vec3) * q^-1) + + const matrix3x3d scale_shear = matrix_set(vqm.x_axis, vqm.y_axis, vqm.z_axis); + return quat_mul_vector3(matrix_mul_vector3(vec3, scale_shear), vqm.rotation); + } + + ////////////////////////////////////////////////////////////////////////// + // Returns the inverse of the input VQM transform. + // If zero scale is contained, the result is undefined. + // For a safe alternative, supply a fallback scale value and a threshold. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK inline vqmd RTM_SIMD_CALL vqm_inverse(const vqmd& input) RTM_NO_EXCEPT + { + // T^-1 = [v, q, M]^-1 = [M^-1 * (q^-1 * -v * q), q^-1, q * (q * M * q^-1)^-1 * q^-1] + // Note that (q * M * q^-1)^-1 != (q^-1 * M^-1 * q) + // However, let us convert that last part into matrix representation + // q * (q * M * q^-1)^-1 * q^-1 = Mq * (Mq * M)^-1 + // Mq * (Mq * M)^-1 = (Mq * M^-1) * Mq^-1 + // (Mq * M^-1) * Mq^-1 = (q * M^-1 * q^-1) * Mq^-1 + // Unfortunately, we cannot convert the remaining Mq^-1 matrix back into a quaternion product + // because it does not rotate anything (multiplication is on left side instead of right) + // However, we can solve this by introducing the identity matrix + // (q * M^-1 * q^-1) * (Mq^-1 * I) = (q * M^-1 * q^-1) * (q^-1 * I * q) + // This is better because it allows us to compute a single matrix inverse as opposed to two + + const matrix3x3d scale_shear = matrix_set(input.x_axis, input.y_axis, input.z_axis); + + const matrix3x3d inv_scale_shear = matrix_inverse(scale_shear); + const quatd inv_rotation = quat_conjugate(input.rotation); + + // Rotate the inverse scale/shear matrix + matrix3x3d inv_rotated_scale_shear; + inv_rotated_scale_shear.x_axis = quat_mul_vector3(inv_scale_shear.x_axis, input.rotation); + inv_rotated_scale_shear.y_axis = quat_mul_vector3(inv_scale_shear.y_axis, input.rotation); + inv_rotated_scale_shear.z_axis = quat_mul_vector3(inv_scale_shear.z_axis, input.rotation); + + // Build our inverse rotation matrix + // TODO: We could build the matrix directly from the quaternion which is cheaper than rotating 3 axes, need to profile + matrix3x3d inv_rotation_mtx = matrix_identity(); + inv_rotation_mtx.x_axis = quat_mul_vector3(inv_rotation_mtx.x_axis, inv_rotation); + inv_rotation_mtx.y_axis = quat_mul_vector3(inv_rotation_mtx.y_axis, inv_rotation); + inv_rotation_mtx.z_axis = quat_mul_vector3(inv_rotation_mtx.z_axis, inv_rotation); + + // Multiply our two matrices + inv_rotated_scale_shear = matrix_mul(inv_rotation_mtx, inv_rotated_scale_shear); + + vqmd result; + result.rotation = inv_rotation; + result.x_axis = inv_rotated_scale_shear.x_axis; + result.y_axis = inv_rotated_scale_shear.y_axis; + result.z_axis = inv_rotated_scale_shear.z_axis; + result.translation = matrix_mul_vector3(quat_mul_vector3(vector_neg(input.translation), inv_rotation), inv_scale_shear); + + return result; + } + + ////////////////////////////////////////////////////////////////////////// + // Converts a VQM transform into a 3x4 affine matrix. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK inline matrix3x4d RTM_SIMD_CALL vqm_to_matrix(const vqmd& input) RTM_NO_EXCEPT + { + matrix3x4d result = matrix_set(input.x_axis, input.y_axis, input.z_axis, input.translation); + + result.x_axis = quat_mul_vector3(result.x_axis, input.rotation); + result.y_axis = quat_mul_vector3(result.y_axis, input.rotation); + result.z_axis = quat_mul_vector3(result.z_axis, input.rotation); + + return result; + } + + ////////////////////////////////////////////////////////////////////////// + // Returns a VQM transforms with the rotation part normalized. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE vqmd RTM_SIMD_CALL qvv_normalize(const vqmd& input) RTM_NO_EXCEPT + { + vqmd result; + result.rotation = quat_normalize(input.rotation); + result.x_axis = input.x_axis; + result.y_axis = input.y_axis; + result.z_axis = input.z_axis; + result.translation = input.translation; + + return result; + } + + ////////////////////////////////////////////////////////////////////////// + // Returns true if the input VQM does not contain any NaN or Inf, otherwise false. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE bool RTM_SIMD_CALL qvv_is_finite(const vqmd& input) RTM_NO_EXCEPT + { + return quat_is_finite(input.rotation) + && vector_is_finite3(input.translation) + && vector_is_finite3(input.x_axis) + && vector_is_finite3(input.y_axis) + && vector_is_finite3(input.z_axis); + } + + RTM_IMPL_VERSION_NAMESPACE_END +} + +RTM_IMPL_FILE_PRAGMA_POP diff --git a/includes/rtm/experimental/vqmf.h b/includes/rtm/experimental/vqmf.h new file mode 100644 index 0000000..f4dff3f --- /dev/null +++ b/includes/rtm/experimental/vqmf.h @@ -0,0 +1,370 @@ +#pragma once + +//////////////////////////////////////////////////////////////////////////////// +// The MIT License (MIT) +// +// Copyright (c) 2024 Nicholas Frechette & Realtime Math contributors +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +//////////////////////////////////////////////////////////////////////////////// + +#include "rtm/math.h" +#include "rtm/matrix3x3f.h" +#include "rtm/quatf.h" +#include "rtm/vector4f.h" +#include "rtm/version.h" +#include "rtm/impl/compiler_utils.h" +#include "rtm/experimental/impl/vqm_common.h" + +RTM_IMPL_FILE_PRAGMA_PUSH + +namespace rtm +{ + RTM_IMPL_VERSION_NAMESPACE_BEGIN + + ////////////////////////////////////////////////////////////////////////// + // See here for details: The VQM-Group and its Applications + // By Michael Aristidou and Xin Li + // + // Key insights: + // - We wish to maintain rotation, translation, and scale/shear separately as they do not + // interpolate the same way. In particular, scale/shear and rotation mix together in the + // upper 3x3 part of affine matrices and it is difficult to manage them correctly as there + // is no unique way to decompose them. + // - Let us define [T1, R1, S1] and [T2, R2, S2] as two transforms with 3 affine matrices each + // We will ignore translation since it mostly lives in its own dimension (4th) and does not + // interfere with scale/shear/rotation. + // - We define multiplication as follow: + // (R2 * S2) * (R1 * S1) = (R3 * S3) + // By construction, we wish R3 = R2 * R1, we substitute + // (R2 * S2) * (R1 * S1) = (R2 * R1 * S3) + // We solve for S3, which is the scale/shear matrix we are looking for + // (R2^-1 * R2) * S2 * (R1 * S1) = R1 * S3 + // R1^-1 * (R2^-1 * R2) * S2 * (R1 * S1) = S3 + // R2^-1 * R2 cancel out and we get + // R1^-1 * S2 * R1 * S1 = S3 + // In plain english, to compute our scale/shear matrix, we rotate S1 into the space of S2 + // by multiplying with R1, then we scale/shear the result, and return into the space of S1 + // by applying the inverse R1 rotation. A sensible result. + // - This is all well and good with matrices, but we wish to retain rotation as a quaternion + // for its numerical stability, compact nature, and superior interpolation. How do we multiply + // a matrix with a quaternion? + // - A key insight is that if we apply a rotation matrix onto any other affine matrix (e.g. a + // scale/shear or other pure rotation matrix), what occurs under the matrix multiplication is + // that each column of the affine matrix is rotated by our rotation matrix. This is something + // we can easily achieve as well with a quaternion: by using the sandwich product. From this + // key insight, the various identities in the paper follow as matrix multiplication with a pure + // rotation matrix is equivalent to the sandwich product of each column of the other matrix. + // + // Some VQM identities: + // - If we treat M as a homogenous quaternion matrix, q a quaternion, and r a pure quaternion, + // then: q * (M * r) * q^-1 = (q * M * q^-1) * r + // In plain english, applying scale/shear to a point and rotating that point is equivalent to + // rotating the scale/shear matrix and applying the result to that point. + // + // - If M and N are homogenous quaternion matrices, and q1 and q2 quaternions then: + // (q2 * N * q2^-1) * (q1 * M * q1^-1) = q2 * (N * (q1 * M * q1^-1)) * q2^-1 + // In plain english, the product of two rotated scale/shear matrices is equivalent to the + // rotated product of one scale/shear matrix with another rotated scale/shear matrix meaning + // rotation can occur before the multiplication or after due to assossiativity. This is + // straightforward to see if the rotations are expressed in matrix form. + ////////////////////////////////////////////////////////////////////////// + + ////////////////////////////////////////////////////////////////////////// + // A VQM transform represents a 3D rotation (quaternion), 3D translation (vector3), + // and 3D non-uniform scale and shear (matrix 3x3). + // VQM forms a group with a well defined multiplication and inverse. Its + // multiplication is assossiative but not commutative (like quaternions/matrices). + // Rotations are assumed to represent a single turn (normalized quaternion). + ////////////////////////////////////////////////////////////////////////// +#if 0 // defined in types.h + struct vqmf + { + // The internal format is meant to be opaque, use vqm_* accessors + quatf rotation; + vector4f translation; + vector4f x_axis; + vector4f y_axis; + vector4f z_axis; + }; +#endif + + template<> struct related_types : related_types {}; + + ////////////////////////////////////////////////////////////////////////// + // Creates a VQM transform from a rotation quaternion, a translation, and a 3D scale. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE vqmf RTM_SIMD_CALL vqm_set(vector4f_arg0 translation, quatf_arg1 rotation, vector4f_arg2 scale) RTM_NO_EXCEPT + { + vector4f zero = vector_zero(); + + vqmf result; + result.rotation = rotation; + result.translation = translation; + result.x_axis = vector_set_x(zero, vector_get_x(scale)); + result.y_axis = vector_set_y(zero, vector_get_y(scale)); + result.z_axis = vector_set_z(zero, vector_get_z(scale)); + + return result; + } + + ////////////////////////////////////////////////////////////////////////// + // Returns the rotation part of a VQM transform. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE quatf RTM_SIMD_CALL vqm_get_rotation(const vqmf& input) RTM_NO_EXCEPT + { + return input.rotation; + } + + ////////////////////////////////////////////////////////////////////////// + // Sets the rotation part of a VQM and returns the new value. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE vqmf RTM_SIMD_CALL vqm_set_rotation(const vqmf& qvm, quatf_arg0 rotation) RTM_NO_EXCEPT + { + vqmf result = qvm; + result.rotation = rotation; + return result; + } + + ////////////////////////////////////////////////////////////////////////// + // Returns the translation part of a VQM transform. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE vector4f RTM_SIMD_CALL vqm_get_translation(const vqmf& input) RTM_NO_EXCEPT + { + return input.translation; + } + + ////////////////////////////////////////////////////////////////////////// + // Sets the translation part of a VQM and returns the new value. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE vqmf RTM_SIMD_CALL vqm_set_translation(const vqmf& qvm, vector4f_arg0 translation) RTM_NO_EXCEPT + { + vqmf result = qvm; + result.translation = translation; + return result; + } + + ////////////////////////////////////////////////////////////////////////// + // Returns the scale part of a VQM transform. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE vector4f RTM_SIMD_CALL vqm_get_scale(const vqmf& input) RTM_NO_EXCEPT + { + vector4f xyxy = vector_mix(input.x_axis, input.y_axis); + return vector_mix(xyxy, input.z_axis); + } + + ////////////////////////////////////////////////////////////////////////// + // Sets the scale part of a VQM and returns the new value. + // This preserves existing shear. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE vqmf RTM_SIMD_CALL vqm_set_scale(const vqmf& qvm, vector4f_arg0 scale) RTM_NO_EXCEPT + { + vqmf result = qvm; + result.x_axis = vector_set_x(qvm.x_axis, vector_get_x(scale)); + result.y_axis = vector_set_y(qvm.y_axis, vector_get_y(scale)); + result.z_axis = vector_set_z(qvm.z_axis, vector_get_z(scale)); + return result; + } + + ////////////////////////////////////////////////////////////////////////// + // Adds two VQM transforms. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE vqmf RTM_SIMD_CALL vqm_add(const vqmf& lhs, const vqmf& rhs) RTM_NO_EXCEPT + { + // T2 + T1 = [v2, q2, M2] + [v1, q1, M1] = [v2 + v1, q2 + q1, M2 + M1] + vqmf result; + result.rotation = quat_add(lhs.rotation, rhs.rotation); + result.translation = vector_add(lhs.translation, rhs.translation); + result.x_axis = vector_add(lhs.x_axis, rhs.x_axis); + result.y_axis = vector_add(lhs.y_axis, rhs.y_axis); + result.z_axis = vector_add(lhs.z_axis, rhs.z_axis); + + return result; + } + + ////////////////////////////////////////////////////////////////////////// + // Multiplies two VQM transforms. + // Multiplication order is as follow: local_to_world = vqm_mul(local_to_object, object_to_world) + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK inline vqmf RTM_SIMD_CALL vqm_mul(const vqmf& lhs, const vqmf& rhs) RTM_NO_EXCEPT + { + // T2 * T1 = [v2, q2, M2] * [v1, q1, M1] = [q2 * (M2 * v1) * q2^-1 + v2, q2 * q1, (q1^-1 * M2 * q1)(q1 * M1 * q1^-1)] + + const quatf inv_lhs_rotation = quat_conjugate(lhs.rotation); + + matrix3x3f lhs_scale_shear = matrix_set(lhs.x_axis, lhs.y_axis, lhs.z_axis); + matrix3x3f rhs_scale_shear = matrix_set(rhs.x_axis, rhs.y_axis, rhs.z_axis); + + vqmf result; + result.rotation = quat_mul(lhs.rotation, rhs.rotation); + result.translation = vector_add(quat_mul_vector3(matrix_mul_vector3(lhs.translation, rhs_scale_shear), rhs.rotation), rhs.translation); + + rhs_scale_shear.x_axis = quat_mul_vector3(rhs_scale_shear.x_axis, inv_lhs_rotation); + rhs_scale_shear.y_axis = quat_mul_vector3(rhs_scale_shear.y_axis, inv_lhs_rotation); + rhs_scale_shear.z_axis = quat_mul_vector3(rhs_scale_shear.z_axis, inv_lhs_rotation); + + lhs_scale_shear.x_axis = quat_mul_vector3(lhs_scale_shear.x_axis, lhs.rotation); + lhs_scale_shear.y_axis = quat_mul_vector3(lhs_scale_shear.y_axis, lhs.rotation); + lhs_scale_shear.z_axis = quat_mul_vector3(lhs_scale_shear.z_axis, lhs.rotation); + + matrix3x3f scale_shear = matrix_mul(rhs_scale_shear, lhs_scale_shear); + result.x_axis = scale_shear.x_axis; + result.y_axis = scale_shear.y_axis; + result.z_axis = scale_shear.z_axis; + + return result; + } + + ////////////////////////////////////////////////////////////////////////// + // Multiplies a VQM transform with a scalar. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE vqmf RTM_SIMD_CALL vqm_mul(const vqmf& vqm, float scalar) RTM_NO_EXCEPT + { + // s * T = s * [v, q, M] = [s * v, s * q, s * M] + vector4f scalar_v = vector_set(scalar); + + vqmf result; + result.rotation = quat_mul(vqm.rotation, scalar); + result.translation = vector_mul(vqm.translation, scalar_v); + result.x_axis = vector_mul(vqm.x_axis, scalar_v); + result.y_axis = vector_mul(vqm.y_axis, scalar_v); + result.z_axis = vector_mul(vqm.z_axis, scalar_v); + + return result; + } + + ////////////////////////////////////////////////////////////////////////// + // Multiplies a VQM transform and a 3D point. + // Multiplication order is as follow: world_position = vqm_mul_point3(local_position, local_to_world) + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK inline vector4f RTM_SIMD_CALL vqm_mul_point3(vector4f_arg0 point, const vqmf& vqm) RTM_NO_EXCEPT + { + // T * p = [v, q, M] * p = (q * (M * p) * q^-1) + v + + const matrix3x3f scale_shear = matrix_set(vqm.x_axis, vqm.y_axis, vqm.z_axis); + return vector_add(quat_mul_vector3(matrix_mul_vector3(point, scale_shear), vqm.rotation), vqm.translation); + } + + ////////////////////////////////////////////////////////////////////////// + // Multiplies a VQM transform and a 3D vector. + // Multiplication order is as follow: world_position = vqm_mul_point3(local_position, local_to_world) + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK inline vector4f RTM_SIMD_CALL vqm_mul_vector3(vector4f_arg0 vec3, const vqmf& vqm) RTM_NO_EXCEPT + { + // T * vec3 = [v, q, M] * p = (q * (M * vec3) * q^-1) + + const matrix3x3f scale_shear = matrix_set(vqm.x_axis, vqm.y_axis, vqm.z_axis); + return quat_mul_vector3(matrix_mul_vector3(vec3, scale_shear), vqm.rotation); + } + + ////////////////////////////////////////////////////////////////////////// + // Returns the inverse of the input VQM transform. + // If zero scale is contained, the result is undefined. + // For a safe alternative, supply a fallback scale value and a threshold. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK inline vqmf RTM_SIMD_CALL vqm_inverse(const vqmf& input) RTM_NO_EXCEPT + { + // T^-1 = [v, q, M]^-1 = [M^-1 * (q^-1 * -v * q), q^-1, q * (q * M * q^-1)^-1 * q^-1] + // Note that (q * M * q^-1)^-1 != (q^-1 * M^-1 * q) + // However, let us convert that last part into matrix representation + // q * (q * M * q^-1)^-1 * q^-1 = Mq * (Mq * M)^-1 + // Mq * (Mq * M)^-1 = (Mq * M^-1) * Mq^-1 + // (Mq * M^-1) * Mq^-1 = (q * M^-1 * q^-1) * Mq^-1 + // Unfortunately, we cannot convert the remaining Mq^-1 matrix back into a quaternion product + // because it does not rotate anything (multiplication is on left side instead of right) + // However, we can solve this by introducing the identity matrix + // (q * M^-1 * q^-1) * (Mq^-1 * I) = (q * M^-1 * q^-1) * (q^-1 * I * q) + // This is better because it allows us to compute a single matrix inverse as opposed to two + + const matrix3x3f scale_shear = matrix_set(input.x_axis, input.y_axis, input.z_axis); + + const matrix3x3f inv_scale_shear = matrix_inverse(scale_shear); + const quatf inv_rotation = quat_conjugate(input.rotation); + + // Rotate the inverse scale/shear matrix + matrix3x3f inv_rotated_scale_shear; + inv_rotated_scale_shear.x_axis = quat_mul_vector3(inv_scale_shear.x_axis, input.rotation); + inv_rotated_scale_shear.y_axis = quat_mul_vector3(inv_scale_shear.y_axis, input.rotation); + inv_rotated_scale_shear.z_axis = quat_mul_vector3(inv_scale_shear.z_axis, input.rotation); + + // Build our inverse rotation matrix + // TODO: We could build the matrix directly from the quaternion which is cheaper than rotating 3 axes, need to profile + matrix3x3f inv_rotation_mtx = matrix_identity(); + inv_rotation_mtx.x_axis = quat_mul_vector3(inv_rotation_mtx.x_axis, inv_rotation); + inv_rotation_mtx.y_axis = quat_mul_vector3(inv_rotation_mtx.y_axis, inv_rotation); + inv_rotation_mtx.z_axis = quat_mul_vector3(inv_rotation_mtx.z_axis, inv_rotation); + + // Multiply our two matrices + inv_rotated_scale_shear = matrix_mul(inv_rotation_mtx, inv_rotated_scale_shear); + + vqmf result; + result.rotation = inv_rotation; + result.x_axis = inv_rotated_scale_shear.x_axis; + result.y_axis = inv_rotated_scale_shear.y_axis; + result.z_axis = inv_rotated_scale_shear.z_axis; + result.translation = matrix_mul_vector3(quat_mul_vector3(vector_neg(input.translation), inv_rotation), inv_scale_shear); + + return result; + } + + ////////////////////////////////////////////////////////////////////////// + // Converts a VQM transform into a 3x4 affine matrix. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK inline matrix3x4f RTM_SIMD_CALL vqm_to_matrix(const vqmf& input) RTM_NO_EXCEPT + { + matrix3x4f result = matrix_set(input.x_axis, input.y_axis, input.z_axis, input.translation); + + result.x_axis = quat_mul_vector3(result.x_axis, input.rotation); + result.y_axis = quat_mul_vector3(result.y_axis, input.rotation); + result.z_axis = quat_mul_vector3(result.z_axis, input.rotation); + + return result; + } + + ////////////////////////////////////////////////////////////////////////// + // Returns a VQM transforms with the rotation part normalized. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE vqmf RTM_SIMD_CALL qvv_normalize(const vqmf& input) RTM_NO_EXCEPT + { + vqmf result; + result.rotation = quat_normalize(input.rotation); + result.x_axis = input.x_axis; + result.y_axis = input.y_axis; + result.z_axis = input.z_axis; + result.translation = input.translation; + + return result; + } + + ////////////////////////////////////////////////////////////////////////// + // Returns true if the input VQM does not contain any NaN or Inf, otherwise false. + ////////////////////////////////////////////////////////////////////////// + RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE bool RTM_SIMD_CALL qvv_is_finite(const vqmf& input) RTM_NO_EXCEPT + { + return quat_is_finite(input.rotation) + && vector_is_finite3(input.translation) + && vector_is_finite3(input.x_axis) + && vector_is_finite3(input.y_axis) + && vector_is_finite3(input.z_axis); + } + + RTM_IMPL_VERSION_NAMESPACE_END +} + +RTM_IMPL_FILE_PRAGMA_POP diff --git a/tests/sources/test_vqm.cpp b/tests/sources/test_vqm.cpp new file mode 100644 index 0000000..5742efb --- /dev/null +++ b/tests/sources/test_vqm.cpp @@ -0,0 +1,373 @@ +//////////////////////////////////////////////////////////////////////////////// +// The MIT License (MIT) +// +// Copyright (c) 2023 Nicholas Frechette & Realtime Math contributors +// +// Permission is hereby granted, free of charge, to any person obtaining a copy +// of this software and associated documentation files (the "Software"), to deal +// in the Software without restriction, including without limitation the rights +// to use, copy, modify, merge, publish, distribute, sublicense, and/or sell +// copies of the Software, and to permit persons to whom the Software is +// furnished to do so, subject to the following conditions: +// +// The above copyright notice and this permission notice shall be included in all +// copies or substantial portions of the Software. +// +// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR +// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, +// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE +// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER +// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, +// OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE +// SOFTWARE. +//////////////////////////////////////////////////////////////////////////////// + +#include "catch2.impl.h" + +#include +#include +#include +#include +#include + +using namespace rtm; + +template +static void test_vqm_impl(const FloatType threshold) +{ + using QuatType = typename related_types::quat; + using Vector4Type = typename related_types::vector4; + using Matrix3x4Type = typename related_types::matrix3x4; + + // Identity validation + { + Vector4Type point = vector_set(FloatType(12.0), FloatType(0.0), FloatType(-130.033)); + + TransformType identity = vqm_identity(); + + Vector4Type mul_point_result = vqm_mul_point3(point, identity); + CHECK(vector_all_near_equal3(mul_point_result, point, threshold)); + + TransformType mul_itself_result = vqm_mul(identity, identity); + CHECK(quat_near_equal(mul_itself_result.rotation, identity.rotation, threshold)); + CHECK(vector_all_near_equal3(mul_itself_result.x_axis, identity.x_axis, threshold)); + CHECK(vector_all_near_equal3(mul_itself_result.y_axis, identity.y_axis, threshold)); + CHECK(vector_all_near_equal3(mul_itself_result.z_axis, identity.z_axis, threshold)); + CHECK(vector_all_near_equal3(mul_itself_result.translation, identity.translation, threshold)); + + TransformType inverse_result = vqm_inverse(identity); + CHECK(quat_near_equal(inverse_result.rotation, identity.rotation, threshold)); + CHECK(vector_all_near_equal3(inverse_result.x_axis, identity.x_axis, threshold)); + CHECK(vector_all_near_equal3(inverse_result.y_axis, identity.y_axis, threshold)); + CHECK(vector_all_near_equal3(inverse_result.z_axis, identity.z_axis, threshold)); + CHECK(vector_all_near_equal3(inverse_result.translation, identity.translation, threshold)); + } + + // Getters and setters + { + QuatType rotation_around_z = quat_from_euler(scalar_deg_to_rad(FloatType(0.0)), scalar_deg_to_rad(FloatType(90.0)), scalar_deg_to_rad(FloatType(0.0))); + Vector4Type translation = vector_set(FloatType(1.0), FloatType(2.0), FloatType(3.0)); + Vector4Type scale = vector_set(FloatType(4.0), FloatType(5.0), FloatType(6.0)); + + TransformType identity = vqm_identity(); + TransformType tx = vqm_set_rotation(identity, rotation_around_z); + CHECK(quat_near_equal(vqm_get_rotation(tx), rotation_around_z, threshold)); + CHECK(vector_all_near_equal3(tx.x_axis, identity.x_axis, threshold)); + CHECK(vector_all_near_equal3(tx.y_axis, identity.y_axis, threshold)); + CHECK(vector_all_near_equal3(tx.z_axis, identity.z_axis, threshold)); + CHECK(vector_all_near_equal3(tx.translation, identity.translation, threshold)); + + tx = vqm_set_translation(tx, translation); + CHECK(quat_near_equal(vqm_get_rotation(tx), rotation_around_z, threshold)); + CHECK(vector_all_near_equal3(tx.x_axis, identity.x_axis, threshold)); + CHECK(vector_all_near_equal3(tx.y_axis, identity.y_axis, threshold)); + CHECK(vector_all_near_equal3(tx.z_axis, identity.z_axis, threshold)); + CHECK(vector_all_near_equal3(vqm_get_translation(tx), translation, threshold)); + + tx = vqm_set_scale(tx, scale); + CHECK(quat_near_equal(vqm_get_rotation(tx), rotation_around_z, threshold)); + CHECK(vector_all_near_equal3(vqm_get_scale(tx), scale, threshold)); + CHECK(vector_all_near_equal3(vqm_get_translation(tx), translation, threshold)); + } + + // Matrix conversion validation + { + QuatType rotation_around_z = quat_from_euler(scalar_deg_to_rad(FloatType(0.0)), scalar_deg_to_rad(FloatType(90.0)), scalar_deg_to_rad(FloatType(0.0))); + Vector4Type translation = vector_set(FloatType(1.0), FloatType(2.0), FloatType(3.0)); + Vector4Type scale = vector_set(FloatType(4.0), FloatType(5.0), FloatType(6.0)); + + Matrix3x4Type src_mtx = matrix_from_qvv(rotation_around_z, translation, scale); + TransformType dst_tx = vqm_set(translation, rotation_around_z, scale); + Matrix3x4Type dst_mtx = vqm_to_matrix(dst_tx); + CHECK(vector_all_near_equal3(src_mtx.x_axis, dst_mtx.x_axis, threshold)); + CHECK(vector_all_near_equal3(src_mtx.y_axis, dst_mtx.y_axis, threshold)); + CHECK(vector_all_near_equal3(src_mtx.z_axis, dst_mtx.z_axis, threshold)); + CHECK(vector_all_near_equal3(src_mtx.w_axis, dst_mtx.w_axis, threshold)); + } + + // VQM + VQM validation + { + // TODO + } + + // VQM * VQM validation + { + QuatType rotation_around_z = quat_from_euler(scalar_deg_to_rad(FloatType(0.0)), scalar_deg_to_rad(FloatType(90.0)), scalar_deg_to_rad(FloatType(0.0))); + Vector4Type translation = vector_set(FloatType(1.0), FloatType(2.0), FloatType(3.0)); + + // All positive scale + Vector4Type scale = vector_set(FloatType(4.0), FloatType(5.0), FloatType(6.0)); + + Matrix3x4Type src_mtx = matrix_from_qvv(rotation_around_z, translation, scale); + src_mtx = matrix_mul(src_mtx, src_mtx); + + TransformType dst_tx = vqm_set(translation, rotation_around_z, scale); + dst_tx = vqm_mul(dst_tx, dst_tx); + Matrix3x4Type dst_mtx = vqm_to_matrix(dst_tx); + CHECK(vector_all_near_equal3(src_mtx.x_axis, dst_mtx.x_axis, threshold)); + CHECK(vector_all_near_equal3(src_mtx.y_axis, dst_mtx.y_axis, threshold)); + CHECK(vector_all_near_equal3(src_mtx.z_axis, dst_mtx.z_axis, threshold)); + CHECK(vector_all_near_equal3(src_mtx.w_axis, dst_mtx.w_axis, threshold)); + + // One negative scale + scale = vector_set(FloatType(-4.0), FloatType(5.0), FloatType(6.0)); + + src_mtx = matrix_from_qvv(rotation_around_z, translation, scale); + src_mtx = matrix_mul(src_mtx, src_mtx); + + dst_tx = vqm_set(translation, rotation_around_z, scale); + dst_tx = vqm_mul(dst_tx, dst_tx); + dst_mtx = vqm_to_matrix(dst_tx); + CHECK(vector_all_near_equal3(src_mtx.x_axis, dst_mtx.x_axis, threshold)); + CHECK(vector_all_near_equal3(src_mtx.y_axis, dst_mtx.y_axis, threshold)); + CHECK(vector_all_near_equal3(src_mtx.z_axis, dst_mtx.z_axis, threshold)); + CHECK(vector_all_near_equal3(src_mtx.w_axis, dst_mtx.w_axis, threshold)); + + // Two negative scale + scale = vector_set(FloatType(-4.0), FloatType(-5.0), FloatType(6.0)); + + src_mtx = matrix_from_qvv(rotation_around_z, translation, scale); + src_mtx = matrix_mul(src_mtx, src_mtx); + + dst_tx = vqm_set(translation, rotation_around_z, scale); + dst_tx = vqm_mul(dst_tx, dst_tx); + dst_mtx = vqm_to_matrix(dst_tx); + CHECK(vector_all_near_equal3(src_mtx.x_axis, dst_mtx.x_axis, threshold)); + CHECK(vector_all_near_equal3(src_mtx.y_axis, dst_mtx.y_axis, threshold)); + CHECK(vector_all_near_equal3(src_mtx.z_axis, dst_mtx.z_axis, threshold)); + CHECK(vector_all_near_equal3(src_mtx.w_axis, dst_mtx.w_axis, threshold)); + + // Three negative scale + scale = vector_set(FloatType(-4.0), FloatType(-5.0), FloatType(-6.0)); + + src_mtx = matrix_from_qvv(rotation_around_z, translation, scale); + src_mtx = matrix_mul(src_mtx, src_mtx); + + dst_tx = vqm_set(translation, rotation_around_z, scale); + dst_tx = vqm_mul(dst_tx, dst_tx); + dst_mtx = vqm_to_matrix(dst_tx); + CHECK(vector_all_near_equal3(src_mtx.x_axis, dst_mtx.x_axis, threshold)); + CHECK(vector_all_near_equal3(src_mtx.y_axis, dst_mtx.y_axis, threshold)); + CHECK(vector_all_near_equal3(src_mtx.z_axis, dst_mtx.z_axis, threshold)); + CHECK(vector_all_near_equal3(src_mtx.w_axis, dst_mtx.w_axis, threshold)); + + // One zero scale + scale = vector_set(FloatType(0.0), FloatType(5.0), FloatType(6.0)); + + src_mtx = matrix_from_qvv(rotation_around_z, translation, scale); + src_mtx = matrix_mul(src_mtx, src_mtx); + + dst_tx = vqm_set(translation, rotation_around_z, scale); + dst_tx = vqm_mul(dst_tx, dst_tx); + dst_mtx = vqm_to_matrix(dst_tx); + CHECK(vector_all_near_equal3(src_mtx.x_axis, dst_mtx.x_axis, threshold)); + CHECK(vector_all_near_equal3(src_mtx.y_axis, dst_mtx.y_axis, threshold)); + CHECK(vector_all_near_equal3(src_mtx.z_axis, dst_mtx.z_axis, threshold)); + CHECK(vector_all_near_equal3(src_mtx.w_axis, dst_mtx.w_axis, threshold)); + + // Two zero scale + scale = vector_set(FloatType(0.0), FloatType(0.0), FloatType(6.0)); + + src_mtx = matrix_from_qvv(rotation_around_z, translation, scale); + src_mtx = matrix_mul(src_mtx, src_mtx); + + dst_tx = vqm_set(translation, rotation_around_z, scale); + dst_tx = vqm_mul(dst_tx, dst_tx); + dst_mtx = vqm_to_matrix(dst_tx); + CHECK(vector_all_near_equal3(src_mtx.x_axis, dst_mtx.x_axis, threshold)); + CHECK(vector_all_near_equal3(src_mtx.y_axis, dst_mtx.y_axis, threshold)); + CHECK(vector_all_near_equal3(src_mtx.z_axis, dst_mtx.z_axis, threshold)); + CHECK(vector_all_near_equal3(src_mtx.w_axis, dst_mtx.w_axis, threshold)); + + // Three zero scale + scale = vector_set(FloatType(0.0), FloatType(0.0), FloatType(0.0)); + + src_mtx = matrix_from_qvv(rotation_around_z, translation, scale); + src_mtx = matrix_mul(src_mtx, src_mtx); + + dst_tx = vqm_set(translation, rotation_around_z, scale); + dst_tx = vqm_mul(dst_tx, dst_tx); + dst_mtx = vqm_to_matrix(dst_tx); + CHECK(vector_all_near_equal3(src_mtx.x_axis, dst_mtx.x_axis, threshold)); + CHECK(vector_all_near_equal3(src_mtx.y_axis, dst_mtx.y_axis, threshold)); + CHECK(vector_all_near_equal3(src_mtx.z_axis, dst_mtx.z_axis, threshold)); + CHECK(vector_all_near_equal3(src_mtx.w_axis, dst_mtx.w_axis, threshold)); + } + + // VQM * scalar validation + { + // TODO + } + + // point/vec3 * VQM validation + { + Vector4Type point = vector_set(FloatType(12.0), FloatType(0.0), FloatType(-130.033)); + + QuatType rotation_around_z = quat_from_euler(scalar_deg_to_rad(FloatType(0.0)), scalar_deg_to_rad(FloatType(90.0)), scalar_deg_to_rad(FloatType(0.0))); + Vector4Type translation = vector_set(FloatType(1.0), FloatType(2.0), FloatType(3.0)); + + // All positive scale + Vector4Type scale = vector_set(FloatType(4.0), FloatType(5.0), FloatType(6.0)); + + Matrix3x4Type src_mtx = matrix_from_qvv(rotation_around_z, translation, scale); + Vector4Type src_point = matrix_mul_point3(point, src_mtx); + + TransformType dst_tx = vqm_set(translation, rotation_around_z, scale); + Vector4Type dst_point = vqm_mul_point3(point, dst_tx); + CHECK(vector_all_near_equal3(src_point, dst_point, threshold)); + + src_point = matrix_mul_vector3(point, src_mtx); + dst_point = vqm_mul_vector3(point, dst_tx); + CHECK(vector_all_near_equal3(src_point, dst_point, threshold)); + + // One negative scale + scale = vector_set(FloatType(-4.0), FloatType(5.0), FloatType(6.0)); + + src_mtx = matrix_from_qvv(rotation_around_z, translation, scale); + src_point = matrix_mul_point3(point, src_mtx); + + dst_tx = vqm_set(translation, rotation_around_z, scale); + dst_point = vqm_mul_point3(point, dst_tx); + CHECK(vector_all_near_equal3(src_point, dst_point, threshold)); + + src_point = matrix_mul_vector3(point, src_mtx); + dst_point = vqm_mul_vector3(point, dst_tx); + CHECK(vector_all_near_equal3(src_point, dst_point, threshold)); + + // Two negative scale + scale = vector_set(FloatType(-4.0), FloatType(-5.0), FloatType(6.0)); + + src_mtx = matrix_from_qvv(rotation_around_z, translation, scale); + src_point = matrix_mul_point3(point, src_mtx); + + dst_tx = vqm_set(translation, rotation_around_z, scale); + dst_point = vqm_mul_point3(point, dst_tx); + CHECK(vector_all_near_equal3(src_point, dst_point, threshold)); + + src_point = matrix_mul_vector3(point, src_mtx); + dst_point = vqm_mul_vector3(point, dst_tx); + CHECK(vector_all_near_equal3(src_point, dst_point, threshold)); + + // Three negative scale + scale = vector_set(FloatType(-4.0), FloatType(-5.0), FloatType(-6.0)); + + src_mtx = matrix_from_qvv(rotation_around_z, translation, scale); + src_point = matrix_mul_point3(point, src_mtx); + + dst_tx = vqm_set(translation, rotation_around_z, scale); + dst_point = vqm_mul_point3(point, dst_tx); + CHECK(vector_all_near_equal3(src_point, dst_point, threshold)); + + src_point = matrix_mul_vector3(point, src_mtx); + dst_point = vqm_mul_vector3(point, dst_tx); + CHECK(vector_all_near_equal3(src_point, dst_point, threshold)); + + // One zero scale + scale = vector_set(FloatType(0.0), FloatType(5.0), FloatType(6.0)); + + src_mtx = matrix_from_qvv(rotation_around_z, translation, scale); + src_point = matrix_mul_point3(point, src_mtx); + + dst_tx = vqm_set(translation, rotation_around_z, scale); + dst_point = vqm_mul_point3(point, dst_tx); + CHECK(vector_all_near_equal3(src_point, dst_point, threshold)); + + src_point = matrix_mul_vector3(point, src_mtx); + dst_point = vqm_mul_vector3(point, dst_tx); + CHECK(vector_all_near_equal3(src_point, dst_point, threshold)); + + // Two zero scale + scale = vector_set(FloatType(0.0), FloatType(0.0), FloatType(6.0)); + + src_mtx = matrix_from_qvv(rotation_around_z, translation, scale); + src_point = matrix_mul_point3(point, src_mtx); + + dst_tx = vqm_set(translation, rotation_around_z, scale); + dst_point = vqm_mul_point3(point, dst_tx); + CHECK(vector_all_near_equal3(src_point, dst_point, threshold)); + + src_point = matrix_mul_vector3(point, src_mtx); + dst_point = vqm_mul_vector3(point, dst_tx); + CHECK(vector_all_near_equal3(src_point, dst_point, threshold)); + + // Three zero scale + scale = vector_set(FloatType(0.0), FloatType(0.0), FloatType(0.0)); + + src_mtx = matrix_from_qvv(rotation_around_z, translation, scale); + src_point = matrix_mul_point3(point, src_mtx); + + dst_tx = vqm_set(translation, rotation_around_z, scale); + dst_point = vqm_mul_point3(point, dst_tx); + CHECK(vector_all_near_equal3(src_point, dst_point, threshold)); + + src_point = matrix_mul_vector3(point, src_mtx); + dst_point = vqm_mul_vector3(point, dst_tx); + CHECK(vector_all_near_equal3(src_point, dst_point, threshold)); + } + + // VQM inverse validation + { + QuatType rotation_around_z = quat_from_euler(scalar_deg_to_rad(FloatType(0.0)), scalar_deg_to_rad(FloatType(90.0)), scalar_deg_to_rad(FloatType(0.0))); + Vector4Type translation = vector_set(FloatType(1.0), FloatType(2.0), FloatType(3.0)); + Vector4Type scale = vector_set(FloatType(4.0), FloatType(5.0), FloatType(6.0)); + + Matrix3x4Type src_mtx = matrix_from_qvv(rotation_around_z, translation, scale); + Matrix3x4Type inv_src_mtx = matrix_inverse(src_mtx); + + TransformType dst_tx = vqm_set(translation, rotation_around_z, scale); + TransformType inv_dst_tx = vqm_inverse(dst_tx); + + Matrix3x4Type inv_dst_mtx = vqm_to_matrix(inv_dst_tx); + CHECK(vector_all_near_equal3(inv_src_mtx.x_axis, inv_dst_mtx.x_axis, threshold)); + CHECK(vector_all_near_equal3(inv_src_mtx.y_axis, inv_dst_mtx.y_axis, threshold)); + CHECK(vector_all_near_equal3(inv_src_mtx.z_axis, inv_dst_mtx.z_axis, threshold)); + CHECK(vector_all_near_equal3(inv_src_mtx.w_axis, inv_dst_mtx.w_axis, threshold)); + + // T * T^-1 = identity + TransformType identity = vqm_identity(); + TransformType inverse_mul_result = vqm_mul(dst_tx, vqm_inverse(dst_tx)); + CHECK(quat_near_equal(inverse_mul_result.rotation, identity.rotation, threshold)); + CHECK(vector_all_near_equal3(inverse_mul_result.x_axis, identity.x_axis, threshold)); + CHECK(vector_all_near_equal3(inverse_mul_result.y_axis, identity.y_axis, threshold)); + CHECK(vector_all_near_equal3(inverse_mul_result.z_axis, identity.z_axis, threshold)); + CHECK(vector_all_near_equal3(inverse_mul_result.translation, identity.translation, threshold)); + + // T^-1 * T = identity + inverse_mul_result = vqm_mul(vqm_inverse(dst_tx), dst_tx); + CHECK(quat_near_equal(inverse_mul_result.rotation, identity.rotation, threshold)); + CHECK(vector_all_near_equal3(inverse_mul_result.x_axis, identity.x_axis, threshold)); + CHECK(vector_all_near_equal3(inverse_mul_result.y_axis, identity.y_axis, threshold)); + CHECK(vector_all_near_equal3(inverse_mul_result.z_axis, identity.z_axis, threshold)); + CHECK(vector_all_near_equal3(inverse_mul_result.translation, identity.translation, threshold)); + } +} + +TEST_CASE("vqmf math", "[math][vqm]") +{ + test_vqm_impl(1.0E-4F); +} + +TEST_CASE("vqmd math", "[math][vqm]") +{ + test_vqm_impl(1.0E-8); +}