Skip to content
New issue

Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.

Sign up for GitHub

By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.

Already on GitHub? Sign in to your account

Jump to bottom

Joint entities rotations #511

Open
wants to merge 3 commits into
base: main
Choose a base branch
from
Open

Joint entities rotations #511

Show file tree
Hide file tree
Changes from all commits
Commits
Show all changes
3 commits
Select commit Hold shift + click to select a range
872e8f0
impl Mul and MulAssign for Rotation
exoexo-dev Sep 8, 2024
b8612bf
allow to set local rotation of revolute joint bodies in 3D
exoexo-dev Sep 8, 2024
7320a8e
allow to set local rotation of spherical joint bodies
exoexo-dev Sep 8, 2024
File filter

Filter by extension

Filter by extension
Conversations
Failed to load comments.
Loading
Jump to
Jump to file
Failed to load files.
Loading
Diff view
Diff view
24 changes: 19 additions & 5 deletions src/dynamics/solver/joints/revolute.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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)
}

Expand All @@ -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 {
Expand Down
18 changes: 12 additions & 6 deletions src/dynamics/solver/joints/spherical.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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.
Expand Down Expand Up @@ -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,
Expand Down Expand Up @@ -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();
Expand Down Expand Up @@ -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();
Expand Down
16 changes: 16 additions & 0 deletions src/position.rs
View file
Open in desktop
Original file line number Diff line number Diff line change
Expand Up @@ -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 {
Copy link
Contributor Author

@exoexo-dev exoexo-dev Sep 8, 2024 •
edited
Loading

Choose a reason for hiding this comment

The reason will be displayed to describe this comment to others. Learn more.

this impl is not needed for this PR, but it probably doesn't hurt to have it

fn mul_assign(&mut self, rhs: Self) {
*self = *self * rhs;
}
}

#[cfg(feature = "3d")]
impl core::ops::Mul<Vector> for Rotation {
type Output = Vector;
Expand Down