From 50b02ccccc49d2fa2edbd74d41335109e185a0bc Mon Sep 17 00:00:00 2001 From: Joona Aalto Date: Sat, 23 Sep 2023 17:10:36 +0300 Subject: [PATCH] Fix contact stability for non-convex colliders (#156) --- .../examples/basic_kinematic_character.rs | 8 +- ...tion_is_deterministic_across_machines.snap | 890 +++++++++--------- src/constraints/penetration.rs | 30 +- src/plugins/narrow_phase.rs | 130 ++- src/plugins/solver.rs | 2 +- 5 files changed, 522 insertions(+), 538 deletions(-) diff --git a/crates/bevy_xpbd_3d/examples/basic_kinematic_character.rs b/crates/bevy_xpbd_3d/examples/basic_kinematic_character.rs index ddd891f8..c86caa10 100644 --- a/crates/bevy_xpbd_3d/examples/basic_kinematic_character.rs +++ b/crates/bevy_xpbd_3d/examples/basic_kinematic_character.rs @@ -129,7 +129,7 @@ fn movement( fn kinematic_collision( collisions: Res, - mut bodies: Query<(&RigidBody, &mut Position)>, + mut bodies: Query<(&RigidBody, &mut Position, &Rotation)>, ) { // Iterate through collisions and move the kinematic body to resolve penetration for contacts in collisions.iter() { @@ -137,7 +137,7 @@ fn kinematic_collision( if !contacts.during_current_substep { continue; } - if let Ok([(rb1, mut position1), (rb2, mut position2)]) = + if let Ok([(rb1, mut position1, rotation1), (rb2, mut position2, _)]) = bodies.get_many_mut([contacts.entity1, contacts.entity2]) { for manifold in contacts.manifolds.iter() { @@ -146,9 +146,9 @@ fn kinematic_collision( continue; } if rb1.is_kinematic() && !rb2.is_kinematic() { - position1.0 -= contact.normal * contact.penetration; + position1.0 -= contact.global_normal1(rotation1) * contact.penetration; } else if rb2.is_kinematic() && !rb1.is_kinematic() { - position2.0 += contact.normal * contact.penetration; + position2.0 += contact.global_normal1(rotation1) * contact.penetration; } } } diff --git a/crates/bevy_xpbd_3d/snapshots/bevy_xpbd_3d__tests__cubes_simulation_is_deterministic_across_machines.snap b/crates/bevy_xpbd_3d/snapshots/bevy_xpbd_3d__tests__cubes_simulation_is_deterministic_across_machines.snap index 9b396c86..096847ab 100644 --- a/crates/bevy_xpbd_3d/snapshots/bevy_xpbd_3d__tests__cubes_simulation_is_deterministic_across_machines.snap +++ b/crates/bevy_xpbd_3d/snapshots/bevy_xpbd_3d__tests__cubes_simulation_is_deterministic_across_machines.snap @@ -9,15 +9,15 @@ expression: bodies ), Transform { translation: Vec3( - -4.2808523, - 0.4999685, - -3.4611125, + -4.2262774, + 0.49999836, + -3.5611808, ), rotation: Quat( - 1.7554208e-5, - 0.19758488, - -7.944141e-6, - 0.98028576, + 2.1220249e-6, + 0.18354411, + -1.4208962e-6, + 0.9830115, ), scale: Vec3( 1.0, @@ -32,15 +32,15 @@ expression: bodies ), Transform { translation: Vec3( - -3.933668, - 0.49997827, - -1.3244047, + -3.926723, + 0.49995816, + -1.3782593, ), rotation: Quat( - 1.4070059e-5, - 0.1305612, - -2.5664124e-6, - 0.99144024, + 7.346789e-6, + 0.15966205, + 9.815922e-6, + 0.9871717, ), scale: Vec3( 1.0, @@ -55,15 +55,15 @@ expression: bodies ), Transform { translation: Vec3( - -3.8560238, - 0.4999683, - 0.83298063, + -3.9405544, + 0.49995267, + 0.8453444, ), rotation: Quat( - 1.4154109e-5, - 0.112213224, - -1.2618709e-5, - 0.9936842, + 6.8830204e-6, + 0.13961706, + 1.9943848e-6, + 0.9902056, ), scale: Vec3( 1.0, @@ -78,15 +78,15 @@ expression: bodies ), Transform { translation: Vec3( - -3.8322668, - 0.49996102, - 2.8924694, + -3.8180623, + 0.4999987, + 2.9127028, ), rotation: Quat( - 1.090863e-5, - 0.11655842, - -5.799492e-6, - 0.99318385, + 3.2484472e-6, + 0.13258989, + -3.4715338e-6, + 0.991171, ), scale: Vec3( 1.0, @@ -101,15 +101,15 @@ expression: bodies ), Transform { translation: Vec3( - -1.871729, - 0.4999095, - -3.4358149, + -1.8968339, + 0.4999566, + -3.52113, ), rotation: Quat( - -1.7355153e-5, - 0.15113826, - 4.491107e-7, - 0.98851264, + -1.0881251e-5, + 0.17011896, + 2.513311e-6, + 0.9854235, ), scale: Vec3( 1.0, @@ -124,15 +124,15 @@ expression: bodies ), Transform { translation: Vec3( - -1.8297117, - 0.4999478, - -1.3351799, + -1.798137, + 0.49994323, + -1.37892, ), rotation: Quat( - 1.23525515e-5, - 0.14159575, - 7.9547306e-7, - 0.98992455, + -6.3761183e-7, + 0.14213482, + 8.613016e-6, + 0.9898473, ), scale: Vec3( 1.0, @@ -147,15 +147,15 @@ expression: bodies ), Transform { translation: Vec3( - -1.7145333, - 0.4999623, - 0.7797917, + -1.8498914, + 0.4999753, + 0.74135125, ), rotation: Quat( - 5.598495e-6, - 0.12386564, - 1.2801787e-6, - 0.992299, + 1.4581477e-5, + 0.13032992, + -7.776751e-6, + 0.9914707, ), scale: Vec3( 1.0, @@ -170,15 +170,15 @@ expression: bodies ), Transform { translation: Vec3( - -1.7581693, - 0.49999738, - 2.8742201, + -1.7432851, + 0.49999794, + 2.857871, ), rotation: Quat( - 5.5115765e-6, - 0.11671545, - -4.6088303e-6, - 0.9931654, + 3.9478937e-6, + 0.13605668, + -3.991767e-6, + 0.9907011, ), scale: Vec3( 1.0, @@ -193,15 +193,15 @@ expression: bodies ), Transform { translation: Vec3( - 0.22874303, - 0.4999233, - -3.5094616, + 0.23195699, + 0.49995002, + -3.6020947, ), rotation: Quat( - -1.0638237e-5, - 0.14188954, - 3.2888086e-6, - 0.9898825, + -1.670087e-5, + 0.1582101, + 9.463987e-7, + 0.9874055, ), scale: Vec3( 1.0, @@ -216,15 +216,15 @@ expression: bodies ), Transform { translation: Vec3( - 0.25914, - 0.49995306, - -1.4134492, + 0.30980664, + 0.4999788, + -1.3611503, ), rotation: Quat( - 9.547222e-6, - 0.1479191, - -3.919611e-7, - 0.9889995, + 1.546818e-5, + 0.1463667, + 6.62782e-7, + 0.9892304, ), scale: Vec3( 1.0, @@ -239,15 +239,15 @@ expression: bodies ), Transform { translation: Vec3( - 0.39905748, - 0.49995488, - 0.7667921, + 0.40833664, + 0.49999106, + 0.82969695, ), rotation: Quat( - 2.6026e-6, - 0.122003846, - -8.53572e-6, - 0.99252963, + 8.350002e-6, + 0.14864042, + 1.7996334e-6, + 0.9888913, ), scale: Vec3( 1.0, @@ -262,15 +262,15 @@ expression: bodies ), Transform { translation: Vec3( - 0.34971786, - 0.49995115, - 2.8560696, + 0.3963089, + 0.4999993, + 2.943874, ), rotation: Quat( - 9.60623e-6, - 0.12706521, - -1.789169e-6, - 0.99189436, + 1.7665851e-6, + 0.14842938, + -1.98549e-6, + 0.988923, ), scale: Vec3( 1.0, @@ -285,15 +285,15 @@ expression: bodies ), Transform { translation: Vec3( - 2.3290455, - 0.49995095, - -3.5218043, + 2.3375318, + 0.49999616, + -3.7299113, ), rotation: Quat( - -3.0293163e-6, - 0.13794287, - 4.106248e-6, - 0.9904402, + 5.69649e-6, + 0.13108972, + -5.1506854e-6, + 0.9913705, ), scale: Vec3( 1.0, @@ -308,15 +308,15 @@ expression: bodies ), Transform { translation: Vec3( - 2.3782558, - 0.49996454, - -1.4007403, + 2.3561568, + 0.49998978, + -1.5878445, ), rotation: Quat( - 5.615095e-6, - 0.13768105, - 5.0614295e-7, - 0.9904766, + 9.109252e-6, + 0.13484776, + 4.749412e-7, + 0.9908663, ), scale: Vec3( 1.0, @@ -331,15 +331,15 @@ expression: bodies ), Transform { translation: Vec3( - 2.472831, - 0.49996758, - 0.72425413, + 2.442345, + 0.49998337, + 0.5792768, ), rotation: Quat( - 2.1177884e-6, - 0.12944159, - -1.8347126e-6, - 0.99158704, + 1.484838e-5, + 0.15672006, + 7.2652992e-6, + 0.98764306, ), scale: Vec3( 1.0, @@ -354,15 +354,15 @@ expression: bodies ), Transform { translation: Vec3( - 2.4231791, - 0.49996126, - 2.8285704, + 2.5143192, + 0.49998024, + 2.8473952, ), rotation: Quat( - 3.668589e-6, - 0.13270181, - 3.1187703e-6, - 0.991156, + -4.865092e-6, + 0.17413911, + 1.1804348e-5, + 0.98472106, ), scale: Vec3( 1.0, @@ -377,15 +377,15 @@ expression: bodies ), Transform { translation: Vec3( - -6.8150816, - 0.49999908, - -2.9598732, + -6.6539173, + 0.49999905, + -3.0665104, ), rotation: Quat( - 0.21582566, - 0.21582511, - 0.6733641, - 0.67336434, + 0.16036975, + 0.16036712, + 0.68868184, + 0.68868077, ), scale: Vec3( 1.0, @@ -400,15 +400,15 @@ expression: bodies ), Transform { translation: Vec3( - -4.613422, - 2.4999228, - -1.6324793, + -4.826741, + 2.4999378, + -1.515056, ), rotation: Quat( - 5.5435926e-6, - 0.14923605, - -3.8059375e-6, - 0.9888016, + 5.0038902e-6, + 0.14127879, + 2.3176458e-6, + 0.98996985, ), scale: Vec3( 1.0, @@ -423,15 +423,15 @@ expression: bodies ), Transform { translation: Vec3( - -4.389008, - 2.4999285, - 0.5228891, + -4.7290606, + 2.4999418, + 0.58596283, ), rotation: Quat( - 2.1365788e-6, - 0.1508865, - -8.52367e-6, - 0.9885511, + 2.044745e-6, + 0.1192556, + -5.0030003e-6, + 0.9928636, ), scale: Vec3( 1.0, @@ -446,15 +446,15 @@ expression: bodies ), Transform { translation: Vec3( - -4.4488225, - 2.4999547, - 2.6629498, + -4.3601375, + 2.4999368, + 2.6145566, ), rotation: Quat( - 2.2520873e-5, - 0.14425053, - -2.2108092e-5, - 0.9895412, + 1.6254893e-5, + 0.14187528, + -1.92912e-5, + 0.98988456, ), scale: Vec3( 1.0, @@ -469,15 +469,15 @@ expression: bodies ), Transform { translation: Vec3( - -2.4219842, - 2.4998739, - -4.212161, + -2.460338, + 2.499943, + -3.9757233, ), rotation: Quat( - -3.2085532e-5, - 0.19260663, - -1.2371634e-5, - 0.98127604, + -9.233557e-6, + 0.16729747, + -1.2055349e-5, + 0.9859065, ), scale: Vec3( 1.0, @@ -492,15 +492,15 @@ expression: bodies ), Transform { translation: Vec3( - -1.9872011, - 2.4999397, - -1.912023, + -2.1301255, + 2.4999263, + -1.6766281, ), rotation: Quat( - 2.5831364e-6, - 0.13315079, - -3.4478587e-6, - 0.9910958, + 1.172138e-6, + 0.14524418, + 4.8920538e-6, + 0.98939586, ), scale: Vec3( 1.0, @@ -515,15 +515,15 @@ expression: bodies ), Transform { translation: Vec3( - -2.0692613, - 2.4999352, - 0.32353857, + -2.3024871, + 2.499916, + 0.47962222, ), rotation: Quat( - -1.8388939e-6, - 0.17447595, - -1.4337936e-5, - 0.98466146, + 2.2038537e-6, + 0.11762403, + -2.80928e-6, + 0.9930582, ), scale: Vec3( 1.0, @@ -538,15 +538,15 @@ expression: bodies ), Transform { translation: Vec3( - -1.8446898, - 2.499903, - 2.472626, + -2.0606182, + 2.4998996, + 2.5273693, ), rotation: Quat( - 1.8243752e-5, - 0.11519989, - -3.3390427e-6, - 0.99334234, + 2.5034875e-5, + 0.12346067, + -2.1054308e-5, + 0.99234945, ), scale: Vec3( 1.0, @@ -561,15 +561,15 @@ expression: bodies ), Transform { translation: Vec3( - -0.17258997, - 2.4998868, - -4.173493, + -0.17280325, + 2.4999108, + -4.1953974, ), rotation: Quat( - -1.9001454e-5, - 0.15730621, - -1.10834335e-5, - 0.9875499, + -1.9545467e-5, + 0.13487116, + -4.9120085e-6, + 0.99086314, ), scale: Vec3( 1.0, @@ -584,15 +584,15 @@ expression: bodies ), Transform { translation: Vec3( - 0.10075573, - 2.4999568, - -1.9057032, + 0.07901489, + 2.4999442, + -1.714156, ), rotation: Quat( - 2.0991577e-6, - 0.13496341, - -1.2461851e-5, - 0.99085057, + 1.5577083e-5, + 0.15191895, + 1.5562659e-6, + 0.98839295, ), scale: Vec3( 1.0, @@ -607,15 +607,15 @@ expression: bodies ), Transform { translation: Vec3( - 0.28708982, - 2.4999528, - 0.30903396, + 0.06530667, + 2.4999275, + 0.4874102, ), rotation: Quat( - 2.0175634e-7, - 0.14203982, - -6.082613e-6, - 0.98986095, + 2.6546693e-6, + 0.16928098, + 1.0508955e-5, + 0.9855678, ), scale: Vec3( 1.0, @@ -630,15 +630,15 @@ expression: bodies ), Transform { translation: Vec3( - 0.25000283, - 2.4999495, - 2.5112715, + 0.35943326, + 2.4999516, + 2.6545174, ), rotation: Quat( - 4.6221794e-6, - 0.11299474, - -1.5866872e-5, - 0.9935956, + 1.7300188e-5, + 0.09629888, + 1.5979526e-7, + 0.99535245, ), scale: Vec3( 1.0, @@ -653,15 +653,15 @@ expression: bodies ), Transform { translation: Vec3( - 2.259403, - 2.499959, - -4.082096, + 2.1681814, + 2.499932, + -4.2093606, ), rotation: Quat( - -1.1213162e-5, - 0.16099952, - 8.305082e-6, - 0.9869545, + -1.558241e-5, + 0.16843592, + 1.0007317e-6, + 0.9857126, ), scale: Vec3( 1.0, @@ -676,15 +676,15 @@ expression: bodies ), Transform { translation: Vec3( - 2.2753742, - 2.499975, - -1.9631094, + 2.1973574, + 2.4999537, + -2.0230927, ), rotation: Quat( - 2.9962378e-6, - 0.14852113, - 3.330543e-6, - 0.98890924, + 1.3387881e-5, + 0.16002584, + -9.465019e-8, + 0.9871128, ), scale: Vec3( 1.0, @@ -699,15 +699,15 @@ expression: bodies ), Transform { translation: Vec3( - 2.3829887, - 2.4999733, - 0.1922172, + 2.2065744, + 2.4999666, + 0.16356395, ), rotation: Quat( - 1.2458987e-6, - 0.15344569, - 2.3846633e-6, - 0.9881571, + 9.4101915e-6, + 0.10407463, + 4.77171e-6, + 0.9945695, ), scale: Vec3( 1.0, @@ -722,15 +722,15 @@ expression: bodies ), Transform { translation: Vec3( - 2.4150975, - 2.4999623, - 2.3902981, + 2.488692, + 2.499952, + 2.2772586, ), rotation: Quat( - 2.3086507e-6, - 0.122624554, - -1.0820457e-6, - 0.99245316, + 4.7712615e-6, + 0.16946936, + 6.5987247e-6, + 0.98553544, ), scale: Vec3( 1.0, @@ -745,15 +745,15 @@ expression: bodies ), Transform { translation: Vec3( - -10.860568, - 0.49999896, - -3.2778966, + -11.035082, + 0.4999994, + -3.7090664, ), rotation: Quat( - -0.08826579, - 0.08826733, - -0.70157665, - 0.7015755, + 1.3385987e-6, + -0.20369437, + -6.275247e-7, + 0.97903454, ), scale: Vec3( 1.0, @@ -768,15 +768,15 @@ expression: bodies ), Transform { translation: Vec3( - -9.947806, - 0.49999905, - -1.4161216, + -9.549925, + 0.49999914, + -0.6892178, ), rotation: Quat( - 0.077625304, - 0.07762316, - 0.70283383, - 0.7028325, + 0.07123954, + 0.071237214, + 0.7035098, + 0.70350844, ), scale: Vec3( 1.0, @@ -791,15 +791,15 @@ expression: bodies ), Transform { translation: Vec3( - -7.533495, - 0.49998936, - 0.9502237, + -7.587587, + 0.4999996, + 1.1305965, ), rotation: Quat( - 0.07624808, - 0.07624006, - 0.70298773, - 0.70298076, + 0.12678885, + 0.12678716, + 0.6956475, + 0.6956466, ), scale: Vec3( 1.0, @@ -814,15 +814,15 @@ expression: bodies ), Transform { translation: Vec3( - -8.994063, - 0.4999978, - 3.6871085, + -7.14953, + 0.4999994, + 3.2699316, ), rotation: Quat( - 0.10229478, - -3.5243152e-6, - 0.99475414, - -1.5435597e-6, + 0.09122436, + 0.09122254, + 0.7011983, + 0.7011972, ), scale: Vec3( 1.0, @@ -837,15 +837,15 @@ expression: bodies ), Transform { translation: Vec3( - -4.263115, - 4.4998507, - -3.5854475, + -4.0485373, + 4.4999084, + -3.6362913, ), rotation: Quat( - -3.1071046e-5, - -0.005596315, - 1.8940453e-5, - 0.9999843, + -7.732101e-6, + 0.00255698, + 1.8393037e-5, + 0.9999967, ), scale: Vec3( 1.0, @@ -860,15 +860,15 @@ expression: bodies ), Transform { translation: Vec3( - -3.590759, - 4.499923, - -1.3968698, + -3.6787837, + 4.4999213, + -1.5449456, ), rotation: Quat( - 1.3973383e-6, - 0.007637522, - -2.3091616e-6, - 0.99997085, + -4.1159433e-6, + 0.03313438, + -4.0296004e-6, + 0.9994509, ), scale: Vec3( 1.0, @@ -883,15 +883,15 @@ expression: bodies ), Transform { translation: Vec3( - -3.595955, - 4.4999323, - 0.6402387, + -4.048178, + 4.4999313, + 0.52660257, ), rotation: Quat( - 2.1597057e-6, - 0.011675238, - -7.5353487e-6, - 0.9999319, + 4.355052e-6, + -0.0050044977, + -8.365385e-6, + 0.9999875, ), scale: Vec3( 1.0, @@ -906,15 +906,15 @@ expression: bodies ), Transform { translation: Vec3( - -3.3588028, - 4.4998956, - 2.9988136, + -3.271152, + 4.4999027, + 2.7267506, ), rotation: Quat( - 1.3696184e-5, - -0.14460245, - -1.3732674e-5, - 0.98948985, + 1.2892647e-5, + -0.09242607, + -1.0899711e-5, + 0.99571955, ), scale: Vec3( 1.0, @@ -929,15 +929,15 @@ expression: bodies ), Transform { translation: Vec3( - -1.8235304, - 4.499875, - -4.024092, + -1.8714477, + 4.4999332, + -3.8852575, ), rotation: Quat( - -3.149693e-5, - -0.003218935, - -2.145219e-6, - 0.9999948, + -1.5450873e-5, + -0.004562731, + -4.023357e-6, + 0.99998957, ), scale: Vec3( 1.0, @@ -952,15 +952,15 @@ expression: bodies ), Transform { translation: Vec3( - -1.4918175, - 4.499935, - -1.597874, + -1.5847538, + 4.4999285, + -1.6400747, ), rotation: Quat( - 3.6171002e-6, - 0.03206979, - -1.6574442e-6, - 0.9994856, + 2.6186428e-6, + 0.006453742, + 8.780998e-8, + 0.9999792, ), scale: Vec3( 1.0, @@ -975,15 +975,15 @@ expression: bodies ), Transform { translation: Vec3( - -1.2995759, - 4.49993, - 0.5269054, + -1.8391577, + 4.4999137, + 0.69582856, ), rotation: Quat( - -3.0011788e-6, - -0.011971369, - -5.1134793e-6, - 0.99992836, + 1.8484921e-7, + 0.028858757, + -2.3530927e-7, + 0.9995835, ), scale: Vec3( 1.0, @@ -998,15 +998,15 @@ expression: bodies ), Transform { translation: Vec3( - -1.2103232, - 4.499882, - 3.3404381, + -0.86039716, + 4.499856, + 3.2348351, ), rotation: Quat( - 2.6005475e-5, - -0.11672331, - 8.146151e-6, - 0.9931645, + 1.7270359e-5, + -0.18016347, + 1.6850285e-6, + 0.9836367, ), scale: Vec3( 1.0, @@ -1021,15 +1021,15 @@ expression: bodies ), Transform { translation: Vec3( - 0.8491453, - 4.499883, - -4.152838, + 1.0746174, + 4.499887, + -4.4307847, ), rotation: Quat( - -1.5649342e-5, - 0.05098479, - 1.3462705e-5, - 0.9986994, + -3.170545e-5, + 0.008868818, + 3.355555e-6, + 0.99996066, ), scale: Vec3( 1.0, @@ -1044,15 +1044,15 @@ expression: bodies ), Transform { translation: Vec3( - 1.9380878, - 4.499931, - -1.9938416, + 1.9252568, + 4.4999266, + -2.2284513, ), rotation: Quat( - -3.164763e-6, - -0.048179384, - 7.762069e-7, - 0.9988387, + 1.358791e-5, + -0.054339092, + -1.06467005e-5, + 0.9985225, ), scale: Vec3( 1.0, @@ -1067,15 +1067,15 @@ expression: bodies ), Transform { translation: Vec3( - 1.4635266, - 4.4999404, - 0.41117892, + 0.3556286, + 4.499931, + 0.5160491, ), rotation: Quat( - 4.1360641e-7, - -0.14204553, - 4.048724e-6, - 0.9898601, + 3.5402074e-6, + 0.033408124, + 1.0017324e-5, + 0.9994418, ), scale: Vec3( 1.0, @@ -1090,15 +1090,15 @@ expression: bodies ), Transform { translation: Vec3( - 1.6743294, - 4.499907, - 3.240356, + 1.8977205, + 4.4998817, + 3.0972981, ), rotation: Quat( - 9.061509e-6, - 0.09324181, - -1.37986e-5, - 0.9956435, + 9.690365e-6, + 0.12031976, + -2.9403623e-6, + 0.9927352, ), scale: Vec3( 1.0, @@ -1113,15 +1113,15 @@ expression: bodies ), Transform { translation: Vec3( - -4.2405953, + -4.2183733, 0.4999994, - -6.974088, + -7.0678115, ), rotation: Quat( - -0.7052065, - -0.051812235, - -0.051813994, - 0.7052054, + -0.70680106, + -0.02081043, + -0.02081214, + 0.70679986, ), scale: Vec3( 1.0, @@ -1136,15 +1136,15 @@ expression: bodies ), Transform { translation: Vec3( - -7.801992, - 1.9492568, - -0.9480146, + -7.7116013, + 2.0050244, + -1.0565078, ), rotation: Quat( - 0.23756787, - -0.22613806, - 0.81588006, - 0.47619623, + 0.17723528, + -0.27912667, + 0.89874506, + -0.28798133, ), scale: Vec3( 1.0, @@ -1159,15 +1159,15 @@ expression: bodies ), Transform { translation: Vec3( - -8.924642, - 2.4999678, - 2.3635445, + -11.959986, + 0.4999985, + 4.3528647, ), rotation: Quat( - -0.03499876, - -0.034995556, - 0.70624554, - 0.7062348, + 0.5658154, + -1.815257e-6, + 0.8245319, + 1.9827276e-7, ), scale: Vec3( 1.0, @@ -1182,15 +1182,15 @@ expression: bodies ), Transform { translation: Vec3( - -5.496507, + -6.0729713, 0.4999994, - 6.51135, + 6.208802, ), rotation: Quat( - 0.68958783, - -0.15642856, - 0.1564272, - 0.6895863, + 0.5104595, + -0.48931825, + 0.4893179, + 0.5104574, ), scale: Vec3( 1.0, @@ -1205,15 +1205,15 @@ expression: bodies ), Transform { translation: Vec3( - -2.464438, - 6.499846, - -4.402035, + -0.8316099, + 0.4999994, + -7.1540723, ), rotation: Quat( - -3.768181e-5, - -0.005659725, - -6.979848e-7, - 0.99998397, + -0.4699715, + 0.46997193, + -0.5283253, + 0.5283233, ), scale: Vec3( 1.0, @@ -1228,15 +1228,15 @@ expression: bodies ), Transform { translation: Vec3( - -2.7314923, - 6.4999294, - -1.8473227, + -2.6363013, + 6.49992, + -2.2863734, ), rotation: Quat( - 6.39091e-6, - 0.073617145, - -1.2347451e-6, - 0.99728656, + 9.824184e-6, + -0.0841772, + 4.776257e-7, + 0.9964508, ), scale: Vec3( 1.0, @@ -1251,15 +1251,15 @@ expression: bodies ), Transform { translation: Vec3( - -2.9114885, - 6.4999247, - 0.2717856, + -3.2863405, + 6.4999266, + 0.3935666, ), rotation: Quat( - 3.468296e-6, - 0.101680666, - -3.2886228e-6, - 0.9948171, + 5.7852903e-6, + -0.00452012, + -6.451019e-6, + 0.9999898, ), scale: Vec3( 1.0, @@ -1274,15 +1274,15 @@ expression: bodies ), Transform { translation: Vec3( - -2.9411807, - 6.4999037, - 2.4014835, + -2.603597, + 6.499864, + 2.7479994, ), rotation: Quat( - 1.2608704e-5, - 0.036469676, - -9.85756e-6, - 0.99933475, + 9.463718e-6, + 0.012097906, + -1.448419e-5, + 0.9999268, ), scale: Vec3( 1.0, @@ -1297,15 +1297,15 @@ expression: bodies ), Transform { translation: Vec3( - -0.28635675, - 6.49986, - -4.552955, + -0.008580337, + 6.49987, + -4.860198, ), rotation: Quat( - -2.63417e-5, - -0.060595896, - 3.3738534e-6, - 0.9981624, + -3.3132837e-5, + -0.06208005, + -3.1977895e-6, + 0.9980712, ), scale: Vec3( 1.0, @@ -1320,15 +1320,15 @@ expression: bodies ), Transform { translation: Vec3( - -0.36941794, - 6.4999175, - -2.1366715, + 0.14060867, + 6.1867003, + -1.8553398, ), rotation: Quat( - 1.0421902e-5, - 0.010490783, - -1.4567826e-5, - 0.999945, + -0.019257072, + -0.030512495, + -0.41433406, + 0.9094094, ), scale: Vec3( 1.0, @@ -1343,15 +1343,15 @@ expression: bodies ), Transform { translation: Vec3( - -0.285444, - 6.499927, - 0.11148358, + -1.1888483, + 6.4999175, + 0.49970502, ), rotation: Quat( - -2.937589e-6, - -0.033960484, - 2.4110946e-6, - 0.9994232, + 3.2975263e-6, + -0.03631929, + 2.472949e-6, + 0.99934024, ), scale: Vec3( 1.0, @@ -1366,15 +1366,15 @@ expression: bodies ), Transform { translation: Vec3( - -0.6425312, - 6.4998775, - 2.8303332, + -0.4520409, + 6.4998817, + 2.7539823, ), rotation: Quat( - 1.8583329e-5, - 0.06412834, - -3.5832193e-6, - 0.9979417, + 1.8681434e-5, + 0.056703705, + 4.219731e-6, + 0.99839103, ), scale: Vec3( 1.0, @@ -1389,15 +1389,15 @@ expression: bodies ), Transform { translation: Vec3( - 10.609751, + 14.963633, 0.4999994, - -6.7881227, + -5.995982, ), rotation: Quat( - 0.4771036, - -0.47710398, - 0.52189374, - -0.52189165, + 0.4247451, + -0.4247457, + 0.5653251, + -0.5653231, ), scale: Vec3( 1.0, @@ -1412,15 +1412,15 @@ expression: bodies ), Transform { translation: Vec3( - 2.1082666, - 6.499923, - -2.1486413, + 6.7241526, + 0.4999994, + -3.781113, ), rotation: Quat( - -2.447195e-6, - -0.019011809, - -5.4155075e-6, - 0.9998193, + 0.3986271, + -0.5840353, + -0.5840354, + -0.39862502, ), scale: Vec3( 1.0, @@ -1435,15 +1435,15 @@ expression: bodies ), Transform { translation: Vec3( - 7.5059934, - 0.4999994, - 2.5172446, + 10.657146, + 0.4999985, + 1.9236538, ), rotation: Quat( - 0.2772865, - 0.27728754, - -0.65047145, - -0.6504696, + -1.8138896e-6, + 0.57131994, + -2.104147e-7, + -0.82072747, ), scale: Vec3( 1.0, @@ -1458,15 +1458,15 @@ expression: bodies ), Transform { translation: Vec3( - 2.245832, - 6.4999104, - 2.5304873, + 1.9273113, + 6.4998965, + 2.8084333, ), rotation: Quat( - 1.2160268e-5, - 0.132467, - -1.8158184e-5, - 0.9911874, + 8.120539e-6, + -0.13246574, + -1.0081462e-5, + 0.9911876, ), scale: Vec3( 1.0, diff --git a/src/constraints/penetration.rs b/src/constraints/penetration.rs index cccec415..77330dcb 100644 --- a/src/constraints/penetration.rs +++ b/src/constraints/penetration.rs @@ -44,19 +44,9 @@ impl XpbdConstraint<2> for PenetrationConstraint { fn solve(&mut self, bodies: [&mut RigidBodyQueryItem; 2], dt: Scalar) { let [body1, body2] = bodies; - // For convex-convex collider contacts, we can compute the penetration at the current state - // using the contact points like the XPBD paper suggests, which reduces explosiveness. - // - // However, non-convex colliders cause convex colliders to sink into them unless we use - // the penetration depth provided by Parry. - // - // Todo: Figure out why this is and use the method below for all collider types in order to fix - // explosions for all contacts. - if self.contact.convex { - let p1 = body1.current_position() + body1.rotation.rotate(self.r1); - let p2 = body2.current_position() + body2.rotation.rotate(self.r2); - self.contact.penetration = (p1 - p2).dot(self.contact.global_normal(&body1.rotation)); - } + let p1 = body1.current_position() + body1.rotation.rotate(self.contact.point1); + let p2 = body2.current_position() + body2.rotation.rotate(self.contact.point2); + self.contact.penetration = (p1 - p2).dot(self.contact.global_normal1(&body1.rotation)); // If penetration depth is under 0, skip the collision if self.contact.penetration <= Scalar::EPSILON { @@ -103,7 +93,7 @@ impl PenetrationConstraint { let compliance = self.compliance; let lagrange = self.normal_lagrange; let penetration = self.contact.penetration; - let normal = self.contact.global_normal(&body1.rotation); + let normal = self.contact.global_normal1(&body1.rotation); let r1 = body1.rotation.rotate(self.r1); let r2 = body2.rotation.rotate(self.r2); @@ -137,15 +127,17 @@ impl PenetrationConstraint { let compliance = self.compliance; let lagrange = self.tangent_lagrange; let penetration = self.contact.penetration; - let normal = self.contact.global_normal(&body1.rotation); + let normal = self.contact.global_normal1(&body1.rotation); let r1 = body1.rotation.rotate(self.r1); let r2 = body2.rotation.rotate(self.r2); // Compute relative motion of the contact points and get the tangential component - let delta_p1 = body1.current_position() - body1.previous_position.0 + r1 - - body1.previous_rotation.rotate(self.r1); - let delta_p2 = body2.current_position() - body2.previous_position.0 + r2 - - body2.previous_rotation.rotate(self.r2); + let delta_p1 = body1.current_position() - body1.previous_position.0 + + body1.rotation.rotate(self.contact.point1) + - body1.previous_rotation.rotate(self.contact.point1); + let delta_p2 = body2.current_position() - body2.previous_position.0 + + body2.rotation.rotate(self.contact.point2) + - body2.previous_rotation.rotate(self.contact.point2); let delta_p = delta_p1 - delta_p2; let delta_p_tangent = delta_p - delta_p.dot(normal) * normal; diff --git a/src/plugins/narrow_phase.rs b/src/plugins/narrow_phase.rs index 816f25de..8704cb5b 100644 --- a/src/plugins/narrow_phase.rs +++ b/src/plugins/narrow_phase.rs @@ -7,7 +7,7 @@ use bevy::prelude::*; #[cfg(feature = "parallel")] use bevy::tasks::{ComputeTaskPool, ParallelSlice}; use indexmap::IndexMap; -use parry::query::{PersistentQueryDispatcher, QueryDispatcher}; +use parry::query::PersistentQueryDispatcher; /// Computes contacts between entities and sends collision events. /// @@ -444,7 +444,10 @@ pub struct ContactManifold { pub contacts: Vec, /// A contact normal shared by all contacts in this manifold, /// expressed in the local space of the first entity. - pub normal: Vector, + pub normal1: Vector, + /// A contact normal shared by all contacts in this manifold, + /// expressed in the local space of the second entity. + pub normal2: Vector, } /// Data related to a contact between two bodies. @@ -455,12 +458,11 @@ pub struct ContactData { /// Contact point on the second entity in local coordinates. pub point2: Vector, /// A contact normal expressed in the local space of the first entity. - pub normal: Vector, + pub normal1: Vector, + /// A contact normal expressed in the local space of the second entity. + pub normal2: Vector, /// Penetration depth. pub penetration: Scalar, - /// True if both colliders are convex. Currently, contacts between - /// convex and non-convex colliders have to be handled differently. - pub(crate) convex: bool, } impl ContactData { @@ -477,8 +479,13 @@ impl ContactData { } /// Returns the world-space contact normal pointing towards the exterior of the first entity. - pub fn global_normal(&self, rotation: &Rotation) -> Vector { - rotation.rotate(self.normal) + pub fn global_normal1(&self, rotation: &Rotation) -> Vector { + rotation.rotate(self.normal1) + } + + /// Returns the world-space contact normal pointing towards the exterior of the second entity. + pub fn global_normal2(&self, rotation: &Rotation) -> Vector { + rotation.rotate(self.normal2) } } @@ -498,76 +505,61 @@ pub(crate) fn compute_contacts( let isometry1 = utils::make_isometry(position1, rotation1); let isometry2 = utils::make_isometry(position2, rotation2); let isometry12 = isometry1.inv_mul(&isometry2); - let convex = collider1.is_convex() && collider2.is_convex(); - - if convex { - // Todo: Reuse manifolds from previous frame to improve performance - let mut manifolds: Vec> = vec![]; - let _ = parry::query::DefaultQueryDispatcher.contact_manifolds( - &isometry12, - collider1.get_shape().0.as_ref(), - collider2.get_shape().0.as_ref(), - prediction_distance, - &mut manifolds, - &mut None, - ); - Contacts { - entity1, - entity2, - manifolds: manifolds - .iter() - .map(|manifold| ContactManifold { + + // Todo: Reuse manifolds from previous frame to improve performance + let mut manifolds: Vec> = vec![]; + let _ = parry::query::DefaultQueryDispatcher.contact_manifolds( + &isometry12, + collider1.get_shape().0.as_ref(), + collider2.get_shape().0.as_ref(), + prediction_distance, + &mut manifolds, + &mut None, + ); + Contacts { + entity1, + entity2, + manifolds: manifolds + .iter() + .filter_map(|manifold| { + let subpos1 = manifold.subshape_pos1.unwrap_or_default(); + let subpos2 = manifold.subshape_pos2.unwrap_or_default(); + let normal1: Vector = subpos1 + .rotation + .transform_vector(&manifold.local_n1) + .normalize() + .into(); + let normal2: Vector = subpos2 + .rotation + .transform_vector(&manifold.local_n2) + .normalize() + .into(); + + // Make sure normals are valid + if !normal1.is_normalized() || !normal2.is_normalized() { + return None; + } + + Some(ContactManifold { entity1, entity2, - normal: manifold.local_n1.into(), + normal1, + normal2, contacts: manifold .contacts() .iter() .map(|contact| ContactData { - point1: contact.local_p1.into(), - point2: contact.local_p2.into(), - normal: manifold.local_n1.into(), + point1: subpos1.transform_point(&contact.local_p1).into(), + point2: subpos2.transform_point(&contact.local_p2).into(), + normal1, + normal2, penetration: -contact.dist, - convex, }) .collect(), }) - .collect(), - during_current_frame: true, - during_current_substep: true, - } - } else { - // For some reason, convex colliders sink into non-convex colliders - // if we use contact manifolds, so we have to compute a single contact point instead. - // Todo: Find out why this is and use contact manifolds for both types of colliders. - let contact = parry::query::DefaultQueryDispatcher - .contact( - &isometry12, - collider1.get_shape().0.as_ref(), - collider2.get_shape().0.as_ref(), - prediction_distance, - ) - .unwrap() - .map(|contact| ContactData { - point1: contact.point1.into(), - point2: contact.point2.into(), - normal: contact.normal1.into(), - penetration: -contact.dist, - convex, - }); - Contacts { - entity1, - entity2, - manifolds: contact.map_or(vec![], |contact| { - vec![ContactManifold { - entity1, - entity2, - contacts: vec![contact], - normal: contact.normal, - }] - }), - during_current_frame: true, - during_current_substep: true, - } + }) + .collect(), + during_current_frame: true, + during_current_substep: true, } } diff --git a/src/plugins/solver.rs b/src/plugins/solver.rs index d7aff51b..3050caeb 100644 --- a/src/plugins/solver.rs +++ b/src/plugins/solver.rs @@ -307,7 +307,7 @@ fn solve_vel( continue; } - let normal = constraint.contact.global_normal(&body1.rotation); + let normal = constraint.contact.global_normal1(&body1.rotation); let r1 = body1.rotation.rotate(constraint.r1); let r2 = body2.rotation.rotate(constraint.r2);