Merge pull request diku-dk#108 from Jax922/feature/speedup-contact-po…

…int-reduction

Speedup contact point reduction phase

python/rainbow/simulators/prox_soft_bodies/collision_detection.py

@@ -284,9 +284,19 @@ def _contact_determination(overlaps, engine, stats, debug_on):
     return stats
 
 
-def _contact_reduction(engine, stats, debug_on):
+def _cp_unique_id(cp) -> tuple:
+    """ This function returns a unique id for a ContactPoint instance.
+    Args:
+        cp (ContactPoint): A instance of the ContactPoint
+
+    Returns:
+        tuple: A unique id for the contact point
     """
-    During contact point computation it may happen that different colliding triangles of one body results
+    return (cp.bodyA, cp.bodyB, tuple(cp.p))
+
+
+def _contact_reduction(engine, stats, debug_on):
+    """ During contact point computation it may happen that different colliding triangles of one body results
     in the same contact point locations wrt to the other signed distance field of the other body. Imagine a spiky
     polygonal cone pointing into a spherical shape. Here all triangles of the spike will result in the same
     contact point at the spiky tip. If the spiky polygonal cone has N triangle faces on the spike then we
@@ -295,30 +305,34 @@ def _contact_reduction(engine, stats, debug_on):
     contact point information. One may think of this as a kind of post-filtering process to clean up the
      contact point information.
 
-     :param engine:      The current engine instance we are working with.
-     :param stats:       A dictionary where to add more profiling and timing measurements.
-     :param debug_on:    Boolean flag for toggling debug (aka profiling) info on and off.
-     :return:            A dictionary with profiling and timing measurements.
+    Args:
+        engine (Engine): The current engine instance we are working with.
+        stats (dict):  A dictionary where to add more profiling and timing measurements.
+        debug_on (bool): Boolean flag for toggling debug (aka profiling) info on and off.
+
+    Returns:
+        dict: A dictionary with profiling and timing measurements.
     """
     reduction_timer = None
     if debug_on:
         reduction_timer = Timer("contact_point_reduction", 8)
         reduction_timer.start()
-    # TODO 2020-09-07 Kristian: This brute force implementation can be implemented better
+
+    seen_ids = set()
     reduced_list = []
-    for cp1 in engine.contact_points:
-        unique = True
-        for cp2 in reduced_list:
-            if {cp1.bodyA, cp1.bodyB} == {cp2.bodyA, cp2.bodyB} and (
-                cp1.p == cp2.p
-            ).all():
-                unique = False
-        if unique:
-            reduced_list.append(cp1)
+
+    for cp in engine.contact_points:
+        uid = _cp_unique_id(cp)
+        if uid not in seen_ids:
+            reduced_list.append(cp)
+            seen_ids.add(uid)
+
     engine.contact_points = reduced_list
+
     if debug_on:
         reduction_timer.end()
         stats["contact_point_reduction"] = reduction_timer.elapsed
+
     return stats