├── .gitignore ├── .travis.yml ├── Cargo.toml ├── LICENSE ├── README.md ├── justfile └── src ├── lib.rs ├── nav_grid.rs ├── nav_islands.rs ├── nav_mesh.rs ├── nav_net.rs └── nav_vec3.rs /.gitignore: -------------------------------------------------------------------------------- 1 | /target 2 | **/*.rs.bk 3 | Cargo.lock 4 | -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | language: rust 2 | rust: 3 | - 1.41.0 4 | cache: cargo 5 | matrix: 6 | fast_finish: true 7 | os: 8 | - windows 9 | - linux 10 | - osx 11 | script: 12 | - cargo test --verbose --all 13 | before_install: 14 | - if [[ "$TRAVIS_OS_NAME" == "linux" ]]; then sudo apt-get install -y libasound2-dev ; sudo apt-get install -y libwebkit2gtk-4.0 ; fi 15 | -------------------------------------------------------------------------------- /Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "navmesh" 3 | version = "0.12.1" 4 | authors = ["Patryk 'PsichiX' Budzynski "] 5 | edition = "2021" 6 | description = "NavMesh, NavNet, NavGrid, NavFreeGrid and NavIslands navigation system" 7 | license = "MIT OR Apache-2.0" 8 | homepage = "https://github.com/PsichiX/navmesh" 9 | repository = "https://github.com/PsichiX/navmesh" 10 | documentation = "https://docs.rs/navmesh" 11 | readme = "./README.md" 12 | 13 | [features] 14 | parallel = ["rayon"] 15 | scalar64 = [] 16 | convert = ["mint"] 17 | 18 | [dependencies] 19 | typid = "1" 20 | petgraph = { version = "0.6", features = ["serde-1"] } 21 | spade = { version = "1.8", features = ["serde_serialize"] } 22 | serde = { version = "1", features = ["derive"] } 23 | rayon = { version = "1.5", optional = true } 24 | approx = "0.5" 25 | mint = { version = "0.5", features = ["serde"], optional = true } 26 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (C) 2019 Patryk 'PsichiX' Budzyński 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | 23 | ================================================================================ 24 | 25 | Apache License 26 | Version 2.0, January 2004 27 | http://www.apache.org/licenses/ 28 | 29 | Copyright (C) 2019 Patryk 'PsichiX' Budzyński 30 | 31 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 32 | 33 | 1. Definitions. 34 | 35 | "License" shall mean the terms and conditions for use, reproduction, 36 | and distribution as defined by Sections 1 through 9 of this document. 37 | 38 | "Licensor" shall mean the copyright owner or entity authorized by 39 | the copyright owner that is granting the License. 40 | 41 | "Legal Entity" shall mean the union of the acting entity and all 42 | other entities that control, are controlled by, or are under common 43 | control with that entity. For the purposes of this definition, 44 | "control" means (i) the power, direct or indirect, to cause the 45 | direction or management of such entity, whether by contract or 46 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 47 | outstanding shares, or (iii) beneficial ownership of such entity. 48 | 49 | "You" (or "Your") shall mean an individual or Legal Entity 50 | exercising permissions granted by this License. 51 | 52 | "Source" form shall mean the preferred form for making modifications, 53 | including but not limited to software source code, documentation 54 | source, and configuration files. 55 | 56 | "Object" form shall mean any form resulting from mechanical 57 | transformation or translation of a Source form, including but 58 | not limited to compiled object code, generated documentation, 59 | and conversions to other media types. 60 | 61 | "Work" shall mean the work of authorship, whether in Source or 62 | Object form, made available under the License, as indicated by a 63 | copyright notice that is included in or attached to the work 64 | (an example is provided in the Appendix below). 65 | 66 | "Derivative Works" shall mean any work, whether in Source or Object 67 | form, that is based on (or derived from) the Work and for which the 68 | editorial revisions, annotations, elaborations, or other modifications 69 | represent, as a whole, an original work of authorship. For the purposes 70 | of this License, Derivative Works shall not include works that remain 71 | separable from, or merely link (or bind by name) to the interfaces of, 72 | the Work and Derivative Works thereof. 73 | 74 | "Contribution" shall mean any work of authorship, including 75 | the original version of the Work and any modifications or additions 76 | to that Work or Derivative Works thereof, that is intentionally 77 | submitted to Licensor for inclusion in the Work by the copyright owner 78 | or by an individual or Legal Entity authorized to submit on behalf of 79 | the copyright owner. For the purposes of this definition, "submitted" 80 | means any form of electronic, verbal, or written communication sent 81 | to the Licensor or its representatives, including but not limited to 82 | communication on electronic mailing lists, source code control systems, 83 | and issue tracking systems that are managed by, or on behalf of, the 84 | Licensor for the purpose of discussing and improving the Work, but 85 | excluding communication that is conspicuously marked or otherwise 86 | designated in writing by the copyright owner as "Not a Contribution." 87 | 88 | "Contributor" shall mean Licensor and any individual or Legal Entity 89 | on behalf of whom a Contribution has been received by Licensor and 90 | subsequently incorporated within the Work. 91 | 92 | 2. Grant of Copyright License. Subject to the terms and conditions of 93 | this License, each Contributor hereby grants to You a perpetual, 94 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 95 | copyright license to reproduce, prepare Derivative Works of, 96 | publicly display, publicly perform, sublicense, and distribute the 97 | Work and such Derivative Works in Source or Object form. 98 | 99 | 3. Grant of Patent License. Subject to the terms and conditions of 100 | this License, each Contributor hereby grants to You a perpetual, 101 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 102 | (except as stated in this section) patent license to make, have made, 103 | use, offer to sell, sell, import, and otherwise transfer the Work, 104 | where such license applies only to those patent claims licensable 105 | by such Contributor that are necessarily infringed by their 106 | Contribution(s) alone or by combination of their Contribution(s) 107 | with the Work to which such Contribution(s) was submitted. If You 108 | institute patent litigation against any entity (including a 109 | cross-claim or counterclaim in a lawsuit) alleging that the Work 110 | or a Contribution incorporated within the Work constitutes direct 111 | or contributory patent infringement, then any patent licenses 112 | granted to You under this License for that Work shall terminate 113 | as of the date such litigation is filed. 114 | 115 | 4. Redistribution. You may reproduce and distribute copies of the 116 | Work or Derivative Works thereof in any medium, with or without 117 | modifications, and in Source or Object form, provided that You 118 | meet the following conditions: 119 | 120 | (a) You must give any other recipients of the Work or 121 | Derivative Works a copy of this License; and 122 | 123 | (b) You must cause any modified files to carry prominent notices 124 | stating that You changed the files; and 125 | 126 | (c) You must retain, in the Source form of any Derivative Works 127 | that You distribute, all copyright, patent, trademark, and 128 | attribution notices from the Source form of the Work, 129 | excluding those notices that do not pertain to any part of 130 | the Derivative Works; and 131 | 132 | (d) If the Work includes a "NOTICE" text file as part of its 133 | distribution, then any Derivative Works that You distribute must 134 | include a readable copy of the attribution notices contained 135 | within such NOTICE file, excluding those notices that do not 136 | pertain to any part of the Derivative Works, in at least one 137 | of the following places: within a NOTICE text file distributed 138 | as part of the Derivative Works; within the Source form or 139 | documentation, if provided along with the Derivative Works; or, 140 | within a display generated by the Derivative Works, if and 141 | wherever such third-party notices normally appear. The contents 142 | of the NOTICE file are for informational purposes only and 143 | do not modify the License. You may add Your own attribution 144 | notices within Derivative Works that You distribute, alongside 145 | or as an addendum to the NOTICE text from the Work, provided 146 | that such additional attribution notices cannot be construed 147 | as modifying the License. 148 | 149 | You may add Your own copyright statement to Your modifications and 150 | may provide additional or different license terms and conditions 151 | for use, reproduction, or distribution of Your modifications, or 152 | for any such Derivative Works as a whole, provided Your use, 153 | reproduction, and distribution of the Work otherwise complies with 154 | the conditions stated in this License. 155 | 156 | 5. Submission of Contributions. Unless You explicitly state otherwise, 157 | any Contribution intentionally submitted for inclusion in the Work 158 | by You to the Licensor shall be under the terms and conditions of 159 | this License, without any additional terms or conditions. 160 | Notwithstanding the above, nothing herein shall supersede or modify 161 | the terms of any separate license agreement you may have executed 162 | with Licensor regarding such Contributions. 163 | 164 | 6. Trademarks. This License does not grant permission to use the trade 165 | names, trademarks, service marks, or product names of the Licensor, 166 | except as required for reasonable and customary use in describing the 167 | origin of the Work and reproducing the content of the NOTICE file. 168 | 169 | 7. Disclaimer of Warranty. Unless required by applicable law or 170 | agreed to in writing, Licensor provides the Work (and each 171 | Contributor provides its Contributions) on an "AS IS" BASIS, 172 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 173 | implied, including, without limitation, any warranties or conditions 174 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 175 | PARTICULAR PURPOSE. You are solely responsible for determining the 176 | appropriateness of using or redistributing the Work and assume any 177 | risks associated with Your exercise of permissions under this License. 178 | 179 | 8. Limitation of Liability. In no event and under no legal theory, 180 | whether in tort (including negligence), contract, or otherwise, 181 | unless required by applicable law (such as deliberate and grossly 182 | negligent acts) or agreed to in writing, shall any Contributor be 183 | liable to You for damages, including any direct, indirect, special, 184 | incidental, or consequential damages of any character arising as a 185 | result of this License or out of the use or inability to use the 186 | Work (including but not limited to damages for loss of goodwill, 187 | work stoppage, computer failure or malfunction, or any and all 188 | other commercial damages or losses), even if such Contributor 189 | has been advised of the possibility of such damages. 190 | 191 | 9. Accepting Warranty or Additional Liability. While redistributing 192 | the Work or Derivative Works thereof, You may choose to offer, 193 | and charge a fee for, acceptance of support, warranty, indemnity, 194 | or other liability obligations and/or rights consistent with this 195 | License. However, in accepting such obligations, You may act only 196 | on Your own behalf and on Your sole responsibility, not on behalf 197 | of any other Contributor, and only if You agree to indemnify, 198 | defend, and hold each Contributor harmless for any liability 199 | incurred by, or claims asserted against, such Contributor by reason 200 | of your accepting any such warranty or additional liability. 201 | 202 | END OF TERMS AND CONDITIONS 203 | 204 | APPENDIX: How to apply the Apache License to your work. 205 | 206 | To apply the Apache License to your work, attach the following 207 | boilerplate notice, with the fields enclosed by brackets "[]" 208 | replaced with your own identifying information. (Don't include 209 | the brackets!) The text should be enclosed in the appropriate 210 | comment syntax for the file format. We also recommend that a 211 | file or class name and description of purpose be included on the 212 | same "printed page" as the copyright notice for easier 213 | identification within third-party archives. 214 | 215 | Copyright [yyyy] [name of copyright owner] 216 | 217 | Licensed under the Apache License, Version 2.0 (the "License"); 218 | you may not use this file except in compliance with the License. 219 | You may obtain a copy of the License at 220 | 221 | http://www.apache.org/licenses/LICENSE-2.0 222 | 223 | Unless required by applicable law or agreed to in writing, software 224 | distributed under the License is distributed on an "AS IS" BASIS, 225 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 226 | See the License for the specific language governing permissions and 227 | limitations under the License. 228 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # NavMesh ![travis-ci status](https://travis-ci.org/PsichiX/navmesh.svg?branch=master) ![crates-io version](https://raster.shields.io/crates/v/navmesh.png) 2 | #### Nav-Mesh path finder for Rust 3 | 4 | ## Installation 5 | Cargo.toml 6 | ```toml 7 | [dependencies] 8 | navmesh = "0.8" 9 | ``` 10 | 11 | ## Example 12 | ```rust 13 | use navmesh::*; 14 | 15 | let vertices = vec![ 16 | (0.0, 0.0, 0.0).into(), // 0 17 | (1.0, 0.0, 0.0).into(), // 1 18 | (2.0, 0.0, 1.0).into(), // 2 19 | (0.0, 1.0, 0.0).into(), // 3 20 | (1.0, 1.0, 0.0).into(), // 4 21 | (2.0, 1.0, 1.0).into(), // 5 22 | ]; 23 | let triangles = vec![ 24 | (0, 1, 4).into(), // 0 25 | (4, 3, 0).into(), // 1 26 | (1, 2, 5).into(), // 2 27 | (5, 4, 1).into(), // 3 28 | ]; 29 | 30 | let mesh = NavMesh::new(vertices, triangles).unwrap(); 31 | let path = mesh 32 | .find_path( 33 | (0.0, 1.0, 0.0).into(), 34 | (1.5, 0.25, 0.5).into(), 35 | NavQuery::Accuracy, 36 | NavPathMode::MidPoints, 37 | ) 38 | .unwrap(); 39 | assert_eq!( 40 | path.into_iter() 41 | .map(|v| ( 42 | (v.x * 10.0) as i32, 43 | (v.y * 10.0) as i32, 44 | (v.z * 10.0) as i32, 45 | )) 46 | .collect::>(), 47 | vec![(0, 10, 0), (10, 5, 0), (15, 2, 5),] 48 | ); 49 | ``` 50 | -------------------------------------------------------------------------------- /justfile: -------------------------------------------------------------------------------- 1 | # List the just recipe list 2 | list: 3 | just --list 4 | 5 | # Mandatory checks to run before pushing changes to repository 6 | checks: 7 | cargo fmt 8 | cargo build 9 | cargo clippy 10 | cargo test 11 | -------------------------------------------------------------------------------- /src/lib.rs: -------------------------------------------------------------------------------- 1 | #[cfg(test)] 2 | #[macro_use] 3 | extern crate approx; 4 | 5 | mod nav_grid; 6 | mod nav_islands; 7 | mod nav_mesh; 8 | mod nav_net; 9 | mod nav_vec3; 10 | 11 | pub use crate::{nav_grid::*, nav_islands::*, nav_mesh::*, nav_net::*, nav_vec3::*}; 12 | 13 | use serde::{Deserialize, Serialize}; 14 | use std::{ 15 | hash::{Hash, Hasher}, 16 | result::Result as StdResult, 17 | }; 18 | 19 | #[cfg(feature = "scalar64")] 20 | pub type Scalar = f64; 21 | #[cfg(not(feature = "scalar64"))] 22 | pub type Scalar = f32; 23 | 24 | /// Error data. 25 | #[derive(Debug, Clone)] 26 | pub enum Error { 27 | /// Trying to construct triangle with vertice index out of vertices list. 28 | /// (triangle index, local vertice index, global vertice index) 29 | TriangleVerticeIndexOutOfBounds(u32, u8, u32), 30 | /// Trying to construct connection with vertice index out of vertices list. 31 | /// (connection index, local vertice index, global vertice index) 32 | ConnectionVerticeIndexOutOfBounds(u32, u8, u32), 33 | /// Could not serialize NavMesh. Contains serialization error string. 34 | CouldNotSerializeNavMesh(String), 35 | /// Could not deserialize NavMesh. Contains deserialization error string. 36 | CouldNotDeserializeNavMesh(String), 37 | /// Trying to use cells container with size not matching cols and rows count. 38 | /// (cells count, cols count, rows count) 39 | CellsCountDoesNotMatchColsRows(usize, usize, usize), 40 | /// Either cols or rows count is zero. 41 | /// (cols count, rows count) 42 | EmptyCells(usize, usize), 43 | /// Trying to use cell coordinate out of bounds. 44 | /// (col, row, cols count, rows count) 45 | InvalidCellCoordinate(usize, usize, usize, usize), 46 | } 47 | 48 | /// Result data. 49 | pub type NavResult = StdResult; 50 | 51 | #[derive(Debug, Default, Copy, Clone, Eq, Serialize, Deserialize)] 52 | pub struct NavConnection(pub u32, pub u32); 53 | 54 | impl Hash for NavConnection { 55 | fn hash(&self, state: &mut H) { 56 | let first = self.0.min(self.1); 57 | let second = self.0.max(self.1); 58 | first.hash(state); 59 | second.hash(state); 60 | } 61 | } 62 | 63 | impl PartialEq for NavConnection { 64 | fn eq(&self, other: &Self) -> bool { 65 | let first = self.0.min(self.1); 66 | let second = self.0.max(self.1); 67 | let ofirst = other.0.min(other.1); 68 | let osecond = other.0.max(other.1); 69 | first == ofirst && second == osecond 70 | } 71 | } 72 | 73 | pub(crate) const ZERO_TRESHOLD: Scalar = 1e-6; 74 | 75 | #[cfg(test)] 76 | mod tests { 77 | use super::*; 78 | 79 | #[test] 80 | fn test_send_sync() { 81 | fn foo() 82 | where 83 | T: Send + Sync, 84 | { 85 | println!("{:?} is Send + Sync", std::any::type_name::()); 86 | } 87 | 88 | foo::(); 89 | foo::(); 90 | foo::(); 91 | foo::(); 92 | foo::>(); 93 | } 94 | 95 | #[test] 96 | fn test_raycast() { 97 | assert_eq!( 98 | NavVec3::raycast_plane( 99 | (-1.0, -1.0, -1.0).into(), 100 | (1.0, 1.0, 1.0).into(), 101 | (0.0, 0.0, 0.0).into(), 102 | (-1.0, 0.0, 0.0).into(), 103 | ) 104 | .unwrap(), 105 | (0.0, 0.0, 0.0).into(), 106 | ); 107 | assert_eq!( 108 | NavVec3::raycast_plane( 109 | (0.0, 0.0, 0.0).into(), 110 | (2.0, 1.0, 1.0).into(), 111 | (1.0, 0.0, 0.0).into(), 112 | (-1.0, 0.0, 0.0).into(), 113 | ) 114 | .unwrap(), 115 | (1.0, 0.5, 0.5).into(), 116 | ); 117 | assert_eq!( 118 | NavVec3::raycast_line( 119 | (-1.0, -1.0, 1.0).into(), 120 | (1.0, 1.0, 1.0).into(), 121 | (1.0, -1.0, 0.0).into(), 122 | (-1.0, 1.0, 0.0).into(), 123 | (-1.0, 0.0, 0.0).into(), 124 | ) 125 | .unwrap(), 126 | (0.0, 0.0, 0.0).into(), 127 | ); 128 | assert_eq!( 129 | NavVec3::raycast_line( 130 | (0.0, 0.0, 0.0).into(), 131 | (2.0, 1.0, 1.0).into(), 132 | (1.0, 0.0, 0.0).into(), 133 | (1.0, 1.0, 0.0).into(), 134 | (1.0, 0.0, 0.0).into(), 135 | ) 136 | .unwrap(), 137 | (1.0, 0.5, 0.0).into(), 138 | ); 139 | assert_eq!( 140 | NavVec3::raycast_triangle( 141 | (0.0, 0.0, 1.0).into(), 142 | (0.0, 0.0, -1.0).into(), 143 | (0.0, -1.0, 0.0).into(), 144 | (1.0, 1.0, 0.0).into(), 145 | (-1.0, 1.0, 0.0).into(), 146 | ) 147 | .unwrap(), 148 | (0.0, 0.0, 0.0).into(), 149 | ); 150 | assert_eq!( 151 | NavVec3::raycast_triangle( 152 | (-1.0, -1.0, 1.0).into(), 153 | (-1.0, -1.0, -1.0).into(), 154 | (0.0, -1.0, 0.0).into(), 155 | (1.0, 1.0, 0.0).into(), 156 | (-1.0, 1.0, 0.0).into(), 157 | ), 158 | None, 159 | ); 160 | } 161 | 162 | #[test] 163 | fn test_line_between_points() { 164 | assert_eq!( 165 | true, 166 | NavVec3::is_line_between_points( 167 | (0.0, -1.0, 0.0).into(), 168 | (0.0, 1.0, 0.0).into(), 169 | (-1.0, 0.0, 0.0).into(), 170 | (1.0, 0.0, 0.0).into(), 171 | (0.0, 0.0, 1.0).into(), 172 | ), 173 | ); 174 | assert_eq!( 175 | false, 176 | NavVec3::is_line_between_points( 177 | (-2.0, -1.0, 0.0).into(), 178 | (-2.0, 1.0, 0.0).into(), 179 | (-1.0, 0.0, 0.0).into(), 180 | (1.0, 0.0, 0.0).into(), 181 | (0.0, 0.0, 1.0).into(), 182 | ), 183 | ); 184 | assert_eq!( 185 | false, 186 | NavVec3::is_line_between_points( 187 | (2.0, -1.0, 0.0).into(), 188 | (2.0, 1.0, 0.0).into(), 189 | (-1.0, 0.0, 0.0).into(), 190 | (1.0, 0.0, 0.0).into(), 191 | (0.0, 0.0, 1.0).into(), 192 | ), 193 | ); 194 | assert_eq!( 195 | true, 196 | NavVec3::is_line_between_points( 197 | (-1.0, -1.0, 0.0).into(), 198 | (-1.0, 1.0, 0.0).into(), 199 | (-1.0, 0.0, 0.0).into(), 200 | (1.0, 0.0, 0.0).into(), 201 | (0.0, 0.0, 1.0).into(), 202 | ), 203 | ); 204 | assert_eq!( 205 | true, 206 | NavVec3::is_line_between_points( 207 | (1.0, -1.0, 0.0).into(), 208 | (1.0, 1.0, 0.0).into(), 209 | (-1.0, 0.0, 0.0).into(), 210 | (1.0, 0.0, 0.0).into(), 211 | (0.0, 0.0, 1.0).into(), 212 | ), 213 | ); 214 | } 215 | 216 | #[test] 217 | fn test_spatials() { 218 | { 219 | let vertices = vec![ 220 | (0.0, 0.0, 0.0).into(), 221 | (2.0, 0.0, 0.0).into(), 222 | (0.0, 2.0, 0.0).into(), 223 | ]; 224 | 225 | let s = NavSpatialObject::new(0, vertices[0], vertices[1], vertices[2]); 226 | assert_eq!(s.normal(), (0.0, 0.0, 1.0).into()); 227 | } 228 | { 229 | let vertices = vec![ 230 | (0.0, 0.0, 0.0).into(), 231 | (2.0, 0.0, 2.0).into(), 232 | (0.0, 2.0, 0.0).into(), 233 | ]; 234 | 235 | let s = NavSpatialObject::new(0, vertices[0], vertices[1], vertices[2]); 236 | assert_eq!(s.normal(), NavVec3::new(-1.0, 0.0, 1.0).normalize()); 237 | } 238 | { 239 | let vertices = vec![ 240 | (1.0, 2.0, 0.0).into(), 241 | (2.0, 2.0, 0.0).into(), 242 | (2.0, 3.0, 0.0).into(), 243 | (1.0, 3.0, 0.0).into(), 244 | ]; 245 | 246 | let s = NavSpatialObject::new(0, vertices[0], vertices[1], vertices[2]); 247 | assert_eq!(s.closest_point(vertices[0]), vertices[0]); 248 | assert_eq!(s.closest_point(vertices[1]), vertices[1]); 249 | assert_eq!(s.closest_point(vertices[2]), vertices[2]); 250 | assert_eq!( 251 | s.closest_point((1.75, 2.25, 0.0).into()), 252 | (1.75, 2.25, 0.0).into() 253 | ); 254 | assert_eq!( 255 | s.closest_point((1.5, 1.0, 0.0).into()), 256 | (1.5, 2.0, 0.0).into() 257 | ); 258 | assert_eq!( 259 | s.closest_point((3.0, 2.5, 0.0).into()), 260 | (2.0, 2.5, 0.0).into() 261 | ); 262 | assert_eq!( 263 | s.closest_point((1.0, 3.0, 0.0).into()), 264 | (1.5, 2.5, 0.0).into() 265 | ); 266 | 267 | let s = NavSpatialObject::new(0, vertices[2], vertices[3], vertices[0]); 268 | assert_eq!(s.closest_point(vertices[2]), vertices[2]); 269 | assert_eq!(s.closest_point(vertices[3]), vertices[3]); 270 | assert_eq!(s.closest_point(vertices[0]), vertices[0]); 271 | assert_eq!( 272 | s.closest_point((1.25, 2.75, 0.0).into()), 273 | (1.25, 2.75, 0.0).into() 274 | ); 275 | assert_eq!( 276 | s.closest_point((2.0, 2.0, 0.0).into()), 277 | (1.5, 2.5, 0.0).into() 278 | ); 279 | assert_eq!( 280 | s.closest_point((1.5, 4.0, 0.0).into()), 281 | (1.5, 3.0, 0.0).into() 282 | ); 283 | assert_eq!( 284 | s.closest_point((0.0, 2.5, 0.0).into()), 285 | (1.0, 2.5, 0.0).into() 286 | ); 287 | } 288 | } 289 | 290 | #[test] 291 | fn test_general() { 292 | let vertices = vec![ 293 | (0.0, 0.0, 0.0).into(), // 0 294 | (1.0, 0.0, 0.0).into(), // 1 295 | (2.0, 0.0, 0.0).into(), // 2 296 | (0.0, 1.0, 0.0).into(), // 3 297 | (1.0, 1.0, 0.0).into(), // 4 298 | (2.0, 1.0, 0.0).into(), // 5 299 | (0.0, 2.0, 0.0).into(), // 6 300 | (1.0, 2.0, 0.0).into(), // 7 301 | ]; 302 | let triangles = vec![ 303 | (0, 1, 4).into(), // 0 304 | (4, 3, 0).into(), // 1 305 | (1, 2, 5).into(), // 2 306 | (5, 4, 1).into(), // 3 307 | (3, 4, 7).into(), // 4 308 | (7, 6, 3).into(), // 5 309 | ]; 310 | let mesh = NavMesh::new(vertices.clone(), triangles.clone()).unwrap(); 311 | { 312 | let path = mesh.find_path_triangles(0, 0).unwrap().0; 313 | assert_eq!(path, vec![0]); 314 | } 315 | { 316 | let path = mesh.find_path_triangles(2, 5).unwrap().0; 317 | assert_eq!(path, vec![2, 3, 0, 1, 4, 5]); 318 | } 319 | { 320 | let path = mesh 321 | .find_path( 322 | (0.0, 0.0, 0.0).into(), 323 | (2.0, 0.0, 0.0).into(), 324 | NavQuery::Accuracy, 325 | NavPathMode::MidPoints, 326 | ) 327 | .unwrap(); 328 | assert_eq!( 329 | path.into_iter() 330 | .map(|v| ( 331 | (v.x * 10.0) as i32, 332 | (v.y * 10.0) as i32, 333 | (v.z * 10.0) as i32, 334 | )) 335 | .collect::>(), 336 | vec![(0, 0, 0), (20, 0, 0),] 337 | ); 338 | let path = mesh 339 | .find_path( 340 | (0.0, 0.0, 0.0).into(), 341 | (2.0, 0.0, 0.0).into(), 342 | NavQuery::Accuracy, 343 | NavPathMode::Accuracy, 344 | ) 345 | .unwrap(); 346 | assert_eq!( 347 | path.into_iter() 348 | .map(|v| ( 349 | (v.x * 10.0) as i32, 350 | (v.y * 10.0) as i32, 351 | (v.z * 10.0) as i32, 352 | )) 353 | .collect::>(), 354 | vec![(0, 0, 0), (20, 0, 0),] 355 | ); 356 | } 357 | { 358 | let path = mesh 359 | .find_path( 360 | (2.0, 0.0, 0.0).into(), 361 | (0.0, 2.0, 0.0).into(), 362 | NavQuery::Accuracy, 363 | NavPathMode::MidPoints, 364 | ) 365 | .unwrap(); 366 | assert_eq!( 367 | path.into_iter() 368 | .map(|v| ( 369 | (v.x * 10.0) as i32, 370 | (v.y * 10.0) as i32, 371 | (v.z * 10.0) as i32, 372 | )) 373 | .collect::>(), 374 | vec![(20, 0, 0), (0, 20, 0),] 375 | ); 376 | let path = mesh 377 | .find_path( 378 | (2.0, 0.0, 0.0).into(), 379 | (0.0, 2.0, 0.0).into(), 380 | NavQuery::Accuracy, 381 | NavPathMode::Accuracy, 382 | ) 383 | .unwrap(); 384 | assert_eq!( 385 | path.into_iter() 386 | .map(|v| ( 387 | (v.x * 10.0) as i32, 388 | (v.y * 10.0) as i32, 389 | (v.z * 10.0) as i32, 390 | )) 391 | .collect::>(), 392 | vec![(20, 0, 0), (10, 10, 0), (0, 20, 0),] 393 | ); 394 | } 395 | { 396 | let path = mesh 397 | .find_path( 398 | (2.0, 1.0, 0.0).into(), 399 | (1.0, 2.0, 0.0).into(), 400 | NavQuery::Accuracy, 401 | NavPathMode::MidPoints, 402 | ) 403 | .unwrap(); 404 | assert_eq!( 405 | path.into_iter() 406 | .map(|v| ( 407 | (v.x * 10.0) as i32, 408 | (v.y * 10.0) as i32, 409 | (v.z * 10.0) as i32, 410 | )) 411 | .collect::>(), 412 | vec![(20, 10, 0), (5, 10, 0), (10, 20, 0),] 413 | ); 414 | let path = mesh 415 | .find_path( 416 | (2.0, 1.0, 0.0).into(), 417 | (1.0, 2.0, 0.0).into(), 418 | NavQuery::Accuracy, 419 | NavPathMode::Accuracy, 420 | ) 421 | .unwrap(); 422 | assert_eq!( 423 | path.into_iter() 424 | .map(|v| ( 425 | (v.x * 10.0) as i32, 426 | (v.y * 10.0) as i32, 427 | (v.z * 10.0) as i32, 428 | )) 429 | .collect::>(), 430 | vec![(20, 10, 0), (10, 10, 0), (10, 20, 0),] 431 | ); 432 | } 433 | { 434 | let path = mesh 435 | .find_path( 436 | (0.5, 0.0, 0.0).into(), 437 | (0.5, 2.0, 0.0).into(), 438 | NavQuery::Accuracy, 439 | NavPathMode::MidPoints, 440 | ) 441 | .unwrap(); 442 | assert_eq!( 443 | path.into_iter() 444 | .map(|v| ( 445 | (v.x * 10.0) as i32, 446 | (v.y * 10.0) as i32, 447 | (v.z * 10.0) as i32, 448 | )) 449 | .collect::>(), 450 | vec![(5, 0, 0), (5, 20, 0),] 451 | ); 452 | let path = mesh 453 | .find_path( 454 | (0.5, 0.0, 0.0).into(), 455 | (0.5, 2.0, 0.0).into(), 456 | NavQuery::Accuracy, 457 | NavPathMode::Accuracy, 458 | ) 459 | .unwrap(); 460 | assert_eq!( 461 | path.into_iter() 462 | .map(|v| ( 463 | (v.x * 10.0) as i32, 464 | (v.y * 10.0) as i32, 465 | (v.z * 10.0) as i32, 466 | )) 467 | .collect::>(), 468 | vec![(5, 0, 0), (5, 20, 0),] 469 | ); 470 | } 471 | 472 | let vertices = vec![ 473 | (0.0, 0.0, 0.0).into(), // 0 474 | (2.0, 0.0, 0.0).into(), // 1 475 | (2.0, 1.0, 0.0).into(), // 2 476 | (1.0, 1.0, 0.0).into(), // 3 477 | (0.0, 2.0, 0.0).into(), // 4 478 | ]; 479 | let triangles = vec![ 480 | (0, 3, 4).into(), // 0 481 | (0, 1, 3).into(), // 1 482 | (1, 2, 3).into(), // 2 483 | ]; 484 | let mesh = NavMesh::new(vertices.clone(), triangles.clone()).unwrap(); 485 | { 486 | let path = mesh.find_path_triangles(0, 2).unwrap().0; 487 | assert_eq!(path, vec![0, 1, 2]); 488 | } 489 | { 490 | let path = mesh 491 | .find_path( 492 | (2.0, 1.0, 0.0).into(), 493 | (0.0, 2.0, 0.0).into(), 494 | NavQuery::Accuracy, 495 | NavPathMode::MidPoints, 496 | ) 497 | .unwrap(); 498 | assert_eq!( 499 | path.into_iter() 500 | .map(|v| ( 501 | (v.x * 10.0) as i32, 502 | (v.y * 10.0) as i32, 503 | (v.z * 10.0) as i32, 504 | )) 505 | .collect::>(), 506 | vec![(20, 10, 0), (5, 5, 0), (0, 20, 0),] 507 | ); 508 | let path = mesh 509 | .find_path( 510 | (2.0, 1.0, 0.0).into(), 511 | (0.0, 2.0, 0.0).into(), 512 | NavQuery::Accuracy, 513 | NavPathMode::Accuracy, 514 | ) 515 | .unwrap(); 516 | assert_eq!( 517 | path.into_iter() 518 | .map(|v| ( 519 | (v.x * 10.0) as i32, 520 | (v.y * 10.0) as i32, 521 | (v.z * 10.0) as i32, 522 | )) 523 | .collect::>(), 524 | vec![(20, 10, 0), (10, 10, 0), (0, 20, 0),] 525 | ); 526 | } 527 | 528 | let vertices = vec![ 529 | (0.0, 0.0, 0.0).into(), // 0 530 | (1.0, 0.0, 0.0).into(), // 1 531 | (2.0, 0.0, 1.0).into(), // 2 532 | (0.0, 1.0, 0.0).into(), // 3 533 | (1.0, 1.0, 0.0).into(), // 4 534 | (2.0, 1.0, 1.0).into(), // 5 535 | ]; 536 | let triangles = vec![ 537 | (0, 1, 4).into(), // 0 538 | (4, 3, 0).into(), // 1 539 | (1, 2, 5).into(), // 2 540 | (5, 4, 1).into(), // 3 541 | ]; 542 | let mesh = NavMesh::new(vertices.clone(), triangles.clone()).unwrap(); 543 | { 544 | let path = mesh.find_path_triangles(1, 2).unwrap().0; 545 | assert_eq!(path, vec![1, 0, 3, 2]); 546 | } 547 | { 548 | let path = mesh 549 | .find_path( 550 | (0.0, 0.5, 0.0).into(), 551 | (2.0, 0.5, 1.0).into(), 552 | NavQuery::Accuracy, 553 | NavPathMode::MidPoints, 554 | ) 555 | .unwrap(); 556 | assert_eq!( 557 | path.into_iter() 558 | .map(|v| ( 559 | (v.x * 10.0) as i32, 560 | (v.y * 10.0) as i32, 561 | (v.z * 10.0) as i32, 562 | )) 563 | .collect::>(), 564 | vec![(0, 5, 0), (10, 5, 0), (20, 5, 10),] 565 | ); 566 | let path = mesh 567 | .find_path( 568 | (0.0, 0.5, 0.0).into(), 569 | (2.0, 0.5, 1.0).into(), 570 | NavQuery::Accuracy, 571 | NavPathMode::Accuracy, 572 | ) 573 | .unwrap(); 574 | assert_eq!( 575 | path.into_iter() 576 | .map(|v| ( 577 | (v.x * 10.0) as i32, 578 | (v.y * 10.0) as i32, 579 | (v.z * 10.0) as i32, 580 | )) 581 | .collect::>(), 582 | vec![(0, 5, 0), (10, 5, 0), (20, 5, 10),] 583 | ); 584 | } 585 | { 586 | let path = mesh 587 | .find_path( 588 | (0.0, 1.0, 0.0).into(), 589 | (2.0, 0.0, 1.0).into(), 590 | NavQuery::Accuracy, 591 | NavPathMode::MidPoints, 592 | ) 593 | .unwrap(); 594 | assert_eq!( 595 | path.into_iter() 596 | .map(|v| ( 597 | (v.x * 10.0) as i32, 598 | (v.y * 10.0) as i32, 599 | (v.z * 10.0) as i32, 600 | )) 601 | .collect::>(), 602 | vec![(0, 10, 0), (10, 5, 0), (20, 0, 10),] 603 | ); 604 | let path = mesh 605 | .find_path( 606 | (0.0, 1.0, 0.0).into(), 607 | (2.0, 0.0, 1.0).into(), 608 | NavQuery::Accuracy, 609 | NavPathMode::Accuracy, 610 | ) 611 | .unwrap(); 612 | assert_eq!( 613 | path.into_iter() 614 | .map(|v| ( 615 | (v.x * 10.0) as i32, 616 | (v.y * 10.0) as i32, 617 | (v.z * 10.0) as i32, 618 | )) 619 | .collect::>(), 620 | vec![(0, 10, 0), (10, 5, 0), (20, 0, 10),] 621 | ); 622 | } 623 | { 624 | let path = mesh 625 | .find_path( 626 | (0.0, 1.0, 0.0).into(), 627 | (1.5, 0.25, 0.5).into(), 628 | NavQuery::Accuracy, 629 | NavPathMode::MidPoints, 630 | ) 631 | .unwrap(); 632 | assert_eq!( 633 | path.into_iter() 634 | .map(|v| ( 635 | (v.x * 10.0) as i32, 636 | (v.y * 10.0) as i32, 637 | (v.z * 10.0) as i32, 638 | )) 639 | .collect::>(), 640 | vec![(0, 10, 0), (10, 5, 0), (15, 2, 5),] 641 | ); 642 | let path = mesh 643 | .find_path( 644 | (0.0, 1.0, 0.0).into(), 645 | (1.2, 0.4, 0.2).into(), 646 | NavQuery::Accuracy, 647 | NavPathMode::Accuracy, 648 | ) 649 | .unwrap(); 650 | assert_eq!( 651 | path.into_iter() 652 | .map(|v| ( 653 | (v.x * 10.0) as i32, 654 | (v.y * 10.0) as i32, 655 | (v.z * 10.0) as i32, 656 | )) 657 | .collect::>(), 658 | vec![(0, 10, 0), (10, 5, 0), (12, 4, 2),] 659 | ); 660 | } 661 | } 662 | 663 | #[test] 664 | fn test_thicken() { 665 | let source = NavMesh::new( 666 | vec![ 667 | [-10.0, -10.0, 0.0].into(), 668 | [10.0, -10.0, 0.0].into(), 669 | [10.0, 10.0, 0.0].into(), 670 | [-10.0, 10.0, 0.0].into(), 671 | ], 672 | vec![[0, 1, 2].into(), [2, 3, 0].into()], 673 | ) 674 | .unwrap(); 675 | let thickened = source.thicken(1.0).unwrap(); 676 | for (a, b) in source.vertices().iter().zip(thickened.vertices().iter()) { 677 | assert_relative_eq!(*b, *a + NavVec3::new(0.0, 0.0, 1.0)); 678 | } 679 | 680 | let source = NavMesh::new( 681 | vec![ 682 | [-5.0, -5.0, -5.0].into(), // 0 683 | [5.0, -5.0, -5.0].into(), // 1 684 | [5.0, 5.0, -5.0].into(), // 2 685 | [-5.0, 5.0, -5.0].into(), // 3 686 | [-5.0, -5.0, 5.0].into(), // 4 687 | [5.0, -5.0, 5.0].into(), // 5 688 | [5.0, 5.0, 5.0].into(), // 6 689 | [-5.0, 5.0, 5.0].into(), // 7 690 | ], 691 | vec![ 692 | [2, 1, 0].into(), 693 | [0, 3, 2].into(), 694 | [4, 5, 6].into(), 695 | [6, 7, 4].into(), 696 | [0, 1, 5].into(), 697 | [5, 4, 0].into(), 698 | [1, 2, 6].into(), 699 | [6, 5, 1].into(), 700 | [2, 3, 7].into(), 701 | [7, 6, 2].into(), 702 | [3, 0, 4].into(), 703 | [4, 7, 3].into(), 704 | ], 705 | ) 706 | .unwrap(); 707 | let thickened = source.thicken(1.0).unwrap(); 708 | let expected = vec![ 709 | NavVec3 { 710 | x: -5.333333333333333, 711 | y: -5.666666666666667, 712 | z: -5.666666666666667, 713 | }, 714 | NavVec3 { 715 | x: 5.816496580927726, 716 | y: -5.408248290463863, 717 | z: -5.408248290463863, 718 | }, 719 | NavVec3 { 720 | x: 5.333333333333333, 721 | y: 5.666666666666667, 722 | z: -5.666666666666667, 723 | }, 724 | NavVec3 { 725 | x: -5.816496580927726, 726 | y: 5.408248290463863, 727 | z: -5.408248290463863, 728 | }, 729 | NavVec3 { 730 | x: -5.666666666666667, 731 | y: -5.333333333333333, 732 | z: 5.666666666666667, 733 | }, 734 | NavVec3 { 735 | x: 5.408248290463863, 736 | y: -5.816496580927726, 737 | z: 5.408248290463863, 738 | }, 739 | NavVec3 { 740 | x: 5.666666666666667, 741 | y: 5.333333333333333, 742 | z: 5.666666666666667, 743 | }, 744 | NavVec3 { 745 | x: -5.408248290463863, 746 | y: 5.816496580927726, 747 | z: 5.408248290463863, 748 | }, 749 | ]; 750 | for (a, b) in expected.iter().zip(thickened.vertices().iter()) { 751 | assert_relative_eq!(a, b); 752 | } 753 | } 754 | 755 | #[test] 756 | fn test_grid() { 757 | let grid = NavGrid::new( 758 | 3, 759 | 3, 760 | vec![true, true, true, true, false, true, true, true, true], 761 | ) 762 | .unwrap(); 763 | let path = grid.find_path((0, 0), (1, 2)).unwrap(); 764 | assert_eq!(path, vec![(0, 0), (0, 1), (0, 2), (1, 2)]); 765 | assert_eq!(grid.find_path((0, 0), (1, 1)), None); 766 | 767 | let grid = NavGrid::with_connections( 768 | 2, 769 | 2, 770 | vec![ 771 | NavGridConnection { 772 | from: (0, 0), 773 | to: (1, 0), 774 | }, 775 | NavGridConnection { 776 | from: (1, 0), 777 | to: (1, 1), 778 | }, 779 | NavGridConnection { 780 | from: (1, 1), 781 | to: (0, 1), 782 | }, 783 | NavGridConnection { 784 | from: (0, 1), 785 | to: (0, 0), 786 | }, 787 | ], 788 | ) 789 | .unwrap(); 790 | let path = grid.find_path((0, 0), (0, 1)).unwrap(); 791 | assert_eq!(path, vec![(0, 0), (1, 0), (1, 1), (0, 1)]); 792 | let mut islands = grid.find_islands(); 793 | for island in &mut islands { 794 | island.sort(); 795 | } 796 | assert_eq!(islands, vec![vec![(0, 0), (0, 1), (1, 0), (1, 1)]]); 797 | 798 | let grid = NavGrid::new( 799 | 3, 800 | 3, 801 | vec![true, true, true, false, false, false, true, true, true], 802 | ) 803 | .unwrap(); 804 | let mut islands = grid.find_islands(); 805 | for island in &mut islands { 806 | island.sort(); 807 | } 808 | assert_eq!( 809 | islands, 810 | vec![vec![(0, 0), (1, 0), (2, 0)], vec![(0, 2), (1, 2), (2, 2)]] 811 | ); 812 | 813 | let grid = NavFreeGrid::new(vec![ 814 | NavFreeGridConnection { 815 | from: (0, 0), 816 | to: (0, 2), 817 | }, 818 | NavFreeGridConnection { 819 | from: (0, 2), 820 | to: (-1, -1), 821 | }, 822 | ]); 823 | let path = grid.find_path((0, 0), (-1, -1)).unwrap(); 824 | assert_eq!(path, vec![(0, 0), (0, 2), (-1, -1)]); 825 | } 826 | 827 | #[test] 828 | fn test_islands() { 829 | let grid_a = NavGrid::new(2, 2, vec![true, true, true, false]).unwrap(); 830 | let grid_b = NavGrid::new(2, 2, vec![true, true, false, true]).unwrap(); 831 | let island_a = NavIslandPortal { 832 | island: grid_a.id(), 833 | portal: None, 834 | }; 835 | let island_a_portal = NavIslandPortal { 836 | island: grid_a.id(), 837 | portal: Some((1, 0)), 838 | }; 839 | let island_b_portal = NavIslandPortal { 840 | island: grid_b.id(), 841 | portal: Some((0, 0)), 842 | }; 843 | let island_b = NavIslandPortal { 844 | island: grid_b.id(), 845 | portal: None, 846 | }; 847 | let islands = NavIslands::new( 848 | vec![ 849 | NavIslandsConnection { 850 | from: island_a.clone(), 851 | to: island_a_portal.clone(), 852 | distance: 1.0, 853 | }, 854 | NavIslandsConnection { 855 | from: island_a_portal.clone(), 856 | to: island_b_portal.clone(), 857 | distance: 0.0, 858 | }, 859 | NavIslandsConnection { 860 | from: island_b_portal.clone(), 861 | to: island_b.clone(), 862 | distance: 1.0, 863 | }, 864 | ], 865 | true, 866 | ); 867 | let (distance, path) = islands.find_path(&island_a, &island_b).unwrap(); 868 | assert_eq!( 869 | path, 870 | vec![&island_a, &island_a_portal, &island_b_portal, &island_b] 871 | ); 872 | assert!((distance - 2.0).abs() < 1.0e-6); 873 | } 874 | } 875 | -------------------------------------------------------------------------------- /src/nav_grid.rs: -------------------------------------------------------------------------------- 1 | use crate::{Error, NavResult, Scalar}; 2 | use petgraph::{ 3 | algo::{astar, tarjan_scc}, 4 | graph::NodeIndex, 5 | visit::EdgeRef, 6 | Directed, Graph, Undirected, 7 | }; 8 | use serde::{Deserialize, Serialize}; 9 | use std::collections::{HashMap, HashSet}; 10 | #[cfg(not(feature = "scalar64"))] 11 | use std::f32::MAX as SCALAR_MAX; 12 | #[cfg(feature = "scalar64")] 13 | use std::f64::MAX as SCALAR_MAX; 14 | use typid::ID; 15 | 16 | #[cfg(feature = "parallel")] 17 | macro_rules! iter { 18 | ($v:expr) => { 19 | $v.par_iter() 20 | }; 21 | } 22 | #[cfg(not(feature = "parallel"))] 23 | macro_rules! iter { 24 | ($v:expr) => { 25 | $v.iter() 26 | }; 27 | } 28 | 29 | #[derive(Debug, Default, Copy, Clone, PartialEq, Eq, Serialize, Deserialize)] 30 | pub struct NavGridConnection { 31 | pub from: (usize, usize), 32 | pub to: (usize, usize), 33 | } 34 | 35 | /// Nav grid identifier. 36 | pub type NavGridID = ID; 37 | 38 | #[derive(Debug, Default, Clone, Serialize, Deserialize)] 39 | pub struct NavGrid { 40 | id: NavGridID, 41 | cols: usize, 42 | rows: usize, 43 | cells: Vec, 44 | costs: Vec, 45 | graph: Graph<(), (), Directed>, 46 | nodes: Vec>, 47 | nodes_map: HashMap, 48 | } 49 | 50 | impl NavGrid { 51 | pub fn new(cols: usize, rows: usize, cells: Vec) -> NavResult { 52 | if cols == 0 || rows == 0 { 53 | return Err(Error::EmptyCells(cols, rows)); 54 | } 55 | if cols * rows != cells.len() { 56 | return Err(Error::CellsCountDoesNotMatchColsRows( 57 | cells.len(), 58 | cols, 59 | rows, 60 | )); 61 | } 62 | let costs = vec![1.0; cells.len()]; 63 | let mut graph = Graph::<(), (), Directed>::with_capacity( 64 | cells.len(), 65 | (cols - 1) * rows + (rows - 1) * cols, 66 | ); 67 | let nodes = (0..cells.len()) 68 | .zip(cells.iter()) 69 | .map(|(_, cell)| { 70 | if *cell { 71 | Some(graph.add_node(())) 72 | } else { 73 | None 74 | } 75 | }) 76 | .collect::>(); 77 | for ca in 0..(cols - 1) { 78 | for r in 0..rows { 79 | let cb = ca + 1; 80 | let ia = r * cols + ca; 81 | let ib = r * cols + cb; 82 | if let (Some(na), Some(nb)) = (nodes[ia], nodes[ib]) { 83 | graph.add_edge(na, nb, ()); 84 | graph.add_edge(nb, na, ()); 85 | } 86 | } 87 | } 88 | for c in 0..cols { 89 | for ra in 0..(rows - 1) { 90 | let rb = ra + 1; 91 | let ia = ra * cols + c; 92 | let ib = rb * cols + c; 93 | if let (Some(na), Some(nb)) = (nodes[ia], nodes[ib]) { 94 | graph.add_edge(na, nb, ()); 95 | graph.add_edge(nb, na, ()); 96 | } 97 | } 98 | } 99 | let nodes_map = iter!(nodes) 100 | .enumerate() 101 | .filter_map(|(i, n)| n.map(|n| (n, i))) 102 | .collect(); 103 | Ok(Self { 104 | id: NavGridID::new(), 105 | cols, 106 | rows, 107 | cells, 108 | costs, 109 | graph, 110 | nodes, 111 | nodes_map, 112 | }) 113 | } 114 | 115 | pub fn with_connections( 116 | cols: usize, 117 | rows: usize, 118 | connections: Vec, 119 | ) -> NavResult { 120 | if cols == 0 || rows == 0 { 121 | return Err(Error::EmptyCells(cols, rows)); 122 | } 123 | let count = cols * rows; 124 | for connection in &connections { 125 | if connection.from.0 >= cols || connection.from.1 >= rows { 126 | return Err(Error::InvalidCellCoordinate( 127 | connection.from.0, 128 | connection.from.1, 129 | cols, 130 | rows, 131 | )); 132 | } 133 | if connection.to.0 >= cols || connection.to.1 >= rows { 134 | return Err(Error::InvalidCellCoordinate( 135 | connection.to.0, 136 | connection.to.1, 137 | cols, 138 | rows, 139 | )); 140 | } 141 | } 142 | let costs = vec![1.0; count]; 143 | let mut graph = 144 | Graph::<(), (), Directed>::with_capacity(count, (cols - 1) * rows + (rows - 1) * cols); 145 | let nodes = (0..count) 146 | .map(|index| { 147 | let coord = (index % cols, index / cols); 148 | if connections.iter().any(|c| c.from == coord || c.to == coord) { 149 | Some(graph.add_node(())) 150 | } else { 151 | None 152 | } 153 | }) 154 | .collect::>(); 155 | for connection in connections { 156 | let ia = connection.from.1 * cols + connection.from.0; 157 | let ib = connection.to.1 * cols + connection.to.0; 158 | if let (Some(na), Some(nb)) = (nodes[ia], nodes[ib]) { 159 | graph.add_edge(na, nb, ()); 160 | } 161 | } 162 | let nodes_map = iter!(nodes) 163 | .enumerate() 164 | .filter_map(|(i, n)| n.map(|n| (n, i))) 165 | .collect(); 166 | Ok(Self { 167 | id: NavGridID::new(), 168 | cols, 169 | rows, 170 | cells: nodes.iter().map(Option::is_some).collect(), 171 | costs, 172 | graph, 173 | nodes, 174 | nodes_map, 175 | }) 176 | } 177 | 178 | #[inline] 179 | pub fn id(&self) -> NavGridID { 180 | self.id 181 | } 182 | 183 | #[inline] 184 | pub fn cells(&self) -> &[bool] { 185 | &self.cells 186 | } 187 | 188 | #[inline] 189 | pub fn cells_costs(&self) -> &[Scalar] { 190 | &self.costs 191 | } 192 | 193 | #[inline] 194 | pub fn set_cell_cost(&mut self, col: usize, row: usize, cost: Scalar) -> Option { 195 | let index = self.index(col, row)?; 196 | let c = self.costs.get_mut(index)?; 197 | let old = *c; 198 | *c = cost.max(0.0); 199 | Some(old) 200 | } 201 | 202 | pub fn neighbors( 203 | &self, 204 | col: usize, 205 | row: usize, 206 | ) -> Option + '_> { 207 | let index = self.index(col, row)?; 208 | let node = self.nodes[index]?; 209 | Some(self.graph.neighbors(node).filter_map(|node| { 210 | self.nodes_map 211 | .get(&node) 212 | .and_then(|index| self.coord(*index)) 213 | })) 214 | } 215 | 216 | pub fn find_path( 217 | &self, 218 | from: (usize, usize), 219 | to: (usize, usize), 220 | ) -> Option> { 221 | self.find_path_custom(from, to, |_, _| true) 222 | } 223 | 224 | // filter params: first col-row, second col-row. 225 | pub fn find_path_custom( 226 | &self, 227 | from: (usize, usize), 228 | to: (usize, usize), 229 | mut filter: F, 230 | ) -> Option> 231 | where 232 | F: FnMut((usize, usize), (usize, usize)) -> bool, 233 | { 234 | let start_index = self.index(from.0, from.1)?; 235 | let end_index = self.index(to.0, to.1)?; 236 | let start_node = (*self.nodes.get(start_index)?)?; 237 | let end_node = (*self.nodes.get(end_index)?)?; 238 | let nodes = astar( 239 | &self.graph, 240 | start_node, 241 | |n| n == end_node, 242 | |e| { 243 | let a = self.nodes_map[&e.source()]; 244 | let b = self.nodes_map[&e.target()]; 245 | if filter(self.coord(a).unwrap(), self.coord(b).unwrap()) { 246 | let a = self.costs[a]; 247 | let b = self.costs[b]; 248 | a * b 249 | } else { 250 | SCALAR_MAX 251 | } 252 | }, 253 | |_| 0.0, 254 | )? 255 | .1; 256 | Some( 257 | nodes 258 | .into_iter() 259 | .filter_map(|n| self.coord(self.nodes_map[&n])) 260 | .collect::>(), 261 | ) 262 | } 263 | 264 | pub fn find_islands(&self) -> Vec> { 265 | tarjan_scc(&self.graph) 266 | .into_iter() 267 | .map(|v| { 268 | v.into_iter() 269 | .filter_map(|n| self.nodes_map.get(&n).and_then(|i| self.coord(*i))) 270 | .collect::>() 271 | }) 272 | .filter(|v| !v.is_empty()) 273 | .collect() 274 | } 275 | 276 | pub fn index(&self, col: usize, row: usize) -> Option { 277 | if col < self.cols && row < self.rows { 278 | Some(row * self.cols + col) 279 | } else { 280 | None 281 | } 282 | } 283 | 284 | pub fn coord(&self, index: usize) -> Option<(usize, usize)> { 285 | let col = index % self.cols; 286 | let row = index / self.cols; 287 | if col < self.cols && row < self.rows { 288 | Some((col, row)) 289 | } else { 290 | None 291 | } 292 | } 293 | } 294 | 295 | #[derive(Debug, Default, Copy, Clone, PartialEq, Eq, Serialize, Deserialize)] 296 | pub struct NavFreeGridConnection { 297 | pub from: (isize, isize), 298 | pub to: (isize, isize), 299 | } 300 | 301 | /// Nav free grid identifier. 302 | pub type NavFreeGridID = ID; 303 | 304 | #[derive(Debug, Default, Clone, Serialize, Deserialize)] 305 | pub struct NavFreeGrid { 306 | id: NavFreeGridID, 307 | cells: Vec<(isize, isize)>, 308 | costs: Vec, 309 | graph: Graph<(), (), Undirected>, 310 | nodes: Vec, 311 | nodes_map: HashMap, 312 | } 313 | 314 | impl NavFreeGrid { 315 | pub fn new(connections: Vec) -> Self { 316 | let cells = connections 317 | .iter() 318 | .map(|c| c.from) 319 | .chain(connections.iter().map(|c| c.to)) 320 | .collect::>() 321 | .into_iter() 322 | .collect::>(); 323 | let costs = vec![1.0; cells.len()]; 324 | let mut graph = Graph::<(), (), Undirected>::with_capacity(cells.len(), connections.len()); 325 | let nodes = (0..cells.len()) 326 | .map(|_| graph.add_node(())) 327 | .collect::>(); 328 | for connection in connections { 329 | let ia = cells.iter().position(|c| connection.from == *c); 330 | let ib = cells.iter().position(|c| connection.to == *c); 331 | if let (Some(ia), Some(ib)) = (ia, ib) { 332 | graph.add_edge(nodes[ia], nodes[ib], ()); 333 | } 334 | } 335 | let nodes_map = iter!(nodes).enumerate().map(|(i, n)| (*n, i)).collect(); 336 | Self { 337 | id: NavFreeGridID::new(), 338 | cells, 339 | costs, 340 | graph, 341 | nodes, 342 | nodes_map, 343 | } 344 | } 345 | 346 | #[inline] 347 | pub fn id(&self) -> NavFreeGridID { 348 | self.id 349 | } 350 | 351 | #[inline] 352 | pub fn cells(&self) -> &[(isize, isize)] { 353 | &self.cells 354 | } 355 | 356 | #[inline] 357 | pub fn cells_costs(&self) -> &[Scalar] { 358 | &self.costs 359 | } 360 | 361 | #[inline] 362 | pub fn set_cell_cost(&mut self, col: isize, row: isize, cost: Scalar) -> Option { 363 | let index = self.index(col, row)?; 364 | let c = self.costs.get_mut(index)?; 365 | let old = *c; 366 | *c = cost.max(0.0); 367 | Some(old) 368 | } 369 | 370 | pub fn neighbors( 371 | &self, 372 | col: isize, 373 | row: isize, 374 | ) -> Option + '_> { 375 | let index = self.index(col, row)?; 376 | let node = self.nodes[index]; 377 | Some(self.graph.neighbors(node).filter_map(|node| { 378 | self.nodes_map 379 | .get(&node) 380 | .and_then(|index| self.coord(*index)) 381 | })) 382 | } 383 | 384 | pub fn find_path( 385 | &self, 386 | from: (isize, isize), 387 | to: (isize, isize), 388 | ) -> Option> { 389 | self.find_path_custom(from, to, |_, _| true) 390 | } 391 | 392 | // filter params: first col-row, second col-row. 393 | pub fn find_path_custom( 394 | &self, 395 | from: (isize, isize), 396 | to: (isize, isize), 397 | mut filter: F, 398 | ) -> Option> 399 | where 400 | F: FnMut((isize, isize), (isize, isize)) -> bool, 401 | { 402 | let start_index = self.index(from.0, from.1)?; 403 | let end_index = self.index(to.0, to.1)?; 404 | let start_node = *self.nodes.get(start_index)?; 405 | let end_node = *self.nodes.get(end_index)?; 406 | let nodes = astar( 407 | &self.graph, 408 | start_node, 409 | |n| n == end_node, 410 | |e| { 411 | let a = self.nodes_map[&e.source()]; 412 | let b = self.nodes_map[&e.target()]; 413 | if filter(self.coord(a).unwrap(), self.coord(b).unwrap()) { 414 | let a = self.costs[a]; 415 | let b = self.costs[b]; 416 | a * b 417 | } else { 418 | SCALAR_MAX 419 | } 420 | }, 421 | |_| 0.0, 422 | )? 423 | .1; 424 | Some( 425 | nodes 426 | .into_iter() 427 | .filter_map(|n| self.coord(self.nodes_map[&n])) 428 | .collect::>(), 429 | ) 430 | } 431 | 432 | pub fn find_islands(&self) -> Vec> { 433 | tarjan_scc(&self.graph) 434 | .into_iter() 435 | .map(|v| { 436 | v.into_iter() 437 | .filter_map(|n| self.nodes_map.get(&n).and_then(|i| self.coord(*i))) 438 | .collect::>() 439 | }) 440 | .filter(|v| !v.is_empty()) 441 | .collect() 442 | } 443 | 444 | pub fn index(&self, col: isize, row: isize) -> Option { 445 | let coord = (col, row); 446 | self.cells.iter().position(|c| coord == *c) 447 | } 448 | 449 | pub fn coord(&self, index: usize) -> Option<(isize, isize)> { 450 | self.cells.get(index).copied() 451 | } 452 | } 453 | -------------------------------------------------------------------------------- /src/nav_islands.rs: -------------------------------------------------------------------------------- 1 | use crate::Scalar; 2 | use petgraph::{ 3 | algo::{astar, tarjan_scc}, 4 | graph::NodeIndex, 5 | visit::EdgeRef, 6 | Directed, Graph, 7 | }; 8 | use serde::{de::DeserializeOwned, Deserialize, Serialize}; 9 | #[cfg(not(feature = "scalar64"))] 10 | use std::f32::MAX as SCALAR_MAX; 11 | #[cfg(feature = "scalar64")] 12 | use std::f64::MAX as SCALAR_MAX; 13 | use std::{ 14 | collections::{HashMap, HashSet}, 15 | hash::Hash, 16 | }; 17 | use typid::ID; 18 | 19 | #[cfg(feature = "parallel")] 20 | macro_rules! iter { 21 | ($v:expr) => { 22 | $v.par_iter() 23 | }; 24 | } 25 | #[cfg(not(feature = "parallel"))] 26 | macro_rules! iter { 27 | ($v:expr) => { 28 | $v.iter() 29 | }; 30 | } 31 | 32 | /// Nav islands identifier. 33 | pub type NavIslandsID = ID>; 34 | 35 | #[derive(Debug, Clone, PartialEq, Eq, Hash, Serialize, Deserialize)] 36 | pub struct NavIslandPortal 37 | where 38 | Island: std::fmt::Debug + Clone + Eq + Hash + Send + Sync, 39 | Portal: std::fmt::Debug + Clone + Eq + Hash + Send + Sync, 40 | { 41 | #[serde(bound(deserialize = "Island: Serialize + DeserializeOwned"))] 42 | pub island: Island, 43 | #[serde(bound(deserialize = "Portal: Serialize + DeserializeOwned"))] 44 | pub portal: Option, 45 | } 46 | 47 | #[derive(Debug, Clone, Serialize, Deserialize)] 48 | pub struct NavIslandsConnection 49 | where 50 | Island: std::fmt::Debug + Clone + Eq + Hash + Send + Sync, 51 | Portal: std::fmt::Debug + Clone + Eq + Hash + Send + Sync, 52 | { 53 | #[serde(bound( 54 | deserialize = "Island: Serialize + DeserializeOwned, Portal: Serialize + DeserializeOwned" 55 | ))] 56 | pub from: NavIslandPortal, 57 | #[serde(bound( 58 | deserialize = "Island: Serialize + DeserializeOwned, Portal: Serialize + DeserializeOwned" 59 | ))] 60 | pub to: NavIslandPortal, 61 | pub distance: Scalar, 62 | } 63 | 64 | #[derive(Debug, Default, Clone, Serialize, Deserialize)] 65 | pub struct NavIslands 66 | where 67 | Island: std::fmt::Debug + Clone + Eq + Hash + Send + Sync, 68 | Portal: std::fmt::Debug + Clone + Eq + Hash + Send + Sync, 69 | { 70 | id: NavIslandsID, 71 | costs: Vec, 72 | #[serde(bound( 73 | deserialize = "Island: Serialize + DeserializeOwned, Portal: Serialize + DeserializeOwned" 74 | ))] 75 | portals: Vec>, 76 | graph: Graph<(), Scalar, Directed>, 77 | nodes: Vec, 78 | nodes_map: HashMap, 79 | } 80 | 81 | impl NavIslands 82 | where 83 | Island: std::fmt::Debug + Clone + Eq + Hash + Send + Sync, 84 | Portal: std::fmt::Debug + Clone + Eq + Hash + Send + Sync, 85 | { 86 | pub fn new(connections: Vec>, both_ways: bool) -> Self { 87 | let portals = connections 88 | .iter() 89 | .map(|c| c.from.clone()) 90 | .chain(connections.iter().map(|c| c.to.clone())) 91 | .collect::>() 92 | .into_iter() 93 | .collect::>(); 94 | let costs = vec![1.0; portals.len()]; 95 | let mut graph = 96 | Graph::<(), Scalar, Directed>::with_capacity(portals.len(), connections.len()); 97 | let nodes = (0..portals.len()) 98 | .map(|_| graph.add_node(())) 99 | .collect::>(); 100 | for connection in connections { 101 | let ia = portals.iter().position(|c| &connection.from == c); 102 | let ib = portals.iter().position(|c| &connection.to == c); 103 | if let (Some(ia), Some(ib)) = (ia, ib) { 104 | graph.add_edge(nodes[ia], nodes[ib], connection.distance); 105 | if both_ways { 106 | graph.add_edge(nodes[ib], nodes[ia], connection.distance); 107 | } 108 | } 109 | } 110 | let nodes_map = iter!(nodes).enumerate().map(|(i, n)| (*n, i)).collect(); 111 | Self { 112 | id: NavIslandsID::new(), 113 | costs, 114 | portals, 115 | graph, 116 | nodes, 117 | nodes_map, 118 | } 119 | } 120 | 121 | #[inline] 122 | pub fn id(&self) -> NavIslandsID { 123 | self.id 124 | } 125 | 126 | #[inline] 127 | pub fn portals(&self) -> &[NavIslandPortal] { 128 | &self.portals 129 | } 130 | 131 | #[inline] 132 | pub fn portals_costs(&self) -> &[Scalar] { 133 | &self.costs 134 | } 135 | 136 | #[inline] 137 | pub fn set_portal_cost( 138 | &mut self, 139 | portal: &NavIslandPortal, 140 | cost: Scalar, 141 | ) -> Option { 142 | let index = self.index(portal)?; 143 | let c = self.costs.get_mut(index)?; 144 | let old = *c; 145 | *c = cost.max(0.0); 146 | Some(old) 147 | } 148 | 149 | pub fn neighbors( 150 | &self, 151 | portal: &NavIslandPortal, 152 | ) -> Option> + '_> { 153 | let index = self.index(portal)?; 154 | let node = self.nodes[index]; 155 | Some(self.graph.neighbors(node).filter_map(|node| { 156 | self.nodes_map 157 | .get(&node) 158 | .and_then(|index| self.portal(*index)) 159 | })) 160 | } 161 | 162 | pub fn find_path( 163 | &self, 164 | from: &NavIslandPortal, 165 | to: &NavIslandPortal, 166 | ) -> Option<(Scalar, Vec<&NavIslandPortal>)> { 167 | self.find_path_custom(from, to, |_, _| true) 168 | } 169 | 170 | // filter params: first island-portal, second island-portal. 171 | pub fn find_path_custom( 172 | &self, 173 | from: &NavIslandPortal, 174 | to: &NavIslandPortal, 175 | mut filter: F, 176 | ) -> Option<(Scalar, Vec<&NavIslandPortal>)> 177 | where 178 | F: FnMut(&NavIslandPortal, &NavIslandPortal) -> bool, 179 | { 180 | let start_index = self.index(from)?; 181 | let end_index = self.index(to)?; 182 | let start_node = *self.nodes.get(start_index)?; 183 | let end_node = *self.nodes.get(end_index)?; 184 | let (distance, nodes) = astar( 185 | &self.graph, 186 | start_node, 187 | |n| n == end_node, 188 | |e| { 189 | let a = self.nodes_map[&e.source()]; 190 | let b = self.nodes_map[&e.target()]; 191 | let w = *e.weight(); 192 | if filter(self.portal(a).unwrap(), self.portal(b).unwrap()) { 193 | let a = self.costs[a]; 194 | let b = self.costs[b]; 195 | w * a * b 196 | } else { 197 | SCALAR_MAX 198 | } 199 | }, 200 | |_| 0.0, 201 | )?; 202 | Some(( 203 | distance, 204 | nodes 205 | .into_iter() 206 | .filter_map(|n| self.portal(self.nodes_map[&n])) 207 | .collect::>(), 208 | )) 209 | } 210 | 211 | pub fn find_islands(&self) -> Vec>> { 212 | tarjan_scc(&self.graph) 213 | .into_iter() 214 | .map(|v| { 215 | v.into_iter() 216 | .filter_map(|n| self.nodes_map.get(&n).and_then(|i| self.portal(*i))) 217 | .collect::>() 218 | }) 219 | .filter(|v| !v.is_empty()) 220 | .collect() 221 | } 222 | 223 | pub fn index(&self, portal: &NavIslandPortal) -> Option { 224 | self.portals.iter().position(|p| portal == p) 225 | } 226 | 227 | pub fn portal(&self, index: usize) -> Option<&NavIslandPortal> { 228 | self.portals.get(index) 229 | } 230 | } 231 | -------------------------------------------------------------------------------- /src/nav_mesh.rs: -------------------------------------------------------------------------------- 1 | use crate::{Error, NavConnection, NavResult, NavVec3, Scalar, ZERO_TRESHOLD}; 2 | use petgraph::{ 3 | algo::{astar, tarjan_scc}, 4 | graph::NodeIndex, 5 | visit::EdgeRef, 6 | Graph, Undirected, 7 | }; 8 | #[cfg(feature = "parallel")] 9 | use rayon::prelude::*; 10 | use serde::{Deserialize, Serialize}; 11 | use spade::{rtree::RTree, BoundingRect, SpatialObject}; 12 | use std::collections::HashMap; 13 | #[cfg(not(feature = "scalar64"))] 14 | use std::f32::MAX as SCALAR_MAX; 15 | #[cfg(feature = "scalar64")] 16 | use std::f64::MAX as SCALAR_MAX; 17 | use typid::ID; 18 | 19 | #[cfg(feature = "parallel")] 20 | macro_rules! iter { 21 | ($v:expr) => { 22 | $v.par_iter() 23 | }; 24 | } 25 | #[cfg(not(feature = "parallel"))] 26 | macro_rules! iter { 27 | ($v:expr) => { 28 | $v.iter() 29 | }; 30 | } 31 | #[cfg(feature = "parallel")] 32 | macro_rules! into_iter { 33 | ($v:expr) => { 34 | $v.into_par_iter() 35 | }; 36 | } 37 | #[cfg(not(feature = "parallel"))] 38 | macro_rules! into_iter { 39 | ($v:expr) => { 40 | $v.into_iter() 41 | }; 42 | } 43 | 44 | /// Nav mash identifier. 45 | pub type NavMeshID = ID; 46 | 47 | /// Nav mesh triangle description - lists used vertices indices. 48 | #[repr(C)] 49 | #[derive(Debug, Default, Copy, Clone, Serialize, Deserialize)] 50 | pub struct NavTriangle { 51 | pub first: u32, 52 | pub second: u32, 53 | pub third: u32, 54 | } 55 | 56 | impl From<(u32, u32, u32)> for NavTriangle { 57 | fn from(value: (u32, u32, u32)) -> Self { 58 | Self { 59 | first: value.0, 60 | second: value.1, 61 | third: value.2, 62 | } 63 | } 64 | } 65 | 66 | impl From<[u32; 3]> for NavTriangle { 67 | fn from(value: [u32; 3]) -> Self { 68 | Self { 69 | first: value[0], 70 | second: value[1], 71 | third: value[2], 72 | } 73 | } 74 | } 75 | 76 | /// Nav mesh area descriptor. Nav mesh area holds information about specific nav mesh triangle. 77 | #[repr(C)] 78 | #[derive(Debug, Default, Clone, Serialize, Deserialize)] 79 | pub struct NavArea { 80 | /// Triangle index. 81 | pub triangle: u32, 82 | /// Area size (triangle area value). 83 | pub size: Scalar, 84 | /// Traverse cost factor. Big values tells that this area is hard to traverse, smaller tells 85 | /// the opposite. 86 | pub cost: Scalar, 87 | /// Triangle center point. 88 | pub center: NavVec3, 89 | /// Radius of sphere that contains this triangle. 90 | pub radius: Scalar, 91 | /// Squared version of `radius`. 92 | pub radius_sqr: Scalar, 93 | } 94 | 95 | impl NavArea { 96 | /// Calculate triangle area value. 97 | /// 98 | /// # Arguments 99 | /// * `a` - first vertice point. 100 | /// * `b` - second vertice point. 101 | /// * `c` - thirs vertice point. 102 | #[inline] 103 | pub fn calculate_area(a: NavVec3, b: NavVec3, c: NavVec3) -> Scalar { 104 | let ab = b - a; 105 | let ac = c - a; 106 | ab.cross(ac).magnitude() * 0.5 107 | } 108 | 109 | /// Calculate triangle center point. 110 | /// 111 | /// # Arguments 112 | /// * `a` - first vertice point. 113 | /// * `b` - second vertice point. 114 | /// * `c` - thirs vertice point. 115 | #[inline] 116 | pub fn calculate_center(a: NavVec3, b: NavVec3, c: NavVec3) -> NavVec3 { 117 | let v = a + b + c; 118 | NavVec3::new(v.x / 3.0, v.y / 3.0, v.z / 3.0) 119 | } 120 | } 121 | 122 | #[derive(Debug, Clone, Serialize, Deserialize)] 123 | pub struct NavSpatialObject { 124 | pub index: usize, 125 | pub a: NavVec3, 126 | pub b: NavVec3, 127 | pub c: NavVec3, 128 | ab: NavVec3, 129 | bc: NavVec3, 130 | ca: NavVec3, 131 | normal: NavVec3, 132 | dab: NavVec3, 133 | dbc: NavVec3, 134 | dca: NavVec3, 135 | } 136 | 137 | impl NavSpatialObject { 138 | pub fn new(index: usize, a: NavVec3, b: NavVec3, c: NavVec3) -> Self { 139 | let ab = b - a; 140 | let bc = c - b; 141 | let ca = a - c; 142 | let normal = (a - b).cross(a - c).normalize(); 143 | let dab = normal.cross(ab); 144 | let dbc = normal.cross(bc); 145 | let dca = normal.cross(ca); 146 | Self { 147 | index, 148 | a, 149 | b, 150 | c, 151 | ab, 152 | bc, 153 | ca, 154 | normal, 155 | dab, 156 | dbc, 157 | dca, 158 | } 159 | } 160 | 161 | #[inline] 162 | pub fn normal(&self) -> NavVec3 { 163 | self.normal 164 | } 165 | 166 | pub fn closest_point(&self, point: NavVec3) -> NavVec3 { 167 | let pab = point.project(self.a, self.b); 168 | let pbc = point.project(self.b, self.c); 169 | let pca = point.project(self.c, self.a); 170 | if pca > 1.0 && pab < 0.0 { 171 | return self.a; 172 | } else if pab > 1.0 && pbc < 0.0 { 173 | return self.b; 174 | } else if pbc > 1.0 && pca < 0.0 { 175 | return self.c; 176 | } else if (0.0..=1.0).contains(&pab) && !point.is_above_plane(self.a, self.dab) { 177 | return NavVec3::unproject(self.a, self.b, pab); 178 | } else if (0.0..=1.0).contains(&pbc) && !point.is_above_plane(self.b, self.dbc) { 179 | return NavVec3::unproject(self.b, self.c, pbc); 180 | } else if (0.0..=1.0).contains(&pca) && !point.is_above_plane(self.c, self.dca) { 181 | return NavVec3::unproject(self.c, self.a, pca); 182 | } 183 | point.project_on_plane(self.a, self.normal) 184 | } 185 | } 186 | 187 | impl SpatialObject for NavSpatialObject { 188 | type Point = NavVec3; 189 | 190 | fn mbr(&self) -> BoundingRect { 191 | let min = NavVec3::new( 192 | self.a.x.min(self.b.x).min(self.c.x), 193 | self.a.y.min(self.b.y).min(self.c.y), 194 | self.a.z.min(self.b.z).min(self.c.z), 195 | ); 196 | let max = NavVec3::new( 197 | self.a.x.max(self.b.x).max(self.c.x), 198 | self.a.y.max(self.b.y).max(self.c.y), 199 | self.a.z.max(self.b.z).max(self.c.z), 200 | ); 201 | BoundingRect::from_corners(&min, &max) 202 | } 203 | 204 | fn distance2(&self, point: &Self::Point) -> Scalar { 205 | (*point - self.closest_point(*point)).sqr_magnitude() 206 | } 207 | } 208 | 209 | /// Quality of querying a point on nav mesh. 210 | #[derive(Debug, Clone, Copy, Serialize, Deserialize)] 211 | pub enum NavQuery { 212 | /// Best quality, totally accurate. 213 | Accuracy, 214 | /// Medium quality, finds point in closest triangle. 215 | Closest, 216 | /// Low quality, finds first triangle in range of query. 217 | ClosestFirst, 218 | } 219 | 220 | /// Quality of finding path. 221 | #[derive(Debug, Clone, Copy, Serialize, Deserialize)] 222 | pub enum NavPathMode { 223 | /// Best quality, finds shortest path. 224 | Accuracy, 225 | /// Medium quality, finds shortest path througs triangles midpoints. 226 | MidPoints, 227 | } 228 | 229 | /// Nav mesh object used to find shortest path between two points. 230 | #[derive(Debug, Default, Clone, Serialize, Deserialize)] 231 | pub struct NavMesh { 232 | id: NavMeshID, 233 | vertices: Vec, 234 | triangles: Vec, 235 | areas: Vec, 236 | // {triangle connection: (distance sqr, vertex connection)} 237 | connections: HashMap, 238 | graph: Graph<(), Scalar, Undirected>, 239 | nodes: Vec, 240 | nodes_map: HashMap, 241 | rtree: RTree, 242 | spatials: Vec, 243 | // {triangle index: [(from, to)]} 244 | hard_edges: HashMap>, 245 | origin: NavVec3, 246 | } 247 | 248 | impl NavMesh { 249 | /// Create new nav mesh object from vertices and triangles. 250 | /// 251 | /// # Arguments 252 | /// * `vertices` - list of vertices points. 253 | /// * `triangles` - list of vertices indices that produces triangles. 254 | /// 255 | /// # Returns 256 | /// `Ok` with nav mesh object or `Err` with `Error::TriangleVerticeIndexOutOfBounds` if input 257 | /// data is invalid. 258 | /// 259 | /// # Example 260 | /// ``` 261 | /// use navmesh::*; 262 | /// 263 | /// let vertices = vec![ 264 | /// (0.0, 0.0, 0.0).into(), // 0 265 | /// (1.0, 0.0, 0.0).into(), // 1 266 | /// (2.0, 0.0, 1.0).into(), // 2 267 | /// (0.0, 1.0, 0.0).into(), // 3 268 | /// (1.0, 1.0, 0.0).into(), // 4 269 | /// (2.0, 1.0, 1.0).into(), // 5 270 | /// ]; 271 | /// let triangles = vec![ 272 | /// (0, 1, 4).into(), // 0 273 | /// (4, 3, 0).into(), // 1 274 | /// (1, 2, 5).into(), // 2 275 | /// (5, 4, 1).into(), // 3 276 | /// ]; 277 | /// 278 | /// let mesh = NavMesh::new(vertices, triangles).unwrap(); 279 | /// ``` 280 | pub fn new(vertices: Vec, triangles: Vec) -> NavResult { 281 | let origin = vertices 282 | .iter() 283 | .cloned() 284 | .fold(NavVec3::default(), |a, v| a + v) 285 | / vertices.len() as Scalar; 286 | 287 | let areas = iter!(triangles) 288 | .enumerate() 289 | .map(|(i, triangle)| { 290 | if triangle.first >= vertices.len() as u32 { 291 | return Err(Error::TriangleVerticeIndexOutOfBounds( 292 | i as u32, 293 | 0, 294 | triangle.first, 295 | )); 296 | } 297 | if triangle.second >= vertices.len() as u32 { 298 | return Err(Error::TriangleVerticeIndexOutOfBounds( 299 | i as u32, 300 | 1, 301 | triangle.second, 302 | )); 303 | } 304 | if triangle.third >= vertices.len() as u32 { 305 | return Err(Error::TriangleVerticeIndexOutOfBounds( 306 | i as u32, 307 | 2, 308 | triangle.third, 309 | )); 310 | } 311 | let first = vertices[triangle.first as usize]; 312 | let second = vertices[triangle.second as usize]; 313 | let third = vertices[triangle.third as usize]; 314 | let center = NavArea::calculate_center(first, second, third); 315 | let radius = (first - center) 316 | .magnitude() 317 | .max((second - center).magnitude()) 318 | .max((third - center).magnitude()); 319 | Ok(NavArea { 320 | triangle: i as u32, 321 | size: NavArea::calculate_area(first, second, third), 322 | cost: 1.0, 323 | center, 324 | radius, 325 | radius_sqr: radius * radius, 326 | }) 327 | }) 328 | .collect::>>()?; 329 | 330 | // {edge: [triangle index]} 331 | let mut edges = HashMap::>::with_capacity(triangles.len() * 3); 332 | for (index, triangle) in triangles.iter().enumerate() { 333 | let edge_a = NavConnection(triangle.first, triangle.second); 334 | let edge_b = NavConnection(triangle.second, triangle.third); 335 | let edge_c = NavConnection(triangle.third, triangle.first); 336 | if let Some(tris) = edges.get_mut(&edge_a) { 337 | tris.push(index); 338 | } else { 339 | edges.insert(edge_a, vec![index]); 340 | } 341 | if let Some(tris) = edges.get_mut(&edge_b) { 342 | tris.push(index); 343 | } else { 344 | edges.insert(edge_b, vec![index]); 345 | } 346 | if let Some(tris) = edges.get_mut(&edge_c) { 347 | tris.push(index); 348 | } else { 349 | edges.insert(edge_c, vec![index]); 350 | } 351 | } 352 | 353 | let connections = into_iter!(iter!(edges) 354 | .flat_map(|(verts, tris)| { 355 | let mut result = HashMap::with_capacity(tris.len() * tris.len()); 356 | for a in tris { 357 | for b in tris { 358 | if a != b { 359 | result.insert(NavConnection(*a as u32, *b as u32), *verts); 360 | } 361 | } 362 | } 363 | result 364 | }) 365 | .collect::>()) 366 | .map(|(tri_conn, vert_conn)| { 367 | let a = areas[tri_conn.0 as usize].center; 368 | let b = areas[tri_conn.1 as usize].center; 369 | let weight = (b - a).sqr_magnitude(); 370 | (tri_conn, (weight, vert_conn)) 371 | }) 372 | .collect::>(); 373 | 374 | let mut graph = Graph::<(), Scalar, Undirected>::new_undirected(); 375 | let nodes = (0..triangles.len()) 376 | .map(|_| graph.add_node(())) 377 | .collect::>(); 378 | graph.extend_with_edges( 379 | iter!(connections) 380 | .map(|(conn, (w, _))| (nodes[conn.0 as usize], nodes[conn.1 as usize], w)) 381 | .collect::>(), 382 | ); 383 | let nodes_map = iter!(nodes).enumerate().map(|(i, n)| (*n, i)).collect(); 384 | 385 | let spatials = iter!(triangles) 386 | .enumerate() 387 | .map(|(index, triangle)| { 388 | NavSpatialObject::new( 389 | index, 390 | vertices[triangle.first as usize], 391 | vertices[triangle.second as usize], 392 | vertices[triangle.third as usize], 393 | ) 394 | }) 395 | .collect::>(); 396 | 397 | let mut rtree = RTree::new(); 398 | for spatial in &spatials { 399 | rtree.insert(spatial.clone()); 400 | } 401 | 402 | let hard_edges = iter!(triangles) 403 | .enumerate() 404 | .filter_map(|(index, triangle)| { 405 | let edge_a = NavConnection(triangle.first, triangle.second); 406 | let edge_b = NavConnection(triangle.second, triangle.third); 407 | let edge_c = NavConnection(triangle.third, triangle.first); 408 | let mut planes = vec![]; 409 | if edges[&edge_a].len() < 2 { 410 | planes.push(( 411 | vertices[triangle.first as usize], 412 | vertices[triangle.second as usize], 413 | )); 414 | } 415 | if edges[&edge_b].len() < 2 { 416 | planes.push(( 417 | vertices[triangle.second as usize], 418 | vertices[triangle.third as usize], 419 | )); 420 | } 421 | if edges[&edge_c].len() < 2 { 422 | planes.push(( 423 | vertices[triangle.third as usize], 424 | vertices[triangle.first as usize], 425 | )); 426 | } 427 | if planes.is_empty() { 428 | None 429 | } else { 430 | Some((index, planes)) 431 | } 432 | }) 433 | .collect::>(); 434 | 435 | Ok(Self { 436 | id: ID::new(), 437 | vertices, 438 | triangles, 439 | areas, 440 | connections, 441 | graph, 442 | nodes, 443 | nodes_map, 444 | rtree, 445 | spatials, 446 | hard_edges, 447 | origin, 448 | }) 449 | } 450 | 451 | pub fn thicken(&self, value: Scalar) -> NavResult { 452 | let shifted = iter!(self.vertices) 453 | .enumerate() 454 | .map(|(i, v)| { 455 | let (mut n, c) = self 456 | .triangles 457 | .iter() 458 | .enumerate() 459 | .filter_map(|(j, t)| { 460 | if t.first == i as u32 || t.second == i as u32 || t.third == i as u32 { 461 | Some(self.spatials[j].normal) 462 | } else { 463 | None 464 | } 465 | }) 466 | .fold((NavVec3::default(), 0), |a, v| (a.0 + v, a.1 + 1)); 467 | if c > 1 { 468 | n = n / c as Scalar; 469 | } 470 | *v + n.normalize() * value 471 | }) 472 | .collect::>(); 473 | Self::new(shifted, self.triangles.clone()) 474 | } 475 | 476 | pub fn scale(&self, value: NavVec3, origin: Option) -> NavResult { 477 | let origin = origin.unwrap_or(self.origin); 478 | let vertices = iter!(self.vertices) 479 | .map(|v| (*v - origin) * value + origin) 480 | .collect::>(); 481 | Self::new(vertices, self.triangles.clone()) 482 | } 483 | 484 | /// Nav mesh identifier. 485 | #[inline] 486 | pub fn id(&self) -> NavMeshID { 487 | self.id 488 | } 489 | 490 | /// Nav mesh origin point. 491 | #[inline] 492 | pub fn origin(&self) -> NavVec3 { 493 | self.origin 494 | } 495 | 496 | /// Reference to list of nav mesh vertices points. 497 | #[inline] 498 | pub fn vertices(&self) -> &[NavVec3] { 499 | &self.vertices 500 | } 501 | 502 | /// Reference to list of nav mesh triangles. 503 | #[inline] 504 | pub fn triangles(&self) -> &[NavTriangle] { 505 | &self.triangles 506 | } 507 | 508 | /// Reference to list of nav mesh area descriptors. 509 | #[inline] 510 | pub fn areas(&self) -> &[NavArea] { 511 | &self.areas 512 | } 513 | 514 | /// Set area cost by triangle index. 515 | /// 516 | /// # Arguments 517 | /// * `index` - triangle index. 518 | /// * `cost` - cost factor. 519 | /// 520 | /// # Returns 521 | /// Old area cost value. 522 | #[inline] 523 | pub fn set_area_cost(&mut self, index: usize, cost: Scalar) -> Scalar { 524 | let area = &mut self.areas[index]; 525 | let old = area.cost; 526 | let cost = cost.max(0.0); 527 | area.cost = cost; 528 | old 529 | } 530 | 531 | /// Find closest point on nav mesh. 532 | /// 533 | /// # Arguments 534 | /// * `point` - query point. 535 | /// * `query` - query quality. 536 | /// 537 | /// # Returns 538 | /// `Some` with point on nav mesh if found or `None` otherwise. 539 | pub fn closest_point(&self, point: NavVec3, query: NavQuery) -> Option { 540 | self.find_closest_triangle(point, query) 541 | .map(|triangle| self.spatials[triangle].closest_point(point)) 542 | } 543 | 544 | /// Find shortest path on nav mesh between two points. 545 | /// 546 | /// # Arguments 547 | /// * `from` - query point from. 548 | /// * `to` - query point to. 549 | /// * `query` - query quality. 550 | /// * `mode` - path finding quality. 551 | /// 552 | /// # Returns 553 | /// `Some` with path points on nav mesh if found or `None` otherwise. 554 | /// 555 | /// # Example 556 | /// ``` 557 | /// use navmesh::*; 558 | /// 559 | /// let vertices = vec![ 560 | /// (0.0, 0.0, 0.0).into(), // 0 561 | /// (1.0, 0.0, 0.0).into(), // 1 562 | /// (2.0, 0.0, 1.0).into(), // 2 563 | /// (0.0, 1.0, 0.0).into(), // 3 564 | /// (1.0, 1.0, 0.0).into(), // 4 565 | /// (2.0, 1.0, 1.0).into(), // 5 566 | /// ]; 567 | /// let triangles = vec![ 568 | /// (0, 1, 4).into(), // 0 569 | /// (4, 3, 0).into(), // 1 570 | /// (1, 2, 5).into(), // 2 571 | /// (5, 4, 1).into(), // 3 572 | /// ]; 573 | /// 574 | /// let mesh = NavMesh::new(vertices, triangles).unwrap(); 575 | /// let path = mesh 576 | /// .find_path( 577 | /// (0.0, 1.0, 0.0).into(), 578 | /// (1.5, 0.25, 0.5).into(), 579 | /// NavQuery::Accuracy, 580 | /// NavPathMode::MidPoints, 581 | /// ) 582 | /// .unwrap(); 583 | /// assert_eq!( 584 | /// path.into_iter() 585 | /// .map(|v| ( 586 | /// (v.x * 10.0) as i32, 587 | /// (v.y * 10.0) as i32, 588 | /// (v.z * 10.0) as i32, 589 | /// )) 590 | /// .collect::>(), 591 | /// vec![(0, 10, 0), (10, 5, 0), (15, 2, 5),] 592 | /// ); 593 | /// ``` 594 | pub fn find_path( 595 | &self, 596 | from: NavVec3, 597 | to: NavVec3, 598 | query: NavQuery, 599 | mode: NavPathMode, 600 | ) -> Option> { 601 | self.find_path_custom(from, to, query, mode, |_, _, _| true) 602 | } 603 | 604 | /// Find shortest path on nav mesh between two points, providing custom filtering function. 605 | /// 606 | /// # Arguments 607 | /// * `from` - query point from. 608 | /// * `to` - query point to. 609 | /// * `query` - query quality. 610 | /// * `mode` - path finding quality. 611 | /// * `filter` - closure that gives you a connection distance squared, first triangle index 612 | /// and second triangle index. 613 | /// 614 | /// # Returns 615 | /// `Some` with path points on nav mesh if found or `None` otherwise. 616 | /// 617 | /// # Example 618 | /// ``` 619 | /// use navmesh::*; 620 | /// 621 | /// let vertices = vec![ 622 | /// (0.0, 0.0, 0.0).into(), // 0 623 | /// (1.0, 0.0, 0.0).into(), // 1 624 | /// (2.0, 0.0, 1.0).into(), // 2 625 | /// (0.0, 1.0, 0.0).into(), // 3 626 | /// (1.0, 1.0, 0.0).into(), // 4 627 | /// (2.0, 1.0, 1.0).into(), // 5 628 | /// ]; 629 | /// let triangles = vec![ 630 | /// (0, 1, 4).into(), // 0 631 | /// (4, 3, 0).into(), // 1 632 | /// (1, 2, 5).into(), // 2 633 | /// (5, 4, 1).into(), // 3 634 | /// ]; 635 | /// 636 | /// let mesh = NavMesh::new(vertices, triangles).unwrap(); 637 | /// let path = mesh 638 | /// .find_path_custom( 639 | /// (0.0, 1.0, 0.0).into(), 640 | /// (1.5, 0.25, 0.5).into(), 641 | /// NavQuery::Accuracy, 642 | /// NavPathMode::MidPoints, 643 | /// |_dist_sqr, _first_idx, _second_idx| true, 644 | /// ) 645 | /// .unwrap(); 646 | /// assert_eq!( 647 | /// path.into_iter() 648 | /// .map(|v| ( 649 | /// (v.x * 10.0) as i32, 650 | /// (v.y * 10.0) as i32, 651 | /// (v.z * 10.0) as i32, 652 | /// )) 653 | /// .collect::>(), 654 | /// vec![(0, 10, 0), (10, 5, 0), (15, 2, 5),] 655 | /// ); 656 | /// ``` 657 | pub fn find_path_custom( 658 | &self, 659 | from: NavVec3, 660 | to: NavVec3, 661 | query: NavQuery, 662 | mode: NavPathMode, 663 | filter: F, 664 | ) -> Option> 665 | where 666 | F: FnMut(Scalar, usize, usize) -> bool, 667 | { 668 | if from.same_as(to) { 669 | return None; 670 | } 671 | let start = self.find_closest_triangle(from, query)?; 672 | let end = self.find_closest_triangle(to, query)?; 673 | let from = self.spatials[start].closest_point(from); 674 | let to = self.spatials[end].closest_point(to); 675 | let (triangles, _) = self.find_path_triangles_custom(start, end, filter)?; 676 | if triangles.is_empty() { 677 | return None; 678 | } else if triangles.len() == 1 { 679 | return Some(vec![from, to]); 680 | } 681 | match mode { 682 | NavPathMode::Accuracy => Some(self.find_path_accuracy(from, to, &triangles)), 683 | NavPathMode::MidPoints => Some(self.find_path_midpoints(from, to, &triangles)), 684 | } 685 | } 686 | 687 | fn find_path_accuracy(&self, from: NavVec3, to: NavVec3, triangles: &[usize]) -> Vec { 688 | #[derive(Debug)] 689 | enum Node { 690 | Point(NavVec3), 691 | // (a, b, normal) 692 | LevelChange(NavVec3, NavVec3, NavVec3), 693 | } 694 | 695 | // TODO: reduce allocations. 696 | if triangles.len() == 2 { 697 | let NavConnection(a, b) = 698 | self.connections[&NavConnection(triangles[0] as u32, triangles[1] as u32)].1; 699 | let a = self.vertices[a as usize]; 700 | let b = self.vertices[b as usize]; 701 | let n = self.spatials[triangles[0]].normal(); 702 | let m = self.spatials[triangles[1]].normal(); 703 | if !NavVec3::is_line_between_points(from, to, a, b, n) { 704 | let da = (from - a).sqr_magnitude(); 705 | let db = (from - b).sqr_magnitude(); 706 | let point = if da < db { a } else { b }; 707 | return vec![from, point, to]; 708 | } else if n.dot(m) < 1.0 - ZERO_TRESHOLD { 709 | let n = (b - a).normalize().cross(n); 710 | if let Some(point) = NavVec3::raycast_line(from, to, a, b, n) { 711 | return vec![from, point, to]; 712 | } 713 | } 714 | return vec![from, to]; 715 | } 716 | let mut start = from; 717 | let mut last_normal = self.spatials[triangles[0]].normal(); 718 | let mut nodes = Vec::with_capacity(triangles.len() - 1); 719 | for triplets in triangles.windows(3) { 720 | let NavConnection(a, b) = 721 | self.connections[&NavConnection(triplets[0] as u32, triplets[1] as u32)].1; 722 | let a = self.vertices[a as usize]; 723 | let b = self.vertices[b as usize]; 724 | let NavConnection(c, d) = 725 | self.connections[&NavConnection(triplets[1] as u32, triplets[2] as u32)].1; 726 | let c = self.vertices[c as usize]; 727 | let d = self.vertices[d as usize]; 728 | let normal = self.spatials[triplets[1]].normal(); 729 | let old_last_normal = last_normal; 730 | last_normal = normal; 731 | if !NavVec3::is_line_between_points(start, c, a, b, normal) 732 | || !NavVec3::is_line_between_points(start, d, a, b, normal) 733 | { 734 | let da = (start - a).sqr_magnitude(); 735 | let db = (start - b).sqr_magnitude(); 736 | start = if da < db { a } else { b }; 737 | nodes.push(Node::Point(start)); 738 | } else if old_last_normal.dot(normal) < 1.0 - ZERO_TRESHOLD { 739 | let normal = self.spatials[triplets[0]].normal(); 740 | let normal = (b - a).normalize().cross(normal); 741 | nodes.push(Node::LevelChange(a, b, normal)); 742 | } 743 | } 744 | { 745 | let NavConnection(a, b) = self.connections[&NavConnection( 746 | triangles[triangles.len() - 2] as u32, 747 | triangles[triangles.len() - 1] as u32, 748 | )] 749 | .1; 750 | let a = self.vertices[a as usize]; 751 | let b = self.vertices[b as usize]; 752 | let n = self.spatials[triangles[triangles.len() - 2]].normal(); 753 | let m = self.spatials[triangles[triangles.len() - 1]].normal(); 754 | if !NavVec3::is_line_between_points(start, to, a, b, n) { 755 | let da = (start - a).sqr_magnitude(); 756 | let db = (start - b).sqr_magnitude(); 757 | let point = if da < db { a } else { b }; 758 | nodes.push(Node::Point(point)); 759 | } else if n.dot(m) < 1.0 - ZERO_TRESHOLD { 760 | let n = (b - a).normalize().cross(n); 761 | nodes.push(Node::LevelChange(a, b, n)); 762 | } 763 | } 764 | 765 | let mut points = Vec::with_capacity(nodes.len() + 2); 766 | points.push(from); 767 | let mut point = from; 768 | for i in 0..nodes.len() { 769 | match nodes[i] { 770 | Node::Point(p) => { 771 | point = p; 772 | points.push(p); 773 | } 774 | Node::LevelChange(a, b, n) => { 775 | let next = nodes 776 | .iter() 777 | .skip(i + 1) 778 | .find_map(|n| match n { 779 | Node::Point(p) => Some(*p), 780 | _ => None, 781 | }) 782 | .unwrap_or(to); 783 | if let Some(p) = NavVec3::raycast_line(point, next, a, b, n) { 784 | points.push(p); 785 | } 786 | } 787 | } 788 | } 789 | points.push(to); 790 | points.dedup(); 791 | points 792 | } 793 | 794 | fn find_path_midpoints(&self, from: NavVec3, to: NavVec3, triangles: &[usize]) -> Vec { 795 | if triangles.len() == 2 { 796 | let NavConnection(a, b) = 797 | self.connections[&NavConnection(triangles[0] as u32, triangles[1] as u32)].1; 798 | let a = self.vertices[a as usize]; 799 | let b = self.vertices[b as usize]; 800 | let n = self.spatials[triangles[0]].normal(); 801 | let m = self.spatials[triangles[1]].normal(); 802 | if n.dot(m) < 1.0 - ZERO_TRESHOLD || !NavVec3::is_line_between_points(from, to, a, b, n) 803 | { 804 | return vec![from, (a + b) * 0.5, to]; 805 | } else { 806 | return vec![from, to]; 807 | } 808 | } 809 | let mut start = from; 810 | let mut last_normal = self.spatials[triangles[0]].normal(); 811 | let mut points = Vec::with_capacity(triangles.len() + 1); 812 | points.push(from); 813 | for triplets in triangles.windows(3) { 814 | let NavConnection(a, b) = 815 | self.connections[&NavConnection(triplets[0] as u32, triplets[1] as u32)].1; 816 | let a = self.vertices[a as usize]; 817 | let b = self.vertices[b as usize]; 818 | let point = (a + b) * 0.5; 819 | let normal = self.spatials[triplets[1]].normal(); 820 | let old_last_normal = last_normal; 821 | last_normal = normal; 822 | if old_last_normal.dot(normal) < 1.0 - ZERO_TRESHOLD { 823 | start = point; 824 | points.push(start); 825 | } else { 826 | let NavConnection(c, d) = 827 | self.connections[&NavConnection(triplets[1] as u32, triplets[2] as u32)].1; 828 | let c = self.vertices[c as usize]; 829 | let d = self.vertices[d as usize]; 830 | let end = (c + d) * 0.5; 831 | if !NavVec3::is_line_between_points(start, end, a, b, normal) { 832 | start = point; 833 | points.push(start); 834 | } 835 | } 836 | } 837 | { 838 | let NavConnection(a, b) = self.connections[&NavConnection( 839 | triangles[triangles.len() - 2] as u32, 840 | triangles[triangles.len() - 1] as u32, 841 | )] 842 | .1; 843 | let a = self.vertices[a as usize]; 844 | let b = self.vertices[b as usize]; 845 | let n = self.spatials[triangles[triangles.len() - 2]].normal(); 846 | let m = self.spatials[triangles[triangles.len() - 1]].normal(); 847 | if n.dot(m) < 1.0 - ZERO_TRESHOLD 848 | || !NavVec3::is_line_between_points(start, to, a, b, n) 849 | { 850 | points.push((a + b) * 0.5); 851 | } 852 | } 853 | points.push(to); 854 | points.dedup(); 855 | points 856 | } 857 | 858 | /// Find shortest path on nav mesh between two points. 859 | /// 860 | /// # Arguments 861 | /// * `from` - query point from. 862 | /// * `to` - query point to. 863 | /// * `query` - query quality. 864 | /// * `mode` - path finding quality. 865 | /// 866 | /// # Returns 867 | /// `Some` with path points on nav mesh and path length if found or `None` otherwise. 868 | /// 869 | /// # Example 870 | /// ``` 871 | /// use navmesh::*; 872 | /// 873 | /// let vertices = vec![ 874 | /// (0.0, 0.0, 0.0).into(), // 0 875 | /// (1.0, 0.0, 0.0).into(), // 1 876 | /// (2.0, 0.0, 1.0).into(), // 2 877 | /// (0.0, 1.0, 0.0).into(), // 3 878 | /// (1.0, 1.0, 0.0).into(), // 4 879 | /// (2.0, 1.0, 1.0).into(), // 5 880 | /// ]; 881 | /// let triangles = vec![ 882 | /// (0, 1, 4).into(), // 0 883 | /// (4, 3, 0).into(), // 1 884 | /// (1, 2, 5).into(), // 2 885 | /// (5, 4, 1).into(), // 3 886 | /// ]; 887 | /// 888 | /// let mesh = NavMesh::new(vertices, triangles).unwrap(); 889 | /// let path = mesh.find_path_triangles(1, 2).unwrap().0; 890 | /// assert_eq!(path, vec![1, 0, 3, 2]); 891 | /// ``` 892 | #[inline] 893 | pub fn find_path_triangles(&self, from: usize, to: usize) -> Option<(Vec, Scalar)> { 894 | self.find_path_triangles_custom(from, to, |_, _, _| true) 895 | } 896 | 897 | /// Find shortest path on nav mesh between two points, providing custom filtering function. 898 | /// 899 | /// # Arguments 900 | /// * `from` - query point from. 901 | /// * `to` - query point to. 902 | /// * `query` - query quality. 903 | /// * `mode` - path finding quality. 904 | /// * `filter` - closure that gives you a connection distance squared, first triangle index 905 | /// and second triangle index. 906 | /// 907 | /// # Returns 908 | /// `Some` with path points on nav mesh and path length if found or `None` otherwise. 909 | /// 910 | /// # Example 911 | /// ``` 912 | /// use navmesh::*; 913 | /// 914 | /// let vertices = vec![ 915 | /// (0.0, 0.0, 0.0).into(), // 0 916 | /// (1.0, 0.0, 0.0).into(), // 1 917 | /// (2.0, 0.0, 1.0).into(), // 2 918 | /// (0.0, 1.0, 0.0).into(), // 3 919 | /// (1.0, 1.0, 0.0).into(), // 4 920 | /// (2.0, 1.0, 1.0).into(), // 5 921 | /// ]; 922 | /// let triangles = vec![ 923 | /// (0, 1, 4).into(), // 0 924 | /// (4, 3, 0).into(), // 1 925 | /// (1, 2, 5).into(), // 2 926 | /// (5, 4, 1).into(), // 3 927 | /// ]; 928 | /// 929 | /// let mesh = NavMesh::new(vertices, triangles).unwrap(); 930 | /// let path = mesh.find_path_triangles_custom( 931 | /// 1, 932 | /// 2, 933 | /// |_dist_sqr, _first_idx, _second_idx| true 934 | /// ).unwrap().0; 935 | /// assert_eq!(path, vec![1, 0, 3, 2]); 936 | /// ``` 937 | #[inline] 938 | pub fn find_path_triangles_custom( 939 | &self, 940 | from: usize, 941 | to: usize, 942 | mut filter: F, 943 | ) -> Option<(Vec, Scalar)> 944 | where 945 | F: FnMut(Scalar, usize, usize) -> bool, 946 | { 947 | let to = self.nodes[to]; 948 | astar( 949 | &self.graph, 950 | self.nodes[from], 951 | |n| n == to, 952 | |e| { 953 | let a = self.nodes_map[&e.source()]; 954 | let b = self.nodes_map[&e.target()]; 955 | let w = *e.weight(); 956 | if filter(w, a, b) { 957 | let a = self.areas[a].cost; 958 | let b = self.areas[b].cost; 959 | w * a * b 960 | } else { 961 | SCALAR_MAX 962 | } 963 | }, 964 | |_| 0.0, 965 | ) 966 | .map(|(c, v)| (iter!(v).map(|v| self.nodes_map[v]).collect(), c)) 967 | } 968 | 969 | pub fn find_triangle_islands(&self) -> Vec> { 970 | tarjan_scc(&self.graph) 971 | .into_iter() 972 | .map(|v| { 973 | v.into_iter() 974 | .filter_map(|n| self.nodes_map.get(&n).copied()) 975 | .collect::>() 976 | }) 977 | .filter(|v| !v.is_empty()) 978 | .collect() 979 | } 980 | 981 | /// Find closest triangle on nav mesh closest to given point. 982 | /// 983 | /// # Arguments 984 | /// * `point` - query point. 985 | /// * `query` - query quality. 986 | /// 987 | /// # Returns 988 | /// `Some` with nav mesh triangle index if found or `None` otherwise. 989 | pub fn find_closest_triangle(&self, point: NavVec3, query: NavQuery) -> Option { 990 | match query { 991 | NavQuery::Accuracy => self.rtree.nearest_neighbor(&point).map(|t| t.index), 992 | NavQuery::ClosestFirst => self.rtree.close_neighbor(&point).map(|t| t.index), 993 | NavQuery::Closest => self 994 | .rtree 995 | .nearest_neighbors(&point) 996 | .into_iter() 997 | .map(|o| (o.distance2(&point), o)) 998 | .fold(None, |a: Option<(Scalar, &NavSpatialObject)>, i| { 999 | if let Some(a) = a { 1000 | if i.0 < a.0 { 1001 | Some(i) 1002 | } else { 1003 | Some(a) 1004 | } 1005 | } else { 1006 | Some(i) 1007 | } 1008 | }) 1009 | .map(|(_, t)| t.index), 1010 | } 1011 | } 1012 | 1013 | /// Find target point on nav mesh path. 1014 | /// 1015 | /// # Arguments 1016 | /// * `path` - path points. 1017 | /// * `point` - source point. 1018 | /// * `offset` - target point offset from the source on path. 1019 | /// 1020 | /// # Returns 1021 | /// `Some` with point and distance from path start point if found or `None` otherwise. 1022 | pub fn path_target_point( 1023 | path: &[NavVec3], 1024 | point: NavVec3, 1025 | offset: Scalar, 1026 | ) -> Option<(NavVec3, Scalar)> { 1027 | let s = Self::project_on_path(path, point, offset); 1028 | Some((Self::point_on_path(path, s)?, s)) 1029 | } 1030 | 1031 | /// Project point on nav mesh path. 1032 | /// 1033 | /// # Arguments 1034 | /// * `path` - path points. 1035 | /// * `point` - source point. 1036 | /// * `offset` - target point offset from the source on path. 1037 | /// 1038 | /// # Returns 1039 | /// Distance from path start point. 1040 | pub fn project_on_path(path: &[NavVec3], point: NavVec3, offset: Scalar) -> Scalar { 1041 | let p = match path.len() { 1042 | 0 | 1 => 0.0, 1043 | 2 => Self::project_on_line(path[0], path[1], point), 1044 | _ => { 1045 | path.windows(2) 1046 | .scan(0.0, |state, pair| { 1047 | let dist = *state; 1048 | *state += (pair[1] - pair[0]).magnitude(); 1049 | Some((dist, pair)) 1050 | }) 1051 | .map(|(dist, pair)| { 1052 | let (p, s) = Self::point_on_line(pair[0], pair[1], point); 1053 | (dist + s, (p - point).sqr_magnitude()) 1054 | }) 1055 | .min_by(|(_, a), (_, b)| a.partial_cmp(b).unwrap()) 1056 | .unwrap() 1057 | .0 1058 | } 1059 | }; 1060 | (p + offset).max(0.0).min(Self::path_length(path)) 1061 | } 1062 | 1063 | /// Find point on nav mesh path at given distance. 1064 | /// 1065 | /// # Arguments 1066 | /// * `path` - path points. 1067 | /// * `s` - Distance from path start point. 1068 | /// 1069 | /// # Returns 1070 | /// `Some` with point on path ot `None` otherwise. 1071 | pub fn point_on_path(path: &[NavVec3], mut s: Scalar) -> Option { 1072 | match path.len() { 1073 | 0 | 1 => None, 1074 | 2 => Some(NavVec3::unproject( 1075 | path[0], 1076 | path[1], 1077 | s / Self::path_length(path), 1078 | )), 1079 | _ => { 1080 | for pair in path.windows(2) { 1081 | let d = (pair[1] - pair[0]).magnitude(); 1082 | if s <= d { 1083 | return Some(NavVec3::unproject(pair[0], pair[1], s / d)); 1084 | } 1085 | s -= d; 1086 | } 1087 | None 1088 | } 1089 | } 1090 | } 1091 | 1092 | /// Calculate path length. 1093 | /// 1094 | /// # Arguments 1095 | /// * `path` - path points. 1096 | /// 1097 | /// # Returns 1098 | /// Path length. 1099 | pub fn path_length(path: &[NavVec3]) -> Scalar { 1100 | match path.len() { 1101 | 0 | 1 => 0.0, 1102 | 2 => (path[1] - path[0]).magnitude(), 1103 | _ => path 1104 | .windows(2) 1105 | .fold(0.0, |a, pair| a + (pair[1] - pair[0]).magnitude()), 1106 | } 1107 | } 1108 | 1109 | fn project_on_line(from: NavVec3, to: NavVec3, point: NavVec3) -> Scalar { 1110 | let d = (to - from).magnitude(); 1111 | let p = point.project(from, to); 1112 | d * p 1113 | } 1114 | 1115 | fn point_on_line(from: NavVec3, to: NavVec3, point: NavVec3) -> (NavVec3, Scalar) { 1116 | let d = (to - from).magnitude(); 1117 | let p = point.project(from, to); 1118 | if p <= 0.0 { 1119 | (from, 0.0) 1120 | } else if p >= 1.0 { 1121 | (to, d) 1122 | } else { 1123 | (NavVec3::unproject(from, to, p), p * d) 1124 | } 1125 | } 1126 | } 1127 | -------------------------------------------------------------------------------- /src/nav_net.rs: -------------------------------------------------------------------------------- 1 | use crate::{Error, NavConnection, NavResult, NavVec3, Scalar}; 2 | use petgraph::{ 3 | algo::{astar, tarjan_scc}, 4 | graph::NodeIndex, 5 | visit::EdgeRef, 6 | Graph, Undirected, 7 | }; 8 | #[cfg(feature = "parallel")] 9 | use rayon::prelude::*; 10 | use serde::{Deserialize, Serialize}; 11 | use spade::{rtree::RTree, BoundingRect, SpatialObject}; 12 | use std::collections::HashMap; 13 | #[cfg(not(feature = "scalar64"))] 14 | use std::f32::MAX as SCALAR_MAX; 15 | #[cfg(feature = "scalar64")] 16 | use std::f64::MAX as SCALAR_MAX; 17 | use typid::ID; 18 | 19 | #[cfg(feature = "parallel")] 20 | macro_rules! iter { 21 | ($v:expr) => { 22 | $v.par_iter() 23 | }; 24 | } 25 | #[cfg(not(feature = "parallel"))] 26 | macro_rules! iter { 27 | ($v:expr) => { 28 | $v.iter() 29 | }; 30 | } 31 | 32 | #[derive(Debug, Clone, Serialize, Deserialize)] 33 | pub struct NavSpatialConnection { 34 | pub connection: NavConnection, 35 | pub index: usize, 36 | pub a: NavVec3, 37 | pub b: NavVec3, 38 | } 39 | 40 | impl NavSpatialConnection { 41 | pub fn new(connection: NavConnection, index: usize, a: NavVec3, b: NavVec3) -> Self { 42 | Self { 43 | connection, 44 | index, 45 | a, 46 | b, 47 | } 48 | } 49 | 50 | pub fn closest_point(&self, point: NavVec3) -> NavVec3 { 51 | let t = point.project(self.a, self.b); 52 | NavVec3::unproject(self.a, self.b, t) 53 | } 54 | } 55 | 56 | impl SpatialObject for NavSpatialConnection { 57 | type Point = NavVec3; 58 | 59 | fn mbr(&self) -> BoundingRect { 60 | let min = NavVec3::new( 61 | self.a.x.min(self.b.x), 62 | self.a.y.min(self.b.y), 63 | self.a.z.min(self.b.z), 64 | ); 65 | let max = NavVec3::new( 66 | self.a.x.max(self.b.x), 67 | self.a.y.max(self.b.y), 68 | self.a.z.max(self.b.z), 69 | ); 70 | BoundingRect::from_corners(&min, &max) 71 | } 72 | 73 | fn distance2(&self, point: &Self::Point) -> Scalar { 74 | (*point - self.closest_point(*point)).sqr_magnitude() 75 | } 76 | } 77 | 78 | /// Nav net identifier. 79 | pub type NavNetID = ID; 80 | 81 | #[derive(Debug, Default, Clone, Serialize, Deserialize)] 82 | pub struct NavNet { 83 | id: NavNetID, 84 | vertices: Vec, 85 | connections: Vec, 86 | distances: Vec, 87 | costs: Vec, 88 | graph: Graph<(), Scalar, Undirected>, 89 | nodes: Vec, 90 | nodes_map: HashMap, 91 | rtree: RTree, 92 | spatials: Vec, 93 | origin: NavVec3, 94 | } 95 | 96 | impl NavNet { 97 | pub fn new(vertices: Vec, connections: Vec) -> NavResult { 98 | let origin = vertices 99 | .iter() 100 | .cloned() 101 | .fold(NavVec3::default(), |a, v| a + v) 102 | / vertices.len() as Scalar; 103 | 104 | let distances = iter!(connections) 105 | .enumerate() 106 | .map(|(i, c)| { 107 | if c.0 as usize >= vertices.len() { 108 | return Err(Error::ConnectionVerticeIndexOutOfBounds(i as u32, 0, c.0)); 109 | } 110 | if c.1 as usize >= vertices.len() { 111 | return Err(Error::ConnectionVerticeIndexOutOfBounds(i as u32, 1, c.1)); 112 | } 113 | let a = vertices[c.0 as usize]; 114 | let b = vertices[c.1 as usize]; 115 | Ok((b - a).sqr_magnitude()) 116 | }) 117 | .collect::>>()?; 118 | 119 | let costs = vec![1.0; vertices.len()]; 120 | 121 | let mut graph = Graph::<(), Scalar, Undirected>::new_undirected(); 122 | let nodes = (0..vertices.len()) 123 | .map(|_| graph.add_node(())) 124 | .collect::>(); 125 | graph.extend_with_edges( 126 | iter!(connections) 127 | .enumerate() 128 | .map(|(i, conn)| (nodes[conn.0 as usize], nodes[conn.1 as usize], distances[i])) 129 | .collect::>(), 130 | ); 131 | let nodes_map = iter!(nodes).enumerate().map(|(i, n)| (*n, i)).collect(); 132 | 133 | let spatials = iter!(connections) 134 | .enumerate() 135 | .map(|(i, connection)| { 136 | NavSpatialConnection::new( 137 | *connection, 138 | i, 139 | vertices[connection.0 as usize], 140 | vertices[connection.1 as usize], 141 | ) 142 | }) 143 | .collect::>(); 144 | 145 | let mut rtree = RTree::new(); 146 | for spatial in &spatials { 147 | rtree.insert(spatial.clone()); 148 | } 149 | 150 | Ok(Self { 151 | id: ID::default(), 152 | vertices, 153 | connections, 154 | distances, 155 | costs, 156 | graph, 157 | nodes, 158 | nodes_map, 159 | rtree, 160 | spatials, 161 | origin, 162 | }) 163 | } 164 | 165 | pub fn scale(&self, value: NavVec3, origin: Option) -> NavResult { 166 | let origin = origin.unwrap_or(self.origin); 167 | let vertices = iter!(self.vertices) 168 | .map(|v| (*v - origin) * value + origin) 169 | .collect::>(); 170 | Self::new(vertices, self.connections.clone()) 171 | } 172 | 173 | #[inline] 174 | pub fn id(&self) -> NavNetID { 175 | self.id 176 | } 177 | 178 | #[inline] 179 | pub fn origin(&self) -> NavVec3 { 180 | self.origin 181 | } 182 | 183 | #[inline] 184 | pub fn vertices(&self) -> &[NavVec3] { 185 | &self.vertices 186 | } 187 | 188 | #[inline] 189 | pub fn connections(&self) -> &[NavConnection] { 190 | &self.connections 191 | } 192 | 193 | #[inline] 194 | pub fn distances(&self) -> &[Scalar] { 195 | &self.distances 196 | } 197 | 198 | #[inline] 199 | pub fn vertices_costs(&self) -> &[Scalar] { 200 | &self.costs 201 | } 202 | 203 | #[inline] 204 | pub fn set_vertice_cost(&mut self, index: usize, cost: Scalar) -> Option { 205 | let c = self.costs.get_mut(index)?; 206 | let old = *c; 207 | *c = cost.max(0.0); 208 | Some(old) 209 | } 210 | 211 | pub fn closest_point(&self, point: NavVec3) -> Option { 212 | let index = self.find_closest_connection(point)?; 213 | Some(self.spatials[index].closest_point(point)) 214 | } 215 | 216 | pub fn find_closest_connection(&self, point: NavVec3) -> Option { 217 | self.rtree.nearest_neighbor(&point).map(|c| c.index) 218 | } 219 | 220 | pub fn find_path(&self, from: NavVec3, to: NavVec3) -> Option> { 221 | self.find_path_custom(from, to, |_, _, _| true) 222 | } 223 | 224 | // filter params: connection distance sqr, first vertex index, second vertex index. 225 | pub fn find_path_custom( 226 | &self, 227 | from: NavVec3, 228 | to: NavVec3, 229 | mut filter: F, 230 | ) -> Option> 231 | where 232 | F: FnMut(Scalar, usize, usize) -> bool, 233 | { 234 | let start_index = self.find_closest_connection(from)?; 235 | let end_index = self.find_closest_connection(to)?; 236 | let start_connection = self.connections[start_index]; 237 | let end_connection = self.connections[end_index]; 238 | let start_point = self.spatials[start_index].closest_point(from); 239 | let end_point = self.spatials[end_index].closest_point(to); 240 | if start_index == end_index { 241 | return Some(vec![start_point, end_point]); 242 | } else if start_point.same_as(end_point) { 243 | return Some(vec![start_point]); 244 | } 245 | let start_vertice = { 246 | let a = self.vertices[start_connection.0 as usize]; 247 | let b = self.vertices[start_connection.1 as usize]; 248 | if (a - start_point).sqr_magnitude() < (b - start_point).sqr_magnitude() { 249 | start_connection.0 as usize 250 | } else { 251 | start_connection.1 as usize 252 | } 253 | }; 254 | let end_vertice = { 255 | let a = self.vertices[end_connection.0 as usize]; 256 | let b = self.vertices[end_connection.1 as usize]; 257 | if (a - end_point).sqr_magnitude() < (b - end_point).sqr_magnitude() { 258 | end_connection.0 as usize 259 | } else { 260 | end_connection.1 as usize 261 | } 262 | }; 263 | let start_node = *self.nodes.get(start_vertice)?; 264 | let end_node = *self.nodes.get(end_vertice)?; 265 | let nodes = astar( 266 | &self.graph, 267 | start_node, 268 | |n| n == end_node, 269 | |e| { 270 | let a = self.nodes_map[&e.source()]; 271 | let b = self.nodes_map[&e.target()]; 272 | let w = *e.weight(); 273 | if filter(w, a, b) { 274 | let a = self.costs[a]; 275 | let b = self.costs[b]; 276 | w * a * b 277 | } else { 278 | SCALAR_MAX 279 | } 280 | }, 281 | |_| 0.0, 282 | )? 283 | .1; 284 | let mut points = nodes 285 | .into_iter() 286 | .map(|n| self.vertices[self.nodes_map[&n]]) 287 | .collect::>(); 288 | if points.len() > 2 { 289 | { 290 | let mut iter = points.iter(); 291 | let a = *iter.next()?; 292 | let b = *iter.next()?; 293 | let t = start_point.project(a, b); 294 | if (0.0..=1.0).contains(&t) { 295 | points[0] = start_point; 296 | } else { 297 | points.insert(0, start_point); 298 | } 299 | } 300 | { 301 | let mut iter = points.iter().rev(); 302 | let a = *iter.next()?; 303 | let b = *iter.next()?; 304 | let t = end_point.project(a, b); 305 | if (0.0..=1.0).contains(&t) { 306 | *points.last_mut()? = end_point; 307 | } else { 308 | points.push(end_point); 309 | } 310 | } 311 | } 312 | Some(points) 313 | } 314 | 315 | pub fn find_islands(&self) -> Vec> { 316 | tarjan_scc(&self.graph) 317 | .into_iter() 318 | .map(|v| { 319 | v.into_iter() 320 | .filter_map(|n| self.nodes_map.get(&n).map(|i| self.vertices[*i])) 321 | .collect::>() 322 | }) 323 | .filter(|v| !v.is_empty()) 324 | .collect() 325 | } 326 | } 327 | -------------------------------------------------------------------------------- /src/nav_vec3.rs: -------------------------------------------------------------------------------- 1 | use crate::{Scalar, ZERO_TRESHOLD}; 2 | use approx::{AbsDiffEq, RelativeEq, UlpsEq}; 3 | use serde::{Deserialize, Serialize}; 4 | use spade::PointN; 5 | use std::ops::{Add, Div, Mul, Neg, Sub}; 6 | 7 | #[repr(C)] 8 | #[derive(Debug, Default, Copy, Clone, PartialEq, Serialize, Deserialize)] 9 | pub struct NavVec3 { 10 | pub x: Scalar, 11 | pub y: Scalar, 12 | pub z: Scalar, 13 | } 14 | 15 | impl NavVec3 { 16 | #[inline] 17 | pub fn new(x: Scalar, y: Scalar, z: Scalar) -> Self { 18 | Self { x, y, z } 19 | } 20 | 21 | #[inline] 22 | pub fn sqr_magnitude(self) -> Scalar { 23 | self.x * self.x + self.y * self.y + self.z * self.z 24 | } 25 | 26 | #[inline] 27 | pub fn magnitude(self) -> Scalar { 28 | self.sqr_magnitude().sqrt() 29 | } 30 | 31 | #[inline] 32 | pub fn same_as(self, other: Self) -> bool { 33 | (other - self).sqr_magnitude() < ZERO_TRESHOLD 34 | } 35 | 36 | #[inline] 37 | pub fn cross(self, other: Self) -> Self { 38 | Self { 39 | x: (self.y * other.z) - (self.z * other.y), 40 | y: (self.z * other.x) - (self.x * other.z), 41 | z: (self.x * other.y) - (self.y * other.x), 42 | } 43 | } 44 | 45 | #[inline] 46 | pub fn dot(self, other: Self) -> Scalar { 47 | self.x * other.x + self.y * other.y + self.z * other.z 48 | } 49 | 50 | #[inline] 51 | pub fn normalize(self) -> Self { 52 | let len = self.magnitude(); 53 | if len < ZERO_TRESHOLD { 54 | Self::new(0.0, 0.0, 0.0) 55 | } else { 56 | Self::new(self.x / len, self.y / len, self.z / len) 57 | } 58 | } 59 | 60 | #[inline] 61 | pub fn abs(self) -> Self { 62 | Self::new(self.x.abs(), self.y.abs(), self.z.abs()) 63 | } 64 | 65 | #[inline] 66 | pub fn lerp(self, other: Self, factor: Scalar) -> Self { 67 | self + (other - self) * factor 68 | } 69 | 70 | #[inline] 71 | pub fn project(self, from: Self, to: Self) -> Scalar { 72 | let diff = to - from; 73 | (self - from).dot(diff) / diff.sqr_magnitude() 74 | } 75 | 76 | #[inline] 77 | pub fn unproject(from: Self, to: Self, t: Scalar) -> Self { 78 | let diff = to - from; 79 | from + Self::new(diff.x * t, diff.y * t, diff.z * t) 80 | } 81 | 82 | #[inline] 83 | pub fn min(self, other: Self) -> Self { 84 | Self::new( 85 | self.x.min(other.x), 86 | self.y.min(other.y), 87 | self.z.min(other.z), 88 | ) 89 | } 90 | 91 | #[inline] 92 | pub fn max(self, other: Self) -> Self { 93 | Self::new( 94 | self.x.max(other.x), 95 | self.y.max(other.y), 96 | self.z.max(other.z), 97 | ) 98 | } 99 | 100 | #[inline] 101 | pub fn distance_to_plane(self, origin: Self, normal: Self) -> Scalar { 102 | normal.dot(self - origin) 103 | } 104 | 105 | #[inline] 106 | pub fn is_above_plane(self, origin: Self, normal: Self) -> bool { 107 | self.distance_to_plane(origin, normal) > -ZERO_TRESHOLD 108 | } 109 | 110 | pub fn project_on_plane(self, origin: Self, normal: Self) -> Self { 111 | let v = self - origin; 112 | let n = normal.normalize(); 113 | let dot = v.dot(n); 114 | let d = NavVec3::new(normal.x * dot, normal.y * dot, normal.z * dot); 115 | self - d 116 | } 117 | 118 | pub fn raycast_plane(from: Self, to: Self, origin: Self, normal: Self) -> Option { 119 | let dir = (to - from).normalize(); 120 | let denom = normal.dot(dir); 121 | if denom.abs() > ZERO_TRESHOLD { 122 | let t = (origin - from).dot(normal) / denom; 123 | if t >= 0.0 && t <= (to - from).magnitude() { 124 | return Some(from + dir * t); 125 | } 126 | } 127 | None 128 | } 129 | 130 | pub fn raycast_line(from: Self, to: Self, a: Self, b: Self, normal: Self) -> Option { 131 | let p = Self::raycast_plane(from, to, a, normal)?; 132 | let t = p.project(a, b).max(0.0).min(1.0); 133 | Some(Self::unproject(a, b, t)) 134 | } 135 | 136 | pub fn raycast_line_exact( 137 | from: Self, 138 | to: Self, 139 | a: Self, 140 | b: Self, 141 | normal: Self, 142 | ) -> Option { 143 | let p = Self::raycast_plane(from, to, a, normal)?; 144 | let t = p.project(a, b); 145 | if (0.0..=1.0).contains(&t) { 146 | Some(Self::unproject(a, b, t)) 147 | } else { 148 | None 149 | } 150 | } 151 | 152 | pub fn raycast_triangle(from: Self, to: Self, a: Self, b: Self, c: Self) -> Option { 153 | let tab = (b - a).normalize(); 154 | let tbc = (c - b).normalize(); 155 | let tca = (a - c).normalize(); 156 | let n = tab.cross(tbc).normalize(); 157 | let contact = Self::raycast_plane(from, to, a, n)?; 158 | let nab = tab.cross(n); 159 | let nbc = tbc.cross(n); 160 | let nca = tca.cross(n); 161 | if contact.is_above_plane(a, -nab) 162 | && contact.is_above_plane(b, -nbc) 163 | && contact.is_above_plane(c, -nca) 164 | { 165 | Some(contact) 166 | } else { 167 | None 168 | } 169 | } 170 | 171 | /// line: (origin, normal) 172 | pub fn planes_intersection(p1: Self, n1: Self, p2: Self, n2: Self) -> Option<(Self, Self)> { 173 | let u = n1.cross(n2); 174 | if u.sqr_magnitude() < ZERO_TRESHOLD { 175 | return None; 176 | } 177 | let a = u.abs(); 178 | let mc = if a.x > a.y { 179 | if a.x > a.z { 180 | 1 181 | } else { 182 | 3 183 | } 184 | } else if a.y > a.z { 185 | 2 186 | } else { 187 | 3 188 | }; 189 | let d1 = -n1.dot(p1); 190 | let d2 = -n2.dot(p2); 191 | let p = match mc { 192 | 1 => Some(Self::new( 193 | 0.0, 194 | (d2 * n1.z - d1 * n2.z) / u.x, 195 | (d1 * n2.y - d2 * n1.y) / u.x, 196 | )), 197 | 2 => Some(Self::new( 198 | (d1 * n2.z - d2 * n1.z) / u.y, 199 | 0.0, 200 | (d2 * n1.x - d1 * n2.x) / u.y, 201 | )), 202 | 3 => Some(Self::new( 203 | (d2 * n1.y - d1 * n2.y) / u.z, 204 | (d1 * n2.x - d2 * n1.x) / u.z, 205 | 0.0, 206 | )), 207 | _ => None, 208 | }?; 209 | Some((p, u.normalize())) 210 | } 211 | 212 | pub fn is_line_between_points(from: Self, to: Self, a: Self, b: Self, normal: Self) -> bool { 213 | let n = (to - from).cross(normal); 214 | let sa = Self::side(n.dot(a - from)); 215 | let sb = Self::side(n.dot(b - from)); 216 | sa != sb 217 | } 218 | 219 | fn side(v: Scalar) -> i8 { 220 | if v.abs() < ZERO_TRESHOLD { 221 | 0 222 | } else { 223 | v.signum() as i8 224 | } 225 | } 226 | } 227 | 228 | impl From<(Scalar, Scalar, Scalar)> for NavVec3 { 229 | fn from(value: (Scalar, Scalar, Scalar)) -> Self { 230 | Self { 231 | x: value.0, 232 | y: value.1, 233 | z: value.2, 234 | } 235 | } 236 | } 237 | 238 | impl From<(Scalar, Scalar)> for NavVec3 { 239 | fn from(value: (Scalar, Scalar)) -> Self { 240 | Self { 241 | x: value.0, 242 | y: value.1, 243 | z: 0.0, 244 | } 245 | } 246 | } 247 | 248 | impl From<[Scalar; 3]> for NavVec3 { 249 | fn from(value: [Scalar; 3]) -> Self { 250 | Self { 251 | x: value[0], 252 | y: value[1], 253 | z: value[2], 254 | } 255 | } 256 | } 257 | 258 | impl From<[Scalar; 2]> for NavVec3 { 259 | fn from(value: [Scalar; 2]) -> Self { 260 | Self { 261 | x: value[0], 262 | y: value[1], 263 | z: 0.0, 264 | } 265 | } 266 | } 267 | 268 | impl Add for NavVec3 { 269 | type Output = Self; 270 | 271 | #[inline] 272 | fn add(self, other: Self) -> Self { 273 | Self { 274 | x: self.x + other.x, 275 | y: self.y + other.y, 276 | z: self.z + other.z, 277 | } 278 | } 279 | } 280 | 281 | impl Add for NavVec3 { 282 | type Output = Self; 283 | 284 | #[inline] 285 | fn add(self, other: Scalar) -> Self { 286 | Self { 287 | x: self.x + other, 288 | y: self.y + other, 289 | z: self.z + other, 290 | } 291 | } 292 | } 293 | 294 | impl Sub for NavVec3 { 295 | type Output = Self; 296 | 297 | #[inline] 298 | fn sub(self, other: Self) -> Self { 299 | Self { 300 | x: self.x - other.x, 301 | y: self.y - other.y, 302 | z: self.z - other.z, 303 | } 304 | } 305 | } 306 | 307 | impl Sub for NavVec3 { 308 | type Output = Self; 309 | 310 | #[inline] 311 | fn sub(self, other: Scalar) -> Self { 312 | Self { 313 | x: self.x - other, 314 | y: self.y - other, 315 | z: self.z - other, 316 | } 317 | } 318 | } 319 | 320 | impl Mul for NavVec3 { 321 | type Output = Self; 322 | 323 | #[inline] 324 | fn mul(self, other: Self) -> Self { 325 | Self { 326 | x: self.x * other.x, 327 | y: self.y * other.y, 328 | z: self.z * other.z, 329 | } 330 | } 331 | } 332 | 333 | impl Mul for NavVec3 { 334 | type Output = Self; 335 | 336 | #[inline] 337 | fn mul(self, other: Scalar) -> Self { 338 | Self { 339 | x: self.x * other, 340 | y: self.y * other, 341 | z: self.z * other, 342 | } 343 | } 344 | } 345 | 346 | impl Div for NavVec3 { 347 | type Output = Self; 348 | 349 | #[inline] 350 | fn div(self, other: Self) -> Self { 351 | Self { 352 | x: self.x / other.x, 353 | y: self.y / other.y, 354 | z: self.z / other.z, 355 | } 356 | } 357 | } 358 | 359 | impl Div for NavVec3 { 360 | type Output = Self; 361 | 362 | #[inline] 363 | fn div(self, other: Scalar) -> Self { 364 | Self { 365 | x: self.x / other, 366 | y: self.y / other, 367 | z: self.z / other, 368 | } 369 | } 370 | } 371 | 372 | impl Neg for NavVec3 { 373 | type Output = Self; 374 | 375 | #[inline] 376 | fn neg(self) -> Self { 377 | Self { 378 | x: -self.x, 379 | y: -self.y, 380 | z: -self.z, 381 | } 382 | } 383 | } 384 | 385 | impl PointN for NavVec3 { 386 | type Scalar = Scalar; 387 | 388 | fn dimensions() -> usize { 389 | 3 390 | } 391 | 392 | fn nth(&self, index: usize) -> &Self::Scalar { 393 | match index { 394 | 0 => &self.x, 395 | 1 => &self.y, 396 | 2 => &self.z, 397 | _ => unreachable!(), 398 | } 399 | } 400 | fn nth_mut(&mut self, index: usize) -> &mut Self::Scalar { 401 | match index { 402 | 0 => &mut self.x, 403 | 1 => &mut self.y, 404 | 2 => &mut self.z, 405 | _ => unreachable!(), 406 | } 407 | } 408 | 409 | fn from_value(value: Self::Scalar) -> Self { 410 | NavVec3::new(value, value, value) 411 | } 412 | } 413 | 414 | impl AbsDiffEq for NavVec3 { 415 | type Epsilon = ::Epsilon; 416 | 417 | fn default_epsilon() -> Self::Epsilon { 418 | Scalar::default_epsilon() 419 | } 420 | 421 | fn abs_diff_eq(&self, other: &Self, epsilon: Self::Epsilon) -> bool { 422 | Scalar::abs_diff_eq(&self.x, &other.x, epsilon) 423 | && Scalar::abs_diff_eq(&self.y, &other.y, epsilon) 424 | && Scalar::abs_diff_eq(&self.z, &other.z, epsilon) 425 | } 426 | } 427 | 428 | impl RelativeEq for NavVec3 { 429 | fn default_max_relative() -> Self::Epsilon { 430 | Scalar::default_max_relative() 431 | } 432 | 433 | fn relative_eq( 434 | &self, 435 | other: &Self, 436 | epsilon: Self::Epsilon, 437 | max_relative: Self::Epsilon, 438 | ) -> bool { 439 | Scalar::relative_eq(&self.x, &other.x, epsilon, max_relative) 440 | && Scalar::relative_eq(&self.y, &other.y, epsilon, max_relative) 441 | && Scalar::relative_eq(&self.z, &other.z, epsilon, max_relative) 442 | } 443 | } 444 | 445 | impl UlpsEq for NavVec3 { 446 | fn default_max_ulps() -> u32 { 447 | Scalar::default_max_ulps() 448 | } 449 | 450 | fn ulps_eq(&self, other: &Self, epsilon: Self::Epsilon, max_ulps: u32) -> bool { 451 | Scalar::ulps_eq(&self.x, &other.x, epsilon, max_ulps) 452 | && Scalar::ulps_eq(&self.y, &other.y, epsilon, max_ulps) 453 | && Scalar::ulps_eq(&self.z, &other.z, epsilon, max_ulps) 454 | } 455 | } 456 | 457 | #[cfg(feature = "mint")] 458 | impl From> for NavVec3 { 459 | fn from(v: mint::Vector3) -> Self { 460 | Self::new(v.x, v.y, v.z) 461 | } 462 | } 463 | 464 | #[cfg(feature = "mint")] 465 | impl From for mint::Vector3 { 466 | fn from(v: NavVec3) -> Self { 467 | Self { 468 | x: v.x, 469 | y: v.y, 470 | z: v.z, 471 | } 472 | } 473 | } 474 | 475 | #[cfg(feature = "mint")] 476 | #[cfg(test)] 477 | mod tests { 478 | use super::*; 479 | 480 | #[test] 481 | fn test_mint() { 482 | let v = NavVec3::new(0.0, 1.0, -1.0); 483 | let _f = mint::Vector3::::from(v); 484 | let _t: mint::Vector3 = v.into(); 485 | 486 | let v = mint::Vector3:: { 487 | x: 0.0, 488 | y: 1.0, 489 | z: -1.0, 490 | }; 491 | let _f = NavVec3::from(v); 492 | let _t: NavVec3 = v.into(); 493 | } 494 | } 495 | --------------------------------------------------------------------------------