anput_physics/constraints/
distance.rs

1use crate::{
2    PhysicsSimulation, Scalar,
3    components::{Mass, PhysicsParticle, Position, Rotation},
4    utils::quat_from_axis_angle,
5};
6use anput::{
7    query::{Include, Lookup},
8    systems::SystemContext,
9    universe::Res,
10    world::World,
11};
12use serde::{Deserialize, Serialize};
13use std::error::Error;
14
15/// Relation between two particles.
16#[derive(Debug, Clone, Copy, PartialEq, Serialize, Deserialize)]
17pub struct DistanceConstraint {
18    pub distance: Scalar,
19    pub compliance: Scalar,
20    pub lambda: Scalar,
21}
22
23pub fn solve_distance_constraint<const LOCKING: bool>(
24    context: SystemContext,
25) -> Result<(), Box<dyn Error>> {
26    let (world, simulation, particle_lookup) = context.fetch::<(
27        &World,
28        Res<LOCKING, &PhysicsSimulation>,
29        Lookup<
30            LOCKING,
31            (
32                &mut Position,
33                Option<&mut Rotation>,
34                &Mass,
35                Include<PhysicsParticle>,
36            ),
37        >,
38    )>()?;
39
40    let mut particle_lookup = particle_lookup.lookup_access(world);
41
42    for (from, constraint, to) in world.relations_mut::<LOCKING, DistanceConstraint>() {
43        let Some((from_position, from_rotation, from_mass, _)) = particle_lookup.access(from)
44        else {
45            continue;
46        };
47        let Some((to_position, to_rotation, to_mass, _)) = particle_lookup.access(to) else {
48            continue;
49        };
50
51        let from_weight = from_mass.inverse();
52        let to_weight = to_mass.inverse();
53        let delta = to_position.current - from_position.current;
54        let distance = delta.magnitude();
55        if distance < Scalar::EPSILON {
56            continue;
57        }
58        let normal = delta / distance;
59        let error = distance - constraint.distance;
60        let alpha = constraint.compliance / (simulation.delta_time * simulation.delta_time);
61        let lambda = -(error + alpha * constraint.lambda) / (from_weight + to_weight + alpha);
62        let impulse = normal * lambda;
63
64        constraint.lambda += lambda;
65        from_position.current -= impulse * from_weight;
66        to_position.current += impulse * to_weight;
67
68        if let Some(from_rotation) = from_rotation {
69            let angular_correction = normal.cross(-impulse) * from_weight;
70            let angle = angular_correction.magnitude();
71            if angle > Scalar::EPSILON {
72                let axis = angular_correction / angle;
73                let delta = quat_from_axis_angle(axis, angle);
74                from_rotation.current = (from_rotation.current * delta).normalized();
75            }
76        }
77        if let Some(to_rotation) = to_rotation {
78            let angular_correction = normal.cross(impulse) * to_weight;
79            let angle = angular_correction.magnitude();
80            if angle > Scalar::EPSILON {
81                let axis = angular_correction / angle;
82                let delta = quat_from_axis_angle(axis, angle);
83                to_rotation.current = (to_rotation.current * delta).normalized();
84            }
85        }
86    }
87
88    Ok(())
89}
anput_physics/constraints/distance.rs

anput_physics/constraints/
distance.rs
