dimforge / rapier

2D and 3D physics engines focused on performance.
https://rapier.rs
Apache License 2.0
3.77k stars 235 forks source link

MultibodyJoint behaves incorrectly - does not react to angular momentum changes #656

Open rsk700 opened 2 weeks ago

rsk700 commented 2 weeks ago

I was trying to use MultibodyJoint and looks like it is simulated incorrectly. I spawned two dynamic RBs, and connected with joint using MultibodyJointSet, on hit Multibody jointed RB does not change its angular momentum, while if using ImpulseJoint for the same simulation it behaves as expected, here is video:

https://github.com/dimforge/rapier/assets/816292/414be731-8d72-40cd-9628-90e41606e173

Test code I was using:

// Cargo.toml

// [package]
// name = "multibody-issue"
// version = "0.1.0"
// edition = "2021"

// [dependencies]
// rapier2d = "0.20.0"
// rapier_testbed2d = "0.20.0"

use rapier2d::prelude::*;
use rapier_testbed2d::{Testbed, TestbedApp};

fn main() {
    let app = TestbedApp::from_builders(
        0,
        vec![
            ("Multibody joint - bad", multi_bad),
            ("Impulse joint - ok", impulse_ok),
        ],
    );
    app.run();
}

fn multi_bad(tb: &mut Testbed) {
    let mut bodies = RigidBodySet::new();
    let mut colliders = ColliderSet::new();
    let impulse_joints = ImpulseJointSet::new();
    let mut multibody_joints = MultibodyJointSet::new();

    let ground = bodies.insert(RigidBodyBuilder::fixed().translation(vector![0.0, -2.0]));
    colliders.insert_with_parent(ColliderBuilder::cuboid(0.5, 0.5), ground, &mut bodies);

    let rbt = vector![0.70, 0.0];
    let rb = bodies.insert(RigidBodyBuilder::dynamic().translation(rbt));
    colliders.insert_with_parent(ColliderBuilder::cuboid(0.3, 0.1).mass(0.1), rb, &mut bodies);

    let rb2 = bodies.insert(RigidBodyBuilder::dynamic().translation(rbt));
    colliders.insert_with_parent(
        ColliderBuilder::cuboid(0.05, 0.05).mass(0.1),
        rb2,
        &mut bodies,
    );

    multibody_joints.insert(
        rb,
        rb2,
        RevoluteJointBuilder::new()
            .local_anchor1(point![0.0, 0.0])
            .local_anchor2(point![0.0, 0.0])
            .contacts_enabled(false),
        true,
    );

    tb.set_world(bodies, colliders, impulse_joints, multibody_joints);
}

fn impulse_ok(tb: &mut Testbed) {
    let mut bodies = RigidBodySet::new();
    let mut colliders = ColliderSet::new();
    let mut impulse_joints = ImpulseJointSet::new();
    let multibody_joints = MultibodyJointSet::new();

    let ground = bodies.insert(RigidBodyBuilder::fixed().translation(vector![0.0, -2.0]));
    colliders.insert_with_parent(ColliderBuilder::cuboid(0.5, 0.5), ground, &mut bodies);

    let rbt = vector![0.70, 0.0];
    let rb = bodies.insert(RigidBodyBuilder::dynamic().translation(rbt));
    colliders.insert_with_parent(ColliderBuilder::cuboid(0.3, 0.1).mass(0.1), rb, &mut bodies);

    let rb2 = bodies.insert(RigidBodyBuilder::dynamic().translation(rbt));
    colliders.insert_with_parent(
        ColliderBuilder::cuboid(0.05, 0.05).mass(0.1),
        rb2,
        &mut bodies,
    );

    impulse_joints.insert(
        rb,
        rb2,
        RevoluteJointBuilder::new()
            .local_anchor1(point![0.0, 0.0])
            .local_anchor2(point![0.0, 0.0])
            .contacts_enabled(false),
        true,
    );

    tb.set_world(bodies, colliders, impulse_joints, multibody_joints);
}