From 5cc441bb6306ac254755c9806528f90f510f004d Mon Sep 17 00:00:00 2001 From: ecker Date: Mon, 20 Jul 2026 20:18:00 -0500 Subject: [PATCH] chasing down demons in the physics system...... --- .../utils/math/physics/constraints/contact.h | 4 +- engine/inc/uf/utils/math/physics/structs.h | 32 +++-- engine/src/engine/object/behavior.cpp | 3 + engine/src/ext/lgs/mis.cpp | 23 +++- .../utils/math/physics/broadphase/island.cpp | 2 +- engine/src/utils/math/physics/common.cpp | 12 +- .../math/physics/constraints/contact.cpp | 103 ++++++++++------ engine/src/utils/math/physics/impl.cpp | 116 ++++++++++++++---- engine/src/utils/math/physics/integration.cpp | 12 +- 9 files changed, 231 insertions(+), 76 deletions(-) diff --git a/engine/inc/uf/utils/math/physics/constraints/contact.h b/engine/inc/uf/utils/math/physics/constraints/contact.h index f57b563b..1de9c0f9 100644 --- a/engine/inc/uf/utils/math/physics/constraints/contact.h +++ b/engine/inc/uf/utils/math/physics/constraints/contact.h @@ -12,7 +12,7 @@ namespace impl { void mergeManifold( pod::Manifold& manifold ); void retrieveManifold( pod::Manifold& current, const pod::Manifold& previous, float distanceThreshold = 0.1f, float separationThreshold = 0.1f, float decay = 0.85f ); void prepareManifoldCache( uf::stl::unordered_map& cache, const uf::stl::vector& islands, const uf::stl::vector& bodies ); - void updateManifoldCache( const uf::stl::vector& manifolds, uf::stl::unordered_map& cache ); + void updateManifoldCache( pod::Island& island, const pod::CollisionEvent::array_t& previous, uf::stl::unordered_map& cache ); void pruneManifoldCache( uf::stl::unordered_map& cache ); void warmupContact( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Contact& c, float dt ); void warmupManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, const pod::Manifold& manifold, float dt ); @@ -20,5 +20,5 @@ namespace impl { void resolveManifold( pod::PhysicsBody& a, pod::PhysicsBody& b, pod::Manifold& manifold, float dt ); void solveManifold( uf::stl::vector& manifolds, float dt ); - void dispatchManifold( pod::Manifold& manifold, pod::CollisionEvent::events_t& events, pod::CollisionEvent::map_t& active, const pod::CollisionEvent::map_t& previous ); + void dispatchManifold( pod::Manifold& manifold, pod::CollisionEvent::events_t& events, pod::CollisionEvent::array_t& active, const pod::CollisionEvent::array_t& previous ); } \ No newline at end of file diff --git a/engine/inc/uf/utils/math/physics/structs.h b/engine/inc/uf/utils/math/physics/structs.h index 767b3167..fb6d421f 100644 --- a/engine/inc/uf/utils/math/physics/structs.h +++ b/engine/inc/uf/utils/math/physics/structs.h @@ -188,6 +188,7 @@ namespace pod { pod::PhysicsBody* a = NULL; pod::PhysicsBody* b = NULL; uf::stl::vector points; + uint32_t primaryContactID = 0; }; struct Constraint { @@ -281,14 +282,15 @@ namespace pod { const pod::PhysicsBody* invoker = NULL; pod::Contact contact = { pod::Vector3f{}, pod::Vector3f{}, FLT_MAX }; }; + + struct PairState { + size_t pairKey; + pod::PhysicsBody* a; + pod::PhysicsBody* b; - struct Island { - bool awake = true; - uf::stl::vector indices; - pod::BVH::pairs_t pairs; - - uf::stl::vector manifolds; - uf::stl::vector constraints; + bool operator<(const PairState& other) const { + return pairKey < other.pairKey; + } }; } @@ -383,6 +385,7 @@ namespace pod { typedef std::pair pairs_t; typedef uf::stl::vector events_t; typedef uf::stl::unordered_map map_t; + typedef uf::stl::vector array_t; pod::CollisionState state = {}; pod::PhysicsBody* a = NULL; @@ -395,6 +398,17 @@ namespace pod { uint32_t featureA = (uint32_t)(-1); uint32_t featureB = (uint32_t)(-1); }; + + struct Island { + bool awake = true; + uf::stl::vector indices; + pod::BVH::pairs_t pairs; + + uf::stl::vector manifolds; + uf::stl::vector constraints; + uf::stl::vector active; + uf::stl::vector events; + }; } namespace pod { @@ -419,6 +433,8 @@ namespace pod { pod::Vector3f pseudoVelocity = {}; pod::Vector3f pseudoAngularVelocity = {}; + pod::Vector3f linearFactor = { 1.0f, 1.0f, 1.0f }; // probably better to just have a bitmask for these....... + pod::Vector3f angularFactor = { 1.0f, 1.0f, 1.0f }; pod::Vector3f inverseInertiaTensor = { 1, 1, 1 }; pod::Vector3f gravity = { NAN, NAN, NAN }; // an invalid gravity will fallback to world gravity @@ -504,7 +520,7 @@ namespace pod { pod::BVH staticBvh; pod::CollisionEvent::events_t collisionEvents; - pod::CollisionEvent::map_t activeCollisions; + pod::CollisionEvent::array_t activeCollisions; }; } diff --git a/engine/src/engine/object/behavior.cpp b/engine/src/engine/object/behavior.cpp index 147d74ed..7a5982c5 100644 --- a/engine/src/engine/object/behavior.cpp +++ b/engine/src/engine/object/behavior.cpp @@ -176,6 +176,9 @@ void uf::ObjectBehavior::initialize( uf::Object& self ) { if ( metadataJsonPhysics["inertia"].is() && !metadataJsonPhysics["inertia"].as() ) { body.inverseInertiaTensor = { 0.0f, 0.0f, 0.0f }; } + + body.linearFactor = uf::vector::decode( metadataJsonPhysics["linearFactor"], body.linearFactor ); + body.angularFactor = uf::vector::decode( metadataJsonPhysics["angularFactor"], body.angularFactor ); } UF_BEHAVIOR_METADATA_BIND_SERIALIZER_HOOKS(metadata, metadataJson); diff --git a/engine/src/ext/lgs/mis.cpp b/engine/src/ext/lgs/mis.cpp index 788b7598..6030ba60 100644 --- a/engine/src/ext/lgs/mis.cpp +++ b/engine/src/ext/lgs/mis.cpp @@ -1540,7 +1540,7 @@ namespace impl { // bind physics PropertyPhysType physType; - // to-do: optimize this as it cuts my FPS by ~30% with lots of small physics objects + // to-do: optimize this as it incurs ~1.5ms even with everything being marked as static if ( ctx.findInheritedProperty( objectID, ctx.properties.physType, physType ) && physType.type != impl::PropertyPhysType::NONE ) { auto& physMeta = node.metadata["physics"]; @@ -1555,7 +1555,10 @@ namespace impl { PropertyPhysAttr physAttr; if ( ctx.findInheritedProperty( objectID, ctx.properties.physAttr, physAttr ) ) { - physMeta["mass"] = 0.0f; // (physType.type == impl::PropertyPhysType::OBB || physAttr.edge_trigger != 0) ? 0.0f : physAttr.mass; + bool isStaticOBB = (physType.type == impl::PropertyPhysType::OBB || physAttr.edge_trigger != 0); + bool isCompletelyResting = (physAttr.rest_axes & 0x7); + + physMeta["mass"] = (isStaticOBB || isCompletelyResting) ? 0.0f : physAttr.mass; physMeta["friction"] = physAttr.base_friction; physMeta["restitution"] = physAttr.elasticity; physMeta["gravity"] = uf::vector::encode( pod::Vector3f{0.0f, -0.0981f * physAttr.gravity, 0.0f} ); @@ -1564,6 +1567,22 @@ namespace impl { physMeta["category"] = "trigger"; physMeta["inertia"] = false; } + + pod::Vector3f linearFactor = { 1.0f, 1.0f, 1.0f }; + if ( physAttr.rest_axes & 0x1 ) linearFactor.x = 0.0f; + if ( physAttr.rest_axes & 0x2 ) linearFactor.y = 0.0f; + if ( physAttr.rest_axes & 0x4 ) linearFactor.z = 0.0f; + + pod::Vector3f angularFactor = { 1.0f, 1.0f, 1.0f }; + if ( !(physAttr.rot_axes & 0x1) ) angularFactor.x = 0.0f; + if ( !(physAttr.rot_axes & 0x2) ) angularFactor.y = 0.0f; + if ( !(physAttr.rot_axes & 0x4) ) angularFactor.z = 0.0f; + + pod::Vector3f linFactor = uf::vector::abs( impl::convertPos_NewDark( linearFactor ) ); + pod::Vector3f angFactor = uf::vector::abs( impl::convertPos_NewDark( angularFactor ) ); + + physMeta["linearFactor"] = uf::vector::encode(linearFactor); + physMeta["angularFactor"] = uf::vector::encode(angularFactor); } PropertyPhysDims physDims; diff --git a/engine/src/utils/math/physics/broadphase/island.cpp b/engine/src/utils/math/physics/broadphase/island.cpp index dd50c561..1d0aea8a 100644 --- a/engine/src/utils/math/physics/broadphase/island.cpp +++ b/engine/src/utils/math/physics/broadphase/island.cpp @@ -92,7 +92,7 @@ void impl::buildIslands( const pod::BVH::pairs_t& pairs, const uf::stl::vector

inverseMass == 0.0f ? b : a; if ( bodies[a]->inverseMass == 0.0f && bodies[b]->inverseMass == 0.0f ) continue; - pod::BVH::index_t root = unionizer.find(a); + pod::BVH::index_t root = unionizer.find(dynamicIndex); if ( rootToIsland.find(root) != rootToIsland.end() ) { pod::BVH::index_t islandID = rootToIsland[root]; diff --git a/engine/src/utils/math/physics/common.cpp b/engine/src/utils/math/physics/common.cpp index 797a6c3b..f5ac8bad 100644 --- a/engine/src/utils/math/physics/common.cpp +++ b/engine/src/utils/math/physics/common.cpp @@ -44,8 +44,12 @@ void impl::updateActivity( pod::PhysicsBody& body, float dt ) { // already asleep if ( !body.activity.awake ) return; - // check if body is moving - float linSpeed2 = uf::vector::magnitude( body.velocity ); + // undo gravity + pod::Vector3f gravity = uf::vector::isValid( body.gravity ) ? body.gravity : body.world->gravity; + pod::Vector3f velocity = body.velocity - (gravity * dt); + + // check if body is moving using the test velocity! + float linSpeed2 = uf::vector::magnitude( velocity ); float angSpeed2 = uf::vector::magnitude( body.angularVelocity ); // body is nearly still @@ -57,7 +61,9 @@ void impl::updateActivity( pod::PhysicsBody& body, float dt ) { if ( body.activity.sleepTimer > threshold ) impl::sleepBody( body ); } // body is moving, reset timer - else impl::wakeBody( body ); + else { + impl::wakeBody( body ); + } } // returns an absolute transform while also allowing offsetting the collision body diff --git a/engine/src/utils/math/physics/constraints/contact.cpp b/engine/src/utils/math/physics/constraints/contact.cpp index c03b1c48..6b5052e4 100644 --- a/engine/src/utils/math/physics/constraints/contact.cpp +++ b/engine/src/utils/math/physics/constraints/contact.cpp @@ -292,11 +292,48 @@ void impl::prepareManifoldCache( uf::stl::unordered_map& } } -void impl::updateManifoldCache( const uf::stl::vector& manifolds, uf::stl::unordered_map& cache ) { - for ( const auto& m : manifolds ) { - auto it = cache.find( impl::makePairKey( *m.a, *m.b ) ); - if ( it == cache.end() ) continue; // assert - it->second = m; +void impl::updateManifoldCache( pod::Island& island, const pod::CollisionEvent::array_t& previous, uf::stl::unordered_map& cache ) { + auto& manifolds = island.manifolds; + auto& active = island.active; + auto& events = island.events; + + auto cacheLifetime = uf::physics::settings.manifoldCacheLifetime ? uf::physics::settings.manifoldCacheLifetime : MAX(1, uf::physics::settings.substeps) * 2; + + for ( auto& m : manifolds ) { + if ( m.points.empty() ) continue; + m.primaryContactID = 0; + + if ( m.points.size() > 1 ) { + float maxImpulse = -1.0f; + float maxPen = -FLT_MAX; + for ( size_t i = 0; i < m.points.size(); ++i ) { + if ( m.points[i].accumulatedNormalImpulse > maxImpulse ) { + m.primaryContactID = i; + maxImpulse = m.points[i].accumulatedNormalImpulse; + } + if ( m.points[i].penetration > maxPen ) { + maxPen = m.points[i].penetration; + m.primaryContactID = i; + } + } + } + + impl::dispatchManifold( m, events, active, previous ); + + if ( uf::physics::settings.warmupSolver ) { + auto it = uf::physics::settings.manifoldsCache.find( impl::makePairKey( *m.a, *m.b ) ); + if ( it != uf::physics::settings.manifoldsCache.end() ) { + it->second = m; + + auto& cachedPoints = it->second.points; + auto newEnd = std::remove_if(cachedPoints.begin(), cachedPoints.end(), + [cacheLifetime](pod::Contact& c) { + c.lifetime++; + return c.lifetime > cacheLifetime; + }); + cachedPoints.erase(newEnd, cachedPoints.end()); + } + } } } @@ -305,18 +342,26 @@ void impl::pruneManifoldCache( uf::stl::unordered_map& ca if ( !cacheLifetime ) { cacheLifetime = MAX(1, uf::physics::settings.substeps) * 2; } + for ( auto itCache = cache.begin(); itCache != cache.end(); ) { auto& manifold = itCache->second; - // prune points that are too old - for ( auto it = manifold.points.begin(); it != manifold.points.end(); ) { - if ( it->lifetime > cacheLifetime ) it = manifold.points.erase(it); - else ++it; + if ( manifold.a && manifold.b && !manifold.a->activity.awake && !manifold.b->activity.awake ) { + ++itCache; + continue; } + // prune points that are too old + auto newEnd = std::remove_if(manifold.points.begin(), manifold.points.end(), + [cacheLifetime](const pod::Contact& c) { return c.lifetime > cacheLifetime; }); + manifold.points.erase(newEnd, manifold.points.end()); + // empty manifold, kill it - if ( manifold.points.empty() ) itCache = cache.erase(itCache); - else ++itCache; + if ( manifold.points.empty() ) { + itCache = cache.erase(itCache); + } else { + ++itCache; + } } } @@ -356,37 +401,23 @@ void impl::solveManifold( uf::stl::vector& manifolds, float dt ) for ( auto i = 0; i < uf::physics::settings.solverIterations; ++i ) for ( auto& manifold : manifolds ) impl::resolveManifold( *manifold.a, *manifold.b, manifold, dt ); } -void impl::dispatchManifold( pod::Manifold& manifold, pod::CollisionEvent::events_t& events, pod::CollisionEvent::map_t& active, const pod::CollisionEvent::map_t& previous ) { - // mark as an active collision +void impl::dispatchManifold( pod::Manifold& manifold, pod::CollisionEvent::events_t& events, pod::CollisionEvent::array_t& active, const pod::CollisionEvent::array_t& previous ) { auto pairKey = impl::makePairKey( *manifold.a, *manifold.b ); - active[pairKey] = { manifold.a, manifold.b }; - // find largest impulse - size_t primaryID = 0; - float maxImpulse = -1.0f; - for ( auto i = 0; i < manifold.points.size(); ++i ) { - auto& c = manifold.points[i]; - if ( c.accumulatedNormalImpulse <= maxImpulse ) continue; - primaryID = i; - maxImpulse = c.accumulatedNormalImpulse; - } - if ( maxImpulse <= EPS ) { - float maxPen = -FLT_MAX; - for ( int i = 0; i < manifold.points.size(); ++i ) { - auto& c = manifold.points[i]; - if ( c.penetration <= maxPen ) continue; - maxPen = c.penetration; - primaryID = i; - } - } - auto& contact = manifold.points[primaryID]; - // dispatch + + active.push_back({ pairKey, manifold.a, manifold.b }); + + auto it = std::lower_bound(previous.begin(), previous.end(), pod::PairState{pairKey, nullptr, nullptr}); + bool isNew = (it == previous.end() || it->pairKey != pairKey); + + auto& contact = manifold.points[manifold.primaryContactID]; + events.emplace_back(pod::CollisionEvent{ - .state = previous.count( pairKey ) == 0 ? pod::CollisionState::ENTER : pod::CollisionState::SUSTAIN, + .state = isNew ? pod::CollisionState::ENTER : pod::CollisionState::SUSTAIN, .a = manifold.a, .b = manifold.b, .point = contact.point, .normal = contact.normal, - .impulse = maxImpulse, + .impulse = contact.accumulatedNormalImpulse, .featureA = contact.featureA, .featureB = contact.featureB, }); diff --git a/engine/src/utils/math/physics/impl.cpp b/engine/src/utils/math/physics/impl.cpp index 714bf497..76db3fd1 100644 --- a/engine/src/utils/math/physics/impl.cpp +++ b/engine/src/utils/math/physics/impl.cpp @@ -56,6 +56,7 @@ void uf::physics::tick( pod::World& world, float dt ) { return; } + //UF_TIMER_MULTITRACE_START("Tick Step Begin"); static float accumulator = 0; accumulator += dt; @@ -65,7 +66,9 @@ void uf::physics::tick( pod::World& world, float dt ) { if ( uf::physics::settings.substeps > 0 ) uf::physics::substep( world, timestep, uf::physics::settings.substeps ); else uf::physics::step( world, timestep ); accumulator -= timestep; + //UF_TIMER_MULTITRACE("Tick Accumulation Step"); } + //UF_TIMER_MULTITRACE_END("Tick Step Complete"); if ( uf::physics::settings.debugDraw.mask != pod::Collider::CATEGORY_NONE ) impl::draw( world, dt ); } @@ -87,6 +90,7 @@ void uf::physics::substep( pod::World& world, float dt, int32_t substeps ) { } } void uf::physics::step( pod::World& world, float dt ) { + //UF_TIMER_MULTITRACE_START("Physics Step Begin"); auto& bodies = world.bodies; auto& constraints = world.constraints; auto& dynamicBvh = world.dynamicBvh; @@ -95,7 +99,7 @@ void uf::physics::step( pod::World& world, float dt ) { auto& collisionEvents = world.collisionEvents; STATIC_THREAD_LOCAL(pod::CollisionEvent::events_t, previousCollisionEvents); - STATIC_THREAD_LOCAL(pod::CollisionEvent::map_t, previousCollisions); + STATIC_THREAD_LOCAL(pod::CollisionEvent::array_t, previousCollisions); std::swap( previousCollisions, activeCollisions ); std::swap( previousCollisionEvents, collisionEvents ); @@ -104,6 +108,8 @@ void uf::physics::step( pod::World& world, float dt ) { ++uf::physics::settings.frameCounter; + activeCollisions.reserve(previousCollisions.size()); + // flatten all transforms into a contiguous buffer static thread_local uf::stl::vector*> originalTransforms; // stores the pointer to the original transform static thread_local uf::stl::vector> flattenedTransforms; // stores the flattened transforms @@ -132,6 +138,7 @@ void uf::physics::step( pod::World& world, float dt ) { for ( auto* body : bodies ) { impl::integrate( *body, dt ); } + //UF_TIMER_MULTITRACE("Integration & Flattening"); // rebuild static bvh if dirty if ( staticBvh.dirty && uf::physics::settings.useSplitBvhs ) { @@ -151,6 +158,8 @@ void uf::physics::step( pod::World& world, float dt ) { } break; } + //UF_TIMER_MULTITRACE("BVH Updates"); + // query for overlaps pod::BVH::pairs_t pairs; impl::queryOverlaps( dynamicBvh, pairs ); @@ -158,19 +167,33 @@ void uf::physics::step( pod::World& world, float dt ) { impl::queryOverlaps( dynamicBvh, staticBvh, pairs ); } + //UF_TIMER_MULTITRACE("Broadphase Overlap Queries"); + // build islands from overlaps STATIC_THREAD_LOCAL(uf::stl::vector, islands); impl::buildIslands( pairs, bodies, constraints, islands ); + //UF_TIMER_MULTITRACE("Island Generation"); + if ( uf::physics::settings.warmupSolver ) impl::prepareManifoldCache( uf::physics::settings.manifoldsCache, islands, bodies ); + std::atomic timeNarrowphase{0}; + std::atomic timeVelocitySolver{0}; + std::atomic timePositionSolver{0}; + std::atomic timeConstraints{0}; + std::atomic timeCache{0}; + // iterate islands //#pragma omp parallel for schedule(dynamic) auto tasks = uf::thread::schedule(true); for ( auto& island : islands ) tasks.queue([&]{ auto& manifolds = island.manifolds; auto& constraints = island.constraints; + auto& active = island.active; + auto& events = island.events; manifolds.clear(); + active.clear(); + events.clear(); // sleeping island, skip (asleep islands shouldn't ever be in here) if ( !island.awake ) return; @@ -183,6 +206,7 @@ void uf::physics::step( pod::World& world, float dt ) { } // iterate overlap pairs + //auto tStart = TIMER_TRACE.elapsed().asMicroseconds(); for ( auto& [ ia, ib ] : island.pairs ) { auto& a = *bodies[ia]; auto& b = *bodies[ib]; @@ -235,38 +259,85 @@ void uf::physics::step( pod::World& world, float dt ) { // store manifold manifolds.emplace_back( manifold ); } + //timeNarrowphase += (TIMER_TRACE.elapsed().asMicroseconds() - tStart); // pass manifolds to solver + //tStart = TIMER_TRACE.elapsed().asMicroseconds(); impl::solveManifold( manifolds, dt ); + //timeVelocitySolver += (TIMER_TRACE.elapsed().asMicroseconds() - tStart); // do position correction + //tStart = TIMER_TRACE.elapsed().asMicroseconds(); impl::solvePositions( manifolds, dt ); + //timePositionSolver += (TIMER_TRACE.elapsed().asMicroseconds() - tStart); // solve constraints + //tStart = TIMER_TRACE.elapsed().asMicroseconds(); impl::solveConstraints( constraints, dt ); + //timeConstraints += (TIMER_TRACE.elapsed().asMicroseconds() - tStart); // cache manifold positions - if ( uf::physics::settings.warmupSolver ) impl::updateManifoldCache( manifolds, uf::physics::settings.manifoldsCache ); + //tStart = TIMER_TRACE.elapsed().asMicroseconds(); + impl::updateManifoldCache( island, previousCollisions, uf::physics::settings.manifoldsCache ); + //timeCache += (TIMER_TRACE.elapsed().asMicroseconds() - tStart); }); uf::thread::execute( tasks ); + //UF_TIMER_MULTITRACE("Narrowphase & Solvers (Threaded)"); + //UF_MSG_DEBUG(" -> Manifold Gen: {} us", timeNarrowphase.load()); + //UF_MSG_DEBUG(" -> Vel Solver : {} us", timeVelocitySolver.load()); + //UF_MSG_DEBUG(" -> Pos Solver : {} us", timePositionSolver.load()); + //UF_MSG_DEBUG(" -> Constraints : {} us", timeConstraints.load()); + //UF_MSG_DEBUG(" -> Cache : {} us", timeCache.load()); - // prune expired manifolds in the cache - if ( uf::physics::settings.warmupSolver ) impl::pruneManifoldCache( uf::physics::settings.manifoldsCache ); + { + activeCollisions.clear(); + size_t totalActive = 0; + size_t totalEvents = 0; + for ( const auto& island : islands ) { + totalActive += island.active.size(); + totalEvents += island.events.size(); + } - for ( auto& island : islands ) for ( auto& manifold : island.manifolds ) { - // dispatch collision events - impl::dispatchManifold( manifold, collisionEvents, activeCollisions, previousCollisions ); - // draw collision events - if ( uf::physics::settings.debugDraw.contacts ) impl::drawManifold( manifold ); - } - // dispatch exiting collisions - for ( auto& [ key, pair ] : previousCollisions ) { - // sustained collision - if ( activeCollisions.count( key ) > 0 ) continue; - // mark as exiting - collisionEvents.emplace_back(pod::CollisionEvent{ - .state = pod::CollisionState::EXIT, - .a = pair.first, - .b = pair.second, - }); + activeCollisions.clear(); + activeCollisions.reserve(totalActive); + collisionEvents.reserve(collisionEvents.size() + totalEvents); + + for ( auto& island : islands ) { + activeCollisions.insert(activeCollisions.end(), island.active.begin(), island.active.end()); + collisionEvents.insert(collisionEvents.end(), island.events.begin(), island.events.end()); + } + //UF_TIMER_MULTITRACE("Event Dispatch (Insert)"); + + std::sort(activeCollisions.begin(), activeCollisions.end()); + + //UF_TIMER_MULTITRACE("Sort Active Array"); + + auto itPrev = previousCollisions.begin(); + auto itAct = activeCollisions.begin(); + + while ( itPrev != previousCollisions.end() ) { + // mark as exiting + if ( itAct == activeCollisions.end() || itPrev->pairKey < itAct->pairKey ) { + collisionEvents.emplace_back(pod::CollisionEvent{ + .state = pod::CollisionState::EXIT, + .a = itPrev->a, + .b = itPrev->b, + }); + + if ( uf::physics::settings.warmupSolver ) uf::physics::settings.manifoldsCache.erase(itPrev->pairKey); + ++itPrev; + } + // sustained collision + else if ( itPrev->pairKey == itAct->pairKey ) { + ++itPrev; + ++itAct; + } + // new collision + else { + ++itAct; + } + } + + //UF_TIMER_MULTITRACE("Event Dispatch (Sweep & Exit)"); } + // snap velocities of bodies for ( auto* b : bodies ) impl::snapVelocity( *b, dt ); @@ -304,6 +375,9 @@ void uf::physics::step( pod::World& world, float dt ) { ); } } + + //UF_TIMER_MULTITRACE("Transform Unflattening"); + //UF_TIMER_MULTITRACE_END("Physics Step Complete"); } void uf::physics::setMass( pod::PhysicsBody& body, float mass ) { @@ -611,7 +685,7 @@ void uf::physics::destroy( uf::Object& object ) { auto* current = &root; while ( current != NULL ) { auto* next = current->next; - uf::physics::destroy( *current ); + uf::physics::destroy( *current ); if ( current != &root ) delete current; current = next; } diff --git a/engine/src/utils/math/physics/integration.cpp b/engine/src/utils/math/physics/integration.cpp index 53d1ee88..5b15518f 100644 --- a/engine/src/utils/math/physics/integration.cpp +++ b/engine/src/utils/math/physics/integration.cpp @@ -188,18 +188,24 @@ void impl::integrate( pod::PhysicsBody& body, float dt ) { auto& world = *body.world; auto& transform = *body.transform; auto fT = uf::transform::flatten( transform ); + auto gravity = uf::physics::getGravity( body ); // linear integration - pod::Vector3f acceleration = body.forceAccumulator * body.inverseMass; - acceleration += uf::physics::getGravity( body ); // apply gravity + pod::Vector3f acceleration = (body.forceAccumulator * body.inverseMass); + acceleration += gravity; // apply gravity + + acceleration = acceleration * body.linearFactor; body.velocity += acceleration * dt; + body.velocity = body.velocity * body.linearFactor; // angular integration { pod::Matrix3f R = uf::quaternion::matrix3( fT.orientation ); pod::Vector3f localTorque = uf::matrix::multiply( uf::matrix::transpose(R), body.torqueAccumulator ); - pod::Vector3f localAngAccel = localTorque * body.inverseInertiaTensor; // element-wise + + pod::Vector3f localAngAccel = (localTorque * body.inverseInertiaTensor) * body.angularFactor; body.angularVelocity += uf::matrix::multiply( R, localAngAccel ) * dt; + body.angularVelocity = body.angularVelocity * body.angularFactor; } // update position