geometry.py

 1
 2
 3
 4
 5
 6
 7
 8
 9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
# SPDX-License-Identifier: AGPL-3.0-or-later
# Copyright (C) 2025 SWGY, Inc
#
# This program is free software: you can redistribute it and/or modify
# it under the terms of the GNU Affero General Public License as published by
# the Free Software Foundation, either version 3 of the License, or
# (at your option) any later version.
#
# This program is distributed in the hope that it will be useful,
# but WITHOUT ANY WARRANTY; without even the implied warranty of
# MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
# GNU Affero General Public License for more details.
#
# You should have received a copy of the GNU Affero General Public License
# along with this program.  If not, see <https://www.gnu.org/licenses/>.
"""
Geometry loading and placement.

Loads the MICH helmet and the SAPI plate meshes (posed twice: front and back
plates) and transforms them into the simulation's world frame, where the sensor
sits at the origin, the helmet faces -Y, and Z is up. The transforms below seat
each asset around that origin sensor; they are applied once at startup, and
`fix_normals()` gives each mesh consistent outward face normals.
"""

import os

import numpy as np
import trimesh


def _load_mesh(path: str, label: str) -> trimesh.base.Trimesh:
    """Load a mesh file, with a clear error if it is missing or unreadable."""
    if not os.path.exists(path):
        raise FileNotFoundError(f"{label} geometry file not found: {path}")
    try:
        return trimesh.load_mesh(path)
    except Exception as e:
        raise ValueError(f"Failed to load {label} geometry from {path}: {e}") from e


def load_helmet(path: str) -> trimesh.base.Trimesh:
    """Load the MICH helmet and orient it around the sensor.

    Rotate +90 deg about X (asset frame -> world Z-up) and scale by 0.5 so the
    shell seats around the origin sensor.
    """
    helmet = _load_mesh(path, "Helmet")
    rotation = trimesh.transformations.rotation_matrix(np.pi / 2, [1, 0, 0])
    scale = trimesh.transformations.scale_matrix(0.5)
    helmet.apply_transform(
        trimesh.transformations.concatenate_matrices(rotation, scale))
    helmet.fix_normals()
    return helmet


def load_vest(path: str) -> trimesh.base.Trimesh:
    """Load the SAPI vest plate and place it in front of the sensor.

    Rotate 180 deg about X, then translate to [0, -0.12, -0.5] so the plate sits
    just in front of (and below) the origin sensor, at the torso position.
    """
    vest = _load_mesh(path, "SAPI vest")
    rotation = trimesh.transformations.rotation_matrix(np.pi, [1, 0, 0])
    translation = trimesh.transformations.translation_matrix([0, -0.12, -0.5])
    vest.apply_transform(
        trimesh.transformations.concatenate_matrices(translation, rotation))
    vest.fix_normals()
    return vest


def load_back_plate(path: str) -> trimesh.base.Trimesh:
    """Load the SAPI plate posed as the back plate.

    Mirror of the front plate: the same 180 deg flip about X, then 180 deg about
    Z so the concave face looks forward (toward the body), translated to
    [0, 0.18, -0.5] -- 0.3 m behind the front plate, at the same height.
    """
    plate = _load_mesh(path, "SAPI back plate")
    rot_x = trimesh.transformations.rotation_matrix(np.pi, [1, 0, 0])
    rot_z = trimesh.transformations.rotation_matrix(np.pi, [0, 0, 1])
    translation = trimesh.transformations.translation_matrix([0, 0.18, -0.5])
    plate.apply_transform(
        trimesh.transformations.concatenate_matrices(translation, rot_z, rot_x))
    plate.fix_normals()
    return plate