Fix halfspaceDistance + add tiny unit-test.

src/narrowphase/details.h

@@ -1955,11 +1955,10 @@ namespace fcl {
       Vec3f n_2 (tf2.getRotation().transpose() * n_w);
       int hint = 0;
       p2 = getSupport(&s, -n_2, true, hint);
-      dist = p2.dot(n_2) - h.d;
-      p1 = p2 - dist * n_2;
-
-      p1 = tf2.transform(p1);
       p2 = tf2.transform(p2);
+
+      dist = (p2 - tf1.getTranslation()).dot(n_w) - h.d;
+      p1 = p2 - dist * n_w;
       normal = n_w;
 
       return dist <= 0;

test/python_unit/CMakeLists.txt

 SET(${PROJECT_NAME}_PYTHON_TESTS
   geometric_shapes
   api
+  collision
   )
 
 FOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS})

test/python_unit/collision.py

+import unittest
+from test_case import TestCase
+import hppfcl
+hppfcl.switchToNumpyArray()
+import numpy as np
+
+def tetahedron():
+    pts = hppfcl.StdVec_Vec3f()
+    pts.append( np.array((0, 0, 0)) )
+    pts.append( np.array((0, 1, 0)) )
+    pts.append( np.array((1, 0, 0)) )
+    pts.append( np.array((0, 0, 1)) )
+    tri = hppfcl.StdVec_Triangle()
+    tri.append(hppfcl.Triangle(0,1,2))
+    tri.append(hppfcl.Triangle(0,1,3))
+    tri.append(hppfcl.Triangle(0,2,3))
+    tri.append(hppfcl.Triangle(1,2,3))
+    return hppfcl.Convex(pts, tri)
+
+class TestMainAPI(TestCase):
+
+    def test_convex_halfspace(self):
+        convex = tetahedron()
+        halfspace = hppfcl.Halfspace(np.array((0,0,1)), 0)
+
+        req=hppfcl.CollisionRequest()
+        res=hppfcl.CollisionResult()
+
+        M1 = hppfcl.Transform3f()
+        M2 = hppfcl.Transform3f(np.eye(3),np.array([0, 0, -0.1]))
+        res.clear()
+        hppfcl.collide(convex, M1, halfspace, M2, req, res)
+        self.assertFalse(hppfcl.collide(convex, M1, halfspace, M2, req, res))
+
+        M1 = hppfcl.Transform3f()
+        M2 = hppfcl.Transform3f(np.eye(3),np.array([0, 0, 0.1]))
+        res.clear()
+        self.assertTrue(hppfcl.collide(convex, M1, halfspace, M2, req, res))
+
+        M1 = hppfcl.Transform3f()
+        M2 = hppfcl.Transform3f(np.eye(3),np.array([0, 0, 2]))
+        res.clear()
+        self.assertTrue(hppfcl.collide(convex, M1, halfspace, M2, req, res))
+
+if __name__ == '__main__':
+    unittest.main()