diff options
Diffstat (limited to 'src/control')
| -rw-r--r-- | src/control/character_controller.rs | 112 | ||||
| -rw-r--r-- | src/control/ray_cast_vehicle_controller.rs | 114 |
2 files changed, 119 insertions, 107 deletions
diff --git a/src/control/character_controller.rs b/src/control/character_controller.rs index ca088b2..23964b5 100644 --- a/src/control/character_controller.rs +++ b/src/control/character_controller.rs @@ -1,6 +1,6 @@ use crate::dynamics::RigidBodySet; use crate::geometry::{ColliderHandle, ColliderSet, ContactManifold, Shape, TOI}; -use crate::math::{Isometry, Point, Real, UnitVector, Vector}; +use crate::math::*; use crate::pipeline::{QueryFilter, QueryFilterFlags, QueryPipeline}; use crate::utils; use na::{RealField, Vector2}; @@ -78,16 +78,16 @@ impl Default for CharacterAutostep { } /// A collision between the character and its environment during its movement. -#[derive(Copy, Clone, Debug)] +#[derive(Copy, Clone, Debug, PartialEq)] pub struct CharacterCollision { /// The collider hit by the character. pub handle: ColliderHandle, /// The position of the character when the collider was hit. - pub character_pos: Isometry<Real>, + pub character_pos: Isometry, /// The translation that was already applied to the character when the hit happens. - pub translation_applied: Vector<Real>, + pub translation_applied: Vector, /// The translations that was still waiting to be applied to the character when the hit happens. - pub translation_remaining: Vector<Real>, + pub translation_remaining: Vector, /// Geometric information about the hit. pub toi: TOI, } @@ -97,7 +97,7 @@ pub struct CharacterCollision { #[derive(Copy, Clone, Debug)] pub struct KinematicCharacterController { /// The direction that goes "up". Used to determine where the floor is, and the floor’s angle. - pub up: UnitVector<Real>, + pub up: UnitVector, /// A small gap to preserve between the character and its surroundings. /// /// This value should not be too large to avoid visual artifacts, but shouldn’t be too small @@ -135,7 +135,7 @@ impl Default for KinematicCharacterController { /// The effective movement computed by the character controller. pub struct EffectiveCharacterMovement { /// The movement to apply. - pub translation: Vector<Real>, + pub translation: Vector, /// Is the character touching the ground after applying `EffectiveKineamticMovement::translation`? pub grounded: bool, /// Is the character sliding down a slope due to slope angle being larger than `min_slope_slide_angle`? @@ -181,8 +181,8 @@ impl KinematicCharacterController { colliders: &ColliderSet, queries: &QueryPipeline, character_shape: &dyn Shape, - character_pos: &Isometry<Real>, - desired_translation: Vector<Real>, + character_pos: &Isometry, + desired_translation: Vector, filter: QueryFilter, mut events: impl FnMut(CharacterCollision), ) -> EffectiveCharacterMovement { @@ -237,8 +237,8 @@ impl KinematicCharacterController { ) { // We hit something, compute the allowed self. let allowed_dist = - (toi.toi - (-toi.normal1.dot(&translation_dir)) * offset).max(0.0); - let allowed_translation = *translation_dir * allowed_dist; + (toi.toi - (-toi.normal1.dot(translation_dir)) * offset).max(0.0); + let allowed_translation = translation_dir.into_inner() * allowed_dist; result.translation += allowed_translation; translation_remaining -= allowed_translation; @@ -315,13 +315,13 @@ impl KinematicCharacterController { colliders: &ColliderSet, queries: &QueryPipeline, character_shape: &dyn Shape, - character_pos: &Isometry<Real>, + character_pos: &Isometry, dims: &Vector2<Real>, filter: QueryFilter, result: &mut EffectiveCharacterMovement, ) -> Option<(ColliderHandle, TOI)> { if let Some(snap_distance) = self.snap_to_ground { - if result.translation.dot(&self.up) < -1.0e-5 { + if result.translation.dot(self.up) < -1.0e-5 { let snap_distance = snap_distance.eval(dims.y); let offset = self.offset.eval(dims.y); if let Some((hit_handle, hit)) = queries.cast_shape( @@ -335,7 +335,7 @@ impl KinematicCharacterController { filter, ) { // Apply the snap. - result.translation -= *self.up * (hit.toi - offset).max(0.0); + result.translation -= self.up.into_inner() * (hit.toi - offset).max(0.0); result.grounded = true; return Some((hit_handle, hit)); } @@ -356,11 +356,11 @@ impl KinematicCharacterController { colliders: &ColliderSet, queries: &QueryPipeline, character_shape: &dyn Shape, - character_pos: &Isometry<Real>, + character_pos: &Isometry, dims: &Vector2<Real>, filter: QueryFilter, - mut kinematic_friction_translation: Option<&mut Vector<Real>>, - mut translation_remaining: Option<&mut Vector<Real>>, + mut kinematic_friction_translation: Option<&mut Vector>, + mut translation_remaining: Option<&mut Vector>, ) -> bool { let prediction = self.predict_ground(dims.y); @@ -406,19 +406,20 @@ impl KinematicCharacterController { if let Some(kinematic_parent) = kinematic_parent { let mut num_active_contacts = 0; let mut manifold_center = Point::origin(); - let normal = -(character_pos * m.local_n1); + let normal = -(character_pos.rotation * m.local_n1); for contact in &m.points { if contact.dist <= prediction { num_active_contacts += 1; - let contact_point = collider.position() * contact.local_p2; + let contact_point = + collider.position().transform_point(&contact.local_p2); let target_vel = kinematic_parent.velocity_at_point(&contact_point); - let normal_target_mvt = target_vel.dot(&normal) * dt; - let normal_current_mvt = translation_remaining.dot(&normal); + let normal_target_mvt = target_vel.dot(normal) * dt; + let normal_current_mvt = translation_remaining.dot(normal); - manifold_center += contact_point.coords; + manifold_center += contact_point.as_vector(); *translation_remaining += normal * (normal_target_mvt - normal_current_mvt); } @@ -429,7 +430,7 @@ impl KinematicCharacterController { &(manifold_center / num_active_contacts as Real), ); let tangent_platform_mvt = - (target_vel - normal * target_vel.dot(&normal)) * dt; + (target_vel - normal * target_vel.dot(normal)) * dt; kinematic_friction_translation.zip_apply( &tangent_platform_mvt, |y, x| { @@ -463,12 +464,12 @@ impl KinematicCharacterController { fn is_grounded_at_contact_manifold( &self, manifold: &ContactManifold, - character_pos: &Isometry<Real>, + character_pos: &Isometry, dims: &Vector2<Real>, ) -> bool { - let normal = -(character_pos * manifold.local_n1); + let normal = -(character_pos.rotation * manifold.local_n1); - if normal.dot(&self.up) >= 1.0e-5 { + if normal.dot(self.up) >= 1.0e-5 { let prediction = self.predict_ground(dims.y); for contact in &manifold.points { if contact.dist <= prediction { @@ -482,9 +483,9 @@ impl KinematicCharacterController { fn handle_slopes( &self, hit: &TOI, - translation_remaining: &Vector<Real>, + translation_remaining: &Vector, result: &mut EffectiveCharacterMovement, - ) -> Vector<Real> { + ) -> Vector { let [vertical_translation, horizontal_translation] = self.split_into_components(translation_remaining); let slope_translation = subtract_hit(*translation_remaining, hit); @@ -498,7 +499,7 @@ impl KinematicCharacterController { // NOTE: part of the slope will already be handled by auto-stepping if it was enabled. // Therefore, `climbing` may not always be `true` when climbing on a slope at // slow speed. - let climbing = self.up.dot(&slope_translation) >= 0.0 && self.up.dot(&hit.normal1) > 0.0; + let climbing = self.up.dot(slope_translation) >= 0.0 && self.up.dot(hit.normal1) > 0.0; if climbing && angle_with_floor >= self.max_slope_climb_angle { // Prevent horizontal movement from pushing through the slope. @@ -513,16 +514,16 @@ impl KinematicCharacterController { } } - fn split_into_components(&self, translation: &Vector<Real>) -> [Vector<Real>; 2] { - let vertical_translation = *self.up * (self.up.dot(translation)); + fn split_into_components(&self, translation: &Vector) -> [Vector; 2] { + let vertical_translation = self.up.into_inner() * (self.up.dot(*translation)); let horizontal_translation = *translation - vertical_translation; [vertical_translation, horizontal_translation] } fn compute_dims(&self, character_shape: &dyn Shape) -> Vector2<Real> { let extents = character_shape.compute_local_aabb().extents(); - let up_extent = extents.dot(&self.up.abs()); - let side_extent = (extents - (*self.up).abs() * up_extent).norm(); + let up_extent = extents.dot(self.up.abs()); + let side_extent = (extents - self.up.into_inner().abs() * up_extent).norm(); Vector2::new(side_extent, up_extent) } @@ -532,11 +533,11 @@ impl KinematicCharacterController { colliders: &ColliderSet, queries: &QueryPipeline, character_shape: &dyn Shape, - character_pos: &Isometry<Real>, + character_pos: &Isometry, dims: &Vector2<Real>, mut filter: QueryFilter, stair_handle: ColliderHandle, - translation_remaining: &mut Vector<Real>, + translation_remaining: &mut Vector, result: &mut EffectiveCharacterMovement, ) -> bool { let autostep = match self.autostep { @@ -563,11 +564,12 @@ impl KinematicCharacterController { filter.flags |= QueryFilterFlags::EXCLUDE_DYNAMIC; } - let shifted_character_pos = Translation::from(*self.up * max_height) * character_pos; + let shifted_character_pos = + Translation::from(self.up.into_inner() * max_height) * character_pos; let horizontal_dir = match (*translation_remaining - - *self.up * translation_remaining.dot(&self.up)) - .try_normalize(1.0e-5) + - self.up.into_inner() * translation_remaining.dot(self.up)) + .try_normalize_eps(1.0e-5) { Some(dir) => dir, None => return false, @@ -626,7 +628,7 @@ impl KinematicCharacterController { let slope_translation = horizontal_slope_translation + vertical_slope_translation; let angle_with_floor = self.up.angle(&hit.normal1); - let climbing = self.up.dot(&slope_translation) >= 0.0; + let climbing = self.up.dot(slope_translation) >= 0.0; if climbing && angle_with_floor > self.max_slope_climb_angle { return false; // The target ramp is too steep. @@ -650,13 +652,13 @@ impl KinematicCharacterController { .unwrap_or(max_height); // Remove the step height from the vertical part of the self. - let step = *self.up * step_height; + let step = self.up.into_inner() * step_height; *translation_remaining -= step; // Advance the collider on the step horizontally, to make sure further // movement won’t just get stuck on its edge. let horizontal_nudge = - horizontal_dir * horizontal_dir.dot(translation_remaining).min(min_width); + horizontal_dir * horizontal_dir.dot(*translation_remaining).min(min_width); *translation_remaining -= horizontal_nudge; result.translation += step + horizontal_nudge; @@ -679,9 +681,9 @@ impl KinematicCharacterController { filter: QueryFilter, ) { let extents = character_shape.compute_local_aabb().extents(); - let up_extent = extents.dot(&self.up.abs()); - let movement_to_transfer = - *collision.toi.normal1 * collision.translation_remaining.dot(&collision.toi.normal1); + let up_extent = extents.dot(self.up.abs()); + let movement_to_transfer = collision.toi.normal1.into_inner() + * collision.translation_remaining.dot(collision.toi.normal1); let prediction = self.predict_ground(up_extent); // TODO: allow custom dispatchers. @@ -712,7 +714,7 @@ impl KinematicCharacterController { for m in &mut manifolds[prev_manifolds_len..] { m.data.rigid_body2 = Some(parent.handle); - m.data.normal = collision.character_pos * m.local_n1; + m.data.normal = collision.character_pos.rotation * m.local_n1; } } } @@ -726,15 +728,23 @@ impl KinematicCharacterController { for manifold in &manifolds { let body_handle = manifold.data.rigid_body2.unwrap(); - let body = &mut bodies[body_handle]; + + // NOTE: we are calling get_mut_internal_with_modification_tracking because this is part + // of the internal rapier codebase (and we use this function to make sure we are + // triggering modification tracking on purpose), but a copy-paste on this character + // controller outside of the rapier codebase could just use + // `&mut bodies[body_handle]` instead. + let Some(body) = bodies.get_mut_internal_with_modification_tracking(body_handle) else { + continue; + }; for pt in &manifold.points { if pt.dist <= prediction { let body_mass = body.mass(); - let contact_point = body.position() * pt.local_p2; + let contact_point = body.position().transform_point(&pt.local_p2); let delta_vel_per_contact = (velocity_to_transfer - body.velocity_at_point(&contact_point)) - .dot(&manifold.data.normal); + .dot(manifold.data.normal); let mass_ratio = body_mass * character_mass / (body_mass + character_mass); body.apply_impulse_at_point( @@ -748,9 +758,9 @@ impl KinematicCharacterController { } } -fn subtract_hit(translation: Vector<Real>, hit: &TOI) -> Vector<Real> { - let surface_correction = (-translation).dot(&hit.normal1).max(0.0); +fn subtract_hit(translation: Vector, hit: &TOI) -> Vector { + let surface_correction = (-translation).dot(hit.normal1).max(0.0); // This fixes some instances of moving through walls let surface_correction = surface_correction * (1.0 + 1.0e-5); - translation + *hit.normal1 * surface_correction + translation + hit.normal1.into_inner() * surface_correction } diff --git a/src/control/ray_cast_vehicle_controller.rs b/src/control/ray_cast_vehicle_controller.rs index 00b11eb..c56d987 100644 --- a/src/control/ray_cast_vehicle_controller.rs +++ b/src/control/ray_cast_vehicle_controller.rs @@ -2,15 +2,15 @@ use crate::dynamics::{RigidBody, RigidBodyHandle, RigidBodySet}; use crate::geometry::{ColliderHandle, ColliderSet, Ray}; -use crate::math::{Point, Real, Rotation, Vector}; +use crate::math::*; use crate::pipeline::{QueryFilter, QueryPipeline}; use crate::utils::{SimdCross, SimdDot}; /// 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>>, + forward_ws: Vec<Vector>, + axle: Vec<Vector>, /// The current forward speed of the vehicle. pub current_vehicle_speed: Real, @@ -65,13 +65,13 @@ impl Default for WheelTuning { /// Objects used to initialize a wheel. struct WheelDesc { /// The position of the wheel, relative to the chassis. - pub chassis_connection_cs: Point<Real>, + pub chassis_connection_cs: Point, /// 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>, + pub direction_cs: Vector, /// The wheel’s axle axis, relative to the chassis. - pub axle_cs: Vector<Real>, + pub axle_cs: Vector, /// 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. @@ -105,18 +105,18 @@ struct WheelDesc { pub struct Wheel { raycast_info: RayCastInfo, - center: Point<Real>, - wheel_direction_ws: Vector<Real>, - wheel_axle_ws: Vector<Real>, + center: Point, + wheel_direction_ws: Vector, + wheel_axle_ws: Vector, /// The position of the wheel, relative to the chassis. - pub chassis_connection_point_cs: Point<Real>, + pub chassis_connection_point_cs: Point, /// 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>, + pub direction_cs: Vector, /// The wheel’s axle axis, relative to the chassis. - pub axle_cs: Vector<Real>, + pub axle_cs: Vector, /// 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. @@ -207,17 +207,17 @@ impl Wheel { } /// The world-space center of the wheel. - pub fn center(&self) -> Point<Real> { + pub fn center(&self) -> Point { self.center } /// The world-space direction of the wheel’s suspension. - pub fn suspension(&self) -> Vector<Real> { + pub fn suspension(&self) -> Vector { self.wheel_direction_ws } /// The world-space direction of the wheel’s axle. - pub fn axle(&self) -> Vector<Real> { + pub fn axle(&self) -> Vector { self.wheel_axle_ws } } @@ -227,13 +227,13 @@ impl Wheel { #[derive(Copy, Clone, Debug, PartialEq, Default)] pub struct RayCastInfo { /// The (world-space) contact normal between the wheel and the floor. - pub contact_normal_ws: Vector<Real>, + pub contact_normal_ws: Vector, /// The (world-space) point hit by the wheel’s ray-cast. - pub contact_point_ws: Point<Real>, + pub contact_point_ws: Point, /// The suspension length for the wheel. pub suspension_length: Real, /// The (world-space) starting point of the ray-cast. - pub hard_point_ws: Point<Real>, + pub hard_point_ws: Point, /// Is the wheel in contact with the ground? pub is_in_contact: bool, /// The collider hit by the ray-cast. @@ -262,9 +262,9 @@ impl DynamicRayCastVehicleController { /// 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>, + chassis_connection_cs: Point, + direction_cs: Vector, + axle_cs: Vector, suspension_rest_length: Real, radius: Real, tuning: &WheelTuning, @@ -296,7 +296,7 @@ impl DynamicRayCastVehicleController { 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; + .as_vector(); } #[cfg(feature = "dim3")] @@ -304,8 +304,8 @@ impl DynamicRayCastVehicleController { 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); + let steering_orn = Rotation::from_scaled_axis(-wheel.wheel_direction_ws * wheel.steering); + wheel.wheel_axle_ws = steering_orn * (chassis.position().rotation * wheel.axle_cs); wheel.center = wheel.raycast_info.hard_point_ws + wheel.wheel_direction_ws * wheel.raycast_info.suspension_length; } @@ -316,9 +316,10 @@ impl DynamicRayCastVehicleController { 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; + wheel.raycast_info.hard_point_ws = + chassis_transform.transform_point(&wheel.chassis_connection_point_cs); + wheel.wheel_direction_ws = chassis_transform.rotation * wheel.direction_cs; + wheel.wheel_axle_ws = chassis_transform.rotation * wheel.axle_cs; } fn ray_cast( @@ -377,13 +378,13 @@ impl DynamicRayCastVehicleController { let denominator = wheel .raycast_info .contact_normal_ws - .dot(&wheel.wheel_direction_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); + .dot(chassis_velocity_at_contact_point); if denominator >= -0.1 { wheel.suspension_relative_velocity = 0.0; @@ -420,9 +421,9 @@ impl DynamicRayCastVehicleController { self.current_vehicle_speed = chassis.linvel().norm(); - let forward_w = chassis.position() * Vector::ith(self.index_forward_axis, 1.0); + let forward_w = chassis.position().rotation * Vector::ith(self.index_forward_axis, 1.0); - if forward_w.dot(chassis.linvel()) < 0.0 { + if forward_w.dot(*chassis.linvel()) < 0.0 { self.current_vehicle_speed *= -1.0; } @@ -467,11 +468,12 @@ impl DynamicRayCastVehicleController { 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); + let mut fwd = + chassis.position().rotation * 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); + let proj2 = fwd.dot(vel); wheel.delta_rotation = (proj2 * dt) / (wheel.radius); wheel.rotation += wheel.delta_rotation; @@ -564,14 +566,14 @@ impl DynamicRayCastVehicleController { 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); + 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) + .try_normalize_eps(1.0e-5) .unwrap_or_else(Vector::zeros); self.forward_ws[i] = surf_normal_ws - .cross(&self.axle[i]) - .try_normalize(1.0e-5) + .cross(self.axle[i]) + .try_normalize_eps(1.0e-5) .unwrap_or_else(Vector::zeros); if let Some(ground_body) = ground_object @@ -694,7 +696,7 @@ impl DynamicRayCastVehicleController { 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())) + * (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); @@ -715,8 +717,8 @@ impl DynamicRayCastVehicleController { struct WheelContactPoint<'a> { body0: &'a RigidBody, body1: Option<&'a RigidBody>, - friction_position_world: Point<Real>, - friction_direction_world: Vector<Real>, + friction_position_world: Point, + friction_direction_world: Vector, jac_diag_ab_inv: Real, max_impulse: Real, } @@ -725,18 +727,18 @@ impl<'a> WheelContactPoint<'a> { pub fn new( body0: &'a RigidBody, body1: Option<&'a RigidBody>, - friction_position_world: Point<Real>, - friction_direction_world: Vector<Real>, + friction_position_world: Point, + friction_direction_world: Vector, max_impulse: Real, ) -> Self { - fn impulse_denominator(body: &RigidBody, pos: &Point<Real>, n: &Vector<Real>) -> Real { - let dpt = pos - body.center_of_mass(); + fn impulse_denominator(body: &RigidBody, pos: &Point, n: &Vector) -> 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) + body.mprops.local_mprops.inv_mass + n.dot(v) } let denom0 = impulse_denominator(body0, &friction_position_world, &friction_direction_world); @@ -768,7 +770,7 @@ impl<'a> WheelContactPoint<'a> { .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); + 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)) @@ -778,17 +780,17 @@ impl<'a> WheelContactPoint<'a> { fn resolve_single_bilateral( body1: &RigidBody, - pt1: &Point<Real>, + pt1: &Point, body2: &RigidBody, - pt2: &Point<Real>, - normal: &Vector<Real>, + pt2: &Point, + normal: &Vector, ) -> 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 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; @@ -800,17 +802,17 @@ fn resolve_single_bilateral( 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); + 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 { +fn resolve_single_unilateral(body1: &RigidBody, pt1: &Point, normal: &Vector) -> Real { let vel1 = body1.velocity_at_point(pt1); let dvel = vel1; - let dpt1 = pt1 - body1.center_of_mass(); + let dpt1 = *pt1 - *body1.center_of_mass(); let aj = dpt1.gcross(*normal); let iaj = body1.mprops.effective_world_inv_inertia_sqrt * aj; @@ -818,7 +820,7 @@ fn resolve_single_unilateral(body1: &RigidBody, pt1: &Point<Real>, normal: &Vect 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); + let rel_vel = normal.dot(dvel); //todo: move this into proper structure let contact_damping = 0.2; |