python_unit/collision.py

Go to the documentation of this file.

import unittest
from test_case import TestCase
import hppfcl

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()