Ticket #1707: rangemanager_vectorization_binsearch.patch

source/simulation2/components/CCmpRangeManager.cpp

@@ -92 +92 @@
  */
 struct EntityData
 {
-    EntityData() : retainInFog(0), owner(-1), inWorld(0), flags(1) { }
+    EntityData(entity_id_t entity = INVALID_ENTITY) : 
+        id(entity), retainInFog(0), owner(-1), inWorld(0), flags(1) 
+    { 
+    }
+
+    entity_id_t id;
     entity_pos_t x, z;
     entity_pos_t visionRange;
     u8 retainInFog; // boolean
@@ -101 +106 @@
     u8 flags; // See GetEntityFlagMask
 };
 
-cassert(sizeof(EntityData) == 16);
+cassert(sizeof(EntityData) == 20);
 
 
 /**
@@ -131 +136 @@
     template<typename S>
     void operator()(S& serialize, const char* UNUSED(name), EntityData& value)
     {
+        serialize.NumberU32_Unbounded("id", value.id);
         serialize.NumberFixed_Unbounded("x", value.x);
         serialize.NumberFixed_Unbounded("z", value.z);
         serialize.NumberFixed_Unbounded("vision", value.visionRange);
@@ -143 +149 @@
 
 
 /**
+ * Search entity by id in a vector of EntityData.
+ * Adapted binary search, for sorted set of (non-negative) integers. The min and max entity id's are used to speed up the search.
+ */
+cassert(sizeof(entity_id_t) <= sizeof(size_t));
+
+template<class EntityDataRandomAccessIterator>
+static inline EntityDataRandomAccessIterator find_entity(EntityDataRandomAccessIterator first, EntityDataRandomAccessIterator last, entity_id_t entity, entity_id_t minentity, entity_id_t maxentity)
+{
+    if (first == last)
+    {
+        return last;
+    }
+    
+    size_t imin, imax, minoffset, maxoffset;
+    imin = 0;
+    imax = (last-first)-1;
+    minoffset = (size_t)minentity-imin;
+    maxoffset = (size_t)maxentity-imax;
+    imin = std::max<size_t>(imin, (entity>maxoffset)?entity-maxoffset:0);
+    imax = std::min<size_t>(imax, (entity>minoffset)?entity-minoffset:0);
+    while(imax >= imin)
+    {
+        size_t imid = imin + ((imax - imin) / 2);
+        entity_id_t entitymid = first[imid].id;
+        if (entity < entitymid)
+        {
+            maxoffset = entitymid - imid;
+            imin = std::max<size_t>(imin, (entity>maxoffset)?entity-maxoffset:0);
+            imax = imid - 1;
+        }
+        else if (entity > entitymid)
+        {
+            minoffset = entitymid - imid;
+            imin = imid + 1;
+            imax = std::min<size_t>(imax, (entity>minoffset)?entity-minoffset:0);
+        }
+        else
+        {
+            return first + imid;
+        }
+    }
+    return last;
+}
+
+template<class EntityDataRandomAccessIterator>
+static inline EntityDataRandomAccessIterator find_entity(EntityDataRandomAccessIterator first, EntityDataRandomAccessIterator last, entity_id_t entity)
+{
+    if (first == last)
+    {
+        return last;
+    }
+    
+    entity_id_t minentity = first->id;
+    entity_id_t maxentity = (last-1)->id;
+    return find_entity(first, last, entity, minentity, maxentity);
+}
+
+/**
+ * Standard binary search
+ */ 
+struct EntityIdComp
+{
+    inline bool operator()(const EntityData &data, const entity_id_t id)
+    {
+        return data.id < id;
+    }
+};
+template<class EntityDataRandomAccessIterator>
+static inline EntityDataRandomAccessIterator find_entity_std(EntityDataRandomAccessIterator first, EntityDataRandomAccessIterator last, entity_id_t entity)
+{
+    EntityIdComp comp;
+    EntityDataRandomAccessIterator it = std::lower_bound(first, last, entity, comp);
+
+    if (it != last && it->id == entity)
+    {
+        return it;
+    }
+    else
+    {
+        return last;
+    }
+}
+
+/**
  * Functor for sorting entities by distance from a source point.
  * It must only be passed entities that are in 'entities'
  * and are currently in the world.
  */
 struct EntityDistanceOrdering
 {
-    EntityDistanceOrdering(const std::map<entity_id_t, EntityData>& entities, const CFixedVector2D& source) :
+    EntityDistanceOrdering(const std::vector<EntityData> &entities, const CFixedVector2D& source) :
         m_EntityData(entities), m_Source(source)
     {
+        m_MinEntityId = m_EntityData.front().id;
+        m_MaxEntityId = m_EntityData.back().id;
     }
 
     bool operator()(entity_id_t a, entity_id_t b)
     {
-        const EntityData& da = m_EntityData.find(a)->second;
-        const EntityData& db = m_EntityData.find(b)->second;
+        const EntityData& da = *find_entity(m_EntityData.begin(), m_EntityData.end(), a, m_MinEntityId, m_MaxEntityId);
+        const EntityData& db = *find_entity(m_EntityData.begin(), m_EntityData.end(), b, m_MinEntityId, m_MaxEntityId);
         CFixedVector2D vecA = CFixedVector2D(da.x, da.z) - m_Source;
         CFixedVector2D vecB = CFixedVector2D(db.x, db.z) - m_Source;
         return (vecA.CompareLength(vecB) < 0);
     }
 
-    const std::map<entity_id_t, EntityData>& m_EntityData;
+    const std::vector<EntityData> m_EntityData;
+    entity_id_t m_MinEntityId, m_MaxEntityId;
     CFixedVector2D m_Source;
 
 private:
@@ -210 +303 @@
     // Range query state:
     tag_t m_QueryNext; // next allocated id
     std::map<tag_t, Query> m_Queries;
-    std::map<entity_id_t, EntityData> m_EntityData;
+    std::vector<EntityData> m_EntityData;
     SpatialSubdivision<entity_id_t> m_Subdivision; // spatial index of m_EntityData
 
     // LOS state:
@@ -284 +377 @@
 
         serialize.NumberU32_Unbounded("query next", m_QueryNext);
         SerializeMap<SerializeU32_Unbounded, SerializeQuery>()(serialize, "queries", m_Queries);
-        SerializeMap<SerializeU32_Unbounded, SerializeEntityData>()(serialize, "entity data", m_EntityData);
+        SerializeVector<SerializeEntityData>()(serialize, "entity data", m_EntityData);
 
         SerializeMap<SerializeI32_Unbounded, SerializeBool>()(serialize, "los reveal all", m_LosRevealAll);
         serialize.Bool("los circular", m_LosCircular);
@@ -335 +428 @@
             // The newly-created entity will have owner -1 and position out-of-world
             // (any initialisation of those values will happen later), so we can just
             // use the default-constructed EntityData here
-            EntityData entdata;
+            EntityData entdata(ent);
 
             // Store the LOS data, if any
             CmpPtr<ICmpVision> cmpVision(GetSimContext(), ent);
@@ -345 +438 @@
                 entdata.retainInFog = (cmpVision->GetRetainInFog() ? 1 : 0);
             }
 
+            // The entity id's are expected to be increasing
+            ENSURE(m_EntityData.empty() || ent > m_EntityData.back().id);
+            
             // Remember this entity
-            m_EntityData.insert(std::make_pair(ent, entdata));
+            m_EntityData.push_back(entdata);
 
             break;
         }
@@ -355 +451 @@
             const CMessagePositionChanged& msgData = static_cast<const CMessagePositionChanged&> (msg);
             entity_id_t ent = msgData.entity;
 
-            std::map<entity_id_t, EntityData>::iterator it = m_EntityData.find(ent);
+            std::vector<EntityData>::iterator it = FindEntityData(ent);
 
             // Ignore if we're not already tracking this entity
             if (it == m_EntityData.end())
@@ -363 +459 @@
 
             if (msgData.inWorld)
             {
-                if (it->second.inWorld)
+                if (it->inWorld)
                 {
-                    CFixedVector2D from(it->second.x, it->second.z);
+                    CFixedVector2D from(it->x, it->z);
                     CFixedVector2D to(msgData.x, msgData.z);
                     m_Subdivision.Move(ent, from, to);
-                    LosMove(it->second.owner, it->second.visionRange, from, to);
+                    LosMove(it->owner, it->visionRange, from, to);
                 }
                 else
                 {
                     CFixedVector2D to(msgData.x, msgData.z);
                     m_Subdivision.Add(ent, to);
-                    LosAdd(it->second.owner, it->second.visionRange, to);
+                    LosAdd(it->owner, it->visionRange, to);
                 }
 
-                it->second.inWorld = 1;
-                it->second.x = msgData.x;
-                it->second.z = msgData.z;
+                it->inWorld = 1;
+                it->x = msgData.x;
+                it->z = msgData.z;
             }
             else
             {
-                if (it->second.inWorld)
+                if (it->inWorld)
                 {
-                    CFixedVector2D from(it->second.x, it->second.z);
+                    CFixedVector2D from(it->x, it->z);
                     m_Subdivision.Remove(ent, from);
-                    LosRemove(it->second.owner, it->second.visionRange, from);
+                    LosRemove(it->owner, it->visionRange, from);
                 }
 
-                it->second.inWorld = 0;
-                it->second.x = entity_pos_t::Zero();
-                it->second.z = entity_pos_t::Zero();
+                it->inWorld = 0;
+                it->x = entity_pos_t::Zero();
+                it->z = entity_pos_t::Zero();
             }
 
             break;
@@ -402 +498 @@
             const CMessageOwnershipChanged& msgData = static_cast<const CMessageOwnershipChanged&> (msg);
             entity_id_t ent = msgData.entity;
 
-            std::map<entity_id_t, EntityData>::iterator it = m_EntityData.find(ent);
+            std::vector<EntityData>::iterator it = FindEntityData(ent);
 
             // Ignore if we're not already tracking this entity
             if (it == m_EntityData.end())
                 break;
 
-            if (it->second.inWorld)
+            if (it->inWorld)
             {
-                CFixedVector2D pos(it->second.x, it->second.z);
-                LosRemove(it->second.owner, it->second.visionRange, pos);
-                LosAdd(msgData.to, it->second.visionRange, pos);
+                CFixedVector2D pos(it->x, it->z);
+                LosRemove(it->owner, it->visionRange, pos);
+                LosAdd(msgData.to, it->visionRange, pos);
             }
 
             ENSURE(-128 <= msgData.to && msgData.to <= 127);
-            it->second.owner = (i8)msgData.to;
+            it->owner = (i8)msgData.to;
 
             break;
         }
@@ -425 +521 @@
             const CMessageDestroy& msgData = static_cast<const CMessageDestroy&> (msg);
             entity_id_t ent = msgData.entity;
 
-            std::map<entity_id_t, EntityData>::iterator it = m_EntityData.find(ent);
+            std::vector<EntityData>::iterator it = FindEntityData(ent);
 
             // Ignore if we're not already tracking this entity
             if (it == m_EntityData.end())
                 break;
 
-            if (it->second.inWorld)
-                m_Subdivision.Remove(ent, CFixedVector2D(it->second.x, it->second.z));
+            if (it->inWorld)
+                m_Subdivision.Remove(ent, CFixedVector2D(it->x, it->z));
 
             // This will be called after Ownership's OnDestroy, so ownership will be set
             // to -1 already and we don't have to do a LosRemove here
-            ENSURE(it->second.owner == -1);
+            ENSURE(it->owner == -1);
 
+            // TODO: be smarter and just mark the data as deleted and then do a bulk-remove once performing queries.
             m_EntityData.erase(it);
 
             break;
@@ -447 +544 @@
             const CMessageVisionRangeChanged& msgData = static_cast<const CMessageVisionRangeChanged&> (msg);
             entity_id_t ent = msgData.entity;
 
-            std::map<entity_id_t, EntityData>::iterator it = m_EntityData.find(ent);
+            std::vector<EntityData>::iterator it = FindEntityData(ent);
 
             // Ignore if we're not already tracking this entity
             if (it == m_EntityData.end())
@@ -457 +554 @@
             if (!cmpVision)
                 break;
 
-            entity_pos_t oldRange = it->second.visionRange;
+            entity_pos_t oldRange = it->visionRange;
             entity_pos_t newRange = msgData.newRange;
 
             // If the range changed and the entity's in-world, we need to manually adjust it
             //  but if it's not in-world, we only need to set the new vision range
-            CFixedVector2D pos(it->second.x, it->second.z);
-            if (it->second.inWorld)
-                LosRemove(it->second.owner, oldRange, pos);
+            CFixedVector2D pos(it->x, it->z);
+            if (it->inWorld)
+                LosRemove(it->owner, oldRange, pos);
 
-            it->second.visionRange = newRange;
+            it->visionRange = newRange;
 
-            if (it->second.inWorld)
-                LosAdd(it->second.owner, newRange, pos);
+            if (it->inWorld)
+                LosAdd(it->owner, newRange, pos);
 
             break;
         }
@@ -555 +652 @@
         m_LosStateRevealed.clear();
         m_LosStateRevealed.resize(m_TerrainVerticesPerSide*m_TerrainVerticesPerSide);
 
-        for (std::map<entity_id_t, EntityData>::const_iterator it = m_EntityData.begin(); it != m_EntityData.end(); ++it)
+        for (std::vector<EntityData>::const_iterator it = m_EntityData.begin(); it != m_EntityData.end(); ++it)
         {
-            if (it->second.inWorld)
-                LosAdd(it->second.owner, it->second.visionRange, CFixedVector2D(it->second.x, it->second.z));
+            if (it->inWorld)
+                LosAdd(it->owner, it->visionRange, CFixedVector2D(it->x, it->z));
         }
 
         for (ssize_t j = 0; j < m_TerrainVerticesPerSide; ++j)
@@ -572 +669 @@
         // (TODO: find the optimal number instead of blindly guessing)
         m_Subdivision.Reset(x1, z1, entity_pos_t::FromInt(8*TERRAIN_TILE_SIZE));
 
-        for (std::map<entity_id_t, EntityData>::const_iterator it = m_EntityData.begin(); it != m_EntityData.end(); ++it)
+        for (std::vector<EntityData>::const_iterator it = m_EntityData.begin(); it != m_EntityData.end(); ++it)
         {
-            if (it->second.inWorld)
-                m_Subdivision.Add(it->first, CFixedVector2D(it->second.x, it->second.z));
+            if (it->inWorld)
+                m_Subdivision.Add(it->id, CFixedVector2D(it->x, it->z));
         }
     }
 
@@ -589 +686 @@
         return id;
     }
 
+    virtual std::vector<EntityData>::iterator FindEntityData(entity_id_t entity)
+    {
+        std::vector<EntityData>::iterator it = find_entity(m_EntityData.begin(), m_EntityData.end(), entity);
+        return it;
+    }
+
     virtual void DestroyActiveQuery(tag_t tag)
     {
         if (m_Queries.find(tag) == m_Queries.end())
@@ -693 +796 @@
 
         u32 ownerMask = CalcOwnerMask(player);
 
-        for (std::map<entity_id_t, EntityData>::const_iterator it = m_EntityData.begin(); it != m_EntityData.end(); ++it)
+        for (std::vector<EntityData>::const_iterator it = m_EntityData.begin(); it != m_EntityData.end(); ++it)
         {
             // Check owner and add to list if it matches
-            if (CalcOwnerMask(it->second.owner) & ownerMask)
-                entities.push_back(it->first);
+            if (CalcOwnerMask(it->owner) & ownerMask)
+                entities.push_back(it->id);
         }
 
         return entities;
@@ -766 +869 @@
     /**
      * Returns whether the given entity matches the given query (ignoring maxRange)
      */
-    bool TestEntityQuery(const Query& q, entity_id_t id, const EntityData& entity)
+    bool TestEntityQuery(const Query& q, const EntityData& entity)
     {
         // Quick filter to ignore entities with the wrong owner
         if (!(CalcOwnerMask(entity.owner) & q.ownersMask))
@@ -781 +884 @@
             return false;
 
         // Ignore self
-        if (id == q.source)
+        if (entity.id == q.source)
             return false;
 
         // Ignore if it's missing the required interface
-        if (q.interface && !GetSimContext().GetComponentManager().QueryInterface(id, q.interface))
+        if (q.interface && !GetSimContext().GetComponentManager().QueryInterface(entity.id, q.interface))
             return false;
 
         return true;
@@ -796 +899 @@
      */
     void PerformQuery(const Query& q, std::vector<entity_id_t>& r)
     {
+        PROFILE("PerformQuery");
         CmpPtr<ICmpPosition> cmpSourcePosition(GetSimContext(), q.source);
         if (!cmpSourcePosition || !cmpSourcePosition->IsInWorld())
             return;
@@ -804 +908 @@
         // Special case: range -1.0 means check all entities ignoring distance
         if (q.maxRange == entity_pos_t::FromInt(-1))
         {
-            for (std::map<entity_id_t, EntityData>::const_iterator it = m_EntityData.begin(); it != m_EntityData.end(); ++it)
+            for (std::vector<EntityData>::const_iterator it = m_EntityData.begin(); it != m_EntityData.end(); ++it)
             {
-                if (!TestEntityQuery(q, it->first, it->second))
+                if (!TestEntityQuery(q, *it))
                     continue;
 
-                r.push_back(it->first);
+                r.push_back(it->id);
             }
         }
         else
@@ -819 +923 @@
 
             for (size_t i = 0; i < ents.size(); ++i)
             {
-                std::map<entity_id_t, EntityData>::const_iterator it = m_EntityData.find(ents[i]);
+                std::vector<EntityData>::const_iterator it = FindEntityData(ents[i]);
                 ENSURE(it != m_EntityData.end());
 
-                if (!TestEntityQuery(q, it->first, it->second))
+                if (!TestEntityQuery(q, *it))
                     continue;
 
                 // Restrict based on precise distance
-                int distVsMax = (CFixedVector2D(it->second.x, it->second.z) - pos).CompareLength(q.maxRange);
+                int distVsMax = (CFixedVector2D(it->x, it->z) - pos).CompareLength(q.maxRange);
                 if (distVsMax > 0)
                     continue;
 
                 if (!q.minRange.IsZero())
                 {
-                    int distVsMin = (CFixedVector2D(it->second.x, it->second.z) - pos).CompareLength(q.minRange);
+                    int distVsMin = (CFixedVector2D(it->x, it->z) - pos).CompareLength(q.minRange);
                     if (distVsMin < 0)
                         continue;
                 }
 
-                r.push_back(it->first);
+                r.push_back(ents[i]);
             }
         }
     }
@@ -945 +1049 @@
     
     virtual void SetEntityFlag(entity_id_t ent, std::string identifier, bool value)
     {
-        std::map<entity_id_t, EntityData>::iterator it = m_EntityData.find(ent);
+        std::vector<EntityData>::iterator it = FindEntityData(ent);
 
         // We don't have this entity
         if (it == m_EntityData.end())
@@ -961 +1065 @@
         }
 
         if (value)
-            it->second.flags |= flag;
+            it->flags |= flag;
         else
-            it->second.flags &= ~flag;
+            it->flags &= ~flag;
     }
 
     // ****************************************************************