phyz_model/body.rs
1//! Rigid body definition.
2
3use crate::material::ContactMaterial;
4use phyz_math::{SpatialInertia, SpatialTransform};
5
6/// A rigid body in the kinematic tree.
7#[derive(Debug, Clone)]
8pub struct Body {
9 /// Name of the body (optional, for debugging).
10 pub name: String,
11 /// Spatial inertia in body-local frame.
12 pub inertia: SpatialInertia,
13 /// Index of the parent body (-1 for world/root).
14 pub parent: i32,
15 /// Index of the joint connecting this body to its parent.
16 pub joint_idx: usize,
17 /// Primary collision geometry, centred on the body frame (if any).
18 ///
19 /// This is the shape used by the contact pipeline. For sources that
20 /// describe several offset shapes per body (URDF, MJCF) the full set
21 /// lives in [`Body::collisions`]; `geometry` mirrors the first one whose
22 /// origin is the identity, so existing single-shape consumers keep working.
23 pub geometry: Option<Geometry>,
24 /// All collision shapes attached to this body, each with its own offset.
25 pub collisions: Vec<GeomInstance>,
26 /// Visual-only shapes attached to this body (not used for contact).
27 pub visuals: Vec<GeomInstance>,
28 /// This body's own contact material, or `None` to use the scene default.
29 ///
30 /// The material applies to **every contact this body is part of** —
31 /// against the ground and against every other body — and the pair is
32 /// resolved by [`ContactMaterial::combine`]. `None` means "whatever the
33 /// scene is made of", which is what keeps existing models unchanged:
34 /// [`crate::Model::contact_materials`] fills those slots with the scene
35 /// material and the assembled problem is bit-identical to the old
36 /// `vec![one; n_bodies]`.
37 ///
38 /// **Which body you attach it to is a physical decision, not a
39 /// bookkeeping one.** `combine` takes the *maximum* friction of the pair,
40 /// so a grippy material grips everything the body touches. Putting grip
41 /// tape's `mu = 1.5` on a skateboard deck also makes the deck's underside
42 /// grab the asphalt when the tail scrapes — measured, that braked a board
43 /// from 0.6 to 0.39 m/s and killed the trick. Putting the same 1.5 on the
44 /// *shoes* raises only the foot-on-deck contacts and leaves the deck-on-
45 /// ground contact at wood's 0.6. See the crate docs on
46 /// [`ContactMaterial::combine`].
47 ///
48 /// # The ground has no material of its own
49 ///
50 /// A contact against the ground plane or a heightfield uses the touching
51 /// body's material **verbatim** — there is no second material to combine
52 /// with, so nothing is combined. That is easy to miss now that bodies
53 /// differ from one another: setting a deck to wood's `0.6` and passing
54 /// asphalt's `0.8` as the scene material gives a deck-on-ground friction
55 /// of `0.6`, not `max(0.6, 0.8)`. The scene material is a *default filler*
56 /// for bodies that carry none, not a property of the ground.
57 ///
58 /// So the friction a body has against the ground is exactly the friction
59 /// you wrote on that body, and a "road surface" is expressed by leaving
60 /// the bodies that touch it on the scene material.
61 pub material: Option<ContactMaterial>,
62}
63
64impl Body {
65 /// Create a body with no attached geometry.
66 pub fn new(name: &str, inertia: SpatialInertia, parent: i32, joint_idx: usize) -> Self {
67 Self {
68 name: name.to_string(),
69 inertia,
70 parent,
71 joint_idx,
72 geometry: None,
73 collisions: Vec::new(),
74 visuals: Vec::new(),
75 material: None,
76 }
77 }
78
79 /// Attach a contact material to this body.
80 ///
81 /// Read [`Body::material`] before choosing which body gets it: friction
82 /// combines by `max`, so the material belongs on the part that should
83 /// grip, not on the part it touches.
84 pub fn with_material(mut self, material: ContactMaterial) -> Self {
85 self.material = Some(material);
86 self
87 }
88}
89
90/// A geometry placed at an offset within a body frame.
91#[derive(Debug, Clone)]
92pub struct GeomInstance {
93 /// Optional name from the source file.
94 pub name: Option<String>,
95 /// Placement of the shape within the body frame.
96 ///
97 /// Follows the same convention as [`crate::Joint::parent_to_joint`]:
98 /// `origin.pos` is the shape origin expressed in body coordinates and
99 /// `origin.rot` is the coordinate transform *body → shape*, so the shape's
100 /// orientation in the body frame is `origin.rot.transpose()`.
101 pub origin: SpatialTransform,
102 /// The shape itself.
103 pub geometry: Geometry,
104}
105
106impl GeomInstance {
107 /// Create a geom instance at the given offset.
108 pub fn new(geometry: Geometry, origin: SpatialTransform) -> Self {
109 Self {
110 name: None,
111 origin,
112 geometry,
113 }
114 }
115
116 /// Create a geom instance centred on the body frame.
117 pub fn centered(geometry: Geometry) -> Self {
118 Self::new(geometry, SpatialTransform::identity())
119 }
120
121 /// True if this shape sits exactly on the body frame.
122 pub fn is_centered(&self) -> bool {
123 const EPS: f64 = 1e-12;
124 self.origin.pos.norm_sq() < EPS
125 && (self.origin.rot - phyz_math::Mat3::identity()).norm_sq() < EPS
126 }
127}
128
129/// Collision geometry types (mirrors `phyz-collision`'s shapes for convenience).
130#[derive(Debug, Clone)]
131pub enum Geometry {
132 /// A ball centred on the body frame origin.
133 Sphere {
134 /// Ball radius, in metres.
135 radius: f64,
136 },
137 /// A capsule (swept sphere) aligned with the body frame Z axis.
138 Capsule {
139 /// Radius of the swept sphere, in metres.
140 radius: f64,
141 /// Distance between the two cap centres, in metres.
142 length: f64,
143 },
144 /// An axis-aligned box centred on the body frame origin.
145 Box {
146 /// Half-extent along each body frame axis, in metres.
147 half_extents: phyz_math::Vec3,
148 },
149 /// A cylinder aligned with the body frame Z axis.
150 Cylinder {
151 /// Cylinder radius, in metres.
152 radius: f64,
153 /// Cylinder height along Z, in metres.
154 height: f64,
155 },
156 /// An arbitrary triangle mesh in body frame coordinates.
157 Mesh {
158 /// Vertex positions in body frame.
159 vertices: Vec<phyz_math::Vec3>,
160 /// Triangles, as triples of indices into `vertices`.
161 faces: Vec<[usize; 3]>,
162 },
163 /// A half-space through the body frame origin.
164 Plane {
165 /// Outward unit normal of the plane, in body frame.
166 normal: phyz_math::Vec3,
167 },
168}
169
170/// What one side of a pairwise quantity is attached to: a body in the model,
171/// or the static world.
172///
173/// # Why this type exists
174///
175/// Contacts, constraints and relative Jacobians all need to express "body `i`
176/// against body `j`, where `j` may be the ground". The engine long spelled
177/// that as a plain `usize` with `usize::MAX` meaning the world — an *in-band
178/// sentinel*, and a hazardous one: `usize::MAX` is a perfectly valid-looking
179/// index, so a stale or miscomputed index that happens to land on it is
180/// silently reinterpreted as "the world" instead of panicking. The failure is
181/// not a crash but a contact that quietly stops pushing back on one side.
182///
183/// `Attachment` makes the two cases distinct at the type level, so the
184/// compiler forces every consumer to say which it means. It carries no
185/// sentinel and cannot be constructed from a bare index by accident — there is
186/// deliberately no `From<usize>` impl, because that would reintroduce exactly
187/// the ambiguity this type removes.
188#[derive(Debug, Clone, Copy, PartialEq, Eq, Hash)]
189pub enum Attachment {
190 /// A body in the model, by index.
191 Body(usize),
192 /// The static world: infinitely massive, never moves, and contributes no
193 /// columns to a Jacobian.
194 World,
195}
196
197impl Attachment {
198 /// The body index, or `None` for [`Attachment::World`].
199 #[inline]
200 pub fn body(self) -> Option<usize> {
201 match self {
202 Attachment::Body(i) => Some(i),
203 Attachment::World => None,
204 }
205 }
206
207 /// Whether this is the static world.
208 #[inline]
209 pub fn is_world(self) -> bool {
210 matches!(self, Attachment::World)
211 }
212}