Fix bug in propagating is_collision properly

Author: mmatl

fcl/fcl.pyx

@@ -545,7 +545,7 @@ def collide(CollisionObject o1, CollisionObject o2,
                                    ),
                                    cresult)
 
-    result.is_collision = cresult.isCollision()
+    result.is_collision = result.is_collision or cresult.isCollision()
 
     cdef vector[defs.Contact] contacts
     cresult.getContacts(contacts)
@@ -582,8 +582,8 @@ def continuousCollide(CollisionObject o1, Transform tf1_end,
                                                     ),
                                                     cresult)
 
-    result.is_collide = cresult.is_collide
-    result.time_of_contact = cresult.time_of_contact
+    result.is_collide = result.is_collide or cresult.is_collide
+    result.time_of_contact = min(cresult.time_of_contact, result.time_of_contact)
     return ret
 
 def distance(CollisionObject o1, CollisionObject o2,
@@ -603,7 +603,7 @@ def distance(CollisionObject o1, CollisionObject o2,
                                     ),
                                     cresult)
 
-    result.min_distance = cresult.min_distance
+    result.min_distance = min(cresult.min_distance, result.min_distance)
     result.nearest_points = [vec3f_to_numpy(cresult.nearest_points[0]),
                              vec3f_to_numpy(cresult.nearest_points[1])]
     result.o1 = c_to_python_collision_geometry(cresult.o1, o1, o2)

fcl/version.py

@@ -1 +1 @@
-__version__ = '0.0.5'
+__version__ = '0.0.6'

test/test_fcl.py

@@ -48,37 +48,44 @@ def test_pairwise_collisions(self):
         cone = fcl.CollisionObject(self.geometry['cone'])
         mesh = fcl.CollisionObject(self.geometry['mesh'])
 
+        result = fcl.CollisionResult()
         ret = fcl.collide(box, cone, self.crequest, result)
         self.assertTrue(ret > 0)
         self.assertTrue(result.is_collision)
 
+        result = fcl.CollisionResult()
         ret = fcl.collide(box, mesh, self.crequest, result)
         self.assertTrue(ret > 0)
         self.assertTrue(result.is_collision)
 
+        result = fcl.CollisionResult()
         ret = fcl.collide(cone, mesh, self.crequest, result)
         self.assertTrue(ret > 0)
         self.assertTrue(result.is_collision)
 
         cone.setTranslation(np.array([0.0, 0.0, -0.6]))
 
+        result = fcl.CollisionResult()
         ret = fcl.collide(box, cone, self.crequest, result)
         self.assertTrue(ret > 0)
         self.assertTrue(result.is_collision)
 
+        result = fcl.CollisionResult()
         ret = fcl.collide(cone, mesh, self.crequest, result)
         self.assertTrue(ret == 0)
         self.assertFalse(result.is_collision)
 
         cone.setTranslation(np.array([0.0, -0.9, 0.0]))
         cone.setRotation(self.x_axis_rot)
 
+        result = fcl.CollisionResult()
         ret = fcl.collide(box, cone, self.crequest, result)
         self.assertTrue(ret > 0)
         self.assertTrue(result.is_collision)
 
         cone.setTranslation(np.array([0.0, -1.1, 0.0]))
 
+        result = fcl.CollisionResult()
         ret = fcl.collide(box, cone, self.crequest, result)
         self.assertTrue(ret == 0)
         self.assertFalse(result.is_collision)
@@ -90,31 +97,38 @@ def test_pairwise_distances(self):
         cone = fcl.CollisionObject(self.geometry['cone'])
         mesh = fcl.CollisionObject(self.geometry['mesh'])
 
+        result = fcl.DistanceResult()
         ret = fcl.distance(box, cone, self.drequest, result)
         self.assertTrue(ret < 0)
 
+        result = fcl.DistanceResult()
         ret = fcl.distance(box, mesh, self.drequest, result)
         self.assertTrue(ret < 0)
 
+        result = fcl.DistanceResult()
         ret = fcl.distance(cone, mesh, self.drequest, result)
         self.assertTrue(ret < 0)
 
         cone.setTranslation(np.array([0.0, 0.0, -0.6]))
 
+        result = fcl.DistanceResult()
         ret = fcl.distance(box, cone, self.drequest, result)
         self.assertTrue(ret < 0)
 
+        result = fcl.DistanceResult()
         ret = fcl.distance(cone, mesh, self.drequest, result)
         self.assertAlmostEqual(ret, 0.1)
 
         cone.setTranslation(np.array([0.0, -0.9, 0.0]))
         cone.setRotation(self.x_axis_rot)
 
+        result = fcl.DistanceResult()
         ret = fcl.distance(box, cone, self.drequest, result)
         self.assertTrue(ret < 0)
 
         cone.setTranslation(np.array([0.0, -1.1, 0.0]))
 
+        result = fcl.DistanceResult()
         ret = fcl.distance(box, cone, self.drequest, result)
         self.assertAlmostEqual(ret, 0.1)
 
@@ -129,7 +143,7 @@ def test_pairwise_continuous_collisions(self):
         ret = fcl.continuousCollide(box, fcl.Transform(),
                                     cone, fcl.Transform(),
                                     request, result)
-        
+
         '''
         ## WHY DOES THIS FAIL ##
         self.assertTrue(result.is_collide)
@@ -225,6 +239,47 @@ def test_updates(self):
         manager.collide(cdata, fcl.defaultCollisionCallback)
         self.assertTrue(cdata.result.is_collision)
 
+    def test_many_objects(self):
+        manager = fcl.DynamicAABBTreeCollisionManager()
+
+        objs = [fcl.CollisionObject(self.geometry['sphere']),
+                fcl.CollisionObject(self.geometry['sphere'], fcl.Transform(np.array([0.0, 0.0, -5.0]))),
+                fcl.CollisionObject(self.geometry['sphere'], fcl.Transform(np.array([0.0, 0.0, 5.0])))]
+
+        manager.registerObjects(objs)
+        manager.setup()
+
+        self.assertTrue(len(manager.getObjects()) == 3)
+
+        # Many-to-many, internal
+        cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
+        manager.collide(cdata, fcl.defaultCollisionCallback)
+        self.assertFalse(cdata.result.is_collision)
+
+        objs[1].setTranslation(np.array([0.0, 0.0, -0.3]))
+        manager.update(objs[1])
+        cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
+        manager.collide(cdata, fcl.defaultCollisionCallback)
+        self.assertTrue(cdata.result.is_collision)
+
+        objs[1].setTranslation(np.array([0.0, 0.0, -5.0]))
+        manager.update(objs[1])
+        cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
+        manager.collide(cdata, fcl.defaultCollisionCallback)
+        self.assertFalse(cdata.result.is_collision)
+
+        objs[2].setTranslation(np.array([0.0, 0.0, 0.3]))
+        manager.update(objs[2])
+        cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
+        manager.collide(cdata, fcl.defaultCollisionCallback)
+        self.assertTrue(cdata.result.is_collision)
+
+        objs[2].setTranslation(np.array([0.0, 0.0, 5.0]))
+        manager.update(objs[2])
+        cdata = fcl.CollisionData(self.crequest, fcl.CollisionResult())
+        manager.collide(cdata, fcl.defaultCollisionCallback)
+        self.assertFalse(cdata.result.is_collision)
+
     def test_managed_distances(self):
         manager1 = fcl.DynamicAABBTreeCollisionManager()
         manager2 = fcl.DynamicAABBTreeCollisionManager()