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

use na::{RealField, Unit};

use crate::force_generator::ForceGenerator;
use crate::math::{ForceType, Point, Vector};
use crate::object::{BodyHandle, BodyPartHandle, BodySet};
use crate::solver::IntegrationParameters;

/// Generator of a force proportional to the distance separating two bodies.
pub struct Spring<N: RealField, Handle: BodyHandle> {
    b1: BodyPartHandle<Handle>,
    b2: BodyPartHandle<Handle>,
    anchor1: Point<N>,
    anchor2: Point<N>,
    length: N,
    stiffness: N,
}

impl<N: RealField, Handle: BodyHandle> Spring<N, Handle> {
    /// Initialize a spring attached to `b1` and `b2` at the points `anchor1` and `anchor2`.
    ///
    /// Anchors are expressed in the local coordinates of the corresponding bodies.
    /// The spring has a rest length of `length` and a stiffness of `stiffness`.
    pub fn new(
        b1: BodyPartHandle<Handle>,
        b2: BodyPartHandle<Handle>,
        anchor1: Point<N>,
        anchor2: Point<N>,
        length: N,
        stiffness: N,
    ) -> Self {
        Spring {
            b1,
            b2,
            anchor1,
            anchor2,
            length,
            stiffness,
        }
    }

    /// Sets the attach point to the first body.
    ///
    /// The anchor is expressed in the local coordinatse of the first body.
    pub fn set_anchor_1(&mut self, anchor: Point<N>) {
        self.anchor1 = anchor;
    }

    /// Sets the attach point to the second body.
    ///
    /// The anchor is expressed in the local coordinatse of the second body.
    pub fn set_anchor_2(&mut self, anchor: Point<N>) {
        self.anchor2 = anchor
    }
}

impl<N: RealField, Handle: BodyHandle> ForceGenerator<N, Handle> for Spring<N, Handle> {
    fn apply(
        &mut self,
        _: &IntegrationParameters<N>,
        bodies: &mut dyn BodySet<N, Handle = Handle>,
    ) {
        let body1 = try_ret!(bodies.get(self.b1.0));
        let body2 = try_ret!(bodies.get(self.b2.0));
        let part1 = try_ret!(body1.part(self.b1.1));
        let part2 = try_ret!(body2.part(self.b2.1));

        let anchor1 = body1.world_point_at_material_point(part1, &self.anchor1);
        let anchor2 = body2.world_point_at_material_point(part2, &self.anchor2);

        let force_dir;
        let delta_length;

        if let Some((dir, length)) = Unit::try_new_and_get(anchor2 - anchor1, N::default_epsilon())
        {
            force_dir = dir;
            delta_length = length - self.length;
        } else {
            force_dir = Vector::y_axis();
            delta_length = -self.length;
        }

        let force = force_dir.as_ref() * delta_length * self.stiffness;
        bodies
            .get_mut(self.b1.0)
            .unwrap()
            .apply_force_at_local_point(self.b1.1, &force, &self.anchor1, ForceType::Force, false);
        bodies
            .get_mut(self.b2.0)
            .unwrap()
            .apply_force_at_local_point(self.b2.1, &-force, &self.anchor2, ForceType::Force, false);
    }
}