linear_sim/constraint/
distance.rs

1use either::Either;
2
3use crate::object;
4use crate::math::*;
5
6use super::{Constraint, Impulses, PostImpulses};
7use super::contact::{CONTACT_DISTANCE, RESTING_VELOCITY};
8
9#[cfg(feature = "derive_serdes")]
10use serde::{Deserialize, Serialize};
11
12/// A distance constraint
13#[cfg_attr(feature = "derive_serdes", derive(Deserialize, Serialize))]
14#[derive(Clone, Debug, PartialEq)]
15pub struct Distance {
16  pub interval : geometry::Interval <NonNegative <f64>>
17}
18
19impl Distance {
20  #[inline]
21  pub const fn new (interval : geometry::Interval <NonNegative <f64>>) -> Self {
22    Distance { interval }
23  }
24}
25
26impl Constraint for Distance {
27  fn constrain_velocity (&self,
28    mut object_a     : Either <&object::Static, &mut object::Dynamic>,
29    object_b         : &mut object::Dynamic,
30    pseudovelocity_a : Option <Vector3 <f64>>,
31    pseudovelocity_b : Vector3 <f64>
32  ) -> Option <Impulses> {
33    let velocity_effective_a = match &object_a {
34      Either::Left (_) => {
35        debug_assert!(pseudovelocity_a.is_none());
36        Vector3::zero()
37      }
38      Either::Right (object_a) => object_a.derivatives.velocity +
39        pseudovelocity_a.unwrap_or (Vector3::zero())
40    };
41    let velocity_effective_b = object_b.derivatives.velocity + pseudovelocity_b;
42    let position_a = match &object_a {
43      Either::Left  (object_a) => object_a.position,
44      Either::Right (object_a) => object_a.position
45    };
46    let position_b        = object_b.position;
47    let a_to_b            = position_b.0 - position_a.0;
48    let distance          = a_to_b.norm();
49    let a_to_b_normalized = a_to_b / *distance;
50    // velocity of B relative to A
51    let velocity_effective_relative = velocity_effective_b - velocity_effective_a;
52    let velocity_normal_component = velocity_effective_relative.dot (a_to_b_normalized);
53    let impulses = {
54      let (impulse_normal_a, impulse_normal_b) = if
55        *distance >= (*self.interval.max() - CONTACT_DISTANCE)
56        && velocity_normal_component > RESTING_VELOCITY * RESTING_VELOCITY
57      {
58        let dvelocity = a_to_b_normalized.dot (velocity_effective_relative);
59        match &mut object_a {
60          Either::Right (object_a) => {
61            let mass_combined    = object_a.mass + object_b.mass;
62            let mass_relative_a  =
63              object_a.mass.mass() * mass_combined.mass_reciprocal();
64            let mass_relative_b  =
65              object_b.mass.mass() * mass_combined.mass_reciprocal();
66            let impulse_normal_a = a_to_b_normalized * dvelocity * mass_relative_a;
67            let impulse_normal_b = -a_to_b_normalized * dvelocity * mass_relative_b;
68            (impulse_normal_a, impulse_normal_b)
69          }
70          Either::Left (_) => {
71            let impulse_normal = -a_to_b_normalized * dvelocity;
72            object_b.derivatives.velocity += impulse_normal;
73            (Vector3::zero(), impulse_normal)
74          }
75        }
76      } else if distance <= self.interval.min()
77        && velocity_normal_component < RESTING_VELOCITY * RESTING_VELOCITY
78      {
79        unimplemented!("TODO")
80      } else {
81        return None
82      };
83      Impulses { impulse_normal_a, impulse_normal_b, .. Impulses::default() }
84    };
85    match &mut object_a {
86      Either::Right (obj_a) => obj_a.derivatives.velocity += impulses.impulse_a(),
87      Either::Left (_)      => {}
88    }
89    object_b.derivatives.velocity += impulses.impulse_b();
90    Some (impulses)
91  }
92
93  fn constrain_position (&mut self,
94    t_reverse        : f64,
95    mut object_a     : Either <&object::Static, &mut object::Dynamic>,
96    object_b         : &mut object::Dynamic,
97    pseudovelocity_a : Option <&mut Vector3 <f64>>,
98    pseudovelocity_b : &mut Vector3 <f64>
99  ) -> Option <PostImpulses> {
100    let t_forward    = -t_reverse;
101    let mut impulses = None;
102    let position_a   = match &object_a {
103      Either::Left  (object_a) => object_a.position,
104      Either::Right (object_a) => object_a.position
105    };
106    let position_b   = object_b.position;
107    let a_to_b       = position_b.0 - position_a.0;
108    let distance     = a_to_b.norm();
109    if distance > self.interval.max() {
110      let excess = *distance - *self.interval.max();
111      let a_to_b_normalized = a_to_b / *distance;
112      let (pseudo_impulse_a, pseudo_impulse_b) = match &mut object_a {
113        Either::Left (_) => {
114          object::lerp (object_b, *pseudovelocity_b, t_reverse);
115          let pseudo_impulse_b =
116            //-((excess + 0.5 * CONTACT_DISTANCE) * a_to_b_normalized) / t_forward;
117            -(CONTACT_DISTANCE.mul_add (0.5, excess) * a_to_b_normalized) / t_forward;
118          *pseudovelocity_b += pseudo_impulse_b;
119          object::lerp (object_b, *pseudovelocity_b, t_forward);
120          (Vector3::zero(), pseudo_impulse_b)
121        }
122        Either::Right (object_a) => {
123          let pseudovelocity_a = pseudovelocity_a.unwrap();
124          object::lerp (*object_a, *pseudovelocity_a, t_reverse);
125          object::lerp (object_b, *pseudovelocity_b, t_reverse);
126          let mass_combined    = object_a.mass + object_b.mass;
127          let mass_relative_a  = object_a.mass.mass() * mass_combined.mass_reciprocal();
128          let mass_relative_b  = object_b.mass.mass() * mass_combined.mass_reciprocal();
129          //let pseudo_impulse   = ((excess + 0.5 * CONTACT_DISTANCE) * a_to_b_normalized)
130          let pseudo_impulse   =
131            (CONTACT_DISTANCE.mul_add (0.5, excess) * a_to_b_normalized) / t_forward;
132          let pseudo_impulse_a = mass_relative_b * pseudo_impulse;
133          let pseudo_impulse_b = -mass_relative_a * pseudo_impulse;
134          *pseudovelocity_a += pseudo_impulse_a;
135          *pseudovelocity_b += pseudo_impulse_b;
136          object::lerp (*object_a, *pseudovelocity_a, t_forward);
137          object::lerp (object_b, *pseudovelocity_b, t_forward);
138          (pseudo_impulse_a, pseudo_impulse_b)
139        }
140      };
141      #[expect(clippy::suboptimal_flops)]
142      if cfg!(debug_assertions) {
143        let position_a = match &object_a {
144          Either::Left  (object_a) => object_a.position,
145          Either::Right (object_a) => object_a.position
146        };
147        let position_b = object_b.position;
148        let distance   = *(position_b.0 - position_a.0).norm();
149        log::trace!(distance; "position corrected distance @ T==1.0");
150        debug_assert!(distance >= 0.0);
151        approx::assert_abs_diff_eq!(
152          distance, *self.interval.max() - 0.5 * CONTACT_DISTANCE,
153          epsilon=0.000_000_001);
154      }
155      impulses = Some (PostImpulses { pseudo_impulse_a, pseudo_impulse_b });
156    } else if distance < self.interval.min() {
157      unimplemented!("TODO")
158    }
159    impulses
160  }
161}
linear_sim/constraint/distance.rs

linear_sim/constraint/
distance.rs
