nichkara/rapier
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

Joint API and joint motors improvements

Browse Source
This commit is contained in:
Sébastien Crozet
2022-02-20 12:55:00 +01:00
committed by Sébastien Crozet
parent e740493b98
commit fb20d72ee2
108 changed files with 2650 additions and 1854 deletions
Download Patch File Download Diff File Expand all files Collapse all files

97
.vscode/tasks.json vendored
View File

@@ -98,6 +98,103 @@
"--pause" "--pause"
], ],
"group": "build" "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"
} }
] ]
]
} }

6
benchmarks2d/balls2.rs
View File

@@ -48,11 +48,9 @@ pub fn init_world(testbed: &mut Testbed) {
}; };
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new(status) let rigid_body = RigidBodyBuilder::new(status).translation(vector![x, y]);
.translation(vector![x, y])
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
} }
} }

20
benchmarks2d/boxes2.rs
View File

@@ -15,25 +15,23 @@ pub fn init_world(testbed: &mut Testbed) {
*/ */
let ground_size = 25.0; 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 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); colliders.insert_with_parent(collider, handle, &mut bodies);
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static()
.rotation(std::f32::consts::FRAC_PI_2) .rotation(std::f32::consts::FRAC_PI_2)
.translation(vector![ground_size, ground_size * 2.0]) .translation(vector![ground_size, ground_size * 2.0]);
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static()
.rotation(std::f32::consts::FRAC_PI_2) .rotation(std::f32::consts::FRAC_PI_2)
.translation(vector![-ground_size, ground_size * 2.0]) .translation(vector![-ground_size, ground_size * 2.0]);
.build();
let handle = bodies.insert(rigid_body); 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); 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; let y = j as f32 * shift + centery + 2.0;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]);
.translation(vector![x, y])
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
} }
} }

20
benchmarks2d/capsules2.rs
View File

@@ -15,25 +15,23 @@ pub fn init_world(testbed: &mut Testbed) {
*/ */
let ground_size = 25.0; 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 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); colliders.insert_with_parent(collider, handle, &mut bodies);
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static()
.rotation(std::f32::consts::FRAC_PI_2) .rotation(std::f32::consts::FRAC_PI_2)
.translation(vector![ground_size, ground_size * 4.0]) .translation(vector![ground_size, ground_size * 4.0]);
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static()
.rotation(std::f32::consts::FRAC_PI_2) .rotation(std::f32::consts::FRAC_PI_2)
.translation(vector![-ground_size, ground_size * 4.0]) .translation(vector![-ground_size, ground_size * 4.0]);
.build();
let handle = bodies.insert(rigid_body); 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); 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; let y = j as f32 * shifty + centery + 3.0;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]);
.translation(vector![x, y])
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
} }
} }

20
benchmarks2d/convex_polygons2.rs
View File

@@ -17,25 +17,23 @@ pub fn init_world(testbed: &mut Testbed) {
*/ */
let ground_size = 30.0; 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 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); colliders.insert_with_parent(collider, handle, &mut bodies);
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static()
.rotation(std::f32::consts::FRAC_PI_2) .rotation(std::f32::consts::FRAC_PI_2)
.translation(vector![ground_size, ground_size * 2.0]) .translation(vector![ground_size, ground_size * 2.0]);
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static()
.rotation(std::f32::consts::FRAC_PI_2) .rotation(std::f32::consts::FRAC_PI_2)
.translation(vector![-ground_size, ground_size * 2.0]) .translation(vector![-ground_size, ground_size * 2.0]);
.build();
let handle = bodies.insert(rigid_body); 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); 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 x = i as f32 * shift - centerx;
let y = j as f32 * shift * 2.0 + centery + 2.0; let y = j as f32 * shift * 2.0 + centery + 2.0;
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]);
.translation(vector![x, y])
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
let mut points = Vec::new(); let mut points = Vec::new();
@@ -69,7 +65,7 @@ pub fn init_world(testbed: &mut Testbed) {
points.push(pt * scale); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
} }
} }

12
benchmarks2d/heightfield2.rs
View File

@@ -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 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); 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; let y = j as f32 * shift + centery + 3.0;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]);
.translation(vector![x, y])
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
if j % 2 == 0 { 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); colliders.insert_with_parent(collider, handle, &mut bodies);
} else { } else {
let collider = ColliderBuilder::ball(rad).build(); let collider = ColliderBuilder::ball(rad);
colliders.insert_with_parent(collider, handle, &mut bodies); colliders.insert_with_parent(collider, handle, &mut bodies);
} }
} }

11
benchmarks2d/joint_ball2.rs
View File

@@ -32,17 +32,16 @@ pub fn init_world(testbed: &mut Testbed) {
RigidBodyType::Dynamic RigidBodyType::Dynamic
}; };
let rigid_body = RigidBodyBuilder::new(status) let rigid_body =
.translation(vector![fk * shift, -fi * shift]) RigidBodyBuilder::new(status).translation(vector![fk * shift, -fi * shift]);
.build();
let child_handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, child_handle, &mut bodies);
// Vertical joint. // Vertical joint.
if i > 0 { if i > 0 {
let parent_handle = *body_handles.last().unwrap(); 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); impulse_joints.insert(parent_handle, child_handle, joint);
} }
@@ -50,7 +49,7 @@ pub fn init_world(testbed: &mut Testbed) {
if k > 0 { if k > 0 {
let parent_index = body_handles.len() - numi; let parent_index = body_handles.len() - numi;
let parent_handle = body_handles[parent_index]; 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); impulse_joints.insert(parent_handle, child_handle, joint);
} }

13
benchmarks2d/joint_fixed2.rs
View File

@@ -38,17 +38,16 @@ pub fn init_world(testbed: &mut Testbed) {
}; };
let rigid_body = RigidBodyBuilder::new(status) let rigid_body = RigidBodyBuilder::new(status)
.translation(vector![x + fk * shift, y - fi * shift]) .translation(vector![x + fk * shift, y - fi * shift]);
.build();
let child_handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, child_handle, &mut bodies);
// Vertical joint. // Vertical joint.
if i > 0 { if i > 0 {
let parent_handle = *body_handles.last().unwrap(); let parent_handle = *body_handles.last().unwrap();
let joint = let joint = FixedJointBuilder::new()
FixedJoint::new().local_frame2(Isometry::translation(0.0, shift)); .local_frame2(Isometry::translation(0.0, shift));
impulse_joints.insert(parent_handle, child_handle, joint); impulse_joints.insert(parent_handle, child_handle, joint);
} }
@@ -56,8 +55,8 @@ pub fn init_world(testbed: &mut Testbed) {
if k > 0 { if k > 0 {
let parent_index = body_handles.len() - num; let parent_index = body_handles.len() - num;
let parent_handle = body_handles[parent_index]; let parent_handle = body_handles[parent_index];
let joint = let joint = FixedJointBuilder::new()
FixedJoint::new().local_frame2(Isometry::translation(-shift, 0.0)); .local_frame2(Isometry::translation(-shift, 0.0));
impulse_joints.insert(parent_handle, child_handle, joint); impulse_joints.insert(parent_handle, child_handle, joint);
} }

16
benchmarks2d/joint_prismatic2.rs
View File

@@ -24,21 +24,17 @@ pub fn init_world(testbed: &mut Testbed) {
for j in 0..50 { for j in 0..50 {
let x = j as f32 * shift * 4.0; let x = j as f32 * shift * 4.0;
let ground = RigidBodyBuilder::new_static() let ground = RigidBodyBuilder::new_static().translation(vector![x, y]);
.translation(vector![x, y])
.build();
let mut curr_parent = bodies.insert(ground); 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); colliders.insert_with_parent(collider, curr_parent, &mut bodies);
for i in 0..num { for i in 0..num {
let y = y - (i + 1) as f32 * shift; let y = y - (i + 1) as f32 * shift;
let density = 1.0; let density = 1.0;
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]);
.translation(vector![x, y])
.build();
let curr_child = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, curr_child, &mut bodies);
let axis = if i % 2 == 0 { 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]) UnitVector::new_normalize(vector![-1.0, 1.0])
}; };
let prism = PrismaticJoint::new(axis) let prism = PrismaticJointBuilder::new(axis)
.local_anchor2(point![0.0, shift]) .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); impulse_joints.insert(curr_parent, curr_child, prism);
curr_parent = curr_child; curr_parent = curr_child;

10
benchmarks2d/pyramid2.rs
View File

@@ -16,9 +16,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 100.0; let ground_size = 100.0;
let ground_thickness = 1.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 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); 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; let y = fi * shift + centery;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]);
.translation(vector![x, y])
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
} }
} }

6
benchmarks3d/balls3.rs
View File

@@ -36,11 +36,9 @@ pub fn init_world(testbed: &mut Testbed) {
let density = 0.477; let density = 0.477;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new(status) let rigid_body = RigidBodyBuilder::new(status).translation(vector![x, y, z]);
.translation(vector![x, y, z])
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
} }
} }

12
benchmarks3d/boxes3.rs
View File

@@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 200.1; let ground_size = 200.1;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let handle = bodies.insert(rigid_body); 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); 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; let z = k as f32 * shift - centerz + offset;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
.translation(vector![x, y, z])
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
} }
} }

12
benchmarks3d/capsules3.rs
View File

@@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 200.1; let ground_size = 200.1;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let handle = bodies.insert(rigid_body); 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); 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; let z = k as f32 * shift - centerz + offset;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
.translation(vector![x, y, z])
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
} }
} }

11
benchmarks3d/ccd3.rs
View File

@@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 100.1; let ground_size = 100.1;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let handle = bodies.insert(rigid_body); 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); 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() let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![x, y, z]) .translation(vector![x, y, z])
.linvel(vector![0.0, -1000.0, 0.0]) .linvel(vector![0.0, -1000.0, 0.0])
.ccd_enabled(true) .ccd_enabled(true);
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
let collider = match j % 5 { let collider = match j % 5 {
@@ -64,7 +61,7 @@ pub fn init_world(testbed: &mut Testbed) {
_ => ColliderBuilder::capsule_y(rad, rad), _ => ColliderBuilder::capsule_y(rad, rad),
}; };
colliders.insert_with_parent(collider.build(), handle, &mut bodies); colliders.insert_with_parent(collider, handle, &mut bodies);
} }
} }

28
benchmarks3d/compound3.rs
View File

@@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 200.1; let ground_size = 200.1;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let handle = bodies.insert(rigid_body); 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); 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; let z = k as f32 * shift * 2.0 - centerz + offset;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
.translation(vector![x, y, z])
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
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) let collider2 = ColliderBuilder::cuboid(rad, rad * 10.0, rad).translation(vector![
.translation(vector![rad * 10.0, rad * 10.0, 0.0]) rad * 10.0,
.build(); rad * 10.0,
let collider3 = ColliderBuilder::cuboid(rad, rad * 10.0, rad) 0.0
.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
]);
colliders.insert_with_parent(collider1, handle, &mut bodies); colliders.insert_with_parent(collider1, handle, &mut bodies);
colliders.insert_with_parent(collider2, handle, &mut bodies); colliders.insert_with_parent(collider2, handle, &mut bodies);
colliders.insert_with_parent(collider3, handle, &mut bodies); colliders.insert_with_parent(collider3, handle, &mut bodies);

14
benchmarks3d/convex_polyhedron3.rs
View File

@@ -18,11 +18,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 200.1; let ground_size = 200.1;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
/* /*
@@ -57,13 +55,9 @@ pub fn init_world(testbed: &mut Testbed) {
} }
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
.translation(vector![x, y, z])
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::round_convex_hull(&points, border_rad) let collider = ColliderBuilder::round_convex_hull(&points, border_rad).unwrap();
.unwrap()
.build();
colliders.insert_with_parent(collider, handle, &mut bodies); colliders.insert_with_parent(collider, handle, &mut bodies);
} }
} }

12
benchmarks3d/heightfield3.rs
View File

@@ -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 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); 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; let z = k as f32 * shift - centerz;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
.translation(vector![x, y, z])
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
if j % 2 == 0 { 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); colliders.insert_with_parent(collider, handle, &mut bodies);
} else { } else {
let collider = ColliderBuilder::ball(rad).build(); let collider = ColliderBuilder::ball(rad);
colliders.insert_with_parent(collider, handle, &mut bodies); colliders.insert_with_parent(collider, handle, &mut bodies);
} }
} }

11
benchmarks3d/joint_ball3.rs
View File

@@ -27,17 +27,16 @@ pub fn init_world(testbed: &mut Testbed) {
RigidBodyType::Dynamic RigidBodyType::Dynamic
}; };
let rigid_body = RigidBodyBuilder::new(status) let rigid_body =
.translation(vector![fk * shift, 0.0, fi * shift]) RigidBodyBuilder::new(status).translation(vector![fk * shift, 0.0, fi * shift]);
.build();
let child_handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, child_handle, &mut bodies);
// Vertical joint. // Vertical joint.
if i > 0 { if i > 0 {
let parent_handle = *body_handles.last().unwrap(); 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); impulse_joints.insert(parent_handle, child_handle, joint);
} }
@@ -45,7 +44,7 @@ pub fn init_world(testbed: &mut Testbed) {
if k > 0 { if k > 0 {
let parent_index = body_handles.len() - num; let parent_index = body_handles.len() - num;
let parent_handle = body_handles[parent_index]; 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); impulse_joints.insert(parent_handle, child_handle, joint);
} }

16
benchmarks3d/joint_fixed3.rs
View File

@@ -40,17 +40,20 @@ pub fn init_world(testbed: &mut Testbed) {
RigidBodyType::Dynamic RigidBodyType::Dynamic
}; };
let rigid_body = RigidBodyBuilder::new(status) let rigid_body = RigidBodyBuilder::new(status).translation(vector![
.translation(vector![x + fk * shift, y, z + fi * shift]) x + fk * shift,
.build(); y,
z + fi * shift
]);
let child_handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, child_handle, &mut bodies);
// Vertical joint. // Vertical joint.
if i > 0 { if i > 0 {
let parent_handle = *body_handles.last().unwrap(); 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); impulse_joints.insert(parent_handle, child_handle, joint);
} }
@@ -58,7 +61,8 @@ pub fn init_world(testbed: &mut Testbed) {
if k > 0 { if k > 0 {
let parent_index = body_handles.len() - num; let parent_index = body_handles.len() - num;
let parent_handle = body_handles[parent_index]; 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); impulse_joints.insert(parent_handle, child_handle, joint);
} }

18
benchmarks3d/joint_prismatic3.rs
View File

@@ -23,23 +23,17 @@ pub fn init_world(testbed: &mut Testbed) {
for j in 0..50 { for j in 0..50 {
let x = j as f32 * shift * 4.0; let x = j as f32 * shift * 4.0;
let ground = RigidBodyBuilder::new_static() let ground = RigidBodyBuilder::new_static().translation(vector![x, y, z]);
.translation(vector![x, y, z])
.build();
let mut curr_parent = bodies.insert(ground); 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); colliders.insert_with_parent(collider, curr_parent, &mut bodies);
for i in 0..num { for i in 0..num {
let z = z + (i + 1) as f32 * shift; let z = z + (i + 1) as f32 * shift;
let density = 1.0; let density = 1.0;
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
.translation(vector![x, y, z])
.build();
let curr_child = bodies.insert(rigid_body); let curr_child = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad, rad, rad) let collider = ColliderBuilder::cuboid(rad, rad, rad).density(density);
.density(density)
.build();
colliders.insert_with_parent(collider, curr_child, &mut bodies); colliders.insert_with_parent(collider, curr_child, &mut bodies);
let axis = if i % 2 == 0 { 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]) 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]) .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); impulse_joints.insert(curr_parent, curr_child, prism);
curr_parent = curr_child; curr_parent = curr_child;

22
benchmarks3d/joint_revolute3.rs
View File

@@ -20,11 +20,9 @@ pub fn init_world(testbed: &mut Testbed) {
for j in 0..50 { for j in 0..50 {
let x = j as f32 * shift * 4.0; let x = j as f32 * shift * 4.0;
let ground = RigidBodyBuilder::new_static() let ground = RigidBodyBuilder::new_static().translation(vector![x, y, 0.0]);
.translation(vector![x, y, 0.0])
.build();
let mut curr_parent = bodies.insert(ground); 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); colliders.insert_with_parent(collider, curr_parent, &mut bodies);
for i in 0..num { for i in 0..num {
@@ -40,13 +38,9 @@ pub fn init_world(testbed: &mut Testbed) {
let mut handles = [curr_parent; 4]; let mut handles = [curr_parent; 4];
for k in 0..4 { for k in 0..4 {
let density = 1.0; let density = 1.0;
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().position(positions[k]);
.position(positions[k])
.build();
handles[k] = bodies.insert(rigid_body); handles[k] = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad, rad, rad) let collider = ColliderBuilder::cuboid(rad, rad, rad).density(density);
.density(density)
.build();
colliders.insert_with_parent(collider, handles[k], &mut bodies); 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 z = Vector::z_axis();
let revs = [ let revs = [
RevoluteJoint::new(z).local_anchor2(point![0.0, 0.0, -shift]), RevoluteJointBuilder::new(z).local_anchor2(point![0.0, 0.0, -shift]),
RevoluteJoint::new(x).local_anchor2(point![-shift, 0.0, 0.0]), RevoluteJointBuilder::new(x).local_anchor2(point![-shift, 0.0, 0.0]),
RevoluteJoint::new(z).local_anchor2(point![0.0, 0.0, -shift]), RevoluteJointBuilder::new(z).local_anchor2(point![0.0, 0.0, -shift]),
RevoluteJoint::new(x).local_anchor2(point![shift, 0.0, 0.0]), RevoluteJointBuilder::new(x).local_anchor2(point![shift, 0.0, 0.0]),
]; ];
impulse_joints.insert(curr_parent, handles[0], revs[0]); impulse_joints.insert(curr_parent, handles[0], revs[0]);

22
benchmarks3d/keva3.rs
View File

@@ -38,15 +38,13 @@ pub fn build_block(
}; };
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![
.translation(vector![
x + dim.x + shift.x, x + dim.x + shift.x,
y + dim.y + shift.y, y + dim.y + shift.y,
z + dim.z + shift.z z + dim.z + shift.z
]) ]);
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, bodies);
testbed.set_initial_body_color(handle, color0); 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 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 { for j in 0..(block_width / (dim.z as f32 * 2.0)) as usize {
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![
.translation(vector![
i as f32 * dim.x * 2.0 + dim.x + shift.x, i as f32 * dim.x * 2.0 + dim.x + shift.x,
dim.y + shift.y + block_height, dim.y + shift.y + block_height,
j as f32 * dim.z * 2.0 + dim.z + shift.z j as f32 * dim.z * 2.0 + dim.z + shift.z
]) ]);
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, bodies);
testbed.set_initial_body_color(handle, color0); testbed.set_initial_body_color(handle, color0);
std::mem::swap(&mut color0, &mut color1); 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_size = 50.0;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
/* /*

12
benchmarks3d/pyramid3.rs
View File

@@ -22,13 +22,11 @@ fn create_pyramid(
- stack_height as f32 * half_extents.z; - stack_height as f32 * half_extents.z;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
.translation(vector![x, y, z])
.build();
let rigid_body_handle = bodies.insert(rigid_body); let rigid_body_handle = bodies.insert(rigid_body);
let collider = 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); 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_size = 50.0;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let ground_handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, ground_handle, &mut bodies);
/* /*

24
benchmarks3d/stacks3.rs
View File

@@ -23,10 +23,9 @@ fn create_tower_circle(
* Translation::new(0.0, y, radius); * Translation::new(0.0, y, radius);
// Build the rigid body. // 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 handle = bodies.insert(rigid_body);
let collider = let collider = ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z);
ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z).build();
colliders.insert_with_parent(collider, handle, bodies); colliders.insert_with_parent(collider, handle, bodies);
} }
} }
@@ -50,12 +49,9 @@ fn create_wall(
- stack_height as f32 * half_extents.z; - stack_height as f32 * half_extents.z;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
.translation(vector![x, y, z])
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
let collider = let collider = ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z);
ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z).build();
colliders.insert_with_parent(collider, handle, bodies); colliders.insert_with_parent(collider, handle, bodies);
} }
} }
@@ -83,12 +79,10 @@ fn create_pyramid(
- stack_height as f32 * half_extents.z; - stack_height as f32 * half_extents.z;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
.translation(vector![x, y, z])
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
let collider = 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); 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_size = 200.0;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
/* /*

12
benchmarks3d/trimesh3.rs
View File

@@ -36,9 +36,9 @@ pub fn init_world(testbed: &mut Testbed) {
let heightfield = HeightField::new(heights, ground_size); let heightfield = HeightField::new(heights, ground_size);
let (vertices, indices) = heightfield.to_trimesh(); 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 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); 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; let z = k as f32 * shift - centerz;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
.translation(vector![x, y, z])
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
if j % 2 == 0 { 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); colliders.insert_with_parent(collider, handle, &mut bodies);
} else { } else {
let collider = ColliderBuilder::ball(rad).build(); let collider = ColliderBuilder::ball(rad);
colliders.insert_with_parent(collider, handle, &mut bodies); colliders.insert_with_parent(collider, handle, &mut bodies);
} }
} }

12
examples2d/add_remove2.rs
View File

@@ -14,11 +14,9 @@ pub fn init_world(testbed: &mut Testbed) {
let platform_handles = positions let platform_handles = positions
.into_iter() .into_iter()
.map(|pos| { .map(|pos| {
let rigid_body = RigidBodyBuilder::new_kinematic_position_based() let rigid_body = RigidBodyBuilder::new_kinematic_position_based().translation(pos);
.translation(pos)
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
handle handle
}) })
@@ -35,11 +33,9 @@ pub fn init_world(testbed: &mut Testbed) {
if state.timestep_id % 10 == 0 { if state.timestep_id % 10 == 0 {
let x = rand::random::<f32>() * 10.0 - 5.0; let x = rand::random::<f32>() * 10.0 - 5.0;
let y = rand::random::<f32>() * 10.0 + 10.0; let y = rand::random::<f32>() * 10.0 + 10.0;
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]);
.translation(vector![x, y])
.build();
let handle = physics.bodies.insert(rigid_body); let handle = physics.bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad, rad).build(); let collider = ColliderBuilder::cuboid(rad, rad);
physics physics
.colliders .colliders
.insert_with_parent(collider, handle, &mut physics.bodies); .insert_with_parent(collider, handle, &mut physics.bodies);

27
examples2d/ccd2.rs
View File

@@ -16,27 +16,24 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 25.0; let ground_size = 25.0;
let ground_thickness = 0.1; 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 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); colliders.insert_with_parent(collider, ground_handle, &mut bodies);
let collider = ColliderBuilder::cuboid(ground_thickness, ground_size) let collider =
.translation(vector![-3.0, 0.0]) ColliderBuilder::cuboid(ground_thickness, ground_size).translation(vector![-3.0, 0.0]);
.build();
colliders.insert_with_parent(collider, ground_handle, &mut bodies); colliders.insert_with_parent(collider, ground_handle, &mut bodies);
let collider = ColliderBuilder::cuboid(ground_thickness, ground_size) let collider =
.translation(vector![6.0, 0.0]) ColliderBuilder::cuboid(ground_thickness, ground_size).translation(vector![6.0, 0.0]);
.build();
colliders.insert_with_parent(collider, ground_handle, &mut bodies); colliders.insert_with_parent(collider, ground_handle, &mut bodies);
let collider = ColliderBuilder::cuboid(ground_thickness, ground_size) let collider = ColliderBuilder::cuboid(ground_thickness, ground_size)
.translation(vector![2.5, 0.0]) .translation(vector![2.5, 0.0])
.sensor(true) .sensor(true)
.active_events(ActiveEvents::INTERSECTION_EVENTS) .active_events(ActiveEvents::INTERSECTION_EVENTS);
.build();
let sensor_handle = colliders.insert_with_parent(collider, ground_handle, &mut bodies); 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() let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![x, y]) .translation(vector![x, y])
.linvel(vector![100.0, -10.0]) .linvel(vector![100.0, -10.0])
.ccd_enabled(true) .ccd_enabled(true);
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
// for part in &compound_parts { // for part in &compound_parts {
// let collider = ColliderBuilder::new(part.1.clone()) // let collider = ColliderBuilder::new(part.1.clone())
// .position_wrt_parent(part.0) // .position_wrt_parent(part.0)
// .build(); // ;
// colliders.insert_with_parent(collider, handle, &mut bodies); // colliders.insert_with_parent(collider, handle, &mut bodies);
// } // }
let collider = ColliderBuilder::new(compound_shape.clone()).build(); let collider = ColliderBuilder::new(compound_shape.clone());
// let collider = ColliderBuilder::cuboid(radx, rady).build(); // let collider = ColliderBuilder::cuboid(radx, rady);
colliders.insert_with_parent(collider, handle, &mut bodies); 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 { if parent_handle1 != ground_handle && prox.collider1 != sensor_handle {
graphics.set_body_color(parent_handle1, color); graphics.set_body_color(parent_handle1, color);
} }
if parent_handle2 != ground_handle && prox.collider2 != sensor_handle { if parent_handle2 != ground_handle && prox.collider2 != sensor_handle {
graphics.set_body_color(parent_handle2, color); graphics.set_body_color(parent_handle2, color);
} }

20
examples2d/collision_groups2.rs
View File

@@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 5.0; let ground_size = 5.0;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height]);
.translation(vector![0.0, -ground_height])
.build();
let floor_handle = bodies.insert(rigid_body); 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); 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) let green_floor = ColliderBuilder::cuboid(1.0, 0.1)
.translation(vector![0.0, 1.0]) .translation(vector![0.0, 1.0])
.collision_groups(GREEN_GROUP) .collision_groups(GREEN_GROUP);
.build();
let green_collider_handle = let green_collider_handle =
colliders.insert_with_parent(green_floor, floor_handle, &mut bodies); 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) let blue_floor = ColliderBuilder::cuboid(1.0, 0.1)
.translation(vector![0.0, 2.0]) .translation(vector![0.0, 2.0])
.collision_groups(BLUE_GROUP) .collision_groups(BLUE_GROUP);
.build();
let blue_collider_handle = colliders.insert_with_parent(blue_floor, floor_handle, &mut bodies); 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]); 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]) (BLUE_GROUP, [0.0, 0.0, 1.0])
}; };
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]);
.translation(vector![x, y])
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad, rad) let collider = ColliderBuilder::cuboid(rad, rad).collision_groups(group);
.collision_groups(group)
.build();
colliders.insert_with_parent(collider, handle, &mut bodies); colliders.insert_with_parent(collider, handle, &mut bodies);
testbed.set_initial_body_color(handle, color); testbed.set_initial_body_color(handle, color);

20
examples2d/convex_polygons2.rs
View File

@@ -17,25 +17,23 @@ pub fn init_world(testbed: &mut Testbed) {
*/ */
let ground_size = 30.0; 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 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); colliders.insert_with_parent(collider, handle, &mut bodies);
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static()
.rotation(std::f32::consts::FRAC_PI_2) .rotation(std::f32::consts::FRAC_PI_2)
.translation(vector![ground_size, ground_size * 2.0]) .translation(vector![ground_size, ground_size * 2.0]);
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static()
.rotation(std::f32::consts::FRAC_PI_2) .rotation(std::f32::consts::FRAC_PI_2)
.translation(vector![-ground_size, ground_size * 2.0]) .translation(vector![-ground_size, ground_size * 2.0]);
.build();
let handle = bodies.insert(rigid_body); 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); 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 x = i as f32 * shift - centerx;
let y = j as f32 * shift * 2.0 + centery + 2.0; let y = j as f32 * shift * 2.0 + centery + 2.0;
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]);
.translation(vector![x, y])
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
let mut points = Vec::new(); let mut points = Vec::new();
@@ -69,7 +65,7 @@ pub fn init_world(testbed: &mut Testbed) {
points.push(pt * scale); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
} }
} }

5
examples2d/damping2.rs
View File

@@ -27,12 +27,11 @@ pub fn init_world(testbed: &mut Testbed) {
.linvel(vector![x * 10.0, y * 10.0]) .linvel(vector![x * 10.0, y * 10.0])
.angvel(100.0) .angvel(100.0)
.linear_damping((i + 1) as f32 * subdiv * 10.0) .linear_damping((i + 1) as f32 * subdiv * 10.0)
.angular_damping((num - i) as f32 * subdiv * 10.0) .angular_damping((num - i) as f32 * subdiv * 10.0);
.build();
let rb_handle = bodies.insert(rb); let rb_handle = bodies.insert(rb);
// Build the collider. // 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); colliders.insert_with_parent(co, rb_handle, &mut bodies);
} }

10
examples2d/debug_box_ball2.rs
View File

@@ -16,19 +16,17 @@ pub fn init_world(testbed: &mut Testbed) {
let rad = 1.0; let rad = 1.0;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static()
.translation(vector![0.0, -rad]) .translation(vector![0.0, -rad])
.rotation(std::f32::consts::PI / 4.0) .rotation(std::f32::consts::PI / 4.0);
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
// Build the dynamic box rigid body. // Build the dynamic box rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![0.0, 3.0 * rad]) .translation(vector![0.0, 3.0 * rad])
.can_sleep(false) .can_sleep(false);
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
/* /*

12
examples2d/drum2.rs
View File

@@ -26,11 +26,9 @@ pub fn init_world(testbed: &mut Testbed) {
let y = j as f32 * shift - centery; let y = j as f32 * shift - centery;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]);
.translation(vector![x, y])
.build();
let handle = bodies.insert(rigid_body); 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); 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. * 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 velocity_based_platform_handle = bodies.insert(platform_body);
let sides = [ let sides = [
@@ -55,11 +53,11 @@ pub fn init_world(testbed: &mut Testbed) {
]; ];
for (hx, hy, pos) in sides { 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); colliders.insert_with_parent(collider, velocity_based_platform_handle, &mut bodies);
} }
for (r, pos) in balls { 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); colliders.insert_with_parent(collider, velocity_based_platform_handle, &mut bodies);
} }

12
examples2d/heightfield2.rs
View File

@@ -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 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); 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; let y = j as f32 * shift + centery + 3.0;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]);
.translation(vector![x, y])
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
if j % 2 == 0 { 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); colliders.insert_with_parent(collider, handle, &mut bodies);
} else { } else {
let collider = ColliderBuilder::ball(rad).build(); let collider = ColliderBuilder::ball(rad);
colliders.insert_with_parent(collider, handle, &mut bodies); colliders.insert_with_parent(collider, handle, &mut bodies);
} }
} }

11
examples2d/joints2.rs
View File

@@ -35,17 +35,16 @@ pub fn init_world(testbed: &mut Testbed) {
RigidBodyType::Dynamic RigidBodyType::Dynamic
}; };
let rigid_body = RigidBodyBuilder::new(status) let rigid_body =
.translation(vector![fk * shift, -fi * shift]) RigidBodyBuilder::new(status).translation(vector![fk * shift, -fi * shift]);
.build();
let child_handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, child_handle, &mut bodies);
// Vertical joint. // Vertical joint.
if i > 0 { if i > 0 {
let parent_handle = *body_handles.last().unwrap(); 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); impulse_joints.insert(parent_handle, child_handle, joint);
} }
@@ -53,7 +52,7 @@ pub fn init_world(testbed: &mut Testbed) {
if k > 0 { if k > 0 {
let parent_index = body_handles.len() - numi; let parent_index = body_handles.len() - numi;
let parent_handle = body_handles[parent_index]; 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); impulse_joints.insert(parent_handle, child_handle, joint);
} }

16
examples2d/locked_rotations2.rs
View File

@@ -19,11 +19,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 5.0; let ground_size = 5.0;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height]);
.translation(vector![0.0, -ground_height])
.build();
let handle = bodies.insert(rigid_body); 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); 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() let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![0.0, 3.0]) .translation(vector![0.0, 3.0])
.lock_translations() .lock_translations();
.build();
let handle = bodies.insert(rigid_body); 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); 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() let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![0.0, 5.0]) .translation(vector![0.0, 5.0])
.rotation(1.0) .rotation(1.0)
.lock_rotations() .lock_rotations();
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
/* /*

14
examples2d/one_way_platforms2.rs
View File

@@ -68,18 +68,16 @@ pub fn init_world(testbed: &mut Testbed) {
/* /*
* Ground * Ground
*/ */
let rigid_body = RigidBodyBuilder::new_static().build(); let rigid_body = RigidBodyBuilder::new_static();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(25.0, 0.5) let collider = ColliderBuilder::cuboid(25.0, 0.5)
.translation(vector![30.0, 2.0]) .translation(vector![30.0, 2.0])
.active_hooks(ActiveHooks::MODIFY_SOLVER_CONTACTS) .active_hooks(ActiveHooks::MODIFY_SOLVER_CONTACTS);
.build();
let platform1 = colliders.insert_with_parent(collider, handle, &mut bodies); let platform1 = colliders.insert_with_parent(collider, handle, &mut bodies);
let collider = ColliderBuilder::cuboid(25.0, 0.5) let collider = ColliderBuilder::cuboid(25.0, 0.5)
.translation(vector![-30.0, -2.0]) .translation(vector![-30.0, -2.0])
.active_hooks(ActiveHooks::MODIFY_SOLVER_CONTACTS) .active_hooks(ActiveHooks::MODIFY_SOLVER_CONTACTS);
.build();
let platform2 = colliders.insert_with_parent(collider, handle, &mut bodies); 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| { testbed.add_callback(move |graphics, physics, _, run_state| {
if run_state.timestep_id % 50 == 0 && physics.bodies.len() <= 7 { if run_state.timestep_id % 50 == 0 && physics.bodies.len() <= 7 {
// Spawn a new cube. // Spawn a new cube.
let collider = ColliderBuilder::cuboid(1.5, 2.0).build(); let collider = ColliderBuilder::cuboid(1.5, 2.0);
let body = RigidBodyBuilder::new_dynamic() let body = RigidBodyBuilder::new_dynamic().translation(vector![20.0, 10.0]);
.translation(vector![20.0, 10.0])
.build();
let handle = physics.bodies.insert(body); let handle = physics.bodies.insert(body);
physics physics
.colliders .colliders

22
examples2d/platform2.rs
View File

@@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 10.0; let ground_size = 10.0;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height]);
.translation(vector![0.0, -ground_height])
.build();
let handle = bodies.insert(rigid_body); 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); 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; let y = j as f32 * shift + centery;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]);
.translation(vector![x, y])
.build();
let handle = bodies.insert(rigid_body); 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); 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. * Setup a position-based kinematic rigid body.
*/ */
let platform_body = RigidBodyBuilder::new_kinematic_velocity_based() let platform_body = RigidBodyBuilder::new_kinematic_velocity_based()
.translation(vector![-10.0 * rad, 1.5 + 0.8]) .translation(vector![-10.0 * rad, 1.5 + 0.8]);
.build();
let velocity_based_platform_handle = bodies.insert(platform_body); 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); colliders.insert_with_parent(collider, velocity_based_platform_handle, &mut bodies);
/* /*
* Setup a velocity-based kinematic rigid body. * Setup a velocity-based kinematic rigid body.
*/ */
let platform_body = RigidBodyBuilder::new_kinematic_position_based() let platform_body = RigidBodyBuilder::new_kinematic_position_based()
.translation(vector![-10.0 * rad, 2.0 + 1.5 + 0.8]) .translation(vector![-10.0 * rad, 2.0 + 1.5 + 0.8]);
.build();
let position_based_platform_handle = bodies.insert(platform_body); 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); colliders.insert_with_parent(collider, position_based_platform_handle, &mut bodies);
/* /*

12
examples2d/polyline2.rs
View File

@@ -27,9 +27,9 @@ pub fn init_world(testbed: &mut Testbed) {
} }
points.push(point![ground_size / 2.0, 40.0]); 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 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); 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; let y = j as f32 * shift + centery + 3.0;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]);
.translation(vector![x, y])
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
if j % 2 == 0 { 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); colliders.insert_with_parent(collider, handle, &mut bodies);
} else { } else {
let collider = ColliderBuilder::ball(rad).build(); let collider = ColliderBuilder::ball(rad);
colliders.insert_with_parent(collider, handle, &mut bodies); colliders.insert_with_parent(collider, handle, &mut bodies);
} }
} }

10
examples2d/pyramid2.rs
View File

@@ -16,9 +16,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 10.0; let ground_size = 10.0;
let ground_thickness = 1.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 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); 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; let y = fi * shift + centery;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]);
.translation(vector![x, y])
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
} }
} }

15
examples2d/restitution2.rs
View File

@@ -16,13 +16,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 20.; let ground_size = 20.;
let ground_height = 1.0; let ground_height = 1.0;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height]);
.translation(vector![0.0, -ground_height])
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height) let collider = ColliderBuilder::cuboid(ground_size, ground_height).restitution(1.0);
.restitution(1.0)
.build();
colliders.insert_with_parent(collider, handle, &mut bodies); colliders.insert_with_parent(collider, handle, &mut bodies);
let num = 10; let num = 10;
@@ -32,12 +28,9 @@ pub fn init_world(testbed: &mut Testbed) {
for i in 0..=num { for i in 0..=num {
let x = (i as f32) - num as f32 / 2.0; let x = (i as f32) - num as f32 / 2.0;
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![x * 2.0, 10.0 * (j as f32 + 1.0)]) .translation(vector![x * 2.0, 10.0 * (j as f32 + 1.0)]);
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::ball(rad) let collider = ColliderBuilder::ball(rad).restitution((i as f32) / (num as f32));
.restitution((i as f32) / (num as f32))
.build();
colliders.insert_with_parent(collider, handle, &mut bodies); colliders.insert_with_parent(collider, handle, &mut bodies);
} }
} }

21
examples2d/sensor2.rs
View File

@@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 200.1; let ground_size = 200.1;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height]);
.translation(vector![0.0, -ground_height])
.build();
let ground_handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, ground_handle, &mut bodies);
/* /*
@@ -37,11 +35,9 @@ pub fn init_world(testbed: &mut Testbed) {
let y = 3.0; let y = 3.0;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]);
.translation(vector![x, y])
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
testbed.set_initial_body_color(handle, [0.5, 0.5, 1.0]); 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. // Rigid body so that the sensor can move.
let sensor = RigidBodyBuilder::new_dynamic() let sensor = RigidBodyBuilder::new_dynamic().translation(vector![0.0, 10.0]);
.translation(vector![0.0, 10.0])
.build();
let sensor_handle = bodies.insert(sensor); let sensor_handle = bodies.insert(sensor);
// Solid cube attached to the sensor which // Solid cube attached to the sensor which
// other colliders can touch. // 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); colliders.insert_with_parent(collider, sensor_handle, &mut bodies);
// We create a collider desc without density because we don't // 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) let sensor_collider = ColliderBuilder::ball(rad * 5.0)
.density(0.0) .density(0.0)
.sensor(true) .sensor(true)
.active_events(ActiveEvents::INTERSECTION_EVENTS) .active_events(ActiveEvents::INTERSECTION_EVENTS);
.build();
colliders.insert_with_parent(sensor_collider, sensor_handle, &mut bodies); colliders.insert_with_parent(sensor_collider, sensor_handle, &mut bodies);
testbed.set_initial_body_color(sensor_handle, [0.5, 1.0, 1.0]); testbed.set_initial_body_color(sensor_handle, [0.5, 1.0, 1.0]);

20
examples2d/trimesh2.rs
View File

@@ -21,25 +21,23 @@ pub fn init_world(testbed: &mut Testbed) {
*/ */
let ground_size = 25.0; 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 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); colliders.insert_with_parent(collider, handle, &mut bodies);
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static()
.rotation(std::f32::consts::FRAC_PI_2) .rotation(std::f32::consts::FRAC_PI_2)
.translation(vector![ground_size, ground_size]) .translation(vector![ground_size, ground_size]);
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static()
.rotation(std::f32::consts::FRAC_PI_2) .rotation(std::f32::consts::FRAC_PI_2)
.translation(vector![-ground_size, ground_size]) .translation(vector![-ground_size, ground_size]);
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
/* /*
@@ -86,12 +84,10 @@ pub fn init_world(testbed: &mut Testbed) {
.collect(); .collect();
for k in 0..5 { for k in 0..5 {
let collider = let collider = ColliderBuilder::trimesh(vertices.clone(), indices.clone());
ColliderBuilder::trimesh(vertices.clone(), indices.clone()).build();
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![ith as f32 * 8.0 - 20.0, 20.0 + k as f32 * 11.0]) .translation(vector![ith as f32 * 8.0 - 20.0, 20.0 + k as f32 * 11.0])
.rotation(angle) .rotation(angle);
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
colliders.insert_with_parent(collider, handle, &mut bodies); colliders.insert_with_parent(collider, handle, &mut bodies);
} }

222
examples3d/articulations3.rs
View File

@@ -13,20 +13,17 @@ fn create_prismatic_joints(
let rad = 0.4; let rad = 0.4;
let shift = 2.0; let shift = 2.0;
let ground = RigidBodyBuilder::new_static() let ground = RigidBodyBuilder::new_static().translation(vector![origin.x, origin.y, origin.z]);
.translation(vector![origin.x, origin.y, origin.z])
.build();
let mut curr_parent = bodies.insert(ground); 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); colliders.insert_with_parent(collider, curr_parent, bodies);
for i in 0..num { for i in 0..num {
let z = origin.z + (i + 1) as f32 * shift; let z = origin.z + (i + 1) as f32 * shift;
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body =
.translation(vector![origin.x, origin.y, z]) RigidBodyBuilder::new_dynamic().translation(vector![origin.x, origin.y, z]);
.build();
let curr_child = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, curr_child, bodies);
let axis = if i % 2 == 0 { let axis = if i % 2 == 0 {
@@ -35,10 +32,10 @@ fn create_prismatic_joints(
UnitVector::new_normalize(vector![-1.0f32, 1.0, 0.0]) 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_anchor1(point![0.0, 0.0, 0.0])
.local_anchor2(point![0.0, 0.0, -shift]) .local_anchor2(point![0.0, 0.0, -shift])
.limit_axis([-2.0, 2.0]); .limits([-2.0, 2.0]);
if use_articulations { if use_articulations {
multibody_joints.insert(curr_parent, curr_child, prism); multibody_joints.insert(curr_parent, curr_child, prism);
@@ -61,20 +58,17 @@ fn create_actuated_prismatic_joints(
let rad = 0.4; let rad = 0.4;
let shift = 2.0; let shift = 2.0;
let ground = RigidBodyBuilder::new_static() let ground = RigidBodyBuilder::new_static().translation(vector![origin.x, origin.y, origin.z]);
.translation(vector![origin.x, origin.y, origin.z])
.build();
let mut curr_parent = bodies.insert(ground); 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); colliders.insert_with_parent(collider, curr_parent, bodies);
for i in 0..num { for i in 0..num {
let z = origin.z + (i + 1) as f32 * shift; let z = origin.z + (i + 1) as f32 * shift;
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body =
.translation(vector![origin.x, origin.y, z]) RigidBodyBuilder::new_dynamic().translation(vector![origin.x, origin.y, z]);
.build();
let curr_child = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, curr_child, bodies);
let axis = if i % 2 == 0 { 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]) UnitVector::new_normalize(vector![-1.0, 1.0, 0.0])
}; };
let mut prism = PrismaticJoint::new(axis) let mut prism = PrismaticJointBuilder::new(axis)
.local_anchor1(point![0.0, 0.0, 0.0]) .local_anchor1(point![0.0, 0.0, shift])
.local_anchor2(point![0.0, 0.0, -shift]); .local_anchor2(point![0.0, 0.0, 0.0])
.build();
if i == 1 { if i == 0 {
prism = prism prism
.limit_axis([-Real::MAX, 5.0]) .set_motor_velocity(2.0, 1.0e5)
.motor_velocity(1.0, 1.0)
// We set a max impulse so that the motor doesn't fight // We set a max impulse so that the motor doesn't fight
// the limits with large forces. // 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 { } else if i > 1 {
prism = prism.motor_position(2.0, 0.01, 1.0); prism
} else { .set_motor_position(2.0, 1.0e3, 1.0e2)
prism = prism .set_motor_max_force(60.0);
.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]);
} }
if use_articulations { if use_articulations {
@@ -127,11 +124,9 @@ fn create_revolute_joints(
let rad = 0.4; let rad = 0.4;
let shift = 2.0; let shift = 2.0;
let ground = RigidBodyBuilder::new_static() let ground = RigidBodyBuilder::new_static().translation(vector![origin.x, origin.y, 0.0]);
.translation(vector![origin.x, origin.y, 0.0])
.build();
let mut curr_parent = bodies.insert(ground); 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); colliders.insert_with_parent(collider, curr_parent, bodies);
for i in 0..num { for i in 0..num {
@@ -146,11 +141,9 @@ fn create_revolute_joints(
let mut handles = [curr_parent; 4]; let mut handles = [curr_parent; 4];
for k in 0..4 { for k in 0..4 {
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().position(positions[k]);
.position(positions[k])
.build();
handles[k] = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handles[k], bodies);
} }
@@ -158,10 +151,10 @@ fn create_revolute_joints(
let x = Vector::x_axis(); let x = Vector::x_axis();
let z = Vector::z_axis(); let z = Vector::z_axis();
let revs = [ let revs = [
RevoluteJoint::new(z).local_anchor2(point![0.0, 0.0, -shift]), RevoluteJointBuilder::new(z).local_anchor2(point![0.0, 0.0, -shift]),
RevoluteJoint::new(x).local_anchor2(point![-shift, 0.0, 0.0]), RevoluteJointBuilder::new(x).local_anchor2(point![-shift, 0.0, 0.0]),
RevoluteJoint::new(z).local_anchor2(point![0.0, 0.0, -shift]), RevoluteJointBuilder::new(z).local_anchor2(point![0.0, 0.0, -shift]),
RevoluteJoint::new(x).local_anchor2(point![shift, 0.0, 0.0]), RevoluteJointBuilder::new(x).local_anchor2(point![shift, 0.0, 0.0]),
]; ];
if use_articulations { if use_articulations {
@@ -188,37 +181,18 @@ fn create_revolute_joints_with_limits(
origin: Point<f32>, origin: Point<f32>,
use_articulations: bool, use_articulations: bool,
) { ) {
let ground = bodies.insert( let ground = bodies.insert(RigidBodyBuilder::new_static().translation(origin.coords));
RigidBodyBuilder::new_static()
.translation(origin.coords)
.build(),
);
let platform1 = bodies.insert( let platform1 = bodies.insert(RigidBodyBuilder::new_dynamic().translation(origin.coords));
RigidBodyBuilder::new_dynamic() colliders.insert_with_parent(ColliderBuilder::cuboid(4.0, 0.2, 2.0), platform1, bodies);
.translation(origin.coords)
.build(),
);
colliders.insert_with_parent(
ColliderBuilder::cuboid(4.0, 0.2, 2.0).build(),
platform1,
bodies,
);
let shift = vector![0.0, 0.0, 6.0]; let shift = vector![0.0, 0.0, 6.0];
let platform2 = bodies.insert( let platform2 =
RigidBodyBuilder::new_dynamic() bodies.insert(RigidBodyBuilder::new_dynamic().translation(origin.coords + shift));
.translation(origin.coords + shift) colliders.insert_with_parent(ColliderBuilder::cuboid(4.0, 0.2, 2.0), platform2, bodies);
.build(),
);
colliders.insert_with_parent(
ColliderBuilder::cuboid(4.0, 0.2, 2.0).build(),
platform2,
bodies,
);
let z = Vector::z_axis(); 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 { if use_articulations {
multibody_joints.insert(ground, platform1, joint1); multibody_joints.insert(ground, platform1, joint1);
@@ -226,9 +200,9 @@ fn create_revolute_joints_with_limits(
impulse_joints.insert(ground, platform1, joint1); impulse_joints.insert(ground, platform1, joint1);
} }
let joint2 = RevoluteJoint::new(z) let joint2 = RevoluteJointBuilder::new(z)
.local_anchor2(-Point::from(shift)) .local_anchor2(-Point::from(shift))
.limit_axis([-0.2, 0.2]); .limits([-0.2, 0.2]);
if use_articulations { if use_articulations {
multibody_joints.insert(platform1, platform2, joint2); 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’s add a couple of cuboids that will fall on the platforms, triggering the joint limits.
let cuboid_body1 = bodies.insert( let cuboid_body1 = bodies.insert(
RigidBodyBuilder::new_dynamic() RigidBodyBuilder::new_dynamic().translation(origin.coords + vector![-2.0, 4.0, 0.0]),
.translation(origin.coords + vector![-2.0, 4.0, 0.0])
.build(),
); );
colliders.insert_with_parent( 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, cuboid_body1,
bodies, bodies,
); );
let cuboid_body2 = bodies.insert( let cuboid_body2 = bodies.insert(
RigidBodyBuilder::new_dynamic() RigidBodyBuilder::new_dynamic()
.translation(origin.coords + shift + vector![2.0, 16.0, 0.0]) .translation(origin.coords + shift + vector![2.0, 16.0, 0.0]),
.build(),
); );
colliders.insert_with_parent( 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, cuboid_body2,
bodies, bodies,
); );
@@ -288,22 +259,20 @@ fn create_fixed_joints(
RigidBodyType::Dynamic RigidBodyType::Dynamic
}; };
let rigid_body = RigidBodyBuilder::new(status) let rigid_body = RigidBodyBuilder::new(status).translation(vector![
.translation(vector![
origin.x + fk * shift, origin.x + fk * shift,
origin.y, origin.y,
origin.z + fi * shift origin.z + fi * shift
]) ]);
.build();
let child_handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, child_handle, bodies);
// Vertical joint. // Vertical joint.
if i > 0 { if i > 0 {
let parent_index = body_handles.len() - num; let parent_index = body_handles.len() - num;
let parent_handle = body_handles[parent_index]; 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 { if use_articulations {
multibody_joints.insert(parent_handle, child_handle, joint); multibody_joints.insert(parent_handle, child_handle, joint);
@@ -316,7 +285,7 @@ fn create_fixed_joints(
if k > 0 { if k > 0 {
let parent_index = body_handles.len() - 1; let parent_index = body_handles.len() - 1;
let parent_handle = body_handles[parent_index]; 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); impulse_joints.insert(parent_handle, child_handle, joint);
} }
@@ -349,17 +318,20 @@ fn create_spherical_joints(
RigidBodyType::Dynamic RigidBodyType::Dynamic
}; };
let rigid_body = RigidBodyBuilder::new(status) let rigid_body = RigidBodyBuilder::new(status).translation(vector![
.translation(vector![fk * shift, 0.0, fi * shift * 2.0]) fk * shift,
.build(); 0.0,
fi * shift * 2.0
]);
let child_handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, child_handle, bodies);
// Vertical joint. // Vertical joint.
if i > 0 { if i > 0 {
let parent_handle = *body_handles.last().unwrap(); 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 { if use_articulations {
multibody_joints.insert(parent_handle, child_handle, joint); multibody_joints.insert(parent_handle, child_handle, joint);
@@ -372,7 +344,7 @@ fn create_spherical_joints(
if k > 0 { if k > 0 {
let parent_index = body_handles.len() - num; let parent_index = body_handles.len() - num;
let parent_handle = body_handles[parent_index]; 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); 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 shift = vector![0.0, 0.0, 3.0];
let ground = bodies.insert( let ground = bodies.insert(RigidBodyBuilder::new_static().translation(origin.coords));
RigidBodyBuilder::new_static()
.translation(origin.coords)
.build(),
);
let ball1 = bodies.insert( let ball1 = bodies.insert(
RigidBodyBuilder::new_dynamic() RigidBodyBuilder::new_dynamic()
.translation(origin.coords + shift) .translation(origin.coords + shift)
.linvel(vector![20.0, 20.0, 0.0]) .linvel(vector![20.0, 20.0, 0.0]),
.build(),
);
colliders.insert_with_parent(
ColliderBuilder::cuboid(1.0, 1.0, 1.0).build(),
ball1,
bodies,
); );
colliders.insert_with_parent(ColliderBuilder::cuboid(1.0, 1.0, 1.0), ball1, bodies);
let ball2 = bodies.insert( let ball2 =
RigidBodyBuilder::new_dynamic() bodies.insert(RigidBodyBuilder::new_dynamic().translation(origin.coords + shift * 2.0));
.translation(origin.coords + shift * 2.0) colliders.insert_with_parent(ColliderBuilder::cuboid(1.0, 1.0, 1.0), ball2, bodies);
.build(),
);
colliders.insert_with_parent(
ColliderBuilder::cuboid(1.0, 1.0, 1.0).build(),
ball2,
bodies,
);
let joint1 = SphericalJoint::new() let joint1 = SphericalJointBuilder::new()
.local_anchor2(Point::from(-shift)) .local_anchor2(Point::from(-shift))
.limit_axis(JointAxis::X, [-0.2, 0.2]) .limits(JointAxis::X, [-0.2, 0.2])
.limit_axis(JointAxis::Y, [-0.2, 0.2]); .limits(JointAxis::Y, [-0.2, 0.2]);
let joint2 = SphericalJoint::new() let joint2 = SphericalJointBuilder::new()
.local_anchor2(Point::from(-shift)) .local_anchor2(Point::from(-shift))
.limit_axis(JointAxis::X, [-0.3, 0.3]) .limits(JointAxis::X, [-0.3, 0.3])
.limit_axis(JointAxis::Y, [-0.3, 0.3]); .limits(JointAxis::Y, [-0.3, 0.3]);
if use_articulations { if use_articulations {
multibody_joints.insert(ground, ball1, joint1); 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. // We will reuse this base configuration for all the impulse_joints here.
let z = Vector::z_axis(); 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(); let mut parent_handle = RigidBodyHandle::invalid();
@@ -474,27 +430,27 @@ fn create_actuated_revolute_joints(
let rigid_body = RigidBodyBuilder::new(status) let rigid_body = RigidBodyBuilder::new(status)
.translation(vector![origin.x, origin.y + shifty, origin.z + fi * shift]) .translation(vector![origin.x, origin.y + shifty, origin.z + fi * shift])
// .rotation(Vector3::new(0.0, fi * 1.1, 0.0)) // .rotation(Vector3::new(0.0, fi * 1.1, 0.0))
.build(); ;
let child_handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, child_handle, bodies);
if i > 0 { if i > 0 {
let mut joint = joint_template.clone(); let mut joint = joint_template.motor_model(MotorModel::AccelerationBased);
if i % 3 == 1 { 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 { } else if i == num - 1 {
let stiffness = 0.2; let stiffness = 200.0;
let damping = 1.0; let damping = 100.0;
joint = joint.motor_position(3.14 / 2.0, stiffness, damping); joint = joint.motor_position(3.14 / 2.0, stiffness, damping);
} }
if i == 1 { if i == 1 {
joint = joint joint = joint
.local_anchor2(point![0.0, 2.0, -shift]) .local_anchor2(point![0.0, 2.0, -shift])
.motor_velocity(-2.0, 0.1); .motor_velocity(-2.0, 100.0);
} }
if use_articulations { if use_articulations {
@@ -521,7 +477,7 @@ fn create_actuated_spherical_joints(
let shift = 2.0; let shift = 2.0;
// We will reuse this base configuration for all the impulse_joints here. // 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(); let mut parent_handle = RigidBodyHandle::invalid();
@@ -540,10 +496,10 @@ fn create_actuated_spherical_joints(
let rigid_body = RigidBodyBuilder::new(status) let rigid_body = RigidBodyBuilder::new(status)
.translation(vector![origin.x, origin.y, origin.z + fi * shift]) .translation(vector![origin.x, origin.y, origin.z + fi * shift])
// .rotation(Vector3::new(0.0, fi * 1.1, 0.0)) // .rotation(Vector3::new(0.0, fi * 1.1, 0.0))
.build(); ;
let child_handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, child_handle, bodies);
if i > 0 { if i > 0 {

24
examples3d/ccd3.rs
View File

@@ -21,12 +21,9 @@ fn create_wall(
- stack_height as f32 * half_extents.z; - stack_height as f32 * half_extents.z;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
.translation(vector![x, y, z])
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
let collider = let collider = ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z);
ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z).build();
colliders.insert_with_parent(collider, handle, bodies); colliders.insert_with_parent(collider, handle, bodies);
k += 1; k += 1;
if k % 2 == 0 { if k % 2 == 0 {
@@ -53,11 +50,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 50.0; let ground_size = 50.0;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let ground_handle = bodies.insert(rigid_body); 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); 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) let collider = ColliderBuilder::ball(1.0)
.density(10.0) .density(10.0)
.sensor(true) .sensor(true)
.active_events(ActiveEvents::INTERSECTION_EVENTS) .active_events(ActiveEvents::INTERSECTION_EVENTS);
.build();
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic()
.linvel(vector![1000.0, 0.0, 0.0]) .linvel(vector![1000.0, 0.0, 0.0])
.translation(vector![-20.0, shift_y + 2.0, 0.0]) .translation(vector![-20.0, shift_y + 2.0, 0.0])
.ccd_enabled(true) .ccd_enabled(true);
.build();
let sensor_handle = bodies.insert(rigid_body); let sensor_handle = bodies.insert(rigid_body);
colliders.insert_with_parent(collider, sensor_handle, &mut bodies); colliders.insert_with_parent(collider, sensor_handle, &mut bodies);
// Second rigid-body with CCD enabled. // 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() let rigid_body = RigidBodyBuilder::new_dynamic()
.linvel(vector![1000.0, 0.0, 0.0]) .linvel(vector![1000.0, 0.0, 0.0])
.translation(vector![-20.0, shift_y + 2.0, shift_z]) .translation(vector![-20.0, shift_y + 2.0, shift_z])
.ccd_enabled(true) .ccd_enabled(true);
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
colliders.insert_with_parent(collider.clone(), handle, &mut bodies); colliders.insert_with_parent(collider.clone(), handle, &mut bodies);
testbed.set_initial_body_color(handle, [0.2, 0.2, 1.0]); testbed.set_initial_body_color(handle, [0.2, 0.2, 1.0]);

20
examples3d/collision_groups3.rs
View File

@@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 5.0; let ground_size = 5.0;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let floor_handle = bodies.insert(rigid_body); 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); 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) let green_floor = ColliderBuilder::cuboid(1.0, 0.1, 1.0)
.translation(vector![0.0, 1.0, 0.0]) .translation(vector![0.0, 1.0, 0.0])
.collision_groups(GREEN_GROUP) .collision_groups(GREEN_GROUP);
.build();
let green_collider_handle = let green_collider_handle =
colliders.insert_with_parent(green_floor, floor_handle, &mut bodies); 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) let blue_floor = ColliderBuilder::cuboid(1.0, 0.1, 1.0)
.translation(vector![0.0, 2.0, 0.0]) .translation(vector![0.0, 2.0, 0.0])
.collision_groups(BLUE_GROUP) .collision_groups(BLUE_GROUP);
.build();
let blue_collider_handle = colliders.insert_with_parent(blue_floor, floor_handle, &mut bodies); 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]); 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]) (BLUE_GROUP, [0.0, 0.0, 1.0])
}; };
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
.translation(vector![x, y, z])
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad, rad, rad) let collider = ColliderBuilder::cuboid(rad, rad, rad).collision_groups(group);
.collision_groups(group)
.build();
colliders.insert_with_parent(collider, handle, &mut bodies); colliders.insert_with_parent(collider, handle, &mut bodies);
testbed.set_initial_body_color(handle, color); testbed.set_initial_body_color(handle, color);

20
examples3d/compound3.rs
View File

@@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 50.0; let ground_size = 50.0;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let handle = bodies.insert(rigid_body); 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); 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; let z = k as f32 * shift * 2.0 - centerz + offset;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
.translation(vector![x, y, z])
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
// First option: attach several colliders to a single rigid-body. // First option: attach several colliders to a single rigid-body.
if j < numy / 2 { 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) let collider2 = ColliderBuilder::cuboid(rad, rad * 10.0, rad)
.translation(vector![rad * 10.0, rad * 10.0, 0.0]) .translation(vector![rad * 10.0, rad * 10.0, 0.0]);
.build();
let collider3 = ColliderBuilder::cuboid(rad, rad * 10.0, rad) let collider3 = ColliderBuilder::cuboid(rad, rad * 10.0, rad)
.translation(vector![-rad * 10.0, rad * 10.0, 0.0]) .translation(vector![-rad * 10.0, rad * 10.0, 0.0]);
.build();
colliders.insert_with_parent(collider1, handle, &mut bodies); colliders.insert_with_parent(collider1, handle, &mut bodies);
colliders.insert_with_parent(collider2, handle, &mut bodies); colliders.insert_with_parent(collider2, handle, &mut bodies);
colliders.insert_with_parent(collider3, 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); colliders.insert_with_parent(collider, handle, &mut bodies);
} }
} }

12
examples3d/convex_decomposition3.rs
View File

@@ -24,11 +24,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 50.0; let ground_size = 50.0;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let handle = bodies.insert(rigid_body); 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); 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 y = (igeom / width) as f32 * shift + 4.0;
let z = k as f32 * shift; let z = k as f32 * shift;
let body = RigidBodyBuilder::new_dynamic() let body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
.translation(vector![x, y, z])
.build();
let handle = bodies.insert(body); let handle = bodies.insert(body);
for shape in &shapes { 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); colliders.insert_with_parent(collider, handle, &mut bodies);
} }
} }

14
examples3d/convex_polyhedron3.rs
View File

@@ -18,11 +18,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 40.0; let ground_size = 40.0;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
/* /*
@@ -54,13 +52,9 @@ pub fn init_world(testbed: &mut Testbed) {
} }
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
.translation(vector![x, y, z])
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::round_convex_hull(&points, border_rad) let collider = ColliderBuilder::round_convex_hull(&points, border_rad).unwrap();
.unwrap()
.build();
colliders.insert_with_parent(collider, handle, &mut bodies); colliders.insert_with_parent(collider, handle, &mut bodies);
} }
} }

5
examples3d/damping3.rs
View File

@@ -27,12 +27,11 @@ pub fn init_world(testbed: &mut Testbed) {
.linvel(vector![x * 10.0, y * 10.0, 0.0]) .linvel(vector![x * 10.0, y * 10.0, 0.0])
.angvel(Vector::z() * 100.0) .angvel(Vector::z() * 100.0)
.linear_damping((i + 1) as f32 * subdiv * 10.0) .linear_damping((i + 1) as f32 * subdiv * 10.0)
.angular_damping((num - i) as f32 * subdiv * 10.0) .angular_damping((num - i) as f32 * subdiv * 10.0);
.build();
let rb_handle = bodies.insert(rb); let rb_handle = bodies.insert(rb);
// Build the collider. // 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); colliders.insert_with_parent(co, rb_handle, &mut bodies);
} }

12
examples3d/debug_add_remove_collider3.rs
View File

@@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 3.0; let ground_size = 3.0;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let ground_handle = bodies.insert(rigid_body); 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 = let mut ground_collider_handle =
colliders.insert_with_parent(collider, ground_handle, &mut bodies); colliders.insert_with_parent(collider, ground_handle, &mut bodies);
@@ -28,11 +26,9 @@ pub fn init_world(testbed: &mut Testbed) {
* Rolling ball * Rolling ball
*/ */
let ball_rad = 0.1; let ball_rad = 0.1;
let rb = RigidBodyBuilder::new_dynamic() let rb = RigidBodyBuilder::new_dynamic().translation(vector![0.0, 0.2, 0.0]);
.translation(vector![0.0, 0.2, 0.0])
.build();
let ball_handle = bodies.insert(rb); 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); colliders.insert_with_parent(collider, ball_handle, &mut bodies);
testbed.add_callback(move |_, physics, _, _| { testbed.add_callback(move |_, physics, _, _| {

23
examples3d/debug_articulations3.rs
View File

@@ -25,17 +25,20 @@ fn create_ball_articulations(
RigidBodyType::Dynamic RigidBodyType::Dynamic
}; };
let rigid_body = RigidBodyBuilder::new(status) let rigid_body = RigidBodyBuilder::new(status).translation(vector![
.translation(vector![fk * shift, 0.0, fi * shift * 2.0]) fk * shift,
.build(); 0.0,
fi * shift * 2.0
]);
let child_handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, child_handle, bodies);
// Vertical multibody_joint. // Vertical multibody_joint.
if i > 0 { if i > 0 {
let parent_handle = *body_handles.last().unwrap(); 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); multibody_joints.insert(parent_handle, child_handle, joint);
} }
@@ -43,7 +46,7 @@ fn create_ball_articulations(
if k > 0 && i > 0 { if k > 0 && i > 0 {
let parent_index = body_handles.len() - num; let parent_index = body_handles.len() - num;
let parent_handle = body_handles[parent_index]; 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 = // let joint =
// PrismaticJoint::new(Vector::y_axis()).local_anchor2(point![-shift, 0.0, 0.0]); // 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]); // 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) let collider = ColliderBuilder::cuboid(30.0, 0.01, 30.0)
.translation(vector![0.0, -3.02, 0.0]) .translation(vector![0.0, -3.02, 0.0])
.rotation(vector![0.1, 0.0, 0.1]) .rotation(vector![0.1, 0.0, 0.1]);
.build();
colliders.insert(collider); 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) let collider = ColliderBuilder::cuboid(30.0, 0.01, 30.0)
.translation(vector![0.0, -3.0, 0.0]) .translation(vector![0.0, -3.0, 0.0])
.rotation(vector![0.1, 0.0, 0.1]) .rotation(vector![0.1, 0.0, 0.1]);
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
colliders.insert_with_parent(collider, handle, &mut bodies); colliders.insert_with_parent(collider, handle, &mut bodies);

10
examples3d/debug_big_colliders3.rs
View File

@@ -13,10 +13,10 @@ pub fn init_world(testbed: &mut Testbed) {
/* /*
* Ground * Ground
*/ */
let rigid_body = RigidBodyBuilder::new_static().build(); let rigid_body = RigidBodyBuilder::new_static();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
let halfspace = SharedShape::new(HalfSpace::new(Vector::y_axis())); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
let mut curr_y = 0.0; 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); let curr_height = 0.1f32.min(curr_width);
curr_y += curr_height * 4.0; curr_y += curr_height * 4.0;
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![0.0, curr_y, 0.0]);
.translation(vector![0.0, curr_y, 0.0])
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
curr_width /= 5.0; curr_width /= 5.0;

12
examples3d/debug_boxes3.rs
View File

@@ -17,11 +17,10 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_height = 0.1; let ground_height = 0.1;
for _ in 0..6 { for _ in 0..6 {
let rigid_body = RigidBodyBuilder::new_static() let rigid_body =
.translation(vector![0.0, -ground_height, 0.0]) RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.build();
let handle = bodies.insert(rigid_body); 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); 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() let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![1.1, 0.0, 0.0]) .translation(vector![1.1, 0.0, 0.0])
// .rotation(vector![0.8, 0.2, 0.1]) // .rotation(vector![0.8, 0.2, 0.1])
.can_sleep(false) .can_sleep(false);
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
} }

12
examples3d/debug_cylinder3.rs
View File

@@ -19,11 +19,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 100.1; let ground_size = 100.1;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
/* /*
@@ -46,11 +44,9 @@ pub fn init_world(testbed: &mut Testbed) {
let z = -centerz + offset; let z = -centerz + offset;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
.translation(vector![x, y, z])
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
/* /*

18
examples3d/debug_dynamic_collider_add3.rs
View File

@@ -16,14 +16,12 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 20.0; let ground_size = 20.0;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let ground_handle = bodies.insert(rigid_body); let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, 0.4) let collider = ColliderBuilder::cuboid(ground_size, ground_height, 0.4)
.friction(0.15) .friction(0.15)
// .restitution(0.5) // .restitution(0.5)
.build(); ;
colliders.insert_with_parent(collider, ground_handle, &mut bodies); 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 ball_rad = 0.1;
let rb = RigidBodyBuilder::new_dynamic() let rb = RigidBodyBuilder::new_dynamic()
.translation(vector![0.0, 0.2, 0.0]) .translation(vector![0.0, 0.2, 0.0])
.linvel(vector![10.0, 0.0, 0.0]) .linvel(vector![10.0, 0.0, 0.0]);
.build();
let ball_handle = bodies.insert(rb); 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); colliders.insert_with_parent(collider, ball_handle, &mut bodies);
let mut linvel = Vector::zeros(); let mut linvel = Vector::zeros();
@@ -49,9 +46,7 @@ pub fn init_world(testbed: &mut Testbed) {
step += 1; step += 1;
// Add a bigger ball collider // Add a bigger ball collider
let collider = ColliderBuilder::ball(ball_rad + 0.01 * (step as f32)) let collider = ColliderBuilder::ball(ball_rad + 0.01 * (step as f32)).density(100.0);
.density(100.0)
.build();
let new_ball_collider_handle = let new_ball_collider_handle =
physics physics
.colliders .colliders
@@ -99,8 +94,7 @@ pub fn init_world(testbed: &mut Testbed) {
// .remove(ground_collider_handle, &mut physics.bodies, true) // .remove(ground_collider_handle, &mut physics.bodies, true)
// .unwrap(); // .unwrap();
let coll = ColliderBuilder::cuboid(ground_size, ground_height + step as f32 * 0.01, 0.4) let coll = ColliderBuilder::cuboid(ground_size, ground_height + step as f32 * 0.01, 0.4)
.friction(0.15) .friction(0.15);
.build();
let new_ground_collider_handle = let new_ground_collider_handle =
physics physics
.colliders .colliders

11
examples3d/debug_friction3.rs
View File

@@ -16,20 +16,17 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 100.0; let ground_size = 100.0;
let ground_height = 0.1; 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 handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size) let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).friction(1.5);
.friction(1.5)
.build();
colliders.insert_with_parent(collider, handle, &mut bodies); colliders.insert_with_parent(collider, handle, &mut bodies);
// Build a dynamic box rigid body. // Build a dynamic box rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![0.0, 1.1, 0.0]) .translation(vector![0.0, 1.1, 0.0])
.rotation(Vector::y() * 0.3) .rotation(Vector::y() * 0.3);
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
let rigid_body = &mut bodies[handle]; let rigid_body = &mut bodies[handle];

16
examples3d/debug_infinite_fall3.rs
View File

@@ -16,29 +16,25 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 100.1; let ground_size = 100.1;
let ground_height = 2.1; let ground_height = 2.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, 4.0, 0.0]);
.translation(vector![0.0, 4.0, 0.0])
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
let rad = 1.0; let rad = 1.0;
// Build the dynamic box rigid body. // Build the dynamic box rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![0.0, 7.0 * rad, 0.0]) .translation(vector![0.0, 7.0 * rad, 0.0])
.can_sleep(false) .can_sleep(false);
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![0.0, 2.0 * rad, 0.0]) .translation(vector![0.0, 2.0 * rad, 0.0])
.can_sleep(false) .can_sleep(false);
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
/* /*

50
examples3d/debug_prismatic3.rs
View File

@@ -7,16 +7,12 @@ fn prismatic_repro(
impulse_joints: &mut ImpulseJointSet, impulse_joints: &mut ImpulseJointSet,
box_center: Point<f32>, box_center: Point<f32>,
) { ) {
let box_rb = bodies.insert( let box_rb = bodies.insert(RigidBodyBuilder::new_dynamic().translation(vector![
RigidBodyBuilder::new_dynamic() box_center.x,
.translation(vector![box_center.x, box_center.y, box_center.z]) box_center.y,
.build(), box_center.z
); ]));
colliders.insert_with_parent( colliders.insert_with_parent(ColliderBuilder::cuboid(1.0, 0.25, 1.0), box_rb, bodies);
ColliderBuilder::cuboid(1.0, 0.25, 1.0).build(),
box_rb,
bodies,
);
let wheel_y = -1.0; let wheel_y = -1.0;
let wheel_positions = vec![ let wheel_positions = vec![
@@ -28,36 +24,28 @@ fn prismatic_repro(
for pos in wheel_positions { for pos in wheel_positions {
let wheel_pos_in_world = box_center + pos; let wheel_pos_in_world = box_center + pos;
let wheel_rb = bodies.insert( let wheel_rb = bodies.insert(RigidBodyBuilder::new_dynamic().translation(vector![
RigidBodyBuilder::new_dynamic()
.translation(vector![
wheel_pos_in_world.x, wheel_pos_in_world.x,
wheel_pos_in_world.y, wheel_pos_in_world.y,
wheel_pos_in_world.z wheel_pos_in_world.z
]) ]));
.build(), colliders.insert_with_parent(ColliderBuilder::ball(0.5), wheel_rb, bodies);
);
colliders.insert_with_parent(ColliderBuilder::ball(0.5).build(), wheel_rb, bodies);
let (stiffness, damping) = (0.05, 0.2); 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]) .local_anchor1(point![pos.x, pos.y, pos.z])
.motor_position(0.0, stiffness, damping); .motor_position(0.0, stiffness, damping);
impulse_joints.insert(box_rb, wheel_rb, prismatic); impulse_joints.insert(box_rb, wheel_rb, prismatic);
} }
// put a small box under one of the wheels // put a small box under one of the wheels
let gravel = bodies.insert( let gravel = bodies.insert(RigidBodyBuilder::new_dynamic().translation(vector![
RigidBodyBuilder::new_dynamic() box_center.x + 1.0,
.translation(vector![box_center.x + 1.0, box_center.y - 2.4, -1.0]) box_center.y - 2.4,
.build(), -1.0
); ]));
colliders.insert_with_parent( colliders.insert_with_parent(ColliderBuilder::cuboid(0.5, 0.1, 0.5), gravel, bodies);
ColliderBuilder::cuboid(0.5, 0.1, 0.5).build(),
gravel,
bodies,
);
} }
pub fn init_world(testbed: &mut Testbed) { 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_size = 50.0;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
prismatic_repro( prismatic_repro(

11
examples3d/debug_rollback3.rs
View File

@@ -16,14 +16,12 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 20.0; let ground_size = 20.0;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let ground_handle = bodies.insert(rigid_body); let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, 0.4) let collider = ColliderBuilder::cuboid(ground_size, ground_height, 0.4)
.friction(0.15) .friction(0.15)
// .restitution(0.5) // .restitution(0.5)
.build(); ;
colliders.insert_with_parent(collider, ground_handle, &mut bodies); 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 ball_rad = 0.1;
let rb = RigidBodyBuilder::new_dynamic() let rb = RigidBodyBuilder::new_dynamic()
.translation(vector![0.0, 0.2, 0.0]) .translation(vector![0.0, 0.2, 0.0])
.linvel(vector![10.0, 0.0, 0.0]) .linvel(vector![10.0, 0.0, 0.0]);
.build();
let ball_handle = bodies.insert(rb); 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); colliders.insert_with_parent(collider, ball_handle, &mut bodies);
let mut linvel = Vector::zeros(); let mut linvel = Vector::zeros();

25
examples3d/debug_shape_modification3.rs
View File

@@ -16,14 +16,12 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 20.0; let ground_size = 20.0;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let ground_handle = bodies.insert(rigid_body); let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size) let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size)
.friction(0.15) .friction(0.15)
// .restitution(0.5) // .restitution(0.5)
.build(); ;
colliders.insert_with_parent(collider, ground_handle, &mut bodies); 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 ball_rad = 0.1;
let rb = RigidBodyBuilder::new_dynamic() let rb = RigidBodyBuilder::new_dynamic()
.translation(vector![0.0, 0.2, 0.0]) .translation(vector![0.0, 0.2, 0.0])
.linvel(vector![10.0, 0.0, 0.0]) .linvel(vector![10.0, 0.0, 0.0]);
.build();
let ball_handle = bodies.insert(rb); 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 ball_coll_handle = colliders.insert_with_parent(collider, ball_handle, &mut bodies);
let mut linvel = Vector::zeros(); let mut linvel = Vector::zeros();
@@ -90,19 +87,17 @@ pub fn init_world(testbed: &mut Testbed) {
let z = k as f32 * shiftz - centerz + offset; let z = k as f32 * shiftz - centerz + offset;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
.translation(vector![x, y, z])
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
let collider = match j % 5 { let collider = match j % 5 {
0 => ColliderBuilder::cuboid(rad, rad, rad).build(), 0 => ColliderBuilder::cuboid(rad, rad, rad),
1 => ColliderBuilder::ball(rad).build(), 1 => ColliderBuilder::ball(rad),
// Rounded cylinders are much more efficient that cylinder, even if the // Rounded cylinders are much more efficient that cylinder, even if the
// rounding margin is small. // rounding margin is small.
2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(), 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0),
3 => ColliderBuilder::cone(rad, rad).build(), 3 => ColliderBuilder::cone(rad, rad),
_ => ColliderBuilder::capsule_y(rad, rad).build(), _ => ColliderBuilder::capsule_y(rad, rad),
}; };
colliders.insert_with_parent(collider, handle, &mut bodies); colliders.insert_with_parent(collider, handle, &mut bodies);

11
examples3d/debug_triangle3.rs
View File

@@ -17,21 +17,18 @@ pub fn init_world(testbed: &mut Testbed) {
point![0.0, 0.0, 10.0], point![0.0, 0.0, 10.0],
]; ];
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, 0.0, 0.0]);
.translation(vector![0.0, 0.0, 0.0])
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
// Dynamic box rigid body. // Dynamic box rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![1.1, 0.01, 0.0]) .translation(vector![1.1, 0.01, 0.0])
// .rotation(Vector3::new(0.8, 0.2, 0.1)) // .rotation(Vector3::new(0.8, 0.2, 0.1))
.can_sleep(false) .can_sleep(false);
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
/* /*

11
examples3d/debug_trimesh3.rs
View File

@@ -41,17 +41,14 @@ pub fn init_world(testbed: &mut Testbed) {
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![0.0, 35.0, 0.0]) .translation(vector![0.0, 35.0, 0.0])
// .rotation(Vector3::new(0.8, 0.2, 0.1)) // .rotation(Vector3::new(0.8, 0.2, 0.1))
.can_sleep(false) .can_sleep(false);
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, 0.0, 0.0]);
.translation(vector![0.0, 0.0, 0.0])
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
testbed.set_initial_body_color(handle, [0.3, 0.3, 0.3]); testbed.set_initial_body_color(handle, [0.3, 0.3, 0.3]);

10
examples3d/domino3.rs
View File

@@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 200.1; let ground_size = 200.1;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let handle = bodies.insert(rigid_body); 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); 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) Translation::new(x * curr_rad, width * 2.0 + ground_height, z * curr_rad)
* tilt * tilt
* rot; * 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 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); colliders.insert_with_parent(collider, handle, &mut bodies);
testbed.set_initial_body_color(handle, colors[i % 2]); testbed.set_initial_body_color(handle, colors[i % 2]);
} else { } else {

16
examples3d/fountain3.rs
View File

@@ -17,23 +17,19 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 100.1; let ground_size = 100.1;
let ground_height = 2.1; // 16.0; let ground_height = 2.1; // 16.0;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
// Callback that will be executed on the main loop to handle proximities. // Callback that will be executed on the main loop to handle proximities.
testbed.add_callback(move |mut graphics, physics, _, run_state| { testbed.add_callback(move |mut graphics, physics, _, run_state| {
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![0.0, 10.0, 0.0]);
.translation(vector![0.0, 10.0, 0.0])
.build();
let handle = physics.bodies.insert(rigid_body); let handle = physics.bodies.insert(rigid_body);
let collider = match run_state.timestep_id % 3 { let collider = match run_state.timestep_id % 3 {
0 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(), 0 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0),
1 => ColliderBuilder::cone(rad, rad).build(), 1 => ColliderBuilder::cone(rad, rad),
_ => ColliderBuilder::cuboid(rad, rad, rad).build(), _ => ColliderBuilder::cuboid(rad, rad, rad),
}; };
physics physics

12
examples3d/harness_capsules3.rs
View File

@@ -16,11 +16,9 @@ pub fn init_world(harness: &mut Harness) {
let ground_size = 200.1; let ground_size = 200.1;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let handle = bodies.insert(rigid_body); 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); 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; let z = k as f32 * shift - centerz + offset;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
.translation(vector![x, y, z])
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
} }
} }

20
examples3d/heightfield3.rs
View File

@@ -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 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); 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; let z = k as f32 * shift - centerz;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
.translation(vector![x, y, z])
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
let collider = match j % 6 { let collider = match j % 6 {
0 => ColliderBuilder::cuboid(rad, rad, rad).build(), 0 => ColliderBuilder::cuboid(rad, rad, rad),
1 => ColliderBuilder::ball(rad).build(), 1 => ColliderBuilder::ball(rad),
// Rounded cylinders are much more efficient that cylinder, even if the // Rounded cylinders are much more efficient that cylinder, even if the
// rounding margin is small. // rounding margin is small.
2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(), 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0),
3 => ColliderBuilder::cone(rad, rad).build(), 3 => ColliderBuilder::cone(rad, rad),
4 => ColliderBuilder::capsule_y(rad, rad).build(), 4 => ColliderBuilder::capsule_y(rad, rad),
_ => { _ => {
let shapes = vec![ let shapes = vec![
( (
@@ -84,7 +82,7 @@ pub fn init_world(testbed: &mut Testbed) {
), ),
]; ];
ColliderBuilder::compound(shapes).build() ColliderBuilder::compound(shapes)
} }
}; };

276
examples3d/joints3.rs
View File

@@ -11,20 +11,17 @@ fn create_prismatic_joints(
let rad = 0.4; let rad = 0.4;
let shift = 2.0; let shift = 2.0;
let ground = RigidBodyBuilder::new_static() let ground = RigidBodyBuilder::new_static().translation(vector![origin.x, origin.y, origin.z]);
.translation(vector![origin.x, origin.y, origin.z])
.build();
let mut curr_parent = bodies.insert(ground); 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); colliders.insert_with_parent(collider, curr_parent, bodies);
for i in 0..num { for i in 0..num {
let z = origin.z + (i + 1) as f32 * shift; let z = origin.z + (i + 1) as f32 * shift;
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body =
.translation(vector![origin.x, origin.y, z]) RigidBodyBuilder::new_dynamic().translation(vector![origin.x, origin.y, z]);
.build();
let curr_child = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, curr_child, bodies);
let axis = if i % 2 == 0 { let axis = if i % 2 == 0 {
@@ -33,10 +30,10 @@ fn create_prismatic_joints(
UnitVector::new_normalize(vector![-1.0f32, 1.0, 0.0]) 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_anchor1(point![0.0, 0.0, shift])
.local_anchor2(point![0.0, 0.0, 0.0]) .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); impulse_joints.insert(curr_parent, curr_child, prism);
@@ -54,20 +51,17 @@ fn create_actuated_prismatic_joints(
let rad = 0.4; let rad = 0.4;
let shift = 2.0; let shift = 2.0;
let ground = RigidBodyBuilder::new_static() let ground = RigidBodyBuilder::new_static().translation(vector![origin.x, origin.y, origin.z]);
.translation(vector![origin.x, origin.y, origin.z])
.build();
let mut curr_parent = bodies.insert(ground); 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); colliders.insert_with_parent(collider, curr_parent, bodies);
for i in 0..num { for i in 0..num {
let z = origin.z + (i + 1) as f32 * shift; let z = origin.z + (i + 1) as f32 * shift;
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body =
.translation(vector![origin.x, origin.y, z]) RigidBodyBuilder::new_dynamic().translation(vector![origin.x, origin.y, z]);
.build();
let curr_child = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, curr_child, bodies);
let axis = if i % 2 == 0 { 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]) 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_anchor1(point![0.0, 0.0, 0.0])
.local_anchor2(point![0.0, 0.0, -shift]); .local_anchor2(point![0.0, 0.0, -shift]);
if i == 1 { if i == 1 {
prism = prism 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) .motor_velocity(JointAxis::X, 1.0, 1.0)
// We set a max impulse so that the motor doesn't fight // We set a max impulse so that the motor doesn't fight
// the limits with large forces. // 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 // We set a max impulse so that the motor doesn't fight
// the limits with large forces. // the limits with large forces.
.motor_max_impulse(JointAxis::X, 0.7) .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); impulse_joints.insert(curr_parent, curr_child, prism);
@@ -114,11 +108,9 @@ fn create_revolute_joints(
let rad = 0.4; let rad = 0.4;
let shift = 2.0; let shift = 2.0;
let ground = RigidBodyBuilder::new_static() let ground = RigidBodyBuilder::new_static().translation(vector![origin.x, origin.y, 0.0]);
.translation(vector![origin.x, origin.y, 0.0])
.build();
let mut curr_parent = bodies.insert(ground); 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); colliders.insert_with_parent(collider, curr_parent, bodies);
for i in 0..num { for i in 0..num {
@@ -133,11 +125,9 @@ fn create_revolute_joints(
let mut handles = [curr_parent; 4]; let mut handles = [curr_parent; 4];
for k in 0..4 { for k in 0..4 {
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().position(positions[k]);
.position(positions[k])
.build();
handles[k] = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handles[k], bodies);
} }
@@ -166,63 +156,41 @@ fn create_revolute_joints_with_limits(
impulse_joints: &mut ImpulseJointSet, impulse_joints: &mut ImpulseJointSet,
origin: Point<f32>, origin: Point<f32>,
) { ) {
let ground = bodies.insert( let ground = bodies.insert(RigidBodyBuilder::new_static().translation(origin.coords));
RigidBodyBuilder::new_static()
.translation(origin.coords)
.build(),
);
let platform1 = bodies.insert( let platform1 = bodies.insert(RigidBodyBuilder::new_dynamic().translation(origin.coords));
RigidBodyBuilder::new_dynamic() colliders.insert_with_parent(ColliderBuilder::cuboid(4.0, 0.2, 2.0), platform1, bodies);
.translation(origin.coords)
.build(),
);
colliders.insert_with_parent(
ColliderBuilder::cuboid(4.0, 0.2, 2.0).build(),
platform1,
bodies,
);
let shift = vector![0.0, 0.0, 6.0]; let shift = vector![0.0, 0.0, 6.0];
let platform2 = bodies.insert( let platform2 =
RigidBodyBuilder::new_dynamic() bodies.insert(RigidBodyBuilder::new_dynamic().translation(origin.coords + shift));
.translation(origin.coords + shift) colliders.insert_with_parent(ColliderBuilder::cuboid(4.0, 0.2, 2.0), platform2, bodies);
.build(),
);
colliders.insert_with_parent(
ColliderBuilder::cuboid(4.0, 0.2, 2.0).build(),
platform2,
bodies,
);
let z = Vector::z_axis(); 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); impulse_joints.insert(ground, platform1, joint1);
let mut joint2 = RevoluteJoint::new(z) let mut joint2 = RevoluteJoint::new(z)
.local_anchor2(shift.into()) .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); impulse_joints.insert(platform1, platform2, joint2);
// Let’s add a couple of cuboids that will fall on the platforms, triggering the joint limits. // Let’s add a couple of cuboids that will fall on the platforms, triggering the joint limits.
let cuboid_body1 = bodies.insert( let cuboid_body1 = bodies.insert(
RigidBodyBuilder::new_dynamic() RigidBodyBuilder::new_dynamic().translation(origin.coords + vector![-2.0, 4.0, 0.0]),
.translation(origin.coords + vector![-2.0, 4.0, 0.0])
.build(),
); );
colliders.insert_with_parent( 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, cuboid_body1,
bodies, bodies,
); );
let cuboid_body2 = bodies.insert( let cuboid_body2 = bodies.insert(
RigidBodyBuilder::new_dynamic() RigidBodyBuilder::new_dynamic()
.translation(origin.coords + shift + vector![2.0, 16.0, 0.0]) .translation(origin.coords + shift + vector![2.0, 16.0, 0.0]),
.build(),
); );
colliders.insert_with_parent( 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, cuboid_body2,
bodies, bodies,
); );
@@ -254,22 +222,20 @@ fn create_fixed_joints(
RigidBodyType::Dynamic RigidBodyType::Dynamic
}; };
let rigid_body = RigidBodyBuilder::new(status) let rigid_body = RigidBodyBuilder::new(status).translation(vector![
.translation(vector![
origin.x + fk * shift, origin.x + fk * shift,
origin.y, origin.y,
origin.z + fi * shift origin.z + fi * shift
]) ]);
.build();
let child_handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, child_handle, bodies);
// Vertical joint. // Vertical joint.
if i > 0 { if i > 0 {
let parent_index = body_handles.len() - num; let parent_index = body_handles.len() - num;
let parent_handle = body_handles[parent_index]; 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); impulse_joints.insert(parent_handle, child_handle, joint);
} }
@@ -277,7 +243,7 @@ fn create_fixed_joints(
if k > 0 { if k > 0 {
let parent_index = body_handles.len() - 1; let parent_index = body_handles.len() - 1;
let parent_handle = body_handles[parent_index]; 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); impulse_joints.insert(parent_handle, child_handle, joint);
} }
@@ -308,17 +274,19 @@ fn create_ball_joints(
RigidBodyType::Dynamic RigidBodyType::Dynamic
}; };
let rigid_body = RigidBodyBuilder::new(status) let rigid_body = RigidBodyBuilder::new(status).translation(vector![
.translation(vector![fk * shift, 0.0, fi * shift * 2.0]) fk * shift,
.build(); 0.0,
fi * shift * 2.0
]);
let child_handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, child_handle, bodies);
// Vertical joint. // Vertical joint.
if i > 0 { if i > 0 {
let parent_handle = *body_handles.last().unwrap(); 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); impulse_joints.insert(parent_handle, child_handle, joint);
} }
@@ -326,7 +294,7 @@ fn create_ball_joints(
if k > 0 { if k > 0 {
let parent_index = body_handles.len() - num; let parent_index = body_handles.len() - num;
let parent_handle = body_handles[parent_index]; 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); 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 shift = vector![0.0, 0.0, 3.0];
let ground = bodies.insert( let ground = bodies.insert(RigidBodyBuilder::new_static().translation(origin.coords));
RigidBodyBuilder::new_static()
.translation(origin.coords)
.build(),
);
let ball1 = bodies.insert( let ball1 = bodies.insert(
RigidBodyBuilder::new_dynamic() RigidBodyBuilder::new_dynamic()
.translation(origin.coords + shift) .translation(origin.coords + shift)
.linvel(vector![20.0, 20.0, 0.0]) .linvel(vector![20.0, 20.0, 0.0]),
.build(),
);
colliders.insert_with_parent(
ColliderBuilder::cuboid(1.0, 1.0, 1.0).build(),
ball1,
bodies,
); );
colliders.insert_with_parent(ColliderBuilder::cuboid(1.0, 1.0, 1.0), ball1, bodies);
let ball2 = bodies.insert( let ball2 =
RigidBodyBuilder::new_dynamic() bodies.insert(RigidBodyBuilder::new_dynamic().translation(origin.coords + shift * 2.0));
.translation(origin.coords + shift * 2.0) colliders.insert_with_parent(ColliderBuilder::cuboid(1.0, 1.0, 1.0), ball2, bodies);
.build(),
);
colliders.insert_with_parent(
ColliderBuilder::cuboid(1.0, 1.0, 1.0).build(),
ball2,
bodies,
);
let mut joint1 = JointData::ball() let mut joint1 = GenericJoint::ball()
.local_anchor2(Point::from(-shift)) .local_anchor2(Point::from(-shift))
.limit_axis(JointAxis::X, [-0.2, 0.2]) .limits(JointAxis::X, [-0.2, 0.2])
.limit_axis(JointAxis::Y, [-0.2, 0.2]); .limits(JointAxis::Y, [-0.2, 0.2]);
impulse_joints.insert(ground, ball1, joint1); impulse_joints.insert(ground, ball1, joint1);
let mut joint2 = JointData::ball() let mut joint2 = GenericJoint::ball()
.local_anchor2(Point::from(-shift)) .local_anchor2(Point::from(-shift))
.limit_axis(JointAxis::X, [-0.3, 0.3]) .limits(JointAxis::X, [-0.3, 0.3])
.limit_axis(JointAxis::Y, [-0.3, 0.3]); .limits(JointAxis::Y, [-0.3, 0.3]);
impulse_joints.insert(ball1, ball2, joint2); impulse_joints.insert(ball1, ball2, joint2);
} }
@@ -418,26 +370,28 @@ fn create_actuated_revolute_joints(
let rigid_body = RigidBodyBuilder::new(status) let rigid_body = RigidBodyBuilder::new(status)
.translation(vector![origin.x, origin.y + shifty, origin.z + fi * shift]) .translation(vector![origin.x, origin.y + shifty, origin.z + fi * shift])
// .rotation(Vector3::new(0.0, fi * 1.1, 0.0)) // .rotation(Vector3::new(0.0, fi * 1.1, 0.0))
.build(); ;
let child_handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, child_handle, bodies);
if i > 0 { if i > 0 {
let mut joint = joint_template.clone(); let mut joint = joint_template
.clone()
.motor_model(MotorModel::AccelerationBased);
if i % 3 == 1 { 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 { } else if i == num - 1 {
let stiffness = 0.2; let stiffness = 0.2;
let damping = 1.0; 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 { if i == 1 {
joint.local_frame2.translation.vector.y = 2.0; 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); impulse_joints.insert(parent_handle, child_handle, joint);
@@ -458,7 +412,7 @@ fn create_actuated_ball_joints(
let shift = 2.0; let shift = 2.0;
// We will reuse this base configuration for all the impulse_joints here. // 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(); let mut parent_handle = RigidBodyHandle::invalid();
@@ -477,10 +431,10 @@ fn create_actuated_ball_joints(
let rigid_body = RigidBodyBuilder::new(status) let rigid_body = RigidBodyBuilder::new(status)
.translation(vector![origin.x, origin.y, origin.z + fi * shift]) .translation(vector![origin.x, origin.y, origin.z + fi * shift])
// .rotation(Vector3::new(0.0, fi * 1.1, 0.0)) // .rotation(Vector3::new(0.0, fi * 1.1, 0.0))
.build(); ;
let child_handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, child_handle, bodies);
if i > 0 { if i > 0 {
@@ -516,40 +470,40 @@ pub fn init_world(testbed: &mut Testbed) {
let mut impulse_joints = ImpulseJointSet::new(); let mut impulse_joints = ImpulseJointSet::new();
let multibody_joints = MultibodyJointSet::new(); let multibody_joints = MultibodyJointSet::new();
create_prismatic_joints( // create_prismatic_joints(
&mut bodies, // &mut bodies,
&mut colliders, // &mut colliders,
&mut impulse_joints, // &mut impulse_joints,
point![20.0, 5.0, 0.0], // point![20.0, 5.0, 0.0],
4, // 4,
); // );
create_actuated_prismatic_joints( // create_actuated_prismatic_joints(
&mut bodies, // &mut bodies,
&mut colliders, // &mut colliders,
&mut impulse_joints, // &mut impulse_joints,
point![25.0, 5.0, 0.0], // point![25.0, 5.0, 0.0],
4, // 4,
); // );
create_revolute_joints( // create_revolute_joints(
&mut bodies, // &mut bodies,
&mut colliders, // &mut colliders,
&mut impulse_joints, // &mut impulse_joints,
point![20.0, 0.0, 0.0], // point![20.0, 0.0, 0.0],
3, // 3,
); // );
create_revolute_joints_with_limits( // create_revolute_joints_with_limits(
&mut bodies, // &mut bodies,
&mut colliders, // &mut colliders,
&mut impulse_joints, // &mut impulse_joints,
point![34.0, 0.0, 0.0], // point![34.0, 0.0, 0.0],
); // );
create_fixed_joints( // create_fixed_joints(
&mut bodies, // &mut bodies,
&mut colliders, // &mut colliders,
&mut impulse_joints, // &mut impulse_joints,
point![0.0, 10.0, 0.0], // point![0.0, 10.0, 0.0],
10, // 10,
); // );
create_actuated_revolute_joints( create_actuated_revolute_joints(
&mut bodies, &mut bodies,
&mut colliders, &mut colliders,
@@ -557,20 +511,20 @@ pub fn init_world(testbed: &mut Testbed) {
point![20.0, 10.0, 0.0], point![20.0, 10.0, 0.0],
6, 6,
); );
create_actuated_ball_joints( // create_actuated_ball_joints(
&mut bodies, // &mut bodies,
&mut colliders, // &mut colliders,
&mut impulse_joints, // &mut impulse_joints,
point![13.0, 10.0, 0.0], // point![13.0, 10.0, 0.0],
3, // 3,
); // );
create_ball_joints(&mut bodies, &mut colliders, &mut impulse_joints, 15); // create_ball_joints(&mut bodies, &mut colliders, &mut impulse_joints, 15);
create_ball_joints_with_limits( // create_ball_joints_with_limits(
&mut bodies, // &mut bodies,
&mut colliders, // &mut colliders,
&mut impulse_joints, // &mut impulse_joints,
point![-5.0, 0.0, 0.0], // point![-5.0, 0.0, 0.0],
); // );
/* /*
* Set up the testbed. * Set up the testbed.

22
examples3d/keva3.rs
View File

@@ -38,15 +38,13 @@ pub fn build_block(
}; };
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![
.translation(vector![
x + dim.x + shift.x, x + dim.x + shift.x,
y + dim.y + shift.y, y + dim.y + shift.y,
z + dim.z + shift.z z + dim.z + shift.z
]) ]);
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, bodies);
testbed.set_initial_body_color(handle, color0); 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 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 { for j in 0..(block_width / (dim.z as f32 * 2.0)) as usize {
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![
.translation(vector![
i as f32 * dim.x * 2.0 + dim.x + shift.x, i as f32 * dim.x * 2.0 + dim.x + shift.x,
dim.y + shift.y + block_height, dim.y + shift.y + block_height,
j as f32 * dim.z * 2.0 + dim.z + shift.z j as f32 * dim.z * 2.0 + dim.z + shift.z
]) ]);
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, bodies);
testbed.set_initial_body_color(handle, color0); testbed.set_initial_body_color(handle, color0);
std::mem::swap(&mut color0, &mut color1); 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_size = 50.0;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
/* /*

18
examples3d/locked_rotations3.rs
View File

@@ -19,11 +19,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 5.0; let ground_size = 5.0;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let handle = bodies.insert(rigid_body); 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); 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() let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![0.0, 3.0, 0.0]) .translation(vector![0.0, 3.0, 0.0])
.lock_translations() .lock_translations()
.restrict_rotations(true, false, false) .restrict_rotations(true, false, false);
.build();
let handle = bodies.insert(rigid_body); 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); 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() let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![0.0, 5.0, 0.0]) .translation(vector![0.0, 5.0, 0.0])
.rotation(Vector::x() * 1.0) .rotation(Vector::x() * 1.0)
.lock_rotations() .lock_rotations();
.build();
let handle = bodies.insert(rigid_body); 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); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
/* /*

14
examples3d/one_way_platforms3.rs
View File

@@ -68,18 +68,16 @@ pub fn init_world(testbed: &mut Testbed) {
/* /*
* Ground * Ground
*/ */
let rigid_body = RigidBodyBuilder::new_static().build(); let rigid_body = RigidBodyBuilder::new_static();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(9.0, 0.5, 25.0) let collider = ColliderBuilder::cuboid(9.0, 0.5, 25.0)
.translation(vector![0.0, 2.0, 30.0]) .translation(vector![0.0, 2.0, 30.0])
.active_hooks(ActiveHooks::MODIFY_SOLVER_CONTACTS) .active_hooks(ActiveHooks::MODIFY_SOLVER_CONTACTS);
.build();
let platform1 = colliders.insert_with_parent(collider, handle, &mut bodies); let platform1 = colliders.insert_with_parent(collider, handle, &mut bodies);
let collider = ColliderBuilder::cuboid(9.0, 0.5, 25.0) let collider = ColliderBuilder::cuboid(9.0, 0.5, 25.0)
.translation(vector![0.0, -2.0, -30.0]) .translation(vector![0.0, -2.0, -30.0])
.active_hooks(ActiveHooks::MODIFY_SOLVER_CONTACTS) .active_hooks(ActiveHooks::MODIFY_SOLVER_CONTACTS);
.build();
let platform2 = colliders.insert_with_parent(collider, handle, &mut bodies); 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| { testbed.add_callback(move |graphics, physics, _, run_state| {
if run_state.timestep_id % 50 == 0 && physics.bodies.len() <= 7 { if run_state.timestep_id % 50 == 0 && physics.bodies.len() <= 7 {
// Spawn a new cube. // Spawn a new cube.
let collider = ColliderBuilder::cuboid(1.0, 2.0, 1.5).build(); let collider = ColliderBuilder::cuboid(1.0, 2.0, 1.5);
let body = RigidBodyBuilder::new_dynamic() let body = RigidBodyBuilder::new_dynamic().translation(vector![0.0, 6.0, 20.0]);
.translation(vector![0.0, 6.0, 20.0])
.build();
let handle = physics.bodies.insert(body); let handle = physics.bodies.insert(body);
physics physics
.colliders .colliders

32
examples3d/platform3.rs
View File

@@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 10.0; let ground_size = 10.0;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let handle = bodies.insert(rigid_body); 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); 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; let z = k as f32 * shift - centerz;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
.translation(vector![x, y, z])
.build();
let handle = bodies.insert(rigid_body); 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); 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. * Setup a velocity-based kinematic rigid body.
*/ */
let platform_body = RigidBodyBuilder::new_kinematic_velocity_based() let platform_body = RigidBodyBuilder::new_kinematic_velocity_based().translation(vector![
.translation(vector![0.0, 1.5 + 0.8, -10.0 * rad]) 0.0,
.build(); 1.5 + 0.8,
-10.0 * rad
]);
let velocity_based_platform_handle = bodies.insert(platform_body); 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); colliders.insert_with_parent(collider, velocity_based_platform_handle, &mut bodies);
/* /*
* Setup a position-based kinematic rigid body. * Setup a position-based kinematic rigid body.
*/ */
let platform_body = RigidBodyBuilder::new_kinematic_position_based() let platform_body = RigidBodyBuilder::new_kinematic_position_based().translation(vector![
.translation(vector![0.0, 2.0 + 1.5 + 0.8, -10.0 * rad]) 0.0,
.build(); 2.0 + 1.5 + 0.8,
-10.0 * rad
]);
let position_based_platform_handle = bodies.insert(platform_body); 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); colliders.insert_with_parent(collider, position_based_platform_handle, &mut bodies);
/* /*

20
examples3d/primitives3.rs
View File

@@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 100.1; let ground_size = 100.1;
let ground_height = 2.1; let ground_height = 2.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let handle = bodies.insert(rigid_body); 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); 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; let z = k as f32 * shiftz - centerz + offset;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
.translation(vector![x, y, z])
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
let collider = match j % 5 { let collider = match j % 5 {
// _ => ColliderBuilder::cuboid(rad, rad, rad).build(), // _ => ColliderBuilder::cuboid(rad, rad, rad),
1 => ColliderBuilder::ball(rad).build(), 1 => ColliderBuilder::ball(rad),
// Rounded cylinders are much more efficient that cylinder, even if the // Rounded cylinders are much more efficient that cylinder, even if the
// rounding margin is small. // rounding margin is small.
2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(), 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0),
3 => ColliderBuilder::cone(rad, rad).build(), 3 => ColliderBuilder::cone(rad, rad),
_ => ColliderBuilder::capsule_y(rad, rad).build(), _ => ColliderBuilder::capsule_y(rad, rad),
}; };
colliders.insert_with_parent(collider, handle, &mut bodies); colliders.insert_with_parent(collider, handle, &mut bodies);

20
examples3d/restitution3.rs
View File

@@ -16,13 +16,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 20.; let ground_size = 20.;
let ground_height = 1.0; let ground_height = 1.0;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, 2.0) let collider = ColliderBuilder::cuboid(ground_size, ground_height, 2.0).restitution(1.0);
.restitution(1.0)
.build();
colliders.insert_with_parent(collider, handle, &mut bodies); colliders.insert_with_parent(collider, handle, &mut bodies);
let num = 10; let num = 10;
@@ -31,13 +27,13 @@ pub fn init_world(testbed: &mut Testbed) {
for j in 0..2 { for j in 0..2 {
for i in 0..=num { for i in 0..=num {
let x = (i as f32) - num as f32 / 2.0; let x = (i as f32) - num as f32 / 2.0;
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![
.translation(vector![x * 2.0, 10.0 * (j as f32 + 1.0), 0.0]) x * 2.0,
.build(); 10.0 * (j as f32 + 1.0),
0.0
]);
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::ball(rad) let collider = ColliderBuilder::ball(rad).restitution((i as f32) / (num as f32));
.restitution((i as f32) / (num as f32))
.build();
colliders.insert_with_parent(collider, handle, &mut bodies); colliders.insert_with_parent(collider, handle, &mut bodies);
} }
} }

21
examples3d/sensor3.rs
View File

@@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 200.1; let ground_size = 200.1;
let ground_height = 0.1; let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static() let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
.translation(vector![0.0, -ground_height, 0.0])
.build();
let ground_handle = bodies.insert(rigid_body); 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); 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; let z = k as f32 * shift - centerz;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
.translation(vector![x, y, z])
.build();
let handle = bodies.insert(rigid_body); 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); colliders.insert_with_parent(collider, handle, &mut bodies);
testbed.set_initial_body_color(handle, [0.5, 0.5, 1.0]); 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. // Rigid body so that the sensor can move.
let sensor = RigidBodyBuilder::new_dynamic() let sensor = RigidBodyBuilder::new_dynamic().translation(vector![0.0, 5.0, 0.0]);
.translation(vector![0.0, 5.0, 0.0])
.build();
let sensor_handle = bodies.insert(sensor); let sensor_handle = bodies.insert(sensor);
// Solid cube attached to the sensor which // Solid cube attached to the sensor which
// other colliders can touch. // 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); colliders.insert_with_parent(collider, sensor_handle, &mut bodies);
// We create a collider desc without density because we don't // 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) let sensor_collider = ColliderBuilder::ball(rad * 5.0)
.density(0.0) .density(0.0)
.sensor(true) .sensor(true)
.active_events(ActiveEvents::INTERSECTION_EVENTS) .active_events(ActiveEvents::INTERSECTION_EVENTS);
.build();
colliders.insert_with_parent(sensor_collider, sensor_handle, &mut bodies); colliders.insert_with_parent(sensor_collider, sensor_handle, &mut bodies);
testbed.set_initial_body_color(sensor_handle, [0.5, 1.0, 1.0]); testbed.set_initial_body_color(sensor_handle, [0.5, 1.0, 1.0]);

20
examples3d/trimesh3.rs
View File

@@ -36,9 +36,9 @@ pub fn init_world(testbed: &mut Testbed) {
let heightfield = HeightField::new(heights, ground_size); let heightfield = HeightField::new(heights, ground_size);
let (vertices, indices) = heightfield.to_trimesh(); 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 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); 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; let z = k as f32 * shift - centerz;
// Build the rigid body. // Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic() let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
.translation(vector![x, y, z])
.build();
let handle = bodies.insert(rigid_body); let handle = bodies.insert(rigid_body);
let collider = match j % 6 { let collider = match j % 6 {
0 => ColliderBuilder::cuboid(rad, rad, rad).build(), 0 => ColliderBuilder::cuboid(rad, rad, rad),
1 => ColliderBuilder::ball(rad).build(), 1 => ColliderBuilder::ball(rad),
// Rounded cylinders are much more efficient that cylinder, even if the // Rounded cylinders are much more efficient that cylinder, even if the
// rounding margin is small. // rounding margin is small.
2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0).build(), 2 => ColliderBuilder::round_cylinder(rad, rad, rad / 10.0),
3 => ColliderBuilder::cone(rad, rad).build(), 3 => ColliderBuilder::cone(rad, rad),
4 => ColliderBuilder::capsule_y(rad, rad).build(), 4 => ColliderBuilder::capsule_y(rad, rad),
_ => { _ => {
let shapes = vec![ let shapes = vec![
( (
@@ -89,7 +87,7 @@ pub fn init_world(testbed: &mut Testbed) {
), ),
]; ];
ColliderBuilder::compound(shapes).build() ColliderBuilder::compound(shapes)
} }
}; };

29
src/dynamics/integration_parameters.rs
View File

@@ -18,10 +18,6 @@ pub struct IntegrationParameters {
/// to numerical instabilities. /// to numerical instabilities.
pub min_ccd_dt: Real, 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) /// 0-1: multiplier for how much of the constraint violation (e.g. contact penetration)
/// will be compensated for during the velocity solve. /// will be compensated for during the velocity solve.
/// If zero, you need to enable the positional solver. /// If zero, you need to enable the positional solver.
@@ -35,6 +31,9 @@ pub struct IntegrationParameters {
/// (default `0.25`). /// (default `0.25`).
pub damping_ratio: Real, 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`). /// Amount of penetration the engine wont attempt to correct (default: `0.001m`).
pub allowed_linear_error: Real, pub allowed_linear_error: Real,
/// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`). /// 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. /// The ERP coefficient, multiplied by the inverse timestep length.
pub fn erp_inv_dt(&self) -> Real { 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. /// The CFM factor to be used in the constraints resolution.
pub fn cfm_factor(&self) -> Real { 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 inv_erp_minus_one = 1.0 / self.erp - 1.0;
// let stiffness = 4.0 * damping_ratio * damping_ratio * projected_mass // let stiffness = 4.0 * damping_ratio * damping_ratio * projected_mass
@@ -124,6 +128,16 @@ impl IntegrationParameters {
// in the constraints solver. // in the constraints solver.
1.0 / (1.0 + cfm_coeff) 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 { impl Default for IntegrationParameters {
@@ -131,9 +145,10 @@ impl Default for IntegrationParameters {
Self { Self {
dt: 1.0 / 60.0, dt: 1.0 / 60.0,
min_ccd_dt: 1.0 / 60.0 / 100.0, min_ccd_dt: 1.0 / 60.0 / 100.0,
velocity_solve_fraction: 1.0,
erp: 0.8, erp: 0.8,
damping_ratio: 0.25, damping_ratio: 0.25,
joint_erp: 1.0,
joint_damping_ratio: 1.0,
allowed_linear_error: 0.001, // 0.005 allowed_linear_error: 0.001, // 0.005
prediction_distance: 0.002, prediction_distance: 0.002,
max_velocity_iterations: 4, max_velocity_iterations: 4,

32
src/dynamics/island_manager.rs
View File

@@ -136,36 +136,6 @@ impl IslandManager {
.chain(self.active_kinematic_set.iter().copied()) .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<Set>(
&self,
island_id: usize,
bodies: &mut Set,
f: impl Fn(RigidBodyHandle, &mut RigidBody) + Send + Sync,
) where
Set: ComponentSet<T>,
{
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")] #[cfg(feature = "parallel")]
pub(crate) fn active_island_range(&self, island_id: usize) -> std::ops::Range<usize> { pub(crate) fn active_island_range(&self, island_id: usize) -> std::ops::Range<usize> {
self.active_islands[island_id]..self.active_islands[island_id + 1] 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 // NOTE: the `.rev()` is here so that two successive timesteps preserve
// the order of the bodies in the `active_dynamic_set` vec. This reversal // the order of the bodies in the `active_dynamic_set` vec. This reversal
// does not seem to affect performances nor stability. However it makes // 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() { for h in self.active_dynamic_set.drain(..).rev() {
let can_sleep = &mut self.can_sleep; let can_sleep = &mut self.can_sleep;
let stack = &mut self.stack; let stack = &mut self.stack;

93
src/dynamics/joint/fixed_joint.rs
View File

@@ -1,10 +1,11 @@
use crate::dynamics::{JointAxesMask, JointData}; use crate::dynamics::{GenericJoint, GenericJointBuilder, JointAxesMask};
use crate::math::{Isometry, Point, Real}; use crate::math::{Isometry, Point, Real};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)] #[derive(Copy, Clone, Debug, PartialEq)]
#[repr(transparent)]
pub struct FixedJoint { pub struct FixedJoint {
data: JointData, data: GenericJoint,
} }
impl Default for FixedJoint { impl Default for FixedJoint {
@@ -14,48 +15,100 @@ impl Default for FixedJoint {
} }
impl FixedJoint { impl FixedJoint {
#[must_use]
pub fn new() -> Self { pub fn new() -> Self {
#[cfg(feature = "dim2")] let data = GenericJointBuilder::new(JointAxesMask::LOCKED_FIXED_AXES).build();
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);
Self { data } Self { data }
} }
#[must_use]
pub fn local_frame1(&self) -> &Isometry<Real> {
&self.data.local_frame1
}
pub fn set_local_frame1(&mut self, local_frame: Isometry<Real>) -> &mut Self {
self.data.set_local_frame1(local_frame);
self
}
#[must_use]
pub fn local_frame2(&self) -> &Isometry<Real> {
&self.data.local_frame2
}
pub fn set_local_frame2(&mut self, local_frame: Isometry<Real>) -> &mut Self {
self.data.set_local_frame2(local_frame);
self
}
#[must_use]
pub fn local_anchor1(&self) -> Point<Real> {
self.data.local_anchor1()
}
pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
self.data.set_local_anchor1(anchor1);
self
}
#[must_use]
pub fn local_anchor2(&self) -> Point<Real> {
self.data.local_anchor2()
}
pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
self.data.set_local_anchor2(anchor2);
self
}
}
impl Into<GenericJoint> 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] #[must_use]
pub fn local_frame1(mut self, local_frame: Isometry<Real>) -> Self { pub fn local_frame1(mut self, local_frame: Isometry<Real>) -> Self {
self.data = self.data.local_frame1(local_frame); self.0.set_local_frame1(local_frame);
self self
} }
#[must_use] #[must_use]
pub fn local_frame2(mut self, local_frame: Isometry<Real>) -> Self { pub fn local_frame2(mut self, local_frame: Isometry<Real>) -> Self {
self.data = self.data.local_frame2(local_frame); self.0.set_local_frame2(local_frame);
self self
} }
#[must_use] #[must_use]
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self { pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
self.data = self.data.local_anchor1(anchor1); self.0.set_local_anchor1(anchor1);
self self
} }
#[must_use] #[must_use]
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self { pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
self.data = self.data.local_anchor2(anchor2); self.0.set_local_anchor2(anchor2);
self self
} }
#[must_use]
pub fn build(self) -> FixedJoint {
self.0
}
} }
impl Into<JointData> for FixedJoint { impl Into<GenericJoint> for FixedJointBuilder {
fn into(self) -> JointData { fn into(self) -> GenericJoint {
self.data self.0.into()
} }
} }

501
src/dynamics/joint/generic_joint.rs Normal file
View File

@@ -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<JointAxis> 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<N> {
pub min: N,
pub max: N,
pub impulse: N,
}
impl<N: SimdRealField<Element = Real>> Default for JointLimits<N> {
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<Real> {
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<Real>,
pub local_frame2: Isometry<Real>,
pub locked_axes: JointAxesMask,
pub limit_axes: JointAxesMask,
pub motor_axes: JointAxesMask,
pub limits: [JointLimits<Real>; 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<Real>) -> Rotation<Real> {
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<Real>) -> &mut Self {
self.local_frame1 = local_frame;
self
}
pub fn set_local_frame2(&mut self, local_frame: Isometry<Real>) -> &mut Self {
self.local_frame2 = local_frame;
self
}
#[must_use]
pub fn local_axis1(&self) -> UnitVector<Real> {
self.local_frame1 * Vector::x_axis()
}
pub fn set_local_axis1(&mut self, local_axis: UnitVector<Real>) -> &mut Self {
self.local_frame1.rotation = Self::complete_ang_frame(local_axis);
self
}
#[must_use]
pub fn local_axis2(&self) -> UnitVector<Real> {
self.local_frame2 * Vector::x_axis()
}
pub fn set_local_axis2(&mut self, local_axis: UnitVector<Real>) -> &mut Self {
self.local_frame2.rotation = Self::complete_ang_frame(local_axis);
self
}
#[must_use]
pub fn local_anchor1(&self) -> Point<Real> {
self.local_frame1.translation.vector.into()
}
pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
self.local_frame1.translation.vector = anchor1.coords;
self
}
#[must_use]
pub fn local_anchor2(&self) -> Point<Real> {
self.local_frame2.translation.vector.into()
}
pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
self.local_frame2.translation.vector = anchor2.coords;
self
}
#[must_use]
pub fn limits(&self, axis: JointAxis) -> Option<&JointLimits<Real>> {
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<MotorModel> {
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<Real>) -> Self {
self.0.set_local_frame1(local_frame);
self
}
#[must_use]
pub fn local_frame2(mut self, local_frame: Isometry<Real>) -> Self {
self.0.set_local_frame2(local_frame);
self
}
#[must_use]
pub fn local_axis1(mut self, local_axis: UnitVector<Real>) -> Self {
self.0.set_local_axis1(local_axis);
self
}
#[must_use]
pub fn local_axis2(mut self, local_axis: UnitVector<Real>) -> Self {
self.0.set_local_axis2(local_axis);
self
}
#[must_use]
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
self.0.set_local_anchor1(anchor1);
self
}
#[must_use]
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> 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
}
}

4
src/dynamics/joint/impulse_joint/impulse_joint.rs
View File

@@ -1,4 +1,4 @@
use crate::dynamics::{ImpulseJointHandle, JointData, RigidBodyHandle}; use crate::dynamics::{GenericJoint, ImpulseJointHandle, RigidBodyHandle};
use crate::math::{Real, SpacialVector}; use crate::math::{Real, SpacialVector};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -10,7 +10,7 @@ pub struct ImpulseJoint {
/// Handle to the second body attached to this joint. /// Handle to the second body attached to this joint.
pub body2: RigidBodyHandle, pub body2: RigidBodyHandle,
pub data: JointData, pub data: GenericJoint,
pub impulses: SpacialVector<Real>, pub impulses: SpacialVector<Real>,
// A joint needs to know its handle to simplify its removal. // A joint needs to know its handle to simplify its removal.

4
src/dynamics/joint/impulse_joint/impulse_joint_set.rs
View File

@@ -3,8 +3,8 @@ use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractio
use crate::data::arena::Arena; use crate::data::arena::Arena;
use crate::data::{BundleSet, Coarena, ComponentSet, ComponentSetMut}; use crate::data::{BundleSet, Coarena, ComponentSet, ComponentSetMut};
use crate::dynamics::{GenericJoint, RigidBodyHandle};
use crate::dynamics::{IslandManager, RigidBodyActivation, RigidBodyIds, RigidBodyType}; 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 joint added to the joint set.
/// The unique identifier of a collider added to a collider set. /// The unique identifier of a collider added to a collider set.
@@ -177,7 +177,7 @@ impl ImpulseJointSet {
&mut self, &mut self,
body1: RigidBodyHandle, body1: RigidBodyHandle,
body2: RigidBodyHandle, body2: RigidBodyHandle,
data: impl Into<JointData>, data: impl Into<GenericJoint>,
) -> ImpulseJointHandle { ) -> ImpulseJointHandle {
let data = data.into(); let data = data.into();
let handle = self.joint_ids.insert(0.into()); let handle = self.joint_ids.insert(0.into());

275
src/dynamics/joint/joint_data.rs
View File

@@ -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<JointAxis> 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<Real> {
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<Real>,
pub local_frame2: Isometry<Real>,
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<Real>) -> Rotation<Real> {
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<Real>) -> Self {
self.local_frame1 = local_frame;
self
}
#[must_use]
pub fn local_frame2(mut self, local_frame: Isometry<Real>) -> Self {
self.local_frame2 = local_frame;
self
}
#[must_use]
pub fn local_axis1(mut self, local_axis: UnitVector<Real>) -> Self {
self.local_frame1.rotation = Self::complete_ang_frame(local_axis);
self
}
#[must_use]
pub fn local_axis2(mut self, local_axis: UnitVector<Real>) -> Self {
self.local_frame2.rotation = Self::complete_ang_frame(local_axis);
self
}
#[must_use]
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
self.local_frame1.translation.vector = anchor1.coords;
self
}
#[must_use]
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> 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
}
}

12
src/dynamics/joint/mod.rs
View File

@@ -1,17 +1,17 @@
pub use self::fixed_joint::FixedJoint; pub use self::fixed_joint::*;
pub use self::impulse_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::motor_model::MotorModel;
pub use self::multibody_joint::*; pub use self::multibody_joint::*;
pub use self::prismatic_joint::PrismaticJoint; pub use self::prismatic_joint::*;
pub use self::revolute_joint::RevoluteJoint; pub use self::revolute_joint::*;
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
pub use self::spherical_joint::SphericalJoint; pub use self::spherical_joint::*;
mod fixed_joint; mod fixed_joint;
mod impulse_joint; mod impulse_joint;
mod joint_data; mod generic_joint;
mod motor_model; mod motor_model;
mod multibody_joint; mod multibody_joint;
mod prismatic_joint; mod prismatic_joint;

49
src/dynamics/joint/motor_model.rs
View File

@@ -5,57 +5,40 @@ use crate::math::Real;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
pub enum MotorModel { pub enum MotorModel {
/// The solved spring-like equation is: /// The solved spring-like equation is:
/// `delta_velocity(t + dt) = stiffness / dt * (target_pos - pos(t)) + damping * (target_vel - vel(t))` /// `acceleration = stiffness * (pos - target_pos) + damping * (vel - target_vel)`
///
/// 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))`
AccelerationBased, AccelerationBased,
// /// The solved spring-like equation is: /// The solved spring-like equation is:
// /// `force(t + dt) = stiffness * (target_pos - pos(t + dt)) + damping * (target_vel - vel(t + dt))` /// `force = stiffness * (pos - target_pos) + damping * (vel - target_vel)`
// ForceBased, ForceBased,
} }
impl Default for MotorModel { impl Default for MotorModel {
fn default() -> Self { fn default() -> Self {
MotorModel::VelocityBased MotorModel::AccelerationBased
} }
} }
impl MotorModel { impl MotorModel {
/// Combines the coefficients used for solving the spring equation. /// Combines the coefficients used for solving the spring equation.
/// ///
/// Returns the new coefficients (stiffness, damping, gamma, keep_inv_lhs) /// Returns the coefficients (erp_inv_dt, cfm_coeff, cfm_gain).
/// 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.
pub fn combine_coefficients( pub fn combine_coefficients(
self, self,
dt: Real, dt: Real,
stiffness: Real, stiffness: Real,
damping: Real, damping: Real,
) -> (Real, Real, Real, bool) { ) -> (Real, Real, Real) {
match self { match self {
MotorModel::VelocityBased => (stiffness * crate::utils::inv(dt), damping, 1.0, true),
MotorModel::AccelerationBased => { MotorModel::AccelerationBased => {
let effective_stiffness = stiffness * dt; let erp_inv_dt = stiffness * crate::utils::inv(dt * stiffness + damping);
let effective_damping = damping * dt; let cfm_coeff = crate::utils::inv(dt * dt * stiffness + dt * damping);
// TODO: Using gamma behaves very badly for some reasons. (erp_inv_dt, cfm_coeff, 0.0)
// Maybe I got the formulation wrong, so let's keep it to 1.0 for now, }
// and get back to this later. MotorModel::ForceBased => {
// let gamma = effective_stiffness * dt + effective_damping; let erp_inv_dt = stiffness * crate::utils::inv(dt * stiffness + damping);
(effective_stiffness, effective_damping, 1.0, true) let cfm_gain = crate::utils::inv(dt * dt * stiffness + dt * damping);
} // MotorModel::ForceBased => { (erp_inv_dt, 0.0, cfm_gain)
// let effective_stiffness = stiffness * dt; }
// let effective_damping = damping * dt;
// let gamma = effective_stiffness * dt + effective_damping;
// (effective_stiffness, effective_damping, gamma, false)
// }
} }
} }
} }

54
src/dynamics/joint/multibody_joint/multibody_joint.rs
View File

@@ -1,6 +1,6 @@
use crate::dynamics::solver::AnyJointVelocityConstraint; use crate::dynamics::solver::AnyJointVelocityConstraint;
use crate::dynamics::{ use crate::dynamics::{
joint, FixedJoint, IntegrationParameters, JointData, Multibody, MultibodyLink, joint, FixedJointBuilder, GenericJoint, IntegrationParameters, Multibody, MultibodyLink,
RigidBodyVelocity, RigidBodyVelocity,
}; };
use crate::math::{ use crate::math::{
@@ -14,13 +14,13 @@ use na::{UnitQuaternion, Vector3};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug)] #[derive(Copy, Clone, Debug)]
pub struct MultibodyJoint { pub struct MultibodyJoint {
pub data: JointData, pub data: GenericJoint,
pub(crate) coords: SpacialVector<Real>, pub(crate) coords: SpacialVector<Real>,
pub(crate) joint_rot: Rotation<Real>, pub(crate) joint_rot: Rotation<Real>,
} }
impl MultibodyJoint { impl MultibodyJoint {
pub fn new(data: JointData) -> Self { pub fn new(data: GenericJoint) -> Self {
Self { Self {
data, data,
coords: na::zero(), coords: na::zero(),
@@ -29,13 +29,13 @@ impl MultibodyJoint {
} }
pub(crate) fn free(pos: Isometry<Real>) -> Self { pub(crate) fn free(pos: Isometry<Real>) -> Self {
let mut result = Self::new(JointData::default()); let mut result = Self::new(GenericJoint::default());
result.set_free_pos(pos); result.set_free_pos(pos);
result result
} }
pub(crate) fn fixed(pos: Isometry<Real>) -> Self { pub(crate) fn fixed(pos: Isometry<Real>) -> 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<Real>) { pub(crate) fn set_free_pos(&mut self, pos: Isometry<Real>) {
@@ -263,19 +263,11 @@ impl MultibodyJoint {
for i in 0..DIM { for i in 0..DIM {
if (locked_bits & (1 << i)) == 0 { if (locked_bits & (1 << i)) == 0 {
if (limit_bits & (1 << i)) != 0 { let limits = if (limit_bits & (1 << i)) != 0 {
joint::unit_joint_limit_constraint( Some([self.data.limits[i].min, self.data.limits[i].max])
params, } else {
multibody, None
link, };
[self.data.limits[i].min, self.data.limits[i].max],
self.coords[i],
dof_id + curr_free_dof,
j_id,
jacobians,
constraints,
);
}
if (motor_bits & (1 << i)) != 0 { if (motor_bits & (1 << i)) != 0 {
joint::unit_joint_motor_constraint( joint::unit_joint_motor_constraint(
@@ -284,6 +276,21 @@ impl MultibodyJoint {
link, link,
&self.data.motors[i], &self.data.motors[i],
self.coords[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, dof_id + curr_free_dof,
j_id, j_id,
jacobians, jacobians,
@@ -310,19 +317,23 @@ impl MultibodyJoint {
// TODO: we should make special cases for multi-angular-dofs limits/motors // TODO: we should make special cases for multi-angular-dofs limits/motors
for i in DIM..SPATIAL_DIM { for i in DIM..SPATIAL_DIM {
if (locked_bits & (1 << i)) == 0 { 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( joint::unit_joint_limit_constraint(
params, params,
multibody, multibody,
link, link,
[self.data.limits[i].min, self.data.limits[i].max], limits,
self.coords[i], self.coords[i],
dof_id + curr_free_dof, dof_id + curr_free_dof,
j_id, j_id,
jacobians, jacobians,
constraints, constraints,
); );
} Some(limits)
} else {
None
};
if (motor_bits & (1 << i)) != 0 { if (motor_bits & (1 << i)) != 0 {
joint::unit_joint_motor_constraint( joint::unit_joint_motor_constraint(
@@ -331,6 +342,7 @@ impl MultibodyJoint {
link, link,
&self.data.motors[i], &self.data.motors[i],
self.coords[i], self.coords[i],
limits,
dof_id + curr_free_dof, dof_id + curr_free_dof,
j_id, j_id,
jacobians, jacobians,

4
src/dynamics/joint/multibody_joint/multibody_joint_set.rs
View File

@@ -1,7 +1,7 @@
use crate::data::{Arena, Coarena, ComponentSet, ComponentSetMut, Index}; use crate::data::{Arena, Coarena, ComponentSet, ComponentSetMut, Index};
use crate::dynamics::joint::MultibodyLink; use crate::dynamics::joint::MultibodyLink;
use crate::dynamics::{ use crate::dynamics::{
IslandManager, JointData, Multibody, MultibodyJoint, RigidBodyActivation, RigidBodyHandle, GenericJoint, IslandManager, Multibody, MultibodyJoint, RigidBodyActivation, RigidBodyHandle,
RigidBodyIds, RigidBodyType, RigidBodyIds, RigidBodyType,
}; };
use crate::geometry::{InteractionGraph, RigidBodyGraphIndex}; use crate::geometry::{InteractionGraph, RigidBodyGraphIndex};
@@ -112,7 +112,7 @@ impl MultibodyJointSet {
&mut self, &mut self,
body1: RigidBodyHandle, body1: RigidBodyHandle,
body2: RigidBodyHandle, body2: RigidBodyHandle,
data: impl Into<JointData>, data: impl Into<GenericJoint>,
) -> Option<MultibodyJointHandle> { ) -> Option<MultibodyJointHandle> {
let data = data.into(); let data = data.into();
let link1 = self.rb2mb.get(body1.0).copied().unwrap_or_else(|| { let link1 = self.rb2mb.get(body1.0).copied().unwrap_or_else(|| {

25
src/dynamics/joint/multibody_joint/unit_multibody_joint.rs
View File

@@ -26,7 +26,8 @@ pub fn unit_joint_limit_constraint(
let min_enabled = curr_pos < limits[0]; let min_enabled = curr_pos < limits[0];
let max_enabled = limits[1] < curr_pos; 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_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]; let rhs_wo_bias = joint_velocity[dof_id];
@@ -54,6 +55,8 @@ pub fn unit_joint_limit_constraint(
inv_lhs: crate::utils::inv(lhs), inv_lhs: crate::utils::inv(lhs),
rhs: rhs_wo_bias + rhs_bias, rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias, rhs_wo_bias,
cfm_coeff,
cfm_gain: 0.0,
writeback_id: WritebackId::Limit(dof_id), writeback_id: WritebackId::Limit(dof_id),
}; };
@@ -71,11 +74,13 @@ pub fn unit_joint_motor_constraint(
link: &MultibodyLink, link: &MultibodyLink,
motor: &JointMotor, motor: &JointMotor,
curr_pos: Real, curr_pos: Real,
limits: Option<[Real; 2]>,
dof_id: usize, dof_id: usize,
j_id: &mut usize, j_id: &mut usize,
jacobians: &mut DVector<Real>, jacobians: &mut DVector<Real>,
constraints: &mut Vec<AnyJointVelocityConstraint>, constraints: &mut Vec<AnyJointVelocityConstraint>,
) { ) {
let inv_dt = params.inv_dt();
let ndofs = multibody.ndofs(); let ndofs = multibody.ndofs();
let joint_velocity = multibody.joint_velocity(link); 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 impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
let mut rhs_wo_bias = 0.0; let mut rhs_wo_bias = 0.0;
if motor_params.stiffness != 0.0 { if motor_params.erp_inv_dt != 0.0 {
rhs_wo_bias += (curr_pos - motor_params.target_pos) * motor_params.stiffness; rhs_wo_bias += (curr_pos - motor_params.target_pos) * motor_params.erp_inv_dt;
} }
if motor_params.damping != 0.0 { 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]; let dvel = joint_velocity[dof_id];
rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping; rhs_wo_bias += dvel - target_vel;
}
let constraint = JointGenericVelocityGroundConstraint { let constraint = JointGenericVelocityGroundConstraint {
mj_lambda2: multibody.solver_id, mj_lambda2: multibody.solver_id,
@@ -109,6 +120,8 @@ pub fn unit_joint_motor_constraint(
joint_id: usize::MAX, joint_id: usize::MAX,
impulse: 0.0, impulse: 0.0,
impulse_bounds, impulse_bounds,
cfm_coeff: motor_params.cfm_coeff,
cfm_gain: motor_params.cfm_gain,
inv_lhs: crate::utils::inv(lhs), inv_lhs: crate::utils::inv(lhs),
rhs: rhs_wo_bias, rhs: rhs_wo_bias,
rhs_wo_bias, rhs_wo_bias,

194
src/dynamics/joint/prismatic_joint.rs
View File

@@ -1,91 +1,215 @@
use crate::dynamics::joint::{JointAxesMask, JointData}; use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask};
use crate::dynamics::{JointAxis, MotorModel}; use crate::dynamics::{JointAxis, MotorModel};
use crate::math::{Point, Real, UnitVector}; use crate::math::{Point, Real, UnitVector};
use super::{JointLimits, JointMotor};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)] #[derive(Copy, Clone, Debug, PartialEq)]
#[repr(transparent)]
pub struct PrismaticJoint { pub struct PrismaticJoint {
data: JointData, data: GenericJoint,
} }
impl PrismaticJoint { impl PrismaticJoint {
pub fn new(axis: UnitVector<Real>) -> Self { pub fn new(axis: UnitVector<Real>) -> Self {
#[cfg(feature = "dim2")] let data = GenericJointBuilder::new(JointAxesMask::LOCKED_PRISMATIC_AXES)
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)
.local_axis1(axis) .local_axis1(axis)
.local_axis2(axis); .local_axis2(axis)
.build();
Self { data } Self { data }
} }
#[must_use]
pub fn local_anchor1(&self) -> Point<Real> {
self.data.local_anchor1()
}
pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
self.data.set_local_anchor1(anchor1);
self
}
#[must_use]
pub fn local_anchor2(&self) -> Point<Real> {
self.data.local_anchor2()
}
pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &mut Self {
self.data.set_local_anchor2(anchor2);
self
}
#[must_use]
pub fn local_axis1(&self) -> UnitVector<Real> {
self.data.local_axis1()
}
pub fn set_local_axis1(&mut self, axis1: UnitVector<Real>) -> &mut Self {
self.data.set_local_axis1(axis1);
self
}
#[must_use]
pub fn local_axis2(&self) -> UnitVector<Real> {
self.data.local_axis2()
}
pub fn set_local_axis2(&mut self, axis2: UnitVector<Real>) -> &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<Real>> {
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<GenericJoint> for PrismaticJoint {
fn into(self) -> GenericJoint {
self.data
}
}
pub struct PrismaticJointBuilder(PrismaticJoint);
impl PrismaticJointBuilder {
pub fn new(axis: UnitVector<Real>) -> Self {
Self(PrismaticJoint::new(axis))
}
#[must_use] #[must_use]
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self { pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
self.data = self.data.local_anchor1(anchor1); self.0.set_local_anchor1(anchor1);
self self
} }
#[must_use] #[must_use]
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self { pub fn local_anchor2(mut self, anchor2: Point<Real>) -> 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<Real>) -> Self {
self.0.set_local_axis1(axis1);
self
}
#[must_use]
pub fn local_axis2(mut self, axis2: UnitVector<Real>) -> Self {
self.0.set_local_axis2(axis2);
self self
} }
/// Set the spring-like model used by the motor to reach the desired target velocity and position. /// 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 { 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 self
} }
/// Sets the target velocity this motor needs to reach. /// Sets the target velocity this motor needs to reach.
#[must_use]
pub fn motor_velocity(mut self, target_vel: Real, factor: Real) -> Self { 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 self
} }
/// Sets the target angle this motor needs to reach. /// 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 { pub fn motor_position(mut self, target_pos: Real, stiffness: Real, damping: Real) -> Self {
self.data = self self.0.set_motor_position(target_pos, stiffness, damping);
.data
.motor_position(JointAxis::X, target_pos, stiffness, damping);
self self
} }
/// Configure both the target angle and target velocity of the motor. /// Configure both the target angle and target velocity of the motor.
pub fn motor_axis( #[must_use]
pub fn set_motor(
mut self, mut self,
target_pos: Real, target_pos: Real,
target_vel: Real, target_vel: Real,
stiffness: Real, stiffness: Real,
damping: Real, damping: Real,
) -> Self { ) -> Self {
self.data = self self.0.set_motor(target_pos, target_vel, stiffness, damping);
.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 self
} }
#[must_use] #[must_use]
pub fn limit_axis(mut self, limits: [Real; 2]) -> Self { pub fn motor_max_force(mut self, max_force: Real) -> Self {
self.data = self.data.limit_axis(JointAxis::X, limits); self.0.set_motor_max_force(max_force);
self 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<JointData> for PrismaticJoint { impl Into<GenericJoint> for PrismaticJointBuilder {
fn into(self) -> JointData { fn into(self) -> GenericJoint {
self.data self.0.into()
} }
} }

177
src/dynamics/joint/revolute_joint.rs
View File

@@ -1,5 +1,5 @@
use crate::dynamics::joint::{JointAxesMask, JointData}; use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask};
use crate::dynamics::{JointAxis, MotorModel}; use crate::dynamics::{JointAxis, JointLimits, JointMotor, MotorModel};
use crate::math::{Point, Real}; use crate::math::{Point, Real};
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
@@ -7,100 +7,197 @@ use crate::math::UnitVector;
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)] #[derive(Copy, Clone, Debug, PartialEq)]
#[repr(transparent)]
pub struct RevoluteJoint { pub struct RevoluteJoint {
data: JointData, data: GenericJoint,
} }
impl RevoluteJoint { impl RevoluteJoint {
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
pub fn new() -> Self { pub fn new() -> Self {
let mask = JointAxesMask::X | JointAxesMask::Y; let data = GenericJointBuilder::new(JointAxesMask::LOCKED_REVOLUTE_AXES);
Self { data: data.build() }
let data = JointData::default().lock_axes(mask);
Self { data }
} }
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
pub fn new(axis: UnitVector<Real>) -> Self { pub fn new(axis: UnitVector<Real>) -> Self {
let mask = JointAxesMask::X let data = GenericJointBuilder::new(JointAxesMask::LOCKED_REVOLUTE_AXES)
| JointAxesMask::Y
| JointAxesMask::Z
| JointAxesMask::ANG_Y
| JointAxesMask::ANG_Z;
let data = JointData::default()
.lock_axes(mask)
.local_axis1(axis) .local_axis1(axis)
.local_axis2(axis); .local_axis2(axis)
.build();
Self { data } Self { data }
} }
pub fn data(&self) -> &JointData { pub fn data(&self) -> &GenericJoint {
&self.data &self.data
} }
#[must_use]
pub fn local_anchor1(&self) -> Point<Real> {
self.data.local_anchor1()
}
pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
self.data.set_local_anchor1(anchor1);
self
}
#[must_use]
pub fn local_anchor2(&self) -> Point<Real> {
self.data.local_anchor2()
}
pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &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<Real>> {
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<GenericJoint> 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<Real>) -> Self {
Self(RevoluteJoint::new(axis))
}
#[must_use] #[must_use]
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self { pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
self.data = self.data.local_anchor1(anchor1); self.0.set_local_anchor1(anchor1);
self self
} }
#[must_use] #[must_use]
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self { pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
self.data = self.data.local_anchor2(anchor2); self.0.set_local_anchor2(anchor2);
self self
} }
/// Set the spring-like model used by the motor to reach the desired target velocity and position. /// 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 { 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 self
} }
/// Sets the target velocity this motor needs to reach. /// Sets the target velocity this motor needs to reach.
#[must_use]
pub fn motor_velocity(mut self, target_vel: Real, factor: Real) -> Self { pub fn motor_velocity(mut self, target_vel: Real, factor: Real) -> Self {
self.data = self self.0.set_motor_velocity(target_vel, factor);
.data
.motor_velocity(JointAxis::AngX, target_vel, factor);
self self
} }
/// Sets the target angle this motor needs to reach. /// 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 { pub fn motor_position(mut self, target_pos: Real, stiffness: Real, damping: Real) -> Self {
self.data = self self.0.set_motor_position(target_pos, stiffness, damping);
.data
.motor_position(JointAxis::AngX, target_pos, stiffness, damping);
self self
} }
/// Configure both the target angle and target velocity of the motor. /// Configure both the target angle and target velocity of the motor.
pub fn motor_axis( #[must_use]
pub fn motor(
mut self, mut self,
target_pos: Real, target_pos: Real,
target_vel: Real, target_vel: Real,
stiffness: Real, stiffness: Real,
damping: Real, damping: Real,
) -> Self { ) -> Self {
self.data = self.0.set_motor(target_pos, target_vel, stiffness, damping);
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 self
} }
#[must_use] #[must_use]
pub fn limit_axis(mut self, limits: [Real; 2]) -> Self { pub fn motor_max_force(mut self, max_force: Real) -> Self {
self.data = self.data.limit_axis(JointAxis::AngX, limits); self.0.set_motor_max_force(max_force);
self 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<JointData> for RevoluteJoint { impl Into<GenericJoint> for RevoluteJointBuilder {
fn into(self) -> JointData { fn into(self) -> GenericJoint {
self.data self.0.into()
} }
} }

146
src/dynamics/joint/spherical_joint.rs
View File

@@ -1,11 +1,12 @@
use crate::dynamics::joint::{JointAxesMask, JointData}; use crate::dynamics::joint::{GenericJoint, GenericJointBuilder, JointAxesMask};
use crate::dynamics::{JointAxis, MotorModel}; use crate::dynamics::{JointAxis, MotorModel};
use crate::math::{Point, Real}; use crate::math::{Point, Real};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)] #[derive(Copy, Clone, Debug, PartialEq)]
#[repr(transparent)]
pub struct SphericalJoint { pub struct SphericalJoint {
data: JointData, data: GenericJoint,
} }
impl Default for SphericalJoint { impl Default for SphericalJoint {
@@ -16,40 +17,128 @@ impl Default for SphericalJoint {
impl SphericalJoint { impl SphericalJoint {
pub fn new() -> Self { pub fn new() -> Self {
let data = let data = GenericJointBuilder::new(JointAxesMask::LOCKED_SPHERICAL_AXES).build();
JointData::default().lock_axes(JointAxesMask::X | JointAxesMask::Y | JointAxesMask::Z);
Self { data } Self { data }
} }
pub fn data(&self) -> &JointData { pub fn data(&self) -> &GenericJoint {
&self.data &self.data
} }
pub fn set_local_anchor1(&mut self, anchor1: Point<Real>) -> &mut Self {
self.data.set_local_anchor1(anchor1);
self
}
pub fn set_local_anchor2(&mut self, anchor2: Point<Real>) -> &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<GenericJoint> 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] #[must_use]
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self { pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
self.data = self.data.local_anchor1(anchor1); self.0.set_local_anchor1(anchor1);
self self
} }
#[must_use] #[must_use]
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self { pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
self.data = self.data.local_anchor2(anchor2); self.0.set_local_anchor2(anchor2);
self self
} }
/// Set the spring-like model used by the motor to reach the desired target velocity and position. /// 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 { 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 self
} }
/// Sets the target velocity this motor needs to reach. /// 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 { 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 self
} }
/// Sets the target angle this motor needs to reach. /// Sets the target angle this motor needs to reach.
#[must_use]
pub fn motor_position( pub fn motor_position(
mut self, mut self,
axis: JointAxis, axis: JointAxis,
@@ -57,14 +146,14 @@ impl SphericalJoint {
stiffness: Real, stiffness: Real,
damping: Real, damping: Real,
) -> Self { ) -> Self {
self.data = self self.0
.data .set_motor_position(axis, target_pos, stiffness, damping);
.motor_position(axis, target_pos, stiffness, damping);
self self
} }
/// Configure both the target angle and target velocity of the motor. /// Configure both the target angle and target velocity of the motor.
pub fn motor_axis( #[must_use]
pub fn motor(
mut self, mut self,
axis: JointAxis, axis: JointAxis,
target_pos: Real, target_pos: Real,
@@ -72,26 +161,31 @@ impl SphericalJoint {
stiffness: Real, stiffness: Real,
damping: Real, damping: Real,
) -> Self { ) -> Self {
self.data = self self.0
.data .set_motor(axis, target_pos, target_vel, stiffness, damping);
.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 self
} }
#[must_use] #[must_use]
pub fn limit_axis(mut self, axis: JointAxis, limits: [Real; 2]) -> Self { pub fn motor_max_force(mut self, axis: JointAxis, max_force: Real) -> Self {
self.data = self.data.limit_axis(axis, limits); self.0.set_motor_max_force(axis, max_force);
self 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<JointData> for SphericalJoint { impl Into<GenericJoint> for SphericalJointBuilder {
fn into(self) -> JointData { fn into(self) -> GenericJoint {
self.data self.0.into()
} }
} }

6
src/dynamics/rigid_body.rs
View File

@@ -1096,3 +1096,9 @@ impl RigidBodyBuilder {
rb rb
} }
} }
impl Into<RigidBody> for RigidBodyBuilder {
fn into(self) -> RigidBody {
self.build()
}
}

3
src/dynamics/rigid_body_set.rs
View File

@@ -121,7 +121,8 @@ impl RigidBodySet {
} }
/// Insert a rigid body into this set and retrieve its handle. /// 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<RigidBody>) -> RigidBodyHandle {
let mut rb = rb.into();
// Make sure the internal links are reset, they may not be // Make sure the internal links are reset, they may not be
// if this rigid-body was obtained by cloning another one. // if this rigid-body was obtained by cloning another one.
rb.reset_internal_references(); rb.reset_internal_references();

2
src/dynamics/solver/generic_velocity_constraint.rs
View File

@@ -261,7 +261,7 @@ impl GenericVelocityConstraint {
let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution) let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
* (vel1 - vel2).dot(&force_dir1); * (vel1 - vel2).dot(&force_dir1);
rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt; 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 = let rhs_bias =
/* is_resting * */ erp_inv_dt * manifold_point.dist.min(0.0); /* is_resting * */ erp_inv_dt * manifold_point.dist.min(0.0);

2
src/dynamics/solver/generic_velocity_ground_constraint.rs
View File

@@ -145,7 +145,7 @@ impl GenericVelocityGroundConstraint {
let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution) let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
* (vel1 - vel2).dot(&force_dir1); * (vel1 - vel2).dot(&force_dir1);
rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt; 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 = let rhs_bias =
/* is_resting * */ erp_inv_dt * manifold_point.dist.min(0.0); /* is_resting * */ erp_inv_dt * manifold_point.dist.min(0.0);

2
src/dynamics/solver/island_solver.rs
View File

@@ -1,6 +1,6 @@
use super::VelocitySolver; use super::VelocitySolver;
use crate::counters::Counters; use crate::counters::Counters;
use crate::data::{BundleSet, ComponentSet, ComponentSetMut}; use crate::data::{ComponentSet, ComponentSetMut};
use crate::dynamics::solver::{ use crate::dynamics::solver::{
AnyGenericVelocityConstraint, AnyJointVelocityConstraint, AnyVelocityConstraint, AnyGenericVelocityConstraint, AnyJointVelocityConstraint, AnyVelocityConstraint,
SolverConstraints, SolverConstraints,

151
src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint.rs
View File

@@ -1,7 +1,7 @@
use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::WritebackId; use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::WritebackId;
use crate::dynamics::solver::joint_constraint::{JointVelocityConstraintBuilder, SolverBody}; use crate::dynamics::solver::joint_constraint::{JointVelocityConstraintBuilder, SolverBody};
use crate::dynamics::solver::DeltaVel; 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::math::{Isometry, Real, DIM};
use crate::prelude::SPATIAL_DIM; use crate::prelude::SPATIAL_DIM;
use na::{DVector, DVectorSlice, DVectorSliceMut}; use na::{DVector, DVectorSlice, DVectorSliceMut};
@@ -25,6 +25,8 @@ pub struct JointGenericVelocityConstraint {
pub inv_lhs: Real, pub inv_lhs: Real,
pub rhs: Real, pub rhs: Real,
pub rhs_wo_bias: Real, pub rhs_wo_bias: Real,
pub cfm_coeff: Real,
pub cfm_gain: Real,
pub writeback_id: WritebackId, pub writeback_id: WritebackId,
} }
@@ -52,6 +54,8 @@ impl JointGenericVelocityConstraint {
inv_lhs: 0.0, inv_lhs: 0.0,
rhs: 0.0, rhs: 0.0,
rhs_wo_bias: 0.0, rhs_wo_bias: 0.0,
cfm_coeff: 0.0,
cfm_gain: 0.0,
writeback_id: WritebackId::Dof(0), writeback_id: WritebackId::Dof(0),
} }
} }
@@ -65,7 +69,7 @@ impl JointGenericVelocityConstraint {
mb2: Option<(&Multibody, usize)>, mb2: Option<(&Multibody, usize)>,
frame1: &Isometry<Real>, frame1: &Isometry<Real>,
frame2: &Isometry<Real>, frame2: &Isometry<Real>,
joint: &JointData, joint: &GenericJoint,
jacobians: &mut DVector<Real>, jacobians: &mut DVector<Real>,
j_id: &mut usize, j_id: &mut usize,
out: &mut [Self], out: &mut [Self],
@@ -83,26 +87,10 @@ impl JointGenericVelocityConstraint {
locked_axes, locked_axes,
); );
for i in 0..DIM { let start = len;
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;
}
}
for i in DIM..SPATIAL_DIM { for i in DIM..SPATIAL_DIM {
if locked_axes & (1 << i) != 0 { if (motor_axes >> DIM) & (1 << i) != 0 {
out[len] = builder.lock_angular_generic( out[len] = builder.motor_angular_generic(
params, params,
jacobians, jacobians,
j_id, j_id,
@@ -112,12 +100,12 @@ impl JointGenericVelocityConstraint {
mb1, mb1,
mb2, mb2,
i - DIM, i - DIM,
WritebackId::Dof(i), &joint.motors[i].motor_params(params.dt),
WritebackId::Motor(i),
); );
len += 1; len += 1;
} }
} }
for i in 0..DIM { for i in 0..DIM {
if motor_axes & (1 << i) != 0 { if motor_axes & (1 << i) != 0 {
out[len] = builder.motor_linear_generic( out[len] = builder.motor_linear_generic(
@@ -137,10 +125,15 @@ impl JointGenericVelocityConstraint {
len += 1; len += 1;
} }
} }
JointVelocityConstraintBuilder::finalize_generic_constraints(
jacobians,
&mut out[start..len],
);
let start = len;
for i in DIM..SPATIAL_DIM { for i in DIM..SPATIAL_DIM {
if (motor_axes >> DIM) & (1 << i) != 0 { if locked_axes & (1 << i) != 0 {
out[len] = builder.motor_angular_generic( out[len] = builder.lock_angular_generic(
params, params,
jacobians, jacobians,
j_id, j_id,
@@ -150,16 +143,14 @@ impl JointGenericVelocityConstraint {
mb1, mb1,
mb2, mb2,
i - DIM, i - DIM,
&joint.motors[i].motor_params(params.dt), WritebackId::Dof(i),
WritebackId::Motor(i),
); );
len += 1; len += 1;
} }
} }
for i in 0..DIM { for i in 0..DIM {
if limit_axes & (1 << i) != 0 { if locked_axes & (1 << i) != 0 {
out[len] = builder.limit_linear_generic( out[len] = builder.lock_linear_generic(
params, params,
jacobians, jacobians,
j_id, j_id,
@@ -169,8 +160,7 @@ impl JointGenericVelocityConstraint {
mb1, mb1,
mb2, mb2,
i, i,
[joint.limits[i].min, joint.limits[i].max], WritebackId::Dof(i),
WritebackId::Limit(i),
); );
len += 1; len += 1;
} }
@@ -194,8 +184,29 @@ impl JointGenericVelocityConstraint {
len += 1; 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 len
} }
@@ -273,7 +284,7 @@ impl JointGenericVelocityConstraint {
let dvel = self.rhs + (vel2 - vel1); let dvel = self.rhs + (vel2 - vel1);
let total_impulse = na::clamp( 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[0],
self.impulse_bounds[1], self.impulse_bounds[1],
); );
@@ -316,6 +327,8 @@ pub struct JointGenericVelocityGroundConstraint {
pub inv_lhs: Real, pub inv_lhs: Real,
pub rhs: Real, pub rhs: Real,
pub rhs_wo_bias: Real, pub rhs_wo_bias: Real,
pub cfm_coeff: Real,
pub cfm_gain: Real,
pub writeback_id: WritebackId, pub writeback_id: WritebackId,
} }
@@ -338,6 +351,8 @@ impl JointGenericVelocityGroundConstraint {
inv_lhs: 0.0, inv_lhs: 0.0,
rhs: 0.0, rhs: 0.0,
rhs_wo_bias: 0.0, rhs_wo_bias: 0.0,
cfm_coeff: 0.0,
cfm_gain: 0.0,
writeback_id: WritebackId::Dof(0), writeback_id: WritebackId::Dof(0),
} }
} }
@@ -350,7 +365,7 @@ impl JointGenericVelocityGroundConstraint {
mb2: (&Multibody, usize), mb2: (&Multibody, usize),
frame1: &Isometry<Real>, frame1: &Isometry<Real>,
frame2: &Isometry<Real>, frame2: &Isometry<Real>,
joint: &JointData, joint: &GenericJoint,
jacobians: &mut DVector<Real>, jacobians: &mut DVector<Real>,
j_id: &mut usize, j_id: &mut usize,
out: &mut [Self], out: &mut [Self],
@@ -368,32 +383,20 @@ impl JointGenericVelocityGroundConstraint {
locked_axes, locked_axes,
); );
for i in 0..DIM { let start = len;
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;
}
}
for i in DIM..SPATIAL_DIM { for i in DIM..SPATIAL_DIM {
if locked_axes & (1 << i) != 0 { if (motor_axes >> DIM) & (1 << i) != 0 {
out[len] = builder.lock_angular_generic_ground( out[len] = builder.motor_angular_generic_ground(
params, params,
jacobians, jacobians,
j_id, j_id,
joint_id, joint_id,
body1, body1,
body2,
mb2, mb2,
i - DIM, i - DIM,
WritebackId::Dof(i), &joint.motors[i].motor_params(params.dt),
WritebackId::Motor(i),
); );
len += 1; 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 { for i in DIM..SPATIAL_DIM {
if (motor_axes >> DIM) & (1 << i) != 0 { if locked_axes & (1 << i) != 0 {
out[len] = builder.motor_angular_generic_ground( out[len] = builder.lock_angular_generic_ground(
params, params,
jacobians, jacobians,
j_id, j_id,
joint_id, joint_id,
body1, body1,
body2,
mb2, mb2,
i - DIM, i - DIM,
&joint.motors[i].motor_params(params.dt), WritebackId::Dof(i),
WritebackId::Motor(i),
); );
len += 1; len += 1;
} }
} }
for i in 0..DIM { for i in 0..DIM {
if limit_axes & (1 << i) != 0 { if locked_axes & (1 << i) != 0 {
out[len] = builder.limit_linear_generic_ground( out[len] = builder.lock_linear_generic_ground(
params, params,
jacobians, jacobians,
j_id, j_id,
@@ -446,8 +452,7 @@ impl JointGenericVelocityGroundConstraint {
body1, body1,
mb2, mb2,
i, i,
[joint.limits[i].min, joint.limits[i].max], WritebackId::Dof(i),
WritebackId::Limit(i),
); );
len += 1; len += 1;
} }
@@ -469,10 +474,26 @@ impl JointGenericVelocityGroundConstraint {
len += 1; 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( JointVelocityConstraintBuilder::finalize_generic_constraints_ground(
jacobians, jacobians,
&mut out[..len], &mut out[start..len],
); );
len len
} }
@@ -511,7 +532,7 @@ impl JointGenericVelocityGroundConstraint {
let dvel = self.rhs + vel2; let dvel = self.rhs + vel2;
let total_impulse = na::clamp( 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[0],
self.impulse_bounds[1], self.impulse_bounds[1],
); );

86
src/dynamics/solver/joint_constraint/joint_generic_velocity_constraint_builder.rs
View File

@@ -113,7 +113,7 @@ impl JointVelocityConstraintBuilder<Real> {
j.copy_from(&wj); 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_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]); let mj_lambda2 = mb2.map(|m| m.0.solver_id).unwrap_or(body2.mj_lambda[0]);
@@ -133,6 +133,8 @@ impl JointVelocityConstraintBuilder<Real> {
inv_lhs: 0.0, inv_lhs: 0.0,
rhs: rhs_wo_bias, rhs: rhs_wo_bias,
rhs_wo_bias, rhs_wo_bias,
cfm_coeff: 0.0,
cfm_gain: 0.0,
writeback_id, writeback_id,
} }
} }
@@ -169,7 +171,7 @@ impl JointVelocityConstraintBuilder<Real> {
ang_jac2, 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; let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt;
c.rhs += rhs_bias; c.rhs += rhs_bias;
c c
@@ -212,7 +214,7 @@ impl JointVelocityConstraintBuilder<Real> {
let min_enabled = dist < limits[0]; let min_enabled = dist < limits[0];
let max_enabled = limits[1] < dist; 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; let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt;
constraint.rhs += rhs_bias; constraint.rhs += rhs_bias;
constraint.impulse_bounds = [ constraint.impulse_bounds = [
@@ -265,20 +267,20 @@ impl JointVelocityConstraintBuilder<Real> {
); );
let mut rhs_wo_bias = 0.0; 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); 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)) let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping; rhs_wo_bias += dvel - motor_params.target_vel;
}
constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse]; constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
constraint.rhs = rhs_wo_bias; constraint.rhs = rhs_wo_bias;
constraint.rhs_wo_bias = 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 constraint
} }
@@ -312,7 +314,7 @@ impl JointVelocityConstraintBuilder<Real> {
ang_jac, ang_jac,
); );
let erp_inv_dt = params.erp_inv_dt(); let erp_inv_dt = params.joint_erp_inv_dt();
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let rhs_bias = self.ang_err.im * erp_inv_dt; let rhs_bias = self.ang_err.im * erp_inv_dt;
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
@@ -364,7 +366,7 @@ impl JointVelocityConstraintBuilder<Real> {
max_enabled as u32 as Real * Real::MAX, 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 = let rhs_bias =
((s_ang - s_limits[1]).max(0.0) - (s_limits[0] - s_ang).max(0.0)) * erp_inv_dt; ((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<Real> {
); );
let mut rhs_wo_bias = 0.0; let mut rhs_wo_bias = 0.0;
if motor_params.stiffness != 0.0 { if motor_params.erp_inv_dt != 0.0 {
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let s_ang_dist = self.ang_err.im; let s_ang_dist = self.ang_err.im;
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
let s_ang_dist = self.ang_err.imag()[_motor_axis]; let s_ang_dist = self.ang_err.imag()[_motor_axis];
let s_target_ang = motor_params.target_pos.sin(); 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); 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; rhs_wo_bias += dvel - motor_params.target_vel;
}
constraint.rhs_wo_bias = rhs_wo_bias; constraint.rhs_wo_bias = rhs_wo_bias;
constraint.rhs = 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.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
constraint constraint
} }
@@ -436,6 +438,11 @@ impl JointVelocityConstraintBuilder<Real> {
// TODO: orthogonalization doesn’t seem to give good results for multibodies? // TODO: orthogonalization doesn’t seem to give good results for multibodies?
const ORTHOGONALIZE: bool = false; const ORTHOGONALIZE: bool = false;
let len = constraints.len(); let len = constraints.len();
if len == 0 {
return;
}
let ndofs1 = constraints[0].ndofs1; let ndofs1 = constraints[0].ndofs1;
let ndofs2 = constraints[0].ndofs2; let ndofs2 = constraints[0].ndofs2;
@@ -449,8 +456,10 @@ impl JointVelocityConstraintBuilder<Real> {
let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2); 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 dot_jj = jac_j1.dot(&w_jac_j1) + jac_j2.dot(&w_jac_j2);
let inv_dot_jj = crate::utils::inv(dot_jj); let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain;
c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs. 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] { if c_j.impulse_bounds != [-Real::MAX, Real::MAX] {
// Don't remove constraints with limited forces from the others // Don't remove constraints with limited forces from the others
@@ -510,7 +519,7 @@ impl JointVelocityConstraintBuilder<Real> {
let vel2 = mb2 let vel2 = mb2
.fill_jacobians(link_id2, lin_jac, ang_jac2, j_id, jacobians) .fill_jacobians(link_id2, lin_jac, ang_jac2, j_id, jacobians)
.1; .1;
let rhs_wo_bias = (vel2 - vel1) * params.velocity_solve_fraction; let rhs_wo_bias = vel2 - vel1;
let mj_lambda2 = mb2.solver_id; let mj_lambda2 = mb2.solver_id;
@@ -524,6 +533,8 @@ impl JointVelocityConstraintBuilder<Real> {
inv_lhs: 0.0, inv_lhs: 0.0,
rhs: rhs_wo_bias, rhs: rhs_wo_bias,
rhs_wo_bias, rhs_wo_bias,
cfm_coeff: 0.0,
cfm_gain: 0.0,
writeback_id, writeback_id,
} }
} }
@@ -556,7 +567,7 @@ impl JointVelocityConstraintBuilder<Real> {
ang_jac2, 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; let rhs_bias = lin_jac.dot(&self.lin_err) * erp_inv_dt;
c.rhs += rhs_bias; c.rhs += rhs_bias;
c c
@@ -595,7 +606,7 @@ impl JointVelocityConstraintBuilder<Real> {
let min_enabled = dist < limits[0]; let min_enabled = dist < limits[0];
let max_enabled = limits[1] < dist; 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; let rhs_bias = ((dist - limits[1]).max(0.0) - (limits[0] - dist).max(0.0)) * erp_inv_dt;
constraint.rhs += rhs_bias; constraint.rhs += rhs_bias;
constraint.impulse_bounds = [ constraint.impulse_bounds = [
@@ -645,20 +656,20 @@ impl JointVelocityConstraintBuilder<Real> {
); );
let mut rhs_wo_bias = 0.0; 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); 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)) let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping; rhs_wo_bias += dvel - motor_params.target_vel;
}
constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse]; constraint.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
constraint.rhs = rhs_wo_bias; constraint.rhs = rhs_wo_bias;
constraint.rhs_wo_bias = 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 constraint
} }
@@ -688,7 +699,7 @@ impl JointVelocityConstraintBuilder<Real> {
ang_jac, ang_jac,
); );
let erp_inv_dt = params.erp_inv_dt(); let erp_inv_dt = params.joint_erp_inv_dt();
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let rhs_bias = self.ang_err.im * erp_inv_dt; let rhs_bias = self.ang_err.im * erp_inv_dt;
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
@@ -736,7 +747,7 @@ impl JointVelocityConstraintBuilder<Real> {
max_enabled as u32 as Real * Real::MAX, 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 = let rhs_bias =
((s_ang - s_limits[1]).max(0.0) - (s_limits[0] - s_ang).max(0.0)) * erp_inv_dt; ((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<Real> {
); );
let mut rhs = 0.0; let mut rhs = 0.0;
if motor_params.stiffness != 0.0 { if motor_params.erp_inv_dt != 0.0 {
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let s_ang_dist = self.ang_err.im; let s_ang_dist = self.ang_err.im;
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
let s_ang_dist = self.ang_err.imag()[_motor_axis]; let s_ang_dist = self.ang_err.imag()[_motor_axis];
let s_target_ang = motor_params.target_pos.sin(); 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); let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
rhs += (dvel - motor_params.target_vel * ang_jac.norm()) * motor_params.damping; rhs += dvel - motor_params.target_vel;
}
constraint.rhs_wo_bias = rhs; constraint.rhs_wo_bias = rhs;
constraint.rhs = 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.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
constraint constraint
} }
@@ -805,6 +816,11 @@ impl JointVelocityConstraintBuilder<Real> {
// TODO: orthogonalization doesn’t seem to give good results for multibodies? // TODO: orthogonalization doesn’t seem to give good results for multibodies?
const ORTHOGONALIZE: bool = false; const ORTHOGONALIZE: bool = false;
let len = constraints.len(); let len = constraints.len();
if len == 0 {
return;
}
let ndofs2 = constraints[0].ndofs2; let ndofs2 = constraints[0].ndofs2;
// Use the modified Gramm-Schmidt orthogonalization. // Use the modified Gramm-Schmidt orthogonalization.
@@ -815,8 +831,10 @@ impl JointVelocityConstraintBuilder<Real> {
let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2); let w_jac_j2 = jacobians.rows(c_j.j_id2 + ndofs2, ndofs2);
let dot_jj = jac_j2.dot(&w_jac_j2); let dot_jj = jac_j2.dot(&w_jac_j2);
let inv_dot_jj = crate::utils::inv(dot_jj); let cfm_gain = dot_jj * c_j.cfm_coeff + c_j.cfm_gain;
c_j.inv_lhs = inv_dot_jj; // Don’t forget to update the inv_lhs. 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] { if c_j.impulse_bounds != [-Real::MAX, Real::MAX] {
// Don't remove constraints with limited forces from the others // Don't remove constraints with limited forces from the others

307
src/dynamics/solver/joint_constraint/joint_velocity_constraint.rs
View File

@@ -1,6 +1,8 @@
use crate::dynamics::solver::joint_constraint::JointVelocityConstraintBuilder; use crate::dynamics::solver::joint_constraint::JointVelocityConstraintBuilder;
use crate::dynamics::solver::DeltaVel; 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::math::{AngVector, AngularInertia, Isometry, Point, Real, Vector, DIM, SPATIAL_DIM};
use crate::utils::{WDot, WReal}; use crate::utils::{WDot, WReal};
@@ -12,10 +14,9 @@ use {
#[derive(Copy, Clone, PartialEq, Debug)] #[derive(Copy, Clone, PartialEq, Debug)]
pub struct MotorParameters<N: WReal> { pub struct MotorParameters<N: WReal> {
pub stiffness: N, pub erp_inv_dt: N,
pub damping: N, pub cfm_coeff: N,
pub gamma: N, pub cfm_gain: N,
// pub keep_lhs: bool,
pub target_pos: N, pub target_pos: N,
pub target_vel: N, pub target_vel: N,
pub max_impulse: N, pub max_impulse: N,
@@ -24,10 +25,9 @@ pub struct MotorParameters<N: WReal> {
impl<N: WReal> Default for MotorParameters<N> { impl<N: WReal> Default for MotorParameters<N> {
fn default() -> Self { fn default() -> Self {
Self { Self {
stiffness: N::zero(), erp_inv_dt: N::zero(),
damping: N::zero(), cfm_coeff: N::zero(),
gamma: N::zero(), cfm_gain: N::zero(),
// keep_lhs: true,
target_pos: N::zero(), target_pos: N::zero(),
target_vel: N::zero(), target_vel: N::zero(),
max_impulse: N::zero(), max_impulse: N::zero(),
@@ -72,6 +72,8 @@ pub struct JointVelocityConstraint<N: WReal, const LANES: usize> {
pub inv_lhs: N, pub inv_lhs: N,
pub rhs: N, pub rhs: N,
pub rhs_wo_bias: N, pub rhs_wo_bias: N,
pub cfm_gain: N,
pub cfm_coeff: N,
pub im1: Vector<N>, pub im1: Vector<N>,
pub im2: Vector<N>, pub im2: Vector<N>,
@@ -91,6 +93,8 @@ impl<N: WReal, const LANES: usize> JointVelocityConstraint<N, LANES> {
ang_jac1: na::zero(), ang_jac1: na::zero(),
ang_jac2: na::zero(), ang_jac2: na::zero(),
inv_lhs: N::zero(), inv_lhs: N::zero(),
cfm_gain: N::zero(),
cfm_coeff: N::zero(),
rhs: N::zero(), rhs: N::zero(),
rhs_wo_bias: N::zero(), rhs_wo_bias: N::zero(),
im1: na::zero(), im1: na::zero(),
@@ -105,7 +109,7 @@ impl<N: WReal, const LANES: usize> JointVelocityConstraint<N, LANES> {
self.ang_jac2.gdot(mj_lambda2.angular) - self.ang_jac1.gdot(mj_lambda1.angular); self.ang_jac2.gdot(mj_lambda2.angular) - self.ang_jac1.gdot(mj_lambda1.angular);
let rhs = dlinvel + dangvel + self.rhs; 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]); .simd_clamp(self.impulse_bounds[0], self.impulse_bounds[1]);
let delta_impulse = total_impulse - self.impulse; let delta_impulse = total_impulse - self.impulse;
self.impulse = total_impulse; self.impulse = total_impulse;
@@ -133,13 +137,14 @@ impl JointVelocityConstraint<Real, 1> {
body2: &SolverBody<Real, 1>, body2: &SolverBody<Real, 1>,
frame1: &Isometry<Real>, frame1: &Isometry<Real>,
frame2: &Isometry<Real>, frame2: &Isometry<Real>,
joint: &JointData, joint: &GenericJoint,
out: &mut [Self], out: &mut [Self],
) -> usize { ) -> usize {
let mut len = 0; let mut len = 0;
let locked_axes = joint.locked_axes.bits(); let locked_axes = joint.locked_axes.bits();
let motor_axes = joint.motor_axes.bits(); let motor_axes = joint.motor_axes.bits() & !locked_axes;
let limit_axes = joint.limit_axes.bits(); let limit_axes = joint.limit_axes.bits() & !locked_axes;
let coupled_axes = joint.coupled_axes.bits();
let builder = JointVelocityConstraintBuilder::new( let builder = JointVelocityConstraintBuilder::new(
frame1, frame1,
@@ -149,13 +154,62 @@ impl JointVelocityConstraint<Real, 1> {
locked_axes, locked_axes,
); );
for i in 0..DIM { let start = len;
if locked_axes & (1 << i) != 0 { for i in DIM..SPATIAL_DIM {
out[len] = if (motor_axes & !coupled_axes) & (1 << i) != 0 {
builder.lock_linear(params, [joint_id], body1, body2, i, WritebackId::Dof(i)); out[len] = builder.motor_angular(
[joint_id],
body1,
body2,
i - DIM,
&joint.motors[i].motor_params(params.dt),
WritebackId::Motor(i),
);
len += 1; 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 { for i in DIM..SPATIAL_DIM {
if locked_axes & (1 << i) != 0 { if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_angular( out[len] = builder.lock_angular(
@@ -169,52 +223,16 @@ impl JointVelocityConstraint<Real, 1> {
len += 1; len += 1;
} }
} }
for i in 0..DIM { for i in 0..DIM {
if motor_axes & (1 << i) != 0 { if locked_axes & (1 << i) != 0 {
out[len] = builder.motor_linear( out[len] =
params, builder.lock_linear(params, [joint_id], body1, body2, i, WritebackId::Dof(i));
[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),
);
len += 1; 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 { 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( out[len] = builder.limit_angular(
params, params,
[joint_id], [joint_id],
@@ -227,8 +245,40 @@ impl JointVelocityConstraint<Real, 1> {
len += 1; 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 len
} }
@@ -349,6 +399,8 @@ pub struct JointVelocityGroundConstraint<N: WReal, const LANES: usize> {
pub ang_jac2: AngVector<N>, pub ang_jac2: AngVector<N>,
pub inv_lhs: N, pub inv_lhs: N,
pub cfm_coeff: N,
pub cfm_gain: N,
pub rhs: N, pub rhs: N,
pub rhs_wo_bias: N, pub rhs_wo_bias: N,
@@ -367,6 +419,8 @@ impl<N: WReal, const LANES: usize> JointVelocityGroundConstraint<N, LANES> {
lin_jac: Vector::zeros(), lin_jac: Vector::zeros(),
ang_jac2: na::zero(), ang_jac2: na::zero(),
inv_lhs: N::zero(), inv_lhs: N::zero(),
cfm_coeff: N::zero(),
cfm_gain: N::zero(),
rhs: N::zero(), rhs: N::zero(),
rhs_wo_bias: N::zero(), rhs_wo_bias: N::zero(),
im2: na::zero(), im2: na::zero(),
@@ -379,7 +433,7 @@ impl<N: WReal, const LANES: usize> JointVelocityGroundConstraint<N, LANES> {
let dangvel = mj_lambda2.angular; let dangvel = mj_lambda2.angular;
let dvel = self.lin_jac.dot(&dlinvel) + self.ang_jac2.gdot(dangvel) + self.rhs; 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]); .simd_clamp(self.impulse_bounds[0], self.impulse_bounds[1]);
let delta_impulse = total_impulse - self.impulse; let delta_impulse = total_impulse - self.impulse;
self.impulse = total_impulse; self.impulse = total_impulse;
@@ -404,13 +458,14 @@ impl JointVelocityGroundConstraint<Real, 1> {
body2: &SolverBody<Real, 1>, body2: &SolverBody<Real, 1>,
frame1: &Isometry<Real>, frame1: &Isometry<Real>,
frame2: &Isometry<Real>, frame2: &Isometry<Real>,
joint: &JointData, joint: &GenericJoint,
out: &mut [Self], out: &mut [Self],
) -> usize { ) -> usize {
let mut len = 0; let mut len = 0;
let locked_axes = joint.locked_axes.bits() as u8; let locked_axes = joint.locked_axes.bits();
let motor_axes = joint.motor_axes.bits() as u8; let motor_axes = joint.motor_axes.bits() & !locked_axes;
let limit_axes = joint.limit_axes.bits() as u8; let limit_axes = joint.limit_axes.bits() & !locked_axes;
let coupled_axes = joint.coupled_axes.bits();
let builder = JointVelocityConstraintBuilder::new( let builder = JointVelocityConstraintBuilder::new(
frame1, frame1,
@@ -420,19 +475,68 @@ impl JointVelocityGroundConstraint<Real, 1> {
locked_axes, 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 { for i in 0..DIM {
if locked_axes & (1 << i) != 0 { if (motor_axes & !coupled_axes) & (1 << i) != 0 {
out[len] = builder.lock_linear_ground( 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, params,
[joint_id], [joint_id],
body1, body1,
body2, body2,
i, i,
WritebackId::Dof(i), &joint.motors[i].motor_params(params.dt),
limits,
WritebackId::Motor(i),
); );
len += 1; 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 { for i in DIM..SPATIAL_DIM {
if locked_axes & (1 << i) != 0 { if locked_axes & (1 << i) != 0 {
out[len] = builder.lock_angular_ground( out[len] = builder.lock_angular_ground(
@@ -446,50 +550,22 @@ impl JointVelocityGroundConstraint<Real, 1> {
len += 1; len += 1;
} }
} }
for i in 0..DIM { for i in 0..DIM {
if motor_axes & (1 << i) != 0 { if locked_axes & (1 << i) != 0 {
out[len] = builder.motor_linear_ground( out[len] = builder.lock_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(
params, params,
[joint_id], [joint_id],
body1, body1,
body2, body2,
i, i,
[joint.limits[i].min, joint.limits[i].max], WritebackId::Dof(i),
WritebackId::Limit(i),
); );
len += 1; len += 1;
} }
} }
for i in DIM..SPATIAL_DIM { 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( out[len] = builder.limit_angular_ground(
params, params,
[joint_id], [joint_id],
@@ -502,8 +578,39 @@ impl JointVelocityGroundConstraint<Real, 1> {
len += 1; 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 len
} }

393
src/dynamics/solver/joint_constraint/joint_velocity_constraint_builder.rs
View File

@@ -3,8 +3,8 @@ use crate::dynamics::solver::joint_constraint::joint_velocity_constraint::{
}; };
use crate::dynamics::solver::joint_constraint::SolverBody; use crate::dynamics::solver::joint_constraint::SolverBody;
use crate::dynamics::solver::MotorParameters; use crate::dynamics::solver::MotorParameters;
use crate::dynamics::{IntegrationParameters, JointIndex}; use crate::dynamics::{IntegrationParameters, JointAxesMask, JointIndex, JointLimits, JointMotor};
use crate::math::{Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM}; use crate::math::{AngVector, Isometry, Matrix, Point, Real, Rotation, Vector, ANG_DIM, DIM};
use crate::utils::{IndexMut2, WCrossMatrix, WDot, WQuat, WReal}; use crate::utils::{IndexMut2, WCrossMatrix, WDot, WQuat, WReal};
use na::SMatrix; use na::SMatrix;
@@ -92,10 +92,12 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let min_enabled = dist.simd_lt(limits[0]); let min_enabled = dist.simd_lt(limits[0]);
let max_enabled = limits[1].simd_lt(dist); 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 = let rhs_bias =
((dist - limits[1]).simd_max(zero) - (limits[0] - dist).simd_max(zero)) * erp_inv_dt; ((dist - limits[1]).simd_max(zero) - (limits[0] - dist).simd_max(zero)) * erp_inv_dt;
constraint.rhs = constraint.rhs_wo_bias + rhs_bias; constraint.rhs = constraint.rhs_wo_bias + rhs_bias;
constraint.cfm_coeff = cfm_coeff;
constraint.impulse_bounds = [ constraint.impulse_bounds = [
N::splat(-Real::INFINITY).select(min_enabled, zero), N::splat(-Real::INFINITY).select(min_enabled, zero),
N::splat(Real::INFINITY).select(max_enabled, zero), N::splat(Real::INFINITY).select(max_enabled, zero),
@@ -104,39 +106,114 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
constraint constraint
} }
pub fn limit_linear_coupled<const LANES: usize>(
&self,
params: &IntegrationParameters,
joint_id: [JointIndex; LANES],
body1: &SolverBody<N, LANES>,
body2: &SolverBody<N, LANES>,
limited_coupled_axes: u8,
limits: &[JointLimits<N>],
writeback_id: WritebackId,
) -> JointVelocityConstraint<N, LANES> {
let zero = N::zero();
let mut lin_jac = Vector::zeros();
let mut ang_jac1: AngVector<N> = na::zero();
let mut ang_jac2: AngVector<N> = 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<const LANES: usize>( pub fn motor_linear<const LANES: usize>(
&self, &self,
params: &IntegrationParameters, params: &IntegrationParameters,
joint_id: [JointIndex; LANES], joint_id: [JointIndex; LANES],
body1: &SolverBody<N, LANES>, body1: &SolverBody<N, LANES>,
body2: &SolverBody<N, LANES>, body2: &SolverBody<N, LANES>,
_locked_ang_axes: u8,
motor_axis: usize, motor_axis: usize,
motor_params: &MotorParameters<N>, motor_params: &MotorParameters<N>,
limits: Option<[N; 2]>,
writeback_id: WritebackId, writeback_id: WritebackId,
) -> JointVelocityConstraint<N, LANES> { ) -> JointVelocityConstraint<N, LANES> {
let inv_dt = N::splat(params.inv_dt());
let mut constraint = let mut constraint =
self.lock_linear(params, joint_id, body1, body2, motor_axis, writeback_id); 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(); 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); 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 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)) let dvel = constraint.lin_jac.dot(&(body2.linvel - body1.linvel))
+ (constraint.ang_jac2.gdot(body2.angvel) - constraint.ang_jac1.gdot(body1.angvel)); + (constraint.ang_jac2.gdot(body2.angvel) - constraint.ang_jac1.gdot(body1.angvel));
rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping; 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.impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
constraint.rhs = rhs_wo_bias; constraint.rhs = rhs_wo_bias;
constraint.rhs_wo_bias = rhs_wo_bias; constraint.rhs_wo_bias = rhs_wo_bias;
@@ -164,10 +241,11 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel)) let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); + (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 erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let rhs_bias = lin_jac.dot(&self.lin_err) * N::splat(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_jac1 = body1.sqrt_ii * ang_jac1;
ang_jac2 = body2.sqrt_ii * ang_jac2; ang_jac2 = body2.sqrt_ii * ang_jac2;
@@ -184,6 +262,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
ang_jac1, ang_jac1,
ang_jac2, ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization. inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs: rhs_wo_bias + rhs_bias, rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias, rhs_wo_bias,
writeback_id, writeback_id,
@@ -220,12 +300,13 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
let ang_jac = self.ang_basis.column(limited_axis).into_owned(); let ang_jac = self.ang_basis.column(limited_axis).into_owned();
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); 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) let rhs_bias = ((s_ang - s_limits[1]).simd_max(zero)
- (s_limits[0] - s_ang).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_jac1 = body1.sqrt_ii * ang_jac;
let ang_jac2 = body2.sqrt_ii * ang_jac; let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -242,6 +323,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
ang_jac1, ang_jac1,
ang_jac2, ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization. inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs: rhs_wo_bias + rhs_bias, rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias, rhs_wo_bias,
writeback_id, writeback_id,
@@ -264,20 +347,17 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let ang_jac = self.basis.column(_motor_axis).into_owned(); let ang_jac = self.basis.column(_motor_axis).into_owned();
let mut rhs_wo_bias = N::zero(); let mut rhs_wo_bias = N::zero();
if motor_params.stiffness != N::zero() { if motor_params.erp_inv_dt != N::zero() {
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let s_ang_dist = self.ang_err.im; let s_ang_dist = self.ang_err.im;
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
let s_ang_dist = self.ang_err.imag()[_motor_axis]; let s_ang_dist = self.ang_err.imag()[_motor_axis];
let s_target_ang = motor_params.target_pos.simd_sin(); 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); let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
rhs_wo_bias += rhs_wo_bias += dvel - motor_params.target_vel;
(dvel - motor_params.target_vel/* * ang_jac.norm() */) * motor_params.damping;
}
let ang_jac1 = body1.sqrt_ii * ang_jac; let ang_jac1 = body1.sqrt_ii * ang_jac;
let ang_jac2 = body2.sqrt_ii * ang_jac; let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -294,6 +374,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
ang_jac1, ang_jac1,
ang_jac2, ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization. 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: rhs_wo_bias,
rhs_wo_bias, rhs_wo_bias,
writeback_id, writeback_id,
@@ -315,13 +397,14 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let ang_jac = self.ang_basis.column(locked_axis).into_owned(); let ang_jac = self.ang_basis.column(locked_axis).into_owned();
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); 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")] #[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")] #[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_jac1 = body1.sqrt_ii * ang_jac;
let ang_jac2 = body2.sqrt_ii * ang_jac; let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -338,6 +421,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
ang_jac1, ang_jac1,
ang_jac2, ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization. inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs: rhs_wo_bias + rhs_bias, rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias, rhs_wo_bias,
writeback_id, writeback_id,
@@ -349,6 +434,11 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
constraints: &mut [JointVelocityConstraint<N, LANES>], constraints: &mut [JointVelocityConstraint<N, LANES>],
) { ) {
let len = constraints.len(); let len = constraints.len();
if len == 0 {
return;
}
let imsum = constraints[0].im1 + constraints[0].im2; let imsum = constraints[0].im1 + constraints[0].im2;
// Use the modified Gram-Schmidt orthogonalization. // Use the modified Gram-Schmidt orthogonalization.
@@ -357,8 +447,10 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let dot_jj = c_j.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac)) 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_jac1.gdot(c_j.ang_jac1)
+ c_j.ang_jac2.gdot(c_j.ang_jac2); + 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); 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)] { if c_j.impulse_bounds != [-N::splat(Real::MAX), N::splat(Real::MAX)] {
// Don't remove constraints with limited forces from the others // Don't remove constraints with limited forces from the others
@@ -369,6 +461,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
for i in (j + 1)..len { for i in (j + 1)..len {
let (c_i, c_j) = constraints.index_mut_const(i, j); 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)) 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_jac1.gdot(c_j.ang_jac1)
+ c_i.ang_jac2.gdot(c_j.ang_jac2); + c_i.ang_jac2.gdot(c_j.ang_jac2);
@@ -396,6 +489,7 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let zero = N::zero(); let zero = N::zero();
let lin_jac = self.basis.column(limited_axis).into_owned(); let lin_jac = self.basis.column(limited_axis).into_owned();
let dist = self.lin_err.dot(&lin_jac); let dist = self.lin_err.dot(&lin_jac);
let min_enabled = dist.simd_lt(limits[0]); let min_enabled = dist.simd_lt(limits[0]);
let max_enabled = limits[1].simd_lt(dist); let max_enabled = limits[1].simd_lt(dist);
@@ -412,11 +506,12 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel)) let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); + (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 erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let rhs_bias = ((dist - limits[1]).simd_max(zero) - (limits[0] - dist).simd_max(zero)) let cfm_coeff = N::splat(params.joint_cfm_coeff());
* N::splat(erp_inv_dt); 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; ang_jac2 = body2.sqrt_ii * ang_jac2;
@@ -429,21 +524,97 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
lin_jac, lin_jac,
ang_jac2, ang_jac2,
inv_lhs: zero, // Will be set during ortogonalization. inv_lhs: zero, // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs: rhs_wo_bias + rhs_bias, rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias, rhs_wo_bias,
writeback_id, writeback_id,
} }
} }
pub fn limit_linear_coupled_ground<const LANES: usize>(
&self,
params: &IntegrationParameters,
joint_id: [JointIndex; LANES],
body1: &SolverBody<N, LANES>,
body2: &SolverBody<N, LANES>,
limited_coupled_axes: u8,
limits: &[JointLimits<N>],
writeback_id: WritebackId,
) -> JointVelocityGroundConstraint<N, LANES> {
let zero = N::zero();
let mut lin_jac = Vector::zeros();
let mut ang_jac1: AngVector<N> = na::zero();
let mut ang_jac2: AngVector<N> = 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<const LANES: usize>( pub fn motor_linear_ground<const LANES: usize>(
&self, &self,
params: &IntegrationParameters,
joint_id: [JointIndex; LANES], joint_id: [JointIndex; LANES],
body1: &SolverBody<N, LANES>, body1: &SolverBody<N, LANES>,
body2: &SolverBody<N, LANES>, body2: &SolverBody<N, LANES>,
motor_axis: usize, motor_axis: usize,
motor_params: &MotorParameters<N>, motor_params: &MotorParameters<N>,
limits: Option<[N; 2]>,
writeback_id: WritebackId, writeback_id: WritebackId,
) -> JointVelocityGroundConstraint<N, LANES> { ) -> JointVelocityGroundConstraint<N, LANES> {
let inv_dt = N::splat(params.inv_dt());
let lin_jac = self.basis.column(motor_axis).into_owned(); let lin_jac = self.basis.column(motor_axis).into_owned();
let ang_jac1 = self.cmat1_basis.column(motor_axis).into_owned(); let ang_jac1 = self.cmat1_basis.column(motor_axis).into_owned();
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
@@ -452,16 +623,21 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let mut ang_jac2 = self.cmat2_basis.column(motor_axis).into_owned(); let mut ang_jac2 = self.cmat2_basis.column(motor_axis).into_owned();
let mut rhs_wo_bias = N::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(&lin_jac); 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 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)) let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); + (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel));
rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping; rhs_wo_bias += dvel - target_vel;
}
ang_jac2 = body2.sqrt_ii * ang_jac2; ang_jac2 = body2.sqrt_ii * ang_jac2;
@@ -474,12 +650,89 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
lin_jac, lin_jac,
ang_jac2, ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization. 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: rhs_wo_bias,
rhs_wo_bias, rhs_wo_bias,
writeback_id, writeback_id,
} }
} }
pub fn motor_linear_coupled_ground<const LANES: usize>(
&self,
params: &IntegrationParameters,
joint_id: [JointIndex; LANES],
body1: &SolverBody<N, LANES>,
body2: &SolverBody<N, LANES>,
motor_coupled_axes: u8,
motors: &[MotorParameters<N>],
limited_coupled_axes: u8,
limits: &[JointLimits<N>],
writeback_id: WritebackId,
) -> JointVelocityGroundConstraint<N, LANES> {
todo!()
/*
let zero = N::zero();
let mut lin_jac = Vector::zeros();
let mut ang_jac1: AngVector<N> = na::zero();
let mut ang_jac2: AngVector<N> = 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<const LANES: usize>( pub fn lock_linear_ground<const LANES: usize>(
&self, &self,
params: &IntegrationParameters, params: &IntegrationParameters,
@@ -498,10 +751,11 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let dvel = lin_jac.dot(&(body2.linvel - body1.linvel)) let dvel = lin_jac.dot(&(body2.linvel - body1.linvel))
+ (ang_jac2.gdot(body2.angvel) - ang_jac1.gdot(body1.angvel)); + (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 erp_inv_dt = N::splat(params.joint_erp_inv_dt());
let rhs_bias = lin_jac.dot(&self.lin_err) * N::splat(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; ang_jac2 = body2.sqrt_ii * ang_jac2;
@@ -514,6 +768,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
lin_jac, lin_jac,
ang_jac2, ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization. inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs: rhs_wo_bias + rhs_bias, rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias, rhs_wo_bias,
writeback_id, writeback_id,
@@ -536,20 +792,17 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let ang_jac = self.basis.column(_motor_axis).into_owned(); let ang_jac = self.basis.column(_motor_axis).into_owned();
let mut rhs_wo_bias = N::zero(); let mut rhs_wo_bias = N::zero();
if motor_params.stiffness != N::zero() { if motor_params.erp_inv_dt != N::zero() {
#[cfg(feature = "dim2")] #[cfg(feature = "dim2")]
let s_ang_dist = self.ang_err.im; let s_ang_dist = self.ang_err.im;
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
let s_ang_dist = self.ang_err.imag()[_motor_axis]; let s_ang_dist = self.ang_err.imag()[_motor_axis];
let s_target_ang = motor_params.target_pos.simd_sin(); 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); let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel);
rhs_wo_bias += rhs_wo_bias += dvel - motor_params.target_vel;
(dvel - motor_params.target_vel/* * ang_jac.norm() */) * motor_params.damping;
}
let ang_jac2 = body2.sqrt_ii * ang_jac; let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -562,6 +815,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
lin_jac: na::zero(), lin_jac: na::zero(),
ang_jac2, ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization. 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: rhs_wo_bias,
rhs_wo_bias, rhs_wo_bias,
writeback_id, writeback_id,
@@ -598,12 +853,13 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
let ang_jac = self.ang_basis.column(limited_axis).into_owned(); let ang_jac = self.ang_basis.column(limited_axis).into_owned();
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); 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) let rhs_bias = ((s_ang - s_limits[1]).simd_max(zero)
- (s_limits[0] - s_ang).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; let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -616,6 +872,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
lin_jac: na::zero(), lin_jac: na::zero(),
ang_jac2, ang_jac2,
inv_lhs: zero, // Will be set during ortogonalization. inv_lhs: zero, // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs: rhs_wo_bias + rhs_bias, rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias, rhs_wo_bias,
writeback_id, writeback_id,
@@ -636,13 +894,14 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]
let ang_jac = self.ang_basis.column(locked_axis).into_owned(); let ang_jac = self.ang_basis.column(locked_axis).into_owned();
let dvel = ang_jac.gdot(body2.angvel) - ang_jac.gdot(body1.angvel); 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")] #[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")] #[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; let ang_jac2 = body2.sqrt_ii * ang_jac;
@@ -655,6 +914,8 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
lin_jac: na::zero(), lin_jac: na::zero(),
ang_jac2, ang_jac2,
inv_lhs: N::zero(), // Will be set during ortogonalization. inv_lhs: N::zero(), // Will be set during ortogonalization.
cfm_coeff,
cfm_gain: N::zero(),
rhs: rhs_wo_bias + rhs_bias, rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias, rhs_wo_bias,
writeback_id, writeback_id,
@@ -666,6 +927,11 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
constraints: &mut [JointVelocityGroundConstraint<N, LANES>], constraints: &mut [JointVelocityGroundConstraint<N, LANES>],
) { ) {
let len = constraints.len(); let len = constraints.len();
if len == 0 {
return;
}
let imsum = constraints[0].im2; let imsum = constraints[0].im2;
// Use the modified Gram-Schmidt orthogonalization. // Use the modified Gram-Schmidt orthogonalization.
@@ -673,18 +939,23 @@ impl<N: WReal> JointVelocityConstraintBuilder<N> {
let c_j = &mut constraints[j]; let c_j = &mut constraints[j];
let dot_jj = c_j.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac)) let dot_jj = c_j.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac))
+ c_j.ang_jac2.gdot(c_j.ang_jac2); + 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.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 // Don't remove constraints with limited forces from the others
// because they may not deliver the necessary forces to fulfill // because they may not deliver the necessary forces to fulfill
// the removed parts of other constraints. // the removed parts of other constraints.
continue; 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 (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)) let dot_ij = c_i.lin_jac.dot(&imsum.component_mul(&c_j.lin_jac))
+ c_i.ang_jac2.gdot(c_j.ang_jac2); + c_i.ang_jac2.gdot(c_j.ang_jac2);
let coeff = dot_ij * inv_dot_jj; let coeff = dot_ij * inv_dot_jj;

18
src/dynamics/solver/velocity_constraint.rs
View File

@@ -5,7 +5,7 @@ use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint};
use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity}; use crate::dynamics::{IntegrationParameters, RigidBodyIds, RigidBodyMassProps, RigidBodyVelocity};
use crate::geometry::{ContactManifold, ContactManifoldIndex}; use crate::geometry::{ContactManifold, ContactManifoldIndex};
use crate::math::{Real, Vector, DIM, MAX_MANIFOLD_POINTS}; 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}; use super::{DeltaVel, VelocityConstraintElement, VelocityConstraintNormalPart};
@@ -239,10 +239,11 @@ impl VelocityConstraint {
.transform_vector(dp2.gcross(-force_dir1)); .transform_vector(dp2.gcross(-force_dir1));
let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass; let imsum = mprops1.effective_inv_mass + mprops2.effective_inv_mass;
let projected_mass = 1.0 let projected_mass = utils::inv(
/ (force_dir1.dot(&imsum.component_mul(&force_dir1)) force_dir1.dot(&imsum.component_mul(&force_dir1))
+ gcross1.gdot(gcross1) + gcross1.gdot(gcross1)
+ gcross2.gdot(gcross2)); + gcross2.gdot(gcross2),
);
let is_bouncy = manifold_point.is_bouncy() as u32 as Real; let is_bouncy = manifold_point.is_bouncy() as u32 as Real;
let is_resting = 1.0 - is_bouncy; 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) let mut rhs_wo_bias = (1.0 + is_bouncy * manifold_point.restitution)
* (vel1 - vel2).dot(&force_dir1); * (vel1 - vel2).dot(&force_dir1);
rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt; 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 let rhs_bias = /* is_resting
* */ erp_inv_dt * */ erp_inv_dt
* (manifold_point.dist + params.allowed_linear_error).min(0.0); * (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.gcross1[j] = gcross1;
constraint.elements[k].tangent_part.gcross2[j] = gcross2; constraint.elements[k].tangent_part.gcross2[j] = gcross2;
constraint.elements[k].tangent_part.rhs[j] = rhs; constraint.elements[k].tangent_part.rhs[j] = rhs;
constraint.elements[k].tangent_part.r[j] = constraint.elements[k].tangent_part.r[j] = if cfg!(feature = "dim2") {
if cfg!(feature = "dim2") { 1.0 / r } else { r }; utils::inv(r)
} else {
r
};
} }
#[cfg(feature = "dim3")] #[cfg(feature = "dim3")]

Some files were not shown because too many files have changed in this diff Show More