diff --git a/Cargo.lock b/Cargo.lock index 2e66800d8..0d1c89f53 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -5128,6 +5128,7 @@ dependencies = [ "num-traits", "num_cpus", "ordered-float", + "parry3d", "postcard", "rand", "rapier3d", @@ -5141,6 +5142,7 @@ dependencies = [ "tinyvec", "tobj", "tritet", + "wide", ] [[package]] @@ -6686,9 +6688,9 @@ dependencies = [ [[package]] name = "wide" -version = "0.7.31" +version = "0.7.32" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "cc0ca27312d1e9218687a4e804cd4b7410bf54125d54d13e5170efbf09066d24" +checksum = "41b5576b9a81633f3e8df296ce0063042a73507636cbe956c61133dd7034ab22" dependencies = [ "bytemuck", "safe_arch", diff --git a/Cargo.toml b/Cargo.toml index 9cf1b3925..e78b29cb8 100644 --- a/Cargo.toml +++ b/Cargo.toml @@ -50,6 +50,8 @@ itertools = "0.14.0" serde_cbor = "0.11.2" postcard = { version = "1.1.1", features = ["use-std"] } bevy_framepace = "0.18" +wide = "0.7.32" +parry3d = "0.18.0" [[bin]] name = "example" diff --git a/prefab/Anaconda.stl b/prefab/Anaconda.stl deleted file mode 100644 index ee5343261..000000000 Binary files a/prefab/Anaconda.stl and /dev/null differ diff --git a/prefab/anaconda-bones.json b/prefab/anaconda-bones.json new file mode 100644 index 000000000..db47e5692 --- /dev/null +++ b/prefab/anaconda-bones.json @@ -0,0 +1,302 @@ +{ + "bone": { + "head": { + "x": 0.6070398688316345, + "y": 3.488184690475464, + "z": 0.19564415514469147 + }, + "tail": { + "x": 0.6236315965652466, + "y": 3.4085373878479004, + "z": 0.1993076056241989 + } + }, + "bone_009": { + "head": { + "x": 0.6236315965652466, + "y": 3.4085373878479004, + "z": 0.1993076056241989 + }, + "tail": { + "x": 0.6183035969734192, + "y": 3.316044569015503, + "z": 0.20030179619789124 + } + }, + "bone_007": { + "head": { + "x": 0.6183035969734192, + "y": 3.316044569015503, + "z": 0.20030179619789124 + }, + "tail": { + "x": 0.5486531853675842, + "y": 3.2271180152893066, + "z": 0.19193406403064728 + } + }, + "bone_008": { + "head": { + "x": 0.5486531853675842, + "y": 3.2271180152893066, + "z": 0.19193406403064728 + }, + "tail": { + "x": 0.4748899042606354, + "y": 3.1820590496063232, + "z": 0.19288358092308044 + } + }, + "bone_006": { + "head": { + "x": 0.4748899042606354, + "y": 3.1820590496063232, + "z": 0.19288358092308044 + }, + "tail": { + "x": 0.32671600580215454, + "y": 3.11191725730896, + "z": 0.19421303272247314 + } + }, + "bone_010": { + "head": { + "x": 0.32671600580215454, + "y": 3.11191725730896, + "z": 0.19421303272247314 + }, + "tail": { + "x": 0.19413162767887115, + "y": 2.997762441635132, + "z": 0.1879468560218811 + } + }, + "bone_004": { + "head": { + "x": 0.19413162767887115, + "y": 2.997762441635132, + "z": 0.1879468560218811 + }, + "tail": { + "x": 0.08068132400512695, + "y": 2.8404202461242676, + "z": 0.18485479056835175 + } + }, + "bone_011": { + "head": { + "x": 0.08068132400512695, + "y": 2.8404202461242676, + "z": 0.18485479056835175 + }, + "tail": { + "x": -0.014009974896907806, + "y": 2.670964241027832, + "z": 0.17771753668785095 + } + }, + "bone_005": { + "head": { + "x": -0.014009974896907806, + "y": 2.670964241027832, + "z": 0.17771753668785095 + }, + "tail": { + "x": -0.07677791267633438, + "y": 2.5024752616882324, + "z": 0.17266269028186798 + } + }, + "bone_012": { + "head": { + "x": -0.07677791267633438, + "y": 2.5024752616882324, + "z": 0.17266269028186798 + }, + "tail": { + "x": -0.11340910941362381, + "y": 2.3294928073883057, + "z": 0.17319327592849731 + } + }, + "bone_002": { + "head": { + "x": -0.11340910941362381, + "y": 2.3294928073883057, + "z": 0.17319327592849731 + }, + "tail": { + "x": -0.08609960973262787, + "y": 2.0730838775634766, + "z": 0.17111866176128387 + } + }, + "bone_015": { + "head": { + "x": -0.08609960973262787, + "y": 2.0730838775634766, + "z": 0.17111866176128387 + }, + "tail": { + "x": -0.05879011005163193, + "y": 1.816674828529358, + "z": 0.16904404759407043 + } + }, + "bone_014": { + "head": { + "x": -0.05879011005163193, + "y": 1.816674828529358, + "z": 0.16904404759407043 + }, + "tail": { + "x": -0.031480610370635986, + "y": 1.5602657794952393, + "z": 0.166969433426857 + } + }, + "bone_016": { + "head": { + "x": -0.031480610370635986, + "y": 1.5602657794952393, + "z": 0.166969433426857 + }, + "tail": { + "x": -0.004171114414930344, + "y": 1.3038568496704102, + "z": 0.16489481925964355 + } + }, + "bone_013": { + "head": { + "x": -0.004171114414930344, + "y": 1.3038568496704102, + "z": 0.16489481925964355 + }, + "tail": { + "x": 0.05561229586601257, + "y": 1.041410207748413, + "z": 0.16239383816719055 + } + }, + "bone_017": { + "head": { + "x": 0.05561229586601257, + "y": 1.041410207748413, + "z": 0.16239383816719055 + }, + "tail": { + "x": 0.15776894986629486, + "y": 0.6679806709289551, + "z": 0.14728130400180817 + } + }, + "bone_021": { + "head": { + "x": 0.15776894986629486, + "y": 0.6679806709289551, + "z": 0.14728130400180817 + }, + "tail": { + "x": 0.1563047468662262, + "y": 0.28363165259361267, + "z": 0.1468256264925003 + } + }, + "bone_001": { + "head": { + "x": 0.1563047468662262, + "y": 0.28363165259361267, + "z": 0.1468256264925003 + }, + "tail": { + "x": 0.14030331373214722, + "y": -0.11586377024650574, + "z": 0.16327451169490814 + } + }, + "bone_019": { + "head": { + "x": 0.14030331373214722, + "y": -0.11586377024650574, + "z": 0.16327451169490814 + }, + "tail": { + "x": 0.08491484820842743, + "y": -0.5309418439865112, + "z": 0.15609216690063477 + } + }, + "bone_018": { + "head": { + "x": 0.08491484820842743, + "y": -0.5309418439865112, + "z": 0.15609216690063477 + }, + "tail": { + "x": 0.02284509874880314, + "y": -0.9243378639221191, + "z": 0.14005453884601593 + } + }, + "bone_020": { + "head": { + "x": 0.02284509874880314, + "y": -0.9243378639221191, + "z": 0.14005453884601593 + }, + "tail": { + "x": -0.03272989019751549, + "y": -1.3230215311050415, + "z": 0.1516072154045105 + } + }, + "bone_003": { + "head": { + "x": -0.03272989019751549, + "y": -1.3230215311050415, + "z": 0.1516072154045105 + }, + "tail": { + "x": -0.0303995069116354, + "y": -1.721837043762207, + "z": 0.1625695526599884 + } + }, + "bone_023": { + "head": { + "x": -0.0303995069116354, + "y": -1.721837043762207, + "z": 0.1625695526599884 + }, + "tail": { + "x": -0.034437455236911774, + "y": -2.1189937591552734, + "z": 0.17180347442626953 + } + }, + "bone_022": { + "head": { + "x": -0.034437455236911774, + "y": -2.1189937591552734, + "z": 0.17180347442626953 + }, + "tail": { + "x": -0.05511781573295593, + "y": -2.520186185836792, + "z": 0.22017113864421844 + } + }, + "bone_024": { + "head": { + "x": -0.05511781573295593, + "y": -2.520186185836792, + "z": 0.22017113864421844 + }, + "tail": { + "x": -0.024445362389087677, + "y": -2.902860403060913, + "z": 0.22326646745204926 + } + } +} diff --git a/prefab/anaconda.stl b/prefab/anaconda.stl new file mode 100644 index 000000000..e269ce77b Binary files /dev/null and b/prefab/anaconda.stl differ diff --git a/prefab/export_highlighted_armature.py b/prefab/export_highlighted_armature.py index 9d99831a0..2ecaa5ccc 100644 --- a/prefab/export_highlighted_armature.py +++ b/prefab/export_highlighted_armature.py @@ -2,14 +2,15 @@ import bpy import json from bpy import context -o = bpy.context.object -out = {} vec3 = lambda vec: {"x": vec.x, "y": vec.y, "z": vec.z} -for b in o.pose.bones: - head = o.matrix_world @ b.matrix @ b.head - tail = o.matrix_world @ b.matrix @ b.tail - name = b.name.replace('.', '_').replace('-', '_').lower() - out[name] = {"head": vec3(head), "tail": vec3(tail)} +bone_head = lambda ctx, bone: vec3(ctx.matrix_world.translation + bone.head_local) +bone_tail = lambda ctx, bone: vec3(ctx.matrix_world.translation + bone.tail_local) +bone = lambda ctx, bone: {"head": bone_head(ctx, bone), "tail": bone_tail(ctx, bone)} +bone_name = lambda name: name.replace('.', '_').replace('-', '_').lower() +arm_context = bpy.context.object +arm_data = arm_context.data + +out = {bone_name(b.name) : bone(arm_context, b) for b in arm_data.bones} print('\n\n\n\n') print(json.dumps(out)) diff --git a/prefab/female_base-bones.json b/prefab/human-female-bones.json similarity index 100% rename from prefab/female_base-bones.json rename to prefab/human-female-bones.json diff --git a/prefab/female_base_500f.stl b/prefab/human-female.stl similarity index 100% rename from prefab/female_base_500f.stl rename to prefab/human-female.stl diff --git a/prefab/human-male-bones.json b/prefab/human-male-bones.json new file mode 100644 index 000000000..bce44d548 --- /dev/null +++ b/prefab/human-male-bones.json @@ -0,0 +1 @@ +{"m_hips": {"head": {"x": 0.003162541426718235, "y": 0.10195900499820709, "z": 0.5904829502105713}, "tail": {"x": 0.010821257717907429, "y": 0.07877334952354431, "z": 0.08994382619857788}}, "m_waist": {"head": {"x": 0.0010016794549301267, "y": 0.10252499580383301, "z": 0.5886345505714417}, "tail": {"x": 0.001001716940663755, "y": 0.152367502450943, "z": 0.8066956996917725}}, "m_spine": {"head": {"x": 0.001001716940663755, "y": 0.152367502450943, "z": 0.8066956996917725}, "tail": {"x": 0.0010017681634053588, "y": 0.18974459171295166, "z": 1.097070336341858}}, "bone_004": {"head": {"x": 0.0010017681634053588, "y": 0.18974459171295166, "z": 1.097070336341858}, "tail": {"x": 0.0010017681634053588, "y": 0.18974459171295166, "z": 1.3747049570083618}}, "m_chest": {"head": {"x": 0.0010017681634053588, "y": 0.18974459171295166, "z": 1.3747049570083618}, "tail": {"x": 0.0010018404573202133, "y": 0.18572551012039185, "z": 1.7932240962982178}}, "bone_002": {"head": {"x": 0.0010018404573202133, "y": 0.18572551012039185, "z": 1.7932240962982178}, "tail": {"x": -0.47783803939819336, "y": 0.18558213114738464, "z": 1.6446514129638672}}, "bone_003": {"head": {"x": 0.0010018404573202133, "y": 0.18572551012039185, "z": 1.7932240962982178}, "tail": {"x": 0.5446823239326477, "y": 0.18519344925880432, "z": 1.6734424829483032}}, "bone_005": {"head": {"x": 0.0010018404573202133, "y": 0.18572551012039185, "z": 1.7932240962982178}, "tail": {"x": 0.0010019127512350678, "y": 0.18170642852783203, "z": 1.934108853340149}}, "bone_018": {"head": {"x": 0.0010019127512350678, "y": 0.18170642852783203, "z": 1.934108853340149}, "tail": {"x": 0.005812562070786953, "y": 0.1836266666650772, "z": 2.1737537384033203}}, "m_head": {"head": {"x": 0.005812562070786953, "y": 0.1836266666650772, "z": 2.1737537384033203}, "tail": {"x": 0.005812613759189844, "y": 0.10908273607492447, "z": 2.375145435333252}}, "m_eyebrows": {"head": {"x": 0.005812613759189844, "y": 0.10908273607492447, "z": 2.375145435333252}, "tail": {"x": 0.005812614690512419, "y": -0.19047966599464417, "z": 2.4332451820373535}}, "m_jaw": {"head": {"x": 0.005812613759189844, "y": 0.10908273607492447, "z": 2.375145435333252}, "tail": {"x": 0.005812568124383688, "y": -0.1758112907409668, "z": 2.0805587768554688}}, "r_upper_arm": {"head": {"x": -0.47783803939819336, "y": 0.18558213114738464, "z": 1.6446514129638672}, "tail": {"x": -1.3812698125839233, "y": 0.20588333904743195, "z": 1.4414451122283936}}, "r_lower_arm": {"head": {"x": -1.3812698125839233, "y": 0.20588333904743195, "z": 1.4414451122283936}, "tail": {"x": -2.076347589492798, "y": 0.044203609228134155, "z": 1.304756760597229}}, "r_hand": {"head": {"x": -2.076347589492798, "y": 0.044203609228134155, "z": 1.304756760597229}, "tail": {"x": -2.343585729598999, "y": 0.056169360876083374, "z": 1.2113057374954224}}, "l_upper_arm": {"head": {"x": 0.5446823239326477, "y": 0.18519344925880432, "z": 1.6734424829483032}, "tail": {"x": 1.3663803339004517, "y": 0.20563922822475433, "z": 1.483242392539978}}, "l_lower_arm": {"head": {"x": 1.3663803339004517, "y": 0.20563922822475433, "z": 1.483242392539978}, "tail": {"x": 2.0663564205169678, "y": 0.04467569291591644, "z": 1.3416621685028076}}, "l_hand": {"head": {"x": 2.0663564205169678, "y": 0.04467569291591644, "z": 1.3416621685028076}, "tail": {"x": 2.3266260623931885, "y": 0.05705520510673523, "z": 1.2540311813354492}}, "r_upper_leg": {"head": {"x": -0.25513601303100586, "y": 0.07877105474472046, "z": 0.1387087106704712}, "tail": {"x": -0.5912657380104065, "y": 0.06895933300256729, "z": -0.9701849222183228}}, "r_lower_leg": {"head": {"x": -0.5912657380104065, "y": 0.06895933300256729, "z": -0.9701849222183228}, "tail": {"x": -0.9735252261161804, "y": 0.16303546726703644, "z": -2.18613862991333}}, "r_ancle": {"head": {"x": -0.9735252261161804, "y": 0.16303546726703644, "z": -2.18613862991333}, "tail": {"x": -1.0920854806900024, "y": -0.16820663213729858, "z": -2.3055927753448486}}, "r_toe": {"head": {"x": -1.0920854806900024, "y": -0.16820663213729858, "z": -2.3055927753448486}, "tail": {"x": -1.1538002490997314, "y": -0.4375842809677124, "z": -2.3457844257354736}}, "l_upper_leg": {"head": {"x": 0.2947828471660614, "y": 0.07972406595945358, "z": 0.13740015029907227}, "tail": {"x": 0.6011503338813782, "y": 0.07002540677785873, "z": -0.9768697023391724}}, "l_lower_leg": {"head": {"x": 0.6011503338813782, "y": 0.07002540677785873, "z": -0.9768697023391724}, "tail": {"x": 0.9844488501548767, "y": 0.163835346698761, "z": -2.193763494491577}}, "l_ancle": {"head": {"x": 0.9844488501548767, "y": 0.163835346698761, "z": -2.193763494491577}, "tail": {"x": 1.1052886247634888, "y": -0.16843479871749878, "z": -2.3099963665008545}}, "l_toe": {"head": {"x": 1.1052886247634888, "y": -0.16843479871749878, "z": -2.3099963665008545}, "tail": {"x": 1.1677669286727905, "y": -0.4420241117477417, "z": -2.3523178100585938}}, "bone": {"head": {"x": 0.010821257717907429, "y": 0.07877334952354431, "z": 0.08994382619857788}, "tail": {"x": 0.2947828471660614, "y": 0.07972406595945358, "z": 0.13740015029907227}}, "bone_001": {"head": {"x": 0.010821257717907429, "y": 0.07877334952354431, "z": 0.08994382619857788}, "tail": {"x": -0.25513601303100586, "y": 0.07877105474472046, "z": 0.1387087106704712}}} diff --git a/prefab/human-male.stl b/prefab/human-male.stl new file mode 100644 index 000000000..33e37e643 Binary files /dev/null and b/prefab/human-male.stl differ diff --git a/prefab/male_base-bones.json b/prefab/male_base-bones.json deleted file mode 100644 index 285319b55..000000000 --- a/prefab/male_base-bones.json +++ /dev/null @@ -1 +0,0 @@ -{"m_hips": {"head": {"x": -0.0012977259466424584, "y": 0.09873443841934204, "z": 0.5799066424369812}, "tail": {"x": 0.010821257717907429, "y": 0.07877334952354431, "z": 0.08994382619857788}}, "m_waist": {"head": {"x": -0.0012977259466424584, "y": 0.09873443841934204, "z": 0.5799066424369812}, "tail": {"x": 0.001001716940663755, "y": 0.152367502450943, "z": 0.8066956996917725}}, "m_spine": {"head": {"x": 0.001001716940663755, "y": 0.152367502450943, "z": 0.8066956996917725}, "tail": {"x": 0.0010017681634053588, "y": 0.18974459171295166, "z": 1.097070336341858}}, "m_chest": {"head": {"x": 0.0010017681634053588, "y": 0.18974459171295166, "z": 1.097070336341858}, "tail": {"x": 0.0011565848253667355, "y": 0.18114709854125977, "z": 1.7951523065567017}}, "bone_018": {"head": {"x": 0.0010019127512350678, "y": 0.18170642852783203, "z": 1.934108853340149}, "tail": {"x": 0.005812574177980423, "y": 0.13230469822883606, "z": 2.073798179626465}}, "m_head": {"head": {"x": 0.005812574177980423, "y": 0.13230469822883606, "z": 2.073798179626465}, "tail": {"x": 0.005812613759189844, "y": 0.10908273607492447, "z": 2.375145435333252}}, "m_eyebrows": {"head": {"x": 0.005812613759189844, "y": 0.10908273607492447, "z": 2.375145435333252}, "tail": {"x": 0.005812614690512419, "y": -0.19047966599464417, "z": 2.4332451820373535}}, "m_jaw": {"head": {"x": 0.005812613759189844, "y": 0.10908273607492447, "z": 2.375145435333252}, "tail": {"x": 0.005812568124383688, "y": -0.1758112907409668, "z": 2.0805587768554688}}, "r_shoulder": {"head": {"x": 0.0011565848253667355, "y": 0.18114709854125977, "z": 1.7951523065567017}, "tail": {"x": -0.5703960061073303, "y": 0.18268556892871857, "z": 1.6796873807907104}}, "r_upper_arm": {"head": {"x": -0.5788930654525757, "y": 0.1853611022233963, "z": 1.6834924221038818}, "tail": {"x": -1.3812698125839233, "y": 0.20588333904743195, "z": 1.4414451122283936}}, "r_lower_arm": {"head": {"x": -1.3812698125839233, "y": 0.20588333904743195, "z": 1.4414451122283936}, "tail": {"x": -2.076347589492798, "y": 0.044203609228134155, "z": 1.304756760597229}}, "r_hand": {"head": {"x": -2.076347589492798, "y": 0.044203609228134155, "z": 1.304756760597229}, "tail": {"x": -2.343585729598999, "y": 0.056169360876083374, "z": 1.2113057374954224}}, "r_finger_index01": {"head": {"x": -2.335391044616699, "y": -0.07816237211227417, "z": 1.215346336364746}, "tail": {"x": -2.4461824893951416, "y": -0.10163837671279907, "z": 1.1580877304077148}}, "r_finger_index01_001": {"head": {"x": -2.4461824893951416, "y": -0.10163837671279907, "z": 1.1580877304077148}, "tail": {"x": -2.55203914642334, "y": -0.12349137663841248, "z": 1.0932888984680176}}, "r_thumb01": {"head": {"x": -2.076347589492798, "y": 0.04420362412929535, "z": 1.3047566413879395}, "tail": {"x": -2.170755624771118, "y": -0.08250358700752258, "z": 1.2216516733169556}}, "r_thumb02": {"head": {"x": -2.170755624771118, "y": -0.08250358700752258, "z": 1.2216516733169556}, "tail": {"x": -2.2050626277923584, "y": -0.1487712264060974, "z": 1.1610702276229858}}, "r_thumb03": {"head": {"x": -2.2050626277923584, "y": -0.1487712264060974, "z": 1.1610702276229858}, "tail": {"x": -2.231705904006958, "y": -0.20103195309638977, "z": 1.1033203601837158}}, "r_mf01": {"head": {"x": -2.349660634994507, "y": -0.009082093834877014, "z": 1.2319086790084839}, "tail": {"x": -2.4427998065948486, "y": -0.016127586364746094, "z": 1.1780006885528564}}, "r_mf02": {"head": {"x": -2.4427998065948486, "y": -0.016127586364746094, "z": 1.1780006885528564}, "tail": {"x": -2.566545009613037, "y": -0.024504736065864563, "z": 1.0831310749053955}}, "r_rf01": {"head": {"x": -2.3528802394866943, "y": 0.05754943937063217, "z": 1.2252631187438965}, "tail": {"x": -2.4380619525909424, "y": 0.05799344927072525, "z": 1.1739680767059326}}, "r_rf02": {"head": {"x": -2.4380619525909424, "y": 0.05799344927072525, "z": 1.1739680767059326}, "tail": {"x": -2.5430376529693604, "y": 0.06054571270942688, "z": 1.0920013189315796}}, "r_pf01": {"head": {"x": -2.3462910652160645, "y": 0.11885695159435272, "z": 1.2159702777862549}, "tail": {"x": -2.422309398651123, "y": 0.12374290823936462, "z": 1.1711504459381104}}, "r_pf02": {"head": {"x": -2.422309398651123, "y": 0.12374290823936462, "z": 1.1711504459381104}, "tail": {"x": -2.495955228805542, "y": 0.12862640619277954, "z": 1.1059892177581787}}, "bone_003": {"head": {"x": -0.5703960061073303, "y": 0.18268556892871857, "z": 1.6796873807907104}, "tail": {"x": -0.5788930654525757, "y": 0.1853611022233963, "z": 1.6834924221038818}}, "l_shoulder": {"head": {"x": 0.0011565848253667355, "y": 0.18114709854125977, "z": 1.7951523065567017}, "tail": {"x": 0.5373091101646423, "y": 0.1827419400215149, "z": 1.6803903579711914}}, "l_upper_arm": {"head": {"x": 0.5446823239326477, "y": 0.18519344925880432, "z": 1.6734424829483032}, "tail": {"x": 1.3663803339004517, "y": 0.20563922822475433, "z": 1.483242392539978}}, "l_lower_arm": {"head": {"x": 1.3663803339004517, "y": 0.20563922822475433, "z": 1.483242392539978}, "tail": {"x": 2.0663564205169678, "y": 0.04467569291591644, "z": 1.3416621685028076}}, "l_hand": {"head": {"x": 2.0663564205169678, "y": 0.04467569291591644, "z": 1.3416621685028076}, "tail": {"x": 2.3266260623931885, "y": 0.05705520510673523, "z": 1.2540311813354492}}, "l_finger_index01": {"head": {"x": 2.3180785179138184, "y": -0.07684838771820068, "z": 1.258310079574585}, "tail": {"x": 2.429262399673462, "y": -0.10054698586463928, "z": 1.2034382820129395}}, "l_finger_index02": {"head": {"x": 2.429262399673462, "y": -0.10054698586463928, "z": 1.2034382820129395}, "tail": {"x": 2.531839609146118, "y": -0.12198156118392944, "z": 1.136545181274414}}, "l_thumb01": {"head": {"x": 2.0663564205169678, "y": 0.04467570781707764, "z": 1.341662049293518}, "tail": {"x": 2.1592934131622314, "y": -0.08540076017379761, "z": 1.2809035778045654}}, "l_thumb02": {"head": {"x": 2.1592934131622314, "y": -0.08540076017379761, "z": 1.2809035778045654}, "tail": {"x": 2.1965291500091553, "y": -0.14615267515182495, "z": 1.2300605773925781}}, "l_thumb03": {"head": {"x": 2.1965291500091553, "y": -0.14615267515182495, "z": 1.2300605773925781}, "tail": {"x": 2.2283248901367188, "y": -0.20005851984024048, "z": 1.1541619300842285}}, "l_mf01": {"head": {"x": 2.330841302871704, "y": -0.007817864418029785, "z": 1.2741056680679321}, "tail": {"x": 2.422492265701294, "y": -0.014490395784378052, "z": 1.2213836908340454}}, "l_mf02": {"head": {"x": 2.422492265701294, "y": -0.014490395784378052, "z": 1.2213836908340454}, "tail": {"x": 2.5443692207336426, "y": -0.022395059466362, "z": 1.1281065940856934}}, "l_rf01": {"head": {"x": 2.334294080734253, "y": 0.05875726044178009, "z": 1.2673252820968628}, "tail": {"x": 2.4181511402130127, "y": 0.059533797204494476, "z": 1.2170979976654053}}, "l_rf02": {"head": {"x": 2.4181511402130127, "y": 0.059533797204494476, "z": 1.2170979976654053}, "tail": {"x": 2.5215868949890137, "y": 0.06247615069150925, "z": 1.1364574432373047}}, "l_pf01": {"head": {"x": 2.328108549118042, "y": 0.1199670359492302, "z": 1.2577956914901733}, "tail": {"x": 2.4029579162597656, "y": 0.12514632940292358, "z": 1.2139159440994263}}, "l_pf02": {"head": {"x": 2.4029579162597656, "y": 0.12514632940292358, "z": 1.2139159440994263}, "tail": {"x": 2.4755702018737793, "y": 0.13029323518276215, "z": 1.1496829986572266}}, "bone_006": {"head": {"x": 0.0011565848253667355, "y": 0.18114709854125977, "z": 1.7951523065567017}, "tail": {"x": 0.0010019127512350678, "y": 0.18170642852783203, "z": 1.934108853340149}}, "bone_007": {"head": {"x": 0.5373091101646423, "y": 0.1827419400215149, "z": 1.6803903579711914}, "tail": {"x": 0.5446823239326477, "y": 0.18519344925880432, "z": 1.6734424829483032}}, "r_upper_leg": {"head": {"x": -0.25513601303100586, "y": 0.07877105474472046, "z": 0.1387087106704712}, "tail": {"x": -0.5912657380104065, "y": 0.06895933300256729, "z": -0.9701849222183228}}, "r_lower_leg": {"head": {"x": -0.5912657380104065, "y": 0.06895933300256729, "z": -0.9701849222183228}, "tail": {"x": -0.9735252261161804, "y": 0.16303546726703644, "z": -2.18613862991333}}, "r_ancle": {"head": {"x": -0.9735252261161804, "y": 0.16303546726703644, "z": -2.18613862991333}, "tail": {"x": -1.0920854806900024, "y": -0.16820663213729858, "z": -2.3055927753448486}}, "r_toe": {"head": {"x": -1.0920854806900024, "y": -0.16820663213729858, "z": -2.3055927753448486}, "tail": {"x": -1.1538002490997314, "y": -0.4375842809677124, "z": -2.3457844257354736}}, "l_upper_leg": {"head": {"x": 0.2947828471660614, "y": 0.07972406595945358, "z": 0.13740015029907227}, "tail": {"x": 0.6011503338813782, "y": 0.07002540677785873, "z": -0.9768697023391724}}, "l_lower_leg": {"head": {"x": 0.6011503338813782, "y": 0.07002540677785873, "z": -0.9768697023391724}, "tail": {"x": 0.9844488501548767, "y": 0.163835346698761, "z": -2.193763494491577}}, "l_ancle": {"head": {"x": 0.9844488501548767, "y": 0.163835346698761, "z": -2.193763494491577}, "tail": {"x": 1.1052886247634888, "y": -0.16843479871749878, "z": -2.3099963665008545}}, "l_toe": {"head": {"x": 1.1052886247634888, "y": -0.16843479871749878, "z": -2.3099963665008545}, "tail": {"x": 1.1677669286727905, "y": -0.4420241117477417, "z": -2.3523178100585938}}, "bone": {"head": {"x": 0.010821257717907429, "y": 0.07877334952354431, "z": 0.08994382619857788}, "tail": {"x": 0.2947828471660614, "y": 0.07972406595945358, "z": 0.13740015029907227}}, "bone_001": {"head": {"x": 0.010821257717907429, "y": 0.07877334952354431, "z": 0.08994382619857788}, "tail": {"x": -0.25513601303100586, "y": 0.07877105474472046, "z": 0.1387087106704712}}} diff --git a/prefab/male_base_500f.stl b/prefab/male_base_500f.stl deleted file mode 100644 index 55ab98608..000000000 Binary files a/prefab/male_base_500f.stl and /dev/null differ diff --git a/src/body.rs b/src/body.rs index db20b9d17..f1cf15f34 100644 --- a/src/body.rs +++ b/src/body.rs @@ -1,4 +1,5 @@ use std::ops::DerefMut; +use std::sync::Arc; use ::bevy::app::{Plugin, Update}; use ::bevy::asset::Assets; @@ -13,6 +14,7 @@ use ::bevy::prelude::{Added, Drag, DragEnd, Entity, + IntoSystemConfigs, Mesh as BevyMesh, Mesh3d, Meshable, @@ -32,7 +34,8 @@ use ::bevy::render::mesh::VertexAttributeValues; use bevy::prelude as bevy; use bevy_ecs::component::ComponentIdFor; use bevy_pbr::MeshMaterial3d; -use nalgebra::{Isometry3, Unit, Vector3}; +use nalgebra::{Isometry3, Translation3, Unit, Vector3}; +use parry3d::shape::SharedShape; use rapier3d::prelude::{Collider, TypedShape}; use crate::camera::OrbitCamera; @@ -57,12 +60,10 @@ impl Plugin for BodyPlugin { .add_systems(Update, Body::draggable_added) .add_systems(Update, Body::collider_added) .add_systems(Update, Body::mesh_added) - .add_systems(Update, Body::rigid_committed) - .add_systems(Update, Body::xpbd_committed) - .add_systems(Update, Body::sync_rigid_building) - .add_systems(Update, Body::sync_xpbd_building) - .add_systems(Update, Body::sync_rigid) - .add_systems(Update, Body::sync_xpbd); + .add_systems(Update, + (Body::rigid_committed, Body::sync_rigid_building, Body::sync_rigid).chain()) + .add_systems(Update, + (Body::xpbd_committed, Body::sync_xpbd_building, Body::sync_xpbd).chain()); } } @@ -144,47 +145,19 @@ pub mod physics { pub struct XPBDVertexIndex(pub Vec); impl XPBDVertexIndex { - pub fn new(bodies: &rapier::RigidBodySet, - xpbd: &xpbd::Set, + pub fn new(xpbd: &xpbd::Set, handle: xpbd::Handle, trimesh: &TriMesh) -> Self { - let vert2rigidbody = |body: &xpbd::Body| { - let point2handle = body.handles - .iter() - .map(|b| { - let point = bodies.get(*b).unwrap().translation(); - (*point, *b) - }) - .collect::, rapier::RigidBodyHandle)>>(); - - trimesh.vertices - .iter() - .map(|point| { - let Some((dist, body)) = - point2handle.iter() - .fold(Option::<(f32, rapier::RigidBodyHandle)>::None, - |found, (pt, body)| { - let dist = (*pt - point).magnitude(); - match found { - | Some((fdist, fbody)) if fdist < dist => { - Some((fdist, fbody)) - }, - | _ => Some((dist, *body)), - } - }) - else { - unreachable!() - }; - if dist >= 0.1 { - panic!(); - } - body - }) - .collect::>() - }; - - Self(xpbd.get(handle, vert2rigidbody).unwrap()) + let face2handle = xpbd.get(handle, |b| b.trimesh.iter().map(|xs| xs.map(|x| b.handles[x])).collect::>()).unwrap(); + let mut v2b: Vec> = trimesh.vertices.iter().map(|_| None).collect(); + trimesh.faces.iter().enumerate().for_each(|(fix, face)| { + face.iter().enumerate().for_each(|(fvix, vix)| { + v2b[(*vix) as usize] = Some(face2handle[fix][fvix]); + }); + }); + let v2b = v2b.into_iter().map(|a| a.unwrap()).collect(); + Self(v2b) } } } @@ -247,6 +220,8 @@ impl Body { }; let body = phy.bodies.get_mut(*h).unwrap(); + body.set_body_type(rapier3d::prelude::RigidBodyType::KinematicPositionBased, true); + let (cam_id, mut cam, _) = cam.single_mut(); cam.enabled = false; @@ -272,13 +247,13 @@ impl Body { let denom = plane_normal.dot(&ray_dir); let t = plane_normal.dot(&(plane_origin - ray_origin)) / denom; let t = ray_origin + t * ray_dir; - body.set_translation(t, true); - body.set_linvel(Vector3::zeros(), true); + body.set_next_kinematic_position(Isometry3 {translation: Translation3::from(t), ..Default::default()}); } fn on_drag_end(t: Trigger>, mut commands: Commands, plane_id: ComponentIdFor, + mut rigid: Query<&physics::RigidBody>, mut cam: Query<&mut OrbitCamera>, mut phy: Query<&mut PhysicsState>) { let mut phy = phy.single_mut(); @@ -287,6 +262,11 @@ impl Body { } phy.ghosting = false; + if let Ok(physics::RigidBody(h)) = rigid.get_mut(t.entity()) { + let body = phy.bodies.get_mut(*h).unwrap(); + body.set_body_type(rapier3d::prelude::RigidBodyType::Dynamic, true); + }; + commands.entity(t.entity()).remove_by_id(plane_id.get()); } @@ -358,7 +338,9 @@ impl Body { let mut phy = phy.single_mut(); let PhysicsState { bodies, colliders, .. } = phy.deref_mut(); for (id, t, builder, mut coll) in &mut q { - coll.0.shape_mut().scale_dyn(&t.0.vector, 10); + let shp = coll.0.shape().scale_dyn(&t.0.vector, 10).unwrap(); + let shp = SharedShape(Arc::from(shp)); + coll.0.set_shape(shp); let body = bodies.insert(builder.0.clone()); colliders.insert_with_parent(coll.0.clone(), body, bodies); commands.entity(id) @@ -385,33 +367,37 @@ impl Body { .. } = phy.deref_mut(); for (id, mut t, mut s, mut builder, mut tri, mut tet, mut bones) in &mut q { + let builder = &mut builder.0; + let scale = &mut s.0.vector; + let translation = builder.transform.translation.vector; + let rotation = builder.transform.rotation; + let bones = &mut bones.0; let tet = &mut tet.0; let tri = &mut tri.0; - let builder = &mut builder.0; tet.scale(scale); - tet.translate(&builder.transform.translation.vector); - tet.rotate(&builder.transform.rotation); + tet.translate(&translation); + tet.rotate(&rotation); tri.scale(scale); - tri.translate(&builder.transform.translation.vector); - tri.rotate_about(tet.centroid(), &builder.transform.rotation); + tri.translate(&translation); + tri.rotate_about(tet.centroid(), &rotation); bones.scale(scale); - bones.translate(&builder.transform.translation.vector); - bones.rotate_about(&tet.centroid(), &builder.transform.rotation); + bones.translate(&translation); + bones.rotate_about(&tet.centroid(), &rotation); builder.transform = Isometry3::identity(); - let (bone_bodies_, extra_constraints) = bones.insert_for_xpbd(0.5, - builder.stiffness.unwrap() - * 5.0, - tet, - bodies, - colliders, - impulse_joints); + let (bone_bodies_, extra_constraints) = { + bones.insert_for_xpbd(builder.stiffness.unwrap() * 1.0, + tet, + bodies, + colliders, + impulse_joints) + }; for body in bone_bodies_.iter() .flat_map(|(_, (a, b, c))| [a, b, c]) @@ -425,7 +411,10 @@ impl Body { Draggable)); } - let xpbd_handle = core::mem::take(builder).mesh(tet.clone()) + let xpbd_handle = core::mem::take(builder).distance_constraint_damping(0.10) + .tetrahedra_volume_constraint_damping(0.10) + .linear_damping(0.10) + .mesh(tri.clone(), tet.clone()) .extra_constraints(extra_constraints) .build(xpbd, bodies, colliders) .unwrap(); @@ -434,7 +423,7 @@ impl Body { t.rotation = Quat::IDENTITY; t.translation = Vec3::splat(0.0); - let verts = physics::XPBDVertexIndex::new(bodies, xpbd, xpbd_handle, tri); + let verts = physics::XPBDVertexIndex::new(xpbd, xpbd_handle, tri); commands.entity(id) .insert((physics::XPBD(xpbd_handle), verts)) .remove::(); @@ -461,7 +450,8 @@ impl Body { } } - fn sync_rigid(phy: Query<&PhysicsState>, mut q: Query<(&physics::RigidBody, &mut Transform), With>) { + fn sync_rigid(phy: Query<&PhysicsState>, + mut q: Query<(&physics::RigidBody, &mut Transform), With>) { let phy = phy.single(); for (body, mut transform) in &mut q { let body = phy.bodies.get(body.0).unwrap(); @@ -475,9 +465,11 @@ impl Body { fn sync_xpbd(mut meshes: ResMut>, phy: Query<&PhysicsState>, - mut q: Query<(&physics::XPBDVertexIndex, &Mesh3d)>) { + mut q: Query<(&physics::XPBDVertexIndex, &Mesh3d, &mut Transform)>) { let phy = phy.single(); - for (bodies, mesh) in &mut q { + for (bodies, mesh, mut t) in &mut q { + *t = Transform::IDENTITY; + let mesh = meshes.get_mut(&mesh.0).unwrap(); let vertices = match mesh.attribute_mut(BevyMesh::ATTRIBUTE_POSITION).unwrap() { diff --git a/src/debug_render.rs b/src/debug_render.rs index 4cae53f1b..2157d941c 100644 --- a/src/debug_render.rs +++ b/src/debug_render.rs @@ -1,9 +1,11 @@ use bevy::app::{App, Plugin, Update}; +use bevy::color::palettes::css::WHITE; use bevy::color::Color; -use bevy::prelude::{Gizmos, Query, ResMut, Resource}; +use bevy::prelude::{Gizmos, Query, ResMut, Resource, Transform}; use nalgebra::{SimdValue, Vector3}; use rapier3d::math::Point; -use rapier3d::prelude::{DebugRenderBackend, +use rapier3d::prelude::{Aabb, + DebugRenderBackend, DebugRenderMode, DebugRenderObject, DebugRenderPipeline, @@ -70,7 +72,7 @@ fn debug_render_scene(mut debug_render: ResMut, &phy.multibody_joints, &phy.narrow_phase); - render_xpbd_bodies(&mut backend, &phy.bodies, &phy.xpbd); + // render_xpbd_bodies(&mut backend, &phy.bodies, &phy.xpbd); } } @@ -80,9 +82,11 @@ pub fn render_xpbd_bodies(backend: &mut BevyLinesRenderBackend, struct Colors { vol: [f32; 4], dst: [f32; 4], + contact: [f32; 4], } let color = Colors { vol: [126.0, 0.9, 0.4, 0.2], - dst: [169.0, 0.9, 0.4, 0.8] }; + dst: [169.0, 0.9, 0.4, 0.8], + contact: [0.0, 1.0, 1.0, 1.0] }; macro_rules! ixs2positions { ($body:expr, $a:expr) => { @@ -101,6 +105,16 @@ pub fn render_xpbd_bodies(backend: &mut BevyLinesRenderBackend, } for (_, body) in xpbd.iter() { + let aabb = body.aabb(); + let translation = aabb.center(); + let hext = aabb.half_extents(); + + backend.gizmos + .cuboid(Transform { translation: <[f32; 3]>::from(translation).into(), + scale: [hext.x, hext.y, hext.z].map(|a| a * 2.0).into(), + ..Default::default() }, + WHITE); + body.constraints .iter() .flat_map(|c| match c { @@ -129,6 +143,11 @@ pub fn render_xpbd_bodies(backend: &mut BevyLinesRenderBackend, | xpbd::constraint::impl_::Constraint::TetrahedronVolume(single) => { vec![(color.vol, ixs2positions!(body, single.idxs))] }, + | xpbd::constraint::impl_::Constraint::Contact(contact) => { + let b = bodies.get(body.handles[contact.idx]).unwrap(); + let t = b.translation(); + vec![(color.contact, vec![*t, contact.contact_point])] + }, }) .for_each(|(color, positions)| { let center = positions.iter() diff --git a/src/either.rs b/src/either.rs index 03192eb5b..b6ab46fb7 100644 --- a/src/either.rs +++ b/src/either.rs @@ -1,3 +1,5 @@ +use core::fmt::Debug; + #[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord, Hash)] pub enum Either { Left(A), @@ -11,6 +13,26 @@ impl Either { } impl Either { + pub fn unwrap_left(self) -> A + where B: Debug + { + match self { + | Self::Left(t) => t, + | Self::Right(e) => panic!("called `Either::unwrap_left()` on a `Right` value: {:?}", + &e), + } + } + + pub fn unwrap_right(self) -> B + where A: Debug + { + match self { + | Self::Right(t) => t, + | Self::Left(e) => panic!("called `Either::unwrap_right()` on a `Left` value: {:?}", + &e), + } + } + pub fn as_ref(&self) -> Either<&A, &B> { match self { | Either::Left(a) => Either::Left(a), diff --git a/src/main.rs b/src/main.rs index 5a173c67e..df9a4a902 100644 --- a/src/main.rs +++ b/src/main.rs @@ -119,6 +119,10 @@ pub fn map_array16(a: [T; 16], f: impl Fn(T) -> R) -> [R; 16] { #[macro_export] macro_rules! map_array { + (3; $a:expr, $f:expr) => {{ + let [a0, a1, a2] = $a; + [$f(a0), $f(a1), $f(a2)] + }}; (4; $a:expr, $f:expr) => {{ let [a0, a1, a2, a3] = $a; [$f(a0), $f(a1), $f(a2), $f(a3)] diff --git a/src/mesh.rs b/src/mesh.rs index 11ae366d9..bbfac9670 100644 --- a/src/mesh.rs +++ b/src/mesh.rs @@ -1,5 +1,6 @@ use fixed::types::{I24F8, I32F32}; use nalgebra::{Scale3, UnitQuaternion, Vector3}; +use parry3d::query::sat::cuboid_segment_find_local_separating_edge_twoway; use rapier3d::prelude::*; use rustc_hash::{FxHashMap, FxHashSet}; use serde::{Deserialize, Serialize}; @@ -15,6 +16,33 @@ pub struct TriMesh { } impl TriMesh { + pub fn new(faces: Vec<[u32; 3]>, vertices: Vec>) -> Self { + let faces = faces.into_iter() + .map(|ixs| ixs.map(|x| vertices[x as usize])) + .collect::>(); + let vertices = vertices.iter() + .copied() + .map(|a| a.map(I32F32::from_num)) + .collect::>() + .into_iter() + .collect::>(); + let faces = faces.into_iter() + .map(|pts| { + pts.map(|a| { + vertices.iter() + .enumerate() + .find(|(_, a_)| **a_ == a.map(I32F32::from_num)) + .unwrap() + .0 as u32 + }) + }) + .collect::>(); + Self { faces, + vertices: vertices.into_iter() + .map(|a| a.map(I32F32::to_num)) + .collect() } + } + pub fn scale(&mut self, scale: &Vector3) { let scale = |pt: &mut Vector3| { pt[0] *= scale[0]; @@ -120,19 +148,40 @@ impl Mesh { let mut vertices: FxHashSet> = Default::default(); let mut edges: FxHashSet<(Vector3, Vector3)> = Default::default(); - tets.iter().for_each(|pts| { - let [(_, a), (_, b), (_, c), (_, d)] = pts; - [a,b,c,d].into_iter().map(|a| a.map(I32F32::from_num)).for_each(|a| {vertices.insert(a);}); - [(a,b), (a,c),(a,d), (b,c), (b,d), (c,d)] - .into_iter() - .map(|(a, b)| (a.map(I32F32::from_num), b.map(I32F32::from_num))) - .map(|(a, b)| if a <= b {(a, b)} else {(b, a)}) - .for_each(|(a, b)| {edges.insert((a, b));}); - }); + let tets = tets.into_iter() + .inspect(|pts| { + let [(_, a), (_, b), (_, c), (_, d)] = pts; + [a, b, c, d].into_iter() + .map(|a| a.map(I32F32::from_num)) + .for_each(|a| { + vertices.insert(a); + }); + [(a, b), (a, c), (a, d), (b, c), (b, d), (c, d)].into_iter() + .map(|(a, b)| { + (a.map(I32F32::from_num), + b.map(I32F32::from_num)) + }) + .map(|(a, b)| { + if a <= b { + (a, b) + } else { + (b, a) + } + }) + .for_each(|(a, b)| { + edges.insert((a, b)); + }); + }) + .map(|pts| pts.map(|(b, pt)| (b, pt.map(I32F32::from_num).map(I32F32::to_num)))) + .collect(); Mesh { tets, - edges: edges.into_iter().map(|(a, b)| (a.map(I32F32::to_num), b.map(I32F32::to_num))).collect(), - vertices: vertices.into_iter().map(|a| a.map(I32F32::to_num)).collect() } + edges: edges.into_iter() + .map(|(a, b)| (a.map(I32F32::to_num), b.map(I32F32::to_num))) + .collect(), + vertices: vertices.into_iter() + .map(|a| a.map(I32F32::to_num)) + .collect() } } pub fn from_stl(stl: &[u8], @@ -480,7 +529,7 @@ impl Bones { let axis = UnitVector::new_normalize(Vector3::y_axis().cross(&d.normalize())); let rot_angle = Vector3::y_axis().dot(&d.normalize()).acos(); let rotation = UnitQuaternion::from_axis_angle(&axis, rot_angle); - let body = bodies.insert(RigidBodyBuilder::new(RigidBodyType::Dynamic).position(Isometry {translation: Translation::from(center), rotation})); + let body = bodies.insert(RigidBodyBuilder::new(RigidBodyType::Dynamic).linear_damping(0.5) .position(Isometry {translation: Translation::from(center), rotation})); colliders.insert_with_parent(ColliderBuilder::round_cylinder(dist / 2.0, 0.01, 0.01).density(10.0), body, bodies); body } @@ -488,7 +537,6 @@ impl Bones { #[allow(clippy::type_complexity)] pub fn insert_for_xpbd( &self, - thresh: f32, xpbd::constraint::Stiffness::YoungPoisson(young, poisson): xpbd::constraint::Stiffness, mesh: &Mesh, bodies: &mut RigidBodySet, @@ -502,27 +550,34 @@ impl Bones { let mut mesh_joints = Vec::<(Vector3, xpbd::constraint::DistanceBuilder)>::new(); - for a in &bones { - for (p, b) in [(a.head, a.head_body), - (a.tail, a.tail_body), - (a.center, a.link_body)] - { - mesh.vertices + let bone_bodies = bones.iter().flat_map(|b| { + [ + (b.head, b.head_body), + (b.tail, b.tail_body), + (b.center, b.link_body), + ] + }).collect::>(); + + let closest_bone_body = |p: Vector3| -> (Vector3, RigidBodyHandle) { + *bone_bodies.iter().min_by_key(|(p_, _)| { + I32F32::from_num((*p_ - p).magnitude()) + }) + .unwrap() + }; + + for v in mesh.vertices .iter() - .copied() - .filter(|p_| (p - p_).magnitude() <= thresh) - .for_each(|p_| { - let dst = (p - p_).magnitude(); - mesh_joints.push((p_, + .copied() { + let (p, body) = closest_bone_body(v); + let dst = (p - v).magnitude(); + mesh_joints.push((v, DistanceBuilder::new().priority(10) - .body_2(b) + .body_2(body) .rest_length(dst) .stiffness(young, poisson))); - }); - } - - dbg!(&mesh_joints); + } + for a in &bones { impulse_joints.insert(a.head_body, a.link_body, SphericalJointBuilder::new().local_anchor2(vector![0.0, diff --git a/src/prefab.rs b/src/prefab.rs index b6c411f81..8ca4b0d54 100644 --- a/src/prefab.rs +++ b/src/prefab.rs @@ -1,6 +1,8 @@ -use std::{collections::VecDeque, sync::LazyLock}; +use std::collections::VecDeque; +use std::sync::LazyLock; -use ::bevy::{prelude::{Cuboid, Meshable}, render::mesh::{Indices, MeshVertexAttribute, VertexAttributeValues}}; +use ::bevy::prelude::{Cuboid, Meshable}; +use ::bevy::render::mesh::{Indices, MeshVertexAttribute, VertexAttributeValues}; use bevy::prelude as bevy; use itertools::Itertools; use nalgebra::{vector, Point, Vector3}; @@ -9,19 +11,27 @@ use rustc_hash::FxHashMap; use crate::mesh::{self, Bones, Mesh}; -pub const HUMAN_MALE_BONES_JSON: &str = include_str!("../prefab/male_base-bones.json"); +pub const ANACONDA_BONES_JSON: &str = include_str!("../prefab/anaconda-bones.json"); +pub static ANACONDA_BONES: LazyLock = + LazyLock::new(|| serde_json::from_str(ANACONDA_BONES_JSON).unwrap()); + +pub const HUMAN_MALE_BONES_JSON: &str = include_str!("../prefab/human-male-bones.json"); pub static HUMAN_MALE_BONES: LazyLock = LazyLock::new(|| serde_json::from_str(HUMAN_MALE_BONES_JSON).unwrap()); -pub const HUMAN_FEMALE_BONES_JSON: &str = include_str!("../prefab/female_base-bones.json"); +pub const HUMAN_FEMALE_BONES_JSON: &str = include_str!("../prefab/human-female-bones.json"); pub static HUMAN_FEMALE_BONES: LazyLock = LazyLock::new(|| serde_json::from_str(HUMAN_FEMALE_BONES_JSON).unwrap()); -pub const HUMAN_MALE_STL: &[u8] = include_bytes!("../prefab/male_base_500f.stl"); +pub const ANACONDA_STL: &[u8] = include_bytes!("../prefab/anaconda.stl"); +pub static ANACONDA_MESH: LazyLock = + LazyLock::new(|| Mesh::from_stl(ANACONDA_STL, Some(0.001), None, None)); + +pub const HUMAN_MALE_STL: &[u8] = include_bytes!("../prefab/human-male.stl"); pub static HUMAN_MALE_MESH: LazyLock = LazyLock::new(|| Mesh::from_stl(HUMAN_MALE_STL, None, None, None)); -pub const HUMAN_FEMALE_STL: &[u8] = include_bytes!("../prefab/female_base_500f.stl"); +pub const HUMAN_FEMALE_STL: &[u8] = include_bytes!("../prefab/human-female.stl"); pub static HUMAN_FEMALE_MESH: LazyLock = LazyLock::new(|| Mesh::from_stl(HUMAN_FEMALE_STL, None, None, None)); @@ -50,6 +60,7 @@ impl Prefab { pub const fn all() -> &'static [Prefab] { &[Self::HumanMale, Self::HumanFemale, + Self::Snake, Self::Cylinder, Self::Cube] } @@ -69,6 +80,16 @@ impl Prefab { pub fn trimesh(self) -> mesh::TriMesh { let (faces, vertices) = match self { + | Prefab::Snake => { + let msh = mesh_loader::stl::from_slice(ANACONDA_STL).unwrap(); + let vertices = msh.meshes[0].vertices + .iter() + .copied() + .map(|[x, y, z]| Vector3::new(x, y, z)) + .collect::>(); + let faces = msh.meshes[0].faces.clone(); + (faces, vertices) + }, | Prefab::HumanMale => { let msh = mesh_loader::stl::from_slice(HUMAN_MALE_STL).unwrap(); let vertices = msh.meshes[0].vertices @@ -90,7 +111,7 @@ impl Prefab { (faces, points) }, | Prefab::Cylinder => { - let (points, faces) = rapier::Cylinder::new(0.5, 0.5).to_trimesh(10); + let (points, faces) = rapier::Cylinder::new(0.5, 0.5).to_trimesh(20); let points = points.into_iter().map(|a| a.coords).collect::>(); (faces, points) }, @@ -101,17 +122,16 @@ impl Prefab { let (points, faces) = subdiv(a); (faces, points) }, - | Prefab::Snake => todo!(), }; - mesh::TriMesh { faces, vertices } + mesh::TriMesh::new(faces, vertices) } pub fn mesh(self) -> (Mesh, Bones) { match self { | Prefab::HumanMale => (HUMAN_MALE_MESH.clone(), HUMAN_MALE_BONES.clone()), | Prefab::HumanFemale => (HUMAN_FEMALE_MESH.clone(), HUMAN_FEMALE_BONES.clone()), - | Prefab::Snake => todo!(), + | Prefab::Snake => (ANACONDA_MESH.clone(), ANACONDA_BONES.clone()), | Prefab::Cylinder => { let tri = self.trimesh(); let mesh = Mesh::from_trimesh(&tri.vertices, &tri.faces, None); @@ -147,11 +167,16 @@ fn bevy_mesh_tris(mesh: &bevy::Mesh) -> (Vec>, Vec<[u32; 3]>) { unreachable!(); }; - let VertexAttributeValues::Float32x3(points) = mesh.attribute(bevy::Mesh::ATTRIBUTE_POSITION).unwrap() else { + let VertexAttributeValues::Float32x3(points) = + mesh.attribute(bevy::Mesh::ATTRIBUTE_POSITION).unwrap() + else { unreachable!(); }; - let points = points.clone().into_iter().map(Vector3::from).collect::>(); + let points = points.clone() + .into_iter() + .map(Vector3::from) + .collect::>(); let mut faces_iter = faces.clone().into_iter(); let mut faces_chunked = vec![]; while let Some(a) = faces_iter.next_array::<3>() { @@ -161,25 +186,24 @@ fn bevy_mesh_tris(mesh: &bevy::Mesh) -> (Vec>, Vec<[u32; 3]>) { (points, faces_chunked) } -fn subdiv((points, faces): (Vec>, Vec<[u32; 3]>)) -> (Vec>, Vec<[u32; 3]>) { +fn subdiv((points, faces): (Vec>, Vec<[u32; 3]>)) + -> (Vec>, Vec<[u32; 3]>) { let mut new_points = points; let mut new_faces = Vec::new(); for ixs in faces { - let [ax,bx,cx] = ixs; - let [a,b,c] = ixs.map(|x| new_points[x as usize]); - let ab = (a + b) / 2.0; - let bc = (b + c) / 2.0; - let ca = (a + c) / 2.0; - let len = new_points.len(); - let [abx,bcx,cax] = [len, len + 1, len + 2].map(|n| n as u32); - new_points.extend([ab,bc,ca]); - new_faces.extend([ - [ax,abx,cax], - [abx,bx,bcx], - [bcx,cx,cax], - [cax,abx,bcx], - ].into_iter()); - } + let [ax, bx, cx] = ixs; + let [a, b, c] = ixs.map(|x| new_points[x as usize]); + let ab = (a + b) / 2.0; + let bc = (b + c) / 2.0; + let ca = (a + c) / 2.0; + let len = new_points.len(); + let [abx, bcx, acx] = [len, len + 1, len + 2].map(|n| n as u32); + new_points.extend([ab, bc, ca]); + new_faces.extend([[ax, abx, acx], + [bx, abx, bcx], + [cx, acx, bcx], + [abx, bcx, acx]].into_iter()); + } (new_points, new_faces) } diff --git a/src/xpbd/builder.rs b/src/xpbd/builder.rs index e9fb597de..d2f09848a 100644 --- a/src/xpbd/builder.rs +++ b/src/xpbd/builder.rs @@ -5,13 +5,23 @@ use fixed::types::I32F32; use graph::ConstraintId; use itertools::Itertools; use nalgebra::Isometry3; +use rapier3d::math::Translation; use rapier3d::na::Vector3; -use rapier3d::prelude::{RigidBodyHandle, *}; +use rapier3d::prelude::{ActiveHooks, + ColliderBuilder, + ColliderHandle, + ColliderSet, + RigidBodyBuilder, + RigidBodyHandle, + RigidBodySet, + RigidBodyType}; use rustc_hash::{FxHashMap, FxHashSet}; use super::constraint::{graph, Constraint, Stiffness}; use super::{constraint, step, Body, Handle, Set}; -use crate::mesh::Mesh; +use crate::either::Either; +use crate::mesh::{Mesh, TriMesh}; +use crate::xpbd::step::Step; fn vec_fixed(a: Vector3) -> Vector3 { a.map(I32F32::from_num) @@ -43,11 +53,13 @@ struct Inserted { pub(self) extra_bodies: FxHashMap>, pub(self) volume_constraints: Vec, pub(self) distance_constraints: Vec, + pub(self) faces: Vec<[RigidBodyHandle; 3]>, } impl Inserted { #[allow(clippy::too_many_arguments)] pub(self) fn insert(mesh: &Mesh, + trimesh: &TriMesh, bodies: &mut RigidBodySet, colliders: &mut ColliderSet, extra_dst: Vec<(Vector3, @@ -56,9 +68,12 @@ impl Inserted { particle_mass: f32, collider_radius: f32, collider_skin: Option, + linear_damping: f32, tet_damping: f32, dst_damping: f32) -> Self { + let TriMesh { faces, vertices } = trimesh; + let mut me = Self::default(); let mut extra_dst = extra_dst.into_iter().map(Some).collect(); me.spawn_points(mesh, @@ -66,12 +81,34 @@ impl Inserted { colliders, particle_mass, collider_radius, - collider_skin, &mut extra_dst); + collider_skin, + linear_damping, + &mut extra_dst); + + me.faces = faces.into_iter() + .map(|a| { + a.map(|x| { + let v = vertices[x as usize].map(I32F32::from_num); + if let Some((body, _)) = me.point2body.get(&v) { + *body + } else { + me.point2body.iter().min_by_key(|a| { + I32F32::from_num((a.0 - v).map(I32F32::to_num::).magnitude()) + }) + .unwrap() + .1 + .0 + } + }) + }) + .collect(); + me.tet_constraints(mesh, tet_damping, stiffness); - me.dst_constraints(bodies, mesh, dst_damping, stiffness); + me.dst_constraints(mesh, dst_damping, stiffness); let dropped_extra_constraints = extra_dst.iter().filter(|a| a.is_some()).count(); if dropped_extra_constraints > 0 { - eprintln!("Dropped {} extra constraints because no matching points could be found!", dropped_extra_constraints); + eprintln!("Dropped {} extra constraints because no matching points could be found!", + dropped_extra_constraints); } me } @@ -83,12 +120,14 @@ impl Inserted { particle_mass: f32, collider_radius: f32, collider_skin: Option, + linear_damping: f32, extra_constraints: &mut Vec, - constraint::DistanceBuilder)>>) { + constraint::DistanceBuilder)>>) { for point in &mesh.vertices { let body = RigidBodyBuilder::new(RigidBodyType::Dynamic).lock_rotations() + .linear_damping(linear_damping) .can_sleep(false) .translation(*point) .build(); @@ -105,26 +144,24 @@ impl Inserted { let coll = colliders.insert_with_parent(coll, body, bodies); self.point2body.insert(vec_fixed(*point), (body, coll)); - let x_dst_dist = extra_constraints.iter().filter_map(|a| a.as_ref()).map(|(p, _)| (*p - point).magnitude()).collect::>(); - dbg!(x_dst_dist); - let cs = - extra_constraints - .iter_mut().filter_map(|opt| { - if let Some((p, _)) = opt { - if (*p - point).magnitude() < 1e-2 { - return opt.take(); - } - } + let cs = extra_constraints.iter_mut() + .filter_map(|opt| { + if let Some((p, _)) = opt { + if (*p - point).magnitude() < 1e-2 { + return opt.take(); + } + } - None - }).collect::>(); + None + }) + .collect::>(); - for (_, c) in cs.into_iter() { - let colls = bodies.get(c.get_body_2()).unwrap().colliders().to_vec(); - self.extra_bodies.insert(c.get_body_2(), colls); - self.distance_constraints.push(c.body_1(body).build()); - } + for (_, c) in cs.into_iter() { + let colls = bodies.get(c.get_body_2()).unwrap().colliders().to_vec(); + self.extra_bodies.insert(c.get_body_2(), colls); + self.distance_constraints.push(c.body_1(body).build()); + } } } @@ -141,12 +178,7 @@ impl Inserted { } } - fn dst_constraints(&mut self, - bodies: &RigidBodySet, - mesh: &Mesh, - damping: f32, - stiffness: Stiffness, -) { + fn dst_constraints(&mut self, mesh: &Mesh, damping: f32, stiffness: Stiffness) { for (p1, p2) in &mesh.edges { let edge = [p1, p2]; let [b1, b2] = edge.map(|p| self.point2body.get(&vec_fixed(*p)).unwrap().0); @@ -160,7 +192,6 @@ impl Inserted { rest_length: dist }; self.distance_constraints.push(c); - } } } @@ -169,6 +200,7 @@ impl Inserted { #[derive(Clone, Debug)] pub struct Builder { pub transform: Isometry3, + pub trimesh: Option, pub mesh: Option, pub collider_radius: Option, pub collider_skin: Option, @@ -176,6 +208,7 @@ pub struct Builder { pub substeps: Option, pub stiffness: Option>, pub extra_constraints: Vec<(Vector3, constraint::DistanceBuilder)>, + pub linear_damping: Option, pub distance_constraint_damping: Option, pub tetrahedra_volume_constraint_damping: Option, } @@ -184,11 +217,13 @@ impl Default for Builder { fn default() -> Self { Self { transform: Isometry3::identity(), mesh: None, + trimesh: None, collider_radius: None, collider_skin: None, mass: None, substeps: None, stiffness: None, + linear_damping: None, distance_constraint_damping: None, tetrahedra_volume_constraint_damping: None, extra_constraints: Default::default() } @@ -229,7 +264,8 @@ impl Builder { } /// Add a tetrahedral mesh (**required**) - pub fn mesh(mut self, mesh: Mesh) -> Self { + pub fn mesh(mut self, trimesh: TriMesh, mesh: Mesh) -> Self { + self.trimesh = Some(trimesh); self.mesh = Some(mesh); self } @@ -248,6 +284,12 @@ impl Builder { self } + /// Set particle linear damping + pub fn linear_damping(mut self, d: f32) -> Self { + self.linear_damping = Some(d); + self + } + /// Set default volume constraint damping (not linear damping ex. air friction) pub fn tetrahedra_volume_constraint_damping(mut self, d: f32) -> Self { self.tetrahedra_volume_constraint_damping = Some(d); @@ -326,8 +368,6 @@ impl Builder { }); } - dbg!("post-packed constraint count", packed.iter().map(|a| a.count()).fold(0, |sum, a| a + sum)); - packed } @@ -339,17 +379,20 @@ impl Builder { -> Result { let Self { transform, mesh, + trimesh, collider_radius, collider_skin, mass, substeps, stiffness, extra_constraints, + linear_damping, tetrahedra_volume_constraint_damping, distance_constraint_damping, } = self; let substeps = substeps.unwrap_or(100); + let trimesh = trimesh.ok_or(Error::MeshRequired)?; let mut mesh = mesh.ok_or(Error::MeshRequired)?; mesh.rotate(&transform.rotation); mesh.translate(&transform.translation.vector); @@ -359,6 +402,7 @@ impl Builder { let stiffness = stiffness.ok_or(Error::StiffnessRequired)?; let Stiffness::YoungPoisson(young, poisson) = stiffness; let collider_radius = collider_radius.unwrap_or(0.02); + let linear_damping = linear_damping.unwrap_or(0.0); let tetrahedra_volume_constraint_damping = tetrahedra_volume_constraint_damping.unwrap_or(0.0); let distance_constraint_damping = distance_constraint_damping.unwrap_or(0.0); @@ -366,6 +410,7 @@ impl Builder { let dts = dt / substeps as f32; let inserted = Inserted::insert(&mesh, + &trimesh, bodies, colliders, extra_constraints, @@ -373,6 +418,7 @@ impl Builder { particle_mass, collider_radius, collider_skin, + linear_damping, tetrahedra_volume_constraint_damping, distance_constraint_damping); @@ -386,29 +432,48 @@ impl Builder { type Handle2Ix = FxHashMap; type Bodies = Vec; - let (handle2ix, bodies): (Handle2Ix, Bodies) = handles.iter() - .copied() - .enumerate() - .map(|(ix, handle)| { - let body = bodies.get(handle).unwrap(); - let body = step::Body::new(dts, body); - ((handle, ix), body) - }) - .collect(); + + let immovable = inserted.extra_bodies + .keys() + .copied() + .collect::>(); + let (handle2ix, bodies): (Handle2Ix, Bodies) = + handles.iter() + .copied() + .enumerate() + .map(|(ix, handle)| { + let body = bodies.get(handle).unwrap(); + let body = step::Body::new(dts, immovable.contains(&handle), body); + ((handle, ix), body) + }) + .collect(); let constraints = Self::build_constraints(inserted.distance_constraints, inserted.volume_constraints, &handle2ix); + let faces = inserted.faces + .iter() + .map(|faces| { + faces.map(|v| { + handles.iter() + .enumerate() + .find_map(|(ix, a)| if *a == v { Some(ix) } else { None }) + .unwrap() + }) + }) + .collect::>(); + let body = Body { dt, + trimesh: faces, dt_substep: dts, substeps, young, poisson, self_collision, handles, - immovable: inserted.extra_bodies.keys().copied().collect(), - step_buf: bodies, + immovable, + step: Step { dt: dts, bodies }, constraints }; Ok(xpbd.insert(body)) diff --git a/src/xpbd/constraint/graph.rs b/src/xpbd/constraint/graph.rs index 9a32e0483..5651f531e 100644 --- a/src/xpbd/constraint/graph.rs +++ b/src/xpbd/constraint/graph.rs @@ -80,7 +80,9 @@ impl Graph { /// divides the constraints into a vec of vecs of constraints, where the inner vecs' /// constraints are guaranteed to be the same priority and share no vertices. pub fn constraints_disj(&self, priorities: &[isize]) -> Vec> { - dbg!("pre-graph constraint count", self.constraints.len(), priorities.len()); + dbg!("pre-graph constraint count", + self.constraints.len(), + priorities.len()); let vertexmask = self.vertices.keys().map(|VertexId(n)| *n).collect::(); let vertexmask0 = vertexmask.clone().zeroed(); @@ -125,7 +127,8 @@ impl Graph { } let sets: Vec> = matchings.into_iter().map(|(_, cs, _)| cs).collect(); - dbg!("post-graph constraint count", sets.iter().fold(0, |sum, a| sum + a.len())); + dbg!("post-graph constraint count", + sets.iter().fold(0, |sum, a| sum + a.len())); sets } } diff --git a/src/xpbd/constraint/impl_.rs b/src/xpbd/constraint/impl_.rs index 7614825db..3521f3cfd 100644 --- a/src/xpbd/constraint/impl_.rs +++ b/src/xpbd/constraint/impl_.rs @@ -28,6 +28,7 @@ pub enum Constraint { Distance(distance::Single), PackedTetrahedronVolume(tetrahedron_volume::Packed), TetrahedronVolume(tetrahedron_volume::Single), + Contact(contact::Contact), } impl Constraint { @@ -96,6 +97,7 @@ impl super::Constraint for Constraint { | Self::Distance(a) => a.apply(step), | Self::PackedTetrahedronVolume(a) => a.apply(step), | Self::TetrahedronVolume(a) => a.apply(step), + | Self::Contact(a) => a.apply(step), } } @@ -105,6 +107,7 @@ impl super::Constraint for Constraint { | Self::Distance(a) => a.priority(), | Self::PackedTetrahedronVolume(a) => a.priority(), | Self::TetrahedronVolume(a) => a.priority(), + | Self::Contact(a) => a.priority(), } } @@ -114,6 +117,7 @@ impl super::Constraint for Constraint { | Self::Distance(a) => a.body_idxs(), | Self::PackedTetrahedronVolume(a) => a.body_idxs(), | Self::TetrahedronVolume(a) => a.body_idxs(), + | Self::Contact(a) => a.body_idxs(), } } @@ -123,6 +127,58 @@ impl super::Constraint for Constraint { | Self::Distance(a) => a.init(handle2ix), | Self::PackedTetrahedronVolume(a) => a.init(handle2ix), | Self::TetrahedronVolume(a) => a.init(handle2ix), + | Self::Contact(a) => a.init(handle2ix), + } + } +} + +/// XPBD <> XPBD collision +pub mod contact { + use nalgebra::{UnitVector3, Vector3}; + + use super::super::Constraint; + + /// A contact between a body and a face on another body + #[derive(Debug, PartialEq, Clone, Copy, serde::Serialize, serde::Deserialize)] + pub struct Contact { + /// Index of particle in [`Step.bodies`] + pub idx: usize, + /// Contact normal + pub normal: UnitVector3, + /// Point nearest to this body on the surface of the other body's mesh + pub contact_point: Vector3, + /// Index of the other body being contacted in the [`crate::xpbd::Set`] + pub other: usize, + /// Index to the face this contact (may) have penetrated on the + /// other body's trimesh + pub face: usize, + } + + impl Constraint for Contact { + fn apply(&mut self, step: &mut crate::xpbd::step::Step) { + let b = &mut step.bodies[self.idx]; + let vmax = 10.0; // m/s + let signed_dist = (b.next.p - self.contact_point).dot(&self.normal.into_inner()); + if signed_dist >= 0.0 { + return; + } + + let depth = (b.next.p - self.contact_point).magnitude(); + let depth = depth.min(vmax * step.dt); + let correction = self.normal.into_inner() * depth; + b.next.p += correction; + } + + fn priority(&self) -> isize { + 100 + } + + fn body_idxs(&self) -> Vec { + vec![] + } + + fn init(&mut self, _: &rustc_hash::FxHashMap) { + () } } } @@ -274,7 +330,8 @@ pub mod distance { bodies: &[step::Body; 2], deltas: [Vector3; 2]) -> [Vector3; 2] - where Vector3: Mul> + where Vector3: Mul>, + ::SimdBool: core::fmt::Debug + Copy { let dt = T::splat(dt); let atilde = compliance / dt.simd_powi(2); @@ -484,7 +541,8 @@ pub mod tetrahedron_volume { volume: T::SimdRealField, bodies: &[step::Body; 4]) -> [Vector3; 4] - where Vector3: Mul> + where Vector3: Mul>, + ::SimdBool: Copy + core::fmt::Debug { let constraint = volume - rest_volume; diff --git a/src/xpbd/handle.rs b/src/xpbd/handle.rs index a0763cd5d..ac3a5ad10 100644 --- a/src/xpbd/handle.rs +++ b/src/xpbd/handle.rs @@ -1,3 +1,3 @@ /// Handle to an xpbd body in an xpbd body set. -#[derive(Copy, Clone, Debug, PartialEq, Eq)] +#[derive(Debug, PartialEq, Clone, Copy, serde::Serialize, serde::Deserialize)] pub struct Handle(pub usize); diff --git a/src/xpbd/mod.rs b/src/xpbd/mod.rs index c919e182f..73c1128d3 100644 --- a/src/xpbd/mod.rs +++ b/src/xpbd/mod.rs @@ -1,9 +1,14 @@ use core::f32; use core::fmt::Debug; +use std::collections::BTreeMap; pub use builder::Builder; use constraint::Constraint; pub use handle::Handle; +use nalgebra::Vector3; +use rapier3d::parry::bounding_volume::Aabb; +use rapier3d::parry::partitioning::Qbvh; +use rapier3d::parry::query::details::RayCompositeShapeToiAndNormalBestFirstVisitor; use rapier3d::prelude::{RigidBodyHandle, *}; use rustc_hash::FxHashSet; pub use set::Set; @@ -23,42 +28,43 @@ pub struct Body { pub substeps: usize, pub young: f32, pub poisson: f32, + pub trimesh: Vec<[usize; 3]>, pub self_collision: FxHashSet, pub handles: Vec, pub immovable: FxHashSet, - pub step_buf: Vec, + pub step: Step, pub constraints: Vec, } impl Body { - /// Do a single simulation step - pub fn step(&mut self, bodies: &mut RigidBodySet) { - for ix in 0..self.handles.len() { - self.step_buf[ix] = step::Body::new(self.dt_substep, bodies.get(self.handles[ix]).unwrap()); + /// Do a single simulation substep + pub fn substep(&mut self) { + let Self { constraints, step, .. } = self; + + for c in constraints.iter_mut() { + c.apply(step); } - let mut step = Step { bodies: core::mem::take(&mut self.step_buf), - dt: self.dt_substep }; + step.predict_next(); - for _ in 0..self.substeps { - for c in self.constraints.iter_mut() { - c.apply(&mut step); - } + self.constraints = core::mem::take(&mut self.constraints).into_iter() + .filter(|a| { + !matches!(a, + constraint::impl_::Constraint::Contact(_)) + }) + .collect(); + } - step.bodies.iter_mut().enumerate().for_each(|(ix, b)| { - if self.immovable.contains(&self.handles[ix]) { - *b = b.immovable(); - } else { - *b = b.predict_next(false, self.dt_substep) - } - }); - } - - self.step_buf = step.bodies; - - for (ix, p) in self.step_buf.iter().enumerate() { + /// Apply simulation results to rapier environment + pub fn apply(&mut self, bodies: &mut RigidBodySet) { + for (ix, p) in self.step.bodies.iter().enumerate() { let handle = self.handles[ix]; let rb = bodies.get_mut(handle).unwrap(); + + if !rb.is_dynamic() { + return; + } + let pred = rb.predict_position_using_velocity_and_forces(self.dt); let dx = p.next.p - pred.translation.vector; if rb.is_dynamic() { @@ -72,4 +78,127 @@ impl Body { } } } + + /// Sync step bodies with rapier world positions + pub fn pre_step(&mut self, bodies: &mut RigidBodySet) { + for ix in 0..self.handles.len() { + self.step + .init_body(ix, bodies.get(self.handles[ix]).unwrap()); + } + } + + pub fn generate_parry_trimesh(&self) -> parry3d::shape::TriMesh { + let vertices = self.step + .bodies + .iter() + .map(|b| Point::from(b.next.p)) + .collect::>(); + let faces = self.trimesh + .iter() + .copied() + .map(|a| a.map(|a| a as u32)) + .collect(); + let mut trimesh = rapier3d::prelude::TriMesh::new(vertices, faces).unwrap(); + trimesh.set_flags(TriMeshFlags::ORIENTED).unwrap(); + trimesh + } + + pub fn generate_contacts(&mut self, + thresh: f32, + ix_of_other: usize, + other: &parry3d::shape::TriMesh) { + if !other.aabb(&Isometry::identity()).intersects(&self.aabb()) { + return; + }; + + let points = self.step + .bodies + .iter() + .map(|b| b.next.p) + .collect::>(); + + for (ix, pt) in points.iter().enumerate() { + let Some((proj, (face, _))) = + other.project_local_point_and_get_location_with_max_dist(&Point::from(*pt), true, thresh) + else { + continue; + }; + if !proj.is_inside { + continue; + }; + + let Some(normal) = other.feature_normal(FeatureId::Face(face)) else { + unreachable!() + }; + + self.constraints.push( + constraint::impl_::Constraint::Contact( + constraint::impl_::contact::Contact { + idx: ix, + normal, + contact_point: proj.point.coords, + other: ix_of_other, + face: face as usize, + } + ), + ); + } + } + + pub fn update_contacts(&mut self, bodies: &[Self]) { + self.constraints + .iter_mut() + .filter_map(|c| match c { + | constraint::impl_::Constraint::Contact(c) => Some(c), + | _ => None, + }) + .for_each(|contact| { + let face = bodies[contact.other].trimesh[contact.face]; + let [a, b, c] = face.map(|x| Point::from(bodies[contact.other].step.bodies[x].next.p)); + let tri = parry3d::shape::Triangle::new(a, b, c); + let proj = + tri.project_local_point(&Point::from(self.step.bodies[contact.idx].next.p), false); + contact.normal = tri.normal().unwrap(); + contact.contact_point = proj.point.coords; + }); + } + + pub fn aabb(&self) -> Aabb { + let (Some(mins), Some(maxs)) = self.step + .bodies + .iter() + .fold((None, None), |(mins, maxs), b| { + let p = b.next.p; + match (mins, maxs) { + | (None, None) => (Some(p), Some(p)), + | (Some(mut mins), Some(mut maxs)) => { + if mins.x > p.x { + mins.x = p.x; + } + if mins.y > p.y { + mins.y = p.y; + } + if mins.z > p.z { + mins.z = p.z; + } + if maxs.x < p.x { + maxs.x = p.x; + } + if maxs.y < p.y { + maxs.y = p.y; + } + if maxs.z < p.z { + maxs.z = p.z; + } + (Some(mins), Some(maxs)) + }, + | _ => unreachable!(), + } + }) + else { + unreachable!() + }; + Aabb { mins: Point::from(mins), + maxs: Point::from(maxs) } + } } diff --git a/src/xpbd/set.rs b/src/xpbd/set.rs index 26a4d8c9c..dc343e39e 100644 --- a/src/xpbd/set.rs +++ b/src/xpbd/set.rs @@ -5,6 +5,7 @@ use std::ops::Deref; use std::ptr::NonNull; use std::sync::{Arc, Mutex, MutexGuard}; +use bevy::winit::select_monitor; use rapier3d::prelude::*; use super::handle::Handle; @@ -59,8 +60,74 @@ impl Set { /// Do a single simulation step pub fn step(&self, bodies: &mut RigidBodySet) { let mut bs = self.bodies.lock().unwrap(); - for b in bs.iter_mut() { - b.step(bodies); + if bs.len() == 0 { + return; + } + + // SAFETY + // Each body needs a mutable reference to other bodies + // to do collision detection, index check prevents + // double mutable reference to the same element, and + // elements are not moved or removed within the vec + // from under either mutable reference. + let (bsa, bsb) = unsafe { + let ptr = (&mut bs) as *mut MutexGuard>; + (ptr.as_mut().unwrap(), ptr.as_mut().unwrap()) + }; + + // WANT + // interleaved substeps of all bodies + // + // eg. substeps for [a,b,c] is [10,50,250] where a:b and b:c is 1:5 + // it should look like: + // + // a: . . + // b: . . . . . . + // c: . . . . . . . . . . . . . . . . . . . . . + // + // [7, 70, 200] - substeps (frequency) + // 1400 - least common multiple + // [200, 20, 7] - substep delays + // then for n in 0..1400, for m in delays, if n % m == 0 do substep + let substeps = bsa.iter().map(|b| b.substeps).collect::>(); + let substep_lcm = substeps.iter() + .skip(1) + .fold(substeps[0], |b, a| (a * b) / gcd(*a, b)); + let substep_interval = substeps.iter().map(|a| substep_lcm / a).collect::>(); + + for b in bsa.iter_mut() { + b.pre_step(bodies); + } + + // At the top of the step, regenerate trimeshes and perform collision detection + // within a threshold. For each potential collision, make contact constraints. + let trimesh = bs.iter() + .map(|b| b.generate_parry_trimesh()) + .collect::>(); + let contact_constraint_thresh = 0.1; + for (ax, ab) in bsa.iter_mut().enumerate() { + for (bx, mesh) in trimesh.iter().enumerate() { + if ax == bx { + continue; + } + ab.generate_contacts(contact_constraint_thresh, bx, mesh); + } + } + + for n in 0..substep_lcm { + for (ax, ab) in bsa.iter_mut().enumerate() { + // update contact constraints with update face + // positions + ab.update_contacts(&bsb); + + if n == 0 || n % substep_interval[ax] == 0 { + ab.substep(); + } + } + } + + for b in bsa.iter_mut() { + b.apply(bodies); } } @@ -135,3 +202,28 @@ impl<'a> Iterator for Iter<'a> { iter.next().map(|(ix, a)| (Handle(ix), a)) } } + +// https://docs.rs/num-integer/0.1.46/src/num_integer/lib.rs.html#870 +fn gcd(mut m: usize, mut n: usize) -> usize { + if m == 0 || n == 0 { + return m | n; + } + + // find common factors of 2 + let shift = (m | n).trailing_zeros(); + + // divide n and m by 2 until odd + m >>= m.trailing_zeros(); + n >>= n.trailing_zeros(); + + while m != n { + if m > n { + m -= n; + m >>= m.trailing_zeros(); + } else { + n -= m; + n >>= n.trailing_zeros(); + } + } + m << shift +} diff --git a/src/xpbd/step.rs b/src/xpbd/step.rs index 212d1d0a6..7c11d2f5d 100644 --- a/src/xpbd/step.rs +++ b/src/xpbd/step.rs @@ -1,21 +1,87 @@ use core::fmt::Debug; -use nalgebra::{Scalar, SimdComplexField, SimdValue}; +use nalgebra::{ArrayStorage, Scalar, SimdComplexField, SimdValue, Vector3}; use rapier3d::prelude::RigidBody; -use simba::simd::WideF32x8; +use simba::simd::{WideBoolF32x8, WideF32x8}; use crate::rigidbody_hack; -#[derive(Debug)] +pub const fn wide_bool(b: [bool; 8]) -> WideBoolF32x8 { + pub const fn b2f(b: bool) -> f32 { + if b { + 1.0 + } else { + 0.0 + } + } + + WideBoolF32x8(wide::f32x8::new(crate::map_array!(8; b, b2f))) +} + +pub const fn pack_vector(f: [Vector3; 8]) -> Vector3 { + pub const fn vector_x(f: Vector3) -> f32 { + f.data.0[0][0] + } + + pub const fn vector_y(f: Vector3) -> f32 { + f.data.0[0][1] + } + + pub const fn vector_z(f: Vector3) -> f32 { + f.data.0[0][2] + } + + let x = crate::map_array!(8; f, vector_x); + let y = crate::map_array!(8; f, vector_y); + let z = crate::map_array!(8; f, vector_z); + Vector3::from_array_storage(ArrayStorage([[WideF32x8(wide::f32x8::new(x)), + WideF32x8(wide::f32x8::new(y)), + WideF32x8(wide::f32x8::new(z))]])) +} + +#[derive(Debug, Clone, serde::Serialize, serde::Deserialize)] pub struct Step { pub dt: f32, pub bodies: Vec, } +impl Step { + pub fn init_body(&mut self, ix: usize, body: &RigidBody) { + let p = *body.translation(); + let v = *body.linvel(); + + let b = &mut self.bodies[ix]; + let f = unsafe { + let body = core::mem::transmute::<&RigidBody, &rigidbody_hack::RigidBody>(body); + body.forces.force + }; + let init = body::State { p, v, f }; + + b.init = init; + b.prev = init; + b.next = init; + *b = b.predict_next(true, self.dt) + } + + pub fn predict_next(&mut self) { + let Self { bodies, .. } = self; + bodies.iter_mut().for_each(|b| { + if b.immovable { + *b = b.immovable(); + } else { + *b = b.predict_next(false, self.dt); + } + }); + } +} + #[derive(Clone, Copy, Debug, serde::Serialize, serde::Deserialize)] -pub struct Body { - #[serde(bound = "T: Copy + Scalar")] +pub struct Body + where T::SimdBool: Copy + Debug +{ + #[serde(bound = "T: Copy + Scalar, ::SimdBool: serde::Serialize + for<'a> serde::Deserialize<'a>")] pub mass: T, + pub immovable: ::SimdBool, pub inv_mass: T, pub linear_damping: T, pub init: body::State, @@ -26,17 +92,20 @@ pub struct Body { impl Body { pub const ZERO_F32: Body = Body { mass: 0.0, inv_mass: 0.0, + immovable: false, linear_damping: 0.0, init: body::State::ZERO_F32, prev: body::State::ZERO_F32, next: body::State::ZERO_F32 }; - pub const ZERO_F32X8: Body = Body { mass: WideF32x8::ZERO, - inv_mass: WideF32x8::ZERO, - linear_damping: WideF32x8::ZERO, - init: body::State::ZERO_F32X8, - prev: body::State::ZERO_F32X8, - next: body::State::ZERO_F32X8 }; + pub const ZERO_F32X8: Body = + Body { mass: WideF32x8::ZERO, + immovable: WideBoolF32x8(wide::f32x8::new([0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0])), + inv_mass: WideF32x8::ZERO, + linear_damping: WideF32x8::ZERO, + init: body::State::ZERO_F32X8, + prev: body::State::ZERO_F32X8, + next: body::State::ZERO_F32X8 }; } impl Body { @@ -52,7 +121,7 @@ impl Body { self.next } - pub fn new(dt_substep: f32, body: &RigidBody) -> Self { + pub fn new(dt_substep: f32, immovable: bool, body: &RigidBody) -> Self { let p = *body.translation(); let v = *body.linvel(); let f = unsafe { @@ -65,6 +134,7 @@ impl Body { let init = body::State { p, v, f }; Self { mass, + immovable, inv_mass, init, linear_damping: body.linear_damping(), @@ -72,10 +142,11 @@ impl Body { next: init }.predict_next(true, dt_substep) } - pub fn pack(ps: &[&Self; 8]) -> Body { - let inv_mass: WideF32x8 = WideF32x8::from(crate::map_array!(8; ps, _.inv_mass)); + pub const fn pack(ps: &[&Self; 8]) -> Body { + let inv_mass: WideF32x8 = WideF32x8(wide::f32x8::new(crate::map_array!(8; ps, _.inv_mass))); Body { mass: WideF32x8::ZERO, + immovable: wide_bool(crate::map_array!(8; ps, _.immovable)), inv_mass, linear_damping: WideF32x8::ZERO, init: body::State::ZERO_F32X8, @@ -84,7 +155,9 @@ impl Body { } } -impl> Body { +impl> Body + where ::SimdBool: Copy + core::fmt::Debug +{ pub fn immovable(mut self) -> Self { self.prev = self.init; self.next = self.init; @@ -123,10 +196,11 @@ impl> Body { pub mod body { use nalgebra::{ArrayStorage, Scalar}; - use num_traits::Zero; use rapier3d::na::Vector3; use simba::simd::WideF32x8; + use super::pack_vector; + #[derive(Clone, Copy, Debug, serde::Serialize, serde::Deserialize)] pub struct State { /// Position @@ -161,12 +235,11 @@ pub mod body { self.p } - pub fn pack(ss: [Self; 8]) -> State { - let p: Vector3 = crate::map_array!(8; ss, _.p).into(); + pub const fn pack(ss: [Self; 8]) -> State { + let p = pack_vector(crate::map_array!(8; ss, _.p)); State { p, - v: Vector3::zero(), - f: Vector3::zero() } + ..Self::ZERO_F32X8 } } } }