Improve performance of straight line edge case handling

Quickhull.h

@@ -113,42 +113,38 @@ private:
         // If the input vector is empty, we're done
         if (input.empty()) return;
 
-        // Find the point which is furthest away from the line, add it to the output
+        // Find the points which are furthest away from the line
-        Point furthest_point;
         float furthest_distance = -1.0;
 
+        std::list<Point> furthest_points;
+
         for (const Point &point : input)
         {
             float this_distance = line.distance_squared_to(point);
+            // It's possible for there to be multiple closests points (e.g. in the case)
+            //  of a rectangle). We need to handle all these properly, so we make a list
+            //  of all points at the furthest distance.
             if (this_distance > furthest_distance)
             {
                 furthest_distance = this_distance;
-                furthest_point = point;
+                furthest_points.clear();
+                furthest_points.emplace_back(point);
             }
-        }
+            else if (this_distance == furthest_distance)
-
-        // TODO: e.g. in the case of a rectangle, it's possible for there to be
-        //  multiple closest points (sometimes all at distance 0). How do we handle
-        //  these properly? We definitely need to remove them all from input later;
-        //  do we also need to handle them all further? This hotfix works, but seems
-        //  like an unnecessarily big performance hit for that edge case.
-        for (const Point &point : input)
-        {
-            float this_distance = line.distance_squared_to(point);
-            if (this_distance == furthest_distance)
             {
-                output.emplace_back(point);
+                furthest_points.emplace_back(point);
             }
         }
-        // Remove the previously checked points from the input -- we can't do that in
-        //  the previous loop because we can't delete from the list we're iterating over
-        input.remove_if([furthest_distance, line](Point point)
-        {
-            return furthest_distance == line.distance_squared_to(point);
-        });
 
-        output.emplace_back(furthest_point);
+        for (const Point &point : furthest_points)
-        input.remove(furthest_point);
+        {
+            // All furthest points we found are part of the hull, so add them to the output
+            //  and remove them from the input.
+            output.emplace_back(point);
+            input.remove(point);
+        }
+
+        Point furthest_point = furthest_points.front();
 
         // Build a triangle with these 3 points
 
@@ -159,7 +155,6 @@ private:
 
         if (line.is_point_right(furthest_point))
         {
-            // TOOD: It's probably more efficient to set the fields?
             new_line1 = Line(line.to(), furthest_point);
             new_line2 = Line(furthest_point, line.from());
         }
@@ -182,6 +177,8 @@ private:
             {
                 left_of_line1.emplace_back(point);
             }
+            // TODO: Are there any possible edge cases where this 'else if' won't work,
+            //  and we need an 'if' instead?
             else if (!new_line2.is_point_right(point))
             {
                 left_of_line2.emplace_back(point);
@@ -189,7 +186,6 @@ private:
         }
 
         // Recursively call get_hull_with_line for each side of the triangle
-        // TODO: We can skip the original one
         get_hull_with_line(left_of_line1, output, new_line1);
         get_hull_with_line(left_of_line2, output, new_line2);
     }