mmd_anim_runtime/runtime/
physics.rs

1use glam::{Mat4, Vec3A};
2
3use super::{IkSolveOptions, RuntimeInstance};
4use crate::{BoneIndex, ik_primitive::constrain_rotation_to_axis};
5
6#[derive(Clone, Copy, Debug, Default, PartialEq, Eq)]
7pub enum PhysicsMode {
8    /// Physics is disabled. Switching to this mode resets the accumulated
9    /// physics tick state.
10    #[default]
11    Off,
12    /// Host-synchronized physics. The host is expected to evaluate animation
13    /// frames before stepping physics, so random seeks should reset/reseed the
14    /// backend before continuing.
15    Trace,
16    /// Wall-clock physics. The host may keep stepping physics while animation
17    /// sampling is paused, using the latest evaluated pose as the kinematic
18    /// target.
19    Live,
20}
21
22impl PhysicsMode {
23    pub fn steps_backend(self) -> bool {
24        matches!(self, Self::Trace | Self::Live)
25    }
26}
27
28#[derive(Clone, Copy, Debug, PartialEq)]
29pub struct PhysicsTickConfig {
30    pub fixed_substep_seconds: f32,
31    pub max_substeps_per_tick: u32,
32}
33
34impl Default for PhysicsTickConfig {
35    fn default() -> Self {
36        Self {
37            fixed_substep_seconds: 1.0 / 120.0,
38            max_substeps_per_tick: 8,
39        }
40    }
41}
42
43impl PhysicsTickConfig {
44    pub fn sanitized(self) -> Self {
45        let default = Self::default();
46        let fixed_substep_seconds =
47            if self.fixed_substep_seconds.is_finite() && self.fixed_substep_seconds > 0.0 {
48                self.fixed_substep_seconds
49            } else {
50                default.fixed_substep_seconds
51            };
52        let max_substeps_per_tick = self.max_substeps_per_tick.max(1);
53        Self {
54            fixed_substep_seconds,
55            max_substeps_per_tick,
56        }
57    }
58}
59
60#[derive(Clone, Copy, Debug, Default, PartialEq)]
61pub struct PhysicsStepStats {
62    pub input_dt_seconds: f32,
63    pub clamped_dt_seconds: f32,
64    pub substeps: u32,
65    pub accumulator_seconds: f32,
66}
67
68impl RuntimeInstance {
69    #[inline]
70    pub fn physics_mode(&self) -> PhysicsMode {
71        self.physics_mode
72    }
73
74    pub fn set_physics_mode(&mut self, mode: PhysicsMode) {
75        if mode == PhysicsMode::Off {
76            self.reset_physics_tick();
77        }
78        self.physics_mode = mode;
79    }
80
81    #[inline]
82    pub fn physics_tick_config(&self) -> PhysicsTickConfig {
83        self.physics_tick_config
84    }
85
86    pub fn set_physics_tick_config(&mut self, config: PhysicsTickConfig) {
87        self.physics_tick_config = config.sanitized();
88        self.physics_accumulator_seconds = self
89            .physics_accumulator_seconds
90            .min(self.max_physics_dt_seconds());
91    }
92
93    #[inline]
94    pub fn physics_accumulator_seconds(&self) -> f32 {
95        self.physics_accumulator_seconds
96    }
97
98    pub fn reset_physics_tick(&mut self) {
99        self.physics_accumulator_seconds = 0.0;
100    }
101
102    pub fn apply_physics_world_matrices(
103        &mut self,
104        physics_world_matrices: &[Option<Mat4>],
105    ) -> usize {
106        let mut updated = 0;
107        let mut earliest_eval_order_position = None;
108        let mut target_world_matrices = self.pose.world_matrices().to_vec();
109        let mut has_physics_target = vec![false; self.model.bone_count()];
110
111        for (bone_index, target_world_matrix) in physics_world_matrices.iter().enumerate() {
112            let Some(target_world_matrix) = target_world_matrix else {
113                continue;
114            };
115            let Some(slot) = target_world_matrices.get_mut(bone_index) else {
116                continue;
117            };
118            *slot = *target_world_matrix;
119            has_physics_target[bone_index] = true;
120        }
121
122        for bone in self.model.eval_order() {
123            let bone_index = bone.as_usize();
124            if has_physics_target[bone_index] {
125                continue;
126            }
127            let local_matrix = self.current_local_matrix_for_physics_scratch(*bone);
128            target_world_matrices[bone_index] = self
129                .model
130                .parent_index(*bone)
131                .map(|parent| target_world_matrices[parent.as_usize()] * local_matrix)
132                .unwrap_or(local_matrix);
133        }
134
135        for bone_index in 0..self.model.bone_count() {
136            if !has_physics_target[bone_index] {
137                continue;
138            }
139
140            let bone = BoneIndex(bone_index as u32);
141            let parent_inverse_world = self
142                .model
143                .parent_index(bone)
144                .map(|parent| target_world_matrices[parent.as_usize()].inverse())
145                .unwrap_or(Mat4::IDENTITY);
146            let local_matrix = parent_inverse_world * target_world_matrices[bone_index];
147            let (scale, rotation, translation) = local_matrix.to_scale_rotation_translation();
148
149            self.pose.set_local_position_offset(
150                bone,
151                Vec3A::from(translation) - self.model.rest_position(bone),
152            );
153            self.pose.set_local_rotation(bone, rotation.normalize());
154            self.pose.set_local_scale(bone, Vec3A::from(scale));
155
156            let eval_order_position = self.model.eval_order_position(bone);
157            earliest_eval_order_position = Some(
158                earliest_eval_order_position.map_or(eval_order_position, |current: usize| {
159                    current.min(eval_order_position)
160                }),
161            );
162            updated += 1;
163        }
164
165        if let Some(start) = earliest_eval_order_position {
166            self.update_world_matrices_from_eval_order_position(start);
167        }
168
169        updated
170    }
171
172    fn current_local_matrix_for_physics_scratch(&self, bone: BoneIndex) -> Mat4 {
173        let mut local_position =
174            self.model.rest_position(bone) + self.pose.local_position_offset(bone);
175        let mut local_rotation = self.pose.local_rotation(bone);
176        let local_scale = self.pose.local_scale(bone);
177
178        if let Some(append_index) = self.model.append_transform_index(bone) {
179            let append = self.model.append_transform(append_index);
180            if append.affect_rotation {
181                local_rotation = (local_rotation * self.pose.append_rotation(bone)).normalize();
182            }
183            if append.affect_translation {
184                local_position += self.pose.append_position_offset(bone);
185            }
186        }
187
188        if let Some(axis) = self.model.fixed_axis_constraint(bone) {
189            local_rotation = constrain_rotation_to_axis(local_rotation, axis);
190        }
191
192        Mat4::from_scale_rotation_translation(
193            local_scale.into(),
194            local_rotation,
195            local_position.into(),
196        )
197    }
198
199    /// Advance the physics clock independently from animation sampling.
200    ///
201    /// This is a no-backend implementation for now: it consumes fixed 120 Hz
202    /// substeps and refreshes the after-physics evaluation phase, but does not
203    /// simulate rigid bodies until a physics backend is attached.
204    pub fn step_physics(&mut self, dt_seconds: f32) -> PhysicsStepStats {
205        self.step_physics_with_ik_options(dt_seconds, IkSolveOptions::default())
206    }
207
208    pub fn step_physics_with_ik_options(
209        &mut self,
210        dt_seconds: f32,
211        options: IkSolveOptions,
212    ) -> PhysicsStepStats {
213        let stats = self.advance_physics_tick_clock(dt_seconds);
214        self.evaluate_current_pose_after_physics_with_ik_options(options);
215        stats
216    }
217
218    pub fn advance_physics_tick_clock(&mut self, dt_seconds: f32) -> PhysicsStepStats {
219        let input_dt_seconds = dt_seconds;
220        let clamped_dt_seconds = self.clamped_physics_dt(dt_seconds);
221        self.physics_accumulator_seconds += clamped_dt_seconds;
222
223        let mut substeps = 0;
224        while self.physics_accumulator_seconds + f32::EPSILON
225            >= self.physics_tick_config.fixed_substep_seconds
226            && substeps < self.physics_tick_config.max_substeps_per_tick
227        {
228            self.physics_accumulator_seconds -= self.physics_tick_config.fixed_substep_seconds;
229            substeps += 1;
230        }
231        if substeps == self.physics_tick_config.max_substeps_per_tick {
232            self.physics_accumulator_seconds = self
233                .physics_accumulator_seconds
234                .min(self.physics_tick_config.fixed_substep_seconds);
235        }
236
237        PhysicsStepStats {
238            input_dt_seconds,
239            clamped_dt_seconds,
240            substeps,
241            accumulator_seconds: self.physics_accumulator_seconds,
242        }
243    }
244
245    fn clamped_physics_dt(&self, dt_seconds: f32) -> f32 {
246        if !dt_seconds.is_finite() || dt_seconds <= 0.0 {
247            return 0.0;
248        }
249        dt_seconds.min(self.max_physics_dt_seconds())
250    }
251
252    fn max_physics_dt_seconds(&self) -> f32 {
253        self.physics_tick_config.fixed_substep_seconds
254            * self.physics_tick_config.max_substeps_per_tick as f32
255    }
256}
mmd_anim_runtime/runtime/physics.rs

mmd_anim_runtime/runtime/
physics.rs
