├── .github ├── dependabot.yml └── workflows │ └── rust.yml ├── .gitignore ├── Cargo.toml ├── README.md ├── benches └── bench.rs └── src ├── cache.rs ├── chunk.rs ├── cubemap.rs ├── lib.rs └── parry.rs /.github/dependabot.yml: -------------------------------------------------------------------------------- 1 | version: 2 2 | updates: 3 | - package-ecosystem: cargo 4 | directory: "/" 5 | schedule: 6 | interval: daily 7 | open-pull-requests-limit: 10 8 | -------------------------------------------------------------------------------- /.github/workflows/rust.yml: -------------------------------------------------------------------------------- 1 | name: CI 2 | 3 | on: 4 | push: 5 | branches: ['master'] 6 | pull_request: 7 | 8 | jobs: 9 | test: 10 | runs-on: ubuntu-latest 11 | steps: 12 | - uses: actions/checkout@v1 13 | - uses: actions-rs/toolchain@v1 14 | with: 15 | profile: minimal 16 | toolchain: stable 17 | override: true 18 | - uses: actions-rs/cargo@v1 19 | with: 20 | command: build 21 | args: --workspace --all-targets --all-features 22 | - uses: actions-rs/cargo@v1 23 | with: 24 | command: test 25 | args: --workspace --all-features 26 | 27 | lint: 28 | runs-on: ubuntu-latest 29 | steps: 30 | - uses: actions/checkout@v1 31 | - uses: actions-rs/toolchain@v1 32 | with: 33 | profile: minimal 34 | toolchain: stable 35 | override: true 36 | components: rustfmt, clippy 37 | - uses: actions-rs/cargo@v1 38 | with: 39 | command: fmt 40 | args: --all -- --check 41 | - uses: actions-rs/cargo@v1 42 | if: always() 43 | with: 44 | command: clippy 45 | args: --workspace --all-targets -- -D warnings 46 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | /target 2 | **/*.rs.bk 3 | Cargo.lock 4 | -------------------------------------------------------------------------------- /Cargo.toml: -------------------------------------------------------------------------------- 1 | [package] 2 | name = "planetmap" 3 | version = "0.1.0" 4 | authors = ["Benjamin Saunders "] 5 | license = "MIT/Apache-2.0" 6 | edition = "2021" 7 | readme = "README.md" 8 | 9 | [package.metadata.docs.rs] 10 | all-features = true 11 | 12 | [features] 13 | parry = ["parry3d-f64", "lru-slab"] 14 | 15 | [dependencies] 16 | na = { package = "nalgebra", version = "0.33" } 17 | slab = "0.4.2" 18 | hashbrown = "0.15" 19 | parry3d-f64 = { version = "0.17.0", optional = true } 20 | num-traits = "0.2.19" 21 | lru-slab = { version = "0.1.1", optional = true } 22 | 23 | [dev-dependencies] 24 | criterion = "0.5.1" 25 | approx = "0.5" 26 | simba = { version = "0.9", features = ["wide"] } 27 | 28 | [[bench]] 29 | name = "bench" 30 | harness = false 31 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # Planetmap 2 | 3 | Planetmap is a library for processing lazily evaluated radial 4 | functions with dynamic level-of-detail. 5 | 6 | ## Screenshots 7 | 8 | ![high orbit](https://user-images.githubusercontent.com/3484507/65808207-c6e2ce80-e18a-11e9-9625-6ad85e5d54d5.png) 9 | ![low orbit](https://user-images.githubusercontent.com/3484507/65808209-c77b6500-e18a-11e9-84d3-9efa44f2f902.png) 10 | ![close](https://user-images.githubusercontent.com/3484507/65808208-c6e2ce80-e18a-11e9-83e1-100f0863473d.png) 11 | 12 | ## Terrain Rendering 13 | 14 | Planetmap is motivated by the desire to model to-scale planetary 15 | terrain in real time on commodity hardware. To accomplish this, it 16 | addresses multiple challenges: 17 | 18 | - Dynamic level of detail 19 | - Streaming larger-than-memory or procedural heightmap data 20 | - Addressing the surface of the sphere without singularities 21 | - Maintaining numerical stability over massive distances 22 | 23 | This is accomplished by organizing data into a 6 virtual quadtrees, 24 | each corresponding to one face of a cubemap. Each node in a quadtree, 25 | referred to as a `Chunk`, can be associated with a square grid of 26 | height samples, each of which represents the altitude relative to sea 27 | level along a ray originating at the center of a planet. Streaming and 28 | level of detail can then be implemented with a `CacheManager` 29 | associated with GPU-resident buffers. 30 | 31 | When generating geometry to be displaced by the heightmap, care must 32 | be taken to ensure numerical stability and avoid hairline cracks 33 | between neighboring chunks. One effective tactic is to upload the 34 | corners of each chunk from the CPU, then find interior points using 35 | interpolation in a vertex shader. 36 | 37 | ## Features 38 | 39 | - `parry` exposes helpers for rapier-compatible collision 40 | detection against the surface of a radial heightmap 41 | - `simd` exposes `simdeez`-based iterators for data-parallel 42 | computation of sample coordinates within a `Chunk` or 43 | `chunk::Coord`, useful for fast noise sampling 44 | -------------------------------------------------------------------------------- /benches/bench.rs: -------------------------------------------------------------------------------- 1 | use criterion::{criterion_group, criterion_main, Criterion}; 2 | #[cfg(feature = "parry")] 3 | use parry3d_f64::{ 4 | query::{PointQuery, QueryDispatcher, Ray, RayCast}, 5 | shape::Ball, 6 | }; 7 | 8 | #[cfg(feature = "parry")] 9 | use planetmap::parry::{FlatTerrain, Planet, PlanetDispatcher}; 10 | use planetmap::*; 11 | 12 | #[cfg(feature = "parry")] 13 | criterion_group!(benches, cache, face_coords, collision); 14 | #[cfg(not(feature = "parry"))] 15 | criterion_group!(benches, cache, face_coords); 16 | criterion_main!(benches); 17 | 18 | fn face_coords(c: &mut Criterion) { 19 | c.bench_function("get vector coordinates on face", |b| { 20 | b.iter(|| { 21 | let v = na::Vector3::new(0.42, 0.17, 0.12); 22 | criterion::black_box(cubemap::Face::coords(&v)); 23 | }); 24 | }); 25 | } 26 | 27 | fn cache(c: &mut Criterion) { 28 | let mut mgr = CacheManager::new(2048, Default::default()); 29 | let mut transfers = Vec::with_capacity(2048); 30 | mgr.update(&[na::Point3::from(na::Vector3::z())], &mut transfers); 31 | resolve_transfers(&mut mgr, &mut transfers); 32 | c.bench_function("best-case update", move |b| { 33 | let mut transfers = Vec::with_capacity(2048); 34 | b.iter(|| mgr.update(&[na::Point3::from(na::Vector3::z())], &mut transfers)); 35 | }); 36 | 37 | let mut mgr = CacheManager::new(2048, Default::default()); 38 | mgr.update(&[na::Point3::from(na::Vector3::z())], &mut transfers); 39 | resolve_transfers(&mut mgr, &mut transfers); 40 | let diagonal1 = na::Point3::from(-na::Vector3::new(1.0, 1.0, 1.0).normalize()); 41 | let diagonal2 = na::Point3::from(na::Vector3::new(1.0, 1.0, 1.0).normalize()); 42 | c.bench_function("worst-case update", move |b| { 43 | let mut which = true; 44 | b.iter(|| { 45 | if which { 46 | mgr.update(&[diagonal1], &mut transfers); 47 | } else { 48 | mgr.update(&[diagonal2], &mut transfers); 49 | } 50 | resolve_transfers(&mut mgr, &mut transfers); 51 | which = !which; 52 | }); 53 | }); 54 | } 55 | 56 | fn resolve_transfers(mgr: &mut CacheManager, transfers: &mut Vec) { 57 | for &mut transfer in transfers { 58 | let slot = mgr.allocate(transfer).unwrap(); 59 | mgr.release(slot); 60 | } 61 | } 62 | 63 | #[cfg(feature = "parry")] 64 | fn collision(c: &mut Criterion) { 65 | use std::sync::Arc; 66 | 67 | const PLANET_RADIUS: f64 = 6371e3; 68 | let ball = Ball { radius: 1.0 }; 69 | let planet = Planet::new( 70 | Arc::new(FlatTerrain::new(2u32.pow(12), 17)), 71 | 32, 72 | PLANET_RADIUS, 73 | ); 74 | 75 | c.bench_function("intersect", |b| { 76 | b.iter(|| { 77 | assert!(PlanetDispatcher 78 | .intersection_test( 79 | &na::Isometry3::translation(PLANET_RADIUS, 0.0, 0.0), 80 | &planet, 81 | &ball, 82 | ) 83 | .unwrap()); 84 | }); 85 | }); 86 | 87 | c.bench_function("short raycast", |b| { 88 | b.iter(|| { 89 | planet.cast_local_ray( 90 | &Ray { 91 | origin: na::Point3::new(PLANET_RADIUS + 1.0, 0.0, 0.0), 92 | dir: na::Vector3::y(), 93 | }, 94 | 1e1, 95 | true, 96 | ); 97 | }); 98 | }); 99 | 100 | c.bench_function("long raycast", |b| { 101 | b.iter(|| { 102 | planet.cast_local_ray( 103 | &Ray { 104 | origin: na::Point3::new(PLANET_RADIUS + 1.0, 0.0, 0.0), 105 | dir: na::Vector3::y(), 106 | }, 107 | 1e3, 108 | true, 109 | ); 110 | }); 111 | }); 112 | 113 | c.bench_function("project point", |b| { 114 | b.iter(|| { 115 | planet.project_point( 116 | &na::Isometry3::identity(), 117 | &na::Point3::new(PLANET_RADIUS + 1.0, 0.0, 0.0), 118 | true, 119 | ) 120 | }); 121 | }); 122 | } 123 | -------------------------------------------------------------------------------- /src/cache.rs: -------------------------------------------------------------------------------- 1 | use hashbrown::HashMap; 2 | use std::ops::{Index, IndexMut}; 3 | 4 | use slab::Slab; 5 | 6 | use crate::cubemap::{Edge, Face}; 7 | use crate::Chunk; 8 | 9 | struct Slot { 10 | chunk: Chunk, 11 | /// Whether the slot is ready for reading 12 | ready: bool, 13 | /// Sequence number of the most recent frame this slot was rendered in 14 | in_frame: u64, 15 | } 16 | 17 | #[derive(Debug, Clone)] 18 | pub struct Config { 19 | /// Maximum depth of quad-tree traversal 20 | pub max_depth: u8, 21 | /// Upper bound for `Neighborhood` fields 22 | /// 23 | /// Should be set to the base 2 logarithm of the number of quads along the edge of a chunk to 24 | /// reduce stitching artifacts across extreme LoD boundaries. 25 | pub max_neighbor_delta: u8, 26 | } 27 | 28 | impl Default for Config { 29 | fn default() -> Self { 30 | Self { 31 | max_depth: 12, 32 | max_neighbor_delta: u8::MAX, 33 | } 34 | } 35 | } 36 | 37 | impl Config { 38 | /// Number of slots needed by this config to represent maximum detail for a worst-case viewpoint 39 | pub fn slots_needed(&self) -> usize { 40 | Face::iter() 41 | .map(Chunk::root) 42 | .map(|x| self.slots_needed_inner(&x)) 43 | .sum() 44 | } 45 | 46 | fn slots_needed_inner(&self, chunk: &Chunk) -> usize { 47 | let viewpoint = na::Point3::from(na::Vector3::new(1.0, 1.0, 1.0).normalize()); 48 | if chunk.depth == self.max_depth || !needs_subdivision(chunk, &viewpoint) { 49 | return 1; 50 | } 51 | chunk 52 | .children() 53 | .iter() 54 | .map(|x| self.slots_needed_inner(x)) 55 | .sum::() 56 | + 1 57 | } 58 | } 59 | 60 | /// Helper for streaming `Chunk`-oriented LoD into a fixed-size cache 61 | pub struct Manager { 62 | chunks: Slab, 63 | index: HashMap, 64 | config: Config, 65 | /// Frame counter used to update `Slot.in_frame` 66 | frame: u64, 67 | render: Vec<(Chunk, Neighborhood, u32)>, 68 | } 69 | 70 | impl Manager { 71 | /// Create a manager for a cache of `slots` slots 72 | pub fn new(slots: usize, config: Config) -> Self { 73 | Self { 74 | chunks: Slab::with_capacity(slots), 75 | index: HashMap::with_capacity(slots), 76 | config, 77 | frame: 0, 78 | render: Vec::with_capacity(slots), 79 | } 80 | } 81 | 82 | /// Compute slots to render for a given set of viewpoints, and to load for improved detail in 83 | /// future passes. 84 | /// 85 | /// Viewpoints should be positioned with regard to the sphere's origin, and scaled such 86 | /// viewpoints on the surface are 1.0 units from the sphere's origin. 87 | /// 88 | /// Writes unavailable chunks needed for target quality to `transfer`. 89 | pub fn update(&mut self, viewpoints: &[na::Point3], transfer: &mut Vec) { 90 | transfer.clear(); 91 | self.render.clear(); 92 | self.walk(viewpoints, transfer); 93 | 94 | // Make room for transfers by discarding chunks that we don't currently need. 95 | let mut available = self.chunks.capacity() - self.chunks.len(); 96 | for idx in 0..self.chunks.capacity() { 97 | if available >= transfer.len() { 98 | break; 99 | } 100 | if !self.chunks.contains(idx) 101 | || !self.chunks[idx].ready 102 | || self.chunks[idx].in_frame == self.frame 103 | { 104 | continue; 105 | } 106 | let old = self.chunks.remove(idx); 107 | self.index.remove(&old.chunk); 108 | available += 1; 109 | } 110 | transfer.truncate(available); 111 | self.frame += 1; 112 | } 113 | 114 | /// Chunks that can be rendered immediately, with their LoD neighborhood and slot index 115 | #[inline] 116 | pub fn renderable(&self) -> &[(Chunk, Neighborhood, u32)] { 117 | &self.render 118 | } 119 | 120 | /// Allocate a slot for writing 121 | /// 122 | /// Determines where in the cache to transfer chunk data to. Returns `None` if the cache is 123 | /// full. 124 | pub fn allocate(&mut self, chunk: Chunk) -> Option { 125 | if self.chunks.len() == self.chunks.capacity() { 126 | return None; 127 | } 128 | let slot = self.chunks.insert(Slot { 129 | chunk, 130 | ready: false, 131 | in_frame: 0, 132 | }) as u32; 133 | let old = self.index.insert(chunk, slot); 134 | debug_assert!( 135 | old.is_none(), 136 | "a slot has already been allocated for this chunk" 137 | ); 138 | Some(slot) 139 | } 140 | 141 | /// Indicate that a previously `allocate`d slot can now be safely read or reused 142 | pub fn release(&mut self, slot: u32) { 143 | debug_assert!( 144 | !self.chunks[slot as usize].ready, 145 | "slot must be allocated to be released" 146 | ); 147 | self.chunks[slot as usize].ready = true; 148 | } 149 | 150 | fn get(&self, chunk: &Chunk) -> Option { 151 | self.index.get(chunk).cloned() 152 | } 153 | 154 | fn walk(&mut self, viewpoints: &[na::Point3], transfers: &mut Vec) { 155 | // Gather the set of chunks we can should render and want to transfer 156 | for chunk in Face::iter().map(Chunk::root) { 157 | let slot = self.get(&chunk); 158 | // Kick off the loop for each face's quadtree 159 | self.walk_inner( 160 | viewpoints, 161 | transfers, 162 | ChunkState { 163 | chunk, 164 | slot, 165 | renderable: slot.map_or(false, |idx| self.chunks[idx as usize].ready), 166 | }, 167 | ); 168 | } 169 | 170 | // Compute the neighborhood of each rendered chunk. 171 | for &mut (chunk, ref mut neighborhood, _) in &mut self.render { 172 | for (edge, neighbor) in Edge::iter().zip(chunk.neighbors().iter()) { 173 | for x in neighbor.path() { 174 | let slot = match self.index.get(&x) { 175 | None => continue, 176 | Some(&x) => x, 177 | }; 178 | let state = &self.chunks[slot as usize]; 179 | if state.ready && state.in_frame == self.frame { 180 | neighborhood[edge] = 181 | (chunk.depth - x.depth).min(self.config.max_neighbor_delta); 182 | break; 183 | } 184 | } 185 | } 186 | } 187 | } 188 | 189 | /// Walk the quadtree below `chunk`, recording chunks to render and transfer. 190 | fn walk_inner( 191 | &mut self, 192 | viewpoints: &[na::Point3], 193 | transfers: &mut Vec, 194 | chunk: ChunkState, 195 | ) { 196 | // If this chunk is already associated with a cache slot, preserve that slot; otherwise, 197 | // tell the caller we want it. 198 | if let Some(idx) = chunk.slot { 199 | self.chunks[idx as usize].in_frame = self.frame; 200 | } else { 201 | transfers.push(chunk.chunk); 202 | } 203 | 204 | let subdivide = chunk.chunk.depth < self.config.max_depth 205 | && viewpoints 206 | .iter() 207 | .any(|v| needs_subdivision(&chunk.chunk, v)); 208 | if !subdivide { 209 | if chunk.renderable { 210 | self.render 211 | .push((chunk.chunk, Neighborhood::default(), chunk.slot.unwrap())); 212 | } 213 | return; 214 | } 215 | 216 | let children = chunk.chunk.children(); 217 | let child_slots = [ 218 | self.get(&children[Edge::Nx]), 219 | self.get(&children[Edge::Ny]), 220 | self.get(&children[Edge::Px]), 221 | self.get(&children[Edge::Py]), 222 | ]; 223 | // The children of this chunk might be rendered if: 224 | let children_renderable = chunk.renderable // this subtree should be rendered at all, and 225 | && child_slots // every child is already resident in the cache 226 | .iter() 227 | .all(|slot| slot.map_or(false, |x| self.chunks[x as usize].ready)); 228 | // If this subtree should be rendered and the children can't be rendered, this chunk must be rendered. 229 | if chunk.renderable && !children_renderable { 230 | self.render 231 | .push((chunk.chunk, Neighborhood::default(), chunk.slot.unwrap())); 232 | } 233 | // Recurse into the children 234 | for (&child, &slot) in children.iter().zip(child_slots.iter()) { 235 | self.walk_inner( 236 | viewpoints, 237 | transfers, 238 | ChunkState { 239 | chunk: child, 240 | renderable: children_renderable, 241 | slot, 242 | }, 243 | ); 244 | } 245 | } 246 | } 247 | 248 | /// For each edge of a particular chunk, this represents the number of LoD levels higher that chunk 249 | /// is than its neighbor on that edge. 250 | /// 251 | /// Smoothly interpolating across chunk boundaries requires careful attention to these values. In 252 | /// particular, any visualization of chunk data should take care to be continuous at the edge with 253 | /// regard to an adjacent lower-detail level if discontinuities are undesirable. For example, 254 | /// terrain using heightmapped tiles should weld together a subset of the vertices on an edge shared 255 | /// with a lower-detail chunk. 256 | /// 257 | /// Note that increases in LoD are not represented here; it is always the responsibility of the 258 | /// higher-detail chunk to account for neighboring lower-detail chunks. 259 | #[derive(Debug, Copy, Clone, Eq, PartialEq, Default)] 260 | pub struct Neighborhood { 261 | /// Decrease in LoD in the local -X direction 262 | pub nx: u8, 263 | /// Decrease in LoD in the local -Y direction 264 | pub ny: u8, 265 | /// Decrease in LoD in the local +X direction 266 | pub px: u8, 267 | /// Decrease in LoD in the local +Y direction 268 | pub py: u8, 269 | } 270 | 271 | impl Index for Neighborhood { 272 | type Output = u8; 273 | fn index(&self, edge: Edge) -> &u8 { 274 | use Edge::*; 275 | match edge { 276 | Nx => &self.nx, 277 | Ny => &self.ny, 278 | Px => &self.px, 279 | Py => &self.py, 280 | } 281 | } 282 | } 283 | 284 | impl IndexMut for Neighborhood { 285 | fn index_mut(&mut self, edge: Edge) -> &mut u8 { 286 | use Edge::*; 287 | match edge { 288 | Nx => &mut self.nx, 289 | Ny => &mut self.ny, 290 | Px => &mut self.px, 291 | Py => &mut self.py, 292 | } 293 | } 294 | } 295 | 296 | struct ChunkState { 297 | chunk: Chunk, 298 | /// Cache slot associated with this chunk, whether or not it's ready 299 | slot: Option, 300 | /// Whether the subtree at this chunk will be rendered 301 | renderable: bool, 302 | } 303 | 304 | fn needs_subdivision(chunk: &Chunk, viewpoint: &na::Point3) -> bool { 305 | // Half-angle of the cone whose edges are simultaneously tangent to the edges of a pair of 306 | // circles inscribed on a chunk at depth D and a neighbor of that chunk at depth D+1. Setting a 307 | // threshold larger than this leads to LoD deltas greater than 1 across edges. 308 | let max_half_angle = 1.0f64.atan2(10.0f64.sqrt()); 309 | 310 | let center = na::Point3::from(chunk.origin().into_inner()); 311 | if center.coords.dot(&viewpoint.coords) < 0.0 { 312 | return false; 313 | } 314 | let distance = na::distance(¢er, viewpoint); 315 | // Half-angle of the circular cone from the camera containing an inscribed sphere 316 | let half_angle = (chunk.edge_length::() * 0.5).atan2(distance); 317 | half_angle >= max_half_angle 318 | } 319 | 320 | #[cfg(test)] 321 | mod test { 322 | use super::*; 323 | 324 | #[test] 325 | fn transfer_completeness() { 326 | let mut mgr = Manager::new(2048, Config::default()); 327 | let mut transfers = Vec::new(); 328 | mgr.update(&[na::Point3::from(na::Vector3::z())], &mut transfers); 329 | assert_eq!(mgr.renderable().len(), 0); 330 | for &transfer in &transfers { 331 | let slot = mgr.allocate(transfer).unwrap(); 332 | mgr.release(slot); 333 | } 334 | mgr.update(&[na::Point3::from(na::Vector3::z())], &mut transfers); 335 | assert_eq!(transfers.len(), 0); 336 | assert_ne!(mgr.renderable().len(), 0); 337 | } 338 | 339 | #[test] 340 | fn slots_needed() { 341 | let viewpoint = na::Point3::from(na::Vector3::new(1.0, 1.0, 1.0).normalize()); 342 | for max_depth in 0..12 { 343 | let config = Config { 344 | max_depth, 345 | ..Config::default() 346 | }; 347 | let needed = config.slots_needed(); 348 | let mut mgr = Manager::new(2048, config); 349 | let mut transfers = Vec::new(); 350 | mgr.update(&[viewpoint], &mut transfers); 351 | assert_eq!(transfers.len(), needed); 352 | } 353 | } 354 | 355 | #[test] 356 | fn neighborhood() { 357 | use crate::cubemap::Coords; 358 | let mut mgr = Manager::new(2048, Config::default()); 359 | let viewpoint = na::Point3::from(na::Vector3::new(1.0, 1.0, 1.0).normalize()); 360 | let mut transfers = Vec::new(); 361 | mgr.update(&[viewpoint], &mut transfers); 362 | assert_eq!(mgr.renderable().len(), 0); 363 | // Get +X to LoD 1, +Y to LoD 0 364 | for &chunk in &[ 365 | Chunk { 366 | coords: Coords { 367 | x: 0, 368 | y: 0, 369 | face: Face::Px, 370 | }, 371 | depth: 0, 372 | }, 373 | Chunk { 374 | coords: Coords { 375 | x: 0, 376 | y: 0, 377 | face: Face::Py, 378 | }, 379 | depth: 0, 380 | }, 381 | Chunk { 382 | coords: Coords { 383 | x: 0, 384 | y: 0, 385 | face: Face::Px, 386 | }, 387 | depth: 1, 388 | }, 389 | Chunk { 390 | coords: Coords { 391 | x: 0, 392 | y: 1, 393 | face: Face::Px, 394 | }, 395 | depth: 1, 396 | }, 397 | Chunk { 398 | coords: Coords { 399 | x: 1, 400 | y: 0, 401 | face: Face::Px, 402 | }, 403 | depth: 1, 404 | }, 405 | Chunk { 406 | coords: Coords { 407 | x: 1, 408 | y: 1, 409 | face: Face::Px, 410 | }, 411 | depth: 1, 412 | }, 413 | ] { 414 | assert!(transfers.contains(&chunk)); 415 | let slot = mgr.allocate(chunk).unwrap(); 416 | mgr.release(slot); 417 | } 418 | 419 | // Validate output 420 | let mut transfers = Vec::new(); 421 | mgr.update(&[viewpoint], &mut transfers); 422 | assert_eq!(mgr.renderable().len(), 5); 423 | use std::collections::HashMap; 424 | let neighbors = mgr 425 | .renderable() 426 | .iter() 427 | .map(|&(chunk, neighbors, _slot)| (chunk, neighbors)) 428 | .collect::>(); 429 | let neighborhood = *neighbors 430 | .get(&Chunk { 431 | coords: Coords { 432 | x: 0, 433 | y: 0, 434 | face: Face::Px, 435 | }, 436 | depth: 1, 437 | }) 438 | .unwrap(); 439 | assert_ne!( 440 | neighborhood, 441 | Neighborhood { 442 | nx: 0, 443 | ny: 0, 444 | px: 0, 445 | py: 0 446 | } 447 | ); 448 | } 449 | 450 | #[test] 451 | fn incremental_transfer() { 452 | let mut mgr = Manager::new(2048, Config::default()); 453 | let viewpoint = na::Point3::from(na::Vector3::new(0.0, 1.0, 0.0)); 454 | let mut transfers = Vec::new(); 455 | mgr.update(&[viewpoint], &mut transfers); 456 | let expected = transfers.len(); 457 | const BATCH_SIZE: usize = 17; 458 | let mut actual = 0; 459 | while !transfers.is_empty() { 460 | for &chunk in transfers.iter().take(BATCH_SIZE) { 461 | actual += 1; 462 | let slot = mgr.allocate(chunk).unwrap(); 463 | mgr.release(slot); 464 | } 465 | transfers.clear(); 466 | mgr.update(&[viewpoint], &mut transfers); 467 | assert_eq!(transfers.len(), expected - actual); 468 | } 469 | } 470 | 471 | #[test] 472 | fn subdivision_sanity() { 473 | let chunk = Chunk { 474 | depth: 10, 475 | coords: crate::cubemap::Coords { 476 | x: 12, 477 | y: 34, 478 | face: Face::Px, 479 | }, 480 | }; 481 | // Verify that a chunk containing the viewpoint gets subdivided 482 | assert!(needs_subdivision( 483 | &chunk, 484 | &na::Point3::from(chunk.direction(&[0.5; 2].into()).into_inner()) 485 | )); 486 | } 487 | } 488 | -------------------------------------------------------------------------------- /src/chunk.rs: -------------------------------------------------------------------------------- 1 | use na::{RealField, SimdRealField}; 2 | 3 | use crate::cubemap::SampleIterSimd; 4 | use crate::cubemap::{Coords, Edge, Face, SampleIter}; 5 | 6 | /// A node of a quadtree on a particular cubemap face 7 | #[derive(Debug, Copy, Clone, Eq, PartialEq, Hash)] 8 | pub struct Chunk { 9 | /// Coordinates within the set of nodes at this depth in the quadtree 10 | pub coords: Coords, 11 | /// Depth in the quadtree 12 | pub depth: u8, 13 | } 14 | 15 | impl Chunk { 16 | /// The top-level chunk corresponding to a particular cubemap face 17 | pub fn root(face: Face) -> Self { 18 | Self { 19 | coords: Coords { x: 0, y: 0, face }, 20 | depth: 0, 21 | } 22 | } 23 | 24 | /// Compute the chunk at `depth` that intersects the vector from the origin towards `dir` 25 | pub fn from_vector(depth: u8, dir: &na::Vector3) -> Self { 26 | Self { 27 | coords: Coords::from_vector(2u32.pow(depth as u32), dir), 28 | depth, 29 | } 30 | } 31 | 32 | /// Select all `Chunk`s of a certain depth intersecting a cone opening towards `direction` with 33 | /// half-angle `theta` 34 | pub fn neighborhood( 35 | depth: u8, 36 | direction: na::Vector3, 37 | theta: f32, 38 | ) -> impl Iterator { 39 | Coords::neighborhood(2u32.pow(depth.into()), direction, theta) 40 | .map(move |coords| Self { coords, depth }) 41 | } 42 | 43 | /// The smallest chunk that contains this chunk 44 | pub fn parent(&self) -> Option { 45 | let depth = self.depth.checked_sub(1)?; 46 | Some(Self { 47 | coords: Coords { 48 | x: self.coords.x / 2, 49 | y: self.coords.y / 2, 50 | face: self.coords.face, 51 | }, 52 | depth, 53 | }) 54 | } 55 | 56 | /// Iterator over the path from this chunk to the root, inclusive 57 | pub fn path(self) -> Path { 58 | Path { chunk: Some(self) } 59 | } 60 | 61 | /// The largest chunks contained by this chunk 62 | pub fn children(&self) -> [Self; 4] { 63 | let depth = self.depth + 1; 64 | let (x, y) = (self.coords.x * 2, self.coords.y * 2); 65 | let face = self.coords.face; 66 | [ 67 | Chunk { 68 | coords: Coords { x, y, face }, 69 | depth, 70 | }, 71 | Chunk { 72 | coords: Coords { x, y: y + 1, face }, 73 | depth, 74 | }, 75 | Chunk { 76 | coords: Coords { x: x + 1, y, face }, 77 | depth, 78 | }, 79 | Chunk { 80 | coords: Coords { 81 | x: x + 1, 82 | y: y + 1, 83 | face, 84 | }, 85 | depth, 86 | }, 87 | ] 88 | } 89 | 90 | /// Chunks that share an edge with this chunk 91 | pub fn neighbors(&self) -> [Self; 4] { 92 | let x = self.coords.neighbors(self.resolution()); 93 | let depth = self.depth; 94 | [ 95 | Chunk { 96 | coords: x[Edge::Nx], 97 | depth, 98 | }, 99 | Chunk { 100 | coords: x[Edge::Ny], 101 | depth, 102 | }, 103 | Chunk { 104 | coords: x[Edge::Px], 105 | depth, 106 | }, 107 | Chunk { 108 | coords: x[Edge::Py], 109 | depth, 110 | }, 111 | ] 112 | } 113 | 114 | /// Length of one of this chunk's edges before mapping onto the sphere 115 | pub fn edge_length(&self) -> N { 116 | Coords::edge_length(self.resolution()) 117 | } 118 | 119 | /// Location of the center of this chunk on the surface of the sphere 120 | /// 121 | /// Shorthand for `direction(&[0.5; 2].into())`. 122 | pub fn origin(&self) -> na::Unit> { 123 | let size = self.edge_length::(); 124 | let vec = na::Vector2::new(self.coords.x, self.coords.y) 125 | .map(f64::from) 126 | .cast::() 127 | .map(|x| (x + na::convert(0.5)) * size - na::convert(1.0)) 128 | .map(crate::cubemap::warp); 129 | self.coords.face.basis() * na::Unit::new_normalize(vec.push(na::convert(1.0))) 130 | } 131 | 132 | /// Returns a grid of resolution^2 directions contained by the chunk, in scan-line order 133 | pub fn samples(&self, resolution: u32) -> SampleIter { 134 | self.coords.samples(self.resolution(), resolution) 135 | } 136 | 137 | /// Returns a grid of resolution^2 directions contained by the chunk, in scan-line order 138 | /// 139 | /// Because this returns data in batches of `S::VF32_WIDTH`, a few excess values will be 140 | /// computed at the end for any `resolution` whose square is not a multiple of the batch size. 141 | pub fn samples_ps(&self, resolution: u32) -> SampleIterSimd 142 | where 143 | S: SimdRealField + Copy, 144 | S::Element: RealField + Copy, 145 | { 146 | self.coords.samples_ps(self.resolution(), resolution) 147 | } 148 | 149 | /// Compute the direction identified by a [0..1]^2 vector on this chunk 150 | pub fn direction( 151 | &self, 152 | coords: &na::Point2, 153 | ) -> na::Unit> { 154 | self.coords.direction(self.resolution(), coords) 155 | } 156 | 157 | /// Number of chunks at this depth along a cubemap edge, i.e. 2^depth 158 | pub fn resolution(&self) -> u32 { 159 | 2u32.pow(self.depth as u32) 160 | } 161 | } 162 | 163 | pub struct Path { 164 | chunk: Option, 165 | } 166 | 167 | impl Iterator for Path { 168 | type Item = Chunk; 169 | fn next(&mut self) -> Option { 170 | let chunk = self.chunk?; 171 | self.chunk = chunk.parent(); 172 | Some(chunk) 173 | } 174 | 175 | fn size_hint(&self) -> (usize, Option) { 176 | (self.len(), Some(self.len())) 177 | } 178 | } 179 | 180 | impl ExactSizeIterator for Path { 181 | fn len(&self) -> usize { 182 | self.chunk.map_or(0, |x| x.depth as usize + 1) 183 | } 184 | } 185 | 186 | #[cfg(test)] 187 | mod test { 188 | use super::*; 189 | use approx::*; 190 | use na::SimdValue; 191 | 192 | #[test] 193 | fn neighbors() { 194 | use self::Face::*; 195 | // Every face chunk's neighbors are the face's neighbors 196 | for face in Face::iter() { 197 | for (&(neighbor_face, _, _), &neighbor_chunk) in 198 | face.neighbors().iter().zip(&Chunk::root(face).neighbors()) 199 | { 200 | assert_eq!(Chunk::root(neighbor_face), neighbor_chunk); 201 | } 202 | } 203 | 204 | let chunk = Chunk::root(Face::Pz).children()[0]; 205 | assert_eq!( 206 | chunk.neighbors()[0], 207 | Chunk { 208 | coords: Coords { 209 | x: 0, 210 | y: 1, 211 | face: Nx 212 | }, 213 | depth: 1, 214 | } 215 | ); 216 | assert_eq!( 217 | chunk.neighbors()[1], 218 | Chunk { 219 | coords: Coords { 220 | x: 0, 221 | y: 1, 222 | face: Ny 223 | }, 224 | depth: 1, 225 | } 226 | ); 227 | assert_eq!( 228 | chunk.neighbors()[2], 229 | Chunk { 230 | coords: Coords { 231 | x: 1, 232 | y: 0, 233 | face: Pz 234 | }, 235 | depth: 1, 236 | } 237 | ); 238 | assert_eq!( 239 | chunk.neighbors()[3], 240 | Chunk { 241 | coords: Coords { 242 | x: 0, 243 | y: 1, 244 | face: Pz 245 | }, 246 | depth: 1, 247 | } 248 | ); 249 | 250 | let chunk = Chunk { 251 | depth: 1, 252 | coords: Coords { 253 | x: 1, 254 | y: 0, 255 | face: Px, 256 | }, 257 | }; 258 | assert_eq!(chunk.neighbors()[0b01].neighbors()[0b10], chunk); 259 | 260 | // Every chunk is its neighbor's neighbor 261 | // Depth 2 to ensure we get all the interesting cases 262 | for grandparent in Face::iter().map(Chunk::root) { 263 | for parent in grandparent.children().iter() { 264 | for chunk in parent.children().iter() { 265 | for &neighbor in chunk.neighbors().iter() { 266 | assert!(neighbor.neighbors().iter().any(|x| x == chunk)); 267 | } 268 | } 269 | } 270 | } 271 | } 272 | 273 | #[test] 274 | fn parents() { 275 | for grandparent in Face::iter().map(Chunk::root) { 276 | for &parent in grandparent.children().iter() { 277 | assert_eq!(parent.parent(), Some(grandparent)); 278 | for chunk in parent.children().iter() { 279 | assert_eq!(chunk.parent(), Some(parent)); 280 | } 281 | } 282 | } 283 | } 284 | 285 | #[test] 286 | fn sample_count() { 287 | let chunk = Chunk::root(Face::Pz); 288 | for i in 0..4 { 289 | assert_eq!(chunk.samples(i).count(), (i * i) as usize); 290 | } 291 | } 292 | 293 | #[test] 294 | fn sample_sanity() { 295 | assert_eq!( 296 | Chunk::root(Face::Pz).samples(1).next().unwrap(), 297 | na::Vector3::z_axis() 298 | ); 299 | 300 | let chunk = Chunk::root(Face::Pz).children()[1]; 301 | assert!(chunk.samples(2).any(|x| x == na::Vector3::z_axis())); 302 | 303 | // Every face's 304 | for face in Face::iter() { 305 | // immediate children 306 | for chunk in &Chunk::root(face).children() { 307 | // each have one sample at exactly the center of the face 308 | assert_eq!( 309 | chunk 310 | .samples(2) 311 | .filter(|&x| x == face.basis() * na::Vector3::z_axis()) 312 | .count(), 313 | 1 314 | ); 315 | // and another at a corner 316 | let corner = 1.0 / 3.0f32.sqrt(); 317 | assert_eq!( 318 | chunk 319 | .samples(2) 320 | .filter(|&x| abs_diff_eq!(x.x.abs(), corner) 321 | && abs_diff_eq!(x.y.abs(), corner) 322 | && abs_diff_eq!(x.z.abs(), corner)) 323 | .count(), 324 | 1 325 | ); 326 | } 327 | } 328 | } 329 | 330 | #[test] 331 | fn sample_lod_boundaries() { 332 | let chunk = Chunk { 333 | depth: 10, 334 | coords: Coords { 335 | x: 12, 336 | y: 34, 337 | face: Face::Pz, 338 | }, 339 | }; 340 | let children = chunk.children(); 341 | let neighbor = chunk.neighbors()[0]; 342 | assert_eq!( 343 | children[0] 344 | .samples(5) 345 | .map(|child_vert| neighbor 346 | .samples(5) 347 | .filter(|&neighbor_vert| neighbor_vert == child_vert) 348 | .count()) 349 | .sum::(), 350 | 3 351 | ); 352 | } 353 | 354 | #[test] 355 | fn origin_sanity() { 356 | assert_eq!(Chunk::root(Face::Pz).origin::(), na::Vector3::z_axis()); 357 | 358 | let chunk = Chunk { 359 | coords: Coords { 360 | x: 12, 361 | y: 47, 362 | face: Face::Px, 363 | }, 364 | depth: 12, 365 | }; 366 | assert_eq!( 367 | chunk, 368 | Chunk::from_vector(chunk.depth, chunk.origin().as_ref()) 369 | ); 370 | 371 | assert_abs_diff_eq!(chunk.origin(), chunk.direction(&na::Point2::new(0.5, 0.5))); 372 | } 373 | 374 | #[test] 375 | fn simd_samples() { 376 | type S = simba::simd::WideF32x4; 377 | 378 | let chunk = Chunk::root(Face::Pz); 379 | 380 | let mut samples = chunk.samples(5); 381 | for coords in chunk.samples_ps::(5) { 382 | let [x, y, z] = coords.map(<[f32; S::LANES]>::from); 383 | for i in 0..S::LANES { 384 | let reference = if let Some(v) = samples.next() { 385 | v 386 | } else { 387 | break; 388 | }; 389 | assert_relative_eq!(x[i], reference.x, epsilon = 1e-4); 390 | assert_relative_eq!(y[i], reference.y, epsilon = 1e-4); 391 | assert_relative_eq!(z[i], reference.z, epsilon = 1e-4); 392 | } 393 | } 394 | } 395 | 396 | #[test] 397 | fn path() { 398 | let leaf = Chunk { 399 | coords: Coords { 400 | x: 1, 401 | y: 1, 402 | face: Face::Py, 403 | }, 404 | depth: 1, 405 | }; 406 | assert_eq!( 407 | leaf.path().collect::>(), 408 | [leaf, leaf.parent().unwrap()] 409 | ); 410 | } 411 | } 412 | -------------------------------------------------------------------------------- /src/cubemap.rs: -------------------------------------------------------------------------------- 1 | use std::cmp::Ordering; 2 | use std::marker::PhantomData; 3 | use std::ops::{Index, IndexMut, Neg}; 4 | use std::{alloc, fmt, mem, ptr}; 5 | 6 | use na::{ComplexField, RealField, SimdRealField}; 7 | use num_traits::identities::One; 8 | 9 | /// A dense, fixed-resolution, warped cube map 10 | /// 11 | /// Useful for storing and manipulating moderate-resolution samplings of radial functions such as 12 | /// spherical heightmaps. Warped to improve spacing of slots compared to a naive cube map, based on 13 | /// the 5th order polynomial method described in "Cube-to-sphere Projections for Procedural 14 | /// Texturing and Beyond" (Zucker, Higashi). 15 | /// 16 | /// For addressing purposes, slots along the edges and at the corners of a face do *not* overlap 17 | /// with their neighbors. Note that `Coords::samples` nonetheless *does* produce samples that will 18 | /// overlap along the edges of neighboring `Coords`. 19 | #[derive(Debug)] 20 | #[repr(C)] 21 | pub struct CubeMap { 22 | resolution: u32, 23 | data: [T], 24 | } 25 | 26 | impl CubeMap { 27 | /// Access a cubemap in existing memory 28 | /// 29 | /// # Safety 30 | /// 31 | /// `ptr` must address a `u32` containing the cubemap's resolution, positioned just before the 32 | /// cubemap's data. The addressed memory must outlive `'a` and have no outstanding unique 33 | /// borrows. 34 | pub unsafe fn from_raw<'a>(ptr: *const u32) -> &'a Self { 35 | let dim = ptr.read() as usize; 36 | let len = dim * dim * 6; 37 | &*(ptr::slice_from_raw_parts(ptr, len) as *const Self) 38 | } 39 | 40 | /// Uniquely access a cubemap in existing memory 41 | /// 42 | /// # Safety 43 | /// 44 | /// `ptr` must address a `u32` containing the cubemap's resolution, positioned just before the 45 | /// cubemap's data. The addressed memory must outlive `'a` and have no outstanding borrows. 46 | pub unsafe fn from_raw_mut<'a>(ptr: *mut u32) -> &'a mut Self { 47 | let dim = ptr.read() as usize; 48 | let len = dim * dim * 6; 49 | &mut *(ptr::slice_from_raw_parts_mut(ptr, len) as *mut Self) 50 | } 51 | 52 | /// Construct a cube map with faces containing `resolution * resolution` slots, each initialized 53 | /// to `value`. 54 | pub fn new(resolution: u32, value: T) -> Box 55 | where 56 | T: Clone, 57 | { 58 | let payload_len = resolution as usize * resolution as usize * 6; 59 | let header_layout = alloc::Layout::new::(); 60 | let (layout, offset) = header_layout 61 | .extend( 62 | alloc::Layout::from_size_align( 63 | mem::size_of::() * payload_len, 64 | mem::align_of::(), 65 | ) 66 | .unwrap(), 67 | ) 68 | .unwrap(); 69 | let layout = layout.pad_to_align(); 70 | unsafe { 71 | let mem = alloc::alloc(layout); 72 | mem.cast::().write(resolution); 73 | let payload = mem.add(offset).cast::(); 74 | for i in 0..payload_len { 75 | payload.add(i).write(value.clone()); 76 | } 77 | Box::from_raw(ptr::slice_from_raw_parts_mut(mem, payload_len) as *mut Self) 78 | } 79 | } 80 | 81 | /// Copy a cube map from a contiguous slice of `resolution` by `resolution` data, in +X, -X, +Y, 82 | /// -Y, +Z, -Z order. 83 | /// 84 | /// Returns `None` if `data.len()` isn't correct for `resolution`, i.e. `resolution * resolution 85 | /// * 6`. 86 | pub fn from_slice(resolution: u32, data: &[T]) -> Option> 87 | where 88 | T: Copy, 89 | { 90 | let payload_len = resolution as usize * resolution as usize * 6; 91 | if data.len() != payload_len { 92 | return None; 93 | } 94 | 95 | let align = mem::align_of::().max(4); // Also the size of the header with padding 96 | let layout = 97 | alloc::Layout::from_size_align(align + mem::size_of::() * payload_len, align) 98 | .unwrap(); 99 | unsafe { 100 | let mem = alloc::alloc(layout); 101 | mem.cast::().write(resolution); 102 | let payload = mem.add(align).cast::(); 103 | for (i, &x) in data.iter().enumerate() { 104 | payload.add(i).write(x); 105 | } 106 | Some(Box::from_raw( 107 | ptr::slice_from_raw_parts_mut(mem, payload_len) as *mut Self, 108 | )) 109 | } 110 | } 111 | 112 | /// Compute a cube map based on the direction of each slot 113 | pub fn from_fn( 114 | resolution: u32, 115 | mut f: impl FnMut(na::Unit>) -> T, 116 | ) -> Box { 117 | let payload_len = resolution as usize * resolution as usize * 6; 118 | let align = mem::align_of::().max(4); // Also the size of the header with padding 119 | let layout = 120 | alloc::Layout::from_size_align(align + mem::size_of::() * payload_len, align) 121 | .unwrap(); 122 | unsafe { 123 | let mem = alloc::alloc(layout); 124 | mem.cast::().write(resolution); 125 | let payload = mem.add(align).cast::(); 126 | for i in 0..payload_len { 127 | payload.add(i).write(f(get_dir(resolution, i).unwrap())); 128 | } 129 | Box::from_raw(ptr::slice_from_raw_parts_mut(mem, payload_len) as *mut Self) 130 | } 131 | } 132 | 133 | pub fn resolution(&self) -> u32 { 134 | self.resolution 135 | } 136 | 137 | pub fn iter(&self) -> Iter { 138 | self.into_iter() 139 | } 140 | 141 | pub fn iter_mut(&mut self) -> IterMut { 142 | self.into_iter() 143 | } 144 | } 145 | 146 | impl AsRef<[T]> for CubeMap { 147 | fn as_ref(&self) -> &[T] { 148 | &self.data 149 | } 150 | } 151 | 152 | impl AsMut<[T]> for CubeMap { 153 | fn as_mut(&mut self) -> &mut [T] { 154 | &mut self.data 155 | } 156 | } 157 | 158 | impl Index for CubeMap { 159 | type Output = [T]; 160 | fn index(&self, face: Face) -> &[T] { 161 | let face_size = self.resolution as usize * self.resolution as usize; 162 | let offset = face_size * face as usize; 163 | &self.data[offset..offset + face_size] 164 | } 165 | } 166 | 167 | impl IndexMut for CubeMap { 168 | fn index_mut(&mut self, face: Face) -> &mut [T] { 169 | let face_size = self.resolution as usize * self.resolution as usize; 170 | let offset = face_size * face as usize; 171 | &mut self.data[offset..offset + face_size] 172 | } 173 | } 174 | 175 | impl Index for CubeMap { 176 | type Output = T; 177 | fn index(&self, coord: Coords) -> &T { 178 | let face_size = self.resolution as usize * self.resolution as usize; 179 | let offset = face_size * coord.face as usize; 180 | &self.data[offset + self.resolution as usize * coord.y as usize + coord.x as usize] 181 | } 182 | } 183 | 184 | impl IndexMut for CubeMap { 185 | fn index_mut(&mut self, coord: Coords) -> &mut T { 186 | let face_size = self.resolution as usize * self.resolution as usize; 187 | let offset = face_size * coord.face as usize; 188 | &mut self.data[offset + self.resolution as usize * coord.y as usize + coord.x as usize] 189 | } 190 | } 191 | 192 | impl<'a, T> Index<&'a na::Vector3> for CubeMap { 193 | type Output = T; 194 | fn index(&self, x: &'a na::Vector3) -> &T { 195 | &self.data[index(self.resolution, x)] 196 | } 197 | } 198 | 199 | impl<'a, T> IndexMut<&'a na::Vector3> for CubeMap { 200 | fn index_mut(&mut self, x: &'a na::Vector3) -> &mut T { 201 | &mut self.data[index(self.resolution, x)] 202 | } 203 | } 204 | 205 | fn index(resolution: u32, x: &na::Vector3) -> usize { 206 | let resolution = resolution as usize; 207 | let (face, coords) = Face::coords(x); 208 | let texcoords = coords * 0.5 + na::Vector2::new(0.5, 0.5); 209 | let texel = discretize(resolution, texcoords); 210 | face as usize * resolution * resolution + texel.1 * resolution + texel.0 211 | } 212 | 213 | impl<'a, T> IntoIterator for &'a CubeMap { 214 | type Item = (na::Unit>, &'a T); 215 | type IntoIter = Iter<'a, T>; 216 | 217 | fn into_iter(self) -> Self::IntoIter { 218 | Iter { 219 | resolution: self.resolution, 220 | data: &self.data, 221 | index: 0, 222 | } 223 | } 224 | } 225 | 226 | pub struct Iter<'a, T> { 227 | resolution: u32, 228 | data: &'a [T], 229 | index: usize, 230 | } 231 | 232 | impl<'a, T> Iterator for Iter<'a, T> { 233 | type Item = (na::Unit>, &'a T); 234 | fn next(&mut self) -> Option { 235 | let dir = get_dir(self.resolution, self.index)?; 236 | let value = &self.data[self.index]; 237 | self.index += 1; 238 | Some((dir, value)) 239 | } 240 | 241 | fn size_hint(&self) -> (usize, Option) { 242 | let total = self.resolution as usize * self.resolution as usize; 243 | let remaining = total - self.index; 244 | (remaining, Some(remaining)) 245 | } 246 | } 247 | 248 | impl ExactSizeIterator for Iter<'_, T> { 249 | fn len(&self) -> usize { 250 | let total = self.resolution as usize * self.resolution as usize; 251 | total - self.index 252 | } 253 | } 254 | 255 | impl<'a, T> IntoIterator for &'a mut CubeMap { 256 | type Item = (na::Unit>, &'a mut T); 257 | type IntoIter = IterMut<'a, T>; 258 | 259 | fn into_iter(self) -> Self::IntoIter { 260 | IterMut { 261 | resolution: self.resolution, 262 | data: &mut self.data, 263 | index: 0, 264 | } 265 | } 266 | } 267 | 268 | pub struct IterMut<'a, T> { 269 | resolution: u32, 270 | data: &'a mut [T], 271 | index: usize, 272 | } 273 | 274 | impl<'a, T> Iterator for IterMut<'a, T> { 275 | type Item = (na::Unit>, &'a mut T); 276 | fn next(&mut self) -> Option { 277 | let dir = get_dir(self.resolution, self.index)?; 278 | let value = &mut self.data[self.index] as *mut T; 279 | self.index += 1; 280 | // We promise calling next twice won't yield the same reference. 281 | Some((dir, unsafe { &mut *value })) 282 | } 283 | 284 | fn size_hint(&self) -> (usize, Option) { 285 | let total = self.resolution as usize * self.resolution as usize; 286 | let remaining = total - self.index; 287 | (remaining, Some(remaining)) 288 | } 289 | } 290 | 291 | impl ExactSizeIterator for IterMut<'_, T> { 292 | fn len(&self) -> usize { 293 | let total = self.resolution as usize * self.resolution as usize; 294 | total - self.index 295 | } 296 | } 297 | 298 | fn get_dir(resolution: u32, index: usize) -> Option>> { 299 | let face_size = resolution as usize * resolution as usize; 300 | if index >= face_size * 6 { 301 | return None; 302 | } 303 | let face = [Face::Px, Face::Nx, Face::Py, Face::Ny, Face::Pz, Face::Nz][index / face_size]; 304 | let rem = index % face_size; 305 | let y = (rem / resolution as usize) as u32; 306 | let x = (rem % resolution as usize) as u32; 307 | Some(Coords { x, y, face }.center(resolution)) 308 | } 309 | 310 | /// Face of a cube map, identified by direction 311 | #[derive(Debug, Copy, Clone, Eq, PartialEq, Hash)] 312 | #[repr(u8)] 313 | pub enum Face { 314 | /// The face in the +X direction 315 | Px, 316 | /// The face in the -X direction 317 | Nx, 318 | /// The face in the +Y direction 319 | Py, 320 | /// The face in the -Y direction 321 | Ny, 322 | /// The face in the +Z direction 323 | Pz, 324 | /// The face in the -Z direction 325 | Nz, 326 | } 327 | 328 | impl fmt::Display for Face { 329 | fn fmt(&self, f: &mut fmt::Formatter) -> fmt::Result { 330 | use self::Face::*; 331 | let s = match *self { 332 | Px => "+X", 333 | Nx => "-X", 334 | Py => "+Y", 335 | Ny => "-Y", 336 | Pz => "+Z", 337 | Nz => "-Z", 338 | }; 339 | f.write_str(s) 340 | } 341 | } 342 | 343 | impl Neg for Face { 344 | type Output = Self; 345 | fn neg(self) -> Self { 346 | use self::Face::*; 347 | match self { 348 | Px => Nx, 349 | Py => Ny, 350 | Pz => Nz, 351 | Nx => Px, 352 | Ny => Py, 353 | Nz => Pz, 354 | } 355 | } 356 | } 357 | 358 | impl Face { 359 | /// Find the face that intersects a vector originating at the center of a cube 360 | pub fn from_vector(x: &na::Vector3) -> Self { 361 | let (&value, &axis) = x 362 | .iter() 363 | .zip(&[Face::Px, Face::Py, Face::Pz]) 364 | .max_by(|(l, _), (r, _)| l.abs().partial_cmp(&r.abs()).unwrap_or(Ordering::Less)) 365 | .unwrap(); 366 | if value.is_sign_negative() { 367 | -axis 368 | } else { 369 | axis 370 | } 371 | } 372 | 373 | /// Compute which `Face` a vector intersects, and where the intersection lies in [-1..1]^2 374 | pub fn coords(x: &na::Vector3) -> (Face, na::Point2) { 375 | let face = Self::from_vector(x); 376 | let wrt_face = face.basis().inverse_transform_vector(x); 377 | ( 378 | face, 379 | na::Point2::from(wrt_face.xy() / wrt_face.z).map(unwarp), 380 | ) 381 | } 382 | 383 | /// Transform from face space (facing +Z) to sphere space (facing the named axis). 384 | pub fn basis(self) -> na::Rotation3 { 385 | use self::Face::*; 386 | let (x, y, z) = match self { 387 | Px => (na::Vector3::z(), -na::Vector3::y(), na::Vector3::x()), 388 | Nx => (-na::Vector3::z(), -na::Vector3::y(), -na::Vector3::x()), 389 | Py => (na::Vector3::x(), -na::Vector3::z(), na::Vector3::y()), 390 | Ny => (na::Vector3::x(), na::Vector3::z(), -na::Vector3::y()), 391 | Pz => (na::Vector3::x(), na::Vector3::y(), na::Vector3::z()), 392 | Nz => (-na::Vector3::x(), na::Vector3::y(), -na::Vector3::z()), 393 | }; 394 | na::Rotation3::from_basis_unchecked(&[x, y, z]) 395 | } 396 | 397 | /// Iterator over all `Face`s 398 | pub fn iter() -> impl Iterator { 399 | [Face::Px, Face::Nx, Face::Py, Face::Ny, Face::Pz, Face::Nz].into_iter() 400 | } 401 | 402 | /// Neighboring faces wrt. local axes [-x, -y, +x, +y]. 403 | /// 404 | /// Returns the neighboring face, the edge of that face, and whether the axis shared with that face is parallel or antiparallel. 405 | /// 406 | /// Index by `sign << 1 | axis`. 407 | pub(crate) fn neighbors(self) -> &'static [(Face, Edge, bool); 4] { 408 | use self::Face::*; 409 | match self { 410 | Px => &[ 411 | (Nz, Edge::Nx, false), 412 | (Py, Edge::Px, false), 413 | (Pz, Edge::Px, false), 414 | (Ny, Edge::Px, true), 415 | ], 416 | Nx => &[ 417 | (Pz, Edge::Nx, false), 418 | (Py, Edge::Nx, true), 419 | (Nz, Edge::Px, false), 420 | (Ny, Edge::Nx, false), 421 | ], 422 | Py => &[ 423 | (Nx, Edge::Ny, true), 424 | (Pz, Edge::Py, true), 425 | (Px, Edge::Ny, false), 426 | (Nz, Edge::Py, false), 427 | ], 428 | Ny => &[ 429 | (Nx, Edge::Py, false), 430 | (Nz, Edge::Ny, false), 431 | (Px, Edge::Py, true), 432 | (Pz, Edge::Ny, true), 433 | ], 434 | Pz => &[ 435 | (Nx, Edge::Nx, false), 436 | (Ny, Edge::Py, true), 437 | (Px, Edge::Px, false), 438 | (Py, Edge::Ny, true), 439 | ], 440 | Nz => &[ 441 | (Px, Edge::Nx, false), 442 | (Ny, Edge::Ny, false), 443 | (Nx, Edge::Px, false), 444 | (Py, Edge::Py, false), 445 | ], 446 | } 447 | } 448 | 449 | /// Compute the direction identified by a [-1..1]^2 vector on this face 450 | pub fn direction( 451 | self, 452 | coords: &na::Point2, 453 | ) -> na::Unit> { 454 | let coords = coords.map(warp); 455 | let dir_z = na::Unit::new_normalize(na::Vector3::new(coords.x, coords.y, N::one())); 456 | self.basis() * dir_z 457 | } 458 | } 459 | 460 | /// Coordinates in a discretized cubemap 461 | #[derive(Debug, Copy, Clone, Eq, PartialEq, Hash)] 462 | pub struct Coords { 463 | pub x: u32, 464 | pub y: u32, 465 | pub face: Face, 466 | } 467 | 468 | impl Coords { 469 | pub fn from_vector(resolution: u32, vector: &na::Vector3) -> Self { 470 | Self::from_vector_with_coords(resolution, vector).0 471 | } 472 | 473 | /// Compute the coordinates of the cell containing `vector`, and its [0..1]^2 coordinates within 474 | /// that cell 475 | pub fn from_vector_with_coords( 476 | resolution: u32, 477 | vector: &na::Vector3, 478 | ) -> (Self, na::Point2) { 479 | let (face, coords) = Face::coords(vector); 480 | let coords = coords * 0.5 + na::Vector2::new(0.5, 0.5); 481 | let (x, y) = discretize(resolution as usize, coords); 482 | ( 483 | Self { 484 | x: x as u32, 485 | y: y as u32, 486 | face, 487 | }, 488 | coords, 489 | ) 490 | } 491 | 492 | pub fn neighbors(&self, resolution: u32) -> [Self; 4] { 493 | let Coords { x, y, face } = *self; 494 | let max = resolution - 1; 495 | let neighbor_chunk = |face: Face, edge: Edge| { 496 | let (neighboring_face, neighbor_edge, parallel_axis) = face.neighbors()[edge]; 497 | let other = match edge { 498 | Edge::Nx | Edge::Px => y, 499 | Edge::Ny | Edge::Py => x, 500 | }; 501 | let other = if parallel_axis { other } else { max - other }; 502 | let (x, y) = match neighbor_edge { 503 | Edge::Nx => (0, other), 504 | Edge::Ny => (other, 0), 505 | Edge::Px => (max, other), 506 | Edge::Py => (other, max), 507 | }; 508 | Coords { 509 | x, 510 | y, 511 | face: neighboring_face, 512 | } 513 | }; 514 | [ 515 | if x == 0 { 516 | neighbor_chunk(face, Edge::Nx) 517 | } else { 518 | Coords { x: x - 1, y, face } 519 | }, 520 | if y == 0 { 521 | neighbor_chunk(face, Edge::Ny) 522 | } else { 523 | Coords { x, y: y - 1, face } 524 | }, 525 | if x == max { 526 | neighbor_chunk(face, Edge::Px) 527 | } else { 528 | Coords { x: x + 1, y, face } 529 | }, 530 | if y == max { 531 | neighbor_chunk(face, Edge::Py) 532 | } else { 533 | Coords { x, y: y + 1, face } 534 | }, 535 | ] 536 | } 537 | 538 | /// Select all `Coords` intersecting a cone opening towards `direction` with half-angle `theta` 539 | pub fn neighborhood( 540 | resolution: u32, 541 | direction: na::Vector3, 542 | theta: f32, 543 | ) -> impl Iterator { 544 | /// Map [-1, 1] to [0, 1] 545 | fn remap(x: f32) -> f32 { 546 | (na::clamp(x, -1.0, 1.0) + 1.0) / 2.0 547 | } 548 | Face::iter() 549 | .filter_map(move |face| { 550 | let basis = face.basis(); 551 | if (basis * na::Vector3::z()).dot(&direction) <= 0.0 { 552 | // TODO: Handle theta ≥ π/2 553 | return None; 554 | } 555 | let local = basis.inverse_transform_vector(&direction); 556 | let rot_x = na::UnitQuaternion::from_axis_angle(&na::Vector3::y_axis(), theta); 557 | let rot_y = na::UnitQuaternion::from_axis_angle(&na::Vector3::x_axis(), -theta); 558 | let coords_with = |r| { 559 | let w: na::Vector3 = r * local; 560 | w.xy() / w.z 561 | }; 562 | let lower_x = coords_with(rot_x.inverse()).x; 563 | if lower_x > 1.0 { 564 | return None; 565 | } 566 | let lower_y = coords_with(rot_y.inverse()).y; 567 | if lower_y > 1.0 { 568 | return None; 569 | } 570 | let lower = na::Vector2::new(lower_x, lower_y); 571 | 572 | let upper_x = coords_with(rot_x).x; 573 | if upper_x < -1.0 { 574 | return None; 575 | } 576 | let upper_y = coords_with(rot_y).y; 577 | if upper_y < -1.0 { 578 | return None; 579 | } 580 | let upper = na::Vector2::new(upper_x, upper_y); 581 | 582 | Some((face, lower.map(unwarp), upper.map(unwarp))) 583 | }) 584 | .flat_map(move |(face, lower, upper)| { 585 | let (x_lower, y_lower) = 586 | discretize(resolution as usize, na::Point2::from(lower.map(remap))); 587 | let (x_upper, y_upper) = 588 | discretize(resolution as usize, na::Point2::from(upper.map(remap))); 589 | (y_lower..=y_upper).flat_map(move |y| { 590 | (x_lower..=x_upper).map(move |x| Self { 591 | x: x as u32, 592 | y: y as u32, 593 | face, 594 | }) 595 | }) 596 | }) 597 | } 598 | 599 | /// The approximate direction represented by these coordinates 600 | pub fn center(&self, resolution: u32) -> na::Unit> { 601 | let texcoord = if resolution == 1 { 602 | na::Point2::new(0.5, 0.5) 603 | } else { 604 | na::Point2::new(self.x as f64, self.y as f64) / ((resolution - 1) as f64) 605 | }; 606 | let on_z = texcoord * 2.0 - na::Vector2::new(1.0, 1.0); 607 | self.face.direction(&on_z.cast()) 608 | } 609 | 610 | /// Compute the direction identified by a point in the [0..1]^2 area covered by these 611 | /// coordinates 612 | pub fn direction( 613 | &self, 614 | resolution: u32, 615 | coords: &na::Point2, 616 | ) -> na::Unit> { 617 | let edge_length = Self::edge_length::(resolution); 618 | let origin_on_face = 619 | na::Point2::from(na::Vector2::new(self.x as f64, self.y as f64).cast() * edge_length) 620 | - na::Vector2::new(1.0, 1.0).cast(); 621 | let pos_on_face = origin_on_face + coords.coords * edge_length; 622 | self.face.direction(&pos_on_face) 623 | } 624 | 625 | /// Length of an edge of the bounding square of the cubemap-space area covered by a coordinate 626 | /// in a cubemap with a particular `resolution` 627 | pub fn edge_length(resolution: u32) -> N { 628 | na::convert::<_, N>(2.0) / na::convert::<_, N>(resolution as f64) 629 | } 630 | 631 | /// Returns a grid of resolution^2 directions contained by these coords, in scan-line order 632 | /// 633 | /// - `face_resolution` represents the number of coordinates along an edge of the cubemap 634 | /// - `chunk_resolution` represents the number of samples along an edge of this specific coordinate 635 | /// 636 | /// Edge/corner samples lie *directly* on the edge/corner, and hence are *not* the centers of 637 | /// traditionally addressed texels. In other words, the first sample has position (0, 0), not 638 | /// (0.5/w, 0.5/h), and the last has position (1, 1), not (1 - 0.5/w, 1 - 0.5/h). This allows 639 | /// for seamless interpolation in the neighborhood of chunk edges/corners without needing access 640 | /// to data for neighboring chunks. 641 | pub fn samples(&self, face_resolution: u32, chunk_resolution: u32) -> SampleIter { 642 | SampleIter { 643 | coords: *self, 644 | face_resolution, 645 | chunk_resolution, 646 | index: 0, 647 | } 648 | } 649 | 650 | /// SIMD variant of `samples`. 651 | /// 652 | /// Because this returns data in batches of `S::VF32_WIDTH`, a few excess values will be 653 | /// computed at the end for any `resolution` whose square is not a multiple of the batch size. 654 | pub fn samples_ps(&self, face_resolution: u32, chunk_resolution: u32) -> SampleIterSimd 655 | where 656 | S: SimdRealField + Copy, 657 | S::Element: RealField + Copy, 658 | { 659 | SampleIterSimd { 660 | coords: *self, 661 | face_resolution, 662 | chunk_resolution, 663 | index: 0, 664 | _simd: PhantomData, 665 | } 666 | } 667 | } 668 | 669 | /// Distort a [-1..1] cube face coordinate such that, when projected onto a sphere, area is close 670 | /// to equal 671 | pub(crate) fn warp(x: N) -> N { 672 | // 5th-order polynomial from "Cube-to-sphere Projections for Procedural Texturing and Beyond" 673 | // (Zucker, Higashi). 674 | let x2 = x * x; 675 | (na::convert::<_, N>(0.745558715593) 676 | + (na::convert::<_, N>(0.130546850193) + na::convert::<_, N>(0.123894434214) * x2) * x2) 677 | * x 678 | } 679 | 680 | /// Derivative of `warp` 681 | fn dwarp(x: N) -> N { 682 | let x2 = x * x; 683 | (na::convert::<_, N>(0.61947217107) * x2 + na::convert::<_, N>(0.391640550579)) * x2 684 | + na::convert::<_, N>(0.745558715593) 685 | } 686 | 687 | /// Inverse of `warp` 688 | pub(crate) fn unwarp(x: N) -> N { 689 | let x2 = x * x; 690 | 691 | // Approximate inverse using a numerically fit curve 692 | let mut estimate = (na::convert::<_, N>(1.34318229552) 693 | + (na::convert::<_, N>(-0.486514066449) + na::convert::<_, N>(0.143331770927) * x2) * x2) 694 | * x; 695 | 696 | // Refine estimate with newton's method 697 | for _ in 0..2 { 698 | estimate -= (warp(estimate) - x) / dwarp(estimate); 699 | } 700 | 701 | estimate 702 | } 703 | 704 | /// Edge of a quad 705 | #[derive(Debug, Copy, Clone, Eq, PartialEq)] 706 | #[repr(u8)] 707 | pub enum Edge { 708 | Nx = 0, 709 | Ny = 1, 710 | Px = 2, 711 | Py = 3, 712 | } 713 | 714 | impl Edge { 715 | /// Iterator over all `Edge`s 716 | pub fn iter() -> impl Iterator { 717 | [Edge::Nx, Edge::Ny, Edge::Px, Edge::Py].into_iter() 718 | } 719 | 720 | /// Unit vector across the edge 721 | pub fn direction(self) -> na::UnitVector2 { 722 | let x = match self { 723 | Edge::Nx => [-1.0, 0.0], 724 | Edge::Ny => [0.0, -1.0], 725 | Edge::Px => [1.0, 0.0], 726 | Edge::Py => [0.0, 1.0], 727 | }; 728 | na::Unit::new_unchecked(na::Vector2::from(x).cast()) 729 | } 730 | } 731 | 732 | impl Neg for Edge { 733 | type Output = Self; 734 | fn neg(self) -> Self { 735 | use self::Edge::*; 736 | match self { 737 | Px => Nx, 738 | Py => Ny, 739 | Nx => Px, 740 | Ny => Py, 741 | } 742 | } 743 | } 744 | 745 | impl Index for [T] { 746 | type Output = T; 747 | fn index(&self, edge: Edge) -> &T { 748 | &self[edge as usize] 749 | } 750 | } 751 | 752 | impl IndexMut for [T] { 753 | fn index_mut(&mut self, edge: Edge) -> &mut T { 754 | &mut self[edge as usize] 755 | } 756 | } 757 | 758 | /// Iterator over sample points distributed in a regular grid across a chunk, including its edges 759 | #[derive(Debug)] 760 | pub struct SampleIter { 761 | coords: Coords, 762 | face_resolution: u32, 763 | chunk_resolution: u32, 764 | index: u32, 765 | } 766 | 767 | impl Iterator for SampleIter { 768 | type Item = na::Unit>; 769 | fn next(&mut self) -> Option>> { 770 | if self.index >= self.chunk_resolution * self.chunk_resolution { 771 | return None; 772 | } 773 | let max = self.chunk_resolution - 1; 774 | let coords = if max == 0 { 775 | na::Point2::new(0.5, 0.5) 776 | } else { 777 | let step = 1.0 / max as f32; 778 | let (x, y) = ( 779 | self.index % self.chunk_resolution, 780 | self.index / self.chunk_resolution, 781 | ); 782 | na::Point2::new(x as f32, y as f32) * step 783 | }; 784 | let dir = self.coords.direction(self.face_resolution, &coords); 785 | self.index += 1; 786 | Some(dir) 787 | } 788 | 789 | fn size_hint(&self) -> (usize, Option) { 790 | let total = self.chunk_resolution * self.chunk_resolution; 791 | let remaining = (total - self.index) as usize; 792 | (remaining, Some(remaining)) 793 | } 794 | } 795 | 796 | impl ExactSizeIterator for SampleIter { 797 | fn len(&self) -> usize { 798 | let total = self.chunk_resolution * self.chunk_resolution; 799 | (total - self.index) as usize 800 | } 801 | } 802 | 803 | /// Iterator over sample points distributed in a regular grid across a chunk, including its edges 804 | /// 805 | /// Hand-vectorized, returning batches of each dimension in a separate register. 806 | #[derive(Debug)] 807 | pub struct SampleIterSimd { 808 | coords: Coords, 809 | face_resolution: u32, 810 | chunk_resolution: u32, 811 | index: u32, 812 | _simd: PhantomData S>, 813 | } 814 | 815 | impl Iterator for SampleIterSimd 816 | where 817 | S: SimdRealField + Copy, 818 | S::Element: RealField + Copy, 819 | { 820 | type Item = [S; 3]; 821 | fn next(&mut self) -> Option { 822 | if self.index >= self.chunk_resolution * self.chunk_resolution { 823 | return None; 824 | } 825 | { 826 | let edge_length = Coords::edge_length::(self.face_resolution); 827 | let origin_on_face_x = na::convert::<_, S::Element>(self.coords.x as f32) 828 | .mul_add(edge_length, -S::Element::one()); 829 | let origin_on_face_y = na::convert::<_, S::Element>(self.coords.y as f32) 830 | .mul_add(edge_length, -S::Element::one()); 831 | let max = self.chunk_resolution - 1; 832 | let (offset_x, offset_y) = if max == 0 { 833 | let v = S::splat(na::convert::<_, S::Element>(0.5) * edge_length); 834 | (v, v) 835 | } else { 836 | let step = edge_length / na::convert(max as f32); 837 | let mut xs = S::zero(); 838 | for i in 0..S::LANES { 839 | xs.replace( 840 | i, 841 | na::convert(((self.index + i as u32) % self.chunk_resolution) as f32), 842 | ); 843 | } 844 | let mut ys = S::zero(); 845 | for i in 0..S::LANES { 846 | ys.replace( 847 | i, 848 | na::convert(((self.index + i as u32) / self.chunk_resolution) as f32), 849 | ); 850 | } 851 | (xs * S::splat(step), ys * S::splat(step)) 852 | }; 853 | let pos_on_face_x = S::splat(origin_on_face_x) + offset_x; 854 | let pos_on_face_y = S::splat(origin_on_face_y) + offset_y; 855 | 856 | let warped_x = warp(pos_on_face_x); 857 | let warped_y = warp(pos_on_face_y); 858 | 859 | let len = warped_y 860 | .simd_mul_add(warped_y, warped_x.simd_mul_add(warped_x, S::one())) 861 | .simd_sqrt(); 862 | let dir_x = warped_x / len; 863 | let dir_y = warped_y / len; 864 | let dir_z = len.simd_recip(); 865 | 866 | let basis = self.coords.face.basis::(); 867 | let basis = basis.matrix(); 868 | let x = S::splat(basis.m11).simd_mul_add( 869 | dir_x, 870 | S::splat(basis.m12).simd_mul_add(dir_y, S::splat(basis.m13) * dir_z), 871 | ); 872 | let y = S::splat(basis.m21).simd_mul_add( 873 | dir_x, 874 | S::splat(basis.m22).simd_mul_add(dir_y, S::splat(basis.m23) * dir_z), 875 | ); 876 | let z = S::splat(basis.m31).simd_mul_add( 877 | dir_x, 878 | S::splat(basis.m32).simd_mul_add(dir_y, S::splat(basis.m33) * dir_z), 879 | ); 880 | 881 | self.index += S::LANES as u32; 882 | Some([x, y, z]) 883 | } 884 | } 885 | 886 | fn size_hint(&self) -> (usize, Option) { 887 | let total = self.chunk_resolution * self.chunk_resolution; 888 | let remaining = (total - self.index) as usize; 889 | let x = (remaining + S::LANES - 1) / S::LANES; 890 | (x, Some(x)) 891 | } 892 | } 893 | 894 | impl ExactSizeIterator for SampleIterSimd 895 | where 896 | S: SimdRealField + Copy, 897 | S::Element: RealField + Copy, 898 | { 899 | fn len(&self) -> usize { 900 | self.size_hint().0 901 | } 902 | } 903 | 904 | /// Map real coordinates in [0, 1)^2 to integer coordinates in [0, n)^2 such that each integer 905 | /// covers exactly the same distance 906 | fn discretize(resolution: usize, texcoords: na::Point2) -> (usize, usize) { 907 | let texcoords = texcoords * resolution as f32; 908 | let max = resolution - 1; 909 | ( 910 | na::clamp(texcoords.x as usize, 0, max), 911 | na::clamp(texcoords.y as usize, 0, max), 912 | ) 913 | } 914 | 915 | #[cfg(test)] 916 | mod test { 917 | use super::*; 918 | use approx::*; 919 | 920 | #[test] 921 | fn index_sanity() { 922 | const RES: u32 = 2048; 923 | assert_eq!( 924 | index(RES, &na::Vector3::x_axis()) as u32, 925 | (RES / 2) * (RES + 1) 926 | ); 927 | } 928 | 929 | #[test] 930 | fn iter() { 931 | for res in 0..8 { 932 | let map = CubeMap::new(res, 0); 933 | assert_eq!(map.iter().count() as u32, res * res * 6); 934 | } 935 | } 936 | 937 | #[test] 938 | fn addressing_roundtrip() { 939 | const RES: u32 = 2049; // must be odd for there to be a point exactly on the axes 940 | for dir in &[ 941 | na::Vector3::x_axis(), 942 | na::Vector3::y_axis(), 943 | na::Vector3::z_axis(), 944 | -na::Vector3::x_axis(), 945 | -na::Vector3::y_axis(), 946 | -na::Vector3::z_axis(), 947 | na::Unit::new_normalize(na::Vector3::new(1.0, 1.0, 1.0)), 948 | ] { 949 | let index = index(RES, dir); 950 | let out = get_dir(RES, index).unwrap(); 951 | assert_eq!(dir, &out); 952 | } 953 | } 954 | 955 | #[test] 956 | fn face_neighbors() { 957 | // Opposite faces are incident to opposite edges 958 | assert_eq!(Face::Px.neighbors()[0b00].1, -Face::Nx.neighbors()[0b10].1); 959 | assert_eq!(Face::Px.neighbors()[0b01].1, -Face::Nx.neighbors()[0b01].1); 960 | assert_eq!(Face::Px.neighbors()[0b10].1, -Face::Nx.neighbors()[0b00].1); 961 | assert_eq!(Face::Px.neighbors()[0b11].1, -Face::Nx.neighbors()[0b11].1); 962 | 963 | assert_eq!(Face::Py.neighbors()[0b00].1, -Face::Ny.neighbors()[0b00].1); 964 | assert_eq!(Face::Py.neighbors()[0b01].1, -Face::Ny.neighbors()[0b11].1); 965 | assert_eq!(Face::Py.neighbors()[0b10].1, -Face::Ny.neighbors()[0b10].1); 966 | assert_eq!(Face::Py.neighbors()[0b11].1, -Face::Ny.neighbors()[0b01].1); 967 | 968 | assert_eq!(Face::Pz.neighbors()[0b00].1, -Face::Nz.neighbors()[0b10].1); 969 | assert_eq!(Face::Pz.neighbors()[0b01].1, -Face::Nz.neighbors()[0b01].1); 970 | assert_eq!(Face::Pz.neighbors()[0b10].1, -Face::Nz.neighbors()[0b00].1); 971 | assert_eq!(Face::Pz.neighbors()[0b11].1, -Face::Nz.neighbors()[0b11].1); 972 | } 973 | 974 | #[test] 975 | fn face_neighbor_axes() { 976 | // Neighboring faces correctly track whether the axes they intersect on in their local 977 | // reference frames are parallel or antiparallel 978 | for face in Face::iter() { 979 | for (edge, &(neighbor, neighbor_edge, parallel)) in Edge::iter().zip(face.neighbors()) { 980 | let local = face.basis() 981 | * match edge { 982 | Edge::Px | Edge::Nx => na::Vector3::y(), 983 | Edge::Py | Edge::Ny => na::Vector3::x(), 984 | }; 985 | let neighbor = neighbor.basis() 986 | * match neighbor_edge { 987 | Edge::Px | Edge::Nx => na::Vector3::y(), 988 | Edge::Py | Edge::Ny => na::Vector3::x(), 989 | }; 990 | let sign = if parallel { 1.0 } else { -1.0 }; 991 | assert_eq!(local, sign * neighbor); 992 | } 993 | } 994 | } 995 | 996 | #[test] 997 | fn face_coord_sanity() { 998 | let o = na::Point2::::origin(); 999 | assert_eq!(Face::coords(&na::Vector3::x()), (Face::Px, o)); 1000 | assert_eq!(Face::coords(&na::Vector3::y()), (Face::Py, o)); 1001 | assert_eq!(Face::coords(&na::Vector3::z()), (Face::Pz, o)); 1002 | assert_eq!(Face::coords(&-na::Vector3::x()), (Face::Nx, o)); 1003 | assert_eq!(Face::coords(&-na::Vector3::y()), (Face::Ny, o)); 1004 | assert_eq!(Face::coords(&-na::Vector3::z()), (Face::Nz, o)); 1005 | } 1006 | 1007 | #[test] 1008 | fn coord_neighborhood() { 1009 | use Face::*; 1010 | for face in Face::iter() { 1011 | let center = face.basis() * na::Vector3::z(); 1012 | assert_eq!( 1013 | Coords::neighborhood(1, center, 0.1).collect::>(), 1014 | vec![Coords { x: 0, y: 0, face }] 1015 | ); 1016 | assert_eq!( 1017 | Coords::neighborhood(3, center, 0.1).collect::>(), 1018 | vec![Coords { x: 1, y: 1, face }] 1019 | ); 1020 | let xs = Coords::neighborhood(2, center, 0.1).collect::>(); 1021 | assert_eq!(xs.len(), 4); 1022 | for expected in (0..2).flat_map(|y| (0..2).map(move |x| Coords { x, y, face })) { 1023 | assert!(xs.contains(&expected)); 1024 | } 1025 | } 1026 | assert_eq!( 1027 | Coords::neighborhood(1, na::Vector3::new(1.0, 1.0, 1.0), 0.1).collect::>(), 1028 | vec![ 1029 | Coords { 1030 | x: 0, 1031 | y: 0, 1032 | face: Px 1033 | }, 1034 | Coords { 1035 | x: 0, 1036 | y: 0, 1037 | face: Py 1038 | }, 1039 | Coords { 1040 | x: 0, 1041 | y: 0, 1042 | face: Pz 1043 | } 1044 | ] 1045 | ); 1046 | assert_eq!( 1047 | Coords::neighborhood(1, na::Vector3::new(1.0, 1.0, 0.0), 0.1).collect::>(), 1048 | vec![ 1049 | Coords { 1050 | x: 0, 1051 | y: 0, 1052 | face: Px 1053 | }, 1054 | Coords { 1055 | x: 0, 1056 | y: 0, 1057 | face: Py 1058 | }, 1059 | ] 1060 | ); 1061 | assert_eq!( 1062 | Coords::neighborhood(5, na::Vector3::new(1.0, 1.0, 1.0), 0.1).count(), 1063 | 3 1064 | ); 1065 | } 1066 | 1067 | #[test] 1068 | fn discretize_sanity() { 1069 | assert_eq!(discretize(100, na::Point2::new(1.0, 1.0)), (99, 99)); 1070 | assert_eq!(discretize(100, na::Point2::new(0.0, 0.0)), (0, 0)); 1071 | assert_eq!(discretize(100, na::Point2::new(0.990, 0.990)), (99, 99)); 1072 | assert_eq!(discretize(100, na::Point2::new(0.989, 0.989)), (98, 98)); 1073 | assert_eq!(discretize(100, na::Point2::new(0.010, 0.010)), (1, 1)); 1074 | assert_eq!(discretize(100, na::Point2::new(0.009, 0.009)), (0, 0)); 1075 | 1076 | assert_eq!(discretize(2, na::Point2::new(0.49, 0.49)), (0, 0)); 1077 | assert_eq!(discretize(2, na::Point2::new(0.50, 0.50)), (1, 1)); 1078 | } 1079 | 1080 | #[test] 1081 | fn from_raw() { 1082 | let data = [1, 0, 1, 2, 3, 4, 5]; 1083 | let x = unsafe { CubeMap::::from_raw(data.as_ptr()) }; 1084 | assert_eq!(x.resolution(), 1); 1085 | assert_eq!(x.as_ref().len(), 6); 1086 | for i in 0..6 { 1087 | assert_eq!(x.as_ref()[i], data[i + 1]); 1088 | } 1089 | } 1090 | 1091 | #[test] 1092 | fn samples_sanity() { 1093 | const COORDS: Coords = Coords { 1094 | x: 0, 1095 | y: 0, 1096 | face: Face::Py, 1097 | }; 1098 | assert_abs_diff_eq!(COORDS.samples(1, 1).next().unwrap(), na::Vector3::y_axis()); 1099 | let corners = COORDS 1100 | .samples(1, 2) 1101 | .map(|x| x.into_inner()) 1102 | .collect::>(); 1103 | let corner = na::Unit::new_normalize(na::Vector3::new(1.0, 1.0, 1.0)); 1104 | assert_abs_diff_eq!( 1105 | corners[..], 1106 | [ 1107 | na::Vector3::new(-corner.x, corner.y, corner.z), 1108 | na::Vector3::new(corner.x, corner.y, corner.z), 1109 | na::Vector3::new(-corner.x, corner.y, -corner.z), 1110 | na::Vector3::new(corner.x, corner.y, -corner.z), 1111 | ][..] 1112 | ); 1113 | } 1114 | 1115 | #[test] 1116 | fn neighboring_samples_align() { 1117 | const LEFT: Coords = Coords { 1118 | x: 0, 1119 | y: 0, 1120 | face: Face::Pz, 1121 | }; 1122 | const RIGHT: Coords = Coords { 1123 | x: 1, 1124 | y: 0, 1125 | face: Face::Pz, 1126 | }; 1127 | let left = LEFT.samples(2, 2).collect::>(); 1128 | let right = RIGHT.samples(2, 2).collect::>(); 1129 | assert_abs_diff_eq!(left[1], right[0]); 1130 | assert_abs_diff_eq!(left[3], right[2]); 1131 | } 1132 | 1133 | #[test] 1134 | fn simd_samples_consistent() { 1135 | const COORDS: Coords = Coords { 1136 | x: 0, 1137 | y: 0, 1138 | face: Face::Py, 1139 | }; 1140 | const FACE_RES: u32 = 1; 1141 | const CHUNK_RES: u32 = 17; 1142 | let scalar = COORDS.samples(FACE_RES, CHUNK_RES); 1143 | let simd = COORDS.samples_ps::(FACE_RES, CHUNK_RES); 1144 | assert_eq!(simd.len(), scalar.len()); 1145 | for (scalar, [x, y, z]) in scalar.zip(simd) { 1146 | dbg!(x, y, z); 1147 | assert_abs_diff_eq!(scalar, na::Unit::new_unchecked(na::Vector3::new(x, y, z))); 1148 | } 1149 | } 1150 | 1151 | #[test] 1152 | fn warp_roundtrip() { 1153 | for i in -100..100 { 1154 | let x = i as f32 / 100.0; 1155 | dbg!(i); 1156 | assert_abs_diff_eq!(x, unwarp(warp(x))); 1157 | } 1158 | } 1159 | 1160 | #[test] 1161 | fn direction_roundtrip() { 1162 | for face in Face::iter() { 1163 | for y in -10..10 { 1164 | for x in -10..10 { 1165 | let p = na::Point2::new(x as f32 / 11.0, y as f32 / 11.0); 1166 | let v = face.direction(&p); 1167 | let (actual_face, actual_p) = Face::coords(&v); 1168 | assert_eq!(face, actual_face); 1169 | assert_abs_diff_eq!(p, actual_p); 1170 | } 1171 | } 1172 | } 1173 | } 1174 | 1175 | #[test] 1176 | fn neighborhood_consistency() { 1177 | let dir = na::Vector3::new(-5_195_083.148, 3_582_099.8, -877_091.25); 1178 | assert_eq!( 1179 | Coords::from_vector(4096, &dir), 1180 | Coords::neighborhood(4096, dir, 0.0).next().unwrap() 1181 | ); 1182 | } 1183 | } 1184 | -------------------------------------------------------------------------------- /src/lib.rs: -------------------------------------------------------------------------------- 1 | pub mod cubemap; 2 | pub use cubemap::CubeMap; 3 | 4 | pub mod chunk; 5 | pub use chunk::Chunk; 6 | 7 | pub mod cache; 8 | pub use cache::Manager as CacheManager; 9 | 10 | #[cfg(feature = "parry")] 11 | pub mod parry; 12 | -------------------------------------------------------------------------------- /src/parry.rs: -------------------------------------------------------------------------------- 1 | //! Collision detection for radial heightmaps 2 | //! 3 | //! Implement [`Terrain`] for your heightmap, then create colliders using it with a [`Planet`]. A 4 | //! query dispatcher that handles both planets and standard shapes can be constructed with 5 | //! `PlanetDispatcher.chain(DefaultQueryDispatcher)`. 6 | 7 | use std::sync::{Arc, Mutex}; 8 | 9 | use hashbrown::hash_map; 10 | use hashbrown::HashMap; 11 | use lru_slab::LruSlab; 12 | use parry3d_f64::{ 13 | bounding_volume::{Aabb, BoundingSphere, BoundingVolume}, 14 | mass_properties::MassProperties, 15 | math::{Isometry, Point, Real, Vector}, 16 | query::{ 17 | details::NormalConstraints, visitors::BoundingVolumeIntersectionsVisitor, ClosestPoints, 18 | Contact, ContactManifold, ContactManifoldsWorkspace, DefaultQueryDispatcher, 19 | NonlinearRigidMotion, PersistentQueryDispatcher, PointProjection, PointQuery, 20 | QueryDispatcher, Ray, RayCast, RayIntersection, ShapeCastHit, ShapeCastOptions, 21 | TypedWorkspaceData, Unsupported, WorkspaceData, 22 | }, 23 | shape::{FeatureId, HalfSpace, Shape, ShapeType, SimdCompositeShape, Triangle, TypedShape}, 24 | utils::IsometryOpt, 25 | }; 26 | 27 | use crate::cubemap::{Coords, Edge}; 28 | 29 | /// Height data source for `Planet` 30 | pub trait Terrain: Send + Sync + 'static { 31 | /// Generate a `resolution * resolution` grid of heights wrt. sea level 32 | fn sample(&self, coords: &Coords, out: &mut [f32]); 33 | /// Number of blocks of samples (chunks) along the edge of a cubemap face 34 | fn face_resolution(&self) -> u32; 35 | /// Number of samples along the edge of a single chunk. `sample` will be supplied buffers sized 36 | /// based for this. Must be at least 2. 37 | fn chunk_resolution(&self) -> u32; 38 | /// The maximum value that will ever be written by `sample` 39 | fn max_height(&self) -> f32; 40 | /// The minimum value that will ever be written by `sample` 41 | fn min_height(&self) -> f32; 42 | } 43 | 44 | /// Trivial `Terrain` impl 45 | #[derive(Debug, Copy, Clone)] 46 | pub struct FlatTerrain { 47 | face_resolution: u32, 48 | chunk_resolution: u32, 49 | } 50 | 51 | impl FlatTerrain { 52 | pub fn new(face_resolution: u32, chunk_resolution: u32) -> Self { 53 | Self { 54 | face_resolution, 55 | chunk_resolution, 56 | } 57 | } 58 | } 59 | 60 | impl Terrain for FlatTerrain { 61 | fn face_resolution(&self) -> u32 { 62 | self.face_resolution 63 | } 64 | 65 | fn chunk_resolution(&self) -> u32 { 66 | self.chunk_resolution 67 | } 68 | 69 | fn max_height(&self) -> f32 { 70 | 0.0 71 | } 72 | fn min_height(&self) -> f32 { 73 | 0.0 74 | } 75 | 76 | fn sample(&self, _: &Coords, out: &mut [f32]) { 77 | for x in out { 78 | *x = 0.0; 79 | } 80 | } 81 | } 82 | 83 | /// A fixed-resolution partially-resident radial heightmap 84 | /// 85 | /// Generates height data on-demand via `Terrain`, preserving it in a fixed-size LRU cache. 86 | pub struct Planet { 87 | terrain: Arc, 88 | radius: f64, 89 | chunk_resolution: u32, 90 | // Future work: could preallocate an arena for height samples 91 | cache: Mutex, 92 | } 93 | 94 | impl Planet { 95 | /// Construct a new collision shape for a radial heightmap defined by `terrain` 96 | /// 97 | /// `terrain` - source of height samples 98 | /// `cache_size` - maximum number of chunks of height data to keep in memory 99 | /// `radius` - distance from origin of points with height 0 100 | pub fn new(terrain: Arc, cache_size: u32, radius: f64) -> Self { 101 | Self { 102 | chunk_resolution: terrain.chunk_resolution(), 103 | terrain, 104 | radius, 105 | cache: Mutex::new(Cache::new(cache_size)), 106 | } 107 | } 108 | 109 | fn max_radius(&self) -> f64 { 110 | self.radius + self.terrain.max_height() as f64 111 | } 112 | fn min_radius(&self) -> f64 { 113 | self.radius + self.terrain.min_height() as f64 114 | } 115 | 116 | fn sample(&self, coords: &Coords) -> Box<[f32]> { 117 | let mut samples = 118 | vec![0.0; self.chunk_resolution as usize * self.chunk_resolution as usize] 119 | .into_boxed_slice(); 120 | self.terrain.sample(coords, &mut samples[..]); 121 | samples 122 | } 123 | 124 | fn feature_id(&self, slot: u32, triangle: u32) -> u32 { 125 | slot * self.chunk_resolution * self.chunk_resolution + triangle 126 | } 127 | 128 | /// Applies the function `f` to all the triangles intersecting the given sphere. Exits early on 129 | /// `false` return. 130 | pub fn map_elements_in_local_sphere( 131 | &self, 132 | bounds: &BoundingSphere, 133 | aabb: &Aabb, 134 | mut f: impl FnMut(&Coords, u32, u32, &Triangle) -> bool, 135 | ) { 136 | let dir = bounds.center().coords; 137 | let distance = dir.norm(); 138 | let cache = &mut *self.cache.lock().unwrap(); 139 | // Iterate over each overlapping chunk 140 | 'outer: for chunk_coords in Coords::neighborhood( 141 | self.terrain.face_resolution(), 142 | dir.cast(), 143 | bounds.radius().atan2(distance) as f32, 144 | ) { 145 | let (slot, data) = cache.get(self, &chunk_coords); 146 | if self.radius + data.max as f64 + bounds.radius() < distance 147 | || self.radius + data.min as f64 - bounds.radius() > distance 148 | { 149 | // Short-circuit if `other` is way above or below this chunk 150 | continue; 151 | } 152 | let patch = Patch::new(&chunk_coords, self.terrain.face_resolution()); 153 | for quad in patch.quads_within(aabb, self.chunk_resolution) { 154 | for (index, triangle) in 155 | quad.triangles(self.radius, self.chunk_resolution, &data.samples) 156 | { 157 | if !f(&chunk_coords, slot, index, &triangle) { 158 | break 'outer; 159 | } 160 | } 161 | } 162 | } 163 | } 164 | } 165 | 166 | impl Clone for Planet { 167 | fn clone(&self) -> Self { 168 | Self { 169 | terrain: self.terrain.clone(), 170 | cache: Mutex::new(self.cache.lock().unwrap().clone()), 171 | ..*self 172 | } 173 | } 174 | } 175 | 176 | impl RayCast for Planet { 177 | fn cast_local_ray_and_get_normal( 178 | &self, 179 | ray: &Ray, 180 | max_toi: Real, 181 | solid: bool, 182 | ) -> Option { 183 | // Find the chunk containing the ray origin 184 | let mut chunk = 185 | Coords::from_vector(self.terrain.face_resolution(), &ray.origin.coords.cast()); 186 | let mut patch = Patch::new(&chunk, self.terrain.face_resolution()); 187 | let mut ray = *ray; 188 | 189 | // Walk along the ray until we hit something 190 | let cache = &mut *self.cache.lock().unwrap(); 191 | loop { 192 | let (slot, data) = cache.get(self, &chunk); 193 | // FIXME: Rays can sometimes slip between bounding planes and the outer edge of a 194 | // quad. To avoid this, we should extend the quad to form a narrow skirt around the 195 | // chunk, or check neighboring chunks when very close to a boundary. 196 | 197 | let mut maybe_hit = None; 198 | let edge = walk_patch(self.chunk_resolution - 1, &patch, &ray, max_toi, |quad| { 199 | let tris = quad 200 | .displace(self.radius, self.chunk_resolution, &data.samples) 201 | .triangles(); 202 | let Some((tri, mut hit)) = tris 203 | .into_iter() 204 | .enumerate() 205 | .filter_map(|(i, t)| { 206 | Some((i, t.cast_local_ray_and_get_normal(&ray, max_toi, solid)?)) 207 | }) 208 | .min_by(|a, b| a.1.time_of_impact.total_cmp(&b.1.time_of_impact)) 209 | else { 210 | return true; 211 | }; 212 | let quad_index = quad.position.y * self.chunk_resolution + quad.position.x; 213 | let index = (quad_index << 1) | tri as u32; 214 | hit.feature = FeatureId::Face(self.feature_id(slot, index)); 215 | maybe_hit = Some(hit); 216 | false 217 | }); 218 | 219 | if let Some(hit) = maybe_hit { 220 | return Some(hit); 221 | } 222 | 223 | match edge { 224 | None => return None, 225 | Some((edge, toi)) => { 226 | chunk = chunk.neighbors(self.terrain.face_resolution())[edge]; 227 | patch = Patch::new(&chunk, self.terrain.face_resolution()); 228 | ray.origin += ray.dir * toi; 229 | } 230 | } 231 | } 232 | } 233 | } 234 | 235 | /// `quad` is row-major vectors from the origin 236 | fn raycast_quad_edges( 237 | ray: &Ray, 238 | [a, b, c, d]: &[na::Vector3; 4], 239 | max_toi: f64, 240 | ) -> Option<(Edge, f64)> { 241 | let edges: [(Edge, [&na::Vector3; 2]); 4] = [ 242 | (Edge::Nx, [c, a]), 243 | (Edge::Ny, [a, b]), 244 | (Edge::Px, [b, d]), 245 | (Edge::Py, [d, c]), 246 | ]; 247 | 248 | let mut closest = None; 249 | for &(edge, [v1, v2]) in edges.iter() { 250 | // Construct inward-facing edge planes 251 | let plane = HalfSpace { 252 | normal: na::Unit::new_normalize(v1.cross(v2)), 253 | }; 254 | // Eliminate planes behind the ray 255 | if plane.normal.as_ref().dot(&ray.dir) >= 0.0 { 256 | continue; 257 | } 258 | if let Some(hit) = plane.cast_local_ray(ray, max_toi, true) { 259 | closest = Some(match closest { 260 | None => (edge, hit), 261 | Some((_, toi)) if hit < toi => (edge, hit), 262 | Some(x) => x, 263 | }); 264 | } 265 | } 266 | closest 267 | } 268 | 269 | impl PointQuery for Planet { 270 | fn project_local_point(&self, pt: &Point, solid: bool) -> PointProjection { 271 | if solid && pt.coords.norm_squared() < self.min_radius() * self.min_radius() { 272 | return PointProjection { 273 | is_inside: true, 274 | point: *pt, 275 | }; 276 | } 277 | // TODO: Handle `solid` near the surface 278 | self.project_local_point_and_get_feature(pt).0 279 | } 280 | 281 | fn project_local_point_and_get_feature( 282 | &self, 283 | pt: &Point, 284 | ) -> (PointProjection, FeatureId) { 285 | // TODO: Optimize/fix this by projecting `pt` onto the cubemap, then scanning *outward* from 286 | // the quad containing the projected point until all remaining triangles must be further 287 | // than the closest triangle found so far, regardless of height 288 | let coords = Coords::from_vector(self.terrain.face_resolution(), &pt.coords.cast()); 289 | let distance2 = |x: &na::Point3| na::distance_squared(x, pt); 290 | let cache = &mut *self.cache.lock().unwrap(); 291 | let (slot, data) = cache.get(self, &coords); 292 | let patch = Patch::new(&coords, self.terrain.face_resolution()); 293 | let (idx, nearest) = patch 294 | .triangles(self.radius, self.chunk_resolution, &data.samples) 295 | .map(|(i, tri)| (i, tri.project_local_point(pt, false))) 296 | .min_by(|(_, x), (_, y)| { 297 | distance2(&x.point) 298 | .partial_cmp(&distance2(&y.point)) 299 | .unwrap() 300 | }) 301 | .unwrap(); 302 | // TODO: Check neighborhood, so we don't miss as many cliff faces 303 | (nearest, FeatureId::Face(self.feature_id(slot, idx))) 304 | } 305 | } 306 | 307 | impl Shape for Planet { 308 | fn compute_local_aabb(&self) -> Aabb { 309 | Aabb::from_half_extents(Point::origin(), Vector::repeat(self.max_radius())) 310 | } 311 | 312 | fn compute_local_bounding_sphere(&self) -> BoundingSphere { 313 | BoundingSphere::new(Point::origin(), self.max_radius()) 314 | } 315 | 316 | fn mass_properties(&self, density: Real) -> MassProperties { 317 | parry3d_f64::shape::Ball { 318 | radius: self.radius, 319 | } 320 | .mass_properties(density) 321 | } 322 | 323 | fn shape_type(&self) -> ShapeType { 324 | ShapeType::Custom 325 | } 326 | 327 | fn as_typed_shape(&self) -> TypedShape<'_> { 328 | TypedShape::Custom(self) 329 | } 330 | 331 | fn ccd_thickness(&self) -> Real { 332 | 0.0 333 | } 334 | 335 | fn ccd_angular_thickness(&self) -> Real { 336 | 0.0 337 | } 338 | 339 | fn clone_dyn(&self) -> Box { 340 | Box::new(self.clone()) 341 | } 342 | 343 | fn scale_dyn(&self, _scale: &Vector, _num_subdivisions: u32) -> Option> { 344 | // Non-uniform scale not supported 345 | None 346 | } 347 | } 348 | 349 | #[derive(Clone)] 350 | struct Cache { 351 | slots: LruSlab, 352 | index: HashMap, 353 | } 354 | 355 | impl Cache { 356 | pub fn new(capacity: u32) -> Self { 357 | Self { 358 | slots: LruSlab::with_capacity(capacity), 359 | index: HashMap::with_capacity(capacity as usize), 360 | } 361 | } 362 | 363 | pub fn get(&mut self, planet: &Planet, coords: &Coords) -> (u32, &ChunkData) { 364 | let (slot, old) = match self.index.entry(*coords) { 365 | hash_map::Entry::Occupied(e) => (*e.get(), None), 366 | hash_map::Entry::Vacant(e) => { 367 | let old = if self.slots.len() == self.slots.capacity() { 368 | let lru = self.slots.lru().unwrap(); 369 | Some(self.slots.remove(lru).coords) 370 | } else { 371 | None 372 | }; 373 | let slot = self 374 | .slots 375 | .insert(ChunkData::new(*coords, planet.sample(coords))); 376 | e.insert(slot); 377 | (slot, old) 378 | } 379 | }; 380 | if let Some(old) = old { 381 | self.index.remove(&old); 382 | } 383 | (slot, self.slots.get_mut(slot)) 384 | } 385 | } 386 | 387 | #[derive(Debug, Clone)] 388 | struct ChunkData { 389 | samples: Box<[f32]>, 390 | coords: Coords, 391 | min: f32, 392 | max: f32, 393 | } 394 | 395 | impl ChunkData { 396 | fn new(coords: Coords, samples: Box<[f32]>) -> Self { 397 | let mut iter = samples.iter().cloned(); 398 | let first = iter.next().expect("empty sample array"); 399 | let mut min = first; 400 | let mut max = first; 401 | for sample in iter { 402 | if sample < min { 403 | min = sample; 404 | } else if sample > max { 405 | max = sample; 406 | } 407 | } 408 | Self { 409 | samples, 410 | coords, 411 | min, 412 | max, 413 | } 414 | } 415 | } 416 | 417 | /// A [`PersistentQueryDispatcher`] that handles `Planet` shapes 418 | pub struct PlanetDispatcher; 419 | 420 | // TODO: Fill these in 421 | impl QueryDispatcher for PlanetDispatcher { 422 | fn intersection_test( 423 | &self, 424 | pos12: &Isometry, 425 | g1: &dyn Shape, 426 | g2: &dyn Shape, 427 | ) -> Result { 428 | if let Some(p1) = g1.downcast_ref::() { 429 | return Ok(intersects(pos12, p1, g2)); 430 | } 431 | if let Some(p2) = g2.downcast_ref::() { 432 | return Ok(intersects(&pos12.inverse(), p2, g1)); 433 | } 434 | Err(Unsupported) 435 | } 436 | 437 | fn distance( 438 | &self, 439 | _pos12: &Isometry, 440 | _g1: &dyn Shape, 441 | _g2: &dyn Shape, 442 | ) -> Result { 443 | Err(Unsupported) 444 | } 445 | 446 | fn contact( 447 | &self, 448 | _pos12: &Isometry, 449 | _g1: &dyn Shape, 450 | _g2: &dyn Shape, 451 | _prediction: Real, 452 | ) -> Result, Unsupported> { 453 | Err(Unsupported) 454 | } 455 | 456 | fn closest_points( 457 | &self, 458 | _pos12: &Isometry, 459 | _g1: &dyn Shape, 460 | _g2: &dyn Shape, 461 | _max_dist: Real, 462 | ) -> Result { 463 | Err(Unsupported) 464 | } 465 | 466 | fn cast_shapes( 467 | &self, 468 | pos12: &Isometry, 469 | vel12: &Vector, 470 | g1: &dyn Shape, 471 | g2: &dyn Shape, 472 | options: ShapeCastOptions, 473 | ) -> Result, Unsupported> { 474 | if let Some(p1) = g1.downcast_ref::() { 475 | return Ok(compute_toi(pos12, vel12, p1, g2, options, false)); 476 | } 477 | if let Some(p2) = g2.downcast_ref::() { 478 | return Ok(compute_toi( 479 | &pos12.inverse(), 480 | &-vel12, 481 | p2, 482 | g1, 483 | options, 484 | true, 485 | )); 486 | } 487 | Err(Unsupported) 488 | } 489 | 490 | fn cast_shapes_nonlinear( 491 | &self, 492 | motion1: &NonlinearRigidMotion, 493 | g1: &dyn Shape, 494 | motion2: &NonlinearRigidMotion, 495 | g2: &dyn Shape, 496 | start_time: Real, 497 | end_time: Real, 498 | stop_at_penetration: bool, 499 | ) -> Result, Unsupported> { 500 | if let Some(p1) = g1.downcast_ref::() { 501 | return Ok(compute_nonlinear_toi( 502 | motion1, 503 | p1, 504 | motion2, 505 | g2, 506 | start_time, 507 | end_time, 508 | stop_at_penetration, 509 | false, 510 | )); 511 | } 512 | if let Some(p2) = g2.downcast_ref::() { 513 | return Ok(compute_nonlinear_toi( 514 | motion2, 515 | p2, 516 | motion1, 517 | g1, 518 | start_time, 519 | end_time, 520 | stop_at_penetration, 521 | false, 522 | )); 523 | } 524 | Err(Unsupported) 525 | } 526 | } 527 | 528 | fn intersects(pos12: &Isometry, planet: &Planet, other: &dyn Shape) -> bool { 529 | // TODO after https://github.com/dimforge/parry/issues/8 530 | let dispatcher = DefaultQueryDispatcher; 531 | let bounds = other.compute_bounding_sphere(pos12); 532 | let aabb = other.compute_aabb(pos12); 533 | let mut intersects = false; 534 | planet.map_elements_in_local_sphere(&bounds, &aabb, |_, _, _, triangle| { 535 | intersects = dispatcher 536 | .intersection_test(pos12, triangle, other) 537 | .unwrap_or(false); 538 | !intersects 539 | }); 540 | intersects 541 | } 542 | 543 | fn compute_toi( 544 | pos12: &Isometry, 545 | vel12: &Vector, 546 | planet: &Planet, 547 | other: &dyn Shape, 548 | options: ShapeCastOptions, 549 | flipped: bool, 550 | ) -> Option { 551 | // TODO after https://github.com/dimforge/parry/issues/8 552 | let dispatcher = DefaultQueryDispatcher; 553 | // TODO: Raycast vs. minkowski sum of chunk bounds and bounding sphere? 554 | let aabb = { 555 | let start = other.compute_aabb(pos12); 556 | let end = start.transform_by(&Isometry::from_parts( 557 | (options.max_time_of_impact * vel12).into(), 558 | na::one(), 559 | )); 560 | start.merged(&end) 561 | }; 562 | let mut closest = None::; 563 | planet.map_elements_in_local_sphere(&aabb.bounding_sphere(), &aabb, |_, _, _, triangle| { 564 | let impact = if flipped { 565 | dispatcher.cast_shapes(&pos12.inverse(), &-vel12, other, triangle, options) 566 | } else { 567 | dispatcher.cast_shapes(pos12, vel12, triangle, other, options) 568 | }; 569 | if let Ok(Some(impact)) = impact { 570 | closest = Some(match closest { 571 | None => impact, 572 | Some(x) if impact.time_of_impact < x.time_of_impact => impact, 573 | Some(x) => x, 574 | }); 575 | } 576 | true 577 | }); 578 | closest 579 | } 580 | 581 | #[allow(clippy::too_many_arguments)] // that's just what it takes 582 | fn compute_nonlinear_toi( 583 | motion_planet: &NonlinearRigidMotion, 584 | planet: &Planet, 585 | motion_other: &NonlinearRigidMotion, 586 | other: &dyn Shape, 587 | start_time: Real, 588 | end_time: Real, 589 | stop_at_penetration: bool, 590 | flipped: bool, 591 | ) -> Option { 592 | // TODO after https://github.com/dimforge/parry/issues/8 593 | let dispatcher = DefaultQueryDispatcher; 594 | // TODO: Select chunks/triangles more conservatively, as discussed in compute_toi 595 | let aabb = { 596 | let start_pos = motion_planet.position_at_time(start_time).inverse() 597 | * motion_other.position_at_time(start_time); 598 | let end_pos = motion_planet.position_at_time(end_time).inverse() 599 | * motion_other.position_at_time(end_time); 600 | let start = other.compute_aabb(&start_pos); 601 | let end = other.compute_aabb(&end_pos); 602 | start.merged(&end) 603 | }; 604 | let mut closest = None::; 605 | planet.map_elements_in_local_sphere(&aabb.bounding_sphere(), &aabb, |_, _, _, triangle| { 606 | let impact = if flipped { 607 | dispatcher.cast_shapes_nonlinear( 608 | motion_other, 609 | other, 610 | motion_planet, 611 | triangle, 612 | start_time, 613 | end_time, 614 | stop_at_penetration, 615 | ) 616 | } else { 617 | dispatcher.cast_shapes_nonlinear( 618 | motion_planet, 619 | triangle, 620 | motion_other, 621 | other, 622 | start_time, 623 | end_time, 624 | stop_at_penetration, 625 | ) 626 | }; 627 | if let Ok(Some(impact)) = impact { 628 | closest = Some(match closest { 629 | None => impact, 630 | Some(x) if impact.time_of_impact < x.time_of_impact => impact, 631 | Some(x) => x, 632 | }); 633 | } 634 | true 635 | }); 636 | closest 637 | } 638 | 639 | impl PersistentQueryDispatcher 640 | for PlanetDispatcher 641 | where 642 | ManifoldData: Default + Clone, 643 | ContactData: Default + Copy, 644 | { 645 | fn contact_manifolds( 646 | &self, 647 | pos12: &Isometry, 648 | g1: &dyn Shape, 649 | g2: &dyn Shape, 650 | prediction: Real, 651 | manifolds: &mut Vec>, 652 | workspace: &mut Option, 653 | ) -> Result<(), Unsupported> { 654 | if let Some(p1) = g1.downcast_ref::() { 655 | if let Some(composite) = g2.as_composite_shape() { 656 | compute_manifolds_vs_composite( 657 | pos12, 658 | &pos12.inverse(), 659 | p1, 660 | composite, 661 | prediction, 662 | manifolds, 663 | workspace, 664 | false, 665 | ); 666 | } else { 667 | compute_manifolds(pos12, p1, g2, prediction, manifolds, workspace, false); 668 | } 669 | return Ok(()); 670 | } 671 | if let Some(p2) = g2.downcast_ref::() { 672 | if let Some(composite) = g2.as_composite_shape() { 673 | compute_manifolds_vs_composite( 674 | &pos12.inverse(), 675 | pos12, 676 | p2, 677 | composite, 678 | prediction, 679 | manifolds, 680 | workspace, 681 | true, 682 | ); 683 | } else { 684 | compute_manifolds( 685 | &pos12.inverse(), 686 | p2, 687 | g1, 688 | prediction, 689 | manifolds, 690 | workspace, 691 | true, 692 | ); 693 | } 694 | return Ok(()); 695 | } 696 | Err(Unsupported) 697 | } 698 | 699 | fn contact_manifold_convex_convex( 700 | &self, 701 | _pos12: &Isometry, 702 | _g1: &dyn Shape, 703 | _g2: &dyn Shape, 704 | _normal_constraints1: Option<&dyn NormalConstraints>, 705 | _normal_constraints2: Option<&dyn NormalConstraints>, 706 | _prediction: Real, 707 | _manifold: &mut ContactManifold, 708 | ) -> Result<(), Unsupported> { 709 | // Planets aren't guaranteed to be convex, so we have no cases to handle here 710 | Err(Unsupported) 711 | } 712 | } 713 | 714 | fn compute_manifolds( 715 | pos12: &Isometry, 716 | planet: &Planet, 717 | other: &dyn Shape, 718 | prediction: Real, 719 | manifolds: &mut Vec>, 720 | workspace: &mut Option, 721 | flipped: bool, 722 | ) where 723 | ManifoldData: Default + Clone, 724 | ContactData: Default + Copy, 725 | { 726 | let workspace = workspace 727 | .get_or_insert_with(|| ContactManifoldsWorkspace(Box::::default())) 728 | .0 729 | .downcast_mut::() 730 | .unwrap(); 731 | let dispatcher = DefaultQueryDispatcher; // TODO after https://github.com/dimforge/parry/issues/8 732 | 733 | workspace.phase ^= true; 734 | let phase = workspace.phase; 735 | 736 | let bounds = other.compute_bounding_sphere(pos12).loosened(prediction); 737 | let aabb = other.compute_aabb(pos12).loosened(prediction); 738 | let mut old_manifolds = std::mem::take(manifolds); 739 | planet.map_elements_in_local_sphere(&bounds, &aabb, |&coords, slot, index, triangle| { 740 | let tri_state = match workspace.state.entry((coords, index)) { 741 | hash_map::Entry::Occupied(e) => { 742 | let tri_state = e.into_mut(); 743 | 744 | let manifold = old_manifolds[tri_state.manifold_index].take(); 745 | tri_state.manifold_index = manifolds.len(); 746 | tri_state.phase = phase; 747 | manifolds.push(manifold); 748 | 749 | tri_state 750 | } 751 | hash_map::Entry::Vacant(e) => { 752 | let tri_state = TriangleState { 753 | manifold_index: manifolds.len(), 754 | phase, 755 | }; 756 | 757 | let id = planet.feature_id(slot, index); 758 | let (id1, id2) = if flipped { (0, id) } else { (id, 0) }; 759 | manifolds.push(ContactManifold::with_data( 760 | id1, 761 | id2, 762 | ManifoldData::default(), 763 | )); 764 | 765 | e.insert(tri_state) 766 | } 767 | }; 768 | 769 | let manifold = &mut manifolds[tri_state.manifold_index]; 770 | 771 | // TODO: Nonconvex, postprocess contact `fid`s once parry's feature ID story is worked out 772 | if flipped { 773 | let _ = dispatcher.contact_manifold_convex_convex( 774 | &pos12.inverse(), 775 | other, 776 | triangle, 777 | None, 778 | None, 779 | prediction, 780 | manifold, 781 | ); 782 | } else { 783 | let _ = dispatcher.contact_manifold_convex_convex( 784 | pos12, triangle, other, None, None, prediction, manifold, 785 | ); 786 | } 787 | true 788 | }); 789 | 790 | workspace.state.retain(|_, x| x.phase == phase); 791 | } 792 | 793 | /// Narrow-phase collision detection state for `Planet` 794 | #[derive(Default, Clone)] 795 | pub struct Workspace { 796 | state: HashMap<(Coords, u32), TriangleState>, 797 | phase: bool, 798 | } 799 | 800 | impl WorkspaceData for Workspace { 801 | fn as_typed_workspace_data(&self) -> TypedWorkspaceData { 802 | TypedWorkspaceData::Custom 803 | } 804 | 805 | fn clone_dyn(&self) -> Box { 806 | Box::new(self.clone()) 807 | } 808 | } 809 | 810 | #[derive(Clone)] 811 | struct TriangleState { 812 | manifold_index: usize, 813 | phase: bool, 814 | } 815 | 816 | #[allow(clippy::too_many_arguments)] // that's just what it takes 817 | fn compute_manifolds_vs_composite( 818 | pos12: &Isometry, 819 | pos21: &Isometry, 820 | planet: &Planet, 821 | other: &dyn SimdCompositeShape, 822 | prediction: Real, 823 | manifolds: &mut Vec>, 824 | workspace: &mut Option, 825 | flipped: bool, 826 | ) where 827 | ManifoldData: Default + Clone, 828 | ContactData: Default + Copy, 829 | { 830 | let workspace = workspace 831 | .get_or_insert_with(|| ContactManifoldsWorkspace(Box::::default())) 832 | .0 833 | .downcast_mut::() 834 | .unwrap(); 835 | let dispatcher = DefaultQueryDispatcher; // TODO after https://github.com/dimforge/parry/issues/8 836 | 837 | workspace.phase ^= true; 838 | let phase = workspace.phase; 839 | 840 | let bvh = other.qbvh(); 841 | 842 | let bounds = bvh 843 | .root_aabb() 844 | .bounding_sphere() 845 | .transform_by(pos12) 846 | .loosened(prediction); 847 | let aabb = bvh.root_aabb().transform_by(pos12).loosened(prediction); 848 | let mut old_manifolds = std::mem::take(manifolds); 849 | planet.map_elements_in_local_sphere(&bounds, &aabb, |&coords, slot, index, triangle| { 850 | let tri_aabb = triangle.compute_aabb(pos21).loosened(prediction); 851 | 852 | let mut visit = |&composite_subshape: &u32| { 853 | other.map_part_at( 854 | composite_subshape, 855 | &mut |composite_part_pos, composite_part_shape, normal_constraints| { 856 | let key = CompositeKey { 857 | chunk_coords: coords, 858 | triangle: index, 859 | composite_subshape, 860 | }; 861 | // TODO: Dedup wrt. convex case 862 | let tri_state = match workspace.state.entry(key) { 863 | hash_map::Entry::Occupied(e) => { 864 | let tri_state = e.into_mut(); 865 | 866 | let manifold = old_manifolds[tri_state.manifold_index].take(); 867 | tri_state.manifold_index = manifolds.len(); 868 | tri_state.phase = phase; 869 | manifolds.push(manifold); 870 | 871 | tri_state 872 | } 873 | hash_map::Entry::Vacant(e) => { 874 | let mut manifold = ContactManifold::new(); 875 | let id = planet.feature_id(slot, index); 876 | if flipped { 877 | manifold.subshape1 = composite_subshape; 878 | manifold.subshape2 = id; 879 | manifold.subshape_pos1 = composite_part_pos.copied(); 880 | } else { 881 | manifold.subshape1 = id; 882 | manifold.subshape2 = composite_subshape; 883 | manifold.subshape_pos2 = composite_part_pos.copied(); 884 | }; 885 | 886 | let tri_state = TriangleState { 887 | manifold_index: manifolds.len(), 888 | phase, 889 | }; 890 | manifolds.push(manifold); 891 | e.insert(tri_state) 892 | } 893 | }; 894 | 895 | let manifold = &mut manifolds[tri_state.manifold_index]; 896 | 897 | if flipped { 898 | let _ = dispatcher.contact_manifold_convex_convex( 899 | &composite_part_pos.inv_mul(pos21), 900 | composite_part_shape, 901 | triangle, 902 | normal_constraints, 903 | None, 904 | prediction, 905 | manifold, 906 | ); 907 | } else { 908 | let _ = dispatcher.contact_manifold_convex_convex( 909 | &composite_part_pos.prepend_to(pos12), 910 | triangle, 911 | composite_part_shape, 912 | None, 913 | normal_constraints, 914 | prediction, 915 | manifold, 916 | ); 917 | } 918 | }, 919 | ); 920 | true 921 | }; 922 | let mut visitor = BoundingVolumeIntersectionsVisitor::new(&tri_aabb, &mut visit); 923 | bvh.traverse_depth_first(&mut visitor); 924 | 925 | true 926 | }); 927 | 928 | workspace.state.retain(|_, x| x.phase == phase); 929 | } 930 | 931 | /// Narrow-phase collision detection state for `Planet` 932 | #[derive(Default, Clone)] 933 | pub struct WorkspaceVsComposite { 934 | state: HashMap, 935 | phase: bool, 936 | } 937 | 938 | #[derive(Eq, PartialEq, Hash, Copy, Clone)] 939 | struct CompositeKey { 940 | chunk_coords: Coords, 941 | triangle: u32, 942 | composite_subshape: u32, 943 | } 944 | 945 | impl WorkspaceData for WorkspaceVsComposite { 946 | fn as_typed_workspace_data(&self) -> TypedWorkspaceData { 947 | TypedWorkspaceData::Custom 948 | } 949 | 950 | fn clone_dyn(&self) -> Box { 951 | Box::new(self.clone()) 952 | } 953 | } 954 | 955 | /// Quad defined by a chunk pre-displacement 956 | /// 957 | /// Generally neither flat nor square. 958 | #[derive(Copy, Clone, Debug)] 959 | struct Patch { 960 | // (0, 0) a--b 961 | // |\ | 962 | // | \| 963 | // c--d (1,1) 964 | a: na::Vector3, 965 | b: na::Vector3, 966 | c: na::Vector3, 967 | d: na::Vector3, 968 | } 969 | 970 | impl Patch { 971 | pub fn new(coords: &Coords, face_resolution: u32) -> Self { 972 | Self { 973 | a: coords 974 | .direction(face_resolution, &[0.0, 0.0].into()) 975 | .into_inner(), 976 | b: coords 977 | .direction(face_resolution, &[1.0, 0.0].into()) 978 | .into_inner(), 979 | c: coords 980 | .direction(face_resolution, &[0.0, 1.0].into()) 981 | .into_inner(), 982 | d: coords 983 | .direction(face_resolution, &[1.0, 1.0].into()) 984 | .into_inner(), 985 | } 986 | } 987 | 988 | /// Map a point from patch space to a direction in sphere space 989 | fn get(&self, p: &na::Point2) -> na::Vector3 { 990 | // Extend the triangle into a parallelogram, then bilinearly interpolate. This guarantees a 991 | // numerically exact result at each vertex, because in that case every vertex's contribution 992 | // is multiplied by 0 or 1 exactly and then summed. This precision ensures that there won't 993 | // be cracks between patches. 994 | let (b, c) = if p.x > p.y { 995 | (self.b, self.a + self.d - self.b) 996 | } else { 997 | (self.a + self.d - self.c, self.c) 998 | }; 999 | let y0 = self.a * (1.0 - p.x) + b * p.x; 1000 | let y1 = c * (1.0 - p.x) + self.d * p.x; 1001 | y0 * (1.0 - p.y) + y1 * p.y 1002 | } 1003 | 1004 | /// Map a direction in sphere space to a point in patch space 1005 | #[inline(always)] 1006 | fn project(&self, dir: &na::Vector3) -> na::Point2 { 1007 | // Project onto each triangle, then select the in-bounds result 1008 | #[inline(always)] 1009 | fn project( 1010 | p: &na::Vector3, 1011 | x: na::Vector3, 1012 | y: na::Vector3, 1013 | dir: &na::Vector3, 1014 | ) -> na::Point2 { 1015 | // t * dir = p + u * x + v * y 1016 | // -p = x * u + y * v - t * dir 1017 | // = [x y dir] [u v -t]^T 1018 | // [u v -t]^T = [x y dir]^-1 . -p 1019 | let m = na::Matrix3::from_columns(&[x, y, *dir]); 1020 | (-(m.try_inverse().unwrap().fixed_view::<2, 3>(0, 0) * p)).into() 1021 | } 1022 | 1023 | let left = project(&self.a, self.d - self.c, self.c - self.a, dir); 1024 | let result = if left.x <= left.y { 1025 | left 1026 | } else { 1027 | project(&self.a, self.b - self.a, self.d - self.b, dir) 1028 | }; 1029 | result.map(|x| x.clamp(0.0, 1.0)) 1030 | } 1031 | 1032 | fn quads(&self, chunk_resolution: u32) -> impl Iterator + '_ { 1033 | let quad_resolution = chunk_resolution - 1; 1034 | (0..quad_resolution).flat_map(move |y| { 1035 | (0..quad_resolution).map(move |x| Quad::new(self, quad_resolution, [x, y].into())) 1036 | }) 1037 | } 1038 | 1039 | fn quads_within<'a>( 1040 | &'a self, 1041 | aabb: &Aabb, 1042 | chunk_resolution: u32, 1043 | ) -> impl Iterator + 'a { 1044 | self.quads_within_inner(aabb, chunk_resolution) 1045 | .into_iter() 1046 | .flatten() 1047 | } 1048 | 1049 | fn quads_within_inner<'a>( 1050 | &'a self, 1051 | aabb: &Aabb, 1052 | chunk_resolution: u32, 1053 | ) -> Option + 'a> { 1054 | let verts = aabb.vertices(); 1055 | let v0 = self.project(&verts[0].coords).coords; 1056 | let (lower, upper) = verts[1..] 1057 | .iter() 1058 | .map(|v| self.project(&v.coords).coords) 1059 | .fold((v0, v0), |(lower, upper), p| { 1060 | (lower.zip_map(&p, f64::min), upper.zip_map(&p, f64::max)) 1061 | }); 1062 | if lower.iter().any(|&v| v == 1.0) || upper.iter().any(|&v| v == 0.0) { 1063 | return None; 1064 | } 1065 | let quad_resolution = chunk_resolution - 1; 1066 | let discretize = |x: f64| ((x * quad_resolution as f64) as u32).min(quad_resolution - 1); 1067 | // FIXME: wrong units! Reuse bounding 1068 | let lower = lower.map(discretize); 1069 | let upper = upper.map(discretize); 1070 | Some((lower.y..=upper.y).flat_map(move |y| { 1071 | (lower.x..=upper.x).map(move |x| Quad::new(self, quad_resolution, [x, y].into())) 1072 | })) 1073 | } 1074 | 1075 | fn triangles<'a>( 1076 | &'a self, 1077 | radius: f64, 1078 | chunk_resolution: u32, 1079 | samples: &'a [f32], 1080 | ) -> impl Iterator + 'a { 1081 | self.quads(chunk_resolution).flat_map(move |quad| { 1082 | let index = quad.index(chunk_resolution); 1083 | quad.displace(radius, chunk_resolution, samples) 1084 | .triangles() 1085 | .into_iter() 1086 | .enumerate() 1087 | .map(move |(i, tri)| ((index << 1) | i as u32, tri)) 1088 | }) 1089 | } 1090 | } 1091 | 1092 | /// Identifies a pair of triangles within a patch 1093 | struct Quad { 1094 | /// Row-major order 1095 | corners: [na::Vector3; 4], 1096 | position: na::Point2, 1097 | } 1098 | 1099 | impl Quad { 1100 | fn new(patch: &Patch, resolution: u32, position: na::Point2) -> Self { 1101 | let offsets = [[0.0, 0.0], [1.0, 0.0], [0.0, 1.0], [1.0, 1.0]]; 1102 | Self { 1103 | corners: offsets.map(|x| { 1104 | patch 1105 | .get(&((position.cast::() + na::Vector2::from(x)) / f64::from(resolution))) 1106 | }), 1107 | position, 1108 | } 1109 | } 1110 | 1111 | fn index(&self, chunk_resolution: u32) -> u32 { 1112 | self.position.y * chunk_resolution + self.position.x 1113 | } 1114 | 1115 | fn displace(&self, radius: f64, chunk_resolution: u32, chunk_samples: &[f32]) -> DisplacedQuad { 1116 | let offsets = [[0, 0], [1, 0], [0, 1], [1, 1]]; 1117 | let mut result = self.corners; 1118 | for (v, offset) in result.iter_mut().zip(offsets) { 1119 | let sample = self.position + na::Vector2::from(offset); 1120 | let displacement = chunk_samples[(sample.y * chunk_resolution + sample.x) as usize]; 1121 | // We deliberately don't normalize `v` in `v * radius` because we're displacing a 1122 | // subdivided patch, not the surface of the sphere directly. 1123 | *v = *v * radius + v.normalize() * f64::from(displacement); 1124 | } 1125 | DisplacedQuad { 1126 | corners: result.map(na::Point3::from), 1127 | } 1128 | } 1129 | 1130 | fn triangles<'a>( 1131 | &'a self, 1132 | radius: f64, 1133 | chunk_resolution: u32, 1134 | samples: &'a [f32], 1135 | ) -> impl Iterator + 'a { 1136 | let index = self.index(chunk_resolution); 1137 | self.displace(radius, chunk_resolution, samples) 1138 | .triangles() 1139 | .into_iter() 1140 | .enumerate() 1141 | .map(move |(i, tri)| ((index << 1) | i as u32, tri)) 1142 | } 1143 | } 1144 | 1145 | struct DisplacedQuad { 1146 | /// Row-major order 1147 | corners: [na::Point3; 4], 1148 | } 1149 | 1150 | impl DisplacedQuad { 1151 | fn triangles(&self) -> [Triangle; 2] { 1152 | let [p0, p1, p2, p3] = self.corners; 1153 | [Triangle::new(p0, p1, p3), Triangle::new(p3, p2, p0)] 1154 | } 1155 | } 1156 | 1157 | /// Invoke `f` on the row-major corners of every quad in `patch` along `ray`, which much originate 1158 | /// within `patch`. Returns the patch edge reached and the ray toi at which the edge was reached, if 1159 | /// any. 1160 | /// 1161 | /// - `ray` must start within `patch` 1162 | /// - `f` returns whether to continue 1163 | fn walk_patch( 1164 | quad_resolution: u32, 1165 | patch: &Patch, 1166 | ray: &Ray, 1167 | max_toi: f64, 1168 | mut f: impl FnMut(&Quad) -> bool, 1169 | ) -> Option<(Edge, f64)> { 1170 | let quad_resolution_f = quad_resolution as f64; 1171 | let start = patch.project(&ray.origin.coords); 1172 | let mut quad = start.map(|x| { 1173 | (x * quad_resolution_f) 1174 | .trunc() 1175 | .clamp(0.0, quad_resolution_f - 1.0) 1176 | }); 1177 | loop { 1178 | let candidate = Quad::new(patch, quad_resolution, quad.map(|x| x as u32)); 1179 | if !f(&candidate) { 1180 | return None; 1181 | } 1182 | // Find the next quad along the ray 1183 | let Some((edge, toi)) = raycast_quad_edges(ray, &candidate.corners, max_toi) else { 1184 | return None; 1185 | }; 1186 | quad += edge.direction().into_inner(); 1187 | if quad.x >= quad_resolution_f 1188 | || quad.y >= quad_resolution_f 1189 | || quad.x < 0.0 1190 | || quad.y < 0.0 1191 | { 1192 | // Reached the edge of the patch 1193 | return Some((edge, toi)); 1194 | } 1195 | } 1196 | } 1197 | 1198 | #[cfg(test)] 1199 | mod tests { 1200 | use approx::{assert_abs_diff_eq, assert_relative_eq}; 1201 | use parry3d_f64::{query::ShapeCastStatus, shape::Ball}; 1202 | 1203 | use crate::cubemap::Face; 1204 | 1205 | use super::*; 1206 | 1207 | #[test] 1208 | fn triangles() { 1209 | let planet = Planet::new(Arc::new(FlatTerrain::new(1, 2)), 32, 1.0); 1210 | let coords = Coords { 1211 | x: 0, 1212 | y: 0, 1213 | face: Face::Pz, 1214 | }; 1215 | let samples = planet.sample(&coords); 1216 | let patch = Patch::new(&coords, planet.terrain.face_resolution()); 1217 | assert_eq!( 1218 | patch 1219 | .triangles(planet.radius, planet.chunk_resolution, &samples) 1220 | .count(), 1221 | 2 1222 | ); 1223 | let expected = 1.0 / 3.0f64.sqrt(); 1224 | for (_, tri) in patch.triangles(planet.radius, planet.chunk_resolution, &samples) { 1225 | assert!(tri.normal().unwrap().z > 0.0); 1226 | for vert in &[tri.a, tri.b, tri.c] { 1227 | assert!(vert.z > 0.0); 1228 | for coord in &vert.coords { 1229 | assert_eq!(coord.abs(), expected); 1230 | } 1231 | assert_relative_eq!(vert.coords.norm(), 1.0); 1232 | } 1233 | } 1234 | } 1235 | 1236 | fn ball_contacts(planet: &Planet, pos: Point, radius: Real) -> usize { 1237 | let dispatcher = PlanetDispatcher.chain(DefaultQueryDispatcher); 1238 | let ball = Ball { radius }; 1239 | let mut manifolds = Vec::>::new(); 1240 | let mut workspace = None; 1241 | dispatcher 1242 | .contact_manifolds( 1243 | &Isometry::translation(pos.x, pos.y, pos.z), 1244 | planet, 1245 | &ball, 1246 | 0.0, 1247 | &mut manifolds, 1248 | &mut workspace, 1249 | ) 1250 | .unwrap(); 1251 | manifolds.iter().map(|m| m.contacts().len()).sum() 1252 | } 1253 | 1254 | #[test] 1255 | fn end_to_end() { 1256 | const PLANET_RADIUS: f64 = 6371e3; 1257 | 1258 | let planet = Planet::new( 1259 | Arc::new(FlatTerrain::new(2u32.pow(12), 17)), 1260 | 32, 1261 | PLANET_RADIUS, 1262 | ); 1263 | 1264 | // We add 0.1 to PLANET_RADIUS in positive tests below to hack around the issue in 1265 | // https://github.com/dimforge/parry/pull/148. Can be removed once fix is released. 1266 | assert!(ball_contacts(&planet, Point::new(2.0, PLANET_RADIUS + 0.1, 0.0), 1.0) >= 2, 1267 | "a ball lying on an axis of a planet with an even number of chunks per face overlaps with at least four triangles"); 1268 | assert_eq!( 1269 | ball_contacts(&planet, Point::new(0.0, PLANET_RADIUS + 2.0, 0.0), 1.0), 1270 | 0 1271 | ); 1272 | assert!(ball_contacts(&planet, Point::new(-1.0, PLANET_RADIUS + 0.1, 0.0), 1.0) > 0); 1273 | 1274 | for i in 0..10 { 1275 | use std::f64; 1276 | let rot = na::UnitQuaternion::from_axis_angle( 1277 | &na::Vector3::z_axis(), 1278 | (i as f64 / 1000.0) * f64::consts::PI * 1e-4, 1279 | ); 1280 | let pos = Point::from(rot * na::Vector3::new(0.0, PLANET_RADIUS + 0.1, 0.0)); 1281 | assert!(ball_contacts(&planet, dbg!(pos), 1.0) > 0); 1282 | } 1283 | } 1284 | 1285 | // Ensure absence of a collision hole arising from mistakenly considering chunk centers *not* to 1286 | // be offset by 0.5 / face_resolution from edges of cubemap faces. 1287 | #[test] 1288 | fn coordinate_center_regression() { 1289 | const PLANET_RADIUS: f64 = 6371e3; 1290 | const BALL_RADIUS: f64 = 50.0; 1291 | let planet = Planet::new( 1292 | Arc::new(FlatTerrain::new(2u32.pow(12), 17)), 1293 | 32, 1294 | PLANET_RADIUS, 1295 | ); 1296 | 1297 | let pos = Point::from( 1298 | Vector::::new(-5_195_083.148, 3_582_099.812, -877_091.267).normalize() 1299 | * PLANET_RADIUS, 1300 | ); 1301 | 1302 | assert!(ball_contacts(&planet, pos, BALL_RADIUS) > 0); 1303 | } 1304 | 1305 | #[test] 1306 | fn cast_shape_smoke() { 1307 | const PLANET_RADIUS: f64 = 6371e3; 1308 | const DISTANCE: f64 = 10.0; 1309 | let ball = Ball { radius: 1.0 }; 1310 | let planet = Planet::new( 1311 | Arc::new(FlatTerrain::new(2u32.pow(12), 17)), 1312 | 32, 1313 | PLANET_RADIUS, 1314 | ); 1315 | 1316 | let impact = PlanetDispatcher 1317 | .cast_shapes( 1318 | &Isometry::translation(PLANET_RADIUS + DISTANCE, 0.0, 0.0), 1319 | &Vector::new(-1.0, 0.0, 0.0), 1320 | &planet, 1321 | &ball, 1322 | ShapeCastOptions { 1323 | max_time_of_impact: 100.0, 1324 | stop_at_penetration: false, 1325 | ..Default::default() 1326 | }, 1327 | ) 1328 | .unwrap() 1329 | .expect("toi not found"); 1330 | assert_eq!(impact.status, ShapeCastStatus::Converged); 1331 | assert_relative_eq!(impact.time_of_impact, DISTANCE - ball.radius); 1332 | assert_relative_eq!(impact.witness1, Point::new(PLANET_RADIUS, 0.0, 0.0)); 1333 | assert_relative_eq!(impact.witness2, Point::new(-ball.radius, 0.0, 0.0)); 1334 | assert_relative_eq!(impact.normal1, Vector::x_axis()); 1335 | assert_relative_eq!(impact.normal2, -Vector::x_axis()); 1336 | } 1337 | 1338 | #[test] 1339 | fn ray_direct() { 1340 | const PLANET_RADIUS: f64 = 6371e3; 1341 | const DISTANCE: f64 = 10.0; 1342 | let planet = Planet::new( 1343 | Arc::new(FlatTerrain::new(2u32.pow(12), 17)), 1344 | 32, 1345 | PLANET_RADIUS, 1346 | ); 1347 | let hit = planet 1348 | .cast_local_ray_and_get_normal( 1349 | &Ray { 1350 | origin: Point::new(PLANET_RADIUS + DISTANCE, 1.0, 1.0), 1351 | dir: -Vector::x(), 1352 | }, 1353 | 100.0, 1354 | true, 1355 | ) 1356 | .expect("hit not found"); 1357 | assert_relative_eq!(hit.time_of_impact, DISTANCE, epsilon = 1e-3); 1358 | assert_relative_eq!(hit.normal, Vector::x_axis(), epsilon = 1e-3); 1359 | 1360 | let hit = planet.cast_local_ray_and_get_normal( 1361 | &Ray { 1362 | origin: Point::new(PLANET_RADIUS + DISTANCE, 1.0, 1.0), 1363 | dir: Vector::x(), 1364 | }, 1365 | 100.0, 1366 | true, 1367 | ); 1368 | assert!(hit.is_none()); 1369 | } 1370 | 1371 | #[test] 1372 | fn ray_perp() { 1373 | const PLANET_RADIUS: f64 = 6371e3; 1374 | const DISTANCE: f64 = 10.0; 1375 | let planet = Planet::new( 1376 | Arc::new(FlatTerrain::new(2u32.pow(12), 17)), 1377 | 32, 1378 | PLANET_RADIUS, 1379 | ); 1380 | 1381 | for &dir in [Vector::x(), Vector::y(), -Vector::x(), -Vector::y()].iter() { 1382 | let hit = planet.cast_local_ray_and_get_normal( 1383 | &Ray { 1384 | origin: Point::new(1.0, 1.0, PLANET_RADIUS + DISTANCE), 1385 | dir, 1386 | }, 1387 | 10000.0, 1388 | true, 1389 | ); 1390 | assert!(hit.is_none()); 1391 | } 1392 | } 1393 | 1394 | #[test] 1395 | fn ray_glancing() { 1396 | const PLANET_RADIUS: f64 = 6371e3; 1397 | const DISTANCE: f64 = 1000.0; 1398 | let planet = Planet::new( 1399 | Arc::new(FlatTerrain::new(2u32.pow(12), 17)), 1400 | 32, 1401 | PLANET_RADIUS, 1402 | ); 1403 | planet 1404 | .cast_local_ray_and_get_normal( 1405 | &Ray { 1406 | origin: Point::new(1.0, 1.0, PLANET_RADIUS + DISTANCE), 1407 | dir: na::Vector3::new(1.5, 1.5, -1.0).normalize(), 1408 | }, 1409 | 1e5, 1410 | true, 1411 | ) 1412 | .expect("hit not found"); 1413 | } 1414 | 1415 | #[test] 1416 | fn intersects_smoke() { 1417 | const PLANET_RADIUS: f64 = 6371e3; 1418 | let ball = Ball { radius: 1.0 }; 1419 | let planet = Planet::new( 1420 | Arc::new(FlatTerrain::new(2u32.pow(12), 17)), 1421 | 32, 1422 | PLANET_RADIUS, 1423 | ); 1424 | 1425 | assert!(PlanetDispatcher 1426 | .intersection_test( 1427 | &Isometry::translation(PLANET_RADIUS, 0.0, 0.0), 1428 | &planet, 1429 | &ball, 1430 | ) 1431 | .unwrap()); 1432 | assert!(!PlanetDispatcher 1433 | .intersection_test( 1434 | &Isometry::translation(PLANET_RADIUS + ball.radius * 2.0, 0.0, 0.0), 1435 | &planet, 1436 | &ball, 1437 | ) 1438 | .unwrap()); 1439 | } 1440 | 1441 | #[test] 1442 | fn cast_shape_nonlinear_smoke() { 1443 | const PLANET_RADIUS: f64 = 6371e3; 1444 | let ball = Ball { radius: 1.0 }; 1445 | let planet = Planet::new( 1446 | Arc::new(FlatTerrain::new(2u32.pow(12), 17)), 1447 | 32, 1448 | PLANET_RADIUS, 1449 | ); 1450 | 1451 | let toi = PlanetDispatcher 1452 | .cast_shapes_nonlinear( 1453 | &NonlinearRigidMotion::constant_position(na::one()), 1454 | &planet, 1455 | &NonlinearRigidMotion { 1456 | start: Isometry::translation(PLANET_RADIUS + ball.radius + 0.5, 0.0, 0.0), 1457 | local_center: na::Point3::origin(), 1458 | linvel: -na::Vector3::x(), 1459 | angvel: na::zero(), 1460 | }, 1461 | &ball, 1462 | 0.0, 1463 | 1.0, 1464 | true, 1465 | ) 1466 | .unwrap() 1467 | .expect("no hit"); 1468 | assert_eq!(toi.status, ShapeCastStatus::Converged); 1469 | assert_relative_eq!(toi.time_of_impact, 0.5); 1470 | assert_relative_eq!(toi.witness1, na::Point3::new(PLANET_RADIUS, 0.0, 0.0)); 1471 | assert_relative_eq!(toi.witness2, na::Point3::new(-ball.radius, 0.0, 0.0)); 1472 | assert_relative_eq!(toi.normal1, na::Vector3::x_axis()); 1473 | assert_relative_eq!(toi.normal2, -na::Vector3::x_axis()); 1474 | 1475 | // Same configuration as above, but too far to hit within the allotted time 1476 | let toi = PlanetDispatcher 1477 | .cast_shapes_nonlinear( 1478 | &NonlinearRigidMotion::constant_position(na::one()), 1479 | &planet, 1480 | &NonlinearRigidMotion { 1481 | start: Isometry::translation(PLANET_RADIUS + ball.radius + 1.5, 0.0, 0.0), 1482 | local_center: na::Point3::origin(), 1483 | linvel: -na::Vector3::x(), 1484 | angvel: na::zero(), 1485 | }, 1486 | &ball, 1487 | 0.0, 1488 | 1.0, 1489 | true, 1490 | ) 1491 | .unwrap(); 1492 | assert!(toi.is_none()); 1493 | } 1494 | 1495 | #[test] 1496 | fn patch_interpolation() { 1497 | let res = 2u32.pow(12); 1498 | let chunk = Coords { 1499 | x: 12, 1500 | y: 47, 1501 | face: Face::Px, 1502 | }; 1503 | let patch = Patch::new(&chunk, res); 1504 | 1505 | // Verify that the corners are consistent. All other points won't be, since 1506 | // `Coords::direction` interpolates on the sphere. 1507 | for coords in [[0.0, 0.0], [0.0, 1.0], [1.0, 0.0], [1.0, 1.0]] { 1508 | // Exact equality is intended here, as a prerequisite for neighboring patches to be 1509 | // seamless. 1510 | assert_eq!( 1511 | patch.get(&coords.into()), 1512 | chunk.direction(res, &coords.into()).into_inner() 1513 | ); 1514 | } 1515 | } 1516 | 1517 | #[test] 1518 | fn patch_projection() { 1519 | let res = 2u32.pow(12); 1520 | let chunk = Coords { 1521 | x: 12, 1522 | y: 47, 1523 | face: Face::Px, 1524 | }; 1525 | let patch = Patch::new(&chunk, res); 1526 | 1527 | let coords = [0.1, 0.4].into(); 1528 | assert_abs_diff_eq!(patch.project(&patch.get(&coords)), coords, epsilon = 1e-4); 1529 | 1530 | let coords = [0.9, 0.7].into(); 1531 | assert_abs_diff_eq!(patch.project(&patch.get(&coords)), coords, epsilon = 1e-4); 1532 | } 1533 | 1534 | #[test] 1535 | fn patch_projection_2() { 1536 | // Regression test for a case that needs the second quadratic solution 1537 | let patch = Patch { 1538 | a: [1.0, 1.1, 0.0].into(), 1539 | b: [1.0, 1.2, 1.3].into(), 1540 | c: [1.0, -0.1, -0.1].into(), 1541 | d: [1.0, 0.0, 1.0].into(), 1542 | }; 1543 | 1544 | let p = patch.project(&na::Vector3::new(1.0, 0.1, 0.1)); 1545 | assert!(p.x >= 0.0 && p.x <= 1.0); 1546 | assert!(p.y >= 0.0 && p.y <= 1.0); 1547 | } 1548 | 1549 | #[test] 1550 | fn patch_projection_3() { 1551 | // Regression test for a case that needs the second quadratic solution 1552 | let patch = Patch { 1553 | a: [1.0, 2.0, 1.0].into(), 1554 | b: [1.0, 2.0, 2.0].into(), 1555 | c: [1.0, 1.0, 1.0].into(), 1556 | d: [1.0, 1.0, 2.0].into(), 1557 | }; 1558 | 1559 | assert_abs_diff_eq!( 1560 | patch.project(&na::Vector3::new(1.0, 1.1, 1.1)), 1561 | na::Point2::new(0.1, 0.9) 1562 | ); 1563 | assert_abs_diff_eq!( 1564 | patch.get(&na::Point2::new(0.1, 0.9)), 1565 | na::Vector3::new(1.0, 1.1, 1.1) 1566 | ); 1567 | } 1568 | 1569 | #[test] 1570 | fn patch_raycast() { 1571 | const RESOLUTION: u32 = 2; 1572 | const Z: f64 = 1e6; 1573 | // RESOLUTION x RESOLUTION square at z=1 1574 | let patch = Patch { 1575 | a: [0.0, 0.0, Z].into(), 1576 | b: [RESOLUTION as f64, 0.0, Z].into(), 1577 | c: [0.0, RESOLUTION as f64, Z].into(), 1578 | d: [RESOLUTION as f64, RESOLUTION as f64, Z].into(), 1579 | }; 1580 | 1581 | let check_ray = |origin, direction, expected_quads: &[[u32; 2]], expected_edge| { 1582 | let mut i = 0; 1583 | let result = walk_patch( 1584 | RESOLUTION, 1585 | &patch, 1586 | &Ray::new(origin, direction), 1587 | 100.0, 1588 | |quad| { 1589 | let expected = [[0.0, 0.0], [1.0, 0.0], [0.0, 1.0], [1.0, 1.0]].map(|offset| { 1590 | (na::Vector2::from(expected_quads[i]).cast::() 1591 | + na::Vector2::from(offset)) 1592 | .push(Z) 1593 | }); 1594 | i += 1; 1595 | for (actual, expected) in quad.corners.into_iter().zip(&expected) { 1596 | assert_abs_diff_eq!(actual, expected); 1597 | } 1598 | true 1599 | }, 1600 | ); 1601 | assert_eq!(result.map(|x| x.0), expected_edge); 1602 | }; 1603 | 1604 | check_ray( 1605 | [0.5, 0.5, Z].into(), 1606 | [1.0, 0.0, 0.0].into(), 1607 | &[[0, 0], [1, 0]], 1608 | Some(Edge::Px), 1609 | ); 1610 | check_ray( 1611 | [0.5, 0.5, Z].into(), 1612 | [-1.0, 0.0, 0.0].into(), 1613 | &[[0, 0]], 1614 | Some(Edge::Nx), 1615 | ); 1616 | check_ray( 1617 | [1.5, 0.5, Z].into(), 1618 | [-1.0, 0.0, 0.0].into(), 1619 | &[[1, 0], [0, 0]], 1620 | Some(Edge::Nx), 1621 | ); 1622 | check_ray( 1623 | [1.5, 1.5, Z].into(), 1624 | [-1.0, 0.0, 0.0].into(), 1625 | &[[1, 1], [0, 1]], 1626 | Some(Edge::Nx), 1627 | ); 1628 | check_ray( 1629 | [1.5, 1.5, Z].into(), 1630 | [0.0, 1.0, 0.0].into(), 1631 | &[[1, 1]], 1632 | Some(Edge::Py), 1633 | ); 1634 | check_ray( 1635 | [1.5, 1.5, Z].into(), 1636 | [0.0, -1.0, 0.0].into(), 1637 | &[[1, 1], [1, 0]], 1638 | Some(Edge::Ny), 1639 | ); 1640 | } 1641 | } 1642 | --------------------------------------------------------------------------------