rhusics_ecs/physics/systems/
contact_resolution.rs

1use std::fmt::Debug;
2use std::marker;
3use std::ops::{Add, Mul, Sub};
4
5use cgmath::{BaseFloat, EuclideanSpace, InnerSpace, Rotation, VectorSpace, Zero};
6use core::{
7    resolve_contact, ApplyAngular, ContactEvent, Inertia, Mass, PartialCrossProduct,
8    PhysicalEntity, ResolveData, Velocity,
9};
10use core::{NextFrame, Pose};
11use shrev::{EventChannel, ReaderId};
12use specs::prelude::{Component, Entity, Read, ReadStorage, Resources, System, WriteStorage};
13
14/// Do single contact, forward resolution.
15///
16/// ### Type parameters:
17///
18/// - `P`: Positional quantity, usually `Point2` or `Point3`
19/// - `R`: Rotational quantity, usually `Basis2` or `Quaternion`
20/// - `I`: Inertia, usually `Scalar` or `Matrix3`
21/// - `A`: Angular velocity, usually `Scalar` or `Vector3`
22/// - `O`: Internal type used for abstracting over cross products in 2D/3D,
23///        usually `Scalar` or `Vector3`
24/// - `T`: Transform type (`BodyPose2` or similar)
25///
26/// ### System function
27///
28/// `fn(EventChannel<ContactEvent>, Mass, PhysicalEntity, T, NextFrame<Velocity>, NextFrame<T>) -> (NextFrame<Velocity>, NextFrame<T>)`
29///
30pub struct ContactResolutionSystem<P, R, I, A, O, T>
31where
32    P: EuclideanSpace + 'static,
33    P::Diff: Debug,
34{
35    contact_reader: Option<ReaderId<ContactEvent<Entity, P>>>,
36    m: marker::PhantomData<(R, I, A, O, T)>,
37}
38
39impl<P, R, I, A, O, T> ContactResolutionSystem<P, R, I, A, O, T>
40where
41    T: Pose<P, R>,
42    P: EuclideanSpace,
43    P::Scalar: BaseFloat,
44    P::Diff: VectorSpace + InnerSpace + Debug + PartialCrossProduct<P::Diff, Output = O>,
45    R: Rotation<P> + ApplyAngular<P::Scalar, A>,
46    O: PartialCrossProduct<P::Diff, Output = P::Diff>,
47    A: PartialCrossProduct<P::Diff, Output = P::Diff> + Clone + Zero,
48    for<'b> &'b A: Sub<O, Output = A> + Add<O, Output = A>,
49    I: Inertia<Orientation = R> + Mul<O, Output = O>,
50{
51    /// Create system.
52    pub fn new() -> Self {
53        Self {
54            contact_reader: None,
55            m: marker::PhantomData,
56        }
57    }
58}
59
60impl<'a, P, R, I, A, O, T> System<'a> for ContactResolutionSystem<P, R, I, A, O, T>
61where
62    T: Pose<P, R> + Component + Send + Sync + 'static,
63    P: EuclideanSpace + Send + Sync + 'static,
64    P::Scalar: BaseFloat + Send + Sync + 'static,
65    P::Diff: VectorSpace
66        + InnerSpace
67        + Debug
68        + Send
69        + Sync
70        + 'static
71        + PartialCrossProduct<P::Diff, Output = O>,
72    R: Rotation<P> + ApplyAngular<P::Scalar, A> + Send + Sync + 'static,
73    O: PartialCrossProduct<P::Diff, Output = P::Diff>,
74    A: PartialCrossProduct<P::Diff, Output = P::Diff> + Clone + Zero + Send + Sync + 'static,
75    for<'b> &'b A: Sub<O, Output = A> + Add<O, Output = A>,
76    I: Inertia<Orientation = R> + Mul<O, Output = O> + Send + Sync + 'static,
77{
78    type SystemData = (
79        Read<'a, EventChannel<ContactEvent<Entity, P>>>,
80        ReadStorage<'a, Mass<P::Scalar, I>>,
81        ReadStorage<'a, PhysicalEntity<P::Scalar>>,
82        WriteStorage<'a, NextFrame<Velocity<P::Diff, A>>>,
83        ReadStorage<'a, T>,
84        WriteStorage<'a, NextFrame<T>>,
85    );
86
87    fn run(&mut self, data: Self::SystemData) {
88        let (contacts, masses, entities, mut next_velocities, poses, mut next_poses) = data;
89
90        // Process contacts since last run
91        for contact in contacts.read(&mut self.contact_reader.as_mut().unwrap()) {
92            // Resolve contact
93            let change_set = match (
94                from_storage(
95                    contact.bodies.0,
96                    &next_velocities,
97                    &next_poses,
98                    &poses,
99                    &masses,
100                    &entities,
101                ),
102                from_storage(
103                    contact.bodies.1,
104                    &next_velocities,
105                    &next_poses,
106                    &poses,
107                    &masses,
108                    &entities,
109                ),
110            ) {
111                (Some(resolve_0), Some(resolve_1)) => {
112                    Some(resolve_contact(&contact.contact, &resolve_0, &resolve_1))
113                }
114                _ => None,
115            };
116            if let Some(cs) = change_set {
117                // Apply computed change sets
118                cs.0.apply(
119                    next_poses.get_mut(contact.bodies.0),
120                    next_velocities.get_mut(contact.bodies.0),
121                );
122                cs.1.apply(
123                    next_poses.get_mut(contact.bodies.1),
124                    next_velocities.get_mut(contact.bodies.1),
125                );
126            }
127        }
128    }
129
130    fn setup(&mut self, res: &mut Resources) {
131        use specs::prelude::SystemData;
132        Self::SystemData::setup(res);
133        self.contact_reader = Some(
134            res.fetch_mut::<EventChannel<ContactEvent<Entity, P>>>()
135                .register_reader(),
136        );
137    }
138}
139
140fn from_storage<'a, P, T, R, A, I>(
141    entity: Entity,
142    next_velocities: &'a WriteStorage<NextFrame<Velocity<P::Diff, A>>>,
143    next_poses: &'a WriteStorage<NextFrame<T>>,
144    poses: &'a ReadStorage<T>,
145    masses: &'a ReadStorage<Mass<P::Scalar, I>>,
146    entities: &'a ReadStorage<PhysicalEntity<P::Scalar>>,
147) -> Option<ResolveData<'a, T, P, R, I, A>>
148where
149    P: EuclideanSpace + Send + Sync + 'static,
150    P::Scalar: BaseFloat + Send + Sync + 'static,
151    P::Diff: Send + Sync + 'static,
152    T: Pose<P, R> + Component + Send + Sync + 'static,
153    R: Rotation<P> + Send + Sync + 'static,
154    A: Clone + Send + Sync + 'static,
155    I: Clone + Send + Sync + 'static,
156{
157    match (entities.get(entity), masses.get(entity), poses.get(entity)) {
158        (Some(e), Some(mass), Some(pose)) if e.active() => Some(ResolveData::new(
159            next_velocities.get(entity),
160            next_poses.get(entity).map(|p| &p.value).unwrap_or(pose),
161            mass,
162            e.material(),
163        )),
164        _ => None,
165    }
166}
rhusics_ecs/physics/systems/contact_resolution.rs

rhusics_ecs/physics/systems/
contact_resolution.rs
