Repository: rdiankov/openrave
Pull request #1500: Avoid N^2 loops in UpdateFromInfo
Author: login: rschlaikjer
Created: 2025-04-03T08:32:39Z | Merged: 2025-05-14T03:56:31Z

Previously, searching for bodies with the same name/id would do a linear search on the bodies in the environment.
Replace with hash lookups to avoid slow iteration for high-body-count environments.

Test cases to confirm behaviour matches existing production have been added in testopenrave MR 141.




Diff:
diff --git a/CMakeLists.txt b/CMakeLists.txt
index 775449971d..d0fb7e69f9 100644
--- a/CMakeLists.txt
+++ b/CMakeLists.txt
@@ -5,7 +5,7 @@ set( CMAKE_ALLOW_LOOSE_LOOP_CONSTRUCTS TRUE )
 # Define here the needed parameters
 set (OPENRAVE_VERSION_MAJOR 0)
 set (OPENRAVE_VERSION_MINOR 167)
-set (OPENRAVE_VERSION_PATCH 0)
+set (OPENRAVE_VERSION_PATCH 1)
 set (OPENRAVE_VERSION ${OPENRAVE_VERSION_MAJOR}.${OPENRAVE_VERSION_MINOR}.${OPENRAVE_VERSION_PATCH})
 set (OPENRAVE_SOVERSION ${OPENRAVE_VERSION_MAJOR}.${OPENRAVE_VERSION_MINOR})
 message(STATUS "Compiling OpenRAVE Version ${OPENRAVE_VERSION}, soversion=${OPENRAVE_SOVERSION}")
diff --git a/docs/source/changelog.rst b/docs/source/changelog.rst
index a31625a43b..206218613e 100644
--- a/docs/source/changelog.rst
+++ b/docs/source/changelog.rst
@@ -3,6 +3,11 @@
 ChangeLog
 #########
 
+Version 0.167.1
+===============
+
+- Avoid N^2 loops in UpdateFromInfo.
+
 Version 0.167.0
 ===============
 
@@ -10,6 +15,7 @@ Version 0.167.0
 
 Version 0.166.1
 ===============
+
 - Add missing enum to `PlannerStatusCode` python binding
 
 Version 0.166.0
@@ -21,7 +27,7 @@ Version 0.166.0
 Version 0.165.0
 ===============
 
-- Replace `serialize` used for hashing with `DigestHsah`
+- Replace `serialize` used for hashing with `DigestHash`.
 
 Version 0.164.0
 ===============
diff --git a/src/libopenrave-core/environment-core.h b/src/libopenrave-core/environment-core.h
index 952e1facf7..c2c308fc2e 100644
--- a/src/libopenrave-core/environment-core.h
+++ b/src/libopenrave-core/environment-core.h
@@ -66,7 +66,8 @@ inline void EnsureVectorSize(std::vector<T>& vec, size_t size)
     }
 }
 
-class TimedUniqueLock : public std::unique_lock<std::timed_mutex> {
+class TimedUniqueLock : public std::unique_lock<std::timed_mutex>
+{
 public:
     /**
      * Try to lock a mutex for a given duration if the duration is a positive value. Otherwise it waits for the lock without a timeout.
@@ -82,7 +83,8 @@ class TimedUniqueLock : public std::unique_lock<std::timed_mutex> {
     }
 };
 
-class TimedSharedLock : public std::shared_lock<std::shared_timed_mutex> {
+class TimedSharedLock : public std::shared_lock<std::shared_timed_mutex>
+{
 public:
     /**
      * Try to lock a mutex for a given duration if the duration is a positive value. Otherwise it waits for the lock without a timeout.
@@ -98,7 +100,8 @@ class TimedSharedLock : public std::shared_lock<std::shared_timed_mutex> {
     }
 };
 
-class TimedExclusiveLock {
+class TimedExclusiveLock
+{
 public:
     /**
      * Try to exclusively lock a mutex for a given duration if the duration is a positive value. Otherwise it waits for the lock without a timeout.
@@ -3002,153 +3005,146 @@ class Environment : public EnvironmentBase
 
         }
 
-        // make a copy of _vecbodies because we will be doing some reordering
-        std::vector<KinBodyPtr> vBodies;
-        std::list<KinBodyPtr> listBodiesTemporarilyRenamed; // in order to avoid clashing names, sometimes bodies are renamed temporariliy and placed in this list. If they targetted again with a different name, then they are put back into the environment
-        {
-            SharedLock lock533(_mutexInterfaces);
-            vBodies = _vecbodies;
+        // If the environment is large but we are only updating a small number of bodies, building a full lookup table is expensive relative to the amount of lookups we actually do.
+        // We can reduce this somewhat by pre-filtering the set of names/ids that could ever be looked up, and only including those in our lookup table.
+        // This incurs (info._vBodyInfos.size() + vBodies.size()) more hash _lookups_, but means that we save up to (vBodies.size() - info._vBodyInfos.size()) _allocations_.
+        // Since we expect vBodies.size() >> info._vBodyInfos.size() in most cases, and allocations are more costly than lookups, this is overall a speedup despite the extra steps.
+        std::unordered_set<string_view> bodyIdsToUpdate; // string_views are over info._vBodyInfos, guaranteed lifetime for the rest of this function
+        std::unordered_set<string_view> bodyNamesToUpdate;
+        for (const KinBody::KinBodyInfoPtr& pKinBodyInfo : info._vBodyInfos) {
+            // If we have more than one info that targets the same id / body name, throw - UpdateFromInfo is not designed to update the same object more than once in a call
+            if (!pKinBodyInfo->_id.empty() && !bodyIdsToUpdate.emplace(pKinBodyInfo->_id).second) {
+                throw OPENRAVE_EXCEPTION_FORMAT(_("Duplicate body info ID '%s' in call to UpdateFromInfo"), pKinBodyInfo->_id, ORE_InvalidArguments);
+            }
+            if (!pKinBodyInfo->_name.empty() && !bodyNamesToUpdate.emplace(pKinBodyInfo->_name).second) {
+                throw OPENRAVE_EXCEPTION_FORMAT(_("Duplicate body info name '%s' in call to UpdateFromInfo"), pKinBodyInfo->_name, ORE_InvalidArguments);
+            }
         }
+
+        std::list<KinBodyPtr> listBodiesTemporarilyRenamed; // in order to avoid clashing names, sometimes bodies are renamed temporarily and placed in this list. If they targeted again with a different name, then they are put back into the environment
+        // Build a lookup table for the bodies available to be matched against from vBodies
+        // When trying to identify candidate matches, we can do two hash lookups instead of a sequential scan.
+        // If we match an existing body, then we just need to remove the id/name maps that linked to it to prevent it getting matched again later.
+        // These maps use string_views over the bodies in the environment; these bodies _are_ removed during iteration; care must be taken to erase the matching entries.
+        std::unordered_map<string_view, KinBodyPtr> mapExistingBodiesById; // Map of body id -> existing body in the environment
+        std::unordered_map<string_view, KinBodyPtr> mapExistingBodiesByName; // Map of body name -> existing body in the environment
+
         {
-            for (std::vector<KinBodyPtr>::iterator it = vBodies.begin(); it != vBodies.end(); ) {
-                if (!*it) {
-                    it = vBodies.erase(it);
+            SharedLock lock533(_mutexInterfaces);
+            for (size_t bodyIndex = 0; bodyIndex < _vecbodies.size(); bodyIndex++) {
+                // It's possible for holes to exist in vBodies since when bodies are removed from the environment their entries in _vecbodies are just nulled out.
+                if (!_vecbodies[bodyIndex]) {
+                    continue;
                 }
-                else {
-                    it++;
+
+                // If this body doesn't have a name or ID that _can_ be matched, there's no need to add a record for it in our lookup table.
+                const KinBody& body = *_vecbodies[bodyIndex];
+                if (bodyIdsToUpdate.find(body.GetId()) == bodyIdsToUpdate.end() && bodyNamesToUpdate.find(body.GetName()) == bodyNamesToUpdate.end()) {
+                    continue;
                 }
+                mapExistingBodiesById[body.GetId()] = _vecbodies[bodyIndex];
+                mapExistingBodiesByName[body.GetName()] = _vecbodies[bodyIndex];
             }
         }
 
-        // Set of indices that have already been used for vBodies
-        std::unordered_set<int> usedBodyIndexSet;
+        // Keep a list of the bodies that we have matched to infos - in the event that the caller did _not_ specify OnlySpecifiedBodiesExact,
+        // when we are finished we will want to remove all bodies in the environment that are _not_ in this list.
+        // Store raw pointers here instead of shared pointers because we don't need to track ownership, just membership.
+        std::unordered_set<KinBody*> setBodiesWithMatchingInfos;
 
         // internally manipulates _vecbodies using _AddKinBody/_AddRobot/_RemoveKinBodyFromIterator
-        for(int inputBodyIndex = 0; inputBodyIndex < (int)info._vBodyInfos.size(); ++inputBodyIndex) {
-            const KinBody::KinBodyInfoConstPtr& pKinBodyInfo = info._vBodyInfos[inputBodyIndex];
+        for(const KinBody::KinBodyInfoPtr& pKinBodyInfo : info._vBodyInfos) {
             const KinBody::KinBodyInfo& kinBodyInfo = *pKinBodyInfo;
             RAVELOG_VERBOSE_FORMAT("env=%s, id '%s', name '%s', _vGrabbedInfos=%d", GetNameId()%pKinBodyInfo->_id%pKinBodyInfo->_name%pKinBodyInfo->_vGrabbedInfos.size());
             RobotBase::RobotBaseInfoConstPtr pRobotBaseInfo = OPENRAVE_DYNAMIC_POINTER_CAST<const RobotBase::RobotBaseInfo>(pKinBodyInfo);
-            KinBodyPtr pMatchExistingBody; // matches to pKinBodyInfo
-            int bodyIndex = -1; // index to vBodies to use. -1 if not used
-            {
-                // find existing body in the env
-                std::vector<KinBodyPtr>::iterator itExistingSameId = vBodies.end();
-                std::vector<KinBodyPtr>::iterator itExistingSameName = vBodies.end();
-                std::vector<KinBodyPtr>::iterator itExistingSameIdName = vBodies.end();
-
-                if( updateMode == UFIM_OnlySpecifiedBodiesExact ) {
-                    // can be any of the bodies, but have to make sure not to overlap
-                    //bodyIndex = inputBodyIndex;
-                    // search only in the unprocessed part of vBodies
-                    for (int ibody = 0; ibody < (int)vBodies.size(); ++ibody) {
-                        if (usedBodyIndexSet.find(ibody) != usedBodyIndexSet.end()) {
-                            continue;
-                        }
-                        const KinBodyPtr& pbody = vBodies[ibody];
-                        if (!pbody) {
-                            continue;
-                        }
-                        bool bIdMatch = !pbody->_id.empty() && pbody->_id == kinBodyInfo._id;
-                        bool bNameMatch = !pbody->_name.empty() && pbody->_name == kinBodyInfo._name;
-                        if( bIdMatch && bNameMatch ) {
-                            itExistingSameIdName = itExistingSameId = itExistingSameName = vBodies.begin() + ibody;
-                            break;
-                        }
-                        if( bIdMatch && itExistingSameId == vBodies.end() ) {
-                            itExistingSameId = vBodies.begin() + ibody;
-                        }
-                        if( bNameMatch && itExistingSameName == vBodies.end() ) {
-                            itExistingSameName = vBodies.begin() + ibody;
-                        }
-                    }
+
+            // Try and match this body info to an existing body
+            KinBodyPtr pMatchExistingBody; // Will be loaded with the existing body to update, if any
+            do {
+                // Check if we have bodies with matching names / ids
+                const std::unordered_map<string_view, KinBodyPtr>::iterator itExistingBodyById = mapExistingBodiesById.find(kinBodyInfo._id);
+                const std::unordered_map<string_view, KinBodyPtr>::iterator itExistingBodyByName = mapExistingBodiesByName.find(kinBodyInfo._name);
+
+                // If we do, preemptively move those matching bodies _out_ of our lookup tables
+                // Note that if ID and name matched two _different_ bodies, we need to do something with the body that has the same name.
+                KinBodyPtr pMatchExistingBodySameId, pMatchExistingBodySameName;
+                if (itExistingBodyById != mapExistingBodiesById.end()) {
+                    pMatchExistingBodySameId = std::move(itExistingBodyById->second);
                 }
-                else {
-                    bodyIndex = inputBodyIndex;
-                    // search only in the unprocessed part of vBodies
-                    if( (int)vBodies.size() > inputBodyIndex ) {
-                        for (std::vector<KinBodyPtr>::iterator itBody = vBodies.begin() + inputBodyIndex; itBody != vBodies.end(); ++itBody) {
-                            if (!(*itBody)) {
-                                continue;
-                            }
-                            bool bIdMatch = !(*itBody)->_id.empty() && (*itBody)->_id == kinBodyInfo._id;
-                            bool bNameMatch = !(*itBody)->_name.empty() && (*itBody)->_name == kinBodyInfo._name;
-                            if( bIdMatch && bNameMatch ) {
-                                itExistingSameIdName = itBody;
-                                itExistingSameId = itBody;
-                                itExistingSameName = itBody;
-                                break;
-                            }
-                            if( bIdMatch && itExistingSameId == vBodies.end() ) {
-                                itExistingSameId = itBody;
-                            }
-                            if( bNameMatch && itExistingSameName == vBodies.end() ) {
-                                itExistingSameName = itBody;
-                            }
-                        }
-                    }
+                if (itExistingBodyByName != mapExistingBodiesByName.end()) {
+                    pMatchExistingBodySameName = std::move(itExistingBodyByName->second);
                 }
 
-                std::vector<KinBodyPtr>::iterator itExisting = itExistingSameIdName;
-                if( itExisting == vBodies.end() ) {
-                    itExisting = itExistingSameId;
-                }
-                if( itExisting == vBodies.end() ) {
-                    itExisting = itExistingSameName;
+                // Pick the best existing body index based on our resolution order
+                pMatchExistingBody = !!pMatchExistingBodySameId ? pMatchExistingBodySameId : pMatchExistingBodySameName;
+
+                // If we didn't match anything, we have to just create a new body based on this info
+                if (!pMatchExistingBody) {
+                    break;
                 }
 
-                // check if interface type changed, if so, remove the body and treat it as a new body
-                if (itExisting != vBodies.end()) {
-                    KinBodyPtr pBody = *itExisting;
-                    bool bInterfaceMatches = pBody->GetXMLId() == pKinBodyInfo->_interfaceType;
-                    if( !bInterfaceMatches || pBody->IsRobot() != pKinBodyInfo->_isRobot ) {
-                        RAVELOG_VERBOSE_FORMAT("env=%s, body '%s' interface is changed, remove old body from environment. xmlid=%s, _interfaceType=%s, isRobot %d != %d", GetNameId()%pBody->_id%pBody->GetXMLId()%pKinBodyInfo->_interfaceType%pBody->IsRobot()%pKinBodyInfo->_isRobot);
-                        itExisting = vBodies.end();
-                        vRemovedBodies.push_back(pBody);
-
-                        ExclusiveLock lock690(_mutexInterfaces);
-                        vector<KinBodyPtr>::iterator itBodyToRemove = std::find(_vecbodies.begin(), _vecbodies.end(), pBody);
-                        if( itBodyToRemove != _vecbodies.end() ) {
-                            _InvalidateKinBodyFromEnvBodyIndex(pBody->GetEnvironmentBodyIndex());
-                        }
-                    }
+                // If we matched a body to reuse, clear the mappings so that we can't double-process it
+                // This also ensures no dangling string_views in the event this body gets destroyed later in the loop
+                mapExistingBodiesById.erase(pMatchExistingBody->GetId());
+                mapExistingBodiesByName.erase(pMatchExistingBody->GetName());
+
+                bool bNameMatchesForDifferentBody = !!pMatchExistingBodySameName && pMatchExistingBody != pMatchExistingBodySameName;
+                if (bNameMatchesForDifferentBody) {
+                    RAVELOG_DEBUG_FORMAT("env=%s, have to clear body name '%s' id=%s for loading body with id=%s", GetNameId() % pMatchExistingBodySameName->GetName() % pMatchExistingBodySameName->GetId() % pMatchExistingBody->GetId());
                 }
 
-                if( itExisting != vBodies.end() ) {
-                    if( itExisting != itExistingSameName && itExistingSameName != vBodies.end() ) {
-                        // new name will conflict with *itExistingSameName, so should change the names to something temporarily
-                        // for now, clear since the body should be processed later again
-                        RAVELOG_DEBUG_FORMAT("env=%s, have to clear body name '%s' id=%s for loading body with id=%s", GetNameId()%(*itExistingSameName)->GetName()%(*itExistingSameName)->GetId()%(*itExisting)->GetId());
-                        (*itExistingSameName)->SetName(_GetUniqueName((*itExistingSameName)->GetName()+"_tempRenamedDueToConflict_"));
-                        listBodiesTemporarilyRenamed.push_back(*itExistingSameName);
-                    }
-                    pMatchExistingBody = *itExisting;
-                    int nMatchingIndex = itExisting-vBodies.begin();
-                    if ( bodyIndex >= 0 && bodyIndex != nMatchingIndex) {
-                        // re-arrange vBodies according to the order of infos
-                        KinBodyPtr pTempBody = vBodies.at(bodyIndex);
-                        vBodies.at(bodyIndex) = pMatchExistingBody;
-                        *itExisting = pTempBody;
+                // If we matched an existing body, but that body is of a different interface to the info (e.g robot vs plain body), then we can't just update, need to recreate it.
+                bool bInterfaceMatches = pMatchExistingBody->GetXMLId() == pKinBodyInfo->_interfaceType;
+                if (!bInterfaceMatches || pMatchExistingBody->IsRobot() != pKinBodyInfo->_isRobot) {
+                    KinBodyPtr pBodyToRemove = std::move(pMatchExistingBody); // Invalidates existing body output
+                    RAVELOG_VERBOSE_FORMAT("env=%s, body '%s' interface is changed, remove old body from environment. xmlid=%s, _interfaceType=%s, isRobot %d != %d", GetNameId() % pBodyToRemove->_id % pBodyToRemove->GetXMLId() % pKinBodyInfo->_interfaceType % pBodyToRemove->IsRobot() % pKinBodyInfo->_isRobot);
+                    vRemovedBodies.push_back(pBodyToRemove);
+
+                    ExclusiveLock lock690(_mutexInterfaces);
+                    vector<KinBodyPtr>::iterator itBodyToRemove = std::find(_vecbodies.begin(), _vecbodies.end(), pBodyToRemove);
+                    if (itBodyToRemove != _vecbodies.end()) {
+                        _InvalidateKinBodyFromEnvBodyIndex(pBodyToRemove->GetEnvironmentBodyIndex());
                     }
+                }
 
-                    if (updateMode == UFIM_OnlySpecifiedBodiesExact) {
-                        usedBodyIndexSet.emplace(nMatchingIndex);
-                    }
+                // If we matched by ID instead of name, it's possible there exists another body in the env with the same name as our body info.
+                // This would cause a conflict if we try and update our current (id matched body) to have the same name.
+                // Since the other body with the same name might get processed again later (by id?), temporarily rename it so that we can continue.
+                if (bNameMatchesForDifferentBody) {
+                    // Since we are renaming a body, and our existing body indices map uses string views over our body names, need to make sure we remove this body's entry / add it back after rename
+                    mapExistingBodiesByName.erase(itExistingBodyByName); // assuming itExistingBodyByName is still valid since it has a different name
+                    pMatchExistingBodySameName->SetName(_GetUniqueName(pMatchExistingBodySameName->GetName() + "_tempRenamedDueToConflict_"));
+                    mapExistingBodiesByName.emplace(pMatchExistingBodySameName->GetName(), pMatchExistingBodySameName); // restores from the std::move
+                    listBodiesTemporarilyRenamed.push_back(pMatchExistingBodySameName);
                 }
-            }
+            } while (0);
 
             KinBodyPtr pInitBody; // body that has to be Init() again
+
+            // In the event that we have to remove the body from the environment to reinitialize it, we need to cache which bodies were grabbing it
+            // Each column of these vectors constitutes a 4-tuple describing a grab - the grabber, the grabbing link, the grab info, and the link indices to ignore.
             std::vector<KinBodyPtr> pGrabbingBodies;
             std::vector<KinBody::LinkPtr> pGrabbingLinks;
             std::vector<rapidjson::Document> rGrabbedUserDataDocuments;
-            std::vector<std::set<int>> linkIndicesToIgnore;
+            std::vector<std::set<int> > linkIndicesToIgnore;
 
+            // Were we able to match an existing body?
             if( !!pMatchExistingBody ) {
-                listBodiesTemporarilyRenamed.remove(pMatchExistingBody); // if targreted, then do not need to remove anymore
-
                 RAVELOG_VERBOSE_FORMAT("env=%s, update existing body id '%s', numGrabbed=%d", GetNameId()%pMatchExistingBody->_id%pMatchExistingBody->GetNumGrabbed());
-                // interface should match at this point
-                // update existing body or robot
+
+                // If we have an existing body to update, make sure that it gets removed from the list of temporarily renamed bodies,
+                // since we're about to give it a proper name / don't want it to get garbage collected later.
+                listBodiesTemporarilyRenamed.remove(pMatchExistingBody);
+
+                // We know that this body has a matching info in the list we were given, so note that it was directly selected by this update.
+                // This doesn't matter for OnlySpecifiedBodiesExact, but if that option is not passed, we need to know to keep it.
+                setBodiesWithMatchingInfos.emplace(pMatchExistingBody.get());
+
+                // Interface should match at this point, since if we had a mismatch in the previous step we should have removed the body already
+                OPENRAVE_ASSERT_OP(pKinBodyInfo->_isRobot, ==, pMatchExistingBody->IsRobot());
+
+                // Perform the actual update, making sure to call the correct virtual method if this is a robot
+
                 UpdateFromInfoResult updateFromInfoResult = UFIR_NoChange;
                 if (pKinBodyInfo->_isRobot && pMatchExistingBody->IsRobot()) {
                     RobotBasePtr pRobot = RaveInterfaceCast<RobotBase>(pMatchExistingBody);
@@ -3162,28 +3158,39 @@ class Environment : public EnvironmentBase
                     updateFromInfoResult = pMatchExistingBody->UpdateFromKinBodyInfo(*pKinBodyInfo);
                 }
                 RAVELOG_VERBOSE_FORMAT("env=%s, update body '%s' from info result %d, numGrabbed=%d", GetNameId() % pMatchExistingBody->_id % static_cast<int>(updateFromInfoResult)%pMatchExistingBody->GetNumGrabbed());
+
+                // If the body didn't change at all, nothing else needs be done. Don't count it as being modified by this update.
                 if (updateFromInfoResult == UFIR_NoChange) {
                     continue;
                 }
+
+                // If the body was either updated or couldn't be updated, bump the modification time and add it to our set of modified bodies
                 if (info._lastModifiedAtUS > pMatchExistingBody->_lastModifiedAtUS) {
                     pMatchExistingBody->_lastModifiedAtUS = info._lastModifiedAtUS;
                 }
                 pMatchExistingBody->_revisionId = info._revisionId;
                 vModifiedBodies.push_back(pMatchExistingBody);
+
+                // If the body could be updated without being removed from the environment, we're all set at this point.
                 if (updateFromInfoResult == UFIR_Success) {
                     continue;
                 }
 
-                // if this body is grabbed by another bodies, save the grabbing link and user data
+                // In some cases, the body can't be updated while still added to the environment (TODO: why?)
+                // If this happens, we need to first save information about which bodies are _grabbing_ this body (grabs are known from the body's own info)
+                // so that we can restore these grabs when the body is added back into the environment.
                 for (const KinBodyWeakPtr& pBody : pMatchExistingBody->_listAttachedBodies) {
+                    // Search the list of attached bodies for grabbers instead of the whole env
                     KinBodyPtr pAttached = pBody.lock();
                     if (!pAttached) {
                         continue;
                     }
+                    // For each attached body, check to see if it is grabbing the body we are about to remove
                     for (const KinBody::MapGrabbedByEnvironmentIndex::value_type& grabPair : pAttached->_grabbedBodiesByEnvironmentIndex) {
                         const GrabbedPtr& pGrabbed = grabPair.second;
                         KinBodyConstPtr pGrabbedBody = pGrabbed->_pGrabbedBody.lock();
                         if( !!pGrabbedBody && pGrabbedBody.get() == &*pMatchExistingBody ) {
+                            // This body is grabbing the body we are about to remove - save all of the grab info so that we can restore it.
                             pGrabbingBodies.push_back(pAttached);
                             pGrabbingLinks.push_back(pGrabbed->_pGrabbingLink);
                             rapidjson::Document rGrabbedUserData;
@@ -3194,7 +3201,7 @@ class Environment : public EnvironmentBase
                     }
                 }
 
-                // updating this body requires removing it and re-adding it to env
+                // Remove this body from the environment so that we can try updating it again
                 {
                     ExclusiveLock lock253(_mutexInterfaces);
                     vector<KinBodyPtr>::iterator itExisting = std::find(_vecbodies.begin(), _vecbodies.end(), pMatchExistingBody);
@@ -3243,8 +3250,9 @@ class Environment : public EnvironmentBase
                     _AddKinBody(pMatchExistingBody, IAM_StrictNameChecking); // internally locks _mutexInterfaces, name guarnateed to be unique
                 }
             }
+
+            // If we weren't able to locate an existing body to update, then we can just create a new one
             else {
-                // for new body or robot
                 KinBodyPtr pNewBody;
                 if (pKinBodyInfo->_isRobot) {
                     RAVELOG_VERBOSE_FORMAT("add new robot id '%s'", pKinBodyInfo->_id);
@@ -3274,21 +3282,14 @@ class Environment : public EnvironmentBase
                     _AddKinBody(pNewBody, IAM_AllowRenaming);
                 }
 
-                if (bodyIndex >= 0) {
-                    if (updateMode == UFIM_OnlySpecifiedBodiesExact) {
-                        usedBodyIndexSet.emplace(bodyIndex);
-                    }
-                    vBodies.insert(vBodies.begin() + bodyIndex, pNewBody);
-                }
-                else {
-                    if (updateMode == UFIM_OnlySpecifiedBodiesExact) {
-                        usedBodyIndexSet.emplace(vBodies.size());
-                    }
-                    vBodies.push_back(pNewBody);
-                }
                 pNewBody->_lastModifiedAtUS = info._lastModifiedAtUS;
                 pNewBody->_revisionId = info._revisionId;
+
+                // Indicate to the caller that this body was created as a result of the call
                 vCreatedBodies.push_back(pNewBody);
+
+                // Internally cache that this body has a matching body info in the event that we were not called with OnlySpecifiedBodiesExact
+                setBodiesWithMatchingInfos.emplace(pNewBody.get());
             }
 
             if (!!pInitBody) {
@@ -3328,26 +3329,24 @@ class Environment : public EnvironmentBase
             }
         }
 
+        // If we were not called with OnlySpecifiedBodiesExact, then any bodies that did not have matching body infos should be removed from the environment.
         if( updateMode != UFIM_OnlySpecifiedBodiesExact ) {
-            // remove extra bodies at the end of vBodies
-            if( vBodies.size() > info._vBodyInfos.size() ) {
-                ExclusiveLock lock009(_mutexInterfaces);
-                for (std::vector<KinBodyPtr>::iterator itBody = vBodies.begin() + info._vBodyInfos.size(); itBody != vBodies.end(); ) {
-                    KinBodyPtr pBody = *itBody;
-                    if (!pBody) {
-                        ++itBody;
-                        continue;
-                    }
-                    RAVELOG_VERBOSE_FORMAT("remove extra body env=%s, id=%s, name=%s", GetNameId()%pBody->_id%pBody->_name);
-
-                    vector<KinBodyPtr>::iterator itBodyToRemove = std::find(_vecbodies.begin(), _vecbodies.end(), pBody);
-                    if( itBodyToRemove != _vecbodies.end() ) {
-                        _InvalidateKinBodyFromEnvBodyIndex(pBody->GetEnvironmentBodyIndex());
-                    }
+            // Iterate the bodies in the environment and remove any that were _not_ linked to a body info
+            ExclusiveLock lockInterfaces(_mutexInterfaces);
+            for (const KinBodyPtr& environmentBody : _vecbodies) {
+                // If this body isn't valid, skip it
+                if (!environmentBody) {
+                    continue;
+                }
 
-                    vRemovedBodies.push_back(pBody);
-                    itBody = vBodies.erase(itBody);
+                // If this body had a matching info record, keep it
+                if (setBodiesWithMatchingInfos.find(environmentBody.get()) != setBodiesWithMatchingInfos.end()) {
+                    continue;
                 }
+
+                // If the body didn't get matched, add it to the set of removed bodies and erase it from the environment
+                RAVELOG_VERBOSE_FORMAT("remove extra body env=%s, id=%s, name=%s", GetNameId()%environmentBody->_id%environmentBody->_name);
+                vRemovedBodies.emplace_back(_InvalidateKinBodyFromEnvBodyIndex(environmentBody->GetEnvironmentBodyIndex()));
             }
         }
 
@@ -3357,44 +3356,38 @@ class Environment : public EnvironmentBase
             vRemovedBodies.push_back(pRenamedBody);
         }
 
-        // after all bodies are added, update the grab states
+        // After all bodies are added, update the grab states
         std::vector<KinBody::GrabbedInfoConstPtr> vGrabbedInfos;
         for(const KinBody::KinBodyInfoPtr& pKinBodyInfo : info._vBodyInfos) {
             const std::string& bodyName = pKinBodyInfo->_name;
 
-            // find existing body in the env, use name since that is more guaranteed to be unique
-            std::vector<KinBodyPtr>::iterator itExistingBody = vBodies.end();
-            FOREACH(itBody, vBodies) {
-                if ((*itBody)->_name == bodyName) {
-                    itExistingBody = itBody;
-                    break;
-                }
+            // Find existing body in the env, use name since that is more guaranteed to be unique
+            KinBodyPtr pExistingBody = GetKinBody(bodyName);
+            if (!pExistingBody) {
+                RAVELOG_WARN_FORMAT("env=%s, could not find body with name='%s'", GetNameId() % bodyName);
+                continue;
             }
 
-            if (itExistingBody != vBodies.end()) {
-                // grabbed infos
-                if ((int)pKinBodyInfo->_vGrabbedInfos.size() != (*itExistingBody)->GetNumGrabbed()) {
-                    RAVELOG_DEBUG_FORMAT("env=%s, body name='%s' updating grab from %d -> %d", GetNameId()%bodyName%(*itExistingBody)->GetNumGrabbed()%pKinBodyInfo->_vGrabbedInfos.size());
-                    // when grab info changes, have to report to caller
-                    if (std::find(vModifiedBodies.begin(), vModifiedBodies.end(), *itExistingBody) == vModifiedBodies.end() && std::find(vCreatedBodies.begin(), vCreatedBodies.end(), *itExistingBody) == vCreatedBodies.end()) {
-                        vModifiedBodies.push_back(*itExistingBody);
-                    }
-                }
-                vGrabbedInfos.clear();
-                vGrabbedInfos.reserve(pKinBodyInfo->_vGrabbedInfos.size());
-                FOREACHC(itGrabbedInfo, pKinBodyInfo->_vGrabbedInfos) {
-                    if (!!GetKinBody((*itGrabbedInfo)->_grabbedname)) {
-                        vGrabbedInfos.push_back(*itGrabbedInfo);
-                    }
-                    else {
-                        RAVELOG_WARN_FORMAT("env=%s, body '%s' grabbed by '%s' is gone, ignoring grabbed info id '%s'", GetNameId()%(*itGrabbedInfo)->_grabbedname%pKinBodyInfo->_name%(*itGrabbedInfo)->_id);
-                    }
+            // Restore the grabbed info
+            if ((int)pKinBodyInfo->_vGrabbedInfos.size() != pExistingBody->GetNumGrabbed()) { // Only marking it as modified if the _count_ of the grabbed bodies seems lossy?
+                RAVELOG_DEBUG_FORMAT("env=%s, body name='%s' updating grab from %d -> %d", GetNameId() % bodyName % pExistingBody->GetNumGrabbed() % pKinBodyInfo->_vGrabbedInfos.size());
+                // when grab info changes, have to report to caller
+                if (std::find(vModifiedBodies.begin(), vModifiedBodies.end(), pExistingBody) == vModifiedBodies.end() && std::find(vCreatedBodies.begin(), vCreatedBodies.end(), pExistingBody) == vCreatedBodies.end()) {
+                    vModifiedBodies.push_back(pExistingBody);
                 }
-                (*itExistingBody)->ResetGrabbed(vGrabbedInfos);
             }
-            else {
-                RAVELOG_WARN_FORMAT("env=%s, could not find body with name='%s'", GetNameId()%bodyName);
+            vGrabbedInfos.clear();
+            vGrabbedInfos.reserve(pKinBodyInfo->_vGrabbedInfos.size());
+            FOREACHC(itGrabbedInfo, pKinBodyInfo->_vGrabbedInfos)
+            {
+                if (!!GetKinBody((*itGrabbedInfo)->_grabbedname)) {
+                    vGrabbedInfos.push_back(*itGrabbedInfo);
+                }
+                else {
+                    RAVELOG_WARN_FORMAT("env=%s, body '%s' grabbed by '%s' is gone, ignoring grabbed info id '%s'", GetNameId() % (*itGrabbedInfo)->_grabbedname % pKinBodyInfo->_name % (*itGrabbedInfo)->_id);
+                }
             }
+            pExistingBody->ResetGrabbed(vGrabbedInfos);
         }
 
         UpdatePublishedBodies();


Reviews:
user: login: rdiankov
state: COMMENTED
author_association: OWNER
submitted_at: 2025-04-28T17:39:35Z
user: login: rdiankov
state: COMMENTED
author_association: OWNER
submitted_at: 2025-04-28T17:48:10Z
user: login: rschlaikjer
state: COMMENTED
author_association: COLLABORATOR
submitted_at: 2025-04-29T01:14:25Z
user: login: woswos
state: COMMENTED
author_association: COLLABORATOR
submitted_at: 2025-05-01T06:59:27Z
user: login: woswos
state: COMMENTED
author_association: COLLABORATOR
submitted_at: 2025-05-01T07:06:40Z
user: login: rschlaikjer
state: COMMENTED
author_association: COLLABORATOR
submitted_at: 2025-05-01T08:45:25Z
user: login: ziyan
state: COMMENTED
author_association: COLLABORATOR
submitted_at: 2025-05-01T14:20:16Z
user: login: ziyan
state: COMMENTED
author_association: COLLABORATOR
submitted_at: 2025-05-01T14:31:12Z
user: login: ziyan
state: APPROVED
author_association: COLLABORATOR
submitted_at: 2025-05-05T22:10:45Z

Review comments:
diff_hunk: @@ -2986,181 +2986,170 @@ class Environment : public EnvironmentBase
             SharedLock lock533(_mutexInterfaces);
             vBodies = _vecbodies;
         }
-        {
-            for (std::vector<KinBodyPtr>::iterator it = vBodies.begin(); it != vBodies.end(); ) {
-                if (!*it) {
-                    it = vBodies.erase(it);
-                }
-                else {
-                    it++;
-                }
+
+        // If the environment is large but we are only updating a small number of bodies, building a full lookup table is expensive relative to the amount of lookups we actually do.
+        // We can reduce this somewhat by pre-filtering the set of names/ids that could ever be looked up, and only including those in our lookup table.
+        // This incurs (info._vBodyInfos.size() + vBodies.size()) more hash _lookups_, but means that we save up to (vBodies.size() - info._vBodyInfos.size()) _allocations_.
+        // Since we expect vBodies.size() >> info._vBodyInfos.size() in most cases, and allocations are more costly than lookups, this is overall a speedup despite the extra steps.
+        std::unordered_set<string_view> bodyIdsToUpdate; // string_views are over info._vBodyInfos, guaranteed lifetime for the rest of this function
+        std::unordered_set<string_view> bodyNamesToUpdate;
+        for (const KinBody::KinBodyInfoPtr& pKinBodyInfo : info._vBodyInfos) {
+            bodyIdsToUpdate.emplace(pKinBodyInfo->_id);
+            bodyNamesToUpdate.emplace(pKinBodyInfo->_name);
+        }
+
+        // Build a lookup table for the bodies available to be matched against from vBodies
+        // When trying to identify candidate matches, we can do two hash lookups instead of a sequential scan.
+        // If we match a body out of vBodies, then we just need to remove the id/name maps that linked to it and reset the body at that location.
+        // These maps use string_views over the bodies in vBodies; these bodies _are_ removed during iteration; care must be taken to erase the matching entries.
+        std::unordered_map<string_view, int> existingBodyIndicesById; // Map of body id -> environmentBodyIndex into vBodies
+        std::unordered_map<string_view, int> existingBodyIndicesByName; // Map of body name -> environmentBodyIndex into vBodies
+        for (size_t bodyIndex = 0; bodyIndex < vBodies.size(); bodyIndex++) {
+            // It's possible for holes to exist in vBodies since when bodies are removed from the environment their entries in _vecbodies are just nulled out.
+            if (!vBodies[bodyIndex]) {
+                continue;
             }
+
+            const KinBody& body = *vBodies[bodyIndex];
+            if (bodyIdsToUpdate.find(body.GetId()) == bodyIdsToUpdate.end() && bodyNamesToUpdate.find(body.GetName()) == bodyNamesToUpdate.end()) {
+                continue;
+            }
+            existingBodyIndicesById[body.GetId()] = bodyIndex;
+            existingBodyIndicesByName[body.GetName()] = bodyIndex;
         }
 
-        // Set of indices that have already been used for vBodies
-        std::unordered_set<int> usedBodyIndexSet;
+        // Keep a list of the bodies that we have matched to infos - in the event that the caller did _not_ specify OnlySpecifiedBodiesExact,
+        // when we are finished we will want to remove all bodies in the environment that are _not_ in this list.
+        // Store raw pointers here instead of shared pointers because we don't need to track ownership, just membership.
+        std::unordered_set<KinBody*> setBodiesWithMatchingInfos;
 
         // internally manipulates _vecbodies using _AddKinBody/_AddRobot/_RemoveKinBodyFromIterator
-        for(int inputBodyIndex = 0; inputBodyIndex < (int)info._vBodyInfos.size(); ++inputBodyIndex) {
-            const KinBody::KinBodyInfoConstPtr& pKinBodyInfo = info._vBodyInfos[inputBodyIndex];
+        for(const KinBody::KinBodyInfoPtr& pKinBodyInfo : info._vBodyInfos ) {
             const KinBody::KinBodyInfo& kinBodyInfo = *pKinBodyInfo;
-            RAVELOG_VERBOSE_FORMAT("env=%s, id '%s', name '%s'", GetNameId()%pKinBodyInfo->_id%pKinBodyInfo->_name);
+            RAVELOG_VERBOSE_FORMAT("env=%s, id '%s', name '%s', _vGrabbedInfos=%d", GetNameId()%pKinBodyInfo->_id%pKinBodyInfo->_name%pKinBodyInfo->_vGrabbedInfos.size());
             RobotBase::RobotBaseInfoConstPtr pRobotBaseInfo = OPENRAVE_DYNAMIC_POINTER_CAST<const RobotBase::RobotBaseInfo>(pKinBodyInfo);
-            KinBodyPtr pMatchExistingBody; // matches to pKinBodyInfo
-            int bodyIndex = -1; // index to vBodies to use. -1 if not used
-            {
-                // find existing body in the env
-                std::vector<KinBodyPtr>::iterator itExistingSameId = vBodies.end();
-                std::vector<KinBodyPtr>::iterator itExistingSameName = vBodies.end();
-                std::vector<KinBodyPtr>::iterator itExistingSameIdName = vBodies.end();
-
-                if( updateMode == UFIM_OnlySpecifiedBodiesExact ) {
-                    // can be any of the bodies, but have to make sure not to overlap
-                    //bodyIndex = inputBodyIndex;
-                    // search only in the unprocessed part of vBodies
-                    for (int ibody = 0; ibody < (int)vBodies.size(); ++ibody) {
-                        if (usedBodyIndexSet.find(ibody) != usedBodyIndexSet.end()) {
-                            continue;
-                        }
-                        const KinBodyPtr& pbody = vBodies[ibody];
-                        if (!pbody) {
-                            continue;
-                        }
-                        bool bIdMatch = !pbody->_id.empty() && pbody->_id == kinBodyInfo._id;
-                        bool bNameMatch = !pbody->_name.empty() && pbody->_name == kinBodyInfo._name;
-                        if( bIdMatch && bNameMatch ) {
-                            itExistingSameIdName = itExistingSameId = itExistingSameName = vBodies.begin() + ibody;
-                            break;
-                        }
-                        if( bIdMatch && itExistingSameId == vBodies.end() ) {
-                            itExistingSameId = vBodies.begin() + ibody;
-                        }
-                        if( bNameMatch && itExistingSameName == vBodies.end() ) {
-                            itExistingSameName = vBodies.begin() + ibody;
-                        }
-                    }
-                }
-                else {
-                    bodyIndex = inputBodyIndex;
-                    // search only in the unprocessed part of vBodies
-                    if( (int)vBodies.size() > inputBodyIndex ) {
-                        for (std::vector<KinBodyPtr>::iterator itBody = vBodies.begin() + inputBodyIndex; itBody != vBodies.end(); ++itBody) {
-                            if (!(*itBody)) {
-                                continue;
-                            }
-                            bool bIdMatch = !(*itBody)->_id.empty() && (*itBody)->_id == kinBodyInfo._id;
-                            bool bNameMatch = !(*itBody)->_name.empty() && (*itBody)->_name == kinBodyInfo._name;
-                            if( bIdMatch && bNameMatch ) {
-                                itExistingSameIdName = itBody;
-                                itExistingSameId = itBody;
-                                itExistingSameName = itBody;
-                                break;
-                            }
-                            if( bIdMatch && itExistingSameId == vBodies.end() ) {
-                                itExistingSameId = itBody;
-                            }
-                            if( bNameMatch && itExistingSameName == vBodies.end() ) {
-                                itExistingSameName = itBody;
-                            }
-                        }
-                    }
-                }
 
-                std::vector<KinBodyPtr>::iterator itExisting = itExistingSameIdName;
-                if( itExisting == vBodies.end() ) {
-                    itExisting = itExistingSameId;
-                }
-                if( itExisting == vBodies.end() ) {
-                    itExisting = itExistingSameName;
-                }
+            // Try and match this body info to an existing body
+            KinBodyPtr pExistingBody; // Will be loaded with the existing body to update, if any
+            do {
+                // Check if we have bodies with matching names / ids
+                const std::unordered_map<string_view, int>::iterator itExistingBodyIndexById = existingBodyIndicesById.find(kinBodyInfo._id);
+                const std::unordered_map<string_view, int>::iterator itExistingBodyIndexByName = existingBodyIndicesByName.find(kinBodyInfo._name);
 
-                // check if interface type changed, if so, remove the body and treat it as a new body
-                if (itExisting != vBodies.end()) {
-                    KinBodyPtr pBody = *itExisting;
-                    bool bInterfaceMatches = pBody->GetXMLId() == pKinBodyInfo->_interfaceType;
-                    if( !bInterfaceMatches || pBody->IsRobot() != pKinBodyInfo->_isRobot ) {
-                        RAVELOG_VERBOSE_FORMAT("env=%s, body '%s' interface is changed, remove old body from environment. xmlid=%s, _interfaceType=%s, isRobot %d != %d", GetNameId()%pBody->_id%pBody->GetXMLId()%pKinBodyInfo->_interfaceType%pBody->IsRobot()%pKinBodyInfo->_isRobot);
-                        itExisting = vBodies.end();
-                        vRemovedBodies.push_back(pBody);
-
-                        ExclusiveLock lock690(_mutexInterfaces);
-                        vector<KinBodyPtr>::iterator itBodyToRemove = std::find(_vecbodies.begin(), _vecbodies.end(), pBody);
-                        if( itBodyToRemove != _vecbodies.end() ) {
-                            _InvalidateKinBodyFromEnvBodyIndex(pBody->GetEnvironmentBodyIndex());
-                        }
-                    }
+                // Get the corresponding indexes into vBodies, or -1 if there was no match
+                int existingBodyIndexSameId = itExistingBodyIndexById != existingBodyIndicesByName.end() ? itExistingBodyIndexById->second : -1;
+                int existingBodyIndexSameName = itExistingBodyIndexByName != existingBodyIndicesByName.end() ? itExistingBodyIndexByName->second : -1;
+
+                // Pick the best existing body index based on our resolution order
+                int existingBodyIndex = existingBodyIndexSameId >= 0 ? existingBodyIndexSameId : existingBodyIndexSameName;
+
+                // If we didn't match anything, we have to just create a new body based on this info
+                if (existingBodyIndex < 0) {
+                    break;
                 }
 
-                if( itExisting != vBodies.end() ) {
-                    if( itExisting != itExistingSameName && itExistingSameName != vBodies.end() ) {
-                        // new name will conflict with *itExistingSameName, so should change the names to something temporarily
-                        // for now, clear since the body should be processed later again
-                        RAVELOG_DEBUG_FORMAT("env=%s, have to clear body name '%s' id=%s for loading body with id=%s", GetNameId()%(*itExistingSameName)->GetName()%(*itExistingSameName)->GetId()%(*itExisting)->GetId());
-                        (*itExistingSameName)->SetName(_GetUniqueName((*itExistingSameName)->GetName()+"_tempRenamedDueToConflict_"));
-                        listBodiesTemporarilyRenamed.push_back(*itExistingSameName);
-                    }
-                    pMatchExistingBody = *itExisting;
-                    int nMatchingIndex = itExisting-vBodies.begin();
-                    if ( bodyIndex >= 0 && bodyIndex != nMatchingIndex) {
-                        // re-arrange vBodies according to the order of infos
-                        KinBodyPtr pTempBody = vBodies.at(bodyIndex);
-                        vBodies.at(bodyIndex) = pMatchExistingBody;
-                        *itExisting = pTempBody;
+                // If we matched a body to reuse, latch it out and clear the mappings so that we can't double-process it
+                pExistingBody = std::move(vBodies[existingBodyIndex]); // Invalidate this slot in vBodies to make sure we don't accidentally re-use it
+                OPENRAVE_ASSERT_OP(pExistingBody, !=, KinBodyPtr()); // Only valid entries should exist in our lookup maps
+                existingBodyIndicesById.erase(pExistingBody->GetId()); // Also ensures no dangling string_views in the event this body gets destroyed later in the loop
+                existingBodyIndicesByName.erase(pExistingBody->GetName());
+
+                // If we matched an existing body, but that body is of a different interface to the info (e.g robot vs plain body), then we can't just update, need to recreate it.
+                bool bInterfaceMatches = pExistingBody->GetXMLId() == pKinBodyInfo->_interfaceType;
+                if (!bInterfaceMatches || pExistingBody->IsRobot() != pKinBodyInfo->_isRobot) {
+                    KinBodyPtr pBodyToRemove = std::move(pExistingBody); // Invalidates existing body output
+                    RAVELOG_VERBOSE_FORMAT("env=%s, body '%s' interface is changed, remove old body from environment. xmlid=%s, _interfaceType=%s, isRobot %d != %d", GetNameId() % pBodyToRemove->_id % pBodyToRemove->GetXMLId() % pKinBodyInfo->_interfaceType % pBodyToRemove->IsRobot() % pKinBodyInfo->_isRobot);
+                    vRemovedBodies.push_back(pBodyToRemove);
+
+                    ExclusiveLock lock690(_mutexInterfaces);
+                    vector<KinBodyPtr>::iterator itBodyToRemove = std::find(_vecbodies.begin(), _vecbodies.end(), pBodyToRemove);
+                    if (itBodyToRemove != _vecbodies.end()) {
+                        _InvalidateKinBodyFromEnvBodyIndex(pBodyToRemove->GetEnvironmentBodyIndex());
                     }
+                }
 
-                    if (updateMode == UFIM_OnlySpecifiedBodiesExact) {
-                        usedBodyIndexSet.emplace(nMatchingIndex);
-                    }
+                // If we matched by ID instead of name, it's possible there exists another body in the env with the same name as our body info.
+                // This would cause a conflict if we try and update our current (id matched body) to have the same name.
+                // Since the other body with the same name might get processed again later (by id?), temporarily rename it so that we can continue.
+                if (!!pExistingBody && existingBodyIndex != existingBodyIndexSameName && existingBodyIndexSameName >= 0) {
+                    const KinBodyPtr& pExistingBodySameName = vBodies[existingBodyIndexSameName];
+                    RAVELOG_DEBUG_FORMAT("env=%s, have to clear body name '%s' id=%s for loading body with id=%s", GetNameId() % pExistingBodySameName->GetName() % pExistingBodySameName->GetId() % pExistingBody->GetId());
+                    pExistingBodySameName->SetName(_GetUniqueName(pExistingBodySameName->GetName() + "_tempRenamedDueToConflict_"));
path: src/libopenrave-core/environment-core.h
user: login: rdiankov
body: @rschlaikjer  Changing name of an existing body invalidates the `string_view` you are managing here, which could corrupt your state.

```c++
        std::unordered_set<string_view> bodyIdsToUpdate; // string_views are over info._vBodyInfos, guaranteed lifetime for the rest of this function
        std::unordered_set<string_view> bodyNamesToUpdate;
```

created_at: 2025-04-28T17:39:34Z
author_association: OWNER
diff_hunk: @@ -2986,181 +2986,170 @@ class Environment : public EnvironmentBase
             SharedLock lock533(_mutexInterfaces);
             vBodies = _vecbodies;
         }
-        {
-            for (std::vector<KinBodyPtr>::iterator it = vBodies.begin(); it != vBodies.end(); ) {
-                if (!*it) {
-                    it = vBodies.erase(it);
-                }
-                else {
-                    it++;
-                }
+
+        // If the environment is large but we are only updating a small number of bodies, building a full lookup table is expensive relative to the amount of lookups we actually do.
+        // We can reduce this somewhat by pre-filtering the set of names/ids that could ever be looked up, and only including those in our lookup table.
+        // This incurs (info._vBodyInfos.size() + vBodies.size()) more hash _lookups_, but means that we save up to (vBodies.size() - info._vBodyInfos.size()) _allocations_.
+        // Since we expect vBodies.size() >> info._vBodyInfos.size() in most cases, and allocations are more costly than lookups, this is overall a speedup despite the extra steps.
+        std::unordered_set<string_view> bodyIdsToUpdate; // string_views are over info._vBodyInfos, guaranteed lifetime for the rest of this function
+        std::unordered_set<string_view> bodyNamesToUpdate;
+        for (const KinBody::KinBodyInfoPtr& pKinBodyInfo : info._vBodyInfos) {
+            bodyIdsToUpdate.emplace(pKinBodyInfo->_id);
+            bodyNamesToUpdate.emplace(pKinBodyInfo->_name);
+        }
+
+        // Build a lookup table for the bodies available to be matched against from vBodies
+        // When trying to identify candidate matches, we can do two hash lookups instead of a sequential scan.
+        // If we match a body out of vBodies, then we just need to remove the id/name maps that linked to it and reset the body at that location.
+        // These maps use string_views over the bodies in vBodies; these bodies _are_ removed during iteration; care must be taken to erase the matching entries.
+        std::unordered_map<string_view, int> existingBodyIndicesById; // Map of body id -> environmentBodyIndex into vBodies
+        std::unordered_map<string_view, int> existingBodyIndicesByName; // Map of body name -> environmentBodyIndex into vBodies
+        for (size_t bodyIndex = 0; bodyIndex < vBodies.size(); bodyIndex++) {
+            // It's possible for holes to exist in vBodies since when bodies are removed from the environment their entries in _vecbodies are just nulled out.
+            if (!vBodies[bodyIndex]) {
+                continue;
             }
+
+            const KinBody& body = *vBodies[bodyIndex];
+            if (bodyIdsToUpdate.find(body.GetId()) == bodyIdsToUpdate.end() && bodyNamesToUpdate.find(body.GetName()) == bodyNamesToUpdate.end()) {
+                continue;
+            }
+            existingBodyIndicesById[body.GetId()] = bodyIndex;
+            existingBodyIndicesByName[body.GetName()] = bodyIndex;
         }
 
-        // Set of indices that have already been used for vBodies
-        std::unordered_set<int> usedBodyIndexSet;
+        // Keep a list of the bodies that we have matched to infos - in the event that the caller did _not_ specify OnlySpecifiedBodiesExact,
+        // when we are finished we will want to remove all bodies in the environment that are _not_ in this list.
+        // Store raw pointers here instead of shared pointers because we don't need to track ownership, just membership.
+        std::unordered_set<KinBody*> setBodiesWithMatchingInfos;
 
         // internally manipulates _vecbodies using _AddKinBody/_AddRobot/_RemoveKinBodyFromIterator
-        for(int inputBodyIndex = 0; inputBodyIndex < (int)info._vBodyInfos.size(); ++inputBodyIndex) {
-            const KinBody::KinBodyInfoConstPtr& pKinBodyInfo = info._vBodyInfos[inputBodyIndex];
+        for(const KinBody::KinBodyInfoPtr& pKinBodyInfo : info._vBodyInfos ) {
             const KinBody::KinBodyInfo& kinBodyInfo = *pKinBodyInfo;
-            RAVELOG_VERBOSE_FORMAT("env=%s, id '%s', name '%s'", GetNameId()%pKinBodyInfo->_id%pKinBodyInfo->_name);
+            RAVELOG_VERBOSE_FORMAT("env=%s, id '%s', name '%s', _vGrabbedInfos=%d", GetNameId()%pKinBodyInfo->_id%pKinBodyInfo->_name%pKinBodyInfo->_vGrabbedInfos.size());
             RobotBase::RobotBaseInfoConstPtr pRobotBaseInfo = OPENRAVE_DYNAMIC_POINTER_CAST<const RobotBase::RobotBaseInfo>(pKinBodyInfo);
-            KinBodyPtr pMatchExistingBody; // matches to pKinBodyInfo
-            int bodyIndex = -1; // index to vBodies to use. -1 if not used
-            {
-                // find existing body in the env
-                std::vector<KinBodyPtr>::iterator itExistingSameId = vBodies.end();
-                std::vector<KinBodyPtr>::iterator itExistingSameName = vBodies.end();
-                std::vector<KinBodyPtr>::iterator itExistingSameIdName = vBodies.end();
-
-                if( updateMode == UFIM_OnlySpecifiedBodiesExact ) {
-                    // can be any of the bodies, but have to make sure not to overlap
-                    //bodyIndex = inputBodyIndex;
-                    // search only in the unprocessed part of vBodies
-                    for (int ibody = 0; ibody < (int)vBodies.size(); ++ibody) {
-                        if (usedBodyIndexSet.find(ibody) != usedBodyIndexSet.end()) {
-                            continue;
-                        }
-                        const KinBodyPtr& pbody = vBodies[ibody];
-                        if (!pbody) {
-                            continue;
-                        }
-                        bool bIdMatch = !pbody->_id.empty() && pbody->_id == kinBodyInfo._id;
-                        bool bNameMatch = !pbody->_name.empty() && pbody->_name == kinBodyInfo._name;
-                        if( bIdMatch && bNameMatch ) {
-                            itExistingSameIdName = itExistingSameId = itExistingSameName = vBodies.begin() + ibody;
-                            break;
-                        }
-                        if( bIdMatch && itExistingSameId == vBodies.end() ) {
-                            itExistingSameId = vBodies.begin() + ibody;
-                        }
-                        if( bNameMatch && itExistingSameName == vBodies.end() ) {
-                            itExistingSameName = vBodies.begin() + ibody;
-                        }
-                    }
-                }
-                else {
-                    bodyIndex = inputBodyIndex;
-                    // search only in the unprocessed part of vBodies
-                    if( (int)vBodies.size() > inputBodyIndex ) {
-                        for (std::vector<KinBodyPtr>::iterator itBody = vBodies.begin() + inputBodyIndex; itBody != vBodies.end(); ++itBody) {
-                            if (!(*itBody)) {
-                                continue;
-                            }
-                            bool bIdMatch = !(*itBody)->_id.empty() && (*itBody)->_id == kinBodyInfo._id;
-                            bool bNameMatch = !(*itBody)->_name.empty() && (*itBody)->_name == kinBodyInfo._name;
-                            if( bIdMatch && bNameMatch ) {
-                                itExistingSameIdName = itBody;
-                                itExistingSameId = itBody;
-                                itExistingSameName = itBody;
-                                break;
-                            }
-                            if( bIdMatch && itExistingSameId == vBodies.end() ) {
-                                itExistingSameId = itBody;
-                            }
-                            if( bNameMatch && itExistingSameName == vBodies.end() ) {
-                                itExistingSameName = itBody;
-                            }
-                        }
-                    }
-                }
 
-                std::vector<KinBodyPtr>::iterator itExisting = itExistingSameIdName;
-                if( itExisting == vBodies.end() ) {
-                    itExisting = itExistingSameId;
-                }
-                if( itExisting == vBodies.end() ) {
-                    itExisting = itExistingSameName;
-                }
+            // Try and match this body info to an existing body
+            KinBodyPtr pExistingBody; // Will be loaded with the existing body to update, if any
+            do {
+                // Check if we have bodies with matching names / ids
+                const std::unordered_map<string_view, int>::iterator itExistingBodyIndexById = existingBodyIndicesById.find(kinBodyInfo._id);
+                const std::unordered_map<string_view, int>::iterator itExistingBodyIndexByName = existingBodyIndicesByName.find(kinBodyInfo._name);
 
-                // check if interface type changed, if so, remove the body and treat it as a new body
-                if (itExisting != vBodies.end()) {
-                    KinBodyPtr pBody = *itExisting;
-                    bool bInterfaceMatches = pBody->GetXMLId() == pKinBodyInfo->_interfaceType;
-                    if( !bInterfaceMatches || pBody->IsRobot() != pKinBodyInfo->_isRobot ) {
-                        RAVELOG_VERBOSE_FORMAT("env=%s, body '%s' interface is changed, remove old body from environment. xmlid=%s, _interfaceType=%s, isRobot %d != %d", GetNameId()%pBody->_id%pBody->GetXMLId()%pKinBodyInfo->_interfaceType%pBody->IsRobot()%pKinBodyInfo->_isRobot);
-                        itExisting = vBodies.end();
-                        vRemovedBodies.push_back(pBody);
-
-                        ExclusiveLock lock690(_mutexInterfaces);
-                        vector<KinBodyPtr>::iterator itBodyToRemove = std::find(_vecbodies.begin(), _vecbodies.end(), pBody);
-                        if( itBodyToRemove != _vecbodies.end() ) {
-                            _InvalidateKinBodyFromEnvBodyIndex(pBody->GetEnvironmentBodyIndex());
-                        }
-                    }
+                // Get the corresponding indexes into vBodies, or -1 if there was no match
+                int existingBodyIndexSameId = itExistingBodyIndexById != existingBodyIndicesByName.end() ? itExistingBodyIndexById->second : -1;
+                int existingBodyIndexSameName = itExistingBodyIndexByName != existingBodyIndicesByName.end() ? itExistingBodyIndexByName->second : -1;
+
+                // Pick the best existing body index based on our resolution order
+                int existingBodyIndex = existingBodyIndexSameId >= 0 ? existingBodyIndexSameId : existingBodyIndexSameName;
+
+                // If we didn't match anything, we have to just create a new body based on this info
+                if (existingBodyIndex < 0) {
+                    break;
                 }
 
-                if( itExisting != vBodies.end() ) {
-                    if( itExisting != itExistingSameName && itExistingSameName != vBodies.end() ) {
-                        // new name will conflict with *itExistingSameName, so should change the names to something temporarily
-                        // for now, clear since the body should be processed later again
-                        RAVELOG_DEBUG_FORMAT("env=%s, have to clear body name '%s' id=%s for loading body with id=%s", GetNameId()%(*itExistingSameName)->GetName()%(*itExistingSameName)->GetId()%(*itExisting)->GetId());
-                        (*itExistingSameName)->SetName(_GetUniqueName((*itExistingSameName)->GetName()+"_tempRenamedDueToConflict_"));
-                        listBodiesTemporarilyRenamed.push_back(*itExistingSameName);
-                    }
-                    pMatchExistingBody = *itExisting;
-                    int nMatchingIndex = itExisting-vBodies.begin();
-                    if ( bodyIndex >= 0 && bodyIndex != nMatchingIndex) {
-                        // re-arrange vBodies according to the order of infos
-                        KinBodyPtr pTempBody = vBodies.at(bodyIndex);
-                        vBodies.at(bodyIndex) = pMatchExistingBody;
-                        *itExisting = pTempBody;
+                // If we matched a body to reuse, latch it out and clear the mappings so that we can't double-process it
+                pExistingBody = std::move(vBodies[existingBodyIndex]); // Invalidate this slot in vBodies to make sure we don't accidentally re-use it
+                OPENRAVE_ASSERT_OP(pExistingBody, !=, KinBodyPtr()); // Only valid entries should exist in our lookup maps
+                existingBodyIndicesById.erase(pExistingBody->GetId()); // Also ensures no dangling string_views in the event this body gets destroyed later in the loop
+                existingBodyIndicesByName.erase(pExistingBody->GetName());
+
+                // If we matched an existing body, but that body is of a different interface to the info (e.g robot vs plain body), then we can't just update, need to recreate it.
+                bool bInterfaceMatches = pExistingBody->GetXMLId() == pKinBodyInfo->_interfaceType;
+                if (!bInterfaceMatches || pExistingBody->IsRobot() != pKinBodyInfo->_isRobot) {
+                    KinBodyPtr pBodyToRemove = std::move(pExistingBody); // Invalidates existing body output
+                    RAVELOG_VERBOSE_FORMAT("env=%s, body '%s' interface is changed, remove old body from environment. xmlid=%s, _interfaceType=%s, isRobot %d != %d", GetNameId() % pBodyToRemove->_id % pBodyToRemove->GetXMLId() % pKinBodyInfo->_interfaceType % pBodyToRemove->IsRobot() % pKinBodyInfo->_isRobot);
+                    vRemovedBodies.push_back(pBodyToRemove);
+
+                    ExclusiveLock lock690(_mutexInterfaces);
+                    vector<KinBodyPtr>::iterator itBodyToRemove = std::find(_vecbodies.begin(), _vecbodies.end(), pBodyToRemove);
+                    if (itBodyToRemove != _vecbodies.end()) {
+                        _InvalidateKinBodyFromEnvBodyIndex(pBodyToRemove->GetEnvironmentBodyIndex());
                     }
+                }
 
-                    if (updateMode == UFIM_OnlySpecifiedBodiesExact) {
-                        usedBodyIndexSet.emplace(nMatchingIndex);
-                    }
+                // If we matched by ID instead of name, it's possible there exists another body in the env with the same name as our body info.
+                // This would cause a conflict if we try and update our current (id matched body) to have the same name.
+                // Since the other body with the same name might get processed again later (by id?), temporarily rename it so that we can continue.
+                if (!!pExistingBody && existingBodyIndex != existingBodyIndexSameName && existingBodyIndexSameName >= 0) {
+                    const KinBodyPtr& pExistingBodySameName = vBodies[existingBodyIndexSameName];
+                    RAVELOG_DEBUG_FORMAT("env=%s, have to clear body name '%s' id=%s for loading body with id=%s", GetNameId() % pExistingBodySameName->GetName() % pExistingBodySameName->GetId() % pExistingBody->GetId());
+                    pExistingBodySameName->SetName(_GetUniqueName(pExistingBodySameName->GetName() + "_tempRenamedDueToConflict_"));
path: src/libopenrave-core/environment-core.h
user: login: rdiankov
body: There are also these variables that use string_view which might also be affected:
```c++
                const std::unordered_map<string_view, int>::iterator itExistingBodyIndexById = existingBodyIndicesById.find(kinBodyInfo._id);
                const std::unordered_map<string_view, int>::iterator itExistingBodyIndexByName = existingBodyIndicesByName.find(kinBodyInfo._name);
```

created_at: 2025-04-28T17:48:10Z
author_association: OWNER
diff_hunk: @@ -2986,181 +2986,170 @@ class Environment : public EnvironmentBase
             SharedLock lock533(_mutexInterfaces);
             vBodies = _vecbodies;
         }
-        {
-            for (std::vector<KinBodyPtr>::iterator it = vBodies.begin(); it != vBodies.end(); ) {
-                if (!*it) {
-                    it = vBodies.erase(it);
-                }
-                else {
-                    it++;
-                }
+
+        // If the environment is large but we are only updating a small number of bodies, building a full lookup table is expensive relative to the amount of lookups we actually do.
+        // We can reduce this somewhat by pre-filtering the set of names/ids that could ever be looked up, and only including those in our lookup table.
+        // This incurs (info._vBodyInfos.size() + vBodies.size()) more hash _lookups_, but means that we save up to (vBodies.size() - info._vBodyInfos.size()) _allocations_.
+        // Since we expect vBodies.size() >> info._vBodyInfos.size() in most cases, and allocations are more costly than lookups, this is overall a speedup despite the extra steps.
+        std::unordered_set<string_view> bodyIdsToUpdate; // string_views are over info._vBodyInfos, guaranteed lifetime for the rest of this function
+        std::unordered_set<string_view> bodyNamesToUpdate;
+        for (const KinBody::KinBodyInfoPtr& pKinBodyInfo : info._vBodyInfos) {
+            bodyIdsToUpdate.emplace(pKinBodyInfo->_id);
+            bodyNamesToUpdate.emplace(pKinBodyInfo->_name);
+        }
+
+        // Build a lookup table for the bodies available to be matched against from vBodies
+        // When trying to identify candidate matches, we can do two hash lookups instead of a sequential scan.
+        // If we match a body out of vBodies, then we just need to remove the id/name maps that linked to it and reset the body at that location.
+        // These maps use string_views over the bodies in vBodies; these bodies _are_ removed during iteration; care must be taken to erase the matching entries.
+        std::unordered_map<string_view, int> existingBodyIndicesById; // Map of body id -> environmentBodyIndex into vBodies
+        std::unordered_map<string_view, int> existingBodyIndicesByName; // Map of body name -> environmentBodyIndex into vBodies
+        for (size_t bodyIndex = 0; bodyIndex < vBodies.size(); bodyIndex++) {
+            // It's possible for holes to exist in vBodies since when bodies are removed from the environment their entries in _vecbodies are just nulled out.
+            if (!vBodies[bodyIndex]) {
+                continue;
             }
+
+            const KinBody& body = *vBodies[bodyIndex];
+            if (bodyIdsToUpdate.find(body.GetId()) == bodyIdsToUpdate.end() && bodyNamesToUpdate.find(body.GetName()) == bodyNamesToUpdate.end()) {
+                continue;
+            }
+            existingBodyIndicesById[body.GetId()] = bodyIndex;
+            existingBodyIndicesByName[body.GetName()] = bodyIndex;
         }
 
-        // Set of indices that have already been used for vBodies
-        std::unordered_set<int> usedBodyIndexSet;
+        // Keep a list of the bodies that we have matched to infos - in the event that the caller did _not_ specify OnlySpecifiedBodiesExact,
+        // when we are finished we will want to remove all bodies in the environment that are _not_ in this list.
+        // Store raw pointers here instead of shared pointers because we don't need to track ownership, just membership.
+        std::unordered_set<KinBody*> setBodiesWithMatchingInfos;
 
         // internally manipulates _vecbodies using _AddKinBody/_AddRobot/_RemoveKinBodyFromIterator
-        for(int inputBodyIndex = 0; inputBodyIndex < (int)info._vBodyInfos.size(); ++inputBodyIndex) {
-            const KinBody::KinBodyInfoConstPtr& pKinBodyInfo = info._vBodyInfos[inputBodyIndex];
+        for(const KinBody::KinBodyInfoPtr& pKinBodyInfo : info._vBodyInfos ) {
             const KinBody::KinBodyInfo& kinBodyInfo = *pKinBodyInfo;
-            RAVELOG_VERBOSE_FORMAT("env=%s, id '%s', name '%s'", GetNameId()%pKinBodyInfo->_id%pKinBodyInfo->_name);
+            RAVELOG_VERBOSE_FORMAT("env=%s, id '%s', name '%s', _vGrabbedInfos=%d", GetNameId()%pKinBodyInfo->_id%pKinBodyInfo->_name%pKinBodyInfo->_vGrabbedInfos.size());
             RobotBase::RobotBaseInfoConstPtr pRobotBaseInfo = OPENRAVE_DYNAMIC_POINTER_CAST<const RobotBase::RobotBaseInfo>(pKinBodyInfo);
-            KinBodyPtr pMatchExistingBody; // matches to pKinBodyInfo
-            int bodyIndex = -1; // index to vBodies to use. -1 if not used
-            {
-                // find existing body in the env
-                std::vector<KinBodyPtr>::iterator itExistingSameId = vBodies.end();
-                std::vector<KinBodyPtr>::iterator itExistingSameName = vBodies.end();
-                std::vector<KinBodyPtr>::iterator itExistingSameIdName = vBodies.end();
-
-                if( updateMode == UFIM_OnlySpecifiedBodiesExact ) {
-                    // can be any of the bodies, but have to make sure not to overlap
-                    //bodyIndex = inputBodyIndex;
-                    // search only in the unprocessed part of vBodies
-                    for (int ibody = 0; ibody < (int)vBodies.size(); ++ibody) {
-                        if (usedBodyIndexSet.find(ibody) != usedBodyIndexSet.end()) {
-                            continue;
-                        }
-                        const KinBodyPtr& pbody = vBodies[ibody];
-                        if (!pbody) {
-                            continue;
-                        }
-                        bool bIdMatch = !pbody->_id.empty() && pbody->_id == kinBodyInfo._id;
-                        bool bNameMatch = !pbody->_name.empty() && pbody->_name == kinBodyInfo._name;
-                        if( bIdMatch && bNameMatch ) {
-                            itExistingSameIdName = itExistingSameId = itExistingSameName = vBodies.begin() + ibody;
-                            break;
-                        }
-                        if( bIdMatch && itExistingSameId == vBodies.end() ) {
-                            itExistingSameId = vBodies.begin() + ibody;
-                        }
-                        if( bNameMatch && itExistingSameName == vBodies.end() ) {
-                            itExistingSameName = vBodies.begin() + ibody;
-                        }
-                    }
-                }
-                else {
-                    bodyIndex = inputBodyIndex;
-                    // search only in the unprocessed part of vBodies
-                    if( (int)vBodies.size() > inputBodyIndex ) {
-                        for (std::vector<KinBodyPtr>::iterator itBody = vBodies.begin() + inputBodyIndex; itBody != vBodies.end(); ++itBody) {
-                            if (!(*itBody)) {
-                                continue;
-                            }
-                            bool bIdMatch = !(*itBody)->_id.empty() && (*itBody)->_id == kinBodyInfo._id;
-                            bool bNameMatch = !(*itBody)->_name.empty() && (*itBody)->_name == kinBodyInfo._name;
-                            if( bIdMatch && bNameMatch ) {
-                                itExistingSameIdName = itBody;
-                                itExistingSameId = itBody;
-                                itExistingSameName = itBody;
-                                break;
-                            }
-                            if( bIdMatch && itExistingSameId == vBodies.end() ) {
-                                itExistingSameId = itBody;
-                            }
-                            if( bNameMatch && itExistingSameName == vBodies.end() ) {
-                                itExistingSameName = itBody;
-                            }
-                        }
-                    }
-                }
 
-                std::vector<KinBodyPtr>::iterator itExisting = itExistingSameIdName;
-                if( itExisting == vBodies.end() ) {
-                    itExisting = itExistingSameId;
-                }
-                if( itExisting == vBodies.end() ) {
-                    itExisting = itExistingSameName;
-                }
+            // Try and match this body info to an existing body
+            KinBodyPtr pExistingBody; // Will be loaded with the existing body to update, if any
+            do {
+                // Check if we have bodies with matching names / ids
+                const std::unordered_map<string_view, int>::iterator itExistingBodyIndexById = existingBodyIndicesById.find(kinBodyInfo._id);
+                const std::unordered_map<string_view, int>::iterator itExistingBodyIndexByName = existingBodyIndicesByName.find(kinBodyInfo._name);
 
-                // check if interface type changed, if so, remove the body and treat it as a new body
-                if (itExisting != vBodies.end()) {
-                    KinBodyPtr pBody = *itExisting;
-                    bool bInterfaceMatches = pBody->GetXMLId() == pKinBodyInfo->_interfaceType;
-                    if( !bInterfaceMatches || pBody->IsRobot() != pKinBodyInfo->_isRobot ) {
-                        RAVELOG_VERBOSE_FORMAT("env=%s, body '%s' interface is changed, remove old body from environment. xmlid=%s, _interfaceType=%s, isRobot %d != %d", GetNameId()%pBody->_id%pBody->GetXMLId()%pKinBodyInfo->_interfaceType%pBody->IsRobot()%pKinBodyInfo->_isRobot);
-                        itExisting = vBodies.end();
-                        vRemovedBodies.push_back(pBody);
-
-                        ExclusiveLock lock690(_mutexInterfaces);
-                        vector<KinBodyPtr>::iterator itBodyToRemove = std::find(_vecbodies.begin(), _vecbodies.end(), pBody);
-                        if( itBodyToRemove != _vecbodies.end() ) {
-                            _InvalidateKinBodyFromEnvBodyIndex(pBody->GetEnvironmentBodyIndex());
-                        }
-                    }
+                // Get the corresponding indexes into vBodies, or -1 if there was no match
+                int existingBodyIndexSameId = itExistingBodyIndexById != existingBodyIndicesByName.end() ? itExistingBodyIndexById->second : -1;
+                int existingBodyIndexSameName = itExistingBodyIndexByName != existingBodyIndicesByName.end() ? itExistingBodyIndexByName->second : -1;
+
+                // Pick the best existing body index based on our resolution order
+                int existingBodyIndex = existingBodyIndexSameId >= 0 ? existingBodyIndexSameId : existingBodyIndexSameName;
+
+                // If we didn't match anything, we have to just create a new body based on this info
+                if (existingBodyIndex < 0) {
+                    break;
                 }
 
-                if( itExisting != vBodies.end() ) {
-                    if( itExisting != itExistingSameName && itExistingSameName != vBodies.end() ) {
-                        // new name will conflict with *itExistingSameName, so should change the names to something temporarily
-                        // for now, clear since the body should be processed later again
-                        RAVELOG_DEBUG_FORMAT("env=%s, have to clear body name '%s' id=%s for loading body with id=%s", GetNameId()%(*itExistingSameName)->GetName()%(*itExistingSameName)->GetId()%(*itExisting)->GetId());
-                        (*itExistingSameName)->SetName(_GetUniqueName((*itExistingSameName)->GetName()+"_tempRenamedDueToConflict_"));
-                        listBodiesTemporarilyRenamed.push_back(*itExistingSameName);
-                    }
-                    pMatchExistingBody = *itExisting;
-                    int nMatchingIndex = itExisting-vBodies.begin();
-                    if ( bodyIndex >= 0 && bodyIndex != nMatchingIndex) {
-                        // re-arrange vBodies according to the order of infos
-                        KinBodyPtr pTempBody = vBodies.at(bodyIndex);
-                        vBodies.at(bodyIndex) = pMatchExistingBody;
-                        *itExisting = pTempBody;
+                // If we matched a body to reuse, latch it out and clear the mappings so that we can't double-process it
+                pExistingBody = std::move(vBodies[existingBodyIndex]); // Invalidate this slot in vBodies to make sure we don't accidentally re-use it
+                OPENRAVE_ASSERT_OP(pExistingBody, !=, KinBodyPtr()); // Only valid entries should exist in our lookup maps
+                existingBodyIndicesById.erase(pExistingBody->GetId()); // Also ensures no dangling string_views in the event this body gets destroyed later in the loop
+                existingBodyIndicesByName.erase(pExistingBody->GetName());
+
+                // If we matched an existing body, but that body is of a different interface to the info (e.g robot vs plain body), then we can't just update, need to recreate it.
+                bool bInterfaceMatches = pExistingBody->GetXMLId() == pKinBodyInfo->_interfaceType;
+                if (!bInterfaceMatches || pExistingBody->IsRobot() != pKinBodyInfo->_isRobot) {
+                    KinBodyPtr pBodyToRemove = std::move(pExistingBody); // Invalidates existing body output
+                    RAVELOG_VERBOSE_FORMAT("env=%s, body '%s' interface is changed, remove old body from environment. xmlid=%s, _interfaceType=%s, isRobot %d != %d", GetNameId() % pBodyToRemove->_id % pBodyToRemove->GetXMLId() % pKinBodyInfo->_interfaceType % pBodyToRemove->IsRobot() % pKinBodyInfo->_isRobot);
+                    vRemovedBodies.push_back(pBodyToRemove);
+
+                    ExclusiveLock lock690(_mutexInterfaces);
+                    vector<KinBodyPtr>::iterator itBodyToRemove = std::find(_vecbodies.begin(), _vecbodies.end(), pBodyToRemove);
+                    if (itBodyToRemove != _vecbodies.end()) {
+                        _InvalidateKinBodyFromEnvBodyIndex(pBodyToRemove->GetEnvironmentBodyIndex());
                     }
+                }
 
-                    if (updateMode == UFIM_OnlySpecifiedBodiesExact) {
-                        usedBodyIndexSet.emplace(nMatchingIndex);
-                    }
+                // If we matched by ID instead of name, it's possible there exists another body in the env with the same name as our body info.
+                // This would cause a conflict if we try and update our current (id matched body) to have the same name.
+                // Since the other body with the same name might get processed again later (by id?), temporarily rename it so that we can continue.
+                if (!!pExistingBody && existingBodyIndex != existingBodyIndexSameName && existingBodyIndexSameName >= 0) {
+                    const KinBodyPtr& pExistingBodySameName = vBodies[existingBodyIndexSameName];
+                    RAVELOG_DEBUG_FORMAT("env=%s, have to clear body name '%s' id=%s for loading body with id=%s", GetNameId() % pExistingBodySameName->GetName() % pExistingBodySameName->GetId() % pExistingBody->GetId());
+                    pExistingBodySameName->SetName(_GetUniqueName(pExistingBodySameName->GetName() + "_tempRenamedDueToConflict_"));
path: src/libopenrave-core/environment-core.h
user: login: rschlaikjer
body: Those first views are over the body _info_ record name strings (`info._vBodyInfos`, per the comment), so they don't need to worry about the data getting changed (info is immutable for this function).

As for the second one, good catch - we handle removing the reused body, but not any fallout from renaming any _non_ matched colliding bodies.
created_at: 2025-04-29T01:14:25Z
author_association: COLLABORATOR
diff_hunk: @@ -2986,181 +2986,179 @@ class Environment : public EnvironmentBase
             SharedLock lock533(_mutexInterfaces);
             vBodies = _vecbodies;
         }
-        {
-            for (std::vector<KinBodyPtr>::iterator it = vBodies.begin(); it != vBodies.end(); ) {
-                if (!*it) {
-                    it = vBodies.erase(it);
-                }
-                else {
-                    it++;
-                }
+
+        // If the environment is large but we are only updating a small number of bodies, building a full lookup table is expensive relative to the amount of lookups we actually do.
+        // We can reduce this somewhat by pre-filtering the set of names/ids that could ever be looked up, and only including those in our lookup table.
+        // This incurs (info._vBodyInfos.size() + vBodies.size()) more hash _lookups_, but means that we save up to (vBodies.size() - info._vBodyInfos.size()) _allocations_.
+        // Since we expect vBodies.size() >> info._vBodyInfos.size() in most cases, and allocations are more costly than lookups, this is overall a speedup despite the extra steps.
+        std::unordered_set<string_view> bodyIdsToUpdate; // string_views are over info._vBodyInfos, guaranteed lifetime for the rest of this function
+        std::unordered_set<string_view> bodyNamesToUpdate;
+        for (const KinBody::KinBodyInfoPtr& pKinBodyInfo : info._vBodyInfos) {
+            // If we have more than one info that targets the same id / body name, throw - UpdateFromInfo is not designed to update the same object more than once in a call
+            if (!pKinBodyInfo->_id.empty() && !bodyIdsToUpdate.emplace(pKinBodyInfo->_id).second) {
+                throw OPENRAVE_EXCEPTION_FORMAT(_("duplicate body info ID '%s' in call to UpdateFromInfo"), pKinBodyInfo->_id, ORE_InvalidArguments);
+            }
+            if (!pKinBodyInfo->_name.empty() && !bodyNamesToUpdate.emplace(pKinBodyInfo->_name).second) {
+                throw OPENRAVE_EXCEPTION_FORMAT(_("duplicate body info name '%s' in call to UpdateFromInfo"), pKinBodyInfo->_name, ORE_InvalidArguments);
+            }
+        }
+
+        // Build a lookup table for the bodies available to be matched against from vBodies
+        // When trying to identify candidate matches, we can do two hash lookups instead of a sequential scan.
+        // If we match a body out of vBodies, then we just need to remove the id/name maps that linked to it and reset the body at that location.
+        // These maps use string_views over the bodies in vBodies; these bodies _are_ removed during iteration; care must be taken to erase the matching entries.
+        std::unordered_map<string_view, int> existingBodyIndicesById; // Map of body id -> environmentBodyIndex into vBodies
+        std::unordered_map<string_view, int> existingBodyIndicesByName; // Map of body name -> environmentBodyIndex into vBodies
+        for (size_t bodyIndex = 0; bodyIndex < vBodies.size(); bodyIndex++) {
+            // It's possible for holes to exist in vBodies since when bodies are removed from the environment their entries in _vecbodies are just nulled out.
+            if (!vBodies[bodyIndex]) {
+                continue;
             }
+
+            const KinBody& body = *vBodies[bodyIndex];
+            if (bodyIdsToUpdate.find(body.GetId()) == bodyIdsToUpdate.end() && bodyNamesToUpdate.find(body.GetName()) == bodyNamesToUpdate.end()) {
+                continue;
+            }
+            existingBodyIndicesById[body.GetId()] = bodyIndex;
+            existingBodyIndicesByName[body.GetName()] = bodyIndex;
         }
 
-        // Set of indices that have already been used for vBodies
-        std::unordered_set<int> usedBodyIndexSet;
+        // Keep a list of the bodies that we have matched to infos - in the event that the caller did _not_ specify OnlySpecifiedBodiesExact,
+        // when we are finished we will want to remove all bodies in the environment that are _not_ in this list.
+        // Store raw pointers here instead of shared pointers because we don't need to track ownership, just membership.
+        std::unordered_set<KinBody*> setBodiesWithMatchingInfos;
 
         // internally manipulates _vecbodies using _AddKinBody/_AddRobot/_RemoveKinBodyFromIterator
-        for(int inputBodyIndex = 0; inputBodyIndex < (int)info._vBodyInfos.size(); ++inputBodyIndex) {
-            const KinBody::KinBodyInfoConstPtr& pKinBodyInfo = info._vBodyInfos[inputBodyIndex];
+        for(const KinBody::KinBodyInfoPtr& pKinBodyInfo : info._vBodyInfos ) {
             const KinBody::KinBodyInfo& kinBodyInfo = *pKinBodyInfo;
             RAVELOG_VERBOSE_FORMAT("env=%s, id '%s', name '%s', _vGrabbedInfos=%d", GetNameId()%pKinBodyInfo->_id%pKinBodyInfo->_name%pKinBodyInfo->_vGrabbedInfos.size());
             RobotBase::RobotBaseInfoConstPtr pRobotBaseInfo = OPENRAVE_DYNAMIC_POINTER_CAST<const RobotBase::RobotBaseInfo>(pKinBodyInfo);
-            KinBodyPtr pMatchExistingBody; // matches to pKinBodyInfo
-            int bodyIndex = -1; // index to vBodies to use. -1 if not used
-            {
-                // find existing body in the env
-                std::vector<KinBodyPtr>::iterator itExistingSameId = vBodies.end();
-                std::vector<KinBodyPtr>::iterator itExistingSameName = vBodies.end();
-                std::vector<KinBodyPtr>::iterator itExistingSameIdName = vBodies.end();
-
-                if( updateMode == UFIM_OnlySpecifiedBodiesExact ) {
path: src/libopenrave-core/environment-core.h
user: login: woswos
body: Is `UFIM_OnlySpecifiedBodiesExact` correctly handled in the new version? I think there is a behavior change in the API after these changes.
created_at: 2025-05-01T06:59:27Z
author_association: COLLABORATOR
diff_hunk: @@ -3305,73 +3297,64 @@ class Environment : public EnvironmentBase
             }
         }
 
-        if( updateMode != UFIM_OnlySpecifiedBodiesExact ) {
-            // remove extra bodies at the end of vBodies
-            if( vBodies.size() > info._vBodyInfos.size() ) {
-                ExclusiveLock lock009(_mutexInterfaces);
-                for (std::vector<KinBodyPtr>::iterator itBody = vBodies.begin() + info._vBodyInfos.size(); itBody != vBodies.end(); ) {
-                    KinBodyPtr pBody = *itBody;
-                    if (!pBody) {
-                        ++itBody;
-                        continue;
-                    }
-                    RAVELOG_VERBOSE_FORMAT("remove extra body env=%s, id=%s, name=%s", GetNameId()%pBody->_id%pBody->_name);
path: src/libopenrave-core/environment-core.h
user: login: woswos
body: Can we keep the logs?
created_at: 2025-05-01T07:06:40Z
author_association: COLLABORATOR
diff_hunk: @@ -2986,181 +2986,179 @@ class Environment : public EnvironmentBase
             SharedLock lock533(_mutexInterfaces);
             vBodies = _vecbodies;
         }
-        {
-            for (std::vector<KinBodyPtr>::iterator it = vBodies.begin(); it != vBodies.end(); ) {
-                if (!*it) {
-                    it = vBodies.erase(it);
-                }
-                else {
-                    it++;
-                }
+
+        // If the environment is large but we are only updating a small number of bodies, building a full lookup table is expensive relative to the amount of lookups we actually do.
+        // We can reduce this somewhat by pre-filtering the set of names/ids that could ever be looked up, and only including those in our lookup table.
+        // This incurs (info._vBodyInfos.size() + vBodies.size()) more hash _lookups_, but means that we save up to (vBodies.size() - info._vBodyInfos.size()) _allocations_.
+        // Since we expect vBodies.size() >> info._vBodyInfos.size() in most cases, and allocations are more costly than lookups, this is overall a speedup despite the extra steps.
+        std::unordered_set<string_view> bodyIdsToUpdate; // string_views are over info._vBodyInfos, guaranteed lifetime for the rest of this function
+        std::unordered_set<string_view> bodyNamesToUpdate;
+        for (const KinBody::KinBodyInfoPtr& pKinBodyInfo : info._vBodyInfos) {
+            // If we have more than one info that targets the same id / body name, throw - UpdateFromInfo is not designed to update the same object more than once in a call
+            if (!pKinBodyInfo->_id.empty() && !bodyIdsToUpdate.emplace(pKinBodyInfo->_id).second) {
+                throw OPENRAVE_EXCEPTION_FORMAT(_("duplicate body info ID '%s' in call to UpdateFromInfo"), pKinBodyInfo->_id, ORE_InvalidArguments);
+            }
+            if (!pKinBodyInfo->_name.empty() && !bodyNamesToUpdate.emplace(pKinBodyInfo->_name).second) {
+                throw OPENRAVE_EXCEPTION_FORMAT(_("duplicate body info name '%s' in call to UpdateFromInfo"), pKinBodyInfo->_name, ORE_InvalidArguments);
+            }
+        }
+
+        // Build a lookup table for the bodies available to be matched against from vBodies
+        // When trying to identify candidate matches, we can do two hash lookups instead of a sequential scan.
+        // If we match a body out of vBodies, then we just need to remove the id/name maps that linked to it and reset the body at that location.
+        // These maps use string_views over the bodies in vBodies; these bodies _are_ removed during iteration; care must be taken to erase the matching entries.
+        std::unordered_map<string_view, int> existingBodyIndicesById; // Map of body id -> environmentBodyIndex into vBodies
+        std::unordered_map<string_view, int> existingBodyIndicesByName; // Map of body name -> environmentBodyIndex into vBodies
+        for (size_t bodyIndex = 0; bodyIndex < vBodies.size(); bodyIndex++) {
+            // It's possible for holes to exist in vBodies since when bodies are removed from the environment their entries in _vecbodies are just nulled out.
+            if (!vBodies[bodyIndex]) {
+                continue;
             }
+
+            const KinBody& body = *vBodies[bodyIndex];
+            if (bodyIdsToUpdate.find(body.GetId()) == bodyIdsToUpdate.end() && bodyNamesToUpdate.find(body.GetName()) == bodyNamesToUpdate.end()) {
+                continue;
+            }
+            existingBodyIndicesById[body.GetId()] = bodyIndex;
+            existingBodyIndicesByName[body.GetName()] = bodyIndex;
         }
 
-        // Set of indices that have already been used for vBodies
-        std::unordered_set<int> usedBodyIndexSet;
+        // Keep a list of the bodies that we have matched to infos - in the event that the caller did _not_ specify OnlySpecifiedBodiesExact,
+        // when we are finished we will want to remove all bodies in the environment that are _not_ in this list.
+        // Store raw pointers here instead of shared pointers because we don't need to track ownership, just membership.
+        std::unordered_set<KinBody*> setBodiesWithMatchingInfos;
 
         // internally manipulates _vecbodies using _AddKinBody/_AddRobot/_RemoveKinBodyFromIterator
-        for(int inputBodyIndex = 0; inputBodyIndex < (int)info._vBodyInfos.size(); ++inputBodyIndex) {
-            const KinBody::KinBodyInfoConstPtr& pKinBodyInfo = info._vBodyInfos[inputBodyIndex];
+        for(const KinBody::KinBodyInfoPtr& pKinBodyInfo : info._vBodyInfos ) {
             const KinBody::KinBodyInfo& kinBodyInfo = *pKinBodyInfo;
             RAVELOG_VERBOSE_FORMAT("env=%s, id '%s', name '%s', _vGrabbedInfos=%d", GetNameId()%pKinBodyInfo->_id%pKinBodyInfo->_name%pKinBodyInfo->_vGrabbedInfos.size());
             RobotBase::RobotBaseInfoConstPtr pRobotBaseInfo = OPENRAVE_DYNAMIC_POINTER_CAST<const RobotBase::RobotBaseInfo>(pKinBodyInfo);
-            KinBodyPtr pMatchExistingBody; // matches to pKinBodyInfo
-            int bodyIndex = -1; // index to vBodies to use. -1 if not used
-            {
-                // find existing body in the env
-                std::vector<KinBodyPtr>::iterator itExistingSameId = vBodies.end();
-                std::vector<KinBodyPtr>::iterator itExistingSameName = vBodies.end();
-                std::vector<KinBodyPtr>::iterator itExistingSameIdName = vBodies.end();
-
-                if( updateMode == UFIM_OnlySpecifiedBodiesExact ) {
path: src/libopenrave-core/environment-core.h
user: login: rschlaikjer
body: Can you elaborate? The `UFIM_OnlySpecifiedBodiesExact` case should be handled down on line 3301. See testopenrave PR 141 for the relevant tests.
created_at: 2025-05-01T08:45:25Z
author_association: COLLABORATOR
diff_hunk: @@ -3305,73 +3297,64 @@ class Environment : public EnvironmentBase
             }
         }
 
-        if( updateMode != UFIM_OnlySpecifiedBodiesExact ) {
-            // remove extra bodies at the end of vBodies
-            if( vBodies.size() > info._vBodyInfos.size() ) {
-                ExclusiveLock lock009(_mutexInterfaces);
-                for (std::vector<KinBodyPtr>::iterator itBody = vBodies.begin() + info._vBodyInfos.size(); itBody != vBodies.end(); ) {
-                    KinBodyPtr pBody = *itBody;
-                    if (!pBody) {
-                        ++itBody;
-                        continue;
-                    }
-                    RAVELOG_VERBOSE_FORMAT("remove extra body env=%s, id=%s, name=%s", GetNameId()%pBody->_id%pBody->_name);
path: src/libopenrave-core/environment-core.h
user: login: ziyan
body: Added back.
created_at: 2025-05-01T14:20:16Z
author_association: COLLABORATOR
diff_hunk: @@ -2986,181 +2986,179 @@ class Environment : public EnvironmentBase
             SharedLock lock533(_mutexInterfaces);
             vBodies = _vecbodies;
         }
-        {
-            for (std::vector<KinBodyPtr>::iterator it = vBodies.begin(); it != vBodies.end(); ) {
-                if (!*it) {
-                    it = vBodies.erase(it);
-                }
-                else {
-                    it++;
-                }
+
+        // If the environment is large but we are only updating a small number of bodies, building a full lookup table is expensive relative to the amount of lookups we actually do.
+        // We can reduce this somewhat by pre-filtering the set of names/ids that could ever be looked up, and only including those in our lookup table.
+        // This incurs (info._vBodyInfos.size() + vBodies.size()) more hash _lookups_, but means that we save up to (vBodies.size() - info._vBodyInfos.size()) _allocations_.
+        // Since we expect vBodies.size() >> info._vBodyInfos.size() in most cases, and allocations are more costly than lookups, this is overall a speedup despite the extra steps.
+        std::unordered_set<string_view> bodyIdsToUpdate; // string_views are over info._vBodyInfos, guaranteed lifetime for the rest of this function
+        std::unordered_set<string_view> bodyNamesToUpdate;
+        for (const KinBody::KinBodyInfoPtr& pKinBodyInfo : info._vBodyInfos) {
+            // If we have more than one info that targets the same id / body name, throw - UpdateFromInfo is not designed to update the same object more than once in a call
+            if (!pKinBodyInfo->_id.empty() && !bodyIdsToUpdate.emplace(pKinBodyInfo->_id).second) {
+                throw OPENRAVE_EXCEPTION_FORMAT(_("duplicate body info ID '%s' in call to UpdateFromInfo"), pKinBodyInfo->_id, ORE_InvalidArguments);
+            }
+            if (!pKinBodyInfo->_name.empty() && !bodyNamesToUpdate.emplace(pKinBodyInfo->_name).second) {
+                throw OPENRAVE_EXCEPTION_FORMAT(_("duplicate body info name '%s' in call to UpdateFromInfo"), pKinBodyInfo->_name, ORE_InvalidArguments);
+            }
+        }
+
+        // Build a lookup table for the bodies available to be matched against from vBodies
+        // When trying to identify candidate matches, we can do two hash lookups instead of a sequential scan.
+        // If we match a body out of vBodies, then we just need to remove the id/name maps that linked to it and reset the body at that location.
+        // These maps use string_views over the bodies in vBodies; these bodies _are_ removed during iteration; care must be taken to erase the matching entries.
+        std::unordered_map<string_view, int> existingBodyIndicesById; // Map of body id -> environmentBodyIndex into vBodies
+        std::unordered_map<string_view, int> existingBodyIndicesByName; // Map of body name -> environmentBodyIndex into vBodies
+        for (size_t bodyIndex = 0; bodyIndex < vBodies.size(); bodyIndex++) {
+            // It's possible for holes to exist in vBodies since when bodies are removed from the environment their entries in _vecbodies are just nulled out.
+            if (!vBodies[bodyIndex]) {
+                continue;
             }
+
+            const KinBody& body = *vBodies[bodyIndex];
+            if (bodyIdsToUpdate.find(body.GetId()) == bodyIdsToUpdate.end() && bodyNamesToUpdate.find(body.GetName()) == bodyNamesToUpdate.end()) {
+                continue;
+            }
+            existingBodyIndicesById[body.GetId()] = bodyIndex;
+            existingBodyIndicesByName[body.GetName()] = bodyIndex;
         }
 
-        // Set of indices that have already been used for vBodies
-        std::unordered_set<int> usedBodyIndexSet;
+        // Keep a list of the bodies that we have matched to infos - in the event that the caller did _not_ specify OnlySpecifiedBodiesExact,
+        // when we are finished we will want to remove all bodies in the environment that are _not_ in this list.
+        // Store raw pointers here instead of shared pointers because we don't need to track ownership, just membership.
+        std::unordered_set<KinBody*> setBodiesWithMatchingInfos;
 
         // internally manipulates _vecbodies using _AddKinBody/_AddRobot/_RemoveKinBodyFromIterator
-        for(int inputBodyIndex = 0; inputBodyIndex < (int)info._vBodyInfos.size(); ++inputBodyIndex) {
-            const KinBody::KinBodyInfoConstPtr& pKinBodyInfo = info._vBodyInfos[inputBodyIndex];
+        for(const KinBody::KinBodyInfoPtr& pKinBodyInfo : info._vBodyInfos ) {
             const KinBody::KinBodyInfo& kinBodyInfo = *pKinBodyInfo;
             RAVELOG_VERBOSE_FORMAT("env=%s, id '%s', name '%s', _vGrabbedInfos=%d", GetNameId()%pKinBodyInfo->_id%pKinBodyInfo->_name%pKinBodyInfo->_vGrabbedInfos.size());
             RobotBase::RobotBaseInfoConstPtr pRobotBaseInfo = OPENRAVE_DYNAMIC_POINTER_CAST<const RobotBase::RobotBaseInfo>(pKinBodyInfo);
-            KinBodyPtr pMatchExistingBody; // matches to pKinBodyInfo
-            int bodyIndex = -1; // index to vBodies to use. -1 if not used
-            {
-                // find existing body in the env
-                std::vector<KinBodyPtr>::iterator itExistingSameId = vBodies.end();
-                std::vector<KinBodyPtr>::iterator itExistingSameName = vBodies.end();
-                std::vector<KinBodyPtr>::iterator itExistingSameIdName = vBodies.end();
-
-                if( updateMode == UFIM_OnlySpecifiedBodiesExact ) {
path: src/libopenrave-core/environment-core.h
user: login: ziyan
body: I think the old logic here was to ignrore duplicated matching in case of `UFIM_OnlySpecifiedBodiesExact`, i.e. if the caller passed in a list of kinbody info that has duplicates. I think that's automatically an error, which is guarded by exceptions added in the beginning of the function.
created_at: 2025-05-01T14:31:12Z
author_association: COLLABORATOR

PR comments:
user: login: rschlaikjer
created_at: 2025-04-03T08:35:24Z
body: While writing tests, noticed a potentially confusing behaviour in current production - 
https://github.com/rdiankov/openrave/pull/1500/files#diff-c5c3a9bad867a7032c72acc1708c24dde1866ad3d1c43ae5fca006461105f850R3238

If we have an update that contains two kin body infos with the same name, we will create two bodies (one renamed to be unique) but then fail this assert that expects both bodies to match the names that were present in the info. Should this assert be removed?
author_association: COLLABORATOR
user: login: rdiankov
created_at: 2025-04-28T15:33:51Z
body: @cielavenir has this gone through the openrave testing pipeline?


author_association: OWNER
user: login: cielavenir
created_at: 2025-04-29T09:42:16Z
body: @rdiankov now openrave ci needs https://github.com/rdiankov/openrave/pull/1496
author_association: COLLABORATOR
user: login: cielavenir
created_at: 2025-04-30T00:05:58Z
body: seems this one is ok
author_association: COLLABORATOR
user: login: ziyan
created_at: 2025-05-01T15:19:27Z
body: LGTM
author_association: COLLABORATOR
user: login: rdiankov
created_at: 2025-05-13T15:49:02Z
body: @rschlaikjer  `vBodies` was not necessary and was getting copied. Furthermore, it was introducing two sets of BodyIndices, one for vBodies and one for _vecbodies, which diverge as update happens and makes the code harder to understand.
By getting rid of `vBodies` we save an expensive copy of all the bodies in the scene and can remove code that managed vBodies.
Please review the changes, thanks.

author_association: OWNER
user: login: rschlaikjer
created_at: 2025-05-14T02:36:46Z
body: Changes look good, thanks. Updated a couple comments to reflect the new behaviour. 
author_association: COLLABORATOR
user: login: rdiankov
created_at: 2025-05-14T03:56:40Z
body: Thanks~
author_association: OWNER