diff --git a/benchmark/amd7950x_sse2/convex_pile.csv b/benchmark/amd7950x_sse2/convex_pile.csv index 4bf71ef..ed6f055 100644 --- a/benchmark/amd7950x_sse2/convex_pile.csv +++ b/benchmark/amd7950x_sse2/convex_pile.csv @@ -1,9 +1,9 @@ threads,ms -1,14441.9 -2,7238.43 -3,4870.13 -4,3697.34 -5,2998.67 -6,2533.13 -7,2209.12 -8,1983.89 +1,13564.7 +2,7058.44 +3,4749.26 +4,3596.92 +5,2921.35 +6,2478.9 +7,2163.02 +8,1919.1 diff --git a/benchmark/amd7950x_sse2/joint_grid.csv b/benchmark/amd7950x_sse2/joint_grid.csv index c09a287..a173bdc 100644 --- a/benchmark/amd7950x_sse2/joint_grid.csv +++ b/benchmark/amd7950x_sse2/joint_grid.csv @@ -1,9 +1,9 @@ threads,ms -1,1323.84 -2,691.063 -3,463.698 -4,353.723 -5,284.09 -6,240.864 -7,209.11 -8,185.953 +1,1341.05 +2,697.405 +3,467.98 +4,355.631 +5,285.912 +6,240.761 +7,210.048 +8,187.1 diff --git a/benchmark/amd7950x_sse2/junkyard.csv b/benchmark/amd7950x_sse2/junkyard.csv index 50e839b..89de3af 100644 --- a/benchmark/amd7950x_sse2/junkyard.csv +++ b/benchmark/amd7950x_sse2/junkyard.csv @@ -1,9 +1,9 @@ threads,ms -1,16467.5 -2,8687.63 -3,5966.06 -4,4569.55 -5,3727.89 -6,3175.51 -7,2801.72 -8,2512.32 +1,14783.1 +2,7859.85 +3,5367.48 +4,4115.03 +5,3368.76 +6,2874.23 +7,2535.89 +8,2277.2 diff --git a/benchmark/amd7950x_sse2/large_pyramid.csv b/benchmark/amd7950x_sse2/large_pyramid.csv index e9b1cc8..c1a9fc3 100644 --- a/benchmark/amd7950x_sse2/large_pyramid.csv +++ b/benchmark/amd7950x_sse2/large_pyramid.csv @@ -1,9 +1,9 @@ threads,ms -1,1993.6 -2,1049.01 -3,712.917 -4,544.335 -5,443.056 -6,384.22 -7,331.626 -8,297.982 +1,1852.7 +2,978.38 +3,665.353 +4,505.116 +5,411.56 +6,356.776 +7,307.866 +8,276.957 diff --git a/benchmark/amd7950x_sse2/large_world.csv b/benchmark/amd7950x_sse2/large_world.csv index 4e0a4c5..9ff7d9a 100644 --- a/benchmark/amd7950x_sse2/large_world.csv +++ b/benchmark/amd7950x_sse2/large_world.csv @@ -1,9 +1,9 @@ threads,ms -1,7.6329 -2,9.7115 -3,10.7648 -4,11.2398 -5,12.029 -6,12.1204 -7,12.2289 -8,12.5125 +1,7.1447 +2,9.0736 +3,10.1672 +4,10.5088 +5,11.0298 +6,11.2858 +7,11.378 +8,11.4947 diff --git a/benchmark/amd7950x_sse2/many_pyramids.csv b/benchmark/amd7950x_sse2/many_pyramids.csv index aa3aee5..dd14013 100644 --- a/benchmark/amd7950x_sse2/many_pyramids.csv +++ b/benchmark/amd7950x_sse2/many_pyramids.csv @@ -1,9 +1,9 @@ threads,ms -1,2075.59 -2,1084.54 -3,722.057 -4,549.644 -5,437.772 -6,372.242 -7,319.13 -8,283.131 +1,1947.41 +2,1002.95 +3,663.846 +4,499.942 +5,405.97 +6,336.977 +7,291.483 +8,255.782 diff --git a/benchmark/amd7950x_sse2/rain.csv b/benchmark/amd7950x_sse2/rain.csv index d41aa27..908ea42 100644 --- a/benchmark/amd7950x_sse2/rain.csv +++ b/benchmark/amd7950x_sse2/rain.csv @@ -1,9 +1,9 @@ threads,ms -1,2102.47 -2,1147.85 -3,803.547 -4,630.96 -5,524.168 -6,452.1 -7,404.499 -8,370.325 +1,2061.72 +2,1139.07 +3,794.05 +4,622.555 +5,516.441 +6,446.451 +7,399.084 +8,364.345 diff --git a/benchmark/amd7950x_sse2/sleep.csv b/benchmark/amd7950x_sse2/sleep.csv index ba99522..da3e5a9 100644 --- a/benchmark/amd7950x_sse2/sleep.csv +++ b/benchmark/amd7950x_sse2/sleep.csv @@ -1,9 +1,9 @@ threads,ms -1,1144.18 -2,613.407 -3,423.032 -4,329.512 -5,272.901 -6,239.65 -7,216.5 -8,200.217 +1,1051.05 +2,568.185 +3,390.055 +4,304.593 +5,251.978 +6,221.25 +7,200.171 +8,185.485 diff --git a/benchmark/amd7950x_sse2/spinner.csv b/benchmark/amd7950x_sse2/spinner.csv index 5dad23c..04d05dd 100644 --- a/benchmark/amd7950x_sse2/spinner.csv +++ b/benchmark/amd7950x_sse2/spinner.csv @@ -1,9 +1,9 @@ threads,ms -1,3173.87 -2,1841.25 -3,1325.07 -4,1061.04 -5,902.193 -6,803.715 -7,734.266 -8,689.781 +1,3152.3 +2,1832.83 +3,1315.75 +4,1058.49 +5,903.367 +6,801.254 +7,738.93 +8,695.326 diff --git a/benchmark/amd7950x_sse2/trees100.csv b/benchmark/amd7950x_sse2/trees100.csv index 27248b4..c276a5f 100644 --- a/benchmark/amd7950x_sse2/trees100.csv +++ b/benchmark/amd7950x_sse2/trees100.csv @@ -1,9 +1,9 @@ threads,ms -1,170.276 -2,112.037 -3,88.2708 -4,78.4451 -5,74.3577 -6,71.868 -7,70.6687 -8,69.1284 +1,170.933 +2,112.916 +3,89.6041 +4,79.6461 +5,75.7754 +6,73.2911 +7,71.7742 +8,70.7292 diff --git a/benchmark/amd7950x_sse2/trees25.csv b/benchmark/amd7950x_sse2/trees25.csv index 99dfa5b..00c842b 100644 --- a/benchmark/amd7950x_sse2/trees25.csv +++ b/benchmark/amd7950x_sse2/trees25.csv @@ -1,9 +1,9 @@ threads,ms -1,673.356 -2,395.07 -3,291.336 -4,249.378 -5,225.709 -6,211.1 -7,201.452 -8,194.379 +1,680.981 +2,400.129 +3,296.389 +4,252.676 +5,229.879 +6,214.117 +7,204.083 +8,196.839 diff --git a/benchmark/amd7950x_sse2/trees50.csv b/benchmark/amd7950x_sse2/trees50.csv index 68aadc7..aafde5d 100644 --- a/benchmark/amd7950x_sse2/trees50.csv +++ b/benchmark/amd7950x_sse2/trees50.csv @@ -1,9 +1,9 @@ threads,ms -1,285.24 -2,174.635 -3,132.161 -4,116.509 -5,107.948 -6,102.503 -7,98.9163 -8,96.5196 +1,295.508 +2,183.122 +3,139.634 +4,121.466 +5,113.262 +6,106.697 +7,103.854 +8,101.959 diff --git a/benchmark/amd7950x_sse2/washer.csv b/benchmark/amd7950x_sse2/washer.csv index c03dccd..bf0a124 100644 --- a/benchmark/amd7950x_sse2/washer.csv +++ b/benchmark/amd7950x_sse2/washer.csv @@ -1,9 +1,9 @@ threads,ms -1,21430.4 -2,11407.7 -3,7908.09 -4,6102.55 -5,5012.68 -6,4272.59 -7,3783.33 -8,3415.5 +1,19931.1 +2,10626.7 +3,7300.85 +4,5627.16 +5,4603.18 +6,3931.89 +7,3454.58 +8,3273.54 diff --git a/benchmark/main.c b/benchmark/main.c index a630c59..fba4067 100644 --- a/benchmark/main.c +++ b/benchmark/main.c @@ -188,7 +188,6 @@ int main( int argc, char** argv ) .solveImpulses = FLT_MAX, .integratePositions = FLT_MAX, .relaxImpulses = FLT_MAX, - .applyRestitution = FLT_MAX, .storeImpulses = FLT_MAX, .splitIslands = FLT_MAX, .transforms = FLT_MAX, diff --git a/data/hulls/axe.txt b/data/hulls/axe.txt new file mode 100644 index 0000000..fe3e7ce --- /dev/null +++ b/data/hulls/axe.txt @@ -0,0 +1 @@ +-0.420565814, 6.27311516, 0.624139011, -0.423157126, 6.43531513, 0.624940872, -0.400363564, 6.24441338, 0.702014863, -0.416245729, 6.17266893, 0.622810602, -0.433529437, 6.29105663, 0.573231816, -0.424020022, 6.62839079, 0.625214338, -0.403299004, 6.41980267, 0.702534437, -0.435781032, 6.44498682, 0.573748231, -0.39546892, 6.13546467, 0.701158941, -0.376558542, 6.21027279, 0.793354034, -0.429776281, 6.19607162, 0.572380662, -0.404258847, 6.12016535, 0.608667254, -0.440633416, 6.30049419, 0.527119458, -0.42315498, 6.82146406, 0.624956846, -0.404275864, 6.62841082, 0.702715218, -0.436531276, 6.62838888, 0.573927343, -0.379765004, 6.40131187, 0.793307424, -0.442748815, 6.45009136, 0.527064621, -0.380489528, 6.07763004, 0.693229854, -0.371210814, 6.09141397, 0.793445706, -0.352786303, 6.17601395, 0.884651542, -0.437107563, 6.20831585, 0.527223408, -0.421900928, 6.14674568, 0.555683434, -0.378666759, 6.10179329, 0.56942004, -0.443256766, 6.30368519, 0.463627696, -0.420562118, 6.98365641, 0.624166369, -0.403294712, 6.83701468, 0.702553868, -0.435780197, 6.81178904, 0.573766887, -0.380831331, 6.62843847, 0.793302, -0.443453908, 6.62839603, 0.52705586, -0.356113642, 6.3827033, 0.88403821, -0.445638776, 6.45190096, 0.462998569, -0.347299904, 6.055583, 0.671490133, -0.354736507, 6.02676058, 0.792831719, -0.347236216, 6.047472, 0.885690987, -0.332682669, 6.14695787, 0.962401867, -0.439286768, 6.21204424, 0.464693397, -0.430589259, 6.16906166, 0.521244645, -0.407282561, 6.1297946, 0.507628977, -0.322530925, 6.10475826, 0.476746678, -0.443575919, 6.30373383, 0.360556096, -0.416241884, 7.08408976, 0.622840762, -0.400356293, 7.01239157, 0.702047944, -0.433527976, 6.96571302, 0.57326442, -0.379758388, 6.85555887, 0.79333353, -0.442748666, 6.80669832, 0.527089894, -0.357219428, 6.62846327, 0.883846343, -0.44643271, 6.62840271, 0.462801337, -0.335903198, 6.36683941, 0.96150583, -0.446347833, 6.4520278, 0.359647483, -0.318150342, 5.99833679, 0.79076165, -0.277453154, 6.0510149, 0.648446918, -0.331143022, 5.97517681, 0.890933752, -0.327310175, 6.01059294, 0.963914692, -0.319883317, 6.12842607, 1.01310015, -0.438956082, 6.21159744, 0.362091303, -0.428903759, 6.15980482, 0.476069212, -0.391884565, 6.13054276, 0.404339224, -0.165798873, 6.09137678, 0.458747327, -0.302039534, 6.1235323, 0.238368377, -0.443767488, 6.30374575, 0.195703521, -0.404255658, 7.13657808, 0.60869509, -0.395461231, 7.12131977, 0.701194584, -0.429774731, 7.06068754, 0.572417438, -0.376547217, 7.04658031, 0.793398738, -0.440633088, 6.95628834, 0.527163327, -0.356105059, 6.8742156, 0.884071171, -0.445638776, 6.80490112, 0.463031411, -0.336973041, 6.62847519, 0.96122092, -0.447271824, 6.62839937, 0.359359771, -0.322692037, 6.35658121, 1.01248837, -0.44677335, 6.45208454, 0.19510901, -0.296186596, 5.93722773, 0.904156804, -0.249230281, 5.98962021, 0.788263321, -0.152502596, 6.0456171, 0.636606455, -0.313261539, 5.93217945, 0.972767949, -0.315197587, 5.98773193, 1.01413727, -0.310311168, 6.11343765, 1.04989588, -0.438757658, 6.21131516, 0.196717665, -0.424416304, 6.1593051, 0.373577535, -0.381670475, 6.13431787, 0.22193633, -7.34580681e-08, 6.08772421, 0.454057723, -0.165297493, 6.11621714, 0.239674717, -0.296461612, 6.12875175, -1.09191251e-05, -0.443831295, 6.30372858, -3.83076112e-05, -0.38048315, 7.17913008, 0.693260372, -0.421899676, 7.11000204, 0.55572027, -0.378664732, 7.15493488, 0.569444001, -0.371198803, 7.16540956, 0.793493092, -0.437107146, 7.04845524, 0.52727282, -0.35277158, 7.0808835, 0.884707689, -0.443256766, 6.95310783, 0.46368432, -0.335893631, 6.89010334, 0.961542308, -0.446347833, 6.80476713, 0.359687001, -0.323624909, 6.62846375, 1.01229727, -0.447775304, 6.62837696, 0.194927484, -0.312764168, 6.34827375, 1.04985118, -0.44691509, 6.45208025, -2.25071708e-05, -0.231420755, 5.92051983, 0.917008758, -0.284012824, 5.88615227, 0.995684922, -0.135754183, 5.98408937, 0.78636533, -1.98313501e-07, 6.04270792, 0.633488476, -0.304644406, 5.90706873, 1.02356648, -0.306217968, 5.96934509, 1.04998815, -0.299889147, 6.09501314, 1.08593857, -0.438691556, 6.21120358, -4.06680119e-05, -0.42147997, 6.15982771, 0.203634292, -0.377859384, 6.13695669, -2.24082669e-05, 0.16579853, 6.09136963, 0.458753914, -2.24478196e-08, 6.10835075, 0.279498875, -0.138670072, 6.11576748, 0.239171118, -0.165076017, 6.11664724, 0.225328684, -0.302039385, 6.12352228, -0.238388941, -0.279497296, 6.12807941, -1.00816333e-05, -0.443767488, 6.30368996, -0.195777908, -0.354731649, 7.23000145, 0.792840838, -0.347295642, 7.20115232, 0.671512187, -0.430588692, 7.08769798, 0.521291852, -0.407281756, 7.1269455, 0.507667124, -0.32253018, 7.15195227, 0.476763695, -0.34722051, 7.20938873, 0.88575083, -0.439286768, 7.044734, 0.464755625, -0.332666337, 7.10996151, 0.962464392, -0.443575948, 6.95305204, 0.360623807, -0.322683156, 6.90034008, 1.0125221, -0.44677332, 6.80466747, 0.195152655, -0.313579917, 6.62847233, 1.0498426, -0.447943091, 6.62834549, 0, -0.302403748, 6.33826208, 1.08645916, -0.44677332, 6.45202351, -0.19515267, -0.12589927, 5.91194773, 0.921137631, -0.225511238, 5.83923531, 1.04820025, -0.284233212, 5.85900593, 1.04935467, -3.23225322e-07, 5.98211765, 0.785567641, 0.152501747, 6.04560709, 0.636612177, -0.301061481, 5.91173029, 1.04980791, -0.295690835, 5.94588757, 1.08509719, -0.285476804, 6.06730127, 1.13435495, -0.438757658, 6.2112689, -0.196796864, -0.420399696, 6.16033602, -3.39308936e-05, -0.381670326, 6.13429689, -0.221979678, 0.32253018, 6.10473871, 0.476763695, 0.165297419, 6.11621332, 0.239682525, 0.086369887, 6.11119843, 0.265819222, -8.3297147e-09, 1.3441813, 0.279498726, -0.0863699168, 6.11149788, 0.265819222, -0.164285332, 6.11605644, 0.226119369, -0.182904437, 6.11789989, 0.207500264, -0.32253021, 6.10473871, -0.476763725, -0.165297419, 6.11621332, -0.23968251, -0.279498905, 6.12807894, 4.02538269e-09, -0.272587925, 6.12646961, -0.0436344296, -0.443575919, 6.30363894, -0.360623807, -0.318164319, 7.25831223, 0.790665567, -0.331150413, 7.28153563, 0.890861154, -0.277451128, 7.20569944, 0.648459554, -0.42890352, 7.09695435, 0.476123691, -0.391884238, 7.12618637, 0.404379696, -0.16579853, 7.16532135, 0.458753914, -0.302039385, 7.1331687, 0.238388941, -0.327292442, 7.2462883, 0.963982165, -0.438956082, 7.04517269, 0.36216414, -0.319868058, 7.12847614, 1.01315844, -0.443767488, 6.95300102, 0.195777923, -0.312761307, 6.90865183, 1.04985857, -0.44691515, 6.80461073, 2.25071708e-05, -0.303243041, 6.62854338, 1.08662438, -0.447775304, 6.62831402, -0.194927514, -0.288269371, 6.32334375, 1.13516843, -0.446347833, 6.45192385, -0.359687001, -3.43185093e-07, 5.90969229, 0.921022594, -0.11619395, 5.8294673, 1.04748738, -0.181410655, 5.74001026, 1.17642462, -0.257047713, 5.81331062, 1.10132825, -0.29030636, 5.87313557, 1.07566071, 0.135752946, 5.98407793, 0.786370695, 0.277451128, 6.05099154, 0.648459554, -0.280808836, 5.91007566, 1.1330471, -0.263933629, 6.02445316, 1.20827043, -0.438956082, 6.21151829, -0.36216414, -0.42147997, 6.15979338, -0.203700453, -0.391884238, 6.13050461, -0.404379696, 0.302039385, 6.12352228, 0.238388941, 0.378664732, 6.1017561, 0.569444001, 0.391884238, 6.13050461, 0.404379696, 0.165075839, 6.11664391, 0.225328803, 0.138657346, 6.11576414, 0.239177421, 0.086369887, 1.3441813, 0.265819073, -0.0863699168, 1.3441813, 0.265819073, -8.61839577e-09, 1.28562617, 0.254942268, -0.164285332, 1.3441813, 0.226119503, -0.226119384, 6.12120199, 0.164285332, -0.378664732, 6.1017561, -0.569443941, -0.16579853, 6.09136963, -0.458753914, -0.277451128, 6.05099154, -0.648459554, -0.165075839, 6.11664391, -0.225328937, -0.138657674, 6.11576414, -0.239177421, -0.279498905, 1.3441813, 1.33275464e-07, -0.272592217, 6.12647057, 0.0436073244, -0.265819341, 6.12511873, -0.0863699093, -0.443256766, 6.30358315, -0.46368432, -0.24925819, 7.26689911, 0.788037598, -0.296260297, 7.31912899, 0.903698802, -0.313283414, 7.32448053, 0.972614944, -0.152501747, 7.21108389, 0.636612177, -0.424416244, 7.09744596, 0.373639077, -0.381670326, 7.12239408, 0.221979663, 8.34697857e-08, 7.16896677, 0.454057723, -0.165297419, 7.14047766, 0.23968251, -0.296461612, 7.12793922, 1.09203329e-05, -0.315180749, 7.26913834, 1.01420152, -0.438757688, 7.04542208, 0.196796849, -0.310311139, 7.14343166, 1.04988754, -0.443831295, 6.9529624, 3.83100269e-05, -0.30241558, 6.91876459, 1.0863992, -0.44677332, 6.80460644, -0.195108995, -0.289206415, 6.62872124, 1.1354053, -0.447271824, 6.62829161, -0.359359771, -0.267019719, 6.3003149, 1.20883405, -0.445638776, 6.45178986, -0.463031411, 0.125897869, 5.91193533, 0.921142876, -3.18628736e-07, 5.82673359, 1.04730546, -0.0998942629, 5.73692417, 1.17197931, -0.15263997, 5.64101887, 1.2949971, -0.226171315, 5.73974991, 1.18931413, -0.272065639, 5.83197594, 1.12433982, 0.249227121, 5.98959064, 0.788276851, 0.347295642, 6.05553865, 0.671512187, -0.258764625, 5.85462618, 1.20741999, -0.238956943, 5.97377586, 1.29493284, -0.439286739, 6.21195698, -0.464755625, -0.424416274, 6.15924549, -0.373639077, -0.407281756, 6.12974548, -0.507667124, 0.381670326, 6.13429689, 0.221979663, 0.296461612, 6.12875175, 1.09203329e-05, 0.407281756, 6.12974548, 0.507667124, 0.404255658, 6.1201129, 0.60869509, 0.424416244, 6.15924501, 0.373639077, 0.164285302, 6.11604118, 0.226119339, 0.182916105, 6.11789751, 0.207488537, 0.164285302, 1.3441813, 0.22611919, 0.0787815228, 1.28562617, 0.242464468, -0.0787815526, 1.28562617, 0.242464468, -9.31532984e-09, 1.26137209, 0.195657715, -0.149851412, 1.28562617, 0.206252888, -0.226119384, 1.3441813, 0.164285466, -0.265819341, 6.12512016, 0.0863699168, -0.404255658, 6.1201129, -0.60869509, -0.347295672, 6.05553865, -0.671512067, 8.3935447e-08, 6.08772421, -0.454057723, -0.152501762, 6.04560709, -0.636612177, -0.249227121, 5.98959112, -0.788276851, -0.164285362, 6.11605835, -0.226119414, -0.182915941, 6.11789751, -0.20748882, -0.0863699391, 6.11133337, -0.265819401, -0.265819341, 1.3441813, 0.0863700435, -0.254942447, 1.28562617, 1.33275464e-07, -0.265819341, 1.3441813, -0.0863697827, -0.226119399, 6.12119913, -0.164285347, -0.440633088, 6.30040264, -0.527163327, -0.135774553, 7.2723217, 0.786026418, -0.231541172, 7.33540201, 0.91606617, -0.284150362, 7.36993647, 0.994881928, -0.304674238, 7.34958363, 1.02339602, 1.98313501e-07, 7.21398306, 0.633488476, -0.42147994, 7.0968976, 0.203700453, -0.377859354, 7.11973381, 2.2412647e-05, 0.165798873, 7.1653142, 0.458747327, 1.51007473e-08, 7.14834023, 0.279498905, -0.165075839, 7.14004707, 0.225328848, -0.138657719, 7.14092684, 0.239177421, -0.302039534, 7.13315868, -0.238368362, -0.279497296, 7.12861156, 1.00826892e-05, -0.306232214, 7.28743124, 1.0499177, -0.438691556, 7.0454874, 4.06390209e-05, -0.299929738, 7.16183329, 1.08574927, -0.443767488, 6.95294523, -0.195703536, -0.288308054, 6.93395376, 1.13498354, -0.446347833, 6.80466318, -0.359647483, -0.268062264, 6.62904978, 1.20894837, -0.44643271, 6.62828827, -0.462801337, -0.242273688, 6.27264404, 1.29497945, -0.442748666, 6.44999266, -0.527089894, 0.231416866, 5.9204855, 0.91702354, 0.116192684, 5.82945538, 1.04749227, -2.22877134e-07, 5.73454285, 1.17186856, -0.0828618631, 5.63490438, 1.2949146, -0.196217477, 5.65075827, 1.29506195, -0.131680921, 5.53568459, 1.41265607, -0.247511044, 5.76750231, 1.20239663, 0.318143785, 5.99827671, 0.790788293, 0.38048315, 6.0775609, 0.693260372, -0.233388737, 5.78941584, 1.29499769, -0.214244157, 5.922575, 1.3815906, -0.437107146, 6.20823574, -0.52727282, -0.42890352, 6.15973663, -0.476123691, -0.421899676, 6.14668894, -0.55572027, 0.377859354, 6.13695717, 2.2412647e-05, 0.42147994, 6.15979338, 0.203700453, 0.302039534, 6.1235323, -0.238368362, 0.279497236, 6.12807941, 1.00814714e-05, 0.421899676, 6.14668894, 0.55572027, 0.42890352, 6.15973663, 0.476123691, 0.416241884, 6.17260122, 0.622840762, 0.438956082, 6.21151829, 0.36216414, 0.226119339, 6.12119913, 0.164285302, 0.226119339, 1.3441813, 0.164285153, 0.149851367, 1.28562617, 0.206252575, 0.0604615882, 1.26137209, 0.186081529, -0.0604616255, 1.26137209, 0.186081529, 0, 1.26137209, 0, -0.206252769, 1.28562617, 0.149851531, -0.115004838, 1.26137209, 0.158290386, -0.38048315, 6.0775609, -0.693260372, -0.416241884, 6.17260122, -0.622840762, -0.318143785, 5.99827671, -0.790788293, 0.165798873, 6.09137678, -0.458747327, 1.98488124e-07, 6.04270792, -0.633488476, 1.93174348e-08, 6.10835075, -0.279499024, -0.135752946, 5.98407793, -0.786370695, -0.231416866, 5.9204855, -0.91702354, -0.164285362, 1.3441813, -0.22611928, -0.0863699391, 1.3441813, -0.265819341, -0.242464721, 1.28562617, 0.0787816867, -0.195657909, 1.26137209, 1.33275464e-07, -0.242464721, 1.28562617, -0.0787814185, -0.226119399, 1.3441813, -0.164285213, -0.433527976, 6.29097795, -0.57326442, -0.125984326, 7.34358931, 0.919766307, -1.56361784e-06, 7.27423811, 0.785173655, -0.225884482, 7.41538906, 1.04568243, -0.284401506, 7.39706802, 1.04848611, -0.3011235, 7.34484911, 1.04951715, 0.152502596, 7.21107388, 0.636606455, -0.420399666, 7.09635496, 3.39203725e-05, -0.381670475, 7.1223731, -0.22193639, 0.322530895, 7.15193272, 0.476746678, 0.165297523, 7.14047384, 0.239674702, -0.0863699168, 7.14467192, 0.265819252, -8.3297147e-09, 7.51029158, 0.279498905, 0.086369887, 7.14462614, 0.265819252, -0.164285332, 7.14005041, 0.226119354, -0.18291603, 7.13879347, 0.207488656, -0.322530925, 7.15193272, -0.476746678, -0.165297493, 7.14047384, -0.239674717, -0.279498905, 7.12861204, -2.3006745e-08, -0.272587925, 7.13022137, 0.043634437, -0.295793891, 7.31065845, 1.08463991, -0.438757658, 7.04537582, -0.196717694, -0.285594136, 7.18956089, 1.13382041, -0.443575919, 6.95295715, -0.360556066, -0.267100781, 6.95749617, 1.20845032, -0.445638746, 6.80479002, -0.462998539, -0.243394509, 6.62857056, 1.29488754, -0.443453968, 6.62829494, -0.52705586, -0.21765025, 6.24379826, 1.3811276, -0.435780197, 6.44490194, -0.573766887, 0.296178102, 5.93715286, 0.904189169, 0.225507602, 5.83920288, 1.04821408, 0.0998933986, 5.73691654, 1.17198241, 1.46375078e-05, 5.6327486, 1.29487169, -0.0709583387, 5.52399302, 1.41659379, -0.220748916, 5.69119883, 1.29507399, -0.171799481, 5.55877066, 1.40032125, -0.118125923, 5.41381836, 1.53769684, 0.354726493, 6.02666378, 0.792871535, 0.395461231, 6.13537121, 0.701194584, -0.20851171, 5.72432184, 1.38256216, -0.193492502, 5.87815857, 1.45549119, -0.429774731, 6.19600344, -0.572417438, -0.430588663, 6.168993, -0.521291852, 0.381670505, 6.13431787, -0.22193639, 0.420399666, 6.16033602, 3.39348662e-05, 0.438757658, 6.2112689, 0.196796849, 0.322530955, 6.10475826, -0.476746678, 0.165297523, 6.11621714, -0.239674702, 0.279498845, 6.12807894, -1.14942331e-08, 0.272587776, 6.12646961, 0.043634709, 0.430588692, 6.168993, 0.521291852, 0.429774731, 6.19600344, 0.572417438, 0.439286739, 6.21195698, 0.464755625, 0.420562148, 6.27303457, 0.624166429, 0.443575919, 6.30363894, 0.360623807, 0.265819192, 6.12511873, 0.0863699019, 0.265819192, 1.3441813, 0.0863697454, 0.206252739, 1.28562617, 0.149851218, 0.115004793, 1.26137209, 0.158290371, 0.186081797, 1.26137209, -0.0604617298, 0.158290669, 1.26137209, -0.115004897, 0.115004912, 1.26137209, -0.15829064, 0.0604616627, 1.26137209, -0.186081842, 1.41108716e-08, 1.26137209, -0.195657983, -0.0604616292, 1.26137209, -0.186081782, -0.115004838, 1.26137209, -0.158290505, -0.15829058, 1.26137209, -0.115004703, -0.186081752, 1.26137209, -0.0604614913, -0.186081752, 1.26137209, 0.0604617558, -0.15829058, 1.26137209, 0.115004949, 0.15829052, 1.26137209, 0.115004636, 0.186081663, 1.26137209, 0.0604614429, 0.195657864, 1.26137209, -1.54655382e-07, -0.354726493, 6.02666378, -0.792871535, -0.395461231, 6.13537121, -0.701194584, -0.420562118, 6.27303457, -0.624166369, -0.296178102, 5.93715286, -0.904189169, 0.152502537, 6.0456171, -0.636606455, 3.03434717e-07, 5.98211765, -0.785567641, 0, 1.3441813, -0.279498965, 0.0863699391, 6.11137819, -0.265819401, -0.125897869, 5.91193533, -0.921142876, -0.225507602, 5.83920288, -1.04821408, -0.149851456, 1.28562617, -0.206252679, -0.0787815675, 1.28562617, -0.242464751, -0.206252798, 1.28562617, -0.149851277, -7.17531657e-06, 7.34565639, 0.919446766, -0.116455019, 7.42408895, 1.04377484, 0.135772571, 7.2723074, 0.786019921, -0.257534236, 7.44143343, 1.09880614, -0.182427585, 7.51092958, 1.16946995, -0.290465444, 7.38311243, 1.07493198, 0.277453154, 7.20567608, 0.648446918, -0.42147997, 7.09686327, -0.203634292, -0.391884565, 7.12614822, -0.404339224, 0.378666759, 7.15489769, 0.56942004, 0.302039534, 7.13315868, 0.238368377, 0.391884565, 7.12614822, 0.404339284, 0.138669819, 7.1409235, 0.239171103, 0.165076047, 7.14004374, 0.225328624, -0.0863699168, 7.51029158, 0.265819252, -8.61839577e-09, 7.56884527, 0.254942477, 0.086369887, 7.51029158, 0.265819252, -0.164285332, 7.51029158, 0.226119339, -0.226119384, 7.13549185, 0.164285317, -0.277453154, 7.20567608, -0.648446918, -0.165798873, 7.1653142, -0.458747327, -0.378666759, 7.15489769, -0.56942004, -0.138670087, 7.1409235, -0.239171118, -0.165076017, 7.14004374, -0.225328773, -0.279498905, 7.51029158, -3.33188659e-08, -0.272592247, 7.13022041, -0.0436073169, -0.265819341, 7.13157225, 0.086369887, -0.281084716, 7.34606075, 1.13183379, -0.438956082, 7.04509354, -0.362091303, -0.264174789, 7.23249292, 1.2071768, -0.443256766, 6.95300579, -0.463627696, -0.242374972, 6.98410416, 1.2944082, -0.442748815, 6.80659962, -0.527064621, -0.218787104, 6.62632561, 1.38085747, -0.436531276, 6.6283021, -0.573927343, -0.196768254, 6.21724606, 1.45480144, -0.42315498, 6.43522692, -0.624956846, 0.284003228, 5.88606691, 0.995721519, 0.331129879, 5.97505283, 0.890983939, 0.181408972, 5.73999453, 1.17643082, 0.257042885, 5.81326675, 1.10134661, 0.0828872696, 5.63488531, 1.294927, 5.89085903e-05, 5.5209794, 1.41647434, -0.0605682731, 5.40043592, 1.53679967, -0.195885792, 5.61455011, 1.38761473, -0.155908078, 5.48248768, 1.48746622, -0.0877592564, 5.26322508, 1.66027737, -0.132033154, 5.3718133, 1.58911765, 0.371198803, 6.09128141, 0.793493092, 0.400356293, 6.24429941, 0.702047944, -0.187964052, 5.66922092, 1.45689499, -0.180399314, 5.84783316, 1.50388217, 0.42147997, 6.15982771, -0.203634292, 0.391884565, 6.13054276, -0.404339284, 0.438691556, 6.21120358, 4.06392865e-05, 0.443767488, 6.30368996, 0.195777923, 0.277453154, 6.0510149, -0.648446918, 0.378666759, 6.10179329, -0.56942004, 0.138670206, 6.11576748, -0.239171103, 0.165076047, 6.11664724, -0.225328892, 0.279498845, 1.3441813, -1.66594333e-07, 0.272592187, 6.12647057, -0.0436073951, 0.437107146, 6.20823574, 0.52727282, 0.433527976, 6.29097795, 0.57326442, 0.443256766, 6.30358315, 0.46368432, 0.42315498, 6.43522692, 0.624956846, 0.446347833, 6.45192385, 0.359687001, 0.242464617, 1.28562617, 0.0787813812, 0.242464766, 1.28562617, -0.0787816197, 0.206252888, 1.28562617, -0.149851471, 0.149851486, 1.28562617, -0.206252873, 0.0787815824, 1.28562617, -0.242464766, 4.13297885e-09, 1.28562617, -0.254942507, 0.254942447, 1.28562617, -9.24175012e-08, -0.371198803, 6.09128141, -0.793493092, -0.331129879, 5.97505283, -0.890983939, -0.400356293, 6.24429941, -0.702047944, -0.284003228, 5.88606691, -0.995721519, 0.135754183, 5.98408937, -0.78636533, 3.43185093e-07, 5.90969229, -0.921022594, 0.0863699391, 1.3441813, -0.265819341, -0.116192684, 5.82945538, -1.04749227, -0.181408972, 5.73999453, -1.17643082, -0.257042885, 5.81326675, -1.10134661, 0.125972882, 7.34356785, 0.919755757, -1.90703722e-05, 7.42648506, 1.04322731, -0.100510523, 7.51302147, 1.16399562, 0.249259219, 7.26686621, 0.788022101, -0.272493392, 7.42337704, 1.12237763, -0.22724396, 7.51252317, 1.18369079, -0.154114068, 7.59775734, 1.27728057, 0.347299904, 7.20110798, 0.671490133, -0.424416333, 7.09738588, -0.373577565, -0.407282531, 7.12689638, -0.507628977, 0.407282561, 7.12689638, 0.507628977, 0.404258847, 7.13652563, 0.608667254, 0.381670445, 7.1223731, 0.22193633, 0.296461612, 7.12793922, -1.09215407e-05, 0.424416304, 7.09738588, 0.373577535, 0.164285302, 7.13977194, 0.226119369, 0.182904527, 7.13879108, 0.207500145, -0.0787815526, 7.56884527, 0.242464662, -9.31532984e-09, 7.59310007, 0.195657909, 0.0787815228, 7.56884527, 0.242464662, 0.164285302, 7.51029158, 0.226119384, -0.226119384, 7.51029158, 0.164285302, -0.149851412, 7.56884527, 0.206252724, -0.249259219, 7.26686621, -0.788022101, -0.152502537, 7.21107388, -0.636606455, -0.347299904, 7.20110798, -0.671490133, -7.36908987e-08, 7.16896677, -0.454057723, -0.404258847, 7.13652563, -0.608667254, -0.164285362, 7.14004517, -0.226119429, -0.0863699391, 7.14450407, -0.265819401, -0.182904348, 7.13879108, -0.207500428, -0.265819341, 7.51029158, -0.0863699466, -0.254942447, 7.56884527, -3.33188659e-08, -0.265819341, 7.51029158, 0.0863698795, -0.265819341, 7.13157082, -0.0863699391, -0.259323657, 7.40087843, 1.20496547, -0.439286768, 7.04464674, -0.464693397, -0.239270881, 7.28179407, 1.29329097, -0.440633416, 6.95619678, -0.527119458, -0.217711896, 7.00849152, 1.38046527, -0.435781032, 6.81170416, -0.573748231, -0.197824031, 6.62135839, 1.45449197, -0.424020022, 6.62830019, -0.625214338, -0.183246627, 6.19645405, 1.50352371, -0.403294712, 6.4196763, -0.702553868, 0.313246608, 5.93203878, 0.972824752, 0.284223408, 5.85891914, 1.04939187, 0.34722051, 6.04730225, 0.88575083, 0.152657047, 5.64098644, 1.295017, 0.226168826, 5.73972654, 1.18932343, 0.27206108, 5.83192062, 1.12435734, 0.0710614175, 5.5239296, 1.41663849, 0.000152599809, 5.39668751, 1.53658283, -0.048076205, 5.26048088, 1.65531445, -0.176622659, 5.55060577, 1.46541834, -0.151534066, 5.44061136, 1.53887141, -0.0674578622, 5.12431622, 1.76779604, -0.109993391, 5.2592659, 1.67399967, -0.139934972, 5.39250278, 1.61204028, 0.376547217, 6.21011066, 0.793398738, 0.403294742, 6.4196763, 0.702553868, -0.175576344, 5.63398933, 1.50477803, -0.171244964, 5.81763649, 1.53953564, 0.424416333, 6.1593051, -0.373577565, 0.438757658, 6.21131516, -0.196717694, 0.407282531, 6.1297946, -0.507628977, 0.443831325, 6.30372858, 3.83076112e-05, 0.44677338, 6.45202351, 0.195152655, 0.249230281, 5.98962021, -0.788263321, 0.347299904, 6.055583, -0.671490133, 0.404258847, 6.12016535, -0.608667254, 0.164285421, 6.11633635, -0.226119518, 0.182904199, 6.11789989, -0.207500741, 0.265819371, 1.3441813, -0.0863699466, 0.265819371, 6.12512016, -0.0863699988, 0.440633059, 6.30040264, 0.527163327, 0.435780168, 6.44490194, 0.573766887, 0.445638776, 6.45178986, 0.463031441, 0.424020022, 6.62830019, 0.625214219, 0.447271824, 6.62829161, 0.359359801, 0.226119488, 1.3441813, -0.164285421, 0.164285421, 1.3441813, -0.226119474, -0.34722051, 6.04730225, -0.88575083, -0.376547217, 6.21011066, -0.793398738, -0.313246608, 5.93203878, -0.972824752, -0.284223408, 5.85891914, -1.04939187, 0.12589927, 5.91194773, -0.921137631, 3.18628736e-07, 5.82673359, -1.04730546, -0.0998933986, 5.73691654, -1.17198241, -0.152652979, 5.64097738, -1.29502368, -0.226168826, 5.73972654, -1.18932343, -0.27206108, 5.83192062, -1.12435734, 0.231536552, 7.33535433, 0.91604346, 0.116421811, 7.42405033, 1.0437535, -3.97457625e-05, 7.5149703, 1.16343486, -0.0837713629, 7.60101795, 1.2749846, 0.318169862, 7.25824928, 0.790637732, -0.248396859, 7.48633003, 1.19830656, -0.197733015, 7.59357834, 1.28236389, -0.133080393, 7.67464352, 1.37233925, 0.380489528, 7.17906094, 0.693229854, -0.428903759, 7.09688616, -0.476069212, -0.421900928, 7.1099453, -0.555683434, 0.421900928, 7.1099453, 0.555683434, 0.428903759, 7.09688616, 0.476069212, 0.416245729, 7.08402205, 0.622810602, 0.377859384, 7.11973429, -2.24082669e-05, 0.42147997, 7.09686327, 0.203634292, 0.302039385, 7.1331687, -0.238388941, 0.279497236, 7.12861156, -1.00825264e-05, 0.438956082, 7.04509354, 0.362091303, 0.226119339, 7.13548899, 0.164285332, -0.0604616255, 7.59310007, 0.186081737, 0.0604615882, 7.59310007, 0.186081737, 0, 7.59310007, 0, 0.149851367, 7.56884527, 0.206252769, 0.226119339, 7.51029158, 0.164285347, -0.206252769, 7.56884527, 0.149851367, -0.115004838, 7.59310007, 0.15829058, -0.318169862, 7.25824928, -0.790637732, -0.231536552, 7.33535433, -0.91604346, -0.135772571, 7.2723074, -0.786019921, -1.98488124e-07, 7.21398306, -0.633488476, -0.380489528, 7.17906094, -0.693229854, 0.16579853, 7.16532135, -0.458753914, -1.83059665e-08, 7.14834023, -0.279499024, -0.416245729, 7.08402205, -0.622810602, -0.164285362, 7.51029158, -0.226119444, -0.0863699391, 7.51029158, -0.265819401, -0.226119399, 7.13548899, -0.164285362, -0.226119399, 7.51029158, -0.164285377, -0.242464721, 7.56884527, -0.0787815824, -0.195657909, 7.59310007, -3.33188659e-08, -0.242464721, 7.56884527, 0.0787815154, -0.234137714, 7.46419048, 1.29128766, -0.437107563, 7.04837513, -0.527223408, -0.214481756, 7.32862949, 1.37963545, -0.433529437, 6.96563435, -0.573231816, -0.196692616, 7.02537203, 1.45423019, -0.423157126, 6.82137585, -0.624940872, -0.184089303, 6.61271, 1.50342572, -0.404275864, 6.62828016, -0.702715218, -0.173716366, 6.16901445, 1.53983116, -0.379758388, 6.40113211, -0.79333353, 0.327292442, 6.01040268, 0.963982165, 0.304629743, 5.9069314, 1.02362251, 0.290297121, 5.87304449, 1.07569623, 0.35277158, 6.17580748, 0.884707689, 0.196224973, 5.65072536, 1.29507935, 0.131752342, 5.53558636, 1.41272449, 0.247509077, 5.76745892, 1.20240414, 0.280805796, 5.90999365, 1.13305914, 0.0608473271, 5.40024948, 1.53693473, 0.000315719255, 5.25732327, 1.65510249, -0.0361039303, 5.11989498, 1.76587057, -0.166660354, 5.51241732, 1.51388383, -0.153856233, 5.45831108, 1.56439376, -0.051867079, 4.99970007, 1.86274755, -0.0882846341, 5.12922621, 1.77260506, -0.121579073, 5.28846741, 1.68832004, -0.146261826, 5.49427271, 1.62141442, 0.379758388, 6.40113211, 0.79333353, 0.404275864, 6.62828016, 0.702715218, -0.167041779, 5.60303307, 1.5393852, -0.162309721, 5.7736063, 1.57522488, 0.438956082, 6.21159744, -0.362091303, 0.428903759, 6.15980482, -0.476069212, 0.443767488, 6.30374575, -0.195703536, 0.421900928, 6.14674568, -0.555683434, 0.44691509, 6.45208025, 2.25071708e-05, 0.447775304, 6.62831402, 0.194927514, 0.318150342, 5.99833679, -0.79076165, 0.231420755, 5.92051983, -0.917008758, 0.380489528, 6.07763004, -0.693229854, 0.416245729, 6.17266893, -0.622810602, 0.226119488, 6.12120199, -0.164285466, 0.442748636, 6.44999266, 0.527089894, 0.436531246, 6.6283021, 0.573927343, 0.44643271, 6.62828827, 0.462801337, 0.423157126, 6.82137585, 0.624940872, 0.446347833, 6.80466318, 0.359647483, -0.327292442, 6.01040268, -0.963982165, -0.35277158, 6.17580748, -0.884707689, -0.304629743, 5.9069314, -1.02362251, -0.290297121, 5.87304449, -1.07569623, 0.11619395, 5.8294673, -1.04748738, 2.22877134e-07, 5.73454285, -1.17186856, -0.0828776583, 5.63486338, -1.29494286, -0.196223646, 5.6507225, -1.2950815, -0.131736115, 5.53554964, -1.41275144, -0.247509077, 5.76745892, -1.20240414, -0.280805796, 5.90999365, -1.13305914, 0.296264559, 7.31904459, 0.903661013, 0.225867122, 7.41530323, 1.04563987, 0.100437641, 7.51294899, 1.16395378, -6.26124965e-05, 7.60239077, 1.27451706, -0.0718580559, 7.67871094, 1.37129664, 0.354741335, 7.22990417, 0.792800844, -0.221962705, 7.5589118, 1.28783321, -0.173203558, 7.66784096, 1.37404954, -0.118389323, 7.73362541, 1.44812167, 0.39546892, 7.12122631, 0.701159, -0.430589259, 7.08762932, -0.521244645, -0.429776281, 7.06061935, -0.572380662, 0.430589229, 7.08762932, 0.521244526, 0.429776281, 7.06061935, 0.572380662, 0.439286768, 7.04464674, 0.464693427, 0.420565814, 6.98357582, 0.624139011, 0.381670356, 7.12239408, -0.221979678, 0.420399696, 7.09635496, -3.39308936e-05, 0.438757658, 7.04537582, 0.196717665, 0.32253018, 7.15195227, -0.476763725, 0.165297419, 7.14047766, -0.239682525, 0.279498845, 7.12861204, 2.09443201e-08, 0.272587895, 7.13022137, -0.0436344929, 0.443575948, 6.95295715, 0.360556096, 0.265819192, 7.13157082, 0.0863699317, 0.115004793, 7.59310007, 0.158290565, 0.195657864, 7.59310007, -8.80176501e-08, 0.186081663, 7.59310007, 0.0604616441, 0.15829052, 7.59310007, 0.115004838, -0.15829058, 7.59310007, 0.115004778, -0.186081752, 7.59310007, 0.0604615882, -0.186081752, 7.59310007, -0.060461659, -0.15829058, 7.59310007, -0.115004867, -0.115004838, 7.59310007, -0.158290669, -0.0604616292, 7.59310007, -0.186081856, 1.41108716e-08, 7.59310007, -0.195658043, 0.0604616627, 7.59310007, -0.186081901, 0.115004912, 7.59310007, -0.158290699, 0.158290669, 7.59310007, -0.115004957, 0.186081797, 7.59310007, -0.0604617968, 0.206252739, 7.56884527, 0.149851412, 0.265819192, 7.51029158, 0.0863699466, -0.296264559, 7.31904459, -0.903661013, -0.354741365, 7.22990417, -0.792800784, -0.125972882, 7.34356785, -0.919755757, -0.225867122, 7.41530323, -1.04563987, 1.57607428e-06, 7.27423811, -0.785173655, 0.152501762, 7.21108389, -0.636612177, -0.39546892, 7.12122631, -0.701158941, 0.0863699391, 7.14480782, -0.265819401, 0, 7.51029158, -0.279499024, -0.420565814, 6.98357582, -0.624139011, -0.149851456, 7.56884527, -0.206252843, -0.0787815675, 7.56884527, -0.242464826, -0.206252798, 7.56884527, -0.149851456, -0.209153906, 7.52507401, 1.37805271, -0.193406716, 7.36416292, 1.45368183, -0.182898492, 7.029459, 1.50331092, -0.403299004, 6.83688831, -0.702534437, -0.174380168, 6.58932781, 1.54009831, -0.380831331, 6.62825251, -0.793302, -0.164808542, 6.12251854, 1.57625866, -0.356105059, 6.38247538, -0.884071171, 0.315180749, 5.98755264, 1.01420152, 0.332666337, 6.14672947, 0.962464333, 0.301048696, 5.91160393, 1.0498569, 0.295682311, 5.94577789, 1.08512998, 0.356105059, 6.38247538, 0.884071171, 0.171834812, 5.55869246, 1.40037119, 0.220750496, 5.69114923, 1.29507947, 0.11831709, 5.41353464, 1.53790116, 0.258765846, 5.85453892, 1.2074163, 0.285477847, 6.06717491, 1.13435245, 0.0486764275, 5.2600441, 1.65563273, 0.000496374036, 5.11762619, 1.76576054, -0.0272732545, 4.99444771, 1.86220205, -0.16278553, 5.51774311, 1.53923464, -0.158053547, 5.56075668, 1.57389069, -0.0697767287, 5.00795269, 1.86402178, -0.0406090058, 4.9021678, 1.93815839, -0.101418696, 5.16672325, 1.77801585, -0.129316807, 5.39069462, 1.69507611, -0.150852427, 5.70313454, 1.62369382, 0.380831361, 6.62825251, 0.79330194, 0.403299004, 6.83688831, 0.702534437, 0.443575919, 6.30373383, -0.360556066, 0.439286768, 6.21204424, -0.464693397, 0.430589229, 6.16906166, -0.521244526, 0.44677335, 6.45208454, -0.19510901, 0.429776281, 6.19607162, -0.572380662, 0.447943091, 6.62834549, 0, 0.44677332, 6.80460644, 0.195108995, 0.296186596, 5.93722773, -0.904156804, 0.354736477, 6.02676058, -0.792831779, 0.225511238, 5.83923531, -1.04820025, 0.39546892, 6.13546467, -0.701159, 0.420565814, 6.27311516, -0.624139011, 0.443453908, 6.62829494, 0.5270558, 0.435781032, 6.81170416, 0.573748231, 0.445638776, 6.80479002, 0.462998539, -0.315180749, 5.98755264, -1.01420152, -0.332666337, 6.14672947, -0.962464392, -0.301048696, 5.91160393, -1.0498569, -0.295682341, 5.94577789, -1.08512998, 0.0998942629, 5.73692417, -1.17197931, 1.06754669e-07, 5.63271523, -1.29489601, -0.0710229874, 5.52384281, -1.41670203, -0.171829507, 5.55868053, -1.40038002, -0.220750168, 5.69114828, -1.29508007, -0.118282102, 5.41345549, -1.53795898, -0.258765846, 5.85453892, -1.2074163, -0.285477847, 6.06717491, -1.13435256, 0.284153521, 7.36982727, 0.994833052, 0.331162542, 7.28140974, 0.890809834, 0.182371825, 7.51076508, 1.16938436, 0.257524669, 7.44125843, 1.0987289, 0.0836544409, 7.60090685, 1.27491915, -8.10358761e-05, 7.68002558, 1.37092125, -0.0638381392, 7.73844814, 1.44730675, 0.371210814, 7.165277, 0.793445706, -0.196977943, 7.62806463, 1.37607217, -0.154556111, 7.7255621, 1.4494499, -0.10910362, 7.76674175, 1.4981035, 0.400363564, 7.0122776, 0.702014863, 0.437107563, 7.04837513, 0.527223408, 0.433529437, 6.96563435, 0.573231816, 0.443256766, 6.95300579, 0.463627696, 0.42147997, 7.0968976, -0.203700453, 0.391884238, 7.12618637, -0.404379696, 0.438691556, 7.0454874, -4.06682775e-05, 0.443767488, 6.95294523, 0.195703521, 0.277451128, 7.20569944, -0.648459554, 0.378664732, 7.15493488, -0.569443941, 0.165075839, 7.14004707, -0.225329116, 0.138657808, 7.14092684, -0.239177436, 0.279498845, 7.51029158, 3.33188659e-08, 0.272592068, 7.13022041, 0.0436075963, 0.265819371, 7.13157225, -0.0863700062, 0.254942447, 7.56884527, -2.57797712e-08, 0.242464617, 7.56884527, 0.0787815824, 4.13297885e-09, 7.56884527, -0.254942596, 0.0787815824, 7.56884527, -0.24246484, 0.149851486, 7.56884527, -0.206252947, 0.206252888, 7.56884527, -0.149851531, 0.242464766, 7.56884527, -0.0787816867, -0.331162542, 7.28140974, -0.890809834, -0.284153521, 7.36982727, -0.994833052, -0.371210814, 7.165277, -0.793445706, 7.17531657e-06, 7.34565639, -0.919446766, -0.116421811, 7.42405033, -1.04375362, -0.18237181, 7.51076508, -1.16938436, -0.257524669, 7.44125843, -1.0987289, 0.135774553, 7.2723217, -0.786026418, -0.400363564, 7.0122776, -0.702014863, 0.0863699391, 7.51029158, -0.265819401, -0.187999129, 7.57260704, 1.4525131, -0.179645151, 7.37955856, 1.50290215, -0.173160419, 7.0107379, 1.54016471, -0.379765004, 6.8553791, -0.793307424, -0.165493831, 6.54015923, 1.57694769, -0.357219428, 6.62822771, -0.883846343, -0.153545499, 6.04509926, 1.62533224, -0.335893631, 6.36658764, -0.961542308, 0.306204319, 5.96919584, 1.05004025, 0.319868058, 6.12821484, 1.01315844, 0.335893631, 6.36658764, 0.961542308, 0.299883217, 6.09487534, 1.08596206, 0.357219428, 6.62822771, 0.883846343, 0.195894644, 5.61448097, 1.38762712, 0.155970037, 5.48235846, 1.48755264, 0.233392969, 5.78930521, 1.29498291, 0.0882868022, 5.26247549, 1.66082299, 0.132228702, 5.3714323, 1.58939183, 0.263942152, 6.02428675, 1.20824122, 0.288278252, 6.32315159, 1.13513815, 0.037062306, 5.11917448, 1.7663964, 0.000642645173, 4.99233627, 1.86228371, -0.0211372972, 4.89682102, 1.9381752, -0.055291675, 4.91052818, 1.93853223, -0.0820840374, 5.04778862, 1.8659091, -0.0333057195, 4.84451103, 1.98705602, -0.109694481, 5.26890564, 1.78174603, -0.134131908, 5.59361362, 1.69768822, 0.379765004, 6.8553791, 0.793307424, 0.443256766, 6.30368519, -0.463627696, 0.446347833, 6.4520278, -0.359647483, 0.437107563, 6.20831585, -0.527223408, 0.447775304, 6.62837696, -0.194927484, 0.433529437, 6.29105663, -0.573231816, 0.44691515, 6.80461073, -2.25071708e-05, 0.331143022, 5.97517681, -0.890933752, 0.284012824, 5.88615227, -0.995684922, 0.371210814, 6.09141397, -0.793445706, 0.181410655, 5.74001026, -1.17642462, 0.257047713, 5.81331062, -1.10132825, 0.400363564, 6.24441338, -0.702014863, 0.423157126, 6.43531513, -0.624940872, 0.442748845, 6.80659962, 0.527064621, -0.306204319, 5.96919584, -1.05004025, -0.319868058, 6.12821484, -1.01315844, -0.299883217, 6.09487534, -1.08596206, 0.0828780904, 5.63486767, -1.29494143, 6.83430699e-08, 5.52084589, -1.41657174, -0.0607482605, 5.40002537, -1.53709853, -0.195893317, 5.61447763, -1.38762927, -0.155962095, 5.48234034, -1.48756576, -0.233392969, 5.78930521, -1.29498291, -0.0881877318, 5.26225138, -1.66098678, -0.132212475, 5.37139559, -1.58941877, -0.263942152, 6.02428675, -1.20824122, -0.288278252, 6.32315159, -1.13513815, 0.313296705, 7.32433367, 0.972555041, 0.284406453, 7.39693022, 1.04842651, 0.347236216, 7.20921898, 0.885690987, 0.154021353, 7.597507, 1.27714825, 0.227212876, 7.5122261, 1.18355775, 0.272495985, 7.42312765, 1.12227738, 0.0717062131, 7.67856789, 1.37121189, -8.9138397e-05, 7.74001026, 1.44700408, -0.0587791502, 7.77257824, 1.49738884, 0.376558542, 7.04641819, 0.793354034, -0.176404133, 7.68248177, 1.45100784, -0.142691046, 7.75699043, 1.4992671, -0.10281264, 7.76332045, 1.53556836, 0.440633446, 6.95619678, 0.527119458, 0.424416274, 7.09744549, -0.373639077, 0.438757658, 7.04542208, -0.196796864, 0.407281756, 7.1269455, -0.507667124, 0.443831295, 6.9529624, -3.83076112e-05, 0.24925819, 7.26689911, -0.788037598, 0.347295672, 7.20115232, -0.671512067, 0.404255658, 7.13657808, -0.60869509, 0.164285421, 7.14006519, -0.226119533, 0.182915717, 7.13879347, -0.207489237, 0.265819371, 7.51029158, -0.0863700137, 0.226119488, 7.13549185, -0.164285481, 0.164285421, 7.51029158, -0.226119533, 0.226119488, 7.51029158, -0.164285481, -0.347236216, 7.20921898, -0.885690987, -0.313296705, 7.32433367, -0.972555041, -0.284406453, 7.39693022, -1.04842651, -0.376558542, 7.04641819, -0.793354034, 0.125984326, 7.34358931, -0.919766366, 1.90713035e-05, 7.42648506, -1.04322731, -0.100437641, 7.51294899, -1.1639539, -0.154021338, 7.59750748, -1.27714837, -0.227212861, 7.51222658, -1.18355787, -0.272495985, 7.42312813, -1.12227738, -0.174300417, 7.59586811, 1.50192142, -0.169981226, 7.36540604, 1.53985846, -0.164309457, 6.95919514, 1.5772475, -0.356113642, 6.87398767, -0.88403821, -0.154325411, 6.4542861, 1.62641227, -0.336973041, 6.62821579, -0.96122092, -0.136949673, 5.92489004, 1.69957638, -0.322683156, 6.3563509, -1.0125221, 0.310299516, 6.11326361, 1.04994082, 0.322683156, 6.3563509, 1.0125221, 0.336973071, 6.62821579, 0.96122098, 0.302403659, 6.33809233, 1.08646119, 0.356113642, 6.87398767, 0.88403821, 0.208517715, 5.72418499, 1.38254106, 0.176638067, 5.55051804, 1.46544015, 0.151602283, 5.44045544, 1.53897703, 0.238972232, 5.97354603, 1.29487979, 0.0683183298, 5.12308788, 1.76869166, 0.11042776, 5.25845146, 1.67459226, 0.140092447, 5.39214706, 1.61228883, 0.267041177, 6.30002451, 1.20875967, 0.28922382, 6.62846136, 1.13534176, 0.0285185073, 4.99350357, 1.86289096, 0.00070787681, 4.89463997, 1.93847227, -0.0172492098, 4.83969688, 1.98765767, -0.0656933337, 4.94938707, 1.9395808, -0.0456514843, 4.85203505, 1.98641872, -0.089870885, 5.14778948, 1.86829364, -0.0293797553, 4.84592104, 2.01536703, -0.11453151, 5.46486425, 1.78430808, 0.445638776, 6.45190096, -0.462998539, 0.440633446, 6.30049419, -0.527119458, 0.447271824, 6.62839937, -0.359359771, 0.44677332, 6.80466747, -0.195152655, 0.435781032, 6.44498682, -0.573748231, 0.313261539, 5.93217945, -0.972767949, 0.347236216, 6.047472, -0.885690987, 0.284233212, 5.85900593, -1.04935467, 0.376558542, 6.21027279, -0.793354034, 0.152653962, 5.64098692, -1.29502034, 0.226171315, 5.73974991, -1.18931413, 0.272065639, 5.83197594, -1.12433982, 0.403299004, 6.41980267, -0.702534437, 0.424020022, 6.62839079, -0.625214219, -0.310299516, 6.11326361, -1.04994082, -0.302403688, 6.33809233, -1.08646119, 0.0710232779, 5.52384567, -1.41670108, 6.11908035e-08, 5.39634132, -1.53683519, -0.0484723262, 5.25958157, -1.65596998, -0.208517715, 5.72418499, -1.38254106, -0.176636085, 5.55051327, -1.46544337, -0.151596978, 5.44044352, -1.53898585, -0.238972232, 5.97354603, -1.29487979, -0.0681531355, 5.12271261, -1.76895523, -0.110389329, 5.25836468, -1.6746558, -0.140088379, 5.392138, -1.61229551, -0.267041177, 6.30002451, -1.20875967, -0.28922379, 6.62846136, -1.13534188, 0.327310175, 7.24609804, 0.963914692, 0.304687709, 7.34943342, 1.0233345, 0.290474147, 7.38294411, 1.07486367, 0.352786303, 7.08067703, 0.884651542, 0.132959202, 7.67432117, 1.37216854, 0.197679698, 7.59313011, 1.28216159, 0.248393595, 7.48589611, 1.19813228, 0.281099916, 7.34575224, 1.13171983, 0.0636710972, 7.73829126, 1.44721365, -8.1035716e-05, 7.77448273, 1.497123, -0.0553583764, 7.77013493, 1.53492284, -0.163202837, 7.71085548, 1.5006249, -0.13459222, 7.75194597, 1.53662348, -0.0971057191, 7.71269083, 1.57380068, 0.438956082, 7.04517269, -0.36216414, 0.42890352, 7.09695435, -0.476123691, 0.443767488, 6.95300102, -0.195777908, 0.421899676, 7.11000204, -0.55572027, 0.231541172, 7.33540201, -0.91606617, 0.318164319, 7.25831223, -0.790665567, 0.38048315, 7.17913008, -0.693260372, 0.416241884, 7.08408976, -0.622840762, -0.352786303, 7.08067703, -0.884651542, -0.327310175, 7.24609804, -0.963914692, -0.304687709, 7.34943342, -1.0233345, -0.290474147, 7.38294411, -1.07486367, 0.116455011, 7.42408943, -1.04377484, 3.97457625e-05, 7.51497078, -1.16343486, -0.083654426, 7.60090733, -1.27491939, -0.132959202, 7.67432165, -1.37216854, -0.197679684, 7.59313011, -1.28216183, -0.248393595, 7.48589659, -1.19813228, -0.281099916, 7.34575224, -1.13171983, -0.164766684, 7.58549356, 1.53900731, -0.161199227, 7.31229305, 1.57711267, -0.153176561, 6.86481619, 1.62701607, -0.335903198, 6.88985157, -0.96150583, -0.137769997, 6.32079172, 1.70092726, -0.323624909, 6.62822723, -1.01229727, -0.117349282, 5.78369045, 1.78621399, -0.312758297, 6.34808064, -1.04987407, 0.312758267, 6.34808064, 1.04987407, 0.323624879, 6.62822723, 1.01229727, 0.335903198, 6.88985157, 0.96150583, 0.303250045, 6.62834406, 1.08659887, 0.187970579, 5.66906881, 1.45687211, 0.214264363, 5.92228556, 1.38152015, 0.166677192, 5.51232624, 1.51391125, 0.153916508, 5.45814562, 1.56449294, 0.24230817, 6.27222395, 1.29485953, 0.0529922247, 4.99808931, 1.86392248, 0.0889889374, 5.12791729, 1.77355981, 0.121905051, 5.2877655, 1.68882775, 0.14636676, 5.49400234, 1.62158072, 0.268097013, 6.62862492, 1.20882142, 0.288331389, 6.93364, 1.1348896, 0.0225109458, 4.89577341, 1.93893945, 0.000645406079, 4.83772469, 1.9881264, -0.0152063211, 4.84176111, 2.01661038, -0.072322011, 5.04623032, 1.94158816, -0.0543647781, 4.8892436, 1.98661113, -0.0404602811, 4.85246658, 2.01357675, -0.0944345891, 5.3367095, 1.87065363, -0.0282536652, 4.92558861, 2.02901626, 0.442748845, 6.45009136, -0.527064621, 0.44643271, 6.62840271, -0.462801337, 0.446347833, 6.80476713, -0.359687001, 0.436531246, 6.62838888, -0.573927343, 0.327310175, 6.01059294, -0.963914692, 0.304644406, 5.90706873, -1.02356648, 0.352786303, 6.17601395, -0.884651542, 0.29030636, 5.87313557, -1.07566071, 0.379765004, 6.40131187, -0.793307424, 0.19622542, 5.65074062, -1.29507506, 0.131736919, 5.53555775, -1.41274858, 0.247511044, 5.76750231, -1.20239663, 0.280808836, 5.91007566, -1.1330471, 0.404275864, 6.62841082, -0.702715218, 0.42315498, 6.82146406, -0.624956846, -0.303250015, 6.62834406, -1.08659887, 0.0607485175, 5.40002775, -1.53709745, 4.00759745e-08, 5.25660801, -1.65562439, -0.0367304198, 5.11841965, -1.76692557, -0.187970579, 5.66906929, -1.45687211, -0.214264363, 5.92228556, -1.38152027, -0.166675866, 5.51232338, -1.51391339, -0.153915182, 5.45814276, -1.56449509, -0.24230817, 6.27222395, -1.29485953, -0.0527618378, 4.99756145, -1.86426473, -0.0889231563, 5.12776756, -1.77366471, -0.12189544, 5.28774405, -1.68884361, -0.14636676, 5.49400234, -1.62158072, -0.268097013, 6.62862492, -1.20882142, -0.288331419, 6.93364048, -1.1348896, 0.332682669, 7.1097331, 0.962401867, 0.315197587, 7.26895905, 1.01413727, 0.301136047, 7.34468889, 1.04945374, 0.295807272, 7.31045818, 1.08456635, 0.173133478, 7.66726589, 1.37378943, 0.11825601, 7.73327065, 1.44793379, 0.221954599, 7.55825949, 1.28757071, 0.259346724, 7.40033817, 1.20476532, 0.285617799, 7.18922472, 1.13370693, 0.0586272925, 7.77243567, 1.49730432, -6.28032212e-05, 7.77237415, 1.53468573, -0.0522531196, 7.72014999, 1.5732882, -0.154106975, 7.70350504, 1.53784931, -0.127243415, 7.70024729, 1.57464099, -0.0895721465, 7.60418129, 1.62608337, 0.439286739, 7.044734, -0.464755625, 0.443575919, 6.95305204, -0.360623807, 0.430588663, 7.08769798, -0.521291852, 0.429774731, 7.06068754, -0.572417438, 0.296260297, 7.31912899, -0.903698802, 0.225884482, 7.41538906, -1.04568243, 0.354731649, 7.23000145, -0.792840838, 0.395461231, 7.12131977, -0.701194584, 0.420562148, 6.98365641, -0.624166429, -0.332682669, 7.1097331, -0.962401867, -0.315197587, 7.26895905, -1.01413727, -0.301136047, 7.34468889, -1.04945362, -0.295807272, 7.31045818, -1.08456635, 0.100510508, 7.51302147, -1.16399562, 6.26124965e-05, 7.60239124, -1.2745173, -0.0717062056, 7.67856884, -1.37121224, -0.173133478, 7.66726637, -1.37378967, -0.11825601, 7.7332716, -1.44793403, -0.221954569, 7.55825996, -1.28757071, -0.259346753, 7.40033865, -1.20476532, -0.285617799, 7.1892252, -1.13370705, -0.156106949, 7.53212023, 1.57649887, -0.150083318, 7.21080923, 1.62722635, -0.136592746, 6.71758652, 1.70192695, -0.322692037, 6.90010977, -1.01248837, -0.118304692, 6.1628499, 1.78703046, -0.313581645, 6.62826824, -1.0498364, -0.0970725417, 5.64330053, 1.87246788, 0.313581616, 6.62826824, 1.04983628, 0.322692007, 6.90010977, 1.01248837, 0.302428544, 6.91854572, 1.0863477, 0.175582141, 5.6338501, 1.50475788, 0.19351469, 5.87783957, 1.45541382, 0.217695028, 6.24326372, 1.38097167, 0.162813172, 5.51762104, 1.53927541, 0.158097699, 5.5605917, 1.57394946, 0.243448347, 6.62794304, 1.29469085, 0.070701696, 5.00623274, 1.86527681, 0.0418563336, 4.90037441, 1.93946683, 0.101935402, 5.16562271, 1.77881742, 0.129524454, 5.39021111, 1.69541645, 0.150909185, 5.70294428, 1.62376213, 0.267143816, 6.95696163, 1.20827627, 0.0185055118, 4.838727, 1.98836493, 0.000502004696, 4.84008598, 2.01723552, -0.0146059608, 4.92170238, 2.0309937, -0.0762245208, 5.22897053, 1.94382489, -0.0595238917, 4.98311138, 1.98849988, -0.0479834527, 4.88813686, 2.00653005, -0.0393222012, 4.93181562, 2.02590203, -0.0284292735, 5.10120678, 2.03431559, 0.443453908, 6.62839603, -0.5270558, 0.445638776, 6.80490112, -0.463031441, 0.435780168, 6.81178904, -0.573766887, 0.332682669, 6.14695787, -0.962401867, 0.315197587, 5.98773193, -1.01413727, 0.301061511, 5.91173029, -1.04980803, 0.356113642, 6.3827033, -0.88403821, 0.295690864, 5.94588757, -1.08509731, 0.380831361, 6.62843847, -0.79330194, 0.17183128, 5.55869865, -1.40037382, 0.220750898, 5.69119453, -1.29507732, 0.11828284, 5.41346264, -1.53795636, 0.258764595, 5.85462618, -1.20741987, 0.285476804, 6.06730127, -1.13435483, 0.403294742, 6.83701468, -0.702553868, -0.302428544, 6.91854572, -1.0863477, 0.048472479, 5.259583, -1.65596926, 1.82675722e-05, 5.11645603, -1.76658082, -0.0280577261, 4.99244785, -1.86357558, -0.175582141, 5.6338501, -1.50475788, -0.19351469, 5.87783957, -1.45541382, -0.217695028, 6.2432642, -1.38097167, -0.162812591, 5.51761961, -1.53927648, -0.158097714, 5.5605917, -1.57394958, -0.243448362, 6.62794304, -1.29469073, -0.0706095472, 5.0060215, -1.86541367, -0.0415702723, 4.89971161, -1.9398433, -0.101918958, 5.16558552, -1.77884376, -0.129524454, 5.39021111, -1.69541645, -0.1509092, 5.70294476, -1.62376201, -0.267143816, 6.95696163, -1.20827627, 0.319883317, 7.1282649, 1.01310015, 0.306247085, 7.28725958, 1.04985535, 0.299945563, 7.16161203, 1.08567941, 0.196966678, 7.62722778, 1.37573528, 0.154478997, 7.72492886, 1.44916391, 0.108982429, 7.76641941, 1.49793279, 0.234171137, 7.46337795, 1.29098642, 0.264215678, 7.23190832, 1.20697439, 0.0552406907, 7.7700243, 1.53485727, -4.05141618e-05, 7.7226162, 1.57310331, -0.0481406599, 7.61165762, 1.62586439, -0.145849407, 7.65074778, 1.57560778, -0.117628343, 7.59171486, 1.6264416, -0.0778012723, 7.42712021, 1.70570052, 0.437107146, 7.04845524, -0.52727282, 0.443256766, 6.95310783, -0.46368432, 0.433527976, 6.96571302, -0.57326442, 0.331150413, 7.28153563, -0.890861154, 0.284150362, 7.36993647, -0.994881928, 0.182427585, 7.51093006, -1.16946995, 0.257534236, 7.44143391, -1.09880614, 0.371198803, 7.16540956, -0.793493092, 0.400356293, 7.01239157, -0.702047944, -0.319883317, 7.1282649, -1.01310015, -0.306247085, 7.28725958, -1.04985535, -0.299945563, 7.16161203, -1.08567941, 0.0837713629, 7.60101843, -1.2749846, 8.10358761e-05, 7.68002605, -1.37092137, -0.0636710972, 7.73829174, -1.44721377, -0.196966678, 7.62722826, -1.3757354, -0.154478982, 7.72492981, -1.44916403, -0.108982429, 7.76642036, -1.49793291, -0.234171137, 7.46337843, -1.29098642, -0.264215708, 7.23190832, -1.20697439, -0.145030111, 7.4263854, 1.6271255, -0.133417845, 7.05154228, 1.70276117, -0.11755465, 6.5398078, 1.78632295, -0.312770367, 6.90844631, -1.04982316, -0.0984066576, 6.00363398, 1.87125874, -0.0784476548, 5.52552032, 1.94556034, 0.312770396, 6.90844631, 1.04982316, 0.167056456, 5.60290003, 1.53938723, 0.1804194, 5.84754229, 1.50381219, 0.196817473, 6.21665764, 1.4546299, 0.218856618, 6.62552404, 1.38060355, 0.162340373, 5.77341318, 1.5752244, 0.242440447, 6.98330593, 1.29414308, 0.056328468, 4.90859222, 1.93994462, 0.0827646628, 5.04634047, 1.86696541, 0.0344584771, 4.84283829, 1.98827696, 0.110019051, 5.26816702, 1.78228164, 0.134237453, 5.59334946, 1.69785237, 0.153577358, 6.04491806, 1.62532771, 0.0161872022, 4.8409934, 2.01717043, 0.000324430963, 4.92021942, 2.03179026, -0.0146771679, 5.09760809, 2.03692341, -0.062284641, 5.16146994, 1.99092174, -0.0492620952, 4.94833803, 2.01471043, -0.0399999246, 5.10707903, 2.03006029, -0.0284084044, 5.3904686, 2.0375762, 0.442748636, 6.80669832, -0.527089894, 0.319883317, 6.12842607, -1.01310015, 0.335903198, 6.36683941, -0.96150583, 0.306217968, 5.96934509, -1.04998815, 0.357219428, 6.62846327, -0.883846343, 0.299889147, 6.09501314, -1.08593857, 0.379758388, 6.85555887, -0.79333353, 0.195893735, 5.61453199, -1.38762784, 0.155964077, 5.48236084, -1.48755884, 0.233388737, 5.78941584, -1.29499769, 0.0881879926, 5.26225376, -1.66098583, 0.132213399, 5.37140512, -1.58941555, 0.263933599, 6.02445316, -1.20827043, 0.288269341, 6.32334375, -1.13516843, 0.0367661901, 5.11838913, -1.76692545, 7.30314059e-05, 4.99069643, -1.86334574, -0.021938825, 4.89444828, -1.93969238, -0.167056456, 5.60290003, -1.53938723, -0.1804194, 5.84754229, -1.50381219, -0.196817473, 6.21665764, -1.4546299, -0.218856618, 6.62552404, -1.38060355, -0.162340373, 5.77341318, -1.5752244, -0.242440447, 6.98330593, -1.29414308, -0.0562140457, 4.9083271, -1.94009519, -0.0827416182, 5.04628754, -1.86699975, -0.0341348536, 4.84207582, -1.98861909, -0.110019051, 5.26816702, -1.78228164, -0.134237453, 5.59334946, -1.69785237, -0.153577358, 6.04491854, -1.62532771, 0.310325265, 7.14323664, 1.04983115, 0.209196433, 7.52403212, 1.37766635, 0.176391736, 7.68156052, 1.45063758, 0.142620966, 7.75641489, 1.49900711, 0.102718733, 7.76307106, 1.53543627, 0.239332169, 7.28091669, 1.2929852, 0.0521771871, 7.72007847, 1.57324588, -2.02571537e-05, 7.61413956, 1.6257844, -0.0416983068, 7.43369055, 1.70603061, -0.135163039, 7.54290199, 1.62682641, -0.102730788, 7.41616774, 1.7051475, -0.0640050694, 7.18740559, 1.79826725, 0.440633059, 6.95628834, -0.527163327, 0.313283414, 7.32448053, -0.972614944, 0.34722051, 7.20938873, -0.88575083, 0.284401506, 7.39706802, -1.04848611, 0.154114053, 7.59775829, -1.27728057, 0.22724396, 7.51252317, -1.18369091, 0.272493392, 7.42337704, -1.12237763, 0.376547217, 7.04658031, -0.793398738, -0.310325265, 7.14323664, -1.04983115, 0.0718580484, 7.67871141, -1.37129664, 8.91421223e-05, 7.74001074, -1.44700432, -0.0586272925, 7.77243662, -1.49730432, -0.209196419, 7.52403259, -1.37766635, -0.176391736, 7.68156147, -1.4506377, -0.142620966, 7.75641584, -1.49900734, -0.102718711, 7.76307154, -1.53543627, -0.239332169, 7.28091717, -1.29298532, -0.12824516, 7.25892591, 1.70361686, -0.114653997, 6.85336113, 1.78643, -0.0990588889, 6.35377979, 1.86454809, -0.0801491663, 5.87332535, 1.94200945, -0.0638028458, 5.45214939, 1.99271357, 0.171265617, 5.81739855, 1.53949034, 0.183291346, 6.19591904, 1.50336802, 0.197900489, 6.62047625, 1.4542129, 0.217796043, 7.00746918, 1.38012445, 0.164839149, 6.12223148, 1.57619464, 0.0664671287, 4.94773769, 1.94078422, 0.0466316715, 4.85019016, 1.98776472, 0.0902988538, 5.14682007, 1.86900091, 0.0302912649, 4.84458351, 2.01634312, 0.114694223, 5.46448946, 1.78457451, 0.136995539, 5.9247427, 1.69962943, 0.154347941, 6.45407391, 1.62635732, 0.0152410129, 4.92120218, 2.0313592, 0.000162212949, 5.09631491, 2.03786111, -0.0146489833, 5.38756657, 2.04051328, -0.0508110635, 5.12418175, 2.01773643, -0.0402561575, 5.39525461, 2.03271747, -0.0293928236, 5.75372314, 2.03175616, 0.322692007, 6.35658121, -1.01248837, 0.310311168, 6.11343765, -1.04989588, 0.336973071, 6.62847519, -0.96122098, 0.356105059, 6.8742156, -0.884071171, 0.302403748, 6.33826208, -1.0864594, 0.20851171, 5.72432184, -1.38256216, 0.176636651, 5.5505743, -1.46544158, 0.151599005, 5.44046402, -1.5389787, 0.238956958, 5.97377586, -1.29493284, 0.0681866035, 5.12265873, -1.76895475, 0.110389657, 5.25836802, -1.6746546, 0.140088752, 5.39215422, -1.61229444, 0.267019719, 6.3003149, -1.20883405, 0.289206415, 6.62872124, -1.1354053, 0.0282006841, 4.99232292, -1.8635757, 0.000182578282, 4.89257717, -1.93964219, -0.0178582687, 4.83720303, -1.98904943, -0.171265617, 5.81739855, -1.53949034, -0.183291376, 6.19591904, -1.50336802, -0.197900474, 6.62047625, -1.45421302, -0.217796043, 7.00746965, -1.38012445, -0.164839163, 6.12223196, -1.57619464, -0.0664385259, 4.94767094, -1.94082189, -0.0465022214, 4.84988499, -1.98790157, -0.0902988538, 5.14682055, -1.86900091, -0.02995014, 4.84376574, -2.01660824, -0.114694223, 5.46448994, -1.78457451, -0.136995554, 5.9247427, -1.69962943, -0.154347956, 6.45407438, -1.6263572, 0.21456033, 7.32750416, 1.37924266, 0.188045904, 7.5714612, 1.45208812, 0.163191572, 7.71001863, 1.50028789, 0.134537905, 7.75150013, 1.53642201, 0.0970451236, 7.71252966, 1.57371521, 0.0481026955, 7.61162186, 1.62584317, -8.10393249e-06, 7.43587637, 1.70613778, -0.03414068, 7.19243813, 1.79928768, -0.118780777, 7.37028551, 1.70443249, -0.0853212401, 7.17901707, 1.79656541, -0.0503955372, 6.89093542, 1.88939822, 0.327292442, 7.2462883, -0.963982165, 0.304674238, 7.34958363, -1.02339602, 0.35277158, 7.0808835, -0.884707689, 0.290465444, 7.38311243, -1.07493198, 0.133080393, 7.67464447, -1.37233925, 0.197733015, 7.59357882, -1.28236389, 0.248396859, 7.48633003, -1.19830656, 0.281084716, 7.34606075, -1.13183379, 0.0638381392, 7.7384491, -1.44730675, 8.10394413e-05, 7.77448368, -1.49712312, -0.0552406833, 7.77002525, -1.53485739, -0.21456033, 7.32750463, -1.37924266, -0.188045889, 7.57146168, -1.45208812, -0.163191572, 7.71001911, -1.50028813, -0.134537905, 7.75150061, -1.53642213, -0.0970451161, 7.71253061, -1.57371521, -0.10915757, 7.04230785, 1.78968847, -0.0972430035, 6.6351347, 1.86094463, -0.082486771, 6.20983076, 1.92838871, -0.0656055138, 5.80210447, 1.98768187, -0.0515887886, 5.41234255, 2.01980615, 0.173753068, 6.16859484, 1.53971398, 0.184158802, 6.61190796, 1.50317168, 0.196785197, 7.02424765, 1.4538554, 0.165530637, 6.53975296, 1.57682431, 0.0728121102, 5.04512024, 1.94239831, 0.0551171787, 4.88763285, 1.98778594, 0.041256167, 4.85095501, 2.01468015, 0.0946485773, 5.33622456, 1.87100732, 0.0288472995, 4.92471313, 2.02965498, 0.117415287, 5.78353024, 1.78631759, 0.137787297, 6.32068825, 1.70091891, 0.153197616, 6.8645606, 1.62693083, 0.014994693, 5.09735823, 2.0371058, 6.4885855e-05, 5.38656187, 2.04152012, -0.0151784141, 5.75188637, 2.03474689, -0.0416460857, 5.75677204, 2.02678132, -0.0325843059, 6.15131998, 2.0098145, 0.312764198, 6.34827375, -1.04985118, 0.323624879, 6.62846375, -1.01229727, 0.335893631, 6.89010334, -0.961542308, 0.303243071, 6.62854338, -1.08662438, 0.214244157, 5.92257547, -1.3815906, 0.187964052, 5.66922092, -1.45689499, 0.166676581, 5.5123806, -1.51391077, 0.153915957, 5.45817566, -1.56449246, 0.242273688, 6.27264404, -1.29497945, 0.0528954677, 4.99734306, -1.86426473, 0.0889527649, 5.12770557, -1.77366424, 0.121895567, 5.28775024, -1.68884313, 0.146365374, 5.49403811, -1.62158537, 0.268062264, 6.62904978, -1.20894837, 0.288308054, 6.93395424, -1.13498354, 0.0222962108, 4.89413595, -1.93969238, 0.000365157146, 4.83534431, -1.98919296, -0.0155049535, 4.83935833, -2.01770115, -0.173753068, 6.16859484, -1.53971398, -0.184158802, 6.61190796, -1.5031718, -0.196785197, 7.02424812, -1.45385551, -0.165530637, 6.53975344, -1.57682431, -0.0728121102, 5.04512024, -1.94239831, -0.0550848171, 4.88755655, -1.98782027, -0.0411197171, 4.8506279, -2.01478624, -0.0946485773, 5.33622456, -1.87100732, -0.0285106804, 4.9238925, -2.02982616, -0.117415287, 5.78353024, -1.78631759, -0.137787297, 6.32068872, -1.70091891, -0.153197616, 6.86456108, -1.62693083, 0.193493158, 7.36292505, 1.45324957, 0.174342945, 7.59482622, 1.50153518, 0.154098243, 7.70285606, 1.53758824, 0.127208352, 7.69995975, 1.57451105, 0.0895418376, 7.6041007, 1.6260407, 0.0416831151, 7.43367624, 1.70602214, -2.02603223e-06, 7.19411469, 1.79962707, -0.0266824048, 6.89409161, 1.89113569, -0.0998013616, 7.13980675, 1.79382503, -0.0681701154, 6.88567495, 1.88650262, -0.039184615, 6.54360771, 1.96470904, 0.315180749, 7.26913834, -1.01420152, 0.332666337, 7.10996151, -0.962464333, 0.3011235, 7.34484911, -1.04951715, 0.295793921, 7.31065845, -1.08463979, 0.173203558, 7.66784191, -1.37404954, 0.118389308, 7.73362637, -1.44812167, 0.221962705, 7.55891228, -1.28783321, 0.259323657, 7.40087891, -1.20496547, 0.285594165, 7.18956089, -1.13382041, 0.0587791502, 7.77257967, -1.49738884, 6.27994959e-05, 7.77237511, -1.53468609, -0.0521771871, 7.72007942, -1.57324588, -0.193493158, 7.36292553, -1.45324957, -0.174342945, 7.5948267, -1.50153518, -0.154098243, 7.70285702, -1.53758836, -0.127208337, 7.69996071, -1.57451129, -0.0895418376, 7.60410166, -1.6260407, -0.0911727995, 6.78909683, 1.86905718, -0.0852598995, 6.45125151, 1.90921497, -0.0692196041, 6.15828943, 1.96963215, -0.0532722846, 5.76993275, 2.01402259, 0.174434513, 6.58870506, 1.53990257, 0.182982653, 7.02843666, 1.50297034, 0.164351523, 6.95868397, 1.57707715, 0.0764695629, 5.22841549, 1.94422984, 0.0600070842, 4.98201656, 1.98929834, 0.0486608408, 4.88673162, 2.00755525, 0.0398468077, 4.93081474, 2.02663231, 0.097158134, 5.64310694, 1.87260938, 0.0287260916, 5.10076904, 2.03463507, 0.118322663, 6.16279316, 1.78705096, 0.136601165, 6.71748447, 1.70189261, 0.150102973, 7.2105279, 1.62712824, 0.0147759952, 5.38746643, 2.04058647, 1.62229662e-05, 5.75126457, 2.03575087, -0.0169224627, 6.15087461, 2.01260638, -0.0457249172, 6.15206146, 2.00516105, 0.313579887, 6.62847233, -1.0498426, 0.322683156, 6.90034008, -1.0125221, 0.30241555, 6.91876459, -1.08639932, 0.193492502, 5.87815905, -1.45549119, 0.21765025, 6.24379873, -1.3811276, 0.175576344, 5.63398933, -1.50477803, 0.162812069, 5.51768303, -1.53927839, 0.158094957, 5.56066322, -1.57395911, 0.243394509, 6.62857056, -1.29488754, 0.0707276389, 5.00577164, -1.86541367, 0.0419043526, 4.89916468, -1.9398433, 0.101945616, 5.16552353, -1.77884352, 0.129523903, 5.39022541, -1.69541848, 0.150904208, 5.70301771, -1.62377942, 0.267100841, 6.95749617, -1.2084502, 0.0185730439, 4.83657789, -1.98904943, 0.00056502287, 4.83752823, -2.01806498, -0.0145677729, 4.91956091, -2.03170156, -0.174434543, 6.58870506, -1.53990245, -0.182982638, 7.02843714, -1.50297046, -0.164351523, 6.95868444, -1.57707715, -0.0764695629, 5.22841549, -1.94422984, -0.0600070879, 4.98201656, -1.98929834, -0.0486001968, 4.88658619, -2.00760245, -0.0397121608, 4.93048668, -2.02670097, -0.097158134, 5.64310741, -1.87260938, -0.0284179356, 5.10000706, -2.03472042, -0.118322648, 6.16279364, -1.78705096, -0.136601165, 6.71748495, -1.70189285, -0.150102958, 7.21052837, -1.62712812, 0.179723725, 7.37843323, 1.50250936, 0.16479966, 7.5846858, 1.53870797, 0.145843774, 7.65032911, 1.57543921, 0.117610797, 7.59157085, 1.62637663, 0.0777891502, 7.42708778, 1.70568359, 0.0341368802, 7.19243479, 1.79928517, 7.27595761e-12, 6.89514399, 1.89171505, -0.0205381233, 6.54484034, 1.96707523, -0.0813234448, 6.85837412, 1.88040352, -0.0540478602, 6.54155254, 1.96076524, 0.306232214, 7.28743172, -1.0499177, 0.319868058, 7.12847614, -1.01315844, 0.299929738, 7.16183329, -1.08574927, 0.196977943, 7.62806511, -1.37607217, 0.154556096, 7.72556305, -1.4494499, 0.10910362, 7.76674271, -1.4981035, 0.234137714, 7.46419096, -1.29128766, 0.264174789, 7.23249292, -1.2071768, 0.0553583764, 7.77013588, -1.53492296, 4.05216124e-05, 7.72261667, -1.57310343, -0.0481026955, 7.61162281, -1.62584317, -0.179723725, 7.37843418, -1.50250936, -0.16479966, 7.58468676, -1.53870797, -0.145843759, 7.65033007, -1.57543921, -0.117610797, 7.59157181, -1.62637663, -0.0777891502, 7.42708874, -1.70568371, -0.0769678131, 6.49564123, 1.93220913, -0.0575385913, 6.1540637, 1.99348664, 0.173225656, 7.00994587, 1.53990054, 0.161238521, 7.31173038, 1.57691634, 0.0785456598, 5.5252986, 1.94572222, 0.0625262335, 5.16092253, 1.99132133, 0.0497349314, 4.94731426, 2.01545763, 0.0402622335, 5.10657883, 2.03042531, 0.0984280631, 6.00358582, 1.87129414, 0.0285271313, 5.39029312, 2.03770375, 0.117556743, 6.53978205, 1.78631449, 0.133425713, 7.05142975, 1.70272171, 0.145040736, 7.42612505, 1.62702906, 0.0152101666, 5.7518611, 2.03476501, 4.36557457e-11, 6.15072632, 2.01353717, 0.312761277, 6.90865183, -1.04985857, 0.196768254, 6.21724606, -1.45480144, 0.180399314, 5.84783363, -1.50388217, 0.218787104, 6.62632608, -1.38085747, 0.167052135, 5.60301018, -1.53940237, 0.162330419, 5.77355957, -1.57525921, 0.242374972, 6.98410416, -1.2944082, 0.0565092787, 4.90770197, -1.94009519, 0.0828480497, 5.04603195, -1.86699975, 0.034803018, 4.84098196, -1.98861897, 0.110046104, 5.26810217, -1.78228211, 0.134235442, 5.59337902, -1.69785917, 0.153566197, 6.04505253, -1.62536645, 0.0166089684, 4.83839893, -2.01770115, 0.000726428814, 4.91765785, -2.03232622, -0.0143783782, 5.09583426, -2.03727698, -0.173225656, 7.00994635, -1.53990054, -0.161238521, 7.31173134, -1.57691634, -0.0785456598, 5.5252986, -1.94572222, -0.0625262335, 5.160923, -1.99132133, -0.0497012697, 4.94723177, -2.0154748, -0.0401389711, 5.10627413, -2.03045964, -0.0984280631, 6.00358582, -1.87129414, -0.0282733459, 5.38966084, -2.03773785, -0.117556743, 6.53978252, -1.78631449, -0.133425713, 7.05143023, -1.70272171, -0.145040736, 7.426126, -1.62702906, 0.170042127, 7.3645339, 1.539554, 0.156128198, 7.53159952, 1.57630551, 0.135160223, 7.54269266, 1.62674236, 0.102723777, 7.41611004, 1.70512152, 0.0640020519, 7.18739748, 1.79826307, 0.0266824029, 6.89409161, 1.89113569, -1.07684173e-09, 6.54525137, 1.96786404, -0.0662636533, 6.52884197, 1.95125997, 0.310311139, 7.14343166, -1.04988742, 0.209153906, 7.52507448, -1.37805271, 0.176404133, 7.68248224, -1.45100784, 0.142691046, 7.75699139, -1.4992671, 0.10281264, 7.76332188, -1.53556836, 0.239270881, 7.28179455, -1.29329097, 0.0522531159, 7.72015095, -1.5732882, 2.0260879e-05, 7.61414051, -1.6257844, -0.0416831151, 7.4336772, -1.70602214, -0.170042127, 7.36453438, -1.539554, -0.156128198, 7.5316, -1.57630575, -0.135160223, 7.54269361, -1.62674236, -0.102723777, 7.41611099, -1.70512152, -0.0639962256, 7.18738413, -1.79826307, 0.0801736712, 5.87327003, 1.9420501, 0.0638994798, 5.45193052, 1.99287343, 0.0510474816, 5.12366915, 2.01811004, 0.0403610803, 5.39505434, 2.03286338, 0.0990588814, 6.35377979, 1.86454821, 0.0294225067, 5.75367928, 2.03178811, 0.114655957, 6.853333, 1.78641987, 0.128249422, 7.25882149, 1.70357823, 0.0169224609, 6.15087461, 2.01260662, 0.197824061, 6.62135887, -1.45449209, 0.183246642, 6.19645452, -1.50352371, 0.171250135, 5.81762505, -1.53954434, 0.217711896, 7.00849199, -1.38046527, 0.164816827, 6.12249994, -1.57627249, 0.0667046234, 4.9470315, -1.94082212, 0.0470926873, 4.84863472, -1.98790157, 0.0904076248, 5.14654684, -1.86900091, 0.0309760757, 4.84209442, -2.016608, 0.114721313, 5.46442795, -1.7845763, 0.136991099, 5.92479658, -1.69964492, 0.154330596, 6.45427513, -1.62642062, 0.0159817841, 4.91834974, -2.03170109, 0.000796743843, 5.09394455, -2.03812909, -0.0142684197, 5.3862009, -2.04065466, -0.0801736638, 5.87327051, -1.9420501, -0.0638994798, 5.451931, -1.99287343, -0.0510166772, 5.12359333, -2.01811862, -0.0402595624, 5.39480162, -2.03287697, -0.0990588814, 6.35378027, -1.86454821, -0.0292366911, 5.75321388, -2.03179669, -0.114655957, 6.85333395, -1.78642011, -0.128249422, 7.25882244, -1.70357835, 0.118779637, 7.37020159, 1.70439863, 0.0853194892, 7.17900276, 1.79655874, 0.0503955372, 6.89093542, 1.88939822, 0.0205381233, 6.54484034, 1.96707499, 0.214481756, 7.32862997, -1.37963545, 0.187999129, 7.57260799, -1.4525131, 0.163202837, 7.71085644, -1.5006249, 0.13459222, 7.75194693, -1.53662348, 0.0971057191, 7.71269178, -1.57380068, 0.0481406599, 7.61165857, -1.62586439, 8.10393249e-06, 7.43587732, -1.70613778, -0.0341252275, 7.19240665, -1.79928517, -0.118779637, 7.37020254, -1.70439863, -0.0853171572, 7.17899799, -1.79655874, -0.0503722318, 6.89087772, -1.88939822, 0.082486771, 6.20983076, 1.92838871, 0.0656296685, 5.80204964, 1.98772156, 0.0516833588, 5.41213799, 2.01995587, 0.0416723192, 5.75672245, 2.02681804, 0.0972430035, 6.6351347, 1.86094463, 0.0325843059, 6.1513195, 2.0098145, 0.109158643, 7.04228163, 1.78967881, 0.196692616, 7.02537251, -1.45423019, 0.184089303, 6.61271048, -1.50342548, 0.173718452, 6.16901016, -1.5398345, 0.165495887, 6.54015446, -1.57695115, 0.073084034, 5.04443645, -1.94239819, 0.0556170121, 4.8862772, -1.98782039, 0.0420155227, 4.84873867, -2.01478624, 0.094758898, 5.33594751, -1.87100732, 0.0298081543, 4.92180204, -2.02982616, 0.117438249, 5.78348351, -1.78632128, 0.137780353, 6.32076883, -1.70094442, 0.153176561, 6.86481667, -1.62701619, 0.0159244686, 5.09452534, -2.03727674, 0.000723321922, 5.38459682, -2.04162741, -0.01483854, 5.75092983, -2.03478217, -0.082486771, 6.20983171, -1.9283886, -0.0656296685, 5.80205011, -1.98772168, -0.0516579822, 5.41207457, -2.01995921, -0.0415979922, 5.75653648, -2.02682137, -0.0972430035, 6.63513565, -1.86094463, -0.0324677639, 6.15102768, -2.0098145, -0.109158643, 7.04228258, -1.78967881, 0.0998010784, 7.13978577, 1.79381657, 0.0681701154, 6.88567495, 1.88650262, 0.0391846187, 6.54360771, 1.96470904, 0.193406716, 7.3641634, -1.45368159, 0.174300402, 7.59586906, -1.50192142, 0.154106975, 7.70350599, -1.53784931, 0.127243415, 7.70024824, -1.57464099, 0.0895721465, 7.60418224, -1.62608337, 0.0416983068, 7.4336915, -1.70603061, 2.00896739e-05, 7.19407034, -1.79962707, -0.0266357865, 6.89397526, -1.89113569, -0.0998004973, 7.13978529, -1.79381657, -0.0681607947, 6.88565254, -1.88650262, -0.0391263478, 6.5434618, -1.96470904, 0.0852598995, 6.45125151, 1.90921497, 0.0692196041, 6.15828943, 1.96963215, 0.0532959253, 5.76988173, 2.01406002, 0.0457249172, 6.15206146, 2.00516129, 0.0911727995, 6.78909683, 1.86905718, 0.182898507, 7.02945948, -1.50331092, 0.17438069, 6.58932686, -1.54009914, 0.164309457, 6.95919561, -1.5772475, 0.076745376, 5.22772217, -1.94422984, 0.0605509356, 4.98064947, -1.98929834, 0.0493626259, 4.88483047, -2.00760245, 0.0408154055, 4.92818165, -2.02670074, 0.09725447, 5.64286518, -1.87260938, 0.0298218466, 5.0977664, -2.03472042, 0.118339621, 6.16276646, -1.78705704, 0.136592761, 6.71758747, -1.70192683, 0.150083318, 7.21080971, -1.62722635, 0.0156700015, 5.38502121, -2.04065466, 0.000560166256, 5.74981976, -2.03577757, -0.0166893825, 6.15028954, -2.01260638, -0.0852598995, 6.45125198, -1.90921497, -0.0692196041, 6.15829039, -1.96963215, -0.0532773472, 5.76983547, -2.01406097, -0.0456783026, 6.15194511, -2.00516129, -0.0911727995, 6.78909779, -1.86905718, 0.0813234448, 6.85837412, 1.8804034, 0.0540478602, 6.54155254, 1.96076524, 0.179645151, 7.37955952, -1.50290215, 0.164766684, 7.58549452, -1.53900731, 0.145849407, 7.65074873, -1.57560778, 0.117628343, 7.59171581, -1.6264416, 0.0778012723, 7.42712116, -1.70570052, 0.0341639854, 7.19238091, -1.79928768, 7.22545592e-05, 6.89496326, -1.89171505, -0.0204215832, 6.54454851, -1.96707499, -0.0813211128, 6.85836887, -1.8804034, -0.0540245548, 6.54149485, -1.96076524, 0.0769678131, 6.49564123, 1.93220913, 0.0575385913, 6.1540637, 1.99348664, 0.173160419, 7.01073837, -1.54016447, 0.161199227, 7.31229401, -1.57711267, 0.0787865147, 5.52469301, -1.94572222, 0.0630778521, 5.15953588, -1.99132097, 0.0505481251, 4.94521999, -2.0154748, 0.0413059145, 5.10385561, -2.03045964, 0.0985028967, 6.00339794, -1.87129414, 0.0295397416, 5.38764858, -2.03773785, 0.11756812, 6.53977442, -1.78632295, 0.133417845, 7.05154276, -1.70276117, 0.145030111, 7.42638636, -1.6271255, 0.0159231331, 5.75001955, -2.03478217, 0.00036127257, 6.14981842, -2.01353717, -0.0769678131, 6.49564219, -1.93220913, -0.0575269386, 6.15403509, -1.99348676, 0.0662636608, 6.52884197, 1.95125997, 0.169981226, 7.36540699, -1.53985834, 0.156106949, 7.53212118, -1.57649887, 0.135163039, 7.54290295, -1.62682641, 0.102730788, 7.41616869, -1.7051475, 0.0640307143, 7.18734217, -1.79826725, 0.0267756358, 6.89385843, -1.89113569, 0.000180636518, 6.5447979, -1.96786392, -0.0662578344, 6.52882814, -1.95125997, 0.0803589672, 5.87280464, -1.94204998, 0.0643811747, 5.45071983, -1.99287343, 0.0518876165, 5.12153244, -2.01811862, 0.0413006507, 5.39265299, -2.03287721, 0.0991127566, 6.35364485, -1.86454809, 0.0302140731, 5.75166512, -2.03179693, 0.114664868, 6.85333443, -1.78643, 0.12824516, 7.25892687, -1.70361686, 0.0173886195, 6.14970303, -2.01260638, 0.118780777, 7.37028646, -1.70443249, 0.0853445455, 7.17895937, -1.79656541, 0.0504980907, 6.89067841, -1.88939822, 0.0207712017, 6.54425526, -1.96707523, 0.0826141834, 6.20951128, -1.9283886, 0.0659966394, 5.80112743, -1.98772156, 0.0524240322, 5.41026592, -2.01995921, 0.0423966795, 5.7548914, -2.02682137, 0.0972865149, 6.63502645, -1.86094463, 0.0330970809, 6.15003109, -2.0098145, 0.10917104, 7.04227495, -1.78968847, 0.0998198763, 7.13976097, -1.79382503, 0.0682633519, 6.8854413, -1.88650262, 0.0394410044, 6.5429635, -1.96470904, 0.0853538215, 6.45101595, -1.90921497, 0.0694599375, 6.15768576, -1.96963215, 0.0538608842, 5.76845932, -2.01406097, 0.0461910777, 6.15089035, -2.00516129, 0.0912266672, 6.78896236, -1.86905718, 0.0813975111, 6.85818863, -1.88040352, 0.0542809404, 6.54096746, -1.96076524, 0.0770952329, 6.49532175, -1.93220913, 0.0579016767, 6.15315151, -1.99348676, 0.0664470047, 6.52838182, -1.95125997 \ No newline at end of file diff --git a/include/box3d/base.h b/include/box3d/base.h index 1a52fb9..8fcf19d 100644 --- a/include/box3d/base.h +++ b/include/box3d/base.h @@ -4,6 +4,7 @@ #pragma once #include +#include #include // Compile-time options. Edit box3d/config.h, or define BOX3D_USER_CONFIG to @@ -93,11 +94,12 @@ /// Prototype for user allocation function. /// @param size the allocation size in bytes /// @param alignment the required alignment, guaranteed to be a power of 2 -typedef void* b3AllocFcn( int32_t size, int32_t alignment ); +typedef void* b3AllocFcn( size_t size, int32_t alignment ); /// Prototype for user free function. /// @param mem the memory previously allocated through `b3AllocFcn` -typedef void b3FreeFcn( void* mem ); +/// @param size the size of the allocation in bytes. +typedef void b3FreeFcn( void* mem, size_t size ); /// Prototype for the user assert callback. Return 0 to skip the debugger break. typedef int b3AssertFcn( const char* condition, const char* fileName, int lineNumber ); @@ -110,7 +112,7 @@ typedef void b3LogFcn( const char* message ); B3_API void b3SetAllocator( b3AllocFcn* allocFcn, b3FreeFcn* freeFcn ); /// Total bytes allocated by Box3D -B3_API int b3GetByteCount( void ); +B3_API int64_t b3GetByteCount( void ); /// Override the default assert callback. /// @param assertFcn a non-null assert callback diff --git a/include/box3d/config.h b/include/box3d/config.h index 1309827..f39eb7e 100644 --- a/include/box3d/config.h +++ b/include/box3d/config.h @@ -31,5 +31,3 @@ // Enable assertions when NDEBUG is defined // #define B3_ENABLE_ASSERT - -// #define B3_RESTITUTION_ITERATIONS diff --git a/include/box3d/constants.h b/include/box3d/constants.h index 320e725..288b5d8 100644 --- a/include/box3d/constants.h +++ b/include/box3d/constants.h @@ -141,11 +141,6 @@ B3_API float b3GetStallThreshold( void ); #define B3_MAX_SHAPES ( 1 << B3_SHAPE_POWER ) #define B3_MAX_CHILD_SHAPES ( 1 << B3_CHILD_POWER ) -/// Increase this if your application needs more accurate restitution. Doing so will -/// slow down the simulation. Must be 1 or more. -#ifndef B3_RESTITUTION_ITERATIONS -#define B3_RESTITUTION_ITERATIONS 1 -#endif /// This is the limit on how many mesh or heightfield triangles a single convex shape can collide with. /// Increasing this will increase stack usage, so be careful. I recommend to simplify your collision data diff --git a/include/box3d/types.h b/include/box3d/types.h index ed4a1d9..08358b5 100644 --- a/include/box3d/types.h +++ b/include/box3d/types.h @@ -537,69 +537,70 @@ typedef struct b3ShapeDef /// @ingroup shape B3_API b3ShapeDef b3DefaultShapeDef( void ); -//! @cond /// Profiling data. Times are in milliseconds. /// @ingroup world typedef struct b3Profile { - float step; - float pairs; - float collide; - float solve; - float solverSetup; - float constraints; - float prepareConstraints; - float integrateVelocities; - float warmStart; - float solveImpulses; - float integratePositions; - float relaxImpulses; - float applyRestitution; - float storeImpulses; - float splitIslands; - float transforms; - float sensorHits; - float jointEvents; - float hitEvents; - float refit; - float bullets; - float sleepIslands; - float sensors; + float step; // + float pairs; // + float collide; // + float solve; // + float solverSetup; // + float constraints; // + float prepareConstraints; // + float integrateVelocities; // + float warmStart; // + float solveImpulses; // + float integratePositions; // + float relaxImpulses; // + float storeImpulses; // + float splitIslands; // + float transforms; // + float sensorHits; // + float jointEvents; // + float hitEvents; // + float refit; // + float bullets; // + float sleepIslands; // + float sensors; // } b3Profile; /// Counters that give details of the simulation size. /// @ingroup world typedef struct b3Counters { - int bodyCount; - int shapeCount; - int contactCount; - int jointCount; - int islandCount; - int stackUsed; - int arenaCapacity; - int staticTreeHeight; - int treeHeight; - int satCallCount; - int satCacheHitCount; - int byteCount; - int taskCount; - int colorCounts[24]; - int manifoldCounts[B3_CONTACT_MANIFOLD_COUNT_BUCKETS]; + int64_t byteCount; // + int bodyCount; // + int shapeCount; // + int contactCount; // + int jointCount; // + int islandCount; // + int stackUsed; // + int arenaCapacity; // + int staticTreeHeight; // + int treeHeight; // + int satCallCount; // + int satCacheHitCount; // + int taskCount; // + int colorCounts[24]; // + int manifoldCounts[B3_CONTACT_MANIFOLD_COUNT_BUCKETS]; // /// Number of contacts touched by the collide pass /// graph contacts + awake-set non-touching - int awakeContactCount; + int awakeContactCount; // /// Number of contacts recycled in the most recent step. int recycledContactCount; - /// Maximum number of time of impact iterations + /// Maximum number of time of impact outer iterations int distanceIterations; + + /// Maximum TOI push backs. int pushBackIterations; + + /// Maximum TOI root solver iterations. int rootIterations; } b3Counters; -//! @endcond /// Joint type enumeration. This is useful because all joint types use b3JointId and sometimes you /// want to get the type of a joint. @@ -2651,12 +2652,14 @@ typedef struct b3ManifoldPoint /// result from the final sub-step. float normalImpulse; - /// The total normal impulse applied during sub-stepping. This is important + /// The total normal impulse applied across sub-stepping and restitution. This is important /// to identify speculative contact points that had an interaction in the time step. + /// This includes the warm starting impulse, the sub-step delta impulse, and the restitution + /// impulse. float totalNormalImpulse; - /// Relative normal velocity pre-solve. Used for hit events. If the normal impulse is - /// zero then there was no hit. Negative means shapes are approaching. + /// Relative normal velocity pre-solve. Negative when approaching. This is only + /// computed if hit events are enabled. float normalVelocity; /// Local point for matching diff --git a/samples/sample.cpp b/samples/sample.cpp index 01ca311..caf879d 100644 --- a/samples/sample.cpp +++ b/samples/sample.cpp @@ -621,7 +621,7 @@ void Sample::DrawMetrics() int count = m_profileWriteIndex - m_profileReadIndex; // Unroll ring buffer into per-field histories. - constexpr int kRowCount = 22; + constexpr int kRowCount = 21; float histories[kRowCount][m_profileCapacity]; float totals[kRowCount] = {}; for ( int i = 0; i < count; ++i ) @@ -640,16 +640,15 @@ void Sample::DrawMetrics() histories[9][i] = p.solveImpulses; histories[10][i] = p.integratePositions; histories[11][i] = p.relaxImpulses; - histories[12][i] = p.applyRestitution; - histories[13][i] = p.storeImpulses; - histories[14][i] = p.splitIslands; - histories[15][i] = p.transforms; - histories[16][i] = p.jointEvents; - histories[17][i] = p.hitEvents; - histories[18][i] = p.refit; - histories[19][i] = p.sleepIslands; - histories[20][i] = p.bullets; - histories[21][i] = p.sensors; + histories[12][i] = p.storeImpulses; + histories[13][i] = p.splitIslands; + histories[14][i] = p.transforms; + histories[15][i] = p.jointEvents; + histories[16][i] = p.hitEvents; + histories[17][i] = p.refit; + histories[18][i] = p.sleepIslands; + histories[19][i] = p.bullets; + histories[20][i] = p.sensors; totals[0] += p.step; totals[1] += p.pairs; @@ -663,16 +662,15 @@ void Sample::DrawMetrics() totals[9] += p.solveImpulses; totals[10] += p.integratePositions; totals[11] += p.relaxImpulses; - totals[12] += p.applyRestitution; - totals[13] += p.storeImpulses; - totals[14] += p.splitIslands; - totals[15] += p.transforms; - totals[16] += p.jointEvents; - totals[17] += p.hitEvents; - totals[18] += p.refit; - totals[19] += p.sleepIslands; - totals[20] += p.bullets; - totals[21] += p.sensors; + totals[12] += p.storeImpulses; + totals[13] += p.splitIslands; + totals[14] += p.transforms; + totals[15] += p.jointEvents; + totals[16] += p.hitEvents; + totals[17] += p.refit; + totals[18] += p.sleepIslands; + totals[19] += p.bullets; + totals[20] += p.sensors; } // Smoothed over the last few frames so bars don't jitter visibly. @@ -728,13 +726,13 @@ void Sample::DrawMetrics() const ImU32 colorDefault = IM_COL32( 220, 220, 220, 255 ); const RowDef rows[kRowCount] = { - { "step", 0, colorStep }, { "pairs", 0, colorPairs }, { "collide", 0, colorCollide }, - { "solve", 0, colorSolve }, { "setup", 1, colorDefault }, { "constraints", 1, colorDefault }, - { "prepare", 2, colorDefault }, { "velocities", 2, colorDefault }, { "warm start", 2, colorDefault }, - { "bias", 2, colorDefault }, { "positions", 2, colorDefault }, { "relax", 2, colorDefault }, - { "restitution", 2, colorDefault }, { "store", 2, colorDefault }, { "split islands", 2, colorDefault }, - { "transforms", 1, colorDefault }, { "joint events", 1, colorDefault }, { "hit events", 1, colorDefault }, - { "refit BVH", 1, colorDefault }, { "sleep", 1, colorDefault }, { "bullets", 1, colorDefault }, + { "step", 0, colorStep }, { "pairs", 0, colorPairs }, { "collide", 0, colorCollide }, + { "solve", 0, colorSolve }, { "setup", 1, colorDefault }, { "constraints", 1, colorDefault }, + { "prepare", 2, colorDefault }, { "velocities", 2, colorDefault }, { "warm start", 2, colorDefault }, + { "bias", 2, colorDefault }, { "positions", 2, colorDefault }, { "relax", 2, colorDefault }, + { "store", 2, colorDefault }, { "split islands", 2, colorDefault }, + { "transforms", 1, colorDefault }, { "joint events", 1, colorDefault }, { "hit events", 1, colorDefault }, + { "refit BVH", 1, colorDefault }, { "sleep", 1, colorDefault }, { "bullets", 1, colorDefault }, { "sensors", 0, colorSensors }, }; @@ -955,7 +953,7 @@ void Sample::DrawMetrics() ImGui::Text( "toi d/p/r = %d/%d/%d", s.distanceIterations, s.pushBackIterations, s.rootIterations ); ImGui::Text( "stack allocator size = %d K", s.stackUsed / 1024 ); ImGui::Text( "arena capacity = %d K", s.arenaCapacity / 1024 ); - ImGui::Text( "total allocation = %d K", s.byteCount / 1024 ); + ImGui::Text( "total allocation = %d K", (int)(s.byteCount / 1024) ); ImGui::Separator(); b3Capacity c = b3World_GetMaxCapacity( m_worldId ); @@ -2175,3 +2173,24 @@ float CastClosestCallback( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float rayContext->hit = true; return fraction; } + +MechanicalEnergy MeasureEnergy( b3WorldId worldId, const b3BodyId* bodyIds, int count ) +{ + b3Vec3 gravity = b3World_GetGravity( worldId ); + + MechanicalEnergy energy = {}; + for ( int i = 0; i < count; ++i ) + { + b3MassData massData = b3Body_GetMassData( bodyIds[i] ); + b3Vec3 v = b3Body_GetLinearVelocity( bodyIds[i] ); + + // The inertia tensor is in the body frame, so bring the angular velocity back to it + b3Vec3 w = b3InvRotateVector( b3Body_GetRotation( bodyIds[i] ), b3Body_GetAngularVelocity( bodyIds[i] ) ); + + energy.linear += 0.5f * massData.mass * b3Dot( v, v ); + energy.angular += 0.5f * b3Dot( w, b3MulMV( massData.inertia, w ) ); + energy.potential -= massData.mass * b3Dot( gravity, b3ToVec3( b3Body_GetWorldCenter( bodyIds[i] ) ) ); + } + + return energy; +} diff --git a/samples/sample.h b/samples/sample.h index 2cc3f25..b283b3b 100644 --- a/samples/sample.h +++ b/samples/sample.h @@ -294,3 +294,19 @@ struct CastClosestContext float CastClosestCallback( b3ShapeId shapeId, b3Pos point, b3Vec3 normal, float fraction, uint64_t materialId, int triangleIndex, int childIndex, void* context ); + +struct MechanicalEnergy +{ + float Total() const + { + return linear + angular + potential; + } + + float linear; + float angular; + float potential; +}; + +// Kinetic and gravitational potential energy summed over the bodies. Restitution samples +// use the total as the invariant that may only decrease. +MechanicalEnergy MeasureEnergy( b3WorldId worldId, const b3BodyId* bodyIds, int count ); diff --git a/samples/sample_issues.cpp b/samples/sample_issues.cpp index e03138c..05fa85a 100644 --- a/samples/sample_issues.cpp +++ b/samples/sample_issues.cpp @@ -8,6 +8,10 @@ #include "box3d/box3d.h" +#include +#include +#include +#include #include class Crash : public Sample @@ -132,65 +136,164 @@ public: static int sampleMultiplePrismatic = RegisterSample( "Issues", "Multiple Prismatic", MultiplePrismatic::Create ); +// bad hull SM_Waterfall_MED_Wide_01 +static const b3Vec3 waterfallPoints[] = { + { 0.100183107, -498.925385, -1275.39966 }, { 0.100183107, -498.925415, 0.125000000 }, + { 0.100183107, 486.343750, 0.125000000 }, { 0.100183107, 486.343719, -1275.39966 }, + { -395.117462, 486.343781, -1462.43750 }, { -395.117462, 486.343750, -96.7426758 }, + { -395.117462, -498.925415, -96.7424469 }, { -395.117462, -498.925446, -1462.52612 }, + { -186.979691, 486.294891, -1462.47949 }, { -298.250000, 486.294891, 0.125000000 }, + { -395.121216, 486.294891, -1462.52612 }, { -186.984360, -498.913361, -1462.48413 }, + { -298.250000, -498.913361, 0.125000000 }, +}; + +static const b3Vec3 quadPoints[] = { + { 100.000000, -142.292389, 130.826111 }, { 99.5354385, -71.3011093, 130.826111 }, + { 99.5930862, -80.1112213, -100.000000 }, { 100.000000, -142.292389, -100.000000 }, + { 99.5930862, -80.1112213, 130.826111 }, +}; + +static const b3Vec3 thinBoxPoints[] = { + { -11.3861933, -24.2451687, -12.0037909 }, { -11.3889809, -24.2466526, -11.9013014 }, + { -11.3804407, -24.3151531, -12.0046492 }, { -11.3832273, -24.3166409, -11.9021587 }, + { -14.4396200, -24.3636723, -12.1324549 }, { -14.4432650, -24.3655701, -12.0299988 }, + { -14.4356947, -24.4337788, -12.1336164 }, { -14.4393377, -24.4356804, -12.0311594 }, +}; + +// Inline point sets are scaled from centimeters. A null point array is a file in data/hulls. +struct HullSource +{ + const char* name; + const b3Vec3* points; + int count; + float scale; +}; + +static const HullSource hullSources[] = { + { "waterfall", waterfallPoints, (int)std::size( waterfallPoints ), 0.01f }, + { "quad", quadPoints, (int)std::size( quadPoints ), 0.01f }, + { "thin box", thinBoxPoints, (int)std::size( thinBoxPoints ), 0.01f }, + { "axe.txt", nullptr, 0, 1.0f }, +}; + +static constexpr int hullSourceCount = (int)std::size( hullSources ); + +// Reads x, y, z triples separated by commas or whitespace. A C source repro is +// also accepted: only the first initializer list is parsed, so counts and other +// numbers in the surrounding code are ignored. +static bool LoadHullPoints( const char* path, std::vector* points ) +{ + points->clear(); + + FILE* file = fopen( path, "rb" ); + if ( file == nullptr ) + { + fprintf( stderr, "Failed to open '%s'\n", path ); + return false; + } + + fseek( file, 0, SEEK_END ); + long size = ftell( file ); + fseek( file, 0, SEEK_SET ); + if ( size < 0 ) + { + fprintf( stderr, "Failed to read '%s'\n", path ); + fclose( file ); + return false; + } + + std::vector text( size + 1 ); + size_t readCount = fread( text.data(), 1, size, file ); + fclose( file ); + text[readCount] = '\0'; + + char* begin = text.data(); + char* end = begin + readCount; + for ( char* c = begin; *c != '\0'; ++c ) + { + if ( *c != '{' ) + { + continue; + } + + char* prev = c - 1; + while ( prev >= begin && isspace( (unsigned char)*prev ) ) + { + --prev; + } + + if ( prev < begin || *prev != '=' ) + { + continue; + } + + begin = c + 1; + int depth = 1; + for ( end = begin; *end != '\0'; ++end ) + { + if ( *end == '{' ) + { + depth += 1; + } + else if ( *end == '}' && --depth == 0 ) + { + break; + } + } + + break; + } + + *end = '\0'; + + std::vector values; + char* cursor = begin; + while ( *cursor != '\0' ) + { + char* stop; + float value = strtof( cursor, &stop ); + if ( stop == cursor ) + { + cursor += 1; + continue; + } + + values.push_back( value ); + cursor = stop; + } + + if ( values.size() % 3 != 0 ) + { + fprintf( stderr, "'%s' has %d values, not a multiple of 3\n", path, (int)values.size() ); + } + + int count = (int)values.size() / 3; + points->resize( count ); + for ( int i = 0; i < count; ++i ) + { + ( *points )[i] = { values[3 * i], values[3 * i + 1], values[3 * i + 2] }; + } + + return count > 0; +} + class HullCrash : public Sample { public: explicit HullCrash( SampleContext* context ) : Sample( context ) { + m_hull = nullptr; + m_sourceIndex = 3; + m_maxVertexCount = B3_MAX_HULL_VERTICES; + + LoadPoints(); + CreateHull(); + if ( m_context->restart == false ) { - m_camera->SetView( 0.0f, 15.0f, 5.0f, b3Pos_zero ); + FrameView(); } - - m_hull = nullptr; - -#if 0 - // bad hull SM_Waterfall_MED_Wide_01 - b3Vec3 points[] = { - { 0.100183107, -498.925385, -1275.39966 }, { 0.100183107, -498.925415, 0.125000000 }, - { 0.100183107, 486.343750, 0.125000000 }, { 0.100183107, 486.343719, -1275.39966 }, - { -395.117462, 486.343781, -1462.43750 }, { -395.117462, 486.343750, -96.7426758 }, - { -395.117462, -498.925415, -96.7424469 }, { -395.117462, -498.925446, -1462.52612 }, - { -186.979691, 486.294891, -1462.47949 }, { -298.250000, 486.294891, 0.125000000 }, - { -395.121216, 486.294891, -1462.52612 }, { -186.984360, -498.913361, -1462.48413 }, - { -298.250000, -498.913361, 0.125000000 }, - }; - -#elif 1 - b3Vec3 points[] = { - { 100.000000, -142.292389, 130.826111 }, { 99.5354385, -71.3011093, 130.826111 }, - { 99.5930862, -80.1112213, -100.000000 }, { 100.000000, -142.292389, -100.000000 }, - { 99.5930862, -80.1112213, 130.826111 }, - }; -#else - b3Vec3 points[] = { - { -11.3861933, -24.2451687, -12.0037909 }, { -11.3889809, -24.2466526, -11.9013014 }, - { -11.3804407, -24.3151531, -12.0046492 }, { -11.3832273, -24.3166409, -11.9021587 }, - { -14.4396200, -24.3636723, -12.1324549 }, { -14.4432650, -24.3655701, -12.0299988 }, - { -14.4356947, -24.4337788, -12.1336164 }, { -14.4393377, -24.4356804, -12.0311594 }, - }; -#endif - - static_assert( sizeof( points ) / sizeof( points[0] ) < m_capacity, "bad" ); - - m_count = sizeof( points ) / sizeof( points[0] ); - for ( int i = 0; i < m_count; ++i ) - { - m_points[i] = 0.01f * points[i]; - } - - // This shift shouldn't be necessary but I'm doing it so the hull - // appears on the screen. - // for ( int i = 0; i < m_count; ++i ) - //{ - // m_points[i] -= m_points[0]; - // m_points[i] *= 0.01f; - //} - - m_hull = b3CreateHull( m_points, m_count, m_count ); - - (void)m_hull; } ~HullCrash() override @@ -201,6 +304,94 @@ public: } } + void LoadPoints() + { + const HullSource& source = hullSources[m_sourceIndex]; + if ( source.points != nullptr ) + { + m_points.assign( source.points, source.points + source.count ); + } + else + { + char path[64]; + snprintf( path, sizeof( path ), "data/hulls/%s", source.name ); + LoadHullPoints( path, &m_points ); + } + + for ( b3Vec3& point : m_points ) + { + point = source.scale * point; + } + + // The reported failure used the point count as the limit + m_maxVertexCount = b3ClampInt( (int)m_points.size(), 4, B3_MAX_HULL_VERTICES ); + } + + void CreateHull() + { + if ( m_hull != nullptr ) + { + b3DestroyHull( m_hull ); + m_hull = nullptr; + } + + int count = (int)m_points.size(); + if ( count >= 4 ) + { + m_hull = b3CreateHull( m_points.data(), count, m_maxVertexCount ); + } + } + + void FrameView() + { + if ( m_points.empty() ) + { + return; + } + + b3AABB bounds = b3MakeAABB( m_points.data(), (int)m_points.size(), 0.0f ); + float radius = b3Length( b3AABB_Extents( bounds ) ); + m_camera->SetView( 0.0f, 15.0f, b3MaxFloat( 3.0f * radius, 1.0f ), b3ToPos( b3AABB_Center( bounds ) ) ); + } + + bool DrawControls() override + { + DrawTextLine( "points = %d", (int)m_points.size() ); + + if ( m_hull != nullptr ) + { + DrawTextLine( "hull: vertices = %d, edges = %d, faces = %d", m_hull->vertexCount, m_hull->edgeCount / 2, + m_hull->faceCount ); + } + else + { + DrawTextLine( "hull creation failed" ); + } + + if ( ImGui::BeginCombo( "Source", hullSources[m_sourceIndex].name ) ) + { + for ( int i = 0; i < hullSourceCount; ++i ) + { + if ( ImGui::Selectable( hullSources[i].name, i == m_sourceIndex ) ) + { + m_sourceIndex = i; + LoadPoints(); + CreateHull(); + FrameView(); + } + } + + ImGui::EndCombo(); + } + + if ( ImGui::SliderInt( "Max Vertices", &m_maxVertexCount, 4, B3_MAX_HULL_VERTICES ) ) + { + CreateHull(); + } + + return true; + } + void Render() override { if ( m_hull != nullptr ) @@ -209,9 +400,9 @@ public: } else { - for ( int i = 0; i < m_count; ++i ) + for ( const b3Vec3& point : m_points ) { - DrawPoint( b3ToPos( m_points[i] ), 5.0f, MakeColor( b3_colorWhite ) ); + DrawPoint( b3ToPos( point ), 5.0f, MakeColor( b3_colorWhite ) ); } } @@ -225,10 +416,10 @@ public: return new HullCrash( sampleContext ); } - static constexpr int m_capacity = 64; + std::vector m_points; b3HullData* m_hull; - b3Vec3 m_points[m_capacity]; - int m_count; + int m_sourceIndex; + int m_maxVertexCount; }; static int sampleHullCrash = RegisterSample( "Issues", "Hull Crash", HullCrash::Create ); @@ -1146,6 +1337,7 @@ public: static int sampleWheelStack = RegisterSample( "Issues", "GMod Wheel Stack", WheelStack::Create ); +// There was a problem where this bouncing box could gain energy. This has been fixed by the deferred restitution update. class RestitutionOvershoot : public Sample { public: @@ -1176,6 +1368,7 @@ public: b3BodyDef boxDef = b3DefaultBodyDef(); boxDef.type = b3_dynamicBody; boxDef.position = { 0.0f, m_dropHeight, 0.0f }; + boxDef.safetyFactor = 0.1f; m_boxBody = b3CreateBody( m_worldId, &boxDef ); b3ShapeDef boxShape = b3DefaultShapeDef(); @@ -1186,6 +1379,9 @@ public: m_maxBounceY = 0.0f; m_bounced = false; m_failed = false; + m_startEnergy = MeasureEnergy( m_worldId, &m_boxBody, 1 ).Total(); + m_maxEnergy = m_startEnergy; + m_energyFailed = false; } void Step() override @@ -1221,9 +1417,29 @@ public: b3Pos markerPoint = { 0.0f, m_dropHeight + m_boxHalf, 0.0f }; DrawPlane( b3Vec3_axisY, markerPoint, MakeColor( b3_colorYellow ) ); + // Perfect restitution and no friction, so the total energy may only decrease. It is the honest + // invariant here: the height check cannot tell a bounce that gained potential energy from one + // that converted it into spin, and this drop does both. + MechanicalEnergy energy = MeasureEnergy( m_worldId, &m_boxBody, 1 ); + float total = energy.Total(); + + if ( total > m_maxEnergy ) + { + m_maxEnergy = total; + } + + if ( m_bounced && total > m_startEnergy * ( 1.0f + m_energyTolerance ) ) + { + m_energyFailed = true; + } + DrawTextLine( "drop height = %.2f m", m_dropHeight ); DrawTextLine( "current y = %.2f m", m_currentY ); DrawTextLine( "max bounce = %.2f m", m_maxBounceY ); + DrawTextLine( "kinetic = %.0f J linear + %.0f J angular", energy.linear, energy.angular ); + DrawTextLine( "potential = %.0f J", energy.potential ); + DrawTextLine( "total = %.0f J (%.2f%% of start, peak %.2f%%)", total, 100.0f * total / m_startEnergy, + 100.0f * m_maxEnergy / m_startEnergy ); if ( m_bounced == false ) { @@ -1237,6 +1453,15 @@ public: { DrawTextLine( "PASS: bounce stays at or below drop height" ); } + + if ( m_energyFailed ) + { + DrawTextLine( "FAIL: total energy increased" ); + } + else if ( m_bounced ) + { + DrawTextLine( "PASS: total energy did not increase" ); + } } static Sample* Create( SampleContext* context ) @@ -1244,11 +1469,16 @@ public: return new RestitutionOvershoot( context ); } + static constexpr float m_energyTolerance = 0.001f; + b3BodyId m_boxBody = {}; float m_currentY = 0.0f; float m_maxBounceY = 0.0f; + float m_startEnergy = 0.0f; + float m_maxEnergy = 0.0f; bool m_bounced = false; bool m_failed = false; + bool m_energyFailed = false; }; static int sampleRestitutionOvershoot = RegisterSample( "Issues", "Restitution Overshoot", RestitutionOvershoot::Create ); diff --git a/samples/sample_shapes.cpp b/samples/sample_shapes.cpp index 15e80da..ba9b834 100644 --- a/samples/sample_shapes.cpp +++ b/samples/sample_shapes.cpp @@ -1,6 +1,7 @@ // SPDX-FileCopyrightText: 2025 Erin Catto // SPDX-License-Identifier: MIT +#include "gfx/debug_adapter.h" #include "gfx/draw.h" #include "human.h" #include "imgui.h" @@ -336,6 +337,528 @@ public: static int sampleRestitution = RegisterSample( "Shapes", "Restitution", Restitution::Create ); + +// Similar to the MeasureSupportedBounce unit test +class SphereStackRestitution : public Sample +{ +public: + static constexpr float m_impactSpeed = 5.0f; + static constexpr int m_maxStackCount = 3; + + explicit SphereStackRestitution( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 20.0f, 10.0f, 12.0f, { 0.0f, 2.5f, 0.0f } ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.0f; + shapeDef.baseMaterial.restitution = 0.0f; + b3BoxHull box = b3MakeBoxHull( 40.0f, 1.0f, 40.0f ); + b3ShapeId groundShapeId = b3CreateHullShape( groundId, &shapeDef, &box.base ); + SetGroundShape( groundShapeId ); + + CreateScene(); + } + + b3BodyId CreateBall( float y, float velocityY, float restitution, bool hitEvents ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, y, 0.0f }; + bodyDef.linearVelocity = { 0.0f, velocityY, 0.0f }; + bodyDef.enableSleep = false; + b3BodyId bodyId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.0f; + shapeDef.baseMaterial.restitution = restitution; + shapeDef.enableHitEvents = hitEvents; + b3Sphere sphere = { b3Vec3_zero, 0.5f }; + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + + return bodyId; + } + + void CreateScene() + { + for ( int i = 0; i < m_bodyCount; ++i ) + { + b3DestroyBody( m_bodyIds[i] ); + } + m_bodyCount = 0; + + if ( m_gravity ) + { + b3World_SetGravity( m_worldId, { 0.0f, -10.0f, 0.0f } ); + } + else + { + b3World_SetGravity( m_worldId, b3Vec3_zero ); + } + + for ( int i = 0; i < m_stackCount; ++i ) + { + m_bodyIds[m_bodyCount] = CreateBall( 0.5f + 1.0f * i, 0.0f, 0.0f, false ); + m_bodyCount += 1; + } + + // Start half a step of travel above contact so the impact lands mid step, matching the test + m_startHeight = 0.5f + 1.0f * m_stackCount + 0.5f * m_impactSpeed * ( 1.0f / 60.0f ); + + m_impactorId = CreateBall( m_startHeight, -m_impactSpeed, m_restitution, true ); + m_bodyIds[m_bodyCount] = m_impactorId; + m_bodyCount += 1; + + m_startEnergy = MeasureEnergy( m_worldId, m_bodyIds, m_bodyCount ).Total(); + m_peakEnergy = m_startEnergy; + + m_coefficient = 0.0f; + m_latched = false; + m_hit = false; + } + + bool DrawControls() override + { + bool rebuild = false; + + ImGui::PushItemWidth( 6.0f * ImGui::GetFontSize() ); + + if ( ImGui::SliderFloat( "Restitution", &m_restitution, 0.0f, 1.0f, "%.2f" ) ) + { + rebuild = true; + } + + if ( ImGui::SliderInt( "Stack", &m_stackCount, 0, m_maxStackCount ) ) + { + rebuild = true; + } + + ImGui::PopItemWidth(); + + if ( ImGui::Checkbox( "Gravity", &m_gravity ) ) + { + rebuild = true; + } + + if ( ImGui::Button( "Reset" ) ) + { + rebuild = true; + } + + if ( rebuild ) + { + CreateScene(); + } + + return true; + } + + void Step() override + { + Sample::Step(); + + b3ContactEvents events = b3World_GetContactEvents( m_worldId ); + if ( events.hitCount > 0 ) + { + m_hit = true; + } + + float vy = b3Body_GetLinearVelocity( m_impactorId ).y; + + // The rebound only means something once the impact has happened and the impactor is leaving + if ( m_latched == false && m_hit && vy > 0.0f ) + { + m_coefficient = vy / m_impactSpeed; + m_latched = true; + } + + float supportSpeed = 0.0f; + for ( int i = 0; i < m_stackCount; ++i ) + { + supportSpeed = b3MaxFloat( supportSpeed, b3Length( b3Body_GetLinearVelocity( m_bodyIds[i] ) ) ); + } + + DrawLine( { -2.0f, m_startHeight, 0.0f }, { 2.0f, m_startHeight, 0.0f }, MakeColor( b3_colorRed ) ); + + MechanicalEnergy energy = MeasureEnergy( m_worldId, m_bodyIds, m_bodyCount ); + float total = energy.Total(); + m_peakEnergy = b3MaxFloat( m_peakEnergy, total ); + + DrawTextLine( "impactor vy = %.3f m/s", vy ); + + DrawTextLine( "coefficient = %.4f live (target %.2f)", vy / m_impactSpeed, m_restitution ); + + if ( m_latched ) + { + DrawTextLine( "coefficient = %.4f at first rebound", m_coefficient ); + } + + DrawTextLine( "support speed = %.4f m/s", supportSpeed ); + + float scale = m_startEnergy != 0.0f ? 100.0f / m_startEnergy : 0.0f; + DrawTextLine( "kinetic = %.3f J linear + %.3f J angular", energy.linear, energy.angular ); + DrawTextLine( "potential = %.3f J", energy.potential ); + DrawTextLine( "total = %.3f J (%.2f%% of start, peak %.2f%%)", total, scale * total, scale * m_peakEnergy ); + } + + static Sample* Create( SampleContext* context ) + { + return new SphereStackRestitution( context ); + } + + b3BodyId m_bodyIds[m_maxStackCount + 1] = {}; + b3BodyId m_impactorId = b3_nullBodyId; + int m_bodyCount = 0; + int m_stackCount = 2; + float m_restitution = 1.0f; + float m_startHeight = 0.0f; + float m_coefficient = 0.0f; + float m_startEnergy = 0.0f; + float m_peakEnergy = 0.0f; + bool m_gravity = false; + bool m_latched = false; + bool m_hit = false; +}; + +static int sampleSphereStackRestitution = RegisterSample( "Shapes", "Sphere Stack Restitution", SphereStackRestitution::Create ); + +// Similar to the MeasureFlatBounce and SpinTest unit tests +class BoxRestitution : public Sample +{ +public: + static constexpr float m_impactSpeed = 5.0f; + + explicit BoxRestitution( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 20.0f, 10.0f, 14.0f, { 0.0f, 3.0f, 0.0f } ); + } + + // Gravity would bias the measured coefficient + b3World_SetGravity( m_worldId, b3Vec3_zero ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.0f; + shapeDef.baseMaterial.restitution = 0.0f; + b3BoxHull box = b3MakeBoxHull( 40.0f, 1.0f, 40.0f ); + b3ShapeId groundShapeId = b3CreateHullShape( groundId, &shapeDef, &box.base ); + SetGroundShape( groundShapeId ); + + CreateScene(); + } + + void CreateScene() + { + if ( B3_IS_NON_NULL( m_boxId ) ) + { + b3DestroyBody( m_boxId ); + } + + // Start half a step of travel above contact so the impact lands mid step, matching the test + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 0.25f + 0.5f * m_impactSpeed * ( 1.0f / 60.0f ), 0.0f }; + bodyDef.linearVelocity = { 0.0f, -m_impactSpeed, 0.0f }; + bodyDef.angularVelocity = { 0.0f, 0.0f, m_spin }; + bodyDef.enableSleep = false; + m_boxId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.baseMaterial.friction = 0.0f; + shapeDef.baseMaterial.restitution = m_restitution; + shapeDef.enableHitEvents = true; + b3BoxHull box = b3MakeBoxHull( 1.0f, 0.25f, 1.0f ); + b3CreateHullShape( m_boxId, &shapeDef, &box.base ); + + m_startEnergy = MeasureEnergy( m_worldId, &m_boxId, 1 ).Total(); + m_peakEnergy = m_startEnergy; + + m_coefficient = 0.0f; + m_latched = false; + m_hit = false; + } + + bool DrawControls() override + { + bool rebuild = false; + + ImGui::PushItemWidth( 6.0f * ImGui::GetFontSize() ); + + if ( ImGui::SliderFloat( "Restitution", &m_restitution, 0.0f, 1.0f, "%.2f" ) ) + { + rebuild = true; + } + + if ( ImGui::SliderFloat( "Spin", &m_spin, 0.0f, 2.0f, "%.2f" ) ) + { + rebuild = true; + } + + ImGui::PopItemWidth(); + + if ( ImGui::Button( "Reset" ) ) + { + rebuild = true; + } + + if ( rebuild ) + { + CreateScene(); + } + + return true; + } + + void Step() override + { + Sample::Step(); + + b3ContactEvents events = b3World_GetContactEvents( m_worldId ); + if ( events.hitCount > 0 ) + { + m_hit = true; + } + + float vy = b3Body_GetLinearVelocity( m_boxId ).y; + b3Vec3 w = b3Body_GetAngularVelocity( m_boxId ); + + if ( m_latched == false && m_hit && vy > 0.0f ) + { + m_coefficient = vy / m_impactSpeed; + m_latched = true; + } + + Vec4 yellow = MakeColor( b3_colorYellow ); + DrawPoint( b3Body_GetWorldPoint( m_boxId, { -1.0f, -0.25f, -1.0f } ), 8.0f, yellow ); + DrawPoint( b3Body_GetWorldPoint( m_boxId, { 1.0f, -0.25f, -1.0f } ), 8.0f, yellow ); + DrawPoint( b3Body_GetWorldPoint( m_boxId, { -1.0f, -0.25f, 1.0f } ), 8.0f, yellow ); + DrawPoint( b3Body_GetWorldPoint( m_boxId, { 1.0f, -0.25f, 1.0f } ), 8.0f, yellow ); + + MechanicalEnergy energy = MeasureEnergy( m_worldId, &m_boxId, 1 ); + float total = energy.Total(); + m_peakEnergy = b3MaxFloat( m_peakEnergy, total ); + + DrawTextLine( "vy = %.3f m/s", vy ); + + DrawTextLine( "coefficient = %.4f live (target %.2f)", vy / m_impactSpeed, m_restitution ); + + if ( m_latched ) + { + DrawTextLine( "coefficient = %.4f at first rebound", m_coefficient ); + } + + // The spin is about z, so any x or y component is residual from the sequential point solve + DrawTextLine( "spin in = %.2f, spin out = %.4f (ideal %.2f at e = 1)", m_spin, w.z, -m_spin ); + DrawTextLine( "off axis spin = %.4f rad/s", sqrtf( w.x * w.x + w.y * w.y ) ); + + float scale = m_startEnergy != 0.0f ? 100.0f / m_startEnergy : 0.0f; + DrawTextLine( "kinetic = %.3f J linear + %.3f J angular", energy.linear, energy.angular ); + DrawTextLine( "potential = %.3f J", energy.potential ); + DrawTextLine( "total = %.3f J (%.2f%% of start, peak %.2f%%)", total, scale * total, scale * m_peakEnergy ); + } + + static Sample* Create( SampleContext* context ) + { + return new BoxRestitution( context ); + } + + b3BodyId m_boxId = b3_nullBodyId; + float m_restitution = 0.9f; + float m_spin = 0.0f; + float m_coefficient = 0.0f; + float m_startEnergy = 0.0f; + float m_peakEnergy = 0.0f; + bool m_latched = false; + bool m_hit = false; +}; + +static int sampleBoxRestitution = RegisterSample( "Shapes", "Box Restitution", BoxRestitution::Create ); + +// Similar to the ImpulseTest unit test. The hit event marks the bounce and carries the approach speed, +// so the impulse the contact reports can be checked against reversing that speed at the coefficient. +// The total runs a little over because the contact also carries the weight for the part of the step +// it is active. +class BounceImpulse : public Sample +{ +public: + explicit BounceImpulse( SampleContext* context ) + : Sample( context ) + { + if ( context->restart == false ) + { + m_camera->SetView( 20.0f, 10.0f, 36.0f, { 0.0f, 8.0f, 0.0f } ); + } + + b3World_SetGravity( m_worldId, { 0.0f, -10.0f, 0.0f } ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = { 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.0f; + shapeDef.baseMaterial.restitution = 0.0f; + b3BoxHull box = b3MakeBoxHull( 40.0f, 1.0f, 40.0f ); + b3ShapeId groundShapeId = b3CreateHullShape( groundId, &shapeDef, &box.base ); + SetGroundShape( groundShapeId ); + + CreateScene(); + } + + void CreateScene() + { + if ( B3_IS_NON_NULL( m_ballId ) ) + { + b3DestroyBody( m_ballId ); + } + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = { 0.0f, 0.5f + m_dropHeight, 0.0f }; + bodyDef.enableSleep = false; + m_ballId = b3CreateBody( m_worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.0f; + shapeDef.baseMaterial.restitution = m_restitution; + shapeDef.enableHitEvents = true; + b3Sphere sphere = { b3Vec3_zero, 0.5f }; + b3CreateSphereShape( m_ballId, &shapeDef, &sphere ); + + m_mass = b3Body_GetMass( m_ballId ); + + m_approachSpeed = 0.0f; + m_totalImpulse = 0.0f; + m_expectedImpulse = 0.0f; + m_contactSteps = 0; + m_hit = false; + m_bouncing = false; + } + + bool DrawControls() override + { + bool rebuild = false; + + ImGui::PushItemWidth( 6.0f * ImGui::GetFontSize() ); + + if ( ImGui::SliderFloat( "Restitution", &m_restitution, 0.0f, 1.0f, "%.2f" ) ) + { + rebuild = true; + } + + if ( ImGui::SliderFloat( "Height", &m_dropHeight, 0.5f, 50.0f, "%.1f" ) ) + { + rebuild = true; + } + + ImGui::PopItemWidth(); + + if ( ImGui::Button( "Reset" ) ) + { + rebuild = true; + } + + if ( rebuild ) + { + CreateScene(); + } + + return true; + } + + void Step() override + { + Sample::Step(); + + if ( m_didStep ) + { + b3ContactEvents events = b3World_GetContactEvents( m_worldId ); + if ( m_hit == false && events.hitCount > 0 ) + { + m_hit = true; + m_bouncing = true; + m_approachSpeed = events.hitEvents[0].approachSpeed; + m_expectedImpulse = ( 1.0f + m_restitution ) * m_mass * m_approachSpeed; + } + + // Keep summing until the contact lets go, a slow bounce can take a few steps to leave + if ( m_bouncing ) + { + b3ContactData contactData[4]; + int contactCount = b3Body_GetContactData( m_ballId, contactData, 4 ); + for ( int c = 0; c < contactCount; ++c ) + { + for ( int m = 0; m < contactData[c].manifoldCount; ++m ) + { + const b3Manifold* manifold = contactData[c].manifolds + m; + for ( int p = 0; p < manifold->pointCount; ++p ) + { + m_totalImpulse += manifold->points[p].totalNormalImpulse; + } + } + } + + if ( contactCount > 0 ) + { + m_contactSteps += 1; + } + else + { + m_bouncing = false; + } + } + } + + float startHeight = 0.5f + m_dropHeight; + DrawLine( { -2.0f, startHeight, 0.0f }, { 2.0f, startHeight, 0.0f }, MakeColor( b3_colorRed ) ); + + DrawTextLine( "vy = %.3f m/s", b3Body_GetLinearVelocity( m_ballId ).y ); + + if ( m_hit ) + { + DrawTextLine( "approach speed = %.3f m/s", m_approachSpeed ); + DrawTextLine( "total impulse = %.2f N s over %d steps", m_totalImpulse, m_contactSteps ); + DrawTextLine( "expected impulse = %.2f N s", m_expectedImpulse ); + } + else + { + DrawTextLine( "waiting for the hit" ); + } + } + + static Sample* Create( SampleContext* context ) + { + return new BounceImpulse( context ); + } + + b3BodyId m_ballId = b3_nullBodyId; + float m_restitution = 0.5f; + float m_dropHeight = 20.0f; + float m_mass = 0.0f; + float m_approachSpeed = 0.0f; + float m_totalImpulse = 0.0f; + float m_expectedImpulse = 0.0f; + int m_contactSteps = 0; + bool m_hit = false; + bool m_bouncing = false; +}; + +static int sampleBounceImpulse = RegisterSample( "Shapes", "Bounce Impulse", BounceImpulse::Create ); + // This shows an optimization when creating many static shapes you can skip having them invoke collision, assuming // dynamic bodies are added after the static bodies. class StaticInvoke : public Sample diff --git a/src/body.c b/src/body.c index 935f9e6..c9cf3a8 100644 --- a/src/body.c +++ b/src/body.c @@ -69,18 +69,41 @@ b3BodyState* b3GetBodyState( b3World* world, b3Body* body ) return NULL; } +void b3RefreshBodyContactIndices( b3World* world, b3Body* body ) +{ + int encodedBodySimIndex = b3EncodeBodySimIndex( body ); + + int contactKey = body->headContactKey; + while ( contactKey != B3_NULL_INDEX ) + { + int edgeIndex = contactKey & 1; + int contactId = contactKey >> 1; + + b3Contact* contact = b3Array_Get( world->contacts, contactId ); + contactKey = contact->edges[edgeIndex].nextKey; + + if ( edgeIndex == 0 ) + { + contact->encodedBodySimA = encodedBodySimIndex; + } + else + { + contact->encodedBodySimB = encodedBodySimIndex; + } + } +} + void b3SyncBodyFlags( b3World* world, b3Body* body ) { - // Never sync transient flags - uint32_t flags = body->flags & ~b3_bodyTransientFlags; - b3BodySim* bodySim = b3GetBodySim( world, body ); - bodySim->flags = flags; + + // Preserve the fast flag for contact recycling. + bodySim->flags = ( bodySim->flags & b3_isFast ) | ( body->flags & ~b3_bodyTransientFlags ); b3BodyState* bodyState = b3GetBodyState( world, body ); if ( bodyState != NULL ) { - bodyState->flags = flags; + bodyState->flags = body->flags & ~b3_bodyTransientFlags; } } @@ -410,6 +433,7 @@ void b3DestroyBody( b3BodyId bodyId ) b3Body* movedBody = b3Array_Get( world->bodies, movedId ); B3_ASSERT( movedBody->localIndex == movedIndex ); movedBody->localIndex = body->localIndex; + b3RefreshBodyContactIndices( world, movedBody ); } // Remove body state from awake set @@ -1120,17 +1144,11 @@ void b3Body_SetTransform( b3BodyId bodyId, b3Pos position, b3Quat rotation ) b3AABB aabb = b3ComputeFatShapeAABB( shape, transform, speculativeDistance ); shape->aabb = aabb; - if ( b3AABB_Contains( shape->fatAABB, aabb ) == false ) + b3AABB* shapeFatAABB = world->fatAABBs.data + shapeId; + if ( b3AABB_Contains( *shapeFatAABB, aabb ) == false ) { - float margin = shape->aabbMargin; - b3AABB fatAABB; - fatAABB.lowerBound.x = aabb.lowerBound.x - margin; - fatAABB.lowerBound.y = aabb.lowerBound.y - margin; - fatAABB.lowerBound.z = aabb.lowerBound.z - margin; - fatAABB.upperBound.x = aabb.upperBound.x + margin; - fatAABB.upperBound.y = aabb.upperBound.y + margin; - fatAABB.upperBound.z = aabb.upperBound.z + margin; - shape->fatAABB = fatAABB; + b3AABB fatAABB = b3AABB_Inflate( aabb, shape->aabbMargin ); + *shapeFatAABB = fatAABB; // The body could be disabled if ( shape->proxyKey != B3_NULL_INDEX ) @@ -1717,7 +1735,7 @@ void b3Body_SetType( b3BodyId bodyId, b3BodyType type ) shapeId = shape->nextShapeId; b3DestroyShapeProxy( shape, &world->broadPhase ); bool forcePairCreation = true; - b3CreateShapeProxy( shape, &world->broadPhase, type, transform, forcePairCreation ); + b3CreateShapeProxy( world, shape, type, transform, forcePairCreation ); } // Relink all joints @@ -2263,7 +2281,7 @@ void b3Body_Enable( b3BodyId bodyId ) b3Shape* shape = b3Array_Get( world->shapes, shapeId ); shapeId = shape->nextShapeId; - b3CreateShapeProxy( shape, &world->broadPhase, proxyType, transform, forcePairCreation ); + b3CreateShapeProxy( world, shape, proxyType, transform, forcePairCreation ); } if ( setId != b3_staticSet ) diff --git a/src/body.h b/src/body.h index 484fecc..0ff4337 100644 --- a/src/body.h +++ b/src/body.h @@ -3,6 +3,8 @@ #pragma once +#include "physics_world.h" + #include "box3d/constants.h" #include "box3d/math_functions.h" #include "box3d/types.h" @@ -29,7 +31,7 @@ enum b3BodyFlags // This body has fixed rotation around the z-axis b3_lockAngularZ = 0x00000020, - // This flag is used for debug draw + // This flag is used for debug draw and contact recycling. Only lives on b3BodySim. b3_isFast = 0x00000040, // This dynamic body does a final CCD pass against all body types, but not other bullets @@ -44,8 +46,9 @@ enum b3BodyFlags // This body has no limit on angular velocity b3_allowFastRotation = 0x00000400, - // This body need's to have its AABB increased - b3_enlargeBounds = 0x00000800, + // This bullet body needs to have its AABB increased. Needed because bullets don't follow + // the standard broad-phase update. + b3_enlargeBulletBounds = 0x00000800, // This body is dynamic so the solver should write to it. // This prevents writing to kinematic bodies that causes a multithreaded sharing @@ -68,7 +71,7 @@ enum b3BodyFlags b3_fixedRotation = b3_lockAngularX | b3_lockAngularY | b3_lockAngularZ, // These flags are transient per time step. These may be different across b3Body, b3BodySim, and b3BodyState. - b3_bodyTransientFlags = b3_isFast | b3_isSpeedCapped | b3_hadTimeOfImpact, + b3_bodyTransientFlags = b3_isSpeedCapped | b3_hadTimeOfImpact, }; // Body organizational details that are not used in the solver. @@ -241,6 +244,38 @@ bool b3WakeBodyWithLock( b3World* world, b3Body* body ); void b3UpdateBodyMassData( b3World* world, b3Body* body ); void b3SyncBodyFlags( b3World* world, b3Body* body ); +void b3RefreshBodyContactIndices( b3World* world, b3Body* body ); + +// Encode the body sim index for storage in the contact. +static inline int b3EncodeBodySimIndex( const b3Body* body ) +{ + if ( body->setIndex == b3_awakeSet ) + { + return body->localIndex; + } + + if ( body->setIndex == b3_staticSet ) + { + return -( body->localIndex + 2 ); + } + + return B3_NULL_INDEX; +} + +static inline bool b3IsStaticSimIndex( int encodedBodySimIndex ) +{ + return encodedBodySimIndex < B3_NULL_INDEX; +} + +static inline int b3DecodeAwakeIndex( int encodedBodySimIndex ) +{ + return encodedBodySimIndex >= 0 ? encodedBodySimIndex : B3_NULL_INDEX; +} + +static inline int b3SleepBodySimIndex( int encodedBodySimIndex ) +{ + return encodedBodySimIndex >= 0 ? B3_NULL_INDEX : encodedBodySimIndex; +} // Make a sweep relative to a base position to keep TOI in float precision far from the origin. static inline b3Sweep b3MakeRelativeSweep( const b3BodySim* bodySim, b3Pos base ) diff --git a/src/constraint_graph.c b/src/constraint_graph.c index 9f06e9e..c5e3d3e 100644 --- a/src/constraint_graph.c +++ b/src/constraint_graph.c @@ -149,8 +149,8 @@ void b3AddContactToGraph( b3World* world, b3Contact* contact ) b3GraphColor* color = graph->colors + colorIndex; contact->colorIndex = colorIndex; contact->localIndex = isScalar ? color->contacts.count : color->convexContacts.count; - contact->bodySimIndexA = bodyA->type == b3_staticBody ? B3_NULL_INDEX : bodyA->localIndex; - contact->bodySimIndexB = bodyB->type == b3_staticBody ? B3_NULL_INDEX : bodyB->localIndex; + contact->encodedBodySimA = b3EncodeBodySimIndex( bodyA ); + contact->encodedBodySimB = b3EncodeBodySimIndex( bodyB ); if ( isScalar ) { diff --git a/src/contact.c b/src/contact.c index 8897706..dab7f1e 100644 --- a/src/contact.c +++ b/src/contact.c @@ -210,6 +210,8 @@ void b3CreateContact( b3World* world, b3Shape* shapeA, b3Shape* shapeB, int chil contact->shapeIdA = shapeIdA; contact->shapeIdB = shapeIdB; contact->childIndex = childIndex; + contact->encodedBodySimA = b3EncodeBodySimIndex( bodyA ); + contact->encodedBodySimB = b3EncodeBodySimIndex( bodyB ); // Both bodies must enable recycling if ( ( bodyA->flags & b3_bodyEnableContactRecycling ) != 0 && ( bodyB->flags & b3_bodyEnableContactRecycling ) != 0 ) diff --git a/src/contact.h b/src/contact.h index 4a29423..f1dd98c 100644 --- a/src/contact.h +++ b/src/contact.h @@ -126,9 +126,9 @@ typedef struct b3Contact // Back index into b3World::contacts int contactId; - // These are transient and cached for improved performance. B3_NULL_INDEX for static bodies. - int bodySimIndexA; - int bodySimIndexB; + // Encoded body sim indices to avoid a cache miss accessing the body in the narrow phase. + int encodedBodySimA; + int encodedBodySimB; // b3ContactFlags uint32_t flags; diff --git a/src/contact_solver.c b/src/contact_solver.c index f796549..d0a9ddc 100644 --- a/src/contact_solver.c +++ b/src/contact_solver.c @@ -33,7 +33,9 @@ void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context ) b3World* world = context->world; b3BodySim* bodySims = context->sims; - b3BodyState* bodyStates = context->states; + b3BodyState* states = context->states; + + float negRestitutionThreshold = -world->restitutionThreshold; float warmStartScale = world->enableWarmStarting ? 1.0f : 0.0f; @@ -82,69 +84,48 @@ void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context ) b3Contact* contact = b3Array_Get( world->contacts, contactId ); B3_ASSERT( contact->contactId == contactId ); - int indexA = contact->bodySimIndexA; - int indexB = contact->bodySimIndexB; + int indexA = b3DecodeAwakeIndex( contact->encodedBodySimA ); + int indexB = b3DecodeAwakeIndex( contact->encodedBodySimB ); #if B3_ENABLE_VALIDATION - if ( indexA != B3_NULL_INDEX ) { b3Body* bodyA = b3Array_Get( world->bodies, contact->edges[0].bodyId ); - B3_ASSERT( indexA == bodyA->localIndex ); - } - - if ( indexB != B3_NULL_INDEX ) - { b3Body* bodyB = b3Array_Get( world->bodies, contact->edges[1].bodyId ); - B3_ASSERT( indexB == bodyB->localIndex ); + B3_ASSERT( contact->encodedBodySimA == b3EncodeBodySimIndex( bodyA ) ); + B3_ASSERT( contact->encodedBodySimB == b3EncodeBodySimIndex( bodyB ) ); } #endif // Body A data float mA; b3Matrix3 iA; - b3Vec3 vA; - b3Vec3 wA; if ( indexA == B3_NULL_INDEX ) { mA = 0.0f; iA = b3Mat3_zero; - vA = b3Vec3_zero; - wA = b3Vec3_zero; } else { b3BodySim* simA = bodySims + indexA; mA = simA->invMass; iA = simA->invInertiaWorld; - - b3BodyState* stateA = bodyStates + indexA; - vA = stateA->linearVelocity; - wA = stateA->angularVelocity; } // Body B data float mB; b3Matrix3 iB; - b3Vec3 vB; - b3Vec3 wB; if ( indexB == B3_NULL_INDEX ) { mB = 0.0f; iB = b3Mat3_zero; - vB = b3Vec3_zero; - wB = b3Vec3_zero; } else { b3BodySim* simB = bodySims + indexB; mB = simB->invMass; iB = simB->invInertiaWorld; - - b3BodyState* stateB = bodyStates + indexB; - vB = stateB->linearVelocity; - wB = stateB->angularVelocity; } int manifoldCount = contact->manifoldCount; @@ -160,9 +141,33 @@ void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context ) contactConstraint->softness = ( contact->flags & b3_contactStaticFlag ) != 0 ? context->staticSoftness : context->contactSoftness; contactConstraint->friction = contact->friction; - contactConstraint->restitution = contact->restitution; contactConstraint->rollingResistance = contact->rollingResistance; + // Only sample contact point normal velocity if needed. + float restitution = contact->restitution; + bool hitEvents = ( contact->flags & b3_simEnableHitEvent ) != 0; + bool sampleVelocity = restitution > 0.0f || hitEvents; + + b3Vec3 vA = b3Vec3_zero; + b3Vec3 wA = b3Vec3_zero; + b3Vec3 vB = b3Vec3_zero; + b3Vec3 wB = b3Vec3_zero; + + if ( sampleVelocity ) + { + if ( indexA != B3_NULL_INDEX ) + { + vA = states[indexA].linearVelocity; + wA = states[indexA].angularVelocity; + } + + if ( indexB != B3_NULL_INDEX ) + { + vB = states[indexB].linearVelocity; + wB = states[indexB].angularVelocity; + } + } + b3ManifoldConstraint* manifoldConstraints = manifoldBase + specs[localIndex].manifoldStart; contactConstraint->constraints = manifoldConstraints; @@ -210,10 +215,21 @@ void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context ) float kNormal = mA + mB + b3Dot( rnA, b3MulMV( iA, rnA ) ) + b3Dot( rnB, b3MulMV( iB, rnB ) ); cp->normalMass = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; - // Save relative velocity for restitution - b3Vec3 vrA = b3Add( vA, b3Cross( wA, rA ) ); - b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); - cp->relativeVelocity = b3Dot( normal, b3Sub( vrB, vrA ) ); + // Only compute the normal velocity terms if needed. + if ( sampleVelocity ) + { + b3Vec3 vrA = b3Add( vA, b3Cross( wA, rA ) ); + b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); + float vn = b3Dot( normal, b3Sub( vrB, vrA ) ); + + cp->restitutionVelocity = vn < negRestitutionThreshold ? -restitution * vn : 0.0f; + mp->normalVelocity = hitEvents ? vn : 0.0f; + } + else + { + cp->restitutionVelocity = 0.0f; + mp->normalVelocity = 0.0f; + } // C0 friction center decay. Needed to prevent spinning top drift (GyroscopicPrecession sample). // Contacts with separation greater than twice the speculative distance only matter for CCD and @@ -372,14 +388,15 @@ void b3WarmStartContacts_Mesh( b3SolverBlock block, b3StepContext* context ) } // Merged normal and friction loops. This is much more stable for the Jenga stack. -void b3SolveContacts_Mesh( b3SolverBlock block, b3StepContext* context, bool useBias ) +// Solve the non-penetration constraints with the soft bias. No friction and no restitution. +void b3PushContacts_Mesh( b3SolverBlock block, b3StepContext* context ) { b3World* world = context->world; b3GraphColor* color = world->constraintGraph.colors + block.colorIndex; b3ContactConstraint* contactConstraints = color->contactConstraints; b3BodyState* states = context->states; - // This is a dummy state to represent a static body because static bodies don't have a solver body. + // This is a dummy state to represent a static body because static bodies have no solver body. b3BodyState dummyState = b3_identityBodyState; // The last block might not be full @@ -414,8 +431,6 @@ void b3SolveContacts_Mesh( b3SolverBlock block, b3StepContext* context, bool use b3Vec3 dp = b3Sub( stateB->deltaPosition, stateA->deltaPosition ); b3Softness softness = contactConstraint->softness; - float friction = contactConstraint->friction; - float rollingResistance = contactConstraint->rollingResistance; for ( int j = 0; j < manifoldCount; ++j ) { @@ -424,9 +439,6 @@ void b3SolveContacts_Mesh( b3SolverBlock block, b3StepContext* context, bool use int pointCount = constraint->pointCount; b3Vec3 normal = constraint->normal; - float totalNormalImpulse = 0.0f; - float totalTwistLimit = 0.0f; - for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) { b3ManifoldConstraintPoint* cp = constraint->points + pointIndex; @@ -440,16 +452,19 @@ void b3SolveContacts_Mesh( b3SolverBlock block, b3StepContext* context, bool use b3Vec3 ds = b3Add( dp, b3Sub( b3RotateVector( dqB, rB ), b3RotateVector( dqA, rA ) ) ); float s = b3Dot( ds, normal ) + cp->baseSeparation; - float velocityBias = 0.0f; - float massScale = 1.0f; - float impulseScale = 0.0f; + float velocityBias; + float massScale; + float impulseScale; if ( s > 0.0f ) { - // speculative bias + // speculative bias is positive velocityBias = s * inv_h; + massScale = 1.0f; + impulseScale = 0.0f; } - else if ( useBias ) + else { + // overlap bias is negative velocityBias = b3MaxFloat( softness.massScale * softness.biasRate * s, -contactSpeed ); massScale = softness.massScale; impulseScale = softness.impulseScale; @@ -467,10 +482,6 @@ void b3SolveContacts_Mesh( b3SolverBlock block, b3StepContext* context, bool use float newImpulse = b3MaxFloat( cp->normalImpulse + deltaImpulse, 0.0f ); deltaImpulse = newImpulse - cp->normalImpulse; cp->normalImpulse = newImpulse; - cp->totalNormalImpulse += newImpulse; - - totalNormalImpulse += newImpulse; - totalTwistLimit += cp->leverArm * cp->normalImpulse; // apply normal impulse b3Vec3 P = b3MulSV( deltaImpulse, normal ); @@ -480,12 +491,164 @@ void b3SolveContacts_Mesh( b3SolverBlock block, b3StepContext* context, bool use vB = b3MulAdd( vB, mB, P ); wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) ); } + } - // No friction when applying bias - if ( useBias == true ) + if ( stateA->flags & b3_dynamicFlag ) + { + stateA->linearVelocity = vA; + stateA->angularVelocity = wA; + } + + if ( stateB->flags & b3_dynamicFlag ) + { + stateB->linearVelocity = vB; + stateB->angularVelocity = wB; + } + } +} + +// Solve contacts: normal, friction, rolling resistance, and restitution. +void b3SolveContacts_Mesh( b3SolverBlock block, b3StepContext* context ) +{ + b3World* world = context->world; + b3GraphColor* color = world->constraintGraph.colors + block.colorIndex; + b3ContactConstraint* contactConstraints = color->contactConstraints; + b3BodyState* states = context->states; + + // This is a dummy state to represent a static body because static bodies have no solver body. + b3BodyState dummyState = b3_identityBodyState; + + // The last block might not be full + int startIndex = block.startIndex; + int endIndex = startIndex + block.count; + + float inv_h = context->inv_h; + + for ( int i = startIndex; i < endIndex; ++i ) + { + b3ContactConstraint* contactConstraint = contactConstraints + i; + int manifoldCount = contactConstraint->manifoldCount; + + int indexA = contactConstraint->indexA; + int indexB = contactConstraint->indexB; + + float mA = contactConstraint->invMassA; + b3Matrix3 iA = contactConstraint->invIA; + float mB = contactConstraint->invMassB; + b3Matrix3 iB = contactConstraint->invIB; + + b3BodyState* stateA = indexA == B3_NULL_INDEX ? &dummyState : states + indexA; + b3Vec3 vA = stateA->linearVelocity; + b3Vec3 wA = stateA->angularVelocity; + b3Quat dqA = stateA->deltaRotation; + + b3BodyState* stateB = indexB == B3_NULL_INDEX ? &dummyState : states + indexB; + b3Vec3 vB = stateB->linearVelocity; + b3Vec3 wB = stateB->angularVelocity; + b3Quat dqB = stateB->deltaRotation; + + b3Vec3 dp = b3Sub( stateB->deltaPosition, stateA->deltaPosition ); + float friction = contactConstraint->friction; + float rollingResistance = contactConstraint->rollingResistance; + + for ( int j = 0; j < manifoldCount; ++j ) + { + b3ManifoldConstraint* constraint = contactConstraint->constraints + j; + + int pointCount = constraint->pointCount; + b3Vec3 normal = constraint->normal; + + float totalNormalImpulse = 0.0f; + float totalTwistLimit = 0.0f; + bool stopRestitution = true; + + // Restitution with more than one point requires iteration to reduce spin. + int iterationCount = 1; + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) { - // Go to next manifold - continue; + if ( constraint->points[pointIndex].restitutionVelocity > 0.0f ) + { + iterationCount = pointCount; + break; + } + } + + float velocityBiases[B3_MAX_MANIFOLD_POINTS]; + + for ( int iteration = 0; iteration < iterationCount; ++iteration ) + { + bool accumulate = iteration + 1 == iterationCount; + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + b3ManifoldConstraintPoint* cp = constraint->points + pointIndex; + + // Fixed anchor points for applying impulses + b3Vec3 rA = cp->rA; + b3Vec3 rB = cp->rB; + + float velocityBias; + if ( iteration == 0 ) + { + // compute current separation + // this is subject to round-off error if the anchor is far from the body center of mass + b3Vec3 ds = b3Add( dp, b3Sub( b3RotateVector( dqB, rB ), b3RotateVector( dqA, rA ) ) ); + float s = b3Dot( ds, normal ) + cp->baseSeparation; + + // speculative bias, zero when overlapped + velocityBias = s > 0.0f ? s * inv_h : 0.0f; + + // Apply restitution. + if ( cp->restitutionVelocity > 0.0f ) + { + velocityBias = b3MinFloat( velocityBias, -cp->restitutionVelocity ); + stopRestitution = stopRestitution && s > 0.0f; + } + + velocityBiases[pointIndex] = velocityBias; + } + else + { + velocityBias = velocityBiases[pointIndex]; + } + + // relative normal velocity at contact + b3Vec3 vrA = b3Add( vA, b3Cross( wA, rA ) ); + b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); + float vn = b3Dot( b3Sub( vrB, vrA ), normal ); + + // incremental normal impulse + float deltaImpulse = -cp->normalMass * ( vn + velocityBias ); + + // clamp the accumulated impulse + float newImpulse = b3MaxFloat( cp->normalImpulse + deltaImpulse, 0.0f ); + deltaImpulse = newImpulse - cp->normalImpulse; + cp->normalImpulse = newImpulse; + + if ( accumulate ) + { + cp->totalNormalImpulse += newImpulse; + totalNormalImpulse += newImpulse; + totalTwistLimit += cp->leverArm * cp->normalImpulse; + } + + // apply normal impulse + b3Vec3 P = b3MulSV( deltaImpulse, normal ); + vA = b3MulSub( vA, mA, P ); + wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) ); + + vB = b3MulAdd( vB, mB, P ); + wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) ); + } + } + + // Stop restitution once all bouncing points are separated. + if ( stopRestitution ) + { + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + constraint->points[pointIndex].restitutionVelocity = 0.0f; + } } // Central twist friction @@ -583,109 +746,6 @@ void b3SolveContacts_Mesh( b3SolverBlock block, b3StepContext* context, bool use } } -void b3ApplyRestitution_Mesh( b3SolverBlock block, b3StepContext* context ) -{ - b3World* world = context->world; - b3GraphColor* color = world->constraintGraph.colors + block.colorIndex; - b3BodyState* states = context->states; - b3ContactConstraint* constraints = color->contactConstraints; - - // This is a dummy state to represent a static body because static bodies don't have a solver body. - b3BodyState dummyState = b3_identityBodyState; - - int startIndex = block.startIndex; - int endIndex = startIndex + block.count; - - float threshold = context->world->restitutionThreshold; - - for ( int constraintIndex = startIndex; constraintIndex < endIndex; ++constraintIndex ) - { - const b3ContactConstraint* contactConstraint = constraints + constraintIndex; - float restitution = contactConstraint->restitution; - if ( restitution == 0.0f ) - { - continue; - } - - int indexA = contactConstraint->indexA; - int indexB = contactConstraint->indexB; - - b3BodyState* stateA = indexA == B3_NULL_INDEX ? &dummyState : states + indexA; - b3BodyState* stateB = indexB == B3_NULL_INDEX ? &dummyState : states + indexB; - - b3Vec3 vA = stateA->linearVelocity; - b3Vec3 wA = stateA->angularVelocity; - b3Vec3 vB = stateB->linearVelocity; - b3Vec3 wB = stateB->angularVelocity; - - float mA = contactConstraint->invMassA; - b3Matrix3 iA = contactConstraint->invIA; - float mB = contactConstraint->invMassB; - b3Matrix3 iB = contactConstraint->invIB; - - int manifoldCount = contactConstraint->manifoldCount; - for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) - { - b3ManifoldConstraint* cm = contactConstraint->constraints + manifoldIndex; - - b3Vec3 normal = cm->normal; - int pointCount = cm->pointCount; - B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); - - for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) - { - b3ManifoldConstraintPoint* cp = cm->points + pointIndex; - - // If the total normal impulse is zero then there was no collision - // this skips speculative contact points that didn't generate an impulse - // The max normal impulse is used in case there was a collision that moved away within the sub-step - // process - if ( cp->relativeVelocity > -threshold || cp->totalNormalImpulse == 0.0f ) - { - continue; - } - - // fixed anchor points - b3Vec3 rA = cp->rA; - b3Vec3 rB = cp->rB; - - // relative normal velocity at contact - b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); - b3Vec3 vrA = b3Add( vA, b3Cross( wA, rA ) ); - float vn = b3Dot( b3Sub( vrB, vrA ), normal ); - - // compute normal impulse - float impulse = -cp->normalMass * ( vn + restitution * cp->relativeVelocity ); - - // clamp the accumulated impulse - float newImpulse = b3MaxFloat( cp->normalImpulse + impulse, 0.0f ); - impulse = newImpulse - cp->normalImpulse; - cp->normalImpulse = newImpulse; - cp->totalNormalImpulse += impulse; - - // apply contact impulse - b3Vec3 P = b3MulSV( impulse, normal ); - vA = b3MulSub( vA, mA, P ); - wA = b3Sub( wA, b3MulMV( iA, b3Cross( rA, P ) ) ); - vB = b3MulAdd( vB, mB, P ); - wB = b3Add( wB, b3MulMV( iB, b3Cross( rB, P ) ) ); - } - - if ( stateA->flags & b3_dynamicFlag ) - { - stateA->linearVelocity = vA; - stateA->angularVelocity = wA; - } - - if ( stateB->flags & b3_dynamicFlag ) - { - stateB->linearVelocity = vB; - stateB->angularVelocity = wB; - } - } - } -} - // Don't need to use spans for colors for this because the constraint to contact association // is already linked by pointer. void b3StoreImpulses_Mesh( b3SolverBlock block, b3StepContext* context, int workerIndex ) @@ -766,7 +826,6 @@ void b3StoreImpulses_Mesh( b3SolverBlock block, b3StepContext* context, int work b3ManifoldPoint* mp = manifold->points + pointIndex; mp->normalImpulse = cp->normalImpulse; mp->totalNormalImpulse = cp->totalNormalImpulse; - mp->normalVelocity = cp->relativeVelocity; if ( checkHitEvents && flagged == false && mp->normalVelocity < negHitThreshold && mp->totalNormalImpulse > 0.0f ) @@ -954,7 +1013,7 @@ typedef struct b3ContactConstraintPointWide b3FloatW totalNormalImpulses; b3FloatW normalMasses; b3FloatW leverArms; - b3FloatW relativeVelocities; + b3FloatW negRestitutionVelocities; } b3ContactConstraintPointWide; // Solves four points @@ -987,11 +1046,6 @@ typedef struct b3ContactConstraintWide b3FloatW tangentVelocity1; b3FloatW tangentVelocity2; - b3FloatW biasRate; - b3FloatW massScale; - b3FloatW impulseScale; - b3FloatW restitution; - b3Manifold* manifolds[B3_SIMD_WIDTH]; // todo store the maximum point count per wide constraint @@ -1019,7 +1073,7 @@ typedef struct b3BodyStateW #if defined( B3_SIMD_SSE2 ) || defined( B3_SIMD_NEON ) -static b3BodyStateW b3GatherBodies( const b3BodyState* states, int* indices ) +B3_FORCE_INLINE b3BodyStateW b3GatherBodies( const b3BodyState* B3_RESTRICT states, const int* B3_RESTRICT indices ) { b3BodyState dummy = { 0 }; dummy.deltaRotation.s = 1.0f; @@ -1051,7 +1105,8 @@ static b3BodyStateW b3GatherBodies( const b3BodyState* states, int* indices ) } // This writes only the velocities back to the solver bodies -static void b3ScatterBodies( b3BodyState* states, int* indices, const b3BodyStateW* simdBody ) +B3_FORCE_INLINE void b3ScatterBodies( b3BodyState* B3_RESTRICT states, const int* B3_RESTRICT indices, + const b3BodyStateW* B3_RESTRICT simdBody ) { const float* vx = (const float*)&simdBody->v.X; const float* vy = (const float*)&simdBody->v.Y; @@ -1156,7 +1211,7 @@ static void b3ScatterBodies( b3BodyState* states, int* indices, const b3BodyStat #else // non-simd -static b3BodyStateW b3GatherBodies( const b3BodyState* states, int* indices ) +B3_FORCE_INLINE b3BodyStateW b3GatherBodies( const b3BodyState* B3_RESTRICT states, const int* B3_RESTRICT indices ) { b3BodyState identity = b3_identityBodyState; @@ -1184,7 +1239,8 @@ static b3BodyStateW b3GatherBodies( const b3BodyState* states, int* indices ) } // This writes only the velocities back to the solver bodies -static void b3ScatterBodies( b3BodyState* states, int* indices, const b3BodyStateW* simdBody ) +B3_FORCE_INLINE void b3ScatterBodies( b3BodyState* B3_RESTRICT states, const int* B3_RESTRICT indices, + const b3BodyStateW* B3_RESTRICT simdBody ) { int index1 = indices[0] - 1; if ( index1 != -1 && ( states[index1].flags & b3_dynamicFlag ) != 0 ) @@ -1242,19 +1298,16 @@ void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context ) b3TracyCZoneNC( prepare_contact, "Prepare Contact", b3_colorYellow, true ); b3World* world = context->world; b3BodySim* sims = context->sims; - b3BodyState* states = context->states; #if B3_ENABLE_VALIDATION b3Body* bodies = world->bodies.data; #endif b3WidePrepareSpan* spans = context->widePrepareSpans; b3ContactConstraintWide* wideBase = context->wideConstraints; - - // Stiffer for static contacts to avoid bodies getting pushed through the ground - b3Softness contactSoftness = context->contactSoftness; - b3Softness staticSoftness = context->staticSoftness; + b3BodyState* states = context->states; float warmStartScale = world->enableWarmStarting ? 1.0f : 0.0f; float invTau = 1.0f / B3_SPECULATIVE_DISTANCE; + float negRestitutionThreshold = -world->restitutionThreshold; int wideIndex = block.startIndex; int endWideIndex = block.startIndex + block.count; @@ -1294,16 +1347,14 @@ void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context ) B3_ASSERT( contact->manifoldCount == 1 ); b3Manifold* manifold = contact->manifolds + 0; - int indexA = contact->bodySimIndexA; - int indexB = contact->bodySimIndexB; + int indexA = b3DecodeAwakeIndex( contact->encodedBodySimA ); + int indexB = b3DecodeAwakeIndex( contact->encodedBodySimB ); #if B3_ENABLE_VALIDATION b3Body* bodyA = bodies + contact->edges[0].bodyId; - int validIndexA = bodyA->setIndex == b3_awakeSet ? bodyA->localIndex : B3_NULL_INDEX; b3Body* bodyB = bodies + contact->edges[1].bodyId; - int validIndexB = bodyB->setIndex == b3_awakeSet ? bodyB->localIndex : B3_NULL_INDEX; - B3_ASSERT( indexA == validIndexA ); - B3_ASSERT( indexB == validIndexB ); + B3_ASSERT( contact->encodedBodySimA == b3EncodeBodySimIndex( bodyA ) ); + B3_ASSERT( contact->encodedBodySimB == b3EncodeBodySimIndex( bodyB ) ); #endif // 0 for null @@ -1314,49 +1365,33 @@ void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context ) // Body A data float mA; b3Matrix3 iA; - b3Vec3 vA; - b3Vec3 wA; if ( indexA == B3_NULL_INDEX ) { mA = 0.0f; iA = b3Mat3_zero; - vA = b3Vec3_zero; - wA = b3Vec3_zero; } else { b3BodySim* simA = sims + indexA; mA = simA->invMass; iA = simA->invInertiaWorld; - - b3BodyState* stateA = states + indexA; - vA = stateA->linearVelocity; - wA = stateA->angularVelocity; } // Body B data float mB; b3Matrix3 iB; - b3Vec3 vB; - b3Vec3 wB; if ( indexB == B3_NULL_INDEX ) { mB = 0.0f; iB = b3Mat3_zero; - vB = b3Vec3_zero; - wB = b3Vec3_zero; } else { b3BodySim* simB = sims + indexB; mB = simB->invMass; iB = simB->invInertiaWorld; - - b3BodyState* stateB = states + indexB; - vB = stateB->linearVelocity; - wB = stateB->angularVelocity; } ( (float*)&constraint->invMassA )[lane] = mA; @@ -1376,8 +1411,6 @@ void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context ) ( (float*)&constraint->invIB.cyz )[lane] = iB.cy.z; ( (float*)&constraint->invIB.czz )[lane] = iB.cz.z; - b3Softness soft = ( indexA == B3_NULL_INDEX || indexB == B3_NULL_INDEX ) ? staticSoftness : contactSoftness; - b3Vec3 normal = manifold->normal; ( (float*)&constraint->normal.X )[lane] = normal.x; ( (float*)&constraint->normal.Y )[lane] = normal.y; @@ -1394,15 +1427,35 @@ void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context ) ( (float*)&constraint->tangent2.Z )[lane] = tangent2.z; ( (float*)&constraint->friction )[lane] = contact->friction; - ( (float*)&constraint->restitution )[lane] = contact->restitution; ( (float*)&constraint->rollingResistance )[lane] = contact->rollingResistance; ( (float*)&constraint->tangentVelocity1 )[lane] = b3Dot( contact->tangentVelocity, tangent1 ); ( (float*)&constraint->tangentVelocity2 )[lane] = b3Dot( contact->tangentVelocity, tangent2 ); - ( (float*)&constraint->biasRate )[lane] = soft.biasRate; - ( (float*)&constraint->massScale )[lane] = soft.massScale; - ( (float*)&constraint->impulseScale )[lane] = soft.impulseScale; + // Only sample contact point normal velocity if needed. + float restitution = contact->restitution; + bool hitEvents = ( contact->flags & b3_simEnableHitEvent ) != 0; + bool sampleVelocity = restitution > 0.0f || hitEvents; + + b3Vec3 vA = b3Vec3_zero; + b3Vec3 wA = b3Vec3_zero; + b3Vec3 vB = b3Vec3_zero; + b3Vec3 wB = b3Vec3_zero; + + if ( sampleVelocity ) + { + if ( indexA != B3_NULL_INDEX ) + { + vA = states[indexA].linearVelocity; + wA = states[indexA].angularVelocity; + } + + if ( indexB != B3_NULL_INDEX ) + { + vB = states[indexB].linearVelocity; + wB = states[indexB].angularVelocity; + } + } int pointCount = manifold->pointCount; constraint->pointCounts[lane] = pointCount; @@ -1413,7 +1466,7 @@ void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context ) for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) { - const b3ManifoldPoint* mp = manifold->points + pointIndex; + b3ManifoldPoint* mp = manifold->points + pointIndex; b3ContactConstraintPointWide* cp = constraint->points + pointIndex; b3Vec3 rA = mp->anchorA; @@ -1446,10 +1499,21 @@ void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context ) float kNormal = mA + mB + b3Dot( rnA, b3MulMV( iA, rnA ) ) + b3Dot( rnB, b3MulMV( iB, rnB ) ); ( (float*)&cp->normalMasses )[lane] = kNormal > 0.0f ? 1.0f / kNormal : 0.0f; - // Save relative velocity for restitution - b3Vec3 vrA = b3Add( vA, b3Cross( wA, rA ) ); - b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); - ( (float*)&cp->relativeVelocities )[lane] = b3Dot( normal, b3Sub( vrB, vrA ) ); + // Get the normal velocity if needed for restitution or hit events. + if ( sampleVelocity ) + { + b3Vec3 vrA = b3Add( vA, b3Cross( wA, rA ) ); + b3Vec3 vrB = b3Add( vB, b3Cross( wB, rB ) ); + float vn = b3Dot( normal, b3Sub( vrB, vrA ) ); + + ( (float*)&cp->negRestitutionVelocities )[lane] = vn < negRestitutionThreshold ? restitution * vn : 0.0f; + mp->normalVelocity = hitEvents ? vn : 0.0f; + } + else + { + ( (float*)&cp->negRestitutionVelocities )[lane] = 0.0f; + mp->normalVelocity = 0.0f; + } } float invWeight = 1.0f / totalFrictionWeight; @@ -1528,7 +1592,7 @@ void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context ) ( (float*)&cp->normalImpulses )[lane] = 0.0f; ( (float*)&cp->totalNormalImpulses )[lane] = 0.0f; ( (float*)&cp->normalMasses )[lane] = 0.0f; - ( (float*)&cp->relativeVelocities )[lane] = 0.0f; + ( (float*)&cp->negRestitutionVelocities )[lane] = 0.0f; ( (float*)&cp->leverArms )[lane] = 0.0f; } } @@ -1613,16 +1677,25 @@ void b3WarmStartContacts_Convex( b3SolverBlock block, b3StepContext* context ) b3TracyCZoneEnd( warm_start_contact ); } -void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context, bool useBias ) +// Solve the non-penetration constraints with the soft bias. No friction and no restitution. +void b3PushContacts_Convex( b3SolverBlock block, b3StepContext* context ) { - b3TracyCZoneNC( solve_contact, "Solve Contact", b3_colorAliceBlue, true ); + b3TracyCZoneNC( push_contact, "Push Contact", b3_colorAliceBlue, true ); b3BodyState* states = context->states; b3ContactConstraintWide* constraints = context->graph->colors[block.colorIndex].wideConstraints; b3FloatW inv_h = b3SplatW( context->inv_h ); b3FloatW contactSpeed = b3SplatW( -context->world->contactSpeed ); b3FloatW oneW = b3SplatW( 1.0f ); - b3FloatW epsilonW = b3SplatW( FLT_EPSILON ); + + // Stiffer for static contacts to avoid bodies getting pushed through the ground. Selected per + // lane from the null body index instead of stored per constraint. + b3FloatW dynamicBiasRate = b3SplatW( context->contactSoftness.massScale * context->contactSoftness.biasRate ); + b3FloatW dynamicMassScale = b3SplatW( context->contactSoftness.massScale ); + b3FloatW dynamicImpulseScale = b3SplatW( context->contactSoftness.impulseScale ); + b3FloatW staticBiasRate = b3SplatW( context->staticSoftness.massScale * context->staticSoftness.biasRate ); + b3FloatW staticMassScale = b3SplatW( context->staticSoftness.massScale ); + b3FloatW staticImpulseScale = b3SplatW( context->staticSoftness.impulseScale ); for ( int wideIndex = block.startIndex; wideIndex < block.startIndex + block.count; ++wideIndex ) { @@ -1637,25 +1710,13 @@ void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context, bool u b3BodyStateW bA = b3GatherBodies( states, c->indexA ); b3BodyStateW bB = b3GatherBodies( states, c->indexB ); - b3FloatW biasRate, massScale, impulseScale; - if ( useBias ) - { - biasRate = b3MulW( c->massScale, c->biasRate ); - massScale = c->massScale; - impulseScale = c->impulseScale; - } - else - { - biasRate = b3ZeroW(); - massScale = oneW; - impulseScale = b3ZeroW(); - } + b3FloatW softMask = b3SoftMaskW( c->indexA, c->indexB ); + b3FloatW biasRate = b3BlendW( dynamicBiasRate, staticBiasRate, softMask ); + b3FloatW massScale = b3BlendW( dynamicMassScale, staticMassScale, softMask ); + b3FloatW impulseScale = b3BlendW( dynamicImpulseScale, staticImpulseScale, softMask ); b3Vec3W dp = b3SubVW( bB.dp, bA.dp ); - b3FloatW totalNormalImpulse = b3ZeroW(); - b3FloatW totalTwistLimit = b3ZeroW(); - for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) { b3ContactConstraintPointWide* cp = c->points + pointIndex; @@ -1675,13 +1736,17 @@ void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context, bool u b3FloatW s = b3AddW( b3DotW( c->normal, ds ), cp->baseSeparations ); // Apply speculative bias if separation is greater than zero, otherwise apply soft constraint bias - b3FloatW mask = b3GreaterThanW( s, b3ZeroW() ); - b3FloatW specBias = b3MulW( s, inv_h ); - b3FloatW softBias = b3MaxW( b3MulW( biasRate, s ), contactSpeed ); - b3FloatW bias = b3BlendW( softBias, specBias, mask ); + b3FloatW separated = b3GreaterThanW( s, b3ZeroW() ); - b3FloatW pointMassScale = b3BlendW( massScale, oneW, mask ); - b3FloatW pointImpulseScale = b3BlendW( impulseScale, b3ZeroW(), mask ); + // Speculative bias - positive + b3FloatW specBias = b3MulW( s, inv_h ); + + // Overlap bias - negative + b3FloatW overlapBias = b3MaxW( b3MulW( biasRate, s ), contactSpeed ); + b3FloatW velocityBias = b3BlendW( overlapBias, specBias, separated ); + + b3FloatW pointMassScale = b3BlendW( massScale, oneW, separated ); + b3FloatW pointImpulseScale = b3BlendW( impulseScale, b3ZeroW(), separated ); // Relative velocity at contact b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, rA ) ); @@ -1689,17 +1754,13 @@ void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context, bool u b3FloatW vn = b3DotW( b3SubVW( vrB, vrA ), c->normal ); // Compute normal impulse - b3FloatW negImpulse = b3AddW( b3MulW( cp->normalMasses, b3AddW( b3MulW( pointMassScale, vn ), bias ) ), + b3FloatW negImpulse = b3AddW( b3MulW( cp->normalMasses, b3AddW( b3MulW( pointMassScale, vn ), velocityBias ) ), b3MulW( pointImpulseScale, cp->normalImpulses ) ); // Clamp the accumulated impulse b3FloatW newImpulse = b3MaxW( b3SubW( cp->normalImpulses, negImpulse ), b3ZeroW() ); b3FloatW deltaImpulse = b3SubW( newImpulse, cp->normalImpulses ); cp->normalImpulses = newImpulse; - cp->totalNormalImpulses = b3AddW( cp->totalNormalImpulses, newImpulse ); - - totalNormalImpulse = b3AddW( totalNormalImpulse, newImpulse ); - totalTwistLimit = b3AddW( totalTwistLimit, b3MulW( cp->leverArms, newImpulse ) ); // Apply contact impulse b3Vec3W P = b3MulSVW( deltaImpulse, c->normal ); @@ -1709,139 +1770,28 @@ void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context, bool u bB.v = b3MulAddSVW( bB.v, c->invMassB, P ); } - // No friction when applying bias - if ( useBias == false ) - { - // Rolling resistance - if ( b3AllZeroW( c->rollingResistance ) == false ) - { - // flip A/B order to negate - b3Vec3W deltaImpulse = b3MulMVW( c->rollingMass, b3SubVW( bA.w, bB.w ) ); - b3Vec3W oldImpulse = c->rollingImpulse; - c->rollingImpulse = b3AddVW( oldImpulse, deltaImpulse ); - - b3FloatW maxImpulse = b3MulW( c->rollingResistance, totalNormalImpulse ); - b3FloatW lengthSquared = b3DotW( c->rollingImpulse, c->rollingImpulse ); - - // if ( magSqr > maxLambda * maxLambda + FLT_EPSILON ) - //{ - // c->rollingImpulse *= maxLambda / sqrtf( magSqr ); - // } - - b3FloatW mask = b3GreaterThanW( lengthSquared, b3MulAddW( epsilonW, maxImpulse, maxImpulse ) ); - - // No approximate _mm_rsqrt_ps here to maintain cross-platform determinism - b3FloatW normalize = b3DivW( maxImpulse, b3AddW( b3SqrtW( lengthSquared ), epsilonW ) ); - b3FloatW scale = b3BlendW( oneW, normalize, mask ); - - // Ensure zero rolling resistance yields no impulse - b3FloatW rollingMask = b3GreaterThanW( c->rollingResistance, b3ZeroW() ); - scale = b3BlendW( b3ZeroW(), scale, rollingMask ); - - c->rollingImpulse = b3MulSVW( scale, c->rollingImpulse ); - - deltaImpulse = b3SubVW( c->rollingImpulse, oldImpulse ); - - bA.w = b3MulSubMVW( bA.w, c->invIA, deltaImpulse ); - bB.w = b3MulAddMVW( bB.w, c->invIB, deltaImpulse ); - } - - // Central twist friction - { - b3FloatW twistSpeed = b3DotW( c->normal, b3SubVW( bB.w, bA.w ) ); - b3FloatW maxLambda = b3MulW( c->friction, totalTwistLimit ); - b3FloatW deltaImpulse = b3NegW( b3MulW( c->twistMass, twistSpeed ) ); - b3FloatW oldImpulse = c->twistImpulse; - c->twistImpulse = b3SymClampW( b3AddW( oldImpulse, deltaImpulse ), maxLambda ); - deltaImpulse = b3SubW( c->twistImpulse, oldImpulse ); - - b3Vec3W L = b3MulSVW( deltaImpulse, c->normal ); - bA.w = b3MulSubMVW( bA.w, c->invIA, L ); - bB.w = b3MulAddMVW( bB.w, c->invIB, L ); - } - - // Central friction - { - b3Vec3W tangent1 = c->tangent1; - b3Vec3W tangent2 = c->tangent2; - - // Fixed anchor points for applying impulses - b3Vec3W rA = c->centerA; - b3Vec3W rB = c->centerB; - - // Relative tangent velocity at contact - b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, rA ) ); - b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, rB ) ); - b3Vec3W vr = b3SubVW( vrB, vrA ); - b3Vec2W vt = { - b3SubW( b3DotW( vr, tangent1 ), c->tangentVelocity1 ), - b3SubW( b3DotW( vr, tangent2 ), c->tangentVelocity2 ), - }; - - // Incremental tangent impulse - b3Vec2W deltaImpulse = b3MulMV2W( c->tangentMass, vt ); - deltaImpulse = (b3Vec2W){ b3NegW( deltaImpulse.x ), b3NegW( deltaImpulse.y ) }; - b3Vec2W newImpulse = b3AddV2W( c->frictionImpulse, deltaImpulse ); - - b3FloatW friction = c->friction; - b3FloatW maxImpulse = b3MulW( friction, totalNormalImpulse ); - - // Clamp the accumulated impulse - b3FloatW lengthSquared = b3AddW( b3MulW( newImpulse.x, newImpulse.x ), b3MulW( newImpulse.y, newImpulse.y ) ); - - // Max impulse can be zero - b3FloatW mask = b3GreaterThanW( lengthSquared, b3MulW( maxImpulse, maxImpulse ) ); - - // No approximate _mm_rsqrt_ps here to maintain cross-platform determinism. Add epsilon to avoid divide by - // zero. - b3FloatW normalize = b3DivW( maxImpulse, b3AddW( b3SqrtW( lengthSquared ), epsilonW ) ); - b3FloatW scale = b3BlendW( oneW, normalize, mask ); - newImpulse = (b3Vec2W){ - b3MulW( scale, newImpulse.x ), - b3MulW( scale, newImpulse.y ), - }; - - deltaImpulse = (b3Vec2W){ - b3SubW( newImpulse.x, c->frictionImpulse.x ), - b3SubW( newImpulse.y, c->frictionImpulse.y ), - }; - - c->frictionImpulse = newImpulse; - - // Apply delta impulse - b3Vec3W P = b3AddVW( b3MulSVW( deltaImpulse.x, tangent1 ), b3MulSVW( deltaImpulse.y, tangent2 ) ); - bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, P ) ); - bA.v = b3MulSubSVW( bA.v, c->invMassA, P ); - bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, P ) ); - bB.v = b3MulAddSVW( bB.v, c->invMassB, P ); - } - } - b3ScatterBodies( states, c->indexA, &bA ); b3ScatterBodies( states, c->indexB, &bB ); } - b3TracyCZoneEnd( solve_contact ); + b3TracyCZoneEnd( push_contact ); } -void b3ApplyRestitution_Convex( b3SolverBlock block, b3StepContext* context ) +// Solve the normal constraint, friction, and rolling resistance. Deferred restitution augments +// the normal constraint. +void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context ) { - b3TracyCZoneNC( restitution, "Restitution", b3_colorDodgerBlue, true ); + b3TracyCZoneNC( solve_contact, "Solve Contact", b3_colorAliceBlue, true ); b3BodyState* states = context->states; b3ContactConstraintWide* constraints = context->graph->colors[block.colorIndex].wideConstraints; - b3FloatW threshold = b3SplatW( context->world->restitutionThreshold ); - b3FloatW zero = b3ZeroW(); + b3FloatW inv_h = b3SplatW( context->inv_h ); + b3FloatW oneW = b3SplatW( 1.0f ); + b3FloatW epsilonW = b3SplatW( FLT_EPSILON ); - for ( int i = block.startIndex; i < block.startIndex + block.count; ++i ) + for ( int wideIndex = block.startIndex; wideIndex < block.startIndex + block.count; ++wideIndex ) { - b3ContactConstraintWide* c = constraints + i; - - if ( b3AllZeroW( c->restitution ) ) - { - // No lanes have restitution. Common case. - continue; - } + b3ContactConstraintWide* c = constraints + wideIndex; _Static_assert( B3_SIMD_WIDTH == 4, "width" ); int pointCount1 = b3MaxInt( c->pointCounts[0], c->pointCounts[1] ); @@ -1849,44 +1799,209 @@ void b3ApplyRestitution_Convex( b3SolverBlock block, b3StepContext* context ) int pointCount = b3MaxInt( pointCount1, pointCount2 ); B3_VALIDATE( 0 < pointCount && pointCount <= B3_MAX_MANIFOLD_POINTS ); - // Single gather for all manifolds b3BodyStateW bA = b3GatherBodies( states, c->indexA ); b3BodyStateW bB = b3GatherBodies( states, c->indexB ); - // Create a mask based on restitution so that lanes with no restitution are not affected - // by the calculations below. - b3FloatW restitutionMask = b3EqualsW( c->restitution, zero ); - - for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + b3FloatW anyRestitution = c->points[0].negRestitutionVelocities; + for ( int pointIndex = 1; pointIndex < pointCount; ++pointIndex ) { - b3ContactConstraintPointWide* cp = c->points + pointIndex; + anyRestitution = b3OrW( anyRestitution, c->points[pointIndex].negRestitutionVelocities ); + } + bool haveRestitution = b3AllZeroW( anyRestitution ) == false; + b3FloatW keepRestitution = b3ZeroW(); - // Set effective mass to zero if restitution should not be applied - b3FloatW mask1 = b3GreaterThanW( b3AddW( cp->relativeVelocities, threshold ), zero ); - b3FloatW mask2 = b3EqualsW( cp->totalNormalImpulses, zero ); - b3FloatW mask = b3OrW( b3OrW( mask1, mask2 ), restitutionMask ); - b3FloatW mass = b3BlendW( cp->normalMasses, zero, mask ); + b3Vec3W dp = b3SubVW( bB.dp, bA.dp ); - // Fixed anchors for impulses - b3Vec3W rA = cp->anchorAs; - b3Vec3W rB = cp->anchorBs; + b3FloatW totalNormalImpulse = b3ZeroW(); + b3FloatW totalTwistLimit = b3ZeroW(); - // Relative velocity at contact + // Restitution with more than one point needs iteration to reduce spin. + // This makes restitution have a significant cost. + int iterationCount = haveRestitution ? pointCount : 1; + b3FloatW velocityBiases[B3_MAX_MANIFOLD_POINTS]; + + for ( int iteration = 0; iteration < iterationCount; ++iteration ) + { + bool accumulate = iteration + 1 == iterationCount; + + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + b3ContactConstraintPointWide* cp = c->points + pointIndex; + + // Fixed anchor points for applying impulses + b3Vec3W rA = cp->anchorAs; + b3Vec3W rB = cp->anchorBs; + + b3FloatW velocityBias; + if ( iteration == 0 ) + { + // Moving anchors for current separation + // todo speed this up using matrices + b3Vec3W rsA = b3RotateVectorW( bA.dq, rA ); + b3Vec3W rsB = b3RotateVectorW( bB.dq, rB ); + + // compute current separation + // this is subject to round-off error if the anchor is far from the body center of mass + b3Vec3W ds = b3AddVW( dp, b3SubVW( rsB, rsA ) ); + b3FloatW s = b3AddW( b3DotW( c->normal, ds ), cp->baseSeparations ); + + // Speculative bias, positive if separated and zero if overlapped + velocityBias = b3MaxW( b3ZeroW(), b3MulW( s, inv_h ) ); + + if ( haveRestitution ) + { + // A lane with no restitution keeps the speculative bias. A bouncing lane stops + // bouncing once every point on its manifold has separated. + b3FloatW restitutionMask = b3GreaterThanW( b3ZeroW(), cp->negRestitutionVelocities ); + b3FloatW separated = b3GreaterThanW( s, b3ZeroW() ); + velocityBias = b3BlendW( velocityBias, cp->negRestitutionVelocities, restitutionMask ); + keepRestitution = b3OrW( keepRestitution, b3AndNotW( restitutionMask, separated ) ); + } + + velocityBiases[pointIndex] = velocityBias; + } + else + { + velocityBias = velocityBiases[pointIndex]; + } + + // Relative velocity at contact + b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, rA ) ); + b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, rB ) ); + b3FloatW vn = b3DotW( b3SubVW( vrB, vrA ), c->normal ); + + // Compute normal impulse + b3FloatW negImpulse = b3MulW( cp->normalMasses, b3AddW( vn, velocityBias ) ); + + // Clamp the accumulated impulse + b3FloatW newImpulse = b3MaxW( b3SubW( cp->normalImpulses, negImpulse ), b3ZeroW() ); + b3FloatW deltaImpulse = b3SubW( newImpulse, cp->normalImpulses ); + cp->normalImpulses = newImpulse; + + // Don't accumulate the total impulse while bouncing. + if ( accumulate ) + { + cp->totalNormalImpulses = b3AddW( cp->totalNormalImpulses, newImpulse ); + totalNormalImpulse = b3AddW( totalNormalImpulse, newImpulse ); + totalTwistLimit = b3AddW( totalTwistLimit, b3MulW( cp->leverArms, newImpulse ) ); + } + + // Apply contact impulse + b3Vec3W P = b3MulSVW( deltaImpulse, c->normal ); + bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, P ) ); + bA.v = b3MulSubSVW( bA.v, c->invMassA, P ); + bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, P ) ); + bB.v = b3MulAddSVW( bB.v, c->invMassB, P ); + } + } + + if ( haveRestitution ) + { + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + b3ContactConstraintPointWide* cp = c->points + pointIndex; + cp->negRestitutionVelocities = b3BlendW( b3ZeroW(), cp->negRestitutionVelocities, keepRestitution ); + } + } + + // Rolling resistance + if ( b3AllZeroW( c->rollingResistance ) == false ) + { + // flip A/B order to negate + b3Vec3W deltaImpulse = b3MulMVW( c->rollingMass, b3SubVW( bA.w, bB.w ) ); + b3Vec3W oldImpulse = c->rollingImpulse; + c->rollingImpulse = b3AddVW( oldImpulse, deltaImpulse ); + + b3FloatW maxImpulse = b3MulW( c->rollingResistance, totalNormalImpulse ); + b3FloatW lengthSquared = b3DotW( c->rollingImpulse, c->rollingImpulse ); + + // if ( magSqr > maxLambda * maxLambda + FLT_EPSILON ) + //{ + // c->rollingImpulse *= maxLambda / sqrtf( magSqr ); + // } + + b3FloatW mask = b3GreaterThanW( lengthSquared, b3MulAddW( epsilonW, maxImpulse, maxImpulse ) ); + + // No approximate _mm_rsqrt_ps here to maintain cross-platform determinism + b3FloatW normalize = b3DivW( maxImpulse, b3AddW( b3SqrtW( lengthSquared ), epsilonW ) ); + b3FloatW scale = b3BlendW( oneW, normalize, mask ); + + // Ensure zero rolling resistance yields no impulse + b3FloatW rollingMask = b3GreaterThanW( c->rollingResistance, b3ZeroW() ); + scale = b3BlendW( b3ZeroW(), scale, rollingMask ); + + c->rollingImpulse = b3MulSVW( scale, c->rollingImpulse ); + + deltaImpulse = b3SubVW( c->rollingImpulse, oldImpulse ); + + bA.w = b3MulSubMVW( bA.w, c->invIA, deltaImpulse ); + bB.w = b3MulAddMVW( bB.w, c->invIB, deltaImpulse ); + } + + // Central twist friction + { + b3FloatW twistSpeed = b3DotW( c->normal, b3SubVW( bB.w, bA.w ) ); + b3FloatW maxLambda = b3MulW( c->friction, totalTwistLimit ); + b3FloatW deltaImpulse = b3NegW( b3MulW( c->twistMass, twistSpeed ) ); + b3FloatW oldImpulse = c->twistImpulse; + c->twistImpulse = b3SymClampW( b3AddW( oldImpulse, deltaImpulse ), maxLambda ); + deltaImpulse = b3SubW( c->twistImpulse, oldImpulse ); + + b3Vec3W L = b3MulSVW( deltaImpulse, c->normal ); + bA.w = b3MulSubMVW( bA.w, c->invIA, L ); + bB.w = b3MulAddMVW( bB.w, c->invIB, L ); + } + + // Central friction + { + b3Vec3W tangent1 = c->tangent1; + b3Vec3W tangent2 = c->tangent2; + + // Fixed anchor points for applying impulses + b3Vec3W rA = c->centerA; + b3Vec3W rB = c->centerB; + + // Relative tangent velocity at contact b3Vec3W vrA = b3AddVW( bA.v, b3CrossW( bA.w, rA ) ); b3Vec3W vrB = b3AddVW( bB.v, b3CrossW( bB.w, rB ) ); - b3FloatW vn = b3DotW( b3SubVW( vrB, vrA ), c->normal ); + b3Vec3W vr = b3SubVW( vrB, vrA ); + b3Vec2W vt = { + b3SubW( b3DotW( vr, tangent1 ), c->tangentVelocity1 ), + b3SubW( b3DotW( vr, tangent2 ), c->tangentVelocity2 ), + }; - // Compute normal impulse - b3FloatW negImpulse = b3MulW( mass, b3AddW( vn, b3MulW( c->restitution, cp->relativeVelocities ) ) ); + // Incremental tangent impulse + b3Vec2W deltaImpulse = b3MulMV2W( c->tangentMass, vt ); + deltaImpulse = (b3Vec2W){ b3NegW( deltaImpulse.x ), b3NegW( deltaImpulse.y ) }; + b3Vec2W newImpulse = b3AddV2W( c->frictionImpulse, deltaImpulse ); + + b3FloatW friction = c->friction; + b3FloatW maxImpulse = b3MulW( friction, totalNormalImpulse ); // Clamp the accumulated impulse - b3FloatW newImpulse = b3MaxW( b3SubW( cp->normalImpulses, negImpulse ), b3ZeroW() ); - b3FloatW deltaImpulse = b3SubW( newImpulse, cp->normalImpulses ); - cp->normalImpulses = newImpulse; - cp->totalNormalImpulses = b3AddW( cp->totalNormalImpulses, deltaImpulse ); + b3FloatW lengthSquared = b3AddW( b3MulW( newImpulse.x, newImpulse.x ), b3MulW( newImpulse.y, newImpulse.y ) ); - // Apply contact impulse - b3Vec3W P = b3MulSVW( deltaImpulse, c->normal ); + // Max impulse can be zero + b3FloatW mask = b3GreaterThanW( lengthSquared, b3MulW( maxImpulse, maxImpulse ) ); + + // No approximate _mm_rsqrt_ps here to maintain cross-platform determinism. Add epsilon to avoid divide by + // zero. + b3FloatW normalize = b3DivW( maxImpulse, b3AddW( b3SqrtW( lengthSquared ), epsilonW ) ); + b3FloatW scale = b3BlendW( oneW, normalize, mask ); + newImpulse = (b3Vec2W){ + b3MulW( scale, newImpulse.x ), + b3MulW( scale, newImpulse.y ), + }; + + deltaImpulse = (b3Vec2W){ + b3SubW( newImpulse.x, c->frictionImpulse.x ), + b3SubW( newImpulse.y, c->frictionImpulse.y ), + }; + + c->frictionImpulse = newImpulse; + + // Apply delta impulse + b3Vec3W P = b3AddVW( b3MulSVW( deltaImpulse.x, tangent1 ), b3MulSVW( deltaImpulse.y, tangent2 ) ); bA.w = b3MulSubMVW( bA.w, c->invIA, b3CrossW( rA, P ) ); bA.v = b3MulSubSVW( bA.v, c->invMassA, P ); bB.w = b3MulAddMVW( bB.w, c->invIB, b3CrossW( rB, P ) ); @@ -1897,7 +2012,7 @@ void b3ApplyRestitution_Convex( b3SolverBlock block, b3StepContext* context ) b3ScatterBodies( states, c->indexB, &bB ); } - b3TracyCZoneEnd( restitution ); + b3TracyCZoneEnd( solve_contact ); } // Store impulses by contact constraint @@ -1982,12 +2097,10 @@ void b3StoreImpulses_Convex( b3SolverBlock block, b3StepContext* context, int wo const b3ContactConstraintPointWide* cp = c->points + pointIndex; const float* normalImpulse = (float*)&cp->normalImpulses; const float* totalNormalImpulse = (float*)&cp->totalNormalImpulses; - const float* normalVelocity = (float*)&cp->relativeVelocities; b3ManifoldPoint* mp = m->points + pointIndex; mp->normalImpulse = normalImpulse[lane]; mp->totalNormalImpulse = totalNormalImpulse[lane]; - mp->normalVelocity = normalVelocity[lane]; } int contactId = contactIds[contactIndex]; @@ -2060,7 +2173,7 @@ void b3WarmStartContacts_Overflow( b3StepContext* context ) b3WarmStartContacts_Mesh( block, context ); } -void b3SolveContacts_Overflow( b3StepContext* context, bool useBias ) +void b3PushContacts_Overflow( b3StepContext* context ) { b3ConstraintGraph* graph = context->graph; b3GraphColor* color = graph->colors + B3_OVERFLOW_INDEX; @@ -2078,10 +2191,10 @@ void b3SolveContacts_Overflow( b3StepContext* context, bool useBias ) .colorIndex = B3_OVERFLOW_INDEX, }; - b3SolveContacts_Mesh( block, context, useBias ); + b3PushContacts_Mesh( block, context ); } -void b3ApplyRestitution_Overflow( b3StepContext* context ) +void b3SolveContacts_Overflow( b3StepContext* context ) { b3ConstraintGraph* graph = context->graph; b3GraphColor* color = graph->colors + B3_OVERFLOW_INDEX; @@ -2099,7 +2212,7 @@ void b3ApplyRestitution_Overflow( b3StepContext* context ) .colorIndex = B3_OVERFLOW_INDEX, }; - b3ApplyRestitution_Mesh( block, context ); + b3SolveContacts_Mesh( block, context ); } void b3StoreImpulses_Overflow( b3StepContext* context ) diff --git a/src/contact_solver.h b/src/contact_solver.h index 0c24903..523e70b 100644 --- a/src/contact_solver.h +++ b/src/contact_solver.h @@ -10,7 +10,7 @@ typedef struct b3ManifoldConstraintPoint { b3Vec3 rA, rB; float baseSeparation; - float relativeVelocity; + float restitutionVelocity; float normalImpulse; float totalNormalImpulse; float normalMass; @@ -46,7 +46,6 @@ typedef struct b3ContactConstraint b3Softness softness; b3Matrix3 rollingMass; float friction; - float restitution; float rollingResistance; int manifoldCount; } b3ContactConstraint; @@ -56,18 +55,18 @@ int b3GetWideContactConstraintByteCount( void ); // Overflow contacts don't fit into the constraint graph coloring void b3PrepareContacts_Overflow( b3StepContext* context ); void b3WarmStartContacts_Overflow( b3StepContext* context ); -void b3SolveContacts_Overflow( b3StepContext* context, bool useBias ); -void b3ApplyRestitution_Overflow( b3StepContext* context ); +void b3PushContacts_Overflow( b3StepContext* context ); +void b3SolveContacts_Overflow( b3StepContext* context ); void b3StoreImpulses_Overflow( b3StepContext* context ); void b3PrepareContacts_Mesh( b3SolverBlock block, b3StepContext* context ); void b3WarmStartContacts_Mesh( b3SolverBlock block, b3StepContext* context ); -void b3SolveContacts_Mesh( b3SolverBlock block, b3StepContext* context, bool useBias ); -void b3ApplyRestitution_Mesh( b3SolverBlock block, b3StepContext* context ); +void b3PushContacts_Mesh( b3SolverBlock block, b3StepContext* context ); +void b3SolveContacts_Mesh( b3SolverBlock block, b3StepContext* context ); void b3StoreImpulses_Mesh( b3SolverBlock block, b3StepContext* context, int workerIndex ); void b3PrepareContacts_Convex( b3SolverBlock block, b3StepContext* context ); void b3WarmStartContacts_Convex( b3SolverBlock block, b3StepContext* context ); -void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context, bool useBias ); -void b3ApplyRestitution_Convex( b3SolverBlock block, b3StepContext* context ); +void b3PushContacts_Convex( b3SolverBlock block, b3StepContext* context ); +void b3SolveContacts_Convex( b3SolverBlock block, b3StepContext* context ); void b3StoreImpulses_Convex( b3SolverBlock block, b3StepContext* context, int workerIndex ); diff --git a/src/core.c b/src/core.c index 0235855..c17e073 100644 --- a/src/core.c +++ b/src/core.c @@ -138,7 +138,7 @@ int b3GetMaxManifoldPoints( void ) static b3AllocFcn* b3_allocFcn = NULL; static b3FreeFcn* b3_freeFcn = NULL; -b3AtomicInt b3_byteCount; +static b3AtomicI64 b3_byteCount; void b3SetAllocator( b3AllocFcn* allocFcn, b3FreeFcn* freeFcn ) { @@ -153,13 +153,11 @@ void* b3Alloc( size_t size ) return NULL; } - // This could cause some sharing issues, however Box3D rarely calls b3Alloc. - // todo this is not true, Box3D allocates a lot. - b3AtomicFetchAddInt( &b3_byteCount, (int)size ); + b3AtomicFetchAddI64( &b3_byteCount, (int)size ); // Allocation must be a multiple of B3_ALIGNMENT (required by spec). // https://en.cppreference.com/w/c/memory/aligned_alloc - int alignedSize = ( ( (int)size - 1 ) | ( B3_ALIGNMENT - 1 ) ) + 1; + size_t alignedSize = ( ( size - 1 ) | ( B3_ALIGNMENT - 1 ) ) + 1; if ( b3_allocFcn != NULL ) { @@ -204,7 +202,8 @@ void b3Free( void* mem, size_t size ) if ( b3_freeFcn != NULL ) { - b3_freeFcn( mem ); + size_t alignedSize = ( ( size - 1 ) | ( B3_ALIGNMENT - 1 ) ) + 1; + b3_freeFcn( mem, alignedSize ); } else { @@ -215,7 +214,7 @@ void b3Free( void* mem, size_t size ) #endif } - b3AtomicFetchAddInt( &b3_byteCount, -(int)size ); + b3AtomicFetchAddI64( &b3_byteCount, -(int64_t)size ); } void* b3GrowAlloc( void* oldMem, int oldSize, int newSize ) @@ -237,9 +236,9 @@ void* b3GrowAllocZeroed( void* oldMem, int oldSize, int newSize ) return newMem; } -int b3GetByteCount( void ) +int64_t b3GetByteCount( void ) { - return b3AtomicLoadInt( &b3_byteCount ); + return b3AtomicLoadI64( &b3_byteCount ); } void* b3AllocZero( size_t size ) diff --git a/src/core.h b/src/core.h index 46eafa8..36d64f8 100644 --- a/src/core.h +++ b/src/core.h @@ -113,9 +113,17 @@ typedef struct b3AtomicU32 uint32_t value; } b3AtomicU32; +typedef struct b3AtomicI64 +{ + // 64-bit atomic wants 8-byte alignment + _Alignas( 8 ) int64_t value; +} b3AtomicI64; + // Minimum memory alignment used for all allocations #define B3_ALIGNMENT 16 +#define B3_RESTRICT restrict + // Returns the number of elements of an array #define B3_ARRAY_COUNT( A ) (int)( sizeof( A ) / sizeof( A[0] ) ) diff --git a/src/physics_world.c b/src/physics_world.c index 3a68a80..d954633 100644 --- a/src/physics_world.c +++ b/src/physics_world.c @@ -20,6 +20,7 @@ #include "scheduler.h" #include "sensor.h" #include "shape.h" +#include "simd.h" #include "solver.h" #include "solver_set.h" @@ -290,6 +291,7 @@ b3WorldId b3CreateWorld( const b3WorldDef* def ) int shapeCapacity = b3MaxInt( 16, def->capacity.staticShapeCount + def->capacity.dynamicShapeCount ); b3Array_Reserve( world->shapes, shapeCapacity ); + b3Array_Reserve( world->fatAABBs, shapeCapacity ); world->hullDatabase = b3Alloc( sizeof( b3HullMap ) ); b3HullMap_init( world->hullDatabase ); @@ -482,6 +484,7 @@ void b3DestroyWorld( b3WorldId worldId ) b3DestroyNameCache( &world->names ); b3Array_Destroy( world->shapes ); + b3Array_Destroy( world->fatAABBs ); b3Array_Destroy( world->contacts ); b3Array_Destroy( world->joints ); @@ -555,6 +558,24 @@ static inline void b3PrefetchContact( const b3Contact* contact ) b3Prefetch( p + 192 ); } +static inline b3BodySim* b3ResolveContactBodySim( b3World* world, b3BodySim* awakeSims, b3BodySim* staticSims, int encodedIndex, + int bodyId ) +{ + if ( encodedIndex >= 0 ) + { + return awakeSims + encodedIndex; + } + + if ( b3IsStaticSimIndex( encodedIndex ) ) + { + return staticSims - encodedIndex - 2; + } + + b3Body* body = b3Array_Get( world->bodies, bodyId ); + b3SolverSet* set = b3Array_Get( world->solverSets, body->setIndex ); + return b3Array_Get( set->bodySims, body->localIndex ); +} + static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* context ) { b3TracyCZoneNC( collide_task, "Collide Task", b3_colorDodgerBlue, true ); @@ -566,7 +587,7 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* int* contactIndices = stepContext->awakeContactIndices; b3Contact* contacts = world->contacts.data; b3Shape* shapes = world->shapes.data; - b3Body* bodies = world->bodies.data; + const b3AABB* fatAABBs = world->fatAABBs.data; b3BodySim* awakeSims = world->solverSets.data[b3_awakeSet].bodySims.data; b3BodySim* staticSims = world->solverSets.data[b3_staticSet].bodySims.data; @@ -595,11 +616,11 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* b3Contact* contact = contacts + contactIndex; B3_VALIDATE( contact->contactId == contactIndex ); - b3Shape* shapeA = shapes + contact->shapeIdA; - b3Shape* shapeB = shapes + contact->shapeIdB; + int shapeIdA = contact->shapeIdA; + int shapeIdB = contact->shapeIdB; // Do proxies still overlap? - bool overlap = b3AABB_Overlaps( shapeA->fatAABB, shapeB->fatAABB ); + bool overlap = b3OverlapV( fatAABBs + shapeIdA, fatAABBs + shapeIdB ); if ( overlap == false ) { // This contact will be destroyed @@ -609,51 +630,29 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* continue; } + b3Shape* shapeA = shapes + shapeIdA; + b3Shape* shapeB = shapes + shapeIdB; + // Update contact respecting shape/body order (A,B) - b3Body* bodyA = bodies + shapeA->bodyId; - b3Body* bodyB = bodies + shapeB->bodyId; - bool isStaticA = bodyA->type == b3_staticBody; - bool isStaticB = bodyB->type == b3_staticBody; + int encodedA = contact->encodedBodySimA; + int encodedB = contact->encodedBodySimB; + b3BodySim* bodySimA = b3ResolveContactBodySim( world, awakeSims, staticSims, encodedA, contact->edges[0].bodyId ); + b3BodySim* bodySimB = b3ResolveContactBodySim( world, awakeSims, staticSims, encodedB, contact->edges[1].bodyId ); + bool wasTouching = ( contact->flags & b3_simTouchingFlag ); - b3BodySim* bodySimA; - b3BodySim* bodySimB; - if ( wasTouching ) - { - B3_ASSERT( bodyA->setIndex == b3_awakeSet || bodyA->setIndex == b3_staticSet ); - B3_ASSERT( bodyB->setIndex == b3_awakeSet || bodyB->setIndex == b3_staticSet ); - bodySimA = ( isStaticA ? staticSims : awakeSims ) + bodyA->localIndex; - bodySimB = ( isStaticB ? staticSims : awakeSims ) + bodyB->localIndex; - } - else - { - // There can be non-touching contacts between awake bodies and sleeping bodies. - { - b3SolverSet* set = b3Array_Get( world->solverSets, bodyA->setIndex ); - bodySimA = b3Array_Get( set->bodySims, bodyA->localIndex ); - } - { - b3SolverSet* set = b3Array_Get( world->solverSets, bodyB->setIndex ); - bodySimB = b3Array_Get( set->bodySims, bodyB->localIndex ); - } - } b3WorldTransform transformA = bodySimA->transform; b3WorldTransform transformB = bodySimB->transform; - bool isFast = ( bodySimA->flags & b3_isFast ) || ( bodySimB->flags & b3_isFast ); + bool isFast = ( ( bodySimA->flags | bodySimB->flags ) & b3_isFast ) != 0; - // These are used by the contact solver. If the contact is between an awake body - // and a sleeping body and the contact begins to touch, the these will be invalid - // but fixed when linked in the constraint graph. - contact->bodySimIndexA = isStaticA ? B3_NULL_INDEX : bodyA->localIndex; - contact->bodySimIndexB = isStaticB ? B3_NULL_INDEX : bodyB->localIndex; float recycleTolerance = wasTouching ? recycleDistance : recycleDistanceNonTouching; // Contact recycling optimization. Please cite this library if you use this optimization. // This is inspired by persistent contact manifolds used in some physics engines, such as PhysX. // However, this allows larger relative motion and has fewer tuning parameters (just one). - if ( isFast == false && recycleDistance > 0.0f && - ( contact->flags & b3_relativeTransformValid ) && ( contact->flags & b3_contactRecycleFlag ) ) + if ( isFast == false && recycleDistance > 0.0f && ( contact->flags & b3_relativeTransformValid ) && + ( contact->flags & b3_contactRecycleFlag ) ) { // The scalar part of b3InvMulQuat is just the quaternion dot product. // cos(relative_angle/2) = scalar(conj(q1) * q2) = dot(q1, q2) @@ -665,8 +664,8 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* b3Transform xf = b3InvMulWorldTransforms( transformA, transformB ); b3Transform xfc = contact->cachedRelativePose; - b3Vec3 maxExtentA = isStaticA ? b3Vec3_zero : bodySimA->maxExtent; - b3Vec3 maxExtentB = isStaticB ? b3Vec3_zero : bodySimB->maxExtent; + b3Vec3 maxExtentA = b3IsStaticSimIndex( encodedA ) ? b3Vec3_zero : bodySimA->maxExtent; + b3Vec3 maxExtentB = b3IsStaticSimIndex( encodedB ) ? b3Vec3_zero : bodySimB->maxExtent; b3Vec3 maxExtent = b3Max( maxExtentA, maxExtentB ); // Variation of Conservative Advancement @@ -772,14 +771,19 @@ static void b3CollideTask( int startIndex, int endIndex, int workerIndex, void* b3SetBit( &taskContext->contactStateBitSet, contactIndex ); } - for ( int manifoldIndex = 0; manifoldIndex < contact->manifoldCount; ++manifoldIndex ) + if ( touching ) { - b3Manifold* manifold = contact->manifolds + manifoldIndex; - for ( int pointIndex = 0; pointIndex < manifold->pointCount; ++pointIndex ) + int manifoldCount = contact->manifoldCount; + for ( int manifoldIndex = 0; manifoldIndex < manifoldCount; ++manifoldIndex ) { - // Cache separation - b3ManifoldPoint* mp = manifold->points + pointIndex; - mp->baseSeparation = mp->separation; + b3Manifold* manifold = contact->manifolds + manifoldIndex; + int pointCount = manifold->pointCount; + for ( int pointIndex = 0; pointIndex < pointCount; ++pointIndex ) + { + // Cache separation + b3ManifoldPoint* mp = manifold->points + pointIndex; + mp->baseSeparation = mp->separation; + } } } } @@ -793,8 +797,6 @@ static void b3AddNonTouchingContact( b3World* world, b3Contact* contact ) b3SolverSet* set = b3Array_Get( world->solverSets, b3_awakeSet ); contact->colorIndex = B3_NULL_INDEX; contact->localIndex = set->contactIndices.count; - contact->bodySimIndexA = B3_NULL_INDEX; - contact->bodySimIndexB = B3_NULL_INDEX; b3Array_Push( set->contactIndices, contact->contactId ); } @@ -1358,7 +1360,7 @@ static bool DrawQueryCallback( int proxyId, uint64_t userData, void* context ) if ( draw->drawBounds ) { - draw->DrawBoundsFcn( shape->fatAABB, b3_colorGold, draw->context ); + draw->DrawBoundsFcn( world->fatAABBs.data[shapeId], b3_colorGold, draw->context ); } return true; @@ -1440,7 +1442,7 @@ void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits ) b3WorldTransform transform = { bodySim->center, bodySim->transform.q }; draw->DrawTransformFcn( transform, draw->context ); - if (body->type == b3_dynamicBody) + if ( body->type == b3_dynamicBody ) { b3Vec3 offset = { 0.05f, 0.05f, 0.05f }; b3Pos p = b3TransformWorldPoint( transform, offset ); @@ -1563,9 +1565,7 @@ void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits ) { // Hack inv_dt for single step debugging float inv_dt = world->inv_dt > 0.0f ? world->inv_dt : 60.0f; - // todo validate - // multiply by one-half due to relax iteration - float force = 0.5f * mp->totalNormalImpulse * inv_dt; + float force = mp->totalNormalImpulse * inv_dt; b3Pos p1 = p; b3Pos p2 = b3OffsetPos( p1, b3MulSV( draw->forceScale * force, normal ) ); draw->DrawSegmentFcn( p1, p2, impulseColor, draw->context ); @@ -1657,7 +1657,7 @@ void b3World_Draw( b3WorldId worldId, b3DebugDraw* draw, uint64_t maskBits ) while ( shapeId != B3_NULL_INDEX ) { b3Shape* shape = b3Array_Get( world->shapes, shapeId ); - aabb = b3AABB_Union( aabb, shape->fatAABB ); + aabb = b3AABB_Union( aabb, world->fatAABBs.data[shapeId] ); shapeCount += 1; shapeId = shape->nextShapeId; } @@ -3575,6 +3575,21 @@ void b3ValidateSolverSets( b3World* world ) B3_ASSERT( b3GetIdCapacity( &world->islandIdPool ) == world->islands.count ); B3_ASSERT( b3GetIdCapacity( &world->solverSetIdPool ) == world->solverSets.count ); + // Ensure the encoded body sim indices are correct. + for ( int contactIndex = 0; contactIndex < world->contacts.count; ++contactIndex ) + { + b3Contact* contact = world->contacts.data + contactIndex; + if ( contact->contactId == B3_NULL_INDEX ) + { + continue; + } + + b3Body* bodyA = b3Array_Get( world->bodies, contact->edges[0].bodyId ); + b3Body* bodyB = b3Array_Get( world->bodies, contact->edges[1].bodyId ); + B3_ASSERT( contact->encodedBodySimA == b3EncodeBodySimIndex( bodyA ) ); + B3_ASSERT( contact->encodedBodySimB == b3EncodeBodySimIndex( bodyB ) ); + } + int activeSetCount = 0; int totalBodyCount = 0; int totalJointCount = 0; @@ -3626,7 +3641,7 @@ void b3ValidateSolverSets( b3World* world ) B3_ASSERT( body->setIndex == setIndex ); B3_ASSERT( body->localIndex == i ); - uint32_t syncedFlags = body->flags & ~b3_bodyTransientFlags; + uint32_t syncedFlags = body->flags & ~( b3_isFast | b3_bodyTransientFlags ); B3_ASSERT( ( bodySim->flags & syncedFlags ) == syncedFlags ); b3BodyState* bodyState = b3GetBodyState( world, body ); @@ -3965,30 +3980,6 @@ void b3ValidateContacts( b3World* world ) if ( touching ) { B3_ASSERT( 0 <= contact->colorIndex && contact->colorIndex < B3_GRAPH_COLOR_COUNT ); - // Validate body sim indices - b3Shape* shapeA = b3Array_Get( world->shapes, contact->shapeIdA ); - b3Shape* shapeB = b3Array_Get( world->shapes, contact->shapeIdB ); - - b3Body* bodyA = b3Array_Get( world->bodies, shapeA->bodyId ); - b3Body* bodyB = b3Array_Get( world->bodies, shapeB->bodyId ); - - if ( bodyA->type == b3_staticBody ) - { - B3_ASSERT( contact->bodySimIndexA == B3_NULL_INDEX ); - } - else - { - B3_ASSERT( contact->bodySimIndexA == bodyA->localIndex ); - } - - if ( bodyB->type == b3_staticBody ) - { - B3_ASSERT( contact->bodySimIndexB == B3_NULL_INDEX ); - } - else - { - B3_ASSERT( contact->bodySimIndexB == bodyB->localIndex ); - } if ( ( contact->flags & b3_simMeshContact ) != 0 || contact->colorIndex == B3_OVERFLOW_INDEX ) { diff --git a/src/physics_world.h b/src/physics_world.h index 47263a6..fcfee9f 100644 --- a/src/physics_world.h +++ b/src/physics_world.h @@ -33,6 +33,7 @@ b3DeclareArray( b3SolverSet ); b3DeclareArray( b3Joint ); b3DeclareArray( b3Contact ); b3DeclareArray( b3Island ); +b3DeclareArray( b3AABB ); b3DeclareArray( b3Shape ); b3DeclareArray( b3Sensor ); b3DeclareArray( b3SensorTaskContext ); @@ -176,6 +177,9 @@ typedef struct b3World // These are sparse arrays that point into the pools above b3Array( b3Shape ) shapes; + // This follows the shape array and is split out for the hot path in the narrow phase. + b3Array( b3AABB ) fatAABBs; + // Compound shapes are static only, so only the broad-phase static pass can meet one. This // lets a world without compounds skip the shape type read in the pair batch. int compoundShapeCount; diff --git a/src/platform.h b/src/platform.h index c6a6d40..856b19b 100644 --- a/src/platform.h +++ b/src/platform.h @@ -132,3 +132,34 @@ static inline uint32_t b3AtomicFetchOrU32( uint32_t* a, uint32_t mask ) #error "Unsupported platform" #endif } + +static inline int64_t b3AtomicFetchAddI64( b3AtomicI64* a, int64_t increment ) +{ +#if defined( _MSC_VER ) + return (int64_t)_InterlockedExchangeAdd64( (__int64*)&a->value, (__int64)increment ); +#elif defined( __GNUC__ ) || defined( __clang__ ) + return __atomic_fetch_add( &a->value, increment, __ATOMIC_SEQ_CST ); +#else +#error "Unsupported platform" +#endif +} + +static inline int64_t b3AtomicLoadI64( b3AtomicI64* a ) +{ +#if defined( _MSC_VER ) && !defined( __clang__ ) && !defined( _M_ARM ) + int64_t value = __iso_volatile_load64( (volatile __int64*)&a->value ); +#if defined( _M_ARM64 ) || defined( _M_ARM64EC ) + __dmb( 0xB ); +#else + _ReadWriteBarrier(); +#endif + return value; +#elif defined( _MSC_VER ) && !defined( __clang__ ) + // 32-bit ARM has no plain atomic 64-bit load + return _InterlockedOr64( (__int64*)&a->value, 0 ); +#elif defined( __GNUC__ ) || defined( __clang__ ) + return __atomic_load_n( &a->value, __ATOMIC_SEQ_CST ); +#else +#error "Unsupported platform" +#endif +} diff --git a/src/recording.h b/src/recording.h index b6b7af8..e7c0cd2 100644 --- a/src/recording.h +++ b/src/recording.h @@ -51,8 +51,8 @@ typedef struct b3World b3World; #define B3_REC_VERSION_MAJOR 6 // Minor tracks op-stream additions that keep the 48 byte header shape. -// Minor version 6 moved the broad phase to the tree against tree pair traversal -#define B3_REC_VERSION_MINOR 6 +// Minor version 7 made the contact body sim locators persistent +#define B3_REC_VERSION_MINOR 7 // File header, fixed 48 bytes. Contains the registry locator so the player // can load geometry before replaying any ops. diff --git a/src/shape.c b/src/shape.c index bd40c96..1f6f38b 100644 --- a/src/shape.c +++ b/src/shape.c @@ -75,7 +75,7 @@ static float b3ComputeShapeMargin( b3Shape* shape ) return b3MinFloat( B3_MAX_AABB_MARGIN, B3_AABB_MARGIN_FRACTION * margin ); } -static void b3UpdateShapeAABBs( b3Shape* shape, b3WorldTransform transform, b3BodyType proxyType ) +static void b3UpdateShapeAABBs( b3Shape* shape, b3AABB* fatAABB, b3WorldTransform transform, b3BodyType proxyType ) { // Compute a bounding box with a speculative margin const float speculativeDistance = B3_SPECULATIVE_DISTANCE; @@ -86,14 +86,7 @@ static void b3UpdateShapeAABBs( b3Shape* shape, b3WorldTransform transform, b3Bo // Smaller margin for static bodies. Cannot be zero due to TOI tolerance. float margin = proxyType == b3_staticBody ? speculativeDistance : aabbMargin; - b3AABB fatAABB; - fatAABB.lowerBound.x = aabb.lowerBound.x - margin; - fatAABB.lowerBound.y = aabb.lowerBound.y - margin; - fatAABB.lowerBound.z = aabb.lowerBound.z - margin; - fatAABB.upperBound.x = aabb.upperBound.x + margin; - fatAABB.upperBound.y = aabb.upperBound.y + margin; - fatAABB.upperBound.z = aabb.upperBound.z + margin; - shape->fatAABB = fatAABB; + *fatAABB = b3AABB_Inflate( aabb, margin ); } static b3Shape* b3CreateShapeInternal( b3World* world, b3Body* body, b3WorldTransform bodyTransform, const b3ShapeDef* def, @@ -105,12 +98,15 @@ static b3Shape* b3CreateShapeInternal( b3World* world, b3Body* body, b3WorldTran if ( shapeId == world->shapes.count ) { b3Array_Push( world->shapes, (b3Shape){ 0 } ); + b3Array_Push( world->fatAABBs, (b3AABB){ 0 } ); } else { B3_ASSERT( world->shapes.data[shapeId].id == B3_NULL_INDEX ); } + B3_ASSERT( world->fatAABBs.count == world->shapes.count ); + b3Shape* shape = b3Array_Get( world->shapes, shapeId ); switch ( shapeType ) @@ -186,7 +182,7 @@ static b3Shape* b3CreateShapeInternal( b3World* world, b3Body* body, b3WorldTran shape->localCentroid = b3GetShapeCentroid( shape ); shape->aabbMargin = b3ComputeShapeMargin( shape ); shape->aabb = (b3AABB){ b3Vec3_zero, b3Vec3_zero }; - shape->fatAABB = (b3AABB){ b3Vec3_zero, b3Vec3_zero }; + world->fatAABBs.data[shapeId] = (b3AABB){ b3Vec3_zero, b3Vec3_zero }; shape->nameId = b3AddName( &world->names, def->name ); shape->generation += 1; @@ -220,7 +216,7 @@ static b3Shape* b3CreateShapeInternal( b3World* world, b3Body* body, b3WorldTran { b3BodyType proxyType = body->type; bool forcePairCreation = def->invokeContactCreation && shape->type != b3_compoundShape; - b3CreateShapeProxy( shape, &world->broadPhase, proxyType, bodyTransform, forcePairCreation ); + b3CreateShapeProxy( world, shape, proxyType, bodyTransform, forcePairCreation ); } // Add to shape doubly linked list @@ -1004,15 +1000,16 @@ int b3CollideMover( b3PlaneResult* planes, int planeCapacity, const b3Shape* sha return planeCount; } -void b3CreateShapeProxy( b3Shape* shape, b3BroadPhase* bp, b3BodyType type, b3WorldTransform transform, bool forcePairCreation ) +void b3CreateShapeProxy( b3World* world, b3Shape* shape, b3BodyType type, b3WorldTransform transform, bool forcePairCreation ) { B3_ASSERT( shape->proxyKey == B3_NULL_INDEX ); - b3UpdateShapeAABBs( shape, transform, type ); + b3AABB* fatAABB = world->fatAABBs.data + shape->id; + b3UpdateShapeAABBs( shape, fatAABB, transform, type ); // Create proxies in the broad-phase. - shape->proxyKey = - b3BroadPhase_CreateProxy( bp, type, shape->fatAABB, shape->filter.categoryBits, shape->id, forcePairCreation ); + shape->proxyKey = b3BroadPhase_CreateProxy( &world->broadPhase, type, *fatAABB, shape->filter.categoryBits, shape->id, + forcePairCreation ); B3_ASSERT( B3_PROXY_TYPE( shape->proxyKey ) < b3_bodyTypeCount ); } @@ -1360,25 +1357,26 @@ static void b3ResetProxy( b3World* world, b3Shape* shape, bool wakeBodies, bool if ( shape->proxyKey != B3_NULL_INDEX ) { b3BodyType proxyType = B3_PROXY_TYPE( shape->proxyKey ); - b3UpdateShapeAABBs( shape, transform, proxyType ); + b3AABB* fatAABB = world->fatAABBs.data + shapeId; + b3UpdateShapeAABBs( shape, fatAABB, transform, proxyType ); if ( destroyProxy ) { b3BroadPhase_DestroyProxy( &world->broadPhase, shape->proxyKey ); bool forcePairCreation = true; - shape->proxyKey = b3BroadPhase_CreateProxy( &world->broadPhase, proxyType, shape->fatAABB, shape->filter.categoryBits, + shape->proxyKey = b3BroadPhase_CreateProxy( &world->broadPhase, proxyType, *fatAABB, shape->filter.categoryBits, shapeId, forcePairCreation ); } else { - b3BroadPhase_MoveProxy( &world->broadPhase, shape->proxyKey, shape->fatAABB ); + b3BroadPhase_MoveProxy( &world->broadPhase, shape->proxyKey, *fatAABB ); } } else { b3BodyType proxyType = body->type; - b3UpdateShapeAABBs( shape, transform, proxyType ); + b3UpdateShapeAABBs( shape, world->fatAABBs.data + shapeId, transform, proxyType ); } b3ValidateSolverSets( world ); diff --git a/src/shape.h b/src/shape.h index 7082bc7..91e8eef 100644 --- a/src/shape.h +++ b/src/shape.h @@ -37,7 +37,6 @@ typedef struct b3Shape float aabbMargin; b3AABB aabb; - b3AABB fatAABB; b3Vec3 localCentroid; int materialCount; @@ -74,7 +73,7 @@ static inline b3SurfaceMaterial* b3GetShapeMaterials( const b3Shape* shape ) return shape->materials != NULL ? shape->materials : (b3SurfaceMaterial*)&shape->material; } -void b3CreateShapeProxy( b3Shape* shape, b3BroadPhase* bp, b3BodyType type, b3WorldTransform transform, bool forcePairCreation ); +void b3CreateShapeProxy( b3World* world, b3Shape* shape, b3BodyType type, b3WorldTransform transform, bool forcePairCreation ); void b3DestroyShapeProxy( b3Shape* shape, b3BroadPhase* bp ); void b3DestroyShapeAllocations( b3World* world, b3Shape* shape ); diff --git a/src/simd.h b/src/simd.h index 258c31e..92ba2bf 100644 --- a/src/simd.h +++ b/src/simd.h @@ -476,6 +476,20 @@ static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b ) return vreinterpretq_f32_u32( vorrq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) ); } +// a & ~b +static inline b3FloatW b3AndNotW( b3FloatW a, b3FloatW b ) +{ + return vreinterpretq_f32_u32( vbicq_u32( vreinterpretq_u32_f32( a ), vreinterpretq_u32_f32( b ) ) ); +} + +static inline b3FloatW b3SoftMaskW( const int* indexA, const int* indexB ) +{ + int32x4_t zero = vdupq_n_s32( 0 ); + uint32x4_t a = vceqq_s32( vld1q_s32( (const int32_t*)indexA ), zero ); + uint32x4_t b = vceqq_s32( vld1q_s32( (const int32_t*)indexB ), zero ); + return vreinterpretq_f32_u32( vorrq_u32( a, b ) ); +} + static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b ) { return vreinterpretq_f32_u32( vcgtq_f32( a, b ) ); @@ -650,6 +664,22 @@ static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b ) return _mm_or_ps( a, b ); } +// a & ~b +static inline b3FloatW b3AndNotW( b3FloatW a, b3FloatW b ) +{ + // Arguments are reversed + return _mm_andnot_ps( b, a ); +} + +// This is used to optimize selection of contact softness. +static inline b3FloatW b3SoftMaskW( const int* indexA, const int* indexB ) +{ + __m128i zero = _mm_setzero_si128(); + __m128i a = _mm_cmpeq_epi32( _mm_loadu_si128( (const __m128i*)indexA ), zero ); + __m128i b = _mm_cmpeq_epi32( _mm_loadu_si128( (const __m128i*)indexB ), zero ); + return _mm_castsi128_ps( _mm_or_si128( a, b ) ); +} + static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b ) { return _mm_cmpgt_ps( a, b ); @@ -834,6 +864,27 @@ static inline b3FloatW b3OrW( b3FloatW a, b3FloatW b ) return r; } +// a & ~b +static inline b3FloatW b3AndNotW( b3FloatW a, b3FloatW b ) +{ + b3FloatW r; + r.x = a.x != 0.0f && b.x == 0.0f ? 1.0f : 0.0f; + r.y = a.y != 0.0f && b.y == 0.0f ? 1.0f : 0.0f; + r.z = a.z != 0.0f && b.z == 0.0f ? 1.0f : 0.0f; + r.w = a.w != 0.0f && b.w == 0.0f ? 1.0f : 0.0f; + return r; +} + +static inline b3FloatW b3SoftMaskW( const int* indexA, const int* indexB ) +{ + b3FloatW r; + r.x = indexA[0] == 0 || indexB[0] == 0 ? 1.0f : 0.0f; + r.y = indexA[1] == 0 || indexB[1] == 0 ? 1.0f : 0.0f; + r.z = indexA[2] == 0 || indexB[2] == 0 ? 1.0f : 0.0f; + r.w = indexA[3] == 0 || indexB[3] == 0 ? 1.0f : 0.0f; + return r; +} + static inline b3FloatW b3GreaterThanW( b3FloatW a, b3FloatW b ) { b3FloatW r; diff --git a/src/solver.c b/src/solver.c index bfb5ccd..2773ea2 100644 --- a/src/solver.c +++ b/src/solver.c @@ -24,8 +24,6 @@ #include #include -_Static_assert( B3_RESTITUTION_ITERATIONS >= 1, "must be 1 or more" ); - // these are useful for solver testing #define ITERATIONS 1 #define RELAX_ITERATIONS 1 @@ -473,7 +471,7 @@ static bool b3ContinuousQueryCallback( int proxyId, uint64_t userData, void* con } // Continuous collision of dynamic versus static -static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext* taskContext ) +static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext* taskContext, float dt ) { b3TracyCZoneNC( ccd, "CCD", b3_colorDarkGoldenRod, true ); @@ -481,7 +479,7 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext* b3SolverSet* awakeSet = b3Array_Get( world->solverSets, b3_awakeSet ); b3BodySim* fastBodySim = b3Array_Get( awakeSet->bodySims, bodySimIndex ); - B3_ASSERT( fastBodySim->flags & b3_isFast ); + B3_VALIDATE( fastBodySim->flags & b3_isFast ); // Re-center the sweep on the fast body so the TOI and the swept query stay in float precision b3Pos base = fastBodySim->center0; @@ -567,6 +565,16 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext* fastBodySim->rotation0 = q; fastBodySim->center0 = center; + // Timeloss means there is a lost gravity contribution. Other forces and torques are ignored for now. + b3BodyState* fastBodyState = b3Array_Get( awakeSet->bodyStates, bodySimIndex ); + b3Vec3 v = fastBodyState->linearVelocity; + float timeLoss = ( 1.0f - context.fraction ) * dt; + b3Vec3 dv = b3MulSV( -timeLoss * fastBodySim->gravityScale, world->gravity ); + dv.x = ( fastBodyState->flags & b3_lockLinearX ) ? 0.0f : dv.x; + dv.y = ( fastBodyState->flags & b3_lockLinearY ) ? 0.0f : dv.y; + dv.z = ( fastBodyState->flags & b3_lockLinearZ ) ? 0.0f : dv.z; + fastBodyState->linearVelocity = b3Add( v, dv ); + // The move event was written before CCD, so correct it with the impact pose b3BodyMoveEvent* event = b3Array_Get( world->bodyMoveEvents, bodySimIndex ); event->transform = fastBodySim->transform; @@ -583,19 +591,20 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext* b3AABB aabb = b3ComputeFatShapeAABB( shape, transform, speculativeScalar ); shape->aabb = aabb; - if ( b3AABB_Contains( shape->fatAABB, aabb ) == false ) + b3AABB* shapeFatAABB = world->fatAABBs.data + shapeId; + if ( b3AABB_Contains( *shapeFatAABB, aabb ) == false ) { - float marginScalar = shape->aabbMargin; - b3Vec3 aabbMargin = { marginScalar, marginScalar, marginScalar }; - shape->fatAABB = (b3AABB){ b3Sub( aabb.lowerBound, aabbMargin ), b3Add( aabb.upperBound, aabbMargin ) }; - - fastBodySim->flags |= b3_enlargeBounds; + *shapeFatAABB = b3AABB_Inflate( aabb, shape->aabbMargin ); // Regular bodies mark the hierarchy as moved using atomic operations. // Bullets are handled separately at a later stage. if ( isBullet == false ) { - b3BroadPhase_MarkProxyMoved( &world->broadPhase, shape->proxyKey, shape->fatAABB ); + b3BroadPhase_MarkProxyMoved( &world->broadPhase, shape->proxyKey, *shapeFatAABB ); + } + else + { + fastBodySim->flags |= b3_enlargeBulletBounds; } } @@ -618,20 +627,18 @@ static void b3SolveContinuous( b3World* world, int bodySimIndex, b3TaskContext* // shape->aabb is still valid from above - if ( b3AABB_Contains( shape->fatAABB, shape->aabb ) == false ) + b3AABB* shapeFatAABB = world->fatAABBs.data + shapeId; + if ( b3AABB_Contains( *shapeFatAABB, shape->aabb ) == false ) { - float marginScalar = shape->aabbMargin; - b3Vec3 aabbMargin = { marginScalar, marginScalar, marginScalar }; - shape->fatAABB = (b3AABB){ - .lowerBound = b3Sub( shape->aabb.lowerBound, aabbMargin ), - .upperBound = b3Add( shape->aabb.upperBound, aabbMargin ), - }; - - fastBodySim->flags |= b3_enlargeBounds; + *shapeFatAABB = b3AABB_Inflate( shape->aabb, shape->aabbMargin ); if ( isBullet == false ) { - b3BroadPhase_MarkProxyMoved( &world->broadPhase, shape->proxyKey, shape->fatAABB ); + b3BroadPhase_MarkProxyMoved( &world->broadPhase, shape->proxyKey, *shapeFatAABB ); + } + else + { + fastBodySim->flags |= b3_enlargeBulletBounds; } } @@ -754,10 +761,13 @@ static void b3FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex, // or b3Body_SetMassData. B3_ASSERT( ( body->flags & b3_dirtyMass ) == 0 ); + // Clear the transient flags (fast, speed capped, had TOI). These flags are conditionally set + // as part of the code below. body->flags &= ~b3_bodyTransientFlags; - body->flags |= ( sim->flags & ( b3_isSpeedCapped | b3_hadTimeOfImpact ) ); - body->flags |= ( state->flags & ( b3_isSpeedCapped | b3_hadTimeOfImpact ) ); - sim->flags &= ~b3_bodyTransientFlags; + sim->flags &= ~( b3_isFast | b3_bodyTransientFlags ); + + // The body state flag knows about speed capping (used for debug draw). + body->flags |= ( state->flags & b3_isSpeedCapped ); state->flags &= ~b3_bodyTransientFlags; if ( enableSleep == false || ( body->flags & b3_enableSleep ) == 0 || sleepVelocity > body->sleepThreshold ) @@ -781,7 +791,7 @@ static void b3FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex, } else { - b3SolveContinuous( world, simIndex, taskContext ); + b3SolveContinuous( world, simIndex, taskContext, timeStep ); } } else @@ -839,14 +849,13 @@ static void b3FinalizeBodiesTask( int startIndex, int endIndex, int workerIndex, b3AABB aabb = b3ComputeFatShapeAABB( shape, transform, speculativeScalar ); shape->aabb = aabb; - if ( b3AABB_Contains( shape->fatAABB, aabb ) == false ) + b3AABB* shapeFatAABB = world->fatAABBs.data + shapeId; + if ( b3AABB_Contains( *shapeFatAABB, aabb ) == false ) { - float marginScalar = shape->aabbMargin; - b3Vec3 aabbMargin = { marginScalar, marginScalar, marginScalar }; - shape->fatAABB = (b3AABB){ b3Sub( aabb.lowerBound, aabbMargin ), b3Add( aabb.upperBound, aabbMargin ) }; + *shapeFatAABB = b3AABB_Inflate( aabb, shape->aabbMargin ); // Mark the hierarchy as moved using atomic operations. - b3BroadPhase_MarkProxyMoved( &world->broadPhase, shape->proxyKey, shape->fatAABB ); + b3BroadPhase_MarkProxyMoved( &world->broadPhase, shape->proxyKey, *shapeFatAABB ); } shapeId = shape->nextShapeId; @@ -1006,13 +1015,11 @@ static void b3ExecuteBlock( b3SolverStage* stage, b3StepContext* context, b3Solv } else if ( blockType == b3_graphWideContactBlock ) { - bool useBias = true; - b3SolveContacts_Convex( block, context, useBias ); + b3PushContacts_Convex( block, context ); } else { - bool useBias = true; - b3SolveContacts_Mesh( block, context, useBias ); + b3PushContacts_Mesh( block, context ); } break; @@ -1028,24 +1035,11 @@ static void b3ExecuteBlock( b3SolverStage* stage, b3StepContext* context, b3Solv } else if ( blockType == b3_graphWideContactBlock ) { - bool useBias = false; - b3SolveContacts_Convex( block, context, useBias ); + b3SolveContacts_Convex( block, context ); } else { - bool useBias = false; - b3SolveContacts_Mesh( block, context, useBias ); - } - break; - - case b3_stageRestitution: - if ( blockType == b3_graphWideContactBlock ) - { - b3ApplyRestitution_Convex( block, context ); - } - else if ( blockType == b3_graphContactBlock ) - { - b3ApplyRestitution_Mesh( block, context ); + b3SolveContacts_Mesh( block, context ); } break; @@ -1188,7 +1182,6 @@ static void b3SolverTask( void* taskContext ) b3_stageSolve, b3_stageIntegratePositions, b3_stageRelax, - b3_stageRestitution, b3_stageStoreImpulses */ @@ -1265,7 +1258,7 @@ static void b3SolverTask( void* taskContext ) { // Overflow constraints have lower priority. Typically these are dynamic-vs-dynamic. b3SolveJoints_Overflow( context, useBias ); - b3SolveContacts_Overflow( context, useBias ); + b3PushContacts_Overflow( context ); for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) { @@ -1293,7 +1286,7 @@ static void b3SolverTask( void* taskContext ) for ( int j = 0; j < RELAX_ITERATIONS; ++j ) { b3SolveJoints_Overflow( context, useBias ); - b3SolveContacts_Overflow( context, useBias ); + b3SolveContacts_Overflow( context ); for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) { @@ -1312,25 +1305,6 @@ static void b3SolverTask( void* taskContext ) // integrate velocities / warm start / solve / integrate positions / relax stageIndex += 1 + activeColorCount + ITERATIONS * activeColorCount + 1 + RELAX_ITERATIONS * activeColorCount; - // Restitution - for ( int iteration = 0; iteration < B3_RESTITUTION_ITERATIONS; ++iteration ) - { - b3ApplyRestitution_Overflow( context ); - - int iterStageIndex = stageIndex; - for ( int colorIndex = 0; colorIndex < activeColorCount; ++colorIndex ) - { - syncBits = ( graphSyncIndex << 16 ) | iterStageIndex; - B3_ASSERT( stages[iterStageIndex].type == b3_stageRestitution ); - b3ExecuteMainStage( stages + iterStageIndex, context, syncBits ); - iterStageIndex += 1; - } - graphSyncIndex += 1; - stageIndex += activeColorCount; - } - - profile->applyRestitution += b3GetMillisecondsAndReset( &ticks ); - // Store impulses b3StoreImpulses_Overflow( context ); @@ -1420,7 +1394,7 @@ static void b3BulletBodyTask( int startIndex, int endIndex, int workerIndex, voi for ( int i = startIndex; i < endIndex; ++i ) { int simIndex = stepContext->bulletBodies[i]; - b3SolveContinuous( stepContext->world, simIndex, taskContext ); + b3SolveContinuous( stepContext->world, simIndex, taskContext, stepContext->dt ); } b3TracyCZoneEnd( bullet_body_task ); @@ -1689,8 +1663,6 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) stageCount += 1; // b3_stageRelax stageCount += RELAX_ITERATIONS * activeColorCount; - // b3_stageRestitution - stageCount += B3_RESTITUTION_ITERATIONS * activeColorCount; // b3_stageStoreWideImpulses stageCount += 1; // b3_stageStoreImpulses @@ -1774,9 +1746,6 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) stage = b3InitStage( stage, b3_stageIntegratePositions, bodyBlocks, bodyDim.count, UINT8_MAX ); stage = b3InitColorStages( stage, b3_stageRelax, RELAX_ITERATIONS, activeColorCount, graphColorBlocks, graphBlockCounts, activeColorIndices ); - // Note: joint blocks mixed in, could have joint limit restitution - stage = b3InitColorStages( stage, b3_stageRestitution, B3_RESTITUTION_ITERATIONS, activeColorCount, graphColorBlocks, - graphBlockCounts, activeColorIndices ); stage = b3InitStage( stage, b3_stageStoreWideImpulses, convexBlocks, convexPrepareDim.count, UINT8_MAX ); stage = b3InitStage( stage, b3_stageStoreImpulses, meshBlocks, meshPrepareDim.count, UINT8_MAX ); @@ -2117,13 +2086,13 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) for ( int i = 0; i < bulletBodyCount; ++i ) { b3BodySim* bulletBodySim = bodySimArray + bulletBodySimIndices[i]; - if ( ( bulletBodySim->flags & b3_enlargeBounds ) == 0 ) + if ( ( bulletBodySim->flags & b3_enlargeBulletBounds ) == 0 ) { continue; } // Clear flag - bulletBodySim->flags &= ~b3_enlargeBounds; + bulletBodySim->flags &= ~b3_enlargeBulletBounds; int bodyId = bulletBodySim->bodyId; B3_ASSERT( 0 <= bodyId && bodyId < world->bodies.count ); @@ -2139,10 +2108,11 @@ void b3Solve( b3World* world, b3StepContext* stepContext ) B3_VALIDATE( B3_PROXY_TYPE( proxyKey ) == b3_dynamicBody ); b3AABB treeAABB = b3DynamicTree_GetAABB( dynamicTree, proxyId ); + b3AABB shapeFatAABB = world->fatAABBs.data[shapeId]; - if ( b3AABB_Contains( treeAABB, shape->fatAABB ) == false ) + if ( b3AABB_Contains( treeAABB, shapeFatAABB ) == false ) { - b3DynamicTree_EnlargeProxy( dynamicTree, proxyId, shape->fatAABB ); + b3DynamicTree_EnlargeProxy( dynamicTree, proxyId, shapeFatAABB ); } shapeId = shape->nextShapeId; diff --git a/src/solver.h b/src/solver.h index 7c8a1c7..3f96a65 100644 --- a/src/solver.h +++ b/src/solver.h @@ -76,7 +76,6 @@ typedef enum b3SolverStageType b3_stageSolve, b3_stageIntegratePositions, b3_stageRelax, - b3_stageRestitution, b3_stageStoreWideImpulses, b3_stageStoreImpulses, } b3SolverStageType; diff --git a/src/solver_set.c b/src/solver_set.c index f050a86..53b75dd 100644 --- a/src/solver_set.c +++ b/src/solver_set.c @@ -62,7 +62,8 @@ void b3WakeSolverSet( b3World* world, int setIndex ) *state = b3_identityBodyState; state->flags = body->flags; - // move non-touching contacts from disabled set to awake set + // Move non-touching contacts from disabled set to awake set. + int encodedBodySimIndex = body->localIndex; int contactKey = body->headContactKey; while ( contactKey != B3_NULL_INDEX ) { @@ -75,6 +76,21 @@ void b3WakeSolverSet( b3World* world, int setIndex ) if ( contact->setIndex != b3_disabledSet ) { B3_ASSERT( contact->setIndex == b3_awakeSet || contact->setIndex == setIndex ); + + // If this contact is awake but not touching I need to fixup the body sim index here. + if ( contact->setIndex == b3_awakeSet ) + { + B3_ASSERT( contact->colorIndex == B3_NULL_INDEX ); + if ( edgeIndex == 0 ) + { + contact->encodedBodySimA = encodedBodySimIndex; + } + else + { + contact->encodedBodySimB = encodedBodySimIndex; + } + } + continue; } @@ -89,6 +105,15 @@ void b3WakeSolverSet( b3World* world, int setIndex ) contact->localIndex = awakeSet->contactIndices.count; b3Array_Push( awakeSet->contactIndices, contactId ); + if ( edgeIndex == 0 ) + { + contact->encodedBodySimA = encodedBodySimIndex; + } + else + { + contact->encodedBodySimB = encodedBodySimIndex; + } + int movedLocalIndex = b3Array_RemoveSwap( disabledSet->contactIndices, localIndex ); if ( movedLocalIndex != B3_NULL_INDEX ) { @@ -227,6 +252,11 @@ void b3TrySleepIsland( b3World* world, int islandId ) b3Body* movedBody = b3Array_Get( world->bodies, movedId ); B3_ASSERT( movedBody->localIndex == movedIndex ); movedBody->localIndex = awakeBodyIndex; + + if ( movedBody->islandId != islandId ) + { + b3RefreshBodyContactIndices( world, movedBody ); + } } // destroy state, no need to clone @@ -245,14 +275,9 @@ void b3TrySleepIsland( b3World* world, int islandId ) b3Contact* contact = b3Array_Get( world->contacts, contactId ); - B3_ASSERT( contact->setIndex == b3_awakeSet || contact->setIndex == b3_disabledSet ); - contactKey = contact->edges[edgeIndex].nextKey; + B3_ASSERT( contact->setIndex == b3_awakeSet ); - if ( contact->setIndex == b3_disabledSet ) - { - // already moved to disabled set by another body in the island - continue; - } + contactKey = contact->edges[edgeIndex].nextKey; if ( contact->colorIndex != B3_NULL_INDEX ) { @@ -261,6 +286,15 @@ void b3TrySleepIsland( b3World* world, int islandId ) continue; } + if ( edgeIndex == 0 ) + { + contact->encodedBodySimA = B3_NULL_INDEX; + } + else + { + contact->encodedBodySimB = B3_NULL_INDEX; + } + // the other body may still be awake, it still may go to sleep and then it will be responsible // for moving this contact to the disabled set. int otherEdgeIndex = edgeIndex ^ 1; @@ -352,6 +386,8 @@ void b3TrySleepIsland( b3World* world, int islandId ) contact->setIndex = sleepSetId; contact->colorIndex = B3_NULL_INDEX; contact->localIndex = sleepContactIndex; + contact->encodedBodySimA = b3SleepBodySimIndex( contact->encodedBodySimA ); + contact->encodedBodySimB = b3SleepBodySimIndex( contact->encodedBodySimB ); } } @@ -530,6 +566,8 @@ void b3MergeSolverSets( b3World* world, int setId1, int setId2 ) void b3TransferBody( b3World* world, b3SolverSet* targetSet, b3SolverSet* sourceSet, b3Body* body ) { + B3_ASSERT( body->headContactKey == B3_NULL_INDEX ); + if ( targetSet == sourceSet ) { return; @@ -543,7 +581,8 @@ void b3TransferBody( b3World* world, b3SolverSet* targetSet, b3SolverSet* source memcpy( targetSim, sourceSim, sizeof( b3BodySim ) ); // Clear transient body flags - targetSim->flags &= ~( b3_isFast | b3_isSpeedCapped | b3_hadTimeOfImpact ); + body->flags &= ~b3_bodyTransientFlags; + targetSim->flags &= ~(b3_isFast | b3_bodyTransientFlags); // Remove body sim from solver set that owns it int movedIndex = b3Array_RemoveSwap( sourceSet->bodySims, sourceIndex ); @@ -555,6 +594,7 @@ void b3TransferBody( b3World* world, b3SolverSet* targetSet, b3SolverSet* source b3Body* movedBody = b3Array_Get( world->bodies, movedId ); B3_ASSERT( movedBody->localIndex == movedIndex ); movedBody->localIndex = sourceIndex; + b3RefreshBodyContactIndices( world, movedBody ); } if ( sourceSet->setIndex == b3_awakeSet ) diff --git a/src/world_snapshot.c b/src/world_snapshot.c index 3bd6cf0..b7c1a21 100644 --- a/src/world_snapshot.c +++ b/src/world_snapshot.c @@ -33,7 +33,7 @@ // Snapshot image magic 'BNS3' and version #define B3_SNAP_MAGIC 0x33534E42u -#define B3_SNAP_VERSION 5u // broad-phase pair traversal, move buffer removed +#define B3_SNAP_VERSION 7u // same-step restitution #define B3_SNAP_FLAG_VALIDATION 0x1u #define B3_SNAP_FLAG_DOUBLE_PRECISION 0x2u @@ -831,8 +831,6 @@ static void b3SerContacts( b3RecBuffer* buf, b3World* world ) // Write raw struct with pointer fields zeroed b3Contact copy = *c; copy.manifolds = NULL; - copy.bodySimIndexA = B3_NULL_INDEX; - copy.bodySimIndexB = B3_NULL_INDEX; if ( copy.flags & b3_simMeshContact ) { copy.meshContact.triangleCache.data = NULL; @@ -889,8 +887,6 @@ static void b3DesContacts( b3SnapReader* r, b3World* world ) b3Contact* dst = world->contacts.data + i; b3SnapR_Bytes( r, dst, sizeof( b3Contact ) ); dst->manifolds = NULL; - dst->bodySimIndexA = B3_NULL_INDEX; - dst->bodySimIndexB = B3_NULL_INDEX; if ( dst->flags & b3_simMeshContact ) { dst->meshContact.triangleCache.data = NULL; @@ -1062,6 +1058,8 @@ int b3SerializeWorld( b3World* world, b3RecBuffer* buf, b3Recording* rec ) // Shape sparse array with geometry interning b3SerShapes( buf, world, rec ); + b3SerPodArray( buf, world->fatAABBs ); + // Contact sparse array with manifold and mesh triangleCache b3SerContacts( buf, world ); @@ -1243,6 +1241,13 @@ bool b3DeserializeIntoShell( const uint8_t* data, int size, b3World* world, b3Re return false; } + b3DesPodArray( r, world->fatAABBs ); + + if ( !r->ok ) + { + return false; + } + // 6. Contact sparse array b3DesContacts( r, world ); diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index 598b1e2..6be43e1 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -25,6 +25,7 @@ set(BOX3D_TEST_FILES test_mover.c test_name_cache.c test_recording.c + test_restitution.c test_sat.c test_shape.c test_table.c diff --git a/test/main.c b/test/main.c index cc98311..9539d51 100644 --- a/test/main.c +++ b/test/main.c @@ -47,6 +47,7 @@ extern int MeshTest( void ); extern int MoverTest( void ); extern int NameCacheTest( void ); extern int RecordingTest( void ); +extern int RestitutionTest( void ); extern int SeparatingAxisTest( void ); extern int ShapeTest( void ); extern int TableTest( void ); @@ -123,6 +124,7 @@ int main( int argc, char** argv ) MAYBE_RUN_TEST( MoverTest ); MAYBE_RUN_TEST( NameCacheTest ); MAYBE_RUN_TEST( RecordingTest ); + MAYBE_RUN_TEST( RestitutionTest ); MAYBE_RUN_TEST( SeparatingAxisTest ); MAYBE_RUN_TEST( ShapeTest ); MAYBE_RUN_TEST( TableTest ); diff --git a/test/test_determinism.c b/test/test_determinism.c index 3ce4bcc..01fa0d5 100644 --- a/test/test_determinism.c +++ b/test/test_determinism.c @@ -18,27 +18,27 @@ // Double precision accumulates body positions in double, so the settle/sleep step and the // state hash differ from the float build. Both modes are internally deterministic. #if defined( BOX3D_DOUBLE_PRECISION ) -#define RAGDOLL_SLEEP_STEP 277 -#define RAGDOLL_HASH 0xF7B5B2D1 +#define RAGDOLL_SLEEP_STEP 272 +#define RAGDOLL_HASH 0xC049067A #define WAVE_PILE_SLEEP_STEP 323 #define WAVE_PILE_HASH 0x2200A547 #define QUERY_SPAWN_SLEEP_STEP 242 #define QUERY_SPAWN_HASH 0x1737F5BC #define QUERY_SPAWN_HIT_COUNT 59 #define QUERY_SPAWN_QUERY_HASH 0x5B4429DC -#define MESH_DROP_SLEEP_STEP 251 -#define MESH_DROP_HASH 0x465381C5 +#define MESH_DROP_SLEEP_STEP 217 +#define MESH_DROP_HASH 0xCF946437 #else -#define RAGDOLL_SLEEP_STEP 272 -#define RAGDOLL_HASH 0xF4992B41 +#define RAGDOLL_SLEEP_STEP 271 +#define RAGDOLL_HASH 0x805719D3 #define WAVE_PILE_SLEEP_STEP 316 #define WAVE_PILE_HASH 0xAA7D15A7 #define QUERY_SPAWN_SLEEP_STEP 242 #define QUERY_SPAWN_HASH 0xB9F993A5 #define QUERY_SPAWN_HIT_COUNT 59 #define QUERY_SPAWN_QUERY_HASH 0xE3271F3D -#define MESH_DROP_SLEEP_STEP 251 -#define MESH_DROP_HASH 0xE58C7240 +#define MESH_DROP_SLEEP_STEP 217 +#define MESH_DROP_HASH 0x256BC40B #endif // The goldens above pin exact values for the default four point manifold. A build that diff --git a/test/test_name_cache.c b/test/test_name_cache.c index a48a6ea..4066017 100644 --- a/test/test_name_cache.c +++ b/test/test_name_cache.c @@ -12,7 +12,7 @@ // Direct unit test of the cache: dedup, reverse lookup, empty/null, arbitrary length, no leak. static int CacheUnit( void ) { - int32_t base = b3GetByteCount(); + int64_t base = b3GetByteCount(); b3NameCache cache = b3CreateNameCache(); @@ -155,7 +155,7 @@ static int NameRoundTrip( void ) // the tracked byte count to return to baseline so a re-load that drops duplicate copies leaks nothing. static int RollbackNames( void ) { - int32_t base = b3GetByteCount(); + int64_t base = b3GetByteCount(); b3Recording* rec = b3CreateRecording( 0 ); ENSURE( rec != NULL ); diff --git a/test/test_restitution.c b/test/test_restitution.c new file mode 100644 index 0000000..18aea9e --- /dev/null +++ b/test/test_restitution.c @@ -0,0 +1,1135 @@ +// SPDX-FileCopyrightText: 2026 Erin Catto +// SPDX-License-Identifier: MIT + +#include "body.h" +#include "physics_world.h" +#include "recording.h" +#include "test_macros.h" + +#include "box3d/box3d.h" +#include "box3d/collision.h" +#include "box3d/math_functions.h" + +#include +#include +#include + +#define TIME_STEP ( 1.0f / 60.0f ) +#define SUB_STEP_COUNT 4 + +// Impact speed shared by every scenario that measures a coefficient. Well above the default +// restitution threshold so the bounce is always armed. +#define IMPACT_SPEED 5.0f + +// Each subtest prints all of its measurements before asserting, and the battery runs every subtest +// so one bad scenario does not hide the rest. +#define RUN_MEASUREMENT( T ) \ + do \ + { \ + if ( T() != 0 ) \ + { \ + printf( " subtest failed: " #T "\n" ); \ + failureCount += 1; \ + } \ + else \ + { \ + printf( " subtest passed: " #T "\n" ); \ + } \ + } \ + while ( false ) + +static b3WorldId MakeWorld( float gravityY ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.gravity = (b3Vec3){ 0.0f, gravityY, 0.0f }; + worldDef.enableSleep = false; + return b3CreateWorld( &worldDef ); +} + +// Ground with its top surface at y = 0 +static void MakeGround( b3WorldId worldId, float restitution ) +{ + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.position = (b3Pos){ 0.0f, -1.0f, 0.0f }; + b3BodyId groundId = b3CreateBody( worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.0f; + shapeDef.baseMaterial.restitution = restitution; + b3BoxHull box = b3MakeBoxHull( 40.0f, 1.0f, 40.0f ); + b3CreateHullShape( groundId, &shapeDef, &box.base ); +} + +static b3BodyId MakeBall( b3WorldId worldId, float x, float y, float velocityY, float restitution ) +{ + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = (b3Pos){ x, y, 0.0f }; + bodyDef.linearVelocity = (b3Vec3){ 0.0f, velocityY, 0.0f }; + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.0f; + shapeDef.baseMaterial.restitution = restitution; + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.5f }; + b3CreateSphereShape( bodyId, &shapeDef, &sphere ); + return bodyId; +} + +// Two free spheres closing head on with no gravity. The coefficient is the ratio of relative normal +// speeds at the contact, which is the mass independent definition. +static float MeasureHeadOn( float restitution, float densityB, float* momentumError ) +{ + b3WorldId worldId = MakeWorld( 0.0f ); + + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.5f }; + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.baseMaterial.friction = 0.0f; + shapeDef.baseMaterial.restitution = restitution; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = (b3Pos){ -1.0f, 0.0f, 0.0f }; + bodyDef.linearVelocity = (b3Vec3){ 0.5f * IMPACT_SPEED, 0.0f, 0.0f }; + b3BodyId idA = b3CreateBody( worldId, &bodyDef ); + b3CreateSphereShape( idA, &shapeDef, &sphere ); + + bodyDef.position = (b3Pos){ 1.0f, 0.0f, 0.0f }; + bodyDef.linearVelocity = (b3Vec3){ -0.5f * IMPACT_SPEED, 0.0f, 0.0f }; + b3BodyId idB = b3CreateBody( worldId, &bodyDef ); + shapeDef.density = densityB; + b3CreateSphereShape( idB, &shapeDef, &sphere ); + + float massA = b3Body_GetMass( idA ); + float massB = b3Body_GetMass( idB ); + float momentum0 = massA * b3Body_GetLinearVelocity( idA ).x + massB * b3Body_GetLinearVelocity( idB ).x; + + for ( int i = 0; i < 120; ++i ) + { + b3World_Step( worldId, TIME_STEP, SUB_STEP_COUNT ); + } + + float vA = b3Body_GetLinearVelocity( idA ).x; + float vB = b3Body_GetLinearVelocity( idB ).x; + float momentum1 = massA * vA + massB * vB; + + b3DestroyWorld( worldId ); + + *momentumError = ( momentum1 - momentum0 ) / ( massA + massB ); + return ( vB - vA ) / IMPACT_SPEED; +} + +// Ball driven into static ground with no gravity so the measurement carries no gravity bias. The +// gap shifts where inside the time step the impact lands. +static float MeasureGroundBounce( float restitution, float gap ) +{ + b3WorldId worldId = MakeWorld( 0.0f ); + MakeGround( worldId, 0.0f ); + + b3BodyId ballId = MakeBall( worldId, 0.0f, 0.5f + gap, -IMPACT_SPEED, restitution ); + + for ( int i = 0; i < 60; ++i ) + { + b3World_Step( worldId, TIME_STEP, SUB_STEP_COUNT ); + } + + float speed = b3Body_GetLinearVelocity( ballId ).y; + b3DestroyWorld( worldId ); + return speed / IMPACT_SPEED; +} + +// Impactor driven onto a column of resting balls pinned against the ground. The support balls are +// dead so only the top contact can bounce. A supported target has infinite effective mass along the +// normal, so the coefficient must not depend on the column height. +static float MeasureSupportedBounce( float restitution, int supportCount ) +{ + b3WorldId worldId = MakeWorld( 0.0f ); + MakeGround( worldId, 0.0f ); + + for ( int i = 0; i < supportCount; ++i ) + { + MakeBall( worldId, 0.0f, 0.5f + 1.0f * i, 0.0f, 0.0f ); + } + + float y = 0.5f + 1.0f * supportCount + 0.5f * IMPACT_SPEED * TIME_STEP; + b3BodyId ballId = MakeBall( worldId, 0.0f, y, -IMPACT_SPEED, restitution ); + + for ( int i = 0; i < 60; ++i ) + { + b3World_Step( worldId, TIME_STEP, SUB_STEP_COUNT ); + } + + float speed = b3Body_GetLinearVelocity( ballId ).y; + b3DestroyWorld( worldId ); + return speed / IMPACT_SPEED; +} + +// Flat box landing on all four corners at once with no gravity +static float MeasureFlatBounce( float restitution, int subStepCount, float* spin ) +{ + b3WorldId worldId = MakeWorld( 0.0f ); + MakeGround( worldId, 0.0f ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = (b3Pos){ 0.0f, 0.25f + 0.5f * IMPACT_SPEED * TIME_STEP, 0.0f }; + bodyDef.linearVelocity = (b3Vec3){ 0.0f, -IMPACT_SPEED, 0.0f }; + b3BodyId boxId = b3CreateBody( worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.baseMaterial.friction = 0.0f; + shapeDef.baseMaterial.restitution = restitution; + b3BoxHull box = b3MakeBoxHull( 1.0f, 0.25f, 1.0f ); + b3CreateHullShape( boxId, &shapeDef, &box.base ); + + for ( int i = 0; i < 60; ++i ) + { + b3World_Step( worldId, TIME_STEP, subStepCount ); + } + + float speed = b3Body_GetLinearVelocity( boxId ).y; + *spin = b3Length( b3Body_GetAngularVelocity( boxId ) ); + b3DestroyWorld( worldId ); + return speed / IMPACT_SPEED; +} + +// Perfectly elastic ball dropped under gravity +static int MeasureDrop( float dropHeight, float* apexes, int capacity ) +{ + b3WorldId worldId = MakeWorld( -10.0f ); + MakeGround( worldId, 0.0f ); + + b3BodyId ballId = MakeBall( worldId, 0.0f, dropHeight, 0.0f, 1.0f ); + + int apexCount = 0; + float previousSpeed = 0.0f; + + for ( int i = 0; i < 4000 && apexCount < capacity; ++i ) + { + b3World_Step( worldId, TIME_STEP, SUB_STEP_COUNT ); + + float speed = b3Body_GetLinearVelocity( ballId ).y; + if ( previousSpeed > 0.0f && speed <= 0.0f ) + { + apexes[apexCount] = (float)b3Body_GetPosition( ballId ).y; + apexCount += 1; + } + previousSpeed = speed; + } + + b3DestroyWorld( worldId ); + return apexCount; +} + +static int HeadOnTest( void ) +{ + static const float restitutions[] = { 0.0f, 0.25f, 0.5f, 0.75f, 1.0f }; + static const float densities[] = { 1.0f, 10.0f, 100.0f }; + const float tolerance = 0.02f; + + int failed = 0; + + for ( int i = 0; i < ARRAY_COUNT( densities ); ++i ) + { + float worstError = 0.0f; + float worstMomentum = 0.0f; + + for ( int j = 0; j < ARRAY_COUNT( restitutions ); ++j ) + { + float momentumError = 0.0f; + float measured = MeasureHeadOn( restitutions[j], densities[i], &momentumError ); + + float error = b3AbsFloat( measured - restitutions[j] ); + if ( error > worstError ) + { + worstError = error; + } + + if ( b3AbsFloat( momentumError ) > b3AbsFloat( worstMomentum ) ) + { + worstMomentum = momentumError; + } + + if ( error > tolerance ) + { + printf( " head on density %.0f e %.2f -> %.4f\n", densities[i], restitutions[j], measured ); + failed = 1; + } + } + + printf( " head on density %6.1f worst error %.4f momentum drift %.2e\n", densities[i], worstError, worstMomentum ); + + if ( b3AbsFloat( worstMomentum ) > 1.0e-4f ) + { + failed = 1; + } + } + + return failed; +} + +static int PhaseTest( void ) +{ + static const float restitutions[] = { 0.25f, 0.5f, 0.9f }; + const int sampleCount = 16; + const float tolerance = 0.05f; + + int failed = 0; + + for ( int j = 0; j < ARRAY_COUNT( restitutions ); ++j ) + { + float minimum = FLT_MAX; + float maximum = -FLT_MAX; + float sum = 0.0f; + + for ( int i = 0; i < sampleCount; ++i ) + { + // Sweep the gap across one step of travel so the impact lands at every phase + float gap = ( (float)i / (float)sampleCount ) * IMPACT_SPEED * TIME_STEP; + float measured = MeasureGroundBounce( restitutions[j], gap ); + minimum = b3MinFloat( minimum, measured ); + maximum = b3MaxFloat( maximum, measured ); + sum += measured; + } + + float mean = sum / sampleCount; + float spread = maximum - minimum; + printf( " phase e %.2f -> mean %.4f spread %.4f [%.4f, %.4f]\n", restitutions[j], mean, spread, minimum, maximum ); + + if ( spread > tolerance || b3AbsFloat( mean - restitutions[j] ) > tolerance ) + { + failed = 1; + } + } + + return failed; +} + +// The bounce is solved as a constraint alongside the support contacts, so the column can supply the +// reaction. A terminal restitution pass has nothing after it to do that and the coefficient decays +// with the column height, which the tolerance is chosen to reject. +static int SupportedTest( void ) +{ + static const float restitutions[] = { 0.5f, 0.9f }; + const float tolerance = 0.2f; + + int failed = 0; + + for ( int j = 0; j < ARRAY_COUNT( restitutions ); ++j ) + { + for ( int n = 0; n <= 3; ++n ) + { + float measured = MeasureSupportedBounce( restitutions[j], n ); + printf( " supported e %.2f supports %d -> %.4f\n", restitutions[j], n, measured ); + + if ( b3AbsFloat( measured - restitutions[j] ) > tolerance ) + { + failed = 1; + } + + // Rebounding faster than the impact is energy from nowhere, whatever the coefficient + if ( measured > 1.01f ) + { + failed = 1; + } + } + } + + return failed; +} + +// A symmetric four point landing. The points are solved in sequence within a relax pass and the +// bounce retires once they all separate, so a small residual spin is expected. The tolerance admits +// that residual and rejects the gross asymmetry of one point taking the whole bounce. +static int FlatLandingTest( void ) +{ + static const float restitutions[] = { 0.5f, 0.9f }; + static const int subStepCounts[] = { 4, 8 }; + const float speedTolerance = 0.1f; + const float spinTolerance = 0.25f; + + int failed = 0; + + for ( int j = 0; j < ARRAY_COUNT( restitutions ); ++j ) + { + for ( int k = 0; k < ARRAY_COUNT( subStepCounts ); ++k ) + { + float spin = 0.0f; + float measured = MeasureFlatBounce( restitutions[j], subStepCounts[k], &spin ); + printf( " flat e %.2f substeps %d -> %.4f spin %.4f\n", restitutions[j], subStepCounts[k], measured, spin ); + + // Only the shipping sub step count is a gate. The wider count is reported so a + // convergence problem can be told apart from a formulation problem. + if ( subStepCounts[k] != SUB_STEP_COUNT ) + { + continue; + } + + if ( b3AbsFloat( measured - restitutions[j] ) > speedTolerance || spin > spinTolerance ) + { + failed = 1; + } + } + } + + return failed; +} + +// A flat box carrying spin about a horizontal axis. All four corners stay in contact and the normal +// constraints are linear in the body velocity, so perfect restitution reverses linear and angular +// velocity. Sweeping the normal solve once per manifold point makes it exact to float precision. One +// sweep left up to 0.03 of residual in each. +static int SpinTest( void ) +{ + static const float spins[] = { 0.0f, 0.5f, 1.0f, 2.0f }; + + int failed = 0; + + for ( int j = 0; j < ARRAY_COUNT( spins ); ++j ) + { + b3WorldId worldId = MakeWorld( 0.0f ); + MakeGround( worldId, 0.0f ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = (b3Pos){ 0.0f, 0.25f + 0.5f * IMPACT_SPEED * TIME_STEP, 0.0f }; + bodyDef.linearVelocity = (b3Vec3){ 0.0f, -IMPACT_SPEED, 0.0f }; + bodyDef.angularVelocity = (b3Vec3){ 0.0f, 0.0f, spins[j] }; + b3BodyId boxId = b3CreateBody( worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.baseMaterial.friction = 0.0f; + shapeDef.baseMaterial.restitution = 1.0f; + b3BoxHull box = b3MakeBoxHull( 1.0f, 0.25f, 1.0f ); + b3CreateHullShape( boxId, &shapeDef, &box.base ); + + for ( int i = 0; i < 60; ++i ) + { + b3World_Step( worldId, TIME_STEP, SUB_STEP_COUNT ); + } + + float speed = b3Body_GetLinearVelocity( boxId ).y; + float spin = b3Body_GetAngularVelocity( boxId ).z; + b3DestroyWorld( worldId ); + + printf( " spin in %+.2f -> vy %+.4f (want %+.4f) wz %+.4f (want %+.4f)\n", spins[j], speed, IMPACT_SPEED, spin, + -spins[j] ); + + if ( b3AbsFloat( speed - IMPACT_SPEED ) > 0.25f || b3AbsFloat( spin + spins[j] ) > 0.25f ) + { + failed = 1; + } + } + + return failed; +} + +// Perfectly elastic ball under gravity. Only heights where continuous collision engages are used. +// Continuous collision lands the ball on the surface, so the bounce is armed from the true impact +// speed and the apex holds. Slower drops resolve the impact inside the overlap and the penetration +// recovery adds height, by design. +static int DropTest( void ) +{ + static const float heights[] = { 40.0f, 20.0f }; + + int failed = 0; + + for ( int j = 0; j < ARRAY_COUNT( heights ); ++j ) + { + float apexes[6] = { 0 }; + int apexCount = MeasureDrop( heights[j], apexes, ARRAY_COUNT( apexes ) ); + + printf( " drop %5.1f ->", heights[j] ); + for ( int i = 0; i < apexCount; ++i ) + { + printf( " %8.3f", apexes[i] ); + } + printf( "\n" ); + + if ( apexCount < ARRAY_COUNT( apexes ) ) + { + failed = 1; + continue; + } + + float highest = apexes[0]; + for ( int i = 1; i < apexCount; ++i ) + { + highest = b3MaxFloat( highest, apexes[i] ); + } + + if ( highest > 1.02f * apexes[0] || apexes[apexCount - 1] < 0.8f * apexes[0] ) + { + failed = 1; + } + } + + return failed; +} + +// A cube dropped flat with an aggressive continuous safety factor so it lands square on the surface. +// Perfectly elastic and frictionless, so it should come back to the drop height with no rotation and +// keep doing that. It now does: the four apexes sit within a centimetre of each other and the spin +// after the landing is zero to float precision. +// +// This used to be the worst subtest in the file and its history is worth keeping. The four points of +// a flat landing are solved in sequence, so the first impulse tilts the cube, the next point sees a +// larger closing speed and delivers more, and the sum overshot the rigid body answer. One sweep gave +// 0.89 rad/s of spin and a 6 percent high first apex, after which the cube tumbled and either shed +// almost all its energy (the deferred scheme collapsed to 0.71, the height of a cube rolling on its +// edges) or gained it. The normal solve is now swept once per manifold point when a bounce is armed, +// which converges the coupling: spin fell to 0.13 rad/s at two sweeps and to zero at three. The +// bounds below reject every one of those earlier behaviors. +static int SingleBoxTest( void ) +{ + b3WorldId worldId = MakeWorld( -10.0f ); + MakeGround( worldId, 0.0f ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.baseMaterial.friction = 0.0f; + shapeDef.baseMaterial.restitution = 1.0f; + b3BoxHull box = b3MakeCubeHull( 0.5f ); + + const float dropHeight = 10.0f; + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = (b3Pos){ 0.0f, dropHeight, 0.0f }; + bodyDef.safetyFactor = 0.01f; + b3BodyId boxId = b3CreateBody( worldId, &bodyDef ); + b3CreateHullShape( boxId, &shapeDef, &box.base ); + + float firstSpin = 0.0f; + float apexes[4] = { 0 }; + float spins[4] = { 0 }; + int apexCount = 0; + float previousSpeed = 0.0f; + + // A nearly elastic bounce from 10 m takes about 165 steps, so budget for four of them + for ( int i = 0; i < 1200 && apexCount < ARRAY_COUNT( apexes ); ++i ) + { + b3World_Step( worldId, TIME_STEP, SUB_STEP_COUNT ); + + float speed = b3Body_GetLinearVelocity( boxId ).y; + + if ( apexCount == 0 && previousSpeed <= 0.0f && speed > 0.0f ) + { + firstSpin = b3Length( b3Body_GetAngularVelocity( boxId ) ); + } + + if ( previousSpeed > 0.0f && speed <= 0.0f ) + { + apexes[apexCount] = (float)b3Body_GetPosition( boxId ).y; + spins[apexCount] = b3Length( b3Body_GetAngularVelocity( boxId ) ); + apexCount += 1; + } + previousSpeed = speed; + } + + b3DestroyWorld( worldId ); + + printf( " single box first bounce spin %.4f apexes", firstSpin ); + for ( int i = 0; i < apexCount; ++i ) + { + printf( " %7.3f (spin %5.2f)", apexes[i], spins[i] ); + } + printf( "\n" ); + + ENSURE( apexCount == ARRAY_COUNT( apexes ) ); + + int failed = 0; + + if ( firstSpin > 0.1f ) + { + failed = 1; + } + + for ( int i = 0; i < apexCount; ++i ) + { + if ( apexes[i] < 0.95f * dropHeight || apexes[i] > 1.02f * dropHeight ) + { + failed = 1; + } + } + + return failed; +} + +static int ThresholdTest( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + float threshold = worldDef.restitutionThreshold; + + static const float scales[] = { 0.5f, 0.9f, 1.5f, 4.0f }; + + int failed = 0; + + for ( int j = 0; j < ARRAY_COUNT( scales ); ++j ) + { + float speed = scales[j] * threshold; + + b3WorldId worldId = MakeWorld( 0.0f ); + MakeGround( worldId, 0.0f ); + b3BodyId ballId = MakeBall( worldId, 0.0f, 0.5f + 0.5f * speed * TIME_STEP, -speed, 1.0f ); + + for ( int i = 0; i < 60; ++i ) + { + b3World_Step( worldId, TIME_STEP, SUB_STEP_COUNT ); + } + + float ratio = b3Body_GetLinearVelocity( ballId ).y / speed; + b3DestroyWorld( worldId ); + + printf( " threshold %.2fx -> %.4f\n", scales[j], ratio ); + + if ( scales[j] < 1.0f && ratio > 0.05f ) + { + failed = 1; + } + + if ( scales[j] > 1.0f && ratio < 0.9f ) + { + failed = 1; + } + } + + return failed; +} + +// High restitution must not wake a settled stack back up +static int RestingTest( void ) +{ + b3WorldId worldId = MakeWorld( -10.0f ); + MakeGround( worldId, 0.9f ); + + const int boxCount = 10; + b3BodyId boxIds[10]; + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.density = 1.0f; + shapeDef.baseMaterial.friction = 0.6f; + shapeDef.baseMaterial.restitution = 0.9f; + b3BoxHull box = b3MakeCubeHull( 0.5f ); + + for ( int i = 0; i < boxCount; ++i ) + { + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = (b3Pos){ 0.0f, 0.5f + 1.0f * i, 0.0f }; + boxIds[i] = b3CreateBody( worldId, &bodyDef ); + b3CreateHullShape( boxIds[i], &shapeDef, &box.base ); + } + + for ( int i = 0; i < 240; ++i ) + { + b3World_Step( worldId, TIME_STEP, SUB_STEP_COUNT ); + } + + float settled = (float)b3Body_GetPosition( boxIds[boxCount - 1] ).y; + float drift = 0.0f; + float peakSpeed = 0.0f; + + for ( int i = 0; i < 300; ++i ) + { + b3World_Step( worldId, TIME_STEP, SUB_STEP_COUNT ); + + float y = (float)b3Body_GetPosition( boxIds[boxCount - 1] ).y; + drift = b3MaxFloat( drift, b3AbsFloat( y - settled ) ); + + for ( int j = 0; j < boxCount; ++j ) + { + peakSpeed = b3MaxFloat( peakSpeed, b3Length( b3Body_GetLinearVelocity( boxIds[j] ) ) ); + } + } + + b3DestroyWorld( worldId ); + + printf( " resting drift %.5f peak speed %.5f\n", drift, peakSpeed ); + + ENSURE( drift < 0.01f ); + ENSURE( peakSpeed < 0.05f ); + return 0; +} + +static uint64_t RunWorkerScene( int workerCount ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.gravity = (b3Vec3){ 0.0f, -10.0f, 0.0f }; + worldDef.enableSleep = false; + worldDef.workerCount = workerCount; + b3WorldId worldId = b3CreateWorld( &worldDef ); + + MakeGround( worldId, 0.6f ); + + for ( int i = 0; i < 20; ++i ) + { + MakeBall( worldId, -10.0f + 1.05f * i, 3.0f + 0.13f * i, 0.0f, 0.6f ); + } + + for ( int i = 0; i < 200; ++i ) + { + b3World_Step( worldId, TIME_STEP, SUB_STEP_COUNT ); + } + + uint64_t hash = b3HashWorldState( b3GetWorldFromId( worldId ) ); + b3DestroyWorld( worldId ); + return hash; +} + +// The armed bounce is per manifold point state, so it must survive the split across workers +// The manifold point approach speed is published only for contacts that opted into hit events. It +// is not a stale value otherwise, it is zero, so a reader can tell "not measured" from "measured +// zero" by whether the shape enables the events. Restitution no longer reads it at all, so this is +// the only thing keeping the field alive. +static float MeasureFirstTouchNormalVelocity( bool enableHitEvents ) +{ + b3WorldId worldId = MakeWorld( 0.0f ); + MakeGround( worldId, 0.0f ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = (b3Pos){ 0.0f, 0.6f, 0.0f }; + b3BodyId ballId = b3CreateBody( worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + shapeDef.baseMaterial.friction = 0.0f; + shapeDef.baseMaterial.restitution = 0.0f; + shapeDef.enableHitEvents = enableHitEvents; + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.5f }; + b3CreateSphereShape( ballId, &shapeDef, &sphere ); + + float normalVelocity = FLT_MAX; + + // Hold the closing speed so the sampled value does not depend on which step the pair is created + for ( int i = 0; i < 20; ++i ) + { + b3Body_SetLinearVelocity( ballId, (b3Vec3){ 0.0f, -IMPACT_SPEED, 0.0f } ); + b3World_Step( worldId, TIME_STEP, SUB_STEP_COUNT ); + + b3ContactData contactData; + if ( b3Body_GetContactData( ballId, &contactData, 1 ) == 1 && contactData.manifoldCount > 0 && + contactData.manifolds[0].pointCount > 0 ) + { + normalVelocity = contactData.manifolds[0].points[0].normalVelocity; + break; + } + } + + b3DestroyWorld( worldId ); + return normalVelocity; +} + +static int NormalVelocityTest( void ) +{ + float quiet = MeasureFirstTouchNormalVelocity( false ); + float published = MeasureFirstTouchNormalVelocity( true ); + + printf( " normal velocity hit events off %.4f on %.4f (closing at %.4f)\n", quiet, published, -IMPACT_SPEED ); + + int failed = 0; + + if ( quiet != 0.0f ) + { + failed = 1; + } + + if ( published > -0.8f * IMPACT_SPEED ) + { + failed = 1; + } + + return failed; +} + +// Mirrors the Restitution Overshoot sample in samples/sample_issues.cpp: a unit cube dropped flat on +// a floor narrower than itself, perfectly elastic, so the four contact points sit inboard of the box +// corners. A perfectly elastic bounce cannot come back higher than it was dropped from. +// +// Run twice, because two different effects used to be tangled together here. The impulse itself must +// never return more speed than it received, which is the restitution invariant and is checked in both +// cases. The apex is a weaker statement: without continuous collision the cube moves 0.23 m in the +// step before contact, just under the 0.25 m that would trigger it, so the first manifold appears +// with the cube already 0.10 m deep. It then bounces from down there and the soft position correction +// lifts it back to the surface for free, which is worth about a tenth of a metre of extra height and +// has nothing to do with restitution. With continuous collision the cube lands on the surface and the +// apex comes in under the drop height. +// Total mechanical energy of a body. Perfect restitution and no friction means this may only +// decrease, which is a stronger statement than any height bound: it also catches a bounce that +// manufactures spin rather than height. +static float MeasureEnergy( b3BodyId bodyId, b3WorldId worldId ) +{ + b3MassData massData = b3Body_GetMassData( bodyId ); + b3Vec3 v = b3Body_GetLinearVelocity( bodyId ); + + // The inertia tensor is in the body frame, so bring the angular velocity back to it + b3Vec3 wLocal = b3InvRotateVector( b3Body_GetRotation( bodyId ), b3Body_GetAngularVelocity( bodyId ) ); + + float kinetic = 0.5f * massData.mass * b3Dot( v, v ) + 0.5f * b3Dot( wLocal, b3MulMV( massData.inertia, wLocal ) ); + float potential = -massData.mass * b3Dot( b3World_GetGravity( worldId ), b3ToVec3( b3Body_GetWorldCenter( bodyId ) ) ); + + return kinetic + potential; +} + +typedef struct OvershootResult +{ + float firstApex; + float firstEnergyRatio; + float peakEnergyRatio; + float speedRatio; + int bounceCount; +} OvershootResult; + +// Mirrors the Restitution Overshoot sample in samples/sample_issues.cpp: a unit cube dropped flat on +// a floor narrower than itself, perfectly elastic, so the four contact points sit inboard of the box +// corners. A perfectly elastic bounce cannot come back higher than it was dropped from and cannot +// gain energy. +static OvershootResult MeasureOvershoot( bool continuous ) +{ + b3WorldId worldId = MakeWorld( -10.0f ); + + b3BodyDef floorDef = b3DefaultBodyDef(); + floorDef.position = (b3Pos){ 0.0f, -0.25f, 0.0f }; + b3BodyId floorId = b3CreateBody( worldId, &floorDef ); + + b3ShapeDef floorShape = b3DefaultShapeDef(); + b3BoxHull floor = b3MakeBoxHull( 0.375f, 0.25f, 0.375f ); + b3CreateHullShape( floorId, &floorShape, &floor.base ); + + const float dropHeight = 10.0f; + + b3BodyDef boxDef = b3DefaultBodyDef(); + boxDef.type = b3_dynamicBody; + boxDef.position = (b3Pos){ 0.0f, dropHeight, 0.0f }; + if ( continuous ) + { + boxDef.safetyFactor = 0.1f; + } + b3BodyId boxId = b3CreateBody( worldId, &boxDef ); + + b3ShapeDef boxShape = b3DefaultShapeDef(); + boxShape.baseMaterial.restitution = 1.0f; + b3BoxHull box = b3MakeCubeHull( 0.5f ); + b3CreateHullShape( boxId, &boxShape, &box.base ); + + OvershootResult result = { 0 }; + + float startEnergy = MeasureEnergy( boxId, worldId ); + float impactSpeed = 0.0f; + float reboundSpeed = 0.0f; + float bounceHeight = 0.0f; + float previousSpeed = 0.0f; + + for ( int i = 0; i < 600 && result.bounceCount < 2; ++i ) + { + float before = b3Body_GetLinearVelocity( boxId ).y; + float heightBefore = (float)b3Body_GetPosition( boxId ).y; + b3World_Step( worldId, TIME_STEP, SUB_STEP_COUNT ); + float speed = b3Body_GetLinearVelocity( boxId ).y; + + if ( reboundSpeed == 0.0f && before < 0.0f && speed > 0.0f ) + { + impactSpeed = -before; + reboundSpeed = speed; + bounceHeight = heightBefore; + } + + if ( reboundSpeed == 0.0f ) + { + continue; + } + + // Every step, not just at the apexes, because that is what the sample checks and the impact + // step itself is where any gain appears + float ratio = MeasureEnergy( boxId, worldId ) / startEnergy; + if ( ratio > result.peakEnergyRatio ) + { + result.peakEnergyRatio = ratio; + } + + if ( previousSpeed > 0.0f && speed <= 0.0f ) + { + if ( result.bounceCount == 0 ) + { + result.firstApex = (float)b3Body_GetPosition( boxId ).y; + result.firstEnergyRatio = ratio; + } + + result.bounceCount += 1; + } + + previousSpeed = speed; + } + + b3DestroyWorld( worldId ); + + result.speedRatio = reboundSpeed / impactSpeed; + + printf( " overshoot continuous %d apex %.4f of %.4f at y %.4f, speed ratio %.4f, energy %.4f then %.4f\n", + continuous ? 1 : 0, result.firstApex, dropHeight, bounceHeight, result.speedRatio, result.firstEnergyRatio, + result.peakEnergyRatio ); + + return result; +} + +// Two runs, because two unrelated effects were tangled together here. +// +// The impulse must never return more speed than it received. That is the restitution invariant and it +// is checked in both runs; the unconverged four point solve failed it by 7.9 percent. +// +// Everything else depends on whether continuous collision engages. Without it the cube moves 0.23 m +// in the step before contact, just under the 0.25 m trigger, so the first manifold appears with the +// cube already 0.10 m deep. It bounces from down there and climbs back through that depth for free, +// which is worth about a tenth of a metre of apex and one percent of energy, and none of it is +// restitution. With continuous collision the cube lands on the surface and both the apex and the +// energy come in under where they started. The safety factor here matches the one the sample sets, +// which is the supported answer for a body whose elastic accuracy matters. +// +// The discrete run is only gated on its first bounce. Later bounces there are a genuine blow-up, not +// a tolerance question: the free height makes the cube tilt, the next landing is a deep corner impact +// at 14 m/s, and energy reaches 159 percent with the cube spinning at 26 rad/s. Lowering the safety +// factor is the fix and the sample does that; the discrete run is kept to pin what happens when it is +// left alone, and because its speed ratio is what caught the real bug. Details in +// .claude/same_step_restitution.md. +static int OvershootTest( void ) +{ + const float dropHeight = 10.0f; + + OvershootResult discrete = MeasureOvershoot( false ); + OvershootResult continuous = MeasureOvershoot( true ); + + int failed = 0; + + if ( discrete.speedRatio > 1.0f || continuous.speedRatio > 1.0f ) + { + failed = 1; + } + + if ( continuous.firstApex > dropHeight || continuous.peakEnergyRatio > 1.001f ) + { + failed = 1; + } + + if ( discrete.firstApex > dropHeight + 0.15f || discrete.firstEnergyRatio > 1.02f ) + { + failed = 1; + } + + if ( discrete.bounceCount < 2 || continuous.bounceCount < 2 ) + { + failed = 1; + } + + return failed; +} + +typedef struct ImpulseResult +{ + float worstError; + float approachSpeed; + float firstSeparation; + float bounceImpulse; + int contactSteps; + int toiSteps; + int toiImpulseSteps; +} ImpulseResult; + +// Ball dropped under gravity with the contact impulse read back every step. Nothing else touches the +// ball, so once gravity is taken out the change in momentum over a step is the impulse the contact +// applied, and that is what the total normal impulse must report. A time of impact step is left out +// of the balance: no contact was solved on it, and the sweep hands back the gravity of the time it +// cut short. +static ImpulseResult MeasureDropImpulse( float restitution, float dropHeight ) +{ + b3WorldId worldId = MakeWorld( -10.0f ); + MakeGround( worldId, 0.0f ); + + b3BodyId ballId = MakeBall( worldId, 0.0f, 0.5f + dropHeight, 0.0f, restitution ); + b3World* world = b3GetWorldFromId( worldId ); + + float mass = b3Body_GetMass( ballId ); + float gravityY = b3World_GetGravity( worldId ).y; + + ImpulseResult result = { 0 }; + bool touched = false; + bool bouncing = false; + + // Enough for the longest fall and the bounce that follows it + for ( int i = 0; i < 240; ++i ) + { + float speedBefore = b3Body_GetLinearVelocity( ballId ).y; + b3World_Step( worldId, TIME_STEP, SUB_STEP_COUNT ); + float speedAfter = b3Body_GetLinearVelocity( ballId ).y; + + float measured = 0.0f; + float separation = 0.0f; + b3ContactData contactData[4]; + int contactCount = b3Body_GetContactData( ballId, contactData, ARRAY_COUNT( contactData ) ); + for ( int c = 0; c < contactCount; ++c ) + { + for ( int m = 0; m < contactData[c].manifoldCount; ++m ) + { + const b3Manifold* manifold = contactData[c].manifolds + m; + for ( int p = 0; p < manifold->pointCount; ++p ) + { + measured += manifold->points[p].totalNormalImpulse; + separation = manifold->points[p].separation; + } + } + } + + // The sim flag is the one written this step, the body flag lags a step behind + b3Body* ball = b3GetBodyFullId( world, ballId ); + b3BodySim* ballSim = b3GetBodySim( world, ball ); + if ( ballSim->flags & b3_hadTimeOfImpact ) + { + result.toiSteps += 1; + if ( measured != 0.0f ) + { + result.toiImpulseSteps += 1; + } + continue; + } + + float expected = mass * ( speedAfter - speedBefore ) - mass * gravityY * TIME_STEP; + result.worstError = b3MaxFloat( result.worstError, b3AbsFloat( measured - expected ) ); + + if ( contactCount > 0 ) + { + if ( touched == false ) + { + touched = true; + bouncing = true; + result.approachSpeed = -speedBefore; + result.firstSeparation = separation; + } + + if ( bouncing ) + { + result.bounceImpulse += measured; + result.contactSteps += 1; + } + } + else + { + bouncing = false; + } + } + + b3DestroyWorld( worldId ); + return result; +} + +// Heights on both sides of the continuous collision threshold, so the impulse is checked for a ball +// that lands inside the overlap and for one the sweep sets down on the surface. The threshold is +// derived from the body so the split survives a change to the default safety factor. +// +// The step balance is the real gate. The bounce total is bracketed as well so the restitution sweep +// means something: the contact has to reverse the approach at the coefficient and may carry the +// weight for at most as long as it lasted. The bounce retires inside the step once the point +// separates, so the ball can lose up to a step of gravity below the reversal. +static int ImpulseTest( void ) +{ + static const float restitutions[] = { 0.0f, 0.25f, 0.5f, 0.75f, 1.0f }; + static const float heights[] = { 1.0f, 5.0f, 20.0f, 45.0f }; + + float mass; + float fastSpeed; + { + b3WorldId worldId = MakeWorld( -10.0f ); + b3BodyId ballId = MakeBall( worldId, 0.0f, 0.5f, 0.0f, 0.0f ); + mass = b3Body_GetMass( ballId ); + fastSpeed = b3Body_GetSafetyFactor( ballId ) * b3Body_GetMinExtent( ballId ) / TIME_STEP; + b3DestroyWorld( worldId ); + } + + const float weightImpulse = mass * 10.0f * TIME_STEP; + + int failed = 0; + + for ( int j = 0; j < ARRAY_COUNT( heights ); ++j ) + { + float impactSpeed = sqrtf( 20.0f * heights[j] ); + bool expectToi = impactSpeed > fastSpeed; + + // Float noise in the solver velocities scales with the impact speed, and the ball is heavy + float tolerance = 1e-5f * mass * ( 10.0f + impactSpeed ); + + for ( int k = 0; k < ARRAY_COUNT( restitutions ); ++k ) + { + ImpulseResult result = MeasureDropImpulse( restitutions[k], heights[j] ); + + float reversal = ( 1.0f + restitutions[k] ) * mass * result.approachSpeed; + float lower = reversal - weightImpulse; + float upper = reversal + weightImpulse * result.contactSteps; + + printf( " impulse drop %4.1f e %.2f toi %d at %+.4f -> worst step error %.1e, bounce %.1f in [%.1f, %.1f] over %d " + "steps\n", + heights[j], restitutions[k], result.toiSteps, result.firstSeparation, result.worstError, result.bounceImpulse, + lower, upper, result.contactSteps ); + + if ( result.worstError > tolerance ) + { + failed = 1; + } + + if ( result.contactSteps == 0 || result.toiImpulseSteps > 0 ) + { + failed = 1; + } + + float slack = result.contactSteps * tolerance; + if ( result.bounceImpulse < lower - slack || result.bounceImpulse > upper + slack ) + { + failed = 1; + } + + if ( ( result.toiSteps > 0 ) != expectToi ) + { + printf( " continuous collision %s at %.1f m/s (threshold %.1f m/s)\n", expectToi ? "expected" : "unexpected", + impactSpeed, fastSpeed ); + failed = 1; + } + } + } + + return failed; +} + +static int WorkerParityTest( void ) +{ + uint64_t hash1 = RunWorkerScene( 1 ); + uint64_t hash2 = RunWorkerScene( 2 ); + uint64_t hash4 = RunWorkerScene( 4 ); + + printf( " worker hashes 0x%016llx 0x%016llx 0x%016llx\n", (unsigned long long)hash1, (unsigned long long)hash2, + (unsigned long long)hash4 ); + + ENSURE( hash1 == hash2 ); + ENSURE( hash1 == hash4 ); + return 0; +} + +int RestitutionTest( void ) +{ + int failureCount = 0; + + RUN_MEASUREMENT( HeadOnTest ); + RUN_MEASUREMENT( PhaseTest ); + RUN_MEASUREMENT( SupportedTest ); + RUN_MEASUREMENT( FlatLandingTest ); + RUN_MEASUREMENT( SpinTest ); + RUN_MEASUREMENT( DropTest ); + RUN_MEASUREMENT( SingleBoxTest ); + RUN_MEASUREMENT( ThresholdTest ); + RUN_MEASUREMENT( RestingTest ); + RUN_MEASUREMENT( NormalVelocityTest ); + RUN_MEASUREMENT( OvershootTest ); + RUN_MEASUREMENT( ImpulseTest ); + RUN_MEASUREMENT( WorkerParityTest ); + + return failureCount > 0 ? 1 : 0; +} diff --git a/test/test_world.c b/test/test_world.c index 33b1bed..7b31785 100644 --- a/test/test_world.c +++ b/test/test_world.c @@ -2,9 +2,11 @@ // SPDX-License-Identifier: MIT #include "benchmarks.h" +#include "body.h" #include "dynamic_tree.h" #include "overflow_color.h" #include "physics_world.h" +#include "recording.h" #include "test_macros.h" #include "box3d/box3d.h" @@ -1276,6 +1278,348 @@ static int TestContinuousMoveEvent( void ) return 0; } +// The contact body sim locators are persistent state. Every contact in every set carries the +// current encoding for both of its bodies: the awake local index, a static tag, or null for a +// body that fell asleep. The scenes below drive each site that can invalidate one. The gate is +// b3ValidateSolverSets, which asserts the invariant on every mutation and every step, so a +// dropped refresh trips there rather than producing a plausible but wrong simulation. Each scene +// also runs at worker counts 1 and 4 with the state hashes compared. + +static b3WorldId CreateLocatorWorld( int workerCount ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + worldDef.workerCount = workerCount; + return b3CreateWorld( &worldDef ); +} + +static b3BodyId CreateLocatorBody( b3WorldId worldId, b3BodyType type, b3Pos position, float hx, float hy, float hz ) +{ + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = type; + bodyDef.position = position; + b3BodyId bodyId = b3CreateBody( worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3BoxHull box = b3MakeBoxHull( hx, hy, hz ); + b3CreateHullShape( bodyId, &shapeDef, &box.base ); + + return bodyId; +} + +static void StepLocatorWorld( b3WorldId worldId, int stepCount ) +{ + for ( int i = 0; i < stepCount; ++i ) + { + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + } +} + +// Steps until the body sleeps or the budget runs out. Returns the steps taken. +static int StepUntilAsleep( b3WorldId worldId, b3BodyId bodyId, int maxSteps ) +{ + for ( int i = 0; i < maxSteps; ++i ) + { + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + if ( b3Body_IsAwake( bodyId ) == false ) + { + return i + 1; + } + } + + return maxSteps; +} + +// Sleep and wake: first touch puts contacts in the graph, sleeping nulls the locators of the +// island bodies, waking restores their awake indices. The plate beside the stack holds a +// non-touching contact across the whole cycle, and that is the contact the wake path has to fix up +// itself. A touching contact would get a fresh encoding from the graph add and hide a dropped +// refresh. +static uint64_t LocatorSleepWakeScene( int workerCount, int* failed ) +{ + b3WorldId worldId = CreateLocatorWorld( workerCount ); + CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ 0.0f, -1.0f, 0.0f }, 5.0f, 1.0f, 5.0f ); + + b3BodyId stackId[3]; + for ( int i = 0; i < 3; ++i ) + { + stackId[i] = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 0.0f, 0.5f + i, 0.0f }, 0.5f, 0.5f, 0.5f ); + } + + // Beside the top box, close enough that the fat bounds overlap and far enough that the + // shapes never touch. Kinematic and awake, so the contact stays in the awake set while the + // stack sleeps. Off to the side so it does not catch the body dropped later. + b3BodyId plateId = CreateLocatorBody( worldId, b3_kinematicBody, (b3Pos){ 1.04f, 2.5f, 0.0f }, 0.5f, 0.5f, 0.5f ); + b3Body_EnableSleep( plateId, false ); + + int steps = StepUntilAsleep( worldId, stackId[0], 300 ); + if ( steps == 300 || b3Body_IsAwake( stackId[2] ) == true ) + { + *failed = 1; + } + + // Wake without contact so the non-touching contact keeps its stale locator if the wake + // path forgets to refresh + b3Body_SetAwake( stackId[0], true ); + StepLocatorWorld( worldId, 5 ); + + if ( b3Body_IsAwake( stackId[2] ) == false ) + { + *failed = 1; + } + + steps = StepUntilAsleep( worldId, stackId[0], 300 ); + if ( steps == 300 ) + { + *failed = 1; + } + + // Dropping a body on the pile drives the wake path through first touch. The pile can settle + // and sleep again, so catch the wake as it happens. + CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 0.0f, 8.0f, 0.0f }, 0.5f, 0.5f, 0.5f ); + + bool woke = false; + for ( int i = 0; i < 180; ++i ) + { + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + woke = woke || b3Body_IsAwake( stackId[0] ); + } + + if ( woke == false ) + { + *failed = 1; + } + + uint64_t hash = b3HashWorldState( b3GetWorldFromId( worldId ) ); + b3DestroyWorld( worldId ); + return hash; +} + +// An island falling asleep and a body leaving the awake set both swap the last awake sim into +// the hole. The pile that never sleeps supplies a moved body that still owns contacts. +static uint64_t LocatorAwakeNeighborScene( int workerCount, int* failed ) +{ + b3WorldId worldId = CreateLocatorWorld( workerCount ); + CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ 0.0f, -1.0f, 0.0f }, 30.0f, 1.0f, 5.0f ); + + // This island sleeps + b3BodyId sleeperId[2]; + for ( int i = 0; i < 2; ++i ) + { + sleeperId[i] = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ -10.0f, 0.5f + i, 0.0f }, 0.5f, 0.5f, 0.5f ); + } + + // Created after the sleepers so its sims sit at the end of the awake set and get swapped + // into the holes the sleeping island leaves behind + b3BodyId spareId = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 20.0f, 0.5f, 0.0f }, 0.5f, 0.5f, 0.5f ); + b3Body_EnableSleep( spareId, false ); + + b3BodyId awakeId[3]; + for ( int i = 0; i < 3; ++i ) + { + awakeId[i] = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 10.0f, 0.5f + i, 0.0f }, 0.5f, 0.5f, 0.5f ); + b3Body_EnableSleep( awakeId[i], false ); + } + + int steps = StepUntilAsleep( worldId, sleeperId[0], 300 ); + if ( steps == 300 || b3Body_IsAwake( awakeId[0] ) == false ) + { + *failed = 1; + } + + StepLocatorWorld( worldId, 10 ); + + // Leaving the awake set swaps a body that owns contacts into the spare's slot + b3Body_Disable( spareId ); + StepLocatorWorld( worldId, 10 ); + + b3Body_Enable( spareId ); + StepLocatorWorld( worldId, 10 ); + + uint64_t hash = b3HashWorldState( b3GetWorldFromId( worldId ) ); + b3DestroyWorld( worldId ); + return hash; +} + +// Destroying a body swaps the last sim of its set into the hole. The moved body keeps its +// contacts, so its side of each one has to follow it. +static uint64_t LocatorDestroyScene( int workerCount, int* failed ) +{ + b3WorldId worldId = CreateLocatorWorld( workerCount ); + CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ 0.0f, -1.0f, 0.0f }, 5.0f, 1.0f, 5.0f ); + + b3BodyId stackId[4]; + for ( int i = 0; i < 4; ++i ) + { + stackId[i] = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 0.0f, 0.5f + i, 0.0f }, 0.5f, 0.5f, 0.5f ); + } + + StepLocatorWorld( worldId, 30 ); + + // The last awake sim moves into this hole while its contacts live + b3DestroyBody( stackId[0] ); + StepLocatorWorld( worldId, 30 ); + + // Three static boxes, one resting box each. Destroying the first moves the last static sim. + b3BodyId staticId[3]; + for ( int i = 0; i < 3; ++i ) + { + float x = 10.0f + 4.0f * i; + staticId[i] = CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ x, -1.0f, 0.0f }, 1.0f, 1.0f, 1.0f ); + CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ x, 0.5f, 0.0f }, 0.5f, 0.5f, 0.5f ); + } + + StepLocatorWorld( worldId, 30 ); + + b3DestroyBody( staticId[0] ); + StepLocatorWorld( worldId, 30 ); + + B3_UNUSED( failed ); + + uint64_t hash = b3HashWorldState( b3GetWorldFromId( worldId ) ); + b3DestroyWorld( worldId ); + return hash; +} + +// A kinematic body parked a hair above a pile keeps a non-touching contact to a body that falls +// asleep, which is the one case the locator cannot name and has to leave to the cold path. +static uint64_t LocatorKinematicScene( int workerCount, int* failed ) +{ + b3WorldId worldId = CreateLocatorWorld( workerCount ); + CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ 0.0f, -1.0f, 0.0f }, 5.0f, 1.0f, 5.0f ); + + b3BodyId boxId = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 0.0f, 0.5f, 0.0f }, 0.5f, 0.5f, 0.5f ); + + // Close enough that the fat bounds overlap and a non-touching contact exists + b3BodyId plateId = CreateLocatorBody( worldId, b3_kinematicBody, (b3Pos){ 0.0f, 1.52f, 0.0f }, 1.0f, 0.5f, 1.0f ); + + int steps = StepUntilAsleep( worldId, boxId, 300 ); + if ( steps == 300 ) + { + *failed = 1; + } + + // Drive the plate down to touch and wake the sleeper + b3Body_SetLinearVelocity( plateId, (b3Vec3){ 0.0f, -1.0f, 0.0f } ); + StepLocatorWorld( worldId, 60 ); + + if ( b3Body_IsAwake( boxId ) == false ) + { + *failed = 1; + } + + uint64_t hash = b3HashWorldState( b3GetWorldFromId( worldId ) ); + b3DestroyWorld( worldId ); + return hash; +} + +// Set type and disable move a body between solver sets, which changes the encoding of every +// contact it has. A joint across two sleeping sets drives the merge path. +static uint64_t LocatorTransferScene( int workerCount, int* failed ) +{ + b3WorldId worldId = CreateLocatorWorld( workerCount ); + CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ 0.0f, -1.0f, 0.0f }, 20.0f, 1.0f, 5.0f ); + + b3BodyId boxId = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 0.0f, 0.5f, 0.0f }, 0.5f, 0.5f, 0.5f ); + StepLocatorWorld( worldId, 30 ); + + b3Body_SetType( boxId, b3_staticBody ); + StepLocatorWorld( worldId, 5 ); + b3Body_SetType( boxId, b3_dynamicBody ); + StepLocatorWorld( worldId, 5 ); + + b3Body_Disable( boxId ); + StepLocatorWorld( worldId, 5 ); + b3Body_Enable( boxId ); + StepLocatorWorld( worldId, 30 ); + + // Two piles far enough apart to sleep in separate sets + b3BodyId sleeperId[2]; + sleeperId[0] = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ -12.0f, 0.5f, 0.0f }, 0.5f, 0.5f, 0.5f ); + sleeperId[1] = CreateLocatorBody( worldId, b3_dynamicBody, (b3Pos){ 12.0f, 0.5f, 0.0f }, 0.5f, 0.5f, 0.5f ); + + int steps = StepUntilAsleep( worldId, sleeperId[0], 300 ); + if ( steps == 300 || b3Body_IsAwake( sleeperId[1] ) == true ) + { + *failed = 1; + } + + b3SphericalJointDef jointDef = b3DefaultSphericalJointDef(); + jointDef.base.bodyIdA = sleeperId[0]; + jointDef.base.bodyIdB = sleeperId[1]; + b3CreateSphericalJoint( worldId, &jointDef ); + + StepLocatorWorld( worldId, 30 ); + + uint64_t hash = b3HashWorldState( b3GetWorldFromId( worldId ) ); + b3DestroyWorld( worldId ); + return hash; +} + +// The fast flag lives on the body sim and gates contact recycling, but a flag sync rebuilds the +// sim flags from the body row, which never carries it. A public setter called on a body in +// flight must not drop it, or the recycle path can take a fast pair. +static int LocatorFastFlagTest( void ) +{ + b3WorldDef worldDef = b3DefaultWorldDef(); + b3WorldId worldId = b3CreateWorld( &worldDef ); + + CreateLocatorBody( worldId, b3_staticBody, (b3Pos){ 0.0f, -1.0f, 0.0f }, 5.0f, 1.0f, 5.0f ); + + b3BodyDef bodyDef = b3DefaultBodyDef(); + bodyDef.type = b3_dynamicBody; + bodyDef.position = (b3Pos){ -20.0f, 2.0f, 0.0f }; + bodyDef.linearVelocity = (b3Vec3){ 400.0f, 0.0f, 0.0f }; + b3BodyId bulletId = b3CreateBody( worldId, &bodyDef ); + + b3ShapeDef shapeDef = b3DefaultShapeDef(); + b3Sphere sphere = { { 0.0f, 0.0f, 0.0f }, 0.1f }; + b3CreateSphereShape( bulletId, &shapeDef, &sphere ); + + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + + b3World* world = b3GetWorldFromId( worldId ); + b3Body* body = b3GetBodyFullId( world, bulletId ); + b3BodySim* sim = b3GetBodySim( world, body ); + ENSURE( ( sim->flags & b3_isFast ) != 0 ); + + // Any public setter that syncs flags. The locks have to actually change or the setter + // returns early and never syncs. + b3MotionLocks locks = { 0 }; + locks.angularX = true; + b3Body_SetMotionLocks( bulletId, locks ); + + sim = b3GetBodySim( world, b3GetBodyFullId( world, bulletId ) ); + ENSURE( ( sim->flags & b3_isFast ) != 0 ); + + b3World_Step( worldId, 1.0f / 60.0f, 4 ); + + b3DestroyWorld( worldId ); + return 0; +} + +static int BodySimLocatorTest( void ) +{ + typedef uint64_t ( *LocatorScene )( int workerCount, int* failed ); + LocatorScene scenes[5] = { + LocatorSleepWakeScene, + LocatorAwakeNeighborScene, + LocatorDestroyScene, + LocatorKinematicScene, + LocatorTransferScene, + }; + + for ( int i = 0; i < 5; ++i ) + { + int failed = 0; + uint64_t hash1 = scenes[i]( 1, &failed ); + uint64_t hash4 = scenes[i]( 4, &failed ); + ENSURE( failed == 0 ); + ENSURE( hash1 == hash4 ); + } + + return LocatorFastFlagTest(); +} + int WorldTest( void ) { RUN_SUBTEST( HelloWorld ); @@ -1300,6 +1644,7 @@ int WorldTest( void ) RUN_SUBTEST( TestHullDatabase ); RUN_SUBTEST( TestEnlargedProxyDestroyed ); RUN_SUBTEST( TestCompoundShapeCount ); + RUN_SUBTEST( BodySimLocatorTest ); return 0; }