From fb20d72ee29de9311a81aec6eb9f02fd2aa35fc4 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Sun, 20 Feb 2022 12:55:00 +0100 Subject: [PATCH] Joint API and joint motors improvements --- .vscode/tasks.json | 97 ++++ benchmarks2d/balls2.rs | 6 +- benchmarks2d/boxes2.rs | 20 +- benchmarks2d/capsules2.rs | 20 +- benchmarks2d/convex_polygons2.rs | 20 +- benchmarks2d/heightfield2.rs | 12 +- benchmarks2d/joint_ball2.rs | 11 +- benchmarks2d/joint_fixed2.rs | 13 +- benchmarks2d/joint_prismatic2.rs | 16 +- benchmarks2d/pyramid2.rs | 10 +- benchmarks3d/balls3.rs | 6 +- benchmarks3d/boxes3.rs | 12 +- benchmarks3d/capsules3.rs | 12 +- benchmarks3d/ccd3.rs | 11 +- benchmarks3d/compound3.rs | 28 +- benchmarks3d/convex_polyhedron3.rs | 14 +- benchmarks3d/heightfield3.rs | 12 +- benchmarks3d/joint_ball3.rs | 11 +- benchmarks3d/joint_fixed3.rs | 16 +- benchmarks3d/joint_prismatic3.rs | 18 +- benchmarks3d/joint_revolute3.rs | 22 +- benchmarks3d/keva3.rs | 34 +- benchmarks3d/pyramid3.rs | 12 +- benchmarks3d/stacks3.rs | 24 +- benchmarks3d/trimesh3.rs | 12 +- examples2d/add_remove2.rs | 12 +- examples2d/ccd2.rs | 27 +- examples2d/collision_groups2.rs | 20 +- examples2d/convex_polygons2.rs | 20 +- examples2d/damping2.rs | 5 +- examples2d/debug_box_ball2.rs | 10 +- examples2d/drum2.rs | 12 +- examples2d/heightfield2.rs | 12 +- examples2d/joints2.rs | 11 +- examples2d/locked_rotations2.rs | 16 +- examples2d/one_way_platforms2.rs | 14 +- examples2d/platform2.rs | 22 +- examples2d/polyline2.rs | 12 +- examples2d/pyramid2.rs | 10 +- examples2d/restitution2.rs | 15 +- examples2d/sensor2.rs | 21 +- examples2d/trimesh2.rs | 20 +- examples3d/articulations3.rs | 228 ++++---- examples3d/ccd3.rs | 24 +- examples3d/collision_groups3.rs | 20 +- examples3d/compound3.rs | 20 +- examples3d/convex_decomposition3.rs | 12 +- examples3d/convex_polyhedron3.rs | 14 +- examples3d/damping3.rs | 5 +- examples3d/debug_add_remove_collider3.rs | 12 +- examples3d/debug_articulations3.rs | 23 +- examples3d/debug_big_colliders3.rs | 10 +- examples3d/debug_boxes3.rs | 12 +- examples3d/debug_cylinder3.rs | 12 +- examples3d/debug_dynamic_collider_add3.rs | 18 +- examples3d/debug_friction3.rs | 11 +- examples3d/debug_infinite_fall3.rs | 16 +- examples3d/debug_prismatic3.rs | 56 +- examples3d/debug_rollback3.rs | 11 +- examples3d/debug_shape_modification3.rs | 25 +- examples3d/debug_triangle3.rs | 11 +- examples3d/debug_trimesh3.rs | 11 +- examples3d/domino3.rs | 10 +- examples3d/fountain3.rs | 16 +- examples3d/harness_capsules3.rs | 12 +- examples3d/heightfield3.rs | 20 +- examples3d/joints3.rs | 282 +++++----- examples3d/keva3.rs | 34 +- examples3d/locked_rotations3.rs | 18 +- examples3d/one_way_platforms3.rs | 14 +- examples3d/platform3.rs | 32 +- examples3d/primitives3.rs | 20 +- examples3d/restitution3.rs | 20 +- examples3d/sensor3.rs | 21 +- examples3d/trimesh3.rs | 20 +- src/dynamics/integration_parameters.rs | 29 +- src/dynamics/island_manager.rs | 32 +- src/dynamics/joint/fixed_joint.rs | 93 +++- src/dynamics/joint/generic_joint.rs | 501 ++++++++++++++++++ .../joint/impulse_joint/impulse_joint.rs | 4 +- .../joint/impulse_joint/impulse_joint_set.rs | 4 +- src/dynamics/joint/joint_data.rs | 275 ---------- src/dynamics/joint/mod.rs | 12 +- src/dynamics/joint/motor_model.rs | 49 +- .../joint/multibody_joint/multibody_joint.rs | 54 +- .../multibody_joint/multibody_joint_set.rs | 4 +- .../multibody_joint/unit_multibody_joint.rs | 27 +- src/dynamics/joint/prismatic_joint.rs | 194 +++++-- src/dynamics/joint/revolute_joint.rs | 177 +++++-- src/dynamics/joint/spherical_joint.rs | 146 ++++- src/dynamics/rigid_body.rs | 6 + src/dynamics/rigid_body_set.rs | 3 +- .../solver/generic_velocity_constraint.rs | 2 +- .../generic_velocity_ground_constraint.rs | 2 +- src/dynamics/solver/island_solver.rs | 2 +- .../joint_generic_velocity_constraint.rs | 151 +++--- ...int_generic_velocity_constraint_builder.rs | 98 ++-- .../joint_velocity_constraint.rs | 307 +++++++---- .../joint_velocity_constraint_builder.rs | 405 +++++++++++--- src/dynamics/solver/velocity_constraint.rs | 18 +- .../solver/velocity_constraint_wide.rs | 14 +- .../solver/velocity_ground_constraint.rs | 18 +- .../solver/velocity_ground_constraint_wide.rs | 12 +- src/geometry/collider.rs | 6 + src/geometry/collider_set.rs | 6 +- src_testbed/harness/mod.rs | 38 +- src_testbed/testbed.rs | 2 +- src_testbed/ui.rs | 15 +- 108 files changed, 2650 insertions(+), 1854 deletions(-) create mode 100644 src/dynamics/joint/generic_joint.rs delete mode 100644 src/dynamics/joint/joint_data.rs diff --git a/.vscode/tasks.json b/.vscode/tasks.json index 0bc892b..de94b50 100644 --- a/.vscode/tasks.json +++ b/.vscode/tasks.json @@ -98,6 +98,103 @@ "--pause" ], "group": "build" + }, + { + "label": "bench 3d (no-simd - release) ", + "type": "shell", + "command": "cargo", + "args": [ + "run", + "--bin", + "all_benchmarks3", + "--release", + "--features", + "other-backends", + "--", + "--pause" + ], + "group": "build" + }, + { + "label": "bench 3d (simd - release) ", + "type": "shell", + "command": "cargo", + "args": [ + "run", + "--bin", + "all_benchmarks3", + "--release", + "--features", + "simd-stable,other-backends", + "--", + "--pause" + ], + "group": "build" + }, + { + "label": "bench 3d (simd - parallel - release) ", + "type": "shell", + "command": "cargo", + "args": [ + "run", + "--bin", + "all_benchmarks3", + "--release", + "--features", + "simd-stable,other-backends,parallel", + "--", + "--pause" + ], + "group": "build" + }, + { + "label": "bench 2d (no-simd - release) ", + "type": "shell", + "command": "cargo", + "args": [ + "run", + "--bin", + "all_benchmarks2", + "--release", + "--features", + "other-backends", + "--", + "--pause" + ], + "group": "build" + }, + { + "label": "bench 2d (simd - release) ", + "type": "shell", + "command": "cargo", + "args": [ + "run", + "--bin", + "all_benchmarks2", + "--release", + "--features", + "simd-stable,other-backends", + "--", + "--pause" + ], + "group": "build" + }, + { + "label": "bench 2d (simd - parallel - release) ", + "type": "shell", + "command": "cargo", + "args": [ + "run", + "--bin", + "all_benchmarks2", + "--release", + "--features", + "simd-stable,other-backends,parallel", + "--", + "--pause" + ], + "group": "build" } ] +] } \ No newline at end of file diff --git a/benchmarks2d/balls2.rs b/benchmarks2d/balls2.rs index 168acaf..a560d65 100644 --- a/benchmarks2d/balls2.rs +++ b/benchmarks2d/balls2.rs @@ -48,11 +48,9 @@ pub fn init_world(testbed: &mut Testbed) { }; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new(status) - .translation(vector![x, y]) - .build(); + let rigid_body = RigidBodyBuilder::new(status).translation(vector![x, y]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::ball(rad).build(); + let collider = ColliderBuilder::ball(rad); colliders.insert_with_parent(collider, handle, &mut bodies); } } diff --git a/benchmarks2d/boxes2.rs b/benchmarks2d/boxes2.rs index c3d7445..0da76a9 100644 --- a/benchmarks2d/boxes2.rs +++ b/benchmarks2d/boxes2.rs @@ -15,25 +15,23 @@ pub fn init_world(testbed: &mut Testbed) { */ let ground_size = 25.0; - let rigid_body = RigidBodyBuilder::new_static().build(); + let rigid_body = RigidBodyBuilder::new_static(); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, 1.2).build(); + let collider = ColliderBuilder::cuboid(ground_size, 1.2); colliders.insert_with_parent(collider, handle, &mut bodies); let rigid_body = RigidBodyBuilder::new_static() .rotation(std::f32::consts::FRAC_PI_2) - .translation(vector![ground_size, ground_size * 2.0]) - .build(); + .translation(vector![ground_size, ground_size * 2.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size * 2.0, 1.2).build(); + let collider = ColliderBuilder::cuboid(ground_size * 2.0, 1.2); colliders.insert_with_parent(collider, handle, &mut bodies); let rigid_body = RigidBodyBuilder::new_static() .rotation(std::f32::consts::FRAC_PI_2) - .translation(vector![-ground_size, ground_size * 2.0]) - .build(); + .translation(vector![-ground_size, ground_size * 2.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size * 2.0, 1.2).build(); + let collider = ColliderBuilder::cuboid(ground_size * 2.0, 1.2); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -52,11 +50,9 @@ pub fn init_world(testbed: &mut Testbed) { let y = j as f32 * shift + centery + 2.0; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad); colliders.insert_with_parent(collider, handle, &mut bodies); } } diff --git a/benchmarks2d/capsules2.rs b/benchmarks2d/capsules2.rs index 3bfa6ab..86e1004 100644 --- a/benchmarks2d/capsules2.rs +++ b/benchmarks2d/capsules2.rs @@ -15,25 +15,23 @@ pub fn init_world(testbed: &mut Testbed) { */ let ground_size = 25.0; - let rigid_body = RigidBodyBuilder::new_static().build(); + let rigid_body = RigidBodyBuilder::new_static(); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, 1.2).build(); + let collider = ColliderBuilder::cuboid(ground_size, 1.2); colliders.insert_with_parent(collider, handle, &mut bodies); let rigid_body = RigidBodyBuilder::new_static() .rotation(std::f32::consts::FRAC_PI_2) - .translation(vector![ground_size, ground_size * 4.0]) - .build(); + .translation(vector![ground_size, ground_size * 4.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size * 4.0, 1.2).build(); + let collider = ColliderBuilder::cuboid(ground_size * 4.0, 1.2); colliders.insert_with_parent(collider, handle, &mut bodies); let rigid_body = RigidBodyBuilder::new_static() .rotation(std::f32::consts::FRAC_PI_2) - .translation(vector![-ground_size, ground_size * 4.0]) - .build(); + .translation(vector![-ground_size, ground_size * 4.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size * 4.0, 1.2).build(); + let collider = ColliderBuilder::cuboid(ground_size * 4.0, 1.2); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -54,11 +52,9 @@ pub fn init_world(testbed: &mut Testbed) { let y = j as f32 * shifty + centery + 3.0; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::capsule_y(rad * 1.5, rad).build(); + let collider = ColliderBuilder::capsule_y(rad * 1.5, rad); colliders.insert_with_parent(collider, handle, &mut bodies); } } diff --git a/benchmarks2d/convex_polygons2.rs b/benchmarks2d/convex_polygons2.rs index 6678460..c340143 100644 --- a/benchmarks2d/convex_polygons2.rs +++ b/benchmarks2d/convex_polygons2.rs @@ -17,25 +17,23 @@ pub fn init_world(testbed: &mut Testbed) { */ let ground_size = 30.0; - let rigid_body = RigidBodyBuilder::new_static().build(); + let rigid_body = RigidBodyBuilder::new_static(); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, 1.2).build(); + let collider = ColliderBuilder::cuboid(ground_size, 1.2); colliders.insert_with_parent(collider, handle, &mut bodies); let rigid_body = RigidBodyBuilder::new_static() .rotation(std::f32::consts::FRAC_PI_2) - .translation(vector![ground_size, ground_size * 2.0]) - .build(); + .translation(vector![ground_size, ground_size * 2.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size * 2.0, 1.2).build(); + let collider = ColliderBuilder::cuboid(ground_size * 2.0, 1.2); colliders.insert_with_parent(collider, handle, &mut bodies); let rigid_body = RigidBodyBuilder::new_static() .rotation(std::f32::consts::FRAC_PI_2) - .translation(vector![-ground_size, ground_size * 2.0]) - .build(); + .translation(vector![-ground_size, ground_size * 2.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size * 2.0, 1.2).build(); + let collider = ColliderBuilder::cuboid(ground_size * 2.0, 1.2); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -57,9 +55,7 @@ pub fn init_world(testbed: &mut Testbed) { let x = i as f32 * shift - centerx; let y = j as f32 * shift * 2.0 + centery + 2.0; - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]); let handle = bodies.insert(rigid_body); let mut points = Vec::new(); @@ -69,7 +65,7 @@ pub fn init_world(testbed: &mut Testbed) { points.push(pt * scale); } - let collider = ColliderBuilder::convex_hull(&points).unwrap().build(); + let collider = ColliderBuilder::convex_hull(&points).unwrap(); colliders.insert_with_parent(collider, handle, &mut bodies); } } diff --git a/benchmarks2d/heightfield2.rs b/benchmarks2d/heightfield2.rs index 04c56da..60fc45f 100644 --- a/benchmarks2d/heightfield2.rs +++ b/benchmarks2d/heightfield2.rs @@ -25,9 +25,9 @@ pub fn init_world(testbed: &mut Testbed) { } }); - let rigid_body = RigidBodyBuilder::new_static().build(); + let rigid_body = RigidBodyBuilder::new_static(); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::heightfield(heights, ground_size).build(); + let collider = ColliderBuilder::heightfield(heights, ground_size); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -46,16 +46,14 @@ pub fn init_world(testbed: &mut Testbed) { let y = j as f32 * shift + centery + 3.0; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]); let handle = bodies.insert(rigid_body); if j % 2 == 0 { - let collider = ColliderBuilder::cuboid(rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad); colliders.insert_with_parent(collider, handle, &mut bodies); } else { - let collider = ColliderBuilder::ball(rad).build(); + let collider = ColliderBuilder::ball(rad); colliders.insert_with_parent(collider, handle, &mut bodies); } } diff --git a/benchmarks2d/joint_ball2.rs b/benchmarks2d/joint_ball2.rs index e856556..9d819fc 100644 --- a/benchmarks2d/joint_ball2.rs +++ b/benchmarks2d/joint_ball2.rs @@ -32,17 +32,16 @@ pub fn init_world(testbed: &mut Testbed) { RigidBodyType::Dynamic }; - let rigid_body = RigidBodyBuilder::new(status) - .translation(vector![fk * shift, -fi * shift]) - .build(); + let rigid_body = + RigidBodyBuilder::new(status).translation(vector![fk * shift, -fi * shift]); let child_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::ball(rad).build(); + let collider = ColliderBuilder::ball(rad); colliders.insert_with_parent(collider, child_handle, &mut bodies); // Vertical joint. if i > 0 { let parent_handle = *body_handles.last().unwrap(); - let joint = RevoluteJoint::new().local_anchor2(point![0.0, shift]); + let joint = RevoluteJointBuilder::new().local_anchor2(point![0.0, shift]); impulse_joints.insert(parent_handle, child_handle, joint); } @@ -50,7 +49,7 @@ pub fn init_world(testbed: &mut Testbed) { if k > 0 { let parent_index = body_handles.len() - numi; let parent_handle = body_handles[parent_index]; - let joint = RevoluteJoint::new().local_anchor2(point![-shift, 0.0]); + let joint = RevoluteJointBuilder::new().local_anchor2(point![-shift, 0.0]); impulse_joints.insert(parent_handle, child_handle, joint); } diff --git a/benchmarks2d/joint_fixed2.rs b/benchmarks2d/joint_fixed2.rs index 690b8cb..f492bd5 100644 --- a/benchmarks2d/joint_fixed2.rs +++ b/benchmarks2d/joint_fixed2.rs @@ -38,17 +38,16 @@ pub fn init_world(testbed: &mut Testbed) { }; let rigid_body = RigidBodyBuilder::new(status) - .translation(vector![x + fk * shift, y - fi * shift]) - .build(); + .translation(vector![x + fk * shift, y - fi * shift]); let child_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::ball(rad).build(); + let collider = ColliderBuilder::ball(rad); colliders.insert_with_parent(collider, child_handle, &mut bodies); // Vertical joint. if i > 0 { let parent_handle = *body_handles.last().unwrap(); - let joint = - FixedJoint::new().local_frame2(Isometry::translation(0.0, shift)); + let joint = FixedJointBuilder::new() + .local_frame2(Isometry::translation(0.0, shift)); impulse_joints.insert(parent_handle, child_handle, joint); } @@ -56,8 +55,8 @@ pub fn init_world(testbed: &mut Testbed) { if k > 0 { let parent_index = body_handles.len() - num; let parent_handle = body_handles[parent_index]; - let joint = - FixedJoint::new().local_frame2(Isometry::translation(-shift, 0.0)); + let joint = FixedJointBuilder::new() + .local_frame2(Isometry::translation(-shift, 0.0)); impulse_joints.insert(parent_handle, child_handle, joint); } diff --git a/benchmarks2d/joint_prismatic2.rs b/benchmarks2d/joint_prismatic2.rs index 20d423e..65e7fe1 100644 --- a/benchmarks2d/joint_prismatic2.rs +++ b/benchmarks2d/joint_prismatic2.rs @@ -24,21 +24,17 @@ pub fn init_world(testbed: &mut Testbed) { for j in 0..50 { let x = j as f32 * shift * 4.0; - let ground = RigidBodyBuilder::new_static() - .translation(vector![x, y]) - .build(); + let ground = RigidBodyBuilder::new_static().translation(vector![x, y]); let mut curr_parent = bodies.insert(ground); - let collider = ColliderBuilder::cuboid(rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad); colliders.insert_with_parent(collider, curr_parent, &mut bodies); for i in 0..num { let y = y - (i + 1) as f32 * shift; let density = 1.0; - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]); let curr_child = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad).density(density).build(); + let collider = ColliderBuilder::cuboid(rad, rad).density(density); colliders.insert_with_parent(collider, curr_child, &mut bodies); let axis = if i % 2 == 0 { @@ -47,9 +43,9 @@ pub fn init_world(testbed: &mut Testbed) { UnitVector::new_normalize(vector![-1.0, 1.0]) }; - let prism = PrismaticJoint::new(axis) + let prism = PrismaticJointBuilder::new(axis) .local_anchor2(point![0.0, shift]) - .limit_axis([-1.5, 1.5]); + .limits([-1.5, 1.5]); impulse_joints.insert(curr_parent, curr_child, prism); curr_parent = curr_child; diff --git a/benchmarks2d/pyramid2.rs b/benchmarks2d/pyramid2.rs index 61636b9..7453344 100644 --- a/benchmarks2d/pyramid2.rs +++ b/benchmarks2d/pyramid2.rs @@ -16,9 +16,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 100.0; let ground_thickness = 1.0; - let rigid_body = RigidBodyBuilder::new_static().build(); + let rigid_body = RigidBodyBuilder::new_static(); let ground_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_thickness).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_thickness); colliders.insert_with_parent(collider, ground_handle, &mut bodies); /* @@ -39,11 +39,9 @@ pub fn init_world(testbed: &mut Testbed) { let y = fi * shift + centery; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad); colliders.insert_with_parent(collider, handle, &mut bodies); } } diff --git a/benchmarks3d/balls3.rs b/benchmarks3d/balls3.rs index b4d5102..4ccf594 100644 --- a/benchmarks3d/balls3.rs +++ b/benchmarks3d/balls3.rs @@ -36,11 +36,9 @@ pub fn init_world(testbed: &mut Testbed) { let density = 0.477; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new(status) - .translation(vector![x, y, z]) - .build(); + let rigid_body = RigidBodyBuilder::new(status).translation(vector![x, y, z]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::ball(rad).density(density).build(); + let collider = ColliderBuilder::ball(rad).density(density); colliders.insert_with_parent(collider, handle, &mut bodies); } } diff --git a/benchmarks3d/boxes3.rs b/benchmarks3d/boxes3.rs index 0250619..e816a63 100644 --- a/benchmarks3d/boxes3.rs +++ b/benchmarks3d/boxes3.rs @@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 200.1; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -44,11 +42,9 @@ pub fn init_world(testbed: &mut Testbed) { let z = k as f32 * shift - centerz + offset; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y, z]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, handle, &mut bodies); } } diff --git a/benchmarks3d/capsules3.rs b/benchmarks3d/capsules3.rs index 5b05f23..ec00e2d 100644 --- a/benchmarks3d/capsules3.rs +++ b/benchmarks3d/capsules3.rs @@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 200.1; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -45,11 +43,9 @@ pub fn init_world(testbed: &mut Testbed) { let z = k as f32 * shift - centerz + offset; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y, z]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::capsule_y(rad, rad).build(); + let collider = ColliderBuilder::capsule_y(rad, rad); colliders.insert_with_parent(collider, handle, &mut bodies); } } diff --git a/benchmarks3d/ccd3.rs b/benchmarks3d/ccd3.rs index d54ebc1..ed137a2 100644 --- a/benchmarks3d/ccd3.rs +++ b/benchmarks3d/ccd3.rs @@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 100.1; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -50,8 +48,7 @@ pub fn init_world(testbed: &mut Testbed) { let rigid_body = RigidBodyBuilder::new_dynamic() .translation(vector![x, y, z]) .linvel(vector![0.0, -1000.0, 0.0]) - .ccd_enabled(true) - .build(); + .ccd_enabled(true); let handle = bodies.insert(rigid_body); let collider = match j % 5 { @@ -64,7 +61,7 @@ pub fn init_world(testbed: &mut Testbed) { _ => ColliderBuilder::capsule_y(rad, rad), }; - colliders.insert_with_parent(collider.build(), handle, &mut bodies); + colliders.insert_with_parent(collider, handle, &mut bodies); } } diff --git a/benchmarks3d/compound3.rs b/benchmarks3d/compound3.rs index 872923c..04e8c78 100644 --- a/benchmarks3d/compound3.rs +++ b/benchmarks3d/compound3.rs @@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 200.1; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -44,17 +42,19 @@ pub fn init_world(testbed: &mut Testbed) { let z = k as f32 * shift * 2.0 - centerz + offset; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y, z]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]); let handle = bodies.insert(rigid_body); - let collider1 = ColliderBuilder::cuboid(rad * 10.0, rad, rad).build(); - let collider2 = ColliderBuilder::cuboid(rad, rad * 10.0, rad) - .translation(vector![rad * 10.0, rad * 10.0, 0.0]) - .build(); - let collider3 = ColliderBuilder::cuboid(rad, rad * 10.0, rad) - .translation(vector![-rad * 10.0, rad * 10.0, 0.0]) - .build(); + let collider1 = ColliderBuilder::cuboid(rad * 10.0, rad, rad); + let collider2 = ColliderBuilder::cuboid(rad, rad * 10.0, rad).translation(vector![ + rad * 10.0, + rad * 10.0, + 0.0 + ]); + let collider3 = ColliderBuilder::cuboid(rad, rad * 10.0, rad).translation(vector![ + -rad * 10.0, + rad * 10.0, + 0.0 + ]); colliders.insert_with_parent(collider1, handle, &mut bodies); colliders.insert_with_parent(collider2, handle, &mut bodies); colliders.insert_with_parent(collider3, handle, &mut bodies); diff --git a/benchmarks3d/convex_polyhedron3.rs b/benchmarks3d/convex_polyhedron3.rs index cb834ea..5d9f363 100644 --- a/benchmarks3d/convex_polyhedron3.rs +++ b/benchmarks3d/convex_polyhedron3.rs @@ -18,11 +18,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 200.1; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -57,13 +55,9 @@ pub fn init_world(testbed: &mut Testbed) { } // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y, z]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::round_convex_hull(&points, border_rad) - .unwrap() - .build(); + let collider = ColliderBuilder::round_convex_hull(&points, border_rad).unwrap(); colliders.insert_with_parent(collider, handle, &mut bodies); } } diff --git a/benchmarks3d/heightfield3.rs b/benchmarks3d/heightfield3.rs index b95f1ee..6bf6fc0 100644 --- a/benchmarks3d/heightfield3.rs +++ b/benchmarks3d/heightfield3.rs @@ -31,9 +31,9 @@ pub fn init_world(testbed: &mut Testbed) { } }); - let rigid_body = RigidBodyBuilder::new_static().build(); + let rigid_body = RigidBodyBuilder::new_static(); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::heightfield(heights, ground_size).build(); + let collider = ColliderBuilder::heightfield(heights, ground_size); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -55,16 +55,14 @@ pub fn init_world(testbed: &mut Testbed) { let z = k as f32 * shift - centerz; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y, z]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]); let handle = bodies.insert(rigid_body); if j % 2 == 0 { - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, handle, &mut bodies); } else { - let collider = ColliderBuilder::ball(rad).build(); + let collider = ColliderBuilder::ball(rad); colliders.insert_with_parent(collider, handle, &mut bodies); } } diff --git a/benchmarks3d/joint_ball3.rs b/benchmarks3d/joint_ball3.rs index 64128ba..4b509af 100644 --- a/benchmarks3d/joint_ball3.rs +++ b/benchmarks3d/joint_ball3.rs @@ -27,17 +27,16 @@ pub fn init_world(testbed: &mut Testbed) { RigidBodyType::Dynamic }; - let rigid_body = RigidBodyBuilder::new(status) - .translation(vector![fk * shift, 0.0, fi * shift]) - .build(); + let rigid_body = + RigidBodyBuilder::new(status).translation(vector![fk * shift, 0.0, fi * shift]); let child_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::ball(rad).build(); + let collider = ColliderBuilder::ball(rad); colliders.insert_with_parent(collider, child_handle, &mut bodies); // Vertical joint. if i > 0 { let parent_handle = *body_handles.last().unwrap(); - let joint = SphericalJoint::new().local_anchor2(point![0.0, 0.0, -shift]); + let joint = SphericalJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift]); impulse_joints.insert(parent_handle, child_handle, joint); } @@ -45,7 +44,7 @@ pub fn init_world(testbed: &mut Testbed) { if k > 0 { let parent_index = body_handles.len() - num; let parent_handle = body_handles[parent_index]; - let joint = SphericalJoint::new().local_anchor2(point![-shift, 0.0, 0.0]); + let joint = SphericalJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]); impulse_joints.insert(parent_handle, child_handle, joint); } diff --git a/benchmarks3d/joint_fixed3.rs b/benchmarks3d/joint_fixed3.rs index b3f4039..5fdba9c 100644 --- a/benchmarks3d/joint_fixed3.rs +++ b/benchmarks3d/joint_fixed3.rs @@ -40,17 +40,20 @@ pub fn init_world(testbed: &mut Testbed) { RigidBodyType::Dynamic }; - let rigid_body = RigidBodyBuilder::new(status) - .translation(vector![x + fk * shift, y, z + fi * shift]) - .build(); + let rigid_body = RigidBodyBuilder::new(status).translation(vector![ + x + fk * shift, + y, + z + fi * shift + ]); let child_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::ball(rad).build(); + let collider = ColliderBuilder::ball(rad); colliders.insert_with_parent(collider, child_handle, &mut bodies); // Vertical joint. if i > 0 { let parent_handle = *body_handles.last().unwrap(); - let joint = FixedJoint::new().local_anchor2(point![0.0, 0.0, -shift]); + let joint = + FixedJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift]); impulse_joints.insert(parent_handle, child_handle, joint); } @@ -58,7 +61,8 @@ pub fn init_world(testbed: &mut Testbed) { if k > 0 { let parent_index = body_handles.len() - num; let parent_handle = body_handles[parent_index]; - let joint = FixedJoint::new().local_anchor2(point![-shift, 0.0, 0.0]); + let joint = + FixedJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]); impulse_joints.insert(parent_handle, child_handle, joint); } diff --git a/benchmarks3d/joint_prismatic3.rs b/benchmarks3d/joint_prismatic3.rs index 80839d7..faa5f93 100644 --- a/benchmarks3d/joint_prismatic3.rs +++ b/benchmarks3d/joint_prismatic3.rs @@ -23,23 +23,17 @@ pub fn init_world(testbed: &mut Testbed) { for j in 0..50 { let x = j as f32 * shift * 4.0; - let ground = RigidBodyBuilder::new_static() - .translation(vector![x, y, z]) - .build(); + let ground = RigidBodyBuilder::new_static().translation(vector![x, y, z]); let mut curr_parent = bodies.insert(ground); - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, curr_parent, &mut bodies); for i in 0..num { let z = z + (i + 1) as f32 * shift; let density = 1.0; - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y, z]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]); let curr_child = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad, rad) - .density(density) - .build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad).density(density); colliders.insert_with_parent(collider, curr_child, &mut bodies); let axis = if i % 2 == 0 { @@ -48,9 +42,9 @@ pub fn init_world(testbed: &mut Testbed) { UnitVector::new_normalize(vector![-1.0, 1.0, 0.0]) }; - let prism = PrismaticJoint::new(axis) + let prism = PrismaticJointBuilder::new(axis) .local_anchor2(point![0.0, 0.0, -shift]) - .limit_axis([-2.0, 0.0]); + .limits([-2.0, 0.0]); impulse_joints.insert(curr_parent, curr_child, prism); curr_parent = curr_child; diff --git a/benchmarks3d/joint_revolute3.rs b/benchmarks3d/joint_revolute3.rs index 8bdf0e9..aa1be50 100644 --- a/benchmarks3d/joint_revolute3.rs +++ b/benchmarks3d/joint_revolute3.rs @@ -20,11 +20,9 @@ pub fn init_world(testbed: &mut Testbed) { for j in 0..50 { let x = j as f32 * shift * 4.0; - let ground = RigidBodyBuilder::new_static() - .translation(vector![x, y, 0.0]) - .build(); + let ground = RigidBodyBuilder::new_static().translation(vector![x, y, 0.0]); let mut curr_parent = bodies.insert(ground); - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, curr_parent, &mut bodies); for i in 0..num { @@ -40,13 +38,9 @@ pub fn init_world(testbed: &mut Testbed) { let mut handles = [curr_parent; 4]; for k in 0..4 { let density = 1.0; - let rigid_body = RigidBodyBuilder::new_dynamic() - .position(positions[k]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().position(positions[k]); handles[k] = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad, rad) - .density(density) - .build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad).density(density); colliders.insert_with_parent(collider, handles[k], &mut bodies); } @@ -55,10 +49,10 @@ pub fn init_world(testbed: &mut Testbed) { let z = Vector::z_axis(); let revs = [ - RevoluteJoint::new(z).local_anchor2(point![0.0, 0.0, -shift]), - RevoluteJoint::new(x).local_anchor2(point![-shift, 0.0, 0.0]), - RevoluteJoint::new(z).local_anchor2(point![0.0, 0.0, -shift]), - RevoluteJoint::new(x).local_anchor2(point![shift, 0.0, 0.0]), + RevoluteJointBuilder::new(z).local_anchor2(point![0.0, 0.0, -shift]), + RevoluteJointBuilder::new(x).local_anchor2(point![-shift, 0.0, 0.0]), + RevoluteJointBuilder::new(z).local_anchor2(point![0.0, 0.0, -shift]), + RevoluteJointBuilder::new(x).local_anchor2(point![shift, 0.0, 0.0]), ]; impulse_joints.insert(curr_parent, handles[0], revs[0]); diff --git a/benchmarks3d/keva3.rs b/benchmarks3d/keva3.rs index ad9e1ae..30e5246 100644 --- a/benchmarks3d/keva3.rs +++ b/benchmarks3d/keva3.rs @@ -38,15 +38,13 @@ pub fn build_block( }; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![ - x + dim.x + shift.x, - y + dim.y + shift.y, - z + dim.z + shift.z - ]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![ + x + dim.x + shift.x, + y + dim.y + shift.y, + z + dim.z + shift.z + ]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z).build(); + let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z); colliders.insert_with_parent(collider, handle, bodies); testbed.set_initial_body_color(handle, color0); @@ -61,15 +59,13 @@ pub fn build_block( for i in 0..(block_width / (dim.x as f32 * 2.0)) as usize { for j in 0..(block_width / (dim.z as f32 * 2.0)) as usize { // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![ - i as f32 * dim.x * 2.0 + dim.x + shift.x, - dim.y + shift.y + block_height, - j as f32 * dim.z * 2.0 + dim.z + shift.z - ]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![ + i as f32 * dim.x * 2.0 + dim.x + shift.x, + dim.y + shift.y + block_height, + j as f32 * dim.z * 2.0 + dim.z + shift.z + ]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z).build(); + let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z); colliders.insert_with_parent(collider, handle, bodies); testbed.set_initial_body_color(handle, color0); std::mem::swap(&mut color0, &mut color1); @@ -92,11 +88,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 50.0; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); colliders.insert_with_parent(collider, handle, &mut bodies); /* diff --git a/benchmarks3d/pyramid3.rs b/benchmarks3d/pyramid3.rs index b378da4..572db45 100644 --- a/benchmarks3d/pyramid3.rs +++ b/benchmarks3d/pyramid3.rs @@ -22,13 +22,11 @@ fn create_pyramid( - stack_height as f32 * half_extents.z; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y, z]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]); let rigid_body_handle = bodies.insert(rigid_body); let collider = - ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z).build(); + ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z); colliders.insert_with_parent(collider, rigid_body_handle, bodies); } } @@ -50,11 +48,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 50.0; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let ground_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); colliders.insert_with_parent(collider, ground_handle, &mut bodies); /* diff --git a/benchmarks3d/stacks3.rs b/benchmarks3d/stacks3.rs index 7fc9097..ff67917 100644 --- a/benchmarks3d/stacks3.rs +++ b/benchmarks3d/stacks3.rs @@ -23,10 +23,9 @@ fn create_tower_circle( * Translation::new(0.0, y, radius); // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic().position(pos).build(); + let rigid_body = RigidBodyBuilder::new_dynamic().position(pos); let handle = bodies.insert(rigid_body); - let collider = - ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z).build(); + let collider = ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z); colliders.insert_with_parent(collider, handle, bodies); } } @@ -50,12 +49,9 @@ fn create_wall( - stack_height as f32 * half_extents.z; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y, z]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]); let handle = bodies.insert(rigid_body); - let collider = - ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z).build(); + let collider = ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z); colliders.insert_with_parent(collider, handle, bodies); } } @@ -83,12 +79,10 @@ fn create_pyramid( - stack_height as f32 * half_extents.z; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y, z]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]); let handle = bodies.insert(rigid_body); let collider = - ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z).build(); + ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z); colliders.insert_with_parent(collider, handle, bodies); } } @@ -110,11 +104,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 200.0; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); colliders.insert_with_parent(collider, handle, &mut bodies); /* diff --git a/benchmarks3d/trimesh3.rs b/benchmarks3d/trimesh3.rs index 1039a09..e9d73ee 100644 --- a/benchmarks3d/trimesh3.rs +++ b/benchmarks3d/trimesh3.rs @@ -36,9 +36,9 @@ pub fn init_world(testbed: &mut Testbed) { let heightfield = HeightField::new(heights, ground_size); let (vertices, indices) = heightfield.to_trimesh(); - let rigid_body = RigidBodyBuilder::new_static().build(); + let rigid_body = RigidBodyBuilder::new_static(); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::trimesh(vertices, indices).build(); + let collider = ColliderBuilder::trimesh(vertices, indices); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -60,16 +60,14 @@ pub fn init_world(testbed: &mut Testbed) { let z = k as f32 * shift - centerz; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y, z]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]); let handle = bodies.insert(rigid_body); if j % 2 == 0 { - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, handle, &mut bodies); } else { - let collider = ColliderBuilder::ball(rad).build(); + let collider = ColliderBuilder::ball(rad); colliders.insert_with_parent(collider, handle, &mut bodies); } } diff --git a/examples2d/add_remove2.rs b/examples2d/add_remove2.rs index 0919da9..dfc96d9 100644 --- a/examples2d/add_remove2.rs +++ b/examples2d/add_remove2.rs @@ -14,11 +14,9 @@ pub fn init_world(testbed: &mut Testbed) { let platform_handles = positions .into_iter() .map(|pos| { - let rigid_body = RigidBodyBuilder::new_kinematic_position_based() - .translation(pos) - .build(); + let rigid_body = RigidBodyBuilder::new_kinematic_position_based().translation(pos); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad * 10.0, rad).build(); + let collider = ColliderBuilder::cuboid(rad * 10.0, rad); colliders.insert_with_parent(collider, handle, &mut bodies); handle }) @@ -35,11 +33,9 @@ pub fn init_world(testbed: &mut Testbed) { if state.timestep_id % 10 == 0 { let x = rand::random::() * 10.0 - 5.0; let y = rand::random::() * 10.0 + 10.0; - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]); let handle = physics.bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad); physics .colliders .insert_with_parent(collider, handle, &mut physics.bodies); diff --git a/examples2d/ccd2.rs b/examples2d/ccd2.rs index 808c2e5..0e64713 100644 --- a/examples2d/ccd2.rs +++ b/examples2d/ccd2.rs @@ -16,27 +16,24 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 25.0; let ground_thickness = 0.1; - let rigid_body = RigidBodyBuilder::new_static().ccd_enabled(true).build(); + let rigid_body = RigidBodyBuilder::new_static().ccd_enabled(true); let ground_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_thickness).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_thickness); colliders.insert_with_parent(collider, ground_handle, &mut bodies); - let collider = ColliderBuilder::cuboid(ground_thickness, ground_size) - .translation(vector![-3.0, 0.0]) - .build(); + let collider = + ColliderBuilder::cuboid(ground_thickness, ground_size).translation(vector![-3.0, 0.0]); colliders.insert_with_parent(collider, ground_handle, &mut bodies); - let collider = ColliderBuilder::cuboid(ground_thickness, ground_size) - .translation(vector![6.0, 0.0]) - .build(); + let collider = + ColliderBuilder::cuboid(ground_thickness, ground_size).translation(vector![6.0, 0.0]); colliders.insert_with_parent(collider, ground_handle, &mut bodies); let collider = ColliderBuilder::cuboid(ground_thickness, ground_size) .translation(vector![2.5, 0.0]) .sensor(true) - .active_events(ActiveEvents::INTERSECTION_EVENTS) - .build(); + .active_events(ActiveEvents::INTERSECTION_EVENTS); let sensor_handle = colliders.insert_with_parent(collider, ground_handle, &mut bodies); /* @@ -72,19 +69,18 @@ pub fn init_world(testbed: &mut Testbed) { let rigid_body = RigidBodyBuilder::new_dynamic() .translation(vector![x, y]) .linvel(vector![100.0, -10.0]) - .ccd_enabled(true) - .build(); + .ccd_enabled(true); let handle = bodies.insert(rigid_body); // for part in &compound_parts { // let collider = ColliderBuilder::new(part.1.clone()) // .position_wrt_parent(part.0) - // .build(); + // ; // colliders.insert_with_parent(collider, handle, &mut bodies); // } - let collider = ColliderBuilder::new(compound_shape.clone()).build(); - // let collider = ColliderBuilder::cuboid(radx, rady).build(); + let collider = ColliderBuilder::new(compound_shape.clone()); + // let collider = ColliderBuilder::cuboid(radx, rady); colliders.insert_with_parent(collider, handle, &mut bodies); } } @@ -114,6 +110,7 @@ pub fn init_world(testbed: &mut Testbed) { if parent_handle1 != ground_handle && prox.collider1 != sensor_handle { graphics.set_body_color(parent_handle1, color); } + if parent_handle2 != ground_handle && prox.collider2 != sensor_handle { graphics.set_body_color(parent_handle2, color); } diff --git a/examples2d/collision_groups2.rs b/examples2d/collision_groups2.rs index 110c32a..9c9cd30 100644 --- a/examples2d/collision_groups2.rs +++ b/examples2d/collision_groups2.rs @@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 5.0; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height]); let floor_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height); colliders.insert_with_parent(collider, floor_handle, &mut bodies); /* @@ -34,8 +32,7 @@ pub fn init_world(testbed: &mut Testbed) { */ let green_floor = ColliderBuilder::cuboid(1.0, 0.1) .translation(vector![0.0, 1.0]) - .collision_groups(GREEN_GROUP) - .build(); + .collision_groups(GREEN_GROUP); let green_collider_handle = colliders.insert_with_parent(green_floor, floor_handle, &mut bodies); @@ -46,8 +43,7 @@ pub fn init_world(testbed: &mut Testbed) { */ let blue_floor = ColliderBuilder::cuboid(1.0, 0.1) .translation(vector![0.0, 2.0]) - .collision_groups(BLUE_GROUP) - .build(); + .collision_groups(BLUE_GROUP); let blue_collider_handle = colliders.insert_with_parent(blue_floor, floor_handle, &mut bodies); testbed.set_initial_collider_color(blue_collider_handle, [0.0, 0.0, 1.0]); @@ -74,13 +70,9 @@ pub fn init_world(testbed: &mut Testbed) { (BLUE_GROUP, [0.0, 0.0, 1.0]) }; - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad) - .collision_groups(group) - .build(); + let collider = ColliderBuilder::cuboid(rad, rad).collision_groups(group); colliders.insert_with_parent(collider, handle, &mut bodies); testbed.set_initial_body_color(handle, color); diff --git a/examples2d/convex_polygons2.rs b/examples2d/convex_polygons2.rs index 060e0c5..b3139c4 100644 --- a/examples2d/convex_polygons2.rs +++ b/examples2d/convex_polygons2.rs @@ -17,25 +17,23 @@ pub fn init_world(testbed: &mut Testbed) { */ let ground_size = 30.0; - let rigid_body = RigidBodyBuilder::new_static().build(); + let rigid_body = RigidBodyBuilder::new_static(); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, 1.2).build(); + let collider = ColliderBuilder::cuboid(ground_size, 1.2); colliders.insert_with_parent(collider, handle, &mut bodies); let rigid_body = RigidBodyBuilder::new_static() .rotation(std::f32::consts::FRAC_PI_2) - .translation(vector![ground_size, ground_size * 2.0]) - .build(); + .translation(vector![ground_size, ground_size * 2.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size * 2.0, 1.2).build(); + let collider = ColliderBuilder::cuboid(ground_size * 2.0, 1.2); colliders.insert_with_parent(collider, handle, &mut bodies); let rigid_body = RigidBodyBuilder::new_static() .rotation(std::f32::consts::FRAC_PI_2) - .translation(vector![-ground_size, ground_size * 2.0]) - .build(); + .translation(vector![-ground_size, ground_size * 2.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size * 2.0, 1.2).build(); + let collider = ColliderBuilder::cuboid(ground_size * 2.0, 1.2); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -57,9 +55,7 @@ pub fn init_world(testbed: &mut Testbed) { let x = i as f32 * shift - centerx; let y = j as f32 * shift * 2.0 + centery + 2.0; - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]); let handle = bodies.insert(rigid_body); let mut points = Vec::new(); @@ -69,7 +65,7 @@ pub fn init_world(testbed: &mut Testbed) { points.push(pt * scale); } - let collider = ColliderBuilder::convex_hull(&points).unwrap().build(); + let collider = ColliderBuilder::convex_hull(&points).unwrap(); colliders.insert_with_parent(collider, handle, &mut bodies); } } diff --git a/examples2d/damping2.rs b/examples2d/damping2.rs index 481fee2..689190c 100644 --- a/examples2d/damping2.rs +++ b/examples2d/damping2.rs @@ -27,12 +27,11 @@ pub fn init_world(testbed: &mut Testbed) { .linvel(vector![x * 10.0, y * 10.0]) .angvel(100.0) .linear_damping((i + 1) as f32 * subdiv * 10.0) - .angular_damping((num - i) as f32 * subdiv * 10.0) - .build(); + .angular_damping((num - i) as f32 * subdiv * 10.0); let rb_handle = bodies.insert(rb); // Build the collider. - let co = ColliderBuilder::cuboid(rad, rad).build(); + let co = ColliderBuilder::cuboid(rad, rad); colliders.insert_with_parent(co, rb_handle, &mut bodies); } diff --git a/examples2d/debug_box_ball2.rs b/examples2d/debug_box_ball2.rs index 8f15b7a..e071c61 100644 --- a/examples2d/debug_box_ball2.rs +++ b/examples2d/debug_box_ball2.rs @@ -16,19 +16,17 @@ pub fn init_world(testbed: &mut Testbed) { let rad = 1.0; let rigid_body = RigidBodyBuilder::new_static() .translation(vector![0.0, -rad]) - .rotation(std::f32::consts::PI / 4.0) - .build(); + .rotation(std::f32::consts::PI / 4.0); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad); colliders.insert_with_parent(collider, handle, &mut bodies); // Build the dynamic box rigid body. let rigid_body = RigidBodyBuilder::new_dynamic() .translation(vector![0.0, 3.0 * rad]) - .can_sleep(false) - .build(); + .can_sleep(false); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::ball(rad).build(); + let collider = ColliderBuilder::ball(rad); colliders.insert_with_parent(collider, handle, &mut bodies); /* diff --git a/examples2d/drum2.rs b/examples2d/drum2.rs index 9bc8a37..57c891f 100644 --- a/examples2d/drum2.rs +++ b/examples2d/drum2.rs @@ -26,11 +26,9 @@ pub fn init_world(testbed: &mut Testbed) { let y = j as f32 * shift - centery; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad); colliders.insert_with_parent(collider, handle, &mut bodies); } } @@ -38,7 +36,7 @@ pub fn init_world(testbed: &mut Testbed) { /* * Setup a velocity-based kinematic rigid body. */ - let platform_body = RigidBodyBuilder::new_kinematic_velocity_based().build(); + let platform_body = RigidBodyBuilder::new_kinematic_velocity_based(); let velocity_based_platform_handle = bodies.insert(platform_body); let sides = [ @@ -55,11 +53,11 @@ pub fn init_world(testbed: &mut Testbed) { ]; for (hx, hy, pos) in sides { - let collider = ColliderBuilder::cuboid(hx, hy).translation(pos).build(); + let collider = ColliderBuilder::cuboid(hx, hy).translation(pos); colliders.insert_with_parent(collider, velocity_based_platform_handle, &mut bodies); } for (r, pos) in balls { - let collider = ColliderBuilder::ball(r).translation(pos).build(); + let collider = ColliderBuilder::ball(r).translation(pos); colliders.insert_with_parent(collider, velocity_based_platform_handle, &mut bodies); } diff --git a/examples2d/heightfield2.rs b/examples2d/heightfield2.rs index 09c604d..65ad76c 100644 --- a/examples2d/heightfield2.rs +++ b/examples2d/heightfield2.rs @@ -25,9 +25,9 @@ pub fn init_world(testbed: &mut Testbed) { } }); - let rigid_body = RigidBodyBuilder::new_static().build(); + let rigid_body = RigidBodyBuilder::new_static(); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::heightfield(heights, ground_size).build(); + let collider = ColliderBuilder::heightfield(heights, ground_size); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -46,16 +46,14 @@ pub fn init_world(testbed: &mut Testbed) { let y = j as f32 * shift + centery + 3.0; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]); let handle = bodies.insert(rigid_body); if j % 2 == 0 { - let collider = ColliderBuilder::cuboid(rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad); colliders.insert_with_parent(collider, handle, &mut bodies); } else { - let collider = ColliderBuilder::ball(rad).build(); + let collider = ColliderBuilder::ball(rad); colliders.insert_with_parent(collider, handle, &mut bodies); } } diff --git a/examples2d/joints2.rs b/examples2d/joints2.rs index 91d86cd..7a7119e 100644 --- a/examples2d/joints2.rs +++ b/examples2d/joints2.rs @@ -35,17 +35,16 @@ pub fn init_world(testbed: &mut Testbed) { RigidBodyType::Dynamic }; - let rigid_body = RigidBodyBuilder::new(status) - .translation(vector![fk * shift, -fi * shift]) - .build(); + let rigid_body = + RigidBodyBuilder::new(status).translation(vector![fk * shift, -fi * shift]); let child_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::ball(rad).build(); + let collider = ColliderBuilder::ball(rad); colliders.insert_with_parent(collider, child_handle, &mut bodies); // Vertical joint. if i > 0 { let parent_handle = *body_handles.last().unwrap(); - let joint = RevoluteJoint::new().local_anchor2(point![0.0, shift]); + let joint = RevoluteJointBuilder::new().local_anchor2(point![0.0, shift]); impulse_joints.insert(parent_handle, child_handle, joint); } @@ -53,7 +52,7 @@ pub fn init_world(testbed: &mut Testbed) { if k > 0 { let parent_index = body_handles.len() - numi; let parent_handle = body_handles[parent_index]; - let joint = RevoluteJoint::new().local_anchor2(point![-shift, 0.0]); + let joint = RevoluteJointBuilder::new().local_anchor2(point![-shift, 0.0]); impulse_joints.insert(parent_handle, child_handle, joint); } diff --git a/examples2d/locked_rotations2.rs b/examples2d/locked_rotations2.rs index 32c528f..c9d74be 100644 --- a/examples2d/locked_rotations2.rs +++ b/examples2d/locked_rotations2.rs @@ -19,11 +19,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 5.0; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -31,10 +29,9 @@ pub fn init_world(testbed: &mut Testbed) { */ let rigid_body = RigidBodyBuilder::new_dynamic() .translation(vector![0.0, 3.0]) - .lock_translations() - .build(); + .lock_translations(); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(2.0, 0.6).build(); + let collider = ColliderBuilder::cuboid(2.0, 0.6); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -43,10 +40,9 @@ pub fn init_world(testbed: &mut Testbed) { let rigid_body = RigidBodyBuilder::new_dynamic() .translation(vector![0.0, 5.0]) .rotation(1.0) - .lock_rotations() - .build(); + .lock_rotations(); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::capsule_y(0.6, 0.4).build(); + let collider = ColliderBuilder::capsule_y(0.6, 0.4); colliders.insert_with_parent(collider, handle, &mut bodies); /* diff --git a/examples2d/one_way_platforms2.rs b/examples2d/one_way_platforms2.rs index 2bed57d..dae3f39 100644 --- a/examples2d/one_way_platforms2.rs +++ b/examples2d/one_way_platforms2.rs @@ -68,18 +68,16 @@ pub fn init_world(testbed: &mut Testbed) { /* * Ground */ - let rigid_body = RigidBodyBuilder::new_static().build(); + let rigid_body = RigidBodyBuilder::new_static(); let handle = bodies.insert(rigid_body); let collider = ColliderBuilder::cuboid(25.0, 0.5) .translation(vector![30.0, 2.0]) - .active_hooks(ActiveHooks::MODIFY_SOLVER_CONTACTS) - .build(); + .active_hooks(ActiveHooks::MODIFY_SOLVER_CONTACTS); let platform1 = colliders.insert_with_parent(collider, handle, &mut bodies); let collider = ColliderBuilder::cuboid(25.0, 0.5) .translation(vector![-30.0, -2.0]) - .active_hooks(ActiveHooks::MODIFY_SOLVER_CONTACTS) - .build(); + .active_hooks(ActiveHooks::MODIFY_SOLVER_CONTACTS); let platform2 = colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -97,10 +95,8 @@ pub fn init_world(testbed: &mut Testbed) { testbed.add_callback(move |graphics, physics, _, run_state| { if run_state.timestep_id % 50 == 0 && physics.bodies.len() <= 7 { // Spawn a new cube. - let collider = ColliderBuilder::cuboid(1.5, 2.0).build(); - let body = RigidBodyBuilder::new_dynamic() - .translation(vector![20.0, 10.0]) - .build(); + let collider = ColliderBuilder::cuboid(1.5, 2.0); + let body = RigidBodyBuilder::new_dynamic().translation(vector![20.0, 10.0]); let handle = physics.bodies.insert(body); physics .colliders diff --git a/examples2d/platform2.rs b/examples2d/platform2.rs index a737e34..325e7ce 100644 --- a/examples2d/platform2.rs +++ b/examples2d/platform2.rs @@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 10.0; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -39,11 +37,9 @@ pub fn init_world(testbed: &mut Testbed) { let y = j as f32 * shift + centery; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad); colliders.insert_with_parent(collider, handle, &mut bodies); } } @@ -52,20 +48,18 @@ pub fn init_world(testbed: &mut Testbed) { * Setup a position-based kinematic rigid body. */ let platform_body = RigidBodyBuilder::new_kinematic_velocity_based() - .translation(vector![-10.0 * rad, 1.5 + 0.8]) - .build(); + .translation(vector![-10.0 * rad, 1.5 + 0.8]); let velocity_based_platform_handle = bodies.insert(platform_body); - let collider = ColliderBuilder::cuboid(rad * 10.0, rad).build(); + let collider = ColliderBuilder::cuboid(rad * 10.0, rad); colliders.insert_with_parent(collider, velocity_based_platform_handle, &mut bodies); /* * Setup a velocity-based kinematic rigid body. */ let platform_body = RigidBodyBuilder::new_kinematic_position_based() - .translation(vector![-10.0 * rad, 2.0 + 1.5 + 0.8]) - .build(); + .translation(vector![-10.0 * rad, 2.0 + 1.5 + 0.8]); let position_based_platform_handle = bodies.insert(platform_body); - let collider = ColliderBuilder::cuboid(rad * 10.0, rad).build(); + let collider = ColliderBuilder::cuboid(rad * 10.0, rad); colliders.insert_with_parent(collider, position_based_platform_handle, &mut bodies); /* diff --git a/examples2d/polyline2.rs b/examples2d/polyline2.rs index 2cde60c..35253e0 100644 --- a/examples2d/polyline2.rs +++ b/examples2d/polyline2.rs @@ -27,9 +27,9 @@ pub fn init_world(testbed: &mut Testbed) { } points.push(point![ground_size / 2.0, 40.0]); - let rigid_body = RigidBodyBuilder::new_static().build(); + let rigid_body = RigidBodyBuilder::new_static(); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::polyline(points, None).build(); + let collider = ColliderBuilder::polyline(points, None); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -48,16 +48,14 @@ pub fn init_world(testbed: &mut Testbed) { let y = j as f32 * shift + centery + 3.0; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]); let handle = bodies.insert(rigid_body); if j % 2 == 0 { - let collider = ColliderBuilder::cuboid(rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad); colliders.insert_with_parent(collider, handle, &mut bodies); } else { - let collider = ColliderBuilder::ball(rad).build(); + let collider = ColliderBuilder::ball(rad); colliders.insert_with_parent(collider, handle, &mut bodies); } } diff --git a/examples2d/pyramid2.rs b/examples2d/pyramid2.rs index 04a7953..1f1440c 100644 --- a/examples2d/pyramid2.rs +++ b/examples2d/pyramid2.rs @@ -16,9 +16,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 10.0; let ground_thickness = 1.0; - let rigid_body = RigidBodyBuilder::new_static().build(); + let rigid_body = RigidBodyBuilder::new_static(); let ground_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_thickness).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_thickness); colliders.insert_with_parent(collider, ground_handle, &mut bodies); /* @@ -39,11 +39,9 @@ pub fn init_world(testbed: &mut Testbed) { let y = fi * shift + centery; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad); colliders.insert_with_parent(collider, handle, &mut bodies); } } diff --git a/examples2d/restitution2.rs b/examples2d/restitution2.rs index 2650ec2..428c6c4 100644 --- a/examples2d/restitution2.rs +++ b/examples2d/restitution2.rs @@ -16,13 +16,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 20.; let ground_height = 1.0; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height) - .restitution(1.0) - .build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height).restitution(1.0); colliders.insert_with_parent(collider, handle, &mut bodies); let num = 10; @@ -32,12 +28,9 @@ pub fn init_world(testbed: &mut Testbed) { for i in 0..=num { let x = (i as f32) - num as f32 / 2.0; let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x * 2.0, 10.0 * (j as f32 + 1.0)]) - .build(); + .translation(vector![x * 2.0, 10.0 * (j as f32 + 1.0)]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::ball(rad) - .restitution((i as f32) / (num as f32)) - .build(); + let collider = ColliderBuilder::ball(rad).restitution((i as f32) / (num as f32)); colliders.insert_with_parent(collider, handle, &mut bodies); } } diff --git a/examples2d/sensor2.rs b/examples2d/sensor2.rs index 9ef5a6b..edd8933 100644 --- a/examples2d/sensor2.rs +++ b/examples2d/sensor2.rs @@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 200.1; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height]); let ground_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height); colliders.insert_with_parent(collider, ground_handle, &mut bodies); /* @@ -37,11 +35,9 @@ pub fn init_world(testbed: &mut Testbed) { let y = 3.0; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad); colliders.insert_with_parent(collider, handle, &mut bodies); testbed.set_initial_body_color(handle, [0.5, 0.5, 1.0]); @@ -52,14 +48,12 @@ pub fn init_world(testbed: &mut Testbed) { */ // Rigid body so that the sensor can move. - let sensor = RigidBodyBuilder::new_dynamic() - .translation(vector![0.0, 10.0]) - .build(); + let sensor = RigidBodyBuilder::new_dynamic().translation(vector![0.0, 10.0]); let sensor_handle = bodies.insert(sensor); // Solid cube attached to the sensor which // other colliders can touch. - let collider = ColliderBuilder::cuboid(rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad); colliders.insert_with_parent(collider, sensor_handle, &mut bodies); // We create a collider desc without density because we don't @@ -67,8 +61,7 @@ pub fn init_world(testbed: &mut Testbed) { let sensor_collider = ColliderBuilder::ball(rad * 5.0) .density(0.0) .sensor(true) - .active_events(ActiveEvents::INTERSECTION_EVENTS) - .build(); + .active_events(ActiveEvents::INTERSECTION_EVENTS); colliders.insert_with_parent(sensor_collider, sensor_handle, &mut bodies); testbed.set_initial_body_color(sensor_handle, [0.5, 1.0, 1.0]); diff --git a/examples2d/trimesh2.rs b/examples2d/trimesh2.rs index 1474b77..debecf9 100644 --- a/examples2d/trimesh2.rs +++ b/examples2d/trimesh2.rs @@ -21,25 +21,23 @@ pub fn init_world(testbed: &mut Testbed) { */ let ground_size = 25.0; - let rigid_body = RigidBodyBuilder::new_static().build(); + let rigid_body = RigidBodyBuilder::new_static(); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, 1.2).build(); + let collider = ColliderBuilder::cuboid(ground_size, 1.2); colliders.insert_with_parent(collider, handle, &mut bodies); let rigid_body = RigidBodyBuilder::new_static() .rotation(std::f32::consts::FRAC_PI_2) - .translation(vector![ground_size, ground_size]) - .build(); + .translation(vector![ground_size, ground_size]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, 1.2).build(); + let collider = ColliderBuilder::cuboid(ground_size, 1.2); colliders.insert_with_parent(collider, handle, &mut bodies); let rigid_body = RigidBodyBuilder::new_static() .rotation(std::f32::consts::FRAC_PI_2) - .translation(vector![-ground_size, ground_size]) - .build(); + .translation(vector![-ground_size, ground_size]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, 1.2).build(); + let collider = ColliderBuilder::cuboid(ground_size, 1.2); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -86,12 +84,10 @@ pub fn init_world(testbed: &mut Testbed) { .collect(); for k in 0..5 { - let collider = - ColliderBuilder::trimesh(vertices.clone(), indices.clone()).build(); + let collider = ColliderBuilder::trimesh(vertices.clone(), indices.clone()); let rigid_body = RigidBodyBuilder::new_dynamic() .translation(vector![ith as f32 * 8.0 - 20.0, 20.0 + k as f32 * 11.0]) - .rotation(angle) - .build(); + .rotation(angle); let handle = bodies.insert(rigid_body); colliders.insert_with_parent(collider, handle, &mut bodies); } diff --git a/examples3d/articulations3.rs b/examples3d/articulations3.rs index 0c87c41..a5dae03 100644 --- a/examples3d/articulations3.rs +++ b/examples3d/articulations3.rs @@ -13,20 +13,17 @@ fn create_prismatic_joints( let rad = 0.4; let shift = 2.0; - let ground = RigidBodyBuilder::new_static() - .translation(vector![origin.x, origin.y, origin.z]) - .build(); + let ground = RigidBodyBuilder::new_static().translation(vector![origin.x, origin.y, origin.z]); let mut curr_parent = bodies.insert(ground); - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, curr_parent, bodies); for i in 0..num { let z = origin.z + (i + 1) as f32 * shift; - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![origin.x, origin.y, z]) - .build(); + let rigid_body = + RigidBodyBuilder::new_dynamic().translation(vector![origin.x, origin.y, z]); let curr_child = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, curr_child, bodies); let axis = if i % 2 == 0 { @@ -35,10 +32,10 @@ fn create_prismatic_joints( UnitVector::new_normalize(vector![-1.0f32, 1.0, 0.0]) }; - let prism = PrismaticJoint::new(axis) + let prism = PrismaticJointBuilder::new(axis) .local_anchor1(point![0.0, 0.0, 0.0]) .local_anchor2(point![0.0, 0.0, -shift]) - .limit_axis([-2.0, 2.0]); + .limits([-2.0, 2.0]); if use_articulations { multibody_joints.insert(curr_parent, curr_child, prism); @@ -61,20 +58,17 @@ fn create_actuated_prismatic_joints( let rad = 0.4; let shift = 2.0; - let ground = RigidBodyBuilder::new_static() - .translation(vector![origin.x, origin.y, origin.z]) - .build(); + let ground = RigidBodyBuilder::new_static().translation(vector![origin.x, origin.y, origin.z]); let mut curr_parent = bodies.insert(ground); - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, curr_parent, bodies); for i in 0..num { let z = origin.z + (i + 1) as f32 * shift; - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![origin.x, origin.y, z]) - .build(); + let rigid_body = + RigidBodyBuilder::new_dynamic().translation(vector![origin.x, origin.y, z]); let curr_child = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, curr_child, bodies); let axis = if i % 2 == 0 { @@ -83,26 +77,29 @@ fn create_actuated_prismatic_joints( UnitVector::new_normalize(vector![-1.0, 1.0, 0.0]) }; - let mut prism = PrismaticJoint::new(axis) - .local_anchor1(point![0.0, 0.0, 0.0]) - .local_anchor2(point![0.0, 0.0, -shift]); + let mut prism = PrismaticJointBuilder::new(axis) + .local_anchor1(point![0.0, 0.0, shift]) + .local_anchor2(point![0.0, 0.0, 0.0]) + .build(); - if i == 1 { - prism = prism - .limit_axis([-Real::MAX, 5.0]) - .motor_velocity(1.0, 1.0) + if i == 0 { + prism + .set_motor_velocity(2.0, 1.0e5) // We set a max impulse so that the motor doesn't fight // the limits with large forces. - .motor_max_impulse(1.0); + .set_limits([-2.0, 5.0]) + .set_motor_max_force(100.0); + } else if i == 1 { + prism + .set_limits([-Real::MAX, 5.0]) + .set_motor_velocity(6.0, 1.0e3) + // We set a max impulse so that the motor doesn't fight + // the limits with large forces. + .set_motor_max_force(100.0); } else if i > 1 { - prism = prism.motor_position(2.0, 0.01, 1.0); - } else { - prism = prism - .motor_velocity(1.0, 1.0) - // We set a max impulse so that the motor doesn't fight - // the limits with large forces. - .motor_max_impulse(0.7) - .limit_axis([-2.0, 5.0]); + prism + .set_motor_position(2.0, 1.0e3, 1.0e2) + .set_motor_max_force(60.0); } if use_articulations { @@ -127,11 +124,9 @@ fn create_revolute_joints( let rad = 0.4; let shift = 2.0; - let ground = RigidBodyBuilder::new_static() - .translation(vector![origin.x, origin.y, 0.0]) - .build(); + let ground = RigidBodyBuilder::new_static().translation(vector![origin.x, origin.y, 0.0]); let mut curr_parent = bodies.insert(ground); - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, curr_parent, bodies); for i in 0..num { @@ -146,11 +141,9 @@ fn create_revolute_joints( let mut handles = [curr_parent; 4]; for k in 0..4 { - let rigid_body = RigidBodyBuilder::new_dynamic() - .position(positions[k]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().position(positions[k]); handles[k] = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, handles[k], bodies); } @@ -158,10 +151,10 @@ fn create_revolute_joints( let x = Vector::x_axis(); let z = Vector::z_axis(); let revs = [ - RevoluteJoint::new(z).local_anchor2(point![0.0, 0.0, -shift]), - RevoluteJoint::new(x).local_anchor2(point![-shift, 0.0, 0.0]), - RevoluteJoint::new(z).local_anchor2(point![0.0, 0.0, -shift]), - RevoluteJoint::new(x).local_anchor2(point![shift, 0.0, 0.0]), + RevoluteJointBuilder::new(z).local_anchor2(point![0.0, 0.0, -shift]), + RevoluteJointBuilder::new(x).local_anchor2(point![-shift, 0.0, 0.0]), + RevoluteJointBuilder::new(z).local_anchor2(point![0.0, 0.0, -shift]), + RevoluteJointBuilder::new(x).local_anchor2(point![shift, 0.0, 0.0]), ]; if use_articulations { @@ -188,37 +181,18 @@ fn create_revolute_joints_with_limits( origin: Point, use_articulations: bool, ) { - let ground = bodies.insert( - RigidBodyBuilder::new_static() - .translation(origin.coords) - .build(), - ); + let ground = bodies.insert(RigidBodyBuilder::new_static().translation(origin.coords)); - let platform1 = bodies.insert( - RigidBodyBuilder::new_dynamic() - .translation(origin.coords) - .build(), - ); - colliders.insert_with_parent( - ColliderBuilder::cuboid(4.0, 0.2, 2.0).build(), - platform1, - bodies, - ); + let platform1 = bodies.insert(RigidBodyBuilder::new_dynamic().translation(origin.coords)); + colliders.insert_with_parent(ColliderBuilder::cuboid(4.0, 0.2, 2.0), platform1, bodies); let shift = vector![0.0, 0.0, 6.0]; - let platform2 = bodies.insert( - RigidBodyBuilder::new_dynamic() - .translation(origin.coords + shift) - .build(), - ); - colliders.insert_with_parent( - ColliderBuilder::cuboid(4.0, 0.2, 2.0).build(), - platform2, - bodies, - ); + let platform2 = + bodies.insert(RigidBodyBuilder::new_dynamic().translation(origin.coords + shift)); + colliders.insert_with_parent(ColliderBuilder::cuboid(4.0, 0.2, 2.0), platform2, bodies); let z = Vector::z_axis(); - let joint1 = RevoluteJoint::new(z).limit_axis([-0.2, 0.2]); + let joint1 = RevoluteJointBuilder::new(z).limits([-0.2, 0.2]); if use_articulations { multibody_joints.insert(ground, platform1, joint1); @@ -226,9 +200,9 @@ fn create_revolute_joints_with_limits( impulse_joints.insert(ground, platform1, joint1); } - let joint2 = RevoluteJoint::new(z) + let joint2 = RevoluteJointBuilder::new(z) .local_anchor2(-Point::from(shift)) - .limit_axis([-0.2, 0.2]); + .limits([-0.2, 0.2]); if use_articulations { multibody_joints.insert(platform1, platform2, joint2); @@ -238,23 +212,20 @@ fn create_revolute_joints_with_limits( // Let’s add a couple of cuboids that will fall on the platforms, triggering the joint limits. let cuboid_body1 = bodies.insert( - RigidBodyBuilder::new_dynamic() - .translation(origin.coords + vector![-2.0, 4.0, 0.0]) - .build(), + RigidBodyBuilder::new_dynamic().translation(origin.coords + vector![-2.0, 4.0, 0.0]), ); colliders.insert_with_parent( - ColliderBuilder::cuboid(0.6, 0.6, 0.6).friction(1.0).build(), + ColliderBuilder::cuboid(0.6, 0.6, 0.6).friction(1.0), cuboid_body1, bodies, ); let cuboid_body2 = bodies.insert( RigidBodyBuilder::new_dynamic() - .translation(origin.coords + shift + vector![2.0, 16.0, 0.0]) - .build(), + .translation(origin.coords + shift + vector![2.0, 16.0, 0.0]), ); colliders.insert_with_parent( - ColliderBuilder::cuboid(0.6, 0.6, 0.6).friction(1.0).build(), + ColliderBuilder::cuboid(0.6, 0.6, 0.6).friction(1.0), cuboid_body2, bodies, ); @@ -288,22 +259,20 @@ fn create_fixed_joints( RigidBodyType::Dynamic }; - let rigid_body = RigidBodyBuilder::new(status) - .translation(vector![ - origin.x + fk * shift, - origin.y, - origin.z + fi * shift - ]) - .build(); + let rigid_body = RigidBodyBuilder::new(status).translation(vector![ + origin.x + fk * shift, + origin.y, + origin.z + fi * shift + ]); let child_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::ball(rad).build(); + let collider = ColliderBuilder::ball(rad); colliders.insert_with_parent(collider, child_handle, bodies); // Vertical joint. if i > 0 { let parent_index = body_handles.len() - num; let parent_handle = body_handles[parent_index]; - let joint = FixedJoint::new().local_anchor2(point![0.0, 0.0, -shift]); + let joint = FixedJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift]); if use_articulations { multibody_joints.insert(parent_handle, child_handle, joint); @@ -316,7 +285,7 @@ fn create_fixed_joints( if k > 0 { let parent_index = body_handles.len() - 1; let parent_handle = body_handles[parent_index]; - let joint = FixedJoint::new().local_anchor2(point![-shift, 0.0, 0.0]); + let joint = FixedJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]); impulse_joints.insert(parent_handle, child_handle, joint); } @@ -349,17 +318,20 @@ fn create_spherical_joints( RigidBodyType::Dynamic }; - let rigid_body = RigidBodyBuilder::new(status) - .translation(vector![fk * shift, 0.0, fi * shift * 2.0]) - .build(); + let rigid_body = RigidBodyBuilder::new(status).translation(vector![ + fk * shift, + 0.0, + fi * shift * 2.0 + ]); let child_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::capsule_z(rad * 1.25, rad).build(); + let collider = ColliderBuilder::capsule_z(rad * 1.25, rad); colliders.insert_with_parent(collider, child_handle, bodies); // Vertical joint. if i > 0 { let parent_handle = *body_handles.last().unwrap(); - let joint = SphericalJoint::new().local_anchor2(point![0.0, 0.0, -shift * 2.0]); + let joint = + SphericalJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift * 2.0]); if use_articulations { multibody_joints.insert(parent_handle, child_handle, joint); @@ -372,7 +344,7 @@ fn create_spherical_joints( if k > 0 { let parent_index = body_handles.len() - num; let parent_handle = body_handles[parent_index]; - let joint = SphericalJoint::new().local_anchor2(point![-shift, 0.0, 0.0]); + let joint = SphericalJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]); impulse_joints.insert(parent_handle, child_handle, joint); } @@ -391,44 +363,28 @@ fn create_spherical_joints_with_limits( ) { let shift = vector![0.0, 0.0, 3.0]; - let ground = bodies.insert( - RigidBodyBuilder::new_static() - .translation(origin.coords) - .build(), - ); + let ground = bodies.insert(RigidBodyBuilder::new_static().translation(origin.coords)); let ball1 = bodies.insert( RigidBodyBuilder::new_dynamic() .translation(origin.coords + shift) - .linvel(vector![20.0, 20.0, 0.0]) - .build(), - ); - colliders.insert_with_parent( - ColliderBuilder::cuboid(1.0, 1.0, 1.0).build(), - ball1, - bodies, + .linvel(vector![20.0, 20.0, 0.0]), ); + colliders.insert_with_parent(ColliderBuilder::cuboid(1.0, 1.0, 1.0), ball1, bodies); - let ball2 = bodies.insert( - RigidBodyBuilder::new_dynamic() - .translation(origin.coords + shift * 2.0) - .build(), - ); - colliders.insert_with_parent( - ColliderBuilder::cuboid(1.0, 1.0, 1.0).build(), - ball2, - bodies, - ); + let ball2 = + bodies.insert(RigidBodyBuilder::new_dynamic().translation(origin.coords + shift * 2.0)); + colliders.insert_with_parent(ColliderBuilder::cuboid(1.0, 1.0, 1.0), ball2, bodies); - let joint1 = SphericalJoint::new() + let joint1 = SphericalJointBuilder::new() .local_anchor2(Point::from(-shift)) - .limit_axis(JointAxis::X, [-0.2, 0.2]) - .limit_axis(JointAxis::Y, [-0.2, 0.2]); + .limits(JointAxis::X, [-0.2, 0.2]) + .limits(JointAxis::Y, [-0.2, 0.2]); - let joint2 = SphericalJoint::new() + let joint2 = SphericalJointBuilder::new() .local_anchor2(Point::from(-shift)) - .limit_axis(JointAxis::X, [-0.3, 0.3]) - .limit_axis(JointAxis::Y, [-0.3, 0.3]); + .limits(JointAxis::X, [-0.3, 0.3]) + .limits(JointAxis::Y, [-0.3, 0.3]); if use_articulations { multibody_joints.insert(ground, ball1, joint1); @@ -453,7 +409,7 @@ fn create_actuated_revolute_joints( // We will reuse this base configuration for all the impulse_joints here. let z = Vector::z_axis(); - let joint_template = RevoluteJoint::new(z).local_anchor2(point![0.0, 0.0, -shift]); + let joint_template = RevoluteJointBuilder::new(z).local_anchor2(point![0.0, 0.0, -shift]); let mut parent_handle = RigidBodyHandle::invalid(); @@ -474,27 +430,27 @@ fn create_actuated_revolute_joints( let rigid_body = RigidBodyBuilder::new(status) .translation(vector![origin.x, origin.y + shifty, origin.z + fi * shift]) // .rotation(Vector3::new(0.0, fi * 1.1, 0.0)) - .build(); + ; let child_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad * 2.0, rad * 6.0 / (fi + 1.0), rad).build(); + let collider = ColliderBuilder::cuboid(rad * 2.0, rad * 6.0 / (fi + 1.0), rad); colliders.insert_with_parent(collider, child_handle, bodies); if i > 0 { - let mut joint = joint_template.clone(); + let mut joint = joint_template.motor_model(MotorModel::AccelerationBased); if i % 3 == 1 { - joint = joint.motor_velocity(-20.0, 0.1); + joint = joint.motor_velocity(-20.0, 100.0); } else if i == num - 1 { - let stiffness = 0.2; - let damping = 1.0; + let stiffness = 200.0; + let damping = 100.0; joint = joint.motor_position(3.14 / 2.0, stiffness, damping); } if i == 1 { joint = joint .local_anchor2(point![0.0, 2.0, -shift]) - .motor_velocity(-2.0, 0.1); + .motor_velocity(-2.0, 100.0); } if use_articulations { @@ -521,7 +477,7 @@ fn create_actuated_spherical_joints( let shift = 2.0; // We will reuse this base configuration for all the impulse_joints here. - let joint_template = SphericalJoint::new().local_anchor1(point![0.0, 0.0, shift]); + let joint_template = SphericalJointBuilder::new().local_anchor1(point![0.0, 0.0, shift]); let mut parent_handle = RigidBodyHandle::invalid(); @@ -540,10 +496,10 @@ fn create_actuated_spherical_joints( let rigid_body = RigidBodyBuilder::new(status) .translation(vector![origin.x, origin.y, origin.z + fi * shift]) // .rotation(Vector3::new(0.0, fi * 1.1, 0.0)) - .build(); + ; let child_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::capsule_y(rad * 2.0 / (fi + 1.0), rad).build(); + let collider = ColliderBuilder::capsule_y(rad * 2.0 / (fi + 1.0), rad); colliders.insert_with_parent(collider, child_handle, bodies); if i > 0 { diff --git a/examples3d/ccd3.rs b/examples3d/ccd3.rs index 2e00b86..c1ade84 100644 --- a/examples3d/ccd3.rs +++ b/examples3d/ccd3.rs @@ -21,12 +21,9 @@ fn create_wall( - stack_height as f32 * half_extents.z; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y, z]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]); let handle = bodies.insert(rigid_body); - let collider = - ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z).build(); + let collider = ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z); colliders.insert_with_parent(collider, handle, bodies); k += 1; if k % 2 == 0 { @@ -53,11 +50,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 50.0; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let ground_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); colliders.insert_with_parent(collider, ground_handle, &mut bodies); /* @@ -97,23 +92,20 @@ pub fn init_world(testbed: &mut Testbed) { let collider = ColliderBuilder::ball(1.0) .density(10.0) .sensor(true) - .active_events(ActiveEvents::INTERSECTION_EVENTS) - .build(); + .active_events(ActiveEvents::INTERSECTION_EVENTS); let rigid_body = RigidBodyBuilder::new_dynamic() .linvel(vector![1000.0, 0.0, 0.0]) .translation(vector![-20.0, shift_y + 2.0, 0.0]) - .ccd_enabled(true) - .build(); + .ccd_enabled(true); let sensor_handle = bodies.insert(rigid_body); colliders.insert_with_parent(collider, sensor_handle, &mut bodies); // Second rigid-body with CCD enabled. - let collider = ColliderBuilder::ball(1.0).density(10.0).build(); + let collider = ColliderBuilder::ball(1.0).density(10.0); let rigid_body = RigidBodyBuilder::new_dynamic() .linvel(vector![1000.0, 0.0, 0.0]) .translation(vector![-20.0, shift_y + 2.0, shift_z]) - .ccd_enabled(true) - .build(); + .ccd_enabled(true); let handle = bodies.insert(rigid_body); colliders.insert_with_parent(collider.clone(), handle, &mut bodies); testbed.set_initial_body_color(handle, [0.2, 0.2, 1.0]); diff --git a/examples3d/collision_groups3.rs b/examples3d/collision_groups3.rs index d43a7fc..c1bc144 100644 --- a/examples3d/collision_groups3.rs +++ b/examples3d/collision_groups3.rs @@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 5.0; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let floor_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); colliders.insert_with_parent(collider, floor_handle, &mut bodies); /* @@ -34,8 +32,7 @@ pub fn init_world(testbed: &mut Testbed) { */ let green_floor = ColliderBuilder::cuboid(1.0, 0.1, 1.0) .translation(vector![0.0, 1.0, 0.0]) - .collision_groups(GREEN_GROUP) - .build(); + .collision_groups(GREEN_GROUP); let green_collider_handle = colliders.insert_with_parent(green_floor, floor_handle, &mut bodies); @@ -46,8 +43,7 @@ pub fn init_world(testbed: &mut Testbed) { */ let blue_floor = ColliderBuilder::cuboid(1.0, 0.1, 1.0) .translation(vector![0.0, 2.0, 0.0]) - .collision_groups(BLUE_GROUP) - .build(); + .collision_groups(BLUE_GROUP); let blue_collider_handle = colliders.insert_with_parent(blue_floor, floor_handle, &mut bodies); testbed.set_initial_collider_color(blue_collider_handle, [0.0, 0.0, 1.0]); @@ -77,13 +73,9 @@ pub fn init_world(testbed: &mut Testbed) { (BLUE_GROUP, [0.0, 0.0, 1.0]) }; - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y, z]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad, rad) - .collision_groups(group) - .build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad).collision_groups(group); colliders.insert_with_parent(collider, handle, &mut bodies); testbed.set_initial_body_color(handle, color); diff --git a/examples3d/compound3.rs b/examples3d/compound3.rs index 433210d..5ecccb9 100644 --- a/examples3d/compound3.rs +++ b/examples3d/compound3.rs @@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 50.0; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -45,20 +43,16 @@ pub fn init_world(testbed: &mut Testbed) { let z = k as f32 * shift * 2.0 - centerz + offset; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y, z]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]); let handle = bodies.insert(rigid_body); // First option: attach several colliders to a single rigid-body. if j < numy / 2 { - let collider1 = ColliderBuilder::cuboid(rad * 10.0, rad, rad).build(); + let collider1 = ColliderBuilder::cuboid(rad * 10.0, rad, rad); let collider2 = ColliderBuilder::cuboid(rad, rad * 10.0, rad) - .translation(vector![rad * 10.0, rad * 10.0, 0.0]) - .build(); + .translation(vector![rad * 10.0, rad * 10.0, 0.0]); let collider3 = ColliderBuilder::cuboid(rad, rad * 10.0, rad) - .translation(vector![-rad * 10.0, rad * 10.0, 0.0]) - .build(); + .translation(vector![-rad * 10.0, rad * 10.0, 0.0]); colliders.insert_with_parent(collider1, handle, &mut bodies); colliders.insert_with_parent(collider2, handle, &mut bodies); colliders.insert_with_parent(collider3, handle, &mut bodies); @@ -79,7 +73,7 @@ pub fn init_world(testbed: &mut Testbed) { ), ]; - let collider = ColliderBuilder::compound(shapes).build(); + let collider = ColliderBuilder::compound(shapes); colliders.insert_with_parent(collider, handle, &mut bodies); } } diff --git a/examples3d/convex_decomposition3.rs b/examples3d/convex_decomposition3.rs index 6daf4d7..33c1611 100644 --- a/examples3d/convex_decomposition3.rs +++ b/examples3d/convex_decomposition3.rs @@ -24,11 +24,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 50.0; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -88,13 +86,11 @@ pub fn init_world(testbed: &mut Testbed) { let y = (igeom / width) as f32 * shift + 4.0; let z = k as f32 * shift; - let body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y, z]) - .build(); + let body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]); let handle = bodies.insert(body); for shape in &shapes { - let collider = ColliderBuilder::new(shape.clone()).build(); + let collider = ColliderBuilder::new(shape.clone()); colliders.insert_with_parent(collider, handle, &mut bodies); } } diff --git a/examples3d/convex_polyhedron3.rs b/examples3d/convex_polyhedron3.rs index 7abcefb..b4c391e 100644 --- a/examples3d/convex_polyhedron3.rs +++ b/examples3d/convex_polyhedron3.rs @@ -18,11 +18,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 40.0; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -54,13 +52,9 @@ pub fn init_world(testbed: &mut Testbed) { } // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y, z]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::round_convex_hull(&points, border_rad) - .unwrap() - .build(); + let collider = ColliderBuilder::round_convex_hull(&points, border_rad).unwrap(); colliders.insert_with_parent(collider, handle, &mut bodies); } } diff --git a/examples3d/damping3.rs b/examples3d/damping3.rs index 6f2edb9..d189482 100644 --- a/examples3d/damping3.rs +++ b/examples3d/damping3.rs @@ -27,12 +27,11 @@ pub fn init_world(testbed: &mut Testbed) { .linvel(vector![x * 10.0, y * 10.0, 0.0]) .angvel(Vector::z() * 100.0) .linear_damping((i + 1) as f32 * subdiv * 10.0) - .angular_damping((num - i) as f32 * subdiv * 10.0) - .build(); + .angular_damping((num - i) as f32 * subdiv * 10.0); let rb_handle = bodies.insert(rb); // Build the collider. - let co = ColliderBuilder::cuboid(rad, rad, rad).build(); + let co = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(co, rb_handle, &mut bodies); } diff --git a/examples3d/debug_add_remove_collider3.rs b/examples3d/debug_add_remove_collider3.rs index dd680d5..e6d8ac6 100644 --- a/examples3d/debug_add_remove_collider3.rs +++ b/examples3d/debug_add_remove_collider3.rs @@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 3.0; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let ground_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, 0.4).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, 0.4); let mut ground_collider_handle = colliders.insert_with_parent(collider, ground_handle, &mut bodies); @@ -28,11 +26,9 @@ pub fn init_world(testbed: &mut Testbed) { * Rolling ball */ let ball_rad = 0.1; - let rb = RigidBodyBuilder::new_dynamic() - .translation(vector![0.0, 0.2, 0.0]) - .build(); + let rb = RigidBodyBuilder::new_dynamic().translation(vector![0.0, 0.2, 0.0]); let ball_handle = bodies.insert(rb); - let collider = ColliderBuilder::ball(ball_rad).density(100.0).build(); + let collider = ColliderBuilder::ball(ball_rad).density(100.0); colliders.insert_with_parent(collider, ball_handle, &mut bodies); testbed.add_callback(move |_, physics, _, _| { diff --git a/examples3d/debug_articulations3.rs b/examples3d/debug_articulations3.rs index 3b75a21..0dbc66a 100644 --- a/examples3d/debug_articulations3.rs +++ b/examples3d/debug_articulations3.rs @@ -25,17 +25,20 @@ fn create_ball_articulations( RigidBodyType::Dynamic }; - let rigid_body = RigidBodyBuilder::new(status) - .translation(vector![fk * shift, 0.0, fi * shift * 2.0]) - .build(); + let rigid_body = RigidBodyBuilder::new(status).translation(vector![ + fk * shift, + 0.0, + fi * shift * 2.0 + ]); let child_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::capsule_z(rad * 1.25, rad).build(); + let collider = ColliderBuilder::capsule_z(rad * 1.25, rad); colliders.insert_with_parent(collider, child_handle, bodies); // Vertical multibody_joint. if i > 0 { let parent_handle = *body_handles.last().unwrap(); - let joint = SphericalJoint::new().local_anchor2(point![0.0, 0.0, -shift * 2.0]); + let joint = + SphericalJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift * 2.0]); multibody_joints.insert(parent_handle, child_handle, joint); } @@ -43,7 +46,7 @@ fn create_ball_articulations( if k > 0 && i > 0 { let parent_index = body_handles.len() - num; let parent_handle = body_handles[parent_index]; - let joint = SphericalJoint::new().local_anchor2(point![-shift, 0.0, 0.0]); + let joint = SphericalJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]); // let joint = // PrismaticJoint::new(Vector::y_axis()).local_anchor2(point![-shift, 0.0, 0.0]); // let joint = FixedJoint::new().local_anchor2(point![-shift, 0.0, 0.0]); @@ -68,15 +71,13 @@ pub fn init_world(testbed: &mut Testbed) { let collider = ColliderBuilder::cuboid(30.0, 0.01, 30.0) .translation(vector![0.0, -3.02, 0.0]) - .rotation(vector![0.1, 0.0, 0.1]) - .build(); + .rotation(vector![0.1, 0.0, 0.1]); colliders.insert(collider); - let rigid_body = RigidBodyBuilder::new_dynamic().build(); + let rigid_body = RigidBodyBuilder::new_dynamic(); let collider = ColliderBuilder::cuboid(30.0, 0.01, 30.0) .translation(vector![0.0, -3.0, 0.0]) - .rotation(vector![0.1, 0.0, 0.1]) - .build(); + .rotation(vector![0.1, 0.0, 0.1]); let handle = bodies.insert(rigid_body); colliders.insert_with_parent(collider, handle, &mut bodies); diff --git a/examples3d/debug_big_colliders3.rs b/examples3d/debug_big_colliders3.rs index 33460dc..d221a05 100644 --- a/examples3d/debug_big_colliders3.rs +++ b/examples3d/debug_big_colliders3.rs @@ -13,10 +13,10 @@ pub fn init_world(testbed: &mut Testbed) { /* * Ground */ - let rigid_body = RigidBodyBuilder::new_static().build(); + let rigid_body = RigidBodyBuilder::new_static(); let handle = bodies.insert(rigid_body); let halfspace = SharedShape::new(HalfSpace::new(Vector::y_axis())); - let collider = ColliderBuilder::new(halfspace).build(); + let collider = ColliderBuilder::new(halfspace); colliders.insert_with_parent(collider, handle, &mut bodies); let mut curr_y = 0.0; @@ -26,11 +26,9 @@ pub fn init_world(testbed: &mut Testbed) { let curr_height = 0.1f32.min(curr_width); curr_y += curr_height * 4.0; - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![0.0, curr_y, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![0.0, curr_y, 0.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(curr_width, curr_height, curr_width).build(); + let collider = ColliderBuilder::cuboid(curr_width, curr_height, curr_width); colliders.insert_with_parent(collider, handle, &mut bodies); curr_width /= 5.0; diff --git a/examples3d/debug_boxes3.rs b/examples3d/debug_boxes3.rs index ddf86db..908985b 100644 --- a/examples3d/debug_boxes3.rs +++ b/examples3d/debug_boxes3.rs @@ -17,11 +17,10 @@ pub fn init_world(testbed: &mut Testbed) { let ground_height = 0.1; for _ in 0..6 { - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = + RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); colliders.insert_with_parent(collider, handle, &mut bodies); } @@ -30,10 +29,9 @@ pub fn init_world(testbed: &mut Testbed) { let rigid_body = RigidBodyBuilder::new_dynamic() .translation(vector![1.1, 0.0, 0.0]) // .rotation(vector![0.8, 0.2, 0.1]) - .can_sleep(false) - .build(); + .can_sleep(false); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(2.0, 0.1, 1.0).build(); + let collider = ColliderBuilder::cuboid(2.0, 0.1, 1.0); colliders.insert_with_parent(collider, handle, &mut bodies); } diff --git a/examples3d/debug_cylinder3.rs b/examples3d/debug_cylinder3.rs index cb087be..b62fb12 100644 --- a/examples3d/debug_cylinder3.rs +++ b/examples3d/debug_cylinder3.rs @@ -19,11 +19,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 100.1; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -46,11 +44,9 @@ pub fn init_world(testbed: &mut Testbed) { let z = -centerz + offset; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y, z]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cylinder(rad, rad).build(); + let collider = ColliderBuilder::cylinder(rad, rad); colliders.insert_with_parent(collider, handle, &mut bodies); /* diff --git a/examples3d/debug_dynamic_collider_add3.rs b/examples3d/debug_dynamic_collider_add3.rs index 27e1b0b..9404a61 100644 --- a/examples3d/debug_dynamic_collider_add3.rs +++ b/examples3d/debug_dynamic_collider_add3.rs @@ -16,14 +16,12 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 20.0; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let ground_handle = bodies.insert(rigid_body); let collider = ColliderBuilder::cuboid(ground_size, ground_height, 0.4) .friction(0.15) // .restitution(0.5) - .build(); + ; colliders.insert_with_parent(collider, ground_handle, &mut bodies); /* @@ -32,10 +30,9 @@ pub fn init_world(testbed: &mut Testbed) { let ball_rad = 0.1; let rb = RigidBodyBuilder::new_dynamic() .translation(vector![0.0, 0.2, 0.0]) - .linvel(vector![10.0, 0.0, 0.0]) - .build(); + .linvel(vector![10.0, 0.0, 0.0]); let ball_handle = bodies.insert(rb); - let collider = ColliderBuilder::ball(ball_rad).density(100.0).build(); + let collider = ColliderBuilder::ball(ball_rad).density(100.0); colliders.insert_with_parent(collider, ball_handle, &mut bodies); let mut linvel = Vector::zeros(); @@ -49,9 +46,7 @@ pub fn init_world(testbed: &mut Testbed) { step += 1; // Add a bigger ball collider - let collider = ColliderBuilder::ball(ball_rad + 0.01 * (step as f32)) - .density(100.0) - .build(); + let collider = ColliderBuilder::ball(ball_rad + 0.01 * (step as f32)).density(100.0); let new_ball_collider_handle = physics .colliders @@ -99,8 +94,7 @@ pub fn init_world(testbed: &mut Testbed) { // .remove(ground_collider_handle, &mut physics.bodies, true) // .unwrap(); let coll = ColliderBuilder::cuboid(ground_size, ground_height + step as f32 * 0.01, 0.4) - .friction(0.15) - .build(); + .friction(0.15); let new_ground_collider_handle = physics .colliders diff --git a/examples3d/debug_friction3.rs b/examples3d/debug_friction3.rs index 44c9fae..b48364d 100644 --- a/examples3d/debug_friction3.rs +++ b/examples3d/debug_friction3.rs @@ -16,20 +16,17 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 100.0; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static().build(); + let rigid_body = RigidBodyBuilder::new_static(); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size) - .friction(1.5) - .build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).friction(1.5); colliders.insert_with_parent(collider, handle, &mut bodies); // Build a dynamic box rigid body. let rigid_body = RigidBodyBuilder::new_dynamic() .translation(vector![0.0, 1.1, 0.0]) - .rotation(Vector::y() * 0.3) - .build(); + .rotation(Vector::y() * 0.3); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(2.0, 1.0, 3.0).friction(1.5).build(); + let collider = ColliderBuilder::cuboid(2.0, 1.0, 3.0).friction(1.5); colliders.insert_with_parent(collider, handle, &mut bodies); let rigid_body = &mut bodies[handle]; diff --git a/examples3d/debug_infinite_fall3.rs b/examples3d/debug_infinite_fall3.rs index 4d950cf..7d03734 100644 --- a/examples3d/debug_infinite_fall3.rs +++ b/examples3d/debug_infinite_fall3.rs @@ -16,29 +16,25 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 100.1; let ground_height = 2.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, 4.0, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, 4.0, 0.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); colliders.insert_with_parent(collider, handle, &mut bodies); let rad = 1.0; // Build the dynamic box rigid body. let rigid_body = RigidBodyBuilder::new_dynamic() .translation(vector![0.0, 7.0 * rad, 0.0]) - .can_sleep(false) - .build(); + .can_sleep(false); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::ball(rad).build(); + let collider = ColliderBuilder::ball(rad); colliders.insert_with_parent(collider, handle, &mut bodies); let rigid_body = RigidBodyBuilder::new_dynamic() .translation(vector![0.0, 2.0 * rad, 0.0]) - .can_sleep(false) - .build(); + .can_sleep(false); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::ball(rad).build(); + let collider = ColliderBuilder::ball(rad); colliders.insert_with_parent(collider, handle, &mut bodies); /* diff --git a/examples3d/debug_prismatic3.rs b/examples3d/debug_prismatic3.rs index 15176f3..9f93237 100644 --- a/examples3d/debug_prismatic3.rs +++ b/examples3d/debug_prismatic3.rs @@ -7,16 +7,12 @@ fn prismatic_repro( impulse_joints: &mut ImpulseJointSet, box_center: Point, ) { - let box_rb = bodies.insert( - RigidBodyBuilder::new_dynamic() - .translation(vector![box_center.x, box_center.y, box_center.z]) - .build(), - ); - colliders.insert_with_parent( - ColliderBuilder::cuboid(1.0, 0.25, 1.0).build(), - box_rb, - bodies, - ); + let box_rb = bodies.insert(RigidBodyBuilder::new_dynamic().translation(vector![ + box_center.x, + box_center.y, + box_center.z + ])); + colliders.insert_with_parent(ColliderBuilder::cuboid(1.0, 0.25, 1.0), box_rb, bodies); let wheel_y = -1.0; let wheel_positions = vec![ @@ -28,36 +24,28 @@ fn prismatic_repro( for pos in wheel_positions { let wheel_pos_in_world = box_center + pos; - let wheel_rb = bodies.insert( - RigidBodyBuilder::new_dynamic() - .translation(vector![ - wheel_pos_in_world.x, - wheel_pos_in_world.y, - wheel_pos_in_world.z - ]) - .build(), - ); - colliders.insert_with_parent(ColliderBuilder::ball(0.5).build(), wheel_rb, bodies); + let wheel_rb = bodies.insert(RigidBodyBuilder::new_dynamic().translation(vector![ + wheel_pos_in_world.x, + wheel_pos_in_world.y, + wheel_pos_in_world.z + ])); + colliders.insert_with_parent(ColliderBuilder::ball(0.5), wheel_rb, bodies); let (stiffness, damping) = (0.05, 0.2); - let prismatic = PrismaticJoint::new(Vector::y_axis()) + let prismatic = PrismaticJointBuilder::new(Vector::y_axis()) .local_anchor1(point![pos.x, pos.y, pos.z]) .motor_position(0.0, stiffness, damping); impulse_joints.insert(box_rb, wheel_rb, prismatic); } // put a small box under one of the wheels - let gravel = bodies.insert( - RigidBodyBuilder::new_dynamic() - .translation(vector![box_center.x + 1.0, box_center.y - 2.4, -1.0]) - .build(), - ); - colliders.insert_with_parent( - ColliderBuilder::cuboid(0.5, 0.1, 0.5).build(), - gravel, - bodies, - ); + let gravel = bodies.insert(RigidBodyBuilder::new_dynamic().translation(vector![ + box_center.x + 1.0, + box_center.y - 2.4, + -1.0 + ])); + colliders.insert_with_parent(ColliderBuilder::cuboid(0.5, 0.1, 0.5), gravel, bodies); } pub fn init_world(testbed: &mut Testbed) { @@ -75,11 +63,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 50.0; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); colliders.insert_with_parent(collider, handle, &mut bodies); prismatic_repro( diff --git a/examples3d/debug_rollback3.rs b/examples3d/debug_rollback3.rs index b795434..b40384c 100644 --- a/examples3d/debug_rollback3.rs +++ b/examples3d/debug_rollback3.rs @@ -16,14 +16,12 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 20.0; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let ground_handle = bodies.insert(rigid_body); let collider = ColliderBuilder::cuboid(ground_size, ground_height, 0.4) .friction(0.15) // .restitution(0.5) - .build(); + ; colliders.insert_with_parent(collider, ground_handle, &mut bodies); /* @@ -32,10 +30,9 @@ pub fn init_world(testbed: &mut Testbed) { let ball_rad = 0.1; let rb = RigidBodyBuilder::new_dynamic() .translation(vector![0.0, 0.2, 0.0]) - .linvel(vector![10.0, 0.0, 0.0]) - .build(); + .linvel(vector![10.0, 0.0, 0.0]); let ball_handle = bodies.insert(rb); - let collider = ColliderBuilder::ball(ball_rad).density(100.0).build(); + let collider = ColliderBuilder::ball(ball_rad).density(100.0); colliders.insert_with_parent(collider, ball_handle, &mut bodies); let mut linvel = Vector::zeros(); diff --git a/examples3d/debug_shape_modification3.rs b/examples3d/debug_shape_modification3.rs index eb2966e..5557131 100644 --- a/examples3d/debug_shape_modification3.rs +++ b/examples3d/debug_shape_modification3.rs @@ -16,14 +16,12 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 20.0; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let ground_handle = bodies.insert(rigid_body); let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size) .friction(0.15) // .restitution(0.5) - .build(); + ; colliders.insert_with_parent(collider, ground_handle, &mut bodies); /* @@ -32,10 +30,9 @@ pub fn init_world(testbed: &mut Testbed) { let ball_rad = 0.1; let rb = RigidBodyBuilder::new_dynamic() .translation(vector![0.0, 0.2, 0.0]) - .linvel(vector![10.0, 0.0, 0.0]) - .build(); + .linvel(vector![10.0, 0.0, 0.0]); let ball_handle = bodies.insert(rb); - let collider = ColliderBuilder::ball(ball_rad).density(100.0).build(); + let collider = ColliderBuilder::ball(ball_rad).density(100.0); let ball_coll_handle = colliders.insert_with_parent(collider, ball_handle, &mut bodies); let mut linvel = Vector::zeros(); @@ -90,19 +87,17 @@ pub fn init_world(testbed: &mut Testbed) { let z = k as f32 * shiftz - centerz + offset; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y, z]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]); let handle = bodies.insert(rigid_body); let collider = match j % 5 { - 0 => ColliderBuilder::cuboid(rad, rad, rad).build(), - 1 => ColliderBuilder::ball(rad).build(), + 0 => ColliderBuilder::cuboid(rad, rad, rad), + 1 => ColliderBuilder::ball(rad), // Rounded cylinders are much more efficient that cylinder, even if the // rounding margin is small. - 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(), - 3 => ColliderBuilder::cone(rad, rad).build(), - _ => ColliderBuilder::capsule_y(rad, rad).build(), + 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0), + 3 => ColliderBuilder::cone(rad, rad), + _ => ColliderBuilder::capsule_y(rad, rad), }; colliders.insert_with_parent(collider, handle, &mut bodies); diff --git a/examples3d/debug_triangle3.rs b/examples3d/debug_triangle3.rs index 8151cf9..e84331a 100644 --- a/examples3d/debug_triangle3.rs +++ b/examples3d/debug_triangle3.rs @@ -17,21 +17,18 @@ pub fn init_world(testbed: &mut Testbed) { point![0.0, 0.0, 10.0], ]; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, 0.0, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, 0.0, 0.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::triangle(vtx[0], vtx[1], vtx[2]).build(); + let collider = ColliderBuilder::triangle(vtx[0], vtx[1], vtx[2]); colliders.insert_with_parent(collider, handle, &mut bodies); // Dynamic box rigid body. let rigid_body = RigidBodyBuilder::new_dynamic() .translation(vector![1.1, 0.01, 0.0]) // .rotation(Vector3::new(0.8, 0.2, 0.1)) - .can_sleep(false) - .build(); + .can_sleep(false); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(20.0, 0.1, 1.0).build(); + let collider = ColliderBuilder::cuboid(20.0, 0.1, 1.0); colliders.insert_with_parent(collider, handle, &mut bodies); /* diff --git a/examples3d/debug_trimesh3.rs b/examples3d/debug_trimesh3.rs index 8e38719..f7fa651 100644 --- a/examples3d/debug_trimesh3.rs +++ b/examples3d/debug_trimesh3.rs @@ -41,17 +41,14 @@ pub fn init_world(testbed: &mut Testbed) { let rigid_body = RigidBodyBuilder::new_dynamic() .translation(vector![0.0, 35.0, 0.0]) // .rotation(Vector3::new(0.8, 0.2, 0.1)) - .can_sleep(false) - .build(); + .can_sleep(false); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(1.0, 2.0, 1.0).build(); + let collider = ColliderBuilder::cuboid(1.0, 2.0, 1.0); colliders.insert_with_parent(collider, handle, &mut bodies); - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, 0.0, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, 0.0, 0.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::trimesh(vtx, idx).build(); + let collider = ColliderBuilder::trimesh(vtx, idx); colliders.insert_with_parent(collider, handle, &mut bodies); testbed.set_initial_body_color(handle, [0.3, 0.3, 0.3]); diff --git a/examples3d/domino3.rs b/examples3d/domino3.rs index 067a86d..f57e2c5 100644 --- a/examples3d/domino3.rs +++ b/examples3d/domino3.rs @@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 200.1; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -55,9 +53,9 @@ pub fn init_world(testbed: &mut Testbed) { Translation::new(x * curr_rad, width * 2.0 + ground_height, z * curr_rad) * tilt * rot; - let rigid_body = RigidBodyBuilder::new_dynamic().position(position).build(); + let rigid_body = RigidBodyBuilder::new_dynamic().position(position); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(thickness, width * 2.0, width).build(); + let collider = ColliderBuilder::cuboid(thickness, width * 2.0, width); colliders.insert_with_parent(collider, handle, &mut bodies); testbed.set_initial_body_color(handle, colors[i % 2]); } else { diff --git a/examples3d/fountain3.rs b/examples3d/fountain3.rs index 4e47878..31ff15b 100644 --- a/examples3d/fountain3.rs +++ b/examples3d/fountain3.rs @@ -17,23 +17,19 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 100.1; let ground_height = 2.1; // 16.0; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); colliders.insert_with_parent(collider, handle, &mut bodies); // Callback that will be executed on the main loop to handle proximities. testbed.add_callback(move |mut graphics, physics, _, run_state| { - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![0.0, 10.0, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![0.0, 10.0, 0.0]); let handle = physics.bodies.insert(rigid_body); let collider = match run_state.timestep_id % 3 { - 0 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(), - 1 => ColliderBuilder::cone(rad, rad).build(), - _ => ColliderBuilder::cuboid(rad, rad, rad).build(), + 0 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0), + 1 => ColliderBuilder::cone(rad, rad), + _ => ColliderBuilder::cuboid(rad, rad, rad), }; physics diff --git a/examples3d/harness_capsules3.rs b/examples3d/harness_capsules3.rs index dae364a..30849d3 100644 --- a/examples3d/harness_capsules3.rs +++ b/examples3d/harness_capsules3.rs @@ -16,11 +16,9 @@ pub fn init_world(harness: &mut Harness) { let ground_size = 200.1; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -45,11 +43,9 @@ pub fn init_world(harness: &mut Harness) { let z = k as f32 * shift - centerz + offset; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y, z]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::capsule_y(rad, rad).build(); + let collider = ColliderBuilder::capsule_y(rad, rad); colliders.insert_with_parent(collider, handle, &mut bodies); } } diff --git a/examples3d/heightfield3.rs b/examples3d/heightfield3.rs index f5a3ba6..e144442 100644 --- a/examples3d/heightfield3.rs +++ b/examples3d/heightfield3.rs @@ -31,9 +31,9 @@ pub fn init_world(testbed: &mut Testbed) { } }); - let rigid_body = RigidBodyBuilder::new_static().build(); + let rigid_body = RigidBodyBuilder::new_static(); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::heightfield(heights, ground_size).build(); + let collider = ColliderBuilder::heightfield(heights, ground_size); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -55,19 +55,17 @@ pub fn init_world(testbed: &mut Testbed) { let z = k as f32 * shift - centerz; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y, z]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]); let handle = bodies.insert(rigid_body); let collider = match j % 6 { - 0 => ColliderBuilder::cuboid(rad, rad, rad).build(), - 1 => ColliderBuilder::ball(rad).build(), + 0 => ColliderBuilder::cuboid(rad, rad, rad), + 1 => ColliderBuilder::ball(rad), // Rounded cylinders are much more efficient that cylinder, even if the // rounding margin is small. - 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(), - 3 => ColliderBuilder::cone(rad, rad).build(), - 4 => ColliderBuilder::capsule_y(rad, rad).build(), + 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0), + 3 => ColliderBuilder::cone(rad, rad), + 4 => ColliderBuilder::capsule_y(rad, rad), _ => { let shapes = vec![ ( @@ -84,7 +82,7 @@ pub fn init_world(testbed: &mut Testbed) { ), ]; - ColliderBuilder::compound(shapes).build() + ColliderBuilder::compound(shapes) } }; diff --git a/examples3d/joints3.rs b/examples3d/joints3.rs index 02aa5f9..8b2c258 100644 --- a/examples3d/joints3.rs +++ b/examples3d/joints3.rs @@ -11,20 +11,17 @@ fn create_prismatic_joints( let rad = 0.4; let shift = 2.0; - let ground = RigidBodyBuilder::new_static() - .translation(vector![origin.x, origin.y, origin.z]) - .build(); + let ground = RigidBodyBuilder::new_static().translation(vector![origin.x, origin.y, origin.z]); let mut curr_parent = bodies.insert(ground); - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, curr_parent, bodies); for i in 0..num { let z = origin.z + (i + 1) as f32 * shift; - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![origin.x, origin.y, z]) - .build(); + let rigid_body = + RigidBodyBuilder::new_dynamic().translation(vector![origin.x, origin.y, z]); let curr_child = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, curr_child, bodies); let axis = if i % 2 == 0 { @@ -33,10 +30,10 @@ fn create_prismatic_joints( UnitVector::new_normalize(vector![-1.0f32, 1.0, 0.0]) }; - let mut prism = JointData::prismatic(axis) + let mut prism = GenericJoint::prismatic(axis) .local_anchor1(point![0.0, 0.0, shift]) .local_anchor2(point![0.0, 0.0, 0.0]) - .limit_axis(JointAxis::X, [-2.0, 2.0]); + .limits(JointAxis::X, [-2.0, 2.0]); impulse_joints.insert(curr_parent, curr_child, prism); @@ -54,20 +51,17 @@ fn create_actuated_prismatic_joints( let rad = 0.4; let shift = 2.0; - let ground = RigidBodyBuilder::new_static() - .translation(vector![origin.x, origin.y, origin.z]) - .build(); + let ground = RigidBodyBuilder::new_static().translation(vector![origin.x, origin.y, origin.z]); let mut curr_parent = bodies.insert(ground); - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, curr_parent, bodies); for i in 0..num { let z = origin.z + (i + 1) as f32 * shift; - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![origin.x, origin.y, z]) - .build(); + let rigid_body = + RigidBodyBuilder::new_dynamic().translation(vector![origin.x, origin.y, z]); let curr_child = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, curr_child, bodies); let axis = if i % 2 == 0 { @@ -76,13 +70,13 @@ fn create_actuated_prismatic_joints( UnitVector::new_normalize(vector![-1.0, 1.0, 0.0]) }; - let mut prism = JointData::prismatic(axis) + let mut prism = GenericJoint::prismatic(axis) .local_anchor1(point![0.0, 0.0, 0.0]) .local_anchor2(point![0.0, 0.0, -shift]); if i == 1 { prism = prism - .limit_axis(JointAxis::X, [-Real::MAX, 5.0]) + .limits(JointAxis::X, [-Real::MAX, 5.0]) .motor_velocity(JointAxis::X, 1.0, 1.0) // We set a max impulse so that the motor doesn't fight // the limits with large forces. @@ -95,7 +89,7 @@ fn create_actuated_prismatic_joints( // We set a max impulse so that the motor doesn't fight // the limits with large forces. .motor_max_impulse(JointAxis::X, 0.7) - .limit_axis(JointAxis::X, [-2.0, 5.0]); + .limits(JointAxis::X, [-2.0, 5.0]); } impulse_joints.insert(curr_parent, curr_child, prism); @@ -114,11 +108,9 @@ fn create_revolute_joints( let rad = 0.4; let shift = 2.0; - let ground = RigidBodyBuilder::new_static() - .translation(vector![origin.x, origin.y, 0.0]) - .build(); + let ground = RigidBodyBuilder::new_static().translation(vector![origin.x, origin.y, 0.0]); let mut curr_parent = bodies.insert(ground); - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, curr_parent, bodies); for i in 0..num { @@ -133,11 +125,9 @@ fn create_revolute_joints( let mut handles = [curr_parent; 4]; for k in 0..4 { - let rigid_body = RigidBodyBuilder::new_dynamic() - .position(positions[k]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().position(positions[k]); handles[k] = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, handles[k], bodies); } @@ -166,63 +156,41 @@ fn create_revolute_joints_with_limits( impulse_joints: &mut ImpulseJointSet, origin: Point, ) { - let ground = bodies.insert( - RigidBodyBuilder::new_static() - .translation(origin.coords) - .build(), - ); + let ground = bodies.insert(RigidBodyBuilder::new_static().translation(origin.coords)); - let platform1 = bodies.insert( - RigidBodyBuilder::new_dynamic() - .translation(origin.coords) - .build(), - ); - colliders.insert_with_parent( - ColliderBuilder::cuboid(4.0, 0.2, 2.0).build(), - platform1, - bodies, - ); + let platform1 = bodies.insert(RigidBodyBuilder::new_dynamic().translation(origin.coords)); + colliders.insert_with_parent(ColliderBuilder::cuboid(4.0, 0.2, 2.0), platform1, bodies); let shift = vector![0.0, 0.0, 6.0]; - let platform2 = bodies.insert( - RigidBodyBuilder::new_dynamic() - .translation(origin.coords + shift) - .build(), - ); - colliders.insert_with_parent( - ColliderBuilder::cuboid(4.0, 0.2, 2.0).build(), - platform2, - bodies, - ); + let platform2 = + bodies.insert(RigidBodyBuilder::new_dynamic().translation(origin.coords + shift)); + colliders.insert_with_parent(ColliderBuilder::cuboid(4.0, 0.2, 2.0), platform2, bodies); let z = Vector::z_axis(); - let mut joint1 = RevoluteJoint::new(z).limit_axis(JointAxis::X, [-0.2, 0.2]); + let mut joint1 = RevoluteJoint::new(z).limits(JointAxis::X, [-0.2, 0.2]); impulse_joints.insert(ground, platform1, joint1); let mut joint2 = RevoluteJoint::new(z) .local_anchor2(shift.into()) - .limit_axis(JointAxis::Z, [-0.2, 0.2]); + .limits(JointAxis::Z, [-0.2, 0.2]); impulse_joints.insert(platform1, platform2, joint2); // Let’s add a couple of cuboids that will fall on the platforms, triggering the joint limits. let cuboid_body1 = bodies.insert( - RigidBodyBuilder::new_dynamic() - .translation(origin.coords + vector![-2.0, 4.0, 0.0]) - .build(), + RigidBodyBuilder::new_dynamic().translation(origin.coords + vector![-2.0, 4.0, 0.0]), ); colliders.insert_with_parent( - ColliderBuilder::cuboid(0.6, 0.6, 0.6).friction(1.0).build(), + ColliderBuilder::cuboid(0.6, 0.6, 0.6).friction(1.0), cuboid_body1, bodies, ); let cuboid_body2 = bodies.insert( RigidBodyBuilder::new_dynamic() - .translation(origin.coords + shift + vector![2.0, 16.0, 0.0]) - .build(), + .translation(origin.coords + shift + vector![2.0, 16.0, 0.0]), ); colliders.insert_with_parent( - ColliderBuilder::cuboid(0.6, 0.6, 0.6).friction(1.0).build(), + ColliderBuilder::cuboid(0.6, 0.6, 0.6).friction(1.0), cuboid_body2, bodies, ); @@ -254,22 +222,20 @@ fn create_fixed_joints( RigidBodyType::Dynamic }; - let rigid_body = RigidBodyBuilder::new(status) - .translation(vector![ - origin.x + fk * shift, - origin.y, - origin.z + fi * shift - ]) - .build(); + let rigid_body = RigidBodyBuilder::new(status).translation(vector![ + origin.x + fk * shift, + origin.y, + origin.z + fi * shift + ]); let child_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::ball(rad).build(); + let collider = ColliderBuilder::ball(rad); colliders.insert_with_parent(collider, child_handle, bodies); // Vertical joint. if i > 0 { let parent_index = body_handles.len() - num; let parent_handle = body_handles[parent_index]; - let joint = JointData::fixed().local_anchor2(point![0.0, 0.0, -shift]); + let joint = GenericJoint::fixed().local_anchor2(point![0.0, 0.0, -shift]); impulse_joints.insert(parent_handle, child_handle, joint); } @@ -277,7 +243,7 @@ fn create_fixed_joints( if k > 0 { let parent_index = body_handles.len() - 1; let parent_handle = body_handles[parent_index]; - let joint = JointData::fixed().local_anchor2(point![-shift, 0.0, 0.0]); + let joint = GenericJoint::fixed().local_anchor2(point![-shift, 0.0, 0.0]); impulse_joints.insert(parent_handle, child_handle, joint); } @@ -308,17 +274,19 @@ fn create_ball_joints( RigidBodyType::Dynamic }; - let rigid_body = RigidBodyBuilder::new(status) - .translation(vector![fk * shift, 0.0, fi * shift * 2.0]) - .build(); + let rigid_body = RigidBodyBuilder::new(status).translation(vector![ + fk * shift, + 0.0, + fi * shift * 2.0 + ]); let child_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::capsule_z(rad * 1.25, rad).build(); + let collider = ColliderBuilder::capsule_z(rad * 1.25, rad); colliders.insert_with_parent(collider, child_handle, bodies); // Vertical joint. if i > 0 { let parent_handle = *body_handles.last().unwrap(); - let joint = JointData::ball().local_anchor2(point![0.0, 0.0, -shift * 2.0]); + let joint = GenericJoint::ball().local_anchor2(point![0.0, 0.0, -shift * 2.0]); impulse_joints.insert(parent_handle, child_handle, joint); } @@ -326,7 +294,7 @@ fn create_ball_joints( if k > 0 { let parent_index = body_handles.len() - num; let parent_handle = body_handles[parent_index]; - let joint = JointData::ball().local_anchor2(point![-shift, 0.0, 0.0]); + let joint = GenericJoint::ball().local_anchor2(point![-shift, 0.0, 0.0]); impulse_joints.insert(parent_handle, child_handle, joint); } @@ -343,45 +311,29 @@ fn create_ball_joints_with_limits( ) { let shift = vector![0.0, 0.0, 3.0]; - let ground = bodies.insert( - RigidBodyBuilder::new_static() - .translation(origin.coords) - .build(), - ); + let ground = bodies.insert(RigidBodyBuilder::new_static().translation(origin.coords)); let ball1 = bodies.insert( RigidBodyBuilder::new_dynamic() .translation(origin.coords + shift) - .linvel(vector![20.0, 20.0, 0.0]) - .build(), - ); - colliders.insert_with_parent( - ColliderBuilder::cuboid(1.0, 1.0, 1.0).build(), - ball1, - bodies, + .linvel(vector![20.0, 20.0, 0.0]), ); + colliders.insert_with_parent(ColliderBuilder::cuboid(1.0, 1.0, 1.0), ball1, bodies); - let ball2 = bodies.insert( - RigidBodyBuilder::new_dynamic() - .translation(origin.coords + shift * 2.0) - .build(), - ); - colliders.insert_with_parent( - ColliderBuilder::cuboid(1.0, 1.0, 1.0).build(), - ball2, - bodies, - ); + let ball2 = + bodies.insert(RigidBodyBuilder::new_dynamic().translation(origin.coords + shift * 2.0)); + colliders.insert_with_parent(ColliderBuilder::cuboid(1.0, 1.0, 1.0), ball2, bodies); - let mut joint1 = JointData::ball() + let mut joint1 = GenericJoint::ball() .local_anchor2(Point::from(-shift)) - .limit_axis(JointAxis::X, [-0.2, 0.2]) - .limit_axis(JointAxis::Y, [-0.2, 0.2]); + .limits(JointAxis::X, [-0.2, 0.2]) + .limits(JointAxis::Y, [-0.2, 0.2]); impulse_joints.insert(ground, ball1, joint1); - let mut joint2 = JointData::ball() + let mut joint2 = GenericJoint::ball() .local_anchor2(Point::from(-shift)) - .limit_axis(JointAxis::X, [-0.3, 0.3]) - .limit_axis(JointAxis::Y, [-0.3, 0.3]); + .limits(JointAxis::X, [-0.3, 0.3]) + .limits(JointAxis::Y, [-0.3, 0.3]); impulse_joints.insert(ball1, ball2, joint2); } @@ -418,26 +370,28 @@ fn create_actuated_revolute_joints( let rigid_body = RigidBodyBuilder::new(status) .translation(vector![origin.x, origin.y + shifty, origin.z + fi * shift]) // .rotation(Vector3::new(0.0, fi * 1.1, 0.0)) - .build(); + ; let child_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad * 2.0, rad * 6.0 / (fi + 1.0), rad).build(); + let collider = ColliderBuilder::cuboid(rad * 2.0, rad * 6.0 / (fi + 1.0), rad); colliders.insert_with_parent(collider, child_handle, bodies); if i > 0 { - let mut joint = joint_template.clone(); + let mut joint = joint_template + .clone() + .motor_model(MotorModel::AccelerationBased); if i % 3 == 1 { - joint = joint.motor_velocity(JointAxis::AngX, -20.0, 0.1); + joint.set_motor_velocity(JointAxis::AngX, -20.0, 0.1); } else if i == num - 1 { let stiffness = 0.2; let damping = 1.0; - joint = joint.motor_position(JointAxis::AngX, 3.14 / 2.0, stiffness, damping); + jointset_.motor_position(JointAxis::AngX, 3.14 / 2.0, stiffness, damping); } if i == 1 { joint.local_frame2.translation.vector.y = 2.0; - joint = joint.motor_velocity(JointAxis::AngX, -2.0, 0.1); + joint.set_motor_velocity(JointAxis::AngX, -2.0, 0.1); } impulse_joints.insert(parent_handle, child_handle, joint); @@ -458,7 +412,7 @@ fn create_actuated_ball_joints( let shift = 2.0; // We will reuse this base configuration for all the impulse_joints here. - let joint_template = JointData::ball().local_anchor1(point![0.0, 0.0, shift]); + let joint_template = GenericJoint::ball().local_anchor1(point![0.0, 0.0, shift]); let mut parent_handle = RigidBodyHandle::invalid(); @@ -477,10 +431,10 @@ fn create_actuated_ball_joints( let rigid_body = RigidBodyBuilder::new(status) .translation(vector![origin.x, origin.y, origin.z + fi * shift]) // .rotation(Vector3::new(0.0, fi * 1.1, 0.0)) - .build(); + ; let child_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::capsule_y(rad * 2.0 / (fi + 1.0), rad).build(); + let collider = ColliderBuilder::capsule_y(rad * 2.0 / (fi + 1.0), rad); colliders.insert_with_parent(collider, child_handle, bodies); if i > 0 { @@ -516,40 +470,40 @@ pub fn init_world(testbed: &mut Testbed) { let mut impulse_joints = ImpulseJointSet::new(); let multibody_joints = MultibodyJointSet::new(); - create_prismatic_joints( - &mut bodies, - &mut colliders, - &mut impulse_joints, - point![20.0, 5.0, 0.0], - 4, - ); - create_actuated_prismatic_joints( - &mut bodies, - &mut colliders, - &mut impulse_joints, - point![25.0, 5.0, 0.0], - 4, - ); - create_revolute_joints( - &mut bodies, - &mut colliders, - &mut impulse_joints, - point![20.0, 0.0, 0.0], - 3, - ); - create_revolute_joints_with_limits( - &mut bodies, - &mut colliders, - &mut impulse_joints, - point![34.0, 0.0, 0.0], - ); - create_fixed_joints( - &mut bodies, - &mut colliders, - &mut impulse_joints, - point![0.0, 10.0, 0.0], - 10, - ); + // create_prismatic_joints( + // &mut bodies, + // &mut colliders, + // &mut impulse_joints, + // point![20.0, 5.0, 0.0], + // 4, + // ); + // create_actuated_prismatic_joints( + // &mut bodies, + // &mut colliders, + // &mut impulse_joints, + // point![25.0, 5.0, 0.0], + // 4, + // ); + // create_revolute_joints( + // &mut bodies, + // &mut colliders, + // &mut impulse_joints, + // point![20.0, 0.0, 0.0], + // 3, + // ); + // create_revolute_joints_with_limits( + // &mut bodies, + // &mut colliders, + // &mut impulse_joints, + // point![34.0, 0.0, 0.0], + // ); + // create_fixed_joints( + // &mut bodies, + // &mut colliders, + // &mut impulse_joints, + // point![0.0, 10.0, 0.0], + // 10, + // ); create_actuated_revolute_joints( &mut bodies, &mut colliders, @@ -557,20 +511,20 @@ pub fn init_world(testbed: &mut Testbed) { point![20.0, 10.0, 0.0], 6, ); - create_actuated_ball_joints( - &mut bodies, - &mut colliders, - &mut impulse_joints, - point![13.0, 10.0, 0.0], - 3, - ); - create_ball_joints(&mut bodies, &mut colliders, &mut impulse_joints, 15); - create_ball_joints_with_limits( - &mut bodies, - &mut colliders, - &mut impulse_joints, - point![-5.0, 0.0, 0.0], - ); + // create_actuated_ball_joints( + // &mut bodies, + // &mut colliders, + // &mut impulse_joints, + // point![13.0, 10.0, 0.0], + // 3, + // ); + // create_ball_joints(&mut bodies, &mut colliders, &mut impulse_joints, 15); + // create_ball_joints_with_limits( + // &mut bodies, + // &mut colliders, + // &mut impulse_joints, + // point![-5.0, 0.0, 0.0], + // ); /* * Set up the testbed. diff --git a/examples3d/keva3.rs b/examples3d/keva3.rs index ad9e1ae..30e5246 100644 --- a/examples3d/keva3.rs +++ b/examples3d/keva3.rs @@ -38,15 +38,13 @@ pub fn build_block( }; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![ - x + dim.x + shift.x, - y + dim.y + shift.y, - z + dim.z + shift.z - ]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![ + x + dim.x + shift.x, + y + dim.y + shift.y, + z + dim.z + shift.z + ]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z).build(); + let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z); colliders.insert_with_parent(collider, handle, bodies); testbed.set_initial_body_color(handle, color0); @@ -61,15 +59,13 @@ pub fn build_block( for i in 0..(block_width / (dim.x as f32 * 2.0)) as usize { for j in 0..(block_width / (dim.z as f32 * 2.0)) as usize { // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![ - i as f32 * dim.x * 2.0 + dim.x + shift.x, - dim.y + shift.y + block_height, - j as f32 * dim.z * 2.0 + dim.z + shift.z - ]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![ + i as f32 * dim.x * 2.0 + dim.x + shift.x, + dim.y + shift.y + block_height, + j as f32 * dim.z * 2.0 + dim.z + shift.z + ]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z).build(); + let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z); colliders.insert_with_parent(collider, handle, bodies); testbed.set_initial_body_color(handle, color0); std::mem::swap(&mut color0, &mut color1); @@ -92,11 +88,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 50.0; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); colliders.insert_with_parent(collider, handle, &mut bodies); /* diff --git a/examples3d/locked_rotations3.rs b/examples3d/locked_rotations3.rs index 5e925b0..e45aefd 100644 --- a/examples3d/locked_rotations3.rs +++ b/examples3d/locked_rotations3.rs @@ -19,11 +19,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 5.0; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -32,10 +30,9 @@ pub fn init_world(testbed: &mut Testbed) { let rigid_body = RigidBodyBuilder::new_dynamic() .translation(vector![0.0, 3.0, 0.0]) .lock_translations() - .restrict_rotations(true, false, false) - .build(); + .restrict_rotations(true, false, false); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(0.2, 0.6, 2.0).build(); + let collider = ColliderBuilder::cuboid(0.2, 0.6, 2.0); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -44,12 +41,11 @@ pub fn init_world(testbed: &mut Testbed) { let rigid_body = RigidBodyBuilder::new_dynamic() .translation(vector![0.0, 5.0, 0.0]) .rotation(Vector::x() * 1.0) - .lock_rotations() - .build(); + .lock_rotations(); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::capsule_y(0.6, 0.4).build(); + let collider = ColliderBuilder::capsule_y(0.6, 0.4); colliders.insert_with_parent(collider, handle, &mut bodies); - let collider = ColliderBuilder::capsule_x(0.6, 0.4).build(); + let collider = ColliderBuilder::capsule_x(0.6, 0.4); colliders.insert_with_parent(collider, handle, &mut bodies); /* diff --git a/examples3d/one_way_platforms3.rs b/examples3d/one_way_platforms3.rs index 4857ae6..b82f564 100644 --- a/examples3d/one_way_platforms3.rs +++ b/examples3d/one_way_platforms3.rs @@ -68,18 +68,16 @@ pub fn init_world(testbed: &mut Testbed) { /* * Ground */ - let rigid_body = RigidBodyBuilder::new_static().build(); + let rigid_body = RigidBodyBuilder::new_static(); let handle = bodies.insert(rigid_body); let collider = ColliderBuilder::cuboid(9.0, 0.5, 25.0) .translation(vector![0.0, 2.0, 30.0]) - .active_hooks(ActiveHooks::MODIFY_SOLVER_CONTACTS) - .build(); + .active_hooks(ActiveHooks::MODIFY_SOLVER_CONTACTS); let platform1 = colliders.insert_with_parent(collider, handle, &mut bodies); let collider = ColliderBuilder::cuboid(9.0, 0.5, 25.0) .translation(vector![0.0, -2.0, -30.0]) - .active_hooks(ActiveHooks::MODIFY_SOLVER_CONTACTS) - .build(); + .active_hooks(ActiveHooks::MODIFY_SOLVER_CONTACTS); let platform2 = colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -97,10 +95,8 @@ pub fn init_world(testbed: &mut Testbed) { testbed.add_callback(move |graphics, physics, _, run_state| { if run_state.timestep_id % 50 == 0 && physics.bodies.len() <= 7 { // Spawn a new cube. - let collider = ColliderBuilder::cuboid(1.0, 2.0, 1.5).build(); - let body = RigidBodyBuilder::new_dynamic() - .translation(vector![0.0, 6.0, 20.0]) - .build(); + let collider = ColliderBuilder::cuboid(1.0, 2.0, 1.5); + let body = RigidBodyBuilder::new_dynamic().translation(vector![0.0, 6.0, 20.0]); let handle = physics.bodies.insert(body); physics .colliders diff --git a/examples3d/platform3.rs b/examples3d/platform3.rs index d1126ec..5f757fa 100644 --- a/examples3d/platform3.rs +++ b/examples3d/platform3.rs @@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 10.0; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -42,11 +40,9 @@ pub fn init_world(testbed: &mut Testbed) { let z = k as f32 * shift - centerz; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y, z]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, handle, &mut bodies); } } @@ -55,21 +51,25 @@ pub fn init_world(testbed: &mut Testbed) { /* * Setup a velocity-based kinematic rigid body. */ - let platform_body = RigidBodyBuilder::new_kinematic_velocity_based() - .translation(vector![0.0, 1.5 + 0.8, -10.0 * rad]) - .build(); + let platform_body = RigidBodyBuilder::new_kinematic_velocity_based().translation(vector![ + 0.0, + 1.5 + 0.8, + -10.0 * rad + ]); let velocity_based_platform_handle = bodies.insert(platform_body); - let collider = ColliderBuilder::cuboid(rad * 10.0, rad, rad * 10.0).build(); + let collider = ColliderBuilder::cuboid(rad * 10.0, rad, rad * 10.0); colliders.insert_with_parent(collider, velocity_based_platform_handle, &mut bodies); /* * Setup a position-based kinematic rigid body. */ - let platform_body = RigidBodyBuilder::new_kinematic_position_based() - .translation(vector![0.0, 2.0 + 1.5 + 0.8, -10.0 * rad]) - .build(); + let platform_body = RigidBodyBuilder::new_kinematic_position_based().translation(vector![ + 0.0, + 2.0 + 1.5 + 0.8, + -10.0 * rad + ]); let position_based_platform_handle = bodies.insert(platform_body); - let collider = ColliderBuilder::cuboid(rad * 10.0, rad, rad * 10.0).build(); + let collider = ColliderBuilder::cuboid(rad * 10.0, rad, rad * 10.0); colliders.insert_with_parent(collider, position_based_platform_handle, &mut bodies); /* diff --git a/examples3d/primitives3.rs b/examples3d/primitives3.rs index c9d0959..a664308 100644 --- a/examples3d/primitives3.rs +++ b/examples3d/primitives3.rs @@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 100.1; let ground_height = 2.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -46,19 +44,17 @@ pub fn init_world(testbed: &mut Testbed) { let z = k as f32 * shiftz - centerz + offset; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y, z]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]); let handle = bodies.insert(rigid_body); let collider = match j % 5 { - // _ => ColliderBuilder::cuboid(rad, rad, rad).build(), - 1 => ColliderBuilder::ball(rad).build(), + // _ => ColliderBuilder::cuboid(rad, rad, rad), + 1 => ColliderBuilder::ball(rad), // Rounded cylinders are much more efficient that cylinder, even if the // rounding margin is small. - 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(), - 3 => ColliderBuilder::cone(rad, rad).build(), - _ => ColliderBuilder::capsule_y(rad, rad).build(), + 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0), + 3 => ColliderBuilder::cone(rad, rad), + _ => ColliderBuilder::capsule_y(rad, rad), }; colliders.insert_with_parent(collider, handle, &mut bodies); diff --git a/examples3d/restitution3.rs b/examples3d/restitution3.rs index 00462d1..834fe22 100644 --- a/examples3d/restitution3.rs +++ b/examples3d/restitution3.rs @@ -16,13 +16,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 20.; let ground_height = 1.0; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, 2.0) - .restitution(1.0) - .build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, 2.0).restitution(1.0); colliders.insert_with_parent(collider, handle, &mut bodies); let num = 10; @@ -31,13 +27,13 @@ pub fn init_world(testbed: &mut Testbed) { for j in 0..2 { for i in 0..=num { let x = (i as f32) - num as f32 / 2.0; - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x * 2.0, 10.0 * (j as f32 + 1.0), 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![ + x * 2.0, + 10.0 * (j as f32 + 1.0), + 0.0 + ]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::ball(rad) - .restitution((i as f32) / (num as f32)) - .build(); + let collider = ColliderBuilder::ball(rad).restitution((i as f32) / (num as f32)); colliders.insert_with_parent(collider, handle, &mut bodies); } } diff --git a/examples3d/sensor3.rs b/examples3d/sensor3.rs index ebd47f3..e09359d 100644 --- a/examples3d/sensor3.rs +++ b/examples3d/sensor3.rs @@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) { let ground_size = 200.1; let ground_height = 0.1; - let rigid_body = RigidBodyBuilder::new_static() - .translation(vector![0.0, -ground_height, 0.0]) - .build(); + let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]); let ground_handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size); colliders.insert_with_parent(collider, ground_handle, &mut bodies); /* @@ -40,11 +38,9 @@ pub fn init_world(testbed: &mut Testbed) { let z = k as f32 * shift - centerz; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y, z]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, handle, &mut bodies); testbed.set_initial_body_color(handle, [0.5, 0.5, 1.0]); @@ -56,14 +52,12 @@ pub fn init_world(testbed: &mut Testbed) { */ // Rigid body so that the sensor can move. - let sensor = RigidBodyBuilder::new_dynamic() - .translation(vector![0.0, 5.0, 0.0]) - .build(); + let sensor = RigidBodyBuilder::new_dynamic().translation(vector![0.0, 5.0, 0.0]); let sensor_handle = bodies.insert(sensor); // Solid cube attached to the sensor which // other colliders can touch. - let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + let collider = ColliderBuilder::cuboid(rad, rad, rad); colliders.insert_with_parent(collider, sensor_handle, &mut bodies); // We create a collider desc without density because we don't @@ -71,8 +65,7 @@ pub fn init_world(testbed: &mut Testbed) { let sensor_collider = ColliderBuilder::ball(rad * 5.0) .density(0.0) .sensor(true) - .active_events(ActiveEvents::INTERSECTION_EVENTS) - .build(); + .active_events(ActiveEvents::INTERSECTION_EVENTS); colliders.insert_with_parent(sensor_collider, sensor_handle, &mut bodies); testbed.set_initial_body_color(sensor_handle, [0.5, 1.0, 1.0]); diff --git a/examples3d/trimesh3.rs b/examples3d/trimesh3.rs index 31a532b..1985671 100644 --- a/examples3d/trimesh3.rs +++ b/examples3d/trimesh3.rs @@ -36,9 +36,9 @@ pub fn init_world(testbed: &mut Testbed) { let heightfield = HeightField::new(heights, ground_size); let (vertices, indices) = heightfield.to_trimesh(); - let rigid_body = RigidBodyBuilder::new_static().build(); + let rigid_body = RigidBodyBuilder::new_static(); let handle = bodies.insert(rigid_body); - let collider = ColliderBuilder::trimesh(vertices, indices).build(); + let collider = ColliderBuilder::trimesh(vertices, indices); colliders.insert_with_parent(collider, handle, &mut bodies); /* @@ -60,19 +60,17 @@ pub fn init_world(testbed: &mut Testbed) { let z = k as f32 * shift - centerz; // Build the rigid body. - let rigid_body = RigidBodyBuilder::new_dynamic() - .translation(vector![x, y, z]) - .build(); + let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]); let handle = bodies.insert(rigid_body); let collider = match j % 6 { - 0 => ColliderBuilder::cuboid(rad, rad, rad).build(), - 1 => ColliderBuilder::ball(rad).build(), + 0 => ColliderBuilder::cuboid(rad, rad, rad), + 1 => ColliderBuilder::ball(rad), // Rounded cylinders are much more efficient that cylinder, even if the // rounding margin is small. - 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(), - 3 => ColliderBuilder::cone(rad, rad).build(), - 4 => ColliderBuilder::capsule_y(rad, rad).build(), + 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0), + 3 => ColliderBuilder::cone(rad, rad), + 4 => ColliderBuilder::capsule_y(rad, rad), _ => { let shapes = vec![ ( @@ -89,7 +87,7 @@ pub fn init_world(testbed: &mut Testbed) { ), ]; - ColliderBuilder::compound(shapes).build() + ColliderBuilder::compound(shapes) } }; diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs index 9e3f686..bab0fe2 100644 --- a/src/dynamics/integration_parameters.rs +++ b/src/dynamics/integration_parameters.rs @@ -18,10 +18,6 @@ pub struct IntegrationParameters { /// to numerical instabilities. pub min_ccd_dt: Real, - /// 0-1: how much of the velocity to dampen out in the constraint solver? - /// (default `1.0`). - pub velocity_solve_fraction: Real, - /// 0-1: multiplier for how much of the constraint violation (e.g. contact penetration) /// will be compensated for during the velocity solve. /// If zero, you need to enable the positional solver. @@ -35,6 +31,9 @@ pub struct IntegrationParameters { /// (default `0.25`). pub damping_ratio: Real, + pub joint_erp: Real, + pub joint_damping_ratio: Real, + /// Amount of penetration the engine wont attempt to correct (default: `0.001m`). pub allowed_linear_error: Real, /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`). @@ -89,12 +88,17 @@ impl IntegrationParameters { /// The ERP coefficient, multiplied by the inverse timestep length. pub fn erp_inv_dt(&self) -> Real { - 0.8 / self.dt + self.erp / self.dt + } + + /// The joint ERP coefficient, multiplied by the inverse timestep length. + pub fn joint_erp_inv_dt(&self) -> Real { + self.joint_erp / self.dt } /// The CFM factor to be used in the constraints resolution. pub fn cfm_factor(&self) -> Real { - // Compute CFM assuming a critically damped spring multiplied by the dampingratio. + // Compute CFM assuming a critically damped spring multiplied by the damping ratio. let inv_erp_minus_one = 1.0 / self.erp - 1.0; // let stiffness = 4.0 * damping_ratio * damping_ratio * projected_mass @@ -124,6 +128,16 @@ impl IntegrationParameters { // in the constraints solver. 1.0 / (1.0 + cfm_coeff) } + + pub fn joint_cfm_coeff(&self) -> Real { + // Compute CFM assuming a critically damped spring multiplied by the damping ratio. + let inv_erp_minus_one = 1.0 / self.joint_erp - 1.0; + inv_erp_minus_one * inv_erp_minus_one + / ((1.0 + inv_erp_minus_one) + * 4.0 + * self.joint_damping_ratio + * self.joint_damping_ratio) + } } impl Default for IntegrationParameters { @@ -131,9 +145,10 @@ impl Default for IntegrationParameters { Self { dt: 1.0 / 60.0, min_ccd_dt: 1.0 / 60.0 / 100.0, - velocity_solve_fraction: 1.0, erp: 0.8, damping_ratio: 0.25, + joint_erp: 1.0, + joint_damping_ratio: 1.0, allowed_linear_error: 0.001, // 0.005 prediction_distance: 0.002, max_velocity_iterations: 4, diff --git a/src/dynamics/island_manager.rs b/src/dynamics/island_manager.rs index 631cf7a..7cf16d6 100644 --- a/src/dynamics/island_manager.rs +++ b/src/dynamics/island_manager.rs @@ -136,36 +136,6 @@ impl IslandManager { .chain(self.active_kinematic_set.iter().copied()) } - /* - #[cfg(feature = "parallel")] - #[inline(always)] - #[allow(dead_code)] - pub(crate) fn foreach_active_island_body_mut_internal_parallel( - &self, - island_id: usize, - bodies: &mut Set, - f: impl Fn(RigidBodyHandle, &mut RigidBody) + Send + Sync, - ) where - Set: ComponentSet, - { - use std::sync::atomic::Ordering; - - let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1]; - let bodies = std::sync::atomic::AtomicPtr::new(&mut bodies as *mut _); - self.active_dynamic_set[island_range] - .par_iter() - .for_each_init( - || bodies.load(Ordering::Relaxed), - |bodies, handle| { - let bodies: &mut Set = unsafe { std::mem::transmute(*bodies) }; - if let Some(rb) = bodies.get_mut_internal(handle.0) { - f(*handle, rb) - } - }, - ); - } - */ - #[cfg(feature = "parallel")] pub(crate) fn active_island_range(&self, island_id: usize) -> std::ops::Range { self.active_islands[island_id]..self.active_islands[island_id + 1] @@ -203,7 +173,7 @@ impl IslandManager { // NOTE: the `.rev()` is here so that two successive timesteps preserve // the order of the bodies in the `active_dynamic_set` vec. This reversal // does not seem to affect performances nor stability. However it makes - // debugging slightly nicer so we keep this rev. + // debugging slightly nicer. for h in self.active_dynamic_set.drain(..).rev() { let can_sleep = &mut self.can_sleep; let stack = &mut self.stack; diff --git a/src/dynamics/joint/fixed_joint.rs b/src/dynamics/joint/fixed_joint.rs index c7ca904..192b7d9 100644 --- a/src/dynamics/joint/fixed_joint.rs +++ b/src/dynamics/joint/fixed_joint.rs @@ -1,10 +1,11 @@ -use crate::dynamics::{JointAxesMask, JointData}; +use crate::dynamics::{GenericJoint, GenericJointBuilder, JointAxesMask}; use crate::math::{Isometry, Point, Real}; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] +#[repr(transparent)] pub struct FixedJoint { - data: JointData, + data: GenericJoint, } impl Default for FixedJoint { @@ -14,48 +15,100 @@ impl Default for FixedJoint { } impl FixedJoint { + #[must_use] pub fn new() -> Self { - #[cfg(feature = "dim2")] - let mask = JointAxesMask::X | JointAxesMask::Y | JointAxesMask::ANG_X; - #[cfg(feature = "dim3")] - let mask = JointAxesMask::X - | JointAxesMask::Y - | JointAxesMask::Z - | JointAxesMask::ANG_X - | JointAxesMask::ANG_Y - | JointAxesMask::ANG_Z; - - let data = JointData::default().lock_axes(mask); + let data = GenericJointBuilder::new(JointAxesMask::LOCKED_FIXED_AXES).build(); Self { data } } + #[must_use] + pub fn local_frame1(&self) -> &Isometry { + &self.data.local_frame1 + } + + pub fn set_local_frame1(&mut self, local_frame: Isometry) -> &mut Self { + self.data.set_local_frame1(local_frame); + self + } + + #[must_use] + pub fn local_frame2(&self) -> &Isometry { + &self.data.local_frame2 + } + + pub fn set_local_frame2(&mut self, local_frame: Isometry) -> &mut Self { + self.data.set_local_frame2(local_frame); + self + } + + #[must_use] + pub fn local_anchor1(&self) -> Point { + self.data.local_anchor1() + } + + pub fn set_local_anchor1(&mut self, anchor1: Point) -> &mut Self { + self.data.set_local_anchor1(anchor1); + self + } + + #[must_use] + pub fn local_anchor2(&self) -> Point { + self.data.local_anchor2() + } + + pub fn set_local_anchor2(&mut self, anchor2: Point) -> &mut Self { + self.data.set_local_anchor2(anchor2); + self + } +} + +impl Into for FixedJoint { + fn into(self) -> GenericJoint { + self.data + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, PartialEq, Default)] +pub struct FixedJointBuilder(FixedJoint); + +impl FixedJointBuilder { + pub fn new() -> Self { + Self(FixedJoint::new()) + } + #[must_use] pub fn local_frame1(mut self, local_frame: Isometry) -> Self { - self.data = self.data.local_frame1(local_frame); + self.0.set_local_frame1(local_frame); self } #[must_use] pub fn local_frame2(mut self, local_frame: Isometry) -> Self { - self.data = self.data.local_frame2(local_frame); + self.0.set_local_frame2(local_frame); self } #[must_use] pub fn local_anchor1(mut self, anchor1: Point) -> Self { - self.data = self.data.local_anchor1(anchor1); + self.0.set_local_anchor1(anchor1); self } #[must_use] pub fn local_anchor2(mut self, anchor2: Point) -> Self { - self.data = self.data.local_anchor2(anchor2); + self.0.set_local_anchor2(anchor2); self } + + #[must_use] + pub fn build(self) -> FixedJoint { + self.0 + } } -impl Into for FixedJoint { - fn into(self) -> JointData { - self.data +impl Into for FixedJointBuilder { + fn into(self) -> GenericJoint { + self.0.into() } } diff --git a/src/dynamics/joint/generic_joint.rs b/src/dynamics/joint/generic_joint.rs new file mode 100644 index 0000000..7455b0d --- /dev/null +++ b/src/dynamics/joint/generic_joint.rs @@ -0,0 +1,501 @@ +use na::SimdRealField; + +use crate::dynamics::solver::MotorParameters; +use crate::dynamics::{FixedJoint, MotorModel, PrismaticJoint, RevoluteJoint}; +use crate::math::{Isometry, Point, Real, Rotation, UnitVector, Vector, SPATIAL_DIM}; +use crate::utils::WBasis; + +#[cfg(feature = "dim3")] +use crate::dynamics::SphericalJoint; + +#[cfg(feature = "dim3")] +bitflags::bitflags! { + #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] + pub struct JointAxesMask: u8 { + const X = 1 << 0; + const Y = 1 << 1; + const Z = 1 << 2; + const ANG_X = 1 << 3; + const ANG_Y = 1 << 4; + const ANG_Z = 1 << 5; + const LOCKED_REVOLUTE_AXES = Self::X.bits | Self::Y.bits | Self::Z.bits | Self::ANG_Y.bits | Self::ANG_Z.bits; + const LOCKED_PRISMATIC_AXES = Self::Y.bits | Self::Z.bits | Self::ANG_X.bits | Self::ANG_Y.bits | Self::ANG_Z.bits; + const LOCKED_FIXED_AXES = Self::X.bits | Self::Y.bits | Self::Z.bits | Self::ANG_X.bits | Self::ANG_Y.bits | Self::ANG_Z.bits; + const LOCKED_SPHERICAL_AXES = Self::X.bits | Self::Y.bits | Self::Z.bits; + const FREE_REVOLUTE_AXES = Self::ANG_X.bits; + const FREE_PRISMATIC_AXES = Self::X.bits; + const FREE_FIXED_AXES = 0; + const FREE_SPHERICAL_AXES = Self::ANG_X.bits | Self::ANG_Y.bits | Self::ANG_Z.bits; + } +} + +#[cfg(feature = "dim2")] +bitflags::bitflags! { + #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] + pub struct JointAxesMask: u8 { + const X = 1 << 0; + const Y = 1 << 1; + const ANG_X = 1 << 2; + const LOCKED_REVOLUTE_AXES = Self::X.bits | Self::Y.bits; + const LOCKED_PRISMATIC_AXES = Self::Y.bits | Self::ANG_X.bits; + const LOCKED_FIXED_AXES = Self::X.bits | Self::Y.bits | Self::ANG_X.bits; + const FREE_REVOLUTE_AXES = Self::ANG_X.bits; + const FREE_PRISMATIC_AXES = Self::X.bits; + const FREE_FIXED_AXES = 0; + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, PartialEq)] +pub enum JointAxis { + X = 0, + Y, + #[cfg(feature = "dim3")] + Z, + AngX, + #[cfg(feature = "dim3")] + AngY, + #[cfg(feature = "dim3")] + AngZ, +} + +impl From for JointAxesMask { + fn from(axis: JointAxis) -> Self { + JointAxesMask::from_bits(1 << axis as usize).unwrap() + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct JointLimits { + pub min: N, + pub max: N, + pub impulse: N, +} + +impl> Default for JointLimits { + fn default() -> Self { + Self { + min: -N::splat(Real::MAX), + max: N::splat(Real::MAX), + impulse: N::splat(0.0), + } + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct JointMotor { + pub target_vel: Real, + pub target_pos: Real, + pub stiffness: Real, + pub damping: Real, + pub max_force: Real, + pub impulse: Real, + pub model: MotorModel, +} + +impl Default for JointMotor { + fn default() -> Self { + Self { + target_pos: 0.0, + target_vel: 0.0, + stiffness: 0.0, + damping: 0.0, + max_force: Real::MAX, + impulse: 0.0, + model: MotorModel::AccelerationBased, // VelocityBased, + } + } +} + +impl JointMotor { + pub(crate) fn motor_params(&self, dt: Real) -> MotorParameters { + let (erp_inv_dt, cfm_coeff, cfm_gain) = + self.model + .combine_coefficients(dt, self.stiffness, self.damping); + MotorParameters { + erp_inv_dt, + cfm_coeff, + cfm_gain, + // keep_lhs, + target_pos: self.target_pos, + target_vel: self.target_vel, + max_impulse: self.max_force * dt, + } + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct GenericJoint { + pub local_frame1: Isometry, + pub local_frame2: Isometry, + pub locked_axes: JointAxesMask, + pub limit_axes: JointAxesMask, + pub motor_axes: JointAxesMask, + pub limits: [JointLimits; SPATIAL_DIM], + pub motors: [JointMotor; SPATIAL_DIM], +} + +impl Default for GenericJoint { + fn default() -> Self { + Self { + local_frame1: Isometry::identity(), + local_frame2: Isometry::identity(), + locked_axes: JointAxesMask::empty(), + limit_axes: JointAxesMask::empty(), + motor_axes: JointAxesMask::empty(), + limits: [JointLimits::default(); SPATIAL_DIM], + motors: [JointMotor::default(); SPATIAL_DIM], + } + } +} + +impl GenericJoint { + #[must_use] + pub fn new(locked_axes: JointAxesMask) -> Self { + *Self::default().lock_axes(locked_axes) + } + + /// Can this joint use SIMD-accelerated constraint formulations? + pub(crate) fn supports_simd_constraints(&self) -> bool { + self.limit_axes.is_empty() && self.motor_axes.is_empty() + } + + fn complete_ang_frame(axis: UnitVector) -> Rotation { + let basis = axis.orthonormal_basis(); + + #[cfg(feature = "dim2")] + { + use na::{Matrix2, Rotation2, UnitComplex}; + let mat = Matrix2::from_columns(&[axis.into_inner(), basis[0]]); + let rotmat = Rotation2::from_matrix_unchecked(mat); + UnitComplex::from_rotation_matrix(&rotmat) + } + + #[cfg(feature = "dim3")] + { + use na::{Matrix3, Rotation3, UnitQuaternion}; + let mat = Matrix3::from_columns(&[axis.into_inner(), basis[0], basis[1]]); + let rotmat = Rotation3::from_matrix_unchecked(mat); + UnitQuaternion::from_rotation_matrix(&rotmat) + } + } + + pub fn lock_axes(&mut self, axes: JointAxesMask) -> &mut Self { + self.locked_axes |= axes; + self + } + + pub fn set_local_frame1(&mut self, local_frame: Isometry) -> &mut Self { + self.local_frame1 = local_frame; + self + } + + pub fn set_local_frame2(&mut self, local_frame: Isometry) -> &mut Self { + self.local_frame2 = local_frame; + self + } + + #[must_use] + pub fn local_axis1(&self) -> UnitVector { + self.local_frame1 * Vector::x_axis() + } + + pub fn set_local_axis1(&mut self, local_axis: UnitVector) -> &mut Self { + self.local_frame1.rotation = Self::complete_ang_frame(local_axis); + self + } + + #[must_use] + pub fn local_axis2(&self) -> UnitVector { + self.local_frame2 * Vector::x_axis() + } + + pub fn set_local_axis2(&mut self, local_axis: UnitVector) -> &mut Self { + self.local_frame2.rotation = Self::complete_ang_frame(local_axis); + self + } + + #[must_use] + pub fn local_anchor1(&self) -> Point { + self.local_frame1.translation.vector.into() + } + + pub fn set_local_anchor1(&mut self, anchor1: Point) -> &mut Self { + self.local_frame1.translation.vector = anchor1.coords; + self + } + + #[must_use] + pub fn local_anchor2(&self) -> Point { + self.local_frame2.translation.vector.into() + } + + pub fn set_local_anchor2(&mut self, anchor2: Point) -> &mut Self { + self.local_frame2.translation.vector = anchor2.coords; + self + } + + #[must_use] + pub fn limits(&self, axis: JointAxis) -> Option<&JointLimits> { + let i = axis as usize; + if self.limit_axes.contains(axis.into()) { + Some(&self.limits[i]) + } else { + None + } + } + + pub fn set_limits(&mut self, axis: JointAxis, limits: [Real; 2]) -> &mut Self { + let i = axis as usize; + self.limit_axes |= axis.into(); + self.limits[i].min = limits[0]; + self.limits[i].max = limits[1]; + self + } + + #[must_use] + pub fn motor_model(&self, axis: JointAxis) -> Option { + let i = axis as usize; + if self.motor_axes.contains(axis.into()) { + Some(self.motors[i].model) + } else { + None + } + } + + /// Set the spring-like model used by the motor to reach the desired target velocity and position. + pub fn set_motor_model(&mut self, axis: JointAxis, model: MotorModel) -> &mut Self { + self.motors[axis as usize].model = model; + self + } + + /// Sets the target velocity this motor needs to reach. + pub fn set_motor_velocity( + &mut self, + axis: JointAxis, + target_vel: Real, + factor: Real, + ) -> &mut Self { + self.set_motor( + axis, + self.motors[axis as usize].target_pos, + target_vel, + 0.0, + factor, + ) + } + + /// Sets the target angle this motor needs to reach. + pub fn set_motor_position( + &mut self, + axis: JointAxis, + target_pos: Real, + stiffness: Real, + damping: Real, + ) -> &mut Self { + self.set_motor(axis, target_pos, 0.0, stiffness, damping) + } + + pub fn set_motor_max_force(&mut self, axis: JointAxis, max_force: Real) -> &mut Self { + self.motors[axis as usize].max_force = max_force; + self + } + + #[must_use] + pub fn motor(&self, axis: JointAxis) -> Option<&JointMotor> { + let i = axis as usize; + if self.motor_axes.contains(axis.into()) { + Some(&self.motors[i]) + } else { + None + } + } + + /// Configure both the target angle and target velocity of the motor. + pub fn set_motor( + &mut self, + axis: JointAxis, + target_pos: Real, + target_vel: Real, + stiffness: Real, + damping: Real, + ) -> &mut Self { + self.motor_axes |= axis.into(); + let i = axis as usize; + self.motors[i].target_vel = target_vel; + self.motors[i].target_pos = target_pos; + self.motors[i].stiffness = stiffness; + self.motors[i].damping = damping; + self + } +} + +macro_rules! joint_conversion_methods( + ($as_joint: ident, $as_joint_mut: ident, $Joint: ty, $axes: expr) => { + #[must_use] + pub fn $as_joint(&self) -> Option<&$Joint> { + if self.locked_axes == $axes { + // SAFETY: this is OK because the target joint type is + // a `repr(transparent)` newtype of `Joint`. + Some(unsafe { std::mem::transmute(self) }) + } else { + None + } + } + + #[must_use] + pub fn $as_joint_mut(&mut self) -> Option<&mut $Joint> { + if self.locked_axes == $axes { + // SAFETY: this is OK because the target joint type is + // a `repr(transparent)` newtype of `Joint`. + Some(unsafe { std::mem::transmute(self) }) + } else { + None + } + } + } +); + +impl GenericJoint { + joint_conversion_methods!( + as_revolute, + as_revolute_mut, + RevoluteJoint, + JointAxesMask::LOCKED_REVOLUTE_AXES + ); + joint_conversion_methods!( + as_fixed, + as_fixed_mut, + FixedJoint, + JointAxesMask::LOCKED_FIXED_AXES + ); + joint_conversion_methods!( + as_prismatic, + as_prismatic_mut, + PrismaticJoint, + JointAxesMask::LOCKED_PRISMATIC_AXES + ); + + #[cfg(feature = "dim3")] + joint_conversion_methods!( + as_spherical, + as_spherical_mut, + SphericalJoint, + JointAxesMask::LOCKED_SPHERICAL_AXES + ); +} + +#[derive(Copy, Clone, Debug)] +pub struct GenericJointBuilder(GenericJoint); + +impl GenericJointBuilder { + #[must_use] + pub fn new(locked_axes: JointAxesMask) -> Self { + Self(GenericJoint::new(locked_axes)) + } + + #[must_use] + pub fn lock_axes(mut self, axes: JointAxesMask) -> Self { + self.0.lock_axes(axes); + self + } + + #[must_use] + pub fn local_frame1(mut self, local_frame: Isometry) -> Self { + self.0.set_local_frame1(local_frame); + self + } + + #[must_use] + pub fn local_frame2(mut self, local_frame: Isometry) -> Self { + self.0.set_local_frame2(local_frame); + self + } + + #[must_use] + pub fn local_axis1(mut self, local_axis: UnitVector) -> Self { + self.0.set_local_axis1(local_axis); + self + } + + #[must_use] + pub fn local_axis2(mut self, local_axis: UnitVector) -> Self { + self.0.set_local_axis2(local_axis); + self + } + + #[must_use] + pub fn local_anchor1(mut self, anchor1: Point) -> Self { + self.0.set_local_anchor1(anchor1); + self + } + + #[must_use] + pub fn local_anchor2(mut self, anchor2: Point) -> Self { + self.0.set_local_anchor2(anchor2); + self + } + + #[must_use] + pub fn limits(mut self, axis: JointAxis, limits: [Real; 2]) -> Self { + self.0.set_limits(axis, limits); + self + } + + /// Set the spring-like model used by the motor to reach the desired target velocity and position. + #[must_use] + pub fn motor_model(mut self, axis: JointAxis, model: MotorModel) -> Self { + self.0.set_motor_model(axis, model); + self + } + + /// Sets the target velocity this motor needs to reach. + #[must_use] + pub fn motor_velocity(mut self, axis: JointAxis, target_vel: Real, factor: Real) -> Self { + self.0.set_motor_velocity(axis, target_vel, factor); + self + } + + /// Sets the target angle this motor needs to reach. + #[must_use] + pub fn motor_position( + mut self, + axis: JointAxis, + target_pos: Real, + stiffness: Real, + damping: Real, + ) -> Self { + self.0 + .set_motor_position(axis, target_pos, stiffness, damping); + self + } + + /// Configure both the target angle and target velocity of the motor. + #[must_use] + pub fn set_motor( + mut self, + axis: JointAxis, + target_pos: Real, + target_vel: Real, + stiffness: Real, + damping: Real, + ) -> Self { + self.0 + .set_motor(axis, target_pos, target_vel, stiffness, damping); + self + } + + #[must_use] + pub fn motor_max_force(mut self, axis: JointAxis, max_force: Real) -> Self { + self.0.set_motor_max_force(axis, max_force); + self + } + + #[must_use] + pub fn build(self) -> GenericJoint { + self.0 + } +} diff --git a/src/dynamics/joint/impulse_joint/impulse_joint.rs b/src/dynamics/joint/impulse_joint/impulse_joint.rs index f3f4f7c..993542a 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint.rs @@ -1,4 +1,4 @@ -use crate::dynamics::{ImpulseJointHandle, JointData, RigidBodyHandle}; +use crate::dynamics::{GenericJoint, ImpulseJointHandle, RigidBodyHandle}; use crate::math::{Real, SpacialVector}; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] @@ -10,7 +10,7 @@ pub struct ImpulseJoint { /// Handle to the second body attached to this joint. pub body2: RigidBodyHandle, - pub data: JointData, + pub data: GenericJoint, pub impulses: SpacialVector, // A joint needs to know its handle to simplify its removal. diff --git a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs index 51d5989..8677772 100644 --- a/src/dynamics/joint/impulse_joint/impulse_joint_set.rs +++ b/src/dynamics/joint/impulse_joint/impulse_joint_set.rs @@ -3,8 +3,8 @@ use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractio use crate::data::arena::Arena; use crate::data::{BundleSet, Coarena, ComponentSet, ComponentSetMut}; +use crate::dynamics::{GenericJoint, RigidBodyHandle}; use crate::dynamics::{IslandManager, RigidBodyActivation, RigidBodyIds, RigidBodyType}; -use crate::dynamics::{JointData, RigidBodyHandle}; /// The unique identifier of a joint added to the joint set. /// The unique identifier of a collider added to a collider set. @@ -177,7 +177,7 @@ impl ImpulseJointSet { &mut self, body1: RigidBodyHandle, body2: RigidBodyHandle, - data: impl Into, + data: impl Into, ) -> ImpulseJointHandle { let data = data.into(); let handle = self.joint_ids.insert(0.into()); diff --git a/src/dynamics/joint/joint_data.rs b/src/dynamics/joint/joint_data.rs deleted file mode 100644 index b1ad6c6..0000000 --- a/src/dynamics/joint/joint_data.rs +++ /dev/null @@ -1,275 +0,0 @@ -use crate::dynamics::solver::MotorParameters; -use crate::dynamics::MotorModel; -use crate::math::{Isometry, Point, Real, Rotation, UnitVector, SPATIAL_DIM}; -use crate::utils::WBasis; - -#[cfg(feature = "dim3")] -bitflags::bitflags! { - #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] - pub struct JointAxesMask: u8 { - const FREE = 0; - const X = 1 << 0; - const Y = 1 << 1; - const Z = 1 << 2; - const ANG_X = 1 << 3; - const ANG_Y = 1 << 4; - const ANG_Z = 1 << 5; - } -} - -#[cfg(feature = "dim2")] -bitflags::bitflags! { - #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] - pub struct JointAxesMask: u8 { - const FREE = 0; - const X = 1 << 0; - const Y = 1 << 1; - const ANG_X = 1 << 2; - } -} - -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Copy, Clone, Debug, PartialEq)] -pub enum JointAxis { - X = 0, - Y, - #[cfg(feature = "dim3")] - Z, - AngX, - #[cfg(feature = "dim3")] - AngY, - #[cfg(feature = "dim3")] - AngZ, -} - -impl From for JointAxesMask { - fn from(axis: JointAxis) -> Self { - JointAxesMask::from_bits(1 << axis as usize).unwrap() - } -} - -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Copy, Clone, Debug, PartialEq)] -pub struct JointLimits { - pub min: Real, - pub max: Real, - pub impulse: Real, -} - -impl Default for JointLimits { - fn default() -> Self { - Self { - min: -Real::MAX, - max: Real::MAX, - impulse: 0.0, - } - } -} - -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Copy, Clone, Debug, PartialEq)] -pub struct JointMotor { - pub target_vel: Real, - pub target_pos: Real, - pub stiffness: Real, - pub damping: Real, - pub max_impulse: Real, - pub impulse: Real, - pub model: MotorModel, -} - -impl Default for JointMotor { - fn default() -> Self { - Self { - target_pos: 0.0, - target_vel: 0.0, - stiffness: 0.0, - damping: 0.0, - max_impulse: Real::MAX, - impulse: 0.0, - model: MotorModel::VelocityBased, - } - } -} - -impl JointMotor { - pub(crate) fn motor_params(&self, dt: Real) -> MotorParameters { - let (stiffness, damping, gamma, _keep_lhs) = - self.model - .combine_coefficients(dt, self.stiffness, self.damping); - MotorParameters { - stiffness, - damping, - gamma, - // keep_lhs, - target_pos: self.target_pos, - target_vel: self.target_vel, - max_impulse: self.max_impulse, - } - } -} - -#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] -#[derive(Copy, Clone, Debug, PartialEq)] -pub struct JointData { - pub local_frame1: Isometry, - pub local_frame2: Isometry, - pub locked_axes: JointAxesMask, - pub limit_axes: JointAxesMask, - pub motor_axes: JointAxesMask, - pub limits: [JointLimits; SPATIAL_DIM], - pub motors: [JointMotor; SPATIAL_DIM], -} - -impl Default for JointData { - fn default() -> Self { - Self { - local_frame1: Isometry::identity(), - local_frame2: Isometry::identity(), - locked_axes: JointAxesMask::FREE, - limit_axes: JointAxesMask::FREE, - motor_axes: JointAxesMask::FREE, - limits: [JointLimits::default(); SPATIAL_DIM], - motors: [JointMotor::default(); SPATIAL_DIM], - } - } -} - -impl JointData { - #[must_use] - pub fn new(locked_axes: JointAxesMask) -> Self { - Self::default().lock_axes(locked_axes) - } - - /// Can this joint use SIMD-accelerated constraint formulations? - pub fn supports_simd_constraints(&self) -> bool { - self.limit_axes.is_empty() && self.motor_axes.is_empty() - } - - #[must_use] - pub fn lock_axes(mut self, axes: JointAxesMask) -> Self { - self.locked_axes |= axes; - self - } - - fn complete_ang_frame(axis: UnitVector) -> Rotation { - let basis = axis.orthonormal_basis(); - - #[cfg(feature = "dim2")] - { - use na::{Matrix2, Rotation2, UnitComplex}; - let mat = Matrix2::from_columns(&[axis.into_inner(), basis[0]]); - let rotmat = Rotation2::from_matrix_unchecked(mat); - UnitComplex::from_rotation_matrix(&rotmat) - } - - #[cfg(feature = "dim3")] - { - use na::{Matrix3, Rotation3, UnitQuaternion}; - let mat = Matrix3::from_columns(&[axis.into_inner(), basis[0], basis[1]]); - let rotmat = Rotation3::from_matrix_unchecked(mat); - UnitQuaternion::from_rotation_matrix(&rotmat) - } - } - - #[must_use] - pub fn local_frame1(mut self, local_frame: Isometry) -> Self { - self.local_frame1 = local_frame; - self - } - - #[must_use] - pub fn local_frame2(mut self, local_frame: Isometry) -> Self { - self.local_frame2 = local_frame; - self - } - - #[must_use] - pub fn local_axis1(mut self, local_axis: UnitVector) -> Self { - self.local_frame1.rotation = Self::complete_ang_frame(local_axis); - self - } - - #[must_use] - pub fn local_axis2(mut self, local_axis: UnitVector) -> Self { - self.local_frame2.rotation = Self::complete_ang_frame(local_axis); - self - } - - #[must_use] - pub fn local_anchor1(mut self, anchor1: Point) -> Self { - self.local_frame1.translation.vector = anchor1.coords; - self - } - - #[must_use] - pub fn local_anchor2(mut self, anchor2: Point) -> Self { - self.local_frame2.translation.vector = anchor2.coords; - self - } - - #[must_use] - pub fn limit_axis(mut self, axis: JointAxis, limits: [Real; 2]) -> Self { - let i = axis as usize; - self.limit_axes |= axis.into(); - self.limits[i].min = limits[0]; - self.limits[i].max = limits[1]; - self - } - - /// Set the spring-like model used by the motor to reach the desired target velocity and position. - #[must_use] - pub fn motor_model(mut self, axis: JointAxis, model: MotorModel) -> Self { - self.motors[axis as usize].model = model; - self - } - - /// Sets the target velocity this motor needs to reach. - #[must_use] - pub fn motor_velocity(self, axis: JointAxis, target_vel: Real, factor: Real) -> Self { - self.motor_axis( - axis, - self.motors[axis as usize].target_pos, - target_vel, - 0.0, - factor, - ) - } - - /// Sets the target angle this motor needs to reach. - #[must_use] - pub fn motor_position( - self, - axis: JointAxis, - target_pos: Real, - stiffness: Real, - damping: Real, - ) -> Self { - self.motor_axis(axis, target_pos, 0.0, stiffness, damping) - } - - /// Configure both the target angle and target velocity of the motor. - #[must_use] - pub fn motor_axis( - mut self, - axis: JointAxis, - target_pos: Real, - target_vel: Real, - stiffness: Real, - damping: Real, - ) -> Self { - self.motor_axes |= axis.into(); - let i = axis as usize; - self.motors[i].target_vel = target_vel; - self.motors[i].target_pos = target_pos; - self.motors[i].stiffness = stiffness; - self.motors[i].damping = damping; - self - } - - #[must_use] - pub fn motor_max_impulse(mut self, axis: JointAxis, max_impulse: Real) -> Self { - self.motors[axis as usize].max_impulse = max_impulse; - self - } -} diff --git a/src/dynamics/joint/mod.rs b/src/dynamics/joint/mod.rs index 6594b83..11b5c0f 100644 --- a/src/dynamics/joint/mod.rs +++ b/src/dynamics/joint/mod.rs @@ -1,17 +1,17 @@ -pub use self::fixed_joint::FixedJoint; +pub use self::fixed_joint::*; pub use self::impulse_joint::*; -pub use self::joint_data::*; +pub use self::generic_joint::*; pub use self::motor_model::MotorModel; pub use self::multibody_joint::*; -pub use self::prismatic_joint::PrismaticJoint; -pub use self::revolute_joint::RevoluteJoint; +pub use self::prismatic_joint::*; +pub use self::revolute_joint::*; #[cfg(feature = "dim3")] -pub use self::spherical_joint::SphericalJoint; +pub use self::spherical_joint::*; mod fixed_joint; mod impulse_joint; -mod joint_data; +mod generic_joint; mod motor_model; mod multibody_joint; mod prismatic_joint; diff --git a/src/dynamics/joint/motor_model.rs b/src/dynamics/joint/motor_model.rs index e5a6549..e8ffffa 100644 --- a/src/dynamics/joint/motor_model.rs +++ b/src/dynamics/joint/motor_model.rs @@ -5,57 +5,40 @@ use crate::math::Real; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub enum MotorModel { /// The solved spring-like equation is: - /// `delta_velocity(t + dt) = stiffness / dt * (target_pos - pos(t)) + damping * (target_vel - vel(t))` - /// - /// Here the `stiffness` is the ratio of position error to be solved at each timestep (like - /// a velocity-based ERP), and the `damping` is the ratio of velocity error to be solved at - /// each timestep. - VelocityBased, - /// The solved spring-like equation is: - /// `acceleration(t + dt) = stiffness * (target_pos - pos(t)) + damping * (target_vel - vel(t))` + /// `acceleration = stiffness * (pos - target_pos) + damping * (vel - target_vel)` AccelerationBased, - // /// The solved spring-like equation is: - // /// `force(t + dt) = stiffness * (target_pos - pos(t + dt)) + damping * (target_vel - vel(t + dt))` - // ForceBased, + /// The solved spring-like equation is: + /// `force = stiffness * (pos - target_pos) + damping * (vel - target_vel)` + ForceBased, } impl Default for MotorModel { fn default() -> Self { - MotorModel::VelocityBased + MotorModel::AccelerationBased } } impl MotorModel { /// Combines the coefficients used for solving the spring equation. /// - /// Returns the new coefficients (stiffness, damping, gamma, keep_inv_lhs) - /// coefficients for the equivalent impulse-based equation. These new - /// coefficients must be used in the following way: - /// - `rhs = (stiffness * pos_err + damping * vel_err) / gamma`. - /// - `new_inv_lhs = gamma * if keep_inv_lhs { inv_lhs } else { 1.0 }`. - /// Note that the returned `gamma` will be zero if both `stiffness` and `damping` are zero. + /// Returns the coefficients (erp_inv_dt, cfm_coeff, cfm_gain). pub fn combine_coefficients( self, dt: Real, stiffness: Real, damping: Real, - ) -> (Real, Real, Real, bool) { + ) -> (Real, Real, Real) { match self { - MotorModel::VelocityBased => (stiffness * crate::utils::inv(dt), damping, 1.0, true), MotorModel::AccelerationBased => { - let effective_stiffness = stiffness * dt; - let effective_damping = damping * dt; - // TODO: Using gamma behaves very badly for some reasons. - // Maybe I got the formulation wrong, so let's keep it to 1.0 for now, - // and get back to this later. - // let gamma = effective_stiffness * dt + effective_damping; - (effective_stiffness, effective_damping, 1.0, true) - } // MotorModel::ForceBased => { - // let effective_stiffness = stiffness * dt; - // let effective_damping = damping * dt; - // let gamma = effective_stiffness * dt + effective_damping; - // (effective_stiffness, effective_damping, gamma, false) - // } + let erp_inv_dt = stiffness * crate::utils::inv(dt * stiffness + damping); + let cfm_coeff = crate::utils::inv(dt * dt * stiffness + dt * damping); + (erp_inv_dt, cfm_coeff, 0.0) + } + MotorModel::ForceBased => { + let erp_inv_dt = stiffness * crate::utils::inv(dt * stiffness + damping); + let cfm_gain = crate::utils::inv(dt * dt * stiffness + dt * damping); + (erp_inv_dt, 0.0, cfm_gain) + } } } } diff --git a/src/dynamics/joint/multibody_joint/multibody_joint.rs b/src/dynamics/joint/multibody_joint/multibody_joint.rs index 1d14680..2f7a71e 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint.rs @@ -1,6 +1,6 @@ use crate::dynamics::solver::AnyJointVelocityConstraint; use crate::dynamics::{ - joint, FixedJoint, IntegrationParameters, JointData, Multibody, MultibodyLink, + joint, FixedJointBuilder, GenericJoint, IntegrationParameters, Multibody, MultibodyLink, RigidBodyVelocity, }; use crate::math::{ @@ -14,13 +14,13 @@ use na::{UnitQuaternion, Vector3}; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug)] pub struct MultibodyJoint { - pub data: JointData, + pub data: GenericJoint, pub(crate) coords: SpacialVector, pub(crate) joint_rot: Rotation, } impl MultibodyJoint { - pub fn new(data: JointData) -> Self { + pub fn new(data: GenericJoint) -> Self { Self { data, coords: na::zero(), @@ -29,13 +29,13 @@ impl MultibodyJoint { } pub(crate) fn free(pos: Isometry) -> Self { - let mut result = Self::new(JointData::default()); + let mut result = Self::new(GenericJoint::default()); result.set_free_pos(pos); result } pub(crate) fn fixed(pos: Isometry) -> Self { - Self::new(FixedJoint::new().local_frame1(pos).into()) + Self::new(FixedJointBuilder::new().local_frame1(pos).build().into()) } pub(crate) fn set_free_pos(&mut self, pos: Isometry) { @@ -263,19 +263,11 @@ impl MultibodyJoint { for i in 0..DIM { if (locked_bits & (1 << i)) == 0 { - if (limit_bits & (1 << i)) != 0 { - joint::unit_joint_limit_constraint( - params, - multibody, - link, - [self.data.limits[i].min, self.data.limits[i].max], - self.coords[i], - dof_id + curr_free_dof, - j_id, - jacobians, - constraints, - ); - } + let limits = if (limit_bits & (1 << i)) != 0 { + Some([self.data.limits[i].min, self.data.limits[i].max]) + } else { + None + }; if (motor_bits & (1 << i)) != 0 { joint::unit_joint_motor_constraint( @@ -284,6 +276,21 @@ impl MultibodyJoint { link, &self.data.motors[i], self.coords[i], + limits, + dof_id + curr_free_dof, + j_id, + jacobians, + constraints, + ); + } + + if (limit_bits & (1 << i)) != 0 { + joint::unit_joint_limit_constraint( + params, + multibody, + link, + [self.data.limits[i].min, self.data.limits[i].max], + self.coords[i], dof_id + curr_free_dof, j_id, jacobians, @@ -310,19 +317,23 @@ impl MultibodyJoint { // TODO: we should make special cases for multi-angular-dofs limits/motors for i in DIM..SPATIAL_DIM { if (locked_bits & (1 << i)) == 0 { - if (limit_bits & (1 << i)) != 0 { + let limits = if (limit_bits & (1 << i)) != 0 { + let limits = [self.data.limits[i].min, self.data.limits[i].max]; joint::unit_joint_limit_constraint( params, multibody, link, - [self.data.limits[i].min, self.data.limits[i].max], + limits, self.coords[i], dof_id + curr_free_dof, j_id, jacobians, constraints, ); - } + Some(limits) + } else { + None + }; if (motor_bits & (1 << i)) != 0 { joint::unit_joint_motor_constraint( @@ -331,6 +342,7 @@ impl MultibodyJoint { link, &self.data.motors[i], self.coords[i], + limits, dof_id + curr_free_dof, j_id, jacobians, diff --git a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs index 3e786e2..7e512a8 100644 --- a/src/dynamics/joint/multibody_joint/multibody_joint_set.rs +++ b/src/dynamics/joint/multibody_joint/multibody_joint_set.rs @@ -1,7 +1,7 @@ use crate::data::{Arena, Coarena, ComponentSet, ComponentSetMut, Index}; use crate::dynamics::joint::MultibodyLink; use crate::dynamics::{ - IslandManager, JointData, Multibody, MultibodyJoint, RigidBodyActivation, RigidBodyHandle, + GenericJoint, IslandManager, Multibody, MultibodyJoint, RigidBodyActivation, RigidBodyHandle, RigidBodyIds, RigidBodyType, }; use crate::geometry::{InteractionGraph, RigidBodyGraphIndex}; @@ -112,7 +112,7 @@ impl MultibodyJointSet { &mut self, body1: RigidBodyHandle, body2: RigidBodyHandle, - data: impl Into, + data: impl Into, ) -> Option { let data = data.into(); let link1 = self.rb2mb.get(body1.0).copied().unwrap_or_else(|| { diff --git a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs index 3367108..42212be 100644 --- a/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs +++ b/src/dynamics/joint/multibody_joint/unit_multibody_joint.rs @@ -26,7 +26,8 @@ pub fn unit_joint_limit_constraint( let min_enabled = curr_pos < limits[0]; let max_enabled = limits[1] < curr_pos; - let erp_inv_dt = params.erp_inv_dt(); + let erp_inv_dt = params.joint_erp_inv_dt(); + let cfm_coeff = params.joint_cfm_coeff(); let rhs_bias = ((curr_pos - limits[1]).max(0.0) - (limits[0] - curr_pos).max(0.0)) * erp_inv_dt; let rhs_wo_bias = joint_velocity[dof_id]; @@ -54,6 +55,8 @@ pub fn unit_joint_limit_constraint( inv_lhs: crate::utils::inv(lhs), rhs: rhs_wo_bias + rhs_bias, rhs_wo_bias, + cfm_coeff, + cfm_gain: 0.0, writeback_id: WritebackId::Limit(dof_id), }; @@ -71,11 +74,13 @@ pub fn unit_joint_motor_constraint( link: &MultibodyLink, motor: &JointMotor, curr_pos: Real, + limits: Option<[Real; 2]>, dof_id: usize, j_id: &mut usize, jacobians: &mut DVector, constraints: &mut Vec, ) { + let inv_dt = params.inv_dt(); let ndofs = multibody.ndofs(); let joint_velocity = multibody.joint_velocity(link); @@ -93,14 +98,20 @@ pub fn unit_joint_motor_constraint( let impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse]; let mut rhs_wo_bias = 0.0; - if motor_params.stiffness != 0.0 { - rhs_wo_bias += (curr_pos - motor_params.target_pos) * motor_params.stiffness; + if motor_params.erp_inv_dt != 0.0 { + rhs_wo_bias += (curr_pos - motor_params.target_pos) * motor_params.erp_inv_dt; } - if motor_params.damping != 0.0 { - let dvel = joint_velocity[dof_id]; - rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping; - } + let mut target_vel = motor_params.target_vel; + if let Some(limits) = limits { + target_vel = target_vel.clamp( + (limits[0] - curr_pos) * inv_dt, + (limits[1] - curr_pos) * inv_dt, + ); + }; + + let dvel = joint_velocity[dof_id]; + rhs_wo_bias += dvel - target_vel; let constraint = JointGenericVelocityGroundConstraint { mj_lambda2: multibody.solver_id, @@ -109,6 +120,8 @@ pub fn unit_joint_motor_constraint( joint_id: usize::MAX, impulse: 0.0, impulse_bounds, + cfm_coeff: motor_params.cfm_coeff, + cfm_gain: motor_params.cfm_gain, inv_lhs: crate::utils::inv(lhs), rhs: rhs_wo_bias, rhs_wo_bias, diff --git a/src/dynamics/joint/prismatic_joint.rs b/src/dynamics/joint/prismatic_joint.rs index 92fabde..e3f7527 100644 --- a/src/dynamics/joint/prismatic_joint.rs +++ b/src/dynamics/joint/prismatic_joint.rs @@ -1,91 +1,215 @@ -use crate::dynamics::joint::{JointAxesMask, JointData}; +use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask}; use crate::dynamics::{JointAxis, MotorModel}; use crate::math::{Point, Real, UnitVector}; +use super::{JointLimits, JointMotor}; + #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] +#[repr(transparent)] pub struct PrismaticJoint { - data: JointData, + data: GenericJoint, } impl PrismaticJoint { pub fn new(axis: UnitVector) -> Self { - #[cfg(feature = "dim2")] - let mask = JointAxesMask::Y | JointAxesMask::ANG_X; - #[cfg(feature = "dim3")] - let mask = JointAxesMask::Y - | JointAxesMask::Z - | JointAxesMask::ANG_X - | JointAxesMask::ANG_Y - | JointAxesMask::ANG_Z; - - let data = JointData::default() - .lock_axes(mask) + let data = GenericJointBuilder::new(JointAxesMask::LOCKED_PRISMATIC_AXES) .local_axis1(axis) - .local_axis2(axis); + .local_axis2(axis) + .build(); Self { data } } + #[must_use] + pub fn local_anchor1(&self) -> Point { + self.data.local_anchor1() + } + + pub fn set_local_anchor1(&mut self, anchor1: Point) -> &mut Self { + self.data.set_local_anchor1(anchor1); + self + } + + #[must_use] + pub fn local_anchor2(&self) -> Point { + self.data.local_anchor2() + } + + pub fn set_local_anchor2(&mut self, anchor2: Point) -> &mut Self { + self.data.set_local_anchor2(anchor2); + self + } + + #[must_use] + pub fn local_axis1(&self) -> UnitVector { + self.data.local_axis1() + } + + pub fn set_local_axis1(&mut self, axis1: UnitVector) -> &mut Self { + self.data.set_local_axis1(axis1); + self + } + + #[must_use] + pub fn local_axis2(&self) -> UnitVector { + self.data.local_axis2() + } + + pub fn set_local_axis2(&mut self, axis2: UnitVector) -> &mut Self { + self.data.set_local_axis2(axis2); + self + } + + #[must_use] + pub fn motor(&self) -> Option<&JointMotor> { + self.data.motor(JointAxis::X) + } + + /// Set the spring-like model used by the motor to reach the desired target velocity and position. + pub fn set_motor_model(&mut self, model: MotorModel) -> &mut Self { + self.data.set_motor_model(JointAxis::X, model); + self + } + + /// Sets the target velocity this motor needs to reach. + pub fn set_motor_velocity(&mut self, target_vel: Real, factor: Real) -> &mut Self { + self.data + .set_motor_velocity(JointAxis::X, target_vel, factor); + self + } + + /// Sets the target angle this motor needs to reach. + pub fn set_motor_position( + &mut self, + target_pos: Real, + stiffness: Real, + damping: Real, + ) -> &mut Self { + self.data + .set_motor_position(JointAxis::X, target_pos, stiffness, damping); + self + } + + /// Configure both the target angle and target velocity of the motor. + pub fn set_motor( + &mut self, + target_pos: Real, + target_vel: Real, + stiffness: Real, + damping: Real, + ) -> &mut Self { + self.data + .set_motor(JointAxis::X, target_pos, target_vel, stiffness, damping); + self + } + + pub fn set_motor_max_force(&mut self, max_force: Real) -> &mut Self { + self.data.set_motor_max_force(JointAxis::X, max_force); + self + } + + #[must_use] + pub fn limits(&self) -> Option<&JointLimits> { + self.data.limits(JointAxis::X) + } + + pub fn set_limits(&mut self, limits: [Real; 2]) -> &mut Self { + self.data.set_limits(JointAxis::X, limits); + self + } +} + +impl Into for PrismaticJoint { + fn into(self) -> GenericJoint { + self.data + } +} + +pub struct PrismaticJointBuilder(PrismaticJoint); + +impl PrismaticJointBuilder { + pub fn new(axis: UnitVector) -> Self { + Self(PrismaticJoint::new(axis)) + } + #[must_use] pub fn local_anchor1(mut self, anchor1: Point) -> Self { - self.data = self.data.local_anchor1(anchor1); + self.0.set_local_anchor1(anchor1); self } #[must_use] pub fn local_anchor2(mut self, anchor2: Point) -> Self { - self.data = self.data.local_anchor2(anchor2); + self.0.set_local_anchor2(anchor2); + self + } + + #[must_use] + pub fn local_axis1(mut self, axis1: UnitVector) -> Self { + self.0.set_local_axis1(axis1); + self + } + + #[must_use] + pub fn local_axis2(mut self, axis2: UnitVector) -> Self { + self.0.set_local_axis2(axis2); self } /// Set the spring-like model used by the motor to reach the desired target velocity and position. + #[must_use] pub fn motor_model(mut self, model: MotorModel) -> Self { - self.data = self.data.motor_model(JointAxis::X, model); + self.0.set_motor_model(model); self } /// Sets the target velocity this motor needs to reach. + #[must_use] pub fn motor_velocity(mut self, target_vel: Real, factor: Real) -> Self { - self.data = self.data.motor_velocity(JointAxis::X, target_vel, factor); + self.0.set_motor_velocity(target_vel, factor); self } /// Sets the target angle this motor needs to reach. + #[must_use] pub fn motor_position(mut self, target_pos: Real, stiffness: Real, damping: Real) -> Self { - self.data = self - .data - .motor_position(JointAxis::X, target_pos, stiffness, damping); + self.0.set_motor_position(target_pos, stiffness, damping); self } /// Configure both the target angle and target velocity of the motor. - pub fn motor_axis( + #[must_use] + pub fn set_motor( mut self, target_pos: Real, target_vel: Real, stiffness: Real, damping: Real, ) -> Self { - self.data = self - .data - .motor_axis(JointAxis::X, target_pos, target_vel, stiffness, damping); - self - } - - pub fn motor_max_impulse(mut self, max_impulse: Real) -> Self { - self.data = self.data.motor_max_impulse(JointAxis::X, max_impulse); + self.0.set_motor(target_pos, target_vel, stiffness, damping); self } #[must_use] - pub fn limit_axis(mut self, limits: [Real; 2]) -> Self { - self.data = self.data.limit_axis(JointAxis::X, limits); + pub fn motor_max_force(mut self, max_force: Real) -> Self { + self.0.set_motor_max_force(max_force); self } + + #[must_use] + pub fn limits(mut self, limits: [Real; 2]) -> Self { + self.0.set_limits(limits); + self + } + + #[must_use] + pub fn build(self) -> PrismaticJoint { + self.0 + } } -impl Into for PrismaticJoint { - fn into(self) -> JointData { - self.data +impl Into for PrismaticJointBuilder { + fn into(self) -> GenericJoint { + self.0.into() } } diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs index 9084c4d..a9d74f5 100644 --- a/src/dynamics/joint/revolute_joint.rs +++ b/src/dynamics/joint/revolute_joint.rs @@ -1,5 +1,5 @@ -use crate::dynamics::joint::{JointAxesMask, JointData}; -use crate::dynamics::{JointAxis, MotorModel}; +use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask}; +use crate::dynamics::{JointAxis, JointLimits, JointMotor, MotorModel}; use crate::math::{Point, Real}; #[cfg(feature = "dim3")] @@ -7,100 +7,197 @@ use crate::math::UnitVector; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] +#[repr(transparent)] pub struct RevoluteJoint { - data: JointData, + data: GenericJoint, } impl RevoluteJoint { #[cfg(feature = "dim2")] pub fn new() -> Self { - let mask = JointAxesMask::X | JointAxesMask::Y; - - let data = JointData::default().lock_axes(mask); - Self { data } + let data = GenericJointBuilder::new(JointAxesMask::LOCKED_REVOLUTE_AXES); + Self { data: data.build() } } #[cfg(feature = "dim3")] pub fn new(axis: UnitVector) -> Self { - let mask = JointAxesMask::X - | JointAxesMask::Y - | JointAxesMask::Z - | JointAxesMask::ANG_Y - | JointAxesMask::ANG_Z; - - let data = JointData::default() - .lock_axes(mask) + let data = GenericJointBuilder::new(JointAxesMask::LOCKED_REVOLUTE_AXES) .local_axis1(axis) - .local_axis2(axis); + .local_axis2(axis) + .build(); Self { data } } - pub fn data(&self) -> &JointData { + pub fn data(&self) -> &GenericJoint { &self.data } + #[must_use] + pub fn local_anchor1(&self) -> Point { + self.data.local_anchor1() + } + + pub fn set_local_anchor1(&mut self, anchor1: Point) -> &mut Self { + self.data.set_local_anchor1(anchor1); + self + } + + #[must_use] + pub fn local_anchor2(&self) -> Point { + self.data.local_anchor2() + } + + pub fn set_local_anchor2(&mut self, anchor2: Point) -> &mut Self { + self.data.set_local_anchor2(anchor2); + self + } + + #[must_use] + pub fn motor(&self) -> Option<&JointMotor> { + self.data.motor(JointAxis::AngX) + } + + /// Set the spring-like model used by the motor to reach the desired target velocity and position. + pub fn set_motor_model(&mut self, model: MotorModel) -> &mut Self { + self.data.set_motor_model(JointAxis::AngX, model); + self + } + + /// Sets the target velocity this motor needs to reach. + pub fn set_motor_velocity(&mut self, target_vel: Real, factor: Real) -> &mut Self { + self.data + .set_motor_velocity(JointAxis::AngX, target_vel, factor); + self + } + + /// Sets the target angle this motor needs to reach. + pub fn set_motor_position( + &mut self, + target_pos: Real, + stiffness: Real, + damping: Real, + ) -> &mut Self { + self.data + .set_motor_position(JointAxis::AngX, target_pos, stiffness, damping); + self + } + + /// Configure both the target angle and target velocity of the motor. + pub fn set_motor( + &mut self, + target_pos: Real, + target_vel: Real, + stiffness: Real, + damping: Real, + ) -> &mut Self { + self.data + .set_motor(JointAxis::AngX, target_pos, target_vel, stiffness, damping); + self + } + + pub fn set_motor_max_force(&mut self, max_force: Real) -> &mut Self { + self.data.set_motor_max_force(JointAxis::AngX, max_force); + self + } + + #[must_use] + pub fn limits(&self) -> Option<&JointLimits> { + self.data.limits(JointAxis::AngX) + } + + pub fn set_limits(&mut self, limits: [Real; 2]) -> &mut Self { + self.data.set_limits(JointAxis::AngX, limits); + self + } +} + +impl Into for RevoluteJoint { + fn into(self) -> GenericJoint { + self.data + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct RevoluteJointBuilder(RevoluteJoint); + +impl RevoluteJointBuilder { + #[cfg(feature = "dim2")] + pub fn new() -> Self { + Self(RevoluteJoint::new()) + } + + #[cfg(feature = "dim3")] + pub fn new(axis: UnitVector) -> Self { + Self(RevoluteJoint::new(axis)) + } + #[must_use] pub fn local_anchor1(mut self, anchor1: Point) -> Self { - self.data = self.data.local_anchor1(anchor1); + self.0.set_local_anchor1(anchor1); self } #[must_use] pub fn local_anchor2(mut self, anchor2: Point) -> Self { - self.data = self.data.local_anchor2(anchor2); + self.0.set_local_anchor2(anchor2); self } /// Set the spring-like model used by the motor to reach the desired target velocity and position. + #[must_use] pub fn motor_model(mut self, model: MotorModel) -> Self { - self.data = self.data.motor_model(JointAxis::AngX, model); + self.0.set_motor_model(model); self } /// Sets the target velocity this motor needs to reach. + #[must_use] pub fn motor_velocity(mut self, target_vel: Real, factor: Real) -> Self { - self.data = self - .data - .motor_velocity(JointAxis::AngX, target_vel, factor); + self.0.set_motor_velocity(target_vel, factor); self } /// Sets the target angle this motor needs to reach. + #[must_use] pub fn motor_position(mut self, target_pos: Real, stiffness: Real, damping: Real) -> Self { - self.data = self - .data - .motor_position(JointAxis::AngX, target_pos, stiffness, damping); + self.0.set_motor_position(target_pos, stiffness, damping); self } /// Configure both the target angle and target velocity of the motor. - pub fn motor_axis( + #[must_use] + pub fn motor( mut self, target_pos: Real, target_vel: Real, stiffness: Real, damping: Real, ) -> Self { - self.data = - self.data - .motor_axis(JointAxis::AngX, target_pos, target_vel, stiffness, damping); - self - } - - pub fn motor_max_impulse(mut self, max_impulse: Real) -> Self { - self.data = self.data.motor_max_impulse(JointAxis::AngX, max_impulse); + self.0.set_motor(target_pos, target_vel, stiffness, damping); self } #[must_use] - pub fn limit_axis(mut self, limits: [Real; 2]) -> Self { - self.data = self.data.limit_axis(JointAxis::AngX, limits); + pub fn motor_max_force(mut self, max_force: Real) -> Self { + self.0.set_motor_max_force(max_force); self } + + #[must_use] + pub fn limits(mut self, limits: [Real; 2]) -> Self { + self.0.set_limits(limits); + self + } + + #[must_use] + pub fn build(self) -> RevoluteJoint { + self.0 + } } -impl Into for RevoluteJoint { - fn into(self) -> JointData { - self.data +impl Into for RevoluteJointBuilder { + fn into(self) -> GenericJoint { + self.0.into() } } diff --git a/src/dynamics/joint/spherical_joint.rs b/src/dynamics/joint/spherical_joint.rs index 194682d..3ff029e 100644 --- a/src/dynamics/joint/spherical_joint.rs +++ b/src/dynamics/joint/spherical_joint.rs @@ -1,11 +1,12 @@ -use crate::dynamics::joint::{JointAxesMask, JointData}; +use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask}; use crate::dynamics::{JointAxis, MotorModel}; use crate::math::{Point, Real}; #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Copy, Clone, Debug, PartialEq)] +#[repr(transparent)] pub struct SphericalJoint { - data: JointData, + data: GenericJoint, } impl Default for SphericalJoint { @@ -16,40 +17,128 @@ impl Default for SphericalJoint { impl SphericalJoint { pub fn new() -> Self { - let data = - JointData::default().lock_axes(JointAxesMask::X | JointAxesMask::Y | JointAxesMask::Z); + let data = GenericJointBuilder::new(JointAxesMask::LOCKED_SPHERICAL_AXES).build(); Self { data } } - pub fn data(&self) -> &JointData { + pub fn data(&self) -> &GenericJoint { &self.data } + pub fn set_local_anchor1(&mut self, anchor1: Point) -> &mut Self { + self.data.set_local_anchor1(anchor1); + self + } + + pub fn set_local_anchor2(&mut self, anchor2: Point) -> &mut Self { + self.data.set_local_anchor2(anchor2); + self + } + + /// Set the spring-like model used by the motor to reach the desired target velocity and position. + pub fn set_motor_model(&mut self, axis: JointAxis, model: MotorModel) -> &mut Self { + self.data.set_motor_model(axis, model); + self + } + + /// Sets the target velocity this motor needs to reach. + pub fn set_motor_velocity( + &mut self, + axis: JointAxis, + target_vel: Real, + factor: Real, + ) -> &mut Self { + self.data.set_motor_velocity(axis, target_vel, factor); + self + } + + /// Sets the target angle this motor needs to reach. + pub fn set_motor_position( + &mut self, + axis: JointAxis, + target_pos: Real, + stiffness: Real, + damping: Real, + ) -> &mut Self { + self.data + .set_motor_position(axis, target_pos, stiffness, damping); + self + } + + /// Configure both the target angle and target velocity of the motor. + pub fn set_motor( + &mut self, + axis: JointAxis, + target_pos: Real, + target_vel: Real, + stiffness: Real, + damping: Real, + ) -> &mut Self { + self.data + .set_motor(axis, target_pos, target_vel, stiffness, damping); + self + } + + pub fn set_motor_max_force(&mut self, axis: JointAxis, max_force: Real) -> &mut Self { + self.data.set_motor_max_force(axis, max_force); + self + } + + pub fn set_limits(&mut self, axis: JointAxis, limits: [Real; 2]) -> &mut Self { + self.data.set_limits(axis, limits); + self + } +} + +impl Into for SphericalJoint { + fn into(self) -> GenericJoint { + self.data + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +#[derive(Copy, Clone, Debug, PartialEq)] +pub struct SphericalJointBuilder(SphericalJoint); + +impl Default for SphericalJointBuilder { + fn default() -> Self { + Self(SphericalJoint::new()) + } +} + +impl SphericalJointBuilder { + pub fn new() -> Self { + Self(SphericalJoint::new()) + } + #[must_use] pub fn local_anchor1(mut self, anchor1: Point) -> Self { - self.data = self.data.local_anchor1(anchor1); + self.0.set_local_anchor1(anchor1); self } #[must_use] pub fn local_anchor2(mut self, anchor2: Point) -> Self { - self.data = self.data.local_anchor2(anchor2); + self.0.set_local_anchor2(anchor2); self } /// Set the spring-like model used by the motor to reach the desired target velocity and position. + #[must_use] pub fn motor_model(mut self, axis: JointAxis, model: MotorModel) -> Self { - self.data = self.data.motor_model(axis, model); + self.0.set_motor_model(axis, model); self } /// Sets the target velocity this motor needs to reach. + #[must_use] pub fn motor_velocity(mut self, axis: JointAxis, target_vel: Real, factor: Real) -> Self { - self.data = self.data.motor_velocity(axis, target_vel, factor); + self.0.set_motor_velocity(axis, target_vel, factor); self } /// Sets the target angle this motor needs to reach. + #[must_use] pub fn motor_position( mut self, axis: JointAxis, @@ -57,14 +146,14 @@ impl SphericalJoint { stiffness: Real, damping: Real, ) -> Self { - self.data = self - .data - .motor_position(axis, target_pos, stiffness, damping); + self.0 + .set_motor_position(axis, target_pos, stiffness, damping); self } /// Configure both the target angle and target velocity of the motor. - pub fn motor_axis( + #[must_use] + pub fn motor( mut self, axis: JointAxis, target_pos: Real, @@ -72,26 +161,31 @@ impl SphericalJoint { stiffness: Real, damping: Real, ) -> Self { - self.data = self - .data - .motor_axis(axis, target_pos, target_vel, stiffness, damping); - self - } - - pub fn motor_max_impulse(mut self, axis: JointAxis, max_impulse: Real) -> Self { - self.data = self.data.motor_max_impulse(axis, max_impulse); + self.0 + .set_motor(axis, target_pos, target_vel, stiffness, damping); self } #[must_use] - pub fn limit_axis(mut self, axis: JointAxis, limits: [Real; 2]) -> Self { - self.data = self.data.limit_axis(axis, limits); + pub fn motor_max_force(mut self, axis: JointAxis, max_force: Real) -> Self { + self.0.set_motor_max_force(axis, max_force); self } + + #[must_use] + pub fn limits(mut self, axis: JointAxis, limits: [Real; 2]) -> Self { + self.0.set_limits(axis, limits); + self + } + + #[must_use] + pub fn build(self) -> SphericalJoint { + self.0 + } } -impl Into for SphericalJoint { - fn into(self) -> JointData { - self.data +impl Into for SphericalJointBuilder { + fn into(self) -> GenericJoint { + self.0.into() } } diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs index db972d4..1b08b50 100644 --- a/src/dynamics/rigid_body.rs +++ b/src/dynamics/rigid_body.rs @@ -1096,3 +1096,9 @@ impl RigidBodyBuilder { rb } } + +impl Into for RigidBodyBuilder { + fn into(self) -> RigidBody { + self.build() + } +} diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs index 34f7bdf..d6999ee 100644 --- a/src/dynamics/rigid_body_set.rs +++ b/src/dynamics/rigid_body_set.rs @@ -121,7 +121,8 @@ impl RigidBodySet { } /// Insert a rigid body into this set and retrieve its handle. - pub fn insert(&mut self, mut rb: RigidBody) -> RigidBodyHandle { + pub fn insert(&mut self, rb: impl Into) -> RigidBodyHandle { + let mut rb = rb.into(); // Make sure the internal links are reset, they may not be // if this rigid-body was obtained by cloning another one. rb.reset_internal_references(); diff --git a/src/dynamics/solver/generic_velocity_constraint.rs b/src/dynamics/solver/generic_velocity_constraint.rs index c1d4134..a304008 100644 --- a/src/dynamics/solver/generic_velocity_constraint.rs +++ b/src/dynamics/solver/generic_velocity_constraint.rs @@ -261,7 +261,7 @@ impl GenericVelocityConstraint { let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1); rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt; - rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction; + rhs_wo_bias *= is_bouncy + is_resting; let rhs_bias = /* is_resting * */ erp_inv_dt * manifold_point.dist.min(0.0); diff --git a/src/dynamics/solver/generic_velocity_ground_constraint.rs b/src/dynamics/solver/generic_velocity_ground_constraint.rs index 9ce824e..336954b 100644 --- a/src/dynamics/solver/generic_velocity_ground_constraint.rs +++ b/src/dynamics/solver/generic_velocity_ground_constraint.rs @@ -145,7 +145,7 @@ impl GenericVelocityGroundConstraint { let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1); rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt; - rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction; + rhs_wo_bias *= is_bouncy + is_resting ; let rhs_bias = /* is_resting * */ erp_inv_dt * manifold_point.dist.min(0.0); diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs index c31cee1..36c42bd 100644 --- a/src/dynamics/solver/island_solver.rs +++ b/src/dynamics/solver/island_solver.rs @@ -1,6 +1,6 @@ use super::VelocitySolver; use crate::counters::Counters; -use crate::data::{BundleSet, ComponentSet, ComponentSetMut}; +use crate::data::{ComponentSet, ComponentSetMut}; use crate::dynamics::solver::{ AnyGenericVelocityConstraint, AnyJointVelocityConstraint, AnyVelocityConstraint, SolverConstraints, diff --git a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs index be42b87..4f26a73 100644 --- a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs @@ -1,7 +1,7 @@ use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::WritebackId; use crate::dynamics::solver::joint_constraint::{JointVelocityConstraintBuilder, SolverBody}; use crate::dynamics::solver::DeltaVel; -use crate::dynamics::{IntegrationParameters, JointData, JointGraphEdge, JointIndex, Multibody}; +use crate::dynamics::{GenericJoint, IntegrationParameters, JointGraphEdge, JointIndex, Multibody}; use crate::math::{Isometry, Real, DIM}; use crate::prelude::SPATIAL_DIM; use na::{DVector, DVectorSlice, DVectorSliceMut}; @@ -25,6 +25,8 @@ pub struct JointGenericVelocityConstraint { pub inv_lhs: Real, pub rhs: Real, pub rhs_wo_bias: Real, + pub cfm_coeff: Real, + pub cfm_gain: Real, pub writeback_id: WritebackId, } @@ -52,6 +54,8 @@ impl JointGenericVelocityConstraint { inv_lhs: 0.0, rhs: 0.0, rhs_wo_bias: 0.0, + cfm_coeff: 0.0, + cfm_gain: 0.0, writeback_id: WritebackId::Dof(0), } } @@ -65,7 +69,7 @@ impl JointGenericVelocityConstraint { mb2: Option<(&Multibody, usize)>, frame1: &Isometry, frame2: &Isometry, - joint: &JointData, + joint: &GenericJoint, jacobians: &mut DVector, j_id: &mut usize, out: &mut [Self], @@ -83,26 +87,10 @@ impl JointGenericVelocityConstraint { locked_axes, ); - for i in 0..DIM { - if locked_axes & (1 << i) != 0 { - out[len] = builder.lock_linear_generic( - params, - jacobians, - j_id, - joint_id, - body1, - body2, - mb1, - mb2, - i, - WritebackId::Dof(i), - ); - len += 1; - } - } + let start = len; for i in DIM..SPATIAL_DIM { - if locked_axes & (1 << i) != 0 { - out[len] = builder.lock_angular_generic( + if (motor_axes >> DIM) & (1 << i) != 0 { + out[len] = builder.motor_angular_generic( params, jacobians, j_id, @@ -112,12 +100,12 @@ impl JointGenericVelocityConstraint { mb1, mb2, i - DIM, - WritebackId::Dof(i), + &joint.motors[i].motor_params(params.dt), + WritebackId::Motor(i), ); len += 1; } } - for i in 0..DIM { if motor_axes & (1 << i) != 0 { out[len] = builder.motor_linear_generic( @@ -137,10 +125,15 @@ impl JointGenericVelocityConstraint { len += 1; } } + JointVelocityConstraintBuilder::finalize_generic_constraints( + jacobians, + &mut out[start..len], + ); + let start = len; for i in DIM..SPATIAL_DIM { - if (motor_axes >> DIM) & (1 << i) != 0 { - out[len] = builder.motor_angular_generic( + if locked_axes & (1 << i) != 0 { + out[len] = builder.lock_angular_generic( params, jacobians, j_id, @@ -150,16 +143,14 @@ impl JointGenericVelocityConstraint { mb1, mb2, i - DIM, - &joint.motors[i].motor_params(params.dt), - WritebackId::Motor(i), + WritebackId::Dof(i), ); len += 1; } } - for i in 0..DIM { - if limit_axes & (1 << i) != 0 { - out[len] = builder.limit_linear_generic( + if locked_axes & (1 << i) != 0 { + out[len] = builder.lock_linear_generic( params, jacobians, j_id, @@ -169,8 +160,7 @@ impl JointGenericVelocityConstraint { mb1, mb2, i, - [joint.limits[i].min, joint.limits[i].max], - WritebackId::Limit(i), + WritebackId::Dof(i), ); len += 1; } @@ -194,8 +184,29 @@ impl JointGenericVelocityConstraint { len += 1; } } + for i in 0..DIM { + if limit_axes & (1 << i) != 0 { + out[len] = builder.limit_linear_generic( + params, + jacobians, + j_id, + joint_id, + body1, + body2, + mb1, + mb2, + i, + [joint.limits[i].min, joint.limits[i].max], + WritebackId::Limit(i), + ); + len += 1; + } + } - JointVelocityConstraintBuilder::finalize_generic_constraints(jacobians, &mut out[..len]); + JointVelocityConstraintBuilder::finalize_generic_constraints( + jacobians, + &mut out[start..len], + ); len } @@ -273,7 +284,7 @@ impl JointGenericVelocityConstraint { let dvel = self.rhs + (vel2 - vel1); let total_impulse = na::clamp( - self.impulse + self.inv_lhs * dvel, + self.impulse + self.inv_lhs * (dvel - self.cfm_gain * self.impulse), self.impulse_bounds[0], self.impulse_bounds[1], ); @@ -316,6 +327,8 @@ pub struct JointGenericVelocityGroundConstraint { pub inv_lhs: Real, pub rhs: Real, pub rhs_wo_bias: Real, + pub cfm_coeff: Real, + pub cfm_gain: Real, pub writeback_id: WritebackId, } @@ -338,6 +351,8 @@ impl JointGenericVelocityGroundConstraint { inv_lhs: 0.0, rhs: 0.0, rhs_wo_bias: 0.0, + cfm_coeff: 0.0, + cfm_gain: 0.0, writeback_id: WritebackId::Dof(0), } } @@ -350,7 +365,7 @@ impl JointGenericVelocityGroundConstraint { mb2: (&Multibody, usize), frame1: &Isometry, frame2: &Isometry, - joint: &JointData, + joint: &GenericJoint, jacobians: &mut DVector, j_id: &mut usize, out: &mut [Self], @@ -368,32 +383,20 @@ impl JointGenericVelocityGroundConstraint { locked_axes, ); - for i in 0..DIM { - if locked_axes & (1 << i) != 0 { - out[len] = builder.lock_linear_generic_ground( - params, - jacobians, - j_id, - joint_id, - body1, - mb2, - i, - WritebackId::Dof(i), - ); - len += 1; - } - } + let start = len; for i in DIM..SPATIAL_DIM { - if locked_axes & (1 << i) != 0 { - out[len] = builder.lock_angular_generic_ground( + if (motor_axes >> DIM) & (1 << i) != 0 { + out[len] = builder.motor_angular_generic_ground( params, jacobians, j_id, joint_id, body1, + body2, mb2, i - DIM, - WritebackId::Dof(i), + &joint.motors[i].motor_params(params.dt), + WritebackId::Motor(i), ); len += 1; } @@ -418,27 +421,30 @@ impl JointGenericVelocityGroundConstraint { } } + JointVelocityConstraintBuilder::finalize_generic_constraints_ground( + jacobians, + &mut out[start..len], + ); + + let start = len; for i in DIM..SPATIAL_DIM { - if (motor_axes >> DIM) & (1 << i) != 0 { - out[len] = builder.motor_angular_generic_ground( + if locked_axes & (1 << i) != 0 { + out[len] = builder.lock_angular_generic_ground( params, jacobians, j_id, joint_id, body1, - body2, mb2, i - DIM, - &joint.motors[i].motor_params(params.dt), - WritebackId::Motor(i), + WritebackId::Dof(i), ); len += 1; } } - for i in 0..DIM { - if limit_axes & (1 << i) != 0 { - out[len] = builder.limit_linear_generic_ground( + if locked_axes & (1 << i) != 0 { + out[len] = builder.lock_linear_generic_ground( params, jacobians, j_id, @@ -446,8 +452,7 @@ impl JointGenericVelocityGroundConstraint { body1, mb2, i, - [joint.limits[i].min, joint.limits[i].max], - WritebackId::Limit(i), + WritebackId::Dof(i), ); len += 1; } @@ -469,10 +474,26 @@ impl JointGenericVelocityGroundConstraint { len += 1; } } + for i in 0..DIM { + if limit_axes & (1 << i) != 0 { + out[len] = builder.limit_linear_generic_ground( + params, + jacobians, + j_id, + joint_id, + body1, + mb2, + i, + [joint.limits[i].min, joint.limits[i].max], + WritebackId::Limit(i), + ); + len += 1; + } + } JointVelocityConstraintBuilder::finalize_generic_constraints_ground( jacobians, - &mut out[..len], + &mut out[start..len], ); len } @@ -511,7 +532,7 @@ impl JointGenericVelocityGroundConstraint { let dvel = self.rhs + vel2; let total_impulse = na::clamp( - self.impulse + self.inv_lhs * dvel, + self.impulse + self.inv_lhs * (dvel - self.cfm_gain * self.impulse), self.impulse_bounds[0], self.impulse_bounds[1], ); diff --git a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs index 5edc815..510f46f 100644 --- a/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs @@ -113,7 +113,7 @@ impl JointVelocityConstraintBuilder { j.copy_from(&wj); } - let rhs_wo_bias = (vel2 - vel1) * params.velocity_solve_fraction; + let rhs_wo_bias = vel2 - vel1; let mj_lambda1 = mb1.map(|m| m.0.solver_id).unwrap_or(body1.mj_lambda[0]); let mj_lambda2 = mb2.map(|m| m.0.solver_id).unwrap_or(body2.mj_lambda[0]); @@ -133,6 +133,8 @@ impl JointVelocityConstraintBuilder { inv_lhs: 0.0, rhs: rhs_wo_bias, rhs_wo_bias, + cfm_coeff: 0.0, + cfm_gain: 0.0, writeback_id, } } @@ -169,7 +171,7 @@ impl JointVelocityConstraintBuilder { ang_jac2, ); - let erp_inv_dt = params.erp_inv_dt(); + let erp_inv_dt = params.joint_erp_inv_dt(); let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt; c.rhs += rhs_bias; c @@ -212,7 +214,7 @@ impl JointVelocityConstraintBuilder { let min_enabled = dist < limits[0]; let max_enabled = limits[1] < dist; - let erp_inv_dt = params.erp_inv_dt(); + let erp_inv_dt = params.joint_erp_inv_dt(); let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt; constraint.rhs += rhs_bias; constraint.impulse_bounds = [ @@ -265,20 +267,20 @@ impl JointVelocityConstraintBuilder { ); let mut rhs_wo_bias = 0.0; - if motor_params.stiffness != 0.0 { + if motor_params.erp_inv_dt != 0.0 { let dist = self.lin_err.dot(&lin_jac); - rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.stiffness; + rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt; } - if motor_params.damping != 0.0 { - let dvel = lin_jac.dot(&(body2.linvel - body1.linvel)) - + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); - rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping; - } + let dvel = lin_jac.dot(&(body2.linvel - body1.linvel)) + + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); + rhs_wo_bias += dvel - motor_params.target_vel; constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse]; constraint.rhs = rhs_wo_bias; constraint.rhs_wo_bias = rhs_wo_bias; + constraint.cfm_coeff = motor_params.cfm_coeff; + constraint.cfm_gain = motor_params.cfm_gain; constraint } @@ -312,7 +314,7 @@ impl JointVelocityConstraintBuilder { ang_jac, ); - let erp_inv_dt = params.erp_inv_dt(); + let erp_inv_dt = params.joint_erp_inv_dt(); #[cfg(feature = "dim2")] let rhs_bias = self.ang_err.im * erp_inv_dt; #[cfg(feature = "dim3")] @@ -364,7 +366,7 @@ impl JointVelocityConstraintBuilder { max_enabled as u32 as Real * Real::MAX, ]; - let erp_inv_dt = params.erp_inv_dt(); + let erp_inv_dt = params.joint_erp_inv_dt(); let rhs_bias = ((s_ang - s_limits[1]).max(0.0) - (s_limits[0] - s_ang).max(0.0)) * erp_inv_dt; @@ -409,22 +411,22 @@ impl JointVelocityConstraintBuilder { ); let mut rhs_wo_bias = 0.0; - if motor_params.stiffness != 0.0 { + if motor_params.erp_inv_dt != 0.0 { #[cfg(feature = "dim2")] let s_ang_dist = self.ang_err.im; #[cfg(feature = "dim3")] let s_ang_dist = self.ang_err.imag()[_motor_axis]; let s_target_ang = motor_params.target_pos.sin(); - rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.stiffness; + rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt; } - if motor_params.damping != 0.0 { - let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); - rhs_wo_bias += (dvel - motor_params.target_vel * ang_jac.norm()) * motor_params.damping; - } + let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); + rhs_wo_bias += dvel - motor_params.target_vel; constraint.rhs_wo_bias = rhs_wo_bias; constraint.rhs = rhs_wo_bias; + constraint.cfm_coeff = motor_params.cfm_coeff; + constraint.cfm_gain = motor_params.cfm_gain; constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse]; constraint } @@ -436,6 +438,11 @@ impl JointVelocityConstraintBuilder { // TODO: orthogonalization doesn’t seem to give good results for multibodies? const ORTHOGONALIZE: bool = false; let len = constraints.len(); + + if len == 0 { + return; + } + let ndofs1 = constraints[0].ndofs1; let ndofs2 = constraints[0].ndofs2; @@ -449,8 +456,10 @@ impl JointVelocityConstraintBuilder { let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2); let dot_jj = jac_j1.dot(&w_jac_j1) + jac_j2.dot(&w_jac_j2); - let inv_dot_jj = crate::utils::inv(dot_jj); - c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs. + let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain; + let inv_dot_jj = crate::utils::simd_inv(dot_jj); + c_j.inv_lhs = crate::utils::simd_inv(dot_jj + cfm_gain); // Don’t forget to update the inv_lhs. + c_j.cfm_gain = cfm_gain; if c_j.impulse_bounds != [-Real::MAX, Real::MAX] { // Don't remove constraints with limited forces from the others @@ -510,7 +519,7 @@ impl JointVelocityConstraintBuilder { let vel2 = mb2 .fill_jacobians(link_id2, lin_jac, ang_jac2, j_id, jacobians) .1; - let rhs_wo_bias = (vel2 - vel1) * params.velocity_solve_fraction; + let rhs_wo_bias = vel2 - vel1; let mj_lambda2 = mb2.solver_id; @@ -524,6 +533,8 @@ impl JointVelocityConstraintBuilder { inv_lhs: 0.0, rhs: rhs_wo_bias, rhs_wo_bias, + cfm_coeff: 0.0, + cfm_gain: 0.0, writeback_id, } } @@ -556,7 +567,7 @@ impl JointVelocityConstraintBuilder { ang_jac2, ); - let erp_inv_dt = params.erp_inv_dt(); + let erp_inv_dt = params.joint_erp_inv_dt(); let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt; c.rhs += rhs_bias; c @@ -595,7 +606,7 @@ impl JointVelocityConstraintBuilder { let min_enabled = dist < limits[0]; let max_enabled = limits[1] < dist; - let erp_inv_dt = params.erp_inv_dt(); + let erp_inv_dt = params.joint_erp_inv_dt(); let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt; constraint.rhs += rhs_bias; constraint.impulse_bounds = [ @@ -645,20 +656,20 @@ impl JointVelocityConstraintBuilder { ); let mut rhs_wo_bias = 0.0; - if motor_params.stiffness != 0.0 { + if motor_params.erp_inv_dt != 0.0 { let dist = self.lin_err.dot(&lin_jac); - rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.stiffness; + rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt; } - if motor_params.damping != 0.0 { - let dvel = lin_jac.dot(&(body2.linvel - body1.linvel)) - + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); - rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping; - } + let dvel = lin_jac.dot(&(body2.linvel - body1.linvel)) + + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); + rhs_wo_bias += dvel - motor_params.target_vel; constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse]; constraint.rhs = rhs_wo_bias; constraint.rhs_wo_bias = rhs_wo_bias; + constraint.cfm_coeff = motor_params.cfm_coeff; + constraint.cfm_gain = motor_params.cfm_gain; constraint } @@ -688,7 +699,7 @@ impl JointVelocityConstraintBuilder { ang_jac, ); - let erp_inv_dt = params.erp_inv_dt(); + let erp_inv_dt = params.joint_erp_inv_dt(); #[cfg(feature = "dim2")] let rhs_bias = self.ang_err.im * erp_inv_dt; #[cfg(feature = "dim3")] @@ -736,7 +747,7 @@ impl JointVelocityConstraintBuilder { max_enabled as u32 as Real * Real::MAX, ]; - let erp_inv_dt = params.erp_inv_dt(); + let erp_inv_dt = params.joint_erp_inv_dt(); let rhs_bias = ((s_ang - s_limits[1]).max(0.0) - (s_limits[0] - s_ang).max(0.0)) * erp_inv_dt; @@ -778,22 +789,22 @@ impl JointVelocityConstraintBuilder { ); let mut rhs = 0.0; - if motor_params.stiffness != 0.0 { + if motor_params.erp_inv_dt != 0.0 { #[cfg(feature = "dim2")] let s_ang_dist = self.ang_err.im; #[cfg(feature = "dim3")] let s_ang_dist = self.ang_err.imag()[_motor_axis]; let s_target_ang = motor_params.target_pos.sin(); - rhs += (s_ang_dist - s_target_ang) * motor_params.stiffness; + rhs += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt; } - if motor_params.damping != 0.0 { - let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); - rhs += (dvel - motor_params.target_vel * ang_jac.norm()) * motor_params.damping; - } + let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); + rhs += dvel - motor_params.target_vel; constraint.rhs_wo_bias = rhs; constraint.rhs = rhs; + constraint.cfm_coeff = motor_params.cfm_coeff; + constraint.cfm_gain = motor_params.cfm_gain; constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse]; constraint } @@ -805,6 +816,11 @@ impl JointVelocityConstraintBuilder { // TODO: orthogonalization doesn’t seem to give good results for multibodies? const ORTHOGONALIZE: bool = false; let len = constraints.len(); + + if len == 0 { + return; + } + let ndofs2 = constraints[0].ndofs2; // Use the modified Gramm-Schmidt orthogonalization. @@ -815,8 +831,10 @@ impl JointVelocityConstraintBuilder { let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2); let dot_jj = jac_j2.dot(&w_jac_j2); - let inv_dot_jj = crate::utils::inv(dot_jj); - c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs. + let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain; + let inv_dot_jj = crate::utils::simd_inv(dot_jj); + c_j.inv_lhs = crate::utils::simd_inv(dot_jj + cfm_gain); // Don’t forget to update the inv_lhs. + c_j.cfm_gain = cfm_gain; if c_j.impulse_bounds != [-Real::MAX, Real::MAX] { // Don't remove constraints with limited forces from the others diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs index d264313..f830521 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs @@ -1,6 +1,8 @@ use crate::dynamics::solver::joint_constraint::JointVelocityConstraintBuilder; use crate::dynamics::solver::DeltaVel; -use crate::dynamics::{IntegrationParameters, JointData, JointGraphEdge, JointIndex}; +use crate::dynamics::{ + GenericJoint, IntegrationParameters, JointAxesMask, JointGraphEdge, JointIndex, +}; use crate::math::{AngVector, AngularInertia, Isometry, Point, Real, Vector, DIM, SPATIAL_DIM}; use crate::utils::{WDot, WReal}; @@ -12,10 +14,9 @@ use { #[derive(Copy, Clone, PartialEq, Debug)] pub struct MotorParameters { - pub stiffness: N, - pub damping: N, - pub gamma: N, - // pub keep_lhs: bool, + pub erp_inv_dt: N, + pub cfm_coeff: N, + pub cfm_gain: N, pub target_pos: N, pub target_vel: N, pub max_impulse: N, @@ -24,10 +25,9 @@ pub struct MotorParameters { impl Default for MotorParameters { fn default() -> Self { Self { - stiffness: N::zero(), - damping: N::zero(), - gamma: N::zero(), - // keep_lhs: true, + erp_inv_dt: N::zero(), + cfm_coeff: N::zero(), + cfm_gain: N::zero(), target_pos: N::zero(), target_vel: N::zero(), max_impulse: N::zero(), @@ -72,6 +72,8 @@ pub struct JointVelocityConstraint { pub inv_lhs: N, pub rhs: N, pub rhs_wo_bias: N, + pub cfm_gain: N, + pub cfm_coeff: N, pub im1: Vector, pub im2: Vector, @@ -91,6 +93,8 @@ impl JointVelocityConstraint { ang_jac1: na::zero(), ang_jac2: na::zero(), inv_lhs: N::zero(), + cfm_gain: N::zero(), + cfm_coeff: N::zero(), rhs: N::zero(), rhs_wo_bias: N::zero(), im1: na::zero(), @@ -105,7 +109,7 @@ impl JointVelocityConstraint { self.ang_jac2.gdot(mj_lambda2.angular) - self.ang_jac1.gdot(mj_lambda1.angular); let rhs = dlinvel + dangvel + self.rhs; - let total_impulse = (self.impulse + self.inv_lhs * rhs) + let total_impulse = (self.impulse + self.inv_lhs * (rhs - self.cfm_gain * self.impulse)) .simd_clamp(self.impulse_bounds[0], self.impulse_bounds[1]); let delta_impulse = total_impulse - self.impulse; self.impulse = total_impulse; @@ -133,13 +137,14 @@ impl JointVelocityConstraint { body2: &SolverBody, frame1: &Isometry, frame2: &Isometry, - joint: &JointData, + joint: &GenericJoint, out: &mut [Self], ) -> usize { let mut len = 0; let locked_axes = joint.locked_axes.bits(); - let motor_axes = joint.motor_axes.bits(); - let limit_axes = joint.limit_axes.bits(); + let motor_axes = joint.motor_axes.bits() & !locked_axes; + let limit_axes = joint.limit_axes.bits() & !locked_axes; + let coupled_axes = joint.coupled_axes.bits(); let builder = JointVelocityConstraintBuilder::new( frame1, @@ -149,13 +154,62 @@ impl JointVelocityConstraint { locked_axes, ); - for i in 0..DIM { - if locked_axes & (1 << i) != 0 { - out[len] = - builder.lock_linear(params, [joint_id], body1, body2, i, WritebackId::Dof(i)); + let start = len; + for i in DIM..SPATIAL_DIM { + if (motor_axes & !coupled_axes) & (1 << i) != 0 { + out[len] = builder.motor_angular( + [joint_id], + body1, + body2, + i - DIM, + &joint.motors[i].motor_params(params.dt), + WritebackId::Motor(i), + ); len += 1; } } + for i in 0..DIM { + if (motor_axes & !coupled_axes) & (1 << i) != 0 { + let limits = if limit_axes & (1 << i) != 0 { + Some([joint.limits[i].min, joint.limits[i].max]) + } else { + None + }; + + out[len] = builder.motor_linear( + params, + [joint_id], + body1, + body2, + i, + &joint.motors[i].motor_params(params.dt), + limits, + WritebackId::Motor(i), + ); + len += 1; + } + } + + if (motor_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 { + // TODO: coupled angular motor constraint. + } + + if (motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 { + // TODO: coupled linear limit constraint. + // out[len] = builder.motor_linear_coupled( + // params, + // [joint_id], + // body1, + // body2, + // limit_axes & coupled_axes, + // &joint.limits, + // WritebackId::Limit(0), // TODO: writeback + // ); + // len += 1; + } + JointVelocityConstraintBuilder::finalize_constraints(&mut out[start..len]); + + let start = len; for i in DIM..SPATIAL_DIM { if locked_axes & (1 << i) != 0 { out[len] = builder.lock_angular( @@ -169,52 +223,16 @@ impl JointVelocityConstraint { len += 1; } } - for i in 0..DIM { - if motor_axes & (1 << i) != 0 { - out[len] = builder.motor_linear( - params, - [joint_id], - body1, - body2, - locked_axes >> DIM, - i, - &joint.motors[i].motor_params(params.dt), - WritebackId::Motor(i), - ); - len += 1; - } - } - for i in DIM..SPATIAL_DIM { - if motor_axes & (1 << i) != 0 { - out[len] = builder.motor_angular( - [joint_id], - body1, - body2, - i - DIM, - &joint.motors[i].motor_params(params.dt), - WritebackId::Motor(i), - ); + if locked_axes & (1 << i) != 0 { + out[len] = + builder.lock_linear(params, [joint_id], body1, body2, i, WritebackId::Dof(i)); len += 1; } } - for i in 0..DIM { - if limit_axes & (1 << i) != 0 { - out[len] = builder.limit_linear( - params, - [joint_id], - body1, - body2, - i, - [joint.limits[i].min, joint.limits[i].max], - WritebackId::Limit(i), - ); - len += 1; - } - } for i in DIM..SPATIAL_DIM { - if limit_axes & (1 << i) != 0 { + if (limit_axes & !coupled_axes) & (1 << i) != 0 { out[len] = builder.limit_angular( params, [joint_id], @@ -227,8 +245,40 @@ impl JointVelocityConstraint { len += 1; } } + for i in 0..DIM { + if (limit_axes & !coupled_axes) & (1 << i) != 0 { + out[len] = builder.limit_linear( + params, + [joint_id], + body1, + body2, + i, + [joint.limits[i].min, joint.limits[i].max], + WritebackId::Limit(i), + ); + len += 1; + } + } + + if (limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 { + // TODO: coupled angular limit constraint. + } + + if (limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 { + // TODO: coupled linear limit constraint. + out[len] = builder.limit_linear_coupled( + params, + [joint_id], + body1, + body2, + limit_axes & coupled_axes, + &joint.limits, + WritebackId::Limit(0), // TODO: writeback + ); + len += 1; + } + JointVelocityConstraintBuilder::finalize_constraints(&mut out[start..len]); - JointVelocityConstraintBuilder::finalize_constraints(&mut out[..len]); len } @@ -349,6 +399,8 @@ pub struct JointVelocityGroundConstraint { pub ang_jac2: AngVector, pub inv_lhs: N, + pub cfm_coeff: N, + pub cfm_gain: N, pub rhs: N, pub rhs_wo_bias: N, @@ -367,6 +419,8 @@ impl JointVelocityGroundConstraint { lin_jac: Vector::zeros(), ang_jac2: na::zero(), inv_lhs: N::zero(), + cfm_coeff: N::zero(), + cfm_gain: N::zero(), rhs: N::zero(), rhs_wo_bias: N::zero(), im2: na::zero(), @@ -379,7 +433,7 @@ impl JointVelocityGroundConstraint { let dangvel = mj_lambda2.angular; let dvel = self.lin_jac.dot(&dlinvel) + self.ang_jac2.gdot(dangvel) + self.rhs; - let total_impulse = (self.impulse + self.inv_lhs * dvel) + let total_impulse = (self.impulse + self.inv_lhs * (dvel - self.cfm_gain * self.impulse)) .simd_clamp(self.impulse_bounds[0], self.impulse_bounds[1]); let delta_impulse = total_impulse - self.impulse; self.impulse = total_impulse; @@ -404,13 +458,14 @@ impl JointVelocityGroundConstraint { body2: &SolverBody, frame1: &Isometry, frame2: &Isometry, - joint: &JointData, + joint: &GenericJoint, out: &mut [Self], ) -> usize { let mut len = 0; - let locked_axes = joint.locked_axes.bits() as u8; - let motor_axes = joint.motor_axes.bits() as u8; - let limit_axes = joint.limit_axes.bits() as u8; + let locked_axes = joint.locked_axes.bits(); + let motor_axes = joint.motor_axes.bits() & !locked_axes; + let limit_axes = joint.limit_axes.bits() & !locked_axes; + let coupled_axes = joint.coupled_axes.bits(); let builder = JointVelocityConstraintBuilder::new( frame1, @@ -420,19 +475,68 @@ impl JointVelocityGroundConstraint { locked_axes, ); + let start = len; + for i in DIM..SPATIAL_DIM { + if (motor_axes & !coupled_axes) & (1 << i) != 0 { + out[len] = builder.motor_angular_ground( + [joint_id], + body1, + body2, + i - DIM, + &joint.motors[i].motor_params(params.dt), + WritebackId::Motor(i), + ); + len += 1; + } + } for i in 0..DIM { - if locked_axes & (1 << i) != 0 { - out[len] = builder.lock_linear_ground( + if (motor_axes & !coupled_axes) & (1 << i) != 0 { + let limits = if limit_axes & (1 << i) != 0 { + Some([joint.limits[i].min, joint.limits[i].max]) + } else { + None + }; + + out[len] = builder.motor_linear_ground( params, [joint_id], body1, body2, i, - WritebackId::Dof(i), + &joint.motors[i].motor_params(params.dt), + limits, + WritebackId::Motor(i), ); len += 1; } } + + if (motor_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 { + // TODO: coupled angular motor constraint. + } + + if (motor_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 { + /* + // TODO: coupled linear motor constraint. + out[len] = builder.motor_linear_coupled_ground( + params, + [joint_id], + body1, + body2, + motor_axes & coupled_axes, + &joint.motors, + limit_axes & coupled_axes, + &joint.limits, + WritebackId::Limit(0), // TODO: writeback + ); + len += 1; + */ + todo!() + } + + JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[start..len]); + + let start = len; for i in DIM..SPATIAL_DIM { if locked_axes & (1 << i) != 0 { out[len] = builder.lock_angular_ground( @@ -446,50 +550,22 @@ impl JointVelocityGroundConstraint { len += 1; } } - for i in 0..DIM { - if motor_axes & (1 << i) != 0 { - out[len] = builder.motor_linear_ground( - [joint_id], - body1, - body2, - i, - &joint.motors[i].motor_params(params.dt), - WritebackId::Motor(i), - ); - len += 1; - } - } - for i in DIM..SPATIAL_DIM { - if motor_axes & (1 << i) != 0 { - out[len] = builder.motor_angular_ground( - [joint_id], - body1, - body2, - i - DIM, - &joint.motors[i].motor_params(params.dt), - WritebackId::Motor(i), - ); - len += 1; - } - } - - for i in 0..DIM { - if limit_axes & (1 << i) != 0 { - out[len] = builder.limit_linear_ground( + if locked_axes & (1 << i) != 0 { + out[len] = builder.lock_linear_ground( params, [joint_id], body1, body2, i, - [joint.limits[i].min, joint.limits[i].max], - WritebackId::Limit(i), + WritebackId::Dof(i), ); len += 1; } } + for i in DIM..SPATIAL_DIM { - if limit_axes & (1 << i) != 0 { + if (limit_axes & !coupled_axes) & (1 << i) != 0 { out[len] = builder.limit_angular_ground( params, [joint_id], @@ -502,8 +578,39 @@ impl JointVelocityGroundConstraint { len += 1; } } + for i in 0..DIM { + if (limit_axes & !coupled_axes) & (1 << i) != 0 { + out[len] = builder.limit_linear_ground( + params, + [joint_id], + body1, + body2, + i, + [joint.limits[i].min, joint.limits[i].max], + WritebackId::Limit(i), + ); + len += 1; + } + } + + if (limit_axes & coupled_axes) & JointAxesMask::ANG_AXES.bits() != 0 { + // TODO: coupled angular limit constraint. + } + + if (limit_axes & coupled_axes) & JointAxesMask::LIN_AXES.bits() != 0 { + out[len] = builder.limit_linear_coupled_ground( + params, + [joint_id], + body1, + body2, + limit_axes & coupled_axes, + &joint.limits, + WritebackId::Limit(0), // TODO: writeback + ); + len += 1; + } + JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[start..len]); - JointVelocityConstraintBuilder::finalize_ground_constraints(&mut out[..len]); len } diff --git a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs index 3aa00f2..95ab288 100644 --- a/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs +++ b/src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs @@ -3,8 +3,8 @@ use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{ }; use crate::dynamics::solver::joint_constraint::SolverBody; use crate::dynamics::solver::MotorParameters; -use crate::dynamics::{IntegrationParameters, JointIndex}; -use crate::math::{Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM}; +use crate::dynamics::{IntegrationParameters, JointAxesMask, JointIndex, JointLimits, JointMotor}; +use crate::math::{AngVector, Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM}; use crate::utils::{IndexMut2, WCrossMatrix, WDot, WQuat, WReal}; use na::SMatrix; @@ -92,10 +92,12 @@ impl JointVelocityConstraintBuilder { let min_enabled = dist.simd_lt(limits[0]); let max_enabled = limits[1].simd_lt(dist); - let erp_inv_dt = N::splat(params.erp_inv_dt()); + let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); + let cfm_coeff = N::splat(params.joint_cfm_coeff()); let rhs_bias = ((dist - limits[1]).simd_max(zero) - (limits[0] - dist).simd_max(zero)) * erp_inv_dt; constraint.rhs = constraint.rhs_wo_bias + rhs_bias; + constraint.cfm_coeff = cfm_coeff; constraint.impulse_bounds = [ N::splat(-Real::INFINITY).select(min_enabled, zero), N::splat(Real::INFINITY).select(max_enabled, zero), @@ -104,39 +106,114 @@ impl JointVelocityConstraintBuilder { constraint } + pub fn limit_linear_coupled( + &self, + params: &IntegrationParameters, + joint_id: [JointIndex; LANES], + body1: &SolverBody, + body2: &SolverBody, + limited_coupled_axes: u8, + limits: &[JointLimits], + writeback_id: WritebackId, + ) -> JointVelocityConstraint { + let zero = N::zero(); + let mut lin_jac = Vector::zeros(); + let mut ang_jac1: AngVector = na::zero(); + let mut ang_jac2: AngVector = na::zero(); + let mut limit = N::zero(); + + for i in 0..DIM { + if limited_coupled_axes & (1 << i) != 0 { + let coeff = self.basis.column(i).dot(&self.lin_err); + lin_jac += self.basis.column(i) * coeff; + #[cfg(feature = "dim2")] + { + ang_jac1 += self.cmat1_basis[i] * coeff; + ang_jac2 += self.cmat2_basis[i] * coeff; + } + #[cfg(feature = "dim3")] + { + ang_jac1 += self.cmat1_basis.column(i) * coeff; + ang_jac2 += self.cmat2_basis.column(i) * coeff; + } + limit += limits[i].max * limits[i].max; + } + } + + limit = limit.simd_sqrt(); + let dist = lin_jac.norm(); + let inv_dist = crate::utils::simd_inv(dist); + lin_jac *= inv_dist; + ang_jac1 *= inv_dist; + ang_jac2 *= inv_dist; + + let dvel = lin_jac.dot(&(body2.linvel - body1.linvel)) + + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); + let rhs_wo_bias = dvel + (dist - limit).simd_min(zero) * N::splat(params.inv_dt()); + + ang_jac1 = body1.sqrt_ii * ang_jac1; + ang_jac2 = body2.sqrt_ii * ang_jac2; + + let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); + let cfm_coeff = N::splat(params.joint_cfm_coeff()); + let rhs_bias = (dist - limit).simd_max(zero) * erp_inv_dt; + let rhs = rhs_wo_bias + rhs_bias; + let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)]; + + JointVelocityConstraint { + joint_id, + mj_lambda1: body1.mj_lambda, + mj_lambda2: body2.mj_lambda, + im1: body1.im, + im2: body2.im, + impulse: N::zero(), + impulse_bounds, + lin_jac, + ang_jac1, + ang_jac2, + inv_lhs: N::zero(), // Will be set during ortogonalization. + cfm_coeff, + cfm_gain: N::zero(), + rhs, + rhs_wo_bias, + writeback_id, + } + } + pub fn motor_linear( &self, params: &IntegrationParameters, joint_id: [JointIndex; LANES], body1: &SolverBody, body2: &SolverBody, - _locked_ang_axes: u8, motor_axis: usize, motor_params: &MotorParameters, + limits: Option<[N; 2]>, writeback_id: WritebackId, ) -> JointVelocityConstraint { + let inv_dt = N::splat(params.inv_dt()); let mut constraint = self.lock_linear(params, joint_id, body1, body2, motor_axis, writeback_id); - // if locked_ang_axes & (1 << motor_axis) != 0 { - // // FIXME: check that this also works for cases - // // when not all the angular axes are locked. - // constraint.ang_jac1 = na::zero(); - // constraint.ang_jac2 = na::zero(); - // } - let mut rhs_wo_bias = N::zero(); - if motor_params.stiffness != N::zero() { + if motor_params.erp_inv_dt != N::zero() { let dist = self.lin_err.dot(&constraint.lin_jac); - rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.stiffness; + rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt; } - if motor_params.damping != N::zero() { - let dvel = constraint.lin_jac.dot(&(body2.linvel - body1.linvel)) - + (constraint.ang_jac2.gdot(body2.angvel) - constraint.ang_jac1.gdot(body1.angvel)); - rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping; - } + let mut target_vel = motor_params.target_vel; + if let Some(limits) = limits { + let dist = self.lin_err.dot(&constraint.lin_jac); + target_vel = + target_vel.simd_clamp((limits[0] - dist) * inv_dt, (limits[1] - dist) * inv_dt); + }; + let dvel = constraint.lin_jac.dot(&(body2.linvel - body1.linvel)) + + (constraint.ang_jac2.gdot(body2.angvel) - constraint.ang_jac1.gdot(body1.angvel)); + rhs_wo_bias += dvel - target_vel; + + constraint.cfm_coeff = motor_params.cfm_coeff; + constraint.cfm_gain = motor_params.cfm_gain; constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse]; constraint.rhs = rhs_wo_bias; constraint.rhs_wo_bias = rhs_wo_bias; @@ -164,10 +241,11 @@ impl JointVelocityConstraintBuilder { let dvel = lin_jac.dot(&(body2.linvel - body1.linvel)) + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); - let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction); + let rhs_wo_bias = dvel; - let erp_inv_dt = params.erp_inv_dt(); - let rhs_bias = lin_jac.dot(&self.lin_err) * N::splat(erp_inv_dt); + let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); + let cfm_coeff = N::splat(params.joint_cfm_coeff()); + let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt; ang_jac1 = body1.sqrt_ii * ang_jac1; ang_jac2 = body2.sqrt_ii * ang_jac2; @@ -184,6 +262,8 @@ impl JointVelocityConstraintBuilder { ang_jac1, ang_jac2, inv_lhs: N::zero(), // Will be set during ortogonalization. + cfm_coeff, + cfm_gain: N::zero(), rhs: rhs_wo_bias + rhs_bias, rhs_wo_bias, writeback_id, @@ -220,12 +300,13 @@ impl JointVelocityConstraintBuilder { #[cfg(feature = "dim3")] let ang_jac = self.ang_basis.column(limited_axis).into_owned(); let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); - let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction); + let rhs_wo_bias = dvel; - let erp_inv_dt = params.erp_inv_dt(); + let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); + let cfm_coeff = N::splat(params.joint_cfm_coeff()); let rhs_bias = ((s_ang - s_limits[1]).simd_max(zero) - (s_limits[0] - s_ang).simd_max(zero)) - * N::splat(erp_inv_dt); + * erp_inv_dt; let ang_jac1 = body1.sqrt_ii * ang_jac; let ang_jac2 = body2.sqrt_ii * ang_jac; @@ -242,6 +323,8 @@ impl JointVelocityConstraintBuilder { ang_jac1, ang_jac2, inv_lhs: N::zero(), // Will be set during ortogonalization. + cfm_coeff, + cfm_gain: N::zero(), rhs: rhs_wo_bias + rhs_bias, rhs_wo_bias, writeback_id, @@ -264,20 +347,17 @@ impl JointVelocityConstraintBuilder { let ang_jac = self.basis.column(_motor_axis).into_owned(); let mut rhs_wo_bias = N::zero(); - if motor_params.stiffness != N::zero() { + if motor_params.erp_inv_dt != N::zero() { #[cfg(feature = "dim2")] let s_ang_dist = self.ang_err.im; #[cfg(feature = "dim3")] let s_ang_dist = self.ang_err.imag()[_motor_axis]; let s_target_ang = motor_params.target_pos.simd_sin(); - rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.stiffness; + rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt; } - if motor_params.damping != N::zero() { - let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); - rhs_wo_bias += - (dvel - motor_params.target_vel/* * ang_jac.norm() */) * motor_params.damping; - } + let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); + rhs_wo_bias += dvel - motor_params.target_vel; let ang_jac1 = body1.sqrt_ii * ang_jac; let ang_jac2 = body2.sqrt_ii * ang_jac; @@ -294,6 +374,8 @@ impl JointVelocityConstraintBuilder { ang_jac1, ang_jac2, inv_lhs: N::zero(), // Will be set during ortogonalization. + cfm_coeff: motor_params.cfm_coeff, + cfm_gain: motor_params.cfm_gain, rhs: rhs_wo_bias, rhs_wo_bias, writeback_id, @@ -315,13 +397,14 @@ impl JointVelocityConstraintBuilder { let ang_jac = self.ang_basis.column(locked_axis).into_owned(); let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); - let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction); + let rhs_wo_bias = dvel; - let erp_inv_dt = params.erp_inv_dt(); + let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); + let cfm_coeff = N::splat(params.joint_cfm_coeff()); #[cfg(feature = "dim2")] - let rhs_bias = self.ang_err.im * N::splat(erp_inv_dt); + let rhs_bias = self.ang_err.im * erp_inv_dt; #[cfg(feature = "dim3")] - let rhs_bias = self.ang_err.imag()[locked_axis] * N::splat(erp_inv_dt); + let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt; let ang_jac1 = body1.sqrt_ii * ang_jac; let ang_jac2 = body2.sqrt_ii * ang_jac; @@ -338,6 +421,8 @@ impl JointVelocityConstraintBuilder { ang_jac1, ang_jac2, inv_lhs: N::zero(), // Will be set during ortogonalization. + cfm_coeff, + cfm_gain: N::zero(), rhs: rhs_wo_bias + rhs_bias, rhs_wo_bias, writeback_id, @@ -349,6 +434,11 @@ impl JointVelocityConstraintBuilder { constraints: &mut [JointVelocityConstraint], ) { let len = constraints.len(); + + if len == 0 { + return; + } + let imsum = constraints[0].im1 + constraints[0].im2; // Use the modified Gram-Schmidt orthogonalization. @@ -357,8 +447,10 @@ impl JointVelocityConstraintBuilder { let dot_jj = c_j.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac)) + c_j.ang_jac1.gdot(c_j.ang_jac1) + c_j.ang_jac2.gdot(c_j.ang_jac2); + let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain; let inv_dot_jj = crate::utils::simd_inv(dot_jj); - c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs. + c_j.inv_lhs = crate::utils::simd_inv(dot_jj + cfm_gain); // Don’t forget to update the inv_lhs. + c_j.cfm_gain = cfm_gain; if c_j.impulse_bounds != [-N::splat(Real::MAX), N::splat(Real::MAX)] { // Don't remove constraints with limited forces from the others @@ -369,6 +461,7 @@ impl JointVelocityConstraintBuilder { for i in (j + 1)..len { let (c_i, c_j) = constraints.index_mut_const(i, j); + let dot_ij = c_i.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac)) + c_i.ang_jac1.gdot(c_j.ang_jac1) + c_i.ang_jac2.gdot(c_j.ang_jac2); @@ -396,6 +489,7 @@ impl JointVelocityConstraintBuilder { let zero = N::zero(); let lin_jac = self.basis.column(limited_axis).into_owned(); let dist = self.lin_err.dot(&lin_jac); + let min_enabled = dist.simd_lt(limits[0]); let max_enabled = limits[1].simd_lt(dist); @@ -412,11 +506,12 @@ impl JointVelocityConstraintBuilder { let dvel = lin_jac.dot(&(body2.linvel - body1.linvel)) + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); - let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction); + let rhs_wo_bias = dvel; - let erp_inv_dt = params.erp_inv_dt(); - let rhs_bias = ((dist - limits[1]).simd_max(zero) - (limits[0] - dist).simd_max(zero)) - * N::splat(erp_inv_dt); + let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); + let cfm_coeff = N::splat(params.joint_cfm_coeff()); + let rhs_bias = + ((dist - limits[1]).simd_max(zero) - (limits[0] - dist).simd_max(zero)) * erp_inv_dt; ang_jac2 = body2.sqrt_ii * ang_jac2; @@ -429,21 +524,97 @@ impl JointVelocityConstraintBuilder { lin_jac, ang_jac2, inv_lhs: zero, // Will be set during ortogonalization. + cfm_coeff, + cfm_gain: N::zero(), rhs: rhs_wo_bias + rhs_bias, rhs_wo_bias, writeback_id, } } + pub fn limit_linear_coupled_ground( + &self, + params: &IntegrationParameters, + joint_id: [JointIndex; LANES], + body1: &SolverBody, + body2: &SolverBody, + limited_coupled_axes: u8, + limits: &[JointLimits], + writeback_id: WritebackId, + ) -> JointVelocityGroundConstraint { + let zero = N::zero(); + let mut lin_jac = Vector::zeros(); + let mut ang_jac1: AngVector = na::zero(); + let mut ang_jac2: AngVector = na::zero(); + let mut limit = N::zero(); + + for i in 0..DIM { + if limited_coupled_axes & (1 << i) != 0 { + let coeff = self.basis.column(i).dot(&self.lin_err); + lin_jac += self.basis.column(i) * coeff; + #[cfg(feature = "dim2")] + { + ang_jac1 += self.cmat1_basis[i] * coeff; + ang_jac2 += self.cmat2_basis[i] * coeff; + } + #[cfg(feature = "dim3")] + { + ang_jac1 += self.cmat1_basis.column(i) * coeff; + ang_jac2 += self.cmat2_basis.column(i) * coeff; + } + limit += limits[i].max * limits[i].max; + } + } + + limit = limit.simd_sqrt(); + let dist = lin_jac.norm(); + let inv_dist = crate::utils::simd_inv(dist); + lin_jac *= inv_dist; + ang_jac1 *= inv_dist; + ang_jac2 *= inv_dist; + + let dvel = lin_jac.dot(&(body2.linvel - body1.linvel)) + + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); + let rhs_wo_bias = dvel + (dist - limit).simd_min(zero) * N::splat(params.inv_dt()); + + ang_jac2 = body2.sqrt_ii * ang_jac2; + + let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); + let cfm_coeff = N::splat(params.joint_cfm_coeff()); + let rhs_bias = (dist - limit).simd_max(zero) * erp_inv_dt; + let rhs = rhs_wo_bias + rhs_bias; + let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)]; + + JointVelocityGroundConstraint { + joint_id, + mj_lambda2: body2.mj_lambda, + im2: body2.im, + impulse: N::zero(), + impulse_bounds, + lin_jac, + ang_jac2, + inv_lhs: N::zero(), // Will be set during ortogonalization. + cfm_coeff, + cfm_gain: N::zero(), + rhs, + rhs_wo_bias, + writeback_id, + } + } + pub fn motor_linear_ground( &self, + params: &IntegrationParameters, joint_id: [JointIndex; LANES], body1: &SolverBody, body2: &SolverBody, motor_axis: usize, motor_params: &MotorParameters, + limits: Option<[N; 2]>, writeback_id: WritebackId, ) -> JointVelocityGroundConstraint { + let inv_dt = N::splat(params.inv_dt()); + let lin_jac = self.basis.column(motor_axis).into_owned(); let ang_jac1 = self.cmat1_basis.column(motor_axis).into_owned(); #[cfg(feature = "dim2")] @@ -452,16 +623,21 @@ impl JointVelocityConstraintBuilder { let mut ang_jac2 = self.cmat2_basis.column(motor_axis).into_owned(); let mut rhs_wo_bias = N::zero(); - if motor_params.stiffness != N::zero() { + if motor_params.erp_inv_dt != N::zero() { let dist = self.lin_err.dot(&lin_jac); - rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.stiffness; + rhs_wo_bias += (dist - motor_params.target_pos) * motor_params.erp_inv_dt; } - if motor_params.damping != N::zero() { - let dvel = lin_jac.dot(&(body2.linvel - body1.linvel)) - + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); - rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping; - } + let mut target_vel = motor_params.target_vel; + if let Some(limits) = limits { + let dist = self.lin_err.dot(&lin_jac); + target_vel = + target_vel.simd_clamp((limits[0] - dist) * inv_dt, (limits[1] - dist) * inv_dt); + }; + + let dvel = lin_jac.dot(&(body2.linvel - body1.linvel)) + + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); + rhs_wo_bias += dvel - target_vel; ang_jac2 = body2.sqrt_ii * ang_jac2; @@ -474,12 +650,89 @@ impl JointVelocityConstraintBuilder { lin_jac, ang_jac2, inv_lhs: N::zero(), // Will be set during ortogonalization. + cfm_coeff: motor_params.cfm_coeff, + cfm_gain: motor_params.cfm_gain, rhs: rhs_wo_bias, rhs_wo_bias, writeback_id, } } + pub fn motor_linear_coupled_ground( + &self, + params: &IntegrationParameters, + joint_id: [JointIndex; LANES], + body1: &SolverBody, + body2: &SolverBody, + motor_coupled_axes: u8, + motors: &[MotorParameters], + limited_coupled_axes: u8, + limits: &[JointLimits], + writeback_id: WritebackId, + ) -> JointVelocityGroundConstraint { + todo!() + /* + let zero = N::zero(); + let mut lin_jac = Vector::zeros(); + let mut ang_jac1: AngVector = na::zero(); + let mut ang_jac2: AngVector = na::zero(); + let mut limit = N::zero(); + + for i in 0..DIM { + if limited_coupled_axes & (1 << i) != 0 { + let coeff = self.basis.column(i).dot(&self.lin_err); + lin_jac += self.basis.column(i) * coeff; + #[cfg(feature = "dim2")] + { + ang_jac1 += self.cmat1_basis[i] * coeff; + ang_jac2 += self.cmat2_basis[i] * coeff; + } + #[cfg(feature = "dim3")] + { + ang_jac1 += self.cmat1_basis.column(i) * coeff; + ang_jac2 += self.cmat2_basis.column(i) * coeff; + } + limit += limits[i].max * limits[i].max; + } + } + + limit = limit.simd_sqrt(); + let dist = lin_jac.norm(); + let inv_dist = crate::utils::simd_inv(dist); + lin_jac *= inv_dist; + ang_jac1 *= inv_dist; + ang_jac2 *= inv_dist; + + let dvel = lin_jac.dot(&(body2.linvel - body1.linvel)) + + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); + let rhs_wo_bias = dvel + (dist - limit).simd_min(zero) * N::splat(params.inv_dt()); + + ang_jac2 = body2.sqrt_ii * ang_jac2; + + let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); + let cfm_coeff = N::splat(params.joint_cfm_coeff()); + let rhs_bias = (dist - limit).simd_max(zero) * erp_inv_dt; + let rhs = rhs_wo_bias + rhs_bias; + let impulse_bounds = [N::zero(), N::splat(Real::INFINITY)]; + + JointVelocityGroundConstraint { + joint_id, + mj_lambda2: body2.mj_lambda, + im2: body2.im, + impulse: N::zero(), + impulse_bounds, + lin_jac, + ang_jac2, + inv_lhs: N::zero(), // Will be set during ortogonalization. + cfm_coeff, + cfm_gain: N::zero(), + rhs, + rhs_wo_bias, + writeback_id, + } + */ + } + pub fn lock_linear_ground( &self, params: &IntegrationParameters, @@ -498,10 +751,11 @@ impl JointVelocityConstraintBuilder { let dvel = lin_jac.dot(&(body2.linvel - body1.linvel)) + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); - let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction); + let rhs_wo_bias = dvel; - let erp_inv_dt = params.erp_inv_dt(); - let rhs_bias = lin_jac.dot(&self.lin_err) * N::splat(erp_inv_dt); + let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); + let cfm_coeff = N::splat(params.joint_cfm_coeff()); + let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt; ang_jac2 = body2.sqrt_ii * ang_jac2; @@ -514,6 +768,8 @@ impl JointVelocityConstraintBuilder { lin_jac, ang_jac2, inv_lhs: N::zero(), // Will be set during ortogonalization. + cfm_coeff, + cfm_gain: N::zero(), rhs: rhs_wo_bias + rhs_bias, rhs_wo_bias, writeback_id, @@ -536,20 +792,17 @@ impl JointVelocityConstraintBuilder { let ang_jac = self.basis.column(_motor_axis).into_owned(); let mut rhs_wo_bias = N::zero(); - if motor_params.stiffness != N::zero() { + if motor_params.erp_inv_dt != N::zero() { #[cfg(feature = "dim2")] let s_ang_dist = self.ang_err.im; #[cfg(feature = "dim3")] let s_ang_dist = self.ang_err.imag()[_motor_axis]; let s_target_ang = motor_params.target_pos.simd_sin(); - rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.stiffness; + rhs_wo_bias += (s_ang_dist - s_target_ang) * motor_params.erp_inv_dt; } - if motor_params.damping != N::zero() { - let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); - rhs_wo_bias += - (dvel - motor_params.target_vel/* * ang_jac.norm() */) * motor_params.damping; - } + let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); + rhs_wo_bias += dvel - motor_params.target_vel; let ang_jac2 = body2.sqrt_ii * ang_jac; @@ -562,6 +815,8 @@ impl JointVelocityConstraintBuilder { lin_jac: na::zero(), ang_jac2, inv_lhs: N::zero(), // Will be set during ortogonalization. + cfm_coeff: motor_params.cfm_coeff, + cfm_gain: motor_params.cfm_gain, rhs: rhs_wo_bias, rhs_wo_bias, writeback_id, @@ -598,12 +853,13 @@ impl JointVelocityConstraintBuilder { #[cfg(feature = "dim3")] let ang_jac = self.ang_basis.column(limited_axis).into_owned(); let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); - let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction); + let rhs_wo_bias = dvel; - let erp_inv_dt = params.erp_inv_dt(); + let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); + let cfm_coeff = N::splat(params.joint_cfm_coeff()); let rhs_bias = ((s_ang - s_limits[1]).simd_max(zero) - (s_limits[0] - s_ang).simd_max(zero)) - * N::splat(erp_inv_dt); + * erp_inv_dt; let ang_jac2 = body2.sqrt_ii * ang_jac; @@ -616,6 +872,8 @@ impl JointVelocityConstraintBuilder { lin_jac: na::zero(), ang_jac2, inv_lhs: zero, // Will be set during ortogonalization. + cfm_coeff, + cfm_gain: N::zero(), rhs: rhs_wo_bias + rhs_bias, rhs_wo_bias, writeback_id, @@ -636,13 +894,14 @@ impl JointVelocityConstraintBuilder { #[cfg(feature = "dim3")] let ang_jac = self.ang_basis.column(locked_axis).into_owned(); let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); - let rhs_wo_bias = dvel * N::splat(params.velocity_solve_fraction); + let rhs_wo_bias = dvel; - let erp_inv_dt = params.erp_inv_dt(); + let erp_inv_dt = N::splat(params.joint_erp_inv_dt()); + let cfm_coeff = N::splat(params.joint_cfm_coeff()); #[cfg(feature = "dim2")] - let rhs_bias = self.ang_err.im * N::splat(erp_inv_dt); + let rhs_bias = self.ang_err.im * erp_inv_dt; #[cfg(feature = "dim3")] - let rhs_bias = self.ang_err.imag()[locked_axis] * N::splat(erp_inv_dt); + let rhs_bias = self.ang_err.imag()[locked_axis] * erp_inv_dt; let ang_jac2 = body2.sqrt_ii * ang_jac; @@ -655,6 +914,8 @@ impl JointVelocityConstraintBuilder { lin_jac: na::zero(), ang_jac2, inv_lhs: N::zero(), // Will be set during ortogonalization. + cfm_coeff, + cfm_gain: N::zero(), rhs: rhs_wo_bias + rhs_bias, rhs_wo_bias, writeback_id, @@ -666,6 +927,11 @@ impl JointVelocityConstraintBuilder { constraints: &mut [JointVelocityGroundConstraint], ) { let len = constraints.len(); + + if len == 0 { + return; + } + let imsum = constraints[0].im2; // Use the modified Gram-Schmidt orthogonalization. @@ -673,18 +939,23 @@ impl JointVelocityConstraintBuilder { let c_j = &mut constraints[j]; let dot_jj = c_j.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac)) + c_j.ang_jac2.gdot(c_j.ang_jac2); - let inv_dot_jj = crate::utils::simd_inv(dot_jj); + let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain; + let inv_dot_jj = crate::utils::simd_inv(dot_jj + cfm_gain); c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs. + c_j.cfm_gain = cfm_gain; - if c_j.impulse_bounds != [-N::splat(Real::MAX), N::splat(Real::MAX)] { + if c_j.impulse_bounds != [-N::splat(Real::MAX), N::splat(Real::MAX)] + || c_j.cfm_gain != N::zero() + { // Don't remove constraints with limited forces from the others // because they may not deliver the necessary forces to fulfill // the removed parts of other constraints. continue; } - for i in (j + 1)..len { + for i in j + 1..len { let (c_i, c_j) = constraints.index_mut_const(i, j); + let dot_ij = c_i.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac)) + c_i.ang_jac2.gdot(c_j.ang_jac2); let coeff = dot_ij * inv_dot_jj; diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs index 5711518..2ce74b5 100644 --- a/src/dynamics/solver/velocity_constraint.rs +++ b/src/dynamics/solver/velocity_constraint.rs @@ -5,7 +5,7 @@ use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint}; use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity}; use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS}; -use crate::utils::{WAngularInertia, WBasis, WCross, WDot, WReal}; +use crate::utils::{self, WAngularInertia, WBasis, WCross, WDot, WReal}; use super::{DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart}; @@ -239,10 +239,11 @@ impl VelocityConstraint { .transform_vector(dp2.gcross(-force_dir1)); let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass; - let projected_mass = 1.0 - / (force_dir1.dot(&imsum.component_mul(&force_dir1)) + let projected_mass = utils::inv( + force_dir1.dot(&imsum.component_mul(&force_dir1)) + gcross1.gdot(gcross1) - + gcross2.gdot(gcross2)); + + gcross2.gdot(gcross2), + ); let is_bouncy = manifold_point.is_bouncy() as u32 as Real; let is_resting = 1.0 - is_bouncy; @@ -250,7 +251,7 @@ impl VelocityConstraint { let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1); rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt; - rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction; + rhs_wo_bias *= is_bouncy + is_resting; let rhs_bias = /* is_resting * */ erp_inv_dt * (manifold_point.dist + params.allowed_linear_error).min(0.0); @@ -285,8 +286,11 @@ impl VelocityConstraint { constraint.elements[k].tangent_part.gcross1[j] = gcross1; constraint.elements[k].tangent_part.gcross2[j] = gcross2; constraint.elements[k].tangent_part.rhs[j] = rhs; - constraint.elements[k].tangent_part.r[j] = - if cfg!(feature = "dim2") { 1.0 / r } else { r }; + constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") { + utils::inv(r) + } else { + r + }; } #[cfg(feature = "dim3")] diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs index 44b91c6..dbcc433 100644 --- a/src/dynamics/solver/velocity_constraint_wide.rs +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -9,7 +9,7 @@ use crate::math::{ }; #[cfg(feature = "dim2")] use crate::utils::WBasis; -use crate::utils::{WAngularInertia, WCross, WDot}; +use crate::utils::{self, WAngularInertia, WCross, WDot}; use num::Zero; use simba::simd::{SimdPartialOrd, SimdValue}; @@ -47,7 +47,6 @@ impl WVelocityConstraint { } let inv_dt = SimdReal::splat(params.inv_dt()); - let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction); let allowed_lin_err = SimdReal::splat(params.allowed_linear_error); let erp_inv_dt = SimdReal::splat(params.erp_inv_dt()); @@ -135,16 +134,17 @@ impl WVelocityConstraint { let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); let imsum = im1 + im2; - let projected_mass = SimdReal::splat(1.0) - / (force_dir1.dot(&imsum.component_mul(&force_dir1)) + let projected_mass = utils::simd_inv( + force_dir1.dot(&imsum.component_mul(&force_dir1)) + gcross1.gdot(gcross1) - + gcross2.gdot(gcross2)); + + gcross2.gdot(gcross2), + ); let projected_velocity = (vel1 - vel2).dot(&force_dir1); let mut rhs_wo_bias = (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity; rhs_wo_bias += dist.simd_max(SimdReal::zero()) * inv_dt; - rhs_wo_bias *= is_bouncy + is_resting * velocity_solve_fraction; + rhs_wo_bias *= is_bouncy + is_resting; let rhs_bias = (dist + allowed_lin_err).simd_min(SimdReal::zero()) * (erp_inv_dt/* * is_resting */); @@ -174,7 +174,7 @@ impl WVelocityConstraint { constraint.elements[k].tangent_part.gcross2[j] = gcross2; constraint.elements[k].tangent_part.rhs[j] = rhs; constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") { - SimdReal::splat(1.0) / r + utils::simd_inv(r) } else { r }; diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs index cf7d9eb..d09df63 100644 --- a/src/dynamics/solver/velocity_ground_constraint.rs +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -5,7 +5,7 @@ use super::{ use crate::math::{Point, Real, Vector, DIM, MAX_MANIFOLD_POINTS}; #[cfg(feature = "dim2")] use crate::utils::WBasis; -use crate::utils::{WAngularInertia, WCross, WDot}; +use crate::utils::{self, WAngularInertia, WCross, WDot}; use crate::data::{BundleSet, ComponentSet}; use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity}; @@ -153,9 +153,10 @@ impl VelocityGroundConstraint { .effective_world_inv_inertia_sqrt .transform_vector(dp2.gcross(-force_dir1)); - let projected_mass = 1.0 - / (force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)) - + gcross2.gdot(gcross2)); + let projected_mass = utils::inv( + force_dir1.dot(&mprops2.effective_inv_mass.component_mul(&force_dir1)) + + gcross2.gdot(gcross2), + ); let is_bouncy = manifold_point.is_bouncy() as u32 as Real; let is_resting = 1.0 - is_bouncy; @@ -163,7 +164,7 @@ impl VelocityGroundConstraint { let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution) * (vel1 - vel2).dot(&force_dir1); rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt; - rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction; + rhs_wo_bias *= is_bouncy + is_resting; let rhs_bias = /* is_resting * */ erp_inv_dt * (manifold_point.dist + params.allowed_linear_error).min(0.0); @@ -194,8 +195,11 @@ impl VelocityGroundConstraint { constraint.elements[k].tangent_part.gcross2[j] = gcross2; constraint.elements[k].tangent_part.rhs[j] = rhs; - constraint.elements[k].tangent_part.r[j] = - if cfg!(feature = "dim2") { 1.0 / r } else { r }; + constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") { + utils::inv(r) + } else { + r + }; } #[cfg(feature = "dim3")] diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs index 65ac46e..9d6deab 100644 --- a/src/dynamics/solver/velocity_ground_constraint_wide.rs +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -10,7 +10,7 @@ use crate::math::{ }; #[cfg(feature = "dim2")] use crate::utils::WBasis; -use crate::utils::{WAngularInertia, WCross, WDot}; +use crate::utils::{self, WAngularInertia, WCross, WDot}; use num::Zero; use simba::simd::{SimdPartialOrd, SimdValue}; @@ -42,7 +42,6 @@ impl WVelocityGroundConstraint { + ComponentSet, { let inv_dt = SimdReal::splat(params.inv_dt()); - let velocity_solve_fraction = SimdReal::splat(params.velocity_solve_fraction); let allowed_lin_err = SimdReal::splat(params.allowed_linear_error); let erp_inv_dt = SimdReal::splat(params.erp_inv_dt()); @@ -142,14 +141,15 @@ impl WVelocityGroundConstraint { { let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); - let projected_mass = SimdReal::splat(1.0) - / (force_dir1.dot(&im2.component_mul(&force_dir1)) + gcross2.gdot(gcross2)); + let projected_mass = utils::simd_inv( + force_dir1.dot(&im2.component_mul(&force_dir1)) + gcross2.gdot(gcross2), + ); let projected_velocity = (vel1 - vel2).dot(&force_dir1); let mut rhs_wo_bias = (SimdReal::splat(1.0) + is_bouncy * restitution) * projected_velocity; rhs_wo_bias += dist.simd_max(SimdReal::zero()) * inv_dt; - rhs_wo_bias *= is_bouncy + is_resting * velocity_solve_fraction; + rhs_wo_bias *= is_bouncy + is_resting; let rhs_bias = (dist + allowed_lin_err).simd_min(SimdReal::zero()) * (erp_inv_dt/* * is_resting */); @@ -174,7 +174,7 @@ impl WVelocityGroundConstraint { constraint.elements[k].tangent_part.gcross2[j] = gcross2; constraint.elements[k].tangent_part.rhs[j] = rhs; constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") { - SimdReal::splat(1.0) / r + utils::simd_inv(r) } else { r }; diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs index f9088b3..eaca5a6 100644 --- a/src/geometry/collider.rs +++ b/src/geometry/collider.rs @@ -802,3 +802,9 @@ impl ColliderBuilder { ) } } + +impl Into for ColliderBuilder { + fn into(self) -> Collider { + self.build() + } +} diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs index 556d190..61675d4 100644 --- a/src/geometry/collider_set.rs +++ b/src/geometry/collider_set.rs @@ -129,7 +129,8 @@ impl ColliderSet { } /// Inserts a new collider to this set and retrieve its handle. - pub fn insert(&mut self, mut coll: Collider) -> ColliderHandle { + pub fn insert(&mut self, coll: impl Into) -> ColliderHandle { + let mut coll = coll.into(); // Make sure the internal links are reset, they may not be // if this rigid-body was obtained by cloning another one. coll.reset_internal_references(); @@ -142,10 +143,11 @@ impl ColliderSet { /// Inserts a new collider to this set, attach it to the given rigid-body, and retrieve its handle. pub fn insert_with_parent( &mut self, - mut coll: Collider, + coll: impl Into, parent_handle: RigidBodyHandle, bodies: &mut RigidBodySet, ) -> ColliderHandle { + let mut coll = coll.into(); // Make sure the internal links are reset, they may not be // if this collider was obtained by cloning another one. coll.reset_internal_references(); diff --git a/src_testbed/harness/mod.rs b/src_testbed/harness/mod.rs index beebe80..59cccc3 100644 --- a/src_testbed/harness/mod.rs +++ b/src_testbed/harness/mod.rs @@ -16,32 +16,58 @@ pub mod plugin; pub struct RunState { #[cfg(feature = "parallel")] pub thread_pool: rapier::rayon::ThreadPool, - pub num_threads: usize, + #[cfg(feature = "parallel")] + num_threads: usize, pub timestep_id: usize, pub time: f32, } impl RunState { + #[cfg(feature = "parallel")] pub fn new() -> Self { - #[cfg(feature = "parallel")] let num_threads = num_cpus::get_physical(); - #[cfg(not(feature = "parallel"))] - let num_threads = 1; - #[cfg(feature = "parallel")] let thread_pool = rapier::rayon::ThreadPoolBuilder::new() .num_threads(num_threads) .build() .unwrap(); Self { - #[cfg(feature = "parallel")] thread_pool: thread_pool, num_threads, timestep_id: 0, time: 0.0, } } + + #[cfg(not(feature = "parallel"))] + pub fn new() -> Self { + Self { + timestep_id: 0, + time: 0.0, + } + } + + #[cfg(feature = "parallel")] + pub fn num_threads(&self) -> usize { + self.num_threads + } + + #[cfg(not(feature = "parallel"))] + pub fn num_threads(&self) -> usize { + 1 + } + + #[cfg(feature = "parallel")] + pub fn set_num_threads(&mut self, num_threads: usize) { + if self.num_threads != num_threads { + self.thread_pool = rapier::rayon::ThreadPoolBuilder::new() + .num_threads(num_threads) + .build() + .unwrap(); + self.num_threads = num_threads; + } + } } pub struct Harness { diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs index 1a4181b..87a9ca6 100644 --- a/src_testbed/testbed.rs +++ b/src_testbed/testbed.rs @@ -535,7 +535,7 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> { &self.harness.physics.impulse_joints, &self.harness.physics.multibody_joints, self.state.selected_backend == PHYSX_BACKEND_TWO_FRICTION_DIR, - self.harness.state.num_threads, + self.harness.state.num_threads(), )); } } diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs index 788cc8f..bed8f85 100644 --- a/src_testbed/ui.rs +++ b/src_testbed/ui.rs @@ -101,32 +101,32 @@ pub fn update_ui(ui_context: &EguiContext, state: &mut TestbedState, harness: &m || state.selected_backend == PHYSX_BACKEND_TWO_FRICTION_DIR { ui.add( - Slider::new(&mut integration_parameters.max_velocity_iterations, 0..=200) + Slider::new(&mut integration_parameters.max_velocity_iterations, 1..=200) .text("pos. iters."), ); ui.add( Slider::new( &mut integration_parameters.max_stabilization_iterations, - 0..=200, + 1..=200, ) .text("vel. iters."), ); } else { ui.add( - Slider::new(&mut integration_parameters.max_velocity_iterations, 0..=200) + Slider::new(&mut integration_parameters.max_velocity_iterations, 1..=200) .text("vel. rest. iters."), ); ui.add( Slider::new( &mut integration_parameters.max_velocity_friction_iterations, - 0..=200, + 1..=200, ) .text("vel. frict. iters."), ); ui.add( Slider::new( &mut integration_parameters.max_stabilization_iterations, - 0..=200, + 1..=200, ) .text("vel. stab. iters."), ); @@ -134,10 +134,11 @@ pub fn update_ui(ui_context: &EguiContext, state: &mut TestbedState, harness: &m #[cfg(feature = "parallel")] { + let mut num_threads = harness.state.num_threads(); ui.add( - Slider::new(&mut harness.state.num_threads, 1..=num_cpus::get_physical()) - .text("num. threads"), + Slider::new(&mut num_threads, 1..=num_cpus::get_physical()).text("num. threads"), ); + harness.state.set_num_threads(num_threads); } ui.add( Slider::new(&mut integration_parameters.max_ccd_substeps, 0..=10).text("CCD substeps"),