anput_physics/
solvers.rs

1use crate::{
2    PhysicsSimulation, Scalar,
3    components::{
4        AngularVelocity, ExternalForces, Gravity, LinearVelocity, Mass, ParticleMaterial, Position,
5        Rotation,
6    },
7    utils::quat_from_axis_angle,
8};
9use anput::{query::Query, systems::SystemContext, universe::Res, world::World};
10use std::error::Error;
11
12pub fn apply_external_forces<const LOCKING: bool>(
13    context: SystemContext,
14) -> Result<(), Box<dyn Error>> {
15    let (world, simulation, query) = context.fetch::<(
16        &World,
17        Res<LOCKING, &PhysicsSimulation>,
18        Query<
19            LOCKING,
20            (
21                &mut ExternalForces,
22                &Mass,
23                &mut LinearVelocity,
24                Option<&mut AngularVelocity>,
25            ),
26        >,
27    )>()?;
28
29    for (external_forces, mass, linear_velocity, angular_velocity) in query.query(world) {
30        linear_velocity.value += external_forces.force * mass.inverse() * simulation.delta_time;
31        linear_velocity.value += external_forces.linear_impulse * mass.inverse();
32
33        if let Some(angular_velocity) = angular_velocity {
34            angular_velocity.value +=
35                external_forces.torque * mass.inverse() * simulation.delta_time;
36            angular_velocity.value += external_forces.angular_impulse * mass.inverse();
37        }
38
39        external_forces.clear();
40    }
41
42    Ok(())
43}
44
45pub fn integrate_velocities<const LOCKING: bool>(
46    context: SystemContext,
47) -> Result<(), Box<dyn Error>> {
48    let (world, simulation, query) = context.fetch::<(
49        &World,
50        Res<LOCKING, &PhysicsSimulation>,
51        Query<
52            LOCKING,
53            (
54                &mut Position,
55                Option<&mut Rotation>,
56                &LinearVelocity,
57                Option<&AngularVelocity>,
58            ),
59        >,
60    )>()?;
61
62    for (position, rotation, linear_velocity, angular_velocity) in query.query(world) {
63        position.current += linear_velocity.value * simulation.delta_time;
64
65        if let Some(rotation) = rotation
66            && let Some(angular_velocity) = angular_velocity
67        {
68            let angle = angular_velocity.value.magnitude() * simulation.delta_time;
69            if angle.abs() > Scalar::EPSILON {
70                let axis = angular_velocity.value / angle;
71                rotation.current =
72                    (rotation.current * quat_from_axis_angle(axis, angle)).normalized();
73            }
74        }
75    }
76
77    Ok(())
78}
79
80pub fn cache_current_as_previous_state<const LOCKING: bool>(
81    context: SystemContext,
82) -> Result<(), Box<dyn Error>> {
83    let (world, query) = context.fetch::<(
84        &World,
85        Query<LOCKING, (&mut Position, Option<&mut Rotation>)>,
86    )>()?;
87
88    for (position, rotation) in query.query(world) {
89        position.cache_current_as_previous();
90        if let Some(rotation) = rotation {
91            rotation.cache_current_as_previous();
92        }
93    }
94
95    Ok(())
96}
97
98pub fn recalculate_velocities<const LOCKING: bool>(
99    context: SystemContext,
100) -> Result<(), Box<dyn Error>> {
101    let (world, simulation, query) = context.fetch::<(
102        &World,
103        Res<LOCKING, &PhysicsSimulation>,
104        Query<
105            LOCKING,
106            (
107                &Position,
108                Option<&Rotation>,
109                &mut LinearVelocity,
110                Option<&mut AngularVelocity>,
111            ),
112        >,
113    )>()?;
114
115    let inverse_delta_time = simulation.inverse_delta_time();
116
117    for (position, rotation, linear_velocity, angular_velocity) in query.query(world) {
118        linear_velocity.value += position.change() * inverse_delta_time;
119
120        if let Some(rotation) = rotation
121            && let Some(velocity) = angular_velocity
122        {
123            let (angle, axis) = rotation.change().into_angle_axis();
124            velocity.value += axis * (angle * inverse_delta_time);
125        }
126    }
127
128    Ok(())
129}
130
131pub fn apply_gravity<const LOCKING: bool>(context: SystemContext) -> Result<(), Box<dyn Error>> {
132    let (world, simulation, query) = context.fetch::<(
133        &World,
134        Res<LOCKING, &PhysicsSimulation>,
135        Query<LOCKING, (Option<&Gravity>, &mut ExternalForces)>,
136    )>()?;
137
138    for (gravity, external_forces) in query.query(world) {
139        let gravity = gravity.map(|v| v.value).unwrap_or(simulation.gravity);
140        external_forces.accumulate_linear_impulse(gravity * simulation.delta_time);
141    }
142
143    Ok(())
144}
145
146pub fn dampening_solver<const LOCKING: bool>(context: SystemContext) -> Result<(), Box<dyn Error>> {
147    let (world, query) = context.fetch::<(
148        &World,
149        Query<LOCKING, (&mut Position, Option<&mut Rotation>, &ParticleMaterial)>,
150    )>()?;
151
152    for (position, rotation, material) in query.query(world) {
153        let mut delta = position.change();
154        delta *= material.linear_damping;
155        if delta.magnitude_squared()
156            < material.linear_rest_threshold * material.linear_rest_threshold
157        {
158            delta = Default::default();
159        }
160        position.current = position.previous() + delta;
161
162        if let Some(rotation) = rotation {
163            let delta = rotation.change();
164            let (mut angle, axis) = delta.into_angle_axis();
165            angle *= material.angular_damping;
166            if angle.abs() < material.angular_rest_threshold {
167                angle = Default::default();
168            }
169            let delta = quat_from_axis_angle(axis, angle);
170            rotation.current = (rotation.previous() * delta).normalized();
171        }
172    }
173
174    Ok(())
175}
anput_physics/solvers.rs

anput_physics/
solvers.rs
