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"
],
"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.
let rigid_body = RigidBodyBuilder::new(status)
.translation(vector![x, y])
.build();
let rigid_body = RigidBodyBuilder::new(status).translation(vector![x, y]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::ball(rad).build();
let collider = ColliderBuilder::ball(rad);
colliders.insert_with_parent(collider, handle, &mut bodies);
}
}

20
benchmarks2d/boxes2.rs
View File

@@ -15,25 +15,23 @@ pub fn init_world(testbed: &mut Testbed) {
*/
let ground_size = 25.0;
let rigid_body = RigidBodyBuilder::new_static().build();
let rigid_body = RigidBodyBuilder::new_static();
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, 1.2).build();
let collider = ColliderBuilder::cuboid(ground_size, 1.2);
colliders.insert_with_parent(collider, handle, &mut bodies);
let rigid_body = RigidBodyBuilder::new_static()
.rotation(std::f32::consts::FRAC_PI_2)
.translation(vector![ground_size, ground_size * 2.0])
.build();
.translation(vector![ground_size, ground_size * 2.0]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size * 2.0, 1.2).build();
let collider = ColliderBuilder::cuboid(ground_size * 2.0, 1.2);
colliders.insert_with_parent(collider, handle, &mut bodies);
let rigid_body = RigidBodyBuilder::new_static()
.rotation(std::f32::consts::FRAC_PI_2)
.translation(vector![-ground_size, ground_size * 2.0])
.build();
.translation(vector![-ground_size, ground_size * 2.0]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size * 2.0, 1.2).build();
let collider = ColliderBuilder::cuboid(ground_size * 2.0, 1.2);
colliders.insert_with_parent(collider, handle, &mut bodies);
/*
@@ -52,11 +50,9 @@ pub fn init_world(testbed: &mut Testbed) {
let y = j as f32 * shift + centery + 2.0;
// Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![x, y])
.build();
let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad, rad).build();
let collider = ColliderBuilder::cuboid(rad, rad);
colliders.insert_with_parent(collider, handle, &mut bodies);
}
}

20
benchmarks2d/capsules2.rs
View File

@@ -15,25 +15,23 @@ pub fn init_world(testbed: &mut Testbed) {
*/
let ground_size = 25.0;
let rigid_body = RigidBodyBuilder::new_static().build();
let rigid_body = RigidBodyBuilder::new_static();
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, 1.2).build();
let collider = ColliderBuilder::cuboid(ground_size, 1.2);
colliders.insert_with_parent(collider, handle, &mut bodies);
let rigid_body = RigidBodyBuilder::new_static()
.rotation(std::f32::consts::FRAC_PI_2)
.translation(vector![ground_size, ground_size * 4.0])
.build();
.translation(vector![ground_size, ground_size * 4.0]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size * 4.0, 1.2).build();
let collider = ColliderBuilder::cuboid(ground_size * 4.0, 1.2);
colliders.insert_with_parent(collider, handle, &mut bodies);
let rigid_body = RigidBodyBuilder::new_static()
.rotation(std::f32::consts::FRAC_PI_2)
.translation(vector![-ground_size, ground_size * 4.0])
.build();
.translation(vector![-ground_size, ground_size * 4.0]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size * 4.0, 1.2).build();
let collider = ColliderBuilder::cuboid(ground_size * 4.0, 1.2);
colliders.insert_with_parent(collider, handle, &mut bodies);
/*
@@ -54,11 +52,9 @@ pub fn init_world(testbed: &mut Testbed) {
let y = j as f32 * shifty + centery + 3.0;
// Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![x, y])
.build();
let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::capsule_y(rad * 1.5, rad).build();
let collider = ColliderBuilder::capsule_y(rad * 1.5, rad);
colliders.insert_with_parent(collider, handle, &mut bodies);
}
}

20
benchmarks2d/convex_polygons2.rs
View File

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

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

11
benchmarks2d/joint_ball2.rs
View File

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

13
benchmarks2d/joint_fixed2.rs
View File

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

16
benchmarks2d/joint_prismatic2.rs
View File

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

10
benchmarks2d/pyramid2.rs
View File

@@ -16,9 +16,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 100.0;
let ground_thickness = 1.0;
let rigid_body = RigidBodyBuilder::new_static().build();
let rigid_body = RigidBodyBuilder::new_static();
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_thickness).build();
let collider = ColliderBuilder::cuboid(ground_size, ground_thickness);
colliders.insert_with_parent(collider, ground_handle, &mut bodies);
/*
@@ -39,11 +39,9 @@ pub fn init_world(testbed: &mut Testbed) {
let y = fi * shift + centery;
// Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![x, y])
.build();
let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad, rad).build();
let collider = ColliderBuilder::cuboid(rad, rad);
colliders.insert_with_parent(collider, handle, &mut bodies);
}
}

6
benchmarks3d/balls3.rs
View File

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

12
benchmarks3d/boxes3.rs
View File

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

12
benchmarks3d/capsules3.rs
View File

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

11
benchmarks3d/ccd3.rs
View File

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

28
benchmarks3d/compound3.rs
View File

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

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

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

11
benchmarks3d/joint_ball3.rs
View File

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

16
benchmarks3d/joint_fixed3.rs
View File

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

18
benchmarks3d/joint_prismatic3.rs
View File

@@ -23,23 +23,17 @@ pub fn init_world(testbed: &mut Testbed) {
for j in 0..50 {
let x = j as f32 * shift * 4.0;
let ground = RigidBodyBuilder::new_static()
.translation(vector![x, y, z])
.build();
let ground = RigidBodyBuilder::new_static().translation(vector![x, y, z]);
let mut curr_parent = bodies.insert(ground);
let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
let collider = ColliderBuilder::cuboid(rad, rad, rad);
colliders.insert_with_parent(collider, curr_parent, &mut bodies);
for i in 0..num {
let z = z + (i + 1) as f32 * shift;
let density = 1.0;
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![x, y, z])
.build();
let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
let curr_child = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad, rad, rad)
.density(density)
.build();
let collider = ColliderBuilder::cuboid(rad, rad, rad).density(density);
colliders.insert_with_parent(collider, curr_child, &mut bodies);
let axis = if i % 2 == 0 {
@@ -48,9 +42,9 @@ pub fn init_world(testbed: &mut Testbed) {
UnitVector::new_normalize(vector![-1.0, 1.0, 0.0])
};
let prism = PrismaticJoint::new(axis)
let prism = PrismaticJointBuilder::new(axis)
.local_anchor2(point![0.0, 0.0, -shift])
.limit_axis([-2.0, 0.0]);
.limits([-2.0, 0.0]);
impulse_joints.insert(curr_parent, curr_child, prism);
curr_parent = curr_child;

22
benchmarks3d/joint_revolute3.rs
View File

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

22
benchmarks3d/keva3.rs
View File

@@ -38,15 +38,13 @@ pub fn build_block(
};
// Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![
let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![
x + dim.x + shift.x,
y + dim.y + shift.y,
z + dim.z + shift.z
])
.build();
]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z).build();
let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z);
colliders.insert_with_parent(collider, handle, bodies);
testbed.set_initial_body_color(handle, color0);
@@ -61,15 +59,13 @@ pub fn build_block(
for i in 0..(block_width / (dim.x as f32 * 2.0)) as usize {
for j in 0..(block_width / (dim.z as f32 * 2.0)) as usize {
// Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![
let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![
i as f32 * dim.x * 2.0 + dim.x + shift.x,
dim.y + shift.y + block_height,
j as f32 * dim.z * 2.0 + dim.z + shift.z
])
.build();
]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z).build();
let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z);
colliders.insert_with_parent(collider, handle, bodies);
testbed.set_initial_body_color(handle, color0);
std::mem::swap(&mut color0, &mut color1);
@@ -92,11 +88,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 50.0;
let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static()
.translation(vector![0.0, -ground_height, 0.0])
.build();
let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size);
colliders.insert_with_parent(collider, handle, &mut bodies);
/*

12
benchmarks3d/pyramid3.rs
View File

@@ -22,13 +22,11 @@ fn create_pyramid(
- stack_height as f32 * half_extents.z;
// Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![x, y, z])
.build();
let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
let rigid_body_handle = bodies.insert(rigid_body);
let collider =
ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z).build();
ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z);
colliders.insert_with_parent(collider, rigid_body_handle, bodies);
}
}
@@ -50,11 +48,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 50.0;
let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static()
.translation(vector![0.0, -ground_height, 0.0])
.build();
let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size);
colliders.insert_with_parent(collider, ground_handle, &mut bodies);
/*

24
benchmarks3d/stacks3.rs
View File

@@ -23,10 +23,9 @@ fn create_tower_circle(
* Translation::new(0.0, y, radius);
// Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic().position(pos).build();
let rigid_body = RigidBodyBuilder::new_dynamic().position(pos);
let handle = bodies.insert(rigid_body);
let collider =
ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z).build();
let collider = ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z);
colliders.insert_with_parent(collider, handle, bodies);
}
}
@@ -50,12 +49,9 @@ fn create_wall(
- stack_height as f32 * half_extents.z;
// Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![x, y, z])
.build();
let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
let handle = bodies.insert(rigid_body);
let collider =
ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z).build();
let collider = ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z);
colliders.insert_with_parent(collider, handle, bodies);
}
}
@@ -83,12 +79,10 @@ fn create_pyramid(
- stack_height as f32 * half_extents.z;
// Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![x, y, z])
.build();
let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y, z]);
let handle = bodies.insert(rigid_body);
let collider =
ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z).build();
ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z);
colliders.insert_with_parent(collider, handle, bodies);
}
}
@@ -110,11 +104,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 200.0;
let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static()
.translation(vector![0.0, -ground_height, 0.0])
.build();
let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size);
colliders.insert_with_parent(collider, handle, &mut bodies);
/*

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

12
examples2d/add_remove2.rs
View File

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

27
examples2d/ccd2.rs
View File

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

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

20
examples2d/convex_polygons2.rs
View File

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

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])
.angvel(100.0)
.linear_damping((i + 1) as f32 * subdiv * 10.0)
.angular_damping((num - i) as f32 * subdiv * 10.0)
.build();
.angular_damping((num - i) as f32 * subdiv * 10.0);
let rb_handle = bodies.insert(rb);
// Build the collider.
let co = ColliderBuilder::cuboid(rad, rad).build();
let co = ColliderBuilder::cuboid(rad, rad);
colliders.insert_with_parent(co, rb_handle, &mut bodies);
}

10
examples2d/debug_box_ball2.rs
View File

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

12
examples2d/drum2.rs
View File

@@ -26,11 +26,9 @@ pub fn init_world(testbed: &mut Testbed) {
let y = j as f32 * shift - centery;
// Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![x, y])
.build();
let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad, rad).build();
let collider = ColliderBuilder::cuboid(rad, rad);
colliders.insert_with_parent(collider, handle, &mut bodies);
}
}
@@ -38,7 +36,7 @@ pub fn init_world(testbed: &mut Testbed) {
/*
* Setup a velocity-based kinematic rigid body.
*/
let platform_body = RigidBodyBuilder::new_kinematic_velocity_based().build();
let platform_body = RigidBodyBuilder::new_kinematic_velocity_based();
let velocity_based_platform_handle = bodies.insert(platform_body);
let sides = [
@@ -55,11 +53,11 @@ pub fn init_world(testbed: &mut Testbed) {
];
for (hx, hy, pos) in sides {
let collider = ColliderBuilder::cuboid(hx, hy).translation(pos).build();
let collider = ColliderBuilder::cuboid(hx, hy).translation(pos);
colliders.insert_with_parent(collider, velocity_based_platform_handle, &mut bodies);
}
for (r, pos) in balls {
let collider = ColliderBuilder::ball(r).translation(pos).build();
let collider = ColliderBuilder::ball(r).translation(pos);
colliders.insert_with_parent(collider, velocity_based_platform_handle, &mut bodies);
}

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

11
examples2d/joints2.rs
View File

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

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_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static()
.translation(vector![0.0, -ground_height])
.build();
let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height).build();
let collider = ColliderBuilder::cuboid(ground_size, ground_height);
colliders.insert_with_parent(collider, handle, &mut bodies);
/*
@@ -31,10 +29,9 @@ pub fn init_world(testbed: &mut Testbed) {
*/
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![0.0, 3.0])
.lock_translations()
.build();
.lock_translations();
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(2.0, 0.6).build();
let collider = ColliderBuilder::cuboid(2.0, 0.6);
colliders.insert_with_parent(collider, handle, &mut bodies);
/*
@@ -43,10 +40,9 @@ pub fn init_world(testbed: &mut Testbed) {
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![0.0, 5.0])
.rotation(1.0)
.lock_rotations()
.build();
.lock_rotations();
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::capsule_y(0.6, 0.4).build();
let collider = ColliderBuilder::capsule_y(0.6, 0.4);
colliders.insert_with_parent(collider, handle, &mut bodies);
/*

14
examples2d/one_way_platforms2.rs
View File

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

22
examples2d/platform2.rs
View File

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

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

10
examples2d/pyramid2.rs
View File

@@ -16,9 +16,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 10.0;
let ground_thickness = 1.0;
let rigid_body = RigidBodyBuilder::new_static().build();
let rigid_body = RigidBodyBuilder::new_static();
let ground_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_thickness).build();
let collider = ColliderBuilder::cuboid(ground_size, ground_thickness);
colliders.insert_with_parent(collider, ground_handle, &mut bodies);
/*
@@ -39,11 +39,9 @@ pub fn init_world(testbed: &mut Testbed) {
let y = fi * shift + centery;
// Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![x, y])
.build();
let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![x, y]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad, rad).build();
let collider = ColliderBuilder::cuboid(rad, rad);
colliders.insert_with_parent(collider, handle, &mut bodies);
}
}

15
examples2d/restitution2.rs
View File

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

21
examples2d/sensor2.rs
View File

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

20
examples2d/trimesh2.rs
View File

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

222
examples3d/articulations3.rs
View File

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

24
examples3d/ccd3.rs
View File

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

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

20
examples3d/compound3.rs
View File

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

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

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

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])
.angvel(Vector::z() * 100.0)
.linear_damping((i + 1) as f32 * subdiv * 10.0)
.angular_damping((num - i) as f32 * subdiv * 10.0)
.build();
.angular_damping((num - i) as f32 * subdiv * 10.0);
let rb_handle = bodies.insert(rb);
// Build the collider.
let co = ColliderBuilder::cuboid(rad, rad, rad).build();
let co = ColliderBuilder::cuboid(rad, rad, rad);
colliders.insert_with_parent(co, rb_handle, &mut bodies);
}

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

23
examples3d/debug_articulations3.rs
View File

@@ -25,17 +25,20 @@ fn create_ball_articulations(
RigidBodyType::Dynamic
};
let rigid_body = RigidBodyBuilder::new(status)
.translation(vector![fk * shift, 0.0, fi * shift * 2.0])
.build();
let rigid_body = RigidBodyBuilder::new(status).translation(vector![
fk * shift,
0.0,
fi * shift * 2.0
]);
let child_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::capsule_z(rad * 1.25, rad).build();
let collider = ColliderBuilder::capsule_z(rad * 1.25, rad);
colliders.insert_with_parent(collider, child_handle, bodies);
// Vertical multibody_joint.
if i > 0 {
let parent_handle = *body_handles.last().unwrap();
let joint = SphericalJoint::new().local_anchor2(point![0.0, 0.0, -shift * 2.0]);
let joint =
SphericalJointBuilder::new().local_anchor2(point![0.0, 0.0, -shift * 2.0]);
multibody_joints.insert(parent_handle, child_handle, joint);
}
@@ -43,7 +46,7 @@ fn create_ball_articulations(
if k > 0 && i > 0 {
let parent_index = body_handles.len() - num;
let parent_handle = body_handles[parent_index];
let joint = SphericalJoint::new().local_anchor2(point![-shift, 0.0, 0.0]);
let joint = SphericalJointBuilder::new().local_anchor2(point![-shift, 0.0, 0.0]);
// let joint =
// PrismaticJoint::new(Vector::y_axis()).local_anchor2(point![-shift, 0.0, 0.0]);
// let joint = FixedJoint::new().local_anchor2(point![-shift, 0.0, 0.0]);
@@ -68,15 +71,13 @@ pub fn init_world(testbed: &mut Testbed) {
let collider = ColliderBuilder::cuboid(30.0, 0.01, 30.0)
.translation(vector![0.0, -3.02, 0.0])
.rotation(vector![0.1, 0.0, 0.1])
.build();
.rotation(vector![0.1, 0.0, 0.1]);
colliders.insert(collider);
let rigid_body = RigidBodyBuilder::new_dynamic().build();
let rigid_body = RigidBodyBuilder::new_dynamic();
let collider = ColliderBuilder::cuboid(30.0, 0.01, 30.0)
.translation(vector![0.0, -3.0, 0.0])
.rotation(vector![0.1, 0.0, 0.1])
.build();
.rotation(vector![0.1, 0.0, 0.1]);
let handle = bodies.insert(rigid_body);
colliders.insert_with_parent(collider, handle, &mut bodies);

10
examples3d/debug_big_colliders3.rs
View File

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

12
examples3d/debug_boxes3.rs
View File

@@ -17,11 +17,10 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_height = 0.1;
for _ in 0..6 {
let rigid_body = RigidBodyBuilder::new_static()
.translation(vector![0.0, -ground_height, 0.0])
.build();
let rigid_body =
RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size);
colliders.insert_with_parent(collider, handle, &mut bodies);
}
@@ -30,10 +29,9 @@ pub fn init_world(testbed: &mut Testbed) {
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![1.1, 0.0, 0.0])
// .rotation(vector![0.8, 0.2, 0.1])
.can_sleep(false)
.build();
.can_sleep(false);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(2.0, 0.1, 1.0).build();
let collider = ColliderBuilder::cuboid(2.0, 0.1, 1.0);
colliders.insert_with_parent(collider, handle, &mut bodies);
}

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

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

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_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static().build();
let rigid_body = RigidBodyBuilder::new_static();
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size)
.friction(1.5)
.build();
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).friction(1.5);
colliders.insert_with_parent(collider, handle, &mut bodies);
// Build a dynamic box rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![0.0, 1.1, 0.0])
.rotation(Vector::y() * 0.3)
.build();
.rotation(Vector::y() * 0.3);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(2.0, 1.0, 3.0).friction(1.5).build();
let collider = ColliderBuilder::cuboid(2.0, 1.0, 3.0).friction(1.5);
colliders.insert_with_parent(collider, handle, &mut bodies);
let rigid_body = &mut bodies[handle];

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_height = 2.1;
let rigid_body = RigidBodyBuilder::new_static()
.translation(vector![0.0, 4.0, 0.0])
.build();
let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, 4.0, 0.0]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size);
colliders.insert_with_parent(collider, handle, &mut bodies);
let rad = 1.0;
// Build the dynamic box rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![0.0, 7.0 * rad, 0.0])
.can_sleep(false)
.build();
.can_sleep(false);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::ball(rad).build();
let collider = ColliderBuilder::ball(rad);
colliders.insert_with_parent(collider, handle, &mut bodies);
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![0.0, 2.0 * rad, 0.0])
.can_sleep(false)
.build();
.can_sleep(false);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::ball(rad).build();
let collider = ColliderBuilder::ball(rad);
colliders.insert_with_parent(collider, handle, &mut bodies);
/*

50
examples3d/debug_prismatic3.rs
View File

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

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

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

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],
];
let rigid_body = RigidBodyBuilder::new_static()
.translation(vector![0.0, 0.0, 0.0])
.build();
let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, 0.0, 0.0]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::triangle(vtx[0], vtx[1], vtx[2]).build();
let collider = ColliderBuilder::triangle(vtx[0], vtx[1], vtx[2]);
colliders.insert_with_parent(collider, handle, &mut bodies);
// Dynamic box rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![1.1, 0.01, 0.0])
// .rotation(Vector3::new(0.8, 0.2, 0.1))
.can_sleep(false)
.build();
.can_sleep(false);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(20.0, 0.1, 1.0).build();
let collider = ColliderBuilder::cuboid(20.0, 0.1, 1.0);
colliders.insert_with_parent(collider, handle, &mut bodies);
/*

11
examples3d/debug_trimesh3.rs
View File

@@ -41,17 +41,14 @@ pub fn init_world(testbed: &mut Testbed) {
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![0.0, 35.0, 0.0])
// .rotation(Vector3::new(0.8, 0.2, 0.1))
.can_sleep(false)
.build();
.can_sleep(false);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(1.0, 2.0, 1.0).build();
let collider = ColliderBuilder::cuboid(1.0, 2.0, 1.0);
colliders.insert_with_parent(collider, handle, &mut bodies);
let rigid_body = RigidBodyBuilder::new_static()
.translation(vector![0.0, 0.0, 0.0])
.build();
let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, 0.0, 0.0]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::trimesh(vtx, idx).build();
let collider = ColliderBuilder::trimesh(vtx, idx);
colliders.insert_with_parent(collider, handle, &mut bodies);
testbed.set_initial_body_color(handle, [0.3, 0.3, 0.3]);

10
examples3d/domino3.rs
View File

@@ -16,11 +16,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 200.1;
let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static()
.translation(vector![0.0, -ground_height, 0.0])
.build();
let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size);
colliders.insert_with_parent(collider, handle, &mut bodies);
/*
@@ -55,9 +53,9 @@ pub fn init_world(testbed: &mut Testbed) {
Translation::new(x * curr_rad, width * 2.0 + ground_height, z * curr_rad)
* tilt
* rot;
let rigid_body = RigidBodyBuilder::new_dynamic().position(position).build();
let rigid_body = RigidBodyBuilder::new_dynamic().position(position);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(thickness, width * 2.0, width).build();
let collider = ColliderBuilder::cuboid(thickness, width * 2.0, width);
colliders.insert_with_parent(collider, handle, &mut bodies);
testbed.set_initial_body_color(handle, colors[i % 2]);
} else {

16
examples3d/fountain3.rs
View File

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

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

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

276
examples3d/joints3.rs
View File

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

22
examples3d/keva3.rs
View File

@@ -38,15 +38,13 @@ pub fn build_block(
};
// Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![
let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![
x + dim.x + shift.x,
y + dim.y + shift.y,
z + dim.z + shift.z
])
.build();
]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z).build();
let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z);
colliders.insert_with_parent(collider, handle, bodies);
testbed.set_initial_body_color(handle, color0);
@@ -61,15 +59,13 @@ pub fn build_block(
for i in 0..(block_width / (dim.x as f32 * 2.0)) as usize {
for j in 0..(block_width / (dim.z as f32 * 2.0)) as usize {
// Build the rigid body.
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![
let rigid_body = RigidBodyBuilder::new_dynamic().translation(vector![
i as f32 * dim.x * 2.0 + dim.x + shift.x,
dim.y + shift.y + block_height,
j as f32 * dim.z * 2.0 + dim.z + shift.z
])
.build();
]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z).build();
let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z);
colliders.insert_with_parent(collider, handle, bodies);
testbed.set_initial_body_color(handle, color0);
std::mem::swap(&mut color0, &mut color1);
@@ -92,11 +88,9 @@ pub fn init_world(testbed: &mut Testbed) {
let ground_size = 50.0;
let ground_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static()
.translation(vector![0.0, -ground_height, 0.0])
.build();
let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size);
colliders.insert_with_parent(collider, handle, &mut bodies);
/*

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_height = 0.1;
let rigid_body = RigidBodyBuilder::new_static()
.translation(vector![0.0, -ground_height, 0.0])
.build();
let rigid_body = RigidBodyBuilder::new_static().translation(vector![0.0, -ground_height, 0.0]);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build();
let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size);
colliders.insert_with_parent(collider, handle, &mut bodies);
/*
@@ -32,10 +30,9 @@ pub fn init_world(testbed: &mut Testbed) {
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![0.0, 3.0, 0.0])
.lock_translations()
.restrict_rotations(true, false, false)
.build();
.restrict_rotations(true, false, false);
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(0.2, 0.6, 2.0).build();
let collider = ColliderBuilder::cuboid(0.2, 0.6, 2.0);
colliders.insert_with_parent(collider, handle, &mut bodies);
/*
@@ -44,12 +41,11 @@ pub fn init_world(testbed: &mut Testbed) {
let rigid_body = RigidBodyBuilder::new_dynamic()
.translation(vector![0.0, 5.0, 0.0])
.rotation(Vector::x() * 1.0)
.lock_rotations()
.build();
.lock_rotations();
let handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::capsule_y(0.6, 0.4).build();
let collider = ColliderBuilder::capsule_y(0.6, 0.4);
colliders.insert_with_parent(collider, handle, &mut bodies);
let collider = ColliderBuilder::capsule_x(0.6, 0.4).build();
let collider = ColliderBuilder::capsule_x(0.6, 0.4);
colliders.insert_with_parent(collider, handle, &mut bodies);
/*

14
examples3d/one_way_platforms3.rs
View File

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

32
examples3d/platform3.rs
View File

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

20
examples3d/primitives3.rs
View File

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

20
examples3d/restitution3.rs
View File

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

21
examples3d/sensor3.rs
View File

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

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

29
src/dynamics/integration_parameters.rs
View File

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

32
src/dynamics/island_manager.rs
View File

@@ -136,36 +136,6 @@ impl IslandManager {
.chain(self.active_kinematic_set.iter().copied())
}
/*
#[cfg(feature = "parallel")]
#[inline(always)]
#[allow(dead_code)]
pub(crate) fn foreach_active_island_body_mut_internal_parallel<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")]
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]
@@ -203,7 +173,7 @@ impl IslandManager {
// NOTE: the `.rev()` is here so that two successive timesteps preserve
// the order of the bodies in the `active_dynamic_set` vec. This reversal
// does not seem to affect performances nor stability. However it makes
// debugging slightly nicer so we keep this rev.
// debugging slightly nicer.
for h in self.active_dynamic_set.drain(..).rev() {
let can_sleep = &mut self.can_sleep;
let stack = &mut self.stack;

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};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
#[derive(Copy, Clone, Debug, PartialEq)]
#[repr(transparent)]
pub struct FixedJoint {
data: JointData,
data: GenericJoint,
}
impl Default for FixedJoint {
@@ -14,48 +15,100 @@ impl Default for FixedJoint {
}
impl FixedJoint {
#[must_use]
pub fn new() -> Self {
#[cfg(feature = "dim2")]
let mask = JointAxesMask::X | JointAxesMask::Y | JointAxesMask::ANG_X;
#[cfg(feature = "dim3")]
let mask = JointAxesMask::X
| JointAxesMask::Y
| JointAxesMask::Z
| JointAxesMask::ANG_X
| JointAxesMask::ANG_Y
| JointAxesMask::ANG_Z;
let data = JointData::default().lock_axes(mask);
let data = GenericJointBuilder::new(JointAxesMask::LOCKED_FIXED_AXES).build();
Self { data }
}
#[must_use]
pub fn local_frame1(&self) -> &Isometry<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]
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
}
#[must_use]
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
}
#[must_use]
pub fn local_anchor1(mut self, anchor1: Point<Real>) -> Self {
self.data = self.data.local_anchor1(anchor1);
self.0.set_local_anchor1(anchor1);
self
}
#[must_use]
pub fn local_anchor2(mut self, anchor2: Point<Real>) -> Self {
self.data = self.data.local_anchor2(anchor2);
self.0.set_local_anchor2(anchor2);
self
}
#[must_use]
pub fn build(self) -> FixedJoint {
self.0
}
}
impl Into<JointData> for FixedJoint {
fn into(self) -> JointData {
self.data
impl Into<GenericJoint> for FixedJointBuilder {
fn into(self) -> GenericJoint {
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};
#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))]
@@ -10,7 +10,7 @@ pub struct ImpulseJoint {
/// Handle to the second body attached to this joint.
pub body2: RigidBodyHandle,
pub data: JointData,
pub data: GenericJoint,
pub impulses: SpacialVector<Real>,
// 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::{BundleSet, Coarena, ComponentSet, ComponentSetMut};
use crate::dynamics::{GenericJoint, RigidBodyHandle};
use crate::dynamics::{IslandManager, RigidBodyActivation, RigidBodyIds, RigidBodyType};
use crate::dynamics::{JointData, RigidBodyHandle};
/// The unique identifier of a joint added to the joint set.
/// The unique identifier of a collider added to a collider set.
@@ -177,7 +177,7 @@ impl ImpulseJointSet {
&mut self,
body1: RigidBodyHandle,
body2: RigidBodyHandle,
data: impl Into<JointData>,
data: impl Into<GenericJoint>,
) -> ImpulseJointHandle {
let data = data.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::joint_data::*;
pub use self::generic_joint::*;
pub use self::motor_model::MotorModel;
pub use self::multibody_joint::*;
pub use self::prismatic_joint::PrismaticJoint;
pub use self::revolute_joint::RevoluteJoint;
pub use self::prismatic_joint::*;
pub use self::revolute_joint::*;
#[cfg(feature = "dim3")]
pub use self::spherical_joint::SphericalJoint;
pub use self::spherical_joint::*;
mod fixed_joint;
mod impulse_joint;
mod joint_data;
mod generic_joint;
mod motor_model;
mod multibody_joint;
mod prismatic_joint;

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))]
pub enum MotorModel {
/// The solved spring-like equation is:
/// `delta_velocity(t + dt) = stiffness / dt * (target_pos - pos(t)) + damping * (target_vel - vel(t))`
///
/// Here the `stiffness` is the ratio of position error to be solved at each timestep (like
/// a velocity-based ERP), and the `damping` is the ratio of velocity error to be solved at
/// each timestep.
VelocityBased,
/// The solved spring-like equation is:
/// `acceleration(t + dt) = stiffness * (target_pos - pos(t)) + damping * (target_vel - vel(t))`
/// `acceleration = stiffness * (pos - target_pos) + damping * (vel - target_vel)`
AccelerationBased,
// /// The solved spring-like equation is:
// /// `force(t + dt) = stiffness * (target_pos - pos(t + dt)) + damping * (target_vel - vel(t + dt))`
// ForceBased,
/// The solved spring-like equation is:
/// `force = stiffness * (pos - target_pos) + damping * (vel - target_vel)`
ForceBased,
}
impl Default for MotorModel {
fn default() -> Self {
MotorModel::VelocityBased
MotorModel::AccelerationBased
}
}
impl MotorModel {
/// Combines the coefficients used for solving the spring equation.
///
/// Returns the new coefficients (stiffness, damping, gamma, keep_inv_lhs)
/// coefficients for the equivalent impulse-based equation. These new
/// coefficients must be used in the following way:
/// - `rhs = (stiffness * pos_err + damping * vel_err) / gamma`.
/// - `new_inv_lhs = gamma * if keep_inv_lhs { inv_lhs } else { 1.0 }`.
/// Note that the returned `gamma` will be zero if both `stiffness` and `damping` are zero.
/// Returns the coefficients (erp_inv_dt, cfm_coeff, cfm_gain).
pub fn combine_coefficients(
self,
dt: Real,
stiffness: Real,
damping: Real,
) -> (Real, Real, Real, bool) {
) -> (Real, Real, Real) {
match self {
MotorModel::VelocityBased => (stiffness * crate::utils::inv(dt), damping, 1.0, true),
MotorModel::AccelerationBased => {
let effective_stiffness = stiffness * dt;
let effective_damping = damping * dt;
// TODO: Using gamma behaves very badly for some reasons.
// Maybe I got the formulation wrong, so let's keep it to 1.0 for now,
// and get back to this later.
// let gamma = effective_stiffness * dt + effective_damping;
(effective_stiffness, effective_damping, 1.0, true)
} // MotorModel::ForceBased => {
// let effective_stiffness = stiffness * dt;
// let effective_damping = damping * dt;
// let gamma = effective_stiffness * dt + effective_damping;
// (effective_stiffness, effective_damping, gamma, false)
// }
let erp_inv_dt = stiffness * crate::utils::inv(dt * stiffness + damping);
let cfm_coeff = crate::utils::inv(dt * dt * stiffness + dt * damping);
(erp_inv_dt, cfm_coeff, 0.0)
}
MotorModel::ForceBased => {
let erp_inv_dt = stiffness * crate::utils::inv(dt * stiffness + damping);
let cfm_gain = crate::utils::inv(dt * dt * stiffness + dt * damping);
(erp_inv_dt, 0.0, cfm_gain)
}
}
}
}

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

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

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::dynamics::joint::MultibodyLink;
use crate::dynamics::{
IslandManager, JointData, Multibody, MultibodyJoint, RigidBodyActivation, RigidBodyHandle,
GenericJoint, IslandManager, Multibody, MultibodyJoint, RigidBodyActivation, RigidBodyHandle,
RigidBodyIds, RigidBodyType,
};
use crate::geometry::{InteractionGraph, RigidBodyGraphIndex};
@@ -112,7 +112,7 @@ impl MultibodyJointSet {
&mut self,
body1: RigidBodyHandle,
body2: RigidBodyHandle,
data: impl Into<JointData>,
data: impl Into<GenericJoint>,
) -> Option<MultibodyJointHandle> {
let data = data.into();
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 max_enabled = limits[1] < curr_pos;
let erp_inv_dt = params.erp_inv_dt();
let erp_inv_dt = params.joint_erp_inv_dt();
let cfm_coeff = params.joint_cfm_coeff();
let rhs_bias = ((curr_pos - limits[1]).max(0.0) - (limits[0] - curr_pos).max(0.0)) * erp_inv_dt;
let rhs_wo_bias = joint_velocity[dof_id];
@@ -54,6 +55,8 @@ pub fn unit_joint_limit_constraint(
inv_lhs: crate::utils::inv(lhs),
rhs: rhs_wo_bias + rhs_bias,
rhs_wo_bias,
cfm_coeff,
cfm_gain: 0.0,
writeback_id: WritebackId::Limit(dof_id),
};
@@ -71,11 +74,13 @@ pub fn unit_joint_motor_constraint(
link: &MultibodyLink,
motor: &JointMotor,
curr_pos: Real,
limits: Option<[Real; 2]>,
dof_id: usize,
j_id: &mut usize,
jacobians: &mut DVector<Real>,
constraints: &mut Vec<AnyJointVelocityConstraint>,
) {
let inv_dt = params.inv_dt();
let ndofs = multibody.ndofs();
let joint_velocity = multibody.joint_velocity(link);
@@ -93,14 +98,20 @@ pub fn unit_joint_motor_constraint(
let impulse_bounds = [-motor_params.max_impulse, motor_params.max_impulse];
let mut rhs_wo_bias = 0.0;
if motor_params.stiffness != 0.0 {
rhs_wo_bias += (curr_pos - motor_params.target_pos) * motor_params.stiffness;
if motor_params.erp_inv_dt != 0.0 {
rhs_wo_bias += (curr_pos - motor_params.target_pos) * motor_params.erp_inv_dt;
}
if motor_params.damping != 0.0 {
let mut target_vel = motor_params.target_vel;
if let Some(limits) = limits {
target_vel = target_vel.clamp(
(limits[0] - curr_pos) * inv_dt,
(limits[1] - curr_pos) * inv_dt,
);
};
let dvel = joint_velocity[dof_id];
rhs_wo_bias += (dvel - motor_params.target_vel) * motor_params.damping;
}
rhs_wo_bias += dvel - target_vel;
let constraint = JointGenericVelocityGroundConstraint {
mj_lambda2: multibody.solver_id,
@@ -109,6 +120,8 @@ pub fn unit_joint_motor_constraint(
joint_id: usize::MAX,
impulse: 0.0,
impulse_bounds,
cfm_coeff: motor_params.cfm_coeff,
cfm_gain: motor_params.cfm_gain,
inv_lhs: crate::utils::inv(lhs),
rhs: rhs_wo_bias,
rhs_wo_bias,

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

6
src/dynamics/rigid_body.rs
View File

@@ -1096,3 +1096,9 @@ impl RigidBodyBuilder {
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.
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
// if this rigid-body was obtained by cloning another one.
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)
* (vel1 - vel2).dot(&force_dir1);
rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction;
rhs_wo_bias *= is_bouncy + is_resting;
let rhs_bias =
/* is_resting * */ erp_inv_dt * manifold_point.dist.min(0.0);

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)
* (vel1 - vel2).dot(&force_dir1);
rhs_wo_bias += manifold_point.dist.max(0.0) * inv_dt;
rhs_wo_bias *= is_bouncy + is_resting * params.velocity_solve_fraction;
rhs_wo_bias *= is_bouncy + is_resting ;
let rhs_bias =
/* is_resting * */ erp_inv_dt * manifold_point.dist.min(0.0);

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

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

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

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

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

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

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

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