perf: Optimize Query cache lookup and add CountEntities method

src/Arch/Core/Query.cs

@@ -1,3 +1,4 @@
+using System.Diagnostics.Contracts;
 using Arch.Core.Extensions;
 using Arch.Core.Extensions.Internal;
 using Arch.Core.Utils;
@@ -571,6 +572,22 @@ internal Query(Archetypes allArchetypes, QueryDescription description)
         _queryDescription = description;
     }
 
+    /// <summary>
+    ///     Counts all <see cref="Entity"/>s that match this query.
+    /// </summary>
+    [Pure]
+    public int CountEntities()
+    {
+        var counter = 0;
+        foreach (var archetype in GetArchetypeIterator())
+        {
+            var entities = archetype.EntityCount;
+            counter += entities;
+        }
+
+        return counter;
+    }
+
     /// <summary>
     ///     Checks whether the specified <see cref="BitSet"/> matches.
     /// </summary>

src/Arch/Core/World.cs

@@ -410,17 +410,30 @@ public void Destroy(Entity entity)
     [Pure]
     public Query Query(in QueryDescription queryDescription)
     {
-        // Looping over all archetypes, their chunks and their entities.
-        var queryCache = QueryCache; // Storing locally to only access the QueryCache once
+        var queryCache = QueryCache; // Store locally to only access the field once
+
+        #if NET6_0_OR_GREATER
+        ref var queryRef = ref CollectionsMarshal.GetValueRefOrAddDefault(queryCache, queryDescription, out bool exists);
+        if (exists)
+        {
+            return queryRef!;
+        }
+        else
+        {
+            var newQuery = new Query(Archetypes, queryDescription);
+            queryRef = newQuery;
+            return newQuery;
+        }
+        #else
         if (queryCache.TryGetValue(queryDescription, out var query))
         {
             return query;
         }
 
         query = new Query(Archetypes, queryDescription);
-        queryCache[queryDescription] = query;
-
+        queryCache.Add(queryDescription, query);
         return query;
+        #endif
     }
 
     /// <summary>