Add a dynamic ray-cast vehicle controller

3 files changed, 820 insertions, 0 deletions

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
+                .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
+}
diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs
index a6384b7..26be983 100644
--- a/src/dynamics/rigid_body.rs
+++ b/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 {