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

Add prelude + use vectors for setting linvel/translation in builders

Browse Source
This commit is contained in:
Crozet Sébastien
2021-05-25 11:00:13 +02:00
parent 47139323e0
commit 1bef66fea9
93 changed files with 1528 additions and 1259 deletions
Download Patch File Download Diff File Expand all files Collapse all files

143
examples3d/joints3.rs
View File

@@ -1,49 +1,44 @@
use na::{Isometry3, Point3, Unit, UnitQuaternion, Vector3};
use rapier3d::dynamics::{
BallJoint, FixedJoint, JointSet, PrismaticJoint, RevoluteJoint, RigidBodyBuilder,
RigidBodyHandle, RigidBodySet, RigidBodyType,
};
use rapier3d::geometry::{ColliderBuilder, ColliderSet};
use rapier3d::prelude::*;
use rapier_testbed3d::Testbed;
fn create_prismatic_joints(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
joints: &mut JointSet,
origin: Point3<f32>,
origin: Point<f32>,
num: usize,
) {
let rad = 0.4;
let shift = 2.0;
let ground = RigidBodyBuilder::new_static()
.translation(origin.x, origin.y, origin.z)
.translation(vector![origin.x, origin.y, origin.z])
.build();
let mut curr_parent = bodies.insert(ground);
let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
colliders.insert(collider, curr_parent, bodies);
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(origin.x, origin.y, z)
.translation(vector![origin.x, origin.y, z])
.build();
let curr_child = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
colliders.insert(collider, curr_child, bodies);
colliders.insert_with_parent(collider, curr_child, bodies);
let axis = if i % 2 == 0 {
Unit::new_normalize(Vector3::new(1.0, 1.0, 0.0))
UnitVector::new_normalize(vector![1.0, 1.0, 0.0])
} else {
Unit::new_normalize(Vector3::new(-1.0, 1.0, 0.0))
UnitVector::new_normalize(vector![-1.0, 1.0, 0.0])
};
let z = Vector3::z();
let z = Vector::z();
let mut prism = PrismaticJoint::new(
Point3::new(0.0, 0.0, 0.0),
point![0.0, 0.0, 0.0],
axis,
z,
Point3::new(0.0, 0.0, -shift),
point![0.0, 0.0, -shift],
axis,
z,
);
@@ -61,40 +56,40 @@ fn create_actuated_prismatic_joints(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
joints: &mut JointSet,
origin: Point3<f32>,
origin: Point<f32>,
num: usize,
) {
let rad = 0.4;
let shift = 2.0;
let ground = RigidBodyBuilder::new_static()
.translation(origin.x, origin.y, origin.z)
.translation(vector![origin.x, origin.y, origin.z])
.build();
let mut curr_parent = bodies.insert(ground);
let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
colliders.insert(collider, curr_parent, bodies);
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(origin.x, origin.y, z)
.translation(vector![origin.x, origin.y, z])
.build();
let curr_child = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
colliders.insert(collider, curr_child, bodies);
colliders.insert_with_parent(collider, curr_child, bodies);
let axis = if i % 2 == 0 {
Unit::new_normalize(Vector3::new(1.0, 1.0, 0.0))
UnitVector::new_normalize(vector![1.0, 1.0, 0.0])
} else {
Unit::new_normalize(Vector3::new(-1.0, 1.0, 0.0))
UnitVector::new_normalize(vector![-1.0, 1.0, 0.0])
};
let z = Vector3::z();
let z = Vector::z();
let mut prism = PrismaticJoint::new(
Point3::new(0.0, 0.0, 0.0),
point![0.0, 0.0, 0.0],
axis,
z,
Point3::new(0.0, 0.0, -shift),
point![0.0, 0.0, -shift],
axis,
z,
);
@@ -128,27 +123,27 @@ fn create_revolute_joints(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
joints: &mut JointSet,
origin: Point3<f32>,
origin: Point<f32>,
num: usize,
) {
let rad = 0.4;
let shift = 2.0;
let ground = RigidBodyBuilder::new_static()
.translation(origin.x, origin.y, 0.0)
.translation(vector![origin.x, origin.y, 0.0])
.build();
let mut curr_parent = bodies.insert(ground);
let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
colliders.insert(collider, curr_parent, bodies);
colliders.insert_with_parent(collider, curr_parent, bodies);
for i in 0..num {
// Create four bodies.
let z = origin.z + i as f32 * shift * 2.0 + shift;
let positions = [
Isometry3::translation(origin.x, origin.y, z),
Isometry3::translation(origin.x + shift, origin.y, z),
Isometry3::translation(origin.x + shift, origin.y, z + shift),
Isometry3::translation(origin.x, origin.y, z + shift),
Isometry::translation(origin.x, origin.y, z),
Isometry::translation(origin.x + shift, origin.y, z),
Isometry::translation(origin.x + shift, origin.y, z + shift),
Isometry::translation(origin.x, origin.y, z + shift),
];
let mut handles = [curr_parent; 4];
@@ -158,19 +153,19 @@ fn create_revolute_joints(
.build();
handles[k] = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad, rad, rad).build();
colliders.insert(collider, handles[k], bodies);
colliders.insert_with_parent(collider, handles[k], bodies);
}
// Setup four joints.
let o = Point3::origin();
let x = Vector3::x_axis();
let z = Vector3::z_axis();
let o = Point::origin();
let x = Vector::x_axis();
let z = Vector::z_axis();
let revs = [
RevoluteJoint::new(o, z, Point3::new(0.0, 0.0, -shift), z),
RevoluteJoint::new(o, x, Point3::new(-shift, 0.0, 0.0), x),
RevoluteJoint::new(o, z, Point3::new(0.0, 0.0, -shift), z),
RevoluteJoint::new(o, x, Point3::new(shift, 0.0, 0.0), x),
RevoluteJoint::new(o, z, point![0.0, 0.0, -shift], z),
RevoluteJoint::new(o, x, point![-shift, 0.0, 0.0], x),
RevoluteJoint::new(o, z, point![0.0, 0.0, -shift], z),
RevoluteJoint::new(o, x, point![shift, 0.0, 0.0], x),
];
joints.insert(bodies, curr_parent, handles[0], revs[0]);
@@ -186,7 +181,7 @@ fn create_fixed_joints(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
joints: &mut JointSet,
origin: Point3<f32>,
origin: Point<f32>,
num: usize,
) {
let rad = 0.4;
@@ -209,18 +204,22 @@ fn create_fixed_joints(
};
let rigid_body = RigidBodyBuilder::new(status)
.translation(origin.x + fk * shift, origin.y, origin.z + fi * shift)
.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();
colliders.insert(collider, child_handle, bodies);
colliders.insert_with_parent(collider, child_handle, bodies);
// Vertical joint.
if i > 0 {
let parent_handle = *body_handles.last().unwrap();
let joint = FixedJoint::new(
Isometry3::identity(),
Isometry3::translation(0.0, 0.0, -shift),
Isometry::identity(),
Isometry::translation(0.0, 0.0, -shift),
);
joints.insert(bodies, parent_handle, child_handle, joint);
}
@@ -230,8 +229,8 @@ fn create_fixed_joints(
let parent_index = body_handles.len() - num;
let parent_handle = body_handles[parent_index];
let joint = FixedJoint::new(
Isometry3::identity(),
Isometry3::translation(-shift, 0.0, 0.0),
Isometry::identity(),
Isometry::translation(-shift, 0.0, 0.0),
);
joints.insert(bodies, parent_handle, child_handle, joint);
}
@@ -264,16 +263,16 @@ fn create_ball_joints(
};
let rigid_body = RigidBodyBuilder::new(status)
.translation(fk * shift, 0.0, fi * shift * 2.0)
.translation(vector![fk * shift, 0.0, fi * shift * 2.0])
.build();
let child_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::capsule_z(rad * 1.25, rad).build();
colliders.insert(collider, child_handle, bodies);
colliders.insert_with_parent(collider, child_handle, bodies);
// Vertical joint.
if i > 0 {
let parent_handle = *body_handles.last().unwrap();
let joint = BallJoint::new(Point3::origin(), Point3::new(0.0, 0.0, -shift * 2.0));
let joint = BallJoint::new(Point::origin(), point![0.0, 0.0, -shift * 2.0]);
joints.insert(bodies, parent_handle, child_handle, joint);
}
@@ -281,7 +280,7 @@ fn create_ball_joints(
if k > 0 {
let parent_index = body_handles.len() - num;
let parent_handle = body_handles[parent_index];
let joint = BallJoint::new(Point3::origin(), Point3::new(-shift, 0.0, 0.0));
let joint = BallJoint::new(Point::origin(), point![-shift, 0.0, 0.0]);
joints.insert(bodies, parent_handle, child_handle, joint);
}
@@ -294,7 +293,7 @@ fn create_actuated_revolute_joints(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
joints: &mut JointSet,
origin: Point3<f32>,
origin: Point<f32>,
num: usize,
) {
let rad = 0.4;
@@ -302,10 +301,10 @@ fn create_actuated_revolute_joints(
// We will reuse this base configuration for all the joints here.
let joint_template = RevoluteJoint::new(
Point3::origin(),
Vector3::z_axis(),
Point3::new(0.0, 0.0, -shift),
Vector3::z_axis(),
Point::origin(),
Vector::z_axis(),
point![0.0, 0.0, -shift],
Vector::z_axis(),
);
let mut parent_handle = RigidBodyHandle::invalid();
@@ -325,13 +324,13 @@ fn create_actuated_revolute_joints(
let shifty = (i >= 1) as u32 as f32 * -2.0;
let rigid_body = RigidBodyBuilder::new(status)
.translation(origin.x, origin.y + shifty, origin.z + fi * shift)
.translation(vector![origin.x, origin.y + shifty, origin.z + fi * shift])
// .rotation(Vector3::new(0.0, fi * 1.1, 0.0))
.build();
let child_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::cuboid(rad * 2.0, rad * 6.0 / (fi + 1.0), rad).build();
colliders.insert(collider, child_handle, bodies);
colliders.insert_with_parent(collider, child_handle, bodies);
if i > 0 {
let mut joint = joint_template.clone();
@@ -360,14 +359,14 @@ fn create_actuated_ball_joints(
bodies: &mut RigidBodySet,
colliders: &mut ColliderSet,
joints: &mut JointSet,
origin: Point3<f32>,
origin: Point<f32>,
num: usize,
) {
let rad = 0.4;
let shift = 2.0;
// We will reuse this base configuration for all the joints here.
let joint_template = BallJoint::new(Point3::new(0.0, 0.0, shift), Point3::origin());
let joint_template = BallJoint::new(point![0.0, 0.0, shift], Point::origin());
let mut parent_handle = RigidBodyHandle::invalid();
@@ -384,24 +383,24 @@ fn create_actuated_ball_joints(
};
let rigid_body = RigidBodyBuilder::new(status)
.translation(origin.x, origin.y, origin.z + fi * shift)
.translation(vector![origin.x, origin.y, origin.z + fi * shift])
// .rotation(Vector3::new(0.0, fi * 1.1, 0.0))
.build();
let child_handle = bodies.insert(rigid_body);
let collider = ColliderBuilder::capsule_y(rad * 2.0 / (fi + 1.0), rad).build();
colliders.insert(collider, child_handle, bodies);
colliders.insert_with_parent(collider, child_handle, bodies);
if i > 0 {
let mut joint = joint_template.clone();
if i == 1 {
joint.configure_motor_velocity(Vector3::new(0.0, 0.5, -2.0), 0.1);
joint.configure_motor_velocity(vector![0.0, 0.5, -2.0], 0.1);
} else if i == num - 1 {
let stiffness = 0.2;
let damping = 1.0;
joint.configure_motor_position(
UnitQuaternion::new(Vector3::new(0.0, 1.0, 3.14 / 2.0)),
Rotation::new(vector![0.0, 1.0, 3.14 / 2.0]),
stiffness,
damping,
);
@@ -426,42 +425,42 @@ pub fn init_world(testbed: &mut Testbed) {
&mut bodies,
&mut colliders,
&mut joints,
Point3::new(20.0, 5.0, 0.0),
point![20.0, 5.0, 0.0],
4,
);
create_actuated_prismatic_joints(
&mut bodies,
&mut colliders,
&mut joints,
Point3::new(25.0, 5.0, 0.0),
point![25.0, 5.0, 0.0],
4,
);
create_revolute_joints(
&mut bodies,
&mut colliders,
&mut joints,
Point3::new(20.0, 0.0, 0.0),
point![20.0, 0.0, 0.0],
3,
);
create_fixed_joints(
&mut bodies,
&mut colliders,
&mut joints,
Point3::new(0.0, 10.0, 0.0),
point![0.0, 10.0, 0.0],
10,
);
create_actuated_revolute_joints(
&mut bodies,
&mut colliders,
&mut joints,
Point3::new(20.0, 10.0, 0.0),
point![20.0, 10.0, 0.0],
6,
);
create_actuated_ball_joints(
&mut bodies,
&mut colliders,
&mut joints,
Point3::new(13.0, 10.0, 0.0),
point![13.0, 10.0, 0.0],
3,
);
create_ball_joints(&mut bodies, &mut colliders, &mut joints, 15);
@@ -470,5 +469,5 @@ pub fn init_world(testbed: &mut Testbed) {
* Set up the testbed.
*/
testbed.set_world(bodies, colliders, joints);
testbed.look_at(Point3::new(15.0, 5.0, 42.0), Point3::new(13.0, 1.0, 1.0));
testbed.look_at(point![15.0, 5.0, 42.0], point![13.0, 1.0, 1.0]);
}