warp-lang 1.6.2__py3-none-win_amd64.whl → 1.7.1__py3-none-win_amd64.whl

warp/tests/{test_collision.py → sim/test_collision.py}

@@ -18,7 +18,12 @@ import unittest
 import warp as wp
 import warp.examples
 import warp.sim
-from warp.sim.collide import *
+from warp.sim.collide import (
+    TriMeshCollisionDetector,
+    init_triangle_collision_data_kernel,
+    triangle_closest_point,
+    vertex_adjacent_to_triangle,
+)
 from warp.sim.integrator_euler import eval_triangles_contact
 from warp.tests.unittest_utils import *
 
@@ -260,229 +265,205 @@ def validate_edge_collisions(
         wp.expect_eq(dist >= min_dist, True)
 
 
-class Example:
-    def __init__(self, device, vs, fs):
-        self.device = device
+def init_model(vs, fs, device):
+    vertices = [wp.vec3(v) for v in vs]
 
-        self.input_scale_factor = 1.0
-        self.renderer_scale_factor = 0.01
-        vertices = [wp.vec3(v) * self.input_scale_factor for v in vs]
+    builder = wp.sim.ModelBuilder()
+    builder.add_cloth_mesh(
+        pos=wp.vec3(0.0, 200.0, 0.0),
+        rot=wp.quat_from_axis_angle(wp.vec3(1.0, 0.0, 0.0), 0.0),
+        scale=1.0,
+        vertices=vertices,
+        indices=fs,
+        vel=wp.vec3(0.0, 0.0, 0.0),
+        density=0.02,
+        tri_ke=0,
+        tri_ka=0,
+        tri_kd=0,
+    )
+    model = builder.finalize(device=device)
 
-        builder = wp.sim.ModelBuilder()
-        builder.add_cloth_mesh(
-            pos=wp.vec3(0.0, 200.0, 0.0),
-            rot=wp.quat_from_axis_angle(wp.vec3(1.0, 0.0, 0.0), 0.0),
-            scale=1.0,
-            vertices=vertices,
-            indices=fs,
-            vel=wp.vec3(0.0, 0.0, 0.0),
-            density=0.02,
-            tri_ke=0,
-            tri_ka=0,
-            tri_kd=0,
-        )
-        self.model = builder.finalize(device=self.device)
-
-        self.collision_detector = TriMeshCollisionDetector(model=self.model)
-
-    def run_vertex_triangle_test(self):
-        rs = [1e-2, 2e-2, 5e-2, 1e-1]
-        for query_radius in rs:
-            self.collision_detector.vertex_triangle_collision_detection(query_radius)
-            vertex_colliding_triangles_count_1 = self.collision_detector.vertex_colliding_triangles_count.numpy()
-            vertex_min_dis_1 = self.collision_detector.vertex_colliding_triangles_min_dist.numpy()
-
-            triangle_colliding_vertices_count_1 = self.collision_detector.triangle_colliding_vertices_count.numpy()
-            triangle_min_dis_1 = self.collision_detector.triangle_colliding_vertices_min_dist.numpy()
-
-            wp.launch(
-                kernel=init_triangle_collision_data_kernel,
-                inputs=[
-                    query_radius,
-                    self.collision_detector.triangle_colliding_vertices_count,
-                    self.collision_detector.triangle_colliding_vertices_min_dist,
-                    self.collision_detector.resize_flags,
-                ],
-                dim=self.model.tri_count,
-                device=self.model.device,
-            )
-
-            wp.launch(
-                kernel=vertex_triangle_collision_detection_brute_force,
-                inputs=[
-                    query_radius,
-                    self.collision_detector.bvh_tris.id,
-                    self.collision_detector.model.particle_q,
-                    self.collision_detector.model.tri_indices,
-                    self.collision_detector.vertex_colliding_triangles,
-                    self.collision_detector.vertex_colliding_triangles_count,
-                    self.collision_detector.vertex_colliding_triangles_offsets,
-                    self.collision_detector.vertex_colliding_triangles_buffer_sizes,
-                    self.collision_detector.vertex_colliding_triangles_min_dist,
-                    self.collision_detector.triangle_colliding_vertices,
-                    self.collision_detector.triangle_colliding_vertices_count,
-                    self.collision_detector.triangle_colliding_vertices_offsets,
-                    self.collision_detector.triangle_colliding_vertices_buffer_sizes,
-                    self.collision_detector.triangle_colliding_vertices_min_dist,
-                    self.collision_detector.resize_flags,
-                ],
-                dim=self.model.particle_count,
-                device=self.model.device,
-            )
-
-            vertex_colliding_triangles_count_2 = self.collision_detector.vertex_colliding_triangles_count.numpy()
-            vertex_min_dis_2 = self.collision_detector.vertex_colliding_triangles_min_dist.numpy()
-
-            triangle_colliding_vertices_count_2 = self.collision_detector.triangle_colliding_vertices_count.numpy()
-            triangle_min_dis_2 = self.collision_detector.triangle_colliding_vertices_min_dist.numpy()
-
-            assert (vertex_colliding_triangles_count_2 == vertex_colliding_triangles_count_1).all()
-            assert (triangle_min_dis_2 == triangle_min_dis_1).all()
-            assert (triangle_colliding_vertices_count_2 == triangle_colliding_vertices_count_1).all()
-            assert (vertex_min_dis_2 == vertex_min_dis_1).all()
-
-            wp.launch(
-                kernel=validate_vertex_collisions,
-                inputs=[
-                    query_radius,
-                    self.collision_detector.bvh_tris.id,
-                    self.collision_detector.model.particle_q,
-                    self.collision_detector.model.tri_indices,
-                    self.collision_detector.vertex_colliding_triangles,
-                    self.collision_detector.vertex_colliding_triangles_count,
-                    self.collision_detector.vertex_colliding_triangles_offsets,
-                    self.collision_detector.vertex_colliding_triangles_buffer_sizes,
-                    self.collision_detector.vertex_colliding_triangles_min_dist,
-                    self.collision_detector.resize_flags,
-                ],
-                dim=self.model.particle_count,
-                device=self.model.device,
-            )
-
-            wp.launch(
-                kernel=validate_triangle_collisions,
-                inputs=[
-                    query_radius,
-                    self.collision_detector.bvh_tris.id,
-                    self.collision_detector.model.particle_q,
-                    self.collision_detector.model.tri_indices,
-                    self.collision_detector.triangle_colliding_vertices,
-                    self.collision_detector.triangle_colliding_vertices_count,
-                    self.collision_detector.triangle_colliding_vertices_offsets,
-                    self.collision_detector.triangle_colliding_vertices_buffer_sizes,
-                    self.collision_detector.triangle_colliding_vertices_min_dist,
-                    self.collision_detector.resize_flags,
-                ],
-                dim=self.model.tri_count,
-                device=self.model.device,
-            )
-
-    def run_edge_edge_test(self):
-        rs = [1e-2, 2e-2, 5e-2, 1e-1]
-        edge_edge_parallel_epsilon = 1e-5
-        for query_radius in rs:
-            self.collision_detector.edge_edge_collision_detection(query_radius)
-            edge_colliding_edges_count_1 = self.collision_detector.edge_colliding_edges_count.numpy()
-            edge_min_dist_1 = self.collision_detector.edge_colliding_edges_min_dist.numpy()
-
-            print(edge_colliding_edges_count_1)
-
-            wp.launch(
-                kernel=edge_edge_collision_detection_brute_force,
-                inputs=[
-                    query_radius,
-                    self.collision_detector.bvh_edges.id,
-                    self.collision_detector.model.particle_q,
-                    self.collision_detector.model.edge_indices,
-                    self.collision_detector.edge_colliding_edges_offsets,
-                    self.collision_detector.edge_colliding_edges_buffer_sizes,
-                    edge_edge_parallel_epsilon,
-                ],
-                outputs=[
-                    self.collision_detector.edge_colliding_edges,
-                    self.collision_detector.edge_colliding_edges_count,
-                    self.collision_detector.edge_colliding_edges_min_dist,
-                    self.collision_detector.resize_flags,
-                ],
-                dim=self.model.edge_count,
-                device=self.model.device,
-            )
-
-            edge_colliding_edges_count_2 = self.collision_detector.edge_colliding_edges_count.numpy()
-            edge_min_dist_2 = self.collision_detector.edge_colliding_edges_min_dist.numpy()
-
-            assert (edge_colliding_edges_count_2 == edge_colliding_edges_count_1).all()
-            assert (edge_min_dist_1 == edge_min_dist_2).all()
-
-            wp.launch(
-                kernel=validate_edge_collisions,
-                inputs=[
-                    query_radius,
-                    self.collision_detector.bvh_edges.id,
-                    self.collision_detector.model.particle_q,
-                    self.collision_detector.model.edge_indices,
-                    self.collision_detector.edge_colliding_edges_offsets,
-                    self.collision_detector.edge_colliding_edges_buffer_sizes,
-                    edge_edge_parallel_epsilon,
-                ],
-                outputs=[
-                    self.collision_detector.edge_colliding_edges,
-                    self.collision_detector.edge_colliding_edges_count,
-                    self.collision_detector.edge_colliding_edges_min_dist,
-                    self.collision_detector.resize_flags,
-                ],
-                dim=self.model.particle_count,
-                device=self.model.device,
-            )
-
-    def set_points_fixed(self, model, fixed_particles):
-        if len(fixed_particles):
-            flags = model.particle_flags.numpy()
-            for fixed_vertex_id in fixed_particles:
-                flags[fixed_vertex_id] = wp.uint32(int(flags[fixed_vertex_id]) & ~int(PARTICLE_FLAG_ACTIVE))
-
-            model.particle_flags = wp.array(flags, device=model.device)
+    collision_detector = TriMeshCollisionDetector(model=model)
 
+    return model, collision_detector
 
-@unittest.skipUnless(USD_AVAILABLE, "Requires usd-core")
-def test_vertex_triangle_collision(test, device):
+
+def get_data():
     from pxr import Usd, UsdGeom
 
-    def get_data():
-        usd_stage = Usd.Stage.Open(os.path.join(warp.examples.get_asset_directory(), "bunny.usd"))
-        usd_geom = UsdGeom.Mesh(usd_stage.GetPrimAtPath("/root/bunny"))
+    usd_stage = Usd.Stage.Open(os.path.join(warp.examples.get_asset_directory(), "bunny.usd"))
+    usd_geom = UsdGeom.Mesh(usd_stage.GetPrimAtPath("/root/bunny"))
+
+    vertices = np.array(usd_geom.GetPointsAttr().Get())
+    faces = np.array(usd_geom.GetFaceVertexIndicesAttr().Get())
 
-        vertices = np.array(usd_geom.GetPointsAttr().Get())
-        faces = np.array(usd_geom.GetFaceVertexIndicesAttr().Get())
+    return vertices, faces
 
-        return vertices, faces
 
+@unittest.skipUnless(USD_AVAILABLE, "Requires usd-core")
+def test_vertex_triangle_collision(test, device):
     vertices, faces = get_data()
 
-    sim = Example(device, vertices, faces)
-    sim.run_vertex_triangle_test()
+    model, collision_detector = init_model(vertices, faces, device)
 
+    rs = [1e-2, 2e-2, 5e-2, 1e-1]
 
-@unittest.skipUnless(USD_AVAILABLE, "Requires usd-core")
-def test_edge_edge_collision(test, device):
-    from pxr import Usd, UsdGeom
+    for query_radius in rs:
+        collision_detector.vertex_triangle_collision_detection(query_radius)
+        vertex_colliding_triangles_count_1 = collision_detector.vertex_colliding_triangles_count.numpy()
+        vertex_min_dis_1 = collision_detector.vertex_colliding_triangles_min_dist.numpy()
 
-    def get_data():
-        usd_stage = Usd.Stage.Open(os.path.join(warp.examples.get_asset_directory(), "bunny.usd"))
-        usd_geom = UsdGeom.Mesh(usd_stage.GetPrimAtPath("/root/bunny"))
+        triangle_colliding_vertices_count_1 = collision_detector.triangle_colliding_vertices_count.numpy()
+        triangle_min_dis_1 = collision_detector.triangle_colliding_vertices_min_dist.numpy()
 
-        vertices = np.array(usd_geom.GetPointsAttr().Get())
-        faces = np.array(usd_geom.GetFaceVertexIndicesAttr().Get())
+        wp.launch(
+            kernel=init_triangle_collision_data_kernel,
+            inputs=[
+                query_radius,
+                collision_detector.triangle_colliding_vertices_count,
+                collision_detector.triangle_colliding_vertices_min_dist,
+                collision_detector.resize_flags,
+            ],
+            dim=model.tri_count,
+            device=device,
+        )
 
-        return vertices, faces
+        wp.launch(
+            kernel=vertex_triangle_collision_detection_brute_force,
+            inputs=[
+                query_radius,
+                collision_detector.bvh_tris.id,
+                collision_detector.model.particle_q,
+                collision_detector.model.tri_indices,
+                collision_detector.vertex_colliding_triangles,
+                collision_detector.vertex_colliding_triangles_count,
+                collision_detector.vertex_colliding_triangles_offsets,
+                collision_detector.vertex_colliding_triangles_buffer_sizes,
+                collision_detector.vertex_colliding_triangles_min_dist,
+                collision_detector.triangle_colliding_vertices,
+                collision_detector.triangle_colliding_vertices_count,
+                collision_detector.triangle_colliding_vertices_offsets,
+                collision_detector.triangle_colliding_vertices_buffer_sizes,
+                collision_detector.triangle_colliding_vertices_min_dist,
+                collision_detector.resize_flags,
+            ],
+            dim=model.particle_count,
+            device=device,
+        )
+
+        vertex_colliding_triangles_count_2 = collision_detector.vertex_colliding_triangles_count.numpy()
+        vertex_min_dis_2 = collision_detector.vertex_colliding_triangles_min_dist.numpy()
+
+        triangle_colliding_vertices_count_2 = collision_detector.triangle_colliding_vertices_count.numpy()
+        triangle_min_dis_2 = collision_detector.triangle_colliding_vertices_min_dist.numpy()
+
+        assert_np_equal(vertex_colliding_triangles_count_2, vertex_colliding_triangles_count_1)
+        assert_np_equal(triangle_min_dis_2, triangle_min_dis_1)
+        assert_np_equal(triangle_colliding_vertices_count_2, triangle_colliding_vertices_count_1)
+        assert_np_equal(vertex_min_dis_2, vertex_min_dis_1)
+
+        wp.launch(
+            kernel=validate_vertex_collisions,
+            inputs=[
+                query_radius,
+                collision_detector.bvh_tris.id,
+                collision_detector.model.particle_q,
+                collision_detector.model.tri_indices,
+                collision_detector.vertex_colliding_triangles,
+                collision_detector.vertex_colliding_triangles_count,
+                collision_detector.vertex_colliding_triangles_offsets,
+                collision_detector.vertex_colliding_triangles_buffer_sizes,
+                collision_detector.vertex_colliding_triangles_min_dist,
+                collision_detector.resize_flags,
+            ],
+            dim=model.particle_count,
+            device=device,
+        )
+
+        wp.launch(
+            kernel=validate_triangle_collisions,
+            inputs=[
+                query_radius,
+                collision_detector.bvh_tris.id,
+                collision_detector.model.particle_q,
+                collision_detector.model.tri_indices,
+                collision_detector.triangle_colliding_vertices,
+                collision_detector.triangle_colliding_vertices_count,
+                collision_detector.triangle_colliding_vertices_offsets,
+                collision_detector.triangle_colliding_vertices_buffer_sizes,
+                collision_detector.triangle_colliding_vertices_min_dist,
+                collision_detector.resize_flags,
+            ],
+            dim=model.tri_count,
+            device=device,
+        )
+
+    wp.synchronize_device(device)
 
+
+@unittest.skipUnless(USD_AVAILABLE, "Requires usd-core")
+def test_edge_edge_collision(test, device):
     vertices, faces = get_data()
 
-    sim = Example(device, vertices, faces)
-    sim.run_edge_edge_test()
+    model, collision_detector = init_model(vertices, faces, device)
+
+    rs = [1e-2, 2e-2, 5e-2, 1e-1]
+    edge_edge_parallel_epsilon = 1e-5
+
+    for query_radius in rs:
+        collision_detector.edge_edge_collision_detection(query_radius)
+        edge_colliding_edges_count_1 = collision_detector.edge_colliding_edges_count.numpy()
+        edge_min_dist_1 = collision_detector.edge_colliding_edges_min_dist.numpy()
+
+        wp.launch(
+            kernel=edge_edge_collision_detection_brute_force,
+            inputs=[
+                query_radius,
+                collision_detector.bvh_edges.id,
+                collision_detector.model.particle_q,
+                collision_detector.model.edge_indices,
+                collision_detector.edge_colliding_edges_offsets,
+                collision_detector.edge_colliding_edges_buffer_sizes,
+                edge_edge_parallel_epsilon,
+            ],
+            outputs=[
+                collision_detector.edge_colliding_edges,
+                collision_detector.edge_colliding_edges_count,
+                collision_detector.edge_colliding_edges_min_dist,
+                collision_detector.resize_flags,
+            ],
+            dim=model.edge_count,
+            device=device,
+        )
 
+        edge_colliding_edges_count_2 = collision_detector.edge_colliding_edges_count.numpy()
+        edge_min_dist_2 = collision_detector.edge_colliding_edges_min_dist.numpy()
+
+        assert_np_equal(edge_colliding_edges_count_2, edge_colliding_edges_count_1)
+        assert_np_equal(edge_min_dist_2, edge_min_dist_1)
+
+        wp.launch(
+            kernel=validate_edge_collisions,
+            inputs=[
+                query_radius,
+                collision_detector.bvh_edges.id,
+                collision_detector.model.particle_q,
+                collision_detector.model.edge_indices,
+                collision_detector.edge_colliding_edges_offsets,
+                collision_detector.edge_colliding_edges_buffer_sizes,
+                edge_edge_parallel_epsilon,
+            ],
+            outputs=[
+                collision_detector.edge_colliding_edges,
+                collision_detector.edge_colliding_edges_count,
+                collision_detector.edge_colliding_edges_min_dist,
+                collision_detector.resize_flags,
+            ],
+            dim=model.particle_count,
+            device=device,
+        )
 
-devices = get_test_devices()
+    wp.synchronize_device(device)
 
 
 def test_particle_collision(test, device):
@@ -591,14 +572,64 @@ def test_particle_collision(test, device):
     test.assertTrue((np.linalg.norm(particle_f_2.numpy(), axis=1) == 0).all())
 
 
+def test_mesh_ground_collision_index(test, device):
+    # create a mesh with 1 triangle for testing
+    vertices = np.array(
+        [
+            [0.0, 0.0, 0.0],
+            [1.0, 1.0, 0.0],
+            [0.5, 2.0, 0.0],
+        ]
+    )
+    mesh = wp.sim.Mesh(vertices=vertices, indices=[0, 1, 2])
+    builder = wp.sim.ModelBuilder()
+    # create body with nonzero mass to ensure it is not static
+    # and contact points will be computed
+    b = builder.add_body(m=1.0)
+    builder.add_shape_mesh(
+        body=b,
+        mesh=mesh,
+        has_shape_collision=False,
+    )
+    # add another mesh that is not in contact
+    b2 = builder.add_body(m=1.0, origin=wp.transform((0.0, 3.0, 0.0), wp.quat_identity()))
+    builder.add_shape_mesh(
+        body=b2,
+        mesh=mesh,
+        has_shape_collision=False,
+    )
+    model = builder.finalize(device=device)
+    test.assertEqual(model.rigid_contact_max, 6)
+    test.assertEqual(model.shape_contact_pair_count, 0)
+    test.assertEqual(model.shape_ground_contact_pair_count, 2)
+    model.ground = True
+    # ensure all the mesh vertices will be within the contact margin
+    model.rigid_contact_margin = 2.0
+    state = model.state()
+    wp.sim.collide(model, state)
+    test.assertEqual(model.rigid_contact_count.numpy()[0], 3)
+    tids = model.rigid_contact_tids.list()
+    test.assertEqual(sorted(tids), [-1, -1, -1, 0, 1, 2])
+    tids = [t for t in tids if t != -1]
+    # retrieve the mesh vertices from the contact thread indices
+    assert_np_equal(model.rigid_contact_point0.numpy()[:3], vertices[tids])
+    assert_np_equal(model.rigid_contact_point1.numpy()[:3, 0], vertices[tids, 0])
+    assert_np_equal(model.rigid_contact_point1.numpy()[:3, 1:], np.zeros((3, 2)))
+    assert_np_equal(model.rigid_contact_normal.numpy()[:3], np.tile([0.0, 1.0, 0.0], (3, 1)))
+
+
+devices = get_test_devices(mode="basic")
+
+
 class TestCollision(unittest.TestCase):
     pass
 
 
 add_function_test(TestCollision, "test_vertex_triangle_collision", test_vertex_triangle_collision, devices=devices)
-add_function_test(TestCollision, "test_edge_edge_collision", test_vertex_triangle_collision, devices=devices)
+add_function_test(TestCollision, "test_edge_edge_collision", test_edge_edge_collision, devices=devices)
 add_function_test(TestCollision, "test_particle_collision", test_particle_collision, devices=devices)
+add_function_test(TestCollision, "test_mesh_ground_collision_index", test_mesh_ground_collision_index, devices=devices)
 
 if __name__ == "__main__":
     wp.clear_kernel_cache()
-    unittest.main(verbosity=2)
+    unittest.main(verbosity=2, failfast=True)

warp/tests/sim/test_inertia.py

@@ -0,0 +1,161 @@
+# SPDX-FileCopyrightText: Copyright (c) 2025 NVIDIA CORPORATION & AFFILIATES. All rights reserved.
+# SPDX-License-Identifier: Apache-2.0
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+# http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import unittest
+
+import numpy as np
+
+import warp as wp
+from warp.render.render_opengl import OpenGLRenderer
+from warp.sim import ModelBuilder
+from warp.sim.inertia import (
+    compute_box_inertia,
+    compute_mesh_inertia,
+    compute_sphere_inertia,
+)
+from warp.tests.unittest_utils import assert_np_equal
+
+
+class TestInertia(unittest.TestCase):
+    def test_cube_mesh_inertia(self):
+        # Unit cube
+        vertices = [
+            [1.0, 0.0, 0.0],
+            [1.0, 0.0, 1.0],
+            [0.0, 0.0, 1.0],
+            [0.0, 0.0, 0.0],
+            [1.0, 1.0, 0.0],
+            [1.0, 1.0, 1.0],
+            [0.0, 1.0, 1.0],
+            [0.0, 1.0, 0.0],
+        ]
+        indices = [
+            [1, 2, 3],
+            [7, 6, 5],
+            [4, 5, 1],
+            [5, 6, 2],
+            [2, 6, 7],
+            [0, 3, 7],
+            [0, 1, 3],
+            [4, 7, 5],
+            [0, 4, 1],
+            [1, 5, 2],
+            [3, 2, 7],
+            [4, 0, 7],
+        ]
+
+        mass_0, com_0, I_0, volume_0 = compute_mesh_inertia(
+            density=1000, vertices=vertices, indices=indices, is_solid=True
+        )
+
+        self.assertAlmostEqual(mass_0, 1000.0, delta=1e-6)
+        self.assertAlmostEqual(volume_0, 1.0, delta=1e-6)
+        assert_np_equal(np.array(com_0), np.array([0.5, 0.5, 0.5]), tol=1e-6)
+
+        # Check against analytical inertia
+        mass_box, com_box, I_box = compute_box_inertia(1000.0, 1.0, 1.0, 1.0)
+        self.assertAlmostEqual(mass_box, mass_0, delta=1e-6)
+        assert_np_equal(np.array(com_box), np.zeros(3), tol=1e-6)
+        assert_np_equal(np.array(I_0), np.array(I_box), tol=1e-4)
+
+        # Compute hollow box inertia
+        mass_0_hollow, com_0_hollow, I_0_hollow, volume_0_hollow = compute_mesh_inertia(
+            density=1000,
+            vertices=vertices,
+            indices=indices,
+            is_solid=False,
+            thickness=0.1,
+        )
+        assert_np_equal(np.array(com_0_hollow), np.array([0.5, 0.5, 0.5]), tol=1e-6)
+
+        # Add vertex between [0.0, 0.0, 0.0] and [1.0, 0.0, 0.0]
+        vertices.append([0.5, 0.0, 0.0])
+        indices[5] = [0, 8, 7]
+        indices.append([8, 3, 7])
+        indices[6] = [0, 1, 8]
+        indices.append([8, 1, 3])
+
+        mass_1, com_1, I_1, volume_1 = compute_mesh_inertia(
+            density=1000, vertices=vertices, indices=indices, is_solid=True
+        )
+
+        # Inertia values should be the same as before
+        self.assertAlmostEqual(mass_1, mass_0, delta=1e-6)
+        self.assertAlmostEqual(volume_1, volume_0, delta=1e-6)
+        assert_np_equal(np.array(com_1), np.array([0.5, 0.5, 0.5]), tol=1e-6)
+        assert_np_equal(np.array(I_1), np.array(I_0), tol=1e-4)
+
+        # Compute hollow box inertia
+        mass_1_hollow, com_1_hollow, I_1_hollow, volume_1_hollow = compute_mesh_inertia(
+            density=1000,
+            vertices=vertices,
+            indices=indices,
+            is_solid=False,
+            thickness=0.1,
+        )
+
+        # Inertia values should be the same as before
+        self.assertAlmostEqual(mass_1_hollow, mass_0_hollow, delta=2e-3)
+        self.assertAlmostEqual(volume_1_hollow, volume_0_hollow, delta=1e-6)
+        assert_np_equal(np.array(com_1_hollow), np.array([0.5, 0.5, 0.5]), tol=1e-6)
+        assert_np_equal(np.array(I_1_hollow), np.array(I_0_hollow), tol=1e-4)
+
+    def test_sphere_mesh_inertia(self):
+        vertices, indices = OpenGLRenderer._create_sphere_mesh(radius=2.5, num_latitudes=500, num_longitudes=500)
+
+        offset = np.array([1.0, 2.0, 3.0], dtype=np.float32)
+        vertices = vertices[:, :3] + offset
+
+        mass_mesh, com_mesh, I_mesh, vol_mesh = compute_mesh_inertia(
+            density=1000,
+            vertices=vertices,
+            indices=indices,
+            is_solid=True,
+        )
+
+        # Check against analytical inertia
+        mass_sphere, _, I_sphere = compute_sphere_inertia(1000.0, 2.5)
+        self.assertAlmostEqual(mass_mesh, mass_sphere, delta=1e2)
+        assert_np_equal(np.array(com_mesh), np.array(offset), tol=2e-3)
+        assert_np_equal(np.array(I_mesh), np.array(I_sphere), tol=4e2)
+        # Check volume
+        self.assertAlmostEqual(vol_mesh, 4.0 / 3.0 * np.pi * 2.5**3, delta=3e-2)
+
+    def test_body_inertia(self):
+        vertices, indices = OpenGLRenderer._create_sphere_mesh(radius=2.5, num_latitudes=500, num_longitudes=500)
+
+        offset = np.array([1.0, 2.0, 3.0], dtype=np.float32)
+        vertices = vertices[:, :3] + offset
+
+        builder = ModelBuilder()
+        b = builder.add_body()
+        tf = wp.transform(wp.vec3(4.0, 5.0, 6.0), wp.quat_rpy(0.5, -0.8, 1.3))
+        builder.add_shape_mesh(
+            b,
+            pos=tf.p,
+            rot=tf.q,
+            mesh=wp.sim.Mesh(vertices=vertices, indices=indices),
+            density=1000.0,
+        )
+        transformed_com = wp.transform_point(tf, wp.vec3(*offset))
+        assert_np_equal(np.array(builder.body_com[0]), np.array(transformed_com), tol=2e-3)
+        mass_sphere, _, I_sphere = compute_sphere_inertia(1000.0, 2.5)
+        assert_np_equal(np.array(builder.body_inertia[0]), np.array(I_sphere), tol=4e2)
+        self.assertAlmostEqual(builder.body_mass[0], mass_sphere, delta=1e2)
+
+
+if __name__ == "__main__":
+    wp.clear_kernel_cache()
+    unittest.main(verbosity=2)