phyz_model/
state.rs

1//! Simulation state — mutable per-timestep data.
2
3use phyz_math::{DVec, SpatialTransform};
4
5/// Mutable simulation state.
6#[derive(Debug, Clone)]
7pub struct State {
8    /// Generalized positions (joint angles/displacements).
9    pub q: DVec,
10    /// Generalized velocities.
11    pub v: DVec,
12    /// Control inputs / applied torques.
13    pub ctrl: DVec,
14    /// Simulation time.
15    pub time: f64,
16
17    // Cached quantities (filled by forward kinematics / dynamics)
18    /// Body-to-world transforms for each body.
19    pub body_xform: Vec<SpatialTransform>,
20    /// External generalized forces (e.g., from contacts).
21    pub qfrc_external: DVec,
22}
23
24impl State {
25    /// Create a zero-initialized state for `nq` position DOFs and `nv` velocity DOFs.
26    pub fn new(nq: usize, nv: usize, nbodies: usize) -> Self {
27        Self {
28            q: DVec::zeros(nq),
29            v: DVec::zeros(nv),
30            ctrl: DVec::zeros(nv),
31            time: 0.0,
32            body_xform: vec![SpatialTransform::identity(); nbodies],
33            qfrc_external: DVec::zeros(nv),
34        }
35    }
36
37    /// Compute total kinetic energy given mass matrix.
38    /// KE = 0.5 * v^T * M * v
39    pub fn kinetic_energy(&self, mass_matrix: &phyz_math::DMat) -> f64 {
40        0.5 * self.v.dot(&(mass_matrix * &self.v))
41    }
42}
phyz_model/state.rs

phyz_model/
state.rs
