diff --git a/Cargo.lock b/Cargo.lock index 0e7024dd4..9af9698b3 100644 --- a/Cargo.lock +++ b/Cargo.lock @@ -107,6 +107,15 @@ dependencies = [ "memchr", ] +[[package]] +name = "alloca" +version = "0.4.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "e5a7d05ea6aea7e9e64d25b9156ba2fee3fdd659e34e41063cd2fc7cd020d7f4" +dependencies = [ + "cc", +] + [[package]] name = "allocator-api2" version = "0.2.21" @@ -353,15 +362,14 @@ dependencies = [ "bevy_transform_interpolation", "bitflags 2.10.0", "bytemuck", - "criterion", + "criterion 0.8.1", "derive_more", "disqualified", "examples_common_2d", "glam 0.30.10", "glam_matrix_extras", - "itertools 0.13.0", + "itertools 0.14.0", "libm", - "nalgebra", "obvhs", "parry2d", "parry2d-f64", @@ -383,14 +391,13 @@ dependencies = [ "bevy_mod_debugdump", "bevy_transform_interpolation", "bitflags 2.10.0", - "criterion", + "criterion 0.8.1", "derive_more", "disqualified", "examples_common_3d", "glam_matrix_extras", - "itertools 0.13.0", + "itertools 0.14.0", "libm", - "nalgebra", "obvhs", "parry3d", "parry3d-f64", @@ -2314,7 +2321,7 @@ dependencies = [ "cast", "ciborium", "clap", - "criterion-plot", + "criterion-plot 0.5.0", "is-terminal", "itertools 0.10.5", "num-traits", @@ -2330,6 +2337,31 @@ dependencies = [ "walkdir", ] +[[package]] +name = "criterion" +version = "0.8.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "4d883447757bb0ee46f233e9dc22eb84d93a9508c9b868687b274fc431d886bf" +dependencies = [ + "alloca", + "anes", + "cast", + "ciborium", + "clap", + "criterion-plot 0.8.1", + "itertools 0.13.0", + "num-traits", + "oorandom", + "page_size", + "plotters", + "rayon", + "regex", + "serde", + "serde_json", + "tinytemplate", + "walkdir", +] + [[package]] name = "criterion-plot" version = "0.5.0" @@ -2340,6 +2372,16 @@ dependencies = [ "itertools 0.10.5", ] +[[package]] +name = "criterion-plot" +version = "0.8.1" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "ed943f81ea2faa8dcecbbfa50164acf95d555afec96a27871663b300e387b2e4" +dependencies = [ + "cast", + "itertools 0.13.0", +] + [[package]] name = "critical-section" version = "1.2.0" @@ -2894,96 +2936,6 @@ dependencies = [ "xml-rs", ] -[[package]] -name = "glam" -version = "0.14.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "333928d5eb103c5d4050533cec0384302db6be8ef7d3cebd30ec6a35350353da" - -[[package]] -name = "glam" -version = "0.15.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "3abb554f8ee44336b72d522e0a7fe86a29e09f839a36022fa869a7dfe941a54b" - -[[package]] -name = "glam" -version = "0.16.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "4126c0479ccf7e8664c36a2d719f5f2c140fbb4f9090008098d2c291fa5b3f16" - -[[package]] -name = "glam" -version = "0.17.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e01732b97afd8508eee3333a541b9f7610f454bb818669e66e90f5f57c93a776" - -[[package]] -name = "glam" -version = "0.18.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "525a3e490ba77b8e326fb67d4b44b4bd2f920f44d4cc73ccec50adc68e3bee34" - -[[package]] -name = "glam" -version = "0.19.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "2b8509e6791516e81c1a630d0bd7fbac36d2fa8712a9da8662e716b52d5051ca" - -[[package]] -name = "glam" -version = "0.20.5" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f43e957e744be03f5801a55472f593d43fabdebf25a4585db250f04d86b1675f" - -[[package]] -name = "glam" -version = "0.21.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "518faa5064866338b013ff9b2350dc318e14cc4fcd6cb8206d7e7c9886c98815" - -[[package]] -name = "glam" -version = "0.22.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "12f597d56c1bd55a811a1be189459e8fad2bbc272616375602443bdfb37fa774" - -[[package]] -name = "glam" -version = "0.23.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8e4afd9ad95555081e109fe1d21f2a30c691b5f0919c67dfa690a2e1eb6bd51c" - -[[package]] -name = "glam" -version = "0.24.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "b5418c17512bdf42730f9032c74e1ae39afc408745ebb2acf72fbc4691c17945" - -[[package]] -name = "glam" -version = "0.25.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "151665d9be52f9bb40fc7966565d39666f2d1e69233571b71b87791c7e0528b3" - -[[package]] -name = "glam" -version = "0.27.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "9e05e7e6723e3455f4818c7b26e855439f7546cf617ef669d1adedb8669e5cb9" - -[[package]] -name = "glam" -version = "0.28.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "779ae4bf7e8421cf91c0b3b64e7e8b40b862fba4d393f59150042de7c4965a94" - -[[package]] -name = "glam" -version = "0.29.3" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "8babf46d4c1c9d92deac9f7be466f76dfc4482b6452fc5024b5e8daf6ffeb3ee" - [[package]] name = "glam" version = "0.30.10" @@ -3020,6 +2972,19 @@ dependencies = [ "serde", ] +[[package]] +name = "glamx" +version = "0.1.3" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "375b4fa374a343fef990a18a6e0413d54bd990c6d7b8c7ada2d3c884275edea3" +dependencies = [ + "approx", + "glam 0.30.10", + "num-traits", + "serde", + "simba", +] + [[package]] name = "glob" version = "0.3.3" @@ -3571,16 +3536,6 @@ dependencies = [ "regex-automata", ] -[[package]] -name = "matrixmultiply" -version = "0.3.10" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a06de3016e9fae57a36fd14dba131fccf49f74b40b7fbdb472f96e361ec71a08" -dependencies = [ - "autocfg", - "rawpointer", -] - [[package]] name = "memchr" version = "2.7.6" @@ -3681,50 +3636,6 @@ dependencies = [ "unicode-ident", ] -[[package]] -name = "nalgebra" -version = "0.34.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "c4d5b3eff5cd580f93da45e64715e8c20a3996342f1e466599cf7a267a0c2f5f" -dependencies = [ - "approx", - "glam 0.14.0", - "glam 0.15.2", - "glam 0.16.0", - "glam 0.17.3", - "glam 0.18.0", - "glam 0.19.0", - "glam 0.20.5", - "glam 0.21.3", - "glam 0.22.0", - "glam 0.23.0", - "glam 0.24.2", - "glam 0.25.0", - "glam 0.27.0", - "glam 0.28.0", - "glam 0.29.3", - "glam 0.30.10", - "matrixmultiply", - "nalgebra-macros", - "num-complex", - "num-rational", - "num-traits", - "serde", - "simba", - "typenum", -] - -[[package]] -name = "nalgebra-macros" -version = "0.3.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "973e7178a678cfd059ccec50887658d482ce16b0aa9da3888ddeab5cd5eb4889" -dependencies = [ - "proc-macro2", - "quote", - "syn", -] - [[package]] name = "nanorand" version = "0.6.1" @@ -3830,16 +3741,6 @@ dependencies = [ "windows-sys 0.61.2", ] -[[package]] -name = "num-bigint" -version = "0.4.6" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "a5e44f723f1133c9deac646763579fdb3ac745e418f2a7af9cd0c431da1f20b9" -dependencies = [ - "num-integer", - "num-traits", -] - [[package]] name = "num-complex" version = "0.4.6" @@ -3847,7 +3748,6 @@ source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "73f88a1307638156682bada9d7604135552957b7818057dcef22705b4d509495" dependencies = [ "num-traits", - "serde", ] [[package]] @@ -3861,26 +3761,6 @@ dependencies = [ "syn", ] -[[package]] -name = "num-integer" -version = "0.1.46" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "7969661fd2958a5cb096e56c8e1ad0444ac2bbcd0061bd28660485a44879858f" -dependencies = [ - "num-traits", -] - -[[package]] -name = "num-rational" -version = "0.4.2" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "f83d14da390562dca69fc84082e73e548e1ad308d24accdedd2720017cb37824" -dependencies = [ - "num-bigint", - "num-integer", - "num-traits", -] - [[package]] name = "num-traits" version = "0.2.19" @@ -4256,6 +4136,16 @@ dependencies = [ "ttf-parser", ] +[[package]] +name = "page_size" +version = "0.6.0" +source = "registry+https://github.com/rust-lang/crates.io-index" +checksum = "30d5b2194ed13191c1999ae0704b7839fb18384fa22e49b57eeaa97d79ce40da" +dependencies = [ + "libc", + "winapi", +] + [[package]] name = "parking" version = "2.2.1" @@ -4287,9 +4177,9 @@ dependencies = [ [[package]] name = "parry2d" -version = "0.25.3" +version = "0.26.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "ef681740349cec3ab9b5996b03b459b383b6998e1ffcb2804e8b57eb1e8491d9" +checksum = "e7b05ce9599a62eec9f6b761cf32647ab1dee43477b5c00ff9221a4b7cb80c4b" dependencies = [ "approx", "arrayvec", @@ -4298,10 +4188,10 @@ dependencies = [ "either", "ena", "foldhash 0.2.0", + "glamx", "hashbrown 0.16.1", "indexmap", "log", - "nalgebra", "num-derive", "num-traits", "ordered-float", @@ -4317,9 +4207,9 @@ dependencies = [ [[package]] name = "parry2d-f64" -version = "0.25.3" +version = "0.26.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "835d4ca2b68026b04a75ca6196da961d8c0895420eacc9cc2201f7ad54775780" +checksum = "f96560d0fcb25ff9020749a94571fffffa2483907d5369be67df3ff278333784" dependencies = [ "approx", "arrayvec", @@ -4328,10 +4218,10 @@ dependencies = [ "either", "ena", "foldhash 0.2.0", + "glamx", "hashbrown 0.16.1", "indexmap", "log", - "nalgebra", "num-derive", "num-traits", "ordered-float", @@ -4347,9 +4237,9 @@ dependencies = [ [[package]] name = "parry3d" -version = "0.25.3" +version = "0.26.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "e99471b7b6870f7fe406d5611dd4b4c9b07aa3e5436b1d27e1515f9832bb0c6b" +checksum = "2e04d21bda5249438b5695e7f08f1655524a6025bcdb186c324b7a66562f2d61" dependencies = [ "approx", "arrayvec", @@ -4358,11 +4248,10 @@ dependencies = [ "either", "ena", "foldhash 0.2.0", - "glam 0.30.10", + "glamx", "hashbrown 0.16.1", "indexmap", "log", - "nalgebra", "num-derive", "num-traits", "ordered-float", @@ -4380,9 +4269,9 @@ dependencies = [ [[package]] name = "parry3d-f64" -version = "0.25.3" +version = "0.26.0" source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "38fe282b81b60a2aee7f24db25ea73b3c82f6451888eeb5936b621adb87aa653" +checksum = "38558e45fda09a243836f590b9890f7f21dd2078dd7aafc876a9e8998d378d42" dependencies = [ "approx", "arrayvec", @@ -4391,10 +4280,10 @@ dependencies = [ "either", "ena", "foldhash 0.2.0", + "glamx", "hashbrown 0.16.1", "indexmap", "log", - "nalgebra", "num-derive", "num-traits", "ordered-float", @@ -4725,12 +4614,6 @@ version = "0.6.2" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "20675572f6f24e9e76ef639bc5552774ed45f1c30e2951e1e99c59888861c539" -[[package]] -name = "rawpointer" -version = "0.2.1" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "60a357793950651c4ed0f3f52338f53b2f809f32d83a07f72909fa13e4c6c1e3" - [[package]] name = "rayon" version = "1.11.0" @@ -4759,7 +4642,7 @@ checksum = "6e7970b4e577b76a96d5e56b5f6662b66d1a4e1f5bb026ee118fc31b373c2752" dependencies = [ "arbitrary-chunks", "block-pseudorand", - "criterion", + "criterion 0.5.1", "partition", "tikv-jemallocator", "voracious_radix_sort", @@ -5699,12 +5582,6 @@ version = "1.0.3" source = "registry+https://github.com/rust-lang/crates.io-index" checksum = "bc7d623258602320d5c55d1bc22793b57daff0ec7efc270ea7d55ce1d5f5471c" -[[package]] -name = "typenum" -version = "1.19.0" -source = "registry+https://github.com/rust-lang/crates.io-index" -checksum = "562d481066bde0658276a35467c4af00bdc6ee726305698a55b86e61d7ad82bb" - [[package]] name = "typewit" version = "1.14.2" diff --git a/crates/avian2d/Cargo.toml b/crates/avian2d/Cargo.toml index 333f3c205..5b0494df9 100644 --- a/crates/avian2d/Cargo.toml +++ b/crates/avian2d/Cargo.toml @@ -37,7 +37,7 @@ enhanced-determinism = [ "parry2d-f64?/enhanced-determinism", ] -default-collider = ["dep:nalgebra"] +default-collider = [] # We unfortunately can't reuse the f32 and f64 features for this, # because Parry uses separate crates for f32 and f64. parry-f32 = ["f32", "dep:parry2d", "default-collider"] @@ -87,16 +87,15 @@ bevy_heavy = { version = "0.4" } bevy_transform_interpolation = { version = "0.4" } libm = { version = "0.2", optional = true } approx = "0.5" -parry2d = { version = "0.25", optional = true } -parry2d-f64 = { version = "0.25", optional = true } -nalgebra = { version = "0.34", features = ["convert-glam030"], optional = true } +parry2d = { version = "0.26", optional = true } +parry2d-f64 = { version = "0.26", optional = true } obvhs = { version = "0.3" } serde = { version = "1", features = ["derive"], optional = true } derive_more = "2" thiserror = "2" arrayvec = "0.7" smallvec = "1.15" -itertools = "0.13" +itertools = "0.14" bitflags = "2.5.0" thread_local = { version = "1.1" } disqualified = { version = "1.0" } @@ -107,7 +106,7 @@ bevy = { version = "0.18.0", default-features = false, features = ["2d", "ui"] } bevy_heavy = { version = "0.4", features = ["approx"] } glam = { version = "0.30", features = ["bytemuck"] } bytemuck = "1.19" -criterion = { version = "0.5", features = ["html_reports"] } +criterion = { version = "0.8", features = ["html_reports"] } bevy_mod_debugdump = { version = "0.15" } [lints] diff --git a/crates/avian3d/Cargo.toml b/crates/avian3d/Cargo.toml index b82d2aace..6c2636a35 100644 --- a/crates/avian3d/Cargo.toml +++ b/crates/avian3d/Cargo.toml @@ -38,7 +38,7 @@ enhanced-determinism = [ "parry3d-f64?/enhanced-determinism", ] -default-collider = ["dep:nalgebra"] +default-collider = [] # We unfortunately can't reuse the f32 and f64 features for this, # because Parry uses separate crates for f32 and f64. parry-f32 = ["f32", "dep:parry3d", "default-collider"] @@ -89,15 +89,14 @@ bevy_heavy = { version = "0.4" } bevy_transform_interpolation = { version = "0.4" } libm = { version = "0.2", optional = true } approx = "0.5" -parry3d = { version = "0.25", optional = true } -parry3d-f64 = { version = "0.25", optional = true } -nalgebra = { version = "0.34", features = ["convert-glam030"], optional = true } +parry3d = { version = "0.26", optional = true } +parry3d-f64 = { version = "0.26", optional = true } obvhs = { version = "0.3" } serde = { version = "1", features = ["derive"], optional = true } derive_more = "2" thiserror = "2" smallvec = "1.15" -itertools = "0.13" +itertools = "0.14" bitflags = "2.5.0" thread_local = { version = "1.1" } disqualified = { version = "1.0" } @@ -111,7 +110,7 @@ bevy = { version = "0.18.0", features = [ "experimental_bevy_feathers", ] } bevy_heavy = { version = "0.4", features = ["approx"] } -criterion = { version = "0.5", features = ["html_reports"] } +criterion = { version = "0.8", features = ["html_reports"] } bevy_mod_debugdump = { version = "0.15" } rand = "0.9" diff --git a/src/collision/collider/mod.rs b/src/collision/collider/mod.rs index 98f186e67..f76655e3d 100644 --- a/src/collision/collider/mod.rs +++ b/src/collision/collider/mod.rs @@ -476,8 +476,8 @@ impl ColliderAabb { pub fn from_shape(shape: &crate::parry::shape::SharedShape) -> Self { let aabb = shape.compute_local_aabb(); Self { - min: aabb.mins.into(), - max: aabb.maxs.into(), + min: aabb.mins, + max: aabb.maxs, } } diff --git a/src/collision/collider/parry/contact_query.rs b/src/collision/collider/parry/contact_query.rs index 39d028153..b48b8e704 100644 --- a/src/collision/collider/parry/contact_query.rs +++ b/src/collision/collider/parry/contact_query.rs @@ -72,8 +72,8 @@ pub fn contact( ) -> Result, UnsupportedShape> { let rotation1: Rotation = rotation1.into(); let rotation2: Rotation = rotation2.into(); - let isometry1 = make_isometry(position1.into(), rotation1); - let isometry2 = make_isometry(position2.into(), rotation2); + let isometry1 = make_pose(position1, rotation1); + let isometry2 = make_pose(position2, rotation2); parry::query::contact( &isometry1, @@ -85,10 +85,10 @@ pub fn contact( .map(|contact| { if let Some(contact) = contact { // Transform contact data into local space - let point1: Vector = rotation1.inverse() * Vector::from(contact.point1); - let point2: Vector = rotation2.inverse() * Vector::from(contact.point2); - let normal1: Vector = (rotation1.inverse() * Vector::from(contact.normal1)).normalize(); - let normal2: Vector = (rotation2.inverse() * Vector::from(contact.normal2)).normalize(); + let point1: Vector = rotation1.inverse() * contact.point1; + let point2: Vector = rotation2.inverse() * contact.point2; + let normal1: Vector = (rotation1.inverse() * contact.normal1).normalize(); + let normal2: Vector = (rotation2.inverse() * contact.normal2).normalize(); // Make sure the normals are valid if !normal1.is_normalized() || !normal2.is_normalized() { @@ -167,8 +167,8 @@ pub fn contact_manifolds( let position2: Position = position2.into(); let rotation1: Rotation = rotation1.into(); let rotation2: Rotation = rotation2.into(); - let isometry1 = make_isometry(position1, rotation1); - let isometry2 = make_isometry(position2, rotation2); + let isometry1 = make_pose(position1, rotation1); + let isometry2 = make_pose(position2, rotation2); let isometry12 = isometry1.inv_mul(&isometry2); // TODO: Reuse manifolds from previous frame to improve performance @@ -199,14 +199,14 @@ pub fn contact_manifolds( prediction_distance, ) { - let normal = rotation1 * Vector::from(contact.normal1); + let normal = rotation1 * contact.normal1; // Make sure the normal is valid if !normal.is_normalized() { return; } - let local_point1: Vector = contact.point1.into(); + let local_point1: Vector = contact.point1; // The contact point is the midpoint of the two points in world space. // The anchors are relative to the positions of the colliders. @@ -231,11 +231,7 @@ pub fn contact_manifolds( } let subpos1 = manifold.subshape_pos1.unwrap_or_default(); - let local_normal: Vector = subpos1 - .rotation - .transform_vector(&manifold.local_n1) - .normalize() - .into(); + let local_normal: Vector = (subpos1.rotation * manifold.local_n1).normalize(); let normal = rotation1 * local_normal; // Make sure the normal is valid @@ -246,7 +242,7 @@ pub fn contact_manifolds( let points = manifold.contacts().iter().map(|contact| { // The contact point is the midpoint of the two points in world space. // The anchors are relative to the positions of the colliders. - let point1 = rotation1 * Vector::from(subpos1.transform_point(&contact.local_p1)); + let point1 = rotation1 * subpos1.transform_point(contact.local_p1); let anchor1 = point1 + normal * contact.dist * 0.5; let anchor2 = anchor1 + (position1.0 - position2.0); let world_point = position1.0 + anchor1; @@ -355,8 +351,8 @@ pub fn closest_points( ) -> Result { let rotation1: Rotation = rotation1.into(); let rotation2: Rotation = rotation2.into(); - let isometry1 = make_isometry(position1.into(), rotation1); - let isometry2 = make_isometry(position2.into(), rotation2); + let isometry1 = make_pose(position1, rotation1); + let isometry2 = make_pose(position2, rotation2); parry::query::closest_points( &isometry1, @@ -368,7 +364,7 @@ pub fn closest_points( .map(|closest_points| match closest_points { parry::query::ClosestPoints::Intersecting => ClosestPoints::Intersecting, parry::query::ClosestPoints::WithinMargin(point1, point2) => { - ClosestPoints::WithinMargin(point1.into(), point2.into()) + ClosestPoints::WithinMargin(point1, point2) } parry::query::ClosestPoints::Disjoint => ClosestPoints::OutsideMargin, }) @@ -432,8 +428,8 @@ pub fn distance( ) -> Result { let rotation1: Rotation = rotation1.into(); let rotation2: Rotation = rotation2.into(); - let isometry1 = make_isometry(position1.into(), rotation1); - let isometry2 = make_isometry(position2.into(), rotation2); + let isometry1 = make_pose(position1, rotation1); + let isometry2 = make_pose(position2, rotation2); parry::query::distance( &isometry1, @@ -500,8 +496,8 @@ pub fn intersection_test( ) -> Result { let rotation1: Rotation = rotation1.into(); let rotation2: Rotation = rotation2.into(); - let isometry1 = make_isometry(position1.into(), rotation1); - let isometry2 = make_isometry(position2.into(), rotation2); + let isometry1 = make_pose(position1, rotation1); + let isometry2 = make_pose(position2, rotation2); parry::query::intersection_test( &isometry1, @@ -588,15 +584,15 @@ pub fn time_of_impact( let velocity1: LinearVelocity = velocity1.into(); let velocity2: LinearVelocity = velocity2.into(); - let isometry1 = make_isometry(position1.into(), rotation1); - let isometry2 = make_isometry(position2.into(), rotation2); + let isometry1 = make_pose(position1, rotation1); + let isometry2 = make_pose(position2, rotation2); parry::query::cast_shapes( &isometry1, - &velocity1.0.into(), + velocity1.0, collider1.shape_scaled().0.as_ref(), &isometry2, - &velocity2.0.into(), + velocity2.0, collider2.shape_scaled().0.as_ref(), ShapeCastOptions { max_time_of_impact, @@ -607,10 +603,10 @@ pub fn time_of_impact( .map(|toi| { toi.map(|toi| TimeOfImpact { time_of_impact: toi.time_of_impact, - point1: toi.witness1.into(), - point2: toi.witness2.into(), - normal1: toi.normal1.into(), - normal2: toi.normal2.into(), + point1: toi.witness1, + point2: toi.witness2, + normal1: toi.normal1, + normal2: toi.normal2, status: toi.status, }) }) diff --git a/src/collision/collider/parry/mod.rs b/src/collision/collider/parry/mod.rs index 50515d241..a55f7bee1 100644 --- a/src/collision/collider/parry/mod.rs +++ b/src/collision/collider/parry/mod.rs @@ -11,16 +11,13 @@ mod primitives3d; pub use primitives2d::{EllipseColliderShape, RegularPolygonColliderShape}; use super::EnlargedAabb; -use crate::{make_isometry, prelude::*}; +use crate::{make_pose, prelude::*}; #[cfg(feature = "collider-from-mesh")] use bevy::mesh::{Indices, VertexAttributeValues}; use bevy::{log, prelude::*}; use contact_query::UnsupportedShape; use itertools::Either; -use parry::{ - math::Point, - shape::{RoundShape, SharedShape, TypedShape, Voxels}, -}; +use parry::shape::{RoundShape, SharedShape, TypedShape, Voxels}; impl> From for Collider { fn from(value: T) -> Self { @@ -412,10 +409,10 @@ impl AnyCollider for Collider { ) -> ColliderAabb { let aabb = self .shape_scaled() - .compute_aabb(&make_isometry(position, rotation)); + .compute_aabb(&make_pose(position, rotation)); ColliderAabb { - min: aabb.mins.into(), - max: aabb.maxs.into(), + min: aabb.mins, + max: aabb.maxs, } } @@ -463,7 +460,7 @@ impl ComputeMassProperties for Collider { fn center_of_mass(&self) -> Vec2 { let props = self.shape_scaled().mass_properties(1.0); - Vector::from(props.local_com).f32() + props.local_com.f32() } fn mass_properties(&self, density: f32) -> MassProperties { @@ -474,10 +471,10 @@ impl ComputeMassProperties for Collider { #[cfg(feature = "2d")] angular_inertia: props.principal_inertia() as f32, #[cfg(feature = "3d")] - principal_angular_inertia: Vector::from(props.principal_inertia()).f32(), + principal_angular_inertia: props.principal_inertia().f32(), #[cfg(feature = "3d")] - local_inertial_frame: Quaternion::from(props.principal_inertia_local_frame).f32(), - center_of_mass: Vector::from(props.local_com).f32(), + local_inertial_frame: props.principal_inertia_local_frame.f32(), + center_of_mass: props.local_com.f32(), } } } @@ -495,17 +492,17 @@ impl ComputeMassProperties for Collider { fn principal_angular_inertia(&self, mass: f32) -> Vec3 { let props = self.shape_scaled().mass_properties(mass as Scalar); - Vector::from(props.principal_inertia()).f32() + props.principal_inertia().f32() } fn local_inertial_frame(&self) -> Quat { let props = self.shape_scaled().mass_properties(1.0); - Quaternion::from(props.principal_inertia_local_frame).f32() + props.principal_inertia_local_frame.f32() } fn center_of_mass(&self) -> Vec3 { let props = self.shape_scaled().mass_properties(1.0); - Vector::from(props.local_com).f32() + props.local_com.f32() } fn mass_properties(&self, density: f32) -> MassProperties { @@ -516,10 +513,10 @@ impl ComputeMassProperties for Collider { #[cfg(feature = "2d")] angular_inertia: props.principal_inertia() as f32, #[cfg(feature = "3d")] - principal_angular_inertia: Vector::from(props.principal_inertia()).f32(), + principal_angular_inertia: props.principal_inertia().f32(), #[cfg(feature = "3d")] - local_inertial_frame: Quaternion::from(props.principal_inertia_local_frame).f32(), - center_of_mass: Vector::from(props.local_com).f32(), + local_inertial_frame: props.principal_inertia_local_frame.f32(), + center_of_mass: props.local_com.f32(), } } } @@ -606,12 +603,10 @@ impl Collider { point: Vector, solid: bool, ) -> (Vector, bool) { - let projection = self.shape_scaled().project_point( - &make_isometry(translation, rotation), - &point.into(), - solid, - ); - (projection.point.into(), projection.is_inside) + let projection = + self.shape_scaled() + .project_point(&make_pose(translation, rotation), point, solid); + (projection.point, projection.is_inside) } /// Computes the minimum distance between the given `point` and `self` transformed by `translation` and `rotation`. @@ -626,11 +621,8 @@ impl Collider { point: Vector, solid: bool, ) -> Scalar { - self.shape_scaled().distance_to_point( - &make_isometry(translation, rotation), - &point.into(), - solid, - ) + self.shape_scaled() + .distance_to_point(&make_pose(translation, rotation), point, solid) } /// Tests whether the given `point` is inside of `self` transformed by `translation` and `rotation`. @@ -641,7 +633,7 @@ impl Collider { point: Vector, ) -> bool { self.shape_scaled() - .contains_point(&make_isometry(translation, rotation), &point.into()) + .contains_point(&make_pose(translation, rotation), point) } /// Computes the distance and normal between the given ray and `self` @@ -666,12 +658,12 @@ impl Collider { solid: bool, ) -> Option<(Scalar, Vector)> { let hit = self.shape_scaled().cast_ray_and_get_normal( - &make_isometry(translation, rotation), - &parry::query::Ray::new(ray_origin.into(), ray_direction.into()), + &make_pose(translation, rotation), + &parry::query::Ray::new(ray_origin, ray_direction), max_distance, solid, ); - hit.map(|hit| (hit.time_of_impact, hit.normal.into())) + hit.map(|hit| (hit.time_of_impact, hit.normal)) } /// Tests whether the given ray intersects `self` transformed by `translation` and `rotation`. @@ -690,8 +682,8 @@ impl Collider { max_distance: Scalar, ) -> bool { self.shape_scaled().intersects_ray( - &make_isometry(translation, rotation), - &parry::query::Ray::new(ray_origin.into(), ray_direction.into()), + &make_pose(translation, rotation), + &parry::query::Ray::new(ray_origin, ray_direction), max_distance, ) } @@ -714,7 +706,7 @@ impl Collider { .into_iter() .map(|(p, r, c)| { ( - make_isometry(*p.into(), r.into()), + make_pose(*p.into(), r.into()), c.into().shape_scaled().clone(), ) }) @@ -797,8 +789,8 @@ impl Collider { /// and its height along the `Y` axis, excluding the hemispheres. pub fn capsule(radius: Scalar, length: Scalar) -> Self { SharedShape::capsule( - (Vector::Y * length * 0.5).into(), - (Vector::NEG_Y * length * 0.5).into(), + Vector::Y * length * 0.5, + Vector::NEG_Y * length * 0.5, radius, ) .into() @@ -806,18 +798,18 @@ impl Collider { /// Creates a collider with a capsule shape defined by its radius and endpoints `a` and `b`. pub fn capsule_endpoints(radius: Scalar, a: Vector, b: Vector) -> Self { - SharedShape::capsule(a.into(), b.into(), radius).into() + SharedShape::capsule(a, b, radius).into() } /// Creates a collider with a [half-space](https://en.wikipedia.org/wiki/Half-space_(geometry)) shape /// defined by the outward normal of its planar boundary. pub fn half_space(outward_normal: Vector) -> Self { - SharedShape::halfspace(nalgebra::Unit::new_normalize(outward_normal.into())).into() + SharedShape::halfspace(outward_normal.normalize_or_zero()).into() } /// Creates a collider with a segment shape defined by its endpoints `a` and `b`. pub fn segment(a: Vector, b: Vector) -> Self { - SharedShape::segment(a.into(), b.into()).into() + SharedShape::segment(a, b).into() } /// Creates a collider with a triangle shape defined by its points `a`, `b`, and `c`. @@ -829,7 +821,7 @@ impl Collider { /// consider using [`Collider::triangle_unchecked`] instead. #[cfg(feature = "2d")] pub fn triangle(a: Vector, b: Vector, c: Vector) -> Self { - let mut triangle = parry::shape::Triangle::new(a.into(), b.into(), c.into()); + let mut triangle = parry::shape::Triangle::new(a, b, c); // Make sure the triangle is counterclockwise. This is needed for collision detection. if triangle.orientation(1e-8) == parry::shape::TriangleOrientation::Clockwise { @@ -847,13 +839,13 @@ impl Collider { /// If you are unsure about the orientation of the triangle, consider using [`Collider::triangle`] instead. #[cfg(feature = "2d")] pub fn triangle_unchecked(a: Vector, b: Vector, c: Vector) -> Self { - SharedShape::triangle(a.into(), b.into(), c.into()).into() + SharedShape::triangle(a, b, c).into() } /// Creates a collider with a triangle shape defined by its points `a`, `b`, and `c`. #[cfg(feature = "3d")] pub fn triangle(a: Vector, b: Vector, c: Vector) -> Self { - SharedShape::triangle(a.into(), b.into(), c.into()).into() + SharedShape::triangle(a, b, c).into() } /// Creates a collider with a regular polygon shape defined by the circumradius and the number of sides. @@ -864,7 +856,6 @@ impl Collider { /// Creates a collider with a polyline shape defined by its vertices and optionally an index buffer. pub fn polyline(vertices: Vec, indices: Option>) -> Self { - let vertices = vertices.into_iter().map(|v| v.into()).collect(); SharedShape::polyline(vertices, indices).into() } @@ -901,7 +892,6 @@ impl Collider { vertices: Vec, indices: Vec<[u32; 3]>, ) -> Result { - let vertices = vertices.into_iter().map(|v| v.into()).collect(); SharedShape::trimesh(vertices, indices).map(|trimesh| trimesh.into()) } @@ -945,7 +935,6 @@ impl Collider { indices: Vec<[u32; 3]>, flags: TrimeshFlags, ) -> Result { - let vertices = vertices.into_iter().map(|v| v.into()).collect(); SharedShape::trimesh_with_flags(vertices, indices, flags.into()) .map(|trimesh| trimesh.into()) } @@ -954,7 +943,6 @@ impl Collider { /// defined by its vertex and index buffers. #[cfg(feature = "2d")] pub fn convex_decomposition(vertices: Vec, indices: Vec<[u32; 2]>) -> Self { - let vertices = vertices.iter().map(|v| (*v).into()).collect::>(); SharedShape::convex_decomposition(&vertices, &indices).into() } @@ -962,7 +950,6 @@ impl Collider { /// defined by its vertex and index buffers. #[cfg(feature = "3d")] pub fn convex_decomposition(vertices: Vec, indices: Vec<[u32; 3]>) -> Self { - let vertices = vertices.iter().map(|v| (*v).into()).collect::>(); SharedShape::convex_decomposition(&vertices, &indices).into() } @@ -975,7 +962,6 @@ impl Collider { indices: Vec<[u32; 2]>, params: &VhacdParameters, ) -> Self { - let vertices = vertices.iter().map(|v| (*v).into()).collect::>(); SharedShape::convex_decomposition_with_params(&vertices, &indices, ¶ms.clone().into()) .into() } @@ -989,7 +975,6 @@ impl Collider { indices: Vec<[u32; 3]>, params: VhacdParameters, ) -> Self { - let vertices = vertices.iter().map(|v| (*v).into()).collect::>(); SharedShape::convex_decomposition_with_params(&vertices, &indices, ¶ms.clone().into()) .into() } @@ -998,7 +983,6 @@ impl Collider { /// the [convex hull](https://en.wikipedia.org/wiki/Convex_hull) of the given points. #[cfg(feature = "2d")] pub fn convex_hull(points: Vec) -> Option { - let points = points.iter().map(|v| (*v).into()).collect::>(); SharedShape::convex_hull(&points).map(Into::into) } @@ -1006,7 +990,6 @@ impl Collider { /// the [convex hull](https://en.wikipedia.org/wiki/Convex_hull) of the given points. #[cfg(feature = "3d")] pub fn convex_hull(points: Vec) -> Option { - let points = points.iter().map(|v| (*v).into()).collect::>(); SharedShape::convex_hull(&points).map(Into::into) } @@ -1015,7 +998,6 @@ impl Collider { /// assumed and not checked. #[cfg(feature = "2d")] pub fn convex_polyline(points: Vec) -> Option { - let points = points.iter().map(|v| (*v).into()).collect::>(); SharedShape::convex_polyline(points).map(Into::into) } @@ -1023,10 +1005,17 @@ impl Collider { /// /// Each voxel has the size `voxel_size` and grid coordinate given by `grid_coordinates`. pub fn voxels(voxel_size: Vector, grid_coordinates: &[IVector]) -> Self { - let shape = Voxels::new( - voxel_size.into(), - &Self::ivec_array_from_point_int_array(grid_coordinates), - ); + #[cfg(all(feature = "2d", feature = "f64"))] + let grid_coordinates = &grid_coordinates + .iter() + .map(|c| c.as_i64vec2()) + .collect::>(); + #[cfg(all(feature = "3d", feature = "f64"))] + let grid_coordinates = &grid_coordinates + .iter() + .map(|c| c.as_i64vec3()) + .collect::>(); + let shape = Voxels::new(voxel_size, grid_coordinates); SharedShape::new(shape).into() } @@ -1034,11 +1023,7 @@ impl Collider { /// /// Each voxel has the size `voxel_size` and contains at least one point from `points`. pub fn voxels_from_points(voxel_size: Vector, points: &[Vector]) -> Self { - SharedShape::voxels_from_points( - voxel_size.into(), - &Self::vec_array_from_point_float_array(points), - ) - .into() + SharedShape::voxels_from_points(voxel_size, points).into() } /// Creates a voxel collider obtained from the decomposition of the given polyline into voxelized convex parts. @@ -1049,8 +1034,7 @@ impl Collider { voxel_size: Scalar, fill_mode: FillMode, ) -> Self { - let vertices = Self::vec_array_from_point_float_array(vertices); - SharedShape::voxelized_mesh(&vertices, indices, voxel_size, fill_mode.into()).into() + SharedShape::voxelized_mesh(vertices, indices, voxel_size, fill_mode.into()).into() } /// Creates a voxel collider obtained from the decomposition of the given trimesh into voxelized convex parts. @@ -1061,8 +1045,7 @@ impl Collider { voxel_size: Scalar, fill_mode: FillMode, ) -> Self { - let vertices = Self::vec_array_from_point_float_array(vertices); - SharedShape::voxelized_mesh(&vertices, indices, voxel_size, fill_mode.into()).into() + SharedShape::voxelized_mesh(vertices, indices, voxel_size, fill_mode.into()).into() } /// Creates a voxel collider obtained from the decomposition of the given `Mesh` into voxelized convex parts. @@ -1112,7 +1095,7 @@ impl Collider { parameters: &VhacdParameters, ) -> Vec { SharedShape::voxelized_convex_decomposition_with_params( - &Self::vec_array_from_point_float_array(vertices), + vertices, indices, ¶meters.clone().into(), ) @@ -1121,30 +1104,6 @@ impl Collider { .collect() } - fn ivec_array_from_point_int_array(points: &[IVector]) -> Vec> { - points - .iter() - .map(|p| { - #[cfg(feature = "2d")] - return Point::new(p.x, p.y); - #[cfg(feature = "3d")] - return Point::new(p.x, p.y, p.z); - }) - .collect::>() - } - - fn vec_array_from_point_float_array(points: &[Vector]) -> Vec> { - points - .iter() - .map(|p| { - #[cfg(feature = "2d")] - return Point::new(p.x, p.y); - #[cfg(feature = "3d")] - return Point::new(p.x, p.y, p.z); - }) - .collect::>() - } - /// Creates a collider with a heightfield shape. /// /// A 2D heightfield is a segment along the `X` axis, subdivided at regular intervals. @@ -1153,7 +1112,7 @@ impl Collider { /// the scaling factor along each axis. #[cfg(feature = "2d")] pub fn heightfield(heights: Vec, scale: Vector) -> Self { - SharedShape::heightfield(heights.into(), scale.into()).into() + SharedShape::heightfield(heights, scale).into() } /// Creates a collider with a heightfield shape. @@ -1177,8 +1136,8 @@ impl Collider { "Each row in `heights` must have the same amount of points" ); - let heights = nalgebra::DMatrix::from_vec(row_count, column_count, data); - SharedShape::heightfield(heights, scale.into()).into() + let heights = parry::utils::Array2::new(row_count, column_count, data); + SharedShape::heightfield(heights, scale).into() } /// Creates a collider with a triangle mesh shape from a `Mesh`. @@ -1534,7 +1493,7 @@ impl Collider { } #[cfg(feature = "collider-from-mesh")] -type VerticesIndices = (Vec>, Vec<[u32; 3]>); +type VerticesIndices = (Vec, Vec<[u32; 3]>); #[cfg(feature = "collider-from-mesh")] fn extract_mesh_vertices_indices(mesh: &Mesh) -> Option { @@ -1573,12 +1532,12 @@ fn scale_shape( ) -> Result { let scale = scale.abs(); match shape.as_typed_shape() { - TypedShape::Cuboid(s) => Ok(SharedShape::new(s.scaled(&scale.abs().into()))), + TypedShape::Cuboid(s) => Ok(SharedShape::new(s.scaled(scale.abs()))), TypedShape::RoundCuboid(s) => Ok(SharedShape::new(RoundShape { border_radius: s.border_radius, - inner_shape: s.inner_shape.scaled(&scale.abs().into()), + inner_shape: s.inner_shape.scaled(scale.abs()), })), - TypedShape::Capsule(c) => match c.scaled(&scale.abs().into(), num_subdivisions) { + TypedShape::Capsule(c) => match c.scaled(scale.abs(), num_subdivisions) { None => { log::error!("Failed to apply scale {} to Capsule shape.", scale); Ok(SharedShape::ball(0.0)) @@ -1599,7 +1558,7 @@ fn scale_shape( } } #[cfg(feature = "3d")] - match b.scaled(&scale.abs().into(), num_subdivisions) { + match b.scaled(scale.abs(), num_subdivisions) { None => { log::error!("Failed to apply scale {} to Ball shape.", scale); Ok(SharedShape::ball(0.0)) @@ -1608,25 +1567,25 @@ fn scale_shape( Some(Either::Right(b)) => Ok(SharedShape::new(b)), } } - TypedShape::Segment(s) => Ok(SharedShape::new(s.scaled(&scale.into()))), - TypedShape::Triangle(t) => Ok(SharedShape::new(t.scaled(&scale.into()))), + TypedShape::Segment(s) => Ok(SharedShape::new(s.scaled(scale))), + TypedShape::Triangle(t) => Ok(SharedShape::new(t.scaled(scale))), TypedShape::RoundTriangle(t) => Ok(SharedShape::new(RoundShape { border_radius: t.border_radius, - inner_shape: t.inner_shape.scaled(&scale.into()), + inner_shape: t.inner_shape.scaled(scale), })), - TypedShape::TriMesh(t) => Ok(SharedShape::new(t.clone().scaled(&scale.into()))), - TypedShape::Polyline(p) => Ok(SharedShape::new(p.clone().scaled(&scale.into()))), - TypedShape::HalfSpace(h) => match h.scaled(&scale.into()) { + TypedShape::TriMesh(t) => Ok(SharedShape::new(t.clone().scaled(scale))), + TypedShape::Polyline(p) => Ok(SharedShape::new(p.clone().scaled(scale))), + TypedShape::HalfSpace(h) => match h.scaled(scale) { None => { log::error!("Failed to apply scale {} to HalfSpace shape.", scale); Ok(SharedShape::ball(0.0)) } Some(scaled) => Ok(SharedShape::new(scaled)), }, - TypedShape::Voxels(v) => Ok(SharedShape::new(v.clone().scaled(&scale.into()))), - TypedShape::HeightField(h) => Ok(SharedShape::new(h.clone().scaled(&scale.into()))), + TypedShape::Voxels(v) => Ok(SharedShape::new(v.clone().scaled(scale))), + TypedShape::HeightField(h) => Ok(SharedShape::new(h.clone().scaled(scale))), #[cfg(feature = "2d")] - TypedShape::ConvexPolygon(cp) => match cp.clone().scaled(&scale.into()) { + TypedShape::ConvexPolygon(cp) => match cp.clone().scaled(scale) { None => { log::error!("Failed to apply scale {} to ConvexPolygon shape.", scale); Ok(SharedShape::ball(0.0)) @@ -1634,7 +1593,7 @@ fn scale_shape( Some(scaled) => Ok(SharedShape::new(scaled)), }, #[cfg(feature = "2d")] - TypedShape::RoundConvexPolygon(cp) => match cp.inner_shape.clone().scaled(&scale.into()) { + TypedShape::RoundConvexPolygon(cp) => match cp.inner_shape.clone().scaled(scale) { None => { log::error!( "Failed to apply scale {} to RoundConvexPolygon shape.", @@ -1648,7 +1607,7 @@ fn scale_shape( })), }, #[cfg(feature = "3d")] - TypedShape::ConvexPolyhedron(cp) => match cp.clone().scaled(&scale.into()) { + TypedShape::ConvexPolyhedron(cp) => match cp.clone().scaled(scale) { None => { log::error!("Failed to apply scale {} to ConvexPolyhedron shape.", scale); Ok(SharedShape::ball(0.0)) @@ -1656,23 +1615,21 @@ fn scale_shape( Some(scaled) => Ok(SharedShape::new(scaled)), }, #[cfg(feature = "3d")] - TypedShape::RoundConvexPolyhedron(cp) => { - match cp.clone().inner_shape.scaled(&scale.into()) { - None => { - log::error!( - "Failed to apply scale {} to RoundConvexPolyhedron shape.", - scale - ); - Ok(SharedShape::ball(0.0)) - } - Some(scaled) => Ok(SharedShape::new(RoundShape { - border_radius: cp.border_radius, - inner_shape: scaled, - })), + TypedShape::RoundConvexPolyhedron(cp) => match cp.clone().inner_shape.scaled(scale) { + None => { + log::error!( + "Failed to apply scale {} to RoundConvexPolyhedron shape.", + scale + ); + Ok(SharedShape::ball(0.0)) } - } + Some(scaled) => Ok(SharedShape::new(RoundShape { + border_radius: cp.border_radius, + inner_shape: scaled, + })), + }, #[cfg(feature = "3d")] - TypedShape::Cylinder(c) => match c.scaled(&scale.abs().into(), num_subdivisions) { + TypedShape::Cylinder(c) => match c.scaled(scale.abs(), num_subdivisions) { None => { log::error!("Failed to apply scale {} to Cylinder shape.", scale); Ok(SharedShape::ball(0.0)) @@ -1681,24 +1638,22 @@ fn scale_shape( Some(Either::Right(b)) => Ok(SharedShape::new(b)), }, #[cfg(feature = "3d")] - TypedShape::RoundCylinder(c) => { - match c.inner_shape.scaled(&scale.abs().into(), num_subdivisions) { - None => { - log::error!("Failed to apply scale {} to RoundCylinder shape.", scale); - Ok(SharedShape::ball(0.0)) - } - Some(Either::Left(scaled)) => Ok(SharedShape::new(RoundShape { - border_radius: c.border_radius, - inner_shape: scaled, - })), - Some(Either::Right(scaled)) => Ok(SharedShape::new(RoundShape { - border_radius: c.border_radius, - inner_shape: scaled, - })), + TypedShape::RoundCylinder(c) => match c.inner_shape.scaled(scale.abs(), num_subdivisions) { + None => { + log::error!("Failed to apply scale {} to RoundCylinder shape.", scale); + Ok(SharedShape::ball(0.0)) } - } + Some(Either::Left(scaled)) => Ok(SharedShape::new(RoundShape { + border_radius: c.border_radius, + inner_shape: scaled, + })), + Some(Either::Right(scaled)) => Ok(SharedShape::new(RoundShape { + border_radius: c.border_radius, + inner_shape: scaled, + })), + }, #[cfg(feature = "3d")] - TypedShape::Cone(c) => match c.scaled(&scale.into(), num_subdivisions) { + TypedShape::Cone(c) => match c.scaled(scale, num_subdivisions) { None => { log::error!("Failed to apply scale {} to Cone shape.", scale); Ok(SharedShape::ball(0.0)) @@ -1707,7 +1662,7 @@ fn scale_shape( Some(Either::Right(b)) => Ok(SharedShape::new(b)), }, #[cfg(feature = "3d")] - TypedShape::RoundCone(c) => match c.inner_shape.scaled(&scale.into(), num_subdivisions) { + TypedShape::RoundCone(c) => match c.inner_shape.scaled(scale, num_subdivisions) { None => { log::error!("Failed to apply scale {} to RoundCone shape.", scale); Ok(SharedShape::ball(0.0)) @@ -1724,20 +1679,20 @@ fn scale_shape( TypedShape::Compound(c) => { let mut scaled = Vec::with_capacity(c.shapes().len()); - for (iso, shape) in c.shapes() { + for (pose, shape) in c.shapes() { + #[cfg(feature = "2d")] scaled.push(( - #[cfg(feature = "2d")] - make_isometry( - Vector::from(iso.translation) * scale, - Rotation::radians(iso.rotation.angle()), - ), - #[cfg(feature = "3d")] - make_isometry( - Vector::from(iso.translation) * scale, - Quaternion::from(iso.rotation), + make_pose( + pose.translation * scale, + Rotation::radians(pose.rotation.angle()), ), scale_shape(shape, scale, num_subdivisions)?, )); + #[cfg(feature = "3d")] + scaled.push(( + make_pose(pose.translation * scale, pose.rotation), + scale_shape(shape, scale, num_subdivisions)?, + )); } Ok(SharedShape::compound(scaled)) } @@ -1761,7 +1716,7 @@ fn scale_shape( let vertices = polygon .vertices(0.0) .into_iter() - .map(|v| v.adjust_precision().into()) + .map(|v| v.adjust_precision()) .collect::>(); return scale_shape( diff --git a/src/collision/collider/parry/primitives2d.rs b/src/collision/collider/parry/primitives2d.rs index 2d565b576..fa0f23da7 100644 --- a/src/collision/collider/parry/primitives2d.rs +++ b/src/collision/collider/parry/primitives2d.rs @@ -3,10 +3,9 @@ use crate::{AdjustPrecision, FRAC_PI_2, PI, Scalar, TAU, Vector, math}; use super::{AsF32, Collider, IntoCollider}; use bevy::prelude::{Deref, DerefMut}; use bevy_math::{bounding::Bounded2d, prelude::*}; -use nalgebra::{Point2, UnitVector2, Vector2}; use parry::{ mass_properties::MassProperties, - math::Isometry, + math::Pose, query::{ PointQuery, RayCast, details::local_ray_intersection_with_support_map_with_params, gjk::VoronoiSimplex, point::local_point_projection_on_support_map, @@ -38,10 +37,10 @@ pub struct EllipseColliderShape(pub Ellipse); impl SupportMap for EllipseColliderShape { #[inline] - fn local_support_point(&self, direction: &Vector2) -> Point2 { + fn local_support_point(&self, direction: Vector) -> Vector { let [a, b] = self.half_size.adjust_precision().to_array(); let denom = (direction.x.powi(2) * a * a + direction.y.powi(2) * b * b).sqrt(); - Point2::new(a * a * direction.x / denom, b * b * direction.y / denom) + Vector::new(a * a * direction.x / denom, b * b * direction.y / denom) } } @@ -52,10 +51,10 @@ impl Shape for EllipseColliderShape { fn scale_dyn( &self, - scale: &parry::math::Vector, + scale: Vector, _num_subdivisions: u32, ) -> Option> { - let half_size = Vector::from(*scale).f32() * self.half_size; + let half_size = scale.f32() * self.half_size; Some(Box::new(EllipseColliderShape(Ellipse::new( half_size.x, half_size.y, @@ -64,37 +63,28 @@ impl Shape for EllipseColliderShape { fn compute_local_aabb(&self) -> parry::bounding_volume::Aabb { let aabb = self.aabb_2d(Isometry2d::IDENTITY); - parry::bounding_volume::Aabb::new( - aabb.min.adjust_precision().into(), - aabb.max.adjust_precision().into(), - ) + parry::bounding_volume::Aabb::new(aabb.min.adjust_precision(), aabb.max.adjust_precision()) } - fn compute_aabb(&self, position: &Isometry) -> parry::bounding_volume::Aabb { - let isometry = math::na_iso_to_iso(position); + fn compute_aabb(&self, position: &Pose) -> parry::bounding_volume::Aabb { + let isometry = math::pose_to_isometry(position); let aabb = self.aabb_2d(isometry); - parry::bounding_volume::Aabb::new( - aabb.min.adjust_precision().into(), - aabb.max.adjust_precision().into(), - ) + parry::bounding_volume::Aabb::new(aabb.min.adjust_precision(), aabb.max.adjust_precision()) } fn compute_local_bounding_sphere(&self) -> parry::bounding_volume::BoundingSphere { let sphere = self.bounding_circle(Isometry2d::IDENTITY); parry::bounding_volume::BoundingSphere::new( - sphere.center.adjust_precision().into(), + sphere.center.adjust_precision(), sphere.radius().adjust_precision(), ) } - fn compute_bounding_sphere( - &self, - position: &Isometry, - ) -> parry::bounding_volume::BoundingSphere { - let isometry = math::na_iso_to_iso(position); + fn compute_bounding_sphere(&self, position: &Pose) -> parry::bounding_volume::BoundingSphere { + let isometry = math::pose_to_isometry(position); let sphere = self.bounding_circle(isometry); parry::bounding_volume::BoundingSphere::new( - sphere.center.adjust_precision().into(), + sphere.center.adjust_precision(), sphere.radius().adjust_precision(), ) } @@ -107,7 +97,7 @@ impl Shape for EllipseColliderShape { let volume = self.area().adjust_precision(); let mass = volume * density; let inertia = mass * self.half_size.length_squared().adjust_precision() / 4.0; - MassProperties::new(Point2::origin(), mass, inertia) + MassProperties::new(Vector::ZERO, mass, inertia) } fn is_convex(&self) -> bool { @@ -153,17 +143,13 @@ impl RayCast for EllipseColliderShape { } impl PointQuery for EllipseColliderShape { - fn project_local_point( - &self, - pt: &parry::math::Point, - solid: bool, - ) -> parry::query::PointProjection { + fn project_local_point(&self, pt: Vector, solid: bool) -> parry::query::PointProjection { local_point_projection_on_support_map(self, &mut VoronoiSimplex::new(), pt, solid) } fn project_local_point_and_get_feature( &self, - pt: &parry::math::Point, + pt: Vector, ) -> (parry::query::PointProjection, parry::shape::FeatureId) { (self.project_local_point(pt, false), FeatureId::Unknown) } @@ -245,7 +231,7 @@ pub struct RegularPolygonColliderShape(pub RegularPolygon); impl SupportMap for RegularPolygonColliderShape { #[inline] - fn local_support_point(&self, direction: &Vector2) -> Point2 { + fn local_support_point(&self, direction: Vector) -> Vector { // TODO: For polygons with a small number of sides, maybe just iterating // through the vertices and comparing dot products is faster? @@ -254,9 +240,9 @@ impl SupportMap for RegularPolygonColliderShape { // Counterclockwise let angle_from_top = if direction.x < 0.0 { - -Vector::from(*direction).angle_to(Vector::Y) + -direction.angle_to(Vector::Y) } else { - TAU - Vector::from(*direction).angle_to(Vector::Y) + TAU - direction.angle_to(Vector::Y) }; // How many rotations of `external_angle` correspond to the vertex closest to the support direction. @@ -266,25 +252,21 @@ impl SupportMap for RegularPolygonColliderShape { let target_angle = n * external_angle + FRAC_PI_2; // Compute the vertex corresponding to the target angle on the unit circle. - Point2::from(circumradius * Vector::from_angle(target_angle)) + circumradius * Vector::from_angle(target_angle) } } impl PolygonalFeatureMap for RegularPolygonColliderShape { #[inline] - fn local_support_feature( - &self, - direction: &UnitVector2, - out_feature: &mut PolygonalFeature, - ) { + fn local_support_feature(&self, direction: Vector, out_feature: &mut PolygonalFeature) { let external_angle = self.external_angle_radians().adjust_precision(); let circumradius = self.circumradius().adjust_precision(); // Counterclockwise let angle_from_top = if direction.x < 0.0 { - -Vector::from(*direction).angle_to(Vector::Y) + -direction.angle_to(Vector::Y) } else { - TAU - Vector::from(*direction).angle_to(Vector::Y) + TAU - direction.angle_to(Vector::Y) }; // How many rotations of `external_angle` correspond to the vertices. @@ -297,8 +279,8 @@ impl PolygonalFeatureMap for RegularPolygonColliderShape { let target_angle2 = n2 * external_angle + FRAC_PI_2; // Compute the vertices corresponding to the target angle on the unit circle. - let vertex1 = Point2::from(circumradius * Vector::from_angle(target_angle1)); - let vertex2 = Point2::from(circumradius * Vector::from_angle(target_angle2)); + let vertex1 = circumradius * Vector::from_angle(target_angle1); + let vertex2 = circumradius * Vector::from_angle(target_angle2); *out_feature = PolygonalFeature { vertices: [vertex1, vertex2], @@ -319,10 +301,10 @@ impl Shape for RegularPolygonColliderShape { fn scale_dyn( &self, - scale: &parry::math::Vector, + scale: Vector, _num_subdivisions: u32, ) -> Option> { - let circumradius = Vector::from(*scale).f32() * self.circumradius(); + let circumradius = scale.f32() * self.circumradius(); Some(Box::new(RegularPolygonColliderShape(RegularPolygon::new( circumradius.length(), self.sides, @@ -331,37 +313,28 @@ impl Shape for RegularPolygonColliderShape { fn compute_local_aabb(&self) -> parry::bounding_volume::Aabb { let aabb = self.aabb_2d(Isometry2d::IDENTITY); - parry::bounding_volume::Aabb::new( - aabb.min.adjust_precision().into(), - aabb.max.adjust_precision().into(), - ) + parry::bounding_volume::Aabb::new(aabb.min.adjust_precision(), aabb.max.adjust_precision()) } - fn compute_aabb(&self, position: &Isometry) -> parry::bounding_volume::Aabb { - let isometry = math::na_iso_to_iso(position); + fn compute_aabb(&self, position: &Pose) -> parry::bounding_volume::Aabb { + let isometry = math::pose_to_isometry(position); let aabb = self.aabb_2d(isometry); - parry::bounding_volume::Aabb::new( - aabb.min.adjust_precision().into(), - aabb.max.adjust_precision().into(), - ) + parry::bounding_volume::Aabb::new(aabb.min.adjust_precision(), aabb.max.adjust_precision()) } fn compute_local_bounding_sphere(&self) -> parry::bounding_volume::BoundingSphere { let sphere = self.bounding_circle(Isometry2d::IDENTITY); parry::bounding_volume::BoundingSphere::new( - sphere.center.adjust_precision().into(), + sphere.center.adjust_precision(), sphere.radius().adjust_precision(), ) } - fn compute_bounding_sphere( - &self, - position: &Isometry, - ) -> parry::bounding_volume::BoundingSphere { - let isometry = math::na_iso_to_iso(position); + fn compute_bounding_sphere(&self, position: &Pose) -> parry::bounding_volume::BoundingSphere { + let isometry = math::pose_to_isometry(position); let sphere = self.bounding_circle(isometry); parry::bounding_volume::BoundingSphere::new( - sphere.center.adjust_precision().into(), + sphere.center.adjust_precision(), sphere.radius().adjust_precision(), ) } @@ -378,7 +351,7 @@ impl Shape for RegularPolygonColliderShape { let angular_inertia = mass * self.circumradius().adjust_precision().powi(2) / 6.0 * (1.0 + 2.0 * half_external_angle.cos().powi(2)); - MassProperties::new(Point2::origin(), mass, angular_inertia) + MassProperties::new(Vector::ZERO, mass, angular_inertia) } fn is_convex(&self) -> bool { @@ -409,25 +382,17 @@ impl Shape for RegularPolygonColliderShape { Some((self as &dyn PolygonalFeatureMap, 0.0)) } - fn feature_normal_at_point( - &self, - feature: FeatureId, - _point: &Point2, - ) -> Option> { + fn feature_normal_at_point(&self, feature: FeatureId, _point: Vector) -> Option { match feature { FeatureId::Face(id) => { let external_angle = self.external_angle_radians().adjust_precision(); let normal_angle = id as Scalar * external_angle - external_angle * 0.5 + FRAC_PI_2; - Some(UnitVector2::new_unchecked( - Vector::from_angle(normal_angle).into(), - )) + Some(Vector::from_angle(normal_angle)) } FeatureId::Vertex(id) => { let external_angle = self.external_angle_radians().adjust_precision(); let normal_angle = id as Scalar * external_angle + FRAC_PI_2; - Some(UnitVector2::new_unchecked( - Vector::from_angle(normal_angle).into(), - )) + Some(Vector::from_angle(normal_angle)) } _ => None, } @@ -452,17 +417,13 @@ impl RayCast for RegularPolygonColliderShape { } impl PointQuery for RegularPolygonColliderShape { - fn project_local_point( - &self, - pt: &parry::math::Point, - solid: bool, - ) -> parry::query::PointProjection { + fn project_local_point(&self, pt: Vector, solid: bool) -> parry::query::PointProjection { local_point_projection_on_support_map(self, &mut VoronoiSimplex::new(), pt, solid) } fn project_local_point_and_get_feature( &self, - pt: &parry::math::Point, + pt: Vector, ) -> (parry::query::PointProjection, parry::shape::FeatureId) { (self.project_local_point(pt, false), FeatureId::Unknown) } diff --git a/src/collision/collider/trimesh_builder.rs b/src/collision/collider/trimesh_builder.rs index a1f458e5f..5e4a7cae5 100644 --- a/src/collision/collider/trimesh_builder.rs +++ b/src/collision/collider/trimesh_builder.rs @@ -259,14 +259,10 @@ impl TrimeshBuilder { Trimesh::default(), move |mut compound_trimesh, (sub_pos, shape)| { sub_builder.shape = shape.clone(); - sub_builder.position = Position( - self.position.0 + self.rotation * Vector::from(sub_pos.translation), - ); - sub_builder.rotation = self - .rotation - .mul_quat(sub_pos.rotation.into()) - .normalize() - .into(); + sub_builder.position = + Position(self.position.0 + self.rotation * sub_pos.translation); + sub_builder.rotation = + self.rotation.mul_quat(sub_pos.rotation).normalize().into(); let trimesh = match sub_builder.build() { Ok(trimesh) => trimesh, Err(error) => { @@ -326,7 +322,7 @@ impl TrimeshBuilder { Ok(Trimesh { vertices: vertices .into_iter() - .map(|v| pos.0 + Vector::from(self.rotation * Vector::from(v))) + .map(|v| pos.0 + self.rotation * v) .collect(), indices, }) diff --git a/src/debug_render/gizmos.rs b/src/debug_render/gizmos.rs index 0003f11c7..c29aeab5e 100644 --- a/src/debug_render/gizmos.rs +++ b/src/debug_render/gizmos.rs @@ -169,8 +169,6 @@ impl PhysicsGizmoExt for Gizmos<'_, '_, PhysicsGizmos> { let position: Position = position.into(); let rotation: Rotation = rotation.into(); - let nalgebra_to_glam = - |points: &[_]| points.iter().map(|p| Vector::from(*p)).collect::>(); match collider.shape_scaled().as_typed_shape() { #[cfg(feature = "2d")] TypedShape::Ball(s) => { @@ -191,7 +189,7 @@ impl PhysicsGizmoExt for Gizmos<'_, '_, PhysicsGizmos> { position.f32(), Rot2::from_sin_cos(rotation.sin as f32, rotation.cos as f32), ), - 2.0 * Vector::from(s.half_extents).f32(), + 2.0 * s.half_extents.f32(), color, ); } @@ -200,46 +198,26 @@ impl PhysicsGizmoExt for Gizmos<'_, '_, PhysicsGizmos> { use bevy_math::bounding::Aabb3d; self.aabb_3d( - Aabb3d::new(Vec3A::ZERO, Vector::from(s.half_extents).f32()), + Aabb3d::new(Vec3A::ZERO, s.half_extents.f32()), Transform::from_translation(position.f32()).with_rotation(rotation.f32()), color, ); } #[cfg(feature = "2d")] TypedShape::Capsule(s) => { - self.draw_line_strip( - nalgebra_to_glam(&s.to_polyline(32)), - position, - rotation, - true, - color, - ); + self.draw_line_strip(s.to_polyline(32), position, rotation, true, color); } #[cfg(feature = "3d")] TypedShape::Capsule(s) => { let (vertices, indices) = s.to_outline(32); - self.draw_polyline( - &nalgebra_to_glam(&vertices), - &indices, - position, - rotation, - color, - ); + self.draw_polyline(&vertices, &indices, position, rotation, color); + } + TypedShape::Segment(s) => { + self.draw_line_strip(vec![s.a, s.b], position, rotation, false, color) + } + TypedShape::Triangle(s) => { + self.draw_line_strip(vec![s.a, s.b, s.c], position, rotation, true, color) } - TypedShape::Segment(s) => self.draw_line_strip( - vec![s.a.into(), s.b.into()], - position, - rotation, - false, - color, - ), - TypedShape::Triangle(s) => self.draw_line_strip( - vec![s.a.into(), s.b.into(), s.c.into()], - position, - rotation, - true, - color, - ), TypedShape::TriMesh(s) => { for tri in s.triangles() { self.draw_collider( @@ -250,13 +228,9 @@ impl PhysicsGizmoExt for Gizmos<'_, '_, PhysicsGizmos> { ); } } - TypedShape::Polyline(s) => self.draw_polyline( - &nalgebra_to_glam(s.vertices()), - s.indices(), - position, - rotation, - color, - ), + TypedShape::Polyline(s) => { + self.draw_polyline(s.vertices(), s.indices(), position, rotation, color) + } #[cfg(feature = "2d")] TypedShape::HalfSpace(s) => { let basis = Vector::new(-s.normal.y, s.normal.x); @@ -283,13 +257,7 @@ impl PhysicsGizmoExt for Gizmos<'_, '_, PhysicsGizmos> { let (vertices, indices) = v.to_polyline(); #[cfg(feature = "3d")] let (vertices, indices) = v.to_outline(); - self.draw_polyline( - &nalgebra_to_glam(&vertices), - &indices, - position, - rotation, - color, - ); + self.draw_polyline(&vertices, &indices, position, rotation, color); } TypedShape::HeightField(s) => { #[cfg(feature = "2d")] @@ -313,84 +281,48 @@ impl PhysicsGizmoExt for Gizmos<'_, '_, PhysicsGizmos> { } TypedShape::Compound(s) => { for (sub_pos, shape) in s.shapes() { - let pos = Position(position.0 + rotation * Vector::from(sub_pos.translation)); + let pos = Position(position.0 + rotation * sub_pos.translation); #[cfg(feature = "2d")] let rot = rotation * Rotation::radians(sub_pos.rotation.angle()); #[cfg(feature = "3d")] - let rot = Rotation((rotation.mul_quat(sub_pos.rotation.into())).normalize()); + let rot = Rotation((rotation.mul_quat(sub_pos.rotation)).normalize()); self.draw_collider(&Collider::from(shape.to_owned()), pos, rot, color); } } #[cfg(feature = "2d")] TypedShape::ConvexPolygon(s) => { - self.draw_line_strip( - nalgebra_to_glam(s.points()), - position, - rotation, - true, - color, - ); + self.draw_line_strip(s.points().to_vec(), position, rotation, true, color); } #[cfg(feature = "3d")] TypedShape::ConvexPolyhedron(s) => { let indices = s .edges() .iter() - .map(|e| [e.vertices.x, e.vertices.y]) + .map(|e| [e.vertices[0], e.vertices[1]]) .collect::>(); - self.draw_polyline( - &nalgebra_to_glam(s.points()), - &indices, - position, - rotation, - color, - ); + self.draw_polyline(s.points(), &indices, position, rotation, color); } #[cfg(feature = "3d")] TypedShape::Cylinder(s) => { let (vertices, indices) = s.to_outline(32); - self.draw_polyline( - &nalgebra_to_glam(&vertices), - &indices, - position, - rotation, - color, - ); + self.draw_polyline(&vertices, &indices, position, rotation, color); } #[cfg(feature = "3d")] TypedShape::Cone(s) => { let (vertices, indices) = s.to_outline(32); - self.draw_polyline( - &nalgebra_to_glam(&vertices), - &indices, - position, - rotation, - color, - ); + self.draw_polyline(&vertices, &indices, position, rotation, color); } // ------------ // Round shapes // ------------ #[cfg(feature = "2d")] TypedShape::RoundCuboid(s) => { - self.draw_line_strip( - nalgebra_to_glam(&s.to_polyline(32)), - position, - rotation, - true, - color, - ); + self.draw_line_strip(s.to_polyline(32), position, rotation, true, color); } #[cfg(feature = "3d")] TypedShape::RoundCuboid(s) => { let (vertices, indices) = s.to_outline(32); - self.draw_polyline( - &nalgebra_to_glam(&vertices), - &indices, - position, - rotation, - color, - ); + self.draw_polyline(&vertices, &indices, position, rotation, color); } TypedShape::RoundTriangle(s) => { // Parry doesn't have a method for the rounded outline, so we have to just use a normal triangle @@ -404,46 +336,22 @@ impl PhysicsGizmoExt for Gizmos<'_, '_, PhysicsGizmos> { } #[cfg(feature = "2d")] TypedShape::RoundConvexPolygon(s) => { - self.draw_line_strip( - nalgebra_to_glam(&s.to_polyline(32)), - position, - rotation, - true, - color, - ); + self.draw_line_strip(s.to_polyline(32), position, rotation, true, color); } #[cfg(feature = "3d")] TypedShape::RoundConvexPolyhedron(s) => { let (vertices, indices) = s.to_outline(32); - self.draw_polyline( - &nalgebra_to_glam(&vertices), - &indices, - position, - rotation, - color, - ); + self.draw_polyline(&vertices, &indices, position, rotation, color); } #[cfg(feature = "3d")] TypedShape::RoundCylinder(s) => { let (vertices, indices) = s.to_outline(32, 32); - self.draw_polyline( - &nalgebra_to_glam(&vertices), - &indices, - position, - rotation, - color, - ); + self.draw_polyline(&vertices, &indices, position, rotation, color); } #[cfg(feature = "3d")] TypedShape::RoundCone(s) => { let (vertices, indices) = s.to_outline(32, 32); - self.draw_polyline( - &nalgebra_to_glam(&vertices), - &indices, - position, - rotation, - color, - ); + self.draw_polyline(&vertices, &indices, position, rotation, color); } TypedShape::Custom(_id) => { #[cfg(feature = "2d")] diff --git a/src/dynamics/ccd/mod.rs b/src/dynamics/ccd/mod.rs index 463969d77..679c06c7d 100644 --- a/src/dynamics/ccd/mod.rs +++ b/src/dynamics/ccd/mod.rs @@ -601,8 +601,8 @@ fn solve_swept_ccd( continue; } - let iso1 = make_isometry(prev_pos1, prev_rot1); - let iso2 = make_isometry(prev_pos2, prev_rot2); + let iso1 = make_pose(prev_pos1, prev_rot1); + let iso2 = make_pose(prev_pos2, prev_rot2); // TODO: Support child colliders let motion1 = NonlinearRigidMotion::new( @@ -701,10 +701,10 @@ fn compute_ccd_toi( if mode == SweepMode::Linear { let hit = cast_shapes( &motion1.start, - &motion1.linvel, + motion1.linvel, collider1.shape_scaled().as_ref(), &motion2.start, - &motion2.linvel, + motion2.linvel, collider2.shape_scaled().as_ref(), ShapeCastOptions { max_time_of_impact: min_toi, @@ -722,10 +722,10 @@ fn compute_ccd_toi( // around the centroid of the first body. let hit = cast_shapes( &motion1.start, - &motion1.linvel, + motion1.linvel, collider1.shape_scaled().as_ref(), &motion2.start, - &motion2.linvel, + motion2.linvel, &parry::shape::Ball::new(prediction_distance), ShapeCastOptions { max_time_of_impact: min_toi, diff --git a/src/math/mod.rs b/src/math/mod.rs index 4e32a3121..e585786a0 100644 --- a/src/math/mod.rs +++ b/src/math/mod.rs @@ -578,11 +578,9 @@ impl MatExt for SymmetricDMat3 { #[allow(clippy::unnecessary_cast)] #[cfg(all(feature = "2d", any(feature = "parry-f32", feature = "parry-f64")))] -pub(crate) fn na_iso_to_iso(isometry: &parry::math::Isometry) -> Isometry2d { - Isometry2d::new( - Vector::from(isometry.translation).f32(), - Rot2::from_sin_cos(isometry.rotation.im as f32, isometry.rotation.re as f32), - ) +pub(crate) fn pose_to_isometry(pose: &parry::math::Pose) -> Isometry2d { + let rotation = Rot2::from_sin_cos(pose.rotation.im as f32, pose.rotation.re as f32); + Isometry2d::new(pose.translation.f32(), rotation) } #[cfg(all( @@ -596,13 +594,13 @@ use crate::prelude::*; feature = "default-collider", any(feature = "parry-f32", feature = "parry-f64") ))] -pub(crate) fn make_isometry( +pub(crate) fn make_pose( position: impl Into, rotation: impl Into, -) -> parry::math::Isometry { +) -> parry::math::Pose2 { let position: Position = position.into(); let rotation: Rotation = rotation.into(); - parry::math::Isometry::::new(position.0.into(), rotation.into()) + parry::math::Pose2::new(position.0, rotation.as_radians()) } #[cfg(all( @@ -610,13 +608,13 @@ pub(crate) fn make_isometry( feature = "default-collider", any(feature = "parry-f32", feature = "parry-f64") ))] -pub(crate) fn make_isometry( +pub(crate) fn make_pose( position: impl Into, rotation: impl Into, -) -> parry::math::Isometry { +) -> parry::math::Pose3 { let position: Position = position.into(); let rotation: Rotation = rotation.into(); - parry::math::Isometry::::new(position.0.into(), rotation.to_scaled_axis().into()) + parry::math::Pose3::new(position.0, rotation.to_scaled_axis()) } /// Computes the skew-symmetric matrix corresponding to the given vector. diff --git a/src/spatial_query/pipeline.rs b/src/spatial_query/pipeline.rs index ccf384834..9f8b3ad56 100644 --- a/src/spatial_query/pipeline.rs +++ b/src/spatial_query/pipeline.rs @@ -4,7 +4,7 @@ use crate::prelude::*; use bevy::prelude::*; use parry::{ bounding_volume::{Aabb, BoundingVolume}, - math::Isometry, + math::Pose, partitioning::{Bvh, BvhBuildStrategy, BvhNode}, query::{ DefaultQueryDispatcher, QueryDispatcher, RayCast, ShapeCastOptions, @@ -18,7 +18,7 @@ use parry::{ #[derive(Clone)] pub(crate) struct BvhProxyData { pub entity: Entity, - pub isometry: Isometry, + pub pose: Pose, pub collider: Collider, pub layers: CollisionLayers, } @@ -109,7 +109,7 @@ impl SpatialQueryPipeline { colliders.map( |(entity, position, rotation, collider, layers)| BvhProxyData { entity, - isometry: make_isometry(position.0, *rotation), + pose: make_pose(position.0, *rotation), collider: collider.clone(), layers: *layers, }, @@ -122,12 +122,11 @@ impl SpatialQueryPipeline { self.proxies.clear(); self.proxies.extend(proxies); - let aabbs = self.proxies.iter().enumerate().map(|(i, proxy)| { - ( - i, - proxy.collider.shape_scaled().compute_aabb(&proxy.isometry), - ) - }); + let aabbs = self + .proxies + .iter() + .enumerate() + .map(|(i, proxy)| (i, proxy.collider.shape_scaled().compute_aabb(&proxy.pose))); self.bvh = Bvh::from_iter(BvhBuildStrategy::Binned, aabbs); } @@ -199,14 +198,14 @@ impl SpatialQueryPipeline { ) -> Option { let composite = self.as_composite_shape_with_predicate(filter, predicate); let pipeline_shape = CompositeShapeRef(&composite); - let ray = parry::query::Ray::new(origin.into(), direction.adjust_precision().into()); + let ray = parry::query::Ray::new(origin, direction.adjust_precision()); pipeline_shape .cast_local_ray_and_get_normal(&ray, max_distance, solid) .map(|(index, hit)| RayHitData { entity: self.proxies[index as usize].entity, distance: hit.time_of_impact, - normal: hit.normal.into(), + normal: hit.normal, }) } @@ -279,7 +278,7 @@ impl SpatialQueryPipeline { ) { let proxies = &self.proxies; - let ray = parry::query::Ray::new(origin.into(), direction.adjust_precision().into()); + let ray = parry::query::Ray::new(origin, direction.adjust_precision()); let hits = self .bvh @@ -292,7 +291,7 @@ impl SpatialQueryPipeline { } let hit = proxy.collider.shape_scaled().cast_ray_and_get_normal( - &proxy.isometry, + &proxy.pose, &ray, max_distance, solid, @@ -301,7 +300,7 @@ impl SpatialQueryPipeline { Some(RayHitData { entity: proxy.entity, distance: hit.time_of_impact, - normal: hit.normal.into(), + normal: hit.normal, }) }); @@ -393,8 +392,8 @@ impl SpatialQueryPipeline { rotation = Rotation::from(shape_rotation); } - let shape_isometry = make_isometry(origin, rotation); - let shape_direction = direction.adjust_precision().into(); + let shape_isometry = make_pose(origin, rotation); + let shape_direction = direction.adjust_precision(); let composite = self.as_composite_shape_with_predicate(filter, predicate); let pipeline_shape = CompositeShapeRef(&composite); @@ -402,7 +401,7 @@ impl SpatialQueryPipeline { .cast_shape( self.dispatcher.as_ref(), &shape_isometry, - &shape_direction, + shape_direction, shape.shape_scaled().as_ref(), ShapeCastOptions { max_time_of_impact: config.max_distance, @@ -414,10 +413,10 @@ impl SpatialQueryPipeline { .map(|(index, hit)| ShapeHitData { entity: self.proxies[index as usize].entity, distance: hit.time_of_impact, - point1: hit.witness1.into(), - point2: hit.witness2.into(), - normal1: hit.normal1.into(), - normal2: hit.normal2.into(), + point1: hit.witness1, + point2: hit.witness2, + normal1: hit.normal1, + normal2: hit.normal2, }) } @@ -518,8 +517,8 @@ impl SpatialQueryPipeline { rotation = Rotation::from(shape_rotation); } - let shape_isometry = make_isometry(origin, rotation); - let shape_direction = direction.adjust_precision().into(); + let shape_isometry = make_pose(origin, rotation); + let shape_direction = direction.adjust_precision(); loop { let composite = self.as_composite_shape_internal(&query_filter); @@ -529,17 +528,17 @@ impl SpatialQueryPipeline { .cast_shape( self.dispatcher.as_ref(), &shape_isometry, - &shape_direction, + shape_direction, shape.shape_scaled().as_ref(), shape_cast_options, ) .map(|(index, hit)| ShapeHitData { entity: self.proxies[index as usize].entity, distance: hit.time_of_impact, - point1: hit.witness1.into(), - point2: hit.witness2.into(), - normal1: hit.normal1.into(), - normal2: hit.normal2.into(), + point1: hit.witness1, + point2: hit.witness2, + normal1: hit.normal1, + normal2: hit.normal2, }); if let Some(hit) = hit { @@ -601,15 +600,14 @@ impl SpatialQueryPipeline { return None; } - let point = point.into(); let composite = self.as_composite_shape_with_predicate(filter, predicate); let pipeline_shape = CompositeShapeRef(&composite); - let (index, projection) = pipeline_shape.project_local_point(&point, solid); + let (index, projection) = pipeline_shape.project_local_point(point, solid); Some(PointProjection { entity: self.proxies[index as usize].entity, - point: projection.point.into(), + point: projection.point, is_inside: projection.is_inside, }) } @@ -655,11 +653,9 @@ impl SpatialQueryPipeline { filter: &SpatialQueryFilter, mut callback: impl FnMut(Entity) -> bool, ) { - let point = point.into(); - let intersecting_entities = self .bvh - .leaves(|node: &BvhNode| node.aabb().contains_local_point(&point)) + .leaves(|node: &BvhNode| node.aabb().contains_local_point(point)) .filter_map(move |leaf| { let proxy = self.proxies.get(leaf as usize)?; @@ -667,7 +663,7 @@ impl SpatialQueryPipeline { && proxy .collider .shape_scaled() - .contains_point(&proxy.isometry, &point) + .contains_point(&proxy.pose, point) { Some(proxy.entity) } else { @@ -712,8 +708,8 @@ impl SpatialQueryPipeline { mut callback: impl FnMut(Entity) -> bool, ) { let aabb = Aabb { - mins: aabb.min.into(), - maxs: aabb.max.into(), + mins: aabb.min, + maxs: aabb.max, }; let intersecting_entities = self @@ -790,7 +786,7 @@ impl SpatialQueryPipeline { rotation = Rotation::from(shape_rotation); } - let shape_isometry = make_isometry(shape_position, rotation); + let shape_isometry = make_pose(shape_position, rotation); let inverse_shape_isometry = shape_isometry.inverse(); let dispatcher = &*self.dispatcher; @@ -806,7 +802,7 @@ impl SpatialQueryPipeline { return None; } - let isometry = inverse_shape_isometry * proxy.isometry; + let isometry = inverse_shape_isometry * proxy.pose; let intersects = dispatcher .intersection_test( &isometry, @@ -835,7 +831,7 @@ impl CompositeShape for QueryPipelineAsCompositeShape<'_> { fn map_part_at( &self, shape_id: u32, - f: &mut dyn FnMut(Option<&Isometry>, &dyn Shape, Option<&dyn NormalConstraints>), + f: &mut dyn FnMut(Option<&Pose>, &dyn Shape, Option<&dyn NormalConstraints>), ) { self.map_untyped_part_at(shape_id, f); } @@ -852,17 +848,13 @@ impl TypedCompositeShape for QueryPipelineAsCompositeShape<'_> { fn map_typed_part_at( &self, shape_id: u32, - mut f: impl FnMut( - Option<&Isometry>, - &Self::PartShape, - Option<&Self::PartNormalConstraints>, - ) -> T, + mut f: impl FnMut(Option<&Pose>, &Self::PartShape, Option<&Self::PartNormalConstraints>) -> T, ) -> Option { let proxy = self.pipeline.proxies.get(shape_id as usize)?; if self.query_filter.test(proxy.entity, proxy.layers) { Some(f( - Some(&proxy.isometry), + Some(&proxy.pose), proxy.collider.shape_scaled().as_ref(), None, )) @@ -874,13 +866,13 @@ impl TypedCompositeShape for QueryPipelineAsCompositeShape<'_> { fn map_untyped_part_at( &self, shape_id: u32, - mut f: impl FnMut(Option<&Isometry>, &dyn Shape, Option<&dyn NormalConstraints>) -> T, + mut f: impl FnMut(Option<&Pose>, &dyn Shape, Option<&dyn NormalConstraints>) -> T, ) -> Option { let proxy = self.pipeline.proxies.get(shape_id as usize)?; if self.query_filter.test(proxy.entity, proxy.layers) { Some(f( - Some(&proxy.isometry), + Some(&proxy.pose), proxy.collider.shape_scaled().as_ref(), None, )) @@ -900,7 +892,7 @@ impl CompositeShape for QueryPipelineAsCompositeShapeWithPredicate<'_, '_> { fn map_part_at( &self, shape_id: u32, - f: &mut dyn FnMut(Option<&Isometry>, &dyn Shape, Option<&dyn NormalConstraints>), + f: &mut dyn FnMut(Option<&Pose>, &dyn Shape, Option<&dyn NormalConstraints>), ) { self.map_untyped_part_at(shape_id, f); } @@ -917,18 +909,14 @@ impl TypedCompositeShape for QueryPipelineAsCompositeShapeWithPredicate<'_, '_> fn map_typed_part_at( &self, shape_id: u32, - mut f: impl FnMut( - Option<&Isometry>, - &Self::PartShape, - Option<&Self::PartNormalConstraints>, - ) -> T, + mut f: impl FnMut(Option<&Pose>, &Self::PartShape, Option<&Self::PartNormalConstraints>) -> T, ) -> Option { if let Some(proxy) = self.pipeline.proxies.get(shape_id as usize) && self.query_filter.test(proxy.entity, proxy.layers) && (self.predicate)(proxy.entity) { Some(f( - Some(&proxy.isometry), + Some(&proxy.pose), proxy.collider.shape_scaled().as_ref(), None, )) @@ -940,14 +928,14 @@ impl TypedCompositeShape for QueryPipelineAsCompositeShapeWithPredicate<'_, '_> fn map_untyped_part_at( &self, shape_id: u32, - mut f: impl FnMut(Option<&Isometry>, &dyn Shape, Option<&dyn NormalConstraints>) -> T, + mut f: impl FnMut(Option<&Pose>, &dyn Shape, Option<&dyn NormalConstraints>) -> T, ) -> Option { if let Some(proxy) = self.pipeline.proxies.get(shape_id as usize) && self.query_filter.test(proxy.entity, proxy.layers) && (self.predicate)(proxy.entity) { Some(f( - Some(&proxy.isometry), + Some(&proxy.pose), proxy.collider.shape_scaled().as_ref(), None, )) diff --git a/src/spatial_query/ray_caster.rs b/src/spatial_query/ray_caster.rs index b949dc388..6f2b03d7d 100644 --- a/src/spatial_query/ray_caster.rs +++ b/src/spatial_query/ray_caster.rs @@ -275,8 +275,8 @@ impl RayCaster { } } else { let ray = parry::query::Ray::new( - self.global_origin().into(), - self.global_direction().adjust_precision().into(), + self.global_origin(), + self.global_direction().adjust_precision(), ); let found_hits = query_pipeline @@ -290,7 +290,7 @@ impl RayCaster { } let hit = proxy.collider.shape_scaled().cast_ray_and_get_normal( - &proxy.isometry, + &proxy.pose, &ray, self.max_distance, self.solid, @@ -299,7 +299,7 @@ impl RayCaster { Some(RayHitData { entity: proxy.entity, distance: hit.time_of_impact, - normal: hit.normal.into(), + normal: hit.normal, }) }) .take(self.max_hits as usize); diff --git a/src/spatial_query/shape_caster.rs b/src/spatial_query/shape_caster.rs index 78a753d66..90bc0c461 100644 --- a/src/spatial_query/shape_caster.rs +++ b/src/spatial_query/shape_caster.rs @@ -366,8 +366,8 @@ impl ShapeCaster { shape_rotation = Rotation::from(self.global_shape_rotation()); } - let shape_isometry = make_isometry(self.global_origin(), shape_rotation); - let shape_direction = self.global_direction().adjust_precision().into(); + let shape_isometry = make_pose(self.global_origin(), shape_rotation); + let shape_direction = self.global_direction().adjust_precision(); while hits.len() < self.max_hits as usize { let composite = query_pipeline.as_composite_shape_internal(&query_filter); @@ -377,17 +377,17 @@ impl ShapeCaster { .cast_shape( query_pipeline.dispatcher.as_ref(), &shape_isometry, - &shape_direction, + shape_direction, self.shape.shape_scaled().as_ref(), shape_cast_options, ) .map(|(index, hit)| ShapeHitData { entity: query_pipeline.proxies[index as usize].entity, distance: hit.time_of_impact, - point1: hit.witness1.into(), - point2: hit.witness2.into(), - normal1: hit.normal1.into(), - normal2: hit.normal2.into(), + point1: hit.witness1, + point2: hit.witness2, + normal1: hit.normal1, + normal2: hit.normal2, }); if let Some(hit) = hit { diff --git a/src/tests/determinism_2d.rs b/src/tests/determinism_2d.rs index 70d27393e..bbc022c24 100644 --- a/src/tests/determinism_2d.rs +++ b/src/tests/determinism_2d.rs @@ -60,7 +60,7 @@ fn cross_platform_determinism_2d() { let hash = compute_hash(app.world(), query); // Update this value if simulation behavior changes. - let expected = 0x2696dd94; + let expected = 0xfcddc55; assert!( hash == expected,