diff --git a/bin/data/config.json b/bin/data/config.json index 2028ab8e..23d8ef51 100644 --- a/bin/data/config.json +++ b/bin/data/config.json @@ -303,7 +303,7 @@ }, "reactphysics": { "global storage": false, - "timescale": 0.01666666666, + "timescale": 0.01666666666, // 0.03333333333, "interpolate": true, "gravity": { "mode": "default", // default / per-object / universal diff --git a/bin/data/scenes/sourceengine/base_sourceengine.json b/bin/data/scenes/sourceengine/base_sourceengine.json index 963a99c7..8d0f6eb9 100644 --- a/bin/data/scenes/sourceengine/base_sourceengine.json +++ b/bin/data/scenes/sourceengine/base_sourceengine.json @@ -16,6 +16,7 @@ "unwrap mesh": true }, "worldspawn_skybox": { + "physics": { "type": "mesh", "static": true, "mass": 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/bin/data/scenes/sourceengine/sourceengine.json b/bin/data/scenes/sourceengine/sourceengine.json index 95a20fe6..feac6dec 100644 --- a/bin/data/scenes/sourceengine/sourceengine.json +++ b/bin/data/scenes/sourceengine/sourceengine.json @@ -1,9 +1,9 @@ { // "import": "./rp_downtown_v2.json" -// "import": "./ss2_medsci1.json" + "import": "./ss2_medsci1.json" // "import": "./test_grid.json" // "import": "./sh2_mcdonalds.json" // "import": "./animal_crossing.json" - "import": "./mds_mcdonalds.json" +// "import": "./mds_mcdonalds.json" // "import": "./gm_construct.json" } \ No newline at end of file diff --git a/engine/inc/uf/utils/math/physics/impl.h b/engine/inc/uf/utils/math/physics/impl.h index a84d12af..4b1eb797 100644 --- a/engine/inc/uf/utils/math/physics/impl.h +++ b/engine/inc/uf/utils/math/physics/impl.h @@ -43,16 +43,42 @@ namespace pod { }; struct BVH { - typedef uf::stl::vector> pair_t; + typedef std::pair pair_t; + typedef uf::stl::vector pairs_t; + struct Node { pod::AABB bounds = {}; - int left = -1; - int right = -1; - int start = 0; - int count = 0; + int32_t left = -1; + int32_t right = -1; + int32_t start = 0; + int32_t count = 0; + + bool asleep = false; }; - uf::stl::vector indices; + struct FlatNode { + pod::AABB bounds = {}; + int32_t start = -1; + int32_t count = -1; + int32_t skipIndex = -1; + + bool asleep = false; + }; + struct UpdatePolicy { + enum class Decision { + NONE, // do nothing + REFIT, // refit bounds + REBUILD // rebuild from scratch + }; + float displacementThreshold = 0.25f; // 25% of AABB size + float overlapThreshold = 2.0f; // 2x growth in root surface area + float dirtyRatioThreshold = 0.3f; // 30% dirty bodies + int maxFramesBeforeRebuild = 60; // force rebuild every 60 frames + }; + + bool dirty = false; + uf::stl::vector indices; uf::stl::vector nodes; + uf::stl::vector flattened; }; struct MeshBVH { @@ -109,6 +135,15 @@ namespace pod { float dynamicFriction = 0.3f; }; + struct Activity { + bool awake = true; + float sleepTimer = 0.0f; + int32_t islandID = -1; + static constexpr float sleepThreshold = 0.5f; // seconds + static constexpr float linearSleepEpsilon = 0.01f; // m/s + static constexpr float angularSleepEpsilon = 0.01f; // rad/s + }; + struct World; // forward declare struct PhysicsBody { @@ -138,6 +173,7 @@ namespace pod { pod::AABB bounds; pod::Collider collider; pod::PhysicsMaterial material; + pod::Activity activity; }; struct Contact { @@ -165,6 +201,11 @@ namespace pod { pod::Contact contact = { pod::Vector3f{}, pod::Vector3f{}, FLT_MAX }; }; + struct Island { + bool awake = true; + uf::stl::vector bodies; + }; + struct World { uf::stl::vector bodies; diff --git a/engine/inc/uf/utils/math/shapes.h b/engine/inc/uf/utils/math/shapes.h index 4e9dfd4f..fa447b72 100644 --- a/engine/inc/uf/utils/math/shapes.h +++ b/engine/inc/uf/utils/math/shapes.h @@ -33,6 +33,9 @@ namespace pod { }; struct TriangleWithNormal : Triangle { + pod::Vector3f normal; + }; + struct TriangleWithNormals : Triangle { pod::Vector3f normals[3]; }; diff --git a/engine/inc/uf/utils/mesh/mesh.h b/engine/inc/uf/utils/mesh/mesh.h index 78e4b94c..f538c0f1 100644 --- a/engine/inc/uf/utils/mesh/mesh.h +++ b/engine/inc/uf/utils/mesh/mesh.h @@ -371,11 +371,11 @@ namespace uf { To* dstPtr = (To*) (dstBuffer.data()); if ( toEnum == uf::renderer::enums::Type::USHORT ) { - for ( size_t i = 0; i < elements; ++i ) dstPtr[i] = uf::quant::quantize_f32u16(srcPtr[i]); + for ( size_t i = 0; i < elements; ++i ) dstPtr[i] = (To) uf::quant::quantize_f32u16(srcPtr[i]); } else if ( fromEnum == uf::renderer::enums::Type::USHORT ) { - for ( size_t i = 0; i < elements; ++i ) dstPtr[i] = uf::quant::dequantize_u16f32(srcPtr[i]); + for ( size_t i = 0; i < elements; ++i ) dstPtr[i] = (To) uf::quant::dequantize_u16f32(srcPtr[i]); } else { - for ( size_t i = 0; i < elements; ++i ) dstPtr[i] = srcPtr[i]; + for ( size_t i = 0; i < elements; ++i ) dstPtr[i] = (To) srcPtr[i]; } srcBuffer.swap( dstBuffer ); diff --git a/engine/src/engine/graph/graph.cpp b/engine/src/engine/graph/graph.cpp index a93263f0..80106b1f 100644 --- a/engine/src/engine/graph/graph.cpp +++ b/engine/src/engine/graph/graph.cpp @@ -1384,6 +1384,7 @@ void uf::graph::process( pod::Graph& graph, int32_t index, uf::Object& parent ) if ( ext::json::isNull( metadataJson["physics"]["min"] ) ) metadataJson["physics"]["min"] = uf::vector::encode( min ); if ( ext::json::isNull( metadataJson["physics"]["max"] ) ) metadataJson["physics"]["max"] = uf::vector::encode( max ); } + #if !UF_GRAPH_EXTENDED if ( type == "mesh" ) { auto& physicsBody = entity.getComponent(); float mass = phyziks["mass"].as(physicsBody.mass); @@ -1395,6 +1396,7 @@ void uf::graph::process( pod::Graph& graph, int32_t index, uf::Object& parent ) uf::physics::impl::create( entity.as(), mesh, mass ); } + #endif } } } diff --git a/engine/src/utils/math/physics/aabb.inl b/engine/src/utils/math/physics/aabb.inl index 1f8a4c2b..a2028307 100644 --- a/engine/src/utils/math/physics/aabb.inl +++ b/engine/src/utils/math/physics/aabb.inl @@ -12,6 +12,11 @@ namespace { */ } + 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 ) { return { uf::vector::min( p1, p2 ) - r, @@ -205,14 +210,14 @@ namespace { if ( !::aabbOverlap( A, B ) ) return false; - // Calculate overlap extents + // calculate overlap extents float overlaps[3] = { std::min(A.max.x, B.max.x) - std::max(A.min.x, B.min.x), std::min(A.max.y, B.max.y) - std::max(A.min.y, B.min.y), std::min(A.max.z, B.max.z) - std::max(A.min.z, B.min.z) }; - // Determine collision axis = smallest overlap + // determine collision axis = smallest overlap int axis = -1; float minOverlap = FLT_MAX; for (int i = 0; i < 3; ++i) { @@ -226,7 +231,7 @@ namespace { pod::Vector3f normal{0,0,0}; normal[axis] = (delta[axis] < 0 ? -1.0f : 1.0f); - // Build manifold contacts: overlap region corners on the separating axis + // build manifold contacts: overlap region corners on the separating axis float xMin = std::max(A.min.x, B.min.x); float xMax = std::min(A.max.x, B.max.x); float yMin = std::max(A.min.y, B.min.y); @@ -234,20 +239,20 @@ namespace { float zMin = std::max(A.min.z, B.min.z); float zMax = std::min(A.max.z, B.max.z); - // On chosen axis, clamp to overlapped rectangle -> 4 potential points - if (axis == 0) { // X-axis separation, so face-on overlap in YZ plane + // on chosen axis, clamp to overlapped rectangle -> 4 potential points + if (axis == 0) { // x-axis separation, so face-on overlap in YZ plane manifold.points.emplace_back(pod::Contact{ { (normal.x > 0 ? A.max.x : A.min.x), yMin, zMin }, normal, minOverlap }); manifold.points.emplace_back(pod::Contact{ { (normal.x > 0 ? A.max.x : A.min.x), yMin, zMax }, normal, minOverlap }); manifold.points.emplace_back(pod::Contact{ { (normal.x > 0 ? A.max.x : A.min.x), yMax, zMin }, normal, minOverlap }); manifold.points.emplace_back(pod::Contact{ { (normal.x > 0 ? A.max.x : A.min.x), yMax, zMax }, normal, minOverlap }); } - 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{ { xMin, (normal.y > 0 ? A.max.y : A.min.y), zMin }, normal, minOverlap }); manifold.points.emplace_back(pod::Contact{ { xMin, (normal.y > 0 ? A.max.y : A.min.y), zMax }, normal, minOverlap }); manifold.points.emplace_back(pod::Contact{ { xMax, (normal.y > 0 ? A.max.y : A.min.y), zMin }, normal, minOverlap }); manifold.points.emplace_back(pod::Contact{ { xMax, (normal.y > 0 ? A.max.y : A.min.y), zMax }, normal, minOverlap }); } - else if (axis == 2) { // Z-axis separation, overlap in XY plane + else if (axis == 2) { // z-axis separation, overlap in XY plane manifold.points.emplace_back(pod::Contact{ { xMin, yMin, (normal.z > 0 ? A.max.z : A.min.z) }, normal, minOverlap }); manifold.points.emplace_back(pod::Contact{ { xMin, yMax, (normal.z > 0 ? A.max.z : A.min.z) }, normal, minOverlap }); manifold.points.emplace_back(pod::Contact{ { xMax, yMin, (normal.z > 0 ? A.max.z : A.min.z) }, normal, minOverlap }); diff --git a/engine/src/utils/math/physics/bvh.inl b/engine/src/utils/math/physics/bvh.inl index 48fc3633..f4e010e7 100644 --- a/engine/src/utils/math/physics/bvh.inl +++ b/engine/src/utils/math/physics/bvh.inl @@ -1,85 +1,5 @@ // BVH namespace { - // collects a list of nodes that are overlapping with each other - void traverseNodePair( const pod::BVH& bvh, int indexA, int indexB, pod::BVH::pair_t& pairs ) { - const auto& nodeA = bvh.nodes[indexA]; - const auto& nodeB = bvh.nodes[indexB]; - - if ( !::aabbOverlap( nodeA.bounds, nodeB.bounds ) ) return; - - if ( nodeA.count > 0 && nodeB.count > 0 ) { - for ( auto i = 0; i < nodeA.count; ++i ) - for ( auto j = 0; j < nodeB.count; ++j ) { - int indexA = bvh.indices[nodeA.start + i]; - int indexB = bvh.indices[nodeB.start + j]; - pairs.emplace_back(std::pair{indexA, indexB}); - } - return; - } - - if ( nodeA.count == 0 ) { - ::traverseNodePair( bvh, nodeA.left, indexB, pairs ); - ::traverseNodePair( bvh, nodeA.right, indexB, pairs ); - } else if ( nodeB.count == 0 ) { - ::traverseNodePair( bvh, indexA, nodeB.left, pairs ); - ::traverseNodePair( bvh, indexA, nodeB.right, pairs ); - } - } - // collects a list of nodes from each BVH that are overlapping with each other (for mesh v mesh) - void traverseNodePair( const pod::BVH& bvhA, int indexA, const pod::BVH& bvhB, int indexB, pod::BVH::pair_t& pairs ) { - const auto& nodeA = bvhA.nodes[indexA]; - const auto& nodeB = bvhB.nodes[indexB]; - - if ( !::aabbOverlap( nodeA.bounds, nodeB.bounds ) ) return; - - if ( nodeA.count > 0 && nodeB.count > 0 ) { - for ( auto i = 0; i < nodeA.count; i++ ) { - for ( auto j = 0; j < nodeB.count; j++ ) { - auto indexA = bvhA.indices[nodeA.start+i]; - auto indexB = bvhB.indices[nodeB.start+j]; - pairs.emplace_back(std::pair{indexA, indexB}); - } - } - return; - } - - if ( nodeA.count == 0 ) { - ::traverseNodePair( bvhA, nodeA.left, bvhB , indexB, pairs ); - ::traverseNodePair( bvhA, nodeA.right, bvhB , indexB, pairs ); - } else if ( nodeB.count == 0 ) { - ::traverseNodePair( bvhA, indexA, bvhB, nodeB.left, pairs ); - ::traverseNodePair( bvhA, indexA, bvhB, nodeB.right, pairs ); - } - } - - void traverseBVH( const pod::BVH& bvh, int nodeIdx, pod::BVH::pair_t& pairs ) { - const auto& node = bvh.nodes[nodeIdx]; - - if ( node.count > 0 ) { - for ( auto i = 0; i < node.count; ++i ) { - for ( auto j = i + 1; j < node.count; ++j ) { - int indexA = bvh.indices[node.start + i]; - int indexB = bvh.indices[node.start + j]; - - if ( !::aabbOverlap( bvh.nodes[indexA].bounds, bvh.nodes[indexB].bounds ) ) { - continue; - } - - pairs.emplace_back(std::pair{indexA, indexB}); - } - } - return; - } - - // recurse children - ::traverseNodePair( bvh, node.left, node.right, pairs ); - } - - void queryOverlaps( const pod::BVH& bvh, pod::BVH::pair_t& outPairs ) { - if ( bvh.nodes.empty() ) return; - ::traverseBVH( bvh, 0, outPairs ); - } - int buildBVHNode( pod::BVH& bvh, const uf::stl::vector& bounds, int start, int end, int capacity = 2 ) { pod::BVH::Node node{}; node.left = -1; @@ -106,7 +26,7 @@ namespace { int axis = (extent.x > extent.y && extent.x > extent.z) ? 0 : (extent.y > extent.z ? 1 : 2); // sort indices by centroid along axis - std::sort( bvh.indices.begin() + start, bvh.indices.begin() + end, [&](size_t a, size_t b) { + std::sort( bvh.indices.begin() + start, bvh.indices.begin() + end, [&](uint32_t a, uint32_t b) { float ca = ::aabbCenter( bounds[a] )[axis]; float cb = ::aabbCenter( bounds[b] )[axis]; return ca < cb; @@ -122,6 +42,125 @@ namespace { return index; } + int buildBVHNode_SAH( pod::BVH& bvh, const uf::stl::vector& bounds, int start, int end, int capacity = 4 ) { + struct Bin { + pod::AABB bounds; + int count = 0; + }; + + pod::BVH::Node node{}; + node.left = -1; + node.right = -1; + node.start = start; + node.count = 0; + node.bounds = bounds[bvh.indices[start]]; + + for ( auto i = start + 1; i < end; ++i ) node.bounds = ::mergeAabb( node.bounds, bounds[bvh.indices[i]] ); + + int count = end - start; + if ( count <= capacity ) { + node.count = count; + int index = (int) bvh.nodes.size(); + bvh.nodes.emplace_back(node); + return index; + } + + constexpr int numBins = 16; + static thread_local Bin bins[numBins]; + for ( auto i = 0; i < numBins; i++ ) bins[i] = {}; + + auto extent = node.bounds.max - node.bounds.min; + int bestAxis = -1, bestSplit = -1; + float bestCost = std::numeric_limits::infinity(); + + for ( auto axis = 0; axis < 3; ++axis ) { + if ( extent[axis] < EPS(1e-6f) ) continue; + + float minC = node.bounds.min[axis]; + float maxC = node.bounds.max[axis]; + float scale = (float) numBins / (maxC - minC); + + for ( auto i = start; i < end; ++i ) { + int idx = bvh.indices[i]; + float c = ::aabbCenter( bounds[idx] )[axis]; + int binID = std::min(numBins - 1, (int)((c - minC) * scale)); + bins[binID].count++; + bins[binID].bounds = ::mergeAabb( bins[binID].bounds, bounds[idx] ); + } + + pod::AABB leftBounds[numBins], rightBounds[numBins]; + int leftCount[numBins] = {}, rightCount[numBins] = {}; + + pod::AABB acc; + int cnt = 0; + for ( auto i = 0; i < numBins; i++ ) { + if ( bins[i].count > 0 ) acc = (cnt == 0) ? bins[i].bounds : ::mergeAabb( acc, bins[i].bounds ); + cnt += bins[i].count; + leftBounds[i] = acc; + leftCount[i] = cnt; + } + + acc = {}; + cnt = 0; + for ( auto i = numBins - 1; i >= 0; i-- ) { + if ( bins[i].count > 0 ) acc = (cnt == 0) ? bins[i].bounds : ::mergeAabb( acc, bins[i].bounds ); + cnt += bins[i].count; + rightBounds[i] = acc; + rightCount[i] = cnt; + } + + float parentArea = ::aabbSurfaceArea(node.bounds); + for ( auto i = 0; i < numBins - 1; i++ ) { + if ( leftCount[i] == 0 || rightCount[i + 1] == 0 ) continue; + float cost = 1.0f + ( + ( ::aabbSurfaceArea(leftBounds[i]) / parentArea ) * leftCount[i] + + ( ::aabbSurfaceArea(rightBounds[i + 1]) / parentArea ) * rightCount[i + 1] + ); + if ( cost < bestCost ) { + bestCost = cost; + bestAxis = axis; + bestSplit = i; + } + } + } + + // fallback: no valid split → make leaf + if ( bestAxis == -1 ) { + node.count = count; + int index = (int) bvh.nodes.size(); + bvh.nodes.emplace_back(node); + return index; + } + + float minC = node.bounds.min[bestAxis]; + float maxC = node.bounds.max[bestAxis]; + float scale = (float) numBins / (maxC - minC); + + auto midIt = std::partition( bvh.indices.begin() + start, bvh.indices.begin() + end, [&](int idx) { + float c = ::aabbCenter( bounds[idx])[bestAxis ]; + int binID = std::min(numBins - 1, (int)((c - minC) * scale)); + return binID <= bestSplit; + }); + + int mid = (int) ( midIt - bvh.indices.begin() ); + + // if partition failed (all left or all right), force leaf + if ( mid == start || mid == end ) { + node.count = count; + int index = (int) bvh.nodes.size(); + bvh.nodes.emplace_back(node); + return index; + } + + int index = (int) bvh.nodes.size(); + bvh.nodes.emplace_back(node); + + node.left = ::buildBVHNode_SAH( bvh, bounds, start, mid, capacity ); + node.right = ::buildBVHNode_SAH( bvh, bounds, mid, end, capacity ); + bvh.nodes[index] = node; + return index; + } + void buildBroadphaseBVH( pod::BVH& bvh, const uf::stl::vector& bodies, int capacity = 2 ) { bvh.indices.clear(); bvh.nodes.clear(); @@ -138,7 +177,13 @@ namespace { } // recursively build BVH from indices - ::buildBVHNode( bvh, bounds, 0, bodies.size(), capacity ); + if ( ::useBvhSahBodies ) ::buildBVHNode_SAH( bvh, bounds, 0, bodies.size(), capacity ); + else ::buildBVHNode( bvh, bounds, 0, bodies.size(), capacity ); + // flatten if requested + if ( ::flattenBvhBodies ) ::flattenBVH( bvh, 0 ); + + // mark as clean + bvh.dirty = false; } void buildMeshBVH( pod::BVH& bvh, const uf::Mesh& mesh, int capacity = 4 ) { @@ -165,62 +210,348 @@ namespace { auto aabb = ::computeTriangleAABB( positions.data(view.vertex.first), positions.stride(), indices.data(view.index.first), mesh.index.size, triIndexID ); auto triID = triIndexID + (view.index.first / 3); - bounds.emplace_back(aabb); - bvh.indices.emplace_back(triID); // triID => mesh.index.buffer[triID * 3]; + if ( triID != bounds.size() ) UF_MSG_DEBUG("triID={}, bounds.size()={}", triID, bounds.size()); + + bounds.emplace_back( aabb ); + bvh.indices.emplace_back( triID ); // triID => mesh.index.buffer[triID * 3]; + } + } + + // recursively build BVH from indices + if ( ::useBvhSahMeshes ) ::buildBVHNode_SAH( bvh, bounds, 0, triangles, capacity ); + else ::buildBVHNode( bvh, bounds, 0, triangles, capacity ); + // flatten if requested + if ( ::flattenBvhMeshes ) ::flattenBVH( bvh, 0 ); + + // mark as clean + bvh.dirty = false; + } +} + +namespace { + pod::BVH::UpdatePolicy::Decision decideBVHUpdate( const pod::BVH& bvh, const uf::stl::vector& bodies, const pod::BVH::UpdatePolicy& policy, int frameCounter ) { + if ( bvh.indices.empty() || bvh.nodes.empty() || bvh.dirty ) return pod::BVH::UpdatePolicy::Decision::REBUILD; + if ( bodies.empty() ) return pod::BVH::UpdatePolicy::Decision::NONE; + + int dirtyCount = 0; + float oldRootArea = ::aabbSurfaceArea( bvh.nodes[0].bounds ); + + // check each body + for ( const auto* body : bodies ) { + pod::AABB newBounds = ::computeAABB(*body); + pod::AABB oldBounds = body->bounds; + + // compute displacement relative to size + pod::Vector3f oldCenter = ( oldBounds.min + oldBounds.max ) * 0.5f; + pod::Vector3f newCenter = ( newBounds.min + newBounds.max ) * 0.5f; + float displacement = uf::vector::distance( newCenter, oldCenter ); + + pod::Vector3f extent = oldBounds.max - oldBounds.min; + float size = std::max({extent.x, extent.y, extent.z, 1e-6f}); + + if ( displacement > policy.displacementThreshold * size ) ++dirtyCount; + } + + float dirtyRatio = (float) dirtyCount / (float) bodies.size(); + + // compute new root bounds + pod::AABB newRoot = bodies[0]->bounds; + for ( auto i = 1; i < bodies.size(); ++i ) { + newRoot = ::mergeAabb(newRoot, bodies[i]->bounds); + } + + float newRootArea = ::aabbSurfaceArea( newRoot ); + if ( dirtyRatio > policy.dirtyRatioThreshold || newRootArea > oldRootArea * policy.overlapThreshold || frameCounter % policy.maxFramesBeforeRebuild == 0 ) { + return pod::BVH::UpdatePolicy::Decision::REBUILD; + } + if ( dirtyCount > 0 ) return pod::BVH::UpdatePolicy::Decision::REFIT; + return pod::BVH::UpdatePolicy::Decision::NONE; + } +} + +namespace { + void refitBVH( pod::BVH& bvh, const uf::stl::vector& bounds ) { + if ( bvh.nodes.empty() ) return; + + // update leaf bounds + #pragma omp parallel for + for ( auto i = 0; i < bvh.nodes.size(); i++ ) { + auto& node = bvh.nodes[i]; + if ( node.count > 0 ) { + // leaf node: recompute bounds from bodies + node.bounds = bounds[bvh.indices[node.start]]; + + for ( int j = 1; j < node.count; j++ ) { + node.bounds = ::mergeAabb(node.bounds, bounds[bvh.indices[node.start + j]] ); + } + } + } + + // update internal nodes bottom-up + for ( int i = (int) bvh.nodes.size() - 1; i >= 0; i-- ) { + auto& node = bvh.nodes[i]; + // internal node + if ( node.count == 0 ) { + node.bounds = ::mergeAabb(bvh.nodes[node.left].bounds, bvh.nodes[node.right].bounds); + } + } + } + + // avoids creating a vector for bounds + void refitBVH( pod::BVH& bvh, const uf::stl::vector& bodies ) { + if ( bvh.nodes.empty() ) return; + + // update leaf bounds + #pragma omp parallel for + for ( auto i = 0; i < bvh.nodes.size(); i++ ) { + auto& node = bvh.nodes[i]; + if ( node.count > 0 ) { + // leaf node: recompute bounds from bodies + auto nodeID = bvh.indices[node.start]; + + node.bounds = ::computeAABB( *bodies[nodeID] ); + node.asleep = !bodies[nodeID]->activity.awake; + + for ( int j = 1; j < node.count; j++ ) { + auto bodyID = bvh.indices[node.start + j]; + node.bounds = ::mergeAabb(node.bounds, ::computeAABB( *bodies[bodyID] ) ); + node.asleep = node.asleep && !bodies[bodyID]->activity.awake; + } + } + } + + // update internal nodes bottom-up + for ( int i = (int) bvh.nodes.size() - 1; i >= 0; i-- ) { + auto& node = bvh.nodes[i]; + // internal node + if ( node.count == 0 ) { + const auto& leftNode = bvh.nodes[node.left]; + const auto& rightNode = bvh.nodes[node.right]; + node.bounds = ::mergeAabb(leftNode.bounds, rightNode.bounds); + node.asleep = leftNode.asleep && rightNode.asleep; + } + } + } + + void refitBVH( pod::BVH& bvh, const uf::Mesh& mesh ) { + int triangles = mesh.index.count / 3; + + uf::stl::vector bounds; + bounds.reserve( triangles ); + + auto views = mesh.makeViews({"position"}); + UF_ASSERT( !views.empty() ); + + // populate initial indices and bounds + for ( auto& view : views ) { + auto& indices = view["index"]; + auto& positions = view["position"]; + + auto tris = view.index.count / 3; + for ( auto triIndexID = 0; triIndexID < tris; ++triIndexID ) { + auto aabb = ::computeTriangleAABB( positions.data(view.vertex.first), positions.stride(), indices.data(view.index.first), mesh.index.size, triIndexID ); + bounds.emplace_back(aabb); } } - UF_MSG_DEBUG("Built BVH with {} triangles from mesh with {} triangles ({} draw commands)", bounds.size(), triangles, views.size()); // recursively build BVH from indices - ::buildBVHNode( bvh, bounds, 0, triangles, capacity ); + ::refitBVH( bvh, bounds ); + } +} + +namespace { + int flattenBVH( pod::BVH& bvh, int nodeID ) { + if ( nodeID == 0 ) bvh.flattened.reserve(bvh.nodes.size()); + + const auto& node = bvh.nodes[nodeID]; + + int flatID = (int) bvh.flattened.size(); + bvh.flattened.emplace_back(); // placeholder + + pod::BVH::FlatNode flat{}; + flat.bounds = node.bounds; + flat.start = -1; + flat.count = -1; + flat.skipIndex = -1; + flat.asleep = node.asleep; + + // leaf + if ( node.count > 0 ) { + flat.start = node.start; + flat.count = node.count; + flat.skipIndex = flatID + 1; // next node after this leaf + bvh.flattened[flatID] = flat; + return flatID + 1; + } + // internal + else { + flat.start = -1; + flat.count = 0; + + int leftID = ::flattenBVH( bvh, node.left ); + int rightID = ::flattenBVH( bvh, node.right ); + + flat.skipIndex = rightID; // skip entire subtree + bvh.flattened[flatID] = flat; + return rightID; + } + } +} + +namespace { + // collects a list of nodes that are overlapping with each other + void traverseNodePair(const pod::BVH& bvh, int nodeAID, int nodeBID, pod::BVH::pairs_t& pairs) { + const auto& nodeA = bvh.nodes[nodeAID]; + const auto& nodeB = bvh.nodes[nodeBID]; + + if ( nodeA.asleep || nodeB.asleep || !::aabbOverlap( nodeA.bounds, nodeB.bounds ) ) return; + + if ( nodeA.count > 0 && nodeB.count > 0 ) { + for ( auto i = 0; i < nodeA.count; ++i ) { + for ( auto j = 0; j < nodeB.count; ++j ) { + int bodyA = bvh.indices[nodeA.start + i]; + int bodyB = bvh.indices[nodeB.start + j]; + if ( bodyA == bodyB ) continue; + if ( bodyA > bodyB ) std::swap( bodyA, bodyB ); + + pairs.emplace_back(bodyA, bodyB); + } + } + return; + } + + if ( nodeA.count == 0 ) { + ::traverseNodePair( bvh, nodeA.left, nodeBID, pairs ); + ::traverseNodePair( bvh, nodeA.right, nodeBID, pairs ); + } + if ( nodeB.count == 0 ) { + ::traverseNodePair( bvh, nodeAID, nodeB.left, pairs ); + ::traverseNodePair( bvh, nodeAID, nodeB.right, pairs ); + } + } + // collects a list of nodes from each BVH that are overlapping with each other (for mesh v mesh) + void traverseNodePair( const pod::BVH& bvhA, int nodeAID, const pod::BVH& bvhB, int nodeBID, pod::BVH::pairs_t& pairs ) { + const auto& nodeA = bvhA.nodes[nodeAID]; + const auto& nodeB = bvhB.nodes[nodeBID]; + + if ( nodeA.asleep || nodeB.asleep || !::aabbOverlap( nodeA.bounds, nodeB.bounds ) ) return; + + if ( nodeA.count > 0 && nodeB.count > 0 ) { + for ( auto i = 0; i < nodeA.count; ++i ) { + for ( auto j = 0; j < nodeB.count; ++j ) { + int bodyA = bvhA.indices[nodeA.start + i]; + int bodyB = bvhB.indices[nodeB.start + j]; + + pairs.emplace_back(bodyA, bodyB); + } + } + return; + } + + if ( nodeA.count == 0 ) { + ::traverseNodePair( bvhA, nodeA.left, bvhB, nodeBID, pairs ); + ::traverseNodePair( bvhA, nodeA.right, bvhB, nodeBID, pairs ); + } + if ( nodeB.count == 0 ) { + ::traverseNodePair( bvhA, nodeAID, bvhB, nodeB.left, pairs ); + ::traverseNodePair( bvhA, nodeAID, bvhB, nodeB.right, pairs ); + } + } + + void traverseBVH( const pod::BVH& bvh, int nodeID, pod::BVH::pairs_t& pairs ) { + const auto& node = bvh.nodes[nodeID]; + + if ( node.count > 0 ) { + for ( auto i = 0; i < node.count; ++i ) { + for ( auto j = i + 1; j < node.count; ++j ) { + int bodyA = bvh.indices[node.start + i]; + int bodyB = bvh.indices[node.start + j]; + + if ( bodyA == bodyB ) continue; + if ( bodyA > bodyB ) std::swap( bodyA, bodyB ); + + pairs.emplace_back(bodyA, bodyB); + } + } + return; + } + + ::traverseNodePair( bvh, node.left, node.right, pairs ); + } + + void queryOverlaps( const pod::BVH& bvh, pod::BVH::pairs_t& outPairs ) { + if ( !bvh.flattened.empty() ) return ::queryFlatOverlaps( bvh, outPairs ); + + if ( bvh.nodes.empty() ) return; + outPairs.reserve(::reserveCount); + ::traverseBVH( bvh, 0, outPairs ); + } + + void queryOverlaps( const pod::BVH& bvhA, const pod::BVH& bvhB, pod::BVH::pairs_t& outPairs ) { + if ( !bvhA.flattened.empty() && !bvhB.flattened.empty() ) return ::queryFlatOverlaps( bvhA, bvhB, outPairs ); + + if ( bvhA.nodes.empty() || bvhB.nodes.empty() ) return; + outPairs.reserve(::reserveCount); + ::traverseNodePair(bvhA, 0, bvhB, 0, outPairs); } } namespace { // query a BVH with an AABB via a stack - 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& outIndices ) { + if ( !bvh.flattened.empty() ) return ::queryFlatBVH( bvh, bounds, outIndices ); + + outIndices.reserve(::reserveCount); + if ( bvh.nodes.empty() ) return; - uf::stl::stack stack; + uf::stl::stack stack; stack.push(0); while ( !stack.empty() ) { int idx = stack.top(); stack.pop(); auto& node = bvh.nodes[idx]; - if ( !::aabbOverlap( bounds, node.bounds ) ) continue; + if ( node.asleep || !::aabbOverlap( bounds, node.bounds ) ) continue; if ( node.count > 0 ) { - for ( auto i = 0; i < node.count; ++i) indices.emplace_back(bvh.indices[node.start + i]); + for ( auto i = 0; i < node.count; ++i) outIndices.emplace_back(bvh.indices[node.start + i]); } else { stack.push(node.left); stack.push(node.right); } } } - void queryBVH( const pod::BVH& bvh, const pod::PhysicsBody& body, uf::stl::vector& indices ) { - return ::queryBVH( bvh, body.bounds, indices ); + void queryBVH( const pod::BVH& bvh, const pod::PhysicsBody& body, uf::stl::vector& outIndices ) { + return ::queryBVH( bvh, body.bounds, outIndices ); } // query a BVH with an AABB via recursion - void queryBVH( const pod::BVH& bvh, const pod::AABB& bounds, uf::stl::vector& indices, int nodeID ) { + void queryBVH( const pod::BVH& bvh, const pod::AABB& bounds, uf::stl::vector& outIndices, int nodeID ) { + if ( !bvh.flattened.empty() ) return ::queryFlatBVH( bvh, bounds, outIndices ); + + if ( nodeID == 0 ) outIndices.reserve(::reserveCount); + const auto& node = bvh.nodes[nodeID]; - if ( !::aabbOverlap( node.bounds, bounds ) ) return; + if ( node.asleep || !::aabbOverlap( node.bounds, bounds ) ) return; if ( node.count > 0 ) { - for ( auto i = 0; i < node.count; ++i ) indices.emplace_back(bvh.indices[node.start + i]); + for ( auto i = 0; i < node.count; ++i ) outIndices.emplace_back(bvh.indices[node.start + i]); return; } // recurse - ::queryBVH( bvh, bounds, indices, node.left ); - ::queryBVH( bvh, bounds, indices, node.right ); + ::queryBVH( bvh, bounds, outIndices, node.left ); + ::queryBVH( bvh, bounds, outIndices, node.right ); } // query a BVH with a ray via a stack - void queryBVH( const pod::BVH& bvh, const pod::Ray& ray, uf::stl::vector& indices, float maxDist ) { - if ( bvh.nodes.empty() ) return; + void queryBVH( const pod::BVH& bvh, const pod::Ray& ray, uf::stl::vector& outIndices, float maxDist ) { + if ( !bvh.flattened.empty() ) return ::queryFlatBVH( bvh, ray, outIndices, maxDist ); - uf::stl::stack stack; + if ( bvh.nodes.empty() ) return; + outIndices.reserve(::reserveCount); + + uf::stl::stack stack; stack.push(0); while ( !stack.empty() ) { @@ -228,11 +559,11 @@ namespace { const auto& node = bvh.nodes[idx]; float tMin, tMax; - if ( !::rayAabbIntersect( ray, node.bounds, tMin, tMax ) ) continue; + if ( node.asleep || !::rayAabbIntersect( ray, node.bounds, tMin, tMax ) ) continue; if ( tMin > maxDist ) continue; if ( node.count > 0 ) { - for ( auto i = 0; i < node.count; ++i) indices.emplace_back(bvh.indices[node.start + i]); + for ( auto i = 0; i < node.count; ++i) outIndices.emplace_back(bvh.indices[node.start + i]); } else { stack.push(node.left); stack.push(node.right); @@ -240,19 +571,212 @@ namespace { } } // query a BVH with a ray via recursion - void queryBVH( const pod::BVH& bvh, const pod::Ray& ray, uf::stl::vector& indices, int nodeID, float maxDist ) { + void queryBVH( const pod::BVH& bvh, const pod::Ray& ray, uf::stl::vector& outIndices, int nodeID, float maxDist ) { + if ( !bvh.flattened.empty() ) return ::queryFlatBVH( bvh, ray, outIndices, maxDist ); + + if ( nodeID == 0 ) outIndices.reserve(::reserveCount); + const auto& node = bvh.nodes[nodeID]; float tMin, tMax; - if ( !::rayAabbIntersect( ray, node.bounds, tMin, tMax ) ) return; + if ( node.asleep || !::rayAabbIntersect( ray, node.bounds, tMin, tMax ) ) return; if ( tMin > maxDist ) return; if ( node.count > 0 ) { - for ( auto i = 0; i < node.count; ++i ) indices.emplace_back(bvh.indices[node.start + i]); + for ( auto i = 0; i < node.count; ++i ) outIndices.emplace_back(bvh.indices[node.start + i]); return; } // recurse - ::queryBVH( bvh, ray, indices, node.left, maxDist ); - ::queryBVH( bvh, ray, indices, node.right, maxDist ); + ::queryBVH( bvh, ray, outIndices, node.left, maxDist ); + ::queryBVH( bvh, ray, outIndices, node.right, maxDist ); } } + + +namespace { + void queryFlatOverlaps( const pod::BVH& bvh, pod::BVH::pairs_t& outPairs ) { + auto& nodes = bvh.flattened; + auto& indices = bvh.indices; + + outPairs.clear(); + outPairs.reserve(::reserveCount); + + for ( auto i = 0; i < (int) nodes.size(); ++i ) { + const auto& nodeA = nodes[i]; + if ( nodeA.count <= 0 || nodeA.asleep ) continue; + + for ( auto j = i + 1; j < (int) nodes.size(); ++j ) { + const auto& nodeB = nodes[j]; + if ( nodeB.count <= 0 || nodeB.asleep ) continue; + + if ( !::aabbOverlap( nodeA.bounds, nodeB.bounds ) ) continue; + + for ( auto ia = 0; ia < nodeA.count; ++ia ) { + for ( auto ib = 0; ib < nodeB.count; ++ib ) { + auto indexA = indices[nodeA.start + ia]; + auto indexB = indices[nodeB.start + ib]; + + if ( indexA == indexB ) continue; + if ( indexA > indexB ) std::swap( indexA, indexB ); + + outPairs.emplace_back( indexA, indexB ); + } + } + } + } + } + void queryFlatOverlaps( const pod::BVH& bvhA, const pod::BVH& bvhB, pod::BVH::pairs_t& outPairs ) { + auto& nodesA = bvhA.flattened; + auto& indicesA = bvhA.indices; + + auto& nodesB = bvhB.flattened; + auto& indicesB = bvhB.indices; + + if ( nodesA.empty() || nodesB.empty() ) return; + + outPairs.clear(); + outPairs.reserve(::reserveCount); + + for ( auto i = 0; i < (int) nodesA.size(); ++i ) { + const auto& nodeA = nodesA[i]; + if ( nodeA.count <= 0 || nodeA.asleep ) continue; + + for ( auto j = 0; j < (int) nodesB.size(); ++j ) { + const auto& nodeB = nodesB[j]; + if ( nodeB.count <= 0 || nodeB.asleep ) continue; + + if ( !::aabbOverlap( nodeA.bounds, nodeB.bounds ) ) continue; + + for ( auto ia = 0; ia < nodeA.count; ++ia ) { + for (auto ib = 0; ib < nodeB.count; ++ib ) { + auto indexA = indicesA[nodeA.start + ia]; + auto indexB = indicesB[nodeB.start + ib]; + + outPairs.emplace_back( indexA, indexB ); + } + } + } + } + } + + void queryFlatBVH( const pod::BVH& bvh, const pod::AABB& bounds, uf::stl::vector& outIndices ) { + auto& nodes = bvh.flattened; + auto& indices = bvh.indices; + + outIndices.reserve(::reserveCount); + + int idx = 0; + while ( idx < (int) nodes.size() ) { + const auto& node = nodes[idx]; + + if ( !node.asleep && ::aabbOverlap( bounds, node.bounds ) ) { + // leaf + if ( node.count > 0 ) { + for ( int i = 0; i < node.count; ++i ) { + outIndices.emplace_back( indices[node.start + i] ); + } + } + ++idx; + } else { + // skip this subtree + idx = node.skipIndex; + } + } + } + void queryFlatBVH( const pod::BVH& bvh, const pod::Ray& ray, uf::stl::vector& outIndices, float maxDist ) { + auto& nodes = bvh.flattened; + auto& indices = bvh.indices; + + outIndices.reserve(::reserveCount); + + int idx = 0; + while ( idx < (int) nodes.size() ) { + const auto& node = nodes[idx]; + float tMin, tMax; + if ( !node.asleep && ::rayAabbIntersect( ray, node.bounds, tMin, tMax ) && tMin <= maxDist ) { + // leaf + if ( node.count > 0 ) { + for ( int i = 0; i < node.count; ++i ) { + outIndices.emplace_back( indices[node.start + i] ); + } + } + ++idx; + } else { + // skip this subtree + idx = node.skipIndex; + } + } + } +} + +namespace { + void buildIslands( const pod::BVH::pairs_t& pairs, const uf::stl::vector& bodies, uf::stl::vector& islands ) { + islands.reserve(::reserveCount); + + int n = (int) bodies.size(); + uf::stl::vector visited(n, -1); + + for ( auto i = 0; i < n; i++ ) { + if ( visited[i] != -1 ) continue; + + // new island + pod::Island island = {}; + uf::stl::stack stack; + stack.push(i); + + while ( !stack.empty() ) { + int idx = stack.top(); stack.pop(); + if ( visited[idx] != -1 ) continue; + visited[idx] = (int) islands.size(); + + island.bodies.emplace_back( bodies[idx] ); + + // traverse neighbors + for ( auto& [a, b] : pairs ) { + int neighbor = -1; + if ( a == idx ) neighbor = b; + else if ( b == idx ) neighbor = a; + if ( neighbor != -1 && visited[neighbor] == -1 ) { + stack.push(neighbor); + } + } + } + + islands.emplace_back( std::move( island ) ); + } + } + + void updateIsland( pod::Island& island, float dt ) { + bool allStill = true; + + for ( auto* b : island.bodies ) { + auto& body = *b; + if ( !body.activity.awake ) continue; + + float linSpeed = uf::vector::norm( body.velocity ); + float angSpeed = uf::vector::norm( body.angularVelocity ); + + if ( linSpeed < pod::Activity::linearSleepEpsilon && angSpeed < pod::Activity::angularSleepEpsilon) { + body.activity.sleepTimer += dt; + } else { + body.activity.sleepTimer = 0.0f; + allStill = false; + } + + if ( body.activity.sleepTimer < pod::Activity::sleepThreshold ) { + allStill = false; + } + } + + // put entire island to sleep + if ( allStill ) { + island.awake = false; + for ( auto* b : island.bodies ) ::sleepBody( *b ); + } + // at least one body is awake + else { + for ( auto* b : island.bodies ) ::wakeBody( *b ); + island.awake = true; + } + } +} \ No newline at end of file diff --git a/engine/src/utils/math/physics/epa.inl b/engine/src/utils/math/physics/epa.inl index 98a58564..7b14346e 100644 --- a/engine/src/utils/math/physics/epa.inl +++ b/engine/src/utils/math/physics/epa.inl @@ -53,6 +53,9 @@ namespace { void expandPolytope( uf::stl::vector& faces, const pod::SupportPoint& p ) { uf::stl::vector remove; uf::stl::vector> borders; + + remove.reserve( faces.size() ); + borders.reserve( faces.size() ); // find faces visible to point for ( auto i = 0; i < faces.size(); ++i ) { diff --git a/engine/src/utils/math/physics/gjk.inl b/engine/src/utils/math/physics/gjk.inl index 8f65a5f8..e47f6460 100644 --- a/engine/src/utils/math/physics/gjk.inl +++ b/engine/src/utils/math/physics/gjk.inl @@ -21,13 +21,13 @@ namespace { } 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 ); + 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]; + if ( d0 > d1 && d0 > d2 ) return tri.points[0]; + if ( d1 > d2 ) return tri.points[1]; + return tri.points[2]; } break; default: { diff --git a/engine/src/utils/math/physics/helpers.inl b/engine/src/utils/math/physics/helpers.inl index 6d9d9ce8..5378b1d8 100644 --- a/engine/src/utils/math/physics/helpers.inl +++ b/engine/src/utils/math/physics/helpers.inl @@ -36,19 +36,32 @@ namespace { bool trianglePlane( const pod::TriangleWithNormal& tri, const pod::PhysicsBody& body, pod::Manifold& manifold, float eps = EPS(1.0e-6f) ); 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 ); - 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 ); + int flattenBVH( pod::BVH& bvh, int 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 ); - 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, int nodeID ); - void queryBVH( const pod::BVH& bvh, const pod::Ray& ray, uf::stl::vector& indices, int nodeID, float maxDist = FLT_MAX ); - 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::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 { @@ -60,6 +73,40 @@ namespace { return (idA << 32) ^ idB; } + void deduplicatePairs( pod::BVH::pairs_t& pairs ) { + // should already be swapped + for (auto& [a, b] : pairs) if (a > b) std::swap(a, b); + std::sort(pairs.begin(), pairs.end()); + pairs.erase(std::unique(pairs.begin(), pairs.end()), pairs.end()); + } + + // marks a body as asleep + void wakeBody( pod::PhysicsBody& body ) { + body.activity.awake = true; + body.activity.sleepTimer = 0.0f; + } + void sleepBody( pod::PhysicsBody& body ) { + body.activity.awake = false; + body.velocity = pod::Vector3f{}; + body.angularVelocity = pod::Vector3f{}; + } + void updateActivity( pod::PhysicsBody& body, float dt ) { + // already asleep + if ( !body.activity.awake ) return; + + // check if body is moving + float linSpeed = uf::vector::norm( body.velocity ); + float angSpeed = uf::vector::norm( body.angularVelocity ); + + // body is nearly still + if ( linSpeed < pod::Activity::linearSleepEpsilon && angSpeed < pod::Activity::angularSleepEpsilon ) { + body.activity.sleepTimer += dt; + if ( body.activity.sleepTimer > pod::Activity::sleepThreshold ) ::sleepBody( body ); + } + // body is moving, reset timer + else ::wakeBody( body ); + } + // returns an absolute transform while also allowing offsetting the collision body // to-do: find a succint way to explain this madness pod::Transform<> getTransform( const pod::PhysicsBody& body ) { @@ -78,6 +125,7 @@ namespace { return ( a.category & b.mask ) && ( b.category & a.mask ); } bool shouldCollide( const pod::PhysicsBody& a, const pod::PhysicsBody& b ) { + // if ( a.isStatic && b.isStatic ) return false; return ::shouldCollide( a.collider, b.collider ); } @@ -240,7 +288,7 @@ namespace { } pod::Vector3f closestPointOnTriangle( const pod::Vector3f& p, const pod::Vector3f& a, const pod::Vector3f& b, const pod::Vector3f& c ) { - // Check if P in vertex region outside A + // check if P in vertex region outside A pod::Vector3f ab = b - a; pod::Vector3f ac = c - a; pod::Vector3f ap = p - a; @@ -248,40 +296,40 @@ namespace { float d2 = uf::vector::dot(ac, ap); if (d1 <= 0 && d2 <= 0) return a; - // Check if P in vertex region outside B + // check if P in vertex region outside B pod::Vector3f bp = p - b; float d3 = uf::vector::dot(ab, bp); float d4 = uf::vector::dot(ac, bp); if (d3 >= 0 && d4 <= d3) return b; - // Check if P in edge region of AB, if so return projection on AB + // check if P in edge region of AB, if so return projection on AB float vc = d1 * d4 - d3 * d2; if (vc <= 0 && d1 >= 0 && d3 <= 0) { float v = d1 / (d1 - d3); return a + ab * v; } - // Check vertex region outside C + // check vertex region outside C pod::Vector3f cp = p - c; float d5 = uf::vector::dot(ab, cp); float d6 = uf::vector::dot(ac, cp); if (d6 >= 0 && d5 <= d6) return c; - // Check edge region of AC + // check edge region of AC float vb = d5 * d2 - d1 * d6; if (vb <= 0 && d2 >= 0 && d6 <= 0) { float w = d2 / (d2 - d6); return a + ac * w; } - // Check edge region of BC + // check edge region of BC float va = d3 * d6 - d5 * d4; if (va <= 0 && (d4 - d3) >= 0 && (d5 - d6) >= 0) { float w = (d4 - d3) / ((d4 - d3) + (d5 - d6)); return b + (c - b) * w; } - // P inside face region. Return projection onto face + // p inside face region. Return projection onto face float denom = 1.0f / (va + vb + vc); float v = vb * denom; float w = vc * denom; diff --git a/engine/src/utils/math/physics/impl.cpp b/engine/src/utils/math/physics/impl.cpp index be2594b5..a5cacb92 100644 --- a/engine/src/utils/math/physics/impl.cpp +++ b/engine/src/utils/math/physics/impl.cpp @@ -6,20 +6,39 @@ namespace { bool warmupSolver = true; - bool blockContactSolver = false; // blockNxN solver is flawed + bool blockContactSolver = true; bool psgContactSolver = true; // iterative solver is flawed bool useGjk = false; // currently don't have a way to broadphase mesh => narrowphase tri via GJK bool fixedStep = true; - int substeps = 4; + + int substeps = 0; + int reserveCount = 32; // increasing these make things lag int broadphaseBvhCapacity = 1; int meshBvhCapacity = 1; - int solverIterations = 5; - float baumgarteCorrectionPercent = 0.005f; // needs to be very small or the correction is too large - float baumgarteCorrectionSlop = 0.001f; + bool flattenBvhBodies = true; + bool flattenBvhMeshes = true; + + // it actually seems slower to use these...... + bool useBvhSahBodies = false; + bool useBvhSahMeshes = false; + + int solverIterations = 10; + float baumgarteCorrectionPercent = 0.2f; + float baumgarteCorrectionSlop = 0.01f; + uf::stl::unordered_map manifoldsCache; + int manifoldCacheLifetime = 6; + + uint32_t frameCounter = 0; + pod::BVH::UpdatePolicy bvhUpdatePolicy = { + .displacementThreshold = 0.25f, + .overlapThreshold = 2.0f, + .dirtyRatioThreshold = 0.3f, + .maxFramesBeforeRebuild = 3600, + }; } #define EPS(x) x // 1.0e-6f @@ -103,17 +122,38 @@ void uf::physics::impl::step( pod::World& world, float dt ) { if ( bodies.empty() ) return; for ( auto* body : bodies ) { + if ( !body->activity.awake ) continue; ::integrate( *body, dt ); } - - uf::stl::vector manifolds; - uf::stl::vector> pairs; - // create BVH - ::buildBroadphaseBVH( bvh, bodies, ::broadphaseBvhCapacity ); + switch ( ::decideBVHUpdate( bvh, bodies, ::bvhUpdatePolicy, ::frameCounter++ ) ) { + case pod::BVH::UpdatePolicy::Decision::REBUILD: { + ::buildBroadphaseBVH( bvh, bodies, ::broadphaseBvhCapacity ); // (re)build + } break; + case pod::BVH::UpdatePolicy::Decision::REFIT: { + ::refitBVH( bvh, bodies ); // refit + } break; + case pod::BVH::UpdatePolicy::Decision::NONE: + default: + // no-op + break; + } + // query for overlaps + pod::BVH::pairs_t pairs; ::queryOverlaps( bvh, pairs ); + ::deduplicatePairs( pairs ); + + // build islands + uf::stl::vector islands; + ::buildIslands( pairs, world.bodies, islands ); + + // update sleep state per island + for ( auto& island : islands ) ::updateIsland( island, dt ); + // iterate overlaps + uf::stl::vector manifolds; + manifolds.reserve(::reserveCount); for ( auto& [ia, ib] : pairs ) { auto& a = *bodies[ia]; auto& b = *bodies[ib]; @@ -138,7 +178,9 @@ void uf::physics::impl::step( pod::World& world, float dt ) { ::reduceContacts( manifold ); // no points remained, skip if ( manifold.points.empty() ) continue; - + // wake up bodies + if ( a.activity.awake && !b.activity.awake ) ::wakeBody( b ); + if ( b.activity.awake && !a.activity.awake ) ::wakeBody( a ); // store manifold manifolds.emplace_back(manifold); } @@ -146,24 +188,10 @@ void uf::physics::impl::step( pod::World& world, float dt ) { // pass manifolds to solver ::solveContacts( manifolds, dt ); + // do position correction + ::solvePositions( manifolds, dt ); - if ( ::warmupSolver ) { - // update cache - for ( auto& manifold : manifolds ) { - ::manifoldsCache[::makePairKey( *manifold.a, *manifold.b )] = manifold; - } - - // prune if too old / empty - for ( auto& [ key, manifold ] : manifoldsCache ) { - // prune manifolds that are X frames old - for ( auto it = manifold.points.begin(); it != manifold.points.end(); ) { - if ( it->lifetime > 3 ) manifold.points.erase(it); - else ++it; - } - // empty manifold, kill it - if ( manifold.points.empty() ) manifoldsCache.erase(key); - } - } + if ( ::warmupSolver ) ::storeManifolds( manifolds, ::manifoldsCache ); // recompute bounds for further queries for ( auto* body : bodies ) { @@ -255,11 +283,11 @@ void uf::physics::impl::updateInertia( pod::PhysicsBody& body ) { } } void uf::physics::impl::applyForce( pod::PhysicsBody& body, const pod::Vector3f& force ) { - if ( body.isStatic ) return; + if ( body.isStatic ) return; ::wakeBody( body ); body.forceAccumulator += force; } void uf::physics::impl::applyForceAtPoint( pod::PhysicsBody body, const pod::Vector3f& force, const pod::Vector3f& point ) { - if ( body.isStatic ) return; + if ( body.isStatic ) return; ::wakeBody( body ); // linear force body.forceAccumulator += force; // angular force @@ -267,21 +295,23 @@ void uf::physics::impl::applyForceAtPoint( pod::PhysicsBody body, const pod::Vec body.torqueAccumulator += uf::vector::cross( r, force ); } void uf::physics::impl::applyImpulse( pod::PhysicsBody& body, const pod::Vector3f& impulse ) { - if ( body.isStatic ) return; + if ( body.isStatic ) return; ::wakeBody( body ); body.velocity += impulse * body.inverseMass; } void uf::physics::impl::applyTorque( pod::PhysicsBody& body, const pod::Vector3f& torque ) { - if ( body.isStatic ) return; + if ( body.isStatic ) return; ::wakeBody( body ); body.torqueAccumulator += torque; } void uf::physics::impl::setVelocity( pod::PhysicsBody& body, const pod::Vector3f& v ) { + ::wakeBody( body ); body.velocity = v; } void uf::physics::impl::applyRotation( pod::PhysicsBody& body, const pod::Quaternion<>& q ) { + ::wakeBody( body ); uf::transform::rotate( *body.transform/*.reference*/, q ); } void uf::physics::impl::applyRotation( pod::PhysicsBody& body, const pod::Vector3f& axis, float angle ) { - applyRotation( body, uf::quaternion::axisAngle( axis, angle ) ); + uf::physics::impl::applyRotation( body, uf::quaternion::axisAngle( axis, angle ) ); } // body creation @@ -300,10 +330,13 @@ pod::PhysicsBody& uf::physics::impl::create( pod::World& world, uf::Object& obje if ( body.isStatic ) { uf::physics::impl::setColliderCategory(body, "STATIC"); uf::physics::impl::setColliderMask(body, "STATIC"); + } else { + uf::physics::impl::setColliderCategory(body, "DYNAMIC"); + uf::physics::impl::setColliderMask(body, "DYNAMIC"); } - // insert into world - world.bodies.emplace_back(&body); + world.bodies.emplace_back(&body); // insert into world + world.bvh.dirty = true; // mark as dirty return body; } @@ -351,9 +384,10 @@ pod::PhysicsBody& uf::physics::impl::create( pod::World& world, uf::Object& obje auto& body = uf::physics::impl::create( world, object, mass, offset ); body.collider.type = pod::ShapeType::MESH; body.collider.u.mesh.mesh = &mesh; - body.collider.u.mesh.bvh = new pod::BVH; - ::buildMeshBVH( *body.collider.u.mesh.bvh, mesh, ::meshBvhCapacity ); + body.collider.u.mesh.bvh = new pod::BVH; + auto& bvh = *body.collider.u.mesh.bvh; + ::buildMeshBVH( bvh, mesh, ::meshBvhCapacity ); body.bounds = ::computeAABB( body ); uf::physics::impl::updateInertia( body ); @@ -428,7 +462,7 @@ pod::RayQuery uf::physics::impl::rayCast( const pod::Ray& ray, const pod::World& auto& bvh = world.bvh; auto& bodies = world.bodies; - uf::stl::vector candidates; + uf::stl::vector candidates; ::queryBVH( bvh, ray, candidates ); for ( auto i : candidates ) { diff --git a/engine/src/utils/math/physics/integration.inl b/engine/src/utils/math/physics/integration.inl index bc44bb05..bd9777d0 100644 --- a/engine/src/utils/math/physics/integration.inl +++ b/engine/src/utils/math/physics/integration.inl @@ -114,33 +114,38 @@ namespace { if ( manifold.points.size() <= 4 ) return; uf::stl::vector result; + result.reserve(4); + for ( auto& c : manifold.points ) { - // prune invalid contacts - if ( !uf::vector::isValid( c.point ) ) continue; + if ( !uf::vector::isValid(c.point) ) continue; bool merged = false; for ( auto& r : result ) { - if ( !::similarContact( c, r ) ) continue; - // merge, pick deeper penetration + if ( !::similarContact(c, r) ) continue; if ( c.penetration > r.penetration ) r = c; merged = true; break; } - if ( !merged ) result.emplace_back(c); + if ( !merged ) { + if ( result.size() < 4 ) { + result.emplace_back(c); + } else { + // Replace weakest if this one is stronger + int weakest = 0; + for ( auto i = 1; i < 4; i++ ) { + if ( result[i].penetration < result[weakest].penetration ) weakest = i; + } + if ( c.penetration > result[weakest].penetration ) result[weakest] = c; + } + } } - - // UF_MSG_DEBUG("Reduced {} => {} contacts", manifold.points.size(), result.size()); - - // keep only deepest + farthest up to 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 = result; } void mergeContacts( pod::Manifold& manifold ) { uf::stl::vector result; - + result.reserve(4); for ( auto& c : manifold.points ) { bool merged = false; for ( auto& r : result ) { @@ -172,6 +177,28 @@ namespace { } } + void storeManifolds( uf::stl::vector& manifolds, uf::stl::unordered_map& manifoldsCache ){ + // update cache + for ( auto& manifold : manifolds ) { + manifoldsCache[::makePairKey( *manifold.a, *manifold.b )] = manifold; + } + + // prune if too old / empty + for ( auto itCache = manifoldsCache.begin(); itCache != manifoldsCache.end(); ) { + auto& manifold = itCache->second; + + // prune manifolds that are X frames old + for ( auto it = manifold.points.begin(); it != manifold.points.end(); ) { + if ( it->lifetime > ::manifoldCacheLifetime ) it = manifold.points.erase(it); + else ++it; + } + + // empty manifold, kill it + if ( manifold.points.empty() ) itCache = manifoldsCache.erase(itCache); + else ++itCache; + } + } + void warmupContacts( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Contact& c, float dt ) { if ( !c.lifetime ) return; // too new @@ -198,12 +225,22 @@ namespace { // baumgarte position correction void positionCorrection( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Contact& contact ) { if ( ::baumgarteCorrectionPercent <= 0 ) return; + if ( a.isStatic && b.isStatic ) return; - float correctionMagnitude = std::max(contact.penetration - ::baumgarteCorrectionSlop, 0.0f) / (a.inverseMass + b.inverseMass) * ::baumgarteCorrectionPercent; - pod::Vector3f correction = contact.normal * correctionMagnitude; + // penetration depth beyond slop + float penetration = std::max( contact.penetration - ::baumgarteCorrectionSlop, 0.0f ); + if ( penetration <= 0.0f ) return; - if ( !a.isStatic ) a.transform/*.reference*/->position -= correction * a.inverseMass; - if ( !b.isStatic ) b.transform/*.reference*/->position += correction * b.inverseMass; + // compute correction magnitude + float invMassA = ( a.isStatic ? 0.0f : a.inverseMass ); + float invMassB = ( b.isStatic ? 0.0f : b.inverseMass ); + float totalInvMass = invMassA + invMassB; + if ( totalInvMass <= EPS(1e-8f) ) return; + + // apply correction vector + pod::Vector3f correction = contact.normal * (penetration / totalInvMass) * ::baumgarteCorrectionPercent; + if ( !a.isStatic ) a.transform->position -= correction * invMassA; + if ( !b.isStatic ) b.transform->position += correction * invMassB; } void integrate( pod::PhysicsBody& body, float dt ) { @@ -236,6 +273,9 @@ namespace { // apply rolling resistance - ::applyRollingResistance(body, dt); + ::applyRollingResistance( body, dt ); + + // update activity state + ::updateActivity( body, dt ); } } \ No newline at end of file diff --git a/engine/src/utils/math/physics/mesh.inl b/engine/src/utils/math/physics/mesh.inl index b76a1ea3..0b8edfa6 100644 --- a/engine/src/utils/math/physics/mesh.inl +++ b/engine/src/utils/math/physics/mesh.inl @@ -19,12 +19,12 @@ namespace { const auto& mesh = a; const auto& aabb = b; - const auto& meshData = *mesh.collider.u.mesh.mesh; const auto& bvh = *mesh.collider.u.mesh.bvh; + const auto& meshData = *mesh.collider.u.mesh.mesh; // transform to local space for BVH query auto bounds = ::transformAabbToLocal( aabb.bounds, ::getTransform( mesh ) ); - uf::stl::vector candidates; + uf::stl::vector candidates; ::queryBVH( bvh, bounds, candidates ); bool hit = false; @@ -42,12 +42,12 @@ namespace { const auto& mesh = a; const auto& sphere = b; - const auto& meshData = *mesh.collider.u.mesh.mesh; const auto& bvh = *mesh.collider.u.mesh.bvh; + const auto& meshData = *mesh.collider.u.mesh.mesh; // transform to local space for BVH query auto bounds = ::transformAabbToLocal( sphere.bounds, ::getTransform( mesh ) ); - uf::stl::vector candidates; + uf::stl::vector candidates; ::queryBVH( bvh, bounds, candidates ); bool hit = false; @@ -67,12 +67,12 @@ namespace { const auto& mesh = a; const auto& plane = b; - const auto& meshData = *mesh.collider.u.mesh.mesh; const auto& bvh = *mesh.collider.u.mesh.bvh; + const auto& meshData = *mesh.collider.u.mesh.mesh; // transform to local space for BVH query auto bounds = ::transformAabbToLocal( plane.bounds, ::getTransform( mesh ) ); - uf::stl::vector candidates; + uf::stl::vector candidates; ::queryBVH( bvh, bounds, candidates ); bool hit = false; @@ -91,12 +91,12 @@ namespace { const auto& mesh = a; const auto& capsule = b; - const auto& meshData = *mesh.collider.u.mesh.mesh; const auto& bvh = *mesh.collider.u.mesh.bvh; + const auto& meshData = *mesh.collider.u.mesh.mesh; // transform to local space for BVH query auto bounds = ::transformAabbToLocal( capsule.bounds, ::getTransform( mesh ) ); - uf::stl::vector candidates; + uf::stl::vector candidates; ::queryBVH( bvh, bounds, candidates ); bool hit = false; @@ -120,8 +120,9 @@ namespace { 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); + pod::BVH::pairs_t pairs; + ::queryOverlaps( bvhA, bvhB, pairs ); + ::deduplicatePairs( pairs ); bool hit = false; // do collision per triangle diff --git a/engine/src/utils/math/physics/ray.inl b/engine/src/utils/math/physics/ray.inl index 12590527..b141b773 100644 --- a/engine/src/utils/math/physics/ray.inl +++ b/engine/src/utils/math/physics/ray.inl @@ -193,8 +193,8 @@ namespace { } bool rayMesh( const pod::Ray& r, const pod::PhysicsBody& body, pod::RayQuery& rayHit ) { - const uf::Mesh& meshData = *body.collider.u.mesh.mesh; - const pod::BVH& bvh = *body.collider.u.mesh.bvh; + const auto& meshData = *body.collider.u.mesh.mesh; + const auto& bvh = *body.collider.u.mesh.bvh; const auto transform = ::getTransform( body ); @@ -202,10 +202,10 @@ namespace { ray.origin = uf::transform::applyInverse( transform, r.origin ); ray.direction = uf::quaternion::rotate( uf::quaternion::inverse( transform.orientation ), r.direction ); - uf::stl::vector indices; - ::queryBVH( bvh, ray, indices ); + uf::stl::vector candidates; + ::queryBVH( bvh, ray, candidates ); - for ( auto triID : indices ) { + for ( auto triID : candidates ) { auto tri = ::fetchTriangle( meshData, triID ); float t, u, v; @@ -213,8 +213,7 @@ namespace { if ( t >= rayHit.contact.penetration ) continue; auto l = ray.origin + ray.direction * t; - auto bary = ::computeBarycentric( l, tri ); - auto n = uf::vector::normalize( ::interpolateWithBarycentric( bary, tri.normals ) ); + auto n = ::triangleNormal( tri ); // push back to world auto p = uf::transform::apply( transform, l); diff --git a/engine/src/utils/math/physics/solvers.inl b/engine/src/utils/math/physics/solvers.inl index 50621364..02a38e00 100644 --- a/engine/src/utils/math/physics/solvers.inl +++ b/engine/src/utils/math/physics/solvers.inl @@ -70,7 +70,7 @@ namespace { ::applyImpulseTo(a, b, rA, rB, tangent * jt); } - ::positionCorrection(a, b, contact); + // ::positionCorrection(a, b, contact); } template @@ -143,7 +143,7 @@ namespace { float cDot = vRel + penetrationBias; - rhs[i] = (cDot < 0.0f) ? -cDot : 0.0f; // RHS is magnitude of correction needed + rhs[i] = (cDot < 0.0f) ? -cDot : 0.0f; // rHS is magnitude of correction needed lambda[i] = contact.accumulatedNormalImpulse; } @@ -177,86 +177,99 @@ namespace { return ::blockNxNSolver<4>( a, b, manifold, dt ); } + struct ContactCache { + pod::Vector3f normal; + pod::Vector3f tangent; + pod::Vector3f rA, rB; + float bias; + float effectiveMassN; + float effectiveMassT; + float accumulatedNormalImpulse; + float accumulatedTangentImpulse; + }; + void blockPGSSolver( pod::PhysicsBody& a, pod::PhysicsBody& 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]; + const int count = std::min( (int)manifold.points.size(), 4 ); + // precompute contact caches + ::ContactCache cache[4]; for ( auto i = 0; i < count; i++ ) { auto& c = manifold.points[i]; - cache[i].normal = c.normal; - cache[i].rA = c.point - ::getPosition( a, true ); - cache[i].rB = c.point - ::getPosition( b, true ); + auto& cc = cache[i]; - // 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 - ); + cc.normal = c.normal; + cc.tangent = ::computeTangent( c.normal ); + cc.rA = c.point - ::getPosition( a, true ); + cc.rB = c.point - ::getPosition( b, true ); + // 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 ); + + // restitution bias + baumgarte float e = std::min( a.material.restitution, b.material.restitution ); - float penetrationBias = std::max(c.penetration - ::baumgarteCorrectionSlop, 0.0f) * (::baumgarteCorrectionPercent / dt); + float penetrationBias = std::max( c.penetration - ::baumgarteCorrectionSlop, 0.0f ) * (::baumgarteCorrectionPercent / dt); + cc.bias = (vn < -1.0f ? -e * vn : 0.0f) + penetrationBias; - cache[i].bias = (vRel < -1.0f ? -e * vRel : 0.0f) + penetrationBias; + // effective mass (normal) + pod::Vector3f rnA = uf::vector::cross( cc.rA, cc.normal ); + pod::Vector3f rnB = uf::vector::cross( cc.rB, cc.normal ); + float Kn = (a.isStatic ? 0.0f : a.inverseMass) + (b.isStatic ? 0.0f : b.inverseMass) + + uf::vector::dot( uf::vector::cross( rnA * a.inverseInertiaTensor, cc.rA ) + uf::vector::cross( rnB * b.inverseInertiaTensor, cc.rB ), cc.normal ); + cc.effectiveMassN = (Kn > 0.0f) ? 1.0f / Kn : 0.0f; - // 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 ); + // effective mass (tangent) + pod::Vector3f rtA = uf::vector::cross( cc.rA, cc.tangent ); + pod::Vector3f rtB = uf::vector::cross( cc.rB, cc.tangent ); + float Kt = (a.isStatic ? 0.0f : a.inverseMass) + (b.isStatic ? 0.0f : b.inverseMass) + + uf::vector::dot( uf::vector::cross( rtA * a.inverseInertiaTensor, cc.rA ) + uf::vector::cross( rtB * b.inverseInertiaTensor, cc.rB ), cc.tangent ); + cc.effectiveMassT = ( Kt > 0.0f ) ? ( 1.0f / Kt ) : 0.0f; - pod::Vector3f Ia_rnA = rnA * a.inverseInertiaTensor; // diag mult - pod::Vector3f Ib_rnB = rnB * b.inverseInertiaTensor; + // warm start + cc.accumulatedNormalImpulse = c.accumulatedNormalImpulse; + cc.accumulatedTangentImpulse = c.accumulatedTangentImpulse; - 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; + // apply warm-start impulses + pod::Vector3f P = cc.normal * cc.accumulatedNormalImpulse + cc.tangent * cc.accumulatedTangentImpulse; + + ::applyImpulseTo(a, b, cc.rA, cc.rB, P); } - // initialize lambdas (warm start) - pod::Vector4f lambda = {}; - for ( auto i = 0; i < count; i++ ) { - lambda[i] = manifold.points[i].accumulatedNormalImpulse; - } - - // iterative PGS loop + // iterative PGS for ( auto iter = 0; iter < ::solverIterations; iter++ ) { - for (int i = 0; i < count; i++) { - auto& c = manifold.points[i]; + for ( auto i = 0; i < count; 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); + // relative velocity + pod::Vector3f dv = ( b.velocity + uf::vector::cross( b.angularVelocity, cc.rB ) ) - ( a.velocity + uf::vector::cross( a.angularVelocity, cc.rA ) ); - // compute delta impulse - float delta = cc.effectiveMass * (-(vn + cc.bias)); + // normal constraint + float vn = uf::vector::dot( dv, cc.normal ); + float lambdaN = cc.effectiveMassN * (-(vn + cc.bias)); + float oldImpulseN = cc.accumulatedNormalImpulse; + cc.accumulatedNormalImpulse = std::max( oldImpulseN + lambdaN, 0.0f ); + float dPn = cc.accumulatedNormalImpulse - oldImpulseN; - // accumulate and clamp - float newLambda = std::max( lambda[i] + delta, 0.0f ); - delta = newLambda - lambda[i]; - lambda[i] = newLambda; + ::applyImpulseTo( a, b, cc.rA, cc.rB, cc.normal * dPn ); - // apply impulse - ::applyImpulseTo( a, b, cc.rA, cc.rB, cc.normal * delta ); + // friction constraint + dv = ( b.velocity + uf::vector::cross( b.angularVelocity, cc.rB ) ) - ( a.velocity + uf::vector::cross( a.angularVelocity, cc.rA ) ); + float vt = uf::vector::dot( dv, cc.tangent ); + float lambdaT = cc.effectiveMassT * (-vt); + float maxFriction = ( a.material.dynamicFriction + b.material.dynamicFriction ) * 0.5f * cc.accumulatedNormalImpulse; - // position correction - ::positionCorrection( a, b, c ); + float oldImpulseT = cc.accumulatedTangentImpulse; + cc.accumulatedTangentImpulse = std::clamp( oldImpulseT + lambdaT, -maxFriction, maxFriction ); + float dPt = cc.accumulatedTangentImpulse - oldImpulseT; + + ::applyImpulseTo( a, b, cc.rA, cc.rB, cc.tangent * dPt ); } } - // store lambdas back + // store impulses back into manifold for ( auto i = 0; i < count; i++ ) { - manifold.points[i].accumulatedNormalImpulse = lambda[i]; - // ::positionCorrection( a, b, manifold.points[i] ); + manifold.points[i].accumulatedNormalImpulse = cache[i].accumulatedNormalImpulse; + manifold.points[i].accumulatedTangentImpulse = cache[i].accumulatedTangentImpulse; } } @@ -274,4 +287,14 @@ namespace { if ( ::warmupSolver ) for ( auto& manifold : manifolds ) ::warmupManifold( *manifold.a, *manifold.b, manifold, dt ); for ( auto i = 0; i < ::solverIterations; ++i ) for ( auto& manifold : manifolds ) ::resolveManifold( *manifold.a, *manifold.b, manifold, dt ); } + + void solvePositions( uf::stl::vector& manifolds, float dt, int iterations = 4 ) { + for ( auto i = 0; i < iterations; ++i ) { + for ( auto& m : manifolds ) { + for ( auto& c : m.points ) { + ::positionCorrection( *m.a, *m.b, c ); + } + } + } + } } \ 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 3befc6ef..766f5902 100644 --- a/engine/src/utils/math/physics/tests.inl +++ b/engine/src/utils/math/physics/tests.inl @@ -928,11 +928,13 @@ TEST(TriangleTriangle_Collision_SimpleOverlap, { pod::TriangleWithNormal triA{ { { {0,0,0}, {1,0,0}, {0,1,0} } }, - { {0,0,1}, {0,0,1}, {0,0,1} }, + {0,0,1}, + //{ {0,0,1}, {0,0,1}, {0,0,1} }, }; pod::TriangleWithNormal triB{ { { {0.2f,0.2f,0}, {0.8f,0.2f,0}, {0.2f,0.8f,0} } }, - { {0,0,1}, {0,0,1}, {0,0,1} }, + {0,0,1}, + //{ {0,0,1}, {0,0,1}, {0,0,1} }, }; auto& bodyA = uf::physics::impl::create( world, objA, triA, 0.0f ); @@ -958,11 +960,13 @@ TEST(TriangleTriangle_Collision_CoplanarOverlap, { pod::TriangleWithNormal triA{ { { {0,0,0}, {2,0,0}, {0,2,0} } }, - { {0,0,1}, {0,0,1}, {0,0,1} }, + {0,0,1}, + //{ {0,0,1}, {0,0,1}, {0,0,1} }, }; pod::TriangleWithNormal triB{ { { {1,1,0}, {2,1,0}, {1,2,0} } }, - { {0,0,1}, {0,0,1}, {0,0,1} }, + {0,0,1}, + //{ {0,0,1}, {0,0,1}, {0,0,1} }, }; auto& bodyA = uf::physics::impl::create(world, objA, triA, 0.0f); @@ -984,11 +988,13 @@ TEST(TriangleTriangle_Collision_TouchingEdge, { pod::TriangleWithNormal triA{ { { {0,0,0}, {1,0,0}, {0.5f,1,0} } }, - { {0,0,1}, {0,0,1}, {0,0,1} }, + {0,0,1}, + //{ {0,0,1}, {0,0,1}, {0,0,1} }, }; pod::TriangleWithNormal triB{ { { {0.5f,1,0}, {1.5f,0,0}, {1,1,0} } }, - { {0,0,1}, {0,0,1}, {0,0,1} }, + {0,0,1}, + //{ {0,0,1}, {0,0,1}, {0,0,1} }, }; auto& bodyA = uf::physics::impl::create(world, objA, triA, 0.0f); @@ -1014,7 +1020,8 @@ TEST(TriangleAabb_Collision_OverlapCenter, { pod::TriangleWithNormal tri { { { {0,0,0}, {1,0,0}, {0,1,0} } }, - { {0,0,1}, {0,0,1}, {0,0,1} }, + {0,0,1}, + //{ {0,0,1}, {0,0,1}, {0,0,1} }, }; pod::AABB box = {{0.25f, 0.25f, -0.25f}, {0.75f, 0.75f, +0.25f}}; @@ -1041,7 +1048,8 @@ TEST(TriangleAabb_Collision_NoOverlap, { pod::TriangleWithNormal tri { { { {0,0,0}, {1,0,0}, {0,1,0} } }, - { {0,0,1}, {0,0,1}, {0,0,1} }, + {0,0,1}, + //{ {0,0,1}, {0,0,1}, {0,0,1} }, }; pod::AABB box = {{2,2,2}, {3,3,3}}; @@ -1066,7 +1074,8 @@ TEST(TrianglePlane_Collision_BelowPlane, { pod::TriangleWithNormal tri{ { { {0,0,-0.1f}, {1,0,-0.1f}, {0,1,-0.1f} } }, - { {0,0,1}, {0,0,1}, {0,0,1} }, + {0,0,1}, + //{ {0,0,1}, {0,0,1}, {0,0,1} }, }; auto& bodyA = uf::physics::impl::create(world, objA, tri, 0.0f); @@ -1093,7 +1102,8 @@ TEST(TrianglePlane_NoCollision_AbovePlane, { pod::TriangleWithNormal tri{ { { {0,0,1}, {1,0,1}, {0,1,1} } }, - { {0,0,-1}, {0,0,-1}, {0,0,-1} }, // facing down, but above plane + {0,0,-1}, + //{ {0,0,-1}, {0,0,-1}, {0,0,-1} }, // facing down, but above plane }; auto& bodyA = uf::physics::impl::create(world, objA, tri, 0.0f); @@ -1113,7 +1123,8 @@ TEST(TriangleSphere_Collision_Tangent, { pod::TriangleWithNormal tri{ { { {0,0,0}, {1,0,0}, {0,1,0} } }, - { {0,0,1}, {0,0,1}, {0,0,1} }, + {0,0,1}, + //{ {0,0,1}, {0,0,1}, {0,0,1} }, }; pod::Sphere sphere = { 0.5f }; // radius @@ -1142,7 +1153,8 @@ TEST(TriangleCapsule_Collision_Overlap, { pod::TriangleWithNormal tri{ { { {0,0,0}, {1,0,0}, {0,1,0} } }, - { {0,0,1}, {0,0,1}, {0,0,1} }, + {0,0,1}, + //{ {0,0,1}, {0,0,1}, {0,0,1} }, }; // Capsule aligned along Z axis, radius 0.2 @@ -1173,7 +1185,8 @@ TEST(TriangleCapsule_Collision_NoOverlap, { pod::TriangleWithNormal tri{ { { {0,0,0}, {1,0,0}, {0,1,0} } }, - { {0,0,1}, {0,0,1}, {0,0,1} }, + {0,0,1}, + //{ {0,0,1}, {0,0,1}, {0,0,1} }, }; pod::Capsule capsule; @@ -1198,7 +1211,8 @@ TEST(TriangleCapsule_Collision_Tangent, { pod::TriangleWithNormal tri{ { { {0,0,0}, {1,0,0}, {0,1,0} } }, - { {0,0,1}, {0,0,1}, {0,0,1} }, + {0,0,1}, + //{ {0,0,1}, {0,0,1}, {0,0,1} }, }; pod::Capsule capsule; @@ -1230,7 +1244,8 @@ TEST(TriangleCapsule_Collision_EdgeAlignment, { pod::TriangleWithNormal tri{ { { {0,0,0}, {2,0,0}, {0,2,0} } }, - { {0,0,1}, {0,0,1}, {0,0,1} }, + {0,0,1}, + //{ {0,0,1}, {0,0,1}, {0,0,1} }, }; pod::Capsule capsule; diff --git a/engine/src/utils/math/physics/triangle.inl b/engine/src/utils/math/physics/triangle.inl index 46c6ae04..d5e26cd1 100644 --- a/engine/src/utils/math/physics/triangle.inl +++ b/engine/src/utils/math/physics/triangle.inl @@ -25,7 +25,8 @@ namespace { return uf::vector::normalize(uf::vector::cross(tri.points[1] - tri.points[0], tri.points[2] - tri.points[0])); } pod::Vector3f triangleNormal( const pod::TriangleWithNormal& tri ) { - return uf::vector::normalize( tri.normals[0] + tri.normals[1] + tri.normals[2] ); + return tri.normal; + //return uf::vector::normalize( tri.normals[0] + tri.normals[1] + tri.normals[2] ); } pod::AABB computeTriangleAABB( const void* vertices, size_t vertexStride, const void* indexData, size_t indexSize, size_t triID ) { @@ -57,16 +58,20 @@ namespace { 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. + size_t triBase = 0; const uf::Mesh::View* found = nullptr; for ( auto& v : views ) { - if ( v.index.first <= triIndexID && triIndexID < v.index.first + v.index.count ) { + auto trisInView = v.index.count / 3; + if (triID < triBase + trisInView) { found = &v; + triID -= triBase; // local triangle index inside this view break; } + triBase += trisInView; } UF_ASSERT( found ); + uint32_t triIndexID = triID * 3; // remap triangle ID to index ID pod::TriangleWithNormal tri; @@ -77,9 +82,6 @@ namespace { 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); @@ -91,7 +93,10 @@ namespace { for ( auto i = 0; i < 3; ++i ) tri.points[i] = *reinterpret_cast(base + idxs[i] * stride); } - if ( normals.valid() ) { + 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); @@ -99,28 +104,29 @@ namespace { 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 ) { + pod::TriangleWithNormal fetchTriangle( const uf::Mesh& mesh, size_t triID, const pod::PhysicsBody& body, bool fast = true ) { auto tri = ::fetchTriangle( mesh, triID ); auto transform = ::getTransform( body ); if ( body.collider.type == pod::ShapeType::MESH ) { - for ( auto i = 0; i < 3; ++i ) { - tri.points[i] = uf::transform::apply( transform, tri.points[i] ); - tri.normals[i] = uf::quaternion::rotate( transform.orientation, tri.normals[i] ); + if ( fast ) { + for ( auto i = 0; i < 3; ++i ) tri.points[i] += transform.position; + } else { + for ( auto i = 0; i < 3; ++i ) tri.points[i] = uf::transform::apply( transform, tri.points[i] ); + tri.normal = uf::quaternion::rotate( transform.orientation, tri.normal ); } } else { #if REORIENT_NORMALS_ON_FETCH auto triCenter = ::triangleCenter( tri ); auto delta = ::getPosition( body ) - triCenter; - for ( auto i = 0; i < 3; ++i ) { - if ( uf::vector::dot(tri.normals[i], delta) < 0.0f ) tri.normals[i] = -tri.normals[i]; - } + if ( uf::vector::dot(tri.normal, delta) < 0.0f ) tri.normal = -tri.normal; #endif } @@ -129,6 +135,7 @@ namespace { bool computeTriangleTriangleSegment( const pod::TriangleWithNormal& A, const pod::TriangleWithNormal& B, pod::Vector3f& p0, pod::Vector3f& p1, float eps = EPS(1e-6f) ) { uf::stl::vector intersections; + intersections.reserve(3); auto checkAndPush = [&]( const pod::Vector3f& pt ) { // avoid duplicates @@ -218,6 +225,7 @@ namespace { // if ( !::triangleTriangleIntersect( a, b ) ) return false; uf::stl::vector axes = { ::triangleNormal( a ), ::triangleNormal( b ) }; + axes.reserve(2+3); pod::Vector3f p0 = {}, p1 = {}; if ( !::computeTriangleTriangleSegment(a, b, p0, p1) ) { @@ -280,7 +288,7 @@ namespace { float penetration = tolerance - dist; #if REORIENT_NORMALS_ON_CONTACT - if ( uf::vector::dot(normal, delta) < 0.0f ) normal = -normal; + if ( uf::vector::dot( normal, delta ) < 0.0f ) normal = -normal; #endif manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); @@ -306,7 +314,7 @@ namespace { float penetration = r - dist; #if REORIENT_NORMALS_ON_CONTACT - if ( uf::vector::dot(normal, delta) < 0.0f ) normal = -normal; + if ( uf::vector::dot( normal, delta ) < 0.0f ) normal = -normal; #endif manifold.points.emplace_back(pod::Contact{ contact, normal, penetration }); @@ -379,7 +387,7 @@ namespace { float penetration = r - dist; #if REORIENT_NORMALS_ON_CONTACT - if ( uf::vector::dot(normal, delta) < 0.0f ) normal = -normal; + if ( uf::vector::dot( normal, delta ) < 0.0f ) normal = -normal; #endif manifold.points.emplace_back(pod::Contact{ contact, normal, penetration });