linear-sim 0.10.3

Minimal linear 3D simulation library
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161

use either::Either;

use crate::object;
use crate::math::*;

use super::{Constraint, Impulses, PostImpulses};
use super::contact::{CONTACT_DISTANCE, RESTING_VELOCITY};

#[cfg(feature = "derive_serdes")]
use serde::{Deserialize, Serialize};

/// A distance constraint
#[cfg_attr(feature = "derive_serdes", derive(Deserialize, Serialize))]
#[derive(Clone, Debug, PartialEq)]
pub struct Distance {
  pub interval : geometry::Interval <NonNegative <f64>>
}

impl Distance {
  #[inline]
  pub const fn new (interval : geometry::Interval <NonNegative <f64>>) -> Self {
    Distance { interval }
  }
}

impl Constraint for Distance {
  fn constrain_velocity (&self,
    mut object_a     : Either <&object::Static, &mut object::Dynamic>,
    object_b         : &mut object::Dynamic,
    pseudovelocity_a : Option <Vector3 <f64>>,
    pseudovelocity_b : Vector3 <f64>
  ) -> Option <Impulses> {
    let velocity_effective_a = match &object_a {
      Either::Left (_) => {
        debug_assert!(pseudovelocity_a.is_none());
        Vector3::zero()
      }
      Either::Right (object_a) => object_a.derivatives.velocity +
        pseudovelocity_a.unwrap_or (Vector3::zero())
    };
    let velocity_effective_b = object_b.derivatives.velocity + pseudovelocity_b;
    let position_a = match &object_a {
      Either::Left  (object_a) => object_a.position,
      Either::Right (object_a) => object_a.position
    };
    let position_b        = object_b.position;
    let a_to_b            = position_b.0 - position_a.0;
    let distance          = a_to_b.norm();
    let a_to_b_normalized = a_to_b / *distance;
    // velocity of B relative to A
    let velocity_effective_relative = velocity_effective_b - velocity_effective_a;
    let velocity_normal_component = velocity_effective_relative.dot (a_to_b_normalized);
    let impulses = {
      let (impulse_normal_a, impulse_normal_b) = if
        *distance >= (*self.interval.max() - CONTACT_DISTANCE)
        && velocity_normal_component > RESTING_VELOCITY * RESTING_VELOCITY
      {
        let dvelocity = a_to_b_normalized.dot (velocity_effective_relative);
        match &mut object_a {
          Either::Right (object_a) => {
            let mass_combined    = object_a.mass + object_b.mass;
            let mass_relative_a  =
              object_a.mass.mass() * mass_combined.mass_reciprocal();
            let mass_relative_b  =
              object_b.mass.mass() * mass_combined.mass_reciprocal();
            let impulse_normal_a = a_to_b_normalized * dvelocity * mass_relative_a;
            let impulse_normal_b = -a_to_b_normalized * dvelocity * mass_relative_b;
            (impulse_normal_a, impulse_normal_b)
          }
          Either::Left (_) => {
            let impulse_normal = -a_to_b_normalized * dvelocity;
            object_b.derivatives.velocity += impulse_normal;
            (Vector3::zero(), impulse_normal)
          }
        }
      } else if distance <= self.interval.min()
        && velocity_normal_component < RESTING_VELOCITY * RESTING_VELOCITY
      {
        unimplemented!("TODO")
      } else {
        return None
      };
      Impulses { impulse_normal_a, impulse_normal_b, .. Impulses::default() }
    };
    match &mut object_a {
      Either::Right (obj_a) => obj_a.derivatives.velocity += impulses.impulse_a(),
      Either::Left (_)      => {}
    }
    object_b.derivatives.velocity += impulses.impulse_b();
    Some (impulses)
  }

  fn constrain_position (&mut self,
    t_reverse        : f64,
    mut object_a     : Either <&object::Static, &mut object::Dynamic>,
    object_b         : &mut object::Dynamic,
    pseudovelocity_a : Option <&mut Vector3 <f64>>,
    pseudovelocity_b : &mut Vector3 <f64>
  ) -> Option <PostImpulses> {
    let t_forward    = -t_reverse;
    let mut impulses = None;
    let position_a   = match &object_a {
      Either::Left  (object_a) => object_a.position,
      Either::Right (object_a) => object_a.position
    };
    let position_b   = object_b.position;
    let a_to_b       = position_b.0 - position_a.0;
    let distance     = a_to_b.norm();
    if distance > self.interval.max() {
      let excess = *distance - *self.interval.max();
      let a_to_b_normalized = a_to_b / *distance;
      let (pseudo_impulse_a, pseudo_impulse_b) = match &mut object_a {
        Either::Left (_) => {
          object::lerp (object_b, *pseudovelocity_b, t_reverse);
          let pseudo_impulse_b =
            //-((excess + 0.5 * CONTACT_DISTANCE) * a_to_b_normalized) / t_forward;
            -(CONTACT_DISTANCE.mul_add (0.5, excess) * a_to_b_normalized) / t_forward;
          *pseudovelocity_b += pseudo_impulse_b;
          object::lerp (object_b, *pseudovelocity_b, t_forward);
          (Vector3::zero(), pseudo_impulse_b)
        }
        Either::Right (object_a) => {
          let pseudovelocity_a = pseudovelocity_a.unwrap();
          object::lerp (*object_a, *pseudovelocity_a, t_reverse);
          object::lerp (object_b, *pseudovelocity_b, t_reverse);
          let mass_combined    = object_a.mass + object_b.mass;
          let mass_relative_a  = object_a.mass.mass() * mass_combined.mass_reciprocal();
          let mass_relative_b  = object_b.mass.mass() * mass_combined.mass_reciprocal();
          //let pseudo_impulse   = ((excess + 0.5 * CONTACT_DISTANCE) * a_to_b_normalized)
          let pseudo_impulse   =
            (CONTACT_DISTANCE.mul_add (0.5, excess) * a_to_b_normalized) / t_forward;
          let pseudo_impulse_a = mass_relative_b * pseudo_impulse;
          let pseudo_impulse_b = -mass_relative_a * pseudo_impulse;
          *pseudovelocity_a += pseudo_impulse_a;
          *pseudovelocity_b += pseudo_impulse_b;
          object::lerp (*object_a, *pseudovelocity_a, t_forward);
          object::lerp (object_b, *pseudovelocity_b, t_forward);
          (pseudo_impulse_a, pseudo_impulse_b)
        }
      };
      #[expect(clippy::suboptimal_flops)]
      if cfg!(debug_assertions) {
        let position_a = match &object_a {
          Either::Left  (object_a) => object_a.position,
          Either::Right (object_a) => object_a.position
        };
        let position_b = object_b.position;
        let distance   = *(position_b.0 - position_a.0).norm();
        log::trace!(distance; "position corrected distance @ T==1.0");
        debug_assert!(distance >= 0.0);
        approx::assert_abs_diff_eq!(
          distance, *self.interval.max() - 0.5 * CONTACT_DISTANCE,
          epsilon=0.000_000_001);
      }
      impulses = Some (PostImpulses { pseudo_impulse_a, pseudo_impulse_b });
    } else if distance < self.interval.min() {
      unimplemented!("TODO")
    }
    impulses
  }
}