diff options
Diffstat (limited to 'src/geometry')
| -rw-r--r-- | src/geometry/narrow_phase.rs | 6 |
1 files changed, 4 insertions, 2 deletions
diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs index 3fda8a6..5171f1b 100644 --- a/src/geometry/narrow_phase.rs +++ b/src/geometry/narrow_phase.rs @@ -13,6 +13,7 @@ use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGrap use crate::math::{Real, Vector}; use crate::pipeline::EventHandler; use cdl::query::{DefaultQueryDispatcher, PersistentQueryDispatcher}; +use cdl::utils::IsometryOpt; use std::collections::HashMap; use std::sync::Arc; @@ -522,10 +523,11 @@ impl NarrowPhase { let mut has_any_active_contact = false; for manifold in &mut pair.manifolds { + let world_pos1 = manifold.subshape_pos1.prepend_to(co1.position()); manifold.data.solver_contacts.clear(); manifold.data.body_pair = BodyPair::new(co1.parent(), co2.parent()); manifold.data.solver_flags = solver_flags; - manifold.data.normal = co1.position() * manifold.local_n1; + manifold.data.normal = world_pos1 * manifold.local_n1; // Sort contacts to keep only these with distances bellow // the prediction, and generate solver contacts. @@ -536,7 +538,7 @@ impl NarrowPhase { if contact.dist < prediction_distance { // Generate the solver contact. let solver_contact = SolverContact { - point: co1.position() * contact.local_p1 + point: world_pos1 * contact.local_p1 + manifold.data.normal * contact.dist / 2.0, dist: contact.dist, friction: (co1.friction + co2.friction) / 2.0, |