diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs index 41fb38919..7121e7fd6 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_pendulum2; 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, "Multi-pendulum", multi_pendulum2::init_world), // ── Characters ───────────────────────────────────────────────────── Example::new( CONTROLS, diff --git a/examples2d/multi_pendulum2.rs b/examples2d/multi_pendulum2.rs new file mode 100644 index 000000000..869c63d92 --- /dev/null +++ b/examples2d/multi_pendulum2.rs @@ -0,0 +1,99 @@ +use rapier_testbed2d::Testbed; +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 + } 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); + + /* + * Join the newly created segment to the previous one. + */ + let joint = RevoluteJointBuilder::new() + .local_anchor1(prev_end_local) + .local_anchor2(Vector::ZERO) + .contacts_enabled(false); + + 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; + prev_end_local = end_local; + prev_end_world += Vector::from_angle(rotation).rotate(end_local); + } + } + + /* + * Set up the testbed. + */ + testbed.set_physics_world(world); + testbed.look_at(Vector::ZERO, 10.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() }]