def testSphereCollision(self): trimesh = gimpact.TriMesh(self.vertices, self.indices) radius = 1.0 center = [5., 5., 5.] contacts = gimpact.trimesh_sphere_collision(trimesh, center, radius, True) self.assertEqual(len(contacts), 1) contacts = gimpact.trimesh_sphere_collision(trimesh, center, radius) self.assertEqual(len(contacts), 3) np.testing.assert_array_almost_equal(contacts[0].point, [5, 5, 4], decimal=2) np.testing.assert_array_almost_equal(contacts[0].normal, [0, 0, -1], decimal=2) self.assertAlmostEqual(contacts[0].depth, 1, 2) self.assertEqual(contacts[0].feature1, 1) np.testing.assert_array_almost_equal(contacts[1].point, [5, 4, 5], decimal=2) np.testing.assert_array_almost_equal(contacts[1].normal, [0, -1, 0], decimal=2) self.assertAlmostEqual(contacts[1].depth, 1, 2) self.assertEqual(contacts[1].feature1, 3) np.testing.assert_array_almost_equal(contacts[2].point, [4, 5, 5], decimal=2) np.testing.assert_array_almost_equal(contacts[2].normal, [-1, 0, 0], decimal=2) self.assertAlmostEqual(contacts[2].depth, 1, 2) self.assertEqual(contacts[2].feature1, 8)
def testCapsuleCollision(self): trimesh = gimpact.TriMesh(self.vertices, self.indices) radius = 1 start = [-1., 0., 5.] stop = [1., 0., 5.] contacts = gimpact.trimesh_capsule_collision(trimesh, start, stop, radius, True) self.assertEqual(len(contacts), 1) contacts = gimpact.trimesh_capsule_collision(trimesh, start, stop, radius) self.assertEqual(len(contacts), 3) np.testing.assert_array_almost_equal(contacts[0].point, [-1, 0, 4], decimal=2) np.testing.assert_array_almost_equal(contacts[0].normal, [0, 0, -1], decimal=2) self.assertAlmostEqual(contacts[0].depth, 1, 2) self.assertEqual(contacts[0].feature1, 0) np.testing.assert_array_almost_equal(contacts[1].point, [0, 0, 4], decimal=2) np.testing.assert_array_almost_equal(contacts[1].normal, [0, 0, -1], decimal=2) self.assertAlmostEqual(contacts[1].depth, 1, 2) self.assertEqual(contacts[1].feature1, 1) np.testing.assert_array_almost_equal(contacts[2].point, [1, 0, 4], decimal=2) np.testing.assert_array_almost_equal(contacts[2].normal, [0, 0, -1], decimal=2) self.assertAlmostEqual(contacts[2].depth, 1, 2) self.assertEqual(contacts[2].feature1, 1)
def testRayCollision(self): trimesh = gimpact.TriMesh(self.vertices, self.indices) start = [0., 0., 0.] direction = [-1., 0., 0.] depth = 100 contact = gimpact.trimesh_ray_collision(trimesh, start, direction, depth) # No contact because ray originates from inside the object self.assertIsNone(contact) contact = gimpact.trimesh_ray_collision(trimesh, [0., 5.1, 0.], direction, depth) self.assertIsNone(contact) contact = gimpact.trimesh_ray_collision(trimesh, [2.5, -6.0, 0.], [0., 1., 0.], depth) np.testing.assert_array_almost_equal(contact.point, [2.5, -5, 0], decimal=2) np.testing.assert_array_almost_equal(contact.normal, [0, 1, 0], decimal=2) self.assertAlmostEqual(contact.depth, 1, 2) self.assertEqual(contact.feature1, 7) contact = gimpact.trimesh_ray_closest_collision( trimesh, np.array([5., 0., 5.]), np.array([0., 0., -1.]), 100) np.testing.assert_array_almost_equal(contact.point, [5, 0, 5], decimal=2) np.testing.assert_array_almost_equal(contact.normal, [0, 0, -1], decimal=2) self.assertAlmostEqual(contact.depth, 0, 2) self.assertEqual(contact.feature1, 1)
def gen_cdmesh_vvnf(vertices, vertex_normals, faces): """ generate cdmesh given vertices, _, and faces :return: gimpact.TriMesh (require gimpact to be installed) author: weiwei date: 20210118 """ return gi.TriMesh(vertices, faces.flatten())
def testAABBSet(self): trimesh = gimpact.TriMesh(self.vertices, self.indices) aabb_set = trimesh.aabb_set np.testing.assert_array_almost_equal(aabb_set.global_bounds.bounds, trimesh.bounds.bounds, decimal=2) t = trimesh.clone() aabb_set = t.aabb_set self.assertEqual(trimesh.triangle_count, t.triangle_count) del t self.assertEqual(len(trimesh.aabb_set), len(aabb_set))
def addInstance(instances, mesh, pose, file, score): g_mesh = gimpact.TriMesh(mesh.trimesh.vertices @ pose[0].T + pose[1], mesh.trimesh.faces.flatten()) toRemove = set() for i in reversed(range(len(instances))): if gimpact.trimesh_trimesh_collision(g_mesh, instances[i][0], True): if instances[i][1] > score: return else: toRemove.add(i) for i in sorted(list(toRemove), reverse=True): instances.pop(i) instances.append((g_mesh, score, mesh, pose, file))
def testTriangle(self): trimesh = gimpact.TriMesh(self.vertices, self.indices) self.assertEqual(trimesh.triangle_count, self.indices.size // 3) for i in range(trimesh.triangle_count): np.testing.assert_array_almost_equal( trimesh.triangle(i), self.vertices[self.indices[i * 3:i * 3 + 3]], decimal=2) trimesh2 = trimesh.clone() for i in range(trimesh.triangle_count): a = np.array(trimesh.triangle(i)) b = np.array(trimesh2.triangle(i)) np.testing.assert_array_almost_equal(a, b, decimal=2)
def __init__(self, identifier, vertices, indices, mask_size=32, transform=None): self.id = identifier self.geometry = gimpact.TriMesh(vertices, indices).decimate(50000) self.excludes = bitarray(mask_size) self.excludes.setall(False) self.excludes[identifier] = True if transform is not None: self.geometry.transform(np.array(transform, np.float32))
def testPlaneCollision(self): trimesh = gimpact.TriMesh(self.vertices, self.indices) contacts = gimpact.trimesh_plane_collision(trimesh, np.array([0., 0., 1., 0.]), True) self.assertEqual(len(contacts), 1) contacts = gimpact.trimesh_plane_collision(trimesh, np.array([0., 0., 1., 0.])) self.assertEqual(len(contacts), self.vertices.shape[0] / 2) for contact, depth in contacts: self.assertAlmostEqual(contact[2], -5, 2) self.assertAlmostEqual(depth, 5, 2) contacts = gimpact.trimesh_plane_collision( trimesh, np.array([0., 0., 1., -5.01])) self.assertEqual(len(contacts), 0) contacts = gimpact.trimesh_plane_collision( trimesh, np.array([0., 0., -1., 5.01])) self.assertEqual(len(contacts), self.vertices.shape[0])
def testTransform(self): trimesh = gimpact.TriMesh(self.vertices, self.indices) t = np.identity(4) t[0:3, 3] = [10, 10, 10] trimesh.transform(t) v = self.vertices[self.indices] v = v @ t[0:3, 0:3].transpose() + t[0:3, 3] for i in range(trimesh.triangle_count): np.testing.assert_array_almost_equal(trimesh.triangle(i), v[i * 3:i * 3 + 3], decimal=2) trimesh2 = trimesh.clone() t = np.identity(4) t[0:2, 0:2] = [[0, -1], [1, 0]] trimesh2.transform(t) v = self.vertices[self.indices] v = v @ t[0:3, 0:3].transpose() for i in range(trimesh2.triangle_count): np.testing.assert_array_almost_equal(trimesh2.triangle(i), v[i * 3:i * 3 + 3], decimal=2)
def testTrimeshCollision(self): trimesh1 = gimpact.TriMesh(self.vertices, self.indices) trimesh2 = trimesh1.clone() contacts = gimpact.trimesh_trimesh_collision(trimesh1, trimesh2, True) self.assertEqual(len(contacts), 1) t = np.identity(4) t[2, 3] = 11 trimesh2.transform(t) contacts = gimpact.trimesh_trimesh_collision(trimesh1, trimesh2) self.assertEqual(len(contacts), 0) t[:3, 3] = [10, 10, 10] trimesh2.transform(t) contacts = gimpact.trimesh_trimesh_collision(trimesh1, trimesh2) np.testing.assert_array_almost_equal(contacts[0].point, [5, 5, 5], decimal=2) np.testing.assert_array_almost_equal(contacts[0].normal, [0, 0, -1], decimal=2) self.assertAlmostEqual(contacts[0].depth, 0, 2) self.assertEqual(contacts[0].feature1, 1) self.assertEqual(contacts[0].feature2, 4)
def testDecimation(self): trimesh = gimpact.TriMesh(self.vertices, self.indices) trimesh2 = trimesh.decimate(8) self.assertEqual(trimesh2.triangle_count, 8)