use crate::{dynamics::solver::xpbd::*, prelude::*};
use bevy::{
ecs::{
entity::{EntityMapper, MapEntities},
reflect::ReflectMapEntities,
},
prelude::*,
};
#[derive(Component, Clone, Copy, Debug, PartialEq, Reflect)]
#[cfg_attr(feature = "serialize", derive(serde::Serialize, serde::Deserialize))]
#[cfg_attr(feature = "serialize", reflect(Serialize, Deserialize))]
#[reflect(Debug, Component, MapEntities, PartialEq)]
pub struct PrismaticJoint {
pub entity1: Entity,
pub entity2: Entity,
pub local_anchor1: Vector,
pub local_anchor2: Vector,
pub free_axis: Vector,
pub free_axis_limits: Option<DistanceLimit>,
pub damping_linear: Scalar,
pub damping_angular: Scalar,
pub position_lagrange: Scalar,
pub align_lagrange: Scalar,
pub compliance: Scalar,
pub force: Vector,
pub align_torque: Torque,
}
impl XpbdConstraint<2> for PrismaticJoint {
fn entities(&self) -> [Entity; 2] {
[self.entity1, self.entity2]
}
fn clear_lagrange_multipliers(&mut self) {
self.position_lagrange = 0.0;
self.align_lagrange = 0.0;
}
fn solve(&mut self, bodies: [&mut RigidBodyQueryItem; 2], dt: Scalar) {
let [body1, body2] = bodies;
let compliance = self.compliance;
let difference = self.get_rotation_difference(&body1.rotation, &body2.rotation);
let mut lagrange = self.align_lagrange;
self.align_torque =
self.align_orientation(body1, body2, difference, &mut lagrange, compliance, dt);
self.align_lagrange = lagrange;
self.force = self.constrain_positions(body1, body2, dt);
}
}
impl Joint for PrismaticJoint {
fn new(entity1: Entity, entity2: Entity) -> Self {
Self {
entity1,
entity2,
local_anchor1: Vector::ZERO,
local_anchor2: Vector::ZERO,
free_axis: Vector::X,
free_axis_limits: None,
damping_linear: 1.0,
damping_angular: 1.0,
position_lagrange: 0.0,
align_lagrange: 0.0,
compliance: 0.0,
force: Vector::ZERO,
#[cfg(feature = "2d")]
align_torque: 0.0,
#[cfg(feature = "3d")]
align_torque: Vector::ZERO,
}
}
fn with_compliance(self, compliance: Scalar) -> Self {
Self { compliance, ..self }
}
fn with_local_anchor_1(self, anchor: Vector) -> Self {
Self {
local_anchor1: anchor,
..self
}
}
fn with_local_anchor_2(self, anchor: Vector) -> Self {
Self {
local_anchor2: anchor,
..self
}
}
fn with_linear_velocity_damping(self, damping: Scalar) -> Self {
Self {
damping_linear: damping,
..self
}
}
fn with_angular_velocity_damping(self, damping: Scalar) -> Self {
Self {
damping_angular: damping,
..self
}
}
fn local_anchor_1(&self) -> Vector {
self.local_anchor1
}
fn local_anchor_2(&self) -> Vector {
self.local_anchor2
}
fn damping_linear(&self) -> Scalar {
self.damping_linear
}
fn damping_angular(&self) -> Scalar {
self.damping_angular
}
}
impl PrismaticJoint {
fn constrain_positions(
&mut self,
body1: &mut RigidBodyQueryItem,
body2: &mut RigidBodyQueryItem,
dt: Scalar,
) -> Vector {
let world_r1 = *body1.rotation * self.local_anchor1;
let world_r2 = *body2.rotation * self.local_anchor2;
let mut delta_x = Vector::ZERO;
let axis1 = *body1.rotation * self.free_axis;
if let Some(limits) = self.free_axis_limits {
delta_x += limits.compute_correction_along_axis(
body1.current_position() + world_r1,
body2.current_position() + world_r2,
axis1,
);
}
let zero_distance_limit = DistanceLimit::ZERO;
#[cfg(feature = "2d")]
{
let axis2 = Vector::new(axis1.y, -axis1.x);
delta_x += zero_distance_limit.compute_correction_along_axis(
body1.current_position() + world_r1,
body2.current_position() + world_r2,
axis2,
);
}
#[cfg(feature = "3d")]
{
let axis2 = axis1.any_orthogonal_vector();
let axis3 = axis1.cross(axis2);
delta_x += zero_distance_limit.compute_correction_along_axis(
body1.current_position() + world_r1,
body2.current_position() + world_r2,
axis2,
);
delta_x += zero_distance_limit.compute_correction_along_axis(
body1.current_position() + world_r1,
body2.current_position() + world_r2,
axis3,
);
}
let magnitude = delta_x.length();
if magnitude <= Scalar::EPSILON {
return Vector::ZERO;
}
let dir = delta_x / magnitude;
let w1 = PositionConstraint::compute_generalized_inverse_mass(self, body1, world_r1, dir);
let w2 = PositionConstraint::compute_generalized_inverse_mass(self, body2, world_r2, dir);
let delta_lagrange = self.compute_lagrange_update(
self.position_lagrange,
magnitude,
&[w1, w2],
self.compliance,
dt,
);
self.position_lagrange += delta_lagrange;
self.apply_positional_lagrange_update(
body1,
body2,
delta_lagrange,
dir,
world_r1,
world_r2,
);
self.compute_force(self.position_lagrange, dir, dt)
}
pub fn with_free_axis(self, axis: Vector) -> Self {
Self {
free_axis: axis,
..self
}
}
pub fn with_limits(self, min: Scalar, max: Scalar) -> Self {
Self {
free_axis_limits: Some(DistanceLimit::new(min, max)),
..self
}
}
#[cfg(feature = "2d")]
fn get_rotation_difference(&self, rot1: &Rotation, rot2: &Rotation) -> Scalar {
rot1.angle_between(*rot2)
}
#[cfg(feature = "3d")]
fn get_rotation_difference(&self, rot1: &Rotation, rot2: &Rotation) -> Vector {
-2.0 * (rot1.0 * rot2.inverse().0).xyz()
}
}
impl PositionConstraint for PrismaticJoint {}
impl AngularConstraint for PrismaticJoint {}
impl MapEntities for PrismaticJoint {
fn map_entities<M: EntityMapper>(&mut self, entity_mapper: &mut M) {
self.entity1 = entity_mapper.map_entity(self.entity1);
self.entity2 = entity_mapper.map_entity(self.entity2);
}
}