340 lines
16 KiB
C++
340 lines
16 KiB
C++
#pragma once
|
|
|
|
////////////////////////////////////////////////////////////////////////////////
|
|
// The MIT License (MIT)
|
|
//
|
|
// Copyright (c) 2018 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/vector4f.h"
|
|
#include "rtm/vector4d.h"
|
|
#include "rtm/quatf.h"
|
|
#include "rtm/quatd.h"
|
|
#include "rtm/type_traits.h"
|
|
#include "rtm/impl/compiler_utils.h"
|
|
|
|
RTM_IMPL_FILE_PRAGMA_PUSH
|
|
|
|
namespace rtm
|
|
{
|
|
namespace rtm_impl
|
|
{
|
|
//////////////////////////////////////////////////////////////////////////
|
|
// A helper struct to convert a quaternion to matrices of similar width.
|
|
// Note: We use a float type as a template argument because GCC loses alignment
|
|
// attributes on template argument types.
|
|
//////////////////////////////////////////////////////////////////////////
|
|
template<typename float_type>
|
|
struct matrix_from_quat_helper
|
|
{
|
|
using quat = typename float_traits<float_type>::quat;
|
|
using vector4 = typename float_traits<float_type>::vector4;
|
|
using matrix3x3 = typename float_traits<float_type>::matrix3x3;
|
|
using matrix3x4 = typename float_traits<float_type>::matrix3x4;
|
|
|
|
RTM_DISABLE_SECURITY_COOKIE_CHECK inline RTM_SIMD_CALL operator matrix3x3() const RTM_NO_EXCEPT
|
|
{
|
|
RTM_ASSERT(quat_is_normalized(quat_input), "Quaternion is not normalized");
|
|
|
|
const float_type x2 = quat_get_x(quat_input) + quat_get_x(quat_input);
|
|
const float_type y2 = quat_get_y(quat_input) + quat_get_y(quat_input);
|
|
const float_type z2 = quat_get_z(quat_input) + quat_get_z(quat_input);
|
|
const float_type xx = quat_get_x(quat_input) * x2;
|
|
const float_type xy = quat_get_x(quat_input) * y2;
|
|
const float_type xz = quat_get_x(quat_input) * z2;
|
|
const float_type yy = quat_get_y(quat_input) * y2;
|
|
const float_type yz = quat_get_y(quat_input) * z2;
|
|
const float_type zz = quat_get_z(quat_input) * z2;
|
|
const float_type wx = quat_get_w(quat_input) * x2;
|
|
const float_type wy = quat_get_w(quat_input) * y2;
|
|
const float_type wz = quat_get_w(quat_input) * z2;
|
|
|
|
const vector4 x_axis = vector_set(float_type(1.0) - (yy + zz), xy + wz, xz - wy, float_type(0.0));
|
|
const vector4 y_axis = vector_set(xy - wz, float_type(1.0) - (xx + zz), yz + wx, float_type(0.0));
|
|
const vector4 z_axis = vector_set(xz + wy, yz - wx, float_type(1.0) - (xx + yy), float_type(0.0));
|
|
return matrix3x3{ x_axis, y_axis, z_axis };
|
|
}
|
|
|
|
RTM_DISABLE_SECURITY_COOKIE_CHECK inline RTM_SIMD_CALL operator matrix3x4() const RTM_NO_EXCEPT
|
|
{
|
|
RTM_ASSERT(quat_is_normalized(quat_input), "Quaternion is not normalized");
|
|
|
|
const float_type x2 = quat_get_x(quat_input) + quat_get_x(quat_input);
|
|
const float_type y2 = quat_get_y(quat_input) + quat_get_y(quat_input);
|
|
const float_type z2 = quat_get_z(quat_input) + quat_get_z(quat_input);
|
|
const float_type xx = quat_get_x(quat_input) * x2;
|
|
const float_type xy = quat_get_x(quat_input) * y2;
|
|
const float_type xz = quat_get_x(quat_input) * z2;
|
|
const float_type yy = quat_get_y(quat_input) * y2;
|
|
const float_type yz = quat_get_y(quat_input) * z2;
|
|
const float_type zz = quat_get_z(quat_input) * z2;
|
|
const float_type wx = quat_get_w(quat_input) * x2;
|
|
const float_type wy = quat_get_w(quat_input) * y2;
|
|
const float_type wz = quat_get_w(quat_input) * z2;
|
|
|
|
const vector4 x_axis = vector_set(float_type(1.0) - (yy + zz), xy + wz, xz - wy, float_type(0.0));
|
|
const vector4 y_axis = vector_set(xy - wz, float_type(1.0) - (xx + zz), yz + wx, float_type(0.0));
|
|
const vector4 z_axis = vector_set(xz + wy, yz - wx, float_type(1.0) - (xx + yy), float_type(0.0));
|
|
const vector4 w_axis = vector_zero();
|
|
return matrix3x4{ x_axis, y_axis, z_axis, w_axis };
|
|
}
|
|
|
|
quat quat_input;
|
|
};
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
// A helper struct to convert a 3D scale vector to matrices of similar width.
|
|
// Note: We use a float type as a template argument because GCC loses alignment
|
|
// attributes on template argument types.
|
|
//////////////////////////////////////////////////////////////////////////
|
|
template<typename float_type>
|
|
struct matrix_from_scale_helper
|
|
{
|
|
using vector4 = typename float_traits<float_type>::vector4;
|
|
using matrix3x3 = typename float_traits<float_type>::matrix3x3;
|
|
using matrix3x4 = typename float_traits<float_type>::matrix3x4;
|
|
|
|
RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE RTM_SIMD_CALL operator matrix3x3() const RTM_NO_EXCEPT
|
|
{
|
|
const vector4 zero = vector_zero();
|
|
const vector4 x_axis = vector_mix<mix4::x, mix4::b, mix4::c, mix4::d>(scale, zero);
|
|
const vector4 y_axis = vector_mix<mix4::a, mix4::y, mix4::c, mix4::d>(scale, zero);
|
|
const vector4 z_axis = vector_mix<mix4::a, mix4::b, mix4::z, mix4::d>(scale, zero);
|
|
return matrix3x3{ x_axis, y_axis, z_axis };
|
|
}
|
|
|
|
RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE RTM_SIMD_CALL operator matrix3x4() const RTM_NO_EXCEPT
|
|
{
|
|
const vector4 zero = vector_zero();
|
|
const vector4 x_axis = vector_mix<mix4::x, mix4::b, mix4::c, mix4::d>(scale, zero);
|
|
const vector4 y_axis = vector_mix<mix4::a, mix4::y, mix4::c, mix4::d>(scale, zero);
|
|
const vector4 z_axis = vector_mix<mix4::a, mix4::b, mix4::z, mix4::d>(scale, zero);
|
|
return matrix3x4{ x_axis, y_axis, z_axis, zero };
|
|
}
|
|
|
|
vector4 scale;
|
|
};
|
|
|
|
RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE constexpr vector4f RTM_SIMD_CALL matrix_get_axis(vector4f_arg0 x_axis, vector4f_arg1 y_axis, vector4f_arg2 z_axis, axis3 axis)
|
|
{
|
|
return axis == axis3::x ? x_axis : (axis == axis3::y ? y_axis : z_axis);
|
|
}
|
|
|
|
RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE constexpr const vector4d& RTM_SIMD_CALL matrix_get_axis(const vector4d& x_axis, const vector4d& y_axis, const vector4d& z_axis, axis3 axis)
|
|
{
|
|
return axis == axis3::x ? x_axis : (axis == axis3::y ? y_axis : z_axis);
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
// Converts a 3x3 matrix into a rotation quaternion.
|
|
//////////////////////////////////////////////////////////////////////////
|
|
RTM_DISABLE_SECURITY_COOKIE_CHECK inline quatf RTM_SIMD_CALL quat_from_matrix(vector4f_arg0 x_axis, vector4f_arg1 y_axis, vector4f_arg2 z_axis) RTM_NO_EXCEPT
|
|
{
|
|
const vector4f zero = vector_zero();
|
|
if (vector_all_near_equal3(x_axis, zero) || vector_all_near_equal3(y_axis, zero) || vector_all_near_equal3(z_axis, zero))
|
|
return quat_identity(); // Zero scale not supported, return the identity
|
|
|
|
const float x_axis_x = vector_get_x(x_axis);
|
|
const float y_axis_y = vector_get_y(y_axis);
|
|
const float z_axis_z = vector_get_z(z_axis);
|
|
|
|
const float mtx_trace = x_axis_x + y_axis_y + z_axis_z;
|
|
if (mtx_trace > 0.0F)
|
|
{
|
|
const float x_axis_y = vector_get_y(x_axis);
|
|
const float x_axis_z = vector_get_z(x_axis);
|
|
|
|
const float y_axis_x = vector_get_x(y_axis);
|
|
const float y_axis_z = vector_get_z(y_axis);
|
|
|
|
const float z_axis_x = vector_get_x(z_axis);
|
|
const float z_axis_y = vector_get_y(z_axis);
|
|
|
|
const float inv_trace = scalar_sqrt_reciprocal(mtx_trace + 1.0F);
|
|
const float half_inv_trace = inv_trace * 0.5F;
|
|
|
|
const float x = (y_axis_z - z_axis_y) * half_inv_trace;
|
|
const float y = (z_axis_x - x_axis_z) * half_inv_trace;
|
|
const float z = (x_axis_y - y_axis_x) * half_inv_trace;
|
|
const float w = scalar_reciprocal(inv_trace) * 0.5F;
|
|
|
|
return quat_normalize(quat_set(x, y, z, w));
|
|
}
|
|
else
|
|
{
|
|
// Note that axis3::xyz have the same values as mix4::xyz
|
|
int32_t best_axis = (int32_t)axis4::x;
|
|
if (y_axis_y > x_axis_x)
|
|
best_axis = (int32_t)axis4::y;
|
|
if (z_axis_z > vector_get_component(matrix_get_axis(x_axis, y_axis, z_axis, axis3(best_axis)), mix4(best_axis)))
|
|
best_axis = (int32_t)axis4::z;
|
|
|
|
const int32_t next_best_axis = (best_axis + 1) % 3;
|
|
const int32_t next_next_best_axis = (next_best_axis + 1) % 3;
|
|
|
|
const float mtx_pseudo_trace = 1.0F +
|
|
vector_get_component(matrix_get_axis(x_axis, y_axis, z_axis, axis3(best_axis)), mix4(best_axis)) -
|
|
vector_get_component(matrix_get_axis(x_axis, y_axis, z_axis, axis3(next_best_axis)), mix4(next_best_axis)) -
|
|
vector_get_component(matrix_get_axis(x_axis, y_axis, z_axis, axis3(next_next_best_axis)), mix4(next_next_best_axis));
|
|
|
|
const float inv_pseudo_trace = scalar_sqrt_reciprocal(mtx_pseudo_trace);
|
|
const float half_inv_pseudo_trace = inv_pseudo_trace * 0.5F;
|
|
|
|
float quat_values[4];
|
|
quat_values[best_axis] = scalar_reciprocal(inv_pseudo_trace) * 0.5F;
|
|
quat_values[next_best_axis] = half_inv_pseudo_trace *
|
|
(vector_get_component(matrix_get_axis(x_axis, y_axis, z_axis, axis3(best_axis)), mix4(next_best_axis)) +
|
|
vector_get_component(matrix_get_axis(x_axis, y_axis, z_axis, axis3(next_best_axis)), mix4(best_axis)));
|
|
quat_values[next_next_best_axis] = half_inv_pseudo_trace *
|
|
(vector_get_component(matrix_get_axis(x_axis, y_axis, z_axis, axis3(best_axis)), mix4(next_next_best_axis)) +
|
|
vector_get_component(matrix_get_axis(x_axis, y_axis, z_axis, axis3(next_next_best_axis)), mix4(best_axis)));
|
|
quat_values[3] = half_inv_pseudo_trace *
|
|
(vector_get_component(matrix_get_axis(x_axis, y_axis, z_axis, axis3(next_best_axis)), mix4(next_next_best_axis)) -
|
|
vector_get_component(matrix_get_axis(x_axis, y_axis, z_axis, axis3(next_next_best_axis)), mix4(next_best_axis)));
|
|
|
|
return quat_normalize(quat_load(&quat_values[0]));
|
|
}
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
// Converts a 3x3 matrix into a rotation quaternion.
|
|
//////////////////////////////////////////////////////////////////////////
|
|
RTM_DISABLE_SECURITY_COOKIE_CHECK inline quatd RTM_SIMD_CALL quat_from_matrix(const vector4d& x_axis, const vector4d& y_axis, const vector4d& z_axis) RTM_NO_EXCEPT
|
|
{
|
|
const vector4d zero = vector_zero();
|
|
if (vector_all_near_equal3(x_axis, zero) || vector_all_near_equal3(y_axis, zero) || vector_all_near_equal3(z_axis, zero))
|
|
return quat_identity(); // Zero scale not supported, return the identity
|
|
|
|
const double x_axis_x = vector_get_x(x_axis);
|
|
const double y_axis_y = vector_get_y(y_axis);
|
|
const double z_axis_z = vector_get_z(z_axis);
|
|
|
|
const double mtx_trace = x_axis_x + y_axis_y + z_axis_z;
|
|
if (mtx_trace > 0.0)
|
|
{
|
|
const double x_axis_y = vector_get_y(x_axis);
|
|
const double x_axis_z = vector_get_z(x_axis);
|
|
|
|
const double y_axis_x = vector_get_x(y_axis);
|
|
const double y_axis_z = vector_get_z(y_axis);
|
|
|
|
const double z_axis_x = vector_get_x(z_axis);
|
|
const double z_axis_y = vector_get_y(z_axis);
|
|
|
|
const double inv_trace = scalar_sqrt_reciprocal(mtx_trace + 1.0);
|
|
const double half_inv_trace = inv_trace * 0.5;
|
|
|
|
const double x = (y_axis_z - z_axis_y) * half_inv_trace;
|
|
const double y = (z_axis_x - x_axis_z) * half_inv_trace;
|
|
const double z = (x_axis_y - y_axis_x) * half_inv_trace;
|
|
const double w = scalar_reciprocal(inv_trace) * 0.5;
|
|
|
|
return quat_normalize(quat_set(x, y, z, w));
|
|
}
|
|
else
|
|
{
|
|
// Note that axis3::xyz have the same values as mix4::xyz
|
|
int32_t best_axis = (int32_t)axis3::x;
|
|
if (y_axis_y > x_axis_x)
|
|
best_axis = (int32_t)axis3::y;
|
|
if (z_axis_z > vector_get_component(matrix_get_axis(x_axis, y_axis, z_axis, axis3(best_axis)), mix4(best_axis)))
|
|
best_axis = (int32_t)axis3::z;
|
|
|
|
const int32_t next_best_axis = (best_axis + 1) % 3;
|
|
const int32_t next_next_best_axis = (next_best_axis + 1) % 3;
|
|
|
|
const double mtx_pseudo_trace = 1.0 +
|
|
vector_get_component(matrix_get_axis(x_axis, y_axis, z_axis, axis3(best_axis)), mix4(best_axis)) -
|
|
vector_get_component(matrix_get_axis(x_axis, y_axis, z_axis, axis3(next_best_axis)), mix4(next_best_axis)) -
|
|
vector_get_component(matrix_get_axis(x_axis, y_axis, z_axis, axis3(next_next_best_axis)), mix4(next_next_best_axis));
|
|
|
|
const double inv_pseudo_trace = scalar_sqrt_reciprocal(mtx_pseudo_trace);
|
|
const double half_inv_pseudo_trace = inv_pseudo_trace * 0.5;
|
|
|
|
double quat_values[4];
|
|
quat_values[best_axis] = scalar_reciprocal(inv_pseudo_trace) * 0.5;
|
|
quat_values[next_best_axis] = half_inv_pseudo_trace *
|
|
(vector_get_component(matrix_get_axis(x_axis, y_axis, z_axis, axis3(best_axis)), mix4(next_best_axis)) +
|
|
vector_get_component(matrix_get_axis(x_axis, y_axis, z_axis, axis3(next_best_axis)), mix4(best_axis)));
|
|
quat_values[next_next_best_axis] = half_inv_pseudo_trace *
|
|
(vector_get_component(matrix_get_axis(x_axis, y_axis, z_axis, axis3(best_axis)), mix4(next_next_best_axis)) +
|
|
vector_get_component(matrix_get_axis(x_axis, y_axis, z_axis, axis3(next_next_best_axis)), mix4(best_axis)));
|
|
quat_values[3] = half_inv_pseudo_trace *
|
|
(vector_get_component(matrix_get_axis(x_axis, y_axis, z_axis, axis3(next_best_axis)), mix4(next_next_best_axis)) -
|
|
vector_get_component(matrix_get_axis(x_axis, y_axis, z_axis, axis3(next_next_best_axis)), mix4(next_best_axis)));
|
|
|
|
return quat_normalize(quat_load(&quat_values[0]));
|
|
}
|
|
}
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
// Converts a rotation quaternion into a 3x3 or 3x4 affine matrix.
|
|
//////////////////////////////////////////////////////////////////////////
|
|
RTM_DISABLE_SECURITY_COOKIE_CHECK constexpr rtm_impl::matrix_from_quat_helper<float> RTM_SIMD_CALL matrix_from_quat(quatf_arg0 quat) RTM_NO_EXCEPT
|
|
{
|
|
return rtm_impl::matrix_from_quat_helper<float>{ quat };
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
// Converts a rotation quaternion into a 3x3 or 3x4 affine matrix.
|
|
//////////////////////////////////////////////////////////////////////////
|
|
RTM_DISABLE_SECURITY_COOKIE_CHECK constexpr rtm_impl::matrix_from_quat_helper<double> RTM_SIMD_CALL matrix_from_quat(const quatd& quat) RTM_NO_EXCEPT
|
|
{
|
|
return rtm_impl::matrix_from_quat_helper<double>{ quat };
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
// Converts a rotation quaternion into a 3x3 or 3x4 affine matrix.
|
|
//////////////////////////////////////////////////////////////////////////
|
|
RTM_DISABLE_SECURITY_COOKIE_CHECK constexpr rtm_impl::matrix_from_quat_helper<float> RTM_SIMD_CALL matrix_from_rotation(quatf_arg0 quat) RTM_NO_EXCEPT
|
|
{
|
|
return rtm_impl::matrix_from_quat_helper<float>{ quat };
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
// Converts a rotation quaternion into a 3x3 or 3x4 affine matrix.
|
|
//////////////////////////////////////////////////////////////////////////
|
|
RTM_DISABLE_SECURITY_COOKIE_CHECK constexpr rtm_impl::matrix_from_quat_helper<double> RTM_SIMD_CALL matrix_from_rotation(const quatd& quat) RTM_NO_EXCEPT
|
|
{
|
|
return rtm_impl::matrix_from_quat_helper<double>{ quat };
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
// Converts a 3D scale vector into a 3x3 or 3x4 affine matrix.
|
|
//////////////////////////////////////////////////////////////////////////
|
|
RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE constexpr rtm_impl::matrix_from_scale_helper<float> RTM_SIMD_CALL matrix_from_scale(vector4f_arg0 scale) RTM_NO_EXCEPT
|
|
{
|
|
return rtm_impl::matrix_from_scale_helper<float>{ scale };
|
|
}
|
|
|
|
//////////////////////////////////////////////////////////////////////////
|
|
// Converts a 3D scale vector into a 3x3 or 3x4 affine matrix.
|
|
//////////////////////////////////////////////////////////////////////////
|
|
RTM_DISABLE_SECURITY_COOKIE_CHECK RTM_FORCE_INLINE constexpr rtm_impl::matrix_from_scale_helper<double> RTM_SIMD_CALL matrix_from_scale(const vector4d& scale) RTM_NO_EXCEPT
|
|
{
|
|
return rtm_impl::matrix_from_scale_helper<double>{ scale };
|
|
}
|
|
}
|
|
|
|
RTM_IMPL_FILE_PRAGMA_POP
|