nphysics2d/joint/
mouse_constraint.rs

1use na::{DVector, RealField, Unit};
2
3use crate::joint::JointConstraint;
4use crate::math::{Point, Vector, DIM};
5use crate::object::{BodyHandle, BodyPartHandle, BodySet};
6use crate::solver::{
7    helper, BilateralConstraint, BilateralGroundConstraint, ForceDirection, ImpulseLimits,
8};
9use crate::solver::{
10    GenericNonlinearConstraint, IntegrationParameters, LinearConstraints,
11    NonlinearConstraintGenerator,
12};
13
14/// A spring-like constraint to be used to drag a body part with the mouse.
15pub struct MouseConstraint<N: RealField + Copy, Handle: BodyHandle> {
16    b1: BodyPartHandle<Handle>,
17    b2: BodyPartHandle<Handle>,
18    anchor1: Point<N>,
19    anchor2: Point<N>,
20    limit: N,
21}
22
23impl<N: RealField + Copy, Handle: BodyHandle> MouseConstraint<N, Handle> {
24    /// Initialize a mouse constraint between two bodies.getPartHandle
25    ///
26    /// Typically, `b1` will be the ground and the anchor the position of the mouse.
27    /// Both anchors are expressed in the local coordinate frames of the corresponding body parts.
28    pub fn new(
29        b1: BodyPartHandle<Handle>,
30        b2: BodyPartHandle<Handle>,
31        anchor1: Point<N>,
32        anchor2: Point<N>,
33        limit: N,
34    ) -> Self {
35        MouseConstraint {
36            b1,
37            b2,
38            anchor1,
39            anchor2,
40            limit,
41        }
42    }
43
44    /// Change the first anchor, expressed in the local space of the first body part.
45    pub fn set_anchor_1(&mut self, anchor1: Point<N>) {
46        self.anchor1 = anchor1;
47    }
48
49    /// Change the first anchor, expressed in the local space of the second body part.
50    pub fn set_anchor_2(&mut self, anchor2: Point<N>) {
51        self.anchor2 = anchor2;
52    }
53}
54
55impl<N: RealField + Copy, Handle: BodyHandle> JointConstraint<N, Handle> for MouseConstraint<N, Handle> {
56    fn num_velocity_constraints(&self) -> usize {
57        DIM
58    }
59
60    fn anchors(&self) -> (BodyPartHandle<Handle>, BodyPartHandle<Handle>) {
61        (self.b1, self.b2)
62    }
63
64    fn velocity_constraints(
65        &mut self,
66        parameters: &IntegrationParameters<N>,
67        bodies: &dyn BodySet<N, Handle = Handle>,
68        ext_vels: &DVector<N>,
69        ground_j_id: &mut usize,
70        j_id: &mut usize,
71        jacobians: &mut [N],
72        constraints: &mut LinearConstraints<N, usize>,
73    ) {
74        let body1 = try_ret!(bodies.get(self.b1.0));
75        let body2 = try_ret!(bodies.get(self.b2.0));
76        let part1 = try_ret!(body1.part(self.b1.1));
77        let part2 = try_ret!(body2.part(self.b2.1));
78
79        /*
80         *
81         * Joint constraints.
82         *
83         */
84        let anchor1 = body1.world_point_at_material_point(part1, &self.anchor1);
85        let anchor2 = body2.world_point_at_material_point(part2, &self.anchor2);
86
87        let assembly_id1 = body1.companion_id();
88        let assembly_id2 = body2.companion_id();
89
90        let limits = ImpulseLimits::Independent {
91            min: -self.limit,
92            max: self.limit,
93        };
94
95        let error = anchor2 - anchor1;
96        let (ext_vels1, ext_vels2) =
97            helper::split_ext_vels(body1, body2, assembly_id1, assembly_id2, ext_vels);
98
99        #[cfg(feature = "dim2")]
100        let canonical_basis = [Vector::x(), Vector::y()];
101        #[cfg(feature = "dim3")]
102        let canonical_basis = [Vector::x(), Vector::y(), Vector::z()];
103
104        for dir in &canonical_basis {
105            let fdir = ForceDirection::Linear(Unit::new_unchecked(*dir));
106            let mut rhs = -error.dot(&*dir) * parameters.erp * parameters.inv_dt();
107            let geom = helper::constraint_pair_geometry(
108                body1,
109                part1,
110                self.b1,
111                body2,
112                part2,
113                self.b2,
114                &anchor1,
115                &anchor2,
116                &fdir,
117                ground_j_id,
118                j_id,
119                jacobians,
120                Some(&ext_vels1),
121                Some(&ext_vels2),
122                Some(&mut rhs),
123            );
124
125            if geom.ndofs1 == 0 || geom.ndofs2 == 0 {
126                constraints
127                    .bilateral_ground
128                    .push(BilateralGroundConstraint::new(
129                        geom,
130                        assembly_id1,
131                        assembly_id2,
132                        limits,
133                        rhs,
134                        N::zero(),
135                        0,
136                    ));
137            } else {
138                constraints.bilateral.push(BilateralConstraint::new(
139                    geom,
140                    assembly_id1,
141                    assembly_id2,
142                    limits,
143                    rhs,
144                    N::zero(),
145                    0,
146                ));
147            }
148        }
149    }
150
151    fn cache_impulses(&mut self, _: &LinearConstraints<N, usize>, _: N) {}
152}
153
154impl<N: RealField + Copy, Handle: BodyHandle> NonlinearConstraintGenerator<N, Handle>
155    for MouseConstraint<N, Handle>
156{
157    fn num_position_constraints(&self, _: &dyn BodySet<N, Handle = Handle>) -> usize {
158        0
159    }
160
161    fn position_constraint(
162        &self,
163        _: &IntegrationParameters<N>,
164        _: usize,
165        _: &mut dyn BodySet<N, Handle = Handle>,
166        _: &mut [N],
167    ) -> Option<GenericNonlinearConstraint<N, Handle>> {
168        None
169    }
170}
nphysics2d/joint/mouse_constraint.rs

nphysics2d/joint/
mouse_constraint.rs
