From 8dd83f7f4954d536de42c9325db5616d08ad4a38 Mon Sep 17 00:00:00 2001 From: ecker Date: Sun, 14 Sep 2025 20:37:34 -0500 Subject: [PATCH] I guess I'll try and do some work (something about trying to force inline the vector operators but failing, and ensuring the internal physics system should work for the dreamcast with its quantized meshes (maybe, it's still quite underperforming compared to RP3D), probably some other things from a week or so ago) --- bin/data/entities/door.json | 3 +- bin/data/entities/prop.json | 1 + engine/inc/uf/utils/math/matrix.h | 108 +++++++------ engine/inc/uf/utils/math/quaternion.h | 60 +++---- engine/inc/uf/utils/math/vector.h | 178 +++++++++++---------- engine/inc/uf/utils/math/vector/pod.inl | 17 +- engine/inc/uf/utils/math/vector/vector.inl | 5 + engine/inc/uf/utils/mesh/mesh.h | 9 +- engine/src/utils/math/physics/aabb.inl | 122 ++++++++------ engine/src/utils/math/physics/bvh.inl | 14 +- engine/src/utils/math/physics/capsule.inl | 5 +- engine/src/utils/math/physics/helpers.inl | 45 ++---- engine/src/utils/math/physics/impl.cpp | 44 ++--- engine/src/utils/math/physics/plane.inl | 10 +- engine/src/utils/math/physics/sphere.inl | 15 +- engine/src/utils/math/physics/triangle.inl | 151 +++++++++++------ 16 files changed, 425 insertions(+), 362 deletions(-) diff --git a/bin/data/entities/door.json b/bin/data/entities/door.json index 66d00173..72049cc4 100644 --- a/bin/data/entities/door.json +++ b/bin/data/entities/door.json @@ -7,8 +7,7 @@ "physics": { "mass": 0, "inertia": [0, 0, 0], - "type": "bounding box", - "recenter": true + "type": "mesh" // "bounding box" } } } \ No newline at end of file diff --git a/bin/data/entities/prop.json b/bin/data/entities/prop.json index c6f629f1..98623d6b 100644 --- a/bin/data/entities/prop.json +++ b/bin/data/entities/prop.json @@ -8,6 +8,7 @@ "physics": { "mass": 0, "type": "bounding box" + // "type": "mesh" } } } \ No newline at end of file diff --git a/engine/inc/uf/utils/math/matrix.h b/engine/inc/uf/utils/math/matrix.h index f1692d7b..902a4281 100644 --- a/engine/inc/uf/utils/math/matrix.h +++ b/engine/inc/uf/utils/math/matrix.h @@ -8,10 +8,16 @@ namespace pod { template struct /*UF_API*/ Matrix { // n-dimensional/unspecialized matrix access - T components[R*C] = {}; + union { + T components[R*C] = {}; + pod::Vector _rows[R]; // to-do: better name + pod::Vector _columns[C]; // to-do: better name + }; // POD information typedef T type_t; typedef T* container_t; + typedef pod::Vector row_t; + typedef pod::Vector col_t; static const size_t rows = R; static const size_t columns = C; // Overload access @@ -23,12 +29,12 @@ namespace pod { FORCE_INLINE const T& operator()(size_t r, size_t c) const; // Arithmetic - Matrix operator*( const Matrix& matrix ) const; - Matrix operator*( T scalar ) const; - Matrix operator+( const Matrix& matrix ) const; - Matrix& operator *=( const Matrix& matrix ); - bool operator==( const Matrix& matrix ) const; - bool operator!=( const Matrix& matrix ) const; + FORCE_INLINE Matrix operator*( const Matrix& matrix ) const; + FORCE_INLINE Matrix operator*( T scalar ) const; + FORCE_INLINE Matrix operator+( const Matrix& matrix ) const; + FORCE_INLINE Matrix& operator *=( const Matrix& matrix ); + FORCE_INLINE bool operator==( const Matrix& matrix ) const; + FORCE_INLINE bool operator!=( const Matrix& matrix ) const; }; template using Matrix2t = Matrix; @@ -48,66 +54,66 @@ namespace uf { namespace matrix { extern bool UF_API reverseInfiniteProjection; - template pod::Matrix4t /*UF_API*/ identity(); + template /*FORCE_INLINE*/ pod::Matrix4t /*UF_API*/ identity(); - template pod::Matrix4t /*UF_API*/ initialize( const T* ); - template pod::Matrix4t /*UF_API*/ initialize( const uf::stl::vector& ); - template pod::Matrix /*UF_API*/ identityi(); + template /*FORCE_INLINE*/ pod::Matrix4t /*UF_API*/ initialize( const T* ); + template /*FORCE_INLINE*/ pod::Matrix4t /*UF_API*/ initialize( const uf::stl::vector& ); + template /*FORCE_INLINE*/ pod::Matrix /*UF_API*/ identityi(); - template bool /*UF_API*/ equals( const T& left, const T& right, float eps = 1.0e-6f ); + template /*FORCE_INLINE*/ bool /*UF_API*/ equals( const T& left, const T& right, float eps = 1.0e-6f ); - template pod::Matrix multiply( const T& left, const U& right ); - template pod::Matrix4t multiply( const pod::Matrix4t& left, const pod::Matrix4t& right ); - template T /*UF_API*/ transpose( const T& matrix ); + template /*FORCE_INLINE*/ pod::Matrix multiply( const T& left, const U& right ); + template /*FORCE_INLINE*/ pod::Matrix4t multiply( const pod::Matrix4t& left, const pod::Matrix4t& right ); + template /*FORCE_INLINE*/ T /*UF_API*/ transpose( const T& matrix ); - template pod::Matrix2t inverse(const pod::Matrix2t& mat ); - template pod::Matrix3t inverse(const pod::Matrix3t& mat ); - template pod::Matrix4t inverse(const pod::Matrix4t& mat ); + template /*FORCE_INLINE*/ pod::Matrix2t inverse(const pod::Matrix2t& mat ); + template /*FORCE_INLINE*/ pod::Matrix3t inverse(const pod::Matrix3t& mat ); + template /*FORCE_INLINE*/ pod::Matrix4t inverse(const pod::Matrix4t& mat ); - template pod::Vector2t multiply(const pod::Matrix2t& mat, const pod::Vector2t& v ); - template pod::Vector3t multiply(const pod::Matrix3t& mat, const pod::Vector3t& v ); - template pod::Vector3t multiply( const pod::Matrix4t& mat, const pod::Vector3t& vector, T w = 1, bool = false ); - template pod::Vector4t multiply( const pod::Matrix4t& mat, const pod::Vector4t& vector, bool = false ); + template /*FORCE_INLINE*/ pod::Vector2t multiply(const pod::Matrix2t& mat, const pod::Vector2t& v ); + template /*FORCE_INLINE*/ pod::Vector3t multiply(const pod::Matrix3t& mat, const pod::Vector3t& v ); + template /*FORCE_INLINE*/ pod::Vector3t multiply( const pod::Matrix4t& mat, const pod::Vector3t& vector, T w = 1, bool = false ); + template /*FORCE_INLINE*/ pod::Vector4t multiply( const pod::Matrix4t& mat, const pod::Vector4t& vector, bool = false ); - template T /*UF_API*/ multiplyAll( const T& matrix, typename T::type_t scalar ); - template T /*UF_API*/ add( const T& lhs, const T& rhs ); + template /*FORCE_INLINE*/ T /*UF_API*/ multiplyAll( const T& matrix, typename T::type_t scalar ); + template /*FORCE_INLINE*/ T /*UF_API*/ add( const T& lhs, const T& rhs ); - template T& /*UF_API*/ inverse_( T& matrix ); - template pod::Matrix multiply_( T& left, const U& right ); - template pod::Matrix multiply_( T& left, const T& right ); - template T& /*UF_API*/ translate_( T& matrix, const pod::Vector3t& vector ); - template T& /*UF_API*/ rotate_( T& matrix, const pod::Vector3t& vector ); - template T& /*UF_API*/ scale_( T& matrix, const pod::Vector3t& vector ); + template /*FORCE_INLINE*/ T& /*UF_API*/ inverse_( T& matrix ); + template /*FORCE_INLINE*/ pod::Matrix multiply_( T& left, const U& right ); + template /*FORCE_INLINE*/ pod::Matrix multiply_( T& left, const T& right ); + template /*FORCE_INLINE*/ T& /*UF_API*/ translate_( T& matrix, const pod::Vector3t& vector ); + template /*FORCE_INLINE*/ T& /*UF_API*/ rotate_( T& matrix, const pod::Vector3t& vector ); + template /*FORCE_INLINE*/ T& /*UF_API*/ scale_( T& matrix, const pod::Vector3t& vector ); - template T /*UF_API*/ translate( const T& matrix, const pod::Vector3t& vector ); - template T /*UF_API*/ rotate( const T& matrix, const pod::Vector3t& vector ); - template T /*UF_API*/ scale( const T& matrix, const pod::Vector3t& vector ); - template pod::Vector3t /*UF_API*/ eulerAngles( const T& matrix ); + template /*FORCE_INLINE*/ T /*UF_API*/ translate( const T& matrix, const pod::Vector3t& vector ); + template /*FORCE_INLINE*/ T /*UF_API*/ rotate( const T& matrix, const pod::Vector3t& vector ); + template /*FORCE_INLINE*/ T /*UF_API*/ scale( const T& matrix, const pod::Vector3t& vector ); + template /*FORCE_INLINE*/ pod::Vector3t /*UF_API*/ eulerAngles( const T& matrix ); - template pod::Matrix4t /*UF_API*/ orthographic( T, T, T, T, T, T ); - template pod::Matrix4t /*UF_API*/ orthographic( T, T, T, T ); - template pod::Matrix4t FORCE_INLINE /*UF_API*/ orthographic( const pod::Vector2t& lr, const pod::Vector2t& bt, const pod::Vector2t& nf ) { + template /*FORCE_INLINE*/ pod::Matrix4t /*UF_API*/ orthographic( T, T, T, T, T, T ); + template /*FORCE_INLINE*/ pod::Matrix4t /*UF_API*/ orthographic( T, T, T, T ); + template /*FORCE_INLINE*/ pod::Matrix4t /*UF_API*/ orthographic( const pod::Vector2t& lr, const pod::Vector2t& bt, const pod::Vector2t& nf ) { return orthographic( lr.x, lr.y, bt.x, bt.y, nf.x, nf.y ); } - template pod::Matrix4t FORCE_INLINE /*UF_API*/ orthographic( const pod::Vector2t& lr, const pod::Vector2t& bt ) { + template /*FORCE_INLINE*/ pod::Matrix4t /*UF_API*/ orthographic( const pod::Vector2t& lr, const pod::Vector2t& bt ) { return orthographic( lr.x, lr.y, bt.x, bt.y ); } - template pod::Matrix4t /*UF_API*/ perspective( T, T, T, T ); - template pod::Matrix4t FORCE_INLINE /*UF_API*/ perspective( T fov, T raidou, const pod::Vector2f& range ) { + template /*FORCE_INLINE*/ pod::Matrix4t /*UF_API*/ perspective( T, T, T, T ); + template /*FORCE_INLINE*/ pod::Matrix4t /*UF_API*/ perspective( T fov, T raidou, const pod::Vector2f& range ) { return perspective( fov, raidou, range.x, range.y ); } - template pod::Matrix4t FORCE_INLINE /*UF_API*/ perspective( T fov, const pod::Vector2ui& size, const pod::Vector2f& range ) { + template /*FORCE_INLINE*/ pod::Matrix4t /*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 - template T& /*UF_API*/ copy( T& destination, const T& source ); - template T& /*UF_API*/ copy( T& destination, typename T::type_t* const source ); + template /*FORCE_INLINE*/ T& /*UF_API*/ copy( T& destination, const T& source ); + template /*FORCE_INLINE*/ T& /*UF_API*/ copy( T& destination, typename T::type_t* const source ); - template uf::stl::string /*UF_API*/ toString( const pod::Matrix& v ); - template ext::json::Value /*UF_API*/ encode( const pod::Matrix& v, const ext::json::EncodingSettings& = {} ); - template pod::Matrix& /*UF_API*/ decode( const ext::json::Value& v, pod::Matrix& ); - template pod::Matrix /*UF_API*/ decode( const ext::json::Value& v, const pod::Matrix& = uf::matrix::identity() ); + template /*FORCE_INLINE*/ uf::stl::string /*UF_API*/ toString( const pod::Matrix& v ); + template /*FORCE_INLINE*/ ext::json::Value /*UF_API*/ encode( const pod::Matrix& v, const ext::json::EncodingSettings& = {} ); + template /*FORCE_INLINE*/ pod::Matrix& /*UF_API*/ decode( const ext::json::Value& v, pod::Matrix& ); + template /*FORCE_INLINE*/ pod::Matrix /*UF_API*/ decode( const ext::json::Value& v, const pod::Matrix& = uf::matrix::identity() ); } } @@ -115,14 +121,14 @@ namespace uf { namespace uf { namespace string { template - uf::stl::string /*UF_API*/ toString( const pod::Matrix& v ); + FORCE_INLINE uf::stl::string /*UF_API*/ toString( const pod::Matrix& v ); } } namespace ext { namespace json { - template ext::json::Value /*UF_API*/ encode( const pod::Matrix& v ); - template pod::Matrix& /*UF_API*/ decode( const ext::json::Value& v, pod::Matrix& ); - template pod::Matrix /*UF_API*/ decode( const ext::json::Value& v, const pod::Matrix& = uf::matrix::identity() ); + template FORCE_INLINE ext::json::Value /*UF_API*/ encode( const pod::Matrix& v ); + template FORCE_INLINE pod::Matrix& /*UF_API*/ decode( const ext::json::Value& v, pod::Matrix& ); + template FORCE_INLINE pod::Matrix /*UF_API*/ decode( const ext::json::Value& v, const pod::Matrix& = uf::matrix::identity() ); } } diff --git a/engine/inc/uf/utils/math/quaternion.h b/engine/inc/uf/utils/math/quaternion.h index e833ca13..546bb76f 100644 --- a/engine/inc/uf/utils/math/quaternion.h +++ b/engine/inc/uf/utils/math/quaternion.h @@ -19,45 +19,45 @@ namespace pod { namespace uf { namespace quaternion { - template pod::Quaternion /*UF_API*/ identity(); - template T /*UF_API*/ multiply( const T& left, const T& right ); - template pod::Vector3t /*UF_API*/ rotate( const pod::Quaternion& left, const pod::Vector3t& right ); - template pod::Vector4t /*UF_API*/ rotate( const pod::Quaternion& left, const pod::Vector4t& right ); - template typename T::type_t /*UF_API*/ sum( const T& vector ); - template typename T::type_t /*UF_API*/ product( const T& vector ); + template /*FORCE_INLINE*/ pod::Quaternion /*UF_API*/ identity(); + template /*FORCE_INLINE*/ T /*UF_API*/ multiply( const T& left, const T& right ); + template /*FORCE_INLINE*/ pod::Vector3t /*UF_API*/ rotate( const pod::Quaternion& left, const pod::Vector3t& right ); + template /*FORCE_INLINE*/ pod::Vector4t /*UF_API*/ rotate( const pod::Quaternion& left, const pod::Vector4t& right ); + template /*FORCE_INLINE*/ typename T::type_t /*UF_API*/ sum( const T& vector ); + template /*FORCE_INLINE*/ typename T::type_t /*UF_API*/ product( const T& vector ); - template typename T::type_t /*UF_API*/ dot( const T& left, const T& right ); - template pod::Angle /*UF_API*/ angle( const T& a, const T& b ); + template /*FORCE_INLINE*/ typename T::type_t /*UF_API*/ dot( const T& left, const T& right ); + template /*FORCE_INLINE*/ pod::Angle /*UF_API*/ angle( const T& a, const T& b ); - template T /*UF_API*/ lerp( const T& from, const T& to, typename T::type_t delta ); - template T /*UF_API*/ slerp( const T& from, const T& to, typename T::type_t delta ); + template /*FORCE_INLINE*/ T /*UF_API*/ lerp( const T& from, const T& to, typename T::type_t delta ); + template /*FORCE_INLINE*/ T /*UF_API*/ slerp( const T& from, const T& to, typename T::type_t delta ); - template typename T::type_t /*UF_API*/ distanceSquared( const T& a, const T& b ); - template typename T::type_t /*UF_API*/ distance( const T& a, const T& b ); - template typename T::type_t /*UF_API*/ magnitude( const T& vector ); - template typename T::type_t /*UF_API*/ norm( const T& vector ); - template T /*UF_API*/ normalize( const T& vector ); + template /*FORCE_INLINE*/ typename T::type_t /*UF_API*/ distanceSquared( const T& a, const T& b ); + template /*FORCE_INLINE*/ typename T::type_t /*UF_API*/ distance( const T& a, const T& b ); + template /*FORCE_INLINE*/ typename T::type_t /*UF_API*/ magnitude( const T& vector ); + template /*FORCE_INLINE*/ typename T::type_t /*UF_API*/ norm( const T& vector ); + template /*FORCE_INLINE*/ T /*UF_API*/ normalize( const T& vector ); - template pod::Matrix4t matrix( const pod::Quaternion& quaternion ); - template pod::Quaternion axisAngle( const pod::Vector3t& axis, T angle ); - template pod::Quaternion unitVectors( const pod::Vector3t& u, const pod::Vector3t& v ); - template pod::Quaternion lookAt( const pod::Vector3t& source, const pod::Vector3t& destination ); + template /*FORCE_INLINE*/ pod::Matrix4t matrix( const pod::Quaternion& quaternion ); + template /*FORCE_INLINE*/ pod::Quaternion axisAngle( const pod::Vector3t& axis, T angle ); + template /*FORCE_INLINE*/ pod::Quaternion unitVectors( const pod::Vector3t& u, const pod::Vector3t& v ); + template /*FORCE_INLINE*/ pod::Quaternion lookAt( const pod::Vector3t& source, const pod::Vector3t& destination ); - template T conjugate( const T& quaternion ); - template T inverse( const T& quaternion ); + template /*FORCE_INLINE*/ T conjugate( const T& quaternion ); + template /*FORCE_INLINE*/ T inverse( const T& quaternion ); - template T& /*UF_API*/ multiply_( T& left, const T& right ); - template T& /*UF_API*/ normalize_( T& vector ); - template T& conjugate_( T& quaternion ); - template T& inverse_( T& quaternion ); + template /*FORCE_INLINE*/ T& /*UF_API*/ multiply_( T& left, const T& right ); + template /*FORCE_INLINE*/ T& /*UF_API*/ normalize_( T& vector ); + template /*FORCE_INLINE*/ T& conjugate_( T& quaternion ); + template /*FORCE_INLINE*/ T& inverse_( T& quaternion ); - template pod::Vector3t eulerAngles( const pod::Quaternion& quaternion ); - template T pitch( const pod::Quaternion& quaternion ); - template T yaw( const pod::Quaternion& quaternion ); - template T roll( const pod::Quaternion& quaternion ); + template /*FORCE_INLINE*/ pod::Vector3t eulerAngles( const pod::Quaternion& quaternion ); + template /*FORCE_INLINE*/ T pitch( const pod::Quaternion& quaternion ); + template /*FORCE_INLINE*/ T yaw( const pod::Quaternion& quaternion ); + template /*FORCE_INLINE*/ T roll( const pod::Quaternion& quaternion ); - template pod::Quaternion fromMatrix( const pod::Matrix4t& matrix ); + template /*FORCE_INLINE*/ pod::Quaternion fromMatrix( const pod::Matrix4t& matrix ); } } diff --git a/engine/inc/uf/utils/math/vector.h b/engine/inc/uf/utils/math/vector.h index cc71dcf9..b3d5e1fb 100644 --- a/engine/inc/uf/utils/math/vector.h +++ b/engine/inc/uf/utils/math/vector.h @@ -80,88 +80,89 @@ namespace pod { // POD vector accessing/manipulation namespace uf { namespace vector { - template pod::Vector1t /*UF_API*/ create( T x ); // creates a 1D vector - template pod::Vector2t /*UF_API*/ create( T x, T y ); // creates a 2D vector - template pod::Vector3t /*UF_API*/ create( T x, T y, T z ); // creates a 3D vector - template pod::Vector4t /*UF_API*/ create( T x, T y, T z, T w ); // creates a 4D vector - template pod::Vector /*UF_API*/ copy( const pod::Vector& = {}); // creates a copy of a vector (for whatever reason) - template pod::Vector /*UF_API*/ cast( const U& from ); // casts one vector of one type to another (of the same size) + template /*FORCE_INLINE*/ pod::Vector1t /*UF_API*/ create( T x ); // creates a 1D vector + template /*FORCE_INLINE*/ pod::Vector2t /*UF_API*/ create( T x, T y ); // creates a 2D vector + template /*FORCE_INLINE*/ pod::Vector3t /*UF_API*/ create( T x, T y, T z ); // creates a 3D vector + template /*FORCE_INLINE*/ pod::Vector4t /*UF_API*/ create( T x, T y, T z, T w ); // creates a 4D vector + template /*FORCE_INLINE*/ pod::Vector /*UF_API*/ copy( const pod::Vector& = {}); // creates a copy of a vector (for whatever reason) + template /*FORCE_INLINE*/ pod::Vector /*UF_API*/ cast( const U& from ); // casts one vector of one type to another (of the same size) // Equality checking - template bool /*UF_API*/ equals( const T& left, const T& right ); // equality check between two vectors (==) - template bool /*UF_API*/ notEquals( const T& left, const T& right ); // equality check between two vectors (==) - template bool /*UF_API*/ less( const T& left, const T& right ); // equality check between two vectors (<) - template bool /*UF_API*/ lessEquals( const T& left, const T& right ); // equality check between two vectors (<=) - template bool /*UF_API*/ greater( const T& left, const T& right ); // equality check between two vectors (>) - template bool /*UF_API*/ greaterEquals( const T& left, const T& right ); // equality check between two vectors (>=) + template /*FORCE_INLINE*/ bool /*UF_API*/ equals( const T& left, const T& right ); // equality check between two vectors (==) + template /*FORCE_INLINE*/ bool /*UF_API*/ notEquals( const T& left, const T& right ); // equality check between two vectors (==) + template /*FORCE_INLINE*/ bool /*UF_API*/ less( const T& left, const T& right ); // equality check between two vectors (<) + template /*FORCE_INLINE*/ bool /*UF_API*/ lessEquals( const T& left, const T& right ); // equality check between two vectors (<=) + template /*FORCE_INLINE*/ bool /*UF_API*/ greater( const T& left, const T& right ); // equality check between two vectors (>) + template /*FORCE_INLINE*/ bool /*UF_API*/ greaterEquals( const T& left, const T& right ); // equality check between two vectors (>=) - template bool /*UF_API*/ isValid( const T& v ); // checks if all components are valid (non NaN, inf, etc.) + template /*FORCE_INLINE*/ bool /*UF_API*/ isValid( const T& v ); // checks if all components are valid (non NaN, inf, etc.) // Basic arithmetic - template T /*UF_API*/ add( const T& left, const T& right ); // adds two vectors of same type and size together - template T /*UF_API*/ add( const T& left, typename T::type_t scalar ); // adds a scalar to every component of the vector - template T /*UF_API*/ add( typename T::type_t scalar, const T& vector ); // adds a scalar to every component of the vector (inverted) - template T /*UF_API*/ subtract( const T& left, const T& right ); // subtracts two vectors of same type and size together - template T /*UF_API*/ subtract( const T& left, typename T::type_t scalar ); // subtracts a scalar to every component of the vector - template T /*UF_API*/ subtract( typename T::type_t scalar, const T& vector ); // subtracts a scalar to every component of the vector (inverted) - template T /*UF_API*/ multiply( const T& left, const T& right ); // multiplies two vectors of same type and size together - template T /*UF_API*/ multiply( const T& vector, typename T::type_t scalar ); // multiplies a scalar to every component of the vector - template T /*UF_API*/ multiply( typename T::type_t scalar, const T& vector ); // multiplies a scalar to every component of the vector (inverted) - template T /*UF_API*/ divide( const T& left, const T& right ); // divides two vectors of same type and size together - template T /*UF_API*/ divide( const T& left, typename T::type_t scalar ); // divides a scalar to every component of the vector - template T /*UF_API*/ divide( typename T::type_t scalar, const T& vector ); // divides a scalar to every component of the vector (inverted) + template /*FORCE_INLINE*/ T /*UF_API*/ add( const T& left, const T& right ); // adds two vectors of same type and size together + template /*FORCE_INLINE*/ T /*UF_API*/ add( const T& left, typename T::type_t scalar ); // adds a scalar to every component of the vector + template /*FORCE_INLINE*/ T /*UF_API*/ add( typename T::type_t scalar, const T& vector ); // adds a scalar to every component of the vector (inverted) + template /*FORCE_INLINE*/ T /*UF_API*/ subtract( const T& left, const T& right ); // subtracts two vectors of same type and size together + template /*FORCE_INLINE*/ T /*UF_API*/ subtract( const T& left, typename T::type_t scalar ); // subtracts a scalar to every component of the vector + template /*FORCE_INLINE*/ T /*UF_API*/ subtract( typename T::type_t scalar, const T& vector ); // subtracts a scalar to every component of the vector (inverted) + template /*FORCE_INLINE*/ T /*UF_API*/ multiply( const T& left, const T& right ); // multiplies two vectors of same type and size together + template /*FORCE_INLINE*/ T /*UF_API*/ multiply( const T& vector, typename T::type_t scalar ); // multiplies a scalar to every component of the vector + template /*FORCE_INLINE*/ T /*UF_API*/ multiply( typename T::type_t scalar, const T& vector ); // multiplies a scalar to every component of the vector (inverted) + template /*FORCE_INLINE*/ T /*UF_API*/ divide( const T& left, const T& right ); // divides two vectors of same type and size together + template /*FORCE_INLINE*/ T /*UF_API*/ divide( const T& left, typename T::type_t scalar ); // divides a scalar to every component of the vector + template /*FORCE_INLINE*/ T /*UF_API*/ divide( typename T::type_t scalar, const T& vector ); // divides a scalar to every component of the vector (inverted) - template T& /*UF_API*/ add_( T& left, const T& right ); // adds two vectors of same type and size together - template T& /*UF_API*/ add_( T& left, typename T::type_t scalar ); // adds a scalar to every component of the vector - template T& /*UF_API*/ add_( typename T::type_t scalar, T& vector ); // adds a scalar to every component of the vector (inverted) - template T& /*UF_API*/ subtract_( T& left, const T& right ); // subtracts two vectors of same type and size together - template T& /*UF_API*/ subtract_( T& left, typename T::type_t scalar ); // subtracts a scalar to every component of the vector - template T& /*UF_API*/ subtract_( typename T::type_t scalar, T& vector ); // subtracts a scalar to every component of the vector (inverted) - template T& /*UF_API*/ multiply_( T& left, const T& right ); // multiplies two vectors of same type and size together - template T& /*UF_API*/ multiply_( T& vector, typename T::type_t scalar ); // multiplies a scalar to every component of the vector - template T& /*UF_API*/ multiply_( typename T::type_t scalar, T& vector ); // multiplies a scalar to every component of the vector (inverted) - template T& /*UF_API*/ divide_( T& left, const T& right ); // divides two vectors of same type and size together - template T& /*UF_API*/ divide_( T& left, typename T::type_t scalar ); // divides a scalar to every component of the vector - template T& /*UF_API*/ divide_( typename T::type_t scalar, T& vector ); // divides a scalar to every component of the vector (inverted) - template T& /*UF_API*/ negate_( T& vector ); // flip sign of all components - template T& /*UF_API*/ normalize_( T& vector ); // normalizes a vector + template /*FORCE_INLINE*/ T& /*UF_API*/ add_( T& left, const T& right ); // adds two vectors of same type and size together + template /*FORCE_INLINE*/ T& /*UF_API*/ add_( T& left, typename T::type_t scalar ); // adds a scalar to every component of the vector + template /*FORCE_INLINE*/ T& /*UF_API*/ add_( typename T::type_t scalar, T& vector ); // adds a scalar to every component of the vector (inverted) + template /*FORCE_INLINE*/ T& /*UF_API*/ subtract_( T& left, const T& right ); // subtracts two vectors of same type and size together + template /*FORCE_INLINE*/ T& /*UF_API*/ subtract_( T& left, typename T::type_t scalar ); // subtracts a scalar to every component of the vector + template /*FORCE_INLINE*/ T& /*UF_API*/ subtract_( typename T::type_t scalar, T& vector ); // subtracts a scalar to every component of the vector (inverted) + template /*FORCE_INLINE*/ T& /*UF_API*/ multiply_( T& left, const T& right ); // multiplies two vectors of same type and size together + template /*FORCE_INLINE*/ T& /*UF_API*/ multiply_( T& vector, typename T::type_t scalar ); // multiplies a scalar to every component of the vector + template /*FORCE_INLINE*/ T& /*UF_API*/ multiply_( typename T::type_t scalar, T& vector ); // multiplies a scalar to every component of the vector (inverted) + template /*FORCE_INLINE*/ T& /*UF_API*/ divide_( T& left, const T& right ); // divides two vectors of same type and size together + template /*FORCE_INLINE*/ T& /*UF_API*/ divide_( T& left, typename T::type_t scalar ); // divides a scalar to every component of the vector + template /*FORCE_INLINE*/ T& /*UF_API*/ divide_( typename T::type_t scalar, T& vector ); // divides a scalar to every component of the vector (inverted) + template /*FORCE_INLINE*/ T& /*UF_API*/ negate_( T& vector ); // flip sign of all components + template /*FORCE_INLINE*/ T& /*UF_API*/ normalize_( T& vector ); // normalizes a vector - template typename T::type_t /*UF_API*/ sum( const T& vector ); // compute the sum of all components - template typename T::type_t /*UF_API*/ product( const T& vector ); // compute the product of all components - template T /*UF_API*/ negate( const T& vector ); // flip sign of all components - template T /*UF_API*/ abs( const T& vector ); // gets the absolute value of a vector - template T /*UF_API*/ min( const T& left, const T& right ); // returns the minimum of each component between two vectors - template T /*UF_API*/ max( const T& left, const T& right ); // returns the maximum of each component between two vectors - template T /*UF_API*/ clamp( const T& vector, const T& min, const T& max ); // clamps a vector between two bounds - template T /*UF_API*/ ceil( const T& vector ); // rounds each component of the the vector up - template T /*UF_API*/ floor( const T& vector ); // rounds each component of the vector down - template T /*UF_API*/ round( const T& vector ); // rounds each component of the vector + template /*FORCE_INLINE*/ typename T::type_t /*UF_API*/ sum( const T& vector ); // compute the sum of all components + template /*FORCE_INLINE*/ typename T::type_t /*UF_API*/ product( const T& vector ); // compute the product of all components + template /*FORCE_INLINE*/ T /*UF_API*/ negate( const T& vector ); // flip sign of all components + template /*FORCE_INLINE*/ T /*UF_API*/ abs( const T& vector ); // gets the absolute value of a vector + template /*FORCE_INLINE*/ T /*UF_API*/ min( const T& left, const T& right ); // returns the minimum of each component between two vectors + template /*FORCE_INLINE*/ T /*UF_API*/ max( const T& left, const T& right ); // returns the maximum of each component between two vectors + template /*FORCE_INLINE*/ T /*UF_API*/ clamp( const T& vector, const T& min, const T& max ); // clamps a vector between two bounds + template /*FORCE_INLINE*/ T /*UF_API*/ ceil( const T& vector ); // rounds each component of the the vector up + template /*FORCE_INLINE*/ T /*UF_API*/ floor( const T& vector ); // rounds each component of the vector down + template /*FORCE_INLINE*/ T /*UF_API*/ round( const T& vector ); // rounds each component of the vector // Complex arithmetic - template typename T::type_t /*UF_API*/ dot( const T& left, const T& right ); // compute the dot product between two vectors - template float /*UF_API*/ angle( const T& a, const T& b ); // compute the angle between two vectors - template float /*UF_API*/ signedAngle( const T& a, const T& b, const T& axis ); // compute the signed angle between two vectors - template T /*UF_API*/ cross( const T& a, const T& b ); // compute the cross product between two vectors + template /*FORCE_INLINE*/ typename T::type_t /*UF_API*/ dot( const T& left, const T& right ); // compute the dot product between two vectors + template /*FORCE_INLINE*/ float /*UF_API*/ angle( const T& a, const T& b ); // compute the angle between two vectors + template /*FORCE_INLINE*/ float /*UF_API*/ signedAngle( const T& a, const T& b, const T& axis ); // compute the signed angle between two vectors + template /*FORCE_INLINE*/ T /*UF_API*/ cross( const T& a, const T& b ); // compute the cross product between two vectors - template T /*UF_API*/ lerp( const T& from, const T& to, double, bool = true ); // linearly interpolate between two vectors - template T /*UF_API*/ lerp( const T& from, const T& to, const T&, bool = true ); // linearly interpolate between two vectors, component-wise + template /*FORCE_INLINE*/ T /*UF_API*/ lerp( const T& from, const T& to, double, bool = true ); // linearly interpolate between two vectors + template /*FORCE_INLINE*/ T /*UF_API*/ lerp( const T& from, const T& to, const T&, bool = true ); // linearly interpolate between two vectors, component-wise - template T /*UF_API*/ slerp( const T& from, const T& to, double, bool = false ); // spherically interpolate between two vectors - template T /*UF_API*/ mix( const T& from, const T& to, double, bool = false ); + template /*FORCE_INLINE*/ T /*UF_API*/ slerp( const T& from, const T& to, double, bool = false ); // spherically interpolate between two vectors + template /*FORCE_INLINE*/ T /*UF_API*/ mix( const T& from, const T& to, double, bool = false ); - template 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::type_t /*UF_API*/ distance( const T& a, const T& b ); // compute the distance between two vectors - template typename T::type_t /*UF_API*/ magnitude( const T& vector ); // gets the magnitude of the vector - template typename T::type_t /*UF_API*/ norm( const T& vector ); // compute the norm (length) of the vector - template T /*UF_API*/ normalize( const T& vector ); // normalizes a vector - template T /*UF_API*/ clampMagnitude( const T& vector ); // clamps the magnitude of a vector - template void /*UF_API*/ orthonormalize( T& x, T& y ); // orthonormalizes a vector against another vector - template T /*UF_API*/ orthonormalize( const T& x, const T& y ); // orthonormalizes a vector against another vector + template /*FORCE_INLINE*/ typename T::type_t /*UF_API*/ distanceSquared( const T& a, const T& b ); // compute the distance between two vectors (doesn't sqrt) + template /*FORCE_INLINE*/ typename T::type_t /*UF_API*/ distance( const T& a, const T& b ); // compute the distance between two vectors + template /*FORCE_INLINE*/ typename T::type_t /*UF_API*/ magnitude( const T& vector ); // gets the magnitude of the vector + template /*FORCE_INLINE*/ typename T::type_t /*UF_API*/ norm( const T& vector ); // compute the norm (length) of the vector + template /*FORCE_INLINE*/ T /*UF_API*/ normalize( const T& vector ); // normalizes a vector + template /*FORCE_INLINE*/ T /*UF_API*/ clampMagnitude( const T& vector ); // clamps the magnitude of a vector + template /*FORCE_INLINE*/ void /*UF_API*/ orthonormalize( T& x, T& y ); // orthonormalizes a vector against another vector + template /*FORCE_INLINE*/ T /*UF_API*/ orthonormalize( const T& x, const T& y ); // orthonormalizes a vector against another vector - template uf::stl::string /*UF_API*/ toString( const T& vector ); // parses a vector as a string - template ext::json::Value encode( const pod::Vector& v, const ext::json::EncodingSettings& = {} ); // parses a vector into a JSON value + template /*FORCE_INLINE*/ size_t /*UF_API*/ hash( const T& vector ); // hashes a vector + template /*FORCE_INLINE*/ uf::stl::string /*UF_API*/ toString( const T& vector ); // parses a vector as a string + template /*FORCE_INLINE*/ ext::json::Value encode( const pod::Vector& v, const ext::json::EncodingSettings& = {} ); // parses a vector into a JSON value - template pod::Vector& decode( const ext::json::Value& v, pod::Vector& ); // parses a JSON value into a vector - template pod::Vector decode( const ext::json::Value& v, const pod::Vector& = {} ); // parses a JSON value into a vector + template /*FORCE_INLINE*/ pod::Vector& decode( const ext::json::Value& v, pod::Vector& ); // parses a JSON value into a vector + template /*FORCE_INLINE*/ pod::Vector decode( const ext::json::Value& v, const pod::Vector& = {} ); // parses a JSON value into a vector - template typename T::type_t /*UF_API*/ mips( const T& size ); // calculate amount of mips to use given a size + template /*FORCE_INLINE*/ typename T::type_t /*UF_API*/ mips( const T& size ); // calculate amount of mips to use given a size } } @@ -197,6 +198,12 @@ namespace uf { FORCE_INLINE explicit operator bool() const { return uf::vector::notEquals(*this, Vector{}); } \ template FORCE_INLINE operator Vector() const { return uf::vector::cast(*this); } +#define DEFINE_VECTOR_EXT(T, N) \ + template FORCE_INLINE pod::Vector operator+( T scalar, const pod::Vector& v ) { return uf::vector::add(scalar, v); }\ + template FORCE_INLINE pod::Vector operator-( T scalar, const pod::Vector& v ) { return uf::vector::subtract(scalar, v); }\ + template FORCE_INLINE pod::Vector operator*( T scalar, const pod::Vector& v ) { return uf::vector::multiply(scalar, v); }\ + template FORCE_INLINE pod::Vector operator/( T scalar, const pod::Vector& v ) { return uf::vector::divide(scalar, v); }\ + namespace pod { template struct Vector { @@ -238,30 +245,35 @@ namespace pod { // external functions -//#include -#include +// scalar operator vector +template FORCE_INLINE pod::Vector operator+( T scalar, const pod::Vector& v ) { return uf::vector::add(scalar, v); } +template FORCE_INLINE pod::Vector operator-( T scalar, const pod::Vector& v ) { return uf::vector::subtract(scalar, v); } +template FORCE_INLINE pod::Vector operator*( T scalar, const pod::Vector& v ) { return uf::vector::multiply(scalar, v); } +template FORCE_INLINE pod::Vector operator/( T scalar, const pod::Vector& v ) { return uf::vector::divide(scalar, v); } + +DEFINE_VECTOR_EXT(T, 2); +DEFINE_VECTOR_EXT(T, 3); +DEFINE_VECTOR_EXT(T, 4); + +// stringify namespace uf { namespace string { - template - uf::stl::string toString( const pod::Vector& v ); + template FORCE_INLINE uf::stl::string toString( const pod::Vector& v ); } } +// jsonify namespace ext { namespace json { - template ext::json::Value encode( const pod::Vector& v ); - template pod::Vector& decode( const ext::json::Value&, pod::Vector& ); - template pod::Vector decode( const ext::json::Value&, const pod::Vector& = {} ); + template FORCE_INLINE ext::json::Value encode( const pod::Vector& v ); + template FORCE_INLINE pod::Vector& decode( const ext::json::Value&, pod::Vector& ); + template FORCE_INLINE pod::Vector decode( const ext::json::Value&, const pod::Vector& = {} ); } } - +// hash namespace std { template struct hash> { - size_t operator()(const pod::Vector& v) const { - size_t hash = 0; - for ( size_t i = 0; i < N; ++i ) hash ^= v[i]; - return hash; - } + FORCE_INLINE size_t operator()(const pod::Vector& v) const; }; } diff --git a/engine/inc/uf/utils/math/vector/pod.inl b/engine/inc/uf/utils/math/vector/pod.inl index f78e13d5..ed4e29e6 100644 --- a/engine/inc/uf/utils/math/vector/pod.inl +++ b/engine/inc/uf/utils/math/vector/pod.inl @@ -276,9 +276,9 @@ T uf::vector::divide( const T& vector, typename T::type_t scalar ) { } #endif T res; - scalar = static_cast(1) / scalar; + float recip = static_cast(1) / scalar; FOR_EACH(T::size, { - res[i] = vector[i] * scalar; + res[i] = vector[i] * recip; }); return res; } @@ -298,9 +298,9 @@ T uf::vector::divide( typename T::type_t scalar, const T& vector ) { } #endif T res; - scalar = static_cast(1) / scalar; + float recip = static_cast(1) / scalar; FOR_EACH(T::size, { - res[i] = scalar / vector[i]; + res[i] = vector[i] * recip; }); return res; } @@ -740,6 +740,15 @@ typename T::type_t uf::vector::mips( const T& size ) { return static_cast(std::floor(std::log2(max))) + 1; } +template +size_t uf::vector::hash( const T& v ) { + size_t hash = 0; + FOR_EACH(T::size, { + hash ^= v[i]; + }); + return hash; +} + template uf::stl::string uf::vector::toString( const T& v ) { uf::stl::stringstream ss; diff --git a/engine/inc/uf/utils/math/vector/vector.inl b/engine/inc/uf/utils/math/vector/vector.inl index d2c0cd06..a41a5361 100644 --- a/engine/inc/uf/utils/math/vector/vector.inl +++ b/engine/inc/uf/utils/math/vector/vector.inl @@ -17,4 +17,9 @@ pod::Vector& /*UF_API*/ ext::json::decode( const ext::json::Value& json, po template pod::Vector /*UF_API*/ ext::json::decode( const ext::json::Value& json, const pod::Vector& v ) { return uf::vector::decode(json, v); +} + +template +size_t std::hash>::operator()( const pod::Vector& v ) const { + return uf::vector::hash( v ); } \ No newline at end of file diff --git a/engine/inc/uf/utils/mesh/mesh.h b/engine/inc/uf/utils/mesh/mesh.h index f538c0f1..f12be968 100644 --- a/engine/inc/uf/utils/mesh/mesh.h +++ b/engine/inc/uf/utils/mesh/mesh.h @@ -191,15 +191,10 @@ namespace uf { return null; } }; - /* - struct Bounds { - pod::Vector3f min = { std::numeric_limits::max(), std::numeric_limits::max(), std::numeric_limits::max() }; - pod::Vector3f max = { -std::numeric_limits::max(), -std::numeric_limits::max(), -std::numeric_limits::max() }; - } bounds; - */ + uf::stl::vector buffers; - // crunge, but it's better this way + // crunge, but it's better this way for streaming in mesh data uf::stl::vector buffer_paths; protected: void _destroy( uf::Mesh::Input& input ); diff --git a/engine/src/utils/math/physics/aabb.inl b/engine/src/utils/math/physics/aabb.inl index c1964082..2058b413 100644 --- a/engine/src/utils/math/physics/aabb.inl +++ b/engine/src/utils/math/physics/aabb.inl @@ -1,24 +1,74 @@ namespace { - inline bool aabbOverlap( const pod::AABB& a, const pod::AABB& b, float eps ) { + FORCE_INLINE bool aabbOverlap( const pod::AABB& a, const pod::AABB& b, float eps ) { + #if UF_USE_SIMD + return uf::simd::all( uf::simd::lessEquals( a.min, b.max ) ) && uf::simd::all( uf::simd::greaterEquals( a.max, b.min ) ); + #else return ( a.min - eps ) <= ( b.max + eps ) && ( a.max + eps ) >= ( b.min - eps ); + #endif } - inline float aabbSurfaceArea(const pod::AABB& aabb) { + FORCE_INLINE float aabbSurfaceArea(const pod::AABB& aabb) { auto d = uf::vector::max( ( aabb.max - aabb.min ), pod::Vector3f{} ); return 2.0f * (d.x * d.y + d.y * d.z + d.z * d.x); } - pod::AABB computeSegmentAABB( const pod::Vector3f& p1, const pod::Vector3f p2, float r ) { + FORCE_INLINE pod::AABB computeSegmentAABB( const pod::Vector3f& p1, const pod::Vector3f p2, float r ) { return { uf::vector::min( p1, p2 ) - r, uf::vector::max( p1, p2 ) + r, }; } - pod::Vector3f closestPointOnAABB(const pod::Vector3f& p, const pod::AABB& box) { + FORCE_INLINE pod::Vector3f closestPointOnAABB(const pod::Vector3f& p, const pod::AABB& box) { return uf::vector::clamp( p, box.min, box.max ); } + FORCE_INLINE pod::AABB computeTriangleAABB( const pod::Triangle& tri ) { + return { + 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] ), + }; + } + + FORCE_INLINE pod::AABB mergeAabb( const pod::AABB& a, const pod::AABB& b ) { + return { + uf::vector::min( a.min, b.min ), + uf::vector::max( a.max, b.max ), + }; + } + + FORCE_INLINE pod::Vector3f aabbCenter( const pod::AABB& aabb ) { + return ( aabb.max + aabb.min ) * 0.5f; + } + FORCE_INLINE pod::Vector3f aabbExtent( const pod::AABB& aabb ) { + return ( aabb.max - aabb.min ) * 0.5f; + } + + pod::AABB transformAabbToWorld( const pod::AABB& localBox, const pod::Transform<>& transform ) { + const auto& q = transform.orientation; + const auto& p = transform.position; + + pod::Vector3f center = (localBox.min + localBox.max) * 0.5f; + pod::Vector3f extents = (localBox.max - localBox.min) * 0.5f; + + pod::Vector3f axisX = uf::quaternion::rotate(q, pod::Vector3f{1,0,0}); + pod::Vector3f axisY = uf::quaternion::rotate(q, pod::Vector3f{0,1,0}); + pod::Vector3f axisZ = uf::quaternion::rotate(q, pod::Vector3f{0,0,1}); + + pod::Vector3f worldCenter = uf::quaternion::rotate(q, center) + p; + + pod::Vector3f worldExtents = { + fabs(axisX.x) * extents.x + fabs(axisY.x) * extents.y + fabs(axisZ.x) * extents.z, + fabs(axisX.y) * extents.x + fabs(axisY.y) * extents.y + fabs(axisZ.y) * extents.z, + fabs(axisX.z) * extents.x + fabs(axisY.z) * extents.y + fabs(axisZ.z) * extents.z + }; + + return { + worldCenter - worldExtents, + worldCenter + worldExtents + }; + } + std::pair getCapsuleSegment( const pod::PhysicsBody& body ) { const auto transform = ::getTransform( body ); const auto& capsule = body.collider.capsule; @@ -34,10 +84,13 @@ namespace { const auto transform = ::getTransform( body ); switch ( body.collider.type ) { case pod::ShapeType::AABB: { + /* return { transform.position + body.collider.aabb.min, transform.position + body.collider.aabb.max, }; + */ + return ::transformAabbToWorld( body.collider.aabb, *body.transform ); } break; case pod::ShapeType::SPHERE: { return { @@ -63,20 +116,13 @@ namespace { return {}; } - pod::AABB computeTriangleAABB( const pod::Triangle& tri ) { - return { - 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] ), - }; - } - float triAabbDistanceSq( const pod::Triangle& tri, const pod::AABB& box ) { float minDistSq = FLT_MAX; - for ( auto i = 0; i < 3; ++i ) { + FOR_EACH(3, { auto cp = ::closestPointOnAABB( tri.points[i], box ); auto d = tri.points[i] - cp; minDistSq = std::min( minDistSq, uf::vector::dot( d, d ) ); - } + }); return minDistSq; } @@ -124,11 +170,11 @@ namespace { if ( !axisTest( {-f2.y, f2.x, 0} ) ) return false; // test AABB face axes - for ( auto i = 0; i < 3; ++i ) { + FOR_EACH(3, { float minVal = std::min({v0[i], v1[i], v2[i]}); float maxVal = std::max({v0[i], v1[i], v2[i]}); if ( minVal > e[i] || maxVal < -e[i] ) return false; - } + }); // test triangle normal axis auto n = uf::vector::cross( f0, f1 ); @@ -139,13 +185,6 @@ namespace { return true; } - pod::AABB mergeAabb( const pod::AABB& a, const pod::AABB& b ) { - return { - uf::vector::min( a.min, b.min ), - uf::vector::max( a.max, b.max ), - }; - } - pod::AABB transformAabbToLocal( const pod::AABB& box, const pod::Transform<>& transform ) { auto inv = uf::transform::inverse( transform ); @@ -166,21 +205,14 @@ namespace { { -FLT_MAX, -FLT_MAX, -FLT_MAX }, }; - for ( auto i = 0; i < 8; ++i ) { + FOR_EACH(8, { auto local = uf::transform::apply( inv, corners[i] ); out.min = uf::vector::min( out.min, local ); out.max = uf::vector::max( out.max, local ); - } + }); return out; } - pod::Vector3f aabbCenter( const pod::AABB& aabb ) { - return ( aabb.max + aabb.min ) * 0.5f; - } - pod::Vector3f aabbExtent( const pod::AABB& aabb ) { - return ( aabb.max - aabb.min ) * 0.5f; - } - bool aabbAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps ) { ASSERT_COLLIDER_TYPES(AABB, AABB); @@ -195,12 +227,12 @@ namespace { // determine collision axis = smallest overlap auto axis = -1; float minOverlap = FLT_MAX; - for ( auto i = 0; i < 3; ++i ) { + FOR_EACH(3, { if ( overlaps[i] < minOverlap ) { minOverlap = overlaps[i]; axis = i; } - } + }); pod::Vector3f delta = ::getPosition( b ) - ::getPosition( a ); pod::Vector3f normal{0,0,0}; @@ -211,13 +243,13 @@ namespace { 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 + 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), 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 + else if ( axis == 1 ) { // y-axis separation, overlap in XZ plane 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 }); @@ -235,30 +267,18 @@ namespace { bool aabbSphere( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps ) { ASSERT_COLLIDER_TYPES( AABB, SPHERE ); - auto start = manifold.points.size(); - if ( !::sphereAabb( b, a, manifold, eps ) ) return false; - for ( auto i = start; i < manifold.points.size(); ++i ) manifold.points[i].normal = -manifold.points[i].normal; - return true; + REVERSE_COLLIDER( a, b, sphereAabb ); } bool aabbPlane( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps ) { ASSERT_COLLIDER_TYPES( AABB, PLANE ); - auto start = manifold.points.size(); - if ( !::planeAabb( b, a, manifold, eps ) ) return false; - for ( auto i = start; i < manifold.points.size(); ++i ) manifold.points[i].normal = -manifold.points[i].normal; - return true; + REVERSE_COLLIDER( a, b, planeAabb ); } bool aabbCapsule( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps ) { ASSERT_COLLIDER_TYPES( AABB, CAPSULE ); - auto start = manifold.points.size(); - if ( !::capsuleAabb( b, a, manifold, eps ) ) return false; - for ( auto i = start; i < manifold.points.size(); ++i ) manifold.points[i].normal = -manifold.points[i].normal; - return true; + REVERSE_COLLIDER( a, b, capsuleAabb ); } bool aabbMesh( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps ) { ASSERT_COLLIDER_TYPES( AABB, MESH ); - auto start = manifold.points.size(); - if ( !::meshAabb( b, a, manifold, eps ) ) return false; - for ( auto i = start; i < manifold.points.size(); ++i ) manifold.points[i].normal = -manifold.points[i].normal; - return true; + REVERSE_COLLIDER( a, b, meshAabb ); } } \ No newline at end of file diff --git a/engine/src/utils/math/physics/bvh.inl b/engine/src/utils/math/physics/bvh.inl index 5f47956c..4ab32737 100644 --- a/engine/src/utils/math/physics/bvh.inl +++ b/engine/src/utils/math/physics/bvh.inl @@ -1,3 +1,13 @@ +namespace { + int32_t flattenBVH( pod::BVH& bvh, int32_t nodeID ); + + void queryFlatBVH( const pod::BVH&, const pod::AABB& bounds, uf::stl::vector& out ); + void queryFlatBVH( const pod::BVH&, const pod::Ray& ray, uf::stl::vector& out, float maxDist = FLT_MAX ); + + void queryFlatOverlaps( const pod::BVH& bvh, pod::BVH::pairs_t& outPairs ); + void queryFlatOverlaps( const pod::BVH& bvhA, const pod::BVH& bvhB, pod::BVH::pairs_t& outPairs ); +} + // BVH namespace { int32_t buildBVHNode( pod::BVH& bvh, const uf::stl::vector& bounds, int32_t start, int32_t end, int32_t capacity = 2 ) { @@ -212,7 +222,7 @@ namespace { auto tris = view.index.count / 3; for ( auto triIndexID = 0; triIndexID < tris; ++triIndexID ) { - auto tri = ::fetchTriangle( positions.data(view.vertex.first), positions.stride(), indices.data(view.index.first), mesh.index.size, triIndexID ); + auto tri = ::fetchTriangle( view, indices, positions, triIndexID ); auto aabb = ::computeTriangleAABB( tri ); auto triID = triIndexID + (view.index.first / 3); @@ -363,7 +373,7 @@ namespace { auto tris = view.index.count / 3; for ( auto triIndexID = 0; triIndexID < tris; ++triIndexID ) { - auto tri = ::fetchTriangle( positions.data(view.vertex.first), positions.stride(), indices.data(view.index.first), mesh.index.size, triIndexID ); + auto tri = ::fetchTriangle( view, indices, positions, triIndexID ); auto aabb = ::computeTriangleAABB( tri ); bounds.emplace_back(aabb); } diff --git a/engine/src/utils/math/physics/capsule.inl b/engine/src/utils/math/physics/capsule.inl index 12aff975..8d95b9ce 100644 --- a/engine/src/utils/math/physics/capsule.inl +++ b/engine/src/utils/math/physics/capsule.inl @@ -104,9 +104,6 @@ namespace { } bool capsuleMesh( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps ) { ASSERT_COLLIDER_TYPES( CAPSULE, MESH ); - auto start = manifold.points.size(); - if ( !::meshCapsule( b, a, manifold, eps ) ) return false; - for ( auto i = start; i < manifold.points.size(); ++i ) manifold.points[i].normal = -manifold.points[i].normal; - return true; + REVERSE_COLLIDER( a, b, meshCapsule ); } } \ No newline at end of file diff --git a/engine/src/utils/math/physics/helpers.inl b/engine/src/utils/math/physics/helpers.inl index 98ad6505..7adf052e 100644 --- a/engine/src/utils/math/physics/helpers.inl +++ b/engine/src/utils/math/physics/helpers.inl @@ -1,3 +1,9 @@ +#define REVERSE_COLLIDER( a, b, fun )\ + auto start = manifold.points.size();\ + if ( !::fun( b, a, manifold, eps ) ) return false;\ + for ( auto i = start; i < manifold.points.size(); ++i ) manifold.points[i].normal = -manifold.points[i].normal;\ + return true; + // forward declare namespace { bool aabbAabb( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); @@ -37,31 +43,16 @@ namespace { bool triangleCapsule( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); // ugh - pod::Vector3f aabbCenter( const pod::AABB& aabb ); - bool aabbOverlap( const pod::AABB& a, const pod::AABB& b, float eps = EPS(1.0e-6f) ); - pod::AABB computeTriangleAABB( const pod::Triangle& tri ); - - void solveContacts( uf::stl::vector& manifolds, float dt ); - - int32_t flattenBVH( pod::BVH& bvh, int32_t nodeID ); - - void traverseNodePair( const pod::BVH& bvh, int32_t leftID, int32_t rightID, pod::BVH::pairs_t& pairs ); - void traverseNodePair( const pod::BVH& a, int32_t nodeA, const pod::BVH& b, int32_t nodeB, pod::BVH::pairs_t& out ); + FORCE_INLINE bool aabbOverlap( const pod::AABB& a, const pod::AABB& b, float eps = EPS(1.0e-6f) ); + pod::Vector3f aabbCenter( const pod::AABB& aabb ); + void queryBVH( const pod::BVH& bvh, const pod::AABB& bounds, uf::stl::vector& indices ); void queryBVH( const pod::BVH& bvh, const pod::AABB& bounds, uf::stl::vector& indices, int32_t nodeID ); - void queryBVH( const pod::BVH& bvh, const pod::Ray& ray, uf::stl::vector& indices, float maxDist = FLT_MAX ); void queryBVH( const pod::BVH& bvh, const pod::Ray& ray, uf::stl::vector& indices, int32_t nodeID, float maxDist = FLT_MAX ); - - void queryFlatBVH( const pod::BVH&, const pod::AABB& bounds, uf::stl::vector& out ); - void queryFlatBVH( const pod::BVH&, const pod::Ray& ray, uf::stl::vector& out, float maxDist = FLT_MAX ); - void queryOverlaps( const pod::BVH& bvh, pod::BVH::pairs_t& outPairs ); void queryOverlaps( const pod::BVH& bvhA, const pod::BVH& bvhB, pod::BVH::pairs_t& outPairs ); - - void queryFlatOverlaps( const pod::BVH& bvh, pod::BVH::pairs_t& outPairs ); - void queryFlatOverlaps( const pod::BVH& bvhA, const pod::BVH& bvhB, pod::BVH::pairs_t& outPairs ); } namespace { @@ -381,22 +372,4 @@ namespace { return best; } - - bool triangleTriangleIntersect( const pod::Triangle& a, const pod::Triangle& b, float eps = EPS(1e-6f) ) { - auto boxA = ::computeTriangleAABB( a ); - auto boxB = ::computeTriangleAABB( b ); - - if ( !::aabbOverlap( boxA, boxB ) ) return false; - - // check vertices of a inside b or vice versa - for ( auto i = 0; i < 3; i++ ) { - auto q = ::closestPointOnTriangle( a.points[i], b ); - if ( uf::vector::magnitude( q - a.points[i] ) < eps ) return true; - } - for ( auto i = 0; i < 3; i++ ) { - auto q = ::closestPointOnTriangle( b.points[i], a ); - if ( uf::vector::magnitude( q - b.points[i] ) < eps ) return true; - } - return false; - } } \ No newline at end of file diff --git a/engine/src/utils/math/physics/impl.cpp b/engine/src/utils/math/physics/impl.cpp index c0fa88de..925df8c4 100644 --- a/engine/src/utils/math/physics/impl.cpp +++ b/engine/src/utils/math/physics/impl.cpp @@ -5,41 +5,45 @@ #include namespace { - bool warmupSolver = true; - bool blockContactSolver = true; - bool psgContactSolver = true; // iterative solver is flawed + bool warmupSolver = true; // cache manifold data to warm up the solver + bool blockContactSolver = true; // use BlockNxN solvers (where N = number of contacts for a manifold) + bool psgContactSolver = true; // use PSG contact solver bool useGjk = false; // currently don't have a way to broadphase mesh => narrowphase tri via GJK - bool fixedStep = true; - - int32_t substeps = 0; - int32_t reserveCount = 32; + bool fixedStep = true; // run physics simulation with a fixed delta time (with accumulation), rather than rely on actual engine deltatime + int32_t substeps = 0; // number of substeps per frame tick + int32_t reserveCount = 32; // amount of elements to reserve for vectors used in this system, to-do: have it tie to a memory pool allocator - // increasing these make things lag - int32_t broadphaseBvhCapacity = 1; - int32_t meshBvhCapacity = 1; + // increasing these make things lag for reasons I can imagine why + int32_t broadphaseBvhCapacity = 1; // number of bodies per leaf node + int32_t meshBvhCapacity = 1; // number of triangles per leaf node - bool flattenBvhBodies = false; // bugged + // additionally flattens a BVH for linear iteration, rather than a recursive / stack-based traversal + bool flattenBvhBodies = true; bool flattenBvhMeshes = true; - // it actually seems slower to use these...... - bool useBvhSahBodies = false; - bool useBvhSahMeshes = false; + // use surface area heuristics for building the BVH, rather than naive splits + bool useBvhSahBodies = false; // it actually seems slower to use these...... + bool useBvhSahMeshes = true; - bool useSplitBvhs = true; // currently bugged if enabled + bool useSplitBvhs = true; // creates separate BVHs for static / dynamic objects + // to-do: find possibly better values for this int32_t solverIterations = 10; float baumgarteCorrectionPercent = 0.2f; float baumgarteCorrectionSlop = 0.01f; uf::stl::unordered_map manifoldsCache; - int32_t manifoldCacheLifetime = 6; + int32_t manifoldCacheLifetime = 6; // to-do: find a good value for this uint32_t frameCounter = 0; + + // to-do: tweak this to not be annoying + // currently seems only reliable when it hits its TTL, but too long of a wait is gross, and too frequent of an update causes lag pod::BVH::UpdatePolicy bvhUpdatePolicy = { .displacementThreshold = 0.25f, .overlapThreshold = 2.0f, .dirtyRatioThreshold = 0.3f, - .maxFramesBeforeRebuild = 60, + .maxFramesBeforeRebuild = 120, }; } @@ -61,7 +65,7 @@ namespace { #include "integration.inl" #include "solvers.inl" -// unused +// unused, as these are from reactphysics float uf::physics::impl::timescale = 1.0f / 60.0f; bool uf::physics::impl::interpolate = false; bool uf::physics::impl::shared = false; @@ -264,9 +268,7 @@ void uf::physics::impl::updateInertia( pod::PhysicsBody& body ) { extents *= extents; // square it; body.inertiaTensor = extents * (body.mass / 12.0f); - // to-do: add overloaded inverse order arithmetic operators - //body.inverseInertiaTensor = 1.0f / body.inertiaTensor; - body.inverseInertiaTensor = { 1.0f / body.inertiaTensor.x, 1.0f / body.inertiaTensor.y, 1.0f / body.inertiaTensor.z }; + body.inverseInertiaTensor = 1.0f / body.inertiaTensor; } break; case pod::ShapeType::SPHERE: { float I = 0.4f * body.mass * body.collider.sphere.radius * body.collider.sphere.radius; diff --git a/engine/src/utils/math/physics/plane.inl b/engine/src/utils/math/physics/plane.inl index f1d92de6..18779e7e 100644 --- a/engine/src/utils/math/physics/plane.inl +++ b/engine/src/utils/math/physics/plane.inl @@ -48,16 +48,10 @@ namespace { } bool planeCapsule( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps ) { ASSERT_COLLIDER_TYPES( PLANE, CAPSULE ); - auto start = manifold.points.size(); - if ( !::capsulePlane( b, a, manifold, eps ) ) return false; - for ( auto i = start; i < manifold.points.size(); ++i ) manifold.points[i].normal = -manifold.points[i].normal; - return true; + REVERSE_COLLIDER( a, b, capsulePlane ); } bool planeMesh( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps ) { ASSERT_COLLIDER_TYPES( PLANE, MESH ); - auto start = manifold.points.size(); - if ( !::meshPlane( b, a, manifold, eps ) ) return false; - for ( auto i = start; i < manifold.points.size(); ++i ) manifold.points[i].normal = -manifold.points[i].normal; - return true; + REVERSE_COLLIDER( a, b, meshPlane ); } } \ No newline at end of file diff --git a/engine/src/utils/math/physics/sphere.inl b/engine/src/utils/math/physics/sphere.inl index 06fc1a26..4e28c57b 100644 --- a/engine/src/utils/math/physics/sphere.inl +++ b/engine/src/utils/math/physics/sphere.inl @@ -53,23 +53,14 @@ namespace { } bool spherePlane( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps ) { ASSERT_COLLIDER_TYPES( SPHERE, PLANE ); - auto start = manifold.points.size(); - if ( !::planeSphere( b, a, manifold, eps ) ) return false; - for ( auto i = start; i < manifold.points.size(); ++i ) manifold.points[i].normal = -manifold.points[i].normal; - return true; + REVERSE_COLLIDER( a, b, planeSphere ); } bool sphereCapsule( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps ) { ASSERT_COLLIDER_TYPES( SPHERE, CAPSULE ); - auto start = manifold.points.size(); - if ( !::capsuleSphere( b, a, manifold, eps ) ) return false; - for ( auto i = start; i < manifold.points.size(); ++i ) manifold.points[i].normal = -manifold.points[i].normal; - return true; + REVERSE_COLLIDER( a, b, capsuleSphere ); } bool sphereMesh( const pod::PhysicsBody& a, const pod::PhysicsBody& b, pod::Manifold& manifold, float eps ) { ASSERT_COLLIDER_TYPES( SPHERE, MESH ); - auto start = manifold.points.size(); - if ( !::meshSphere( b, a, manifold, eps ) ) return false; - for ( auto i = start; i < manifold.points.size(); ++i ) manifold.points[i].normal = -manifold.points[i].normal; - return true; + REVERSE_COLLIDER( a, b, meshSphere ); } } \ No newline at end of file diff --git a/engine/src/utils/math/physics/triangle.inl b/engine/src/utils/math/physics/triangle.inl index 943db670..219fe85b 100644 --- a/engine/src/utils/math/physics/triangle.inl +++ b/engine/src/utils/math/physics/triangle.inl @@ -1,23 +1,9 @@ #define REORIENT_NORMALS_ON_FETCH 0 #define REORIENT_NORMALS_ON_CONTACT 1 -// mesh BVH -namespace { - // to-do: clean this up - uint32_t getIndex( const void* indexData, size_t indexSize, size_t idx ) { - if ( indexSize == sizeof(uint8_t) ) { - auto* ptr = reinterpret_cast(indexData); - return static_cast(ptr[idx]); - } else if ( indexSize == sizeof(uint16_t) ) { - auto* ptr = reinterpret_cast(indexData); - return static_cast(ptr[idx]); - } else if ( indexSize == sizeof(uint32_t) ) { - auto* ptr = reinterpret_cast(indexData); - return ptr[idx]; - } - UF_EXCEPTION("Unsupported index type of size {}", indexSize); - } +#include +namespace { pod::Vector3f triangleCenter( const pod::Triangle& tri ) { return ( tri.points[0] + tri.points[1] + tri.points[2] ) / 3.0f; } @@ -29,12 +15,80 @@ namespace { //return uf::vector::normalize( tri.normals[0] + tri.normals[1] + tri.normals[2] ); } - pod::Triangle fetchTriangle( const void* vertices, size_t vertexStride, const void* indexData, size_t indexSize, size_t triID ) { - return { - *reinterpret_cast(reinterpret_cast(vertices) + ::getIndex( indexData, indexSize, (triID * 3) + 0 ) * vertexStride), - *reinterpret_cast(reinterpret_cast(vertices) + ::getIndex( indexData, indexSize, (triID * 3) + 1 ) * vertexStride), - *reinterpret_cast(reinterpret_cast(vertices) + ::getIndex( indexData, indexSize, (triID * 3) + 2 ) * vertexStride), - }; + bool triangleTriangleIntersect( const pod::Triangle& a, const pod::Triangle& b, float eps = EPS(1e-6f) ) { + auto boxA = ::computeTriangleAABB( a ); + auto boxB = ::computeTriangleAABB( b ); + + if ( !::aabbOverlap( boxA, boxB ) ) return false; + + // check vertices of a inside b or vice versa + FOR_EACH(3, { + auto q = ::closestPointOnTriangle( a.points[i], b ); + if ( uf::vector::magnitude( q - a.points[i] ) < eps ) return true; + }); + FOR_EACH(3, { + auto q = ::closestPointOnTriangle( b.points[i], a ); + if ( uf::vector::magnitude( q - b.points[i] ) < eps ) return true; + }); + return false; + } + + size_t getIndex( const void* pointer, size_t stride, size_t index ) { + #define CAST_INDEX(T) case sizeof(T): return ((T*) pointer)[index]; + switch ( stride ) { + CAST_INDEX(uint8_t); + CAST_INDEX(uint16_t); + CAST_INDEX(uint32_t); + default: { + UF_EXCEPTION("invalid stride type: {}", stride); + } break; + } + } + size_t getIndex( const uf::Mesh::View& view, const uf::Mesh::AttributeView& indices, size_t index ) { + return ::getIndex( indices.data(view.index.first), indices.stride(), index ); + } + pod::Vector3f getVertex( const uf::Mesh::View& view, const uf::Mesh::AttributeView& positions, size_t index ) { + const auto stride = positions.stride(); + #define CAST_VERTEX(T) {\ + const T* vertices = (T*) positions.data(view.vertex.first + index);\ + return { vertices[0], vertices[1], vertices[2], };\ + } + #define DEQUANTIZE_VERTEX(T) {\ + const T* vertices = (T*) positions.data(view.vertex.first + index);\ + return { uf::quant::dequantize(vertices[0]), uf::quant::dequantize(vertices[1]), uf::quant::dequantize(vertices[2]), };\ + } + + switch ( positions.attribute.descriptor.type ) { + // dequantize + case uf::renderer::enums::Type::USHORT: + case uf::renderer::enums::Type::SHORT: { + DEQUANTIZE_VERTEX(uint16_t); + } break; + case uf::renderer::enums::Type::FLOAT: { + CAST_VERTEX(float); + } break; + #if UF_USE_FLOAT16 + case uf::renderer::enums::Type::HALF: { + CAST_VERTEX(std::float16_t); + } break; + #endif + #if UF_USE_BFLOAT16 + case uf::renderer::enums::Type::BFLOAT: { + CAST_VERTEX(std::bfloat16_t); + } break; + #endif + default: UF_EXCEPTION("unsupported vertex type: {}", positions.attribute.descriptor.type); break; + } + // return ::getVertex( positions.data(view.vertex.first), positions.stride(), index ); + } + + pod::Triangle fetchTriangle( const uf::Mesh::View& view, const uf::Mesh::AttributeView& indices, const uf::Mesh::AttributeView& positions, size_t triID ) { + auto index = triID * 3; + pod::Triangle tri; + FOR_EACH(3, { + tri.points[i] = ::getVertex( view, positions, ::getIndex( view, indices, index + i ) ); + }); + return tri; } pod::TriangleWithNormal fetchTriangle( const uf::Mesh& mesh, size_t triID ) { @@ -43,49 +97,42 @@ namespace { // find which view contains this triangle index. size_t triBase = 0; - const uf::Mesh::View* found = nullptr; + const uf::Mesh::View* view = nullptr; for ( auto& v : views ) { auto trisInView = v.index.count / 3; if (triID < triBase + trisInView) { - found = &v; + view = &v; triID -= triBase; // local triangle index inside this view break; } triBase += trisInView; } - UF_ASSERT( found ); + UF_ASSERT( view ); - auto& positions = (*found)["position"]; - auto& normals = (*found)["normal"]; - auto& indices = (*found)["index"]; + auto& positions = (*view)["position"]; + auto& normals = (*view)["normal"]; + auto& indices = (*view)["index"]; - pod::TriangleWithNormal tri = { ::fetchTriangle( positions.data(found->vertex.first), positions.stride(), indices.data(found->index.first), mesh.index.size, triID ) }; + pod::TriangleWithNormal tri = { ::fetchTriangle( *view, indices, positions, triID ) }; tri.normal = uf::vector::normalize(uf::vector::cross(tri.points[1] - tri.points[0], tri.points[2] - tri.points[0])); - /* - if ( false && normals.valid() ) { - auto* base = reinterpret_cast(normals.data(found->vertex.first)); - size_t stride = normals.stride(); - for ( auto i = 0; i < 3; ++i ) tri.normals[i] = *reinterpret_cast(base + idxs[i] * stride); - } else { - auto normal = ::triangleNormal( (pod::Triangle&) tri ); - for ( auto i = 0; i < 3; ++i ) tri.normals[i] = normal; - } - */ - return tri; } // if body is a mesh, apply its transform to the triangles, else reorient the normal with respect to the body - pod::TriangleWithNormal fetchTriangle( const uf::Mesh& mesh, size_t triID, const pod::PhysicsBody& body, bool fast = true ) { + pod::TriangleWithNormal fetchTriangle( const uf::Mesh& mesh, size_t triID, const pod::PhysicsBody& body, bool fast = false ) { auto tri = ::fetchTriangle( mesh, triID ); auto transform = ::getTransform( body ); if ( body.collider.type == pod::ShapeType::MESH ) { if ( fast ) { - for ( auto i = 0; i < 3; ++i ) tri.points[i] += transform.position; + FOR_EACH(3, { + tri.points[i] += transform.position; + }); } else { - for ( auto i = 0; i < 3; ++i ) tri.points[i] = uf::transform::apply( transform, tri.points[i] ); + FOR_EACH(3, { + tri.points[i] = uf::transform::apply( transform, tri.points[i] ); + }); tri.normal = uf::quaternion::rotate( transform.orientation, tri.normal ); } } @@ -132,24 +179,24 @@ namespace { // clip edges of A against plane of B const pod::Vector3f At[3] = { A.points[0], A.points[1], A.points[2] }; - for ( auto i = 0; i < 3; ++i ) { + FOR_EACH(3, { auto j = ( i + 1 ) % 3; pod::Vector3f p; if ( intersectSegmentPlane( At[i], At[j], nB, dB, p ) ) { // check if intersection lies inside triangle B if ( ::pointInTriangle( p, B ) ) checkAndPush(p); } - } + }); // clip edges of B against plane of A const pod::Vector3f Bt[3] = { B.points[0], B.points[1], B.points[2] }; - for ( auto i = 0; i < 3; ++i ) { + FOR_EACH(3, { auto j = ( i + 1 ) % 3; pod::Vector3f p; if ( intersectSegmentPlane( Bt[i], Bt[j], nA, dA, p ) ) { if ( ::pointInTriangle( p, A ) ) checkAndPush(p); } - } + }); if ( intersections.empty() ) return false; @@ -295,7 +342,9 @@ namespace { bool hit = false; pod::Vector3f dist; - for ( auto i = 0; i < 3; i++ ) dist[i] = uf::vector::dot(normal, tri.points[i] ) - d; + FOR_EACH(3, { + dist[i] = uf::vector::dot(normal, tri.points[i] ) - d; + }); // completely on one side bool allAbove = ( dist.x > eps && dist.y > eps && dist.z > eps ); @@ -305,10 +354,10 @@ namespace { if ( allBelow ) { hit = true; - for ( auto i = 0; i < 3; i++ ) { + FOR_EACH(3, { float penetration = -dist[i]; - manifold.points.emplace_back(pod::Contact{tri.points[i], normal, penetration}); - } + manifold.points.emplace_back(pod::Contact{tri.points[i], normal, -dist[i]}); + }); return hit; }