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 RevoluteJoint {
pub entity1: Entity,
pub entity2: Entity,
pub local_anchor1: Vector,
pub local_anchor2: Vector,
#[cfg(feature = "2d")]
pub(crate) aligned_axis: Vector3,
#[cfg(feature = "3d")]
pub aligned_axis: Vector,
pub angle_limit: Option<AngleLimit>,
pub damping_linear: Scalar,
pub damping_angular: Scalar,
pub position_lagrange: Scalar,
pub align_lagrange: Scalar,
pub angle_limit_lagrange: Scalar,
pub compliance: Scalar,
pub force: Vector,
pub align_torque: Torque,
pub angle_limit_torque: Torque,
}
impl XpbdConstraint<2> for RevoluteJoint {
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;
self.angle_limit_lagrange = 0.0;
}
fn solve(&mut self, bodies: [&mut RigidBodyQueryItem; 2], dt: Scalar) {
let [body1, body2] = bodies;
let compliance = self.compliance;
#[cfg(feature = "3d")]
{
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.angle_limit_torque = self.apply_angle_limits(body1, body2, dt);
let mut lagrange = self.position_lagrange;
self.force = self.align_position(
body1,
body2,
self.local_anchor1,
self.local_anchor2,
&mut lagrange,
compliance,
dt,
);
self.position_lagrange = lagrange;
}
}
impl Joint for RevoluteJoint {
fn new(entity1: Entity, entity2: Entity) -> Self {
Self {
entity1,
entity2,
local_anchor1: Vector::ZERO,
local_anchor2: Vector::ZERO,
aligned_axis: Vector3::Z,
angle_limit: None,
damping_linear: 1.0,
damping_angular: 1.0,
position_lagrange: 0.0,
align_lagrange: 0.0,
angle_limit_lagrange: 0.0,
compliance: 0.0,
force: Vector::ZERO,
#[cfg(feature = "2d")]
align_torque: 0.0,
#[cfg(feature = "3d")]
align_torque: Vector::ZERO,
#[cfg(feature = "2d")]
angle_limit_torque: 0.0,
#[cfg(feature = "3d")]
angle_limit_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 RevoluteJoint {
#[cfg(feature = "3d")]
pub fn with_aligned_axis(self, axis: Vector) -> Self {
Self {
aligned_axis: axis,
..self
}
}
pub fn with_angle_limits(self, min: Scalar, max: Scalar) -> Self {
Self {
angle_limit: Some(AngleLimit::new(min, max)),
..self
}
}
#[cfg(feature = "3d")]
fn get_rotation_difference(&self, rot1: &Rotation, rot2: &Rotation) -> Vector3 {
let a1 = rot1 * self.aligned_axis;
let a2 = rot2 * self.aligned_axis;
a1.cross(a2)
}
#[allow(clippy::too_many_arguments)]
fn apply_angle_limits(
&mut self,
body1: &mut RigidBodyQueryItem,
body2: &mut RigidBodyQueryItem,
dt: Scalar,
) -> Torque {
let Some(Some(correction)) = self.angle_limit.map(|angle_limit| {
#[cfg(feature = "2d")]
{
angle_limit.compute_correction(*body1.rotation, *body2.rotation, dt)
}
#[cfg(feature = "3d")]
{
let a1 = *body1.rotation * self.aligned_axis;
let b1 = *body1.rotation * self.aligned_axis.any_orthonormal_vector();
let b2 = *body2.rotation * self.aligned_axis.any_orthonormal_vector();
angle_limit.compute_correction(a1, b1, b2, dt)
}
}) else {
return Torque::ZERO;
};
let mut lagrange = self.angle_limit_lagrange;
let torque =
self.align_orientation(body1, body2, correction, &mut lagrange, self.compliance, dt);
self.angle_limit_lagrange = lagrange;
torque
}
}
impl PositionConstraint for RevoluteJoint {}
impl AngularConstraint for RevoluteJoint {}
impl MapEntities for RevoluteJoint {
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);
}
}