Optimize narrow phase contact detection for phyiscs simulation code I'm trying to optimize the narrow phase of the contact detection in my code. I'm using OpenMP for multithreading.
That is the starting code:
// Narrow Phase
        const unsigned int NbPairs = pair_manager_-&gt;get_n_pairs();
        delete[] sum_rad_;
        sum_rad_ = new double[NbPairs];
        delete[] distance_vector_;
        distance_vector_ = new Vector3d[NbPairs];
        delete[] sq_dist_;
        sq_dist_ = new double[NbPairs];
        const Pair * Pairs = pair_manager_-&gt;get_pairs();
        int n_active = 0;
        auto * active = new bool[NbPairs];
#pragma omp parallel default(none) shared(active, Pairs, n_active) firstprivate(NbPairs) num_threads(current_num_threads_)
        {
#pragma omp for schedule(static)
            for (int i = 0; i&lt;NbPairs; i++) {
                const int id_0 = Pairs[i].id_0_;
                const int id_1 = Pairs[i].id_1_;
                sum_rad_[i] = radius_[id_0]+radius_[id_1];
                distance_vector_[i] = position_[id_1]-position_[id_0];
                sq_dist_[i] = distance_vector_[i].squaredNorm();
            }
            Pair * PairsNonConst = pair_manager_-&gt;get_pairs_non_const();
#pragma omp for schedule(static) reduction(+:n_active)
            for (int i = 0; i&lt;NbPairs; i++) {
                if (sq_dist_[i]&lt;sum_rad_[i]*sum_rad_[i]) {
                    active[i] = true;
                    n_active++;
                } else {
                    PairsNonConst[i].data_id = -1;
                    active[i] = false;
                }
            }
        }
        delete[] active_pairs_;
        active_pairs_ = new int[n_active];
        n_active_pairs_ = n_active;
        int j = 0;
        // FIXME: is it possible to include it in the parallel loop? -&gt; I guess so, then do merge reduction
        for (int i = 0; i&lt;NbPairs; i++) {
            if (active[i]) {
                active_pairs_[j] = i;
                j++;
            }
        }
        assert(j==n_active_pairs_);
        delete[] active;

The pair manager contains the candidate contact pairs, which has to be checked in the narrow phase. I only have spherical objects, so this is fairly easy (if squared sum of radii is larger than the squared distance of the objects, then we have a contact).
In a previous developing phase, I splitted the two for loops to get performance benefits, so that the first one does not contain any if statement. So, in the first one I calculate all the relevant quantities for all contact candidates, in the second one I have to check every candidate for actual contact.
After that, I have to loop over the active array and save the indexes of each active contact pair. In another part of the code, I'll then loop over this array active_pairs_ and calculate the contact forces, i.e. by indirectly accessing sq_dist_ and distance_vector_ in this way:
#pragma omp for schedule(static)
for (int i=0; i&lt;n_active_pairs_; i++) {
   int id_pair = active_pairs_[i];
   const Vector3d distance = distance_vector_[id_pair];
   // ...
}

Ideally, I would like to save only the quantities for the actual contact pairs in sum_rad_, distance_vector_, and sq_dist_ so that in this for loop I can only loop over those arrays.
Also, this would save me the last serial loop in the narrow phase. On the other hand it would mean merging the two loops in the parallel region (unless someone has a better idea on how to do that).
Last but not least, with my approach, I cannot have a proper first touch (important on NUMA systems). It would be nice to get this somehow as well.
What are your thoughts?
delete[] sum_rad_;
    sum_rad_ = new double[NbPairs];
    delete[] distance_vector_;
    distance_vector_ = new Vector3d[NbPairs];
    delete[] sq_dist_;
    sq_dist_ = new double[NbPairs];


Why all these bare array-pointers that you have to manage yourself?  You'd have much more maintainable code with std::vector or even just std::unique_ptr&lt;T[]&gt;.