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
#![allow(missing_docs)]
use downcast_rs::Downcast;
use na::{DVector, Real};
use crate::object::{BodyPartHandle, BodySet};
use crate::solver::{ConstraintSet, IntegrationParameters, NonlinearConstraintGenerator};
pub type ConstraintHandle = usize;
pub trait JointConstraint<N: Real>: NonlinearConstraintGenerator<N> + Downcast + Send + Sync {
fn is_active(&self, bodies: &BodySet<N>) -> bool {
let (b1, b2) = self.anchors();
let body1 = try_ret!(bodies.body(b1.0), false);
let body2 = try_ret!(bodies.body(b2.0), false);
let ndofs1 = body1.status_dependent_ndofs();
let ndofs2 = body2.status_dependent_ndofs();
(ndofs1 != 0 && body1.is_active()) || (ndofs2 != 0 && body2.is_active())
}
fn num_velocity_constraints(&self) -> usize;
fn anchors(&self) -> (BodyPartHandle, BodyPartHandle);
fn velocity_constraints(
&mut self,
params: &IntegrationParameters<N>,
bodies: &BodySet<N>,
ext_vels: &DVector<N>,
ground_j_id: &mut usize,
j_id: &mut usize,
jacobians: &mut [N],
velocity_constraints: &mut ConstraintSet<N>,
);
fn cache_impulses(&mut self, constraints: &ConstraintSet<N>);
}
impl_downcast!(JointConstraint<N> where N: Real);