nphysics2d/joint/
unit_constraint.rs

1use na::{DVector, RealField, Unit};
2
3use crate::math::{Point, Vector};
4use crate::object::{Body, BodyHandle, BodyPart, BodyPartHandle};
5use crate::solver::{
6    helper, BilateralConstraint, BilateralGroundConstraint, ForceDirection,
7    GenericNonlinearConstraint, ImpulseLimits, IntegrationParameters, LinearConstraints,
8};
9
10pub fn build_linear_limits_velocity_constraint<N: RealField + Copy, B: ?Sized + Body<N>, H: BodyHandle>(
11    body1: &B,
12    part1: &dyn BodyPart<N>,
13    handle1: BodyPartHandle<H>,
14    body2: &B,
15    part2: &dyn BodyPart<N>,
16    handle2: BodyPartHandle<H>,
17    assembly_id1: usize,
18    assembly_id2: usize,
19    anchor1: &Point<N>,
20    anchor2: &Point<N>,
21    axis: &Unit<Vector<N>>,
22    min: Option<N>,
23    max: Option<N>,
24    ext_vels: &DVector<N>,
25    impulse: N,
26    impulse_id: usize,
27    ground_j_id: &mut usize,
28    j_id: &mut usize,
29    jacobians: &mut [N],
30    constraints: &mut LinearConstraints<N, usize>,
31) {
32    let offset = axis.dot(&(anchor2 - anchor1));
33
34    let (unilateral, dir) = match (min, max) {
35        (None, None) => {
36            return;
37        }
38        (Some(min), Some(max)) => {
39            if relative_eq!(min, max) {
40                (false, *axis)
41            } else {
42                if offset <= min {
43                    (true, -*axis)
44                } else if offset >= max {
45                    (true, *axis)
46                } else {
47                    return;
48                }
49            }
50        }
51        (Some(min), None) => {
52            if offset <= min {
53                (true, -*axis)
54            } else {
55                return;
56            }
57        }
58        (None, Some(max)) => {
59            if offset >= max {
60                (true, *axis)
61            } else {
62                return;
63            }
64        }
65    };
66
67    let (ext_vels1, ext_vels2) =
68        helper::split_ext_vels(body1, body2, assembly_id1, assembly_id2, ext_vels);
69    let force = ForceDirection::Linear(dir);
70    let mut rhs = N::zero();
71    let geom = helper::constraint_pair_geometry(
72        body1,
73        part1,
74        handle1,
75        body2,
76        part2,
77        handle2,
78        anchor1,
79        anchor2,
80        &force,
81        ground_j_id,
82        j_id,
83        jacobians,
84        Some(&ext_vels1),
85        Some(&ext_vels2),
86        Some(&mut rhs),
87    );
88
89    // FIXME: generate unilateral constraints for unilateral limits.
90    let limits = if unilateral {
91        ImpulseLimits::Independent {
92            min: N::zero(),
93            max: N::max_value(),
94        }
95    } else {
96        ImpulseLimits::Independent {
97            min: -N::max_value(),
98            max: N::max_value(),
99        }
100    };
101
102    if geom.ndofs1 == 0 || geom.ndofs2 == 0 {
103        constraints
104            .bilateral_ground
105            .push(BilateralGroundConstraint::new(
106                geom,
107                assembly_id1,
108                assembly_id2,
109                limits,
110                rhs,
111                impulse,
112                impulse_id,
113            ));
114    } else {
115        constraints.bilateral.push(BilateralConstraint::new(
116            geom,
117            assembly_id1,
118            assembly_id2,
119            limits,
120            rhs,
121            impulse,
122            impulse_id,
123        ));
124    }
125}
126
127pub fn build_linear_limits_position_constraint<N: RealField + Copy, B: ?Sized + Body<N>, H: BodyHandle>(
128    parameters: &IntegrationParameters<N>,
129    body1: &B,
130    part1: &dyn BodyPart<N>,
131    handle1: BodyPartHandle<H>,
132    body2: &B,
133    part2: &dyn BodyPart<N>,
134    handle2: BodyPartHandle<H>,
135    anchor1: &Point<N>,
136    anchor2: &Point<N>,
137    axis: &Unit<Vector<N>>,
138    min: Option<N>,
139    max: Option<N>,
140    jacobians: &mut [N],
141) -> Option<GenericNonlinearConstraint<N, H>> {
142    let offset = axis.dot(&(anchor2 - anchor1));
143    let mut error = N::zero();
144    let mut dir = *axis;
145
146    if let Some(min) = min {
147        error = min - offset;
148        dir = -*axis;
149    }
150
151    if error < N::zero() {
152        if let Some(max) = max {
153            error = offset - max;
154            dir = *axis;
155        }
156    }
157
158    if error > parameters.allowed_linear_error {
159        let mut j_id = 0;
160        let mut ground_j_id = 0;
161
162        let geom = helper::constraint_pair_geometry(
163            body1,
164            part1,
165            handle1,
166            body2,
167            part2,
168            handle2,
169            anchor1,
170            anchor2,
171            &ForceDirection::Linear(dir),
172            &mut ground_j_id,
173            &mut j_id,
174            jacobians,
175            None,
176            None,
177            None,
178        );
179
180        let rhs = -error;
181        let constraint = GenericNonlinearConstraint::new(
182            handle1,
183            Some(handle2),
184            false,
185            geom.ndofs1,
186            geom.ndofs2,
187            geom.wj_id1,
188            geom.wj_id2,
189            rhs,
190            geom.r,
191        );
192
193        Some(constraint)
194    } else {
195        None
196    }
197}
198
199/*
200pub fn build_angular_limit_velocity_constraint<N: RealField + Copy>(
201    parameters: &IntegrationParameters<N>,
202    body1: &BodyPart<N>,
203    body2: &BodyPart<N>,
204    pos1: &Isometry<N>,
205    pos2: &Isometry<N>,
206    assembly_id1: usize,
207    assembly_id2: usize,
208    anchor1: &Point<N>,
209    anchor2: &Point<N>,
210    axis: &Unit<Vector<N>>,
211    min: Option<N>,
212    max: Option<N>,
213    ext_vels: &DVector<N>,
214    impulse: N,
215    impulse_id: usize,
216    ground_j_id: &mut usize,
217    j_id: &mut usize,
218    jacobians: &mut [N],
219    constraints: &mut ConstraintSet<N, usize>,
220) {
221}
222*/
nphysics2d/joint/unit_constraint.rs

nphysics2d/joint/
unit_constraint.rs
