438 lines
16 KiB
C++
438 lines
16 KiB
C++
/**
|
|
This file is a part of our_dick
|
|
Copyright (C) 2020 rexy712
|
|
|
|
This program is free software: you can redistribute it and/or modify
|
|
it under the terms of the GNU Affero General Public License as published by
|
|
the Free Software Foundation, either version 3 of the License, or
|
|
(at your option) any later version.
|
|
|
|
This program is distributed in the hope that it will be useful,
|
|
but WITHOUT ANY WARRANTY; without even the implied warranty of
|
|
MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
|
|
GNU Affero General Public License for more details.
|
|
|
|
You should have received a copy of the GNU Affero General Public License
|
|
along with this program. If not, see <http://www.gnu.org/licenses/>.
|
|
*/
|
|
|
|
#ifndef REXY_QUAT_TPP
|
|
#define REXY_QUAT_TPP
|
|
|
|
#include <cmath> //sin, cos, tan, sqrt, etc
|
|
#include "mat.hpp"
|
|
#include "vec.hpp"
|
|
|
|
namespace math{
|
|
|
|
template<Scalar T>
|
|
constexpr quaternion<T>::quaternion(void):
|
|
quaternion(id_initialize){}
|
|
template<Scalar T>
|
|
constexpr quaternion<T>::quaternion(detail::zero_initialize_t):
|
|
m_data{0, 0, 0, 0}{}
|
|
template<Scalar T>
|
|
constexpr quaternion<T>::quaternion(detail::id_initialize_t):
|
|
m_data{1, 0, 0, 0}{}
|
|
template<Scalar T>
|
|
constexpr quaternion<T>::quaternion(detail::no_initialize_t){}
|
|
|
|
template<Scalar T>
|
|
constexpr quaternion<T>::quaternion(detail::manual_initialize_t,
|
|
value_type w, value_type x,
|
|
value_type y, value_type z):
|
|
m_data{w, x, y, z}{}
|
|
|
|
template<Scalar T>
|
|
quaternion<T>::quaternion(const mat3<T>& rotmat){
|
|
auto tr = rotmat.trace();
|
|
if(tr > 0){
|
|
auto f = 0.5 / sqrt(tr + 1.0);
|
|
m_data[0] = 0.25 / f;
|
|
m_data[1] = (rotmat.get(5) - rotmat.get(7)) * f;
|
|
m_data[2] = (rotmat.get(6) - rotmat.get(2)) * f;
|
|
m_data[3] = (rotmat.get(1) - rotmat.get(3)) * f;
|
|
}else if((rotmat.get(0) > rotmat.get(4)) && (rotmat.get(0) > rotmat.get(8))){
|
|
auto f = sqrt(1.0 + rotmat.get(0) - rotmat.get(4) - rotmat.get(8)) * 2.0;
|
|
m_data[0] = (rotmat.get(5) - rotmat.get(7)) / f;
|
|
m_data[1] = f * 0.25;
|
|
m_data[2] = (rotmat.get(3) + rotmat.get(1)) / f;
|
|
m_data[3] = (rotmat.get(6) + rotmat.get(2)) / f;
|
|
}else if(rotmat.get(4) > rotmat.get(8)){
|
|
auto f = sqrt(1.0 + rotmat.get(4) - rotmat.get(0) - rotmat.get(8)) * 2.0;
|
|
m_data[0] = (rotmat.get(6) - rotmat.get(2)) / f;
|
|
m_data[1] = (rotmat.get(3) + rotmat.get(1)) / f;
|
|
m_data[2] = f * 0.25;
|
|
m_data[3] = (rotmat.get(7) + rotmat.get(5)) / f;
|
|
}else{
|
|
auto f = sqrt(1.0 + rotmat.get(8) - rotmat.get(0) - rotmat.get(4)) * 2.0;
|
|
m_data[0] = (rotmat.get(1) - rotmat.get(3)) / f;
|
|
m_data[1] = (rotmat.get(6) + rotmat.get(2)) / f;
|
|
m_data[2] = (rotmat.get(7) + rotmat.get(5)) / f;
|
|
m_data[3] = f * 0.25;
|
|
}
|
|
}
|
|
template<Scalar T>
|
|
quaternion<T>::quaternion(const mat4<T>& rotmat):
|
|
quaternion(mat3<T>{rotmat.get(0), rotmat.get(1), rotmat.get(2),
|
|
rotmat.get(4), rotmat.get(5), rotmat.get(6),
|
|
rotmat.get(8), rotmat.get(9), rotmat.get(10)}){}
|
|
template<Scalar T>
|
|
quaternion<T>::quaternion(value_type bank, value_type heading, value_type attitude){
|
|
bank /= value_type{2};
|
|
heading /= value_type{2};
|
|
attitude /= value_type{2};
|
|
value_type cos_heading = std::cos(heading);
|
|
value_type sin_heading = std::sin(heading);
|
|
value_type cos_attitude = std::cos(attitude);
|
|
value_type sin_attitude = std::sin(attitude);
|
|
value_type cos_bank = std::cos(bank);
|
|
value_type sin_bank = std::sin(bank);
|
|
|
|
m_data[0] = (cos_heading * cos_attitude * cos_bank) - (sin_heading * sin_attitude * sin_bank);
|
|
m_data[1] = (sin_heading * sin_attitude * cos_bank) + (cos_heading * cos_attitude * sin_bank);
|
|
m_data[2] = (sin_heading * cos_attitude * cos_bank) + (cos_heading * sin_attitude * sin_bank);
|
|
m_data[3] = (cos_heading * sin_attitude * cos_bank) - (sin_heading * cos_attitude * sin_bank);
|
|
}
|
|
template<Scalar T>
|
|
quaternion<T>::quaternion(const vec3<T>& angles):
|
|
quaternion(angles.x(), angles.y(), angles.z()){}
|
|
template<Scalar T>
|
|
quaternion<T>::quaternion(value_type angle, const vec3<value_type>& axis):
|
|
quaternion(angle, axis.get_x(), axis.get_y(), axis.get_z()){}
|
|
template<Scalar T>
|
|
quaternion<T>::quaternion(value_type angle, value_type x, value_type y, value_type z){
|
|
angle /= value_type{2.0};
|
|
value_type sin_angle = std::sin(angle);
|
|
m_data[0] = std::cos(angle);
|
|
m_data[1] = sin_angle * x;
|
|
m_data[2] = sin_angle * y;
|
|
m_data[3] = sin_angle * z;
|
|
}
|
|
|
|
template<Scalar T>
|
|
constexpr quaternion<T>::operator pointer(void){
|
|
return m_data;
|
|
}
|
|
template<Scalar T>
|
|
constexpr quaternion<T>::operator const_pointer(void)const{
|
|
return m_data;
|
|
}
|
|
template<Scalar T>
|
|
constexpr auto quaternion<T>::operator[](size_type i) -> reference{
|
|
return m_data[i];
|
|
}
|
|
template<Scalar T>
|
|
constexpr auto quaternion<T>::operator[](size_type i)const -> const_reference{
|
|
return m_data[i];
|
|
}
|
|
template<Scalar T>
|
|
constexpr auto quaternion<T>::get(size_type i) -> reference{
|
|
return m_data[i];
|
|
}
|
|
template<Scalar T>
|
|
constexpr auto quaternion<T>::get(size_type i)const -> const_reference{
|
|
return m_data[i];
|
|
}
|
|
template<Scalar T>
|
|
constexpr auto quaternion<T>::w(void) -> reference{
|
|
return m_data[0];
|
|
}
|
|
template<Scalar T>
|
|
constexpr auto quaternion<T>::w(void)const -> const_reference{
|
|
return m_data[0];
|
|
}
|
|
template<Scalar T>
|
|
constexpr auto quaternion<T>::x(void) -> reference{
|
|
return m_data[1];
|
|
}
|
|
template<Scalar T>
|
|
constexpr auto quaternion<T>::x(void)const -> const_reference{
|
|
return m_data[1];
|
|
}
|
|
template<Scalar T>
|
|
constexpr auto quaternion<T>::y(void) -> reference{
|
|
return m_data[2];
|
|
}
|
|
template<Scalar T>
|
|
constexpr auto quaternion<T>::y(void)const -> const_reference{
|
|
return m_data[2];
|
|
}
|
|
template<Scalar T>
|
|
constexpr auto quaternion<T>::z(void) -> reference{
|
|
return m_data[3];
|
|
}
|
|
template<Scalar T>
|
|
constexpr auto quaternion<T>::z(void)const -> const_reference{
|
|
return m_data[3];
|
|
}
|
|
template<Scalar T>
|
|
void quaternion<T>::set_axis(value_type x, value_type y, value_type z){
|
|
value_type sin_angle = std::sin(std::acos(m_data[0]));
|
|
m_data[1] = sin_angle * x;
|
|
m_data[2] = sin_angle * y;
|
|
m_data[3] = sin_angle * z;
|
|
}
|
|
template<Scalar T>
|
|
void quaternion<T>::set_axis(const vec3<value_type>& v){
|
|
set_axis(v.x(), v.y(), v.z());
|
|
}
|
|
template<Scalar T>
|
|
auto quaternion<T>::get_axis(void)const -> vec3<value_type>{
|
|
quaternion tmp(*this);
|
|
if(m_data[0] > value_type{1.0})
|
|
tmp.set_normalized();
|
|
value_type s = std::sqrt(1 - tmp.m_data[0] * tmp.m_data[0]);
|
|
if(s <= value_type{0.001})
|
|
return vec3<T>(1, 0, 0);
|
|
return vec3<T>(tmp.data[1] / s, tmp.data[2] / s, tmp.data[3] / s);
|
|
}
|
|
template<Scalar T>
|
|
void quaternion<T>::set_angle(value_type t){
|
|
t /= value_type{2.0};
|
|
value_type old_sin_angle = std::sin(std::acos(m_data[0]));
|
|
value_type sin_angle = std::sin(t);
|
|
m_data[0] = std::cos(t);
|
|
m_data[1] = (m_data[1] / old_sin_angle) * sin_angle;
|
|
m_data[2] = (m_data[2] / old_sin_angle) * sin_angle;
|
|
m_data[3] = (m_data[3] / old_sin_angle) * sin_angle;
|
|
}
|
|
template<Scalar T>
|
|
auto quaternion<T>::get_angle(void)const -> value_type{
|
|
return 2.0 * std::acos(m_data[0]);
|
|
}
|
|
template<Scalar T>
|
|
auto quaternion<T>::norm(void)const -> value_type{
|
|
return m_data[0] * m_data[0] + m_data[1] * m_data[1] + m_data[2] * m_data[2] + m_data[3] * m_data[3];
|
|
}
|
|
template<Scalar T>
|
|
quaternion<T> quaternion<T>::conjugate(void)const{
|
|
return quaternion(manual_initialize, m_data[0], -m_data[1], -m_data[2], -m_data[3]);
|
|
}
|
|
template<Scalar T>
|
|
quaternion<T> quaternion<T>::inverse(void)const{
|
|
return conjugate() / norm();
|
|
}
|
|
template<Scalar T>
|
|
auto quaternion<T>::magnitude(void)const -> value_type{
|
|
return std::sqrt(norm());
|
|
}
|
|
template<Scalar T>
|
|
quaternion<T> quaternion<T>::normalize(void)const{
|
|
value_type mag = magnitude();
|
|
return quaternion(manual_initialize, m_data[0] / mag, m_data[1] / mag, m_data[2] / mag, m_data[3] / mag);
|
|
}
|
|
template<Scalar T>
|
|
auto quaternion<T>::get_right(void)const -> vec3<value_type>{
|
|
return vec3<value_type>(1 - 2 * ((m_data[2] * m_data[2]) + (m_data[3] * m_data[3])),
|
|
2 * ((m_data[1] * m_data[2]) - (m_data[3] * m_data[0])),
|
|
2 * ((m_data[1] * m_data[3]) + (m_data[2] * m_data[0])));
|
|
}
|
|
template<Scalar T>
|
|
auto quaternion<T>::get_up(void)const -> vec3<value_type>{
|
|
return vec3<value_type>( 2 * ((m_data[1] * m_data[2]) + (m_data[3] * m_data[0])),
|
|
1 - 2 * ((m_data[1] * m_data[1]) + (m_data[3] * m_data[3])),
|
|
2 * ((m_data[2] * m_data[3]) - (m_data[1] * m_data[0])));
|
|
}
|
|
template<Scalar T>
|
|
auto quaternion<T>::get_forward(void)const -> vec3<value_type>{
|
|
return vec3<value_type>( 2 * ((m_data[1] * m_data[3]) - (m_data[2] * m_data[0])),
|
|
2 * ((m_data[2] * m_data[3]) + (m_data[1] * m_data[0])),
|
|
1 - 2 * ((m_data[1] * m_data[1]) + (m_data[2] * m_data[2])));
|
|
}
|
|
template<Scalar T>
|
|
auto quaternion<T>::to_vec3(void)const -> vec3<value_type>{
|
|
return vec3<value_type>(m_data[1], m_data[2], m_data[3]);
|
|
}
|
|
template<Scalar T>
|
|
auto quaternion<T>::to_vec4(void)const -> vec4<value_type>{
|
|
return vec4<value_type>(m_data[1], m_data[2], m_data[3]);
|
|
}
|
|
template<Scalar T>
|
|
auto quaternion<T>::to_mat3(void)const -> mat3<value_type>{
|
|
mat3<value_type> m;
|
|
|
|
value_type xx = m_data[1] * m_data[1];
|
|
value_type yy = m_data[2] * m_data[2];
|
|
value_type zz = m_data[3] * m_data[3];
|
|
|
|
value_type xy = m_data[1] * m_data[2];
|
|
value_type xz = m_data[1] * m_data[3];
|
|
value_type xw = m_data[1] * m_data[0];
|
|
value_type yz = m_data[2] * m_data[3];
|
|
value_type yw = m_data[2] * m_data[0];
|
|
value_type zw = m_data[3] * m_data[0];
|
|
|
|
m.get(0) = 1 - 2 * (yy + zz);
|
|
m.get(1) = 2 * (xy + zw);
|
|
m.get(2) = 2 * (xz - yw);
|
|
m.get(3) = 2 * (xy - zw);
|
|
m.get(4) = 1 - 2 * (xx + zz);
|
|
m.get(5) = 2 * (yz + xw);
|
|
m.get(6) = 2 * (xz + yw);
|
|
m.get(7) = 2 * (yz - xw);
|
|
m.get(8) = 1 - 2 * (xx + yy);
|
|
return m;
|
|
}
|
|
template<Scalar T>
|
|
auto quaternion<T>::to_mat4(void)const -> mat4<value_type>{
|
|
mat4<value_type> m;
|
|
|
|
value_type xx = m_data[1] * m_data[1];
|
|
value_type yy = m_data[2] * m_data[2];
|
|
value_type zz = m_data[3] * m_data[3];
|
|
|
|
value_type xy = m_data[1] * m_data[2];
|
|
value_type xz = m_data[1] * m_data[3];
|
|
value_type xw = m_data[1] * m_data[0];
|
|
value_type yz = m_data[2] * m_data[3];
|
|
value_type yw = m_data[2] * m_data[0];
|
|
value_type zw = m_data[3] * m_data[0];
|
|
|
|
m.get(0) = 1 - 2 * (yy + zz);
|
|
m.get(1) = 2 * (xy + zw);
|
|
m.get(2) = 2 * (xz - yw);
|
|
m.get(3) = 0;
|
|
m.get(4) = 2 * (xy - zw);
|
|
m.get(5) = 1 - 2 * (xx + zz);
|
|
m.get(6) = 2 * (yz + xw);
|
|
m.get(7) = 0;
|
|
m.get(8) = 2 * (xz + yw);
|
|
m.get(9) = 2 * (yz - xw);
|
|
m.get(10) = 1 - 2 * (xx + yy);
|
|
m.get(11) = 0;
|
|
m.get(12) = 0;
|
|
m.get(13) = 0;
|
|
m.get(14) = 0;
|
|
m.get(15) = 1;
|
|
return m;
|
|
}
|
|
template<Scalar T>
|
|
auto quaternion<T>::to_euler_angles(void)const -> vec3<value_type>{
|
|
value_type ww = m_data[0] * m_data[0];
|
|
value_type xx = m_data[1] * m_data[1];
|
|
value_type yy = m_data[2] * m_data[2];
|
|
value_type zz = m_data[3] * m_data[3];
|
|
|
|
value_type correction = ww + xx + yy + zz;
|
|
value_type test = m_data[1] * m_data[2] + m_data[3] * m_data[0];
|
|
if(test > 0.499 * correction){
|
|
return vec3<value_type>(0, 2 * std::atan2(m_data[1], m_data[0]), pi<value_type>() / 2.0);
|
|
}else if(test < -0.499 * correction){
|
|
return vec3<value_type>(0, -2 * std::atan2(m_data[1], m_data[0]), -pi<value_type>() / 2.0);
|
|
}
|
|
return vec3<value_type>(std::atan2((2 * m_data[1] * m_data[0]) - (2 * m_data[2] * m_data[3]), ww - xx + yy - zz),
|
|
std::atan2((2 * m_data[2] * m_data[0]) - (2 * m_data[1] * m_data[3]), xx - yy - zz + ww),
|
|
std::asin(2 * test / correction));
|
|
}
|
|
template<Scalar T>
|
|
auto quaternion<T>::to_axis_angle(void)const -> std::pair<value_type,vec3<value_type>>{
|
|
quaternion q(*this);
|
|
if(m_data[0] > 1.0)
|
|
q = q.normalize();
|
|
|
|
value_type s = std::sqrt(1 - q.m_data[0] * q.m_data[0]);
|
|
if(s <= value_type{0.001}){
|
|
return {2 * std::acos(q.m_data[0]), {1, 0, 0}};
|
|
}
|
|
return {2 * std::acos(q.m_data[0]), {q.m_data[1] / s, q.m_data[2] / s, q.m_data[3] / s}};
|
|
}
|
|
|
|
template<Scalar T, Scalar U>
|
|
bool operator==(const quaternion<T>& left, const quaternion<U>& right){
|
|
return left.w() == right.w() &&
|
|
left.x() == right.x() &&
|
|
left.y() == right.y() &&
|
|
left.z() == right.z();
|
|
}
|
|
template<Scalar T, Scalar U>
|
|
bool operator!=(const quaternion<T>& left, const quaternion<U>& right){
|
|
return !(left == right);
|
|
}
|
|
template<Scalar T>
|
|
auto operator-(const quaternion<T>& left){
|
|
using res_t = T;
|
|
return quaternion<res_t>(manual_initialize, -left.w(), -left.x(), -left.y(), -left.z());
|
|
}
|
|
template<Scalar T, Scalar U>
|
|
auto operator-(const quaternion<T>& left, const quaternion<U>& right){
|
|
using res_t = decltype(std::declval<T>() - std::declval<U>());
|
|
return quaternion<res_t>(manual_initialize, left.w() - right.w(), left.x() - right.x(),
|
|
left.y() - right.y(), left.z() - right.z());
|
|
}
|
|
template<Scalar T, Scalar U>
|
|
auto operator+(const quaternion<T>& left, const quaternion<U>& right){
|
|
using res_t = decltype(std::declval<T>() + std::declval<U>());
|
|
return quaternion<res_t>(manual_initialize, left.w() + right.w(), left.x() + right.x(),
|
|
left.y() + right.y(), left.z() + right.z());
|
|
}
|
|
template<Scalar T, Scalar U>
|
|
auto operator*(const quaternion<T>& left, const quaternion<U>& right){
|
|
using res_t = decltype(std::declval<T>() * std::declval<U>());
|
|
return quaternion<res_t>(manual_initialize,
|
|
(right.w() * left.w()) - (right.x() * left.x()) -
|
|
(right.y() * left.y()) - (right.z() * left.z()),
|
|
(right.w() * left.x()) + (right.x() * left.w()) +
|
|
(right.y() * left.z()) - (right.z() * left.y()),
|
|
(right.w() * left.y()) - (right.x() * left.z()) +
|
|
(right.y() * left.w()) + (right.z() * left.x()),
|
|
(right.w() * left.z()) + (right.x() * left.y()) -
|
|
(right.y() * left.x()) + (right.z() * left.w()));
|
|
}
|
|
template<Scalar T, Scalar U>
|
|
auto operator*(const quaternion<T>& left, const vec3<U>& right){
|
|
return left.to_mat3() * right;
|
|
}
|
|
template<Scalar T, Scalar U>
|
|
auto operator*(const quaternion<T>& left, U&& right){
|
|
using res_t = decltype(std::declval<T>() * std::declval<U>());
|
|
return quaternion<res_t>(manual_initialize, left.w() * right, left.x() * right, left.y() * right, left.z() * right);
|
|
}
|
|
template<Scalar T, Scalar U>
|
|
auto operator/(const quaternion<T>& left, U&& right){
|
|
using res_t = decltype(std::declval<T>() / std::declval<U>());
|
|
return quaternion<res_t>(manual_initialize, left.w() / right, left.x() / right, left.y() / right, left.z() / right);
|
|
}
|
|
template<Scalar T, Scalar U>
|
|
decltype(auto) operator+=(quaternion<T>& left, const quaternion<U>& right){
|
|
left.w() += right.w();
|
|
left.x() += right.x();
|
|
left.y() += right.y();
|
|
left.z() += right.z();
|
|
return left;
|
|
}
|
|
template<Scalar T, Scalar U>
|
|
decltype(auto) operator-=(quaternion<T>& left, const quaternion<U>& right){
|
|
left.w() -= right.w();
|
|
left.x() -= right.x();
|
|
left.y() -= right.y();
|
|
left.z() -= right.z();
|
|
return left;
|
|
}
|
|
template<Scalar T, Scalar U>
|
|
decltype(auto) operator*=(quaternion<T>& left, const quaternion<U>& right){
|
|
left = left * right;
|
|
return left;
|
|
}
|
|
|
|
template<Scalar T, Scalar U>
|
|
decltype(auto) operator*=(quaternion<T>& left, U&& right){
|
|
left.w() *= right;
|
|
left.x() *= right;
|
|
left.y() *= right;
|
|
left.z() *= right;
|
|
return left;
|
|
}
|
|
template<Scalar T, Scalar U>
|
|
decltype(auto) operator/=(quaternion<T>& left, U&& right){
|
|
left.w() /= right;
|
|
left.x() /= right;
|
|
left.y() /= right;
|
|
left.z() /= right;
|
|
return left;
|
|
}
|
|
|
|
}
|
|
|
|
#endif
|