#![allow(missing_docs)]
use downcast_rs::Downcast;
use generational_arena::Arena;
use na::{DVector, RealField};
use crate::object::{BodyHandle, BodyPartHandle, BodySet, DefaultBodyHandle};
use crate::solver::{IntegrationParameters, LinearConstraints, NonlinearConstraintGenerator};
pub trait JointConstraintSet<N: RealField, Handle: BodyHandle> {
type JointConstraint: ?Sized + JointConstraint<N, Handle>;
type Handle: Copy;
fn get(&self, handle: Self::Handle) -> Option<&Self::JointConstraint>;
fn get_mut(&mut self, handle: Self::Handle) -> Option<&mut Self::JointConstraint>;
fn contains(&self, handle: Self::Handle) -> bool;
fn foreach(&self, f: impl FnMut(Self::Handle, &Self::JointConstraint));
fn foreach_mut(&mut self, f: impl FnMut(Self::Handle, &mut Self::JointConstraint));
fn pop_insertion_event(
&mut self,
) -> Option<(Self::Handle, BodyPartHandle<Handle>, BodyPartHandle<Handle>)>;
fn pop_removal_event(
&mut self,
) -> Option<(Self::Handle, BodyPartHandle<Handle>, BodyPartHandle<Handle>)>;
fn remove(&mut self, to_remove: Self::Handle);
}
pub struct DefaultJointConstraintSet<N: RealField, Handle: BodyHandle = DefaultBodyHandle> {
constraints: Arena<Box<dyn JointConstraint<N, Handle>>>,
inserted: Vec<(
DefaultJointConstraintHandle,
BodyPartHandle<Handle>,
BodyPartHandle<Handle>,
)>,
removed: Vec<(
DefaultJointConstraintHandle,
BodyPartHandle<Handle>,
BodyPartHandle<Handle>,
)>,
}
impl<N: RealField, Handle: BodyHandle> DefaultJointConstraintSet<N, Handle> {
pub fn new() -> Self {
DefaultJointConstraintSet {
constraints: Arena::new(),
inserted: Vec::new(),
removed: Vec::new(),
}
}
pub fn insert(
&mut self,
constraint: impl JointConstraint<N, Handle>,
) -> DefaultJointConstraintHandle {
self.insert_boxed(Box::new(constraint))
}
pub fn insert_boxed(
&mut self,
constraint: Box<dyn JointConstraint<N, Handle>>,
) -> DefaultJointConstraintHandle {
let (part1, part2) = constraint.anchors();
let handle = self.constraints.insert(constraint);
self.inserted.push((handle, part1, part2));
handle
}
pub fn remove(
&mut self,
to_remove: DefaultJointConstraintHandle,
) -> Option<Box<dyn JointConstraint<N, Handle>>> {
let res = self.constraints.remove(to_remove)?;
let (part1, part2) = res.anchors();
self.removed.push((to_remove, part1, part2));
Some(res)
}
pub fn contains(&self, handle: DefaultJointConstraintHandle) -> bool {
self.constraints.contains(handle)
}
pub fn get(
&self,
handle: DefaultJointConstraintHandle,
) -> Option<&dyn JointConstraint<N, Handle>> {
self.constraints.get(handle).map(|b| &**b)
}
pub fn get_mut(
&mut self,
handle: DefaultJointConstraintHandle,
) -> Option<&mut dyn JointConstraint<N, Handle>> {
self.constraints.get_mut(handle).map(|b| &mut **b)
}
pub fn iter(
&self,
) -> impl Iterator<
Item = (
DefaultJointConstraintHandle,
&dyn JointConstraint<N, Handle>,
),
> {
self.constraints.iter().map(|b| (b.0, &**b.1))
}
pub fn iter_mut(
&mut self,
) -> impl Iterator<
Item = (
DefaultJointConstraintHandle,
&mut dyn JointConstraint<N, Handle>,
),
> {
self.constraints.iter_mut().map(|b| (b.0, &mut **b.1))
}
}
impl<N: RealField, Handle: BodyHandle> JointConstraintSet<N, Handle>
for DefaultJointConstraintSet<N, Handle>
{
type JointConstraint = dyn JointConstraint<N, Handle>;
type Handle = DefaultJointConstraintHandle;
fn get(&self, handle: Self::Handle) -> Option<&Self::JointConstraint> {
self.get(handle)
}
fn get_mut(&mut self, handle: Self::Handle) -> Option<&mut Self::JointConstraint> {
self.get_mut(handle)
}
fn contains(&self, handle: Self::Handle) -> bool {
self.contains(handle)
}
fn foreach(&self, mut f: impl FnMut(Self::Handle, &Self::JointConstraint)) {
for (h, b) in self.iter() {
f(h, b)
}
}
fn foreach_mut(&mut self, mut f: impl FnMut(Self::Handle, &mut Self::JointConstraint)) {
for (h, b) in self.iter_mut() {
f(h, b)
}
}
fn pop_insertion_event(
&mut self,
) -> Option<(Self::Handle, BodyPartHandle<Handle>, BodyPartHandle<Handle>)> {
self.inserted.pop()
}
fn pop_removal_event(
&mut self,
) -> Option<(Self::Handle, BodyPartHandle<Handle>, BodyPartHandle<Handle>)> {
self.removed.pop()
}
fn remove(&mut self, to_remove: Self::Handle) {
let _ = self.remove(to_remove);
}
}
pub type DefaultJointConstraintHandle = generational_arena::Index;
pub trait JointConstraint<N: RealField, Handle: BodyHandle>:
NonlinearConstraintGenerator<N, Handle> + Downcast + Send + Sync
{
fn is_active(&self, bodies: &dyn BodySet<N, Handle = Handle>) -> bool {
let (b1, b2) = self.anchors();
let body1 = try_ret!(bodies.get(b1.0), false);
let body2 = try_ret!(bodies.get(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<Handle>, BodyPartHandle<Handle>);
fn velocity_constraints(
&mut self,
parameters: &IntegrationParameters<N>,
bodies: &dyn BodySet<N, Handle = Handle>,
ext_vels: &DVector<N>,
ground_j_id: &mut usize,
j_id: &mut usize,
jacobians: &mut [N],
velocity_constraints: &mut LinearConstraints<N, usize>,
);
fn cache_impulses(&mut self, constraints: &LinearConstraints<N, usize>, inv_dt: N);
fn is_broken(&self) -> bool {
false
}
}
impl_downcast!(JointConstraint<N, Handle> where N: RealField, Handle: BodyHandle);