From 4affa2b659226c4ea73d3370c5c4c61e2e344fa1 Mon Sep 17 00:00:00 2001 From: Kyzzsa Date: Mon, 29 Jun 2026 16:44:47 +0800 Subject: [PATCH 1/5] Fix SIMD joint constraints - Correct local frame in JointConstraintBuilderSimd when the rigidbody's center of mass is not the same as its world position - Add a multi-pendulum test case --- examples2d/all_examples2.rs | 2 + examples2d/multi_pendulum.rs | 66 +++++++++++++++++++ .../joint_constraint_builder.rs | 12 +++- 3 files changed, 78 insertions(+), 2 deletions(-) create mode 100644 examples2d/multi_pendulum.rs diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs index 41fb38919..f63421639 100644 --- a/examples2d/all_examples2.rs +++ b/examples2d/all_examples2.rs @@ -21,6 +21,7 @@ mod inverse_kinematics2; mod joint_motor_position2; mod joints2; mod locked_rotations2; +mod multi_pendulum; mod one_way_platforms2; mod pin_slot_joint2; mod platform2; @@ -100,6 +101,7 @@ pub async fn main() { "Inverse kinematics", inverse_kinematics2::init_world, ), + Example::new(JOINTS, "Double pendulum", multi_pendulum::init_world), // ── Characters ───────────────────────────────────────────────────── Example::new( CONTROLS, diff --git a/examples2d/multi_pendulum.rs b/examples2d/multi_pendulum.rs new file mode 100644 index 000000000..d7bd1752a --- /dev/null +++ b/examples2d/multi_pendulum.rs @@ -0,0 +1,66 @@ +use rapier_testbed2d::Testbed; +use rapier2d::prelude::*; + +use rand::distr::{Distribution, StandardUniform}; +use rand::{SeedableRng, rngs::StdRng}; + +pub fn init_world(testbed: &mut Testbed) { + let mut world = PhysicsWorld::new(); + world.gravity = Vector::new(0.0, -9.81); + + let settings = testbed.example_settings_mut(); + + let randomize = settings.get_or_set_bool("Randomize", false); + let count = settings.get_or_set_u32("Pendulum Count", 6, 1..=128); + let segments = settings.get_or_set_u32("Pendulum Segments", 3, 1..=8); + + let mut rng = StdRng::seed_from_u64(0); + let distribution = StandardUniform; + + let half_length = 1.0; + for i in 0..count { + let base_pos = Vector::new(i as f32 * 10.0 * half_length, 0.0); + let base = RigidBodyBuilder::fixed().translation(base_pos); + let base_collider = ColliderBuilder::ball(0.2).mass(0.0); + let (base_handle, _) = world.insert(base, base_collider); + + let mut prev_handle = base_handle; + let mut prev_end_local = Vector::ZERO; + let mut prev_end_world = base_pos; + + for _ in 0..segments { + let rotation = if randomize { + let rand_val: f32 = distribution.sample(&mut rng); + (rand_val - 0.5) * std::f32::consts::PI + } else { + 0.0 + }; + + let body = RigidBodyBuilder::dynamic() + .translation(prev_end_world) + .rotation(rotation) + .can_sleep(false); + + let collider = ColliderBuilder::capsule_x(half_length, 0.2 * half_length) + .translation(Vector::new(half_length, 0.0)); + + let (handle, _) = world.insert(body, collider); + + let joint = RevoluteJointBuilder::new() + .local_anchor1(prev_end_local) + .local_anchor2(Vector::ZERO) + .contacts_enabled(false); + + world.insert_impulse_joint(prev_handle, handle, joint); + + let end_local = Vector::new(2.0 * half_length, 0.0); + + prev_handle = handle; + prev_end_local = end_local; + prev_end_world += Vector::from_angle(rotation).rotate(end_local); + } + } + + testbed.set_physics_world(world); + testbed.look_at(Vector::ZERO, 40.0); +} diff --git a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs index 51ad2aeb1..8b1f6164e 100644 --- a/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_constraint_builder.rs @@ -141,13 +141,21 @@ impl JointConstraintBuilderSimd { }]; let local_frame1 = array![|ii| if body1[ii] != u32::MAX { - (joint[ii].data.local_frame1).into() + (joint[ii] + .data + .local_frame1 + .append_translation(-rb1[ii].mprops.local_mprops.local_com)) + .into() } else { (rb1[ii].pos.position * joint[ii].data.local_frame1).into() }] .into(); let local_frame2 = array![|ii| if body2[ii] != u32::MAX { - (joint[ii].data.local_frame2).into() + (joint[ii] + .data + .local_frame2 + .append_translation(-rb2[ii].mprops.local_mprops.local_com)) + .into() } else { (rb2[ii].pos.position * joint[ii].data.local_frame2).into() }] From c5c55e4617d492812b265eaa869c48a93232e3f2 Mon Sep 17 00:00:00 2001 From: Kyzzsa Date: Mon, 29 Jun 2026 17:20:07 +0800 Subject: [PATCH 2/5] Improve quality of the test scene --- examples2d/multi_pendulum.rs | 18 +++++++++--------- 1 file changed, 9 insertions(+), 9 deletions(-) diff --git a/examples2d/multi_pendulum.rs b/examples2d/multi_pendulum.rs index d7bd1752a..2d5d33a47 100644 --- a/examples2d/multi_pendulum.rs +++ b/examples2d/multi_pendulum.rs @@ -4,6 +4,8 @@ use rapier2d::prelude::*; use rand::distr::{Distribution, StandardUniform}; use rand::{SeedableRng, rngs::StdRng}; +const HALF_LENGTH: f32 = 1.0; + pub fn init_world(testbed: &mut Testbed) { let mut world = PhysicsWorld::new(); world.gravity = Vector::new(0.0, -9.81); @@ -11,18 +13,16 @@ pub fn init_world(testbed: &mut Testbed) { let settings = testbed.example_settings_mut(); let randomize = settings.get_or_set_bool("Randomize", false); - let count = settings.get_or_set_u32("Pendulum Count", 6, 1..=128); + let count = settings.get_or_set_u32("Pendulum Count", 6, 1..=16); let segments = settings.get_or_set_u32("Pendulum Segments", 3, 1..=8); let mut rng = StdRng::seed_from_u64(0); let distribution = StandardUniform; - let half_length = 1.0; for i in 0..count { - let base_pos = Vector::new(i as f32 * 10.0 * half_length, 0.0); + let base_pos = Vector::new(i as f32 * 10.0 * HALF_LENGTH, 0.0); let base = RigidBodyBuilder::fixed().translation(base_pos); - let base_collider = ColliderBuilder::ball(0.2).mass(0.0); - let (base_handle, _) = world.insert(base, base_collider); + let base_handle = world.insert_body(base); let mut prev_handle = base_handle; let mut prev_end_local = Vector::ZERO; @@ -41,8 +41,8 @@ pub fn init_world(testbed: &mut Testbed) { .rotation(rotation) .can_sleep(false); - let collider = ColliderBuilder::capsule_x(half_length, 0.2 * half_length) - .translation(Vector::new(half_length, 0.0)); + let collider = ColliderBuilder::capsule_x(HALF_LENGTH, 0.2 * HALF_LENGTH) + .translation(Vector::new(HALF_LENGTH, 0.0)); let (handle, _) = world.insert(body, collider); @@ -53,7 +53,7 @@ pub fn init_world(testbed: &mut Testbed) { world.insert_impulse_joint(prev_handle, handle, joint); - let end_local = Vector::new(2.0 * half_length, 0.0); + let end_local = Vector::new(2.0 * HALF_LENGTH, 0.0); prev_handle = handle; prev_end_local = end_local; @@ -62,5 +62,5 @@ pub fn init_world(testbed: &mut Testbed) { } testbed.set_physics_world(world); - testbed.look_at(Vector::ZERO, 40.0); + testbed.look_at(Vector::ZERO, 10.0); } From 98b977eb29a3566fac51eff34275c2ef285c996b Mon Sep 17 00:00:00 2001 From: Kyzzsa Date: Mon, 29 Jun 2026 17:35:30 +0800 Subject: [PATCH 3/5] Add some comments --- examples2d/multi_pendulum.rs | 33 +++++++++++++++++++++++++++++++++ 1 file changed, 33 insertions(+) diff --git a/examples2d/multi_pendulum.rs b/examples2d/multi_pendulum.rs index 2d5d33a47..869c63d92 100644 --- a/examples2d/multi_pendulum.rs +++ b/examples2d/multi_pendulum.rs @@ -4,31 +4,55 @@ use rapier2d::prelude::*; use rand::distr::{Distribution, StandardUniform}; use rand::{SeedableRng, rngs::StdRng}; +/* + * Length of each pendulum segment. + */ const HALF_LENGTH: f32 = 1.0; pub fn init_world(testbed: &mut Testbed) { + /* + * Physics world with gravity. + */ let mut world = PhysicsWorld::new(); world.gravity = Vector::new(0.0, -9.81); + /* + * User‑controllable parameters from the testbed UI. + */ let settings = testbed.example_settings_mut(); let randomize = settings.get_or_set_bool("Randomize", false); let count = settings.get_or_set_u32("Pendulum Count", 6, 1..=16); let segments = settings.get_or_set_u32("Pendulum Segments", 3, 1..=8); + /* + * Random number generator for randomized pendulum angles. + */ let mut rng = StdRng::seed_from_u64(0); let distribution = StandardUniform; + /* + * Create pendulums + */ for i in 0..count { + /* + * Create a fixed base for the pendulum. + */ let base_pos = Vector::new(i as f32 * 10.0 * HALF_LENGTH, 0.0); let base = RigidBodyBuilder::fixed().translation(base_pos); let base_handle = world.insert_body(base); + /* + * Record the end for next iteration. + */ let mut prev_handle = base_handle; let mut prev_end_local = Vector::ZERO; let mut prev_end_world = base_pos; for _ in 0..segments { + /* + * Create a new segment, with a random rotation if randomize is set to true. + */ let rotation = if randomize { let rand_val: f32 = distribution.sample(&mut rng); (rand_val - 0.5) * std::f32::consts::PI @@ -46,6 +70,9 @@ pub fn init_world(testbed: &mut Testbed) { let (handle, _) = world.insert(body, collider); + /* + * Join the newly created segment to the previous one. + */ let joint = RevoluteJointBuilder::new() .local_anchor1(prev_end_local) .local_anchor2(Vector::ZERO) @@ -53,6 +80,9 @@ pub fn init_world(testbed: &mut Testbed) { world.insert_impulse_joint(prev_handle, handle, joint); + /* + * Record the end for next iteration. + */ let end_local = Vector::new(2.0 * HALF_LENGTH, 0.0); prev_handle = handle; @@ -61,6 +91,9 @@ pub fn init_world(testbed: &mut Testbed) { } } + /* + * Set up the testbed. + */ testbed.set_physics_world(world); testbed.look_at(Vector::ZERO, 10.0); } From 3a7ded61b3298492f9301a1c4e2cbf12e1a138af Mon Sep 17 00:00:00 2001 From: Kyzzsa Date: Mon, 29 Jun 2026 17:43:38 +0800 Subject: [PATCH 4/5] Correct example name --- examples2d/all_examples2.rs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs index f63421639..c7f45cb8c 100644 --- a/examples2d/all_examples2.rs +++ b/examples2d/all_examples2.rs @@ -101,7 +101,7 @@ pub async fn main() { "Inverse kinematics", inverse_kinematics2::init_world, ), - Example::new(JOINTS, "Double pendulum", multi_pendulum::init_world), + Example::new(JOINTS, "Multi-pendulum", multi_pendulum::init_world), // ── Characters ───────────────────────────────────────────────────── Example::new( CONTROLS, From eb46fce212ee95fdc7f854aaa4c975700db92ec7 Mon Sep 17 00:00:00 2001 From: Kyzzsa Date: Mon, 29 Jun 2026 17:51:56 +0800 Subject: [PATCH 5/5] Correct example file name --- examples2d/all_examples2.rs | 4 ++-- examples2d/{multi_pendulum.rs => multi_pendulum2.rs} | 0 2 files changed, 2 insertions(+), 2 deletions(-) rename examples2d/{multi_pendulum.rs => multi_pendulum2.rs} (100%) diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs index c7f45cb8c..7121e7fd6 100644 --- a/examples2d/all_examples2.rs +++ b/examples2d/all_examples2.rs @@ -21,7 +21,7 @@ mod inverse_kinematics2; mod joint_motor_position2; mod joints2; mod locked_rotations2; -mod multi_pendulum; +mod multi_pendulum2; mod one_way_platforms2; mod pin_slot_joint2; mod platform2; @@ -101,7 +101,7 @@ pub async fn main() { "Inverse kinematics", inverse_kinematics2::init_world, ), - Example::new(JOINTS, "Multi-pendulum", multi_pendulum::init_world), + Example::new(JOINTS, "Multi-pendulum", multi_pendulum2::init_world), // ── Characters ───────────────────────────────────────────────────── Example::new( CONTROLS, diff --git a/examples2d/multi_pendulum.rs b/examples2d/multi_pendulum2.rs similarity index 100% rename from examples2d/multi_pendulum.rs rename to examples2d/multi_pendulum2.rs