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)