Commit 5d20d8c9 authored by Joseph Mirabel's avatar Joseph Mirabel
Browse files

Fix halfspaceDistance + add tiny unit-test.

parent 7ad91cce
......@@ -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;
......
SET(${PROJECT_NAME}_PYTHON_TESTS
geometric_shapes
api
collision
)
FOREACH(TEST ${${PROJECT_NAME}_PYTHON_TESTS})
......
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()
Markdown is supported
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment