From 754a48b7ff6d8c58b1ee08651e60112900b60455 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?S=C3=A9bastien=20Crozet?= Date: Tue, 25 Aug 2020 22:10:25 +0200 Subject: [PATCH] First public release of Rapier. --- .github/FUNDING.yml | 12 + .gitignore | 8 + Cargo.toml | 12 + LICENSE | 201 ++ README.md | 30 + build/rapier2d/Cargo.toml | 53 + build/rapier3d/Cargo.toml | 53 + build/rapier_testbed2d/Cargo.toml | 45 + build/rapier_testbed3d/Cargo.toml | 47 + examples2d/Cargo.toml | 27 + examples2d/all_examples2.rs | 90 + examples2d/balls2.rs | 68 + examples2d/boxes2.rs | 73 + examples2d/capsules2.rs | 76 + examples2d/debug_box_ball2.rs | 44 + examples2d/heightfield2.rs | 72 + examples2d/joints2.rs | 75 + examples2d/kinematic2.rs | 93 + examples2d/pyramid2.rs | 60 + examples2d/sensor2.rs | 101 + examples2d/stress_joint_ball2.rs | 72 + examples2d/stress_joint_fixed2.rs | 85 + examples2d/stress_joint_prismatic2.rs | 69 + examples3d/Cargo.toml | 27 + examples3d/all_examples3.rs | 110 ++ examples3d/balls3.rs | 29 + examples3d/boxes3.rs | 68 + examples3d/capsules3.rs | 69 + examples3d/debug_boxes3.rs | 47 + examples3d/debug_triangle3.rs | 48 + examples3d/domino3.rs | 87 + examples3d/heightfield3.rs | 78 + examples3d/joints3.rs | 272 +++ examples3d/kinematic3.rs | 97 + examples3d/pyramid3.rs | 85 + examples3d/sensor3.rs | 105 ++ examples3d/stacks3.rs | 195 ++ examples3d/stress_joint_ball3.rs | 70 + examples3d/stress_joint_fixed3.rs | 92 + examples3d/stress_joint_prismatic3.rs | 81 + examples3d/stress_joint_revolute3.rs | 89 + examples3d/stress_keva3.rs | 148 ++ examples3d/trimesh3.rs | 88 + src/counters/ccd_counters.rs | 49 + src/counters/collision_detection_counters.rs | 32 + src/counters/mod.rs | 225 +++ src/counters/solver_counters.rs | 67 + src/counters/stages_counters.rs | 48 + src/counters/timer.rs | 53 + src/data/arena.rs | 1159 ++++++++++++ src/data/graph.rs | 830 +++++++++ src/data/mod.rs | 4 + src/dynamics/integration_parameters.rs | 207 +++ src/dynamics/joint/ball_joint.rs | 34 + src/dynamics/joint/fixed_joint.rs | 33 + src/dynamics/joint/joint.rs | 112 ++ src/dynamics/joint/joint_set.rs | 218 +++ src/dynamics/joint/mod.rs | 16 + src/dynamics/joint/prismatic_joint.rs | 193 ++ src/dynamics/joint/revolute_joint.rs | 46 + src/dynamics/mass_properties.rs | 206 ++ src/dynamics/mass_properties_ball.rs | 30 + src/dynamics/mass_properties_capsule.rs | 60 + src/dynamics/mass_properties_cuboid.rs | 33 + src/dynamics/mass_properties_polygon.rs | 144 ++ src/dynamics/mod.rs | 30 + src/dynamics/rigid_body.rs | 441 +++++ src/dynamics/rigid_body_set.rs | 518 ++++++ src/dynamics/solver/categorization.rs | 70 + src/dynamics/solver/delta_vel.rs | 18 + src/dynamics/solver/interaction_groups.rs | 434 +++++ src/dynamics/solver/island_solver.rs | 73 + .../ball_position_constraint.rs | 165 ++ .../ball_position_constraint_wide.rs | 199 ++ .../ball_velocity_constraint.rs | 238 +++ .../ball_velocity_constraint_wide.rs | 329 ++++ .../fixed_position_constraint.rs | 139 ++ .../fixed_velocity_constraint.rs | 370 ++++ .../fixed_velocity_constraint_wide.rs | 472 +++++ .../joint_constraint/joint_constraint.rs | 340 ++++ .../joint_position_constraint.rs | 169 ++ src/dynamics/solver/joint_constraint/mod.rs | 65 + .../prismatic_position_constraint.rs | 170 ++ .../prismatic_velocity_constraint.rs | 558 ++++++ .../prismatic_velocity_constraint_wide.rs | 687 +++++++ .../revolute_position_constraint.rs | 142 ++ .../revolute_velocity_constraint.rs | 294 +++ .../revolute_velocity_constraint_wide.rs | 397 ++++ src/dynamics/solver/mod.rs | 56 + src/dynamics/solver/parallel_island_solver.rs | 259 +++ .../solver/parallel_position_solver.rs | 582 ++++++ .../solver/parallel_velocity_solver.rs | 485 +++++ src/dynamics/solver/position_constraint.rs | 246 +++ .../solver/position_constraint_wide.rs | 217 +++ .../solver/position_ground_constraint.rs | 189 ++ .../solver/position_ground_constraint_wide.rs | 199 ++ src/dynamics/solver/position_solver.rs | 451 +++++ src/dynamics/solver/velocity_constraint.rs | 401 ++++ .../solver/velocity_constraint_wide.rs | 344 ++++ .../solver/velocity_ground_constraint.rs | 300 +++ .../solver/velocity_ground_constraint_wide.rs | 300 +++ src/dynamics/solver/velocity_solver.rs | 405 ++++ src/geometry/ball.rs | 16 + src/geometry/broad_phase.rs | 255 +++ src/geometry/broad_phase_multi_sap.rs | 645 +++++++ src/geometry/capsule.rs | 168 ++ src/geometry/collider.rs | 373 ++++ src/geometry/collider_set.rs | 139 ++ src/geometry/contact.rs | 485 +++++ .../ball_ball_contact_generator.rs | 98 + .../ball_convex_contact_generator.rs | 85 + .../ball_polygon_contact_generator.rs | 0 .../capsule_capsule_contact_generator.rs | 200 ++ .../contact_generator/contact_dispatcher.rs | 127 ++ .../contact_generator/contact_generator.rs | 222 +++ .../cuboid_capsule_contact_generator.rs | 188 ++ .../cuboid_cuboid_contact_generator.rs | 155 ++ .../cuboid_polygon_contact_generator.rs | 0 .../cuboid_triangle_contact_generator.rs | 171 ++ .../heightfield_shape_contact_generator.rs | 189 ++ src/geometry/contact_generator/mod.rs | 71 + .../polygon_polygon_contact_generator.rs | 298 +++ .../trimesh_shape_contact_generator.rs | 194 ++ .../voxels_shape_contact_generator.rs | 0 src/geometry/cuboid.rs | 234 +++ src/geometry/cuboid_feature2d.rs | 128 ++ src/geometry/cuboid_feature3d.rs | 516 +++++ src/geometry/interaction_graph.rs | 259 +++ src/geometry/mod.rs | 80 + src/geometry/narrow_phase.rs | 483 +++++ src/geometry/polygon.rs | 76 + src/geometry/polygon_intersection.rs | 263 +++ src/geometry/polyhedron_feature3d.rs | 284 +++ src/geometry/proximity.rs | 31 + .../ball_ball_proximity_detector.rs | 68 + .../ball_convex_proximity_detector.rs | 53 + .../ball_polygon_proximity_detector.rs | 0 .../cuboid_cuboid_proximity_detector.rs | 79 + .../cuboid_polygon_proximity_detector.rs | 0 .../cuboid_triangle_proximity_detector.rs | 88 + src/geometry/proximity_detector/mod.rs | 30 + .../polygon_polygon_proximity_detector.rs | 54 + .../proximity_detector/proximity_detector.rs | 212 +++ .../proximity_dispatcher.rs | 136 ++ .../trimesh_shape_proximity_detector.rs | 133 ++ .../voxels_shape_proximity_detector.rs | 0 src/geometry/sat.rs | 294 +++ src/geometry/triangle.rs | 9 + src/geometry/trimesh.rs | 122 ++ src/geometry/waabb.rs | 116 ++ src/geometry/z_order.rs | 70 + src/lib.rs | 255 +++ src/pipeline/collision_pipeline.rs | 111 ++ src/pipeline/event_handler.rs | 51 + src/pipeline/mod.rs | 9 + src/pipeline/physics_pipeline.rs | 332 ++++ src/utils.rs | 1333 +++++++++++++ src_testbed/Inconsolata.otf | Bin 0 -> 58464 bytes src_testbed/box2d_backend.rs | 237 +++ src_testbed/engine.rs | 694 +++++++ src_testbed/lib.rs | 52 + src_testbed/nphysics_backend.rs | 212 +++ src_testbed/objects/ball.rs | 73 + src_testbed/objects/box_node.rs | 73 + src_testbed/objects/capsule.rs | 74 + src_testbed/objects/convex.rs | 77 + src_testbed/objects/heightfield.rs | 120 ++ src_testbed/objects/mesh.rs | 108 ++ src_testbed/objects/mod.rs | 10 + src_testbed/objects/node.rs | 164 ++ src_testbed/objects/plane.rs | 132 ++ src_testbed/objects/polyline.rs | 79 + src_testbed/physx_backend.rs | 604 ++++++ src_testbed/testbed.rs | 1652 +++++++++++++++++ src_testbed/ui.rs | 568 ++++++ 175 files changed, 32819 insertions(+) create mode 100644 .github/FUNDING.yml create mode 100644 .gitignore create mode 100644 Cargo.toml create mode 100644 LICENSE create mode 100644 README.md create mode 100644 build/rapier2d/Cargo.toml create mode 100644 build/rapier3d/Cargo.toml create mode 100644 build/rapier_testbed2d/Cargo.toml create mode 100644 build/rapier_testbed3d/Cargo.toml create mode 100644 examples2d/Cargo.toml create mode 100644 examples2d/all_examples2.rs create mode 100644 examples2d/balls2.rs create mode 100644 examples2d/boxes2.rs create mode 100644 examples2d/capsules2.rs create mode 100644 examples2d/debug_box_ball2.rs create mode 100644 examples2d/heightfield2.rs create mode 100644 examples2d/joints2.rs create mode 100644 examples2d/kinematic2.rs create mode 100644 examples2d/pyramid2.rs create mode 100644 examples2d/sensor2.rs create mode 100644 examples2d/stress_joint_ball2.rs create mode 100644 examples2d/stress_joint_fixed2.rs create mode 100644 examples2d/stress_joint_prismatic2.rs create mode 100644 examples3d/Cargo.toml create mode 100644 examples3d/all_examples3.rs create mode 100644 examples3d/balls3.rs create mode 100644 examples3d/boxes3.rs create mode 100644 examples3d/capsules3.rs create mode 100644 examples3d/debug_boxes3.rs create mode 100644 examples3d/debug_triangle3.rs create mode 100644 examples3d/domino3.rs create mode 100644 examples3d/heightfield3.rs create mode 100644 examples3d/joints3.rs create mode 100644 examples3d/kinematic3.rs create mode 100644 examples3d/pyramid3.rs create mode 100644 examples3d/sensor3.rs create mode 100644 examples3d/stacks3.rs create mode 100644 examples3d/stress_joint_ball3.rs create mode 100644 examples3d/stress_joint_fixed3.rs create mode 100644 examples3d/stress_joint_prismatic3.rs create mode 100644 examples3d/stress_joint_revolute3.rs create mode 100644 examples3d/stress_keva3.rs create mode 100644 examples3d/trimesh3.rs create mode 100644 src/counters/ccd_counters.rs create mode 100644 src/counters/collision_detection_counters.rs create mode 100644 src/counters/mod.rs create mode 100644 src/counters/solver_counters.rs create mode 100644 src/counters/stages_counters.rs create mode 100644 src/counters/timer.rs create mode 100644 src/data/arena.rs create mode 100644 src/data/graph.rs create mode 100644 src/data/mod.rs create mode 100644 src/dynamics/integration_parameters.rs create mode 100644 src/dynamics/joint/ball_joint.rs create mode 100644 src/dynamics/joint/fixed_joint.rs create mode 100644 src/dynamics/joint/joint.rs create mode 100644 src/dynamics/joint/joint_set.rs create mode 100644 src/dynamics/joint/mod.rs create mode 100644 src/dynamics/joint/prismatic_joint.rs create mode 100644 src/dynamics/joint/revolute_joint.rs create mode 100644 src/dynamics/mass_properties.rs create mode 100644 src/dynamics/mass_properties_ball.rs create mode 100644 src/dynamics/mass_properties_capsule.rs create mode 100644 src/dynamics/mass_properties_cuboid.rs create mode 100644 src/dynamics/mass_properties_polygon.rs create mode 100644 src/dynamics/mod.rs create mode 100644 src/dynamics/rigid_body.rs create mode 100644 src/dynamics/rigid_body_set.rs create mode 100644 src/dynamics/solver/categorization.rs create mode 100644 src/dynamics/solver/delta_vel.rs create mode 100644 src/dynamics/solver/interaction_groups.rs create mode 100644 src/dynamics/solver/island_solver.rs create mode 100644 src/dynamics/solver/joint_constraint/ball_position_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs create mode 100644 src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs create mode 100644 src/dynamics/solver/joint_constraint/fixed_position_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/joint_position_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/mod.rs create mode 100644 src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs create mode 100644 src/dynamics/solver/joint_constraint/revolute_position_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs create mode 100644 src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs create mode 100644 src/dynamics/solver/mod.rs create mode 100644 src/dynamics/solver/parallel_island_solver.rs create mode 100644 src/dynamics/solver/parallel_position_solver.rs create mode 100644 src/dynamics/solver/parallel_velocity_solver.rs create mode 100644 src/dynamics/solver/position_constraint.rs create mode 100644 src/dynamics/solver/position_constraint_wide.rs create mode 100644 src/dynamics/solver/position_ground_constraint.rs create mode 100644 src/dynamics/solver/position_ground_constraint_wide.rs create mode 100644 src/dynamics/solver/position_solver.rs create mode 100644 src/dynamics/solver/velocity_constraint.rs create mode 100644 src/dynamics/solver/velocity_constraint_wide.rs create mode 100644 src/dynamics/solver/velocity_ground_constraint.rs create mode 100644 src/dynamics/solver/velocity_ground_constraint_wide.rs create mode 100644 src/dynamics/solver/velocity_solver.rs create mode 100644 src/geometry/ball.rs create mode 100644 src/geometry/broad_phase.rs create mode 100644 src/geometry/broad_phase_multi_sap.rs create mode 100644 src/geometry/capsule.rs create mode 100644 src/geometry/collider.rs create mode 100644 src/geometry/collider_set.rs create mode 100644 src/geometry/contact.rs create mode 100644 src/geometry/contact_generator/ball_ball_contact_generator.rs create mode 100644 src/geometry/contact_generator/ball_convex_contact_generator.rs create mode 100644 src/geometry/contact_generator/ball_polygon_contact_generator.rs create mode 100644 src/geometry/contact_generator/capsule_capsule_contact_generator.rs create mode 100644 src/geometry/contact_generator/contact_dispatcher.rs create mode 100644 src/geometry/contact_generator/contact_generator.rs create mode 100644 src/geometry/contact_generator/cuboid_capsule_contact_generator.rs create mode 100644 src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs create mode 100644 src/geometry/contact_generator/cuboid_polygon_contact_generator.rs create mode 100644 src/geometry/contact_generator/cuboid_triangle_contact_generator.rs create mode 100644 src/geometry/contact_generator/heightfield_shape_contact_generator.rs create mode 100644 src/geometry/contact_generator/mod.rs create mode 100644 src/geometry/contact_generator/polygon_polygon_contact_generator.rs create mode 100644 src/geometry/contact_generator/trimesh_shape_contact_generator.rs create mode 100644 src/geometry/contact_generator/voxels_shape_contact_generator.rs create mode 100644 src/geometry/cuboid.rs create mode 100644 src/geometry/cuboid_feature2d.rs create mode 100644 src/geometry/cuboid_feature3d.rs create mode 100644 src/geometry/interaction_graph.rs create mode 100644 src/geometry/mod.rs create mode 100644 src/geometry/narrow_phase.rs create mode 100644 src/geometry/polygon.rs create mode 100644 src/geometry/polygon_intersection.rs create mode 100644 src/geometry/polyhedron_feature3d.rs create mode 100644 src/geometry/proximity.rs create mode 100644 src/geometry/proximity_detector/ball_ball_proximity_detector.rs create mode 100644 src/geometry/proximity_detector/ball_convex_proximity_detector.rs create mode 100644 src/geometry/proximity_detector/ball_polygon_proximity_detector.rs create mode 100644 src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs create mode 100644 src/geometry/proximity_detector/cuboid_polygon_proximity_detector.rs create mode 100644 src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs create mode 100644 src/geometry/proximity_detector/mod.rs create mode 100644 src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs create mode 100644 src/geometry/proximity_detector/proximity_detector.rs create mode 100644 src/geometry/proximity_detector/proximity_dispatcher.rs create mode 100644 src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs create mode 100644 src/geometry/proximity_detector/voxels_shape_proximity_detector.rs create mode 100644 src/geometry/sat.rs create mode 100644 src/geometry/triangle.rs create mode 100644 src/geometry/trimesh.rs create mode 100644 src/geometry/waabb.rs create mode 100644 src/geometry/z_order.rs create mode 100644 src/lib.rs create mode 100644 src/pipeline/collision_pipeline.rs create mode 100644 src/pipeline/event_handler.rs create mode 100644 src/pipeline/mod.rs create mode 100644 src/pipeline/physics_pipeline.rs create mode 100644 src/utils.rs create mode 100644 src_testbed/Inconsolata.otf create mode 100644 src_testbed/box2d_backend.rs create mode 100644 src_testbed/engine.rs create mode 100644 src_testbed/lib.rs create mode 100644 src_testbed/nphysics_backend.rs create mode 100644 src_testbed/objects/ball.rs create mode 100644 src_testbed/objects/box_node.rs create mode 100644 src_testbed/objects/capsule.rs create mode 100644 src_testbed/objects/convex.rs create mode 100644 src_testbed/objects/heightfield.rs create mode 100644 src_testbed/objects/mesh.rs create mode 100644 src_testbed/objects/mod.rs create mode 100644 src_testbed/objects/node.rs create mode 100644 src_testbed/objects/plane.rs create mode 100644 src_testbed/objects/polyline.rs create mode 100644 src_testbed/physx_backend.rs create mode 100644 src_testbed/testbed.rs create mode 100644 src_testbed/ui.rs diff --git a/.github/FUNDING.yml b/.github/FUNDING.yml new file mode 100644 index 000000000..5ee5fec9b --- /dev/null +++ b/.github/FUNDING.yml @@ -0,0 +1,12 @@ +# These are supported funding model platforms + +github: [ "dimforge" ] # Replace with up to 4 GitHub Sponsors-enabled usernames e.g., [user1, user2] +patreon: # Replace with a single Patreon username +open_collective: # Replace with a single Open Collective username +ko_fi: # Replace with a single Ko-fi username +tidelift: # Replace with a single Tidelift platform-name/package-name e.g., npm/babel +community_bridge: # Replace with a single Community Bridge project-name e.g., cloud-foundry +liberapay: # Replace with a single Liberapay username +issuehunt: # Replace with a single IssueHunt username +otechie: # Replace with a single Otechie username +custom: # Replace with up to 4 custom sponsorship URLs e.g., ['link1', 'link2'] diff --git a/.gitignore b/.gitignore new file mode 100644 index 000000000..d8c2e335c --- /dev/null +++ b/.gitignore @@ -0,0 +1,8 @@ +**/*.rs.bk +Cargo.lock +node_modules +target +.idea +.DS_Store +package-lock.json +**/*.csv \ No newline at end of file diff --git a/Cargo.toml b/Cargo.toml new file mode 100644 index 000000000..cf890180a --- /dev/null +++ b/Cargo.toml @@ -0,0 +1,12 @@ +[workspace] +members = [ "build/rapier2d", "build/rapier_testbed2d", "examples2d", "examples2d/wasm", + "build/rapier3d", "build/rapier_testbed3d", "examples3d", "examples3d/wasm",] + +[patch.crates-io] +#wrapped2d = { git = "https://github.com/Bastacyclop/rust_box2d.git" } + +[profile.release] +debug = false +codegen-units = 1 +#opt-level = 1 +#lto = true diff --git a/LICENSE b/LICENSE new file mode 100644 index 000000000..97f4383a3 --- /dev/null +++ b/LICENSE @@ -0,0 +1,201 @@ + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright 2020 Sébastien Crozet + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/README.md b/README.md new file mode 100644 index 000000000..9ce06b08d --- /dev/null +++ b/README.md @@ -0,0 +1,30 @@ +

+ crates.io +

+

+ + + + + Build status + + + crates.io + + + + +

+

+ + Website | Documentation +

+ +----- + +

+2D and 3D physics engines +for the Rust programming language. +

+ +----- diff --git a/build/rapier2d/Cargo.toml b/build/rapier2d/Cargo.toml new file mode 100644 index 000000000..8afa44318 --- /dev/null +++ b/build/rapier2d/Cargo.toml @@ -0,0 +1,53 @@ +# Name idea: bident for 2D and trident for 3D +[package] +name = "rapier2d" +version = "0.1.0" +authors = [ "Sébastien Crozet " ] +description = "2-dimensional physics engine in Rust." +documentation = "http://rapier.rs/rustdoc/rapier2d/index.html" +homepage = "http://rapier.rs" +repository = "https://github.com/rustsim/rapier" +readme = "README.md" +keywords = [ "physics", "dynamics", "rigid", "real-time", "joints" ] +license = "Apache-2.0" +edition = "2018" + +[features] +default = [ "dim2" ] +dim2 = [ ] +parallel = [ "rayon" ] +simd-stable = [ "simba/wide", "simd-is-enabled" ] +simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] +# Do not enable this feature directly. It is automatically +# enabled with the "simd-stable" or "simd-nightly" feature. +simd-is-enabled = [ ] +wasm-bindgen = [ "instant/wasm-bindgen" ] +serde-serialize = [ "nalgebra/serde-serialize", "ncollide2d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde", "arrayvec/serde" ] +enhanced-determinism = [ "simba/libm_force", "indexmap" ] + +[lib] +name = "rapier2d" +path = "../../src/lib.rs" +required-features = [ "dim2" ] + + +[dependencies] +vec_map = "0.8" +instant = { version = "0.1", features = [ "now" ]} +num-traits = "0.2" +nalgebra = "0.22" +ncollide2d = "0.24" +simba = "0.2" +approx = "0.3" +rayon = { version = "1", optional = true } +crossbeam = "0.7" +generational-arena = "0.2" +arrayvec = "0.5" +bit-vec = "0.6" +rustc-hash = "1" +serde = { version = "1", features = [ "derive" ], optional = true } +indexmap = { version = "1", features = [ "serde-1" ], optional = true } + +[dev-dependencies] +bincode = "1" +serde = { version = "1", features = [ "derive" ] } diff --git a/build/rapier3d/Cargo.toml b/build/rapier3d/Cargo.toml new file mode 100644 index 000000000..00eb77df0 --- /dev/null +++ b/build/rapier3d/Cargo.toml @@ -0,0 +1,53 @@ +# Name idea: bident for 2D and trident for 3D +[package] +name = "rapier3d" +version = "0.1.0" +authors = [ "Sébastien Crozet " ] +description = "3-dimensional physics engine in Rust." +documentation = "http://rapier.rs/rustdoc/rapier2d/index.html" +homepage = "http://rapier.rs" +repository = "https://github.com/rustsim/rapier" +readme = "README.md" +keywords = [ "physics", "dynamics", "rigid", "real-time", "joints" ] +license = "Apache-2.0" +edition = "2018" + +[features] +default = [ "dim3" ] +dim3 = [ ] +parallel = [ "rayon" ] +simd-stable = [ "simba/wide", "simd-is-enabled" ] +simd-nightly = [ "simba/packed_simd", "simd-is-enabled" ] +# Do not enable this feature directly. It is automatically +# enabled with the "simd-stable" or "simd-nightly" feature. +simd-is-enabled = [ ] +wasm-bindgen = [ "instant/wasm-bindgen" ] +serde-serialize = [ "nalgebra/serde-serialize", "ncollide3d/serde-serialize", "serde", "generational-arena/serde", "bit-vec/serde" ] +enhanced-determinism = [ "simba/libm_force", "indexmap" ] + +[lib] +name = "rapier3d" +path = "../../src/lib.rs" +required-features = [ "dim3" ] + + +[dependencies] +vec_map = "0.8" +instant = { version = "0.1", features = [ "now" ]} +num-traits = "0.2" +nalgebra = "0.22" +ncollide3d = "0.24" +simba = "0.2" +approx = "0.3" +rayon = { version = "1", optional = true } +crossbeam = "0.7" +generational-arena = "0.2" +arrayvec = "0.5" +bit-vec = "0.6" +rustc-hash = "1" +serde = { version = "1", features = [ "derive" ], optional = true } +indexmap = { version = "1", features = [ "serde-1" ], optional = true } + +[dev-dependencies] +bincode = "1" +serde = { version = "1", features = [ "derive" ] } diff --git a/build/rapier_testbed2d/Cargo.toml b/build/rapier_testbed2d/Cargo.toml new file mode 100644 index 000000000..b4e82a230 --- /dev/null +++ b/build/rapier_testbed2d/Cargo.toml @@ -0,0 +1,45 @@ +[package] +name = "rapier_testbed2d" +version = "0.1.0" +authors = [ "Sébastien Crozet " ] +description = "Testbed for the 2-dimensional physics engine in Rust." +homepage = "http://rapier.org" +repository = "https://github.com/rustsim/rapier" +keywords = [ "physics", "dynamics", "rigid", "real-time", "joints" ] +license = "Apache-2.0" +edition = "2018" + + +[lib] +name = "rapier_testbed2d" +path = "../../src_testbed/lib.rs" +required-features = [ "dim2" ] + +[features] +default = [ "dim2" ] +dim2 = [ ] +parallel = [ "rapier2d/parallel", "num_cpus" ] +other-backends = [ "wrapped2d", "nphysics2d" ] + + +[dependencies] +nalgebra = "0.22" +kiss3d = { version = "0.25", features = [ "conrod" ] } +rand = "0.7" +rand_pcg = "0.2" +instant = { version = "0.1", features = [ "web-sys", "now" ]} +bitflags = "1" +num_cpus = { version = "1", optional = true } +wrapped2d = { version = "0.4", optional = true } +ncollide2d = "0.24" +nphysics2d = { version = "0.17", optional = true } +crossbeam = "0.7" +bincode = "1" +flexbuffers = "0.1" +md5 = "0.7" + + +[dependencies.rapier2d] +path = "../rapier2d" +version = "0.1" +features = [ "serde-serialize" ] diff --git a/build/rapier_testbed3d/Cargo.toml b/build/rapier_testbed3d/Cargo.toml new file mode 100644 index 000000000..31cdbb932 --- /dev/null +++ b/build/rapier_testbed3d/Cargo.toml @@ -0,0 +1,47 @@ +[package] +name = "rapier_testbed3d" +version = "0.1.0" +authors = [ "Sébastien Crozet " ] +description = "Testbed for the 3-dimensional physics engine in Rust." +homepage = "http://rapier.org" +repository = "https://github.com/rustsim/rapier" +keywords = [ "physics", "dynamics", "rigid", "real-time", "joints" ] +license = "Apache-2.0" +edition = "2018" + + +[lib] +name = "rapier_testbed3d" +path = "../../src_testbed/lib.rs" +required-features = [ "dim3" ] + +[features] +default = [ "dim3" ] +dim3 = [ ] +parallel = [ "rapier3d/parallel", "num_cpus" ] +other-backends = [ "physx", "physx-sys", "glam", "nphysics3d" ] + +[dependencies] +nalgebra = "0.22" +kiss3d = { version = "0.25", features = [ "conrod" ] } +rand = "0.7" +rand_pcg = "0.2" +instant = { version = "0.1", features = [ "web-sys", "now" ]} +bitflags = "1" +glam = { version = "0.8", optional = true } +num_cpus = { version = "1", optional = true } +ncollide3d = "0.24" +nphysics3d = { version = "0.17", optional = true } +physx = { version = "0.6", optional = true } +physx-sys = { version = "0.4", optional = true } +crossbeam = "0.7" +bincode = "1" +flexbuffers = "0.1" +serde_cbor = "0.11" +md5 = "0.7" +serde = { version = "1", features = [ "derive" ] } + +[dependencies.rapier3d] +path = "../rapier3d" +version = "0.1" +features = [ "serde-serialize" ] diff --git a/examples2d/Cargo.toml b/examples2d/Cargo.toml new file mode 100644 index 000000000..7e979482b --- /dev/null +++ b/examples2d/Cargo.toml @@ -0,0 +1,27 @@ +[package] +name = "nphysics-examples-2d" +version = "0.1.0" +authors = [ "Sébastien Crozet " ] +edition = "2018" + +[features] +parallel = [ "rapier2d/parallel", "rapier_testbed2d/parallel" ] +simd-stable = [ "rapier2d/simd-stable" ] +simd-nightly = [ "rapier2d/simd-nightly" ] +other-backends = [ "rapier_testbed2d/other-backends" ] +enhanced-determinism = [ "rapier2d/enhanced-determinism" ] + +[dependencies] +rand = "0.7" +Inflector = "0.11" +nalgebra = "0.22" + +[dependencies.rapier_testbed2d] +path = "../build/rapier_testbed2d" + +[dependencies.rapier2d] +path = "../build/rapier2d" + +[[bin]] +name = "all_examples2" +path = "./all_examples2.rs" \ No newline at end of file diff --git a/examples2d/all_examples2.rs b/examples2d/all_examples2.rs new file mode 100644 index 000000000..7b8bf0905 --- /dev/null +++ b/examples2d/all_examples2.rs @@ -0,0 +1,90 @@ +#![allow(dead_code)] + +extern crate nalgebra as na; + +#[cfg(target_arch = "wasm32")] +use wasm_bindgen::prelude::*; + +use inflector::Inflector; + +use rapier_testbed2d::Testbed; +use std::cmp::Ordering; + +mod balls2; +mod boxes2; +mod capsules2; +mod debug_box_ball2; +mod heightfield2; +mod joints2; +mod kinematic2; +mod pyramid2; +mod sensor2; +mod stress_joint_ball2; +mod stress_joint_fixed2; +mod stress_joint_prismatic2; + +fn demo_name_from_command_line() -> Option { + let mut args = std::env::args(); + + while let Some(arg) = args.next() { + if &arg[..] == "--example" { + return args.next(); + } + } + + None +} + +#[cfg(any(target_arch = "wasm32", target_arch = "asmjs"))] +fn demo_name_from_url() -> Option { + None + // let window = stdweb::web::window(); + // let hash = window.location()?.search().ok()?; + // Some(hash[1..].to_string()) +} + +#[cfg(not(any(target_arch = "wasm32", target_arch = "asmjs")))] +fn demo_name_from_url() -> Option { + None +} + +#[cfg_attr(target_arch = "wasm32", wasm_bindgen(start))] +pub fn main() { + let demo = demo_name_from_command_line() + .or_else(|| demo_name_from_url()) + .unwrap_or(String::new()) + .to_camel_case(); + + let mut builders: Vec<(_, fn(&mut Testbed))> = vec![ + ("Balls", balls2::init_world), + ("Boxes", boxes2::init_world), + ("Capsules", capsules2::init_world), + ("Heightfield", heightfield2::init_world), + ("Joints", joints2::init_world), + ("Kinematic", kinematic2::init_world), + ("Pyramid", pyramid2::init_world), + ("Sensor", sensor2::init_world), + ("(Debug) box ball", debug_box_ball2::init_world), + ("(Stress test) joint ball", stress_joint_ball2::init_world), + ("(Stress test) joint fixed", stress_joint_fixed2::init_world), + ( + "(Stress test) joint prismatic", + stress_joint_prismatic2::init_world, + ), + ]; + + // Lexicographic sort, with stress tests moved at the end of the list. + builders.sort_by(|a, b| match (a.0.starts_with("("), b.0.starts_with("(")) { + (true, true) | (false, false) => a.0.cmp(b.0), + (true, false) => Ordering::Greater, + (false, true) => Ordering::Less, + }); + + let i = builders + .iter() + .position(|builder| builder.0.to_camel_case().as_str() == demo.as_str()) + .unwrap_or(0); + let testbed = Testbed::from_builders(i, builders); + + testbed.run() +} diff --git a/examples2d/balls2.rs b/examples2d/balls2.rs new file mode 100644 index 000000000..4c85d869a --- /dev/null +++ b/examples2d/balls2.rs @@ -0,0 +1,68 @@ +use na::Point2; +use rapier2d::dynamics::{BodyStatus, JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier2d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + /* + * Ground + */ + let _ground_size = 25.0; + + /* + let ground_shape = ShapeHandle::new(Cuboid::new(Vector2::new(ground_size, 1.0))); + + let ground_handle = bodies.insert(Ground::new()); + let co = ColliderDesc::new(ground_shape) + .translation(-Vector2::y()) + .build(BodyPartHandle(ground_handle, 0)); + colliders.insert(co); + */ + + /* + * Create the balls + */ + let num = 50; + let rad = 1.0; + + let shiftx = rad * 2.5; + let shifty = rad * 2.0; + let centerx = shiftx * (num as f32) / 2.0; + let centery = shifty / 2.0; + + for i in 0..num { + for j in 0usize..num * 5 { + let x = i as f32 * shiftx - centerx; + let y = j as f32 * shifty + centery; + + let status = if j == 0 { + BodyStatus::Static + } else { + BodyStatus::Dynamic + }; + + // Build the rigid body. + let rigid_body = RigidBodyBuilder::new(status).translation(x, y).build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::ball(rad).density(1.0).build(); + colliders.insert(collider, handle, &mut bodies); + } + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point2::new(0.0, 2.5), 5.0); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Balls", init_world)]); + testbed.run() +} diff --git a/examples2d/boxes2.rs b/examples2d/boxes2.rs new file mode 100644 index 000000000..8b09def0e --- /dev/null +++ b/examples2d/boxes2.rs @@ -0,0 +1,73 @@ +use na::Point2; +use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier2d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + /* + * Ground + */ + let ground_size = 25.0; + + let rigid_body = RigidBodyBuilder::new_static().build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, 1.2).build(); + colliders.insert(collider, handle, &mut bodies); + + let rigid_body = RigidBodyBuilder::new_static() + .rotation(std::f32::consts::FRAC_PI_2) + .translation(ground_size, ground_size * 2.0) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size * 2.0, 1.2).build(); + colliders.insert(collider, handle, &mut bodies); + + let rigid_body = RigidBodyBuilder::new_static() + .rotation(std::f32::consts::FRAC_PI_2) + .translation(-ground_size, ground_size * 2.0) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size * 2.0, 1.2).build(); + colliders.insert(collider, handle, &mut bodies); + + /* + * Create the cubes + */ + let num = 26; + let rad = 0.5; + + let shift = rad * 2.0; + let centerx = shift * (num as f32) / 2.0; + let centery = shift / 2.0; + + for i in 0..num { + for j in 0usize..num * 5 { + let x = i as f32 * shift - centerx; + let y = j as f32 * shift + centery + 2.0; + + // Build the rigid body. + let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad).density(1.0).build(); + colliders.insert(collider, handle, &mut bodies); + } + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point2::new(0.0, 50.0), 10.0); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Balls", init_world)]); + testbed.run() +} diff --git a/examples2d/capsules2.rs b/examples2d/capsules2.rs new file mode 100644 index 000000000..041edf5cf --- /dev/null +++ b/examples2d/capsules2.rs @@ -0,0 +1,76 @@ +use na::Point2; +use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier2d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + /* + * Ground + */ + let ground_size = 25.0; + + let rigid_body = RigidBodyBuilder::new_static().build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, 1.2).build(); + colliders.insert(collider, handle, &mut bodies); + + let rigid_body = RigidBodyBuilder::new_static() + .rotation(std::f32::consts::FRAC_PI_2) + .translation(ground_size, ground_size * 4.0) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size * 4.0, 1.2).build(); + colliders.insert(collider, handle, &mut bodies); + + let rigid_body = RigidBodyBuilder::new_static() + .rotation(std::f32::consts::FRAC_PI_2) + .translation(-ground_size, ground_size * 4.0) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size * 4.0, 1.2).build(); + colliders.insert(collider, handle, &mut bodies); + + /* + * Create the cubes + */ + let num = 26; + let rad = 0.5; + + let shift = rad * 2.0; + let shifty = rad * 5.0; + let centerx = shift * (num as f32) / 2.0; + let centery = shift / 2.0; + + for i in 0..num { + for j in 0usize..num * 5 { + let x = i as f32 * shift - centerx; + let y = j as f32 * shifty + centery + 3.0; + + // Build the rigid body. + let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::capsule_y(rad * 1.5, rad) + .density(1.0) + .build(); + colliders.insert(collider, handle, &mut bodies); + } + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point2::new(0.0, 50.0), 10.0); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Balls", init_world)]); + testbed.run() +} diff --git a/examples2d/debug_box_ball2.rs b/examples2d/debug_box_ball2.rs new file mode 100644 index 000000000..2cc5e45e3 --- /dev/null +++ b/examples2d/debug_box_ball2.rs @@ -0,0 +1,44 @@ +use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier2d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + /* + * Ground + */ + let rad = 1.0; + let rigid_body = RigidBodyBuilder::new_static() + .translation(0.0, -rad) + .rotation(std::f32::consts::PI / 4.0) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad).build(); + colliders.insert(collider, handle, &mut bodies); + + // Build the dynamic box rigid body. + let rigid_body = RigidBodyBuilder::new_dynamic() + .translation(0.0, 3.0 * rad) + .can_sleep(false) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::ball(rad).density(1.0).build(); + colliders.insert(collider, handle, &mut bodies); + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + // testbed.look_at(Point2::new(10.0, 10.0, 10.0), Point2::origin()); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]); + testbed.run() +} diff --git a/examples2d/heightfield2.rs b/examples2d/heightfield2.rs new file mode 100644 index 000000000..b03afe3ac --- /dev/null +++ b/examples2d/heightfield2.rs @@ -0,0 +1,72 @@ +use na::{DVector, Point2, Vector2}; +use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier2d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + /* + * Ground + */ + let ground_size = Vector2::new(50.0, 1.0); + let nsubdivs = 2000; + + let heights = DVector::from_fn(nsubdivs + 1, |i, _| { + if i == 0 || i == nsubdivs { + 80.0 + } else { + (i as f32 * ground_size.x / (nsubdivs as f32)).cos() * 2.0 + } + }); + + let rigid_body = RigidBodyBuilder::new_static().build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::heightfield(heights, ground_size).build(); + colliders.insert(collider, handle, &mut bodies); + + /* + * Create the cubes + */ + let num = 26; + let rad = 0.5; + + let shift = rad * 2.0; + let centerx = shift * (num / 2) as f32; + let centery = shift / 2.0; + + for i in 0..num { + for j in 0usize..num * 5 { + let x = i as f32 * shift - centerx; + let y = j as f32 * shift + centery + 3.0; + + // Build the rigid body. + let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build(); + let handle = bodies.insert(rigid_body); + + if j % 2 == 0 { + let collider = ColliderBuilder::cuboid(rad, rad).density(1.0).build(); + colliders.insert(collider, handle, &mut bodies); + } else { + let collider = ColliderBuilder::ball(rad).density(1.0).build(); + colliders.insert(collider, handle, &mut bodies); + } + } + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point2::new(0.0, 50.0), 10.0); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Heightfield", init_world)]); + testbed.run() +} diff --git a/examples2d/joints2.rs b/examples2d/joints2.rs new file mode 100644 index 000000000..c3df921c7 --- /dev/null +++ b/examples2d/joints2.rs @@ -0,0 +1,75 @@ +use na::Point2; +use rapier2d::dynamics::{BallJoint, BodyStatus, JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier2d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut joints = JointSet::new(); + + /* + * Create the balls + */ + // Build the rigid body. + // NOTE: a smaller radius (e.g. 0.1) breaks Box2D so + // in order to be able to compare rapier with Box2D, + // we set it to 0.4. + let rad = 0.4; + let numi = 100; // Num vertical nodes. + let numk = 100; // Num horizontal nodes. + let shift = 1.0; + + let mut body_handles = Vec::new(); + + for k in 0..numk { + for i in 0..numi { + let fk = k as f32; + let fi = i as f32; + + let status = if i == 0 && (k % 4 == 0 || k == numk - 1) { + BodyStatus::Static + } else { + BodyStatus::Dynamic + }; + + let rigid_body = RigidBodyBuilder::new(status) + .translation(fk * shift, -fi * shift) + .build(); + let child_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::ball(rad).density(1.0).build(); + colliders.insert(collider, child_handle, &mut bodies); + + // Vertical joint. + if i > 0 { + let parent_handle = *body_handles.last().unwrap(); + let joint = BallJoint::new(Point2::origin(), Point2::new(0.0, shift)); + joints.insert(&mut bodies, parent_handle, child_handle, joint); + } + + // Horizontal joint. + if k > 0 { + let parent_index = body_handles.len() - numi; + let parent_handle = body_handles[parent_index]; + let joint = BallJoint::new(Point2::origin(), Point2::new(-shift, 0.0)); + joints.insert(&mut bodies, parent_handle, child_handle, joint); + } + + body_handles.push(child_handle); + } + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point2::new(numk as f32 * rad, numi as f32 * -rad), 5.0); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]); + testbed.run() +} diff --git a/examples2d/kinematic2.rs b/examples2d/kinematic2.rs new file mode 100644 index 000000000..d979cd04c --- /dev/null +++ b/examples2d/kinematic2.rs @@ -0,0 +1,93 @@ +use na::Point2; +use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier2d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + /* + * Ground. + */ + let ground_size = 10.0; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::new_static() + .translation(0.0, -ground_height) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height).build(); + colliders.insert(collider, handle, &mut bodies); + + /* + * Create the boxes + */ + let num = 6; + let rad = 0.2; + + let shift = rad * 2.0; + let centerx = shift * num as f32 / 2.0; + let centery = shift / 2.0 + 3.04; + + for i in 0usize..num { + for j in 0usize..num * 50 { + let x = i as f32 * shift - centerx; + let y = j as f32 * shift + centery; + + // Build the rigid body. + let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad).density(1.0).build(); + colliders.insert(collider, handle, &mut bodies); + } + } + + /* + * Setup a kinematic rigid body. + */ + let platform_body = RigidBodyBuilder::new_kinematic() + .translation(-10.0 * rad, 1.5 + 0.8) + .build(); + let platform_handle = bodies.insert(platform_body); + let collider = ColliderBuilder::cuboid(rad * 10.0, rad) + .density(1.0) + .build(); + colliders.insert(collider, platform_handle, &mut bodies); + + /* + * Setup a callback to control the platform. + */ + testbed.add_callback(move |bodies, _, _, _, time| { + let mut platform = bodies.get_mut(platform_handle).unwrap(); + let mut next_pos = platform.position; + + let dt = 0.016; + next_pos.translation.vector.y += (time * 5.0).sin() * dt; + next_pos.translation.vector.x += time.sin() * 5.0 * dt; + + if next_pos.translation.vector.x >= rad * 10.0 { + next_pos.translation.vector.x -= dt; + } + if next_pos.translation.vector.x <= -rad * 10.0 { + next_pos.translation.vector.x += dt; + } + + platform.set_next_kinematic_position(next_pos); + }); + + /* + * Run the simulation. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point2::new(0.0, 1.0), 40.0); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Kinematic body", init_world)]); + testbed.run() +} diff --git a/examples2d/pyramid2.rs b/examples2d/pyramid2.rs new file mode 100644 index 000000000..4d6fe0746 --- /dev/null +++ b/examples2d/pyramid2.rs @@ -0,0 +1,60 @@ +use na::Point2; +use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier2d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + /* + * Ground + */ + let ground_size = 100.0; + let ground_thickness = 1.0; + + let rigid_body = RigidBodyBuilder::new_static().build(); + let ground_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_thickness).build(); + colliders.insert(collider, ground_handle, &mut bodies); + + /* + * Create the cubes + */ + let num = 100; + let rad = 0.5; + + let shift = rad * 2.0; + let centerx = shift * (num as f32) / 2.0; + let centery = shift / 2.0 + ground_thickness + rad * 1.5; + + for i in 0usize..num { + for j in i..num { + let fj = j as f32; + let fi = i as f32; + let x = (fi * shift / 2.0) + (fj - fi) * shift - centerx; + let y = fi * shift + centery; + + // Build the rigid body. + let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad).density(1.0).build(); + colliders.insert(collider, handle, &mut bodies); + } + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point2::new(0.0, 2.5), 5.0); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Balls", init_world)]); + testbed.run() +} diff --git a/examples2d/sensor2.rs b/examples2d/sensor2.rs new file mode 100644 index 000000000..75634a8b8 --- /dev/null +++ b/examples2d/sensor2.rs @@ -0,0 +1,101 @@ +use na::{Point2, Point3}; +use rapier2d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier2d::geometry::{ColliderBuilder, ColliderSet, Proximity}; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + /* + * Ground. + */ + let ground_size = 200.1; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::new_static() + .translation(0.0, -ground_height) + .build(); + let ground_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height).build(); + colliders.insert(collider, ground_handle, &mut bodies); + + /* + * Create some boxes. + */ + let num = 10; + let rad = 0.2; + + let shift = rad * 2.0; + let centerx = shift * num as f32 / 2.0; + + for i in 0usize..num { + let x = i as f32 * shift - centerx; + let y = 3.0; + + // Build the rigid body. + let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad).density(1.0).build(); + colliders.insert(collider, handle, &mut bodies); + + testbed.set_body_color(handle, Point3::new(0.5, 0.5, 1.0)); + } + + /* + * Create a cube that will have a ball-shaped sensor attached. + */ + + // Rigid body so that the sensor can move. + let sensor = RigidBodyBuilder::new_dynamic() + .translation(0.0, 10.0) + .build(); + let sensor_handle = bodies.insert(sensor); + + // Solid cube attached to the sensor which + // other colliders can touch. + let collider = ColliderBuilder::cuboid(rad, rad).density(1.0).build(); + colliders.insert(collider, sensor_handle, &mut bodies); + + // We create a collider desc without density because we don't + // want it to contribute to the rigid body mass. + let sensor_collider = ColliderBuilder::ball(rad * 5.0).sensor(true).build(); + colliders.insert(sensor_collider, sensor_handle, &mut bodies); + + testbed.set_body_color(sensor_handle, Point3::new(0.5, 1.0, 1.0)); + + // Callback that will be executed on the main loop to handle proximities. + testbed.add_callback(move |_, colliders, events, graphics, _| { + while let Ok(prox) = events.proximity_events.try_recv() { + let color = match prox.new_status { + Proximity::WithinMargin | Proximity::Intersecting => Point3::new(1.0, 1.0, 0.0), + Proximity::Disjoint => Point3::new(0.5, 0.5, 1.0), + }; + + let parent_handle1 = colliders.get(prox.collider1).unwrap().parent(); + let parent_handle2 = colliders.get(prox.collider2).unwrap().parent(); + + if parent_handle1 != ground_handle && parent_handle1 != sensor_handle { + graphics.set_body_color(parent_handle1, color); + } + if parent_handle2 != ground_handle && parent_handle2 != sensor_handle { + graphics.set_body_color(parent_handle2, color); + } + } + }); + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point2::new(0.0, 1.0), 100.0); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Sensor", init_world)]); + testbed.run() +} diff --git a/examples2d/stress_joint_ball2.rs b/examples2d/stress_joint_ball2.rs new file mode 100644 index 000000000..55db1b787 --- /dev/null +++ b/examples2d/stress_joint_ball2.rs @@ -0,0 +1,72 @@ +use na::Point2; +use rapier2d::dynamics::{BallJoint, BodyStatus, JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier2d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut joints = JointSet::new(); + + /* + * Create the balls + */ + // Build the rigid body. + let rad = 0.4; + let numi = 100; // Num vertical nodes. + let numk = 100; // Num horizontal nodes. + let shift = 1.0; + + let mut body_handles = Vec::new(); + + for k in 0..numk { + for i in 0..numi { + let fk = k as f32; + let fi = i as f32; + + let status = if k >= numk / 2 - 3 && k <= numk / 2 + 3 && i == 0 { + BodyStatus::Static + } else { + BodyStatus::Dynamic + }; + + let rigid_body = RigidBodyBuilder::new(status) + .translation(fk * shift, -fi * shift) + .build(); + let child_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::ball(rad).density(1.0).build(); + colliders.insert(collider, child_handle, &mut bodies); + + // Vertical joint. + if i > 0 { + let parent_handle = *body_handles.last().unwrap(); + let joint = BallJoint::new(Point2::origin(), Point2::new(0.0, shift)); + joints.insert(&mut bodies, parent_handle, child_handle, joint); + } + + // Horizontal joint. + if k > 0 { + let parent_index = body_handles.len() - numi; + let parent_handle = body_handles[parent_index]; + let joint = BallJoint::new(Point2::origin(), Point2::new(-shift, 0.0)); + joints.insert(&mut bodies, parent_handle, child_handle, joint); + } + + body_handles.push(child_handle); + } + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point2::new(numk as f32 * rad, numi as f32 * -rad), 5.0); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]); + testbed.run() +} diff --git a/examples2d/stress_joint_fixed2.rs b/examples2d/stress_joint_fixed2.rs new file mode 100644 index 000000000..31b3e4ff9 --- /dev/null +++ b/examples2d/stress_joint_fixed2.rs @@ -0,0 +1,85 @@ +use na::{Isometry2, Point2}; +use rapier2d::dynamics::{BodyStatus, FixedJoint, JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier2d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut joints = JointSet::new(); + + /* + * Create the balls + */ + // Build the rigid body. + let rad = 0.4; + let num = 30; // Num vertical nodes. + let shift = 1.0; + + let mut body_handles = Vec::new(); + + for xx in 0..4 { + let x = xx as f32 * shift * (num as f32 + 2.0); + + for yy in 0..4 { + let y = yy as f32 * shift * (num as f32 + 4.0); + + for k in 0..num { + for i in 0..num { + let fk = k as f32; + let fi = i as f32; + + let status = if k == 0 { + BodyStatus::Static + } else { + BodyStatus::Dynamic + }; + + let rigid_body = RigidBodyBuilder::new(status) + .translation(x + fk * shift, y - fi * shift) + .build(); + let child_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::ball(rad).density(1.0).build(); + colliders.insert(collider, child_handle, &mut bodies); + + // Vertical joint. + if i > 0 { + let parent_handle = *body_handles.last().unwrap(); + let joint = FixedJoint::new( + Isometry2::identity(), + Isometry2::translation(0.0, shift), + ); + joints.insert(&mut bodies, parent_handle, child_handle, joint); + } + + // Horizontal joint. + if k > 0 { + let parent_index = body_handles.len() - num; + let parent_handle = body_handles[parent_index]; + let joint = FixedJoint::new( + Isometry2::identity(), + Isometry2::translation(-shift, 0.0), + ); + joints.insert(&mut bodies, parent_handle, child_handle, joint); + } + + body_handles.push(child_handle); + } + } + } + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point2::new(50.0, 50.0), 5.0); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]); + testbed.run() +} diff --git a/examples2d/stress_joint_prismatic2.rs b/examples2d/stress_joint_prismatic2.rs new file mode 100644 index 000000000..91ef48ed0 --- /dev/null +++ b/examples2d/stress_joint_prismatic2.rs @@ -0,0 +1,69 @@ +use na::{Point2, Unit, Vector2}; +use rapier2d::dynamics::{JointSet, PrismaticJoint, RigidBodyBuilder, RigidBodySet}; +use rapier2d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed2d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut joints = JointSet::new(); + + /* + * Create the balls + */ + // Build the rigid body. + let rad = 0.4; + let num = 10; + let shift = 1.0; + + for l in 0..25 { + let y = l as f32 * shift * (num as f32 + 2.0) * 2.0; + + for j in 0..50 { + let x = j as f32 * shift * 4.0; + + let ground = RigidBodyBuilder::new_static().translation(x, y).build(); + let mut curr_parent = bodies.insert(ground); + let collider = ColliderBuilder::cuboid(rad, rad).build(); + colliders.insert(collider, curr_parent, &mut bodies); + + for i in 0..num { + let y = y - (i + 1) as f32 * shift; + let density = 1.0; + let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y).build(); + let curr_child = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad).density(density).build(); + colliders.insert(collider, curr_child, &mut bodies); + + let axis = if i % 2 == 0 { + Unit::new_normalize(Vector2::new(1.0, 1.0)) + } else { + Unit::new_normalize(Vector2::new(-1.0, 1.0)) + }; + + let mut prism = + PrismaticJoint::new(Point2::origin(), axis, Point2::new(0.0, shift), axis); + prism.limits_enabled = true; + prism.limits[0] = -1.5; + prism.limits[1] = 1.5; + joints.insert(&mut bodies, curr_parent, curr_child, prism); + + curr_parent = curr_child; + } + } + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point2::new(80.0, 80.0), 15.0); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]); + testbed.run() +} diff --git a/examples3d/Cargo.toml b/examples3d/Cargo.toml new file mode 100644 index 000000000..317669609 --- /dev/null +++ b/examples3d/Cargo.toml @@ -0,0 +1,27 @@ +[package] +name = "nphysics-examples-3d" +version = "0.1.0" +authors = [ "Sébastien Crozet " ] +edition = "2018" + +[features] +parallel = [ "rapier3d/parallel", "rapier_testbed3d/parallel" ] +simd-stable = [ "rapier3d/simd-stable" ] +simd-nightly = [ "rapier3d/simd-nightly" ] +other-backends = [ "rapier_testbed3d/other-backends" ] +enhanced-determinism = [ "rapier3d/enhanced-determinism" ] + +[dependencies] +rand = "0.7" +Inflector = "0.11" +nalgebra = "0.22" + +[dependencies.rapier_testbed3d] +path = "../build/rapier_testbed3d" + +[dependencies.rapier3d] +path = "../build/rapier3d" + +[[bin]] +name = "all_examples3" +path = "./all_examples3.rs" \ No newline at end of file diff --git a/examples3d/all_examples3.rs b/examples3d/all_examples3.rs new file mode 100644 index 000000000..10b8f33ba --- /dev/null +++ b/examples3d/all_examples3.rs @@ -0,0 +1,110 @@ +#![allow(dead_code)] + +extern crate nalgebra as na; + +#[cfg(target_arch = "wasm32")] +use wasm_bindgen::prelude::*; + +use inflector::Inflector; + +use rapier_testbed3d::Testbed; +use std::cmp::Ordering; + +mod balls3; +mod boxes3; +mod capsules3; +mod debug_boxes3; +mod debug_triangle3; +mod domino3; +mod heightfield3; +mod joints3; +mod kinematic3; +mod pyramid3; +mod sensor3; +mod stacks3; +mod stress_joint_ball3; +mod stress_joint_fixed3; +mod stress_joint_prismatic3; +mod stress_joint_revolute3; +mod stress_keva3; +mod trimesh3; + +fn demo_name_from_command_line() -> Option { + let mut args = std::env::args(); + + while let Some(arg) = args.next() { + if &arg[..] == "--example" { + return args.next(); + } + } + + None +} + +#[cfg(any(target_arch = "wasm32", target_arch = "asmjs"))] +fn demo_name_from_url() -> Option { + None + // let window = stdweb::web::window(); + // let hash = window.location()?.search().ok()?; + // if hash.len() > 0 { + // Some(hash[1..].to_string()) + // } else { + // None + // } +} + +#[cfg(not(any(target_arch = "wasm32", target_arch = "asmjs")))] +fn demo_name_from_url() -> Option { + None +} + +#[cfg_attr(target_arch = "wasm32", wasm_bindgen(start))] +pub fn main() { + let demo = demo_name_from_command_line() + .or_else(|| demo_name_from_url()) + .unwrap_or(String::new()) + .to_camel_case(); + + let mut builders: Vec<(_, fn(&mut Testbed))> = vec![ + ("Balls", balls3::init_world), + ("Boxes", boxes3::init_world), + ("Capsules", capsules3::init_world), + ("Domino", domino3::init_world), + ("Heightfield", heightfield3::init_world), + ("Joints", joints3::init_world), + ("Kinematic", kinematic3::init_world), + ("Stacks", stacks3::init_world), + ("Pyramid", pyramid3::init_world), + ("Sensor", sensor3::init_world), + ("Trimesh", trimesh3::init_world), + ("(Debug) boxes", debug_boxes3::init_world), + ("(Debug) triangle", debug_triangle3::init_world), + ("(Stress test) joint ball", stress_joint_ball3::init_world), + ("(Stress test) joint fixed", stress_joint_fixed3::init_world), + ( + "(Stress test) joint revolute", + stress_joint_revolute3::init_world, + ), + ( + "(Stress test) joint prismatic", + stress_joint_prismatic3::init_world, + ), + ("(Stress test) keva tower", stress_keva3::init_world), + ]; + + // Lexicographic sort, with stress tests moved at the end of the list. + builders.sort_by(|a, b| match (a.0.starts_with("("), b.0.starts_with("(")) { + (true, true) | (false, false) => a.0.cmp(b.0), + (true, false) => Ordering::Greater, + (false, true) => Ordering::Less, + }); + + let i = builders + .iter() + .position(|builder| builder.0.to_camel_case().as_str() == demo.as_str()) + .unwrap_or(0); + + let testbed = Testbed::from_builders(i, builders); + + testbed.run() +} diff --git a/examples3d/balls3.rs b/examples3d/balls3.rs new file mode 100644 index 000000000..df0c21cb0 --- /dev/null +++ b/examples3d/balls3.rs @@ -0,0 +1,29 @@ +use na::Point3; +use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + let rb = RigidBodyBuilder::new_dynamic().build(); + let co = ColliderBuilder::cuboid(0.5, 0.5, 0.5).build(); + let h = bodies.insert(rb); + colliders.insert(co, h, &mut bodies); + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin()); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Balls", init_world)]); + testbed.run() +} diff --git a/examples3d/boxes3.rs b/examples3d/boxes3.rs new file mode 100644 index 000000000..eb8a472fb --- /dev/null +++ b/examples3d/boxes3.rs @@ -0,0 +1,68 @@ +use na::Point3; +use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + /* + * Ground + */ + let ground_size = 200.1; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::new_static() + .translation(0.0, -ground_height, 0.0) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + colliders.insert(collider, handle, &mut bodies); + + /* + * Create the cubes + */ + let num = 8; + let rad = 1.0; + + let shift = rad * 2.0 + rad; + let centerx = shift * (num / 2) as f32; + let centery = shift / 2.0; + let centerz = shift * (num / 2) as f32; + + let mut offset = -(num as f32) * (rad * 2.0 + rad) * 0.5; + + for j in 0usize..47 { + for i in 0..num { + for k in 0usize..num { + let x = i as f32 * shift - centerx + offset; + let y = j as f32 * shift + centery + 3.0; + let z = k as f32 * shift - centerz + offset; + + // Build the rigid body. + let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad, rad).density(1.0).build(); + colliders.insert(collider, handle, &mut bodies); + } + } + + offset -= 0.05 * rad * (num as f32 - 1.0); + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin()); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]); + testbed.run() +} diff --git a/examples3d/capsules3.rs b/examples3d/capsules3.rs new file mode 100644 index 000000000..e6aad407d --- /dev/null +++ b/examples3d/capsules3.rs @@ -0,0 +1,69 @@ +use na::Point3; +use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + /* + * Ground + */ + let ground_size = 200.1; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::new_static() + .translation(0.0, -ground_height, 0.0) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + colliders.insert(collider, handle, &mut bodies); + + /* + * Create the cubes + */ + let num = 8; + let rad = 1.0; + + let shift = rad * 2.0 + rad; + let shifty = rad * 4.0; + let centerx = shift * (num / 2) as f32; + let centery = shift / 2.0; + let centerz = shift * (num / 2) as f32; + + let mut offset = -(num as f32) * (rad * 2.0 + rad) * 0.5; + + for j in 0usize..47 { + for i in 0..num { + for k in 0usize..num { + let x = i as f32 * shift - centerx + offset; + let y = j as f32 * shifty + centery + 3.0; + let z = k as f32 * shift - centerz + offset; + + // Build the rigid body. + let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::capsule_y(rad, rad).density(1.0).build(); + colliders.insert(collider, handle, &mut bodies); + } + } + + offset -= 0.05 * rad * (num as f32 - 1.0); + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin()); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]); + testbed.run() +} diff --git a/examples3d/debug_boxes3.rs b/examples3d/debug_boxes3.rs new file mode 100644 index 000000000..bd7c3444c --- /dev/null +++ b/examples3d/debug_boxes3.rs @@ -0,0 +1,47 @@ +use na::{Point3, Vector3}; +use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + /* + * Ground + */ + let ground_size = 100.1; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::new_static() + .translation(0.0, -ground_height, 0.0) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + colliders.insert(collider, handle, &mut bodies); + + // Build the dynamic box rigid body. + let rigid_body = RigidBodyBuilder::new_dynamic() + .translation(1.1, 0.0, 0.0) + .rotation(Vector3::new(0.8, 0.2, 0.1)) + .can_sleep(false) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(2.0, 0.1, 1.0).density(1.0).build(); + colliders.insert(collider, handle, &mut bodies); + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point3::new(10.0, 10.0, 10.0), Point3::origin()); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]); + testbed.run() +} diff --git a/examples3d/debug_triangle3.rs b/examples3d/debug_triangle3.rs new file mode 100644 index 000000000..c9d7751f4 --- /dev/null +++ b/examples3d/debug_triangle3.rs @@ -0,0 +1,48 @@ +use na::Point3; +use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + // Triangle ground. + let vtx = [ + Point3::new(-10.0, 0.0, -10.0), + Point3::new(10.0, 0.0, -10.0), + Point3::new(0.0, 0.0, 10.0), + ]; + + let rigid_body = RigidBodyBuilder::new_static() + .translation(0.0, 0.0, 0.0) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::triangle(vtx[0], vtx[1], vtx[2]).build(); + colliders.insert(collider, handle, &mut bodies); + + // Dynamic box rigid body. + let rigid_body = RigidBodyBuilder::new_dynamic() + .translation(1.1, 0.01, 0.0) + // .rotation(Vector3::new(0.8, 0.2, 0.1)) + .can_sleep(false) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(20.0, 0.1, 1.0).density(1.0).build(); + colliders.insert(collider, handle, &mut bodies); + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point3::new(10.0, 10.0, 10.0), Point3::origin()); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]); + testbed.run() +} diff --git a/examples3d/domino3.rs b/examples3d/domino3.rs new file mode 100644 index 000000000..a62ba41e9 --- /dev/null +++ b/examples3d/domino3.rs @@ -0,0 +1,87 @@ +use na::{Point3, Translation3, UnitQuaternion, Vector3}; +use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + /* + * Ground + */ + let ground_size = 200.1; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::new_static() + .translation(0.0, -ground_height, 0.0) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + colliders.insert(collider, handle, &mut bodies); + + /* + * Create the cubes + */ + let num = 4000; + let width = 1.0; + let thickness = 0.1; + + let colors = [Point3::new(0.7, 0.5, 0.9), Point3::new(0.6, 1.0, 0.6)]; + + let mut curr_angle = 0.0; + let mut curr_rad = 10.0; + let mut prev_angle; + let mut skip = 0; + for i in 0..num { + let perimeter = 2.0 * std::f32::consts::PI * curr_rad; + let spacing = thickness * 4.0; + prev_angle = curr_angle; + curr_angle += 2.0 * std::f32::consts::PI * spacing / perimeter; + let (x, z) = curr_angle.sin_cos(); + + // Build the rigid body. + let two_pi = 2.0 * std::f32::consts::PI; + let nudged = curr_angle % two_pi < prev_angle % two_pi; + let tilt = if nudged || i == num - 1 { 0.2 } else { 0.0 }; + + if skip == 0 { + let rot = UnitQuaternion::new(Vector3::y() * curr_angle); + let tilt = UnitQuaternion::new(rot * Vector3::z() * tilt); + let position = + Translation3::new(x * curr_rad, width * 2.0 + ground_height, z * curr_rad) + * tilt + * rot; + let rigid_body = RigidBodyBuilder::new_dynamic().position(position).build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(thickness, width * 2.0, width) + .density(1.0) + .build(); + colliders.insert(collider, handle, &mut bodies); + testbed.set_body_color(handle, colors[i % 2]); + } else { + skip -= 1; + } + + if nudged { + skip = 5; + } + + curr_rad += 1.5 / perimeter; + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin()); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]); + testbed.run() +} diff --git a/examples3d/heightfield3.rs b/examples3d/heightfield3.rs new file mode 100644 index 000000000..f9d4e4940 --- /dev/null +++ b/examples3d/heightfield3.rs @@ -0,0 +1,78 @@ +use na::{DMatrix, Point3, Vector3}; +use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + /* + * Ground + */ + let ground_size = Vector3::new(200.0, 1.0, 200.0); + let nsubdivs = 20; + + let heights = DMatrix::from_fn(nsubdivs + 1, nsubdivs + 1, |i, j| { + if i == 0 || i == nsubdivs || j == 0 || j == nsubdivs { + 10.0 + } else { + let x = i as f32 * ground_size.x / (nsubdivs as f32); + let z = j as f32 * ground_size.z / (nsubdivs as f32); + x.sin() + z.cos() + } + }); + + let rigid_body = RigidBodyBuilder::new_static().build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::heightfield(heights, ground_size).build(); + colliders.insert(collider, handle, &mut bodies); + + /* + * Create the cubes + */ + let num = 8; + let rad = 1.0; + + let shift = rad * 2.0 + rad; + let centerx = shift * (num / 2) as f32; + let centery = shift / 2.0; + let centerz = shift * (num / 2) as f32; + + for j in 0usize..47 { + for i in 0..num { + for k in 0usize..num { + let x = i as f32 * shift - centerx; + let y = j as f32 * shift + centery + 3.0; + let z = k as f32 * shift - centerz; + + // Build the rigid body. + let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); + let handle = bodies.insert(rigid_body); + + if j % 2 == 0 { + let collider = ColliderBuilder::cuboid(rad, rad, rad).density(1.0).build(); + colliders.insert(collider, handle, &mut bodies); + } else { + let collider = ColliderBuilder::ball(rad).density(1.0).build(); + colliders.insert(collider, handle, &mut bodies); + } + } + } + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin()); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]); + testbed.run() +} diff --git a/examples3d/joints3.rs b/examples3d/joints3.rs new file mode 100644 index 000000000..c640770c0 --- /dev/null +++ b/examples3d/joints3.rs @@ -0,0 +1,272 @@ +use na::{Isometry3, Point3, Unit, Vector3}; +use rapier3d::dynamics::{ + BallJoint, BodyStatus, FixedJoint, JointSet, PrismaticJoint, RevoluteJoint, RigidBodyBuilder, + RigidBodySet, +}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed3d::Testbed; + +fn create_prismatic_joints( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + joints: &mut JointSet, + origin: Point3, + num: usize, +) { + let rad = 0.4; + let shift = 1.0; + + let ground = RigidBodyBuilder::new_static() + .translation(origin.x, origin.y, origin.z) + .build(); + let mut curr_parent = bodies.insert(ground); + let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + colliders.insert(collider, curr_parent, bodies); + + for i in 0..num { + let z = origin.z + (i + 1) as f32 * shift; + let density = 1.0; + let rigid_body = RigidBodyBuilder::new_dynamic() + .translation(origin.x, origin.y, z) + .build(); + let curr_child = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad, rad) + .density(density) + .build(); + colliders.insert(collider, curr_child, bodies); + + let axis = if i % 2 == 0 { + Unit::new_normalize(Vector3::new(1.0, 1.0, 0.0)) + } else { + Unit::new_normalize(Vector3::new(-1.0, 1.0, 0.0)) + }; + + let z = Vector3::z(); + let mut prism = PrismaticJoint::new( + Point3::origin(), + axis, + z, + Point3::new(0.0, 0.0, -shift), + axis, + z, + ); + prism.limits_enabled = true; + prism.limits[0] = -2.0; + prism.limits[1] = 2.0; + joints.insert(bodies, curr_parent, curr_child, prism); + + curr_parent = curr_child; + } +} + +fn create_revolute_joints( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + joints: &mut JointSet, + origin: Point3, + num: usize, +) { + let rad = 0.4; + let shift = 2.0; + + let ground = RigidBodyBuilder::new_static() + .translation(origin.x, origin.y, 0.0) + .build(); + let mut curr_parent = bodies.insert(ground); + let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + colliders.insert(collider, curr_parent, bodies); + + for i in 0..num { + // Create four bodies. + let z = origin.z + i as f32 * shift * 2.0 + shift; + let positions = [ + Isometry3::translation(origin.x, origin.y, z), + Isometry3::translation(origin.x + shift, origin.y, z), + Isometry3::translation(origin.x + shift, origin.y, z + shift), + Isometry3::translation(origin.x, origin.y, z + shift), + ]; + + let mut handles = [curr_parent; 4]; + for k in 0..4 { + let density = 1.0; + let rigid_body = RigidBodyBuilder::new_dynamic() + .position(positions[k]) + .build(); + handles[k] = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad, rad) + .density(density) + .build(); + colliders.insert(collider, handles[k], bodies); + } + + // Setup four joints. + let o = Point3::origin(); + let x = Vector3::x_axis(); + let z = Vector3::z_axis(); + + let revs = [ + RevoluteJoint::new(o, z, Point3::new(0.0, 0.0, -shift), z), + RevoluteJoint::new(o, x, Point3::new(-shift, 0.0, 0.0), x), + RevoluteJoint::new(o, z, Point3::new(0.0, 0.0, -shift), z), + RevoluteJoint::new(o, x, Point3::new(shift, 0.0, 0.0), x), + ]; + + joints.insert(bodies, curr_parent, handles[0], revs[0]); + joints.insert(bodies, handles[0], handles[1], revs[1]); + joints.insert(bodies, handles[1], handles[2], revs[2]); + joints.insert(bodies, handles[2], handles[3], revs[3]); + + curr_parent = handles[3]; + } +} + +fn create_fixed_joints( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + joints: &mut JointSet, + origin: Point3, + num: usize, +) { + let rad = 0.4; + let shift = 1.0; + + let mut body_handles = Vec::new(); + + for k in 0..num { + for i in 0..num { + let fk = k as f32; + let fi = i as f32; + + // NOTE: the num - 2 test is to avoid two consecutive + // fixed bodies. Because physx will crash if we add + // a joint between these. + let status = if i == 0 && (k % 4 == 0 && k != num - 2 || k == num - 1) { + BodyStatus::Static + } else { + BodyStatus::Dynamic + }; + + let rigid_body = RigidBodyBuilder::new(status) + .translation(origin.x + fk * shift, origin.y, origin.z + fi * shift) + .build(); + let child_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::ball(rad).density(1.0).build(); + colliders.insert(collider, child_handle, bodies); + + // Vertical joint. + if i > 0 { + let parent_handle = *body_handles.last().unwrap(); + let joint = FixedJoint::new( + Isometry3::identity(), + Isometry3::translation(0.0, 0.0, -shift), + ); + joints.insert(bodies, parent_handle, child_handle, joint); + } + + // Horizontal joint. + if k > 0 { + let parent_index = body_handles.len() - num; + let parent_handle = body_handles[parent_index]; + let joint = FixedJoint::new( + Isometry3::identity(), + Isometry3::translation(-shift, 0.0, 0.0), + ); + joints.insert(bodies, parent_handle, child_handle, joint); + } + + body_handles.push(child_handle); + } + } +} + +fn create_ball_joints( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + joints: &mut JointSet, + num: usize, +) { + let rad = 0.4; + let shift = 1.0; + + let mut body_handles = Vec::new(); + + for k in 0..num { + for i in 0..num { + let fk = k as f32; + let fi = i as f32; + + let status = if i == 0 && (k % 4 == 0 || k == num - 1) { + BodyStatus::Static + } else { + BodyStatus::Dynamic + }; + + let rigid_body = RigidBodyBuilder::new(status) + .translation(fk * shift, 0.0, fi * shift) + .build(); + let child_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::ball(rad).density(1.0).build(); + colliders.insert(collider, child_handle, bodies); + + // Vertical joint. + if i > 0 { + let parent_handle = *body_handles.last().unwrap(); + let joint = BallJoint::new(Point3::origin(), Point3::new(0.0, 0.0, -shift)); + joints.insert(bodies, parent_handle, child_handle, joint); + } + + // Horizontal joint. + if k > 0 { + let parent_index = body_handles.len() - num; + let parent_handle = body_handles[parent_index]; + let joint = BallJoint::new(Point3::origin(), Point3::new(-shift, 0.0, 0.0)); + joints.insert(bodies, parent_handle, child_handle, joint); + } + + body_handles.push(child_handle); + } + } +} + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut joints = JointSet::new(); + + create_prismatic_joints( + &mut bodies, + &mut colliders, + &mut joints, + Point3::new(20.0, 10.0, 0.0), + 5, + ); + create_revolute_joints( + &mut bodies, + &mut colliders, + &mut joints, + Point3::new(20.0, 0.0, 0.0), + 3, + ); + create_fixed_joints( + &mut bodies, + &mut colliders, + &mut joints, + Point3::new(0.0, 10.0, 0.0), + 5, + ); + create_ball_joints(&mut bodies, &mut colliders, &mut joints, 15); + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point3::new(15.0, 5.0, 42.0), Point3::new(13.0, 1.0, 1.0)); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]); + testbed.run() +} diff --git a/examples3d/kinematic3.rs b/examples3d/kinematic3.rs new file mode 100644 index 000000000..d6f20147c --- /dev/null +++ b/examples3d/kinematic3.rs @@ -0,0 +1,97 @@ +use na::Point3; +use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + /* + * Ground. + */ + let ground_size = 10.0; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::new_static() + .translation(0.0, -ground_height, 0.0) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + colliders.insert(collider, handle, &mut bodies); + + /* + * Create the boxes + */ + let num = 6; + let rad = 0.2; + + let shift = rad * 2.0; + let centerx = shift * num as f32 / 2.0; + let centery = shift / 2.0 + 3.04; + let centerz = shift * num as f32 / 2.0; + + for i in 0usize..num { + for j in 0usize..num { + for k in 0usize..num { + let x = i as f32 * shift - centerx; + let y = j as f32 * shift + centery; + let z = k as f32 * shift - centerz; + + // Build the rigid body. + let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad, rad).density(1.0).build(); + colliders.insert(collider, handle, &mut bodies); + } + } + } + + /* + * Setup a kinematic rigid body. + */ + let platform_body = RigidBodyBuilder::new_kinematic() + .translation(0.0, 1.5 + 0.8, -10.0 * rad) + .build(); + let platform_handle = bodies.insert(platform_body); + let collider = ColliderBuilder::cuboid(rad * 10.0, rad, rad * 10.0) + .density(1.0) + .build(); + colliders.insert(collider, platform_handle, &mut bodies); + + /* + * Setup a callback to control the platform. + */ + testbed.add_callback(move |bodies, _, _, _, time| { + let mut platform = bodies.get_mut(platform_handle).unwrap(); + let mut next_pos = platform.position; + + let dt = 0.016; + next_pos.translation.vector.y += (time * 5.0).sin() * dt; + next_pos.translation.vector.z += time.sin() * 5.0 * dt; + + if next_pos.translation.vector.z >= rad * 10.0 { + next_pos.translation.vector.z -= dt; + } + if next_pos.translation.vector.z <= -rad * 10.0 { + next_pos.translation.vector.z += dt; + } + + platform.set_next_kinematic_position(next_pos); + }); + + /* + * Run the simulation. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point3::new(-10.0, 5.0, -10.0), Point3::origin()); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Kinematic body", init_world)]); + testbed.run() +} diff --git a/examples3d/pyramid3.rs b/examples3d/pyramid3.rs new file mode 100644 index 000000000..e2aae39e4 --- /dev/null +++ b/examples3d/pyramid3.rs @@ -0,0 +1,85 @@ +use na::{Point3, Vector3}; +use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed3d::Testbed; + +fn create_pyramid( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + offset: Vector3, + stack_height: usize, + half_extents: Vector3, +) { + let shift = half_extents * 2.5; + for i in 0usize..stack_height { + for j in i..stack_height { + for k in i..stack_height { + let fi = i as f32; + let fj = j as f32; + let fk = k as f32; + let x = (fi * shift.x / 2.0) + (fk - fi) * shift.x + offset.x + - stack_height as f32 * half_extents.x; + let y = fi * shift.y + offset.y; + let z = (fi * shift.z / 2.0) + (fj - fi) * shift.z + offset.z + - stack_height as f32 * half_extents.z; + + // Build the rigid body. + let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); + let rigid_body_handle = bodies.insert(rigid_body); + + let collider = + ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z) + .density(1.0) + .build(); + colliders.insert(collider, rigid_body_handle, bodies); + } + } + } +} + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + /* + * Ground + */ + let ground_size = 50.0; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::new_static() + .translation(0.0, -ground_height, 0.0) + .build(); + let ground_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + colliders.insert(collider, ground_handle, &mut bodies); + + /* + * Create the cubes + */ + let cube_size = 1.0; + let hext = Vector3::repeat(cube_size); + let bottomy = cube_size; + create_pyramid( + &mut bodies, + &mut colliders, + Vector3::new(0.0, bottomy, 0.0), + 24, + hext, + ); + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin()); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]); + testbed.run() +} diff --git a/examples3d/sensor3.rs b/examples3d/sensor3.rs new file mode 100644 index 000000000..50c051f4c --- /dev/null +++ b/examples3d/sensor3.rs @@ -0,0 +1,105 @@ +use na::Point3; +use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet, Proximity}; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + /* + * Ground. + */ + let ground_size = 200.1; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::new_static() + .translation(0.0, -ground_height, 0.0) + .build(); + let ground_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + colliders.insert(collider, ground_handle, &mut bodies); + + /* + * Create some boxes. + */ + let num = 10; + let rad = 0.2; + + let shift = rad * 2.0; + let centerx = shift * num as f32 / 2.0; + let centerz = shift * num as f32 / 2.0; + + for i in 0usize..num { + for k in 0usize..num { + let x = i as f32 * shift - centerx; + let y = 3.0; + let z = k as f32 * shift - centerz; + + // Build the rigid body. + let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad, rad).density(1.0).build(); + colliders.insert(collider, handle, &mut bodies); + + testbed.set_body_color(handle, Point3::new(0.5, 0.5, 1.0)); + } + } + + /* + * Create a cube that will have a ball-shaped sensor attached. + */ + + // Rigid body so that the sensor can move. + let sensor = RigidBodyBuilder::new_dynamic() + .translation(0.0, 10.0, 0.0) + .build(); + let sensor_handle = bodies.insert(sensor); + + // Solid cube attached to the sensor which + // other colliders can touch. + let collider = ColliderBuilder::cuboid(rad, rad, rad).density(1.0).build(); + colliders.insert(collider, sensor_handle, &mut bodies); + + // We create a collider desc without density because we don't + // want it to contribute to the rigid body mass. + let sensor_collider = ColliderBuilder::ball(rad * 5.0).sensor(true).build(); + colliders.insert(sensor_collider, sensor_handle, &mut bodies); + + testbed.set_body_color(sensor_handle, Point3::new(0.5, 1.0, 1.0)); + + // Callback that will be executed on the main loop to handle proximities. + testbed.add_callback(move |_, colliders, events, graphics, _| { + while let Ok(prox) = events.proximity_events.try_recv() { + let color = match prox.new_status { + Proximity::WithinMargin | Proximity::Intersecting => Point3::new(1.0, 1.0, 0.0), + Proximity::Disjoint => Point3::new(0.5, 0.5, 1.0), + }; + + let parent_handle1 = colliders.get(prox.collider1).unwrap().parent(); + let parent_handle2 = colliders.get(prox.collider2).unwrap().parent(); + + if parent_handle1 != ground_handle && parent_handle1 != sensor_handle { + graphics.set_body_color(parent_handle1, color); + } + if parent_handle2 != ground_handle && parent_handle2 != sensor_handle { + graphics.set_body_color(parent_handle2, color); + } + } + }); + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point3::new(-6.0, 4.0, -6.0), Point3::new(0.0, 1.0, 0.0)); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Sensor", init_world)]); + testbed.run() +} diff --git a/examples3d/stacks3.rs b/examples3d/stacks3.rs new file mode 100644 index 000000000..fd2ae27f0 --- /dev/null +++ b/examples3d/stacks3.rs @@ -0,0 +1,195 @@ +use na::{Point3, Translation3, UnitQuaternion, Vector3}; +use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed3d::Testbed; + +fn create_tower_circle( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + offset: Vector3, + stack_height: usize, + nsubdivs: usize, + half_extents: Vector3, +) { + let ang_step = std::f32::consts::PI * 2.0 / nsubdivs as f32; + let radius = 1.3 * nsubdivs as f32 * half_extents.x / std::f32::consts::PI; + + let shift = half_extents * 2.0; + for i in 0usize..stack_height { + for j in 0..nsubdivs { + let fj = j as f32; + let fi = i as f32; + let y = fi * shift.y; + let pos = Translation3::from(offset) + * UnitQuaternion::new(Vector3::y() * (fi / 2.0 + fj) * ang_step) + * Translation3::new(0.0, y, radius); + + // Build the rigid body. + let rigid_body = RigidBodyBuilder::new_dynamic().position(pos).build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z) + .density(1.0) + .build(); + colliders.insert(collider, handle, bodies); + } + } +} + +fn create_wall( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + offset: Vector3, + stack_height: usize, + half_extents: Vector3, +) { + let shift = half_extents * 2.0; + for i in 0usize..stack_height { + for j in i..stack_height { + let fj = j as f32; + let fi = i as f32; + let x = offset.x; + let y = fi * shift.y + offset.y; + let z = (fi * shift.z / 2.0) + (fj - fi) * shift.z + offset.z + - stack_height as f32 * half_extents.z; + + // Build the rigid body. + let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z) + .density(1.0) + .build(); + colliders.insert(collider, handle, bodies); + } + } +} + +fn create_pyramid( + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + offset: Vector3, + stack_height: usize, + half_extents: Vector3, +) { + let shift = half_extents * 2.0; + + for i in 0usize..stack_height { + for j in i..stack_height { + for k in i..stack_height { + let fi = i as f32; + let fj = j as f32; + let fk = k as f32; + let x = (fi * shift.x / 2.0) + (fk - fi) * shift.x + offset.x + - stack_height as f32 * half_extents.x; + let y = fi * shift.y + offset.y; + let z = (fi * shift.z / 2.0) + (fj - fi) * shift.z + offset.z + - stack_height as f32 * half_extents.z; + + // Build the rigid body. + let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); + let handle = bodies.insert(rigid_body); + let collider = + ColliderBuilder::cuboid(half_extents.x, half_extents.y, half_extents.z) + .density(1.0) + .build(); + colliders.insert(collider, handle, bodies); + } + } + } +} + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + /* + * Ground + */ + let ground_size = 200.0; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::new_static() + .translation(0.0, -ground_height, 0.0) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + colliders.insert(collider, handle, &mut bodies); + + /* + * Create the cubes + */ + let cube_size = 1.0; + let hext = Vector3::repeat(cube_size); + let bottomy = cube_size * 50.0; + create_pyramid( + &mut bodies, + &mut colliders, + Vector3::new(-110.0, bottomy, 0.0), + 12, + hext, + ); + create_pyramid( + &mut bodies, + &mut colliders, + Vector3::new(-80.0, bottomy, 0.0), + 12, + hext, + ); + create_pyramid( + &mut bodies, + &mut colliders, + Vector3::new(-50.0, bottomy, 0.0), + 12, + hext, + ); + create_pyramid( + &mut bodies, + &mut colliders, + Vector3::new(-20.0, bottomy, 0.0), + 12, + hext, + ); + create_wall( + &mut bodies, + &mut colliders, + Vector3::new(-2.0, bottomy, 0.0), + 12, + hext, + ); + create_wall( + &mut bodies, + &mut colliders, + Vector3::new(4.0, bottomy, 0.0), + 12, + hext, + ); + create_wall( + &mut bodies, + &mut colliders, + Vector3::new(10.0, bottomy, 0.0), + 12, + hext, + ); + create_tower_circle( + &mut bodies, + &mut colliders, + Vector3::new(25.0, bottomy, 0.0), + 8, + 24, + hext, + ); + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin()); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]); + testbed.run() +} diff --git a/examples3d/stress_joint_ball3.rs b/examples3d/stress_joint_ball3.rs new file mode 100644 index 000000000..bbff217cc --- /dev/null +++ b/examples3d/stress_joint_ball3.rs @@ -0,0 +1,70 @@ +use na::Point3; +use rapier3d::dynamics::{BallJoint, BodyStatus, JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut joints = JointSet::new(); + + let rad = 0.4; + let num = 100; + let shift = 1.0; + + let mut body_handles = Vec::new(); + + for k in 0..num { + for i in 0..num { + let fk = k as f32; + let fi = i as f32; + + let status = if i == 0 && (k % 4 == 0 || k == num - 1) { + BodyStatus::Static + } else { + BodyStatus::Dynamic + }; + + let rigid_body = RigidBodyBuilder::new(status) + .translation(fk * shift, 0.0, fi * shift) + .build(); + let child_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::ball(rad).density(1.0).build(); + colliders.insert(collider, child_handle, &mut bodies); + + // Vertical joint. + if i > 0 { + let parent_handle = *body_handles.last().unwrap(); + let joint = BallJoint::new(Point3::origin(), Point3::new(0.0, 0.0, -shift)); + joints.insert(&mut bodies, parent_handle, child_handle, joint); + } + + // Horizontal joint. + if k > 0 { + let parent_index = body_handles.len() - num; + let parent_handle = body_handles[parent_index]; + let joint = BallJoint::new(Point3::origin(), Point3::new(-shift, 0.0, 0.0)); + joints.insert(&mut bodies, parent_handle, child_handle, joint); + } + + body_handles.push(child_handle); + } + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at( + Point3::new(-110.0, -46.0, 170.0), + Point3::new(54.0, -38.0, 29.0), + ); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]); + testbed.run() +} diff --git a/examples3d/stress_joint_fixed3.rs b/examples3d/stress_joint_fixed3.rs new file mode 100644 index 000000000..b85879dd1 --- /dev/null +++ b/examples3d/stress_joint_fixed3.rs @@ -0,0 +1,92 @@ +use na::{Isometry3, Point3}; +use rapier3d::dynamics::{BodyStatus, FixedJoint, JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut joints = JointSet::new(); + + let rad = 0.4; + let num = 5; + let shift = 1.0; + + let mut body_handles = Vec::new(); + + for m in 0..10 { + let z = m as f32 * shift * (num as f32 + 2.0); + + for l in 0..10 { + let y = l as f32 * shift * 3.0; + + for j in 0..5 { + let x = j as f32 * shift * (num as f32) * 2.0; + + for k in 0..num { + for i in 0..num { + let fk = k as f32; + let fi = i as f32; + + // NOTE: the num - 2 test is to avoid two consecutive + // fixed bodies. Because physx will crash if we add + // a joint between these. + + let status = if i == 0 && (k % 4 == 0 && k != num - 2 || k == num - 1) { + BodyStatus::Static + } else { + BodyStatus::Dynamic + }; + + let rigid_body = RigidBodyBuilder::new(status) + .translation(x + fk * shift, y, z + fi * shift) + .build(); + let child_handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::ball(rad).density(1.0).build(); + colliders.insert(collider, child_handle, &mut bodies); + + // Vertical joint. + if i > 0 { + let parent_handle = *body_handles.last().unwrap(); + let joint = FixedJoint::new( + Isometry3::identity(), + Isometry3::translation(0.0, 0.0, -shift), + ); + joints.insert(&mut bodies, parent_handle, child_handle, joint); + } + + // Horizontal joint. + if k > 0 { + let parent_index = body_handles.len() - num; + let parent_handle = body_handles[parent_index]; + let joint = FixedJoint::new( + Isometry3::identity(), + Isometry3::translation(-shift, 0.0, 0.0), + ); + joints.insert(&mut bodies, parent_handle, child_handle, joint); + } + + body_handles.push(child_handle); + } + } + } + } + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at( + Point3::new(-38.0, 14.0, 108.0), + Point3::new(46.0, 12.0, 23.0), + ); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]); + testbed.run() +} diff --git a/examples3d/stress_joint_prismatic3.rs b/examples3d/stress_joint_prismatic3.rs new file mode 100644 index 000000000..3267adafd --- /dev/null +++ b/examples3d/stress_joint_prismatic3.rs @@ -0,0 +1,81 @@ +use na::{Point3, Unit, Vector3}; +use rapier3d::dynamics::{JointSet, PrismaticJoint, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut joints = JointSet::new(); + + let rad = 0.4; + let num = 5; + let shift = 1.0; + + for m in 0..8 { + let z = m as f32 * shift * (num as f32 + 2.0); + + for l in 0..8 { + let y = l as f32 * shift * (num as f32) * 2.0; + + for j in 0..50 { + let x = j as f32 * shift * 4.0; + + let ground = RigidBodyBuilder::new_static().translation(x, y, z).build(); + let mut curr_parent = bodies.insert(ground); + let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + colliders.insert(collider, curr_parent, &mut bodies); + + for i in 0..num { + let z = z + (i + 1) as f32 * shift; + let density = 1.0; + let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); + let curr_child = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad, rad) + .density(density) + .build(); + colliders.insert(collider, curr_child, &mut bodies); + + let axis = if i % 2 == 0 { + Unit::new_normalize(Vector3::new(1.0, 1.0, 0.0)) + } else { + Unit::new_normalize(Vector3::new(-1.0, 1.0, 0.0)) + }; + + let z = Vector3::z(); + let mut prism = PrismaticJoint::new( + Point3::origin(), + axis, + z, + Point3::new(0.0, 0.0, -shift), + axis, + z, + ); + prism.limits_enabled = true; + prism.limits[0] = -2.0; + prism.limits[1] = 2.0; + joints.insert(&mut bodies, curr_parent, curr_child, prism); + + curr_parent = curr_child; + } + } + } + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at( + Point3::new(262.0, 63.0, 124.0), + Point3::new(101.0, 4.0, -3.0), + ); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]); + testbed.run() +} diff --git a/examples3d/stress_joint_revolute3.rs b/examples3d/stress_joint_revolute3.rs new file mode 100644 index 000000000..040fc3e7c --- /dev/null +++ b/examples3d/stress_joint_revolute3.rs @@ -0,0 +1,89 @@ +use na::{Isometry3, Point3, Vector3}; +use rapier3d::dynamics::{JointSet, RevoluteJoint, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let mut joints = JointSet::new(); + + let rad = 0.4; + let num = 10; + let shift = 2.0; + + for l in 0..4 { + let y = l as f32 * shift * (num as f32) * 3.0; + + for j in 0..50 { + let x = j as f32 * shift * 4.0; + + let ground = RigidBodyBuilder::new_static() + .translation(x, y, 0.0) + .build(); + let mut curr_parent = bodies.insert(ground); + let collider = ColliderBuilder::cuboid(rad, rad, rad).build(); + colliders.insert(collider, curr_parent, &mut bodies); + + for i in 0..num { + // Create four bodies. + let z = i as f32 * shift * 2.0 + shift; + let positions = [ + Isometry3::translation(x, y, z), + Isometry3::translation(x + shift, y, z), + Isometry3::translation(x + shift, y, z + shift), + Isometry3::translation(x, y, z + shift), + ]; + + let mut handles = [curr_parent; 4]; + for k in 0..4 { + let density = 1.0; + let rigid_body = RigidBodyBuilder::new_dynamic() + .position(positions[k]) + .build(); + handles[k] = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(rad, rad, rad) + .density(density) + .build(); + colliders.insert(collider, handles[k], &mut bodies); + } + + // Setup four joints. + let o = Point3::origin(); + let x = Vector3::x_axis(); + let z = Vector3::z_axis(); + + let revs = [ + RevoluteJoint::new(o, z, Point3::new(0.0, 0.0, -shift), z), + RevoluteJoint::new(o, x, Point3::new(-shift, 0.0, 0.0), x), + RevoluteJoint::new(o, z, Point3::new(0.0, 0.0, -shift), z), + RevoluteJoint::new(o, x, Point3::new(shift, 0.0, 0.0), x), + ]; + + joints.insert(&mut bodies, curr_parent, handles[0], revs[0]); + joints.insert(&mut bodies, handles[0], handles[1], revs[1]); + joints.insert(&mut bodies, handles[1], handles[2], revs[2]); + joints.insert(&mut bodies, handles[2], handles[3], revs[3]); + + curr_parent = handles[3]; + } + } + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at( + Point3::new(478.0, 83.0, 228.0), + Point3::new(134.0, 83.0, -116.0), + ); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Joints", init_world)]); + testbed.run() +} diff --git a/examples3d/stress_keva3.rs b/examples3d/stress_keva3.rs new file mode 100644 index 000000000..2a41e4709 --- /dev/null +++ b/examples3d/stress_keva3.rs @@ -0,0 +1,148 @@ +use na::{Point3, Vector3}; +use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed3d::Testbed; + +pub fn build_block( + testbed: &mut Testbed, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + half_extents: Vector3, + shift: Vector3, + mut numx: usize, + numy: usize, + mut numz: usize, +) { + let dimensions = [half_extents.xyz(), half_extents.zyx()]; + let block_width = 2.0 * half_extents.z * numx as f32; + let block_height = 2.0 * half_extents.y * numy as f32; + let spacing = (half_extents.z * numx as f32 - half_extents.x) / (numz as f32 - 1.0); + let mut color0 = Point3::new(0.7, 0.5, 0.9); + let mut color1 = Point3::new(0.6, 1.0, 0.6); + + for i in 0..numy { + std::mem::swap(&mut numx, &mut numz); + let dim = dimensions[i % 2]; + let y = dim.y * i as f32 * 2.0; + + for j in 0..numx { + let x = if i % 2 == 0 { + spacing * j as f32 * 2.0 + } else { + dim.x * j as f32 * 2.0 + }; + + for k in 0..numz { + let z = if i % 2 == 0 { + dim.z * k as f32 * 2.0 + } else { + spacing * k as f32 * 2.0 + }; + + // Build the rigid body. + let rigid_body = RigidBodyBuilder::new_dynamic() + .translation( + x + dim.x + shift.x, + y + dim.y + shift.y, + z + dim.z + shift.z, + ) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z) + .density(1.0) + .build(); + colliders.insert(collider, handle, bodies); + + testbed.set_body_color(handle, color0); + std::mem::swap(&mut color0, &mut color1); + } + } + } + + // Close the top. + let dim = half_extents.zxy(); + + for i in 0..(block_width / (dim.x as f32 * 2.0)) as usize { + for j in 0..(block_width / (dim.z as f32 * 2.0)) as usize { + // Build the rigid body. + let rigid_body = RigidBodyBuilder::new_dynamic() + .translation( + i as f32 * dim.x * 2.0 + dim.x + shift.x, + dim.y + shift.y + block_height, + j as f32 * dim.z * 2.0 + dim.z + shift.z, + ) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(dim.x, dim.y, dim.z) + .density(1.0) + .build(); + colliders.insert(collider, handle, bodies); + testbed.set_body_color(handle, color0); + std::mem::swap(&mut color0, &mut color1); + } + } +} + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + /* + * Ground + */ + let ground_size = 50.0; + let ground_height = 0.1; + + let rigid_body = RigidBodyBuilder::new_static() + .translation(0.0, -ground_height, 0.0) + .build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::cuboid(ground_size, ground_height, ground_size).build(); + colliders.insert(collider, handle, &mut bodies); + + /* + * Create the cubes + */ + let half_extents = Vector3::new(0.02, 0.1, 0.4) / 2.0 * 10.0; + let mut block_height = 0.0; + // These should only be set to odd values otherwise + // the blocks won't align in the nicest way. + let numy = [0, 9, 13, 17, 21, 41]; + let mut num_blocks_built = 0; + + for i in (1..=5).rev() { + let numx = i; + let numy = numy[i]; + let numz = numx * 3 + 1; + let block_width = numx as f32 * half_extents.z * 2.0; + build_block( + testbed, + &mut bodies, + &mut colliders, + half_extents, + Vector3::new(-block_width / 2.0, block_height, -block_width / 2.0), + numx, + numy, + numz, + ); + block_height += numy as f32 * half_extents.y * 2.0 + half_extents.x * 2.0; + num_blocks_built += numx * numy * numz; + } + + println!("Num keva blocks: {}", num_blocks_built); + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin()); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]); + testbed.run() +} diff --git a/examples3d/trimesh3.rs b/examples3d/trimesh3.rs new file mode 100644 index 000000000..c8f564b9c --- /dev/null +++ b/examples3d/trimesh3.rs @@ -0,0 +1,88 @@ +use na::Point3; +use rapier3d::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; +use rapier3d::geometry::{ColliderBuilder, ColliderSet}; +use rapier_testbed3d::Testbed; + +pub fn init_world(testbed: &mut Testbed) { + /* + * World + */ + let mut bodies = RigidBodySet::new(); + let mut colliders = ColliderSet::new(); + let joints = JointSet::new(); + + /* + * Ground + */ + let ground_size = 200.0f32; + let ground_height = 1.0; + let nsubdivs = 20; + + let quad = rapier3d::ncollide::procedural::quad(ground_size, ground_size, nsubdivs, nsubdivs); + let indices = quad + .flat_indices() + .chunks(3) + .map(|is| Point3::new(is[0], is[2], is[1])) + .collect(); + let mut vertices = quad.coords; + + // ncollide generates a quad with `z` as the normal. + // so we switch z and y here and set a random altitude at each point. + for p in vertices.iter_mut() { + p.z = p.y; + p.y = (p.x.sin() + p.z.cos()) * ground_height; + + if p.x.abs() == ground_size / 2.0 || p.z.abs() == ground_size / 2.0 { + p.y = 10.0; + } + } + + let rigid_body = RigidBodyBuilder::new_static().build(); + let handle = bodies.insert(rigid_body); + let collider = ColliderBuilder::trimesh(vertices, indices).build(); + colliders.insert(collider, handle, &mut bodies); + + /* + * Create the cubes + */ + let num = 8; + let rad = 1.0; + + let shift = rad * 2.0 + rad; + let centerx = shift * (num / 2) as f32; + let centery = shift / 2.0; + let centerz = shift * (num / 2) as f32; + + for j in 0usize..47 { + for i in 0..num { + for k in 0usize..num { + let x = i as f32 * shift - centerx; + let y = j as f32 * shift + centery + 3.0; + let z = k as f32 * shift - centerz; + + // Build the rigid body. + let rigid_body = RigidBodyBuilder::new_dynamic().translation(x, y, z).build(); + let handle = bodies.insert(rigid_body); + + if j % 2 == 0 { + let collider = ColliderBuilder::cuboid(rad, rad, rad).density(1.0).build(); + colliders.insert(collider, handle, &mut bodies); + } else { + let collider = ColliderBuilder::ball(rad).density(1.0).build(); + colliders.insert(collider, handle, &mut bodies); + } + } + } + } + + /* + * Set up the testbed. + */ + testbed.set_world(bodies, colliders, joints); + testbed.look_at(Point3::new(100.0, 100.0, 100.0), Point3::origin()); +} + +fn main() { + let testbed = Testbed::from_builders(0, vec![("Boxes", init_world)]); + testbed.run() +} diff --git a/src/counters/ccd_counters.rs b/src/counters/ccd_counters.rs new file mode 100644 index 000000000..682adfcb7 --- /dev/null +++ b/src/counters/ccd_counters.rs @@ -0,0 +1,49 @@ +use crate::counters::Timer; +use std::fmt::{Display, Formatter, Result}; + +/// Performance counters related to continuous collision detection (CCD). +#[derive(Default, Clone, Copy)] +pub struct CCDCounters { + /// The number of substeps actually performed by the CCD resolution. + pub num_substeps: usize, + /// The total time spent for TOI computation in the CCD resolution. + pub toi_computation_time: Timer, + /// The total time spent for force computation and integration in the CCD resolution. + pub solver_time: Timer, + /// The total time spent by the broad-phase in the CCD resolution. + pub broad_phase_time: Timer, + /// The total time spent by the narrow-phase in the CCD resolution. + pub narrow_phase_time: Timer, +} + +impl CCDCounters { + /// Creates a new counter initialized to zero. + pub fn new() -> Self { + CCDCounters { + num_substeps: 0, + toi_computation_time: Timer::new(), + solver_time: Timer::new(), + broad_phase_time: Timer::new(), + narrow_phase_time: Timer::new(), + } + } + + /// Resets this counter to 0. + pub fn reset(&mut self) { + self.num_substeps = 0; + self.toi_computation_time.reset(); + self.solver_time.reset(); + self.broad_phase_time.reset(); + self.narrow_phase_time.reset(); + } +} + +impl Display for CCDCounters { + fn fmt(&self, f: &mut Formatter) -> Result { + writeln!(f, "Number of substeps: {}", self.num_substeps)?; + writeln!(f, "TOI computation time: {}", self.toi_computation_time)?; + writeln!(f, "Constraints solver time: {}", self.solver_time)?; + writeln!(f, "Broad-phase time: {}", self.broad_phase_time)?; + writeln!(f, "Narrow-phase time: {}", self.narrow_phase_time) + } +} diff --git a/src/counters/collision_detection_counters.rs b/src/counters/collision_detection_counters.rs new file mode 100644 index 000000000..d164452a4 --- /dev/null +++ b/src/counters/collision_detection_counters.rs @@ -0,0 +1,32 @@ +use crate::counters::Timer; +use std::fmt::{Display, Formatter, Result}; + +/// Performance counters related to collision detection. +#[derive(Default, Clone, Copy)] +pub struct CollisionDetectionCounters { + /// Number of contact pairs detected. + pub ncontact_pairs: usize, + /// Time spent for the broad-phase of the collision detection. + pub broad_phase_time: Timer, + /// Time spent for the narrow-phase of the collision detection. + pub narrow_phase_time: Timer, +} + +impl CollisionDetectionCounters { + /// Creates a new counter initialized to zero. + pub fn new() -> Self { + CollisionDetectionCounters { + ncontact_pairs: 0, + broad_phase_time: Timer::new(), + narrow_phase_time: Timer::new(), + } + } +} + +impl Display for CollisionDetectionCounters { + fn fmt(&self, f: &mut Formatter) -> Result { + writeln!(f, "Number of contact pairs: {}", self.ncontact_pairs)?; + writeln!(f, "Broad-phase time: {}", self.broad_phase_time)?; + writeln!(f, "Narrow-phase time: {}", self.narrow_phase_time) + } +} diff --git a/src/counters/mod.rs b/src/counters/mod.rs new file mode 100644 index 000000000..c17235063 --- /dev/null +++ b/src/counters/mod.rs @@ -0,0 +1,225 @@ +//! Counters for benchmarking various parts of the physics engine. + +use std::fmt::{Display, Formatter, Result}; + +pub use self::ccd_counters::CCDCounters; +pub use self::collision_detection_counters::CollisionDetectionCounters; +pub use self::solver_counters::SolverCounters; +pub use self::stages_counters::StagesCounters; +pub use self::timer::Timer; + +mod ccd_counters; +mod collision_detection_counters; +mod solver_counters; +mod stages_counters; +mod timer; + +/// Aggregation of all the performances counters tracked by nphysics. +#[derive(Clone, Copy)] +pub struct Counters { + /// Whether thi counter is enabled or not. + pub enabled: bool, + /// Timer for a whole timestep. + pub step_time: Timer, + /// Timer used for debugging. + pub custom: Timer, + /// Counters of every satge of one time step. + pub stages: StagesCounters, + /// Counters of the collision-detection stage. + pub cd: CollisionDetectionCounters, + /// Counters of the constraints resolution and force computation stage. + pub solver: SolverCounters, + /// Counters for the CCD resolution stage. + pub ccd: CCDCounters, +} + +impl Counters { + /// Create a new set of counters initialized to wero. + pub fn new(enabled: bool) -> Self { + Counters { + enabled, + step_time: Timer::new(), + custom: Timer::new(), + stages: StagesCounters::new(), + cd: CollisionDetectionCounters::new(), + solver: SolverCounters::new(), + ccd: CCDCounters::new(), + } + } + + /// Enable all the counters. + pub fn enable(&mut self) { + self.enabled = true; + } + + /// Return `true` if the counters are enabled. + pub fn enabled(&self) -> bool { + self.enabled + } + + /// Disable all the counters. + pub fn disable(&mut self) { + self.enabled = false; + } + + /// Notify that the time-step has started. + pub fn step_started(&mut self) { + if self.enabled { + self.step_time.start(); + } + } + + /// Notfy that the time-step has finished. + pub fn step_completed(&mut self) { + if self.enabled { + self.step_time.pause(); + } + } + + /// Total time spent for one of the physics engine. + pub fn step_time(&self) -> f64 { + self.step_time.time() + } + + /// Notify that the custom operation has started. + pub fn custom_started(&mut self) { + if self.enabled { + self.custom.start(); + } + } + + /// Notfy that the custom operation has finished. + pub fn custom_completed(&mut self) { + if self.enabled { + self.custom.pause(); + } + } + + /// Total time of a custom event. + pub fn custom_time(&self) -> f64 { + self.custom.time() + } + + /// Set the number of constraints generated. + pub fn set_nconstraints(&mut self, n: usize) { + self.solver.nconstraints = n; + } + + /// Set the number of contacts generated. + pub fn set_ncontacts(&mut self, n: usize) { + self.solver.ncontacts = n; + } + + /// Set the number of contact pairs generated. + pub fn set_ncontact_pairs(&mut self, n: usize) { + self.cd.ncontact_pairs = n; + } +} + +macro_rules! measure_method { + ($started:ident, $stopped:ident, $time:ident, $info:ident. $timer:ident) => { + impl Counters { + /// Start this timer. + pub fn $started(&mut self) { + if self.enabled { + self.$info.$timer.start() + } + } + + /// Stop this timer. + pub fn $stopped(&mut self) { + if self.enabled { + self.$info.$timer.pause() + } + } + + /// Gets the time elapsed for this timer. + pub fn $time(&self) -> f64 { + if self.enabled { + self.$info.$timer.time() + } else { + 0.0 + } + } + } + }; +} + +measure_method!( + update_started, + update_completed, + update_time, + stages.update_time +); +measure_method!( + collision_detection_started, + collision_detection_completed, + collision_detection_time, + stages.collision_detection_time +); +measure_method!( + island_construction_started, + island_construction_completed, + island_construction_time, + stages.island_construction_time +); +measure_method!( + solver_started, + solver_completed, + solver_time, + stages.solver_time +); +measure_method!(ccd_started, ccd_completed, ccd_time, stages.ccd_time); + +measure_method!( + assembly_started, + assembly_completed, + assembly_time, + solver.velocity_assembly_time +); +measure_method!( + velocity_resolution_started, + velocity_resolution_completed, + velocity_resolution_time, + solver.velocity_resolution_time +); +measure_method!( + velocity_update_started, + velocity_update_completed, + velocity_update_time, + solver.velocity_update_time +); +measure_method!( + position_resolution_started, + position_resolution_completed, + position_resolution_time, + solver.position_resolution_time +); +measure_method!( + broad_phase_started, + broad_phase_completed, + broad_phase_time, + cd.broad_phase_time +); +measure_method!( + narrow_phase_started, + narrow_phase_completed, + narrow_phase_time, + cd.narrow_phase_time +); + +impl Display for Counters { + fn fmt(&self, f: &mut Formatter) -> Result { + writeln!(f, "Total timestep time: {}", self.step_time)?; + self.stages.fmt(f)?; + self.cd.fmt(f)?; + self.solver.fmt(f)?; + writeln!(f, "Custom timer: {}", self.custom) + } +} + +impl Default for Counters { + fn default() -> Self { + Self::new(false) + } +} diff --git a/src/counters/solver_counters.rs b/src/counters/solver_counters.rs new file mode 100644 index 000000000..babcf419d --- /dev/null +++ b/src/counters/solver_counters.rs @@ -0,0 +1,67 @@ +use crate::counters::Timer; +use std::fmt::{Display, Formatter, Result}; + +/// Performance counters related to constraints resolution. +#[derive(Default, Clone, Copy)] +pub struct SolverCounters { + /// Number of constraints generated. + pub nconstraints: usize, + /// Number of contacts found. + pub ncontacts: usize, + /// Time spent for the resolution of the constraints (force computation). + pub velocity_resolution_time: Timer, + /// Time spent for the assembly of all the velocity constraints. + pub velocity_assembly_time: Timer, + /// Time spent for the update of the velocity of the bodies. + pub velocity_update_time: Timer, + /// Time spent for the assembly of all the position constraints. + pub position_assembly_time: Timer, + /// Time spent for the update of the position of the bodies. + pub position_resolution_time: Timer, +} + +impl SolverCounters { + /// Creates a new counter initialized to zero. + pub fn new() -> Self { + SolverCounters { + nconstraints: 0, + ncontacts: 0, + velocity_assembly_time: Timer::new(), + velocity_resolution_time: Timer::new(), + velocity_update_time: Timer::new(), + position_assembly_time: Timer::new(), + position_resolution_time: Timer::new(), + } + } + + /// Reset all the counters to zero. + pub fn reset(&mut self) { + self.nconstraints = 0; + self.ncontacts = 0; + self.velocity_resolution_time.reset(); + self.velocity_assembly_time.reset(); + self.velocity_update_time.reset(); + self.position_assembly_time.reset(); + self.position_resolution_time.reset(); + } +} + +impl Display for SolverCounters { + fn fmt(&self, f: &mut Formatter) -> Result { + writeln!(f, "Number of contacts: {}", self.ncontacts)?; + writeln!(f, "Number of constraints: {}", self.nconstraints)?; + writeln!(f, "Velocity assembly time: {}", self.velocity_assembly_time)?; + writeln!( + f, + "Velocity resolution time: {}", + self.velocity_resolution_time + )?; + writeln!(f, "Velocity update time: {}", self.velocity_update_time)?; + writeln!(f, "Position assembly time: {}", self.position_assembly_time)?; + writeln!( + f, + "Position resolution time: {}", + self.position_resolution_time + ) + } +} diff --git a/src/counters/stages_counters.rs b/src/counters/stages_counters.rs new file mode 100644 index 000000000..856b759da --- /dev/null +++ b/src/counters/stages_counters.rs @@ -0,0 +1,48 @@ +use crate::counters::Timer; +use std::fmt::{Display, Formatter, Result}; + +/// Performance counters related to each stage of the time step. +#[derive(Default, Clone, Copy)] +pub struct StagesCounters { + /// Time spent for updating the kinematic and dynamics of every body. + pub update_time: Timer, + /// Total time spent for the collision detection (including both broad- and narrow- phases). + pub collision_detection_time: Timer, + /// Time spent for the computation of collision island and body activation/deactivation (sleeping). + pub island_construction_time: Timer, + /// Total time spent for the constraints resolution and position update.t + pub solver_time: Timer, + /// Total time spent for CCD and CCD resolution. + pub ccd_time: Timer, +} + +impl StagesCounters { + /// Create a new counter intialized to zero. + pub fn new() -> Self { + StagesCounters { + update_time: Timer::new(), + collision_detection_time: Timer::new(), + island_construction_time: Timer::new(), + solver_time: Timer::new(), + ccd_time: Timer::new(), + } + } +} + +impl Display for StagesCounters { + fn fmt(&self, f: &mut Formatter) -> Result { + writeln!(f, "Update time: {}", self.update_time)?; + writeln!( + f, + "Collision detection time: {}", + self.collision_detection_time + )?; + writeln!( + f, + "Island construction time: {}", + self.island_construction_time + )?; + writeln!(f, "Solver time: {}", self.solver_time)?; + writeln!(f, "CCD time: {}", self.ccd_time) + } +} diff --git a/src/counters/timer.rs b/src/counters/timer.rs new file mode 100644 index 000000000..fd2506380 --- /dev/null +++ b/src/counters/timer.rs @@ -0,0 +1,53 @@ +use std::fmt::{Display, Error, Formatter}; + +/// A timer. +#[derive(Copy, Clone, Debug, Default)] +pub struct Timer { + time: f64, + start: Option, +} + +impl Timer { + /// Creates a new timer initialized to zero and not started. + pub fn new() -> Self { + Timer { + time: 0.0, + start: None, + } + } + + /// Resets the timer to 0. + pub fn reset(&mut self) { + self.time = 0.0 + } + + /// Start the timer. + pub fn start(&mut self) { + self.time = 0.0; + self.start = Some(instant::now()); + } + + /// Pause the timer. + pub fn pause(&mut self) { + if let Some(start) = self.start { + self.time += instant::now() - start; + } + self.start = None; + } + + /// Resume the timer. + pub fn resume(&mut self) { + self.start = Some(instant::now()); + } + + /// The measured time between the last `.start()` and `.pause()` calls. + pub fn time(&self) -> f64 { + self.time + } +} + +impl Display for Timer { + fn fmt(&self, f: &mut Formatter) -> Result<(), Error> { + write!(f, "{}s", self.time) + } +} diff --git a/src/data/arena.rs b/src/data/arena.rs new file mode 100644 index 000000000..fcec01754 --- /dev/null +++ b/src/data/arena.rs @@ -0,0 +1,1159 @@ +//! Arena adapted from the generational-arena crate. +//! +//! See https://github.com/fitzgen/generational-arena/blob/master/src/lib.rs. +//! This has been modified to have a fully deterministic deserialization (including for the order of +//! Index attribution after a deserialization of the arena. +use std::cmp; +use std::iter::{self, Extend, FromIterator, FusedIterator}; +use std::mem; +use std::ops; +use std::slice; +use std::vec; + +/// The `Arena` allows inserting and removing elements that are referred to by +/// `Index`. +/// +/// [See the module-level documentation for example usage and motivation.](./index.html) +#[derive(Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct Arena { + items: Vec>, + generation: u64, + free_list_head: Option, + len: usize, +} + +#[derive(Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +enum Entry { + Free { next_free: Option }, + Occupied { generation: u64, value: T }, +} + +/// An index (and generation) into an `Arena`. +/// +/// To get an `Index`, insert an element into an `Arena`, and the `Index` for +/// that element will be returned. +/// +/// # Examples +/// +/// ```ignore +/// use rapier::data::arena::Arena; +/// +/// let mut arena = Arena::new(); +/// let idx = arena.insert(123); +/// assert_eq!(arena[idx], 123); +/// ``` +#[derive(Clone, Copy, Debug, PartialEq, Eq, PartialOrd, Ord, Hash)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct Index { + index: usize, + generation: u64, +} + +impl Index { + /// Create a new `Index` from its raw parts. + /// + /// The parts must have been returned from an earlier call to + /// `into_raw_parts`. + /// + /// Providing arbitrary values will lead to malformed indices and ultimately + /// panics. + pub fn from_raw_parts(a: usize, b: u64) -> Index { + Index { + index: a, + generation: b, + } + } + + /// Convert this `Index` into its raw parts. + /// + /// This niche method is useful for converting an `Index` into another + /// identifier type. Usually, you should prefer a newtype wrapper around + /// `Index` like `pub struct MyIdentifier(Index);`. However, for external + /// types whose definition you can't customize, but which you can construct + /// instances of, this method can be useful. + pub fn into_raw_parts(self) -> (usize, u64) { + (self.index, self.generation) + } +} + +const DEFAULT_CAPACITY: usize = 4; + +impl Default for Arena { + fn default() -> Arena { + Arena::new() + } +} + +impl Arena { + /// Constructs a new, empty `Arena`. + /// + /// # Examples + /// + /// ```ignore + /// use rapier::data::arena::Arena; + /// + /// let mut arena = Arena::::new(); + /// # let _ = arena; + /// ``` + pub fn new() -> Arena { + Arena::with_capacity(DEFAULT_CAPACITY) + } + + /// Constructs a new, empty `Arena` with the specified capacity. + /// + /// The `Arena` will be able to hold `n` elements without further allocation. + /// + /// # Examples + /// + /// ```ignore + /// use rapier::data::arena::Arena; + /// + /// let mut arena = Arena::with_capacity(10); + /// + /// // These insertions will not require further allocation. + /// for i in 0..10 { + /// assert!(arena.try_insert(i).is_ok()); + /// } + /// + /// // But now we are at capacity, and there is no more room. + /// assert!(arena.try_insert(99).is_err()); + /// ``` + pub fn with_capacity(n: usize) -> Arena { + let n = cmp::max(n, 1); + let mut arena = Arena { + items: Vec::new(), + generation: 0, + free_list_head: None, + len: 0, + }; + arena.reserve(n); + arena + } + + /// Clear all the items inside the arena, but keep its allocation. + /// + /// # Examples + /// + /// ```ignore + /// use rapier::data::arena::Arena; + /// + /// let mut arena = Arena::with_capacity(1); + /// arena.insert(42); + /// arena.insert(43); + /// + /// arena.clear(); + /// + /// assert_eq!(arena.capacity(), 2); + /// ``` + pub fn clear(&mut self) { + self.items.clear(); + + let end = self.items.capacity(); + self.items.extend((0..end).map(|i| { + if i == end - 1 { + Entry::Free { next_free: None } + } else { + Entry::Free { + next_free: Some(i + 1), + } + } + })); + self.free_list_head = Some(0); + self.len = 0; + } + + /// Attempts to insert `value` into the arena using existing capacity. + /// + /// This method will never allocate new capacity in the arena. + /// + /// If insertion succeeds, then the `value`'s index is returned. If + /// insertion fails, then `Err(value)` is returned to give ownership of + /// `value` back to the caller. + /// + /// # Examples + /// + /// ```ignore + /// use rapier::data::arena::Arena; + /// + /// let mut arena = Arena::new(); + /// + /// match arena.try_insert(42) { + /// Ok(idx) => { + /// // Insertion succeeded. + /// assert_eq!(arena[idx], 42); + /// } + /// Err(x) => { + /// // Insertion failed. + /// assert_eq!(x, 42); + /// } + /// }; + /// ``` + #[inline] + pub fn try_insert(&mut self, value: T) -> Result { + match self.try_alloc_next_index() { + None => Err(value), + Some(index) => { + self.items[index.index] = Entry::Occupied { + generation: self.generation, + value, + }; + Ok(index) + } + } + } + + /// Attempts to insert the value returned by `create` into the arena using existing capacity. + /// `create` is called with the new value's associated index, allowing values that know their own index. + /// + /// This method will never allocate new capacity in the arena. + /// + /// If insertion succeeds, then the new index is returned. If + /// insertion fails, then `Err(create)` is returned to give ownership of + /// `create` back to the caller. + /// + /// # Examples + /// + /// ```ignore + /// use rapier::data::arena::{Arena, Index}; + /// + /// let mut arena = Arena::new(); + /// + /// match arena.try_insert_with(|idx| (42, idx)) { + /// Ok(idx) => { + /// // Insertion succeeded. + /// assert_eq!(arena[idx].0, 42); + /// assert_eq!(arena[idx].1, idx); + /// } + /// Err(x) => { + /// // Insertion failed. + /// } + /// }; + /// ``` + #[inline] + pub fn try_insert_with T>(&mut self, create: F) -> Result { + match self.try_alloc_next_index() { + None => Err(create), + Some(index) => { + self.items[index.index] = Entry::Occupied { + generation: self.generation, + value: create(index), + }; + Ok(index) + } + } + } + + #[inline] + fn try_alloc_next_index(&mut self) -> Option { + match self.free_list_head { + None => None, + Some(i) => match self.items[i] { + Entry::Occupied { .. } => panic!("corrupt free list"), + Entry::Free { next_free } => { + self.free_list_head = next_free; + self.len += 1; + Some(Index { + index: i, + generation: self.generation, + }) + } + }, + } + } + + /// Insert `value` into the arena, allocating more capacity if necessary. + /// + /// The `value`'s associated index in the arena is returned. + /// + /// # Examples + /// + /// ```ignore + /// use rapier::data::arena::Arena; + /// + /// let mut arena = Arena::new(); + /// + /// let idx = arena.insert(42); + /// assert_eq!(arena[idx], 42); + /// ``` + #[inline] + pub fn insert(&mut self, value: T) -> Index { + match self.try_insert(value) { + Ok(i) => i, + Err(value) => self.insert_slow_path(value), + } + } + + /// Insert the value returned by `create` into the arena, allocating more capacity if necessary. + /// `create` is called with the new value's associated index, allowing values that know their own index. + /// + /// The new value's associated index in the arena is returned. + /// + /// # Examples + /// + /// ```ignore + /// use rapier::data::arena::{Arena, Index}; + /// + /// let mut arena = Arena::new(); + /// + /// let idx = arena.insert_with(|idx| (42, idx)); + /// assert_eq!(arena[idx].0, 42); + /// assert_eq!(arena[idx].1, idx); + /// ``` + #[inline] + pub fn insert_with(&mut self, create: impl FnOnce(Index) -> T) -> Index { + match self.try_insert_with(create) { + Ok(i) => i, + Err(create) => self.insert_with_slow_path(create), + } + } + + #[inline(never)] + fn insert_slow_path(&mut self, value: T) -> Index { + let len = self.items.len(); + self.reserve(len); + self.try_insert(value) + .map_err(|_| ()) + .expect("inserting will always succeed after reserving additional space") + } + + #[inline(never)] + fn insert_with_slow_path(&mut self, create: impl FnOnce(Index) -> T) -> Index { + let len = self.items.len(); + self.reserve(len); + self.try_insert_with(create) + .map_err(|_| ()) + .expect("inserting will always succeed after reserving additional space") + } + + /// Remove the element at index `i` from the arena. + /// + /// If the element at index `i` is still in the arena, then it is + /// returned. If it is not in the arena, then `None` is returned. + /// + /// # Examples + /// + /// ```ignore + /// use rapier::data::arena::Arena; + /// + /// let mut arena = Arena::new(); + /// let idx = arena.insert(42); + /// + /// assert_eq!(arena.remove(idx), Some(42)); + /// assert_eq!(arena.remove(idx), None); + /// ``` + pub fn remove(&mut self, i: Index) -> Option { + if i.index >= self.items.len() { + return None; + } + + match self.items[i.index] { + Entry::Occupied { generation, .. } if i.generation == generation => { + let entry = mem::replace( + &mut self.items[i.index], + Entry::Free { + next_free: self.free_list_head, + }, + ); + self.generation += 1; + self.free_list_head = Some(i.index); + self.len -= 1; + + match entry { + Entry::Occupied { + generation: _, + value, + } => Some(value), + _ => unreachable!(), + } + } + _ => None, + } + } + + /// Retains only the elements specified by the predicate. + /// + /// In other words, remove all indices such that `predicate(index, &value)` returns `false`. + /// + /// # Examples + /// + /// ```ignore + /// use rapier::data::arena::Arena; + /// + /// let mut crew = Arena::new(); + /// crew.extend(&["Jim Hawkins", "John Silver", "Alexander Smollett", "Israel Hands"]); + /// let pirates = ["John Silver", "Israel Hands"]; // too dangerous to keep them around + /// crew.retain(|_index, member| !pirates.contains(member)); + /// let mut crew_members = crew.iter().map(|(_, member)| **member); + /// assert_eq!(crew_members.next(), Some("Jim Hawkins")); + /// assert_eq!(crew_members.next(), Some("Alexander Smollett")); + /// assert!(crew_members.next().is_none()); + /// ``` + pub fn retain(&mut self, mut predicate: impl FnMut(Index, &mut T) -> bool) { + for i in 0..self.capacity() { + let remove = match &mut self.items[i] { + Entry::Occupied { generation, value } => { + let index = Index { + index: i, + generation: *generation, + }; + if predicate(index, value) { + None + } else { + Some(index) + } + } + + _ => None, + }; + if let Some(index) = remove { + self.remove(index); + } + } + } + + /// Is the element at index `i` in the arena? + /// + /// Returns `true` if the element at `i` is in the arena, `false` otherwise. + /// + /// # Examples + /// + /// ```ignore + /// use rapier::data::arena::Arena; + /// + /// let mut arena = Arena::new(); + /// let idx = arena.insert(42); + /// + /// assert!(arena.contains(idx)); + /// arena.remove(idx); + /// assert!(!arena.contains(idx)); + /// ``` + pub fn contains(&self, i: Index) -> bool { + self.get(i).is_some() + } + + /// Get a shared reference to the element at index `i` if it is in the + /// arena. + /// + /// If the element at index `i` is not in the arena, then `None` is returned. + /// + /// # Examples + /// + /// ```ignore + /// use rapier::data::arena::Arena; + /// + /// let mut arena = Arena::new(); + /// let idx = arena.insert(42); + /// + /// assert_eq!(arena.get(idx), Some(&42)); + /// arena.remove(idx); + /// assert!(arena.get(idx).is_none()); + /// ``` + pub fn get(&self, i: Index) -> Option<&T> { + match self.items.get(i.index) { + Some(Entry::Occupied { generation, value }) if *generation == i.generation => { + Some(value) + } + _ => None, + } + } + + /// Get an exclusive reference to the element at index `i` if it is in the + /// arena. + /// + /// If the element at index `i` is not in the arena, then `None` is returned. + /// + /// # Examples + /// + /// ```ignore + /// use rapier::data::arena::Arena; + /// + /// let mut arena = Arena::new(); + /// let idx = arena.insert(42); + /// + /// *arena.get_mut(idx).unwrap() += 1; + /// assert_eq!(arena.remove(idx), Some(43)); + /// assert!(arena.get_mut(idx).is_none()); + /// ``` + pub fn get_mut(&mut self, i: Index) -> Option<&mut T> { + match self.items.get_mut(i.index) { + Some(Entry::Occupied { generation, value }) if *generation == i.generation => { + Some(value) + } + _ => None, + } + } + + /// Get a pair of exclusive references to the elements at index `i1` and `i2` if it is in the + /// arena. + /// + /// If the element at index `i1` or `i2` is not in the arena, then `None` is returned for this + /// element. + /// + /// # Panics + /// + /// Panics if `i1` and `i2` are pointing to the same item of the arena. + /// + /// # Examples + /// + /// ```ignore + /// use rapier::data::arena::Arena; + /// + /// let mut arena = Arena::new(); + /// let idx1 = arena.insert(0); + /// let idx2 = arena.insert(1); + /// + /// { + /// let (item1, item2) = arena.get2_mut(idx1, idx2); + /// + /// *item1.unwrap() = 3; + /// *item2.unwrap() = 4; + /// } + /// + /// assert_eq!(arena[idx1], 3); + /// assert_eq!(arena[idx2], 4); + /// ``` + pub fn get2_mut(&mut self, i1: Index, i2: Index) -> (Option<&mut T>, Option<&mut T>) { + let len = self.items.len(); + + if i1.index == i2.index { + assert!(i1.generation != i2.generation); + + if i1.generation > i2.generation { + return (self.get_mut(i1), None); + } + return (None, self.get_mut(i2)); + } + + if i1.index >= len { + return (None, self.get_mut(i2)); + } else if i2.index >= len { + return (self.get_mut(i1), None); + } + + let (raw_item1, raw_item2) = { + let (xs, ys) = self.items.split_at_mut(cmp::max(i1.index, i2.index)); + if i1.index < i2.index { + (&mut xs[i1.index], &mut ys[0]) + } else { + (&mut ys[0], &mut xs[i2.index]) + } + }; + + let item1 = match raw_item1 { + Entry::Occupied { generation, value } if *generation == i1.generation => Some(value), + _ => None, + }; + + let item2 = match raw_item2 { + Entry::Occupied { generation, value } if *generation == i2.generation => Some(value), + _ => None, + }; + + (item1, item2) + } + + /// Get the length of this arena. + /// + /// The length is the number of elements the arena holds. + /// + /// # Examples + /// + /// ```ignore + /// use rapier::data::arena::Arena; + /// + /// let mut arena = Arena::new(); + /// assert_eq!(arena.len(), 0); + /// + /// let idx = arena.insert(42); + /// assert_eq!(arena.len(), 1); + /// + /// let _ = arena.insert(0); + /// assert_eq!(arena.len(), 2); + /// + /// assert_eq!(arena.remove(idx), Some(42)); + /// assert_eq!(arena.len(), 1); + /// ``` + pub fn len(&self) -> usize { + self.len + } + + /// Returns true if the arena contains no elements + /// + /// # Examples + /// + /// ```ignore + /// use rapier::data::arena::Arena; + /// + /// let mut arena = Arena::new(); + /// assert!(arena.is_empty()); + /// + /// let idx = arena.insert(42); + /// assert!(!arena.is_empty()); + /// + /// assert_eq!(arena.remove(idx), Some(42)); + /// assert!(arena.is_empty()); + /// ``` + pub fn is_empty(&self) -> bool { + self.len == 0 + } + + /// Get the capacity of this arena. + /// + /// The capacity is the maximum number of elements the arena can hold + /// without further allocation, including however many it currently + /// contains. + /// + /// # Examples + /// + /// ```ignore + /// use rapier::data::arena::Arena; + /// + /// let mut arena = Arena::with_capacity(10); + /// assert_eq!(arena.capacity(), 10); + /// + /// // `try_insert` does not allocate new capacity. + /// for i in 0..10 { + /// assert!(arena.try_insert(1).is_ok()); + /// assert_eq!(arena.capacity(), 10); + /// } + /// + /// // But `insert` will if the arena is already at capacity. + /// arena.insert(0); + /// assert!(arena.capacity() > 10); + /// ``` + pub fn capacity(&self) -> usize { + self.items.len() + } + + /// Allocate space for `additional_capacity` more elements in the arena. + /// + /// # Panics + /// + /// Panics if this causes the capacity to overflow. + /// + /// # Examples + /// + /// ```ignore + /// use rapier::data::arena::Arena; + /// + /// let mut arena = Arena::with_capacity(10); + /// arena.reserve(5); + /// assert_eq!(arena.capacity(), 15); + /// # let _: Arena = arena; + /// ``` + pub fn reserve(&mut self, additional_capacity: usize) { + let start = self.items.len(); + let end = self.items.len() + additional_capacity; + let old_head = self.free_list_head; + self.items.reserve_exact(additional_capacity); + self.items.extend((start..end).map(|i| { + if i == end - 1 { + Entry::Free { + next_free: old_head, + } + } else { + Entry::Free { + next_free: Some(i + 1), + } + } + })); + self.free_list_head = Some(start); + } + + /// Iterate over shared references to the elements in this arena. + /// + /// Yields pairs of `(Index, &T)` items. + /// + /// Order of iteration is not defined. + /// + /// # Examples + /// + /// ```ignore + /// use rapier::data::arena::Arena; + /// + /// let mut arena = Arena::new(); + /// for i in 0..10 { + /// arena.insert(i * i); + /// } + /// + /// for (idx, value) in arena.iter() { + /// println!("{} is at index {:?}", value, idx); + /// } + /// ``` + pub fn iter(&self) -> Iter { + Iter { + len: self.len, + inner: self.items.iter().enumerate(), + } + } + + /// Iterate over exclusive references to the elements in this arena. + /// + /// Yields pairs of `(Index, &mut T)` items. + /// + /// Order of iteration is not defined. + /// + /// # Examples + /// + /// ```ignore + /// use rapier::data::arena::Arena; + /// + /// let mut arena = Arena::new(); + /// for i in 0..10 { + /// arena.insert(i * i); + /// } + /// + /// for (_idx, value) in arena.iter_mut() { + /// *value += 5; + /// } + /// ``` + pub fn iter_mut(&mut self) -> IterMut { + IterMut { + len: self.len, + inner: self.items.iter_mut().enumerate(), + } + } + + /// Iterate over elements of the arena and remove them. + /// + /// Yields pairs of `(Index, T)` items. + /// + /// Order of iteration is not defined. + /// + /// Note: All elements are removed even if the iterator is only partially consumed or not consumed at all. + /// + /// # Examples + /// + /// ```ignore + /// use rapier::data::arena::Arena; + /// + /// let mut arena = Arena::new(); + /// let idx_1 = arena.insert("hello"); + /// let idx_2 = arena.insert("world"); + /// + /// assert!(arena.get(idx_1).is_some()); + /// assert!(arena.get(idx_2).is_some()); + /// for (idx, value) in arena.drain() { + /// assert!((idx == idx_1 && value == "hello") || (idx == idx_2 && value == "world")); + /// } + /// assert!(arena.get(idx_1).is_none()); + /// assert!(arena.get(idx_2).is_none()); + /// ``` + pub fn drain(&mut self) -> Drain { + Drain { + inner: self.items.drain(..).enumerate(), + } + } + + /// Given an i of `usize` without a generation, get a shared reference + /// to the element and the matching `Index` of the entry behind `i`. + /// + /// This method is useful when you know there might be an element at the + /// position i, but don't know its generation or precise Index. + /// + /// Use cases include using indexing such as Hierarchical BitMap Indexing or + /// other kinds of bit-efficient indexing. + /// + /// You should use the `get` method instead most of the time. + pub fn get_unknown_gen(&self, i: usize) -> Option<(&T, Index)> { + match self.items.get(i) { + Some(Entry::Occupied { generation, value }) => Some(( + value, + Index { + generation: *generation, + index: i, + }, + )), + _ => None, + } + } + + /// Given an i of `usize` without a generation, get an exclusive reference + /// to the element and the matching `Index` of the entry behind `i`. + /// + /// This method is useful when you know there might be an element at the + /// position i, but don't know its generation or precise Index. + /// + /// Use cases include using indexing such as Hierarchical BitMap Indexing or + /// other kinds of bit-efficient indexing. + /// + /// You should use the `get_mut` method instead most of the time. + pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut T, Index)> { + match self.items.get_mut(i) { + Some(Entry::Occupied { generation, value }) => Some(( + value, + Index { + generation: *generation, + index: i, + }, + )), + _ => None, + } + } +} + +impl IntoIterator for Arena { + type Item = T; + type IntoIter = IntoIter; + fn into_iter(self) -> Self::IntoIter { + IntoIter { + len: self.len, + inner: self.items.into_iter(), + } + } +} + +/// An iterator over the elements in an arena. +/// +/// Yields `T` items. +/// +/// Order of iteration is not defined. +/// +/// # Examples +/// +/// ```ignore +/// use rapier::data::arena::Arena; +/// +/// let mut arena = Arena::new(); +/// for i in 0..10 { +/// arena.insert(i * i); +/// } +/// +/// for value in arena { +/// assert!(value < 100); +/// } +/// ``` +#[derive(Clone, Debug)] +pub struct IntoIter { + len: usize, + inner: vec::IntoIter>, +} + +impl Iterator for IntoIter { + type Item = T; + + fn next(&mut self) -> Option { + loop { + match self.inner.next() { + Some(Entry::Free { .. }) => continue, + Some(Entry::Occupied { value, .. }) => { + self.len -= 1; + return Some(value); + } + None => { + debug_assert_eq!(self.len, 0); + return None; + } + } + } + } + + fn size_hint(&self) -> (usize, Option) { + (self.len, Some(self.len)) + } +} + +impl DoubleEndedIterator for IntoIter { + fn next_back(&mut self) -> Option { + loop { + match self.inner.next_back() { + Some(Entry::Free { .. }) => continue, + Some(Entry::Occupied { value, .. }) => { + self.len -= 1; + return Some(value); + } + None => { + debug_assert_eq!(self.len, 0); + return None; + } + } + } + } +} + +impl ExactSizeIterator for IntoIter { + fn len(&self) -> usize { + self.len + } +} + +impl FusedIterator for IntoIter {} + +impl<'a, T> IntoIterator for &'a Arena { + type Item = (Index, &'a T); + type IntoIter = Iter<'a, T>; + fn into_iter(self) -> Self::IntoIter { + self.iter() + } +} + +/// An iterator over shared references to the elements in an arena. +/// +/// Yields pairs of `(Index, &T)` items. +/// +/// Order of iteration is not defined. +/// +/// # Examples +/// +/// ```ignore +/// use rapier::data::arena::Arena; +/// +/// let mut arena = Arena::new(); +/// for i in 0..10 { +/// arena.insert(i * i); +/// } +/// +/// for (idx, value) in &arena { +/// println!("{} is at index {:?}", value, idx); +/// } +/// ``` +#[derive(Clone, Debug)] +pub struct Iter<'a, T: 'a> { + len: usize, + inner: iter::Enumerate>>, +} + +impl<'a, T> Iterator for Iter<'a, T> { + type Item = (Index, &'a T); + + fn next(&mut self) -> Option { + loop { + match self.inner.next() { + Some((_, &Entry::Free { .. })) => continue, + Some(( + index, + &Entry::Occupied { + generation, + ref value, + }, + )) => { + self.len -= 1; + let idx = Index { index, generation }; + return Some((idx, value)); + } + None => { + debug_assert_eq!(self.len, 0); + return None; + } + } + } + } + + fn size_hint(&self) -> (usize, Option) { + (self.len, Some(self.len)) + } +} + +impl<'a, T> DoubleEndedIterator for Iter<'a, T> { + fn next_back(&mut self) -> Option { + loop { + match self.inner.next_back() { + Some((_, &Entry::Free { .. })) => continue, + Some(( + index, + &Entry::Occupied { + generation, + ref value, + }, + )) => { + self.len -= 1; + let idx = Index { index, generation }; + return Some((idx, value)); + } + None => { + debug_assert_eq!(self.len, 0); + return None; + } + } + } + } +} + +impl<'a, T> ExactSizeIterator for Iter<'a, T> { + fn len(&self) -> usize { + self.len + } +} + +impl<'a, T> FusedIterator for Iter<'a, T> {} + +impl<'a, T> IntoIterator for &'a mut Arena { + type Item = (Index, &'a mut T); + type IntoIter = IterMut<'a, T>; + fn into_iter(self) -> Self::IntoIter { + self.iter_mut() + } +} + +/// An iterator over exclusive references to elements in this arena. +/// +/// Yields pairs of `(Index, &mut T)` items. +/// +/// Order of iteration is not defined. +/// +/// # Examples +/// +/// ```ignore +/// use rapier::data::arena::Arena; +/// +/// let mut arena = Arena::new(); +/// for i in 0..10 { +/// arena.insert(i * i); +/// } +/// +/// for (_idx, value) in &mut arena { +/// *value += 5; +/// } +/// ``` +#[derive(Debug)] +pub struct IterMut<'a, T: 'a> { + len: usize, + inner: iter::Enumerate>>, +} + +impl<'a, T> Iterator for IterMut<'a, T> { + type Item = (Index, &'a mut T); + + fn next(&mut self) -> Option { + loop { + match self.inner.next() { + Some((_, &mut Entry::Free { .. })) => continue, + Some(( + index, + &mut Entry::Occupied { + generation, + ref mut value, + }, + )) => { + self.len -= 1; + let idx = Index { index, generation }; + return Some((idx, value)); + } + None => { + debug_assert_eq!(self.len, 0); + return None; + } + } + } + } + + fn size_hint(&self) -> (usize, Option) { + (self.len, Some(self.len)) + } +} + +impl<'a, T> DoubleEndedIterator for IterMut<'a, T> { + fn next_back(&mut self) -> Option { + loop { + match self.inner.next_back() { + Some((_, &mut Entry::Free { .. })) => continue, + Some(( + index, + &mut Entry::Occupied { + generation, + ref mut value, + }, + )) => { + self.len -= 1; + let idx = Index { index, generation }; + return Some((idx, value)); + } + None => { + debug_assert_eq!(self.len, 0); + return None; + } + } + } + } +} + +impl<'a, T> ExactSizeIterator for IterMut<'a, T> { + fn len(&self) -> usize { + self.len + } +} + +impl<'a, T> FusedIterator for IterMut<'a, T> {} + +/// An iterator that removes elements from the arena. +/// +/// Yields pairs of `(Index, T)` items. +/// +/// Order of iteration is not defined. +/// +/// Note: All elements are removed even if the iterator is only partially consumed or not consumed at all. +/// +/// # Examples +/// +/// ```ignore +/// use rapier::data::arena::Arena; +/// +/// let mut arena = Arena::new(); +/// let idx_1 = arena.insert("hello"); +/// let idx_2 = arena.insert("world"); +/// +/// assert!(arena.get(idx_1).is_some()); +/// assert!(arena.get(idx_2).is_some()); +/// for (idx, value) in arena.drain() { +/// assert!((idx == idx_1 && value == "hello") || (idx == idx_2 && value == "world")); +/// } +/// assert!(arena.get(idx_1).is_none()); +/// assert!(arena.get(idx_2).is_none()); +/// ``` +#[derive(Debug)] +pub struct Drain<'a, T: 'a> { + inner: iter::Enumerate>>, +} + +impl<'a, T> Iterator for Drain<'a, T> { + type Item = (Index, T); + + fn next(&mut self) -> Option { + loop { + match self.inner.next() { + Some((_, Entry::Free { .. })) => continue, + Some((index, Entry::Occupied { generation, value })) => { + let idx = Index { index, generation }; + return Some((idx, value)); + } + None => return None, + } + } + } +} + +impl Extend for Arena { + fn extend>(&mut self, iter: I) { + for t in iter { + self.insert(t); + } + } +} + +impl FromIterator for Arena { + fn from_iter>(iter: I) -> Self { + let iter = iter.into_iter(); + let (lower, upper) = iter.size_hint(); + let cap = upper.unwrap_or(lower); + let cap = cmp::max(cap, 1); + let mut arena = Arena::with_capacity(cap); + arena.extend(iter); + arena + } +} + +impl ops::Index for Arena { + type Output = T; + + fn index(&self, index: Index) -> &Self::Output { + self.get(index).expect("No element at index") + } +} + +impl ops::IndexMut for Arena { + fn index_mut(&mut self, index: Index) -> &mut Self::Output { + self.get_mut(index).expect("No element at index") + } +} diff --git a/src/data/graph.rs b/src/data/graph.rs new file mode 100644 index 000000000..ea27e0302 --- /dev/null +++ b/src/data/graph.rs @@ -0,0 +1,830 @@ +// This is basically a stripped down version of petgraph's UnGraph. +// - It is not generic wrt. the index type (we always use u32). +// - It preserves associated edge iteration order after Serialization/Deserialization. +// - It is always undirected. +//! A stripped-down version of petgraph's UnGraph. + +use std::cmp::max; +use std::ops::{Index, IndexMut}; + +/// Node identifier. +#[derive(Copy, Clone, Default, PartialEq, PartialOrd, Eq, Ord, Hash, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct NodeIndex(u32); + +impl NodeIndex { + #[inline] + pub fn new(x: u32) -> Self { + NodeIndex(x) + } + + #[inline] + pub fn index(self) -> usize { + self.0 as usize + } + + #[inline] + pub fn end() -> Self { + NodeIndex(crate::INVALID_U32) + } + + fn _into_edge(self) -> EdgeIndex { + EdgeIndex(self.0) + } +} + +impl From for NodeIndex { + fn from(ix: u32) -> Self { + NodeIndex(ix) + } +} + +/// Edge identifier. +#[derive(Copy, Clone, Default, PartialEq, PartialOrd, Eq, Ord, Hash, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct EdgeIndex(u32); + +impl EdgeIndex { + #[inline] + pub fn new(x: u32) -> Self { + EdgeIndex(x) + } + + #[inline] + pub fn index(self) -> usize { + self.0 as usize + } + + /// An invalid `EdgeIndex` used to denote absence of an edge, for example + /// to end an adjacency list. + #[inline] + pub fn end() -> Self { + EdgeIndex(crate::INVALID_U32) + } + + fn _into_node(self) -> NodeIndex { + NodeIndex(self.0) + } +} + +impl From for EdgeIndex { + fn from(ix: u32) -> Self { + EdgeIndex(ix) + } +} + +#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub enum Direction { + Outgoing = 0, + Incoming = 1, +} + +impl Direction { + fn opposite(self) -> Direction { + match self { + Direction::Outgoing => Direction::Incoming, + Direction::Incoming => Direction::Outgoing, + } + } +} + +const DIRECTIONS: [Direction; 2] = [Direction::Outgoing, Direction::Incoming]; + +/// The graph's node type. +#[derive(Debug, Copy, Clone)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct Node { + /// Associated node data. + pub weight: N, + /// Next edge in outgoing and incoming edge lists. + next: [EdgeIndex; 2], +} + +/// The graph's edge type. +#[derive(Debug, Copy, Clone)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct Edge { + /// Associated edge data. + pub weight: E, + /// Next edge in outgoing and incoming edge lists. + next: [EdgeIndex; 2], + /// Start and End node index + node: [NodeIndex; 2], +} + +impl Edge { + /// Return the source node index. + pub fn source(&self) -> NodeIndex { + self.node[0] + } + + /// Return the target node index. + pub fn target(&self) -> NodeIndex { + self.node[1] + } +} + +#[derive(Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct Graph { + pub(crate) nodes: Vec>, + pub(crate) edges: Vec>, +} + +enum Pair { + Both(T, T), + One(T), + None, +} + +/// Get mutable references at index `a` and `b`. +fn index_twice(arr: &mut [T], a: usize, b: usize) -> Pair<&mut T> { + if max(a, b) >= arr.len() { + Pair::None + } else if a == b { + Pair::One(&mut arr[max(a, b)]) + } else { + // safe because a, b are in bounds and distinct + unsafe { + let ar = &mut *(arr.get_unchecked_mut(a) as *mut _); + let br = &mut *(arr.get_unchecked_mut(b) as *mut _); + Pair::Both(ar, br) + } + } +} + +impl Graph { + /// Create a new `Graph` with estimated capacity. + pub fn with_capacity(nodes: usize, edges: usize) -> Self { + Graph { + nodes: Vec::with_capacity(nodes), + edges: Vec::with_capacity(edges), + } + } + + /// Add a node (also called vertex) with associated data `weight` to the graph. + /// + /// Computes in **O(1)** time. + /// + /// Return the index of the new node. + /// + /// **Panics** if the Graph is at the maximum number of nodes for its index + /// type (N/A if usize). + pub fn add_node(&mut self, weight: N) -> NodeIndex { + let node = Node { + weight, + next: [EdgeIndex::end(), EdgeIndex::end()], + }; + assert!(self.nodes.len() != crate::INVALID_USIZE); + let node_idx = NodeIndex::new(self.nodes.len() as u32); + self.nodes.push(node); + node_idx + } + + /// Access the weight for node `a`. + /// + /// Also available with indexing syntax: `&graph[a]`. + pub fn node_weight(&self, a: NodeIndex) -> Option<&N> { + self.nodes.get(a.index()).map(|n| &n.weight) + } + + /// Access the weight for edge `a`. + /// + /// Also available with indexing syntax: `&graph[a]`. + pub fn edge_weight(&self, a: EdgeIndex) -> Option<&E> { + self.edges.get(a.index()).map(|e| &e.weight) + } + + /// Access the weight for edge `a` mutably. + /// + /// Also available with indexing syntax: `&mut graph[a]`. + pub fn edge_weight_mut(&mut self, a: EdgeIndex) -> Option<&mut E> { + self.edges.get_mut(a.index()).map(|e| &mut e.weight) + } + + /// Add an edge from `a` to `b` to the graph, with its associated + /// data `weight`. + /// + /// Return the index of the new edge. + /// + /// Computes in **O(1)** time. + /// + /// **Panics** if any of the nodes don't exist.
+ /// **Panics** if the Graph is at the maximum number of edges for its index + /// type (N/A if usize). + /// + /// **Note:** `Graph` allows adding parallel (“duplicate”) edges. If you want + /// to avoid this, use [`.update_edge(a, b, weight)`](#method.update_edge) instead. + pub fn add_edge(&mut self, a: NodeIndex, b: NodeIndex, weight: E) -> EdgeIndex { + assert!(self.edges.len() != crate::INVALID_USIZE); + let edge_idx = EdgeIndex::new(self.edges.len() as u32); + let mut edge = Edge { + weight, + node: [a, b], + next: [EdgeIndex::end(); 2], + }; + match index_twice(&mut self.nodes, a.index(), b.index()) { + Pair::None => panic!("Graph::add_edge: node indices out of bounds"), + Pair::One(an) => { + edge.next = an.next; + an.next[0] = edge_idx; + an.next[1] = edge_idx; + } + Pair::Both(an, bn) => { + // a and b are different indices + edge.next = [an.next[0], bn.next[1]]; + an.next[0] = edge_idx; + bn.next[1] = edge_idx; + } + } + self.edges.push(edge); + edge_idx + } + + /// Access the source and target nodes for `e`. + pub fn edge_endpoints(&self, e: EdgeIndex) -> Option<(NodeIndex, NodeIndex)> { + self.edges + .get(e.index()) + .map(|ed| (ed.source(), ed.target())) + } + + /// Remove `a` from the graph if it exists, and return its weight. + /// If it doesn't exist in the graph, return `None`. + /// + /// Apart from `a`, this invalidates the last node index in the graph + /// (that node will adopt the removed node index). Edge indices are + /// invalidated as they would be following the removal of each edge + /// with an endpoint in `a`. + /// + /// Computes in **O(e')** time, where **e'** is the number of affected + /// edges, including *n* calls to `.remove_edge()` where *n* is the number + /// of edges with an endpoint in `a`, and including the edges with an + /// endpoint in the displaced node. + pub fn remove_node(&mut self, a: NodeIndex) -> Option { + self.nodes.get(a.index())?; + for d in &DIRECTIONS { + let k = *d as usize; + + // Remove all edges from and to this node. + loop { + let next = self.nodes[a.index()].next[k]; + if next == EdgeIndex::end() { + break; + } + let ret = self.remove_edge(next); + debug_assert!(ret.is_some()); + let _ = ret; + } + } + + // Use swap_remove -- only the swapped-in node is going to change + // NodeIndex, so we only have to walk its edges and update them. + + let node = self.nodes.swap_remove(a.index()); + + // Find the edge lists of the node that had to relocate. + // It may be that no node had to relocate, then we are done already. + let swap_edges = match self.nodes.get(a.index()) { + None => return Some(node.weight), + Some(ed) => ed.next, + }; + + // The swapped element's old index + let old_index = NodeIndex::new(self.nodes.len() as u32); + let new_index = a; + + // Adjust the starts of the out edges, and ends of the in edges. + for &d in &DIRECTIONS { + let k = d as usize; + let mut edges = edges_walker_mut(&mut self.edges, swap_edges[k], d); + while let Some(curedge) = edges.next_edge() { + debug_assert!(curedge.node[k] == old_index); + curedge.node[k] = new_index; + } + } + Some(node.weight) + } + + /// For edge `e` with endpoints `edge_node`, replace links to it, + /// with links to `edge_next`. + fn change_edge_links( + &mut self, + edge_node: [NodeIndex; 2], + e: EdgeIndex, + edge_next: [EdgeIndex; 2], + ) { + for &d in &DIRECTIONS { + let k = d as usize; + let node = match self.nodes.get_mut(edge_node[k].index()) { + Some(r) => r, + None => { + debug_assert!( + false, + "Edge's endpoint dir={:?} index={:?} not found", + d, edge_node[k] + ); + return; + } + }; + let fst = node.next[k]; + if fst == e { + //println!("Updating first edge 0 for node {}, set to {}", edge_node[0], edge_next[0]); + node.next[k] = edge_next[k]; + } else { + let mut edges = edges_walker_mut(&mut self.edges, fst, d); + while let Some(curedge) = edges.next_edge() { + if curedge.next[k] == e { + curedge.next[k] = edge_next[k]; + break; // the edge can only be present once in the list. + } + } + } + } + } + + /// Remove an edge and return its edge weight, or `None` if it didn't exist. + /// + /// Apart from `e`, this invalidates the last edge index in the graph + /// (that edge will adopt the removed edge index). + /// + /// Computes in **O(e')** time, where **e'** is the size of four particular edge lists, for + /// the vertices of `e` and the vertices of another affected edge. + pub fn remove_edge(&mut self, e: EdgeIndex) -> Option { + // every edge is part of two lists, + // outgoing and incoming edges. + // Remove it from both + let (edge_node, edge_next) = match self.edges.get(e.index()) { + None => return None, + Some(x) => (x.node, x.next), + }; + // Remove the edge from its in and out lists by replacing it with + // a link to the next in the list. + self.change_edge_links(edge_node, e, edge_next); + self.remove_edge_adjust_indices(e) + } + + fn remove_edge_adjust_indices(&mut self, e: EdgeIndex) -> Option { + // swap_remove the edge -- only the removed edge + // and the edge swapped into place are affected and need updating + // indices. + let edge = self.edges.swap_remove(e.index()); + let swap = match self.edges.get(e.index()) { + // no elment needed to be swapped. + None => return Some(edge.weight), + Some(ed) => ed.node, + }; + let swapped_e = EdgeIndex::new(self.edges.len() as u32); + + // Update the edge lists by replacing links to the old index by references to the new + // edge index. + self.change_edge_links(swap, swapped_e, [e, e]); + Some(edge.weight) + } + + /// Return an iterator of all edges of `a`. + /// + /// - `Directed`: Outgoing edges from `a`. + /// - `Undirected`: All edges connected to `a`. + /// + /// Produces an empty iterator if the node doesn't exist.
+ /// Iterator element type is `EdgeReference`. + pub fn edges(&self, a: NodeIndex) -> Edges { + self.edges_directed(a, Direction::Outgoing) + } + + /// Return an iterator of all edges of `a`, in the specified direction. + /// + /// - `Directed`, `Outgoing`: All edges from `a`. + /// - `Directed`, `Incoming`: All edges to `a`. + /// - `Undirected`, `Outgoing`: All edges connected to `a`, with `a` being the source of each + /// edge. + /// - `Undirected`, `Incoming`: All edges connected to `a`, with `a` being the target of each + /// edge. + /// + /// Produces an empty iterator if the node `a` doesn't exist.
+ /// Iterator element type is `EdgeReference`. + pub fn edges_directed(&self, a: NodeIndex, dir: Direction) -> Edges { + Edges { + skip_start: a, + edges: &self.edges, + direction: dir, + next: match self.nodes.get(a.index()) { + None => [EdgeIndex::end(), EdgeIndex::end()], + Some(n) => n.next, + }, + } + } + + /* + /// Return an iterator over all the edges connecting `a` and `b`. + /// + /// - `Directed`: Outgoing edges from `a`. + /// - `Undirected`: All edges connected to `a`. + /// + /// Iterator element type is `EdgeReference`. + pub fn edges_connecting(&self, a: NodeIndex, b: NodeIndex) -> EdgesConnecting { + EdgesConnecting { + target_node: b, + edges: self.edges_directed(a, Direction::Outgoing), + ty: PhantomData, + } + } + */ + + /// Lookup an edge from `a` to `b`. + /// + /// Computes in **O(e')** time, where **e'** is the number of edges + /// connected to `a` (and `b`, if the graph edges are undirected). + pub fn find_edge(&self, a: NodeIndex, b: NodeIndex) -> Option { + self.find_edge_undirected(a, b).map(|(ix, _)| ix) + } + + /// Lookup an edge between `a` and `b`, in either direction. + /// + /// If the graph is undirected, then this is equivalent to `.find_edge()`. + /// + /// Return the edge index and its directionality, with `Outgoing` meaning + /// from `a` to `b` and `Incoming` the reverse, + /// or `None` if the edge does not exist. + pub fn find_edge_undirected( + &self, + a: NodeIndex, + b: NodeIndex, + ) -> Option<(EdgeIndex, Direction)> { + match self.nodes.get(a.index()) { + None => None, + Some(node) => self.find_edge_undirected_from_node(node, b), + } + } + + fn find_edge_undirected_from_node( + &self, + node: &Node, + b: NodeIndex, + ) -> Option<(EdgeIndex, Direction)> { + for &d in &DIRECTIONS { + let k = d as usize; + let mut edix = node.next[k]; + while let Some(edge) = self.edges.get(edix.index()) { + if edge.node[1 - k] == b { + return Some((edix, d)); + } + edix = edge.next[k]; + } + } + None + } + + /// Access the internal node array. + pub fn raw_nodes(&self) -> &[Node] { + &self.nodes + } + + /// Access the internal edge array. + pub fn raw_edges(&self) -> &[Edge] { + &self.edges + } + + /// Accessor for data structure internals: the first edge in the given direction. + pub fn first_edge(&self, a: NodeIndex, dir: Direction) -> Option { + match self.nodes.get(a.index()) { + None => None, + Some(node) => { + let edix = node.next[dir as usize]; + if edix == EdgeIndex::end() { + None + } else { + Some(edix) + } + } + } + } + + /// Accessor for data structure internals: the next edge for the given direction. + pub fn next_edge(&self, e: EdgeIndex, dir: Direction) -> Option { + match self.edges.get(e.index()) { + None => None, + Some(node) => { + let edix = node.next[dir as usize]; + if edix == EdgeIndex::end() { + None + } else { + Some(edix) + } + } + } + } +} + +/// An iterator over either the nodes without edges to them or from them. +pub struct Externals<'a, N: 'a> { + iter: std::iter::Enumerate>>, + dir: Direction, +} + +impl<'a, N: 'a> Iterator for Externals<'a, N> { + type Item = NodeIndex; + fn next(&mut self) -> Option { + let k = self.dir as usize; + loop { + match self.iter.next() { + None => return None, + Some((index, node)) => { + if node.next[k] == EdgeIndex::end() && node.next[1 - k] == EdgeIndex::end() { + return Some(NodeIndex::new(index as u32)); + } else { + continue; + } + } + } + } + } +} + +/// Iterator over the neighbors of a node. +/// +/// Iterator element type is `NodeIndex`. +/// +/// Created with [`.neighbors()`][1], [`.neighbors_directed()`][2] or +/// [`.neighbors_undirected()`][3]. +/// +/// [1]: struct.Graph.html#method.neighbors +/// [2]: struct.Graph.html#method.neighbors_directed +/// [3]: struct.Graph.html#method.neighbors_undirected +pub struct Neighbors<'a, E: 'a> { + /// starting node to skip over + skip_start: NodeIndex, + edges: &'a [Edge], + next: [EdgeIndex; 2], +} + +impl<'a, E> Iterator for Neighbors<'a, E> { + type Item = NodeIndex; + + fn next(&mut self) -> Option { + // First any outgoing edges + match self.edges.get(self.next[0].index()) { + None => {} + Some(edge) => { + self.next[0] = edge.next[0]; + return Some(edge.node[1]); + } + } + // Then incoming edges + // For an "undirected" iterator (traverse both incoming + // and outgoing edge lists), make sure we don't double + // count selfloops by skipping them in the incoming list. + while let Some(edge) = self.edges.get(self.next[1].index()) { + self.next[1] = edge.next[1]; + if edge.node[0] != self.skip_start { + return Some(edge.node[0]); + } + } + None + } +} + +struct EdgesWalkerMut<'a, E: 'a> { + edges: &'a mut [Edge], + next: EdgeIndex, + dir: Direction, +} + +fn edges_walker_mut( + edges: &mut [Edge], + next: EdgeIndex, + dir: Direction, +) -> EdgesWalkerMut { + EdgesWalkerMut { edges, next, dir } +} + +impl<'a, E> EdgesWalkerMut<'a, E> { + fn next_edge(&mut self) -> Option<&mut Edge> { + self.next().map(|t| t.1) + } + + fn next(&mut self) -> Option<(EdgeIndex, &mut Edge)> { + let this_index = self.next; + let k = self.dir as usize; + match self.edges.get_mut(self.next.index()) { + None => None, + Some(edge) => { + self.next = edge.next[k]; + Some((this_index, edge)) + } + } + } +} + +/// Iterator over the edges of from or to a node +pub struct Edges<'a, E: 'a> { + /// starting node to skip over + skip_start: NodeIndex, + edges: &'a [Edge], + + /// Next edge to visit. + next: [EdgeIndex; 2], + + /// For directed graphs: the direction to iterate in + /// For undirected graphs: the direction of edges + direction: Direction, +} + +impl<'a, E> Iterator for Edges<'a, E> { + type Item = EdgeReference<'a, E>; + + fn next(&mut self) -> Option { + // type direction | iterate over reverse + // | + // Directed Outgoing | outgoing no + // Directed Incoming | incoming no + // Undirected Outgoing | both incoming + // Undirected Incoming | both outgoing + + // For iterate_over, "both" is represented as None. + // For reverse, "no" is represented as None. + let (iterate_over, reverse) = (None, Some(self.direction.opposite())); + + if iterate_over.unwrap_or(Direction::Outgoing) == Direction::Outgoing { + let i = self.next[0].index(); + if let Some(Edge { node, weight, next }) = self.edges.get(i) { + self.next[0] = next[0]; + return Some(EdgeReference { + index: EdgeIndex(i as u32), + node: if reverse == Some(Direction::Outgoing) { + swap_pair(*node) + } else { + *node + }, + weight, + }); + } + } + + if iterate_over.unwrap_or(Direction::Incoming) == Direction::Incoming { + while let Some(Edge { node, weight, next }) = self.edges.get(self.next[1].index()) { + let edge_index = self.next[1]; + self.next[1] = next[1]; + // In any of the "both" situations, self-loops would be iterated over twice. + // Skip them here. + if iterate_over.is_none() && node[0] == self.skip_start { + continue; + } + + return Some(EdgeReference { + index: edge_index, + node: if reverse == Some(Direction::Incoming) { + swap_pair(*node) + } else { + *node + }, + weight, + }); + } + } + + None + } +} + +fn swap_pair(mut x: [T; 2]) -> [T; 2] { + x.swap(0, 1); + x +} + +impl<'a, E> Clone for Edges<'a, E> { + fn clone(&self) -> Self { + Edges { + skip_start: self.skip_start, + edges: self.edges, + next: self.next, + direction: self.direction, + } + } +} + +/// Index the `Graph` by `NodeIndex` to access node weights. +/// +/// **Panics** if the node doesn't exist. +impl Index for Graph { + type Output = N; + fn index(&self, index: NodeIndex) -> &N { + &self.nodes[index.index()].weight + } +} + +/// Index the `Graph` by `NodeIndex` to access node weights. +/// +/// **Panics** if the node doesn't exist. +impl IndexMut for Graph { + fn index_mut(&mut self, index: NodeIndex) -> &mut N { + &mut self.nodes[index.index()].weight + } +} + +/// Index the `Graph` by `EdgeIndex` to access edge weights. +/// +/// **Panics** if the edge doesn't exist. +impl Index for Graph { + type Output = E; + fn index(&self, index: EdgeIndex) -> &E { + &self.edges[index.index()].weight + } +} + +/// Index the `Graph` by `EdgeIndex` to access edge weights. +/// +/// **Panics** if the edge doesn't exist. +impl IndexMut for Graph { + fn index_mut(&mut self, index: EdgeIndex) -> &mut E { + &mut self.edges[index.index()].weight + } +} + +/// A “walker” object that can be used to step through the edge list of a node. +/// +/// Created with [`.detach()`](struct.Neighbors.html#method.detach). +/// +/// The walker does not borrow from the graph, so it lets you step through +/// neighbors or incident edges while also mutating graph weights, as +/// in the following example: +pub struct WalkNeighbors { + skip_start: NodeIndex, + next: [EdgeIndex; 2], +} + +impl Clone for WalkNeighbors { + fn clone(&self) -> Self { + WalkNeighbors { + skip_start: self.skip_start, + next: self.next, + } + } +} + +/// Reference to a `Graph` edge. +#[derive(Debug)] +pub struct EdgeReference<'a, E: 'a> { + index: EdgeIndex, + node: [NodeIndex; 2], + weight: &'a E, +} + +impl<'a, E: 'a> EdgeReference<'a, E> { + #[inline] + pub fn id(&self) -> EdgeIndex { + self.index + } + + #[inline] + pub fn weight(&self) -> &'a E { + self.weight + } +} + +impl<'a, E> Clone for EdgeReference<'a, E> { + fn clone(&self) -> Self { + *self + } +} + +impl<'a, E> Copy for EdgeReference<'a, E> {} + +impl<'a, E> PartialEq for EdgeReference<'a, E> +where + E: PartialEq, +{ + fn eq(&self, rhs: &Self) -> bool { + self.index == rhs.index && self.weight == rhs.weight + } +} + +/// Iterator over all nodes of a graph. +pub struct NodeReferences<'a, N: 'a> { + iter: std::iter::Enumerate>>, +} + +impl<'a, N> Iterator for NodeReferences<'a, N> { + type Item = (NodeIndex, &'a N); + + fn next(&mut self) -> Option { + self.iter + .next() + .map(|(i, node)| (NodeIndex::new(i as u32), &node.weight)) + } + + fn size_hint(&self) -> (usize, Option) { + self.iter.size_hint() + } +} + +impl<'a, N> DoubleEndedIterator for NodeReferences<'a, N> { + fn next_back(&mut self) -> Option { + self.iter + .next_back() + .map(|(i, node)| (NodeIndex::new(i as u32), &node.weight)) + } +} + +impl<'a, N> ExactSizeIterator for NodeReferences<'a, N> {} diff --git a/src/data/mod.rs b/src/data/mod.rs new file mode 100644 index 000000000..7c004428b --- /dev/null +++ b/src/data/mod.rs @@ -0,0 +1,4 @@ +//! Data structures modified with guaranteed deterministic behavior after deserialization. + +pub mod arena; +pub(crate) mod graph; diff --git a/src/dynamics/integration_parameters.rs b/src/dynamics/integration_parameters.rs new file mode 100644 index 000000000..faf22e12b --- /dev/null +++ b/src/dynamics/integration_parameters.rs @@ -0,0 +1,207 @@ +/// Parameters for a time-step of the physics engine. +#[derive(Clone)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct IntegrationParameters { + /// The timestep (default: `1.0 / 60.0`) + dt: f32, + /// The inverse of `dt`. + inv_dt: f32, + // /// If `true` and if rapier is compiled with the `parallel` feature, this will enable rayon-based multithreading (default: `true`). + // /// + // /// This parameter is ignored if rapier is not compiled with is `parallel` feature. + // /// Refer to rayon's documentation regarding how to configure the number of threads with either + // /// `rayon::ThreadPoolBuilder::new().num_threads(4).build_global().unwrap()` or `ThreadPool::install`. + // /// Note that using only one thread with `multithreading_enabled` set to `true` will result on a slower + // /// simulation than setting `multithreading_enabled` to `false`. + // pub multithreading_enabled: bool, + /// If `true`, the world's `step` method will stop right after resolving exactly one CCD event (default: `false`). + /// This allows the user to take action during a timestep, in-between two CCD events. + pub return_after_ccd_substep: bool, + /// The Error Reduction Parameter in `[0, 1]` is the proportion of + /// the positional error to be corrected at each time step (default: `0.2`). + pub erp: f32, + /// The Error Reduction Parameter for joints in `[0, 1]` is the proportion of + /// the positional error to be corrected at each time step (default: `0.2`). + pub joint_erp: f32, + /// Each cached impulse are multiplied by this coefficient in `[0, 1]` + /// when they are re-used to initialize the solver (default `1.0`). + pub warmstart_coeff: f32, + /// Contacts at points where the involved bodies have a relative + /// velocity smaller than this threshold wont be affected by the restitution force (default: `1.0`). + pub restitution_velocity_threshold: f32, + /// Amount of penetration the engine wont attempt to correct (default: `0.001m`). + pub allowed_linear_error: f32, + /// The maximal distance separating two objects that will generate predictive contacts (default: `0.002`). + pub prediction_distance: f32, + /// Amount of angular drift of joint limits the engine wont + /// attempt to correct (default: `0.001rad`). + pub allowed_angular_error: f32, + /// Maximum linear correction during one step of the non-linear position solver (default: `0.2`). + pub max_linear_correction: f32, + /// Maximum angular correction during one step of the non-linear position solver (default: `0.2`). + pub max_angular_correction: f32, + /// Maximum nonlinear SOR-prox scaling parameter when the constraint + /// correction direction is close to the kernel of the involved multibody's + /// jacobian (default: `0.2`). + pub max_stabilization_multiplier: f32, + /// Maximum number of iterations performed by the velocity constraints solver (default: `4`). + pub max_velocity_iterations: usize, + /// Maximum number of iterations performed by the position-based constraints solver (default: `1`). + pub max_position_iterations: usize, + /// Minimum number of dynamic bodies in each active island (default: `128`). + pub min_island_size: usize, + /// Maximum number of iterations performed by the position-based constraints solver for CCD steps (default: `10`). + /// + /// This should be sufficiently high so all penetration get resolved. For example, if CCD cause your + /// objects to stutter, that may be because the number of CCD position iterations is too low, causing + /// them to remain stuck in a penetration configuration for a few frames. + /// + /// The highest this number, the highest its computational cost. + pub max_ccd_position_iterations: usize, + /// Maximum number of substeps performed by the solver (default: `1`). + pub max_ccd_substeps: usize, + /// Controls the number of Proximity::Intersecting events generated by a trigger during CCD resolution (default: `false`). + /// + /// If false, triggers will only generate one Proximity::Intersecting event during a step, even + /// if another colliders repeatedly enters and leaves the triggers during multiple CCD substeps. + /// + /// If true, triggers will generate as many Proximity::Intersecting and Proximity::Disjoint/Proximity::WithinMargin + /// events as the number of times a collider repeatedly enters and leaves the triggers during multiple CCD substeps. + /// This is more computationally intensive. + pub multiple_ccd_substep_sensor_events_enabled: bool, + /// Whether penetration are taken into account in CCD resolution (default: `false`). + /// + /// If this is set to `false` two penetrating colliders will not be considered to have any time of impact + /// while they are penetrating. This may end up allowing some tunelling, but will avoid stuttering effect + /// when the constraints solver fails to completely separate two colliders after a CCD contact. + /// + /// If this is set to `true`, two penetrating colliders will be considered to have a time of impact + /// equal to 0 until the constraints solver manages to separate them. This will prevent tunnelling + /// almost completely, but may introduce stuttering effects when the constraints solver fails to completely + /// separate two colliders after a CCD contact. + // FIXME: this is a very binary way of handling penetration. + // We should provide a more flexible solution by letting the user choose some + // minimal amount of movement applied to an object that get stuck. + pub ccd_on_penetration_enabled: bool, +} + +impl IntegrationParameters { + /// Creates a set of integration parameters with the given values. + pub fn new( + dt: f32, + // multithreading_enabled: bool, + erp: f32, + joint_erp: f32, + warmstart_coeff: f32, + restitution_velocity_threshold: f32, + allowed_linear_error: f32, + allowed_angular_error: f32, + max_linear_correction: f32, + max_angular_correction: f32, + prediction_distance: f32, + max_stabilization_multiplier: f32, + max_velocity_iterations: usize, + max_position_iterations: usize, + max_ccd_position_iterations: usize, + max_ccd_substeps: usize, + return_after_ccd_substep: bool, + multiple_ccd_substep_sensor_events_enabled: bool, + ccd_on_penetration_enabled: bool, + ) -> Self { + IntegrationParameters { + dt, + inv_dt: if dt == 0.0 { 0.0 } else { 1.0 / dt }, + // multithreading_enabled, + erp, + joint_erp, + warmstart_coeff, + restitution_velocity_threshold, + allowed_linear_error, + allowed_angular_error, + max_linear_correction, + max_angular_correction, + prediction_distance, + max_stabilization_multiplier, + max_velocity_iterations, + max_position_iterations, + // FIXME: what is the optimal value for min_island_size? + // It should not be too big so that we don't end up with + // huge islands that don't fit in cache. + // However we don't want it to be too small and end up with + // tons of islands, reducing SIMD parallelism opportunities. + min_island_size: 128, + max_ccd_position_iterations, + max_ccd_substeps, + return_after_ccd_substep, + multiple_ccd_substep_sensor_events_enabled, + ccd_on_penetration_enabled, + } + } + + /// The current time-stepping length. + #[inline(always)] + pub fn dt(&self) -> f32 { + self.dt + } + + /// The inverse of the time-stepping length. + /// + /// This is zero if `self.dt` is zero. + #[inline(always)] + pub fn inv_dt(&self) -> f32 { + self.inv_dt + } + + /// Sets the time-stepping length. + /// + /// This automatically recompute `self.inv_dt`. + #[inline] + pub fn set_dt(&mut self, dt: f32) { + assert!(dt >= 0.0, "The time-stepping length cannot be negative."); + self.dt = dt; + if dt == 0.0 { + self.inv_dt = 0.0 + } else { + self.inv_dt = 1.0 / dt + } + } + + /// Sets the inverse time-stepping length (i.e. the frequency). + /// + /// This automatically recompute `self.dt`. + #[inline] + pub fn set_inv_dt(&mut self, inv_dt: f32) { + self.inv_dt = inv_dt; + if inv_dt == 0.0 { + self.dt = 0.0 + } else { + self.dt = 1.0 / inv_dt + } + } +} + +impl Default for IntegrationParameters { + fn default() -> Self { + Self::new( + 1.0 / 60.0, + // true, + 0.2, + 0.2, + 1.0, + 1.0, + 0.005, + 0.001, + 0.2, + 0.2, + 0.002, + 0.2, + 4, + 1, + 10, + 1, + false, + false, + false, + ) + } +} diff --git a/src/dynamics/joint/ball_joint.rs b/src/dynamics/joint/ball_joint.rs new file mode 100644 index 000000000..ec255d49d --- /dev/null +++ b/src/dynamics/joint/ball_joint.rs @@ -0,0 +1,34 @@ +use crate::math::{Point, Vector}; + +#[derive(Copy, Clone)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// A joint that removes all relative linear motion between a pair of points on two bodies. +pub struct BallJoint { + /// Where the ball joint is attached on the first body, expressed in the first body local frame. + pub local_anchor1: Point, + /// Where the ball joint is attached on the first body, expressed in the first body local frame. + pub local_anchor2: Point, + /// The impulse applied by this joint on the first body. + /// + /// The impulse applied to the second body is given by `-impulse`. + pub impulse: Vector, +} + +impl BallJoint { + /// Creates a new Ball joint from two anchors given on the local spaces of the respective bodies. + pub fn new(local_anchor1: Point, local_anchor2: Point) -> Self { + Self::with_impulse(local_anchor1, local_anchor2, Vector::zeros()) + } + + pub(crate) fn with_impulse( + local_anchor1: Point, + local_anchor2: Point, + impulse: Vector, + ) -> Self { + Self { + local_anchor1, + local_anchor2, + impulse, + } + } +} diff --git a/src/dynamics/joint/fixed_joint.rs b/src/dynamics/joint/fixed_joint.rs new file mode 100644 index 000000000..0731cfb09 --- /dev/null +++ b/src/dynamics/joint/fixed_joint.rs @@ -0,0 +1,33 @@ +use crate::math::{Isometry, SpacialVector}; + +#[derive(Copy, Clone)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// A joint that prevents all relative movement between two bodies. +/// +/// Given two frames of references, this joint aims to ensure these frame always coincide in world-space. +pub struct FixedJoint { + /// The frame of reference for the first body affected by this joint, expressed in the local frame + /// of the first body. + pub local_anchor1: Isometry, + /// The frame of reference for the second body affected by this joint, expressed in the local frame + /// of the first body. + pub local_anchor2: Isometry, + /// The impulse applied to the first body affected by this joint. + /// + /// The impulse applied to the second body affected by this joint is given by `-impulse`. + /// This combines both linear and angular impulses: + /// - In 2D, `impulse.xy()` gives the linear impulse, and `impulse.z` the angular impulse. + /// - In 3D, `impulse.xyz()` gives the linear impulse, and `(impulse[3], impulse[4], impulse[5])` the angular impulse. + pub impulse: SpacialVector, +} + +impl FixedJoint { + /// Creates a new fixed joint from the frames of reference of both bodies. + pub fn new(local_anchor1: Isometry, local_anchor2: Isometry) -> Self { + Self { + local_anchor1, + local_anchor2, + impulse: SpacialVector::zeros(), + } + } +} diff --git a/src/dynamics/joint/joint.rs b/src/dynamics/joint/joint.rs new file mode 100644 index 000000000..074f802d5 --- /dev/null +++ b/src/dynamics/joint/joint.rs @@ -0,0 +1,112 @@ +#[cfg(feature = "dim3")] +use crate::dynamics::RevoluteJoint; +use crate::dynamics::{BallJoint, FixedJoint, JointHandle, PrismaticJoint, RigidBodyHandle}; + +#[derive(Copy, Clone)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// An enum grouping all possible types of joints. +pub enum JointParams { + /// A Ball joint that removes all relative linear degrees of freedom between the affected bodies. + BallJoint(BallJoint), + /// A fixed joint that removes all relative degrees of freedom between the affected bodies. + FixedJoint(FixedJoint), + /// A prismatic joint that removes all degrees of degrees of freedom between the affected + /// bodies except for the translation along one axis. + PrismaticJoint(PrismaticJoint), + #[cfg(feature = "dim3")] + /// A revolute joint that removes all degrees of degrees of freedom between the affected + /// bodies except for the translation along one axis. + RevoluteJoint(RevoluteJoint), +} + +impl JointParams { + /// An integer identifier for each type of joint. + pub fn type_id(&self) -> usize { + match self { + JointParams::BallJoint(_) => 0, + JointParams::FixedJoint(_) => 1, + JointParams::PrismaticJoint(_) => 2, + #[cfg(feature = "dim3")] + JointParams::RevoluteJoint(_) => 3, + } + } + + /// Gets a reference to the underlying ball joint, if `self` is one. + pub fn as_ball_joint(&self) -> Option<&BallJoint> { + if let JointParams::BallJoint(j) = self { + Some(j) + } else { + None + } + } + + /// Gets a reference to the underlying fixed joint, if `self` is one. + pub fn as_fixed_joint(&self) -> Option<&FixedJoint> { + if let JointParams::FixedJoint(j) = self { + Some(j) + } else { + None + } + } + + /// Gets a reference to the underlying prismatic joint, if `self` is one. + pub fn as_prismatic_joint(&self) -> Option<&PrismaticJoint> { + if let JointParams::PrismaticJoint(j) = self { + Some(j) + } else { + None + } + } + + /// Gets a reference to the underlying revolute joint, if `self` is one. + #[cfg(feature = "dim3")] + pub fn as_revolute_joint(&self) -> Option<&RevoluteJoint> { + if let JointParams::RevoluteJoint(j) = self { + Some(j) + } else { + None + } + } +} + +impl From for JointParams { + fn from(j: BallJoint) -> Self { + JointParams::BallJoint(j) + } +} + +impl From for JointParams { + fn from(j: FixedJoint) -> Self { + JointParams::FixedJoint(j) + } +} + +#[cfg(feature = "dim3")] +impl From for JointParams { + fn from(j: RevoluteJoint) -> Self { + JointParams::RevoluteJoint(j) + } +} + +impl From for JointParams { + fn from(j: PrismaticJoint) -> Self { + JointParams::PrismaticJoint(j) + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// A joint attached to two bodies. +pub struct Joint { + /// Handle to the first body attached to this joint. + pub body1: RigidBodyHandle, + /// Handle to the second body attached to this joint. + pub body2: RigidBodyHandle, + // A joint needs to know its handle to simplify its removal. + pub(crate) handle: JointHandle, + #[cfg(feature = "parallel")] + pub(crate) constraint_index: usize, + #[cfg(feature = "parallel")] + pub(crate) position_constraint_index: usize, + /// The joint geometric parameters and impulse. + pub params: JointParams, +} diff --git a/src/dynamics/joint/joint_set.rs b/src/dynamics/joint/joint_set.rs new file mode 100644 index 000000000..51d743289 --- /dev/null +++ b/src/dynamics/joint/joint_set.rs @@ -0,0 +1,218 @@ +use super::Joint; +use crate::geometry::{InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex}; + +use crate::data::arena::{Arena, Index}; +use crate::dynamics::{JointParams, RigidBodyHandle, RigidBodySet}; + +/// The unique identifier of a joint added to the joint set. +pub type JointHandle = Index; +pub(crate) type JointIndex = usize; +pub(crate) type JointGraphEdge = crate::data::graph::Edge; + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// A set of joints that can be handled by a physics `World`. +pub struct JointSet { + joint_ids: Arena, // Map joint handles to edge ids on the graph. + joint_graph: InteractionGraph, +} + +impl JointSet { + /// Creates a new empty set of joints. + pub fn new() -> Self { + Self { + joint_ids: Arena::new(), + joint_graph: InteractionGraph::new(), + } + } + + /// An always-invalid joint handle. + pub fn invalid_handle() -> JointHandle { + JointHandle::from_raw_parts(crate::INVALID_USIZE, crate::INVALID_U64) + } + + /// The number of joints on this set. + pub fn len(&self) -> usize { + self.joint_graph.graph.edges.len() + } + + /// Retrieve the joint graph where edges are joints and nodes are rigid body handles. + pub fn joint_graph(&self) -> &InteractionGraph { + &self.joint_graph + } + + /// Is the given joint handle valid? + pub fn contains(&self, handle: JointHandle) -> bool { + self.joint_ids.contains(handle) + } + + /// Gets the joint with the given handle. + pub fn get(&self, handle: JointHandle) -> Option<&Joint> { + let id = self.joint_ids.get(handle)?; + self.joint_graph.graph.edge_weight(*id) + } + + /// Gets the joint with the given handle without a known generation. + /// + /// This is useful when you know you want the joint at position `i` but + /// don't know what is its current generation number. Generation numbers are + /// used to protect from the ABA problem because the joint position `i` + /// are recycled between two insertion and a removal. + /// + /// Using this is discouraged in favor of `self.get(handle)` which does not + /// suffer form the ABA problem. + pub fn get_unknown_gen(&self, i: usize) -> Option<(&Joint, JointHandle)> { + let (id, handle) = self.joint_ids.get_unknown_gen(i)?; + Some((self.joint_graph.graph.edge_weight(*id)?, handle)) + } + + /// Iterates through all the joint on this set. + pub fn iter(&self) -> impl Iterator { + self.joint_graph.graph.edges.iter().map(|e| &e.weight) + } + + /// Iterates mutably through all the joint on this set. + pub fn iter_mut(&mut self) -> impl Iterator { + self.joint_graph + .graph + .edges + .iter_mut() + .map(|e| &mut e.weight) + } + + // /// The set of joints as an array. + // pub(crate) fn joints(&self) -> &[JointGraphEdge] { + // // self.joint_graph + // // .graph + // // .edges + // // .iter_mut() + // // .map(|e| &mut e.weight) + // } + + #[cfg(not(feature = "parallel"))] + pub(crate) fn joints_mut(&mut self) -> &mut [JointGraphEdge] { + &mut self.joint_graph.graph.edges[..] + } + + #[cfg(feature = "parallel")] + pub(crate) fn joints_vec_mut(&mut self) -> &mut Vec { + &mut self.joint_graph.graph.edges + } + + /// Inserts a new joint into this set and retrieve its handle. + pub fn insert( + &mut self, + bodies: &mut RigidBodySet, + body1: RigidBodyHandle, + body2: RigidBodyHandle, + joint_params: J, + ) -> JointHandle + where + J: Into, + { + let handle = self.joint_ids.insert(0.into()); + let joint = Joint { + body1, + body2, + handle, + #[cfg(feature = "parallel")] + constraint_index: 0, + #[cfg(feature = "parallel")] + position_constraint_index: 0, + params: joint_params.into(), + }; + + let (rb1, rb2) = bodies.get2_mut_internal(joint.body1, joint.body2); + let (rb1, rb2) = ( + rb1.expect("Attempt to attach a joint to a non-existing body."), + rb2.expect("Attempt to attach a joint to a non-existing body."), + ); + + // NOTE: the body won't have a graph index if it does not + // have any joint attached. + if !InteractionGraph::::is_graph_index_valid(rb1.joint_graph_index) { + rb1.joint_graph_index = self.joint_graph.graph.add_node(joint.body1); + } + + if !InteractionGraph::::is_graph_index_valid(rb2.joint_graph_index) { + rb2.joint_graph_index = self.joint_graph.graph.add_node(joint.body2); + } + + let id = self + .joint_graph + .add_edge(rb1.joint_graph_index, rb2.joint_graph_index, joint); + + self.joint_ids[handle] = id; + handle + } + + /// Retrieve all the joints happening between two active bodies. + // NOTE: this is very similar to the code from NarrowPhase::select_active_interactions. + pub(crate) fn select_active_interactions( + &self, + bodies: &RigidBodySet, + out: &mut Vec>, + ) { + for out_island in &mut out[..bodies.num_islands()] { + out_island.clear(); + } + + // FIXME: don't iterate through all the interactions. + for (i, edge) in self.joint_graph.graph.edges.iter().enumerate() { + let joint = &edge.weight; + let rb1 = &bodies[joint.body1]; + let rb2 = &bodies[joint.body2]; + + if (rb1.is_dynamic() || rb2.is_dynamic()) + && (!rb1.is_dynamic() || !rb1.is_sleeping()) + && (!rb2.is_dynamic() || !rb2.is_sleeping()) + { + let island_index = if !rb1.is_dynamic() { + rb2.active_island_id + } else { + rb1.active_island_id + }; + + out[island_index].push(i); + } + } + } + + pub(crate) fn remove_rigid_body( + &mut self, + deleted_id: RigidBodyGraphIndex, + bodies: &mut RigidBodySet, + ) { + if InteractionGraph::<()>::is_graph_index_valid(deleted_id) { + // We have to delete each joint one by one in order to: + // - Wake-up the attached bodies. + // - Update our Handle -> graph edge mapping. + // Delete the node. + let to_delete: Vec<_> = self + .joint_graph + .interactions_with(deleted_id) + .map(|e| (e.0, e.1, e.2.handle)) + .collect(); + for (h1, h2, to_delete_handle) in to_delete { + let to_delete_edge_id = self.joint_ids.remove(to_delete_handle).unwrap(); + self.joint_graph.graph.remove_edge(to_delete_edge_id); + + // Update the id of the edge which took the place of the deleted one. + if let Some(j) = self.joint_graph.graph.edge_weight_mut(to_delete_edge_id) { + self.joint_ids[j.handle] = to_delete_edge_id; + } + + // Wake up the attached bodies. + bodies.wake_up(h1); + bodies.wake_up(h2); + } + + if let Some(other) = self.joint_graph.remove_node(deleted_id) { + // One rigid-body joint graph index may have been invalidated + // so we need to update it. + if let Some(replacement) = bodies.get_mut_internal(other) { + replacement.joint_graph_index = deleted_id; + } + } + } + } +} diff --git a/src/dynamics/joint/mod.rs b/src/dynamics/joint/mod.rs new file mode 100644 index 000000000..b4dd60ecd --- /dev/null +++ b/src/dynamics/joint/mod.rs @@ -0,0 +1,16 @@ +pub use self::ball_joint::BallJoint; +pub use self::fixed_joint::FixedJoint; +pub use self::joint::{Joint, JointParams}; +pub(crate) use self::joint_set::{JointGraphEdge, JointIndex}; +pub use self::joint_set::{JointHandle, JointSet}; +pub use self::prismatic_joint::PrismaticJoint; +#[cfg(feature = "dim3")] +pub use self::revolute_joint::RevoluteJoint; + +mod ball_joint; +mod fixed_joint; +mod joint; +mod joint_set; +mod prismatic_joint; +#[cfg(feature = "dim3")] +mod revolute_joint; diff --git a/src/dynamics/joint/prismatic_joint.rs b/src/dynamics/joint/prismatic_joint.rs new file mode 100644 index 000000000..a6fd55854 --- /dev/null +++ b/src/dynamics/joint/prismatic_joint.rs @@ -0,0 +1,193 @@ +use crate::math::{Isometry, Point, Vector, DIM}; +use crate::utils::WBasis; +use na::Unit; +#[cfg(feature = "dim2")] +use na::Vector2; +#[cfg(feature = "dim3")] +use na::Vector5; + +#[derive(Copy, Clone)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// A joint that removes all relative motion between two bodies, except for the translations along one axis. +pub struct PrismaticJoint { + /// Where the prismatic joint is attached on the first body, expressed in the local space of the first attached body. + pub local_anchor1: Point, + /// Where the prismatic joint is attached on the second body, expressed in the local space of the second attached body. + pub local_anchor2: Point, + pub(crate) local_axis1: Unit>, + pub(crate) local_axis2: Unit>, + pub(crate) basis1: [Vector; DIM - 1], + pub(crate) basis2: [Vector; DIM - 1], + /// The impulse applied by this joint on the first body. + /// + /// The impulse applied to the second body is given by `-impulse`. + #[cfg(feature = "dim3")] + pub impulse: Vector5, + /// The impulse applied by this joint on the first body. + /// + /// The impulse applied to the second body is given by `-impulse`. + #[cfg(feature = "dim2")] + pub impulse: Vector2, + /// Whether or not this joint should enforce translational limits along its axis. + pub limits_enabled: bool, + /// The min an max relative position of the attached bodies along this joint's axis. + pub limits: [f32; 2], + /// The impulse applied by this joint on the first body to enforce the position limit along this joint's axis. + /// + /// The impulse applied to the second body is given by `-impulse`. + pub limits_impulse: f32, + // pub motor_enabled: bool, + // pub target_motor_vel: f32, + // pub max_motor_impulse: f32, + // pub motor_impulse: f32, +} + +impl PrismaticJoint { + /// Creates a new prismatic joint with the given point of applications and axis, all expressed + /// in the local-space of the affected bodies. + #[cfg(feature = "dim2")] + pub fn new( + local_anchor1: Point, + local_axis1: Unit>, + local_anchor2: Point, + local_axis2: Unit>, + ) -> Self { + Self { + local_anchor1, + local_anchor2, + local_axis1, + local_axis2, + basis1: local_axis1.orthonormal_basis(), + basis2: local_axis2.orthonormal_basis(), + impulse: na::zero(), + limits_enabled: false, + limits: [-f32::MAX, f32::MAX], + limits_impulse: 0.0, + // motor_enabled: false, + // target_motor_vel: 0.0, + // max_motor_impulse: f32::MAX, + // motor_impulse: 0.0, + } + } + + /// Creates a new prismatic joint with the given point of applications and axis, all expressed + /// in the local-space of the affected bodies. + /// + /// The local tangent are vector orthogonal to the local axis. It is used to compute a basis orthonormal + /// to the joint's axis. If this tangent is set to zero, te orthonormal basis will be automatically + /// computed arbitrarily. + #[cfg(feature = "dim3")] + pub fn new( + local_anchor1: Point, + local_axis1: Unit>, + local_tangent1: Vector, + local_anchor2: Point, + local_axis2: Unit>, + local_tangent2: Vector, + ) -> Self { + let basis1 = if let Some(local_bitangent1) = + Unit::try_new(local_axis1.cross(&local_tangent1), 1.0e-3) + { + [ + local_bitangent1.into_inner(), + local_bitangent1.cross(&local_axis1), + ] + } else { + local_axis1.orthonormal_basis() + }; + + let basis2 = if let Some(local_bitangent2) = + Unit::try_new(local_axis2.cross(&local_tangent2), 2.0e-3) + { + [ + local_bitangent2.into_inner(), + local_bitangent2.cross(&local_axis2), + ] + } else { + local_axis2.orthonormal_basis() + }; + + Self { + local_anchor1, + local_anchor2, + local_axis1, + local_axis2, + basis1, + basis2, + impulse: na::zero(), + limits_enabled: false, + limits: [-f32::MAX, f32::MAX], + limits_impulse: 0.0, + // motor_enabled: false, + // target_motor_vel: 0.0, + // max_motor_impulse: f32::MAX, + // motor_impulse: 0.0, + } + } + + /// The local axis of this joint, expressed in the local-space of the first attached body. + pub fn local_axis1(&self) -> Unit> { + self.local_axis1 + } + + /// The local axis of this joint, expressed in the local-space of the second attached body. + pub fn local_axis2(&self) -> Unit> { + self.local_axis2 + } + + // FIXME: precompute this? + #[cfg(feature = "dim2")] + pub(crate) fn local_frame1(&self) -> Isometry { + use na::{Matrix2, Rotation2, UnitComplex}; + + let mat = Matrix2::from_columns(&[self.local_axis1.into_inner(), self.basis1[0]]); + let rotmat = Rotation2::from_matrix_unchecked(mat); + let rotation = UnitComplex::from_rotation_matrix(&rotmat); + let translation = self.local_anchor1.coords.into(); + Isometry::from_parts(translation, rotation) + } + + // FIXME: precompute this? + #[cfg(feature = "dim2")] + pub(crate) fn local_frame2(&self) -> Isometry { + use na::{Matrix2, Rotation2, UnitComplex}; + + let mat = Matrix2::from_columns(&[self.local_axis2.into_inner(), self.basis2[0]]); + let rotmat = Rotation2::from_matrix_unchecked(mat); + let rotation = UnitComplex::from_rotation_matrix(&rotmat); + let translation = self.local_anchor2.coords.into(); + Isometry::from_parts(translation, rotation) + } + + // FIXME: precompute this? + #[cfg(feature = "dim3")] + pub(crate) fn local_frame1(&self) -> Isometry { + use na::{Matrix3, Rotation3, UnitQuaternion}; + + let mat = Matrix3::from_columns(&[ + self.local_axis1.into_inner(), + self.basis1[0], + self.basis1[1], + ]); + let rotmat = Rotation3::from_matrix_unchecked(mat); + let rotation = UnitQuaternion::from_rotation_matrix(&rotmat); + let translation = self.local_anchor1.coords.into(); + Isometry::from_parts(translation, rotation) + } + + // FIXME: precompute this? + #[cfg(feature = "dim3")] + pub(crate) fn local_frame2(&self) -> Isometry { + use na::{Matrix3, Rotation3, UnitQuaternion}; + + let mat = Matrix3::from_columns(&[ + self.local_axis2.into_inner(), + self.basis2[0], + self.basis2[1], + ]); + let rotmat = Rotation3::from_matrix_unchecked(mat); + let rotation = UnitQuaternion::from_rotation_matrix(&rotmat); + let translation = self.local_anchor2.coords.into(); + Isometry::from_parts(translation, rotation) + } +} diff --git a/src/dynamics/joint/revolute_joint.rs b/src/dynamics/joint/revolute_joint.rs new file mode 100644 index 000000000..cdb424b14 --- /dev/null +++ b/src/dynamics/joint/revolute_joint.rs @@ -0,0 +1,46 @@ +use crate::math::{Point, Vector}; +use crate::utils::WBasis; +use na::{Unit, Vector5}; + +#[derive(Copy, Clone)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// A joint that removes all relative motion between two bodies, except for the rotations along one axis. +pub struct RevoluteJoint { + /// Where the revolute joint is attached on the first body, expressed in the local space of the first attached body. + pub local_anchor1: Point, + /// Where the revolute joint is attached on the second body, expressed in the local space of the second attached body. + pub local_anchor2: Point, + /// The rotation axis of this revolute joint expressed in the local space of the first attached body. + pub local_axis1: Unit>, + /// The rotation axis of this revolute joint expressed in the local space of the second attached body. + pub local_axis2: Unit>, + /// The basis orthonormal to `local_axis1`, expressed in the local space of the first attached body. + pub basis1: [Vector; 2], + /// The basis orthonormal to `local_axis2`, expressed in the local space of the second attached body. + pub basis2: [Vector; 2], + /// The impulse applied by this joint on the first body. + /// + /// The impulse applied to the second body is given by `-impulse`. + pub impulse: Vector5, +} + +impl RevoluteJoint { + /// Creates a new revolute joint with the given point of applications and axis, all expressed + /// in the local-space of the affected bodies. + pub fn new( + local_anchor1: Point, + local_axis1: Unit>, + local_anchor2: Point, + local_axis2: Unit>, + ) -> Self { + Self { + local_anchor1, + local_anchor2, + local_axis1, + local_axis2, + basis1: local_axis1.orthonormal_basis(), + basis2: local_axis2.orthonormal_basis(), + impulse: na::zero(), + } + } +} diff --git a/src/dynamics/mass_properties.rs b/src/dynamics/mass_properties.rs new file mode 100644 index 000000000..cc2979c9c --- /dev/null +++ b/src/dynamics/mass_properties.rs @@ -0,0 +1,206 @@ +use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Vector}; +use crate::utils; +use num::Zero; +use std::ops::{Add, AddAssign}; +#[cfg(feature = "dim3")] +use {na::Matrix3, std::ops::MulAssign}; + +#[derive(Copy, Clone, Debug, PartialEq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// The local mass properties of a rigid-body. +pub struct MassProperties { + /// The center of mass of a rigid-body expressed in its local-space. + pub local_com: Point, + /// The inverse of the mass of a rigid-body. + /// + /// If this is zero, the rigid-body is assumed to have infinite mass. + pub inv_mass: f32, + /// The inverse of the principal angular inertia of the rigid-body. + /// + /// Components set to zero are assumed to be infinite along the corresponding principal axis. + pub inv_principal_inertia_sqrt: AngVector, + #[cfg(feature = "dim3")] + /// The principal vectors of the local angular inertia tensor of the rigid-body. + pub principal_inertia_local_frame: Rotation, +} + +impl MassProperties { + #[cfg(feature = "dim2")] + pub(crate) fn new(local_com: Point, mass: f32, principal_inertia: f32) -> Self { + let inv_mass = utils::inv(mass); + let inv_principal_inertia_sqrt = utils::inv(principal_inertia.sqrt()); + Self { + local_com, + inv_mass, + inv_principal_inertia_sqrt, + } + } + + #[cfg(feature = "dim3")] + pub(crate) fn new(local_com: Point, mass: f32, principal_inertia: AngVector) -> Self { + Self::with_principal_inertia_frame(local_com, mass, principal_inertia, Rotation::identity()) + } + + #[cfg(feature = "dim3")] + pub(crate) fn with_principal_inertia_frame( + local_com: Point, + mass: f32, + principal_inertia: AngVector, + principal_inertia_local_frame: Rotation, + ) -> Self { + let inv_mass = utils::inv(mass); + let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.sqrt())); + Self { + local_com, + inv_mass, + inv_principal_inertia_sqrt, + principal_inertia_local_frame, + } + } + + /// The world-space center of mass of the rigid-body. + pub fn world_com(&self, pos: &Isometry) -> Point { + pos * self.local_com + } + + #[cfg(feature = "dim2")] + /// The world-space inverse angular inertia tensor of the rigid-body. + pub fn world_inv_inertia_sqrt(&self, _rot: &Rotation) -> AngularInertia { + self.inv_principal_inertia_sqrt + } + + #[cfg(feature = "dim3")] + /// The world-space inverse angular inertia tensor of the rigid-body. + pub fn world_inv_inertia_sqrt(&self, rot: &Rotation) -> AngularInertia { + if !self.inv_principal_inertia_sqrt.is_zero() { + let mut lhs = (rot * self.principal_inertia_local_frame) + .to_rotation_matrix() + .into_inner(); + let rhs = lhs.transpose(); + lhs.column_mut(0) + .mul_assign(self.inv_principal_inertia_sqrt.x); + lhs.column_mut(1) + .mul_assign(self.inv_principal_inertia_sqrt.y); + lhs.column_mut(2) + .mul_assign(self.inv_principal_inertia_sqrt.z); + let inertia = lhs * rhs; + AngularInertia::from_sdp_matrix(inertia) + } else { + AngularInertia::zero() + } + } + + #[cfg(feature = "dim3")] + /// Reconstructs the angular inertia tensor of the rigid body from its principal inertia values and axii. + pub fn reconstruct_inertia_matrix(&self) -> Matrix3 { + let principal_inertia = self.inv_principal_inertia_sqrt.map(|e| utils::inv(e * e)); + self.principal_inertia_local_frame.to_rotation_matrix() + * Matrix3::from_diagonal(&principal_inertia) + * self + .principal_inertia_local_frame + .inverse() + .to_rotation_matrix() + } + + #[cfg(feature = "dim2")] + pub(crate) fn construct_shifted_inertia_matrix(&self, shift: Vector) -> f32 { + if self.inv_mass != 0.0 { + let mass = 1.0 / self.inv_mass; + let i = utils::inv(self.inv_principal_inertia_sqrt * self.inv_principal_inertia_sqrt); + i + shift.norm_squared() * mass + } else { + 0.0 + } + } + + #[cfg(feature = "dim3")] + pub(crate) fn construct_shifted_inertia_matrix(&self, shift: Vector) -> Matrix3 { + if self.inv_mass != 0.0 { + let mass = 1.0 / self.inv_mass; + let matrix = self.reconstruct_inertia_matrix(); + let diag = shift.norm_squared(); + let diagm = Matrix3::from_diagonal_element(diag); + matrix + (diagm + shift * shift.transpose()) * mass + } else { + Matrix3::zeros() + } + } +} + +impl Zero for MassProperties { + fn zero() -> Self { + Self { + inv_mass: 0.0, + inv_principal_inertia_sqrt: na::zero(), + #[cfg(feature = "dim3")] + principal_inertia_local_frame: Rotation::identity(), + local_com: Point::origin(), + } + } + + fn is_zero(&self) -> bool { + *self == Self::zero() + } +} + +impl Add for MassProperties { + type Output = Self; + + #[cfg(feature = "dim2")] + fn add(self, other: MassProperties) -> Self { + if self.is_zero() { + return other; + } else if other.is_zero() { + return self; + } + + let m1 = utils::inv(self.inv_mass); + let m2 = utils::inv(other.inv_mass); + let inv_mass = utils::inv(m1 + m2); + let local_com = (self.local_com * m1 + other.local_com.coords * m2) * inv_mass; + let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com); + let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com); + let inertia = i1 + i2; + let inv_principal_inertia_sqrt = utils::inv(inertia.sqrt()); + + Self { + local_com, + inv_mass, + inv_principal_inertia_sqrt, + } + } + + #[cfg(feature = "dim3")] + fn add(self, other: MassProperties) -> Self { + if self.is_zero() { + return other; + } else if other.is_zero() { + return self; + } + + let m1 = utils::inv(self.inv_mass); + let m2 = utils::inv(other.inv_mass); + let inv_mass = utils::inv(m1 + m2); + let local_com = (self.local_com * m1 + other.local_com.coords * m2) * inv_mass; + let i1 = self.construct_shifted_inertia_matrix(local_com - self.local_com); + let i2 = other.construct_shifted_inertia_matrix(local_com - other.local_com); + let inertia = i1 + i2; + let eigen = inertia.symmetric_eigen(); + let principal_inertia_local_frame = Rotation::from_matrix(&eigen.eigenvectors); + let principal_inertia = eigen.eigenvalues; + let inv_principal_inertia_sqrt = principal_inertia.map(|e| utils::inv(e.sqrt())); + + Self { + local_com, + inv_mass, + inv_principal_inertia_sqrt, + principal_inertia_local_frame, + } + } +} + +impl AddAssign for MassProperties { + fn add_assign(&mut self, rhs: MassProperties) { + *self = *self + rhs + } +} diff --git a/src/dynamics/mass_properties_ball.rs b/src/dynamics/mass_properties_ball.rs new file mode 100644 index 000000000..ac5790af2 --- /dev/null +++ b/src/dynamics/mass_properties_ball.rs @@ -0,0 +1,30 @@ +use crate::dynamics::MassProperties; +#[cfg(feature = "dim3")] +use crate::math::Vector; +use crate::math::{Point, PrincipalAngularInertia}; + +impl MassProperties { + pub(crate) fn ball_volume_unit_angular_inertia( + radius: f32, + ) -> (f32, PrincipalAngularInertia) { + #[cfg(feature = "dim2")] + { + let volume = std::f32::consts::PI * radius * radius; + let i = radius * radius / 2.0; + (volume, i) + } + #[cfg(feature = "dim3")] + { + let volume = std::f32::consts::PI * radius * radius * radius * 4.0 / 3.0; + let i = radius * radius * 2.0 / 5.0; + + (volume, Vector::repeat(i)) + } + } + + pub(crate) fn from_ball(density: f32, radius: f32) -> Self { + let (vol, unit_i) = Self::ball_volume_unit_angular_inertia(radius); + let mass = vol * density; + Self::new(Point::origin(), mass, unit_i * mass) + } +} diff --git a/src/dynamics/mass_properties_capsule.rs b/src/dynamics/mass_properties_capsule.rs new file mode 100644 index 000000000..5f089581b --- /dev/null +++ b/src/dynamics/mass_properties_capsule.rs @@ -0,0 +1,60 @@ +use crate::dynamics::MassProperties; +#[cfg(feature = "dim3")] +use crate::geometry::Capsule; +use crate::math::{Point, PrincipalAngularInertia, Vector}; + +impl MassProperties { + fn cylinder_y_volume_unit_inertia( + half_height: f32, + radius: f32, + ) -> (f32, PrincipalAngularInertia) { + #[cfg(feature = "dim2")] + { + Self::cuboid_volume_unit_inertia(Vector::new(radius, half_height)) + } + + #[cfg(feature = "dim3")] + { + let volume = half_height * radius * radius * std::f32::consts::PI * 2.0; + let sq_radius = radius * radius; + let sq_height = half_height * half_height * 4.0; + let off_principal = (sq_radius * 3.0 + sq_height) / 12.0; + + let inertia = Vector::new(off_principal, sq_radius / 2.0, off_principal); + (volume, inertia) + } + } + + pub(crate) fn from_capsule(density: f32, a: Point, b: Point, radius: f32) -> Self { + let half_height = (b - a).norm() / 2.0; + let (cyl_vol, cyl_unit_i) = Self::cylinder_y_volume_unit_inertia(half_height, radius); + let (ball_vol, ball_unit_i) = Self::ball_volume_unit_angular_inertia(radius); + let cap_vol = cyl_vol + ball_vol; + let cap_mass = cap_vol * density; + let mut cap_unit_i = cyl_unit_i + ball_unit_i; + let local_com = na::center(&a, &b); + + #[cfg(feature = "dim2")] + { + let h = half_height * 2.0; + let extra = h * h * 0.5 + h * radius * 3.0 / 8.0; + cap_unit_i += extra; + Self::new(local_com, cap_mass, cap_unit_i * cap_mass) + } + + #[cfg(feature = "dim3")] + { + let h = half_height * 2.0; + let extra = h * h * 0.5 + h * radius * 3.0 / 8.0; + cap_unit_i.x += extra; + cap_unit_i.z += extra; + let local_frame = Capsule::new(a, b, radius).rotation_wrt_y(); + Self::with_principal_inertia_frame( + local_com, + cap_mass, + cap_unit_i * cap_mass, + local_frame, + ) + } + } +} diff --git a/src/dynamics/mass_properties_cuboid.rs b/src/dynamics/mass_properties_cuboid.rs new file mode 100644 index 000000000..2d870cf1d --- /dev/null +++ b/src/dynamics/mass_properties_cuboid.rs @@ -0,0 +1,33 @@ +use crate::dynamics::MassProperties; +use crate::math::{Point, PrincipalAngularInertia, Vector}; + +impl MassProperties { + pub(crate) fn cuboid_volume_unit_inertia( + half_extents: Vector, + ) -> (f32, PrincipalAngularInertia) { + #[cfg(feature = "dim2")] + { + let volume = half_extents.x * half_extents.y * 4.0; + let ix = (half_extents.x * half_extents.x) / 3.0; + let iy = (half_extents.y * half_extents.y) / 3.0; + + (volume, ix + iy) + } + + #[cfg(feature = "dim3")] + { + let volume = half_extents.x * half_extents.y * half_extents.z * 8.0; + let ix = (half_extents.x * half_extents.x) / 3.0; + let iy = (half_extents.y * half_extents.y) / 3.0; + let iz = (half_extents.z * half_extents.z) / 3.0; + + (volume, Vector::new(iy + iz, ix + iz, ix + iy)) + } + } + + pub(crate) fn from_cuboid(density: f32, half_extents: Vector) -> Self { + let (vol, unit_i) = Self::cuboid_volume_unit_inertia(half_extents); + let mass = vol * density; + Self::new(Point::origin(), mass, unit_i * mass) + } +} diff --git a/src/dynamics/mass_properties_polygon.rs b/src/dynamics/mass_properties_polygon.rs new file mode 100644 index 000000000..c87e888e2 --- /dev/null +++ b/src/dynamics/mass_properties_polygon.rs @@ -0,0 +1,144 @@ +use crate::dynamics::MassProperties; +use crate::math::Point; + +impl MassProperties { + pub(crate) fn from_polygon(density: f32, vertices: &[Point]) -> MassProperties { + let (area, com) = convex_polygon_area_and_center_of_mass(vertices); + + if area == 0.0 { + return MassProperties::new(com, 0.0, 0.0); + } + + let mut itot = 0.0; + let factor = 1.0 / 6.0; + + let mut iterpeek = vertices.iter().peekable(); + let firstelement = *iterpeek.peek().unwrap(); // store first element to close the cycle in the end with unwrap_or + while let Some(elem) = iterpeek.next() { + let area = triangle_area(&com, elem, iterpeek.peek().unwrap_or(&firstelement)); + + // algorithm adapted from Box2D + let e1 = *elem - com; + let e2 = **(iterpeek.peek().unwrap_or(&firstelement)) - com; + + let ex1 = e1[0]; + let ey1 = e1[1]; + let ex2 = e2[0]; + let ey2 = e2[1]; + + let intx2 = ex1 * ex1 + ex2 * ex1 + ex2 * ex2; + let inty2 = ey1 * ey1 + ey2 * ey1 + ey2 * ey2; + + let ipart = factor * (intx2 + inty2); + + itot += ipart * area; + } + + Self::new(com, area * density, itot * density) + } +} + +fn convex_polygon_area_and_center_of_mass(convex_polygon: &[Point]) -> (f32, Point) { + let geometric_center = convex_polygon + .iter() + .fold(Point::origin(), |e1, e2| e1 + e2.coords) + / convex_polygon.len() as f32; + let mut res = Point::origin(); + let mut areasum = 0.0; + + let mut iterpeek = convex_polygon.iter().peekable(); + let firstelement = *iterpeek.peek().unwrap(); // Stores first element to close the cycle in the end with unwrap_or. + while let Some(elem) = iterpeek.next() { + let (a, b, c) = ( + elem, + iterpeek.peek().unwrap_or(&firstelement), + &geometric_center, + ); + let area = triangle_area(a, b, c); + let center = (a.coords + b.coords + c.coords) / 3.0; + + res += center * area; + areasum += area; + } + + if areasum == 0.0 { + (areasum, geometric_center) + } else { + (areasum, res / areasum) + } +} + +pub fn triangle_area(pa: &Point, pb: &Point, pc: &Point) -> f32 { + // Kahan's formula. + let a = na::distance(pa, pb); + let b = na::distance(pb, pc); + let c = na::distance(pc, pa); + + let (c, b, a) = sort3(&a, &b, &c); + let a = *a; + let b = *b; + let c = *c; + + let sqr = (a + (b + c)) * (c - (a - b)) * (c + (a - b)) * (a + (b - c)); + + sqr.sqrt() * 0.25 +} + +/// Sorts a set of three values in increasing order. +#[inline] +pub fn sort3<'a>(a: &'a f32, b: &'a f32, c: &'a f32) -> (&'a f32, &'a f32, &'a f32) { + let a_b = *a > *b; + let a_c = *a > *c; + let b_c = *b > *c; + + let sa; + let sb; + let sc; + + // Sort the three values. + // FIXME: move this to the utilities? + if a_b { + // a > b + if a_c { + // a > c + sc = a; + + if b_c { + // b > c + sa = c; + sb = b; + } else { + // b <= c + sa = b; + sb = c; + } + } else { + // a <= c + sa = b; + sb = a; + sc = c; + } + } else { + // a < b + if !a_c { + // a <= c + sa = a; + + if b_c { + // b > c + sb = c; + sc = b; + } else { + sb = b; + sc = c; + } + } else { + // a > c + sa = c; + sb = a; + sc = b; + } + } + + (sa, sb, sc) +} diff --git a/src/dynamics/mod.rs b/src/dynamics/mod.rs new file mode 100644 index 000000000..4499d95cc --- /dev/null +++ b/src/dynamics/mod.rs @@ -0,0 +1,30 @@ +//! Structures related to dynamics: bodies, joints, etc. + +pub use self::integration_parameters::IntegrationParameters; +pub(crate) use self::joint::JointIndex; +#[cfg(feature = "dim3")] +pub use self::joint::RevoluteJoint; +pub use self::joint::{ + BallJoint, FixedJoint, Joint, JointHandle, JointParams, JointSet, PrismaticJoint, +}; +pub use self::mass_properties::MassProperties; +pub use self::rigid_body::{ActivationStatus, BodyStatus, RigidBody, RigidBodyBuilder}; +pub use self::rigid_body_set::{BodyPair, RigidBodyHandle, RigidBodyMut, RigidBodySet}; +// #[cfg(not(feature = "parallel"))] +pub(crate) use self::joint::JointGraphEdge; +#[cfg(not(feature = "parallel"))] +pub(crate) use self::solver::IslandSolver; +#[cfg(feature = "parallel")] +pub(crate) use self::solver::ParallelIslandSolver; + +mod integration_parameters; +mod joint; +mod mass_properties; +mod mass_properties_ball; +mod mass_properties_capsule; +mod mass_properties_cuboid; +#[cfg(feature = "dim2")] +mod mass_properties_polygon; +mod rigid_body; +mod rigid_body_set; +mod solver; diff --git a/src/dynamics/rigid_body.rs b/src/dynamics/rigid_body.rs new file mode 100644 index 000000000..584dc9673 --- /dev/null +++ b/src/dynamics/rigid_body.rs @@ -0,0 +1,441 @@ +use crate::dynamics::MassProperties; +use crate::geometry::{ColliderHandle, InteractionGraph, RigidBodyGraphIndex}; +use crate::math::{AngVector, AngularInertia, Isometry, Point, Rotation, Translation, Vector}; +use crate::utils::{WCross, WDot}; +use num::Zero; + +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// The status of a body, governing the way it is affected by external forces. +pub enum BodyStatus { + /// A `BodyStatus::Dynamic` body can be affected by all external forces. + Dynamic, + /// A `BodyStatus::Static` body cannot be affected by external forces. + Static, + /// A `BodyStatus::Kinematic` body cannot be affected by any external forces but can be controlled + /// by the user at the position level while keeping realistic one-way interaction with dynamic bodies. + /// + /// One-way interaction means that a kinematic body can push a dynamic body, but a kinematic body + /// cannot be pushed by anything. In other words, the trajectory of a kinematic body can only be + /// modified by the user and is independent from any contact or joint it is involved in. + Kinematic, + // Semikinematic, // A kinematic that performs automatic CCD with the static environment toi avoid traversing it? + // Disabled, +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// A rigid body. +/// +/// To create a new rigid-body, use the `RigidBodyBuilder` structure. +#[derive(Debug)] +pub struct RigidBody { + /// The world-space position of the rigid-body. + pub position: Isometry, + pub(crate) predicted_position: Isometry, + /// The local mass properties of the rigid-body. + pub mass_properties: MassProperties, + /// The world-space center of mass of the rigid-body. + pub world_com: Point, + /// The square-root of the inverse angular inertia tensor of the rigid-body. + pub world_inv_inertia_sqrt: AngularInertia, + /// The linear velocity of the rigid-body. + pub linvel: Vector, + /// The angular velocity of the rigid-body. + pub angvel: AngVector, + pub(crate) linacc: Vector, + pub(crate) angacc: AngVector, + pub(crate) colliders: Vec, + /// Whether or not this rigid-body is sleeping. + pub activation: ActivationStatus, + pub(crate) joint_graph_index: RigidBodyGraphIndex, + pub(crate) active_island_id: usize, + pub(crate) active_set_id: usize, + pub(crate) active_set_offset: usize, + pub(crate) active_set_timestamp: u32, + /// The status of the body, governing how it is affected by external forces. + pub body_status: BodyStatus, +} + +impl Clone for RigidBody { + fn clone(&self) -> Self { + Self { + colliders: Vec::new(), + joint_graph_index: RigidBodyGraphIndex::new(crate::INVALID_U32), + active_island_id: crate::INVALID_USIZE, + active_set_id: crate::INVALID_USIZE, + active_set_offset: crate::INVALID_USIZE, + active_set_timestamp: crate::INVALID_U32, + ..*self + } + } +} + +impl RigidBody { + fn new() -> Self { + Self { + position: Isometry::identity(), + predicted_position: Isometry::identity(), + mass_properties: MassProperties::zero(), + world_com: Point::origin(), + world_inv_inertia_sqrt: AngularInertia::zero(), + linvel: Vector::zeros(), + angvel: na::zero(), + linacc: Vector::zeros(), + angacc: na::zero(), + colliders: Vec::new(), + activation: ActivationStatus::new_active(), + joint_graph_index: InteractionGraph::<()>::invalid_graph_index(), + active_island_id: 0, + active_set_id: 0, + active_set_offset: 0, + active_set_timestamp: 0, + body_status: BodyStatus::Dynamic, + } + } + + pub(crate) fn integrate_accelerations(&mut self, dt: f32, gravity: Vector) { + if self.mass_properties.inv_mass != 0.0 { + self.linvel += (gravity + self.linacc) * dt; + self.angvel += self.angacc * dt; + + // Reset the accelerations. + self.linacc = na::zero(); + self.angacc = na::zero(); + } + } + + /// The handles of colliders attached to this rigid body. + pub fn colliders(&self) -> &[ColliderHandle] { + &self.colliders[..] + } + + /// Is this rigid body dynamic? + /// + /// A dynamic body can move freely and is affected by forces. + pub fn is_dynamic(&self) -> bool { + self.body_status == BodyStatus::Dynamic + } + + /// Is this rigid body kinematic? + /// + /// A kinematic body can move freely but is not affected by forces. + pub fn is_kinematic(&self) -> bool { + self.body_status == BodyStatus::Kinematic + } + + /// Is this rigid body static? + /// + /// A static body cannot move and is not affected by forces. + pub fn is_static(&self) -> bool { + self.body_status == BodyStatus::Static + } + + /// The mass of this rigid body. + /// + /// Returns zero if this rigid body has an infinite mass. + pub fn mass(&self) -> f32 { + crate::utils::inv(self.mass_properties.inv_mass) + } + + /// Put this rigid body to sleep. + /// + /// A sleeping body no longer moves and is no longer simulated by the physics engine unless + /// it is waken up. It can be woken manually with `self.wake_up` or automatically due to + /// external forces like contacts. + pub fn sleep(&mut self) { + self.activation.energy = 0.0; + self.activation.sleeping = true; + self.linvel = na::zero(); + self.angvel = na::zero(); + } + + /// Wakes up this rigid body if it is sleeping. + pub fn wake_up(&mut self) { + self.activation.sleeping = false; + + if self.activation.energy == 0.0 && self.is_dynamic() { + self.activation.energy = self.activation.threshold.abs() * 2.0; + } + } + + pub(crate) fn update_energy(&mut self) { + let mix_factor = 0.01; + let new_energy = (1.0 - mix_factor) * self.activation.energy + + mix_factor * (self.linvel.norm_squared() + self.angvel.gdot(self.angvel)); + self.activation.energy = new_energy.min(self.activation.threshold.abs() * 4.0); + } + + /// Is this rigid body sleeping? + pub fn is_sleeping(&self) -> bool { + self.activation.sleeping + } + + fn integrate_velocity(&self, dt: f32) -> Isometry { + let com = &self.position * self.mass_properties.local_com; // FIXME: use non-origin center of masses. + let shift = Translation::from(com.coords); + shift * Isometry::new(self.linvel * dt, self.angvel * dt) * shift.inverse() + } + pub(crate) fn integrate(&mut self, dt: f32) { + self.position = self.integrate_velocity(dt) * self.position; + } + + /// Sets the position of this rigid body. + pub fn set_position(&mut self, pos: Isometry) { + self.position = pos; + + // TODO: update the predicted position for dynamic bodies too? + if self.is_static() { + self.predicted_position = pos; + } + } + + /// If this rigid body is kinematic, sets its future position after the next timestep integration. + pub fn set_next_kinematic_position(&mut self, pos: Isometry) { + if self.is_kinematic() { + self.predicted_position = pos; + } + } + + pub(crate) fn compute_velocity_from_predicted_position(&mut self, inv_dt: f32) { + let dpos = self.predicted_position * self.position.inverse(); + #[cfg(feature = "dim2")] + { + self.angvel = dpos.rotation.angle() * inv_dt; + } + #[cfg(feature = "dim3")] + { + self.angvel = dpos.rotation.scaled_axis() * inv_dt; + } + self.linvel = dpos.translation.vector * inv_dt; + } + + pub(crate) fn update_predicted_position(&mut self, dt: f32) { + self.predicted_position = self.integrate_velocity(dt) * self.position; + } + + pub(crate) fn update_world_mass_properties(&mut self) { + self.world_com = self.mass_properties.world_com(&self.position); + self.world_inv_inertia_sqrt = self + .mass_properties + .world_inv_inertia_sqrt(&self.position.rotation); + } + + /* + * Application of forces/impulses. + */ + /// Applies a force at the center-of-mass of this rigid-body. + pub fn apply_force(&mut self, force: Vector) { + if self.body_status == BodyStatus::Dynamic { + self.linacc += force * self.mass_properties.inv_mass; + } + } + + /// Applies an impulse at the center-of-mass of this rigid-body. + pub fn apply_impulse(&mut self, impulse: Vector) { + if self.body_status == BodyStatus::Dynamic { + self.linvel += impulse * self.mass_properties.inv_mass; + } + } + + /// Applies a torque at the center-of-mass of this rigid-body. + #[cfg(feature = "dim2")] + pub fn apply_torque(&mut self, torque: f32) { + if self.body_status == BodyStatus::Dynamic { + self.angacc += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque); + } + } + + /// Applies a torque at the center-of-mass of this rigid-body. + #[cfg(feature = "dim3")] + pub fn apply_torque(&mut self, torque: Vector) { + if self.body_status == BodyStatus::Dynamic { + self.angacc += self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque); + } + } + + /// Applies an impulsive torque at the center-of-mass of this rigid-body. + #[cfg(feature = "dim2")] + pub fn apply_torque_impulse(&mut self, torque_impulse: f32) { + if self.body_status == BodyStatus::Dynamic { + self.angvel += + self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque_impulse); + } + } + + /// Applies an impulsive torque at the center-of-mass of this rigid-body. + #[cfg(feature = "dim3")] + pub fn apply_torque_impulse(&mut self, torque_impulse: Vector) { + if self.body_status == BodyStatus::Dynamic { + self.angvel += + self.world_inv_inertia_sqrt * (self.world_inv_inertia_sqrt * torque_impulse); + } + } + + /// Applies a force at the given world-space point of this rigid-body. + pub fn apply_force_at_point(&mut self, force: Vector, point: Point) { + let torque = (point - self.world_com).gcross(force); + self.apply_force(force); + self.apply_torque(torque); + } + + /// Applies an impulse at the given world-space point of this rigid-body. + pub fn apply_impulse_at_point(&mut self, impulse: Vector, point: Point) { + let torque_impulse = (point - self.world_com).gcross(impulse); + self.apply_impulse(impulse); + self.apply_torque_impulse(torque_impulse); + } +} + +/// A builder for rigid-bodies. +pub struct RigidBodyBuilder { + position: Isometry, + linvel: Vector, + angvel: AngVector, + body_status: BodyStatus, + can_sleep: bool, +} + +impl RigidBodyBuilder { + /// Initialize a new builder for a rigid body which is either static, dynamic, or kinematic. + pub fn new(body_status: BodyStatus) -> Self { + Self { + position: Isometry::identity(), + linvel: Vector::zeros(), + angvel: na::zero(), + body_status, + can_sleep: true, + } + } + + /// Initializes the builder of a new static rigid body. + pub fn new_static() -> Self { + Self::new(BodyStatus::Static) + } + + /// Initializes the builder of a new kinematic rigid body. + pub fn new_kinematic() -> Self { + Self::new(BodyStatus::Kinematic) + } + + /// Initializes the builder of a new dynamic rigid body. + pub fn new_dynamic() -> Self { + Self::new(BodyStatus::Dynamic) + } + + /// Sets the initial translation of the rigid-body to be created. + #[cfg(feature = "dim2")] + pub fn translation(mut self, x: f32, y: f32) -> Self { + self.position.translation.x = x; + self.position.translation.y = y; + self + } + + /// Sets the initial translation of the rigid-body to be created. + #[cfg(feature = "dim3")] + pub fn translation(mut self, x: f32, y: f32, z: f32) -> Self { + self.position.translation.x = x; + self.position.translation.y = y; + self.position.translation.z = z; + self + } + + /// Sets the initial orientation of the rigid-body to be created. + pub fn rotation(mut self, angle: AngVector) -> Self { + self.position.rotation = Rotation::new(angle); + self + } + + /// Sets the initial position (translation and orientation) of the rigid-body to be created. + pub fn position(mut self, pos: Isometry) -> Self { + self.position = pos; + self + } + + /// Sets the initial linear velocity of the rigid-body to be created. + #[cfg(feature = "dim2")] + pub fn linvel(mut self, x: f32, y: f32) -> Self { + self.linvel = Vector::new(x, y); + self + } + + /// Sets the initial linear velocity of the rigid-body to be created. + #[cfg(feature = "dim3")] + pub fn linvel(mut self, x: f32, y: f32, z: f32) -> Self { + self.linvel = Vector::new(x, y, z); + self + } + + /// Sets the initial angular velocity of the rigid-body to be created. + pub fn angvel(mut self, angvel: AngVector) -> Self { + self.angvel = angvel; + self + } + + /// Sets whether or not the rigid-body to be created can sleep if it reaches a dynamic equilibrium. + pub fn can_sleep(mut self, can_sleep: bool) -> Self { + self.can_sleep = can_sleep; + self + } + + /// Build a new rigid-body with the parameters configured with this builder. + pub fn build(&self) -> RigidBody { + let mut rb = RigidBody::new(); + rb.predicted_position = self.position; // FIXME: compute the correct value? + rb.set_position(self.position); + rb.linvel = self.linvel; + rb.angvel = self.angvel; + rb.body_status = self.body_status; + + if !self.can_sleep { + rb.activation.threshold = -1.0; + } + + rb + } +} + +/// The activation status of a body. +/// +/// This controls whether a body is sleeping or not. +/// If the threshold is negative, the body never sleeps. +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct ActivationStatus { + /// The threshold pseudo-kinetic energy bellow which the body can fall asleep. + pub threshold: f32, + /// The current pseudo-kinetic energy of the body. + pub energy: f32, + /// Is this body already sleeping? + pub sleeping: bool, +} + +impl ActivationStatus { + /// The default amount of energy bellow which a body can be put to sleep by nphysics. + pub fn default_threshold() -> f32 { + 0.01 + } + + /// Create a new activation status initialised with the default activation threshold and is active. + pub fn new_active() -> Self { + ActivationStatus { + threshold: Self::default_threshold(), + energy: Self::default_threshold() * 4.0, + sleeping: false, + } + } + + /// Create a new activation status initialised with the default activation threshold and is inactive. + pub fn new_inactive() -> Self { + ActivationStatus { + threshold: Self::default_threshold(), + energy: 0.0, + sleeping: true, + } + } + + /// Returns `true` if the body is not asleep. + #[inline] + pub fn is_active(&self) -> bool { + self.energy != 0.0 + } +} diff --git a/src/dynamics/rigid_body_set.rs b/src/dynamics/rigid_body_set.rs new file mode 100644 index 000000000..99d6f1075 --- /dev/null +++ b/src/dynamics/rigid_body_set.rs @@ -0,0 +1,518 @@ +#[cfg(feature = "parallel")] +use rayon::prelude::*; + +use crate::data::arena::Arena; +use crate::dynamics::{Joint, RigidBody}; +use crate::geometry::{ColliderSet, ContactPair, InteractionGraph}; +use crossbeam::channel::{Receiver, Sender}; +use std::ops::{Deref, DerefMut, Index, IndexMut}; + +/// A mutable reference to a rigid-body. +pub struct RigidBodyMut<'a> { + rb: &'a mut RigidBody, + was_sleeping: bool, + handle: RigidBodyHandle, + sender: &'a Sender, +} + +impl<'a> RigidBodyMut<'a> { + fn new( + handle: RigidBodyHandle, + rb: &'a mut RigidBody, + sender: &'a Sender, + ) -> Self { + Self { + was_sleeping: rb.is_sleeping(), + handle, + sender, + rb, + } + } +} + +impl<'a> Deref for RigidBodyMut<'a> { + type Target = RigidBody; + fn deref(&self) -> &RigidBody { + &*self.rb + } +} + +impl<'a> DerefMut for RigidBodyMut<'a> { + fn deref_mut(&mut self) -> &mut RigidBody { + self.rb + } +} + +impl<'a> Drop for RigidBodyMut<'a> { + fn drop(&mut self) { + if self.was_sleeping && !self.rb.is_sleeping() { + self.sender.send(self.handle).unwrap(); + } + } +} + +/// The unique handle of a rigid body added to a `RigidBodySet`. +pub type RigidBodyHandle = crate::data::arena::Index; + +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// A pair of rigid body handles. +pub struct BodyPair { + /// The first rigid body handle. + pub body1: RigidBodyHandle, + /// The second rigid body handle. + pub body2: RigidBodyHandle, +} + +impl BodyPair { + pub(crate) fn new(body1: RigidBodyHandle, body2: RigidBodyHandle) -> Self { + BodyPair { body1, body2 } + } + + pub(crate) fn swap(self) -> Self { + Self::new(self.body2, self.body1) + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// A set of rigid bodies that can be handled by a physics pipeline. +pub struct RigidBodySet { + // NOTE: the pub(crate) are needed by the broad phase + // to avoid borrowing issues. It is also needed for + // parallelism because the `Receiver` breaks the Sync impl. + // Could we avoid this? + pub(crate) bodies: Arena, + pub(crate) active_dynamic_set: Vec, + pub(crate) active_kinematic_set: Vec, + // Set of inactive bodies which have been modified. + // This typically include static bodies which have been modified. + pub(crate) modified_inactive_set: Vec, + pub(crate) active_islands: Vec, + active_set_timestamp: u32, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + can_sleep: Vec, // Workspace. + #[cfg_attr(feature = "serde-serialize", serde(skip))] + stack: Vec, // Workspace. + #[cfg_attr( + feature = "serde-serialize", + serde(skip, default = "crossbeam::channel::unbounded") + )] + activation_channel: (Sender, Receiver), +} + +impl RigidBodySet { + /// Create a new empty set of rigid bodies. + pub fn new() -> Self { + RigidBodySet { + bodies: Arena::new(), + active_dynamic_set: Vec::new(), + active_kinematic_set: Vec::new(), + modified_inactive_set: Vec::new(), + active_islands: Vec::new(), + active_set_timestamp: 0, + can_sleep: Vec::new(), + stack: Vec::new(), + activation_channel: crossbeam::channel::unbounded(), + } + } + + /// An always-invalid rigid-body handle. + pub fn invalid_handle() -> RigidBodyHandle { + RigidBodyHandle::from_raw_parts(crate::INVALID_USIZE, crate::INVALID_U64) + } + + /// The number of rigid bodies on this set. + pub fn len(&self) -> usize { + self.bodies.len() + } + + pub(crate) fn activate(&mut self, handle: RigidBodyHandle) { + let mut rb = &mut self.bodies[handle]; + if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) { + rb.active_set_id = self.active_dynamic_set.len(); + self.active_dynamic_set.push(handle); + } + } + + /// Is the given body handle valid? + pub fn contains(&self, handle: RigidBodyHandle) -> bool { + self.bodies.contains(handle) + } + + /// Insert a rigid body into this set and retrieve its handle. + pub fn insert(&mut self, rb: RigidBody) -> RigidBodyHandle { + let handle = self.bodies.insert(rb); + let rb = &mut self.bodies[handle]; + rb.active_set_id = self.active_dynamic_set.len(); + + if !rb.is_sleeping() && rb.is_dynamic() { + self.active_dynamic_set.push(handle); + } + + if rb.is_kinematic() { + self.active_kinematic_set.push(handle); + } + + if !rb.is_dynamic() { + self.modified_inactive_set.push(handle); + } + + handle + } + + pub(crate) fn num_islands(&self) -> usize { + self.active_islands.len() - 1 + } + + pub(crate) fn remove_internal(&mut self, handle: RigidBodyHandle) -> Option { + let rb = self.bodies.remove(handle)?; + // Remove this body from the active dynamic set. + if self.active_dynamic_set.get(rb.active_set_id) == Some(&handle) { + self.active_dynamic_set.swap_remove(rb.active_set_id); + + if let Some(replacement) = self.active_dynamic_set.get(rb.active_set_id) { + self.bodies[*replacement].active_set_id = rb.active_set_id; + } + } + + Some(rb) + } + + /// Forces the specified rigid-body to wake up if it is dynamic. + pub fn wake_up(&mut self, handle: RigidBodyHandle) { + if let Some(rb) = self.bodies.get_mut(handle) { + if rb.is_dynamic() { + rb.wake_up(); + + if self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) { + rb.active_set_id = self.active_dynamic_set.len(); + self.active_dynamic_set.push(handle); + } + } + } + } + + /// Gets the rigid-body with the given handle without a known generation. + /// + /// This is useful when you know you want the rigid-body at position `i` but + /// don't know what is its current generation number. Generation numbers are + /// used to protect from the ABA problem because the rigid-body position `i` + /// are recycled between two insertion and a removal. + /// + /// Using this is discouraged in favor of `self.get(handle)` which does not + /// suffer form the ABA problem. + pub fn get_unknown_gen(&self, i: usize) -> Option<(&RigidBody, RigidBodyHandle)> { + self.bodies.get_unknown_gen(i) + } + + /// Gets a mutable reference to the rigid-body with the given handle without a known generation. + /// + /// This is useful when you know you want the rigid-body at position `i` but + /// don't know what is its current generation number. Generation numbers are + /// used to protect from the ABA problem because the rigid-body position `i` + /// are recycled between two insertion and a removal. + /// + /// Using this is discouraged in favor of `self.get_mut(handle)` which does not + /// suffer form the ABA problem. + pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut RigidBody, RigidBodyHandle)> { + self.bodies.get_unknown_gen_mut(i) + } + + /// Gets the rigid-body with the given handle. + pub fn get(&self, handle: RigidBodyHandle) -> Option<&RigidBody> { + self.bodies.get(handle) + } + + /// Gets a mutable reference to the rigid-body with the given handle. + pub fn get_mut(&mut self, handle: RigidBodyHandle) -> Option { + let sender = &self.activation_channel.0; + self.bodies + .get_mut(handle) + .map(|rb| RigidBodyMut::new(handle, rb, sender)) + } + + pub(crate) fn get_mut_internal(&mut self, handle: RigidBodyHandle) -> Option<&mut RigidBody> { + self.bodies.get_mut(handle) + } + + pub(crate) fn get2_mut_internal( + &mut self, + h1: RigidBodyHandle, + h2: RigidBodyHandle, + ) -> (Option<&mut RigidBody>, Option<&mut RigidBody>) { + self.bodies.get2_mut(h1, h2) + } + + /// Iterates through all the rigid-bodies on this set. + pub fn iter(&self) -> impl Iterator { + self.bodies.iter() + } + + /// Iterates mutably through all the rigid-bodies on this set. + pub fn iter_mut(&mut self) -> impl Iterator { + let sender = &self.activation_channel.0; + self.bodies + .iter_mut() + .map(move |(h, rb)| (h, RigidBodyMut::new(h, rb, sender))) + } + + /// Iter through all the active dynamic rigid-bodies on this set. + pub fn iter_active_dynamic<'a>( + &'a self, + ) -> impl Iterator { + let bodies: &'a _ = &self.bodies; + self.active_dynamic_set + .iter() + .filter_map(move |h| Some((*h, bodies.get(*h)?))) + } + + #[cfg(not(feature = "parallel"))] + pub(crate) fn iter_active_island<'a>( + &'a self, + island_id: usize, + ) -> impl Iterator { + let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1]; + let bodies: &'a _ = &self.bodies; + self.active_dynamic_set[island_range] + .iter() + .filter_map(move |h| Some((*h, bodies.get(*h)?))) + } + + #[inline(always)] + pub(crate) fn foreach_active_body_mut_internal( + &mut self, + mut f: impl FnMut(RigidBodyHandle, &mut RigidBody), + ) { + for handle in &self.active_dynamic_set { + if let Some(rb) = self.bodies.get_mut(*handle) { + f(*handle, rb) + } + } + + for handle in &self.active_kinematic_set { + if let Some(rb) = self.bodies.get_mut(*handle) { + f(*handle, rb) + } + } + } + + #[inline(always)] + pub(crate) fn foreach_active_dynamic_body_mut_internal( + &mut self, + mut f: impl FnMut(RigidBodyHandle, &mut RigidBody), + ) { + for handle in &self.active_dynamic_set { + if let Some(rb) = self.bodies.get_mut(*handle) { + f(*handle, rb) + } + } + } + + #[inline(always)] + pub(crate) fn foreach_active_kinematic_body_mut_internal( + &mut self, + mut f: impl FnMut(RigidBodyHandle, &mut RigidBody), + ) { + for handle in &self.active_kinematic_set { + if let Some(rb) = self.bodies.get_mut(*handle) { + f(*handle, rb) + } + } + } + + #[inline(always)] + #[cfg(not(feature = "parallel"))] + pub(crate) fn foreach_active_island_body_mut_internal( + &mut self, + island_id: usize, + mut f: impl FnMut(RigidBodyHandle, &mut RigidBody), + ) { + let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1]; + for handle in &self.active_dynamic_set[island_range] { + if let Some(rb) = self.bodies.get_mut(*handle) { + f(*handle, rb) + } + } + } + + #[cfg(feature = "parallel")] + #[inline(always)] + #[allow(dead_code)] + pub(crate) fn foreach_active_island_body_mut_internal_parallel( + &mut self, + island_id: usize, + f: impl Fn(RigidBodyHandle, &mut RigidBody) + Send + Sync, + ) { + use std::sync::atomic::Ordering; + + let island_range = self.active_islands[island_id]..self.active_islands[island_id + 1]; + let bodies = std::sync::atomic::AtomicPtr::new(&mut self.bodies as *mut _); + self.active_dynamic_set[island_range] + .par_iter() + .for_each_init( + || bodies.load(Ordering::Relaxed), + |bodies, handle| { + let bodies: &mut Arena = unsafe { std::mem::transmute(*bodies) }; + if let Some(rb) = bodies.get_mut(*handle) { + f(*handle, rb) + } + }, + ); + } + + // pub(crate) fn active_dynamic_set(&self) -> &[RigidBodyHandle] { + // &self.active_dynamic_set + // } + + pub(crate) fn active_island_range(&self, island_id: usize) -> std::ops::Range { + self.active_islands[island_id]..self.active_islands[island_id + 1] + } + + pub(crate) fn active_island(&self, island_id: usize) -> &[RigidBodyHandle] { + &self.active_dynamic_set[self.active_island_range(island_id)] + } + + pub(crate) fn maintain_active_set(&mut self) { + for handle in self.activation_channel.1.try_iter() { + if let Some(rb) = self.bodies.get_mut(handle) { + // Push the body to the active set if it is not + // sleeping and if it is not already inside of the active set. + if !rb.is_sleeping() // May happen if the body was put to sleep manually. + && rb.is_dynamic() // Only dynamic bodies are in the active dynamic set. + && self.active_dynamic_set.get(rb.active_set_id) != Some(&handle) + { + rb.active_set_id = self.active_dynamic_set.len(); // This will handle the case where the activation_channel contains duplicates. + self.active_dynamic_set.push(handle); + } + } + } + } + + pub(crate) fn update_active_set_with_contacts( + &mut self, + colliders: &ColliderSet, + contact_graph: &InteractionGraph, + joint_graph: &InteractionGraph, + min_island_size: usize, + ) { + assert!( + min_island_size > 0, + "The minimum island size must be at least 1." + ); + + // Update the energy of every rigid body and + // keep only those that may not sleep. + // let t = instant::now(); + self.active_set_timestamp += 1; + self.stack.clear(); + self.can_sleep.clear(); + + // NOTE: the `.rev()` is here so that two successive timesteps preserve + // the order of the bodies in the `active_dynamic_set` vec. This reversal + // does not seem to affect performances nor stability. However it makes + // debugging slightly nicer so we keep this rev. + for h in self.active_dynamic_set.drain(..).rev() { + let rb = &mut self.bodies[h]; + rb.update_energy(); + if rb.activation.energy <= rb.activation.threshold { + // Mark them as sleeping for now. This will + // be set to false during the graph traversal + // if it should not be put to sleep. + rb.activation.sleeping = true; + self.can_sleep.push(h); + } else { + self.stack.push(h); + } + } + + // println!("Selection: {}", instant::now() - t); + + // let t = instant::now(); + // Propagation of awake state and awake island computation through the + // traversal of the interaction graph. + self.active_islands.clear(); + self.active_islands.push(0); + + // The max avoid underflow when the stack is empty. + let mut island_marker = self.stack.len().max(1) - 1; + + while let Some(handle) = self.stack.pop() { + let rb = &mut self.bodies[handle]; + + if rb.active_set_timestamp == self.active_set_timestamp || !rb.is_dynamic() { + // We already visited this body and its neighbors. + // Also, we don't propagate awake state through static bodies. + continue; + } else if self.stack.len() < island_marker { + if self.active_dynamic_set.len() - *self.active_islands.last().unwrap() + >= min_island_size + { + // We are starting a new island. + self.active_islands.push(self.active_dynamic_set.len()); + } + + island_marker = self.stack.len(); + } + + rb.wake_up(); + rb.active_island_id = self.active_islands.len() - 1; + rb.active_set_id = self.active_dynamic_set.len(); + rb.active_set_offset = rb.active_set_id - self.active_islands[rb.active_island_id]; + rb.active_set_timestamp = self.active_set_timestamp; + self.active_dynamic_set.push(handle); + + // Read all the contacts and push objects touching this one. + for collider_handle in &rb.colliders { + let collider = &colliders[*collider_handle]; + + for inter in contact_graph.interactions_with(collider.contact_graph_index) { + for manifold in &inter.2.manifolds { + if manifold.num_active_contacts() > 0 { + let other = + crate::utils::other_handle((inter.0, inter.1), *collider_handle); + let other_body = colliders[other].parent; + self.stack.push(other_body); + break; + } + } + } + } + + for inter in joint_graph.interactions_with(rb.joint_graph_index) { + let other = crate::utils::other_handle((inter.0, inter.1), handle); + self.stack.push(other); + } + } + + self.active_islands.push(self.active_dynamic_set.len()); + // println!( + // "Extraction: {}, num islands: {}", + // instant::now() - t, + // self.active_islands.len() - 1 + // ); + + // Actually put to sleep bodies which have not been detected as awake. + // let t = instant::now(); + for h in &self.can_sleep { + let b = &mut self.bodies[*h]; + if b.activation.sleeping { + b.sleep(); + } + } + // println!("Activation: {}", instant::now() - t); + } +} + +impl Index for RigidBodySet { + type Output = RigidBody; + + fn index(&self, index: RigidBodyHandle) -> &RigidBody { + &self.bodies[index] + } +} + +impl IndexMut for RigidBodySet { + fn index_mut(&mut self, index: RigidBodyHandle) -> &mut RigidBody { + &mut self.bodies[index] + } +} diff --git a/src/dynamics/solver/categorization.rs b/src/dynamics/solver/categorization.rs new file mode 100644 index 000000000..9ddf79178 --- /dev/null +++ b/src/dynamics/solver/categorization.rs @@ -0,0 +1,70 @@ +use crate::dynamics::{JointGraphEdge, JointIndex, RigidBodySet}; +use crate::geometry::{ContactManifold, ContactManifoldIndex, KinematicsCategory}; + +pub(crate) fn categorize_position_contacts( + bodies: &RigidBodySet, + manifolds: &[&mut ContactManifold], + manifold_indices: &[ContactManifoldIndex], + out_point_point_ground: &mut Vec, + out_plane_point_ground: &mut Vec, + out_point_point: &mut Vec, + out_plane_point: &mut Vec, +) { + for manifold_i in manifold_indices { + let manifold = &manifolds[*manifold_i]; + let rb1 = &bodies[manifold.body_pair.body1]; + let rb2 = &bodies[manifold.body_pair.body2]; + + if !rb1.is_dynamic() || !rb2.is_dynamic() { + match manifold.kinematics.category { + KinematicsCategory::PointPoint => out_point_point_ground.push(*manifold_i), + KinematicsCategory::PlanePoint => out_plane_point_ground.push(*manifold_i), + } + } else { + match manifold.kinematics.category { + KinematicsCategory::PointPoint => out_point_point.push(*manifold_i), + KinematicsCategory::PlanePoint => out_plane_point.push(*manifold_i), + } + } + } +} + +pub(crate) fn categorize_velocity_contacts( + bodies: &RigidBodySet, + manifolds: &[&mut ContactManifold], + manifold_indices: &[ContactManifoldIndex], + out_ground: &mut Vec, + out_not_ground: &mut Vec, +) { + for manifold_i in manifold_indices { + let manifold = &manifolds[*manifold_i]; + let rb1 = &bodies[manifold.body_pair.body1]; + let rb2 = &bodies[manifold.body_pair.body2]; + + if !rb1.is_dynamic() || !rb2.is_dynamic() { + out_ground.push(*manifold_i) + } else { + out_not_ground.push(*manifold_i) + } + } +} + +pub(crate) fn categorize_joints( + bodies: &RigidBodySet, + joints: &[JointGraphEdge], + joint_indices: &[JointIndex], + ground_joints: &mut Vec, + nonground_joints: &mut Vec, +) { + for joint_i in joint_indices { + let joint = &joints[*joint_i].weight; + let rb1 = &bodies[joint.body1]; + let rb2 = &bodies[joint.body2]; + + if !rb1.is_dynamic() || !rb2.is_dynamic() { + ground_joints.push(*joint_i); + } else { + nonground_joints.push(*joint_i); + } + } +} diff --git a/src/dynamics/solver/delta_vel.rs b/src/dynamics/solver/delta_vel.rs new file mode 100644 index 000000000..c4a424b0e --- /dev/null +++ b/src/dynamics/solver/delta_vel.rs @@ -0,0 +1,18 @@ +use crate::math::{AngVector, Vector}; +use na::{Scalar, SimdRealField}; + +#[derive(Copy, Clone, Debug)] +//#[repr(align(64))] +pub(crate) struct DeltaVel { + pub linear: Vector, + pub angular: AngVector, +} + +impl DeltaVel { + pub fn zero() -> Self { + Self { + linear: na::zero(), + angular: na::zero(), + } + } +} diff --git a/src/dynamics/solver/interaction_groups.rs b/src/dynamics/solver/interaction_groups.rs new file mode 100644 index 000000000..04acaaff5 --- /dev/null +++ b/src/dynamics/solver/interaction_groups.rs @@ -0,0 +1,434 @@ +use crate::dynamics::{BodyPair, JointGraphEdge, JointIndex, RigidBodySet}; +use crate::geometry::{ContactManifold, ContactManifoldIndex}; +#[cfg(feature = "simd-is-enabled")] +use { + crate::math::{SIMD_LAST_INDEX, SIMD_WIDTH}, + vec_map::VecMap, +}; + +pub(crate) trait PairInteraction { + fn body_pair(&self) -> BodyPair; +} + +impl<'a> PairInteraction for &'a mut ContactManifold { + fn body_pair(&self) -> BodyPair { + self.body_pair + } +} + +impl<'a> PairInteraction for JointGraphEdge { + fn body_pair(&self) -> BodyPair { + BodyPair::new(self.weight.body1, self.weight.body2) + } +} + +#[cfg(feature = "parallel")] +pub(crate) struct ParallelInteractionGroups { + bodies_color: Vec, // Workspace. + interaction_indices: Vec, // Workspace. + interaction_colors: Vec, // Workspace. + sorted_interactions: Vec, + groups: Vec, +} + +#[cfg(feature = "parallel")] +impl ParallelInteractionGroups { + pub fn new() -> Self { + Self { + bodies_color: Vec::new(), + interaction_indices: Vec::new(), + interaction_colors: Vec::new(), + sorted_interactions: Vec::new(), + groups: Vec::new(), + } + } + + pub fn group(&self, i: usize) -> &[usize] { + let range = self.groups[i]..self.groups[i + 1]; + &self.sorted_interactions[range] + } + pub fn num_groups(&self) -> usize { + self.groups.len() - 1 + } + + pub fn group_interactions( + &mut self, + island_id: usize, + bodies: &RigidBodySet, + interactions: &[Interaction], + interaction_indices: &[usize], + ) { + let num_island_bodies = bodies.active_island(island_id).len(); + self.bodies_color.clear(); + self.interaction_indices.clear(); + self.groups.clear(); + self.sorted_interactions.clear(); + self.interaction_colors.clear(); + + let mut color_len = [0; 128]; + self.bodies_color.resize(num_island_bodies, 0u128); + self.interaction_indices + .extend_from_slice(interaction_indices); + self.interaction_colors.resize(interaction_indices.len(), 0); + let bcolors = &mut self.bodies_color; + + for (interaction_id, color) in self + .interaction_indices + .iter() + .zip(self.interaction_colors.iter_mut()) + { + let body_pair = interactions[*interaction_id].body_pair(); + let rb1 = &bodies[body_pair.body1]; + let rb2 = &bodies[body_pair.body2]; + + match (rb1.is_static(), rb2.is_static()) { + (false, false) => { + let color_mask = + bcolors[rb1.active_set_offset] | bcolors[rb2.active_set_offset]; + *color = (!color_mask).trailing_zeros() as usize; + color_len[*color] += 1; + bcolors[rb1.active_set_offset] |= 1 << *color; + bcolors[rb2.active_set_offset] |= 1 << *color; + } + (true, false) => { + let color_mask = bcolors[rb2.active_set_offset]; + *color = (!color_mask).trailing_zeros() as usize; + color_len[*color] += 1; + bcolors[rb2.active_set_offset] |= 1 << *color; + } + (false, true) => { + let color_mask = bcolors[rb1.active_set_offset]; + *color = (!color_mask).trailing_zeros() as usize; + color_len[*color] += 1; + bcolors[rb1.active_set_offset] |= 1 << *color; + } + (true, true) => unreachable!(), + } + } + + let mut sort_offsets = [0; 128]; + let mut last_offset = 0; + + for i in 0..128 { + if color_len[i] == 0 { + break; + } + + self.groups.push(last_offset); + sort_offsets[i] = last_offset; + last_offset += color_len[i]; + } + + self.sorted_interactions + .resize(interaction_indices.len(), 0); + + for (interaction_id, color) in interaction_indices + .iter() + .zip(self.interaction_colors.iter()) + { + self.sorted_interactions[sort_offsets[*color]] = *interaction_id; + sort_offsets[*color] += 1; + } + + self.groups.push(self.sorted_interactions.len()); + } +} + +pub(crate) struct InteractionGroups { + #[cfg(feature = "simd-is-enabled")] + buckets: VecMap<([usize; SIMD_WIDTH], usize)>, + #[cfg(feature = "simd-is-enabled")] + body_masks: Vec, + #[cfg(feature = "simd-is-enabled")] + pub grouped_interactions: Vec, + pub nongrouped_interactions: Vec, +} + +impl InteractionGroups { + pub fn new() -> Self { + Self { + #[cfg(feature = "simd-is-enabled")] + buckets: VecMap::new(), + #[cfg(feature = "simd-is-enabled")] + body_masks: Vec::new(), + #[cfg(feature = "simd-is-enabled")] + grouped_interactions: Vec::new(), + nongrouped_interactions: Vec::new(), + } + } + + // FIXME: there is a lot of duplicated code with group_manifolds here. + // But we don't refactor just now because we may end up with distinct + // grouping strategies in the future. + #[cfg(not(feature = "simd-is-enabled"))] + pub fn group_joints( + &mut self, + _island_id: usize, + _bodies: &RigidBodySet, + _interactions: &[JointGraphEdge], + interaction_indices: &[JointIndex], + ) { + self.nongrouped_interactions + .extend_from_slice(interaction_indices); + } + + #[cfg(feature = "simd-is-enabled")] + pub fn group_joints( + &mut self, + island_id: usize, + bodies: &RigidBodySet, + interactions: &[JointGraphEdge], + interaction_indices: &[JointIndex], + ) { + // NOTE: in 3D we have up to 10 different joint types. + // In 2D we only have 5 joint types. + #[cfg(feature = "dim3")] + const NUM_JOINT_TYPES: usize = 10; + #[cfg(feature = "dim2")] + const NUM_JOINT_TYPES: usize = 5; + + // The j-th bit of joint_type_conflicts[i] indicates that the + // j-th bucket contains a joint with a type different than `i`. + let mut joint_type_conflicts = [0u128; NUM_JOINT_TYPES]; + + // Note: each bit of a body mask indicates what bucket already contains + // a constraints involving this body. + // FIXME: currently, this is a bit overconservative because when a bucket + // is full, we don't clear the corresponding body mask bit. This may result + // in less grouped constraints. + self.body_masks + .resize(bodies.active_island(island_id).len(), 0u128); + + // NOTE: each bit of the occupied mask indicates what bucket already + // contains at least one constraint. + let mut occupied_mask = 0u128; + + for interaction_i in interaction_indices { + let interaction = &interactions[*interaction_i].weight; + let body1 = &bodies[interaction.body1]; + let body2 = &bodies[interaction.body2]; + let is_static1 = !body1.is_dynamic(); + let is_static2 = !body2.is_dynamic(); + + if is_static1 && is_static2 { + continue; + } + + let ijoint = interaction.params.type_id(); + let i1 = body1.active_set_offset; + let i2 = body2.active_set_offset; + let conflicts = + self.body_masks[i1] | self.body_masks[i2] | joint_type_conflicts[ijoint]; + let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts. + let conflictfree_occupied_targets = conflictfree_targets & occupied_mask; + + let target_index = if conflictfree_occupied_targets != 0 { + // Try to fill partial WContacts first. + conflictfree_occupied_targets.trailing_zeros() + } else { + conflictfree_targets.trailing_zeros() + }; + + if target_index == 128 { + // The interaction conflicts with every bucket we can manage. + // So push it in an nongrouped interaction list that won't be combined with + // any other interactions. + self.nongrouped_interactions.push(*interaction_i); + continue; + } + + let target_mask_bit = 1 << target_index; + + let bucket = self + .buckets + .entry(target_index as usize) + .or_insert_with(|| ([0; SIMD_WIDTH], 0)); + + if bucket.1 == SIMD_LAST_INDEX { + // We completed our group. + (bucket.0)[SIMD_LAST_INDEX] = *interaction_i; + self.grouped_interactions.extend_from_slice(&bucket.0); + bucket.1 = 0; + occupied_mask &= !target_mask_bit; + + for k in 0..NUM_JOINT_TYPES { + joint_type_conflicts[k] &= !target_mask_bit; + } + } else { + (bucket.0)[bucket.1] = *interaction_i; + bucket.1 += 1; + occupied_mask |= target_mask_bit; + + for k in 0..ijoint { + joint_type_conflicts[k] |= target_mask_bit; + } + for k in ijoint + 1..NUM_JOINT_TYPES { + joint_type_conflicts[k] |= target_mask_bit; + } + } + + // NOTE: static bodies don't transmit forces. Therefore they don't + // imply any interaction conflicts. + if !is_static1 { + self.body_masks[i1] |= target_mask_bit; + } + + if !is_static2 { + self.body_masks[i2] |= target_mask_bit; + } + } + + self.nongrouped_interactions.extend( + self.buckets + .values() + .flat_map(|e| e.0.iter().take(e.1).copied()), + ); + self.buckets.clear(); + self.body_masks.iter_mut().for_each(|e| *e = 0); + + assert!( + self.grouped_interactions.len() % SIMD_WIDTH == 0, + "Invalid SIMD contact grouping." + ); + + // println!( + // "Num grouped interactions: {}, nongrouped: {}", + // self.grouped_interactions.len(), + // self.nongrouped_interactions.len() + // ); + } + + pub fn clear_groups(&mut self) { + #[cfg(feature = "simd-is-enabled")] + self.grouped_interactions.clear(); + self.nongrouped_interactions.clear(); + } + + #[cfg(not(feature = "simd-is-enabled"))] + pub fn group_manifolds( + &mut self, + _island_id: usize, + _bodies: &RigidBodySet, + _interactions: &[&mut ContactManifold], + interaction_indices: &[ContactManifoldIndex], + ) { + self.nongrouped_interactions + .extend_from_slice(interaction_indices); + } + + #[cfg(feature = "simd-is-enabled")] + pub fn group_manifolds( + &mut self, + island_id: usize, + bodies: &RigidBodySet, + interactions: &[&mut ContactManifold], + interaction_indices: &[ContactManifoldIndex], + ) { + // Note: each bit of a body mask indicates what bucket already contains + // a constraints involving this body. + // FIXME: currently, this is a bit overconservative because when a bucket + // is full, we don't clear the corresponding body mask bit. This may result + // in less grouped contacts. + // NOTE: body_masks and buckets are already cleared/zeroed at the end of each sort loop. + self.body_masks + .resize(bodies.active_island(island_id).len(), 0u128); + + // NOTE: each bit of the occupied mask indicates what bucket already + // contains at least one constraint. + let mut occupied_mask = 0u128; + let max_interaction_points = interaction_indices + .iter() + .map(|i| interactions[*i].num_active_contacts()) + .max() + .unwrap_or(1); + + // FIXME: find a way to reduce the number of iteration. + // There must be a way to iterate just once on every interaction indices + // instead of MAX_MANIFOLD_POINTS times. + for k in 1..=max_interaction_points { + for interaction_i in interaction_indices { + let interaction = &interactions[*interaction_i]; + + // FIXME: how could we avoid iterating + // on each interaction at every iteration on k? + if interaction.num_active_contacts() != k { + continue; + } + + let body1 = &bodies[interaction.body_pair.body1]; + let body2 = &bodies[interaction.body_pair.body2]; + let is_static1 = !body1.is_dynamic(); + let is_static2 = !body2.is_dynamic(); + + // FIXME: don't generate interactions between static bodies in the first place. + if is_static1 && is_static2 { + continue; + } + + let i1 = body1.active_set_offset; + let i2 = body2.active_set_offset; + let conflicts = self.body_masks[i1] | self.body_masks[i2]; + let conflictfree_targets = !(conflicts & occupied_mask); // The & is because we consider empty buckets as free of conflicts. + let conflictfree_occupied_targets = conflictfree_targets & occupied_mask; + + let target_index = if conflictfree_occupied_targets != 0 { + // Try to fill partial WContacts first. + conflictfree_occupied_targets.trailing_zeros() + } else { + conflictfree_targets.trailing_zeros() + }; + + if target_index == 128 { + // The interaction conflicts with every bucket we can manage. + // So push it in an nongrouped interaction list that won't be combined with + // any other interactions. + self.nongrouped_interactions.push(*interaction_i); + continue; + } + + let target_mask_bit = 1 << target_index; + + let bucket = self + .buckets + .entry(target_index as usize) + .or_insert_with(|| ([0; SIMD_WIDTH], 0)); + + if bucket.1 == SIMD_LAST_INDEX { + // We completed our group. + (bucket.0)[SIMD_LAST_INDEX] = *interaction_i; + self.grouped_interactions.extend_from_slice(&bucket.0); + bucket.1 = 0; + occupied_mask = occupied_mask & (!target_mask_bit); + } else { + (bucket.0)[bucket.1] = *interaction_i; + bucket.1 += 1; + occupied_mask = occupied_mask | target_mask_bit; + } + + // NOTE: static bodies don't transmit forces. Therefore they don't + // imply any interaction conflicts. + if !is_static1 { + self.body_masks[i1] |= target_mask_bit; + } + + if !is_static2 { + self.body_masks[i2] |= target_mask_bit; + } + } + + self.nongrouped_interactions.extend( + self.buckets + .values() + .flat_map(|e| e.0.iter().take(e.1).copied()), + ); + self.buckets.clear(); + self.body_masks.iter_mut().for_each(|e| *e = 0); + occupied_mask = 0u128; + } + + assert!( + self.grouped_interactions.len() % SIMD_WIDTH == 0, + "Invalid SIMD contact grouping." + ); + } +} diff --git a/src/dynamics/solver/island_solver.rs b/src/dynamics/solver/island_solver.rs new file mode 100644 index 000000000..7ce142abe --- /dev/null +++ b/src/dynamics/solver/island_solver.rs @@ -0,0 +1,73 @@ +use super::{PositionSolver, VelocitySolver}; +use crate::counters::Counters; +use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet}; +use crate::geometry::{ContactManifold, ContactManifoldIndex}; + +pub struct IslandSolver { + velocity_solver: VelocitySolver, + position_solver: PositionSolver, +} + +impl IslandSolver { + pub fn new() -> Self { + Self { + velocity_solver: VelocitySolver::new(), + position_solver: PositionSolver::new(), + } + } + + pub fn solve_island( + &mut self, + island_id: usize, + counters: &mut Counters, + params: &IntegrationParameters, + bodies: &mut RigidBodySet, + manifolds: &mut [&mut ContactManifold], + manifold_indices: &[ContactManifoldIndex], + joints: &mut [JointGraphEdge], + joint_indices: &[JointIndex], + ) { + if manifold_indices.len() != 0 || joint_indices.len() != 0 { + counters.solver.velocity_assembly_time.resume(); + self.velocity_solver.init_constraints( + island_id, + params, + bodies, + manifolds, + &manifold_indices, + joints, + &joint_indices, + ); + counters.solver.velocity_assembly_time.pause(); + + counters.solver.velocity_resolution_time.resume(); + self.velocity_solver + .solve_constraints(island_id, params, bodies, manifolds, joints); + counters.solver.velocity_resolution_time.pause(); + + counters.solver.position_assembly_time.resume(); + self.position_solver.init_constraints( + island_id, + params, + bodies, + manifolds, + &manifold_indices, + joints, + &joint_indices, + ); + counters.solver.position_assembly_time.pause(); + } + + counters.solver.velocity_update_time.resume(); + bodies + .foreach_active_island_body_mut_internal(island_id, |_, rb| rb.integrate(params.dt())); + counters.solver.velocity_update_time.pause(); + + if manifold_indices.len() != 0 || joint_indices.len() != 0 { + counters.solver.position_resolution_time.resume(); + self.position_solver + .solve_constraints(island_id, params, bodies); + counters.solver.position_resolution_time.pause(); + } + } +} diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs new file mode 100644 index 000000000..21a537ed7 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint.rs @@ -0,0 +1,165 @@ +use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody}; +#[cfg(feature = "dim2")] +use crate::math::SdpMatrix; +use crate::math::{AngularInertia, Isometry, Point, Rotation}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; + +#[derive(Debug)] +pub(crate) struct BallPositionConstraint { + position1: usize, + position2: usize, + + local_com1: Point, + local_com2: Point, + + im1: f32, + im2: f32, + + ii1: AngularInertia, + ii2: AngularInertia, + + local_anchor1: Point, + local_anchor2: Point, +} + +impl BallPositionConstraint { + pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &BallJoint) -> Self { + Self { + local_com1: rb1.mass_properties.local_com, + local_com2: rb2.mass_properties.local_com, + im1: rb1.mass_properties.inv_mass, + im2: rb2.mass_properties.inv_mass, + ii1: rb1.world_inv_inertia_sqrt.squared(), + ii2: rb2.world_inv_inertia_sqrt.squared(), + local_anchor1: cparams.local_anchor1, + local_anchor2: cparams.local_anchor2, + position1: rb1.active_set_offset, + position2: rb2.active_set_offset, + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + let mut position1 = positions[self.position1 as usize]; + let mut position2 = positions[self.position2 as usize]; + + let anchor1 = position1 * self.local_anchor1; + let anchor2 = position2 * self.local_anchor2; + + let com1 = position1 * self.local_com1; + let com2 = position2 * self.local_com2; + + let err = anchor1 - anchor2; + + let centered_anchor1 = anchor1 - com1; + let centered_anchor2 = anchor2 - com2; + + let cmat1 = centered_anchor1.gcross_matrix(); + let cmat2 = centered_anchor2.gcross_matrix(); + + // NOTE: the -cmat1 is just a simpler way of doing cmat1.transpose() + // because it is anti-symmetric. + #[cfg(feature = "dim3")] + let lhs = self.ii1.quadform(&cmat1).add_diagonal(self.im1) + + self.ii2.quadform(&cmat2).add_diagonal(self.im2); + + // In 2D we just unroll the computation because + // it's just easier that way. It is also + // faster because in 2D lhs will be symmetric. + #[cfg(feature = "dim2")] + let lhs = { + let m11 = + self.im1 + self.im2 + cmat1.x * cmat1.x * self.ii1 + cmat2.x * cmat2.x * self.ii2; + let m12 = cmat1.x * cmat1.y * self.ii1 + cmat2.x * cmat2.y * self.ii2; + let m22 = + self.im1 + self.im2 + cmat1.y * cmat1.y * self.ii1 + cmat2.y * cmat2.y * self.ii2; + SdpMatrix::new(m11, m12, m22) + }; + + let inv_lhs = lhs.inverse_unchecked(); + let impulse = inv_lhs * -(err * params.joint_erp); + + position1.translation.vector += self.im1 * impulse; + position2.translation.vector -= self.im2 * impulse; + + let angle1 = self.ii1.transform_vector(centered_anchor1.gcross(impulse)); + let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse)); + + position1.rotation = Rotation::new(angle1) * position1.rotation; + position2.rotation = Rotation::new(angle2) * position2.rotation; + + positions[self.position1 as usize] = position1; + positions[self.position2 as usize] = position2; + } +} + +#[derive(Debug)] +pub(crate) struct BallPositionGroundConstraint { + position2: usize, + anchor1: Point, + im2: f32, + ii2: AngularInertia, + local_anchor2: Point, + local_com2: Point, +} + +impl BallPositionGroundConstraint { + pub fn from_params( + rb1: &RigidBody, + rb2: &RigidBody, + cparams: &BallJoint, + flipped: bool, + ) -> Self { + if flipped { + // Note the only thing that is flipped here + // are the local_anchors. The rb1 and rb2 have + // already been flipped by the caller. + Self { + anchor1: rb1.predicted_position * cparams.local_anchor2, + im2: rb2.mass_properties.inv_mass, + ii2: rb2.world_inv_inertia_sqrt.squared(), + local_anchor2: cparams.local_anchor1, + position2: rb2.active_set_offset, + local_com2: rb2.mass_properties.local_com, + } + } else { + Self { + anchor1: rb1.predicted_position * cparams.local_anchor1, + im2: rb2.mass_properties.inv_mass, + ii2: rb2.world_inv_inertia_sqrt.squared(), + local_anchor2: cparams.local_anchor2, + position2: rb2.active_set_offset, + local_com2: rb2.mass_properties.local_com, + } + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + let mut position2 = positions[self.position2 as usize]; + + let anchor2 = position2 * self.local_anchor2; + let com2 = position2 * self.local_com2; + + let err = self.anchor1 - anchor2; + let centered_anchor2 = anchor2 - com2; + let cmat2 = centered_anchor2.gcross_matrix(); + + #[cfg(feature = "dim3")] + let lhs = self.ii2.quadform(&cmat2).add_diagonal(self.im2); + + #[cfg(feature = "dim2")] + let lhs = { + let m11 = self.im2 + cmat2.x * cmat2.x * self.ii2; + let m12 = cmat2.x * cmat2.y * self.ii2; + let m22 = self.im2 + cmat2.y * cmat2.y * self.ii2; + SdpMatrix::new(m11, m12, m22) + }; + + let inv_lhs = lhs.inverse_unchecked(); + let impulse = inv_lhs * -(err * params.joint_erp); + position2.translation.vector -= self.im2 * impulse; + + let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse)); + position2.rotation = Rotation::new(angle2) * position2.rotation; + positions[self.position2 as usize] = position2; + } +} diff --git a/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs new file mode 100644 index 000000000..c552d5715 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/ball_position_constraint_wide.rs @@ -0,0 +1,199 @@ +use crate::dynamics::{BallJoint, IntegrationParameters, RigidBody}; +#[cfg(feature = "dim2")] +use crate::math::SdpMatrix; +use crate::math::{AngularInertia, Isometry, Point, Rotation, SimdFloat, SIMD_WIDTH}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; +use simba::simd::SimdValue; + +#[derive(Debug)] +pub(crate) struct WBallPositionConstraint { + position1: [usize; SIMD_WIDTH], + position2: [usize; SIMD_WIDTH], + + local_com1: Point, + local_com2: Point, + + im1: SimdFloat, + im2: SimdFloat, + + ii1: AngularInertia, + ii2: AngularInertia, + + local_anchor1: Point, + local_anchor2: Point, +} + +impl WBallPositionConstraint { + pub fn from_params( + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&BallJoint; SIMD_WIDTH], + ) -> Self { + let local_com1 = Point::from(array![|ii| rbs1[ii].mass_properties.local_com; SIMD_WIDTH]); + let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]); + let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii1 = AngularInertia::::from( + array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ) + .squared(); + let ii2 = AngularInertia::::from( + array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ) + .squared(); + let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); + let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); + let position1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; + let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + + Self { + local_com1, + local_com2, + im1, + im2, + ii1, + ii2, + local_anchor1, + local_anchor2, + position1, + position2, + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + let mut position1 = Isometry::from(array![|ii| positions[self.position1[ii]]; SIMD_WIDTH]); + let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]); + + let anchor1 = position1 * self.local_anchor1; + let anchor2 = position2 * self.local_anchor2; + + let com1 = position1 * self.local_com1; + let com2 = position2 * self.local_com2; + + let err = anchor1 - anchor2; + + let centered_anchor1 = anchor1 - com1; + let centered_anchor2 = anchor2 - com2; + + let cmat1 = centered_anchor1.gcross_matrix(); + let cmat2 = centered_anchor2.gcross_matrix(); + + // NOTE: the -cmat1 is just a simpler way of doing cmat1.transpose() + // because it is anti-symmetric. + #[cfg(feature = "dim3")] + let lhs = self.ii1.quadform(&cmat1).add_diagonal(self.im1) + + self.ii2.quadform(&cmat2).add_diagonal(self.im2); + + // In 2D we just unroll the computation because + // it's just easier that way. + #[cfg(feature = "dim2")] + let lhs = { + let m11 = + self.im1 + self.im2 + cmat1.x * cmat1.x * self.ii1 + cmat2.x * cmat2.x * self.ii2; + let m12 = cmat1.x * cmat1.y * self.ii1 + cmat2.x * cmat2.y * self.ii2; + let m22 = + self.im1 + self.im2 + cmat1.y * cmat1.y * self.ii1 + cmat2.y * cmat2.y * self.ii2; + SdpMatrix::new(m11, m12, m22) + }; + + let inv_lhs = lhs.inverse_unchecked(); + let impulse = inv_lhs * -(err * SimdFloat::splat(params.joint_erp)); + + position1.translation.vector += impulse * self.im1; + position2.translation.vector -= impulse * self.im2; + + let angle1 = self.ii1.transform_vector(centered_anchor1.gcross(impulse)); + let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse)); + + position1.rotation = Rotation::new(angle1) * position1.rotation; + position2.rotation = Rotation::new(angle2) * position2.rotation; + + for ii in 0..SIMD_WIDTH { + positions[self.position1[ii]] = position1.extract(ii); + } + for ii in 0..SIMD_WIDTH { + positions[self.position2[ii]] = position2.extract(ii); + } + } +} + +#[derive(Debug)] +pub(crate) struct WBallPositionGroundConstraint { + position2: [usize; SIMD_WIDTH], + anchor1: Point, + im2: SimdFloat, + ii2: AngularInertia, + local_anchor2: Point, + local_com2: Point, +} + +impl WBallPositionGroundConstraint { + pub fn from_params( + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&BallJoint; SIMD_WIDTH], + flipped: [bool; SIMD_WIDTH], + ) -> Self { + let position1 = Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]); + let anchor1 = position1 + * Point::from(array![|ii| if flipped[ii] { + cparams[ii].local_anchor2 + } else { + cparams[ii].local_anchor1 + }; SIMD_WIDTH]); + let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2 = AngularInertia::::from( + array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ) + .squared(); + let local_anchor2 = Point::from(array![|ii| if flipped[ii] { + cparams[ii].local_anchor1 + } else { + cparams[ii].local_anchor2 + }; SIMD_WIDTH]); + let position2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let local_com2 = Point::from(array![|ii| rbs2[ii].mass_properties.local_com; SIMD_WIDTH]); + + Self { + anchor1, + im2, + ii2, + local_anchor2, + position2, + local_com2, + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + let mut position2 = Isometry::from(array![|ii| positions[self.position2[ii]]; SIMD_WIDTH]); + + let anchor2 = position2 * self.local_anchor2; + let com2 = position2 * self.local_com2; + + let err = self.anchor1 - anchor2; + let centered_anchor2 = anchor2 - com2; + let cmat2 = centered_anchor2.gcross_matrix(); + + #[cfg(feature = "dim3")] + let lhs = self.ii2.quadform(&cmat2).add_diagonal(self.im2); + + #[cfg(feature = "dim2")] + let lhs = { + let m11 = self.im2 + cmat2.x * cmat2.x * self.ii2; + let m12 = cmat2.x * cmat2.y * self.ii2; + let m22 = self.im2 + cmat2.y * cmat2.y * self.ii2; + SdpMatrix::new(m11, m12, m22) + }; + + let inv_lhs = lhs.inverse_unchecked(); + let impulse = inv_lhs * -(err * SimdFloat::splat(params.joint_erp)); + position2.translation.vector -= impulse * self.im2; + + let angle2 = self.ii2.transform_vector(centered_anchor2.gcross(-impulse)); + position2.rotation = Rotation::new(angle2) * position2.rotation; + + for ii in 0..SIMD_WIDTH { + positions[self.position2[ii]] = position2.extract(ii); + } + } +} diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs new file mode 100644 index 000000000..97ba24445 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint.rs @@ -0,0 +1,238 @@ +use crate::dynamics::solver::DeltaVel; +use crate::dynamics::{ + BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, +}; +use crate::math::{SdpMatrix, Vector}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; + +#[derive(Debug)] +pub(crate) struct BallVelocityConstraint { + mj_lambda1: usize, + mj_lambda2: usize, + + joint_id: JointIndex, + + rhs: Vector, + pub(crate) impulse: Vector, + + gcross1: Vector, + gcross2: Vector, + + inv_lhs: SdpMatrix, + + im1: f32, + im2: f32, +} + +impl BallVelocityConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: JointIndex, + rb1: &RigidBody, + rb2: &RigidBody, + cparams: &BallJoint, + ) -> Self { + let anchor1 = rb1.position * cparams.local_anchor1 - rb1.world_com; + let anchor2 = rb2.position * cparams.local_anchor2 - rb2.world_com; + + let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1); + let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2); + let im1 = rb1.mass_properties.inv_mass; + let im2 = rb2.mass_properties.inv_mass; + + let rhs = -(vel1 - vel2); + let lhs; + + let cmat1 = anchor1.gcross_matrix(); + let cmat2 = anchor2.gcross_matrix(); + + #[cfg(feature = "dim3")] + { + lhs = rb2 + .world_inv_inertia_sqrt + .squared() + .quadform(&cmat2) + .add_diagonal(im2) + + rb1 + .world_inv_inertia_sqrt + .squared() + .quadform(&cmat1) + .add_diagonal(im1); + } + + // In 2D we just unroll the computation because + // it's just easier that way. + #[cfg(feature = "dim2")] + { + let ii1 = rb1.world_inv_inertia_sqrt.squared(); + let ii2 = rb2.world_inv_inertia_sqrt.squared(); + let m11 = im1 + im2 + cmat1.x * cmat1.x * ii1 + cmat2.x * cmat2.x * ii2; + let m12 = cmat1.x * cmat1.y * ii1 + cmat2.x * cmat2.y * ii2; + let m22 = im1 + im2 + cmat1.y * cmat1.y * ii1 + cmat2.y * cmat2.y * ii2; + lhs = SdpMatrix::new(m11, m12, m22) + } + + let gcross1 = rb1.world_inv_inertia_sqrt.transform_lin_vector(anchor1); + let gcross2 = rb2.world_inv_inertia_sqrt.transform_lin_vector(anchor2); + + let inv_lhs = lhs.inverse_unchecked(); + + BallVelocityConstraint { + joint_id, + mj_lambda1: rb1.active_set_offset, + mj_lambda2: rb2.active_set_offset, + im1, + im2, + impulse: cparams.impulse * params.warmstart_coeff, + gcross1, + gcross2, + rhs, + inv_lhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + mj_lambda1.linear += self.im1 * self.impulse; + mj_lambda1.angular += self.gcross1.gcross(self.impulse); + mj_lambda2.linear -= self.im2 * self.impulse; + mj_lambda2.angular -= self.gcross2.gcross(self.impulse); + + mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + let vel1 = mj_lambda1.linear + mj_lambda1.angular.gcross(self.gcross1); + let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2); + let dvel = -vel1 + vel2 + self.rhs; + + let impulse = self.inv_lhs * dvel; + self.impulse += impulse; + + mj_lambda1.linear += self.im1 * impulse; + mj_lambda1.angular += self.gcross1.gcross(impulse); + + mj_lambda2.linear -= self.im2 * impulse; + mj_lambda2.angular -= self.gcross2.gcross(impulse); + + mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + let joint = &mut joints_all[self.joint_id].weight; + if let JointParams::BallJoint(ball) = &mut joint.params { + ball.impulse = self.impulse + } + } +} + +#[derive(Debug)] +pub(crate) struct BallVelocityGroundConstraint { + mj_lambda2: usize, + joint_id: JointIndex, + rhs: Vector, + impulse: Vector, + gcross2: Vector, + inv_lhs: SdpMatrix, + im2: f32, +} + +impl BallVelocityGroundConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: JointIndex, + rb1: &RigidBody, + rb2: &RigidBody, + cparams: &BallJoint, + flipped: bool, + ) -> Self { + let (anchor1, anchor2) = if flipped { + ( + rb1.position * cparams.local_anchor2 - rb1.world_com, + rb2.position * cparams.local_anchor1 - rb2.world_com, + ) + } else { + ( + rb1.position * cparams.local_anchor1 - rb1.world_com, + rb2.position * cparams.local_anchor2 - rb2.world_com, + ) + }; + + let im2 = rb2.mass_properties.inv_mass; + let vel1 = rb1.linvel + rb1.angvel.gcross(anchor1); + let vel2 = rb2.linvel + rb2.angvel.gcross(anchor2); + let rhs = vel2 - vel1; + + let cmat2 = anchor2.gcross_matrix(); + let gcross2 = rb2.world_inv_inertia_sqrt.transform_lin_vector(anchor2); + + let lhs; + + #[cfg(feature = "dim3")] + { + lhs = rb2 + .world_inv_inertia_sqrt + .squared() + .quadform(&cmat2) + .add_diagonal(im2); + } + + #[cfg(feature = "dim2")] + { + let ii2 = rb2.world_inv_inertia_sqrt.squared(); + let m11 = im2 + cmat2.x * cmat2.x * ii2; + let m12 = cmat2.x * cmat2.y * ii2; + let m22 = im2 + cmat2.y * cmat2.y * ii2; + lhs = SdpMatrix::new(m11, m12, m22) + } + + let inv_lhs = lhs.inverse_unchecked(); + + BallVelocityGroundConstraint { + joint_id, + mj_lambda2: rb2.active_set_offset, + im2, + impulse: cparams.impulse * params.warmstart_coeff, + gcross2, + rhs, + inv_lhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + mj_lambda2.linear -= self.im2 * self.impulse; + mj_lambda2.angular -= self.gcross2.gcross(self.impulse); + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2); + let dvel = vel2 + self.rhs; + + let impulse = self.inv_lhs * dvel; + self.impulse += impulse; + + mj_lambda2.linear -= self.im2 * impulse; + mj_lambda2.angular -= self.gcross2.gcross(impulse); + + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + // FIXME: duplicated code with the non-ground constraint. + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + let joint = &mut joints_all[self.joint_id].weight; + if let JointParams::BallJoint(ball) = &mut joint.params { + ball.impulse = self.impulse + } + } +} diff --git a/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs new file mode 100644 index 000000000..b96f3b87d --- /dev/null +++ b/src/dynamics/solver/joint_constraint/ball_velocity_constraint_wide.rs @@ -0,0 +1,329 @@ +use crate::dynamics::solver::DeltaVel; +use crate::dynamics::{ + BallJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, +}; +use crate::math::{ + AngVector, AngularInertia, Isometry, Point, SdpMatrix, SimdFloat, Vector, SIMD_WIDTH, +}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; +use simba::simd::SimdValue; + +#[derive(Debug)] +pub(crate) struct WBallVelocityConstraint { + mj_lambda1: [usize; SIMD_WIDTH], + mj_lambda2: [usize; SIMD_WIDTH], + + joint_id: [JointIndex; SIMD_WIDTH], + + rhs: Vector, + pub(crate) impulse: Vector, + + gcross1: Vector, + gcross2: Vector, + + inv_lhs: SdpMatrix, + + im1: SimdFloat, + im2: SimdFloat, +} + +impl WBallVelocityConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: [JointIndex; SIMD_WIDTH], + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&BallJoint; SIMD_WIDTH], + ) -> Self { + let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); + let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); + let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii1_sqrt = AngularInertia::::from( + array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ); + let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; + + let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); + let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); + let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( + array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ); + let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + + let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); + let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); + let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + + let anchor1 = position1 * local_anchor1 - world_com1; + let anchor2 = position2 * local_anchor2 - world_com2; + + let vel1: Vector = linvel1 + angvel1.gcross(anchor1); + let vel2: Vector = linvel2 + angvel2.gcross(anchor2); + let rhs = -(vel1 - vel2); + let lhs; + + let cmat1 = anchor1.gcross_matrix(); + let cmat2 = anchor2.gcross_matrix(); + + #[cfg(feature = "dim3")] + { + lhs = ii2_sqrt.squared().quadform(&cmat2).add_diagonal(im2) + + ii1_sqrt.squared().quadform(&cmat1).add_diagonal(im1); + } + + // In 2D we just unroll the computation because + // it's just easier that way. + #[cfg(feature = "dim2")] + { + let ii1 = ii1_sqrt.squared(); + let ii2 = ii2_sqrt.squared(); + let m11 = im1 + im2 + cmat1.x * cmat1.x * ii1 + cmat2.x * cmat2.x * ii2; + let m12 = cmat1.x * cmat1.y * ii1 + cmat2.x * cmat2.y * ii2; + let m22 = im1 + im2 + cmat1.y * cmat1.y * ii1 + cmat2.y * cmat2.y * ii2; + lhs = SdpMatrix::new(m11, m12, m22) + } + + let gcross1 = ii1_sqrt.transform_lin_vector(anchor1); + let gcross2 = ii2_sqrt.transform_lin_vector(anchor2); + + let inv_lhs = lhs.inverse_unchecked(); + + WBallVelocityConstraint { + joint_id, + mj_lambda1, + mj_lambda2, + im1, + im2, + impulse: impulse * SimdFloat::splat(params.warmstart_coeff), + gcross1, + gcross2, + rhs, + inv_lhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], + ), + }; + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + mj_lambda1.linear += self.impulse * self.im1; + mj_lambda1.angular += self.gcross1.gcross(self.impulse); + mj_lambda2.linear -= self.impulse * self.im2; + mj_lambda2.angular -= self.gcross2.gcross(self.impulse); + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); + mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii); + } + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1: DeltaVel = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], + ), + }; + let mut mj_lambda2: DeltaVel = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let vel1 = mj_lambda1.linear + mj_lambda1.angular.gcross(self.gcross1); + let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2); + let dvel = -vel1 + vel2 + self.rhs; + + let impulse = self.inv_lhs * dvel; + self.impulse += impulse; + + mj_lambda1.linear += impulse * self.im1; + mj_lambda1.angular += self.gcross1.gcross(impulse); + + mj_lambda2.linear -= impulse * self.im2; + mj_lambda2.angular -= self.gcross2.gcross(impulse); + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); + mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii); + } + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + for ii in 0..SIMD_WIDTH { + let joint = &mut joints_all[self.joint_id[ii]].weight; + if let JointParams::BallJoint(ball) = &mut joint.params { + ball.impulse = self.impulse.extract(ii) + } + } + } +} + +#[derive(Debug)] +pub(crate) struct WBallVelocityGroundConstraint { + mj_lambda2: [usize; SIMD_WIDTH], + joint_id: [JointIndex; SIMD_WIDTH], + rhs: Vector, + pub(crate) impulse: Vector, + gcross2: Vector, + inv_lhs: SdpMatrix, + im2: SimdFloat, +} + +impl WBallVelocityGroundConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: [JointIndex; SIMD_WIDTH], + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&BallJoint; SIMD_WIDTH], + flipped: [bool; SIMD_WIDTH], + ) -> Self { + let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); + let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); + let local_anchor1 = Point::from( + array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH], + ); + + let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); + let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); + let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( + array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ); + let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + + let local_anchor2 = Point::from( + array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH], + ); + let impulse = Vector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + + let anchor1 = position1 * local_anchor1 - world_com1; + let anchor2 = position2 * local_anchor2 - world_com2; + + let vel1: Vector = linvel1 + angvel1.gcross(anchor1); + let vel2: Vector = linvel2 + angvel2.gcross(anchor2); + let rhs = vel2 - vel1; + let lhs; + + let cmat2 = anchor2.gcross_matrix(); + let gcross2 = ii2_sqrt.transform_lin_vector(anchor2); + + #[cfg(feature = "dim3")] + { + lhs = ii2_sqrt.squared().quadform(&cmat2).add_diagonal(im2); + } + + // In 2D we just unroll the computation because + // it's just easier that way. + #[cfg(feature = "dim2")] + { + let ii2 = ii2_sqrt.squared(); + let m11 = im2 + cmat2.x * cmat2.x * ii2; + let m12 = cmat2.x * cmat2.y * ii2; + let m22 = im2 + cmat2.y * cmat2.y * ii2; + lhs = SdpMatrix::new(m11, m12, m22) + } + + let inv_lhs = lhs.inverse_unchecked(); + + WBallVelocityGroundConstraint { + joint_id, + mj_lambda2, + im2, + impulse: impulse * SimdFloat::splat(params.warmstart_coeff), + gcross2, + rhs, + inv_lhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + mj_lambda2.linear -= self.impulse * self.im2; + mj_lambda2.angular -= self.gcross2.gcross(self.impulse); + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2: DeltaVel = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let vel2 = mj_lambda2.linear + mj_lambda2.angular.gcross(self.gcross2); + let dvel = vel2 + self.rhs; + + let impulse = self.inv_lhs * dvel; + self.impulse += impulse; + + mj_lambda2.linear -= impulse * self.im2; + mj_lambda2.angular -= self.gcross2.gcross(impulse); + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + for ii in 0..SIMD_WIDTH { + let joint = &mut joints_all[self.joint_id[ii]].weight; + if let JointParams::BallJoint(ball) = &mut joint.params { + ball.impulse = self.impulse.extract(ii) + } + } + } +} diff --git a/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs new file mode 100644 index 000000000..24001cdb2 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/fixed_position_constraint.rs @@ -0,0 +1,139 @@ +use crate::dynamics::{FixedJoint, IntegrationParameters, RigidBody}; +use crate::math::{AngularInertia, Isometry, Point, Rotation}; +use crate::utils::WAngularInertia; + +#[derive(Debug)] +pub(crate) struct FixedPositionConstraint { + position1: usize, + position2: usize, + local_anchor1: Isometry, + local_anchor2: Isometry, + local_com1: Point, + local_com2: Point, + im1: f32, + im2: f32, + ii1: AngularInertia, + ii2: AngularInertia, + + lin_inv_lhs: f32, + ang_inv_lhs: AngularInertia, +} + +impl FixedPositionConstraint { + pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &FixedJoint) -> Self { + let ii1 = rb1.world_inv_inertia_sqrt.squared(); + let ii2 = rb2.world_inv_inertia_sqrt.squared(); + let im1 = rb1.mass_properties.inv_mass; + let im2 = rb2.mass_properties.inv_mass; + let lin_inv_lhs = 1.0 / (im1 + im2); + let ang_inv_lhs = (ii1 + ii2).inverse(); + + Self { + local_anchor1: cparams.local_anchor1, + local_anchor2: cparams.local_anchor2, + position1: rb1.active_set_offset, + position2: rb2.active_set_offset, + im1, + im2, + ii1, + ii2, + local_com1: rb1.mass_properties.local_com, + local_com2: rb2.mass_properties.local_com, + lin_inv_lhs, + ang_inv_lhs, + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + let mut position1 = positions[self.position1 as usize]; + let mut position2 = positions[self.position2 as usize]; + + // Angular correction. + let anchor1 = position1 * self.local_anchor1; + let anchor2 = position2 * self.local_anchor2; + let ang_err = anchor2.rotation * anchor1.rotation.inverse(); + #[cfg(feature = "dim3")] + let ang_impulse = self + .ang_inv_lhs + .transform_vector(ang_err.scaled_axis() * params.joint_erp); + #[cfg(feature = "dim2")] + let ang_impulse = self + .ang_inv_lhs + .transform_vector(ang_err.angle() * params.joint_erp); + position1.rotation = + Rotation::new(self.ii1.transform_vector(ang_impulse)) * position1.rotation; + position2.rotation = + Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation; + + // Linear correction. + let anchor1 = position1 * Point::from(self.local_anchor1.translation.vector); + let anchor2 = position2 * Point::from(self.local_anchor2.translation.vector); + let err = anchor2 - anchor1; + let impulse = err * (self.lin_inv_lhs * params.joint_erp); + position1.translation.vector += self.im1 * impulse; + position2.translation.vector -= self.im2 * impulse; + + positions[self.position1 as usize] = position1; + positions[self.position2 as usize] = position2; + } +} + +#[derive(Debug)] +pub(crate) struct FixedPositionGroundConstraint { + position2: usize, + anchor1: Isometry, + local_anchor2: Isometry, + local_com2: Point, + im2: f32, + ii2: AngularInertia, + impulse: f32, +} + +impl FixedPositionGroundConstraint { + pub fn from_params( + rb1: &RigidBody, + rb2: &RigidBody, + cparams: &FixedJoint, + flipped: bool, + ) -> Self { + let anchor1; + let local_anchor2; + + if flipped { + anchor1 = rb1.predicted_position * cparams.local_anchor2; + local_anchor2 = cparams.local_anchor1; + } else { + anchor1 = rb1.predicted_position * cparams.local_anchor1; + local_anchor2 = cparams.local_anchor2; + }; + + Self { + anchor1, + local_anchor2, + position2: rb2.active_set_offset, + im2: rb2.mass_properties.inv_mass, + ii2: rb2.world_inv_inertia_sqrt.squared(), + local_com2: rb2.mass_properties.local_com, + impulse: 0.0, + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + let mut position2 = positions[self.position2 as usize]; + + // Angular correction. + let anchor2 = position2 * self.local_anchor2; + let ang_err = anchor2.rotation * self.anchor1.rotation.inverse(); + position2.rotation = ang_err.powf(-params.joint_erp) * position2.rotation; + + // Linear correction. + let anchor1 = Point::from(self.anchor1.translation.vector); + let anchor2 = position2 * Point::from(self.local_anchor2.translation.vector); + let err = anchor2 - anchor1; + // NOTE: no need to divide by im2 just to multiply right after. + let impulse = err * params.joint_erp; + position2.translation.vector -= impulse; + + positions[self.position2 as usize] = position2; + } +} diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs new file mode 100644 index 000000000..e4187c8d0 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint.rs @@ -0,0 +1,370 @@ +use crate::dynamics::solver::DeltaVel; +use crate::dynamics::{ + FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, +}; +use crate::math::{AngularInertia, Dim, SpacialVector, Vector}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; +#[cfg(feature = "dim2")] +use na::{Matrix3, Vector3}; +#[cfg(feature = "dim3")] +use na::{Matrix6, Vector6, U3}; + +#[derive(Debug)] +pub(crate) struct FixedVelocityConstraint { + mj_lambda1: usize, + mj_lambda2: usize, + + joint_id: JointIndex, + + impulse: SpacialVector, + + #[cfg(feature = "dim3")] + inv_lhs: Matrix6, // FIXME: replace by Cholesky. + #[cfg(feature = "dim3")] + rhs: Vector6, + + #[cfg(feature = "dim2")] + inv_lhs: Matrix3, // FIXME: replace by Cholesky. + #[cfg(feature = "dim2")] + rhs: Vector3, + + im1: f32, + im2: f32, + + ii1: AngularInertia, + ii2: AngularInertia, + + ii1_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, + + r1: Vector, + r2: Vector, +} + +impl FixedVelocityConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: JointIndex, + rb1: &RigidBody, + rb2: &RigidBody, + cparams: &FixedJoint, + ) -> Self { + let anchor1 = rb1.position * cparams.local_anchor1; + let anchor2 = rb2.position * cparams.local_anchor2; + let im1 = rb1.mass_properties.inv_mass; + let im2 = rb2.mass_properties.inv_mass; + let ii1 = rb1.world_inv_inertia_sqrt.squared(); + let ii2 = rb2.world_inv_inertia_sqrt.squared(); + let r1 = anchor1.translation.vector - rb1.world_com.coords; + let r2 = anchor2.translation.vector - rb2.world_com.coords; + let rmat1 = r1.gcross_matrix(); + let rmat2 = r2.gcross_matrix(); + + #[allow(unused_mut)] // For 2D + let mut lhs; + + #[cfg(feature = "dim3")] + { + let lhs00 = + ii1.quadform(&rmat1).add_diagonal(im1) + ii2.quadform(&rmat2).add_diagonal(im2); + let lhs10 = ii1.transform_matrix(&rmat1) + ii2.transform_matrix(&rmat2); + let lhs11 = (ii1 + ii2).into_matrix(); + + // Note that Cholesky only reads the lower-triangular part of the matrix + // so we don't need to fill lhs01. + lhs = Matrix6::zeros(); + lhs.fixed_slice_mut::(0, 0) + .copy_from(&lhs00.into_matrix()); + lhs.fixed_slice_mut::(3, 0).copy_from(&lhs10); + lhs.fixed_slice_mut::(3, 3).copy_from(&lhs11); + } + + // In 2D we just unroll the computation because + // it's just easier that way. + #[cfg(feature = "dim2")] + { + let m11 = im1 + im2 + rmat1.x * rmat1.x * ii1 + rmat2.x * rmat2.x * ii2; + let m12 = rmat1.x * rmat1.y * ii1 + rmat2.x * rmat2.y * ii2; + let m22 = im1 + im2 + rmat1.y * rmat1.y * ii1 + rmat2.y * rmat2.y * ii2; + let m13 = rmat1.x * ii1 + rmat2.x * ii2; + let m23 = rmat1.y * ii1 + rmat2.y * ii2; + let m33 = ii1 + ii2; + lhs = Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33) + } + + // NOTE: we don't use cholesky in 2D because we only have a 3x3 matrix + // for which a textbook inverse is still efficient. + #[cfg(feature = "dim2")] + let inv_lhs = lhs.try_inverse().expect("Singular system."); + #[cfg(feature = "dim3")] + let inv_lhs = lhs.cholesky().expect("Singular system.").inverse(); + + let lin_dvel = -rb1.linvel - rb1.angvel.gcross(r1) + rb2.linvel + rb2.angvel.gcross(r2); + let ang_dvel = -rb1.angvel + rb2.angvel; + + #[cfg(feature = "dim2")] + let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); + + #[cfg(feature = "dim3")] + let rhs = Vector6::new( + lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, + ); + + FixedVelocityConstraint { + joint_id, + mj_lambda1: rb1.active_set_offset, + mj_lambda2: rb2.active_set_offset, + im1, + im2, + ii1, + ii2, + ii1_sqrt: rb1.world_inv_inertia_sqrt, + ii2_sqrt: rb2.world_inv_inertia_sqrt, + impulse: cparams.impulse * params.warmstart_coeff, + inv_lhs, + r1, + r2, + rhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = self.impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = self.impulse.fixed_rows::(3).into_owned(); + + mj_lambda1.linear += self.im1 * lin_impulse; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + + mj_lambda2.linear -= self.im2 * lin_impulse; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let dlinvel = -mj_lambda1.linear - ang_vel1.gcross(self.r1) + + mj_lambda2.linear + + ang_vel2.gcross(self.r2); + let dangvel = -ang_vel1 + ang_vel2; + + #[cfg(feature = "dim2")] + let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs; + #[cfg(feature = "dim3")] + let rhs = Vector6::new( + dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, + ) + self.rhs; + + let impulse = self.inv_lhs * rhs; + self.impulse += impulse; + let lin_impulse = impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = impulse.fixed_rows::(3).into_owned(); + + mj_lambda1.linear += self.im1 * lin_impulse; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + + mj_lambda2.linear -= self.im2 * lin_impulse; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + let joint = &mut joints_all[self.joint_id].weight; + if let JointParams::FixedJoint(fixed) = &mut joint.params { + fixed.impulse = self.impulse; + } + } +} + +#[derive(Debug)] +pub(crate) struct FixedVelocityGroundConstraint { + mj_lambda2: usize, + + joint_id: JointIndex, + + impulse: SpacialVector, + + #[cfg(feature = "dim3")] + inv_lhs: Matrix6, // FIXME: replace by Cholesky. + #[cfg(feature = "dim3")] + rhs: Vector6, + + #[cfg(feature = "dim2")] + inv_lhs: Matrix3, // FIXME: replace by Cholesky. + #[cfg(feature = "dim2")] + rhs: Vector3, + + im2: f32, + ii2: AngularInertia, + ii2_sqrt: AngularInertia, + r2: Vector, +} + +impl FixedVelocityGroundConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: JointIndex, + rb1: &RigidBody, + rb2: &RigidBody, + cparams: &FixedJoint, + flipped: bool, + ) -> Self { + let (anchor1, anchor2) = if flipped { + ( + rb1.position * cparams.local_anchor2, + rb2.position * cparams.local_anchor1, + ) + } else { + ( + rb1.position * cparams.local_anchor1, + rb2.position * cparams.local_anchor2, + ) + }; + + let r1 = anchor1.translation.vector - rb1.world_com.coords; + + let im2 = rb2.mass_properties.inv_mass; + let ii2 = rb2.world_inv_inertia_sqrt.squared(); + let r2 = anchor2.translation.vector - rb2.world_com.coords; + let rmat2 = r2.gcross_matrix(); + + #[allow(unused_mut)] // For 2D. + let mut lhs; + + #[cfg(feature = "dim3")] + { + let lhs00 = ii2.quadform(&rmat2).add_diagonal(im2); + let lhs10 = ii2.transform_matrix(&rmat2); + let lhs11 = ii2.into_matrix(); + + // Note that Cholesky only reads the lower-triangular part of the matrix + // so we don't need to fill lhs01. + lhs = Matrix6::zeros(); + lhs.fixed_slice_mut::(0, 0) + .copy_from(&lhs00.into_matrix()); + lhs.fixed_slice_mut::(3, 0).copy_from(&lhs10); + lhs.fixed_slice_mut::(3, 3).copy_from(&lhs11); + } + + // In 2D we just unroll the computation because + // it's just easier that way. + #[cfg(feature = "dim2")] + { + let m11 = im2 + rmat2.x * rmat2.x * ii2; + let m12 = rmat2.x * rmat2.y * ii2; + let m22 = im2 + rmat2.y * rmat2.y * ii2; + let m13 = rmat2.x * ii2; + let m23 = rmat2.y * ii2; + let m33 = ii2; + lhs = Matrix3::new(m11, m12, m13, m12, m22, m23, m13, m23, m33) + } + + #[cfg(feature = "dim2")] + let inv_lhs = lhs.try_inverse().expect("Singular system."); + #[cfg(feature = "dim3")] + let inv_lhs = lhs.cholesky().expect("Singular system.").inverse(); + + let lin_dvel = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1); + let ang_dvel = rb2.angvel - rb1.angvel; + + #[cfg(feature = "dim2")] + let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); + #[cfg(feature = "dim3")] + let rhs = Vector6::new( + lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, + ); + + FixedVelocityGroundConstraint { + joint_id, + mj_lambda2: rb2.active_set_offset, + im2, + ii2, + ii2_sqrt: rb2.world_inv_inertia_sqrt, + impulse: cparams.impulse * params.warmstart_coeff, + inv_lhs, + r2, + rhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = self.impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = self.impulse.fixed_rows::(3).into_owned(); + + mj_lambda2.linear -= self.im2 * lin_impulse; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let dlinvel = mj_lambda2.linear + ang_vel2.gcross(self.r2); + let dangvel = ang_vel2; + #[cfg(feature = "dim2")] + let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs; + #[cfg(feature = "dim3")] + let rhs = Vector6::new( + dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, + ) + self.rhs; + + let impulse = self.inv_lhs * rhs; + + self.impulse += impulse; + let lin_impulse = impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = impulse.fixed_rows::(3).into_owned(); + + mj_lambda2.linear -= self.im2 * lin_impulse; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + // FIXME: duplicated code with the non-ground constraint. + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + let joint = &mut joints_all[self.joint_id].weight; + if let JointParams::FixedJoint(fixed) = &mut joint.params { + fixed.impulse = self.impulse; + } + } +} diff --git a/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs new file mode 100644 index 000000000..7c87b2c11 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/fixed_velocity_constraint_wide.rs @@ -0,0 +1,472 @@ +use simba::simd::SimdValue; + +use crate::dynamics::solver::DeltaVel; +use crate::dynamics::{ + FixedJoint, IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RigidBody, +}; +use crate::math::{ + AngVector, AngularInertia, CrossMatrix, Dim, Isometry, Point, SimdFloat, SpacialVector, Vector, + SIMD_WIDTH, +}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; +#[cfg(feature = "dim3")] +use na::{Cholesky, Matrix6, Vector6, U3}; +#[cfg(feature = "dim2")] +use { + crate::utils::SdpMatrix3, + na::{Matrix3, Vector3}, +}; + +#[derive(Debug)] +pub(crate) struct WFixedVelocityConstraint { + mj_lambda1: [usize; SIMD_WIDTH], + mj_lambda2: [usize; SIMD_WIDTH], + + joint_id: [JointIndex; SIMD_WIDTH], + + impulse: SpacialVector, + + #[cfg(feature = "dim3")] + inv_lhs: Matrix6, // FIXME: replace by Cholesky. + #[cfg(feature = "dim3")] + rhs: Vector6, + + #[cfg(feature = "dim2")] + inv_lhs: Matrix3, + #[cfg(feature = "dim2")] + rhs: Vector3, + + im1: SimdFloat, + im2: SimdFloat, + + ii1: AngularInertia, + ii2: AngularInertia, + + ii1_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, + + r1: Vector, + r2: Vector, +} + +impl WFixedVelocityConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: [JointIndex; SIMD_WIDTH], + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&FixedJoint; SIMD_WIDTH], + ) -> Self { + let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); + let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); + let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii1_sqrt = AngularInertia::::from( + array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ); + let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; + + let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); + let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); + let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( + array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ); + let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + + let local_anchor1 = Isometry::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); + let local_anchor2 = Isometry::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); + let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + + let anchor1 = position1 * local_anchor1; + let anchor2 = position2 * local_anchor2; + let ii1 = ii1_sqrt.squared(); + let ii2 = ii2_sqrt.squared(); + let r1 = anchor1.translation.vector - world_com1.coords; + let r2 = anchor2.translation.vector - world_com2.coords; + let rmat1: CrossMatrix<_> = r1.gcross_matrix(); + let rmat2: CrossMatrix<_> = r2.gcross_matrix(); + + #[allow(unused_mut)] // For 2D. + let mut lhs; + + #[cfg(feature = "dim3")] + { + let lhs00 = + ii1.quadform(&rmat1).add_diagonal(im1) + ii2.quadform(&rmat2).add_diagonal(im2); + let lhs10 = ii1.transform_matrix(&rmat1) + ii2.transform_matrix(&rmat2); + let lhs11 = (ii1 + ii2).into_matrix(); + + // Note that Cholesky only reads the lower-triangular part of the matrix + // so we don't need to fill lhs01. + lhs = Matrix6::zeros(); + lhs.fixed_slice_mut::(0, 0) + .copy_from(&lhs00.into_matrix()); + lhs.fixed_slice_mut::(3, 0).copy_from(&lhs10); + lhs.fixed_slice_mut::(3, 3).copy_from(&lhs11); + } + + // In 2D we just unroll the computation because + // it's just easier that way. + #[cfg(feature = "dim2")] + { + let m11 = im1 + im2 + rmat1.x * rmat1.x * ii1 + rmat2.x * rmat2.x * ii2; + let m12 = rmat1.x * rmat1.y * ii1 + rmat2.x * rmat2.y * ii2; + let m22 = im1 + im2 + rmat1.y * rmat1.y * ii1 + rmat2.y * rmat2.y * ii2; + let m13 = rmat1.x * ii1 + rmat2.x * ii2; + let m23 = rmat1.y * ii1 + rmat2.y * ii2; + let m33 = ii1 + ii2; + lhs = SdpMatrix3::new(m11, m12, m13, m22, m23, m33) + } + + // NOTE: we don't use cholesky in 2D because we only have a 3x3 matrix + // for which a textbook inverse is still efficient. + #[cfg(feature = "dim2")] + let inv_lhs = lhs.inverse_unchecked().into_matrix(); // FIXME: don't extract the matrix? + #[cfg(feature = "dim3")] + let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); + + let lin_dvel = -linvel1 - angvel1.gcross(r1) + linvel2 + angvel2.gcross(r2); + let ang_dvel = -angvel1 + angvel2; + + #[cfg(feature = "dim2")] + let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); + + #[cfg(feature = "dim3")] + let rhs = Vector6::new( + lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, + ); + + WFixedVelocityConstraint { + joint_id, + mj_lambda1, + mj_lambda2, + im1, + im2, + ii1, + ii2, + ii1_sqrt, + ii2_sqrt, + impulse: impulse * SimdFloat::splat(params.warmstart_coeff), + inv_lhs, + r1, + r2, + rhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], + ), + }; + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = self.impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = self.impulse.fixed_rows::(3).into_owned(); + + mj_lambda1.linear += lin_impulse * self.im1; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + + mj_lambda2.linear -= lin_impulse * self.im2; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); + mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii); + } + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1: DeltaVel = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], + ), + }; + let mut mj_lambda2: DeltaVel = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let dlinvel = -mj_lambda1.linear - ang_vel1.gcross(self.r1) + + mj_lambda2.linear + + ang_vel2.gcross(self.r2); + let dangvel = -ang_vel1 + ang_vel2; + + #[cfg(feature = "dim2")] + let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs; + #[cfg(feature = "dim3")] + let rhs = Vector6::new( + dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, + ) + self.rhs; + + let impulse = self.inv_lhs * rhs; + self.impulse += impulse; + let lin_impulse = impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = impulse.fixed_rows::(3).into_owned(); + + mj_lambda1.linear += lin_impulse * self.im1; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + + mj_lambda2.linear -= lin_impulse * self.im2; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); + mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii); + } + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + for ii in 0..SIMD_WIDTH { + let joint = &mut joints_all[self.joint_id[ii]].weight; + if let JointParams::FixedJoint(fixed) = &mut joint.params { + fixed.impulse = self.impulse.extract(ii) + } + } + } +} + +#[derive(Debug)] +pub(crate) struct WFixedVelocityGroundConstraint { + mj_lambda2: [usize; SIMD_WIDTH], + + joint_id: [JointIndex; SIMD_WIDTH], + + impulse: SpacialVector, + + #[cfg(feature = "dim3")] + inv_lhs: Matrix6, // FIXME: replace by Cholesky. + #[cfg(feature = "dim3")] + rhs: Vector6, + + #[cfg(feature = "dim2")] + inv_lhs: Matrix3, + #[cfg(feature = "dim2")] + rhs: Vector3, + + im2: SimdFloat, + ii2: AngularInertia, + ii2_sqrt: AngularInertia, + r2: Vector, +} + +impl WFixedVelocityGroundConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: [JointIndex; SIMD_WIDTH], + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&FixedJoint; SIMD_WIDTH], + flipped: [bool; SIMD_WIDTH], + ) -> Self { + let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); + let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); + + let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); + let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); + let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( + array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ); + let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + + let local_anchor1 = Isometry::from( + array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH], + ); + let local_anchor2 = Isometry::from( + array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH], + ); + let impulse = SpacialVector::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + + let anchor1 = position1 * local_anchor1; + let anchor2 = position2 * local_anchor2; + let ii2 = ii2_sqrt.squared(); + let r1 = anchor1.translation.vector - world_com1.coords; + let r2 = anchor2.translation.vector - world_com2.coords; + let rmat2: CrossMatrix<_> = r2.gcross_matrix(); + + #[allow(unused_mut)] // For 2D. + let mut lhs; + + #[cfg(feature = "dim3")] + { + let lhs00 = ii2.quadform(&rmat2).add_diagonal(im2); + let lhs10 = ii2.transform_matrix(&rmat2); + let lhs11 = ii2.into_matrix(); + + lhs = Matrix6::zeros(); + lhs.fixed_slice_mut::(0, 0) + .copy_from(&lhs00.into_matrix()); + lhs.fixed_slice_mut::(3, 0).copy_from(&lhs10); + lhs.fixed_slice_mut::(3, 3).copy_from(&lhs11); + } + + // In 2D we just unroll the computation because + // it's just easier that way. + #[cfg(feature = "dim2")] + { + let m11 = im2 + rmat2.x * rmat2.x * ii2; + let m12 = rmat2.x * rmat2.y * ii2; + let m22 = im2 + rmat2.y * rmat2.y * ii2; + let m13 = rmat2.x * ii2; + let m23 = rmat2.y * ii2; + let m33 = ii2; + lhs = SdpMatrix3::new(m11, m12, m13, m22, m23, m33) + } + + #[cfg(feature = "dim2")] + let inv_lhs = lhs.inverse_unchecked().into_matrix(); // FIXME: don't do into_matrix? + #[cfg(feature = "dim3")] + let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); + + let lin_dvel = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1); + let ang_dvel = angvel2 - angvel1; + + #[cfg(feature = "dim2")] + let rhs = Vector3::new(lin_dvel.x, lin_dvel.y, ang_dvel); + #[cfg(feature = "dim3")] + let rhs = Vector6::new( + lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y, ang_dvel.z, + ); + + WFixedVelocityGroundConstraint { + joint_id, + mj_lambda2, + im2, + ii2, + ii2_sqrt, + impulse: impulse * SimdFloat::splat(params.warmstart_coeff), + inv_lhs, + r2, + rhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = self.impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = self.impulse.fixed_rows::(3).into_owned(); + + mj_lambda2.linear -= lin_impulse * self.im2; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2: DeltaVel = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + let dlinvel = mj_lambda2.linear + ang_vel2.gcross(self.r2); + let dangvel = ang_vel2; + #[cfg(feature = "dim2")] + let rhs = Vector3::new(dlinvel.x, dlinvel.y, dangvel) + self.rhs; + #[cfg(feature = "dim3")] + let rhs = Vector6::new( + dlinvel.x, dlinvel.y, dlinvel.z, dangvel.x, dangvel.y, dangvel.z, + ) + self.rhs; + + let impulse = self.inv_lhs * rhs; + + self.impulse += impulse; + let lin_impulse = impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = impulse[2]; + #[cfg(feature = "dim3")] + let ang_impulse = impulse.fixed_rows::(3).into_owned(); + + mj_lambda2.linear -= lin_impulse * self.im2; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + // FIXME: duplicated code with the non-ground constraint. + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + for ii in 0..SIMD_WIDTH { + let joint = &mut joints_all[self.joint_id[ii]].weight; + if let JointParams::FixedJoint(fixed) = &mut joint.params { + fixed.impulse = self.impulse.extract(ii) + } + } + } +} diff --git a/src/dynamics/solver/joint_constraint/joint_constraint.rs b/src/dynamics/solver/joint_constraint/joint_constraint.rs new file mode 100644 index 000000000..8b1f8bfec --- /dev/null +++ b/src/dynamics/solver/joint_constraint/joint_constraint.rs @@ -0,0 +1,340 @@ +use super::{ + BallVelocityConstraint, BallVelocityGroundConstraint, FixedVelocityConstraint, + FixedVelocityGroundConstraint, PrismaticVelocityConstraint, PrismaticVelocityGroundConstraint, +}; +#[cfg(feature = "dim3")] +use super::{RevoluteVelocityConstraint, RevoluteVelocityGroundConstraint}; +#[cfg(feature = "simd-is-enabled")] +use super::{ + WBallVelocityConstraint, WBallVelocityGroundConstraint, WFixedVelocityConstraint, + WFixedVelocityGroundConstraint, WPrismaticVelocityConstraint, + WPrismaticVelocityGroundConstraint, +}; +#[cfg(feature = "dim3")] +#[cfg(feature = "simd-is-enabled")] +use super::{WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint}; +use crate::dynamics::solver::DeltaVel; +use crate::dynamics::{ + IntegrationParameters, Joint, JointGraphEdge, JointIndex, JointParams, RigidBodySet, +}; +#[cfg(feature = "simd-is-enabled")] +use crate::math::SIMD_WIDTH; + +pub(crate) enum AnyJointVelocityConstraint { + BallConstraint(BallVelocityConstraint), + BallGroundConstraint(BallVelocityGroundConstraint), + #[cfg(feature = "simd-is-enabled")] + WBallConstraint(WBallVelocityConstraint), + #[cfg(feature = "simd-is-enabled")] + WBallGroundConstraint(WBallVelocityGroundConstraint), + FixedConstraint(FixedVelocityConstraint), + FixedGroundConstraint(FixedVelocityGroundConstraint), + #[cfg(feature = "simd-is-enabled")] + WFixedConstraint(WFixedVelocityConstraint), + #[cfg(feature = "simd-is-enabled")] + WFixedGroundConstraint(WFixedVelocityGroundConstraint), + PrismaticConstraint(PrismaticVelocityConstraint), + PrismaticGroundConstraint(PrismaticVelocityGroundConstraint), + #[cfg(feature = "simd-is-enabled")] + WPrismaticConstraint(WPrismaticVelocityConstraint), + #[cfg(feature = "simd-is-enabled")] + WPrismaticGroundConstraint(WPrismaticVelocityGroundConstraint), + #[cfg(feature = "dim3")] + RevoluteConstraint(RevoluteVelocityConstraint), + #[cfg(feature = "dim3")] + RevoluteGroundConstraint(RevoluteVelocityGroundConstraint), + #[cfg(feature = "dim3")] + #[cfg(feature = "simd-is-enabled")] + WRevoluteConstraint(WRevoluteVelocityConstraint), + #[cfg(feature = "dim3")] + #[cfg(feature = "simd-is-enabled")] + WRevoluteGroundConstraint(WRevoluteVelocityGroundConstraint), + #[allow(dead_code)] // The Empty variant is only used with parallel code. + Empty, +} + +impl AnyJointVelocityConstraint { + #[cfg(feature = "parallel")] + pub fn num_active_constraints(_: &Joint) -> usize { + 1 + } + + pub fn from_joint( + params: &IntegrationParameters, + joint_id: JointIndex, + joint: &Joint, + bodies: &RigidBodySet, + ) -> Self { + let rb1 = &bodies[joint.body1]; + let rb2 = &bodies[joint.body2]; + + match &joint.params { + JointParams::BallJoint(p) => AnyJointVelocityConstraint::BallConstraint( + BallVelocityConstraint::from_params(params, joint_id, rb1, rb2, p), + ), + JointParams::FixedJoint(p) => AnyJointVelocityConstraint::FixedConstraint( + FixedVelocityConstraint::from_params(params, joint_id, rb1, rb2, p), + ), + JointParams::PrismaticJoint(p) => AnyJointVelocityConstraint::PrismaticConstraint( + PrismaticVelocityConstraint::from_params(params, joint_id, rb1, rb2, p), + ), + #[cfg(feature = "dim3")] + JointParams::RevoluteJoint(p) => AnyJointVelocityConstraint::RevoluteConstraint( + RevoluteVelocityConstraint::from_params(params, joint_id, rb1, rb2, p), + ), + } + } + + #[cfg(feature = "simd-is-enabled")] + pub fn from_wide_joint( + params: &IntegrationParameters, + joint_id: [JointIndex; SIMD_WIDTH], + joints: [&Joint; SIMD_WIDTH], + bodies: &RigidBodySet, + ) -> Self { + let rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH]; + let rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH]; + + match &joints[0].params { + JointParams::BallJoint(_) => { + let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH]; + AnyJointVelocityConstraint::WBallConstraint(WBallVelocityConstraint::from_params( + params, joint_id, rbs1, rbs2, joints, + )) + } + JointParams::FixedJoint(_) => { + let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH]; + AnyJointVelocityConstraint::WFixedConstraint(WFixedVelocityConstraint::from_params( + params, joint_id, rbs1, rbs2, joints, + )) + } + JointParams::PrismaticJoint(_) => { + let joints = + array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH]; + AnyJointVelocityConstraint::WPrismaticConstraint( + WPrismaticVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints), + ) + } + #[cfg(feature = "dim3")] + JointParams::RevoluteJoint(_) => { + let joints = + array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH]; + AnyJointVelocityConstraint::WRevoluteConstraint( + WRevoluteVelocityConstraint::from_params(params, joint_id, rbs1, rbs2, joints), + ) + } + } + } + + pub fn from_joint_ground( + params: &IntegrationParameters, + joint_id: JointIndex, + joint: &Joint, + bodies: &RigidBodySet, + ) -> Self { + let mut rb1 = &bodies[joint.body1]; + let mut rb2 = &bodies[joint.body2]; + let flipped = !rb2.is_dynamic(); + + if flipped { + std::mem::swap(&mut rb1, &mut rb2); + } + + match &joint.params { + JointParams::BallJoint(p) => AnyJointVelocityConstraint::BallGroundConstraint( + BallVelocityGroundConstraint::from_params(params, joint_id, rb1, rb2, p, flipped), + ), + JointParams::FixedJoint(p) => AnyJointVelocityConstraint::FixedGroundConstraint( + FixedVelocityGroundConstraint::from_params(params, joint_id, rb1, rb2, p, flipped), + ), + JointParams::PrismaticJoint(p) => { + AnyJointVelocityConstraint::PrismaticGroundConstraint( + PrismaticVelocityGroundConstraint::from_params( + params, joint_id, rb1, rb2, p, flipped, + ), + ) + } + #[cfg(feature = "dim3")] + JointParams::RevoluteJoint(p) => AnyJointVelocityConstraint::RevoluteGroundConstraint( + RevoluteVelocityGroundConstraint::from_params( + params, joint_id, rb1, rb2, p, flipped, + ), + ), + } + } + + #[cfg(feature = "simd-is-enabled")] + pub fn from_wide_joint_ground( + params: &IntegrationParameters, + joint_id: [JointIndex; SIMD_WIDTH], + joints: [&Joint; SIMD_WIDTH], + bodies: &RigidBodySet, + ) -> Self { + let mut rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH]; + let mut rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH]; + let mut flipped = [false; SIMD_WIDTH]; + + for ii in 0..SIMD_WIDTH { + if !rbs2[ii].is_dynamic() { + std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]); + flipped[ii] = true; + } + } + + match &joints[0].params { + JointParams::BallJoint(_) => { + let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH]; + AnyJointVelocityConstraint::WBallGroundConstraint( + WBallVelocityGroundConstraint::from_params( + params, joint_id, rbs1, rbs2, joints, flipped, + ), + ) + } + JointParams::FixedJoint(_) => { + let joints = array![|ii| joints[ii].params.as_fixed_joint().unwrap(); SIMD_WIDTH]; + AnyJointVelocityConstraint::WFixedGroundConstraint( + WFixedVelocityGroundConstraint::from_params( + params, joint_id, rbs1, rbs2, joints, flipped, + ), + ) + } + JointParams::PrismaticJoint(_) => { + let joints = + array![|ii| joints[ii].params.as_prismatic_joint().unwrap(); SIMD_WIDTH]; + AnyJointVelocityConstraint::WPrismaticGroundConstraint( + WPrismaticVelocityGroundConstraint::from_params( + params, joint_id, rbs1, rbs2, joints, flipped, + ), + ) + } + #[cfg(feature = "dim3")] + JointParams::RevoluteJoint(_) => { + let joints = + array![|ii| joints[ii].params.as_revolute_joint().unwrap(); SIMD_WIDTH]; + AnyJointVelocityConstraint::WRevoluteGroundConstraint( + WRevoluteVelocityGroundConstraint::from_params( + params, joint_id, rbs1, rbs2, joints, flipped, + ), + ) + } + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + match self { + AnyJointVelocityConstraint::BallConstraint(c) => c.warmstart(mj_lambdas), + AnyJointVelocityConstraint::BallGroundConstraint(c) => c.warmstart(mj_lambdas), + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WBallConstraint(c) => c.warmstart(mj_lambdas), + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WBallGroundConstraint(c) => c.warmstart(mj_lambdas), + AnyJointVelocityConstraint::FixedConstraint(c) => c.warmstart(mj_lambdas), + AnyJointVelocityConstraint::FixedGroundConstraint(c) => c.warmstart(mj_lambdas), + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WFixedConstraint(c) => c.warmstart(mj_lambdas), + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WFixedGroundConstraint(c) => c.warmstart(mj_lambdas), + AnyJointVelocityConstraint::PrismaticConstraint(c) => c.warmstart(mj_lambdas), + AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.warmstart(mj_lambdas), + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WPrismaticConstraint(c) => c.warmstart(mj_lambdas), + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WPrismaticGroundConstraint(c) => c.warmstart(mj_lambdas), + #[cfg(feature = "dim3")] + AnyJointVelocityConstraint::RevoluteConstraint(c) => c.warmstart(mj_lambdas), + #[cfg(feature = "dim3")] + AnyJointVelocityConstraint::RevoluteGroundConstraint(c) => c.warmstart(mj_lambdas), + #[cfg(feature = "dim3")] + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WRevoluteConstraint(c) => c.warmstart(mj_lambdas), + #[cfg(feature = "dim3")] + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WRevoluteGroundConstraint(c) => c.warmstart(mj_lambdas), + AnyJointVelocityConstraint::Empty => unreachable!(), + } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + match self { + AnyJointVelocityConstraint::BallConstraint(c) => c.solve(mj_lambdas), + AnyJointVelocityConstraint::BallGroundConstraint(c) => c.solve(mj_lambdas), + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WBallConstraint(c) => c.solve(mj_lambdas), + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WBallGroundConstraint(c) => c.solve(mj_lambdas), + AnyJointVelocityConstraint::FixedConstraint(c) => c.solve(mj_lambdas), + AnyJointVelocityConstraint::FixedGroundConstraint(c) => c.solve(mj_lambdas), + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WFixedConstraint(c) => c.solve(mj_lambdas), + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WFixedGroundConstraint(c) => c.solve(mj_lambdas), + AnyJointVelocityConstraint::PrismaticConstraint(c) => c.solve(mj_lambdas), + AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => c.solve(mj_lambdas), + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WPrismaticConstraint(c) => c.solve(mj_lambdas), + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WPrismaticGroundConstraint(c) => c.solve(mj_lambdas), + #[cfg(feature = "dim3")] + AnyJointVelocityConstraint::RevoluteConstraint(c) => c.solve(mj_lambdas), + #[cfg(feature = "dim3")] + AnyJointVelocityConstraint::RevoluteGroundConstraint(c) => c.solve(mj_lambdas), + #[cfg(feature = "dim3")] + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WRevoluteConstraint(c) => c.solve(mj_lambdas), + #[cfg(feature = "dim3")] + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WRevoluteGroundConstraint(c) => c.solve(mj_lambdas), + AnyJointVelocityConstraint::Empty => unreachable!(), + } + } + + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + match self { + AnyJointVelocityConstraint::BallConstraint(c) => c.writeback_impulses(joints_all), + + AnyJointVelocityConstraint::BallGroundConstraint(c) => c.writeback_impulses(joints_all), + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WBallConstraint(c) => c.writeback_impulses(joints_all), + + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WBallGroundConstraint(c) => { + c.writeback_impulses(joints_all) + } + AnyJointVelocityConstraint::FixedConstraint(c) => c.writeback_impulses(joints_all), + AnyJointVelocityConstraint::FixedGroundConstraint(c) => { + c.writeback_impulses(joints_all) + } + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WFixedConstraint(c) => c.writeback_impulses(joints_all), + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WFixedGroundConstraint(c) => { + c.writeback_impulses(joints_all) + } + AnyJointVelocityConstraint::PrismaticConstraint(c) => c.writeback_impulses(joints_all), + AnyJointVelocityConstraint::PrismaticGroundConstraint(c) => { + c.writeback_impulses(joints_all) + } + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WPrismaticConstraint(c) => c.writeback_impulses(joints_all), + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WPrismaticGroundConstraint(c) => { + c.writeback_impulses(joints_all) + } + #[cfg(feature = "dim3")] + AnyJointVelocityConstraint::RevoluteConstraint(c) => c.writeback_impulses(joints_all), + #[cfg(feature = "dim3")] + AnyJointVelocityConstraint::RevoluteGroundConstraint(c) => { + c.writeback_impulses(joints_all) + } + #[cfg(feature = "dim3")] + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WRevoluteConstraint(c) => c.writeback_impulses(joints_all), + #[cfg(feature = "dim3")] + #[cfg(feature = "simd-is-enabled")] + AnyJointVelocityConstraint::WRevoluteGroundConstraint(c) => { + c.writeback_impulses(joints_all) + } + AnyJointVelocityConstraint::Empty => unreachable!(), + } + } +} diff --git a/src/dynamics/solver/joint_constraint/joint_position_constraint.rs b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs new file mode 100644 index 000000000..47736878e --- /dev/null +++ b/src/dynamics/solver/joint_constraint/joint_position_constraint.rs @@ -0,0 +1,169 @@ +use super::{ + BallPositionConstraint, BallPositionGroundConstraint, FixedPositionConstraint, + FixedPositionGroundConstraint, PrismaticPositionConstraint, PrismaticPositionGroundConstraint, +}; +#[cfg(feature = "dim3")] +use super::{RevolutePositionConstraint, RevolutePositionGroundConstraint}; +#[cfg(feature = "simd-is-enabled")] +use super::{WBallPositionConstraint, WBallPositionGroundConstraint}; +use crate::dynamics::{IntegrationParameters, Joint, JointParams, RigidBodySet}; +use crate::math::Isometry; +#[cfg(feature = "simd-is-enabled")] +use crate::math::SIMD_WIDTH; + +pub(crate) enum AnyJointPositionConstraint { + BallJoint(BallPositionConstraint), + BallGroundConstraint(BallPositionGroundConstraint), + #[cfg(feature = "simd-is-enabled")] + WBallJoint(WBallPositionConstraint), + #[cfg(feature = "simd-is-enabled")] + WBallGroundConstraint(WBallPositionGroundConstraint), + FixedJoint(FixedPositionConstraint), + FixedGroundConstraint(FixedPositionGroundConstraint), + PrismaticJoint(PrismaticPositionConstraint), + PrismaticGroundConstraint(PrismaticPositionGroundConstraint), + #[cfg(feature = "dim3")] + RevoluteJoint(RevolutePositionConstraint), + #[cfg(feature = "dim3")] + RevoluteGroundConstraint(RevolutePositionGroundConstraint), + #[allow(dead_code)] // The Empty variant is only used with parallel code. + Empty, +} + +impl AnyJointPositionConstraint { + #[cfg(feature = "parallel")] + pub fn num_active_constraints(joint: &Joint, grouped: bool) -> usize { + #[cfg(feature = "simd-is-enabled")] + if !grouped { + 1 + } else { + match &joint.params { + JointParams::BallJoint(_) => 1, + _ => SIMD_WIDTH, // For joints that don't support SIMD position constraints yet. + } + } + + #[cfg(not(feature = "simd-is-enabled"))] + { + 1 + } + } + + pub fn from_joint(joint: &Joint, bodies: &RigidBodySet) -> Self { + let rb1 = &bodies[joint.body1]; + let rb2 = &bodies[joint.body2]; + + match &joint.params { + JointParams::BallJoint(p) => AnyJointPositionConstraint::BallJoint( + BallPositionConstraint::from_params(rb1, rb2, p), + ), + JointParams::FixedJoint(p) => AnyJointPositionConstraint::FixedJoint( + FixedPositionConstraint::from_params(rb1, rb2, p), + ), + JointParams::PrismaticJoint(p) => AnyJointPositionConstraint::PrismaticJoint( + PrismaticPositionConstraint::from_params(rb1, rb2, p), + ), + #[cfg(feature = "dim3")] + JointParams::RevoluteJoint(p) => AnyJointPositionConstraint::RevoluteJoint( + RevolutePositionConstraint::from_params(rb1, rb2, p), + ), + } + } + + #[cfg(feature = "simd-is-enabled")] + pub fn from_wide_joint(joints: [&Joint; SIMD_WIDTH], bodies: &RigidBodySet) -> Option { + let rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH]; + let rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH]; + + match &joints[0].params { + JointParams::BallJoint(_) => { + let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH]; + Some(AnyJointPositionConstraint::WBallJoint( + WBallPositionConstraint::from_params(rbs1, rbs2, joints), + )) + } + JointParams::FixedJoint(_) => None, + JointParams::PrismaticJoint(_) => None, + #[cfg(feature = "dim3")] + JointParams::RevoluteJoint(_) => None, + } + } + + pub fn from_joint_ground(joint: &Joint, bodies: &RigidBodySet) -> Self { + let mut rb1 = &bodies[joint.body1]; + let mut rb2 = &bodies[joint.body2]; + let flipped = !rb2.is_dynamic(); + + if flipped { + std::mem::swap(&mut rb1, &mut rb2); + } + + match &joint.params { + JointParams::BallJoint(p) => AnyJointPositionConstraint::BallGroundConstraint( + BallPositionGroundConstraint::from_params(rb1, rb2, p, flipped), + ), + JointParams::FixedJoint(p) => AnyJointPositionConstraint::FixedGroundConstraint( + FixedPositionGroundConstraint::from_params(rb1, rb2, p, flipped), + ), + JointParams::PrismaticJoint(p) => { + AnyJointPositionConstraint::PrismaticGroundConstraint( + PrismaticPositionGroundConstraint::from_params(rb1, rb2, p, flipped), + ) + } + #[cfg(feature = "dim3")] + JointParams::RevoluteJoint(p) => AnyJointPositionConstraint::RevoluteGroundConstraint( + RevolutePositionGroundConstraint::from_params(rb1, rb2, p, flipped), + ), + } + } + + #[cfg(feature = "simd-is-enabled")] + pub fn from_wide_joint_ground( + joints: [&Joint; SIMD_WIDTH], + bodies: &RigidBodySet, + ) -> Option { + let mut rbs1 = array![|ii| &bodies[joints[ii].body1]; SIMD_WIDTH]; + let mut rbs2 = array![|ii| &bodies[joints[ii].body2]; SIMD_WIDTH]; + let mut flipped = [false; SIMD_WIDTH]; + + for ii in 0..SIMD_WIDTH { + if !rbs2[ii].is_dynamic() { + std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]); + flipped[ii] = true; + } + } + + match &joints[0].params { + JointParams::BallJoint(_) => { + let joints = array![|ii| joints[ii].params.as_ball_joint().unwrap(); SIMD_WIDTH]; + Some(AnyJointPositionConstraint::WBallGroundConstraint( + WBallPositionGroundConstraint::from_params(rbs1, rbs2, joints, flipped), + )) + } + JointParams::FixedJoint(_) => None, + JointParams::PrismaticJoint(_) => None, + #[cfg(feature = "dim3")] + JointParams::RevoluteJoint(_) => None, + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + match self { + AnyJointPositionConstraint::BallJoint(c) => c.solve(params, positions), + AnyJointPositionConstraint::BallGroundConstraint(c) => c.solve(params, positions), + #[cfg(feature = "simd-is-enabled")] + AnyJointPositionConstraint::WBallJoint(c) => c.solve(params, positions), + #[cfg(feature = "simd-is-enabled")] + AnyJointPositionConstraint::WBallGroundConstraint(c) => c.solve(params, positions), + AnyJointPositionConstraint::FixedJoint(c) => c.solve(params, positions), + AnyJointPositionConstraint::FixedGroundConstraint(c) => c.solve(params, positions), + AnyJointPositionConstraint::PrismaticJoint(c) => c.solve(params, positions), + AnyJointPositionConstraint::PrismaticGroundConstraint(c) => c.solve(params, positions), + #[cfg(feature = "dim3")] + AnyJointPositionConstraint::RevoluteJoint(c) => c.solve(params, positions), + #[cfg(feature = "dim3")] + AnyJointPositionConstraint::RevoluteGroundConstraint(c) => c.solve(params, positions), + AnyJointPositionConstraint::Empty => unreachable!(), + } + } +} diff --git a/src/dynamics/solver/joint_constraint/mod.rs b/src/dynamics/solver/joint_constraint/mod.rs new file mode 100644 index 000000000..dc604a520 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/mod.rs @@ -0,0 +1,65 @@ +pub(self) use ball_position_constraint::{BallPositionConstraint, BallPositionGroundConstraint}; +#[cfg(feature = "simd-is-enabled")] +pub(self) use ball_position_constraint_wide::{ + WBallPositionConstraint, WBallPositionGroundConstraint, +}; +pub(self) use ball_velocity_constraint::{BallVelocityConstraint, BallVelocityGroundConstraint}; +#[cfg(feature = "simd-is-enabled")] +pub(self) use ball_velocity_constraint_wide::{ + WBallVelocityConstraint, WBallVelocityGroundConstraint, +}; +pub(self) use fixed_position_constraint::{FixedPositionConstraint, FixedPositionGroundConstraint}; +pub(self) use fixed_velocity_constraint::{FixedVelocityConstraint, FixedVelocityGroundConstraint}; +#[cfg(feature = "simd-is-enabled")] +pub(self) use fixed_velocity_constraint_wide::{ + WFixedVelocityConstraint, WFixedVelocityGroundConstraint, +}; +pub(crate) use joint_constraint::AnyJointVelocityConstraint; +pub(crate) use joint_position_constraint::AnyJointPositionConstraint; +pub(self) use prismatic_position_constraint::{ + PrismaticPositionConstraint, PrismaticPositionGroundConstraint, +}; +pub(self) use prismatic_velocity_constraint::{ + PrismaticVelocityConstraint, PrismaticVelocityGroundConstraint, +}; +#[cfg(feature = "simd-is-enabled")] +pub(self) use prismatic_velocity_constraint_wide::{ + WPrismaticVelocityConstraint, WPrismaticVelocityGroundConstraint, +}; +#[cfg(feature = "dim3")] +pub(self) use revolute_position_constraint::{ + RevolutePositionConstraint, RevolutePositionGroundConstraint, +}; +#[cfg(feature = "dim3")] +pub(self) use revolute_velocity_constraint::{ + RevoluteVelocityConstraint, RevoluteVelocityGroundConstraint, +}; +#[cfg(feature = "dim3")] +#[cfg(feature = "simd-is-enabled")] +pub(self) use revolute_velocity_constraint_wide::{ + WRevoluteVelocityConstraint, WRevoluteVelocityGroundConstraint, +}; + +mod ball_position_constraint; +#[cfg(feature = "simd-is-enabled")] +mod ball_position_constraint_wide; +mod ball_velocity_constraint; +#[cfg(feature = "simd-is-enabled")] +mod ball_velocity_constraint_wide; +mod fixed_position_constraint; +mod fixed_velocity_constraint; +#[cfg(feature = "simd-is-enabled")] +mod fixed_velocity_constraint_wide; +mod joint_constraint; +mod joint_position_constraint; +mod prismatic_position_constraint; +mod prismatic_velocity_constraint; +#[cfg(feature = "simd-is-enabled")] +mod prismatic_velocity_constraint_wide; +#[cfg(feature = "dim3")] +mod revolute_position_constraint; +#[cfg(feature = "dim3")] +mod revolute_velocity_constraint; +#[cfg(feature = "dim3")] +#[cfg(feature = "simd-is-enabled")] +mod revolute_velocity_constraint_wide; diff --git a/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs new file mode 100644 index 000000000..c3e4b988b --- /dev/null +++ b/src/dynamics/solver/joint_constraint/prismatic_position_constraint.rs @@ -0,0 +1,170 @@ +use crate::dynamics::{IntegrationParameters, PrismaticJoint, RigidBody}; +use crate::math::{AngularInertia, Isometry, Point, Rotation, Vector}; +use crate::utils::WAngularInertia; +use na::Unit; + +#[derive(Debug)] +pub(crate) struct PrismaticPositionConstraint { + position1: usize, + position2: usize, + + im1: f32, + im2: f32, + + ii1: AngularInertia, + ii2: AngularInertia, + + lin_inv_lhs: f32, + ang_inv_lhs: AngularInertia, + + limits: [f32; 2], + + local_frame1: Isometry, + local_frame2: Isometry, + + local_axis1: Unit>, + local_axis2: Unit>, +} + +impl PrismaticPositionConstraint { + pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &PrismaticJoint) -> Self { + let ii1 = rb1.world_inv_inertia_sqrt.squared(); + let ii2 = rb2.world_inv_inertia_sqrt.squared(); + let im1 = rb1.mass_properties.inv_mass; + let im2 = rb2.mass_properties.inv_mass; + let lin_inv_lhs = 1.0 / (im1 + im2); + let ang_inv_lhs = (ii1 + ii2).inverse(); + + Self { + im1, + im2, + ii1, + ii2, + lin_inv_lhs, + ang_inv_lhs, + local_frame1: cparams.local_frame1(), + local_frame2: cparams.local_frame2(), + local_axis1: cparams.local_axis1, + local_axis2: cparams.local_axis2, + position1: rb1.active_set_offset, + position2: rb2.active_set_offset, + limits: cparams.limits, + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + let mut position1 = positions[self.position1 as usize]; + let mut position2 = positions[self.position2 as usize]; + + // Angular correction. + let frame1 = position1 * self.local_frame1; + let frame2 = position2 * self.local_frame2; + let ang_err = frame2.rotation * frame1.rotation.inverse(); + #[cfg(feature = "dim2")] + let ang_impulse = self + .ang_inv_lhs + .transform_vector(ang_err.angle() * params.joint_erp); + #[cfg(feature = "dim3")] + let ang_impulse = self + .ang_inv_lhs + .transform_vector(ang_err.scaled_axis() * params.joint_erp); + position1.rotation = + Rotation::new(self.ii1.transform_vector(ang_impulse)) * position1.rotation; + position2.rotation = + Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation; + + // Linear correction. + let anchor1 = position1 * Point::from(self.local_frame1.translation.vector); + let anchor2 = position2 * Point::from(self.local_frame2.translation.vector); + let axis1 = position1 * self.local_axis1; + let dpos = anchor2 - anchor1; + let limit_err = dpos.dot(&axis1); + let mut err = dpos - *axis1 * limit_err; + + if limit_err < self.limits[0] { + err += *axis1 * (limit_err - self.limits[0]); + } else if limit_err > self.limits[1] { + err += *axis1 * (limit_err - self.limits[1]); + } + + let impulse = err * (self.lin_inv_lhs * params.joint_erp); + position1.translation.vector += self.im1 * impulse; + position2.translation.vector -= self.im2 * impulse; + + positions[self.position1 as usize] = position1; + positions[self.position2 as usize] = position2; + } +} + +#[derive(Debug)] +pub(crate) struct PrismaticPositionGroundConstraint { + position2: usize, + frame1: Isometry, + local_frame2: Isometry, + axis1: Unit>, + local_axis2: Unit>, + limits: [f32; 2], +} + +impl PrismaticPositionGroundConstraint { + pub fn from_params( + rb1: &RigidBody, + rb2: &RigidBody, + cparams: &PrismaticJoint, + flipped: bool, + ) -> Self { + let frame1; + let local_frame2; + let axis1; + let local_axis2; + + if flipped { + frame1 = rb1.predicted_position * cparams.local_frame2(); + local_frame2 = cparams.local_frame1(); + axis1 = rb1.predicted_position * cparams.local_axis2; + local_axis2 = cparams.local_axis1; + } else { + frame1 = rb1.predicted_position * cparams.local_frame1(); + local_frame2 = cparams.local_frame2(); + axis1 = rb1.predicted_position * cparams.local_axis1; + local_axis2 = cparams.local_axis2; + }; + + Self { + frame1, + local_frame2, + axis1, + local_axis2, + position2: rb2.active_set_offset, + limits: cparams.limits, + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + let mut position2 = positions[self.position2 as usize]; + + // Angular correction. + let frame2 = position2 * self.local_frame2; + let ang_err = frame2.rotation * self.frame1.rotation.inverse(); + position2.rotation = ang_err.powf(-params.joint_erp) * position2.rotation; + + // Linear correction. + let anchor1 = Point::from(self.frame1.translation.vector); + let anchor2 = position2 * Point::from(self.local_frame2.translation.vector); + let dpos = anchor2 - anchor1; + let limit_err = dpos.dot(&self.axis1); + let mut err = dpos - *self.axis1 * limit_err; + + if limit_err < self.limits[0] { + err += *self.axis1 * (limit_err - self.limits[0]); + } else if limit_err > self.limits[1] { + err += *self.axis1 * (limit_err - self.limits[1]); + } + + // NOTE: no need to divide by im2 just to multiply right after. + let impulse = err * params.joint_erp; + position2.translation.vector -= impulse; + + positions[self.position2 as usize] = position2; + } +} diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs new file mode 100644 index 000000000..49cfc7aca --- /dev/null +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint.rs @@ -0,0 +1,558 @@ +use crate::dynamics::solver::DeltaVel; +use crate::dynamics::{ + IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody, +}; +use crate::math::{AngularInertia, Vector}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; +#[cfg(feature = "dim3")] +use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; +#[cfg(feature = "dim2")] +use { + crate::utils::SdpMatrix2, + na::{Matrix2, Vector2}, +}; + +#[cfg(feature = "dim2")] +type LinImpulseDim = na::U1; +#[cfg(feature = "dim3")] +type LinImpulseDim = na::U2; + +#[derive(Debug)] +pub(crate) struct PrismaticVelocityConstraint { + mj_lambda1: usize, + mj_lambda2: usize, + + joint_id: JointIndex, + + r1: Vector, + r2: Vector, + + #[cfg(feature = "dim3")] + inv_lhs: Matrix5, + #[cfg(feature = "dim3")] + rhs: Vector5, + #[cfg(feature = "dim3")] + impulse: Vector5, + + #[cfg(feature = "dim2")] + inv_lhs: Matrix2, + #[cfg(feature = "dim2")] + rhs: Vector2, + #[cfg(feature = "dim2")] + impulse: Vector2, + + limits_impulse: f32, + limits_forcedirs: Option<(Vector, Vector)>, + limits_rhs: f32, + + #[cfg(feature = "dim2")] + basis1: Vector2, + #[cfg(feature = "dim3")] + basis1: Matrix3x2, + + im1: f32, + im2: f32, + + ii1_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, +} + +impl PrismaticVelocityConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: JointIndex, + rb1: &RigidBody, + rb2: &RigidBody, + cparams: &PrismaticJoint, + ) -> Self { + // Linear part. + let anchor1 = rb1.position * cparams.local_anchor1; + let anchor2 = rb2.position * cparams.local_anchor2; + let axis1 = rb1.position * cparams.local_axis1; + let axis2 = rb2.position * cparams.local_axis2; + #[cfg(feature = "dim2")] + let basis1 = rb1.position * cparams.basis1[0]; + #[cfg(feature = "dim3")] + let basis1 = Matrix3x2::from_columns(&[ + rb1.position * cparams.basis1[0], + rb1.position * cparams.basis1[1], + ]); + + // #[cfg(feature = "dim2")] + // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) + // .to_rotation_matrix() + // .into_inner(); + // #[cfg(feature = "dim3")] + // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) + // .unwrap_or(Rotation::identity()) + // .to_rotation_matrix() + // .into_inner(); + // let basis2 = r21 * basis1; + // NOTE: we use basis2 := basis1 for now is that allows + // simplifications of the computation without introducing + // much instabilities. + + let im1 = rb1.mass_properties.inv_mass; + let ii1 = rb1.world_inv_inertia_sqrt.squared(); + let r1 = anchor1 - rb1.world_com; + let r1_mat = r1.gcross_matrix(); + + let im2 = rb2.mass_properties.inv_mass; + let ii2 = rb2.world_inv_inertia_sqrt.squared(); + let r2 = anchor2 - rb2.world_com; + let r2_mat = r2.gcross_matrix(); + + #[allow(unused_mut)] // For 2D. + let mut lhs; + + #[cfg(feature = "dim3")] + { + let r1_mat_b1 = r1_mat * basis1; + let r2_mat_b1 = r2_mat * basis1; + + lhs = Matrix5::zeros(); + let lhs00 = ii1.quadform3x2(&r1_mat_b1).add_diagonal(im1) + + ii2.quadform3x2(&r2_mat_b1).add_diagonal(im2); + let lhs10 = ii1 * r1_mat_b1 + ii2 * r2_mat_b1; + let lhs11 = (ii1 + ii2).into_matrix(); + lhs.fixed_slice_mut::(0, 0) + .copy_from(&lhs00.into_matrix()); + lhs.fixed_slice_mut::(2, 0).copy_from(&lhs10); + lhs.fixed_slice_mut::(2, 2).copy_from(&lhs11); + } + + #[cfg(feature = "dim2")] + { + let b1r1 = basis1.dot(&r1_mat); + let b2r2 = basis1.dot(&r2_mat); + let m11 = im1 + im2 + b1r1 * ii1 * b1r1 + b2r2 * ii2 * b2r2; + let m12 = basis1.dot(&r1_mat) * ii1 + basis1.dot(&r2_mat) * ii2; + let m22 = ii1 + ii2; + lhs = SdpMatrix2::new(m11, m12, m22); + } + + let anchor_linvel1 = rb1.linvel + rb1.angvel.gcross(r1); + let anchor_linvel2 = rb2.linvel + rb2.angvel.gcross(r2); + + // NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix + // for which a textbook inverse is still efficient. + #[cfg(feature = "dim2")] + let inv_lhs = lhs.inverse_unchecked().into_matrix(); + #[cfg(feature = "dim3")] + let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); + + let lin_rhs = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1)); + let ang_rhs = rb2.angvel - rb1.angvel; + + #[cfg(feature = "dim2")] + let rhs = Vector2::new(lin_rhs.x, ang_rhs); + #[cfg(feature = "dim3")] + let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z); + + // Setup limit constraint. + let mut limits_forcedirs = None; + let mut limits_rhs = 0.0; + let mut limits_impulse = 0.0; + + if cparams.limits_enabled { + let danchor = anchor2 - anchor1; + let dist = danchor.dot(&axis1); + + // FIXME: we should allow both limits to be active at + // the same time, and allow predictive constraint activation. + if dist < cparams.limits[0] { + limits_forcedirs = Some((-axis1.into_inner(), axis2.into_inner())); + limits_rhs = anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1); + limits_impulse = cparams.limits_impulse; + } else if dist > cparams.limits[1] { + limits_forcedirs = Some((axis1.into_inner(), -axis2.into_inner())); + limits_rhs = -anchor_linvel2.dot(&axis2) + anchor_linvel1.dot(&axis1); + limits_impulse = cparams.limits_impulse; + } + } + + PrismaticVelocityConstraint { + joint_id, + mj_lambda1: rb1.active_set_offset, + mj_lambda2: rb2.active_set_offset, + im1, + ii1_sqrt: rb1.world_inv_inertia_sqrt, + im2, + ii2_sqrt: rb2.world_inv_inertia_sqrt, + impulse: cparams.impulse * params.warmstart_coeff, + limits_impulse: limits_impulse * params.warmstart_coeff, + limits_forcedirs, + limits_rhs, + basis1, + inv_lhs, + rhs, + r1, + r2, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + let lin_impulse = self.basis1 * self.impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = self.impulse.y; + #[cfg(feature = "dim3")] + let ang_impulse = self.impulse.fixed_rows::(2).into_owned(); + + mj_lambda1.linear += self.im1 * lin_impulse; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + + mj_lambda2.linear -= self.im2 * lin_impulse; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs { + mj_lambda1.linear += limits_forcedir1 * (self.im1 * self.limits_impulse); + mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse); + } + + mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + /* + * Joint consraint. + */ + let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + let lin_vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1); + let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2); + let lin_dvel = self.basis1.tr_mul(&(lin_vel2 - lin_vel1)); + let ang_dvel = ang_vel2 - ang_vel1; + #[cfg(feature = "dim2")] + let rhs = Vector2::new(lin_dvel.x, ang_dvel) + self.rhs; + #[cfg(feature = "dim3")] + let rhs = + Vector5::new(lin_dvel.x, lin_dvel.y, ang_dvel.x, ang_dvel.y, ang_dvel.z) + self.rhs; + let impulse = self.inv_lhs * rhs; + self.impulse += impulse; + let lin_impulse = self.basis1 * impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = impulse.y; + #[cfg(feature = "dim3")] + let ang_impulse = impulse.fixed_rows::(2).into_owned(); + + mj_lambda1.linear += self.im1 * lin_impulse; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + + mj_lambda2.linear -= self.im2 * lin_impulse; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + /* + * Joint limits. + */ + if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs { + // FIXME: the transformation by ii2_sqrt could be avoided by + // reusing some computations above. + let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2))) + + limits_forcedir1.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1))) + + self.limits_rhs; + let new_impulse = (self.limits_impulse - lin_dvel / (self.im1 + self.im2)).max(0.0); + let dimpulse = new_impulse - self.limits_impulse; + self.limits_impulse = new_impulse; + + mj_lambda1.linear += limits_forcedir1 * (self.im1 * dimpulse); + mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse); + } + + mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + let joint = &mut joints_all[self.joint_id].weight; + if let JointParams::PrismaticJoint(revolute) = &mut joint.params { + revolute.impulse = self.impulse; + revolute.limits_impulse = self.limits_impulse; + } + } +} + +#[derive(Debug)] +pub(crate) struct PrismaticVelocityGroundConstraint { + mj_lambda2: usize, + + joint_id: JointIndex, + + r2: Vector, + + #[cfg(feature = "dim2")] + inv_lhs: Matrix2, + #[cfg(feature = "dim2")] + rhs: Vector2, + #[cfg(feature = "dim2")] + impulse: Vector2, + + #[cfg(feature = "dim3")] + inv_lhs: Matrix5, + #[cfg(feature = "dim3")] + rhs: Vector5, + #[cfg(feature = "dim3")] + impulse: Vector5, + + limits_impulse: f32, + limits_rhs: f32, + + axis2: Vector, + #[cfg(feature = "dim2")] + basis1: Vector2, + #[cfg(feature = "dim3")] + basis1: Matrix3x2, + limits_forcedir2: Option>, + + im2: f32, + ii2_sqrt: AngularInertia, +} + +impl PrismaticVelocityGroundConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: JointIndex, + rb1: &RigidBody, + rb2: &RigidBody, + cparams: &PrismaticJoint, + flipped: bool, + ) -> Self { + let anchor2; + let anchor1; + let axis2; + let axis1; + let basis1; + + if flipped { + anchor2 = rb2.position * cparams.local_anchor1; + anchor1 = rb1.position * cparams.local_anchor2; + axis2 = rb2.position * cparams.local_axis1; + axis1 = rb1.position * cparams.local_axis2; + #[cfg(feature = "dim2")] + { + basis1 = rb1.position * cparams.basis2[0]; + } + #[cfg(feature = "dim3")] + { + basis1 = Matrix3x2::from_columns(&[ + rb1.position * cparams.basis2[0], + rb1.position * cparams.basis2[1], + ]); + } + } else { + anchor2 = rb2.position * cparams.local_anchor2; + anchor1 = rb1.position * cparams.local_anchor1; + axis2 = rb2.position * cparams.local_axis2; + axis1 = rb1.position * cparams.local_axis1; + #[cfg(feature = "dim2")] + { + basis1 = rb1.position * cparams.basis1[0]; + } + #[cfg(feature = "dim3")] + { + basis1 = Matrix3x2::from_columns(&[ + rb1.position * cparams.basis1[0], + rb1.position * cparams.basis1[1], + ]); + } + }; + + // #[cfg(feature = "dim2")] + // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) + // .to_rotation_matrix() + // .into_inner(); + // #[cfg(feature = "dim3")] + // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) + // .unwrap_or(Rotation::identity()) + // .to_rotation_matrix() + // .into_inner(); + // let basis2 = r21 * basis1; + // NOTE: we use basis2 := basis1 for now is that allows + // simplifications of the computation without introducing + // much instabilities. + + let im2 = rb2.mass_properties.inv_mass; + let ii2 = rb2.world_inv_inertia_sqrt.squared(); + let r1 = anchor1 - rb1.world_com; + let r2 = anchor2 - rb2.world_com; + let r2_mat = r2.gcross_matrix(); + + #[allow(unused_mut)] // For 2D. + let mut lhs; + + #[cfg(feature = "dim3")] + { + let r2_mat_b1 = r2_mat * basis1; + + lhs = Matrix5::zeros(); + let lhs00 = ii2.quadform3x2(&r2_mat_b1).add_diagonal(im2); + let lhs10 = ii2 * r2_mat_b1; + let lhs11 = ii2.into_matrix(); + lhs.fixed_slice_mut::(0, 0) + .copy_from(&lhs00.into_matrix()); + lhs.fixed_slice_mut::(2, 0).copy_from(&lhs10); + lhs.fixed_slice_mut::(2, 2).copy_from(&lhs11); + } + + #[cfg(feature = "dim2")] + { + let b2r2 = basis1.dot(&r2_mat); + let m11 = im2 + b2r2 * ii2 * b2r2; + let m12 = basis1.dot(&r2_mat) * ii2; + let m22 = ii2; + lhs = SdpMatrix2::new(m11, m12, m22); + } + + let anchor_linvel1 = rb1.linvel + rb1.angvel.gcross(r1); + let anchor_linvel2 = rb2.linvel + rb2.angvel.gcross(r2); + + // NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix + // for which a textbook inverse is still efficient. + #[cfg(feature = "dim2")] + let inv_lhs = lhs.inverse_unchecked().into_matrix(); + #[cfg(feature = "dim3")] + let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); + + let lin_rhs = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1)); + let ang_rhs = rb2.angvel - rb1.angvel; + + #[cfg(feature = "dim2")] + let rhs = Vector2::new(lin_rhs.x, ang_rhs); + #[cfg(feature = "dim3")] + let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z); + + // Setup limit constraint. + let mut limits_forcedir2 = None; + let mut limits_rhs = 0.0; + let mut limits_impulse = 0.0; + + if cparams.limits_enabled { + let danchor = anchor2 - anchor1; + let dist = danchor.dot(&axis1); + + // FIXME: we should allow both limits to be active at + // the same time. + // FIXME: allow predictive constraint activation. + if dist < cparams.limits[0] { + limits_forcedir2 = Some(axis2.into_inner()); + limits_rhs = anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1); + limits_impulse = cparams.limits_impulse; + } else if dist > cparams.limits[1] { + limits_forcedir2 = Some(-axis2.into_inner()); + limits_rhs = -anchor_linvel2.dot(&axis2) + anchor_linvel1.dot(&axis1); + limits_impulse = cparams.limits_impulse; + } + } + + PrismaticVelocityGroundConstraint { + joint_id, + mj_lambda2: rb2.active_set_offset, + im2, + ii2_sqrt: rb2.world_inv_inertia_sqrt, + impulse: cparams.impulse * params.warmstart_coeff, + limits_impulse: limits_impulse * params.warmstart_coeff, + basis1, + inv_lhs, + rhs, + r2, + axis2: axis2.into_inner(), + limits_forcedir2, + limits_rhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + let lin_impulse = self.basis1 * self.impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = self.impulse.y; + #[cfg(feature = "dim3")] + let ang_impulse = self.impulse.fixed_rows::(2).into_owned(); + + mj_lambda2.linear -= self.im2 * lin_impulse; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + if let Some(limits_forcedir2) = self.limits_forcedir2 { + mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse); + } + + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + /* + * Joint consraint. + */ + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2); + let lin_dvel = self.basis1.tr_mul(&lin_vel2); + let ang_dvel = ang_vel2; + #[cfg(feature = "dim2")] + let rhs = Vector2::new(lin_dvel.x, ang_dvel) + self.rhs; + #[cfg(feature = "dim3")] + let rhs = + Vector5::new(lin_dvel.x, lin_dvel.y, ang_dvel.x, ang_dvel.y, ang_dvel.z) + self.rhs; + let impulse = self.inv_lhs * rhs; + self.impulse += impulse; + let lin_impulse = self.basis1 * impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = impulse.y; + #[cfg(feature = "dim3")] + let ang_impulse = impulse.fixed_rows::(2).into_owned(); + + mj_lambda2.linear -= self.im2 * lin_impulse; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + /* + * Joint limits. + */ + if let Some(limits_forcedir2) = self.limits_forcedir2 { + // FIXME: the transformation by ii2_sqrt could be avoided by + // reusing some computations above. + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2))) + + self.limits_rhs; + let new_impulse = (self.limits_impulse - lin_dvel / self.im2).max(0.0); + let dimpulse = new_impulse - self.limits_impulse; + self.limits_impulse = new_impulse; + + mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse); + } + + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + // FIXME: duplicated code with the non-ground constraint. + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + let joint = &mut joints_all[self.joint_id].weight; + if let JointParams::PrismaticJoint(revolute) = &mut joint.params { + revolute.impulse = self.impulse; + revolute.limits_impulse = self.limits_impulse; + } + } +} diff --git a/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs new file mode 100644 index 000000000..bb81f2365 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/prismatic_velocity_constraint_wide.rs @@ -0,0 +1,687 @@ +use simba::simd::{SimdBool as _, SimdPartialOrd, SimdValue}; + +use crate::dynamics::solver::DeltaVel; +use crate::dynamics::{ + IntegrationParameters, JointGraphEdge, JointIndex, JointParams, PrismaticJoint, RigidBody, +}; +use crate::math::{ + AngVector, AngularInertia, Isometry, Point, SimdBool, SimdFloat, Vector, SIMD_WIDTH, +}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; +#[cfg(feature = "dim3")] +use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; +#[cfg(feature = "dim2")] +use { + crate::utils::SdpMatrix2, + na::{Matrix2, Vector2}, +}; + +#[cfg(feature = "dim2")] +type LinImpulseDim = na::U1; +#[cfg(feature = "dim3")] +type LinImpulseDim = na::U2; + +#[derive(Debug)] +pub(crate) struct WPrismaticVelocityConstraint { + mj_lambda1: [usize; SIMD_WIDTH], + mj_lambda2: [usize; SIMD_WIDTH], + + joint_id: [JointIndex; SIMD_WIDTH], + + r1: Vector, + r2: Vector, + + #[cfg(feature = "dim3")] + inv_lhs: Matrix5, + #[cfg(feature = "dim3")] + rhs: Vector5, + #[cfg(feature = "dim3")] + impulse: Vector5, + + #[cfg(feature = "dim2")] + inv_lhs: Matrix2, + #[cfg(feature = "dim2")] + rhs: Vector2, + #[cfg(feature = "dim2")] + impulse: Vector2, + + limits_impulse: SimdFloat, + limits_forcedirs: Option<(Vector, Vector)>, + limits_rhs: SimdFloat, + + #[cfg(feature = "dim2")] + basis1: Vector2, + #[cfg(feature = "dim3")] + basis1: Matrix3x2, + + im1: SimdFloat, + im2: SimdFloat, + + ii1_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, +} + +impl WPrismaticVelocityConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: [JointIndex; SIMD_WIDTH], + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&PrismaticJoint; SIMD_WIDTH], + ) -> Self { + let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); + let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); + let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii1_sqrt = AngularInertia::::from( + array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ); + let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; + + let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); + let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); + let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( + array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ); + let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + + let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); + let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); + let local_axis1 = Vector::from(array![|ii| *cparams[ii].local_axis1; SIMD_WIDTH]); + let local_axis2 = Vector::from(array![|ii| *cparams[ii].local_axis2; SIMD_WIDTH]); + + #[cfg(feature = "dim2")] + let local_basis1 = [Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH])]; + #[cfg(feature = "dim3")] + let local_basis1 = [ + Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH]), + Vector::from(array![|ii| cparams[ii].basis1[1]; SIMD_WIDTH]), + ]; + + #[cfg(feature = "dim2")] + let impulse = Vector2::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + #[cfg(feature = "dim3")] + let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + + let anchor1 = position1 * local_anchor1; + let anchor2 = position2 * local_anchor2; + let axis1 = position1 * local_axis1; + let axis2 = position2 * local_axis2; + + #[cfg(feature = "dim2")] + let basis1 = position1 * local_basis1[0]; + #[cfg(feature = "dim3")] + let basis1 = + Matrix3x2::from_columns(&[position1 * local_basis1[0], position1 * local_basis1[1]]); + + // #[cfg(feature = "dim2")] + // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) + // .to_rotation_matrix() + // .into_inner(); + // #[cfg(feature = "dim3")] + // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) + // .unwrap_or(Rotation::identity()) + // .to_rotation_matrix() + // .into_inner(); + // let basis2 = r21 * basis1; + // NOTE: we use basis2 := basis1 for now is that allows + // simplifications of the computation without introducing + // much instabilities. + + let ii1 = ii1_sqrt.squared(); + let r1 = anchor1 - world_com1; + let r1_mat = r1.gcross_matrix(); + + let ii2 = ii2_sqrt.squared(); + let r2 = anchor2 - world_com2; + let r2_mat = r2.gcross_matrix(); + + #[allow(unused_mut)] // For 2D. + let mut lhs; + + #[cfg(feature = "dim3")] + { + let r1_mat_b1 = r1_mat * basis1; + let r2_mat_b1 = r2_mat * basis1; + + lhs = Matrix5::zeros(); + let lhs00 = ii1.quadform3x2(&r1_mat_b1).add_diagonal(im1) + + ii2.quadform3x2(&r2_mat_b1).add_diagonal(im2); + let lhs10 = ii1 * r1_mat_b1 + ii2 * r2_mat_b1; + let lhs11 = (ii1 + ii2).into_matrix(); + lhs.fixed_slice_mut::(0, 0) + .copy_from(&lhs00.into_matrix()); + lhs.fixed_slice_mut::(2, 0).copy_from(&lhs10); + lhs.fixed_slice_mut::(2, 2).copy_from(&lhs11); + } + + #[cfg(feature = "dim2")] + { + let b1r1 = basis1.dot(&r1_mat); + let b2r2 = basis1.dot(&r2_mat); + let m11 = im1 + im2 + b1r1 * ii1 * b1r1 + b2r2 * ii2 * b2r2; + let m12 = basis1.dot(&r1_mat) * ii1 + basis1.dot(&r2_mat) * ii2; + let m22 = ii1 + ii2; + lhs = SdpMatrix2::new(m11, m12, m22); + } + + let anchor_linvel1 = linvel1 + angvel1.gcross(r1); + let anchor_linvel2 = linvel2 + angvel2.gcross(r2); + + // NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix + // for which a textbook inverse is still efficient. + #[cfg(feature = "dim2")] + let inv_lhs = lhs.inverse_unchecked().into_matrix(); + #[cfg(feature = "dim3")] + let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); + + let lin_rhs = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1)); + let ang_rhs = angvel2 - angvel1; + + #[cfg(feature = "dim2")] + let rhs = Vector2::new(lin_rhs.x, ang_rhs); + #[cfg(feature = "dim3")] + let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z); + + // Setup limit constraint. + let mut limits_forcedirs = None; + let mut limits_rhs = na::zero(); + let mut limits_impulse = na::zero(); + let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]); + + if limits_enabled.any() { + let danchor = anchor2 - anchor1; + let dist = danchor.dot(&axis1); + + // FIXME: we should allow both limits to be active at + // the same time + allow predictive constraint activation. + let min_limit = SimdFloat::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]); + let max_limit = SimdFloat::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]); + let lim_impulse = SimdFloat::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]); + + let min_enabled = dist.simd_lt(min_limit); + let max_enabled = dist.simd_gt(max_limit); + let _0: SimdFloat = na::zero(); + let _1: SimdFloat = na::one(); + let sign = _1.select(min_enabled, (-_1).select(max_enabled, _0)); + + if sign != _0 { + limits_forcedirs = Some((axis1 * -sign, axis2 * sign)); + limits_rhs = (anchor_linvel2.dot(&axis2) - anchor_linvel1.dot(&axis1)) * sign; + limits_impulse = lim_impulse.select(min_enabled | max_enabled, _0); + } + } + + WPrismaticVelocityConstraint { + joint_id, + mj_lambda1, + mj_lambda2, + im1, + ii1_sqrt, + im2, + ii2_sqrt, + impulse: impulse * SimdFloat::splat(params.warmstart_coeff), + limits_impulse: limits_impulse * SimdFloat::splat(params.warmstart_coeff), + limits_forcedirs, + limits_rhs, + basis1, + inv_lhs, + rhs, + r1, + r2, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], + ), + }; + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let lin_impulse = self.basis1 * self.impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = self.impulse.y; + #[cfg(feature = "dim3")] + let ang_impulse = self.impulse.fixed_rows::(2).into_owned(); + + mj_lambda1.linear += lin_impulse * self.im1; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + + mj_lambda2.linear -= lin_impulse * self.im2; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs { + mj_lambda1.linear += limits_forcedir1 * (self.im1 * self.limits_impulse); + mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse); + } + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); + mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii); + } + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], + ), + }; + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + /* + * Joint consraint. + */ + let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + let lin_vel1 = mj_lambda1.linear + ang_vel1.gcross(self.r1); + let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2); + let lin_dvel = self.basis1.tr_mul(&(lin_vel2 - lin_vel1)); + let ang_dvel = ang_vel2 - ang_vel1; + #[cfg(feature = "dim2")] + let rhs = Vector2::new(lin_dvel.x, ang_dvel) + self.rhs; + #[cfg(feature = "dim3")] + let rhs = + Vector5::new(lin_dvel.x, lin_dvel.y, ang_dvel.x, ang_dvel.y, ang_dvel.z) + self.rhs; + let impulse = self.inv_lhs * rhs; + self.impulse += impulse; + let lin_impulse = self.basis1 * impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = impulse.y; + #[cfg(feature = "dim3")] + let ang_impulse = impulse.fixed_rows::(2).into_owned(); + + mj_lambda1.linear += lin_impulse * self.im1; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + + mj_lambda2.linear -= lin_impulse * self.im2; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + /* + * Joint limits. + */ + if let Some((limits_forcedir1, limits_forcedir2)) = self.limits_forcedirs { + // FIXME: the transformation by ii2_sqrt could be avoided by + // reusing some computations above. + let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2))) + + limits_forcedir1.dot(&(mj_lambda1.linear + ang_vel1.gcross(self.r1))) + + self.limits_rhs; + let new_impulse = + (self.limits_impulse - lin_dvel / (self.im1 + self.im2)).simd_max(na::zero()); + let dimpulse = new_impulse - self.limits_impulse; + self.limits_impulse = new_impulse; + + mj_lambda1.linear += limits_forcedir1 * (self.im1 * dimpulse); + mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse); + } + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); + mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii); + } + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + for ii in 0..SIMD_WIDTH { + let joint = &mut joints_all[self.joint_id[ii]].weight; + if let JointParams::PrismaticJoint(rev) = &mut joint.params { + rev.impulse = self.impulse.extract(ii); + rev.limits_impulse = self.limits_impulse.extract(ii); + } + } + } +} + +#[derive(Debug)] +pub(crate) struct WPrismaticVelocityGroundConstraint { + mj_lambda2: [usize; SIMD_WIDTH], + + joint_id: [JointIndex; SIMD_WIDTH], + + r2: Vector, + + #[cfg(feature = "dim2")] + inv_lhs: Matrix2, + #[cfg(feature = "dim2")] + rhs: Vector2, + #[cfg(feature = "dim2")] + impulse: Vector2, + + #[cfg(feature = "dim3")] + inv_lhs: Matrix5, + #[cfg(feature = "dim3")] + rhs: Vector5, + #[cfg(feature = "dim3")] + impulse: Vector5, + + limits_impulse: SimdFloat, + limits_rhs: SimdFloat, + + axis2: Vector, + #[cfg(feature = "dim2")] + basis1: Vector2, + #[cfg(feature = "dim3")] + basis1: Matrix3x2, + limits_forcedir2: Option>, + + im2: SimdFloat, + ii2_sqrt: AngularInertia, +} + +impl WPrismaticVelocityGroundConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: [JointIndex; SIMD_WIDTH], + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&PrismaticJoint; SIMD_WIDTH], + flipped: [bool; SIMD_WIDTH], + ) -> Self { + let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); + let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); + + let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); + let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); + let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( + array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ); + let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + + #[cfg(feature = "dim2")] + let impulse = Vector2::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + #[cfg(feature = "dim3")] + let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + + let local_anchor1 = Point::from( + array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH], + ); + let local_anchor2 = Point::from( + array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH], + ); + let local_axis1 = Vector::from( + array![|ii| if flipped[ii] { *cparams[ii].local_axis2 } else { *cparams[ii].local_axis1 }; SIMD_WIDTH], + ); + let local_axis2 = Vector::from( + array![|ii| if flipped[ii] { *cparams[ii].local_axis1 } else { *cparams[ii].local_axis2 }; SIMD_WIDTH], + ); + + #[cfg(feature = "dim2")] + let basis1 = position1 + * Vector::from( + array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH], + ); + #[cfg(feature = "dim3")] + let basis1 = Matrix3x2::from_columns(&[ + position1 + * Vector::from( + array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH], + ), + position1 + * Vector::from( + array![|ii| if flipped[ii] { cparams[ii].basis2[1] } else { cparams[ii].basis1[1] }; SIMD_WIDTH], + ), + ]); + + let anchor1 = position1 * local_anchor1; + let anchor2 = position2 * local_anchor2; + let axis1 = position1 * local_axis1; + let axis2 = position2 * local_axis2; + + // #[cfg(feature = "dim2")] + // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) + // .to_rotation_matrix() + // .into_inner(); + // #[cfg(feature = "dim3")] + // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) + // .unwrap_or(Rotation::identity()) + // .to_rotation_matrix() + // .into_inner(); + // let basis2 = r21 * basis1; + // NOTE: we use basis2 := basis1 for now is that allows + // simplifications of the computation without introducing + // much instabilities. + let ii2 = ii2_sqrt.squared(); + let r1 = anchor1 - world_com1; + let r2 = anchor2 - world_com2; + let r2_mat = r2.gcross_matrix(); + + #[allow(unused_mut)] // For 2D. + let mut lhs; + + #[cfg(feature = "dim3")] + { + let r2_mat_b1 = r2_mat * basis1; + + lhs = Matrix5::zeros(); + let lhs00 = ii2.quadform3x2(&r2_mat_b1).add_diagonal(im2); + let lhs10 = ii2 * r2_mat_b1; + let lhs11 = ii2.into_matrix(); + lhs.fixed_slice_mut::(0, 0) + .copy_from(&lhs00.into_matrix()); + lhs.fixed_slice_mut::(2, 0).copy_from(&lhs10); + lhs.fixed_slice_mut::(2, 2).copy_from(&lhs11); + } + + #[cfg(feature = "dim2")] + { + let b2r2 = basis1.dot(&r2_mat); + let m11 = im2 + b2r2 * ii2 * b2r2; + let m12 = basis1.dot(&r2_mat) * ii2; + let m22 = ii2; + lhs = SdpMatrix2::new(m11, m12, m22); + } + + let anchor_linvel1 = linvel1 + angvel1.gcross(r1); + let anchor_linvel2 = linvel2 + angvel2.gcross(r2); + + // NOTE: we don't use Cholesky in 2D because we only have a 2x2 matrix + // for which a textbook inverse is still efficient. + #[cfg(feature = "dim2")] + let inv_lhs = lhs.inverse_unchecked().into_matrix(); + #[cfg(feature = "dim3")] + let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); + + let lin_rhs = basis1.tr_mul(&(anchor_linvel2 - anchor_linvel1)); + let ang_rhs = angvel2 - angvel1; + + #[cfg(feature = "dim2")] + let rhs = Vector2::new(lin_rhs.x, ang_rhs); + #[cfg(feature = "dim3")] + let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, ang_rhs.x, ang_rhs.y, ang_rhs.z); + + // Setup limit constraint. + let mut limits_forcedir2 = None; + let mut limits_rhs = na::zero(); + let mut limits_impulse = na::zero(); + let limits_enabled = SimdBool::from(array![|ii| cparams[ii].limits_enabled; SIMD_WIDTH]); + + if limits_enabled.any() { + let danchor = anchor2 - anchor1; + let dist = danchor.dot(&axis1); + + // FIXME: we should allow both limits to be active at + // the same time + allow predictive constraint activation. + let min_limit = SimdFloat::from(array![|ii| cparams[ii].limits[0]; SIMD_WIDTH]); + let max_limit = SimdFloat::from(array![|ii| cparams[ii].limits[1]; SIMD_WIDTH]); + let lim_impulse = SimdFloat::from(array![|ii| cparams[ii].limits_impulse; SIMD_WIDTH]); + + let use_min = dist.simd_lt(min_limit); + let use_max = dist.simd_gt(max_limit); + let _0: SimdFloat = na::zero(); + let _1: SimdFloat = na::one(); + let sign = _1.select(use_min, (-_1).select(use_max, _0)); + + if sign != _0 { + limits_forcedir2 = Some(axis2 * sign); + limits_rhs = anchor_linvel2.dot(&axis2) * sign - anchor_linvel1.dot(&axis1) * sign; + limits_impulse = lim_impulse.select(use_min | use_max, _0); + } + } + + WPrismaticVelocityGroundConstraint { + joint_id, + mj_lambda2, + im2, + ii2_sqrt, + impulse: impulse * SimdFloat::splat(params.warmstart_coeff), + limits_impulse: limits_impulse * SimdFloat::splat(params.warmstart_coeff), + basis1, + inv_lhs, + rhs, + r2, + axis2, + limits_forcedir2, + limits_rhs, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let lin_impulse = self.basis1 * self.impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = self.impulse.y; + #[cfg(feature = "dim3")] + let ang_impulse = self.impulse.fixed_rows::(2).into_owned(); + + mj_lambda2.linear -= lin_impulse * self.im2; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + if let Some(limits_forcedir2) = self.limits_forcedir2 { + mj_lambda2.linear += limits_forcedir2 * (self.im2 * self.limits_impulse); + } + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + /* + * Joint consraint. + */ + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + let lin_vel2 = mj_lambda2.linear + ang_vel2.gcross(self.r2); + let lin_dvel = self.basis1.tr_mul(&lin_vel2); + let ang_dvel = ang_vel2; + #[cfg(feature = "dim2")] + let rhs = Vector2::new(lin_dvel.x, ang_dvel) + self.rhs; + #[cfg(feature = "dim3")] + let rhs = + Vector5::new(lin_dvel.x, lin_dvel.y, ang_dvel.x, ang_dvel.y, ang_dvel.z) + self.rhs; + let impulse = self.inv_lhs * rhs; + self.impulse += impulse; + let lin_impulse = self.basis1 * impulse.fixed_rows::(0).into_owned(); + #[cfg(feature = "dim2")] + let ang_impulse = impulse.y; + #[cfg(feature = "dim3")] + let ang_impulse = impulse.fixed_rows::(2).into_owned(); + + mj_lambda2.linear -= lin_impulse * self.im2; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + /* + * Joint limits. + */ + if let Some(limits_forcedir2) = self.limits_forcedir2 { + // FIXME: the transformation by ii2_sqrt could be avoided by + // reusing some computations above. + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + + let lin_dvel = limits_forcedir2.dot(&(mj_lambda2.linear + ang_vel2.gcross(self.r2))) + + self.limits_rhs; + let new_impulse = (self.limits_impulse - lin_dvel / self.im2).simd_max(na::zero()); + let dimpulse = new_impulse - self.limits_impulse; + self.limits_impulse = new_impulse; + + mj_lambda2.linear += limits_forcedir2 * (self.im2 * dimpulse); + } + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + // FIXME: duplicated code with the non-ground constraint. + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + for ii in 0..SIMD_WIDTH { + let joint = &mut joints_all[self.joint_id[ii]].weight; + if let JointParams::PrismaticJoint(rev) = &mut joint.params { + rev.impulse = self.impulse.extract(ii); + rev.limits_impulse = self.limits_impulse.extract(ii); + } + } + } +} diff --git a/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs new file mode 100644 index 000000000..876ce3121 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/revolute_position_constraint.rs @@ -0,0 +1,142 @@ +use crate::dynamics::{IntegrationParameters, RevoluteJoint, RigidBody}; +use crate::math::{AngularInertia, Isometry, Point, Rotation, Vector}; +use crate::utils::WAngularInertia; +use na::Unit; + +#[derive(Debug)] +pub(crate) struct RevolutePositionConstraint { + position1: usize, + position2: usize, + + im1: f32, + im2: f32, + + ii1: AngularInertia, + ii2: AngularInertia, + + lin_inv_lhs: f32, + ang_inv_lhs: AngularInertia, + + local_anchor1: Point, + local_anchor2: Point, + + local_axis1: Unit>, + local_axis2: Unit>, +} + +impl RevolutePositionConstraint { + pub fn from_params(rb1: &RigidBody, rb2: &RigidBody, cparams: &RevoluteJoint) -> Self { + let ii1 = rb1.world_inv_inertia_sqrt.squared(); + let ii2 = rb2.world_inv_inertia_sqrt.squared(); + let im1 = rb1.mass_properties.inv_mass; + let im2 = rb2.mass_properties.inv_mass; + let lin_inv_lhs = 1.0 / (im1 + im2); + let ang_inv_lhs = (ii1 + ii2).inverse(); + + Self { + im1, + im2, + ii1, + ii2, + lin_inv_lhs, + ang_inv_lhs, + local_anchor1: cparams.local_anchor1, + local_anchor2: cparams.local_anchor2, + local_axis1: cparams.local_axis1, + local_axis2: cparams.local_axis2, + position1: rb1.active_set_offset, + position2: rb2.active_set_offset, + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + let mut position1 = positions[self.position1 as usize]; + let mut position2 = positions[self.position2 as usize]; + + let axis1 = position1 * self.local_axis1; + let axis2 = position2 * self.local_axis2; + let delta_rot = + Rotation::rotation_between_axis(&axis1, &axis2).unwrap_or(Rotation::identity()); + let ang_error = delta_rot.scaled_axis() * params.joint_erp; + let ang_impulse = self.ang_inv_lhs.transform_vector(ang_error); + + position1.rotation = + Rotation::new(self.ii1.transform_vector(ang_impulse)) * position1.rotation; + position2.rotation = + Rotation::new(self.ii2.transform_vector(-ang_impulse)) * position2.rotation; + + let anchor1 = position1 * self.local_anchor1; + let anchor2 = position2 * self.local_anchor2; + + let delta_tra = anchor2 - anchor1; + let lin_error = delta_tra * params.joint_erp; + let lin_impulse = self.lin_inv_lhs * lin_error; + + position1.translation.vector += self.im1 * lin_impulse; + position2.translation.vector -= self.im2 * lin_impulse; + + positions[self.position1 as usize] = position1; + positions[self.position2 as usize] = position2; + } +} + +#[derive(Debug)] +pub(crate) struct RevolutePositionGroundConstraint { + position2: usize, + anchor1: Point, + local_anchor2: Point, + axis1: Unit>, + local_axis2: Unit>, +} + +impl RevolutePositionGroundConstraint { + pub fn from_params( + rb1: &RigidBody, + rb2: &RigidBody, + cparams: &RevoluteJoint, + flipped: bool, + ) -> Self { + let anchor1; + let local_anchor2; + let axis1; + let local_axis2; + + if flipped { + anchor1 = rb1.predicted_position * cparams.local_anchor2; + local_anchor2 = cparams.local_anchor1; + axis1 = rb1.predicted_position * cparams.local_axis2; + local_axis2 = cparams.local_axis1; + } else { + anchor1 = rb1.predicted_position * cparams.local_anchor1; + local_anchor2 = cparams.local_anchor2; + axis1 = rb1.predicted_position * cparams.local_axis1; + local_axis2 = cparams.local_axis2; + }; + + Self { + anchor1, + local_anchor2, + axis1, + local_axis2, + position2: rb2.active_set_offset, + } + } + + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + let mut position2 = positions[self.position2 as usize]; + + let axis2 = position2 * self.local_axis2; + + let delta_rot = + Rotation::scaled_rotation_between_axis(&axis2, &self.axis1, params.joint_erp) + .unwrap_or(Rotation::identity()); + position2.rotation = delta_rot * position2.rotation; + + let anchor2 = position2 * self.local_anchor2; + let delta_tra = anchor2 - self.anchor1; + let lin_error = delta_tra * params.joint_erp; + position2.translation.vector -= lin_error; + + positions[self.position2 as usize] = position2; + } +} diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs new file mode 100644 index 000000000..caf8e4070 --- /dev/null +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint.rs @@ -0,0 +1,294 @@ +use crate::dynamics::solver::DeltaVel; +use crate::dynamics::{ + IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody, +}; +use crate::math::{AngularInertia, Vector}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; +use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; + +#[derive(Debug)] +pub(crate) struct RevoluteVelocityConstraint { + mj_lambda1: usize, + mj_lambda2: usize, + + joint_id: JointIndex, + + r1: Vector, + r2: Vector, + + inv_lhs: Matrix5, + rhs: Vector5, + impulse: Vector5, + + basis1: Matrix3x2, + + im1: f32, + im2: f32, + + ii1_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, +} + +impl RevoluteVelocityConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: JointIndex, + rb1: &RigidBody, + rb2: &RigidBody, + cparams: &RevoluteJoint, + ) -> Self { + // Linear part. + let anchor1 = rb1.position * cparams.local_anchor1; + let anchor2 = rb2.position * cparams.local_anchor2; + let basis1 = Matrix3x2::from_columns(&[ + rb1.position * cparams.basis1[0], + rb1.position * cparams.basis1[1], + ]); + + // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) + // .unwrap_or(Rotation::identity()) + // .to_rotation_matrix() + // .into_inner(); + // let basis2 = r21 * basis1; + // NOTE: to simplify, we use basis2 = basis1. + // Though we may want to test if that does not introduce any instability. + let im1 = rb1.mass_properties.inv_mass; + let im2 = rb2.mass_properties.inv_mass; + + let ii1 = rb1.world_inv_inertia_sqrt.squared(); + let r1 = anchor1 - rb1.world_com; + let r1_mat = r1.gcross_matrix(); + + let ii2 = rb2.world_inv_inertia_sqrt.squared(); + let r2 = anchor2 - rb2.world_com; + let r2_mat = r2.gcross_matrix(); + + let mut lhs = Matrix5::zeros(); + let lhs00 = + ii2.quadform(&r2_mat).add_diagonal(im2) + ii1.quadform(&r1_mat).add_diagonal(im1); + let lhs10 = basis1.tr_mul(&(ii2 * r2_mat + ii1 * r1_mat)); + let lhs11 = (ii1 + ii2).quadform3x2(&basis1).into_matrix(); + + // Note that cholesky won't read the upper-right part + // of lhs so we don't have to fill it. + lhs.fixed_slice_mut::(0, 0) + .copy_from(&lhs00.into_matrix()); + lhs.fixed_slice_mut::(3, 0).copy_from(&lhs10); + lhs.fixed_slice_mut::(3, 3).copy_from(&lhs11); + + let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); + + let lin_rhs = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1); + let ang_rhs = basis1.tr_mul(&(rb2.angvel - rb1.angvel)); + let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y); + + RevoluteVelocityConstraint { + joint_id, + mj_lambda1: rb1.active_set_offset, + mj_lambda2: rb2.active_set_offset, + im1, + ii1_sqrt: rb1.world_inv_inertia_sqrt, + basis1, + im2, + ii2_sqrt: rb2.world_inv_inertia_sqrt, + impulse: cparams.impulse * params.warmstart_coeff, + inv_lhs, + rhs, + r1, + r2, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); + let ang_impulse = self.basis1 * self.impulse.fixed_rows::(3).into_owned(); + + mj_lambda1.linear += self.im1 * lin_impulse; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + + mj_lambda2.linear -= self.im2 * lin_impulse; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2) + - mj_lambda1.linear + - ang_vel1.gcross(self.r1); + let ang_dvel = self.basis1.tr_mul(&(ang_vel2 - ang_vel1)); + let rhs = + Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs; + let impulse = self.inv_lhs * rhs; + self.impulse += impulse; + let lin_impulse = impulse.fixed_rows::(0).into_owned(); + let ang_impulse = self.basis1 * impulse.fixed_rows::(3).into_owned(); + + mj_lambda1.linear += self.im1 * lin_impulse; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + + mj_lambda2.linear -= self.im2 * lin_impulse; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + let joint = &mut joints_all[self.joint_id].weight; + if let JointParams::RevoluteJoint(revolute) = &mut joint.params { + revolute.impulse = self.impulse; + } + } +} + +#[derive(Debug)] +pub(crate) struct RevoluteVelocityGroundConstraint { + mj_lambda2: usize, + + joint_id: JointIndex, + + r2: Vector, + + inv_lhs: Matrix5, + rhs: Vector5, + impulse: Vector5, + + basis1: Matrix3x2, + + im2: f32, + + ii2_sqrt: AngularInertia, +} + +impl RevoluteVelocityGroundConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: JointIndex, + rb1: &RigidBody, + rb2: &RigidBody, + cparams: &RevoluteJoint, + flipped: bool, + ) -> Self { + let anchor2; + let anchor1; + let basis1; + + if flipped { + anchor1 = rb1.position * cparams.local_anchor2; + anchor2 = rb2.position * cparams.local_anchor1; + basis1 = Matrix3x2::from_columns(&[ + rb1.position * cparams.basis2[0], + rb1.position * cparams.basis2[1], + ]); + } else { + anchor1 = rb1.position * cparams.local_anchor1; + anchor2 = rb2.position * cparams.local_anchor2; + basis1 = Matrix3x2::from_columns(&[ + rb1.position * cparams.basis1[0], + rb1.position * cparams.basis1[1], + ]); + }; + + // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) + // .unwrap_or(Rotation::identity()) + // .to_rotation_matrix() + // .into_inner(); + // let basis2 = /*r21 * */ basis1; + let im2 = rb2.mass_properties.inv_mass; + let ii2 = rb2.world_inv_inertia_sqrt.squared(); + let r1 = anchor1 - rb1.world_com; + let r2 = anchor2 - rb2.world_com; + let r2_mat = r2.gcross_matrix(); + + let mut lhs = Matrix5::zeros(); + let lhs00 = ii2.quadform(&r2_mat).add_diagonal(im2); + let lhs10 = basis1.tr_mul(&(ii2 * r2_mat)); + let lhs11 = ii2.quadform3x2(&basis1).into_matrix(); + + // Note that cholesky won't read the upper-right part + // of lhs so we don't have to fill it. + lhs.fixed_slice_mut::(0, 0) + .copy_from(&lhs00.into_matrix()); + lhs.fixed_slice_mut::(3, 0).copy_from(&lhs10); + lhs.fixed_slice_mut::(3, 3).copy_from(&lhs11); + + let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); + + let lin_rhs = rb2.linvel + rb2.angvel.gcross(r2) - rb1.linvel - rb1.angvel.gcross(r1); + let ang_rhs = basis1.tr_mul(&(rb2.angvel - rb1.angvel)); + let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y); + + RevoluteVelocityGroundConstraint { + joint_id, + mj_lambda2: rb2.active_set_offset, + im2, + ii2_sqrt: rb2.world_inv_inertia_sqrt, + impulse: cparams.impulse * params.warmstart_coeff, + basis1, + inv_lhs, + rhs, + r2, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); + let ang_impulse = self.basis1 * self.impulse.fixed_rows::(3).into_owned(); + + mj_lambda2.linear -= self.im2 * lin_impulse; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2); + let ang_dvel = self.basis1.tr_mul(&ang_vel2); + let rhs = + Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs; + let impulse = self.inv_lhs * rhs; + self.impulse += impulse; + let lin_impulse = impulse.fixed_rows::(0).into_owned(); + let ang_impulse = self.basis1 * impulse.fixed_rows::(3).into_owned(); + + mj_lambda2.linear -= self.im2 * lin_impulse; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + // FIXME: duplicated code with the non-ground constraint. + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + let joint = &mut joints_all[self.joint_id].weight; + if let JointParams::RevoluteJoint(revolute) = &mut joint.params { + revolute.impulse = self.impulse; + } + } +} diff --git a/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs new file mode 100644 index 000000000..5eeac181e --- /dev/null +++ b/src/dynamics/solver/joint_constraint/revolute_velocity_constraint_wide.rs @@ -0,0 +1,397 @@ +use simba::simd::SimdValue; + +use crate::dynamics::solver::DeltaVel; +use crate::dynamics::{ + IntegrationParameters, JointGraphEdge, JointIndex, JointParams, RevoluteJoint, RigidBody, +}; +use crate::math::{AngVector, AngularInertia, Isometry, Point, SimdFloat, Vector, SIMD_WIDTH}; +use crate::utils::{WAngularInertia, WCross, WCrossMatrix}; +use na::{Cholesky, Matrix3x2, Matrix5, Vector5, U2, U3}; + +#[derive(Debug)] +pub(crate) struct WRevoluteVelocityConstraint { + mj_lambda1: [usize; SIMD_WIDTH], + mj_lambda2: [usize; SIMD_WIDTH], + + joint_id: [JointIndex; SIMD_WIDTH], + + r1: Vector, + r2: Vector, + + inv_lhs: Matrix5, + rhs: Vector5, + impulse: Vector5, + + basis1: Matrix3x2, + + im1: SimdFloat, + im2: SimdFloat, + + ii1_sqrt: AngularInertia, + ii2_sqrt: AngularInertia, +} + +impl WRevoluteVelocityConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: [JointIndex; SIMD_WIDTH], + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&RevoluteJoint; SIMD_WIDTH], + ) -> Self { + let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); + let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); + let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii1_sqrt = AngularInertia::::from( + array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ); + let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; + + let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); + let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); + let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( + array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ); + let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + + let local_anchor1 = Point::from(array![|ii| cparams[ii].local_anchor1; SIMD_WIDTH]); + let local_anchor2 = Point::from(array![|ii| cparams[ii].local_anchor2; SIMD_WIDTH]); + let local_basis1 = [ + Vector::from(array![|ii| cparams[ii].basis1[0]; SIMD_WIDTH]), + Vector::from(array![|ii| cparams[ii].basis1[1]; SIMD_WIDTH]), + ]; + let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + + let anchor1 = position1 * local_anchor1; + let anchor2 = position2 * local_anchor2; + let basis1 = + Matrix3x2::from_columns(&[position1 * local_basis1[0], position1 * local_basis1[1]]); + + // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) + // .unwrap_or(Rotation::identity()) + // .to_rotation_matrix() + // .into_inner(); + // let basis2 = r21 * basis1; + // NOTE: to simplify, we use basis2 = basis1. + // Though we may want to test if that does not introduce any instability. + let ii1 = ii1_sqrt.squared(); + let r1 = anchor1 - world_com1; + let r1_mat = r1.gcross_matrix(); + + let ii2 = ii2_sqrt.squared(); + let r2 = anchor2 - world_com2; + let r2_mat = r2.gcross_matrix(); + + let mut lhs = Matrix5::zeros(); + let lhs00 = + ii2.quadform(&r2_mat).add_diagonal(im2) + ii1.quadform(&r1_mat).add_diagonal(im1); + let lhs10 = basis1.tr_mul(&(ii2 * r2_mat + ii1 * r1_mat)); + let lhs11 = (ii1 + ii2).quadform3x2(&basis1).into_matrix(); + + // Note that cholesky won't read the upper-right part + // of lhs so we don't have to fill it. + lhs.fixed_slice_mut::(0, 0) + .copy_from(&lhs00.into_matrix()); + lhs.fixed_slice_mut::(3, 0).copy_from(&lhs10); + lhs.fixed_slice_mut::(3, 3).copy_from(&lhs11); + + let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); + + let lin_rhs = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1); + let ang_rhs = basis1.tr_mul(&(angvel2 - angvel1)); + let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y); + + WRevoluteVelocityConstraint { + joint_id, + mj_lambda1, + mj_lambda2, + im1, + ii1_sqrt, + basis1, + im2, + ii2_sqrt, + impulse: impulse * SimdFloat::splat(params.warmstart_coeff), + inv_lhs, + rhs, + r1, + r2, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], + ), + }; + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); + let ang_impulse = self.basis1 * self.impulse.fixed_rows::(3).into_owned(); + + mj_lambda1.linear += lin_impulse * self.im1; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + + mj_lambda2.linear -= lin_impulse * self.im2; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); + mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii); + } + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], + ), + }; + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let ang_vel1 = self.ii1_sqrt.transform_vector(mj_lambda1.angular); + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2) + - mj_lambda1.linear + - ang_vel1.gcross(self.r1); + let ang_dvel = self.basis1.tr_mul(&(ang_vel2 - ang_vel1)); + let rhs = + Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs; + let impulse = self.inv_lhs * rhs; + self.impulse += impulse; + let lin_impulse = impulse.fixed_rows::(0).into_owned(); + let ang_impulse = self.basis1 * impulse.fixed_rows::(3).into_owned(); + + mj_lambda1.linear += lin_impulse * self.im1; + mj_lambda1.angular += self + .ii1_sqrt + .transform_vector(ang_impulse + self.r1.gcross(lin_impulse)); + + mj_lambda2.linear -= lin_impulse * self.im2; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); + mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii); + } + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + for ii in 0..SIMD_WIDTH { + let joint = &mut joints_all[self.joint_id[ii]].weight; + if let JointParams::RevoluteJoint(rev) = &mut joint.params { + rev.impulse = self.impulse.extract(ii) + } + } + } +} + +#[derive(Debug)] +pub(crate) struct WRevoluteVelocityGroundConstraint { + mj_lambda2: [usize; SIMD_WIDTH], + + joint_id: [JointIndex; SIMD_WIDTH], + + r2: Vector, + + inv_lhs: Matrix5, + rhs: Vector5, + impulse: Vector5, + + basis1: Matrix3x2, + + im2: SimdFloat, + + ii2_sqrt: AngularInertia, +} + +impl WRevoluteVelocityGroundConstraint { + pub fn from_params( + params: &IntegrationParameters, + joint_id: [JointIndex; SIMD_WIDTH], + rbs1: [&RigidBody; SIMD_WIDTH], + rbs2: [&RigidBody; SIMD_WIDTH], + cparams: [&RevoluteJoint; SIMD_WIDTH], + flipped: [bool; SIMD_WIDTH], + ) -> Self { + let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); + let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + let world_com1 = Point::from(array![|ii| rbs1[ii].world_com; SIMD_WIDTH]); + + let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); + let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + let world_com2 = Point::from(array![|ii| rbs2[ii].world_com; SIMD_WIDTH]); + let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2_sqrt = AngularInertia::::from( + array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH], + ); + let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + let impulse = Vector5::from(array![|ii| cparams[ii].impulse; SIMD_WIDTH]); + + let local_anchor1 = Point::from( + array![|ii| if flipped[ii] { cparams[ii].local_anchor2 } else { cparams[ii].local_anchor1 }; SIMD_WIDTH], + ); + let local_anchor2 = Point::from( + array![|ii| if flipped[ii] { cparams[ii].local_anchor1 } else { cparams[ii].local_anchor2 }; SIMD_WIDTH], + ); + let basis1 = Matrix3x2::from_columns(&[ + position1 + * Vector::from( + array![|ii| if flipped[ii] { cparams[ii].basis2[0] } else { cparams[ii].basis1[0] }; SIMD_WIDTH], + ), + position1 + * Vector::from( + array![|ii| if flipped[ii] { cparams[ii].basis2[1] } else { cparams[ii].basis1[1] }; SIMD_WIDTH], + ), + ]); + + let anchor1 = position1 * local_anchor1; + let anchor2 = position2 * local_anchor2; + + // let r21 = Rotation::rotation_between_axis(&axis1, &axis2) + // .unwrap_or(Rotation::identity()) + // .to_rotation_matrix() + // .into_inner(); + // let basis2 = /*r21 * */ basis1; + let ii2 = ii2_sqrt.squared(); + let r1 = anchor1 - world_com1; + let r2 = anchor2 - world_com2; + let r2_mat = r2.gcross_matrix(); + + let mut lhs = Matrix5::zeros(); + let lhs00 = ii2.quadform(&r2_mat).add_diagonal(im2); + let lhs10 = basis1.tr_mul(&(ii2 * r2_mat)); + let lhs11 = ii2.quadform3x2(&basis1).into_matrix(); + + // Note that cholesky won't read the upper-right part + // of lhs so we don't have to fill it. + lhs.fixed_slice_mut::(0, 0) + .copy_from(&lhs00.into_matrix()); + lhs.fixed_slice_mut::(3, 0).copy_from(&lhs10); + lhs.fixed_slice_mut::(3, 3).copy_from(&lhs11); + + let inv_lhs = Cholesky::new_unchecked(lhs).inverse(); + + let lin_rhs = linvel2 + angvel2.gcross(r2) - linvel1 - angvel1.gcross(r1); + let ang_rhs = basis1.tr_mul(&(angvel2 - angvel1)); + let rhs = Vector5::new(lin_rhs.x, lin_rhs.y, lin_rhs.z, ang_rhs.x, ang_rhs.y); + + WRevoluteVelocityGroundConstraint { + joint_id, + mj_lambda2, + im2, + ii2_sqrt, + impulse: impulse * SimdFloat::splat(params.warmstart_coeff), + basis1, + inv_lhs, + rhs, + r2, + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let lin_impulse = self.impulse.fixed_rows::(0).into_owned(); + let ang_impulse = self.basis1 * self.impulse.fixed_rows::(3).into_owned(); + + mj_lambda2.linear -= lin_impulse * self.im2; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let ang_vel2 = self.ii2_sqrt.transform_vector(mj_lambda2.angular); + let lin_dvel = mj_lambda2.linear + ang_vel2.gcross(self.r2); + let ang_dvel = self.basis1.tr_mul(&ang_vel2); + let rhs = + Vector5::new(lin_dvel.x, lin_dvel.y, lin_dvel.z, ang_dvel.x, ang_dvel.y) + self.rhs; + let impulse = self.inv_lhs * rhs; + self.impulse += impulse; + let lin_impulse = impulse.fixed_rows::(0).into_owned(); + let ang_impulse = self.basis1 * impulse.fixed_rows::(3).into_owned(); + + mj_lambda2.linear -= lin_impulse * self.im2; + mj_lambda2.angular -= self + .ii2_sqrt + .transform_vector(ang_impulse + self.r2.gcross(lin_impulse)); + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + // FIXME: duplicated code with the non-ground constraint. + pub fn writeback_impulses(&self, joints_all: &mut [JointGraphEdge]) { + for ii in 0..SIMD_WIDTH { + let joint = &mut joints_all[self.joint_id[ii]].weight; + if let JointParams::RevoluteJoint(rev) = &mut joint.params { + rev.impulse = self.impulse.extract(ii) + } + } + } +} diff --git a/src/dynamics/solver/mod.rs b/src/dynamics/solver/mod.rs new file mode 100644 index 000000000..4d80f562e --- /dev/null +++ b/src/dynamics/solver/mod.rs @@ -0,0 +1,56 @@ +#[cfg(not(feature = "parallel"))] +pub(crate) use self::island_solver::IslandSolver; +#[cfg(feature = "parallel")] +pub(crate) use self::parallel_island_solver::{ParallelIslandSolver, ThreadContext}; +#[cfg(feature = "parallel")] +pub(self) use self::parallel_position_solver::ParallelPositionSolver; +#[cfg(feature = "parallel")] +pub(self) use self::parallel_velocity_solver::ParallelVelocitySolver; +#[cfg(not(feature = "parallel"))] +pub(self) use self::position_solver::PositionSolver; +#[cfg(not(feature = "parallel"))] +pub(self) use self::velocity_solver::VelocitySolver; +pub(self) use delta_vel::DeltaVel; +pub(self) use interaction_groups::*; +pub(self) use joint_constraint::*; +pub(self) use position_constraint::*; +#[cfg(feature = "simd-is-enabled")] +pub(self) use position_constraint_wide::*; +pub(self) use position_ground_constraint::*; +#[cfg(feature = "simd-is-enabled")] +pub(self) use position_ground_constraint_wide::*; +pub(self) use velocity_constraint::*; +#[cfg(feature = "simd-is-enabled")] +pub(self) use velocity_constraint_wide::*; +pub(self) use velocity_ground_constraint::*; +#[cfg(feature = "simd-is-enabled")] +pub(self) use velocity_ground_constraint_wide::*; + +mod categorization; +mod delta_vel; +mod interaction_groups; +#[cfg(not(feature = "parallel"))] +mod island_solver; +mod joint_constraint; +#[cfg(feature = "parallel")] +mod parallel_island_solver; +#[cfg(feature = "parallel")] +mod parallel_position_solver; +#[cfg(feature = "parallel")] +mod parallel_velocity_solver; +mod position_constraint; +#[cfg(feature = "simd-is-enabled")] +mod position_constraint_wide; +mod position_ground_constraint; +#[cfg(feature = "simd-is-enabled")] +mod position_ground_constraint_wide; +#[cfg(not(feature = "parallel"))] +mod position_solver; +mod velocity_constraint; +#[cfg(feature = "simd-is-enabled")] +mod velocity_constraint_wide; +mod velocity_ground_constraint; +#[cfg(feature = "simd-is-enabled")] +mod velocity_ground_constraint_wide; +#[cfg(not(feature = "parallel"))] +mod velocity_solver; diff --git a/src/dynamics/solver/parallel_island_solver.rs b/src/dynamics/solver/parallel_island_solver.rs new file mode 100644 index 000000000..edbfa409b --- /dev/null +++ b/src/dynamics/solver/parallel_island_solver.rs @@ -0,0 +1,259 @@ +use super::{DeltaVel, ParallelInteractionGroups, ParallelVelocitySolver}; +use crate::dynamics::solver::ParallelPositionSolver; +use crate::dynamics::{IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet}; +use crate::geometry::{ContactManifold, ContactManifoldIndex}; +use crate::math::Isometry; +use crate::utils::WAngularInertia; +use rayon::Scope; +use std::sync::atomic::{AtomicUsize, Ordering}; + +#[macro_export] +#[doc(hidden)] +macro_rules! concurrent_loop { + (let batch_size = $batch_size: expr; + for $elt: ident in $array: ident[$index_stream:expr,$index_count:expr] $f: expr) => { + let max_index = $array.len(); + + if max_index > 0 { + loop { + let start_index = $index_stream.fetch_add($batch_size, Ordering::SeqCst); + if start_index > max_index { + break; + } + + let end_index = (start_index + $batch_size).min(max_index); + for $elt in &$array[start_index..end_index] { + $f + } + + $index_count.fetch_add(end_index - start_index, Ordering::SeqCst); + } + } + }; + + (let batch_size = $batch_size: expr; + for $elt: ident in $array: ident[$index_stream:expr] $f: expr) => { + let max_index = $array.len(); + + if max_index > 0 { + loop { + let start_index = $index_stream.fetch_add($batch_size, Ordering::SeqCst); + if start_index > max_index { + break; + } + + let end_index = (start_index + $batch_size).min(max_index); + for $elt in &$array[start_index..end_index] { + $f + } + } + } + }; +} + +pub(crate) struct ThreadContext { + pub batch_size: usize, + // Velocity solver. + pub constraint_initialization_index: AtomicUsize, + pub num_initialized_constraints: AtomicUsize, + pub joint_constraint_initialization_index: AtomicUsize, + pub num_initialized_joint_constraints: AtomicUsize, + pub warmstart_contact_index: AtomicUsize, + pub num_warmstarted_contacts: AtomicUsize, + pub warmstart_joint_index: AtomicUsize, + pub num_warmstarted_joints: AtomicUsize, + pub solve_interaction_index: AtomicUsize, + pub num_solved_interactions: AtomicUsize, + pub impulse_writeback_index: AtomicUsize, + pub joint_writeback_index: AtomicUsize, + pub body_integration_index: AtomicUsize, + pub num_integrated_bodies: AtomicUsize, + // Position solver. + pub position_constraint_initialization_index: AtomicUsize, + pub num_initialized_position_constraints: AtomicUsize, + pub position_joint_constraint_initialization_index: AtomicUsize, + pub num_initialized_position_joint_constraints: AtomicUsize, + pub solve_position_interaction_index: AtomicUsize, + pub num_solved_position_interactions: AtomicUsize, + pub position_writeback_index: AtomicUsize, +} + +impl ThreadContext { + pub fn new(batch_size: usize) -> Self { + ThreadContext { + batch_size, // TODO perhaps there is some optimal value we can compute depending on the island size? + constraint_initialization_index: AtomicUsize::new(0), + num_initialized_constraints: AtomicUsize::new(0), + joint_constraint_initialization_index: AtomicUsize::new(0), + num_initialized_joint_constraints: AtomicUsize::new(0), + num_warmstarted_contacts: AtomicUsize::new(0), + warmstart_contact_index: AtomicUsize::new(0), + num_warmstarted_joints: AtomicUsize::new(0), + warmstart_joint_index: AtomicUsize::new(0), + solve_interaction_index: AtomicUsize::new(0), + num_solved_interactions: AtomicUsize::new(0), + impulse_writeback_index: AtomicUsize::new(0), + joint_writeback_index: AtomicUsize::new(0), + body_integration_index: AtomicUsize::new(0), + num_integrated_bodies: AtomicUsize::new(0), + position_constraint_initialization_index: AtomicUsize::new(0), + num_initialized_position_constraints: AtomicUsize::new(0), + position_joint_constraint_initialization_index: AtomicUsize::new(0), + num_initialized_position_joint_constraints: AtomicUsize::new(0), + solve_position_interaction_index: AtomicUsize::new(0), + num_solved_position_interactions: AtomicUsize::new(0), + position_writeback_index: AtomicUsize::new(0), + } + } + + pub fn lock_until_ge(val: &AtomicUsize, target: usize) { + if target > 0 { + // let backoff = crossbeam::utils::Backoff::new(); + std::sync::atomic::fence(Ordering::SeqCst); + while val.load(Ordering::Relaxed) < target { + // backoff.spin(); + // std::thread::yield_now(); + } + } + } +} + +pub struct ParallelIslandSolver { + mj_lambdas: Vec>, + positions: Vec>, + parallel_groups: ParallelInteractionGroups, + parallel_joint_groups: ParallelInteractionGroups, + parallel_velocity_solver: ParallelVelocitySolver, + parallel_position_solver: ParallelPositionSolver, + thread: ThreadContext, +} + +impl ParallelIslandSolver { + pub fn new() -> Self { + Self { + mj_lambdas: Vec::new(), + positions: Vec::new(), + parallel_groups: ParallelInteractionGroups::new(), + parallel_joint_groups: ParallelInteractionGroups::new(), + parallel_velocity_solver: ParallelVelocitySolver::new(), + parallel_position_solver: ParallelPositionSolver::new(), + thread: ThreadContext::new(8), + } + } + + pub fn solve_island<'s>( + &'s mut self, + scope: &Scope<'s>, + island_id: usize, + params: &'s IntegrationParameters, + bodies: &'s mut RigidBodySet, + manifolds: &'s mut Vec<&'s mut ContactManifold>, + manifold_indices: &'s [ContactManifoldIndex], + joints: &'s mut Vec, + joint_indices: &[JointIndex], + ) { + let num_threads = rayon::current_num_threads(); + let num_task_per_island = num_threads; // (num_threads / num_islands).max(1); // TODO: not sure this is the best value. Also, perhaps it is better to interleave tasks of each island? + self.thread = ThreadContext::new(8); // TODO: could we compute some kind of optimal value here? + self.parallel_groups + .group_interactions(island_id, bodies, manifolds, manifold_indices); + self.parallel_joint_groups + .group_interactions(island_id, bodies, joints, joint_indices); + self.parallel_velocity_solver.init_constraint_groups( + island_id, + bodies, + manifolds, + &self.parallel_groups, + joints, + &self.parallel_joint_groups, + ); + self.parallel_position_solver.init_constraint_groups( + island_id, + bodies, + manifolds, + &self.parallel_groups, + joints, + &self.parallel_joint_groups, + ); + + self.mj_lambdas.clear(); + self.mj_lambdas + .resize(bodies.active_island(island_id).len(), DeltaVel::zero()); + self.positions.clear(); + self.positions + .resize(bodies.active_island(island_id).len(), Isometry::identity()); + + for _ in 0..num_task_per_island { + // We use AtomicPtr because it is Send+Sync while *mut is not. + // See https://internals.rust-lang.org/t/shouldnt-pointers-be-send-sync-or/8818 + let thread = &self.thread; + let mj_lambdas = std::sync::atomic::AtomicPtr::new(&mut self.mj_lambdas as *mut _); + let positions = std::sync::atomic::AtomicPtr::new(&mut self.positions as *mut _); + let bodies = std::sync::atomic::AtomicPtr::new(bodies as *mut _); + let manifolds = std::sync::atomic::AtomicPtr::new(manifolds as *mut _); + let joints = std::sync::atomic::AtomicPtr::new(joints as *mut _); + let parallel_velocity_solver = + std::sync::atomic::AtomicPtr::new(&mut self.parallel_velocity_solver as *mut _); + let parallel_position_solver = + std::sync::atomic::AtomicPtr::new(&mut self.parallel_position_solver as *mut _); + + scope.spawn(move |_| { + // Transmute *mut -> &mut + let mj_lambdas: &mut Vec> = + unsafe { std::mem::transmute(mj_lambdas.load(Ordering::Relaxed)) }; + let positions: &mut Vec> = + unsafe { std::mem::transmute(positions.load(Ordering::Relaxed)) }; + let bodies: &mut RigidBodySet = + unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) }; + let manifolds: &mut Vec<&mut ContactManifold> = + unsafe { std::mem::transmute(manifolds.load(Ordering::Relaxed)) }; + let joints: &mut Vec = + unsafe { std::mem::transmute(joints.load(Ordering::Relaxed)) }; + let parallel_velocity_solver: &mut ParallelVelocitySolver = unsafe { + std::mem::transmute(parallel_velocity_solver.load(Ordering::Relaxed)) + }; + let parallel_position_solver: &mut ParallelPositionSolver = unsafe { + std::mem::transmute(parallel_position_solver.load(Ordering::Relaxed)) + }; + + enable_flush_to_zero!(); // Ensure this is enabled on each thread. + parallel_velocity_solver.fill_constraints(&thread, params, bodies, manifolds, joints); + parallel_position_solver.fill_constraints(&thread, params, bodies, manifolds, joints); + parallel_velocity_solver + .solve_constraints(&thread, params, manifolds, joints, mj_lambdas); + + // Write results back to rigid bodies and integrate velocities. + let island_range = bodies.active_island_range(island_id); + let active_bodies = &bodies.active_dynamic_set[island_range]; + let bodies = &mut bodies.bodies; + + concurrent_loop! { + let batch_size = thread.batch_size; + for handle in active_bodies[thread.body_integration_index, thread.num_integrated_bodies] { + let rb = &mut bodies[*handle]; + let dvel = mj_lambdas[rb.active_set_offset]; + rb.linvel += dvel.linear; + rb.angvel += rb.world_inv_inertia_sqrt.transform_vector(dvel.angular); + rb.integrate(params.dt()); + positions[rb.active_set_offset] = rb.position; + } + } + + // We need to way for every body to be integrated because it also + // initialized `positions` to the updated values. + ThreadContext::lock_until_ge(&thread.num_integrated_bodies, active_bodies.len()); + + parallel_position_solver.solve_constraints(&thread, params, positions); + + // Write results back to rigid bodies. + concurrent_loop! { + let batch_size = thread.batch_size; + for handle in active_bodies[thread.position_writeback_index] { + let rb = &mut bodies[*handle]; + rb.set_position(positions[rb.active_set_offset]); + } + } + }) + } + } +} diff --git a/src/dynamics/solver/parallel_position_solver.rs b/src/dynamics/solver/parallel_position_solver.rs new file mode 100644 index 000000000..b7a78769e --- /dev/null +++ b/src/dynamics/solver/parallel_position_solver.rs @@ -0,0 +1,582 @@ +use super::ParallelInteractionGroups; +use super::{AnyJointPositionConstraint, AnyPositionConstraint, ThreadContext}; +use crate::dynamics::solver::categorization::{categorize_joints, categorize_position_contacts}; +use crate::dynamics::solver::{InteractionGroups, PositionConstraint, PositionGroundConstraint}; +use crate::dynamics::{IntegrationParameters, JointGraphEdge, RigidBodySet}; +use crate::geometry::ContactManifold; +use crate::math::Isometry; +#[cfg(feature = "simd-is-enabled")] +use crate::{ + dynamics::solver::{WPositionConstraint, WPositionGroundConstraint}, + simd::SIMD_WIDTH, +}; +use std::sync::atomic::Ordering; + +pub(crate) enum PositionConstraintDesc { + NongroundNongrouped(usize), + GroundNongrouped(usize), + #[cfg(feature = "simd-is-enabled")] + NongroundGrouped([usize; SIMD_WIDTH]), + #[cfg(feature = "simd-is-enabled")] + GroundGrouped([usize; SIMD_WIDTH]), +} + +pub(crate) struct ParallelPositionSolverContactPart { + pub point_point: Vec, + pub plane_point: Vec, + pub ground_point_point: Vec, + pub ground_plane_point: Vec, + pub interaction_groups: InteractionGroups, + pub ground_interaction_groups: InteractionGroups, + pub constraints: Vec, + pub constraint_descs: Vec<(usize, PositionConstraintDesc)>, + pub parallel_desc_groups: Vec, +} + +pub(crate) struct ParallelPositionSolverJointPart { + pub not_ground_interactions: Vec, + pub ground_interactions: Vec, + pub interaction_groups: InteractionGroups, + pub ground_interaction_groups: InteractionGroups, + pub constraints: Vec, + pub constraint_descs: Vec<(usize, PositionConstraintDesc)>, + pub parallel_desc_groups: Vec, +} + +impl ParallelPositionSolverContactPart { + pub fn new() -> Self { + Self { + point_point: Vec::new(), + plane_point: Vec::new(), + ground_point_point: Vec::new(), + ground_plane_point: Vec::new(), + interaction_groups: InteractionGroups::new(), + ground_interaction_groups: InteractionGroups::new(), + constraints: Vec::new(), + constraint_descs: Vec::new(), + parallel_desc_groups: Vec::new(), + } + } +} +impl ParallelPositionSolverJointPart { + pub fn new() -> Self { + Self { + not_ground_interactions: Vec::new(), + ground_interactions: Vec::new(), + interaction_groups: InteractionGroups::new(), + ground_interaction_groups: InteractionGroups::new(), + constraints: Vec::new(), + constraint_descs: Vec::new(), + parallel_desc_groups: Vec::new(), + } + } +} + +impl ParallelPositionSolverJointPart { + pub fn init_constraints_groups( + &mut self, + island_id: usize, + bodies: &RigidBodySet, + joints: &mut [JointGraphEdge], + joint_groups: &ParallelInteractionGroups, + ) { + let mut total_num_constraints = 0; + let num_groups = joint_groups.num_groups(); + + self.interaction_groups.clear_groups(); + self.ground_interaction_groups.clear_groups(); + self.parallel_desc_groups.clear(); + self.constraint_descs.clear(); + self.parallel_desc_groups.push(0); + + for i in 0..num_groups { + let group = joint_groups.group(i); + + self.not_ground_interactions.clear(); + self.ground_interactions.clear(); + categorize_joints( + bodies, + joints, + group, + &mut self.ground_interactions, + &mut self.not_ground_interactions, + ); + + #[cfg(feature = "simd-is-enabled")] + let start_grouped = self.interaction_groups.grouped_interactions.len(); + let start_nongrouped = self.interaction_groups.nongrouped_interactions.len(); + #[cfg(feature = "simd-is-enabled")] + let start_grouped_ground = self.ground_interaction_groups.grouped_interactions.len(); + let start_nongrouped_ground = + self.ground_interaction_groups.nongrouped_interactions.len(); + + self.interaction_groups.group_joints( + island_id, + bodies, + joints, + &self.not_ground_interactions, + ); + self.ground_interaction_groups.group_joints( + island_id, + bodies, + joints, + &self.ground_interactions, + ); + + // Compute constraint indices. + for interaction_i in + &self.interaction_groups.nongrouped_interactions[start_nongrouped..] + { + let joint = &mut joints[*interaction_i].weight; + joint.position_constraint_index = total_num_constraints; + self.constraint_descs.push(( + total_num_constraints, + PositionConstraintDesc::NongroundNongrouped(*interaction_i), + )); + total_num_constraints += + AnyJointPositionConstraint::num_active_constraints(joint, false); + } + + #[cfg(feature = "simd-is-enabled")] + for interaction_i in + self.interaction_groups.grouped_interactions[start_grouped..].chunks(SIMD_WIDTH) + { + let joint = &mut joints[interaction_i[0]].weight; + joint.position_constraint_index = total_num_constraints; + self.constraint_descs.push(( + total_num_constraints, + PositionConstraintDesc::NongroundGrouped( + array![|ii| interaction_i[ii]; SIMD_WIDTH], + ), + )); + total_num_constraints += + AnyJointPositionConstraint::num_active_constraints(joint, true); + } + + for interaction_i in + &self.ground_interaction_groups.nongrouped_interactions[start_nongrouped_ground..] + { + let joint = &mut joints[*interaction_i].weight; + joint.position_constraint_index = total_num_constraints; + self.constraint_descs.push(( + total_num_constraints, + PositionConstraintDesc::GroundNongrouped(*interaction_i), + )); + total_num_constraints += + AnyJointPositionConstraint::num_active_constraints(joint, false); + } + + #[cfg(feature = "simd-is-enabled")] + for interaction_i in self.ground_interaction_groups.grouped_interactions + [start_grouped_ground..] + .chunks(SIMD_WIDTH) + { + let joint = &mut joints[interaction_i[0]].weight; + joint.position_constraint_index = total_num_constraints; + self.constraint_descs.push(( + total_num_constraints, + PositionConstraintDesc::GroundGrouped( + array![|ii| interaction_i[ii]; SIMD_WIDTH], + ), + )); + total_num_constraints += + AnyJointPositionConstraint::num_active_constraints(joint, true); + } + + self.parallel_desc_groups.push(self.constraint_descs.len()); + } + + // Resize the constraints set. + self.constraints.clear(); + self.constraints + .resize_with(total_num_constraints, || AnyJointPositionConstraint::Empty) + } + + fn fill_constraints( + &mut self, + thread: &ThreadContext, + bodies: &RigidBodySet, + joints_all: &[JointGraphEdge], + ) { + let descs = &self.constraint_descs; + + crate::concurrent_loop! { + let batch_size = thread.batch_size; + for desc in descs[thread.position_joint_constraint_initialization_index, thread.num_initialized_position_joint_constraints] { + match &desc.1 { + PositionConstraintDesc::NongroundNongrouped(joint_id) => { + let joint = &joints_all[*joint_id].weight; + let constraint = AnyJointPositionConstraint::from_joint( + joint, + bodies, + ); + self.constraints[joint.position_constraint_index] = constraint; + } + PositionConstraintDesc::GroundNongrouped(joint_id) => { + let joint = &joints_all[*joint_id].weight; + let constraint = AnyJointPositionConstraint::from_joint_ground( + joint, + bodies, + ); + self.constraints[joint.position_constraint_index] = constraint; + } + #[cfg(feature = "simd-is-enabled")] + PositionConstraintDesc::NongroundGrouped(joint_id) => { + let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH]; + if let Some(constraint) = AnyJointPositionConstraint::from_wide_joint( + joints, bodies, + ) { + self.constraints[joints[0].position_constraint_index] = constraint + } else { + for ii in 0..SIMD_WIDTH { + let constraint = AnyJointPositionConstraint::from_joint(joints[ii], bodies); + self.constraints[joints[0].position_constraint_index + ii] = constraint; + } + } + } + #[cfg(feature = "simd-is-enabled")] + PositionConstraintDesc::GroundGrouped(joint_id) => { + let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH]; + if let Some(constraint) = AnyJointPositionConstraint::from_wide_joint_ground( + joints, bodies, + ) { + self.constraints[joints[0].position_constraint_index] = constraint + } else { + for ii in 0..SIMD_WIDTH { + let constraint = AnyJointPositionConstraint::from_joint_ground(joints[ii], bodies); + self.constraints[joints[0].position_constraint_index + ii] = constraint; + } + } + } + } + } + } + } +} + +impl ParallelPositionSolverContactPart { + pub fn init_constraints_groups( + &mut self, + island_id: usize, + bodies: &RigidBodySet, + manifolds: &mut [&mut ContactManifold], + manifold_groups: &ParallelInteractionGroups, + ) { + let mut total_num_constraints = 0; + let num_groups = manifold_groups.num_groups(); + + self.interaction_groups.clear_groups(); + self.ground_interaction_groups.clear_groups(); + self.parallel_desc_groups.clear(); + self.constraint_descs.clear(); + self.parallel_desc_groups.push(0); + + for i in 0..num_groups { + let group = manifold_groups.group(i); + + self.ground_point_point.clear(); + self.ground_plane_point.clear(); + self.point_point.clear(); + self.plane_point.clear(); + categorize_position_contacts( + bodies, + manifolds, + group, + &mut self.ground_point_point, + &mut self.ground_plane_point, + &mut self.point_point, + &mut self.plane_point, + ); + + #[cfg(feature = "simd-is-enabled")] + let start_grouped = self.interaction_groups.grouped_interactions.len(); + let start_nongrouped = self.interaction_groups.nongrouped_interactions.len(); + #[cfg(feature = "simd-is-enabled")] + let start_grouped_ground = self.ground_interaction_groups.grouped_interactions.len(); + let start_nongrouped_ground = + self.ground_interaction_groups.nongrouped_interactions.len(); + + self.interaction_groups.group_manifolds( + island_id, + bodies, + manifolds, + &self.point_point, + ); + self.interaction_groups.group_manifolds( + island_id, + bodies, + manifolds, + &self.plane_point, + ); + self.ground_interaction_groups.group_manifolds( + island_id, + bodies, + manifolds, + &self.ground_point_point, + ); + self.ground_interaction_groups.group_manifolds( + island_id, + bodies, + manifolds, + &self.ground_plane_point, + ); + + // Compute constraint indices. + for interaction_i in + &self.interaction_groups.nongrouped_interactions[start_nongrouped..] + { + let manifold = &mut *manifolds[*interaction_i]; + manifold.position_constraint_index = total_num_constraints; + self.constraint_descs.push(( + total_num_constraints, + PositionConstraintDesc::NongroundNongrouped(*interaction_i), + )); + total_num_constraints += PositionConstraint::num_active_constraints(manifold); + } + + #[cfg(feature = "simd-is-enabled")] + for interaction_i in + self.interaction_groups.grouped_interactions[start_grouped..].chunks(SIMD_WIDTH) + { + let manifold = &mut *manifolds[interaction_i[0]]; + manifold.position_constraint_index = total_num_constraints; + self.constraint_descs.push(( + total_num_constraints, + PositionConstraintDesc::NongroundGrouped( + array![|ii| interaction_i[ii]; SIMD_WIDTH], + ), + )); + total_num_constraints += PositionConstraint::num_active_constraints(manifold); + } + + for interaction_i in + &self.ground_interaction_groups.nongrouped_interactions[start_nongrouped_ground..] + { + let manifold = &mut *manifolds[*interaction_i]; + manifold.position_constraint_index = total_num_constraints; + self.constraint_descs.push(( + total_num_constraints, + PositionConstraintDesc::GroundNongrouped(*interaction_i), + )); + total_num_constraints += PositionConstraint::num_active_constraints(manifold); + } + + #[cfg(feature = "simd-is-enabled")] + for interaction_i in self.ground_interaction_groups.grouped_interactions + [start_grouped_ground..] + .chunks(SIMD_WIDTH) + { + let manifold = &mut *manifolds[interaction_i[0]]; + manifold.position_constraint_index = total_num_constraints; + self.constraint_descs.push(( + total_num_constraints, + PositionConstraintDesc::GroundGrouped( + array![|ii| interaction_i[ii]; SIMD_WIDTH], + ), + )); + total_num_constraints += PositionConstraint::num_active_constraints(manifold); + } + + self.parallel_desc_groups.push(self.constraint_descs.len()); + } + + // Resize the constraints set. + self.constraints.clear(); + self.constraints + .resize_with(total_num_constraints, || AnyPositionConstraint::Empty) + } + + fn fill_constraints( + &mut self, + thread: &ThreadContext, + params: &IntegrationParameters, + bodies: &RigidBodySet, + manifolds_all: &[&mut ContactManifold], + ) { + let descs = &self.constraint_descs; + + crate::concurrent_loop! { + let batch_size = thread.batch_size; + for desc in descs[thread.position_constraint_initialization_index, thread.num_initialized_position_constraints] { + match &desc.1 { + PositionConstraintDesc::NongroundNongrouped(manifold_id) => { + let manifold = &*manifolds_all[*manifold_id]; + PositionConstraint::generate( + params, + manifold, + bodies, + &mut self.constraints, + false, + ); + } + PositionConstraintDesc::GroundNongrouped(manifold_id) => { + let manifold = &*manifolds_all[*manifold_id]; + PositionGroundConstraint::generate( + params, + manifold, + bodies, + &mut self.constraints, + false, + ); + } + #[cfg(feature = "simd-is-enabled")] + PositionConstraintDesc::NongroundGrouped(manifold_id) => { + let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH]; + WPositionConstraint::generate( + params, + manifolds, + bodies, + &mut self.constraints, + false, + ); + } + #[cfg(feature = "simd-is-enabled")] + PositionConstraintDesc::GroundGrouped(manifold_id) => { + let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH]; + WPositionGroundConstraint::generate( + params, + manifolds, + bodies, + &mut self.constraints, + false, + ); + } + } + } + } + } +} + +pub(crate) struct ParallelPositionSolver { + part: ParallelPositionSolverContactPart, + joint_part: ParallelPositionSolverJointPart, +} + +impl ParallelPositionSolver { + pub fn new() -> Self { + Self { + part: ParallelPositionSolverContactPart::new(), + joint_part: ParallelPositionSolverJointPart::new(), + } + } + + pub fn init_constraint_groups( + &mut self, + island_id: usize, + bodies: &RigidBodySet, + manifolds: &mut [&mut ContactManifold], + manifold_groups: &ParallelInteractionGroups, + joints: &mut [JointGraphEdge], + joint_groups: &ParallelInteractionGroups, + ) { + self.part + .init_constraints_groups(island_id, bodies, manifolds, manifold_groups); + self.joint_part + .init_constraints_groups(island_id, bodies, joints, joint_groups); + } + + pub fn fill_constraints( + &mut self, + thread: &ThreadContext, + params: &IntegrationParameters, + bodies: &RigidBodySet, + manifolds: &[&mut ContactManifold], + joints: &[JointGraphEdge], + ) { + self.part + .fill_constraints(thread, params, bodies, manifolds); + self.joint_part.fill_constraints(thread, bodies, joints); + ThreadContext::lock_until_ge( + &thread.num_initialized_position_constraints, + self.part.constraint_descs.len(), + ); + ThreadContext::lock_until_ge( + &thread.num_initialized_position_joint_constraints, + self.joint_part.constraint_descs.len(), + ); + } + + pub fn solve_constraints( + &mut self, + thread: &ThreadContext, + params: &IntegrationParameters, + positions: &mut [Isometry], + ) { + if self.part.constraint_descs.len() == 0 { + return; + } + + /* + * Solve constraints. + */ + { + // Each thread will concurrently grab thread.batch_size constraint desc to + // solve. If the batch size is large enough for to cross the boundary of + // a palallel_desc_group, we have to wait util the current group is finished + // before starting the next one. + let mut start_index = thread + .solve_position_interaction_index + .fetch_add(thread.batch_size, Ordering::SeqCst); + let mut batch_size = thread.batch_size; + let contact_descs = &self.part.constraint_descs[..]; + let joint_descs = &self.joint_part.constraint_descs[..]; + let mut target_num_desc = 0; + let mut shift = 0; + + for _ in 0..params.max_position_iterations { + macro_rules! solve { + ($part: expr) => { + // Joint groups. + for group in $part.parallel_desc_groups.windows(2) { + let num_descs_in_group = group[1] - group[0]; + target_num_desc += num_descs_in_group; + + while start_index < group[1] { + let end_index = (start_index + batch_size).min(group[1]); + + let constraints = if end_index == $part.constraint_descs.len() { + &mut $part.constraints[$part.constraint_descs[start_index].0..] + } else { + &mut $part.constraints[$part.constraint_descs[start_index].0 + ..$part.constraint_descs[end_index].0] + }; + + for constraint in constraints { + constraint.solve(params, positions); + } + + let num_solved = end_index - start_index; + batch_size -= num_solved; + + thread + .num_solved_position_interactions + .fetch_add(num_solved, Ordering::SeqCst); + + if batch_size == 0 { + start_index = thread + .solve_position_interaction_index + .fetch_add(thread.batch_size, Ordering::SeqCst); + start_index -= shift; + batch_size = thread.batch_size; + } else { + start_index += num_solved; + } + } + ThreadContext::lock_until_ge( + &thread.num_solved_position_interactions, + target_num_desc, + ); + } + }; + } + + solve!(self.joint_part); + shift += joint_descs.len(); + start_index -= joint_descs.len(); + solve!(self.part); + shift += contact_descs.len(); + start_index -= contact_descs.len(); + } + } + } +} diff --git a/src/dynamics/solver/parallel_velocity_solver.rs b/src/dynamics/solver/parallel_velocity_solver.rs new file mode 100644 index 000000000..303a18fd4 --- /dev/null +++ b/src/dynamics/solver/parallel_velocity_solver.rs @@ -0,0 +1,485 @@ +use super::ParallelInteractionGroups; +use super::{AnyJointVelocityConstraint, AnyVelocityConstraint, DeltaVel, ThreadContext}; +use crate::dynamics::solver::categorization::{categorize_joints, categorize_velocity_contacts}; +use crate::dynamics::solver::{InteractionGroups, VelocityConstraint, VelocityGroundConstraint}; +use crate::dynamics::{IntegrationParameters, JointGraphEdge, RigidBodySet}; +use crate::geometry::ContactManifold; +#[cfg(feature = "simd-is-enabled")] +use crate::{ + dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint}, + simd::SIMD_WIDTH, +}; +use std::sync::atomic::Ordering; + +pub(crate) enum VelocityConstraintDesc { + NongroundNongrouped(usize), + GroundNongrouped(usize), + #[cfg(feature = "simd-is-enabled")] + NongroundGrouped([usize; SIMD_WIDTH]), + #[cfg(feature = "simd-is-enabled")] + GroundGrouped([usize; SIMD_WIDTH]), +} + +pub(crate) struct ParallelVelocitySolverPart { + pub not_ground_interactions: Vec, + pub ground_interactions: Vec, + pub interaction_groups: InteractionGroups, + pub ground_interaction_groups: InteractionGroups, + pub constraints: Vec, + pub constraint_descs: Vec<(usize, VelocityConstraintDesc)>, + pub parallel_desc_groups: Vec, +} + +impl ParallelVelocitySolverPart { + pub fn new() -> Self { + Self { + not_ground_interactions: Vec::new(), + ground_interactions: Vec::new(), + interaction_groups: InteractionGroups::new(), + ground_interaction_groups: InteractionGroups::new(), + constraints: Vec::new(), + constraint_descs: Vec::new(), + parallel_desc_groups: Vec::new(), + } + } +} + +macro_rules! impl_init_constraints_group { + ($Constraint: ty, $Interaction: ty, $categorize: ident, $group: ident, $num_active_constraints: path, $empty_constraint: expr $(, $weight: ident)*) => { + impl ParallelVelocitySolverPart<$Constraint> { + pub fn init_constraints_groups( + &mut self, + island_id: usize, + bodies: &RigidBodySet, + interactions: &mut [$Interaction], + interaction_groups: &ParallelInteractionGroups, + ) { + let mut total_num_constraints = 0; + let num_groups = interaction_groups.num_groups(); + + self.interaction_groups.clear_groups(); + self.ground_interaction_groups.clear_groups(); + self.parallel_desc_groups.clear(); + self.constraint_descs.clear(); + self.parallel_desc_groups.push(0); + + for i in 0..num_groups { + let group = interaction_groups.group(i); + + self.not_ground_interactions.clear(); + self.ground_interactions.clear(); + $categorize( + bodies, + interactions, + group, + &mut self.ground_interactions, + &mut self.not_ground_interactions, + ); + + #[cfg(feature = "simd-is-enabled")] + let start_grouped = self.interaction_groups.grouped_interactions.len(); + let start_nongrouped = self.interaction_groups.nongrouped_interactions.len(); + #[cfg(feature = "simd-is-enabled")] + let start_grouped_ground = self.ground_interaction_groups.grouped_interactions.len(); + let start_nongrouped_ground = self.ground_interaction_groups.nongrouped_interactions.len(); + + self.interaction_groups.$group( + island_id, + bodies, + interactions, + &self.not_ground_interactions, + ); + self.ground_interaction_groups.$group( + island_id, + bodies, + interactions, + &self.ground_interactions, + ); + + // Compute constraint indices. + for interaction_i in &self.interaction_groups.nongrouped_interactions[start_nongrouped..] { + let interaction = &mut interactions[*interaction_i]$(.$weight)*; + interaction.constraint_index = total_num_constraints; + self.constraint_descs.push(( + total_num_constraints, + VelocityConstraintDesc::NongroundNongrouped(*interaction_i), + )); + total_num_constraints += $num_active_constraints(interaction); + } + + #[cfg(feature = "simd-is-enabled")] + for interaction_i in + self.interaction_groups.grouped_interactions[start_grouped..].chunks(SIMD_WIDTH) + { + let interaction = &mut interactions[interaction_i[0]]$(.$weight)*; + interaction.constraint_index = total_num_constraints; + self.constraint_descs.push(( + total_num_constraints, + VelocityConstraintDesc::NongroundGrouped( + array![|ii| interaction_i[ii]; SIMD_WIDTH], + ), + )); + total_num_constraints += $num_active_constraints(interaction); + } + + for interaction_i in + &self.ground_interaction_groups.nongrouped_interactions[start_nongrouped_ground..] + { + let interaction = &mut interactions[*interaction_i]$(.$weight)*; + interaction.constraint_index = total_num_constraints; + self.constraint_descs.push(( + total_num_constraints, + VelocityConstraintDesc::GroundNongrouped(*interaction_i), + )); + total_num_constraints += $num_active_constraints(interaction); + } + + #[cfg(feature = "simd-is-enabled")] + for interaction_i in self.ground_interaction_groups.grouped_interactions + [start_grouped_ground..] + .chunks(SIMD_WIDTH) + { + let interaction = &mut interactions[interaction_i[0]]$(.$weight)*; + interaction.constraint_index = total_num_constraints; + self.constraint_descs.push(( + total_num_constraints, + VelocityConstraintDesc::GroundGrouped( + array![|ii| interaction_i[ii]; SIMD_WIDTH], + ), + )); + total_num_constraints += $num_active_constraints(interaction); + } + + self.parallel_desc_groups.push(self.constraint_descs.len()); + } + + // Resize the constraints set. + self.constraints.clear(); + self.constraints + .resize_with(total_num_constraints, || $empty_constraint) + } + } + } +} + +impl_init_constraints_group!( + AnyVelocityConstraint, + &mut ContactManifold, + categorize_velocity_contacts, + group_manifolds, + VelocityConstraint::num_active_constraints, + AnyVelocityConstraint::Empty +); + +impl_init_constraints_group!( + AnyJointVelocityConstraint, + JointGraphEdge, + categorize_joints, + group_joints, + AnyJointVelocityConstraint::num_active_constraints, + AnyJointVelocityConstraint::Empty, + weight +); + +impl ParallelVelocitySolverPart { + fn fill_constraints( + &mut self, + thread: &ThreadContext, + params: &IntegrationParameters, + bodies: &RigidBodySet, + manifolds_all: &[&mut ContactManifold], + ) { + let descs = &self.constraint_descs; + + crate::concurrent_loop! { + let batch_size = thread.batch_size; + for desc in descs[thread.constraint_initialization_index, thread.num_initialized_constraints] { + match &desc.1 { + VelocityConstraintDesc::NongroundNongrouped(manifold_id) => { + let manifold = &*manifolds_all[*manifold_id]; + VelocityConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.constraints, false); + } + VelocityConstraintDesc::GroundNongrouped(manifold_id) => { + let manifold = &*manifolds_all[*manifold_id]; + VelocityGroundConstraint::generate(params, *manifold_id, manifold, bodies, &mut self.constraints, false); + } + #[cfg(feature = "simd-is-enabled")] + VelocityConstraintDesc::NongroundGrouped(manifold_id) => { + let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH]; + WVelocityConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.constraints, false); + } + #[cfg(feature = "simd-is-enabled")] + VelocityConstraintDesc::GroundGrouped(manifold_id) => { + let manifolds = array![|ii| &*manifolds_all[manifold_id[ii]]; SIMD_WIDTH]; + WVelocityGroundConstraint::generate(params, *manifold_id, manifolds, bodies, &mut self.constraints, false); + } + } + } + } + } +} + +impl ParallelVelocitySolverPart { + fn fill_constraints( + &mut self, + thread: &ThreadContext, + params: &IntegrationParameters, + bodies: &RigidBodySet, + joints_all: &[JointGraphEdge], + ) { + let descs = &self.constraint_descs; + + crate::concurrent_loop! { + let batch_size = thread.batch_size; + for desc in descs[thread.joint_constraint_initialization_index, thread.num_initialized_joint_constraints] { + match &desc.1 { + VelocityConstraintDesc::NongroundNongrouped(joint_id) => { + let joint = &joints_all[*joint_id].weight; + let constraint = AnyJointVelocityConstraint::from_joint(params, *joint_id, joint, bodies); + self.constraints[joint.constraint_index] = constraint; + } + VelocityConstraintDesc::GroundNongrouped(joint_id) => { + let joint = &joints_all[*joint_id].weight; + let constraint = AnyJointVelocityConstraint::from_joint_ground(params, *joint_id, joint, bodies); + self.constraints[joint.constraint_index] = constraint; + } + #[cfg(feature = "simd-is-enabled")] + VelocityConstraintDesc::NongroundGrouped(joint_id) => { + let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH]; + let constraint = AnyJointVelocityConstraint::from_wide_joint(params, *joint_id, joints, bodies); + self.constraints[joints[0].constraint_index] = constraint; + } + #[cfg(feature = "simd-is-enabled")] + VelocityConstraintDesc::GroundGrouped(joint_id) => { + let joints = array![|ii| &joints_all[joint_id[ii]].weight; SIMD_WIDTH]; + let constraint = AnyJointVelocityConstraint::from_wide_joint_ground(params, *joint_id, joints, bodies); + self.constraints[joints[0].constraint_index] = constraint; + } + } + } + } + } +} + +pub(crate) struct ParallelVelocitySolver { + part: ParallelVelocitySolverPart, + joint_part: ParallelVelocitySolverPart, +} + +impl ParallelVelocitySolver { + pub fn new() -> Self { + Self { + part: ParallelVelocitySolverPart::new(), + joint_part: ParallelVelocitySolverPart::new(), + } + } + + pub fn init_constraint_groups( + &mut self, + island_id: usize, + bodies: &RigidBodySet, + manifolds: &mut [&mut ContactManifold], + manifold_groups: &ParallelInteractionGroups, + joints: &mut [JointGraphEdge], + joint_groups: &ParallelInteractionGroups, + ) { + self.part + .init_constraints_groups(island_id, bodies, manifolds, manifold_groups); + self.joint_part + .init_constraints_groups(island_id, bodies, joints, joint_groups); + } + + pub fn fill_constraints( + &mut self, + thread: &ThreadContext, + params: &IntegrationParameters, + bodies: &RigidBodySet, + manifolds: &[&mut ContactManifold], + joints: &[JointGraphEdge], + ) { + self.part + .fill_constraints(thread, params, bodies, manifolds); + self.joint_part + .fill_constraints(thread, params, bodies, joints); + ThreadContext::lock_until_ge( + &thread.num_initialized_constraints, + self.part.constraint_descs.len(), + ); + ThreadContext::lock_until_ge( + &thread.num_initialized_joint_constraints, + self.joint_part.constraint_descs.len(), + ); + } + + pub fn solve_constraints( + &mut self, + thread: &ThreadContext, + params: &IntegrationParameters, + manifolds_all: &mut [&mut ContactManifold], + joints_all: &mut [JointGraphEdge], + mj_lambdas: &mut [DeltaVel], + ) { + if self.part.constraint_descs.len() == 0 && self.joint_part.constraint_descs.len() == 0 { + return; + } + + /* + * Warmstart constraints. + */ + { + // Each thread will concurrently grab thread.batch_size constraint desc to + // solve. If the batch size is large enough for to cross the boundary of + // a parallel_desc_group, we have to wait util the current group is finished + // before starting the next one. + let mut target_num_desc = 0; + let mut start_index = thread + .warmstart_contact_index + .fetch_add(thread.batch_size, Ordering::SeqCst); + let mut batch_size = thread.batch_size; + let mut shift = 0; + + macro_rules! warmstart( + ($part: expr) => { + for group in $part.parallel_desc_groups.windows(2) { + let num_descs_in_group = group[1] - group[0]; + target_num_desc += num_descs_in_group; + + while start_index < group[1] { + let end_index = (start_index + batch_size).min(group[1]); + + let constraints = if end_index == $part.constraint_descs.len() { + &mut $part.constraints[$part.constraint_descs[start_index].0..] + } else { + &mut $part.constraints[$part.constraint_descs[start_index].0..$part.constraint_descs[end_index].0] + }; + + for constraint in constraints { + constraint.warmstart(mj_lambdas); + } + + let num_solved = end_index - start_index; + batch_size -= num_solved; + + thread + .num_warmstarted_contacts + .fetch_add(num_solved, Ordering::SeqCst); + + if batch_size == 0 { + start_index = thread + .warmstart_contact_index + .fetch_add(thread.batch_size, Ordering::SeqCst); + start_index -= shift; + batch_size = thread.batch_size; + } else { + start_index += num_solved; + } + } + + ThreadContext::lock_until_ge(&thread.num_warmstarted_contacts, target_num_desc); + } + } + ); + + warmstart!(self.joint_part); + shift = self.joint_part.constraint_descs.len(); + start_index -= shift; + warmstart!(self.part); + } + + /* + * Solve constraints. + */ + { + // Each thread will concurrently grab thread.batch_size constraint desc to + // solve. If the batch size is large enough for to cross the boundary of + // a parallel_desc_group, we have to wait util the current group is finished + // before starting the next one. + let mut start_index = thread + .solve_interaction_index + .fetch_add(thread.batch_size, Ordering::SeqCst); + let mut batch_size = thread.batch_size; + let contact_descs = &self.part.constraint_descs[..]; + let joint_descs = &self.joint_part.constraint_descs[..]; + let mut target_num_desc = 0; + let mut shift = 0; + + for _ in 0..params.max_velocity_iterations { + macro_rules! solve { + ($part: expr) => { + // Joint groups. + for group in $part.parallel_desc_groups.windows(2) { + let num_descs_in_group = group[1] - group[0]; + + target_num_desc += num_descs_in_group; + + while start_index < group[1] { + let end_index = (start_index + batch_size).min(group[1]); + + let constraints = if end_index == $part.constraint_descs.len() { + &mut $part.constraints[$part.constraint_descs[start_index].0..] + } else { + &mut $part.constraints[$part.constraint_descs[start_index].0 + ..$part.constraint_descs[end_index].0] + }; + + // println!( + // "Solving a constraint {:?}.", + // rayon::current_thread_index() + // ); + for constraint in constraints { + constraint.solve(mj_lambdas); + } + + let num_solved = end_index - start_index; + batch_size -= num_solved; + + thread + .num_solved_interactions + .fetch_add(num_solved, Ordering::SeqCst); + + if batch_size == 0 { + start_index = thread + .solve_interaction_index + .fetch_add(thread.batch_size, Ordering::SeqCst); + start_index -= shift; + batch_size = thread.batch_size; + } else { + start_index += num_solved; + } + } + ThreadContext::lock_until_ge( + &thread.num_solved_interactions, + target_num_desc, + ); + } + }; + } + + solve!(self.joint_part); + shift += joint_descs.len(); + start_index -= joint_descs.len(); + solve!(self.part); + shift += contact_descs.len(); + start_index -= contact_descs.len(); + } + } + + /* + * Writeback impulses. + */ + let joint_constraints = &self.joint_part.constraints; + let contact_constraints = &self.part.constraints; + crate::concurrent_loop! { + let batch_size = thread.batch_size; + for constraint in joint_constraints[thread.joint_writeback_index] { + constraint.writeback_impulses(joints_all); + } + } + crate::concurrent_loop! { + let batch_size = thread.batch_size; + for constraint in contact_constraints[thread.impulse_writeback_index] { + constraint.writeback_impulses(manifolds_all); + } + } + } +} diff --git a/src/dynamics/solver/position_constraint.rs b/src/dynamics/solver/position_constraint.rs new file mode 100644 index 000000000..608a34204 --- /dev/null +++ b/src/dynamics/solver/position_constraint.rs @@ -0,0 +1,246 @@ +use crate::dynamics::solver::PositionGroundConstraint; +#[cfg(feature = "simd-is-enabled")] +use crate::dynamics::solver::{WPositionConstraint, WPositionGroundConstraint}; +use crate::dynamics::{IntegrationParameters, RigidBodySet}; +use crate::geometry::{ContactManifold, KinematicsCategory}; +use crate::math::{ + AngularInertia, Isometry, Point, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS, +}; +use crate::utils::{WAngularInertia, WCross, WDot}; + +pub(crate) enum AnyPositionConstraint { + #[cfg(feature = "simd-is-enabled")] + GroupedPointPointGround(WPositionGroundConstraint), + #[cfg(feature = "simd-is-enabled")] + GroupedPlanePointGround(WPositionGroundConstraint), + NongroupedPointPointGround(PositionGroundConstraint), + NongroupedPlanePointGround(PositionGroundConstraint), + #[cfg(feature = "simd-is-enabled")] + GroupedPointPoint(WPositionConstraint), + #[cfg(feature = "simd-is-enabled")] + GroupedPlanePoint(WPositionConstraint), + NongroupedPointPoint(PositionConstraint), + NongroupedPlanePoint(PositionConstraint), + #[allow(dead_code)] // The Empty variant is only used with parallel code. + Empty, +} + +impl AnyPositionConstraint { + pub fn solve(&self, params: &IntegrationParameters, positions: &mut [Isometry]) { + match self { + #[cfg(feature = "simd-is-enabled")] + AnyPositionConstraint::GroupedPointPointGround(c) => { + c.solve_point_point(params, positions) + } + #[cfg(feature = "simd-is-enabled")] + AnyPositionConstraint::GroupedPlanePointGround(c) => { + c.solve_plane_point(params, positions) + } + AnyPositionConstraint::NongroupedPointPointGround(c) => { + c.solve_point_point(params, positions) + } + AnyPositionConstraint::NongroupedPlanePointGround(c) => { + c.solve_plane_point(params, positions) + } + #[cfg(feature = "simd-is-enabled")] + AnyPositionConstraint::GroupedPointPoint(c) => c.solve_point_point(params, positions), + #[cfg(feature = "simd-is-enabled")] + AnyPositionConstraint::GroupedPlanePoint(c) => c.solve_plane_point(params, positions), + AnyPositionConstraint::NongroupedPointPoint(c) => { + c.solve_point_point(params, positions) + } + AnyPositionConstraint::NongroupedPlanePoint(c) => { + c.solve_plane_point(params, positions) + } + AnyPositionConstraint::Empty => unreachable!(), + } + } +} + +pub(crate) struct PositionConstraint { + pub rb1: usize, + pub rb2: usize, + // NOTE: the points are relative to the center of masses. + pub local_p1: [Point; MAX_MANIFOLD_POINTS], + pub local_p2: [Point; MAX_MANIFOLD_POINTS], + pub local_n1: Vector, + pub num_contacts: u8, + pub radius: f32, + pub im1: f32, + pub im2: f32, + pub ii1: AngularInertia, + pub ii2: AngularInertia, + pub erp: f32, + pub max_linear_correction: f32, +} + +impl PositionConstraint { + #[cfg(feature = "parallel")] + pub fn num_active_constraints(manifold: &ContactManifold) -> usize { + let rest = manifold.num_active_contacts() % MAX_MANIFOLD_POINTS != 0; + manifold.num_active_contacts() / MAX_MANIFOLD_POINTS + rest as usize + } + + pub fn generate( + params: &IntegrationParameters, + manifold: &ContactManifold, + bodies: &RigidBodySet, + out_constraints: &mut Vec, + push: bool, + ) { + let rb1 = &bodies[manifold.body_pair.body1]; + let rb2 = &bodies[manifold.body_pair.body2]; + let shift1 = manifold.local_n1 * -manifold.kinematics.radius1; + let shift2 = manifold.local_n2 * -manifold.kinematics.radius2; + let radius = + manifold.kinematics.radius1 + manifold.kinematics.radius2 /*- params.allowed_linear_error*/; + + for (l, manifold_points) in manifold + .active_contacts() + .chunks(MAX_MANIFOLD_POINTS) + .enumerate() + { + let mut local_p1 = [Point::origin(); MAX_MANIFOLD_POINTS]; + let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS]; + + for l in 0..manifold_points.len() { + local_p1[l] = manifold_points[l].local_p1 + shift1; + local_p2[l] = manifold_points[l].local_p2 + shift2; + } + + let constraint = PositionConstraint { + rb1: rb1.active_set_offset, + rb2: rb2.active_set_offset, + local_p1, + local_p2, + local_n1: manifold.local_n1, + radius, + im1: rb1.mass_properties.inv_mass, + im2: rb2.mass_properties.inv_mass, + ii1: rb1.world_inv_inertia_sqrt.squared(), + ii2: rb2.world_inv_inertia_sqrt.squared(), + num_contacts: manifold_points.len() as u8, + erp: params.erp, + max_linear_correction: params.max_linear_correction, + }; + + if push { + if manifold.kinematics.category == KinematicsCategory::PointPoint { + out_constraints.push(AnyPositionConstraint::NongroupedPointPoint(constraint)); + } else { + out_constraints.push(AnyPositionConstraint::NongroupedPlanePoint(constraint)); + } + } else { + if manifold.kinematics.category == KinematicsCategory::PointPoint { + out_constraints[manifold.constraint_index + l] = + AnyPositionConstraint::NongroupedPointPoint(constraint); + } else { + out_constraints[manifold.constraint_index + l] = + AnyPositionConstraint::NongroupedPlanePoint(constraint); + } + } + } + } + + pub fn solve_point_point( + &self, + params: &IntegrationParameters, + positions: &mut [Isometry], + ) { + // FIXME: can we avoid most of the multiplications by pos1/pos2? + // Compute jacobians. + let mut pos1 = positions[self.rb1]; + let mut pos2 = positions[self.rb2]; + let allowed_err = params.allowed_linear_error; + let target_dist = self.radius - allowed_err; + + for k in 0..self.num_contacts as usize { + let p1 = pos1 * self.local_p1[k]; + let p2 = pos2 * self.local_p2[k]; + let dpos = p2 - p1; + + let sqdist = dpos.norm_squared(); + + // NOTE: only works for the point-point case. + if sqdist < target_dist * target_dist { + let dist = sqdist.sqrt(); + let n = dpos / dist; + let err = ((dist - target_dist) * self.erp).max(-self.max_linear_correction); + let dp1 = p1.coords - pos1.translation.vector; + let dp2 = p2.coords - pos2.translation.vector; + + let gcross1 = dp1.gcross(n); + let gcross2 = -dp2.gcross(n); + let ii_gcross1 = self.ii1.transform_vector(gcross1); + let ii_gcross2 = self.ii2.transform_vector(gcross2); + + // Compute impulse. + let inv_r = + self.im1 + self.im2 + gcross1.gdot(ii_gcross1) + gcross2.gdot(ii_gcross2); + let impulse = err / inv_r; + + // Apply impulse. + let tra1 = Translation::from(n * (impulse * self.im1)); + let tra2 = Translation::from(n * (-impulse * self.im2)); + let rot1 = Rotation::new(ii_gcross1 * impulse); + let rot2 = Rotation::new(ii_gcross2 * impulse); + + pos1 = Isometry::from_parts(tra1 * pos1.translation, rot1 * pos1.rotation); + pos2 = Isometry::from_parts(tra2 * pos2.translation, rot2 * pos2.rotation); + } + } + + positions[self.rb1] = pos1; + positions[self.rb2] = pos2; + } + + pub fn solve_plane_point( + &self, + params: &IntegrationParameters, + positions: &mut [Isometry], + ) { + // FIXME: can we avoid most of the multiplications by pos1/pos2? + // Compute jacobians. + let mut pos1 = positions[self.rb1]; + let mut pos2 = positions[self.rb2]; + let allowed_err = params.allowed_linear_error; + let target_dist = self.radius - allowed_err; + + for k in 0..self.num_contacts as usize { + let n1 = pos1 * self.local_n1; + let p1 = pos1 * self.local_p1[k]; + let p2 = pos2 * self.local_p2[k]; + let dpos = p2 - p1; + let dist = dpos.dot(&n1); + + if dist < target_dist { + let p1 = p2 - n1 * dist; + let err = ((dist - target_dist) * self.erp).max(-self.max_linear_correction); + let dp1 = p1.coords - pos1.translation.vector; + let dp2 = p2.coords - pos2.translation.vector; + + let gcross1 = dp1.gcross(n1); + let gcross2 = -dp2.gcross(n1); + let ii_gcross1 = self.ii1.transform_vector(gcross1); + let ii_gcross2 = self.ii2.transform_vector(gcross2); + + // Compute impulse. + let inv_r = + self.im1 + self.im2 + gcross1.gdot(ii_gcross1) + gcross2.gdot(ii_gcross2); + let impulse = err / inv_r; + + // Apply impulse. + let tra1 = Translation::from(n1 * (impulse * self.im1)); + let tra2 = Translation::from(n1 * (-impulse * self.im2)); + let rot1 = Rotation::new(ii_gcross1 * impulse); + let rot2 = Rotation::new(ii_gcross2 * impulse); + + pos1 = Isometry::from_parts(tra1 * pos1.translation, rot1 * pos1.rotation); + pos2 = Isometry::from_parts(tra2 * pos2.translation, rot2 * pos2.rotation); + } + } + + positions[self.rb1] = pos1; + positions[self.rb2] = pos2; + } +} diff --git a/src/dynamics/solver/position_constraint_wide.rs b/src/dynamics/solver/position_constraint_wide.rs new file mode 100644 index 000000000..8828c3d9e --- /dev/null +++ b/src/dynamics/solver/position_constraint_wide.rs @@ -0,0 +1,217 @@ +use super::AnyPositionConstraint; +use crate::dynamics::{IntegrationParameters, RigidBodySet}; +use crate::geometry::{ContactManifold, KinematicsCategory}; +use crate::math::{ + AngularInertia, Isometry, Point, Rotation, SimdFloat, Translation, Vector, MAX_MANIFOLD_POINTS, + SIMD_WIDTH, +}; +use crate::utils::{WAngularInertia, WCross, WDot}; + +use num::Zero; +use simba::simd::{SimdBool as _, SimdComplexField, SimdPartialOrd, SimdValue}; + +pub(crate) struct WPositionConstraint { + pub rb1: [usize; SIMD_WIDTH], + pub rb2: [usize; SIMD_WIDTH], + // NOTE: the points are relative to the center of masses. + pub local_p1: [Point; MAX_MANIFOLD_POINTS], + pub local_p2: [Point; MAX_MANIFOLD_POINTS], + pub local_n1: Vector, + pub radius: SimdFloat, + pub im1: SimdFloat, + pub im2: SimdFloat, + pub ii1: AngularInertia, + pub ii2: AngularInertia, + pub erp: SimdFloat, + pub max_linear_correction: SimdFloat, + pub num_contacts: u8, +} + +impl WPositionConstraint { + pub fn generate( + params: &IntegrationParameters, + manifolds: [&ContactManifold; SIMD_WIDTH], + bodies: &RigidBodySet, + out_constraints: &mut Vec, + push: bool, + ) { + let rbs1 = array![|ii| bodies.get(manifolds[ii].body_pair.body1).unwrap(); SIMD_WIDTH]; + let rbs2 = array![|ii| bodies.get(manifolds[ii].body_pair.body2).unwrap(); SIMD_WIDTH]; + + let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let sqrt_ii1: AngularInertia = + AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); + let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let sqrt_ii2: AngularInertia = + AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); + + let local_n1 = Vector::from(array![|ii| manifolds[ii].local_n1; SIMD_WIDTH]); + let local_n2 = Vector::from(array![|ii| manifolds[ii].local_n2; SIMD_WIDTH]); + + let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]); + let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]); + + let rb1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; + let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + + let radius = radius1 + radius2 /*- SimdFloat::splat(params.allowed_linear_error)*/; + + for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) { + let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH]; + let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); + + let mut constraint = WPositionConstraint { + rb1, + rb2, + local_p1: [Point::origin(); MAX_MANIFOLD_POINTS], + local_p2: [Point::origin(); MAX_MANIFOLD_POINTS], + local_n1, + radius, + im1, + im2, + ii1: sqrt_ii1.squared(), + ii2: sqrt_ii2.squared(), + erp: SimdFloat::splat(params.erp), + max_linear_correction: SimdFloat::splat(params.max_linear_correction), + num_contacts: num_points as u8, + }; + + let shift1 = local_n1 * -radius1; + let shift2 = local_n2 * -radius2; + + for i in 0..num_points { + let local_p1 = + Point::from(array![|ii| manifold_points[ii][i].local_p1; SIMD_WIDTH]); + let local_p2 = + Point::from(array![|ii| manifold_points[ii][i].local_p2; SIMD_WIDTH]); + + constraint.local_p1[i] = local_p1 + shift1; + constraint.local_p2[i] = local_p2 + shift2; + } + + if push { + if manifolds[0].kinematics.category == KinematicsCategory::PointPoint { + out_constraints.push(AnyPositionConstraint::GroupedPointPoint(constraint)); + } else { + out_constraints.push(AnyPositionConstraint::GroupedPlanePoint(constraint)); + } + } else { + if manifolds[0].kinematics.category == KinematicsCategory::PointPoint { + out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] = + AnyPositionConstraint::GroupedPointPoint(constraint); + } else { + out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] = + AnyPositionConstraint::GroupedPlanePoint(constraint); + } + } + } + } + + pub fn solve_point_point( + &self, + params: &IntegrationParameters, + positions: &mut [Isometry], + ) { + // FIXME: can we avoid most of the multiplications by pos1/pos2? + // Compute jacobians. + let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]); + let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]); + let allowed_err = SimdFloat::splat(params.allowed_linear_error); + let target_dist = self.radius - allowed_err; + + for k in 0..self.num_contacts as usize { + let p1 = pos1 * self.local_p1[k]; + let p2 = pos2 * self.local_p2[k]; + + let dpos = p2 - p1; + let sqdist = dpos.norm_squared(); + + if sqdist.simd_lt(target_dist * target_dist).any() { + let dist = sqdist.simd_sqrt(); + let n = dpos / dist; + let err = ((dist - target_dist) * self.erp) + .simd_clamp(-self.max_linear_correction, SimdFloat::zero()); + let dp1 = p1.coords - pos1.translation.vector; + let dp2 = p2.coords - pos2.translation.vector; + + let gcross1 = dp1.gcross(n); + let gcross2 = -dp2.gcross(n); + let ii_gcross1 = self.ii1.transform_vector(gcross1); + let ii_gcross2 = self.ii2.transform_vector(gcross2); + + // Compute impulse. + let inv_r = + self.im1 + self.im2 + gcross1.gdot(ii_gcross1) + gcross2.gdot(ii_gcross2); + let impulse = err / inv_r; + + // Apply impulse. + pos1.translation = Translation::from(n * (impulse * self.im1)) * pos1.translation; + pos1.rotation = Rotation::new(ii_gcross1 * impulse) * pos1.rotation; + pos2.translation = Translation::from(n * (-impulse * self.im2)) * pos2.translation; + pos2.rotation = Rotation::new(ii_gcross2 * impulse) * pos2.rotation; + } + } + + for ii in 0..SIMD_WIDTH { + positions[self.rb1[ii]] = pos1.extract(ii); + } + + for ii in 0..SIMD_WIDTH { + positions[self.rb2[ii]] = pos2.extract(ii); + } + } + + pub fn solve_plane_point( + &self, + params: &IntegrationParameters, + positions: &mut [Isometry], + ) { + // FIXME: can we avoid most of the multiplications by pos1/pos2? + // Compute jacobians. + let mut pos1 = Isometry::from(array![|ii| positions[self.rb1[ii]]; SIMD_WIDTH]); + let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]); + let allowed_err = SimdFloat::splat(params.allowed_linear_error); + let target_dist = self.radius - allowed_err; + + for k in 0..self.num_contacts as usize { + let n1 = pos1 * self.local_n1; + let p1 = pos1 * self.local_p1[k]; + let p2 = pos2 * self.local_p2[k]; + let dpos = p2 - p1; + let dist = dpos.dot(&n1); + + // NOTE: this condition does not seem to be useful perfomancewise? + if dist.simd_lt(target_dist).any() { + // NOTE: only works for the point-point case. + let p1 = p2 - n1 * dist; + let err = ((dist - target_dist) * self.erp) + .simd_clamp(-self.max_linear_correction, SimdFloat::zero()); + let dp1 = p1.coords - pos1.translation.vector; + let dp2 = p2.coords - pos2.translation.vector; + + let gcross1 = dp1.gcross(n1); + let gcross2 = -dp2.gcross(n1); + let ii_gcross1 = self.ii1.transform_vector(gcross1); + let ii_gcross2 = self.ii2.transform_vector(gcross2); + + // Compute impulse. + let inv_r = + self.im1 + self.im2 + gcross1.gdot(ii_gcross1) + gcross2.gdot(ii_gcross2); + let impulse = err / inv_r; + + // Apply impulse. + pos1.translation = Translation::from(n1 * (impulse * self.im1)) * pos1.translation; + pos1.rotation = Rotation::new(ii_gcross1 * impulse) * pos1.rotation; + pos2.translation = Translation::from(n1 * (-impulse * self.im2)) * pos2.translation; + pos2.rotation = Rotation::new(ii_gcross2 * impulse) * pos2.rotation; + } + } + + for ii in 0..SIMD_WIDTH { + positions[self.rb1[ii]] = pos1.extract(ii); + } + for ii in 0..SIMD_WIDTH { + positions[self.rb2[ii]] = pos2.extract(ii); + } + } +} diff --git a/src/dynamics/solver/position_ground_constraint.rs b/src/dynamics/solver/position_ground_constraint.rs new file mode 100644 index 000000000..e6e83c61b --- /dev/null +++ b/src/dynamics/solver/position_ground_constraint.rs @@ -0,0 +1,189 @@ +use super::AnyPositionConstraint; +use crate::dynamics::{IntegrationParameters, RigidBodySet}; +use crate::geometry::{ContactManifold, KinematicsCategory}; +use crate::math::{ + AngularInertia, Isometry, Point, Rotation, Translation, Vector, MAX_MANIFOLD_POINTS, +}; +use crate::utils::{WAngularInertia, WCross, WDot}; + +pub(crate) struct PositionGroundConstraint { + pub rb2: usize, + // NOTE: the points are relative to the center of masses. + pub p1: [Point; MAX_MANIFOLD_POINTS], + pub local_p2: [Point; MAX_MANIFOLD_POINTS], + pub n1: Vector, + pub num_contacts: u8, + pub radius: f32, + pub im2: f32, + pub ii2: AngularInertia, + pub erp: f32, + pub max_linear_correction: f32, +} + +impl PositionGroundConstraint { + pub fn generate( + params: &IntegrationParameters, + manifold: &ContactManifold, + bodies: &RigidBodySet, + out_constraints: &mut Vec, + push: bool, + ) { + let mut rb1 = &bodies[manifold.body_pair.body1]; + let mut rb2 = &bodies[manifold.body_pair.body2]; + let flip = !rb2.is_dynamic(); + + let local_n1; + let local_n2; + + if flip { + std::mem::swap(&mut rb1, &mut rb2); + local_n1 = manifold.local_n2; + local_n2 = manifold.local_n1; + } else { + local_n1 = manifold.local_n1; + local_n2 = manifold.local_n2; + }; + + let shift1 = local_n1 * -manifold.kinematics.radius1; + let shift2 = local_n2 * -manifold.kinematics.radius2; + let radius = + manifold.kinematics.radius1 + manifold.kinematics.radius2 /* - params.allowed_linear_error */; + + for (l, manifold_points) in manifold + .active_contacts() + .chunks(MAX_MANIFOLD_POINTS) + .enumerate() + { + let mut p1 = [Point::origin(); MAX_MANIFOLD_POINTS]; + let mut local_p2 = [Point::origin(); MAX_MANIFOLD_POINTS]; + + if flip { + // Don't forget that we already swapped rb1 and rb2 above. + // So if we flip, only manifold_points[k].{local_p1,local_p2} have to + // be swapped. + for k in 0..manifold_points.len() { + p1[k] = rb1.predicted_position * (manifold_points[k].local_p2 + shift1); + local_p2[k] = manifold_points[k].local_p1 + shift2; + } + } else { + for k in 0..manifold_points.len() { + p1[k] = rb1.predicted_position * (manifold_points[k].local_p1 + shift1); + local_p2[k] = manifold_points[k].local_p2 + shift2; + } + } + + let constraint = PositionGroundConstraint { + rb2: rb2.active_set_offset, + p1, + local_p2, + n1: rb1.predicted_position * local_n1, + radius, + im2: rb2.mass_properties.inv_mass, + ii2: rb2.world_inv_inertia_sqrt.squared(), + num_contacts: manifold_points.len() as u8, + erp: params.erp, + max_linear_correction: params.max_linear_correction, + }; + + if push { + if manifold.kinematics.category == KinematicsCategory::PointPoint { + out_constraints.push(AnyPositionConstraint::NongroupedPointPointGround( + constraint, + )); + } else { + out_constraints.push(AnyPositionConstraint::NongroupedPlanePointGround( + constraint, + )); + } + } else { + if manifold.kinematics.category == KinematicsCategory::PointPoint { + out_constraints[manifold.constraint_index + l] = + AnyPositionConstraint::NongroupedPointPointGround(constraint); + } else { + out_constraints[manifold.constraint_index + l] = + AnyPositionConstraint::NongroupedPlanePointGround(constraint); + } + } + } + } + pub fn solve_point_point( + &self, + params: &IntegrationParameters, + positions: &mut [Isometry], + ) { + // FIXME: can we avoid most of the multiplications by pos1/pos2? + // Compute jacobians. + let mut pos2 = positions[self.rb2]; + let allowed_err = params.allowed_linear_error; + let target_dist = self.radius - allowed_err; + + for k in 0..self.num_contacts as usize { + let p1 = self.p1[k]; + let p2 = pos2 * self.local_p2[k]; + let dpos = p2 - p1; + + let sqdist = dpos.norm_squared(); + + // NOTE: only works for the point-point case. + if sqdist < target_dist * target_dist { + let dist = sqdist.sqrt(); + let n = dpos / dist; + let err = ((dist - target_dist) * self.erp).max(-self.max_linear_correction); + let dp2 = p2.coords - pos2.translation.vector; + + let gcross2 = -dp2.gcross(n); + let ii_gcross2 = self.ii2.transform_vector(gcross2); + + // Compute impulse. + let inv_r = self.im2 + gcross2.gdot(ii_gcross2); + let impulse = err / inv_r; + + // Apply impulse. + let tra2 = Translation::from(n * (-impulse * self.im2)); + let rot2 = Rotation::new(ii_gcross2 * impulse); + pos2 = Isometry::from_parts(tra2 * pos2.translation, rot2 * pos2.rotation); + } + } + + positions[self.rb2] = pos2; + } + + pub fn solve_plane_point( + &self, + params: &IntegrationParameters, + positions: &mut [Isometry], + ) { + // FIXME: can we avoid most of the multiplications by pos1/pos2? + // Compute jacobians. + let mut pos2 = positions[self.rb2]; + let allowed_err = params.allowed_linear_error; + let target_dist = self.radius - allowed_err; + + for k in 0..self.num_contacts as usize { + let n1 = self.n1; + let p1 = self.p1[k]; + let p2 = pos2 * self.local_p2[k]; + let dpos = p2 - p1; + let dist = dpos.dot(&n1); + + if dist < target_dist { + let err = ((dist - target_dist) * self.erp).max(-self.max_linear_correction); + let dp2 = p2.coords - pos2.translation.vector; + + let gcross2 = -dp2.gcross(n1); + let ii_gcross2 = self.ii2.transform_vector(gcross2); + + // Compute impulse. + let inv_r = self.im2 + gcross2.gdot(ii_gcross2); + let impulse = err / inv_r; + + // Apply impulse. + let tra2 = Translation::from(n1 * (-impulse * self.im2)); + let rot2 = Rotation::new(ii_gcross2 * impulse); + pos2 = Isometry::from_parts(tra2 * pos2.translation, rot2 * pos2.rotation); + } + } + + positions[self.rb2] = pos2; + } +} diff --git a/src/dynamics/solver/position_ground_constraint_wide.rs b/src/dynamics/solver/position_ground_constraint_wide.rs new file mode 100644 index 000000000..faa928bbc --- /dev/null +++ b/src/dynamics/solver/position_ground_constraint_wide.rs @@ -0,0 +1,199 @@ +use super::AnyPositionConstraint; +use crate::dynamics::{IntegrationParameters, RigidBodySet}; +use crate::geometry::{ContactManifold, KinematicsCategory}; +use crate::math::{ + AngularInertia, Isometry, Point, Rotation, SimdFloat, Translation, Vector, MAX_MANIFOLD_POINTS, + SIMD_WIDTH, +}; +use crate::utils::{WAngularInertia, WCross, WDot}; + +use num::Zero; +use simba::simd::{SimdBool as _, SimdComplexField, SimdPartialOrd, SimdValue}; + +pub(crate) struct WPositionGroundConstraint { + pub rb2: [usize; SIMD_WIDTH], + // NOTE: the points are relative to the center of masses. + pub p1: [Point; MAX_MANIFOLD_POINTS], + pub local_p2: [Point; MAX_MANIFOLD_POINTS], + pub n1: Vector, + pub radius: SimdFloat, + pub im2: SimdFloat, + pub ii2: AngularInertia, + pub erp: SimdFloat, + pub max_linear_correction: SimdFloat, + pub num_contacts: u8, +} + +impl WPositionGroundConstraint { + pub fn generate( + params: &IntegrationParameters, + manifolds: [&ContactManifold; SIMD_WIDTH], + bodies: &RigidBodySet, + out_constraints: &mut Vec, + push: bool, + ) { + let mut rbs1 = array![|ii| bodies.get(manifolds[ii].body_pair.body1).unwrap(); SIMD_WIDTH]; + let mut rbs2 = array![|ii| bodies.get(manifolds[ii].body_pair.body2).unwrap(); SIMD_WIDTH]; + let mut flipped = [false; SIMD_WIDTH]; + + for ii in 0..SIMD_WIDTH { + if !rbs2[ii].is_dynamic() { + flipped[ii] = true; + std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]); + } + } + + let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let sqrt_ii2: AngularInertia = + AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); + + let local_n1 = Vector::from( + array![|ii| if flipped[ii] { manifolds[ii].local_n2 } else { manifolds[ii].local_n1 }; SIMD_WIDTH], + ); + let local_n2 = Vector::from( + array![|ii| if flipped[ii] { manifolds[ii].local_n1 } else { manifolds[ii].local_n2 }; SIMD_WIDTH], + ); + + let radius1 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius1; SIMD_WIDTH]); + let radius2 = SimdFloat::from(array![|ii| manifolds[ii].kinematics.radius2; SIMD_WIDTH]); + + let position1 = Isometry::from(array![|ii| rbs1[ii].predicted_position; SIMD_WIDTH]); + + let rb2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + + let radius = radius1 + radius2 /*- SimdFloat::splat(params.allowed_linear_error)*/; + + let n1 = position1 * local_n1; + + for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) { + let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH]; + let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); + + let mut constraint = WPositionGroundConstraint { + rb2, + p1: [Point::origin(); MAX_MANIFOLD_POINTS], + local_p2: [Point::origin(); MAX_MANIFOLD_POINTS], + n1, + radius, + im2, + ii2: sqrt_ii2.squared(), + erp: SimdFloat::splat(params.erp), + max_linear_correction: SimdFloat::splat(params.max_linear_correction), + num_contacts: num_points as u8, + }; + + for i in 0..num_points { + let local_p1 = Point::from( + array![|ii| if flipped[ii] { manifold_points[ii][i].local_p2 } else { manifold_points[ii][i].local_p1 }; SIMD_WIDTH], + ); + let local_p2 = Point::from( + array![|ii| if flipped[ii] { manifold_points[ii][i].local_p1 } else { manifold_points[ii][i].local_p2 }; SIMD_WIDTH], + ); + + constraint.p1[i] = position1 * local_p1 - n1 * radius1; + constraint.local_p2[i] = local_p2 - local_n2 * radius2; + } + + if push { + if manifolds[0].kinematics.category == KinematicsCategory::PointPoint { + out_constraints + .push(AnyPositionConstraint::GroupedPointPointGround(constraint)); + } else { + out_constraints + .push(AnyPositionConstraint::GroupedPlanePointGround(constraint)); + } + } else { + if manifolds[0].kinematics.category == KinematicsCategory::PointPoint { + out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] = + AnyPositionConstraint::GroupedPointPointGround(constraint); + } else { + out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] = + AnyPositionConstraint::GroupedPlanePointGround(constraint); + } + } + } + } + pub fn solve_point_point( + &self, + params: &IntegrationParameters, + positions: &mut [Isometry], + ) { + // FIXME: can we avoid most of the multiplications by pos1/pos2? + // Compute jacobians. + let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]); + let allowed_err = SimdFloat::splat(params.allowed_linear_error); + let target_dist = self.radius - allowed_err; + + for k in 0..self.num_contacts as usize { + let p1 = self.p1[k]; + let p2 = pos2 * self.local_p2[k]; + + let dpos = p2 - p1; + let sqdist = dpos.norm_squared(); + + if sqdist.simd_lt(target_dist * target_dist).any() { + let dist = sqdist.simd_sqrt(); + let n = dpos / dist; + let err = ((dist - target_dist) * self.erp) + .simd_clamp(-self.max_linear_correction, SimdFloat::zero()); + let dp2 = p2.coords - pos2.translation.vector; + let gcross2 = -dp2.gcross(n); + let ii_gcross2 = self.ii2.transform_vector(gcross2); + + // Compute impulse. + let inv_r = self.im2 + gcross2.gdot(ii_gcross2); + let impulse = err / inv_r; + + // Apply impulse. + pos2.translation = Translation::from(n * (-impulse * self.im2)) * pos2.translation; + pos2.rotation = Rotation::new(ii_gcross2 * impulse) * pos2.rotation; + } + } + + for ii in 0..SIMD_WIDTH { + positions[self.rb2[ii]] = pos2.extract(ii); + } + } + + pub fn solve_plane_point( + &self, + params: &IntegrationParameters, + positions: &mut [Isometry], + ) { + // FIXME: can we avoid most of the multiplications by pos1/pos2? + // Compute jacobians. + let mut pos2 = Isometry::from(array![|ii| positions[self.rb2[ii]]; SIMD_WIDTH]); + let allowed_err = SimdFloat::splat(params.allowed_linear_error); + let target_dist = self.radius - allowed_err; + + for k in 0..self.num_contacts as usize { + let n1 = self.n1; + let p1 = self.p1[k]; + let p2 = pos2 * self.local_p2[k]; + let dpos = p2 - p1; + let dist = dpos.dot(&n1); + + // NOTE: this condition does not seem to be useful perfomancewise? + if dist.simd_lt(target_dist).any() { + let err = ((dist - target_dist) * self.erp) + .simd_clamp(-self.max_linear_correction, SimdFloat::zero()); + let dp2 = p2.coords - pos2.translation.vector; + + let gcross2 = -dp2.gcross(n1); + let ii_gcross2 = self.ii2.transform_vector(gcross2); + + // Compute impulse. + let inv_r = self.im2 + gcross2.gdot(ii_gcross2); + let impulse = err / inv_r; + + // Apply impulse. + pos2.translation = Translation::from(n1 * (-impulse * self.im2)) * pos2.translation; + pos2.rotation = Rotation::new(ii_gcross2 * impulse) * pos2.rotation; + } + } + + for ii in 0..SIMD_WIDTH { + positions[self.rb2[ii]] = pos2.extract(ii); + } + } +} diff --git a/src/dynamics/solver/position_solver.rs b/src/dynamics/solver/position_solver.rs new file mode 100644 index 000000000..2cfd629b1 --- /dev/null +++ b/src/dynamics/solver/position_solver.rs @@ -0,0 +1,451 @@ +use super::{ + AnyJointPositionConstraint, InteractionGroups, PositionConstraint, PositionGroundConstraint, +}; +#[cfg(feature = "simd-is-enabled")] +use super::{WPositionConstraint, WPositionGroundConstraint}; +use crate::dynamics::solver::categorization::{categorize_joints, categorize_position_contacts}; +use crate::dynamics::{ + solver::AnyPositionConstraint, IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet, +}; +use crate::geometry::{ContactManifold, ContactManifoldIndex}; +use crate::math::Isometry; +#[cfg(feature = "simd-is-enabled")] +use crate::math::SIMD_WIDTH; + +pub(crate) struct PositionSolverJointPart { + pub nonground_joints: Vec, + pub ground_joints: Vec, + pub nonground_joint_groups: InteractionGroups, + pub ground_joint_groups: InteractionGroups, + pub constraints: Vec, +} + +impl PositionSolverJointPart { + pub fn new() -> Self { + Self { + nonground_joints: Vec::new(), + ground_joints: Vec::new(), + nonground_joint_groups: InteractionGroups::new(), + ground_joint_groups: InteractionGroups::new(), + constraints: Vec::new(), + } + } +} + +pub(crate) struct PositionSolverPart { + pub point_point_manifolds: Vec, + pub plane_point_manifolds: Vec, + pub point_point_ground_manifolds: Vec, + pub plane_point_ground_manifolds: Vec, + pub point_point_groups: InteractionGroups, + pub plane_point_groups: InteractionGroups, + pub point_point_ground_groups: InteractionGroups, + pub plane_point_ground_groups: InteractionGroups, + pub constraints: Vec, +} + +impl PositionSolverPart { + pub fn new() -> Self { + Self { + point_point_manifolds: Vec::new(), + plane_point_manifolds: Vec::new(), + point_point_ground_manifolds: Vec::new(), + plane_point_ground_manifolds: Vec::new(), + point_point_groups: InteractionGroups::new(), + plane_point_groups: InteractionGroups::new(), + point_point_ground_groups: InteractionGroups::new(), + plane_point_ground_groups: InteractionGroups::new(), + constraints: Vec::new(), + } + } +} + +pub(crate) struct PositionSolver { + positions: Vec>, + part: PositionSolverPart, + joint_part: PositionSolverJointPart, +} + +impl PositionSolver { + pub fn new() -> Self { + Self { + positions: Vec::new(), + part: PositionSolverPart::new(), + joint_part: PositionSolverJointPart::new(), + } + } + + pub fn init_constraints( + &mut self, + island_id: usize, + params: &IntegrationParameters, + bodies: &RigidBodySet, + manifolds: &[&mut ContactManifold], + manifold_indices: &[ContactManifoldIndex], + joints: &[JointGraphEdge], + joint_constraint_indices: &[JointIndex], + ) { + self.part + .init_constraints(island_id, params, bodies, manifolds, manifold_indices); + self.joint_part.init_constraints( + island_id, + params, + bodies, + joints, + joint_constraint_indices, + ); + } + + pub fn solve_constraints( + &mut self, + island_id: usize, + params: &IntegrationParameters, + bodies: &mut RigidBodySet, + ) { + self.positions.clear(); + self.positions.extend( + bodies + .iter_active_island(island_id) + .map(|(_, b)| b.position), + ); + + for _ in 0..params.max_position_iterations { + for constraint in &self.joint_part.constraints { + constraint.solve(params, &mut self.positions) + } + + for constraint in &self.part.constraints { + constraint.solve(params, &mut self.positions) + } + } + + bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { + rb.set_position(self.positions[rb.active_set_offset]) + }); + } +} + +impl PositionSolverPart { + pub fn init_constraints( + &mut self, + island_id: usize, + params: &IntegrationParameters, + bodies: &RigidBodySet, + manifolds_all: &[&mut ContactManifold], + manifold_indices: &[ContactManifoldIndex], + ) { + self.point_point_ground_manifolds.clear(); + self.plane_point_ground_manifolds.clear(); + self.point_point_manifolds.clear(); + self.plane_point_manifolds.clear(); + categorize_position_contacts( + bodies, + manifolds_all, + manifold_indices, + &mut self.point_point_ground_manifolds, + &mut self.plane_point_ground_manifolds, + &mut self.point_point_manifolds, + &mut self.plane_point_manifolds, + ); + + self.point_point_groups.clear_groups(); + self.point_point_groups.group_manifolds( + island_id, + bodies, + manifolds_all, + &self.point_point_manifolds, + ); + self.plane_point_groups.clear_groups(); + self.plane_point_groups.group_manifolds( + island_id, + bodies, + manifolds_all, + &self.plane_point_manifolds, + ); + + self.point_point_ground_groups.clear_groups(); + self.point_point_ground_groups.group_manifolds( + island_id, + bodies, + manifolds_all, + &self.point_point_ground_manifolds, + ); + self.plane_point_ground_groups.clear_groups(); + self.plane_point_ground_groups.group_manifolds( + island_id, + bodies, + manifolds_all, + &self.plane_point_ground_manifolds, + ); + + self.constraints.clear(); + + /* + * Init non-ground contact constraints. + */ + #[cfg(feature = "simd-is-enabled")] + { + compute_grouped_constraints( + params, + bodies, + manifolds_all, + &self.point_point_groups.grouped_interactions, + &mut self.constraints, + ); + compute_grouped_constraints( + params, + bodies, + manifolds_all, + &self.plane_point_groups.grouped_interactions, + &mut self.constraints, + ); + } + compute_nongrouped_constraints( + params, + bodies, + manifolds_all, + &self.point_point_groups.nongrouped_interactions, + &mut self.constraints, + ); + compute_nongrouped_constraints( + params, + bodies, + manifolds_all, + &self.plane_point_groups.nongrouped_interactions, + &mut self.constraints, + ); + + /* + * Init ground contact constraints. + */ + #[cfg(feature = "simd-is-enabled")] + { + compute_grouped_ground_constraints( + params, + bodies, + manifolds_all, + &self.point_point_ground_groups.grouped_interactions, + &mut self.constraints, + ); + compute_grouped_ground_constraints( + params, + bodies, + manifolds_all, + &self.plane_point_ground_groups.grouped_interactions, + &mut self.constraints, + ); + } + compute_nongrouped_ground_constraints( + params, + bodies, + manifolds_all, + &self.point_point_ground_groups.nongrouped_interactions, + &mut self.constraints, + ); + compute_nongrouped_ground_constraints( + params, + bodies, + manifolds_all, + &self.plane_point_ground_groups.nongrouped_interactions, + &mut self.constraints, + ); + } +} + +impl PositionSolverJointPart { + pub fn init_constraints( + &mut self, + island_id: usize, + params: &IntegrationParameters, + bodies: &RigidBodySet, + joints: &[JointGraphEdge], + joint_constraint_indices: &[JointIndex], + ) { + self.ground_joints.clear(); + self.nonground_joints.clear(); + categorize_joints( + bodies, + joints, + joint_constraint_indices, + &mut self.ground_joints, + &mut self.nonground_joints, + ); + + self.nonground_joint_groups.clear_groups(); + self.nonground_joint_groups + .group_joints(island_id, bodies, joints, &self.nonground_joints); + + self.ground_joint_groups.clear_groups(); + self.ground_joint_groups + .group_joints(island_id, bodies, joints, &self.ground_joints); + + /* + * Init ground joint constraints. + */ + self.constraints.clear(); + compute_nongrouped_joint_ground_constraints( + params, + bodies, + joints, + &self.ground_joint_groups.nongrouped_interactions, + &mut self.constraints, + ); + + #[cfg(feature = "simd-is-enabled")] + { + compute_grouped_joint_ground_constraints( + params, + bodies, + joints, + &self.ground_joint_groups.grouped_interactions, + &mut self.constraints, + ); + } + + /* + * Init non-ground joint constraints. + */ + compute_nongrouped_joint_constraints( + params, + bodies, + joints, + &self.nonground_joint_groups.nongrouped_interactions, + &mut self.constraints, + ); + + #[cfg(feature = "simd-is-enabled")] + { + compute_grouped_joint_constraints( + params, + bodies, + joints, + &self.nonground_joint_groups.grouped_interactions, + &mut self.constraints, + ); + } + } +} + +fn compute_nongrouped_constraints( + params: &IntegrationParameters, + bodies: &RigidBodySet, + manifolds_all: &[&mut ContactManifold], + manifold_indices: &[ContactManifoldIndex], + output: &mut Vec, +) { + for manifold in manifold_indices.iter().map(|i| &manifolds_all[*i]) { + PositionConstraint::generate(params, manifold, bodies, output, true) + } +} + +#[cfg(feature = "simd-is-enabled")] +fn compute_grouped_constraints( + params: &IntegrationParameters, + bodies: &RigidBodySet, + manifolds_all: &[&mut ContactManifold], + manifold_indices: &[ContactManifoldIndex], + output: &mut Vec, +) { + for manifolds_i in manifold_indices.chunks_exact(SIMD_WIDTH) { + let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH]; + WPositionConstraint::generate(params, manifolds, bodies, output, true) + } +} + +fn compute_nongrouped_ground_constraints( + params: &IntegrationParameters, + bodies: &RigidBodySet, + manifolds_all: &[&mut ContactManifold], + manifold_indices: &[ContactManifoldIndex], + output: &mut Vec, +) { + for manifold in manifold_indices.iter().map(|i| &manifolds_all[*i]) { + PositionGroundConstraint::generate(params, manifold, bodies, output, true) + } +} + +#[cfg(feature = "simd-is-enabled")] +fn compute_grouped_ground_constraints( + params: &IntegrationParameters, + bodies: &RigidBodySet, + manifolds_all: &[&mut ContactManifold], + manifold_indices: &[ContactManifoldIndex], + output: &mut Vec, +) { + for manifolds_i in manifold_indices.chunks_exact(SIMD_WIDTH) { + let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH]; + WPositionGroundConstraint::generate(params, manifolds, bodies, output, true); + } +} + +fn compute_nongrouped_joint_ground_constraints( + _params: &IntegrationParameters, + bodies: &RigidBodySet, + joints_all: &[JointGraphEdge], + joint_indices: &[JointIndex], + output: &mut Vec, +) { + for joint_i in joint_indices { + let joint = &joints_all[*joint_i].weight; + let pos_constraint = AnyJointPositionConstraint::from_joint_ground(joint, bodies); + output.push(pos_constraint); + } +} + +#[cfg(feature = "simd-is-enabled")] +fn compute_grouped_joint_ground_constraints( + _params: &IntegrationParameters, + bodies: &RigidBodySet, + joints_all: &[JointGraphEdge], + joint_indices: &[JointIndex], + output: &mut Vec, +) { + for joint_i in joint_indices.chunks_exact(SIMD_WIDTH) { + let joints = array![|ii| &joints_all[joint_i[ii]].weight; SIMD_WIDTH]; + if let Some(pos_constraint) = + AnyJointPositionConstraint::from_wide_joint_ground(joints, bodies) + { + output.push(pos_constraint); + } else { + for joint in joints.iter() { + output.push(AnyJointPositionConstraint::from_joint_ground( + *joint, bodies, + )) + } + } + } +} + +fn compute_nongrouped_joint_constraints( + _params: &IntegrationParameters, + bodies: &RigidBodySet, + joints_all: &[JointGraphEdge], + joint_indices: &[JointIndex], + output: &mut Vec, +) { + for joint_i in joint_indices { + let joint = &joints_all[*joint_i]; + let pos_constraint = AnyJointPositionConstraint::from_joint(&joint.weight, bodies); + output.push(pos_constraint); + } +} + +#[cfg(feature = "simd-is-enabled")] +fn compute_grouped_joint_constraints( + _params: &IntegrationParameters, + bodies: &RigidBodySet, + joints_all: &[JointGraphEdge], + joint_indices: &[JointIndex], + output: &mut Vec, +) { + for joint_i in joint_indices.chunks_exact(SIMD_WIDTH) { + let joints = array![|ii| &joints_all[joint_i[ii]].weight; SIMD_WIDTH]; + if let Some(pos_constraint) = AnyJointPositionConstraint::from_wide_joint(joints, bodies) { + output.push(pos_constraint); + } else { + for joint in joints.iter() { + output.push(AnyJointPositionConstraint::from_joint(*joint, bodies)) + } + } + } +} diff --git a/src/dynamics/solver/velocity_constraint.rs b/src/dynamics/solver/velocity_constraint.rs new file mode 100644 index 000000000..9212e8967 --- /dev/null +++ b/src/dynamics/solver/velocity_constraint.rs @@ -0,0 +1,401 @@ +use super::DeltaVel; +use crate::dynamics::solver::VelocityGroundConstraint; +#[cfg(feature = "simd-is-enabled")] +use crate::dynamics::solver::{WVelocityConstraint, WVelocityGroundConstraint}; +use crate::dynamics::{IntegrationParameters, RigidBodySet}; +use crate::geometry::{ContactManifold, ContactManifoldIndex}; +use crate::math::{AngVector, Vector, DIM, MAX_MANIFOLD_POINTS}; +use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; +use simba::simd::SimdPartialOrd; + +//#[repr(align(64))] +#[derive(Copy, Clone, Debug)] +pub(crate) enum AnyVelocityConstraint { + NongroupedGround(VelocityGroundConstraint), + Nongrouped(VelocityConstraint), + #[cfg(feature = "simd-is-enabled")] + GroupedGround(WVelocityGroundConstraint), + #[cfg(feature = "simd-is-enabled")] + Grouped(WVelocityConstraint), + #[allow(dead_code)] // The Empty variant is only used with parallel code. + Empty, +} + +impl AnyVelocityConstraint { + #[cfg(target_arch = "wasm32")] + pub fn as_nongrouped_mut(&mut self) -> Option<&mut VelocityConstraint> { + if let AnyVelocityConstraint::Nongrouped(c) = self { + Some(c) + } else { + None + } + } + + #[cfg(target_arch = "wasm32")] + pub fn as_nongrouped_ground_mut(&mut self) -> Option<&mut VelocityGroundConstraint> { + if let AnyVelocityConstraint::NongroupedGround(c) = self { + Some(c) + } else { + None + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + match self { + AnyVelocityConstraint::NongroupedGround(c) => c.warmstart(mj_lambdas), + AnyVelocityConstraint::Nongrouped(c) => c.warmstart(mj_lambdas), + #[cfg(feature = "simd-is-enabled")] + AnyVelocityConstraint::GroupedGround(c) => c.warmstart(mj_lambdas), + #[cfg(feature = "simd-is-enabled")] + AnyVelocityConstraint::Grouped(c) => c.warmstart(mj_lambdas), + AnyVelocityConstraint::Empty => unreachable!(), + } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + match self { + AnyVelocityConstraint::NongroupedGround(c) => c.solve(mj_lambdas), + AnyVelocityConstraint::Nongrouped(c) => c.solve(mj_lambdas), + #[cfg(feature = "simd-is-enabled")] + AnyVelocityConstraint::GroupedGround(c) => c.solve(mj_lambdas), + #[cfg(feature = "simd-is-enabled")] + AnyVelocityConstraint::Grouped(c) => c.solve(mj_lambdas), + AnyVelocityConstraint::Empty => unreachable!(), + } + } + + pub fn writeback_impulses(&self, manifold_all: &mut [&mut ContactManifold]) { + match self { + AnyVelocityConstraint::NongroupedGround(c) => c.writeback_impulses(manifold_all), + AnyVelocityConstraint::Nongrouped(c) => c.writeback_impulses(manifold_all), + #[cfg(feature = "simd-is-enabled")] + AnyVelocityConstraint::GroupedGround(c) => c.writeback_impulses(manifold_all), + #[cfg(feature = "simd-is-enabled")] + AnyVelocityConstraint::Grouped(c) => c.writeback_impulses(manifold_all), + AnyVelocityConstraint::Empty => unreachable!(), + } + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct VelocityConstraintElementPart { + pub gcross1: AngVector, + pub gcross2: AngVector, + pub rhs: f32, + pub impulse: f32, + pub r: f32, +} + +#[cfg(not(target_arch = "wasm32"))] +impl VelocityConstraintElementPart { + fn zero() -> Self { + Self { + gcross1: na::zero(), + gcross2: na::zero(), + rhs: 0.0, + impulse: 0.0, + r: 0.0, + } + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct VelocityConstraintElement { + pub normal_part: VelocityConstraintElementPart, + pub tangent_part: [VelocityConstraintElementPart; DIM - 1], +} + +#[cfg(not(target_arch = "wasm32"))] +impl VelocityConstraintElement { + pub fn zero() -> Self { + Self { + normal_part: VelocityConstraintElementPart::zero(), + tangent_part: [VelocityConstraintElementPart::zero(); DIM - 1], + } + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct VelocityConstraint { + pub dir1: Vector, // Non-penetration force direction for the first body. + pub im1: f32, + pub im2: f32, + pub limit: f32, + pub mj_lambda1: usize, + pub mj_lambda2: usize, + pub manifold_id: ContactManifoldIndex, + pub manifold_contact_id: usize, + pub num_contacts: u8, + pub elements: [VelocityConstraintElement; MAX_MANIFOLD_POINTS], +} + +impl VelocityConstraint { + #[cfg(feature = "parallel")] + pub fn num_active_constraints(manifold: &ContactManifold) -> usize { + let rest = manifold.num_active_contacts() % MAX_MANIFOLD_POINTS != 0; + manifold.num_active_contacts() / MAX_MANIFOLD_POINTS + rest as usize + } + + pub fn generate( + params: &IntegrationParameters, + manifold_id: ContactManifoldIndex, + manifold: &ContactManifold, + bodies: &RigidBodySet, + out_constraints: &mut Vec, + push: bool, + ) { + let rb1 = &bodies[manifold.body_pair.body1]; + let rb2 = &bodies[manifold.body_pair.body2]; + let mj_lambda1 = rb1.active_set_offset; + let mj_lambda2 = rb2.active_set_offset; + let force_dir1 = rb1.position * (-manifold.local_n1); + let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff; + + for (l, manifold_points) in manifold + .active_contacts() + .chunks(MAX_MANIFOLD_POINTS) + .enumerate() + { + #[cfg(not(target_arch = "wasm32"))] + let mut constraint = VelocityConstraint { + dir1: force_dir1, + elements: [VelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], + im1: rb1.mass_properties.inv_mass, + im2: rb2.mass_properties.inv_mass, + limit: manifold.friction, + mj_lambda1, + mj_lambda2, + manifold_id, + manifold_contact_id: l * MAX_MANIFOLD_POINTS, + num_contacts: manifold_points.len() as u8, + }; + + // TODO: this is a WIP optimization for WASM platforms. + // For some reasons, the compiler does not inline the `Vec::push` method + // in this method. This generates two memset and one memcpy which are both very + // expansive. + // This would likely be solved by some kind of "placement-push" (like emplace in C++). + // In the mean time, a workaround is to "push" using `.resize_with` and `::uninit()` to + // avoid spurious copying. + // Is this optimization beneficial when targeting non-WASM platforms? + // + // NOTE: joints have the same problem, but it is not easy to refactor the code that way + // for the moment. + #[cfg(target_arch = "wasm32")] + let constraint = if push { + let new_len = out_constraints.len() + 1; + unsafe { + out_constraints.resize_with(new_len, || { + AnyVelocityConstraint::Nongrouped( + std::mem::MaybeUninit::uninit().assume_init(), + ) + }); + } + out_constraints + .last_mut() + .unwrap() + .as_nongrouped_mut() + .unwrap() + } else { + unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable. + }; + + #[cfg(target_arch = "wasm32")] + { + constraint.dir1 = force_dir1; + constraint.im1 = rb1.mass_properties.inv_mass; + constraint.im2 = rb2.mass_properties.inv_mass; + constraint.limit = manifold.friction; + constraint.mj_lambda1 = mj_lambda1; + constraint.mj_lambda2 = mj_lambda2; + constraint.manifold_id = manifold_id; + constraint.manifold_contact_id = l * MAX_MANIFOLD_POINTS; + constraint.num_contacts = manifold_points.len() as u8; + } + + for k in 0..manifold_points.len() { + let manifold_point = &manifold_points[k]; + let dp1 = (rb1.position * manifold_point.local_p1).coords + - rb1.position.translation.vector; + let dp2 = (rb2.position * manifold_point.local_p2).coords + - rb2.position.translation.vector; + + let vel1 = rb1.linvel + rb1.angvel.gcross(dp1); + let vel2 = rb2.linvel + rb2.angvel.gcross(dp2); + + // Normal part. + { + let gcross1 = rb1 + .world_inv_inertia_sqrt + .transform_vector(dp1.gcross(force_dir1)); + let gcross2 = rb2 + .world_inv_inertia_sqrt + .transform_vector(dp2.gcross(-force_dir1)); + + let r = 1.0 + / (rb1.mass_properties.inv_mass + + rb2.mass_properties.inv_mass + + gcross1.gdot(gcross1) + + gcross2.gdot(gcross2)); + + let rhs = (vel1 - vel2).dot(&force_dir1) + + manifold_point.dist.max(0.0) * params.inv_dt(); + + let impulse = manifold_points[k].impulse * warmstart_coeff; + + constraint.elements[k].normal_part = VelocityConstraintElementPart { + gcross1, + gcross2, + rhs, + impulse, + r, + }; + } + + // Tangent parts. + { + let tangents1 = force_dir1.orthonormal_basis(); + + for j in 0..DIM - 1 { + let gcross1 = rb1 + .world_inv_inertia_sqrt + .transform_vector(dp1.gcross(tangents1[j])); + let gcross2 = rb2 + .world_inv_inertia_sqrt + .transform_vector(dp2.gcross(-tangents1[j])); + let r = 1.0 + / (rb1.mass_properties.inv_mass + + rb2.mass_properties.inv_mass + + gcross1.gdot(gcross1) + + gcross2.gdot(gcross2)); + let rhs = (vel1 - vel2).dot(&tangents1[j]); + #[cfg(feature = "dim2")] + let impulse = manifold_points[k].tangent_impulse * warmstart_coeff; + #[cfg(feature = "dim3")] + let impulse = manifold_points[k].tangent_impulse[j] * warmstart_coeff; + + constraint.elements[k].tangent_part[j] = VelocityConstraintElementPart { + gcross1, + gcross2, + rhs, + impulse, + r, + }; + } + } + } + + #[cfg(not(target_arch = "wasm32"))] + if push { + out_constraints.push(AnyVelocityConstraint::Nongrouped(constraint)); + } else { + out_constraints[manifold.constraint_index + l] = + AnyVelocityConstraint::Nongrouped(constraint); + } + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = DeltaVel::zero(); + let mut mj_lambda2 = DeltaVel::zero(); + + for i in 0..self.num_contacts as usize { + let elt = &self.elements[i].normal_part; + mj_lambda1.linear += self.dir1 * (self.im1 * elt.impulse); + mj_lambda1.angular += elt.gcross1 * elt.impulse; + + mj_lambda2.linear += self.dir1 * (-self.im2 * elt.impulse); + mj_lambda2.angular += elt.gcross2 * elt.impulse; + + // FIXME: move this out of the for loop? + let tangents1 = self.dir1.orthonormal_basis(); + + for j in 0..DIM - 1 { + let elt = &self.elements[i].tangent_part[j]; + mj_lambda1.linear += tangents1[j] * (self.im1 * elt.impulse); + mj_lambda1.angular += elt.gcross1 * elt.impulse; + + mj_lambda2.linear += tangents1[j] * (-self.im2 * elt.impulse); + mj_lambda2.angular += elt.gcross2 * elt.impulse; + } + } + + mj_lambdas[self.mj_lambda1 as usize].linear += mj_lambda1.linear; + mj_lambdas[self.mj_lambda1 as usize].angular += mj_lambda1.angular; + mj_lambdas[self.mj_lambda2 as usize].linear += mj_lambda2.linear; + mj_lambdas[self.mj_lambda2 as usize].angular += mj_lambda2.angular; + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = mj_lambdas[self.mj_lambda1 as usize]; + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + // Solve friction. + for i in 0..self.num_contacts as usize { + let tangents1 = self.dir1.orthonormal_basis(); + + for j in 0..DIM - 1 { + let normal_elt = &self.elements[i].normal_part; + let elt = &mut self.elements[i].tangent_part[j]; + let dimpulse = tangents1[j].dot(&mj_lambda1.linear) + + elt.gcross1.gdot(mj_lambda1.angular) + - tangents1[j].dot(&mj_lambda2.linear) + + elt.gcross2.gdot(mj_lambda2.angular) + + elt.rhs; + let limit = self.limit * normal_elt.impulse; + let new_impulse = (elt.impulse - elt.r * dimpulse).simd_clamp(-limit, limit); + let dlambda = new_impulse - elt.impulse; + elt.impulse = new_impulse; + + mj_lambda1.linear += tangents1[j] * (self.im1 * dlambda); + mj_lambda1.angular += elt.gcross1 * dlambda; + + mj_lambda2.linear += tangents1[j] * (-self.im2 * dlambda); + mj_lambda2.angular += elt.gcross2 * dlambda; + } + } + + // Solve penetration. + for i in 0..self.num_contacts as usize { + let elt = &mut self.elements[i].normal_part; + let dimpulse = self.dir1.dot(&mj_lambda1.linear) + elt.gcross1.gdot(mj_lambda1.angular) + - self.dir1.dot(&mj_lambda2.linear) + + elt.gcross2.gdot(mj_lambda2.angular) + + elt.rhs; + let new_impulse = (elt.impulse - elt.r * dimpulse).max(0.0); + let dlambda = new_impulse - elt.impulse; + elt.impulse = new_impulse; + + mj_lambda1.linear += self.dir1 * (self.im1 * dlambda); + mj_lambda1.angular += elt.gcross1 * dlambda; + + mj_lambda2.linear += self.dir1 * (-self.im2 * dlambda); + mj_lambda2.angular += elt.gcross2 * dlambda; + } + + mj_lambdas[self.mj_lambda1 as usize] = mj_lambda1; + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { + let manifold = &mut manifolds_all[self.manifold_id]; + let k_base = self.manifold_contact_id; + + for k in 0..self.num_contacts as usize { + let active_contacts = manifold.active_contacts_mut(); + active_contacts[k_base + k].impulse = self.elements[k].normal_part.impulse; + #[cfg(feature = "dim2")] + { + active_contacts[k_base + k].tangent_impulse = + self.elements[k].tangent_part[0].impulse; + } + #[cfg(feature = "dim3")] + { + active_contacts[k_base + k].tangent_impulse = [ + self.elements[k].tangent_part[0].impulse, + self.elements[k].tangent_part[1].impulse, + ]; + } + } + } +} diff --git a/src/dynamics/solver/velocity_constraint_wide.rs b/src/dynamics/solver/velocity_constraint_wide.rs new file mode 100644 index 000000000..8f387a59e --- /dev/null +++ b/src/dynamics/solver/velocity_constraint_wide.rs @@ -0,0 +1,344 @@ +use super::{AnyVelocityConstraint, DeltaVel}; +use crate::dynamics::{IntegrationParameters, RigidBodySet}; +use crate::geometry::{ContactManifold, ContactManifoldIndex}; +use crate::math::{ + AngVector, AngularInertia, Isometry, Point, SimdFloat, Vector, DIM, MAX_MANIFOLD_POINTS, + SIMD_WIDTH, +}; +use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; +use num::Zero; +use simba::simd::{SimdPartialOrd, SimdValue}; + +#[derive(Copy, Clone, Debug)] +pub(crate) struct WVelocityConstraintElementPart { + pub gcross1: AngVector, + pub gcross2: AngVector, + pub rhs: SimdFloat, + pub impulse: SimdFloat, + pub r: SimdFloat, +} + +impl WVelocityConstraintElementPart { + pub fn zero() -> Self { + Self { + gcross1: AngVector::zero(), + gcross2: AngVector::zero(), + rhs: SimdFloat::zero(), + impulse: SimdFloat::zero(), + r: SimdFloat::zero(), + } + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct WVelocityConstraintElement { + pub normal_part: WVelocityConstraintElementPart, + pub tangent_parts: [WVelocityConstraintElementPart; DIM - 1], +} + +impl WVelocityConstraintElement { + pub fn zero() -> Self { + Self { + normal_part: WVelocityConstraintElementPart::zero(), + tangent_parts: [WVelocityConstraintElementPart::zero(); DIM - 1], + } + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct WVelocityConstraint { + pub dir1: Vector, // Non-penetration force direction for the first body. + pub elements: [WVelocityConstraintElement; MAX_MANIFOLD_POINTS], + pub num_contacts: u8, + pub im1: SimdFloat, + pub im2: SimdFloat, + pub limit: SimdFloat, + pub mj_lambda1: [usize; SIMD_WIDTH], + pub mj_lambda2: [usize; SIMD_WIDTH], + pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH], + pub manifold_contact_id: usize, +} + +impl WVelocityConstraint { + pub fn generate( + params: &IntegrationParameters, + manifold_id: [ContactManifoldIndex; SIMD_WIDTH], + manifolds: [&ContactManifold; SIMD_WIDTH], + bodies: &RigidBodySet, + out_constraints: &mut Vec, + push: bool, + ) { + let inv_dt = SimdFloat::splat(params.inv_dt()); + let rbs1 = array![|ii| &bodies[manifolds[ii].body_pair.body1]; SIMD_WIDTH]; + let rbs2 = array![|ii| &bodies[manifolds[ii].body_pair.body2]; SIMD_WIDTH]; + + let im1 = SimdFloat::from(array![|ii| rbs1[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii1: AngularInertia = + AngularInertia::from(array![|ii| rbs1[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); + + let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + + let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); + + let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2: AngularInertia = + AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); + + let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + + let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); + + let force_dir1 = position1 * -Vector::from(array![|ii| manifolds[ii].local_n1; SIMD_WIDTH]); + + let mj_lambda1 = array![|ii| rbs1[ii].active_set_offset; SIMD_WIDTH]; + let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + + let friction = SimdFloat::from(array![|ii| manifolds[ii].friction; SIMD_WIDTH]); + let warmstart_multiplier = + SimdFloat::from(array![|ii| manifolds[ii].warmstart_multiplier; SIMD_WIDTH]); + let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff); + + for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) { + let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH]; + let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); + + let mut constraint = WVelocityConstraint { + dir1: force_dir1, + elements: [WVelocityConstraintElement::zero(); MAX_MANIFOLD_POINTS], + im1, + im2, + limit: friction, + mj_lambda1, + mj_lambda2, + manifold_id, + manifold_contact_id: l, + num_contacts: num_points as u8, + }; + + for k in 0..num_points { + // FIXME: can we avoid the multiplications by position1/position2 here? + // By working as much as possible in local-space. + let p1 = position1 + * Point::from(array![|ii| manifold_points[ii][k].local_p1; SIMD_WIDTH]); + let p2 = position2 + * Point::from(array![|ii| manifold_points[ii][k].local_p2; SIMD_WIDTH]); + + let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); + + let impulse = + SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]); + + let dp1 = p1.coords - position1.translation.vector; + let dp2 = p2.coords - position2.translation.vector; + + let vel1 = linvel1 + angvel1.gcross(dp1); + let vel2 = linvel2 + angvel2.gcross(dp2); + + // Normal part. + { + let gcross1 = ii1.transform_vector(dp1.gcross(force_dir1)); + let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); + + let r = SimdFloat::splat(1.0) + / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); + let rhs = + (vel1 - vel2).dot(&force_dir1) + dist.simd_max(SimdFloat::zero()) * inv_dt; + + constraint.elements[k].normal_part = WVelocityConstraintElementPart { + gcross1, + gcross2, + rhs, + impulse: impulse * warmstart_coeff, + r, + }; + } + + // tangent parts. + let tangents1 = force_dir1.orthonormal_basis(); + + for j in 0..DIM - 1 { + #[cfg(feature = "dim2")] + let impulse = SimdFloat::from( + array![|ii| manifold_points[ii][k].tangent_impulse; SIMD_WIDTH], + ); + #[cfg(feature = "dim3")] + let impulse = SimdFloat::from( + array![|ii| manifold_points[ii][k].tangent_impulse[j]; SIMD_WIDTH], + ); + + let gcross1 = ii1.transform_vector(dp1.gcross(tangents1[j])); + let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); + let r = SimdFloat::splat(1.0) + / (im1 + im2 + gcross1.gdot(gcross1) + gcross2.gdot(gcross2)); + let rhs = (vel1 - vel2).dot(&tangents1[j]); + + constraint.elements[k].tangent_parts[j] = WVelocityConstraintElementPart { + gcross1, + gcross2, + rhs, + impulse: impulse * warmstart_coeff, + r, + }; + } + } + + if push { + out_constraints.push(AnyVelocityConstraint::Grouped(constraint)); + } else { + out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] = + AnyVelocityConstraint::Grouped(constraint); + } + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + for i in 0..self.num_contacts as usize { + let elt = &self.elements[i].normal_part; + mj_lambda1.linear += self.dir1 * (self.im1 * elt.impulse); + mj_lambda1.angular += elt.gcross1 * elt.impulse; + + mj_lambda2.linear += self.dir1 * (-self.im2 * elt.impulse); + mj_lambda2.angular += elt.gcross2 * elt.impulse; + + // FIXME: move this out of the for loop? + let tangents1 = self.dir1.orthonormal_basis(); + + for j in 0..DIM - 1 { + let elt = &self.elements[i].tangent_parts[j]; + mj_lambda1.linear += tangents1[j] * (self.im1 * elt.impulse); + mj_lambda1.angular += elt.gcross1 * elt.impulse; + + mj_lambda2.linear += tangents1[j] * (-self.im2 * elt.impulse); + mj_lambda2.angular += elt.gcross2 * elt.impulse; + } + } + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); + mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii); + } + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda1 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda1[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![ |ii| mj_lambdas[ self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![ |ii| mj_lambdas[ self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + // Solve friction first. + for i in 0..self.num_contacts as usize { + // FIXME: move this out of the for loop? + let tangents1 = self.dir1.orthonormal_basis(); + let normal_elt = &self.elements[i].normal_part; + + for j in 0..DIM - 1 { + let elt = &mut self.elements[i].tangent_parts[j]; + let dimpulse = tangents1[j].dot(&mj_lambda1.linear) + + elt.gcross1.gdot(mj_lambda1.angular) + - tangents1[j].dot(&mj_lambda2.linear) + + elt.gcross2.gdot(mj_lambda2.angular) + + elt.rhs; + let limit = self.limit * normal_elt.impulse; + let new_impulse = (elt.impulse - elt.r * dimpulse).simd_clamp(-limit, limit); + let dlambda = new_impulse - elt.impulse; + elt.impulse = new_impulse; + + mj_lambda1.linear += tangents1[j] * (self.im1 * dlambda); + mj_lambda1.angular += elt.gcross1 * dlambda; + mj_lambda2.linear += tangents1[j] * (-self.im2 * dlambda); + mj_lambda2.angular += elt.gcross2 * dlambda; + } + } + + // Solve non-penetration after friction. + for i in 0..self.num_contacts as usize { + let elt = &mut self.elements[i].normal_part; + let dimpulse = self.dir1.dot(&mj_lambda1.linear) + elt.gcross1.gdot(mj_lambda1.angular) + - self.dir1.dot(&mj_lambda2.linear) + + elt.gcross2.gdot(mj_lambda2.angular) + + elt.rhs; + let new_impulse = (elt.impulse - elt.r * dimpulse).simd_max(SimdFloat::zero()); + let dlambda = new_impulse - elt.impulse; + elt.impulse = new_impulse; + + mj_lambda1.linear += self.dir1 * (self.im1 * dlambda); + mj_lambda1.angular += elt.gcross1 * dlambda; + mj_lambda2.linear += self.dir1 * (-self.im2 * dlambda); + mj_lambda2.angular += elt.gcross2 * dlambda; + } + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda1[ii] as usize].linear = mj_lambda1.linear.extract(ii); + mj_lambdas[self.mj_lambda1[ii] as usize].angular = mj_lambda1.angular.extract(ii); + } + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { + for k in 0..self.num_contacts as usize { + let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into(); + let tangent_impulses: [_; SIMD_WIDTH] = + self.elements[k].tangent_parts[0].impulse.into(); + #[cfg(feature = "dim3")] + let bitangent_impulses: [_; SIMD_WIDTH] = + self.elements[k].tangent_parts[1].impulse.into(); + + for ii in 0..SIMD_WIDTH { + let manifold = &mut manifolds_all[self.manifold_id[ii]]; + let k_base = self.manifold_contact_id; + let active_contacts = manifold.active_contacts_mut(); + active_contacts[k_base + k].impulse = impulses[ii]; + + #[cfg(feature = "dim2")] + { + active_contacts[k_base + k].tangent_impulse = tangent_impulses[ii]; + } + #[cfg(feature = "dim3")] + { + active_contacts[k_base + k].tangent_impulse = + [tangent_impulses[ii], bitangent_impulses[ii]]; + } + } + } + } +} diff --git a/src/dynamics/solver/velocity_ground_constraint.rs b/src/dynamics/solver/velocity_ground_constraint.rs new file mode 100644 index 000000000..457c3b36a --- /dev/null +++ b/src/dynamics/solver/velocity_ground_constraint.rs @@ -0,0 +1,300 @@ +use super::{AnyVelocityConstraint, DeltaVel}; +use crate::math::{AngVector, Vector, DIM, MAX_MANIFOLD_POINTS}; +use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; + +use crate::dynamics::{IntegrationParameters, RigidBodySet}; +use crate::geometry::{ContactManifold, ContactManifoldIndex}; +use simba::simd::SimdPartialOrd; + +#[derive(Copy, Clone, Debug)] +pub(crate) struct VelocityGroundConstraintElementPart { + pub gcross2: AngVector, + pub rhs: f32, + pub impulse: f32, + pub r: f32, +} + +#[cfg(not(target_arch = "wasm32"))] +impl VelocityGroundConstraintElementPart { + fn zero() -> Self { + Self { + gcross2: na::zero(), + rhs: 0.0, + impulse: 0.0, + r: 0.0, + } + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct VelocityGroundConstraintElement { + pub normal_part: VelocityGroundConstraintElementPart, + pub tangent_part: [VelocityGroundConstraintElementPart; DIM - 1], +} + +#[cfg(not(target_arch = "wasm32"))] +impl VelocityGroundConstraintElement { + pub fn zero() -> Self { + Self { + normal_part: VelocityGroundConstraintElementPart::zero(), + tangent_part: [VelocityGroundConstraintElementPart::zero(); DIM - 1], + } + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct VelocityGroundConstraint { + pub dir1: Vector, // Non-penetration force direction for the first body. + pub im2: f32, + pub limit: f32, + pub mj_lambda2: usize, + pub manifold_id: ContactManifoldIndex, + pub manifold_contact_id: usize, + pub num_contacts: u8, + pub elements: [VelocityGroundConstraintElement; MAX_MANIFOLD_POINTS], +} + +impl VelocityGroundConstraint { + pub fn generate( + params: &IntegrationParameters, + manifold_id: ContactManifoldIndex, + manifold: &ContactManifold, + bodies: &RigidBodySet, + out_constraints: &mut Vec, + push: bool, + ) { + let mut rb1 = &bodies[manifold.body_pair.body1]; + let mut rb2 = &bodies[manifold.body_pair.body2]; + let flipped = !rb2.is_dynamic(); + + if flipped { + std::mem::swap(&mut rb1, &mut rb2); + } + + let mj_lambda2 = rb2.active_set_offset; + let force_dir1 = if flipped { + // NOTE: we already swapped rb1 and rb2 + // so we multiply by rb1.position. + rb1.position * (-manifold.local_n2) + } else { + rb1.position * (-manifold.local_n1) + }; + + let warmstart_coeff = manifold.warmstart_multiplier * params.warmstart_coeff; + + for (l, manifold_points) in manifold + .active_contacts() + .chunks(MAX_MANIFOLD_POINTS) + .enumerate() + { + #[cfg(not(target_arch = "wasm32"))] + let mut constraint = VelocityGroundConstraint { + dir1: force_dir1, + elements: [VelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], + im2: rb2.mass_properties.inv_mass, + limit: manifold.friction, + mj_lambda2, + manifold_id, + manifold_contact_id: l * MAX_MANIFOLD_POINTS, + num_contacts: manifold_points.len() as u8, + }; + + // TODO: this is a WIP optimization for WASM platforms. + // For some reasons, the compiler does not inline the `Vec::push` method + // in this method. This generates two memset and one memcpy which are both very + // expansive. + // This would likely be solved by some kind of "placement-push" (like emplace in C++). + // In the mean time, a workaround is to "push" using `.resize_with` and `::uninit()` to + // avoid spurious copying. + // Is this optimization beneficial when targeting non-WASM platforms? + // + // NOTE: joints have the same problem, but it is not easy to refactor the code that way + // for the moment. + #[cfg(target_arch = "wasm32")] + let constraint = if push { + let new_len = out_constraints.len() + 1; + unsafe { + out_constraints.resize_with(new_len, || { + AnyVelocityConstraint::NongroupedGround( + std::mem::MaybeUninit::uninit().assume_init(), + ) + }); + } + out_constraints + .last_mut() + .unwrap() + .as_nongrouped_ground_mut() + .unwrap() + } else { + unreachable!(); // We don't have parallelization on WASM yet, so this is unreachable. + }; + + #[cfg(target_arch = "wasm32")] + { + constraint.dir1 = force_dir1; + constraint.im2 = rb2.mass_properties.inv_mass; + constraint.limit = manifold.friction; + constraint.mj_lambda2 = mj_lambda2; + constraint.manifold_id = manifold_id; + constraint.manifold_contact_id = l * MAX_MANIFOLD_POINTS; + constraint.num_contacts = manifold_points.len() as u8; + } + + for k in 0..manifold_points.len() { + let manifold_point = &manifold_points[k]; + let (p1, p2) = if flipped { + // NOTE: we already swapped rb1 and rb2 + // so we multiply by rb2.position. + ( + rb1.position * manifold_point.local_p2, + rb2.position * manifold_point.local_p1, + ) + } else { + ( + rb1.position * manifold_point.local_p1, + rb2.position * manifold_point.local_p2, + ) + }; + let dp2 = p2.coords - rb2.position.translation.vector; + let dp1 = p1.coords - rb1.position.translation.vector; + let vel1 = rb1.linvel + rb1.angvel.gcross(dp1); + let vel2 = rb2.linvel + rb2.angvel.gcross(dp2); + + // Normal part. + { + let gcross2 = rb2 + .world_inv_inertia_sqrt + .transform_vector(dp2.gcross(-force_dir1)); + + let r = 1.0 / (rb2.mass_properties.inv_mass + gcross2.gdot(gcross2)); + let rhs = -vel2.dot(&force_dir1) + + vel1.dot(&force_dir1) + + manifold_point.dist.max(0.0) * params.inv_dt(); + let impulse = manifold_points[k].impulse * warmstart_coeff; + + constraint.elements[k].normal_part = VelocityGroundConstraintElementPart { + gcross2, + rhs, + impulse, + r, + }; + } + + // Tangent parts. + { + let tangents1 = force_dir1.orthonormal_basis(); + + for j in 0..DIM - 1 { + let gcross2 = rb2 + .world_inv_inertia_sqrt + .transform_vector(dp2.gcross(-tangents1[j])); + let r = 1.0 / (rb2.mass_properties.inv_mass + gcross2.gdot(gcross2)); + let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]); + #[cfg(feature = "dim2")] + let impulse = manifold_points[k].tangent_impulse * warmstart_coeff; + #[cfg(feature = "dim3")] + let impulse = manifold_points[k].tangent_impulse[j] * warmstart_coeff; + + constraint.elements[k].tangent_part[j] = + VelocityGroundConstraintElementPart { + gcross2, + rhs, + impulse, + r, + }; + } + } + } + + #[cfg(not(target_arch = "wasm32"))] + if push { + out_constraints.push(AnyVelocityConstraint::NongroupedGround(constraint)); + } else { + out_constraints[manifold.constraint_index + l] = + AnyVelocityConstraint::NongroupedGround(constraint); + } + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = DeltaVel::zero(); + let tangents1 = self.dir1.orthonormal_basis(); + + for i in 0..self.num_contacts as usize { + let elt = &self.elements[i].normal_part; + mj_lambda2.linear += self.dir1 * (-self.im2 * elt.impulse); + mj_lambda2.angular += elt.gcross2 * elt.impulse; + + for j in 0..DIM - 1 { + let elt = &self.elements[i].tangent_part[j]; + mj_lambda2.linear += tangents1[j] * (-self.im2 * elt.impulse); + mj_lambda2.angular += elt.gcross2 * elt.impulse; + } + } + + mj_lambdas[self.mj_lambda2 as usize].linear += mj_lambda2.linear; + mj_lambdas[self.mj_lambda2 as usize].angular += mj_lambda2.angular; + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = mj_lambdas[self.mj_lambda2 as usize]; + + // Solve friction. + let tangents1 = self.dir1.orthonormal_basis(); + + for i in 0..self.num_contacts as usize { + for j in 0..DIM - 1 { + let normal_elt = &self.elements[i].normal_part; + let elt = &mut self.elements[i].tangent_part[j]; + let dimpulse = -tangents1[j].dot(&mj_lambda2.linear) + + elt.gcross2.gdot(mj_lambda2.angular) + + elt.rhs; + let limit = self.limit * normal_elt.impulse; + let new_impulse = (elt.impulse - elt.r * dimpulse).simd_clamp(-limit, limit); + let dlambda = new_impulse - elt.impulse; + elt.impulse = new_impulse; + + mj_lambda2.linear += tangents1[j] * (-self.im2 * dlambda); + mj_lambda2.angular += elt.gcross2 * dlambda; + } + } + + // Solve penetration. + for i in 0..self.num_contacts as usize { + let elt = &mut self.elements[i].normal_part; + let dimpulse = + -self.dir1.dot(&mj_lambda2.linear) + elt.gcross2.gdot(mj_lambda2.angular) + elt.rhs; + let new_impulse = (elt.impulse - elt.r * dimpulse).max(0.0); + let dlambda = new_impulse - elt.impulse; + elt.impulse = new_impulse; + + mj_lambda2.linear += self.dir1 * (-self.im2 * dlambda); + mj_lambda2.angular += elt.gcross2 * dlambda; + } + + mj_lambdas[self.mj_lambda2 as usize] = mj_lambda2; + } + + // FIXME: duplicated code. This is exactly the same as in the non-ground velocity constraint. + pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { + let manifold = &mut manifolds_all[self.manifold_id]; + let k_base = self.manifold_contact_id; + + for k in 0..self.num_contacts as usize { + let active_contacts = manifold.active_contacts_mut(); + active_contacts[k_base + k].impulse = self.elements[k].normal_part.impulse; + #[cfg(feature = "dim2")] + { + active_contacts[k_base + k].tangent_impulse = + self.elements[k].tangent_part[0].impulse; + } + #[cfg(feature = "dim3")] + { + active_contacts[k_base + k].tangent_impulse = [ + self.elements[k].tangent_part[0].impulse, + self.elements[k].tangent_part[1].impulse, + ]; + } + } + } +} diff --git a/src/dynamics/solver/velocity_ground_constraint_wide.rs b/src/dynamics/solver/velocity_ground_constraint_wide.rs new file mode 100644 index 000000000..ec9e18699 --- /dev/null +++ b/src/dynamics/solver/velocity_ground_constraint_wide.rs @@ -0,0 +1,300 @@ +use super::{AnyVelocityConstraint, DeltaVel}; +use crate::dynamics::{IntegrationParameters, RigidBodySet}; +use crate::geometry::{ContactManifold, ContactManifoldIndex}; +use crate::math::{ + AngVector, AngularInertia, Isometry, Point, SimdFloat, Vector, DIM, MAX_MANIFOLD_POINTS, + SIMD_WIDTH, +}; +use crate::utils::{WAngularInertia, WBasis, WCross, WDot}; +use num::Zero; +use simba::simd::{SimdPartialOrd, SimdValue}; + +#[derive(Copy, Clone, Debug)] +pub(crate) struct WVelocityGroundConstraintElementPart { + pub gcross2: AngVector, + pub rhs: SimdFloat, + pub impulse: SimdFloat, + pub r: SimdFloat, +} + +impl WVelocityGroundConstraintElementPart { + pub fn zero() -> Self { + Self { + gcross2: AngVector::zero(), + rhs: SimdFloat::zero(), + impulse: SimdFloat::zero(), + r: SimdFloat::zero(), + } + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct WVelocityGroundConstraintElement { + pub normal_part: WVelocityGroundConstraintElementPart, + pub tangent_parts: [WVelocityGroundConstraintElementPart; DIM - 1], +} + +impl WVelocityGroundConstraintElement { + pub fn zero() -> Self { + Self { + normal_part: WVelocityGroundConstraintElementPart::zero(), + tangent_parts: [WVelocityGroundConstraintElementPart::zero(); DIM - 1], + } + } +} + +#[derive(Copy, Clone, Debug)] +pub(crate) struct WVelocityGroundConstraint { + pub dir1: Vector, // Non-penetration force direction for the first body. + pub elements: [WVelocityGroundConstraintElement; MAX_MANIFOLD_POINTS], + pub num_contacts: u8, + pub im2: SimdFloat, + pub limit: SimdFloat, + pub mj_lambda2: [usize; SIMD_WIDTH], + pub manifold_id: [ContactManifoldIndex; SIMD_WIDTH], + pub manifold_contact_id: usize, +} + +impl WVelocityGroundConstraint { + pub fn generate( + params: &IntegrationParameters, + manifold_id: [ContactManifoldIndex; SIMD_WIDTH], + manifolds: [&ContactManifold; SIMD_WIDTH], + bodies: &RigidBodySet, + out_constraints: &mut Vec, + push: bool, + ) { + let inv_dt = SimdFloat::splat(params.inv_dt()); + let mut rbs1 = array![|ii| &bodies[manifolds[ii].body_pair.body1]; SIMD_WIDTH]; + let mut rbs2 = array![|ii| &bodies[manifolds[ii].body_pair.body2]; SIMD_WIDTH]; + let mut flipped = [false; SIMD_WIDTH]; + + for ii in 0..SIMD_WIDTH { + if !rbs2[ii].is_dynamic() { + std::mem::swap(&mut rbs1[ii], &mut rbs2[ii]); + flipped[ii] = true; + } + } + + let im2 = SimdFloat::from(array![|ii| rbs2[ii].mass_properties.inv_mass; SIMD_WIDTH]); + let ii2: AngularInertia = + AngularInertia::from(array![|ii| rbs2[ii].world_inv_inertia_sqrt; SIMD_WIDTH]); + + let linvel1 = Vector::from(array![|ii| rbs1[ii].linvel; SIMD_WIDTH]); + let angvel1 = AngVector::::from(array![|ii| rbs1[ii].angvel; SIMD_WIDTH]); + + let linvel2 = Vector::from(array![|ii| rbs2[ii].linvel; SIMD_WIDTH]); + let angvel2 = AngVector::::from(array![|ii| rbs2[ii].angvel; SIMD_WIDTH]); + + let position1 = Isometry::from(array![|ii| rbs1[ii].position; SIMD_WIDTH]); + let position2 = Isometry::from(array![|ii| rbs2[ii].position; SIMD_WIDTH]); + + let force_dir1 = position1 + * -Vector::from( + array![|ii| if flipped[ii] { manifolds[ii].local_n2 } else { manifolds[ii].local_n1 }; SIMD_WIDTH], + ); + + let mj_lambda2 = array![|ii| rbs2[ii].active_set_offset; SIMD_WIDTH]; + + let friction = SimdFloat::from(array![|ii| manifolds[ii].friction; SIMD_WIDTH]); + let warmstart_multiplier = + SimdFloat::from(array![|ii| manifolds[ii].warmstart_multiplier; SIMD_WIDTH]); + let warmstart_coeff = warmstart_multiplier * SimdFloat::splat(params.warmstart_coeff); + + for l in (0..manifolds[0].num_active_contacts()).step_by(MAX_MANIFOLD_POINTS) { + let manifold_points = array![|ii| &manifolds[ii].active_contacts()[l..]; SIMD_WIDTH]; + let num_points = manifold_points[0].len().min(MAX_MANIFOLD_POINTS); + + let mut constraint = WVelocityGroundConstraint { + dir1: force_dir1, + elements: [WVelocityGroundConstraintElement::zero(); MAX_MANIFOLD_POINTS], + im2, + limit: friction, + mj_lambda2, + manifold_id, + manifold_contact_id: l, + num_contacts: num_points as u8, + }; + + for k in 0..num_points { + let p1 = position1 + * Point::from( + array![|ii| if flipped[ii] { manifold_points[ii][k].local_p2 } else { manifold_points[ii][k].local_p1 }; SIMD_WIDTH], + ); + let p2 = position2 + * Point::from( + array![|ii| if flipped[ii] { manifold_points[ii][k].local_p1 } else { manifold_points[ii][k].local_p2 }; SIMD_WIDTH], + ); + + let dist = SimdFloat::from(array![|ii| manifold_points[ii][k].dist; SIMD_WIDTH]); + + let impulse = + SimdFloat::from(array![|ii| manifold_points[ii][k].impulse; SIMD_WIDTH]); + let dp1 = p1.coords - position1.translation.vector; + let dp2 = p2.coords - position2.translation.vector; + + let vel1 = linvel1 + angvel1.gcross(dp1); + let vel2 = linvel2 + angvel2.gcross(dp2); + + // Normal part. + { + let gcross2 = ii2.transform_vector(dp2.gcross(-force_dir1)); + + let r = SimdFloat::splat(1.0) / (im2 + gcross2.gdot(gcross2)); + let rhs = -vel2.dot(&force_dir1) + + vel1.dot(&force_dir1) + + dist.simd_max(SimdFloat::zero()) * inv_dt; + + constraint.elements[k].normal_part = WVelocityGroundConstraintElementPart { + gcross2, + rhs, + impulse: impulse * warmstart_coeff, + r, + }; + } + + // tangent parts. + let tangents1 = force_dir1.orthonormal_basis(); + + for j in 0..DIM - 1 { + #[cfg(feature = "dim2")] + let impulse = SimdFloat::from( + array![|ii| manifold_points[ii][k].tangent_impulse; SIMD_WIDTH], + ); + #[cfg(feature = "dim3")] + let impulse = SimdFloat::from( + array![|ii| manifold_points[ii][k].tangent_impulse[j]; SIMD_WIDTH], + ); + + let gcross2 = ii2.transform_vector(dp2.gcross(-tangents1[j])); + let r = SimdFloat::splat(1.0) / (im2 + gcross2.gdot(gcross2)); + let rhs = -vel2.dot(&tangents1[j]) + vel1.dot(&tangents1[j]); + + constraint.elements[k].tangent_parts[j] = + WVelocityGroundConstraintElementPart { + gcross2, + rhs, + impulse: impulse * warmstart_coeff, + r, + }; + } + } + + if push { + out_constraints.push(AnyVelocityConstraint::GroupedGround(constraint)); + } else { + out_constraints[manifolds[0].constraint_index + l / MAX_MANIFOLD_POINTS] = + AnyVelocityConstraint::GroupedGround(constraint); + } + } + } + + pub fn warmstart(&self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![|ii| mj_lambdas[self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + let tangents1 = self.dir1.orthonormal_basis(); + + for i in 0..self.num_contacts as usize { + let elt = &self.elements[i].normal_part; + mj_lambda2.linear += self.dir1 * (-self.im2 * elt.impulse); + mj_lambda2.angular += elt.gcross2 * elt.impulse; + + for j in 0..DIM - 1 { + let elt = &self.elements[i].tangent_parts[j]; + mj_lambda2.linear += tangents1[j] * (-self.im2 * elt.impulse); + mj_lambda2.angular += elt.gcross2 * elt.impulse; + } + } + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + pub fn solve(&mut self, mj_lambdas: &mut [DeltaVel]) { + let mut mj_lambda2 = DeltaVel { + linear: Vector::from( + array![ |ii| mj_lambdas[ self.mj_lambda2[ii] as usize].linear; SIMD_WIDTH], + ), + angular: AngVector::from( + array![ |ii| mj_lambdas[ self.mj_lambda2[ii] as usize].angular; SIMD_WIDTH], + ), + }; + + // Solve friction first. + let tangents1 = self.dir1.orthonormal_basis(); + + for i in 0..self.num_contacts as usize { + let normal_elt = &self.elements[i].normal_part; + + for j in 0..DIM - 1 { + let elt = &mut self.elements[i].tangent_parts[j]; + let dimpulse = -tangents1[j].dot(&mj_lambda2.linear) + + elt.gcross2.gdot(mj_lambda2.angular) + + elt.rhs; + let limit = self.limit * normal_elt.impulse; + let new_impulse = (elt.impulse - elt.r * dimpulse).simd_clamp(-limit, limit); + let dlambda = new_impulse - elt.impulse; + elt.impulse = new_impulse; + + mj_lambda2.linear += tangents1[j] * (-self.im2 * dlambda); + mj_lambda2.angular += elt.gcross2 * dlambda; + } + } + + // Solve non-penetration after friction. + for i in 0..self.num_contacts as usize { + let elt = &mut self.elements[i].normal_part; + let dimpulse = + -self.dir1.dot(&mj_lambda2.linear) + elt.gcross2.gdot(mj_lambda2.angular) + elt.rhs; + let new_impulse = (elt.impulse - elt.r * dimpulse).simd_max(SimdFloat::zero()); + let dlambda = new_impulse - elt.impulse; + elt.impulse = new_impulse; + + mj_lambda2.linear += self.dir1 * (-self.im2 * dlambda); + mj_lambda2.angular += elt.gcross2 * dlambda; + } + + for ii in 0..SIMD_WIDTH { + mj_lambdas[self.mj_lambda2[ii] as usize].linear = mj_lambda2.linear.extract(ii); + mj_lambdas[self.mj_lambda2[ii] as usize].angular = mj_lambda2.angular.extract(ii); + } + } + + // FIXME: duplicated code. This is exactly the same as in the non-ground velocity constraint. + pub fn writeback_impulses(&self, manifolds_all: &mut [&mut ContactManifold]) { + for k in 0..self.num_contacts as usize { + let impulses: [_; SIMD_WIDTH] = self.elements[k].normal_part.impulse.into(); + let tangent_impulses: [_; SIMD_WIDTH] = + self.elements[k].tangent_parts[0].impulse.into(); + #[cfg(feature = "dim3")] + let bitangent_impulses: [_; SIMD_WIDTH] = + self.elements[k].tangent_parts[1].impulse.into(); + + for ii in 0..SIMD_WIDTH { + let manifold = &mut manifolds_all[self.manifold_id[ii]]; + let k_base = self.manifold_contact_id; + let active_contacts = manifold.active_contacts_mut(); + active_contacts[k_base + k].impulse = impulses[ii]; + + #[cfg(feature = "dim2")] + { + active_contacts[k_base + k].tangent_impulse = tangent_impulses[ii]; + } + #[cfg(feature = "dim3")] + { + active_contacts[k_base + k].tangent_impulse = + [tangent_impulses[ii], bitangent_impulses[ii]]; + } + } + } + } +} diff --git a/src/dynamics/solver/velocity_solver.rs b/src/dynamics/solver/velocity_solver.rs new file mode 100644 index 000000000..593eb0f3e --- /dev/null +++ b/src/dynamics/solver/velocity_solver.rs @@ -0,0 +1,405 @@ +use super::{ + AnyJointVelocityConstraint, InteractionGroups, VelocityConstraint, VelocityGroundConstraint, +}; +#[cfg(feature = "simd-is-enabled")] +use super::{WVelocityConstraint, WVelocityGroundConstraint}; +use crate::dynamics::solver::categorization::{categorize_joints, categorize_velocity_contacts}; +use crate::dynamics::{ + solver::{AnyVelocityConstraint, DeltaVel}, + IntegrationParameters, JointGraphEdge, JointIndex, RigidBodySet, +}; +use crate::geometry::{ContactManifold, ContactManifoldIndex}; +#[cfg(feature = "simd-is-enabled")] +use crate::math::SIMD_WIDTH; +use crate::utils::WAngularInertia; + +pub(crate) struct VelocitySolver { + pub mj_lambdas: Vec>, + pub contact_part: VelocitySolverPart, + pub joint_part: VelocitySolverPart, +} + +impl VelocitySolver { + pub fn new() -> Self { + Self { + mj_lambdas: Vec::new(), + contact_part: VelocitySolverPart::new(), + joint_part: VelocitySolverPart::new(), + } + } + + pub fn init_constraints( + &mut self, + island_id: usize, + params: &IntegrationParameters, + bodies: &RigidBodySet, + manifolds: &[&mut ContactManifold], + manifold_indices: &[ContactManifoldIndex], + joints: &[JointGraphEdge], + joint_constraint_indices: &[JointIndex], + ) { + self.contact_part + .init_constraints(island_id, params, bodies, manifolds, manifold_indices); + self.joint_part.init_constraints( + island_id, + params, + bodies, + joints, + joint_constraint_indices, + ) + } + + pub fn solve_constraints( + &mut self, + island_id: usize, + params: &IntegrationParameters, + bodies: &mut RigidBodySet, + manifolds_all: &mut [&mut ContactManifold], + joints_all: &mut [JointGraphEdge], + ) { + self.mj_lambdas.clear(); + self.mj_lambdas + .resize(bodies.active_island(island_id).len(), DeltaVel::zero()); + + /* + * Warmstart constraints. + */ + for constraint in &self.joint_part.constraints { + constraint.warmstart(&mut self.mj_lambdas[..]); + } + + for constraint in &self.contact_part.constraints { + constraint.warmstart(&mut self.mj_lambdas[..]); + } + + /* + * Solve constraints. + */ + for _ in 0..params.max_velocity_iterations { + for constraint in &mut self.joint_part.constraints { + constraint.solve(&mut self.mj_lambdas[..]); + } + + for constraint in &mut self.contact_part.constraints { + constraint.solve(&mut self.mj_lambdas[..]); + } + } + + // Update velocities. + bodies.foreach_active_island_body_mut_internal(island_id, |_, rb| { + let dvel = self.mj_lambdas[rb.active_set_offset]; + rb.linvel += dvel.linear; + rb.angvel += rb.world_inv_inertia_sqrt.transform_vector(dvel.angular); + }); + + // Write impulses back into the manifold structures. + for constraint in &self.joint_part.constraints { + constraint.writeback_impulses(joints_all); + } + + for constraint in &self.contact_part.constraints { + constraint.writeback_impulses(manifolds_all); + } + } +} + +pub(crate) struct VelocitySolverPart { + pub not_ground_interactions: Vec, + pub ground_interactions: Vec, + pub interaction_groups: InteractionGroups, + pub ground_interaction_groups: InteractionGroups, + pub constraints: Vec, +} + +impl VelocitySolverPart { + pub fn new() -> Self { + Self { + not_ground_interactions: Vec::new(), + ground_interactions: Vec::new(), + interaction_groups: InteractionGroups::new(), + ground_interaction_groups: InteractionGroups::new(), + constraints: Vec::new(), + } + } +} + +impl VelocitySolverPart { + pub fn init_constraint_groups( + &mut self, + island_id: usize, + bodies: &RigidBodySet, + manifolds: &[&mut ContactManifold], + manifold_indices: &[ContactManifoldIndex], + ) { + self.not_ground_interactions.clear(); + self.ground_interactions.clear(); + categorize_velocity_contacts( + bodies, + manifolds, + manifold_indices, + &mut self.ground_interactions, + &mut self.not_ground_interactions, + ); + + self.interaction_groups.clear_groups(); + self.interaction_groups.group_manifolds( + island_id, + bodies, + manifolds, + &self.not_ground_interactions, + ); + + self.ground_interaction_groups.clear_groups(); + self.ground_interaction_groups.group_manifolds( + island_id, + bodies, + manifolds, + &self.ground_interactions, + ); + + // NOTE: uncomment this do disable SIMD contact resolution. + // self.interaction_groups + // .nongrouped_interactions + // .append(&mut self.interaction_groups.grouped_interactions); + // self.ground_interaction_groups + // .nongrouped_interactions + // .append(&mut self.ground_interaction_groups.grouped_interactions); + } + + pub fn init_constraints( + &mut self, + island_id: usize, + params: &IntegrationParameters, + bodies: &RigidBodySet, + manifolds: &[&mut ContactManifold], + manifold_indices: &[ContactManifoldIndex], + ) { + self.init_constraint_groups(island_id, bodies, manifolds, manifold_indices); + self.constraints.clear(); + #[cfg(feature = "simd-is-enabled")] + { + self.compute_grouped_constraints(params, bodies, manifolds); + } + self.compute_nongrouped_constraints(params, bodies, manifolds); + #[cfg(feature = "simd-is-enabled")] + { + self.compute_grouped_ground_constraints(params, bodies, manifolds); + } + self.compute_nongrouped_ground_constraints(params, bodies, manifolds); + } + + #[cfg(feature = "simd-is-enabled")] + fn compute_grouped_constraints( + &mut self, + params: &IntegrationParameters, + bodies: &RigidBodySet, + manifolds_all: &[&mut ContactManifold], + ) { + for manifolds_i in self + .interaction_groups + .grouped_interactions + .chunks_exact(SIMD_WIDTH) + { + let manifold_id = array![|ii| manifolds_i[ii]; SIMD_WIDTH]; + let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH]; + WVelocityConstraint::generate( + params, + manifold_id, + manifolds, + bodies, + &mut self.constraints, + true, + ); + } + } + + fn compute_nongrouped_constraints( + &mut self, + params: &IntegrationParameters, + bodies: &RigidBodySet, + manifolds_all: &[&mut ContactManifold], + ) { + for manifold_i in &self.interaction_groups.nongrouped_interactions { + let manifold = &manifolds_all[*manifold_i]; + VelocityConstraint::generate( + params, + *manifold_i, + manifold, + bodies, + &mut self.constraints, + true, + ); + } + } + + #[cfg(feature = "simd-is-enabled")] + fn compute_grouped_ground_constraints( + &mut self, + params: &IntegrationParameters, + bodies: &RigidBodySet, + manifolds_all: &[&mut ContactManifold], + ) { + for manifolds_i in self + .ground_interaction_groups + .grouped_interactions + .chunks_exact(SIMD_WIDTH) + { + let manifold_id = array![|ii| manifolds_i[ii]; SIMD_WIDTH]; + let manifolds = array![|ii| &*manifolds_all[manifolds_i[ii]]; SIMD_WIDTH]; + WVelocityGroundConstraint::generate( + params, + manifold_id, + manifolds, + bodies, + &mut self.constraints, + true, + ); + } + } + + fn compute_nongrouped_ground_constraints( + &mut self, + params: &IntegrationParameters, + bodies: &RigidBodySet, + manifolds_all: &[&mut ContactManifold], + ) { + for manifold_i in &self.ground_interaction_groups.nongrouped_interactions { + let manifold = &manifolds_all[*manifold_i]; + VelocityGroundConstraint::generate( + params, + *manifold_i, + manifold, + bodies, + &mut self.constraints, + true, + ) + } + } +} + +impl VelocitySolverPart { + pub fn init_constraints( + &mut self, + island_id: usize, + params: &IntegrationParameters, + bodies: &RigidBodySet, + joints: &[JointGraphEdge], + joint_constraint_indices: &[JointIndex], + ) { + // Generate constraints for joints. + self.not_ground_interactions.clear(); + self.ground_interactions.clear(); + categorize_joints( + bodies, + joints, + joint_constraint_indices, + &mut self.ground_interactions, + &mut self.not_ground_interactions, + ); + + self.constraints.clear(); + + self.interaction_groups.clear_groups(); + self.interaction_groups.group_joints( + island_id, + bodies, + joints, + &self.not_ground_interactions, + ); + + self.ground_interaction_groups.clear_groups(); + self.ground_interaction_groups.group_joints( + island_id, + bodies, + joints, + &self.ground_interactions, + ); + // NOTE: uncomment this do disable SIMD joint resolution. + // self.interaction_groups + // .nongrouped_interactions + // .append(&mut self.interaction_groups.grouped_interactions); + // self.ground_interaction_groups + // .nongrouped_interactions + // .append(&mut self.ground_interaction_groups.grouped_interactions); + + self.compute_nongrouped_joint_ground_constraints(params, bodies, joints); + #[cfg(feature = "simd-is-enabled")] + { + self.compute_grouped_joint_ground_constraints(params, bodies, joints); + } + self.compute_nongrouped_joint_constraints(params, bodies, joints); + #[cfg(feature = "simd-is-enabled")] + { + self.compute_grouped_joint_constraints(params, bodies, joints); + } + } + + fn compute_nongrouped_joint_ground_constraints( + &mut self, + params: &IntegrationParameters, + bodies: &RigidBodySet, + joints_all: &[JointGraphEdge], + ) { + for joint_i in &self.ground_interaction_groups.nongrouped_interactions { + let joint = &joints_all[*joint_i].weight; + let vel_constraint = + AnyJointVelocityConstraint::from_joint_ground(params, *joint_i, joint, bodies); + self.constraints.push(vel_constraint); + } + } + + #[cfg(feature = "simd-is-enabled")] + fn compute_grouped_joint_ground_constraints( + &mut self, + params: &IntegrationParameters, + bodies: &RigidBodySet, + joints_all: &[JointGraphEdge], + ) { + for joints_i in self + .ground_interaction_groups + .grouped_interactions + .chunks_exact(SIMD_WIDTH) + { + let joints_id = array![|ii| joints_i[ii]; SIMD_WIDTH]; + let joints = array![|ii| &joints_all[joints_i[ii]].weight; SIMD_WIDTH]; + let vel_constraint = AnyJointVelocityConstraint::from_wide_joint_ground( + params, joints_id, joints, bodies, + ); + self.constraints.push(vel_constraint); + } + } + + fn compute_nongrouped_joint_constraints( + &mut self, + params: &IntegrationParameters, + bodies: &RigidBodySet, + joints_all: &[JointGraphEdge], + ) { + for joint_i in &self.interaction_groups.nongrouped_interactions { + let joint = &joints_all[*joint_i].weight; + let vel_constraint = + AnyJointVelocityConstraint::from_joint(params, *joint_i, joint, bodies); + self.constraints.push(vel_constraint); + } + } + + #[cfg(feature = "simd-is-enabled")] + fn compute_grouped_joint_constraints( + &mut self, + params: &IntegrationParameters, + bodies: &RigidBodySet, + joints_all: &[JointGraphEdge], + ) { + for joints_i in self + .interaction_groups + .grouped_interactions + .chunks_exact(SIMD_WIDTH) + { + let joints_id = array![|ii| joints_i[ii]; SIMD_WIDTH]; + let joints = array![|ii| &joints_all[joints_i[ii]].weight; SIMD_WIDTH]; + let vel_constraint = + AnyJointVelocityConstraint::from_wide_joint(params, joints_id, joints, bodies); + self.constraints.push(vel_constraint); + } + } +} diff --git a/src/geometry/ball.rs b/src/geometry/ball.rs new file mode 100644 index 000000000..7f4ad0381 --- /dev/null +++ b/src/geometry/ball.rs @@ -0,0 +1,16 @@ +#[cfg(feature = "simd-is-enabled")] +use crate::math::{Point, SimdFloat}; + +#[cfg(feature = "simd-is-enabled")] +#[derive(Copy, Clone, Debug)] +pub(crate) struct WBall { + pub center: Point, + pub radius: SimdFloat, +} + +#[cfg(feature = "simd-is-enabled")] +impl WBall { + pub fn new(center: Point, radius: SimdFloat) -> Self { + WBall { center, radius } + } +} diff --git a/src/geometry/broad_phase.rs b/src/geometry/broad_phase.rs new file mode 100644 index 000000000..a43b7af23 --- /dev/null +++ b/src/geometry/broad_phase.rs @@ -0,0 +1,255 @@ +use crate::geometry::ColliderHandle; +use ncollide::bounding_volume::AABB; +#[cfg(feature = "simd-is-enabled")] +use { + crate::geometry::WAABB, + crate::math::{Point, SIMD_WIDTH}, + crate::utils::WVec, + simba::simd::SimdBool as _, +}; + +#[derive(Copy, Clone, Debug, PartialEq, Eq, Hash)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct ColliderPair { + pub collider1: ColliderHandle, + pub collider2: ColliderHandle, +} + +impl ColliderPair { + pub fn new(collider1: ColliderHandle, collider2: ColliderHandle) -> Self { + ColliderPair { + collider1, + collider2, + } + } + + pub fn new_sorted(collider1: ColliderHandle, collider2: ColliderHandle) -> Self { + if collider1.into_raw_parts().0 <= collider2.into_raw_parts().0 { + Self::new(collider1, collider2) + } else { + Self::new(collider2, collider1) + } + } + + pub fn swap(self) -> Self { + Self::new(self.collider2, self.collider1) + } + + pub fn zero() -> Self { + Self { + collider1: ColliderHandle::from_raw_parts(0, 0), + collider2: ColliderHandle::from_raw_parts(0, 0), + } + } +} + +pub struct WAABBHierarchyIntersections { + curr_level_interferences: Vec, + next_level_interferences: Vec, +} + +impl WAABBHierarchyIntersections { + pub fn new() -> Self { + Self { + curr_level_interferences: Vec::new(), + next_level_interferences: Vec::new(), + } + } + + pub fn computed_interferences(&self) -> &[usize] { + &self.curr_level_interferences[..] + } + + pub(crate) fn computed_interferences_mut(&mut self) -> &mut Vec { + &mut self.curr_level_interferences + } +} + +#[cfg(feature = "simd-is-enabled")] +#[derive(Clone)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct WAABBHierarchy { + levels: Vec>, +} + +#[cfg(feature = "simd-is-enabled")] +impl WAABBHierarchy { + pub fn new(aabbs: &[AABB]) -> Self { + let mut waabbs: Vec<_> = aabbs + .chunks_exact(SIMD_WIDTH) + .map(|aabbs| WAABB::from(array![|ii| aabbs[ii]; SIMD_WIDTH])) + .collect(); + + if aabbs.len() % SIMD_WIDTH != 0 { + let first_i = (aabbs.len() / SIMD_WIDTH) * SIMD_WIDTH; + let last_i = aabbs.len() - 1; + let last_waabb = + WAABB::from(array![|ii| aabbs[(first_i + ii).min(last_i)]; SIMD_WIDTH]); + waabbs.push(last_waabb); + } + + let mut levels = vec![waabbs]; + + loop { + let last_level = levels.last().unwrap(); + let mut next_level = Vec::new(); + + for chunk in last_level.chunks_exact(SIMD_WIDTH) { + let mins = Point::from(array![|ii| chunk[ii].mins.horizontal_inf(); SIMD_WIDTH]); + let maxs = Point::from(array![|ii| chunk[ii].maxs.horizontal_sup(); SIMD_WIDTH]); + next_level.push(WAABB::new(mins, maxs)); + } + + // Deal with the last non-exact chunk. + if last_level.len() % SIMD_WIDTH != 0 { + let first_id = (last_level.len() / SIMD_WIDTH) * SIMD_WIDTH; + let last_id = last_level.len() - 1; + let mins = array![|ii| last_level[(first_id + ii).min(last_id)] + .mins + .horizontal_inf(); SIMD_WIDTH]; + let maxs = array![|ii| last_level[(first_id + ii).min(last_id)] + .maxs + .horizontal_sup(); SIMD_WIDTH]; + + let mins = Point::from(mins); + let maxs = Point::from(maxs); + next_level.push(WAABB::new(mins, maxs)); + } + + if next_level.len() == 1 { + levels.push(next_level); + break; + } + + levels.push(next_level); + } + + Self { levels } + } + + pub fn compute_interferences_with( + &self, + aabb: AABB, + workspace: &mut WAABBHierarchyIntersections, + ) { + let waabb1 = WAABB::splat(aabb); + workspace.next_level_interferences.clear(); + workspace.curr_level_interferences.clear(); + workspace.curr_level_interferences.push(0); + + for level in self.levels.iter().rev() { + for i in &workspace.curr_level_interferences { + // This `if let` handle the case when `*i` is out of bounds because + // the initial number of aabbs was not a power of SIMD_WIDTH. + if let Some(waabb2) = level.get(*i) { + // NOTE: using `intersect.bitmask()` and performing bit comparisons + // is much more efficient than testing if each intersect.extract(i) is true. + let intersect = waabb1.intersects_lanewise(waabb2); + let bitmask = intersect.bitmask(); + + for j in 0..SIMD_WIDTH { + if (bitmask & (1 << j)) != 0 { + workspace.next_level_interferences.push(i * SIMD_WIDTH + j) + } + } + } + } + + std::mem::swap( + &mut workspace.curr_level_interferences, + &mut workspace.next_level_interferences, + ); + workspace.next_level_interferences.clear(); + } + } +} + +#[cfg(not(feature = "simd-is-enabled"))] +#[derive(Clone)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct WAABBHierarchy { + levels: Vec>>, +} + +#[cfg(not(feature = "simd-is-enabled"))] +impl WAABBHierarchy { + const GROUP_SIZE: usize = 4; + + pub fn new(aabbs: &[AABB]) -> Self { + use ncollide::bounding_volume::BoundingVolume; + + let mut levels = vec![aabbs.to_vec()]; + + loop { + let last_level = levels.last().unwrap(); + let mut next_level = Vec::new(); + + for chunk in last_level.chunks(Self::GROUP_SIZE) { + let mut merged = chunk[0]; + for aabb in &chunk[1..] { + merged.merge(aabb) + } + + next_level.push(merged); + } + + if next_level.len() == 1 { + levels.push(next_level); + break; + } + + levels.push(next_level); + } + + Self { levels } + } + + pub fn compute_interferences_with( + &self, + aabb1: AABB, + workspace: &mut WAABBHierarchyIntersections, + ) { + use ncollide::bounding_volume::BoundingVolume; + + workspace.next_level_interferences.clear(); + workspace.curr_level_interferences.clear(); + workspace.curr_level_interferences.push(0); + + for level in self.levels[1..].iter().rev() { + for i in &workspace.curr_level_interferences { + for j in 0..Self::GROUP_SIZE { + if let Some(aabb2) = level.get(*i + j) { + if aabb1.intersects(aabb2) { + workspace + .next_level_interferences + .push((i + j) * Self::GROUP_SIZE) + } + } + } + } + + std::mem::swap( + &mut workspace.curr_level_interferences, + &mut workspace.next_level_interferences, + ); + workspace.next_level_interferences.clear(); + } + + // Last level. + for i in &workspace.curr_level_interferences { + for j in 0..Self::GROUP_SIZE { + if let Some(aabb2) = self.levels[0].get(*i + j) { + if aabb1.intersects(aabb2) { + workspace.next_level_interferences.push(i + j) + } + } + } + } + + std::mem::swap( + &mut workspace.curr_level_interferences, + &mut workspace.next_level_interferences, + ); + workspace.next_level_interferences.clear(); + } +} diff --git a/src/geometry/broad_phase_multi_sap.rs b/src/geometry/broad_phase_multi_sap.rs new file mode 100644 index 000000000..8356339f8 --- /dev/null +++ b/src/geometry/broad_phase_multi_sap.rs @@ -0,0 +1,645 @@ +use crate::dynamics::RigidBodySet; +use crate::geometry::{ColliderHandle, ColliderPair, ColliderSet}; +use crate::math::{Point, Vector, DIM}; +#[cfg(feature = "enhanced-determinism")] +use crate::utils::FxHashMap32 as HashMap; +use bit_vec::BitVec; +use ncollide::bounding_volume::{BoundingVolume, AABB}; +#[cfg(not(feature = "enhanced-determinism"))] +use rustc_hash::FxHashMap as HashMap; +use std::cmp::Ordering; +use std::ops::{Index, IndexMut}; + +const NUM_SENTINELS: usize = 1; +const NEXT_FREE_SENTINEL: u32 = u32::MAX; +const SENTINEL_VALUE: f32 = f32::MAX; +const CELL_WIDTH: f32 = 20.0; + +pub enum BroadPhasePairEvent { + AddPair(ColliderPair), + DeletePair(ColliderPair), +} + +fn sort2(a: u32, b: u32) -> (u32, u32) { + assert_ne!(a, b); + + if a < b { + (a, b) + } else { + (b, a) + } +} + +fn point_key(point: Point) -> Point { + (point / CELL_WIDTH).coords.map(|e| e.floor() as i32).into() +} + +fn region_aabb(index: Point) -> AABB { + let mins = index.coords.map(|i| i as f32 * CELL_WIDTH).into(); + let maxs = mins + Vector::repeat(CELL_WIDTH); + AABB::new(mins, maxs) +} + +#[derive(Copy, Clone, Debug, PartialEq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +struct Endpoint { + value: f32, + packed_flag_proxy: u32, +} + +const START_FLAG_MASK: u32 = 0b1 << 31; +const PROXY_MASK: u32 = u32::MAX ^ START_FLAG_MASK; +const START_SENTINEL_TAG: u32 = u32::MAX; +const END_SENTINEL_TAG: u32 = u32::MAX ^ START_FLAG_MASK; + +impl Endpoint { + pub fn start_endpoint(value: f32, proxy: u32) -> Self { + Self { + value, + packed_flag_proxy: proxy | START_FLAG_MASK, + } + } + + pub fn end_endpoint(value: f32, proxy: u32) -> Self { + Self { + value, + packed_flag_proxy: proxy & PROXY_MASK, + } + } + + pub fn start_sentinel() -> Self { + Self { + value: -SENTINEL_VALUE, + packed_flag_proxy: START_SENTINEL_TAG, + } + } + + pub fn end_sentinel() -> Self { + Self { + value: SENTINEL_VALUE, + packed_flag_proxy: END_SENTINEL_TAG, + } + } + + pub fn is_sentinel(self) -> bool { + self.packed_flag_proxy & PROXY_MASK == PROXY_MASK + } + + pub fn proxy(self) -> u32 { + self.packed_flag_proxy & PROXY_MASK + } + + pub fn is_start(self) -> bool { + (self.packed_flag_proxy & START_FLAG_MASK) != 0 + } + + pub fn is_end(self) -> bool { + (self.packed_flag_proxy & START_FLAG_MASK) == 0 + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +struct SAPAxis { + min_bound: f32, + max_bound: f32, + endpoints: Vec, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + new_endpoints: Vec<(Endpoint, usize)>, // Workspace +} + +impl SAPAxis { + fn new(min_bound: f32, max_bound: f32) -> Self { + assert!(min_bound <= max_bound); + + Self { + min_bound, + max_bound, + endpoints: vec![Endpoint::start_sentinel(), Endpoint::end_sentinel()], + new_endpoints: Vec::new(), + } + } + + fn batch_insert( + &mut self, + dim: usize, + new_proxies: &[usize], + proxies: &Proxies, + reporting: Option<&mut HashMap<(u32, u32), bool>>, + ) { + if new_proxies.is_empty() { + return; + } + + self.new_endpoints.clear(); + + for proxy_id in new_proxies { + let proxy = &proxies[*proxy_id]; + assert!(proxy.aabb.mins[dim] <= self.max_bound); + assert!(proxy.aabb.maxs[dim] >= self.min_bound); + let start_endpoint = Endpoint::start_endpoint(proxy.aabb.mins[dim], *proxy_id as u32); + let end_endpoint = Endpoint::end_endpoint(proxy.aabb.maxs[dim], *proxy_id as u32); + + self.new_endpoints.push((start_endpoint, 0)); + self.new_endpoints.push((end_endpoint, 0)); + } + + self.new_endpoints + .sort_by(|a, b| a.0.value.partial_cmp(&b.0.value).unwrap_or(Ordering::Equal)); + + let mut curr_existing_index = self.endpoints.len() - NUM_SENTINELS - 1; + let new_num_endpoints = self.endpoints.len() + self.new_endpoints.len(); + self.endpoints + .resize(new_num_endpoints, Endpoint::end_sentinel()); + let mut curr_shift_index = new_num_endpoints - NUM_SENTINELS - 1; + + // Sort the endpoints. + // TODO: specialize for the case where this is the + // first time we insert endpoints to this axis? + for new_endpoint in self.new_endpoints.iter_mut().rev() { + loop { + let existing_endpoint = self.endpoints[curr_existing_index]; + if existing_endpoint.value <= new_endpoint.0.value { + break; + } + + self.endpoints[curr_shift_index] = existing_endpoint; + + curr_shift_index -= 1; + curr_existing_index -= 1; + } + + self.endpoints[curr_shift_index] = new_endpoint.0; + new_endpoint.1 = curr_shift_index; + curr_shift_index -= 1; + } + + // Report pairs using a single mbp pass on each new endpoint. + let endpoints_wo_last_sentinel = &self.endpoints[..self.endpoints.len() - 1]; + if let Some(reporting) = reporting { + for (endpoint, endpoint_id) in self.new_endpoints.drain(..).filter(|e| e.0.is_start()) { + let proxy1 = &proxies[endpoint.proxy() as usize]; + let min = endpoint.value; + let max = proxy1.aabb.maxs[dim]; + + for endpoint2 in &endpoints_wo_last_sentinel[endpoint_id + 1..] { + if endpoint2.proxy() == endpoint.proxy() { + continue; + } + + let proxy2 = &proxies[endpoint2.proxy() as usize]; + + // NOTE: some pairs with equal aabb.mins[dim] may end up being reported twice. + if (endpoint2.is_start() && endpoint2.value < max) + || (endpoint2.is_end() && proxy2.aabb.mins[dim] <= min) + { + // Report pair. + if proxy1.aabb.intersects(&proxy2.aabb) { + // Report pair. + let pair = sort2(endpoint.proxy(), endpoint2.proxy()); + reporting.insert(pair, true); + } + } + } + } + } + } + + fn delete_out_of_bounds_proxies(&self, existing_proxies: &mut BitVec) -> bool { + let mut deleted_any = false; + for endpoint in &self.endpoints { + if endpoint.value < self.min_bound { + if endpoint.is_end() { + existing_proxies.set(endpoint.proxy() as usize, false); + deleted_any = true; + } + } else { + break; + } + } + + for endpoint in self.endpoints.iter().rev() { + if endpoint.value > self.max_bound { + if endpoint.is_start() { + existing_proxies.set(endpoint.proxy() as usize, false); + deleted_any = true; + } + } else { + break; + } + } + + deleted_any + } + + fn delete_out_of_bounds_endpoints(&mut self, existing_proxies: &BitVec) { + self.endpoints + .retain(|endpt| endpt.is_sentinel() || existing_proxies[endpt.proxy() as usize]) + } + + fn update_endpoints( + &mut self, + dim: usize, + proxies: &Proxies, + reporting: &mut HashMap<(u32, u32), bool>, + ) { + let last_endpoint = self.endpoints.len() - NUM_SENTINELS; + for i in NUM_SENTINELS..last_endpoint { + let mut endpoint_i = self.endpoints[i]; + let aabb_i = proxies[endpoint_i.proxy() as usize].aabb; + + if endpoint_i.is_start() { + endpoint_i.value = aabb_i.mins[dim]; + } else { + endpoint_i.value = aabb_i.maxs[dim]; + } + + let mut j = i; + + if endpoint_i.is_start() { + while endpoint_i.value < self.endpoints[j - 1].value { + let endpoint_j = self.endpoints[j - 1]; + self.endpoints[j] = endpoint_j; + + if endpoint_j.is_end() { + // Report start collision. + if aabb_i.intersects(&proxies[endpoint_j.proxy() as usize].aabb) { + let pair = sort2(endpoint_i.proxy(), endpoint_j.proxy()); + reporting.insert(pair, true); + } + } + + j -= 1; + } + } else { + while endpoint_i.value < self.endpoints[j - 1].value { + let endpoint_j = self.endpoints[j - 1]; + self.endpoints[j] = endpoint_j; + + if endpoint_j.is_start() { + // Report end collision. + if !aabb_i.intersects(&proxies[endpoint_j.proxy() as usize].aabb) { + let pair = sort2(endpoint_i.proxy(), endpoint_j.proxy()); + reporting.insert(pair, false); + } + } + + j -= 1; + } + } + + self.endpoints[j] = endpoint_i; + } + + // println!( + // "Num start swaps: {}, end swaps: {}, dim: {}", + // num_start_swaps, num_end_swaps, dim + // ); + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +struct SAPRegion { + axii: [SAPAxis; DIM], + existing_proxies: BitVec, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + to_insert: Vec, // Workspace + need_update: bool, +} + +impl SAPRegion { + pub fn new(bounds: AABB) -> Self { + let axii = [ + SAPAxis::new(bounds.mins.x, bounds.maxs.x), + SAPAxis::new(bounds.mins.y, bounds.maxs.y), + #[cfg(feature = "dim3")] + SAPAxis::new(bounds.mins.z, bounds.maxs.z), + ]; + SAPRegion { + axii, + existing_proxies: BitVec::new(), + to_insert: Vec::new(), + need_update: false, + } + } + + pub fn predelete_proxy(&mut self, _proxy_id: usize) { + // We keep the proxy_id as argument for uniformity with the "preupdate" + // method. However we don't actually need it because the deletion will be + // handled transparently during the next update. + self.need_update = true; + } + + pub fn preupdate_proxy(&mut self, proxy_id: usize) -> bool { + let mask_len = self.existing_proxies.len(); + if proxy_id >= mask_len { + self.existing_proxies.grow(proxy_id + 1 - mask_len, false); + } + + if !self.existing_proxies[proxy_id] { + self.to_insert.push(proxy_id); + self.existing_proxies.set(proxy_id, true); + false + } else { + self.need_update = true; + true + } + } + + pub fn update(&mut self, proxies: &Proxies, reporting: &mut HashMap<(u32, u32), bool>) { + if self.need_update { + // Update endpoints. + let mut deleted_any = false; + for dim in 0..DIM { + self.axii[dim].update_endpoints(dim, proxies, reporting); + deleted_any = self.axii[dim] + .delete_out_of_bounds_proxies(&mut self.existing_proxies) + || deleted_any; + } + + if deleted_any { + for dim in 0..DIM { + self.axii[dim].delete_out_of_bounds_endpoints(&self.existing_proxies); + } + } + + self.need_update = false; + } + + if !self.to_insert.is_empty() { + // Insert new proxies. + for dim in 1..DIM { + self.axii[dim].batch_insert(dim, &self.to_insert, proxies, None); + } + self.axii[0].batch_insert(0, &self.to_insert, proxies, Some(reporting)); + self.to_insert.clear(); + } + } +} + +/// A broad-phase based on multiple Sweep-and-Prune instances running of disjoint region of the 3D world. +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct BroadPhase { + proxies: Proxies, + regions: HashMap, SAPRegion>, + deleted_any: bool, + // We could think serializing this workspace is useless. + // It turns out is is important to serialize at least its capacity + // and restore this capacity when deserializing the hashmap. + // This is because the order of future elements inserted into the + // hashmap depends on its capacity (because the internal bucket indices + // depend on this capacity). So not restoring this capacity may alter + // the order at which future elements are reported. This will in turn + // alter the order at which the pairs are registered in the narrow-phase, + // thus altering the order of the contact manifold. In the end, this + // alters the order of the resolution of contacts, resulting in + // diverging simulation after restoration of a snapshot. + #[cfg_attr( + feature = "serde-serialize", + serde( + serialize_with = "crate::utils::serialize_hashmap_capacity", + deserialize_with = "crate::utils::deserialize_hashmap_capacity" + ) + )] + reporting: HashMap<(u32, u32), bool>, // Workspace +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub(crate) struct BroadPhaseProxy { + handle: ColliderHandle, + aabb: AABB, + next_free: u32, +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +struct Proxies { + elements: Vec, + first_free: u32, +} + +impl Proxies { + pub fn new() -> Self { + Self { + elements: Vec::new(), + first_free: NEXT_FREE_SENTINEL, + } + } + + pub fn insert(&mut self, proxy: BroadPhaseProxy) -> usize { + if self.first_free != NEXT_FREE_SENTINEL { + let proxy_id = self.first_free; + self.first_free = self.elements[self.first_free as usize].next_free; + self.elements[self.first_free as usize] = proxy; + proxy_id as usize + } else { + self.elements.push(proxy); + self.elements.len() - 1 + } + } + + pub fn remove(&mut self, proxy_id: usize) { + self.elements[proxy_id].next_free = self.first_free; + self.first_free = proxy_id as u32; + } + + // // FIXME: take holes into account? + // pub fn get(&self, i: usize) -> Option<&BroadPhaseProxy> { + // self.elements.get(i) + // } + + // FIXME: take holes into account? + pub fn get_mut(&mut self, i: usize) -> Option<&mut BroadPhaseProxy> { + self.elements.get_mut(i) + } +} + +impl Index for Proxies { + type Output = BroadPhaseProxy; + fn index(&self, i: usize) -> &BroadPhaseProxy { + self.elements.index(i) + } +} + +impl IndexMut for Proxies { + fn index_mut(&mut self, i: usize) -> &mut BroadPhaseProxy { + self.elements.index_mut(i) + } +} + +impl BroadPhase { + /// Create a new empty broad-phase. + pub fn new() -> Self { + BroadPhase { + proxies: Proxies::new(), + regions: HashMap::default(), + reporting: HashMap::default(), + deleted_any: false, + } + } + + pub(crate) fn remove_colliders(&mut self, handles: &[ColliderHandle], colliders: &ColliderSet) { + for collider in handles.iter().filter_map(|h| colliders.get(*h)) { + if collider.proxy_index == crate::INVALID_USIZE { + // This collider has not been added to the broad-phase yet. + continue; + } + + let proxy = &mut self.proxies[collider.proxy_index]; + + // Push the proxy to infinity, but not beyond the sentinels. + proxy.aabb.mins.coords.fill(SENTINEL_VALUE / 2.0); + proxy.aabb.maxs.coords.fill(SENTINEL_VALUE / 2.0); + // Discretize the AABB to find the regions that need to be invalidated. + let start = point_key(proxy.aabb.mins); + let end = point_key(proxy.aabb.maxs); + + #[cfg(feature = "dim2")] + for i in start.x..=end.x { + for j in start.y..=end.y { + if let Some(region) = self.regions.get_mut(&Point::new(i, j)) { + region.predelete_proxy(collider.proxy_index); + self.deleted_any = true; + } + } + } + + #[cfg(feature = "dim3")] + for i in start.x..=end.x { + for j in start.y..=end.y { + for k in start.z..=end.z { + if let Some(region) = self.regions.get_mut(&Point::new(i, j, k)) { + region.predelete_proxy(collider.proxy_index); + self.deleted_any = true; + } + } + } + } + + self.proxies.remove(collider.proxy_index); + } + } + + pub(crate) fn update_aabbs( + &mut self, + prediction_distance: f32, + bodies: &RigidBodySet, + colliders: &mut ColliderSet, + ) { + // First, if we have any pending removals we have + // to deal with them now because otherwise we will + // end up with an ABA problems when reusing proxy + // ids. + self.complete_removals(); + + for body_handle in bodies + .active_dynamic_set + .iter() + .chain(bodies.active_kinematic_set.iter()) + { + for handle in &bodies[*body_handle].colliders { + let collider = &mut colliders[*handle]; + let aabb = collider.compute_aabb().loosened(prediction_distance / 2.0); + + if let Some(proxy) = self.proxies.get_mut(collider.proxy_index) { + proxy.aabb = aabb; + } else { + let proxy = BroadPhaseProxy { + handle: *handle, + aabb, + next_free: NEXT_FREE_SENTINEL, + }; + collider.proxy_index = self.proxies.insert(proxy); + } + + // Discretize the aabb. + let proxy_id = collider.proxy_index; + // let start = Point::origin(); + // let end = Point::origin(); + let start = point_key(aabb.mins); + let end = point_key(aabb.maxs); + + #[cfg(feature = "dim2")] + for i in start.x..=end.x { + for j in start.y..=end.y { + let region_key = Point::new(i, j); + let region_bounds = region_aabb(region_key); + let region = self + .regions + .entry(region_key) + .or_insert_with(|| SAPRegion::new(region_bounds)); + let _ = region.preupdate_proxy(proxy_id); + } + } + + #[cfg(feature = "dim3")] + for i in start.x..=end.x { + for j in start.y..=end.y { + for k in start.z..=end.z { + let region_key = Point::new(i, j, k); + let region_bounds = region_aabb(region_key); + let region = self + .regions + .entry(region_key) + .or_insert_with(|| SAPRegion::new(region_bounds)); + let _ = region.preupdate_proxy(proxy_id); + } + } + } + } + } + } + + pub(crate) fn complete_removals(&mut self) { + if self.deleted_any { + for (_, region) in &mut self.regions { + region.update(&self.proxies, &mut self.reporting); + } + + // NOTE: we don't care about reporting pairs. + self.reporting.clear(); + self.deleted_any = false; + } + } + + pub(crate) fn find_pairs(&mut self, out_events: &mut Vec) { + // println!("num regions: {}", self.regions.len()); + + self.reporting.clear(); + for (_, region) in &mut self.regions { + region.update(&self.proxies, &mut self.reporting) + } + + // Convert reports to broad phase events. + // let t = instant::now(); + // let mut num_add_events = 0; + // let mut num_delete_events = 0; + + for ((proxy1, proxy2), colliding) in &self.reporting { + let proxy1 = &self.proxies[*proxy1 as usize]; + let proxy2 = &self.proxies[*proxy2 as usize]; + + let handle1 = proxy1.handle; + let handle2 = proxy2.handle; + + if *colliding { + out_events.push(BroadPhasePairEvent::AddPair(ColliderPair::new( + handle1, handle2, + ))); + // num_add_events += 1; + } else { + out_events.push(BroadPhasePairEvent::DeletePair(ColliderPair::new( + handle1, handle2, + ))); + // num_delete_events += 1; + } + } + + // println!( + // "Event conversion time: {}, add: {}/{}, delete: {}/{}", + // instant::now() - t, + // num_add_events, + // out_events.len(), + // num_delete_events, + // out_events.len() + // ); + } +} diff --git a/src/geometry/capsule.rs b/src/geometry/capsule.rs new file mode 100644 index 000000000..0d754afa2 --- /dev/null +++ b/src/geometry/capsule.rs @@ -0,0 +1,168 @@ +use crate::geometry::AABB; +use crate::math::{Isometry, Point, Rotation, Vector}; +use approx::AbsDiffEq; +use na::Unit; +use ncollide::query::{PointProjection, PointQuery}; +use ncollide::shape::{FeatureId, Segment}; + +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// A capsule shape defined as a segment with a radius. +pub struct Capsule { + /// The first endpoint of the capsule. + pub a: Point, + /// The second enpdoint of the capsule. + pub b: Point, + /// The radius of the capsule. + pub radius: f32, +} + +impl Capsule { + /// Creates a new capsule aligned with the `x` axis and with the given half-height an radius. + pub fn new_x(half_height: f32, radius: f32) -> Self { + let b = Point::from(Vector::x() * half_height); + Self::new(-b, b, radius) + } + + /// Creates a new capsule aligned with the `y` axis and with the given half-height an radius. + pub fn new_y(half_height: f32, radius: f32) -> Self { + let b = Point::from(Vector::y() * half_height); + Self::new(-b, b, radius) + } + + /// Creates a new capsule aligned with the `z` axis and with the given half-height an radius. + #[cfg(feature = "dim3")] + pub fn new_z(half_height: f32, radius: f32) -> Self { + let b = Point::from(Vector::z() * half_height); + Self::new(-b, b, radius) + } + + /// Creates a new capsule defined as the segment between `a` and `b` and with the given `radius`. + pub fn new(a: Point, b: Point, radius: f32) -> Self { + Self { a, b, radius } + } + + /// The axis-aligned bounding box of this capsule. + pub fn aabb(&self, pos: &Isometry) -> AABB { + let a = pos * self.a; + let b = pos * self.b; + let mins = a.coords.inf(&b.coords) - Vector::repeat(self.radius); + let maxs = a.coords.sup(&b.coords) + Vector::repeat(self.radius); + AABB::new(mins.into(), maxs.into()) + } + + /// The height of this capsule. + pub fn height(&self) -> f32 { + (self.b - self.a).norm() + } + + /// The half-height of this capsule. + pub fn half_height(&self) -> f32 { + self.height() / 2.0 + } + + /// The center of this capsule. + pub fn center(&self) -> Point { + na::center(&self.a, &self.b) + } + + /// Creates a new capsule equal to `self` with all its endpoints transformed by `pos`. + pub fn transform_by(&self, pos: &Isometry) -> Self { + Self::new(pos * self.a, pos * self.b, self.radius) + } + + /// The rotation `r` such that `r * Y` is collinear with `b - a`. + pub fn rotation_wrt_y(&self) -> Rotation { + let mut dir = self.b - self.a; + if dir.y < 0.0 { + dir = -dir; + } + + #[cfg(feature = "dim2")] + { + Rotation::rotation_between(&Vector::y(), &dir) + } + + #[cfg(feature = "dim3")] + { + Rotation::rotation_between(&Vector::y(), &dir).unwrap_or(Rotation::identity()) + } + } + + /// The transform `t` such that `t * Y` is collinear with `b - a` and such that `t * origin = (b + a) / 2.0`. + pub fn transform_wrt_y(&self) -> Isometry { + let rot = self.rotation_wrt_y(); + Isometry::from_parts(self.center().coords.into(), rot) + } +} + +// impl SupportMap for Capsule { +// fn local_support_point(&self, dir: &Vector) -> Point { +// let dir = Unit::try_new(dir, 0.0).unwrap_or(Vector::y_axis()); +// self.local_support_point_toward(&dir) +// } +// +// fn local_support_point_toward(&self, dir: &Unit) -> Point { +// if dir.dot(&self.a.coords) > dir.dot(&self.b.coords) { +// self.a + **dir * self.radius +// } else { +// self.b + **dir * self.radius +// } +// } +// } + +// TODO: this code has been extracted from ncollide and added here +// so we can modify it to fit with our new definition of capsule. +// Wa should find a way to avoid this code duplication. +impl PointQuery for Capsule { + #[inline] + fn project_point( + &self, + m: &Isometry, + pt: &Point, + solid: bool, + ) -> PointProjection { + let seg = Segment::new(self.a, self.b); + let proj = seg.project_point(m, pt, solid); + let dproj = *pt - proj.point; + + if let Some((dir, dist)) = Unit::try_new_and_get(dproj, f32::default_epsilon()) { + let inside = dist <= self.radius; + if solid && inside { + return PointProjection::new(true, *pt); + } else { + return PointProjection::new(inside, proj.point + dir.into_inner() * self.radius); + } + } else if solid { + return PointProjection::new(true, *pt); + } + + #[cfg(feature = "dim2")] + if let Some(dir) = seg.normal() { + let dir = m * *dir; + PointProjection::new(true, proj.point + dir * self.radius) + } else { + // The segment has no normal, likely because it degenerates to a point. + PointProjection::new(true, proj.point + Vector::ith(1, self.radius)) + } + + #[cfg(feature = "dim3")] + if let Some(dir) = seg.direction() { + use crate::utils::WBasis; + let dir = m * dir.orthonormal_basis()[0]; + PointProjection::new(true, proj.point + dir * self.radius) + } else { + // The segment has no normal, likely because it degenerates to a point. + PointProjection::new(true, proj.point + Vector::ith(1, self.radius)) + } + } + + #[inline] + fn project_point_with_feature( + &self, + m: &Isometry, + pt: &Point, + ) -> (PointProjection, FeatureId) { + (self.project_point(m, pt, false), FeatureId::Face(0)) + } +} diff --git a/src/geometry/collider.rs b/src/geometry/collider.rs new file mode 100644 index 000000000..2457212f3 --- /dev/null +++ b/src/geometry/collider.rs @@ -0,0 +1,373 @@ +use crate::dynamics::{MassProperties, RigidBodyHandle, RigidBodySet}; +use crate::geometry::{ + Ball, Capsule, ColliderGraphIndex, Contact, Cuboid, HeightField, InteractionGraph, Polygon, + Proximity, Triangle, Trimesh, +}; +use crate::math::{Isometry, Point, Vector}; +use na::Point3; +use ncollide::bounding_volume::{HasBoundingVolume, AABB}; +use num::Zero; + +#[derive(Clone)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// An enum grouping all the possible shape of a collider. +pub enum Shape { + /// A ball shape. + Ball(Ball), + /// A convex polygon shape. + Polygon(Polygon), + /// A cuboid shape. + Cuboid(Cuboid), + /// A capsule shape. + Capsule(Capsule), + /// A triangle shape. + Triangle(Triangle), + /// A triangle mesh shape. + Trimesh(Trimesh), + /// A heightfield shape. + HeightField(HeightField), +} + +impl Shape { + /// Gets a reference to the underlying ball shape, if `self` is one. + pub fn as_ball(&self) -> Option<&Ball> { + match self { + Shape::Ball(b) => Some(b), + _ => None, + } + } + + /// Gets a reference to the underlying polygon shape, if `self` is one. + pub fn as_polygon(&self) -> Option<&Polygon> { + match self { + Shape::Polygon(p) => Some(p), + _ => None, + } + } + + /// Gets a reference to the underlying cuboid shape, if `self` is one. + pub fn as_cuboid(&self) -> Option<&Cuboid> { + match self { + Shape::Cuboid(c) => Some(c), + _ => None, + } + } + + /// Gets a reference to the underlying capsule shape, if `self` is one. + pub fn as_capsule(&self) -> Option<&Capsule> { + match self { + Shape::Capsule(c) => Some(c), + _ => None, + } + } + + /// Gets a reference to the underlying triangle mesh shape, if `self` is one. + pub fn as_trimesh(&self) -> Option<&Trimesh> { + match self { + Shape::Trimesh(c) => Some(c), + _ => None, + } + } + + /// Gets a reference to the underlying heightfield shape, if `self` is one. + pub fn as_heightfield(&self) -> Option<&HeightField> { + match self { + Shape::HeightField(h) => Some(h), + _ => None, + } + } + + /// Gets a reference to the underlying triangle shape, if `self` is one. + pub fn as_triangle(&self) -> Option<&Triangle> { + match self { + Shape::Triangle(c) => Some(c), + _ => None, + } + } + + /// Computes the axis-aligned bounding box of this shape. + pub fn compute_aabb(&self, position: &Isometry) -> AABB { + match self { + Shape::Ball(ball) => ball.bounding_volume(position), + Shape::Polygon(poly) => poly.aabb(position), + Shape::Capsule(caps) => caps.aabb(position), + Shape::Cuboid(cuboid) => cuboid.bounding_volume(position), + Shape::Triangle(triangle) => triangle.bounding_volume(position), + Shape::Trimesh(trimesh) => trimesh.aabb(position), + Shape::HeightField(heightfield) => heightfield.bounding_volume(position), + } + } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// A geometric entity that can be attached to a body so it can be affected by contacts and proximity queries. +/// +/// To build a new collider, use the `ColliderBuilder` structure. +pub struct Collider { + shape: Shape, + density: f32, + is_sensor: bool, + pub(crate) parent: RigidBodyHandle, + pub(crate) delta: Isometry, + pub(crate) position: Isometry, + pub(crate) predicted_position: Isometry, + /// The friction coefficient of this collider. + pub friction: f32, + /// The restitution coefficient of this collider. + pub restitution: f32, + pub(crate) contact_graph_index: ColliderGraphIndex, + pub(crate) proximity_graph_index: ColliderGraphIndex, + pub(crate) proxy_index: usize, +} + +impl Clone for Collider { + fn clone(&self) -> Self { + Self { + shape: self.shape.clone(), + parent: RigidBodySet::invalid_handle(), + contact_graph_index: ColliderGraphIndex::new(crate::INVALID_U32), + proximity_graph_index: ColliderGraphIndex::new(crate::INVALID_U32), + proxy_index: crate::INVALID_USIZE, + ..*self + } + } +} + +impl Collider { + /// The rigid body this collider is attached to. + pub fn parent(&self) -> RigidBodyHandle { + self.parent + } + + /// Is this collider a sensor? + pub fn is_sensor(&self) -> bool { + self.is_sensor + } + + #[doc(hidden)] + pub fn set_position_debug(&mut self, position: Isometry) { + self.position = position; + } + + /// The position of this collider expressed in the local-space of the rigid-body it is attached to. + pub fn delta(&self) -> &Isometry { + &self.delta + } + + /// The world-space position of this collider. + pub fn position(&self) -> &Isometry { + &self.position + } + + /// The density of this collider. + pub fn density(&self) -> f32 { + self.density + } + + /// The geometric shape of this collider. + pub fn shape(&self) -> &Shape { + &self.shape + } + + /// Compute the axis-aligned bounding box of this collider. + pub fn compute_aabb(&self) -> AABB { + self.shape.compute_aabb(&self.position) + } + + // pub(crate) fn compute_aabb_with_prediction(&self) -> AABB { + // let aabb1 = self.shape.compute_aabb(&self.position); + // let aabb2 = self.shape.compute_aabb(&self.predicted_position); + // aabb1.merged(&aabb2) + // } + + /// Compute the local-space mass properties of this collider. + pub fn mass_properties(&self) -> MassProperties { + match &self.shape { + Shape::Ball(ball) => MassProperties::from_ball(self.density, ball.radius), + #[cfg(feature = "dim2")] + Shape::Polygon(p) => MassProperties::from_polygon(self.density, p.vertices()), + #[cfg(feature = "dim3")] + Shape::Polygon(_p) => unimplemented!(), + Shape::Cuboid(c) => MassProperties::from_cuboid(self.density, c.half_extents), + Shape::Capsule(caps) => { + MassProperties::from_capsule(self.density, caps.a, caps.b, caps.radius) + } + Shape::Triangle(_) => MassProperties::zero(), + Shape::Trimesh(_) => MassProperties::zero(), + Shape::HeightField(_) => MassProperties::zero(), + } + } +} + +/// A structure responsible for building a new collider. +#[derive(Clone)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct ColliderBuilder { + /// The shape of the collider to be built. + pub shape: Shape, + /// The density of the collider to be built. + pub density: f32, + /// The friction coefficient of the collider to be built. + pub friction: f32, + /// The restitution coefficient of the collider to be built. + pub restitution: f32, + /// The position of this collider relative to the local frame of the rigid-body it is attached to. + pub delta: Isometry, + /// Is this collider a sensor? + pub is_sensor: bool, +} + +impl ColliderBuilder { + /// Initialize a new collider builder with the given shape. + pub fn new(shape: Shape) -> Self { + Self { + shape, + density: 1.0, + friction: Self::default_friction(), + restitution: 0.0, + delta: Isometry::identity(), + is_sensor: false, + } + } + + /// Initialize a new collider builder with a ball shape defined by its radius. + pub fn ball(radius: f32) -> Self { + Self::new(Shape::Ball(Ball::new(radius))) + } + + /// Initialize a new collider builder with a cuboid shape defined by its half-extents. + #[cfg(feature = "dim2")] + pub fn cuboid(hx: f32, hy: f32) -> Self { + let cuboid = Cuboid { + half_extents: Vector::new(hx, hy), + }; + + Self::new(Shape::Cuboid(cuboid)) + + /* + use crate::math::Point; + let vertices = vec![ + Point::new(hx, -hy), + Point::new(hx, hy), + Point::new(-hx, hy), + Point::new(-hx, -hy), + ]; + let normals = vec![Vector::x(), Vector::y(), -Vector::x(), -Vector::y()]; + let polygon = Polygon::new(vertices, normals); + + Self::new(Shape::Polygon(polygon)) + */ + } + + /// Initialize a new collider builder with a capsule shape aligned with the `x` axis. + pub fn capsule_x(half_height: f32, radius: f32) -> Self { + let capsule = Capsule::new_x(half_height, radius); + Self::new(Shape::Capsule(capsule)) + } + + /// Initialize a new collider builder with a capsule shape aligned with the `y` axis. + pub fn capsule_y(half_height: f32, radius: f32) -> Self { + let capsule = Capsule::new_y(half_height, radius); + Self::new(Shape::Capsule(capsule)) + } + + /// Initialize a new collider builder with a capsule shape aligned with the `z` axis. + #[cfg(feature = "dim3")] + pub fn capsule_z(half_height: f32, radius: f32) -> Self { + let capsule = Capsule::new_z(half_height, radius); + Self::new(Shape::Capsule(capsule)) + } + + /// Initialize a new collider builder with a cuboid shape defined by its half-extents. + #[cfg(feature = "dim3")] + pub fn cuboid(hx: f32, hy: f32, hz: f32) -> Self { + let cuboid = Cuboid { + half_extents: Vector::new(hx, hy, hz), + }; + + Self::new(Shape::Cuboid(cuboid)) + } + + /// Initializes a collider builder with a segment shape. + /// + /// A segment shape is modeled by a capsule with a 0 radius. + pub fn segment(a: Point, b: Point) -> Self { + let capsule = Capsule::new(a, b, 0.0); + Self::new(Shape::Capsule(capsule)) + } + + /// Initializes a collider builder with a triangle shape. + pub fn triangle(a: Point, b: Point, c: Point) -> Self { + let triangle = Triangle::new(a, b, c); + Self::new(Shape::Triangle(triangle)) + } + + /// Initializes a collider builder with a triangle mesh shape defined by its vertex and index buffers. + pub fn trimesh(vertices: Vec>, indices: Vec>) -> Self { + let trimesh = Trimesh::new(vertices, indices); + Self::new(Shape::Trimesh(trimesh)) + } + + /// Initializes a collider builder with a heightfield shape defined by its set of height and a scale + /// factor along each coordinate axis. + #[cfg(feature = "dim2")] + pub fn heightfield(heights: na::DVector, scale: Vector) -> Self { + let heightfield = HeightField::new(heights, scale); + Self::new(Shape::HeightField(heightfield)) + } + + /// Initializes a collider builder with a heightfield shape defined by its set of height and a scale + /// factor along each coordinate axis. + #[cfg(feature = "dim3")] + pub fn heightfield(heights: na::DMatrix, scale: Vector) -> Self { + let heightfield = HeightField::new(heights, scale); + Self::new(Shape::HeightField(heightfield)) + } + + /// The default friction coefficient used by the collider builder. + pub fn default_friction() -> f32 { + 0.5 + } + + /// Sets whether or not the collider built by this builder is a sensor. + pub fn sensor(mut self, is_sensor: bool) -> Self { + self.is_sensor = is_sensor; + self + } + + /// Sets the friction coefficient of the collider this builder will build. + pub fn friction(mut self, friction: f32) -> Self { + self.friction = friction; + self + } + + /// Sets the density of the collider this builder will build. + pub fn density(mut self, density: f32) -> Self { + self.density = density; + self + } + + /// Set the position of this collider in the local-space of the rigid-body it is attached to. + pub fn delta(mut self, delta: Isometry) -> Self { + self.delta = delta; + self + } + + /// Buildes a new collider attached to the given rigid-body. + pub fn build(&self) -> Collider { + Collider { + shape: self.shape.clone(), + density: self.density, + friction: self.friction, + restitution: self.restitution, + delta: self.delta, + is_sensor: self.is_sensor, + parent: RigidBodySet::invalid_handle(), + position: Isometry::identity(), + predicted_position: Isometry::identity(), + contact_graph_index: InteractionGraph::::invalid_graph_index(), + proximity_graph_index: InteractionGraph::::invalid_graph_index(), + proxy_index: crate::INVALID_USIZE, + } + } +} diff --git a/src/geometry/collider_set.rs b/src/geometry/collider_set.rs new file mode 100644 index 000000000..73d4a066a --- /dev/null +++ b/src/geometry/collider_set.rs @@ -0,0 +1,139 @@ +use crate::data::arena::Arena; +use crate::dynamics::{RigidBodyHandle, RigidBodySet}; +use crate::geometry::Collider; +use std::ops::{Index, IndexMut}; + +/// The unique identifier of a collider added to a collider set. +pub type ColliderHandle = crate::data::arena::Index; + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// A set of colliders that can be handled by a physics `World`. +pub struct ColliderSet { + pub(crate) colliders: Arena, +} + +impl ColliderSet { + /// Create a new empty set of colliders. + pub fn new() -> Self { + ColliderSet { + colliders: Arena::new(), + } + } + + /// An always-invalid collider handle. + pub fn invalid_handle() -> ColliderHandle { + ColliderHandle::from_raw_parts(crate::INVALID_USIZE, crate::INVALID_U64) + } + + /// Iterate through all the colliders on this set. + pub fn iter(&self) -> impl Iterator { + self.colliders.iter() + } + + /// The number of colliders on this set. + pub fn len(&self) -> usize { + self.colliders.len() + } + + /// Is this collider handle valid? + pub fn contains(&self, handle: ColliderHandle) -> bool { + self.colliders.contains(handle) + } + + /// Inserts a new collider to this set and retrieve its handle. + pub fn insert( + &mut self, + mut coll: Collider, + parent_handle: RigidBodyHandle, + bodies: &mut RigidBodySet, + ) -> ColliderHandle { + let mass_properties = coll.mass_properties(); + coll.parent = parent_handle; + let parent = bodies + .get_mut_internal(parent_handle) + .expect("Parent rigid body not found."); + coll.position = parent.position * coll.delta; + coll.predicted_position = parent.predicted_position * coll.delta; + let handle = self.colliders.insert(coll); + parent.colliders.push(handle); + parent.mass_properties += mass_properties; + parent.update_world_mass_properties(); + bodies.activate(parent_handle); + handle + } + + pub(crate) fn remove_internal(&mut self, handle: ColliderHandle) -> Option { + self.colliders.remove(handle) + } + + /// Gets the collider with the given handle without a known generation. + /// + /// This is useful when you know you want the collider at position `i` but + /// don't know what is its current generation number. Generation numbers are + /// used to protect from the ABA problem because the collider position `i` + /// are recycled between two insertion and a removal. + /// + /// Using this is discouraged in favor of `self.get(handle)` which does not + /// suffer form the ABA problem. + pub fn get_unknown_gen(&self, i: usize) -> Option<(&Collider, ColliderHandle)> { + self.colliders.get_unknown_gen(i) + } + + /// Gets a mutable reference to the collider with the given handle without a known generation. + /// + /// This is useful when you know you want the collider at position `i` but + /// don't know what is its current generation number. Generation numbers are + /// used to protect from the ABA problem because the collider position `i` + /// are recycled between two insertion and a removal. + /// + /// Using this is discouraged in favor of `self.get_mut(handle)` which does not + /// suffer form the ABA problem. + pub fn get_unknown_gen_mut(&mut self, i: usize) -> Option<(&mut Collider, ColliderHandle)> { + self.colliders.get_unknown_gen_mut(i) + } + + /// Get the collider with the given handle. + pub fn get(&self, handle: ColliderHandle) -> Option<&Collider> { + self.colliders.get(handle) + } + + /// Gets a mutable reference to the collider with the given handle. + pub fn get_mut(&mut self, handle: ColliderHandle) -> Option<&mut Collider> { + self.colliders.get_mut(handle) + } + + pub(crate) fn get2_mut_internal( + &mut self, + h1: ColliderHandle, + h2: ColliderHandle, + ) -> (Option<&mut Collider>, Option<&mut Collider>) { + self.colliders.get2_mut(h1, h2) + } + + // pub fn iter_mut(&mut self) -> impl Iterator { + // // let sender = &self.activation_channel_sender; + // self.colliders.iter_mut().map(move |(h, rb)| { + // (h, ColliderMut::new(h, rb /*sender.clone()*/)) + // }) + // } + + // pub(crate) fn iter_mut_internal( + // &mut self, + // ) -> impl Iterator { + // self.colliders.iter_mut() + // } +} + +impl Index for ColliderSet { + type Output = Collider; + + fn index(&self, index: ColliderHandle) -> &Collider { + &self.colliders[index] + } +} + +impl IndexMut for ColliderSet { + fn index_mut(&mut self, index: ColliderHandle) -> &mut Collider { + &mut self.colliders[index] + } +} diff --git a/src/geometry/contact.rs b/src/geometry/contact.rs new file mode 100644 index 000000000..0beec0a17 --- /dev/null +++ b/src/geometry/contact.rs @@ -0,0 +1,485 @@ +use crate::dynamics::BodyPair; +use crate::geometry::contact_generator::ContactPhase; +use crate::geometry::{Collider, ColliderPair, ColliderSet}; +use crate::math::{Isometry, Point, Vector}; +use std::any::Any; +#[cfg(feature = "simd-is-enabled")] +use { + crate::math::{SimdFloat, SIMD_WIDTH}, + simba::simd::SimdValue, +}; + +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// The type local linear approximation of the neighborhood of a pair contact points on two shapes +pub enum KinematicsCategory { + /// Both neighborhoods are assimilated to a single point. + PointPoint, + /// The first shape's neighborhood at the contact point is assimilated to a plane while + /// the second is assimilated to a point. + PlanePoint, +} + +#[derive(Copy, Clone, Debug, PartialEq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// Local contact geometry at the neighborhood of a pair of contact points. +pub struct ContactKinematics { + /// The local contact geometry. + pub category: KinematicsCategory, + /// The dilation applied to the first contact geometry. + pub radius1: f32, + /// The dilation applied to the second contact geometry. + pub radius2: f32, +} + +impl Default for ContactKinematics { + fn default() -> Self { + ContactKinematics { + category: KinematicsCategory::PointPoint, + radius1: 0.0, + radius2: 0.0, + } + } +} + +#[cfg(feature = "simd-is-enabled")] +pub(crate) struct WContact { + pub local_p1: Point, + pub local_p2: Point, + pub local_n1: Vector, + pub local_n2: Vector, + pub dist: SimdFloat, + pub fid1: [u8; SIMD_WIDTH], + pub fid2: [u8; SIMD_WIDTH], +} + +#[cfg(feature = "simd-is-enabled")] +impl WContact { + pub fn extract(&self, i: usize) -> (Contact, Vector, Vector) { + let c = Contact { + local_p1: self.local_p1.extract(i), + local_p2: self.local_p2.extract(i), + dist: self.dist.extract(i), + impulse: 0.0, + tangent_impulse: Contact::zero_tangent_impulse(), + fid1: self.fid1[i], + fid2: self.fid2[i], + }; + + (c, self.local_n1.extract(i), self.local_n2.extract(i)) + } +} + +#[derive(Copy, Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// A single contact between two collider. +pub struct Contact { + /// The contact point in the local-space of the first collider. + pub local_p1: Point, + /// The contact point in the local-space of the second collider. + pub local_p2: Point, + /// The impulse, along the contact normal, applied by this contact to the first collider's rigid-body. + /// + /// The impulse applied to the second collider's rigid-body is given by `-impulse`. + pub impulse: f32, + /// The friction impulse along the vector orthonormal to the contact normal, applied to the first + /// collider's rigid-body. + #[cfg(feature = "dim2")] + pub tangent_impulse: f32, + /// The friction impulses along the basis orthonormal to the contact normal, applied to the first + /// collider's rigid-body. + #[cfg(feature = "dim3")] + pub tangent_impulse: [f32; 2], + /// The identifier of the subshape of the first collider involved in this contact. + /// + /// For primitive shapes like cuboid, ball, etc., this is 0. + /// For shapes like trimesh and heightfield this identifies the specific triangle + /// involved in the contact. + pub fid1: u8, + /// The identifier of the subshape of the second collider involved in this contact. + /// + /// For primitive shapes like cuboid, ball, etc., this is 0. + /// For shapes like trimesh and heightfield this identifies the specific triangle + /// involved in the contact. + pub fid2: u8, + /// The distance between the two colliders along the contact normal. + /// + /// If this is negative, the colliders are penetrating. + pub dist: f32, +} + +impl Contact { + pub(crate) fn new( + local_p1: Point, + local_p2: Point, + fid1: u8, + fid2: u8, + dist: f32, + ) -> Self { + Self { + local_p1, + local_p2, + impulse: 0.0, + #[cfg(feature = "dim2")] + tangent_impulse: 0.0, + #[cfg(feature = "dim3")] + tangent_impulse: [0.0; 2], + fid1, + fid2, + dist, + } + } + + #[cfg(feature = "dim2")] + pub(crate) fn zero_tangent_impulse() -> f32 { + 0.0 + } + + #[cfg(feature = "dim3")] + pub(crate) fn zero_tangent_impulse() -> [f32; 2] { + [0.0, 0.0] + } + + pub(crate) fn copy_geometry_from(&mut self, contact: Contact) { + self.local_p1 = contact.local_p1; + self.local_p2 = contact.local_p2; + self.fid1 = contact.fid1; + self.fid2 = contact.fid2; + self.dist = contact.dist; + } + + // pub(crate) fn swap(self) -> Self { + // Self { + // local_p1: self.local_p2, + // local_p2: self.local_p1, + // impulse: self.impulse, + // tangent_impulse: self.tangent_impulse, + // fid1: self.fid2, + // fid2: self.fid1, + // dist: self.dist, + // } + // } +} + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// The description of all the contacts between a pair of colliders. +pub struct ContactPair { + /// The pair of colliders involved. + pub pair: ColliderPair, + /// The set of contact manifolds between the two colliders. + /// + /// All contact manifold contain themselves contact points between the colliders. + pub manifolds: Vec, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) generator: Option, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) generator_workspace: Option>, +} + +impl ContactPair { + pub(crate) fn new( + pair: ColliderPair, + generator: ContactPhase, + generator_workspace: Option>, + ) -> Self { + Self { + pair, + manifolds: Vec::new(), + generator: Some(generator), + generator_workspace, + } + } + + /// Does this contact pair have any active contact? + /// + /// An active contact is a contact that may result in a non-zero contact force. + pub fn has_any_active_contact(&self) -> bool { + for manifold in &self.manifolds { + if manifold.num_active_contacts != 0 { + return true; + } + } + + false + } + + pub(crate) fn single_manifold<'a, 'b>( + &'a mut self, + colliders: &'b ColliderSet, + ) -> ( + &'b Collider, + &'b Collider, + &'a mut ContactManifold, + Option<&'a mut (dyn Any + Send + Sync)>, + ) { + let coll1 = &colliders[self.pair.collider1]; + let coll2 = &colliders[self.pair.collider2]; + + if self.manifolds.len() == 0 { + let manifold = ContactManifold::from_colliders(self.pair, coll1, coll2); + self.manifolds.push(manifold); + } + + // We have to make sure the order of the returned collider + // match the order of the pair stored inside of the manifold. + // (This order can be modified by the contact determination algorithm). + let manifold = &mut self.manifolds[0]; + if manifold.pair.collider1 == self.pair.collider1 { + ( + coll1, + coll2, + manifold, + self.generator_workspace.as_mut().map(|w| &mut **w), + ) + } else { + ( + coll2, + coll1, + manifold, + self.generator_workspace.as_mut().map(|w| &mut **w), + ) + } + } +} + +#[derive(Clone, Debug)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// A contact manifold between two colliders. +/// +/// A contact manifold describes a set of contacts between two colliders. All the contact +/// part of the same contact manifold share the same contact normal and contact kinematics. +pub struct ContactManifold { + // NOTE: use a SmallVec instead? + // And for 2D use an ArrayVec since there will never be more than 2 contacts anyways. + #[cfg(feature = "dim2")] + pub(super) points: arrayvec::ArrayVec<[Contact; 2]>, + #[cfg(feature = "dim3")] + pub(super) points: Vec, + /// The number of active contacts on this contact manifold. + /// + /// Active contacts are these that may result in contact forces. + pub num_active_contacts: usize, + /// The contact normal of all the contacts of this manifold, expressed in the local space of the first collider. + pub local_n1: Vector, + /// The contact normal of all the contacts of this manifold, expressed in the local space of the second collider. + pub local_n2: Vector, + /// The contact kinematics of all the contacts of this manifold. + pub kinematics: ContactKinematics, + // The following are set by the narrow-phase. + /// The pair of body involved in this contact manifold. + pub body_pair: BodyPair, + /// The pair of colliders involved in this contact manifold. + pub pair: ColliderPair, + /// The pair of subshapes involved in this contact manifold. + pub subshape_index_pair: (usize, usize), + pub(crate) warmstart_multiplier: f32, + // We put the friction and restitution here because + // this avoids reading the colliders inside of the + // contact preparation method. + /// The friction coefficient for of all the contacts on this contact manifold. + pub friction: f32, + /// The restitution coefficient for all the contacts on this contact manifold. + pub restitution: f32, + // The following are set by the constraints solver. + pub(crate) constraint_index: usize, + pub(crate) position_constraint_index: usize, +} + +impl ContactManifold { + pub(crate) fn new( + pair: ColliderPair, + subshapes: (usize, usize), + body_pair: BodyPair, + friction: f32, + restitution: f32, + ) -> ContactManifold { + Self { + #[cfg(feature = "dim2")] + points: arrayvec::ArrayVec::new(), + #[cfg(feature = "dim3")] + points: Vec::new(), + num_active_contacts: 0, + local_n1: Vector::zeros(), + local_n2: Vector::zeros(), + pair, + subshape_index_pair: subshapes, + body_pair, + kinematics: ContactKinematics::default(), + warmstart_multiplier: Self::min_warmstart_multiplier(), + friction, + restitution, + constraint_index: 0, + position_constraint_index: 0, + } + } + + pub(crate) fn take(&mut self) -> Self { + ContactManifold { + #[cfg(feature = "dim2")] + points: self.points.clone(), + #[cfg(feature = "dim3")] + points: std::mem::replace(&mut self.points, Vec::new()), + num_active_contacts: self.num_active_contacts, + local_n1: self.local_n1, + local_n2: self.local_n2, + kinematics: self.kinematics, + body_pair: self.body_pair, + pair: self.pair, + subshape_index_pair: self.subshape_index_pair, + warmstart_multiplier: self.warmstart_multiplier, + friction: self.friction, + restitution: self.restitution, + constraint_index: self.constraint_index, + position_constraint_index: self.position_constraint_index, + } + } + + pub(crate) fn from_colliders(pair: ColliderPair, coll1: &Collider, coll2: &Collider) -> Self { + Self::with_subshape_indices(pair, coll1, coll2, 0, 0) + } + + pub(crate) fn with_subshape_indices( + pair: ColliderPair, + coll1: &Collider, + coll2: &Collider, + subshape1: usize, + subshape2: usize, + ) -> Self { + Self::new( + pair, + (subshape1, subshape2), + BodyPair::new(coll1.parent, coll2.parent), + (coll1.friction + coll2.friction) * 0.5, + (coll1.restitution + coll2.restitution) * 0.5, + ) + } + + pub(crate) fn min_warmstart_multiplier() -> f32 { + // Multiplier used to reduce the amount of warm-starting. + // This coefficient increases exponentially over time, until it reaches 1.0. + // This will reduce significant overshoot at the timesteps that + // follow a timestep involving high-velocity impacts. + 0.01 + } + + /// Number of active contacts on this contact manifold. + #[inline] + pub fn num_active_contacts(&self) -> usize { + self.num_active_contacts + } + + /// The slice of all the active contacts on this contact manifold. + /// + /// Active contacts are contacts that may end up generating contact forces. + #[inline] + pub fn active_contacts(&self) -> &[Contact] { + &self.points[..self.num_active_contacts] + } + + #[inline] + pub(crate) fn active_contacts_mut(&mut self) -> &mut [Contact] { + &mut self.points[..self.num_active_contacts] + } + + /// The slice of all the contacts, active or not, on this contact manifold. + #[inline] + pub fn all_contacts(&self) -> &[Contact] { + &self.points + } + + pub(crate) fn swap_identifiers(&mut self) { + self.pair = self.pair.swap(); + self.body_pair = self.body_pair.swap(); + self.subshape_index_pair = (self.subshape_index_pair.1, self.subshape_index_pair.0); + } + + pub(crate) fn update_warmstart_multiplier(&mut self) { + // In 2D, tall stacks will actually suffer from this + // because oscillation due to inaccuracies in 2D often + // cause contacts to break, which would result in + // a reset of the warmstart multiplier. + if cfg!(feature = "dim2") { + self.warmstart_multiplier = 1.0; + return; + } + + for pt in &self.points { + if pt.impulse != 0.0 { + self.warmstart_multiplier = (self.warmstart_multiplier * 2.0).min(1.0); + return; + } + } + + // Reset the multiplier. + self.warmstart_multiplier = Self::min_warmstart_multiplier() + } + + #[inline] + pub(crate) fn try_update_contacts(&mut self, pos12: &Isometry) -> bool { + if self.points.len() == 0 { + return false; + } + + // const DOT_THRESHOLD: f32 = 0.crate::COS_10_DEGREES; + const DOT_THRESHOLD: f32 = crate::utils::COS_5_DEGREES; + + let local_n2 = pos12 * self.local_n2; + + if -self.local_n1.dot(&local_n2) < DOT_THRESHOLD { + return false; + } + + for pt in &mut self.points { + let local_p2 = pos12 * pt.local_p2; + let dpt = local_p2 - pt.local_p1; + let dist = dpt.dot(&self.local_n1); + + if dist * pt.dist < 0.0 { + // We switched between penetrating/non-penetrating. + // The may result in other contacts to appear. + return false; + } + let new_local_p1 = local_p2 - self.local_n1 * dist; + + let dist_threshold = 0.001; // FIXME: this should not be hard-coded. + if na::distance_squared(&pt.local_p1, &new_local_p1) > dist_threshold { + return false; + } + + pt.dist = dist; + pt.local_p1 = new_local_p1; + } + + true + } + + /// Sort the contacts of this contact manifold such that the active contacts are in the first + /// positions of the array. + #[inline] + pub(crate) fn sort_contacts(&mut self, prediction_distance: f32) { + let num_contacts = self.points.len(); + match num_contacts { + 0 => { + self.num_active_contacts = 0; + } + 1 => { + self.num_active_contacts = (self.points[0].dist < prediction_distance) as usize; + } + _ => { + let mut first_inactive_index = num_contacts; + + self.num_active_contacts = 0; + while self.num_active_contacts != first_inactive_index { + if self.points[self.num_active_contacts].dist >= prediction_distance { + // Swap with the last contact. + self.points + .swap(self.num_active_contacts, first_inactive_index - 1); + first_inactive_index -= 1; + } else { + self.num_active_contacts += 1; + } + } + } + } + } +} diff --git a/src/geometry/contact_generator/ball_ball_contact_generator.rs b/src/geometry/contact_generator/ball_ball_contact_generator.rs new file mode 100644 index 000000000..4889aafbf --- /dev/null +++ b/src/geometry/contact_generator/ball_ball_contact_generator.rs @@ -0,0 +1,98 @@ +use crate::geometry::contact_generator::PrimitiveContactGenerationContext; +use crate::geometry::{Contact, KinematicsCategory}; +use crate::math::Point; +#[cfg(feature = "simd-is-enabled")] +use { + crate::geometry::contact_generator::PrimitiveContactGenerationContextSimd, + crate::geometry::{WBall, WContact}, + crate::math::{Isometry, SimdFloat, SIMD_WIDTH}, + simba::simd::SimdValue, +}; + +#[cfg(feature = "simd-is-enabled")] +fn generate_contacts_simd(ball1: &WBall, ball2: &WBall, pos21: &Isometry) -> WContact { + let dcenter = ball2.center - ball1.center; + let center_dist = dcenter.magnitude(); + let normal = dcenter / center_dist; + + WContact { + local_p1: ball1.center + normal * ball1.radius, + local_p2: pos21 * (ball2.center - normal * ball2.radius), + local_n1: normal, + local_n2: pos21 * -normal, + fid1: [0; SIMD_WIDTH], + fid2: [0; SIMD_WIDTH], + dist: center_dist - ball1.radius - ball2.radius, + } +} + +#[cfg(feature = "simd-is-enabled")] +pub fn generate_contacts_ball_ball_simd(ctxt: &mut PrimitiveContactGenerationContextSimd) { + let pos_ba = ctxt.positions2.inverse() * ctxt.positions1; + let radii_a = + SimdFloat::from(array![|ii| ctxt.shapes1[ii].as_ball().unwrap().radius; SIMD_WIDTH]); + let radii_b = + SimdFloat::from(array![|ii| ctxt.shapes2[ii].as_ball().unwrap().radius; SIMD_WIDTH]); + + let wball_a = WBall::new(Point::origin(), radii_a); + let wball_b = WBall::new(pos_ba.inverse_transform_point(&Point::origin()), radii_b); + let contacts = generate_contacts_simd(&wball_a, &wball_b, &pos_ba); + + for (i, manifold) in ctxt.manifolds.iter_mut().enumerate() { + // FIXME: compare the dist before extracting the contact. + let (contact, local_n1, local_n2) = contacts.extract(i); + if contact.dist <= ctxt.prediction_distance { + if manifold.points.len() != 0 { + manifold.points[0].copy_geometry_from(contact); + } else { + manifold.points.push(contact); + } + + manifold.local_n1 = local_n1; + manifold.local_n2 = local_n2; + manifold.kinematics.category = KinematicsCategory::PointPoint; + manifold.kinematics.radius1 = radii_a.extract(i); + manifold.kinematics.radius2 = radii_b.extract(i); + manifold.update_warmstart_multiplier(); + } else { + manifold.points.clear(); + } + + manifold.sort_contacts(ctxt.prediction_distance); + } +} + +pub fn generate_contacts_ball_ball(ctxt: &mut PrimitiveContactGenerationContext) { + let pos_ba = ctxt.position2.inverse() * ctxt.position1; + let radius_a = ctxt.shape1.as_ball().unwrap().radius; + let radius_b = ctxt.shape2.as_ball().unwrap().radius; + + let dcenter = pos_ba.inverse_transform_point(&Point::origin()).coords; + let center_dist = dcenter.magnitude(); + let dist = center_dist - radius_a - radius_b; + + if dist < ctxt.prediction_distance { + let local_n1 = dcenter / center_dist; + let local_n2 = pos_ba.inverse_transform_vector(&-local_n1); + let local_p1 = local_n1 * radius_a; + let local_p2 = local_n2 * radius_b; + let contact = Contact::new(local_p1.into(), local_p2.into(), 0, 0, dist); + + if ctxt.manifold.points.len() != 0 { + ctxt.manifold.points[0].copy_geometry_from(contact); + } else { + ctxt.manifold.points.push(contact); + } + + ctxt.manifold.local_n1 = local_n1; + ctxt.manifold.local_n2 = local_n2; + ctxt.manifold.kinematics.category = KinematicsCategory::PointPoint; + ctxt.manifold.kinematics.radius1 = radius_a; + ctxt.manifold.kinematics.radius2 = radius_b; + ctxt.manifold.update_warmstart_multiplier(); + } else { + ctxt.manifold.points.clear(); + } + + ctxt.manifold.sort_contacts(ctxt.prediction_distance); +} diff --git a/src/geometry/contact_generator/ball_convex_contact_generator.rs b/src/geometry/contact_generator/ball_convex_contact_generator.rs new file mode 100644 index 000000000..a1878324a --- /dev/null +++ b/src/geometry/contact_generator/ball_convex_contact_generator.rs @@ -0,0 +1,85 @@ +use crate::geometry::contact_generator::PrimitiveContactGenerationContext; +use crate::geometry::{Ball, Contact, KinematicsCategory, Shape}; +use crate::math::Isometry; +use na::Unit; +use ncollide::query::PointQuery; + +pub fn generate_contacts_ball_convex(ctxt: &mut PrimitiveContactGenerationContext) { + if let Shape::Ball(ball1) = ctxt.shape1 { + ctxt.manifold.swap_identifiers(); + + match ctxt.shape2 { + Shape::Triangle(tri2) => do_generate_contacts(tri2, ball1, ctxt, true), + Shape::Cuboid(cube2) => do_generate_contacts(cube2, ball1, ctxt, true), + Shape::Capsule(capsule2) => do_generate_contacts(capsule2, ball1, ctxt, true), + _ => unimplemented!(), + } + } else if let Shape::Ball(ball2) = ctxt.shape2 { + match ctxt.shape1 { + Shape::Triangle(tri1) => do_generate_contacts(tri1, ball2, ctxt, false), + Shape::Cuboid(cube1) => do_generate_contacts(cube1, ball2, ctxt, false), + Shape::Capsule(capsule1) => do_generate_contacts(capsule1, ball2, ctxt, false), + _ => unimplemented!(), + } + } + + ctxt.manifold.sort_contacts(ctxt.prediction_distance); +} + +fn do_generate_contacts>( + point_query1: &P, + ball2: &Ball, + ctxt: &mut PrimitiveContactGenerationContext, + swapped: bool, +) { + let position1; + let position2; + + if swapped { + position1 = ctxt.position2; + position2 = ctxt.position1; + } else { + position1 = ctxt.position1; + position2 = ctxt.position2; + } + + let local_p2_1 = position1.inverse_transform_point(&position2.translation.vector.into()); + + // TODO: add a `project_local_point` to the PointQuery trait to avoid + // the identity isometry. + let proj = + point_query1.project_point(&Isometry::identity(), &local_p2_1, cfg!(feature = "dim3")); + let dpos = local_p2_1 - proj.point; + + #[allow(unused_mut)] // Because `mut local_n1, mut dist` is needed in 2D but not in 3D. + if let Some((mut local_n1, mut dist)) = Unit::try_new_and_get(dpos, 0.0) { + #[cfg(feature = "dim2")] + if proj.is_inside { + local_n1 = -local_n1; + dist = -dist; + } + + if dist <= ball2.radius + ctxt.prediction_distance { + let local_n2 = position2.inverse_transform_vector(&(position1 * -*local_n1)); + let local_p2 = (local_n2 * ball2.radius).into(); + + let contact_point = Contact::new(proj.point, local_p2, 0, 0, dist - ball2.radius); + if ctxt.manifold.points.len() != 1 { + ctxt.manifold.points.clear(); + ctxt.manifold.points.push(contact_point); + } else { + // Copy only the geometry so we keep the warmstart impulses. + ctxt.manifold.points[0].copy_geometry_from(contact_point); + } + + ctxt.manifold.local_n1 = *local_n1; + ctxt.manifold.local_n2 = local_n2; + ctxt.manifold.kinematics.category = KinematicsCategory::PlanePoint; + ctxt.manifold.kinematics.radius1 = 0.0; + ctxt.manifold.kinematics.radius2 = ball2.radius; + ctxt.manifold.update_warmstart_multiplier(); + } else { + ctxt.manifold.points.clear(); + } + } +} diff --git a/src/geometry/contact_generator/ball_polygon_contact_generator.rs b/src/geometry/contact_generator/ball_polygon_contact_generator.rs new file mode 100644 index 000000000..e69de29bb diff --git a/src/geometry/contact_generator/capsule_capsule_contact_generator.rs b/src/geometry/contact_generator/capsule_capsule_contact_generator.rs new file mode 100644 index 000000000..3800ce6dd --- /dev/null +++ b/src/geometry/contact_generator/capsule_capsule_contact_generator.rs @@ -0,0 +1,200 @@ +use crate::geometry::contact_generator::PrimitiveContactGenerationContext; +use crate::geometry::{Capsule, Contact, ContactManifold, KinematicsCategory, Shape}; +use crate::math::Isometry; +use crate::math::Vector; +use approx::AbsDiffEq; +use na::Unit; +#[cfg(feature = "dim2")] +use ncollide::shape::{Segment, SegmentPointLocation}; + +pub fn generate_contacts_capsule_capsule(ctxt: &mut PrimitiveContactGenerationContext) { + if let (Shape::Capsule(capsule1), Shape::Capsule(capsule2)) = (ctxt.shape1, ctxt.shape2) { + generate_contacts( + ctxt.prediction_distance, + capsule1, + ctxt.position1, + capsule2, + ctxt.position2, + ctxt.manifold, + ); + } + + ctxt.manifold.update_warmstart_multiplier(); + ctxt.manifold.sort_contacts(ctxt.prediction_distance); +} + +#[cfg(feature = "dim2")] +pub fn generate_contacts<'a>( + prediction_distance: f32, + capsule1: &'a Capsule, + pos1: &'a Isometry, + capsule2: &'a Capsule, + pos2: &'a Isometry, + manifold: &mut ContactManifold, +) { + // FIXME: the contact kinematics is not correctly set here. + // We use the common "Point-Plane" kinematics with zero radius everytime. + // Instead we should select point/point ore point-plane (with non-zero + // radius for the point) depending on the features involved in the contact. + let pos12 = pos1.inverse() * pos2; + let pos21 = pos12.inverse(); + + let capsule2_1 = capsule2.transform_by(&pos12); + let (loc1, loc2) = ncollide::query::closest_points_segment_segment_with_locations_nD( + (&capsule1.a, &capsule1.b), + (&capsule2_1.a, &capsule2_1.b), + ); + + // We do this clone to perform contact tracking and transfer impulses. + // FIXME: find a more efficient way of doing this. + let old_manifold_points = manifold.points.clone(); + manifold.points.clear(); + + let swapped = false; + + let fid1 = if let SegmentPointLocation::OnVertex(v1) = loc1 { + v1 as u8 * 2 + } else { + 1 + }; + let fid2 = if let SegmentPointLocation::OnVertex(v2) = loc2 { + v2 as u8 * 2 + } else { + 1 + }; + + let bcoords1 = loc1.barycentric_coordinates(); + let bcoords2 = loc2.barycentric_coordinates(); + let local_p1 = capsule1.a * bcoords1[0] + capsule1.b.coords * bcoords1[1]; + let local_p2 = capsule2_1.a * bcoords2[0] + capsule2_1.b.coords * bcoords2[1]; + + let local_n1 = + Unit::try_new(local_p2 - local_p1, f32::default_epsilon()).unwrap_or(Vector::y_axis()); + let dist = (local_p2 - local_p1).dot(&local_n1) - capsule1.radius - capsule2.radius; + + if dist <= prediction_distance { + let local_n2 = pos21 * -local_n1; + let contact = Contact::new(local_p1, pos21 * local_p2, fid1, fid2, dist); + manifold.points.push(contact); + + manifold.local_n1 = *local_n1; + manifold.local_n2 = *local_n2; + manifold.kinematics.category = KinematicsCategory::PlanePoint; + manifold.kinematics.radius1 = 0.0; + manifold.kinematics.radius2 = 0.0; + } else { + // No contact within tolerance. + return; + } + + let seg1 = Segment::new(capsule1.a, capsule1.b); + let seg2 = Segment::new(capsule2_1.a, capsule2_1.b); + + if let (Some(dir1), Some(dir2)) = (seg1.direction(), seg2.direction()) { + if dir1.dot(&dir2).abs() >= crate::utils::COS_FRAC_PI_8 + && dir1.dot(&local_n1).abs() < crate::utils::SIN_FRAC_PI_8 + { + // Capsules axii are almost parallel and are almost perpendicular to the normal. + // Find a second contact point. + if let Some((clip_a, clip_b)) = crate::geometry::clip_segments_with_normal( + (capsule1.a, capsule1.b), + (capsule2_1.a, capsule2_1.b), + *local_n1, + ) { + let contact = + if (clip_a.0 - local_p1).norm_squared() > f32::default_epsilon() * 100.0 { + // Use clip_a as the second contact. + Contact::new( + clip_a.0, + pos21 * clip_a.1, + clip_a.2 as u8, + clip_a.3 as u8, + (clip_a.1 - clip_a.0).dot(&local_n1), + ) + } else { + // Use clip_b as the second contact. + Contact::new( + clip_b.0, + pos21 * clip_b.1, + clip_b.2 as u8, + clip_b.3 as u8, + (clip_b.1 - clip_b.0).dot(&local_n1), + ) + }; + + manifold.points.push(contact); + } + } + } + + if swapped { + for point in &mut manifold.points { + point.local_p1 += manifold.local_n1 * capsule2.radius; + point.local_p2 += manifold.local_n2 * capsule1.radius; + point.dist -= capsule1.radius + capsule2.radius; + } + } else { + for point in &mut manifold.points { + point.local_p1 += manifold.local_n1 * capsule1.radius; + point.local_p2 += manifold.local_n2 * capsule2.radius; + point.dist -= capsule1.radius + capsule2.radius; + } + } + + super::match_contacts(manifold, &old_manifold_points, swapped); +} + +#[cfg(feature = "dim3")] +pub fn generate_contacts<'a>( + prediction_distance: f32, + capsule1: &'a Capsule, + pos1: &'a Isometry, + capsule2: &'a Capsule, + pos2: &'a Isometry, + manifold: &mut ContactManifold, +) { + let pos12 = pos1.inverse() * pos2; + let pos21 = pos12.inverse(); + + let capsule2_1 = capsule1.transform_by(&pos12); + let (loc1, loc2) = ncollide::query::closest_points_segment_segment_with_locations_nD( + (&capsule1.a, &capsule1.b), + (&capsule2_1.a, &capsule2_1.b), + ); + + { + let bcoords1 = loc1.barycentric_coordinates(); + let bcoords2 = loc2.barycentric_coordinates(); + let local_p1 = capsule1.a * bcoords1[0] + capsule1.b.coords * bcoords1[1]; + let local_p2 = capsule2_1.a * bcoords2[0] + capsule2_1.b.coords * bcoords2[1]; + + let local_n1 = + Unit::try_new(local_p2 - local_p1, f32::default_epsilon()).unwrap_or(Vector::y_axis()); + let dist = (local_p2 - local_p1).dot(&local_n1) - capsule1.radius - capsule2.radius; + + if dist <= prediction_distance { + let local_n2 = pos21 * -local_n1; + let contact = Contact::new( + local_p1 + *local_n1 * capsule1.radius, + pos21 * local_p2 + *local_n2 * capsule2.radius, + 0, + 0, + dist, + ); + + if manifold.points.len() != 0 { + manifold.points[0].copy_geometry_from(contact); + } else { + manifold.points.push(contact); + } + + manifold.local_n1 = *local_n1; + manifold.local_n2 = *local_n2; + manifold.kinematics.category = KinematicsCategory::PlanePoint; + manifold.kinematics.radius1 = 0.0; + manifold.kinematics.radius2 = 0.0; + } else { + manifold.points.clear(); + } + } +} diff --git a/src/geometry/contact_generator/contact_dispatcher.rs b/src/geometry/contact_generator/contact_dispatcher.rs new file mode 100644 index 000000000..8c846e0c1 --- /dev/null +++ b/src/geometry/contact_generator/contact_dispatcher.rs @@ -0,0 +1,127 @@ +use crate::geometry::contact_generator::{ + ContactGenerator, ContactPhase, HeightFieldShapeContactGeneratorWorkspace, + PrimitiveContactGenerator, TrimeshShapeContactGeneratorWorkspace, +}; +use crate::geometry::Shape; +use std::any::Any; + +/// Trait implemented by structures responsible for selecting a collision-detection algorithm +/// for a given pair of shapes. +pub trait ContactDispatcher { + /// Select the collision-detection algorithm for the given pair of primitive shapes. + fn dispatch_primitives( + &self, + shape1: &Shape, + shape2: &Shape, + ) -> ( + PrimitiveContactGenerator, + Option>, + ); + /// Select the collision-detection algorithm for the given pair of non-primitive shapes. + fn dispatch( + &self, + shape1: &Shape, + shape2: &Shape, + ) -> (ContactPhase, Option>); +} + +/// The default contact dispatcher used by Rapier. +pub struct DefaultContactDispatcher; + +impl ContactDispatcher for DefaultContactDispatcher { + fn dispatch_primitives( + &self, + shape1: &Shape, + shape2: &Shape, + ) -> ( + PrimitiveContactGenerator, + Option>, + ) { + match (shape1, shape2) { + (Shape::Ball(_), Shape::Ball(_)) => ( + PrimitiveContactGenerator { + generate_contacts: super::generate_contacts_ball_ball, + #[cfg(feature = "simd-is-enabled")] + generate_contacts_simd: super::generate_contacts_ball_ball_simd, + ..PrimitiveContactGenerator::default() + }, + None, + ), + (Shape::Cuboid(_), Shape::Cuboid(_)) => ( + PrimitiveContactGenerator { + generate_contacts: super::generate_contacts_cuboid_cuboid, + ..PrimitiveContactGenerator::default() + }, + None, + ), + (Shape::Polygon(_), Shape::Polygon(_)) => ( + PrimitiveContactGenerator { + generate_contacts: super::generate_contacts_polygon_polygon, + ..PrimitiveContactGenerator::default() + }, + None, + ), + (Shape::Capsule(_), Shape::Capsule(_)) => ( + PrimitiveContactGenerator { + generate_contacts: super::generate_contacts_capsule_capsule, + ..PrimitiveContactGenerator::default() + }, + None, + ), + (Shape::Cuboid(_), Shape::Ball(_)) + | (Shape::Ball(_), Shape::Cuboid(_)) + | (Shape::Triangle(_), Shape::Ball(_)) + | (Shape::Ball(_), Shape::Triangle(_)) + | (Shape::Capsule(_), Shape::Ball(_)) + | (Shape::Ball(_), Shape::Capsule(_)) => ( + PrimitiveContactGenerator { + generate_contacts: super::generate_contacts_ball_convex, + ..PrimitiveContactGenerator::default() + }, + None, + ), + (Shape::Capsule(_), Shape::Cuboid(_)) | (Shape::Cuboid(_), Shape::Capsule(_)) => ( + PrimitiveContactGenerator { + generate_contacts: super::generate_contacts_cuboid_capsule, + ..PrimitiveContactGenerator::default() + }, + None, + ), + (Shape::Triangle(_), Shape::Cuboid(_)) | (Shape::Cuboid(_), Shape::Triangle(_)) => ( + PrimitiveContactGenerator { + generate_contacts: super::generate_contacts_cuboid_triangle, + ..PrimitiveContactGenerator::default() + }, + None, + ), + _ => (PrimitiveContactGenerator::default(), None), + } + } + + fn dispatch( + &self, + shape1: &Shape, + shape2: &Shape, + ) -> (ContactPhase, Option>) { + match (shape1, shape2) { + (Shape::Trimesh(_), _) | (_, Shape::Trimesh(_)) => ( + ContactPhase::NearPhase(ContactGenerator { + generate_contacts: super::generate_contacts_trimesh_shape, + ..ContactGenerator::default() + }), + Some(Box::new(TrimeshShapeContactGeneratorWorkspace::new())), + ), + (Shape::HeightField(_), _) | (_, Shape::HeightField(_)) => ( + ContactPhase::NearPhase(ContactGenerator { + generate_contacts: super::generate_contacts_heightfield_shape, + ..ContactGenerator::default() + }), + Some(Box::new(HeightFieldShapeContactGeneratorWorkspace::new())), + ), + _ => { + let (gen, workspace) = self.dispatch_primitives(shape1, shape2); + (ContactPhase::ExactPhase(gen), workspace) + } + } + } +} diff --git a/src/geometry/contact_generator/contact_generator.rs b/src/geometry/contact_generator/contact_generator.rs new file mode 100644 index 000000000..9dd0050f5 --- /dev/null +++ b/src/geometry/contact_generator/contact_generator.rs @@ -0,0 +1,222 @@ +use crate::geometry::{ + Collider, ColliderSet, ContactDispatcher, ContactEvent, ContactManifold, ContactPair, Shape, +}; +use crate::math::Isometry; +#[cfg(feature = "simd-is-enabled")] +use crate::math::{SimdFloat, SIMD_WIDTH}; +use crate::pipeline::EventHandler; +use std::any::Any; + +#[derive(Copy, Clone)] +pub enum ContactPhase { + NearPhase(ContactGenerator), + ExactPhase(PrimitiveContactGenerator), +} + +impl ContactPhase { + #[inline] + pub fn generate_contacts( + self, + mut context: ContactGenerationContext, + events: &dyn EventHandler, + ) { + let had_contacts_before = context.pair.has_any_active_contact(); + + match self { + Self::NearPhase(gen) => (gen.generate_contacts)(&mut context), + Self::ExactPhase(gen) => { + // Build the primitive context from the non-primitive context and dispatch. + let (collider1, collider2, manifold, workspace) = + context.pair.single_manifold(context.colliders); + let mut context2 = PrimitiveContactGenerationContext { + prediction_distance: context.prediction_distance, + collider1, + collider2, + shape1: collider1.shape(), + shape2: collider2.shape(), + position1: collider1.position(), + position2: collider2.position(), + manifold, + workspace, + }; + + (gen.generate_contacts)(&mut context2) + } + } + + if had_contacts_before != context.pair.has_any_active_contact() { + if had_contacts_before { + events.handle_contact_event(ContactEvent::Stopped( + context.pair.pair.collider1, + context.pair.pair.collider2, + )); + } else { + events.handle_contact_event(ContactEvent::Started( + context.pair.pair.collider1, + context.pair.pair.collider2, + )) + } + } + } + + #[cfg(feature = "simd-is-enabled")] + #[inline] + pub fn generate_contacts_simd( + self, + mut context: ContactGenerationContextSimd, + events: &dyn EventHandler, + ) { + let mut had_contacts_before = [false; SIMD_WIDTH]; + + for (i, pair) in context.pairs.iter().enumerate() { + had_contacts_before[i] = pair.has_any_active_contact() + } + + match self { + Self::NearPhase(gen) => (gen.generate_contacts_simd)(&mut context), + Self::ExactPhase(gen) => { + // Build the primitive context from the non-primitive context and dispatch. + use arrayvec::ArrayVec; + let mut colliders_arr: ArrayVec<[(&Collider, &Collider); SIMD_WIDTH]> = + ArrayVec::new(); + let mut manifold_arr: ArrayVec<[&mut ContactManifold; SIMD_WIDTH]> = + ArrayVec::new(); + let mut workspace_arr: ArrayVec< + [Option<&mut (dyn Any + Send + Sync)>; SIMD_WIDTH], + > = ArrayVec::new(); + + for pair in context.pairs.iter_mut() { + let (collider1, collider2, manifold, workspace) = + pair.single_manifold(context.colliders); + colliders_arr.push((collider1, collider2)); + manifold_arr.push(manifold); + workspace_arr.push(workspace); + } + + let max_index = colliders_arr.len() - 1; + let colliders1 = array![|ii| colliders_arr[ii.min(max_index)].0; SIMD_WIDTH]; + let colliders2 = array![|ii| colliders_arr[ii.min(max_index)].1; SIMD_WIDTH]; + + let mut context2 = PrimitiveContactGenerationContextSimd { + prediction_distance: context.prediction_distance, + colliders1, + colliders2, + shapes1: array![|ii| colliders1[ii].shape(); SIMD_WIDTH], + shapes2: array![|ii| colliders2[ii].shape(); SIMD_WIDTH], + positions1: &Isometry::from( + array![|ii| *colliders1[ii].position(); SIMD_WIDTH], + ), + positions2: &Isometry::from( + array![|ii| *colliders2[ii].position(); SIMD_WIDTH], + ), + manifolds: manifold_arr.as_mut_slice(), + workspaces: workspace_arr.as_mut_slice(), + }; + + (gen.generate_contacts_simd)(&mut context2) + } + } + + for (i, pair) in context.pairs.iter().enumerate() { + if had_contacts_before[i] != pair.has_any_active_contact() { + if had_contacts_before[i] { + events.handle_contact_event(ContactEvent::Stopped( + pair.pair.collider1, + pair.pair.collider2, + )) + } else { + events.handle_contact_event(ContactEvent::Started( + pair.pair.collider1, + pair.pair.collider2, + )) + } + } + } + } +} + +pub struct PrimitiveContactGenerationContext<'a> { + pub prediction_distance: f32, + pub collider1: &'a Collider, + pub collider2: &'a Collider, + pub shape1: &'a Shape, + pub shape2: &'a Shape, + pub position1: &'a Isometry, + pub position2: &'a Isometry, + pub manifold: &'a mut ContactManifold, + pub workspace: Option<&'a mut (dyn Any + Send + Sync)>, +} + +#[cfg(feature = "simd-is-enabled")] +pub struct PrimitiveContactGenerationContextSimd<'a, 'b> { + pub prediction_distance: f32, + pub colliders1: [&'a Collider; SIMD_WIDTH], + pub colliders2: [&'a Collider; SIMD_WIDTH], + pub shapes1: [&'a Shape; SIMD_WIDTH], + pub shapes2: [&'a Shape; SIMD_WIDTH], + pub positions1: &'a Isometry, + pub positions2: &'a Isometry, + pub manifolds: &'a mut [&'b mut ContactManifold], + pub workspaces: &'a mut [Option<&'b mut (dyn Any + Send + Sync)>], +} + +#[derive(Copy, Clone)] +pub struct PrimitiveContactGenerator { + pub generate_contacts: fn(&mut PrimitiveContactGenerationContext), + #[cfg(feature = "simd-is-enabled")] + pub generate_contacts_simd: fn(&mut PrimitiveContactGenerationContextSimd), +} + +impl PrimitiveContactGenerator { + fn unimplemented_fn(_ctxt: &mut PrimitiveContactGenerationContext) {} + #[cfg(feature = "simd-is-enabled")] + fn unimplemented_simd_fn(_ctxt: &mut PrimitiveContactGenerationContextSimd) {} +} + +impl Default for PrimitiveContactGenerator { + fn default() -> Self { + Self { + generate_contacts: Self::unimplemented_fn, + #[cfg(feature = "simd-is-enabled")] + generate_contacts_simd: Self::unimplemented_simd_fn, + } + } +} + +pub struct ContactGenerationContext<'a> { + pub dispatcher: &'a dyn ContactDispatcher, + pub prediction_distance: f32, + pub colliders: &'a ColliderSet, + pub pair: &'a mut ContactPair, +} + +#[cfg(feature = "simd-is-enabled")] +pub struct ContactGenerationContextSimd<'a, 'b> { + pub dispatcher: &'a dyn ContactDispatcher, + pub prediction_distance: f32, + pub colliders: &'a ColliderSet, + pub pairs: &'a mut [&'b mut ContactPair], +} + +#[derive(Copy, Clone)] +pub struct ContactGenerator { + pub generate_contacts: fn(&mut ContactGenerationContext), + #[cfg(feature = "simd-is-enabled")] + pub generate_contacts_simd: fn(&mut ContactGenerationContextSimd), +} + +impl ContactGenerator { + fn unimplemented_fn(_ctxt: &mut ContactGenerationContext) {} + #[cfg(feature = "simd-is-enabled")] + fn unimplemented_simd_fn(_ctxt: &mut ContactGenerationContextSimd) {} +} + +impl Default for ContactGenerator { + fn default() -> Self { + Self { + generate_contacts: Self::unimplemented_fn, + #[cfg(feature = "simd-is-enabled")] + generate_contacts_simd: Self::unimplemented_simd_fn, + } + } +} diff --git a/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs new file mode 100644 index 000000000..a7857a1da --- /dev/null +++ b/src/geometry/contact_generator/cuboid_capsule_contact_generator.rs @@ -0,0 +1,188 @@ +use crate::geometry::contact_generator::PrimitiveContactGenerationContext; +#[cfg(feature = "dim3")] +use crate::geometry::PolyhedronFace; +use crate::geometry::{cuboid, sat, Capsule, ContactManifold, Cuboid, KinematicsCategory, Shape}; +#[cfg(feature = "dim2")] +use crate::geometry::{CuboidFeature, CuboidFeatureFace}; +use crate::math::Isometry; +use crate::math::Vector; +use ncollide::shape::Segment; + +pub fn generate_contacts_cuboid_capsule(ctxt: &mut PrimitiveContactGenerationContext) { + if let (Shape::Cuboid(cube1), Shape::Capsule(capsule2)) = (ctxt.shape1, ctxt.shape2) { + generate_contacts( + ctxt.prediction_distance, + cube1, + ctxt.position1, + capsule2, + ctxt.position2, + ctxt.manifold, + false, + ); + ctxt.manifold.update_warmstart_multiplier(); + } else if let (Shape::Capsule(capsule1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) { + generate_contacts( + ctxt.prediction_distance, + cube2, + ctxt.position2, + capsule1, + ctxt.position1, + ctxt.manifold, + true, + ); + ctxt.manifold.update_warmstart_multiplier(); + } + ctxt.manifold.sort_contacts(ctxt.prediction_distance); +} + +pub fn generate_contacts<'a>( + prediction_distance: f32, + cube1: &'a Cuboid, + mut pos1: &'a Isometry, + capsule2: &'a Capsule, + mut pos2: &'a Isometry, + manifold: &mut ContactManifold, + swapped: bool, +) { + let mut pos12 = pos1.inverse() * pos2; + let mut pos21 = pos12.inverse(); + + if (!swapped && manifold.try_update_contacts(&pos12)) + || (swapped && manifold.try_update_contacts(&pos21)) + { + return; + } + + let segment2 = Segment::new(capsule2.a, capsule2.b); + + /* + * + * Point-Face cases. + * + */ + let sep1 = sat::cube_support_map_find_local_separating_normal_oneway(cube1, &segment2, &pos12); + if sep1.0 > capsule2.radius + prediction_distance { + manifold.points.clear(); + return; + } + + #[cfg(feature = "dim3")] + let sep2 = (-f32::MAX, Vector::x()); + #[cfg(feature = "dim2")] + let sep2 = sat::segment_cuboid_find_local_separating_normal_oneway(&segment2, cube1, &pos21); + if sep2.0 > capsule2.radius + prediction_distance { + manifold.points.clear(); + return; + } + + /* + * + * Edge-Edge cases. + * + */ + #[cfg(feature = "dim2")] + let sep3 = (-f32::MAX, Vector::x()); // This case does not exist in 2D. + #[cfg(feature = "dim3")] + let sep3 = + sat::cube_segment_find_local_separating_edge_twoway(cube1, &segment2, &pos12, &pos21); + if sep3.0 > capsule2.radius + prediction_distance { + manifold.points.clear(); + return; + } + + /* + * + * Select the best combination of features + * and get the polygons to clip. + * + */ + let mut swapped_reference = false; + let mut best_sep = sep1; + + if sep2.0 > sep1.0 && sep2.0 > sep3.0 { + // The reference shape will be the second shape. + // std::mem::swap(&mut cube1, &mut capsule2); + std::mem::swap(&mut pos1, &mut pos2); + std::mem::swap(&mut pos12, &mut pos21); + best_sep = sep2; + swapped_reference = true; + } else if sep3.0 > sep1.0 { + best_sep = sep3; + } + + let feature1; + let mut feature2; + + #[cfg(feature = "dim2")] + { + if swapped_reference { + feature1 = CuboidFeatureFace::from(segment2); + feature2 = cuboid::support_face(cube1, pos21 * -best_sep.1); + } else { + feature1 = cuboid::support_face(cube1, best_sep.1); + feature2 = CuboidFeatureFace::from(segment2); + } + } + #[cfg(feature = "dim3")] + { + if swapped_reference { + feature1 = PolyhedronFace::from(segment2); + feature2 = cuboid::polyhedron_support_face(cube1, pos21 * -best_sep.1); + } else { + feature1 = cuboid::polyhedron_support_face(cube1, best_sep.1); + feature2 = PolyhedronFace::from(segment2); + } + } + + feature2.transform_by(&pos12); + + if swapped ^ swapped_reference { + manifold.swap_identifiers(); + } + + // We do this clone to perform contact tracking and transfer impulses. + // FIXME: find a more efficient way of doing this. + let old_manifold_points = manifold.points.clone(); + manifold.points.clear(); + + #[cfg(feature = "dim2")] + CuboidFeature::face_face_contacts( + prediction_distance + capsule2.radius, + &feature1, + &best_sep.1, + &feature2, + &pos21, + manifold, + ); + #[cfg(feature = "dim3")] + PolyhedronFace::contacts( + prediction_distance + capsule2.radius, + &feature1, + &best_sep.1, + &feature2, + &pos21, + manifold, + ); + + // Adjust points to take the radius into account. + manifold.local_n1 = best_sep.1; + manifold.local_n2 = pos21 * -best_sep.1; + manifold.kinematics.category = KinematicsCategory::PlanePoint; + manifold.kinematics.radius1 = 0.0; + manifold.kinematics.radius2 = 0.0; + + if swapped_reference { + for point in &mut manifold.points { + point.local_p1 += manifold.local_n1 * capsule2.radius; + point.dist -= capsule2.radius; + } + } else { + for point in &mut manifold.points { + point.local_p2 += manifold.local_n2 * capsule2.radius; + point.dist -= capsule2.radius; + } + } + + // Transfer impulses. + super::match_contacts(manifold, &old_manifold_points, swapped ^ swapped_reference); +} diff --git a/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs new file mode 100644 index 000000000..d879a222a --- /dev/null +++ b/src/geometry/contact_generator/cuboid_cuboid_contact_generator.rs @@ -0,0 +1,155 @@ +use crate::geometry::contact_generator::PrimitiveContactGenerationContext; +use crate::geometry::{cuboid, sat, ContactManifold, CuboidFeature, KinematicsCategory, Shape}; +use crate::math::Isometry; +#[cfg(feature = "dim2")] +use crate::math::Vector; +use ncollide::shape::Cuboid; + +pub fn generate_contacts_cuboid_cuboid(ctxt: &mut PrimitiveContactGenerationContext) { + if let (Shape::Cuboid(cube1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) { + generate_contacts( + ctxt.prediction_distance, + cube1, + ctxt.position1, + cube2, + ctxt.position2, + ctxt.manifold, + ); + } else { + unreachable!() + } + + ctxt.manifold.update_warmstart_multiplier(); + ctxt.manifold.sort_contacts(ctxt.prediction_distance); +} + +pub fn generate_contacts<'a>( + prediction_distance: f32, + mut cube1: &'a Cuboid, + mut pos1: &'a Isometry, + mut cube2: &'a Cuboid, + mut pos2: &'a Isometry, + manifold: &mut ContactManifold, +) { + let mut pos12 = pos1.inverse() * pos2; + let mut pos21 = pos12.inverse(); + + if manifold.try_update_contacts(&pos12) { + return; + } + + /* + * + * Point-Face + * + */ + let sep1 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cube1, cube2, &pos12, &pos21); + if sep1.0 > prediction_distance { + manifold.points.clear(); + return; + } + + let sep2 = sat::cuboid_cuboid_find_local_separating_normal_oneway(cube2, cube1, &pos21, &pos12); + if sep2.0 > prediction_distance { + manifold.points.clear(); + return; + } + + /* + * + * Edge-Edge cases + * + */ + #[cfg(feature = "dim2")] + let sep3 = (-f32::MAX, Vector::x()); // This case does not exist in 2D. + #[cfg(feature = "dim3")] + let sep3 = sat::cuboid_cuboid_find_local_separating_edge_twoway(cube1, cube2, &pos12, &pos21); + if sep3.0 > prediction_distance { + manifold.points.clear(); + return; + } + + /* + * + * Select the best combination of features + * and get the polygons to clip. + * + */ + let mut swapped = false; + let mut best_sep = sep1; + + if sep2.0 > sep1.0 && sep2.0 > sep3.0 { + // The reference shape will be the second shape. + std::mem::swap(&mut cube1, &mut cube2); + std::mem::swap(&mut pos1, &mut pos2); + std::mem::swap(&mut pos12, &mut pos21); + manifold.swap_identifiers(); + best_sep = sep2; + swapped = true; + } else if sep3.0 > sep1.0 { + best_sep = sep3; + } + + // We do this clone to perform contact tracking and transfer impulses. + // FIXME: find a more efficient way of doing this. + let old_manifold_points = manifold.points.clone(); + manifold.points.clear(); + + // Now the reference feature is from `cube1` and the best separation is `best_sep`. + // Everything must be expressed in the local-space of `cube1` for contact clipping. + let feature1 = cuboid::support_feature(cube1, best_sep.1); + let mut feature2 = cuboid::support_feature(cube2, pos21 * -best_sep.1); + feature2.transform_by(&pos12); + + match (&feature1, &feature2) { + (CuboidFeature::Face(f1), CuboidFeature::Vertex(v2)) => { + CuboidFeature::face_vertex_contacts(f1, &best_sep.1, v2, &pos21, manifold) + } + #[cfg(feature = "dim3")] + (CuboidFeature::Face(f1), CuboidFeature::Edge(e2)) => CuboidFeature::face_edge_contacts( + prediction_distance, + f1, + &best_sep.1, + e2, + &pos21, + manifold, + false, + ), + (CuboidFeature::Face(f1), CuboidFeature::Face(f2)) => CuboidFeature::face_face_contacts( + prediction_distance, + f1, + &best_sep.1, + f2, + &pos21, + manifold, + ), + #[cfg(feature = "dim3")] + (CuboidFeature::Edge(e1), CuboidFeature::Edge(e2)) => { + CuboidFeature::edge_edge_contacts(e1, &best_sep.1, e2, &pos21, manifold) + } + #[cfg(feature = "dim3")] + (CuboidFeature::Edge(e1), CuboidFeature::Face(f2)) => { + // Since f2 is also expressed in the local-space of the first + // feature, the position we provide here is pos21. + CuboidFeature::face_edge_contacts( + prediction_distance, + f2, + &-best_sep.1, + e1, + &pos21, + manifold, + true, + ) + } + _ => unreachable!(), // The other cases are not possible. + } + + manifold.local_n1 = best_sep.1; + manifold.local_n2 = pos21 * -best_sep.1; + manifold.kinematics.category = KinematicsCategory::PlanePoint; + manifold.kinematics.radius1 = 0.0; + manifold.kinematics.radius2 = 0.0; + + // Transfer impulses. + super::match_contacts(manifold, &old_manifold_points, swapped); +} diff --git a/src/geometry/contact_generator/cuboid_polygon_contact_generator.rs b/src/geometry/contact_generator/cuboid_polygon_contact_generator.rs new file mode 100644 index 000000000..e69de29bb diff --git a/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs new file mode 100644 index 000000000..1a0358df4 --- /dev/null +++ b/src/geometry/contact_generator/cuboid_triangle_contact_generator.rs @@ -0,0 +1,171 @@ +use crate::geometry::contact_generator::PrimitiveContactGenerationContext; +#[cfg(feature = "dim3")] +use crate::geometry::PolyhedronFace; +use crate::geometry::{cuboid, sat, ContactManifold, Cuboid, KinematicsCategory, Shape, Triangle}; +use crate::math::Isometry; +#[cfg(feature = "dim2")] +use crate::{ + geometry::{triangle, CuboidFeature}, + math::Vector, +}; + +pub fn generate_contacts_cuboid_triangle(ctxt: &mut PrimitiveContactGenerationContext) { + if let (Shape::Cuboid(cube1), Shape::Triangle(triangle2)) = (ctxt.shape1, ctxt.shape2) { + generate_contacts( + ctxt.prediction_distance, + cube1, + ctxt.position1, + triangle2, + ctxt.position2, + ctxt.manifold, + false, + ); + ctxt.manifold.update_warmstart_multiplier(); + } else if let (Shape::Triangle(triangle1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) { + generate_contacts( + ctxt.prediction_distance, + cube2, + ctxt.position2, + triangle1, + ctxt.position1, + ctxt.manifold, + true, + ); + ctxt.manifold.update_warmstart_multiplier(); + } + ctxt.manifold.sort_contacts(ctxt.prediction_distance); +} + +pub fn generate_contacts<'a>( + prediction_distance: f32, + cube1: &'a Cuboid, + mut pos1: &'a Isometry, + triangle2: &'a Triangle, + mut pos2: &'a Isometry, + manifold: &mut ContactManifold, + swapped: bool, +) { + let mut pos12 = pos1.inverse() * pos2; + let mut pos21 = pos12.inverse(); + + if (!swapped && manifold.try_update_contacts(&pos12)) + || (swapped && manifold.try_update_contacts(&pos21)) + { + return; + } + + /* + * + * Point-Face cases. + * + */ + let sep1 = sat::cube_support_map_find_local_separating_normal_oneway(cube1, triangle2, &pos12); + if sep1.0 > prediction_distance { + manifold.points.clear(); + return; + } + + let sep2 = sat::triangle_cuboid_find_local_separating_normal_oneway(triangle2, cube1, &pos21); + if sep2.0 > prediction_distance { + manifold.points.clear(); + return; + } + + /* + * + * Edge-Edge cases. + * + */ + #[cfg(feature = "dim2")] + let sep3 = (-f32::MAX, Vector::x()); // This case does not exist in 2D. + #[cfg(feature = "dim3")] + let sep3 = + sat::cube_triangle_find_local_separating_edge_twoway(cube1, triangle2, &pos12, &pos21); + if sep3.0 > prediction_distance { + manifold.points.clear(); + return; + } + + /* + * + * Select the best combination of features + * and get the polygons to clip. + * + */ + let mut swapped_reference = false; + let mut best_sep = sep1; + + if sep2.0 > sep1.0 && sep2.0 > sep3.0 { + // The reference shape will be the second shape. + // std::mem::swap(&mut cube1, &mut triangle2); + std::mem::swap(&mut pos1, &mut pos2); + std::mem::swap(&mut pos12, &mut pos21); + best_sep = sep2; + swapped_reference = true; + } else if sep3.0 > sep1.0 { + best_sep = sep3; + } + + let feature1; + let mut feature2; + + #[cfg(feature = "dim2")] + { + if swapped_reference { + feature1 = triangle::support_face(triangle2, best_sep.1); + feature2 = cuboid::support_face(cube1, pos21 * -best_sep.1); + } else { + feature1 = cuboid::support_face(cube1, best_sep.1); + feature2 = triangle::support_face(triangle2, pos21 * -best_sep.1); + } + } + #[cfg(feature = "dim3")] + { + if swapped_reference { + feature1 = PolyhedronFace::from(*triangle2); + feature2 = cuboid::polyhedron_support_face(cube1, pos21 * -best_sep.1); + } else { + feature1 = cuboid::polyhedron_support_face(cube1, best_sep.1); + feature2 = PolyhedronFace::from(*triangle2); + } + } + + feature2.transform_by(&pos12); + + if swapped ^ swapped_reference { + manifold.swap_identifiers(); + } + + // We do this clone to perform contact tracking and transfer impulses. + // FIXME: find a more efficient way of doing this. + let old_manifold_points = manifold.points.clone(); + manifold.points.clear(); + + #[cfg(feature = "dim2")] + CuboidFeature::face_face_contacts( + prediction_distance, + &feature1, + &best_sep.1, + &feature2, + &pos21, + manifold, + ); + #[cfg(feature = "dim3")] + PolyhedronFace::contacts( + prediction_distance, + &feature1, + &best_sep.1, + &feature2, + &pos21, + manifold, + ); + + manifold.local_n1 = best_sep.1; + manifold.local_n2 = pos21 * -best_sep.1; + manifold.kinematics.category = KinematicsCategory::PlanePoint; + manifold.kinematics.radius1 = 0.0; + manifold.kinematics.radius2 = 0.0; + + // Transfer impulses. + super::match_contacts(manifold, &old_manifold_points, swapped ^ swapped_reference); +} diff --git a/src/geometry/contact_generator/heightfield_shape_contact_generator.rs b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs new file mode 100644 index 000000000..f59a94bdd --- /dev/null +++ b/src/geometry/contact_generator/heightfield_shape_contact_generator.rs @@ -0,0 +1,189 @@ +use crate::geometry::contact_generator::{ + ContactGenerationContext, PrimitiveContactGenerationContext, PrimitiveContactGenerator, +}; +#[cfg(feature = "dim2")] +use crate::geometry::Capsule; +use crate::geometry::{Collider, ContactManifold, HeightField, Shape}; +use crate::ncollide::bounding_volume::BoundingVolume; +#[cfg(feature = "dim3")] +use crate::{geometry::Triangle, math::Point}; +use std::any::Any; +use std::collections::hash_map::Entry; +use std::collections::HashMap; + +struct SubDetector { + generator: PrimitiveContactGenerator, + manifold_id: usize, + timestamp: bool, + workspace: Option>, +} + +pub struct HeightFieldShapeContactGeneratorWorkspace { + timestamp: bool, + old_manifolds: Vec, + sub_detectors: HashMap, +} + +impl HeightFieldShapeContactGeneratorWorkspace { + pub fn new() -> Self { + Self { + timestamp: false, + old_manifolds: Vec::new(), + sub_detectors: HashMap::default(), + } + } +} + +pub fn generate_contacts_heightfield_shape(ctxt: &mut ContactGenerationContext) { + let collider1 = &ctxt.colliders[ctxt.pair.pair.collider1]; + let collider2 = &ctxt.colliders[ctxt.pair.pair.collider2]; + + if let Shape::HeightField(heightfield1) = collider1.shape() { + do_generate_contacts(heightfield1, collider1, collider2, ctxt, false) + } else if let Shape::HeightField(heightfield2) = collider2.shape() { + do_generate_contacts(heightfield2, collider2, collider1, ctxt, true) + } +} + +fn do_generate_contacts( + heightfield1: &HeightField, + collider1: &Collider, + collider2: &Collider, + ctxt: &mut ContactGenerationContext, + _flipped: bool, +) { + let workspace: &mut HeightFieldShapeContactGeneratorWorkspace = ctxt + .pair + .generator_workspace + .as_mut() + .expect("The HeightFieldShapeContactGeneratorWorkspace is missing.") + .downcast_mut() + .expect("Invalid workspace type, expected a HeightFieldShapeContactGeneratorWorkspace."); + + /* + * Detect if the detector context has been reset. + */ + if !ctxt.pair.manifolds.is_empty() && workspace.sub_detectors.is_empty() { + // Rebuild the subdetector hashmap. + for (manifold_id, manifold) in ctxt.pair.manifolds.iter().enumerate() { + let subshape_id = if manifold.pair.collider1 == ctxt.pair.pair.collider1 { + manifold.subshape_index_pair.0 + } else { + manifold.subshape_index_pair.1 + }; + println!( + "Restoring for {} [chosen with {:?}]", + subshape_id, manifold.subshape_index_pair + ); + + // Use dummy shapes for the dispatch. + #[cfg(feature = "dim2")] + let sub_shape1 = + Shape::Capsule(Capsule::new(na::Point::origin(), na::Point::origin(), 0.0)); + #[cfg(feature = "dim3")] + let sub_shape1 = Shape::Triangle(Triangle::new( + Point::origin(), + Point::origin(), + Point::origin(), + )); + let (generator, workspace2) = ctxt + .dispatcher + .dispatch_primitives(&sub_shape1, collider2.shape()); + + let sub_detector = SubDetector { + generator, + manifold_id, + timestamp: workspace.timestamp, + workspace: workspace2, + }; + + workspace.sub_detectors.insert(subshape_id, sub_detector); + } + } + + let new_timestamp = !workspace.timestamp; + workspace.timestamp = new_timestamp; + + /* + * Compute interferences. + */ + let pos12 = collider1.position.inverse() * collider2.position; + // TODO: somehow precompute the AABB and reuse it? + let ls_aabb2 = collider2 + .shape() + .compute_aabb(&pos12) + .loosened(ctxt.prediction_distance); + + std::mem::swap(&mut workspace.old_manifolds, &mut ctxt.pair.manifolds); + ctxt.pair.manifolds.clear(); + let coll_pair = ctxt.pair.pair; + let manifolds = &mut ctxt.pair.manifolds; + let prediction_distance = ctxt.prediction_distance; + let dispatcher = ctxt.dispatcher; + + heightfield1.map_elements_in_local_aabb(&ls_aabb2, &mut |i, part1, _| { + #[cfg(feature = "dim2")] + let sub_shape1 = Shape::Capsule(Capsule::new(part1.a, part1.b, 0.0)); + #[cfg(feature = "dim3")] + let sub_shape1 = Shape::Triangle(*part1); + let sub_detector = match workspace.sub_detectors.entry(i) { + Entry::Occupied(entry) => { + let sub_detector = entry.into_mut(); + let manifold = workspace.old_manifolds[sub_detector.manifold_id].take(); + sub_detector.manifold_id = manifolds.len(); + sub_detector.timestamp = new_timestamp; + manifolds.push(manifold); + sub_detector + } + Entry::Vacant(entry) => { + let (generator, workspace2) = + dispatcher.dispatch_primitives(&sub_shape1, collider2.shape()); + let sub_detector = SubDetector { + generator, + manifold_id: manifolds.len(), + timestamp: new_timestamp, + workspace: workspace2, + }; + let manifold = + ContactManifold::with_subshape_indices(coll_pair, collider1, collider2, i, 0); + manifolds.push(manifold); + + entry.insert(sub_detector) + } + }; + + let manifold = &mut manifolds[sub_detector.manifold_id]; + + let mut ctxt2 = if coll_pair.collider1 != manifold.pair.collider1 { + PrimitiveContactGenerationContext { + prediction_distance, + collider1: collider2, + collider2: collider1, + shape1: collider2.shape(), + shape2: &sub_shape1, + position1: collider2.position(), + position2: collider1.position(), + manifold, + workspace: sub_detector.workspace.as_deref_mut(), + } + } else { + PrimitiveContactGenerationContext { + prediction_distance, + collider1, + collider2, + shape1: &sub_shape1, + shape2: collider2.shape(), + position1: collider1.position(), + position2: collider2.position(), + manifold, + workspace: sub_detector.workspace.as_deref_mut(), + } + }; + + (sub_detector.generator.generate_contacts)(&mut ctxt2) + }); + + workspace + .sub_detectors + .retain(|_, detector| detector.timestamp == new_timestamp) +} diff --git a/src/geometry/contact_generator/mod.rs b/src/geometry/contact_generator/mod.rs new file mode 100644 index 000000000..ecd254061 --- /dev/null +++ b/src/geometry/contact_generator/mod.rs @@ -0,0 +1,71 @@ +pub use self::ball_ball_contact_generator::generate_contacts_ball_ball; +#[cfg(feature = "simd-is-enabled")] +pub use self::ball_ball_contact_generator::generate_contacts_ball_ball_simd; +pub use self::ball_convex_contact_generator::generate_contacts_ball_convex; +pub use self::capsule_capsule_contact_generator::generate_contacts_capsule_capsule; +pub use self::contact_dispatcher::{ContactDispatcher, DefaultContactDispatcher}; +pub use self::contact_generator::{ + ContactGenerationContext, ContactGenerator, ContactPhase, PrimitiveContactGenerationContext, + PrimitiveContactGenerator, +}; +#[cfg(feature = "simd-is-enabled")] +pub use self::contact_generator::{ + ContactGenerationContextSimd, PrimitiveContactGenerationContextSimd, +}; +pub use self::cuboid_capsule_contact_generator::generate_contacts_cuboid_capsule; +pub use self::cuboid_cuboid_contact_generator::generate_contacts_cuboid_cuboid; +pub use self::cuboid_triangle_contact_generator::generate_contacts_cuboid_triangle; +pub use self::heightfield_shape_contact_generator::{ + generate_contacts_heightfield_shape, HeightFieldShapeContactGeneratorWorkspace, +}; +pub use self::polygon_polygon_contact_generator::generate_contacts_polygon_polygon; +pub use self::trimesh_shape_contact_generator::{ + generate_contacts_trimesh_shape, TrimeshShapeContactGeneratorWorkspace, +}; + +#[cfg(feature = "dim2")] +pub(crate) use self::polygon_polygon_contact_generator::{ + clip_segments, clip_segments_with_normal, +}; + +mod ball_ball_contact_generator; +mod ball_convex_contact_generator; +mod ball_polygon_contact_generator; +mod capsule_capsule_contact_generator; +mod contact_dispatcher; +mod contact_generator; +mod cuboid_capsule_contact_generator; +mod cuboid_cuboid_contact_generator; +mod cuboid_polygon_contact_generator; +mod cuboid_triangle_contact_generator; +mod heightfield_shape_contact_generator; +mod polygon_polygon_contact_generator; +mod trimesh_shape_contact_generator; + +use crate::geometry::{Contact, ContactManifold}; + +pub(crate) fn match_contacts( + manifold: &mut ContactManifold, + old_contacts: &[Contact], + swapped: bool, +) { + for contact in &mut manifold.points { + if !swapped { + for old_contact in old_contacts { + if contact.fid1 == old_contact.fid1 && contact.fid2 == old_contact.fid2 { + // Transfer impulse cache. + contact.impulse = old_contact.impulse; + contact.tangent_impulse = old_contact.tangent_impulse; + } + } + } else { + for old_contact in old_contacts { + if contact.fid1 == old_contact.fid2 && contact.fid2 == old_contact.fid1 { + // Transfer impulse cache. + contact.impulse = old_contact.impulse; + contact.tangent_impulse = old_contact.tangent_impulse; + } + } + } + } +} diff --git a/src/geometry/contact_generator/polygon_polygon_contact_generator.rs b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs new file mode 100644 index 000000000..33b54e4ef --- /dev/null +++ b/src/geometry/contact_generator/polygon_polygon_contact_generator.rs @@ -0,0 +1,298 @@ +use crate::geometry::contact_generator::PrimitiveContactGenerationContext; +use crate::geometry::{sat, Contact, ContactManifold, KinematicsCategory, Polygon, Shape}; +use crate::math::{Isometry, Point}; +#[cfg(feature = "dim2")] +use crate::{math::Vector, utils}; + +pub fn generate_contacts_polygon_polygon(ctxt: &mut PrimitiveContactGenerationContext) { + if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) { + generate_contacts( + polygon1, + &ctxt.position1, + polygon2, + &ctxt.position2, + ctxt.manifold, + ); + ctxt.manifold.update_warmstart_multiplier(); + } else { + unreachable!() + } + + ctxt.manifold.sort_contacts(ctxt.prediction_distance); +} + +fn generate_contacts<'a>( + mut p1: &'a Polygon, + mut m1: &'a Isometry, + mut p2: &'a Polygon, + mut m2: &'a Isometry, + manifold: &'a mut ContactManifold, +) { + let mut m12 = m1.inverse() * m2; + let mut m21 = m12.inverse(); + + if manifold.try_update_contacts(&m12) { + return; + } + + let mut sep1 = sat::polygon_polygon_compute_separation_features(p1, p2, &m12); + if sep1.0 > 0.0 { + manifold.points.clear(); + return; + } + + let mut sep2 = sat::polygon_polygon_compute_separation_features(p2, p1, &m21); + if sep2.0 > 0.0 { + manifold.points.clear(); + return; + } + + let mut swapped = false; + if sep2.0 > sep1.0 { + std::mem::swap(&mut sep1, &mut sep2); + std::mem::swap(&mut m1, &mut m2); + std::mem::swap(&mut p1, &mut p2); + std::mem::swap(&mut m12, &mut m21); + manifold.swap_identifiers(); + swapped = true; + } + + let support_face1 = sep1.1; + let local_n1 = p1.normals[support_face1]; + let local_n2 = m21 * -local_n1; + let support_face2 = p2.support_face(&local_n2); + let len1 = p1.vertices.len(); + let len2 = p2.vertices.len(); + + let seg1 = ( + p1.vertices[support_face1], + p1.vertices[(support_face1 + 1) % len1], + ); + let seg2 = ( + m12 * p2.vertices[support_face2], + m12 * p2.vertices[(support_face2 + 1) % len2], + ); + if let Some((clip_a, clip_b)) = clip_segments(seg1, seg2) { + let dist_a = (clip_a.1 - clip_a.0).dot(&local_n1); + let dist_b = (clip_b.1 - clip_b.0).dot(&local_n1); + + let mut impulses_a = (0.0, Contact::zero_tangent_impulse()); + let mut impulses_b = (0.0, Contact::zero_tangent_impulse()); + + let fids_a = ( + ((support_face1 * 2 + clip_a.2) % (len1 * 2)) as u8, + ((support_face2 * 2 + clip_a.3) % (len2 * 2)) as u8, + ); + + let fids_b = ( + ((support_face1 * 2 + clip_b.2) % (len1 * 2)) as u8, + ((support_face2 * 2 + clip_b.3) % (len2 * 2)) as u8, + ); + + if manifold.points.len() != 0 { + assert_eq!(manifold.points.len(), 2); + + // We already had 2 points in the previous iteration. + // Match the features to see if we keep the cached impulse. + let original_fids_a; + let original_fids_b; + + // NOTE: the previous manifold may have its bodies swapped wrt. our new manifold. + // So we have to adjust accordingly the features we will be comparing. + if swapped { + original_fids_a = (manifold.points[0].fid1, manifold.points[0].fid2); + original_fids_b = (manifold.points[1].fid1, manifold.points[1].fid2); + } else { + original_fids_a = (manifold.points[0].fid2, manifold.points[0].fid1); + original_fids_b = (manifold.points[1].fid2, manifold.points[1].fid1); + } + + if fids_a == original_fids_a { + impulses_a = ( + manifold.points[0].impulse, + manifold.points[0].tangent_impulse, + ); + } else if fids_a == original_fids_b { + impulses_a = ( + manifold.points[1].impulse, + manifold.points[1].tangent_impulse, + ); + } + if fids_b == original_fids_a { + impulses_b = ( + manifold.points[0].impulse, + manifold.points[0].tangent_impulse, + ); + } else if fids_b == original_fids_b { + impulses_b = ( + manifold.points[1].impulse, + manifold.points[1].tangent_impulse, + ); + } + } + + manifold.points.clear(); + manifold.points.push(Contact { + local_p1: clip_a.0, + local_p2: m21 * clip_a.1, + impulse: impulses_a.0, + tangent_impulse: impulses_a.1, + fid1: fids_a.0, + fid2: fids_a.1, + dist: dist_a, + }); + + manifold.points.push(Contact { + local_p1: clip_b.0, + local_p2: m21 * clip_b.1, + impulse: impulses_b.0, + tangent_impulse: impulses_b.1, + fid1: fids_b.0, + fid2: fids_b.1, + dist: dist_b, + }); + + manifold.local_n1 = local_n1; + manifold.local_n2 = local_n2; + manifold.kinematics.category = KinematicsCategory::PlanePoint; + manifold.kinematics.radius1 = 0.0; + manifold.kinematics.radius2 = 0.0; + } else { + manifold.points.clear(); + } +} + +// Features in clipping points are: +// 0 = First vertex. +// 1 = On the face. +// 2 = Second vertex. +pub(crate) type ClippingPoints = (Point, Point, usize, usize); + +#[cfg(feature = "dim2")] +pub(crate) fn clip_segments_with_normal( + mut seg1: (Point, Point), + mut seg2: (Point, Point), + normal: Vector, +) -> Option<(ClippingPoints, ClippingPoints)> { + use crate::utils::WBasis; + let tangent = normal.orthonormal_basis()[0]; + + let mut range1 = [seg1.0.coords.dot(&tangent), seg1.1.coords.dot(&tangent)]; + let mut range2 = [seg2.0.coords.dot(&tangent), seg2.1.coords.dot(&tangent)]; + let mut features1 = [0, 2]; + let mut features2 = [0, 2]; + + if range1[1] < range1[0] { + range1.swap(0, 1); + features1.swap(0, 1); + std::mem::swap(&mut seg1.0, &mut seg1.1); + } + + if range2[1] < range2[0] { + range2.swap(0, 1); + features2.swap(0, 1); + std::mem::swap(&mut seg2.0, &mut seg2.1); + } + + if range2[0] > range1[1] || range1[0] > range2[1] { + // No clip point. + return None; + } + + let ca = if range2[0] > range1[0] { + let bcoord = (range2[0] - range1[0]) * utils::inv(range1[1] - range1[0]); + let p1 = seg1.0 + (seg1.1 - seg1.0) * bcoord; + let p2 = seg2.0; + + (p1, p2, 1, features2[0]) + } else { + let bcoord = (range1[0] - range2[0]) * utils::inv(range2[1] - range2[0]); + let p1 = seg1.0; + let p2 = seg2.0 + (seg2.1 - seg2.0) * bcoord; + + (p1, p2, features1[0], 1) + }; + + let cb = if range2[1] < range1[1] { + let bcoord = (range2[1] - range1[0]) * utils::inv(range1[1] - range1[0]); + let p1 = seg1.0 + (seg1.1 - seg1.0) * bcoord; + let p2 = seg2.1; + + (p1, p2, 1, features2[1]) + } else { + let bcoord = (range1[1] - range2[0]) * utils::inv(range2[1] - range2[0]); + let p1 = seg1.1; + let p2 = seg2.0 + (seg2.1 - seg2.0) * bcoord; + + (p1, p2, features1[1], 1) + }; + + Some((ca, cb)) +} + +pub(crate) fn clip_segments( + mut seg1: (Point, Point), + mut seg2: (Point, Point), +) -> Option<(ClippingPoints, ClippingPoints)> { + // NOTE: no need to normalize the tangent. + let tangent1 = seg1.1 - seg1.0; + let sqnorm_tangent1 = tangent1.norm_squared(); + + let mut range1 = [0.0, sqnorm_tangent1]; + let mut range2 = [ + (seg2.0 - seg1.0).dot(&tangent1), + (seg2.1 - seg1.0).dot(&tangent1), + ]; + let mut features1 = [0, 2]; + let mut features2 = [0, 2]; + + if range1[1] < range1[0] { + range1.swap(0, 1); + features1.swap(0, 1); + std::mem::swap(&mut seg1.0, &mut seg1.1); + } + + if range2[1] < range2[0] { + range2.swap(0, 1); + features2.swap(0, 1); + std::mem::swap(&mut seg2.0, &mut seg2.1); + } + + if range2[0] > range1[1] || range1[0] > range2[1] { + // No clip point. + return None; + } + + let length1 = range1[1] - range1[0]; + let length2 = range2[1] - range2[0]; + + let ca = if range2[0] > range1[0] { + let bcoord = (range2[0] - range1[0]) / length1; + let p1 = seg1.0 + tangent1 * bcoord; + let p2 = seg2.0; + + (p1, p2, 1, features2[0]) + } else { + let bcoord = (range1[0] - range2[0]) / length2; + let p1 = seg1.0; + let p2 = seg2.0 + (seg2.1 - seg2.0) * bcoord; + + (p1, p2, features1[0], 1) + }; + + let cb = if range2[1] < range1[1] { + let bcoord = (range2[1] - range1[0]) / length1; + let p1 = seg1.0 + tangent1 * bcoord; + let p2 = seg2.1; + + (p1, p2, 1, features2[1]) + } else { + let bcoord = (range1[1] - range2[0]) / length2; + let p1 = seg1.1; + let p2 = seg2.0 + (seg2.1 - seg2.0) * bcoord; + + (p1, p2, features1[1], 1) + }; + + Some((ca, cb)) +} diff --git a/src/geometry/contact_generator/trimesh_shape_contact_generator.rs b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs new file mode 100644 index 000000000..2d2309b1f --- /dev/null +++ b/src/geometry/contact_generator/trimesh_shape_contact_generator.rs @@ -0,0 +1,194 @@ +use crate::geometry::contact_generator::{ + ContactGenerationContext, PrimitiveContactGenerationContext, +}; +use crate::geometry::{Collider, ContactManifold, Shape, Trimesh, WAABBHierarchyIntersections}; +use crate::ncollide::bounding_volume::{BoundingVolume, AABB}; + +pub struct TrimeshShapeContactGeneratorWorkspace { + interferences: WAABBHierarchyIntersections, + local_aabb2: AABB, + old_interferences: Vec, + old_manifolds: Vec, +} + +impl TrimeshShapeContactGeneratorWorkspace { + pub fn new() -> Self { + Self { + interferences: WAABBHierarchyIntersections::new(), + local_aabb2: AABB::new_invalid(), + old_interferences: Vec::new(), + old_manifolds: Vec::new(), + } + } +} + +pub fn generate_contacts_trimesh_shape(ctxt: &mut ContactGenerationContext) { + let collider1 = &ctxt.colliders[ctxt.pair.pair.collider1]; + let collider2 = &ctxt.colliders[ctxt.pair.pair.collider2]; + + if let Shape::Trimesh(trimesh1) = collider1.shape() { + do_generate_contacts(trimesh1, collider1, collider2, ctxt, false) + } else if let Shape::Trimesh(trimesh2) = collider2.shape() { + do_generate_contacts(trimesh2, collider2, collider1, ctxt, true) + } +} + +fn do_generate_contacts( + trimesh1: &Trimesh, + collider1: &Collider, + collider2: &Collider, + ctxt: &mut ContactGenerationContext, + flipped: bool, +) { + let workspace: &mut TrimeshShapeContactGeneratorWorkspace = ctxt + .pair + .generator_workspace + .as_mut() + .expect("The TrimeshShapeContactGeneratorWorkspace is missing.") + .downcast_mut() + .expect("Invalid workspace type, expected a TrimeshShapeContactGeneratorWorkspace."); + + /* + * Compute interferences. + */ + let pos12 = collider1.position.inverse() * collider2.position; + // TODO: somehow precompute the AABB and reuse it? + let mut new_local_aabb2 = collider2 + .shape() + .compute_aabb(&pos12) + .loosened(ctxt.prediction_distance); + let same_local_aabb2 = workspace.local_aabb2.contains(&new_local_aabb2); + + if !same_local_aabb2 { + let extra_margin = + (new_local_aabb2.maxs - new_local_aabb2.mins).map(|e| (e / 10.0).min(0.1)); + new_local_aabb2.mins -= extra_margin; + new_local_aabb2.maxs += extra_margin; + + let local_aabb2 = new_local_aabb2; // .loosened(ctxt.prediction_distance * 2.0); // FIXME: what would be the best value? + std::mem::swap( + &mut workspace.old_interferences, + workspace.interferences.computed_interferences_mut(), + ); + std::mem::swap(&mut workspace.old_manifolds, &mut ctxt.pair.manifolds); + ctxt.pair.manifolds.clear(); + + if workspace.old_interferences.is_empty() && !workspace.old_manifolds.is_empty() { + // This happens if for some reasons the contact generator context was lost + // and rebuilt. In this case, we hate to reconstruct the `old_interferences` + // array using the subshape ids from the contact manifolds. + // TODO: always rely on the subshape ids instead of maintaining `.ord_interferences` ? + let ctxt_collider1 = ctxt.pair.pair.collider1; + workspace.old_interferences = workspace + .old_manifolds + .iter() + .map(|manifold| { + if manifold.pair.collider1 == ctxt_collider1 { + manifold.subshape_index_pair.0 + } else { + manifold.subshape_index_pair.1 + } + }) + .collect(); + } + assert_eq!( + workspace + .old_interferences + .len() + .min(trimesh1.num_triangles()), + workspace.old_manifolds.len() + ); + + trimesh1 + .waabbs() + .compute_interferences_with(local_aabb2, &mut workspace.interferences); + workspace.local_aabb2 = local_aabb2; + } + + /* + * Dispatch to the specific solver by keeping the previous manifold if we already had one. + */ + let new_interferences = workspace.interferences.computed_interferences(); + let mut old_inter_it = workspace.old_interferences.drain(..).peekable(); + let mut old_manifolds_it = workspace.old_manifolds.drain(..); + + for (i, triangle_id) in new_interferences.iter().enumerate() { + if *triangle_id >= trimesh1.num_triangles() { + // Because of SIMD padding, the broad-phase may return tiangle indices greater + // than the max. + continue; + } + if !same_local_aabb2 { + loop { + match old_inter_it.peek() { + Some(old_triangle_id) if *old_triangle_id < *triangle_id => { + old_inter_it.next(); + old_manifolds_it.next(); + } + _ => break, + } + } + + let manifold = if old_inter_it.peek() != Some(triangle_id) { + // We don't have a manifold for this triangle yet. + if flipped { + ContactManifold::with_subshape_indices( + ctxt.pair.pair, + collider2, + collider1, + *triangle_id, + 0, + ) + } else { + ContactManifold::with_subshape_indices( + ctxt.pair.pair, + collider1, + collider2, + 0, + *triangle_id, + ) + } + } else { + // We already have a manifold for this triangle. + old_inter_it.next(); + old_manifolds_it.next().unwrap() + }; + + ctxt.pair.manifolds.push(manifold); + } + + let manifold = &mut ctxt.pair.manifolds[i]; + let triangle1 = Shape::Triangle(trimesh1.triangle(*triangle_id)); + let (generator, mut workspace2) = ctxt + .dispatcher + .dispatch_primitives(&triangle1, collider2.shape()); + + let mut ctxt2 = if ctxt.pair.pair.collider1 != manifold.pair.collider1 { + PrimitiveContactGenerationContext { + prediction_distance: ctxt.prediction_distance, + collider1: collider2, + collider2: collider1, + shape1: collider2.shape(), + shape2: &triangle1, + position1: collider2.position(), + position2: collider1.position(), + manifold, + workspace: workspace2.as_deref_mut(), + } + } else { + PrimitiveContactGenerationContext { + prediction_distance: ctxt.prediction_distance, + collider1, + collider2, + shape1: &triangle1, + shape2: collider2.shape(), + position1: collider1.position(), + position2: collider2.position(), + manifold, + workspace: workspace2.as_deref_mut(), + } + }; + + (generator.generate_contacts)(&mut ctxt2); + } +} diff --git a/src/geometry/contact_generator/voxels_shape_contact_generator.rs b/src/geometry/contact_generator/voxels_shape_contact_generator.rs new file mode 100644 index 000000000..e69de29bb diff --git a/src/geometry/cuboid.rs b/src/geometry/cuboid.rs new file mode 100644 index 000000000..89e1a1eb8 --- /dev/null +++ b/src/geometry/cuboid.rs @@ -0,0 +1,234 @@ +#[cfg(feature = "dim3")] +use crate::geometry::PolyhedronFace; +use crate::geometry::{Cuboid, CuboidFeature, CuboidFeatureFace}; +use crate::math::{Point, Vector}; +use crate::utils::WSign; + +pub fn local_support_point(cube: &Cuboid, local_dir: Vector) -> Point { + local_dir.copy_sign_to(cube.half_extents).into() +} + +// #[cfg(feature = "dim2")] +// pub fn polygon_ref( +// cuboid: Cuboid, +// out_vertices: &mut [Point; 4], +// out_normals: &mut [Vector; 4], +// ) -> PolygonRef { +// *out_vertices = [ +// Point::new(cuboid.half_extents.x, -cuboid.half_extents.y), +// Point::new(cuboid.half_extents.x, cuboid.half_extents.y), +// Point::new(-cuboid.half_extents.x, cuboid.half_extents.y), +// Point::new(-cuboid.half_extents.x, -cuboid.half_extents.y), +// ]; +// *out_normals = [Vector::x(), Vector::y(), -Vector::x(), -Vector::y()]; +// +// PolygonRef { +// vertices: &out_vertices[..], +// normals: &out_normals[..], +// } +// } + +#[cfg(feature = "dim2")] +pub fn vertex_feature_id(vertex: Point) -> u8 { + ((vertex.x.to_bits() >> 31) & 0b001 | (vertex.y.to_bits() >> 30) & 0b010) as u8 +} + +// #[cfg(feature = "dim3")] +// pub fn vertex_feature_id(vertex: Point) -> u8 { +// ((vertex.x.to_bits() >> 31) & 0b001 +// | (vertex.y.to_bits() >> 30) & 0b010 +// | (vertex.z.to_bits() >> 29) & 0b100) as u8 +// } + +#[cfg(feature = "dim3")] +pub fn polyhedron_support_face(cube: &Cuboid, local_dir: Vector) -> PolyhedronFace { + support_face(cube, local_dir).into() +} + +#[cfg(feature = "dim2")] +pub(crate) fn support_feature(cube: &Cuboid, local_dir: Vector) -> CuboidFeature { + // In 2D, it is best for stability to always return a face. + // It won't have any notable impact on performances anyway. + CuboidFeature::Face(support_face(cube, local_dir)) + + /* + let amax = local_dir.amax(); + + const MAX_DOT_THRESHOLD: f32 = 0.98480775301; // 10 degrees. + + if amax > MAX_DOT_THRESHOLD { + // Support face. + CuboidFeature::Face(cube.support_face(local_dir)) + } else { + // Support vertex + CuboidFeature::Vertex(cube.support_vertex(local_dir)) + } + */ +} + +#[cfg(feature = "dim3")] +pub(crate) fn support_feature(cube: &Cuboid, local_dir: Vector) -> CuboidFeature { + CuboidFeature::Face(support_face(cube, local_dir)) + /* + const MAX_DOT_THRESHOLD: f32 = crate::utils::COS_10_DEGREES; + const MIN_DOT_THRESHOLD: f32 = 1.0 - MAX_DOT_THRESHOLD; + + let amax = local_dir.amax(); + let amin = local_dir.amin(); + + if amax > MAX_DOT_THRESHOLD { + // Support face. + CuboidFeature::Face(support_face(cube, local_dir)) + } else if amin < MIN_DOT_THRESHOLD { + // Support edge. + CuboidFeature::Edge(support_edge(cube, local_dir)) + } else { + // Support vertex. + CuboidFeature::Vertex(support_vertex(cube, local_dir)) + } + */ +} + +// #[cfg(feature = "dim3")] +// pub(crate) fn support_vertex(cube: &Cuboid, local_dir: Vector) -> CuboidFeatureVertex { +// let vertex = local_support_point(cube, local_dir); +// let vid = vertex_feature_id(vertex); +// +// CuboidFeatureVertex { vertex, vid } +// } + +// #[cfg(feature = "dim3")] +// pub(crate) fn support_edge(cube: &Cuboid, local_dir: Vector) -> CuboidFeatureEdge { +// let he = cube.half_extents; +// let i = local_dir.iamin(); +// let j = (i + 1) % 3; +// let k = (i + 2) % 3; +// let mut a = Point::origin(); +// a[i] = he[i]; +// a[j] = local_dir[j].copy_sign_to(he[j]); +// a[k] = local_dir[k].copy_sign_to(he[k]); +// +// let mut b = a; +// b[i] = -he[i]; +// +// let vid1 = vertex_feature_id(a); +// let vid2 = vertex_feature_id(b); +// let eid = (vid1.max(vid2) << 3) | vid1.min(vid2) | 0b11_000_000; +// +// CuboidFeatureEdge { +// vertices: [a, b], +// vids: [vid1, vid2], +// eid, +// } +// } + +#[cfg(feature = "dim2")] +pub fn support_face(cube: &Cuboid, local_dir: Vector) -> CuboidFeatureFace { + let he = cube.half_extents; + let i = local_dir.iamin(); + let j = (i + 1) % 2; + let mut a = Point::origin(); + a[i] = he[i]; + a[j] = local_dir[j].copy_sign_to(he[j]); + + let mut b = a; + b[i] = -he[i]; + + let vid1 = vertex_feature_id(a); + let vid2 = vertex_feature_id(b); + let fid = (vid1.max(vid2) << 2) | vid1.min(vid2) | 0b11_00_00; + + CuboidFeatureFace { + vertices: [a, b], + vids: [vid1, vid2], + fid, + } +} + +#[cfg(feature = "dim3")] +pub(crate) fn support_face(cube: &Cuboid, local_dir: Vector) -> CuboidFeatureFace { + // NOTE: can we use the orthonormal basis of local_dir + // to make this AoSoA friendly? + let he = cube.half_extents; + let iamax = local_dir.iamax(); + let sign = local_dir[iamax].copy_sign_to(1.0); + + let vertices = match iamax { + 0 => [ + Point::new(he.x * sign, he.y, he.z), + Point::new(he.x * sign, -he.y, he.z), + Point::new(he.x * sign, -he.y, -he.z), + Point::new(he.x * sign, he.y, -he.z), + ], + 1 => [ + Point::new(he.x, he.y * sign, he.z), + Point::new(-he.x, he.y * sign, he.z), + Point::new(-he.x, he.y * sign, -he.z), + Point::new(he.x, he.y * sign, -he.z), + ], + 2 => [ + Point::new(he.x, he.y, he.z * sign), + Point::new(he.x, -he.y, he.z * sign), + Point::new(-he.x, -he.y, he.z * sign), + Point::new(-he.x, he.y, he.z * sign), + ], + _ => unreachable!(), + }; + + pub fn vid(i: u8) -> u8 { + // Each vertex has an even feature id. + i * 2 + } + + let sign_index = ((sign as i8 + 1) / 2) as usize; + // The vertex id as numbered depending on the sign of the vertex + // component. A + sign means the corresponding bit is 0 while a - + // sign means the corresponding bit is 1. + // For exampl the vertex [2.0, -1.0, -3.0] has the id 0b011 + let vids = match iamax { + 0 => [ + [vid(0b000), vid(0b010), vid(0b011), vid(0b001)], + [vid(0b100), vid(0b110), vid(0b111), vid(0b101)], + ][sign_index], + 1 => [ + [vid(0b000), vid(0b100), vid(0b101), vid(0b001)], + [vid(0b010), vid(0b110), vid(0b111), vid(0b011)], + ][sign_index], + 2 => [ + [vid(0b000), vid(0b010), vid(0b110), vid(0b100)], + [vid(0b001), vid(0b011), vid(0b111), vid(0b101)], + ][sign_index], + _ => unreachable!(), + }; + + // The feature ids of edges is obtained from the vertex ids + // of their endpoints. + // Assuming vid1 > vid2, we do: (vid1 << 3) | vid2 | 0b11000000 + // + let eids = match iamax { + 0 => [ + [0b11_010_000, 0b11_011_010, 0b11_011_001, 0b11_001_000], + [0b11_110_100, 0b11_111_110, 0b11_111_101, 0b11_101_100], + ][sign_index], + 1 => [ + [0b11_100_000, 0b11_101_100, 0b11_101_001, 0b11_001_000], + [0b11_110_010, 0b11_111_110, 0b11_111_011, 0b11_011_010], + ][sign_index], + 2 => [ + [0b11_010_000, 0b11_110_010, 0b11_110_100, 0b11_100_000], + [0b11_011_001, 0b11_111_011, 0b11_111_101, 0b11_101_001], + ][sign_index], + _ => unreachable!(), + }; + + // The face with normals [x, y, z] are numbered [10, 11, 12]. + // The face with negated normals are numbered [13, 14, 15]. + let fid = iamax + sign_index * 3 + 10; + + CuboidFeatureFace { + vertices, + vids, + eids, + fid: fid as u8, + } +} diff --git a/src/geometry/cuboid_feature2d.rs b/src/geometry/cuboid_feature2d.rs new file mode 100644 index 000000000..7e8001f20 --- /dev/null +++ b/src/geometry/cuboid_feature2d.rs @@ -0,0 +1,128 @@ +use crate::geometry::{self, Contact, ContactManifold}; +use crate::math::{Isometry, Point, Vector}; +use ncollide::shape::Segment; + +#[derive(Debug)] +#[allow(dead_code)] +pub enum CuboidFeature { + Face(CuboidFeatureFace), + Vertex(CuboidFeatureVertex), +} + +#[derive(Debug)] +pub struct CuboidFeatureVertex { + pub vertex: Point, + pub vid: u8, +} + +impl CuboidFeatureVertex { + pub fn transform_by(&mut self, iso: &Isometry) { + self.vertex = iso * self.vertex; + } +} + +#[derive(Debug)] +pub struct CuboidFeatureFace { + pub vertices: [Point; 2], + pub vids: [u8; 2], + pub fid: u8, +} + +impl From> for CuboidFeatureFace { + fn from(seg: Segment) -> Self { + CuboidFeatureFace { + vertices: [seg.a, seg.b], + vids: [0, 2], + fid: 1, + } + } +} + +impl CuboidFeatureFace { + pub fn transform_by(&mut self, iso: &Isometry) { + self.vertices[0] = iso * self.vertices[0]; + self.vertices[1] = iso * self.vertices[1]; + } +} + +impl CuboidFeature { + pub fn transform_by(&mut self, iso: &Isometry) { + match self { + CuboidFeature::Face(face) => face.transform_by(iso), + CuboidFeature::Vertex(vertex) => vertex.transform_by(iso), + } + } + + /// Compute contacts points between a face and a vertex. + /// + /// This method assume we already know that at least one contact exists. + pub fn face_vertex_contacts( + face1: &CuboidFeatureFace, + sep_axis1: &Vector, + vertex2: &CuboidFeatureVertex, + pos21: &Isometry, + manifold: &mut ContactManifold, + ) { + let tangent1 = face1.vertices[1] - face1.vertices[0]; + let normal1 = Vector::new(-tangent1.y, tangent1.x); + let denom = -normal1.dot(&sep_axis1); + let dist = (face1.vertices[0] - vertex2.vertex).dot(&normal1) / denom; + let local_p2 = vertex2.vertex; + let local_p1 = vertex2.vertex - dist * sep_axis1; + + manifold.points.push(Contact { + local_p1, + local_p2: pos21 * local_p2, + impulse: 0.0, + tangent_impulse: Contact::zero_tangent_impulse(), + fid1: face1.fid, + fid2: vertex2.vid, + dist, + }); + } + + pub fn face_face_contacts( + _prediction_distance: f32, + face1: &CuboidFeatureFace, + normal1: &Vector, + face2: &CuboidFeatureFace, + pos21: &Isometry, + manifold: &mut ContactManifold, + ) { + if let Some((clip_a, clip_b)) = geometry::clip_segments( + (face1.vertices[0], face1.vertices[1]), + (face2.vertices[0], face2.vertices[1]), + ) { + let fids1 = [face1.vids[0], face1.fid, face1.vids[1]]; + let fids2 = [face2.vids[0], face2.fid, face2.vids[1]]; + + let dist = (clip_a.1 - clip_a.0).dot(normal1); + if true { + // dist < prediction_distance { + manifold.points.push(Contact { + local_p1: clip_a.0, + local_p2: pos21 * clip_a.1, + impulse: 0.0, + tangent_impulse: Contact::zero_tangent_impulse(), + fid1: fids1[clip_a.2], + fid2: fids2[clip_a.3], + dist, + }); + } + + let dist = (clip_b.1 - clip_b.0).dot(normal1); + if true { + // dist < prediction_distance { + manifold.points.push(Contact { + local_p1: clip_b.0, + local_p2: pos21 * clip_b.1, + impulse: 0.0, + tangent_impulse: Contact::zero_tangent_impulse(), + fid1: fids1[clip_b.2], + fid2: fids2[clip_b.3], + dist, + }); + } + } + } +} diff --git a/src/geometry/cuboid_feature3d.rs b/src/geometry/cuboid_feature3d.rs new file mode 100644 index 000000000..3a4b675b3 --- /dev/null +++ b/src/geometry/cuboid_feature3d.rs @@ -0,0 +1,516 @@ +use crate::geometry::{Contact, ContactManifold}; +use crate::math::{Isometry, Point, Vector}; +use crate::utils::WBasis; +use na::Point2; + +#[derive(Debug)] +#[allow(dead_code)] +pub(crate) enum CuboidFeature { + Face(CuboidFeatureFace), + Edge(CuboidFeatureEdge), + Vertex(CuboidFeatureVertex), +} + +#[derive(Debug)] +pub(crate) struct CuboidFeatureVertex { + pub vertex: Point, + pub vid: u8, +} + +impl CuboidFeatureVertex { + pub fn transform_by(&mut self, iso: &Isometry) { + self.vertex = iso * self.vertex; + } +} + +#[derive(Debug)] +pub(crate) struct CuboidFeatureEdge { + pub vertices: [Point; 2], + pub vids: [u8; 2], + pub eid: u8, +} + +impl CuboidFeatureEdge { + pub fn transform_by(&mut self, iso: &Isometry) { + self.vertices[0] = iso * self.vertices[0]; + self.vertices[1] = iso * self.vertices[1]; + } +} + +#[derive(Debug)] +pub(crate) struct CuboidFeatureFace { + pub vertices: [Point; 4], + pub vids: [u8; 4], // Feature ID of the vertices. + pub eids: [u8; 4], // Feature ID of the edges. + pub fid: u8, // Feature ID of the face. +} + +impl CuboidFeatureFace { + pub fn transform_by(&mut self, iso: &Isometry) { + self.vertices[0] = iso * self.vertices[0]; + self.vertices[1] = iso * self.vertices[1]; + self.vertices[2] = iso * self.vertices[2]; + self.vertices[3] = iso * self.vertices[3]; + } +} + +impl CuboidFeature { + pub fn transform_by(&mut self, iso: &Isometry) { + match self { + CuboidFeature::Face(face) => face.transform_by(iso), + CuboidFeature::Edge(edge) => edge.transform_by(iso), + CuboidFeature::Vertex(vertex) => vertex.transform_by(iso), + } + } + + /// Compute contacts points between a face and a vertex. + /// + /// This method assume we already know that at least one contact exists. + pub fn face_vertex_contacts( + face1: &CuboidFeatureFace, + sep_axis1: &Vector, + vertex2: &CuboidFeatureVertex, + pos21: &Isometry, + manifold: &mut ContactManifold, + ) { + let normal1 = + (face1.vertices[0] - face1.vertices[1]).cross(&(face1.vertices[2] - face1.vertices[1])); + let denom = -normal1.dot(&sep_axis1); + let dist = (face1.vertices[0] - vertex2.vertex).dot(&normal1) / denom; + let local_p2 = vertex2.vertex; + let local_p1 = vertex2.vertex - dist * sep_axis1; + + manifold.points.push(Contact { + local_p1, + local_p2: pos21 * local_p2, + impulse: 0.0, + tangent_impulse: Contact::zero_tangent_impulse(), + fid1: face1.fid, + fid2: vertex2.vid, + dist, + }); + } + + /// Compute contacts points between a face and an edge. + /// + /// This method assume we already know that at least one contact exists. + pub fn face_edge_contacts( + prediction_distance: f32, + face1: &CuboidFeatureFace, + sep_axis1: &Vector, + edge2: &CuboidFeatureEdge, + pos21: &Isometry, + manifold: &mut ContactManifold, + flipped: bool, + ) { + // Project the faces to a 2D plane for contact clipping. + // The plane they are projected onto has normal sep_axis1 + // and contains the origin (this is numerically OK because + // we are not working in world-space here). + let basis = sep_axis1.orthonormal_basis(); + let projected_face1 = [ + Point2::new( + face1.vertices[0].coords.dot(&basis[0]), + face1.vertices[0].coords.dot(&basis[1]), + ), + Point2::new( + face1.vertices[1].coords.dot(&basis[0]), + face1.vertices[1].coords.dot(&basis[1]), + ), + Point2::new( + face1.vertices[2].coords.dot(&basis[0]), + face1.vertices[2].coords.dot(&basis[1]), + ), + Point2::new( + face1.vertices[3].coords.dot(&basis[0]), + face1.vertices[3].coords.dot(&basis[1]), + ), + ]; + let projected_edge2 = [ + Point2::new( + edge2.vertices[0].coords.dot(&basis[0]), + edge2.vertices[0].coords.dot(&basis[1]), + ), + Point2::new( + edge2.vertices[1].coords.dot(&basis[0]), + edge2.vertices[1].coords.dot(&basis[1]), + ), + ]; + + // Now we have to compute the intersection between all pairs of + // edges from the face 1 with the edge 2 + for i in 0..4 { + let projected_edge1 = [projected_face1[i], projected_face1[(i + 1) % 4]]; + + if let Some(bcoords) = closest_points_line2d(projected_edge1, projected_edge2) { + if bcoords.0 > 0.0 && bcoords.0 < 1.0 && bcoords.1 > 0.0 && bcoords.1 < 1.0 { + // Found a contact between the two edges. + let edge1 = [face1.vertices[i], face1.vertices[(i + 1) % 4]]; + let local_p1 = edge1[0] * (1.0 - bcoords.0) + edge1[1].coords * bcoords.0; + let local_p2 = edge2.vertices[0] * (1.0 - bcoords.1) + + edge2.vertices[1].coords * bcoords.1; + let dist = (local_p2 - local_p1).dot(&sep_axis1); + + if dist < prediction_distance { + if flipped { + manifold.points.push(Contact { + local_p1: local_p2, + // All points are expressed in the locale space of the first shape + // (even if there was a flip). So the point we need to transform by + // pos21 is the one that will go into local_p2. + local_p2: pos21 * local_p1, + impulse: 0.0, + tangent_impulse: Contact::zero_tangent_impulse(), + fid1: edge2.eid, + fid2: face1.eids[i], + dist, + }); + } else { + manifold.points.push(Contact { + local_p1, + local_p2: pos21 * local_p2, + impulse: 0.0, + tangent_impulse: Contact::zero_tangent_impulse(), + fid1: face1.eids[i], + fid2: edge2.eid, + dist, + }); + } + } + } + } + } + + // Project the two points from the edge into the face. + let normal1 = + (face1.vertices[2] - face1.vertices[1]).cross(&(face1.vertices[0] - face1.vertices[1])); + + 'point_loop2: for i in 0..2 { + let p2 = projected_edge2[i]; + + let sign = (projected_face1[0] - projected_face1[3]).perp(&(p2 - projected_face1[3])); + for j in 0..3 { + let new_sign = + (projected_face1[j + 1] - projected_face1[j]).perp(&(p2 - projected_face1[j])); + if new_sign * sign < 0.0 { + // The point lies outside. + continue 'point_loop2; + } + } + + // All the perp had the same sign: the point is inside of the other shapes projection. + // Output the contact. + let denom = -normal1.dot(&sep_axis1); + let dist = (face1.vertices[0] - edge2.vertices[i]).dot(&normal1) / denom; + let local_p2 = edge2.vertices[i]; + let local_p1 = edge2.vertices[i] - dist * sep_axis1; + + if dist < prediction_distance { + if flipped { + manifold.points.push(Contact { + local_p1: local_p2, + // All points are expressed in the locale space of the first shape + // (even if there was a flip). So the point we need to transform by + // pos21 is the one that will go into local_p2. + local_p2: pos21 * local_p1, + impulse: 0.0, + tangent_impulse: Contact::zero_tangent_impulse(), + fid1: edge2.vids[i], + fid2: face1.fid, + dist, + }); + } else { + manifold.points.push(Contact { + local_p1, + local_p2: pos21 * local_p2, + impulse: 0.0, + tangent_impulse: Contact::zero_tangent_impulse(), + fid1: face1.fid, + fid2: edge2.vids[i], + dist, + }); + } + } + } + } + + /// Compute contacts points between two edges. + /// + /// This method assume we already know that at least one contact exists. + pub fn edge_edge_contacts( + edge1: &CuboidFeatureEdge, + sep_axis1: &Vector, + edge2: &CuboidFeatureEdge, + pos21: &Isometry, + manifold: &mut ContactManifold, + ) { + let basis = sep_axis1.orthonormal_basis(); + let projected_edge1 = [ + Point2::new( + edge1.vertices[0].coords.dot(&basis[0]), + edge1.vertices[0].coords.dot(&basis[1]), + ), + Point2::new( + edge1.vertices[1].coords.dot(&basis[0]), + edge1.vertices[1].coords.dot(&basis[1]), + ), + ]; + let projected_edge2 = [ + Point2::new( + edge2.vertices[0].coords.dot(&basis[0]), + edge2.vertices[0].coords.dot(&basis[1]), + ), + Point2::new( + edge2.vertices[1].coords.dot(&basis[0]), + edge2.vertices[1].coords.dot(&basis[1]), + ), + ]; + + if let Some(bcoords) = closest_points_line2d(projected_edge1, projected_edge2) { + let local_p1 = + edge1.vertices[0] * (1.0 - bcoords.0) + edge1.vertices[1].coords * bcoords.0; + let local_p2 = + edge2.vertices[0] * (1.0 - bcoords.1) + edge2.vertices[1].coords * bcoords.1; + let dist = (local_p2 - local_p1).dot(&sep_axis1); + + manifold.points.push(Contact { + local_p1, + local_p2: pos21 * local_p2, // NOTE: local_p2 is expressed in the local space of cube1. + impulse: 0.0, + tangent_impulse: Contact::zero_tangent_impulse(), + fid1: edge1.eid, + fid2: edge2.eid, + dist, + }); + } + } + + pub fn face_face_contacts( + _prediction_distance: f32, + face1: &CuboidFeatureFace, + sep_axis1: &Vector, + face2: &CuboidFeatureFace, + pos21: &Isometry, + manifold: &mut ContactManifold, + ) { + // Project the faces to a 2D plane for contact clipping. + // The plane they are projected onto has normal sep_axis1 + // and contains the origin (this is numerically OK because + // we are not working in world-space here). + let basis = sep_axis1.orthonormal_basis(); + let projected_face1 = [ + Point2::new( + face1.vertices[0].coords.dot(&basis[0]), + face1.vertices[0].coords.dot(&basis[1]), + ), + Point2::new( + face1.vertices[1].coords.dot(&basis[0]), + face1.vertices[1].coords.dot(&basis[1]), + ), + Point2::new( + face1.vertices[2].coords.dot(&basis[0]), + face1.vertices[2].coords.dot(&basis[1]), + ), + Point2::new( + face1.vertices[3].coords.dot(&basis[0]), + face1.vertices[3].coords.dot(&basis[1]), + ), + ]; + let projected_face2 = [ + Point2::new( + face2.vertices[0].coords.dot(&basis[0]), + face2.vertices[0].coords.dot(&basis[1]), + ), + Point2::new( + face2.vertices[1].coords.dot(&basis[0]), + face2.vertices[1].coords.dot(&basis[1]), + ), + Point2::new( + face2.vertices[2].coords.dot(&basis[0]), + face2.vertices[2].coords.dot(&basis[1]), + ), + Point2::new( + face2.vertices[3].coords.dot(&basis[0]), + face2.vertices[3].coords.dot(&basis[1]), + ), + ]; + + // Also find all the vertices located inside of the other projected face. + let normal1 = + (face1.vertices[2] - face1.vertices[1]).cross(&(face1.vertices[0] - face1.vertices[1])); + let normal2 = + (face2.vertices[2] - face2.vertices[1]).cross(&(face2.vertices[0] - face2.vertices[1])); + + // NOTE: The loop iterating on all the vertices for face1 is different from + // the one iterating on all the vertices of face2. In the second loop, we + // classify every point wrt. every edge on the other face. This will give + // us bit masks to filter out several edge-edge intersections. + 'point_loop1: for i in 0..4 { + let p1 = projected_face1[i]; + + let sign = (projected_face2[0] - projected_face2[3]).perp(&(p1 - projected_face2[3])); + for j in 0..3 { + let new_sign = + (projected_face2[j + 1] - projected_face2[j]).perp(&(p1 - projected_face2[j])); + if new_sign * sign < 0.0 { + // The point lies outside. + continue 'point_loop1; + } + } + + // All the perp had the same sign: the point is inside of the other shapes projection. + // Output the contact. + let denom = normal2.dot(&sep_axis1); + let dist = (face2.vertices[0] - face1.vertices[i]).dot(&normal2) / denom; + let local_p1 = face1.vertices[i]; + let local_p2 = face1.vertices[i] + dist * sep_axis1; + + if true { + // dist <= prediction_distance { + manifold.points.push(Contact { + local_p1, + local_p2: pos21 * local_p2, + impulse: 0.0, + tangent_impulse: Contact::zero_tangent_impulse(), + fid1: face1.vids[i], + fid2: face2.fid, + dist, + }); + } + } + + let is_clockwise1 = (projected_face1[0] - projected_face1[1]) + .perp(&(projected_face1[2] - projected_face1[1])) + >= 0.0; + let mut vertex_class2 = [0u8; 4]; + + for i in 0..4 { + let p2 = projected_face2[i]; + + let sign = (projected_face1[0] - projected_face1[3]).perp(&(p2 - projected_face1[3])); + vertex_class2[i] |= ((sign >= 0.0) as u8) << 3; + + for j in 0..3 { + let sign = + (projected_face1[j + 1] - projected_face1[j]).perp(&(p2 - projected_face1[j])); + vertex_class2[i] |= ((sign >= 0.0) as u8) << j; + } + + if !is_clockwise1 { + vertex_class2[i] = (!vertex_class2[i]) & 0b01111; + } + + if vertex_class2[i] == 0 { + // All the perp had the same sign: the point is inside of the other shapes projection. + // Output the contact. + let denom = -normal1.dot(&sep_axis1); + let dist = (face1.vertices[0] - face2.vertices[i]).dot(&normal1) / denom; + let local_p2 = face2.vertices[i]; + let local_p1 = face2.vertices[i] - dist * sep_axis1; + + if true { + // dist < prediction_distance { + manifold.points.push(Contact { + local_p1, + local_p2: pos21 * local_p2, + impulse: 0.0, + tangent_impulse: Contact::zero_tangent_impulse(), + fid1: face1.fid, + fid2: face2.vids[i], + dist, + }); + } + } + } + + // Now we have to compute the intersection between all pairs of + // edges from the face 1 and from the face2. + for j in 0..4 { + let projected_edge2 = [projected_face2[j], projected_face2[(j + 1) % 4]]; + + if (vertex_class2[j] & vertex_class2[(j + 1) % 4]) != 0 { + continue; + } + + let edge_class2 = vertex_class2[j] | vertex_class2[(j + 1) % 4]; + + for i in 0..4 { + if (edge_class2 & (1 << i)) != 0 { + let projected_edge1 = [projected_face1[i], projected_face1[(i + 1) % 4]]; + if let Some(bcoords) = closest_points_line2d(projected_edge1, projected_edge2) { + if bcoords.0 > 0.0 && bcoords.0 < 1.0 && bcoords.1 > 0.0 && bcoords.1 < 1.0 + { + // Found a contact between the two edges. + let edge1 = (face1.vertices[i], face1.vertices[(i + 1) % 4]); + let edge2 = (face2.vertices[j], face2.vertices[(j + 1) % 4]); + let local_p1 = edge1.0 * (1.0 - bcoords.0) + edge1.1.coords * bcoords.0; + let local_p2 = edge2.0 * (1.0 - bcoords.1) + edge2.1.coords * bcoords.1; + let dist = (local_p2 - local_p1).dot(&sep_axis1); + + if true { + // dist <= prediction_distance { + manifold.points.push(Contact { + local_p1, + local_p2: pos21 * local_p2, + impulse: 0.0, + tangent_impulse: Contact::zero_tangent_impulse(), + fid1: face1.eids[i], + fid2: face2.eids[j], + dist, + }); + } + } + } + } + } + } + } +} + +/// Compute the barycentric coordinates of the intersection between the two given lines. +/// Returns `None` if the lines are parallel. +fn closest_points_line2d(edge1: [Point2; 2], edge2: [Point2; 2]) -> Option<(f32, f32)> { + use approx::AbsDiffEq; + + // Inspired by Real-time collision detection by Christer Ericson. + let dir1 = edge1[1] - edge1[0]; + let dir2 = edge2[1] - edge2[0]; + let r = edge1[0] - edge2[0]; + + let a = dir1.norm_squared(); + let e = dir2.norm_squared(); + let f = dir2.dot(&r); + + let eps = f32::default_epsilon(); + + if a <= eps && e <= eps { + Some((0.0, 0.0)) + } else if a <= eps { + Some((0.0, f / e)) + } else { + let c = dir1.dot(&r); + if e <= eps { + Some((-c / a, 0.0)) + } else { + let b = dir1.dot(&dir2); + let ae = a * e; + let bb = b * b; + let denom = ae - bb; + + // Use absolute and ulps error to test collinearity. + let parallel = denom <= eps || approx::ulps_eq!(ae, bb); + + let s = if !parallel { + (b * f - c * e) / denom + } else { + 0.0 + }; + + if parallel { + None + } else { + Some((s, (b * s + f) / e)) + } + } + } +} diff --git a/src/geometry/interaction_graph.rs b/src/geometry/interaction_graph.rs new file mode 100644 index 000000000..c0cf09346 --- /dev/null +++ b/src/geometry/interaction_graph.rs @@ -0,0 +1,259 @@ +use crate::data::graph::{Direction, EdgeIndex, Graph, NodeIndex}; +use crate::geometry::ColliderHandle; + +/// Index of a node of the interaction graph. +pub type ColliderGraphIndex = NodeIndex; +/// Index of a node of the interaction graph. +pub type RigidBodyGraphIndex = NodeIndex; +/// Temporary index to and edge of the interaction graph. +pub type TemporaryInteractionIndex = EdgeIndex; + +/// A graph where nodes are collision objects and edges are contact or proximity algorithms. +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct InteractionGraph { + pub(crate) graph: Graph, +} + +impl InteractionGraph { + /// Creates a new empty collection of collision objects. + pub fn new() -> Self { + InteractionGraph { + graph: Graph::with_capacity(10, 10), + } + } + + /// The underlying raw graph structure of this interaction graph. + pub fn raw_graph(&self) -> &Graph { + &self.graph + } + + pub(crate) fn invalid_graph_index() -> ColliderGraphIndex { + ColliderGraphIndex::new(crate::INVALID_U32) + } + + pub(crate) fn is_graph_index_valid(index: ColliderGraphIndex) -> bool { + index.index() != crate::INVALID_USIZE + } + + pub(crate) fn add_edge( + &mut self, + index1: ColliderGraphIndex, + index2: ColliderGraphIndex, + interaction: T, + ) -> TemporaryInteractionIndex { + self.graph.add_edge(index1, index2, interaction) + } + + pub(crate) fn remove_edge( + &mut self, + index1: ColliderGraphIndex, + index2: ColliderGraphIndex, + ) -> Option { + let id = self.graph.find_edge(index1, index2)?; + self.graph.remove_edge(id) + } + + /// Removes a handle from this graph and returns a handle that must have its graph index changed to `id`. + /// + /// When a node is removed, another node of the graph takes it place. This means that the `ColliderGraphIndex` + /// of the collision object returned by this method will be equal to `id`. Thus if you maintain + /// a map between `CollisionObjectSlabHandle` and `ColliderGraphIndex`, then you should update this + /// map to associate `id` to the handle returned by this method. For example: + /// + /// ```.ignore + /// // Let `id` be the graph index of the collision object we want to remove. + /// if let Some(other_handle) = graph.remove_node(id) { + /// // The graph index of `other_handle` changed to `id` due to the removal. + /// map.insert(other_handle, id) ; + /// } + /// ``` + #[must_use = "The graph index of the collision object returned by this method has been changed to `id`."] + pub(crate) fn remove_node(&mut self, id: ColliderGraphIndex) -> Option { + let _ = self.graph.remove_node(id); + self.graph.node_weight(id).cloned() + } + + /// All the interactions pairs on this graph. + pub fn interaction_pairs(&self) -> impl Iterator { + self.graph.raw_edges().iter().map(move |edge| { + ( + self.graph[edge.source()], + self.graph[edge.target()], + &edge.weight, + ) + }) + } + + /// The interaction between the two collision objects identified by their graph index. + pub fn interaction_pair( + &self, + id1: ColliderGraphIndex, + id2: ColliderGraphIndex, + ) -> Option<(ColliderHandle, ColliderHandle, &T)> { + self.graph.find_edge(id1, id2).and_then(|edge| { + let endpoints = self.graph.edge_endpoints(edge)?; + let h1 = self.graph.node_weight(endpoints.0)?; + let h2 = self.graph.node_weight(endpoints.1)?; + let weight = self.graph.edge_weight(edge)?; + Some((*h1, *h2, weight)) + }) + } + + /// The interaction between the two collision objects identified by their graph index. + pub fn interaction_pair_mut( + &mut self, + id1: ColliderGraphIndex, + id2: ColliderGraphIndex, + ) -> Option<(ColliderHandle, ColliderHandle, &mut T)> { + let edge = self.graph.find_edge(id1, id2)?; + let endpoints = self.graph.edge_endpoints(edge)?; + let h1 = *self.graph.node_weight(endpoints.0)?; + let h2 = *self.graph.node_weight(endpoints.1)?; + let weight = self.graph.edge_weight_mut(edge)?; + Some((h1, h2, weight)) + } + + /// All the interaction involving the collision object with graph index `id`. + pub fn interactions_with( + &self, + id: ColliderGraphIndex, + ) -> impl Iterator { + self.graph.edges(id).filter_map(move |e| { + let endpoints = self.graph.edge_endpoints(e.id()).unwrap(); + Some((self.graph[endpoints.0], self.graph[endpoints.1], e.weight())) + }) + } + + /// Gets the interaction with the given index. + pub fn index_interaction( + &self, + id: TemporaryInteractionIndex, + ) -> Option<(ColliderHandle, ColliderHandle, &T)> { + if let (Some(e), Some(endpoints)) = + (self.graph.edge_weight(id), self.graph.edge_endpoints(id)) + { + Some((self.graph[endpoints.0], self.graph[endpoints.1], e)) + } else { + None + } + } + + /// All the mutable references to interactions involving the collision object with graph index `id`. + pub fn interactions_with_mut( + &mut self, + id: ColliderGraphIndex, + ) -> impl Iterator< + Item = ( + ColliderHandle, + ColliderHandle, + TemporaryInteractionIndex, + &mut T, + ), + > { + let incoming_edge = self.graph.first_edge(id, Direction::Incoming); + let outgoing_edge = self.graph.first_edge(id, Direction::Outgoing); + + InteractionsWithMut { + graph: &mut self.graph, + incoming_edge, + outgoing_edge, + } + } + + // /// All the collision object handles of collision objects interacting with the collision object with graph index `id`. + // pub fn colliders_interacting_with<'a>( + // &'a self, + // id: ColliderGraphIndex, + // ) -> impl Iterator + 'a { + // self.graph.edges(id).filter_map(move |e| { + // let inter = e.weight(); + // + // if e.source() == id { + // Some(self.graph[e.target()]) + // } else { + // Some(self.graph[e.source()]) + // } + // }) + // } + + // /// All the collision object handles of collision objects in contact with the collision object with graph index `id`. + // pub fn colliders_in_contact_with<'a>( + // &'a self, + // id: ColliderGraphIndex, + // ) -> impl Iterator + 'a { + // self.graph.edges(id).filter_map(move |e| { + // let inter = e.weight(); + // + // if inter.is_contact() && Self::is_interaction_effective(inter) { + // if e.source() == id { + // Some(self.graph[e.target()]) + // } else { + // Some(self.graph[e.source()]) + // } + // } else { + // None + // } + // }) + // } + // + // /// All the collision object handles of collision objects in proximity of with the collision object with graph index `id`. + // /// for details. + // pub fn colliders_in_proximity_of<'a>( + // &'a self, + // id: ColliderGraphIndex, + // ) -> impl Iterator + 'a { + // self.graph.edges(id).filter_map(move |e| { + // if let Interaction::Proximity(_, prox) = e.weight() { + // if *prox == Proximity::Intersecting { + // if e.source() == id { + // return Some(self.graph[e.target()]); + // } else { + // return Some(self.graph[e.source()]); + // } + // } + // } + // + // None + // }) + // } +} + +pub struct InteractionsWithMut<'a, T> { + graph: &'a mut Graph, + incoming_edge: Option, + outgoing_edge: Option, +} + +impl<'a, T> Iterator for InteractionsWithMut<'a, T> { + type Item = ( + ColliderHandle, + ColliderHandle, + TemporaryInteractionIndex, + &'a mut T, + ); + + #[inline] + fn next( + &mut self, + ) -> Option<( + ColliderHandle, + ColliderHandle, + TemporaryInteractionIndex, + &'a mut T, + )> { + if let Some(edge) = self.incoming_edge { + self.incoming_edge = self.graph.next_edge(edge, Direction::Incoming); + let endpoints = self.graph.edge_endpoints(edge).unwrap(); + let (co1, co2) = (self.graph[endpoints.0], self.graph[endpoints.1]); + let interaction = &mut self.graph[edge]; + return Some((co1, co2, edge, unsafe { std::mem::transmute(interaction) })); + } + + let edge = self.outgoing_edge?; + self.outgoing_edge = self.graph.next_edge(edge, Direction::Outgoing); + let endpoints = self.graph.edge_endpoints(edge).unwrap(); + let (co1, co2) = (self.graph[endpoints.0], self.graph[endpoints.1]); + let interaction = &mut self.graph[edge]; + Some((co1, co2, edge, unsafe { std::mem::transmute(interaction) })) + } +} diff --git a/src/geometry/mod.rs b/src/geometry/mod.rs new file mode 100644 index 000000000..4f72778fb --- /dev/null +++ b/src/geometry/mod.rs @@ -0,0 +1,80 @@ +//! Structures related to geometry: colliders, shapes, etc. + +pub use self::broad_phase_multi_sap::BroadPhase; +pub use self::capsule::Capsule; +pub use self::collider::{Collider, ColliderBuilder, Shape}; +pub use self::collider_set::{ColliderHandle, ColliderSet}; +pub use self::contact::{ + Contact, ContactKinematics, ContactManifold, ContactPair, KinematicsCategory, +}; +pub use self::contact_generator::{ContactDispatcher, DefaultContactDispatcher}; +#[cfg(feature = "dim2")] +pub(crate) use self::cuboid_feature2d::{CuboidFeature, CuboidFeatureFace}; +#[cfg(feature = "dim3")] +pub(crate) use self::cuboid_feature3d::{CuboidFeature, CuboidFeatureFace}; +pub use self::interaction_graph::{ + ColliderGraphIndex, InteractionGraph, RigidBodyGraphIndex, TemporaryInteractionIndex, +}; +pub use self::narrow_phase::NarrowPhase; +pub use self::polygon::Polygon; +pub use self::proximity::ProximityPair; +pub use self::proximity_detector::{DefaultProximityDispatcher, ProximityDispatcher}; +pub use self::trimesh::Trimesh; +pub use ncollide::query::Proximity; + +/// A cuboid shape. +pub type Cuboid = ncollide::shape::Cuboid; +/// A triangle shape. +pub type Triangle = ncollide::shape::Triangle; +/// A ball shape. +pub type Ball = ncollide::shape::Ball; +/// A heightfield shape. +pub type HeightField = ncollide::shape::HeightField; +/// An axis-aligned bounding box. +pub type AABB = ncollide::bounding_volume::AABB; +/// Event triggered when two non-sensor colliders start or stop being in contact. +pub type ContactEvent = ncollide::pipeline::ContactEvent; +/// Event triggered when a sensor collider starts or stop being in proximity with another collider (sensor or not). +pub type ProximityEvent = ncollide::pipeline::ProximityEvent; + +#[cfg(feature = "simd-is-enabled")] +pub(crate) use self::ball::WBall; +pub(crate) use self::broad_phase::{ColliderPair, WAABBHierarchy, WAABBHierarchyIntersections}; +pub(crate) use self::broad_phase_multi_sap::BroadPhasePairEvent; +#[cfg(feature = "simd-is-enabled")] +pub(crate) use self::contact::WContact; +#[cfg(feature = "dim2")] +pub(crate) use self::contact_generator::{clip_segments, clip_segments_with_normal}; +pub(crate) use self::narrow_phase::ContactManifoldIndex; +#[cfg(feature = "dim3")] +pub(crate) use self::polyhedron_feature3d::PolyhedronFace; +#[cfg(feature = "simd-is-enabled")] +pub(crate) use self::waabb::WAABB; +//pub(crate) use self::z_order::z_cmp_floats; + +mod ball; +mod broad_phase; +mod broad_phase_multi_sap; +mod capsule; +mod collider; +mod collider_set; +mod contact; +mod contact_generator; +pub(crate) mod cuboid; +#[cfg(feature = "dim2")] +mod cuboid_feature2d; +#[cfg(feature = "dim3")] +mod cuboid_feature3d; +mod interaction_graph; +mod narrow_phase; +mod polygon; +#[cfg(feature = "dim3")] +mod polyhedron_feature3d; +mod proximity; +mod proximity_detector; +pub(crate) mod sat; +pub(crate) mod triangle; +mod trimesh; +#[cfg(feature = "simd-is-enabled")] +mod waabb; +//mod z_order; diff --git a/src/geometry/narrow_phase.rs b/src/geometry/narrow_phase.rs new file mode 100644 index 000000000..3eb2c308a --- /dev/null +++ b/src/geometry/narrow_phase.rs @@ -0,0 +1,483 @@ +#[cfg(feature = "parallel")] +use rayon::prelude::*; + +use crate::dynamics::RigidBodySet; +use crate::geometry::contact_generator::{ + ContactDispatcher, ContactGenerationContext, DefaultContactDispatcher, +}; +use crate::geometry::proximity_detector::{ + DefaultProximityDispatcher, ProximityDetectionContext, ProximityDispatcher, +}; +//#[cfg(feature = "simd-is-enabled")] +//use crate::geometry::{ +// contact_generator::ContactGenerationContextSimd, +// proximity_detector::ProximityDetectionContextSimd, WBall, +//}; +use crate::geometry::{ + BroadPhasePairEvent, ColliderHandle, ContactEvent, ProximityEvent, ProximityPair, +}; +use crate::geometry::{ColliderSet, ContactManifold, ContactPair, InteractionGraph}; +//#[cfg(feature = "simd-is-enabled")] +//use crate::math::{SimdFloat, SIMD_WIDTH}; +use crate::ncollide::query::Proximity; +use crate::pipeline::EventHandler; +//use simba::simd::SimdValue; + +/// The narrow-phase responsible for computing precise contact information between colliders. +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct NarrowPhase { + contact_graph: InteractionGraph, + proximity_graph: InteractionGraph, + // ball_ball: Vec, // Workspace: Vec<*mut ContactPair>, + // shape_shape: Vec, // Workspace: Vec<*mut ContactPair>, + // ball_ball_prox: Vec, // Workspace: Vec<*mut ProximityPair>, + // shape_shape_prox: Vec, // Workspace: Vec<*mut ProximityPair>, +} + +pub(crate) type ContactManifoldIndex = usize; + +impl NarrowPhase { + /// Creates a new empty narrow-phase. + pub fn new() -> Self { + Self { + contact_graph: InteractionGraph::new(), + proximity_graph: InteractionGraph::new(), + // ball_ball: Vec::new(), + // shape_shape: Vec::new(), + // ball_ball_prox: Vec::new(), + // shape_shape_prox: Vec::new(), + } + } + + /// The contact graph containing all contact pairs and their contact information. + pub fn contact_graph(&self) -> &InteractionGraph { + &self.contact_graph + } + + /// The proximity graph containing all proximity pairs and their proximity information. + pub fn proximity_graph(&self) -> &InteractionGraph { + &self.proximity_graph + } + + // #[cfg(feature = "parallel")] + // pub fn contact_pairs(&self) -> &[ContactPair] { + // &self.contact_graph.interactions + // } + + // pub fn contact_pairs_mut(&mut self) -> &mut [ContactPair] { + // &mut self.contact_graph.interactions + // } + + // #[cfg(feature = "parallel")] + // pub(crate) fn contact_pairs_vec_mut(&mut self) -> &mut Vec { + // &mut self.contact_graph.interactions + // } + + pub(crate) fn remove_colliders( + &mut self, + handles: &[ColliderHandle], + colliders: &mut ColliderSet, + bodies: &mut RigidBodySet, + ) { + for handle in handles { + if let Some(collider) = colliders.get(*handle) { + let proximity_graph_id = collider.proximity_graph_index; + let contact_graph_id = collider.contact_graph_index; + + // Wake up every body in contact with the deleted collider. + for (a, b, _) in self.contact_graph.interactions_with(contact_graph_id) { + if let Some(parent) = colliders.get(a).map(|c| c.parent) { + bodies.wake_up(parent) + } + + if let Some(parent) = colliders.get(b).map(|c| c.parent) { + bodies.wake_up(parent) + } + } + + // We have to manage the fact that one other collider will + // hive its graph index changed because of the node's swap-remove. + if let Some(replacement) = self + .proximity_graph + .remove_node(proximity_graph_id) + .and_then(|h| colliders.get_mut(h)) + { + replacement.proximity_graph_index = proximity_graph_id; + } + + if let Some(replacement) = self + .contact_graph + .remove_node(contact_graph_id) + .and_then(|h| colliders.get_mut(h)) + { + replacement.contact_graph_index = contact_graph_id; + } + } + } + } + + pub(crate) fn register_pairs( + &mut self, + colliders: &mut ColliderSet, + broad_phase_events: &[BroadPhasePairEvent], + events: &dyn EventHandler, + ) { + for event in broad_phase_events { + match event { + BroadPhasePairEvent::AddPair(pair) => { + // println!("Adding pair: {:?}", *pair); + if let (Some(co1), Some(co2)) = + colliders.get2_mut_internal(pair.collider1, pair.collider2) + { + if co1.is_sensor() || co2.is_sensor() { + let gid1 = co1.proximity_graph_index; + let gid2 = co2.proximity_graph_index; + + // NOTE: the collider won't have a graph index as long + // as it does not interact with anything. + if !InteractionGraph::::is_graph_index_valid(gid1) { + co1.proximity_graph_index = + self.proximity_graph.graph.add_node(pair.collider1); + } + + if !InteractionGraph::::is_graph_index_valid(gid2) { + co2.proximity_graph_index = + self.proximity_graph.graph.add_node(pair.collider2); + } + + if self.proximity_graph.graph.find_edge(gid1, gid2).is_none() { + let dispatcher = DefaultProximityDispatcher; + let generator = dispatcher.dispatch(co1.shape(), co2.shape()); + let interaction = + ProximityPair::new(*pair, generator.0, generator.1); + let _ = self.proximity_graph.add_edge( + co1.proximity_graph_index, + co2.proximity_graph_index, + interaction, + ); + } + } else { + // NOTE: same code as above, but for the contact graph. + // TODO: refactor both pieces of code somehow? + let gid1 = co1.contact_graph_index; + let gid2 = co2.contact_graph_index; + + // NOTE: the collider won't have a graph index as long + // as it does not interact with anything. + if !InteractionGraph::::is_graph_index_valid(gid1) { + co1.contact_graph_index = + self.contact_graph.graph.add_node(pair.collider1); + } + + if !InteractionGraph::::is_graph_index_valid(gid2) { + co2.contact_graph_index = + self.contact_graph.graph.add_node(pair.collider2); + } + + if self.contact_graph.graph.find_edge(gid1, gid2).is_none() { + let dispatcher = DefaultContactDispatcher; + let generator = dispatcher.dispatch(co1.shape(), co2.shape()); + let interaction = ContactPair::new(*pair, generator.0, generator.1); + let _ = self.contact_graph.add_edge( + co1.contact_graph_index, + co2.contact_graph_index, + interaction, + ); + } + } + } + } + BroadPhasePairEvent::DeletePair(pair) => { + if let (Some(co1), Some(co2)) = + colliders.get2_mut_internal(pair.collider1, pair.collider2) + { + if co1.is_sensor() || co2.is_sensor() { + let prox_pair = self + .proximity_graph + .remove_edge(co1.proximity_graph_index, co2.proximity_graph_index); + + // Emit a proximity lost event if we had a proximity before removing the edge. + if let Some(prox) = prox_pair { + if prox.proximity != Proximity::Disjoint { + let prox_event = ProximityEvent::new( + pair.collider1, + pair.collider2, + prox.proximity, + Proximity::Disjoint, + ); + events.handle_proximity_event(prox_event) + } + } + } else { + let contact_pair = self + .contact_graph + .remove_edge(co1.contact_graph_index, co2.contact_graph_index); + + // Emit a contact stopped event if we had a proximity before removing the edge. + if let Some(ctct) = contact_pair { + if ctct.has_any_active_contact() { + events.handle_contact_event(ContactEvent::Stopped( + pair.collider1, + pair.collider2, + )) + } + } + } + } + } + } + } + } + + pub(crate) fn compute_proximities( + &mut self, + prediction_distance: f32, + bodies: &RigidBodySet, + colliders: &ColliderSet, + events: &dyn EventHandler, + ) { + par_iter_mut!(&mut self.proximity_graph.graph.edges).for_each(|edge| { + let pair = &mut edge.weight; + let co1 = &colliders[pair.pair.collider1]; + let co2 = &colliders[pair.pair.collider2]; + + // FIXME: avoid lookup into bodies. + let rb1 = &bodies[co1.parent]; + let rb2 = &bodies[co2.parent]; + + if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic()) + { + // No need to update this contact because nothing moved. + return; + } + + let dispatcher = DefaultProximityDispatcher; + if pair.detector.is_none() { + // We need a redispatch for this detector. + // This can happen, e.g., after restoring a snapshot of the narrow-phase. + let (detector, workspace) = dispatcher.dispatch(co1.shape(), co2.shape()); + pair.detector = Some(detector); + pair.detector_workspace = workspace; + } + + let context = ProximityDetectionContext { + dispatcher: &dispatcher, + prediction_distance, + colliders, + pair, + }; + + context + .pair + .detector + .unwrap() + .detect_proximity(context, events); + }); + + /* + // First, group pairs. + // NOTE: the transmutes here are OK because the Vec are all cleared + // before we leave this method. + // We do this in order to avoid reallocating those vecs each time + // we compute the contacts. Unsafe is necessary because we can't just + // store a Vec<&mut ProximityPair> into the NarrowPhase struct without + // polluting the World with lifetimes. + let ball_ball_prox: &mut Vec<&mut ProximityPair> = + unsafe { std::mem::transmute(&mut self.ball_ball_prox) }; + let shape_shape_prox: &mut Vec<&mut ProximityPair> = + unsafe { std::mem::transmute(&mut self.shape_shape_prox) }; + + let bodies = &bodies.bodies; + + // FIXME: don't iterate through all the interactions. + for pair in &mut self.proximity_graph.interactions { + let co1 = &colliders[pair.pair.collider1]; + let co2 = &colliders[pair.pair.collider2]; + + // FIXME: avoid lookup into bodies. + let rb1 = &bodies[co1.parent]; + let rb2 = &bodies[co2.parent]; + + if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic()) + { + // No need to update this proximity because nothing moved. + continue; + } + + match (co1.shape(), co2.shape()) { + (Shape::Ball(_), Shape::Ball(_)) => ball_ball_prox.push(pair), + _ => shape_shape_prox.push(pair), + } + } + + par_chunks_mut!(ball_ball_prox, SIMD_WIDTH).for_each(|pairs| { + let context = ProximityDetectionContextSimd { + dispatcher: &DefaultProximityDispatcher, + prediction_distance, + colliders, + pairs, + }; + context.pairs[0] + .detector + .detect_proximity_simd(context, events); + }); + + par_iter_mut!(shape_shape_prox).for_each(|pair| { + let context = ProximityDetectionContext { + dispatcher: &DefaultProximityDispatcher, + prediction_distance, + colliders, + pair, + }; + + context.pair.detector.detect_proximity(context, events); + }); + + ball_ball_prox.clear(); + shape_shape_prox.clear(); + */ + } + + pub(crate) fn compute_contacts( + &mut self, + prediction_distance: f32, + bodies: &RigidBodySet, + colliders: &ColliderSet, + events: &dyn EventHandler, + ) { + par_iter_mut!(&mut self.contact_graph.graph.edges).for_each(|edge| { + let pair = &mut edge.weight; + let co1 = &colliders[pair.pair.collider1]; + let co2 = &colliders[pair.pair.collider2]; + + // FIXME: avoid lookup into bodies. + let rb1 = &bodies[co1.parent]; + let rb2 = &bodies[co2.parent]; + + if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic()) + { + // No need to update this contact because nothing moved. + return; + } + + let dispatcher = DefaultContactDispatcher; + if pair.generator.is_none() { + // We need a redispatch for this generator. + // This can happen, e.g., after restoring a snapshot of the narrow-phase. + let (generator, workspace) = dispatcher.dispatch(co1.shape(), co2.shape()); + pair.generator = Some(generator); + pair.generator_workspace = workspace; + } + + let context = ContactGenerationContext { + dispatcher: &dispatcher, + prediction_distance, + colliders, + pair, + }; + + context + .pair + .generator + .unwrap() + .generate_contacts(context, events); + }); + + /* + // First, group pairs. + // NOTE: the transmutes here are OK because the Vec are all cleared + // before we leave this method. + // We do this in order to avoid reallocating those vecs each time + // we compute the contacts. Unsafe is necessary because we can't just + // store a Vec<&mut ContactPair> into the NarrowPhase struct without + // polluting the World with lifetimes. + let ball_ball: &mut Vec<&mut ContactPair> = + unsafe { std::mem::transmute(&mut self.ball_ball) }; + let shape_shape: &mut Vec<&mut ContactPair> = + unsafe { std::mem::transmute(&mut self.shape_shape) }; + + let bodies = &bodies.bodies; + + // FIXME: don't iterate through all the interactions. + for pair in &mut self.contact_graph.interactions { + let co1 = &colliders[pair.pair.collider1]; + let co2 = &colliders[pair.pair.collider2]; + + // FIXME: avoid lookup into bodies. + let rb1 = &bodies[co1.parent]; + let rb2 = &bodies[co2.parent]; + + if (rb1.is_sleeping() || !rb1.is_dynamic()) && (rb2.is_sleeping() || !rb2.is_dynamic()) + { + // No need to update this contact because nothing moved. + continue; + } + + match (co1.shape(), co2.shape()) { + (Shape::Ball(_), Shape::Ball(_)) => ball_ball.push(pair), + _ => shape_shape.push(pair), + } + } + + par_chunks_mut!(ball_ball, SIMD_WIDTH).for_each(|pairs| { + let context = ContactGenerationContextSimd { + dispatcher: &DefaultContactDispatcher, + prediction_distance, + colliders, + pairs, + }; + context.pairs[0] + .generator + .generate_contacts_simd(context, events); + }); + + par_iter_mut!(shape_shape).for_each(|pair| { + let context = ContactGenerationContext { + dispatcher: &DefaultContactDispatcher, + prediction_distance, + colliders, + pair, + }; + + context.pair.generator.generate_contacts(context, events); + }); + + ball_ball.clear(); + shape_shape.clear(); + */ + } + + /// Retrieve all the interactions with at least one contact point, happening between two active bodies. + // NOTE: this is very similar to the code from JointSet::select_active_interactions. + pub(crate) fn sort_and_select_active_contacts<'a>( + &'a mut self, + bodies: &RigidBodySet, + out_manifolds: &mut Vec<&'a mut ContactManifold>, + out: &mut Vec>, + ) { + for out_island in &mut out[..bodies.num_islands()] { + out_island.clear(); + } + + // FIXME: don't iterate through all the interactions. + for inter in self.contact_graph.graph.edges.iter_mut() { + for manifold in &mut inter.weight.manifolds { + let rb1 = &bodies[manifold.body_pair.body1]; + let rb2 = &bodies[manifold.body_pair.body2]; + if manifold.num_active_contacts() != 0 + && (!rb1.is_dynamic() || !rb1.is_sleeping()) + && (!rb2.is_dynamic() || !rb2.is_sleeping()) + { + let island_index = if !rb1.is_dynamic() { + rb2.active_island_id + } else { + rb1.active_island_id + }; + + out[island_index].push(out_manifolds.len()); + out_manifolds.push(manifold); + } + } + } + } +} diff --git a/src/geometry/polygon.rs b/src/geometry/polygon.rs new file mode 100644 index 000000000..cdb101240 --- /dev/null +++ b/src/geometry/polygon.rs @@ -0,0 +1,76 @@ +use crate::math::{Isometry, Point, Vector}; +use ncollide::bounding_volume::AABB; + +#[derive(Clone)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// A convex planar polygon. +pub struct Polygon { + pub(crate) vertices: Vec>, + pub(crate) normals: Vec>, +} + +impl Polygon { + /// Builds a new polygon from a set of vertices and normals. + /// + /// The vertices must be ordered in such a way that two consecutive + /// vertices determines an edge of the polygon. For example `vertices[0], vertices[1]` + /// is an edge, `vertices[1], vertices[2]` is the next edge, etc. The last edge will + /// be `vertices[vertices.len() - 1], vertices[0]`. + /// The vertices must be given in counter-clockwise order. + /// The vertices must form a convex polygon. + /// + /// One normal must be provided per edge and mut point towards the outside of the polygon. + pub fn new(vertices: Vec>, normals: Vec>) -> Self { + Self { vertices, normals } + } + + /// Compute the axis-aligned bounding box of the polygon. + pub fn aabb(&self, pos: &Isometry) -> AABB { + let p0 = pos * self.vertices[0]; + let mut mins = p0; + let mut maxs = p0; + + for pt in &self.vertices[1..] { + let pt = pos * pt; + mins = mins.inf(&pt); + maxs = maxs.sup(&pt); + } + + AABB::new(mins.into(), maxs.into()) + } + + /// The vertices of this polygon. + pub fn vertices(&self) -> &[Point] { + &self.vertices + } + + pub(crate) fn support_point(&self, dir: &Vector) -> usize { + let mut best_dot = -f32::MAX; + let mut best_i = 0; + + for (i, pt) in self.vertices.iter().enumerate() { + let dot = pt.coords.dot(&dir); + if dot > best_dot { + best_dot = dot; + best_i = i; + } + } + + best_i + } + + pub(crate) fn support_face(&self, dir: &Vector) -> usize { + let mut max_dot = -f32::MAX; + let mut max_dot_i = 0; + + for (i, normal) in self.normals.iter().enumerate() { + let dot = normal.dot(dir); + if dot > max_dot { + max_dot = dot; + max_dot_i = i; + } + } + + max_dot_i + } +} diff --git a/src/geometry/polygon_intersection.rs b/src/geometry/polygon_intersection.rs new file mode 100644 index 000000000..a2186dfcb --- /dev/null +++ b/src/geometry/polygon_intersection.rs @@ -0,0 +1,263 @@ +use na::{Point2, Real}; + +use shape::SegmentPointLocation; +use utils::{self, SegmentsIntersection, TriangleOrientation}; + +#[derive(Copy, Clone, Debug, PartialEq, Eq)] +enum InFlag { + PIn, + QIn, + Unknown, +} + +/// Location of a point on a polyline. +pub enum PolylinePointLocation { + /// Point on a vertex. + OnVertex(usize), + /// Point on an edge. + OnEdge(usize, usize, [N; 2]), +} + +impl PolylinePointLocation { + /// Computes the point corresponding to this location. + pub fn to_point(&self, pts: &[Point2]) -> Point2 { + match self { + PolylinePointLocation::OnVertex(i) => pts[*i], + PolylinePointLocation::OnEdge(i1, i2, bcoords) => { + pts[*i1] * bcoords[0] + pts[*i2].coords * bcoords[1] + } + } + } + + fn from_segment_point_location(a: usize, b: usize, loc: SegmentPointLocation) -> Self { + match loc { + SegmentPointLocation::OnVertex(0) => PolylinePointLocation::OnVertex(a), + SegmentPointLocation::OnVertex(1) => PolylinePointLocation::OnVertex(b), + SegmentPointLocation::OnVertex(_) => unreachable!(), + SegmentPointLocation::OnEdge(bcoords) => PolylinePointLocation::OnEdge(a, b, bcoords), + } + } +} + +/// Computes the intersection points of two convex polygons. +/// +/// The resulting polygon is output vertex-by-vertex to the `out` closure. +pub fn convex_polygons_intersection_points( + poly1: &[Point2], + poly2: &[Point2], + out: &mut Vec>, +) { + convex_polygons_intersection(poly1, poly2, |loc1, loc2| { + if let Some(loc1) = loc1 { + out.push(loc1.to_point(poly1)) + } else if let Some(loc2) = loc2 { + out.push(loc2.to_point(poly2)) + } + }) +} + +/// Computes the intersection of two convex polygons. +/// +/// The resulting polygon is output vertex-by-vertex to the `out` closure. +pub fn convex_polygons_intersection( + poly1: &[Point2], + poly2: &[Point2], + mut out: impl FnMut(Option>, Option>), +) { + // FIXME: this does not handle correctly the case where the + // first triangle of the polygon is degenerate. + let rev1 = poly1.len() > 2 + && utils::triangle_orientation(&poly1[0], &poly1[1], &poly1[2]) + == TriangleOrientation::Clockwise; + let rev2 = poly2.len() > 2 + && utils::triangle_orientation(&poly2[0], &poly2[1], &poly2[2]) + == TriangleOrientation::Clockwise; + + // println!("rev1: {}, rev2: {}", rev1, rev2); + + let n = poly1.len(); + let m = poly2.len(); + + let mut a = 0; + let mut b = 0; + let mut aa = 0; + let mut ba = 0; + let mut inflag = InFlag::Unknown; + let mut first_point_found = false; + + // Quit when both adv. indices have cycled, or one has cycled twice. + while (aa < n || ba < m) && aa < 2 * n && ba < 2 * m { + let (a1, a2) = if rev1 { + ((n - a) % n, n - a - 1) + } else { + ((a + n - 1) % n, a) + }; + + let (b1, b2) = if rev2 { + ((m - b) % m, m - b - 1) + } else { + ((b + m - 1) % m, b) + }; + + // println!("Current indices: ({}, {}), ({}, {})", a1, a2, b1, b2); + + let dir_edge1 = poly1[a2] - poly1[a1]; + let dir_edge2 = poly2[b2] - poly2[b1]; + + let cross = utils::triangle_orientation( + &Point2::origin(), + &Point2::from_coordinates(dir_edge1), + &Point2::from_coordinates(dir_edge2), + ); + let aHB = utils::triangle_orientation(&poly2[b1], &poly2[b2], &poly1[a2]); + let bHA = utils::triangle_orientation(&poly1[a1], &poly1[a2], &poly2[b2]); + + // If edge1 & edge2 intersect, update inflag. + if let Some(inter) = + utils::segments_intersection(&poly1[a1], &poly1[a2], &poly2[b1], &poly2[b2]) + { + match inter { + SegmentsIntersection::Point { loc1, loc2 } => { + let loc1 = PolylinePointLocation::from_segment_point_location(a1, a2, loc1); + let loc2 = PolylinePointLocation::from_segment_point_location(b1, b2, loc2); + out(Some(loc1), Some(loc2)); + + if inflag == InFlag::Unknown && !first_point_found { + // This is the first point. + aa = 0; + ba = 0; + first_point_found = true; + } + + // Update inflag. + if aHB == TriangleOrientation::Counterclockwise { + inflag = InFlag::PIn; + } else if bHA == TriangleOrientation::Counterclockwise { + inflag = InFlag::QIn; + } + } + SegmentsIntersection::Segment { + first_loc1, + first_loc2, + second_loc1, + second_loc2, + } => { + // Special case: edge1 & edge2 overlap and oppositely oriented. + if dir_edge1.dot(&dir_edge2) < N::zero() { + let loc1 = + PolylinePointLocation::from_segment_point_location(a1, a2, first_loc1); + let loc2 = + PolylinePointLocation::from_segment_point_location(b1, b2, first_loc2); + out(Some(loc1), Some(loc2)); + + let loc1 = + PolylinePointLocation::from_segment_point_location(a1, a2, second_loc1); + let loc2 = + PolylinePointLocation::from_segment_point_location(b1, b2, second_loc2); + out(Some(loc1), Some(loc2)); + + return; + } + } + } + } + + // Special case: edge1 & edge2 parallel and separated. + if cross == TriangleOrientation::Degenerate + && aHB == TriangleOrientation::Clockwise + && bHA == TriangleOrientation::Clockwise + { + return; + } + // Special case: edge1 & edge2 collinear. + else if cross == TriangleOrientation::Degenerate + && aHB == TriangleOrientation::Degenerate + && bHA == TriangleOrientation::Degenerate + { + // Advance but do not output point. + if inflag == InFlag::PIn { + b = advance(b, &mut ba, m); + } else { + a = advance(a, &mut aa, n); + } + } + // Generic cases. + else if cross == TriangleOrientation::Counterclockwise { + if bHA == TriangleOrientation::Counterclockwise { + if inflag == InFlag::PIn { + out(Some(PolylinePointLocation::OnVertex(a2)), None) + } + a = advance(a, &mut aa, n); + } else { + if inflag == InFlag::QIn { + out(None, Some(PolylinePointLocation::OnVertex(b2))) + } + b = advance(b, &mut ba, m); + } + } else { + // We have cross == TriangleOrientation::Clockwise. + if aHB == TriangleOrientation::Counterclockwise { + if inflag == InFlag::QIn { + out(None, Some(PolylinePointLocation::OnVertex(b2))) + } + b = advance(b, &mut ba, m); + } else { + if inflag == InFlag::PIn { + out(Some(PolylinePointLocation::OnVertex(a2)), None) + } + a = advance(a, &mut aa, n); + } + } + } + + if !first_point_found { + // No intersection: test if one polygon completely encloses the other. + let mut orient = TriangleOrientation::Degenerate; + let mut ok = true; + + for a in 0..n { + let a1 = (a + n - 1) % n; // a - 1 + let new_orient = utils::triangle_orientation(&poly1[a1], &poly1[a], &poly2[0]); + + if orient == TriangleOrientation::Degenerate { + orient = new_orient + } else if new_orient != orient && new_orient != TriangleOrientation::Degenerate { + ok = false; + break; + } + } + + if ok { + for b in 0..m { + out(None, Some(PolylinePointLocation::OnVertex(b))) + } + } + + let mut orient = TriangleOrientation::Degenerate; + let mut ok = true; + + for b in 0..m { + let b1 = (b + m - 1) % m; // b - 1 + let new_orient = utils::triangle_orientation(&poly2[b1], &poly2[b], &poly1[0]); + + if orient == TriangleOrientation::Degenerate { + orient = new_orient + } else if new_orient != orient && new_orient != TriangleOrientation::Degenerate { + ok = false; + break; + } + } + + if ok { + for a in 0..n { + out(Some(PolylinePointLocation::OnVertex(a)), None) + } + } + } +} + +#[inline] +fn advance(a: usize, aa: &mut usize, n: usize) -> usize { + *aa += 1; + (a + 1) % n +} diff --git a/src/geometry/polyhedron_feature3d.rs b/src/geometry/polyhedron_feature3d.rs new file mode 100644 index 000000000..94870a47d --- /dev/null +++ b/src/geometry/polyhedron_feature3d.rs @@ -0,0 +1,284 @@ +use crate::geometry::{Contact, ContactManifold, CuboidFeatureFace, Triangle}; +use crate::math::{Isometry, Point, Vector}; +use crate::utils::WBasis; +use na::Point2; +use ncollide::shape::Segment; + +#[derive(Debug)] +pub struct PolyhedronFace { + pub vertices: [Point; 4], + pub vids: [u8; 4], // Feature ID of the vertices. + pub eids: [u8; 4], // Feature ID of the edges. + pub fid: u8, // Feature ID of the face. + pub num_vertices: usize, +} + +impl From for PolyhedronFace { + fn from(face: CuboidFeatureFace) -> Self { + Self { + vertices: face.vertices, + vids: face.vids, + eids: face.eids, + fid: face.fid, + num_vertices: 4, + } + } +} + +impl From for PolyhedronFace { + fn from(tri: Triangle) -> Self { + Self { + vertices: [tri.a, tri.b, tri.c, tri.c], + vids: [0, 2, 4, 4], + eids: [1, 3, 5, 5], + fid: 0, + num_vertices: 3, + } + } +} + +impl From> for PolyhedronFace { + fn from(seg: Segment) -> Self { + Self { + vertices: [seg.a, seg.b, seg.b, seg.b], + vids: [0, 2, 2, 2], + eids: [1, 1, 1, 1], + fid: 0, + num_vertices: 2, + } + } +} + +impl PolyhedronFace { + pub fn transform_by(&mut self, iso: &Isometry) { + for v in &mut self.vertices[0..self.num_vertices] { + *v = iso * *v; + } + } + + pub fn contacts( + prediction_distance: f32, + face1: &PolyhedronFace, + sep_axis1: &Vector, + face2: &PolyhedronFace, + pos21: &Isometry, + manifold: &mut ContactManifold, + ) { + // Project the faces to a 2D plane for contact clipping. + // The plane they are projected onto has normal sep_axis1 + // and contains the origin (this is numerically OK because + // we are not working in world-space here). + let basis = sep_axis1.orthonormal_basis(); + let projected_face1 = [ + Point2::new( + face1.vertices[0].coords.dot(&basis[0]), + face1.vertices[0].coords.dot(&basis[1]), + ), + Point2::new( + face1.vertices[1].coords.dot(&basis[0]), + face1.vertices[1].coords.dot(&basis[1]), + ), + Point2::new( + face1.vertices[2].coords.dot(&basis[0]), + face1.vertices[2].coords.dot(&basis[1]), + ), + Point2::new( + face1.vertices[3].coords.dot(&basis[0]), + face1.vertices[3].coords.dot(&basis[1]), + ), + ]; + let projected_face2 = [ + Point2::new( + face2.vertices[0].coords.dot(&basis[0]), + face2.vertices[0].coords.dot(&basis[1]), + ), + Point2::new( + face2.vertices[1].coords.dot(&basis[0]), + face2.vertices[1].coords.dot(&basis[1]), + ), + Point2::new( + face2.vertices[2].coords.dot(&basis[0]), + face2.vertices[2].coords.dot(&basis[1]), + ), + Point2::new( + face2.vertices[3].coords.dot(&basis[0]), + face2.vertices[3].coords.dot(&basis[1]), + ), + ]; + + // Also find all the vertices located inside of the other projected face. + if face2.num_vertices > 2 { + let normal2 = (face2.vertices[2] - face2.vertices[1]) + .cross(&(face2.vertices[0] - face2.vertices[1])); + + let last_index2 = face2.num_vertices as usize - 1; + 'point_loop1: for i in 0..face1.num_vertices as usize { + let p1 = projected_face1[i]; + + let sign = (projected_face2[0] - projected_face2[last_index2]) + .perp(&(p1 - projected_face2[last_index2])); + for j in 0..last_index2 { + let new_sign = (projected_face2[j + 1] - projected_face2[j]) + .perp(&(p1 - projected_face2[j])); + if new_sign * sign < 0.0 { + // The point lies outside. + continue 'point_loop1; + } + } + + // All the perp had the same sign: the point is inside of the other shapes projection. + // Output the contact. + let denom = normal2.dot(&sep_axis1); + let dist = (face2.vertices[0] - face1.vertices[i]).dot(&normal2) / denom; + let local_p1 = face1.vertices[i]; + let local_p2 = face1.vertices[i] + dist * sep_axis1; + + if dist <= prediction_distance { + manifold.points.push(Contact { + local_p1, + local_p2: pos21 * local_p2, + impulse: 0.0, + tangent_impulse: Contact::zero_tangent_impulse(), + fid1: face1.vids[i], + fid2: face2.fid, + dist, + }); + } + } + } + + if face1.num_vertices > 2 { + let normal1 = (face1.vertices[2] - face1.vertices[1]) + .cross(&(face1.vertices[0] - face1.vertices[1])); + + let last_index1 = face1.num_vertices as usize - 1; + 'point_loop2: for i in 0..face2.num_vertices as usize { + let p2 = projected_face2[i]; + + let sign = (projected_face1[0] - projected_face1[last_index1]) + .perp(&(p2 - projected_face1[last_index1])); + for j in 0..last_index1 { + let new_sign = (projected_face1[j + 1] - projected_face1[j]) + .perp(&(p2 - projected_face1[j])); + + if new_sign * sign < 0.0 { + // The point lies outside. + continue 'point_loop2; + } + } + + // All the perp had the same sign: the point is inside of the other shapes projection. + // Output the contact. + let denom = -normal1.dot(&sep_axis1); + let dist = (face1.vertices[0] - face2.vertices[i]).dot(&normal1) / denom; + let local_p2 = face2.vertices[i]; + let local_p1 = face2.vertices[i] - dist * sep_axis1; + + if true { + // dist <= prediction_distance { + manifold.points.push(Contact { + local_p1, + local_p2: pos21 * local_p2, + impulse: 0.0, + tangent_impulse: Contact::zero_tangent_impulse(), + fid1: face1.fid, + fid2: face2.vids[i], + dist, + }); + } + } + } + + // Now we have to compute the intersection between all pairs of + // edges from the face 1 and from the face2. + for j in 0..face2.num_vertices { + let projected_edge2 = [ + projected_face2[j], + projected_face2[(j + 1) % face2.num_vertices], + ]; + + for i in 0..face1.num_vertices { + let projected_edge1 = [ + projected_face1[i], + projected_face1[(i + 1) % face1.num_vertices], + ]; + if let Some(bcoords) = closest_points_line2d(projected_edge1, projected_edge2) { + if bcoords.0 > 0.0 && bcoords.0 < 1.0 && bcoords.1 > 0.0 && bcoords.1 < 1.0 { + // Found a contact between the two edges. + let edge1 = ( + face1.vertices[i], + face1.vertices[(i + 1) % face1.num_vertices], + ); + let edge2 = ( + face2.vertices[j], + face2.vertices[(j + 1) % face2.num_vertices], + ); + let local_p1 = edge1.0 * (1.0 - bcoords.0) + edge1.1.coords * bcoords.0; + let local_p2 = edge2.0 * (1.0 - bcoords.1) + edge2.1.coords * bcoords.1; + let dist = (local_p2 - local_p1).dot(&sep_axis1); + + if dist <= prediction_distance { + manifold.points.push(Contact { + local_p1, + local_p2: pos21 * local_p2, + impulse: 0.0, + tangent_impulse: Contact::zero_tangent_impulse(), + fid1: face1.eids[i], + fid2: face2.eids[j], + dist, + }); + } + } + } + } + } + } +} + +/// Compute the barycentric coordinates of the intersection between the two given lines. +/// Returns `None` if the lines are parallel. +fn closest_points_line2d(edge1: [Point2; 2], edge2: [Point2; 2]) -> Option<(f32, f32)> { + use approx::AbsDiffEq; + + // Inspired by Real-time collision detection by Christer Ericson. + let dir1 = edge1[1] - edge1[0]; + let dir2 = edge2[1] - edge2[0]; + let r = edge1[0] - edge2[0]; + + let a = dir1.norm_squared(); + let e = dir2.norm_squared(); + let f = dir2.dot(&r); + + let eps = f32::default_epsilon(); + + if a <= eps && e <= eps { + Some((0.0, 0.0)) + } else if a <= eps { + Some((0.0, f / e)) + } else { + let c = dir1.dot(&r); + if e <= eps { + Some((-c / a, 0.0)) + } else { + let b = dir1.dot(&dir2); + let ae = a * e; + let bb = b * b; + let denom = ae - bb; + + // Use absolute and ulps error to test collinearity. + let parallel = denom <= eps || approx::ulps_eq!(ae, bb); + + let s = if !parallel { + (b * f - c * e) / denom + } else { + 0.0 + }; + + if parallel { + None + } else { + Some((s, (b * s + f) / e)) + } + } + } +} diff --git a/src/geometry/proximity.rs b/src/geometry/proximity.rs new file mode 100644 index 000000000..88e6e7644 --- /dev/null +++ b/src/geometry/proximity.rs @@ -0,0 +1,31 @@ +use crate::geometry::proximity_detector::ProximityPhase; +use crate::geometry::{ColliderPair, Proximity}; +use std::any::Any; + +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// The description of the proximity of two colliders. +pub struct ProximityPair { + /// The pair of collider involved. + pub pair: ColliderPair, + /// The state of proximity between the two colliders. + pub proximity: Proximity, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) detector: Option, + #[cfg_attr(feature = "serde-serialize", serde(skip))] + pub(crate) detector_workspace: Option>, +} + +impl ProximityPair { + pub(crate) fn new( + pair: ColliderPair, + detector: ProximityPhase, + detector_workspace: Option>, + ) -> Self { + Self { + pair, + proximity: Proximity::Disjoint, + detector: Some(detector), + detector_workspace, + } + } +} diff --git a/src/geometry/proximity_detector/ball_ball_proximity_detector.rs b/src/geometry/proximity_detector/ball_ball_proximity_detector.rs new file mode 100644 index 000000000..2106c9f5b --- /dev/null +++ b/src/geometry/proximity_detector/ball_ball_proximity_detector.rs @@ -0,0 +1,68 @@ +use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext; + +use crate::geometry::Proximity; +use crate::math::Point; +#[cfg(feature = "simd-is-enabled")] +use { + crate::geometry::{proximity_detector::PrimitiveProximityDetectionContextSimd, WBall}, + crate::math::{SimdFloat, SIMD_WIDTH}, + simba::simd::SimdValue, +}; + +#[cfg(feature = "simd-is-enabled")] +fn ball_distance_simd(ball1: &WBall, ball2: &WBall) -> SimdFloat { + let dcenter = ball2.center - ball1.center; + let center_dist = dcenter.magnitude(); + center_dist - ball1.radius - ball2.radius +} + +#[cfg(feature = "simd-is-enabled")] +pub fn detect_proximity_ball_ball_simd( + ctxt: &mut PrimitiveProximityDetectionContextSimd, +) -> [Proximity; SIMD_WIDTH] { + let pos_ba = ctxt.positions2.inverse() * ctxt.positions1; + let radii_a = + SimdFloat::from(array![|ii| ctxt.shapes1[ii].as_ball().unwrap().radius; SIMD_WIDTH]); + let radii_b = + SimdFloat::from(array![|ii| ctxt.shapes2[ii].as_ball().unwrap().radius; SIMD_WIDTH]); + + let wball_a = WBall::new(Point::origin(), radii_a); + let wball_b = WBall::new(pos_ba.inverse_transform_point(&Point::origin()), radii_b); + let distances = ball_distance_simd(&wball_a, &wball_b); + let mut proximities = [Proximity::Disjoint; SIMD_WIDTH]; + + for i in 0..SIMD_WIDTH { + // FIXME: compare the dist before computing the proximity. + let dist = distances.extract(i); + if dist > ctxt.prediction_distance { + proximities[i] = Proximity::Disjoint; + } else if dist > 0.0 { + proximities[i] = Proximity::WithinMargin; + } else { + proximities[i] = Proximity::Intersecting + } + } + + proximities +} + +pub fn detect_proximity_ball_ball(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity { + let pos_ba = ctxt.position2.inverse() * ctxt.position1; + let radius_a = ctxt.shape1.as_ball().unwrap().radius; + let radius_b = ctxt.shape2.as_ball().unwrap().radius; + + let center_a = Point::origin(); + let center_b = pos_ba.inverse_transform_point(&Point::origin()); + + let dcenter = center_b - center_a; + let center_dist = dcenter.magnitude(); + let dist = center_dist - radius_a - radius_b; + + if dist > ctxt.prediction_distance { + Proximity::Disjoint + } else if dist > 0.0 { + Proximity::WithinMargin + } else { + Proximity::Intersecting + } +} diff --git a/src/geometry/proximity_detector/ball_convex_proximity_detector.rs b/src/geometry/proximity_detector/ball_convex_proximity_detector.rs new file mode 100644 index 000000000..b00337d75 --- /dev/null +++ b/src/geometry/proximity_detector/ball_convex_proximity_detector.rs @@ -0,0 +1,53 @@ +use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext; +use crate::geometry::{Ball, Proximity, Shape}; +use crate::math::Isometry; +use ncollide::query::PointQuery; + +pub fn detect_proximity_ball_convex(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity { + if let Shape::Ball(ball1) = ctxt.shape1 { + match ctxt.shape2 { + Shape::Triangle(tri2) => do_detect_proximity(tri2, ball1, &ctxt), + Shape::Cuboid(cube2) => do_detect_proximity(cube2, ball1, &ctxt), + _ => unimplemented!(), + } + } else if let Shape::Ball(ball2) = ctxt.shape2 { + match ctxt.shape1 { + Shape::Triangle(tri1) => do_detect_proximity(tri1, ball2, &ctxt), + Shape::Cuboid(cube1) => do_detect_proximity(cube1, ball2, &ctxt), + _ => unimplemented!(), + } + } else { + panic!("Invalid shape types provide.") + } +} + +fn do_detect_proximity>( + point_query1: &P, + ball2: &Ball, + ctxt: &PrimitiveProximityDetectionContext, +) -> Proximity { + let local_p2_1 = ctxt + .position1 + .inverse_transform_point(&ctxt.position2.translation.vector.into()); + + // TODO: add a `project_local_point` to the PointQuery trait to avoid + // the identity isometry. + let proj = + point_query1.project_point(&Isometry::identity(), &local_p2_1, cfg!(feature = "dim3")); + let dpos = local_p2_1 - proj.point; + let dist = dpos.norm(); + + if proj.is_inside { + return Proximity::Intersecting; + } + + if dist <= ball2.radius + ctxt.prediction_distance { + if dist <= ball2.radius { + Proximity::Intersecting + } else { + Proximity::WithinMargin + } + } else { + Proximity::Disjoint + } +} diff --git a/src/geometry/proximity_detector/ball_polygon_proximity_detector.rs b/src/geometry/proximity_detector/ball_polygon_proximity_detector.rs new file mode 100644 index 000000000..e69de29bb diff --git a/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs b/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs new file mode 100644 index 000000000..b68ebf93a --- /dev/null +++ b/src/geometry/proximity_detector/cuboid_cuboid_proximity_detector.rs @@ -0,0 +1,79 @@ +use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext; +use crate::geometry::{sat, Proximity, Shape}; +use crate::math::Isometry; +use ncollide::shape::Cuboid; + +pub fn detect_proximity_cuboid_cuboid(ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity { + if let (Shape::Cuboid(cube1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) { + detect_proximity( + ctxt.prediction_distance, + cube1, + ctxt.position1, + cube2, + ctxt.position2, + ) + } else { + unreachable!() + } +} + +pub fn detect_proximity<'a>( + prediction_distance: f32, + cube1: &'a Cuboid, + pos1: &'a Isometry, + cube2: &'a Cuboid, + pos2: &'a Isometry, +) -> Proximity { + let pos12 = pos1.inverse() * pos2; + let pos21 = pos12.inverse(); + + /* + * + * Point-Face + * + */ + let sep1 = + sat::cuboid_cuboid_find_local_separating_normal_oneway(cube1, cube2, &pos12, &pos21).0; + if sep1 > prediction_distance { + return Proximity::Disjoint; + } + + let sep2 = + sat::cuboid_cuboid_find_local_separating_normal_oneway(cube2, cube1, &pos21, &pos12).0; + if sep2 > prediction_distance { + return Proximity::Disjoint; + } + + /* + * + * Edge-Edge cases + * + */ + #[cfg(feature = "dim2")] + let sep3 = -f32::MAX; // This case does not exist in 2D. + #[cfg(feature = "dim3")] + let sep3 = sat::cuboid_cuboid_find_local_separating_edge_twoway(cube1, cube2, &pos12, &pos21).0; + if sep3 > prediction_distance { + return Proximity::Disjoint; + } + + if sep2 > sep1 && sep2 > sep3 { + if sep2 > 0.0 { + Proximity::WithinMargin + } else { + Proximity::Intersecting + } + } else if sep3 > sep1 { + if sep3 > 0.0 { + Proximity::WithinMargin + } else { + Proximity::Intersecting + } + } else { + if sep1 > 0.0 { + Proximity::WithinMargin + } else { + Proximity::Intersecting + } + } +} diff --git a/src/geometry/proximity_detector/cuboid_polygon_proximity_detector.rs b/src/geometry/proximity_detector/cuboid_polygon_proximity_detector.rs new file mode 100644 index 000000000..e69de29bb diff --git a/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs b/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs new file mode 100644 index 000000000..12f3b4a6f --- /dev/null +++ b/src/geometry/proximity_detector/cuboid_triangle_proximity_detector.rs @@ -0,0 +1,88 @@ +use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext; +use crate::geometry::{sat, Cuboid, Proximity, Shape, Triangle}; +use crate::math::Isometry; + +pub fn detect_proximity_cuboid_triangle( + ctxt: &mut PrimitiveProximityDetectionContext, +) -> Proximity { + if let (Shape::Cuboid(cube1), Shape::Triangle(triangle2)) = (ctxt.shape1, ctxt.shape2) { + detect_proximity( + ctxt.prediction_distance, + cube1, + ctxt.position1, + triangle2, + ctxt.position2, + ) + } else if let (Shape::Triangle(triangle1), Shape::Cuboid(cube2)) = (ctxt.shape1, ctxt.shape2) { + detect_proximity( + ctxt.prediction_distance, + cube2, + ctxt.position2, + triangle1, + ctxt.position1, + ) + } else { + panic!("Invalid shape types") + } +} + +pub fn detect_proximity<'a>( + prediction_distance: f32, + cube1: &'a Cuboid, + pos1: &'a Isometry, + triangle2: &'a Triangle, + pos2: &'a Isometry, +) -> Proximity { + let pos12 = pos1.inverse() * pos2; + let pos21 = pos12.inverse(); + + /* + * + * Point-Face cases. + * + */ + let sep1 = + sat::cube_support_map_find_local_separating_normal_oneway(cube1, triangle2, &pos12).0; + if sep1 > prediction_distance { + return Proximity::Disjoint; + } + + let sep2 = sat::triangle_cuboid_find_local_separating_normal_oneway(triangle2, cube1, &pos21).0; + if sep2 > prediction_distance { + return Proximity::Disjoint; + } + + /* + * + * Edge-Edge cases. + * + */ + #[cfg(feature = "dim2")] + let sep3 = -f32::MAX; // This case does not exist in 2D. + #[cfg(feature = "dim3")] + let sep3 = + sat::cube_triangle_find_local_separating_edge_twoway(cube1, triangle2, &pos12, &pos21).0; + if sep3 > prediction_distance { + return Proximity::Disjoint; + } + + if sep2 > sep1 && sep2 > sep3 { + if sep2 > 0.0 { + Proximity::WithinMargin + } else { + Proximity::Intersecting + } + } else if sep3 > sep1 { + if sep3 > 0.0 { + Proximity::WithinMargin + } else { + Proximity::Intersecting + } + } else { + if sep1 > 0.0 { + Proximity::WithinMargin + } else { + Proximity::Intersecting + } + } +} diff --git a/src/geometry/proximity_detector/mod.rs b/src/geometry/proximity_detector/mod.rs new file mode 100644 index 000000000..a99372f0a --- /dev/null +++ b/src/geometry/proximity_detector/mod.rs @@ -0,0 +1,30 @@ +pub use self::ball_ball_proximity_detector::detect_proximity_ball_ball; +#[cfg(feature = "simd-is-enabled")] +pub use self::ball_ball_proximity_detector::detect_proximity_ball_ball_simd; +pub use self::ball_convex_proximity_detector::detect_proximity_ball_convex; +pub use self::cuboid_cuboid_proximity_detector::detect_proximity_cuboid_cuboid; +pub use self::cuboid_triangle_proximity_detector::detect_proximity_cuboid_triangle; +pub use self::polygon_polygon_proximity_detector::detect_proximity_polygon_polygon; +pub use self::proximity_detector::{ + PrimitiveProximityDetectionContext, PrimitiveProximityDetector, ProximityDetectionContext, + ProximityDetector, ProximityPhase, +}; +#[cfg(feature = "simd-is-enabled")] +pub use self::proximity_detector::{ + PrimitiveProximityDetectionContextSimd, ProximityDetectionContextSimd, +}; +pub use self::proximity_dispatcher::{DefaultProximityDispatcher, ProximityDispatcher}; +pub use self::trimesh_shape_proximity_detector::{ + detect_proximity_trimesh_shape, TrimeshShapeProximityDetectorWorkspace, +}; + +mod ball_ball_proximity_detector; +mod ball_convex_proximity_detector; +mod ball_polygon_proximity_detector; +mod cuboid_cuboid_proximity_detector; +mod cuboid_polygon_proximity_detector; +mod cuboid_triangle_proximity_detector; +mod polygon_polygon_proximity_detector; +mod proximity_detector; +mod proximity_dispatcher; +mod trimesh_shape_proximity_detector; diff --git a/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs b/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs new file mode 100644 index 000000000..f0e049f2e --- /dev/null +++ b/src/geometry/proximity_detector/polygon_polygon_proximity_detector.rs @@ -0,0 +1,54 @@ +use crate::geometry::proximity_detector::PrimitiveProximityDetectionContext; +use crate::geometry::{sat, Polygon, Proximity, Shape}; +use crate::math::Isometry; + +pub fn detect_proximity_polygon_polygon( + ctxt: &mut PrimitiveProximityDetectionContext, +) -> Proximity { + if let (Shape::Polygon(polygon1), Shape::Polygon(polygon2)) = (ctxt.shape1, ctxt.shape2) { + detect_proximity( + ctxt.prediction_distance, + polygon1, + &ctxt.position1, + polygon2, + &ctxt.position2, + ) + } else { + unreachable!() + } +} + +fn detect_proximity<'a>( + prediction_distance: f32, + p1: &'a Polygon, + m1: &'a Isometry, + p2: &'a Polygon, + m2: &'a Isometry, +) -> Proximity { + let m12 = m1.inverse() * m2; + let m21 = m12.inverse(); + + let sep1 = sat::polygon_polygon_compute_separation_features(p1, p2, &m12); + if sep1.0 > prediction_distance { + return Proximity::Disjoint; + } + + let sep2 = sat::polygon_polygon_compute_separation_features(p2, p1, &m21); + if sep2.0 > prediction_distance { + return Proximity::Disjoint; + } + + if sep2.0 > sep1.0 { + if sep2.0 > 0.0 { + Proximity::WithinMargin + } else { + Proximity::Intersecting + } + } else { + if sep1.0 > 0.0 { + Proximity::WithinMargin + } else { + Proximity::Intersecting + } + } +} diff --git a/src/geometry/proximity_detector/proximity_detector.rs b/src/geometry/proximity_detector/proximity_detector.rs new file mode 100644 index 000000000..76e8cd7ca --- /dev/null +++ b/src/geometry/proximity_detector/proximity_detector.rs @@ -0,0 +1,212 @@ +use crate::geometry::{ + Collider, ColliderSet, Proximity, ProximityDispatcher, ProximityEvent, ProximityPair, Shape, +}; +use crate::math::Isometry; +#[cfg(feature = "simd-is-enabled")] +use crate::math::{SimdFloat, SIMD_WIDTH}; +use crate::pipeline::EventHandler; +use std::any::Any; + +#[derive(Copy, Clone)] +pub enum ProximityPhase { + NearPhase(ProximityDetector), + ExactPhase(PrimitiveProximityDetector), +} + +impl ProximityPhase { + #[inline] + pub fn detect_proximity( + self, + mut context: ProximityDetectionContext, + events: &dyn EventHandler, + ) { + let proximity = match self { + Self::NearPhase(gen) => (gen.detect_proximity)(&mut context), + Self::ExactPhase(gen) => { + // Build the primitive context from the non-primitive context and dispatch. + let collider1 = &context.colliders[context.pair.pair.collider1]; + let collider2 = &context.colliders[context.pair.pair.collider2]; + + let mut context2 = PrimitiveProximityDetectionContext { + prediction_distance: context.prediction_distance, + collider1, + collider2, + shape1: collider1.shape(), + shape2: collider2.shape(), + position1: collider1.position(), + position2: collider2.position(), + workspace: context.pair.detector_workspace.as_mut().map(|w| &mut **w), + }; + + (gen.detect_proximity)(&mut context2) + } + }; + + if context.pair.proximity != proximity { + events.handle_proximity_event(ProximityEvent::new( + context.pair.pair.collider1, + context.pair.pair.collider2, + context.pair.proximity, + proximity, + )) + } + + context.pair.proximity = proximity; + } + + #[cfg(feature = "simd-is-enabled")] + #[inline] + pub fn detect_proximity_simd( + self, + mut context: ProximityDetectionContextSimd, + events: &dyn EventHandler, + ) { + let proximities = match self { + Self::NearPhase(gen) => (gen.detect_proximity_simd)(&mut context), + Self::ExactPhase(gen) => { + // Build the primitive context from the non-primitive context and dispatch. + use arrayvec::ArrayVec; + let mut colliders_arr: ArrayVec<[(&Collider, &Collider); SIMD_WIDTH]> = + ArrayVec::new(); + let mut workspace_arr: ArrayVec< + [Option<&mut (dyn Any + Send + Sync)>; SIMD_WIDTH], + > = ArrayVec::new(); + + for pair in context.pairs.iter_mut() { + let collider1 = &context.colliders[pair.pair.collider1]; + let collider2 = &context.colliders[pair.pair.collider2]; + colliders_arr.push((collider1, collider2)); + workspace_arr.push(pair.detector_workspace.as_mut().map(|w| &mut **w)); + } + + let max_index = colliders_arr.len() - 1; + let colliders1 = array![|ii| colliders_arr[ii.min(max_index)].0; SIMD_WIDTH]; + let colliders2 = array![|ii| colliders_arr[ii.min(max_index)].1; SIMD_WIDTH]; + + let mut context2 = PrimitiveProximityDetectionContextSimd { + prediction_distance: context.prediction_distance, + colliders1, + colliders2, + shapes1: array![|ii| colliders1[ii].shape(); SIMD_WIDTH], + shapes2: array![|ii| colliders2[ii].shape(); SIMD_WIDTH], + positions1: &Isometry::from( + array![|ii| *colliders1[ii].position(); SIMD_WIDTH], + ), + positions2: &Isometry::from( + array![|ii| *colliders2[ii].position(); SIMD_WIDTH], + ), + workspaces: workspace_arr.as_mut_slice(), + }; + + (gen.detect_proximity_simd)(&mut context2) + } + }; + + for (i, pair) in context.pairs.iter_mut().enumerate() { + if pair.proximity != proximities[i] { + events.handle_proximity_event(ProximityEvent::new( + pair.pair.collider1, + pair.pair.collider2, + pair.proximity, + proximities[i], + )) + } + pair.proximity = proximities[i]; + } + } +} + +pub struct PrimitiveProximityDetectionContext<'a> { + pub prediction_distance: f32, + pub collider1: &'a Collider, + pub collider2: &'a Collider, + pub shape1: &'a Shape, + pub shape2: &'a Shape, + pub position1: &'a Isometry, + pub position2: &'a Isometry, + pub workspace: Option<&'a mut (dyn Any + Send + Sync)>, +} + +#[cfg(feature = "simd-is-enabled")] +pub struct PrimitiveProximityDetectionContextSimd<'a, 'b> { + pub prediction_distance: f32, + pub colliders1: [&'a Collider; SIMD_WIDTH], + pub colliders2: [&'a Collider; SIMD_WIDTH], + pub shapes1: [&'a Shape; SIMD_WIDTH], + pub shapes2: [&'a Shape; SIMD_WIDTH], + pub positions1: &'a Isometry, + pub positions2: &'a Isometry, + pub workspaces: &'a mut [Option<&'b mut (dyn Any + Send + Sync)>], +} + +#[derive(Copy, Clone)] +pub struct PrimitiveProximityDetector { + pub detect_proximity: fn(&mut PrimitiveProximityDetectionContext) -> Proximity, + #[cfg(feature = "simd-is-enabled")] + pub detect_proximity_simd: + fn(&mut PrimitiveProximityDetectionContextSimd) -> [Proximity; SIMD_WIDTH], +} + +impl PrimitiveProximityDetector { + fn unimplemented_fn(_ctxt: &mut PrimitiveProximityDetectionContext) -> Proximity { + Proximity::Disjoint + } + #[cfg(feature = "simd-is-enabled")] + fn unimplemented_simd_fn( + _ctxt: &mut PrimitiveProximityDetectionContextSimd, + ) -> [Proximity; SIMD_WIDTH] { + [Proximity::Disjoint; SIMD_WIDTH] + } +} + +impl Default for PrimitiveProximityDetector { + fn default() -> Self { + Self { + detect_proximity: Self::unimplemented_fn, + #[cfg(feature = "simd-is-enabled")] + detect_proximity_simd: Self::unimplemented_simd_fn, + } + } +} + +pub struct ProximityDetectionContext<'a> { + pub dispatcher: &'a dyn ProximityDispatcher, + pub prediction_distance: f32, + pub colliders: &'a ColliderSet, + pub pair: &'a mut ProximityPair, +} + +#[cfg(feature = "simd-is-enabled")] +pub struct ProximityDetectionContextSimd<'a, 'b> { + pub dispatcher: &'a dyn ProximityDispatcher, + pub prediction_distance: f32, + pub colliders: &'a ColliderSet, + pub pairs: &'a mut [&'b mut ProximityPair], +} + +#[derive(Copy, Clone)] +pub struct ProximityDetector { + pub detect_proximity: fn(&mut ProximityDetectionContext) -> Proximity, + #[cfg(feature = "simd-is-enabled")] + pub detect_proximity_simd: fn(&mut ProximityDetectionContextSimd) -> [Proximity; SIMD_WIDTH], +} + +impl ProximityDetector { + fn unimplemented_fn(_ctxt: &mut ProximityDetectionContext) -> Proximity { + Proximity::Disjoint + } + #[cfg(feature = "simd-is-enabled")] + fn unimplemented_simd_fn(_ctxt: &mut ProximityDetectionContextSimd) -> [Proximity; SIMD_WIDTH] { + [Proximity::Disjoint; SIMD_WIDTH] + } +} + +impl Default for ProximityDetector { + fn default() -> Self { + Self { + detect_proximity: Self::unimplemented_fn, + #[cfg(feature = "simd-is-enabled")] + detect_proximity_simd: Self::unimplemented_simd_fn, + } + } +} diff --git a/src/geometry/proximity_detector/proximity_dispatcher.rs b/src/geometry/proximity_detector/proximity_dispatcher.rs new file mode 100644 index 000000000..6d6b4c5dd --- /dev/null +++ b/src/geometry/proximity_detector/proximity_dispatcher.rs @@ -0,0 +1,136 @@ +use crate::geometry::proximity_detector::{ + PrimitiveProximityDetector, ProximityDetector, ProximityPhase, + TrimeshShapeProximityDetectorWorkspace, +}; +use crate::geometry::Shape; +use std::any::Any; + +/// Trait implemented by structures responsible for selecting a collision-detection algorithm +/// for a given pair of shapes. +pub trait ProximityDispatcher { + /// Select the proximity detection algorithm for the given pair of primitive shapes. + fn dispatch_primitives( + &self, + shape1: &Shape, + shape2: &Shape, + ) -> ( + PrimitiveProximityDetector, + Option>, + ); + /// Select the proximity detection algorithm for the given pair of non-primitive shapes. + fn dispatch( + &self, + shape1: &Shape, + shape2: &Shape, + ) -> (ProximityPhase, Option>); +} + +/// The default proximity dispatcher used by Rapier. +pub struct DefaultProximityDispatcher; + +impl ProximityDispatcher for DefaultProximityDispatcher { + fn dispatch_primitives( + &self, + shape1: &Shape, + shape2: &Shape, + ) -> ( + PrimitiveProximityDetector, + Option>, + ) { + match (shape1, shape2) { + (Shape::Ball(_), Shape::Ball(_)) => ( + PrimitiveProximityDetector { + #[cfg(feature = "simd-is-enabled")] + detect_proximity_simd: super::detect_proximity_ball_ball_simd, + detect_proximity: super::detect_proximity_ball_ball, + ..PrimitiveProximityDetector::default() + }, + None, + ), + (Shape::Cuboid(_), Shape::Cuboid(_)) => ( + PrimitiveProximityDetector { + detect_proximity: super::detect_proximity_cuboid_cuboid, + ..PrimitiveProximityDetector::default() + }, + None, + ), + (Shape::Polygon(_), Shape::Polygon(_)) => ( + PrimitiveProximityDetector { + detect_proximity: super::detect_proximity_polygon_polygon, + ..PrimitiveProximityDetector::default() + }, + None, + ), + (Shape::Triangle(_), Shape::Ball(_)) => ( + PrimitiveProximityDetector { + detect_proximity: super::detect_proximity_ball_convex, + ..PrimitiveProximityDetector::default() + }, + None, + ), + (Shape::Ball(_), Shape::Triangle(_)) => ( + PrimitiveProximityDetector { + detect_proximity: super::detect_proximity_ball_convex, + ..PrimitiveProximityDetector::default() + }, + None, + ), + (Shape::Cuboid(_), Shape::Ball(_)) => ( + PrimitiveProximityDetector { + detect_proximity: super::detect_proximity_ball_convex, + ..PrimitiveProximityDetector::default() + }, + None, + ), + (Shape::Ball(_), Shape::Cuboid(_)) => ( + PrimitiveProximityDetector { + detect_proximity: super::detect_proximity_ball_convex, + ..PrimitiveProximityDetector::default() + }, + None, + ), + (Shape::Triangle(_), Shape::Cuboid(_)) => ( + PrimitiveProximityDetector { + detect_proximity: super::detect_proximity_cuboid_triangle, + ..PrimitiveProximityDetector::default() + }, + None, + ), + (Shape::Cuboid(_), Shape::Triangle(_)) => ( + PrimitiveProximityDetector { + detect_proximity: super::detect_proximity_cuboid_triangle, + ..PrimitiveProximityDetector::default() + }, + None, + ), + _ => (PrimitiveProximityDetector::default(), None), + } + } + + fn dispatch( + &self, + shape1: &Shape, + shape2: &Shape, + ) -> (ProximityPhase, Option>) { + match (shape1, shape2) { + (Shape::Trimesh(_), _) => ( + ProximityPhase::NearPhase(ProximityDetector { + detect_proximity: super::detect_proximity_trimesh_shape, + ..ProximityDetector::default() + }), + Some(Box::new(TrimeshShapeProximityDetectorWorkspace::new())), + ), + (_, Shape::Trimesh(_)) => ( + ProximityPhase::NearPhase(ProximityDetector { + detect_proximity: super::detect_proximity_trimesh_shape, + ..ProximityDetector::default() + }), + Some(Box::new(TrimeshShapeProximityDetectorWorkspace::new())), + ), + _ => { + let (gen, workspace) = self.dispatch_primitives(shape1, shape2); + (ProximityPhase::ExactPhase(gen), workspace) + } + } + } +} diff --git a/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs new file mode 100644 index 000000000..3dd73817f --- /dev/null +++ b/src/geometry/proximity_detector/trimesh_shape_proximity_detector.rs @@ -0,0 +1,133 @@ +use crate::geometry::proximity_detector::{ + PrimitiveProximityDetectionContext, ProximityDetectionContext, +}; +use crate::geometry::{Collider, Proximity, Shape, Trimesh, WAABBHierarchyIntersections}; +use crate::ncollide::bounding_volume::{BoundingVolume, AABB}; + +pub struct TrimeshShapeProximityDetectorWorkspace { + interferences: WAABBHierarchyIntersections, + local_aabb2: AABB, + old_interferences: Vec, +} + +impl TrimeshShapeProximityDetectorWorkspace { + pub fn new() -> Self { + Self { + interferences: WAABBHierarchyIntersections::new(), + local_aabb2: AABB::new_invalid(), + old_interferences: Vec::new(), + } + } +} + +pub fn detect_proximity_trimesh_shape(ctxt: &mut ProximityDetectionContext) -> Proximity { + let collider1 = &ctxt.colliders[ctxt.pair.pair.collider1]; + let collider2 = &ctxt.colliders[ctxt.pair.pair.collider2]; + + if let Shape::Trimesh(trimesh1) = collider1.shape() { + do_detect_proximity(trimesh1, collider1, collider2, ctxt) + } else if let Shape::Trimesh(trimesh2) = collider2.shape() { + do_detect_proximity(trimesh2, collider2, collider1, ctxt) + } else { + panic!("Invalid shape types provided.") + } +} + +fn do_detect_proximity( + trimesh1: &Trimesh, + collider1: &Collider, + collider2: &Collider, + ctxt: &mut ProximityDetectionContext, +) -> Proximity { + let workspace: &mut TrimeshShapeProximityDetectorWorkspace = ctxt + .pair + .detector_workspace + .as_mut() + .expect("The TrimeshShapeProximityDetectorWorkspace is missing.") + .downcast_mut() + .expect("Invalid workspace type, expected a TrimeshShapeProximityDetectorWorkspace."); + + /* + * Compute interferences. + */ + let pos12 = collider1.position.inverse() * collider2.position; + // TODO: somehow precompute the AABB and reuse it? + let mut new_local_aabb2 = collider2 + .shape() + .compute_aabb(&pos12) + .loosened(ctxt.prediction_distance); + let same_local_aabb2 = workspace.local_aabb2.contains(&new_local_aabb2); + + if !same_local_aabb2 { + let extra_margin = + (new_local_aabb2.maxs - new_local_aabb2.mins).map(|e| (e / 10.0).min(0.1)); + new_local_aabb2.mins -= extra_margin; + new_local_aabb2.maxs += extra_margin; + + let local_aabb2 = new_local_aabb2; // .loosened(ctxt.prediction_distance * 2.0); // FIXME: what would be the best value? + std::mem::swap( + &mut workspace.old_interferences, + &mut workspace.interferences.computed_interferences_mut(), + ); + + trimesh1 + .waabbs() + .compute_interferences_with(local_aabb2, &mut workspace.interferences); + workspace.local_aabb2 = local_aabb2; + } + + /* + * Dispatch to the specific solver by keeping the previous manifold if we already had one. + */ + let new_interferences = workspace.interferences.computed_interferences(); + let mut old_inter_it = workspace.old_interferences.drain(..).peekable(); + let mut best_proximity = Proximity::Disjoint; + + for triangle_id in new_interferences.iter() { + if *triangle_id >= trimesh1.num_triangles() { + // Because of SIMD padding, the broad-phase may return tiangle indices greater + // than the max. + continue; + } + + if !same_local_aabb2 { + loop { + match old_inter_it.peek() { + Some(old_triangle_id) if *old_triangle_id < *triangle_id => { + old_inter_it.next(); + } + _ => break, + } + } + + if old_inter_it.peek() != Some(triangle_id) { + } else { + old_inter_it.next(); + }; + } + + let triangle1 = Shape::Triangle(trimesh1.triangle(*triangle_id)); + let (proximity_detector, mut workspace2) = ctxt + .dispatcher + .dispatch_primitives(&triangle1, collider2.shape()); + + let mut ctxt2 = PrimitiveProximityDetectionContext { + prediction_distance: ctxt.prediction_distance, + collider1, + collider2, + shape1: &triangle1, + shape2: collider2.shape(), + position1: collider1.position(), + position2: collider2.position(), + workspace: workspace2.as_mut().map(|w| &mut **w), + }; + + match (proximity_detector.detect_proximity)(&mut ctxt2) { + Proximity::Intersecting => return Proximity::Intersecting, + Proximity::WithinMargin => best_proximity = Proximity::WithinMargin, + Proximity::Disjoint => {} + } + } + + best_proximity +} diff --git a/src/geometry/proximity_detector/voxels_shape_proximity_detector.rs b/src/geometry/proximity_detector/voxels_shape_proximity_detector.rs new file mode 100644 index 000000000..e69de29bb diff --git a/src/geometry/sat.rs b/src/geometry/sat.rs new file mode 100644 index 000000000..0666c045e --- /dev/null +++ b/src/geometry/sat.rs @@ -0,0 +1,294 @@ +use crate::geometry::{cuboid, Cuboid, Polygon, Triangle}; +use crate::math::{Isometry, Point, Vector, DIM}; +use crate::utils::WSign; +use na::Unit; +use ncollide::shape::{Segment, SupportMap}; + +pub fn polygon_polygon_compute_separation_features( + p1: &Polygon, + p2: &Polygon, + m12: &Isometry, +) -> (f32, usize, usize) { + let mut max_separation = -f32::MAX; + let mut separation_features = (0, 0); + + for (i, (p1, n1)) in p1.vertices.iter().zip(p1.normals.iter()).enumerate() { + let j = p2.support_point(&m12.inverse_transform_vector(&-n1)); + let dpt = m12 * p2.vertices[j] - p1; + let separation = dpt.dot(n1); + + if separation > max_separation { + max_separation = separation; + separation_features = (i, j); + } + } + + (max_separation, separation_features.0, separation_features.1) +} + +#[cfg(feature = "dim3")] +pub fn cuboid_cuboid_compute_separation_wrt_local_line( + cube1: &Cuboid, + cube2: &Cuboid, + pos12: &Isometry, + pos21: &Isometry, + axis1: &Vector, +) -> (f32, Vector) { + let signum = pos12.translation.vector.dot(axis1).copy_sign_to(1.0); + let axis1 = axis1 * signum; + let local_pt1 = cuboid::local_support_point(cube1, axis1); + let local_pt2 = cuboid::local_support_point(cube2, pos21 * -axis1); + let pt2 = pos12 * local_pt2; + let separation = (pt2 - local_pt1).dot(&axis1); + (separation, axis1) +} + +#[cfg(feature = "dim3")] +pub fn cuboid_cuboid_find_local_separating_edge_twoway( + cube1: &Cuboid, + cube2: &Cuboid, + pos12: &Isometry, + pos21: &Isometry, +) -> (f32, Vector) { + use approx::AbsDiffEq; + let mut best_separation = -std::f32::MAX; + let mut best_dir = Vector::zeros(); + + let x2 = pos12 * Vector::x(); + let y2 = pos12 * Vector::y(); + let z2 = pos12 * Vector::z(); + + // We have 3 * 3 = 9 axii to test. + let axii = [ + // Vector::{x, y ,z}().cross(y2) + Vector::new(0.0, -x2.z, x2.y), + Vector::new(x2.z, 0.0, -x2.x), + Vector::new(-x2.y, x2.x, 0.0), + // Vector::{x, y ,z}().cross(y2) + Vector::new(0.0, -y2.z, y2.y), + Vector::new(y2.z, 0.0, -y2.x), + Vector::new(-y2.y, y2.x, 0.0), + // Vector::{x, y ,z}().cross(y2) + Vector::new(0.0, -z2.z, z2.y), + Vector::new(z2.z, 0.0, -z2.x), + Vector::new(-z2.y, z2.x, 0.0), + ]; + + for axis1 in &axii { + let norm1 = axis1.norm(); + if norm1 > f32::default_epsilon() { + let (separation, axis1) = cuboid_cuboid_compute_separation_wrt_local_line( + cube1, + cube2, + pos12, + pos21, + &(axis1 / norm1), + ); + + if separation > best_separation { + best_separation = separation; + best_dir = axis1; + } + } + } + + (best_separation, best_dir) +} + +pub fn cuboid_cuboid_find_local_separating_normal_oneway( + cube1: &Cuboid, + cube2: &Cuboid, + pos12: &Isometry, + pos21: &Isometry, +) -> (f32, Vector) { + let mut best_separation = -std::f32::MAX; + let mut best_dir = Vector::zeros(); + + for i in 0..DIM { + let sign = pos12.translation.vector[i].copy_sign_to(1.0); + let axis1 = Vector::ith(i, sign); + let local_pt2 = cuboid::local_support_point(cube2, pos21 * -axis1); + let pt2 = pos12 * local_pt2; + let separation = pt2[i] * sign - cube1.half_extents[i]; + + if separation > best_separation { + best_separation = separation; + best_dir = axis1; + } + } + + (best_separation, best_dir) +} + +/* + * + * + * Triangles. + * + * + */ + +#[cfg(feature = "dim3")] +pub fn cube_support_map_compute_separation_wrt_local_line>( + cube1: &Cuboid, + shape2: &S, + pos12: &Isometry, + pos21: &Isometry, + axis1: &Unit>, +) -> (f32, Unit>) { + let signum = pos12.translation.vector.dot(axis1).copy_sign_to(1.0); + let axis1 = Unit::new_unchecked(**axis1 * signum); + let local_pt1 = cuboid::local_support_point(cube1, *axis1); + let local_pt2 = shape2.local_support_point_toward(&(pos21 * -axis1)); + let pt2 = pos12 * local_pt2; + let separation = (pt2 - local_pt1).dot(&axis1); + (separation, axis1) +} + +#[cfg(feature = "dim3")] +pub fn cube_support_map_find_local_separating_edge_twoway( + cube1: &Cuboid, + shape2: &impl SupportMap, + axii: &[Vector], + pos12: &Isometry, + pos21: &Isometry, +) -> (f32, Vector) { + use approx::AbsDiffEq; + let mut best_separation = -std::f32::MAX; + let mut best_dir = Vector::zeros(); + + for axis1 in axii { + if let Some(axis1) = Unit::try_new(*axis1, f32::default_epsilon()) { + let (separation, axis1) = cube_support_map_compute_separation_wrt_local_line( + cube1, shape2, pos12, pos21, &axis1, + ); + + if separation > best_separation { + best_separation = separation; + best_dir = *axis1; + } + } + } + + (best_separation, best_dir) +} + +#[cfg(feature = "dim3")] +pub fn cube_triangle_find_local_separating_edge_twoway( + cube1: &Cuboid, + triangle2: &Triangle, + pos12: &Isometry, + pos21: &Isometry, +) -> (f32, Vector) { + let x2 = pos12 * (triangle2.b - triangle2.a); + let y2 = pos12 * (triangle2.c - triangle2.b); + let z2 = pos12 * (triangle2.a - triangle2.c); + + // We have 3 * 3 = 3 axii to test. + let axii = [ + // Vector::{x, y ,z}().cross(y2) + Vector::new(0.0, -x2.z, x2.y), + Vector::new(x2.z, 0.0, -x2.x), + Vector::new(-x2.y, x2.x, 0.0), + // Vector::{x, y ,z}().cross(y2) + Vector::new(0.0, -y2.z, y2.y), + Vector::new(y2.z, 0.0, -y2.x), + Vector::new(-y2.y, y2.x, 0.0), + // Vector::{x, y ,z}().cross(y2) + Vector::new(0.0, -z2.z, z2.y), + Vector::new(z2.z, 0.0, -z2.x), + Vector::new(-z2.y, z2.x, 0.0), + ]; + + cube_support_map_find_local_separating_edge_twoway(cube1, triangle2, &axii, pos12, pos21) +} + +#[cfg(feature = "dim3")] +pub fn cube_segment_find_local_separating_edge_twoway( + cube1: &Cuboid, + segment2: &Segment, + pos12: &Isometry, + pos21: &Isometry, +) -> (f32, Vector) { + let x2 = pos12 * (segment2.b - segment2.a); + + let axii = [ + // Vector::{x, y ,z}().cross(y2) + Vector::new(0.0, -x2.z, x2.y), + Vector::new(x2.z, 0.0, -x2.x), + Vector::new(-x2.y, x2.x, 0.0), + ]; + + cube_support_map_find_local_separating_edge_twoway(cube1, segment2, &axii, pos12, pos21) +} + +pub fn cube_support_map_find_local_separating_normal_oneway>( + cube1: &Cuboid, + shape2: &S, + pos12: &Isometry, +) -> (f32, Vector) { + let mut best_separation = -std::f32::MAX; + let mut best_dir = Vector::zeros(); + + for i in 0..DIM { + for sign in &[-1.0, 1.0] { + let axis1 = Vector::ith(i, *sign); + let pt2 = shape2.support_point_toward(&pos12, &Unit::new_unchecked(-axis1)); + let separation = pt2[i] * *sign - cube1.half_extents[i]; + + if separation > best_separation { + best_separation = separation; + best_dir = axis1; + } + } + } + + (best_separation, best_dir) +} + +// NOTE: this only works with cuboid on the rhs because it has its symmetry origin at zero +// (therefore we can check only one normal direction). +pub fn point_cuboid_find_local_separating_normal_oneway( + point1: Point, + normal1: Option>>, + shape2: &Cuboid, + pos12: &Isometry, +) -> (f32, Vector) { + let mut best_separation = -std::f32::MAX; + let mut best_dir = Vector::zeros(); + + if let Some(normal1) = normal1 { + let axis1 = if (pos12.translation.vector - point1.coords).dot(&normal1) >= 0.0 { + normal1 + } else { + -normal1 + }; + + let pt2 = shape2.support_point_toward(&pos12, &-axis1); + let separation = (pt2 - point1).dot(&axis1); + + if separation > best_separation { + best_separation = separation; + best_dir = *axis1; + } + } + + (best_separation, best_dir) +} + +pub fn triangle_cuboid_find_local_separating_normal_oneway( + triangle1: &Triangle, + shape2: &Cuboid, + pos12: &Isometry, +) -> (f32, Vector) { + point_cuboid_find_local_separating_normal_oneway(triangle1.a, triangle1.normal(), shape2, pos12) +} + +#[cfg(feature = "dim2")] +pub fn segment_cuboid_find_local_separating_normal_oneway( + segment1: &Segment, + shape2: &Cuboid, + pos12: &Isometry, +) -> (f32, Vector) { + point_cuboid_find_local_separating_normal_oneway(segment1.a, segment1.normal(), shape2, pos12) +} diff --git a/src/geometry/triangle.rs b/src/geometry/triangle.rs new file mode 100644 index 000000000..02375b72b --- /dev/null +++ b/src/geometry/triangle.rs @@ -0,0 +1,9 @@ +#[cfg(feature = "dim2")] +use crate::geometry::{CuboidFeatureFace, Triangle}; +#[cfg(feature = "dim2")] +use crate::math::Vector; + +#[cfg(feature = "dim2")] +pub fn support_face(_triangle: &Triangle, _local_dir: Vector) -> CuboidFeatureFace { + unimplemented!() +} diff --git a/src/geometry/trimesh.rs b/src/geometry/trimesh.rs new file mode 100644 index 000000000..62731b6d6 --- /dev/null +++ b/src/geometry/trimesh.rs @@ -0,0 +1,122 @@ +use crate::geometry::{Triangle, WAABBHierarchy}; +use crate::math::{Isometry, Point}; +use na::Point3; +use ncollide::bounding_volume::{HasBoundingVolume, AABB}; + +#[derive(Clone)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +/// A triangle mesh. +pub struct Trimesh { + waabb_tree: WAABBHierarchy, + aabb: AABB, + vertices: Vec>, + indices: Vec>, +} + +impl Trimesh { + /// Creates a new triangle mesh from a vertex buffer and an index buffer. + pub fn new(vertices: Vec>, indices: Vec>) -> Self { + assert!( + vertices.len() > 1, + "A triangle mesh must contain at least one point." + ); + assert!( + indices.len() > 1, + "A triangle mesh must contain at least one triangle." + ); + + // z-sort the indices. + // indices.sort_unstable_by(|idx, jdx| { + // let ti = Triangle::new( + // vertices[idx[0] as usize], + // vertices[idx[1] as usize], + // vertices[idx[2] as usize], + // ); + // let tj = Triangle::new( + // vertices[jdx[0] as usize], + // vertices[jdx[1] as usize], + // vertices[jdx[2] as usize], + // ); + // let center_i = (ti.a.coords + ti.b.coords + ti.c.coords) / 3.0; + // let center_j = (tj.a.coords + tj.b.coords + tj.c.coords) / 3.0; + // crate::geometry::z_cmp_floats(center_i.as_slice(), center_j.as_slice()) + // .unwrap_or(std::cmp::Ordering::Equal) + // }); + let aabb = AABB::from_points(&vertices); + + let aabbs: Vec<_> = indices + .iter() + .map(|idx| { + Triangle::new( + vertices[idx[0] as usize], + vertices[idx[1] as usize], + vertices[idx[2] as usize], + ) + .local_bounding_volume() + }) + .collect(); + + let waabb_tree = WAABBHierarchy::new(&aabbs); + + Self { + waabb_tree, + aabb, + vertices, + indices, + } + } + + /// Compute the axis-aligned bounding box of this triangle mesh. + pub fn aabb(&self, pos: &Isometry) -> AABB { + self.aabb.transform_by(pos) + } + + pub(crate) fn waabbs(&self) -> &WAABBHierarchy { + &self.waabb_tree + } + + /// The number of triangles forming this mesh. + pub fn num_triangles(&self) -> usize { + self.indices.len() + } + + /// An iterator through all the triangles of this mesh. + pub fn triangles(&self) -> impl Iterator + '_ { + self.indices.iter().map(move |ids| { + Triangle::new( + self.vertices[ids.x as usize], + self.vertices[ids.y as usize], + self.vertices[ids.z as usize], + ) + }) + } + + /// Get the `i`-th triangle of this mesh. + pub fn triangle(&self, i: usize) -> Triangle { + let idx = self.indices[i]; + Triangle::new( + self.vertices[idx.x as usize], + self.vertices[idx.y as usize], + self.vertices[idx.z as usize], + ) + } + + /// The vertex buffer of this mesh. + pub fn vertices(&self) -> &[Point] { + &self.vertices[..] + } + + /// The index buffer of this mesh. + pub fn indices(&self) -> &[Point3] { + &self.indices + } + + /// A flat view of the index buffer of this mesh. + pub fn flat_indices(&self) -> &[u32] { + unsafe { + let len = self.indices.len() * 3; + let data = self.indices.as_ptr() as *const u32; + std::slice::from_raw_parts(data, len) + } + } +} diff --git a/src/geometry/waabb.rs b/src/geometry/waabb.rs new file mode 100644 index 000000000..c3853bcf5 --- /dev/null +++ b/src/geometry/waabb.rs @@ -0,0 +1,116 @@ +#[cfg(feature = "serde-serialize")] +use crate::math::DIM; +use crate::math::{Point, SimdBool, SimdFloat, SIMD_WIDTH}; +use ncollide::bounding_volume::AABB; +use simba::simd::{SimdPartialOrd, SimdValue}; + +#[derive(Debug, Copy, Clone)] +pub(crate) struct WAABB { + pub mins: Point, + pub maxs: Point, +} + +#[cfg(feature = "serde-serialize")] +impl serde::Serialize for WAABB { + fn serialize(&self, serializer: S) -> Result + where + S: serde::Serializer, + { + use serde::ser::SerializeStruct; + + let mins: Point<[f32; SIMD_WIDTH]> = Point::from( + self.mins + .coords + .map(|e| array![|ii| e.extract(ii); SIMD_WIDTH]), + ); + let maxs: Point<[f32; SIMD_WIDTH]> = Point::from( + self.maxs + .coords + .map(|e| array![|ii| e.extract(ii); SIMD_WIDTH]), + ); + let mut waabb = serializer.serialize_struct("WAABB", 2)?; + waabb.serialize_field("mins", &mins)?; + waabb.serialize_field("maxs", &maxs)?; + waabb.end() + } +} + +#[cfg(feature = "serde-serialize")] +impl<'de> serde::Deserialize<'de> for WAABB { + fn deserialize(deserializer: D) -> Result + where + D: serde::Deserializer<'de>, + { + struct Visitor {}; + impl<'de> serde::de::Visitor<'de> for Visitor { + type Value = WAABB; + fn expecting(&self, formatter: &mut std::fmt::Formatter) -> std::fmt::Result { + write!( + formatter, + "two arrays containing at least {} floats", + SIMD_WIDTH * DIM * 2 + ) + } + + fn visit_seq(self, mut seq: A) -> Result + where + A: serde::de::SeqAccess<'de>, + { + let mins: Point<[f32; SIMD_WIDTH]> = seq + .next_element()? + .ok_or_else(|| serde::de::Error::invalid_length(0, &self))?; + let maxs: Point<[f32; SIMD_WIDTH]> = seq + .next_element()? + .ok_or_else(|| serde::de::Error::invalid_length(1, &self))?; + let mins = Point::from(mins.coords.map(|e| SimdFloat::from(e))); + let maxs = Point::from(maxs.coords.map(|e| SimdFloat::from(e))); + Ok(WAABB { mins, maxs }) + } + } + + deserializer.deserialize_struct("WAABB", &["mins", "maxs"], Visitor {}) + } +} + +impl WAABB { + pub fn new(mins: Point, maxs: Point) -> Self { + Self { mins, maxs } + } + + pub fn splat(aabb: AABB) -> Self { + Self { + mins: Point::splat(aabb.mins), + maxs: Point::splat(aabb.maxs), + } + } + + #[cfg(feature = "dim2")] + pub fn intersects_lanewise(&self, other: &WAABB) -> SimdBool { + self.mins.x.simd_le(other.maxs.x) + & other.mins.x.simd_le(self.maxs.x) + & self.mins.y.simd_le(other.maxs.y) + & other.mins.y.simd_le(self.maxs.y) + } + + #[cfg(feature = "dim3")] + pub fn intersects_lanewise(&self, other: &WAABB) -> SimdBool { + self.mins.x.simd_le(other.maxs.x) + & other.mins.x.simd_le(self.maxs.x) + & self.mins.y.simd_le(other.maxs.y) + & other.mins.y.simd_le(self.maxs.y) + & self.mins.z.simd_le(other.maxs.z) + & other.mins.z.simd_le(self.maxs.z) + } +} + +impl From<[AABB; SIMD_WIDTH]> for WAABB { + fn from(aabbs: [AABB; SIMD_WIDTH]) -> Self { + let mins = array![|ii| aabbs[ii].mins; SIMD_WIDTH]; + let maxs = array![|ii| aabbs[ii].maxs; SIMD_WIDTH]; + + WAABB { + mins: Point::from(mins), + maxs: Point::from(maxs), + } + } +} diff --git a/src/geometry/z_order.rs b/src/geometry/z_order.rs new file mode 100644 index 000000000..669354768 --- /dev/null +++ b/src/geometry/z_order.rs @@ -0,0 +1,70 @@ +use num_traits::float::FloatCore; +use std::cmp::Ordering; + +#[allow(dead_code)] // We don't use this currently, but migth in the future. +pub fn z_cmp_ints(lhs: &[usize], rhs: &[usize]) -> Ordering { + assert_eq!( + lhs.len(), + rhs.len(), + "Cannot compare array with different lengths." + ); + + let mut msd = 0; + + for dim in 1..rhs.len() { + if less_msb(lhs[msd] ^ rhs[msd], lhs[dim] ^ rhs[dim]) { + msd = dim; + } + } + + lhs[msd].cmp(&rhs[msd]) +} + +fn less_msb(x: usize, y: usize) -> bool { + x < y && x < (x ^ y) +} + +// Fast construction of k-Nearest Neighbor Graphs for Point Clouds +// Michael Connor, Piyush Kumar +// Algorithm 1 +// +// http://compgeom.com/~piyush/papers/tvcg_stann.pdf +pub fn z_cmp_floats(p1: &[f32], p2: &[f32]) -> Option { + assert_eq!( + p1.len(), + p2.len(), + "Cannot compare array with different lengths." + ); + let mut x = 0; + let mut dim = 0; + + for j in 0..p1.len() { + let y = xor_msb_float(p1[j], p2[j]); + if x < y { + x = y; + dim = j; + } + } + + p1[dim].partial_cmp(&p2[dim]) +} + +fn xor_msb_float(fa: f32, fb: f32) -> i16 { + let (mantissa1, exponent1, _sign1) = fa.integer_decode(); + let (mantissa2, exponent2, _sign2) = fb.integer_decode(); + let x = exponent1; // To keep the same notation as the paper. + let y = exponent2; // To keep the same notation as the paper. + + if x == y { + let z = msdb(mantissa1, mantissa2); + x - z + } else if y < x { + x + } else { + y + } +} + +fn msdb(x: u64, y: u64) -> i16 { + 64i16 - (x ^ y).leading_zeros() as i16 +} diff --git a/src/lib.rs b/src/lib.rs new file mode 100644 index 000000000..aae0bd024 --- /dev/null +++ b/src/lib.rs @@ -0,0 +1,255 @@ +//! # Rapier +//! +//! Rapier is a set of two Rust crates `rapier2d` and `rapier3d` for efficient cross-platform +//! physics simulation. It target application include video games, animation, robotics, etc. +//! +//! Rapier has some unique features for collaborative applications: +//! - The ability to snapshot the state of the physics engine, and restore it later. +//! - The ability to run a perfectly deterministic simulation on different machine, as long as they +//! are compliant with the IEEE 754-2008 floating point standard. + +#![deny(missing_docs)] + +pub extern crate nalgebra as na; +#[cfg(feature = "dim2")] +pub extern crate ncollide2d as ncollide; +#[cfg(feature = "dim3")] +pub extern crate ncollide3d as ncollide; +#[cfg(feature = "serde")] +#[macro_use] +extern crate serde; +extern crate num_traits as num; +// #[macro_use] +// extern crate array_macro; + +#[cfg(feature = "parallel")] +pub use rayon; + +#[cfg(all( + feature = "simd-is-enabled", + not(feature = "simd-stable"), + not(feature = "simd-nightly") +))] +std::compile_error!("The `simd-is-enabled` feature should not be enabled explicitly. Please enable the `simd-stable` or the `simd-nightly` feature instead."); +#[cfg(all(feature = "simd-is-enabled", feature = "enhanced-determinism"))] +std::compile_error!( + "SIMD cannot be enabled when the `enhanced-determinism` feature is also enabled." +); + +macro_rules! enable_flush_to_zero( + () => { + let _flush_to_zero = crate::utils::FlushToZeroDenormalsAreZeroFlags::flush_denormal_to_zero(); + } +); + +#[cfg(feature = "simd-is-enabled")] +macro_rules! array( + ($callback: expr; SIMD_WIDTH) => { + { + #[inline(always)] + #[allow(dead_code)] + fn create_arr(mut callback: impl FnMut(usize) -> T) -> [T; SIMD_WIDTH] { + [callback(0usize), callback(1usize), callback(2usize), callback(3usize)] + + // [callback(0usize), callback(1usize), callback(2usize), callback(3usize), + // callback(4usize), callback(5usize), callback(6usize), callback(7usize)] + + // [callback(0usize), callback(1usize), callback(2usize), callback(3usize), + // callback(4usize), callback(5usize), callback(6usize), callback(7usize), + // callback(8usize), callback(9usize), callback(10usize), callback(11usize), + // callback(12usize), callback(13usize), callback(14usize), callback(15usize)] + } + + create_arr($callback) + } + } +); + +#[allow(unused_macros)] +macro_rules! par_iter { + ($t: expr) => {{ + #[cfg(not(feature = "parallel"))] + let it = $t.iter(); + + #[cfg(feature = "parallel")] + let it = $t.par_iter(); + it + }}; +} + +macro_rules! par_iter_mut { + ($t: expr) => {{ + #[cfg(not(feature = "parallel"))] + let it = $t.iter_mut(); + + #[cfg(feature = "parallel")] + let it = $t.par_iter_mut(); + it + }}; +} + +// macro_rules! par_chunks_mut { +// ($t: expr, $sz: expr) => {{ +// #[cfg(not(feature = "parallel"))] +// let it = $t.chunks_mut($sz); +// +// #[cfg(feature = "parallel")] +// let it = $t.par_chunks_mut($sz); +// it +// }}; +// } + +#[allow(unused_macros)] +macro_rules! try_ret { + ($val: expr) => { + try_ret!($val, ()) + }; + ($val: expr, $ret: expr) => { + if let Some(val) = $val { + val + } else { + return $ret; + } + }; +} + +// macro_rules! try_continue { +// ($val: expr) => { +// if let Some(val) = $val { +// val +// } else { +// continue; +// } +// }; +// } + +pub(crate) const INVALID_U32: u32 = u32::MAX; +pub(crate) const INVALID_U64: u64 = u64::MAX; +pub(crate) const INVALID_USIZE: usize = INVALID_U32 as usize; + +pub mod counters; +pub mod data; +pub mod dynamics; +pub mod geometry; +pub mod pipeline; +pub mod utils; + +#[cfg(feature = "dim2")] +/// Math primitives used throughout Rapier. +pub mod math { + #[cfg(feature = "simd-is-enabled")] + pub use super::simd::*; + use na::{Isometry2, Matrix2, Point2, Translation2, UnitComplex, Vector2, Vector3, U1, U2}; + + /// The dimension of the physics simulated by this crate. + pub const DIM: usize = 2; + /// The maximum number of point a contact manifold can hold. + pub const MAX_MANIFOLD_POINTS: usize = 2; + /// The dimension of the physics simulated by this crate, given as a type-level-integer. + pub type Dim = U2; + /// The maximum number of angular degrees of freedom of a rigid body given as a type-level-integer. + pub type AngDim = U1; + /// A 2D isometry, i.e., a rotation followed by a translation. + pub type Isometry = Isometry2; + /// A 2D vector. + pub type Vector = Vector2; + /// A scalar used for angular velocity. + /// + /// This is called `AngVector` for coherence with the 3D version of this crate. + pub type AngVector = N; + /// A 2D point. + pub type Point = Point2; + /// A 2D rotation expressed as an unit complex number. + pub type Rotation = UnitComplex; + /// A 2D translation. + pub type Translation = Translation2; + /// The angular inertia of a rigid body. + pub type AngularInertia = N; + /// The principal angular inertia of a rigid body. + pub type PrincipalAngularInertia = N; + /// A matrix that represent the cross product with a given vector. + pub type CrossMatrix = Vector2; + /// A 2x2 matrix. + pub type Matrix = Matrix2; + /// A vector with a dimension equal to the maximum number of degrees of freedom of a rigid body. + pub type SpacialVector = Vector3; + /// A 2D symmetric-definite-positive matrix. + pub type SdpMatrix = crate::utils::SdpMatrix2; +} + +#[cfg(feature = "dim3")] +/// Math primitives used throughout Rapier. +pub mod math { + #[cfg(feature = "simd-is-enabled")] + pub use super::simd::*; + use na::{Isometry3, Matrix3, Point3, Translation3, UnitQuaternion, Vector3, Vector6, U3}; + + /// The dimension of the physics simulated by this crate. + pub const DIM: usize = 3; + /// The maximum number of point a contact manifold can hold. + pub const MAX_MANIFOLD_POINTS: usize = 4; + /// The dimension of the physics simulated by this crate, given as a type-level-integer. + pub type Dim = U3; + /// The maximum number of angular degrees of freedom of a rigid body given as a type-level-integer. + pub type AngDim = U3; + /// A 3D isometry, i.e., a rotation followed by a translation. + pub type Isometry = Isometry3; + /// A 3D vector. + pub type Vector = Vector3; + /// An axis-angle vector used for angular velocity. + pub type AngVector = Vector3; + /// A 3D point. + pub type Point = Point3; + /// A 3D rotation expressed as an unit quaternion. + pub type Rotation = UnitQuaternion; + /// A 3D translation. + pub type Translation = Translation3; + /// The angular inertia of a rigid body. + pub type AngularInertia = crate::utils::SdpMatrix3; + /// The principal angular inertia of a rigid body. + pub type PrincipalAngularInertia = Vector3; + /// A matrix that represent the cross product with a given vector. + pub type CrossMatrix = Matrix3; + /// A 3x3 matrix. + pub type Matrix = Matrix3; + /// A vector with a dimension equal to the maximum number of degrees of freedom of a rigid body. + pub type SpacialVector = Vector6; + /// A 3D symmetric-definite-positive matrix. + pub type SdpMatrix = crate::utils::SdpMatrix3; +} + +#[cfg(feature = "simd-is-enabled")] +mod simd { + #[allow(unused_imports)] + #[cfg(feature = "simd-nightly")] + use simba::simd::{f32x16, f32x4, f32x8, m32x16, m32x4, m32x8, u8x16, u8x4, u8x8}; + #[cfg(feature = "simd-stable")] + use simba::simd::{WideBoolF32x4, WideF32x4}; + + /// The number of lanes of a SIMD number. + pub const SIMD_WIDTH: usize = 4; + /// SIMD_WIDTH - 1 + pub const SIMD_LAST_INDEX: usize = 3; + #[cfg(not(feature = "simd-nightly"))] + /// A SIMD float with SIMD_WIDTH lanes. + pub type SimdFloat = WideF32x4; + #[cfg(not(feature = "simd-nightly"))] + /// A SIMD bool with SIMD_WIDTH lanes. + pub type SimdBool = WideBoolF32x4; + #[cfg(feature = "simd-nightly")] + /// A SIMD float with SIMD_WIDTH lanes. + pub type SimdFloat = f32x4; + #[cfg(feature = "simd-nightly")] + /// A bool float with SIMD_WIDTH lanes. + pub type SimdBool = m32x4; + + // pub const SIMD_WIDTH: usize = 8; + // pub const SIMD_LAST_INDEX: usize = 7; + // pub type SimdFloat = f32x8; + // pub type SimdBool = m32x8; + + // pub const SIMD_WIDTH: usize = 16; + // pub const SIMD_LAST_INDEX: usize = 15; + // pub type SimdFloat = f32x16; + // pub type SimdBool = m32x16; +} diff --git a/src/pipeline/collision_pipeline.rs b/src/pipeline/collision_pipeline.rs new file mode 100644 index 000000000..018429515 --- /dev/null +++ b/src/pipeline/collision_pipeline.rs @@ -0,0 +1,111 @@ +//! Physics pipeline structures. + +use crate::dynamics::{JointSet, RigidBody, RigidBodyHandle, RigidBodySet}; +use crate::geometry::{BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, NarrowPhase}; +use crate::pipeline::EventHandler; + +/// The collision pipeline, responsible for performing collision detection between colliders. +/// +/// This structure only contains temporary data buffers. It can be dropped and replaced by a fresh +/// copy at any time. For performance reasons it is recommended to reuse the same physics pipeline +/// instance to benefit from the cached data. +// NOTE: this contains only workspace data, so there is no point in making this serializable. +pub struct CollisionPipeline { + broadphase_collider_pairs: Vec, + broad_phase_events: Vec, + empty_joints: JointSet, +} + +#[allow(dead_code)] +fn check_pipeline_send_sync() { + fn do_test() {} + do_test::(); +} + +impl CollisionPipeline { + /// Initializes a new physics pipeline. + pub fn new() -> CollisionPipeline { + CollisionPipeline { + broadphase_collider_pairs: Vec::new(), + broad_phase_events: Vec::new(), + empty_joints: JointSet::new(), + } + } + + /// Executes one step of the collision detection. + pub fn step( + &mut self, + prediction_distance: f32, + broad_phase: &mut BroadPhase, + narrow_phase: &mut NarrowPhase, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + events: &dyn EventHandler, + ) { + bodies.maintain_active_set(); + self.broadphase_collider_pairs.clear(); + + broad_phase.update_aabbs(prediction_distance, bodies, colliders); + + self.broad_phase_events.clear(); + broad_phase.find_pairs(&mut self.broad_phase_events); + + narrow_phase.register_pairs(colliders, &self.broad_phase_events, events); + + narrow_phase.compute_contacts(prediction_distance, bodies, colliders, events); + narrow_phase.compute_proximities(prediction_distance, bodies, colliders, events); + + bodies.update_active_set_with_contacts( + colliders, + narrow_phase.contact_graph(), + self.empty_joints.joint_graph(), + 0, + ); + + // // Update kinematic bodies velocities. + // bodies.foreach_active_kinematic_body_mut_internal(|_, body| { + // body.compute_velocity_from_predicted_position(integration_parameters.inv_dt()); + // }); + + // Update colliders positions and kinematic bodies positions. + bodies.foreach_active_body_mut_internal(|_, rb| { + if rb.is_kinematic() { + rb.position = rb.predicted_position; + } else { + rb.update_predicted_position(0.0); + } + + for handle in &rb.colliders { + let collider = &mut colliders[*handle]; + collider.position = rb.position * collider.delta; + collider.predicted_position = rb.predicted_position * collider.delta; + } + }); + + bodies.modified_inactive_set.clear(); + } + + /// Remove a rigid-body and all its associated data. + pub fn remove_rigid_body( + &mut self, + handle: RigidBodyHandle, + broad_phase: &mut BroadPhase, + narrow_phase: &mut NarrowPhase, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + ) -> Option { + // Remove the body. + let body = bodies.remove_internal(handle)?; + + // Remove this rigid-body from the broad-phase and narrow-phase. + broad_phase.remove_colliders(&body.colliders, colliders); + narrow_phase.remove_colliders(&body.colliders, colliders, bodies); + + // Remove all colliders attached to this body. + for collider in &body.colliders { + colliders.remove_internal(*collider); + } + + Some(body) + } +} diff --git a/src/pipeline/event_handler.rs b/src/pipeline/event_handler.rs new file mode 100644 index 000000000..67e4a788d --- /dev/null +++ b/src/pipeline/event_handler.rs @@ -0,0 +1,51 @@ +use crate::geometry::{ContactEvent, ProximityEvent}; +use crossbeam::channel::Sender; + +/// Trait implemented by structures responsible for handling events generated by the physics engine. +/// +/// Implementors of this trait will typically collect these events for future processing. +pub trait EventHandler: Send + Sync { + /// Handle a proximity event. + /// + /// A proximity event is emitted when the state of proximity between two colliders changes. + fn handle_proximity_event(&self, event: ProximityEvent); + /// Handle a contact event. + /// + /// A contact event is emitted when two collider start or stop touching, independently from the + /// number of contact points involved. + fn handle_contact_event(&self, event: ContactEvent); +} + +impl EventHandler for () { + fn handle_proximity_event(&self, _event: ProximityEvent) {} + fn handle_contact_event(&self, _event: ContactEvent) {} +} + +/// A physics event handler that collects events into a crossbeam channel. +pub struct ChannelEventCollector { + proximity_event_sender: Sender, + contact_event_sender: Sender, +} + +impl ChannelEventCollector { + /// Initialize a new physics event handler from crossbeam channel senders. + pub fn new( + proximity_event_sender: Sender, + contact_event_sender: Sender, + ) -> Self { + Self { + proximity_event_sender, + contact_event_sender, + } + } +} + +impl EventHandler for ChannelEventCollector { + fn handle_proximity_event(&self, event: ProximityEvent) { + let _ = self.proximity_event_sender.send(event); + } + + fn handle_contact_event(&self, event: ContactEvent) { + let _ = self.contact_event_sender.send(event); + } +} diff --git a/src/pipeline/mod.rs b/src/pipeline/mod.rs new file mode 100644 index 000000000..6298d1845 --- /dev/null +++ b/src/pipeline/mod.rs @@ -0,0 +1,9 @@ +//! Structure for combining the various physics components to perform an actual simulation. + +pub use collision_pipeline::CollisionPipeline; +pub use event_handler::{ChannelEventCollector, EventHandler}; +pub use physics_pipeline::PhysicsPipeline; + +mod collision_pipeline; +mod event_handler; +mod physics_pipeline; diff --git a/src/pipeline/physics_pipeline.rs b/src/pipeline/physics_pipeline.rs new file mode 100644 index 000000000..6e18a0340 --- /dev/null +++ b/src/pipeline/physics_pipeline.rs @@ -0,0 +1,332 @@ +//! Physics pipeline structures. + +use crate::counters::Counters; +#[cfg(not(feature = "parallel"))] +use crate::dynamics::IslandSolver; +use crate::dynamics::{IntegrationParameters, JointSet, RigidBody, RigidBodyHandle, RigidBodySet}; +#[cfg(feature = "parallel")] +use crate::dynamics::{JointGraphEdge, ParallelIslandSolver as IslandSolver}; +use crate::geometry::{ + BroadPhase, BroadPhasePairEvent, ColliderPair, ColliderSet, ContactManifoldIndex, NarrowPhase, +}; +use crate::math::Vector; +use crate::pipeline::EventHandler; + +/// The physics pipeline, responsible for stepping the whole physics simulation. +/// +/// This structure only contains temporary data buffers. It can be dropped and replaced by a fresh +/// copy at any time. For performance reasons it is recommended to reuse the same physics pipeline +/// instance to benefit from the cached data. +/// +/// Rapier relies on a time-stepping scheme. Its force computations +/// uses two solvers: +/// - A velocity based solver based on PGS which computes forces for contact and joint constraints. +/// - A position based solver based on non-linear PGS which performs constraint stabilization (i.e. correction of errors like penetrations). +// NOTE: this contains only workspace data, so there is no point in making this serializable. +pub struct PhysicsPipeline { + /// Counters used for benchmarking only. + pub counters: Counters, + manifold_indices: Vec>, + joint_constraint_indices: Vec>, + broadphase_collider_pairs: Vec, + broad_phase_events: Vec, + solvers: Vec, +} + +impl Default for PhysicsPipeline { + fn default() -> Self { + PhysicsPipeline::new() + } +} + +#[allow(dead_code)] +fn check_pipeline_send_sync() { + fn do_test() {} + do_test::(); +} + +impl PhysicsPipeline { + /// Initializes a new physics pipeline. + pub fn new() -> PhysicsPipeline { + PhysicsPipeline { + counters: Counters::new(false), + solvers: Vec::new(), + manifold_indices: Vec::new(), + joint_constraint_indices: Vec::new(), + broadphase_collider_pairs: Vec::new(), + broad_phase_events: Vec::new(), + } + } + + /// Executes one timestep of the physics simulation. + pub fn step( + &mut self, + gravity: &Vector, + integration_parameters: &IntegrationParameters, + broad_phase: &mut BroadPhase, + narrow_phase: &mut NarrowPhase, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + joints: &mut JointSet, + events: &dyn EventHandler, + ) { + // println!("Step"); + self.counters.step_started(); + bodies.maintain_active_set(); + + self.counters.stages.collision_detection_time.start(); + self.counters.cd.broad_phase_time.start(); + self.broadphase_collider_pairs.clear(); + // let t = instant::now(); + broad_phase.update_aabbs( + integration_parameters.prediction_distance, + bodies, + colliders, + ); + // println!("Update AABBs time: {}", instant::now() - t); + + // let t = instant::now(); + self.broad_phase_events.clear(); + broad_phase.find_pairs(&mut self.broad_phase_events); + // println!("Find pairs time: {}", instant::now() - t); + + narrow_phase.register_pairs(colliders, &self.broad_phase_events, events); + self.counters.cd.broad_phase_time.pause(); + + // println!("Num contact pairs: {}", pairs.len()); + + self.counters.cd.narrow_phase_time.start(); + + // let t = instant::now(); + narrow_phase.compute_contacts( + integration_parameters.prediction_distance, + bodies, + colliders, + events, + ); + narrow_phase.compute_proximities( + integration_parameters.prediction_distance, + bodies, + colliders, + events, + ); + // println!("Compute contact time: {}", instant::now() - t); + + self.counters.stages.island_construction_time.start(); + bodies.update_active_set_with_contacts( + colliders, + narrow_phase.contact_graph(), + joints.joint_graph(), + integration_parameters.min_island_size, + ); + self.counters.stages.island_construction_time.pause(); + + // Update kinematic bodies velocities. + bodies.foreach_active_kinematic_body_mut_internal(|_, body| { + body.compute_velocity_from_predicted_position(integration_parameters.inv_dt()); + }); + + if self.manifold_indices.len() < bodies.num_islands() { + self.manifold_indices + .resize(bodies.num_islands(), Vec::new()); + } + + if self.joint_constraint_indices.len() < bodies.num_islands() { + self.joint_constraint_indices + .resize(bodies.num_islands(), Vec::new()); + } + + let mut manifolds = Vec::new(); + narrow_phase.sort_and_select_active_contacts( + bodies, + &mut manifolds, + &mut self.manifold_indices, + ); + joints.select_active_interactions(bodies, &mut self.joint_constraint_indices); + + self.counters.cd.narrow_phase_time.pause(); + self.counters.stages.collision_detection_time.pause(); + + self.counters.stages.update_time.start(); + bodies.foreach_active_dynamic_body_mut_internal(|_, b| { + b.update_world_mass_properties(); + b.integrate_accelerations(integration_parameters.dt(), *gravity) + }); + self.counters.stages.update_time.pause(); + + self.counters.solver.reset(); + self.counters.stages.solver_time.start(); + if self.solvers.len() < bodies.num_islands() { + self.solvers + .resize_with(bodies.num_islands(), || IslandSolver::new()); + } + + #[cfg(not(feature = "parallel"))] + { + enable_flush_to_zero!(); + + for island_id in 0..bodies.num_islands() { + self.solvers[island_id].solve_island( + island_id, + &mut self.counters, + integration_parameters, + bodies, + &mut manifolds[..], + &self.manifold_indices[island_id], + joints.joints_mut(), + &self.joint_constraint_indices[island_id], + ) + } + } + + #[cfg(feature = "parallel")] + { + use crate::geometry::ContactManifold; + use rayon::prelude::*; + use std::sync::atomic::Ordering; + + let num_islands = bodies.num_islands(); + let solvers = &mut self.solvers[..num_islands]; + let bodies = &std::sync::atomic::AtomicPtr::new(bodies as *mut _); + let manifolds = &std::sync::atomic::AtomicPtr::new(&mut manifolds as *mut _); + let joints = &std::sync::atomic::AtomicPtr::new(joints.joints_vec_mut() as *mut _); + let manifold_indices = &self.manifold_indices[..]; + let joint_constraint_indices = &self.joint_constraint_indices[..]; + + rayon::scope(|scope| { + enable_flush_to_zero!(); + + solvers + .par_iter_mut() + .enumerate() + .for_each(|(island_id, solver)| { + let bodies: &mut RigidBodySet = + unsafe { std::mem::transmute(bodies.load(Ordering::Relaxed)) }; + let manifolds: &mut Vec<&mut ContactManifold> = + unsafe { std::mem::transmute(manifolds.load(Ordering::Relaxed)) }; + let joints: &mut Vec = + unsafe { std::mem::transmute(joints.load(Ordering::Relaxed)) }; + + solver.solve_island( + scope, + island_id, + integration_parameters, + bodies, + manifolds, + &manifold_indices[island_id], + joints, + &joint_constraint_indices[island_id], + ) + }); + }); + } + + // Update colliders positions and kinematic bodies positions. + // FIXME: do this in the solver? + bodies.foreach_active_body_mut_internal(|_, rb| { + if rb.is_kinematic() { + rb.position = rb.predicted_position; + rb.linvel = na::zero(); + rb.angvel = na::zero(); + } else { + rb.update_predicted_position(integration_parameters.dt()); + } + + for handle in &rb.colliders { + let collider = &mut colliders[*handle]; + collider.position = rb.position * collider.delta; + collider.predicted_position = rb.predicted_position * collider.delta; + } + }); + + self.counters.stages.solver_time.pause(); + + bodies.modified_inactive_set.clear(); + self.counters.step_completed(); + } + + /// Remove a rigid-body and all its associated data. + pub fn remove_rigid_body( + &mut self, + handle: RigidBodyHandle, + broad_phase: &mut BroadPhase, + narrow_phase: &mut NarrowPhase, + bodies: &mut RigidBodySet, + colliders: &mut ColliderSet, + joints: &mut JointSet, + ) -> Option { + // Remove the body. + let body = bodies.remove_internal(handle)?; + + // Remove this rigid-body from the broad-phase and narrow-phase. + broad_phase.remove_colliders(&body.colliders, colliders); + narrow_phase.remove_colliders(&body.colliders, colliders, bodies); + + // Remove all joints attached to this body. + joints.remove_rigid_body(body.joint_graph_index, bodies); + + // Remove all colliders attached to this body. + for collider in &body.colliders { + colliders.remove_internal(*collider); + } + + Some(body) + } +} + +#[cfg(test)] +mod test { + use crate::dynamics::{JointSet, RigidBodyBuilder, RigidBodySet}; + use crate::geometry::{BroadPhase, ColliderSet, NarrowPhase}; + use crate::pipeline::PhysicsPipeline; + + #[test] + fn rigid_body_removal_before_step() { + let mut colliders = ColliderSet::new(); + let mut joints = JointSet::new(); + let mut pipeline = PhysicsPipeline::new(); + let mut bf = BroadPhase::new(); + let mut nf = NarrowPhase::new(); + + let mut set = RigidBodySet::new(); + let rb = RigidBodyBuilder::new_dynamic().build(); + + // Check that removing the body right after inserting it works. + let h1 = set.insert(rb.clone()); + pipeline.remove_rigid_body(h1, &mut bf, &mut nf, &mut set, &mut colliders, &mut joints); + } + + #[test] + fn rigid_body_removal_snapshot_handle_determinism() { + let mut colliders = ColliderSet::new(); + let mut joints = JointSet::new(); + let mut pipeline = PhysicsPipeline::new(); + let mut bf = BroadPhase::new(); + let mut nf = NarrowPhase::new(); + + let mut set = RigidBodySet::new(); + let rb = RigidBodyBuilder::new_dynamic().build(); + let h1 = set.insert(rb.clone()); + let h2 = set.insert(rb.clone()); + let h3 = set.insert(rb.clone()); + + pipeline.remove_rigid_body(h1, &mut bf, &mut nf, &mut set, &mut colliders, &mut joints); + pipeline.remove_rigid_body(h3, &mut bf, &mut nf, &mut set, &mut colliders, &mut joints); + pipeline.remove_rigid_body(h2, &mut bf, &mut nf, &mut set, &mut colliders, &mut joints); + + let ser_set = bincode::serialize(&set).unwrap(); + let mut set2: RigidBodySet = bincode::deserialize(&ser_set).unwrap(); + + let h1a = set.insert(rb.clone()); + let h2a = set.insert(rb.clone()); + let h3a = set.insert(rb.clone()); + + let h1b = set2.insert(rb.clone()); + let h2b = set2.insert(rb.clone()); + let h3b = set2.insert(rb.clone()); + + assert_eq!(h1a, h1b); + assert_eq!(h2a, h2b); + assert_eq!(h3a, h3b); + } +} diff --git a/src/utils.rs b/src/utils.rs new file mode 100644 index 000000000..91ce41ce0 --- /dev/null +++ b/src/utils.rs @@ -0,0 +1,1333 @@ +//! Miscellaneous utilities. + +use crate::dynamics::RigidBodyHandle; +#[cfg(all(feature = "enhanced-determinism", feature = "serde-serialize"))] +use indexmap::IndexMap as HashMap; +use na::{Matrix2, Matrix3, Matrix3x2, Point2, Point3, Scalar, SimdRealField, Vector2, Vector3}; +use num::Zero; +#[cfg(feature = "simd-is-enabled")] +use simba::simd::SimdValue; +#[cfg(all(not(feature = "enhanced-determinism"), feature = "serde-serialize"))] +use std::collections::HashMap; +use std::ops::{Add, Mul}; +#[cfg(feature = "simd-is-enabled")] +use {crate::simd::SimdFloat, na::SimdPartialOrd, num::One}; + +// pub(crate) const SIN_10_DEGREES: f32 = 0.17364817766; +// pub(crate) const COS_10_DEGREES: f32 = 0.98480775301; +// pub(crate) const COS_45_DEGREES: f32 = 0.70710678118; +// pub(crate) const SIN_45_DEGREES: f32 = COS_45_DEGREES; +pub(crate) const COS_5_DEGREES: f32 = 0.99619469809; +#[cfg(feature = "dim2")] +pub(crate) const COS_FRAC_PI_8: f32 = 0.92387953251; +#[cfg(feature = "dim2")] +pub(crate) const SIN_FRAC_PI_8: f32 = 0.38268343236; + +pub(crate) fn inv(val: f32) -> f32 { + if val == 0.0 { + 0.0 + } else { + 1.0 / val + } +} + +/// Trait to copy the sign of each component of one scalar/vector/matrix to another. +pub trait WSign: Sized { + // See SIMD implementations of copy_sign there: https://stackoverflow.com/a/57872652 + /// Copy the sign of each component of `self` to the corresponding component of `to`. + fn copy_sign_to(self, to: Rhs) -> Rhs; +} + +impl WSign for f32 { + fn copy_sign_to(self, to: Self) -> Self { + let signbit: u32 = (-0.0f32).to_bits(); + f32::from_bits((signbit & self.to_bits()) | ((!signbit) & to.to_bits())) + } +} + +impl> WSign> for N { + fn copy_sign_to(self, to: Vector2) -> Vector2 { + Vector2::new(self.copy_sign_to(to.x), self.copy_sign_to(to.y)) + } +} + +impl> WSign> for N { + fn copy_sign_to(self, to: Vector3) -> Vector3 { + Vector3::new( + self.copy_sign_to(to.x), + self.copy_sign_to(to.y), + self.copy_sign_to(to.z), + ) + } +} + +impl> WSign> for Vector2 { + fn copy_sign_to(self, to: Vector2) -> Vector2 { + Vector2::new(self.x.copy_sign_to(to.x), self.y.copy_sign_to(to.y)) + } +} + +impl> WSign> for Vector3 { + fn copy_sign_to(self, to: Vector3) -> Vector3 { + Vector3::new( + self.x.copy_sign_to(to.x), + self.y.copy_sign_to(to.y), + self.z.copy_sign_to(to.z), + ) + } +} + +#[cfg(feature = "simd-is-enabled")] +impl WSign for SimdFloat { + fn copy_sign_to(self, to: SimdFloat) -> SimdFloat { + self.simd_copysign(to) + } +} + +pub(crate) trait WComponent: Sized { + type Element; + + fn min_component(self) -> Self::Element; + fn max_component(self) -> Self::Element; +} + +impl WComponent for f32 { + type Element = f32; + + fn min_component(self) -> Self::Element { + self + } + fn max_component(self) -> Self::Element { + self + } +} + +#[cfg(feature = "simd-is-enabled")] +impl WComponent for SimdFloat { + type Element = f32; + + fn min_component(self) -> Self::Element { + self.simd_horizontal_min() + } + fn max_component(self) -> Self::Element { + self.simd_horizontal_max() + } +} + +/// Trait to compute the orthonormal basis of a vector. +pub trait WBasis: Sized { + /// The type of the array of orthonormal vectors. + type Basis; + /// Computes the vectors which, when combined with `self`, form an orthonormal basis. + fn orthonormal_basis(self) -> Self::Basis; +} + +impl WBasis for Vector2 { + type Basis = [Vector2; 1]; + fn orthonormal_basis(self) -> [Vector2; 1] { + [Vector2::new(-self.y, self.x)] + } +} + +impl> WBasis for Vector3 { + type Basis = [Vector3; 2]; + // Robust and branchless implementation from Pixar: + // https://graphics.pixar.com/library/OrthonormalB/paper.pdf + fn orthonormal_basis(self) -> [Vector3; 2] { + let sign = self.z.copy_sign_to(N::one()); + let a = -N::one() / (sign + self.z); + let b = self.x * self.y * a; + + [ + Vector3::new( + N::one() + sign * self.x * self.x * a, + sign * b, + -sign * self.x, + ), + Vector3::new(b, sign + self.y * self.y * a, -self.y), + ] + } +} + +pub(crate) trait WVec: Sized { + type Element; + + fn horizontal_inf(&self) -> Self::Element; + fn horizontal_sup(&self) -> Self::Element; +} + +impl WVec for Vector2 +where + N::Element: Scalar, +{ + type Element = Vector2; + + fn horizontal_inf(&self) -> Self::Element { + Vector2::new(self.x.min_component(), self.y.min_component()) + } + + fn horizontal_sup(&self) -> Self::Element { + Vector2::new(self.x.max_component(), self.y.max_component()) + } +} + +impl WVec for Point2 +where + N::Element: Scalar, +{ + type Element = Point2; + + fn horizontal_inf(&self) -> Self::Element { + Point2::new(self.x.min_component(), self.y.min_component()) + } + + fn horizontal_sup(&self) -> Self::Element { + Point2::new(self.x.max_component(), self.y.max_component()) + } +} + +impl WVec for Vector3 +where + N::Element: Scalar, +{ + type Element = Vector3; + + fn horizontal_inf(&self) -> Self::Element { + Vector3::new( + self.x.min_component(), + self.y.min_component(), + self.z.min_component(), + ) + } + + fn horizontal_sup(&self) -> Self::Element { + Vector3::new( + self.x.max_component(), + self.y.max_component(), + self.z.max_component(), + ) + } +} + +impl WVec for Point3 +where + N::Element: Scalar, +{ + type Element = Point3; + + fn horizontal_inf(&self) -> Self::Element { + Point3::new( + self.x.min_component(), + self.y.min_component(), + self.z.min_component(), + ) + } + + fn horizontal_sup(&self) -> Self::Element { + Point3::new( + self.x.max_component(), + self.y.max_component(), + self.z.max_component(), + ) + } +} + +pub(crate) trait WCrossMatrix: Sized { + type CrossMat; + + fn gcross_matrix(self) -> Self::CrossMat; +} + +impl WCrossMatrix for Vector3 { + type CrossMat = Matrix3; + + #[inline] + #[rustfmt::skip] + fn gcross_matrix(self) -> Self::CrossMat { + Matrix3::new( + 0.0, -self.z, self.y, + self.z, 0.0, -self.x, + -self.y, self.x, 0.0, + ) + } +} + +impl WCrossMatrix for Vector2 { + type CrossMat = Vector2; + + #[inline] + fn gcross_matrix(self) -> Self::CrossMat { + Vector2::new(-self.y, self.x) + } +} + +pub(crate) trait WCross: Sized { + type Result; + fn gcross(&self, rhs: Rhs) -> Self::Result; +} + +impl WCross> for Vector3 { + type Result = Self; + + fn gcross(&self, rhs: Vector3) -> Self::Result { + self.cross(&rhs) + } +} + +impl WCross> for Vector2 { + type Result = f32; + + fn gcross(&self, rhs: Vector2) -> Self::Result { + self.x * rhs.y - self.y * rhs.x + } +} + +impl WCross> for f32 { + type Result = Vector2; + + fn gcross(&self, rhs: Vector2) -> Self::Result { + Vector2::new(-rhs.y * *self, rhs.x * *self) + } +} + +pub(crate) trait WDot: Sized { + type Result; + fn gdot(&self, rhs: Rhs) -> Self::Result; +} + +impl WDot> for Vector3 { + type Result = f32; + + fn gdot(&self, rhs: Vector3) -> Self::Result { + self.x * rhs.x + self.y * rhs.y + self.z * rhs.z + } +} + +impl WDot> for Vector2 { + type Result = f32; + + fn gdot(&self, rhs: Vector2) -> Self::Result { + self.x * rhs.x + self.y * rhs.y + } +} + +impl WDot for f32 { + type Result = f32; + + fn gdot(&self, rhs: f32) -> Self::Result { + *self * rhs + } +} + +#[cfg(feature = "simd-is-enabled")] +impl WCrossMatrix for Vector3 { + type CrossMat = Matrix3; + + #[inline] + #[rustfmt::skip] + fn gcross_matrix(self) -> Self::CrossMat { + Matrix3::new( + SimdFloat::zero(), -self.z, self.y, + self.z, SimdFloat::zero(), -self.x, + -self.y, self.x, SimdFloat::zero(), + ) + } +} + +#[cfg(feature = "simd-is-enabled")] +impl WCrossMatrix for Vector2 { + type CrossMat = Vector2; + + #[inline] + fn gcross_matrix(self) -> Self::CrossMat { + Vector2::new(-self.y, self.x) + } +} + +#[cfg(feature = "simd-is-enabled")] +impl WCross> for Vector3 { + type Result = Vector3; + + fn gcross(&self, rhs: Self) -> Self::Result { + self.cross(&rhs) + } +} + +#[cfg(feature = "simd-is-enabled")] +impl WCross> for SimdFloat { + type Result = Vector2; + + fn gcross(&self, rhs: Vector2) -> Self::Result { + Vector2::new(-rhs.y * *self, rhs.x * *self) + } +} + +#[cfg(feature = "simd-is-enabled")] +impl WCross> for Vector2 { + type Result = SimdFloat; + + fn gcross(&self, rhs: Self) -> Self::Result { + let yx = Vector2::new(rhs.y, rhs.x); + let prod = self.component_mul(&yx); + prod.x - prod.y + } +} + +#[cfg(feature = "simd-is-enabled")] +impl WDot> for Vector3 { + type Result = SimdFloat; + + fn gdot(&self, rhs: Vector3) -> Self::Result { + self.x * rhs.x + self.y * rhs.y + self.z * rhs.z + } +} + +#[cfg(feature = "simd-is-enabled")] +impl WDot> for Vector2 { + type Result = SimdFloat; + + fn gdot(&self, rhs: Vector2) -> Self::Result { + self.x * rhs.x + self.y * rhs.y + } +} + +#[cfg(feature = "simd-is-enabled")] +impl WDot for SimdFloat { + type Result = SimdFloat; + + fn gdot(&self, rhs: SimdFloat) -> Self::Result { + *self * rhs + } +} + +pub(crate) trait WAngularInertia { + type AngVector; + type LinVector; + type AngMatrix; + fn inverse(&self) -> Self; + fn transform_lin_vector(&self, pt: Self::LinVector) -> Self::LinVector; + fn transform_vector(&self, pt: Self::AngVector) -> Self::AngVector; + fn squared(&self) -> Self; + fn transform_matrix(&self, mat: &Self::AngMatrix) -> Self::AngMatrix; + fn into_matrix(self) -> Self::AngMatrix; +} + +impl WAngularInertia for f32 { + type AngVector = f32; + type LinVector = Vector2; + type AngMatrix = f32; + + fn inverse(&self) -> Self { + if *self != 0.0 { + 1.0 / *self + } else { + 0.0 + } + } + + fn transform_lin_vector(&self, pt: Vector2) -> Vector2 { + *self * pt + } + fn transform_vector(&self, pt: f32) -> f32 { + *self * pt + } + + fn squared(&self) -> f32 { + *self * *self + } + + fn transform_matrix(&self, mat: &Self::AngMatrix) -> Self::AngMatrix { + mat * *self + } + + fn into_matrix(self) -> Self::AngMatrix { + self + } +} + +#[cfg(feature = "simd-is-enabled")] +impl WAngularInertia for SimdFloat { + type AngVector = SimdFloat; + type LinVector = Vector2; + type AngMatrix = SimdFloat; + + fn inverse(&self) -> Self { + let zero = ::zero(); + let is_zero = self.simd_eq(zero); + (::one() / *self).select(is_zero, zero) + } + + fn transform_lin_vector(&self, pt: Vector2) -> Vector2 { + pt * *self + } + + fn transform_vector(&self, pt: SimdFloat) -> SimdFloat { + *self * pt + } + + fn squared(&self) -> SimdFloat { + *self * *self + } + + fn transform_matrix(&self, mat: &Self::AngMatrix) -> Self::AngMatrix { + *mat * *self + } + + fn into_matrix(self) -> Self::AngMatrix { + self + } +} + +/// A 2x2 symmetric-definite-positive matrix. +#[derive(Copy, Clone, Debug, PartialEq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct SdpMatrix2 { + /// The component at the first row and first column of this matrix. + pub m11: N, + /// The component at the first row and second column of this matrix. + pub m12: N, + /// The component at the second row and second column of this matrix. + pub m22: N, +} + +impl SdpMatrix2 { + /// A new SDP 2x2 matrix with the given components. + /// + /// Because the matrix is symmetric, only the lower off-diagonal component is required. + pub fn new(m11: N, m12: N, m22: N) -> Self { + Self { m11, m12, m22 } + } + + /// Build an `SdpMatrix2` structure from a plain matrix, assuming it is SDP. + /// + /// No check is performed to ensure `mat` is actually SDP. + pub fn from_sdp_matrix(mat: na::Matrix2) -> Self { + Self { + m11: mat.m11, + m12: mat.m12, + m22: mat.m22, + } + } + + /// Create a new SDP matrix filled with zeros. + pub fn zero() -> Self { + Self { + m11: N::zero(), + m12: N::zero(), + m22: N::zero(), + } + } + + /// Create a new SDP matrix with its diagonal filled with `val`, and its off-diagonal elements set to zero. + pub fn diagonal(val: N) -> Self { + Self { + m11: val, + m12: N::zero(), + m22: val, + } + } + + /// Adds `val` to the diagonal components of `self`. + pub fn add_diagonal(&mut self, elt: N) -> Self { + Self { + m11: self.m11 + elt, + m12: self.m12, + m22: self.m22 + elt, + } + } + + /// Compute the inverse of this SDP matrix without performing any inversibility check. + pub fn inverse_unchecked(&self) -> Self { + let determinant = self.m11 * self.m22 - self.m12 * self.m12; + let m11 = self.m22 / determinant; + let m12 = -self.m12 / determinant; + let m22 = self.m11 / determinant; + + Self { m11, m12, m22 } + } + + /// Convert this SDP matrix to a regular matrix representation. + pub fn into_matrix(self) -> Matrix2 { + Matrix2::new(self.m11, self.m12, self.m12, self.m22) + } +} + +impl Add> for SdpMatrix2 { + type Output = Self; + + fn add(self, rhs: SdpMatrix2) -> Self { + Self::new(self.m11 + rhs.m11, self.m12 + rhs.m12, self.m22 + rhs.m22) + } +} + +impl Mul> for SdpMatrix2 { + type Output = Vector2; + + fn mul(self, rhs: Vector2) -> Self::Output { + Vector2::new( + self.m11 * rhs.x + self.m12 * rhs.y, + self.m12 * rhs.x + self.m22 * rhs.y, + ) + } +} + +/// A 3x3 symmetric-definite-positive matrix. +#[derive(Copy, Clone, Debug, PartialEq)] +#[cfg_attr(feature = "serde-serialize", derive(Serialize, Deserialize))] +pub struct SdpMatrix3 { + /// The component at the first row and first column of this matrix. + pub m11: N, + /// The component at the first row and second column of this matrix. + pub m12: N, + /// The component at the first row and third column of this matrix. + pub m13: N, + /// The component at the second row and second column of this matrix. + pub m22: N, + /// The component at the second row and third column of this matrix. + pub m23: N, + /// The component at the third row and third column of this matrix. + pub m33: N, +} + +impl SdpMatrix3 { + /// A new SDP 3x3 matrix with the given components. + /// + /// Because the matrix is symmetric, only the lower off-diagonal components is required. + pub fn new(m11: N, m12: N, m13: N, m22: N, m23: N, m33: N) -> Self { + Self { + m11, + m12, + m13, + m22, + m23, + m33, + } + } + + /// Build an `SdpMatrix3` structure from a plain matrix, assuming it is SDP. + /// + /// No check is performed to ensure `mat` is actually SDP. + pub fn from_sdp_matrix(mat: na::Matrix3) -> Self { + Self { + m11: mat.m11, + m12: mat.m12, + m13: mat.m13, + m22: mat.m22, + m23: mat.m23, + m33: mat.m33, + } + } + + /// Create a new SDP matrix filled with zeros. + pub fn zero() -> Self { + Self { + m11: N::zero(), + m12: N::zero(), + m13: N::zero(), + m22: N::zero(), + m23: N::zero(), + m33: N::zero(), + } + } + + /// Create a new SDP matrix with its diagonal filled with `val`, and its off-diagonal elements set to zero. + pub fn diagonal(val: N) -> Self { + Self { + m11: val, + m12: N::zero(), + m13: N::zero(), + m22: val, + m23: N::zero(), + m33: val, + } + } + + /// Are all components of this matrix equal to zero? + pub fn is_zero(&self) -> bool { + self.m11.is_zero() + && self.m12.is_zero() + && self.m13.is_zero() + && self.m22.is_zero() + && self.m23.is_zero() + && self.m33.is_zero() + } + + /// Compute the inverse of this SDP matrix without performing any inversibility check. + pub fn inverse_unchecked(&self) -> Self { + let minor_m12_m23 = self.m22 * self.m33 - self.m23 * self.m23; + let minor_m11_m23 = self.m12 * self.m33 - self.m13 * self.m23; + let minor_m11_m22 = self.m12 * self.m23 - self.m13 * self.m22; + + let determinant = + self.m11 * minor_m12_m23 - self.m12 * minor_m11_m23 + self.m13 * minor_m11_m22; + let inv_det = N::one() / determinant; + + SdpMatrix3 { + m11: minor_m12_m23 * inv_det, + m12: -minor_m11_m23 * inv_det, + m13: minor_m11_m22 * inv_det, + m22: (self.m11 * self.m33 - self.m13 * self.m13) * inv_det, + m23: (self.m13 * self.m12 - self.m23 * self.m11) * inv_det, + m33: (self.m11 * self.m22 - self.m12 * self.m12) * inv_det, + } + } + + /// Compute the quadratic form `m.transpose() * self * m`. + pub fn quadform3x2(&self, m: &Matrix3x2) -> SdpMatrix2 { + let x0 = self.m11 * m.m11 + self.m12 * m.m21 + self.m13 * m.m31; + let y0 = self.m12 * m.m11 + self.m22 * m.m21 + self.m23 * m.m31; + let z0 = self.m13 * m.m11 + self.m23 * m.m21 + self.m33 * m.m31; + + let x1 = self.m11 * m.m12 + self.m12 * m.m22 + self.m13 * m.m32; + let y1 = self.m12 * m.m12 + self.m22 * m.m22 + self.m23 * m.m32; + let z1 = self.m13 * m.m12 + self.m23 * m.m22 + self.m33 * m.m32; + + let m11 = m.m11 * x0 + m.m21 * y0 + m.m31 * z0; + let m12 = m.m11 * x1 + m.m21 * y1 + m.m31 * z1; + let m22 = m.m12 * x1 + m.m22 * y1 + m.m32 * z1; + + SdpMatrix2 { m11, m12, m22 } + } + + /// Compute the quadratic form `m.transpose() * self * m`. + pub fn quadform(&self, m: &Matrix3) -> Self { + let x0 = self.m11 * m.m11 + self.m12 * m.m21 + self.m13 * m.m31; + let y0 = self.m12 * m.m11 + self.m22 * m.m21 + self.m23 * m.m31; + let z0 = self.m13 * m.m11 + self.m23 * m.m21 + self.m33 * m.m31; + + let x1 = self.m11 * m.m12 + self.m12 * m.m22 + self.m13 * m.m32; + let y1 = self.m12 * m.m12 + self.m22 * m.m22 + self.m23 * m.m32; + let z1 = self.m13 * m.m12 + self.m23 * m.m22 + self.m33 * m.m32; + + let x2 = self.m11 * m.m13 + self.m12 * m.m23 + self.m13 * m.m33; + let y2 = self.m12 * m.m13 + self.m22 * m.m23 + self.m23 * m.m33; + let z2 = self.m13 * m.m13 + self.m23 * m.m23 + self.m33 * m.m33; + + let m11 = m.m11 * x0 + m.m21 * y0 + m.m31 * z0; + let m12 = m.m11 * x1 + m.m21 * y1 + m.m31 * z1; + let m13 = m.m11 * x2 + m.m21 * y2 + m.m31 * z2; + + let m22 = m.m12 * x1 + m.m22 * y1 + m.m32 * z1; + let m23 = m.m12 * x2 + m.m22 * y2 + m.m32 * z2; + let m33 = m.m13 * x2 + m.m23 * y2 + m.m33 * z2; + + Self { + m11, + m12, + m13, + m22, + m23, + m33, + } + } + + /// Adds `elt` to the diagonal components of `self`. + pub fn add_diagonal(&self, elt: N) -> Self { + Self { + m11: self.m11 + elt, + m12: self.m12, + m13: self.m13, + m22: self.m22 + elt, + m23: self.m23, + m33: self.m33 + elt, + } + } +} + +impl> Add> for SdpMatrix3 { + type Output = SdpMatrix3; + + fn add(self, rhs: SdpMatrix3) -> Self::Output { + SdpMatrix3 { + m11: self.m11 + rhs.m11, + m12: self.m12 + rhs.m12, + m13: self.m13 + rhs.m13, + m22: self.m22 + rhs.m22, + m23: self.m23 + rhs.m23, + m33: self.m33 + rhs.m33, + } + } +} + +impl Mul> for SdpMatrix3 { + type Output = Vector3; + + fn mul(self, rhs: Vector3) -> Self::Output { + let x = self.m11 * rhs.x + self.m12 * rhs.y + self.m13 * rhs.z; + let y = self.m12 * rhs.x + self.m22 * rhs.y + self.m23 * rhs.z; + let z = self.m13 * rhs.x + self.m23 * rhs.y + self.m33 * rhs.z; + Vector3::new(x, y, z) + } +} + +impl Mul> for SdpMatrix3 { + type Output = Matrix3; + + fn mul(self, rhs: Matrix3) -> Self::Output { + let x0 = self.m11 * rhs.m11 + self.m12 * rhs.m21 + self.m13 * rhs.m31; + let y0 = self.m12 * rhs.m11 + self.m22 * rhs.m21 + self.m23 * rhs.m31; + let z0 = self.m13 * rhs.m11 + self.m23 * rhs.m21 + self.m33 * rhs.m31; + + let x1 = self.m11 * rhs.m12 + self.m12 * rhs.m22 + self.m13 * rhs.m32; + let y1 = self.m12 * rhs.m12 + self.m22 * rhs.m22 + self.m23 * rhs.m32; + let z1 = self.m13 * rhs.m12 + self.m23 * rhs.m22 + self.m33 * rhs.m32; + + let x2 = self.m11 * rhs.m13 + self.m12 * rhs.m23 + self.m13 * rhs.m33; + let y2 = self.m12 * rhs.m13 + self.m22 * rhs.m23 + self.m23 * rhs.m33; + let z2 = self.m13 * rhs.m13 + self.m23 * rhs.m23 + self.m33 * rhs.m33; + + Matrix3::new(x0, x1, x2, y0, y1, y2, z0, z1, z2) + } +} + +impl Mul> for SdpMatrix3 { + type Output = Matrix3x2; + + fn mul(self, rhs: Matrix3x2) -> Self::Output { + let x0 = self.m11 * rhs.m11 + self.m12 * rhs.m21 + self.m13 * rhs.m31; + let y0 = self.m12 * rhs.m11 + self.m22 * rhs.m21 + self.m23 * rhs.m31; + let z0 = self.m13 * rhs.m11 + self.m23 * rhs.m21 + self.m33 * rhs.m31; + + let x1 = self.m11 * rhs.m12 + self.m12 * rhs.m22 + self.m13 * rhs.m32; + let y1 = self.m12 * rhs.m12 + self.m22 * rhs.m22 + self.m23 * rhs.m32; + let z1 = self.m13 * rhs.m12 + self.m23 * rhs.m22 + self.m33 * rhs.m32; + + Matrix3x2::new(x0, x1, y0, y1, z0, z1) + } +} + +impl WAngularInertia for SdpMatrix3 { + type AngVector = Vector3; + type LinVector = Vector3; + type AngMatrix = Matrix3; + + fn inverse(&self) -> Self { + let minor_m12_m23 = self.m22 * self.m33 - self.m23 * self.m23; + let minor_m11_m23 = self.m12 * self.m33 - self.m13 * self.m23; + let minor_m11_m22 = self.m12 * self.m23 - self.m13 * self.m22; + + let determinant = + self.m11 * minor_m12_m23 - self.m12 * minor_m11_m23 + self.m13 * minor_m11_m22; + + if determinant.is_zero() { + Self::zero() + } else { + SdpMatrix3 { + m11: minor_m12_m23 / determinant, + m12: -minor_m11_m23 / determinant, + m13: minor_m11_m22 / determinant, + m22: (self.m11 * self.m33 - self.m13 * self.m13) / determinant, + m23: (self.m13 * self.m12 - self.m23 * self.m11) / determinant, + m33: (self.m11 * self.m22 - self.m12 * self.m12) / determinant, + } + } + } + + fn squared(&self) -> Self { + SdpMatrix3 { + m11: self.m11 * self.m11 + self.m12 * self.m12 + self.m13 * self.m13, + m12: self.m11 * self.m12 + self.m12 * self.m22 + self.m13 * self.m23, + m13: self.m11 * self.m13 + self.m12 * self.m23 + self.m13 * self.m33, + m22: self.m12 * self.m12 + self.m22 * self.m22 + self.m23 * self.m23, + m23: self.m12 * self.m13 + self.m22 * self.m23 + self.m23 * self.m33, + m33: self.m13 * self.m13 + self.m23 * self.m23 + self.m33 * self.m33, + } + } + + fn transform_lin_vector(&self, v: Vector3) -> Vector3 { + self.transform_vector(v) + } + + fn transform_vector(&self, v: Vector3) -> Vector3 { + let x = self.m11 * v.x + self.m12 * v.y + self.m13 * v.z; + let y = self.m12 * v.x + self.m22 * v.y + self.m23 * v.z; + let z = self.m13 * v.x + self.m23 * v.y + self.m33 * v.z; + Vector3::new(x, y, z) + } + + #[rustfmt::skip] + fn into_matrix(self) -> Matrix3 { + Matrix3::new( + self.m11, self.m12, self.m13, + self.m12, self.m22, self.m23, + self.m13, self.m23, self.m33, + ) + } + + #[rustfmt::skip] + fn transform_matrix(&self, m: &Matrix3) -> Matrix3 { + *self * *m + } +} + +#[cfg(feature = "simd-is-enabled")] +impl WAngularInertia for SdpMatrix3 { + type AngVector = Vector3; + type LinVector = Vector3; + type AngMatrix = Matrix3; + + fn inverse(&self) -> Self { + let minor_m12_m23 = self.m22 * self.m33 - self.m23 * self.m23; + let minor_m11_m23 = self.m12 * self.m33 - self.m13 * self.m23; + let minor_m11_m22 = self.m12 * self.m23 - self.m13 * self.m22; + + let determinant = + self.m11 * minor_m12_m23 - self.m12 * minor_m11_m23 + self.m13 * minor_m11_m22; + + let zero = ::zero(); + let is_zero = determinant.simd_eq(zero); + let inv_det = (::one() / determinant).select(is_zero, zero); + + SdpMatrix3 { + m11: minor_m12_m23 * inv_det, + m12: -minor_m11_m23 * inv_det, + m13: minor_m11_m22 * inv_det, + m22: (self.m11 * self.m33 - self.m13 * self.m13) * inv_det, + m23: (self.m13 * self.m12 - self.m23 * self.m11) * inv_det, + m33: (self.m11 * self.m22 - self.m12 * self.m12) * inv_det, + } + } + + fn transform_lin_vector(&self, v: Vector3) -> Vector3 { + self.transform_vector(v) + } + + fn transform_vector(&self, v: Vector3) -> Vector3 { + let x = self.m11 * v.x + self.m12 * v.y + self.m13 * v.z; + let y = self.m12 * v.x + self.m22 * v.y + self.m23 * v.z; + let z = self.m13 * v.x + self.m23 * v.y + self.m33 * v.z; + Vector3::new(x, y, z) + } + + fn squared(&self) -> Self { + SdpMatrix3 { + m11: self.m11 * self.m11 + self.m12 * self.m12 + self.m13 * self.m13, + m12: self.m11 * self.m12 + self.m12 * self.m22 + self.m13 * self.m23, + m13: self.m11 * self.m13 + self.m12 * self.m23 + self.m13 * self.m33, + m22: self.m12 * self.m12 + self.m22 * self.m22 + self.m23 * self.m23, + m23: self.m12 * self.m13 + self.m22 * self.m23 + self.m23 * self.m33, + m33: self.m13 * self.m13 + self.m23 * self.m23 + self.m33 * self.m33, + } + } + + #[rustfmt::skip] + fn into_matrix(self) -> Matrix3 { + Matrix3::new( + self.m11, self.m12, self.m13, + self.m12, self.m22, self.m23, + self.m13, self.m23, self.m33, + ) + } + + #[rustfmt::skip] + fn transform_matrix(&self, m: &Matrix3) -> Matrix3 { + let x0 = self.m11 * m.m11 + self.m12 * m.m21 + self.m13 * m.m31; + let y0 = self.m12 * m.m11 + self.m22 * m.m21 + self.m23 * m.m31; + let z0 = self.m13 * m.m11 + self.m23 * m.m21 + self.m33 * m.m31; + + let x1 = self.m11 * m.m12 + self.m12 * m.m22 + self.m13 * m.m32; + let y1 = self.m12 * m.m12 + self.m22 * m.m22 + self.m23 * m.m32; + let z1 = self.m13 * m.m12 + self.m23 * m.m22 + self.m33 * m.m32; + + let x2 = self.m11 * m.m13 + self.m12 * m.m23 + self.m13 * m.m33; + let y2 = self.m12 * m.m13 + self.m22 * m.m23 + self.m23 * m.m33; + let z2 = self.m13 * m.m13 + self.m23 * m.m23 + self.m33 * m.m33; + + Matrix3::new( + x0, x1, x2, + y0, y1, y2, + z0, z1, z2, + ) + } +} + +impl From<[SdpMatrix3; 4]> for SdpMatrix3 +where + T: From<[f32; 4]>, +{ + fn from(data: [SdpMatrix3; 4]) -> Self { + SdpMatrix3 { + m11: T::from([data[0].m11, data[1].m11, data[2].m11, data[3].m11]), + m12: T::from([data[0].m12, data[1].m12, data[2].m12, data[3].m12]), + m13: T::from([data[0].m13, data[1].m13, data[2].m13, data[3].m13]), + m22: T::from([data[0].m22, data[1].m22, data[2].m22, data[3].m22]), + m23: T::from([data[0].m23, data[1].m23, data[2].m23, data[3].m23]), + m33: T::from([data[0].m33, data[1].m33, data[2].m33, data[3].m33]), + } + } +} + +#[cfg(feature = "simd-nightly")] +impl From<[SdpMatrix3; 8]> for SdpMatrix3 { + fn from(data: [SdpMatrix3; 8]) -> Self { + SdpMatrix3 { + m11: simba::simd::f32x8::from([ + data[0].m11, + data[1].m11, + data[2].m11, + data[3].m11, + data[4].m11, + data[5].m11, + data[6].m11, + data[7].m11, + ]), + m12: simba::simd::f32x8::from([ + data[0].m12, + data[1].m12, + data[2].m12, + data[3].m12, + data[4].m12, + data[5].m12, + data[6].m12, + data[7].m12, + ]), + m13: simba::simd::f32x8::from([ + data[0].m13, + data[1].m13, + data[2].m13, + data[3].m13, + data[4].m13, + data[5].m13, + data[6].m13, + data[7].m13, + ]), + m22: simba::simd::f32x8::from([ + data[0].m22, + data[1].m22, + data[2].m22, + data[3].m22, + data[4].m22, + data[5].m22, + data[6].m22, + data[7].m22, + ]), + m23: simba::simd::f32x8::from([ + data[0].m23, + data[1].m23, + data[2].m23, + data[3].m23, + data[4].m23, + data[5].m23, + data[6].m23, + data[7].m23, + ]), + m33: simba::simd::f32x8::from([ + data[0].m33, + data[1].m33, + data[2].m33, + data[3].m33, + data[4].m33, + data[5].m33, + data[6].m33, + data[7].m33, + ]), + } + } +} + +#[cfg(feature = "simd-nightly")] +impl From<[SdpMatrix3; 16]> for SdpMatrix3 { + fn from(data: [SdpMatrix3; 16]) -> Self { + SdpMatrix3 { + m11: simba::simd::f32x16::from([ + data[0].m11, + data[1].m11, + data[2].m11, + data[3].m11, + data[4].m11, + data[5].m11, + data[6].m11, + data[7].m11, + data[8].m11, + data[9].m11, + data[10].m11, + data[11].m11, + data[12].m11, + data[13].m11, + data[14].m11, + data[15].m11, + ]), + m12: simba::simd::f32x16::from([ + data[0].m12, + data[1].m12, + data[2].m12, + data[3].m12, + data[4].m12, + data[5].m12, + data[6].m12, + data[7].m12, + data[8].m12, + data[9].m12, + data[10].m12, + data[11].m12, + data[12].m12, + data[13].m12, + data[14].m12, + data[15].m12, + ]), + m13: simba::simd::f32x16::from([ + data[0].m13, + data[1].m13, + data[2].m13, + data[3].m13, + data[4].m13, + data[5].m13, + data[6].m13, + data[7].m13, + data[8].m13, + data[9].m13, + data[10].m13, + data[11].m13, + data[12].m13, + data[13].m13, + data[14].m13, + data[15].m13, + ]), + m22: simba::simd::f32x16::from([ + data[0].m22, + data[1].m22, + data[2].m22, + data[3].m22, + data[4].m22, + data[5].m22, + data[6].m22, + data[7].m22, + data[8].m22, + data[9].m22, + data[10].m22, + data[11].m22, + data[12].m22, + data[13].m22, + data[14].m22, + data[15].m22, + ]), + m23: simba::simd::f32x16::from([ + data[0].m23, + data[1].m23, + data[2].m23, + data[3].m23, + data[4].m23, + data[5].m23, + data[6].m23, + data[7].m23, + data[8].m23, + data[9].m23, + data[10].m23, + data[11].m23, + data[12].m23, + data[13].m23, + data[14].m23, + data[15].m23, + ]), + m33: simba::simd::f32x16::from([ + data[0].m33, + data[1].m33, + data[2].m33, + data[3].m33, + data[4].m33, + data[5].m33, + data[6].m33, + data[7].m33, + data[8].m33, + data[9].m33, + data[10].m33, + data[11].m33, + data[12].m33, + data[13].m33, + data[14].m33, + data[15].m33, + ]), + } + } +} + +// This is an RAII structure that enables flushing denormal numbers +// to zero, and automatically reseting previous flags once it is dropped. +#[derive(Clone, Debug, PartialEq, Eq)] +pub(crate) struct FlushToZeroDenormalsAreZeroFlags { + original_flags: u32, +} + +impl FlushToZeroDenormalsAreZeroFlags { + #[cfg(not(all( + not(feature = "enhanced-determinism"), + any(target_arch = "x86_64", target_arch = "x86"), + target_feature = "sse" + )))] + pub fn flush_denormal_to_zero() -> Self { + Self { original_flags: 0 } + } + + #[cfg(all( + not(feature = "enhanced-determinism"), + any(target_arch = "x86", target_arch = "x86_64"), + target_feature = "sse" + ))] + pub fn flush_denormal_to_zero() -> Self { + unsafe { + #[cfg(target_arch = "x86")] + use std::arch::x86::{_mm_getcsr, _mm_setcsr, _MM_FLUSH_ZERO_ON}; + #[cfg(target_arch = "x86_64")] + use std::arch::x86_64::{_mm_getcsr, _mm_setcsr, _MM_FLUSH_ZERO_ON}; + + // Flush denormals & underflows to zero as this as a significant impact on the solver's performances. + // To enable this we need to set the bit 15 (given by _MM_FLUSH_ZERO_ON) and the bit 6 (for denormals-are-zero). + // See https://software.intel.com/content/www/us/en/develop/articles/x87-and-sse-floating-point-assists-in-ia-32-flush-to-zero-ftz-and-denormals-are-zero-daz.html + let original_flags = _mm_getcsr(); + _mm_setcsr(original_flags | _MM_FLUSH_ZERO_ON | (1 << 6)); + Self { original_flags } + } + } +} + +#[cfg(all( + not(feature = "enhanced-determinism"), + any(target_arch = "x86", target_arch = "x86_64"), + target_feature = "sse" +))] +impl Drop for FlushToZeroDenormalsAreZeroFlags { + fn drop(&mut self) { + #[cfg(target_arch = "x86")] + unsafe { + std::arch::x86::_mm_setcsr(self.original_flags) + } + #[cfg(target_arch = "x86_64")] + unsafe { + std::arch::x86_64::_mm_setcsr(self.original_flags) + } + } +} + +#[cfg(feature = "serde-serialize")] +pub(crate) fn serialize_hashmap_capacity( + map: &HashMap, + s: S, +) -> Result { + s.serialize_u64(map.capacity() as u64) +} + +#[cfg(feature = "serde-serialize")] +pub(crate) fn deserialize_hashmap_capacity< + 'de, + D: serde::Deserializer<'de>, + K, + V, + H: std::hash::BuildHasher + Default, +>( + d: D, +) -> Result, D::Error> { + struct CapacityVisitor; + impl<'de> serde::de::Visitor<'de> for CapacityVisitor { + type Value = u64; + + fn expecting(&self, formatter: &mut std::fmt::Formatter) -> std::fmt::Result { + write!(formatter, "an integer between 0 and 2^64") + } + + fn visit_u64(self, val: u64) -> Result { + Ok(val) + } + } + + let capacity = d.deserialize_u64(CapacityVisitor)? as usize; + Ok(HashMap::with_capacity_and_hasher( + capacity, + Default::default(), + )) +} + +/* + * FxHasher taken from rustc_hash, except that it does not depend on the pointer size. + */ +#[cfg(feature = "enhanced-determinism")] +pub(crate) type FxHashMap32 = + indexmap::IndexMap>; + +const K: u32 = 0x9e3779b9; + +pub(crate) struct FxHasher32 { + hash: u32, +} + +impl Default for FxHasher32 { + #[inline] + fn default() -> FxHasher32 { + FxHasher32 { hash: 0 } + } +} + +impl FxHasher32 { + #[inline] + fn add_to_hash(&mut self, i: u32) { + use std::ops::BitXor; + self.hash = self.hash.rotate_left(5).bitxor(i).wrapping_mul(K); + } +} + +impl std::hash::Hasher for FxHasher32 { + #[inline] + fn write(&mut self, mut bytes: &[u8]) { + use std::convert::TryInto; + let read_u32 = |bytes: &[u8]| u32::from_ne_bytes(bytes[..4].try_into().unwrap()); + let mut hash = FxHasher32 { hash: self.hash }; + assert!(std::mem::size_of::() <= 8); + while bytes.len() >= std::mem::size_of::() { + hash.add_to_hash(read_u32(bytes) as u32); + bytes = &bytes[std::mem::size_of::()..]; + } + if (std::mem::size_of::() > 4) && (bytes.len() >= 4) { + hash.add_to_hash(u32::from_ne_bytes(bytes[..4].try_into().unwrap()) as u32); + bytes = &bytes[4..]; + } + if (std::mem::size_of::() > 2) && bytes.len() >= 2 { + hash.add_to_hash(u16::from_ne_bytes(bytes[..2].try_into().unwrap()) as u32); + bytes = &bytes[2..]; + } + if (std::mem::size_of::() > 1) && bytes.len() >= 1 { + hash.add_to_hash(bytes[0] as u32); + } + self.hash = hash.hash; + } + + #[inline] + fn write_u8(&mut self, i: u8) { + self.add_to_hash(i as u32); + } + + #[inline] + fn write_u16(&mut self, i: u16) { + self.add_to_hash(i as u32); + } + + #[inline] + fn write_u32(&mut self, i: u32) { + self.add_to_hash(i as u32); + } + + #[inline] + fn write_u64(&mut self, i: u64) { + self.add_to_hash(i as u32); + self.add_to_hash((i >> 32) as u32); + } + + #[inline] + fn write_usize(&mut self, i: usize) { + self.add_to_hash(i as u32); + } + + #[inline] + fn finish(&self) -> u64 { + self.hash as u64 + } +} + +pub(crate) fn other_handle( + pair: (RigidBodyHandle, RigidBodyHandle), + handle: RigidBodyHandle, +) -> RigidBodyHandle { + if pair.0 == handle { + pair.1 + } else { + pair.0 + } +} diff --git a/src_testbed/Inconsolata.otf b/src_testbed/Inconsolata.otf new file mode 100644 index 0000000000000000000000000000000000000000..348889828d894b9b57fb6435497d5c034448cb1b GIT binary patch literal 58464 zcmd43cXSq2_vpXpNufW~&lJooQN#OO~?|av}Yu$f-xZKY^yPdYrKKq=RNy~QaT1qByBtR zM};O&L^5U;Ik>oC!*@%cS{GPST+KB{}6AApMe@w5wH{Kr^4R1C7SM3iWXr>RoJmCLg{f7)2Q8%#RHtd~6 zr|*znBjT-otzcPs0l~4ohQw@myxm%tsO_lHmsYFtJqB_eG1(de-Za zwdO;~6mSKKY+5pH+JAmcD=RsmF6H#%Q!4+^3#e;aNoR|8n2lDlXDOAJD)5>}E3x`f z2(TU?6(s3>6MWi>Pg8%bvl3xp&zC7ULjZ#(453uS^dM00U#mA7^c7peiiHPMHqEp~ z`2$8yWm;Q(akM|otJg2TzpQ_T6`h7#lF7P}hWjMg7o3I%NC{t;G(1o;`KG7gL6Xh4 zH4P7jewRjBff=Z)Dix;UmgKe$rr|!xY&}lH1EiTROBx<1xqai(@E{5E{hWpeLq8_< zWT?bQFBt~xOZ$7vNMIW%@zNix8T7Yg05F!)a7h4aSVk(5yi;EvQbU?rU!@?pfRjT@L?FwOSVg|?bO2F*!*uF7C z!-n;b32WP=Sy;<>Osk2oW&`@f#3sa)3+q2@SbX)06%+al9S}b(p+drd!4)uHv1P+% zG_DDw(#IS51mIwAc6&jN?GqQ95I4BjFbd7-e_whxTv+|hWBLvl&hLwX`pSD!MZ(3ul$-Zj7Exv8O9lldx`NKlP ziiVY-r}e{zg)Jyjw8)o5zABodShz7*4U^fH&&q7&wL+~*Ris$(^>nqsY$)zRv1 z#abggYnwf5N5k@l6$}f*+AIIn8V3L8KePQmG_%*_5=p6@qLEw95pU0xJ$v+A$k_{L z9-pmuX5AV0^sQ47UcZ}K>8%`AL93`$%qn4(vC3JIR%K#NWv|C=td45I>gmDmR)6q; zivO2?vt^FVl^=-wKUw)?o|RGN%dhgAERco7$3?`>KV`8ju>xhOER*F{7WrFN$|@^A zV_ZWV`H;ow7@I%O2S)`((cykb`na4$BcaD#zrwoRE`p zN>0leIV(OIh4HM%824aw>swJ)1Np}aw|X+>4dpLc zVFg)PnTHH`h~EmfGT|#0zR}0(YxQF1$f%=@ld7Nozx)f6B2rYsrI-|#5>ir1Nogq~ zWr>UBSz8rZSrIJfD5)$}h|JX_nq^&GYDi6~CAH;MRz@AEOEj-94G0~Lq%mu`sl3LT zX)Y~@m93;TF}tm_llIa3hueBWC+4GtE3(eegj%6yNNH2cE$z=d+Rn@l?U%jspMmhW95t^gsW8#Y+W` zdW?_G!JEbE?JzP;1)Yr((OqmSWbK~}IVO7n>~4DV-Gyd+>@qa5;us%DVv zk`K=*jd#3)2Q*=hyD*1Cn4|aPWBF31Go}j|vwv8%ds&r<%)7ZH>3Sv}Ra8S&OW{tqs;L>xgyEx^6wRj4y*Pr>~H&q%Xo(!`Hyq($~p1 zz&FJAwr`B@L-MvSec$@N_bv1-^R4x5_3iN;_MP-4`mXuz_#TnN1qWmf$Pth?AT*$O zK-qwZfNBA?0_p{{3g{5fC7^#me8AfQqXXU#_#|Lbz?6WQ0doU>30NAiDqvl}mViA0 zM*>a;Tn)Gxa6ce9FhgMWz+8a^0*eJ!3al1bE3kQByTI22y9M?M91@riI5P0Pz=?sA zgUSa*2i0pqhTA-*LPXkcWZG|3+Hd8w-zsUpRnvZ}rTs>y{l1d+8&T;6^1@=o3zHEq zY(~5=8u7wv#0#?#FYHFVFdXs1a^wrkkuNMqzOWqm!gAyb%aJcEN4~He`NDGK3(Ju& zEJwYt9QDF-#7pUjm*NpGP2HBqF%HnBI-qRBBEZjCnD-agCe3{v?wC# zMUx_;UbHEq@{2}ARDRj2$}j7v{IZV9FYBoMvX06x>!|#)j><3VsQj{yDlhA(^0JO9 zFYBoCvW_Y*>!|Xwjw&zfsPeLoDlhA(@=9!6Y;RJ|L8O-n@xA)Q#0(!Amk{^Hu>K?C z`^Ur%j~!5{Qr*bZpQs@N`V5Ur#b42%$a-~Ce_rV`U}&G=L*5u1GonJDUh#c;4ULOc zq~Fk9Z^bCmtIzOZ9uhaC7dB$+#Py4djTzL;lWgm8+x8(>96Y#JJCD}cqjl~ZH>_75 zGSgvgymB3y`c_Oc53iRBJ?RFi<{Ef~`e|@zT(4m*JfRjIrDgx&vHf}t9X@1mui?Ym zc%=42>2~{n)7z!BqF1WLUS5TLJmJ0`j7en-_6mbN^RcPWqs0B&*ies@;IaF7Fd@zS zu(aRd+JNEz))_^YV?4dI1sWCk%AkHjV`5?llkX4c1AA~xLc+ip^x}IB9UAv`Urq!b z^45Ta0li`BG!Cth5HobZ8~wcY|J%OK29p+9saoX*vHkio8NG%z@}!!)9PFW9TZehA z80F!kVur?5h>c4a+$*7fS|qco);HD^YpV6FHO-pNmNC!IF*xUIxj? zg-B+}B7Vs#+1S2vaA?mZA(C72NM1Ix{8E4eKp_d0!ldT^uSwCn|1U|A^|Lk4`o)@W z{c8PYEwC0^zq2X+Vf|?>ww73bSxc>D)^ckF+vG}Xm9^ShWBp^TwOnhRwVsW0qqWJ} zY;8$RtgNkU$}Wl3DbntVBvw?>%dejFoZn zzI-Gf$cHlCOE5?gt-aPhYrl2CI!NL;TfUPS95oKfr`BPThojap>$r8oI%%DzVbO zi=@d`3TcS=ET4}wbO34STQbrY;0yEx`GS4veCd4|q>nG7FOx5`FN@Fb%j(O< zyiW6FxB6+$c%(OPsu1qH~2WQEj<#qqUY^|~_$-Y15 z;9MC(X69*^XJDQW^K8y@E^pC%aRmkx_@Gc!p*n@S78+RS$3lxkJB9Wu+?n%frLeEU zW`->h0h3|AAYS^k7B!voh){(c>dxIiVrWoviRW=(@OkRDtoEIrB{_c zTvp1KD%ZW-$a3e)yXALRe4}Dq#R-)PM^uTZ7tuPRN5qiGqLC&lHtOA|303A**--6V z^ykrEMt>bWCHmXw>CrQzzl)w7JvaKt=%1s1iT*WuLGYr=4q~Y#{4;uLz zmupNqH2(2PMF z23;SVb8zD!{@8i3>tj#FK8_2Es}Xl0K68Bj_)_sz}zA+7&~L^hOyVj<$V9O4+nkt)rY^2-~LIf z3B@Pa6S_=zYr=06HcvP=;rVAtlUhuAYtna<{{FJ_m*c+taPo-BAAMcn>zZG;`MUqt zW4@mG_5E*}elu#y;3;#ate#qUYWZ*XPoFw{@$_BOugq985f0?ia+U$Kk2YPX}3RVlRs&-Kj|-j(r^BxAN)zv{YhW? zlRoh$IsT-#{Yi2Dq&NIY-Tg`J{YlOIN%j3nHT+4D{-iSgq$2*Le15;N%1m>NR6Os5 z7zvy07;|wn<^1n~o#~;Vn+?qo0Jk#cOmC&Zko@DoE;??inuPrzszx#YI2+n52{mT( z0LS#|K^&M>qo>$q#sGiQP?q7uF?`@#P=Tcm&l=-+gu~Xk2~;7RCa-R z#tF47hLx!@MpMQh8FU4sj~hb^Z3K4hp%}^htxXeZ*b~z;K%XwQjj!u`2?@(+Oxrh{ z++i90Mi#zB*W&JL*RJ0cV>%2&Wl|zo$^yXiPbu#G1Y3h|xJInpE^?;;?f1i-wH$6r zHHXeG0_?AgNUzcka+6_P+bYI1`gHKs`>xHTC< z2}ppEJF{)%uXh~Aq0d~()=WT9c|e8fSe<#oq2eftxxTQC{L%u@WjsKx0X(@%?U4Ja zyp5dFa`{~&KSoeocM6a^0C3|MK)3mTFOL9Hw3WFwI7WUOlTj%0YvDKkk z`4PH)9^&hj1)a7oc9Pv7M)4Y*@l9BQsM(m;NoULthmm~{ikSO5V(V`+d1gg`4LmVL zjAWY#f6GF!cG(?cj@FY|_!6EeIcA~sB(q|5ka=?6O;0N#1*1EL*mO>tdXRaLe51h5 z>$DU)i_Y^ha07EW48n)de2Xbc4Y0r-ie(2_wgWZ_5VQ`BXIWh2>Nr$z@R&o>hpFF{ zQ|H>V67gCpbm5#Bsecppv&DF<*^mxS^+gbC@3Yexd9wy({{}#n;(!)A)Il4qpr_KM z75aaic8wId3U=*|V=_)1qX}k1+27UydG3nP}j-V8|>j?!HFK*v^r!j)}`oI5c0oo6Lv;{)jkiF zu9IjO`G{Hc#pT9)%FjDplRW7f0jTCS~0Shv3xG(FHM?T6g*Z+fp23 zmV{B^($=seuRz<91XioN%i^afk<$RxIzwyPP>eLI%Nh$U z=L&3F9a$zR5qD^}G#!r4E&_;D>j<_XOhE&{&a;j&TaOCe$PTu2y3p>Y6t~WD=thzY z&_Ry82dEYbXnJ3Fkx4mh)<>|~&NW$#6yD_;b34&y_BNrtu`A$QBD7m8z^+YkjJfh9 zSmHaVDD>~pyl2BUz9qLV01dAsBhGJ z9hTJk8dmWzK*>J=5mCAxs?|Vr9%G#v@yi8JT$$H3(s>rENecQwi)VwKue-2%Zc)tr zAy}z#>bPqg!YOywX5A&vaRJAL$17Wby;_Wkxp_`1uhC4I%;W^+sseA(y@73c&Z9&8&VUVEq9W zV=DnqR3+ejM`1Zyl5!HEfFw0%#d^FN4l%a~AfOK{>y}N&r1@`-@pY-`GF~5)MeXm^ zVx;YKhn5vU*9`gwi)O$$8u)7^z_2&Pm=PxdD@M3XsH{G>zKG8%xK-j1{_9&jfaK#+%*zkiQ>;+h><~O#hA{`p()qfpv*XGm8rpi-&Vtj z+Jf4}S8Y~iLxg)(v5i?iPEuPSQ%W;2Vl{!CITq=FtYE$&u&u>_Oc%X&pW4pAWqj&{ z6d^OTBIY8KbE$+B)jYA`Ms}4h+a)Aid8Jo7WMXCPkYdVv^`y&Q;}`m?iGfMP)vW48 zT;uz3s^g^<1gy%ACQ8pbiSZ@8!O)mlg=pfmZ&3Yc5=zli!K%hlXTP&BCT^mJ-XFQd zj##jPYqc0IMh@qPc91DH)4sJyKUN|!PXRYj67iw*Ob?O zrf&@O{T`!>gD}y50Vvs#n#x^p#D5}K^!cM-_$z>5;wkPUn{33J9V*tbSeQ^v0y;^JBtY>JMbwCe$0ThP*^ z&n#W#5aGWB+wm!2Q-6xfvuJL$=|0UfC+}dSc$7_@ld%+-EUY69CK2PkixKO-fQ$lD7C*o0jj4M&nsg znF!84No8XcPmPhS>e`CeE=nbf*0|u-M>&}&v!aKPo-JUyQ?A;!9!BkStgV41?6g$Z zTpR3~lnsqANP@zooYnaa%|T_wPrDEc3JtwNvCOC5fY+Jk;Gj1V-HpGRnM=TCmj&B+ z0&K^pU~38r1HO8>Hv8asRHJ-QpMQ*E@aMwNtGB27PFT4`0U3^=nDnM{^E(Xqx>J<* zy&*c_Z4lpJxh%65V5ce~)HJi`#1C2~EQ0(tajq=<@qSnzvR9hl_QPnZp=jy?ZO5sp zLOV`HxKwfzaAr9|w=b$kXHdUdGM4ynm2fZ2Li5wCk9#WZqCCBVg@& z!)EW>jp8AuR^Ir+F;d`&Fn`{uwe%1gCj~x3^6Dd40lMIy3zEV4G4W&u!0ZuY9{qq$ zhVo!fY#kxJYmh2q)s;@ffEH6X-aQ}1Tn3?Cuc=R%OPg`i)5jeR)w$u-XbSJ>ecPC? zXCeO`Ys+lU0&V|XiaSc`dR+2cyRjt(y9DRX4gUnB@(;?o)0(wn`R;H}L?crjA z$jGX)eAQ{(TbEtjX7*g{FFWRv$F>J7@Y{M)S-%{W5-V-CU7SavYa?{NAz+yf&Z(L( zc1;AVddPH{75QkM`JpUzwbu2vbTvAIT2dUMjcC(NI7YU0jdZ`|WyDRoI_N)vP0K&hzz-aneFRmrp&+Ve!wm>K&!DxZn9ZW^4Vq9e996ZnY&f``8Ywr4ot;sqx? ztjXd2S|PB~?+TsV<+AJda`fRL`c)D-IW* z4|OG*@3VvdJ_Vs)y9>7?HpP#TlvZz1ToUZC)qlWBmZ-s6QKcXV`Dy?PWfH81+N22? zUF;M_{7yLFPzuE-v05BqGXR|^y8LH$)`504#$orXN^{MDCJyl2IhQPGG*q(#i#${z z^nKy5*gpaIHo?uf3EIPMV8O0>*`5V9OLUA)%q_MC^aOpBh}j5TZ;MV;oUJjppi8JM zJy#uk>rj_abXyqAzX4s>Bj0%eqLrfnkt?A^kJnB%yg`$sMqR|cgT;fpm>(DI5V(c` z_I(A|UmlQH+oc0xj(4T2X}C3Hq(d5RQrth>KuyR0ow#%U&5!SN+NdJ1w%e|;?&vK+ z%1=2Z^<-jodmocEYU-H$b)68^ej#mpJdu&|0-H5&m22emJTHIO-OpOq&kiY0jWJbi zV=A4~0!~z>361e=Tb*yF*-n}JE+YT2ZT;HD{i{>V<_z(4>oh z!^@|p&V(rl$JE6&V3%?_M6(GL&yGYTe^anaxe*TACxG)xYhxIyLCN=!upKeL7GL_MmG4- zUveDKHAx5_&A>X$WlNH#WKM*~^BCFC36^9*aan){ySwe|U<~e|#pY<3&B|H_IQjts zcXTzbC$O5!v4GR}gq5_>L2!E>6<@ASSG}uE3Yv`3MpGe53C{_OeFH0d2-LhAzzR*) zs*b0#iOvHcZr?3>30rX<7EMFUJ7K@={>gtw;xwcGQEYm%3O3YGzM#BvjTExD6W?UTEQ_6{&|vOtpHjncC^CA zU(k(xQBdpjwAsHaA(Uslt2sMZ?md8FLnu~luhKPEJHTqR)tos9t4beqHYM0}@hxb_ z0-$BXW6jaaLMvh&Xp2Or=xbg2JeXkN_U@&J!2B}>Qw2vMVpO=mpI{GIgHpP+P0HF5 zQy~{oI=O^8&B@{(jDvOUC#a{E0*-EUkPL!$zBR@BEKZZC3z~alA}+@SYX0sWb!KkU z1KH*dIaFPEq)XMI*N`@SsirWo)!eP=l6bv}-nGoo@_q#Oz{j-F?7yS%z?%qNc@1z_ z>pFNB{tb3kb1@I#{#j@jr@B~F=eU@|>-5K0B$zcE@;RB(uzF5NDV;p>%hbs$&GWlI zrlX?6Qbt*N>*Ae-%4*5G67Wt&$&<23+SN^DHyU@N@UdJvS3!ES0Z%WI?cGAu3i)dwN+_Cql#Grsl~=CQ$O$#I8C`3Q4Yb_xFQ zCgyIp1zave@xpzFDa{W^%tpOt$619(zi?R=O%O^x;t;#Hf;}U4O3(`7JWRxw;0wYL zCp%csFbD79WJcbe(bZJpibIWu9j34s#LMYjmO?(ESJhJ-VATM$3i;p{JPW9rkz$_H zVC8-Tlm38;tyH{xVJusH@Cxa%11i(5$5h>TQ%}OF^WC(XOO~C%VQEK1Af}EZM`yYw zWn(crNDpu&KXy6YOt%r}LW|5XZE>K*^!EM}+3UEcm8A}?zC>JN+qj*>W)yBUL4o(C z!A_zd*cGjmXFafc;!XNt1$P$ir6rIcBhnQ+zF$xX7y}5L2=KLbDe6)Wj1|DQHWxqV zq4DTUC$esW_Jb!&6?7s?eUVl&slBQ*g@7s96?)OlO`G{Ut`oT+PBD1);D^izrq_ShIs2&eVG(jE#iXXMPHm!xgE8hbOHH8-1 z2CT+HS|YEOwG{@q>;!Y6)_g)EH-)QEE3l(C!EzM_JJ!*`#nXWu+2?9JnlQ&D0U5wX zBg4BBnk0XEHIn>5^AF$#h92j*e)=#EbBp=1PKZ6C_0c&0jV^VdA z7Yd{E+Nmoj5_nGRS~$>v^W9<)|_uu)|!AfK+{5#wN(pPO`@NxMP493%uL>r3;1cZ@W5#rLUVIC9HL4KkJqblwlJH#c(lEz&3rR7=DdPk+vu4kEge1D1hujW zDj@)0MVD~1SQy@wVAet~t34oSgbnR9MpIsM*in5BP3Zp)PX2s?&VHF&F5&GdmM^um zc^J6_Y*-$^Pje_^i~eS3bi?=FviC{Vs~H=Y{|Lq>z_vu-WId#{Axj zD*1`7tJ19zvZgpV0mC2}`@qi^4#*w^t#kxf>2WT!uffVkvz_6~TgT%%qleldH5h6u zArB>P<8iWU8BnV5kjnQI0xLWZ%spf49{9_3@F_=JJTV;5ItK81XN0-|%(A97>0nET z16%dMbVI5tO#asOV+bRn;9?QYbmQbv-HbcULV6Se0P_Io3-Atf5SbjUcf=j z*&H@2GAH25H!gerb+D8U4%ltx#vB>y()|O4Va;3zO9Q_2sZD^MSW!p^GTO!(wh!!^ zW=>l89EjIScyI5pQAC~BOj1ICjiY4;3#A^(aRjW$dO&zRirEf<72nEGN!b*I#fX09 z%2pvss|J>M$7W`#VC-3OiY5w>Tpt=A9zt7*wtNQmqy)6U(GGoxa?!{oFj_H#sc$}z zyW$CFS|VQZb(_oo?uh4MwVKTzd+siyAJU=fUEGk`9LxLTS2C-WguKd#uIw%*#a`=% z*cx+qu$!AX=**C(t0sCP3f+j+fF)?;?gW<85>`?hmrf4^_%UQX!ti+;xK4Z8SLl|or^Q8=t#}!1yW?2U=KC zkILSoI%``WH>3^&dg>j=$mY>bNL?LJJuP^wdazD@m&PK_cEi$@!!+;8O~CnO6wj{^ zMzpj*y8~8mHqr%uN~1}h99nFo44P`L9Hf{hE5he{+Io%d9bmp4M+{5dF7&LRFW9nY z)MNrAIxh+<`%@3cZ#^oNXbmA?B&>=TxTO?dI|r(-Jtn=N;%i4W=DGH>@OL;w_Q|#p zH$jXnt%1y?f}*FhTkkSZ=H^PsvJd088Y30hRkaGVu#G9Z#N~9x3qIcItgVY!SBGj+ zPiJTaPSj#+82|lr=IH6H-B;{|+PV5VsKp<S0Q~G~|7o+0h>7d;pgD9_I39 zbnw0L6hr?M4kH7>GTm}nq_5E=$<~mDbJIj8AD)An?jP;cb+Dj|&~99C7>1c(SG^}a zH%s9~DeDJ1oC#Xl#;TCnRVU5sgWzj#Q}H)|Y^MPQb&S$Y0?WJ}Ec-sC#ex;^E-(FC z1x0^;>?ieck?dpR-31(ylYv6T8z?4k0=sy^VQ8;ccc^&P`{>s%PsI&7dyZUuw@r_y z14_(A;kw3@(2vl%HQHr=jB{{|HE=VW1iRE+Egh^t#pXy9go_UVw``Z~z}-~0ySY$cm2u7gegK}hP}lt95EiClb8HCM+Z6!yI#aB;8Bpsr@4=kzhu%~* z(J}g?8x!|wsDw1tXHACuJN{#9*ktb_OMr3>y)fJ@;NH+t9K z%YG70Q^|mL9@)lf{tUy$lo5uikPX(&W{vF7ihcbUH3`n;5^!!n4Qhjtl!jpG%Xz0) zeOE56(OqA+c_(NsJo|6eZB>^>m-0D;p~-aTNHSQxR=7MX?iNy0M+%*25B1xxTmr%G z6sNsz(+L&^S0$D%oUPkv(6a_EUdOPSqd!mwZ;vAMeMd1?z1c!O@|;GRKi_sz*L&*Y z^CrLQ=AMFEO7L`tPHnddl`|;%Mgh`21z11Zbg31I5{xBxcj0c*&RGM8zv!I# z>;yJC5G~Bd4aCU&XfSG|o3nP?#)R&4jp;E1VxB!%YF*fd^LBBY?Wx&^2glB2*dgs&fYwT3VX|C?zZR+X{Ewsd2n#L+{6PMQ)hw$@dxT=1C z(ZBE`{2a&7KUoJX?`&vKui4n#f}j)~MD>zi-5{VWAb+$&IrN(H`j4|&;v=Az$cFy$ zY-nYv=)%6OSObS{zezEVD{Knd&(KI{S!#e4$O`byr&vhehm$ zWmn1G`&_wNxH!T$P%A9}lyCrfGXnBoav0zpE|%Y-1<$V7w0pVN@1h@qM?ZAD!#@4q z-T`}N?OGH9R@sD%Ai%+T@DB#N4C6Jx(>Z`G8yy6XccoS4a!pM8J`;Az)bc?!a`{u6 zMt%(SWHE=RGzskTK$qxN25>bW#S2W0NEeE?Y7yRXj-#{K=06?cgmB*bR8n`^fbd~@ z(QK`~h!3Ya&W#c*D^Dra2v5-LRKt?{GKRG>$@>j7ZGeR9J}P+=XqMzHYo|6=a{u6! zHI7R0A83SjOrmQzshyQlA?WJ>NZHlUbWqIm`flnVv269x8|MZ`uP6vr-jJpau>|J9 zk`BAMT^W6y3Um39q)w07(@q$l>wi+Qx#Gv5PNlh)SId{Zi@_`EpE~a*F%C=W3+BQd zF!c{}j&S1njJaYX7kV4*dn*w>o(*tsFCyx1=GYyrXX z7MpVIQw}gfeWvmxn~BQ;t7L*pU-#F8SloM)_k1;%sC~*|&U(;xeWw;bBJ6<{H{lc# z^S%ennysxEc}45`)Wvoke9g@30@k|+*oB>-SnPc?Uus^3kaq&0%q1by%4cKq#5&N_*CS3be&%F7jO=X-xc(zx z&q8%_-l%tpl!9u7P`1x*kLZIG`^jM>&cn%9+$DhMKxe)T?arTILB&viatTICLm}!f z)4o0J=mJv~B4nx4xzEuK-r0O|ghe5XcD@)EPUborA_m?f*N5jLN6i z)n>zZ_<0*R>@$Pl*Ir4B<@KBb?5~bjI}Cp9*I~732nf7t>-^54v%Z>T)xgO@2!vgw z$@?a_c!1AlC_8D7f6#NhSx8tlD~~JTi(@wMS|`MODJWb!ifW*Z!gWiVwd0PX!5wy; zSeGD|?i}SaI>PCSAatg&gZilz3iqc7F1H2h;b51YZ3E!c0BCp0fnDz9z)b{uRuydD zNSn&Wx{P4H^AJ;?(b@$tTm&I0F+~`tofLD$(L!lF-36B3ht>f`MqXV{l~VR60hS1d zkY_#$xj%4OoR^uG)YomIw&0KKFoHYILiOTvs(QHC!I1}G&um_;@bE8am97SV%?H>s zNj?vzL)O#;B$Y%VWtdGRYpJ9}Kd{V?9DJ~-P4yAI9bnO#=s#KPGVm1;f4mYb^q$ba z_R~PU2M;qZ>303Bh0XfJP0aIe1#Fla3Gw!Xwl!c?eg?HvTZi-0FUsQG5@5l?*uT63 zR$y&ZpLGBWIwaId+#yqSXy+rBfK=MXodZ3W*!vnTQR^ac%baQmFT51Iy=~FV@dwxr zeQ0IYaWjTXSQxl?!zEIm2K#vr*zMwAiPMF}^^Hx&I0x`}zKef-tgf;1fGXbi!{1yU z>%0`H_aG_R9A;MaO)13^9Hg$rtL#(aD4)Q>dipz!kZ>DpliJdUtN*t3${w{P`OAng z4NoFFx<0lJ;55?eCZbZgj>3q15VEgyn3PvtJg1r3I{zU=p0m)F#LehmT;visjZNH| zjFF?I9je;5P%GSp4@*uLp_99XNHhoj^|@Fo@HyDMjSe=~+VnaZtkjP#v-u~7M;tBQ zP78jKqr6_5wP*$3K-QdvmCn00OGh?`>=&>k(V?b^u&jX;vw2tCS9htckguS6fiy#U z?<{4_Ln7T+fVIHEY7h9K%qF7cc%LQ8jleC?su>nOX-c&)3ZJw4AAt0L}(EL zuSq!o7KEgE(h+&fQZ1Yh zTsh$h&@0&5G!?99dayM|7#%aWwc1`e5wri45zr;;lyJ1W!}J6TBiGf%5&nRm*vn?M zGzQ$#$acCI*s(YV>HP;)=hP%5b5??NLU*`wgavbPgW#3tpe26f(8tZto(}{oo#3*3 zN2muCwLq_*b;%a)!M-rY#n_eEYVz-}wF>{*9Vso>&y8v$iMgqD3jAhb82cy`q(y-Rh91*3EIjtZRu zTUl9HKJCDc{EcMJ8eqrAIJ9$!%PMUKH){oL=fPH>dGb5llCME9G+8i~tAS^D4y+Qj z9b%VGY31LQRV9zI3LV4ViFL5jhYR&$RD5Hx{3J7$Qgkkpp4*I6dx}qoJ5<#3IqZ>< zc!-2PhJXKibV8FMJ_$iG-y$2DhQ|u;fnD{3Wf}%{rh>~nT#ceyb1?<+yKxTXZh&jL ziXJ=#oD6Wtg1#0UqbJ4!C&N8=*oC&v<9zu06wyaYkZSO*deX(cKrs?eAGJi-pLAPu&sG}T)cfze=1m072E5og8i!g zvq0m8yKHeq4Z_iUji6}-7q=KWhGzJuy=P-BIZH9&$7h3YVL(sfi39So*F>)TS zFt-pgXBUd5UpwA&ankB&Rg6Qw<|veESG2KN08l8FV#O$1YZ|0AWz{h_l_2=%4iq0R zM96!E>`9!BA1?qrzK>4Ow_s1AX$K!rQqFmdbjsY%=`h#1P~1}oV#y9*>$2P0jwgQ* z8wTZcLt1mKT0;lS^sqzP=(P&(1MzxH7~`VHQCj;IeUjX7V=HtQW{Q1=be8Hi?72c4 z3%Agg=6Zth`&Xf6YT?l5USJtl3435emq6M|X^}S5e><-_(*5hO$lbZQ-0irKx86p~ z*U@DlYXYv6fq%HQjgwHPxmAkd@v~H9PM`F|V;qv*o>2ExL9sz=p$#QAdC-QCwKV)f zzX1v)>+GC<9ikUHk9Y8H+w(WUY>JK@r*~4C1VnQqZ?3-qR;~fG{Wye#Oog^>lh(Y~ zJ38$A9m1XaHvXDJXya0sLEjYv`sNJ5mu{`G^|9;~7YXm>qw5nKyu7E2=|sS-S}<;p z55}QYM@J6ch*&@(YBl^=V6iP_S}Ez|tKA%iqmqm8eS; zc@rZkMI96`dTy5M4cC|wA4y1Col(8a6&`zswBu#>#fj89n2V8i%JL4oyyi~BbyKo@ zm#{Q89K-rCtgJby7bgGfN3&v)s@=kvwdALH_>wT;? z?iA&UZwp)UWhB=&q&T{$x>wP;HrLb3T$XTVK;ixjh=gnABcZ2g@XcS~7s9p8>ALX! zIF-4Q;-IrivB`pum2~iy#jd*doLib|Ya8@KQkoM{NeS6v1%o-&mHEbqa#0~<-b6WT zG|EZwFoGYWbg!JvRNNQpH}_XTc(TU@3r6Uk14tSUxDy6=_`u~-u_MML7ufA@y!!ck zPp=Z*A+Gp_bhSg;GuGZ;oUk7Ea%oPJN62o_g;cY#uwQG0U8>H%VG`Jel{PN{WCmMi zVYud7N?VO^(>V%$b)XGZhK?Vm_=soZbn zLOb#n*pXKOr?rCc5-x+46|6k1k)mOMZ~~{4n(sXi^yZR}4qrIQjGrBEF|l~n(X3mJ zc%NExCr^LNWYQjbf8OToEtk0sd}uRI1E|+D#b_Yik)Xa{!TSN(zX9Z?EfV&lFgedC zmgukZRB;c(F9k-r3;|G1|2aU9@Xkv_t3e zQ`Na-upK*HJ(u!V{fuw)aUK3$^Hprm5cICHb(qurz>YQ*LXciO>x~)%o;Sc{*#y|k zB7DjAJy?# zRS+yRMb(Q_*xPm*LI2ro;%D?53mcpz$*Qy4Tu`64e&l23tef` zwxWnv+yfRg)y6^+SkZ=Hr-+GCrmadB&#tiWObGdN!OvODVb*KuD!BM6Wj-C!zr1nn zG*#{S+`*3u*aVUL*QwAv$c|d@yHFp0AULsx)udt2vIbI=q7K2o5rUGn3w~#{OLTdf z7EU^mlvN)mn{#b#*3i2`U0xfRjHttkkT~PUsppXCpiqiv66w zxJXvD8F`(jd*RRu7DPPdDYSy0YsH}!J+b`Hpz>a*;h~hb<&GWT5{e%Q8`v6~!L3B` z(~jN}TC$hGG)-TCGviLNdd3RFi#Wt;tlB5&{Up7?=fA?Axo%~tgFn$&Ei11Y;c^_4u$bv*iO zqraN$kSNgrO-pI&J~ThsB*r*+KSyn7`plS@)WuWV7Qu zb_hi^5XjO9^|SxzG#{Dl0Jp7&b=3#;G)iXiSQ`h90IX|8@kw2Wcdvg@@da5_e8ODJ z<+uxVT^F!`zrpVB$8f=UHpgXFgVbyRO>ciBqOGedbG^bsClJcA4Qj4}fV}lsPE4?4 zheJjKuwV0o{A6?&KcNcn6E^DPke&Bw4ZDt0=Y#g;PjO&9x* zD!5GHPc{o=5a3*0z^Qop!fxaM&s4%zmh}i-{Q#`0I>3e8SgIcdkRXRy89!30UTk zN?x()3LQ1O@jo5~A-R4Pw8*!Fg}olELUrc&G8cFvy~8SA1oajbNT~`)J}d>6_bZpq>4+CP4c9P>@@iIwUCtkZn2BVP z>LUYPPDrgBj&#&%Zl9xAE<4!OPS9%UTHo2(#+o|YzF%pvIo43&@e~*Fv4BfoB9!>2 z%^K*9WFbGrQyamut%7@eskeK_Y;t&Yc%LvSCIHJ7=K?Zx5j^88qQ0ttp!+W4*AJ}p zAcyp(H6Zg6X!$eIQ_1H&2g|<`qxbj0($kB%^nqg3W-c<^+$9L!KGUXwiF^RX9@8Bc zOZjS@l1qrXa6v{$tXr5ZSrU*nkz&zCVA+oWN{k1D#yHH$7_fpZF(vu)D9qg&Be%{A zwl6Mpfp?ceNU{LO$7*@$6vWK@Jm_Vf40Ug!%HGeYyvuajT-pKk?jf+tfuc#@xx30c zrNfr9BfQ%|R9xj20{15h4g3-;`7ZV%#yPa0$Y#&M_nssoC%)!{yiP3pa|0?8AR!$& zhD|$;!e>7^A>NDkOS>W0K;S>vhWYfE) zVEL{he5RrR3My@di*7csv+p~h#^UWxll*M7&gZTWoB4d39MPP88~Gh|5#4?R>~ssT zUGX-3+*pAQnEl@f9{&}cmKxq|QJwc4l94k|OC1G-zGBnMiY`7_9<0(>FyCHbX;l_< z%AL1yUp^F0+oJi(Uvq<#>m7z^q0O8(hIROWuq^rkP8+5D>aaIm0vx-qZJ6sV^0~XP zYPUmZ!B&Sql;q8G({ox&(>EX#A7ZltKLFIrZ0lXDzEc>Gr-y`e^1deL$zz=lod5Y1 zsZ}|TohsK^kHzQMvdjNw|4%vnXwvxp3bRAHa92`%zHOdg-uU0XO7*^%`0YIj>582G zDVF4@rS5;Rx9y?*e{hP)_faZ%@V_)xwlty<8{TFXCBH`foEhkF&Ygm@4_EM)Lu6ma zAYD%ZTxxCO2v@;w1^{kU23#HQ0BQqn)J(6qcLOU==gl*}F8I>nOwCuvvC$x-jb;6yQg}M0*+}*N{KZxZSs!7hZm5%9JJu=u%yG%I1#^{8^HIeA*3Vg9e&EV0Si}^QkSJUm z=CzFpp5*<}!h(%#-hEO#zc+VyJzN7z(m|I&c$C>CN2?CI(Ce-~vB}g|$HG5Qh!yEv z=3^tWPt?)V9TtWz5?X*Cr6+B)%})y|OnOa)B5f`uT7Vr3RpPmg3XgZhQpT((oSlMd zzIIycr6-7{UhEa`272*to3jJU%RJ2OFw=w3lqP*0V0aFJpmAEl|5ifIK7!a6C}hV! z%#GVF+k*}5ehIMPn$Yf@0h2vo_iHJ(K&SimbQ|=P;8N8ga#=@j?Euj9nc%!~4p3|- z#iD%y(GfN}9Px-Uzv%9IzMeyzjsxr-B@}0Y?aA-O<{w)_SkxWS9>1XaOAO$rk8Kj$ z-e6q`H_|bLBvvYIa_K<%;V$}x90sF=O~N_}A?rEV^U2Vx;SM2cAcE#uCxyu$3cJ-; zO4ZPAlHXUDau5Y`4(ijn*bn?2q30`Tv-kEiFVj1;`SchJz7`5=DZ?U_nj*fi8Y#v9 z9;I$9aY;1WIU)Mtv(=!J`=7wHYc5KB!~Or2cOKwb6qj!~y~WUIYaMr3j)TMG;hV6%nQD&_Q}{DWuQ|y@n+B|2y|hFp=Qy z{&&CU`<`8R=H$#dXU_C9<()g@uvEW+<_}xM^-i3do1)yTJC8L>{FQsGaXzcgZyxPp zerV^=UZ8w*TrlEOQoH;j<*?Lq*Rel&M(}0o%6@#YarXa;@=a?2Yp%vN#!?OXLf!s>Yk{<@7pp41 z%G+pems78l_#-1KH50k;l)^rk&7UOK&}qoMxLX&Z?~cW;I!&?coJTGeme}DBD%*9f zqKwHSwon5@X`}YYv$U3x4;z~KSj7m;;H-w_zPuV82fB325t z;q{>n*=Aw1KgC;frD92xi1*z|hGpGSuy=MlCn__iQd{+3kb$K98rrvt=#1=@&yfiI z)?gZXN@uMfM24+Lq!FKfMlt56!@4*cU5!Uepbn0OSh_~D99#vzu^*ttAeuIxxX!Xz zlXH*=xhCxicXPMJy16%4k{4nvr-Ee`1M~YEl8$XFwhrvNB)Dh_@vHcjuzNMvOi!c_ z$!Ci8u2{vKZ9QhgPS_4HbRF#d7(G?|kYUhiO2X_prKNWcB~A9?-Wt@eyugW_3<1tS z{iy+1^rHmWYFXrT-CWSg4HT1`4PegxNL-S|3GrIQq@q617>jL$sg?r!@(4Ipwh@zR z)e6O`?NV4#GW)3Jtys`{z^@nzXi3c}&&$vr7^M(wxKzq@R!dl)vk|~-=_^I%zc)k@ zC>?b3OhbS;qFdWZlDxU41>7G;tl3{$?6JHAcI*vBkw~GYRR^qj67P0u*FNz8-hDDc z+v#&9U}|HcE}8`Y=wTEGZ{5ZB5M%nU;XBt-*V)ai1?-!-?=zneSH0~lfhVKaBS!*+I=NJ*dR?4?g9vw zyjAiwfMJSN*AYPE3X9}6eMRyb_pzq$bXK&nUuCsr?>GhF=y9^LCtES$z_!`vD&alX z*2pFz;KE91_6N`|1XxnkY43nLr=r5^%Wit)6p5g`FVyT6n&jN2S;2J#i&_A7;yDXi zU4;=SP8#gZ0}!_te*|k2$+b7t1ejI|;_;yxc%m(-E+R_Kq!QFWL=&m3)#a$T_k66{ zkA{vM0pUtR1gdOCp?ZFVz>Zfn2g9Bbw6C)T`$&+XmrW)>J3j*a>B-_C7t$CS4PL>z9z661E+su!te#*?VtI&Dw8Kit~(2wYiCgC(N^Csr^bJ) zZI8(Jt-xo{C-?d09Nl6HkzY?-X^feAFEs)_$-N7 zzl=b!S%oF`$;~K#>Z%HSjzqkfN*Stet-zjm-ac>Hc7x|sAt{OoVA9QZ_w*yapL9k+ z%xCG>DE!dTfG`!{-D|wfu(A16IcWFDs(;kz&8Qap9hq`U$IRpt*dq~i!OB$tnL|+j;$uJ zS`vgZ(x$>%fmKY;Yt$^XEz)^*H{nlu5nXy3*u|xqVc;i2LJqLjVWsUNRB#8V=Ry>A z^=*sw#~{=NE&{W|;g%gD5e2W*@^yeSmDqk0fg+!4{888CX9HE!3yQiQP;as#ncWFj zV>VikQ*Ns2Mz8}Jw*d7!=N-FIcH;t_Z4kW;p z&!l->t)ywkNf7^w)~5kvH|l=6l!|{3)LUzcGfYuS1}s{dY_5W19k}^DPD85|{Wio8f%LHFhg<$PJ0lc^x(7S?Set6Qb{LQwQGhR}xO*<$})klpq zjAmmH@|Z`!Ow@6Gt7aHwl(91>0w8+fKDQ4Ki)T8)>wd&(2^%jMGdADyIWCM^kgb|pg2T5U?a#t@H_p(Ld-#PS1Rr=VPwmATTf zd!UsZD!uLdBZi*;six&cm9s$GYpfG<8T(W(uy9G$u};$RK8^=Xn*&jvT5>;a^i~HH zRS6~@Rb?>j16^QO(6D#THAEH(w&emKWdnkdKPoay+frqo1lu{#nFCKM0zMPIJ!Lgu z;A#Vyp!h7cv@m`Ed+ig+bIVsT9O`a@sz_sqahb;codAm*V6h*!7vR$bTJHW_qCtGp zR+Mbs0JZ0Nu+_9Q)n>5MirKdNW3s^^Dm#1OWF08q02i45DC-C2E6a)(@&KD-PwgoKvN;>@P=%V zU||9(YEdP-yUK$!Y(6QCGepMb(}}{b1a@57(D8_@UhyYu<3Z16i*FW6b;EJ>BiLF7w) z*7)IXk;4DEFZHhkJ2NtWn+ScoYR1%B#Am;!n9SRc)yX*16?u&z+`-(lPuhs5{Mt%u`w`!pdeEv@+tY8h$hw>FVzI;XpUx5>ZYHYk^ zd%XLs3hX61b3Vd1Aw|2d2f0(45!~m4eUdSkb)k|^(&ef}Jf!4Kt1wvJ!??i3zKluX z4G+Tn_ssvi*-dbMCZ!af{Xj}#3!T#5+k(1J9|(4U-yQh1b))pmq`5Sr(2q2%Y6Ppd z6P5}&WC@&4vH3g*wG16lmCBKpR&%BHW#5m9ot?SVFwJH%V&4c?tWBN)%Y0C1!|-Os z04jqs11# ziNbrg48Aw;F(r8iv5H@l^oie*YIZ!BtMrF<&ss2!$H|fnz0$DxSp$r(cbp&hwuh6H z*=^-#TU55`lIwdyntHx&z)B0VoHKw6>u)w0ObZ56t~sXeZwng+T^F*I#I0gM#65GtkGHe zJy7SaS*Ig0geE`02nG*fnx);QBF32fm2s)d*>jH>|)u z(t7d-vPVoNkpFv{3`Mn5Obwi2+mpG0@|7lHzf3|r5mH_qp?MF`bguxgfGuF5GP&~4 z0)#J>dWmjs0RwNM=)_nl&to$c@c224vP7qSW&t`+&<^a32*ISJ&x|sR<97iLWCM2h zgSNV>6nuj8e2=$Q+MI9mis?K1={9}d1 z1T7e)<{`=2_MsMVcaTN?+6j=v?`tOM9@pF0uIygMUEW;vajTMYuEe3c%-v!e_Aes_ z`(PDCVcaC_QIR{+6a^~GhM3w>T3nfi1aK}x3VWCYl(-*n`I54jR+R~*f=QNrVy1vn zgwOcgFyZ=Dv8faf*!+=>pxhRb3p?{?f~uAl*fIjD5sIYlzasMu-zCDLF)80;4$Hz; zwznjbW=&xgEk&r*HvktXDm!hoBBgXsUNwStU;`^4`%p!REr0M9Pad`A=Tx>SSjFiFB8+IsG@0?u+p>x6|zO7E3}c2wTh#9*Dy!w6jjPEj&ip2-Y22R zMX?;nUA2hB@U#Z&f9H&u+%)RGl{BC#HDdcE(^FpG;T%hm`7kGVont85u98UE98t~s zNolZZrX;WaZqcp|0IpX-W%d}bYc&Dck%opOPwCmemmyUyZ$C@+X8li~p~xYChrflt z_;P_tda8{r1yf+cSDCWxcFm(_c*MOU=CJB{6sM=dE7uDwZ5-g}cgUY<3wBCc|M7j6 zoas3wEzG|e)T>*;d;>J8yx*ewXyo?I?qGhugI)iEq}k_28{ow?3WC1{_K2arf&n+C z3QbzerD__+1iWV?j~PDD&c07);aXk%-1mxQh?h>=XtJSQcL1wj1MRBXJW3*G`Vy$k zCK~6??E)ve)Z^9A#T#1gZ(J8%BENEfFfDYe!p>Gk{i=?HE!Fn4ss3_iIYZ5S=);Z=*fc^$M^6F^;0iwBo>k#N=C`OG(Rx<&DPtg+Ig&~(5?sLC>Eo3wXJJOo;ms~U|=41u-RL8w+wp>ZNu+fG7k;wLb6 zHcE~Th80o?B?&vEKX}YFv;z`r+i$_nKBdV(CA?V=8!!~vnFGK)QY_{LK5*r-UCzTP z>Fx!-SP5)}$Pu<*<@W3XoyL7DvmE|$K4y`Xrf12OhLtA2yNuTy-tFvW6%6HUCP=8; z?(OEqR}Nd2;;cb0RD5vJva`;zr*qrW9tHIs`-TLHKDkN10Lxyw7SCB_$W%mr%AVejMZff9Qga-oH$_k2`V&H<)}m;PRpwImaBG zM-H4X2V6}L8%K__0R`JcofHaB+JtiwO>Q&3^?_5HX%xA9;B@=L9b7T)9szGV+Et*| zoVPZzYmL#)(XN6KS8Z~Soa>3F2~<6#Zi5r#>86K)_#TobRFL64K0jg!&V9y zx|<3+Ss!ZgqneR-x2Dh;{Osc{bl7vjQm1Q1oa4V^Je@PBxu2b{ScHkS7hglGT7=lT zBbxAY473XhvpWh}@&nL*|5OCGK4}40WTL-`hPrAA0lU4XNJFCv8rY<)#R{u0)VKqH zss}CN<5Nl0CrBgs&)h;HcMP8L)17>x67?!2m-7|bul%&3G*1dv#i9xqt7@i0phi!A$%y~9!D!tm3Kfbe@ruibA1$3fq(Ei_O1jgk&RmWVZxStQ`%06 z9j1t5KV`#A8q266q(CjO=nPey9}jAEP|P7G8kWb$3A-;IEhUR0luF)}>mF#yi%FZk z+LrR%w+>cz2SDPJfHO>Z?Bt&n6YGb;QkYWMiO*<8Ne{q*q0X%H7n!{5P9srI*9JUg z8!n-F7Et$IK-(aN4L(dExLOypY_)p2ah43CBpp_{UPhJ-fTQ063RyBBblZ}_{PmJy zHJb7*8A8;pC4(hP2EZMb41~*DG7xQHO9tA82gbz3SK%xLKJ+HUr>8W>A(NDCKRF-o z3(2JJsd-@Y@7DY}cT*Zbu(PTP%7W$D2-QJtVfnPRSRB+rGBHF`#ux!RVU^TI>gxjc zyO4}bK`3bo0prpv>O6Qh6{UiELh$_*P(9mLQfb6J9Rurnz0Ql7tu|Mb;jPKe>Aj5ot0>9HwIGCVcm=6{D zJ9j~SUHX&X)yq_%a|Lt$vmnkxYce-HKhxlrHzxW%oDo3YayDeYSCXwD#a zw&?z06w->m(qyI{mFBZ-D6F+V3t{;hfeV^aK#z@pntG1%V1J6OjG|!Ca|n8|2ukX{ z3hh!Nw2HJS`+O6{kg~wg_Me37dsI^o7ZkeA4HJ!f>m#&zyF%kgSideeV#|MAAiS;n z30u1k0vE;+wp2d^vOb39DZN^~0`S-YL)F&M#MG%$3PajWXfNaSUPg$UrNKNNp!eio zJjZ>GS~;qznpoDnEAixG5>LkXG&T{KQD{t7iUl~ z#X=Q><#5WNm23g2<}(CF%u>Ty>pn{PY`Oq1yt8Ir)D8KtOlZGQNvhaGikiLOVlMTC zVWZ%CXr*Z;i~|ZA7h}-l-^Bt}S%XmQbg1QK0qT9FAhw7UR_p@=-OqTZ0-m*~p$S+m z<*`#yMO{&4cLvM;#DHZAZKz;Z!7{y2>G>vD&QqG!uoldo;><->RAS8_x0KUrpGKa^ zKf{L1L7gX5>~$TD^B9!uf8Lmdz>n(zS*ePBq`OIzz3@eW`#lj;qF`4rF@$~uc4(|c z9=m7;_$atxeZbC1&!{pJ?B3S&E&EU+U( zCfz$ST@3?=rz`_%HAm9BCY&0{fU-^TR%~WrMmkC7JUPK&m#=8b{$NwXs2K}Lm`ssf z3J0VmD)O0lOkl!zL`x^Zzy1SQ!$d^|4g~Y^29rji0;V9SS~jwPO=Xbt!1Ac3Us@z# zy#jmA(40Q1hF`9Hki*zM^0lCkh&_BSWG@*I%XPx*$}+U+p8&pf6e2Y&8pSZMVwH{T z$K2`nciZmuutD`v%YFLdj`lz21L-EW4shH0ql(5Kkw&Fd%BTcF{rQ&f!V^+cq4MU6fC55*G znCDpySg9y8?;8S@%(Uzau+)zfkDZ33`oS?k!)&rFci*>S79n49@I8n8@bIVCbqEmD zh4A@a*XaW-xPhVX90#k~1MvU_;?g7Q}hcsh-fP zcOx16+=k&$2^mICYFGZ&kQus8-upnv{`q~N8U(G-l-Tp$2O`o!-v@FYd5|kxHIcI}@FQd3M%Q-h>tL@=dP$iisenN@nh7{Rc>b}Pm z*08#EUP_<0RR!^4y87-Di@&rA+3R$Dn5tQs)rcmk^1+LGeFRCED8FcACw9kA-=Wib zREn_N2R`inMWJ%tD<<`8%Jku}FwSDF@*d7}Ey>aHJT+>$lm@1=KQnH);kWWJsP(8c z8{w7XMQwKC5sN9{L6l9n230md+?Vh1NwsYU>`=~p*!kJ4PA5FPp@LcrjFWKp@h9G= zC+OTBmYpL9$kY=vEV+eFFST+zz5BXcxy~5$t(!L>-*NbYyPBagr?GCChDs+IiMh zDk1Al(?Jc_MXlOw)oJXus1nOt#r5<=221!*! z$$C^(#MarTf6AHM(Vq#>mSUi&;)(qmGpsi_pB?d4K(;lzn&gn7bZAz2D= z`4Zs7I|jQn8;~{?@39Xx!N-bsgC#)*5S^XzzG9Hx3@iIGak>~_?3+7@)3XGa9S_Yr z8g6DwFwZBcdH3gKs}dDvDs+cIr;nn+Jz_CqoN2&j7_!lLmB4xwo~rN*AY>&%^*^;( zqqNa9zK@})S%TFS!Ai3=ZGsCNop;P)UO=r)Rj8O0)Ml(kE@qb|bL|M$qO{7h|VY(mfD@%wK zVu`)ygPk%&LXrG~5)>j-)Ukrm=UBI4j_5ilr1M8>CsQdGy zeNBn%)}wt)U&?5HkaLdK*TA$m_dM8p{0w(^*%uIvS;(-!d>z!gA)@LvLlU=uc}@hl zpDy%==2=GpsL;Hp3&DK0DeCQ~I1_aGR_+1f`!0Yz-$SA~Rg}=RuBhA}O#h{4 z94+iYL&Y5bM?hiEIBMGHz3~3AXB=-LSz?dQ+bi`9dRvZ$@)3DY!-suya^FAC=%j4t zJ{0-&?!|?$@;==!u-3QL`@}9YNoR*3jmq(OtB%tu&Re&3`M}%<1HMJinYMsay()&7cJP;)IY9V$m+<-=}!M-nP_Br0f;!Waa$s|B=O^~hP& zabXPA*vN7UP!$lBiZ_aKQ8m{nj>)@{_b|lweM^jGdPjH_JL(=7zfP#z8%>w9H}^8Y zE!QpVR}vuILhHtJ@H6TI4t0f=wFcV3QefHDh1OOY?~O?OU%mr)d#?<@?vH8*rCOjT z-_mx@p7$_FcJ@*v3+b`;(*gf0avpvxYML^&bAYHsjk5o zR)d;32+Vm#Ec;dR;e|D~bIOA`ml58$YSH;JFw`R#)5pP6&Kq(4URjC3N_N9T*5O29U z8Wk1&BwZn6$->D}@Qb!lH1-$adyIgVxyWF0I%7;+0ceL*+hiGS#>b#JwzUr&&?87-|lQWPWRa{_m7ox~nVaholwdR;J3;Jsv z^7P4{_406Hu3jF(zD=(@p2C5++ar_D#?Q{D-42;Xr?8#haSr$p{?Q9e9WrWy#a=hI ztMo3gE*E4=xcL>(m>U-2iQWjUlVYxR1F$c_g5U5OiJKZ=uo2nTd$fl)VKsO|GdK=` z*w7iVL02+?N+qAjG zmgV2@PCksX9y2swagZX_@e6?S#_Y<=d}3p__GSciJwY+4a-q_IqWuYUX|iD^Fcr)k zB4nAVhHb+^U?n}FeIGy@vsVs7+}`>yglj*-UqAtK-Gs&>^aC9I0Pi;1ft@%8?8;=Y zlQ9zH=pqSzU^r2f_|9N3UR31y5k<$nizuqnV(g12g!T>Ms(FTd*{nK$X;JnM8VE~} zD*FcB*d2g!9iYWa3y*qG)7QNLfom;ps<}RNT)4Pinwo%IU-> zc+Yv4Rt(PGxs~fev z4cPo-Fj}?VGjYR{5MLS~R2#Rec`LB-bn!ZC9^kB_(7@ZXyPu z?C4VD%AEmAmQqQstf8$0J0=M^@*dtJLkJVvUT8s@$<$5(X%@7$VCBv$R1LMli9(9g zSqnK&yb}d$G7=zXF7JHF*lnjfQN(14a2;C`Nya1E8_F!1q7=B}i*T*b`_4tuxo`m3 z^K<0a!uAhZ1a0%0!6NJ=d`vk_&r6Z>}{5ZnnBg_{=GaA^sCWWv#cUT-kN|dpBG8+3s7}-dQW7O5A8EyK0Xa zM$VT|RwDxqK~psY#v{;bQrD`^uZoR`svrzGvA_0 zRfPdyyQ2*w>v_1FimP$7D0N?1=ISb@rA9%_i!ic(LpXY(qRkWqYkZtwJ6r@S^BCAy zP65mtWhjQRjSxTZG1TQ>f^CaebgbTZ51hk$^h>aVE1>1Hz^c@)p4mSfjTr+%x|Z`mP>lH<29M#{&WTF90qIIFwA;+WQw! zK=$sRQR08jBA;Pk(X-%hzNwgPv@^`bF$?PPz7lVX9f+$YJ{og)4axYUU?JTBHIwm1 z(#q6b!wn2Nv2@;W>g!u9vh{q%&^ixyHp8{O>5R})bn;h>xke#1n?G z9s?Ra9`DUpjQjH;*&L(f-G=}}Yl6_i0UnV;`^18_9o=?4Xx_Uh4xaKsUtB+CPtFM`*VaXrELJ&{>xb4R?G2rI zH{l-W4VL6ZIOlq;{n$>`Q3`!&jA2mksTiPY(p8)rvi`Ur%efy55QB;lFCo5gA1b;% zZD@L}T1g$Y6{9Tcpnv3oE6O6)vGWb?CBmaWqpLzOu*K_8@^^2!nv10SmaBml>u||i zS`BEqw_IsPng^iCEm!1Y=}C*j1q#KQGE(cMnJrzTnX#?Hn|C2ugjD${2y&~GtIYji zgLWEB(|ctt+x=90hcOm?;^h=&y8=@UBiDT+kt+K+px$6W+dA;o!%n-tdot-ssSCA9 zGqCk@H94#ZcHmt@nBpYqz&Bv(1hoCjNGL@@&%`!`7LnpCCn9!N6qKV0*q2&b^uPUJ zXGV&GSc3hlS3ZrdCAR7R{_%Fr0Qed?Pp?90{9BK=$0<$wnL`9^HW9&5Kd}4L48=d~ z8wG6SWw?Rh3qd>dv4DSZtQ*OC#ZdQkL&etPLPe!#S){zPh0Qz!_a8pijd9C6*DZ+} zjFyl}=yNW!`xlONQ`dhwb}GLD-a2sUOeL{=IlVZ%CVQg09a9uf`uBdMS!xH6n!R;N zO{FoKz4WtSZ7RV{t_F7RyPEQ8=OwUBj-jYRHMRAMVsLl}e()4Pje&Sw-3(ZDDbAXc z4abD0{DwN2>E*mI+S!av-bze%CsZGq3hVse6xe-WC$1X?=O-=JM`iIIn~rz8{7hqS zo2r02u4?*^OR@O*n3!sx2D^Z@;P?@kjd4@qaT0O(MQkQNBwDiT?=_B4s)dEseg@2Q z0ho6kfX9~#?=N7U7XV&C25IkW0$smaD4Yw`wM0}uq`{^-M*g>tw4L*=i)fr%Gui2u z{T6lQGQu)#muSUaYXkt0k~d4F@~2GqnC3~rUTBIY7aFFDd&fBIV$EyrSw4VKj+vi zKXKY}uvDt@-+8csecUvrNo;U78NA{iv!45Bjwzis7 z)|!5lznw6hW)ZCwDXY3q11r|15d_~4h13Gibo zSd^5nA|wB%;Z^>#K?+vZgE%(>L9t8Ihj3gxsISMZu7FJ zd)k`4a|E*05)n#Y4M;Agz)Au#ngUWHXsCATTmw9lshPWOv>1gt80PJX@XzlBY`X^S z$U?xe6`J{^vnzf0Cyc7SEsij?1|qZ$L#Sql5%AMx7O5Ko=+FmRvwIBt4Ju|l;ZF{O zA9Dur?SmEb|3%PtJSbRKLk4%kxbY#hElngriRqHKQ=byXvozGJEiKIS-!#3@(z%o3 zD?8wFPb@K%d>(NXQWI8K6d<_12o19cy$GzdD85kvZ<(*avW6J2?Q}YRN*Pka9I1|kOeXxCx zX=lzD(9vKnRueqrZVSytEym|7&~k`KRa~sV41ua7C`xK9SZD)HUM0fnUBPb5fL1)( zqK+Rk6z*x~CxJ8dU|pWA7;{4aM`jzWU4TVZzl%5HoF*+&K{=%WDKa;*PghaEgAu56 z)k7%p4TW)AE;}WidB5&OF^qx3m9dC#Tq7{G0ujWFLLloQ%>vI`VNi|<_7m~BrU85`i;CC?&AXD6 zm-B4Zszh3biu{>?`(K1rH->;GCV@5jLCUX19RqBB6hipd3UD3lE=-duGZGNpz@Xh9 zS`_05DR>FZu5Si7Wp(RdSN7qG=$B&^w@z*47A%fGXo~iKkhV3-od3 zqyJNW|x+ zoXeW9!K%dxSZ&UsY(pgA?GJQecQaw3<6!(+8{yX7EnD@+z!Y-76^)VmtKfHEZ_&bv z0-mS?c=?RsUc5In?cy?C4mwA$oA;fj)$KWn&Oka3*Gr(Y+Bos3!mB|SM!lV~yPjwe&9H`x9R z3wv=uuq4A!{)1*6lmjh35iDt^X5Rj?luP6GT1v-PVJDWtudo(Y-4`s%#nGNPgxxhJ z$b`Dw8J3DLU?sp@>4G&vQVx)D=;r2UgfH}>_C^l)gWOMx9+1GfJ;K;YU&hI8g! z3--Hs&wr%w-UK`S8{h#p`MU&n~fON{irU4^YfxlLhM|*d>5^8{qRT zw9IlAv{_Q1htlzLhMK8FXjjF2eU%}#B@Md=6~Lk<7}8y>l>%1Z3cjOT7fE#s~`)N8r~;QdGptfY|wx4&L#g zx~{)Y686QCq)Uu7_WmIjqmn1s0Y~WTeQ=5~MdUd{6nC6tD6*Vnbb1KK#*Xu@n55X_ zB;}!epMvG>tpPp;et)ncInLzK*+cPpLbHdGevXQmd!8)1_fRHGy2Bm{D%^W00kQ_j zIUhqzuAtwTaXyhjX!*%Owr6s$E+av=U0nw4wyVoXlXG<$A)On|oS#0O3tT^CX(6w< z@H5~-FlW|B+vk=+8Mj{@q;7>&qiUh1Uui`9(=hDejSOiavR*(Yc_Ug4%E#R`wFZTsOp-u=bV=|M^izg5Lh9 zBih}MI-XVTM;*y)c^adAQt}!!2P~c8&J9k=NODs8MS3L%aWS$Jy0$o<4D&FPGPqWrvW5 zQ6^aY{?Qa~jZ<$%iFHPH>S*jq{iizQ*Io*&?i`-wx1 zVgrERnZ3}Td|AS~hSH>|+Ix(Jk=={jrc26Mi}&t}RAXtC(>_E> z9($LcllV<1T7E!Q^`uC09)enZA_~?m0gD=q_?8(KgXOK|)NG#NE zMQF`Ru>Fr1Mx`}?6Y+SHwkb3QfF<-K4bGBPe$x@_XYZwOo%fh*S=tQ94EG4g)(NCC;OCfS*W8)sJ8ts0mj0sG<(N2v(IGtC$2poqE!FbD!Gtw-_+D zKn)roU=iNZ%h322Pwm0YeQJ*ox1QRghPJQ{QWevP`^ep3l_vorPilr>x|9mt1qiP% z$97a`n#IiF5-h*Bz^+vX^FE@%Ix4UYU>90JE82iYrOJLJu+(@9!Wlq7Ff6ZBKoM^P zZ7sA?#qpNf3J7@=Z}4J3;C4gva+-v+A=+pcgKcR?jEA2lieG;uLZ16O5HEHRXm{B$COu{7-1ob-6O_S!ta&= zZI7*)zBEC}K3O^1+ons5`&UT#pP`kpQQK+Q-6}Lo}&qY>%($-tMc<`Q}{O zf4AMnw(DLJHEBOud_eJ?ObKkRg(L}+l>EfP zsJxP$CrX9DQ?d|pKJP3MuCaAd=zZW3MUi`J*#5tQ)(ci*a_ynJ2|aQdF+`k2b{|FK ze8eH$^^W|5C~){nafD8(;_ z*Ay3qtAg8xdk@zRR}B}7`x18#?k8MxTwPpe+`YIqxH>qYx59P6-Gyt76L|?+2`74D za3U9hD~XeERdEe)O>ht3Bn=I558>i))p0FxA}8@yz=^ElO{_-9|oox>f+CE@nswic*=$)Dsm5+~&@WvOwJrxLjPa8jpF z;-vnC_9ku+PRdN$Pk)@`GXy8)EOjpa!MMEeMZtY=QYWHM>b@}N1zpNl>NmfWwvgY2 z~gKx(qDo_<#E3) -> b2::Vec2 { + b2::Vec2 { x: v.x, y: v.y } +} + +fn b2_vec_to_na_vec(v: b2::Vec2) -> Vector2 { + Vector2::new(v.x, v.y) +} + +fn b2_transform_to_na_isometry(v: b2::Transform) -> Isometry2 { + Isometry2::new(b2_vec_to_na_vec(v.pos), v.rot.angle()) +} + +pub struct Box2dWorld { + world: b2::World, + rapier2box2d: HashMap, +} + +impl Box2dWorld { + pub fn from_rapier( + gravity: Vector2, + bodies: &RigidBodySet, + colliders: &ColliderSet, + joints: &JointSet, + ) -> Self { + let mut world = b2::World::new(&na_vec_to_b2_vec(gravity)); + world.set_continuous_physics(false); + + let mut res = Box2dWorld { + world, + rapier2box2d: HashMap::new(), + }; + + res.insert_bodies(bodies); + res.insert_colliders(colliders); + res.insert_joints(joints); + res + } + + fn insert_bodies(&mut self, bodies: &RigidBodySet) { + for (handle, body) in bodies.iter() { + let body_type = if !body.is_dynamic() { + b2::BodyType::Static + } else { + b2::BodyType::Dynamic + }; + + let linear_damping = 0.0; + let angular_damping = 0.0; + + // if let Some(rb) = body.downcast_ref::>() { + // linear_damping = rb.linear_damping(); + // angular_damping = rb.angular_damping(); + // } else { + // linear_damping = 0.0; + // angular_damping = 0.0; + // } + + let def = b2::BodyDef { + body_type, + position: na_vec_to_b2_vec(body.position.translation.vector), + angle: body.position.rotation.angle(), + linear_velocity: na_vec_to_b2_vec(body.linvel), + angular_velocity: body.angvel, + linear_damping, + angular_damping, + ..b2::BodyDef::new() + }; + let b2_handle = self.world.create_body(&def); + self.rapier2box2d.insert(handle, b2_handle); + + // Collider. + let mut b2_body = self.world.body_mut(b2_handle); + b2_body.set_bullet(false /* collider.is_ccd_enabled() */); + } + } + + fn insert_colliders(&mut self, colliders: &ColliderSet) { + for (_, collider) in colliders.iter() { + let b2_body_handle = self.rapier2box2d[&collider.parent()]; + let mut b2_body = self.world.body_mut(b2_body_handle); + Self::create_fixture(&collider, &mut *b2_body); + } + } + + fn insert_joints(&mut self, joints: &JointSet) { + for joint in joints.iter() { + let body_a = self.rapier2box2d[&joint.body1]; + let body_b = self.rapier2box2d[&joint.body2]; + + match &joint.params { + JointParams::BallJoint(params) => { + let def = RevoluteJointDef { + body_a, + body_b, + collide_connected: true, + local_anchor_a: na_vec_to_b2_vec(params.local_anchor1.coords), + local_anchor_b: na_vec_to_b2_vec(params.local_anchor2.coords), + reference_angle: 0.0, + enable_limit: false, + lower_angle: 0.0, + upper_angle: 0.0, + enable_motor: false, + motor_speed: 0.0, + max_motor_torque: 0.0, + }; + + self.world.create_joint(&def); + } + JointParams::FixedJoint(params) => { + let def = WeldJointDef { + body_a, + body_b, + collide_connected: true, + local_anchor_a: na_vec_to_b2_vec(params.local_anchor1.translation.vector), + local_anchor_b: na_vec_to_b2_vec(params.local_anchor2.translation.vector), + reference_angle: 0.0, + frequency: 0.0, + damping_ratio: 0.0, + }; + + self.world.create_joint(&def); + } + JointParams::PrismaticJoint(params) => { + let def = PrismaticJointDef { + body_a, + body_b, + collide_connected: true, + local_anchor_a: na_vec_to_b2_vec(params.local_anchor1.coords), + local_anchor_b: na_vec_to_b2_vec(params.local_anchor2.coords), + local_axis_a: na_vec_to_b2_vec(params.local_axis1().into_inner()), + reference_angle: 0.0, + enable_limit: params.limits_enabled, + lower_translation: params.limits[0], + upper_translation: params.limits[1], + enable_motor: false, + max_motor_force: 0.0, + motor_speed: 0.0, + }; + + self.world.create_joint(&def); + } + } + } + } + + fn create_fixture(collider: &Collider, body: &mut b2::MetaBody) { + let center = na_vec_to_b2_vec(collider.delta().translation.vector); + let mut fixture_def = b2::FixtureDef::new(); + + fixture_def.restitution = 0.0; + fixture_def.friction = collider.friction; + fixture_def.density = collider.density(); + fixture_def.is_sensor = collider.is_sensor(); + fixture_def.filter = b2::Filter::new(); + + match collider.shape() { + Shape::Ball(b) => { + let mut b2_shape = b2::CircleShape::new(); + b2_shape.set_radius(b.radius); + b2_shape.set_position(center); + body.create_fixture(&b2_shape, &mut fixture_def); + } + Shape::Cuboid(c) => { + let b2_shape = b2::PolygonShape::new_box(c.half_extents.x, c.half_extents.y); + body.create_fixture(&b2_shape, &mut fixture_def); + } + Shape::Polygon(poly) => { + let points: Vec<_> = poly + .vertices() + .iter() + .map(|p| collider.delta() * p) + .map(|p| na_vec_to_b2_vec(p.coords)) + .collect(); + let b2_shape = b2::PolygonShape::new_with(&points); + body.create_fixture(&b2_shape, &mut fixture_def); + } + Shape::HeightField(heightfield) => { + let mut segments = heightfield.segments(); + let seg1 = segments.next().unwrap(); + let mut vertices = vec![ + na_vec_to_b2_vec(seg1.a.coords), + na_vec_to_b2_vec(seg1.b.coords), + ]; + + // TODO: this will not handle holes properly. + segments.for_each(|seg| { + vertices.push(na_vec_to_b2_vec(seg.b.coords)); + }); + + let b2_shape = b2::ChainShape::new_chain(&vertices); + body.create_fixture(&b2_shape, &mut fixture_def); + } + _ => eprintln!("Creating a shape unknown to the Box2d backend."), + } + } + + pub fn step(&mut self, counters: &mut Counters, params: &IntegrationParameters) { + // self.world.set_continuous_physics(world.integration_parameters.max_ccd_substeps != 0); + + counters.step_started(); + self.world.step( + params.dt(), + params.max_velocity_iterations as i32, + params.max_position_iterations as i32, + ); + counters.step_completed(); + } + + pub fn sync(&self, bodies: &mut RigidBodySet, colliders: &mut ColliderSet) { + for (handle, mut body) in bodies.iter_mut() { + if let Some(pb2_handle) = self.rapier2box2d.get(&handle) { + let b2_body = self.world.body(*pb2_handle); + let pos = b2_transform_to_na_isometry(b2_body.transform().clone()); + body.set_position(pos); + + for coll_handle in body.colliders() { + let collider = &mut colliders[*coll_handle]; + collider.set_position_debug(pos * collider.delta()); + } + } + } + } +} diff --git a/src_testbed/engine.rs b/src_testbed/engine.rs new file mode 100644 index 000000000..62ab37efd --- /dev/null +++ b/src_testbed/engine.rs @@ -0,0 +1,694 @@ +#[cfg(feature = "dim3")] +use kiss3d::camera::ArcBall as Camera; +#[cfg(feature = "dim2")] +use kiss3d::planar_camera::Sidescroll as Camera; +use kiss3d::window::Window; + +use na::Point3; + +use crate::math::Point; +use crate::objects::ball::Ball; +use crate::objects::box_node::Box as BoxNode; +use crate::objects::convex::Convex; +use crate::objects::heightfield::HeightField; +use crate::objects::node::{GraphicsNode, Node}; +use rapier::dynamics::{RigidBodyHandle, RigidBodySet}; +use rapier::geometry::{Collider, ColliderHandle, ColliderSet, Shape}; +//use crate::objects::capsule::Capsule; +//use crate::objects::convex::Convex; +//#[cfg(feature = "fluids")] +//use crate::objects::fluid::Fluid as FluidNode; +//#[cfg(feature = "dim3")] +//use crate::objects::mesh::Mesh; +//use crate::objects::plane::Plane; +//#[cfg(feature = "dim2")] +//use crate::objects::polyline::Polyline; +//#[cfg(feature = "fluids")] +//use crate::objects::FluidRenderingMode; +use crate::objects::capsule::Capsule; +use crate::objects::mesh::Mesh; +use rand::{Rng, SeedableRng}; +use rand_pcg::Pcg32; +use std::collections::HashMap; + +pub trait GraphicsWindow { + fn remove_graphics_node(&mut self, node: &mut GraphicsNode); + fn draw_graphics_line(&mut self, p1: &Point, p2: &Point, color: &Point3); +} + +impl GraphicsWindow for Window { + fn remove_graphics_node(&mut self, node: &mut GraphicsNode) { + #[cfg(feature = "dim2")] + self.remove_planar_node(node); + #[cfg(feature = "dim3")] + self.remove_node(node); + } + + fn draw_graphics_line(&mut self, p1: &Point, p2: &Point, color: &Point3) { + #[cfg(feature = "dim2")] + self.draw_planar_line(p1, p2, color); + #[cfg(feature = "dim3")] + self.draw_line(p1, p2, color); + } +} + +pub struct GraphicsManager { + rand: Pcg32, + b2sn: HashMap>, + #[cfg(feature = "fluids")] + f2sn: HashMap, + #[cfg(feature = "fluids")] + boundary2sn: HashMap, + b2color: HashMap>, + b2wireframe: HashMap, + #[cfg(feature = "fluids")] + f2color: HashMap>, + ground_color: Point3, + camera: Camera, + ground_handle: Option, + #[cfg(feature = "fluids")] + fluid_rendering_mode: FluidRenderingMode, + #[cfg(feature = "fluids")] + render_boundary_particles: bool, +} + +impl GraphicsManager { + pub fn new() -> GraphicsManager { + let mut camera; + + #[cfg(feature = "dim3")] + { + camera = Camera::new(Point3::new(10.0, 10.0, 10.0), Point3::new(0.0, 0.0, 0.0)); + camera.set_rotate_modifiers(Some(kiss3d::event::Modifiers::Control)); + } + + #[cfg(feature = "dim2")] + { + camera = Camera::new(); + camera.set_zoom(50.0); + } + + GraphicsManager { + camera, + rand: Pcg32::seed_from_u64(0), + b2sn: HashMap::new(), + #[cfg(feature = "fluids")] + f2sn: HashMap::new(), + #[cfg(feature = "fluids")] + boundary2sn: HashMap::new(), + b2color: HashMap::new(), + #[cfg(feature = "fluids")] + f2color: HashMap::new(), + ground_color: Point3::new(0.5, 0.5, 0.5), + b2wireframe: HashMap::new(), + ground_handle: None, + #[cfg(feature = "fluids")] + fluid_rendering_mode: FluidRenderingMode::StaticColor, + #[cfg(feature = "fluids")] + render_boundary_particles: false, + } + } + + pub fn set_ground_handle(&mut self, handle: Option) { + self.ground_handle = handle + } + + #[cfg(feature = "fluids")] + pub fn set_fluid_rendering_mode(&mut self, mode: FluidRenderingMode) { + self.fluid_rendering_mode = mode; + } + + #[cfg(feature = "fluids")] + pub fn enable_boundary_particles_rendering(&mut self, enabled: bool) { + self.render_boundary_particles = enabled; + + for sn in self.boundary2sn.values_mut() { + sn.scene_node_mut().set_visible(enabled); + } + } + + pub fn clear(&mut self, window: &mut Window) { + for sns in self.b2sn.values_mut() { + for sn in sns.iter_mut() { + if let Some(node) = sn.scene_node_mut() { + window.remove_graphics_node(node); + } + } + } + + #[cfg(feature = "fluids")] + for sn in self.f2sn.values_mut().chain(self.boundary2sn.values_mut()) { + let node = sn.scene_node_mut(); + window.remove_graphics_node(node); + } + + self.b2sn.clear(); + #[cfg(feature = "fluids")] + self.f2sn.clear(); + #[cfg(feature = "fluids")] + self.boundary2sn.clear(); + self.b2color.clear(); + self.b2wireframe.clear(); + self.rand = Pcg32::seed_from_u64(0); + } + + pub fn remove_body_nodes(&mut self, window: &mut Window, body: RigidBodyHandle) { + if let Some(sns) = self.b2sn.get_mut(&body) { + for sn in sns.iter_mut() { + if let Some(node) = sn.scene_node_mut() { + window.remove_graphics_node(node); + } + } + } + + self.b2sn.remove(&body); + } + + #[cfg(feature = "fluids")] + pub fn set_fluid_color(&mut self, f: FluidHandle, color: Point3) { + self.f2color.insert(f, color); + + if let Some(n) = self.f2sn.get_mut(&f) { + n.set_color(color) + } + } + + pub fn set_body_color(&mut self, b: RigidBodyHandle, color: Point3) { + self.b2color.insert(b, color); + + if let Some(ns) = self.b2sn.get_mut(&b) { + for n in ns.iter_mut() { + n.set_color(color) + } + } + } + + pub fn set_body_wireframe(&mut self, b: RigidBodyHandle, enabled: bool) { + self.b2wireframe.insert(b, enabled); + + if let Some(ns) = self.b2sn.get_mut(&b) { + for n in ns.iter_mut().filter_map(|n| n.scene_node_mut()) { + if enabled { + n.set_surface_rendering_activation(true); + n.set_lines_width(1.0); + } else { + n.set_surface_rendering_activation(false); + n.set_lines_width(1.0); + } + } + } + } + + pub fn toggle_wireframe_mode(&mut self, colliders: &ColliderSet, enabled: bool) { + for n in self.b2sn.values_mut().flat_map(|val| val.iter_mut()) { + let force_wireframe = if let Some(collider) = colliders.get(n.collider()) { + collider.is_sensor() + || self + .b2wireframe + .get(&collider.parent()) + .cloned() + .unwrap_or(false) + } else { + false + }; + + if let Some(node) = n.scene_node_mut() { + if force_wireframe || enabled { + node.set_lines_width(1.0); + node.set_surface_rendering_activation(false); + } else { + node.set_lines_width(0.0); + node.set_surface_rendering_activation(true); + } + } + } + } + + fn gen_color(rng: &mut Pcg32) -> Point3 { + let mut color: Point3 = rng.gen(); + color *= 1.5; + color.x = color.x.min(1.0); + color.y = color.y.min(1.0); + color.z = color.z.min(1.0); + color + } + + fn alloc_color(&mut self, handle: RigidBodyHandle, is_static: bool) -> Point3 { + let mut color = self.ground_color; + + if !is_static { + match self.b2color.get(&handle).cloned() { + Some(c) => color = c, + None => color = Self::gen_color(&mut self.rand), + } + } + + self.set_body_color(handle, color); + + color + } + + #[cfg(feature = "fluids")] + pub fn add_fluid( + &mut self, + window: &mut Window, + handle: FluidHandle, + fluid: &Fluid, + particle_radius: f32, + ) { + let rand = &mut self.rand; + let color = *self + .f2color + .entry(handle) + .or_insert_with(|| Self::gen_color(rand)); + + self.add_fluid_with_color(window, handle, fluid, particle_radius, color); + } + + #[cfg(feature = "fluids")] + pub fn add_boundary( + &mut self, + window: &mut Window, + handle: BoundaryHandle, + boundary: &Boundary, + particle_radius: f32, + ) { + let color = self.ground_color; + let node = FluidNode::new(particle_radius, &boundary.positions, color, window); + self.boundary2sn.insert(handle, node); + } + + #[cfg(feature = "fluids")] + pub fn add_fluid_with_color( + &mut self, + window: &mut Window, + handle: FluidHandle, + fluid: &Fluid, + particle_radius: f32, + color: Point3, + ) { + let node = FluidNode::new(particle_radius, &fluid.positions, color, window); + self.f2sn.insert(handle, node); + } + + pub fn add( + &mut self, + window: &mut Window, + handle: RigidBodyHandle, + bodies: &RigidBodySet, + colliders: &ColliderSet, + ) { + let body = bodies.get(handle).unwrap(); + + let color = self + .b2color + .get(&handle) + .cloned() + .unwrap_or_else(|| self.alloc_color(handle, !body.is_dynamic())); + + self.add_with_color(window, handle, bodies, colliders, color) + } + + pub fn add_with_color( + &mut self, + window: &mut Window, + handle: RigidBodyHandle, + bodies: &RigidBodySet, + colliders: &ColliderSet, + color: Point3, + ) { + // let body = bodies.get(handle).unwrap(); + let mut new_nodes = Vec::new(); + + for collider_handle in bodies[handle].colliders() { + let collider = &colliders[*collider_handle]; + self.add_collider(window, *collider_handle, collider, color, &mut new_nodes); + } + + new_nodes.iter_mut().for_each(|n| n.update(colliders)); + + for node in new_nodes.iter_mut().filter_map(|n| n.scene_node_mut()) { + if self.b2wireframe.get(&handle).cloned() == Some(true) { + node.set_lines_width(1.0); + node.set_surface_rendering_activation(false); + } else { + node.set_lines_width(0.0); + node.set_surface_rendering_activation(true); + } + } + + let nodes = self.b2sn.entry(handle).or_insert_with(Vec::new); + nodes.append(&mut new_nodes); + } + + fn add_collider( + &mut self, + window: &mut Window, + handle: ColliderHandle, + collider: &Collider, + color: Point3, + out: &mut Vec, + ) { + match collider.shape() { + Shape::Ball(ball) => { + out.push(Node::Ball(Ball::new(handle, ball.radius, color, window))) + } + Shape::Polygon(poly) => out.push(Node::Convex(Convex::new( + handle, + poly.vertices().to_vec(), + color, + window, + ))), + Shape::Cuboid(cuboid) => out.push(Node::Box(BoxNode::new( + handle, + cuboid.half_extents, + color, + window, + ))), + Shape::Capsule(capsule) => { + out.push(Node::Capsule(Capsule::new(handle, capsule, color, window))) + } + Shape::Triangle(triangle) => out.push(Node::Mesh(Mesh::new( + handle, + vec![triangle.a, triangle.b, triangle.c], + vec![Point3::new(0, 1, 2)], + color, + window, + ))), + Shape::Trimesh(trimesh) => out.push(Node::Mesh(Mesh::new( + handle, + trimesh.vertices().to_vec(), + trimesh + .indices() + .iter() + .map(|idx| na::convert(*idx)) + .collect(), + color, + window, + ))), + Shape::HeightField(heightfield) => out.push(Node::HeightField(HeightField::new( + handle, + heightfield, + color, + window, + ))), + } + } + + /* + fn add_plane( + &mut self, + window: &mut Window, + object: DefaultColliderHandle, + colliders: &DefaultColliderSet, + shape: &shape::Plane, + color: Point3, + out: &mut Vec, + ) { + let pos = colliders.get(object).unwrap().position(); + let position = Point::from(pos.translation.vector); + let normal = pos * shape.normal(); + + out.push(Node::Plane(Plane::new( + object, colliders, &position, &normal, color, window, + ))) + } + + #[cfg(feature = "dim2")] + fn add_polyline( + &mut self, + window: &mut Window, + object: DefaultColliderHandle, + colliders: &DefaultColliderSet, + delta: Isometry, + shape: &shape::Polyline, + color: Point3, + out: &mut Vec, + ) { + let vertices = shape.points().to_vec(); + let indices = shape.edges().iter().map(|e| e.indices).collect(); + + out.push(Node::Polyline(Polyline::new( + object, colliders, delta, vertices, indices, color, window, + ))) + } + + #[cfg(feature = "dim3")] + fn add_mesh( + &mut self, + window: &mut Window, + object: DefaultColliderHandle, + colliders: &DefaultColliderSet, + delta: Isometry, + shape: &TriMesh, + color: Point3, + out: &mut Vec, + ) { + let points = shape.points(); + let faces = shape.faces(); + + let is = faces + .iter() + .map(|f| Point3::new(f.indices.x as u32, f.indices.y as u32, f.indices.z as u32)) + .collect(); + + out.push(Node::Mesh(Mesh::new( + object, + colliders, + delta, + points.to_vec(), + is, + color, + window, + ))) + } + + fn add_heightfield( + &mut self, + window: &mut Window, + object: DefaultColliderHandle, + colliders: &DefaultColliderSet, + delta: Isometry, + heightfield: &shape::HeightField, + color: Point3, + out: &mut Vec, + ) { + out.push(Node::HeightField(HeightField::new( + object, + colliders, + delta, + heightfield, + color, + window, + ))) + } + + fn add_capsule( + &mut self, + window: &mut Window, + object: DefaultColliderHandle, + colliders: &DefaultColliderSet, + delta: Isometry, + shape: &shape::Capsule, + color: Point3, + out: &mut Vec, + ) { + let margin = colliders.get(object).unwrap().margin(); + out.push(Node::Capsule(Capsule::new( + object, + colliders, + delta, + shape.radius() + margin, + shape.height(), + color, + window, + ))) + } + + fn add_ball( + &mut self, + window: &mut Window, + object: DefaultColliderHandle, + colliders: &DefaultColliderSet, + delta: Isometry, + shape: &shape::Ball, + color: Point3, + out: &mut Vec, + ) { + let margin = colliders.get(object).unwrap().margin(); + out.push(Node::Ball(Ball::new( + object, + colliders, + delta, + shape.radius() + margin, + color, + window, + ))) + } + + fn add_box( + &mut self, + window: &mut Window, + object: DefaultColliderHandle, + colliders: &DefaultColliderSet, + delta: Isometry, + shape: &Cuboid, + color: Point3, + out: &mut Vec, + ) { + let margin = colliders.get(object).unwrap().margin(); + + out.push(Node::Box(Box::new( + object, + colliders, + delta, + shape.half_extents() + Vector::repeat(margin), + color, + window, + ))) + } + + #[cfg(feature = "dim2")] + fn add_convex( + &mut self, + window: &mut Window, + object: DefaultColliderHandle, + colliders: &DefaultColliderSet, + delta: Isometry, + shape: &ConvexPolygon, + color: Point3, + out: &mut Vec, + ) { + let points = shape.points(); + + out.push(Node::Convex(Convex::new( + object, + colliders, + delta, + points.to_vec(), + color, + window, + ))) + } + + #[cfg(feature = "dim3")] + fn add_convex( + &mut self, + window: &mut Window, + object: DefaultColliderHandle, + colliders: &DefaultColliderSet, + delta: Isometry, + shape: &ConvexHull, + color: Point3, + out: &mut Vec, + ) { + let mut chull = transformation::convex_hull(shape.points()); + chull.replicate_vertices(); + chull.recompute_normals(); + + out.push(Node::Convex(Convex::new( + object, colliders, delta, &chull, color, window, + ))) + } + */ + + #[cfg(feature = "fluids")] + pub fn draw_fluids(&mut self, liquid_world: &LiquidWorld) { + for (i, fluid) in liquid_world.fluids().iter() { + if let Some(node) = self.f2sn.get_mut(&i) { + node.update_with_fluid(fluid, self.fluid_rendering_mode) + } + } + + if self.render_boundary_particles { + for (i, boundary) in liquid_world.boundaries().iter() { + if let Some(node) = self.boundary2sn.get_mut(&i) { + node.update_with_boundary(boundary) + } + } + } + } + + pub fn draw(&mut self, colliders: &ColliderSet, window: &mut Window) { + // use kiss3d::camera::Camera; + // println!( + // "camera eye {:?}, at: {:?}", + // self.camera.eye(), + // self.camera.at() + // ); + for (_, ns) in self.b2sn.iter_mut() { + for n in ns.iter_mut() { + n.update(colliders); + n.draw(window); + } + } + } + + // pub fn draw_positions(&mut self, window: &mut Window, rbs: &RigidBodies) { + // for (_, ns) in self.b2sn.iter_mut() { + // for n in ns.iter_mut() { + // let object = n.object(); + // let rb = rbs.get(object).expect("Rigid body not found."); + + // // if let WorldObjectBorrowed::RigidBody(rb) = object { + // let t = rb.position(); + // let center = rb.center_of_mass(); + + // let rotmat = t.rotation.to_rotation_matrix().unwrap(); + // let x = rotmat.column(0) * 0.25f32; + // let y = rotmat.column(1) * 0.25f32; + // let z = rotmat.column(2) * 0.25f32; + + // window.draw_line(center, &(*center + x), &Point3::new(1.0, 0.0, 0.0)); + // window.draw_line(center, &(*center + y), &Point3::new(0.0, 1.0, 0.0)); + // window.draw_line(center, &(*center + z), &Point3::new(0.0, 0.0, 1.0)); + // // } + // } + // } + // } + + pub fn camera(&self) -> &Camera { + &self.camera + } + + pub fn camera_mut(&mut self) -> &mut Camera { + &mut self.camera + } + + #[cfg(feature = "dim3")] + pub fn look_at(&mut self, eye: Point, at: Point) { + self.camera.look_at(eye, at); + } + + #[cfg(feature = "dim2")] + pub fn look_at(&mut self, at: Point, zoom: f32) { + self.camera.look_at(at, zoom); + } + + pub fn body_nodes(&self, handle: RigidBodyHandle) -> Option<&Vec> { + self.b2sn.get(&handle) + } + + pub fn body_nodes_mut(&mut self, handle: RigidBodyHandle) -> Option<&mut Vec> { + self.b2sn.get_mut(&handle) + } + + pub fn nodes(&self) -> impl Iterator { + self.b2sn.values().flat_map(|val| val.iter()) + } + + pub fn nodes_mut(&mut self) -> impl Iterator { + self.b2sn.values_mut().flat_map(|val| val.iter_mut()) + } + + #[cfg(feature = "dim3")] + pub fn set_up_axis(&mut self, up_axis: na::Vector3) { + self.camera.set_up_axis(up_axis); + } +} + +impl Default for GraphicsManager { + fn default() -> Self { + Self::new() + } +} diff --git a/src_testbed/lib.rs b/src_testbed/lib.rs new file mode 100644 index 000000000..e077bb1bf --- /dev/null +++ b/src_testbed/lib.rs @@ -0,0 +1,52 @@ +#[macro_use] +extern crate kiss3d; +extern crate nalgebra as na; +#[cfg(feature = "dim2")] +extern crate ncollide2d as ncollide; +#[cfg(feature = "dim3")] +extern crate ncollide3d as ncollide; +#[cfg(all(feature = "dim2", feature = "other-backends"))] +extern crate nphysics2d as nphysics; +#[cfg(all(feature = "dim3", feature = "other-backends"))] +extern crate nphysics3d as nphysics; +#[cfg(feature = "dim2")] +extern crate rapier2d as rapier; +#[cfg(feature = "dim3")] +extern crate rapier3d as rapier; + +#[macro_use] +extern crate bitflags; + +#[cfg(feature = "log")] +#[macro_use] +extern crate log; + +pub use crate::engine::GraphicsManager; +pub use crate::testbed::Testbed; + +#[cfg(all(feature = "dim2", feature = "other-backends"))] +mod box2d_backend; +mod engine; +#[cfg(feature = "other-backends")] +mod nphysics_backend; +pub mod objects; +#[cfg(all(feature = "dim3", feature = "other-backends"))] +mod physx_backend; +mod testbed; +mod ui; + +#[cfg(feature = "dim2")] +pub mod math { + pub type Isometry = na::Isometry2; + pub type Vector = na::Vector2; + pub type Point = na::Point2; + pub type Translation = na::Translation2; +} + +#[cfg(feature = "dim3")] +pub mod math { + pub type Isometry = na::Isometry3; + pub type Vector = na::Vector3; + pub type Point = na::Point3; + pub type Translation = na::Translation3; +} diff --git a/src_testbed/nphysics_backend.rs b/src_testbed/nphysics_backend.rs new file mode 100644 index 000000000..43f3bf9d4 --- /dev/null +++ b/src_testbed/nphysics_backend.rs @@ -0,0 +1,212 @@ +use ncollide::shape::{Ball, Capsule, Cuboid, ShapeHandle}; +use nphysics::force_generator::DefaultForceGeneratorSet; +use nphysics::joint::{ + DefaultJointConstraintSet, FixedConstraint, PrismaticConstraint, RevoluteConstraint, +}; +use nphysics::object::{ + BodyPartHandle, ColliderDesc, DefaultBodyHandle, DefaultBodySet, DefaultColliderSet, + RigidBodyDesc, +}; +use nphysics::world::{DefaultGeometricalWorld, DefaultMechanicalWorld}; +use rapier::counters::Counters; +use rapier::dynamics::{ + IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet, +}; +use rapier::geometry::{Collider, ColliderSet, Shape}; +use rapier::math::{Isometry, Vector}; +use std::collections::HashMap; +#[cfg(feature = "dim3")] +use {ncollide::shape::TriMesh, nphysics::joint::BallConstraint}; + +pub struct NPhysicsWorld { + rapier2nphysics: HashMap, + mechanical_world: DefaultMechanicalWorld, + geometrical_world: DefaultGeometricalWorld, + bodies: DefaultBodySet, + colliders: DefaultColliderSet, + joints: DefaultJointConstraintSet, + force_generators: DefaultForceGeneratorSet, +} + +impl NPhysicsWorld { + pub fn from_rapier( + gravity: Vector, + bodies: &RigidBodySet, + colliders: &ColliderSet, + joints: &JointSet, + ) -> Self { + let mut rapier2nphysics = HashMap::new(); + + let mechanical_world = DefaultMechanicalWorld::new(gravity); + let geometrical_world = DefaultGeometricalWorld::new(); + let mut nphysics_bodies = DefaultBodySet::new(); + let mut nphysics_colliders = DefaultColliderSet::new(); + let mut nphysics_joints = DefaultJointConstraintSet::new(); + let force_generators = DefaultForceGeneratorSet::new(); + + for (rapier_handle, rb) in bodies.iter() { + // let material = physics.create_material(rb.collider.friction, rb.collider.friction, 0.0); + let nphysics_rb = RigidBodyDesc::new().position(rb.position).build(); + let nphysics_rb_handle = nphysics_bodies.insert(nphysics_rb); + + rapier2nphysics.insert(rapier_handle, nphysics_rb_handle); + } + + for (_, collider) in colliders.iter() { + let parent = &bodies[collider.parent()]; + let nphysics_rb_handle = rapier2nphysics[&collider.parent()]; + if let Some(collider) = + nphysics_collider_from_rapier_collider(&collider, parent.is_dynamic()) + { + let nphysics_collider = collider.build(BodyPartHandle(nphysics_rb_handle, 0)); + nphysics_colliders.insert(nphysics_collider); + } else { + eprintln!("Creating shape unknown to the nphysics backend.") + } + } + + for joint in joints.iter() { + let b1 = BodyPartHandle(rapier2nphysics[&joint.body1], 0); + let b2 = BodyPartHandle(rapier2nphysics[&joint.body2], 0); + + match &joint.params { + JointParams::FixedJoint(params) => { + let c = FixedConstraint::new( + b1, + b2, + params.local_anchor1.translation.vector.into(), + params.local_anchor1.rotation, + params.local_anchor2.translation.vector.into(), + params.local_anchor2.rotation, + ); + nphysics_joints.insert(c); + } + #[cfg(feature = "dim3")] + JointParams::BallJoint(params) => { + let c = BallConstraint::new(b1, b2, params.local_anchor1, params.local_anchor2); + nphysics_joints.insert(c); + } + #[cfg(feature = "dim2")] + JointParams::BallJoint(params) => { + let c = + RevoluteConstraint::new(b1, b2, params.local_anchor1, params.local_anchor2); + nphysics_joints.insert(c); + } + #[cfg(feature = "dim3")] + JointParams::RevoluteJoint(params) => { + let c = RevoluteConstraint::new( + b1, + b2, + params.local_anchor1, + params.local_axis1, + params.local_anchor2, + params.local_axis2, + ); + nphysics_joints.insert(c); + } + JointParams::PrismaticJoint(params) => { + let mut c = PrismaticConstraint::new( + b1, + b2, + params.local_anchor1, + params.local_axis1(), + params.local_anchor2, + ); + + if params.limits_enabled { + c.enable_min_offset(params.limits[0]); + c.enable_max_offset(params.limits[1]); + } + + nphysics_joints.insert(c); + } + } + } + + Self { + rapier2nphysics, + mechanical_world, + geometrical_world, + bodies: nphysics_bodies, + colliders: nphysics_colliders, + joints: nphysics_joints, + force_generators, + } + } + + pub fn step(&mut self, counters: &mut Counters, params: &IntegrationParameters) { + self.mechanical_world + .integration_parameters + .max_position_iterations = params.max_position_iterations; + self.mechanical_world + .integration_parameters + .max_velocity_iterations = params.max_velocity_iterations; + self.mechanical_world + .integration_parameters + .set_dt(params.dt()); + + counters.step_started(); + self.mechanical_world.step( + &mut self.geometrical_world, + &mut self.bodies, + &mut self.colliders, + &mut self.joints, + &mut self.force_generators, + ); + counters.step_completed(); + } + + pub fn sync(&self, bodies: &mut RigidBodySet, colliders: &mut ColliderSet) { + for (rapier_handle, nphysics_handle) in self.rapier2nphysics.iter() { + let mut rb = bodies.get_mut(*rapier_handle).unwrap(); + let ra = self.bodies.rigid_body(*nphysics_handle).unwrap(); + let pos = *ra.position(); + rb.set_position(pos); + + for coll_handle in rb.colliders() { + let collider = &mut colliders[*coll_handle]; + collider.set_position_debug(pos * collider.delta()); + } + } + } +} + +fn nphysics_collider_from_rapier_collider( + collider: &Collider, + is_dynamic: bool, +) -> Option> { + let margin = ColliderDesc::::default_margin(); + let mut pos = Isometry::identity(); + + let shape = match collider.shape() { + Shape::Cuboid(cuboid) => { + ShapeHandle::new(Cuboid::new(cuboid.half_extents.map(|e| e - margin))) + } + Shape::Ball(ball) => ShapeHandle::new(Ball::new(ball.radius - margin)), + Shape::Capsule(capsule) => { + pos = capsule.transform_wrt_y(); + ShapeHandle::new(Capsule::new(capsule.half_height(), capsule.radius)) + } + Shape::HeightField(heightfield) => ShapeHandle::new(heightfield.clone()), + #[cfg(feature = "dim3")] + Shape::Trimesh(trimesh) => ShapeHandle::new(TriMesh::new( + trimesh.vertices().to_vec(), + trimesh + .indices() + .iter() + .map(|idx| na::convert(*idx)) + .collect(), + None, + )), + _ => return None, + }; + + let density = if is_dynamic { collider.density() } else { 0.0 }; + + Some( + ColliderDesc::new(shape) + .position(pos) + .density(density) + .sensor(collider.is_sensor()), + ) +} diff --git a/src_testbed/objects/ball.rs b/src_testbed/objects/ball.rs new file mode 100644 index 000000000..f72c2847b --- /dev/null +++ b/src_testbed/objects/ball.rs @@ -0,0 +1,73 @@ +use crate::objects::node::{self, GraphicsNode}; +use kiss3d::window::Window; +use na::Point3; +use rapier::geometry::{ColliderHandle, ColliderSet}; +use rapier::math::Isometry; + +pub struct Ball { + color: Point3, + base_color: Point3, + gfx: GraphicsNode, + collider: ColliderHandle, +} + +impl Ball { + pub fn new( + collider: ColliderHandle, + radius: f32, + color: Point3, + window: &mut Window, + ) -> Ball { + #[cfg(feature = "dim2")] + let node = window.add_circle(radius); + #[cfg(feature = "dim3")] + let node = window.add_sphere(radius); + + let mut res = Ball { + color, + base_color: color, + gfx: node, + collider, + }; + + // res.gfx.set_texture_from_file(&Path::new("media/kitten.png"), "kitten"); + res.gfx.set_color(color.x, color.y, color.z); + res + } + + pub fn select(&mut self) { + self.color = Point3::new(1.0, 0.0, 0.0); + } + + pub fn unselect(&mut self) { + self.color = self.base_color; + } + + pub fn set_color(&mut self, color: Point3) { + self.gfx.set_color(color.x, color.y, color.z); + self.color = color; + self.base_color = color; + } + + pub fn update(&mut self, colliders: &ColliderSet) { + node::update_scene_node( + &mut self.gfx, + colliders, + self.collider, + &self.color, + &Isometry::identity(), + ); + } + + pub fn scene_node(&self) -> &GraphicsNode { + &self.gfx + } + + pub fn scene_node_mut(&mut self) -> &mut GraphicsNode { + &mut self.gfx + } + + pub fn object(&self) -> ColliderHandle { + self.collider + } +} diff --git a/src_testbed/objects/box_node.rs b/src_testbed/objects/box_node.rs new file mode 100644 index 000000000..493ffba49 --- /dev/null +++ b/src_testbed/objects/box_node.rs @@ -0,0 +1,73 @@ +use crate::objects::node::{self, GraphicsNode}; +use kiss3d::window; +use na::Point3; +use rapier::geometry::{ColliderHandle, ColliderSet}; +use rapier::math::{Isometry, Vector}; + +pub struct Box { + color: Point3, + base_color: Point3, + gfx: GraphicsNode, + collider: ColliderHandle, +} + +impl Box { + pub fn new( + collider: ColliderHandle, + half_extents: Vector, + color: Point3, + window: &mut window::Window, + ) -> Box { + let extents = half_extents * 2.0; + #[cfg(feature = "dim2")] + let node = window.add_rectangle(extents.x, extents.y); + #[cfg(feature = "dim3")] + let node = window.add_cube(extents.x, extents.y, extents.z); + + let mut res = Box { + color, + base_color: color, + gfx: node, + collider, + }; + + res.gfx.set_color(color.x, color.y, color.z); + res + } + + pub fn select(&mut self) { + self.color = Point3::new(1.0, 0.0, 0.0); + } + + pub fn unselect(&mut self) { + self.color = self.base_color; + } + + pub fn set_color(&mut self, color: Point3) { + self.gfx.set_color(color.x, color.y, color.z); + self.color = color; + self.base_color = color; + } + + pub fn update(&mut self, colliders: &ColliderSet) { + node::update_scene_node( + &mut self.gfx, + colliders, + self.collider, + &self.color, + &Isometry::identity(), + ); + } + + pub fn scene_node(&self) -> &GraphicsNode { + &self.gfx + } + + pub fn scene_node_mut(&mut self) -> &mut GraphicsNode { + &mut self.gfx + } + + pub fn object(&self) -> ColliderHandle { + self.collider + } +} diff --git a/src_testbed/objects/capsule.rs b/src_testbed/objects/capsule.rs new file mode 100644 index 000000000..23160be8e --- /dev/null +++ b/src_testbed/objects/capsule.rs @@ -0,0 +1,74 @@ +use crate::objects::node::{self, GraphicsNode}; +use kiss3d::window; +use na::Point3; +use rapier::geometry::{self, ColliderHandle, ColliderSet}; +use rapier::math::Isometry; + +pub struct Capsule { + color: Point3, + base_color: Point3, + gfx: GraphicsNode, + collider: ColliderHandle, +} + +impl Capsule { + pub fn new( + collider: ColliderHandle, + capsule: &geometry::Capsule, + color: Point3, + window: &mut window::Window, + ) -> Capsule { + let r = capsule.radius; + let h = capsule.half_height() * 2.0; + #[cfg(feature = "dim2")] + let node = window.add_planar_capsule(r, h); + #[cfg(feature = "dim3")] + let node = window.add_capsule(r, h); + + let mut res = Capsule { + color, + base_color: color, + gfx: node, + collider, + }; + + res.gfx.set_color(color.x, color.y, color.z); + res + } + + pub fn select(&mut self) { + self.color = Point3::new(1.0, 0.0, 0.0); + } + + pub fn unselect(&mut self) { + self.color = self.base_color; + } + + pub fn update(&mut self, colliders: &ColliderSet) { + node::update_scene_node( + &mut self.gfx, + colliders, + self.collider, + &self.color, + &Isometry::identity(), + ); + } + + pub fn set_color(&mut self, color: Point3) { + self.gfx.set_color(color.x, color.y, color.z); + self.color = color; + self.base_color = color; + } + + pub fn scene_node(&self) -> &GraphicsNode { + &self.gfx + } + + pub fn scene_node_mut(&mut self) -> &mut GraphicsNode { + &mut self.gfx + } + + pub fn object(&self) -> ColliderHandle { + self.collider + } +} diff --git a/src_testbed/objects/convex.rs b/src_testbed/objects/convex.rs new file mode 100644 index 000000000..0347144bd --- /dev/null +++ b/src_testbed/objects/convex.rs @@ -0,0 +1,77 @@ +#![allow(warnings)] // TODO: remove this. + +#[cfg(feature = "dim2")] +use crate::math::Vector; +use crate::math::{Isometry, Point}; +use crate::objects::node::{self, GraphicsNode}; +use kiss3d::window::Window; +use na::Point3; +use rapier::geometry::{ColliderHandle, ColliderSet}; + +pub struct Convex { + color: Point3, + base_color: Point3, + gfx: GraphicsNode, + body: ColliderHandle, +} + +impl Convex { + pub fn new( + body: ColliderHandle, + vertices: Vec>, + color: Point3, + window: &mut Window, + ) -> Convex { + #[cfg(feature = "dim2")] + let node = window.add_convex_polygon(vertices, Vector::from_element(1.0)); + #[cfg(feature = "dim3")] + let node = unimplemented!(); + + let mut res = Convex { + color, + base_color: color, + gfx: node, + body, + }; + + // res.gfx.set_texture_from_file(&Path::new("media/kitten.png"), "kitten"); + res.gfx.set_color(color.x, color.y, color.z); + res + } + + pub fn select(&mut self) { + self.color = Point3::new(1.0, 0.0, 0.0); + } + + pub fn unselect(&mut self) { + self.color = self.base_color; + } + + pub fn set_color(&mut self, color: Point3) { + self.gfx.set_color(color.x, color.y, color.z); + self.color = color; + self.base_color = color; + } + + pub fn update(&mut self, colliders: &ColliderSet) { + node::update_scene_node( + &mut self.gfx, + colliders, + self.body, + &self.color, + &Isometry::identity(), + ); + } + + pub fn scene_node(&self) -> &GraphicsNode { + &self.gfx + } + + pub fn scene_node_mut(&mut self) -> &mut GraphicsNode { + &mut self.gfx + } + + pub fn object(&self) -> ColliderHandle { + self.body + } +} diff --git a/src_testbed/objects/heightfield.rs b/src_testbed/objects/heightfield.rs new file mode 100644 index 000000000..081559290 --- /dev/null +++ b/src_testbed/objects/heightfield.rs @@ -0,0 +1,120 @@ +#[cfg(feature = "dim3")] +use crate::objects::node::{self, GraphicsNode}; +use kiss3d::window::Window; +use na::{self, Point3}; +use ncollide::shape; +#[cfg(feature = "dim3")] +use ncollide::transformation::ToTriMesh; +use rapier::geometry::{ColliderHandle, ColliderSet}; +#[cfg(feature = "dim2")] +use rapier::math::Point; +#[cfg(feature = "dim3")] +use rapier::math::Vector; + +pub struct HeightField { + color: Point3, + base_color: Point3, + #[cfg(feature = "dim2")] + vertices: Vec>, + #[cfg(feature = "dim3")] + gfx: GraphicsNode, + collider: ColliderHandle, +} + +impl HeightField { + #[cfg(feature = "dim2")] + pub fn new( + collider: ColliderHandle, + heightfield: &shape::HeightField, + color: Point3, + _: &mut Window, + ) -> HeightField { + let mut vertices = Vec::new(); + + for seg in heightfield.segments() { + vertices.push(seg.a); + vertices.push(seg.b); + } + + HeightField { + color, + base_color: color, + vertices, + collider, + } + } + + #[cfg(feature = "dim3")] + pub fn new( + collider: ColliderHandle, + heightfield: &shape::HeightField, + color: Point3, + window: &mut Window, + ) -> HeightField { + let mesh = heightfield.to_trimesh(()); + + let mut res = HeightField { + color, + base_color: color, + gfx: window.add_trimesh(mesh, Vector::repeat(1.0)), + collider: collider, + }; + + res.gfx.enable_backface_culling(false); + res.gfx.set_color(color.x, color.y, color.z); + + res + } + + pub fn select(&mut self) { + self.color = Point3::new(1.0, 0.0, 0.0); + } + + pub fn unselect(&mut self) { + self.color = self.base_color; + } + + pub fn set_color(&mut self, color: Point3) { + #[cfg(feature = "dim3")] + { + self.gfx.set_color(color.x, color.y, color.z); + } + self.color = color; + self.base_color = color; + } + + #[cfg(feature = "dim3")] + pub fn update(&mut self, colliders: &ColliderSet) { + node::update_scene_node( + &mut self.gfx, + colliders, + self.collider, + &self.color, + &na::Isometry::identity(), + ); + } + + #[cfg(feature = "dim2")] + pub fn update(&mut self, _colliders: &ColliderSet) {} + + #[cfg(feature = "dim3")] + pub fn scene_node(&self) -> &GraphicsNode { + &self.gfx + } + + #[cfg(feature = "dim3")] + pub fn scene_node_mut(&mut self) -> &mut GraphicsNode { + &mut self.gfx + } + + pub fn object(&self) -> ColliderHandle { + self.collider + } + + #[cfg(feature = "dim2")] + pub fn draw(&mut self, window: &mut Window) { + for vtx in self.vertices.chunks(2) { + window.draw_planar_line(&vtx[0], &vtx[1], &self.color) + } + } +} diff --git a/src_testbed/objects/mesh.rs b/src_testbed/objects/mesh.rs new file mode 100644 index 000000000..5187a8b5f --- /dev/null +++ b/src_testbed/objects/mesh.rs @@ -0,0 +1,108 @@ +use crate::objects::node::{self, GraphicsNode}; +use kiss3d::window; +use na::Point3; +use rapier::geometry::{ColliderHandle, ColliderSet}; +use rapier::math::{Isometry, Point}; +use std::cell::RefCell; +use std::rc::Rc; + +pub struct Mesh { + color: Point3, + base_color: Point3, + gfx: GraphicsNode, + collider: ColliderHandle, +} + +impl Mesh { + pub fn new( + collider: ColliderHandle, + vertices: Vec>, + indices: Vec>, + color: Point3, + window: &mut window::Window, + ) -> Mesh { + let vs = vertices; + let is = indices.into_iter().map(na::convert).collect(); + + let mesh; + let gfx; + + #[cfg(feature = "dim2")] + { + mesh = kiss3d::resource::PlanarMesh::new(vs, is, None, false); + gfx = window.add_planar_mesh( + Rc::new(RefCell::new(mesh)), + crate::math::Vector::from_element(1.0), + ); + } + + #[cfg(feature = "dim3")] + { + mesh = kiss3d::resource::Mesh::new(vs, is, None, None, false); + gfx = window.add_mesh(Rc::new(RefCell::new(mesh)), na::Vector3::from_element(1.0)); + } + + let mut res = Mesh { + color, + base_color: color, + gfx, + collider, + }; + + res.gfx.enable_backface_culling(false); + res.gfx.set_color(color.x, color.y, color.z); + res + } + + pub fn select(&mut self) { + self.color = Point3::new(1.0, 0.0, 0.0); + } + + pub fn unselect(&mut self) { + self.color = self.base_color; + } + + pub fn set_color(&mut self, color: Point3) { + self.gfx.set_color(color.x, color.y, color.z); + self.color = color; + self.base_color = color; + } + + pub fn update(&mut self, colliders: &ColliderSet) { + node::update_scene_node( + &mut self.gfx, + colliders, + self.collider, + &self.color, + &Isometry::identity(), + ); + + // // Update if some deformation occurred. + // // FIXME: don't update if it did not move. + // if let Some(c) = colliders.get(self.collider) { + // if let ColliderAnchor::OnDeformableBody { .. } = c.anchor() { + // let shape = c.shape().as_shape::>().unwrap(); + // let vtx = shape.points(); + // + // self.gfx.modify_vertices(&mut |vertices| { + // for (v, new_v) in vertices.iter_mut().zip(vtx.iter()) { + // *v = *new_v + // } + // }); + // self.gfx.recompute_normals(); + // } + // } + } + + pub fn scene_node(&self) -> &GraphicsNode { + &self.gfx + } + + pub fn scene_node_mut(&mut self) -> &mut GraphicsNode { + &mut self.gfx + } + + pub fn object(&self) -> ColliderHandle { + self.collider + } +} diff --git a/src_testbed/objects/mod.rs b/src_testbed/objects/mod.rs new file mode 100644 index 000000000..82895b38d --- /dev/null +++ b/src_testbed/objects/mod.rs @@ -0,0 +1,10 @@ +pub mod ball; +pub mod box_node; +pub mod capsule; +pub mod convex; +pub mod heightfield; +pub mod mesh; +pub mod node; +//pub mod plane; +//#[cfg(feature = "dim2")] +//pub mod polyline; diff --git a/src_testbed/objects/node.rs b/src_testbed/objects/node.rs new file mode 100644 index 000000000..93b5eac8d --- /dev/null +++ b/src_testbed/objects/node.rs @@ -0,0 +1,164 @@ +use crate::objects::ball::Ball; +use crate::objects::box_node::Box; +use crate::objects::capsule::Capsule; +use crate::objects::convex::Convex; +use crate::objects::heightfield::HeightField; +use crate::objects::mesh::Mesh; +//use crate::objects::plane::Plane; +//#[cfg(feature = "dim2")] +//use crate::objects::polyline::Polyline; +use kiss3d::window::Window; +use na::Point3; + +use rapier::geometry::{ColliderHandle, ColliderSet}; +use rapier::math::Isometry; + +#[cfg(feature = "dim2")] +pub type GraphicsNode = kiss3d::scene::PlanarSceneNode; +#[cfg(feature = "dim3")] +pub type GraphicsNode = kiss3d::scene::SceneNode; + +pub enum Node { + // Plane(Plane), + Ball(Ball), + Box(Box), + HeightField(HeightField), + Capsule(Capsule), + // #[cfg(feature = "dim2")] + // Polyline(Polyline), + Mesh(Mesh), + Convex(Convex), +} + +impl Node { + pub fn select(&mut self) { + match *self { + // Node::Plane(ref mut n) => n.select(), + Node::Ball(ref mut n) => n.select(), + Node::Box(ref mut n) => n.select(), + Node::Capsule(ref mut n) => n.select(), + Node::HeightField(ref mut n) => n.select(), + // #[cfg(feature = "dim2")] + // Node::Polyline(ref mut n) => n.select(), + Node::Mesh(ref mut n) => n.select(), + Node::Convex(ref mut n) => n.select(), + } + } + + pub fn unselect(&mut self) { + match *self { + // Node::Plane(ref mut n) => n.unselect(), + Node::Ball(ref mut n) => n.unselect(), + Node::Box(ref mut n) => n.unselect(), + Node::Capsule(ref mut n) => n.unselect(), + Node::HeightField(ref mut n) => n.unselect(), + // #[cfg(feature = "dim2")] + // Node::Polyline(ref mut n) => n.unselect(), + Node::Mesh(ref mut n) => n.unselect(), + Node::Convex(ref mut n) => n.unselect(), + } + } + + pub fn update(&mut self, colliders: &ColliderSet) { + match *self { + // Node::Plane(ref mut n) => n.update(colliders), + Node::Ball(ref mut n) => n.update(colliders), + Node::Box(ref mut n) => n.update(colliders), + Node::Capsule(ref mut n) => n.update(colliders), + Node::HeightField(ref mut n) => n.update(colliders), + // #[cfg(feature = "dim2")] + // Node::Polyline(ref mut n) => n.update(colliders), + Node::Mesh(ref mut n) => n.update(colliders), + Node::Convex(ref mut n) => n.update(colliders), + } + } + + #[cfg(feature = "dim2")] + pub fn draw(&mut self, window: &mut Window) { + match *self { + // Node::Polyline(ref mut n) => n.draw(_window), + Node::HeightField(ref mut n) => n.draw(window), + // Node::Plane(ref mut n) => n.draw(_window), + _ => {} + } + } + + #[cfg(feature = "dim3")] + pub fn draw(&mut self, _: &mut Window) {} + + pub fn scene_node(&self) -> Option<&GraphicsNode> { + match *self { + // #[cfg(feature = "dim3")] + // Node::Plane(ref n) => Some(n.scene_node()), + Node::Ball(ref n) => Some(n.scene_node()), + Node::Box(ref n) => Some(n.scene_node()), + Node::Capsule(ref n) => Some(n.scene_node()), + #[cfg(feature = "dim3")] + Node::HeightField(ref n) => Some(n.scene_node()), + Node::Mesh(ref n) => Some(n.scene_node()), + Node::Convex(ref n) => Some(n.scene_node()), + #[cfg(feature = "dim2")] + _ => None, + } + } + + pub fn scene_node_mut(&mut self) -> Option<&mut GraphicsNode> { + match *self { + // #[cfg(feature = "dim3")] + // Node::Plane(ref mut n) => Some(n.scene_node_mut()), + Node::Ball(ref mut n) => Some(n.scene_node_mut()), + Node::Box(ref mut n) => Some(n.scene_node_mut()), + Node::Capsule(ref mut n) => Some(n.scene_node_mut()), + #[cfg(feature = "dim3")] + Node::HeightField(ref mut n) => Some(n.scene_node_mut()), + Node::Mesh(ref mut n) => Some(n.scene_node_mut()), + Node::Convex(ref mut n) => Some(n.scene_node_mut()), + #[cfg(feature = "dim2")] + _ => None, + } + } + + pub fn collider(&self) -> ColliderHandle { + match *self { + // Node::Plane(ref n) => n.object(), + Node::Ball(ref n) => n.object(), + Node::Box(ref n) => n.object(), + Node::Capsule(ref n) => n.object(), + Node::HeightField(ref n) => n.object(), + // #[cfg(feature = "dim2")] + // Node::Polyline(ref n) => n.object(), + Node::Mesh(ref n) => n.object(), + Node::Convex(ref n) => n.object(), + } + } + + pub fn set_color(&mut self, color: Point3) { + match *self { + // Node::Plane(ref mut n) => n.set_color(color), + Node::Ball(ref mut n) => n.set_color(color), + Node::Box(ref mut n) => n.set_color(color), + Node::Capsule(ref mut n) => n.set_color(color), + Node::HeightField(ref mut n) => n.set_color(color), + // #[cfg(feature = "dim2")] + // Node::Polyline(ref mut n) => n.set_color(color), + Node::Mesh(ref mut n) => n.set_color(color), + Node::Convex(ref mut n) => n.set_color(color), + } + } +} + +pub fn update_scene_node( + node: &mut GraphicsNode, + colliders: &ColliderSet, + handle: ColliderHandle, + color: &Point3, + delta: &Isometry, +) { + if let Some(co) = colliders.get(handle) { + node.set_local_transformation(co.position() * delta); + node.set_color(color.x, color.y, color.z); + } else { + node.set_visible(false); + node.unlink(); + } +} diff --git a/src_testbed/objects/plane.rs b/src_testbed/objects/plane.rs new file mode 100644 index 000000000..218e97aa5 --- /dev/null +++ b/src_testbed/objects/plane.rs @@ -0,0 +1,132 @@ +#[cfg(feature = "dim3")] +use crate::objects::node::GraphicsNode; +use kiss3d::window::Window; +use na::Point3; +#[cfg(feature = "dim3")] +use na::Vector3; +#[cfg(feature = "dim2")] +use nphysics::math::{Point, Vector}; +use nphysics::object::{DefaultColliderHandle, DefaultColliderSet}; +#[cfg(feature = "dim3")] +use num::Zero; + +#[cfg(feature = "dim3")] +pub struct Plane { + gfx: GraphicsNode, + collider: DefaultColliderHandle, +} + +#[cfg(feature = "dim2")] +pub struct Plane { + color: Point3, + base_color: Point3, + position: Point, + normal: na::Unit>, + collider: DefaultColliderHandle, +} + +impl Plane { + #[cfg(feature = "dim2")] + pub fn new( + collider: DefaultColliderHandle, + colliders: &DefaultColliderSet, + position: &Point, + normal: &Vector, + color: Point3, + _: &mut Window, + ) -> Plane { + let mut res = Plane { + color, + base_color: color, + position: *position, + normal: na::Unit::new_normalize(*normal), + collider, + }; + + res.update(colliders); + res + } + + #[cfg(feature = "dim3")] + pub fn new( + collider: DefaultColliderHandle, + colliders: &DefaultColliderSet, + world_pos: &Point3, + world_normal: &Vector3, + color: Point3, + window: &mut Window, + ) -> Plane { + let mut res = Plane { + gfx: window.add_quad(100.0, 100.0, 10, 10), + collider, + }; + + if colliders + .get(collider) + .unwrap() + .query_type() + .is_proximity_query() + { + res.gfx.set_surface_rendering_activation(false); + res.gfx.set_lines_width(1.0); + } + + res.gfx.set_color(color.x, color.y, color.z); + + let up = if world_normal.z.is_zero() && world_normal.y.is_zero() { + Vector3::z() + } else { + Vector3::x() + }; + + res.gfx + .reorient(world_pos, &(*world_pos + *world_normal), &up); + + res.update(colliders); + + res + } + + pub fn select(&mut self) {} + + pub fn unselect(&mut self) {} + + pub fn update(&mut self, _: &DefaultColliderSet) { + // FIXME: atm we assume the plane does not move + } + + #[cfg(feature = "dim3")] + pub fn set_color(&mut self, color: Point3) { + self.gfx.set_color(color.x, color.y, color.z); + } + + #[cfg(feature = "dim2")] + pub fn set_color(&mut self, color: Point3) { + self.color = color; + self.base_color = color; + } + + #[cfg(feature = "dim3")] + pub fn scene_node(&self) -> &GraphicsNode { + &self.gfx + } + + #[cfg(feature = "dim3")] + pub fn scene_node_mut(&mut self) -> &mut GraphicsNode { + &mut self.gfx + } + + pub fn object(&self) -> DefaultColliderHandle { + self.collider + } + + #[cfg(feature = "dim2")] + pub fn draw(&mut self, window: &mut Window) { + let orth = Vector::new(-self.normal.y, self.normal.x); + window.draw_planar_line( + &(self.position - orth * 50.0), + &(self.position + orth * 50.0), + &self.color, + ); + } +} diff --git a/src_testbed/objects/polyline.rs b/src_testbed/objects/polyline.rs new file mode 100644 index 000000000..77841aeb4 --- /dev/null +++ b/src_testbed/objects/polyline.rs @@ -0,0 +1,79 @@ +use kiss3d::window::Window; +use na::{Isometry2, Point2, Point3}; +use ncollide2d::shape; +use nphysics2d::object::{ColliderAnchor, DefaultColliderHandle, DefaultColliderSet}; + +pub struct Polyline { + color: Point3, + base_color: Point3, + vertices: Vec>, + indices: Vec>, + collider: DefaultColliderHandle, + pos: Isometry2, +} + +impl Polyline { + pub fn new( + collider: DefaultColliderHandle, + colliders: &DefaultColliderSet, + _: Isometry2, + vertices: Vec>, + indices: Vec>, + color: Point3, + _: &mut Window, + ) -> Polyline { + let mut res = Polyline { + color, + pos: Isometry2::identity(), + base_color: color, + vertices, + indices, + collider, + }; + + res.update(colliders); + res + } + + pub fn select(&mut self) { + self.color = Point3::new(1.0, 0.0, 0.0); + } + + pub fn unselect(&mut self) { + self.color = self.base_color; + } + + pub fn set_color(&mut self, color: Point3) { + self.color = color; + self.base_color = color; + } + + pub fn update(&mut self, colliders: &DefaultColliderSet) { + // Update if some deformation occurred. + // FIXME: don't update if it did not move. + if let Some(c) = colliders.get(self.collider) { + self.pos = *c.position(); + if let ColliderAnchor::OnDeformableBody { .. } = c.anchor() { + let shape = c.shape().as_shape::>().unwrap(); + self.vertices = shape.points().to_vec(); + self.indices.clear(); + + for e in shape.edges() { + self.indices.push(e.indices); + } + } + } + } + + pub fn object(&self) -> DefaultColliderHandle { + self.collider + } + + pub fn draw(&mut self, window: &mut Window) { + for idx in &self.indices { + let p1 = self.pos * self.vertices[idx.x]; + let p2 = self.pos * self.vertices[idx.y]; + window.draw_planar_line(&p1, &p2, &self.color) + } + } +} diff --git a/src_testbed/physx_backend.rs b/src_testbed/physx_backend.rs new file mode 100644 index 000000000..9ae7bb8c9 --- /dev/null +++ b/src_testbed/physx_backend.rs @@ -0,0 +1,604 @@ +#![allow(dead_code)] + +use na::{Isometry3, Matrix3, Matrix4, Point3, Rotation3, Translation3, UnitQuaternion, Vector3}; +use physx::prelude::*; +use rapier::counters::Counters; +use rapier::dynamics::{ + IntegrationParameters, JointParams, JointSet, RigidBodyHandle, RigidBodySet, +}; +use rapier::geometry::{Collider, ColliderSet, Shape}; +use rapier::utils::WBasis; +use std::collections::HashMap; + +const PX_PHYSICS_VERSION: u32 = physx::version(4, 1, 1); + +trait IntoNa { + type Output; + fn into_na(self) -> Self::Output; +} + +impl IntoNa for glam::Mat4 { + type Output = Matrix4; + fn into_na(self) -> Self::Output { + self.to_cols_array_2d().into() + } +} + +trait IntoPhysx { + type Output; + fn into_physx(self) -> Self::Output; +} + +impl IntoPhysx for Vector3 { + type Output = physx_sys::PxVec3; + fn into_physx(self) -> Self::Output { + physx_sys::PxVec3 { + x: self.x, + y: self.y, + z: self.z, + } + } +} + +impl IntoPhysx for Point3 { + type Output = physx_sys::PxVec3; + fn into_physx(self) -> Self::Output { + physx_sys::PxVec3 { + x: self.x, + y: self.y, + z: self.z, + } + } +} + +impl IntoPhysx for Isometry3 { + type Output = physx_sys::PxTransform; + fn into_physx(self) -> Self::Output { + physx::transform::gl_to_px_tf(self.into_glam()) + } +} + +trait IntoGlam { + type Output; + fn into_glam(self) -> Self::Output; +} + +impl IntoGlam for Vector3 { + type Output = glam::Vec3; + fn into_glam(self) -> Self::Output { + glam::vec3(self.x, self.y, self.z) + } +} + +impl IntoGlam for Point3 { + type Output = glam::Vec3; + fn into_glam(self) -> Self::Output { + glam::vec3(self.x, self.y, self.z) + } +} + +impl IntoGlam for Matrix4 { + type Output = glam::Mat4; + fn into_glam(self) -> Self::Output { + glam::Mat4::from_cols_array_2d(&self.into()) + } +} + +impl IntoGlam for Isometry3 { + type Output = glam::Mat4; + fn into_glam(self) -> Self::Output { + glam::Mat4::from_cols_array_2d(&self.to_homogeneous().into()) + } +} + +thread_local! { +pub static FOUNDATION: std::cell::RefCell = std::cell::RefCell::new(Foundation::new(PX_PHYSICS_VERSION)); +} + +pub struct PhysxWorld { + physics: Physics, + cooking: Cooking, + scene: Scene, + rapier2physx: HashMap, +} + +impl PhysxWorld { + pub fn from_rapier( + gravity: Vector3, + integration_parameters: &IntegrationParameters, + bodies: &RigidBodySet, + colliders: &ColliderSet, + joints: &JointSet, + use_two_friction_directions: bool, + num_threads: usize, + ) -> Self { + let mut rapier2physx = HashMap::new(); + let mut physics = FOUNDATION.with(|f| { + PhysicsBuilder::default() + .load_extensions(false) + .build(&mut *f.borrow_mut()) + }); + let mut cooking = FOUNDATION.with(|f| unsafe { + let sc = physx_sys::PxTolerancesScale_new(); + let params = physx_sys::PxCookingParams_new(&sc); + Cooking::new(PX_PHYSICS_VERSION, &mut *f.borrow_mut(), params) + }); + + let scene_desc = MySceneBuilder::default() + .set_gravity(gravity.into_glam()) + .set_simulation_threading(SimulationThreadType::Dedicated(num_threads as u32)) + // .set_broad_phase_type(BroadPhaseType::SweepAndPrune) + // .set_solver_type(physx_sys::PxSolverType::eTGS) + .build_desc(&mut physics); + + let raw_scene = + unsafe { physx_sys::PxPhysics_createScene_mut(physics.get_raw_mut(), &scene_desc) }; + + // FIXME: we do this because we are also using two + // friction directions. We should add to rapier the option to use + // one friction direction too, and perhaps an equivalent of physX + // ePATCH friction type. + if use_two_friction_directions { + unsafe { + physx_sys::PxScene_setFrictionType_mut( + raw_scene, + physx_sys::PxFrictionType::eTWO_DIRECTIONAL, + ); + } + } + + let mut scene = Scene::new(raw_scene); + + for (rapier_handle, rb) in bodies.iter() { + use physx::rigid_dynamic::RigidDynamic; + use physx::rigid_static::RigidStatic; + use physx::transform; + + let pos = transform::gl_to_px_tf(rb.position.to_homogeneous().into_glam()); + if rb.is_dynamic() { + let actor = unsafe { + physx_sys::PxPhysics_createRigidDynamic_mut(physics.get_raw_mut(), &pos) + }; + + unsafe { + physx_sys::PxRigidDynamic_setSolverIterationCounts_mut( + actor, + integration_parameters.max_position_iterations as u32, + integration_parameters.max_velocity_iterations as u32, + ); + } + + let physx_handle = scene.add_dynamic(RigidDynamic::new(actor)); + rapier2physx.insert(rapier_handle, physx_handle); + } else { + let actor = unsafe { + physx_sys::PxPhysics_createRigidStatic_mut(physics.get_raw_mut(), &pos) + }; + + let physx_handle = scene.add_actor(RigidStatic::new(actor)); + rapier2physx.insert(rapier_handle, physx_handle); + } + } + + for (_, collider) in colliders.iter() { + if let Some((px_collider, collider_pos)) = + physx_collider_from_rapier_collider(&collider) + { + let material = physics.create_material( + collider.friction, + collider.friction, + collider.restitution, + ); + let geometry = cooking.make_geometry(px_collider); + let flags = if collider.is_sensor() { + physx_sys::PxShapeFlags { + mBits: physx_sys::PxShapeFlag::eTRIGGER_SHAPE as u8, + } + } else { + physx_sys::PxShapeFlags { + mBits: physx_sys::PxShapeFlag::eSIMULATION_SHAPE as u8, + } + }; + + let handle = rapier2physx[&collider.parent()]; + let parent_body = &bodies[collider.parent()]; + let parent = if !parent_body.is_dynamic() { + scene.get_static_mut(handle).unwrap().as_ptr_mut().ptr + as *mut physx_sys::PxRigidActor + } else { + scene.get_dynamic_mut(handle).unwrap().as_ptr_mut().ptr + as *mut physx_sys::PxRigidActor + }; + + unsafe { + let shape = physx_sys::PxPhysics_createShape_mut( + physics.get_raw_mut(), + geometry.as_raw(), + material, + true, + flags.into(), + ); + let pose = collider_pos.into_physx(); + physx_sys::PxShape_setLocalPose_mut(shape, &pose); + physx_sys::PxRigidActor_attachShape_mut(parent, shape); + }; + } + } + + let mut res = Self { + physics, + cooking, + scene, + rapier2physx, + }; + + res.setup_joints(joints); + res + } + + fn setup_joints(&mut self, joints: &JointSet) { + unsafe { + for joint in joints.iter() { + let actor1 = self.rapier2physx[&joint.body1]; + let actor2 = self.rapier2physx[&joint.body2]; + + match &joint.params { + JointParams::BallJoint(params) => { + let frame1 = physx::transform::gl_to_px_tf( + Isometry3::new(params.local_anchor1.coords, na::zero()).into_glam(), + ); + let frame2 = physx::transform::gl_to_px_tf( + Isometry3::new(params.local_anchor2.coords, na::zero()).into_glam(), + ); + + physx_sys::phys_PxSphericalJointCreate( + self.physics.get_raw_mut(), + actor1.0 as *mut _, + &frame1 as *const _, + actor2.0 as *mut _, + &frame2 as *const _, + ); + } + JointParams::RevoluteJoint(params) => { + // NOTE: orthonormal_basis() returns the two basis vectors. + // However we only use one and recompute the other just to + // make sure our basis is right-handed. + let basis1a = params.local_axis1.orthonormal_basis()[0]; + let basis2a = params.local_axis2.orthonormal_basis()[0]; + let basis1b = params.local_axis1.cross(&basis1a); + let basis2b = params.local_axis2.cross(&basis2a); + + let rotmat1 = Rotation3::from_matrix_unchecked(Matrix3::from_columns(&[ + params.local_axis1.into_inner(), + basis1a, + basis1b, + ])); + let rotmat2 = Rotation3::from_matrix_unchecked(Matrix3::from_columns(&[ + params.local_axis2.into_inner(), + basis2a, + basis2b, + ])); + let axisangle1 = rotmat1.scaled_axis(); + let axisangle2 = rotmat2.scaled_axis(); + + let frame1 = physx::transform::gl_to_px_tf( + Isometry3::new(params.local_anchor1.coords, axisangle1).into_glam(), + ); + let frame2 = physx::transform::gl_to_px_tf( + Isometry3::new(params.local_anchor2.coords, axisangle2).into_glam(), + ); + + physx_sys::phys_PxRevoluteJointCreate( + self.physics.get_raw_mut(), + actor1.0 as *mut _, + &frame1 as *const _, + actor2.0 as *mut _, + &frame2 as *const _, + ); + } + + JointParams::PrismaticJoint(params) => { + // NOTE: orthonormal_basis() returns the two basis vectors. + // However we only use one and recompute the other just to + // make sure our basis is right-handed. + let basis1a = params.local_axis1().orthonormal_basis()[0]; + let basis2a = params.local_axis2().orthonormal_basis()[0]; + let basis1b = params.local_axis1().cross(&basis1a); + let basis2b = params.local_axis2().cross(&basis2a); + + let rotmat1 = Rotation3::from_matrix_unchecked(Matrix3::from_columns(&[ + params.local_axis1().into_inner(), + basis1a, + basis1b, + ])); + let rotmat2 = Rotation3::from_matrix_unchecked(Matrix3::from_columns(&[ + params.local_axis2().into_inner(), + basis2a, + basis2b, + ])); + + let axisangle1 = rotmat1.scaled_axis(); + let axisangle2 = rotmat2.scaled_axis(); + + let frame1 = physx::transform::gl_to_px_tf( + Isometry3::new(params.local_anchor1.coords, axisangle1).into_glam(), + ); + let frame2 = physx::transform::gl_to_px_tf( + Isometry3::new(params.local_anchor2.coords, axisangle2).into_glam(), + ); + + let joint = physx_sys::phys_PxPrismaticJointCreate( + self.physics.get_raw_mut(), + actor1.0 as *mut _, + &frame1 as *const _, + actor2.0 as *mut _, + &frame2 as *const _, + ); + + if params.limits_enabled { + let limits = physx_sys::PxJointLinearLimitPair { + restitution: 0.0, + bounceThreshold: 0.0, + stiffness: 0.0, + damping: 0.0, + contactDistance: 0.01, + lower: params.limits[0], + upper: params.limits[1], + }; + physx_sys::PxPrismaticJoint_setLimit_mut(joint, &limits); + physx_sys::PxPrismaticJoint_setPrismaticJointFlag_mut( + joint, + physx_sys::PxPrismaticJointFlag::eLIMIT_ENABLED, + true, + ); + } + } + JointParams::FixedJoint(params) => { + let frame1 = + physx::transform::gl_to_px_tf(params.local_anchor1.into_glam()); + let frame2 = + physx::transform::gl_to_px_tf(params.local_anchor2.into_glam()); + + physx_sys::phys_PxFixedJointCreate( + self.physics.get_raw_mut(), + actor1.0 as *mut _, + &frame1 as *const _, + actor2.0 as *mut _, + &frame2 as *const _, + ); + } + } + } + } + } + + pub fn step(&mut self, counters: &mut Counters, params: &IntegrationParameters) { + counters.step_started(); + self.scene.step(params.dt(), true); + counters.step_completed(); + } + + pub fn sync(&self, bodies: &mut RigidBodySet, colliders: &mut ColliderSet) { + for (rapier_handle, physx_handle) in self.rapier2physx.iter() { + let mut rb = bodies.get_mut(*rapier_handle).unwrap(); + let ra = self.scene.get_rigid_actor(*physx_handle).unwrap(); + let pos = ra.get_global_pose().into_na(); + let iso = na::convert_unchecked(pos); + rb.set_position(iso); + + if rb.is_kinematic() {} + + for coll_handle in rb.colliders() { + let collider = &mut colliders[*coll_handle]; + collider.set_position_debug(iso * collider.delta()); + } + } + } +} + +fn physx_collider_from_rapier_collider( + collider: &Collider, +) -> Option<(ColliderDesc, Isometry3)> { + let mut local_pose = Isometry3::identity(); + let desc = match collider.shape() { + Shape::Cuboid(cuboid) => ColliderDesc::Box( + cuboid.half_extents.x, + cuboid.half_extents.y, + cuboid.half_extents.z, + ), + Shape::Ball(ball) => ColliderDesc::Sphere(ball.radius), + Shape::Capsule(capsule) => { + let center = capsule.center(); + let mut dir = capsule.b - capsule.a; + + if dir.x < 0.0 { + dir = -dir; + } + + let rot = UnitQuaternion::rotation_between(&Vector3::x(), &dir); + local_pose = + Translation3::from(center.coords) * rot.unwrap_or(UnitQuaternion::identity()); + ColliderDesc::Capsule(capsule.radius, capsule.height()) + } + Shape::Trimesh(trimesh) => ColliderDesc::TriMesh { + vertices: trimesh + .vertices() + .iter() + .map(|pt| pt.into_physx()) + .collect(), + indices: trimesh.flat_indices().to_vec(), + mesh_scale: Vector3::repeat(1.0).into_glam(), + }, + _ => { + eprintln!("Creating a shape unknown to the PhysX backend."); + return None; + } + }; + + Some((desc, local_pose)) +} + +/* + * + * XXX: All the remaining code is a duplicate from physx-rs to allow more customizations. + * + */ +use physx::scene::SimulationThreadType; + +pub struct MySceneBuilder { + gravity: glam::Vec3, + simulation_filter_shader: Option, + simulation_threading: Option, + broad_phase_type: BroadPhaseType, + use_controller_manager: bool, + controller_manager_locking: bool, + call_default_filter_shader_first: bool, + use_ccd: bool, + enable_ccd_resweep: bool, + solver_type: u32, +} + +impl Default for MySceneBuilder { + fn default() -> Self { + Self { + gravity: glam::Vec3::new(0.0, -9.80665, 0.0), // standard gravity value + call_default_filter_shader_first: true, + simulation_filter_shader: None, + simulation_threading: None, + broad_phase_type: BroadPhaseType::SweepAndPrune, + use_controller_manager: false, + controller_manager_locking: false, + use_ccd: false, + enable_ccd_resweep: false, + solver_type: physx_sys::PxSolverType::ePGS, + } + } +} + +impl MySceneBuilder { + /// Set the gravity for the scene. + /// + /// Default: [0.0, -9.80665, 0.0] (standard gravity) + pub fn set_gravity(&mut self, gravity: glam::Vec3) -> &mut Self { + self.gravity = gravity; + self + } + + /// Set a callback to be invoked on various simulation events. Note: + /// Currently only handles collision events + /// + /// Default: not set + pub fn set_simulation_filter_shader( + &mut self, + simulation_filter_shader: physx_sys::SimulationFilterShader, + ) -> &mut Self { + self.simulation_filter_shader = Some(simulation_filter_shader); + self + } + + /// Enable the controller manager on the scene. + /// + /// Default: false, false + pub fn use_controller_manager( + &mut self, + use_controller_manager: bool, + locking_enabled: bool, + ) -> &mut Self { + self.use_controller_manager = use_controller_manager; + self.controller_manager_locking = locking_enabled; + self + } + + pub fn set_solver_type(&mut self, solver_type: u32) -> &mut Self { + self.solver_type = solver_type; + self + } + + /// Sets whether the filter should begin by calling the default filter shader + /// PxDefaultSimulationFilterShader that emulates the PhysX 2.8 rules. + /// + /// Default: true + pub fn set_call_default_filter_shader_first( + &mut self, + call_default_filter_shader_first: bool, + ) -> &mut Self { + self.call_default_filter_shader_first = call_default_filter_shader_first; + self + } + + /// Set the number of threads to use for simulation + /// + /// Default: not set + pub fn set_simulation_threading( + &mut self, + simulation_threading: SimulationThreadType, + ) -> &mut Self { + self.simulation_threading = Some(simulation_threading); + self + } + + /// Set collision detection type + /// + /// Default: Sweep and prune + pub fn set_broad_phase_type(&mut self, broad_phase_type: BroadPhaseType) -> &mut Self { + self.broad_phase_type = broad_phase_type; + self + } + + /// Set if CCD (continuous collision detection) should be available for use in the scene. + /// Doesn't automatically enable it for all rigid bodies, they still need to be flagged. + /// + /// If you don't set enable_ccd_resweep to true, eDISABLE_CCD_RESWEEP is set, which improves performance + /// at the cost of accuracy right after bounces. + /// + /// Default: false, false + pub fn set_use_ccd(&mut self, use_ccd: bool, enable_ccd_resweep: bool) -> &mut Self { + self.use_ccd = use_ccd; + self.enable_ccd_resweep = enable_ccd_resweep; + self + } + + pub(super) fn build_desc(&self, physics: &mut Physics) -> physx_sys::PxSceneDesc { + unsafe { + let tolerances = physics.get_tolerances_scale(); + let mut scene_desc = physx_sys::PxSceneDesc_new(tolerances); + + let dispatcher = match self.simulation_threading.as_ref().expect("foo") { + SimulationThreadType::Default => { + physx_sys::phys_PxDefaultCpuDispatcherCreate(1, std::ptr::null_mut()) as *mut _ + } + SimulationThreadType::Dedicated(count) => { + physx_sys::phys_PxDefaultCpuDispatcherCreate(*count, std::ptr::null_mut()) + as *mut _ + } + SimulationThreadType::Shared(dispatcher) => *dispatcher as *mut _, + }; + + scene_desc.cpuDispatcher = dispatcher; + scene_desc.gravity = physx::transform::gl_to_px_v3(self.gravity); + if self.use_ccd { + scene_desc.flags.mBits |= physx_sys::PxSceneFlag::eENABLE_CCD; + if !self.enable_ccd_resweep { + scene_desc.flags.mBits |= physx_sys::PxSceneFlag::eDISABLE_CCD_RESWEEP; + } + } + if let Some(filter_shader) = self.simulation_filter_shader { + physx_sys::enable_custom_filter_shader( + &mut scene_desc as *mut physx_sys::PxSceneDesc, + filter_shader, + if self.call_default_filter_shader_first { + 1 + } else { + 0 + }, + ); + } else { + scene_desc.filterShader = physx_sys::get_default_simulation_filter_shader(); + } + + scene_desc.broadPhaseType = self.broad_phase_type.into(); + scene_desc.solverType = self.solver_type; + scene_desc + } + } +} diff --git a/src_testbed/testbed.rs b/src_testbed/testbed.rs new file mode 100644 index 000000000..ab709ed54 --- /dev/null +++ b/src_testbed/testbed.rs @@ -0,0 +1,1652 @@ +use std::env; +use std::mem; +use std::path::Path; +use std::rc::Rc; + +use crate::engine::GraphicsManager; +#[cfg(feature = "fluids")] +use crate::objects::FluidRenderingMode; +use crate::ui::TestbedUi; +use crossbeam::channel::Receiver; +use kiss3d::camera::Camera; +use kiss3d::event::Event; +use kiss3d::event::{Action, Key, WindowEvent}; +use kiss3d::light::Light; +use kiss3d::loader::obj; +use kiss3d::planar_camera::PlanarCamera; +use kiss3d::post_processing::PostProcessingEffect; +use kiss3d::text::Font; +use kiss3d::window::{State, Window}; +use na::{self, Point2, Point3, Vector3}; +use rapier::dynamics::{ + ActivationStatus, IntegrationParameters, JointSet, RigidBodyHandle, RigidBodySet, +}; +use rapier::geometry::{BroadPhase, ColliderSet, ContactEvent, NarrowPhase, ProximityEvent}; +use rapier::math::Vector; +use rapier::pipeline::{ChannelEventCollector, PhysicsPipeline}; +#[cfg(feature = "fluids")] +use salva::{coupling::ColliderCouplingSet, object::FluidHandle, LiquidWorld}; + +#[cfg(all(feature = "dim2", feature = "other-backends"))] +use crate::box2d_backend::Box2dWorld; +#[cfg(feature = "other-backends")] +use crate::nphysics_backend::NPhysicsWorld; +#[cfg(all(feature = "dim3", feature = "other-backends"))] +use crate::physx_backend::PhysxWorld; + +const RAPIER_BACKEND: usize = 0; +const NPHYSICS_BACKEND: usize = 1; +#[cfg(feature = "dim2")] +const BOX2D_BACKEND: usize = 2; +pub(crate) const PHYSX_BACKEND_PATCH_FRICTION: usize = 2; +pub(crate) const PHYSX_BACKEND_TWO_FRICTION_DIR: usize = 3; + +#[derive(PartialEq)] +pub enum RunMode { + Running, + Stop, + Step, + Quit, +} + +#[cfg(not(feature = "log"))] +fn usage(exe_name: &str) { + println!("Usage: {} [OPTION] ", exe_name); + println!(); + println!("Options:"); + println!(" --help - prints this help message and exits."); + println!(" --pause - do not start the simulation right away."); +} + +#[cfg(feature = "log")] +fn usage(exe_name: &str) { + info!("Usage: {} [OPTION] ", exe_name); + info!(""); + info!("Options:"); + info!(" --help - prints this help message and exits."); + info!(" --pause - do not start the simulation right away."); +} + +bitflags! { + #[derive(Default)] + pub struct TestbedStateFlags: u32 { + const NONE = 0; + const SLEEP = 1 << 0; + const SUB_STEPPING = 1 << 1; + const SHAPES = 1 << 2; + const JOINTS = 1 << 3; + const AABBS = 1 << 4; + const CONTACT_POINTS = 1 << 5; + const CONTACT_NORMALS = 1 << 6; + const CENTER_OF_MASSES = 1 << 7; + const WIREFRAME = 1 << 8; + const STATISTICS = 1 << 9; + const PROFILE = 1 << 10; + const DEBUG = 1 << 11; + } +} + +bitflags! { + pub struct TestbedActionFlags: u32 { + const RESET_WORLD_GRAPHICS = 1 << 0; + const EXAMPLE_CHANGED = 1 << 1; + const RESTART = 1 << 2; + const BACKEND_CHANGED = 1 << 3; + const TAKE_SNAPSHOT = 1 << 4; + const RESTORE_SNAPSHOT = 1 << 5; + } +} + +pub struct PhysicsSnapshot { + timestep_id: usize, + broad_phase: Vec, + narrow_phase: Vec, + bodies: Vec, + colliders: Vec, + joints: Vec, +} + +impl PhysicsSnapshot { + fn new( + timestep_id: usize, + broad_phase: &BroadPhase, + narrow_phase: &NarrowPhase, + bodies: &RigidBodySet, + colliders: &ColliderSet, + joints: &JointSet, + ) -> bincode::Result { + Ok(Self { + timestep_id, + broad_phase: bincode::serialize(broad_phase)?, + narrow_phase: bincode::serialize(narrow_phase)?, + bodies: bincode::serialize(bodies)?, + colliders: bincode::serialize(colliders)?, + joints: bincode::serialize(joints)?, + }) + } + + fn restore( + &self, + ) -> bincode::Result<( + usize, + BroadPhase, + NarrowPhase, + RigidBodySet, + ColliderSet, + JointSet, + )> { + Ok(( + self.timestep_id, + bincode::deserialize(&self.broad_phase)?, + bincode::deserialize(&self.narrow_phase)?, + bincode::deserialize(&self.bodies)?, + bincode::deserialize(&self.colliders)?, + bincode::deserialize(&self.joints)?, + )) + } + + fn print_snapshot_len(&self) { + let total = self.broad_phase.len() + + self.narrow_phase.len() + + self.bodies.len() + + self.colliders.len() + + self.joints.len(); + println!("Snapshot length: {}B", total); + println!("|_ broad_phase: {}B", self.broad_phase.len()); + println!("|_ narrow_phase: {}B", self.narrow_phase.len()); + println!("|_ bodies: {}B", self.bodies.len()); + println!("|_ colliders: {}B", self.colliders.len()); + println!("|_ joints: {}B", self.joints.len()); + } +} + +pub struct PhysicsEvents { + pub contact_events: Receiver, + pub proximity_events: Receiver, +} + +impl PhysicsEvents { + fn poll_all(&self) { + while let Ok(_) = self.contact_events.try_recv() {} + while let Ok(_) = self.proximity_events.try_recv() {} + } +} + +pub struct TestbedState { + pub running: RunMode, + pub draw_colls: bool, + // pub grabbed_object: Option, + // pub grabbed_object_constraint: Option, + pub grabbed_object_plane: (Point3, Vector3), + pub can_grab_behind_ground: bool, + pub drawing_ray: Option>, + pub prev_flags: TestbedStateFlags, + pub flags: TestbedStateFlags, + pub action_flags: TestbedActionFlags, + pub backend_names: Vec<&'static str>, + pub example_names: Vec<&'static str>, + pub selected_example: usize, + pub selected_backend: usize, + pub physx_use_two_friction_directions: bool, + pub num_threads: usize, + pub snapshot: Option, + #[cfg(feature = "parallel")] + pub thread_pool: rapier::rayon::ThreadPool, + pub timestep_id: usize, +} + +#[cfg(feature = "fluids")] +struct FluidsState { + world: LiquidWorld, + coupling: ColliderCouplingSet, +} + +pub struct Testbed { + builders: Vec<(&'static str, fn(&mut Testbed))>, + #[cfg(feature = "fluids")] + fluids: Option, + gravity: Vector, + integration_parameters: IntegrationParameters, + broad_phase: BroadPhase, + narrow_phase: NarrowPhase, + bodies: RigidBodySet, + colliders: ColliderSet, + joints: JointSet, + physics_pipeline: PhysicsPipeline, + window: Option>, + graphics: GraphicsManager, + nsteps: usize, + camera_locked: bool, // Used so that the camera can remain the same before and after we change backend or press the restart button. + callbacks: Callbacks, + #[cfg(feature = "fluids")] + callbacks_fluids: CallbacksFluids, + time: f32, + hide_counters: bool, + // persistant_contacts: HashMap, + font: Rc, + // cursor_pos: Point2, + events: PhysicsEvents, + event_handler: ChannelEventCollector, + ui: TestbedUi, + state: TestbedState, + #[cfg(all(feature = "dim2", feature = "other-backends"))] + box2d: Option, + #[cfg(all(feature = "dim3", feature = "other-backends"))] + physx: Option, + #[cfg(feature = "other-backends")] + nphysics: Option, +} + +type Callbacks = Vec< + Box, +>; + +#[cfg(feature = "fluids")] +type CallbacksFluids = Vec< + Box< + dyn FnMut( + &mut LiquidWorld, + &mut ColliderCouplingSet, + &mut RigidBodySet, + &mut GraphicsManager, + f32, + ), + >, +>; + +impl Testbed { + pub fn new_empty() -> Testbed { + let graphics = GraphicsManager::new(); + + #[cfg(feature = "dim3")] + let mut window = Box::new(Window::new("rapier: 3d demo")); + #[cfg(feature = "dim2")] + let mut window = Box::new(Window::new("rapier: 2d demo")); + window.set_background_color(0.85, 0.85, 0.85); + window.set_framerate_limit(Some(60)); + window.set_light(Light::StickToCamera); + + let flags = TestbedStateFlags::SLEEP; + let ui = TestbedUi::new(&mut window); + + #[allow(unused_mut)] + let mut backend_names = vec!["rapier"]; + #[cfg(feature = "other-backends")] + backend_names.push("nphysics"); + #[cfg(all(feature = "dim2", feature = "other-backends"))] + backend_names.push("box2d"); + #[cfg(all(feature = "dim3", feature = "other-backends"))] + backend_names.push("physx (patch friction)"); + #[cfg(all(feature = "dim3", feature = "other-backends"))] + backend_names.push("physx (two friction dir)"); + + #[cfg(feature = "parallel")] + let num_threads = num_cpus::get_physical(); + #[cfg(not(feature = "parallel"))] + let num_threads = 1; + + #[cfg(feature = "parallel")] + let thread_pool = rapier::rayon::ThreadPoolBuilder::new() + .num_threads(num_threads) + .build() + .unwrap(); + + let state = TestbedState { + running: RunMode::Running, + draw_colls: false, + // grabbed_object: None, + // grabbed_object_constraint: None, + grabbed_object_plane: (Point3::origin(), na::zero()), + can_grab_behind_ground: false, + drawing_ray: None, + snapshot: None, + prev_flags: flags, + flags, + action_flags: TestbedActionFlags::empty(), + backend_names, + example_names: Vec::new(), + selected_example: 0, + timestep_id: 0, + selected_backend: RAPIER_BACKEND, + physx_use_two_friction_directions: true, + num_threads, + #[cfg(feature = "parallel")] + thread_pool, + }; + + let gravity = Vector::y() * -9.81; + let integration_parameters = IntegrationParameters::default(); + let broad_phase = BroadPhase::new(); + let narrow_phase = NarrowPhase::new(); + let bodies = RigidBodySet::new(); + let colliders = ColliderSet::new(); + let joints = JointSet::new(); + let contact_channel = crossbeam::channel::unbounded(); + let proximity_channel = crossbeam::channel::unbounded(); + let event_handler = ChannelEventCollector::new(proximity_channel.0, contact_channel.0); + let events = PhysicsEvents { + contact_events: contact_channel.1, + proximity_events: proximity_channel.1, + }; + + Testbed { + builders: Vec::new(), + #[cfg(feature = "fluids")] + fluids: None, + gravity, + integration_parameters, + broad_phase, + narrow_phase, + bodies, + colliders, + joints, + physics_pipeline: PhysicsPipeline::new(), + callbacks: Vec::new(), + #[cfg(feature = "fluids")] + callbacks_fluids: Vec::new(), + window: Some(window), + graphics, + nsteps: 1, + camera_locked: false, + time: 0.0, + hide_counters: true, + // persistant_contacts: HashMap::new(), + font: Font::default(), + // cursor_pos: Point2::new(0.0f32, 0.0), + ui, + event_handler, + events, + state, + #[cfg(all(feature = "dim2", feature = "other-backends"))] + box2d: None, + #[cfg(all(feature = "dim3", feature = "other-backends"))] + physx: None, + #[cfg(feature = "other-backends")] + nphysics: None, + } + } + + pub fn new(bodies: RigidBodySet, colliders: ColliderSet, joints: JointSet) -> Self { + let mut res = Self::new_empty(); + res.set_world(bodies, colliders, joints); + res + } + + pub fn from_builders(default: usize, builders: Vec<(&'static str, fn(&mut Self))>) -> Self { + let mut res = Testbed::new_empty(); + res.state + .action_flags + .set(TestbedActionFlags::EXAMPLE_CHANGED, true); + res.state.selected_example = default; + res.set_builders(builders); + res + } + + pub fn set_number_of_steps_per_frame(&mut self, nsteps: usize) { + self.nsteps = nsteps + } + + pub fn allow_grabbing_behind_ground(&mut self, allow: bool) { + self.state.can_grab_behind_ground = allow; + } + + pub fn hide_performance_counters(&mut self) { + self.hide_counters = true; + } + + pub fn show_performance_counters(&mut self) { + self.hide_counters = false; + } + + pub fn set_world(&mut self, bodies: RigidBodySet, colliders: ColliderSet, joints: JointSet) { + println!("Num bodies: {}", bodies.len()); + println!("Num joints: {}", joints.len()); + self.bodies = bodies; + self.colliders = colliders; + self.joints = joints; + self.broad_phase = BroadPhase::new(); + self.narrow_phase = NarrowPhase::new(); + self.state + .action_flags + .set(TestbedActionFlags::RESET_WORLD_GRAPHICS, true); + self.time = 0.0; + self.state.timestep_id = 0; + self.physics_pipeline.counters.enable(); + + #[cfg(all(feature = "dim2", feature = "other-backends"))] + { + if self.state.selected_backend == BOX2D_BACKEND { + self.box2d = Some(Box2dWorld::from_rapier( + self.gravity, + &self.bodies, + &self.colliders, + &self.joints, + )); + } + } + + #[cfg(all(feature = "dim3", feature = "other-backends"))] + { + if self.state.selected_backend == PHYSX_BACKEND_PATCH_FRICTION + || self.state.selected_backend == PHYSX_BACKEND_TWO_FRICTION_DIR + { + self.physx = Some(PhysxWorld::from_rapier( + self.gravity, + &self.integration_parameters, + &self.bodies, + &self.colliders, + &self.joints, + self.state.selected_backend == PHYSX_BACKEND_TWO_FRICTION_DIR, + self.state.num_threads, + )); + } + } + + #[cfg(feature = "other-backends")] + { + if self.state.selected_backend == NPHYSICS_BACKEND { + self.nphysics = Some(NPhysicsWorld::from_rapier( + self.gravity, + &self.bodies, + &self.colliders, + &self.joints, + )); + } + } + } + + #[cfg(feature = "fluids")] + pub fn set_liquid_world( + &mut self, + mut liquid_world: LiquidWorld, + coupling: ColliderCouplingSet, + ) { + liquid_world.counters.enable(); + self.fluids = Some(FluidsState { + world: liquid_world, + coupling, + }); + } + + pub fn set_builders(&mut self, builders: Vec<(&'static str, fn(&mut Self))>) { + self.state.example_names = builders.iter().map(|e| e.0).collect(); + self.builders = builders + } + + #[cfg(feature = "dim2")] + pub fn look_at(&mut self, at: Point2, zoom: f32) { + if !self.camera_locked { + self.graphics.look_at(at, zoom); + } + } + + #[cfg(feature = "dim3")] + pub fn look_at(&mut self, eye: Point3, at: Point3) { + if !self.camera_locked { + self.graphics.look_at(eye, at); + } + } + + pub fn set_body_color(&mut self, body: RigidBodyHandle, color: Point3) { + self.graphics.set_body_color(body, color); + } + + #[cfg(feature = "fluids")] + pub fn set_fluid_color(&mut self, fluid: FluidHandle, color: Point3) { + self.graphics.set_fluid_color(fluid, color); + } + + pub fn set_body_wireframe(&mut self, body: RigidBodyHandle, wireframe_enabled: bool) { + self.graphics.set_body_wireframe(body, wireframe_enabled); + } + + #[cfg(feature = "fluids")] + pub fn set_fluid_rendering_mode(&mut self, mode: FluidRenderingMode) { + self.graphics.set_fluid_rendering_mode(mode) + } + + #[cfg(feature = "fluids")] + pub fn enable_boundary_particles_rendering(&mut self, enabled: bool) { + self.graphics.enable_boundary_particles_rendering(enabled) + } + + // pub fn world(&self) -> &Box { + // &self.world + // } + + pub fn graphics_mut(&mut self) -> &mut GraphicsManager { + &mut self.graphics + } + + #[cfg(feature = "dim3")] + pub fn set_up_axis(&mut self, up_axis: Vector3) { + self.graphics.set_up_axis(up_axis); + } + + pub fn load_obj(path: &str) -> Vec<(Vec>, Vec)> { + let path = Path::new(path); + let empty = Path::new("_some_non_existant_folder"); // dont bother loading mtl files correctly + let objects = obj::parse_file(&path, &empty, "").expect("Unable to open the obj file."); + + let mut res = Vec::new(); + + for (_, m, _) in objects.into_iter() { + let vertices = m.coords().read().unwrap().to_owned().unwrap(); + let indices = m.faces().read().unwrap().to_owned().unwrap(); + + let mut flat_indices = Vec::new(); + + for i in indices.into_iter() { + flat_indices.push(i.x as usize); + flat_indices.push(i.y as usize); + flat_indices.push(i.z as usize); + } + + let m = (vertices, flat_indices); + + res.push(m); + } + + res + } + + fn clear(&mut self, window: &mut Window) { + self.callbacks.clear(); + #[cfg(feature = "fluids")] + self.callbacks_fluids.clear(); + // self.persistant_contacts.clear(); + // self.state.grabbed_object = None; + // self.state.grabbed_object_constraint = None; + self.state.can_grab_behind_ground = false; + self.graphics.clear(window); + } + + pub fn add_callback< + F: FnMut(&mut RigidBodySet, &mut ColliderSet, &PhysicsEvents, &mut GraphicsManager, f32) + + 'static, + >( + &mut self, + callback: F, + ) { + self.callbacks.push(Box::new(callback)); + } + + #[cfg(feature = "fluids")] + pub fn add_callback_with_fluids< + F: FnMut( + &mut LiquidWorld, + &mut ColliderCouplingSet, + &mut RigidBodySet, + &mut GraphicsManager, + f32, + ) + 'static, + >( + &mut self, + callback: F, + ) { + self.callbacks_fluids.push(Box::new(callback)); + } + + pub fn run(mut self) { + let mut args = env::args(); + let mut benchmark_mode = false; + + if args.len() > 1 { + let exname = args.next().unwrap(); + for arg in args { + if &arg[..] == "--help" || &arg[..] == "-h" { + usage(&exname[..]); + return; + } else if &arg[..] == "--pause" { + self.state.running = RunMode::Stop; + } else if &arg[..] == "--bench" { + benchmark_mode = true; + } + } + } + + if benchmark_mode { + use std::fs::File; + use std::io::{BufWriter, Write}; + // Don't enter the main loop. We will just step the simulation here. + let mut results = Vec::new(); + let builders = mem::replace(&mut self.builders, Vec::new()); + let backend_names = self.state.backend_names.clone(); + + for builder in builders { + results.clear(); + println!("Running benchmark for {}", builder.0); + const NUM_ITERS: usize = 1000; + + for (backend_id, backend) in backend_names.iter().enumerate() { + println!("|_ using backend {}", backend); + self.state.selected_backend = backend_id; + if cfg!(feature = "dim3") + && (backend_id == PHYSX_BACKEND_PATCH_FRICTION + || backend_id == PHYSX_BACKEND_TWO_FRICTION_DIR) + { + self.integration_parameters.max_velocity_iterations = 1; + self.integration_parameters.max_position_iterations = 4; + } else { + self.integration_parameters.max_velocity_iterations = 4; + self.integration_parameters.max_position_iterations = 1; + } + // Init world. + (builder.1)(&mut self); + // Run the simulation. + let mut timings = Vec::new(); + for k in 0..=NUM_ITERS { + { + // FIXME: code duplicated from self.step() + if self.state.selected_backend == RAPIER_BACKEND { + #[cfg(feature = "parallel")] + { + let gravity = &self.gravity; + let params = &self.integration_parameters; + let pipeline = &mut self.physics_pipeline; + let narrow_phase = &mut self.narrow_phase; + let broad_phase = &mut self.broad_phase; + let bodies = &mut self.bodies; + let colliders = &mut self.colliders; + let joints = &mut self.joints; + let event_handler = &self.event_handler; + self.state.thread_pool.install(|| { + pipeline.step( + gravity, + params, + broad_phase, + narrow_phase, + bodies, + colliders, + joints, + event_handler, + ); + }); + } + + #[cfg(not(feature = "parallel"))] + self.physics_pipeline.step( + &self.gravity, + &self.integration_parameters, + &mut self.broad_phase, + &mut self.narrow_phase, + &mut self.bodies, + &mut self.colliders, + &mut self.joints, + &self.event_handler, + ); + + #[cfg(feature = "fluids")] + { + fluids_time = instant::now(); + if let Some(fluids) = &mut self.fluids { + let dt = self.world.timestep(); + let gravity = &self.world.gravity; + fluids.world.step_with_coupling( + dt, + gravity, + &mut fluids + .coupling + .as_manager_mut(&self.colliders, &mut self.bodies), + ); + } + + fluids_time = instant::now() - fluids_time; + } + } + + #[cfg(all(feature = "dim2", feature = "other-backends"))] + { + if self.state.selected_backend == BOX2D_BACKEND { + self.box2d.as_mut().unwrap().step( + &mut self.physics_pipeline.counters, + &self.integration_parameters, + ); + self.box2d + .as_mut() + .unwrap() + .sync(&mut self.bodies, &mut self.colliders); + } + } + + #[cfg(all(feature = "dim3", feature = "other-backends"))] + { + if self.state.selected_backend == PHYSX_BACKEND_PATCH_FRICTION + || self.state.selected_backend == PHYSX_BACKEND_TWO_FRICTION_DIR + { + // println!("Step"); + self.physx.as_mut().unwrap().step( + &mut self.physics_pipeline.counters, + &self.integration_parameters, + ); + self.physx + .as_mut() + .unwrap() + .sync(&mut self.bodies, &mut self.colliders); + } + } + + #[cfg(feature = "other-backends")] + { + if self.state.selected_backend == NPHYSICS_BACKEND { + self.nphysics.as_mut().unwrap().step( + &mut self.physics_pipeline.counters, + &self.integration_parameters, + ); + self.nphysics + .as_mut() + .unwrap() + .sync(&mut self.bodies, &mut self.colliders); + } + } + } + + // Skip the first update. + if k > 0 { + timings.push(self.physics_pipeline.counters.step_time.time()); + } + } + results.push(timings); + } + + // Write the result as a csv file. + let filename = format!("{}.csv", builder.0); + let mut file = BufWriter::new(File::create(filename).unwrap()); + + write!(file, "{}", backend_names[0]).unwrap(); + for backend in &backend_names[1..] { + write!(file, ",{}", backend).unwrap(); + } + writeln!(file).unwrap(); + + for i in 0..results[0].len() { + write!(file, "{}", results[0][i]).unwrap(); + for result in &results[1..] { + write!(file, ",{}", result[i]).unwrap(); + } + writeln!(file).unwrap(); + } + } + } else { + let window = mem::replace(&mut self.window, None).unwrap(); + window.render_loop(self); + } + } + + fn handle_common_event<'b>(&mut self, event: Event<'b>) -> Event<'b> { + match event.value { + WindowEvent::Key(Key::T, Action::Release, _) => { + if self.state.running == RunMode::Stop { + self.state.running = RunMode::Running; + } else { + self.state.running = RunMode::Stop; + } + } + WindowEvent::Key(Key::S, Action::Release, _) => self.state.running = RunMode::Step, + WindowEvent::Key(Key::R, Action::Release, _) => self + .state + .action_flags + .set(TestbedActionFlags::EXAMPLE_CHANGED, true), + WindowEvent::Key(Key::D, Action::Release, _) => { + // Delete 10% of the remaining dynamic bodies. + let dynamic_bodies: Vec<_> = self + .bodies + .iter() + .filter(|e| e.1.is_dynamic()) + .map(|e| e.0) + .collect(); + let num_to_delete = (dynamic_bodies.len() / 10).max(1); + for to_delete in &dynamic_bodies[..num_to_delete] { + self.physics_pipeline.remove_rigid_body( + *to_delete, + &mut self.broad_phase, + &mut self.narrow_phase, + &mut self.bodies, + &mut self.colliders, + &mut self.joints, + ); + } + } + _ => {} + } + + event + } + + #[cfg(feature = "dim2")] + fn handle_special_event(&mut self, window: &mut Window, _event: Event) { + if window.is_conrod_ui_capturing_mouse() { + return; + } + + /* + match event.value { + WindowEvent::MouseButton(MouseButton::Button1, Action::Press, modifier) => { + let all_groups = &CollisionGroups::new(); + for b in self.geometrical_world.interferences_with_point( + &self.colliders, + &self.cursor_pos, + all_groups, + ) { + if !b.1.query_type().is_proximity_query() + && Some(b.1.body()) != self.ground_handle + { + if let ColliderAnchor::OnBodyPart { body_part, .. } = b.1.anchor() { + self.state.grabbed_object = Some(*body_part); + } else { + continue; + } + } + } + + if modifier.contains(Modifiers::Shift) { + if let Some(body_part) = self.state.grabbed_object { + if Some(body_part.0) != self.ground_handle { + self.graphics.remove_body_nodes(window, body_part.0); + self.bodies.remove(body_part.0); + } + } + + self.state.grabbed_object = None; + } else if modifier.contains(Modifiers::Alt) { + self.state.drawing_ray = Some(self.cursor_pos); + } else if !modifier.contains(Modifiers::Control) { + if let Some(body) = self.state.grabbed_object { + if let Some(joint) = self.state.grabbed_object_constraint { + let _ = self.constraints.remove(joint); + } + + let body_pos = self + .bodies + .get(body.0) + .unwrap() + .part(body.1) + .unwrap() + .position(); + let attach1 = self.cursor_pos; + let attach2 = body_pos.inverse() * attach1; + + if let Some(ground) = self.ground_handle { + let joint = MouseConstraint::new( + BodyPartHandle(ground, 0), + body, + attach1, + attach2, + 1.0, + ); + self.state.grabbed_object_constraint = + Some(self.constraints.insert(joint)); + } + + for node in self.graphics.body_nodes_mut(body.0).unwrap().iter_mut() { + node.select() + } + } + + event.inhibited = true; + } else { + self.state.grabbed_object = None; + } + } + WindowEvent::MouseButton(MouseButton::Button1, Action::Release, _) => { + if let Some(body) = self.state.grabbed_object { + for n in self.graphics.body_nodes_mut(body.0).unwrap().iter_mut() { + n.unselect() + } + } + + if let Some(joint) = self.state.grabbed_object_constraint { + let _ = self.constraints.remove(joint); + } + + if let Some(start) = self.state.drawing_ray { + self.graphics + .add_ray(Ray::new(start, self.cursor_pos - start)); + } + + self.state.drawing_ray = None; + self.state.grabbed_object = None; + self.state.grabbed_object_constraint = None; + } + WindowEvent::CursorPos(x, y, modifiers) => { + self.cursor_pos.x = x as f32; + self.cursor_pos.y = y as f32; + + self.cursor_pos = self + .graphics + .camera() + .unproject(&self.cursor_pos, &na::convert(window.size())); + + let attach2 = self.cursor_pos; + if self.state.grabbed_object.is_some() { + if let Some(constraint) = self + .state + .grabbed_object_constraint + .and_then(|joint| self.constraints.get_mut(joint)) + .and_then(|joint| { + joint.downcast_mut::>() + }) + { + constraint.set_anchor_1(attach2); + } + } + + event.inhibited = + modifiers.contains(Modifiers::Control) || modifiers.contains(Modifiers::Shift); + } + _ => {} + } + */ + } + + #[cfg(feature = "dim3")] + fn handle_special_event(&mut self, window: &mut Window, _event: Event) { + if window.is_conrod_ui_capturing_mouse() { + return; + } + + /* + match event.value { + WindowEvent::MouseButton(MouseButton::Button1, Action::Press, modifier) => { + if modifier.contains(Modifiers::Alt) { + let size = window.size(); + let (pos, dir) = self + .graphics + .camera() + .unproject(&self.cursor_pos, &na::convert(size)); + let ray = Ray::new(pos, dir); + self.graphics.add_ray(ray); + + event.inhibited = true; + } else if modifier.contains(Modifiers::Shift) { + // XXX: huge and ugly code duplication for the ray cast. + let size = window.size(); + let (pos, dir) = self + .graphics + .camera() + .unproject(&self.cursor_pos, &na::convert(size)); + let ray = Ray::new(pos, dir); + + // cast the ray + let mut mintoi = Bounded::max_value(); + let mut minb = None; + + let all_groups = CollisionGroups::new(); + for (_, b, inter) in self.geometrical_world.interferences_with_ray( + &self.colliders, + &ray, + std::f32::MAX, + &all_groups, + ) { + if !b.query_type().is_proximity_query() && inter.toi < mintoi { + mintoi = inter.toi; + + let subshape = b.shape().subshape_containing_feature(inter.feature); + minb = Some(b.body_part(subshape)); + } + } + + if let Some(body_part) = minb { + if modifier.contains(Modifiers::Control) { + if Some(body_part.0) != self.ground_handle { + self.graphics.remove_body_nodes(window, body_part.0); + self.bodies.remove(body_part.0); + } + } else { + self.bodies + .get_mut(body_part.0) + .unwrap() + .apply_force_at_point( + body_part.1, + &(ray.dir.normalize() * 0.01), + &ray.point_at(mintoi), + ForceType::Impulse, + true, + ); + } + } + + event.inhibited = true; + } else if !modifier.contains(Modifiers::Control) { + match self.state.grabbed_object { + Some(body) => { + for n in self.graphics.body_nodes_mut(body.0).unwrap().iter_mut() { + n.unselect() + } + } + None => {} + } + + // XXX: huge and uggly code duplication for the ray cast. + let size = window.size(); + let (pos, dir) = self + .graphics + .camera() + .unproject(&self.cursor_pos, &na::convert(size)); + let ray = Ray::new(pos, dir); + + // cast the ray + let mut mintoi = Bounded::max_value(); + let mut minb = None; + + let all_groups = CollisionGroups::new(); + for (_, b, inter) in self.geometrical_world.interferences_with_ray( + &self.colliders, + &ray, + std::f32::MAX, + &all_groups, + ) { + if ((Some(b.body()) != self.ground_handle) + || self.state.can_grab_behind_ground) + && !b.query_type().is_proximity_query() + && inter.toi < mintoi + { + mintoi = inter.toi; + + let subshape = b.shape().subshape_containing_feature(inter.feature); + minb = Some(b.body_part(subshape)); + } + } + + if let Some(body_part_handle) = minb { + if self + .bodies + .get(body_part_handle.0) + .unwrap() + .status_dependent_ndofs() + != 0 + { + self.state.grabbed_object = minb; + for n in self + .graphics + .body_nodes_mut(body_part_handle.0) + .unwrap() + .iter_mut() + { + if let Some(joint) = self.state.grabbed_object_constraint { + let constraint = self.constraints.remove(joint).unwrap(); + let (b1, b2) = constraint.anchors(); + self.bodies.get_mut(b1.0).unwrap().activate(); + self.bodies.get_mut(b2.0).unwrap().activate(); + } + + let attach1 = ray.origin + ray.dir * mintoi; + let attach2 = { + let body = self.bodies.get_mut(body_part_handle.0).unwrap(); + body.activate(); + let part = body.part(body_part_handle.1).unwrap(); + body.material_point_at_world_point(part, &attach1) + }; + + if let Some(ground_handle) = self.ground_handle { + let constraint = MouseConstraint::new( + BodyPartHandle(ground_handle, 0), + body_part_handle, + attach1, + attach2, + 1.0, + ); + self.state.grabbed_object_plane = (attach1, -ray.dir); + self.state.grabbed_object_constraint = + Some(self.constraints.insert(constraint)); + } + + n.select() + } + } + } + + event.inhibited = true; + } + } + WindowEvent::MouseButton(MouseButton::Button1, Action::Release, _) => { + if let Some(body_part) = self.state.grabbed_object { + for n in self + .graphics + .body_nodes_mut(body_part.0) + .unwrap() + .iter_mut() + { + n.unselect() + } + } + + if let Some(joint) = self.state.grabbed_object_constraint { + let constraint = self.constraints.remove(joint).unwrap(); + let (b1, b2) = constraint.anchors(); + self.bodies.get_mut(b1.0).unwrap().activate(); + self.bodies.get_mut(b2.0).unwrap().activate(); + } + + self.state.grabbed_object = None; + self.state.grabbed_object_constraint = None; + } + WindowEvent::CursorPos(x, y, modifiers) => { + self.cursor_pos.x = x as f32; + self.cursor_pos.y = y as f32; + + // update the joint + if let Some(joint) = self.state.grabbed_object_constraint { + let size = window.size(); + let (pos, dir) = self + .graphics + .camera() + .unproject(&self.cursor_pos, &na::convert(size)); + let (ref ppos, ref pdir) = self.state.grabbed_object_plane; + + if let Some(inter) = query::ray_toi_with_plane(ppos, pdir, &Ray::new(pos, dir)) + { + let joint = self + .constraints + .get_mut(joint) + .unwrap() + .downcast_mut::>() + .unwrap(); + joint.set_anchor_1(pos + dir * inter) + } + } + + event.inhibited = modifiers.contains(Modifiers::Shift); + } + _ => {} + } + */ + } +} + +type CameraEffects<'a> = ( + Option<&'a mut dyn Camera>, + Option<&'a mut dyn PlanarCamera>, + Option<&'a mut dyn PostProcessingEffect>, +); + +impl State for Testbed { + fn cameras_and_effect(&mut self) -> CameraEffects<'_> { + #[cfg(feature = "dim2")] + let result = ( + None, + Some(self.graphics.camera_mut() as &mut dyn PlanarCamera), + None, + ); + #[cfg(feature = "dim3")] + let result = ( + Some(self.graphics.camera_mut() as &mut dyn Camera), + None, + None, + ); + result + } + + fn step(&mut self, window: &mut Window) { + self.ui + .update(window, &mut self.integration_parameters, &mut self.state); + + // Handle UI actions. + { + let backend_changed = self + .state + .action_flags + .contains(TestbedActionFlags::BACKEND_CHANGED); + if backend_changed { + // Marking the example as changed will make the simulation + // restart with the selected backend. + self.state + .action_flags + .set(TestbedActionFlags::BACKEND_CHANGED, false); + self.state + .action_flags + .set(TestbedActionFlags::EXAMPLE_CHANGED, true); + self.camera_locked = true; + } + + let restarted = self + .state + .action_flags + .contains(TestbedActionFlags::RESTART); + if restarted { + self.state + .action_flags + .set(TestbedActionFlags::RESTART, false); + self.camera_locked = true; + self.state + .action_flags + .set(TestbedActionFlags::EXAMPLE_CHANGED, true); + } + + let example_changed = self + .state + .action_flags + .contains(TestbedActionFlags::EXAMPLE_CHANGED); + if example_changed { + self.state + .action_flags + .set(TestbedActionFlags::EXAMPLE_CHANGED, false); + self.clear(window); + self.builders[self.state.selected_example].1(self); + self.camera_locked = false; + } + + if self + .state + .action_flags + .contains(TestbedActionFlags::TAKE_SNAPSHOT) + { + self.state + .action_flags + .set(TestbedActionFlags::TAKE_SNAPSHOT, false); + self.state.snapshot = PhysicsSnapshot::new( + self.state.timestep_id, + &self.broad_phase, + &self.narrow_phase, + &self.bodies, + &self.colliders, + &self.joints, + ) + .ok(); + + if let Some(snap) = &self.state.snapshot { + snap.print_snapshot_len(); + } + } + + if self + .state + .action_flags + .contains(TestbedActionFlags::RESTORE_SNAPSHOT) + { + self.state + .action_flags + .set(TestbedActionFlags::RESTORE_SNAPSHOT, false); + if let Some(snapshot) = &self.state.snapshot { + if let Ok(w) = snapshot.restore() { + self.clear(window); + self.graphics.clear(window); + self.broad_phase = w.1; + self.narrow_phase = w.2; + self.set_world(w.3, w.4, w.5); + self.state.timestep_id = w.0; + } + } + } + + if self + .state + .action_flags + .contains(TestbedActionFlags::RESET_WORLD_GRAPHICS) + { + self.state + .action_flags + .set(TestbedActionFlags::RESET_WORLD_GRAPHICS, false); + for (handle, _) in self.bodies.iter() { + self.graphics + .add(window, handle, &self.bodies, &self.colliders); + } + + #[cfg(feature = "fluids")] + { + if let Some(fluids) = &self.fluids { + let radius = fluids.world.particle_radius(); + + for (handle, fluid) in fluids.world.fluids().iter() { + self.graphics.add_fluid(window, handle, fluid, radius); + } + + for (handle, boundary) in fluids.world.boundaries().iter() { + self.graphics.add_boundary(window, handle, boundary, radius); + } + } + } + } + + if example_changed + || self.state.prev_flags.contains(TestbedStateFlags::WIREFRAME) + != self.state.flags.contains(TestbedStateFlags::WIREFRAME) + { + self.graphics.toggle_wireframe_mode( + &self.colliders, + self.state.flags.contains(TestbedStateFlags::WIREFRAME), + ) + } + + if self.state.prev_flags.contains(TestbedStateFlags::SLEEP) + != self.state.flags.contains(TestbedStateFlags::SLEEP) + { + if self.state.flags.contains(TestbedStateFlags::SLEEP) { + for (_, mut body) in self.bodies.iter_mut() { + body.activation.threshold = ActivationStatus::default_threshold(); + } + } else { + for (_, mut body) in self.bodies.iter_mut() { + body.wake_up(); + body.activation.threshold = -1.0; + } + } + } + + if self + .state + .prev_flags + .contains(TestbedStateFlags::SUB_STEPPING) + != self.state.flags.contains(TestbedStateFlags::SUB_STEPPING) + { + self.integration_parameters.return_after_ccd_substep = + self.state.flags.contains(TestbedStateFlags::SUB_STEPPING); + } + + if self.state.prev_flags.contains(TestbedStateFlags::SHAPES) + != self.state.flags.contains(TestbedStateFlags::SHAPES) + { + unimplemented!() + } + + if self.state.prev_flags.contains(TestbedStateFlags::JOINTS) + != self.state.flags.contains(TestbedStateFlags::JOINTS) + { + unimplemented!() + } + + if example_changed + || self.state.prev_flags.contains(TestbedStateFlags::AABBS) + != self.state.flags.contains(TestbedStateFlags::AABBS) + { + if self.state.flags.contains(TestbedStateFlags::AABBS) { + // self.graphics + // .show_aabbs(&self.geometrical_world, &self.colliders, window) + } else { + // self.graphics.hide_aabbs(window) + } + } + + if self + .state + .prev_flags + .contains(TestbedStateFlags::CENTER_OF_MASSES) + != self + .state + .flags + .contains(TestbedStateFlags::CENTER_OF_MASSES) + { + unimplemented!() + } + } + + self.state.prev_flags = self.state.flags; + + for event in window.events().iter() { + let event = self.handle_common_event(event); + self.handle_special_event(window, event); + } + + #[cfg(feature = "fluids")] + let mut fluids_time = 0.0; + + if self.state.running != RunMode::Stop { + for _ in 0..self.nsteps { + self.state.timestep_id += 1; + if self.state.selected_backend == RAPIER_BACKEND { + #[cfg(feature = "parallel")] + { + let gravity = &self.gravity; + let params = &self.integration_parameters; + let pipeline = &mut self.physics_pipeline; + let narrow_phase = &mut self.narrow_phase; + let broad_phase = &mut self.broad_phase; + let bodies = &mut self.bodies; + let colliders = &mut self.colliders; + let joints = &mut self.joints; + let event_handler = &self.event_handler; + self.state.thread_pool.install(|| { + pipeline.step( + gravity, + params, + broad_phase, + narrow_phase, + bodies, + colliders, + joints, + event_handler, + ); + }); + } + + #[cfg(not(feature = "parallel"))] + self.physics_pipeline.step( + &self.gravity, + &self.integration_parameters, + &mut self.broad_phase, + &mut self.narrow_phase, + &mut self.bodies, + &mut self.colliders, + &mut self.joints, + &self.event_handler, + ); + + #[cfg(feature = "fluids")] + { + fluids_time = instant::now(); + if let Some(fluids) = &mut self.fluids { + let dt = self.world.timestep(); + let gravity = &self.world.gravity; + fluids.world.step_with_coupling( + dt, + gravity, + &mut fluids + .coupling + .as_manager_mut(&self.colliders, &mut self.bodies), + ); + } + + fluids_time = instant::now() - fluids_time; + } + } + + #[cfg(all(feature = "dim2", feature = "other-backends"))] + { + if self.state.selected_backend == BOX2D_BACKEND { + self.box2d.as_mut().unwrap().step( + &mut self.physics_pipeline.counters, + &self.integration_parameters, + ); + self.box2d + .as_mut() + .unwrap() + .sync(&mut self.bodies, &mut self.colliders); + } + } + + #[cfg(all(feature = "dim3", feature = "other-backends"))] + { + if self.state.selected_backend == PHYSX_BACKEND_PATCH_FRICTION + || self.state.selected_backend == PHYSX_BACKEND_TWO_FRICTION_DIR + { + // println!("Step"); + self.physx.as_mut().unwrap().step( + &mut self.physics_pipeline.counters, + &self.integration_parameters, + ); + self.physx + .as_mut() + .unwrap() + .sync(&mut self.bodies, &mut self.colliders); + } + } + + #[cfg(feature = "other-backends")] + { + if self.state.selected_backend == NPHYSICS_BACKEND { + self.nphysics.as_mut().unwrap().step( + &mut self.physics_pipeline.counters, + &self.integration_parameters, + ); + self.nphysics + .as_mut() + .unwrap() + .sync(&mut self.bodies, &mut self.colliders); + } + } + + for f in &mut self.callbacks { + f( + &mut self.bodies, + &mut self.colliders, + &self.events, + &mut self.graphics, + self.time, + ) + } + + #[cfg(feature = "fluids")] + { + if let Some(fluid_state) = &mut self.fluids { + for f in &mut self.callbacks_fluids { + f( + &mut fluid_state.world, + &mut fluid_state.coupling, + &mut self.world, + &mut self.bodies, + &mut self.graphics, + self.time, + ) + } + } + } + + self.events.poll_all(); + + // if true { + // // !self.hide_counters { + // #[cfg(not(feature = "log"))] + // println!("{}", self.world.counters); + // #[cfg(feature = "log")] + // debug!("{}", self.world.counters); + // } + self.time += self.integration_parameters.dt(); + } + } + + self.graphics.draw(&self.colliders, window); + + #[cfg(feature = "fluids")] + { + if let Some(fluids) = &self.fluids { + self.graphics.draw_fluids(&fluids.world) + } + } + + if self.state.flags.contains(TestbedStateFlags::CONTACT_POINTS) { + draw_contacts(window, &self.narrow_phase, &self.bodies); + } + + if self.state.running == RunMode::Step { + self.state.running = RunMode::Stop; + } + + if self.state.running == RunMode::Quit { + window.close() + } + + let color = Point3::new(0.0, 0.0, 0.0); + let counters = self.physics_pipeline.counters; + let mut profile = String::new(); + + if self.state.flags.contains(TestbedStateFlags::PROFILE) { + profile = format!( + r#"{} +Total: {:.2}ms +Collision detection: {:.2}ms +|_ Broad-phase: {:.2}ms + Narrow-phase: {:.2}ms +Island computation: {:.2}ms +Solver: {:.2}ms +|_ Velocity assembly: {:.2}ms + Velocity resolution: {:.2}ms + Velocity integration: {:.2}ms + Position assembly: {:.2}ms + Position resolution: {:.2}ms +CCD: {:.2}ms +|_ # of substeps: {} + TOI computation: {:.2}ms + Broad-phase: {:.2}ms + Narrow-phase: {:.2}ms + Solver: {:.2}ms"#, + profile, + counters.step_time(), + counters.collision_detection_time(), + counters.broad_phase_time(), + counters.narrow_phase_time(), + counters.island_construction_time(), + counters.solver_time(), + counters.solver.velocity_assembly_time.time(), + counters.velocity_resolution_time(), + counters.solver.velocity_update_time.time(), + counters.solver.position_assembly_time.time(), + counters.position_resolution_time(), + counters.ccd_time(), + counters.ccd.num_substeps, + counters.ccd.toi_computation_time.time(), + counters.ccd.broad_phase_time.time(), + counters.ccd.narrow_phase_time.time(), + counters.ccd.solver_time.time(), + ); + + #[cfg(feature = "fluids")] + { + profile = format!( + r#"{} +Fluids: {:.2}ms + "#, + profile, fluids_time, + ) + } + } + + if self.state.flags.contains(TestbedStateFlags::DEBUG) { + let hash_bf = md5::compute(&bincode::serialize(&self.broad_phase).unwrap()); + let hash_nf = md5::compute(&bincode::serialize(&self.narrow_phase).unwrap()); + let hash_bodies = md5::compute(&bincode::serialize(&self.bodies).unwrap()); + let hash_colliders = md5::compute(&bincode::serialize(&self.colliders).unwrap()); + let hash_joints = md5::compute(&bincode::serialize(&self.joints).unwrap()); + profile = format!( + r#"{} +Hashes at frame: {} +|_ Broad phase: {:?} +|_ Narrow phase: {:?} +|_ Bodies: {:?} +|_ Colliders: {:?} +|_ Joints: {:?}"#, + profile, + self.state.timestep_id, + hash_bf, + hash_nf, + hash_bodies, + hash_colliders, + hash_joints + ); + } + + window.draw_text(&profile, &Point2::origin(), 45.0, &self.font, &color); + } +} + +fn draw_contacts(window: &mut Window, nf: &NarrowPhase, bodies: &RigidBodySet) { + for (_, _, pair) in nf.contact_graph().interaction_pairs() { + for manifold in &pair.manifolds { + for pt in manifold.all_contacts() { + let color = if pt.dist > 0.0 { + Point3::new(0.0, 0.0, 1.0) + } else { + Point3::new(1.0, 0.0, 0.0) + }; + let pos1 = bodies[manifold.body_pair.body1].position; + let pos2 = bodies[manifold.body_pair.body2].position; + let start = pos1 * pt.local_p1; + let end = pos2 * pt.local_p2; + let n = pos1 * manifold.local_n1; + + use crate::engine::GraphicsWindow; + window.draw_graphics_line(&start, &(start + n * 0.4), &Point3::new(0.5, 1.0, 0.5)); + window.draw_graphics_line(&start, &end, &color); + } + } + } +} diff --git a/src_testbed/ui.rs b/src_testbed/ui.rs new file mode 100644 index 000000000..f88db9db9 --- /dev/null +++ b/src_testbed/ui.rs @@ -0,0 +1,568 @@ +use kiss3d::conrod::{self, Borderable, Colorable, Labelable, Positionable, Sizeable, Widget}; +use kiss3d::window::Window; +use rapier::dynamics::IntegrationParameters; + +use crate::testbed::{RunMode, TestbedActionFlags, TestbedState, TestbedStateFlags}; + +const SIDEBAR_W: f64 = 200.0; +const ELEMENT_W: f64 = SIDEBAR_W - 20.0; +const ELEMENT_H: f64 = 20.0; +const VSPACE: f64 = 4.0; +const TITLE_VSPACE: f64 = 4.0; +const LEFT_MARGIN: f64 = 10.0; +const ALPHA: f32 = 0.9; + +widget_ids! { + pub struct ConrodIds { + canvas, + title_backends_list, + title_demos_list, + title_slider_vel_iter, + title_slider_pos_iter, + title_slider_num_threads, + title_slider_ccd_substeps, + title_slider_min_island_size, + title_warmstart_coeff, + title_frequency, + backends_list, + demos_list, + button_pause, + button_single_step, + button_restart, + button_quit, + button_prev_example, + button_next_example, + button_take_snapshot, + button_restore_snapshot, + slider_vel_iter, + slider_pos_iter, + slider_num_threads, + slider_ccd_substeps, + slider_min_island_size, + slider_warmstart_coeff, + slider_frequency, + toggle_sleep, + toggle_warm_starting, + toggle_sub_stepping, + toggle_shapes, + toggle_joints, + toggle_aabbs, + toggle_contact_points, + toggle_contact_normals, + toggle_center_of_masses, + toggle_statistics, + toggle_profile, + toggle_debug, + toggle_wireframe, + separator0, + separator1, + separator2, + } +} + +pub struct TestbedUi { + ids: ConrodIds, +} + +impl TestbedUi { + pub fn new(window: &mut Window) -> Self { + use conrod::position::{Align, Direction, Padding, Position, Relative}; + + let mut ui = window.conrod_ui_mut(); + ui.theme = conrod::Theme { + name: "Testbed theme".to_string(), + padding: Padding::none(), + x_position: Position::Relative(Relative::Align(Align::Start), None), + y_position: Position::Relative(Relative::Direction(Direction::Backwards, 20.0), None), + background_color: conrod::color::DARK_CHARCOAL.alpha(ALPHA), + shape_color: conrod::color::LIGHT_CHARCOAL.alpha(ALPHA), + border_color: conrod::color::BLACK.alpha(ALPHA), + border_width: 0.0, + label_color: conrod::color::WHITE.alpha(ALPHA), + font_id: None, + font_size_large: 15, + font_size_medium: 11, + font_size_small: 8, + widget_styling: conrod::theme::StyleMap::default(), + mouse_drag_threshold: 0.0, + double_click_threshold: std::time::Duration::from_millis(500), + }; + + Self { + ids: ConrodIds::new(ui.widget_id_generator()), + } + } + + pub fn update( + &mut self, + window: &mut Window, + integration_parameters: &mut IntegrationParameters, + state: &mut TestbedState, + ) { + let ui_root = window.conrod_ui().window; + let mut ui = window.conrod_ui_mut().set_widgets(); + conrod::widget::Canvas::new() + // .title_bar("Demos") + // .title_bar_color(conrod::color::Color::Rgba(1.0, 0.0, 0.0, 1.0)) + // .pad(100.0) + // .pad_left(MARGIN) + // .pad_right(MARGIN) + .scroll_kids_vertically() + .mid_right_with_margin(10.0) + .w(SIDEBAR_W) + .padded_h_of(ui_root, 10.0) + .set(self.ids.canvas, &mut ui); + + // NOTE: If examples_names is empty, we can't change the backend because + // we have no way to properly restart the simulation. + if state.backend_names.len() > 1 && !state.example_names.is_empty() { + /* + * Backend drop-down. + */ + conrod::widget::Text::new("Select backend:") + .top_left_with_margins_on(self.ids.canvas, VSPACE, LEFT_MARGIN) + .set(self.ids.title_backends_list, &mut ui); + + for selected in conrod::widget::DropDownList::new( + &state.backend_names, + Some(state.selected_backend), + ) + .align_middle_x_of(self.ids.canvas) + .down_from(self.ids.title_backends_list, TITLE_VSPACE) + .left_justify_label() + .w_h(ELEMENT_W, ELEMENT_H) + .color(conrod::color::LIGHT_CHARCOAL) // No alpha. + .set(self.ids.backends_list, &mut ui) + { + if selected != state.selected_backend { + #[cfg(feature = "dim3")] + fn is_physx(id: usize) -> bool { + id == crate::testbed::PHYSX_BACKEND_PATCH_FRICTION + || id == crate::testbed::PHYSX_BACKEND_TWO_FRICTION_DIR + } + + #[cfg(all(feature = "dim3", feature = "other-backends"))] + if (is_physx(state.selected_backend) && !is_physx(selected)) + || (!is_physx(state.selected_backend) && is_physx(selected)) + { + // PhysX defaults (4 position iterations, 1 velocity) are the + // opposite of rapier's (4 velocity iterations, 1 position). + std::mem::swap( + &mut integration_parameters.max_position_iterations, + &mut integration_parameters.max_velocity_iterations, + ); + } + + state.selected_backend = selected; + state + .action_flags + .set(TestbedActionFlags::BACKEND_CHANGED, true) + } + } + + separator( + self.ids.canvas, + self.ids.backends_list, + self.ids.separator0, + &mut ui, + ); + } else { + conrod::widget::Text::new("") + .top_left_with_margins_on(self.ids.canvas, 0.0, LEFT_MARGIN) + .set(self.ids.separator0, &mut ui); + } + + let display_ticks = state.example_names.len() > 1; + let _select_example_title = if display_ticks { + "Select example:" + } else { + "Current example:" + }; + let tick_width = if display_ticks { 20.0 } else { 0.0 }; + + /* + * Examples drop-down. + */ + conrod::widget::Text::new("Select example:") + .down_from(self.ids.separator0, VSPACE) + // .top_left_with_margins_on(self.ids.canvas, VSPACE, LEFT_MARGIN) + // .w_h(ELEMENT_W, ELEMENT_H) + .set(self.ids.title_demos_list, &mut ui); + + for selected in + conrod::widget::DropDownList::new(&state.example_names, Some(state.selected_example)) + // .mid_top_with_margin_on(self.ids.canvas, 20.0) + .align_middle_x_of(self.ids.canvas) + .down_from(self.ids.title_demos_list, TITLE_VSPACE) + // .right_from(self.ids.button_prev_example, 0.0) + .left_justify_label() + .w_h(ELEMENT_W - tick_width, ELEMENT_H) + .color(conrod::color::LIGHT_CHARCOAL) // No alpha. + .set(self.ids.demos_list, &mut ui) + { + if selected != state.selected_example { + state.selected_example = selected; + state + .action_flags + .set(TestbedActionFlags::EXAMPLE_CHANGED, true) + } + } + + if display_ticks { + for _click in conrod::widget::Button::new() + .label("<") + .align_middle_x_of(self.ids.canvas) + .left_from(self.ids.demos_list, 0.0) + .w_h(10.0, ELEMENT_H) + .enabled(state.selected_example > 0) + .color(conrod::color::LIGHT_CHARCOAL) // No alpha. + .set(self.ids.button_prev_example, &mut ui) + { + if state.selected_example > 0 { + state.selected_example -= 1; + state + .action_flags + .set(TestbedActionFlags::EXAMPLE_CHANGED, true) + } + } + + for _click in conrod::widget::Button::new() + .label(">") + .align_middle_x_of(self.ids.canvas) + .right_from(self.ids.demos_list, 0.0) + .w_h(10.0, ELEMENT_H) + .enabled(state.selected_example + 1 < state.example_names.len()) + .color(conrod::color::LIGHT_CHARCOAL) // No alpha. + .set(self.ids.button_next_example, &mut ui) + { + if state.selected_example + 1 < state.example_names.len() { + state.selected_example += 1; + state + .action_flags + .set(TestbedActionFlags::EXAMPLE_CHANGED, true) + } + } + } + + separator( + self.ids.canvas, + self.ids.demos_list, + self.ids.separator1, + &mut ui, + ); + + let curr_vel_iters = integration_parameters.max_velocity_iterations; + let curr_pos_iters = integration_parameters.max_position_iterations; + #[cfg(feature = "parallel")] + let curr_num_threads = state.num_threads; + let curr_max_ccd_substeps = integration_parameters.max_ccd_substeps; + let curr_min_island_size = integration_parameters.min_island_size; + let curr_warmstart_coeff = integration_parameters.warmstart_coeff; + let curr_frequency = integration_parameters.inv_dt().round() as usize; + + conrod::widget::Text::new("Vel. Iters.:") + .down_from(self.ids.separator1, VSPACE) + .set(self.ids.title_slider_vel_iter, &mut ui); + + for val in conrod::widget::Slider::new(curr_vel_iters as f32, 0.0, 200.0) + .label(&curr_vel_iters.to_string()) + .align_middle_x_of(self.ids.canvas) + .down_from(self.ids.title_slider_vel_iter, TITLE_VSPACE) + .w_h(ELEMENT_W, ELEMENT_H) + .set(self.ids.slider_vel_iter, &mut ui) + { + integration_parameters.max_velocity_iterations = val as usize; + } + + conrod::widget::Text::new("Pos. Iters.:") + .down_from(self.ids.slider_vel_iter, VSPACE) + .set(self.ids.title_slider_pos_iter, &mut ui); + + for val in conrod::widget::Slider::new(curr_pos_iters as f32, 0.0, 200.0) + .label(&curr_pos_iters.to_string()) + .align_middle_x_of(self.ids.canvas) + .down_from(self.ids.title_slider_pos_iter, TITLE_VSPACE) + .w_h(ELEMENT_W, ELEMENT_H) + .set(self.ids.slider_pos_iter, &mut ui) + { + integration_parameters.max_position_iterations = val as usize; + } + + #[cfg(feature = "parallel")] + { + conrod::widget::Text::new("Num. Threads.:") + .down_from(self.ids.slider_pos_iter, VSPACE) + .set(self.ids.title_slider_num_threads, &mut ui); + + for val in conrod::widget::Slider::new( + curr_num_threads as f32, + 1.0, + num_cpus::get_physical() as f32, + ) + .label(&curr_num_threads.to_string()) + .align_middle_x_of(self.ids.canvas) + .down_from(self.ids.title_slider_num_threads, TITLE_VSPACE) + .w_h(ELEMENT_W, ELEMENT_H) + .set(self.ids.slider_num_threads, &mut ui) + { + if state.num_threads != val as usize { + state.num_threads = val as usize; + state.thread_pool = rapier::rayon::ThreadPoolBuilder::new() + .num_threads(state.num_threads) + .build() + .unwrap(); + } + } + } + + conrod::widget::Text::new("CCD substeps:") + .down_from( + if cfg!(feature = "parallel") { + self.ids.slider_num_threads + } else { + self.ids.slider_pos_iter + }, + VSPACE, + ) + .set(self.ids.title_slider_ccd_substeps, &mut ui); + + for val in conrod::widget::Slider::new(curr_max_ccd_substeps as f32, 0.0, 10.0) + .label(&curr_max_ccd_substeps.to_string()) + .align_middle_x_of(self.ids.canvas) + .down_from(self.ids.title_slider_ccd_substeps, TITLE_VSPACE) + .w_h(ELEMENT_W, ELEMENT_H) + .set(self.ids.slider_ccd_substeps, &mut ui) + { + integration_parameters.max_ccd_substeps = val as usize; + } + + conrod::widget::Text::new("Min island size:") + .down_from(self.ids.slider_ccd_substeps, VSPACE) + .set(self.ids.title_slider_min_island_size, &mut ui); + + for val in conrod::widget::Slider::new(curr_min_island_size as f32, 1.0, 10000.0) + .label(&curr_min_island_size.to_string()) + .align_middle_x_of(self.ids.canvas) + .down_from(self.ids.title_slider_min_island_size, TITLE_VSPACE) + .w_h(ELEMENT_W, ELEMENT_H) + .set(self.ids.slider_min_island_size, &mut ui) + { + integration_parameters.min_island_size = val as usize; + } + + conrod::widget::Text::new("Warm-start coeff.:") + .down_from(self.ids.slider_min_island_size, VSPACE) + .set(self.ids.title_warmstart_coeff, &mut ui); + + for val in conrod::widget::Slider::new(curr_warmstart_coeff as f32, 0.0, 1.0) + .label(&format!("{:.2}", curr_warmstart_coeff)) + .align_middle_x_of(self.ids.canvas) + .down_from(self.ids.title_warmstart_coeff, TITLE_VSPACE) + .w_h(ELEMENT_W, ELEMENT_H) + .set(self.ids.slider_warmstart_coeff, &mut ui) + { + integration_parameters.warmstart_coeff = val; + } + + conrod::widget::Text::new("Frequency:") + .down_from(self.ids.slider_warmstart_coeff, VSPACE) + .set(self.ids.title_frequency, &mut ui); + + for val in conrod::widget::Slider::new(curr_frequency as f32, 0.0, 240.0) + .label(&format!("{:.2}Hz", curr_frequency)) + .align_middle_x_of(self.ids.canvas) + .down_from(self.ids.title_frequency, TITLE_VSPACE) + .w_h(ELEMENT_W, ELEMENT_H) + .set(self.ids.slider_frequency, &mut ui) + { + integration_parameters.set_inv_dt(val.round()); + } + + let toggle_list = [ + ("Sleep", self.ids.toggle_sleep, TestbedStateFlags::SLEEP), + // ("Warm Starting", self.ids.toggle_warm_starting, TestbedStateFlags::WARM_STARTING), + ( + "Sub-Stepping", + self.ids.toggle_sub_stepping, + TestbedStateFlags::SUB_STEPPING, + ), + ("", self.ids.separator2, TestbedStateFlags::NONE), + // ("Shapes", self.ids.toggle_shapes, TestbedStateFlags::SHAPES), + // ("Joints", self.ids.toggle_joints, TestbedStateFlags::JOINTS), + ("AABBs", self.ids.toggle_aabbs, TestbedStateFlags::AABBS), + ( + "Contacts", + self.ids.toggle_contact_points, + TestbedStateFlags::CONTACT_POINTS, + ), + // ("ContactManifold Normals", self.ids.toggle_contact_normals, TestbedStateFlags::CONTACT_NORMALS), + ( + "Wireframe", + self.ids.toggle_wireframe, + TestbedStateFlags::WIREFRAME, + ), + // ("Center of Masses", self.ids.toggle_center_of_masses, TestbedStateFlags::CENTER_OF_MASSES), + // ("Statistics", self.ids.toggle_statistics, TestbedStateFlags::STATISTICS), + ( + "Profile", + self.ids.toggle_profile, + TestbedStateFlags::PROFILE, + ), + ( + "Debug infos", + self.ids.toggle_debug, + TestbedStateFlags::DEBUG, + ), + ]; + + toggles( + &toggle_list, + self.ids.canvas, + self.ids.slider_frequency, + &mut ui, + &mut state.flags, + ); + + let label = if state.running == RunMode::Stop { + "Start (T)" + } else { + "Pause (T)" + }; + for _press in conrod::widget::Button::new() + .label(label) + .align_middle_x_of(self.ids.canvas) + .down_from(self.ids.toggle_debug, VSPACE) + .w_h(ELEMENT_W, ELEMENT_H) + .set(self.ids.button_pause, &mut ui) + { + if state.running == RunMode::Stop { + state.running = RunMode::Running + } else { + state.running = RunMode::Stop + } + } + + for _press in conrod::widget::Button::new() + .label("Single Step (S)") + .align_middle_x_of(self.ids.canvas) + .down_from(self.ids.button_pause, VSPACE) + .set(self.ids.button_single_step, &mut ui) + { + state.running = RunMode::Step + } + + for _press in conrod::widget::Button::new() + .label("Take snapshot") + .align_middle_x_of(self.ids.canvas) + .down_from(self.ids.button_single_step, VSPACE) + .set(self.ids.button_take_snapshot, &mut ui) + { + state + .action_flags + .set(TestbedActionFlags::TAKE_SNAPSHOT, true); + } + + for _press in conrod::widget::Button::new() + .label("Restore snapshot") + .align_middle_x_of(self.ids.canvas) + .down_from(self.ids.button_take_snapshot, VSPACE) + .set(self.ids.button_restore_snapshot, &mut ui) + { + state + .action_flags + .set(TestbedActionFlags::RESTORE_SNAPSHOT, true); + } + + let before_quit_button_id = if !state.example_names.is_empty() { + for _press in conrod::widget::Button::new() + .label("Restart (R)") + .align_middle_x_of(self.ids.canvas) + .down_from(self.ids.button_restore_snapshot, VSPACE) + .set(self.ids.button_restart, &mut ui) + { + state.action_flags.set(TestbedActionFlags::RESTART, true); + } + + self.ids.button_restart + } else { + self.ids.button_restore_snapshot + }; + + #[cfg(not(target_arch = "wasm32"))] + for _press in conrod::widget::Button::new() + .label("Quit (Esc)") + .align_middle_x_of(self.ids.canvas) + .down_from(before_quit_button_id, VSPACE) + .set(self.ids.button_quit, &mut ui) + { + state.running = RunMode::Quit + } + } +} + +fn toggles( + toggles: &[(&str, conrod::widget::Id, TestbedStateFlags)], + canvas: conrod::widget::Id, + prev: conrod::widget::Id, + ui: &mut conrod::UiCell, + flags: &mut TestbedStateFlags, +) { + toggle( + toggles[0].0, + toggles[0].2, + canvas, + prev, + toggles[0].1, + ui, + flags, + ); + + for win in toggles.windows(2) { + toggle(win[1].0, win[1].2, canvas, win[0].1, win[1].1, ui, flags) + } +} + +fn toggle( + title: &str, + flag: TestbedStateFlags, + canvas: conrod::widget::Id, + prev: conrod::widget::Id, + curr: conrod::widget::Id, + ui: &mut conrod::UiCell, + flags: &mut TestbedStateFlags, +) { + if title == "" { + // This is a separator. + separator(canvas, prev, curr, ui) + } else { + for _pressed in conrod::widget::Toggle::new(flags.contains(flag)) + .mid_left_with_margin_on(canvas, LEFT_MARGIN) + .down_from(prev, VSPACE) + .w_h(20.0 /*ELEMENT_W*/, ELEMENT_H) + .label(title) + .label_color(kiss3d::conrod::color::WHITE) + .label_x(conrod::position::Relative::Direction( + conrod::position::Direction::Forwards, + 5.0, + )) + .border(2.0) + // .border_color(kiss3d::conrod::color::WHITE) + .set(curr, ui) + { + flags.toggle(flag) + } + } +} + +fn separator( + canvas: conrod::widget::Id, + prev: conrod::widget::Id, + curr: conrod::widget::Id, + ui: &mut conrod::UiCell, +) { + conrod::widget::Line::centred([-ELEMENT_W / 2.0, 0.0], [ELEMENT_W / 2.0, 0.0]) + .align_middle_x_of(canvas) + .down_from(prev, VSPACE) + .w(ELEMENT_W) + .set(curr, ui); +}