Add a dynamic ray-cast vehicle controller

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),

examples3d/vehicle_controller3.rs

@@ -0,0 +1,174 @@
+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 mut 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, hh, hw);
+    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, -hh, hw],
+        point![hw, -hh, -hw],
+        point![-hw, -hh, hw],
+        point![-hw, -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..4 {
+        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 some stairs.
+     */
+    let stair_width = 1.0;
+    let stair_height = 0.1;
+    for i in 0..10 {
+        let x = i as f32 * stair_width / 2.0;
+        let y = i as f32 * stair_height * 1.5 + 3.0;
+
+        let collider = ColliderBuilder::cuboid(stair_width / 2.0, stair_height / 2.0, stair_width)
+            .translation(vector![x, y, 0.0]);
+        colliders.insert(collider);
+    }
+
+    /*
+     * Create a slope we can climb.
+     */
+    let slope_angle = 0.2;
+    let slope_size = 2.0;
+    let collider = ColliderBuilder::cuboid(slope_size, ground_height, slope_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, 1.0, 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![-8.0, 5.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());
+}

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;

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
+                .clamp(min_suspension_length, max_suspension_length);
+            wheel.raycast_info.contact_point_ws = ray.point_at(hit.toi);
+
+            let denominator = wheel
+                .raycast_info
+                .contact_normal_ws
+                .dot(&wheel.wheel_direction_ws);
+            let chassis_velocity_at_contact_point =
+                chassis.velocity_at_point(&wheel.raycast_info.contact_point_ws);
+            let proj_vel = wheel
+                .raycast_info
+                .contact_normal_ws
+                .dot(&chassis_velocity_at_contact_point);
+
+            if denominator >= -0.1 {
+                wheel.suspension_relative_velocity = 0.0;
+                wheel.clipped_inv_contact_dot_suspension = 1.0 / 0.1;
+            } else {
+                let inv = -1.0 / denominator;
+                wheel.suspension_relative_velocity = proj_vel * inv;
+                wheel.clipped_inv_contact_dot_suspension = inv;
+            }
+        } else {
+            // No contact, put wheel info as in rest position
+            wheel.raycast_info.suspension_length = wheel.suspension_rest_length;
+            wheel.suspension_relative_velocity = 0.0;
+            wheel.raycast_info.contact_normal_ws = -wheel.wheel_direction_ws;
+            wheel.clipped_inv_contact_dot_suspension = 1.0;
+        }
+    }
+
+    /// Updates the vehicle’s velocity based on its suspension, engine force, and brake.
+    pub fn update_vehicle(
+        &mut self,
+        dt: Real,
+        bodies: &mut RigidBodySet,
+        colliders: &ColliderSet,
+        queries: &QueryPipeline,
+        filter: QueryFilter,
+    ) {
+        let num_wheels = self.wheels.len();
+        let chassis = &bodies[self.chassis];
+
+        for i in 0..num_wheels {
+            self.update_wheel_transform(chassis, i);
+        }
+
+        self.current_vehicle_speed = chassis.linvel().norm();
+
+        let forward_w = chassis.position() * Vector::ith(self.index_forward_axis, 1.0);
+
+        if forward_w.dot(chassis.linvel()) < 0.0 {
+            self.current_vehicle_speed *= -1.0;
+        }
+
+        //
+        // simulate suspension
+        //
+
+        for wheel_id in 0..self.wheels.len() {
+            self.ray_cast(bodies, colliders, queries, filter, chassis, wheel_id);
+        }
+
+        let chassis_mass = chassis.mass();
+        self.update_suspension(chassis_mass);
+
+        let chassis = bodies
+            .get_mut_internal_with_modification_tracking(self.chassis)
+            .unwrap();
+
+        for wheel in &mut self.wheels {
+            if wheel.engine_force > 0.0 {
+                chassis.wake_up(true);
+            }
+
+            // apply suspension force
+            let mut suspension_force = wheel.wheel_suspension_force;
+
+            if suspension_force > wheel.max_suspension_force {
+                suspension_force = wheel.max_suspension_force;
+            }
+
+            let impulse = wheel.raycast_info.contact_normal_ws * suspension_force * dt;
+            chassis.apply_impulse_at_point(impulse, wheel.raycast_info.contact_point_ws, false);
+        }
+
+        self.update_friction(bodies, colliders, dt);
+
+        let chassis = bodies
+            .get_mut_internal_with_modification_tracking(self.chassis)
+            .unwrap();
+
+        for wheel in &mut self.wheels {
+            let vel = chassis.velocity_at_point(&wheel.raycast_info.hard_point_ws);
+
+            if wheel.raycast_info.is_in_contact {
+                let mut fwd = chassis.position() * Vector::ith(self.index_forward_axis, 1.0);
+                let proj = fwd.dot(&wheel.raycast_info.contact_normal_ws);
+                fwd -= wheel.raycast_info.contact_normal_ws * proj;
+
+                let proj2 = fwd.dot(&vel);
+
+                wheel.delta_rotation = (proj2 * dt) / (wheel.radius);
+                wheel.rotation += wheel.delta_rotation;
+            } else {
+                wheel.rotation += wheel.delta_rotation;
+            }
+
+            wheel.delta_rotation *= 0.99; //damping of rotation when not in contact
+        }
+    }
+
+    /// Reference to all the wheels attached to this vehicle.
+    pub fn wheels(&self) -> &[Wheel] {
+        &self.wheels
+    }
+
+    /// Mutable reference to all the wheels attached to this vehicle.
+    pub fn wheels_mut(&mut self) -> &mut [Wheel] {
+        &mut self.wheels
+    }
+
+    fn update_suspension(&mut self, chassis_mass: Real) {
+        for w_it in 0..self.wheels.len() {
+            let wheels = &mut self.wheels[w_it];
+
+            if wheels.raycast_info.is_in_contact {
+                let mut force;
+                //	Spring
+                {
+                    let rest_length = wheels.suspension_rest_length;
+                    let current_length = wheels.raycast_info.suspension_length;
+                    let length_diff = rest_length - current_length;
+
+                    force = wheels.suspension_stiffness
+                        * length_diff
+                        * wheels.clipped_inv_contact_dot_suspension;
+                }
+
+                // Damper
+                {
+                    let projected_rel_vel = wheels.suspension_relative_velocity;
+                    {
+                        let susp_damping = if projected_rel_vel < 0.0 {
+                            wheels.damping_compression
+                        } else {
+                            wheels.damping_relaxation
+                        };
+                        force -= susp_damping * projected_rel_vel;
+                    }
+                }
+
+                // RESULT
+                wheels.wheel_suspension_force = (force * chassis_mass).max(0.0);
+            } else {
+                wheels.wheel_suspension_force = 0.0;
+            }
+        }
+    }
+
+    const SIDE_FRICTION_STIFFNESS2: Real = 1.0;
+
+    fn update_friction(&mut self, bodies: &mut RigidBodySet, colliders: &ColliderSet, dt: Real) {
+        let num_wheels = self.wheels.len();
+
+        if num_wheels == 0 {
+            return;
+        }
+
+        self.forward_ws.resize(num_wheels, Default::default());
+        self.axle.resize(num_wheels, Default::default());
+
+        let mut num_wheels_on_ground = 0;
+
+        //TODO: collapse all those loops into one!
+        for wheel in &mut self.wheels {
+            let ground_object = wheel.raycast_info.ground_object;
+
+            if ground_object.is_some() {
+                num_wheels_on_ground += 1;
+            }
+
+            wheel.side_impulse = 0.0;
+            wheel.forward_impulse = 0.0;
+        }
+
+        {
+            for i in 0..num_wheels {
+                let wheel = &mut self.wheels[i];
+                let ground_object = wheel.raycast_info.ground_object;
+
+                if ground_object.is_some() {
+                    self.axle[i] = wheel.wheel_axle_ws;
+
+                    let surf_normal_ws = wheel.raycast_info.contact_normal_ws;
+                    let proj = self.axle[i].dot(&surf_normal_ws);
+                    self.axle[i] -= surf_normal_ws * proj;
+                    self.axle[i] = self.axle[i]
+                        .try_normalize(1.0e-5)
+                        .unwrap_or_else(Vector::zeros);
+                    self.forward_ws[i] = surf_normal_ws
+                        .cross(&self.axle[i])
+                        .try_normalize(1.0e-5)
+                        .unwrap_or_else(Vector::zeros);
+
+                    if let Some(ground_body) = ground_object
+                        .and_then(|h| colliders[h].parent())
+                        .map(|h| &bodies[h])
+                        .filter(|b| b.is_dynamic())
+                    {
+                        wheel.side_impulse = resolve_single_bilateral(
+                            &bodies[self.chassis],
+                            &wheel.raycast_info.contact_point_ws,
+                            &ground_body,
+                            &wheel.raycast_info.contact_point_ws,
+                            &self.axle[i],
+                        );
+                    } else {
+                        wheel.side_impulse = resolve_single_unilateral(
+                            &bodies[self.chassis],
+                            &wheel.raycast_info.contact_point_ws,
+                            &self.axle[i],
+                        );
+                    }
+
+                    wheel.side_impulse *= Self::SIDE_FRICTION_STIFFNESS2;
+                }
+            }
+        }
+
+        let side_factor = 1.0;
+        let fwd_factor = 0.5;
+
+        let mut sliding = false;
+        {
+            for wheel_id in 0..num_wheels {
+                let wheel = &mut self.wheels[wheel_id];
+                let ground_object = wheel.raycast_info.ground_object;
+
+                let mut rolling_friction = 0.0;
+
+                if ground_object.is_some() {
+                    if wheel.engine_force != 0.0 {
+                        rolling_friction = wheel.engine_force * dt;
+                    } else {
+                        let default_rolling_friction_impulse = 0.0;
+                        let max_impulse = if wheel.brake != 0.0 {
+                            wheel.brake
+                        } else {
+                            default_rolling_friction_impulse
+                        };
+                        let contact_pt = WheelContactPoint::new(
+                            &bodies[self.chassis],
+                            ground_object
+                                .and_then(|h| colliders[h].parent())
+                                .map(|h| &bodies[h]),
+                            wheel.raycast_info.contact_point_ws,
+                            self.forward_ws[wheel_id],
+                            max_impulse,
+                        );
+                        assert!(num_wheels_on_ground > 0);
+                        rolling_friction = contact_pt.calc_rolling_friction(num_wheels_on_ground);
+                    }
+                }
+
+                //switch between active rolling (throttle), braking and non-active rolling friction (no throttle/break)
+
+                wheel.forward_impulse = 0.0;
+                wheel.skid_info = 1.0;
+
+                if ground_object.is_some() {
+                    let max_imp = wheel.wheel_suspension_force * dt * wheel.friction_slip;
+                    let max_imp_side = max_imp;
+                    let max_imp_squared = max_imp * max_imp_side;
+                    assert!(max_imp_squared >= 0.0);
+
+                    wheel.forward_impulse = rolling_friction;
+
+                    let x = wheel.forward_impulse * fwd_factor;
+                    let y = wheel.side_impulse * side_factor;
+
+                    let impulse_squared = x * x + y * y;
+
+                    if impulse_squared > max_imp_squared {
+                        sliding = true;
+
+                        let factor = max_imp * crate::utils::inv(impulse_squared.sqrt());
+                        wheel.skid_info *= factor;
+                    }
+                }
+            }
+        }
+
+        if sliding {
+            for wheel in &mut self.wheels {
+                if wheel.side_impulse != 0.0 {
+                    if wheel.skid_info < 1.0 {
+                        wheel.forward_impulse *= wheel.skid_info;
+                        wheel.side_impulse *= wheel.skid_info;
+                    }
+                }
+            }
+        }
+
+        // apply the impulses
+        {
+            let chassis = bodies
+                .get_mut_internal_with_modification_tracking(self.chassis)
+                .unwrap();
+
+            for wheel_id in 0..num_wheels {
+                let wheel = &self.wheels[wheel_id];
+                let mut impulse_point = wheel.raycast_info.contact_point_ws;
+
+                if wheel.forward_impulse != 0.0 {
+                    chassis.apply_impulse_at_point(
+                        self.forward_ws[wheel_id] * wheel.forward_impulse,
+                        impulse_point,
+                        false,
+                    );
+                }
+                if wheel.side_impulse != 0.0 {
+                    let side_impulse = self.axle[wheel_id] * wheel.side_impulse;
+
+                    let v_chassis_world_up =
+                        chassis.position().rotation * Vector::ith(self.index_up_axis, 1.0);
+                    impulse_point -= v_chassis_world_up
+                        * (v_chassis_world_up.dot(&(impulse_point - chassis.center_of_mass()))
+                            * (1.0 - wheel.roll_influence));
+
+                    chassis.apply_impulse_at_point(side_impulse, impulse_point, false);
+
+                    // TODO: apply friction impulse on the ground
+                    // let ground_object = self.wheels[wheel_id].raycast_info.ground_object;
+                    // ground_object.apply_impulse_at_point(
+                    //     -side_impulse,
+                    //     wheels.raycast_info.contact_point_ws,
+                    //     false,
+                    // );
+                }
+            }
+        }
+    }
+}
+
+struct WheelContactPoint<'a> {
+    body0: &'a RigidBody,
+    body1: Option<&'a RigidBody>,
+    friction_position_world: Point<Real>,
+    friction_direction_world: Vector<Real>,
+    jac_diag_ab_inv: Real,
+    max_impulse: Real,
+}
+
+impl<'a> WheelContactPoint<'a> {
+    pub fn new(
+        body0: &'a RigidBody,
+        body1: Option<&'a RigidBody>,
+        friction_position_world: Point<Real>,
+        friction_direction_world: Vector<Real>,
+        max_impulse: Real,
+    ) -> Self {
+        fn impulse_denominator(body: &RigidBody, pos: &Point<Real>, n: &Vector<Real>) -> Real {
+            let dpt = pos - body.center_of_mass();
+            let gcross = dpt.gcross(*n);
+            let v = (body.mprops.effective_world_inv_inertia_sqrt
+                * (body.mprops.effective_world_inv_inertia_sqrt * gcross))
+                .gcross(dpt);
+            // TODO: take the effective inv mass into account instead of the inv_mass?
+            body.mprops.local_mprops.inv_mass + n.dot(&v)
+        }
+        let denom0 =
+            impulse_denominator(body0, &friction_position_world, &friction_direction_world);
+        let denom1 = body1
+            .map(|body1| {
+                impulse_denominator(body1, &friction_position_world, &friction_direction_world)
+            })
+            .unwrap_or(0.0);
+        let relaxation = 1.0;
+        let jac_diag_ab_inv = relaxation / (denom0 + denom1);
+
+        Self {
+            body0,
+            body1,
+            friction_position_world,
+            friction_direction_world,
+            jac_diag_ab_inv,
+            max_impulse,
+        }
+    }
+
+    pub fn calc_rolling_friction(&self, num_wheels_on_ground: usize) -> Real {
+        let contact_pos_world = self.friction_position_world;
+        let max_impulse = self.max_impulse;
+
+        let vel1 = self.body0.velocity_at_point(&contact_pos_world);
+        let vel2 = self
+            .body1
+            .map(|b| b.velocity_at_point(&contact_pos_world))
+            .unwrap_or_else(Vector::zeros);
+        let vel = vel1 - vel2;
+        let vrel = self.friction_direction_world.dot(&vel);
+
+        // calculate friction that moves us to zero relative velocity
+        (-vrel * self.jac_diag_ab_inv / (num_wheels_on_ground as Real))
+            .clamp(-max_impulse, max_impulse)
+    }
+}
+
+fn resolve_single_bilateral(
+    body1: &RigidBody,
+    pt1: &Point<Real>,
+    body2: &RigidBody,
+    pt2: &Point<Real>,
+    normal: &Vector<Real>,
+) -> Real {
+    let vel1 = body1.velocity_at_point(pt1);
+    let vel2 = body2.velocity_at_point(pt2);
+    let dvel = vel1 - vel2;
+
+    let dpt1 = pt1 - body1.center_of_mass();
+    let dpt2 = pt2 - body2.center_of_mass();
+    let aj = dpt1.gcross(*normal);
+    let bj = dpt2.gcross(-*normal);
+    let iaj = body1.mprops.effective_world_inv_inertia_sqrt * aj;
+    let ibj = body2.mprops.effective_world_inv_inertia_sqrt * bj;
+
+    // TODO: take the effective_inv_mass into account?
+    let im1 = body1.mprops.local_mprops.inv_mass;
+    let im2 = body2.mprops.local_mprops.inv_mass;
+
+    let jac_diag_ab = im1 + im2 + iaj.gdot(iaj) + ibj.gdot(ibj);
+    let jac_diag_ab_inv = crate::utils::inv(jac_diag_ab);
+    let rel_vel = normal.dot(&dvel);
+
+    //todo: move this into proper structure
+    let contact_damping = 0.2;
+    -contact_damping * rel_vel * jac_diag_ab_inv
+}
+
+fn resolve_single_unilateral(body1: &RigidBody, pt1: &Point<Real>, normal: &Vector<Real>) -> Real {
+    let vel1 = body1.velocity_at_point(pt1);
+    let dvel = vel1;
+    let dpt1 = pt1 - body1.center_of_mass();
+    let aj = dpt1.gcross(*normal);
+    let iaj = body1.mprops.effective_world_inv_inertia_sqrt * aj;
+
+    // TODO: take the effective_inv_mass into account?
+    let im1 = body1.mprops.local_mprops.inv_mass;
+    let jac_diag_ab = im1 + iaj.gdot(iaj);
+    let jac_diag_ab_inv = crate::utils::inv(jac_diag_ab);
+    let rel_vel = normal.dot(&dvel);
+
+    //todo: move this into proper structure
+    let contact_damping = 0.2;
+    -contact_damping * rel_vel * jac_diag_ab_inv
+}

src/dynamics/rigid_body.rs

@@ -146,6 +146,12 @@ impl RigidBody {
         }
     }
 
+    /// The world-space center-of-mass of this rigid-body.
+    #[inline]
+    pub fn center_of_mass(&self) -> &Point<Real> {
+        &self.mprops.world_com
+    }
+
     /// The mass-properties of this rigid-body.
     #[inline]
     pub fn mass_properties(&self) -> &MassProperties {

src_testbed/testbed.rs

@@ -9,6 +9,8 @@ use crate::{debug_render, ui};
 use crate::{graphics::GraphicsManager, harness::RunState};
 
 use na::{self, Point2, Point3, Vector3};
+#[cfg(feature = "dim3")]
+use rapier::control::DynamicRayCastVehicleController;
 use rapier::control::KinematicCharacterController;
 use rapier::dynamics::{
     ImpulseJointSet, IntegrationParameters, MultibodyJointSet, RigidBodyActivation,
@@ -100,6 +102,8 @@ pub struct TestbedState {
     pub draw_colls: bool,
     pub highlighted_body: Option<RigidBodyHandle>,
     pub character_body: Option<RigidBodyHandle>,
+    #[cfg(feature = "dim3")]
+    pub vehicle_controller: Option<DynamicRayCastVehicleController>,
     //    pub grabbed_object: Option<DefaultBodyPartHandle>,
     //    pub grabbed_object_constraint: Option<DefaultJointConstraintHandle>,
     pub grabbed_object_plane: (Point3<f32>, Vector3<f32>),
@@ -178,6 +182,8 @@ impl TestbedApp {
             draw_colls: false,
             highlighted_body: None,
             character_body: None,
+            #[cfg(feature = "dim3")]
+            vehicle_controller: None,
             //            grabbed_object: None,
             //            grabbed_object_constraint: None,
             grabbed_object_plane: (Point3::origin(), na::zero()),
@@ -456,6 +462,11 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
         self.state.character_body = Some(handle);
     }
 
+    #[cfg(feature = "dim3")]
+    pub fn set_vehicle_controller(&mut self, controller: DynamicRayCastVehicleController) {
+        self.state.vehicle_controller = Some(controller);
+    }
+
     pub fn allow_grabbing_behind_ground(&mut self, allow: bool) {
         self.state.can_grab_behind_ground = allow;
     }
@@ -511,8 +522,12 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
             .action_flags
             .set(TestbedActionFlags::RESET_WORLD_GRAPHICS, true);
 
-        self.state.character_body = None;
         self.state.highlighted_body = None;
+        self.state.character_body = None;
+        #[cfg(feature = "dim3")]
+        {
+            self.state.vehicle_controller = None;
+        }
 
         #[cfg(all(feature = "dim2", feature = "other-backends"))]
         {
@@ -624,6 +639,50 @@ impl<'a, 'b, 'c, 'd, 'e, 'f> Testbed<'a, 'b, 'c, 'd, 'e, 'f> {
         self.plugins.0.push(Box::new(plugin));
     }
 
+    #[cfg(feature = "dim3")]
+    fn update_vehicle_controller(&mut self, events: &Input<KeyCode>) {
+        if self.state.running == RunMode::Stop {
+            return;
+        }
+
+        if let Some(vehicle) = &mut self.state.vehicle_controller {
+            let mut engine_force = 0.0;
+            let mut steering_angle = 0.0;
+
+            for key in events.get_pressed() {
+                match *key {
+                    KeyCode::Right => {
+                        steering_angle += -0.1;
+                    }
+                    KeyCode::Left => {
+                        steering_angle += 0.1;
+                    }
+                    KeyCode::Up => {
+                        engine_force += 0.1;
+                    }
+                    KeyCode::Down => {
+                        engine_force += -0.1;
+                    }
+                    _ => {}
+                }
+            }
+
+            let wheels = vehicle.wheels_mut();
+            wheels[0].engine_force = engine_force;
+            wheels[0].steering = steering_angle;
+            wheels[1].engine_force = engine_force;
+            wheels[1].steering = steering_angle;
+
+            vehicle.update_vehicle(
+                self.harness.physics.integration_parameters.dt,
+                &mut self.harness.physics.bodies,
+                &self.harness.physics.colliders,
+                &self.harness.physics.query_pipeline,
+                QueryFilter::exclude_dynamic().exclude_rigid_body(vehicle.chassis),
+            );
+        }
+    }
+
     fn update_character_controller(&mut self, events: &Input<KeyCode>) {
         if self.state.running == RunMode::Stop {
             return;
@@ -1063,6 +1122,10 @@ fn update_testbed(
 
         testbed.handle_common_events(&*keys);
         testbed.update_character_controller(&*keys);
+        #[cfg(feature = "dim3")]
+        {
+            testbed.update_vehicle_controller(&*keys);
+        }
     }
 
     // Update UI