diff --git a/src/dynamics/solver/joints/revolute.rs b/src/dynamics/solver/joints/revolute.rs index 7a8ed698..78a2b47b 100644 --- a/src/dynamics/solver/joints/revolute.rs +++ b/src/dynamics/solver/joints/revolute.rs @@ -25,6 +25,12 @@ pub struct RevoluteJoint { pub local_anchor1: Vector, /// Attachment point on the second body. pub local_anchor2: Vector, + /// Rotation applied on the first body. This allows to orient the body relative to the `aligned_axis`. + #[cfg(feature = "3d")] + pub local_rotation1: Rotation, + /// Rotation applied on the second body. + #[cfg(feature = "3d")] + pub local_rotation2: Rotation, /// A unit vector that controls which axis should be aligned for both entities. /// /// In 2D this should always be the Z axis. @@ -105,6 +111,10 @@ impl Joint for RevoluteJoint { entity2, local_anchor1: Vector::ZERO, local_anchor2: Vector::ZERO, + #[cfg(feature = "3d")] + local_rotation1: Rotation::default(), + #[cfg(feature = "3d")] + local_rotation2: Rotation::default(), aligned_axis: Vector3::Z, angle_limit: None, damping_linear: 1.0, @@ -194,8 +204,8 @@ impl RevoluteJoint { #[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; + let a1 = *rot1 * self.local_rotation1 * self.aligned_axis; + let a2 = *rot2 * self.local_rotation2 * self.aligned_axis; a1.cross(a2) } @@ -215,9 +225,13 @@ impl RevoluteJoint { #[cfg(feature = "3d")] { // [n, n1, n2] = [a1, b1, b2], where [a, b, c] are perpendicular unit axes on the bodies. - 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(); + let a1 = *body1.rotation * self.local_rotation1 * self.aligned_axis; + let b1 = *body1.rotation + * self.local_rotation1 + * self.aligned_axis.any_orthonormal_vector(); + let b2 = *body2.rotation + * self.local_rotation2 + * self.aligned_axis.any_orthonormal_vector(); angle_limit.compute_correction(a1, b1, b2, dt) } }) else { diff --git a/src/dynamics/solver/joints/spherical.rs b/src/dynamics/solver/joints/spherical.rs index c30f1d4f..41b47667 100644 --- a/src/dynamics/solver/joints/spherical.rs +++ b/src/dynamics/solver/joints/spherical.rs @@ -25,6 +25,10 @@ pub struct SphericalJoint { pub local_anchor1: Vector, /// Attachment point on the second body. pub local_anchor2: Vector, + /// Rotation applied on the first body. This allows to orient the body relative to the `swing_axis`. + pub local_rotation1: Rotation, + /// Rotation applied on the second body. + pub local_rotation2: Rotation, /// An axis that the attached bodies can swing around. This is normally the x-axis. pub swing_axis: Vector3, /// An axis that the attached bodies can twist around. This is normally the y-axis. @@ -96,6 +100,8 @@ impl Joint for SphericalJoint { entity2, local_anchor1: Vector::ZERO, local_anchor2: Vector::ZERO, + local_rotation1: Rotation::default(), + local_rotation2: Rotation::default(), swing_axis: Vector3::X, twist_axis: Vector3::Y, swing_limit: None, @@ -193,8 +199,8 @@ impl SphericalJoint { dt: Scalar, ) -> Torque { if let Some(joint_limit) = self.swing_limit { - let a1 = *body1.rotation * self.swing_axis; - let a2 = *body2.rotation * self.swing_axis; + let a1 = *body1.rotation * self.local_rotation1 * self.swing_axis; + let a2 = *body2.rotation * self.local_rotation2 * self.swing_axis; let n = a1.cross(a2); let n_magnitude = n.length(); @@ -230,11 +236,11 @@ impl SphericalJoint { dt: Scalar, ) -> Torque { if let Some(joint_limit) = self.twist_limit { - let a1 = *body1.rotation * self.swing_axis; - let a2 = *body2.rotation * self.swing_axis; + let a1 = *body1.rotation * self.local_rotation1 * self.swing_axis; + let a2 = *body2.rotation * self.local_rotation2 * self.swing_axis; - let b1 = *body1.rotation * self.twist_axis; - let b2 = *body2.rotation * self.twist_axis; + let b1 = *body1.rotation * self.local_rotation1 * self.twist_axis; + let b2 = *body2.rotation * self.local_rotation2 * self.twist_axis; let n = a1 + a2; let n_magnitude = n.length(); diff --git a/src/position.rs b/src/position.rs index 8d519f04..95a2e601 100644 --- a/src/position.rs +++ b/src/position.rs @@ -712,6 +712,22 @@ impl Rotation { } } +#[cfg(feature = "3d")] +impl std::ops::Mul for Rotation { + type Output = Self; + + fn mul(self, rhs: Self) -> Self::Output { + Self(self.0 * rhs.0) + } +} + +#[cfg(feature = "3d")] +impl std::ops::MulAssign for Rotation { + fn mul_assign(&mut self, rhs: Self) { + *self = *self * rhs; + } +} + #[cfg(feature = "3d")] impl core::ops::Mul for Rotation { type Output = Vector;