SoftMinMax: Improve performance by caching the maximal heap size

SimoxUtility/math/SoftMinMax.cpp

@@ -37,6 +37,8 @@ namespace simox::math
         }
         this->percentile = percentile;
         this->num_elements = numValues;
+
+        allowed_heap_size_cache = allowed_heap_size();
     }
 
     void SoftMinMax::add(float value)
@@ -49,7 +51,7 @@ namespace simox::math
             throw std::logic_error(msg.str());
         }
 
-        if (min_queue.size() < allowed_heap_size())
+        if (min_queue.size() < allowed_heap_size_cache)
         {
             // Heaps not full yet
             min_queue.push(value);

SimoxUtility/math/SoftMinMax.h

@@ -66,6 +66,8 @@ namespace simox::math
         /// The number of elements to be added.
         std::size_t num_elements = 0;
 
+        std::size_t allowed_heap_size_cache = 0;
+
 
         using Container = std::vector<float>;
         using MaxCompare = std::less<float>;     // Results in max heap.