* c/nondominated.h (find_nondominated_set_3d_helper): Rename as

   find_nondominated_3d_impl.
   (find_nondominated_3d_impl_sorted): New. Assumes input is sorted.
   Simplify implementation by removing nondom and setting rows to NULL instead.
 * c/nondominated_kung.h (find_nondominated_set_3d_helper): Copy that keeps
   the previous behavior.

Author: MLopez-Ibanez

c/nondominated.h

@@ -339,18 +339,16 @@ find_nondominated_set_2d_(const double * restrict points, size_t size,
    D. Jesus, Luís Paquete, A software library for archiving nondominated
    points, GECCO 2021. https://github.com/TLDart/nondLib/blob/main/nondlib.hpp
 
-   rows will be sorted by cmp_ppdouble_asc_rev_3d().
-   max_dim will be != 3 only if the rows pointers have been shifted.
+   rows should be already sorted by cmp_ppdouble_asc_rev_3d().
+
+   When find_one_dominated, return as soon as it finds one dominated point.
 */
-static inline size_t
-find_nondominated_set_3d_helper(const double * restrict points,
-                                const double ** rows,
-                                size_t size, dimension_t max_dim,
-                                const bool keep_weakly, boolvec * restrict nondom)
+static __force_inline__ size_t
+find_nondominated_3d_impl_sorted(const double ** rows, size_t size,
+                                 const bool keep_weakly,
+                                 const bool find_one_dominated)
 {
     ASSUME(size > 1);
-    // Sort in ascending lexicographic order from the last dimension.
-    qsort_typesafe(rows, size, cmp_ppdouble_asc_rev_3d);
 
     avl_tree_t tree;
     avl_init_tree(&tree, qsort_cmp_pdouble_asc_x_nonzero);
@@ -364,99 +362,123 @@ find_nondominated_set_3d_helper(const double * restrict points,
     avl_insert_after(&tree, node - 1, node);
 
     // In this context, size means "no dominated solution found".
-    size_t new_size = size, j = 1;
-    const double * last_dom = NULL;
+    size_t new_size = size, k = 0;
+    size_t last_dom_pos = size;
     const double * restrict pk = rows[0];
-    do {
-        bool dominated;
+    for (size_t j = 1; j < size; j++) {
         const double * restrict pj = rows[j];
         DEBUG2(printf_point("pj = [ ", pj, 3, " ], "));
-        if (pk[0] > pj[0] || pk[1] > pj[1]) {
+
+        // First, handle duplicates and points that are dominated by the
+        // immediate previous one.
+        if (unlikely(pk[0] <= pj[0] && pk[1] <= pj[1])) {
+            DEBUG2(printf_point("weakly dominated by pk: ", pk, 3, "\n"));
+            const bool k_eq_j = (pk[0] == pj[0]) & (pk[1] == pj[1]) & (pk[2] == pj[2]);
+            if (likely(!k_eq_j)) { // it is not a duplicate, so it is non-weakly dominated.
+                goto j_is_dominated;
+            } else if (!keep_weakly) { // Don't keep duplicates
+                // Only the first duplicated point is kept.
+                if (pk < pj) {
+                    goto j_is_dominated;
+                }
+                if (find_one_dominated) {
+                    // In this context, it means "position of the first dominated solution found".
+                    new_size = k;
+                    goto early_end;
+                }
+                last_dom_pos = k;
+                rows[k] = NULL;
+                new_size--;
+                // Fall to j_is_NOT_dominated
+            } else if (last_dom_pos == k) // pk was dominated, then this one is also dominated;
+                goto j_is_dominated;
+            // Fall to j_is_NOT_dominated
+
+        } else {
+            // Check if pj is dominated by a point in the tree.
+            assert(pk[0] > pj[0] || pk[1] > pj[1]);
             avl_node_t * nodeaux;
             int res = avl_search_closest(&tree, pj, &nodeaux);
             assert(res != 0);
             if (res > 0) { // nodeaux goes before pj
                 const double * restrict prev = nodeaux->item;
+                DEBUG2(printf_point("res > 0: prev: ", prev, 3, "\n"));
                 assert(prev[0] != sentinel[0]);
                 assert(prev[0] <= pj[0]);
-                dominated = prev[1] <= pj[1];
-                DEBUG2(printf_point("res > 0: prev: ", prev, 3, "\n"));
+                if (prev[1] <= pj[1])
+                    goto j_is_dominated;
                 nodeaux = nodeaux->next;
             } else if (nodeaux->prev) { // nodeaux goes after pj, so move to the next one.
                 const double * restrict prev = nodeaux->prev->item;
+                DEBUG2(printf_point("res < 0: prev: ", prev, 3, "\n"));
                 assert(prev[0] != sentinel[0]);
                 assert(prev[0] <= pj[0]);
-                dominated = prev[1] <= pj[1];
-                DEBUG2(printf_point("res < 0: prev: ", prev, 3, "\n"));
-            } else {
-                dominated = false;
+                if (prev[1] <= pj[1])
+                    goto j_is_dominated;
             }
 
-            if (!dominated) { // pj is NOT dominated by a point in the tree.
-                const double * restrict point = nodeaux->item;
+            // pj is NOT dominated by a point in the tree.
+            const double * restrict point = nodeaux->item;
+            assert(pj[0] <= point[0]);
+            // Delete everything in the tree that is dominated by pj.
+            while (pj[1] <= point[1]) {
+                DEBUG2(printf_point("delete point: ", point, 3, "\n"));
                 assert(pj[0] <= point[0]);
-                // Delete everything in the tree that is dominated by pj.
-                while (pj[1] <= point[1]) {
-                    DEBUG2(printf_point("delete point: ", point, 3, "\n"));
-                    assert(pj[0] <= point[0]);
-                    nodeaux = nodeaux->next;
-                    point = nodeaux->item;
-                    /* FIXME: A possible speed up is to delete without
-                       rebalancing the tree because avl_insert_before() will
-                       rebalance. */
-                    avl_unlink_node(&tree, nodeaux->prev);
-                }
-                DEBUG2((point == sentinel)
-                       ? printf_point("insert before sentinel: ", sentinel, 2, "\n")
-                       : printf_point("insert before point: ", point, 3, "\n"));
-                (++node)->item = pj;
-                avl_insert_before(&tree, nodeaux, node);
-            }
-        } else {
-            // Handle duplicates and points that are dominated by the immediate
-            // previous one.
-            const bool k_eq_j = (pk[0] == pj[0]) & (pk[1] == pj[1]) & (pk[2] == pj[2]);
-            if (!keep_weakly) { // We don't keep duplicates;
-                dominated = true;
-                if (unlikely(k_eq_j) && pj < pk) // Only the first duplicated point is kept.
-                    SWAP(pk, pj);
-            } else { // or it is not a duplicate, so it is non-weakly dominated;
-                dominated = likely(!k_eq_j)
-                    // or pk was dominated, then this one is also dominated.
-                    || last_dom == pk;
+                nodeaux = nodeaux->next;
+                point = nodeaux->item;
+                /* FIXME: A possible speed up is to delete without
+                   rebalancing the tree because avl_insert_before() will
+                   rebalance. */
+                avl_unlink_node(&tree, nodeaux->prev);
             }
-            DEBUG2(printf_point("weakly dominated by pk: ", pk, 3, "\n"));
+            DEBUG2((point == sentinel)
+                   ? printf_point("insert before sentinel: ", sentinel, 2, "\n")
+                   : printf_point("insert before point: ", point, 3, "\n"));
+            (++node)->item = pj;
+            avl_insert_before(&tree, nodeaux, node);
+            // Fall to j_is_NOT_dominated
         }
-        if (dominated) { // pj is dominated by a point in the tree or by prev.
-            /* Map the order in rows[], which is sorted, to the original order
-               in points. */
-            size_t pos_last_dom = row_index_from_ptr(points, pj, max_dim);
-            if (unlikely(nondom == NULL)) {
-                // In this context, it means "position of the first dominated solution found".
-                new_size = pos_last_dom;
-                goto early_end;
-            }
-            nondom[pos_last_dom] = false;
-            last_dom = pj;
-            new_size--;
-        } else {
-            pk = pj;
+        // j_is_NOT_dominated
+        pk = pj;
+        k = j;
+        continue;
+
+    j_is_dominated: // pj is dominated by a point in the tree or by pk.
+        if (find_one_dominated) {
+            // In this context, it means "position of the first dominated solution found".
+            new_size = j;
+            goto early_end;
         }
-        j++;
-    } while (j < size);
+        last_dom_pos = j;
+        rows[j] = NULL;
+        new_size--;
+    }
 
 early_end:
     free(tnodes);
     return new_size;
 }
 
+static __force_inline__ size_t
+find_nondominated_3d_impl(const double ** restrict rows, size_t size,
+                          const bool keep_weakly,
+                          const bool find_one_dominated)
+{
+    // Sort in ascending lexicographic order from the last dimension.
+    qsort_typesafe(rows, size, cmp_ppdouble_asc_rev_3d);
+    // Help GCC generate all possible specializations of this function.
+    return keep_weakly
+        ? find_nondominated_3d_impl_sorted(rows, size,  true, find_one_dominated)
+        : find_nondominated_3d_impl_sorted(rows, size, false, find_one_dominated);
+}
+
 static inline size_t
 find_dominated_3d_(const double * restrict points, size_t size, bool keep_weakly)
 {
     const double ** rows = generate_row_pointers(points, size, 3);
-    size_t pos = keep_weakly
-        ? find_nondominated_set_3d_helper(points, rows, size, 3, true, /* nondom=*/NULL)
-        : find_nondominated_set_3d_helper(points, rows, size, 3, false, /* nondom=*/NULL);
+    size_t pos = find_nondominated_3d_impl(rows, size, keep_weakly, /* find_one_dominated=*/true);
+    if (pos < size)
+        pos = row_index_from_ptr(points, rows[pos], 3);
     free(rows);
     return pos;
 }
@@ -469,11 +491,20 @@ static inline size_t
 find_nondominated_set_3d_(const double * restrict points, size_t size,
                           const bool keep_weakly, boolvec * restrict nondom)
 {
-    const double ** rows = generate_row_pointers(points, size, 3);
     ASSUME(nondom != NULL);
-    size_t new_size = keep_weakly
-        ? find_nondominated_set_3d_helper(points, rows, size, 3, true, nondom)
-        : find_nondominated_set_3d_helper(points, rows, size, 3, false, nondom);
+    const double ** rows = generate_row_pointers(points, size, 3);
+    size_t new_size = find_nondominated_3d_impl(rows, size, keep_weakly, /* find_one_dominated=*/false);
+
+    if (new_size < size) {
+        memset(nondom, false, size * sizeof(*nondom));
+        size_t k = 0, n = 0;
+        do {
+            while (rows[k] == NULL) k++; // Find next nondominated (there must be at least one).
+            nondom[row_index_from_ptr(points, rows[k], 3)] = true;
+            n++;
+            k++;
+        } while (n < new_size);
+    }
     free(rows);
     return new_size;
 }

c/nondominated_kung.h

@@ -618,6 +618,112 @@ kung_merge(const double * points,
     }
 }
 
+static inline size_t
+find_nondominated_set_3d_helper(const double * restrict points,
+                                const double ** rows,
+                                size_t size, dimension_t max_dim,
+                                const bool keep_weakly, boolvec * restrict nondom)
+{
+    ASSUME(size > 1);
+
+    avl_tree_t tree;
+    avl_init_tree(&tree, qsort_cmp_pdouble_asc_x_nonzero);
+    avl_node_t * tnodes = malloc((size+1) * sizeof(*tnodes));
+    avl_node_t * node = tnodes;
+    node->item = rows[0];
+    avl_insert_top(&tree, node);
+
+    const double sentinel[] = { INFINITY, -INFINITY };
+    (++node)->item = sentinel;
+    avl_insert_after(&tree, node - 1, node);
+
+    // In this context, size means "no dominated solution found".
+    size_t new_size = size, j = 1;
+    const double * last_dom = NULL;
+    const double * restrict pk = rows[0];
+    do {
+        bool dominated;
+        const double * restrict pj = rows[j];
+        DEBUG2(printf_point("pj = [ ", pj, 3, " ], "));
+        if (pk[0] > pj[0] || pk[1] > pj[1]) {
+            avl_node_t * nodeaux;
+            int res = avl_search_closest(&tree, pj, &nodeaux);
+            assert(res != 0);
+            if (res > 0) { // nodeaux goes before pj
+                const double * restrict prev = nodeaux->item;
+                assert(prev[0] != sentinel[0]);
+                assert(prev[0] <= pj[0]);
+                dominated = prev[1] <= pj[1];
+                DEBUG2(printf_point("res > 0: prev: ", prev, 3, "\n"));
+                nodeaux = nodeaux->next;
+            } else if (nodeaux->prev) { // nodeaux goes after pj, so move to the next one.
+                const double * restrict prev = nodeaux->prev->item;
+                assert(prev[0] != sentinel[0]);
+                assert(prev[0] <= pj[0]);
+                dominated = prev[1] <= pj[1];
+                DEBUG2(printf_point("res < 0: prev: ", prev, 3, "\n"));
+            } else {
+                dominated = false;
+            }
+
+            if (!dominated) { // pj is NOT dominated by a point in the tree.
+                const double * restrict point = nodeaux->item;
+                assert(pj[0] <= point[0]);
+                // Delete everything in the tree that is dominated by pj.
+                while (pj[1] <= point[1]) {
+                    DEBUG2(printf_point("delete point: ", point, 3, "\n"));
+                    assert(pj[0] <= point[0]);
+                    nodeaux = nodeaux->next;
+                    point = nodeaux->item;
+                    /* FIXME: A possible speed up is to delete without
+                       rebalancing the tree because avl_insert_before() will
+                       rebalance. */
+                    avl_unlink_node(&tree, nodeaux->prev);
+                }
+                DEBUG2((point == sentinel)
+                       ? printf_point("insert before sentinel: ", sentinel, 2, "\n")
+                       : printf_point("insert before point: ", point, 3, "\n"));
+                (++node)->item = pj;
+                avl_insert_before(&tree, nodeaux, node);
+            }
+        } else {
+            // Handle duplicates and points that are dominated by the immediate
+            // previous one.
+            const bool k_eq_j = (pk[0] == pj[0]) & (pk[1] == pj[1]) & (pk[2] == pj[2]);
+            if (!keep_weakly) { // We don't keep duplicates;
+                dominated = true;
+                if (unlikely(k_eq_j) && pj < pk) // Only the first duplicated point is kept.
+                    SWAP(pk, pj);
+            } else { // or it is not a duplicate, so it is non-weakly dominated;
+                dominated = likely(!k_eq_j)
+                    // or pk was dominated, then this one is also dominated.
+                    || last_dom == pk;
+            }
+            DEBUG2(printf_point("weakly dominated by pk: ", pk, 3, "\n"));
+        }
+        if (dominated) { // pj is dominated by a point in the tree or by prev.
+            /* Map the order in rows[], which is sorted, to the original order
+               in points. */
+            size_t pos_last_dom = row_index_from_ptr(points, pj, max_dim);
+            if (unlikely(nondom == NULL)) {
+                // In this context, it means "position of the first dominated solution found".
+                new_size = pos_last_dom;
+                goto early_end;
+            }
+            nondom[pos_last_dom] = false;
+            last_dom = pj;
+            new_size--;
+        } else {
+            pk = pj;
+        }
+        j++;
+    } while (j < size);
+
+early_end:
+    free(tnodes);
+    return new_size;
+}
+
 static size_t
 maxima_top(const double * points, const double ** rows, size_t size,
            dimension_t dim, dimension_t max_dim,
@@ -637,7 +743,8 @@ maxima_rec_dim(const double * points, const double ** rows, size_t size,
     shift_to_next_dimension(r, rows, size);
     const double * const points_shifted = points + 1;
     if (dim == 4) { // We can reach this base case if one dimension has all equal values.
-        // find_nondominated_set_3d_helper() will sort using cmp_ppdouble_asc_rev_3d().
+        // Sort in ascending lexicographic order from the last dimension.
+        qsort_typesafe(r, size, cmp_ppdouble_asc_rev_3d);
         new_size = keep_weakly // Help GCC generate specialized code for true/false.
             ? find_nondominated_set_3d_helper(points_shifted, r, size, max_dim, true, nondom)
             : find_nondominated_set_3d_helper(points_shifted, r, size, max_dim, false, nondom);