fix: min distance nearest points fix, add to tests

Author: mjd3

README.md

@@ -362,4 +362,4 @@ for coll_pair in objs_in_collision:
 ```
 >>> Object obj1 in collision with object obj2!
 ```
-For more examples, see `example/example.py`.
+For more examples, see `examples/example.py`.

src/fcl/fcl.pyx

@@ -684,13 +684,14 @@ def distance(CollisionObject o1, CollisionObject o2,
                                     ),
                                     cresult)
 
-    result.min_distance = min(cresult.min_distance, result.min_distance)
-    result.nearest_points = [vec3d_to_numpy(cresult.nearest_points[0]),
-                             vec3d_to_numpy(cresult.nearest_points[1])]
-    result.o1 = c_to_python_collision_geometry(cresult.o1, o1, o2)
-    result.o2 = c_to_python_collision_geometry(cresult.o2, o1, o2)
-    result.b1 = cresult.b1
-    result.b2 = cresult.b2
+    if result.min_distance > cresult.min_distance:
+        result.min_distance = cresult.min_distance
+        result.nearest_points = [vec3d_to_numpy(cresult.nearest_points[0]),
+                                vec3d_to_numpy(cresult.nearest_points[1])]
+        result.o1 = c_to_python_collision_geometry(cresult.o1, o1, o2)
+        result.o2 = c_to_python_collision_geometry(cresult.o2, o1, o2)
+        result.b1 = cresult.b1
+        result.b2 = cresult.b2
     return dis
 
 ###############################################################################