use crate::dynamics::ccd::RigidBodyMotion;
use crate::dynamics::RigidBody;
use crate::math::{Isometry, Real};
use parry::motion::{ConstantLinearVelocityRigidMotion, RigidMotion};
#[derive(Copy, Clone, Debug)]
pub struct CCDData {
pub position0: Isometry<Real>,
pub time: Real,
}
impl CCDData {
pub fn new(body: &RigidBody, dt: Real) -> Self {
Self {
position0: body.position_before_integration,
time: 0.0,
}
}
pub fn advance(&mut self, dt: Real, body: &RigidBody, time_ratio: Real) {
let motion = self.motion(dt, body, 0.0);
self.position0 = motion.position_at_time(time_ratio);
}
pub fn validate_advancement(&mut self, body: &RigidBody) {
self.position0 = body.position
}
pub fn clamp_advancement(&self, body: &mut RigidBody) {
body.position.translation = self.position0.translation;
}
pub fn motion(&self, dt: Real, body: &RigidBody, time_origin: Real) -> RigidBodyMotion {
if body.is_ccd_enabled(dt) {
let p0 = Isometry::from_parts(self.position0.translation, body.position.rotation);
let motion = ConstantLinearVelocityRigidMotion::new(time_origin, p0, body.linvel);
RigidBodyMotion::RigidLinear(motion)
} else {
RigidBodyMotion::Static(body.position)
}
}
pub fn safe_position(&self, body: &RigidBody) -> Isometry<Real> {
Isometry::from_parts(self.position0.translation, body.position.rotation)
}
}