├── .editorconfig ├── .gitignore ├── .travis.yml ├── CHANGELOG.md ├── Cargo.toml ├── LICENSE-APACHE ├── LICENSE-MIT ├── README.md ├── bors.toml ├── rhusics-core ├── Cargo.toml └── src │ ├── body_pose.rs │ ├── collide │ ├── broad.rs │ ├── mod.rs │ └── narrow.rs │ ├── collide2d.rs │ ├── collide3d.rs │ ├── ecs.rs │ ├── lib.rs │ ├── physics │ ├── force.rs │ ├── mass.rs │ ├── mod.rs │ ├── resolution.rs │ ├── simple.rs │ ├── util.rs │ ├── velocity.rs │ └── volumes.rs │ ├── physics2d.rs │ └── physics3d.rs ├── rhusics-ecs ├── Cargo.toml ├── examples │ ├── basic2d.rs │ ├── basic3d.rs │ ├── spatial2d.rs │ └── spatial3d.rs └── src │ ├── collide │ ├── mod.rs │ └── systems │ │ ├── basic.rs │ │ ├── mod.rs │ │ ├── spatial_collision.rs │ │ └── spatial_sort.rs │ ├── collide2d.rs │ ├── collide3d.rs │ ├── lib.rs │ ├── physics │ ├── mod.rs │ ├── resources.rs │ ├── setup.rs │ └── systems │ │ ├── contact_resolution.rs │ │ ├── current_frame.rs │ │ ├── mod.rs │ │ └── next_frame.rs │ ├── physics2d.rs │ └── physics3d.rs ├── rhusics-transform ├── Cargo.toml └── src │ └── lib.rs └── rustfmt.toml /.editorconfig: -------------------------------------------------------------------------------- 1 | root = true 2 | 3 | [*] 4 | end_of_line = lf 5 | charset = utf-8 6 | trim_trailing_whitespace = true 7 | insert_final_newline = true 8 | indent_style = space 9 | indent_size = 4 10 | 11 | [*.md] 12 | trim_trailing_whitespace = false -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | /target/ 2 | **/*.rs.bk 3 | Cargo.lock 4 | .idea/ 5 | *.iml 6 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: false 2 | language: rust 3 | rust: 4 | - nightly 5 | - beta 6 | - stable 7 | branches: 8 | only: 9 | # This is where pull requests from "bors r+" are built. 10 | - staging 11 | # This is where pull requests from "bors try" are built. 12 | - trying 13 | # Uncomment this to enable building pull requests. 14 | - master 15 | matrix: 16 | allow_failures: 17 | - rust: nightly 18 | script: 19 | - | 20 | cargo build --all && 21 | cargo test --all && 22 | cargo test --all --all-features 23 | cache: cargo 24 | -------------------------------------------------------------------------------- /CHANGELOG.md: -------------------------------------------------------------------------------- 1 | ## Changelog 2 | 3 | ### v0.9 [0.5 for rhusics-transform] 4 | - updated shred to 0.10. 5 | - updated specs to 0.16, modified to new API ("World" instead of "Resources"). 6 | - updated boxed class refs to include dyn, as requested by compiler. 7 | - fixed bug in toml files where was always using serde. 8 | - BREAKING CHANGE: use "serializable" instead of "serde" to get serialization. 9 | Using just "serde" won't compile any more. 10 | 11 | ### v0.8 [0.5 for rhusics-transform] 12 | - Update cgmath to 0.17 13 | - Update collision to 0.20 14 | - Update approx to 0.3 15 | - No API changes, but recompilation necessary as supporting traits changed. 16 | 17 | ### v0.7 18 | 19 | - Update specs version to 0.14 20 | 21 | ### v0.4 22 | 23 | - Update to new version of collision: 24 | * BREAKING CHANGE: Primitive2 and Primitive3 have new variants 25 | * BREAKING CHANGE: Signature change of `GJK::intersect` 26 | - refactor: Rename feature `eders` to `serde` 27 | - fix: Crashed if a collision was detected, but the Entity was removed before collision resolution 28 | - feat: Implement Pose for cgmath::Decomposed 29 | - feat: Abstract the time ECS resource, to make it pluggable, BREAKING CHANGE 30 | - refactor: Adapt ECS integration to new Specs, BREAKING CHANGE 31 | 32 | ### v0.3 33 | 34 | - Core code is now generic wrt transform type 35 | - Trait required to implement for the transform type is broken out to a separate crate `rhusics-transform` 36 | 37 | ### v0.2 38 | 39 | - Update to new version of collision 40 | * BREAKING CHANGE: Primitive2 and Primitive3 have new variants 41 | -------------------------------------------------------------------------------- /Cargo.toml: -------------------------------------------------------------------------------- 1 | [workspace] 2 | members = ["rhusics-transform", "rhusics-core", "rhusics-ecs"] 3 | -------------------------------------------------------------------------------- /LICENSE-APACHE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /LICENSE-MIT: -------------------------------------------------------------------------------- 1 | Copyright (c) 2017 Rhusics developers 2 | 3 | Permission is hereby granted, free of charge, to any 4 | person obtaining a copy of this software and associated 5 | documentation files (the "Software"), to deal in the 6 | Software without restriction, including without 7 | limitation the rights to use, copy, modify, merge, 8 | publish, distribute, sublicense, and/or sell copies of 9 | the Software, and to permit persons to whom the Software 10 | is furnished to do so, subject to the following 11 | conditions: 12 | 13 | The above copyright notice and this permission notice 14 | shall be included in all copies or substantial portions 15 | of the Software. 16 | 17 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF 18 | ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED 19 | TO THE WARRANTIES OF MERCHANTABILITY, FITNESS FOR A 20 | PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT 21 | SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY 22 | CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION 23 | OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR 24 | IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER 25 | DEALINGS IN THE SOFTWARE. 26 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | Physics library for use in Specs, using cgmath and collision-rs. 2 | 3 | # Features: 4 | 5 | * Has support for all primitives in collision-rs 6 | * Has support for the following broad phase algorithms in collision-rs: 7 | * Brute force 8 | * Sweep and Prune 9 | * Narrow phase collision detection using GJK, and optionally EPA for full contact information 10 | * [`specs::System`](https://docs.rs/specs/0.9.5/specs/trait.System.html) for collision 11 | detection working on user supplied transform, and shape components. 12 | Can optionally use broad and/or narrow phase detection. 13 | Library supplies a transform implementation for convenience. 14 | * [`specs::System`](https://docs.rs/specs/0.9.5/specs/trait.System.html) for spatial 15 | sorting on user supplied transform, and shape components. 16 | * Has support for doing spatial sort/collision detection using the collision-rs DBVT. 17 | * Support for doing broad phase using the collision-rs DBVT. 18 | * Continuous collision detection, using GJK 19 | * Simple rigid body implementation with single contact forward resolution 20 | 21 | # TODO: 22 | 23 | * Impulse solver 24 | * Integrator implementations (Euler, RK4, etc.) 25 | * Parallel solver implementation 26 | 27 | ## License 28 | 29 | Licensed under either of 30 | 31 | * Apache License, Version 2.0, ([LICENSE-APACHE](LICENSE-APACHE) or http://www.apache.org/licenses/LICENSE-2.0) 32 | * MIT license ([LICENSE-MIT](LICENSE-MIT) or http://opensource.org/licenses/MIT) 33 | 34 | at your option. 35 | 36 | ### Contribution 37 | 38 | We are a community project that welcomes contribution from anyone. If you're interested in helping out, you can contact 39 | us either through GitHub, or via [`gitter`](https://gitter.im/collision-rs/Lobby). 40 | 41 | Unless you explicitly state otherwise, any contribution intentionally submitted 42 | for inclusion in the work by you, as defined in the Apache-2.0 license, shall be dual licensed as above, without any 43 | additional terms or conditions. 44 | -------------------------------------------------------------------------------- /bors.toml: -------------------------------------------------------------------------------- 1 | status = [ 2 | ] 3 | -------------------------------------------------------------------------------- /rhusics-core/Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "rhusics-core" 3 | version = "0.9.0" 4 | authors = [ 5 | "Simon Rönnberg ", 6 | "Thomas O'Dell " 7 | ] 8 | repository = "https://github.com/rustgd/rhusics.git" 9 | homepage = "https://github.com/rustgd/rhusics.git" 10 | 11 | license = "MIT OR Apache-2.0" 12 | documentation = "https://docs.rs/rhusics-core" 13 | description = "Physics library for use with `specs`" 14 | 15 | keywords = ["gamedev", "cgmath", "specs", "physics"] 16 | 17 | [features] 18 | serializable = ["serde", "cgmath/serde", "collision/serde"] 19 | 20 | [dependencies] 21 | cgmath = "0.17" 22 | collision = "0.20" 23 | rhusics-transform = { version = "0.5.0", path = "../rhusics-transform" } 24 | specs = { version = "0.16", optional = true } 25 | serde = { version = "1.0", optional = true, features = ["derive"]} 26 | 27 | [dev-dependencies] 28 | approx = "0.3" 29 | serde_json = "1.0" 30 | -------------------------------------------------------------------------------- /rhusics-core/src/body_pose.rs: -------------------------------------------------------------------------------- 1 | use cgmath::{BaseFloat, EuclideanSpace, InnerSpace, One, Rotation, Transform, VectorSpace}; 2 | use collision::{Interpolate, TranslationInterpolate}; 3 | 4 | use Pose; 5 | 6 | /// Transform component used throughout the library 7 | #[derive(Clone, Debug, PartialEq)] 8 | #[cfg_attr(target_feature = "serializable", derive(Serialize, Deserialize))] 9 | pub struct BodyPose { 10 | dirty: bool, 11 | position: P, 12 | rotation: R, 13 | inverse_rotation: R, 14 | } 15 | 16 | impl Pose for BodyPose 17 | where 18 | P: EuclideanSpace, 19 | P::Scalar: BaseFloat, 20 | R: Rotation

, 21 | { 22 | /// Create a new [`BodyPose`](struct.BodyPose.html) with initial state given by the supplied 23 | /// position and rotation. 24 | fn new(position: P, rotation: R) -> Self { 25 | Self { 26 | dirty: true, 27 | position, 28 | inverse_rotation: rotation.invert(), 29 | rotation, 30 | } 31 | } 32 | 33 | /// Set the rotation. Will also compute the inverse rotation. Sets the dirty flag. 34 | fn set_rotation(&mut self, rotation: R) { 35 | self.rotation = rotation; 36 | self.inverse_rotation = self.rotation.invert(); 37 | self.dirty = true; 38 | } 39 | 40 | /// Set the position. Sets the dirty flag. 41 | fn set_position(&mut self, position: P) { 42 | self.position = position; 43 | self.dirty = true; 44 | } 45 | 46 | /// Borrows the rotation attribute 47 | fn rotation(&self) -> R { 48 | self.rotation 49 | } 50 | 51 | /// Borrows the position attribute 52 | fn position(&self) -> P { 53 | self.position 54 | } 55 | } 56 | 57 | impl BodyPose 58 | where 59 | P: EuclideanSpace, 60 | P::Scalar: BaseFloat, 61 | R: Rotation

, 62 | { 63 | /// Clear the dirty flag 64 | pub fn clear(&mut self) { 65 | self.dirty = false; 66 | } 67 | } 68 | 69 | impl Transform

for BodyPose 70 | where 71 | P: EuclideanSpace, 72 | P::Scalar: BaseFloat, 73 | R: Rotation

, 74 | { 75 | fn one() -> Self { 76 | Self::new(P::origin(), R::one()) 77 | } 78 | 79 | fn look_at(eye: P, center: P, up: P::Diff) -> Self { 80 | let rot = R::look_at(center - eye, up); 81 | let disp = rot.rotate_vector(P::origin() - eye); 82 | Self::new(P::from_vec(disp), rot) 83 | } 84 | 85 | fn transform_vector(&self, vec: P::Diff) -> P::Diff { 86 | self.rotation.rotate_vector(vec) 87 | } 88 | 89 | fn inverse_transform_vector(&self, vec: P::Diff) -> Option { 90 | Some(self.inverse_rotation.rotate_vector(vec)) 91 | } 92 | 93 | fn transform_point(&self, point: P) -> P { 94 | self.rotation.rotate_point(point) + self.position.to_vec() 95 | } 96 | 97 | fn concat(&self, other: &Self) -> Self { 98 | Self::new( 99 | self.position + self.rotation.rotate_point(other.position).to_vec(), 100 | self.rotation * other.rotation, 101 | ) 102 | } 103 | 104 | fn inverse_transform(&self) -> Option { 105 | Some(Self::new( 106 | self.rotation.rotate_point(self.position) * -P::Scalar::one(), 107 | self.inverse_rotation, 108 | )) 109 | } 110 | } 111 | 112 | impl TranslationInterpolate for BodyPose 113 | where 114 | P: EuclideanSpace, 115 | P::Scalar: BaseFloat, 116 | P::Diff: VectorSpace + InnerSpace, 117 | R: Rotation

+ Clone, 118 | { 119 | fn translation_interpolate(&self, other: &Self, amount: P::Scalar) -> Self { 120 | BodyPose::new( 121 | P::from_vec(self.position.to_vec().lerp(other.position.to_vec(), amount)), 122 | other.rotation, 123 | ) 124 | } 125 | } 126 | 127 | impl Interpolate for BodyPose 128 | where 129 | P: EuclideanSpace, 130 | P::Scalar: BaseFloat, 131 | P::Diff: VectorSpace + InnerSpace, 132 | R: Rotation

+ Interpolate, 133 | { 134 | fn interpolate(&self, other: &Self, amount: P::Scalar) -> Self { 135 | BodyPose::new( 136 | P::from_vec(self.position.to_vec().lerp(other.position.to_vec(), amount)), 137 | self.rotation.interpolate(&other.rotation, amount), 138 | ) 139 | } 140 | } 141 | -------------------------------------------------------------------------------- /rhusics-core/src/collide/broad.rs: -------------------------------------------------------------------------------- 1 | //! Broad phase 2 | 3 | pub use collision::algorithm::broad_phase::*; 4 | 5 | use collision::prelude::*; 6 | 7 | use super::{CollisionData, GetId}; 8 | 9 | /// Broad phase 10 | /// 11 | /// ### Type parameters: 12 | /// 13 | /// - `A`: Shape type, must be able to return its bounding volume 14 | pub trait BroadPhase: Send 15 | where 16 | A: HasBound, 17 | { 18 | /// Compute potential collider pairs 19 | fn find_potentials(&mut self, shapes: &mut [A]) -> Vec<(usize, usize)>; 20 | } 21 | 22 | impl BroadPhase for BruteForce 23 | where 24 | A: HasBound, 25 | A::Bound: Discrete, 26 | { 27 | fn find_potentials(&mut self, shapes: &mut [A]) -> Vec<(usize, usize)> { 28 | self.find_collider_pairs(shapes) 29 | } 30 | } 31 | 32 | impl BroadPhase for SweepAndPrune 33 | where 34 | A: HasBound, 35 | A::Bound: Discrete, 36 | V: Variance + Send, 37 | { 38 | fn find_potentials(&mut self, shapes: &mut [A]) -> Vec<(usize, usize)> { 39 | self.find_collider_pairs(shapes) 40 | } 41 | } 42 | 43 | /// Perform broad phase collision detection on the given data, with the given broad phase 44 | /// Will return a list of ids provided by `GetId` from the broad phase data from `C` 45 | /// 46 | /// ### Type parameters: 47 | /// 48 | /// - `C`: Collision data 49 | /// - `I`: Id, returned by `GetId` on `D`, primary id for a collider 50 | /// - `P`: Primitive 51 | /// - `T`: Transform 52 | /// - `B`: Bounding volume 53 | /// - `Y`: Collider, see `Collider` for more information 54 | /// - `D`: Broad phase data 55 | pub fn broad_collide(data: &C, broad: &mut Box>) -> Vec<(I, I)> 56 | where 57 | C: CollisionData, 58 | P: Primitive, 59 | D: HasBound + GetId, 60 | B: Bound, 61 | { 62 | let mut info = data.get_broad_data(); 63 | broad 64 | .find_potentials(&mut info) 65 | .iter() 66 | .map(|&(a, b)| (info[a].id(), info[b].id())) 67 | .collect::>() 68 | } 69 | -------------------------------------------------------------------------------- /rhusics-core/src/collide/mod.rs: -------------------------------------------------------------------------------- 1 | //! Generic data structures and algorithms for collision detection 2 | 3 | pub use collision::prelude::Primitive; 4 | pub use collision::{CollisionStrategy, ComputeBound, Contact}; 5 | 6 | pub mod broad; 7 | pub mod narrow; 8 | 9 | use std::collections::HashSet; 10 | use std::fmt::Debug; 11 | use std::hash::Hash; 12 | 13 | use cgmath::prelude::*; 14 | use collision::dbvt::{DynamicBoundingVolumeTree, TreeValue, TreeValueWrapped}; 15 | use collision::prelude::*; 16 | 17 | use self::broad::{broad_collide, BroadPhase}; 18 | use self::narrow::{narrow_collide, NarrowPhase}; 19 | 20 | /// Used to check if two shapes should be checked for collisions 21 | pub trait Collider { 22 | /// Should shapes generate contact events 23 | fn should_generate_contacts(&self, other: &Self) -> bool; 24 | } 25 | 26 | impl<'a> Collider for () { 27 | fn should_generate_contacts(&self, _: &Self) -> bool { 28 | true 29 | } 30 | } 31 | 32 | /// Control continuous mode for shapes 33 | #[derive(Debug, Clone, PartialOrd, PartialEq)] 34 | #[cfg_attr(feature = "serializable", derive(Serialize, Deserialize))] 35 | pub enum CollisionMode { 36 | /// Discrete collision mode 37 | Discrete, 38 | 39 | /// Continuous collision mode 40 | Continuous, 41 | } 42 | 43 | /// Contains all contact information for a single contact, together with IDs of the colliding bodies 44 | /// 45 | /// # Type parameters 46 | /// 47 | /// - `ID`: The ID type of the body. This is supplied by the user of the library. In the ECS case, 48 | /// this will be [`Entity`](https://docs.rs/specs/0.9.5/specs/struct.Entity.html). 49 | /// - `V`: cgmath vector type 50 | #[derive(Debug, Clone)] 51 | pub struct ContactEvent 52 | where 53 | P: EuclideanSpace, 54 | P::Diff: Debug, 55 | { 56 | /// The ids of the two colliding bodies 57 | pub bodies: (ID, ID), 58 | 59 | /// The contact between the colliding bodies 60 | pub contact: Contact

, 61 | } 62 | 63 | impl ContactEvent 64 | where 65 | ID: Clone + Debug, 66 | P: EuclideanSpace, 67 | P::Diff: VectorSpace + Zero + Debug, 68 | { 69 | /// Create a new contact event 70 | pub fn new(bodies: (ID, ID), contact: Contact

) -> Self { 71 | Self { bodies, contact } 72 | } 73 | 74 | /// Convenience function to create a contact set with a simple [`Contact`](struct.Contact.html). 75 | pub fn new_simple(strategy: CollisionStrategy, bodies: (ID, ID)) -> Self { 76 | Self::new(bodies, Contact::new(strategy)) 77 | } 78 | } 79 | 80 | /// Collision shape describing a complete collision object in the collision world. 81 | /// 82 | /// Can handle both convex shapes, and concave shapes, by subdividing the concave shapes into 83 | /// multiple convex shapes. This task is up to the user of the library to perform, no subdivision is 84 | /// done automatically in the library. 85 | /// 86 | /// Contains cached information about the base bounding box containing all primitives, 87 | /// in model space coordinates. Also contains a cached version of the transformed bounding box, 88 | /// in world space coordinates. 89 | /// 90 | /// Also have details about what collision strategy/mode to use for contact resolution with this 91 | /// shape. 92 | /// 93 | /// ### Type parameters: 94 | /// 95 | /// - `P`: Primitive type 96 | /// - `T`: Transform type 97 | /// - `B`: Bounding volume type 98 | /// - `Y`: Shape type (see `Collider`) 99 | #[derive(Debug, Clone)] 100 | #[cfg_attr(feature = "serializable", derive(Serialize, Deserialize))] 101 | pub struct CollisionShape 102 | where 103 | P: Primitive, 104 | { 105 | /// Enable/Disable collision detection for this shape 106 | pub enabled: bool, 107 | base_bound: B, 108 | transformed_bound: B, 109 | primitives: Vec<(P, T)>, 110 | strategy: CollisionStrategy, 111 | mode: CollisionMode, 112 | ty: Y, 113 | } 114 | 115 | impl CollisionShape 116 | where 117 | P: Primitive + ComputeBound, 118 | B: Bound + Union + Clone, 119 | T: Transform, 120 | Y: Default, 121 | { 122 | /// Create a new collision shape, with multiple collision primitives. 123 | /// 124 | /// Will compute and cache the base bounding box that contains all the given primitives, 125 | /// in model space coordinates. 126 | /// 127 | /// # Parameters 128 | /// 129 | /// - `strategy`: The collision strategy to use for this shape. 130 | /// - `primitives`: List of all primitives that make up this shape. 131 | /// - `ty`: The shape type, use () if not needed 132 | pub fn new_complex( 133 | strategy: CollisionStrategy, 134 | mode: CollisionMode, 135 | primitives: Vec<(P, T)>, 136 | ty: Y, 137 | ) -> Self { 138 | let bound: B = get_bound(&primitives); 139 | Self { 140 | base_bound: bound.clone(), 141 | primitives, 142 | enabled: true, 143 | transformed_bound: bound, 144 | strategy, 145 | mode, 146 | ty, 147 | } 148 | } 149 | 150 | /// Convenience function to create a simple collision shape with only a single given primitive, 151 | /// with no local-to-model transform. 152 | /// 153 | /// # Parameters 154 | /// 155 | /// - `strategy`: The collision strategy to use for this shape. 156 | /// - `primitive`: The collision primitive. 157 | pub fn new_simple(strategy: CollisionStrategy, mode: CollisionMode, primitive: P) -> Self { 158 | Self::new_complex( 159 | strategy, 160 | mode, 161 | vec![(primitive, T::one())], 162 | Default::default(), 163 | ) 164 | } 165 | 166 | /// Convenience function to create a simple collision shape with only a single given primitive, 167 | /// and a shape type, with no local-to-model transform. 168 | /// 169 | /// # Parameters 170 | /// 171 | /// - `strategy`: The collision strategy to use for this shape. 172 | /// - `primitive`: The collision primitive. 173 | pub fn new_simple_with_type( 174 | strategy: CollisionStrategy, 175 | mode: CollisionMode, 176 | primitive: P, 177 | ty: Y, 178 | ) -> Self { 179 | Self::new_complex(strategy, mode, vec![(primitive, T::one())], ty) 180 | } 181 | 182 | /// Convenience function to create a simple collision shape with only a single given primitive, 183 | /// with a given local-to-model transform. 184 | /// 185 | /// # Parameters 186 | /// 187 | /// - `strategy`: The collision strategy to use for this shape. 188 | /// - `primitive`: The collision primitive. 189 | /// - `transform`: Local-to-model transform of the primitive. 190 | pub fn new_simple_offset( 191 | strategy: CollisionStrategy, 192 | mode: CollisionMode, 193 | primitive: P, 194 | transform: T, 195 | ) -> Self { 196 | Self::new_complex( 197 | strategy, 198 | mode, 199 | vec![(primitive, transform)], 200 | Default::default(), 201 | ) 202 | } 203 | 204 | /// Update the cached transformed bounding box in world space coordinates. 205 | /// 206 | /// If the end transform is given, that will always be used. If the collision mode of the shape 207 | /// is `Continuous`, both the start and end transforms will be added to the transformed bounding 208 | /// box. This will make broad phase detect collisions for the whole transformation path. 209 | /// 210 | /// ## Parameters 211 | /// 212 | /// - `start`: Current model-to-world transform of the shape at the start of the frame. 213 | /// - `end`: Optional model-to-world transform of the shaped at the end of the frame. 214 | pub fn update(&mut self, start: &T, end: Option<&T>) { 215 | self.transformed_bound = match end { 216 | None => self.base_bound.transform_volume(start), 217 | Some(end_t) => { 218 | let base = self.base_bound.transform_volume(end_t); 219 | if self.mode == CollisionMode::Continuous { 220 | base.union(&self.base_bound.transform_volume(start)) 221 | } else { 222 | base 223 | } 224 | } 225 | }; 226 | } 227 | 228 | /// Return the current transformed bound for the shape 229 | /// 230 | pub fn bound(&self) -> &B { 231 | &self.transformed_bound 232 | } 233 | 234 | /// Borrow the primitives of the shape 235 | pub fn primitives(&self) -> &Vec<(P, T)> { 236 | &self.primitives 237 | } 238 | } 239 | 240 | impl HasBound for CollisionShape 241 | where 242 | P: Primitive + ComputeBound, 243 | B: Bound + Union + Clone, 244 | T: Transform, 245 | Y: Default, 246 | { 247 | type Bound = B; 248 | 249 | fn bound(&self) -> &Self::Bound { 250 | &self.transformed_bound 251 | } 252 | } 253 | 254 | fn get_bound(primitives: &[(P, T)]) -> B 255 | where 256 | P: Primitive + ComputeBound, 257 | B: Bound + Union, 258 | T: Transform, 259 | { 260 | primitives 261 | .iter() 262 | .map(|&(ref p, ref t)| p.compute_bound().transform_volume(t)) 263 | .fold(B::empty(), |bound, b| bound.union(&b)) 264 | } 265 | 266 | /// Collision data used for performing a full broad + narrow phase 267 | pub trait CollisionData 268 | where 269 | P: Primitive, 270 | { 271 | /// Get the list of data to perform broad phase on 272 | fn get_broad_data(&self) -> Vec; 273 | /// Get shape 274 | fn get_shape(&self, id: I) -> Option<&CollisionShape>; 275 | /// Get pose 276 | fn get_pose(&self, id: I) -> Option<&T>; 277 | /// Get the dirty poses, used by tree broad phase 278 | fn get_dirty_poses(&self) -> Vec { 279 | Vec::default() 280 | } 281 | /// Get the next pose if possible 282 | fn get_next_pose(&self, id: I) -> Option<&T>; 283 | } 284 | 285 | /// Trait used to extract the lookup id used by `CollisionData`, given the output from a broad phase 286 | pub trait GetId { 287 | /// Get the id 288 | fn id(&self) -> I; 289 | } 290 | 291 | impl GetId for TreeValueWrapped 292 | where 293 | B: Bound, 294 | I: Copy + Debug + Hash + Eq, 295 | ::Diff: Debug, 296 | { 297 | fn id(&self) -> I { 298 | self.value 299 | } 300 | } 301 | 302 | /// Do basic collision detection (not using a DBVT) 303 | /// 304 | /// ### Type parameters: 305 | /// 306 | /// - `C`: Collision data 307 | /// - `I`: Id, returned by `GetId` on `D`, primary id for a collider 308 | /// - `P`: Primitive 309 | /// - `T`: Transform 310 | /// - `B`: Bounding volume 311 | /// - `Y`: Collider, see `Collider` for more information 312 | /// - `D`: Broad phase data 313 | pub fn basic_collide( 314 | data: &C, 315 | broad: &mut Box>, 316 | narrow: &Option>>, 317 | ) -> Vec> 318 | where 319 | C: CollisionData, 320 | P: Primitive, 321 | ::Diff: Debug, 322 | I: Copy + Debug, 323 | D: HasBound + GetId, 324 | B: Bound, 325 | { 326 | let potentials = broad_collide(data, broad); 327 | if potentials.is_empty() { 328 | return Vec::default(); 329 | } 330 | match *narrow { 331 | Some(ref narrow) => narrow_collide(data, narrow, &potentials), 332 | None => potentials 333 | .iter() 334 | .map(|&(left, right)| { 335 | ContactEvent::new_simple(CollisionStrategy::CollisionOnly, (left, right)) 336 | }).collect::>(), 337 | } 338 | } 339 | 340 | /// Do collision detection using a DBVT 341 | /// 342 | /// ### Type parameters: 343 | /// 344 | /// - `C`: Collision data 345 | /// - `I`: Id, returned by `GetId` on `D`, primary id for a collider 346 | /// - `P`: Primitive 347 | /// - `T`: Transform 348 | /// - `B`: Bounding volume 349 | /// - `Y`: Collider, see `Collider` for more information 350 | /// - `D`: `TreeValue` in DBVT 351 | pub fn tree_collide( 352 | data: &C, 353 | tree: &mut DynamicBoundingVolumeTree, 354 | broad: &mut Option>>, 355 | narrow: &Option>>, 356 | ) -> Vec> 357 | where 358 | C: CollisionData, 359 | P: Primitive, 360 | ::Diff: Debug, 361 | I: Copy + Debug + Hash + Eq, 362 | D: HasBound + GetId + TreeValue, 363 | B: Bound 364 | + Clone 365 | + SurfaceArea::Scalar> 366 | + Contains 367 | + Union 368 | + Discrete, 369 | { 370 | use collision::algorithm::broad_phase::DbvtBroadPhase; 371 | let potentials = match *broad { 372 | Some(ref mut broad) => { 373 | let p = broad.find_potentials(tree.values_mut()); 374 | tree.reindex_values(); 375 | p 376 | } 377 | None => { 378 | let dirty_entities = data.get_dirty_poses().into_iter().collect::>(); 379 | let dirty = tree 380 | .values() 381 | .iter() 382 | .map(|&(_, ref v)| dirty_entities.contains(&v.id())) 383 | .collect::>(); 384 | DbvtBroadPhase.find_collider_pairs(tree, &dirty[..]) 385 | } 386 | }; 387 | let potentials = potentials 388 | .iter() 389 | .map(|&(ref l, ref r)| (tree.values()[*l].1.id(), tree.values()[*r].1.id())) 390 | .collect::>(); 391 | match *narrow { 392 | Some(ref narrow) => narrow_collide(data, narrow, &potentials), 393 | None => potentials 394 | .iter() 395 | .map(|&(left, right)| { 396 | ContactEvent::new_simple(CollisionStrategy::CollisionOnly, (left, right)) 397 | }).collect::>(), 398 | } 399 | } 400 | 401 | #[cfg(test)] 402 | mod tests { 403 | #[cfg(feature="serializable")] 404 | use CollisionMode; 405 | 406 | #[cfg(feature="serializable")] 407 | #[test] 408 | fn test_serialization() { 409 | let p = CollisionMode::Continuous; 410 | let j = serde_json::to_string(&p); 411 | assert!(j.is_ok()); 412 | let q = serde_json::from_str(j.unwrap().as_str()); 413 | assert!(q.is_ok()); 414 | assert_eq!(p, q.unwrap()); 415 | } 416 | } 417 | -------------------------------------------------------------------------------- /rhusics-core/src/collide/narrow.rs: -------------------------------------------------------------------------------- 1 | //! Generic narrow phase collision detection algorithms. 2 | //! 3 | //! Currently only supports GJK/EPA. 4 | 5 | use std::fmt::Debug; 6 | use std::ops::Neg; 7 | 8 | use cgmath::prelude::*; 9 | use cgmath::BaseFloat; 10 | use collision::algorithm::minkowski::{SimplexProcessor, EPA, GJK}; 11 | use collision::prelude::*; 12 | use collision::{CollisionStrategy, Contact, Interpolate, Primitive}; 13 | 14 | use collide::{Collider, CollisionData, CollisionMode, CollisionShape, ContactEvent}; 15 | 16 | /// Base trait implemented by all narrow phase algorithms. 17 | /// 18 | /// # Type parameters: 19 | /// 20 | /// - `P`: collision primitive type 21 | /// - `T`: model-to-world transform type 22 | /// - `B`: Bounding volume 23 | /// - `Y`: Shape type (see `Collider`) 24 | pub trait NarrowPhase: Send 25 | where 26 | P: Primitive, 27 | ::Diff: Debug, 28 | { 29 | /// Check if two shapes collides, and give a contact manifold for the contact with the largest 30 | /// penetration depth. 31 | /// 32 | /// # Parameters: 33 | /// 34 | /// - `left`: the left shape 35 | /// - `left_transform`: model-to-world transform for the left shape 36 | /// - `right`: the right shape 37 | /// - `right_transform`: model-to-world transform for the right shape 38 | /// 39 | /// # Returns: 40 | /// 41 | /// Optionally returns the contact manifold for the contact with largest penetration depth 42 | fn collide( 43 | &self, 44 | left: &CollisionShape, 45 | left_transform: &T, 46 | right: &CollisionShape, 47 | right_transform: &T, 48 | ) -> Option>; 49 | 50 | /// Check if two shapes collides along the given transformation paths, and give a contact 51 | /// manifold for the contact with the earliest time of impact. 52 | /// 53 | /// Will only use continuous detection if one of the shapes have `Continuous` collision mode. 54 | /// 55 | /// 56 | /// # Parameters: 57 | /// 58 | /// - `left`: the left shape 59 | /// - `left_start_transform`: model-to-world transform for the left shape, at start of frame 60 | /// - `left_end_transform`: model-to-world transform for the left shape, at end of frame 61 | /// - `right`: the right shape 62 | /// - `right_start_transform`: model-to-world transform for the right shape, at start of frame 63 | /// - `right_end_transform`: model-to-world transform for the right shape, at end of frame 64 | /// 65 | /// # Returns: 66 | /// 67 | /// Optionally returns the contact manifold for the contact with largest penetration depth 68 | fn collide_continuous( 69 | &self, 70 | left: &CollisionShape, 71 | left_start_transform: &T, 72 | left_end_transform: Option<&T>, 73 | right: &CollisionShape, 74 | right_start_transform: &T, 75 | right_end_transform: Option<&T>, 76 | ) -> Option>; 77 | } 78 | 79 | impl NarrowPhase for GJK::Scalar> 80 | where 81 | P: Primitive, 82 | P::Point: EuclideanSpace, 83 | ::Scalar: BaseFloat + Send + Sync + 'static, 84 | ::Diff: Debug 85 | + InnerSpace 86 | + Array::Scalar> 87 | + Neg::Diff>, 88 | S: SimplexProcessor + Send, 89 | E: EPA + Send, 90 | T: Transform 91 | + Interpolate<::Scalar> 92 | + TranslationInterpolate<::Scalar>, 93 | Y: Collider, 94 | { 95 | fn collide( 96 | &self, 97 | left: &CollisionShape, 98 | left_transform: &T, 99 | right: &CollisionShape, 100 | right_transform: &T, 101 | ) -> Option> { 102 | if !left.enabled 103 | || !right.enabled 104 | || left.primitives.is_empty() 105 | || right.primitives.is_empty() 106 | || !left.ty.should_generate_contacts(&right.ty) 107 | { 108 | return None; 109 | } 110 | 111 | let strategy = max(&left.strategy, &right.strategy); 112 | self.intersection_complex( 113 | &strategy, 114 | &left.primitives, 115 | left_transform, 116 | &right.primitives, 117 | right_transform, 118 | ) 119 | } 120 | 121 | fn collide_continuous( 122 | &self, 123 | left: &CollisionShape, 124 | left_start_transform: &T, 125 | left_end_transform: Option<&T>, 126 | right: &CollisionShape, 127 | right_start_transform: &T, 128 | right_end_transform: Option<&T>, 129 | ) -> Option> { 130 | if !left.ty.should_generate_contacts(&right.ty) { 131 | return None; 132 | } 133 | 134 | // fallback to start transforms if end transforms are not available 135 | let left_end_transform = match left_end_transform { 136 | Some(t) => t, 137 | None => left_start_transform, 138 | }; 139 | let right_end_transform = match right_end_transform { 140 | Some(t) => t, 141 | None => right_start_transform, 142 | }; 143 | 144 | if left.mode == CollisionMode::Continuous || right.mode == CollisionMode::Continuous { 145 | let strategy = max(&left.strategy, &right.strategy); 146 | // if the start of the transformation path has collision, return that contact 147 | self.collide(left, left_start_transform, right, right_start_transform) 148 | .or_else(|| { 149 | // do time of impact calculation 150 | self.intersection_complex_time_of_impact( 151 | &strategy, 152 | &left.primitives, 153 | left_start_transform..left_end_transform, 154 | &right.primitives, 155 | right_start_transform..right_end_transform, 156 | ) 157 | }) 158 | } else { 159 | self.collide(left, left_end_transform, right, right_end_transform) 160 | } 161 | } 162 | } 163 | 164 | fn max(left: &CollisionStrategy, right: &CollisionStrategy) -> CollisionStrategy { 165 | if left > right { 166 | left.clone() 167 | } else { 168 | right.clone() 169 | } 170 | } 171 | 172 | /// Perform narrow phase collision detection on the given potential collider pairs, using the given 173 | /// narrow phase 174 | /// 175 | /// ### Type parameters: 176 | /// 177 | /// - `C`: Collision data 178 | /// - `I`: Id, returned by `GetId` on `D`, primary id for a collider 179 | /// - `P`: Primitive 180 | /// - `T`: Transform 181 | /// - `B`: Bounding volume, not used here, but required for `CollisionData` 182 | /// - `Y`: Collider, see `Collider` for more information, not used here, but required for 183 | /// `CollisionData` 184 | /// - `D`: Broad phase data, not used here, but required for `CollisionData` 185 | pub fn narrow_collide( 186 | data: &C, 187 | narrow: &Box>, 188 | potentials: &[(I, I)], 189 | ) -> Vec> 190 | where 191 | C: CollisionData, 192 | P: Primitive, 193 | ::Diff: Debug, 194 | I: Copy + Debug, 195 | { 196 | potentials 197 | .iter() 198 | .filter_map(|&(left, right)| { 199 | let left_shape = data.get_shape(left); 200 | let right_shape = data.get_shape(right); 201 | let left_pose = data.get_pose(left); 202 | let right_pose = data.get_pose(right); 203 | let left_next_pose = data.get_next_pose(left); 204 | let right_next_pose = data.get_next_pose(right); 205 | if left_shape.is_none() 206 | || right_shape.is_none() 207 | || left_pose.is_none() 208 | || right_pose.is_none() 209 | { 210 | None 211 | } else { 212 | narrow 213 | .collide_continuous( 214 | left_shape.unwrap(), 215 | left_pose.unwrap(), 216 | left_next_pose, 217 | right_shape.unwrap(), 218 | right_pose.unwrap(), 219 | right_next_pose, 220 | ).map(|contact| ContactEvent::new((left, right), contact)) 221 | } 222 | }).collect::>() 223 | } 224 | 225 | #[cfg(test)] 226 | mod tests { 227 | 228 | use cgmath::{BaseFloat, Basis2, Decomposed, Rad, Rotation2, Vector2}; 229 | use collision::algorithm::minkowski::GJK2; 230 | use collision::primitive::Rectangle; 231 | use collision::Aabb2; 232 | 233 | use collide::narrow::NarrowPhase; 234 | use collide::*; 235 | 236 | fn transform(x: S, y: S, angle: S) -> Decomposed, Basis2> 237 | where 238 | S: BaseFloat, 239 | { 240 | Decomposed { 241 | disp: Vector2::new(x, y), 242 | rot: Rotation2::from_angle(Rad(angle)), 243 | scale: S::one(), 244 | } 245 | } 246 | 247 | #[test] 248 | fn test_gjk_continuous_2d_f32() { 249 | let left = CollisionShape::<_, _, Aabb2<_>, ()>::new_simple( 250 | CollisionStrategy::FullResolution, 251 | CollisionMode::Continuous, 252 | Rectangle::new(10., 10.), 253 | ); 254 | let left_start_transform = transform::(0., 0., 0.); 255 | let left_end_transform = transform(30., 0., 0.); 256 | let right = CollisionShape::new_simple( 257 | CollisionStrategy::FullResolution, 258 | CollisionMode::Discrete, 259 | Rectangle::new(10., 10.), 260 | ); 261 | let right_transform = transform(15., 0., 0.); 262 | let gjk = GJK2::::new(); 263 | 264 | assert!( 265 | gjk.collide_continuous( 266 | &left, 267 | &left_start_transform, 268 | Some(&left_start_transform), 269 | &right, 270 | &right_transform, 271 | Some(&right_transform) 272 | ).is_none() 273 | ); 274 | 275 | let contact = gjk 276 | .collide_continuous( 277 | &left, 278 | &left_start_transform, 279 | Some(&left_end_transform), 280 | &right, 281 | &right_transform, 282 | Some(&right_transform), 283 | ).unwrap(); 284 | 285 | assert_ulps_eq!(0.16666666666666666, contact.time_of_impact); 286 | 287 | println!("{:?}", contact); 288 | } 289 | 290 | #[test] 291 | fn test_gjk_continuous_2d_f64() { 292 | let left = CollisionShape::<_, _, Aabb2<_>, ()>::new_simple( 293 | CollisionStrategy::FullResolution, 294 | CollisionMode::Continuous, 295 | Rectangle::new(10., 10.), 296 | ); 297 | let left_start_transform = transform::(0., 0., 0.); 298 | let left_end_transform = transform(30., 0., 0.); 299 | let right = CollisionShape::new_simple( 300 | CollisionStrategy::FullResolution, 301 | CollisionMode::Discrete, 302 | Rectangle::new(10., 10.), 303 | ); 304 | let right_transform = transform(15., 0., 0.); 305 | let gjk = GJK2::::new(); 306 | 307 | assert!( 308 | gjk.collide_continuous( 309 | &left, 310 | &left_start_transform, 311 | Some(&left_start_transform), 312 | &right, 313 | &right_transform, 314 | Some(&right_transform) 315 | ).is_none() 316 | ); 317 | 318 | let contact = gjk 319 | .collide_continuous( 320 | &left, 321 | &left_start_transform, 322 | Some(&left_end_transform), 323 | &right, 324 | &right_transform, 325 | Some(&right_transform), 326 | ).unwrap(); 327 | 328 | assert_ulps_eq!(0.16666666666666666, contact.time_of_impact); 329 | 330 | println!("{:?}", contact); 331 | } 332 | } 333 | -------------------------------------------------------------------------------- /rhusics-core/src/collide2d.rs: -------------------------------------------------------------------------------- 1 | //! Type wrappers and convenience functions for 2D collision detection 2 | 3 | pub use collision::algorithm::minkowski::GJK2; 4 | pub use collision::primitive::{Circle, ConvexPolygon, Particle2, Rectangle, Square}; 5 | 6 | use cgmath::{Basis2, Point2}; 7 | use collision::algorithm::broad_phase::BruteForce; 8 | use collision::primitive::Primitive2; 9 | use collision::Aabb2; 10 | 11 | use collide::*; 12 | use BodyPose; 13 | 14 | /// Collision shape for 2D, see [`CollisionShape`](../collide/struct.CollisionShape.html) for more 15 | /// information 16 | /// 17 | /// ### Type parameters: 18 | /// 19 | /// - `S`: Scalar type (f32 or f64) 20 | /// - `T`: Transform 21 | /// - `Y`: Collider type, see `Collider` for more information 22 | pub type CollisionShape2 = CollisionShape, T, Aabb2, Y>; 23 | 24 | /// Broad phase brute force algorithm for 2D, see 25 | /// [`BruteForce`](../collide/broad/struct.BruteForce.html) for more information. 26 | pub type BroadBruteForce2 = BruteForce; 27 | 28 | /// Broad phase sweep and prune algorithm 29 | /// 30 | /// ### Type parameters: 31 | /// 32 | /// - `S`: Scalar type (f32 or f64) 33 | pub type SweepAndPrune2 = ::collision::algorithm::broad_phase::SweepAndPrune2>; 34 | 35 | /// Body pose transform for 2D, see [`BodyPos`e](../struct.BodyPose.html) for more information. 36 | /// 37 | /// ### Type parameters: 38 | /// 39 | /// - `S`: Scalar type (f32 or f64) 40 | pub type BodyPose2 = BodyPose, Basis2>; 41 | -------------------------------------------------------------------------------- /rhusics-core/src/collide3d.rs: -------------------------------------------------------------------------------- 1 | //! Type wrappers and convenience functions for 3D collision detection 2 | 3 | pub use collision::algorithm::minkowski::GJK3; 4 | pub use collision::primitive::{ConvexPolyhedron, Cuboid, Particle3, Sphere}; 5 | 6 | use cgmath::{Point3, Quaternion}; 7 | use collision::algorithm::broad_phase::BruteForce; 8 | use collision::primitive::Primitive3; 9 | use collision::Aabb3; 10 | 11 | use collide::*; 12 | use BodyPose; 13 | 14 | /// Collision shape for 3D, see [`CollisionShape`](../collide/struct.CollisionShape.html) for more 15 | /// information 16 | /// 17 | /// ### Type parameters: 18 | /// 19 | /// - `S`: Scalar type (f32 or f64) 20 | /// - `T`: Transform 21 | /// - `Y`: Collider type, see `Collider` for more information 22 | pub type CollisionShape3 = CollisionShape, T, Aabb3, Y>; 23 | 24 | /// Broad phase brute force algorithm for 3D, see 25 | /// [`BruteForce`](../collide/broad/struct.BruteForce.html) for more information. 26 | pub type BroadBruteForce3 = BruteForce; 27 | 28 | /// Broad phase sweep and prune algorithm 29 | /// 30 | /// ### Type parameters: 31 | /// 32 | /// - `S`: Scalar type (f32 or f64) 33 | pub type SweepAndPrune3 = ::collision::algorithm::broad_phase::SweepAndPrune3>; 34 | 35 | /// Body pose transform for 3D, see [`BodyPose`](../struct.BodyPose.html) for more information. 36 | /// 37 | /// ### Type parameters: 38 | /// 39 | /// - `S`: Scalar type (f32 or f64) 40 | pub type BodyPose3 = BodyPose, Quaternion>; 41 | -------------------------------------------------------------------------------- /rhusics-core/src/ecs.rs: -------------------------------------------------------------------------------- 1 | //! ECS Component declarations for data structures in the crate, this needs to be here and not in 2 | //! rhusics-ecs because of the orphan rule. 3 | 4 | use cgmath::prelude::*; 5 | use cgmath::BaseFloat; 6 | use collision::prelude::*; 7 | use specs::prelude::{Component, DenseVecStorage, FlaggedStorage}; 8 | 9 | use collide::CollisionShape; 10 | use physics::{ForceAccumulator, Mass, PhysicalEntity, Velocity}; 11 | use {BodyPose, NextFrame}; 12 | 13 | impl Component for BodyPose 14 | where 15 | P: EuclideanSpace + Send + Sync + 'static, 16 | P::Scalar: BaseFloat, 17 | R: Rotation

+ Send + Sync + 'static, 18 | { 19 | type Storage = FlaggedStorage>; 20 | } 21 | 22 | impl Component for NextFrame 23 | where 24 | T: Send + Sync + 'static, 25 | { 26 | type Storage = FlaggedStorage>; 27 | } 28 | 29 | impl Component for CollisionShape 30 | where 31 | T: Send + Sync + 'static, 32 | Y: Send + Sync + 'static, 33 | P: Primitive + Send + Sync + 'static, 34 | B: Bound + Send + Sync + 'static, 35 | { 36 | type Storage = DenseVecStorage>; 37 | } 38 | 39 | impl Component for Velocity 40 | where 41 | V: Send + Sync + 'static + Clone, 42 | A: Send + Sync + 'static + Clone, 43 | { 44 | type Storage = DenseVecStorage; 45 | } 46 | 47 | impl Component for Mass 48 | where 49 | S: Send + Sync + 'static, 50 | I: Send + Sync + 'static, 51 | { 52 | type Storage = DenseVecStorage; 53 | } 54 | 55 | impl Component for PhysicalEntity 56 | where 57 | S: Send + Sync + 'static, 58 | { 59 | type Storage = DenseVecStorage; 60 | } 61 | 62 | impl Component for ForceAccumulator 63 | where 64 | F: Send + Sync + 'static, 65 | A: Send + Sync + 'static, 66 | { 67 | type Storage = DenseVecStorage; 68 | } 69 | -------------------------------------------------------------------------------- /rhusics-core/src/lib.rs: -------------------------------------------------------------------------------- 1 | //! # Rhusics physics library 2 | //! 3 | //! A physics library. 4 | //! Uses [`cgmath`](https://github.com/brendanzab/cgmath/) for all computation. 5 | //! 6 | //! Features: 7 | //! 8 | //! * Two different broad phase collision detection implementations: 9 | //! * Brute force 10 | //! * Sweep and Prune 11 | //! * Narrow phase collision detection using GJK, and optionally EPA for full contact information 12 | //! * Functions for collision detection working on user supplied transform, and 13 | //! [`CollisionShape`](collide/struct.CollisionShape.html) components. 14 | //! Can optionally use broad and/or narrow phase detection. 15 | //! Library supplies a transform implementation [`BodyPose`](struct.BodyPose.html) for 16 | //! convenience. 17 | //! * Uses single precision as default, can be changed to double precision with the `double` 18 | //! feature. 19 | //! * Has support for doing spatial sort/collision detection using the collision-rs DBVT. 20 | //! * Support for doing broad phase using the collision-rs DBVT. 21 | //! * Has support for all primitives in collision-rs 22 | //! 23 | 24 | #![deny( 25 | missing_docs, 26 | trivial_casts, 27 | unsafe_code, 28 | unstable_features, 29 | unused_import_braces, 30 | unused_qualifications 31 | )] 32 | #![allow(unknown_lints, type_complexity, borrowed_box)] 33 | 34 | extern crate cgmath; 35 | extern crate collision; 36 | extern crate rhusics_transform; 37 | 38 | #[cfg(feature = "specs")] 39 | extern crate specs; 40 | 41 | #[cfg(test)] 42 | #[macro_use] 43 | extern crate approx; 44 | 45 | #[cfg(feature = "serializable")] 46 | #[macro_use] 47 | extern crate serde; 48 | 49 | pub use body_pose::BodyPose; 50 | pub use collide::broad::{BroadPhase, BruteForce, SweepAndPrune2, SweepAndPrune3}; 51 | pub use collide::narrow::NarrowPhase; 52 | pub use collide::{ 53 | basic_collide, tree_collide, Collider, CollisionData, CollisionMode, CollisionShape, 54 | CollisionStrategy, Contact, ContactEvent, GetId, Primitive, 55 | }; 56 | pub use physics::simple::{next_frame_integration, next_frame_pose}; 57 | pub use physics::{ 58 | resolve_contact, ApplyAngular, ForceAccumulator, Inertia, Mass, Material, PartialCrossProduct, 59 | PhysicalEntity, ResolveData, SingleChangeSet, Velocity, Volume, WorldParameters, 60 | }; 61 | pub use rhusics_transform::{PhysicsTime, Pose}; 62 | 63 | pub mod collide2d; 64 | pub mod collide3d; 65 | pub mod physics2d; 66 | pub mod physics3d; 67 | 68 | mod body_pose; 69 | mod collide; 70 | #[cfg(feature = "specs")] 71 | mod ecs; 72 | mod physics; 73 | 74 | /// Wrapper for data computed for the next frame 75 | #[derive(Clone, Debug, PartialEq)] 76 | #[cfg_attr(feature = "serializable", derive(Serialize, Deserialize))] 77 | pub struct NextFrame { 78 | /// Wrapped value 79 | pub value: T, 80 | } 81 | -------------------------------------------------------------------------------- /rhusics-core/src/physics/force.rs: -------------------------------------------------------------------------------- 1 | use cgmath::{BaseFloat, EuclideanSpace, Transform, VectorSpace, Zero}; 2 | 3 | use super::PartialCrossProduct; 4 | 5 | /// Force accumulator for a physical entity. 6 | /// 7 | /// Will be consumed when doing force integration for the next frame. 8 | /// 9 | /// ### Type parameters: 10 | /// 11 | /// - `F`: Force type, usually `Vector2` or `Vector3` 12 | /// - `T`: Torque force, usually `Scalar` or `Vector3` 13 | #[derive(Debug)] 14 | #[cfg_attr(feature = "serializable", derive(Serialize, Deserialize))] 15 | pub struct ForceAccumulator { 16 | force: F, 17 | torque: T, 18 | } 19 | 20 | impl Default for ForceAccumulator 21 | where 22 | F: VectorSpace + Zero, 23 | T: Zero + Copy + Clone, 24 | { 25 | fn default() -> Self { 26 | ForceAccumulator::new() 27 | } 28 | } 29 | 30 | impl ForceAccumulator 31 | where 32 | F: VectorSpace + Zero, 33 | T: Zero + Copy + Clone, 34 | { 35 | /// Create a new force accumulator 36 | pub fn new() -> Self { 37 | Self { 38 | force: F::zero(), 39 | torque: T::zero(), 40 | } 41 | } 42 | 43 | /// Add a force vector to the accumulator 44 | pub fn add_force(&mut self, force: F) { 45 | self.force = self.force + force; 46 | } 47 | 48 | /// Add a torque vector to the accumulator 49 | pub fn add_torque(&mut self, torque: T) { 50 | self.torque = self.torque + torque; 51 | } 52 | 53 | /// Add a force on a given point on the body 54 | /// 55 | /// If the force vector does not pass directly through the origin of the body, as expressed by 56 | /// the pose, torque will occur. 57 | /// Note that no validation is made on the given position to make sure it's actually contained 58 | /// in the shape of the body. 59 | /// 60 | /// ### Parameters: 61 | /// 62 | /// - `force`: Force to apply 63 | /// - `position`: Position on the body to apply the force at. 64 | /// - `pose`: Current pose of the body, used to compute the world coordinates of the body center 65 | /// of mass 66 | pub fn add_force_at_point(&mut self, force: F, position: P, pose: &B) 67 | where 68 | P: EuclideanSpace, 69 | P::Scalar: BaseFloat, 70 | B: Transform

, 71 | F: PartialCrossProduct, 72 | { 73 | let current_pos = pose.transform_point(P::origin()); 74 | let r = position - current_pos; 75 | self.add_force(force); 76 | self.add_torque(r.cross(&force)); 77 | } 78 | 79 | /// Consume the accumulated force 80 | /// 81 | /// Returns he current accumulated force. The force in the accumulator is reset. 82 | pub fn consume_force(&mut self) -> F { 83 | let v = self.force; 84 | self.force = F::zero(); 85 | v 86 | } 87 | 88 | /// Consume the torque 89 | /// 90 | /// Returns the current accumulated torque. The torque in the accumulator is reset. 91 | pub fn consume_torque(&mut self) -> T { 92 | let v = self.torque; 93 | self.torque = T::zero(); 94 | v 95 | } 96 | } 97 | 98 | #[cfg(test)] 99 | mod tests_f32 { 100 | use cgmath::{Point2, Point3, Transform, Vector2, Vector3, Zero}; 101 | 102 | use super::ForceAccumulator; 103 | use physics2d::BodyPose2; 104 | use physics3d::BodyPose3; 105 | 106 | #[test] 107 | fn test_add_force() { 108 | let mut forces = ForceAccumulator::, f32>::new(); 109 | forces.add_force(Vector2::new(0., 2.)); 110 | forces.add_force(Vector2::new(1.4, 2.)); 111 | assert_eq!(Vector2::new(1.4, 4.), forces.consume_force()); 112 | assert_eq!(Vector2::zero(), forces.consume_force()); 113 | assert_eq!(0., forces.consume_torque()); 114 | 115 | let mut forces = ForceAccumulator::, f32>::new(); 116 | forces.add_force(Vector3::new(0., 2., -1.)); 117 | forces.add_force(Vector3::new(1.4, 2., -1.)); 118 | assert_eq!(Vector3::new(1.4, 4., -2.), forces.consume_force()); 119 | assert_eq!(Vector3::zero(), forces.consume_force()); 120 | assert_eq!(0., forces.consume_torque()); 121 | } 122 | 123 | #[test] 124 | fn test_add_torque() { 125 | let mut forces = ForceAccumulator::, f32>::new(); 126 | forces.add_torque(0.2); 127 | forces.add_torque(1.4); 128 | assert_ulps_eq!(1.6, forces.consume_torque()); 129 | assert_eq!(Vector2::zero(), forces.consume_force()); 130 | assert_eq!(0., forces.consume_torque()); 131 | 132 | let mut forces = ForceAccumulator::, f32>::new(); 133 | forces.add_torque(0.2); 134 | forces.add_torque(1.4); 135 | assert_ulps_eq!(1.6, forces.consume_torque()); 136 | assert_eq!(Vector3::zero(), forces.consume_force()); 137 | assert_eq!(0., forces.consume_torque()); 138 | } 139 | 140 | #[test] 141 | fn test_add_force_at_point_2d() { 142 | let mut forces = ForceAccumulator::, f32>::new(); 143 | // add at origin -> no torque 144 | forces.add_force_at_point(Vector2::new(1., 1.), Point2::new(0., 0.), &BodyPose2::one()); 145 | assert_eq!(Vector2::new(1., 1.), forces.consume_force()); 146 | assert_eq!(0., forces.consume_torque()); 147 | // add pointed at origin -> no torque 148 | forces.add_force_at_point( 149 | Vector2::new(1., 1.), 150 | Point2::new(-1., -1.), 151 | &BodyPose2::one(), 152 | ); 153 | assert_eq!(Vector2::new(1., 1.), forces.consume_force()); 154 | assert_eq!(0., forces.consume_torque()); 155 | // add outside with offset -> torque 156 | forces.add_force_at_point( 157 | Vector2::new(1., 1.), 158 | Point2::new(-1., 0.), 159 | &BodyPose2::one(), 160 | ); 161 | assert_eq!(Vector2::new(1., 1.), forces.consume_force()); 162 | assert_eq!(-1., forces.consume_torque()); 163 | } 164 | 165 | #[test] 166 | fn test_add_force_at_point_3d() { 167 | let mut forces = ForceAccumulator::, Vector3>::new(); 168 | // add at origin -> no torque 169 | forces.add_force_at_point( 170 | Vector3::new(1., 1., 1.), 171 | Point3::new(0., 0., 0.), 172 | &BodyPose3::one(), 173 | ); 174 | assert_eq!(Vector3::new(1., 1., 1.), forces.consume_force()); 175 | assert_eq!(Vector3::zero(), forces.consume_torque()); 176 | // add pointed at origin -> no torque 177 | forces.add_force_at_point( 178 | Vector3::new(1., 1., 1.), 179 | Point3::new(-1., -1., -1.), 180 | &BodyPose3::one(), 181 | ); 182 | assert_eq!(Vector3::new(1., 1., 1.), forces.consume_force()); 183 | assert_eq!(Vector3::zero(), forces.consume_torque()); 184 | // add outside with offset -> torque 185 | forces.add_force_at_point( 186 | Vector3::new(1., 1., 1.), 187 | Point3::new(-1., 0., 0.), 188 | &BodyPose3::one(), 189 | ); 190 | assert_eq!(Vector3::new(1., 1., 1.), forces.consume_force()); 191 | assert_eq!(Vector3::new(0., 1., -1.), forces.consume_torque()); 192 | } 193 | } 194 | 195 | #[cfg(test)] 196 | mod tests_f64 { 197 | use cgmath::{Point2, Point3, Transform, Vector2, Vector3, Zero}; 198 | 199 | use super::ForceAccumulator; 200 | use physics2d::BodyPose2; 201 | use physics3d::BodyPose3; 202 | 203 | #[test] 204 | fn test_add_force() { 205 | let mut forces = ForceAccumulator::, f64>::new(); 206 | forces.add_force(Vector2::new(0., 2.)); 207 | forces.add_force(Vector2::new(1.4, 2.)); 208 | assert_eq!(Vector2::new(1.4, 4.), forces.consume_force()); 209 | assert_eq!(Vector2::zero(), forces.consume_force()); 210 | assert_eq!(0., forces.consume_torque()); 211 | 212 | let mut forces = ForceAccumulator::, f64>::new(); 213 | forces.add_force(Vector3::new(0., 2., -1.)); 214 | forces.add_force(Vector3::new(1.4, 2., -1.)); 215 | assert_eq!(Vector3::new(1.4, 4., -2.), forces.consume_force()); 216 | assert_eq!(Vector3::zero(), forces.consume_force()); 217 | assert_eq!(0., forces.consume_torque()); 218 | } 219 | 220 | #[test] 221 | fn test_add_torque() { 222 | let mut forces = ForceAccumulator::, f64>::new(); 223 | forces.add_torque(0.2); 224 | forces.add_torque(1.4); 225 | assert_ulps_eq!(1.6, forces.consume_torque()); 226 | assert_eq!(Vector2::zero(), forces.consume_force()); 227 | assert_eq!(0., forces.consume_torque()); 228 | 229 | let mut forces = ForceAccumulator::, f64>::new(); 230 | forces.add_torque(0.2); 231 | forces.add_torque(1.4); 232 | assert_ulps_eq!(1.6, forces.consume_torque()); 233 | assert_eq!(Vector3::zero(), forces.consume_force()); 234 | assert_eq!(0., forces.consume_torque()); 235 | } 236 | 237 | #[test] 238 | fn test_add_force_at_point_2d() { 239 | let mut forces = ForceAccumulator::, f64>::new(); 240 | // add at origin -> no torque 241 | forces.add_force_at_point(Vector2::new(1., 1.), Point2::new(0., 0.), &BodyPose2::one()); 242 | assert_eq!(Vector2::new(1., 1.), forces.consume_force()); 243 | assert_eq!(0., forces.consume_torque()); 244 | // add pointed at origin -> no torque 245 | forces.add_force_at_point( 246 | Vector2::new(1., 1.), 247 | Point2::new(-1., -1.), 248 | &BodyPose2::one(), 249 | ); 250 | assert_eq!(Vector2::new(1., 1.), forces.consume_force()); 251 | assert_eq!(0., forces.consume_torque()); 252 | // add outside with offset -> torque 253 | forces.add_force_at_point( 254 | Vector2::new(1., 1.), 255 | Point2::new(-1., 0.), 256 | &BodyPose2::one(), 257 | ); 258 | assert_eq!(Vector2::new(1., 1.), forces.consume_force()); 259 | assert_eq!(-1., forces.consume_torque()); 260 | } 261 | 262 | #[test] 263 | fn test_add_force_at_point_3d() { 264 | let mut forces = ForceAccumulator::, Vector3>::new(); 265 | // add at origin -> no torque 266 | forces.add_force_at_point( 267 | Vector3::new(1., 1., 1.), 268 | Point3::new(0., 0., 0.), 269 | &BodyPose3::one(), 270 | ); 271 | assert_eq!(Vector3::new(1., 1., 1.), forces.consume_force()); 272 | assert_eq!(Vector3::zero(), forces.consume_torque()); 273 | // add pointed at origin -> no torque 274 | forces.add_force_at_point( 275 | Vector3::new(1., 1., 1.), 276 | Point3::new(-1., -1., -1.), 277 | &BodyPose3::one(), 278 | ); 279 | assert_eq!(Vector3::new(1., 1., 1.), forces.consume_force()); 280 | assert_eq!(Vector3::zero(), forces.consume_torque()); 281 | // add outside with offset -> torque 282 | forces.add_force_at_point( 283 | Vector3::new(1., 1., 1.), 284 | Point3::new(-1., 0., 0.), 285 | &BodyPose3::one(), 286 | ); 287 | assert_eq!(Vector3::new(1., 1., 1.), forces.consume_force()); 288 | assert_eq!(Vector3::new(0., 1., -1.), forces.consume_torque()); 289 | } 290 | } 291 | -------------------------------------------------------------------------------- /rhusics-core/src/physics/mass.rs: -------------------------------------------------------------------------------- 1 | use std; 2 | use std::ops::Mul; 3 | 4 | use cgmath::{BaseFloat, Basis2, Matrix, Matrix3, Quaternion, SquareMatrix, Zero}; 5 | 6 | use super::{Material, Volume}; 7 | 8 | /// Mass 9 | /// 10 | /// Mass properties for a body. Inertia is the moment of inertia in the base pose. For retrieving 11 | /// the inertia tensor for the body in its current orientation, see `world_inertia` and 12 | /// `world_inverse_inertia`. 13 | /// 14 | /// ### Type parameters: 15 | /// 16 | /// - `I`: Inertia type, usually `Scalar` or `Matrix3`, see `Inertia` for more information. 17 | #[derive(Debug, Clone)] 18 | #[cfg_attr(feature = "serializable", derive(Serialize, Deserialize))] 19 | pub struct Mass { 20 | mass: S, 21 | inverse_mass: S, 22 | 23 | inertia: I, 24 | inverse_inertia: I, 25 | } 26 | 27 | /// Moment of inertia, used for abstracting over 2D/3D inertia tensors 28 | pub trait Inertia: Mul + Copy { 29 | /// Orientation type for rotating the inertia to create a world space inertia tensor 30 | type Orientation; 31 | /// Return the infinite inertia 32 | fn infinite() -> Self; 33 | /// Compute the inverse of the inertia 34 | fn invert(&self) -> Self; 35 | /// Compute the inertia tensor in the bodies given orientation 36 | fn tensor(&self, orientation: &Self::Orientation) -> Self; 37 | } 38 | 39 | impl Inertia for f32 { 40 | type Orientation = Basis2; 41 | 42 | fn infinite() -> Self { 43 | std::f32::INFINITY 44 | } 45 | 46 | fn invert(&self) -> Self { 47 | if *self == 0. || self.is_infinite() { 48 | 0. 49 | } else { 50 | 1. / *self 51 | } 52 | } 53 | 54 | fn tensor(&self, _: &Basis2) -> Self { 55 | *self 56 | } 57 | } 58 | 59 | impl Inertia for f64 { 60 | type Orientation = Basis2; 61 | 62 | fn invert(&self) -> Self { 63 | if *self == 0. || self.is_infinite() { 64 | 0. 65 | } else { 66 | 1. / *self 67 | } 68 | } 69 | 70 | fn tensor(&self, _: &Basis2) -> Self { 71 | *self 72 | } 73 | 74 | fn infinite() -> Self { 75 | std::f64::INFINITY 76 | } 77 | } 78 | 79 | impl Inertia for Matrix3 80 | where 81 | S: BaseFloat, 82 | { 83 | type Orientation = Quaternion; 84 | 85 | fn invert(&self) -> Self { 86 | if self.x.x.is_infinite() { 87 | Matrix3::zero() 88 | } else { 89 | SquareMatrix::invert(self).unwrap_or_else(Matrix3::zero) 90 | } 91 | } 92 | 93 | fn tensor(&self, orientation: &Quaternion) -> Self { 94 | let mat3 = Matrix3::from(*orientation); 95 | mat3 * (*self * mat3.transpose()) 96 | } 97 | 98 | fn infinite() -> Self { 99 | Matrix3::from_value(S::infinity()) 100 | } 101 | } 102 | 103 | impl Mass 104 | where 105 | S: BaseFloat, 106 | I: Inertia, 107 | { 108 | /// Create new mass object 109 | pub fn new(mass: S) -> Self { 110 | Self::new_with_inertia(mass, I::infinite()) 111 | } 112 | 113 | /// Create new infinite mass object 114 | pub fn infinite() -> Self { 115 | Self::new_with_inertia(S::infinity(), I::infinite()) 116 | } 117 | 118 | /// Create new mass object with given inertia 119 | pub fn new_with_inertia(mass: S, inertia: I) -> Self { 120 | let inverse_mass = if mass.is_infinite() { 121 | S::zero() 122 | } else { 123 | S::one() / mass 124 | }; 125 | let inverse_inertia = inertia.invert(); 126 | Mass { 127 | mass, 128 | inverse_mass, 129 | inertia, 130 | inverse_inertia, 131 | } 132 | } 133 | 134 | /// Compute mass from the given volume shape and material 135 | pub fn from_volume_and_material(volume: &V, material: &Material) -> Self 136 | where 137 | V: Volume, 138 | { 139 | volume.get_mass(material) 140 | } 141 | 142 | /// Get mass 143 | pub fn mass(&self) -> S { 144 | self.mass 145 | } 146 | 147 | /// Get inverse mass 148 | pub fn inverse_mass(&self) -> S { 149 | self.inverse_mass 150 | } 151 | 152 | /// Get inertia in local space 153 | pub fn local_inertia(&self) -> I { 154 | self.inertia 155 | } 156 | 157 | /// Get inertia tensor in world space 158 | /// 159 | /// ### Parameters: 160 | /// 161 | /// - `orientation`: The current orientation of the body 162 | pub fn world_inertia(&self, orientation: &I::Orientation) -> I { 163 | self.inertia.tensor(orientation) 164 | } 165 | 166 | /// Get inverse inertia in local space 167 | pub fn local_inverse_inertia(&self) -> I { 168 | self.inverse_inertia 169 | } 170 | 171 | /// Get inverse inertia tensor in world space 172 | /// 173 | /// ### Parameters: 174 | /// 175 | /// - `orientation`: The current orientation of the body 176 | pub fn world_inverse_inertia(&self, orientation: &I::Orientation) -> I { 177 | self.inverse_inertia.tensor(orientation) 178 | } 179 | } 180 | -------------------------------------------------------------------------------- /rhusics-core/src/physics/mod.rs: -------------------------------------------------------------------------------- 1 | //! Physics related functionality 2 | //! 3 | 4 | pub use self::force::ForceAccumulator; 5 | pub use self::mass::{Inertia, Mass}; 6 | pub use self::resolution::{resolve_contact, ResolveData, SingleChangeSet}; 7 | pub use self::util::PartialCrossProduct; 8 | pub use self::velocity::{ApplyAngular, Velocity}; 9 | pub use self::volumes::Volume; 10 | 11 | pub mod simple; 12 | 13 | mod resolution; 14 | 15 | mod force; 16 | mod mass; 17 | mod util; 18 | mod velocity; 19 | mod volumes; 20 | 21 | use cgmath::{BaseFloat, VectorSpace}; 22 | 23 | /// Global parameters for the physics world 24 | #[derive(Debug, Clone)] 25 | #[cfg_attr(feature = "serializable", derive(Serialize, Deserialize))] 26 | pub struct WorldParameters { 27 | gravity: V, 28 | damping: S, 29 | } 30 | 31 | impl Default for WorldParameters 32 | where 33 | V: VectorSpace, 34 | S: BaseFloat, 35 | { 36 | fn default() -> Self { 37 | WorldParameters::new(V::zero()) 38 | } 39 | } 40 | 41 | impl WorldParameters 42 | where 43 | V: VectorSpace, 44 | S: BaseFloat, 45 | { 46 | /// Setup global parameters for the physics world 47 | pub fn new(gravity: V) -> Self { 48 | WorldParameters { 49 | gravity, 50 | damping: S::from(0.99).unwrap(), 51 | } 52 | } 53 | 54 | /// Set global damping, can be overriden by individual physical entities 55 | pub fn with_damping(mut self, damping: S) -> Self { 56 | self.damping = damping; 57 | self 58 | } 59 | 60 | /// Get gravity 61 | pub fn gravity(&self) -> V { 62 | self.gravity 63 | } 64 | 65 | /// Get global damping 66 | pub fn damping(&self) -> S { 67 | self.damping 68 | } 69 | 70 | /// Get damping for a specific physics entity 71 | pub fn entity_damping(&self, body: Option) -> S { 72 | body.unwrap_or(self.damping) 73 | } 74 | } 75 | 76 | /// Physics material 77 | /// 78 | /// Used to describe physical properties of physical entities, such as density and restitution. 79 | /// 80 | /// The default material has density 1, such that only the volume affects its mass, and restitution 81 | /// 1, such that all energy is preserved in collisions. 82 | #[derive(Debug, Clone)] 83 | #[cfg_attr(feature = "serializable", derive(Serialize, Deserialize))] 84 | pub struct Material { 85 | density: f32, 86 | restitution: f32, 87 | } 88 | 89 | impl Default for Material { 90 | fn default() -> Self { 91 | Material::new(1., 1.) 92 | } 93 | } 94 | 95 | impl Material { 96 | /// Rock 97 | pub const ROCK: Material = Material { 98 | density: 0.6, 99 | restitution: 0.1, 100 | }; 101 | /// Wood 102 | pub const WOOD: Material = Material { 103 | density: 0.3, 104 | restitution: 0.2, 105 | }; 106 | /// Metal 107 | pub const METAL: Material = Material { 108 | density: 1.2, 109 | restitution: 0.05, 110 | }; 111 | /// Bouncy Ball 112 | pub const BOUNCY_BALL: Material = Material { 113 | density: 0.3, 114 | restitution: 0.8, 115 | }; 116 | /// Super Ball 117 | pub const SUPER_BALL: Material = Material { 118 | density: 0.3, 119 | restitution: 0.95, 120 | }; 121 | /// Pillow 122 | pub const PILLOW: Material = Material { 123 | density: 0.1, 124 | restitution: 0.2, 125 | }; 126 | /// Static 127 | pub const STATIC: Material = Material { 128 | density: 0.0, 129 | restitution: 0.4, 130 | }; 131 | 132 | /// Create new material 133 | pub fn new(density: f32, restitution: f32) -> Self { 134 | Self { 135 | density, 136 | restitution, 137 | } 138 | } 139 | 140 | /// Get density 141 | pub fn density(&self) -> S 142 | where 143 | S: BaseFloat, 144 | { 145 | S::from(self.density).unwrap() 146 | } 147 | 148 | /// Get restitution 149 | pub fn restitution(&self) -> S 150 | where 151 | S: BaseFloat, 152 | { 153 | S::from(self.restitution).unwrap() 154 | } 155 | } 156 | 157 | /// Physical entity 158 | #[derive(Debug, Clone)] 159 | #[cfg_attr(feature = "serializable", derive(Serialize, Deserialize))] 160 | pub struct PhysicalEntity { 161 | material: Material, 162 | gravity_scale: S, 163 | damping: Option, 164 | active: bool, 165 | } 166 | 167 | impl Default for PhysicalEntity 168 | where 169 | S: BaseFloat, 170 | { 171 | fn default() -> Self { 172 | PhysicalEntity::new(Material::default()) 173 | } 174 | } 175 | 176 | impl PhysicalEntity 177 | where 178 | S: BaseFloat, 179 | { 180 | /// Create new physical entity 181 | /// 182 | /// ## Parameters: 183 | /// 184 | /// - material: physical material (`Material`) 185 | pub fn new(material: Material) -> Self { 186 | Self { 187 | material, 188 | gravity_scale: S::one(), 189 | damping: None, 190 | active: true, 191 | } 192 | } 193 | 194 | /// Set the amount that gravity will affect this entity 195 | /// The gravity constant is set globally for the physics world. 196 | pub fn with_gravity_scale(mut self, gravity_scale: S) -> Self { 197 | self.gravity_scale = gravity_scale; 198 | self 199 | } 200 | 201 | /// Override the velocity damping for the entity 202 | /// The physics world control have a global damping set which is overriden by this. 203 | pub fn with_damping(mut self, damping: S) -> Self { 204 | self.damping = Some(damping); 205 | self 206 | } 207 | 208 | /// Get material 209 | pub fn material(&self) -> &Material { 210 | &self.material 211 | } 212 | 213 | /// Get gravity scale 214 | pub fn gravity_scale(&self) -> S { 215 | self.gravity_scale 216 | } 217 | 218 | /// Get entity specific damping 219 | pub fn damping(&self) -> Option { 220 | self.damping 221 | } 222 | 223 | /// Is entity active ? 224 | pub fn active(&self) -> bool { 225 | self.active 226 | } 227 | 228 | /// Set entity to active, meaning physics will act on it 229 | pub fn activate(&mut self) { 230 | self.active = true; 231 | } 232 | 233 | /// Set entity to inactive, meaning physics will not act on it 234 | pub fn deactivate(&mut self) { 235 | self.active = false; 236 | } 237 | } 238 | -------------------------------------------------------------------------------- /rhusics-core/src/physics/resolution.rs: -------------------------------------------------------------------------------- 1 | use std::fmt::Debug; 2 | use std::marker; 3 | use std::ops::{Add, Mul, Sub}; 4 | 5 | use cgmath::num_traits::{Float, NumCast}; 6 | use cgmath::{BaseFloat, EuclideanSpace, InnerSpace, One, Rotation, Zero}; 7 | use collision::Contact; 8 | 9 | use super::{Inertia, Mass, Material, PartialCrossProduct, Velocity}; 10 | use {NextFrame, Pose}; 11 | 12 | const POSITIONAL_CORRECTION_PERCENT: f32 = 0.2; 13 | const POSITIONAL_CORRECTION_K_SLOP: f32 = 0.01; 14 | 15 | /// Changes computed from contact resolution. 16 | /// 17 | /// Optionally contains the new pose and/or velocity of a body after contact resolution. 18 | /// 19 | /// ### Type parameters: 20 | /// 21 | /// - `B`: Transform type (`BodyPose3` or similar) 22 | /// - `P`: Point type, usually `Point2` or `Point3` 23 | /// - `R`: Rotational quantity, usually `Basis2` or `Quaternion` 24 | /// - `A`: Angular velocity, usually `Scalar` or `Vector3` 25 | #[derive(Debug, PartialEq)] 26 | pub struct SingleChangeSet 27 | where 28 | P: EuclideanSpace, 29 | P::Scalar: BaseFloat, 30 | R: Rotation

, 31 | A: Clone, 32 | B: Pose, 33 | { 34 | pose: Option, 35 | velocity: Option>>, 36 | m: marker::PhantomData<(P, R)>, 37 | } 38 | 39 | impl Default for SingleChangeSet 40 | where 41 | P: EuclideanSpace, 42 | P::Scalar: BaseFloat, 43 | R: Rotation

, 44 | A: Clone, 45 | B: Pose, 46 | { 47 | fn default() -> Self { 48 | Self { 49 | pose: None, 50 | velocity: None, 51 | m: marker::PhantomData, 52 | } 53 | } 54 | } 55 | 56 | impl SingleChangeSet 57 | where 58 | P: EuclideanSpace, 59 | P::Scalar: BaseFloat, 60 | R: Rotation

, 61 | A: Clone, 62 | B: Pose, 63 | { 64 | #[allow(dead_code)] 65 | fn new(pose: Option, velocity: Option>>) -> Self { 66 | SingleChangeSet { 67 | pose, 68 | velocity, 69 | m: marker::PhantomData, 70 | } 71 | } 72 | 73 | fn add_pose(&mut self, pose: Option) { 74 | self.pose = pose; 75 | } 76 | 77 | fn add_velocity(&mut self, velocity: Option>>) { 78 | self.velocity = velocity; 79 | } 80 | 81 | /// Apply any changes to the next frame pose and/or velocity 82 | pub fn apply( 83 | self, 84 | pose: Option<&mut NextFrame>, 85 | velocity: Option<&mut NextFrame>>, 86 | ) { 87 | if let (Some(pose), Some(update_pose)) = (pose, self.pose) { 88 | pose.value = update_pose; 89 | } 90 | if let (Some(velocity), Some(update_velocity)) = (velocity, self.velocity) { 91 | *velocity = update_velocity; 92 | } 93 | } 94 | } 95 | 96 | /// Data used for contact resolution 97 | /// 98 | /// ### Type parameters: 99 | /// 100 | /// - `B`: Transform type (`BodyPose3` or similar) 101 | /// - `P`: Point type, usually `Point2` or `Point3` 102 | /// - `R`: Rotational quantity, usually `Basis2` or `Quaternion` 103 | /// - `A`: Angular velocity, usually `Scalar` or `Vector3` 104 | /// - `I`: Inertia, usually `Scalar` or `Matrix3` 105 | pub struct ResolveData<'a, B, P, R, I, A> 106 | where 107 | P: EuclideanSpace + 'a, 108 | P::Scalar: BaseFloat, 109 | R: Rotation

+ 'a, 110 | I: 'a, 111 | A: Clone + 'a, 112 | B: Pose + 'a, 113 | { 114 | /// Velocity for next frame 115 | pub velocity: Option<&'a NextFrame>>, 116 | /// Position for next frame 117 | pub pose: &'a B, 118 | /// Mass 119 | pub mass: &'a Mass, 120 | /// Material 121 | pub material: &'a Material, 122 | m: marker::PhantomData<(P, R)>, 123 | } 124 | 125 | impl<'a, B, P, R, I, A> ResolveData<'a, B, P, R, I, A> 126 | where 127 | P: EuclideanSpace + 'a, 128 | P::Scalar: BaseFloat, 129 | R: Rotation

+ 'a, 130 | I: 'a, 131 | A: Clone + 'a, 132 | B: Pose + 'a, 133 | { 134 | /// Create resolve data 135 | pub fn new( 136 | velocity: Option<&'a NextFrame>>, 137 | pose: &'a B, 138 | mass: &'a Mass, 139 | material: &'a Material, 140 | ) -> Self { 141 | ResolveData { 142 | velocity, 143 | pose, 144 | mass, 145 | material, 146 | m: marker::PhantomData, 147 | } 148 | } 149 | } 150 | 151 | /// Perform contact resolution. 152 | /// 153 | /// Will compute any new poses and/or velocities, by doing impulse resolution of the given contact. 154 | /// 155 | /// ### Parameters: 156 | /// 157 | /// - `contact`: The contact; contact normal must point from shape A -> B 158 | /// - `a`: Resolution data for shape A 159 | /// - `b`: Resolution data for shape B 160 | /// 161 | /// ### Returns 162 | /// 163 | /// Tuple of change sets, first change set is for shape A, second change set for shape B. 164 | /// 165 | /// ### Type parameters: 166 | /// 167 | /// - `B`: Transform type (`BodyPose3` or similar) 168 | /// - `P`: Point type, usually `Point2` or `Point3` 169 | /// - `R`: Rotational quantity, usually `Basis2` or `Quaternion` 170 | /// - `A`: Angular velocity, usually `Scalar` or `Vector3` 171 | /// - `I`: Inertia, usually `Scalar` or `Matrix3` 172 | /// - `O`: Internal type used for unifying cross products for 2D/3D, usually `Scalar` or `Vector3` 173 | pub fn resolve_contact<'a, B, P, R, I, A, O>( 174 | contact: &Contact

, 175 | a: &ResolveData<'a, B, P, R, I, A>, 176 | b: &ResolveData<'a, B, P, R, I, A>, 177 | ) -> (SingleChangeSet, SingleChangeSet) 178 | where 179 | P: EuclideanSpace + 'a, 180 | P::Scalar: BaseFloat, 181 | R: Rotation

+ 'a, 182 | P::Diff: Debug + Zero + Clone + InnerSpace + PartialCrossProduct, 183 | O: PartialCrossProduct, 184 | A: PartialCrossProduct + Clone + Zero + 'a, 185 | &'a A: Sub + Add, 186 | I: Inertia + Mul, 187 | B: Pose + 'a, 188 | { 189 | let a_velocity = a.velocity.map(|v| v.value.clone()).unwrap_or_default(); 190 | let b_velocity = b.velocity.map(|v| v.value.clone()).unwrap_or_default(); 191 | let a_inverse_mass = a.mass.inverse_mass(); 192 | let b_inverse_mass = b.mass.inverse_mass(); 193 | let total_inverse_mass = a_inverse_mass + b_inverse_mass; 194 | 195 | // Do positional correction, so bodies aren't penetrating as much any longer. 196 | let (a_position_new, b_position_new) = 197 | positional_correction(contact, a.pose, b.pose, a_inverse_mass, b_inverse_mass); 198 | 199 | let mut a_set = SingleChangeSet::default(); 200 | a_set.add_pose(a_position_new); 201 | let mut b_set = SingleChangeSet::default(); 202 | b_set.add_pose(b_position_new); 203 | 204 | // This only happens when we have 2 infinite masses colliding. We only do positional correction 205 | // for the bodies and return early 206 | if total_inverse_mass == P::Scalar::zero() { 207 | return (a_set, b_set); 208 | } 209 | 210 | let r_a = contact.contact_point - a.pose.transform_point(P::origin()); 211 | let r_b = contact.contact_point - b.pose.transform_point(P::origin()); 212 | 213 | let p_a_dot = *a_velocity.linear() + a_velocity.angular().cross(&r_a); 214 | let p_b_dot = *b_velocity.linear() + b_velocity.angular().cross(&r_b); 215 | 216 | let rv = p_b_dot - p_a_dot; 217 | let velocity_along_normal = contact.normal.dot(rv); 218 | 219 | // Check if shapes are already separating, if so only do positional correction 220 | if velocity_along_normal > P::Scalar::zero() { 221 | return (a_set, b_set); 222 | } 223 | 224 | // Compute impulse force 225 | let a_res: P::Scalar = a.material.restitution(); 226 | let b_res: P::Scalar = b.material.restitution(); 227 | let e = a_res.min(b_res); 228 | let numerator = -(P::Scalar::one() + e) * velocity_along_normal; 229 | 230 | let a_tensor = a.mass.world_inverse_inertia(&a.pose.rotation()); 231 | let b_tensor = b.mass.world_inverse_inertia(&b.pose.rotation()); 232 | 233 | let term3 = contact 234 | .normal 235 | .dot((a_tensor * (r_a.cross(&contact.normal))).cross(&r_a)); 236 | let term4 = contact 237 | .normal 238 | .dot((b_tensor * (r_b.cross(&contact.normal))).cross(&r_b)); 239 | 240 | let j = numerator / (a_inverse_mass + b_inverse_mass + term3 + term4); 241 | let impulse = contact.normal * j; 242 | 243 | // Compute new velocities based on mass and the computed impulse 244 | let a_velocity_new = a.velocity.map(|v| NextFrame { 245 | value: Velocity::new( 246 | *v.value.linear() - impulse * a_inverse_mass, 247 | v.value.angular() - a_tensor * r_a.cross(&impulse), 248 | ), 249 | }); 250 | 251 | let b_velocity_new = b.velocity.map(|v| NextFrame { 252 | value: Velocity::new( 253 | *v.value.linear() + impulse * b_inverse_mass, 254 | v.value.angular() + b_tensor * r_b.cross(&impulse), 255 | ), 256 | }); 257 | 258 | a_set.add_velocity(a_velocity_new); 259 | b_set.add_velocity(b_velocity_new); 260 | 261 | (a_set, b_set) 262 | } 263 | 264 | /// Do positional correction for colliding bodies. 265 | /// 266 | /// Will only do correction for a percentage of the penetration depth, to avoid stability issues. 267 | /// 268 | /// ### Parameters: 269 | /// 270 | /// - `contact`: Contact information, normal must point from A -> B 271 | /// - `a_position`: World coordinates of center of mass for body A 272 | /// - `b_position`: World coordinates of center of mass for body B 273 | /// - `a_inverse_mass`: Inverse mass of body A 274 | /// - `b_inverse_mass`: Inverse mass of body B 275 | /// 276 | /// ### Returns: 277 | /// 278 | /// Tuple with new positions for the given bodies 279 | /// 280 | /// ### Type parameters: 281 | /// 282 | /// - `S`: Scalar type 283 | /// - `B`: Transform type (`BodyPose3` or similar) 284 | /// - `P`: Positional quantity, usually `Point2` or `Point3` 285 | /// - `R`: Rotational quantity, usually `Basis2` or `Quaternion` 286 | fn positional_correction( 287 | contact: &Contact

, 288 | a_position: &B, 289 | b_position: &B, 290 | a_inverse_mass: S, 291 | b_inverse_mass: S, 292 | ) -> (Option, Option) 293 | where 294 | S: BaseFloat, 295 | P: EuclideanSpace, 296 | R: Rotation

, 297 | P::Diff: Debug + Zero + Clone + InnerSpace, 298 | B: Pose, 299 | { 300 | let total_inverse_mass = a_inverse_mass + b_inverse_mass; 301 | let k_slop: S = NumCast::from(POSITIONAL_CORRECTION_K_SLOP).unwrap(); 302 | let percent: S = NumCast::from(POSITIONAL_CORRECTION_PERCENT).unwrap(); 303 | let correction_penetration_depth = contact.penetration_depth - k_slop; 304 | let correction_magnitude = 305 | correction_penetration_depth.max(S::zero()) / total_inverse_mass * percent; 306 | let correction = contact.normal * correction_magnitude; 307 | let a_position_new = new_pose(a_position, correction * -a_inverse_mass); 308 | let b_position_new = new_pose(b_position, correction * b_inverse_mass); 309 | (Some(a_position_new), Some(b_position_new)) 310 | } 311 | 312 | fn new_pose(next_frame: &B, correction: P::Diff) -> B 313 | where 314 | P: EuclideanSpace, 315 | P::Scalar: BaseFloat, 316 | R: Rotation

, 317 | P::Diff: Clone, 318 | B: Pose, 319 | { 320 | B::new(next_frame.position() + correction, next_frame.rotation()) 321 | } 322 | 323 | #[cfg(test)] 324 | mod tests { 325 | use cgmath::{Basis2, One, Point2, Vector2}; 326 | use collision::{CollisionStrategy, Contact}; 327 | 328 | use super::*; 329 | use collide::ContactEvent; 330 | use BodyPose; 331 | 332 | #[test] 333 | fn test_resolve_2d_f32() { 334 | let mass = Mass::::new_with_inertia(0.5, 0.); 335 | let material = Material::default(); 336 | let left_velocity = NextFrame { 337 | value: Velocity::new(Vector2::::new(1., 0.), 0.), 338 | }; 339 | let left_pose = BodyPose::new(Point2::origin(), Basis2::one()); 340 | let right_velocity = NextFrame { 341 | value: Velocity::new(Vector2::new(-2., 0.), 0.), 342 | }; 343 | let right_pose = BodyPose::new(Point2::new(1., 0.), Basis2::one()); 344 | let contact = ContactEvent::new( 345 | (1, 2), 346 | Contact::new_impl(CollisionStrategy::FullResolution, Vector2::new(1., 0.), 0.5), 347 | ); 348 | let set = resolve_contact( 349 | &contact.contact, 350 | &ResolveData::new(Some(&left_velocity), &left_pose, &mass, &material), 351 | &ResolveData::new(Some(&right_velocity), &right_pose, &mass, &material), 352 | ); 353 | assert_eq!( 354 | ( 355 | SingleChangeSet::new( 356 | Some(BodyPose::new( 357 | Point2::new(-0.04900000075250864, 0.), 358 | Basis2::one() 359 | )), 360 | Some(NextFrame { 361 | value: Velocity::new(Vector2::new(-2., 0.), 0.), 362 | }), 363 | ), 364 | SingleChangeSet::new( 365 | Some(BodyPose::new( 366 | Point2::new(1.0490000007525087, 0.), 367 | Basis2::one() 368 | )), 369 | Some(NextFrame { 370 | value: Velocity::new(Vector2::new(1., 0.), 0.), 371 | }), 372 | ) 373 | ), 374 | set 375 | ); 376 | } 377 | 378 | #[test] 379 | fn test_resolve_2d_f64() { 380 | let mass = Mass::::new_with_inertia(0.5, 0.); 381 | let material = Material::default(); 382 | let left_velocity = NextFrame { 383 | value: Velocity::new(Vector2::::new(1., 0.), 0.), 384 | }; 385 | let left_pose = BodyPose::new(Point2::origin(), Basis2::one()); 386 | let right_velocity = NextFrame { 387 | value: Velocity::new(Vector2::new(-2., 0.), 0.), 388 | }; 389 | let right_pose = BodyPose::new(Point2::new(1., 0.), Basis2::one()); 390 | let contact = ContactEvent::new( 391 | (1, 2), 392 | Contact::new_impl(CollisionStrategy::FullResolution, Vector2::new(1., 0.), 0.5), 393 | ); 394 | let set = resolve_contact( 395 | &contact.contact, 396 | &ResolveData::new(Some(&left_velocity), &left_pose, &mass, &material), 397 | &ResolveData::new(Some(&right_velocity), &right_pose, &mass, &material), 398 | ); 399 | assert_eq!( 400 | ( 401 | SingleChangeSet::new( 402 | Some(BodyPose::new( 403 | Point2::new(-0.04900000075250864, 0.), 404 | Basis2::one() 405 | )), 406 | Some(NextFrame { 407 | value: Velocity::new(Vector2::new(-2., 0.), 0.), 408 | }), 409 | ), 410 | SingleChangeSet::new( 411 | Some(BodyPose::new( 412 | Point2::new(1.0490000007525087, 0.), 413 | Basis2::one() 414 | )), 415 | Some(NextFrame { 416 | value: Velocity::new(Vector2::new(1., 0.), 0.), 417 | }), 418 | ) 419 | ), 420 | set 421 | ); 422 | } 423 | } 424 | -------------------------------------------------------------------------------- /rhusics-core/src/physics/simple.rs: -------------------------------------------------------------------------------- 1 | //! Simple force integration and impulse solver 2 | 3 | use std::ops::Mul; 4 | 5 | use cgmath::num_traits::Float; 6 | use cgmath::{BaseFloat, EuclideanSpace, InnerSpace, Rotation, VectorSpace, Zero}; 7 | 8 | use super::{ 9 | ApplyAngular, ForceAccumulator, Inertia, Mass, PhysicalEntity, Velocity, WorldParameters, 10 | }; 11 | use {NextFrame, Pose}; 12 | 13 | /// Do force integration for next frame. 14 | /// 15 | /// ### Parameters: 16 | /// 17 | /// - `data`: Iterator over tuple with: 18 | /// - Velocity for the next frame, will be updated 19 | /// - Pose for the next frame, used to compute the inertia tensor for the body in the next frame 20 | /// - Force accumulator, will be consumed and added to the velocity 21 | /// - Mass, used by integration 22 | /// - PhysicalEntity, used for gravity and damping calculation 23 | /// - `world_params`: World physics parameters like gravity and global damping 24 | /// - `dt`: Time step 25 | /// 26 | /// ### Type parameters: 27 | /// 28 | /// - `D`: Iterator type 29 | /// - `P`: Point, usually `Point2` or `Point3` 30 | /// - `A`: Angular velocity, usually `Scalar` or `Vector3` 31 | /// - `I`: Inertia, usually `Scalar` or `Matrix3` 32 | /// - `R`: Rotational quantity, usually `Basis2` or `Quaternion` 33 | pub fn next_frame_integration<'a, T, D, P, A, I, R>( 34 | data: D, 35 | world_params: &WorldParameters, 36 | dt: P::Scalar, 37 | ) where 38 | D: Iterator< 39 | Item = ( 40 | &'a mut NextFrame>, 41 | &'a NextFrame, 42 | &'a mut ForceAccumulator, 43 | &'a Mass, 44 | &'a PhysicalEntity, 45 | ), 46 | >, 47 | T: Pose + 'a, 48 | P: EuclideanSpace + 'a, 49 | P::Scalar: BaseFloat, 50 | P::Diff: VectorSpace + InnerSpace + 'a, 51 | I: Inertia + Mul + 'a, 52 | A: Mul + Clone + Copy + Zero + 'a, 53 | R: Rotation

+ ApplyAngular + 'a, 54 | { 55 | // Do force integration 56 | for (next_velocity, next_pose, force, mass, entity) in data.filter(|(_, _, _, _, e)| e.active()) 57 | { 58 | let a = force.consume_force() * mass.inverse_mass() 59 | + world_params.gravity() * entity.gravity_scale(); 60 | let new_velocity = *next_velocity.value.linear() + a * dt; 61 | let damp = world_params.entity_damping(entity.damping()).powf(dt); 62 | next_velocity.value.set_linear(new_velocity * damp); 63 | let a = mass.world_inverse_inertia(&next_pose.value.rotation()) * force.consume_torque(); 64 | let new_velocity = *next_velocity.value.angular() + a * dt; 65 | next_velocity.value.set_angular(new_velocity); 66 | } 67 | } 68 | 69 | /// Compute next frame pose 70 | /// 71 | /// ### Parameters: 72 | /// 73 | /// - `data`: Iterator over tuple with: 74 | /// - Velocity for the next frame, used to compute next frame pose 75 | /// - Pose for the current frame, will be updated 76 | /// - Pose for the next frame, will be updated 77 | /// - `dt`: Time step 78 | /// 79 | /// ### Type parameters: 80 | /// 81 | /// - `D`: Iterator type 82 | /// - `P`: Point, usually `Point2` or `Point3` 83 | /// - `A`: Angular velocity, usually `Scalar` or `Vector3` 84 | /// - `R`: Rotational quantity, usually `Basis2` or `Quaternion` 85 | pub fn next_frame_pose<'a, B, D, P, A, R>(data: D, dt: P::Scalar) 86 | where 87 | D: Iterator< 88 | Item = ( 89 | &'a NextFrame>, 90 | &'a B, 91 | &'a mut NextFrame, 92 | &'a PhysicalEntity, 93 | ), 94 | >, 95 | B: Pose + 'a, 96 | P: EuclideanSpace + 'a, 97 | P::Scalar: BaseFloat, 98 | P::Diff: VectorSpace + InnerSpace + 'a, 99 | A: Mul + Clone + Copy + Zero + 'a, 100 | R: Rotation

+ ApplyAngular + 'a, 101 | { 102 | for (next_velocity, pose, next_pose, _) in data.filter(|(_, _, _, e)| e.active()) { 103 | next_pose.value = next_velocity.value.apply(pose, dt) 104 | } 105 | } 106 | -------------------------------------------------------------------------------- /rhusics-core/src/physics/util.rs: -------------------------------------------------------------------------------- 1 | use cgmath::{BaseFloat, Vector2, Vector3}; 2 | 3 | /// Cross product abstraction 4 | pub trait PartialCrossProduct { 5 | /// Output 6 | type Output; 7 | /// Compute cross product 8 | fn cross(&self, other: &RHS) -> Self::Output; 9 | } 10 | 11 | impl PartialCrossProduct> for S 12 | where 13 | S: BaseFloat, 14 | { 15 | type Output = Vector2; 16 | 17 | fn cross(&self, other: &Vector2) -> Self::Output { 18 | Vector2::new(-*self * other.y, *self * other.x) 19 | } 20 | } 21 | 22 | impl PartialCrossProduct for Vector2 23 | where 24 | S: BaseFloat, 25 | { 26 | type Output = S; 27 | fn cross(&self, other: &Vector2) -> S { 28 | self.x * other.y - self.y * other.x 29 | } 30 | } 31 | 32 | impl PartialCrossProduct for Vector3 33 | where 34 | S: BaseFloat, 35 | { 36 | type Output = Vector3; 37 | fn cross(&self, other: &Vector3) -> Vector3 { 38 | Vector3::cross(*self, *other) 39 | } 40 | } 41 | -------------------------------------------------------------------------------- /rhusics-core/src/physics/velocity.rs: -------------------------------------------------------------------------------- 1 | use cgmath::{ 2 | BaseFloat, Basis2, EuclideanSpace, Euler, Quaternion, Rad, Rotation, Rotation2, Vector3, 3 | VectorSpace, Zero, 4 | }; 5 | 6 | use Pose; 7 | 8 | /// Velocity 9 | /// 10 | /// ### Type parameters: 11 | /// 12 | /// - `L`: Linear velocity, usually `Vector2` or `Vector3` 13 | /// - `A`: Angular velocity, usually `Scalar` or `Vector3` 14 | #[derive(Debug, Clone, PartialEq)] 15 | #[cfg_attr(feature = "serializable", derive(Serialize, Deserialize))] 16 | pub struct Velocity { 17 | linear: L, 18 | angular: A, 19 | } 20 | 21 | impl Default for Velocity 22 | where 23 | L: Zero, 24 | A: Clone + Zero, 25 | { 26 | fn default() -> Self { 27 | Self::new(L::zero(), A::zero()) 28 | } 29 | } 30 | 31 | impl Velocity 32 | where 33 | L: Zero, 34 | A: Clone + Zero, 35 | { 36 | /// Create new velocity object, with both linear and angular velocity 37 | pub fn new(linear: L, angular: A) -> Self { 38 | Self { linear, angular } 39 | } 40 | 41 | /// Create new velocity object with only linear velocity 42 | pub fn from_linear(linear: L) -> Self { 43 | Self::new(linear, A::zero()) 44 | } 45 | 46 | /// Set linear velocity 47 | pub fn set_linear(&mut self, linear: L) { 48 | self.linear = linear; 49 | } 50 | 51 | /// Get linear velocity 52 | pub fn linear(&self) -> &L { 53 | &self.linear 54 | } 55 | 56 | /// Set angular velocity 57 | pub fn set_angular(&mut self, angular: A) { 58 | self.angular = angular; 59 | } 60 | 61 | /// Get angular velocity 62 | pub fn angular(&self) -> &A { 63 | &self.angular 64 | } 65 | 66 | /// Apply velocity to pose. 67 | /// 68 | /// ### Parameters: 69 | /// 70 | /// - `pose`: Pose to apply the velocity to 71 | /// - `dt`: Time step 72 | /// 73 | /// ### Type parameters: 74 | /// 75 | /// - `B`: Transform type (`BodyPose3` or similar) 76 | /// - `P`: Positional quantity, usually `Point2` or `Point3` 77 | /// - `R`: Rotational quantity, usually `Basis2` or `Quaternion` 78 | pub fn apply(&self, pose: &B, dt: L::Scalar) -> B 79 | where 80 | P: EuclideanSpace, 81 | L: VectorSpace, 82 | L::Scalar: BaseFloat, 83 | R: ApplyAngular + Rotation

, 84 | B: Pose, 85 | { 86 | B::new( 87 | self.apply_linear(pose.position(), dt), 88 | self.apply_angular(pose.rotation(), dt), 89 | ) 90 | } 91 | 92 | /// Apply linear velocity to a positional quantity 93 | /// 94 | /// ### Parameters: 95 | /// 96 | /// - `linear`: Positional value 97 | /// - `dt`: Time step 98 | /// 99 | /// ### Type parameters: 100 | /// 101 | /// - `P`: Positional quantity, usually `Point2` or `Point3` 102 | pub fn apply_linear

(&self, linear: P, dt: L::Scalar) -> P 103 | where 104 | P: EuclideanSpace, 105 | L::Scalar: BaseFloat, 106 | L: VectorSpace, 107 | { 108 | linear + self.linear * dt 109 | } 110 | 111 | /// Apply angular velocity to a rotational quantity 112 | /// 113 | /// ### Parameters: 114 | /// 115 | /// - `rotation`: Rotational value 116 | /// - `dt`: Time step 117 | /// 118 | /// ### Type parameters: 119 | /// 120 | /// - `R`: Rotational quantity, usually `Basis2` or `Quaternion` 121 | pub fn apply_angular(&self, rotation: R, dt: L::Scalar) -> R 122 | where 123 | R: ApplyAngular, 124 | L: VectorSpace, 125 | L::Scalar: BaseFloat, 126 | { 127 | rotation.apply(&self.angular, dt) 128 | } 129 | } 130 | 131 | /// Apply an angular velocity to a rotational quantity 132 | /// 133 | /// ### Type parameters: 134 | /// 135 | /// - `A`: Angular velocity, usually `Scalar` or `Vector3` 136 | pub trait ApplyAngular { 137 | /// Apply given velocity 138 | fn apply(&self, velocity: &A, dt: S) -> Self; 139 | } 140 | 141 | impl ApplyAngular for S 142 | where 143 | S: BaseFloat, 144 | { 145 | fn apply(&self, velocity: &S, dt: S) -> Self { 146 | *self + *velocity * dt 147 | } 148 | } 149 | 150 | impl ApplyAngular for Basis2 151 | where 152 | S: BaseFloat, 153 | { 154 | fn apply(&self, velocity: &S, dt: S) -> Self { 155 | *self * Basis2::from_angle(Rad(*velocity * dt)) 156 | } 157 | } 158 | 159 | impl ApplyAngular> for Quaternion 160 | where 161 | S: BaseFloat, 162 | { 163 | fn apply(&self, velocity: &Vector3, dt: S) -> Self { 164 | self * Quaternion::from(Euler { 165 | x: Rad(velocity.x * dt), 166 | y: Rad(velocity.y * dt), 167 | z: Rad(velocity.z * dt), 168 | }) 169 | } 170 | } 171 | 172 | #[cfg(test)] 173 | mod tests_f32 { 174 | use cgmath::{Basis2, Point2, Point3, Rad, Rotation2, Rotation3, Transform, Vector2}; 175 | 176 | use super::*; 177 | use physics2d::BodyPose2; 178 | use physics3d::BodyPose3; 179 | 180 | #[test] 181 | fn test_velocity_linear() { 182 | let velocity = Velocity::new(Vector2::new(1., 1.), 0.); 183 | let pose = Point2::::new(0., 0.); 184 | let pose = velocity.apply_linear(pose, 0.1); 185 | assert_eq!(Point2::new(0.1, 0.1), pose); 186 | } 187 | 188 | #[test] 189 | fn test_velocity_2d_angular() { 190 | let velocity = Velocity::new(Vector2::new(1., 1.), 1.); 191 | let orientation = Basis2::::from_angle(Rad(0.)); 192 | let orientation = velocity.apply_angular(orientation, 0.1); 193 | assert_eq!(Basis2::from_angle(Rad(0.1)), orientation); 194 | } 195 | 196 | #[test] 197 | fn test_velocity_3d_angular() { 198 | let velocity = Velocity::new(Vector3::new(1., 1., 1.), Vector3::new(0., 1., 0.)); 199 | 200 | let orientation = Quaternion::::from_angle_y(Rad(0.)); 201 | let orientation = velocity.apply_angular(orientation, 0.1); 202 | assert_eq!(Quaternion::from_angle_y(Rad(0.1)), orientation); 203 | } 204 | 205 | #[test] 206 | fn test_velocity_full_2d() { 207 | let velocity = Velocity::new(Vector2::new(1., 1.), 1.); 208 | let pose = BodyPose2::::one(); 209 | let pose = velocity.apply(&pose, 0.1); 210 | assert_eq!(Point2::new(0.1, 0.1), pose.position()); 211 | assert_eq!(Basis2::from_angle(Rad(0.1)), pose.rotation()); 212 | } 213 | 214 | #[test] 215 | fn test_velocity_full_3d() { 216 | let velocity = Velocity::new(Vector3::new(1., 1., 1.), Vector3::new(0., 1., 0.)); 217 | let pose = BodyPose3::::one(); 218 | let pose = velocity.apply(&pose, 0.1); 219 | assert_eq!(Point3::new(0.1, 0.1, 0.1), pose.position()); 220 | assert_eq!(Quaternion::from_angle_y(Rad(0.1)), pose.rotation()); 221 | } 222 | 223 | #[test] 224 | fn test_apply_angular_basis2() { 225 | let orientation = Basis2::::from_angle(Rad(0.)); 226 | let velocity = 0.5; 227 | let orientation = orientation.apply(&velocity, 0.1); 228 | let orientation = orientation.apply(&velocity, 0.1); 229 | let orientation = orientation.apply(&velocity, 0.1); 230 | let orientation = orientation.apply(&velocity, 0.1); 231 | 232 | assert_ulps_eq!(Basis2::from_angle(Rad(0.2)), orientation); 233 | } 234 | 235 | #[test] 236 | fn test_apply_angular_real() { 237 | let orientation: f32 = 0.; 238 | let velocity = 0.5; 239 | let orientation = orientation.apply(&velocity, 0.1); 240 | let orientation = orientation.apply(&velocity, 0.1); 241 | let orientation = orientation.apply(&velocity, 0.1); 242 | let orientation = orientation.apply(&velocity, 0.1); 243 | 244 | assert_eq!(0.2, orientation); 245 | } 246 | 247 | #[test] 248 | fn test_apply_angular_quat() { 249 | let orientation = Quaternion::::from_angle_x(Rad(0.)); 250 | let velocity = Vector3::new(0.5, 0., 0.); 251 | let orientation = orientation.apply(&velocity, 0.1); 252 | let orientation = orientation.apply(&velocity, 0.1); 253 | let orientation = orientation.apply(&velocity, 0.1); 254 | let orientation = orientation.apply(&velocity, 0.1); 255 | 256 | assert_ulps_eq!(Quaternion::from_angle_x(Rad(0.2)), orientation); 257 | } 258 | } 259 | 260 | #[cfg(test)] 261 | mod tests_f64 { 262 | use cgmath::{Basis2, Point2, Point3, Rad, Rotation2, Rotation3, Transform, Vector2}; 263 | 264 | use super::*; 265 | use physics2d::BodyPose2; 266 | use physics3d::BodyPose3; 267 | 268 | #[test] 269 | fn test_velocity_linear() { 270 | let velocity = Velocity::new(Vector2::new(1., 1.), 0.); 271 | let pose = Point2::::new(0., 0.); 272 | let pose = velocity.apply_linear(pose, 0.1); 273 | assert_eq!(Point2::new(0.1, 0.1), pose); 274 | } 275 | 276 | #[test] 277 | fn test_velocity_2d_angular() { 278 | let velocity = Velocity::new(Vector2::new(1., 1.), 1.); 279 | let orientation = Basis2::::from_angle(Rad(0.)); 280 | let orientation = velocity.apply_angular(orientation, 0.1); 281 | assert_eq!(Basis2::from_angle(Rad(0.1)), orientation); 282 | } 283 | 284 | #[test] 285 | fn test_velocity_3d_angular() { 286 | let velocity = Velocity::new(Vector3::new(1., 1., 1.), Vector3::new(0., 1., 0.)); 287 | 288 | let orientation = Quaternion::::from_angle_y(Rad(0.)); 289 | let orientation = velocity.apply_angular(orientation, 0.1); 290 | assert_eq!(Quaternion::from_angle_y(Rad(0.1)), orientation); 291 | } 292 | 293 | #[test] 294 | fn test_velocity_full_2d() { 295 | let velocity = Velocity::new(Vector2::new(1., 1.), 1.); 296 | let pose = BodyPose2::::one(); 297 | let pose = velocity.apply(&pose, 0.1); 298 | assert_eq!(Point2::new(0.1, 0.1), pose.position()); 299 | assert_eq!(Basis2::from_angle(Rad(0.1)), pose.rotation()); 300 | } 301 | 302 | #[test] 303 | fn test_velocity_full_3d() { 304 | let velocity = Velocity::new(Vector3::new(1., 1., 1.), Vector3::new(0., 1., 0.)); 305 | let pose = BodyPose3::::one(); 306 | let pose = velocity.apply(&pose, 0.1); 307 | assert_eq!(Point3::new(0.1, 0.1, 0.1), pose.position()); 308 | assert_eq!(Quaternion::from_angle_y(Rad(0.1)), pose.rotation()); 309 | } 310 | 311 | #[test] 312 | fn test_apply_angular_basis2() { 313 | let orientation = Basis2::::from_angle(Rad(0.)); 314 | let velocity = 0.5; 315 | let orientation = orientation.apply(&velocity, 0.1); 316 | let orientation = orientation.apply(&velocity, 0.1); 317 | let orientation = orientation.apply(&velocity, 0.1); 318 | let orientation = orientation.apply(&velocity, 0.1); 319 | 320 | assert_ulps_eq!(Basis2::from_angle(Rad(0.2)), orientation); 321 | } 322 | 323 | #[test] 324 | fn test_apply_angular_real() { 325 | let orientation: f64 = 0.; 326 | let velocity = 0.5; 327 | let orientation = orientation.apply(&velocity, 0.1); 328 | let orientation = orientation.apply(&velocity, 0.1); 329 | let orientation = orientation.apply(&velocity, 0.1); 330 | let orientation = orientation.apply(&velocity, 0.1); 331 | 332 | assert_eq!(0.2, orientation); 333 | } 334 | 335 | #[test] 336 | fn test_apply_angular_quat() { 337 | let orientation = Quaternion::::from_angle_x(Rad(0.)); 338 | let velocity = Vector3::new(0.5, 0., 0.); 339 | let orientation = orientation.apply(&velocity, 0.1); 340 | let orientation = orientation.apply(&velocity, 0.1); 341 | let orientation = orientation.apply(&velocity, 0.1); 342 | let orientation = orientation.apply(&velocity, 0.1); 343 | 344 | assert_ulps_eq!(Quaternion::from_angle_x(Rad(0.2)), orientation); 345 | } 346 | } 347 | -------------------------------------------------------------------------------- /rhusics-core/src/physics/volumes.rs: -------------------------------------------------------------------------------- 1 | use cgmath::{ 2 | BaseFloat, EuclideanSpace, InnerSpace, Matrix3, Point2, Point3, SquareMatrix, Transform, 3 | Vector3, Zero, 4 | }; 5 | use collision::primitive::*; 6 | use collision::{Aabb, Aabb2, Aabb3, Bound, ComputeBound, Primitive, Union}; 7 | 8 | use super::{Inertia, Mass, Material, PartialCrossProduct}; 9 | use collide::CollisionShape; 10 | 11 | /// Describe a shape with volume 12 | /// 13 | /// ### Type parameters: 14 | /// 15 | /// - `I`: Inertia type, see `Inertia` for more information 16 | pub trait Volume { 17 | /// Compute the mass of the shape based on its material 18 | fn get_mass(&self, material: &Material) -> Mass; 19 | } 20 | 21 | impl Volume for Circle 22 | where 23 | S: BaseFloat + Inertia, 24 | { 25 | fn get_mass(&self, material: &Material) -> Mass { 26 | use std::f64::consts::PI; 27 | let pi = S::from(PI).unwrap(); 28 | let mass = pi * self.radius * self.radius * material.density(); 29 | let inertia = mass * self.radius * self.radius / (S::one() + S::one()); 30 | Mass::new_with_inertia(mass, inertia) 31 | } 32 | } 33 | 34 | impl Volume for Rectangle 35 | where 36 | S: BaseFloat + Inertia, 37 | { 38 | fn get_mass(&self, material: &Material) -> Mass { 39 | let b: Aabb2 = self.compute_bound(); 40 | let mass = b.volume() * material.density(); 41 | let inertia = 42 | mass * (b.dim().x * b.dim().x + b.dim().y * b.dim().y) / S::from(12.).unwrap(); 43 | Mass::new_with_inertia(mass, inertia) 44 | } 45 | } 46 | 47 | impl Volume for Square 48 | where 49 | S: BaseFloat + Inertia, 50 | { 51 | fn get_mass(&self, material: &Material) -> Mass { 52 | let b: Aabb2 = self.compute_bound(); 53 | let mass = b.volume() * material.density(); 54 | let inertia = 55 | mass * (b.dim().x * b.dim().x + b.dim().y * b.dim().y) / S::from(12.).unwrap(); 56 | Mass::new_with_inertia(mass, inertia) 57 | } 58 | } 59 | 60 | impl Volume for ConvexPolygon 61 | where 62 | S: BaseFloat + Inertia, 63 | { 64 | fn get_mass(&self, material: &Material) -> Mass { 65 | let mut area = S::zero(); 66 | let mut denom = S::zero(); 67 | for i in 0..self.vertices.len() { 68 | let j = if i == self.vertices.len() - 1 { 69 | 0 70 | } else { 71 | i + 1 72 | }; 73 | let p0 = self.vertices[i].to_vec(); 74 | let p1 = self.vertices[j].to_vec(); 75 | let a = p0.cross(&p1).abs(); 76 | let b = p0.dot(p0) + p0.dot(p1) + p1.dot(p1); 77 | denom += a * b; 78 | area += a; 79 | } 80 | let mass = area * S::from(0.5).unwrap() * material.density(); 81 | let inertia = mass / S::from(6.).unwrap() * denom / area; 82 | Mass::new_with_inertia(mass, inertia) 83 | } 84 | } 85 | 86 | impl Volume> for Sphere 87 | where 88 | S: BaseFloat, 89 | { 90 | fn get_mass(&self, material: &Material) -> Mass> { 91 | use std::f64::consts::PI; 92 | let pi = S::from(PI).unwrap(); 93 | let mass = S::from(4. / 3.).unwrap() 94 | * pi 95 | * self.radius 96 | * self.radius 97 | * self.radius 98 | * material.density(); 99 | let inertia = S::from(2. / 5.).unwrap() * mass * self.radius * self.radius; 100 | Mass::new_with_inertia(mass, Matrix3::from_value(inertia)) 101 | } 102 | } 103 | 104 | impl Volume> for Cuboid 105 | where 106 | S: BaseFloat, 107 | { 108 | fn get_mass(&self, material: &Material) -> Mass> { 109 | let b: Aabb3 = self.compute_bound(); 110 | let mass = b.volume() * material.density(); 111 | let x2 = b.dim().x * b.dim().x; 112 | let y2 = b.dim().y * b.dim().y; 113 | let z2 = b.dim().z * b.dim().z; 114 | let mnorm = mass / S::from(12.).unwrap(); 115 | let inertia = Matrix3::from_diagonal(Vector3::new(y2 + z2, x2 + z2, x2 + y2) * mnorm); 116 | Mass::new_with_inertia(mass, inertia) 117 | } 118 | } 119 | 120 | impl Volume> for Cube 121 | where 122 | S: BaseFloat, 123 | { 124 | fn get_mass(&self, material: &Material) -> Mass> { 125 | let b: Aabb3 = self.compute_bound(); 126 | let mass = b.volume() * material.density(); 127 | let x2 = b.dim().x * b.dim().x; 128 | let y2 = b.dim().y * b.dim().y; 129 | let z2 = b.dim().z * b.dim().z; 130 | let mnorm = mass / S::from(12.).unwrap(); 131 | let inertia = Matrix3::from_diagonal(Vector3::new(y2 + z2, x2 + z2, x2 + y2) * mnorm); 132 | Mass::new_with_inertia(mass, inertia) 133 | } 134 | } 135 | 136 | fn poly_sub_expr_calc(w0: S, w1: S, w2: S) -> (S, S, S, S, S, S) 137 | where 138 | S: BaseFloat, 139 | { 140 | let t0 = w0 + w1; 141 | let t1 = w0 * w0; 142 | let t2 = t1 + t0 * w1; 143 | let f1 = t0 + w2; 144 | let f2 = t2 + w2 * f1; 145 | let f3 = w0 * t0 + w1 * t2 + w2 * f2; 146 | ( 147 | f1, 148 | f2, 149 | f3, 150 | f2 + w0 * (f1 + w0), 151 | f2 + w1 * (f1 + w1), 152 | f2 + w2 * (f1 + w2), 153 | ) 154 | } 155 | 156 | const ONE_6: f64 = 1. / 6.; 157 | const ONE_24: f64 = 1. / 24.; 158 | const ONE_60: f64 = 1. / 60.; 159 | const ONE_120: f64 = 1. / 120.; 160 | const POLY_SCALE: [f64; 10] = [ 161 | ONE_6, ONE_24, ONE_24, ONE_24, ONE_60, ONE_60, ONE_60, ONE_120, ONE_120, ONE_120, 162 | ]; 163 | 164 | impl Volume> for ConvexPolyhedron 165 | where 166 | S: BaseFloat, 167 | { 168 | // Volume of tetrahedron is 1/6 * a.cross(b).dot(c) where a = B - C, b = A - C, c = Origin - C 169 | // Sum for all faces 170 | fn get_mass(&self, material: &Material) -> Mass> { 171 | let mut intg: [S; 10] = [S::zero(); 10]; 172 | for (p0, p1, p2) in self.faces_iter() { 173 | let v1 = p1 - p0; // a1, b1, c1 174 | let v2 = p2 - p0; // a2, b2, c2 175 | let d = v1.cross(v2); // d0, d1, d2 176 | let (f1x, f2x, f3x, g0x, g1x, g2x) = poly_sub_expr_calc(p0.x, p1.x, p2.x); 177 | let (_, f2y, f3y, g0y, g1y, g2y) = poly_sub_expr_calc(p0.y, p1.y, p2.y); 178 | let (_, f2z, f3z, g0z, g1z, g2z) = poly_sub_expr_calc(p0.z, p1.z, p2.z); 179 | intg[0] += d.x * f1x; 180 | intg[1] += d.x * f2x; 181 | intg[2] += d.y * f2y; 182 | intg[3] += d.z * f2z; 183 | intg[4] += d.x * f3x; 184 | intg[5] += d.y * f3y; 185 | intg[6] += d.z * f3z; 186 | intg[7] += d.x * (p0.y * g0x + p1.y * g1x + p2.y * g2x); 187 | intg[8] += d.y * (p0.z * g0y + p1.z * g1y + p2.z * g2y); 188 | intg[9] += d.z * (p0.x * g0z + p1.x * g1z + p2.x * g2z); 189 | } 190 | for i in 0..10 { 191 | intg[i] *= S::from(POLY_SCALE[i]).unwrap(); 192 | } 193 | let cm = Point3::new(intg[1] / intg[0], intg[2] / intg[0], intg[3] / intg[0]); 194 | let mut inertia = Matrix3::zero(); 195 | inertia.x.x = intg[5] + intg[6] - intg[0] * (cm.y * cm.y + cm.z * cm.z); 196 | inertia.y.y = intg[4] + intg[6] - intg[0] * (cm.x * cm.x + cm.z * cm.z); 197 | inertia.z.z = intg[4] + intg[5] - intg[0] * (cm.x * cm.x + cm.y * cm.y); 198 | inertia.x.y = -(intg[7] - intg[0] * cm.x * cm.y); 199 | inertia.y.z = -(intg[8] - intg[0] * cm.y * cm.z); 200 | inertia.x.z = -(intg[9] - intg[0] * cm.x * cm.z); 201 | Mass::new_with_inertia( 202 | intg[0] * material.density(), 203 | inertia * material.density::(), 204 | ) 205 | } 206 | } 207 | 208 | impl Volume for Primitive2 209 | where 210 | S: BaseFloat + Inertia, 211 | { 212 | fn get_mass(&self, material: &Material) -> Mass { 213 | use collision::primitive::Primitive2::*; 214 | match *self { 215 | Particle(_) | Line(_) => Mass::new(material.density()), 216 | Circle(ref circle) => circle.get_mass(material), 217 | Rectangle(ref rectangle) => rectangle.get_mass(material), 218 | Square(ref square) => square.get_mass(material), 219 | ConvexPolygon(ref polygon) => polygon.get_mass(material), 220 | } 221 | } 222 | } 223 | 224 | impl Volume> for Capsule 225 | where 226 | S: BaseFloat, 227 | { 228 | fn get_mass(&self, material: &Material) -> Mass> { 229 | use std::f64::consts::PI; 230 | let pi = S::from(PI).unwrap(); 231 | let rsq = self.radius() * self.radius(); 232 | let hsq = self.height() * self.height(); 233 | let two = S::one() + S::one(); 234 | let three = two + S::one(); 235 | let four = two + two; 236 | let five = three + two; 237 | let eight = five + three; 238 | let twelve = eight + four; 239 | let c_m = pi * rsq * self.height() * material.density(); 240 | let h_m = pi * two / three * rsq * self.radius() * material.density(); 241 | let mass = c_m + two * h_m; 242 | let c_i_xz = hsq / twelve + rsq / four; 243 | let h_i_xz = rsq * two / five + hsq / two + self.height() * self.radius() * three / eight; 244 | let i_xz = c_m * c_i_xz + h_m * h_i_xz * two; 245 | let i_y = c_m * rsq / two + h_m * rsq * four / five; 246 | let inertia = Matrix3::from_diagonal(Vector3::new(i_xz, i_y, i_xz)); 247 | Mass::new_with_inertia(mass, inertia) 248 | } 249 | } 250 | 251 | impl Volume> for Cylinder 252 | where 253 | S: BaseFloat, 254 | { 255 | fn get_mass(&self, material: &Material) -> Mass> { 256 | use std::f64::consts::PI; 257 | let pi = S::from(PI).unwrap(); 258 | let rsq = self.radius() * self.radius(); 259 | let volume = pi * rsq * self.height(); 260 | let mass = volume * material.density(); 261 | let two = S::one() + S::one(); 262 | let three = S::one() + two; 263 | let twelve = three * two * two; 264 | let i_y = mass * rsq / two; 265 | let i_xz = mass / twelve * (three * rsq + self.height() * self.height()); 266 | let inertia = Matrix3::from_diagonal(Vector3::new(i_xz, i_y, i_xz)); 267 | Mass::new_with_inertia(mass, inertia) 268 | } 269 | } 270 | 271 | impl Volume> for Primitive3 272 | where 273 | S: BaseFloat, 274 | { 275 | fn get_mass(&self, material: &Material) -> Mass> { 276 | use collision::primitive::Primitive3::*; 277 | match *self { 278 | Particle(_) | Quad(_) => Mass::new(material.density()), 279 | Sphere(ref sphere) => sphere.get_mass(material), 280 | Cuboid(ref cuboid) => cuboid.get_mass(material), 281 | Cube(ref cube) => cube.get_mass(material), 282 | Capsule(ref capsule) => capsule.get_mass(material), 283 | Cylinder(ref cylinder) => cylinder.get_mass(material), 284 | ConvexPolyhedron(ref polyhedra) => polyhedra.get_mass(material), 285 | } 286 | } 287 | } 288 | 289 | // Composite inertia : sum(I_i + M_i * d_i^2) 290 | // I_i : Inertia of primitive with index i 291 | // M_i : Mass of primitive with index i 292 | // d_i : Offset from composite center of mass to primitive center of mass 293 | impl Volume for CollisionShape 294 | where 295 | S: BaseFloat + Inertia, 296 | P: Volume + Primitive> + ComputeBound, 297 | B: Bound> + Clone + Union, 298 | T: Transform>, 299 | Y: Default, 300 | { 301 | fn get_mass(&self, material: &Material) -> Mass { 302 | let (mass, inertia) = self 303 | .primitives() 304 | .iter() 305 | .map(|p| (p.0.get_mass(material), &p.1)) 306 | .fold((S::zero(), S::zero()), |(a_m, a_i), (m, t)| { 307 | (a_m + m.mass(), a_i + m.local_inertia() + m.mass() * d2(t)) 308 | }); 309 | Mass::new_with_inertia(mass, inertia) 310 | } 311 | } 312 | 313 | fn d2(t: &T) -> S 314 | where 315 | S: BaseFloat, 316 | T: Transform>, 317 | { 318 | let p = t.transform_point(Point2::origin()).to_vec(); 319 | p.dot(p) 320 | } 321 | 322 | impl Volume> for CollisionShape 323 | where 324 | S: BaseFloat, 325 | P: Volume> + Primitive> + ComputeBound, 326 | B: Bound> + Clone + Union, 327 | T: Transform>, 328 | Y: Default, 329 | { 330 | fn get_mass(&self, material: &Material) -> Mass> { 331 | let (mass, inertia) = self 332 | .primitives() 333 | .iter() 334 | .map(|p| (p.0.get_mass(material), &p.1)) 335 | .fold((S::zero(), Matrix3::zero()), |(a_m, a_i), (m, t)| { 336 | (a_m + m.mass(), a_i + m.local_inertia() + d3(t) * m.mass()) 337 | }); 338 | Mass::new_with_inertia(mass, inertia) 339 | } 340 | } 341 | 342 | fn d3(t: &T) -> Matrix3 343 | where 344 | S: BaseFloat, 345 | T: Transform>, 346 | { 347 | let o = t.transform_point(Point3::origin()).to_vec(); 348 | let d2 = o.magnitude2(); 349 | let mut j = Matrix3::from_value(d2); 350 | j.x += o * -o.x; 351 | j.y += o * -o.y; 352 | j.z += o * -o.z; 353 | j 354 | } 355 | -------------------------------------------------------------------------------- /rhusics-core/src/physics2d.rs: -------------------------------------------------------------------------------- 1 | //! 2D structures for physics 2 | 3 | pub use collide2d::*; 4 | 5 | use cgmath::Vector2; 6 | 7 | use physics::{ForceAccumulator, Mass, Velocity}; 8 | 9 | /// 2D velocity 10 | /// 11 | /// ### Type parameters: 12 | /// 13 | /// - `S`: Scalar type (f32 or f64) 14 | pub type Velocity2 = Velocity, S>; 15 | 16 | /// 2D mass 17 | /// 18 | /// ### Type parameters: 19 | /// 20 | /// - `S`: Scalar type (f32 or f64) (also `Inertia` type for 2D) 21 | pub type Mass2 = Mass; 22 | 23 | /// 2D force accumulator 24 | /// 25 | /// ### Type parameters: 26 | /// 27 | /// - `S`: Scalar type (f32 or f64) 28 | pub type ForceAccumulator2 = ForceAccumulator, S>; 29 | -------------------------------------------------------------------------------- /rhusics-core/src/physics3d.rs: -------------------------------------------------------------------------------- 1 | //! 3D structures for physics 2 | 3 | pub use collide3d::*; 4 | 5 | use cgmath::{Matrix3, Vector3}; 6 | 7 | use physics::{ForceAccumulator, Mass, Velocity}; 8 | 9 | /// 3D velocity 10 | /// 11 | /// ### Type parameters: 12 | /// 13 | /// - `S`: Scalar type (f32 or f64) 14 | pub type Velocity3 = Velocity, Vector3>; 15 | 16 | /// 3D mass 17 | /// 18 | /// ### Type parameters: 19 | /// 20 | /// - `S`: Scalar type (f32 or f64) 21 | pub type Mass3 = Mass>; 22 | 23 | /// 3D force accumulator 24 | /// 25 | /// ### Type parameters: 26 | /// 27 | /// - `S`: Scalar type (f32 or f64) 28 | pub type ForceAccumulator3 = ForceAccumulator, Vector3>; 29 | -------------------------------------------------------------------------------- /rhusics-ecs/Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "rhusics-ecs" 3 | version = "0.9.0" 4 | authors = [ 5 | "Simon Rönnberg ", 6 | "Thomas O'Dell " 7 | ] 8 | repository = "https://github.com/rustgd/rhusics.git" 9 | homepage = "https://github.com/rustgd/rhusics.git" 10 | 11 | license = "MIT OR Apache-2.0" 12 | documentation = "https://docs.rs/rhusics-ecs" 13 | description = "Physics library for use with `specs`" 14 | 15 | keywords = ["gamedev", "cgmath", "specs", "physics"] 16 | 17 | [features] 18 | serializable = ["serde", "cgmath/serde", "collision/serde", "rhusics-core/serializable"] 19 | 20 | [dependencies] 21 | cgmath = "0.17" 22 | collision = { version = "0.20" } 23 | failure = "0.1" 24 | rhusics-core = { version = "0.9.0", path = "../rhusics-core", features = ["specs"] } 25 | specs = { version = "0.16" } 26 | shred = { version = "0.10" } 27 | shred-derive = { version = "0.6" } 28 | shrev = { version = "1.1" } 29 | serde = { version = "1.0", optional = true, features = ["derive"] } 30 | 31 | [[example]] 32 | name = "basic2d" 33 | 34 | [[example]] 35 | name = "basic3d" 36 | 37 | [[example]] 38 | name = "spatial3d" 39 | 40 | [[example]] 41 | name = "spatial2d" 42 | -------------------------------------------------------------------------------- /rhusics-ecs/examples/basic2d.rs: -------------------------------------------------------------------------------- 1 | extern crate cgmath; 2 | extern crate rhusics_core; 3 | extern crate rhusics_ecs; 4 | extern crate shrev; 5 | extern crate specs; 6 | 7 | use cgmath::{Point2, Rad, Rotation2, Transform}; 8 | use shrev::EventChannel; 9 | use specs::prelude::{Builder, RunNow, World, WorldExt}; 10 | 11 | use rhusics_core::Pose; 12 | use rhusics_ecs::collide2d::{ 13 | BasicCollisionSystem2, BodyPose2, BroadBruteForce2, CollisionMode, CollisionShape2, 14 | CollisionStrategy, ContactEvent2, Rectangle, GJK2, 15 | }; 16 | 17 | pub fn main() { 18 | let mut world = World::new(); 19 | 20 | let mut system = BasicCollisionSystem2::, ()>::new() 21 | .with_broad_phase(BroadBruteForce2::default()) 22 | .with_narrow_phase(GJK2::new()); 23 | system.setup(&mut world); 24 | 25 | let mut reader_1 = world 26 | .write_resource::>>() 27 | .register_reader(); 28 | 29 | world 30 | .create_entity() 31 | .with(CollisionShape2::, ()>::new_simple( 32 | CollisionStrategy::FullResolution, 33 | CollisionMode::Discrete, 34 | Rectangle::new(10., 10.).into(), 35 | )).with(BodyPose2::::one()) 36 | .build(); 37 | 38 | world 39 | .create_entity() 40 | .with(CollisionShape2::, ()>::new_simple( 41 | CollisionStrategy::FullResolution, 42 | CollisionMode::Discrete, 43 | Rectangle::new(10., 10.).into(), 44 | )).with(BodyPose2::::new( 45 | Point2::new(3., 2.), 46 | Rotation2::from_angle(Rad(0.)), 47 | )).build(); 48 | 49 | system.run_now(&world); 50 | println!( 51 | "Contacts: {:?}", 52 | world 53 | .read_resource::>>() 54 | .read(&mut reader_1) 55 | .collect::>() 56 | ); 57 | } 58 | -------------------------------------------------------------------------------- /rhusics-ecs/examples/basic3d.rs: -------------------------------------------------------------------------------- 1 | extern crate cgmath; 2 | extern crate rhusics_core; 3 | extern crate rhusics_ecs; 4 | extern crate shrev; 5 | extern crate specs; 6 | 7 | use cgmath::{Point3, Quaternion, Rad, Rotation3, Transform}; 8 | use shrev::EventChannel; 9 | use specs::prelude::{Builder, RunNow, World, WorldExt}; 10 | 11 | use rhusics_core::Pose; 12 | use rhusics_ecs::collide3d::{ 13 | BasicCollisionSystem3, BodyPose3, BroadBruteForce3, CollisionMode, CollisionShape3, 14 | CollisionStrategy, ContactEvent3, Cuboid, GJK3, 15 | }; 16 | 17 | pub fn main() { 18 | let mut world = World::new(); 19 | let mut system = BasicCollisionSystem3::, ()>::new() 20 | .with_broad_phase(BroadBruteForce3::default()) 21 | .with_narrow_phase(GJK3::new()); 22 | system.setup(&mut world); 23 | 24 | let mut reader_1 = world 25 | .write_resource::>>() 26 | .register_reader(); 27 | 28 | world 29 | .create_entity() 30 | .with(CollisionShape3::, ()>::new_simple( 31 | CollisionStrategy::FullResolution, 32 | CollisionMode::Discrete, 33 | Cuboid::new(10., 10., 10.).into(), 34 | )).with(BodyPose3::::one()) 35 | .build(); 36 | 37 | world 38 | .create_entity() 39 | .with(CollisionShape3::, ()>::new_simple( 40 | CollisionStrategy::FullResolution, 41 | CollisionMode::Discrete, 42 | Cuboid::new(10., 10., 10.).into(), 43 | )).with(BodyPose3::::new( 44 | Point3::new(3., 2., 0.), 45 | Quaternion::from_angle_z(Rad(0.)), 46 | )).build(); 47 | 48 | system.run_now(&world); 49 | println!( 50 | "Contacts: {:?}", 51 | world 52 | .read_resource::>>() 53 | .read(&mut reader_1) 54 | .collect::>() 55 | ); 56 | } 57 | -------------------------------------------------------------------------------- /rhusics-ecs/examples/spatial2d.rs: -------------------------------------------------------------------------------- 1 | extern crate cgmath; 2 | extern crate collision; 3 | extern crate rhusics_core; 4 | extern crate rhusics_ecs; 5 | extern crate shrev; 6 | extern crate specs; 7 | 8 | use cgmath::{Point2, Rad, Rotation2, Transform, Vector2}; 9 | use collision::dbvt::query_ray_closest; 10 | use collision::Ray2; 11 | use shrev::EventChannel; 12 | use specs::prelude::{Builder, ReadExpect, System, World, WorldExt}; 13 | 14 | use rhusics_core::{PhysicalEntity, Pose}; 15 | use rhusics_ecs::physics2d::{ 16 | BodyPose2, CollisionMode, CollisionShape2, CollisionStrategy, ContactEvent2, 17 | ContactResolutionSystem2, CurrentFrameUpdateSystem2, DynamicBoundingVolumeTree2, Mass2, 18 | NextFrameSetupSystem2, Rectangle, SpatialCollisionSystem2, SpatialSortingSystem2, GJK2, 19 | }; 20 | use rhusics_ecs::WithPhysics; 21 | 22 | struct RayCastSystem; 23 | 24 | impl<'a> System<'a> for RayCastSystem { 25 | type SystemData = (ReadExpect<'a, DynamicBoundingVolumeTree2>,); 26 | 27 | fn run(&mut self, (tree,): Self::SystemData) { 28 | println!("{:?}", *tree); 29 | let ray = Ray2::new(Point2::new(-4., 10.), Vector2::new(0., -1.)); 30 | if let Some((v, p)) = query_ray_closest(&*tree, ray) { 31 | println!("hit bounding volume of {:?} at point {:?}", v.value, p); 32 | } 33 | } 34 | } 35 | 36 | pub fn main() { 37 | let mut world = World::new(); 38 | let mut sort = SpatialSortingSystem2::, ()>::new(); 39 | let mut collide = 40 | SpatialCollisionSystem2::, ()>::new().with_narrow_phase(GJK2::new()); 41 | let mut raycast = RayCastSystem; 42 | let mut impulse_solver = CurrentFrameUpdateSystem2::>::new(); 43 | let mut next_frame = NextFrameSetupSystem2::>::new(); 44 | let mut contact_resolution = ContactResolutionSystem2::>::new(); 45 | 46 | sort.setup(&mut world); 47 | collide.setup(&mut world); 48 | raycast.setup(&mut world); 49 | impulse_solver.setup(&mut world); 50 | next_frame.setup(&mut world); 51 | contact_resolution.setup(&mut world); 52 | 53 | world 54 | .create_entity() 55 | .with_static_physical_entity( 56 | CollisionShape2::, ()>::new_simple( 57 | CollisionStrategy::FullResolution, 58 | CollisionMode::Discrete, 59 | Rectangle::new(10., 10.).into(), 60 | ), 61 | BodyPose2::::one(), 62 | PhysicalEntity::default(), 63 | Mass2::new(1.), 64 | ).build(); 65 | 66 | world 67 | .create_entity() 68 | .with_static_physical_entity( 69 | CollisionShape2::, ()>::new_simple( 70 | CollisionStrategy::FullResolution, 71 | CollisionMode::Discrete, 72 | Rectangle::new(10., 10.).into(), 73 | ), 74 | BodyPose2::::new(Point2::new(2., 2.), Rotation2::from_angle(Rad(0.))), 75 | PhysicalEntity::default(), 76 | Mass2::new(1.), 77 | ).build(); 78 | 79 | let mut reader_1 = world 80 | .write_resource::>>() 81 | .register_reader(); 82 | 83 | { 84 | use specs::prelude::RunNow; 85 | sort.run_now(&world); 86 | collide.run_now(&world); 87 | 88 | println!( 89 | "Contacts: {:?}", 90 | world 91 | .read_resource::>>() 92 | .read(&mut reader_1) 93 | .collect::>() 94 | ); 95 | 96 | raycast.run_now(&world); 97 | impulse_solver.run_now(&world); 98 | next_frame.run_now(&world); 99 | contact_resolution.run_now(&world); 100 | } 101 | } 102 | -------------------------------------------------------------------------------- /rhusics-ecs/examples/spatial3d.rs: -------------------------------------------------------------------------------- 1 | extern crate cgmath; 2 | extern crate collision; 3 | extern crate rhusics_core; 4 | extern crate rhusics_ecs; 5 | extern crate shrev; 6 | extern crate specs; 7 | 8 | use cgmath::{Point3, Quaternion, Rad, Rotation3, Transform, Vector3}; 9 | use collision::dbvt::query_ray_closest; 10 | use collision::Ray3; 11 | use shrev::EventChannel; 12 | use specs::prelude::{Builder, ReadExpect, System, World, WorldExt}; 13 | 14 | use rhusics_core::{PhysicalEntity, Pose}; 15 | use rhusics_ecs::physics3d::{ 16 | BodyPose3, CollisionMode, CollisionShape3, CollisionStrategy, ContactEvent3, 17 | ContactResolutionSystem3, Cuboid, CurrentFrameUpdateSystem3, DynamicBoundingVolumeTree3, Mass3, 18 | NextFrameSetupSystem3, SpatialCollisionSystem3, SpatialSortingSystem3, GJK3, 19 | }; 20 | use rhusics_ecs::WithPhysics; 21 | 22 | struct RayCastSystem; 23 | 24 | impl<'a> System<'a> for RayCastSystem { 25 | type SystemData = (ReadExpect<'a, DynamicBoundingVolumeTree3>,); 26 | 27 | fn run(&mut self, (tree,): Self::SystemData) { 28 | let ray = Ray3::new(Point3::new(-4., 10., 0.), Vector3::new(0., -1., 0.)); 29 | if let Some((v, p)) = query_ray_closest(&*tree, ray) { 30 | println!("hit bounding volume of {:?} at point {:?}", v.value, p); 31 | } 32 | } 33 | } 34 | 35 | pub fn main() { 36 | let mut world = World::new(); 37 | let mut sort = SpatialSortingSystem3::, ()>::new(); 38 | let mut collide = 39 | SpatialCollisionSystem3::, ()>::new().with_narrow_phase(GJK3::new()); 40 | let mut raycast = RayCastSystem; 41 | let mut impulse_solver = CurrentFrameUpdateSystem3::>::new(); 42 | let mut next_frame = NextFrameSetupSystem3::>::new(); 43 | let mut contact_resolution = ContactResolutionSystem3::>::new(); 44 | 45 | sort.setup(&mut world); 46 | collide.setup(&mut world); 47 | raycast.setup(&mut world); 48 | impulse_solver.setup(&mut world); 49 | next_frame.setup(&mut world); 50 | contact_resolution.setup(&mut world); 51 | 52 | world 53 | .create_entity() 54 | .with_static_physical_entity( 55 | CollisionShape3::, ()>::new_simple( 56 | CollisionStrategy::FullResolution, 57 | CollisionMode::Discrete, 58 | Cuboid::new(10., 10., 10.).into(), 59 | ), 60 | BodyPose3::one(), 61 | PhysicalEntity::default(), 62 | Mass3::new(1.), 63 | ).build(); 64 | 65 | world 66 | .create_entity() 67 | .with_static_physical_entity( 68 | CollisionShape3::, ()>::new_simple( 69 | CollisionStrategy::FullResolution, 70 | CollisionMode::Discrete, 71 | Cuboid::new(10., 10., 10.).into(), 72 | ), 73 | BodyPose3::new(Point3::new(3., 2., 0.), Quaternion::from_angle_z(Rad(0.))), 74 | PhysicalEntity::default(), 75 | Mass3::new(1.), 76 | ).build(); 77 | 78 | let mut reader_1 = world 79 | .write_resource::>>() 80 | .register_reader(); 81 | { 82 | use specs::prelude::RunNow; 83 | sort.run_now(&world); 84 | collide.run_now(&world); 85 | println!( 86 | "Contacts: {:?}", 87 | world 88 | .read_resource::>>() 89 | .read(&mut reader_1) 90 | .collect::>() 91 | ); 92 | raycast.run_now(&world); 93 | 94 | impulse_solver.run_now(&world); 95 | next_frame.run_now(&world); 96 | contact_resolution.run_now(&world); 97 | } 98 | } 99 | -------------------------------------------------------------------------------- /rhusics-ecs/src/collide/mod.rs: -------------------------------------------------------------------------------- 1 | //! Contains collision detection components, resources and systems for use with `specs` 2 | pub use self::systems::*; 3 | 4 | mod systems; 5 | -------------------------------------------------------------------------------- /rhusics-ecs/src/collide/systems/basic.rs: -------------------------------------------------------------------------------- 1 | use std::fmt::Debug; 2 | 3 | use cgmath::prelude::*; 4 | use collision::prelude::*; 5 | use shrev::EventChannel; 6 | use specs::prelude::{Component, Entities, Entity, Join, ReadStorage, System, Write, WriteStorage}; 7 | 8 | use core::{ 9 | basic_collide, BroadPhase, CollisionData, CollisionShape, ContactEvent, GetId, NarrowPhase, 10 | NextFrame, Primitive, 11 | }; 12 | 13 | /// Collision detection [system](https://docs.rs/specs/0.9.5/specs/trait.System.html) for use with 14 | /// [`specs`](https://docs.rs/specs/0.9.5/specs/). 15 | /// 16 | /// Has support for both broad phase and narrow phase collision detection. Will only do narrow phase 17 | /// if both broad and narrow phase is activated. 18 | /// 19 | /// Can handle any transform component type, as long as the type implements 20 | /// [`Transform`](https://docs.rs/cgmath/0.15.0/cgmath/trait.Transform.html), and as long as the 21 | /// storage is wrapped in a 22 | /// [`FlaggedStorage`](https://docs.rs/specs/0.9.5/specs/struct.FlaggedStorage.html). 23 | /// 24 | /// ### Type parameters: 25 | /// 26 | /// - `P`: Shape primitive 27 | /// - `T`: Transform 28 | /// - `D`: Data accepted by broad phase 29 | /// - `B`: Bounding volume 30 | /// - `Y`: Shape type, see `Collider` 31 | /// 32 | /// ### System Function: 33 | /// 34 | /// `fn(Entities, T, NextFrame, CollisionShape) -> (CollisionShape, EventChannel)` 35 | pub struct BasicCollisionSystem 36 | where 37 | P: Primitive, 38 | B: Bound, 39 | { 40 | narrow: Option>>, 41 | broad: Option>>, 42 | } 43 | 44 | impl BasicCollisionSystem 45 | where 46 | P: Primitive + ComputeBound + Send + Sync + 'static, 47 | P::Point: Debug + Send + Sync + 'static, 48 | ::Scalar: Send + Sync + 'static, 49 | ::Diff: Debug + Send + Sync + 'static, 50 | T: Component + Transform + Send + Sync + Clone + 'static, 51 | Y: Default + Send + Sync + 'static, 52 | B: Bound + Send + Sync + 'static + Union + Clone, 53 | D: HasBound + From<(Entity, B)> + GetId, 54 | { 55 | /// Create a new collision detection system, with no broad or narrow phase activated. 56 | pub fn new() -> Self { 57 | Self { 58 | narrow: None, 59 | broad: None, 60 | } 61 | } 62 | 63 | /// Specify what narrow phase algorithm to use 64 | pub fn with_narrow_phase + 'static>(mut self, narrow: N) -> Self { 65 | self.narrow = Some(Box::new(narrow)); 66 | self 67 | } 68 | 69 | /// Specify what broad phase algorithm to use 70 | pub fn with_broad_phase + 'static>(mut self, broad: V) -> Self { 71 | self.broad = Some(Box::new(broad)); 72 | self 73 | } 74 | } 75 | 76 | impl<'a, P, T, Y, D, B> System<'a> for BasicCollisionSystem 77 | where 78 | P: Primitive + ComputeBound + Send + Sync + 'static, 79 | P::Point: Debug + Send + Sync + 'static, 80 | ::Scalar: Send + Sync + 'static, 81 | ::Diff: Debug + Send + Sync + 'static, 82 | T: Component + Transform + Send + Sync + Clone + 'static, 83 | Y: Default + Send + Sync + 'static, 84 | B: Bound + Send + Sync + 'static + Union + Clone, 85 | D: HasBound + From<(Entity, B)> + GetId, 86 | { 87 | type SystemData = ( 88 | Entities<'a>, 89 | ReadStorage<'a, T>, 90 | ReadStorage<'a, NextFrame>, 91 | WriteStorage<'a, CollisionShape>, 92 | Write<'a, EventChannel>>, 93 | ); 94 | 95 | fn run(&mut self, system_data: Self::SystemData) { 96 | let (entities, poses, next_poses, mut shapes, mut event_channel) = system_data; 97 | 98 | if let Some(ref mut broad) = self.broad { 99 | for (entity, pose, shape) in (&*entities, &poses, &mut shapes).join() { 100 | shape.update(pose, next_poses.get(entity).map(|p| &p.value)); 101 | } 102 | event_channel.iter_write(basic_collide( 103 | &BasicCollisionData { 104 | poses, 105 | shapes, 106 | next_poses, 107 | entities, 108 | }, 109 | broad, 110 | &self.narrow, 111 | )); 112 | } 113 | } 114 | } 115 | 116 | /// Collision data used by ECS systems 117 | struct BasicCollisionData<'a, P, T, B, Y> 118 | where 119 | P: Primitive + ComputeBound + Send + Sync + 'static, 120 | P::Point: Debug + Send + Sync + 'static, 121 | ::Scalar: Send + Sync + 'static, 122 | ::Diff: Debug + Send + Sync + 'static, 123 | T: Component + Transform + Send + Sync + Clone + 'static, 124 | Y: Default + Send + Sync + 'static, 125 | B: Bound + Send + Sync + 'static + Union + Clone, 126 | { 127 | /// collision shapes 128 | pub shapes: WriteStorage<'a, CollisionShape>, 129 | /// current frame poses 130 | pub poses: ReadStorage<'a, T>, 131 | /// next frame poses 132 | pub next_poses: ReadStorage<'a, NextFrame>, 133 | /// entities 134 | pub entities: Entities<'a>, 135 | } 136 | 137 | impl<'a, P, T, B, Y, D> CollisionData for BasicCollisionData<'a, P, T, B, Y> 138 | where 139 | P: Primitive + ComputeBound + Send + Sync + 'static, 140 | P::Point: Debug + Send + Sync + 'static, 141 | ::Scalar: Send + Sync + 'static, 142 | ::Diff: Debug + Send + Sync + 'static, 143 | T: Component + Transform + Send + Sync + Clone + 'static, 144 | Y: Default + Send + Sync + 'static, 145 | B: Bound + Send + Sync + 'static + Union + Clone, 146 | D: HasBound + From<(Entity, B)> + GetId, 147 | { 148 | fn get_broad_data(&self) -> Vec { 149 | (&*self.entities, &self.shapes) 150 | .join() 151 | .map(|(entity, shape)| (entity, shape.bound().clone()).into()) 152 | .collect::>() 153 | } 154 | 155 | fn get_shape(&self, id: Entity) -> Option<&CollisionShape> { 156 | self.shapes.get(id) 157 | } 158 | 159 | fn get_pose(&self, id: Entity) -> Option<&T> { 160 | self.poses.get(id) 161 | } 162 | 163 | fn get_next_pose(&self, id: Entity) -> Option<&T> { 164 | self.next_poses.get(id).as_ref().map(|p| &p.value) 165 | } 166 | } 167 | -------------------------------------------------------------------------------- /rhusics-ecs/src/collide/systems/mod.rs: -------------------------------------------------------------------------------- 1 | //! Contains systems for collision detection and spatial querying 2 | 3 | pub use self::basic::BasicCollisionSystem; 4 | pub use self::spatial_collision::SpatialCollisionSystem; 5 | pub use self::spatial_sort::SpatialSortingSystem; 6 | 7 | mod basic; 8 | mod spatial_collision; 9 | mod spatial_sort; 10 | -------------------------------------------------------------------------------- /rhusics-ecs/src/collide/systems/spatial_collision.rs: -------------------------------------------------------------------------------- 1 | use std::fmt::Debug; 2 | 3 | use cgmath::prelude::*; 4 | use cgmath::BaseFloat; 5 | use collision::dbvt::{DynamicBoundingVolumeTree, TreeValue}; 6 | use collision::prelude::*; 7 | use shrev::EventChannel; 8 | use specs::prelude::{ 9 | BitSet, Component, ComponentEvent, Entities, Entity, Join, ReadStorage, ReaderId, World, 10 | System, Tracked, Write, 11 | }; 12 | 13 | use core::{ 14 | tree_collide, BroadPhase, CollisionData, CollisionShape, ContactEvent, GetId, NarrowPhase, 15 | NextFrame, Primitive, 16 | }; 17 | 18 | /// Collision detection [system](https://docs.rs/specs/0.9.5/specs/trait.System.html) for use with 19 | /// [`specs`](https://docs.rs/specs/0.9.5/specs/). 20 | /// 21 | /// Will perform spatial sorting of the collision world. 22 | /// 23 | /// Has support for both broad phase and narrow phase collision detection. Will only do narrow phase 24 | /// if both broad and narrow phase is activated. If no broad phase is set, it will use a DBVT based 25 | /// broad phase that has complexity O(m log^2 n), where m is the number of shapes that have a dirty 26 | /// pose. 27 | /// 28 | /// Can handle any transform component type, as long as the type implements 29 | /// [`Transform`](https://docs.rs/cgmath/0.15.0/cgmath/trait.Transform.html), and as long as the 30 | /// storage is wrapped in 31 | /// [`FlaggedStorage`](https://docs.rs/specs/0.9.5/specs/struct.FlaggedStorage.html). 32 | /// 33 | /// ### Type parameters: 34 | /// 35 | /// - `P`: Shape primitive 36 | /// - `T`: Transform 37 | /// - `D`: Data accepted by broad phase 38 | /// - `Y`: Shape type, see `Collider` 39 | /// 40 | /// ### System Function: 41 | /// 42 | /// `fn(Entities, T, NextFrame, CollisionShape, DynamicBoundingVolumeTree) -> (DynamicBoundingVolumeTree, EventChannel)` 43 | pub struct SpatialCollisionSystem 44 | where 45 | P: Primitive, 46 | B: Bound, 47 | { 48 | narrow: Option>>, 49 | broad: Option>>, 50 | dirty: BitSet, 51 | pose_reader: Option>, 52 | next_pose_reader: Option>, 53 | } 54 | 55 | impl SpatialCollisionSystem 56 | where 57 | P: Primitive + Send + Sync + 'static, 58 | ::Diff: Debug, 59 | ::Scalar: BaseFloat, 60 | B: Clone 61 | + Debug 62 | + Send 63 | + Sync 64 | + 'static 65 | + Bound 66 | + Union 67 | + Contains 68 | + SurfaceArea::Scalar>, 69 | T: Transform + Component, 70 | D: HasBound, 71 | { 72 | /// Create a new collision detection system, with no broad or narrow phase activated. 73 | pub fn new() -> Self { 74 | SpatialCollisionSystem { 75 | narrow: None, 76 | broad: None, 77 | dirty: BitSet::default(), 78 | pose_reader: None, 79 | next_pose_reader: None, 80 | } 81 | } 82 | 83 | /// Specify what narrow phase algorithm to use 84 | pub fn with_narrow_phase + 'static>(mut self, narrow: N) -> Self { 85 | self.narrow = Some(Box::new(narrow)); 86 | self 87 | } 88 | 89 | /// Specify what broad phase algorithm to use 90 | pub fn with_broad_phase + 'static>(mut self, broad: V) -> Self { 91 | self.broad = Some(Box::new(broad)); 92 | self 93 | } 94 | } 95 | 96 | impl<'a, P, T, Y, B, D> System<'a> for SpatialCollisionSystem 97 | where 98 | P: Primitive + ComputeBound + Send + Sync + 'static, 99 | P::Point: EuclideanSpace, 100 | ::Scalar: BaseFloat + Send + Sync + 'static, 101 | B: Clone 102 | + Debug 103 | + Send 104 | + Sync 105 | + 'static 106 | + Bound 107 | + Union 108 | + Discrete 109 | + Contains 110 | + SurfaceArea::Scalar>, 111 | ::Diff: Debug + Send + Sync + 'static, 112 | P::Point: Debug + Send + Sync + 'static, 113 | T: Component + Clone + Debug + Transform + Send + Sync + 'static, 114 | T::Storage: Tracked, 115 | Y: Default + Send + Sync + 'static, 116 | D: Send + Sync + 'static + TreeValue + HasBound + GetId, 117 | { 118 | type SystemData = ( 119 | Entities<'a>, 120 | ReadStorage<'a, T>, 121 | ReadStorage<'a, NextFrame>, 122 | ReadStorage<'a, CollisionShape>, 123 | Write<'a, EventChannel>>, 124 | Write<'a, DynamicBoundingVolumeTree>, 125 | ); 126 | 127 | fn run(&mut self, system_data: Self::SystemData) { 128 | let (entities, poses, next_poses, shapes, mut event_channel, mut tree) = system_data; 129 | self.dirty.clear(); 130 | 131 | for event in poses.channel().read(self.pose_reader.as_mut().unwrap()) { 132 | match event { 133 | ComponentEvent::Inserted(index) => { 134 | self.dirty.add(*index); 135 | } 136 | ComponentEvent::Modified(index) => { 137 | self.dirty.add(*index); 138 | } 139 | ComponentEvent::Removed(index) => { 140 | self.dirty.remove(*index); 141 | } 142 | } 143 | } 144 | for event in next_poses 145 | .channel() 146 | .read(self.next_pose_reader.as_mut().unwrap()) 147 | { 148 | match event { 149 | ComponentEvent::Inserted(index) => { 150 | self.dirty.add(*index); 151 | } 152 | ComponentEvent::Modified(index) => { 153 | self.dirty.add(*index); 154 | } 155 | ComponentEvent::Removed(index) => { 156 | self.dirty.remove(*index); 157 | } 158 | } 159 | } 160 | 161 | event_channel.iter_write(tree_collide( 162 | &SpatialCollisionData { 163 | poses, 164 | shapes, 165 | next_poses, 166 | entities, 167 | dirty: &self.dirty, 168 | }, 169 | &mut *tree, 170 | &mut self.broad, 171 | &self.narrow, 172 | )); 173 | } 174 | 175 | fn setup(&mut self, res: &mut World) { 176 | use specs::prelude::{SystemData, WriteStorage}; 177 | Self::SystemData::setup(res); 178 | let mut poses = WriteStorage::::fetch(res); 179 | self.pose_reader = Some(poses.register_reader()); 180 | let mut next_poses = WriteStorage::>::fetch(res); 181 | self.next_pose_reader = Some(next_poses.register_reader()); 182 | } 183 | } 184 | 185 | /// Collision data used by ECS systems 186 | pub struct SpatialCollisionData<'a, P, T, B, Y> 187 | where 188 | P: Primitive + ComputeBound + Send + Sync + 'static, 189 | P::Point: Debug + Send + Sync + 'static, 190 | ::Scalar: Send + Sync + 'static, 191 | ::Diff: Debug + Send + Sync + 'static, 192 | T: Component + Transform + Send + Sync + Clone + 'static, 193 | Y: Default + Send + Sync + 'static, 194 | B: Bound + Send + Sync + 'static + Union + Clone, 195 | { 196 | /// collision shapes 197 | pub shapes: ReadStorage<'a, CollisionShape>, 198 | /// current frame poses 199 | pub poses: ReadStorage<'a, T>, 200 | /// next frame poses 201 | pub next_poses: ReadStorage<'a, NextFrame>, 202 | /// entities 203 | pub entities: Entities<'a>, 204 | /// dirty poses 205 | pub dirty: &'a BitSet, 206 | } 207 | 208 | impl<'a, P, T, B, Y, D> CollisionData 209 | for SpatialCollisionData<'a, P, T, B, Y> 210 | where 211 | P: Primitive + ComputeBound + Send + Sync + 'static, 212 | P::Point: Debug + Send + Sync + 'static, 213 | ::Scalar: Send + Sync + 'static, 214 | ::Diff: Debug + Send + Sync + 'static, 215 | T: Component + Transform + Send + Sync + Clone + 'static, 216 | Y: Default + Send + Sync + 'static, 217 | B: Bound + Send + Sync + 'static + Union + Clone, 218 | { 219 | fn get_broad_data(&self) -> Vec { 220 | Vec::default() 221 | } 222 | 223 | fn get_shape(&self, id: Entity) -> Option<&CollisionShape> { 224 | self.shapes.get(id) 225 | } 226 | 227 | fn get_pose(&self, id: Entity) -> Option<&T> { 228 | self.poses.get(id) 229 | } 230 | 231 | fn get_dirty_poses(&self) -> Vec { 232 | (&*self.entities, self.dirty, &self.shapes) 233 | .join() 234 | .map(|(entity, _, _)| entity) 235 | .collect() 236 | } 237 | 238 | fn get_next_pose(&self, id: Entity) -> Option<&T> { 239 | self.next_poses.get(id).as_ref().map(|p| &p.value) 240 | } 241 | } 242 | -------------------------------------------------------------------------------- /rhusics-ecs/src/collide/systems/spatial_sort.rs: -------------------------------------------------------------------------------- 1 | use std::collections::HashMap; 2 | use std::fmt::Debug; 3 | use std::marker::PhantomData; 4 | 5 | use cgmath::prelude::*; 6 | use cgmath::BaseFloat; 7 | use collision::dbvt::{DynamicBoundingVolumeTree, TreeValue}; 8 | use collision::prelude::*; 9 | use specs::prelude::{ 10 | BitSet, Component, ComponentEvent, Entities, Entity, Join, ReadStorage, ReaderId, World, 11 | System, Tracked, Write, WriteStorage, 12 | }; 13 | 14 | use core::{CollisionShape, NextFrame, Primitive}; 15 | 16 | /// Spatial sorting [system](https://docs.rs/specs/0.9.5/specs/trait.System.html) for use with 17 | /// [`specs`](https://docs.rs/specs/0.9.5/specs/). 18 | /// 19 | /// Will perform spatial sorting of the collision world. Uses a Dynamic Bounding Volume Tree for 20 | /// sorting. Will update entries in the tree where the pose is dirty. 21 | /// 22 | /// Can handle any transform component type, as long as the type implements 23 | /// [`Transform`](https://docs.rs/cgmath/0.15.0/cgmath/trait.Transform.html), and as long as the 24 | /// storage is wrapped in 25 | /// [`FlaggedStorage`](https://docs.rs/specs/0.9.5/specs/struct.FlaggedStorage.html) 26 | /// 27 | /// ## Type parameters: 28 | /// 29 | /// - `P`: Primitive type, needs to implement `Primitive`. 30 | /// - `T`: Transform type, needs to implement `Transform` and have `FlaggedStorage`. 31 | /// - `D`: Type of values stored in the DBVT, needs to implement `TreeValue` and 32 | /// `From<(Entity, CollisionShape)>` 33 | /// - `B`: Bounding volume 34 | /// - `Y`: Shape type, see `Collider` 35 | /// 36 | /// ### System Function: 37 | /// 38 | /// `fn(Entities, T, NextFrame, CollisionShape) -> (CollisionShape, DynamicBoundingVolumeTree)` 39 | #[derive(Debug)] 40 | pub struct SpatialSortingSystem { 41 | entities: HashMap, 42 | dead: Vec, 43 | updated: BitSet, 44 | removed: BitSet, 45 | pose_reader: Option>, 46 | next_pose_reader: Option>, 47 | marker: PhantomData<(P, T, Y, B, D)>, 48 | } 49 | 50 | impl SpatialSortingSystem { 51 | /// Create a new sorting system. 52 | pub fn new() -> Self { 53 | SpatialSortingSystem { 54 | entities: HashMap::default(), 55 | marker: PhantomData, 56 | updated: BitSet::default(), 57 | removed: BitSet::default(), 58 | pose_reader: None, 59 | next_pose_reader: None, 60 | dead: Vec::default(), 61 | } 62 | } 63 | } 64 | 65 | impl<'a, P, T, Y, D, B> System<'a> for SpatialSortingSystem 66 | where 67 | P: Primitive + ComputeBound + Send + Sync + 'static, 68 | B: Clone 69 | + Debug 70 | + Send 71 | + Sync 72 | + Union 73 | + Bound 74 | + Contains 75 | + SurfaceArea::Scalar> 76 | + Send 77 | + Sync 78 | + 'static, 79 | P::Point: Debug, 80 | ::Scalar: BaseFloat + Send + Sync + 'static, 81 | ::Diff: Debug + Send + Sync, 82 | T: Component + Clone + Debug + Transform + Send + Sync, 83 | T::Storage: Tracked, 84 | Y: Default + Send + Sync + 'static, 85 | D: Send + Sync + 'static + TreeValue + From<(Entity, B)>, 86 | { 87 | type SystemData = ( 88 | Entities<'a>, 89 | ReadStorage<'a, T>, 90 | ReadStorage<'a, NextFrame>, 91 | WriteStorage<'a, CollisionShape>, 92 | Write<'a, DynamicBoundingVolumeTree>, 93 | ); 94 | 95 | fn run(&mut self, (entities, poses, next_poses, mut shapes, mut tree): Self::SystemData) { 96 | self.updated.clear(); 97 | self.removed.clear(); 98 | 99 | for event in poses.channel().read(self.pose_reader.as_mut().unwrap()) { 100 | match event { 101 | ComponentEvent::Inserted(index) => { 102 | self.updated.add(*index); 103 | } 104 | ComponentEvent::Modified(index) => { 105 | self.updated.add(*index); 106 | } 107 | ComponentEvent::Removed(index) => { 108 | self.updated.remove(*index); 109 | self.removed.add(*index); 110 | } 111 | } 112 | } 113 | 114 | // remove entities that are missing from the tree 115 | self.dead.clear(); 116 | for (entity, node_index) in &self.entities { 117 | if self.removed.contains(entity.id()) { 118 | tree.remove(*node_index); 119 | self.dead.push(*entity); 120 | } 121 | } 122 | for entity in &self.dead { 123 | self.entities.remove(entity); 124 | } 125 | 126 | // Check for updated poses 127 | // Uses FlaggedStorage 128 | for (entity, pose, shape, _) in (&*entities, &poses, &mut shapes, &self.updated).join() { 129 | shape.update(pose, None); 130 | 131 | // Update the wrapper in the tree for the shape 132 | match self.entities.get(&entity).cloned() { 133 | // Update existing 134 | Some(node_index) => { 135 | tree.update_node(node_index, (entity, shape.bound().clone()).into()); 136 | } 137 | // Insert new 138 | None => { 139 | let node_index = tree.insert((entity, shape.bound().clone()).into()); 140 | self.entities.insert(entity, node_index); 141 | } 142 | } 143 | } 144 | 145 | self.updated.clear(); 146 | 147 | for event in next_poses 148 | .channel() 149 | .read(self.next_pose_reader.as_mut().unwrap()) 150 | { 151 | match event { 152 | ComponentEvent::Inserted(index) => { 153 | self.updated.add(*index); 154 | } 155 | ComponentEvent::Modified(index) => { 156 | self.updated.add(*index); 157 | } 158 | ComponentEvent::Removed(index) => { 159 | self.updated.remove(*index); 160 | } 161 | } 162 | } 163 | 164 | // Check for updated next frame poses 165 | // Uses FlaggedStorage 166 | for (entity, pose, next_pose, shape, _) in 167 | (&*entities, &poses, &next_poses, &mut shapes, &self.updated).join() 168 | { 169 | shape.update(pose, Some(&next_pose.value)); 170 | 171 | // Update the wrapper in the tree for the shape 172 | if let Some(node_index) = self.entities.get(&entity).cloned() { 173 | tree.update_node(node_index, (entity, shape.bound().clone()).into()); 174 | } 175 | } 176 | 177 | // process possibly updated values 178 | tree.update(); 179 | 180 | // do refitting 181 | tree.do_refit(); 182 | } 183 | 184 | fn setup(&mut self, res: &mut World) { 185 | use specs::prelude::SystemData; 186 | Self::SystemData::setup(res); 187 | let mut poses = WriteStorage::::fetch(res); 188 | self.pose_reader = Some(poses.register_reader()); 189 | let mut next_poses = WriteStorage::>::fetch(res); 190 | self.next_pose_reader = Some(next_poses.register_reader()); 191 | } 192 | } 193 | -------------------------------------------------------------------------------- /rhusics-ecs/src/collide2d.rs: -------------------------------------------------------------------------------- 1 | //! Type wrappers and convenience functions for 2D collision detection 2 | 3 | pub use collision::algorithm::minkowski::GJK2; 4 | pub use collision::primitive::{Circle, ConvexPolygon, Particle2, Rectangle}; 5 | 6 | pub use core::collide2d::*; 7 | pub use core::{CollisionMode, CollisionStrategy}; 8 | 9 | use cgmath::Point2; 10 | use collision::dbvt::{DynamicBoundingVolumeTree, TreeValueWrapped}; 11 | use collision::primitive::Primitive2; 12 | use collision::Aabb2; 13 | use specs::prelude::Entity; 14 | 15 | use collide::{BasicCollisionSystem, SpatialCollisionSystem, SpatialSortingSystem}; 16 | use core::ContactEvent; 17 | 18 | /// Contact event for 2D 19 | /// 20 | /// ### Type parameters: 21 | /// 22 | /// - `S`: Scalar type (f32 or f64) 23 | pub type ContactEvent2 = ContactEvent>; 24 | 25 | /// Basic collision system for 2D, see 26 | /// [`BasicCollisionSystem`](../collide/ecs/struct.BasicCollisionSystem.html) for more information. 27 | /// 28 | /// ### Type parameters: 29 | /// 30 | /// - `S`: Scalar type (f32 or f64) 31 | /// - `T`: Transform 32 | /// - `Y`: Collider type, see `Collider` for more information 33 | pub type BasicCollisionSystem2 = 34 | BasicCollisionSystem, T, TreeValueWrapped>, Aabb2, Y>; 35 | 36 | /// Spatial sorting system for 2D, see 37 | /// [`SpatialSortingSystem`](../collide/ecs/struct.SpatialSortingSystem.html) for more information. 38 | /// 39 | /// ### Type parameters: 40 | /// 41 | /// - `S`: Scalar type (f32 or f64) 42 | /// - `T`: Transform 43 | /// - `Y`: Collider type, see `Collider` for more information 44 | pub type SpatialSortingSystem2 = 45 | SpatialSortingSystem, T, TreeValueWrapped>, Aabb2, Y>; 46 | 47 | /// Spatial collision system for 2D, see 48 | /// [`SpatialCollisionSystem`](../collide/ecs/struct.SpatialCollisionSystem.html) for more 49 | /// information. 50 | /// 51 | /// ### Type parameters: 52 | /// 53 | /// - `S`: Scalar type (f32 or f64) 54 | /// - `T`: Transform 55 | /// - `Y`: Collider type, see `Collider` for more information 56 | pub type SpatialCollisionSystem2 = SpatialCollisionSystem< 57 | Primitive2, 58 | T, 59 | (usize, TreeValueWrapped>), 60 | Aabb2, 61 | Y, 62 | >; 63 | 64 | /// Dynamic bounding volume tree for 2D 65 | /// 66 | /// ### Type parameters: 67 | /// 68 | /// - `S`: Scalar type (f32 or f64) 69 | pub type DynamicBoundingVolumeTree2 = 70 | DynamicBoundingVolumeTree>>; 71 | -------------------------------------------------------------------------------- /rhusics-ecs/src/collide3d.rs: -------------------------------------------------------------------------------- 1 | //! Type wrappers and convenience functions for 3D collision detection 2 | 3 | pub use collision::algorithm::minkowski::GJK3; 4 | pub use collision::primitive::{ConvexPolyhedron, Cuboid, Particle3, Sphere}; 5 | 6 | pub use core::collide3d::*; 7 | pub use core::{CollisionMode, CollisionStrategy}; 8 | 9 | use cgmath::Point3; 10 | use collision::dbvt::{DynamicBoundingVolumeTree, TreeValueWrapped}; 11 | use collision::primitive::Primitive3; 12 | use collision::Aabb3; 13 | use specs::prelude::Entity; 14 | 15 | use collide::{BasicCollisionSystem, SpatialCollisionSystem, SpatialSortingSystem}; 16 | use core::ContactEvent; 17 | 18 | /// Contact event for 2D 19 | /// 20 | /// ### Type parameters: 21 | /// 22 | /// - `S`: Scalar type (f32 or f64) 23 | pub type ContactEvent3 = ContactEvent>; 24 | 25 | /// ECS collision system for 3D, see 26 | /// [`BasicCollisionSystem`](../collide/ecs/struct.BasicCollisionSystem.html) for more information. 27 | /// 28 | /// ### Type parameters: 29 | /// 30 | /// - `S`: Scalar type (f32 or f64) 31 | /// - `T`: Transform 32 | /// - `Y`: Collider type, see `Collider` for more information 33 | pub type BasicCollisionSystem3 = 34 | BasicCollisionSystem, T, TreeValueWrapped>, Aabb3, Y>; 35 | 36 | /// Spatial sorting system for 3D, see 37 | /// [`SpatialSortingSystem`](../collide/ecs/struct.SpatialSortingSystem.html) for more information. 38 | /// 39 | /// ### Type parameters: 40 | /// 41 | /// - `S`: Scalar type (f32 or f64) 42 | /// - `T`: Transform 43 | /// - `Y`: Collider type, see `Collider` for more information 44 | pub type SpatialSortingSystem3 = 45 | SpatialSortingSystem, T, TreeValueWrapped>, Aabb3, Y>; 46 | 47 | /// Spatial collision system for 3D, see 48 | /// [`SpatialCollisionSystem`](../collide/ecs/struct.SpatialCollisionSystem.html) for more 49 | /// information. 50 | /// 51 | /// ### Type parameters: 52 | /// 53 | /// - `S`: Scalar type (f32 or f64) 54 | /// - `T`: Transform 55 | /// - `Y`: Collider type, see `Collider` for more information 56 | pub type SpatialCollisionSystem3 = SpatialCollisionSystem< 57 | Primitive3, 58 | T, 59 | (usize, TreeValueWrapped>), 60 | Aabb3, 61 | Y, 62 | >; 63 | 64 | /// Dynamic bounding volume tree for 3D 65 | /// 66 | /// ### Type parameters: 67 | /// 68 | /// - `S`: Scalar type (f32 or f64) 69 | pub type DynamicBoundingVolumeTree3 = 70 | DynamicBoundingVolumeTree>>; 71 | -------------------------------------------------------------------------------- /rhusics-ecs/src/lib.rs: -------------------------------------------------------------------------------- 1 | //! # Rhusics physics library 2 | //! 3 | //! A physics library. 4 | //! Uses [`cgmath`](https://github.com/brendanzab/cgmath/) for all computation. 5 | //! 6 | //! Features: 7 | //! 8 | //! * Two different broad phase collision detection implementations: 9 | //! * Brute force 10 | //! * Sweep and Prune 11 | //! * Narrow phase collision detection using GJK, and optionally EPA for full contact information 12 | //! * Functions for collision detection working on user supplied transform, and 13 | //! [`CollisionShape`](collide/struct.CollisionShape.html) components. 14 | //! Can optionally use broad and/or narrow phase detection. 15 | //! Library supplies a transform implementation [`BodyPose`](struct.BodyPose.html) for 16 | //! convenience. 17 | //! * Uses single precision as default, can be changed to double precision with the `double` 18 | //! feature. 19 | //! * Has support for doing spatial sort/collision detection using the collision-rs DBVT. 20 | //! * Support for doing broad phase using the collision-rs DBVT. 21 | //! * Has support for all primitives in collision-rs 22 | //! 23 | 24 | #![deny( 25 | missing_docs, 26 | trivial_casts, 27 | unsafe_code, 28 | unstable_features, 29 | unused_import_braces, 30 | unused_qualifications 31 | )] 32 | #![allow(unknown_lints, type_complexity, new_without_default_derive)] 33 | 34 | extern crate cgmath; 35 | extern crate collision; 36 | extern crate failure; 37 | extern crate rhusics_core as core; 38 | extern crate shred; 39 | extern crate shrev; 40 | extern crate specs; 41 | 42 | extern crate shred_derive; 43 | 44 | #[cfg(feature = "serializable")] 45 | #[macro_use] 46 | extern crate serde; 47 | 48 | pub use collide::{BasicCollisionSystem, SpatialCollisionSystem, SpatialSortingSystem}; 49 | pub use physics::{ 50 | setup_dispatch, ContactResolutionSystem, CurrentFrameUpdateSystem, DeltaTime, 51 | NextFrameSetupSystem, PhysicalEntityCreationError, PhysicalEntityParts, WithPhysics, 52 | }; 53 | 54 | pub mod collide2d; 55 | pub mod collide3d; 56 | pub mod physics2d; 57 | pub mod physics3d; 58 | 59 | mod collide; 60 | mod physics; 61 | -------------------------------------------------------------------------------- /rhusics-ecs/src/physics/mod.rs: -------------------------------------------------------------------------------- 1 | //! Contains physics components, resources and systems for use with `specs` 2 | pub use self::resources::*; 3 | pub use self::setup::*; 4 | pub use self::systems::*; 5 | 6 | mod resources; 7 | mod setup; 8 | mod systems; 9 | -------------------------------------------------------------------------------- /rhusics-ecs/src/physics/resources.rs: -------------------------------------------------------------------------------- 1 | use std::marker; 2 | 3 | use cgmath::{BaseFloat, EuclideanSpace, Rotation, VectorSpace, Zero}; 4 | use collision::Bound; 5 | use specs::error::Error as SpecsError; 6 | use specs::prelude::{Builder, Component, Entity, SystemData, World, WriteStorage, ResourceId}; 7 | 8 | use core::{ 9 | CollisionShape, ForceAccumulator, Mass, NextFrame, PhysicalEntity, PhysicsTime, Pose, 10 | Primitive, Velocity, 11 | }; 12 | use failure::Fail; 13 | 14 | /// Time step resource 15 | /// 16 | /// ### Type parameters: 17 | /// 18 | /// - `S`: Scalar 19 | #[derive(Debug)] 20 | #[cfg_attr(feature = "serializable", derive(Serialize, Deserialize))] 21 | pub struct DeltaTime 22 | where 23 | S: BaseFloat, 24 | { 25 | /// Delta time since last frame 26 | pub delta_seconds: S, 27 | } 28 | 29 | impl Default for DeltaTime 30 | where 31 | S: BaseFloat, 32 | { 33 | fn default() -> Self { 34 | DeltaTime { 35 | delta_seconds: S::zero(), 36 | } 37 | } 38 | } 39 | 40 | impl PhysicsTime for DeltaTime 41 | where 42 | S: BaseFloat, 43 | { 44 | fn delta_seconds(&self) -> S { 45 | self.delta_seconds 46 | } 47 | } 48 | 49 | /// Adds physical entity builder functions to `EntityBuilder` 50 | pub trait WithPhysics { 51 | /// Add dynamic physical entity components to entity 52 | /// 53 | /// ### Type parameters: 54 | /// 55 | /// - `P`: Collision Primitive 56 | /// - `Y`: Collider 57 | /// - `R`: Rotational quantity 58 | /// - `V`: Vector 59 | /// - `A`: Angular velocity 60 | /// - `I`: Inertia 61 | /// - `B`: Bounding volume 62 | /// - `T`: Transform 63 | fn with_dynamic_physical_entity( 64 | self, 65 | shape: CollisionShape, 66 | pose: T, 67 | velocity: Velocity, 68 | physical_entity: PhysicalEntity, 69 | mass: Mass, 70 | ) -> Self 71 | where 72 | T: Pose + Clone + Component + Send + Sync + 'static, 73 | P: Primitive + Send + Sync + 'static, 74 | B: Bound + Send + Sync + 'static, 75 | P::Point: EuclideanSpace + Send + Sync + 'static, 76 | V::Scalar: BaseFloat + Send + Sync + 'static, 77 | R: Rotation + Send + Sync + 'static, 78 | V: VectorSpace + Zero + Clone + Send + Sync + 'static, 79 | A: Copy + Zero + Clone + Send + Sync + 'static, 80 | Y: Send + Sync + 'static, 81 | I: Send + Sync + 'static; 82 | 83 | /// Add static physical entity components to entity 84 | /// 85 | /// ### Type parameters: 86 | /// 87 | /// - `S`: Scalar (f32 or f64) 88 | /// - `P`: Collision Primitive 89 | /// - `Y`: Collider 90 | /// - `R`: Rotational quantity 91 | /// - `I`: Inertia 92 | /// - `B`: Bounding volume 93 | /// - `T`: Transform 94 | fn with_static_physical_entity( 95 | self, 96 | shape: CollisionShape, 97 | pose: T, 98 | physical_entity: PhysicalEntity, 99 | mass: Mass, 100 | ) -> Self 101 | where 102 | T: Pose + Clone + Component + Send + Sync + 'static, 103 | S: BaseFloat + Send + Sync + 'static, 104 | P: Primitive + Send + Sync + 'static, 105 | B: Bound + Send + Sync + 'static, 106 | P::Point: EuclideanSpace + Send + Sync + 'static, 107 | R: Rotation + Send + Sync + 'static, 108 | Y: Send + Sync + 'static, 109 | I: Send + Sync + 'static; 110 | } 111 | 112 | impl WithPhysics for U { 113 | fn with_dynamic_physical_entity( 114 | self, 115 | shape: CollisionShape, 116 | pose: T, 117 | velocity: Velocity, 118 | physical_entity: PhysicalEntity, 119 | mass: Mass, 120 | ) -> Self 121 | where 122 | T: Pose + Clone + Component + Send + Sync + 'static, 123 | P: Primitive + Send + Sync + 'static, 124 | B: Bound + Send + Sync + 'static, 125 | P::Point: EuclideanSpace + Send + Sync + 'static, 126 | R: Rotation + Send + Sync + 'static, 127 | V: VectorSpace + Zero + Clone + Send + Sync + 'static, 128 | V::Scalar: BaseFloat + Send + Sync + 'static, 129 | A: Copy + Clone + Zero + Send + Sync + 'static, 130 | Y: Send + Sync + 'static, 131 | I: Send + Sync + 'static, 132 | { 133 | self.with_static_physical_entity(shape, pose.clone(), physical_entity, mass) 134 | .with(NextFrame { value: pose }) 135 | .with(velocity.clone()) 136 | .with(NextFrame { value: velocity }) 137 | .with(ForceAccumulator::::new()) 138 | } 139 | 140 | fn with_static_physical_entity( 141 | self, 142 | shape: CollisionShape, 143 | pose: T, 144 | physical_entity: PhysicalEntity, 145 | mass: Mass, 146 | ) -> Self 147 | where 148 | T: Pose + Clone + Component + Send + Sync + 'static, 149 | S: BaseFloat + Send + Sync + 'static, 150 | P: Primitive + Send + Sync + 'static, 151 | B: Bound + Send + Sync + 'static, 152 | P::Point: EuclideanSpace + Send + Sync + 'static, 153 | R: Rotation + Send + Sync + 'static, 154 | Y: Send + Sync + 'static, 155 | I: Send + Sync + 'static, 156 | { 157 | self.with(shape).with(physical_entity).with(mass).with(pose) 158 | } 159 | } 160 | 161 | /// SystemData for easier creation of physical entities. 162 | /// 163 | /// ### Type parameters: 164 | /// 165 | /// - `P`: Collision Primitive 166 | /// - `Y`: Collider 167 | /// - `R`: Rotational quantity 168 | /// - `V`: Linear velocity 169 | /// - `A`: Angular velocity 170 | /// - `I`: Inertia 171 | /// - `B`: Bounding volume 172 | /// - `T`: Transform 173 | #[derive(SystemData)] 174 | pub struct PhysicalEntityParts<'a, P, Y, R, V, A, I, B, T> 175 | where 176 | T: Pose + Clone + Component + Send + Sync + 'static, 177 | P: Primitive + Send + Sync + 'static, 178 | B: Bound + Send + Sync + 'static, 179 | P::Point: EuclideanSpace + Send + Sync + 'static, 180 | V::Scalar: BaseFloat + Send + Sync + 'static, 181 | V: VectorSpace + Zero + Clone + Send + Sync + 'static, 182 | A: Copy + Zero + Clone + Send + Sync + 'static, 183 | Y: Send + Sync + 'static, 184 | I: Send + Sync + 'static, 185 | R: Rotation + Send + Sync + 'static, 186 | { 187 | /// Collision shapes 188 | pub shapes: WriteStorage<'a, CollisionShape>, 189 | /// Body transforms 190 | pub poses: WriteStorage<'a, T>, 191 | /// Physical entities 192 | pub entities: WriteStorage<'a, PhysicalEntity>, 193 | /// Mass 194 | pub masses: WriteStorage<'a, Mass>, 195 | /// Velocity 196 | pub velocities: WriteStorage<'a, Velocity>, 197 | /// Next frame transform 198 | pub next_poses: WriteStorage<'a, NextFrame>, 199 | /// Next frame velocity 200 | pub next_velocities: WriteStorage<'a, NextFrame>>, 201 | /// Forces 202 | pub forces: WriteStorage<'a, ForceAccumulator>, 203 | m: marker::PhantomData, 204 | } 205 | 206 | /// Error returned when physical entity setup fails 207 | #[derive(Debug, Fail)] 208 | pub enum PhysicalEntityCreationError { 209 | /// Error returned when attempted to initialise a physical entity on a dead entity 210 | #[fail(display = "dead entity")] 211 | DeadEntity, 212 | } 213 | 214 | impl From for PhysicalEntityCreationError { 215 | fn from(_: SpecsError) -> Self { 216 | PhysicalEntityCreationError::DeadEntity 217 | } 218 | } 219 | 220 | impl<'a, P, Y, R, V, A, I, B, T> PhysicalEntityParts<'a, P, Y, R, V, A, I, B, T> 221 | where 222 | T: Pose + Clone + Component + Send + Sync + 'static, 223 | P: Primitive + Send + Sync + 'static, 224 | B: Bound + Send + Sync + 'static, 225 | P::Point: EuclideanSpace + Send + Sync + 'static, 226 | V::Scalar: BaseFloat + Send + Sync + 'static, 227 | V: VectorSpace + Zero + Clone + Send + Sync + 'static, 228 | A: Copy + Zero + Clone + Send + Sync + 'static, 229 | Y: Send + Sync + 'static, 230 | I: Send + Sync + 'static, 231 | R: Rotation + Send + Sync + 'static, 232 | { 233 | /// Extract physical entity storage from `World` 234 | pub fn new(world: &'a World) -> Self { 235 | Self::fetch(&world) 236 | } 237 | 238 | /// Setup static physical entity for given entity. 239 | pub fn static_entity( 240 | &mut self, 241 | entity: Entity, 242 | shape: CollisionShape, 243 | pose: T, 244 | physical_entity: PhysicalEntity, 245 | mass: Mass, 246 | ) -> Result<(), PhysicalEntityCreationError> { 247 | self.shapes.insert(entity, shape)?; 248 | self.poses.insert(entity, pose)?; 249 | self.entities.insert(entity, physical_entity)?; 250 | self.masses.insert(entity, mass)?; 251 | Ok(()) 252 | } 253 | 254 | /// Setup dynamic physical entity for given entity. 255 | pub fn dynamic_entity( 256 | &mut self, 257 | entity: Entity, 258 | shape: CollisionShape, 259 | pose: T, 260 | velocity: Velocity, 261 | physical_entity: PhysicalEntity, 262 | mass: Mass, 263 | ) -> Result<(), PhysicalEntityCreationError> { 264 | self.static_entity(entity, shape, pose.clone(), physical_entity, mass)?; 265 | self.next_poses.insert(entity, NextFrame { value: pose })?; 266 | self.velocities.insert(entity, velocity.clone())?; 267 | self.next_velocities 268 | .insert(entity, NextFrame { value: velocity })?; 269 | self.forces 270 | .insert(entity, ForceAccumulator::::new())?; 271 | Ok(()) 272 | } 273 | } 274 | 275 | #[cfg(test)] 276 | mod tests { 277 | use super::PhysicalEntityParts; 278 | use cgmath::{Matrix3, Quaternion, Vector3}; 279 | use collision::primitive::Primitive3; 280 | use collision::Aabb3; 281 | use core::collide3d::BodyPose3; 282 | use specs::prelude::{SystemData, World, WorldExt}; 283 | 284 | type PhysicalEntityPartsTest<'a> = PhysicalEntityParts< 285 | 'a, 286 | Primitive3, 287 | (), 288 | Quaternion, 289 | Vector3, 290 | Vector3, 291 | Matrix3, 292 | Aabb3, 293 | BodyPose3, 294 | >; 295 | 296 | #[test] 297 | fn test() { 298 | let mut world = World::new(); 299 | PhysicalEntityPartsTest::setup(&mut world); 300 | PhysicalEntityPartsTest::new(&world); 301 | } 302 | } 303 | -------------------------------------------------------------------------------- /rhusics-ecs/src/physics/setup.rs: -------------------------------------------------------------------------------- 1 | use std::fmt::Debug; 2 | use std::ops::{Add, Mul, Sub}; 3 | 4 | use cgmath::{ 5 | BaseFloat, Basis2, EuclideanSpace, InnerSpace, Matrix3, Point2, Point3, Quaternion, Rotation, 6 | Transform, Vector3, Zero, 7 | }; 8 | use collision::dbvt::TreeValue; 9 | use collision::{Bound, ComputeBound, Contains, Discrete, HasBound, SurfaceArea, Union}; 10 | use core::{ 11 | ApplyAngular, BroadPhase, GetId, Inertia, NarrowPhase, PartialCrossProduct, PhysicsTime, Pose, 12 | Primitive, 13 | }; 14 | use specs::prelude::{Component, DispatcherBuilder, Entity, Tracked}; 15 | 16 | /// Create systems and add to a `Dispatcher` graph. 17 | /// 18 | /// ### Parameters 19 | /// 20 | /// - `dispatcher`: The dispatcher to add the systems to. 21 | /// - `broad_phase`: Broad phase to use 22 | /// - `narrow_phase`: Narrow phase to use 23 | /// - `spatial`: If spatial or basic collision detection should be used 24 | /// 25 | /// ### Type parameters: 26 | /// 27 | /// - `P`: Shape primitive 28 | /// - `T`: Pose (transform) 29 | /// - `B`: Bounding volume 30 | /// - `D`: Broad phase data, usually `TreeValueWrapped`. 31 | /// - `Y`: Collider 32 | /// - `V`: Broad phase algorithm 33 | /// - `N`: Narrow phase algorithm 34 | /// - `R`: Rotational quantity, `Basis2` or `Quaternion` 35 | /// - `A`: Angular velocity, `Scalar` or `Vector3` 36 | /// - `I`: Inertia, `Scalar` or `Matrix3` 37 | /// - `DT`: Time quantity, usually `DeltaTime` 38 | /// - `O`: Internal type used to abstract cross product for 2D vs 3D, `Scalar` or `Vector3` 39 | pub fn setup_dispatch<'a, 'b, P, T, B, D, Y, V, N, R, A, I, DT, O>( 40 | dispatcher: &mut DispatcherBuilder<'a, 'b>, 41 | broad_phase: V, 42 | narrow_phase: N, 43 | spatial: bool, 44 | ) where 45 | V: BroadPhase + BroadPhase<(usize, D)> + 'static, 46 | N: NarrowPhase + 'static, 47 | P: Primitive + ComputeBound + Send + Sync + 'static, 48 | P::Point: Debug + Send + Sync + 'static, 49 | ::Scalar: BaseFloat + Send + Sync + 'static, 50 | ::Diff: 51 | InnerSpace 52 | + PartialCrossProduct<::Diff, Output = O> 53 | + Debug 54 | + Send 55 | + Sync 56 | + 'static, 57 | T: Debug + Component + Pose + Transform + Send + Sync + Clone + 'static, 58 | T::Storage: Tracked, 59 | Y: Default + Send + Sync + 'static, 60 | B: Bound 61 | + Send 62 | + Sync 63 | + 'static 64 | + Union 65 | + Clone 66 | + Contains 67 | + SurfaceArea::Scalar> 68 | + Discrete 69 | + Debug, 70 | (usize, D): HasBound, 71 | D: TreeValue 72 | + HasBound 73 | + From<(Entity, B)> 74 | + GetId 75 | + Send 76 | + Sync 77 | + 'static, 78 | R: Rotation 79 | + ApplyAngular<::Scalar, A> 80 | + Send 81 | + Sync 82 | + 'static, 83 | I: Inertia + Mul + Mul + Send + Sync + 'static, 84 | A: Mul<::Scalar, Output = A> 85 | + PartialCrossProduct< 86 | ::Diff, 87 | Output = ::Diff, 88 | > + Zero 89 | + Clone 90 | + Copy 91 | + Send 92 | + Sync 93 | + 'static, 94 | DT: PhysicsTime<::Scalar> + Default + Send + Sync + 'static, 95 | O: PartialCrossProduct< 96 | ::Diff, 97 | Output = ::Diff, 98 | > + Send 99 | + Sync 100 | + 'static, 101 | for<'c> &'c A: Sub + Add, 102 | { 103 | use { 104 | BasicCollisionSystem, ContactResolutionSystem, CurrentFrameUpdateSystem, 105 | NextFrameSetupSystem, SpatialCollisionSystem, SpatialSortingSystem, 106 | }; 107 | dispatcher.add( 108 | CurrentFrameUpdateSystem::::new(), 109 | "physics_solver_system", 110 | &[], 111 | ); 112 | dispatcher.add( 113 | NextFrameSetupSystem::::new(), 114 | "next_frame_setup", 115 | &["physics_solver_system"], 116 | ); 117 | if spatial { 118 | dispatcher.add( 119 | SpatialSortingSystem::::new(), 120 | "spatial_sorting_system", 121 | &["next_frame_setup"], 122 | ); 123 | dispatcher.add( 124 | SpatialCollisionSystem::::new() 125 | .with_broad_phase(broad_phase) 126 | .with_narrow_phase(narrow_phase), 127 | "collision_system", 128 | &["spatial_sorting_system"], 129 | ); 130 | } else { 131 | dispatcher.add( 132 | BasicCollisionSystem::::new() 133 | .with_broad_phase(broad_phase) 134 | .with_narrow_phase(narrow_phase), 135 | "collision_system", 136 | &["next_frame_setup"], 137 | ); 138 | } 139 | dispatcher.add( 140 | ContactResolutionSystem::::new(), 141 | "contact_resolution", 142 | &["collision_system"], 143 | ); 144 | } 145 | 146 | /// Create systems for 2D and add to a `Dispatcher` graph. 147 | /// 148 | /// ### Parameters 149 | /// 150 | /// - `dispatcher`: The dispatcher to add the systems to. 151 | /// - `broad_phase`: Broad phase to use 152 | /// - `narrow_phase`: Narrow phase to use 153 | /// - `spatial`: If spatial or basic collision detection should be used 154 | /// 155 | /// ### Type parameters: 156 | /// 157 | /// - `S`: Scalar type, `f32` or `f64` 158 | /// - `P`: Shape primitive 159 | /// - `T`: Pose (transform) 160 | /// - `B`: Bounding volume 161 | /// - `D`: Broad phase data, usually `TreeValueWrapped`. 162 | /// - `Y`: Collider 163 | /// - `V`: Broad phase algorithm 164 | /// - `N`: Narrow phase algorithm 165 | /// - `DT`: Time quantity, usually `DeltaTime` 166 | pub fn setup_dispatch_2d<'a, 'b, S, P, T, B, D, Y, V, N, DT>( 167 | dispatcher: &mut DispatcherBuilder<'a, 'b>, 168 | broad_phase: V, 169 | narrow_phase: N, 170 | spatial: bool, 171 | ) where 172 | V: BroadPhase + BroadPhase<(usize, D)> + 'static, 173 | N: NarrowPhase + 'static, 174 | P: Primitive> + ComputeBound + Send + Sync + 'static, 175 | S: Inertia> + BaseFloat + Send + Sync + 'static, 176 | T: Component 177 | + Pose, Basis2> 178 | + Debug 179 | + Transform> 180 | + Send 181 | + Sync 182 | + Clone 183 | + 'static, 184 | T::Storage: Tracked, 185 | Y: Default + Send + Sync + 'static, 186 | B: Bound> 187 | + Send 188 | + Sync 189 | + 'static 190 | + Union 191 | + Clone 192 | + Contains 193 | + SurfaceArea 194 | + Discrete 195 | + Debug, 196 | (usize, D): HasBound, 197 | D: TreeValue 198 | + HasBound 199 | + From<(Entity, B)> 200 | + GetId 201 | + Send 202 | + Sync 203 | + 'static, 204 | DT: PhysicsTime + Default + Send + Sync + 'static, 205 | for<'c> &'c S: Sub + Add, 206 | { 207 | setup_dispatch::, S, S, DT, S>( 208 | dispatcher, 209 | broad_phase, 210 | narrow_phase, 211 | spatial, 212 | ); 213 | } 214 | 215 | /// Create systems for 3sD and add to a `Dispatcher` graph. 216 | /// 217 | /// ### Parameters 218 | /// 219 | /// - `dispatcher`: The dispatcher to add the systems to. 220 | /// - `broad_phase`: Broad phase to use 221 | /// - `narrow_phase`: Narrow phase to use 222 | /// - `spatial`: If spatial or basic collision detection should be used 223 | /// 224 | /// ### Type parameters: 225 | /// 226 | /// - `S`: Scalar type, `f32` or `f64` 227 | /// - `P`: Shape primitive 228 | /// - `T`: Pose (transform) 229 | /// - `B`: Bounding volume 230 | /// - `D`: Broad phase data, usually `TreeValueWrapped`. 231 | /// - `Y`: Collider 232 | /// - `V`: Broad phase algorithm 233 | /// - `N`: Narrow phase algorithm 234 | /// - `DT`: Time quantity, usually `DeltaTime` 235 | pub fn setup_dispatch_3d( 236 | dispatcher: &mut DispatcherBuilder, 237 | broad_phase: V, 238 | narrow_phase: N, 239 | spatial: bool, 240 | ) where 241 | V: BroadPhase + BroadPhase<(usize, D)> + 'static, 242 | N: NarrowPhase + 'static, 243 | P: Primitive> + ComputeBound + Send + Sync + 'static, 244 | S: BaseFloat + Send + Sync + 'static, 245 | T: Component 246 | + Pose, Quaternion> 247 | + Transform> 248 | + Debug 249 | + Send 250 | + Sync 251 | + Clone 252 | + 'static, 253 | T::Storage: Tracked, 254 | Y: Default + Send + Sync + 'static, 255 | B: Bound> 256 | + Send 257 | + Sync 258 | + 'static 259 | + Union 260 | + Clone 261 | + Contains 262 | + SurfaceArea 263 | + Discrete 264 | + Debug, 265 | (usize, D): HasBound, 266 | D: TreeValue 267 | + HasBound 268 | + From<(Entity, B)> 269 | + GetId 270 | + Send 271 | + Sync 272 | + 'static, 273 | DT: PhysicsTime + Default + Send + Sync + 'static, 274 | { 275 | setup_dispatch::, Vector3, Matrix3, DT, Vector3>( 276 | dispatcher, 277 | broad_phase, 278 | narrow_phase, 279 | spatial, 280 | ); 281 | } 282 | 283 | #[cfg(test)] 284 | mod tests { 285 | 286 | use super::*; 287 | use collide2d::{BodyPose2, SweepAndPrune2, GJK2}; 288 | use collide3d::{BodyPose3, SweepAndPrune3, GJK3}; 289 | use collision::dbvt::TreeValueWrapped; 290 | use collision::primitive::{Primitive2, Primitive3}; 291 | use collision::{Aabb2, Aabb3}; 292 | use DeltaTime; 293 | 294 | #[test] 295 | fn test_dispatch() { 296 | let mut builder = DispatcherBuilder::new(); 297 | setup_dispatch::< 298 | Primitive2, 299 | BodyPose2, 300 | Aabb2, 301 | TreeValueWrapped>, 302 | (), 303 | _, 304 | _, 305 | _, 306 | _, 307 | f32, 308 | DeltaTime, 309 | _, 310 | >(&mut builder, SweepAndPrune2::new(), GJK2::new(), false); 311 | } 312 | 313 | #[test] 314 | fn test_dispatch_2d() { 315 | let mut builder = DispatcherBuilder::new(); 316 | setup_dispatch_2d::< 317 | _, 318 | Primitive2, 319 | BodyPose2, 320 | Aabb2, 321 | TreeValueWrapped>, 322 | (), 323 | _, 324 | _, 325 | DeltaTime, 326 | >(&mut builder, SweepAndPrune2::new(), GJK2::new(), false); 327 | } 328 | 329 | #[test] 330 | fn test_dispatch_3d() { 331 | let mut builder = DispatcherBuilder::new(); 332 | setup_dispatch_3d::< 333 | _, 334 | Primitive3, 335 | BodyPose3, 336 | Aabb3, 337 | TreeValueWrapped>, 338 | (), 339 | _, 340 | _, 341 | DeltaTime, 342 | >(&mut builder, SweepAndPrune3::new(), GJK3::new(), false); 343 | } 344 | } 345 | -------------------------------------------------------------------------------- /rhusics-ecs/src/physics/systems/contact_resolution.rs: -------------------------------------------------------------------------------- 1 | use std::fmt::Debug; 2 | use std::marker; 3 | use std::ops::{Add, Mul, Sub}; 4 | 5 | use cgmath::{BaseFloat, EuclideanSpace, InnerSpace, Rotation, VectorSpace, Zero}; 6 | use core::{ 7 | resolve_contact, ApplyAngular, ContactEvent, Inertia, Mass, PartialCrossProduct, 8 | PhysicalEntity, ResolveData, Velocity, 9 | }; 10 | use core::{NextFrame, Pose}; 11 | use shrev::{EventChannel, ReaderId}; 12 | use specs::prelude::{Component, Entity, Read, ReadStorage, World, System, WriteStorage}; 13 | 14 | /// Do single contact, forward resolution. 15 | /// 16 | /// ### Type parameters: 17 | /// 18 | /// - `P`: Positional quantity, usually `Point2` or `Point3` 19 | /// - `R`: Rotational quantity, usually `Basis2` or `Quaternion` 20 | /// - `I`: Inertia, usually `Scalar` or `Matrix3` 21 | /// - `A`: Angular velocity, usually `Scalar` or `Vector3` 22 | /// - `O`: Internal type used for abstracting over cross products in 2D/3D, 23 | /// usually `Scalar` or `Vector3` 24 | /// - `T`: Transform type (`BodyPose2` or similar) 25 | /// 26 | /// ### System function 27 | /// 28 | /// `fn(EventChannel, Mass, PhysicalEntity, T, NextFrame, NextFrame) -> (NextFrame, NextFrame)` 29 | /// 30 | pub struct ContactResolutionSystem 31 | where 32 | P: EuclideanSpace + 'static, 33 | P::Diff: Debug, 34 | { 35 | contact_reader: Option>>, 36 | m: marker::PhantomData<(R, I, A, O, T)>, 37 | } 38 | 39 | impl ContactResolutionSystem 40 | where 41 | T: Pose, 42 | P: EuclideanSpace, 43 | P::Scalar: BaseFloat, 44 | P::Diff: VectorSpace + InnerSpace + Debug + PartialCrossProduct, 45 | R: Rotation

+ ApplyAngular, 46 | O: PartialCrossProduct, 47 | A: PartialCrossProduct + Clone + Zero, 48 | for<'b> &'b A: Sub + Add, 49 | I: Inertia + Mul, 50 | { 51 | /// Create system. 52 | pub fn new() -> Self { 53 | Self { 54 | contact_reader: None, 55 | m: marker::PhantomData, 56 | } 57 | } 58 | } 59 | 60 | impl<'a, P, R, I, A, O, T> System<'a> for ContactResolutionSystem 61 | where 62 | T: Pose + Component + Send + Sync + 'static, 63 | P: EuclideanSpace + Send + Sync + 'static, 64 | P::Scalar: BaseFloat + Send + Sync + 'static, 65 | P::Diff: VectorSpace 66 | + InnerSpace 67 | + Debug 68 | + Send 69 | + Sync 70 | + 'static 71 | + PartialCrossProduct, 72 | R: Rotation

+ ApplyAngular + Send + Sync + 'static, 73 | O: PartialCrossProduct, 74 | A: PartialCrossProduct + Clone + Zero + Send + Sync + 'static, 75 | for<'b> &'b A: Sub + Add, 76 | I: Inertia + Mul + Send + Sync + 'static, 77 | { 78 | type SystemData = ( 79 | Read<'a, EventChannel>>, 80 | ReadStorage<'a, Mass>, 81 | ReadStorage<'a, PhysicalEntity>, 82 | WriteStorage<'a, NextFrame>>, 83 | ReadStorage<'a, T>, 84 | WriteStorage<'a, NextFrame>, 85 | ); 86 | 87 | fn run(&mut self, data: Self::SystemData) { 88 | let (contacts, masses, entities, mut next_velocities, poses, mut next_poses) = data; 89 | 90 | // Process contacts since last run 91 | for contact in contacts.read(&mut self.contact_reader.as_mut().unwrap()) { 92 | // Resolve contact 93 | let change_set = match ( 94 | from_storage( 95 | contact.bodies.0, 96 | &next_velocities, 97 | &next_poses, 98 | &poses, 99 | &masses, 100 | &entities, 101 | ), 102 | from_storage( 103 | contact.bodies.1, 104 | &next_velocities, 105 | &next_poses, 106 | &poses, 107 | &masses, 108 | &entities, 109 | ), 110 | ) { 111 | (Some(resolve_0), Some(resolve_1)) => { 112 | Some(resolve_contact(&contact.contact, &resolve_0, &resolve_1)) 113 | } 114 | _ => None, 115 | }; 116 | if let Some(cs) = change_set { 117 | // Apply computed change sets 118 | cs.0.apply( 119 | next_poses.get_mut(contact.bodies.0), 120 | next_velocities.get_mut(contact.bodies.0), 121 | ); 122 | cs.1.apply( 123 | next_poses.get_mut(contact.bodies.1), 124 | next_velocities.get_mut(contact.bodies.1), 125 | ); 126 | } 127 | } 128 | } 129 | 130 | fn setup(&mut self, res: &mut World) { 131 | use specs::prelude::SystemData; 132 | Self::SystemData::setup(res); 133 | self.contact_reader = Some( 134 | res.fetch_mut::>>() 135 | .register_reader(), 136 | ); 137 | } 138 | } 139 | 140 | fn from_storage<'a, P, T, R, A, I>( 141 | entity: Entity, 142 | next_velocities: &'a WriteStorage>>, 143 | next_poses: &'a WriteStorage>, 144 | poses: &'a ReadStorage, 145 | masses: &'a ReadStorage>, 146 | entities: &'a ReadStorage>, 147 | ) -> Option> 148 | where 149 | P: EuclideanSpace + Send + Sync + 'static, 150 | P::Scalar: BaseFloat + Send + Sync + 'static, 151 | P::Diff: Send + Sync + 'static, 152 | T: Pose + Component + Send + Sync + 'static, 153 | R: Rotation

+ Send + Sync + 'static, 154 | A: Clone + Send + Sync + 'static, 155 | I: Clone + Send + Sync + 'static, 156 | { 157 | match (entities.get(entity), masses.get(entity), poses.get(entity)) { 158 | (Some(e), Some(mass), Some(pose)) if e.active() => Some(ResolveData::new( 159 | next_velocities.get(entity), 160 | next_poses.get(entity).map(|p| &p.value).unwrap_or(pose), 161 | mass, 162 | e.material(), 163 | )), 164 | _ => None, 165 | } 166 | } 167 | -------------------------------------------------------------------------------- /rhusics-ecs/src/physics/systems/current_frame.rs: -------------------------------------------------------------------------------- 1 | use std::fmt::Debug; 2 | use std::marker; 3 | 4 | use cgmath::{BaseFloat, EuclideanSpace, InnerSpace, Rotation, VectorSpace, Zero}; 5 | use core::{NextFrame, PhysicalEntity, Pose, Velocity}; 6 | use specs::prelude::{Component, Join, ReadStorage, System, WriteStorage}; 7 | 8 | /// Current frame update system. 9 | /// 10 | /// Will update positions and velocities for the current frame, based on `NextFrame` values. 11 | /// 12 | /// ### Type parameters: 13 | /// 14 | /// - `P`: Positional quantity, usually `Point2` or `Point3` 15 | /// - `R`: Rotational quantity, usually `Basis2` or `Quaternion` 16 | /// - `A`: Angular velocity, usually `Scalar` or `Vector3` 17 | /// - `T`: Transform type (`BodyPose2` or similar) 18 | /// 19 | /// ### System function: 20 | /// 21 | /// `fn(NextFrame, NextFrame) -> (Velocity, T)` 22 | pub struct CurrentFrameUpdateSystem { 23 | m: marker::PhantomData<(P, R, A, T)>, 24 | } 25 | 26 | impl CurrentFrameUpdateSystem 27 | where 28 | P: EuclideanSpace, 29 | P::Diff: VectorSpace + InnerSpace + Debug, 30 | P::Scalar: BaseFloat, 31 | R: Rotation

, 32 | A: Clone + Zero, 33 | T: Pose, 34 | { 35 | /// Create system. 36 | pub fn new() -> Self { 37 | Self { 38 | m: marker::PhantomData, 39 | } 40 | } 41 | } 42 | 43 | impl<'a, P, R, A, T> System<'a> for CurrentFrameUpdateSystem 44 | where 45 | P: EuclideanSpace + Send + Sync + 'static, 46 | P::Diff: VectorSpace + InnerSpace + Debug + Send + Sync + 'static, 47 | P::Scalar: BaseFloat + Send + Sync + 'static, 48 | R: Rotation

+ Send + Sync + 'static, 49 | A: Clone + Zero + Send + Sync + 'static, 50 | T: Pose + Component + Clone + Send + Sync + 'static, 51 | { 52 | type SystemData = ( 53 | ReadStorage<'a, PhysicalEntity>, 54 | WriteStorage<'a, Velocity>, 55 | ReadStorage<'a, NextFrame>>, 56 | WriteStorage<'a, T>, 57 | ReadStorage<'a, NextFrame>, 58 | ); 59 | 60 | fn run(&mut self, data: Self::SystemData) { 61 | let (entities, mut velocities, next_velocities, mut poses, next_poses) = data; 62 | 63 | // Update current pose 64 | for (_, next, pose) in (&entities, &next_poses, &mut poses) 65 | .join() 66 | .filter(|(e, ..)| e.active()) 67 | { 68 | *pose = next.value.clone(); 69 | } 70 | 71 | // Update current velocity 72 | for (_, next, velocity) in (&entities, &next_velocities, &mut velocities) 73 | .join() 74 | .filter(|(e, ..)| e.active()) 75 | { 76 | *velocity = next.value.clone(); 77 | } 78 | } 79 | } 80 | -------------------------------------------------------------------------------- /rhusics-ecs/src/physics/systems/mod.rs: -------------------------------------------------------------------------------- 1 | //! Physics systems 2 | 3 | pub use self::contact_resolution::ContactResolutionSystem; 4 | pub use self::current_frame::CurrentFrameUpdateSystem; 5 | pub use self::next_frame::NextFrameSetupSystem; 6 | 7 | mod contact_resolution; 8 | mod current_frame; 9 | mod next_frame; 10 | -------------------------------------------------------------------------------- /rhusics-ecs/src/physics/systems/next_frame.rs: -------------------------------------------------------------------------------- 1 | use std::fmt::Debug; 2 | use std::marker; 3 | use std::ops::Mul; 4 | 5 | use cgmath::{BaseFloat, EuclideanSpace, InnerSpace, Rotation, VectorSpace, Zero}; 6 | use core::{ 7 | next_frame_integration, next_frame_pose, ApplyAngular, ForceAccumulator, Inertia, Mass, 8 | NextFrame, PhysicalEntity, PhysicsTime, Pose, Velocity, WorldParameters, 9 | }; 10 | use specs::prelude::{Component, Join, Read, ReadStorage, System, WriteStorage}; 11 | 12 | /// Setup the next frames positions and velocities. 13 | /// 14 | /// ### Type parameters: 15 | /// 16 | /// - `P`: Positional quantity, usually `Point2` or `Point3` 17 | /// - `R`: Rotational quantity, usually `Basis2` or `Quaternion` 18 | /// - `I`: Inertia, usually `Scalar` or `Matrix3` 19 | /// - `A`: Angular velocity, usually `Scalar` or `Vector3` 20 | /// - `T`: Transform type (`BodyPose2` or similar) 21 | /// 22 | /// ### System function 23 | /// 24 | /// `fn(DeltaTime, Mass, T, ForceAccumulator) -> (ForceAccumulator, NextFrame, NextFrame)` 25 | pub struct NextFrameSetupSystem { 26 | m: marker::PhantomData<(P, R, I, A, T, D)>, 27 | } 28 | 29 | impl NextFrameSetupSystem 30 | where 31 | T: Pose, 32 | P: EuclideanSpace, 33 | P::Scalar: BaseFloat, 34 | P::Diff: VectorSpace + InnerSpace + Debug, 35 | R: Rotation

+ ApplyAngular, 36 | I: Inertia + Mul, 37 | A: Mul + Zero + Clone + Copy, 38 | D: PhysicsTime + Default, 39 | { 40 | /// Create system. 41 | pub fn new() -> Self { 42 | Self { 43 | m: marker::PhantomData, 44 | } 45 | } 46 | } 47 | 48 | impl<'a, P, R, I, A, T, D> System<'a> for NextFrameSetupSystem 49 | where 50 | T: Pose + Component + Send + Sync + 'static, 51 | P: EuclideanSpace + Send + Sync + 'static, 52 | P::Scalar: BaseFloat + Send + Sync + 'static, 53 | P::Diff: VectorSpace + InnerSpace + Debug + Send + Sync + 'static, 54 | R: Rotation

+ ApplyAngular + Send + Sync + 'static, 55 | I: Inertia + Mul + Send + Sync + 'static, 56 | A: Mul + Zero + Clone + Copy + Send + Sync + 'static, 57 | D: PhysicsTime + Default + Send + Sync + 'static, 58 | { 59 | type SystemData = ( 60 | Read<'a, D>, 61 | Read<'a, WorldParameters>, 62 | ReadStorage<'a, PhysicalEntity>, 63 | ReadStorage<'a, Mass>, 64 | WriteStorage<'a, NextFrame>>, 65 | ReadStorage<'a, T>, 66 | WriteStorage<'a, NextFrame>, 67 | WriteStorage<'a, ForceAccumulator>, 68 | ); 69 | 70 | fn run(&mut self, data: Self::SystemData) { 71 | let ( 72 | time, 73 | params, 74 | entities, 75 | masses, 76 | mut next_velocities, 77 | poses, 78 | mut next_poses, 79 | mut forces, 80 | ) = data; 81 | 82 | // Do force integration 83 | next_frame_integration( 84 | ( 85 | &mut next_velocities, 86 | &next_poses, 87 | &mut forces, 88 | &masses, 89 | &entities, 90 | ) 91 | .join(), 92 | &*params, 93 | time.delta_seconds(), 94 | ); 95 | 96 | // Compute next frames position 97 | next_frame_pose( 98 | (&next_velocities, &poses, &mut next_poses, &entities).join(), 99 | time.delta_seconds(), 100 | ); 101 | } 102 | } 103 | -------------------------------------------------------------------------------- /rhusics-ecs/src/physics2d.rs: -------------------------------------------------------------------------------- 1 | //! 2D physics ECS 2 | 3 | pub use collide2d::*; 4 | pub use core::physics2d::*; 5 | pub use physics::setup_dispatch_2d; 6 | 7 | use cgmath::{Basis2, Point2, Vector2}; 8 | use collision::primitive::Primitive2; 9 | use collision::Aabb2; 10 | 11 | use physics::{ 12 | ContactResolutionSystem, CurrentFrameUpdateSystem, DeltaTime, NextFrameSetupSystem, 13 | PhysicalEntityParts, 14 | }; 15 | 16 | /// Current frame integrator system for 2D 17 | /// 18 | /// ### Type parameters: 19 | /// 20 | /// - `S`: Scalar type (f32 or f64) 21 | /// - `T`: Transform 22 | pub type CurrentFrameUpdateSystem2 = CurrentFrameUpdateSystem, Basis2, S, T>; 23 | 24 | /// Resolution system for 2D 25 | /// 26 | /// ### Type parameters: 27 | /// 28 | /// - `S`: Scalar type (f32 or f64) 29 | /// - `T`: Transform type (`BodyPose2` or similar) 30 | pub type ContactResolutionSystem2 = ContactResolutionSystem, Basis2, S, S, S, T>; 31 | 32 | /// Next frame setup system for 2D 33 | /// 34 | /// ### Type parameters: 35 | /// 36 | /// - `S`: Scalar type (f32 or f64) 37 | /// - `T`: Transform type (`BodyPose2` or similar) 38 | pub type NextFrameSetupSystem2 = 39 | NextFrameSetupSystem, Basis2, S, S, T, DeltaTime>; 40 | 41 | /// SystemData for 2D 42 | /// 43 | /// ### Type parameters: 44 | /// 45 | /// - `S`: Scalar type (f32 or f64) 46 | /// - `T`: Transform type (`BodyPose2` or similar) 47 | /// - `Y`: Collision shape type, see `Collider` 48 | pub type PhysicalEntityParts2<'a, S, T, Y> = 49 | PhysicalEntityParts<'a, Primitive2, Y, Basis2, Vector2, S, S, Aabb2, T>; 50 | -------------------------------------------------------------------------------- /rhusics-ecs/src/physics3d.rs: -------------------------------------------------------------------------------- 1 | //! 3D physics ECS 2 | 3 | pub use collide3d::*; 4 | pub use core::physics3d::*; 5 | pub use physics::setup_dispatch_3d; 6 | 7 | use cgmath::{Matrix3, Point3, Quaternion, Vector3}; 8 | use collision::primitive::Primitive3; 9 | use collision::Aabb3; 10 | 11 | use physics::{ 12 | ContactResolutionSystem, CurrentFrameUpdateSystem, DeltaTime, NextFrameSetupSystem, 13 | PhysicalEntityParts, 14 | }; 15 | 16 | /// Current frame integrator system for 2D 17 | /// 18 | /// ### Type parameters: 19 | /// 20 | /// - `S`: Scalar type (f32 or f64) 21 | /// - `T`: Transform type (`BodyPose3` or similar) 22 | pub type CurrentFrameUpdateSystem3 = 23 | CurrentFrameUpdateSystem, Quaternion, Vector3, T>; 24 | 25 | /// Resolution system for 2D 26 | /// 27 | /// ### Type parameters: 28 | /// 29 | /// - `S`: Scalar type (f32 or f64) 30 | /// - `T`: Transform type (`BodyPose3` or similar) 31 | pub type ContactResolutionSystem3 = 32 | ContactResolutionSystem, Quaternion, Matrix3, Vector3, Vector3, T>; 33 | 34 | /// Next frame setup system for 2D 35 | /// 36 | /// ### Type parameters: 37 | /// 38 | /// - `S`: Scalar type (f32 or f64) 39 | /// - `T`: Transform type (`BodyPose3` or similar) 40 | pub type NextFrameSetupSystem3 = 41 | NextFrameSetupSystem, Quaternion, Matrix3, Vector3, T, DeltaTime>; 42 | 43 | /// SystemData for 3D 44 | /// 45 | /// ### Type parameters: 46 | /// 47 | /// - `S`: Scalar type (f32 or f64) 48 | /// - `T`: Transform type (`BodyPose3` or similar) 49 | /// - `Y`: Collision shape type, see `Collider` 50 | pub type PhysicalEntityParts3<'a, S, T, Y> = PhysicalEntityParts< 51 | 'a, 52 | Primitive3, 53 | Y, 54 | Quaternion, 55 | Vector3, 56 | Vector3, 57 | Matrix3, 58 | Aabb3, 59 | T, 60 | >; 61 | -------------------------------------------------------------------------------- /rhusics-transform/Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "rhusics-transform" 3 | version = "0.5.0" 4 | authors = ["Simon Rönnberg "] 5 | repository = "https://github.com/rustgd/rhusics.git" 6 | homepage = "https://github.com/rustgd/rhusics.git" 7 | 8 | license = "MIT OR Apache-2.0" 9 | documentation = "https://docs.rs/rhusics-transform" 10 | description = "Physics library for use with `specs`" 11 | 12 | keywords = ["gamedev", "cgmath", "specs", "physics"] 13 | 14 | [dependencies] 15 | cgmath = "0.17" 16 | collision = { version = "0.20" } 17 | -------------------------------------------------------------------------------- /rhusics-transform/src/lib.rs: -------------------------------------------------------------------------------- 1 | extern crate cgmath; 2 | extern crate collision; 3 | 4 | use cgmath::{BaseFloat, Decomposed, EuclideanSpace, One, Rotation, Transform}; 5 | pub use collision::{Interpolate, TranslationInterpolate}; 6 | 7 | /// Pose abstraction 8 | pub trait Pose: Transform

9 | where 10 | P: EuclideanSpace, 11 | { 12 | /// New pose 13 | fn new(position: P, rotation: R) -> Self; 14 | /// Set rotation 15 | fn set_rotation(&mut self, rotation: R); 16 | /// Set position 17 | fn set_position(&mut self, position: P); 18 | /// Read rotation 19 | fn rotation(&self) -> R; 20 | /// Read position 21 | fn position(&self) -> P; 22 | } 23 | 24 | pub trait PhysicsTime { 25 | fn delta_seconds(&self) -> S; 26 | } 27 | 28 | impl Pose for Decomposed 29 | where 30 | P: EuclideanSpace, 31 | P::Scalar: BaseFloat, 32 | R: Rotation

, 33 | { 34 | fn new(position: P, rotation: R) -> Self { 35 | Decomposed { 36 | rot: rotation, 37 | disp: position.to_vec(), 38 | scale: P::Scalar::one(), 39 | } 40 | } 41 | 42 | fn set_rotation(&mut self, rotation: R) { 43 | self.rot = rotation; 44 | } 45 | 46 | fn set_position(&mut self, position: P) { 47 | self.disp = position.to_vec(); 48 | } 49 | 50 | fn rotation(&self) -> R { 51 | self.rot 52 | } 53 | 54 | fn position(&self) -> P { 55 | P::from_vec(self.disp) 56 | } 57 | } 58 | -------------------------------------------------------------------------------- /rustfmt.toml: -------------------------------------------------------------------------------- 1 | reorder_imports=true 2 | --------------------------------------------------------------------------------