425 lines
13 KiB
C++
425 lines
13 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_MAT_TPP
|
|
#define REXY_MAT_TPP
|
|
|
|
#include <cstdlib> //size_t
|
|
#include <cmath> //sin, cos, abs
|
|
#include <type_traits> //decay_t, declval
|
|
#include "detail/matrix.hpp"
|
|
#include "quat.hpp"
|
|
|
|
namespace math{
|
|
|
|
namespace detail{
|
|
template<class T>
|
|
static constexpr const T& min(const T& l, const T& r){
|
|
return l < r ? l : r;
|
|
}
|
|
}
|
|
|
|
template<class T, size_t R, size_t C>
|
|
template<size_t... Ss>
|
|
constexpr matrix_base<T,R,C>::matrix_base(std::integer_sequence<size_type,Ss...>):
|
|
m_data{Ss...}{}
|
|
|
|
template<class T, size_t R, size_t C>
|
|
constexpr matrix_base<T,R,C>::matrix_base(void):
|
|
matrix_base(typename detail::default_initialization_matrix<Columns,Rows>::tuple{}){}
|
|
|
|
template<class T, size_t R, size_t C>
|
|
constexpr matrix_base<T,R,C>::matrix_base(detail::zero_initialize_t):
|
|
m_data{}{}
|
|
template<class T, size_t R, size_t C>
|
|
constexpr matrix_base<T,R,C>::matrix_base(detail::no_initialize_t){}
|
|
|
|
template<class T, size_t R, size_t C>
|
|
constexpr matrix_base<T,R,C>::matrix_base(value_type v){
|
|
for(size_type i = 0; i < Columns*Rows; ++i)
|
|
m_data[i] = v;
|
|
}
|
|
template<class T, size_t R, size_t C>
|
|
template<class... Args, std::enable_if_t<(std::is_convertible_v<Args,T> && ...),int>>
|
|
constexpr matrix_base<T,R,C>::matrix_base(Args&&... args):
|
|
m_data{static_cast<value_type>(std::forward<Args>(args))...}{}
|
|
|
|
template<class T, size_t R, size_t C>
|
|
template<class U>
|
|
constexpr matrix_base<T,R,C>::matrix_base(const matrix_base<U,Columns,Rows>& m){
|
|
using mat = matrix_base<U,Columns,Rows>;
|
|
for(typename mat::size_type i = 0; i < mat::Columns*mat::Rows; ++i)
|
|
m_data[i] = m.get(i);
|
|
}
|
|
|
|
template<class T, size_t R, size_t C>
|
|
template<class U, size_t TR, size_t TC>
|
|
constexpr matrix_base<T,R,C>& matrix_base<T,R,C>::operator=(const matrix_base<U,TR,TC>& m){
|
|
constexpr auto cols = detail::min(TC, C);
|
|
constexpr auto rws = detail::min(TR, R);
|
|
for(size_type i = 0;i < cols;++i){
|
|
for(size_type j = 0;j < rws;++j){
|
|
get(i, j) = m.get(i, j);
|
|
}
|
|
}
|
|
return *this;
|
|
}
|
|
|
|
|
|
template<class T, size_t R, size_t C>
|
|
constexpr auto matrix_base<T,R,C>::operator[](size_type x){
|
|
return detail::mat_ref_obj<value_type,Rows>{m_data, x};
|
|
}
|
|
template<class T, size_t R, size_t C>
|
|
constexpr auto matrix_base<T,R,C>::operator[](size_type x)const{
|
|
return detail::mat_ref_obj<const value_type,Rows>{m_data, x};
|
|
}
|
|
template<class T, size_t R, size_t C>
|
|
constexpr auto matrix_base<T,R,C>::get(size_type x, size_type y) -> reference{
|
|
return m_data[(x*Rows)+y];
|
|
}
|
|
template<class T, size_t R, size_t C>
|
|
constexpr auto matrix_base<T,R,C>::get(size_type x, size_type y)const -> const_reference{
|
|
return m_data[(x*Rows)+y];
|
|
}
|
|
template<class T, size_t R, size_t C>
|
|
constexpr auto matrix_base<T,R,C>::get(size_type i) -> reference{
|
|
return m_data[i];
|
|
}
|
|
template<class T, size_t R, size_t C>
|
|
constexpr auto matrix_base<T,R,C>::get(size_type i)const -> const_reference{
|
|
return m_data[i];
|
|
}
|
|
|
|
template<class T, size_t R, size_t C>
|
|
constexpr auto matrix_base<T,R,C>::columns(void)const -> size_type{
|
|
return Columns;
|
|
}
|
|
template<class T, size_t R, size_t C>
|
|
constexpr auto matrix_base<T,R,C>::rows(void)const -> size_type{
|
|
return Rows;
|
|
}
|
|
template<class T, size_t R, size_t C>
|
|
constexpr auto matrix_base<T,R,C>::size(void)const -> size_type{
|
|
return Columns*Rows;
|
|
}
|
|
|
|
template<class T, size_t R, size_t C>
|
|
constexpr auto matrix_base<T,R,C>::raw(void) -> pointer{
|
|
return m_data;
|
|
}
|
|
template<class T, size_t R, size_t C>
|
|
constexpr auto matrix_base<T,R,C>::raw(void)const -> const_pointer{
|
|
return m_data;
|
|
}
|
|
template<class T, size_t R, size_t C>
|
|
constexpr matrix_base<T,R,C>::operator pointer(void){
|
|
return m_data;
|
|
}
|
|
template<class T, size_t R, size_t C>
|
|
constexpr matrix_base<T,R,C>::operator const_pointer(void)const{
|
|
return m_data;
|
|
}
|
|
|
|
template<class T, size_t R, size_t C>
|
|
template<size_t TR, size_t TC, std::enable_if_t<TR <= R && TC <= C,int>>
|
|
constexpr matrix<T,R,C>::matrix(const matrix_base<value_type,TR,TC>& other){
|
|
for(size_type i = 0;i < TC;++i){
|
|
for(size_type j = 0;j < TR;++j){
|
|
get(i, j) = other.get(i, j);
|
|
}
|
|
}
|
|
}
|
|
template<class T, size_t R, size_t C>
|
|
template<class U>
|
|
constexpr matrix<T,R,C>::matrix(const matrix<U,R,C>& other){
|
|
for(size_type i = 0;i < C;++i){
|
|
for(size_type j = 0;j < R;++j){
|
|
get(i, j) = other.get(i, j);
|
|
}
|
|
}
|
|
}
|
|
|
|
template<class T, size_t R, size_t C>
|
|
template<class U>
|
|
constexpr matrix<T,R,C>& matrix<T,R,C>::operator=(const matrix<U,R,C>& m){
|
|
base::operator=(m);
|
|
return *this;
|
|
}
|
|
|
|
template<class T, size_t R>
|
|
constexpr matrix<T,R,R>::matrix(detail::id_initialize_t):
|
|
base(){}
|
|
template<class T, size_t R>
|
|
template<size_t TR, size_t TC, std::enable_if_t<TR <= R && TC <= R,int>>
|
|
constexpr matrix<T,R,R>::matrix(const matrix_base<value_type,TR,TC>& other):
|
|
matrix(id_initialize)
|
|
{
|
|
for(size_type i = 0;i < TC;++i){
|
|
for(size_type j = 0;j < TR;++j){
|
|
get(i, j) = other.get(i, j);
|
|
}
|
|
}
|
|
}
|
|
template<class T, size_t R>
|
|
template<class U>
|
|
constexpr matrix<T,R,R>::matrix(const matrix<U,R,R>& other){
|
|
for(size_type i = 0;i < R;++i){
|
|
for(size_type j = 0;j < R;++j){
|
|
get(i, j) = other.get(i, j);
|
|
}
|
|
}
|
|
}
|
|
|
|
|
|
template<class T, size_t R>
|
|
template<class U>
|
|
constexpr matrix<T,R,R>& matrix<T,R,R>::operator=(const matrix<U,R,R>& m){
|
|
base::operator=(m);
|
|
return *this;
|
|
}
|
|
|
|
template<class T, size_t R>
|
|
constexpr auto matrix<T,R,R>::determinate(void)const -> value_type{
|
|
return math::determinate(*this);
|
|
}
|
|
template<class T, size_t R>
|
|
constexpr auto matrix<T,R,R>::trace(void)const -> value_type{
|
|
value_type sum = 0;
|
|
for(size_type i = 0; i < R; ++i){
|
|
sum += this->get(i, i);
|
|
}
|
|
return sum;
|
|
}
|
|
template<class T, size_t R>
|
|
constexpr matrix<T,R,R> matrix<T,R,R>::transpose(void)const{
|
|
matrix m(no_initialize);
|
|
for(size_type i = 0; i < R; ++i){
|
|
for(size_type j = 0; j < R; ++j){
|
|
m.get(j, i) = this->get(i, j);
|
|
}
|
|
}
|
|
return m;
|
|
}
|
|
template<class T, size_t R>
|
|
constexpr matrix<T,R,R> matrix<T,R,R>::inverse(void)const{
|
|
return math::inverse(*this);
|
|
}
|
|
|
|
|
|
template<class T, size_t R>
|
|
constexpr T determinate(const matrix<T,R,R>& m){
|
|
return detail::determinate_helper<T,R>::perform(m);
|
|
}
|
|
template<class T, size_t R>
|
|
constexpr matrix<T,R,R> inverse(const matrix<T,R,R>& m){
|
|
return detail::inverse_helper<T,R>::perform(m);
|
|
}
|
|
|
|
template<class T>
|
|
matrix<T,2,2> rotation2d_pure(T angle){
|
|
return rotation2d_pure(std::sin(angle), std::cos(angle));
|
|
}
|
|
template<class T>
|
|
constexpr matrix<T,2,2> rotation2d_pure(T sin, T cos){
|
|
return matrix<T,2,2>(cos, sin, -sin, cos);
|
|
}
|
|
template<class T>
|
|
constexpr matrix<T,2,2> scale2d(T x, T y){
|
|
return matrix<T,2,2>(x, T{0}, T{0}, y);
|
|
}
|
|
|
|
template<class T>
|
|
matrix<T,3,3> rotation2d(T angle){
|
|
return rotation2d(std::sin(angle), std::cos(angle));
|
|
}
|
|
template<class T>
|
|
constexpr matrix<T,3,3> rotation2d(T sin, T cos){
|
|
return matrix<T,3,3>(cos, -sin, T{0},
|
|
sin, cos, T{0},
|
|
T{0}, T{0}, T{1});
|
|
}
|
|
template<class T>
|
|
matrix<T,3,3> rotation2d(T x, T y, T z){
|
|
quaternion<T> q(x, y, z);
|
|
return q.to_mat3();
|
|
}
|
|
|
|
template<class T>
|
|
constexpr matrix<T,4,4> rotation3d(T angle_x, T angle_y, T angle_z){
|
|
quaternion<T> q(angle_x, angle_y, angle_z);
|
|
return q.to_mat4();
|
|
}
|
|
template<class T>
|
|
constexpr matrix<T,4,4> translation3d(T x, T y, T z){
|
|
return matrix<T,4,4>(T{1}, T{0}, T{0}, T{0},
|
|
T{0}, T{1}, T{0}, T{0},
|
|
T{0}, T{0}, T{1}, T{0},
|
|
x, y, z, T{1});
|
|
}
|
|
template<class T>
|
|
constexpr matrix<T,4,4> scale3d(T x, T y, T z){
|
|
return matrix<T,4,4>(x, T{0}, T{0}, T{0},
|
|
T{0}, y, T{0}, T{0},
|
|
T{0}, T{0}, z, T{0},
|
|
T{0}, T{0}, T{0}, T{1});
|
|
}
|
|
|
|
|
|
template<class T, class U, size_t R, size_t C>
|
|
constexpr bool operator==(const matrix_base<T,R,C>& left, const matrix_base<U,R,C> right){
|
|
for(size_t i = 0; i < left.size(); ++i){
|
|
if(left.get(i) != right.get(i))
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
template<class T, class U, size_t R, size_t C>
|
|
constexpr bool operator!=(const matrix_base<T,R,C>& left, const matrix_base<U,R,C> right){
|
|
return !(left == right);
|
|
}
|
|
|
|
template<class T, class U, size_t R1, size_t C1, size_t C2>
|
|
constexpr auto operator*(const matrix<T,R1,C1>& left, const matrix<U,C1,C2>& right){
|
|
using res_t = decltype(std::declval<T>() * std::declval<U>());
|
|
matrix<res_t,R1,C2> res(zero_initialize);
|
|
size_t index = 0;
|
|
for(size_t i = 0; i < right.rows(); ++i){
|
|
for(size_t j = 0; j < left.columns(); ++j){
|
|
for(size_t k = 0; k < left.rows(); ++k){
|
|
res.get(index) += right.get(i, k) * left.get(k, j);
|
|
}
|
|
++index;
|
|
}
|
|
}
|
|
return res;
|
|
}
|
|
template<class T, class U, size_t C, size_t R> requires Compatible_Scalar<U,T>
|
|
constexpr auto operator*(const matrix<T,R,C>& left, U&& right){
|
|
using res_t = decltype(std::declval<T>() * std::declval<U>());
|
|
matrix<res_t,R,C> res(no_initialize);
|
|
for(size_t i = 0; i < left.size(); ++i){
|
|
res.get(i) = left.get(i) * std::forward<U>(right);
|
|
}
|
|
return res;
|
|
}
|
|
template<class T, class U, size_t C, size_t R> requires Compatible_Scalar<U,T>
|
|
constexpr auto operator*(U&& left, const matrix<T,R,C>& right){
|
|
using res_t = decltype(std::declval<T>() * std::declval<U>());
|
|
matrix<res_t,R,C> res(no_initialize);
|
|
for(size_t i = 0; i < right.size(); ++i){
|
|
res.get(i) = std::forward<U>(left) * right.get(i);
|
|
}
|
|
return res;
|
|
}
|
|
template<class T, class U, size_t C, size_t R> requires Compatible_Scalar<U,T>
|
|
constexpr auto operator/(const matrix<T,R,C>& left, U&& right){
|
|
using res_t = decltype(std::declval<T>() / std::declval<U>());
|
|
matrix<res_t,R,C> res(no_initialize);
|
|
for(size_t i = 0; i < left.size(); ++i){
|
|
res.get(i) = left.get(i) / std::forward<U>(right);
|
|
}
|
|
return res;
|
|
}
|
|
template<class T, class U, size_t C, size_t R>
|
|
constexpr auto operator+(const matrix<T,R,C>& left, const matrix<U,R,C>& right){
|
|
using res_t = decltype(std::declval<T>() + std::declval<U>());
|
|
matrix<res_t,R,C> res(no_initialize);
|
|
for(size_t i = 0; i < left.size(); ++i){
|
|
res.get(i) = left.get(i) + right.get(i);
|
|
}
|
|
return res;
|
|
}
|
|
template<class T, class U, size_t C, size_t R>
|
|
constexpr auto operator-(const matrix<T,R,C>& left, const matrix<U,R,C>& right){
|
|
using res_t = decltype(std::declval<T>() - std::declval<U>());
|
|
matrix<res_t,R,C> res(no_initialize);
|
|
for(size_t i = 0; i < left.size(); ++i){
|
|
res.get(i) = left.get(i) - right.get(i);
|
|
}
|
|
return res;
|
|
}
|
|
template<class T, class U, size_t C, size_t R>
|
|
constexpr auto operator-(const matrix<T,R,C>& left){
|
|
using res_t = decltype(-std::declval<U>());
|
|
matrix<res_t,R,C> res(no_initialize);
|
|
for(size_t i = 0; i < left.size(); ++i){
|
|
res.get(i) = -left.get(i);
|
|
}
|
|
return res;
|
|
}
|
|
template<class T, size_t R, size_t C>
|
|
constexpr auto abs(const matrix_base<T,R,C>& left){
|
|
matrix<T,R,C> res(no_initialize);
|
|
for(size_t i = 0; i < left.size(); ++i){
|
|
res.get(i) = std::abs(left.get(i));
|
|
}
|
|
return res;
|
|
}
|
|
template<class T, class U, class V, size_t R, size_t C>
|
|
constexpr bool fuzzy_eq(const matrix_base<T,R,C>& left, const matrix_base<U,R,C>& right, const V& epsilon){
|
|
for(size_t i = 0;i < left.size();++i){
|
|
if(std::abs(left.get(i) - right.get(i)) > epsilon)
|
|
return false;
|
|
}
|
|
return true;
|
|
}
|
|
template<class T, class U, class V, size_t R, size_t C>
|
|
constexpr bool fuzzy_neq(const matrix_base<T,R,C>& left, const matrix_base<U,R,C>& right, const V& epsilon){
|
|
return !fuzzy_eq(left, right, epsilon);
|
|
}
|
|
|
|
template<class T, class U, size_t R>
|
|
constexpr decltype(auto) operator*=(matrix<T,R,R>& left, const matrix<U,R,R>& right){
|
|
//have to evaluate entire expression first since matrix multiplication depends on reusing many elements
|
|
//cannot be expression templatized, TODO
|
|
return (left = (left * right));
|
|
}
|
|
template<class T, class U, size_t C, size_t R> requires Compatible_Scalar<U,T>
|
|
constexpr decltype(auto) operator*=(matrix<T,R,C>& left, U&& right){
|
|
for(size_t i = 0; i < left.size(); ++i){
|
|
left.get(i) = left.get(i) * std::forward<U>(right);
|
|
}
|
|
return left;
|
|
}
|
|
template<class T, class U, size_t C, size_t R> requires Compatible_Scalar<U,T>
|
|
constexpr decltype(auto) operator/=(matrix<T,R,C>& left, U&& right){
|
|
for(size_t i = 0; i < left.size(); ++i){
|
|
left.get(i) = left.get(i) / std::forward<U>(right);
|
|
}
|
|
return left;
|
|
}
|
|
template<class T, class U, size_t C, size_t R>
|
|
constexpr decltype(auto) operator+=(matrix<T,R,C>& left, const matrix<U,R,C>& right){
|
|
for(size_t i = 0; i < left.size(); ++i){
|
|
left.get(i) = left.get(i) + right.get(i);
|
|
}
|
|
return left;
|
|
}
|
|
template<class T, class U, size_t C, size_t R>
|
|
constexpr decltype(auto) operator-=(matrix<T,R,C>& left, const matrix<U,R,C>& right){
|
|
for(size_t i = 0; i < left.size(); ++i){
|
|
left.get(i) = left.get(i) - right.get(i);
|
|
}
|
|
return left;
|
|
}
|
|
|
|
}
|
|
|
|
#endif
|