the whole trick of this algorithm is, that m_worldObjects and worldObjects contain the pointers in a sorted order, so no unordered_set I guess. If set is implemented as a tree then going to the next element in order is slower than n, granted, but not as slow as n*log(n), methinks. Ideas of a better structure than set that inserts sorted?