ecker/engine
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases 1 Wiki Activity

bigly refractors vector/quaternion/matrix all around (and beginning to ensure SIMD optimizations for physics)

Browse Source
This commit is contained in:
ecker 2025-09-04 00:46:01 -05:00
parent 593bdc93cd
commit c87eac5e05
34 changed files with 1680 additions and 5093 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

6
Makefile
View File

@ -66,7 +66,7 @@ ifneq (,$(findstring win64,$(ARCH)))
REQ_DEPS += meshoptimizer toml xatlas curl ffx:fsr dc:texconv # vall_e cpptrace # openvr # ncurses draco discord bullet ultralight-ux
FLAGS += -march=native -g # -flto # -g
endif
REQ_DEPS += $(RENDERER) json:nlohmann zlib luajit reactphysics simd ctti gltf imgui fmt freetype openal ogg wav
REQ_DEPS += $(RENDERER) json:nlohmann zlib luajit r:eactphysics simd ctti gltf imgui fmt freetype openal ogg wav
FLAGS += -DUF_ENV_WINDOWS -DUF_ENV_WIN64 -DWIN32_LEAN_AND_MEAN
DEPS += -lgdi32 -ldwmapi
LINKS += #-Wl,-subsystem,windows
@ -76,13 +76,13 @@ else ifneq (,$(findstring linux,$(ARCH)))
REQ_DEPS += toml xatlas curl dc:texconv # meshoptimizer ffx:fsr cpptrace vall_e # ncurses openvr draco discord bullet ultralight-ux
FLAGS += -march=native -g # -flto # -g
endif
REQ_DEPS += $(RENDERER) json:nlohmann zlib luajit reactphysics simd ctti gltf imgui fmt freetype openal ogg wav
REQ_DEPS += $(RENDERER) json:nlohmann zlib luajit r:eactphysics simd ctti gltf imgui fmt freetype openal ogg wav
FLAGS += -DUF_ENV_LINUX -fPIC
DEPS += -pthread -ldl -lX11 -lXrandr
INCS := -I./dep/master/include $(INCS)
else ifneq (,$(findstring dreamcast,$(ARCH)))
FLAGS += -DUF_ENV_DREAMCAST # -DUF_LEAN_AND_MEAN # this apparently crashes
REQ_DEPS += opengl gldc json:nlohmann zlib lua reactphysics simd ctti fmt freetype openal aldc ogg wav png # imgui
REQ_DEPS += opengl gldc json:nlohmann zlib lua r:eactphysics simd ctti fmt freetype openal aldc ogg wav png # imgui
INCS := -I./dep/dreamcast/include $(INCS)
endif

2
bin/exe/default/renderer
View File

@ -1 +1 @@
vulkan
opengl

6
engine/inc/uf/utils/math/math.h
View File

@ -10,6 +10,12 @@
#define UF_EZ_VEC4(vec, size) vec[0], size > 1 ? vec[1] : 0, size > 2 ? vec[2] : 0, size > 3 ? vec[3] : 0
#endif
#define NUM pod::Math::num_t
#ifndef M_PI
#define M_PI 3.141592653589793f
#endif
namespace pod {
namespace Math {
typedef float num_t;

171
engine/inc/uf/utils/math/matrix.h
View File

@ -5,49 +5,41 @@
#include "math.h"
namespace pod {
template<typename T = pod::Math::num_t, size_t R = 4, size_t C = R>
#if UF_MATRIX_ALIGNED
struct /*UF_API*/ alignas(16) Matrix {
#else
template<typename T = NUM, size_t R = 4, size_t C = R>
struct /*UF_API*/ Matrix {
#endif
// n-dimensional/unspecialized matrix access
T components[R*C] = {};
// T components[R][C] = {};
// T* array = (T*) this;
// POD information
typedef T type_t;
typedef T* container_t;
static const uint_fast8_t rows = R;
static const uint_fast8_t columns = C;
static const size_t rows = R;
static const size_t columns = C;
// Overload access
// Accessing via subscripts
inline T& operator[](uint_fast8_t i);
inline const T& operator[](uint_fast8_t i) const;
inline T& operator[](size_t i);
inline const T& operator[](size_t i) const;
inline T& operator()(uint_fast8_t r, uint_fast8_t c);
inline const T& operator()(uint_fast8_t r, uint_fast8_t c) const;
inline T& operator()(size_t r, size_t c);
inline const T& operator()(size_t r, size_t c) const;
// Arithmetic
Matrix<T,R,C> operator()() const; // Creation
Matrix<T,R,C> operator-() const; // Negation
Matrix<T,R,C> operator*( const Matrix<T,R,C>& matrix ) const; // Multiplication between two matrices
Matrix<T,R,C> operator*( T scalar ) const; // Multiplication between two matrices
Matrix<T,R,C> operator+( const Matrix<T,R,C>& matrix ) const; // Multiplication between two matrices
Matrix<T,R,C>& operator *=( const Matrix<T,R,C>& matrix ); // Multiplication set between two matrices
bool operator==( const Matrix<T,R,C>& matrix ) const; // Equality check between two matrices (equals)
bool operator!=( const Matrix<T,R,C>& matrix ) const; // Equality check between two matrices (not equals)
Matrix<T,R,C> operator*( const Matrix<T,R,C>& matrix ) const;
Matrix<T,R,C> operator*( T scalar ) const;
Matrix<T,R,C> operator+( const Matrix<T,R,C>& matrix ) const;
Matrix<T,R,C>& operator *=( const Matrix<T,R,C>& matrix );
bool operator==( const Matrix<T,R,C>& matrix ) const;
bool operator!=( const Matrix<T,R,C>& matrix ) const;
};
template<typename T = pod::Math::num_t> using Matrix2t = Matrix<T,2>;
template<typename T = NUM> using Matrix2t = Matrix<T,2>;
typedef Matrix2t<> Matrix2;
typedef Matrix2t<float> Matrix2f;
template<typename T = pod::Math::num_t> using Matrix3t = Matrix<T,3>;
template<typename T = NUM> using Matrix3t = Matrix<T,3>;
typedef Matrix3t<> Matrix3;
typedef Matrix3t<float> Matrix3f;
template<typename T = pod::Math::num_t> using Matrix4t = Matrix<T,4>;
template<typename T = NUM> using Matrix4t = Matrix<T,4>;
typedef Matrix4t<> Matrix4;
typedef Matrix4t<float> Matrix4f;
}
@ -56,61 +48,56 @@ namespace uf {
namespace matrix {
extern bool UF_API reverseInfiniteProjection;
template<typename T=pod::Math::num_t> pod::Matrix4t<T> /*UF_API*/ identity();
template<typename T=NUM> pod::Matrix4t<T> /*UF_API*/ identity();
template<typename T=pod::Math::num_t> pod::Matrix4t<T> /*UF_API*/ initialize( const T* );
template<typename T=pod::Math::num_t> pod::Matrix4t<T> /*UF_API*/ initialize( const uf::stl::vector<T>& );
template<typename T=NUM> pod::Matrix4t<T> /*UF_API*/ initialize( const T* );
template<typename T=NUM> pod::Matrix4t<T> /*UF_API*/ initialize( const uf::stl::vector<T>& );
template<typename T> pod::Matrix<typename T::type_t, T::columns, T::columns> /*UF_API*/ identityi();
// Equality checking
template<typename T=pod::Matrix4> int /*UF_API*/ compareTo( const T& left, const T& right ); // Equality check between two matrices (less than)
template<typename T=pod::Matrix4> bool /*UF_API*/ equals( const T& left, const T& right ); // Equality check between two matrices (equals)
template<typename T=pod::Matrix4> bool /*UF_API*/ equals( const T& left, const T& right, float eps ); // Equality check between two matrices (equals)
// Basic arithmetic
// template<typename T=pod::Matrix4> pod::Matrix<typename T::type_t, C, C> /*UF_API*/ multiply( const T& left, const T& right ); // Multiplies two matrices of same type and size together
template<typename T, typename U> pod::Matrix<typename T::type_t, T::columns, T::columns> multiply( const T& left, const U& right ); // Multiplies two matrices of same type and size together
template<typename T> pod::Matrix4t<T> multiply( const pod::Matrix4t<T>& left, const pod::Matrix4t<T>& right ); // Multiplies two matrices of same type and size together
template<typename T=pod::Matrix4> bool /*UF_API*/ equals( const T& left, const T& right, float eps = 1.0e-6f );
template<typename T, typename U> pod::Matrix<typename T::type_t, T::columns, T::columns> multiply( const T& left, const U& right );
template<typename T> pod::Matrix4t<T> multiply( const pod::Matrix4t<T>& left, const pod::Matrix4t<T>& right );
template<typename T=pod::Matrix4> T /*UF_API*/ transpose( const T& matrix );
template<typename T=pod::Math::num_t> pod::Matrix2t<T> inverse(const pod::Matrix2t<T>& mat );
template<typename T=pod::Math::num_t> pod::Matrix3t<T> inverse(const pod::Matrix3t<T>& mat );
template<typename T=pod::Math::num_t> pod::Matrix4t<T> inverse(const pod::Matrix4t<T>& mat );
template<typename T=NUM> pod::Matrix2t<T> inverse(const pod::Matrix2t<T>& mat );
template<typename T=NUM> pod::Matrix3t<T> inverse(const pod::Matrix3t<T>& mat );
template<typename T=NUM> pod::Matrix4t<T> inverse(const pod::Matrix4t<T>& mat );
template<typename T=pod::Math::num_t> pod::Vector2t<T> multiply(const pod::Matrix2t<T>& mat, const pod::Vector2t<T>& v );
template<typename T=pod::Math::num_t> pod::Vector3t<T> multiply(const pod::Matrix3t<T>& mat, const pod::Vector3t<T>& v );
template<typename T=pod::Math::num_t> pod::Vector3t<T> multiply( const pod::Matrix4t<T>& mat, const pod::Vector3t<T>& vector, T w = 1, bool = false );
template<typename T=pod::Math::num_t> pod::Vector4t<T> multiply( const pod::Matrix4t<T>& mat, const pod::Vector4t<T>& vector, bool = false );
template<typename T=NUM> pod::Vector2t<T> multiply(const pod::Matrix2t<T>& mat, const pod::Vector2t<T>& v );
template<typename T=NUM> pod::Vector3t<T> multiply(const pod::Matrix3t<T>& mat, const pod::Vector3t<T>& v );
template<typename T=NUM> pod::Vector3t<T> multiply( const pod::Matrix4t<T>& mat, const pod::Vector3t<T>& vector, T w = 1, bool = false );
template<typename T=NUM> pod::Vector4t<T> multiply( const pod::Matrix4t<T>& mat, const pod::Vector4t<T>& vector, bool = false );
template<typename T=pod::Matrix4> T /*UF_API*/ multiplyAll( const T& matrix, typename T::type_t scalar );
template<typename T=pod::Matrix4> T /*UF_API*/ add( const T& lhs, const T& rhs );
// Writes to first value
template<typename T=pod::Matrix4> T& /*UF_API*/ invert( T& matrix ); // Flip sign of all components
// template<typename T=pod::Matrix4> pod::Matrix<typename T::type_t, C, C>& /*UF_API*/ multiply_( T& left, const T& right ); // Multiplies two matrices of same type and size together
template<typename T, typename U> pod::Matrix<typename T::type_t, T::columns, T::columns> multiply_( T& left, const U& right ); // Multiplies two matrices of same type and size together
template<typename T=pod::Matrix4> T& /*UF_API*/ inverse_( T& matrix );
template<typename T, typename U> pod::Matrix<typename T::type_t, T::columns, T::columns> multiply_( T& left, const U& right );
template<typename T> pod::Matrix<typename T::type_t, T::columns, T::columns> multiply_( T& left, const T& right );
template<typename T=pod::Matrix4> T& /*UF_API*/ translate_( T& matrix, const pod::Vector3t<typename T::type_t>& vector );
template<typename T=pod::Matrix4> T& /*UF_API*/ rotate_( T& matrix, const pod::Vector3t<typename T::type_t>& vector );
template<typename T=pod::Matrix4> T& /*UF_API*/ scale_( T& matrix, const pod::Vector3t<typename T::type_t>& vector );
// Complex arithmetic
template<typename T=pod::Matrix4> T /*UF_API*/ translate( const T& matrix, const pod::Vector3t<typename T::type_t>& vector );
template<typename T=pod::Matrix4> T /*UF_API*/ rotate( const T& matrix, const pod::Vector3t<typename T::type_t>& vector );
template<typename T=pod::Matrix4> T /*UF_API*/ scale( const T& matrix, const pod::Vector3t<typename T::type_t>& vector );
template<typename T=pod::Matrix4> pod::Vector3t<typename T::type_t> /*UF_API*/ eulerAngles( const T& matrix );
template<typename T=pod::Math::num_t> pod::Matrix4t<T> /*UF_API*/ orthographic( T, T, T, T, T, T );
template<typename T=pod::Math::num_t> pod::Matrix4t<T> /*UF_API*/ orthographic( T, T, T, T );
template<typename T=pod::Math::num_t> pod::Matrix4t<T> inline /*UF_API*/ orthographic( const pod::Vector2t<T>& lr, const pod::Vector2t<T>& bt, const pod::Vector2t<T>& nf ) {
template<typename T=NUM> pod::Matrix4t<T> /*UF_API*/ orthographic( T, T, T, T, T, T );
template<typename T=NUM> pod::Matrix4t<T> /*UF_API*/ orthographic( T, T, T, T );
template<typename T=NUM> pod::Matrix4t<T> inline /*UF_API*/ orthographic( const pod::Vector2t<T>& lr, const pod::Vector2t<T>& bt, const pod::Vector2t<T>& nf ) {
return orthographic<T>( lr.x, lr.y, bt.x, bt.y, nf.x, nf.y );
}
template<typename T=pod::Math::num_t> pod::Matrix4t<T> inline /*UF_API*/ orthographic( const pod::Vector2t<T>& lr, const pod::Vector2t<T>& bt ) {
template<typename T=NUM> pod::Matrix4t<T> inline /*UF_API*/ orthographic( const pod::Vector2t<T>& lr, const pod::Vector2t<T>& bt ) {
return orthographic<T>( lr.x, lr.y, bt.x, bt.y );
}
template<typename T=pod::Math::num_t> pod::Matrix4t<T> /*UF_API*/ perspective( T, T, T, T );
template<typename T=pod::Math::num_t> pod::Matrix4t<T> inline /*UF_API*/ perspective( T fov, T raidou, const pod::Vector2f& range ) {
template<typename T=NUM> pod::Matrix4t<T> /*UF_API*/ perspective( T, T, T, T );
template<typename T=NUM> pod::Matrix4t<T> inline /*UF_API*/ perspective( T fov, T raidou, const pod::Vector2f& range ) {
return perspective( fov, raidou, range.x, range.y );
}
template<typename T=pod::Math::num_t> pod::Matrix4t<T> inline /*UF_API*/ perspective( T fov, const pod::Vector2ui& size, const pod::Vector2f& range ) {
template<typename T=NUM> pod::Matrix4t<T> inline /*UF_API*/ perspective( T fov, const pod::Vector2ui& size, const pod::Vector2f& range ) {
return perspective( fov, (T) size.x / (T) size.y, range.x, range.y );
}
// Setting
@ -124,82 +111,6 @@ namespace uf {
}
}
#if UF_USE_CLASS_OF_PODS
namespace uf {
template<typename T = pod::Math::num_t, std::size_t R = 4, std::size_t C = R>
class /*UF_API*/ Matrix {
public:
// Easily access POD's type
typedef pod::Matrix<T,R,C> pod_t;
// Replicate POD information
typedef T type_t;
typedef T* container_t;
static const uint_fast8_t rows = R;
static const uint_fast8_t columns = C;
protected:
// POD storage
Matrix<T,R,C>::pod_t m_pod;
public:
// C-tor
Matrix(); // initializes POD to 'def'
Matrix(const Matrix<T,R,C>::pod_t& pod); // copies POD altogether
Matrix(const T components[R][C]); // copies data into POD from 'components' (typed as C array)
Matrix(const T components[R*C]); // copies data into POD from 'components' (typed as C array)
Matrix(const uf::stl::vector<T>& components); // copies data into POD from 'components' (typed as std::matrix<T>)
// D-tor
// Unneccesary
// POD access
Matrix<T,R,C>::pod_t& data(); // Returns a reference of POD
const Matrix<T,R,C>::pod_t& data() const; // Returns a const-reference of POD
template<typename Q> typename Matrix<Q,R,C>::pod_t convert() const; // Returns a POD converted
// Alternative POD access
T* get(); // Returns a pointer to the entire array
const T* get() const; // Returns a const-pointer to the entire array
T& getComponent( uint_fast8_t i ); // Returns a reference to a single element
const T& getComponent( uint_fast8_t i ) const; // Returns a const-reference to a single element
// POD manipulation
T* set(const T components[R][C]); // Sets the entire array
T* set(const T components[R*C]); // Sets the entire array
T& setComponent( uint_fast8_t i, const T& value ); // Sets a single element
// Validation
bool isValid() const; // Checks if all components are valid (non NaN, inf, etc.)
// Basic arithmetic
inline uf::Matrix<T,C,C> multiply( const Matrix<T,R,C>& matrix ) const; // Multiplies two matrices of same type and size together
inline uf::Matrix<T,C,C> multiply( const Matrix<T,R,C>& matrix ); // Multiplies two matrices of same type and size together
inline pod::Vector3t<T> multiply( const pod::Vector3t<T>& vector ) const;
inline pod::Vector4t<T> multiply( const pod::Vector4t<T>& vector ) const;
inline uf::Matrix<T,R,C>& negate(); // Flip sign of all components
inline uf::Matrix<T,R,C>& translate( const pod::Vector3t<T>& vector );
inline uf::Matrix<T,R,C>& rotate( const pod::Vector3t<T>& vector );
inline uf::Matrix<T,R,C>& scale( const pod::Vector3t<T>& vector );
inline uf::Matrix<T,R,C>& invert();
inline uf::Matrix<T,R,C> inverse() const;
template<typename U>
inline uf::Matrix<T,C,C> multiply( const U& matrix ) const; // Multiplies two matrices of same type and size together
template<typename U>
inline uf::Matrix<T,C,C> multiply( const U& matrix ); // Multiplies two matrices of same type and size together
// Overloaded ops
// Accessing via subscripts
T& operator[](uint_fast8_t i);
const T& operator[](uint_fast8_t i) const;
// Arithmetic
inline Matrix<T,R,C> operator-() const; // Negation
inline Matrix<T,C,C> operator*( const Matrix<T,R,C>& matrix ) const; // Multiplication between two matrices
inline Matrix<T,C,C>& operator *=( const Matrix<T,R,C>& matrix ); // Multiplication set between two matrices
template<typename U> inline Matrix<T,C,C> operator*( const U& matrix ) const; // Multiplication between two matrices
inline bool operator==( const Matrix<T,R,C>& matrix ) const; // Equality check between two matrices (equals)
inline bool operator!=( const Matrix<T,R,C>& matrix ) const; // Equality check between two matrices (not equals)
inline operator pod_t&() { return this->m_pod; }
inline operator const pod_t&() const { return this->m_pod; }
};
template<typename T = pod::Math::num_t> using Matrix4t = Matrix<T,4>;
typedef Matrix4t<> Matrix4;
typedef Matrix4t<float> Matrix4f;
}
#endif
#include <sstream>
namespace uf {
namespace string {

207
engine/inc/uf/utils/math/matrix/class.inl
View File

@ -1,207 +0,0 @@
// C-tor
// initializes POD to 'def'
template<typename T, size_t R, size_t C>
uf::Matrix<T,R,C>::Matrix() {
this->m_pod = uf::matrix::identityi<R,C>();
}
// copies POD altogether
template<typename T, size_t R, size_t C>
uf::Matrix<T,R,C>::Matrix(const Matrix<T,R,C>::pod_t& pod ) :
m_pod(pod)
{
}
// copies data into POD from 'components' (typed as C array)
template<typename T, size_t R, size_t C>
uf::Matrix<T,R,C>::Matrix(const T components[R][C] ) {
this->set(components);
}
template<typename T, size_t R, size_t C>
uf::Matrix<T,R,C>::Matrix(const T components[R*C] ) {
this->set(components);
}
// copies data into POD from 'components' (typed as std::matrix<T>)
template<typename T, size_t R, size_t C>
uf::Matrix<T,R,C>::Matrix(const uf::stl::vector<T>& components ) {
this->m_pod = uf::matrix::initialize( components );
}
// D-tor
// Unneccesary
// POD access
// Returns a reference of POD
template<typename T, size_t R, size_t C>
typename uf::Matrix<T,R,C>::pod_t& uf::Matrix<T,R,C>::data() {
return this->m_pod;
}
// Returns a const-reference of POD
template<typename T, size_t R, size_t C>
const typename uf::Matrix<T,R,C>::pod_t& uf::Matrix<T,R,C>::data() const {
return this->m_pod;
}
// Returns a const-reference of POD
template<typename T, size_t R, size_t C>
template<typename Q>
typename uf::Matrix<Q,R,C>::pod_t uf::Matrix<T,R,C>::convert() const {
typename uf::Matrix<Q,R,C>::pod_t converted;
for ( uint_fast8_t i = 0; i < R*C; ++i )
converted.components[i] = (float) this->m_pod.components[i];
return converted;
}
// Alternative POD access
// Returns a pointer to the entire array
template<typename T, size_t R, size_t C>
T* uf::Matrix<T,R,C>::get() {
return (T*) this->m_pod.components;
}
// Returns a const-pointer to the entire array
template<typename T, size_t R, size_t C>
const T* uf::Matrix<T,R,C>::get() const {
return (T*) this->m_pod.components;
}
// Returns a reference to a single element
template<typename T, size_t R, size_t C>
T& uf::Matrix<T,R,C>::getComponent( uint_fast8_t i ) {
return this->m_pod.components[i];
}
// Returns a const-reference to a single element
template<typename T, size_t R, size_t C>
const T& uf::Matrix<T,R,C>::getComponent( uint_fast8_t i ) const {
return this->m_pod.components[i];
}
// POD manipulation
// Sets the entire array
template<typename T, size_t R, size_t C>
T* uf::Matrix<T,R,C>::set(const T components[R][C] ) {
for ( uint_fast8_t r = 0; r < R; ++r )
for ( uint_fast8_t c = 0; c < C; ++c )
this->m_pod.components[r+c*C] = components[r][c];
return (T*) this->m_pod.components;
}
template<typename T, size_t R, size_t C>
T* uf::Matrix<T,R,C>::set(const T components[R*C] ) {
// memcpy( this->m_pod.components, components, sizeof(this->m_pod) );
for ( uint_fast8_t i = 0; i < R*C; ++i )
this->m_pod.components[i] = components[i];
/*
for ( size_t r = 0; r < R; ++r )
for ( size_t c = 0; c < C; ++c )
this->m_pod.components[r+c*C] = components[r+c*C];
*/
return (T*) this->m_pod.components;
}
// Sets a single element
template<typename T, size_t R, size_t C>
T& uf::Matrix<T,R,C>::setComponent( uint_fast8_t i, const T& value ) {
this->m_pod.components[i] = value;
}
// Validation
// Checks if all components are valid (non NaN, inf, etc.)
template<typename T, size_t R, size_t C>
bool uf::Matrix<T,R,C>::isValid() const {
T val;
for ( uint_fast8_t i = 0; i < R * C; ++i )
if ( (val = this->m_pod.components[i]) != val ) return false;
return true;
}
// Basic arithmetic
// Multiplies two matrices of same type and size together
template<typename T, size_t R, size_t C>
inline uf::Matrix<T,C,C> uf::Matrix<T,R,C>::multiply( const Matrix<T,R,C>& matrix ) {
return uf::matrix::multiply(this->m_pod, matrix.data());
}
template<typename T, size_t R, size_t C>
inline uf::Matrix<T,C,C> uf::Matrix<T,R,C>::multiply( const Matrix<T,R,C>& matrix ) const {
return uf::matrix::multiply(this->m_pod, matrix.data());
}
// Flip sign of all components
template<typename T, size_t R, size_t C>
inline uf::Matrix<T,R,C>& uf::Matrix<T,R,C>::translate( const pod::Vector3t<T>& vector ) {
uf::matrix::translate(this->m_pod, vector);
return *this;
}
template<typename T, size_t R, size_t C>
inline uf::Matrix<T,R,C>& uf::Matrix<T,R,C>::rotate( const pod::Vector3t<T>& vector ) {
uf::matrix::rotate(this->m_pod, vector);
return *this;
}
template<typename T, size_t R, size_t C>
inline uf::Matrix<T,R,C>& uf::Matrix<T,R,C>::scale( const pod::Vector3t<T>& vector ) {
uf::matrix::scale(this->m_pod, vector);
return *this;
}
template<typename T, size_t R, size_t C>
inline uf::Matrix<T,R,C>& uf::Matrix<T,R,C>::invert() {
return uf::matrix::invert(this->m_pod);
}
template<typename T, size_t R, size_t C>
inline uf::Matrix<T,R,C> uf::Matrix<T,R,C>::inverse() const {
return uf::matrix::inverse(this->m_pod);
}
template<typename T, size_t R, size_t C>
template<typename U> inline uf::Matrix<T,C,C> uf::Matrix<T,R,C>::multiply( const U& matrix ) const {
return uf::matrix::multiply(this->m_pod, matrix.data());
}
template<typename T, size_t R, size_t C>
template<typename U> inline uf::Matrix<T,C,C> uf::Matrix<T,R,C>::multiply( const U& matrix ) {
return uf::matrix::multiply(this->m_pod, matrix.data());
}
template<typename T, size_t R, size_t C>
inline pod::Vector3t<T> uf::Matrix<T,R,C>::multiply( const pod::Vector3t<T>& vector ) const {
return uf::matrix::multiply(this->m_pod, vector);
}
template<typename T, size_t R, size_t C>
inline pod::Vector4t<T> uf::Matrix<T,R,C>::multiply( const pod::Vector4t<T>& vector ) const {
return uf::matrix::multiply(this->m_pod, vector);
}
// Overloaded ops
// Accessing via subscripts
/*
template<typename T, size_t R, size_t C>
T* uf::Matrix<T,R,C>::operator[](size_t i) {
return this->m_pod[i];
}
template<typename T, size_t R, size_t C>
const T* uf::Matrix<T,R,C>::operator[](size_t i) const {
return this->m_pod[i];
}
*/
template<typename T, size_t R, size_t C>
T& uf::Matrix<T,R,C>::operator[](uint_fast8_t i) {
return this->m_pod[i];
}
template<typename T, size_t R, size_t C>
const T& uf::Matrix<T,R,C>::operator[](uint_fast8_t i) const {
return this->m_pod[i];
}
// Arithmetic
// Negation
template<typename T, size_t R, size_t C>
inline uf::Matrix<T,R,C> uf::Matrix<T,R,C>::operator-() const {
return this->inverse();
}
// Multiplication between two matrices
template<typename T, size_t R, size_t C>
inline uf::Matrix<T,C,C> uf::Matrix<T,R,C>::operator*( const Matrix<T,R,C>& matrix ) const {
return this->multiply(matrix);
}
// Multiplication set between two matrices
template<typename T, size_t R, size_t C>
inline uf::Matrix<T,C,C>& uf::Matrix<T,R,C>::operator *=( const Matrix<T,R,C>& matrix ) {
return this->multiply(matrix);
}
template<typename T, size_t R, size_t C>
template<typename U> inline uf::Matrix<T,C,C> uf::Matrix<T,R,C>::operator*( const U& matrix ) const {
return this->multiply(matrix);
}
// Equality check between two matrices (equals)
template<typename T, size_t R, size_t C>
inline bool uf::Matrix<T,R,C>::operator==( const Matrix<T,R,C>& matrix ) const {
return uf::matrix::equals( this->m_pod, matrix.m_pod );
}
// Equality check between two matrices (not equals)
template<typename T, size_t R, size_t C>
inline bool uf::Matrix<T,R,C>::operator!=( const Matrix<T,R,C>& matrix ) const {
return !uf::matrix::equals( this->m_pod, matrix.m_pod );
}

615
engine/inc/uf/utils/math/matrix/pod.inl
View File

@ -1,171 +1,101 @@
#if !__clang__ && __GNUC__
#pragma GCC push_options
#pragma GCC optimize ("unroll-loops")
#endif
#define FOR_EACH_2D( R, C, F ) for_each_index<R>([&](auto r) { for_each_index<C>([&](auto c) F ); });
#define ROW_MAJOR_INDEX( R, C, r, c ) (r * C + c)
#define COL_MAJOR_INDEX( R, C, r, c ) (c * R + r)
#define INDEX( R, C, r, c ) COL_MAJOR_INDEX( R, C, r, c )
// Overloaded ops
// Accessing via subscripts
template<typename T, size_t R, size_t C>
inline T& pod::Matrix<T,R,C>::operator[](uint_fast8_t i) {
// static T null = 0.0/0.0;
// if ( i >= R*C ) return null;
inline T& pod::Matrix<T,R,C>::operator[](size_t i) {
return this->components[i];
}
template<typename T, size_t R, size_t C>
inline const T& pod::Matrix<T,R,C>::operator[](uint_fast8_t i) const {
// static T null = 0.0/0.0;
// if ( i >= R*C ) return null;
inline const T& pod::Matrix<T,R,C>::operator[](size_t i) const {
return this->components[i];
}
template<typename T, size_t R, size_t C>
pod::Matrix<T,R,C> pod::Matrix<T,R,C>::operator()() const {
pod::Matrix<T,R,C> matrix;
#pragma unroll // GCC unroll C
for ( uint_fast8_t c = 0; c < C; ++c )
#pragma unroll // GCC unroll R
for ( uint_fast8_t r = 0; r < R; ++r )
matrix[r+c*C] = (r == c ? 1 : 0);
return matrix;
inline T& pod::Matrix<T,R,C>::operator()(size_t r, size_t c) {
return this->components[INDEX( R, C, r, c )];
}
template<typename T, size_t R, size_t C>
inline T& pod::Matrix<T,R,C>::operator()(uint_fast8_t r, uint_fast8_t c) {
return this->components[r+c*C];
inline const T& pod::Matrix<T,R,C>::operator()(size_t r, size_t c) const {
return this->components[INDEX( R, C, r, c )];
}
template<typename T, size_t R, size_t C>
inline const T& pod::Matrix<T,R,C>::operator()(uint_fast8_t r, uint_fast8_t c) const {
return this->components[r+c*C];
}
/*
template<typename T, size_t R, size_t C>
T* pod::Matrix<T,R,C>::operator[](size_t i) {
return this->components[i];
}
template<typename T, size_t R, size_t C>
const T* pod::Matrix<T,R,C>::operator[](size_t i) const {
return this->components[i];
}
*/
template<typename T>
pod::Matrix4t<T> /*UF_API*/ uf::matrix::identity() {
ALIGN16 pod::Matrix4t<T> matrix;
#pragma unroll // GCC unroll 4
for ( uint_fast8_t c = 0; c < 4; ++c )
#pragma unroll // GCC unroll 4
for ( uint_fast8_t r = 0; r < 4; ++r )
matrix[r+c*4] = (r == c ? 1 : 0);
pod::Matrix4t<T> matrix;
FOR_EACH_2D(4, 4, {
matrix(r, c) = (r == c ? T{1} : T{0});
});
return matrix;
}
template<typename T>
pod::Matrix4t<T> /*UF_API*/ uf::matrix::initialize( const T* list ) {
ALIGN16 pod::Matrix4t<T> matrix;
// memcpy(&matrix[0], list, sizeof(matrix));
#pragma unroll // GCC unroll 16
for ( uint_fast8_t i = 0; i < 16; ++i )
pod::Matrix4t<T> matrix;
FOR_EACH(16, {
matrix.components[i] = list[i];
});
/*
for ( uint_fast8_t r = 0; r < 4; ++r )
for ( uint_fast8_t c = 0; c < 4; ++c )
matrix[r+c*4] = list[r+c*4];
*/
return matrix;
}
template<typename T>
pod::Matrix4t<T> /*UF_API*/ uf::matrix::initialize( const uf::stl::vector<T>& list ) {
ALIGN16 pod::Matrix4t<T> matrix;
pod::Matrix4t<T> matrix;
if ( list.size() != 16 ) return matrix;
// memcpy(&matrix[0], &list[0], sizeof(matrix));
#pragma unroll // GCC unroll 16
for ( uint_fast8_t i = 0; i < 16; ++i )
FOR_EACH(16, {
matrix.components[i] = list[i];
});
/*
#pragma unroll // GCC unroll 4
for ( uint_fast8_t r = 0; r < 4; ++r )
#pragma unroll // GCC unroll 4
for ( uint_fast8_t c = 0; c < 4; ++c )
matrix[r+c*4] = list[r+c*4];
*/
return matrix;
}
template<typename T> pod::Matrix<typename T::type_t, T::columns, T::columns> uf::matrix::identityi(){
ALIGN16 pod::Matrix<typename T::type_t, T::columns, T::columns> matrix;
#pragma unroll // GCC unroll T::columns
for ( uint_fast8_t c = 0; c < T::columns; ++c )
#pragma unroll // GCC unroll T::rows
for ( uint_fast8_t r = 0; r < T::rows; ++r )
matrix[r+c*T::columns] = (r == c ? 1 : 0);
pod::Matrix<typename T::type_t, T::columns, T::columns> matrix;
FOR_EACH_2D(T::rows, T::columns, {
matrix(r, c) = (r == c ? 1 : 0);
});
return matrix;
}
// Arithmetic
// Negation
template<typename T, size_t R, size_t C>
inline pod::Matrix<T,R,C> pod::Matrix<T,R,C>::operator-() const {
return uf::matrix::inverse(*this);
}
// Multiplication between two matrices
template<typename T, size_t R, size_t C>
inline pod::Matrix<T,R,C> pod::Matrix<T,R,C>::operator*( const Matrix<T,R,C>& matrix ) const {
return uf::matrix::multiply(*this, matrix);
}
// Multiplication between two matrices
template<typename T, size_t R, size_t C>
inline pod::Matrix<T,R,C> pod::Matrix<T,R,C>::operator*( T scalar ) const {
return uf::matrix::multiplyAll(*this, scalar);
}
// Multiplication between two matrices
template<typename T, size_t R, size_t C>
inline pod::Matrix<T,R,C> pod::Matrix<T,R,C>::operator+( const Matrix<T,R,C>& matrix ) const {
return uf::matrix::add(*this, matrix);
}
// Multiplication set between two matrices
template<typename T, size_t R, size_t C>
inline pod::Matrix<T,R,C>& pod::Matrix<T,R,C>::operator *=( const Matrix<T,R,C>& matrix ) {
return uf::matrix::multiply(*this, matrix);
inline pod::Matrix<T,R,C>& pod::Matrix<T,R,C>::operator*=( const Matrix<T,R,C>& matrix ) {
return uf::matrix::multiply_(*this, matrix);
}
// Equality check between two matrices (equals)
template<typename T, size_t R, size_t C>
inline bool pod::Matrix<T,R,C>::operator==( const Matrix<T,R,C>& matrix ) const {
return uf::matrix::equals( *this, matrix );
}
// Equality check between two matrices (not equals)
template<typename T, size_t R, size_t C>
inline bool pod::Matrix<T,R,C>::operator!=( const Matrix<T,R,C>& matrix ) const {
return !uf::matrix::equals( *this, matrix );
}
// Equality checking
// Equality check between two matrices (less than)
template<typename T> int uf::matrix::compareTo( const T& left, const T& right ) {
return memcmp( &left[0], &right[0], sizeof(left) );
}
// Equality check between two matrices (equals)
template<typename T> bool uf::matrix::equals( const T& left, const T& right ) {
return uf::matrix::compareTo(left, right) == 0;
}
template<typename T> bool uf::matrix::equals( const T& left, const T& right, float eps ) {
bool equals = true;
for ( size_t i = 0; i < 16; ++i ) {
if ( abs(left[i] - right[i]) <= eps ) continue;
equals = false;
break;
}
return equals;
bool result = true;
FOR_EACH(T::rows * T::columns, {
if ( fabs(left[i] - right[i]) > eps ) result = false;
});
return result;
}
// Basic arithmetic
// Multiplies two matrices of same type and size together
template<typename T> pod::Matrix<T,4,4> uf::matrix::multiply( const pod::Matrix<T,4,4>& left, const pod::Matrix<T,4,4>& right ) {
ALIGN16 pod::Matrix<T,4,4> res;
pod::Matrix<T,4,4> res;
#if UF_USE_SIMD
auto row1 = uf::simd::load(&left[0]);
auto row2 = uf::simd::load(&left[4]);
auto row3 = uf::simd::load(&left[8]);
auto row4 = uf::simd::load(&left[12]);
#pragma unroll // GCC unroll 4
for( uint_fast8_t i = 0; i < 4; i++) {
FOR_EACH(4, {
auto brod1 = uf::simd::set(right[4*i + 0]);
auto brod2 = uf::simd::set(right[4*i + 1]);
auto brod3 = uf::simd::set(right[4*i + 2]);
@ -178,7 +108,7 @@ template<typename T> pod::Matrix<T,4,4> uf::matrix::multiply( const pod::Matrix<
uf::simd::mul(brod3, row3),
uf::simd::mul(brod4, row4)));
uf::simd::store(row, &res[4*i]);
}
});
return res;
#elif UF_ENV_DREAMCAST
@ -191,40 +121,15 @@ template<typename T> pod::Matrix<T,4,4> uf::matrix::multiply( const pod::Matrix<
// MATH_Load_Matrix_Product( (ALL_FLOATS_STRUCT*) &left[0], (ALL_FLOATS_STRUCT*) &right[0] );
// MATH_Store_XMTRX( (ALL_FLOATS_STRUCT*) &res[0]);
return res;
#elif 0
//
float* dstPtr = &res[0];
const float* leftPtr = &right[0];
#pragma unroll // GCC unroll 4
for (uint_fast8_t i = 0; i < 4; ++i) {
#pragma unroll // GCC unroll 4
for (uint_fast8_t j = 0; j < 4; ++j) {
const float* rightPtr = &left[0] + j;
float sum = leftPtr[0] * rightPtr[0];
#pragma unroll // GCC unroll 3
for (uint_fast8_t n = 1; n < 4; ++n) {
rightPtr += 4;
sum += leftPtr[n] * rightPtr[0];
}
*dstPtr++ = sum;
#else
#if 1
FOR_EACH_2D(4, 4, {
T sum = T{0};
for (size_t k = 0; k < 4; ++k) {
sum += left(r, k) * right(k, c);
}
leftPtr += 4;
}
return res;
#elif 0
// don't know if it's more performant than below
uint_fast8_t i = 0;
#pragma unroll // GCC unroll 4
for ( uint_fast8_t c = 0; c < 4; c++ ) {
#pragma unroll // GCC unroll 4
for ( uint_fast8_t r = 0; r < 4; r++ ) {
res[i++] = uf::vector::dot( { right[0+c*4], right[1+c*4], right[2+c*4], right[3+c*4] }, { left[r+0*4], left[r+1*4], left[r+2*4], left[r+3*4] } );
}
}
res(r, c) = sum;
});
return res;
#else
// it works
@ -249,22 +154,23 @@ template<typename T> pod::Matrix<T,4,4> uf::matrix::multiply( const pod::Matrix<
dst3 = srcA0 * srcB3[0] + srcA1 * srcB3[1] + srcA2 * srcB3[2] + srcA3 * srcB3[3];
return res;
#endif
#endif
}
template<typename T, typename U> pod::Matrix<typename T::type_t, T::columns, T::columns> uf::matrix::multiply( const T& left, const U& right ) {
ALIGN16 pod::Matrix<typename T::type_t,T::rows,T::columns> res;
#if 1
pod::Matrix<typename T::type_t,T::rows,T::columns> res;
float* dstPtr = &res[0];
const float* leftPtr = &right[0];
const float* leftPtr = &left[0];
#pragma unroll // GCC unroll T::rows
for (uint_fast8_t i = 0; i < T::rows; ++i) {
for ( auto i = 0; i < T::rows; ++i) {
#pragma unroll // GCC unroll T::columns
for (uint_fast8_t j = 0; j < T::columns; ++j) {
const float* rightPtr = &left[0] + j;
for ( auto j = 0; j < T::columns; ++j) {
const float* rightPtr = &right[0] + j;
float sum = leftPtr[0] * rightPtr[0];
#pragma unroll // GCC unroll T::columns - 1
for (uint_fast8_t n = 1; n < T::columns; ++n) {
for ( auto n = 1; n < T::columns; ++n) {
rightPtr += T::columns;
sum += leftPtr[n] * rightPtr[0];
}
@ -273,51 +179,35 @@ template<typename T, typename U> pod::Matrix<typename T::type_t, T::columns, T::
leftPtr += T::columns;
}
#else
uint_fast8_t i = 0;
#pragma unroll // GCC unroll
for ( uint_fast8_t col = 0; col < R; col++ ) {
#pragma unroll // GCC unroll
for ( uint_fast8_t row = 0; row < C; row++ ) {
auto& sum = res[i++];
#pragma unroll // GCC unroll
for ( uint_fast8_t i = 0; i < C; i++ )
sum += right[i + col * C] * left[row + i * R];
}
}
#endif
return res;
}
template<typename T> T /*UF_API*/ uf::matrix::multiplyAll( const T& m, typename T::type_t scalar ) {
ALIGN16 T matrix;
#pragma unroll // GCC unroll T::rows * T::columns
for ( uint_fast8_t i = 0; i < T::rows * T::columns; ++i )
T matrix;
FOR_EACH(T::rows * T::columns, {
matrix[i] = m[i] * scalar;
});
return matrix;
}
template<typename T> T /*UF_API*/ uf::matrix::add( const T& lhs, const T& rhs ) {
ALIGN16 T matrix;
#pragma unroll // GCC unroll T::rows * T::columns
for ( uint_fast8_t i = 0; i < T::rows * T::columns; ++i )
T matrix;
FOR_EACH(T::rows * T::columns, {
matrix[i] = lhs[i] + rhs[i];
});
return matrix;
}
// Transpose matrix
template<typename T> T uf::matrix::transpose( const T& matrix ) {
ALIGN16 T transpose;
#pragma unroll // GCC unroll T::rows
for ( typename T::type_t r = 0; r < T::rows; ++r )
#pragma unroll // GCC unroll T::columns
for ( typename T::type_t c = 0; c < T::columns; ++c )
transpose[c * T::rows + r] = matrix[r * T::columns + c];
T transpose;
FOR_EACH_2D(T::rows, T::columns, {
transpose(c, r) = matrix(r, c);
});
return transpose;
}
//
template<typename T> pod::Matrix2t<T> uf::matrix::inverse( const pod::Matrix2t<T>& m ) {
T det = m[0] * m[3] - m[1] * m[2];
if ( std::fabs(det) < 1e-12f ) return m;
@ -331,98 +221,37 @@ template<typename T> pod::Matrix2t<T> uf::matrix::inverse( const pod::Matrix2t<T
}
template<typename T> pod::Matrix3t<T> uf::matrix::inverse( const pod::Matrix3t<T>& m ) {
// matrix elements
const T* a = &m[0];
T det = a[0]*(a[4]*a[8] - a[5]*a[7])
- a[1]*(a[3]*a[8] - a[5]*a[6])
+ a[2]*(a[3]*a[7] - a[4]*a[6]);
if (std::fabs(det) < 1e-12f) return m; // singular
T det = a[0]*(a[4]*a[8] - a[5]*a[7]) - a[1]*(a[3]*a[8] - a[5]*a[6]) + a[2]*(a[3]*a[7] - a[4]*a[6]);
if ( std::fabs(det) < 1e-12f ) return m; // singular
T invDet = static_cast<T>(1) / det;
return pod::Matrix3t<T>{
(a[4]*a[8] - a[5]*a[7]) * invDet,
(a[2]*a[7] - a[1]*a[8]) * invDet,
(a[1]*a[5] - a[2]*a[4]) * invDet,
(a[5]*a[6] - a[3]*a[8]) * invDet,
(a[0]*a[8] - a[2]*a[6]) * invDet,
(a[2]*a[3] - a[0]*a[5]) * invDet,
(a[3]*a[7] - a[4]*a[6]) * invDet,
(a[1]*a[6] - a[0]*a[7]) * invDet,
(a[0]*a[4] - a[1]*a[3]) * invDet,
(a[4]*a[8] - a[5]*a[7]) * invDet, (a[2]*a[7] - a[1]*a[8]) * invDet, (a[1]*a[5] - a[2]*a[4]) * invDet,
(a[5]*a[6] - a[3]*a[8]) * invDet, (a[0]*a[8] - a[2]*a[6]) * invDet, (a[2]*a[3] - a[0]*a[5]) * invDet,
(a[3]*a[7] - a[4]*a[6]) * invDet, (a[1]*a[6] - a[0]*a[7]) * invDet, (a[0]*a[4] - a[1]*a[3]) * invDet,
};
}
template<typename T> pod::Matrix4t<T> uf::matrix::inverse( const pod::Matrix4t<T>& m ) {
const T* a = &m[0];
ALIGN16 pod::Matrix4t<T> inv;
pod::Matrix4t<T> inv;
inv[0] = a[5] * (a[10]*a[15] - a[11]*a[14]) -
a[9] * (a[6]*a[15] - a[7]*a[14]) +
a[13]* (a[6]*a[11] - a[7]*a[10]);
inv[4] = - a[4] * (a[10]*a[15] - a[11]*a[14]) +
a[8] * (a[6]*a[15] - a[7]*a[14]) -
a[12]* (a[6]*a[11] - a[7]*a[10]);
inv[8] = a[4] * (a[9]*a[15] - a[11]*a[13]) -
a[8] * (a[5]*a[15] - a[7]*a[13]) +
a[12]* (a[5]*a[11] - a[7]*a[9]);
inv[12] = - a[4] * (a[9]*a[14] - a[10]*a[13]) +
a[8] * (a[5]*a[14] - a[6]*a[13]) -
a[12]* (a[5]*a[10] - a[6]*a[9]);
inv[1] = - a[1] * (a[10]*a[15] - a[11]*a[14]) +
a[9] * (a[2]*a[15] - a[3]*a[14]) -
a[13]* (a[2]*a[11] - a[3]*a[10]);
inv[5] = a[0] * (a[10]*a[15] - a[11]*a[14]) -
a[8] * (a[2]*a[15] - a[3]*a[14]) +
a[12]* (a[2]*a[11] - a[3]*a[10]);
inv[9] = - a[0] * (a[9]*a[15] - a[11]*a[13]) +
a[8] * (a[1]*a[15] - a[3]*a[13]) -
a[12]* (a[1]*a[11] - a[3]*a[9]);
inv[13] = a[0] * (a[9]*a[14] - a[10]*a[13]) -
a[8] * (a[1]*a[14] - a[2]*a[13]) +
a[12]* (a[1]*a[10] - a[2]*a[9]);
inv[2] = a[1] * (a[6]*a[15] - a[7]*a[14]) -
a[5] * (a[2]*a[15] - a[3]*a[14]) +
a[13]* (a[2]*a[7] - a[3]*a[6]);
inv[6] = - a[0] * (a[6]*a[15] - a[7]*a[14]) +
a[4] * (a[2]*a[15] - a[3]*a[14]) -
a[12]* (a[2]*a[7] - a[3]*a[6]);
inv[10] = a[0] * (a[5]*a[15] - a[7]*a[13]) -
a[4] * (a[1]*a[15] - a[3]*a[13]) +
a[12]* (a[1]*a[7] - a[3]*a[5]);
inv[14] = - a[0] * (a[5]*a[14] - a[6]*a[13]) +
a[4] * (a[1]*a[14] - a[2]*a[13]) -
a[12]* (a[1]*a[6] - a[2]*a[5]);
inv[3] = - a[1] * (a[6]*a[11] - a[7]*a[10]) +
a[5] * (a[2]*a[11] - a[3]*a[10]) -
a[9] * (a[2]*a[7] - a[3]*a[6]);
inv[7] = a[0] * (a[6]*a[11] - a[7]*a[10]) -
a[4] * (a[2]*a[11] - a[3]*a[10]) +
a[8] * (a[2]*a[7] - a[3]*a[6]);
inv[11] = - a[0] * (a[5]*a[11] - a[7]*a[9]) +
a[4] * (a[1]*a[11] - a[3]*a[9]) -
a[8] * (a[1]*a[7] - a[3]*a[5]);
inv[15] = a[0] * (a[5]*a[10] - a[6]*a[9]) -
a[4] * (a[1]*a[10] - a[2]*a[9]) +
a[8] * (a[1]*a[6] - a[2]*a[5]);
inv[0] = a[5] * (a[10]*a[15] - a[11]*a[14]) - a[9] * (a[6]*a[15] - a[7]*a[14]) + a[13]* (a[6]*a[11] - a[7]*a[10]);
inv[4] = - a[4] * (a[10]*a[15] - a[11]*a[14]) + a[8] * (a[6]*a[15] - a[7]*a[14]) - a[12]* (a[6]*a[11] - a[7]*a[10]);
inv[8] = a[4] * (a[9]*a[15] - a[11]*a[13]) - a[8] * (a[5]*a[15] - a[7]*a[13]) + a[12]* (a[5]*a[11] - a[7]*a[9]);
inv[12] = - a[4] * (a[9]*a[14] - a[10]*a[13]) + a[8] * (a[5]*a[14] - a[6]*a[13]) - a[12]* (a[5]*a[10] - a[6]*a[9]);
inv[1] = - a[1] * (a[10]*a[15] - a[11]*a[14]) + a[9] * (a[2]*a[15] - a[3]*a[14]) - a[13]* (a[2]*a[11] - a[3]*a[10]);
inv[5] = a[0] * (a[10]*a[15] - a[11]*a[14]) - a[8] * (a[2]*a[15] - a[3]*a[14]) + a[12]* (a[2]*a[11] - a[3]*a[10]);
inv[9] = - a[0] * (a[9]*a[15] - a[11]*a[13]) + a[8] * (a[1]*a[15] - a[3]*a[13]) - a[12]* (a[1]*a[11] - a[3]*a[9]);
inv[13] = a[0] * (a[9]*a[14] - a[10]*a[13]) - a[8] * (a[1]*a[14] - a[2]*a[13]) + a[12]* (a[1]*a[10] - a[2]*a[9]);
inv[2] = a[1] * (a[6]*a[15] - a[7]*a[14]) - a[5] * (a[2]*a[15] - a[3]*a[14]) + a[13]* (a[2]*a[7] - a[3]*a[6]);
inv[6] = - a[0] * (a[6]*a[15] - a[7]*a[14]) + a[4] * (a[2]*a[15] - a[3]*a[14]) - a[12]* (a[2]*a[7] - a[3]*a[6]);
inv[10] = a[0] * (a[5]*a[15] - a[7]*a[13]) - a[4] * (a[1]*a[15] - a[3]*a[13]) + a[12]* (a[1]*a[7] - a[3]*a[5]);
inv[14] = - a[0] * (a[5]*a[14] - a[6]*a[13]) + a[4] * (a[1]*a[14] - a[2]*a[13]) - a[12]* (a[1]*a[6] - a[2]*a[5]);
inv[3] = - a[1] * (a[6]*a[11] - a[7]*a[10]) + a[5] * (a[2]*a[11] - a[3]*a[10]) - a[9] * (a[2]*a[7] - a[3]*a[6]);
inv[7] = a[0] * (a[6]*a[11] - a[7]*a[10]) - a[4] * (a[2]*a[11] - a[3]*a[10]) + a[8] * (a[2]*a[7] - a[3]*a[6]);
inv[11] = - a[0] * (a[5]*a[11] - a[7]*a[9]) + a[4] * (a[1]*a[11] - a[3]*a[9]) - a[8] * (a[1]*a[7] - a[3]*a[5]);
inv[15] = a[0] * (a[5]*a[10] - a[6]*a[9]) - a[4] * (a[1]*a[10] - a[2]*a[9]) + a[8] * (a[1]*a[6] - a[2]*a[5]);
// determinant
T det = a[0]*inv[0] + a[1] * inv[4] + a[2] * inv[8] + a[3] * inv[12];
@ -433,171 +262,184 @@ template<typename T> pod::Matrix4t<T> uf::matrix::inverse( const pod::Matrix4t<T
return inv;
}
template<typename T> pod::Vector3t<T> uf::matrix::multiply( const pod::Matrix4t<T>& mat, const pod::Vector3t<T>& vector, T w, bool div ) {
return uf::matrix::multiply( mat, pod::Vector4t<T>{ vector[0], vector[1], vector[2], w }, div );
template<typename T> pod::Vector3t<T> uf::matrix::multiply( const pod::Matrix4t<T>& mat, const pod::Vector3t<T>& v, T w, bool div ) {
auto res4 = uf::matrix::multiply(mat, pod::Vector4t<T>{ v[0], v[1], v[2], w }, div);
return pod::Vector3t<T>{ res4[0], res4[1], res4[2] };
}
template<typename T>
pod::Vector2t<T> uf::matrix::multiply(const pod::Matrix2t<T>& mat, const pod::Vector2t<T>& v ) {
return pod::Vector2t<T>{
v[0]* mat[0] + v[1]* mat[2],
v[0]* mat[1] + v[1]* mat[3],
};
v[0] * mat(0,0) + v[1] * mat(0,1),
v[0] * mat(1,0) + v[1] * mat(1,1)
};
}
template<typename T>
pod::Vector3t<T> uf::matrix::multiply(const pod::Matrix3t<T>& mat, const pod::Vector3t<T>& v ) {
return pod::Vector3t<T>{
v[0]* mat[0] + v[1]* mat[3] + v[2] * mat[6],
v[0]* mat[1] + v[1]* mat[4] + v[2] * mat[7],
v[0]* mat[2] + v[1]* mat[5] + v[2] * mat[8],
v[0] * mat(0,0) + v[1] * mat(0,1) + v[2] * mat(0,2),
v[0] * mat(1,0) + v[1] * mat(1,1) + v[2] * mat(1,2),
v[0] * mat(2,0) + v[1] * mat(2,1) + v[2] * mat(2,2)
};
}
template<typename T> pod::Vector4t<T> uf::matrix::multiply( const pod::Matrix4t<T>& mat, const pod::Vector4t<T>& vector, bool div ) {
template<typename T> pod::Vector4t<T> uf::matrix::multiply( const pod::Matrix4t<T>& mat, const pod::Vector4t<T>& v, bool div ) {
#if UF_ENV_DREAMCAST
MATH_Load_XMTRX( (ALL_FLOATS_STRUCT*) &mat[0] );
auto t = MATH_Matrix_Transform( vector[0], vector[1], vector[2], vector[3] );
auto t = MATH_Matrix_Transform( v[0], v[1], v[2], v[3] );
auto res = *((pod::Vector4t<T>*) &t);
if ( div && res.w > 0 ) res /= res.w;
return res;
#else
ALIGN16 auto res = pod::Vector4t<T>{
vector[0] * mat[0] + vector[1] * mat[4] + vector[2] * mat[8] + vector[3] * mat[12],
vector[0] * mat[1] + vector[1] * mat[5] + vector[2] * mat[9] + vector[3] * mat[13],
vector[0] * mat[2] + vector[1] * mat[6] + vector[2] * mat[10] + vector[3] * mat[14],
vector[0] * mat[3] + vector[1] * mat[7] + vector[2] * mat[11] + vector[3] * mat[15]
auto res = pod::Vector4t<T>{
v[0] * mat(0,0) + v[1] * mat(0,1) + v[2] * mat(0,2) + v[3] * mat(0,3),
v[0] * mat(1,0) + v[1] * mat(1,1) + v[2] * mat(1,2) + v[3] * mat(1,3),
v[0] * mat(2,0) + v[1] * mat(2,1) + v[2] * mat(2,2) + v[3] * mat(2,3),
v[0] * mat(3,0) + v[1] * mat(3,1) + v[2] * mat(3,2) + v[3] * mat(3,3)
};
if ( div && res.w > 0 ) res /= res.w;
return res;
#endif
}
// Writes to first value
template<typename T> T& uf::matrix::invert( T& matrix ) {
return matrix = uf::matrix::inverse((const T&) matrix);
// functions that serve as the basis to creating SRT matrices, specifically for applying to an identity matrix
template<typename T> T uf::matrix::translate( const T& matrix, const pod::Vector3t<typename T::type_t>& vector ) {
T res = matrix;
res(0,3) = vector.x;
res(1,3) = vector.y;
res(2,3) = vector.z;
return res;
}
template<typename T> T uf::matrix::rotate( const T& matrix, const pod::Vector3t<typename T::type_t>& vector ) {
T res = matrix;
if (vector.x != 0) {
T Rx = uf::matrix::identity<T>();
Rx(1,1) = cos(vector.x); Rx(1,2) = -sin(vector.x);
Rx(2,1) = sin(vector.x); Rx(2,2) = cos(vector.x);
res = uf::matrix::multiply(res, Rx);
}
if (vector.y != 0) {
T Ry = uf::matrix::identity<T>();
Ry(0,0) = cos(vector.y); Ry(0,2) = sin(vector.y);
Ry(2,0) = -sin(vector.y); Ry(2,2) = cos(vector.y);
res = uf::matrix::multiply(res, Ry);
}
if (vector.z != 0) {
T Rz = uf::matrix::identity<T>();
Rz(0,0) = cos(vector.z); Rz(0,1) = -sin(vector.z);
Rz(1,0) = sin(vector.z); Rz(1,1) = cos(vector.z);
res = uf::matrix::multiply(res, Rz);
}
return res;
}
template<typename T> T uf::matrix::scale( const T& matrix, const pod::Vector3t<typename T::type_t>& vector ) {
T res = matrix;
res(0,0) = vector.x;
res(1,1) = vector.y;
res(2,2) = vector.z;
return res;
}
template<typename T> pod::Matrix<typename T::type_t, T::columns, T::columns> uf::matrix::multiply_( T& left, const T& right ) {
return left = uf::matrix::multiply((const T&) left, right);
}
template<typename T> T& uf::matrix::translate_( T& matrix, const pod::Vector3t<typename T::type_t>& vector ) {
matrix[12] = vector.x;
matrix[13] = vector.y;
matrix[14] = vector.z;
return matrix;
return matrix = uf::matrix::translate((const T&) matrix, vector);
}
template<typename T> T& uf::matrix::rotate_( T& matrix, const pod::Vector3t<typename T::type_t>& vector ) {
if ( vector.x != 0 ) {
matrix[5] = cos( vector.x );
matrix[6] = sin( vector.x );
matrix[9] = -1 * sin( vector.x );
matrix[10] = cos( vector.x );
}
if ( vector.y != 0 ) {
matrix[0] = cos( vector.y );
matrix[2] = -1 * sin( vector.y );
matrix[8] = sin( vector.y );
matrix[10] = cos( vector.y );
}
if ( vector.z != 0 ) {
matrix[0] = cos( vector.z );
matrix[1] = sin( vector.z );
matrix[4] = -1 * sin( vector.z );
matrix[5] = cos( vector.z );
}
return matrix;
return matrix = uf::matrix::rotate((const T&) matrix, vector);
}
template<typename T> T& uf::matrix::scale_( T& matrix, const pod::Vector3t<typename T::type_t>& vector ) {
matrix[0] = vector.x;
matrix[5] = vector.y;
matrix[10] = vector.z;
return matrix;
return matrix = uf::matrix::scale((const T&) matrix, vector);
}
// Complex arithmetic
template<typename T> T uf::matrix::translate( const T& matrix, const pod::Vector3t<typename T::type_t>& vector ) {
ALIGN16 T res = matrix;
res[12] = vector.x;
res[13] = vector.y;
res[14] = vector.z;
return res;
template<typename T> T& uf::matrix::inverse_( T& matrix ) {
return matrix = uf::matrix::inverse((const T&) matrix);
}
template<typename T> T uf::matrix::rotate( const T& matrix, const pod::Vector3t<typename T::type_t>& vector ) {
ALIGN16 T res = matrix;
if ( vector.x != 0 ) {
res[5] = cos( vector.x );
res[6] = sin( vector.x );
res[9] = -1 * sin( vector.x );
res[10] = cos( vector.x );
}
if ( vector.y != 0 ) {
res[0] = cos( vector.y );
res[2] = -1 * sin( vector.y );
res[8] = sin( vector.y );
res[10] = cos( vector.y );
}
if ( vector.z != 0 ) {
res[0] = cos( vector.z );
res[1] = sin( vector.z );
res[4] = -1 * sin( vector.z );
res[5] = cos( vector.z );
}
return res;
}
template<typename T> T uf::matrix::scale( const T& matrix, const pod::Vector3t<typename T::type_t>& vector ) {
ALIGN16 T res = matrix;
res[0] = vector.x;
res[5] = vector.y;
res[10] = vector.z;
return res;
}
template<typename T>
pod::Matrix4t<T> /*UF_API*/ uf::matrix::orthographic( T l, T r, T b, T t, T f, T n ) {
ALIGN16 pod::Matrix4t<T> m = uf::matrix::identity();
m[0*4+0] = 2 / (r - l);
m[1*4+1] = 2 / (t - b);
m[2*4+2] = - 2 / (f - n);
m[3*4+0] = - (r + l) / (r - l);
m[3*4+1] = - (t + b) / (t - b);
m[3*4+2] = - (f + n) / (f - n);
pod::Matrix4t<T> m = uf::matrix::identity();
m(0,0) = static_cast<T>(2) / (r - l);
m(1,1) = static_cast<T>(2) / (t - b);
m(2,2) = static_cast<T>(-2) / (f - n);
// Translation terms go in the last column (col = 3)
m(0,3) = - (r + l) / (r - l);
m(1,3) = - (t + b) / (t - b);
m(2,3) = - (f + n) / (f - n);
return m;
/*
uf::stl::vector<T> m = {
2 / (r - l), 0, 0, 0,
0, 2 / (t - b), 0, 0,
0, 0, -2 / (f - n), 0,
-(r + l) / (r - l), -(t + b) / (t - b), -(f + n) / (f - n), 1,
};
return uf::matrix::initialize(m);
*/
}
template<typename T>
pod::Matrix4t<T> /*UF_API*/ uf::matrix::orthographic( T l, T r, T b, T t ) {
return pod::Matrix4t<T>({
2 / (r - l), 0, 0, 0,
0, 2 / (t - b), 0, 0,
0, 0, 1, 0,
-(r + l) / (r - l), -(t+b)/(t-b), 0, 1
});
pod::Matrix4t<T> m = uf::matrix::identity();
m(0,0) = static_cast<T>(2) / (r - l);
m(1,1) = static_cast<T>(2) / (t - b);
m(2,2) = static_cast<T>(1);
m(0,3) = - (r + l) / (r - l);
m(1,3) = - (t + b) / (t - b);
}
template<typename T>
pod::Matrix4t<T> /*UF_API*/ uf::matrix::perspective( T fov, T raidou, T znear, T zfar ) {
if (uf::matrix::reverseInfiniteProjection) {
T f = static_cast<T>(1) / tan(static_cast<T>(0.5) * fov);
#if UF_USE_OPENGL
pod::Matrix4t<T> m = uf::matrix::identity<T>();
m(0,0) = f / raidou;
m(1,1) = f;
m(2,2) = 0;
m(2,3) = znear;
m(3,2) = 1;
m(3,3) = 0;
return m;
#elif UF_USE_VULKAN
pod::Matrix4t<T> m = uf::matrix::identity<T>();
m(0,0) = f / raidou;
m(1,1) = -f; // Vulkan flips Y
m(2,2) = 0;
m(2,3) = znear;
m(3,2) = 1;
m(3,3) = 0;
return m;
#endif
} else {
T range = znear - zfar;
T f = tan(static_cast<T>(0.5) * fov);
T Sx = static_cast<T>(1) / (f * raidou);
T Sy = static_cast<T>(1) / f;
T Sz = (zfar + znear) / range;
T Pz = (static_cast<T>(2) * zfar * znear) / range;
#if UF_USE_VULKAN
Sy = -Sy; // Vulkan NDC has inverted Y
#endif
pod::Matrix4t<T> m = uf::matrix::identity<T>();
m(0,0) = Sx;
m(1,1) = Sy;
m(2,2) = Sz;
m(2,3) = Pz;
m(3,2) = -1;
m(3,3) = 0;
return m;
}
#if 0
if ( uf::matrix::reverseInfiniteProjection ) {
T f = static_cast<T>(1) / tan( static_cast<T>(0.5) * fov );
#if UF_USE_OPENGL
return pod::Matrix4t<T>({
f / raidou, 0, 0, 0,
0, f, 0, 0,
0, 0, 0, 1,
0, 0, znear, 0
f / raidou, 0, 0, 0,
0, f, 0, 0,
0, 0, 0, 1,
0, 0, znear, 0
});
#elif UF_USE_VULKAN
return pod::Matrix4t<T>({
f / raidou, 0, 0, 0,
0, -f, 0, 0,
0, 0, 0, 1,
0, 0, znear, 0
f / raidou, 0, 0, 0,
0, -f, 0, 0,
0, 0, 0, 1,
0, 0, znear, 0
});
#endif
} else {
@ -618,17 +460,18 @@ pod::Matrix4t<T> /*UF_API*/ uf::matrix::perspective( T fov, T raidou, T znear, T
0, 0, Pz, 0
});
}
#endif
}
template<typename T> T& uf::matrix::copy( T& destination, const T& source ) {
#pragma unroll // GCC unroll 16
for ( uint_fast8_t i = 0; i < 16; ++i )
for ( auto i = 0; i < 16; ++i )
destination[i] = source[i];
return destination;
}
template<typename T> T& uf::matrix::copy( T& destination, typename T::type_t* const source ) {
#pragma unroll // GCC unroll 16
for ( uint_fast8_t i = 0; i < 16; ++i )
for ( auto i = 0; i < 16; ++i )
destination[i] = source[i];
return destination;
@ -650,11 +493,11 @@ ext::json::Value /*UF_API*/ uf::matrix::encode( const pod::Matrix<T,R,C>& m, con
ext::json::Value json;
if ( settings.quantize )
#pragma unroll // GCC unroll R*C
for ( uint_fast8_t i = 0; i < R*C; ++i )
for ( auto i = 0; i < R*C; ++i )
json[i] = uf::math::quantizeShort( m[i] );
else
#pragma unroll // GCC unroll R*C
for ( uint_fast8_t i = 0; i < R*C; ++i )
for ( auto i = 0; i < R*C; ++i )
json[i] = m[i];
return json;
@ -663,10 +506,10 @@ template<typename T, size_t R, size_t C>
pod::Matrix<T,R,C>& /*UF_API*/ uf::matrix::decode( const ext::json::Value& json, pod::Matrix<T,R,C>& m ) {
if ( ext::json::isArray(json) )
#pragma unroll // GCC unroll T::size
for ( uint_fast8_t i = 0; i < R*C && i < json.size(); ++i )
for ( auto i = 0; i < R*C && i < json.size(); ++i )
m[i] = json[i].as<T>(m[i]);
else if ( ext::json::isObject(json) ) {
uint_fast8_t i = 0;
auto i = 0;
ext::json::forEach(json, [&](const ext::json::Value& c){
if ( i >= R*C ) return;
m[i] = c.as<T>(m[i]);
@ -678,13 +521,13 @@ pod::Matrix<T,R,C>& /*UF_API*/ uf::matrix::decode( const ext::json::Value& json,
template<typename T, size_t R, size_t C>
pod::Matrix<T,R,C> /*UF_API*/ uf::matrix::decode( const ext::json::Value& json, const pod::Matrix<T,R,C>& _m ) {
ALIGN16 pod::Matrix<T,R,C> m = _m;
pod::Matrix<T,R,C> m = _m;
if ( ext::json::isArray(json) )
#pragma unroll // GCC unroll T::size
for ( uint_fast8_t i = 0; i < R*C && i < json.size(); ++i )
for ( auto i = 0; i < R*C && i < json.size(); ++i )
m[i] = json[i].as<T>(_m[i]);
else if ( ext::json::isObject(json) ) {
uint_fast8_t i = 0;
auto i = 0;
ext::json::forEach(json, [&](const ext::json::Value& c){
if ( i >= R*C ) return;
m[i] = c.as<T>(_m[i]);
@ -699,17 +542,13 @@ uf::stl::string /*UF_API*/ uf::matrix::toString( const pod::Matrix<T,R,C>& m ) {
uf::stl::stringstream ss;
ss << "Matrix(\n\t";
#pragma unroll // GCC unroll C
for ( uint_fast8_t c = 0; c < C; ++c ) {
for ( auto c = 0; c < C; ++c ) {
#pragma unroll // GCC unroll R
for ( uint_fast8_t r = 0; r < R; ++r ) {
for ( auto r = 0; r < R; ++r ) {
ss << m[r+c*C] << ", ";
}
if ( c + 1 < C ) ss << "\n\t";
}
ss << "\n)";
return ss.str();
}
#if !__clang__ && __GNUC__
#pragma GCC pop_options
#endif
}

95
engine/inc/uf/utils/math/physics/impl.h
View File

@ -8,6 +8,7 @@
#include <uf/utils/memory/vector.h>
#include <uf/utils/memory/unordered_map.h>
#include <uf/utils/memory/unordered_set.h>
#include <uf/engine/object/object.h>
#include <cfloat>
@ -27,9 +28,9 @@ namespace pod {
};
struct SupportPoint {
pod::Vector3f p;
pod::Vector3f pA;
pod::Vector3f pB;
alignas(16) pod::Vector3f p;
alignas(16) pod::Vector3f pA;
alignas(16) pod::Vector3f pB;
};
struct Simplex {
@ -38,16 +39,30 @@ namespace pod {
struct Face {
pod::SupportPoint a, b, c;
pod::Vector3f normal;
alignas(16) pod::Vector3f normal;
float distance;
};
struct BVH {
typedef std::pair<int32_t,int32_t> pair_t;
typedef uf::stl::vector<pair_t> pairs_t;
struct PairHash {
size_t operator()( const pair_t& p ) const noexcept {
uint64_t a = (uint64_t) std::min(p.first, p.second);
uint64_t b = (uint64_t) std::max(p.first, p.second);
return (a << 32) ^ b;
}
};
struct PairEq {
bool operator()( const pair_t& a, const pair_t& b ) const noexcept {
return (a.first == b.first && a.second == b.second) || (a.first == b.second && a.second == b.first);
}
};
typedef uf::stl::unordered_set<pair_t, PairHash, PairEq> pairs_t;
struct Node {
pod::AABB bounds = {};
alignas(16) pod::AABB bounds = {};
int32_t left = -1;
int32_t right = -1;
int32_t start = 0;
@ -56,7 +71,7 @@ namespace pod {
bool asleep = false;
};
struct FlatNode {
pod::AABB bounds = {};
alignas(16) pod::AABB bounds = {};
int32_t start = -1;
int32_t count = -1;
int32_t skipIndex = -1;
@ -65,13 +80,13 @@ namespace pod {
};
struct UpdatePolicy {
enum class Decision {
NONE, // do nothing
NONE, // do nothing
REFIT, // refit bounds
REBUILD // rebuild from scratch
};
float displacementThreshold = 0.25f; // 25% of AABB size
float overlapThreshold = 2.0f; // 2x growth in root surface area
float dirtyRatioThreshold = 0.3f; // 30% dirty bodies
float overlapThreshold = 2.0f; // 2x growth in root surface area
float dirtyRatioThreshold = 0.3f; // 30% dirty bodies
int maxFramesBeforeRebuild = 60; // force rebuild every 60 frames
};
@ -92,27 +107,27 @@ namespace pod {
struct Collider {
// what it is
enum CategoryMask : uint32_t {
CATEGORY_NONE = 0,
CATEGORY_STATIC = 1 << 0,
CATEGORY_DYNAMIC = 1 << 1,
CATEGORY_PLAYER = 1 << 2,
CATEGORY_NPC = 1 << 3,
CATEGORY_TRIGGER = 1 << 4,
CATEGORY_NONE = 0,
CATEGORY_STATIC = 1 << 0,
CATEGORY_DYNAMIC = 1 << 1,
CATEGORY_PLAYER = 1 << 2,
CATEGORY_NPC = 1 << 3,
CATEGORY_TRIGGER = 1 << 4,
CATEGORY_PROJECTILE = 1 << 5,
CATEGORY_CHARACTER = CATEGORY_PLAYER | CATEGORY_NPC,
CATEGORY_ALL = 0xFFFFFFFF
CATEGORY_ALL = 0xFFFFFFFF
};
// what it collides with
enum CollisionMask : uint32_t {
MASK_NONE = 0,
MASK_STATIC = CATEGORY_DYNAMIC | CATEGORY_PLAYER | CATEGORY_NPC | CATEGORY_PROJECTILE,
MASK_DYNAMIC = CATEGORY_STATIC | CATEGORY_DYNAMIC | CATEGORY_PLAYER | CATEGORY_NPC,
MASK_PLAYER = CATEGORY_STATIC | CATEGORY_DYNAMIC | CATEGORY_NPC | CATEGORY_PROJECTILE,
MASK_NPC = CATEGORY_STATIC | CATEGORY_DYNAMIC | CATEGORY_PLAYER | CATEGORY_PROJECTILE,
MASK_TRIGGER = CATEGORY_PLAYER | CATEGORY_NPC,
MASK_NONE = 0,
MASK_STATIC = CATEGORY_DYNAMIC | CATEGORY_PLAYER | CATEGORY_NPC | CATEGORY_PROJECTILE,
MASK_DYNAMIC = CATEGORY_STATIC | CATEGORY_DYNAMIC | CATEGORY_PLAYER | CATEGORY_NPC,
MASK_PLAYER = CATEGORY_STATIC | CATEGORY_DYNAMIC | CATEGORY_NPC | CATEGORY_PROJECTILE,
MASK_NPC = CATEGORY_STATIC | CATEGORY_DYNAMIC | CATEGORY_PLAYER | CATEGORY_PROJECTILE,
MASK_TRIGGER = CATEGORY_PLAYER | CATEGORY_NPC,
MASK_PROJECTILE = CATEGORY_STATIC | CATEGORY_DYNAMIC | CATEGORY_PLAYER | CATEGORY_NPC,
MASK_CHARACTER = MASK_PLAYER | MASK_NPC,
MASK_ALL = 0xFFFFFFFF
MASK_CHARACTER = MASK_PLAYER | MASK_NPC,
MASK_ALL = 0xFFFFFFFF
};
pod::ShapeType type;
@ -149,31 +164,30 @@ namespace pod {
struct PhysicsBody {
pod::World* world = NULL;
uf::Object* object = NULL;
// pod::Transform<> transform = {};
pod::Transform<>* transform = NULL;
pod::Vector3f offset = {};
bool isStatic = false;
float mass = 1.0f;
float inverseMass = 1.0f;
pod::Vector3f velocity = {};
pod::Vector3f forceAccumulator = {};
alignas(16) pod::Vector3f offset = {};
pod::Vector3f angularVelocity = {};
pod::Vector3f torqueAccumulator = {};
alignas(16) pod::Vector3f velocity = {};
alignas(16) pod::Vector3f forceAccumulator = {};
pod::Vector3f inertiaTensor = { 1, 1, 1 };
pod::Vector3f inverseInertiaTensor = { 1, 1, 1 };
alignas(16) pod::Vector3f angularVelocity = {};
alignas(16) pod::Vector3f torqueAccumulator = {};
pod::Vector3f gravity = { NAN, NAN, NAN }; // an invalid gravity will fallback to world gravity
alignas(16) pod::Vector3f inertiaTensor = { 1, 1, 1 };
alignas(16) pod::Vector3f inverseInertiaTensor = { 1, 1, 1 };
pod::AABB bounds;
pod::Collider collider;
pod::PhysicsMaterial material;
pod::Activity activity;
alignas(16) pod::Vector3f gravity = { NAN, NAN, NAN }; // an invalid gravity will fallback to world gravity
alignas(16) pod::AABB bounds;
alignas(16) pod::Collider collider;
alignas(16) pod::PhysicsMaterial material;
alignas(16) pod::Activity activity;
};
struct Contact {
@ -210,7 +224,8 @@ namespace pod {
uf::stl::vector<pod::PhysicsBody*> bodies;
pod::Vector3f gravity = { 0, -9.81f, 0 };
pod::BVH bvh;
pod::BVH dynamicBvh;
pod::BVH staticBvh;
};
}

162
engine/inc/uf/utils/math/quaternion.h
View File

@ -14,48 +14,43 @@
#include "math.h"
namespace pod {
// Simple quaterions (designed [to store in arrays] with minimal headaches)
template<typename T = pod::Math::num_t> using Quaternion = Vector4t<T>;
template<typename T = NUM> using Quaternion = Vector4t<T>;
}
namespace uf {
namespace quaternion {
// Equality checking
template<typename T> std::size_t /*UF_API*/ compareTo( const T& left, const T& right ); // Equality check between two vectors (less than)
template<typename T> bool /*UF_API*/ equals( const T& left, const T& right ); // Equality check between two vectors (equals)
// Basic arithmetic
template<typename T> T /*UF_API*/ multiply( const T& left, const T& right ); // Multiplies two vectors of same type and size together
template<typename T = pod::Math::num_t> pod::Quaternion<T> /*UF_API*/ identity(); // Multiplies two vectors of same type and size together
template<typename T> pod::Vector3t<T> /*UF_API*/ rotate( const pod::Quaternion<T>& left, const pod::Vector3t<T>& right ); // Multiplies two vectors of same type and size together
template<typename T> pod::Vector4t<T> /*UF_API*/ rotate( const pod::Quaternion<T>& left, const pod::Vector4t<T>& right ); // Multiplies two vectors of same type and size together
template<typename T> typename T::type_t /*UF_API*/ sum( const T& vector ); // Compute the sum of all components
template<typename T> typename T::type_t /*UF_API*/ product( const T& vector ); // Compute the product of all components
template<typename T> T /*UF_API*/ negate( const T& vector ); // Flip sign of all components
// Writes to first value
template<typename T> T& /*UF_API*/ multiply( T& left, const T& right ); // Multiplies two vectors of same type and size together
template<typename T> T& /*UF_API*/ negate( T& vector ); // Flip sign of all components
template<typename T> T& /*UF_API*/ normalize( T& vector ); // Normalizes a vector
// Complex arithmetic
template<typename T> typename T::type_t /*UF_API*/ dot( const T& left, const T& right ); // Compute the dot product between two vectors
template<typename T> pod::Angle /*UF_API*/ angle( const T& a, const T& b ); // Compute the angle between two vectors
template<typename T> T /*UF_API*/ lerp( const T& from, const T& to, typename T::type_t delta ); // Linearly interpolate between two vectors
template<typename T> T /*UF_API*/ slerp( const T& from, const T& to, typename T::type_t delta ); // Spherically interpolate between two vectors
template<typename T> typename T::type_t /*UF_API*/ distanceSquared( const T& a, const T& b ); // Gets the magnitude of the vector
template<typename T> typename T::type_t /*UF_API*/ distance( const T& a, const T& b ); // Gets the magnitude of the vector
template<typename T> typename T::type_t /*UF_API*/ magnitude( const T& vector ); // Gets the magnitude of the vector
template<typename T> typename T::type_t /*UF_API*/ norm( const T& vector ); // Compute the norm of the vector
template<typename T> T /*UF_API*/ normalize( const T& vector ); // Normalizes a vector
// Quaternion ops
template<typename T> pod::Matrix4t<typename T::type_t> matrix( const T& quaternion );
template<typename T = NUM> pod::Quaternion<T> /*UF_API*/ identity();
template<typename T> T /*UF_API*/ multiply( const T& left, const T& right );
template<typename T> pod::Vector3t<T> /*UF_API*/ rotate( const pod::Quaternion<T>& left, const pod::Vector3t<T>& right );
template<typename T> pod::Vector4t<T> /*UF_API*/ rotate( const pod::Quaternion<T>& left, const pod::Vector4t<T>& right );
template<typename T> typename T::type_t /*UF_API*/ sum( const T& vector );
template<typename T> typename T::type_t /*UF_API*/ product( const T& vector );
template<typename T> typename T::type_t /*UF_API*/ dot( const T& left, const T& right );
template<typename T> pod::Angle /*UF_API*/ angle( const T& a, const T& b );
template<typename T> T /*UF_API*/ lerp( const T& from, const T& to, typename T::type_t delta );
template<typename T> T /*UF_API*/ slerp( const T& from, const T& to, typename T::type_t delta );
template<typename T> typename T::type_t /*UF_API*/ distanceSquared( const T& a, const T& b );
template<typename T> typename T::type_t /*UF_API*/ distance( const T& a, const T& b );
template<typename T> typename T::type_t /*UF_API*/ magnitude( const T& vector );
template<typename T> typename T::type_t /*UF_API*/ norm( const T& vector );
template<typename T> T /*UF_API*/ normalize( const T& vector );
template<typename T> pod::Matrix4t<T> matrix( const pod::Quaternion<T>& quaternion );
template<typename T> pod::Quaternion<T> axisAngle( const pod::Vector3t<T>& axis, T angle );
template<typename T> pod::Quaternion<T> unitVectors( const pod::Vector3t<T>& u, const pod::Vector3t<T>& v );
template<typename T> pod::Quaternion<T> lookAt( const pod::Vector3t<T>& source, const pod::Vector3t<T>& destination );
template<typename T> T conjugate( const T& quaternion );
template<typename T> T inverse( const T& quaternion );
template<typename T> T& conjugate( T& quaternion );
template<typename T> T& inverse( T& quaternion );
template<typename T> T& /*UF_API*/ multiply_( T& left, const T& right );
template<typename T> T& /*UF_API*/ normalize_( T& vector );
template<typename T> T& conjugate_( T& quaternion );
template<typename T> T& inverse_( T& quaternion );
template<typename T> pod::Vector3t<T> eulerAngles( const pod::Quaternion<T>& quaternion );
template<typename T> T pitch( const pod::Quaternion<T>& quaternion );
@ -66,105 +61,4 @@ namespace uf {
}
}
#if UF_USE_CLASS_OF_PODS
namespace uf {
template<typename T = pod::Math::num_t>
class /*UF_API*/ Quaternion {
public:
// Easily access POD's type
typedef pod::Quaternion<T> pod_t;
// Replicate POD information
typedef T type_t;
typedef T* container_t;
static const std::size_t size = 4;
protected:
// POD storage
Quaternion<T>::pod_t m_pod;
public:
T& x = m_pod.x;
T& y = m_pod.y;
T& z = m_pod.z;
T& w = m_pod.w;
public:
// C-tor
Quaternion(); // initializes POD to 'def'
Quaternion(T def); // initializes POD to 'def'
Quaternion(T x, T y, T z, T w); // initializes POD to 'def'
Quaternion(const Quaternion<T>::pod_t& pod); // copies POD altogether
Quaternion(const T components[4]); // copies data into POD from 'components' (typed as C array)
Quaternion(const uf::stl::vector<T>& components); // copies data into POD from 'components' (typed as uf::stl::vector<T>)
// D-tor
// Unneccesary
// POD access
Quaternion<T>::pod_t& data(); // Returns a reference of POD
const Quaternion<T>::pod_t& data() const; // Returns a const-reference of POD
// Alternative POD access
T* get(); // Returns a pointer to the entire array
const T* get() const; // Returns a const-pointer to the entire array
T& getComponent( std::size_t i ); // Returns a reference to a single element
const T& getComponent( std::size_t i ) const; // Returns a const-reference to a single element
// POD manipulation
T* set(const T components[4]); // Sets the entire array
T& setComponent( std::size_t i, const T& value ); // Sets a single element
// Validation
bool isValid() const; // Checks if all components are valid (non NaN, inf, etc.)
// Basic arithmetic
inline uf::Quaternion<T>& multiply( const Quaternion<T>& quaternion ); // Multiplies two quaternions of same type and size together
inline uf::Quaternion<T> multiply( const Quaternion<T>& quaternion ) const; // Multiplies two quaternions of same type and size together
inline uf::Vector3t<T> rotate( const Vector3t<T>& quaternion ) const; // Multiplies a quaternion and a vector of same type and size together
inline uf::Vector4t<T> rotate( const Vector4t<T>& quaternion ) const; // Multiplies a quaternion and a vector of same type and size together
inline uf::Quaternion<T>& negate(); // Flip sign of all components
// Complex arithmetic
inline T dot( const Quaternion<T> right ) const; // Compute the dot product between two quaternions
inline pod::Angle angle( const Quaternion<T>& b ) const; // Compute the angle between two quaternions
inline uf::Quaternion<T> lerp( const Quaternion<T> to, typename T::type_t delta ) const; // Linearly interpolate between two quaternions
inline uf::Quaternion<T> slerp( const Quaternion<T> to, typename T::type_t delta ) const; // Spherically interpolate between two quaternions
inline T distanceSquared( const Quaternion<T> b ) const; // Compute the distance between two quaternions (doesn't sqrt)
inline T distance( const Quaternion<T> b ) const; // Compute the distance between two quaternions
inline T magnitude() const; // Gets the magnitude of the quaternion
inline T norm() const; // Compute the norm of the quaternion
inline uf::Quaternion<T>& normalize(); // Normalizes a quaternion
uf::Quaternion<T> getNormalized() const; // Return a normalized quaternion
// Quaternion ops
inline uf::Matrix4t<T> matrix() const;
inline uf::Quaternion<T>& axisAngle( const Vector3t<T>& axis, T angle );
inline uf::Quaternion<T>& unitVectors( const Vector3t<T>& u, const Vector3t<T>& v );
inline uf::Quaternion<T>& conjugate();
inline uf::Quaternion<T>& inverse();
inline uf::Quaternion<T> getConjugate() const;
inline uf::Quaternion<T> getInverse() const;
inline uf::stl::string toString() const;
// Overloaded ops
// Accessing via subscripts
T& operator[](std::size_t i);
const T& operator[](std::size_t i) const;
// Arithmetic
inline Quaternion<T> operator-() const; // Negation
inline Quaternion<T> operator*( const Quaternion<T>& quaternion ) const; // Multiplication between two quaternions
inline Vector3t<T> operator*( const Vector3t<T>& vector ) const; // Multiplication between a quaternion and a vector (rotates vector)
inline Vector4t<T> operator*( const Vector4t<T>& vector ) const; // Multiplication between a quaternion and a vector (rotates vector)
inline Matrix4t<T> operator*( const Matrix4t<T>& matrix ) const; // Multiplication between a quaternion and a matrix
inline Quaternion<T>& operator *=( const Quaternion<T>& quaternion ); // Multiplication set between two quaternions
inline bool operator==( const Quaternion<T>& quaternion ) const; // Equality check between two quaternions (equals)
inline bool operator!=( const Quaternion<T>& quaternion ) const; // Equality check between two quaternions (not equals)
inline bool operator<( const Quaternion<T>& quaternion ) const; // Equality check between two quaternions (less than)
inline bool operator<=( const Quaternion<T>& quaternion ) const; // Equality check between two quaternions (less than or equals)
inline bool operator>( const Quaternion<T>& quaternion ) const; // Equality check between two quaternions (greater than)
inline bool operator>=( const Quaternion<T>& quaternion ) const; // Equality check between two quaternions (greater than or equals)
inline operator pod_t&() { return this->m_pod; }
inline operator const pod_t&() const { return this->m_pod; }
};
}
#endif
#include "quaternion/quaternion.inl"
#ifdef UF_USE_GLM_TMP
#undef UF_USE_GLM
#undef UF_USE_GLM_TMP
#endif
#include "quaternion/quaternion.inl"

278
engine/inc/uf/utils/math/quaternion/class.inl
View File

@ -1,278 +0,0 @@
// C-tor
// initializes POD to 'def'
template<typename T>
uf::Quaternion<T>::Quaternion() {
}
// initializes POD to 'def'
template<typename T>
uf::Quaternion<T>::Quaternion(T def) :
m_pod( {def, def, def, 1} )
{
}
// initializes POD to 'def'
template<typename T>
uf::Quaternion<T>::Quaternion(T x, T y, T z, T w) :
m_pod( {x, y, z, w} )
{
}
// copies POD altogether
template<typename T>
uf::Quaternion<T>::Quaternion(const Quaternion<T>::pod_t& pod) :
m_pod(pod)
{
}
// copies data into POD from 'components' (typed as C array)
template<typename T>
uf::Quaternion<T>::Quaternion(const T components[4] ) :
m_pod( { components[0], components[1], components[2], components[3] } )
{
}
// copies data into POD from 'components' (typed as uf::stl::vector<T>)
template<typename T>
uf::Quaternion<T>::Quaternion(const uf::stl::vector<T>& components) {
memcpy( this->m_pod.components, &components[0], 4 );
}
// D-tor
// Unneccesary
// POD access
// Returns a reference of POD
template<typename T>
typename uf::Quaternion<T>::pod_t& uf::Quaternion<T>::data() {
return this->m_pod;
}
// Returns a const-reference of POD
template<typename T>
const typename uf::Quaternion<T>::pod_t& uf::Quaternion<T>::data() const {
return this->m_pod;
}
// Alternative POD access
// Returns a pointer to the entire array
template<typename T>
T* uf::Quaternion<T>::get() {
return this->m_pod.components;
}
// Returns a const-pointer to the entire array
template<typename T>
const T* uf::Quaternion<T>::get() const {
return this->m_pod.components;
}
// Returns a reference to a single element
template<typename T>
T& uf::Quaternion<T>::getComponent( std::size_t i ) {
return this->m_pod.components[i];
}
// Returns a const-reference to a single element
template<typename T>
const T& uf::Quaternion<T>::getComponent( std::size_t i ) const {
return this->m_pod.components[i];
}
// POD manipulation
// Sets the entire array
template<typename T>
T* uf::Quaternion<T>::set(const T components[4]) {
for ( std::size_t i = 0; i < 4; ++i ) this->m_pod[i] = components[i];
}
// Sets a single element
template<typename T>
T& uf::Quaternion<T>::setComponent( std::size_t i, const T& value ) {
this->m_pod[i] = value;
}
// Validation
// Checks if all components are valid (non NaN, inf, etc.)
template<typename T>
bool uf::Quaternion<T>::isValid() const {
for ( std::size_t i = 0; i < 4; ++i ) if ( this->m_pod[i] != this->m_pod[i] ) return false;
return true;
}
// Basic arithmetic
// Multiplies two quaternions of same type and size together
template<typename T>
inline uf::Quaternion<T>& uf::Quaternion<T>::multiply( const Quaternion<T>& b ) {
uf::quaternion::multiply(this->m_pod, b.m_pod);
return *this;
}
template<typename T>
inline uf::Quaternion<T> uf::Quaternion<T>::multiply( const Quaternion<T>& b ) const {
return uf::quaternion::multiply(this->m_pod, b.m_pod);
}
template<typename T>
inline uf::Vector3t<T> uf::Quaternion<T>::rotate( const Vector3t<T>& b ) const {
return uf::quaternion::rotate(this->m_pod, b.data());
}
template<typename T>
inline uf::Vector4t<T> uf::Quaternion<T>::rotate( const Vector4t<T>& b ) const {
return uf::quaternion::rotate(this->m_pod, b.data());
}
// Flip sign of all components
template<typename T>
inline uf::Quaternion<T>& uf::Quaternion<T>::negate() {
uf::quaternion::negate(this->m_pod);
return *this;
}
// Complex arithmetic
// Compute the dot product between two quaternions
template<typename T>
inline T uf::Quaternion<T>::dot( const Quaternion<T> right ) const {
return uf::quaternion::dot( this->m_pod, right.m_pod );
}
// Compute the angle between two quaternions
template<typename T>
inline pod::Angle uf::Quaternion<T>::angle( const Quaternion<T>& b ) const {
return uf::quaternion::angle( this->m_pod, b.m_pod );
}
// Linearly interpolate between two quaternions
template<typename T>
inline uf::Quaternion<T> uf::Quaternion<T>::lerp( const Quaternion<T> to, typename T::type_t delta ) const {
return uf::quaternion::lerp( this->m_pod, to.m_pod, delta );
}
// Spherically interpolate between two quaternions
template<typename T>
inline uf::Quaternion<T> uf::Quaternion<T>::slerp( const Quaternion<T> to, typename T::type_t delta ) const {
return uf::quaternion::slerp( this->m_pod, to.m_pod, delta );
}
// Compute the distance between two quaternions (doesn't sqrt)
template<typename T>
inline T uf::Quaternion<T>::distanceSquared( const Quaternion<T> b ) const {
return uf::quaternion::distanceSquared(this->m_pod, b.m_pod);
}
// Compute the distance between two quaternions
template<typename T>
inline T uf::Quaternion<T>::distance( const Quaternion<T> b ) const {
return uf::quaternion::distance(this->m_pod, b.m_pod);
}
// Gets the magnitude of the quaternion
template<typename T>
inline T uf::Quaternion<T>::magnitude() const {
return uf::quaternion::magnitude(this->m_pod);
}
// Compute the norm of the quaternion
template<typename T>
inline T uf::Quaternion<T>::norm() const {
return uf::quaternion::norm(this->m_pod);
}
// Normalizes a quaternion
template<typename T>
inline uf::Quaternion<T>& uf::Quaternion<T>::normalize() {
uf::quaternion::normalize(this->m_pod);
return *this;
}
// Return a normalized quaternion
template<typename T>
uf::Quaternion<T> uf::Quaternion<T>::getNormalized() const {
return uf::quaternion::normalize(this->m_pod);
}
// Quaternion ops
template<typename T>
inline uf::Matrix4t<T> uf::Quaternion<T>::matrix() const {
return uf::quaternion::matrix(this->m_pod);
}
template<typename T>
inline uf::Quaternion<T>& uf::Quaternion<T>::axisAngle( const Vector3t<T>& axis, T angle ) {
this->m_pod = uf::quaternion::axisAngle(axis, angle);
return *this;
}
template<typename T>
inline uf::Quaternion<T>& uf::Quaternion<T>::unitVectors( const Vector3t<T>& u, const Vector3t<T>& v ) {
this->m_pod = uf::quaternion::unitVectors(u, v);
return *this;
}
template<typename T>
inline uf::Quaternion<T>& uf::Quaternion<T>::conjugate() {
return uf::quaternion::conjugate(this->m_pod);
}
template<typename T>
inline uf::Quaternion<T>& uf::Quaternion<T>::inverse() {
return uf::quaternion::inverse(this->m_pod);
}
template<typename T>
inline uf::Quaternion<T> uf::Quaternion<T>::getConjugate() const {
return uf::quaternion::conjugate(this->m_pod);
}
template<typename T>
inline uf::Quaternion<T> uf::Quaternion<T>::getInverse() const {
return uf::quaternion::inverse(this->m_pod);
}
template<typename T>
inline uf::stl::string uf::Quaternion<T>::toString() const {
return uf::vector::toString(this->m_pod);
}
// Overloaded ops
// Accessing via subscripts
template<typename T>
T& uf::Quaternion<T>::operator[](std::size_t i) {
return this->m_pod[i];
}
template<typename T>
const T& uf::Quaternion<T>::operator[](std::size_t i) const {
return this->m_pod[i];
}
// Arithmetic
// Negation
template<typename T>
inline uf::Quaternion<T> uf::Quaternion<T>::operator-() const {
return this->negate();
}
// Multiplication between two quaternions
template<typename T>
inline uf::Quaternion<T> uf::Quaternion<T>::operator*( const Quaternion<T>& quaternion ) const {
return this->multiply(quaternion);
}
// Multiplication between a quaternion and a vector (rotates vector)
template<typename T>
inline uf::Vector3t<T> uf::Quaternion<T>::operator*( const Vector3t<T>& vector ) const {
return this->multiply(vector);
}
template<typename T>
inline uf::Vector4t<T> uf::Quaternion<T>::operator*( const Vector4t<T>& vector ) const {
return this->multiply(vector);
}
// Multiplication between a quaternion and a matrix
template<typename T>
inline uf::Matrix4t<T> uf::Quaternion<T>::operator*( const Matrix4t<T>& matrix ) const {
return this->multiply(matrix);
}
// Multiplication set between two quaternions
template<typename T>
inline uf::Quaternion<T>& uf::Quaternion<T>::operator *=( const Quaternion<T>& quaternion ) {
return this->multiply(quaternion);
}
// Equality check between two quaternions (equals)
template<typename T>
inline bool uf::Quaternion<T>::operator==( const Quaternion<T>& quaternion ) const {
return uf::quaternion::equals(this->m_pod, quaternion.m_pod);
}
// Equality check between two quaternions (not equals)
template<typename T>
inline bool uf::Quaternion<T>::operator!=( const Quaternion<T>& quaternion ) const {
return !uf::quaternion::equals(this->m_pod, quaternion.m_pod);
}
// Equality check between two quaternions (less than)
template<typename T>
inline bool uf::Quaternion<T>::operator<( const Quaternion<T>& quaternion ) const {
return !(uf::quaternion::compareTo(this->m_pod, quaternion.m_pod) < 0);
}
// Equality check between two quaternions (less than or equals)
template<typename T>
inline bool uf::Quaternion<T>::operator<=( const Quaternion<T>& quaternion ) const {
return !(uf::quaternion::compareTo(this->m_pod, quaternion.m_pod) <= 0);
}
// Equality check between two quaternions (greater than)
template<typename T>
inline bool uf::Quaternion<T>::operator>( const Quaternion<T>& quaternion ) const {
return !(uf::quaternion::compareTo(this->m_pod, quaternion.m_pod) > 0);
}
// Equality check between two quaternions (greater than or equals)
template<typename T>
inline bool uf::Quaternion<T>::operator>=( const Quaternion<T>& quaternion ) const {
return !(uf::quaternion::compareTo(this->m_pod, quaternion.m_pod) >= 0);
}

358
engine/inc/uf/utils/math/quaternion/pod.inl
View File

@ -1,69 +1,43 @@
// Equality checking
// Equality check between two quaternions (less than)
template<typename T> size_t uf::quaternion::compareTo( const T& left, const T& right ) {
return uf::vector::compareTo(left, right);
// return uf::quaternion::angle(left) > uf::quaternion::angle(right);
namespace pod {
// Simple quaterions (designed [to store in arrays] with minimal headaches)
template<typename T = NUM> using Quaternion = Vector4t<T>;
}
// Equality check between two quaternions (equals)
template<typename T> bool uf::quaternion::equals( const T& left, const T& right ) {
return uf::quaternion::compareTo( left, right ) == 0;
}
// Basic arithmetic
// Multiplies two quaternions of same type and size together
template<typename T> T uf::quaternion::multiply( const T& left, const T& right ) {
T q1 = uf::quaternion::normalize(left);
T q2 = uf::quaternion::normalize(right);
T q;
q.x = q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y;
q.y = q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z;
q.z = q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x;
q.w = q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z;
return uf::quaternion::normalize( q );
}
// Multiplies this quaternion by a scalar
/*
template<typename T> T uf::quaternion::multiply( const T& quaternion, const typename T::type_t& scalar ) {
return uf::vector::multiply( quaternion, scalar );
}
*/
// Flip sign of all components
template<typename T> T uf::quaternion::negate( const T& quaternion ) {
return uf::quaternion::inverse(quaternion);
}
// snip header
template<typename T> pod::Quaternion<T> uf::quaternion::identity() {
return pod::Quaternion<T>{ 0, 0, 0, 1 };
}
// Writes to first value
// Multiplies two quaternions of same type and size together
template<typename T> T& uf::quaternion::multiply( T& left, const T& right ) {
return left = uf::quaternion::multiply((const T&) left, right );
template<typename T> T uf::quaternion::multiply( const T& q1, const T& q2 ) {
#if 0 && UF_USE_SIMD
if constexpr (std::is_same_v<typename T::type_t, float>) {
return uf::simd::quatMul( q1 , q2 );
}
#endif
return {
q1.w * q2.x + q1.x * q2.w + q1.y * q2.z - q1.z * q2.y,
q1.w * q2.y + q1.y * q2.w + q1.z * q2.x - q1.x * q2.z,
q1.w * q2.z + q1.z * q2.w + q1.x * q2.y - q1.y * q2.x,
q1.w * q2.w - q1.x * q2.x - q1.y * q2.y - q1.z * q2.z,
};
}
// Multiplies a quaternion and a vector of same type and size together
template<typename T> pod::Vector3t<T> uf::quaternion::rotate( const pod::Quaternion<T>& left, const pod::Vector3t<T>& right ) {
pod::Vector3t<T> qVec = { left.x, left.y, left.z };
const T s = left.w;
return uf::vector::multiply( qVec, static_cast<T>(2) * uf::vector::dot( qVec, right ) ) + uf::vector::multiply( right, s*s - uf::vector::dot( qVec, qVec )) + ( uf::vector::cross( qVec, right ) * static_cast<T>(2) * s );
template<typename T> pod::Vector3t<T> uf::quaternion::rotate( const pod::Quaternion<T>& Q, const pod::Vector3t<T>& v ) {
#if 0 && UF_USE_SIMD
if constexpr (std::is_same_v<T,float>) {
return uf::simd::quatRot( Q, v );
}
#endif
pod::Vector3t<T> q = { Q.x, Q.y, Q.z };
const T s = Q.w;
return uf::vector::multiply(q, static_cast<T>(2) * uf::vector::dot(q, v)) + uf::vector::multiply(v, s*s - uf::vector::dot(q, q)) + (uf::vector::cross(q, v) * static_cast<T>(2) * s);
}
template<typename T> pod::Vector4t<T> uf::quaternion::rotate( const pod::Quaternion<T>& left, const pod::Vector4t<T>& right ) {
pod::Vector3t<T> vector = uf::quaternion::rotate( left, {right.x, right.y, right.z} );
return {vector.x, vector.y, vector.z, left.w};
template<typename T> pod::Vector4t<T> uf::quaternion::rotate( const pod::Quaternion<T>& q, const pod::Vector4t<T>& v ) {
pod::Vector3t<T> vector = uf::quaternion::rotate(q, { v.x, v.y, v.z });
return { vector.x, vector.y, vector.z, v.w };
}
// Flip sign of all components
template<typename T> T& uf::quaternion::negate( T& quaternion ) {
return quaternion = uf::quaternion::negate((const T&) quaternion);
}
// Normalizes a quaternion
template<typename T> T& uf::quaternion::normalize( T& quaternion ) {
return quaternion = uf::quaternion::normalize((const T&) quaternion);
}
// Complex arithmetic
// Compute the dot product between two quaternions
template<typename T> typename T::type_t uf::quaternion::dot( const T& left, const T& right ) {
return uf::vector::dot(left, right);
}
// Compute the angle between two quaternions
template<typename T> pod::Angle uf::quaternion::angle( const T& a, const T& b ) {
T tmp = b * uf::quaternion::inverse(a);
return acosf(tmp.w) * static_cast<typename T::type_t>(2);
@ -76,103 +50,80 @@ template<typename T> pod::Vector3t<T> uf::quaternion::eulerAngles( const pod::Qu
};
}
template<typename T> T uf::quaternion::pitch( const pod::Quaternion<T>& q ) {
T const y = static_cast<T>(2) * (q.y * q.z + q.w * q.x);
#if UF_USE_SIMD
uf::simd::value Q = q;
pod::Quaternion<T> s = uf::simd::mul( Q, Q );
T const x = s.w - s.x - s.y - s.z;
#else
T const x = q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z;
#endif
const T y = static_cast<T>(2) * (q.y * q.z + q.w * q.x);
auto s = uf::vector::multiply( q, q );
const T x = s.w - s.x - s.y - s.z;
T epsilon = std::numeric_limits<T>::epsilon();
if ( fabs(x) < epsilon && fabs(y) < epsilon ) //avoid atan2(0,0) - handle singularity - Matiis
return static_cast<T>(static_cast<T>(2) * atan2(q.x, q.w));
if ( fabs(x) < epsilon && fabs(y) < epsilon ) return static_cast<T>(static_cast<T>(2) * atan2(q.x, q.w));
return static_cast<T>(atan2(y, x));
}
template<typename T> T uf::quaternion::yaw( const pod::Quaternion<T>& q ) {
return asin(std::clamp(static_cast<T>(-2) * (q.x * q.z - q.w * q.y), static_cast<T>(-1), static_cast<T>(1)));
}
template<typename T> T uf::quaternion::roll( const pod::Quaternion<T>& q ) {
T const y = static_cast<T>(2) * (q.x * q.y + q.w * q.z);
#if UF_USE_SIMD
uf::simd::value Q = q;
pod::Quaternion<T> s = uf::simd::mul( Q, Q );
T const x = s.w - s.x - s.y - s.z;
#else
T const x = q.w * q.w - q.x * q.x - q.y * q.y + q.z * q.z;
#endif
const T y = static_cast<T>(2) * (q.x * q.y + q.w * q.z);
auto s = uf::vector::multiply( q, q );
const T x = s.w - s.x - s.y - s.z;
T epsilon = std::numeric_limits<T>::epsilon();
if ( fabs(x) < epsilon && fabs(y) < epsilon ) //avoid atan2(0,0) - handle singularity - Matiis
return static_cast<T>(0);
if ( fabs(x) < epsilon && fabs(y) < epsilon ) return static_cast<T>(0);
return static_cast<T>(atan2(y, x));
}
// Linearly interpolate between two quaternions
template<typename T> T uf::quaternion::lerp( const T& from, const T& to, typename T::type_t delta ) {
return uf::vector::lerp( from, to, delta );
}
// Spherically interpolate between two quaternions
template<typename T> T uf::quaternion::slerp( const T& x, const T& y, typename T::type_t a ) {
T z = y;
auto cosTheta = uf::quaternion::dot( x, y );
if( cosTheta < 0 ) {
auto cosTheta = uf::quaternion::dot(x, y);
if ( cosTheta < 0 ) {
z = -y;
cosTheta = -cosTheta;
}
if( cosTheta > 1 - std::numeric_limits<typename T::type_t>::epsilon() ) return uf::vector::mix( x, z, a );
typename T::type_t angle = acos(cosTheta);
return uf::vector::divide( uf::vector::add(uf::vector::multiply(x, sin(static_cast<typename T::type_t>(1) - a * angle)), uf::vector::multiply(z, sin(a * angle))), sin(angle) );
// return (x * sin((static_cast<typename T::type_t>(1) - a) * angle) + z * sin(a * angle)) / sin(angle);
}
if (cosTheta > 1 - std::numeric_limits<typename T::type_t>::epsilon()) return uf::vector::mix(x, z, a);
// Compute the distance between two quaternions (doesn't sqrt)
typename T::type_t angle = acos(cosTheta);
// return ( sin( ( 1 - a ) * angle) * x + sin( a * angle ) * y ) / sin( angle );
return uf::vector::divide( uf::vector::add( uf::vector::multiply(x, sin((1 - a) * angle)), uf::vector::multiply(z, sin(a * angle)) ), sin( angle ) );
}
template<typename T> typename T::type_t uf::quaternion::distanceSquared( const T& a, const T& b ) {
return uf::vector::distanceSquared(a, b);
}
// Compute the distance between two quaternions
template<typename T> typename T::type_t uf::quaternion::distance( const T& a, const T& b ) {
return uf::vector::distance(a, b);
}
// Gets the magnitude of the quaternion
template<typename T> typename T::type_t uf::quaternion::magnitude( const T& quaternion ) {
return uf::vector::magnitude(quaternion);
}
// Compute the norm of the quaternion
template<typename T> typename T::type_t uf::quaternion::norm( const T& quaternion ) {
return uf::vector::norm(quaternion);
}
// Normalizes a quaternion
template<typename T> T uf::quaternion::normalize( const T& quaternion ) {
return uf::vector::normalize(quaternion);
}
template<typename T> pod::Matrix4t<T> uf::quaternion::matrix( const pod::Quaternion<T>& q ) {
#if UF_USE_SIMD
if constexpr ( std::is_same_v<T,float> ) {
return uf::simd::quatMat( q );
}
#endif
auto normal = uf::quaternion::normalize(q);
// Quaternion ops
template<typename T> pod::Matrix4t<typename T::type_t> uf::quaternion::matrix( const T& q ) {
T normal = uf::quaternion::normalize( q );
const T xx = 2 * normal.x * normal.x;
const T xy = 2 * normal.x * normal.y;
const T xz = 2 * normal.x * normal.z;
const T xw = 2 * normal.x * normal.w;
const typename T::type_t xx = 2 * normal.x * normal.x;
const typename T::type_t xy = 2 * normal.x * normal.y;
const typename T::type_t xz = 2 * normal.x * normal.z;
const typename T::type_t xw = 2 * normal.x * -normal.w;
const T yy = 2 * normal.y * normal.y;
const T yz = 2 * normal.y * normal.z;
const T yw = 2 * normal.y * normal.w;
const typename T::type_t yy = 2 * normal.y * normal.y;
const typename T::type_t yz = 2 * normal.y * normal.z;
const typename T::type_t yw = 2 * normal.y * -normal.w;
const T zz = 2 * normal.z * normal.z;
const T zw = 2 * normal.z * normal.w;
const typename T::type_t zz = 2 * normal.z * normal.z;
const typename T::type_t zw = 2 * normal.z * -normal.w;
// const typename T::type_t ww = w * w;
return pod::Matrix4t<typename T::type_t>({
1 - yy - zz, xy - zw, xz + yw, 0,
xy + zw, 1 - xx - zz, yz - xw, 0,
xz - yw, yz + xw, 1 - xx - yy, 0,
0, 0, 0, 1
return pod::Matrix4t<T>({
1 - yy - zz, xy + zw, xz - yw, 0,
xy - zw, 1 - xx - zz, yz + xw, 0,
xz + yw, yz - xw, 1 - xx - yy, 0,
0, 0, 0, 1
});
}
template<typename T> pod::Quaternion<T> uf::quaternion::axisAngle( const pod::Vector3t<T>& axis, T angle ) {
@ -180,112 +131,105 @@ template<typename T> pod::Quaternion<T> uf::quaternion::axisAngle( const pod::Ve
T sinAngle = sin( angle * static_cast<T>(0.5) );
T cosAngle = cos( angle * static_cast<T>(0.5) );
#if UF_USE_SIMD
q = uf::simd::mul( uf::simd::value(axis.x, axis.y, axis.z, static_cast<T>(1) ), uf::simd::value(sinAngle, sinAngle, sinAngle, cosAngle) );
#else
q.x = axis.x * sinAngle;
q.y = axis.y * sinAngle;
q.z = axis.z * sinAngle;
q.w = cosAngle;
#endif
uf::quaternion::normalize(q);
return q;
q = pod::Vector4t<T>{ axis.x, axis.y, axis.z, 1 } * pod::Vector4t<T>{ sinAngle, sinAngle, sinAngle, cosAngle };
return uf::quaternion::normalize( q );
}
template<typename T> pod::Quaternion<T> uf::quaternion::unitVectors( const pod::Vector3t<T>& u, const pod::Vector3t<T>& v ) {
T dot = uf::vector::dot(u, v);
static const T EPSILON = static_cast<T>(0.00001);
if ( dot + 1 < EPSILON ) return uf::quaternion::axisAngle( uf::vector::normalize(u), static_cast<T>(3.1415926) );
T mag = sqrt( static_cast<T>(2) + static_cast<T>(2) * dot );
pod::Vector3t<T> w = uf::vector::multiply(uf::vector::cross(u, v), (static_cast<T>(1) / mag));
return {
.x = w.x,
.y = w.y,
.z = w.z,
.w = mag * static_cast<T>(0.5)
};
static const T EPSILON = static_cast<T>(1e-6);
pod::Vector3t<T> uNorm = uf::vector::normalize( u );
pod::Vector3t<T> vNorm = uf::vector::normalize( v );
T dot = uf::vector::dot( uNorm, vNorm );
if ( dot < -1 + EPSILON ) {
pod::Vector3t<T> orthogonal = (fabs(uNorm.x) > fabs(uNorm.z)) ? pod::Vector3t<T>{ -uNorm.y, uNorm.x, 0 } : pod::Vector3t<T>{ 0, -uNorm.z, uNorm.y };
orthogonal = uf::vector::normalize( orthogonal );
return uf::quaternion::axisAngle( orthogonal, static_cast<T>(M_PI) );
}
pod::Vector3t<T> cross = uf::vector::cross(uNorm, vNorm);
T s = sqrt((1 + dot) * 2);
return uf::quaternion::normalize({
.x = cross.x / s,
.y = cross.y / s,
.z = cross.z / s,
.w = s * static_cast<T>(0.5)
});
}
template<typename T> pod::Quaternion<T> uf::quaternion::lookAt( const pod::Vector3t<T>& at, const pod::Vector3t<T>& _up ) {
pod::Vector3t<T> up = _up;
pod::Vector3t<T> forward = uf::vector::normalize( at ) ;
uf::vector::orthonormalize( up, forward );
pod::Vector3t<T> right = uf::vector::cross( up, forward );
pod::Quaternion<T> q;
#if UF_USE_SIMD
T w = sqrtf(static_cast<T>(1) + right.x + up.y + forward.z) * static_cast<T>(0.5);
float w4_recip = static_cast<T>(1) / (static_cast<T>(4) * w);
q = uf::simd::mul( uf::simd::sub( uf::simd::value( forward.y, right.z, up.x, static_cast<T>(0) ), uf::simd::value( up.z, forward.x, right.y, static_cast<T>(0) ) ), w4_recip );
q.w = w;
#else
q.w = sqrtf(static_cast<T>(1) + right.x + up.y + forward.z) * static_cast<T>(0.5);
float w4_recip = static_cast<T>(1) / (static_cast<T>(4) * q.w);
q.x = (forward.y - up.z) * w4_recip;
q.y = (right.z - forward.x) * w4_recip;
q.z = (up.x - right.y) * w4_recip;
#endif
return uf::quaternion::inverse( uf::quaternion::normalize( q ) );
// return q;
pod::Vector3t<T> forward = uf::vector::normalize(at);
pod::Vector3t<T> up = uf::vector::orthonormalize( _up, forward );
pod::Vector3t<T> right = uf::vector::cross(up, forward);
pod::Matrix4t<T> m({
right.x, up.x, forward.x, 0,
right.y, up.y, forward.y, 0,
right.z, up.z, forward.z, 0,
0, 0, 0, 1
});
return uf::quaternion::normalize( uf::quaternion::fromMatrix( m ) );
}
template<typename T> T& uf::quaternion::conjugate( T& q ) {
#if UF_USE_SIMD
return q = uf::simd::mul( q, static_cast<typename T::type_t>(-1) );
#endif
return q = {
.x = -q.x,
.y = -q.y,
.z = -q.z,
.w = q.w
};
}
template<typename T> T uf::quaternion::conjugate( const T& q ) {
#if UF_USE_SIMD
return uf::simd::mul( q, static_cast<typename T::type_t>(-1) );
#endif
return {
.x = -q.x,
.y = -q.y,
.z = -q.z,
.w = q.w
};
}
template<typename T> T& uf::quaternion::inverse( T& q ) {
#if UF_USE_SIMD
uf::simd::value Q = q;
return q = uf::simd::div( uf::simd::mul( Q, { static_cast<typename T::type_t>(-1), static_cast<typename T::type_t>(-1), static_cast<typename T::type_t>(-1), static_cast<typename T::type_t>(1) } ), uf::simd::dot( Q, Q ) );
#endif
return q = uf::quaternion::conjugate( (const T&) q ) / uf::quaternion::dot( q, q );
return uf::vector::multiply( q, { -1, -1, -1, 1 } );
}
template<typename T> T uf::quaternion::inverse( const T& q ) {
#if UF_USE_SIMD
uf::simd::value Q = q;
return uf::simd::div( uf::simd::mul( Q, { static_cast<typename T::type_t>(-1), static_cast<typename T::type_t>(-1), static_cast<typename T::type_t>(-1), static_cast<typename T::type_t>(1) } ), uf::simd::dot( Q, Q ) );
#endif
return uf::quaternion::conjugate( q ) / uf::quaternion::dot( q, q );
}
template<typename T> T& uf::quaternion::multiply_( T& left, const T& right ) {
return left = uf::quaternion::multiply((const T&) left, right );
}
template<typename T> T& uf::quaternion::normalize_( T& q ) {
return q = uf::quaternion::normalize((const T&) q);
}
template<typename T> T& uf::quaternion::conjugate_( T& q ) {
return q = uf::quaternion::conjugate((const T&) q);
}
template<typename T> T& uf::quaternion::inverse_( T& q ) {
return q = uf::quaternion::inverse((const T&) q);
}
template<typename T> pod::Quaternion<T> uf::quaternion::fromMatrix( const pod::Matrix4t<T>& m ) {
pod::Quaternion<T> q;
T m0 = m[(4*0)+0];
T m5 = m[(4*1)+1];
T m10 = m[(4*2)+2];
#if UF_USE_SIMD
q = uf::simd::div( uf::simd::sqrt( uf::simd::max( static_cast<T>(0), uf::simd::add( uf::simd::add( uf::simd::add( static_cast<T>(1), uf::simd::value( m0, m0, -m0, -m0 ) ), uf::simd::value( m5, -m5, -m5, -m5 ) ), { m10, -m10, -m10, m10 } ) ) ), 2.0f );
pod::Vector4f signs = uf::simd::sub( uf::simd::value( m[(4*1)+2], m[(4*2)+0], m[(4*0)+1], static_cast<T>(0) ), uf::simd::value( m[(4*2)+1], m[(4*0)+2], m[(4*1)+0], 0.0f ) );
return {
copysign( q.x, signs.x ),
copysign( q.y, signs.y ),
copysign( q.z, signs.z ),
q.w
};
#else
q.w = sqrt(fmax(0, 1 + m0 + m5 + m10)) * static_cast<T>(0.5);
q.x = sqrt(fmax(0, 1 + m0 - m5 - m10)) * static_cast<T>(0.5);
q.y = sqrt(fmax(0, 1 - m0 + m5 - m10)) * static_cast<T>(0.5);
q.z = sqrt(fmax(0, 1 - m0 - m5 + m10)) * static_cast<T>(0.5);
T m00 = m[0], m01 = m[1], m02 = m[2];
T m10 = m[4], m11 = m[5], m12 = m[6];
T m20 = m[8], m21 = m[9], m22 = m[10];
q.x = copysign(q.x, m[(4*1)+2] - m[(4*2)+1]);
q.y = copysign(q.y, m[(4*2)+0] - m[(4*0)+2]);
q.z = copysign(q.z, m[(4*0)+1] - m[(4*1)+0]);
#endif
return q;
T trace = m00 + m11 + m22;
if ( trace > 0 ) {
T s = sqrt(trace + 1) * static_cast<T>(2);
q.w = static_cast<T>(0.25) * s;
q.x = (m21 - m12) / s;
q.y = (m02 - m20) / s;
q.z = (m10 - m01) / s;
}
else if ( m00 > m11 && m00 > m22 ) {
T s = sqrt(1 + m00 - m11 - m22) * static_cast<T>(2);
q.w = (m21 - m12) / s;
q.x = static_cast<T>(0.25) * s;
q.y = (m01 + m10) / s;
q.z = (m02 + m20) / s;
}
else if ( m11 > m22 ) {
T s = sqrt(1 + m11 - m00 - m22) * static_cast<T>(2);
q.w = (m02 - m20) / s;
q.x = (m01 + m10) / s;
q.y = static_cast<T>(0.25) * s;
q.z = (m12 + m21) / s;
}
else {
T s = sqrt(1 + m22 - m00 - m11) * static_cast<T>(2);
q.w = (m10 - m01) / s;
q.x = (m02 + m20) / s;
q.y = (m12 + m21) / s;
q.z = static_cast<T>(0.25) * s;
}
return uf::quaternion::normalize(q);
}

9
engine/inc/uf/utils/math/quaternion/quaternion.inl
View File

@ -1,6 +1,7 @@
#pragma once
#include "pod.inl"
#if UF_USE_CLASS_OF_PODS
#include "class.inl"
#endif
#if UF_USE_SIMD
#include "simd.inl"
#endif
#include "pod.inl"

146
engine/inc/uf/utils/math/quaternion/simd.inl Normal file
View File

@ -0,0 +1,146 @@
namespace uf {
namespace simd {
inline value<float> /*UF_API*/ quatMul( value<float>, value<float> );
inline value<float> /*UF_API*/ quatRot( value<float>, value<float> );
inline pod::Matrix4f /*UF_API*/ quatMat( value<float> );
}
}
inline uf::simd::value<float> uf::simd::quatMul( uf::simd::value<float> Q1, uf::simd::value<float> Q2 ) {
//__m128 Q1 = q1;
//__m128 Q2 = q2;
// Broadcast q1.w, q1.x, q1.y, q1.z
__m128 q1w = _mm_shuffle_ps(Q1, Q1, _MM_SHUFFLE(3,3,3,3));
__m128 q1x = _mm_shuffle_ps(Q1, Q1, _MM_SHUFFLE(0,0,0,0));
__m128 q1y = _mm_shuffle_ps(Q1, Q1, _MM_SHUFFLE(1,1,1,1));
__m128 q1z = _mm_shuffle_ps(Q1, Q1, _MM_SHUFFLE(2,2,2,2));
// Shuffle q2 into (x,y,z,w) permutations
__m128 q2xyzw = Q2; // (x,y,z,w)
__m128 q2wzyx = _mm_shuffle_ps(Q2, Q2, _MM_SHUFFLE(0,1,2,3)); // (w,z,y,x)
__m128 q2yzxw = _mm_shuffle_ps(Q2, Q2, _MM_SHUFFLE(3,0,2,1)); // (y,z,x,w)
__m128 q2zxyw = _mm_shuffle_ps(Q2, Q2, _MM_SHUFFLE(3,1,0,2)); // (z,x,y,w)
// Compute terms
__m128 t0 = _mm_mul_ps(q1w, q2xyzw); // w1 * (x2,y2,z2,w2)
__m128 t1 = _mm_mul_ps(q1x, q2wzyx); // x1 * (w2,z2,y2,x2)
__m128 t2 = _mm_mul_ps(q1y, q2yzxw); // y1 * (y2,z2,x2,w2)
__m128 t3 = _mm_mul_ps(q1z, q2zxyw); // z1 * (z2,x2,y2,w2)
// Signs: (+,+,+,+), (+,-,+,-), (-,+,-,+), (+,-,-,+)
const __m128 sign1 = _mm_set_ps( 1.f,-1.f, 1.f,-1.f);
const __m128 sign2 = _mm_set_ps(-1.f, 1.f,-1.f, 1.f);
const __m128 sign3 = _mm_set_ps( 1.f,-1.f,-1.f, 1.f);
t1 = _mm_mul_ps(t1, sign1);
t2 = _mm_mul_ps(t2, sign2);
t3 = _mm_mul_ps(t3, sign3);
__m128 result = _mm_add_ps(_mm_add_ps(t0, t1), _mm_add_ps(t2, t3));
return result;
}
inline uf::simd::value<float> uf::simd::quatRot( uf::simd::value<float> Q, uf::simd::value<float> V ) {
//__m128 Q = q; // (x,y,z,w)
//__m128 V = v; // (vx,vy,vz,0)
// Extract q.xyz and q.w
__m128 qxyz = _mm_and_ps(Q, _mm_castsi128_ps(_mm_set_epi32(0, -1, -1, -1))); // mask out w
__m128 qw = _mm_shuffle_ps(Q, Q, _MM_SHUFFLE(3,3,3,3));
// dot(q.xyz, v)
#if SSE_INSTR_SET >= 4
__m128 dot_qv = _mm_dp_ps(qxyz, V, 0x71); // result in lowest lane
#else
__m128 mul = _mm_mul_ps(qxyz, V);
__m128 shuf = _mm_movehdup_ps(mul);
__m128 sums = _mm_add_ps(mul, shuf);
shuf = _mm_movehl_ps(shuf, sums);
sums = _mm_add_ss(sums, shuf);
__m128 dot_qv = sums;
#endif
__m128 term1 = _mm_mul_ps(_mm_mul_ps(dot_qv, _mm_set1_ps(2.0f)), qxyz);
// dot(q.xyz, q.xyz)
#if SSE_INSTR_SET >= 4
__m128 dot_qq = _mm_dp_ps(qxyz, qxyz, 0x71);
#else
__m128 mul2 = _mm_mul_ps(qxyz, qxyz);
__m128 shuf2 = _mm_movehdup_ps(mul2);
__m128 sums2 = _mm_add_ps(mul2, shuf2);
shuf2 = _mm_movehl_ps(shuf2, sums2);
sums2 = _mm_add_ss(sums2, shuf2);
__m128 dot_qq = sums2;
#endif
__m128 w2 = _mm_mul_ps(qw, qw);
__m128 coeff = _mm_sub_ps(w2, dot_qq);
__m128 term2 = _mm_mul_ps(coeff, V);
// cross(q.xyz, v)
__m128 q_yzx = _mm_shuffle_ps(qxyz, qxyz, _MM_SHUFFLE(3,0,2,1));
__m128 v_yzx = _mm_shuffle_ps(V, V, _MM_SHUFFLE(3,0,2,1));
__m128 cross = _mm_sub_ps(_mm_mul_ps(qxyz, v_yzx), _mm_mul_ps(q_yzx, V));
cross = _mm_shuffle_ps(cross, cross, _MM_SHUFFLE(3,0,2,1));
__m128 term3 = _mm_mul_ps(_mm_mul_ps(cross, qw), _mm_set1_ps(2.0f));
// Final result
__m128 result = _mm_add_ps(_mm_add_ps(term1, term2), term3);
return result;
}
inline pod::Matrix4f uf::simd::quatMat( uf::simd::value<float> Q ) {
// Shuffle out components
__m128 qx = _mm_shuffle_ps(Q, Q, _MM_SHUFFLE(0,0,0,0));
__m128 qy = _mm_shuffle_ps(Q, Q, _MM_SHUFFLE(1,1,1,1));
__m128 qz = _mm_shuffle_ps(Q, Q, _MM_SHUFFLE(2,2,2,2));
__m128 qw = _mm_shuffle_ps(Q, Q, _MM_SHUFFLE(3,3,3,3));
// Compute squares
__m128 xx = _mm_mul_ps(qx, qx);
__m128 yy = _mm_mul_ps(qy, qy);
__m128 zz = _mm_mul_ps(qz, qz);
// Cross terms
__m128 xy = _mm_mul_ps(qx, qy);
__m128 xz = _mm_mul_ps(qx, qz);
__m128 yz = _mm_mul_ps(qy, qz);
__m128 xw = _mm_mul_ps(qx, qw);
__m128 yw = _mm_mul_ps(qy, qw);
__m128 zw = _mm_mul_ps(qz, qw);
__m128 two = _mm_set1_ps(2.0f);
xx = _mm_mul_ps(xx, two);
yy = _mm_mul_ps(yy, two);
zz = _mm_mul_ps(zz, two);
xy = _mm_mul_ps(xy, two);
xz = _mm_mul_ps(xz, two);
yz = _mm_mul_ps(yz, two);
xw = _mm_mul_ps(xw, two);
yw = _mm_mul_ps(yw, two);
zw = _mm_mul_ps(zw, two);
pod::Matrix4f M;
M[0] = 1.0f - _mm_cvtss_f32(yy) - _mm_cvtss_f32(zz);
M[1] = _mm_cvtss_f32(xy) + _mm_cvtss_f32(zw);
M[2] = _mm_cvtss_f32(xz) - _mm_cvtss_f32(yw);
M[3] = 0.0f;
M[4] = _mm_cvtss_f32(xy) - _mm_cvtss_f32(zw);
M[5] = 1.0f - _mm_cvtss_f32(xx) - _mm_cvtss_f32(zz);
M[6] = _mm_cvtss_f32(yz) + _mm_cvtss_f32(xw);
M[7] = 0.0f;
M[8] = _mm_cvtss_f32(xz) + _mm_cvtss_f32(yw);
M[9] = _mm_cvtss_f32(yz) - _mm_cvtss_f32(xw);
M[10] = 1.0f - _mm_cvtss_f32(xx) - _mm_cvtss_f32(yy);
M[11] = 0.0f;
M[12] = 0.0f;
M[13] = 0.0f;
M[14] = 0.0f;
M[15] = 1.0f;
return M;
}

36
engine/inc/uf/utils/math/rayt.h
View File

@ -1,36 +0,0 @@
#pragma once
#if 0
#include <uf/config.h>
#include <cstdint>
namespace pod {
struct UF_API RTPrimitive {
static const uint32_t EMPTY = (uint32_t) -1;
static const uint32_t CUBE = 1;
static const uint32_t LEAF = 2;
static const uint32_t TREE = 3;
static const uint32_t ROOT = 4;
pod::Vector4f position; // 4 * 4 = 16 bytes
uint32_t type; // 4 * 1 = 4 bytes
};
struct UF_API Light {
pod::Vector3f position;
pod::Vector3f color;
};
struct UF_API Tree {
static const size_t TREE_SIZE = 8;
pod::Vector4f position; // 4 * 4 = 16 bytes
uint32_t type; // 4 * 1 = 4 bytes
uint32_t children[Tree::TREE_SIZE]; // 4 * 8 = 32 bytes
};
}
namespace uf {
namespace primitive {
uf::stl::vector<pod::Tree> UF_API populate( const uf::stl::vector<pod::RTPrimitive>& cubes );
uf::stl::vector<pod::Tree> UF_API populateEntirely( const uf::stl::vector<pod::RTPrimitive>& cubes );
uf::stl::vector<pod::Tree> UF_API populateEntirely( const uf::stl::vector<pod::Tree>& trees, bool = false );
void UF_API test( const uf::stl::vector<pod::RTPrimitive>& cubes, const uf::stl::vector<pod::Tree>& trees );
}
}
#endif

16
engine/inc/uf/utils/math/shapes.h
View File

@ -5,13 +5,13 @@
namespace pod {
struct Plane {
pod::Vector3f normal;
alignas(16) pod::Vector3f normal;
float offset;
};
struct AABB {
pod::Vector3f min;
pod::Vector3f max;
alignas(16) pod::Vector3f min;
alignas(16) pod::Vector3f max;
};
struct Sphere {
@ -24,19 +24,19 @@ namespace pod {
};
struct Ray {
pod::Vector3f origin;
pod::Vector3f direction;
alignas(16) pod::Vector3f origin;
alignas(16) pod::Vector3f direction;
};
struct Triangle {
pod::Vector3f points[3];
alignas(16) pod::Vector3f points[3];
};
struct TriangleWithNormal : Triangle {
pod::Vector3f normal;
alignas(16) pod::Vector3f normal;
};
struct TriangleWithNormals : Triangle {
pod::Vector3f normals[3];
alignas(16) pod::Vector3f normals[3];
};
template<typename T>

4
engine/inc/uf/utils/math/transform/transform.inl
View File

@ -65,13 +65,13 @@ template<typename T> pod::Transform<T> /*UF_API*/ uf::transform::reorient( const
template<typename T> pod::Transform<T>& /*UF_API*/ uf::transform::rotate( pod::Transform<T>& transform, const pod::Vector3t<T>& axis, pod::Math::num_t delta ) {
pod::Quaternion<> quat = uf::quaternion::axisAngle( axis, delta );
transform.orientation = uf::vector::normalize(uf::quaternion::multiply(transform.orientation, quat));
transform.orientation = uf::vector::normalize(uf::quaternion::multiply(quat, transform.orientation));
transform = uf::transform::reorient(transform);
return transform;
}
template<typename T> pod::Transform<T>& /*UF_API*/ uf::transform::rotate( pod::Transform<T>& transform, const pod::Quaternion<T>& quat ) {
transform.orientation = uf::vector::normalize(uf::quaternion::multiply(transform.orientation, quat));
transform.orientation = uf::vector::normalize(uf::quaternion::multiply(quat, transform.orientation));
transform = uf::transform::reorient(transform);
return transform;

594
engine/inc/uf/utils/math/vector.h
View File

@ -5,11 +5,14 @@
#include "math.h"
#include <sstream>
#include <uf/utils/memory/vector.h>
#include <cmath>
#include <cstring>
#include <array>
#include <algorithm>
#include <cstddef>
#include <stdint.h>
#include <uf/utils/memory/vector.h>
#include <uf/ext/json/json.h>
#include <uf/utils/serialize/serializer.h>
#include <uf/utils/math/angle.h>
@ -19,51 +22,11 @@
#endif
namespace pod {
// Simple vectors (designed [to store in arrays] with minimal headaches)
template<typename T = pod::Math::num_t, size_t N = 3>
struct /*UF_API*/ Vector {
// n-dimensional/unspecialized vector access
T components[N];
// POD information
typedef T type_t;
typedef T* container_t;
static const size_t size = N;
// Overload access
// Accessing via subscripts
T& operator[](size_t i);
const T& operator[](size_t i) const;
// Arithmetic
Vector<T,N> operator()() const; // Negation
Vector<T,N> operator-() const; // Negation
Vector<T,N> operator+( const Vector<T,N>& vector ) const; // Addition between two vectors
Vector<T,N> operator-( const Vector<T,N>& vector ) const; // Subtraction between two vectors
Vector<T,N> operator*( const Vector<T,N>& vector ) const; // Multiplication between two vectors
Vector<T,N> operator/( const Vector<T,N>& vector ) const; // Division between two vectors
Vector<T,N> operator+( T scalar ) const; // Multiplication with scalar
Vector<T,N> operator-( T scalar ) const; // Multiplication with scalar
Vector<T,N> operator*( T scalar ) const; // Multiplication with scalar
Vector<T,N> operator/( T scalar ) const; // Division with scalar
Vector<T,N>& operator +=( const Vector<T,N>& vector ); // Addition set between two vectors
Vector<T,N>& operator -=( const Vector<T,N>& vector ); // Subtraction set between two vectors
Vector<T,N>& operator *=( const Vector<T,N>& vector ); // Multiplication set between two vectors
Vector<T,N>& operator /=( const Vector<T,N>& vector ); // Division set between two vectors
Vector<T,N>& operator +=( T scalar ); // Multiplication set with scalar
Vector<T,N>& operator -=( T scalar ); // Multiplication set with scalar
Vector<T,N>& operator *=( T scalar ); // Multiplication set with scalar
Vector<T,N>& operator /=( T scalar ); // Division set with scalar
bool operator==( const Vector<T,N>& vector ) const; // Equality check between two vectors (equals)
bool operator!=( const Vector<T,N>& vector ) const; // Equality check between two vectors (not equals)
bool operator<( const Vector<T,N>& vector ) const; // Equality check between two vectors (less than)
bool operator<=( const Vector<T,N>& vector ) const; // Equality check between two vectors (less than or equals)
bool operator>( const Vector<T,N>& vector ) const; // Equality check between two vectors (greater than)
bool operator>=( const Vector<T,N>& vector ) const; // Equality check between two vectors (greater than or equals)
template<typename T, size_t N>
struct Vector;
template<typename U, size_t M> Vector<T,N>& operator=( const Vector<U,M>& vector );
template<typename U, size_t M> operator Vector<U,M>();
explicit inline operator bool() const;
};
template<typename T = float> using Vector1t = Vector<T,1>;
typedef Vector1t<pod::Math::num_t> Vector1;
typedef Vector1t<NUM> Vector1;
typedef Vector1t<int32_t> Vector1i;
typedef Vector1t<uint32_t> Vector1ui;
@ -72,7 +35,7 @@ namespace pod {
typedef Vector1t<double> Vector1d;
template<typename T = float> using Vector2t = Vector<T,2>;
typedef Vector2t<pod::Math::num_t> Vector2;
typedef Vector2t<NUM> Vector2;
typedef Vector2t<int32_t> Vector2i;
typedef Vector2t<uint32_t> Vector2ui;
@ -81,7 +44,7 @@ namespace pod {
typedef Vector2t<double> Vector2d;
template<typename T = float> using Vector3t = Vector<T,3>;
typedef Vector3t<pod::Math::num_t> Vector3;
typedef Vector3t<NUM> Vector3;
typedef Vector3t<int32_t> Vector3i;
typedef Vector3t<uint32_t> Vector3ui;
typedef Vector3t<uint8_t> ColorRGB;
@ -91,7 +54,7 @@ namespace pod {
typedef Vector3t<double> Vector3d;
template<typename T = float> using Vector4t = Vector<T,4>;
typedef Vector4t<pod::Math::num_t> Vector4;
typedef Vector4t<NUM> Vector4;
typedef Vector4t<int32_t> Vector4i;
typedef Vector4t<uint32_t> Vector4ui;
typedef Vector4t<uint8_t> ColorRgba;
@ -113,217 +76,167 @@ namespace pod {
#endif
}
// POD vector accessing/manipulation
namespace uf {
namespace vector {
template<typename T> pod::Vector1t<T> /*UF_API*/ create( T x );
template<typename T> pod::Vector2t<T> /*UF_API*/ create( T x, T y );
template<typename T> pod::Vector3t<T> /*UF_API*/ create( T x, T y, T z );
template<typename T> pod::Vector4t<T> /*UF_API*/ create( T x, T y, T z, T w );
template<typename T, size_t N> pod::Vector<T, N> /*UF_API*/ copy( const pod::Vector<T, N>& = {});
template<typename T, size_t N, typename U> pod::Vector<T, N> /*UF_API*/ cast( const U& from );
template<typename T> pod::Vector1t<T> /*UF_API*/ create( T x ); // creates a 1D vector
template<typename T> pod::Vector2t<T> /*UF_API*/ create( T x, T y ); // creates a 2D vector
template<typename T> pod::Vector3t<T> /*UF_API*/ create( T x, T y, T z ); // creates a 3D vector
template<typename T> pod::Vector4t<T> /*UF_API*/ create( T x, T y, T z, T w ); // creates a 4D vector
template<typename T, size_t N> pod::Vector<T, N> /*UF_API*/ copy( const pod::Vector<T, N>& = {}); // creates a copy of a vector (for whatever reason)
template<typename T, size_t N, typename U> pod::Vector<T, N> /*UF_API*/ cast( const U& from ); // casts one vector of one type to another (of the same size)
// Equality checking
template<typename T> int /*UF_API*/ compareTo( const T& left, const T& right ); // Equality check between two vectors (less than)
template<typename T> bool /*UF_API*/ equals( const T& left, const T& right ); // Equality check between two vectors (equals)
template<typename T> bool /*UF_API*/ equals( const T& left, const T& right ); // equality check between two vectors (==)
template<typename T> bool /*UF_API*/ notEquals( const T& left, const T& right ); // equality check between two vectors (==)
template<typename T> bool /*UF_API*/ less( const T& left, const T& right ); // equality check between two vectors (<)
template<typename T> bool /*UF_API*/ lessEquals( const T& left, const T& right ); // equality check between two vectors (<=)
template<typename T> bool /*UF_API*/ greater( const T& left, const T& right ); // equality check between two vectors (>)
template<typename T> bool /*UF_API*/ greaterEquals( const T& left, const T& right ); // equality check between two vectors (>=)
template<typename T> bool /*UF_API*/ isValid( const T& v ); // Checks if all components are valid (non NaN, inf, etc.)
template<typename T> bool /*UF_API*/ isValid( const T& v ); // checks if all components are valid (non NaN, inf, etc.)
// Basic arithmetic
template<typename T> T /*UF_API*/ add( const T& left, const T& right ); // Adds two vectors of same type and size together
template<typename T> T /*UF_API*/ add( const T& left, /*const typename T::type_t&*/ typename T::type_t scalar ); // Adds two vectors of same type and size together
template<typename T> T /*UF_API*/ subtract( const T& left, const T& right ); // Subtracts two vectors of same type and size together
template<typename T> T /*UF_API*/ subtract( const T& left, /*const typename T::type_t&*/ typename T::type_t scalar ); // Subtracts two vectors of same type and size together
template<typename T> T /*UF_API*/ multiply( const T& left, const T& right ); // Multiplies two vectors of same type and size together
template<typename T> T /*UF_API*/ multiply( const T& vector, /*const typename T::type_t&*/ typename T::type_t scalar ); // Multiplies this vector by a scalar
template<typename T> T /*UF_API*/ divide( const T& left, const T& right ); // Divides two vectors of same type and size together
template<typename T> T /*UF_API*/ divide( const T& left, /*const typename T::type_t&*/ typename T::type_t scalar ); // Divides this vector by a scalar
template<typename T> typename T::type_t /*UF_API*/ sum( const T& vector ); // Compute the sum of all components
template<typename T> typename T::type_t /*UF_API*/ product( const T& vector ); // Compute the product of all components
template<typename T> T /*UF_API*/ negate( const T& vector ); // Flip sign of all components
template<typename T> T /*UF_API*/ abs( const T& vector );
// Writes to first value
template<typename T> T& /*UF_API*/ add_( T& left, const T& right ); // Adds two vectors of same type and size together
template<typename T> T& /*UF_API*/ add_( T& left, /*const typename T::type_t&*/ typename T::type_t scalar ); // Adds two vectors of same type and size together
template<typename T> T& /*UF_API*/ subtract_( T& left, const T& right ); // Subtracts two vectors of same type and size together
template<typename T> T& /*UF_API*/ subtract_( T& left, /*const typename T::type_t&*/ typename T::type_t scalar ); // Subtracts two vectors of same type and size together
template<typename T> T& /*UF_API*/ multiply_( T& left, const T& right ); // Multiplies two vectors of same type and size together
template<typename T> T& /*UF_API*/ multiply_( T& vector, /*const typename T::type_t&*/ typename T::type_t scalar ); // Multiplies this vector by a scalar
template<typename T> T& /*UF_API*/ divide_( T& left, const T& right ); // Divides two vectors of same type and size together
template<typename T> T& /*UF_API*/ divide_( T& left, /*const typename T::type_t&*/ typename T::type_t scalar ); // Divides this vector by a scalar
template<typename T> T& /*UF_API*/ negate_( T& vector ); // Flip sign of all components
template<typename T> T& /*UF_API*/ normalize_( T& vector ); // Normalizes a vector
template<typename T> T /*UF_API*/ add( const T& left, const T& right ); // adds two vectors of same type and size together
template<typename T> T /*UF_API*/ add( const T& left, typename T::type_t scalar ); // adds a scalar to every component of the vector
template<typename T> T /*UF_API*/ add( typename T::type_t scalar, const T& vector ); // adds a scalar to every component of the vector (inverted)
template<typename T> T /*UF_API*/ subtract( const T& left, const T& right ); // subtracts two vectors of same type and size together
template<typename T> T /*UF_API*/ subtract( const T& left, typename T::type_t scalar ); // subtracts a scalar to every component of the vector
template<typename T> T /*UF_API*/ subtract( typename T::type_t scalar, const T& vector ); // subtracts a scalar to every component of the vector (inverted)
template<typename T> T /*UF_API*/ multiply( const T& left, const T& right ); // multiplies two vectors of same type and size together
template<typename T> T /*UF_API*/ multiply( const T& vector, typename T::type_t scalar ); // multiplies a scalar to every component of the vector
template<typename T> T /*UF_API*/ multiply( typename T::type_t scalar, const T& vector ); // multiplies a scalar to every component of the vector (inverted)
template<typename T> T /*UF_API*/ divide( const T& left, const T& right ); // divides two vectors of same type and size together
template<typename T> T /*UF_API*/ divide( const T& left, typename T::type_t scalar ); // divides a scalar to every component of the vector
template<typename T> T /*UF_API*/ divide( typename T::type_t scalar, const T& vector ); // divides a scalar to every component of the vector (inverted)
template<typename T> T /*UF_API*/ min( const T& left, const T& right ); //
template<typename T> T /*UF_API*/ max( const T& left, const T& right ); //
template<typename T> T /*UF_API*/ ceil( const T& vector ); //
template<typename T> T /*UF_API*/ floor( const T& vector ); //
template<typename T> T /*UF_API*/ round( const T& vector ); //
template<typename T> T& /*UF_API*/ add_( T& left, const T& right ); // adds two vectors of same type and size together
template<typename T> T& /*UF_API*/ add_( T& left, typename T::type_t scalar ); // adds a scalar to every component of the vector
template<typename T> T& /*UF_API*/ add_( typename T::type_t scalar, T& vector ); // adds a scalar to every component of the vector (inverted)
template<typename T> T& /*UF_API*/ subtract_( T& left, const T& right ); // subtracts two vectors of same type and size together
template<typename T> T& /*UF_API*/ subtract_( T& left, typename T::type_t scalar ); // subtracts a scalar to every component of the vector
template<typename T> T& /*UF_API*/ subtract_( typename T::type_t scalar, T& vector ); // subtracts a scalar to every component of the vector (inverted)
template<typename T> T& /*UF_API*/ multiply_( T& left, const T& right ); // multiplies two vectors of same type and size together
template<typename T> T& /*UF_API*/ multiply_( T& vector, typename T::type_t scalar ); // multiplies a scalar to every component of the vector
template<typename T> T& /*UF_API*/ multiply_( typename T::type_t scalar, T& vector ); // multiplies a scalar to every component of the vector (inverted)
template<typename T> T& /*UF_API*/ divide_( T& left, const T& right ); // divides two vectors of same type and size together
template<typename T> T& /*UF_API*/ divide_( T& left, typename T::type_t scalar ); // divides a scalar to every component of the vector
template<typename T> T& /*UF_API*/ divide_( typename T::type_t scalar, T& vector ); // divides a scalar to every component of the vector (inverted)
template<typename T> T& /*UF_API*/ negate_( T& vector ); // flip sign of all components
template<typename T> T& /*UF_API*/ normalize_( T& vector ); // normalizes a vector
template<typename T> typename T::type_t /*UF_API*/ sum( const T& vector ); // compute the sum of all components
template<typename T> typename T::type_t /*UF_API*/ product( const T& vector ); // compute the product of all components
template<typename T> T /*UF_API*/ negate( const T& vector ); // flip sign of all components
template<typename T> T /*UF_API*/ abs( const T& vector ); // gets the absolute value of a vector
template<typename T> T /*UF_API*/ min( const T& left, const T& right ); // returns the minimum of each component between two vectors
template<typename T> T /*UF_API*/ max( const T& left, const T& right ); // returns the maximum of each component between two vectors
template<typename T> T /*UF_API*/ clamp( const T& vector, const T& min, const T& max ); // clamps a vector between two bounds
template<typename T> T /*UF_API*/ ceil( const T& vector ); // rounds each component of the the vector up
template<typename T> T /*UF_API*/ floor( const T& vector ); // rounds each component of the vector down
template<typename T> T /*UF_API*/ round( const T& vector ); // rounds each component of the vector
// Complex arithmetic
template<typename T> typename T::type_t /*UF_API*/ dot( const T& left, const T& right ); // Compute the dot product between two vectors
template<typename T> float /*UF_API*/ angle( const T& a, const T& b ); // Compute the angle between two vectors
template<typename T> float /*UF_API*/ signedAngle( const T& a, const T& b, const T& axis ); // Compute the signed angle between two vectors
template<typename T> T /*UF_API*/ cross( const T& a, const T& b ); // Compute the cross product between two vectors
template<typename T> typename T::type_t /*UF_API*/ dot( const T& left, const T& right ); // compute the dot product between two vectors
template<typename T> float /*UF_API*/ angle( const T& a, const T& b ); // compute the angle between two vectors
template<typename T> float /*UF_API*/ signedAngle( const T& a, const T& b, const T& axis ); // compute the signed angle between two vectors
template<typename T> T /*UF_API*/ cross( const T& a, const T& b ); // compute the cross product between two vectors
template<typename T> T /*UF_API*/ lerp( const T& from, const T& to, double, bool = true ); // Linearly interpolate between two vectors
template<typename T> T /*UF_API*/ lerp( const T& from, const T& to, const T&, bool = true ); // Linearly interpolate between two vectors
template<typename T> T /*UF_API*/ lerp( const T& from, const T& to, double, bool = true ); // linearly interpolate between two vectors
template<typename T> T /*UF_API*/ lerp( const T& from, const T& to, const T&, bool = true ); // linearly interpolate between two vectors, component-wise
template<typename T> T /*UF_API*/ slerp( const T& from, const T& to, double, bool = false); // Spherically interpolate between two vectors
template<typename T> T /*UF_API*/ mix( const T& from, const T& to, double, bool = false ); //
template<typename T> T /*UF_API*/ slerp( const T& from, const T& to, double, bool = false ); // spherically interpolate between two vectors
template<typename T> T /*UF_API*/ mix( const T& from, const T& to, double, bool = false );
template<typename T> typename T::type_t /*UF_API*/ distanceSquared( const T& a, const T& b ); // Compute the distance between two vectors (doesn't sqrt)
template<typename T> typename T::type_t /*UF_API*/ distance( const T& a, const T& b ); // Compute the distance between two vectors
template<typename T> typename T::type_t /*UF_API*/ norm( const T& vector ); // Compute the norm of the vector
template<typename T> typename T::type_t /*UF_API*/ magnitude( const T& vector ); // Gets the magnitude of the vector
template<typename T> T /*UF_API*/ normalize( const T& vector ); // Normalizes a vector
template<typename T> T /*UF_API*/ clampMagnitude( const T& vector ); // Clamps the magnitude of a vector
template<typename T> void /*UF_API*/ orthonormalize( T& x, T& y ); // Orthonormalizes a vector against another vector
template<typename T> T /*UF_API*/ orthonormalize( const T& x, const T& y ); // Orthonormalizes a vector against another vector
template<typename T> typename T::type_t /*UF_API*/ distanceSquared( const T& a, const T& b ); // compute the distance between two vectors (doesn't sqrt)
template<typename T> typename T::type_t /*UF_API*/ distance( const T& a, const T& b ); // compute the distance between two vectors
template<typename T> typename T::type_t /*UF_API*/ magnitude( const T& vector ); // gets the magnitude of the vector
template<typename T> typename T::type_t /*UF_API*/ norm( const T& vector ); // compute the norm (length) of the vector
template<typename T> T /*UF_API*/ normalize( const T& vector ); // normalizes a vector
template<typename T> T /*UF_API*/ clampMagnitude( const T& vector ); // clamps the magnitude of a vector
template<typename T> void /*UF_API*/ orthonormalize( T& x, T& y ); // orthonormalizes a vector against another vector
template<typename T> T /*UF_API*/ orthonormalize( const T& x, const T& y ); // orthonormalizes a vector against another vector
template<typename T> uf::stl::string /*UF_API*/ toString( const T& vector ); // Parses a vector as a string
template<typename T, size_t N> ext::json::Value encode( const pod::Vector<T,N>& v, const ext::json::EncodingSettings& = {} ); // Parses a vector into a JSON value
template<typename T> uf::stl::string /*UF_API*/ toString( const T& vector ); // parses a vector as a string
template<typename T, size_t N> ext::json::Value encode( const pod::Vector<T,N>& v, const ext::json::EncodingSettings& = {} ); // parses a vector into a JSON value
template<typename T, size_t N> pod::Vector<T,N>& decode( const ext::json::Value& v, pod::Vector<T,N>& ); // Parses a JSON value into a vector
template<typename T, size_t N> pod::Vector<T,N> decode( const ext::json::Value& v, const pod::Vector<T,N>& = {} ); // Parses a JSON value into a vector
template<typename T, size_t N> pod::Vector<T,N>& decode( const ext::json::Value& v, pod::Vector<T,N>& ); // parses a JSON value into a vector
template<typename T, size_t N> pod::Vector<T,N> decode( const ext::json::Value& v, const pod::Vector<T,N>& = {} ); // parses a JSON value into a vector
template<typename T> typename T::type_t /*UF_API*/ mips( const T& size ); // Calculate amount of mips to use given a size
template<typename T> typename T::type_t /*UF_API*/ mips( const T& size ); // calculate amount of mips to use given a size
}
}
#if UF_USE_CLASS_OF_PODS
namespace uf {
// Provides operations for POD vector
template<typename T = pod::Math::num_t, size_t N = 3>
class /*UF_API*/ Vector {
public:
// Easily access POD's type
typedef pod::Vector<T,N> pod_t;
// Replicate POD information
typedef T type_t;
typedef T* container_t;
static const size_t size = N;
protected:
// POD storage
Vector<T,N>::pod_t m_pod;
public:
// C-tor
Vector(); // initializes POD to 'def'
Vector(T def); // initializes POD to 'def'
Vector(const Vector<T,N-1>& v, T w);
Vector(const Vector<T,N>::pod_t& pod); // copies POD altogether
Vector(const T components[N]); // copies data into POD from 'components' (typed as C array)
Vector(const uf::stl::vector<T>& components); // copies data into POD from 'components' (typed as uf::stl::vector<T>)
// D-tor
// Unneccesary
// POD access
Vector<T,N>::pod_t& data(); // Returns a reference of POD
const Vector<T,N>::pod_t& data() const; // Returns a const-reference of POD
// Alternative POD access
T* get(); // Returns a pointer to the entire array
const T* get() const; // Returns a const-pointer to the entire array
T& getComponent( size_t i ); // Returns a reference to a single element
const T& getComponent( size_t i ) const; // Returns a const-reference to a single element
// POD manipulation
T* set(const T components[N]); // Sets the entire array
T& setComponent( size_t i, const T& value ); // Sets a single element
// Validation
bool isValid() const; // Checks if all components are valid (non NaN, inf, etc.)
// Basic arithmetic
inline uf::Vector<T,N>& add( const Vector<T, N>& vector ); // Adds two vectors of same type and size together
inline uf::Vector<T,N>& add( T scalar ); // Adds two vectors of same type and size together
inline uf::Vector<T,N>& subtract( const Vector<T, N>& vector ); // Subtracts two vectors of same type and size together
inline uf::Vector<T,N>& subtract( T scalar ); // Subtracts two vectors of same type and size together
inline uf::Vector<T,N>& multiply( const Vector<T, N>& vector ); // Multiplies two vectors of same type and size together
inline uf::Vector<T,N>& multiply( T scalar ); // Multiplies this vector by a scalar
inline uf::Vector<T,N>& divide( const Vector<T, N>& vector ); // Divides two vectors of same type and size together
inline uf::Vector<T,N>& divide( T scalar ); // Divides this vector by a scalar
inline T sum() const; // Compute the sum of all components
inline T product() const; // Compute the product of all components
inline uf::Vector<T,N>& negate(); // Flip sign of all components
// Complex arithmetic
inline T dot( const Vector<T,N> right ) const; // Compute the dot product between two vectors
inline float angle( const Vector<T,N>& b ) const; // Compute the angle between two vectors
inline uf::Vector<T,N> lerp( const Vector<T,N> to, double delta ) const; // Linearly interpolate between two vectors
inline uf::Vector<T,N> slerp( const Vector<T,N> to, double delta ) const; // Spherically interpolate between two vectors
inline T distanceSquared( const Vector<T,N> b ) const; // Compute the distance between two vectors (doesn't sqrt)
inline T distance( const Vector<T,N> b ) const; // Compute the distance between two vectors
inline T magnitude() const; // Gets the magnitude of the vector
inline T norm() const; // Compute the norm of the vector
inline uf::Vector<T,N>& normalize(); // Normalizes a vector
uf::Vector<T,N> getNormalized() const; // Return a normalized vector
inline uf::stl::string toString() const;
// Overloaded ops
// Accessing via subscripts
T& operator[](size_t i);
const T& operator[](size_t i) const;
// Arithmetic
inline Vector<T,N> operator-() const; // Negation
inline Vector<T,N> operator+( const Vector<T,N>& vector ) const; // Addition between two vectors
inline Vector<T,N> operator-( const Vector<T,N>& vector ) const; // Subtraction between two vectors
inline Vector<T,N> operator*( const Vector<T,N>& vector ) const; // Multiplication between two vectors
inline Vector<T,N> operator/( const Vector<T,N>& vector ) const; // Division between two vectors
inline Vector<T,N> operator+( T scalar ) const; // Multiplication with scalar
inline Vector<T,N> operator-( T scalar ) const; // Multiplication with scalar
inline Vector<T,N> operator*( T scalar ) const; // Multiplication with scalar
inline Vector<T,N> operator/( T scalar ) const; // Division with scalar
inline Vector<T,N>& operator +=( const Vector<T,N>& vector ); // Addition set between two vectors
inline Vector<T,N>& operator -=( const Vector<T,N>& vector ); // Subtraction set between two vectors
inline Vector<T,N>& operator *=( const Vector<T,N>& vector ); // Multiplication set between two vectors
inline Vector<T,N>& operator /=( const Vector<T,N>& vector ); // Division set between two vectors
inline Vector<T,N>& operator +=( T scalar ); // Multiplication set with scalar
inline Vector<T,N>& operator -=( T scalar ); // Multiplication set with scalar
inline Vector<T,N>& operator *=( T scalar ); // Multiplication set with scalar
inline Vector<T,N>& operator /=( T scalar ); // Division set with scalar
inline bool operator==( const Vector<T,N>& vector ) const; // Equality check between two vectors (equals)
inline bool operator!=( const Vector<T,N>& vector ) const; // Equality check between two vectors (not equals)
inline bool operator<( const Vector<T,N>& vector ) const; // Equality check between two vectors (less than)
inline bool operator<=( const Vector<T,N>& vector ) const; // Equality check between two vectors (less than or equals)
inline bool operator>( const Vector<T,N>& vector ) const; // Equality check between two vectors (greater than)
inline bool operator>=( const Vector<T,N>& vector ) const; // Equality check between two vectors (greater than or equals)
inline operator pod_t&() { return this->m_pod; }
inline operator const pod_t&() const { return this->m_pod; }
#define DEFINE_VECTOR(T, N) \
using type_t = T;\
using container_t = T*;\
static constexpr size_t size = N;\
inline T& operator[](size_t i) { return components[i]; }\
inline const T& operator[](size_t i) const { return components[i]; }\
inline Vector<T,N> operator-() const { return uf::vector::negate(*this); } \
inline Vector<T,N> operator+(const Vector<T,N>& rhs) const { return uf::vector::add(*this, rhs); } \
inline Vector<T,N> operator-(const Vector<T,N>& rhs) const { return uf::vector::subtract(*this, rhs); } \
inline Vector<T,N> operator*(const Vector<T,N>& rhs) const { return uf::vector::multiply(*this, rhs); } \
inline Vector<T,N> operator/(const Vector<T,N>& rhs) const { return uf::vector::divide(*this, rhs); } \
inline Vector<T,N> operator+(type_t scalar) const { return uf::vector::add(*this, scalar); } \
inline Vector<T,N> operator-(type_t scalar) const { return uf::vector::subtract(*this, scalar); } \
inline Vector<T,N> operator*(type_t scalar) const { return uf::vector::multiply(*this, scalar); } \
inline Vector<T,N> operator/(type_t scalar) const { return uf::vector::divide(*this, scalar); } \
inline Vector<T,N>& operator+=(const Vector<T,N>& rhs) { return uf::vector::add_(*this, rhs); } \
inline Vector<T,N>& operator-=(const Vector<T,N>& rhs) { return uf::vector::subtract_(*this, rhs); } \
inline Vector<T,N>& operator*=(const Vector<T,N>& rhs) { return uf::vector::multiply_(*this, rhs); } \
inline Vector<T,N>& operator/=(const Vector<T,N>& rhs) { return uf::vector::divide_(*this, rhs); } \
inline Vector<T,N>& operator+=(type_t scalar) { return uf::vector::add_(*this, scalar); } \
inline Vector<T,N>& operator-=(type_t scalar) { return uf::vector::subtract_(*this, scalar); } \
inline Vector<T,N>& operator*=(type_t scalar) { return uf::vector::multiply_(*this, scalar); } \
inline Vector<T,N>& operator/=(type_t scalar) { return uf::vector::divide_(*this, scalar); } \
inline bool operator==(const Vector<T,N>& rhs) const { return uf::vector::equals(*this, rhs); } \
inline bool operator!=(const Vector<T,N>& rhs) const { return uf::vector::notEquals(*this, rhs); } \
inline bool operator<(const Vector<T,N>& rhs) const { return uf::vector::less(*this, rhs); } \
inline bool operator<=(const Vector<T,N>& rhs) const { return uf::vector::lessEquals(*this, rhs); } \
inline bool operator>(const Vector<T,N>& rhs) const { return uf::vector::greater(*this, rhs); } \
inline bool operator>=(const Vector<T,N>& rhs) const { return uf::vector::greaterEquals(*this, rhs); } \
inline explicit operator bool() const { return uf::vector::notEquals(*this, Vector<T,N>{}); } \
template<typename U, size_t M> inline operator Vector<U,M>() const { return uf::vector::cast<U,M>(*this); }
namespace pod {
template<typename T, size_t N>
struct Vector {
union {
T components[N];
};
DEFINE_VECTOR(T, N);
};
template<typename T = float> using Vector1t = Vector<T,1>;
typedef Vector1t<pod::Math::num_t> Vector1;
typedef Vector1t<int32_t> Vector1i;
typedef Vector1t<uint32_t> Vector1ui;
template<typename T>
struct Vector<T,2> {
union {
struct { T x, y; };
T components[2];
};
typedef Vector1t<long> Vector1l;
typedef Vector1t<float> Vector1f;
typedef Vector1t<double> Vector1d;
template<typename T = float> using Vector2t = Vector<T,2>;
typedef Vector2t<pod::Math::num_t> Vector2;
typedef Vector2t<int32_t> Vector2i;
typedef Vector2t<uint32_t> Vector2ui;
DEFINE_VECTOR(T, 2);
};
template<typename T>
struct Vector<T,3> {
union {
struct { T x, y, z; };
T components[3];
};
typedef Vector2t<long> Vector2l;
typedef Vector2t<float> Vector2f;
typedef Vector2t<double> Vector2d;
DEFINE_VECTOR(T, 3);
};
template<typename T = float> using Vector3t = Vector<T,3>;
typedef Vector3t<pod::Math::num_t> Vector3;
typedef Vector3t<int32_t> Vector3i;
typedef Vector3t<uint32_t> Vector3ui;
typedef Vector3t<uint8_t> ColorRGB;
template<typename T>
struct Vector<T,4> {
union {
struct { T x, y, z, w; };
T components[4];
};
typedef Vector3t<long> Vector3l;
typedef Vector3t<float> Vector3f;
typedef Vector3t<double> Vector3d;
template<typename T = float> using Vector4t = Vector<T,4>;
typedef Vector4t<pod::Math::num_t> Vector4;
typedef Vector4t<int32_t> Vector4i;
typedef Vector4t<uint32_t> Vector4ui;
typedef Vector4t<uint8_t> ColorRgba;
typedef Vector4t<long> Vector4l;
typedef Vector4t<float> Vector4f;
typedef Vector4t<double> Vector4d;
DEFINE_VECTOR(T, 4);
};
}
#endif
// external functions
//#include <uf/utils/string/ext.h>
#include <sstream>
@ -341,217 +254,6 @@ namespace ext {
}
}
namespace pod {
template<typename T>
struct /*UF_API*/ Vector<T,1> {
// XY access
T x;
// n-dimensional/unspecialized vector access
// T* components = (T*) this;
// POD information
typedef T type_t;
typedef T* container_t;
static const size_t size = 1;
// Overload access
// Accessing via subscripts
T& operator[](size_t i);
const T& operator[](size_t i) const;
// Arithmetic
inline Vector<T,1> operator()() const; // Creation
inline Vector<T,1> operator-() const; // Negation
inline Vector<T,1> operator+( const Vector<T,1>& vector ) const; // Addition between two vectors
inline Vector<T,1> operator-( const Vector<T,1>& vector ) const; // Subtraction between two vectors
inline Vector<T,1> operator*( const Vector<T,1>& vector ) const; // Multiplication between two vectors
inline Vector<T,1> operator/( const Vector<T,1>& vector ) const; // Division between two vectors
inline Vector<T,1> operator+( T scalar ) const; // Multiplication with scalar
inline Vector<T,1> operator-( T scalar ) const; // Multiplication with scalar
inline Vector<T,1> operator*( T scalar ) const; // Multiplication with scalar
inline Vector<T,1> operator/( T scalar ) const; // Division with scalar
inline Vector<T,1>& operator +=( const Vector<T,1>& vector ); // Addition set between two vectors
inline Vector<T,1>& operator -=( const Vector<T,1>& vector ); // Subtraction set between two vectors
inline Vector<T,1>& operator *=( const Vector<T,1>& vector ); // Multiplication set between two vectors
inline Vector<T,1>& operator /=( const Vector<T,1>& vector ); // Division set between two vectors
inline Vector<T,1>& operator +=( T scalar ); // Multiplication set with scalar
inline Vector<T,1>& operator -=( T scalar ); // Multiplication set with scalar
inline Vector<T,1>& operator *=( T scalar ); // Multiplication set with scalar
inline Vector<T,1>& operator /=( T scalar ); // Division set with scalar
inline bool operator==( const Vector<T,1>& vector ) const; // Equality check between two vectors (equals)
inline bool operator!=( const Vector<T,1>& vector ) const; // Equality check between two vectors (not equals)
inline bool operator<( const Vector<T,1>& vector ) const; // Equality check between two vectors (less than)
inline bool operator<=( const Vector<T,1>& vector ) const; // Equality check between two vectors (less than or equals)
inline bool operator>( const Vector<T,1>& vector ) const; // Equality check between two vectors (greater than)
inline bool operator>=( const Vector<T,1>& vector ) const; // Equality check between two vectors (greater than or equals)
template<typename U, size_t M> Vector<T,1>& operator=( const Vector<U,M>& vector );
template<typename U, size_t M> operator Vector<U,M>();
explicit inline operator bool() const;
#if 0
#if UF_USE_SIMD
Vector<T,1>& operator=( const __m128 );
operator __m128() const;
#endif
#endif
};
template<typename T>
struct /*UF_API*/ Vector<T,2> {
// XY access
T x;
T y;
// n-dimensional/unspecialized vector access
// T* components = (T*) this;
// POD information
typedef T type_t;
typedef T* container_t;
static const size_t size = 2;
// Overload access
// Accessing via subscripts
T& operator[](size_t i);
const T& operator[](size_t i) const;
// Arithmetic
inline Vector<T,2> operator()() const; // Creation
inline Vector<T,2> operator-() const; // Negation
inline Vector<T,2> operator+( const Vector<T,2>& vector ) const; // Addition between two vectors
inline Vector<T,2> operator-( const Vector<T,2>& vector ) const; // Subtraction between two vectors
inline Vector<T,2> operator*( const Vector<T,2>& vector ) const; // Multiplication between two vectors
inline Vector<T,2> operator/( const Vector<T,2>& vector ) const; // Division between two vectors
inline Vector<T,2> operator+( T scalar ) const; // Multiplication with scalar
inline Vector<T,2> operator-( T scalar ) const; // Multiplication with scalar
inline Vector<T,2> operator*( T scalar ) const; // Multiplication with scalar
inline Vector<T,2> operator/( T scalar ) const; // Division with scalar
inline Vector<T,2>& operator +=( const Vector<T,2>& vector ); // Addition set between two vectors
inline Vector<T,2>& operator -=( const Vector<T,2>& vector ); // Subtraction set between two vectors
inline Vector<T,2>& operator *=( const Vector<T,2>& vector ); // Multiplication set between two vectors
inline Vector<T,2>& operator /=( const Vector<T,2>& vector ); // Division set between two vectors
inline Vector<T,2>& operator +=( T scalar ); // Multiplication set with scalar
inline Vector<T,2>& operator -=( T scalar ); // Multiplication set with scalar
inline Vector<T,2>& operator *=( T scalar ); // Multiplication set with scalar
inline Vector<T,2>& operator /=( T scalar ); // Division set with scalar
inline bool operator==( const Vector<T,2>& vector ) const; // Equality check between two vectors (equals)
inline bool operator!=( const Vector<T,2>& vector ) const; // Equality check between two vectors (not equals)
inline bool operator<( const Vector<T,2>& vector ) const; // Equality check between two vectors (less than)
inline bool operator<=( const Vector<T,2>& vector ) const; // Equality check between two vectors (less than or equals)
inline bool operator>( const Vector<T,2>& vector ) const; // Equality check between two vectors (greater than)
inline bool operator>=( const Vector<T,2>& vector ) const; // Equality check between two vectors (greater than or equals)
template<typename U, size_t M> Vector<T,2>& operator=( const Vector<U,M>& vector );
template<typename U, size_t M> operator Vector<U,M>();
explicit inline operator bool() const;
#if 0
#if UF_USE_SIMD
Vector<T,2>& operator=( const __m128 );
operator __m128() const;
#endif
#endif
};
template<typename T>
struct /*UF_API*/ Vector<T,3> {
// XYZ access
T x;
T y;
T z;
// n-dimensional/unspecialized vector access
// T* components = (T*) this;
// POD information
typedef T type_t;
typedef T* container_t;
static const size_t size = 3;
// Overload access
// Accessing via subscripts
T& operator[](size_t i);
const T& operator[](size_t i) const;
// Arithmetic
inline Vector<T,3> operator()() const; // Creation
inline Vector<T,3> operator-() const; // Negation
inline Vector<T,3> operator+( const Vector<T,3>& vector ) const; // Addition between two vectors
inline Vector<T,3> operator-( const Vector<T,3>& vector ) const; // Subtraction between two vectors
inline Vector<T,3> operator*( const Vector<T,3>& vector ) const; // Multiplication between two vectors
inline Vector<T,3> operator/( const Vector<T,3>& vector ) const; // Division between two vectors
inline Vector<T,3> operator+( T scalar ) const; // Multiplication with scalar
inline Vector<T,3> operator-( T scalar ) const; // Multiplication with scalar
inline Vector<T,3> operator*( T scalar ) const; // Multiplication with scalar
inline Vector<T,3> operator/( T scalar ) const; // Division with scalar
inline Vector<T,3>& operator +=( const Vector<T,3>& vector ); // Addition set between two vectors
inline Vector<T,3>& operator -=( const Vector<T,3>& vector ); // Subtraction set between two vectors
inline Vector<T,3>& operator *=( const Vector<T,3>& vector ); // Multiplication set between two vectors
inline Vector<T,3>& operator /=( const Vector<T,3>& vector ); // Division set between two vectors
inline Vector<T,3>& operator +=( T scalar ); // Multiplication set with scalar
inline Vector<T,3>& operator -=( T scalar ); // Multiplication set with scalar
inline Vector<T,3>& operator *=( T scalar ); // Multiplication set with scalar
inline Vector<T,3>& operator /=( T scalar ); // Division set with scalar
inline bool operator==( const Vector<T,3>& vector ) const; // Equality check between two vectors (equals)
inline bool operator!=( const Vector<T,3>& vector ) const; // Equality check between two vectors (not equals)
inline bool operator<( const Vector<T,3>& vector ) const; // Equality check between two vectors (less than)
inline bool operator<=( const Vector<T,3>& vector ) const; // Equality check between two vectors (less than or equals)
inline bool operator>( const Vector<T,3>& vector ) const; // Equality check between two vectors (greater than)
inline bool operator>=( const Vector<T,3>& vector ) const; // Equality check between two vectors (greater than or equals)
template<typename U, size_t M> Vector<T,3>& operator=( const Vector<U,M>& vector );
template<typename U, size_t M> operator Vector<U,M>();
explicit inline operator bool() const;
#if 0
#if UF_USE_SIMD
Vector<T,3>& operator=( const __m128 );
operator __m128() const;
#endif
#endif
};
template<typename T>
#if UF_VECTOR_ALIGNED
struct /*UF_API*/ alignas(16) Vector<T,4> {
#else
struct /*UF_API*/ Vector<T,4> {
#endif
// XYZW access
T x;
T y;
T z;
T w;
// n-dimensional/unspecialized vector access
// T* components = (T*) this;
// POD information
typedef T type_t;
typedef T* container_t;
static const size_t size = 4;
// Overload access
// Accessing via subscripts
T& operator[](size_t i);
const T& operator[](size_t i) const;
// Arithmetic
inline Vector<T,4> operator()() const; // Creation
inline Vector<T,4> operator-() const; // Negation
inline Vector<T,4> operator+( const Vector<T,4>& vector ) const; // Addition between two vectors
inline Vector<T,4> operator-( const Vector<T,4>& vector ) const; // Subtraction between two vectors
inline Vector<T,4> operator*( const Vector<T,4>& vector ) const; // Multiplication between two vectors
inline Vector<T,4> operator/( const Vector<T,4>& vector ) const; // Division between two vectors
inline Vector<T,4> operator+( T scalar ) const; // Multiplication with scalar
inline Vector<T,4> operator-( T scalar ) const; // Multiplication with scalar
inline Vector<T,4> operator*( T scalar ) const; // Multiplication with scalar
inline Vector<T,4> operator/( T scalar ) const; // Division with scalar
inline Vector<T,4>& operator +=( const Vector<T,4>& vector ); // Addition set between two vectors
inline Vector<T,4>& operator -=( const Vector<T,4>& vector ); // Subtraction set between two vectors
inline Vector<T,4>& operator *=( const Vector<T,4>& vector ); // Multiplication set between two vectors
inline Vector<T,4>& operator /=( const Vector<T,4>& vector ); // Division set between two vectors
inline Vector<T,4>& operator +=( T scalar ); // Multiplication set with scalar
inline Vector<T,4>& operator -=( T scalar ); // Multiplication set with scalar
inline Vector<T,4>& operator *=( T scalar ); // Multiplication set with scalar
inline Vector<T,4>& operator /=( T scalar ); // Division set with scalar
inline bool operator==( const Vector<T,4>& vector ) const; // Equality check between two vectors (equals)
inline bool operator!=( const Vector<T,4>& vector ) const; // Equality check between two vectors (not equals)
inline bool operator<( const Vector<T,4>& vector ) const; // Equality check between two vectors (less than)
inline bool operator<=( const Vector<T,4>& vector ) const; // Equality check between two vectors (less than or equals)
inline bool operator>( const Vector<T,4>& vector ) const; // Equality check between two vectors (greater than)
inline bool operator>=( const Vector<T,4>& vector ) const; // Equality check between two vectors (greater than or equals)
template<typename U, size_t M> Vector<T,4>& operator=( const Vector<U,M>& vector );
template<typename U, size_t M> operator Vector<U,M>();
explicit inline operator bool() const;
#if 0
#if UF_USE_SIMD
Vector<T,4>& operator=( const __m128 );
operator __m128() const;
#endif
#endif
};
}
namespace std {
template<typename T, size_t N>
struct hash<pod::Vector<T,N>> {

256
engine/inc/uf/utils/math/vector/class.inl
View File

@ -1,256 +0,0 @@
// C-tor
template<typename T, std::size_t N> // initializes POD to 'def'
uf::Vector<T,N>::Vector() {
}
template<typename T, std::size_t N> // initializes POD to 'def'
uf::Vector<T,N>::Vector(T def) {
for ( std::size_t i = 0; i < N; ++i ) this->m_pod[i] = def;
}
template<typename T, std::size_t N> // copies POD altogether
uf::Vector<T,N>::Vector(const typename uf::Vector<T,N>::pod_t& pod) : m_pod(pod) {
}
template<typename T, std::size_t N> // copies data into POD from 'components' (typed as C array)
uf::Vector<T,N>::Vector(const T components[N]) {
this->set(&components[0]);
}
template<typename T, std::size_t N> // copies data into POD from 'components' (typed as uf::stl::vector<T>)
uf::Vector<T,N>::Vector(const uf::stl::vector<T>& components) {
if ( components.size() >= N ) this->set(&components[0]);
}
// D-tor
// Unneccesary
// POD access
template<typename T, std::size_t N> // Returns a reference of POD
typename uf::Vector<T,N>::pod_t& uf::Vector<T,N>::data() {
return this->m_pod;
}
template<typename T, std::size_t N> // Returns a const-reference of POD
const typename uf::Vector<T,N>::pod_t& uf::Vector<T,N>::data() const {
return this->m_pod;
}
// Alternative POD access
template<typename T, std::size_t N> // Returns a pointer to the entire array
T* uf::Vector<T,N>::get() {
return &this->m_pod[0];
}
template<typename T, std::size_t N> // Returns a const-pointer to the entire array
const T* uf::Vector<T,N>::get() const {
return &this->m_pod[0];
}
template<typename T, std::size_t N> // Returns a reference to a single element
T& uf::Vector<T,N>::getComponent( std::size_t i ) {
return this->m_pod[i];
}
template<typename T, std::size_t N> // Returns a const-reference to a single element
const T& uf::Vector<T,N>::getComponent( std::size_t i ) const {
return this->m_pod[i];
}
// POD manipulation
template<typename T, std::size_t N> // Sets the entire array
T* uf::Vector<T,N>::set(const T components[N]) {
for ( std::size_t i = 0; i < N; ++i ) this->m_pod[i] = components[i];
}
template<typename T, std::size_t N> // Sets a single element
T& uf::Vector<T,N>::setComponent( std::size_t i, const T& value ) {
this->m_pod[i] = value;
}
// Validation
template<typename T, std::size_t N> // Checks if all components are valid (non NaN, inf, etc.)
bool uf::Vector<T,N>::isValid() const {
return uf::vector::isValid( this->m_pod );
}
// Basic arithmetic
template<typename T, std::size_t N> // Adds two vectors of same type and size together
inline uf::Vector<T,N>& uf::Vector<T,N>::add( const Vector<T, N>& vector ) {
return uf::vector::add( this->m_pod, vector.data() );
}
template<typename T, std::size_t N> // Multiplies this vector by a scalar
inline uf::Vector<T,N>& uf::Vector<T,N>::add( T scalar ) {
return uf::vector::add( this->m_pod, scalar );
}
template<typename T, std::size_t N> // Subtracts two vectors of same type and size together
inline uf::Vector<T,N>& uf::Vector<T,N>::subtract( const Vector<T, N>& vector ) {
return uf::vector::subtract( this->m_pod, vector.data() );
}
template<typename T, std::size_t N> // Multiplies this vector by a scalar
inline uf::Vector<T,N>& uf::Vector<T,N>::subtract( T scalar ) {
return uf::vector::subtract( this->m_pod, scalar );
}
template<typename T, std::size_t N> // Multiplies two vectors of same type and size together
inline uf::Vector<T,N>& uf::Vector<T,N>::multiply( const Vector<T, N>& vector ) {
return uf::vector::multiply( this->m_pod, vector.data() );
}
template<typename T, std::size_t N> // Multiplies this vector by a scalar
inline uf::Vector<T,N>& uf::Vector<T,N>::multiply( T scalar ) {
return uf::vector::multiply( this->m_pod, scalar );
}
template<typename T, std::size_t N> // Divides two vectors of same type and size together
inline uf::Vector<T,N>& uf::Vector<T,N>::divide( const Vector<T, N>& vector ) {
return uf::vector::divide( this->m_pod, vector.data() );
}
template<typename T, std::size_t N> // Divides this vector by a scalar
inline uf::Vector<T,N>& uf::Vector<T,N>::divide( T scalar ) {
return uf::vector::divide( this->m_pod, scalar );
}
template<typename T, std::size_t N> // Compute the sum of all components
inline T uf::Vector<T,N>::sum() const {
return uf::vector::sum( this->m_pod );
}
template<typename T, std::size_t N> // Compute the product of all components
inline T uf::Vector<T,N>::product() const {
return uf::vector::product( this->m_pod );
}
template<typename T, std::size_t N> // Flip sign of all components
inline uf::Vector<T,N>& uf::Vector<T,N>::negate() {
return uf::vector::negate( this->m_pod );
}
// Complex arithmetic
template<typename T, std::size_t N> // Compute the dot product between two vectors
inline T uf::Vector<T,N>::dot( const Vector<T,N> right ) const {
return uf::vector::dot( this->m_pod, right );
}
template<typename T, std::size_t N> // Compute the angle between two vectors
inline float uf::Vector<T,N>::angle( const Vector<T,N>& b ) const {
return uf::vector::angle( this->m_pod, b );
}
template<typename T, std::size_t N> // Linearly interpolate between two vectors
inline uf::Vector<T,N> uf::Vector<T,N>::lerp( const Vector<T,N> to, double delta ) const {
return uf::vector::lerp( this->m_pod, to, delta );
}
template<typename T, std::size_t N> // Spherically interpolate between two vectors
inline uf::Vector<T,N> uf::Vector<T,N>::slerp( const Vector<T,N> to, double delta ) const {
return uf::vector::slerp( this->m_pod, to, delta );
}
template<typename T, std::size_t N> // Compute the distance between two vectors (doesn't sqrt)
inline T uf::Vector<T,N>::distanceSquared( const Vector<T,N> b ) const {
return uf::vector::distanceSquared( this->m_pod, b );
}
template<typename T, std::size_t N> // Compute the distance between two vectors
inline T uf::Vector<T,N>::distance( const Vector<T,N> b ) const {
return uf::vector::distance( this->m_pod, b );
}
template<typename T, std::size_t N> // Gets the magnitude of the vector
inline T uf::Vector<T,N>::magnitude() const {
return uf::vector::magnitude( this->m_pod );
}
template<typename T, std::size_t N> // Compute the norm of the vector
inline T uf::Vector<T,N>::norm() const {
return uf::vector::norm( this->m_pod );
}
template<typename T, std::size_t N> // Normalizes a vector
inline uf::Vector<T,N>& uf::Vector<T,N>::normalize() {
return uf::vector::normalize( this->m_pod );
}
template<typename T, std::size_t N> // Return a normalized vector
uf::Vector<T,N> uf::Vector<T,N>::getNormalized() const {
return uf::vector::normalize( this->m_pod );
}
template<typename T, std::size_t N> // Return a string
uf::stl::string uf::Vector<T,N>::toString() const {
return uf::vector::toString( this->m_pod );
}
// Overloaded ops
template<typename T, std::size_t N>
T& uf::Vector<T,N>::operator[](std::size_t i) {
return this->m_pod[i];
}
template<typename T, std::size_t N>
const T& uf::Vector<T,N>::operator[](std::size_t i) const {
return this->m_pod[i];
}
// Arithmetic
template<typename T, std::size_t N> // Negation
inline uf::Vector<T,N> uf::Vector<T,N>::operator-() const {
return uf::vector::negate( this->m_pod );
}
template<typename T, std::size_t N> // Addition between two vectors
inline uf::Vector<T,N> uf::Vector<T,N>::operator+( const uf::Vector<T,N>& vector ) const {
return uf::vector::add( this->m_pod, vector.data() );
}
template<typename T, std::size_t N> // Subtraction between two vectors
inline uf::Vector<T,N> uf::Vector<T,N>::operator-( const uf::Vector<T,N>& vector ) const {
return uf::vector::subtract( this->m_pod, vector.data() );
}
template<typename T, std::size_t N> // Multiplication between two vectors
inline uf::Vector<T,N> uf::Vector<T,N>::operator*( const uf::Vector<T,N>& vector ) const {
return uf::vector::multiply( this->m_pod, vector.data() );
}
template<typename T, std::size_t N> // Division between two vectors
inline uf::Vector<T,N> uf::Vector<T,N>::operator/( const uf::Vector<T,N>& vector ) const {
return uf::vector::divide( this->m_pod, vector.data() );
}
template<typename T, std::size_t N> // Multiplication with scalar
inline uf::Vector<T,N> uf::Vector<T,N>::operator+( T scalar ) const {
return uf::vector::add( this->m_pod, scalar );
}
template<typename T, std::size_t N> // Multiplication with scalar
inline uf::Vector<T,N> uf::Vector<T,N>::operator-( T scalar ) const {
return uf::vector::subtract( this->m_pod, scalar );
}
template<typename T, std::size_t N> // Multiplication with scalar
inline uf::Vector<T,N> uf::Vector<T,N>::operator*( T scalar ) const {
return uf::vector::multiply( this->m_pod, scalar );
}
template<typename T, std::size_t N> // Division with scalar
inline uf::Vector<T,N> uf::Vector<T,N>::operator/( T scalar ) const {
return uf::vector::divide( this->m_pod, scalar );
}
template<typename T, std::size_t N> // Addition set between two vectors
inline uf::Vector<T,N>& uf::Vector<T,N>::operator +=( const uf::Vector<T,N>& vector ) {
return uf::vector::add( this->m_pod, vector.data() );
}
template<typename T, std::size_t N> // Subtraction set between two vectors
inline uf::Vector<T,N>& uf::Vector<T,N>::operator -=( const uf::Vector<T,N>& vector ) {
return uf::vector::subtract( this->m_pod, vector.data() );
}
template<typename T, std::size_t N> // Multiplication set between two vectors
inline uf::Vector<T,N>& uf::Vector<T,N>::operator *=( const uf::Vector<T,N>& vector ) {
return uf::vector::multiply( this->m_pod, vector.data() );
}
template<typename T, std::size_t N> // Division set between two vectors
inline uf::Vector<T,N>& uf::Vector<T,N>::operator /=( const uf::Vector<T,N>& vector ) {
return uf::vector::divide( this->m_pod, vector.data() );
}
template<typename T, std::size_t N> // Multiplication set with scalar
inline uf::Vector<T,N>& uf::Vector<T,N>::operator +=( T scalar ) {
return uf::vector::add( this->m_pod, scalar );
}
template<typename T, std::size_t N> // Multiplication set with scalar
inline uf::Vector<T,N>& uf::Vector<T,N>::operator -=( T scalar ) {
return uf::vector::subtract( this->m_pod, scalar );
}
template<typename T, std::size_t N> // Multiplication set with scalar
inline uf::Vector<T,N>& uf::Vector<T,N>::operator *=( T scalar ) {
return uf::vector::multiply( this->m_pod, scalar );
}
template<typename T, std::size_t N> // Division set with scalar
inline uf::Vector<T,N>& uf::Vector<T,N>::operator /=( T scalar ) {
return uf::vector::divide( this->m_pod, scalar );
}
template<typename T, std::size_t N> // Equality check between two vectors (equals)
inline bool uf::Vector<T,N>::operator==( const uf::Vector<T,N>& vector ) const {
return uf::vector::equals(this->m_pod, vector.data() );
}
template<typename T, std::size_t N> // Equality check between two vectors (not equals)
inline bool uf::Vector<T,N>::operator!=( const uf::Vector<T,N>& vector ) const {
return !uf::vector::equals(this->m_pod, vector.data() );
}
template<typename T, std::size_t N> // Equality check between two vectors (less than)
inline bool uf::Vector<T,N>::operator<( const uf::Vector<T,N>& vector ) const {
return uf::vector::compareTo(this->m_pod, vector.data() ) < 0;
}
template<typename T, std::size_t N> // Equality check between two vectors (less than or equals)
inline bool uf::Vector<T,N>::operator<=( const uf::Vector<T,N>& vector ) const {
return uf::vector::compareTo(this->m_pod, vector.data() ) <= 0;
}
template<typename T, std::size_t N> // Equality check between two vectors (greater than)
inline bool uf::Vector<T,N>::operator>( const uf::Vector<T,N>& vector ) const {
return uf::vector::compareTo(this->m_pod, vector.data() ) > 0;
}
template<typename T, std::size_t N> // Equality check between two vectors (greater than or equals)
inline bool uf::Vector<T,N>::operator>=( const uf::Vector<T,N>& vector ) const {
return uf::vector::compareTo(this->m_pod, vector.data() ) >= 0;
}

710
engine/inc/uf/utils/math/vector/pod.inl
View File

File diff suppressed because it is too large Load Diff

1976
engine/inc/uf/utils/math/vector/redundancy.inl
View File

File diff suppressed because it is too large Load Diff

125
engine/inc/uf/utils/math/vector/simd.h
View File

@ -6,6 +6,30 @@
#include <stdfloat>
#endif
#define DEFINE_SIMD(T)\
inline value<T> /*UF_API*/ load( const T* );\
inline void /*UF_API*/ store( value<T>, T* );\
inline value<T> /*UF_API*/ set( T );\
inline value<T> /*UF_API*/ set( T, T, T, T );\
inline value<T> /*UF_API*/ add( value<T>, value<T> );\
inline value<T> /*UF_API*/ sub( value<T>, value<T> );\
inline value<T> /*UF_API*/ mul( value<T>, value<T> );\
inline value<T> /*UF_API*/ div( value<T>, value<T> );\
inline value<T> /*UF_API*/ min( value<T>, value<T> );\
inline value<T> /*UF_API*/ max( value<T>, value<T> );\
inline bool /*UF_API*/ all( value<T> );\
inline bool /*UF_API*/ any( value<T> );\
inline value<T> /*UF_API*/ less( value<T>, value<T> );\
inline value<T> /*UF_API*/ lessEquals( value<T>, value<T> );\
inline value<T> /*UF_API*/ greater( value<T>, value<T> );\
inline value<T> /*UF_API*/ greaterEquals( value<T>, value<T> );\
inline value<T> /*UF_API*/ equals( value<T>, value<T> );\
inline value<T> /*UF_API*/ notEquals( value<T>, value<T> );\
inline value<T> /*UF_API*/ sqrt( value<T> );\
inline value<T> /*UF_API*/ hadd( value<T>, value<T> );\
inline T /*UF_API*/ dot( value<T>, value<T> );\
template<size_t N = 4> inline pod::Vector<T,N> vector( const value<T> );\
namespace uf {
namespace simd {
template<typename T>
@ -39,7 +63,7 @@ namespace uf {
};
template<typename T>
class /**UF_API**/ alignas(16) value {
class /*UF_API*/ alignas(16) value {
private:
// __m128 m_value;
typedef typename traits<T>::value value_type;
@ -57,100 +81,39 @@ namespace uf {
inline value(const pod::Vector<T,3>& rhs);
inline value(const pod::Vector<T,4>& rhs);
inline value operator+( const value& y );
inline value operator-( const value& y );
inline value operator*( const value& y );
inline value operator/( const value& y );
inline value operator+( const value& rhs );
inline value operator-( const value& rhs );
inline value operator*( const value& rhs );
inline value operator/( const value& rhs );
inline value operator<( const value& rhs );
inline value operator<=( const value& rhs );
inline value operator>( const value& rhs );
inline value operator>=( const value& rhs );
inline value operator==( const value& rhs );
inline value operator!=( const value& rhs );
inline value& operator=(const value_type& rhs);
inline value& operator=(const value& rhs);
inline value& operator=(const pod::Vector4f& rhs);
inline value& operator=(const pod::Vector<T,4>& rhs);
inline operator value_type() const;
template<size_t N> inline operator pod::Vector<T,N>() const;
};
inline value<float> /**UF_API**/ load( const float* );
inline void /**UF_API**/ store( value<float>, float* );
inline value<float> /**UF_API**/ set( float );
inline value<float> /**UF_API**/ set( float, float, float, float );
inline value<float> /**UF_API**/ add( value<float>, value<float> );
inline value<float> /**UF_API**/ sub( value<float>, value<float> );
inline value<float> /**UF_API**/ mul( value<float>, value<float> );
inline value<float> /**UF_API**/ div( value<float>, value<float> );
inline value<float> /**UF_API**/ min( value<float>, value<float> );
inline value<float> /**UF_API**/ max( value<float>, value<float> );
inline value<float> /**UF_API**/ sqrt( value<float> );
// inline value<float> /**UF_API**/ hadd( value<float>, value<float> );
inline float /**UF_API**/ dot( value<float>, value<float> );
template<size_t N=4> inline pod::Vector<float,N> vector( const value<float> );
inline value<int32_t> /**UF_API**/ load( const int32_t* );
inline void /**UF_API**/ store( value<int32_t>, int32_t* );
inline value<int32_t> /**UF_API**/ set( int32_t );
inline value<int32_t> /**UF_API**/ set( int32_t, int32_t, int32_t, int32_t );
inline value<int32_t> /**UF_API**/ add( value<int32_t>, value<int32_t> );
inline value<int32_t> /**UF_API**/ sub( value<int32_t>, value<int32_t> );
inline value<int32_t> /**UF_API**/ mul( value<int32_t>, value<int32_t> );
inline value<int32_t> /**UF_API**/ div( value<int32_t>, value<int32_t> );
inline value<int32_t> /**UF_API**/ min( value<int32_t>, value<int32_t> );
inline value<int32_t> /**UF_API**/ max( value<int32_t>, value<int32_t> );
inline value<int32_t> /**UF_API**/ sqrt( value<int32_t> );
// inline value<int32_t> /**UF_API**/ hadd( value<int32_t>, value<int32_t> );
inline int32_t /**UF_API**/ dot( value<int32_t>, value<int32_t> );
template<size_t N=4> inline pod::Vector<int32_t,N> vector( const value<int32_t> );
inline value<uint> /**UF_API**/ load( const uint* );
inline void /**UF_API**/ store( value<uint>, uint* );
inline value<uint> /**UF_API**/ set( uint );
inline value<uint> /**UF_API**/ set( uint, uint, uint, uint );
inline value<uint> /**UF_API**/ add( value<uint>, value<uint> );
inline value<uint> /**UF_API**/ sub( value<uint>, value<uint> );
inline value<uint> /**UF_API**/ mul( value<uint>, value<uint> );
inline value<uint> /**UF_API**/ div( value<uint>, value<uint> );
inline value<uint> /**UF_API**/ min( value<uint>, value<uint> );
inline value<uint> /**UF_API**/ max( value<uint>, value<uint> );
inline value<uint> /**UF_API**/ sqrt( value<uint> );
// inline value<uint> /**UF_API**/ hadd( value<uint>, value<uint> );
inline uint /**UF_API**/ dot( value<uint>, value<uint> );
template<size_t N=4> inline pod::Vector<uint,N> vector( const value<uint> );
DEFINE_SIMD(float);
DEFINE_SIMD(int32_t);
DEFINE_SIMD(uint32_t);
// these are effectively NOPs
#if UF_USE_FLOAT16
inline value<std::float16_t> /**UF_API**/ load( const std::float16_t* ) { return {}; }
inline void /**UF_API**/ store( value<std::float16_t>, std::float16_t* ) { return; }
inline value<std::float16_t> /**UF_API**/ set( std::float16_t ) { return {}; }
inline value<std::float16_t> /**UF_API**/ set( std::float16_t, std::float16_t, std::float16_t, std::float16_t ) { return {}; }
inline value<std::float16_t> /**UF_API**/ add( value<std::float16_t>, value<std::float16_t> ) { return {}; }
inline value<std::float16_t> /**UF_API**/ sub( value<std::float16_t>, value<std::float16_t> ) { return {}; }
inline value<std::float16_t> /**UF_API**/ mul( value<std::float16_t>, value<std::float16_t> ) { return {}; }
inline value<std::float16_t> /**UF_API**/ div( value<std::float16_t>, value<std::float16_t> ) { return {}; }
inline value<std::float16_t> /**UF_API**/ min( value<std::float16_t>, value<std::float16_t> ) { return {}; }
inline value<std::float16_t> /**UF_API**/ max( value<std::float16_t>, value<std::float16_t> ) { return {}; }
inline value<std::float16_t> /**UF_API**/ sqrt( value<std::float16_t> ) { return {}; }
// inline value<std::float16_t> /**UF_API**/ hadd( value<std::float16_t>, value<std::float16_t> ) { return {}; }
inline std::float16_t /**UF_API**/ dot( value<std::float16_t>, value<std::float16_t> ) { return {}; }
template<size_t N=4> inline pod::Vector<std::float16_t,N> vector( const value<std::float16_t> ) { return {}; }
DEFINE_SIMD(std::float16_t)
#endif
#if UF_USE_BFLOAT16
inline value<std::bfloat16_t> /**UF_API**/ load( const std::bfloat16_t* ) { return {}; }
inline void /**UF_API**/ store( value<std::bfloat16_t>, std::bfloat16_t* ) { return; }
inline value<std::bfloat16_t> /**UF_API**/ set( std::bfloat16_t ) { return {}; }
inline value<std::bfloat16_t> /**UF_API**/ set( std::bfloat16_t, std::bfloat16_t, std::bfloat16_t, std::bfloat16_t ) { return {}; }
inline value<std::bfloat16_t> /**UF_API**/ add( value<std::bfloat16_t>, value<std::bfloat16_t> ) { return {}; }
inline value<std::bfloat16_t> /**UF_API**/ sub( value<std::bfloat16_t>, value<std::bfloat16_t> ) { return {}; }
inline value<std::bfloat16_t> /**UF_API**/ mul( value<std::bfloat16_t>, value<std::bfloat16_t> ) { return {}; }
inline value<std::bfloat16_t> /**UF_API**/ div( value<std::bfloat16_t>, value<std::bfloat16_t> ) { return {}; }
inline value<std::bfloat16_t> /**UF_API**/ min( value<std::bfloat16_t>, value<std::bfloat16_t> ) { return {}; }
inline value<std::bfloat16_t> /**UF_API**/ max( value<std::bfloat16_t>, value<std::bfloat16_t> ) { return {}; }
inline value<std::bfloat16_t> /**UF_API**/ sqrt( value<std::bfloat16_t> ) { return {}; }
// inline value<std::bfloat16_t> /**UF_API**/ hadd( value<std::bfloat16_t>, value<std::bfloat16_t> ) { return {}; }
inline std::bfloat16_t /**UF_API**/ dot( value<std::bfloat16_t>, value<std::bfloat16_t> ) { return {}; }
template<size_t N=4> inline pod::Vector<std::bfloat16_t,N> vector( const value<std::bfloat16_t> ) { return {}; }
DEFINE_SIMD(std::bfloat16_t)
#endif
}
}
#include "simd.inl"

435
engine/inc/uf/utils/math/vector/simd.inl
View File

@ -1,5 +1,12 @@
#include <uf/utils/memory/alignment.h>
namespace {
inline __m128i bias_unsigned(__m128i v) {
const __m128i signbit = _mm_set1_epi32(0x80000000);
return _mm_xor_si128(v, signbit);
}
}
template<typename T>
inline uf::simd::value<T>::value() {}
template<typename T>
@ -39,6 +46,30 @@ inline uf::simd::value<T> uf::simd::value<T>::operator/( const value& rhs ) {
return uf::simd::div( *this, rhs );
}
template<typename T>
inline uf::simd::value<T> uf::simd::value<T>::operator<( const value& rhs ) {
return uf::simd::less( *this, rhs );
}
template<typename T>
inline uf::simd::value<T> uf::simd::value<T>::operator<=( const value& rhs ) {
return uf::simd::lessEquals( *this, rhs );
}
template<typename T>
inline uf::simd::value<T> uf::simd::value<T>::operator>( const value& rhs ) {
return uf::simd::greater( *this, rhs );
}
template<typename T>
inline uf::simd::value<T> uf::simd::value<T>::operator>=( const value& rhs ) {
return uf::simd::greaterEquals( *this, rhs );
}
template<typename T>
inline uf::simd::value<T> uf::simd::value<T>::operator==( const value& rhs ) {
return uf::simd::equals( *this, rhs );
}
template<typename T>
inline uf::simd::value<T> uf::simd::value<T>::operator!=( const value& rhs ) {
return uf::simd::notEquals( *this, rhs );
}
template<typename T>
inline uf::simd::value<T>& uf::simd::value<T>::operator=(const uf::simd::value<T>::value_type& rhs) {
m_value = rhs;
return *this;
@ -49,7 +80,7 @@ inline uf::simd::value<T>& uf::simd::value<T>::operator=(const value& rhs) {
return *this;
}
template<typename T>
inline uf::simd::value<T>& uf::simd::value<T>::operator=(const pod::Vector4f& rhs) {
inline uf::simd::value<T>& uf::simd::value<T>::operator=(const pod::Vector<T,4>& rhs) {
m_value = uf::simd::load(&rhs[0]);
return *this;
}
@ -71,121 +102,120 @@ inline pod::Vector<float,N> uf::simd::vector( const uf::simd::value<float> v ){
return uf::vector::cast<float,N>(r);
}
template<size_t N>
inline pod::Vector<int,N> uf::simd::vector( const uf::simd::value<int> v ){
inline pod::Vector<int32_t,N> uf::simd::vector( const uf::simd::value<int32_t> v ){
pod::Vector4i r;
uf::simd::store( v, &r[0] );
return uf::vector::cast<int,N>(r);
return uf::vector::cast<int32_t,N>(r);
}
template<size_t N>
inline pod::Vector<uint,N> uf::simd::vector( const uf::simd::value<uint> v ){
inline pod::Vector<uint32_t,N> uf::simd::vector( const uf::simd::value<uint32_t> v ){
pod::Vector4ui r;
uf::simd::store( v, &r[0] );
return uf::vector::cast<uint,N>(r);
return uf::vector::cast<uint32_t,N>(r);
}
inline uf::simd::value<float> /*UF_API*/ uf::simd::load( const float* f ) {
#if UF_VECTOR_ALIGNED
return _mm_load_ps(f);
#else
if ( uf::aligned(f, 16) ) return _mm_load_ps(f);
alignas(16) float s[4];
memcpy( &s[0], f, sizeof(float) * 4 );
return _mm_loadu_ps(s);
#endif
inline uf::simd::value<float> uf::simd::load( const float* f ) {
// if ( uf::aligned(f, 16) ) return _mm_load_ps(f);
return _mm_loadu_ps(f);
}
inline void /*UF_API*/ uf::simd::store( uf::simd::value<float> v, float* f ) {
#if UF_VECTOR_ALIGNED
return _mm_store_ps(f, v);
#else
if ( uf::aligned(f, 16) ) return _mm_store_ps(f, v);
alignas(16) float s[4];
_mm_store_ps(&s[0], v);
memcpy( f, &s[0], sizeof(float) * 4 );
#endif
inline void uf::simd::store( uf::simd::value<float> v, float* f ) {
/* if ( uf::aligned(f, 16) ) _mm_store_ps(f, v);
else */ _mm_storeu_ps(f, v);
}
inline uf::simd::value<float> /*UF_API*/ uf::simd::set( float f ) {
inline uf::simd::value<float> uf::simd::set( float f ) {
return _mm_set1_ps(f);
}
inline uf::simd::value<float> /*UF_API*/ uf::simd::set( float x, float y, float z, float w ) {
inline uf::simd::value<float> uf::simd::set( float x, float y, float z, float w ) {
return _mm_setr_ps(x, y, z, w);
}
inline uf::simd::value<float> /*UF_API*/ uf::simd::add( uf::simd::value<float> x, uf::simd::value<float> y ) {
inline uf::simd::value<float> uf::simd::add( uf::simd::value<float> x, uf::simd::value<float> y ) {
return _mm_add_ps( x, y );
}
inline uf::simd::value<float> /*UF_API*/ uf::simd::sub( uf::simd::value<float> x, uf::simd::value<float> y ) {
inline uf::simd::value<float> uf::simd::sub( uf::simd::value<float> x, uf::simd::value<float> y ) {
return _mm_sub_ps( x, y );
}
inline uf::simd::value<float> /*UF_API*/ uf::simd::mul( uf::simd::value<float> x, uf::simd::value<float> y ) {
inline uf::simd::value<float> uf::simd::mul( uf::simd::value<float> x, uf::simd::value<float> y ) {
return _mm_mul_ps( x, y );
}
inline uf::simd::value<float> /*UF_API*/ uf::simd::div( uf::simd::value<float> x, uf::simd::value<float> y ) {
inline uf::simd::value<float> uf::simd::div( uf::simd::value<float> x, uf::simd::value<float> y ) {
return _mm_div_ps( x, y );
}
/*
inline uf::simd::value<float> uf::simd::hadd( uf::simd::value<float> x, uf::simd::value<float> y ) {
#if 0
return _mm_hadd_ps( x, y );
#else
__m128 shuf = _mm_movehdup_ps(v);
__m128 sums = _mm_add_ps(v, shuf);
shuf = _mm_movehl_ps(shuf, sums);
sums = _mm_add_ss(sums, shuf);
return _mm_cvtss_f32(sums);
#endif
}
*/
inline uf::simd::value<float> /*UF_API*/ uf::simd::min( uf::simd::value<float> x, uf::simd::value<float> y ) {
inline uf::simd::value<float> uf::simd::min( uf::simd::value<float> x, uf::simd::value<float> y ) {
return _mm_min_ps( x, y );
}
inline uf::simd::value<float> /*UF_API*/ uf::simd::max( uf::simd::value<float> x, uf::simd::value<float> y ) {
inline uf::simd::value<float> uf::simd::max( uf::simd::value<float> x, uf::simd::value<float> y ) {
return _mm_max_ps( x, y );
}
inline uf::simd::value<float> /*UF_API*/ uf::simd::sqrt( uf::simd::value<float> v ) {
inline bool uf::simd::all( uf::simd::value<float> mask) {
return _mm_movemask_ps(mask) == 0xF; // all 4 bits set
}
inline bool uf::simd::any( uf::simd::value<float> mask) {
return _mm_movemask_ps(mask) != 0x0; // any bit set
}
inline uf::simd::value<float> uf::simd::less( uf::simd::value<float> x, uf::simd::value<float> y ) {
return _mm_cmplt_ps( x, y );
}
inline uf::simd::value<float> uf::simd::lessEquals( uf::simd::value<float> x, uf::simd::value<float> y ) {
return _mm_cmple_ps( x, y );
}
inline uf::simd::value<float> uf::simd::greater( uf::simd::value<float> x, uf::simd::value<float> y ) {
return _mm_cmpgt_ps( x, y );
}
inline uf::simd::value<float> uf::simd::greaterEquals( uf::simd::value<float> x, uf::simd::value<float> y ) {
return _mm_cmpge_ps( x, y );
}
inline uf::simd::value<float> uf::simd::equals( uf::simd::value<float> x, uf::simd::value<float> y ) {
return _mm_cmpeq_ps( x, y );
}
inline uf::simd::value<float> uf::simd::notEquals( uf::simd::value<float> x, uf::simd::value<float> y ) {
return _mm_cmpneq_ps( x, y );
}
inline uf::simd::value<float> uf::simd::sqrt( uf::simd::value<float> v ) {
return _mm_sqrt_ps( v );
}
inline float /*UF_API*/ uf::simd::dot( uf::simd::value<float> x, uf::simd::value<float> y ) {
inline float uf::simd::dot( uf::simd::value<float> x, uf::simd::value<float> y ) {
#if SSE_INSTR_SET >= 5
float res;
__m128 result = _mm_dp_ps(x, y, 0xFF);
_mm_store_ss(&res, result);
return res;
// return uf::simd::vector<float,4>( result )[0];
__m128 result = _mm_dp_ps(x, y, 0xF1);
return _mm_cvtss_f32(result);
#elif SSE_INSTR_SET >= 3
__m128 mulRes = _mm_mul_ps(x, y);
__m128 shufReg = _mm_movehdup_ps(mulRes);
__m128 sumsReg = _mm_add_ps(mulRes, shufReg);
shufReg = _mm_movehl_ps(shufReg, sumsReg);
sumsReg = _mm_add_ss(sumsReg, shufReg);
return _mm_cvtss_f32(sumsReg);
shufReg = _mm_movehl_ps(shufReg, sumsReg);
sumsReg = _mm_add_ss(sumsReg, shufReg);
return _mm_cvtss_f32(sumsReg);
#else
return uf::vector::sum( uf::simd::vector( uf::simd::mul( x, y ) ) );
#endif
}
inline uf::simd::value<int32_t> /*UF_API*/ uf::simd::load( const int32_t* f ) {
inline uf::simd::value<int32_t> uf::simd::load( const int32_t* f ) {
#if SSE_INSTR_SET >= 3
#if UF_VECTOR_ALIGNED
return _mm_load_si128((__m128i*) f);
#else
if ( uf::aligned(f, 16) ) return _mm_load_si128((__m128i*) f);
alignas(16) int32_t s[4];
memcpy( &s[0], f, sizeof(int32_t) * 4 );
return _mm_load_si128((__m128i*) s);
#endif
// if ( uf::aligned(f, 16) ) return _mm_load_si128(reinterpret_cast<const __m128i*>(f));
return _mm_loadu_si128(reinterpret_cast<const __m128i*>(f));
#else
return uf::simd::value<int32_t>( f[0], f[1], f[2], f[3] );
#endif
}
inline void /*UF_API*/ uf::simd::store( uf::simd::value<int32_t> v, int32_t* f ) {
inline void uf::simd::store( uf::simd::value<int32_t> v, int32_t* f ) {
#if SSE_INSTR_SET >= 3
#if UF_VECTOR_ALIGNED
return _mm_store_si128((__m128i*) f, v);
/*if ( uf::aligned(f, 16) ) _mm_store_si128(reinterpret_cast<__m128i*>(f), v);
else*/ _mm_storeu_si128(reinterpret_cast<__m128i*>(f), v);
#else
if ( uf::aligned(f, 16) ) return _mm_store_si128((__m128i*) f, v);
alignas(16) int32_t s[4];
_mm_store_si128((__m128i*) &s[0], v);
memcpy( f, &s[0], sizeof(int32_t) * 4 );
#endif
#else
union {
__m128i x;
int32_t y[4];
} kludge;
union { __m128i x; int32_t y[4]; } kludge;
kludge.x = v;
f[0] = kludge.y[0];
f[1] = kludge.y[1];
@ -193,28 +223,28 @@ inline void /*UF_API*/ uf::simd::store( uf::simd::value<int32_t> v, int32_t* f )
f[3] = kludge.y[3];
#endif
}
inline uf::simd::value<int32_t> /*UF_API*/ uf::simd::set( int32_t f ) {
inline uf::simd::value<int32_t> uf::simd::set( int32_t f ) {
return _mm_set1_epi32(f);
}
inline uf::simd::value<int32_t> /*UF_API*/ uf::simd::set( int32_t x, int32_t y, int32_t z, int32_t w ) {
inline uf::simd::value<int32_t> uf::simd::set( int32_t x, int32_t y, int32_t z, int32_t w ) {
return _mm_setr_epi32(x, y, z, w);
}
inline uf::simd::value<int32_t> /*UF_API*/ uf::simd::add( uf::simd::value<int32_t> x, uf::simd::value<int32_t> y ) {
auto X = uf::simd::vector( x );
auto Y = uf::simd::vector( y );
return uf::simd::set( X[0] + Y[0], X[1] + Y[1], X[2] + Y[2], X[3] + Y[3] );
inline uf::simd::value<int32_t> uf::simd::add( uf::simd::value<int32_t> x, uf::simd::value<int32_t> y ) {
return _mm_add_epi32(x, y);
}
inline uf::simd::value<int32_t> /*UF_API*/ uf::simd::sub( uf::simd::value<int32_t> x, uf::simd::value<int32_t> y ) {
auto X = uf::simd::vector( x );
auto Y = uf::simd::vector( y );
return uf::simd::set( X[0] - Y[0], X[1] - Y[1], X[2] - Y[2], X[3] - Y[3] );
inline uf::simd::value<int32_t> uf::simd::sub( uf::simd::value<int32_t> x, uf::simd::value<int32_t> y ) {
return _mm_sub_epi32(x, y);
}
inline uf::simd::value<int32_t> /*UF_API*/ uf::simd::mul( uf::simd::value<int32_t> x, uf::simd::value<int32_t> y ) {
auto X = uf::simd::vector( x );
auto Y = uf::simd::vector( y );
return uf::simd::set( X[0] * Y[0], X[1] * Y[1], X[2] * Y[2], X[3] * Y[3] );
inline uf::simd::value<int32_t> uf::simd::mul( uf::simd::value<int32_t> x, uf::simd::value<int32_t> y ) {
#if SSE_INSTR_SET >= 4
return _mm_mullo_epi32(x, y);
#else
auto X = uf::simd::vector(x);
auto Y = uf::simd::vector(y);
return uf::simd::set(X[0]*Y[0], X[1]*Y[1], X[2]*Y[2], X[3]*Y[3]);
#endif
}
inline uf::simd::value<int32_t> /*UF_API*/ uf::simd::div( uf::simd::value<int32_t> x, uf::simd::value<int32_t> y ) {
inline uf::simd::value<int32_t> uf::simd::div( uf::simd::value<int32_t> x, uf::simd::value<int32_t> y ) {
auto X = uf::simd::vector( x );
auto Y = uf::simd::vector( y );
return uf::simd::set( X[0] / Y[0], X[1] / Y[1], X[2] / Y[2], X[3] / Y[3] );
@ -226,57 +256,94 @@ inline uf::simd::value<int32_t> uf::simd::hadd( uf::simd::value<int32_t> x, uf::
return uf::simd::set( X[0] + Y[0], X[1] + Y[1], X[2] + Y[2], X[3] + Y[3] );
}
*/
inline uf::simd::value<int32_t> /*UF_API*/ uf::simd::min( uf::simd::value<int32_t> x, uf::simd::value<int32_t> y ) {
auto X = uf::simd::vector( x );
auto Y = uf::simd::vector( y );
return uf::simd::set( std::min(X[0], Y[0]), std::min(X[1], Y[1]), std::min(X[2], Y[2]), std::min(X[3], Y[3]) );
inline uf::simd::value<int32_t> uf::simd::min( uf::simd::value<int32_t> x, uf::simd::value<int32_t> y ) {
#if SSE_INSTR_SET >= 4
return _mm_min_epi32(x, y);
#else
auto X = uf::simd::vector(x);
auto Y = uf::simd::vector(y);
return uf::simd::set(std::min(X[0],Y[0]), std::min(X[1],Y[1]), std::min(X[2],Y[2]), std::min(X[3],Y[3]));
#endif
}
inline uf::simd::value<int32_t> /*UF_API*/ uf::simd::max( uf::simd::value<int32_t> x, uf::simd::value<int32_t> y ) {
auto X = uf::simd::vector( x );
auto Y = uf::simd::vector( y );
return uf::simd::set( std::max(X[0], Y[0]), std::max(X[1], Y[1]), std::max(X[2], Y[2]), std::max(X[3], Y[3]) );
inline uf::simd::value<int32_t> uf::simd::max( uf::simd::value<int32_t> x, uf::simd::value<int32_t> y ) {
#if SSE_INSTR_SET >= 4
return _mm_max_epi32(x, y);
#else
auto X = uf::simd::vector(x);
auto Y = uf::simd::vector(y);
return uf::simd::set(std::max(X[0],Y[0]), std::max(X[1],Y[1]), std::max(X[2],Y[2]), std::max(X[3],Y[3]));
#endif
}
inline uf::simd::value<int32_t> /*UF_API*/ uf::simd::sqrt( uf::simd::value<int32_t> v ) {
inline bool uf::simd::all( uf::simd::value<int32_t> mask) {
return _mm_movemask_epi8( mask ) == 0xFFFF; // all 4 bits set
}
inline bool uf::simd::any( uf::simd::value<int32_t> mask) {
return _mm_movemask_epi8( mask ) != 0x0; // any bit set
}
inline uf::simd::value<int32_t> uf::simd::less( uf::simd::value<int32_t> x, uf::simd::value<int32_t> y ) {
#if SSE_INSTR_SET >= 4
return _mm_cmplt_epi32( x, y );
#else
auto X = vector( x ), Y = vector( y );
return set(X[0] < Y[0], X[1] < Y[1], X[2] < Y[2], X[3] < Y[3]);
#endif
}
inline uf::simd::value<int32_t> uf::simd::lessEquals( uf::simd::value<int32_t> x, uf::simd::value<int32_t> y ) {
#if SSE_INSTR_SET >= 4
__m128i gt = _mm_cmpgt_epi32(x, y);
return _mm_xor_si128(gt, _mm_set1_epi32(-1));
#else
auto X = vector( x ), Y = vector( y );
return uf::simd::set(X[0] <= Y[0], X[1] <= Y[1], X[2] <= Y[2], X[3] <= Y[3]);
#endif
}
inline uf::simd::value<int32_t> uf::simd::greater( uf::simd::value<int32_t> x, uf::simd::value<int32_t> y ) {
#if SSE_INSTR_SET >= 4
return _mm_cmpgt_epi32( x, y );
#else
auto X = vector( x ), Y = vector( y );
return uf::simd::set(X[0] > Y[0], X[1] > Y[1], X[2] > Y[2], X[3] > Y[3]);
#endif
}
inline uf::simd::value<int32_t> uf::simd::greaterEquals( uf::simd::value<int32_t> x, uf::simd::value<int32_t> y ) {
#if SSE_INSTR_SET >= 4
__m128i gt = _mm_cmplt_epi32(x, y);
return _mm_xor_si128(gt, _mm_set1_epi32(-1));
#else
auto X = vector( x ), Y = vector( y );
return uf::simd::set(X[0] >= Y[0], X[1] >= Y[1], X[2] >= Y[2], X[3] >= Y[3]);
#endif
}
inline uf::simd::value<int32_t> uf::simd::equals( uf::simd::value<int32_t> x, uf::simd::value<int32_t> y ) {
return _mm_cmpeq_epi32(x, y);
}
inline uf::simd::value<int32_t> uf::simd::notEquals( uf::simd::value<int32_t> x, uf::simd::value<int32_t> y ) {
return _mm_xor_si128(_mm_cmpeq_epi32(x, y), _mm_set1_epi32(-1));
}
inline uf::simd::value<int32_t> uf::simd::sqrt( uf::simd::value<int32_t> v ) {
auto V = uf::simd::vector( v );
return uf::simd::set( (int32_t) std::sqrt(V[0]), (int32_t) std::sqrt(V[1]), (int32_t) std::sqrt(V[2]), (int32_t) std::sqrt(V[3]) );
}
inline int32_t /*UF_API*/ uf::simd::dot( uf::simd::value<int32_t> x, uf::simd::value<int32_t> y ) {
inline int32_t uf::simd::dot( uf::simd::value<int32_t> x, uf::simd::value<int32_t> y ) {
auto X = uf::simd::vector( x );
auto Y = uf::simd::vector( y );
return X[0] * Y[0] + X[1] * Y[1] + X[2] * Y[2] + X[3] * Y[3];
}
inline uf::simd::value<uint32_t> /*UF_API*/ uf::simd::load( const uint32_t* f ) {
inline uf::simd::value<uint32_t> uf::simd::load( const uint32_t* f ) {
#if SSE_INSTR_SET >= 3
#if UF_VECTOR_ALIGNED
return _mm_load_si128((__m128i*) f);
#else
if ( uf::aligned(f, 16) ) return _mm_load_si128((__m128i*) f);
alignas(16) uint32_t s[4];
memcpy( &s[0], f, sizeof(uint32_t) * 4 );
return _mm_load_si128((__m128i*) &s[0]);
#endif
// if ( uf::aligned(f, 16) ) return _mm_load_si128(reinterpret_cast<const __m128i*>(f));
return _mm_loadu_si128(reinterpret_cast<const __m128i*>(f));
#else
return uf::simd::value<uint32_t>( f[0], f[1], f[2], f[3] );
#endif
}
inline void /*UF_API*/ uf::simd::store( uf::simd::value<uint32_t> v, uint32_t* f ) {
inline void uf::simd::store( uf::simd::value<uint32_t> v, uint32_t* f ) {
#if SSE_INSTR_SET >= 3
#if UF_VECTOR_ALIGNED
return _mm_store_si128((__m128i*) f, v);
/*if ( uf::aligned(f, 16) ) _mm_store_si128(reinterpret_cast<__m128i*>(f), v);
else*/ _mm_storeu_si128(reinterpret_cast<__m128i*>(f), v);
#else
if ( uf::aligned(f, 16) ) return _mm_store_si128((__m128i*) f, v);
alignas(16) uint32_t s[4];
_mm_store_si128((__m128i*) &s[0], v);
memcpy( f, &s[0], sizeof(uint32_t) * 4 );
#endif
#else
union {
__m128i x;
uint32_t y[4];
} kludge;
union { __m128i x; uint32_t y[4]; } kludge;
kludge.x = v;
f[0] = kludge.y[0];
f[1] = kludge.y[1];
@ -284,50 +351,28 @@ inline void /*UF_API*/ uf::simd::store( uf::simd::value<uint32_t> v, uint32_t* f
f[3] = kludge.y[3];
#endif
}
inline uf::simd::value<uint32_t> /*UF_API*/ uf::simd::set( uint32_t f ) {
#if 0
union {
__m128i x;
uint32_t y[4];
} kludge;
kludge.y[0] = f;
kludge.y[1] = f;
kludge.y[2] = f;
kludge.y[3] = f;
#else
inline uf::simd::value<uint32_t> uf::simd::set( uint32_t f ) {
return _mm_set1_epi32(f);
#endif
}
inline uf::simd::value<uint32_t> /*UF_API*/ uf::simd::set( uint32_t x, uint32_t y, uint32_t z, uint32_t w ) {
#if 0
union {
__m128i x;
uint32_t y[4];
} kludge;
kludge.y[0] = x;
kludge.y[1] = y;
kludge.y[2] = z;
kludge.y[3] = w;
#else
inline uf::simd::value<uint32_t> uf::simd::set( uint32_t x, uint32_t y, uint32_t z, uint32_t w ) {
return _mm_setr_epi32(x, y, z, w);
}
inline uf::simd::value<uint32_t> uf::simd::add( uf::simd::value<uint32_t> x, uf::simd::value<uint32_t> y ) {
return _mm_add_epi32(x, y);
}
inline uf::simd::value<uint32_t> uf::simd::sub( uf::simd::value<uint32_t> x, uf::simd::value<uint32_t> y ) {
return _mm_sub_epi32(x, y);
}
inline uf::simd::value<uint32_t> uf::simd::mul( uf::simd::value<uint32_t> x, uf::simd::value<uint32_t> y ) {
#if SSE_INSTR_SET >= 4
return _mm_mullo_epi32(x, y);
#else
auto X = uf::simd::vector(x);
auto Y = uf::simd::vector(y);
return uf::simd::set(X[0]*Y[0], X[1]*Y[1], X[2]*Y[2], X[3]*Y[3]);
#endif
}
inline uf::simd::value<uint32_t> /*UF_API*/ uf::simd::add( uf::simd::value<uint32_t> x, uf::simd::value<uint32_t> y ) {
auto X = uf::simd::vector( x );
auto Y = uf::simd::vector( y );
return uf::simd::set( X[0] + Y[0], X[1] + Y[1], X[2] + Y[2], X[3] + Y[3] );
}
inline uf::simd::value<uint32_t> /*UF_API*/ uf::simd::sub( uf::simd::value<uint32_t> x, uf::simd::value<uint32_t> y ) {
auto X = uf::simd::vector( x );
auto Y = uf::simd::vector( y );
return uf::simd::set( X[0] - Y[0], X[1] - Y[1], X[2] - Y[2], X[3] - Y[3] );
}
inline uf::simd::value<uint32_t> /*UF_API*/ uf::simd::mul( uf::simd::value<uint32_t> x, uf::simd::value<uint32_t> y ) {
auto X = uf::simd::vector( x );
auto Y = uf::simd::vector( y );
return uf::simd::set( X[0] * Y[0], X[1] * Y[1], X[2] * Y[2], X[3] * Y[3] );
}
inline uf::simd::value<uint32_t> /*UF_API*/ uf::simd::div( uf::simd::value<uint32_t> x, uf::simd::value<uint32_t> y ) {
inline uf::simd::value<uint32_t> uf::simd::div( uf::simd::value<uint32_t> x, uf::simd::value<uint32_t> y ) {
auto X = uf::simd::vector( x );
auto Y = uf::simd::vector( y );
return uf::simd::set( X[0] / Y[0], X[1] / Y[1], X[2] / Y[2], X[3] / Y[3] );
@ -339,21 +384,81 @@ inline uf::simd::value<uint32_t> uf::simd::hadd( uf::simd::value<uint32_t> x, uf
return uf::simd::set( X[0] + Y[0], X[1] + Y[1], X[2] + Y[2], X[3] + Y[3] );
}
*/
inline uf::simd::value<uint32_t> /*UF_API*/ uf::simd::min( uf::simd::value<uint32_t> x, uf::simd::value<uint32_t> y ) {
auto X = uf::simd::vector( x );
auto Y = uf::simd::vector( y );
return uf::simd::set( std::min(X[0], Y[0]), std::min(X[1], Y[1]), std::min(X[2], Y[2]), std::min(X[3], Y[3]) );
inline uf::simd::value<uint32_t> uf::simd::min( uf::simd::value<uint32_t> x, uf::simd::value<uint32_t> y ) {
#if SSE_INSTR_SET >= 4
return _mm_min_epu32(x, y); // unsigned min
#else
auto X = uf::simd::vector(x);
auto Y = uf::simd::vector(y);
return uf::simd::set(std::min(X[0],Y[0]), std::min(X[1],Y[1]), std::min(X[2],Y[2]), std::min(X[3],Y[3]));
#endif
}
inline uf::simd::value<uint32_t> /*UF_API*/ uf::simd::max( uf::simd::value<uint32_t> x, uf::simd::value<uint32_t> y ) {
auto X = uf::simd::vector( x );
auto Y = uf::simd::vector( y );
return uf::simd::set( std::max(X[0], Y[0]), std::max(X[1], Y[1]), std::max(X[2], Y[2]), std::max(X[3], Y[3]) );
inline uf::simd::value<uint32_t> uf::simd::max( uf::simd::value<uint32_t> x, uf::simd::value<uint32_t> y ) {
#if SSE_INSTR_SET >= 4
return _mm_max_epu32(x, y); // unsigned max
#else
auto X = uf::simd::vector(x);
auto Y = uf::simd::vector(y);
return uf::simd::set(std::max(X[0],Y[0]), std::max(X[1],Y[1]), std::max(X[2],Y[2]), std::max(X[3],Y[3]));
#endif
}
inline uf::simd::value<uint32_t> /*UF_API*/ uf::simd::sqrt( uf::simd::value<uint32_t> v ) {
inline bool uf::simd::all( uf::simd::value<uint32_t> mask) {
return _mm_movemask_epi8( mask ) == 0xFFFF; // all 4 bits set
}
inline bool uf::simd::any( uf::simd::value<uint32_t> mask) {
return _mm_movemask_epi8( mask ) != 0x0; // any bit set
}
inline uf::simd::value<uint32_t> uf::simd::less( uf::simd::value<uint32_t> x, uf::simd::value<uint32_t> y ) {
#if SSE_INSTR_SET >= 4
return _mm_cmplt_epi32( ::bias_unsigned( x ), ::bias_unsigned( y ) );
#else
auto X = vector( x ), Y = vector( y );
return set(X[0] < Y[0], X[1] < Y[1], X[2] < Y[2], X[3] < Y[3]);
#endif
}
inline uf::simd::value<uint32_t> uf::simd::lessEquals(value<uint32_t> x, value<uint32_t> y) {
#if SSE_INSTR_SET >= 2
// a <= b <=> !(a > b)
__m128i bx = ::bias_unsigned(x);
__m128i by = ::bias_unsigned(y);
__m128i gt = _mm_cmpgt_epi32(bx, by); // signed compare
return _mm_xor_si128(gt, _mm_set1_epi32(-1)); // invert mask
#else
auto X = vector(x), Y = vector(y);
return set(X[0] <= Y[0], X[1] <= Y[1], X[2] <= Y[2], X[3] <= Y[3]);
#endif
}
inline uf::simd::value<uint32_t> uf::simd::greater( uf::simd::value<uint32_t> x, uf::simd::value<uint32_t> y ) {
#if SSE_INSTR_SET >= 4
return _mm_cmpgt_epi32( ::bias_unsigned( x ), ::bias_unsigned( y ) );
#else
auto X = vector( x ), Y = vector( y );
return uf::simd::set(X[0] > Y[0], X[1] > Y[1], X[2] > Y[2], X[3] > Y[3]);
#endif
}
inline uf::simd::value<uint32_t> uf::simd::greaterEquals(value<uint32_t> x, value<uint32_t> y) {
#if SSE_INSTR_SET >= 2
// a >= b <=> !(a < b)
__m128i bx = ::bias_unsigned(x);
__m128i by = ::bias_unsigned(y);
__m128i lt = _mm_cmplt_epi32(bx, by); // signed compare
return _mm_xor_si128(lt, _mm_set1_epi32(-1)); // invert mask
#else
auto X = vector(x), Y = vector(y);
return set(X[0] >= Y[0], X[1] >= Y[1], X[2] >= Y[2], X[3] >= Y[3]);
#endif
}
inline uf::simd::value<uint32_t> uf::simd::equals( uf::simd::value<uint32_t> x, uf::simd::value<uint32_t> y ) {
return _mm_cmpeq_epi32(x, y);
}
inline uf::simd::value<uint32_t> uf::simd::notEquals( uf::simd::value<uint32_t> x, uf::simd::value<uint32_t> y ) {
return _mm_xor_si128(_mm_cmpeq_epi32(x, y), _mm_set1_epi32(-1));
}
inline uf::simd::value<uint32_t> uf::simd::sqrt( uf::simd::value<uint32_t> v ) {
auto V = uf::simd::vector( v );
return uf::simd::set( (uint32_t) std::sqrt(V[0]), (uint32_t) std::sqrt(V[1]), (uint32_t) std::sqrt(V[2]), (uint32_t) std::sqrt(V[3]) );
}
inline uint32_t /*UF_API*/ uf::simd::dot( uf::simd::value<uint32_t> x, uf::simd::value<uint32_t> y ) {
inline uint32_t uf::simd::dot( uf::simd::value<uint32_t> x, uf::simd::value<uint32_t> y ) {
auto X = uf::simd::vector( x );
auto Y = uf::simd::vector( y );
return X[0] * Y[0] + X[1] * Y[1] + X[2] * Y[2] + X[3] * Y[3];

3
engine/inc/uf/utils/math/vector/vector.inl
View File

@ -1,7 +1,4 @@
#include "pod.inl"
#if UF_USE_CLASS_OF_PODS
#include "class.inl"
#endif
template<typename T, size_t N>
uf::stl::string /*UF_API*/ uf::string::toString( const pod::Vector<T,N>& v ) {

23
engine/inc/uf/utils/memory/unordered_set.h Normal file
View File

@ -0,0 +1,23 @@
#pragma once
#include <uf/config.h>
#include "./allocator.h"
#include <unordered_set>
#include "vector.h"
namespace uf {
namespace stl {
template<
class Key,
class Hash = std::hash<Key>,
class KeyEqual = std::equal_to<Key>,
#if UF_MEMORYPOOL_USE_STL_ALLOCATOR
class Allocator = std::allocator<Key>
#else
class Allocator = uf::Allocator<Key>
#endif
>
using unordered_set = std::unordered_set<Key, Hash, KeyEqual, Allocator>;
}
}

2
engine/src/engine/graph/graph.cpp
View File

@ -1297,7 +1297,7 @@ void uf::graph::process( pod::Graph& graph, int32_t index, uf::Object& parent )
if ( tag["transform"]["offset"].as<bool>() ) {
auto parsed = uf::transform::decode( tag["transform"], pod::Transform<>{} );
transform.position += parsed.position;
transform.orientation = uf::quaternion::multiply( transform.orientation, parsed.orientation );
transform.orientation = uf::quaternion::multiply( parsed.orientation, transform.orientation );
} else {
transform = uf::transform::decode( tag["transform"], transform );
if ( tag["transform"]["parent"].is<uf::stl::string>() ) {

2
engine/src/ext/lua/usertypes/vector.cpp
View File

@ -191,7 +191,7 @@ namespace binds {
return uf::quaternion::axisAngle( arg.as<pod::Vector3f>(), angle );
} else if ( arg.is<sol::table>() ) {
sol::table table = arg.as<sol::table>();
return uf::quaternion::axisAngle( pod::Vector3f{ table[0], table[1], table[2] }, angle );
return uf::quaternion::axisAngle( pod::Vector3f{ (float) table[0], table[1], table[2] }, angle );
}
return ::Quaternion{};
}

2
engine/src/ext/vulkan/graphic.cpp
View File

@ -1588,7 +1588,7 @@ void ext::vulkan::Graphic::generateTopAccelerationStructure( const uf::stl::vect
for ( auto& blas : graphic->accelerationStructures.bottoms ) {
auto& instance = instances[blas.instanceID];
auto mat = modelMatrices[instance.objectID];
mat = uf::matrix::transpose(mat);
mat = uf::matrix::transpose(mat); // might need to not do this
auto& instanceVK = instancesVK.emplace_back();
memcpy(&instanceVK.transform, &mat, sizeof(instanceVK.transform));

73
engine/src/utils/math/physics/aabb.inl
View File

@ -1,15 +1,6 @@
namespace {
bool aabbOverlap( const pod::AABB& a, const pod::AABB& b, float eps ) {
for (int axis=0; axis<3; ++axis) {
if (a.max[axis] + eps < b.min[axis] - eps) return false;
if (a.min[axis] - eps > b.max[axis] + eps) return false;
}
return true;
/*
return !(a.max.x < b.min.x + eps || a.min.x > b.max.x - eps ||
a.max.y < b.min.y + eps || a.min.y > b.max.y - eps ||
a.max.z < b.min.z + eps || a.min.z > b.max.z - eps);
*/
inline bool aabbOverlap( const pod::AABB& a, const pod::AABB& b, float eps ) {
return ( a.min - eps ) <= ( b.max + eps ) && ( a.max + eps ) >= ( b.min - eps );
}
inline float aabbSurfaceArea(const pod::AABB& aabb) {
@ -25,11 +16,7 @@ namespace {
}
pod::Vector3f closestPointOnAABB(const pod::Vector3f& p, const pod::AABB& box) {
return {
std::max(box.min.x, std::min(p.x, box.max.x)),
std::max(box.min.y, std::min(p.y, box.max.y)),
std::max(box.min.z, std::min(p.z, box.max.z))
};
return uf::vector::clamp( p, box.min, box.max );
}
std::pair<pod::Vector3f, pod::Vector3f> getCapsuleSegment( const pod::PhysicsBody& body ) {
@ -78,16 +65,8 @@ namespace {
pod::AABB computeTriangleAABB( const pod::Triangle& tri ) {
return {
{
std::min({tri.points[0].x, tri.points[1].x, tri.points[2].x}),
std::min({tri.points[0].y, tri.points[1].y, tri.points[2].y}),
std::min({tri.points[0].z, tri.points[1].z, tri.points[2].z}),
},
{
std::max({tri.points[0].x, tri.points[1].x, tri.points[2].x}),
std::max({tri.points[0].y, tri.points[1].y, tri.points[2].y}),
std::max({tri.points[0].z, tri.points[1].z, tri.points[2].z}),
},
uf::vector::min( uf::vector::min( tri.points[0], tri.points[1] ), tri.points[2] ),
uf::vector::max( uf::vector::max( tri.points[0], tri.points[1] ), tri.points[2] ),
};
}
@ -211,17 +190,13 @@ namespace {
if ( !::aabbOverlap( A, B ) ) return false;
// calculate overlap extents
float overlaps[3] = {
std::min(A.max.x, B.max.x) - std::max(A.min.x, B.min.x),
std::min(A.max.y, B.max.y) - std::max(A.min.y, B.min.y),
std::min(A.max.z, B.max.z) - std::max(A.min.z, B.min.z)
};
auto overlaps = uf::vector::min( A.max, B.max ) - uf::vector::max( A.min, B.min );
// determine collision axis = smallest overlap
int axis = -1;
float minOverlap = FLT_MAX;
for (int i = 0; i < 3; ++i) {
if (overlaps[i] < minOverlap) {
for ( auto i = 0; i < 3; ++i ) {
if ( overlaps[i] < minOverlap ) {
minOverlap = overlaps[i];
axis = i;
}
@ -232,31 +207,27 @@ namespace {
normal[axis] = (delta[axis] < 0 ? -1.0f : 1.0f);
// build manifold contacts: overlap region corners on the separating axis
float xMin = std::max(A.min.x, B.min.x);
float xMax = std::min(A.max.x, B.max.x);
float yMin = std::max(A.min.y, B.min.y);
float yMax = std::min(A.max.y, B.max.y);
float zMin = std::max(A.min.z, B.min.z);
float zMax = std::min(A.max.z, B.max.z);
auto Min = uf::vector::max( A.min, B.min );
auto Max = uf::vector::min( A.max, B.max );
// on chosen axis, clamp to overlapped rectangle -> 4 potential points
if (axis == 0) { // x-axis separation, so face-on overlap in YZ plane
manifold.points.emplace_back(pod::Contact{ { (normal.x > 0 ? A.max.x : A.min.x), yMin, zMin }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { (normal.x > 0 ? A.max.x : A.min.x), yMin, zMax }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { (normal.x > 0 ? A.max.x : A.min.x), yMax, zMin }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { (normal.x > 0 ? A.max.x : A.min.x), yMax, zMax }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { (normal.x > 0 ? A.max.x : A.min.x), Min.y, Min.z }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { (normal.x > 0 ? A.max.x : A.min.x), Min.y, Max.z }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { (normal.x > 0 ? A.max.x : A.min.x), Max.y, Min.z }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { (normal.x > 0 ? A.max.x : A.min.x), Max.y, Max.z }, normal, minOverlap });
}
else if (axis == 1) { // y-axis separation, overlap in XZ plane
manifold.points.emplace_back(pod::Contact{ { xMin, (normal.y > 0 ? A.max.y : A.min.y), zMin }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { xMin, (normal.y > 0 ? A.max.y : A.min.y), zMax }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { xMax, (normal.y > 0 ? A.max.y : A.min.y), zMin }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { xMax, (normal.y > 0 ? A.max.y : A.min.y), zMax }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { Min.x, (normal.y > 0 ? A.max.y : A.min.y), Min.z }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { Min.x, (normal.y > 0 ? A.max.y : A.min.y), Max.z }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { Max.x, (normal.y > 0 ? A.max.y : A.min.y), Min.z }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { Max.x, (normal.y > 0 ? A.max.y : A.min.y), Max.z }, normal, minOverlap });
}
else if (axis == 2) { // z-axis separation, overlap in XY plane
manifold.points.emplace_back(pod::Contact{ { xMin, yMin, (normal.z > 0 ? A.max.z : A.min.z) }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { xMin, yMax, (normal.z > 0 ? A.max.z : A.min.z) }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { xMax, yMin, (normal.z > 0 ? A.max.z : A.min.z) }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { xMax, yMax, (normal.z > 0 ? A.max.z : A.min.z) }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { Min.x, Min.y, (normal.z > 0 ? A.max.z : A.min.z) }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { Min.x, Max.y, (normal.z > 0 ? A.max.z : A.min.z) }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { Max.x, Min.y, (normal.z > 0 ? A.max.z : A.min.z) }, normal, minOverlap });
manifold.points.emplace_back(pod::Contact{ { Max.x, Max.y, (normal.z > 0 ? A.max.z : A.min.z) }, normal, minOverlap });
}
return true;

24
engine/src/utils/math/physics/bvh.inl
View File

@ -162,6 +162,8 @@ namespace {
}
void buildBroadphaseBVH( pod::BVH& bvh, const uf::stl::vector<pod::PhysicsBody*>& bodies, int capacity = 2 ) {
if ( bodies.empty() ) return;
bvh.indices.clear();
bvh.nodes.clear();
bvh.indices.reserve(bodies.size());
@ -207,7 +209,8 @@ namespace {
auto tris = view.index.count / 3;
for ( auto triIndexID = 0; triIndexID < tris; ++triIndexID ) {
auto aabb = ::computeTriangleAABB( positions.data(view.vertex.first), positions.stride(), indices.data(view.index.first), mesh.index.size, triIndexID );
auto tri = ::fetchTriangle( positions.data(view.vertex.first), positions.stride(), indices.data(view.index.first), mesh.index.size, triIndexID );
auto aabb = ::computeTriangleAABB( tri );
auto triID = triIndexID + (view.index.first / 3);
if ( triID != bounds.size() ) UF_MSG_DEBUG("triID={}, bounds.size()={}", triID, bounds.size());
@ -349,7 +352,8 @@ namespace {
auto tris = view.index.count / 3;
for ( auto triIndexID = 0; triIndexID < tris; ++triIndexID ) {
auto aabb = ::computeTriangleAABB( positions.data(view.vertex.first), positions.stride(), indices.data(view.index.first), mesh.index.size, triIndexID );
auto tri = ::fetchTriangle( positions.data(view.vertex.first), positions.stride(), indices.data(view.index.first), mesh.index.size, triIndexID );
auto aabb = ::computeTriangleAABB( tri );
bounds.emplace_back(aabb);
}
}
@ -414,7 +418,7 @@ namespace {
if ( bodyA == bodyB ) continue;
if ( bodyA > bodyB ) std::swap( bodyA, bodyB );
pairs.emplace_back(bodyA, bodyB);
pairs.emplace(bodyA, bodyB);
}
}
return;
@ -442,7 +446,7 @@ namespace {
int bodyA = bvhA.indices[nodeA.start + i];
int bodyB = bvhB.indices[nodeB.start + j];
pairs.emplace_back(bodyA, bodyB);
pairs.emplace(bodyA, bodyB);
}
}
return;
@ -470,7 +474,7 @@ namespace {
if ( bodyA == bodyB ) continue;
if ( bodyA > bodyB ) std::swap( bodyA, bodyB );
pairs.emplace_back(bodyA, bodyB);
pairs.emplace(bodyA, bodyB);
}
}
return;
@ -499,12 +503,12 @@ namespace {
namespace {
// query a BVH with an AABB via a stack
void queryBVH( const pod::BVH& bvh, const pod::AABB& bounds, uf::stl::vector<int32_t>& outIndices ) {
if ( bvh.nodes.empty() ) return;
if ( !bvh.flattened.empty() ) return ::queryFlatBVH( bvh, bounds, outIndices );
outIndices.reserve(::reserveCount);
if ( bvh.nodes.empty() ) return;
uf::stl::stack<int32_t> stack;
stack.push(0);
@ -598,7 +602,6 @@ namespace {
auto& nodes = bvh.flattened;
auto& indices = bvh.indices;
outPairs.clear();
outPairs.reserve(::reserveCount);
for ( auto i = 0; i < (int) nodes.size(); ++i ) {
@ -619,7 +622,7 @@ namespace {
if ( indexA == indexB ) continue;
if ( indexA > indexB ) std::swap( indexA, indexB );
outPairs.emplace_back( indexA, indexB );
outPairs.emplace( indexA, indexB );
}
}
}
@ -634,7 +637,6 @@ namespace {
if ( nodesA.empty() || nodesB.empty() ) return;
outPairs.clear();
outPairs.reserve(::reserveCount);
for ( auto i = 0; i < (int) nodesA.size(); ++i ) {
@ -652,7 +654,7 @@ namespace {
auto indexA = indicesA[nodeA.start + ia];
auto indexB = indicesB[nodeB.start + ib];
outPairs.emplace_back( indexA, indexB );
outPairs.emplace( indexA, indexB );
}
}
}

7
engine/src/utils/math/physics/helpers.inl
View File

@ -73,13 +73,6 @@ namespace {
return (idA << 32) ^ idB;
}
void deduplicatePairs( pod::BVH::pairs_t& pairs ) {
// should already be swapped
for (auto& [a, b] : pairs) if (a > b) std::swap(a, b);
std::sort(pairs.begin(), pairs.end());
pairs.erase(std::unique(pairs.begin(), pairs.end()), pairs.end());
}
// marks a body as asleep
void wakeBody( pod::PhysicsBody& body ) {
body.activity.awake = true;

42
engine/src/utils/math/physics/impl.cpp
View File

@ -25,8 +25,10 @@ namespace {
bool useBvhSahBodies = false;
bool useBvhSahMeshes = false;
bool useSplitBvhs = false; // currently bugged if enabled
int solverIterations = 10;
float baumgarteCorrectionPercent = 0.2f;
float baumgarteCorrectionPercent = 0.02f;
float baumgarteCorrectionSlop = 0.01f;
uf::stl::unordered_map<size_t, pod::Manifold> manifoldsCache;
@ -117,21 +119,34 @@ void uf::physics::impl::substep( pod::World& world, float dt, int substeps ) {
}
void uf::physics::impl::step( pod::World& world, float dt ) {
auto& bodies = world.bodies;
auto& bvh = world.bvh;
auto& dynamicBvh = world.dynamicBvh;
auto& staticBvh = world.staticBvh;
uf::stl::vector<pod::PhysicsBody*> staticBodies;
uf::stl::vector<pod::PhysicsBody*> dynamicBodies;
if ( bodies.empty() ) return;
++::frameCounter;
for ( auto* body : bodies ) {
( body->isStatic ? staticBodies : dynamicBodies ).emplace_back(body);
if ( !body->activity.awake ) continue;
::integrate( *body, dt );
}
switch ( ::decideBVHUpdate( bvh, bodies, ::bvhUpdatePolicy, ::frameCounter++ ) ) {
// rebuild static bvh if diry
if ( staticBvh.dirty && ::useSplitBvhs ) {
::buildBroadphaseBVH( staticBvh, staticBodies, ::broadphaseBvhCapacity ); // (re)build
}
switch ( ::decideBVHUpdate( dynamicBvh, ::useSplitBvhs ? dynamicBodies : bodies, ::bvhUpdatePolicy, ::frameCounter ) ) {
case pod::BVH::UpdatePolicy::Decision::REBUILD: {
::buildBroadphaseBVH( bvh, bodies, ::broadphaseBvhCapacity ); // (re)build
::buildBroadphaseBVH( dynamicBvh, ::useSplitBvhs ? dynamicBodies : bodies, ::broadphaseBvhCapacity ); // (re)build
} break;
case pod::BVH::UpdatePolicy::Decision::REFIT: {
::refitBVH( bvh, bodies ); // refit
::refitBVH( dynamicBvh, ::useSplitBvhs ? dynamicBodies : bodies ); // refit
} break;
case pod::BVH::UpdatePolicy::Decision::NONE:
default:
@ -141,12 +156,12 @@ void uf::physics::impl::step( pod::World& world, float dt ) {
// query for overlaps
pod::BVH::pairs_t pairs;
::queryOverlaps( bvh, pairs );
::deduplicatePairs( pairs );
::queryOverlaps( dynamicBvh, pairs );
if ( ::useSplitBvhs ) ::queryOverlaps( dynamicBvh, staticBvh, pairs );
// build islands
uf::stl::vector<pod::Island> islands;
::buildIslands( pairs, world.bodies, islands );
::buildIslands( pairs, bodies, islands );
// update sleep state per island
for ( auto& island : islands ) ::updateIsland( island, dt );
@ -194,7 +209,7 @@ void uf::physics::impl::step( pod::World& world, float dt ) {
if ( ::warmupSolver ) ::storeManifolds( manifolds, ::manifoldsCache );
// recompute bounds for further queries
for ( auto* body : bodies ) {
for ( auto* body : dynamicBodies ) {
body->bounds = ::computeAABB( *body );
}
}
@ -330,13 +345,14 @@ pod::PhysicsBody& uf::physics::impl::create( pod::World& world, uf::Object& obje
if ( body.isStatic ) {
uf::physics::impl::setColliderCategory(body, "STATIC");
uf::physics::impl::setColliderMask(body, "STATIC");
world.staticBvh.dirty = true; // mark as dirty
} else {
uf::physics::impl::setColliderCategory(body, "DYNAMIC");
uf::physics::impl::setColliderMask(body, "DYNAMIC");
world.dynamicBvh.dirty = true; // mark as dirty
}
world.bodies.emplace_back(&body); // insert into world
world.bvh.dirty = true; // mark as dirty
return body;
}
@ -459,11 +475,13 @@ pod::RayQuery uf::physics::impl::rayCast( const pod::Ray& ray, const pod::World&
pod::RayQuery rayHit;
rayHit.contact.penetration = maxDistance;
auto& bvh = world.bvh;
auto& dynamicBvh = world.dynamicBvh;
auto& staticBvh = world.dynamicBvh;
auto& bodies = world.bodies;
uf::stl::vector<int32_t> candidates;
::queryBVH( bvh, ray, candidates );
::queryBVH( dynamicBvh, ray, candidates );
if ( ::useSplitBvhs ) ::queryBVH( staticBvh, ray, candidates );
for ( auto i : candidates ) {
auto* b = bodies[i];

1
engine/src/utils/math/physics/mesh.inl
View File

@ -122,7 +122,6 @@ namespace {
// compute overlaps between one BVH and another BVH
pod::BVH::pairs_t pairs;
::queryOverlaps( bvhA, bvhB, pairs );
::deduplicatePairs( pairs );
bool hit = false;
// do collision per triangle

2
engine/src/utils/math/physics/solvers.inl
View File

@ -148,7 +148,7 @@ namespace {
}
residual = rhs - uf::matrix::multiply( K, lambda );
pod::Matrix<T,N> Kinv = uf::matrix::invert( K );
pod::Matrix<T,N> Kinv = uf::matrix::inverse( K );
pod::Vector<T,N> dLambda = uf::matrix::multiply( Kinv, residual );
for ( auto i = 0; i < N; i++ ) {

47
engine/src/utils/math/physics/triangle.inl
View File

@ -29,28 +29,11 @@ namespace {
//return uf::vector::normalize( tri.normals[0] + tri.normals[1] + tri.normals[2] );
}
pod::AABB computeTriangleAABB( const void* vertices, size_t vertexStride, const void* indexData, size_t indexSize, size_t triID ) {
auto triIndexID = triID * 3;
uint32_t i0 = ::getIndex( indexData, indexSize, triIndexID + 0 );
uint32_t i1 = ::getIndex( indexData, indexSize, triIndexID + 1 );
uint32_t i2 = ::getIndex( indexData, indexSize, triIndexID + 2 );
auto& v0 = *reinterpret_cast<const pod::Vector3f*>(reinterpret_cast<const uint8_t*>(vertices) + i0 * vertexStride);
auto& v1 = *reinterpret_cast<const pod::Vector3f*>(reinterpret_cast<const uint8_t*>(vertices) + i1 * vertexStride);
auto& v2 = *reinterpret_cast<const pod::Vector3f*>(reinterpret_cast<const uint8_t*>(vertices) + i2 * vertexStride);
pod::Triangle fetchTriangle( const void* vertices, size_t vertexStride, const void* indexData, size_t indexSize, size_t triID ) {
return {
{
std::min({v0.x, v1.x, v2.x}),
std::min({v0.y, v1.y, v2.y}),
std::min({v0.z, v1.z, v2.z}),
},
{
std::max({v0.x, v1.x, v2.x}),
std::max({v0.y, v1.y, v2.y}),
std::max({v0.z, v1.z, v2.z}),
}
*reinterpret_cast<const pod::Vector3f*>(reinterpret_cast<const uint8_t*>(vertices) + ::getIndex( indexData, indexSize, (triID * 3) + 0 ) * vertexStride),
*reinterpret_cast<const pod::Vector3f*>(reinterpret_cast<const uint8_t*>(vertices) + ::getIndex( indexData, indexSize, (triID * 3) + 1 ) * vertexStride),
*reinterpret_cast<const pod::Vector3f*>(reinterpret_cast<const uint8_t*>(vertices) + ::getIndex( indexData, indexSize, (triID * 3) + 2 ) * vertexStride),
};
}
@ -71,28 +54,12 @@ namespace {
triBase += trisInView;
}
UF_ASSERT( found );
uint32_t triIndexID = triID * 3; // remap triangle ID to index ID
pod::TriangleWithNormal tri;
auto& positions = (*found)["position"];
auto& normals = (*found)["normal"];
auto& indices = (*found)["index"];
const void* indexBase = indices.data(found->index.first);
size_t indexSize = mesh.index.size;
uint32_t idxs[3];
// to-do: just make this a macro that could have a parallel hint
for ( auto i = 0; i < 3; ++i ) idxs[i] = getIndex(indexBase, indexSize, triIndexID + i);
{
auto* base = reinterpret_cast<const uint8_t*>(positions.data(found->vertex.first));
size_t stride = positions.stride();
for ( auto i = 0; i < 3; ++i ) tri.points[i] = *reinterpret_cast<const pod::Vector3f*>(base + idxs[i] * stride);
}
pod::TriangleWithNormal tri = { ::fetchTriangle( positions.data(found->vertex.first), positions.stride(), indices.data(found->index.first), mesh.index.size, triID ) };
tri.normal = uf::vector::normalize(uf::vector::cross(tri.points[1] - tri.points[0], tri.points[2] - tri.points[0]));
/*
@ -373,7 +340,7 @@ namespace {
auto bounds = ::computeSegmentAABB( p1, p2, r );
// to-do: derive proper delta
pod::Vector3f closestSeg, closest;
pod::Vector3f closestSeg = {}, closest = {};
float dist2 = ::segmentTriangleDistanceSq( p1, p2, tri, closestSeg, closest );
if ( !uf::vector::isValid( closest ) ) return false;

338
engine/src/utils/math/rayt.cpp
View File

@ -1,338 +0,0 @@
#if 0
#include <uf/utils/math/collision.h>
#include <uf/utils/math/rayt.h>
#include <iostream>
// compile-time assert to ensure objects are of same size, if using multiple types of objects
// takes a vector of primitives and groups them under leaves (end point of a tree)
uf::stl::vector<pod::Tree> uf::primitive::populate( const uf::stl::vector<pod::Primitive>& cubes ) { // assert(cubes.size() > 0);
uf::stl::vector<pod::Tree> trees;
uf::stl::vector<const pod::Primitive*> copy;
uf::stl::vector<const pod::Primitive*> alloced;
copy.reserve( cubes.size() );
// std::cout << "Size: " << cubes.size() << std::endl;
for ( const auto& cube : cubes ) {
copy.push_back( &cube );
}
trees.reserve( copy.size() / pod::Tree::TREE_SIZE );
alloced.reserve( copy.size() );
const float cubeRoot = std::cbrt(pod::Tree::TREE_SIZE);
for ( const auto& cube : cubes ) {
// already allocated, skip
if ( std::find( alloced.begin(), alloced.end(), &cube ) != alloced.end() ) continue;
size_t index = 0;
pod::Tree tree;
// sort by closest cube
std::sort( copy.begin(), copy.end(), [&]( const pod::Primitive* l, const pod::Primitive* r ) {
return uf::vector::distanceSquared( cube.position, l->position ) < uf::vector::distanceSquared( cube.position, r->position );
} );
// std::cout << trees.size() << ": " << alloced.size() << std::endl;
while ( index < pod::Tree::TREE_SIZE && copy.begin() != copy.end() ) {
const pod::Primitive* ptr = *copy.begin();
copy.erase( copy.begin() );
tree.children[index++] = (ptr - &cubes[0]);
}
for ( size_t i = index; i < pod::Tree::TREE_SIZE; ++i ) {
tree.children[i] = pod::Primitive::EMPTY;
}
// mark children as allocated
/*
for ( int i = 0; i < pod::Tree::TREE_SIZE && i < index; ++i ) {
const auto& cube = cubes.at(tree.children[i]);
alloced.push_back(&cube);
tree.primitive.position.x += cube.position.x; tree.primitive.position.y += cube.position.y; tree.primitive.position.z += cube.position.z;
}
tree.primitive.position.x /= pod::Tree::TREE_SIZE;
tree.primitive.position.y /= pod::Tree::TREE_SIZE;
tree.primitive.position.z /= pod::Tree::TREE_SIZE;
*/
pod::Vector3f min = { 0, 0, 0 }, max = { 0, 0, 0 };
for ( int i = 0; i < pod::Tree::TREE_SIZE && i < index; ++i ) {
const auto& cube = cubes.at(tree.children[i]);
alloced.push_back(&cube);
if ( i == 0 ) {
min.x = cube.position.x - cube.position.w;
min.y = cube.position.y - cube.position.w;
min.z = cube.position.z - cube.position.w;
max.x = cube.position.x + cube.position.w;
max.y = cube.position.y + cube.position.w;
max.z = cube.position.z + cube.position.w;
continue;
}
min.x = std::min( cube.position.x - cube.position.w, min.x ); min.y = std::min( cube.position.y - cube.position.w, min.y ); min.z = std::min( cube.position.z - cube.position.w, min.z );
max.x = std::max( cube.position.x + cube.position.w, max.x ); max.y = std::max( cube.position.y + cube.position.w, max.y ); max.z = std::max( cube.position.z + cube.position.w, max.z );
}
tree.position.x = (max.x + min.x) * 0.5f;
tree.position.y = (max.y + min.y) * 0.5f;
tree.position.z = (max.z + min.z) * 0.5f;
tree.position.w = std::max( tree.position.w, (max.x - min.x) * 0.5f );
tree.position.w = std::max( tree.position.w, (max.y - min.y) * 0.5f );
tree.position.w = std::max( tree.position.w, (max.z - min.z) * 0.5f );
// tree.position.w = 0.5;
tree.type = pod::Primitive::LEAF; // leaves point to cubes
trees.push_back(tree);
}
return trees;
}
// takes a list of leaves (ends of trees) and properly populate an entire tree
uf::stl::vector<pod::Tree> uf::primitive::populateEntirely( const uf::stl::vector<pod::Tree>& trees, bool rooted ) { // assert(trees.size() > 0);
// generate first layer
uf::stl::vector<pod::Tree> tree = trees;
uf::stl::vector<uint32_t> referred;
uf::stl::vector<uint32_t> queue;
uint32_t iteration = 0;
uint32_t start = 0;
/*
std::cout << "Primitives:\n\t";
for ( auto& prim : primitives ) {
size_t i = &prim - &primitives[0];
std::cout << (i < 10 ? "0" : "" ) << i << " ";
if ( (i+1) % 8 == 0 ) std::cout << "\n\t";
}
std::cout << std::endl;
*/
loop: {
// std::cout << "Iteration #" << ++iteration << std::endl;
referred.clear();
// grab all unreferenced trees
for ( auto& branch : tree ) {
if ( branch.type != pod::Primitive::TREE ) continue;
referred.insert( referred.end(), &branch.children[0], &branch.children[7] );
// std::cout << "Tree #" << (&branch - &tree[0]) << " refers to:\n";
for ( uint i = 0; i < pod::Tree::TREE_SIZE; ++i ) {
// std::cout << "\t" << branch.children[i] << " ";
referred.push_back(branch.children[i]);
}
// std::cout << std::endl;
}
queue.clear();
// check if referred
// std::cout << "Queueing:\n\t";
for ( auto& branch : tree ) {
size_t i = &branch - &tree[0];
if ( std::find( referred.begin(), referred.end(), i ) != referred.end() ) continue;
// std::cout << i << " ";
queue.push_back( &branch - &tree[0] );
}
// std::cout << std::endl;
// grab the branches' primitives
uf::stl::vector<pod::Primitive> enqueued; enqueued.reserve( queue.size() );
for ( uint32_t i : queue ) {
pod::Tree t = tree.at(i);
pod::Primitive primitive;
primitive.position = t.position;
primitive.position.w = t.position.w;
primitive.type = i;
enqueued.push_back( primitive );
}
// create a parent tree
uf::stl::vector<pod::Tree> newTree = populate( enqueued );
// convert indices and type
for ( auto& branch : newTree ) {
branch.type = pod::Primitive::TREE;
branch.position.w = (iteration+1.0f) * 0.5f;
for ( size_t i = 0; i < pod::Tree::TREE_SIZE; ++i ) {
size_t index = branch.children[i];
if ( index == pod::Primitive::EMPTY ) continue;
branch.children[i] = enqueued.at(index).type;
}
}
// copy new tree to beginning of tree list
tree.insert( tree.end(), newTree.begin(), newTree.end() );
start = tree.size();
// cannot divide furter
if ( newTree.size() <= 1 ) goto finished;
}
goto loop;
finished:
if ( rooted ) tree.at(tree.size()-1).type = pod::Primitive::ROOT;
// std::cout << "Full tree:\n";
for ( auto& branch : tree ) {
// std::cout << "\t" << (&branch - &tree[0]) << " (type: " << (branch.type) << ") has children:\n\t\t";
for ( size_t i = 0; i < pod::Tree::TREE_SIZE; ++i ) {
// std::cout << branch.children[i] << " ";
}
// std::cout << std::endl;
}
// std::cout << std::endl;
for ( auto& branch : tree ) {
// std::cout << (&branch - &tree[0]) << " (type: " << (branch.type) << ") primitive:\n";
// std::cout << "\tPosition: " << branch.position.x << ", " << branch.position.y << ", " << branch.position.z << "\n\tSize: " << branch.position.w << "\n\t\t";
for ( size_t i = 0; i < pod::Tree::TREE_SIZE; ++i ) {
// std::cout << branch.children[i] << " ";
}
// std::cout << std::endl;
/*
if ( branch.type == pod::Primitive::LEAF ) {
for ( size_t i = 0; i < pod::Tree::TREE_SIZE; ++i ) {
if ( branch.children[i] == pod::Primitive::EMPTY ) continue;
auto& pr = primitives.at(branch.children[i]);
// std::cout << "\t\tPosition: " << pr.position.x << ", " << pr.position.y << ", " << pr.position.z << "\n\t\tSize: " << pr.position.w << std::endl;
}
} else {
for ( size_t i = 0; i < pod::Tree::TREE_SIZE; ++i ) {
if ( branch.children[i] == pod::Primitive::EMPTY ) continue;
auto& pr = tree.at(branch.children[i]);
// std::cout << "\t\tPosition: " << pr.position.x << ", " << pr.position.y << ", " << pr.position.z << "\n\t\tSize: " << pr.position.w << std::endl;
}
}
*/
}
// std::cout << std::endl;
return tree;
}
////////////////////////
namespace {
#define EPSILON 0.0001f
#define MAXLEN 1000.0f
#define SHADOW 0.5f
#define TREE_SIZE 8
#define TREE_STACK 8
#define PRIMITIVE_TYPE_EMPTY UINT32_MAX
#define PRIMITIVE_TYPE_CUBE 1
#define PRIMITIVE_TYPE_LEAF 2
#define PRIMITIVE_TYPE_TREE 3
#define PRIMITIVE_TYPE_ROOT 4
struct StackIterator {
uint tree;
uint child;
};
struct Stack {
int pointer;
StackIterator container[TREE_STACK];
} stack;
void printStack( ) {
std::cout << "[*] Pointer @ " << stack.pointer << std::endl;
for ( int i = stack.pointer; i >= 0; --i ) {
std::cout << "[*] " << stack.container[i].tree << ", " << stack.container[i].child << std::endl;
}
std::cout << std::endl;
}
StackIterator popStack( ) {
StackIterator top = stack.container[stack.pointer];
stack.container[stack.pointer--] = { UINT32_MAX, UINT32_MAX };
std::cout << "[?] popped stack" << std::endl;
printStack();
return top;
}
StackIterator topStack( ) {
StackIterator pointer = stack.container[stack.pointer];
printStack();
return pointer;
}
void pushStack( StackIterator item ) {
stack.container[++stack.pointer] = item;
std::cout << "[?] pushed to stack" << std::endl;
printStack();
}
float cubeIntersect( const pod::Vector3f& rayO, const pod::Vector3f& rayD, const pod::Vector3f& rayDRecip, const pod::Primitive& cube) {
float t[10];
t[1] = ( cube.position.x - cube.position.w - rayO.x) / rayD.x;
t[2] = ( cube.position.x + cube.position.w - rayO.x) / rayD.x;
t[3] = ( cube.position.y - cube.position.w - rayO.y) / rayD.y;
t[4] = ( cube.position.y + cube.position.w - rayO.y) / rayD.y;
t[5] = ( cube.position.z - cube.position.w - rayO.z) / rayD.z;
t[6] = ( cube.position.z + cube.position.w - rayO.z) / rayD.z;
t[7] = std::max(std::max(std::min(t[1], t[2]), std::min(t[3], t[4])), std::min(t[5], t[6]));
t[8] = std::min(std::min(std::max(t[1], t[2]), std::max(t[3], t[4])), std::max(t[5], t[6]));
t[9] = (t[8] < 0 || t[7] > t[8]) ? 0.0 : t[7];
return t[9];
}
float treeIntersect( const pod::Vector3f& rayO, const pod::Vector3f& rayD, const pod::Vector3f& rayDRecip, const pod::Tree& tree ) {
pod::Primitive treecube;
treecube.type = tree.type;
treecube.position = tree.position;
return cubeIntersect( rayO, rayD, rayDRecip, treecube );
}
}
void uf::primitive::test( const uf::stl::vector<pod::Primitive>& cubes, const uf::stl::vector<pod::Tree>& trees ) {
pod::Vector3f rayO = { 0, 5, 0 };
pod::Vector3f rayD = uf::vector::normalize( pod::Vector3f{ 0, -1, -0.25 } );
pod::Vector3f rayDRecip = { 1.0f / rayD.x, 1.0f / rayD.y, 1.0f / rayD.z };
float resT = MAXLEN;
uint root = trees.size() - 1;
uint id = UINT32_MAX;
if ( root == UINT32_MAX ) return;
// set up stack
stack.pointer = -1;
for ( uint i = 0; i < TREE_STACK; ++i ) stack.container[i] = {UINT32_MAX, UINT32_MAX};
pushStack({ root, UINT32_MAX });
while ( true ) {
StackIterator it = popStack();
// end of stack
if ( it.tree == UINT32_MAX ) break;
pod::Tree tree = trees[it.tree];
// invalid tree
if ( tree.type == PRIMITIVE_TYPE_EMPTY ) break;
// new tree, parse collision
if ( it.child == UINT32_MAX ) {
float t = treeIntersect( rayO, rayD, rayDRecip, tree );
// bad intersection with this tree, continue with next iteration
std::cout << "[?] Collision: 0 < " << t << " < " << resT << std::endl;
if ( t <= EPSILON || t >= resT ) continue;
// push back with new stack
it.child = 0;
pushStack( it );
// continue with next iteration
continue;
} else if ( it.child >= TREE_SIZE ) {
// no new children, continue with next iteration
continue;
} else {
// is leaf
if ( tree.type == PRIMITIVE_TYPE_LEAF ) {
// check children for a match
for ( uint i = 0; i < TREE_SIZE; ++i ) {
uint branchId = tree.children[i];
// unallocated, skip
if ( branchId == UINT32_MAX ) continue;
pod::Primitive primitive = cubes[branchId];
if ( primitive.type == PRIMITIVE_TYPE_EMPTY ) continue;
float t = cubeIntersect( rayO, rayD, rayDRecip, primitive );
// branch intersects with ray, set as new parent
if ( (t <= EPSILON) || (t >= resT) ) continue;
id = branchId;
resT = t;
}
// continue with next iteration
continue;
}
// parse children
uint branchId = tree.children[it.child++];
// add new iterator to the stack
pushStack( it );
// unused child, continue with next iteration
if ( branchId == UINT32_MAX ) continue;
// tree branch, push to stack
// the first if block will check its collision
it.tree = branchId;
it.child = UINT32_MAX;
pushStack( it );
continue;
}
}
std::cout << "ID: " << id << std::endl;
}
#endif