diff options
| -rw-r--r-- | examples3d/all_examples3.rs | 2 | ||||
| -rw-r--r-- | examples3d/debug_prismatic3.rs | 110 | ||||
| -rw-r--r-- | examples3d/joints3.rs | 4 | ||||
| -rw-r--r-- | src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs | 55 |
4 files changed, 158 insertions, 13 deletions
diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index 8122583..2611a2f 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -23,6 +23,7 @@ mod debug_cylinder3; mod debug_dynamic_collider_add3; mod debug_friction3; mod debug_infinite_fall3; +mod debug_prismatic3; mod debug_rollback3; mod debug_triangle3; mod debug_trimesh3; @@ -109,6 +110,7 @@ pub fn main() { ("(Debug) trimesh", debug_trimesh3::init_world), ("(Debug) cylinder", debug_cylinder3::init_world), ("(Debug) infinite fall", debug_infinite_fall3::init_world), + ("(Debug) prismatic", debug_prismatic3::init_world), ("(Debug) rollback", debug_rollback3::init_world), ]; diff --git a/examples3d/debug_prismatic3.rs b/examples3d/debug_prismatic3.rs new file mode 100644 index 0000000..5ccf2eb --- /dev/null +++ b/examples3d/debug_prismatic3.rs @@ -0,0 +1,110 @@ +use na::{Point3, Vector3}; +use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed3d::Testbed; + +fn prismatic_repro( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + joints: &mut JointSet, + box_center: Point3<f32>, +) { + let box_rb = bodies.insert( + RigidBodyBuilder::new_dynamic() + .translation(box_center.x, box_center.y, box_center.z) + .build(), + ); + colliders.insert( + ColliderBuilder::cuboid(1.0, 0.25, 1.0).build(), + box_rb, + bodies, + ); + + let wheel_y = -1.0; + let wheel_positions = vec![ + Vector3::new(1.0, wheel_y, -1.0), + Vector3::new(-1.0, wheel_y, -1.0), + Vector3::new(1.0, wheel_y, 1.0), + Vector3::new(-1.0, wheel_y, 1.0), + ]; + + for pos in wheel_positions { + let wheel_pos_in_world = box_center + pos; + let wheel_rb = bodies.insert( + RigidBodyBuilder::new_dynamic() + .translation( + wheel_pos_in_world.x, + wheel_pos_in_world.y, + wheel_pos_in_world.z, + ) + .build(), + ); + colliders.insert(ColliderBuilder::ball(0.5).build(), wheel_rb, bodies); + + let mut prismatic = rapier3d::dynamics::PrismaticJoint::new( + Point3::new(pos.x, pos.y, pos.z), + Vector3::y_axis(), + Vector3::default(), + Point3::new(0.0, 0.0, 0.0), + Vector3::y_axis(), + Vector3::default(), + ); + prismatic.configure_motor_model(rapier3d::dynamics::SpringModel::VelocityBased); + let (stiffness, damping) = (0.05, 0.2); + prismatic.configure_motor_position(0.0, stiffness, damping); + + joints.insert(bodies, box_rb, wheel_rb, prismatic); + } + + // put a small box under one of the wheels + let gravel = bodies.insert( + RigidBodyBuilder::new_dynamic() + .translation(box_center.x + 1.0, box_center.y - 2.4, -1.0) + .build(), + ); + colliders.insert( + ColliderBuilder::cuboid(0.5, 0.1, 0.5).build(), + gravel, + bodies, + ); +} + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut joints = JointSet::new(); + + /* + * Ground + */ + let ground_size = 50.0; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::new_static() + .translation(0.0, -ground_height, 0.0) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + colliders.insert(collider, handle, &mut bodies); + + prismatic_repro( + &mut bodies, + &mut colliders, + &mut joints, + Point3::new(0.0, 5.0, 0.0), + ); + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point3::new(10.0, 10.0, 10.0), Point3::origin()); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]); + testbed.run() +} diff --git a/examples3d/joints3.rs b/examples3d/joints3.rs index 1246bc4..6ba9461 100644 --- a/examples3d/joints3.rs +++ b/examples3d/joints3.rs @@ -107,12 +107,12 @@ fn create_actuated_prismatic_joints( // the limits with large forces. prism.motor_max_impulse = 1.0; } else if i > 1 { - prism.configure_motor_position(2.0, 0.2, 1.0); + prism.configure_motor_position(2.0, 0.01, 1.0); } else { prism.configure_motor_velocity(1.0, 1.0); // We set a max impulse so that the motor doesn't fight // the limits with large forces. - prism.motor_max_impulse = 1.0; + prism.motor_max_impulse = 0.7; prism.limits_enabled = true; prism.limits[0] = -2.0; prism.limits[1] = 5.0; diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs index 23f746f..9d64193 100644 --- a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs @@ -83,6 +83,7 @@ impl PrismaticVelocityConstraint { let anchor2 = rb2.position * joint.local_anchor2; let axis1 = rb1.position * joint.local_axis1; let axis2 = rb2.position * joint.local_axis2; + #[cfg(feature = "dim2")] let basis1 = rb1.position * joint.basis1[0]; #[cfg(feature = "dim3")] @@ -179,6 +180,8 @@ impl PrismaticVelocityConstraint { */ let mut motor_rhs = 0.0; let mut motor_inv_lhs = 0.0; + let gcross1 = r1.gcross(*axis1); + let gcross2 = r2.gcross(*axis2); let (stiffness, damping, gamma, keep_lhs) = joint.motor_model.combine_coefficients( params.dt, @@ -192,12 +195,24 @@ impl PrismaticVelocityConstraint { } if damping != 0.0 { - let curr_vel = rb2.linvel.dot(&axis2) - rb1.linvel.dot(&axis1); + let curr_vel = rb2.linvel.dot(&axis2) + rb2.angvel.gdot(gcross2) + - rb1.linvel.dot(&axis1) + - rb1.angvel.gdot(gcross1); motor_rhs += (curr_vel - joint.motor_target_vel) * damping; } if stiffness != 0.0 || damping != 0.0 { - motor_inv_lhs = if keep_lhs { gamma / (im1 + im2) } else { gamma }; + motor_inv_lhs = if keep_lhs { + let inv_projected_mass = crate::utils::inv( + im1 + im2 + + gcross1.gdot(ii1.transform_vector(gcross1)) + + gcross2.gdot(ii2.transform_vector(gcross2)), + ); + + gamma * inv_projected_mass + } else { + gamma + }; motor_rhs /= gamma; } @@ -236,8 +251,6 @@ impl PrismaticVelocityConstraint { limits_rhs += ((dist - max_limit).max(0.0) - (min_limit - dist).max(0.0)) * velocity_based_erp_inv_dt; - let gcross1 = r1.gcross(*axis1); - let gcross2 = r2.gcross(*axis2); limits_inv_lhs = crate::utils::inv( im1 + im2 + gcross1.gdot(ii1.transform_vector(gcross1)) @@ -301,8 +314,16 @@ impl PrismaticVelocityConstraint { .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); // Warmstart motors. - mj_lambda1.linear += self.motor_axis1 * (self.im1 * self.motor_impulse); - mj_lambda2.linear -= self.motor_axis2 * (self.im2 * self.motor_impulse); + if self.motor_impulse != 0.0 { + let lin_impulse1 = self.motor_axis1 * self.motor_impulse; + let lin_impulse2 = self.motor_axis2 * self.motor_impulse; + + mj_lambda1.linear += lin_impulse1 * self.im1; + mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(lin_impulse1)); + + mj_lambda2.linear -= lin_impulse2 * self.im2; + mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(lin_impulse2)); + } // Warmstart limits. if self.limits_active { @@ -382,19 +403,31 @@ impl PrismaticVelocityConstraint { fn solve_motors(&mut self, mj_lambda1: &mut DeltaVel<Real>, mj_lambda2: &mut DeltaVel<Real>) { if self.motor_inv_lhs != 0.0 { - let lin_dvel = self.motor_axis2.dot(&mj_lambda2.linear) - - self.motor_axis1.dot(&mj_lambda1.linear) + let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let dvel = self + .motor_axis2 + .dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2))) + - self + .motor_axis1 + .dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1))) + self.motor_rhs; let new_impulse = na::clamp( - self.motor_impulse + lin_dvel * self.motor_inv_lhs, + self.motor_impulse + dvel * self.motor_inv_lhs, -self.motor_max_impulse, self.motor_max_impulse, ); let dimpulse = new_impulse - self.motor_impulse; self.motor_impulse = new_impulse; - mj_lambda1.linear += self.motor_axis1 * (self.im1 * dimpulse); - mj_lambda2.linear -= self.motor_axis2 * (self.im2 * dimpulse); + let lin_impulse1 = self.motor_axis1 * dimpulse; + let lin_impulse2 = self.motor_axis2 * dimpulse; + + mj_lambda1.linear += lin_impulse1 * self.im1; + mj_lambda1.angular += self.ii1_sqrt.transform_vector(self.r1.gcross(lin_impulse1)); + mj_lambda2.linear -= lin_impulse2 * self.im2; + mj_lambda2.angular -= self.ii2_sqrt.transform_vector(self.r2.gcross(lin_impulse2)); } } |