1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 180 181 182 183 184 185 186 187 188 189 190 191 192 193 194 195 196 197 198 199 200 201 202 203 204 205 206 207 208 209 210 211 212 213 214 215 216 217 218 219 220 221 222 223 224 225 226 227 228 229 230 231 232 233 234 235 236 237 238 239 240 241 242 243 244 245 246 247 248 249 250 251 252 253 254 255 256 257 258 259 260 261 262 263 264 265 266 267 268
use crate::dynamics::{BodyPair, RigidBodyHandle}; use crate::geometry::{ColliderPair, Contact, ContactManifold}; use crate::math::{Point, Real, Vector}; use parry::query::ContactManifoldsWorkspace; bitflags::bitflags! { #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// Flags affecting the behavior of the constraints solver for a given contact manifold. pub struct SolverFlags: u32 { /// The constraint solver will take this contact manifold into /// account for force computation. const COMPUTE_IMPULSES = 0b001; /// The user-defined physics hooks will be used to /// modify the solver contacts of this contact manifold. const MODIFY_SOLVER_CONTACTS = 0b010; } } impl Default for SolverFlags { fn default() -> Self { SolverFlags::COMPUTE_IMPULSES } } #[derive(Copy, Clone, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// A single contact between two collider. pub struct ContactData { /// The impulse, along the contact normal, applied by this contact to the first collider's rigid-body. /// /// The impulse applied to the second collider's rigid-body is given by `-impulse`. pub impulse: Real, /// The friction impulse along the vector orthonormal to the contact normal, applied to the first /// collider's rigid-body. #[cfg(feature = "dim2")] pub tangent_impulse: Real, /// The friction impulses along the basis orthonormal to the contact normal, applied to the first /// collider's rigid-body. #[cfg(feature = "dim3")] pub tangent_impulse: na::Vector2<Real>, /// The target velocity correction at the contact point. pub rhs: Real, } impl Default for ContactData { fn default() -> Self { Self { impulse: 0.0, tangent_impulse: na::zero(), rhs: 0.0, } } } #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] #[derive(Clone)] /// The description of all the contacts between a pair of colliders. pub struct ContactPair { /// The pair of colliders involved. pub pair: ColliderPair, /// The set of contact manifolds between the two colliders. /// /// All contact manifold contain themselves contact points between the colliders. pub manifolds: Vec<ContactManifold>, /// Is there any active contact in this contact pair? pub has_any_active_contact: bool, pub(crate) workspace: Option<ContactManifoldsWorkspace>, } impl ContactPair { pub(crate) fn new(pair: ColliderPair) -> Self { Self { pair, has_any_active_contact: false, manifolds: Vec::new(), workspace: None, } } /// Finds the contact with the smallest signed distance. /// /// If the colliders involved in this contact pair are penetrating, then /// this returns the contact with the largest penetration depth. /// /// Returns a reference to the contact, as well as the contact manifold /// it is part of. pub fn find_deepest_contact(&self) -> Option<(&ContactManifold, &Contact)> { let mut deepest = None; for m2 in &self.manifolds { let deepest_candidate = m2.find_deepest_contact(); deepest = match (deepest, deepest_candidate) { (_, None) => deepest, (None, Some(c2)) => Some((m2, c2)), (Some((m1, c1)), Some(c2)) => { if c1.dist <= c2.dist { Some((m1, c1)) } else { Some((m2, c2)) } } } } deepest } } #[derive(Clone, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] /// A contact manifold between two colliders. /// /// A contact manifold describes a set of contacts between two colliders. All the contact /// part of the same contact manifold share the same contact normal and contact kinematics. pub struct ContactManifoldData { // The following are set by the narrow-phase. /// The pair of body involved in this contact manifold. pub body_pair: BodyPair, pub(crate) warmstart_multiplier: Real, // The two following are set by the constraints solver. #[cfg_attr(feature = "serde-serialize", serde(skip))] pub(crate) constraint_index: usize, #[cfg_attr(feature = "serde-serialize", serde(skip))] pub(crate) position_constraint_index: usize, // We put the following fields here to avoids reading the colliders inside of the // contact preparation method. /// Flags used to control some aspects of the constraints solver for this contact manifold. pub solver_flags: SolverFlags, /// The world-space contact normal shared by all the contact in this contact manifold. // NOTE: read the comment of `solver_contacts` regarding serialization. It applies // to this field as well. pub normal: Vector<Real>, /// The contacts that will be seen by the constraints solver for computing forces. // NOTE: unfortunately, we can't ignore this field when serialize // the contact manifold data. The reason is that the solver contacts // won't be updated for sleeping bodies. So it means that for one // frame, we won't have any solver contacts when waking up an island // after a deserialization. Not only does this break post-snapshot // determinism, but it will also skip constraint resolution for these // contacts during one frame. // // An alternative would be to skip the serialization of `solver_contacts` and // find a way to recompute them right after the deserialization process completes. // However, this would be an expensive operation. And doing this efficiently as part // of the narrow-phase update or the contact manifold collect will likely lead to tricky // bugs too. // // So right now it is best to just serialize this field and keep it that way until it // is proven to be actually problematic in real applications (in terms of snapshot size for example). pub solver_contacts: Vec<SolverContact>, /// The relative dominance of the bodies involved in this contact manifold. pub relative_dominance: i16, /// A user-defined piece of data. pub user_data: u32, } /// A contact seen by the constraints solver for computing forces. #[derive(Copy, Clone, Debug)] #[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] pub struct SolverContact { /// The index of the manifold contact used to generate this solver contact. pub(crate) contact_id: u8, /// The world-space contact point. pub point: Point<Real>, /// The distance between the two original contacts points along the contact normal. /// If negative, this is measures the penetration depth. pub dist: Real, /// The effective friction coefficient at this contact point. pub friction: Real, /// The effective restitution coefficient at this contact point. pub restitution: Real, /// The desired tangent relative velocity at the contact point. /// /// This is set to zero by default. Set to a non-zero value to /// simulate, e.g., conveyor belts. pub tangent_velocity: Vector<Real>, /// The warmstart impulse, along the contact normal, applied by this contact to the first collider's rigid-body. pub warmstart_impulse: Real, /// The warmstart friction impulse along the vector orthonormal to the contact normal, applied to the first /// collider's rigid-body. #[cfg(feature = "dim2")] pub warmstart_tangent_impulse: Real, /// The warmstart friction impulses along the basis orthonormal to the contact normal, applied to the first /// collider's rigid-body. #[cfg(feature = "dim3")] pub warmstart_tangent_impulse: na::Vector2<Real>, /// The last velocity correction targeted by this contact. pub prev_rhs: Real, } impl SolverContact { /// Should we treat this contact as a bouncy contact? /// If `true`, use [`Self::restitution`]. pub fn is_bouncy(&self) -> bool { let is_new = self.warmstart_impulse == 0.0; if is_new { // Treat new collisions as bouncing at first, unless we have zero restitution. self.restitution > 0.0 } else { // If the contact is still here one step later, it is now a resting contact. // The exception is very high restitutions, which can never rest self.restitution >= 1.0 } } } impl Default for ContactManifoldData { fn default() -> Self { Self::new( BodyPair::new(RigidBodyHandle::invalid(), RigidBodyHandle::invalid()), SolverFlags::empty(), ) } } impl ContactManifoldData { pub(crate) fn new(body_pair: BodyPair, solver_flags: SolverFlags) -> ContactManifoldData { Self { body_pair, warmstart_multiplier: Self::min_warmstart_multiplier(), constraint_index: 0, position_constraint_index: 0, solver_flags, normal: Vector::zeros(), solver_contacts: Vec::new(), relative_dominance: 0, user_data: 0, } } /// Number of actives contacts, i.e., contacts that will be seen by /// the constraints solver. #[inline] pub fn num_active_contacts(&self) -> usize { self.solver_contacts.len() } pub(crate) fn min_warmstart_multiplier() -> Real { // Multiplier used to reduce the amount of warm-starting. // This coefficient increases exponentially over time, until it reaches 1.0. // This will reduce significant overshoot at the timesteps that // follow a timestep involving high-velocity impacts. 1.0 // 0.01 } // pub(crate) fn update_warmstart_multiplier(manifold: &mut ContactManifold) { // // In 2D, tall stacks will actually suffer from this // // because oscillation due to inaccuracies in 2D often // // cause contacts to break, which would result in // // a reset of the warmstart multiplier. // if cfg!(feature = "dim2") { // manifold.data.warmstart_multiplier = 1.0; // return; // } // // for pt in &manifold.points { // if pt.data.impulse != 0.0 { // manifold.data.warmstart_multiplier = // (manifold.data.warmstart_multiplier * 2.0).min(1.0); // return; // } // } // // // Reset the multiplier. // manifold.data.warmstart_multiplier = Self::min_warmstart_multiplier() // } }