diff options
34 files changed, 1742 insertions, 226 deletions
diff --git a/CHANGELOG.md b/CHANGELOG.md index 5d31a08..72c99a1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -1,8 +1,29 @@ + +## Unreleased +### Added +- Add `RigidBody::set_enabled`, `RigidBody::is_enabled`, `RigidBodyBuilder::enabled` to enable/disable a rigid-body + without having to delete it. Disabling a rigid-body attached to a multibody joint isn’t supported yet. +- Add `Collider::set_enabled`, `Collider::is_enabled`, `ColliderBuilder::enabled` to enable/disable a collider + without having to delete it. +- Add `GenericJoint::set_enabled`, `GenericJoint::is_enabled` to enable/disable a joint without having to delete it. + Disabling a multibody joint isn’t supported yet. +- Add `DynamicRayCastVehicleController`, a vehicle controller based on ray-casting and dynamic rigid-bodies (mostly + a port of the vehicle controller from Bullet physics). + +### Modified +- Add the `QueryPipeline` as an optional argument to `PhysicsPipeline::step` and `CollisionPipeline::step`. If this + argument is specified, then the query pipeline will be incrementally (i.e. more efficiently) update at the same time as + these other pipelines. In that case, calling `QueryPipeline::update` a `PhysicsPipeline::step` isn’t needed. +- `RigidBody::set_body_type` now takes an extra boolean argument indicating if the rigid-body should be woken-up + (if it becomes dynamic). + +### Fix +- Fix bug resulting in rigid-bodies being awakened after they are created, even if they are created sleeping. + ## v0.16.1 (10 Nov. 2022) ### Fix - Fixed docs build on `docs.rs`. - ## v0.16.0 (30 Oct. 2022) ### Added - Implement `Copy` for `CharacterCollision`. diff --git a/benchmarks3d/all_benchmarks3.rs b/benchmarks3d/all_benchmarks3.rs index 6a3f756..bca730b 100644 --- a/benchmarks3d/all_benchmarks3.rs +++ b/benchmarks3d/all_benchmarks3.rs @@ -20,6 +20,7 @@ mod joint_fixed3; mod joint_prismatic3; mod joint_revolute3; mod keva3; +mod many_static3; mod pyramid3; mod stacks3; mod trimesh3; @@ -54,6 +55,7 @@ pub fn main() { ("CCD", ccd3::init_world), ("Compound", compound3::init_world), ("Convex polyhedron", convex_polyhedron3::init_world), + ("Many static", many_static3::init_world), ("Heightfield", heightfield3::init_world), ("Stacks", stacks3::init_world), ("Pyramid", pyramid3::init_world), diff --git a/benchmarks3d/many_static3.rs b/benchmarks3d/many_static3.rs new file mode 100644 index 0000000..3a0dbea --- /dev/null +++ b/benchmarks3d/many_static3.rs @@ -0,0 +1,52 @@ +use rapier3d::prelude::*; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + /* + * Create the balls + */ + let num = 50; + let rad = 1.0; + + let shift = rad * 2.0 + 1.0; + let centerx = shift * (num as f32) / 2.0; + let centery = shift / 2.0; + let centerz = shift * (num as f32) / 2.0; + + for i in 0..num { + for j in 0usize..num { + for k in 0..num { + let x = i as f32 * shift - centerx; + let y = j as f32 * shift + centery; + let z = k as f32 * shift - centerz; + + let status = if j < num - 1 { + RigidBodyType::Fixed + } else { + RigidBodyType::Dynamic + }; + let density = 0.477; + + // Build the rigid body. + let rigid_body = RigidBodyBuilder::new(status).translation(vector![x, y, z]); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::ball(rad).density(density); + colliders.insert_with_parent(collider, handle, &mut bodies); + } + } + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.look_at(point![100.0, 100.0, 100.0], Point::origin()); +} diff --git a/crates/rapier2d-f64/Cargo.toml b/crates/rapier2d-f64/Cargo.toml index b949095..60ea169 100644 --- a/crates/rapier2d-f64/Cargo.toml +++ b/crates/rapier2d-f64/Cargo.toml @@ -52,7 +52,7 @@ vec_map = { version = "0.8", optional = true } instant = { version = "0.1", features = [ "now" ], optional = true } num-traits = "0.2" nalgebra = "0.31" -parry2d-f64 = "^0.11.1" +parry2d-f64 = "0.12" simba = "0.7" approx = "0.5" rayon = { version = "1", optional = true } diff --git a/crates/rapier2d/Cargo.toml b/crates/rapier2d/Cargo.toml index 2cc9004..115069c 100644 --- a/crates/rapier2d/Cargo.toml +++ b/crates/rapier2d/Cargo.toml @@ -52,7 +52,7 @@ vec_map = { version = "0.8", optional = true } instant = { version = "0.1", features = [ "now" ], optional = true } num-traits = "0.2" nalgebra = "0.31" -parry2d = "^0.11.1" +parry2d = "0.12" simba = "0.7" approx = "0.5" rayon = { version = "1", optional = true } diff --git a/crates/rapier3d-f64/Cargo.toml b/crates/rapier3d-f64/Cargo.toml index 929e688..79c2783 100644 --- a/crates/rapier3d-f64/Cargo.toml +++ b/crates/rapier3d-f64/Cargo.toml @@ -52,7 +52,7 @@ vec_map = { version = "0.8", optional = true } instant = { version = "0.1", features = [ "now" ], optional = true } num-traits = "0.2" nalgebra = "0.31" -parry3d-f64 = "^0.11.1" +parry3d-f64 = "0.12" simba = "0.7" approx = "0.5" rayon = { version = "1", optional = true } diff --git a/crates/rapier3d/Cargo.toml b/crates/rapier3d/Cargo.toml index 942e618..88c5281 100644 --- a/crates/rapier3d/Cargo.toml +++ b/crates/rapier3d/Cargo.toml @@ -52,7 +52,7 @@ vec_map = { version = "0.8", optional = true } instant = { version = "0.1", features = [ "now" ], optional = true } num-traits = "0.2" nalgebra = "0.31" -parry3d = "^0.11.1" +parry3d = "0.12" simba = "0.7" approx = "0.5" rayon = { version = "1", optional = true } diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs index 749cec0..194d7cb 100644 --- a/examples3d/all_examples3.rs +++ b/examples3d/all_examples3.rs @@ -44,6 +44,7 @@ mod primitives3; mod restitution3; mod sensor3; mod trimesh3; +mod vehicle_controller3; fn demo_name_from_command_line() -> Option<String> { let mut args = std::env::args(); @@ -101,6 +102,7 @@ pub fn main() { ("Restitution", restitution3::init_world), ("Sensor", sensor3::init_world), ("TriMesh", trimesh3::init_world), + ("Vehicle controller", vehicle_controller3::init_world), ("Keva tower", keva3::init_world), ("Newton cradle", newton_cradle3::init_world), ("(Debug) multibody_joints", debug_articulations3::init_world), diff --git a/examples3d/vehicle_controller3.rs b/examples3d/vehicle_controller3.rs new file mode 100644 index 0000000..1228c82 --- /dev/null +++ b/examples3d/vehicle_controller3.rs @@ -0,0 +1,160 @@ +use rapier3d::control::{DynamicRayCastVehicleController, WheelTuning}; +use rapier3d::prelude::*; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let impulse_joints = ImpulseJointSet::new(); + let multibody_joints = MultibodyJointSet::new(); + + /* + * Ground + */ + let ground_size = 5.0; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::fixed().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); + colliders.insert_with_parent(collider, floor_handle, &mut bodies); + + /* + * Vehicle we will control manually. + */ + let hw = 0.3; + let hh = 0.15; + let rigid_body = RigidBodyBuilder::dynamic().translation(vector![0.0, 1.0, 0.0]); + let vehicle_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(hw * 2.0, hh, hw).density(100.0); + colliders.insert_with_parent(collider, vehicle_handle, &mut bodies); + + let mut tuning = WheelTuning::default(); + tuning.suspension_stiffness = 100.0; + tuning.suspension_damping = 10.0; + let mut vehicle = DynamicRayCastVehicleController::new(vehicle_handle); + let wheel_positions = [ + point![hw * 1.5, -hh, hw], + point![hw * 1.5, -hh, -hw], + point![-hw * 1.5, -hh, hw], + point![-hw * 1.5, -hh, -hw], + ]; + + for pos in wheel_positions { + vehicle.add_wheel(pos, -Vector::y(), Vector::z(), hh, hh / 4.0, &tuning); + } + + /* + * Create the cubes + */ + let num = 8; + let rad = 0.1; + + let shift = rad * 2.0; + let centerx = shift * (num / 2) as f32; + let centery = rad; + + for j in 0usize..1 { + for k in 0usize..4 { + for i in 0..num { + let x = i as f32 * shift - centerx; + let y = j as f32 * shift + centery; + let z = k as f32 * shift + centerx; + + let rigid_body = RigidBodyBuilder::dynamic().translation(vector![x, y, z]); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad, rad); + colliders.insert_with_parent(collider, handle, &mut bodies); + } + } + } + + /* + * Create a slope we can climb. + */ + let slope_angle = 0.2; + let slope_size = 2.0; + let collider = ColliderBuilder::cuboid(slope_size, ground_height, ground_size) + .translation(vector![ground_size + slope_size, -ground_height + 0.4, 0.0]) + .rotation(Vector::z() * slope_angle); + colliders.insert(collider); + + /* + * Create a slope we can’t climb. + */ + let impossible_slope_angle = 0.9; + let impossible_slope_size = 2.0; + let collider = ColliderBuilder::cuboid(slope_size, ground_height, ground_size) + .translation(vector![ + ground_size + slope_size * 2.0 + impossible_slope_size - 0.9, + -ground_height + 2.3, + 0.0 + ]) + .rotation(Vector::z() * impossible_slope_angle); + colliders.insert(collider); + + // /* + // * Create a moving platform. + // */ + // let body = RigidBodyBuilder::kinematic_velocity_based().translation(vector![-8.0, 1.5, 0.0]); + // // .rotation(-0.3); + // let platform_handle = bodies.insert(body); + // let collider = ColliderBuilder::cuboid(2.0, ground_height, 2.0); + // colliders.insert_with_parent(collider, platform_handle, &mut bodies); + + /* + * More complex ground. + */ + let ground_size = Vector::new(10.0, 0.4, 10.0); + let nsubdivs = 20; + + let heights = DMatrix::from_fn(nsubdivs + 1, nsubdivs + 1, |i, j| { + -(i as f32 * ground_size.x / (nsubdivs as f32) / 2.0).cos() + - (j as f32 * ground_size.z / (nsubdivs as f32) / 2.0).cos() + }); + + let collider = + ColliderBuilder::heightfield(heights, ground_size).translation(vector![-7.0, 0.0, 0.0]); + colliders.insert(collider); + + // /* + // * A tilting dynamic body with a limited joint. + // */ + // let ground = RigidBodyBuilder::fixed().translation(vector![0.0, 5.0, 0.0]); + // let ground_handle = bodies.insert(ground); + // let body = RigidBodyBuilder::dynamic().translation(vector![0.0, 5.0, 0.0]); + // let handle = bodies.insert(body); + // let collider = ColliderBuilder::cuboid(1.0, 0.1, 2.0); + // colliders.insert_with_parent(collider, handle, &mut bodies); + // let joint = RevoluteJointBuilder::new(Vector::z_axis()).limits([-0.3, 0.3]); + // impulse_joints.insert(ground_handle, handle, joint, true); + + // /* + // * Setup a callback to move the platform. + // */ + // testbed.add_callback(move |_, physics, _, run_state| { + // let linvel = vector![ + // (run_state.time * 2.0).sin() * 2.0, + // (run_state.time * 5.0).sin() * 1.5, + // 0.0 + // ]; + // // let angvel = run_state.time.sin() * 0.5; + // + // // Update the velocity-based kinematic body by setting its velocity. + // if let Some(platform) = physics.bodies.get_mut(platform_handle) { + // platform.set_linvel(linvel, true); + // // NOTE: interaction with rotating platforms isn’t handled very well yet. + // // platform.set_angvel(angvel, true); + // } + // }); + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, impulse_joints, multibody_joints); + testbed.set_vehicle_controller(vehicle); + testbed.look_at(point!(10.0, 10.0, 10.0), Point::origin()); +} diff --git a/src/control/character_controller.rs b/src/control/character_controller.rs index ebed952..28ba475 100644 --- a/src/control/character_controller.rs +++ b/src/control/character_controller.rs @@ -133,7 +133,7 @@ impl Default for KinematicCharacterController { pub struct EffectiveCharacterMovement { /// The movement to apply. pub translation: Vector<Real>, - /// Is the character touching the ground after applying `EffictiveKineamticMovement::translation`? + /// Is the character touching the ground after applying `EffectiveKineamticMovement::translation`? pub grounded: bool, } diff --git a/src/control/mod.rs b/src/control/mod.rs index 0290f2c..3f7dea1 100644 --- a/src/control/mod.rs +++ b/src/control/mod.rs @@ -5,4 +5,10 @@ pub use self::character_controller::{ KinematicCharacterController, }; +#[cfg(feature = "dim3")] +pub use self::ray_cast_vehicle_controller::{DynamicRayCastVehicleController, Wheel, WheelTuning}; + mod character_controller; + +#[cfg(feature = "dim3")] +mod ray_cast_vehicle_controller; diff --git a/src/control/ray_cast_vehicle_controller.rs b/src/control/ray_cast_vehicle_controller.rs new file mode 100644 index 0000000..42bb112 --- /dev/null +++ b/src/control/ray_cast_vehicle_controller.rs @@ -0,0 +1,808 @@ +//! A vehicle controller based on ray-casting, ported and modified from Bullet’s `btRaycastVehicle`. + +use crate::dynamics::{RigidBody, RigidBodyHandle, RigidBodySet}; +use crate::geometry::{ColliderHandle, ColliderSet, Ray}; +use crate::math::{Point, Real, Rotation, Vector}; +use crate::pipeline::{QueryFilter, QueryPipeline}; +use crate::utils::{WCross, WDot}; + +/// A character controller to simulate vehicles using ray-casting for the wheels. +pub struct DynamicRayCastVehicleController { + wheels: Vec<Wheel>, + forward_ws: Vec<Vector<Real>>, + axle: Vec<Vector<Real>>, + /// The current forward speed of the vehicle. + pub current_vehicle_speed: Real, + + /// Handle of the vehicle’s chassis. + pub chassis: RigidBodyHandle, + /// The chassis’ local _up_ direction (`0 = x, 1 = y, 2 = z`) + pub index_up_axis: usize, + /// The chassis’ local _forward_ direction (`0 = x, 1 = y, 2 = z`) + pub index_forward_axis: usize, +} + +#[derive(Copy, Clone, Debug, PartialEq)] +/// Parameters affecting the physical behavior of a wheel. +pub struct WheelTuning { + /// The suspension stiffness. + /// + /// Increase this value if the suspension appears to not push the vehicle strong enough. + pub suspension_stiffness: Real, + /// The suspension’s damping when it is being compressed. + pub suspension_compression: Real, + /// The suspension’s damping when it is being released. + /// + /// Increase this value if the suspension appears to overshoot. + pub suspension_damping: Real, + /// The maximum distance the suspension can travel before and after its resting length. + pub max_suspension_travel: Real, + /// Parameter controlling how much traction the tire his. + /// + /// The larger the value, the more instantaneous braking will happen (with the risk of + /// causing the vehicle to flip if it’s too strong). + pub friction_slip: Real, + /// The maximum force applied by the suspension. + pub max_suspension_force: Real, +} + +impl Default for WheelTuning { + fn default() -> Self { + Self { + suspension_stiffness: 5.88, + suspension_compression: 0.83, + suspension_damping: 0.88, + max_suspension_travel: 5.0, + friction_slip: 10.5, + max_suspension_force: 6000.0, + } + } +} + +/// Objects used to initialize a wheel. +struct WheelDesc { + /// The position of the wheel, relative to the chassis. + pub chassis_connection_cs: Point<Real>, + /// The direction of the wheel’s suspension, relative to the chassis. + /// + /// The ray-casting will happen following this direction to detect the ground. + pub direction_cs: Vector<Real>, + /// The wheel’s axle axis, relative to the chassis. + pub axle_cs: Vector<Real>, + /// The rest length of the wheel’s suspension spring. + pub suspension_rest_length: Real, + /// The maximum distance the suspension can travel before and after its resting length. + pub max_suspension_travel: Real, + /// The wheel’s radius. + pub radius: Real, + + /// The suspension stiffness. + /// + /// Increase this value if the suspension appears to not push the vehicle strong enough. + pub suspension_stiffness: Real, + /// The suspension’s damping when it is being compressed. + pub damping_compression: Real, + /// The suspension’s damping when it is being released. + /// + /// Increase this value if the suspension appears to overshoot. + pub damping_relaxation: Real, + /// Parameter controlling how much traction the tire his. + /// + /// The larger the value, the more instantaneous braking will happen (with the risk of + /// causing the vehicle to flip if it’s too strong). + pub friction_slip: Real, + /// The maximum force applied by the suspension. + pub max_suspension_force: Real, +} + +#[derive(Copy, Clone, Debug, PartialEq)] +/// A wheel attached to a vehicle. +pub struct Wheel { + raycast_info: RayCastInfo, + + center: Point<Real>, + wheel_direction_ws: Vector<Real>, + wheel_axle_ws: Vector<Real>, + + /// The position of the wheel, relative to the chassis. + pub chassis_connection_point_cs: Point<Real>, + /// The direction of the wheel’s suspension, relative to the chassis. + /// + /// The ray-casting will happen following this direction to detect the ground. + pub direction_cs: Vector<Real>, + /// The wheel’s axle axis, relative to the chassis. + pub axle_cs: Vector<Real>, + /// The rest length of the wheel’s suspension spring. + pub suspension_rest_length: Real, + /// The maximum distance the suspension can travel before and after its resting length. + pub max_suspension_travel: Real, + /// The wheel’s radius. + pub radius: Real, + /// The suspension stiffness. + /// + /// Increase this value if the suspension appears to not push the vehicle strong enough. + pub suspension_stiffness: Real, + /// The suspension’s damping when it is being compressed. + pub damping_compression: Real, + /// The suspension’s damping when it is being released. + /// + /// Increase this value if the suspension appears to overshoot. + pub damping_relaxation: Real, + /// Parameter controlling how much traction the tire his. + /// + /// The larger the value, the more instantaneous braking will happen (with the risk of + /// causing the vehicle to flip if it’s too strong). + friction_slip: Real, + /// The wheel’s current rotation on its axle. + pub rotation: Real, + delta_rotation: Real, + roll_influence: Real, // TODO: make this public? + /// The maximum force applied by the suspension. + pub max_suspension_force: Real, + + /// The forward impulses applied by the wheel on the chassis. + pub forward_impulse: Real, + /// The side impulses applied by the wheel on the chassis. + pub side_impulse: Real, + + /// The steering angle for this wheel. + pub steering: Real, + /// The forward force applied by this wheel on the chassis. + pub engine_force: Real, + /// The maximum amount of braking impulse applied to slow down the vehicle. + pub brake: Real, + + clipped_inv_contact_dot_suspension: Real, + suspension_relative_velocity: Real, + /// The force applied by the suspension. + pub wheel_suspension_force: Real, + skid_info: Real, +} + +impl Wheel { + fn new(info: WheelDesc) -> Self { + Self { + raycast_info: RayCastInfo::default(), + suspension_rest_length: info.suspension_rest_length, + max_suspension_travel: info.max_suspension_travel, + radius: info.radius, + suspension_stiffness: info.suspension_stiffness, + damping_compression: info.damping_compression, + damping_relaxation: info.damping_relaxation, + chassis_connection_point_cs: info.chassis_connection_cs, + direction_cs: info.direction_cs, + axle_cs: info.axle_cs, + wheel_direction_ws: info.direction_cs, + wheel_axle_ws: info.axle_cs, + center: Point::origin(), + friction_slip: info.friction_slip, + steering: 0.0, + engine_force: 0.0, + rotation: 0.0, + delta_rotation: 0.0, + brake: 0.0, + roll_influence: 0.1, + clipped_inv_contact_dot_suspension: 0.0, + suspension_relative_velocity: 0.0, + wheel_suspension_force: 0.0, + max_suspension_force: info.max_suspension_force, + skid_info: 0.0, + side_impulse: 0.0, + forward_impulse: 0.0, + } + } + + /// The world-space center of the wheel. + pub fn center(&self) -> Point<Real> { + self.center + } + + /// The world-space direction of the wheel’s suspension. + pub fn suspension(&self) -> Vector<Real> { + self.wheel_direction_ws + } + + /// The world-space direction of the wheel’s axle. + pub fn axle(&self) -> Vector<Real> { + self.wheel_axle_ws + } +} + +#[derive(Copy, Clone, Debug, PartialEq, Default)] +struct RayCastInfo { + // set by raycaster + contact_normal_ws: Vector<Real>, //contact normal + contact_point_ws: Point<Real>, //raycast hitpoint + suspension_length: Real, + hard_point_ws: Point<Real>, //raycast starting point + is_in_contact: bool, + ground_object: Option<ColliderHandle>, +} + +impl DynamicRayCastVehicleController { + /// Creates a new vehicle represented by the given rigid-body. + /// + /// Wheels have to be attached afterwards calling [`Self::add_wheel`]. + pub fn new(chassis: RigidBodyHandle) -> Self { + Self { + wheels: vec![], + forward_ws: vec![], + axle: vec![], + current_vehicle_speed: 0.0, + chassis, + index_up_axis: 1, + index_forward_axis: 0, + } + } + + // + // basically most of the code is general for 2 or 4 wheel vehicles, but some of it needs to be reviewed + // + /// Adds a wheel to this vehicle. + pub fn add_wheel( + &mut self, + chassis_connection_cs: Point<Real>, + direction_cs: Vector<Real>, + axle_cs: Vector<Real>, + suspension_rest_length: Real, + radius: Real, + tuning: &WheelTuning, + ) -> &mut Wheel { + let ci = WheelDesc { + chassis_connection_cs, + direction_cs, + axle_cs, + suspension_rest_length, + radius, + suspension_stiffness: tuning.suspension_stiffness, + damping_compression: tuning.suspension_compression, + damping_relaxation: tuning.suspension_damping, + friction_slip: tuning.friction_slip, + max_suspension_travel: tuning.max_suspension_travel, + max_suspension_force: tuning.max_suspension_force, + }; + + let wheel_id = self.wheels.len(); + self.wheels.push(Wheel::new(ci)); + + &mut self.wheels[wheel_id] + } + + #[cfg(feature = "dim2")] + fn update_wheel_transform(&mut self, chassis: &RigidBody, wheel_index: usize) { + self.update_wheel_transforms_ws(chassis, wheel_index); + let wheel = &mut self.wheels[wheel_index]; + wheel.center = (wheel.raycast_info.hard_point_ws + + wheel.wheel_direction_ws * wheel.raycast_info.suspension_length) + .coords; + } + + #[cfg(feature = "dim3")] + fn update_wheel_transform(&mut self, chassis: &RigidBody, wheel_index: usize) { + self.update_wheel_transforms_ws(chassis, wheel_index); + let wheel = &mut self.wheels[wheel_index]; + + let steering_orn = Rotation::new(-wheel.wheel_direction_ws * wheel.steering); + wheel.wheel_axle_ws = steering_orn * (chassis.position() * wheel.axle_cs); + wheel.center = wheel.raycast_info.hard_point_ws + + wheel.wheel_direction_ws * wheel.raycast_info.suspension_length; + } + + fn update_wheel_transforms_ws(&mut self, chassis: &RigidBody, wheel_id: usize) { + let wheel = &mut self.wheels[wheel_id]; + wheel.raycast_info.is_in_contact = false; + + let chassis_transform = chassis.position(); + + wheel.raycast_info.hard_point_ws = chassis_transform * wheel.chassis_connection_point_cs; + wheel.wheel_direction_ws = chassis_transform * wheel.direction_cs; + wheel.wheel_axle_ws = chassis_transform * wheel.axle_cs; + } + + fn ray_cast( + &mut self, + bodies: &RigidBodySet, + colliders: &ColliderSet, + queries: &QueryPipeline, + filter: QueryFilter, + chassis: &RigidBody, + wheel_id: usize, + ) { + let wheel = &mut self.wheels[wheel_id]; + let raylen = wheel.suspension_rest_length + wheel.radius; + let rayvector = wheel.wheel_direction_ws * raylen; + let source = wheel.raycast_info.hard_point_ws; + wheel.raycast_info.contact_point_ws = source + rayvector; + let ray = Ray::new(source, rayvector); + let hit = queries.cast_ray_and_get_normal(bodies, colliders, &ray, 1.0, true, filter); + + wheel.raycast_info.ground_object = None; + + if let Some((collider_hit, mut hit)) = hit { + if hit.toi == 0.0 { + let collider = &colliders[collider_hit]; + let up_ray = Ray::new(source + rayvector, -rayvector); + if let Some(hit2) = + collider + .shape + .cast_ray_and_get_normal(collider.position(), &up_ray, 1.0, false) + { + hit.normal = -hit2.normal; + } + + if hit.normal == Vector::zeros() { + // If the hit is still not defined, set the normal. + hit.normal = -wheel.wheel_direction_ws; + } + } + + wheel.raycast_info.contact_normal_ws = hit.normal; + wheel.raycast_info.is_in_contact = true; + wheel.raycast_info.ground_object = Some(collider_hit); + + let hit_distance = hit.toi * raylen; + wheel.raycast_info.suspension_length = hit_distance - wheel.radius; + + // clamp on max suspension travel + let min_suspension_length = wheel.suspension_rest_length - wheel.max_suspension_travel; + let max_suspension_length = wheel.suspension_rest_length + wheel.max_suspension_travel; + wheel.raycast_info.suspension_length = wheel + .raycast_info + .suspension_length + .clam |