diff --git a/bin/data/entities/player.json b/bin/data/entities/player.json index 1a45ac99..87482c0b 100644 --- a/bin/data/entities/player.json +++ b/bin/data/entities/player.json @@ -77,18 +77,18 @@ "gravity": [ 0, -9.81, 0 ], "inertia": [ 0, 0, 0 ], - // "type": "sphere", - // "radius": 2, + "type": "sphere", + "radius": 2, - "type": "capsule", - "radius": 1, - "height": 2, + // "type": "capsule", + // "radius": 1, + // "height": 2, // "type": "bounding box", // "min": [ -1.0, -2.0, -1.0 ], // "max": [ 1.0, 0.0, 1.0 ], - "mass": 100, + "mass": 1, "friction": 0.95, "restitution": 0.0, diff --git a/bin/data/scenes/sourceengine/test_grid.json b/bin/data/scenes/sourceengine/test_grid.json index e8add596..af2e1755 100644 --- a/bin/data/scenes/sourceengine/test_grid.json +++ b/bin/data/scenes/sourceengine/test_grid.json @@ -7,9 +7,9 @@ "metadata": { "graph": { "tags": { - " worldspawn": { - // "physics": { "type": "bounding box", "static": true }, - "physics": { "type": "plane", "static": true, "direction": [ 0, 1, 0 ], "offset": 0 }, + "worldspawn": { + "physics": { "type": "bounding box", "static": true }, + // "physics": { "type": "plane", "static": true, "direction": [ 0, 1, 0 ], "offset": 0 }, "grid": { "size": [8,1,8], "epsilon": 0.001, "cleanup": true, "print": true, "clip": true }, "optimize meshlets": { "simplify": 0.125, "print": false }, "unwrap mesh": true diff --git a/engine/inc/uf/utils/math/matrix.h b/engine/inc/uf/utils/math/matrix.h index b134832b..871689fa 100644 --- a/engine/inc/uf/utils/math/matrix.h +++ b/engine/inc/uf/utils/math/matrix.h @@ -38,6 +38,11 @@ namespace pod { bool operator==( const Matrix& matrix ) const; // Equality check between two matrices (equals) bool operator!=( const Matrix& matrix ) const; // Equality check between two matrices (not equals) }; + + template using Matrix2t = Matrix; + typedef Matrix2t<> Matrix2; + typedef Matrix2t Matrix2f; + template using Matrix3t = Matrix; typedef Matrix3t<> Matrix3; typedef Matrix3t Matrix3f; @@ -62,14 +67,20 @@ namespace uf { template bool /*UF_API*/ equals( const T& left, const T& right ); // Equality check between two matrices (equals) template bool /*UF_API*/ equals( const T& left, const T& right, float eps ); // Equality check between two matrices (equals) // Basic arithmetic - // template pod::Matrix /*UF_API*/ multiply( const T& left, const T& right ); // Multiplies two matrices of same type and size together - template pod::Matrix multiply( const T& left, const U& right ); // Multiplies two matrices of same type and size together - template pod::Matrix multiply( const pod::Matrix& left, const pod::Matrix& right ); // Multiplies two matrices of same type and size together - template T /*UF_API*/ transpose( const T& matrix ); // Flip sign of all components - template T /*UF_API*/ inverse( const T& matrix ); // Flip sign of all components + // template pod::Matrix /*UF_API*/ multiply( const T& left, const T& right ); // Multiplies two matrices of same type and size together + template pod::Matrix multiply( const T& left, const U& right ); // Multiplies two matrices of same type and size together + template pod::Matrix4t multiply( const pod::Matrix4t& left, const pod::Matrix4t& right ); // Multiplies two matrices of same type and size together + template 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 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 T /*UF_API*/ multiplyAll( const T& matrix, typename T::type_t scalar ); template T /*UF_API*/ add( const T& lhs, const T& rhs ); // Writes to first value diff --git a/engine/inc/uf/utils/math/matrix/pod.inl b/engine/inc/uf/utils/math/matrix/pod.inl index 5ee98827..7f4a4f3a 100644 --- a/engine/inc/uf/utils/math/matrix/pod.inl +++ b/engine/inc/uf/utils/math/matrix/pod.inl @@ -317,125 +317,141 @@ template T uf::matrix::transpose( const T& matrix ) { return transpose; } -// Flip sign of all components -template T uf::matrix::inverse( const T& matrix ) { - if ( T::rows != 4 || T::columns != 4 ) return matrix; +// +template pod::Matrix2t uf::matrix::inverse( const pod::Matrix2t& m ) { + T det = m[0] * m[3] - m[1] * m[2]; + if ( std::fabs(det) < 1e-12f ) return m; - const typename T::type_t* m = &matrix[0]; - ALIGN16 typename T::type_t inv[16]; - typename T::type_t det; - uint_fast8_t i; + T invDet = 1 / det; + + return pod::Matrix2t{ + m[3] * invDet, -m[1] * invDet, + -m[2] * invDet, m[0] * invDet, + }; +} - inv[0] = m[5] * m[10] * m[15] - - m[5] * m[11] * m[14] - - m[9] * m[6] * m[15] + - m[9] * m[7] * m[14] + - m[13] * m[6] * m[11] - - m[13] * m[7] * m[10]; - inv[4] = -m[4] * m[10] * m[15] + - m[4] * m[11] * m[14] + - m[8] * m[6] * m[15] - - m[8] * m[7] * m[14] - - m[12] * m[6] * m[11] + - m[12] * m[7] * m[10]; - inv[8] = m[4] * m[9] * m[15] - - m[4] * m[11] * m[13] - - m[8] * m[5] * m[15] + - m[8] * m[7] * m[13] + - m[12] * m[5] * m[11] - - m[12] * m[7] * m[9]; - inv[12] = -m[4] * m[9] * m[14] + - m[4] * m[10] * m[13] + - m[8] * m[5] * m[14] - - m[8] * m[6] * m[13] - - m[12] * m[5] * m[10] + - m[12] * m[6] * m[9]; - inv[1] = -m[1] * m[10] * m[15] + - m[1] * m[11] * m[14] + - m[9] * m[2] * m[15] - - m[9] * m[3] * m[14] - - m[13] * m[2] * m[11] + - m[13] * m[3] * m[10]; - inv[5] = m[0] * m[10] * m[15] - - m[0] * m[11] * m[14] - - m[8] * m[2] * m[15] + - m[8] * m[3] * m[14] + - m[12] * m[2] * m[11] - - m[12] * m[3] * m[10]; - inv[9] = -m[0] * m[9] * m[15] + - m[0] * m[11] * m[13] + - m[8] * m[1] * m[15] - - m[8] * m[3] * m[13] - - m[12] * m[1] * m[11] + - m[12] * m[3] * m[9]; - inv[13] = m[0] * m[9] * m[14] - - m[0] * m[10] * m[13] - - m[8] * m[1] * m[14] + - m[8] * m[2] * m[13] + - m[12] * m[1] * m[10] - - m[12] * m[2] * m[9]; - inv[2] = m[1] * m[6] * m[15] - - m[1] * m[7] * m[14] - - m[5] * m[2] * m[15] + - m[5] * m[3] * m[14] + - m[13] * m[2] * m[7] - - m[13] * m[3] * m[6]; - inv[6] = -m[0] * m[6] * m[15] + - m[0] * m[7] * m[14] + - m[4] * m[2] * m[15] - - m[4] * m[3] * m[14] - - m[12] * m[2] * m[7] + - m[12] * m[3] * m[6]; - inv[10] = m[0] * m[5] * m[15] - - m[0] * m[7] * m[13] - - m[4] * m[1] * m[15] + - m[4] * m[3] * m[13] + - m[12] * m[1] * m[7] - - m[12] * m[3] * m[5]; - inv[14] = -m[0] * m[5] * m[14] + - m[0] * m[6] * m[13] + - m[4] * m[1] * m[14] - - m[4] * m[2] * m[13] - - m[12] * m[1] * m[6] + - m[12] * m[2] * m[5]; - inv[3] = -m[1] * m[6] * m[11] + - m[1] * m[7] * m[10] + - m[5] * m[2] * m[11] - - m[5] * m[3] * m[10] - - m[9] * m[2] * m[7] + - m[9] * m[3] * m[6]; - inv[7] = m[0] * m[6] * m[11] - - m[0] * m[7] * m[10] - - m[4] * m[2] * m[11] + - m[4] * m[3] * m[10] + - m[8] * m[2] * m[7] - - m[8] * m[3] * m[6]; - inv[11] = -m[0] * m[5] * m[11] + - m[0] * m[7] * m[9] + - m[4] * m[1] * m[11] - - m[4] * m[3] * m[9] - - m[8] * m[1] * m[7] + - m[8] * m[3] * m[5]; - inv[15] = m[0] * m[5] * m[10] - - m[0] * m[6] * m[9] - - m[4] * m[1] * m[10] + - m[4] * m[2] * m[9] + - m[8] * m[1] * m[6] - - m[8] * m[2] * m[5]; +template pod::Matrix3t uf::matrix::inverse( const pod::Matrix3t& 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]); - det = m[0] * inv[0] + m[1] * inv[4] + m[2] * inv[8] + m[3] * inv[12]; - if (det == 0) return matrix; - det = 1.0 / det; - ALIGN16 T inverted; - #pragma unroll // GCC unroll 16 - for ( i = 0; i < 16; ++i ) - inverted[i] = inv[i] * det; + if (std::fabs(det) < 1e-12f) return m; // singular - return inverted; + T invDet = static_cast(1) / det; + + return pod::Matrix3t{ + (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 pod::Matrix4t uf::matrix::inverse( const pod::Matrix4t& m ) { + const T* a = &m[0]; + ALIGN16 pod::Matrix4t 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]); + + // determinant + T det = a[0]*inv[0] + a[1] * inv[4] + a[2] * inv[8] + a[3] * inv[12]; + if ( std::fabs(det) < 1e-12f ) return m; // singular + + T invDet = 1 / det; + for (int i = 0; i < 16; ++i ) inv[i] *= invDet; + + return inv; } template pod::Vector3t uf::matrix::multiply( const pod::Matrix4t& mat, const pod::Vector3t& vector, T w, bool div ) { return uf::matrix::multiply( mat, pod::Vector4t{ vector[0], vector[1], vector[2], w }, div ); } +template +pod::Vector2t uf::matrix::multiply(const pod::Matrix2t& mat, const pod::Vector2t& v ) { + return pod::Vector2t{ + v[0]* mat[0] + v[1]* mat[2], + v[0]* mat[1] + v[1]* mat[3], + }; +} + +template +pod::Vector3t uf::matrix::multiply(const pod::Matrix3t& mat, const pod::Vector3t& v ) { + return pod::Vector3t{ + 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], + }; +} template pod::Vector4t uf::matrix::multiply( const pod::Matrix4t& mat, const pod::Vector4t& vector, bool div ) { #if UF_ENV_DREAMCAST MATH_Load_XMTRX( (ALL_FLOATS_STRUCT*) &mat[0] ); @@ -455,8 +471,6 @@ template pod::Vector4t uf::matrix::multiply( const pod::Matrix4t< #endif } // Writes to first value -// Multiplies two matrices of same type and size together -// Flip sign of all components template T& uf::matrix::invert( T& matrix ) { return matrix = uf::matrix::inverse((const T&) matrix); } @@ -542,11 +556,11 @@ template pod::Matrix4t /*UF_API*/ uf::matrix::orthographic( T l, T r, T b, T t, T f, T n ) { ALIGN16 pod::Matrix4t 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); + 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); return m; /* uf::stl::vector m = { @@ -575,15 +589,15 @@ pod::Matrix4t /*UF_API*/ uf::matrix::perspective( T fov, T raidou, T znear, T return pod::Matrix4t({ f / raidou, 0, 0, 0, 0, f, 0, 0, - 0, 0, 0, 1, - 0, 0, znear, 0 + 0, 0, 0, 1, + 0, 0, znear, 0 }); #elif UF_USE_VULKAN return pod::Matrix4t({ f / raidou, 0, 0, 0, 0, -f, 0, 0, - 0, 0, 0, 1, - 0, 0, znear, 0 + 0, 0, 0, 1, + 0, 0, znear, 0 }); #endif } else { diff --git a/engine/inc/uf/utils/math/physics/impl.h b/engine/inc/uf/utils/math/physics/impl.h index 56a61221..1fb6c07d 100644 --- a/engine/inc/uf/utils/math/physics/impl.h +++ b/engine/inc/uf/utils/math/physics/impl.h @@ -22,22 +22,21 @@ namespace pod { SPHERE, PLANE, CAPSULE, + TRIANGLE, MESH, }; struct SupportPoint { - pod::Vector3f p; // Minkowski difference point - pod::Vector3f pA; // original support point in A - pod::Vector3f pB; // original support point in B + pod::Vector3f p; + pod::Vector3f pA; + pod::Vector3f pB; }; struct Simplex { - //uf::stl::vector pts; uf::stl::vector pts; }; struct Face { - // pod::Vector3f a,b,c; pod::SupportPoint a, b, c; pod::Vector3f normal; float distance; @@ -56,7 +55,7 @@ namespace pod { uf::stl::vector nodes; }; - struct MeshCollider { + struct MeshBVH { pod::BVH* bvh; const uf::Mesh* mesh; }; @@ -71,7 +70,8 @@ namespace pod { pod::AABB aabb; pod::Plane plane; pod::Capsule capsule; - pod::MeshCollider mesh; + pod::TriangleWithNormal triangle; + pod::MeshBVH mesh; } u; }; diff --git a/engine/inc/uf/utils/math/shapes.h b/engine/inc/uf/utils/math/shapes.h index 85baafd0..4e9dfd4f 100644 --- a/engine/inc/uf/utils/math/shapes.h +++ b/engine/inc/uf/utils/math/shapes.h @@ -32,8 +32,8 @@ namespace pod { pod::Vector3f points[3]; }; - struct TriangleWithNormal : Triangle { // to-do: find a better name - pod::Vector3f normals[3]; + struct TriangleWithNormal : Triangle { + pod::Vector3f normals[3]; }; template diff --git a/engine/inc/uf/utils/tests/tests.h b/engine/inc/uf/utils/tests/tests.h index 6369e80d..17a16420 100644 --- a/engine/inc/uf/utils/tests/tests.h +++ b/engine/inc/uf/utils/tests/tests.h @@ -26,6 +26,34 @@ UF_MSG_DEBUG("[{}] {}", test.passes ? "PASS" : "FAIL", test.message);\ } + #define EXPECT_GT(a,b) { \ + auto& test = uf::unitTests::tests[uf::unitTests::current];\ + test.passes = (((a) > (b)));\ + test.message = ::fmt::format("EXPECT_GT({}, {}), ({} > {})", #a, #b, a, b);\ + UF_MSG_DEBUG("[{}] {}", test.passes ? "PASS" : "FAIL", test.message);\ + } + + #define EXPECT_LT(a,b) { \ + auto& test = uf::unitTests::tests[uf::unitTests::current];\ + test.passes = (((a) < (b)));\ + test.message = ::fmt::format("EXPECT_LT({}, {}), ({} < {})", #a, #b, a, b);\ + UF_MSG_DEBUG("[{}] {}", test.passes ? "PASS" : "FAIL", test.message);\ + } + + #define EXPECT_GE(a,b) { \ + auto& test = uf::unitTests::tests[uf::unitTests::current];\ + test.passes = (((a) >= (b)));\ + test.message = ::fmt::format("EXPECT_GE({}, {}), ({} >= {})", #a, #b, a, b);\ + UF_MSG_DEBUG("[{}] {}", test.passes ? "PASS" : "FAIL", test.message);\ + } + + #define EXPECT_LE(a,b) { \ + auto& test = uf::unitTests::tests[uf::unitTests::current];\ + test.passes = (((a) <= (b)));\ + test.message = ::fmt::format("EXPECT_LE({}, {}), ({} <= {})", #a, #b, a, b);\ + UF_MSG_DEBUG("[{}] {}", test.passes ? "PASS" : "FAIL", test.message);\ + } + #define EXPECT_NEAR(a,b,eps) { \ auto& test = uf::unitTests::tests[uf::unitTests::current];\ test.passes = (std::fabs((a)-(b)) <= (eps));\ diff --git a/engine/src/engine/ext/player/behavior.cpp b/engine/src/engine/ext/player/behavior.cpp index 0b603baf..424c5a56 100644 --- a/engine/src/engine/ext/player/behavior.cpp +++ b/engine/src/engine/ext/player/behavior.cpp @@ -297,7 +297,7 @@ void ext::PlayerBehavior::tick( uf::Object& self ) { if ( query.hit ) { UF_MSG_DEBUG("{}: {} | {}", query.contact.penetration, uf::string::toString(*query.body->object), uf::vector::toString(physics.velocity)); stats.floored = true; - // if ( physics.velocity.y < 0.0f ) physics.velocity.y = 0.0f; + if ( physics.velocity.y < 0.0f ) physics.velocity.y = 0.0f; } #endif } diff --git a/engine/src/utils/math/physics/epa.inl b/engine/src/utils/math/physics/epa.inl index af52d923..d26629c0 100644 --- a/engine/src/utils/math/physics/epa.inl +++ b/engine/src/utils/math/physics/epa.inl @@ -77,7 +77,7 @@ namespace { } } - pod::Contact epa( const pod::RigidBody& A, const pod::RigidBody& B, const pod::Simplex& simplex, int maxIterations = 64, float eps = EPS(1.0e-4f) ) { + pod::Contact epa( const pod::RigidBody& a, const pod::RigidBody& b, const pod::Simplex& simplex, int maxIterations = 64, float eps = EPS(1.0e-4f) ) { UF_ASSERT( ::isValidSimplex(simplex) ); auto faces = ::initialPolytope(simplex); @@ -98,7 +98,7 @@ namespace { auto& f = faces[idx]; // new support - auto sp = ::supportMinkowskiDetailed( A, B, f.normal ); + auto sp = ::supportMinkowskiDetailed( a, b, f.normal ); float d = uf::vector::dot( sp.p, f.normal ); // convergence check @@ -120,11 +120,6 @@ namespace { penetration = -penetration; } - // discard invalid contact - if ( !std::isfinite(contact.x) || !std::isfinite(contact.y) || !std::isfinite(contact.z) || penetration <= 0.0f ) { - return {}; - } - return { contact, normal, penetration }; } diff --git a/engine/src/utils/math/physics/gjk.inl b/engine/src/utils/math/physics/gjk.inl index 69064cf9..901f41d8 100644 --- a/engine/src/utils/math/physics/gjk.inl +++ b/engine/src/utils/math/physics/gjk.inl @@ -15,14 +15,21 @@ namespace { auto up = uf::quaternion::rotate( body.transform->orientation, pod::Vector3f{0,1,0} ); auto p1 = body.transform->position + up * body.collider.u.capsule.halfHeight; auto p2 = body.transform->position - up * body.collider.u.capsule.halfHeight; - // get closest end - auto end = ( uf::vector::dot( dir, p1 ) > uf::vector::dot( dir, p2 ) ) ? p1 : p2; - + auto end = ( uf::vector::dot( dir, p1 ) > uf::vector::dot( dir, p2 ) ) ? p1 : p2; // get closest end return end + uf::vector::normalize( dir ) * body.collider.u.capsule.radius; } - // to-do: mesh - default: { + case pod::ShapeType::TRIANGLE: { + const auto& tri = body.collider.u.triangle; + float d0 = uf::vector::dot( tri.points[0], dir ); + float d1 = uf::vector::dot( tri.points[1], dir ); + float d2 = uf::vector::dot( tri.points[2], dir ); + if ( d0 > d1 && d0 > d2 ) return tri.points[0]; + if ( d1 > d2 ) return tri.points[1]; + return tri.points[2]; + } break; + + default: { } break; } return {}; diff --git a/engine/src/utils/math/physics/helpers.inl b/engine/src/utils/math/physics/helpers.inl index 5c3485b2..229b92f9 100644 --- a/engine/src/utils/math/physics/helpers.inl +++ b/engine/src/utils/math/physics/helpers.inl @@ -30,10 +30,16 @@ namespace { bool meshCapsule( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); bool meshMesh( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool triangleTriangle( const pod::TriangleWithNormal& a, const pod::TriangleWithNormal& b, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool triangleAabb( const pod::TriangleWithNormal& tri, const pod::RigidBody& body, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool triangleSphere( const pod::TriangleWithNormal& tri, const pod::RigidBody& body, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool trianglePlane( const pod::TriangleWithNormal& tri, const pod::RigidBody& body, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool triangleCapsule( const pod::TriangleWithNormal& tri, const pod::RigidBody& body, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); + bool aabbOverlap( const pod::AABB& a, const pod::AABB& b, float eps = EPS(1.0e-6f) ); pod::AABB computeTriangleAABB( const pod::Triangle& tri ); - void reduceContacts( pod::Manifold& manifold ); + void solveContacts( uf::stl::vector& manifolds, float dt ); void traverseNodePair( const pod::BVH& bvh, int leftID, int rightID, pod::BVH::pair_t& pairs ); void traverseNodePair( const pod::BVH& a, int nodeA, const pod::BVH& b, int nodeB, pod::BVH::pair_t& out ); diff --git a/engine/src/utils/math/physics/impl.cpp b/engine/src/utils/math/physics/impl.cpp index 904561ac..1e81a910 100644 --- a/engine/src/utils/math/physics/impl.cpp +++ b/engine/src/utils/math/physics/impl.cpp @@ -8,12 +8,12 @@ namespace { bool warmupSolver = false; bool blockContactSolver = false; bool useGjk = false; - bool fixedStep = true; + bool fixedStep = false; int substeps = 0; - int solverIterations = 20; - float baumgarteCorrectionPercent = 0.4f; - float baumgarteCorrectionSlop = 0.01f; + int solverIterations = 10; + float baumgarteCorrectionPercent = 0.2f; + float baumgarteCorrectionSlop = 0.001f; uf::stl::unordered_map manifoldsCache; } @@ -26,12 +26,14 @@ namespace { #include "sphere.inl" #include "plane.inl" #include "capsule.inl" +#include "triangle.inl" #include "mesh.inl" #include "ray.inl" #include "bvh.inl" #include "gjk.inl" #include "epa.inl" #include "integration.inl" +#include "solvers.inl" // unused float uf::physics::impl::timescale = 1.0f / 60.0f; @@ -120,6 +122,10 @@ void uf::physics::impl::step( pod::World& world, float dt ) { } // retrieve accumulated impulses if ( ::warmupSolver ) ::retrieveContacts( manifold, ::manifoldsCache[::makePairKey( a, b )] ); + // merge similar contacts from a mesh to ensure continuity + if ( a.collider.type == pod::ShapeType::MESH || b.collider.type == pod::ShapeType::MESH ) { + ::mergeContacts( manifold ); + } // keep four most important contacts ::reduceContacts( manifold ); #if 0 @@ -128,6 +134,8 @@ void uf::physics::impl::step( pod::World& world, float dt ) { UF_MSG_DEBUG("contact={}, normal={}, depth={}", uf::vector::toString( contact.point ), uf::vector::toString( contact.normal ), contact.penetration ); } #endif + // no points remained, skip + if ( manifold.points.empty() ) continue; // store manifold manifolds.emplace_back(manifold); diff --git a/engine/src/utils/math/physics/integration.inl b/engine/src/utils/math/physics/integration.inl index 69192a10..7c395554 100644 --- a/engine/src/utils/math/physics/integration.inl +++ b/engine/src/utils/math/physics/integration.inl @@ -104,31 +104,62 @@ namespace { return false; } - bool similarContact( const pod::Contact& a, const pod::Contact& b, float eps = 1.0e-3f, float distSq = 0.95f ) { - return uf::vector::distanceSquared(a.point, b.point) < eps && uf::vector::dot(a.normal, b.normal) > distSq; + bool similarContact( const pod::Contact& a, const pod::Contact& b, float distSq = 1.0e-2f, float norm = 0.9f ) { + return uf::vector::distanceSquared(a.point, b.point) < distSq && uf::vector::dot(a.normal, b.normal) > norm; } void reduceContacts( pod::Manifold& manifold ) { if ( manifold.points.size() <= 4 ) return; - uf::stl::vector reduced; + uf::stl::vector result; for ( auto& c : manifold.points ) { + // prune invalid contacts + if ( !uf::vector::isValid( c.point ) ) continue; + bool merged = false; - for ( auto& r : reduced ) { + for ( auto& r : result ) { if ( !::similarContact( c, r ) ) continue; // merge, pick deeper penetration - if (c.penetration > r.penetration) r = c; + if ( c.penetration > r.penetration ) r = c; merged = true; break; } - if ( !merged ) reduced.emplace_back(c); + if ( !merged ) result.emplace_back(c); } + + UF_MSG_DEBUG("Reduced {} => {} contacts", manifold.points.size(), result.size()); // keep only deepest + farthest up to 4 - std::sort(reduced.begin(), reduced.end(), [](auto& a, auto& b){ return a.penetration > b.penetration; }); - if (reduced.size() > 4) reduced.resize(4); + std::sort(result.begin(), result.end(), [](auto& a, auto& b){ return a.penetration > b.penetration; }); + if ( result.size() > 4 ) result.resize(4); - manifold.points = reduced; + manifold.points = result; + } + + void mergeContacts( pod::Manifold& manifold ) { + uf::stl::vector result; + + for ( auto& c : manifold.points ) { + bool merged = false; + for ( auto& r : result ) { + if ( !::similarContact( c, r ) ) continue; + // merge: average position + normal, keep max penetration + r.point = ( r.point + c.point ) * 0.5f; + r.normal = uf::vector::normalize( r.normal + c.normal ); + r.penetration = std::max( r.penetration, c.penetration ); + merged = true; + break; + } + if ( !merged ) result.emplace_back( c ); + } + + UF_MSG_DEBUG("Merged {} => {} contacts", manifold.points.size(), result.size()); + + for ( auto& c : result ) { + UF_MSG_DEBUG("contact={}, normal={}, depth={}", uf::vector::toString( c.point ), uf::vector::toString( c.normal ), c.penetration ); + } + + manifold.points = result; } void retrieveContacts( pod::Manifold& current, const pod::Manifold& previous, float decay = 0.35f ) { @@ -163,6 +194,8 @@ namespace { // baumgarte position correction void positionCorrection( pod::RigidBody& a, pod::RigidBody& b, const pod::Contact& contact ) { + if ( ::baumgarteCorrectionPercent <= 0 ) return; + float correctionMagnitude = std::max(contact.penetration - ::baumgarteCorrectionSlop, 0.0f) / (a.inverseMass + b.inverseMass) * ::baumgarteCorrectionPercent; pod::Vector3f correction = contact.normal * correctionMagnitude; @@ -170,108 +203,6 @@ namespace { if ( !b.isStatic ) b.transform->position += correction * b.inverseMass; } - void resolveContact( pod::RigidBody& a, pod::RigidBody& b, pod::Contact& contact, float dt ) { - // relative positions from centers to contact point - pod::Vector3f rA = contact.point - a.transform->position; - pod::Vector3f rB = contact.point - b.transform->position; - - // relative velocity at contact - pod::Vector3f vA = a.velocity + uf::vector::cross(a.angularVelocity, rA); - pod::Vector3f vB = b.velocity + uf::vector::cross(b.angularVelocity, rB); - pod::Vector3f rv = vB - vA; - - // normal contact velocity - float velAlongNormal = uf::vector::dot(rv, contact.normal); - float velTolerance = 0; // -1.0e3f; - if ( velAlongNormal > velTolerance ) return; // if separating, no impulse - - // compute restitution (bounce) - float e = std::min(a.material.restitution, b.material.restitution); - - // nullify restitution if velocity is small enough - if ( fabs(velAlongNormal) < 1.0f) e = 0.0f; - - // effective inverse mass along normal - float invMassN = ::computeEffectiveMass(a, b, rA, rB, contact.normal); - - // normal impulse scalar - float jn = -(1.0f + e) * velAlongNormal; - jn /= invMassN; - if ( ::warmupSolver ) { - float jnOld = contact.accumulatedNormalImpulse; - float jnNew = std::max(0.0f, jnOld + jn); - float jnDelta = jnNew - jnOld; - contact.accumulatedNormalImpulse = jnNew; - jn = jnDelta; - } - - pod::Vector3f normalImpulse = contact.normal * jn; - ::applyImpulseTo(a, b, rA, rB, normalImpulse); - - // tangent direction - pod::Vector3f tangent = rv - contact.normal * uf::vector::dot(rv, contact.normal); - float tangentMag = uf::vector::magnitude(tangent); - if (tangentMag > EPS(1e-6f)) { - tangent /= tangentMag; - - // effective mass along tangent - float invMassT = ::computeEffectiveMass(a, b, rA, rB, tangent); - - // tangential relative velocity - float vt = uf::vector::dot(rv, tangent); - - // required tangential impulse to cancel tangent velocity - float jt = -vt / invMassT; - - // friction coefficients - float mu_s = std::sqrt(a.material.staticFriction * b.material.staticFriction); - float mu_d = std::sqrt(a.material.dynamicFriction * b.material.dynamicFriction); - - if ( std::fabs(jt) > jn * mu_s) jt = -jn * mu_d; // dynamic friction: resist sliding proportionally - - if ( ::warmupSolver ) { - float maxFriction = mu_s * contact.accumulatedNormalImpulse; - float jtOld = contact.accumulatedTangentImpulse; - float jtNew = std::max(-maxFriction, std::min(jtOld + jt, maxFriction)); - float jtDelta = jtNew - jtOld; - contact.accumulatedTangentImpulse = jtNew; - contact.tangent = tangent; - jt = jtDelta; - } - - pod::Vector3f frictionImpulse = tangent * jt; - ::applyImpulseTo(a, b, rA, rB, frictionImpulse); - } - - ::positionCorrection(a, b, contact); - } - - void solveContacts( uf::stl::vector& manifolds, float dt ) { - if ( ::warmupSolver ) { - for ( auto& m : manifolds ) { - for ( auto& c : m.points ) { - ::warmupContacts(*m.a, *m.b, c, dt); - } - } - } - - for ( auto i = 0; i < ::solverIterations; ++i ) { - for ( auto& m : manifolds ) { - #if 0 - if ( ::blockContactSolver ) { - //::solveManifoldBlockN( m, dt ); - ::solveManifoldBlock2x2( m, dt ); - } else { - for ( auto& c : m.points ) { - ::resolveContact(*m.a, *m.b, c, dt); - } - } - #endif - for ( auto& c : m.points ) ::resolveContact(*m.a, *m.b, c, dt); - } - } - } - void integrate( pod::RigidBody& body, float dt ) { // only integrate dynamic bodies if ( body.isStatic || body.mass == 0 ) return; diff --git a/engine/src/utils/math/physics/mesh.inl b/engine/src/utils/math/physics/mesh.inl index 3a7ed90f..4491b8b9 100644 --- a/engine/src/utils/math/physics/mesh.inl +++ b/engine/src/utils/math/physics/mesh.inl @@ -1,121 +1,18 @@ -#define REORIENT_NORMALS_ON_FETCH 0 -#define REORIENT_NORMALS_ON_CONTACT 0 +/* +auto bounds = ::transformAabbToLocal( aabb.bounds, *mesh.transform ); +*/ +/* +// transform capsule line segments to local space + p1 = uf::transform::applyInverse( *mesh.transform, p1 ); + p2 = uf::transform::applyInverse( *mesh.transform, p2 ); -// 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); - } - 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 ); +contact = uf::transform::apply( *mesh.transform, contact ); +normal = uf::quaternion::rotate( mesh.transform->orientation, normal ); - auto& v0 = *reinterpret_cast(reinterpret_cast(vertices) + i0 * vertexStride); - auto& v1 = *reinterpret_cast(reinterpret_cast(vertices) + i1 * vertexStride); - auto& v2 = *reinterpret_cast(reinterpret_cast(vertices) + i2 * vertexStride); - - 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}), - } - }; - } - - pod::TriangleWithNormal fetchTriangle( const uf::Mesh& mesh, size_t triID ) { - auto views = mesh.makeViews({"position", "normal"}); - UF_ASSERT(!views.empty()); - - uint32_t triIndexID = triID * 3; // remap triangle ID to index ID - // find which view contains this triangle index. - const uf::Mesh::View* found = nullptr; - for ( auto& v : views ) { - if ( v.index.first <= triIndexID && triIndexID < v.index.first + v.index.count ) { - found = &v; - break; - } - } - UF_ASSERT( found ); - - 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; - - // reset back to local indices range - triIndexID -= found->index.first; - - 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(positions.data(found->vertex.first)); - size_t stride = positions.stride(); - - for ( auto i = 0; i < 3; ++i ) tri.points[i] = *reinterpret_cast(base + idxs[i] * stride); - } - - if ( normals.valid() && false ) { - 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 = uf::vector::normalize(uf::vector::cross(tri.points[1]-tri.points[0], tri.points[2]-tri.points[0])); - 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::RigidBody& body ) { - auto tri = ::fetchTriangle( mesh, triID ); - if ( body.collider.type == pod::ShapeType::MESH ) { - if ( body.transform ) { - for ( auto i = 0; i < 3; ++i ) { - tri.points[i] = uf::transform::apply(*body.transform, tri.points[i]); - tri.normals[i] = uf::quaternion::rotate(body.transform->orientation, tri.normals[i]); - } - } - } - else { - #if REORIENT_NORMALS_ON_FETCH - auto triCenter = (tri.points[0] + tri.points[1] + tri.points[2]) / 3.0f; - auto delta = body.transform->position - triCenter; - for ( auto i = 0; i < 3; ++i ) { - if ( uf::vector::dot(tri.normals[i], delta) < 0.0f ) tri.normals[i] = -tri.normals[i]; - } - #endif - } - - return tri; - } -} +*/ // namespace { @@ -128,47 +25,22 @@ namespace { const auto& meshData = *mesh.collider.u.mesh.mesh; const auto& bvh = *mesh.collider.u.mesh.bvh; + // transform to local space for BVH query auto bounds = ::transformAabbToLocal( aabb.bounds, *mesh.transform ); - uf::stl::vector candidates; - ::queryBVH(bvh, bounds, candidates); + ::queryBVH( bvh, bounds, candidates ); bool hit = false; + // do collision per triangle for ( auto triID : candidates ) { - auto tri = ::fetchTriangle( meshData, triID, aabb ); - auto closestTri = ::closestPointOnTriangle( ::aabbCenter( bounds ), tri ); - auto closestAabb = ::closestPointOnAABB( closestTri, bounds ); - - if ( !uf::vector::isValid( closestTri ) ) { - //UF_MSG_DEBUG("tri #{}={}, {}, {}", triID, uf::vector::toString( tri.points[0] ), uf::vector::toString( tri.points[1] ), uf::vector::toString( tri.points[2] )); - //UF_MSG_DEBUG("closestTri={}, closestAabb={}", uf::vector::toString( closestTri ), uf::vector::toString( closestAabb )); - continue; - } - - auto delta = closestAabb - closestTri; // direction between points - float dist2 = uf::vector::magnitude( delta ); - float tolerance = 1.0e-3; // std::max( 1.0e-3f, uf::vector::magnitude(aabb.velocity) * manifold.dt * 0.5f ); - if ( dist2 >= tolerance ) continue; - float dist = std::sqrt( dist2 ); - - auto contact = closestTri; // ( closestAabb + closestTri ) * 0.5f; // center of points - auto normal = ::normalizeDelta( delta, dist, eps ); // uf::vector::normalize( ::interpolateWithBarycentric( tri.normals, ::computeBarycentric( contact, tri ) )); - float penetration = tolerance - dist; - - #if REORIENT_NORMALS_ON_CONTACT - if ( uf::vector::dot(normal, delta) < 0.0f ) normal = -normal; - #endif - - contact = uf::transform::apply( *mesh.transform, contact ); - normal = uf::quaternion::rotate( mesh.transform->orientation, normal ); - - manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); + auto tri = ::fetchTriangle( meshData, triID, mesh ); // transform triangle to world space + if ( !::triangleAabb( tri, aabb, manifold, eps ) ) continue; hit = true; } return hit; } bool meshSphere( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { - ASSERT_COLLIDER_TYPES(MESH, SPHERE); + ASSERT_COLLIDER_TYPES( MESH, SPHERE ); const auto& mesh = a; const auto& sphere = b; @@ -176,42 +48,16 @@ namespace { const auto& meshData = *mesh.collider.u.mesh.mesh; const auto& bvh = *mesh.collider.u.mesh.bvh; - auto bounds = ::transformAabbToLocal( ::computeAABB( sphere ), *mesh.transform ); - - auto center = ::aabbCenter( bounds ); - float r = sphere.collider.u.sphere.radius; - + // transform to local space for BVH query + auto bounds = ::transformAabbToLocal( sphere.bounds, *mesh.transform ); uf::stl::vector candidates; - ::queryBVH(bvh, bounds, candidates); + ::queryBVH( bvh, bounds, candidates ); bool hit = false; + // do collision per triangle for ( auto triID : candidates ) { - auto tri = ::fetchTriangle( meshData, triID, sphere ); - auto closest = ::closestPointOnTriangle( ::aabbCenter(bounds), tri ); - - if ( !uf::vector::isValid( closest ) ) { - //UF_MSG_DEBUG("tri #{}={}, {}, {}", triID, uf::vector::toString( tri.points[0] ), uf::vector::toString( tri.points[1] ), uf::vector::toString( tri.points[2] )); - //UF_MSG_DEBUG("closest={}", uf::vector::toString( closest )); - continue; - } - - auto delta = center - closest; - float dist2 = uf::vector::magnitude(delta); - if ( dist2 > r * r ) continue; - float dist = std::sqrt( dist2 ); - - auto contact = closest; - auto normal = ::normalizeDelta( delta, dist, eps ); - float penetration = r - dist; - - #if REORIENT_NORMALS_ON_CONTACT - if ( uf::vector::dot(normal, delta) < 0.0f ) normal = -normal; - #endif - - contact = uf::transform::apply(*mesh.transform, contact); - normal = uf::quaternion::rotate(mesh.transform->orientation, normal); - - manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); + auto tri = ::fetchTriangle( meshData, triID, mesh ); // transform triangle to world space + if ( !::triangleSphere( tri, sphere, manifold, eps ) ) continue; hit = true; } @@ -219,11 +65,31 @@ namespace { } // to-do bool meshPlane( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { - ASSERT_COLLIDER_TYPES(MESH, PLANE); - return false; + ASSERT_COLLIDER_TYPES( MESH, PLANE ); + + const auto& mesh = a; + const auto& plane = b; + + const auto& meshData = *mesh.collider.u.mesh.mesh; + const auto& bvh = *mesh.collider.u.mesh.bvh; + + // transform to local space for BVH query + auto bounds = ::transformAabbToLocal( plane.bounds, *mesh.transform ); + uf::stl::vector candidates; + ::queryBVH( bvh, bounds, candidates ); + + bool hit = false; + // do collision per triangle + for ( auto triID : candidates ) { + auto tri = ::fetchTriangle( meshData, triID, mesh ); // transform triangle to world space + if ( !::trianglePlane( tri, plane, manifold, eps ) ) continue; + hit = true; + } + + return hit; } bool meshCapsule( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { - ASSERT_COLLIDER_TYPES(MESH, CAPSULE); + ASSERT_COLLIDER_TYPES( MESH, CAPSULE ); const auto& mesh = a; const auto& capsule = b; @@ -231,42 +97,18 @@ namespace { const auto& meshData = *mesh.collider.u.mesh.mesh; const auto& bvh = *mesh.collider.u.mesh.bvh; - // capsule line segment in world space - auto [ p1, p2 ] = ::getCapsuleSegment(b); - float r = b.collider.u.capsule.radius; - - auto bounds = ::transformAabbToLocal( ::computeSegmentAABB(p1, p2, r), *a.transform ); - + // transform to local space for BVH query + auto bounds = ::transformAabbToLocal( capsule.bounds, *mesh.transform ); uf::stl::vector candidates; - ::queryBVH(bvh, bounds, candidates); + ::queryBVH( bvh, bounds, candidates ); + + UF_MSG_DEBUG("candidates={}", candidates.size()); - // for some reason (the segment points), it's just easier to transform the triangle to world space, than compare in local then transform the contact to world bool hit = false; + // do collision per triangle for ( auto triID : candidates ) { - auto tri = ::fetchTriangle( meshData, triID, mesh ); - - pod::Vector3f closestSeg, closestTri; - float dist2 = ::segmentTriangleDistanceSq( p1, p2, tri, closestSeg, closestTri ); - - if ( !uf::vector::isValid( closestTri ) ) { - //UF_MSG_DEBUG("tri #{}={}, {}, {}", triID, uf::vector::toString( tri.points[0] ), uf::vector::toString( tri.points[1] ), uf::vector::toString( tri.points[2] )); - //UF_MSG_DEBUG("closestTri={}", uf::vector::toString( closestTri )); - continue; - } - - if ( dist2 > r * r ) continue; - float dist = std::sqrt( dist2 ); - auto delta = ( closestSeg - closestTri ); - - auto contact = closestTri; // ( closestSeg + closestTri ) * 0.5f; // center of points - auto normal = ::normalizeDelta( delta, dist, eps ); // uf::vector::normalize( ::interpolateWithBarycentric( tri.normals, ::computeBarycentric( contact, tri ) )); - float penetration = r - dist; - - #if REORIENT_NORMALS_ON_CONTACT - if ( uf::vector::dot(normal, delta) < 0.0f ) normal = -normal; - #endif - - manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); + auto tri = ::fetchTriangle( meshData, triID, mesh ); // transform triangle to world space + if ( !::triangleCapsule( tri, capsule, manifold, eps ) ) continue; hit = true; } @@ -276,28 +118,24 @@ namespace { bool meshMesh( const pod::RigidBody& a, const pod::RigidBody& b, pod::Manifold& manifold, float eps ) { ASSERT_COLLIDER_TYPES( MESH, MESH ); - auto& bodyA = a; - auto& bvhA = *a.collider.u.mesh.bvh; - auto& meshA = *a.collider.u.mesh.mesh; + const auto& bvhA = *a.collider.u.mesh.bvh; + const auto& meshA = *a.collider.u.mesh.mesh; - auto& bodyB = b; - auto& meshB = *b.collider.u.mesh.mesh; - auto& bvhB = *b.collider.u.mesh.bvh; + const auto& meshB = *b.collider.u.mesh.mesh; + const auto& bvhB = *b.collider.u.mesh.bvh; + // compute overlaps between one BVH and another BVH pod::BVH::pair_t pairs; ::traverseNodePair(bvhA, 0, bvhB, 0, pairs); - for (auto [idA, idB] : pairs) { - auto tA = ::fetchTriangle( meshA, idA, bodyA ); - auto tB = ::fetchTriangle( meshB, idB, bodyB ); - - // narrowphase tri-tri overlap: SAT - if ( ::triangleTriangleIntersect( tA, tB ) ) { - pod::Vector3f contact = ( tA.points[0] + tB.points[0] )*0.5f; - pod::Vector3f n = uf::vector::normalize( uf::vector::cross( tA.points[1] - tA.points[0], tA.points[2] - tA.points[0] ) ); - manifold.points.emplace_back(pod::Contact{ contact, n, 0.001f }); - } + bool hit = false; + // do collision per triangle + for (auto [ idA, idB] : pairs ) { + auto tA = ::fetchTriangle( meshA, idA, a ); // transform triangles to world space + auto tB = ::fetchTriangle( meshB, idB, b ); + if ( !::triangleTriangle( tA, tB, manifold, eps ) ) continue; + hit = true; } - return !manifold.points.empty(); + return hit; } } \ No newline at end of file diff --git a/engine/src/utils/math/physics/solvers.inl b/engine/src/utils/math/physics/solvers.inl new file mode 100644 index 00000000..b1baf2dc --- /dev/null +++ b/engine/src/utils/math/physics/solvers.inl @@ -0,0 +1,278 @@ +namespace { + void iterativeImpulseSolver( pod::RigidBody& a, pod::RigidBody& b, pod::Contact& contact, float dt ) { + // relative positions from centers to contact point + pod::Vector3f rA = contact.point - a.transform->position; + pod::Vector3f rB = contact.point - b.transform->position; + + // relative velocity at contact + pod::Vector3f vA = a.velocity + uf::vector::cross(a.angularVelocity, rA); + pod::Vector3f vB = b.velocity + uf::vector::cross(b.angularVelocity, rB); + pod::Vector3f rv = vB - vA; + + // normal contact velocity + float velAlongNormal = uf::vector::dot(rv, contact.normal); + float velTolerance = 0; // -1.0e3f; + if ( velAlongNormal > velTolerance ) return; // if separating, no impulse + + // compute restitution (bounce) + float e = std::min(a.material.restitution, b.material.restitution); + + // nullify restitution if velocity is small enough + if ( fabs(velAlongNormal) < 1.0f) e = 0.0f; + + // effective inverse mass along normal + float invMassN = ::computeEffectiveMass(a, b, rA, rB, contact.normal); + + // normal impulse scalar + float jn = -(1.0f + e) * velAlongNormal; + jn /= invMassN; + if ( ::warmupSolver ) { + float jnOld = contact.accumulatedNormalImpulse; + float jnNew = std::max(0.0f, jnOld + jn); + float jnDelta = jnNew - jnOld; + contact.accumulatedNormalImpulse = jnNew; + jn = jnDelta; + } + + pod::Vector3f normalImpulse = contact.normal * jn; + ::applyImpulseTo(a, b, rA, rB, normalImpulse); + + // tangent direction + pod::Vector3f tangent = rv - contact.normal * uf::vector::dot(rv, contact.normal); + float tangentMag = uf::vector::magnitude(tangent); + if (tangentMag > EPS(1e-6f)) { + tangent /= tangentMag; + + // effective mass along tangent + float invMassT = ::computeEffectiveMass(a, b, rA, rB, tangent); + + // tangential relative velocity + float vt = uf::vector::dot(rv, tangent); + + // required tangential impulse to cancel tangent velocity + float jt = -vt / invMassT; + + // friction coefficients + float mu_s = std::sqrt(a.material.staticFriction * b.material.staticFriction); + float mu_d = std::sqrt(a.material.dynamicFriction * b.material.dynamicFriction); + + if ( std::fabs(jt) > jn * mu_s) jt = -jn * mu_d; // dynamic friction: resist sliding proportionally + + if ( ::warmupSolver ) { + float maxFriction = mu_s * contact.accumulatedNormalImpulse; + float jtOld = contact.accumulatedTangentImpulse; + float jtNew = std::max(-maxFriction, std::min(jtOld + jt, maxFriction)); + float jtDelta = jtNew - jtOld; + contact.accumulatedTangentImpulse = jtNew; + contact.tangent = tangent; + jt = jtDelta; + } + + pod::Vector3f frictionImpulse = tangent * jt; + ::applyImpulseTo(a, b, rA, rB, frictionImpulse); + } + + ::positionCorrection(a, b, contact); + } + + template + void blockNxNSolver( pod::RigidBody& a, pod::RigidBody& b, pod::Manifold& manifold, float dt ) { + pod::Matrix K = {}; + pod::Vector rhs = {}; + pod::Vector lambda = {}; + pod::Vector residual = {}; + + pod::Vector3f relVelLinear = b.velocity - a.velocity; + // precompute inverse masses + float invMassA = (a.isStatic ? 0.0f : a.inverseMass); + float invMassB = (b.isStatic ? 0.0f : b.inverseMass); + + for (int i = 0; i < N; i++) { + pod::Vector3f rA_i = manifold.points[i].point - a.transform->position; + pod::Vector3f rB_i = manifold.points[i].point - b.transform->position; + + for (int j = 0; j < N; j++) { + pod::Vector3f rA_j = manifold.points[j].point - a.transform->position; + pod::Vector3f rB_j = manifold.points[j].point - b.transform->position; + + pod::Vector3f n_i = manifold.points[i].normal; + pod::Vector3f n_j = manifold.points[j].normal; + + float termLinear = (invMassA + invMassB) * uf::vector::dot(n_i, n_j); + + // angular parts + pod::Vector3f raXnj = uf::vector::cross(rA_j, n_j); + pod::Vector3f rbXnj = uf::vector::cross(rB_j, n_j); + + pod::Vector3f Ia_raXnj = a.inverseInertiaTensor * raXnj; + pod::Vector3f Ib_rbXnj = b.inverseInertiaTensor * rbXnj; + + pod::Vector3f crossA = uf::vector::cross(Ia_raXnj, rA_i); + pod::Vector3f crossB = uf::vector::cross(Ib_rbXnj, rB_i); + + float termAngular = uf::vector::dot(n_i, crossA + crossB); + + K(i,j) = termLinear + termAngular; + } + + K(i,i) += 1e-3f; + } + + for ( auto i = 0; i < N; i++ ) { + float vRel = uf::vector::dot( relVelLinear, manifold.points[i].normal ); + + float penetrationBias = std::max( manifold.points[i].penetration - ::baumgarteCorrectionSlop, 0.0f) * (::baumgarteCorrectionPercent / dt ); + float cDot = vRel + penetrationBias; + + rhs[i] = (cDot < 0.0f) ? -cDot : 0.0f; + lambda[i] = manifold.points[i].accumulatedNormalImpulse; + } + + residual = rhs - uf::matrix::multiply( K, lambda ); + pod::Matrix Kinv = uf::matrix::invert( K ); + pod::Vector dLambda = uf::matrix::multiply( Kinv, residual ); + + for ( auto i = 0; i < N; i++ ) { + float newLambda = std::max(lambda[i] + dLambda[i], 0.0f); + dLambda[i] = newLambda - lambda[i]; + lambda[i] = newLambda; + manifold.points[i].accumulatedNormalImpulse = newLambda; + } + + pod::Vector3f impulse{}; + for ( auto i = 0; i < N; i++) { + impulse += manifold.points[i].normal * dLambda[i]; + } + + if ( !a.isStatic ) a.velocity -= impulse * a.inverseMass; + if ( !b.isStatic ) b.velocity += impulse * b.inverseMass; + } + + void block2x2Solver( pod::RigidBody& a, pod::RigidBody& b, pod::Manifold& manifold, float dt ) { + return ::blockNxNSolver<2>( a, b, manifold, dt ); + } + void block3x3Solver( pod::RigidBody& a, pod::RigidBody& b, pod::Manifold& manifold, float dt ) { + return ::blockNxNSolver<3>( a, b, manifold, dt ); + } + void block4x4Solver( pod::RigidBody& a, pod::RigidBody& b, pod::Manifold& manifold, float dt ) { + return ::blockNxNSolver<4>( a, b, manifold, dt ); + } + + void blockPGSSolver( pod::RigidBody& a, pod::RigidBody& b, pod::Manifold& manifold, float dt ) { + const int count = std::min( (int) manifold.points.size(), 4 ); + + // precompute inv mass + float invMassA = ( a.isStatic ? 0.0f : a.inverseMass ); + float invMassB = ( b.isStatic ? 0.0f : b.inverseMass ); + + // precompute Jacobians for each contact + struct ContactCache { + pod::Vector3f normal; + pod::Vector3f rA, rB; + float bias; + float effectiveMass; + } cache[4]; + + for ( auto i = 0; i < count; i++ ) { + auto& c = manifold.points[i]; + cache[i].normal = c.normal; + cache[i].rA = c.point - a.transform->position; + cache[i].rB = c.point - b.transform->position; + + // bias = restitution + Baumgarte correction + float vRel = uf::vector::dot( + (b.velocity + uf::vector::cross(b.angularVelocity, cache[i].rB)) - + (a.velocity + uf::vector::cross(a.angularVelocity, cache[i].rA)), + cache[i].normal + ); + + float e = std::min( a.material.restitution, b.material.restitution ); + float penetrationBias = std::max(c.penetration - ::baumgarteCorrectionSlop, 0.0f) * (::baumgarteCorrectionPercent / dt); + + cache[i].bias = (vRel < -1.0f ? -e * vRel : 0.0f) + penetrationBias; + + // effective mass = 1 / (J M^-1 J^T) + pod::Vector3f rnA = uf::vector::cross( cache[i].rA, cache[i].normal ); + pod::Vector3f rnB = uf::vector::cross( cache[i].rB, cache[i].normal ); + + pod::Vector3f Ia_rnA = rnA * a.inverseInertiaTensor; // diag mult + pod::Vector3f Ib_rnB = rnB * b.inverseInertiaTensor; + + float Knormal = invMassA + invMassB + uf::vector::dot(uf::vector::cross(Ia_rnA, cache[i].rA) + uf::vector::cross(Ib_rnB, cache[i].rB), cache[i].normal); + + cache[i].effectiveMass = (Knormal > 0.0f) ? 1.0f / Knormal : 0.0f; + } + + // initialize lambdas (warm start) + pod::Vector4f lambda = {}; + for ( auto i = 0; i < count; i++ ) { + lambda[i] = manifold.points[i].accumulatedNormalImpulse; + } + + // iterative PGS loop + for ( auto iter = 0; iter < ::solverIterations; iter++ ) { + for (int i = 0; i < count; i++) { + auto& c = manifold.points[i]; + auto& cc = cache[i]; + + // relative velocity along normal + pod::Vector3f dv = (b.velocity + uf::vector::cross(b.angularVelocity, cc.rB)) - (a.velocity + uf::vector::cross(a.angularVelocity, cc.rA)); + float vn = uf::vector::dot(dv, cc.normal); + + // compute delta impulse + float delta = cc.effectiveMass * (-(vn + cc.bias)); + + // accumulate and clamp + float newLambda = std::max( lambda[i] + delta, 0.0f ); + delta = newLambda - lambda[i]; + lambda[i] = newLambda; + + // apply impulse + pod::Vector3f P = cc.normal * delta; + if ( !a.isStatic ) { + a.velocity -= P * invMassA; + a.angularVelocity -= uf::vector::cross(cc.rA, P) * a.inverseInertiaTensor; + } + if ( !b.isStatic ) { + b.velocity += P * invMassB; + b.angularVelocity += uf::vector::cross(cc.rB, P) * b.inverseInertiaTensor; + } + } + } + + // store lambdas back + for ( auto i = 0; i < count; i++ ) { + manifold.points[i].accumulatedNormalImpulse = lambda[i]; + } + } + + void resolveManifold( pod::RigidBody& a, pod::RigidBody& b, pod::Manifold& manifold, float dt ) { + if ( manifold.points.size() == 2 ) return ::block2x2Solver( a, b, manifold, dt ); + if ( manifold.points.size() == 3 ) return ::block3x3Solver( a, b, manifold, dt ); + if ( manifold.points.size() == 4 ) return ::block4x4Solver( a, b, manifold, dt ); + + for ( auto& contact : manifold.points ) { + ::iterativeImpulseSolver( a, b, contact, dt ); + } + } + + void solveContacts( uf::stl::vector& manifolds, float dt ) { + for ( auto& m : manifolds ) { + ::blockPGSSolver( *m.a, *m.b, m, dt ); + } + /* + if ( ::warmupSolver ) { + for ( auto& m : manifolds ) { + for ( auto& c : m.points ) { + ::warmupContacts( *m.a, *m.b, c, dt ); + } + } + } + for ( auto i = 0; i < ::solverIterations; ++i ) { + for ( auto& m : manifolds ) { + ::resolveManifold( *m.a, *m.b, m, dt ); + } + } + */ + } +} \ No newline at end of file diff --git a/engine/src/utils/math/physics/tests.inl b/engine/src/utils/math/physics/tests.inl index a2ad6d33..99c4183f 100644 --- a/engine/src/utils/math/physics/tests.inl +++ b/engine/src/utils/math/physics/tests.inl @@ -28,7 +28,7 @@ namespace { } // list of unit tests to "standardly" verify the system works, but honestly this is a mess - +#if 0 TEST(SphereSphere_Collision, { pod::World world; uf::Object objA, objB; @@ -496,320 +496,508 @@ TEST(AabbPlane_RestingNoSink, { }) TEST(CapsuleSphere_Collision, { - pod::World world; - uf::Object capObj, sphObj; - auto& cap = uf::physics::impl::create(world, capObj, pod::Capsule{0.5f,1.0f}, 1.0f); - auto& sph = uf::physics::impl::create(world, sphObj, pod::Sphere{0.5f}, 1.0f); + pod::World world; + uf::Object capObj, sphObj; + auto& cap = uf::physics::impl::create(world, capObj, pod::Capsule{0.5f,1.0f}, 1.0f); + auto& sph = uf::physics::impl::create(world, sphObj, pod::Sphere{0.5f}, 1.0f); - cap.transform->position = {0,0,0}; - sph.transform->position = {0,0.5f,0}; // overlap + cap.transform->position = {0,0,0}; + sph.transform->position = {0,0.5f,0}; // overlap - pod::Manifold m; - bool collided = capsuleSphere(cap, sph, m); - EXPECT_TRUE(collided); - EXPECT_TRUE(!m.points.empty()); + pod::Manifold m; + bool collided = capsuleSphere(cap, sph, m); + EXPECT_TRUE(collided); + EXPECT_TRUE(!m.points.empty()); }) TEST(CapsuleSphere_NoCollision, { - pod::World world; - uf::Object capObj, sphObj; - auto& cap = uf::physics::impl::create(world, capObj, pod::Capsule{0.5f,1.0f}, 1.0f); - auto& sph = uf::physics::impl::create(world, sphObj, pod::Sphere{0.5f}, 1.0f); + pod::World world; + uf::Object capObj, sphObj; + auto& cap = uf::physics::impl::create(world, capObj, pod::Capsule{0.5f,1.0f}, 1.0f); + auto& sph = uf::physics::impl::create(world, sphObj, pod::Sphere{0.5f}, 1.0f); - cap.transform->position = {0,0,0}; - sph.transform->position = {0,5,0}; // too far + cap.transform->position = {0,0,0}; + sph.transform->position = {0,5,0}; // too far - pod::Manifold m; - bool collided = capsuleSphere(cap, sph, m); - EXPECT_TRUE(!collided); + pod::Manifold m; + bool collided = capsuleSphere(cap, sph, m); + EXPECT_TRUE(!collided); }) TEST(AabbSphere_Collision, { - pod::World world; - uf::Object objA, objB; - auto& box = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); - auto& sphere= uf::physics::impl::create(world, objB, pod::Sphere{0.5f}, 1.0f); + pod::World world; + uf::Object objA, objB; + auto& box = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); + auto& sphere= uf::physics::impl::create(world, objB, pod::Sphere{0.5f}, 1.0f); - box.transform->position = {0,0,0}; - sphere.transform->position = {0.75f,0,0}; // Intersecting into box + box.transform->position = {0,0,0}; + sphere.transform->position = {0.75f,0,0}; // Intersecting into box - pod::Manifold m; - bool collided = aabbSphere(box,sphere,m); - EXPECT_TRUE(collided); - EXPECT_TRUE(!m.points.empty()); + pod::Manifold m; + bool collided = aabbSphere(box,sphere,m); + EXPECT_TRUE(collided); + EXPECT_TRUE(!m.points.empty()); }) TEST(AabbSphere_NoCollision, { - pod::World world; - uf::Object objA, objB; - auto& box = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); - auto& sphere= uf::physics::impl::create(world, objB, pod::Sphere{0.5f}, 1.0f); + pod::World world; + uf::Object objA, objB; + auto& box = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); + auto& sphere= uf::physics::impl::create(world, objB, pod::Sphere{0.5f}, 1.0f); - box.transform->position = {0,0,0}; - sphere.transform->position = {5,0,0}; // too far away + box.transform->position = {0,0,0}; + sphere.transform->position = {5,0,0}; // too far away - pod::Manifold m; - bool collided = aabbSphere(box,sphere,m); - EXPECT_TRUE(!collided); + pod::Manifold m; + bool collided = aabbSphere(box,sphere,m); + EXPECT_TRUE(!collided); }) TEST(AabbPlane_Collision, { - pod::World world; - uf::Object objA, objB; - auto& box = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); - auto& plane = uf::physics::impl::create(world, objB, pod::Plane{{0,1,0},0.0f}, 0.0f); + pod::World world; + uf::Object objA, objB; + auto& box = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); + auto& plane = uf::physics::impl::create(world, objB, pod::Plane{{0,1,0},0.0f}, 0.0f); - box.transform->position = {0,0.5f,0}; // half interpenetrating plane + box.transform->position = {0,0.5f,0}; // half interpenetrating plane - pod::Manifold m; - bool collided = aabbPlane(box,plane,m); - EXPECT_TRUE(collided); - EXPECT_TRUE(!m.points.empty()); + pod::Manifold m; + bool collided = aabbPlane(box,plane,m); + EXPECT_TRUE(collided); + EXPECT_TRUE(!m.points.empty()); }) TEST(AabbPlane_NoCollision, { - pod::World world; - uf::Object objA, objB; - auto& box = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); - auto& plane = uf::physics::impl::create(world, objB, pod::Plane{{0,1,0},0.0f}, 0.0f); + pod::World world; + uf::Object objA, objB; + auto& box = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); + auto& plane = uf::physics::impl::create(world, objB, pod::Plane{{0,1,0},0.0f}, 0.0f); - box.transform->position = {0,5,0}; // clearly above + box.transform->position = {0,5,0}; // clearly above - pod::Manifold m; - bool collided = aabbPlane(box,plane,m); - EXPECT_TRUE(!collided); + pod::Manifold m; + bool collided = aabbPlane(box,plane,m); + EXPECT_TRUE(!collided); }) TEST(AabbCapsule_Collision, { - pod::World world; - uf::Object objA, objB; - auto& box = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); - auto& cap = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); + pod::World world; + uf::Object objA, objB; + auto& box = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); + auto& cap = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); - box.transform->position = {0,0,0}; - cap.transform->position = {0,0.5f,0}; // partially overlapping + box.transform->position = {0,0,0}; + cap.transform->position = {0,0.5f,0}; // partially overlapping - pod::Manifold m; - bool collided = aabbCapsule(box,cap,m); - EXPECT_TRUE(collided); - EXPECT_TRUE(!m.points.empty()); + pod::Manifold m; + bool collided = aabbCapsule(box,cap,m); + EXPECT_TRUE(collided); + EXPECT_TRUE(!m.points.empty()); }) TEST(AabbCapsule_NoCollision, { - pod::World world; - uf::Object objA, objB; - auto& box = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); - auto& cap = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); + pod::World world; + uf::Object objA, objB; + auto& box = uf::physics::impl::create(world, objA, pod::AABB{{-1,-1,-1},{1,1,1}}, 1.0f); + auto& cap = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); - box.transform->position = {0,0,0}; - cap.transform->position = {0,5,0}; + box.transform->position = {0,0,0}; + cap.transform->position = {0,5,0}; - pod::Manifold m; - bool collided = aabbCapsule(box,cap,m); - EXPECT_TRUE(!collided); + pod::Manifold m; + bool collided = aabbCapsule(box,cap,m); + EXPECT_TRUE(!collided); }) TEST(SphereCapsule_Collision, { - pod::World world; - uf::Object objA, objB; - auto& sphere = uf::physics::impl::create(world, objA, pod::Sphere{0.5f}, 1.0f); - auto& cap = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); + pod::World world; + uf::Object objA, objB; + auto& sphere = uf::physics::impl::create(world, objA, pod::Sphere{0.5f}, 1.0f); + auto& cap = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); - sphere.transform->position = {0,0.0f,0}; - cap.transform->position = {0,0.25f,0}; + sphere.transform->position = {0,0.0f,0}; + cap.transform->position = {0,0.25f,0}; - pod::Manifold m; - bool collided = sphereCapsule(sphere,cap,m); - EXPECT_TRUE(collided); - EXPECT_TRUE(!m.points.empty()); + pod::Manifold m; + bool collided = sphereCapsule(sphere,cap,m); + EXPECT_TRUE(collided); + EXPECT_TRUE(!m.points.empty()); }) TEST(SphereCapsule_NoCollision, { - pod::World world; - uf::Object objA, objB; - auto& sphere = uf::physics::impl::create(world, objA, pod::Sphere{0.5f}, 1.0f); - auto& cap = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); + pod::World world; + uf::Object objA, objB; + auto& sphere = uf::physics::impl::create(world, objA, pod::Sphere{0.5f}, 1.0f); + auto& cap = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); - sphere.transform->position = {0,5,0}; - cap.transform->position = {0,0,0}; + sphere.transform->position = {0,5,0}; + cap.transform->position = {0,0,0}; - pod::Manifold m; - bool collided = sphereCapsule(sphere,cap,m); - EXPECT_TRUE(!collided); + pod::Manifold m; + bool collided = sphereCapsule(sphere,cap,m); + EXPECT_TRUE(!collided); }) TEST(PlanePlane_NoCollision, { - pod::World world; - uf::Object objA, objB; - auto& planeA= uf::physics::impl::create(world, objA, pod::Plane{{0,1,0},0.0f}, 0.0f); - auto& planeB= uf::physics::impl::create(world, objB, pod::Plane{{0,0,1},0.0f}, 0.0f); + pod::World world; + uf::Object objA, objB; + auto& planeA= uf::physics::impl::create(world, objA, pod::Plane{{0,1,0},0.0f}, 0.0f); + auto& planeB= uf::physics::impl::create(world, objB, pod::Plane{{0,0,1},0.0f}, 0.0f); - pod::Manifold m; - bool collided = planePlane(planeA,planeB,m); - EXPECT_TRUE(!collided); // always false in your engine + pod::Manifold m; + bool collided = planePlane(planeA,planeB,m); + EXPECT_TRUE(!collided); // always false in your engine }) TEST(PlaneCapsule_Collision, { - pod::World world; - uf::Object objA, objB; - auto& plane = uf::physics::impl::create(world, objA, pod::Plane{{0,1,0},0.0f}, 0.0f); - auto& cap = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); + pod::World world; + uf::Object objA, objB; + auto& plane = uf::physics::impl::create(world, objA, pod::Plane{{0,1,0},0.0f}, 0.0f); + auto& cap = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); - cap.transform->position = {0,0.25f,0}; // foot intersecting + cap.transform->position = {0,0.25f,0}; // foot intersecting - pod::Manifold m; - bool collided = planeCapsule(plane,cap,m); - EXPECT_TRUE(collided); - EXPECT_TRUE(!m.points.empty()); + pod::Manifold m; + bool collided = planeCapsule(plane,cap,m); + EXPECT_TRUE(collided); + EXPECT_TRUE(!m.points.empty()); }) TEST(PlaneCapsule_NoCollision, { - pod::World world; - uf::Object objA, objB; - auto& plane = uf::physics::impl::create(world, objA, pod::Plane{{0,1,0},0.0f}, 0.0f); - auto& cap = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); + pod::World world; + uf::Object objA, objB; + auto& plane = uf::physics::impl::create(world, objA, pod::Plane{{0,1,0},0.0f}, 0.0f); + auto& cap = uf::physics::impl::create(world, objB, pod::Capsule{0.5f,1.0f}, 1.0f); - cap.transform->position = {0,5,0}; // far above + cap.transform->position = {0,5,0}; // far above - pod::Manifold m; - bool collided = planeCapsule(plane,cap,m); - EXPECT_TRUE(!collided); + pod::Manifold m; + bool collided = planeCapsule(plane,cap,m); + EXPECT_TRUE(!collided); }) TEST(MeshSphere_Collision, { - pod::World world; - uf::Object objMesh, objSphere; + pod::World world; + uf::Object objMesh, objSphere; - // Create mesh body (a plane on Y=0, size=1) - auto mesh = ::generateMesh(1.0f); - auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f); // static mesh - meshBody.transform->position = {0,0,0}; + // Create mesh body (a plane on Y=0, size=1) + auto mesh = ::generateMesh(1.0f); + auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f); // static mesh + meshBody.transform->position = {0,0,0}; - // Sphere just above plane, radius 1, intersects - auto& sphereBody = uf::physics::impl::create(world, objSphere, pod::Sphere{2.0f}, 1.0f); - sphereBody.transform->position = {0,0.5f,0}; // half below plane (since plane is at y=0) + // Sphere just above plane, radius 1, intersects + auto& sphereBody = uf::physics::impl::create(world, objSphere, pod::Sphere{2.0f}, 1.0f); + sphereBody.transform->position = {0,0.5f,0}; // half below plane (since plane is at y=0) - pod::Manifold m; - bool collided = meshSphere(meshBody, sphereBody, m); - EXPECT_TRUE(collided); - EXPECT_TRUE(!m.points.empty()); - if ( !m.points.empty() ) EXPECT_TRUE(m.points[0].penetration > 0.0f); + pod::Manifold m; + bool collided = meshSphere(meshBody, sphereBody, m); + EXPECT_TRUE(collided); + EXPECT_TRUE(!m.points.empty()); + if ( !m.points.empty() ) EXPECT_TRUE(m.points[0].penetration > 0.0f); }) TEST(MeshSphere_NoCollision, { - pod::World world; - uf::Object objMesh, objSphere; + pod::World world; + uf::Object objMesh, objSphere; - auto mesh = ::generateMesh(1.0f); - auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f); - meshBody.transform->position = {0,0,0}; + auto mesh = ::generateMesh(1.0f); + auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f); + meshBody.transform->position = {0,0,0}; - auto& sphereBody = uf::physics::impl::create(world, objSphere, pod::Sphere{0.5f}, 1.0f); - sphereBody.transform->position = {0,5.0f,0}; // far above plane + auto& sphereBody = uf::physics::impl::create(world, objSphere, pod::Sphere{0.5f}, 1.0f); + sphereBody.transform->position = {0,5.0f,0}; // far above plane - pod::Manifold m; - bool collided = meshSphere(meshBody, sphereBody, m); - EXPECT_FALSE(collided); + pod::Manifold m; + bool collided = meshSphere(meshBody, sphereBody, m); + EXPECT_FALSE(collided); }) TEST(MeshAabb_Collision, { - pod::World world; - uf::Object objMesh, objBox; + pod::World world; + uf::Object objMesh, objBox; - auto mesh = ::generateMesh(2.0f); - auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f); - meshBody.transform->position = {0,0,0}; + auto mesh = ::generateMesh(2.0f); + auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f); + meshBody.transform->position = {0,0,0}; - pod::AABB box = { {-0.5f,-0.5f,-0.5f}, {0.5f,0.5f,0.5f} }; - auto& boxBody = uf::physics::impl::create(world, objBox, box, 1.0f); - boxBody.transform->position = {0,0.25f,0}; // overlaps plane + pod::AABB box = { {-0.5f,-0.5f,-0.5f}, {0.5f,0.5f,0.5f} }; + auto& boxBody = uf::physics::impl::create(world, objBox, box, 1.0f); + boxBody.transform->position = {0,0.25f,0}; // overlaps plane - pod::Manifold m; - bool collided = meshAabb(meshBody, boxBody, m); - EXPECT_TRUE(collided); - EXPECT_TRUE(!m.points.empty()); + pod::Manifold m; + bool collided = meshAabb(meshBody, boxBody, m); + EXPECT_TRUE(collided); + EXPECT_TRUE(!m.points.empty()); }) TEST(MeshAabb_NoCollision, { - pod::World world; - uf::Object objMesh, objBox; + pod::World world; + uf::Object objMesh, objBox; - auto mesh = ::generateMesh(2.0f); - auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f); - meshBody.transform->position = {0,0,0}; + auto mesh = ::generateMesh(2.0f); + auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f); + meshBody.transform->position = {0,0,0}; - pod::AABB box = { {-0.5f,-0.5f,-0.5f}, {0.5f,0.5f,0.5f} }; - auto& boxBody = uf::physics::impl::create(world, objBox, box, 1.0f); - boxBody.transform->position = {0,5.0f,0}; // above plane, no overlap + pod::AABB box = { {-0.5f,-0.5f,-0.5f}, {0.5f,0.5f,0.5f} }; + auto& boxBody = uf::physics::impl::create(world, objBox, box, 1.0f); + boxBody.transform->position = {0,5.0f,0}; // above plane, no overlap - pod::Manifold m; - bool collided = meshAabb(meshBody, boxBody, m); - EXPECT_FALSE(collided); + pod::Manifold m; + bool collided = meshAabb(meshBody, boxBody, m); + EXPECT_FALSE(collided); }) TEST(RayMesh_Hit, { - pod::World world; - uf::Object objMesh; + pod::World world; + uf::Object objMesh; - auto mesh = ::generateMesh(1.0f); - auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f); - meshBody.transform->position = {0,0,0}; + auto mesh = ::generateMesh(1.0f); + auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f); + meshBody.transform->position = {0,0,0}; - pod::Ray ray{ {0,1,0}, {0,-1,0} }; // from above, pointing down - pod::RayQuery hit = uf::physics::impl::rayCast(ray, world, 100.0f); + pod::Ray ray{ {0,1,0}, {0,-1,0} }; // from above, pointing down + pod::RayQuery hit = uf::physics::impl::rayCast(ray, world, 100.0f); - EXPECT_TRUE(hit.hit); - EXPECT_TRUE(hit.contact.penetration > 0.0f); + EXPECT_TRUE(hit.hit); + EXPECT_TRUE(hit.contact.penetration > 0.0f); }) TEST(RayMesh_Miss, { - pod::World world; - uf::Object objMesh; + pod::World world; + uf::Object objMesh; - auto mesh = ::generateMesh(1.0f); - auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f); - meshBody.transform->position = {0,0,0}; + auto mesh = ::generateMesh(1.0f); + auto& meshBody = uf::physics::impl::create(world, objMesh, mesh, 0.0f); + meshBody.transform->position = {0,0,0}; - pod::Ray ray{ {0,2,0}, {1,0,0} }; // parallel, goes sideways - pod::RayQuery hit = uf::physics::impl::rayCast(ray, world, 100.0f); + pod::Ray ray{ {0,2,0}, {1,0,0} }; // parallel, goes sideways + pod::RayQuery hit = uf::physics::impl::rayCast(ray, world, 100.0f); - EXPECT_FALSE(hit.hit); + EXPECT_FALSE(hit.hit); }) TEST(MeshMesh_Collision, { - pod::World world; - uf::Object objA, objB; + pod::World world; + uf::Object objA, objB; - auto mesh = ::generateMesh(1.0f); + auto mesh = ::generateMesh(1.0f); - auto& meshA = uf::physics::impl::create(world, objA, mesh, 0.0f); - auto& meshB = uf::physics::impl::create(world, objB, mesh, 0.0f); + auto& meshA = uf::physics::impl::create(world, objA, mesh, 0.0f); + auto& meshB = uf::physics::impl::create(world, objB, mesh, 0.0f); - meshA.transform->position = {0,0,0}; - meshB.transform->position = {0,0,0}; // same location + meshA.transform->position = {0,0,0}; + meshB.transform->position = {0,0,0}; // same location - pod::Manifold m; - bool collided = meshMesh(meshA, meshB, m); - EXPECT_TRUE(collided); + pod::Manifold m; + bool collided = meshMesh(meshA, meshB, m); + EXPECT_TRUE(collided); }) TEST(MeshMesh_NoCollision, { - pod::World world; - uf::Object objA, objB; + pod::World world; + uf::Object objA, objB; - auto mesh = ::generateMesh(1.0f); + auto mesh = ::generateMesh(1.0f); - auto& meshA = uf::physics::impl::create(world, objA, mesh, 0.0f); - auto& meshB = uf::physics::impl::create(world, objB, mesh, 0.0f); + auto& meshA = uf::physics::impl::create(world, objA, mesh, 0.0f); + auto& meshB = uf::physics::impl::create(world, objB, mesh, 0.0f); - meshA.transform->position = {0,0,0}; - meshB.transform->position = {0,10.0f,0}; // too far apart + meshA.transform->position = {0,0,0}; + meshB.transform->position = {0,10.0f,0}; // too far apart + + pod::Manifold m; + bool collided = meshMesh(meshA, meshB, m); + EXPECT_FALSE(collided); +}) +#endif + +#define EPS 1.0e-4 +#if 0 +TEST(TriangleTriangle_Collision_SimpleOverlap, { + // Two identical triangles overlapping on XY plane + pod::TriangleWithNormal triA { + { { {0,0,0}, {1,0,0}, {0,1,0} } }, + { {0,0,1}, {0,0,1}, {0,0,1} }, + }; + pod::TriangleWithNormal triB { + { { {0.25f,0.25f,0}, {1.25f,0.25f,0}, {0.25f,1.25f,0} } }, + { {0,0,1}, {0,0,1}, {0,0,1} }, + }; + + pod::Manifold m; + bool collided = triangleTriangle( triA, triB, m, EPS ); + + EXPECT_TRUE( collided ); + if ( !m.points.empty() ) { + EXPECT_NEAR(m.points[0].point.z, 0, EPS); // contact should be on z=0 plane + EXPECT_NEAR(uf::vector::norm(m.points[0].normal), 1.0f, EPS); + EXPECT_GE(m.points[0].penetration, 0.0f); + } +}) +#endif + +TEST(TriangleAabb_Collision_CenterInside, { + pod::TriangleWithNormal tri { + { { {0,0,0}, {1,0,0}, {0,1,0} } }, + { {0,0,1}, {0,0,1}, {0,0,1} }, + }; + + // Make cube overlapping + pod::RigidBody box; + pod::Transform<> transform; + box.transform = &transform; + box.collider.type = pod::ShapeType::AABB; + box.collider.u.aabb.min = {0.25f, 0.25f, -0.1f}; + box.collider.u.aabb.max = {0.75f, 0.75f, +0.1f}; + box.transform->position = ::aabbCenter( box.collider.u.aabb ); + box.bounds = ::computeAABB( box ); + + pod::Manifold m; + bool collided = triangleAabb(tri, box, m, EPS); + + EXPECT_TRUE(collided); + if ( !m.points.empty() ) { + EXPECT_NEAR(uf::vector::norm(m.points[0].normal), 1.0f, EPS); + EXPECT_GE(m.points[0].penetration, 0.0f); + + UF_MSG_DEBUG("contact={}, normal={}, depth={}", uf::vector::toString( m.points[0].point ), uf::vector::toString( m.points[0].normal ), m.points[0].penetration ); + } +}) + +TEST(TriangleSphere_Collision_SphereTouchingTriangle, { + pod::TriangleWithNormal tri { + { { {0,-1,0}, {1,1,0}, {-1,1,0} } }, + { {0,0,1}, {0,0,1}, {0,0,1} }, + }; + + pod::RigidBody sphere; + pod::Transform<> transform; + sphere.transform = &transform; + sphere.collider.type = pod::ShapeType::SPHERE; + sphere.collider.u.sphere.radius = 1.0f; + sphere.transform->position = {0,0,0.5f}; // Place sphere just above triangle so it penetrates slightly + sphere.bounds = ::computeAABB( sphere ); + + pod::Manifold m; + bool collided = triangleSphere(tri, sphere, m, EPS); + + EXPECT_TRUE(collided); + if ( !m.points.empty() ) { + EXPECT_NEAR(m.points[0].point.z, 0.0f, 0.5f); + EXPECT_NEAR(uf::vector::norm(m.points[0].normal), 1.0f, EPS); + + UF_MSG_DEBUG("contact={}, normal={}, depth={}", uf::vector::toString( m.points[0].point ), uf::vector::toString( m.points[0].normal ), m.points[0].penetration ); + EXPECT_GE(m.points[0].penetration, 0.0f); + } +}) + +TEST(TriangleCapsule_Collision_CapsuleIntersectingEdge, { + pod::TriangleWithNormal tri { + { { {0,0,0}, {2,0,0}, {1,1,0} } }, + { {0,0,1}, {0,0,1}, {0,0,1} }, + }; + + pod::RigidBody capsule; + pod::Transform<> transform; + capsule.transform = &transform; + + capsule.collider.type = pod::ShapeType::CAPSULE; + capsule.collider.u.capsule.radius = 0.5f; + auto [ p1, p2 ] = std::pair{ pod::Vector3f{1,0.5f,-1}, pod::Vector3f{1,0.5f,1} }; + capsule.bounds = computeSegmentAABB(p1, p2, capsule.collider.u.capsule.radius); + + pod::Manifold m; + bool collided = triangleCapsule(tri, capsule, m, EPS); + + EXPECT_TRUE(collided); + if ( !m.points.empty() ) { + EXPECT_NEAR(uf::vector::norm(m.points[0].normal), 1.0f, EPS); + EXPECT_GE(m.points[0].penetration, 0.0f); + + UF_MSG_DEBUG("contact={}, normal={}, depth={}", uf::vector::toString( m.points[0].point ), uf::vector::toString( m.points[0].normal ), m.points[0].penetration ); + } +}) + +TEST(TriangleSphere_Collision_SphereTangentFace, { + // Triangle is in Z=0 plane + pod::TriangleWithNormal tri { + { { {0,-1,0}, {1,1,0}, {-1,1,0} } }, + { {0,0,1}, {0,0,1}, {0,0,1} }, + }; + + pod::RigidBody sphere; + pod::Transform<> transform; + sphere.transform = &transform; + sphere.collider.type = pod::ShapeType::SPHERE; + sphere.collider.u.sphere.radius = 1.0f; + sphere.transform->position = {0, 0, 1.0f}; // center exactly 1 unit above plane + sphere.bounds = ::computeAABB(sphere); pod::Manifold m; - bool collided = meshMesh(meshA, meshB, m); - EXPECT_FALSE(collided); + bool collided = triangleSphere(tri, sphere, m, EPS); + + // Should either detect a grazing collision or at least not error + EXPECT_TRUE(collided); + if (!m.points.empty()) { + EXPECT_NEAR(m.points[0].penetration, 0.0f, 1e-4f); + EXPECT_NEAR(uf::vector::norm(m.points[0].normal), 1.0f, EPS); + } +}) + +TEST(TriangleSphere_Collision_SphereTangentEdge, { + // Triangle tilted in XY plane, edge from (0,0,0) to (1,0,0) + pod::TriangleWithNormal tri { + { { {0,0,0}, {1,0,0}, {0,1,0} } }, + { {0,0,1}, {0,0,1}, {0,0,1} }, + }; + + pod::RigidBody sphere; + pod::Transform<> transform; + sphere.transform = &transform; + sphere.collider.type = pod::ShapeType::SPHERE; + sphere.collider.u.sphere.radius = 0.5f; + + // Place sphere center exactly 0.5 units away from edge line + sphere.transform->position = {0.5f, -0.5f, 0.0f}; + sphere.bounds = ::computeAABB(sphere); + + pod::Manifold m; + bool collided = triangleSphere(tri, sphere, m, EPS); + + EXPECT_TRUE(collided); // Tangential along edge + if (!m.points.empty()) { + EXPECT_NEAR(m.points[0].penetration, 0.0f, 1e-4f); + } +}) + +TEST(TriangleCapsule_Collision_TangentVertex, { + pod::TriangleWithNormal tri { + { { {0,0,0}, {1,0,0}, {0,1,0} } }, + { {0,0,1}, {0,0,1}, {0,0,1} }, + }; + + pod::RigidBody capsule; + pod::Transform<> transform; + capsule.transform = &transform; + + capsule.collider.type = pod::ShapeType::CAPSULE; + capsule.collider.u.capsule.radius = 0.25f; + + // Align segment so it hovers exactly through the vertex (0,0,0) + pod::Vector3f p1{0.0f, -0.25f, 0.0f}; + pod::Vector3f p2{0.0f, -0.25f, 1.0f}; + + capsule.bounds = computeSegmentAABB(p1, p2, capsule.collider.u.capsule.radius); + + pod::Manifold m; + bool collided = triangleCapsule(tri, capsule, m, EPS); + + EXPECT_TRUE(collided); + if (!m.points.empty()) { + EXPECT_NEAR(m.points[0].penetration, 0.0f, 1e-4f); + } }) \ No newline at end of file diff --git a/engine/src/utils/math/physics/triangle.inl b/engine/src/utils/math/physics/triangle.inl new file mode 100644 index 00000000..4a796ea1 --- /dev/null +++ b/engine/src/utils/math/physics/triangle.inl @@ -0,0 +1,233 @@ +#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); + } + 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(reinterpret_cast(vertices) + i0 * vertexStride); + auto& v1 = *reinterpret_cast(reinterpret_cast(vertices) + i1 * vertexStride); + auto& v2 = *reinterpret_cast(reinterpret_cast(vertices) + i2 * vertexStride); + + 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}), + } + }; + } + + pod::TriangleWithNormal fetchTriangle( const uf::Mesh& mesh, size_t triID ) { + auto views = mesh.makeViews({"position", "normal"}); + UF_ASSERT(!views.empty()); + + uint32_t triIndexID = triID * 3; // remap triangle ID to index ID + // find which view contains this triangle index. + const uf::Mesh::View* found = nullptr; + for ( auto& v : views ) { + if ( v.index.first <= triIndexID && triIndexID < v.index.first + v.index.count ) { + found = &v; + break; + } + } + UF_ASSERT( found ); + + 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; + + // reset back to local indices range + triIndexID -= found->index.first; + + 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(positions.data(found->vertex.first)); + size_t stride = positions.stride(); + + for ( auto i = 0; i < 3; ++i ) tri.points[i] = *reinterpret_cast(base + idxs[i] * stride); + } + + if ( 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 = uf::vector::normalize(uf::vector::cross(tri.points[1]-tri.points[0], tri.points[2]-tri.points[0])); + 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::RigidBody& body ) { + auto tri = ::fetchTriangle( mesh, triID ); + if ( body.collider.type == pod::ShapeType::MESH ) { + if ( body.transform ) { + for ( auto i = 0; i < 3; ++i ) { + tri.points[i] = uf::transform::apply(*body.transform, tri.points[i]); + tri.normals[i] = uf::quaternion::rotate(body.transform->orientation, tri.normals[i]); + } + } + } + else { + #if REORIENT_NORMALS_ON_FETCH + auto triCenter = (tri.points[0] + tri.points[1] + tri.points[2]) / 3.0f; + auto delta = body.transform->position - triCenter; + for ( auto i = 0; i < 3; ++i ) { + if ( uf::vector::dot(tri.normals[i], delta) < 0.0f ) tri.normals[i] = -tri.normals[i]; + } + #endif + } + + return tri; + } + + pod::Vector3f computeTriangleNormal( const pod::TriangleWithNormal& tri ) { + return uf::vector::normalize(( tri.normals[0] + tri.normals[1] + tri.normals[2] ) / 3.0f); + } +} + +/* + +#if REORIENT_NORMALS_ON_CONTACT + if ( uf::vector::dot(normal, delta) < 0.0f ) normal = -normal; +#endif + + +// uf::vector::normalize( ::interpolateWithBarycentric( ::computeBarycentric( contact, tri ), tri.normals ) ); + +*/ + +// triangle colliders +namespace { + bool triangleTriangle( const pod::TriangleWithNormal& a, const pod::TriangleWithNormal& b, pod::Manifold& manifold, float eps ) { + if ( !::triangleTriangleIntersect( a, b ) ) return false; + + // to-do: properly derive the contact information + auto contact = ( a.points[0] + b.points[0] ) * 0.5f; // center point + auto normal = uf::vector::normalize( uf::vector::cross( a.points[1] - a.points[0], a.points[2] - a.points[0] ) ); + float penetration = 0.001f; + manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); + return true; + } + + bool triangleAabb( const pod::TriangleWithNormal& tri, const pod::RigidBody& body, pod::Manifold& manifold, float eps ) { + const auto& aabb = body; + + auto closest = ::closestPointOnTriangle( ::aabbCenter( aabb.bounds ), tri ); + auto closestAabb = ::closestPointOnAABB( closest, aabb.bounds ); + + if ( !uf::vector::isValid( closest ) ) return false; + + // to-do: derive proper delta + auto delta = closestAabb - closest; + float dist2 = uf::vector::dot( delta, delta ); + float tolerance = 1.0e-3; + if ( dist2 >= tolerance ) return false; + float dist = std::sqrt( dist2 ); + + // to-do: properly derive the contact information + auto contact = closest; + auto normal = ( dist > eps ) ? ( delta / dist ) : ::computeTriangleNormal( tri ); + float penetration = tolerance - dist; + + #if REORIENT_NORMALS_ON_CONTACT + if ( uf::vector::dot(normal, delta) < 0.0f ) normal = -normal; + #endif + + manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); + return true; + } + bool triangleSphere( const pod::TriangleWithNormal& tri, const pod::RigidBody& body, pod::Manifold& manifold, float eps ) { + const auto& sphere = body; + + float r = sphere.collider.u.sphere.radius; + auto center = ::aabbCenter( sphere.bounds ); + auto closest = ::closestPointOnTriangle( center, tri ); + + if ( !uf::vector::isValid( closest ) ) return false; + + // to-do: derive proper delta + auto delta = center - closest; + float dist2 = uf::vector::dot(delta, delta); + if ( dist2 > r * r ) return false; + float dist = std::sqrt(dist2); + + auto contact = closest; + auto normal = ( dist > eps ) ? ( delta / dist ) : ::computeTriangleNormal( tri ); + float penetration = r - dist; + + #if REORIENT_NORMALS_ON_CONTACT + if ( uf::vector::dot(normal, delta) < 0.0f ) normal = -normal; + #endif + + manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); + return true; + } + // to-do + bool trianglePlane( const pod::TriangleWithNormal& tri, const pod::RigidBody& body, pod::Manifold& manifold, float eps ) { + return false; + } + bool triangleCapsule( const pod::TriangleWithNormal& tri, const pod::RigidBody& body, pod::Manifold& manifold, float eps ) { + const auto& capsule = body; + + float r = capsule.collider.u.capsule.radius; + auto [ p1, p2 ] = ::getCapsuleSegment( capsule ); + auto bounds = ::computeSegmentAABB( p1, p2, r ); + + // to-do: derive proper delta + pod::Vector3f closestSeg, closest; + float dist2 = ::segmentTriangleDistanceSq( p1, p2, tri, closestSeg, closest ); + + if ( !uf::vector::isValid( closest ) ) return false; + if ( dist2 > r * r ) return false; + float dist = std::sqrt( dist2 ); + auto delta = ( closestSeg - closest ); + + // to-do: properly derive the contact information + auto contact = closest; + auto normal = ( dist > eps ) ? ( delta / dist ) : ::computeTriangleNormal( tri ); + float penetration = r - dist; + + #if REORIENT_NORMALS_ON_CONTACT + if ( uf::vector::dot(normal, delta) < 0.0f ) normal = -normal; + #endif + + manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); + return true; + } +} \ No newline at end of file