From da142573c610325ef16459ce72ae6b1ee16c28a5 Mon Sep 17 00:00:00 2001 From: Louis Langholtz Date: Tue, 10 Oct 2023 23:19:40 -0600 Subject: [PATCH] Changes to reduce AabbTreeWorld cognitive complexity --- Library/source/playrho/d2/AabbTreeWorld.cpp | 288 +++++++++++--------- 1 file changed, 153 insertions(+), 135 deletions(-) diff --git a/Library/source/playrho/d2/AabbTreeWorld.cpp b/Library/source/playrho/d2/AabbTreeWorld.cpp index 6a4cdd2075..7c2ea2d810 100644 --- a/Library/source/playrho/d2/AabbTreeWorld.cpp +++ b/Library/source/playrho/d2/AabbTreeWorld.cpp @@ -394,10 +394,7 @@ bool FlagForFiltering(ObjectPool& contactBuffer, BodyID bodyA, auto anyFlagged = false; for (const auto& ci: contactsBodyB) { auto& contact = contactBuffer[to_underlying(std::get(ci))]; - const auto bA = GetBodyA(contact); - const auto bB = GetBodyB(contact); - const auto other = (bA != bodyB)? bA: bB; - if (other == bodyA) { + if (GetOtherBody(contact, bodyB) == bodyA) { // Flag the contact for filtering at the next time step (where either // body is awake). contact.FlagForFiltering(); @@ -535,6 +532,13 @@ auto FindTypeValue(const std::vector& container, const Value& value) return (it != last)? std::optional{it}: std::optional{}; } +template +auto Find(const C& c, const V& value) -> decltype(find(begin(c), end(c), value), bool{}) +{ + const auto last = end(c); + return find(begin(c), last, value) != last; +} + void Erase(BodyContactIDs& contacts, const std::function& callback) { auto last = end(contacts); @@ -609,29 +613,38 @@ auto ForMatchingProxies(const DynamicTree& tree, ShapeID shapeId, Function f) std::pair, std::vector> GetOldAndNewShapeIDs(const Body& oldBody, const Body& newBody) { - auto oldShapeIds = std::vector{}; - auto newShapeIds = std::vector{}; - auto oldmap = std::map{}; - auto newmap = std::map{}; - for (auto&& i: oldBody.GetShapes()) { - ++oldmap[i]; - --newmap[i]; - } - for (auto&& i: newBody.GetShapes()) { - --oldmap[i]; - ++newmap[i]; - } - for (auto&& entry: oldmap) { - for (auto i = 0; i < entry.second; ++i) { - oldShapeIds.push_back(entry.first); + if (IsEnabled(oldBody) && IsEnabled(newBody)) { + auto oldShapeIds = std::vector{}; + auto newShapeIds = std::vector{}; + auto oldmap = std::map{}; + auto newmap = std::map{}; + for (auto&& i: oldBody.GetShapes()) { + ++oldmap[i]; + --newmap[i]; + } + for (auto&& i: newBody.GetShapes()) { + --oldmap[i]; + ++newmap[i]; + } + for (auto&& entry: oldmap) { + for (auto i = 0; i < entry.second; ++i) { + oldShapeIds.push_back(entry.first); + } } - } - for (auto&& entry: newmap) { - for (auto i = 0; i < entry.second; ++i) { - newShapeIds.push_back(entry.first); + for (auto&& entry: newmap) { + for (auto i = 0; i < entry.second; ++i) { + newShapeIds.push_back(entry.first); + } } + return {oldShapeIds, newShapeIds}; + } + if (IsEnabled(newBody)) { + return {std::vector{}, newBody.GetShapes()}; } - return std::make_pair(oldShapeIds, newShapeIds); + if (IsEnabled(oldBody)) { + return {oldBody.GetShapes(), std::vector{}}; + } + return {}; } template @@ -726,6 +739,68 @@ auto GetProxyKeysOpts(const WorldConf& conf) -> pmr::PoolMemoryOptions return {conf.reserveBuffers, conf.reserveContactKeys * sizeof(AabbTreeWorld::ProxyKey)}; } +auto IsGeomChanged(const Shape& shape0, const Shape& shape1) -> bool +{ + const auto numKids0 = GetChildCount(shape0); + const auto numKids1 = GetChildCount(shape1); + if (numKids0 != numKids1) { + return true; + } + for (auto child = 0u; child < numKids1; ++child) { + const auto distanceProxy0 = GetChild(shape0, child); + const auto distanceProxy1 = GetChild(shape1, child); + if (distanceProxy0 != distanceProxy1) { + return true; + } + } + return false; +} + +auto Append(std::vector>& fixtures, + BodyID bodyId, Span shapeIds) -> void +{ + for (const auto shapeId: shapeIds) { + fixtures.emplace_back(bodyId, shapeId); + } +} + +template +auto Validate(const Container& container, const Span& ids) +-> decltype(container.at(to_underlying(ElementType{})), std::declval()) +{ + for (const auto& id: ids) { + container.at(to_underlying(id)); + } +} + +auto SetIsActive(ObjectPool& contacts, + const Span>& bodyContacts) -> void +{ + for (const auto& elem: bodyContacts) { + contacts[to_underlying(std::get(elem))].SetIsActive(); + } +} + +auto UnsetIsActive(ObjectPool& contacts, // force newline + const Span>& bodyContacts, + BodyID id, // force newline + const Span& bodies) -> void +{ + // sleep associated contacts whose other body is also asleep + for (const auto& elem: bodyContacts) { + auto& contact = contacts[to_underlying(std::get(elem))]; + if (!bodies[to_underlying(GetOtherBody(contact, id))].IsAwake()) { + contact.UnsetIsActive(); + } + } +} + +auto SetAwake(ObjectPool& bodies, const Contact& c) -> void +{ + bodies[to_underlying(GetBodyA(c))].SetAwake(); + bodies[to_underlying(GetBodyB(c))].SetAwake(); +} + } // anonymous namespace AabbTreeWorld::AabbTreeWorld(const WorldConf& conf): @@ -881,10 +956,7 @@ BodyID CreateBody(AabbTreeWorld& world, Body body) if (size(world.m_bodies) >= MaxBodies) { throw LengthError("CreateBody: operation would exceed MaxBodies"); } - // confirm all shapeIds are valid... - for (const auto& shapeId: body.GetShapes()) { - world.m_shapeBuffer.at(to_underlying(shapeId)); - } + Validate(world.m_shapeBuffer, Span(body.GetShapes())); const auto id = static_cast( static_cast(world.m_bodyBuffer.Allocate(std::move(body)))); world.m_islanded.bodies.resize(size(world.m_bodyBuffer)); @@ -897,10 +969,7 @@ BodyID CreateBody(AabbTreeWorld& world, Body body) world.m_bodies.push_back(id); const auto& bufferedBody = world.m_bodyBuffer[to_underlying(id)]; if (IsEnabled(bufferedBody)) { - const auto shapes = bufferedBody.GetShapes(); - for (const auto& shapeId: shapes) { - world.m_fixturesForProxies.emplace_back(id, shapeId); - } + Append(world.m_fixturesForProxies, id, bufferedBody.GetShapes()); } return id; } @@ -1153,65 +1222,47 @@ void SetShape(AabbTreeWorld& world, ShapeID id, Shape def) // NOLINT(readability if (world.m_shapeBuffer.FindFree(to_underlying(id))) { throw InvalidArgument(idIsDestroyedMsg); } - const auto geometryChanged = [](const Shape& shape0, const Shape& shape1){ - const auto numKids0 = GetChildCount(shape0); - const auto numKids1 = GetChildCount(shape1); - if (numKids0 != numKids1) { - return true; - } - for (auto child = 0u; child < numKids1; ++child) { - const auto distanceProxy0 = GetChild(shape0, child); - const auto distanceProxy1 = GetChild(shape1, child); - if (distanceProxy0 != distanceProxy1) { - return true; - } - } - return false; - }(shape, def); + const auto geometryChanged = IsGeomChanged(shape, def); for (auto&& b: world.m_bodyBuffer) { - const auto bodyId = BodyID(static_cast(&b - data(world.m_bodyBuffer))); - for (const auto& shapeId: b.GetShapes()) { - if (shapeId == id) { - SetAwake(b); - if (geometryChanged && IsEnabled(b)) { - auto& bodyProxies = world.m_bodyProxies[to_underlying(bodyId)]; - const auto lastProxy = end(bodyProxies); - bodyProxies.erase(std::remove_if(begin(bodyProxies), lastProxy, - [&world,shapeId](DynamicTree::Size idx){ - const auto leafData = world.m_tree.GetLeafData(idx); - if (leafData.shapeId == shapeId) { - world.m_tree.DestroyLeaf(idx); - EraseFirst(world.m_proxiesForContacts, idx); - return true; - } - return false; - }), lastProxy); - // Destroy any contacts associated with the fixture. - Erase(world.m_bodyContacts[to_underlying(bodyId)], [&world,bodyId,shapeId,&b](ContactID contactID) { - const auto& contact = world.m_contactBuffer[to_underlying(contactID)]; - if (!IsFor(contact, bodyId, shapeId)) { - return false; - } - world.Destroy(contactID, &b); - return true; - }); - const auto fixture = std::make_pair(bodyId, shapeId); - EraseAll(world.m_fixturesForProxies, fixture); - DestroyProxies(world.m_tree, FindProxies(world.m_tree, bodyId, shapeId), world.m_proxiesForContacts); - world.m_fixturesForProxies.push_back(fixture); + if (!Find(b.GetShapes(), id)) { + continue; + } + SetAwake(b); + if (geometryChanged && IsEnabled(b)) { + const auto bodyId = BodyID(static_cast(&b - data(world.m_bodyBuffer))); + auto& bodyProxies = world.m_bodyProxies[to_underlying(bodyId)]; + const auto lastProxy = end(bodyProxies); + bodyProxies.erase(std::remove_if(begin(bodyProxies), lastProxy, + [&world,id](DynamicTree::Size idx){ + const auto leafData = world.m_tree.GetLeafData(idx); + if (leafData.shapeId == id) { + world.m_tree.DestroyLeaf(idx); + EraseFirst(world.m_proxiesForContacts, idx); + return true; } - } + return false; + }), lastProxy); + // Destroy any contacts associated with the fixture. + Erase(world.m_bodyContacts[to_underlying(bodyId)], [&world,bodyId,id,&b](ContactID contactID) { + const auto& contact = world.m_contactBuffer[to_underlying(contactID)]; + if (!IsFor(contact, bodyId, id)) { + return false; + } + world.Destroy(contactID, &b); + return true; + }); + const auto fixture = std::make_pair(bodyId, id); + EraseAll(world.m_fixturesForProxies, fixture); + DestroyProxies(world.m_tree, FindProxies(world.m_tree, bodyId, id), world.m_proxiesForContacts); + world.m_fixturesForProxies.push_back(fixture); } } if (GetFilter(shape) != GetFilter(def)) { auto anyNeedFiltering = false; for (auto& c: world.m_contactBuffer) { - const auto shapeIdA = GetShapeA(c); - const auto shapeIdB = GetShapeB(c); - if (shapeIdA == id || shapeIdB == id) { - c.FlagForFiltering(); - world.m_bodyBuffer[to_underlying(GetBodyA(c))].SetAwake(); - world.m_bodyBuffer[to_underlying(GetBodyB(c))].SetAwake(); + if (IsFor(c, id)) { + FlagForFiltering(c); + SetAwake(world.m_bodyBuffer, c); anyNeedFiltering = true; } } @@ -1225,15 +1276,13 @@ void SetShape(AabbTreeWorld& world, ShapeID id, Shape def) // NOLINT(readability if ((IsSensor(shape) != IsSensor(def)) || (GetFriction(shape) != GetFriction(def)) || (GetRestitution(shape) != GetRestitution(def)) || geometryChanged) { for (auto&& c: world.m_contactBuffer) { - if (GetShapeA(c) == id || GetShapeB(c) == id) { - c.FlagForUpdating(); - world.m_bodyBuffer[to_underlying(GetBodyA(c))].SetAwake(); - world.m_bodyBuffer[to_underlying(GetBodyB(c))].SetAwake(); + if (IsFor(c, id)) { + FlagForUpdating(c); + SetAwake(world.m_bodyBuffer, c); } } } world.m_shapeBuffer[to_underlying(id)] = std::move(def); - // TODO: anything else that needs doing? } void AabbTreeWorld::AddToIsland(Island& island, BodyID seedID, @@ -1313,9 +1362,7 @@ void AabbTreeWorld::AddContactsToIsland(Island& island, BodyStack& stack, const auto& contact = m_contactBuffer[to_underlying(contactID)]; if (IsEnabled(contact) && IsTouching(contact) && !IsSensor(contact)) { - const auto bodyA = GetBodyA(contact); - const auto bodyB = GetBodyB(contact); - const auto other = (bodyID != bodyA)? bodyA: bodyB; + const auto other = GetOtherBody(contact, bodyID); island.contacts.push_back(contactID); m_islanded.contacts[to_underlying(contactID)] = true; if (!m_islanded.bodies[to_underlying(other)]) @@ -1937,9 +1984,7 @@ AabbTreeWorld::ProcessContactsForTOI( // NOLINT(readability-function-cognitive-c if (!m_islanded.contacts[to_underlying(contactID)]) { auto& contact = m_contactBuffer[to_underlying(contactID)]; if (!contact.IsSensor()) { - const auto bodyIdA = GetBodyA(contact); - const auto bodyIdB = GetBodyB(contact); - const auto otherId = (bodyIdA != id)? bodyIdA: bodyIdB; + const auto otherId = GetOtherBody(contact, id); auto& other = m_bodyBuffer[to_underlying(otherId)]; if (bodyImpenetrable || IsImpenetrable(other)) { const auto otherIslanded = m_islanded.bodies[to_underlying(otherId)]; @@ -2582,16 +2627,14 @@ void AabbTreeWorld::Update( // NOLINT(readability-function-cognitive-complexity) } } -void SetBody(AabbTreeWorld& world, BodyID id, Body value) // NOLINT(readability-function-cognitive-complexity) +void SetBody(AabbTreeWorld& world, BodyID id, Body value) { if (IsLocked(world)) { throw WrongState(worldIsLockedMsg); } - // confirm id and all shapeIds are valid... + // Validate id and all the new body's shapeIds... const auto& body = world.m_bodyBuffer.at(to_underlying(id)); - for (const auto& shapeId: value.GetShapes()) { - world.m_shapeBuffer.at(to_underlying(shapeId)); - } + Validate(world.m_shapeBuffer, Span(value.GetShapes())); if (world.m_bodyBuffer.FindFree(to_underlying(id))) { throw InvalidArgument(idIsDestroyedMsg); } @@ -2619,27 +2662,13 @@ void SetBody(AabbTreeWorld& world, BodyID id, Body value) // NOLINT(readability- break; } } - auto oldShapeIds = std::vector{}; - auto newShapeIds = std::vector{}; - if (IsEnabled(value) && IsEnabled(body)) { - auto result = GetOldAndNewShapeIDs(body, value); - oldShapeIds = std::move(result.first); - newShapeIds = std::move(result.second); - } - else if (IsEnabled(value)) { - newShapeIds = value.GetShapes(); - } - else if (IsEnabled(body)) { - oldShapeIds = body.GetShapes(); - } - if (!empty(oldShapeIds)) { + const auto shapeIds = GetOldAndNewShapeIDs(body, value); + if (!empty(shapeIds.first)) { auto& bodyProxies = world.m_bodyProxies[to_underlying(id)]; const auto lastProxy = end(bodyProxies); bodyProxies.erase(std::remove_if(begin(bodyProxies), lastProxy, - [&world,&oldShapeIds](DynamicTree::Size idx){ - const auto leafData = world.m_tree.GetLeafData(idx); - const auto last = end(oldShapeIds); - if (std::find(begin(oldShapeIds), last, leafData.shapeId) != last) { + [&world,&shapeIds](DynamicTree::Size idx){ + if (Find(shapeIds.first, world.m_tree.GetLeafData(idx).shapeId)) { world.m_tree.DestroyLeaf(idx); EraseFirst(world.m_proxiesForContacts, idx); return true; @@ -2647,7 +2676,7 @@ void SetBody(AabbTreeWorld& world, BodyID id, Body value) // NOLINT(readability- return false; }), lastProxy); } - for (auto&& shapeId: oldShapeIds) { + for (auto&& shapeId: shapeIds.first) { // Destroy any contacts associated with the fixture. Erase(world.m_bodyContacts[to_underlying(id)], [&world,id,shapeId,&body](ContactID contactID) { const auto& contact = world.m_contactBuffer[to_underlying(contactID)]; @@ -2660,29 +2689,18 @@ void SetBody(AabbTreeWorld& world, BodyID id, Body value) // NOLINT(readability- EraseAll(world.m_fixturesForProxies, std::make_pair(id, shapeId)); DestroyProxies(world.m_tree, FindProxies(world.m_tree, id, shapeId), world.m_proxiesForContacts); } - for (auto&& shapeId: newShapeIds) { - world.m_fixturesForProxies.emplace_back(id, shapeId); - } + Append(world.m_fixturesForProxies, id, shapeIds.second); if (GetTransformation(body) != GetTransformation(value)) { FlagForUpdating(world.m_contactBuffer, world.m_bodyContacts[to_underlying(id)]); addToBodiesForSync = true; } if (IsAwake(body) != IsAwake(value)) { - // Update associated contacts if (IsAwake(value)) { - for (const auto& elem: world.m_bodyContacts[to_underlying(id)]) { - world.m_contactBuffer[to_underlying(std::get(elem))].SetIsActive(); - } + SetIsActive(world.m_contactBuffer, world.m_bodyContacts[to_underlying(id)]); } - else { // sleep associated contacts whose other body is also asleep - for (const auto& elem: world.m_bodyContacts[to_underlying(id)]) { - auto& contact = world.m_contactBuffer[to_underlying(std::get(elem))]; - const auto otherID = (GetBodyA(contact) != id) - ? GetBodyA(contact): GetBodyB(contact); - if (!world.m_bodyBuffer[to_underlying(otherID)].IsAwake()) { - contact.UnsetIsActive(); - } - } + else { + UnsetIsActive(world.m_contactBuffer, world.m_bodyContacts[to_underlying(id)], + id, world.m_bodyBuffer); } } if (addToBodiesForSync) {