picea/constraints/
point.rs

1use picea_macro_tools::Fields;
2
3use crate::{
4    element::ID,
5    math::{point::Point, vector::Vector, FloatNum},
6    scene::context::ConstraintParameters,
7};
8
9use super::{compute_soft_constraints_params, ConstraintObject, JoinConstraintConfig};
10
11#[derive(Fields)]
12pub struct PointConstraint<Obj: ConstraintObject> {
13    #[r]
14    id: u32,
15    #[r]
16    obj_id: ID,
17    #[r]
18    #[w]
19    fixed_point: Point,
20    #[r]
21    move_point: Point, // bind with element
22    #[r]
23    total_lambda: FloatNum,
24    // force_soft_factor: FloatNum,
25    // position_fix_factor: FloatNum,
26    // distance must large than zero
27    position_bias: FloatNum,
28    soft_part: FloatNum,
29    mass_effective: FloatNum,
30    obj: *mut Obj,
31    #[r]
32    #[w]
33    config: JoinConstraintConfig,
34}
35
36impl<Obj: ConstraintObject> PointConstraint<Obj> {
37    pub fn new(
38        id: u32,
39        obj_id: ID,
40        fixed_point: Point,
41        move_point: Point,
42        config: JoinConstraintConfig,
43    ) -> Self {
44        assert!(
45            config.distance >= 0.,
46            "distance must large than or equal to zero"
47        );
48
49        Self {
50            id,
51            obj_id,
52            fixed_point,
53            move_point,
54            total_lambda: 0.,
55            position_bias: 0.,
56            soft_part: 0.,
57            mass_effective: 0.,
58            obj: std::ptr::null_mut(),
59            config,
60        }
61    }
62
63    pub fn stretch_length(&self) -> Vector {
64        (self.move_point, self.fixed_point).into()
65    }
66
67    pub(crate) unsafe fn reset_params(
68        &mut self,
69        move_point: Point,
70        obj: *mut Obj,
71        delta_time: FloatNum,
72    ) {
73        self.move_point = move_point;
74        self.total_lambda = 0.;
75
76        let meta = (*obj).meta();
77        let mass = meta.mass();
78        let inv_mass = meta.inv_mass();
79        let inv_moment_of_inertia = meta.inv_moment_of_inertia();
80
81        let (force_soft_factor, position_fix_factor) = if self.config.hard {
82            (0., 1.)
83        } else {
84            compute_soft_constraints_params(
85                mass,
86                self.config.damping_ratio,
87                self.config.frequency,
88                delta_time,
89            )
90        };
91
92        let strength_length = self.stretch_length();
93        let n = -strength_length.normalize();
94
95        let position_bias = position_fix_factor
96            * (strength_length.abs() - self.config.distance)
97            * delta_time.recip();
98
99        let soft_part = force_soft_factor * delta_time.recip();
100
101        let r_t: Vector = ((*obj).center_point(), self.move_point).into();
102
103        let mass_effective = inv_mass + (r_t ^ n).powf(2.) * inv_moment_of_inertia;
104
105        // self.force_soft_factor = force_soft_factor;
106        // self.position_fix_factor = position_fix_factor;
107        self.position_bias = position_bias;
108        self.soft_part = soft_part;
109        self.mass_effective = mass_effective;
110        self.obj = obj;
111    }
112
113    pub(crate) unsafe fn solve(&mut self, parameters: &ConstraintParameters) {
114        let strength_length = self.stretch_length();
115        if strength_length.abs() < parameters.max_allow_permeate {
116            // no constraint if there is no need
117            return;
118        }
119
120        let obj = &mut *self.obj;
121
122        let &mut Self {
123            position_bias,
124            mass_effective,
125            soft_part,
126            ..
127        } = self;
128
129        let r_t: Vector = (obj.center_point(), self.move_point).into();
130
131        let n = -strength_length.normalize();
132
133        let v: Vector = obj.compute_point_velocity(&self.move_point);
134
135        let jv_b: f32 = -(v * n + position_bias);
136
137        let lambda = jv_b * (soft_part + mass_effective).recip();
138
139        let impulse = n * lambda;
140
141        // TODO restrict here
142
143        obj.meta_mut().apply_impulse(impulse, r_t);
144    }
145}
picea/constraints/point.rs

picea/constraints/
point.rs
