├── LICENSE ├── README.md └── src ├── AStar.cpp ├── AStar.hpp ├── Coord.hpp ├── JPS.cpp ├── JPS.hpp ├── JPSplus.cpp ├── JPSplus.hpp ├── PathFinder.cpp ├── PathFinder.hpp ├── WallTracing.cpp ├── WallTracing.hpp └── geometry.hpp /LICENSE: -------------------------------------------------------------------------------- 1 | MIT License 2 | 3 | Copyright (c) 2018 Ayrat Shaikhov 4 | 5 | Permission is hereby granted, free of charge, to any person obtaining a copy 6 | of this software and associated documentation files (the "Software"), to deal 7 | in the Software without restriction, including without limitation the rights 8 | to use, copy, modify, merge, publish, distribute, sublicense, and/or sell 9 | copies of the Software, and to permit persons to whom the Software is 10 | furnished to do so, subject to the following conditions: 11 | 12 | The above copyright notice and this permission notice shall be included in all 13 | copies or substantial portions of the Software. 14 | 15 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 16 | IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 17 | FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 18 | AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 19 | LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, 20 | OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE 21 | SOFTWARE. 22 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # rts-path-finding 2 | C++ implementation of path finding algorithms for RTS-games with grid map. 3 | 4 | ## How to use 5 | 6 | This code is used in my RTS-like game with custom in-house engine. I copied it as is, without implementation of many classes, like vec2, Array, MemoryManager etc. So if somebody wants to use it, he needs to replace them. 7 | 8 | Algorithms work on grid map with maximum side size 2^16. If cell is not walkable there is a wall. Also there is a possibility to add round obstacles with arbitrary radius. Borders of the input map must be unwalkable because of there is no check if coordinates are out of borders for optimization reasons. 9 | 10 | There are 2 phases of finding path: 11 | 1. Rough - position of joints restricted to center of cells. Path can be found using one of 3 algorithms: A*, JPS or JSP+. Obstables have no influence on resulting path. 12 | 2. Precise - works in continuous space using "Wall Tracing" 13 | 14 | That scheme was gotten from game "Dota 2". First you search rough path to destination point, then precise path from current position to some point on rough path. After a while when distance to that point become short enough, you search precise path to another point on rough path. 15 | 16 | Unlike many other implementations of A* and JPS, that can find closest path to some unreachable location. JPS uses improved algorithm that doesn't cut edges and was published in 2012 (http://harabor.net/data/papers/harabor-grastien-socs12.pdf). JPS+ cache distances to jump points. 17 | 18 | Search time to unreachable locations with grid size 256x256 and low number of walls: 19 | 20 | Name | Time 21 | -----|----- 22 | A* | ~26 ms 23 | JSP | ~7.5 ms 24 | JPS+ | ~0.5 ms 25 | 26 | ## Wall Tracing 27 | 28 | The most hard part is "Wall Tracing" algorithm. I was impressed with "Dota 2" path finding system and started to search how to realize it. But everything I found was only 1 mention [here](http://liquipedia.net/dota2/Pathfinding) without any explanation. So I started to invent it myself. 29 | 30 | This implementation doesn't use trigonometric functions and works very fast for short distances (< 0.2 ms). Found paths are not guaranteed to be shortest, but in most cases looks good. 31 | -------------------------------------------------------------------------------- /src/AStar.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "AStar.hpp" 3 | #include "PathFinder.hpp" 4 | #include "Map.hpp" 5 | 6 | using namespace nook; 7 | 8 | AStar::AStar() { 9 | m_size = map()->size(); 10 | 11 | int count = m_size.width * m_size.height; 12 | m_maxIter = m_size.width * 4; 13 | m_map = memoryManager().allocOnStack(count); 14 | m_queue.init(m_maxIter, memoryManager().allocOnStack::Item>(m_maxIter)); 15 | m_cameFrom.init(count, memoryManager().allocOnStack(count)); 16 | m_cost.init(count, memoryManager().allocOnStack(count)); 17 | m_cameFrom.setCount(count); 18 | m_cost.setCount(count); 19 | 20 | for (int y = 0; y < m_size.height; y++) 21 | for (int x = 0; x < m_size.width; x++) 22 | m_map[pathFinder().index(x, y)] = map()->getCell(x, y)->walkable; 23 | } 24 | 25 | void AStar::find(Coord start, Coord end, Array& path) { 26 | Point2 neighbours[] = { 27 | { -1, 0 }, 28 | { 0, -1 }, 29 | { 1, 0 }, 30 | { 0, 1 } 31 | }; 32 | 33 | m_queue.clear(); 34 | std::memset(m_cost.buf(), 0, m_cost.count() * sizeof(int)); 35 | 36 | m_queue.insert(start, 0); 37 | int startIdx = pathFinder().index(start.x, start.y); 38 | m_cameFrom[startIdx] = start; 39 | m_cost[startIdx] = 0; 40 | 41 | Coord bestCoord = start; 42 | U32 nearest = pathFinder().heuristic(start, end); 43 | int iter = 0; 44 | 45 | while (m_queue.count()) { 46 | Coord cur = m_queue.pop(); 47 | int curIdx = pathFinder().index(cur.x, cur.y); 48 | iter++; 49 | 50 | if (cur == end) { 51 | bestCoord = end; 52 | break; 53 | } 54 | if (iter == m_maxIter) 55 | break; 56 | 57 | for (Point2 n : neighbours) { 58 | Coord next(cur.x + n.x, cur.y + n.y); 59 | int nextIdx = pathFinder().index(next.x, next.y); 60 | if (m_map[nextIdx] && !m_cost[nextIdx]) { 61 | U32 cost = m_cost[curIdx] + 1; 62 | m_cost[nextIdx] = cost; 63 | U32 dist = pathFinder().heuristic(next, end); 64 | if (dist < nearest) { 65 | nearest = dist; 66 | bestCoord = next; 67 | } 68 | U32 priority = cost + dist; 69 | m_queue.insert(next, priority); 70 | m_cameFrom[nextIdx] = cur; 71 | } 72 | } 73 | } 74 | 75 | // Create Path 76 | while (bestCoord != start) { 77 | path.push(map()->getPos(bestCoord.point())); 78 | bestCoord = m_cameFrom[pathFinder().index(bestCoord.x, bestCoord.y)]; 79 | } 80 | path.push(map()->getPos(start.point())); 81 | } 82 | -------------------------------------------------------------------------------- /src/AStar.hpp: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | 4 | #include "Coord.hpp" 5 | 6 | class AStar { 7 | public: 8 | AStar(); 9 | 10 | void find(Coord start, Coord end, nook::Array& path); 11 | 12 | private: 13 | enum Direction { 14 | NONE = 0, 15 | NORTH = 1, 16 | SOUTH = 2, 17 | EAST = 4, 18 | WEST = 8, 19 | NORTHEAST = 16, 20 | NORTHWEST = 32, 21 | SOUTHEAST = 64, 22 | SOUTHWEST = 128 23 | }; 24 | 25 | nook::Size m_size; 26 | bool* m_map; 27 | nook::U32 m_maxIter; 28 | nook::PriorityQueue m_queue; 29 | nook::Array m_cameFrom; 30 | nook::Array m_cost; 31 | }; 32 | -------------------------------------------------------------------------------- /src/Coord.hpp: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | 4 | #include "misc/Common.hpp" 5 | 6 | struct Coord { 7 | nook::U16 x, y; 8 | 9 | Coord() = default; 10 | Coord(nook::U16 x, nook::U16 y) : x(x), y(y) {} 11 | bool operator==(Coord c) { return x == c.x && y == c.y; } 12 | bool operator!=(Coord c) { return x != c.x || y != c.y; } 13 | 14 | nook::Point2 point() const { return nook::Point2(x, y); } 15 | }; 16 | -------------------------------------------------------------------------------- /src/JPS.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "JPS.hpp" 3 | #include "PathFinder.hpp" 4 | #include "Map.hpp" 5 | 6 | using namespace nook; 7 | 8 | JPS::JPS() { 9 | m_size = map()->size(); 10 | 11 | int count = m_size.width * m_size.height; 12 | int queueSize = m_size.width; 13 | m_map = memoryManager().allocOnStack(count); 14 | m_queue.init(queueSize, memoryManager().allocOnStack::Item>(queueSize)); 15 | m_cameFrom.init(count, memoryManager().allocOnStack(count)); 16 | m_cost.init(count, memoryManager().allocOnStack(count)); 17 | m_cameFrom.setCount(count); 18 | m_cost.setCount(count); 19 | 20 | for (int y = 0; y < m_size.height; y++) 21 | for (int x = 0; x < m_size.width; x++) 22 | m_map[pathFinder().index(x, y)] = map()->getCell(x, y)->walkable; 23 | } 24 | 25 | void JPS::find(Coord start, Coord end, Array& path) { 26 | m_queue.clear(); 27 | std::memset(m_cost.buf(), 0xff, m_cost.count() * sizeof(int)); 28 | 29 | if (start == end) { 30 | path.push(map()->getPos(start.point())); 31 | return; 32 | } 33 | 34 | int startIdx = pathFinder().index(start.x, start.y); 35 | m_cameFrom[startIdx] = start; 36 | m_cost[startIdx] = 0; 37 | 38 | m_best = start; 39 | m_bestC = 0; 40 | m_bestH = pathFinder().heuristic(start, end); 41 | int iter = 0; 42 | 43 | jumpN(start, end); 44 | jumpS(start, end); 45 | jumpW(start, end); 46 | jumpE(start, end); 47 | jumpNW(start, end); 48 | jumpNE(start, end); 49 | jumpSW(start, end); 50 | jumpSE(start, end); 51 | 52 | while (m_queue.count()) { 53 | Coord cur = m_queue.pop(); 54 | if (cur == end) { 55 | m_best = end; 56 | break; 57 | } 58 | 59 | int ci = pathFinder().index(cur.x, cur.y); 60 | iter++; 61 | 62 | Coord from = m_cameFrom[ci]; 63 | int bi = ci - m_size.width; 64 | int ti = ci + m_size.width; 65 | 66 | if (cur.y == from.y) { 67 | if (cur.x > from.x) { 68 | if (m_map[bi] && !m_map[bi - 1]) { 69 | jumpS(cur, end); 70 | jumpSE(cur, end); 71 | } 72 | if (m_map[ti] && !m_map[ti - 1]) { 73 | jumpN(cur, end); 74 | jumpNE(cur, end); 75 | } 76 | jumpE(cur, end); 77 | } 78 | else { 79 | if (m_map[bi] && !m_map[bi + 1]) { 80 | jumpS(cur, end); 81 | jumpSW(cur, end); 82 | } 83 | if (m_map[ti] && !m_map[ti + 1]) { 84 | jumpN(cur, end); 85 | jumpNW(cur, end); 86 | } 87 | jumpW(cur, end); 88 | } 89 | } 90 | else if (cur.y < from.y) { 91 | if (cur.x == from.x) { 92 | if (m_map[ci - 1] && !m_map[ti - 1]) { 93 | jumpW(cur, end); 94 | jumpSW(cur, end); 95 | } 96 | if (m_map[ci + 1] && !m_map[ti + 1]) { 97 | jumpE(cur, end); 98 | jumpSE(cur, end); 99 | } 100 | jumpS(cur, end); 101 | } 102 | else if (cur.x > from.x) 103 | jumpSE(cur, end); 104 | else 105 | jumpSW(cur, end); 106 | } 107 | else { // cur.y > from.y 108 | if (cur.x == from.x) { 109 | if (m_map[ci - 1] && !m_map[bi - 1]) { 110 | jumpW(cur, end); 111 | jumpNW(cur, end); 112 | } 113 | if (m_map[ci + 1] && !m_map[bi + 1]) { 114 | jumpE(cur, end); 115 | jumpNE(cur, end); 116 | } 117 | jumpN(cur, end); 118 | } 119 | else if (cur.x > from.x) 120 | jumpNE(cur, end); 121 | else 122 | jumpNW(cur, end); 123 | } 124 | } 125 | 126 | // Create Path 127 | Coord c = m_best; 128 | while (c != start) { 129 | path.push(map()->getPos(c.point())); 130 | c = m_cameFrom[pathFinder().index(c.x, c.y)]; 131 | } 132 | path.push(map()->getPos(start.point())); 133 | } 134 | 135 | int JPS::jumpN(Coord from, Coord goal) { 136 | int dist = 0; 137 | int fromi = pathFinder().index(from.x, from.y); 138 | int curi = fromi; 139 | int nexti = curi + m_size.width; 140 | Coord next(from.x, from.y + 1); 141 | 142 | while (true) { 143 | if (!m_map[nexti]) 144 | break; 145 | 146 | dist++; 147 | U32 cost = m_cost[fromi] + dist; 148 | 149 | if (next == goal) { 150 | if (cost < m_cost[nexti]) { 151 | m_queue.insert(next, cost); 152 | m_cameFrom[nexti] = from; 153 | m_cost[nexti] = cost; 154 | } 155 | return dist; 156 | } 157 | 158 | U32 h = pathFinder().heuristic(next, goal); 159 | if (h < m_bestH || (h == m_bestH && cost < m_bestC)) { 160 | m_bestH = h; 161 | m_bestC = cost; 162 | m_best = next; 163 | m_cameFrom[nexti] = from; 164 | } 165 | 166 | if ((m_map[nexti + 1] && !m_map[curi + 1]) || (m_map[nexti - 1] && !m_map[curi - 1])) { 167 | if (cost < m_cost[nexti]) { 168 | m_queue.insert(next, h + cost); 169 | m_cameFrom[nexti] = from; 170 | m_cost[nexti] = cost; 171 | } 172 | return dist; 173 | } 174 | 175 | curi = nexti; 176 | nexti += m_size.width; 177 | next.y++; 178 | } 179 | return dist; 180 | } 181 | 182 | int JPS::jumpE(Coord from, Coord goal) { 183 | int dist = 0; 184 | int fromi = pathFinder().index(from.x, from.y); 185 | int curi = fromi; 186 | int nexti = curi + 1; 187 | Coord next(from.x + 1, from.y); 188 | 189 | while (true) { 190 | if (!m_map[nexti]) 191 | break; 192 | 193 | dist++; 194 | U32 cost = m_cost[fromi] + dist; 195 | 196 | if (next == goal) { 197 | if (cost < m_cost[nexti]) { 198 | m_queue.insert(next, cost); 199 | m_cameFrom[nexti] = from; 200 | m_cost[nexti] = cost; 201 | } 202 | return dist; 203 | } 204 | 205 | U32 h = pathFinder().heuristic(next, goal); 206 | if (h < m_bestH || (h == m_bestH && cost < m_bestC)) { 207 | m_bestH = h; 208 | m_bestC = cost; 209 | m_best = next; 210 | m_cameFrom[nexti] = from; 211 | } 212 | 213 | if ((m_map[nexti + m_size.width] && !m_map[curi + m_size.width]) || 214 | (m_map[nexti - m_size.width] && !m_map[curi - m_size.width])) { 215 | if (cost < m_cost[nexti]) { 216 | m_queue.insert(next, h + cost); 217 | m_cameFrom[nexti] = from; 218 | m_cost[nexti] = cost; 219 | } 220 | return dist; 221 | } 222 | 223 | curi = nexti; 224 | nexti++; 225 | next.x++; 226 | } 227 | return dist; 228 | } 229 | 230 | int JPS::jumpS(Coord from, Coord goal) { 231 | int dist = 0; 232 | int fromi = pathFinder().index(from.x, from.y); 233 | int curi = fromi; 234 | int nexti = curi - m_size.width; 235 | Coord next(from.x, from.y - 1); 236 | 237 | while (true) { 238 | if (!m_map[nexti]) 239 | break; 240 | 241 | dist++; 242 | U32 cost = m_cost[fromi] + dist; 243 | 244 | if (next == goal) { 245 | if (cost < m_cost[nexti]) { 246 | m_queue.insert(next, cost); 247 | m_cameFrom[nexti] = from; 248 | m_cost[nexti] = cost; 249 | } 250 | return dist; 251 | } 252 | 253 | U32 h = pathFinder().heuristic(next, goal); 254 | if (h < m_bestH || (h == m_bestH && cost < m_bestC)) { 255 | m_bestH = h; 256 | m_bestC = cost; 257 | m_best = next; 258 | m_cameFrom[nexti] = from; 259 | } 260 | 261 | if ((m_map[nexti + 1] && !m_map[curi + 1]) || (m_map[nexti - 1] && !m_map[curi - 1])) { 262 | if (cost < m_cost[nexti]) { 263 | m_queue.insert(next, h + cost); 264 | m_cameFrom[nexti] = from; 265 | m_cost[nexti] = cost; 266 | } 267 | return dist; 268 | } 269 | 270 | curi = nexti; 271 | nexti -= m_size.width; 272 | next.y--; 273 | } 274 | return dist; 275 | } 276 | 277 | int JPS::jumpW(Coord from, Coord goal) { 278 | int dist = 0; 279 | int fromi = pathFinder().index(from.x, from.y); 280 | int curi = fromi; 281 | int nexti = curi - 1; 282 | Coord next(from.x - 1, from.y); 283 | 284 | while (true) { 285 | if (!m_map[nexti]) 286 | break; 287 | 288 | dist++; 289 | U32 cost = m_cost[fromi] + dist; 290 | 291 | if (next == goal) { 292 | if (cost < m_cost[nexti]) { 293 | m_queue.insert(next, cost); 294 | m_cameFrom[nexti] = from; 295 | m_cost[nexti] = cost; 296 | } 297 | return dist; 298 | } 299 | 300 | U32 h = pathFinder().heuristic(next, goal); 301 | if (h < m_bestH || (h == m_bestH && cost < m_bestC)) { 302 | m_bestH = h; 303 | m_bestC = cost; 304 | m_best = next; 305 | m_cameFrom[nexti] = from; 306 | } 307 | 308 | if ((m_map[nexti + m_size.width] && !m_map[curi + m_size.width]) || 309 | (m_map[nexti - m_size.width] && !m_map[curi - m_size.width])) { 310 | if (cost < m_cost[nexti]) { 311 | m_queue.insert(next, h + cost); 312 | m_cameFrom[nexti] = from; 313 | m_cost[nexti] = cost; 314 | } 315 | return dist; 316 | } 317 | 318 | curi = nexti; 319 | nexti--; 320 | next.x--; 321 | } 322 | return dist; 323 | } 324 | 325 | void JPS::jumpNE(Coord from, Coord goal) { 326 | int fromi = pathFinder().index(from.x, from.y); 327 | int curi = fromi; 328 | int nexti = curi + m_size.width + 1; 329 | Coord next(from.x + 1, from.y + 1); 330 | U32 cost = m_cost[fromi] + 1; 331 | 332 | if (!m_map[curi + 1] || !m_map[nexti - 1]) 333 | return; 334 | 335 | while (true) { 336 | if (!m_map[nexti]) 337 | break; 338 | 339 | if (cost < m_cost[nexti]) { 340 | m_cost[nexti] = cost; 341 | m_cameFrom[nexti] = from; 342 | 343 | U32 h = pathFinder().heuristic(next, goal); 344 | if (h < m_bestH || (h == m_bestH && cost < m_bestC)) { 345 | m_bestH = h; 346 | m_bestC = cost; 347 | m_best = next; 348 | } 349 | } 350 | 351 | if (next == goal) { 352 | if (cost == m_cost[nexti]) 353 | m_queue.insert(next, cost); 354 | break; 355 | } 356 | 357 | bool n = jumpN(next, goal); 358 | bool e = jumpE(next, goal); 359 | if (!(n & e)) 360 | break; 361 | 362 | curi = nexti; 363 | nexti += m_size.width + 1; 364 | next = Coord(next.x + 1, next.y + 1); 365 | cost++; 366 | } 367 | } 368 | 369 | void JPS::jumpSE(Coord from, Coord goal) { 370 | int fromi = pathFinder().index(from.x, from.y); 371 | int curi = fromi; 372 | int nexti = curi - m_size.width + 1; 373 | Coord next(from.x + 1, from.y - 1); 374 | U32 cost = m_cost[fromi] + 1; 375 | 376 | if (!m_map[curi + 1] || !m_map[nexti - 1]) 377 | return; 378 | 379 | while (true) { 380 | if (!m_map[nexti]) 381 | break; 382 | 383 | if (cost < m_cost[nexti]) { 384 | m_cost[nexti] = cost; 385 | m_cameFrom[nexti] = from; 386 | 387 | U32 h = pathFinder().heuristic(next, goal); 388 | if (h < m_bestH || (h == m_bestH && cost < m_bestC)) { 389 | m_bestH = h; 390 | m_bestC = cost; 391 | m_best = next; 392 | } 393 | } 394 | 395 | if (next == goal) { 396 | if (cost == m_cost[nexti]) 397 | m_queue.insert(next, cost); 398 | break; 399 | } 400 | 401 | bool s = jumpS(next, goal); 402 | bool e = jumpE(next, goal); 403 | if (!(s & e)) 404 | break; 405 | 406 | curi = nexti; 407 | nexti -= m_size.width - 1; 408 | next = Coord(next.x + 1, next.y - 1); 409 | cost++; 410 | } 411 | } 412 | 413 | void JPS::jumpSW(Coord from, Coord goal) { 414 | int fromi = pathFinder().index(from.x, from.y); 415 | int curi = fromi; 416 | int nexti = curi - m_size.width - 1; 417 | Coord next(from.x - 1, from.y - 1); 418 | U32 cost = m_cost[fromi] + 1; 419 | 420 | if (!m_map[curi - 1] || !m_map[nexti + 1]) 421 | return; 422 | 423 | while (true) { 424 | if (!m_map[nexti]) 425 | break; 426 | 427 | if (cost < m_cost[nexti]) { 428 | m_cost[nexti] = cost; 429 | m_cameFrom[nexti] = from; 430 | 431 | U32 h = pathFinder().heuristic(next, goal); 432 | if (h < m_bestH || (h == m_bestH && cost < m_bestC)) { 433 | m_bestH = h; 434 | m_bestC = cost; 435 | m_best = next; 436 | } 437 | } 438 | 439 | if (next == goal) { 440 | if (cost == m_cost[nexti]) 441 | m_queue.insert(next, cost); 442 | break; 443 | } 444 | 445 | bool s = jumpS(next, goal); 446 | bool w = jumpW(next, goal); 447 | if (!(s & w)) 448 | break; 449 | 450 | curi = nexti; 451 | nexti -= m_size.width + 1; 452 | next = Coord(next.x - 1, next.y - 1); 453 | cost++; 454 | } 455 | } 456 | 457 | void JPS::jumpNW(Coord from, Coord goal) { 458 | int fromi = pathFinder().index(from.x, from.y); 459 | int curi = fromi; 460 | int nexti = curi + m_size.width - 1; 461 | Coord next(from.x - 1, from.y + 1); 462 | U32 cost = m_cost[fromi] + 1; 463 | 464 | if (!m_map[curi - 1] || !m_map[nexti + 1]) 465 | return; 466 | 467 | while (true) { 468 | if (!m_map[nexti]) 469 | break; 470 | 471 | if (cost < m_cost[nexti]) { 472 | m_cost[nexti] = cost; 473 | m_cameFrom[nexti] = from; 474 | 475 | U32 h = pathFinder().heuristic(next, goal); 476 | if (h < m_bestH || (h == m_bestH && cost < m_bestC)) { 477 | m_bestH = h; 478 | m_bestC = cost; 479 | m_best = next; 480 | } 481 | } 482 | 483 | if (next == goal) { 484 | if (cost == m_cost[nexti]) 485 | m_queue.insert(next, cost); 486 | break; 487 | } 488 | 489 | bool n = jumpN(next, goal); 490 | bool w = jumpW(next, goal); 491 | if (!(n & w)) 492 | break; 493 | 494 | curi = nexti; 495 | nexti += m_size.width - 1; 496 | next = Coord(next.x - 1, next.y + 1); 497 | cost++; 498 | } 499 | } 500 | -------------------------------------------------------------------------------- /src/JPS.hpp: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | 4 | #include "Coord.hpp" 5 | 6 | class JPS { 7 | public: 8 | JPS(); 9 | void find(Coord start, Coord end, nook::Array& path); 10 | 11 | private: 12 | int jumpN(Coord from, Coord goal); 13 | int jumpS(Coord from, Coord goal); 14 | int jumpW(Coord from, Coord goal); 15 | int jumpE(Coord from, Coord goal); 16 | void jumpNW(Coord from, Coord goal); 17 | void jumpNE(Coord from, Coord goal); 18 | void jumpSW(Coord from, Coord goal); 19 | void jumpSE(Coord from, Coord goal); 20 | 21 | nook::Size m_size; 22 | bool* m_map; 23 | nook::PriorityQueue m_queue; 24 | nook::Array m_cameFrom; 25 | nook::Array m_cost; 26 | 27 | Coord m_best; 28 | nook::U32 m_bestC; 29 | nook::U32 m_bestH; 30 | }; 31 | -------------------------------------------------------------------------------- /src/JPSplus.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "JPSplus.hpp" 3 | #include "PathFinder.hpp" 4 | #include "Map.hpp" 5 | 6 | using namespace nook; 7 | 8 | JPSplus::JPSplus() { 9 | m_size = map()->size(); 10 | 11 | int count = m_size.width * m_size.height; 12 | int queueSize = m_size.width; 13 | m_map = memoryManager().allocOnStack(count); 14 | m_jps = memoryManager().allocOnStack(count); 15 | m_queue.init(queueSize, memoryManager().allocOnStack::Item>(queueSize)); 16 | m_cameFrom.init(count, memoryManager().allocOnStack(count)); 17 | m_cost.init(count, memoryManager().allocOnStack(count)); 18 | m_cameFrom.setCount(count); 19 | m_cost.setCount(count); 20 | 21 | update(); 22 | } 23 | 24 | void JPSplus::update() { 25 | for (int y = 0; y < m_size.height; y++) 26 | for (int x = 0; x < m_size.width; x++) 27 | m_map[pathFinder().index(x, y)] = map()->getCell(x, y)->walkable; 28 | 29 | for (int y = 1; y < m_size.height - 1; y++) 30 | for (int x = 1; x < m_size.width - 1; x++) { 31 | JP& jp = m_jps[pathFinder().index(x, y)]; 32 | Coord c(x, y); 33 | jp.n = getJumpN(c); 34 | jp.e = getJumpE(c); 35 | jp.s = getJumpS(c); 36 | jp.w = getJumpW(c); 37 | jp.ne = getJumpNE(c); 38 | jp.se = getJumpSE(c); 39 | jp.sw = getJumpSW(c); 40 | jp.nw = getJumpNW(c); 41 | } 42 | } 43 | 44 | void JPSplus::find(Coord start, Coord end, nook::Array& path) { 45 | m_queue.clear(); 46 | std::memset(m_cost.buf(), 0xff, m_cost.count() * sizeof(int)); 47 | 48 | if (start == end) { 49 | path.push(map()->getPos(start.point())); 50 | return; 51 | } 52 | 53 | int startIdx = pathFinder().index(start.x, start.y); 54 | m_cameFrom[startIdx] = start; 55 | m_cost[startIdx] = 0; 56 | 57 | m_best = start; 58 | m_bestC = 0; 59 | m_bestH = pathFinder().heuristic(start, end); 60 | 61 | jumpN(start, end); 62 | jumpE(start, end); 63 | jumpS(start, end); 64 | jumpW(start, end); 65 | jumpNE(start, end); 66 | jumpSE(start, end); 67 | jumpSW(start, end); 68 | jumpNW(start, end); 69 | 70 | while (m_queue.count()) { 71 | Coord cur = m_queue.pop(); 72 | if (cur == end) { 73 | m_best = end; 74 | break; 75 | } 76 | 77 | int ci = pathFinder().index(cur.x, cur.y); 78 | 79 | Coord from = m_cameFrom[ci]; 80 | int bi = ci - m_size.width; 81 | int ti = ci + m_size.width; 82 | 83 | if (cur.y == from.y) { 84 | if (cur.x > from.x) { 85 | if (m_map[bi] & !m_map[bi - 1]) { 86 | jumpS(cur, end); 87 | jumpSE(cur, end); 88 | } 89 | if (m_map[ti] & !m_map[ti - 1]) { 90 | jumpN(cur, end); 91 | jumpNE(cur, end); 92 | } 93 | jumpE(cur, end); 94 | } 95 | else { 96 | if (m_map[bi] & !m_map[bi + 1]) { 97 | jumpS(cur, end); 98 | jumpSW(cur, end); 99 | } 100 | if (m_map[ti] & !m_map[ti + 1]) { 101 | jumpN(cur, end); 102 | jumpNW(cur, end); 103 | } 104 | jumpW(cur, end); 105 | } 106 | } 107 | else if (cur.y < from.y) { 108 | if (cur.x == from.x) { 109 | if (m_map[ci - 1] & !m_map[ti - 1]) { 110 | jumpW(cur, end); 111 | jumpSW(cur, end); 112 | } 113 | if (m_map[ci + 1] & !m_map[ti + 1]) { 114 | jumpE(cur, end); 115 | jumpSE(cur, end); 116 | } 117 | jumpS(cur, end); 118 | } 119 | else if (cur.x > from.x) 120 | jumpSE(cur, end); 121 | else 122 | jumpSW(cur, end); 123 | } 124 | else { // cur.y > from.y 125 | if (cur.x == from.x) { 126 | if (m_map[ci - 1] & !m_map[bi - 1]) { 127 | jumpW(cur, end); 128 | jumpNW(cur, end); 129 | } 130 | if (m_map[ci + 1] & !m_map[bi + 1]) { 131 | jumpE(cur, end); 132 | jumpNE(cur, end); 133 | } 134 | jumpN(cur, end); 135 | } 136 | else if (cur.x > from.x) 137 | jumpNE(cur, end); 138 | else 139 | jumpNW(cur, end); 140 | } 141 | } 142 | 143 | // Create Path 144 | Coord c = m_best; 145 | while (c != start) { 146 | path.push(map()->getPos(c.point())); 147 | c = m_cameFrom[pathFinder().index(c.x, c.y)]; 148 | } 149 | path.push(map()->getPos(start.point())); 150 | } 151 | 152 | U16 JPSplus::getJumpN(Coord from) { 153 | int curi = pathFinder().index(from.x, from.y); 154 | int nexti = curi + m_size.width; 155 | U16 d = 0; 156 | 157 | while (m_map[nexti]) { 158 | d++; 159 | if ((m_map[nexti + 1] && !m_map[curi + 1]) || (m_map[nexti - 1] && !m_map[curi - 1])) 160 | return d | BIT(15); 161 | curi = nexti; 162 | nexti += m_size.width; 163 | } 164 | return d; 165 | } 166 | 167 | U16 JPSplus::getJumpE(Coord from) { 168 | int curi = pathFinder().index(from.x, from.y); 169 | int nexti = curi + 1; 170 | U16 d = 0; 171 | 172 | while (m_map[nexti]) { 173 | d++; 174 | if ((m_map[nexti + m_size.width] && !m_map[curi + m_size.width]) || (m_map[nexti - m_size.width] && !m_map[curi - m_size.width])) 175 | return d | BIT(15); 176 | curi = nexti; 177 | nexti++; 178 | } 179 | return d; 180 | } 181 | 182 | U16 JPSplus::getJumpS(Coord from) { 183 | int curi = pathFinder().index(from.x, from.y); 184 | int nexti = curi - m_size.width; 185 | U16 d = 0; 186 | 187 | while (m_map[nexti]) { 188 | d++; 189 | if ((m_map[nexti + 1] && !m_map[curi + 1]) || (m_map[nexti - 1] && !m_map[curi - 1])) 190 | return d | BIT(15); 191 | curi = nexti; 192 | nexti -= m_size.width; 193 | } 194 | return d; 195 | } 196 | 197 | U16 JPSplus::getJumpW(Coord from) { 198 | int curi = pathFinder().index(from.x, from.y); 199 | int nexti = curi - 1; 200 | U16 d = 0; 201 | 202 | while (m_map[nexti]) { 203 | d++; 204 | if ((m_map[nexti + m_size.width] && !m_map[curi + m_size.width]) || (m_map[nexti - m_size.width] && !m_map[curi - m_size.width])) 205 | return d | BIT(15); 206 | curi = nexti; 207 | nexti--; 208 | } 209 | return d; 210 | } 211 | 212 | U16 JPSplus::getJumpNE(Coord from) { 213 | Coord next = from; 214 | int nexti = pathFinder().index(from.x, from.y); 215 | U16 d = 0; 216 | while (true) { 217 | if (!m_map[nexti + m_size.width] || !m_map[nexti + 1]) 218 | break; 219 | nexti += m_size.width + 1; 220 | if (!m_map[nexti]) 221 | break; 222 | d++; 223 | next.x++; 224 | next.y++; 225 | } 226 | return d; 227 | } 228 | 229 | U16 JPSplus::getJumpSE(Coord from) { 230 | Coord next = from; 231 | int nexti = pathFinder().index(from.x, from.y); 232 | U16 d = 0; 233 | while (true) { 234 | if (!m_map[nexti - m_size.width] || !m_map[nexti + 1]) 235 | break; 236 | nexti -= m_size.width - 1; 237 | if (!m_map[nexti]) 238 | break; 239 | d++; 240 | next.x++; 241 | next.y--; 242 | } 243 | return d; 244 | } 245 | 246 | U16 JPSplus::getJumpSW(Coord from) { 247 | Coord next = from; 248 | int nexti = pathFinder().index(from.x, from.y); 249 | U16 d = 0; 250 | while (true) { 251 | if (!m_map[nexti - m_size.width] || !m_map[nexti - 1]) 252 | break; 253 | nexti -= m_size.width + 1; 254 | if (!m_map[nexti]) 255 | break; 256 | d++; 257 | next.x--; 258 | next.y--; 259 | } 260 | return d; 261 | } 262 | 263 | U16 JPSplus::getJumpNW(Coord from) { 264 | Coord next = from; 265 | int nexti = pathFinder().index(from.x, from.y); 266 | U16 d = 0; 267 | while (true) { 268 | if (!m_map[nexti + m_size.width] || !m_map[nexti - 1]) 269 | break; 270 | nexti += m_size.width - 1; 271 | if (!m_map[nexti]) 272 | break; 273 | d++; 274 | next.x--; 275 | next.y++; 276 | } 277 | return d; 278 | } 279 | 280 | void JPSplus::jumpN(Coord from, Coord goal) { 281 | int fromi = pathFinder().index(from.x, from.y); 282 | U16 d = m_jps[fromi].n; 283 | U16 dist = d & ~(BIT(15)); 284 | U16 endy = from.y + dist; 285 | 286 | if (from.x == goal.x && inRange(from.y, endy, goal.y)) { 287 | int goali = pathFinder().index(goal.x, goal.y); 288 | U32 cost = m_cost[fromi] + goal.y - from.y; 289 | if (cost < m_cost[goali]) { 290 | m_queue.insert(goal, cost); 291 | m_cameFrom[goali] = from; 292 | m_cost[goali] = cost; 293 | } 294 | else if (cost == m_cost[goali]) 295 | m_queue.insert(goal, cost); 296 | return; 297 | } 298 | 299 | if (d & BIT(15)) { 300 | U32 cost = m_cost[fromi] + dist; 301 | Coord end(from.x, endy); 302 | int endi = pathFinder().index(end.x, end.y); 303 | if (cost < m_cost[endi]) { 304 | U32 h = pathFinder().heuristic(end, goal); 305 | m_queue.insert(end, h + cost); 306 | m_cameFrom[endi] = from; 307 | m_cost[endi] = cost; 308 | } 309 | } 310 | 311 | if (goal.y > from.y) { 312 | Coord c(from.x, min2(goal.y, endy)); 313 | U32 h = pathFinder().heuristic(c, goal); 314 | U32 cost = m_cost[fromi] + c.y - from.y; 315 | if (h < m_bestH || (h == m_bestH && cost < m_bestC)) { 316 | m_bestC = cost; 317 | m_bestH = h; 318 | m_best = c; 319 | int ci = pathFinder().index(c.x, c.y); 320 | if (cost < m_cost[ci]) { 321 | m_cost[ci] = cost; 322 | m_cameFrom[ci] = from; 323 | } 324 | } 325 | } 326 | } 327 | 328 | void JPSplus::jumpE(Coord from, Coord goal) { 329 | int fromi = pathFinder().index(from.x, from.y); 330 | U16 d = m_jps[fromi].e; 331 | U16 dist = d & ~(BIT(15)); 332 | U16 endx = from.x + dist; 333 | 334 | if (from.y == goal.y && inRange(from.x, endx, goal.x)) { 335 | int goali = pathFinder().index(goal.x, goal.y); 336 | U32 cost = m_cost[fromi] + goal.x - from.x; 337 | if (cost < m_cost[goali]) { 338 | m_queue.insert(goal, cost); 339 | m_cameFrom[goali] = from; 340 | m_cost[goali] = cost; 341 | } 342 | else if (cost == m_cost[goali]) 343 | m_queue.insert(goal, cost); 344 | return; 345 | } 346 | 347 | if (d & BIT(15)) { 348 | U32 cost = m_cost[fromi] + dist; 349 | Coord end(endx, from.y); 350 | int endi = pathFinder().index(end.x, end.y); 351 | if (cost < m_cost[endi]) { 352 | U32 h = pathFinder().heuristic(end, goal); 353 | m_queue.insert(end, h + cost); 354 | m_cameFrom[endi] = from; 355 | m_cost[endi] = cost; 356 | } 357 | } 358 | 359 | if (goal.x > from.x) { 360 | Coord c(min2(goal.x, endx), from.y); 361 | U32 h = pathFinder().heuristic(c, goal); 362 | U32 cost = m_cost[fromi] + c.x - from.x; 363 | if (h < m_bestH || (h == m_bestH && cost < m_bestC)) { 364 | m_bestC = cost; 365 | m_bestH = h; 366 | m_best = c; 367 | int ci = pathFinder().index(c.x, c.y); 368 | if (cost < m_cost[ci]) { 369 | m_cost[ci] = cost; 370 | m_cameFrom[ci] = from; 371 | } 372 | } 373 | } 374 | } 375 | 376 | void JPSplus::jumpS(Coord from, Coord goal) { 377 | int fromi = pathFinder().index(from.x, from.y); 378 | U16 d = m_jps[fromi].s; 379 | U16 dist = d & ~(BIT(15)); 380 | U16 endy = from.y - dist; 381 | 382 | if (from.x == goal.x && inRange(endy, from.y, goal.y)) { 383 | int goali = pathFinder().index(goal.x, goal.y); 384 | U32 cost = m_cost[fromi] + from.y - goal.y; 385 | if (cost < m_cost[goali]) { 386 | m_queue.insert(goal, cost); 387 | m_cameFrom[goali] = from; 388 | m_cost[goali] = cost; 389 | } 390 | else if (cost == m_cost[goali]) 391 | m_queue.insert(goal, cost); 392 | return; 393 | } 394 | 395 | if (d & BIT(15)) { 396 | U32 cost = m_cost[fromi] + dist; 397 | Coord end(from.x, endy); 398 | int endi = pathFinder().index(end.x, end.y); 399 | if (cost < m_cost[endi]) { 400 | U32 h = pathFinder().heuristic(end, goal); 401 | m_queue.insert(end, h + cost); 402 | m_cameFrom[endi] = from; 403 | m_cost[endi] = cost; 404 | } 405 | } 406 | 407 | if (goal.y < from.y) { 408 | Coord c(from.x, max2(goal.y, endy)); 409 | U32 h = pathFinder().heuristic(c, goal); 410 | U32 cost = m_cost[fromi] + from.y - c.y; 411 | if (h < m_bestH || (h == m_bestH && cost < m_bestC)) { 412 | m_bestC = cost; 413 | m_bestH = h; 414 | m_best = c; 415 | int ci = pathFinder().index(c.x, c.y); 416 | if (cost < m_cost[ci]) { 417 | m_cost[ci] = cost; 418 | m_cameFrom[ci] = from; 419 | } 420 | } 421 | } 422 | } 423 | 424 | void JPSplus::jumpW(Coord from, Coord goal) { 425 | int fromi = pathFinder().index(from.x, from.y); 426 | U16 d = m_jps[fromi].w; 427 | U16 dist = d & ~(BIT(15)); 428 | U16 endx = from.x - dist; 429 | 430 | if (from.y == goal.y && inRange(endx, from.x, goal.x)) { 431 | int goali = pathFinder().index(goal.x, goal.y); 432 | U32 cost = m_cost[fromi] + from.x - goal.x; 433 | if (cost < m_cost[goali]) { 434 | m_queue.insert(goal, cost); 435 | m_cameFrom[goali] = from; 436 | m_cost[goali] = cost; 437 | } 438 | else if (cost == m_cost[goali]) 439 | m_queue.insert(goal, cost); 440 | return; 441 | } 442 | 443 | if (d & BIT(15)) { 444 | U32 cost = m_cost[fromi] + dist; 445 | Coord end(endx, from.y); 446 | int endi = pathFinder().index(end.x, end.y); 447 | if (cost < m_cost[endi]) { 448 | U32 h = pathFinder().heuristic(end, goal); 449 | m_queue.insert(end, h + cost); 450 | m_cameFrom[endi] = from; 451 | m_cost[endi] = cost; 452 | } 453 | } 454 | 455 | if (goal.x < from.x) { 456 | Coord c(max2(goal.x, endx), from.y); 457 | U32 h = pathFinder().heuristic(c, goal); 458 | U32 cost = m_cost[fromi] + from.x - c.x; 459 | if (h < m_bestH || (h == m_bestH && cost < m_bestC)) { 460 | m_bestC = cost; 461 | m_bestH = h; 462 | m_best = c; 463 | int ci = pathFinder().index(c.x, c.y); 464 | if (cost < m_cost[ci]) { 465 | m_cost[ci] = cost; 466 | m_cameFrom[ci] = from; 467 | } 468 | } 469 | } 470 | } 471 | 472 | void JPSplus::jumpNE(Coord from, Coord goal) { 473 | int fromi = pathFinder().index(from.x, from.y); 474 | U16 dist = m_jps[fromi].ne; 475 | Coord next = from; 476 | int nexti = fromi; 477 | U32 cost = m_cost[fromi]; 478 | 479 | for (int i = 0; i < dist; i++) { 480 | next.x++; 481 | next.y++; 482 | nexti += m_size.width + 1; 483 | cost++; 484 | if (cost >= m_cost[nexti]) 485 | break; 486 | m_cameFrom[nexti] = from; 487 | m_cost[nexti] = cost; 488 | jumpN(next, goal); 489 | jumpE(next, goal); 490 | 491 | U32 h = pathFinder().heuristic(next, goal); 492 | if (h < m_bestH || (h == m_bestH && cost < m_bestC)) { 493 | m_bestC = cost; 494 | m_bestH = h; 495 | m_best = next; 496 | } 497 | } 498 | } 499 | 500 | void JPSplus::jumpSE(Coord from, Coord goal) { 501 | int fromi = pathFinder().index(from.x, from.y); 502 | U16 dist = m_jps[fromi].se; 503 | Coord next = from; 504 | int nexti = fromi; 505 | U32 cost = m_cost[fromi]; 506 | 507 | for (int i = 0; i < dist; i++) { 508 | next.x++; 509 | next.y--; 510 | nexti -= m_size.width - 1; 511 | cost++; 512 | if (cost >= m_cost[nexti]) 513 | break; 514 | m_cameFrom[nexti] = from; 515 | m_cost[nexti] = cost; 516 | jumpS(next, goal); 517 | jumpE(next, goal); 518 | 519 | U32 h = pathFinder().heuristic(next, goal); 520 | if (h < m_bestH || (h == m_bestH && cost < m_bestC)) { 521 | m_bestC = cost; 522 | m_bestH = h; 523 | m_best = next; 524 | } 525 | } 526 | } 527 | 528 | void JPSplus::jumpSW(Coord from, Coord goal) { 529 | int fromi = pathFinder().index(from.x, from.y); 530 | U16 dist = m_jps[fromi].sw; 531 | Coord next = from; 532 | int nexti = fromi; 533 | U32 cost = m_cost[fromi]; 534 | 535 | for (int i = 0; i < dist; i++) { 536 | next.x--; 537 | next.y--; 538 | nexti -= m_size.width + 1; 539 | cost++; 540 | if (cost >= m_cost[nexti]) 541 | break; 542 | m_cameFrom[nexti] = from; 543 | m_cost[nexti] = cost; 544 | jumpS(next, goal); 545 | jumpW(next, goal); 546 | 547 | U32 h = pathFinder().heuristic(next, goal); 548 | if (h < m_bestH || (h == m_bestH && cost < m_bestC)) { 549 | m_bestC = cost; 550 | m_bestH = h; 551 | m_best = next; 552 | } 553 | } 554 | } 555 | 556 | void JPSplus::jumpNW(Coord from, Coord goal) { 557 | int fromi = pathFinder().index(from.x, from.y); 558 | U16 dist = m_jps[fromi].nw; 559 | Coord next = from; 560 | int nexti = fromi; 561 | U32 cost = m_cost[fromi]; 562 | 563 | for (int i = 0; i < dist; i++) { 564 | next.x--; 565 | next.y++; 566 | nexti += m_size.width - 1; 567 | cost++; 568 | if (cost >= m_cost[nexti]) 569 | break; 570 | m_cameFrom[nexti] = from; 571 | m_cost[nexti] = cost; 572 | jumpN(next, goal); 573 | jumpW(next, goal); 574 | 575 | U32 h = pathFinder().heuristic(next, goal); 576 | if (h < m_bestH || (h == m_bestH && cost < m_bestC)) { 577 | m_bestC = cost; 578 | m_bestH = h; 579 | m_best = next; 580 | } 581 | } 582 | } 583 | -------------------------------------------------------------------------------- /src/JPSplus.hpp: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | 4 | #include "Coord.hpp" 5 | 6 | class JPSplus { 7 | public: 8 | JPSplus(); 9 | 10 | void update(); 11 | void find(Coord start, Coord end, nook::Array& path); 12 | 13 | private: 14 | struct JP { 15 | nook::U16 n; 16 | nook::U16 ne; 17 | nook::U16 e; 18 | nook::U16 se; 19 | nook::U16 s; 20 | nook::U16 sw; 21 | nook::U16 w; 22 | nook::U16 nw; 23 | }; 24 | 25 | nook::U16 getJumpN(Coord from); 26 | nook::U16 getJumpE(Coord from); 27 | nook::U16 getJumpS(Coord from); 28 | nook::U16 getJumpW(Coord from); 29 | nook::U16 getJumpNE(Coord from); 30 | nook::U16 getJumpSE(Coord from); 31 | nook::U16 getJumpSW(Coord from); 32 | nook::U16 getJumpNW(Coord from); 33 | 34 | void jumpN(Coord from, Coord goal); 35 | void jumpE(Coord from, Coord goal); 36 | void jumpS(Coord from, Coord goal); 37 | void jumpW(Coord from, Coord goal); 38 | void jumpNE(Coord from, Coord goal); 39 | void jumpSE(Coord from, Coord goal); 40 | void jumpSW(Coord from, Coord goal); 41 | void jumpNW(Coord from, Coord goal); 42 | 43 | nook::Size m_size; 44 | bool* m_map; 45 | JP* m_jps; 46 | nook::PriorityQueue m_queue; 47 | nook::Array m_cameFrom; 48 | nook::Array m_cost; 49 | 50 | Coord m_best; 51 | nook::U32 m_bestC; 52 | nook::U32 m_bestH; 53 | }; 54 | -------------------------------------------------------------------------------- /src/PathFinder.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "PathFinder.hpp" 3 | #include "Map.hpp" 4 | #include "scenes/GameScene.hpp" 5 | 6 | #include "resource/ResourceManager.hpp" 7 | 8 | using namespace nook; 9 | 10 | PathFinder* PathFinder::s_instance; 11 | 12 | PathFinder::PathFinder() { 13 | s_instance = this; 14 | m_size = map()->size(); 15 | 16 | m_jpsPlus = memoryManager().createOnStack(); 17 | m_wallTracing = memoryManager().createOnStack(); 18 | 19 | // Visual Path 20 | m_pathObject.partCount = 1; 21 | m_pathObject.parts = memoryManager().allocOnStack(); 22 | m_pathObject.boundingSphere.pos = 0.0f; 23 | m_pathObject.boundingSphere.radius = m_size.width / 2; 24 | 25 | Mesh* mesh = resourceManager().findResource(SH("color-mesh")); 26 | MaterialDesc* md = (MaterialDesc*)mesh->materials(); 27 | 28 | m_pathObject.parts->material.init(md); 29 | m_pathObject.parts->material.setUniform(SH("uModel"), &m_pathTransform); 30 | 31 | DynamicBuffer* buf = DynamicBuffer::create(KB(8), KB(1)); 32 | m_pathObject.parts->vao = DynamicVAO::create(mesh->submeshes()->input, buf, 0); 33 | m_pathObject.parts->indexOffset = 0; 34 | m_pathObject.parts->mode = RenderMode::LineStrip; 35 | m_pathTransform = mat4::identity(); 36 | 37 | U16* inds = buf->mapIbo(); 38 | for (U16 i = 0; i < 512; i++) 39 | inds[i] = i; 40 | buf->unmapIbo(); 41 | } 42 | 43 | PathFinder::~PathFinder() { 44 | m_wallTracing->~WallTracing(); 45 | s_instance = nullptr; 46 | DynamicBuffer* buf = ((DynamicVAO*)m_pathObject.parts->vao)->buffer(); 47 | memoryManager().remove(m_pathObject.parts->vao); 48 | memoryManager().remove(buf); 49 | } 50 | 51 | void PathFinder::findRough(vec2 start, vec2 end, Array& path) { 52 | Point2 s = map()->getCoord(start); 53 | Point2 e = map()->getCoord(end); 54 | s.x = clamp(s.x, 1, m_size.width - 2); 55 | s.y = clamp(s.y, 1, m_size.height - 2); 56 | m_jpsPlus->find(Coord(s.x, s.y), Coord(e.x, e.y), path); 57 | } 58 | 59 | void PathFinder::showPath(Array& path) { 60 | if (path.count() == 0) 61 | return; 62 | 63 | DynamicBuffer* buf = ((DynamicVAO*)m_pathObject.parts->vao)->buffer(); 64 | vec3* verts = (vec3*)buf->mapVbo(); 65 | for (vec2 p : path) 66 | *verts++ = vec3(p.x, 0.01f, p.y); 67 | buf->unmapVbo(); 68 | m_pathObject.parts->indexCount = path.count(); 69 | 70 | if (!m_pathObject.node) 71 | gameScene()->renderTree().add(&m_pathObject); 72 | } 73 | -------------------------------------------------------------------------------- /src/PathFinder.hpp: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | 4 | #include "AStar.hpp" 5 | #include "JPS.hpp" 6 | #include "JPSplus.hpp" 7 | #include "WallTracing.hpp" 8 | 9 | #include "visual/3D/RenderObject.hpp" 10 | 11 | class PathFinder { 12 | public: 13 | static PathFinder* s_instance; 14 | 15 | PathFinder(); 16 | ~PathFinder(); 17 | 18 | void findRough(nook::vec2 start, nook::vec2 end, nook::Array& path); 19 | void showPath(nook::Array& path); 20 | 21 | int index(int x, int y) { return y * m_size.width + x; } 22 | 23 | nook::U32 heuristic(Coord c1, Coord c2) { 24 | // int x = (int)c1.x - (int)c2.x; 25 | // int y = (int)c1.y - (int)c2.y; 26 | // return x * x + y * y; 27 | int dx = std::abs((int)c1.x - (int)c2.x); 28 | int dy = std::abs((int)c1.y - (int)c2.y); 29 | return nook::max2(dx, dy); 30 | } 31 | 32 | WallTracing* wallTracing() { return m_wallTracing; } 33 | 34 | private: 35 | nook::Size m_size; 36 | AStar* m_astar; 37 | JPS* m_jps; 38 | JPSplus* m_jpsPlus; 39 | WallTracing* m_wallTracing; 40 | 41 | nook::RenderObject m_pathObject; 42 | nook::mat4 m_pathTransform; 43 | }; 44 | 45 | inline PathFinder& pathFinder() { 46 | return *PathFinder::s_instance; 47 | } 48 | -------------------------------------------------------------------------------- /src/WallTracing.cpp: -------------------------------------------------------------------------------- 1 | 2 | #include "WallTracing.hpp" 3 | #include "Map.hpp" 4 | 5 | using namespace nook; 6 | 7 | namespace { 8 | const float kr2 = 1.08f; 9 | const int kMaxIter = 20; 10 | 11 | inline U32 getCell(int x, int y) { 12 | U32 cell = 0; 13 | cell |= map()->getCell(x - 1, y - 1)->walkable; 14 | cell |= map()->getCell(x, y - 1)->walkable << 1; 15 | cell |= map()->getCell(x, y)->walkable << 2; 16 | cell |= map()->getCell(x - 1, y)->walkable << 3; 17 | return cell; 18 | } 19 | 20 | inline float heuristics(vec2 p1, vec2 p2) { 21 | return (p1 - p2).length(); 22 | } 23 | } 24 | 25 | WallTracing::WallTracing() { 26 | m_curCheck = 1; 27 | m_curRequest = 1; 28 | 29 | m_dummyObstacle.radius = 0.2f; 30 | 31 | m_size = map()->size(); 32 | m_regionCount.x = ceilDiv(m_size.width, RegionSize); 33 | m_regionCount.y = ceilDiv(m_size.height, RegionSize); 34 | int rc = m_regionCount.x * m_regionCount.y; 35 | 36 | m_regions = memoryManager().allocOnStack(rc); 37 | for (int i = 0; i < rc; i++) 38 | new (m_regions + i) Region(); 39 | 40 | const U32 maxQueue = 20; 41 | m_queue.init(maxQueue, memoryManager().allocOnStack::Item>(maxQueue)); 42 | 43 | List1 corners; 44 | nook::Size hs = map()->size() / 2; 45 | 46 | auto pushCorner = [&](Coord c, vec2 n, bool o) { 47 | Corner* corner = m_cornerPool.alloc(); 48 | corner->coord = c; 49 | corner->outer = o; 50 | corner->pos = vec2((int)c.x - hs.width, (int)c.y - hs.height); 51 | corner->normal = n; 52 | corner->right = corner->left = nullptr; 53 | corner->checked = m_curCheck; 54 | corner->request = m_curRequest; 55 | 56 | Coord rc = getRegion(corner); 57 | m_regions[rc.y * m_regionCount.x + rc.x].corners.push(corner); 58 | corners.push(corner); 59 | }; 60 | 61 | for (int y = 1; y < m_size.height; y++) 62 | for (int x = 1; x < m_size.width; x++) { 63 | U32 cell = getCell(x, y); 64 | switch (cell) { 65 | case 1: // ▜ 66 | pushCorner(Coord(x, y), vec2(-1.0f, -1.0f), false); 67 | break; 68 | case 2: // ▛ 69 | pushCorner(Coord(x, y), vec2(1.0f, -1.0f), false); 70 | break; 71 | case 4: // ▙ 72 | pushCorner(Coord(x, y), vec2(1.0f, 1.0f), false); 73 | break; 74 | case 8: // ▟ 75 | pushCorner(Coord(x, y), vec2(-1.0f, 1.0f), false); 76 | break; 77 | case 5: // ▚ 78 | pushCorner(Coord(x, y), vec2(-1.0f, -1.0f), false); 79 | pushCorner(Coord(x, y), vec2(1.0f, 1.0f), false); 80 | break; 81 | case 10: // ▞ 82 | pushCorner(Coord(x, y), vec2(1.0f, -1.0f), false); 83 | pushCorner(Coord(x, y), vec2(-1.0f, 1.0f), false); 84 | break; 85 | case 7: // ┛ 86 | pushCorner(Coord(x, y), vec2(1.0f, -1.0f), true); 87 | break; 88 | case 11: // ┗ 89 | pushCorner(Coord(x, y), vec2(-1.0f, -1.0f), true); 90 | break; 91 | case 13: // ┏ 92 | pushCorner(Coord(x, y), vec2(-1.0f, 1.0f), true); 93 | break; 94 | case 14: // ┓ 95 | pushCorner(Coord(x, y), vec2(1.0f, 1.0f), true); 96 | break; 97 | default: 98 | break; 99 | } 100 | } 101 | 102 | for (Corner* corner : corners) { 103 | int dx = 0; 104 | int dy = 0; 105 | int dir = 0; 106 | U32 wall = 0; 107 | 108 | if (corner->outer) { 109 | if (corner->normal.x == 1.0f) { 110 | if (corner->normal.y == 1.0f) { 111 | dx = -1; 112 | wall = 12; 113 | dir = 2; 114 | } 115 | else { 116 | dy = 1; 117 | wall = 6; 118 | dir = 8; 119 | } 120 | } 121 | else { 122 | if (corner->normal.y == 1.0f) { 123 | dy = -1; 124 | wall = 9; 125 | dir = 4; 126 | } 127 | else { 128 | dx = 1; 129 | wall = 3; 130 | dir = 1; 131 | } 132 | } 133 | } 134 | else { // inner 135 | if (corner->normal.x == 1.0f) { 136 | if (corner->normal.y == 1.0f) { 137 | dy = 1; 138 | wall = 6; 139 | dir = 8; 140 | } 141 | else { 142 | dx = 1; 143 | wall = 3; 144 | dir = 1; 145 | } 146 | } 147 | else { 148 | if (corner->normal.y == 1.0f) { 149 | dx = -1; 150 | wall = 12; 151 | dir = 2; 152 | } 153 | else { 154 | dy = -1; 155 | wall = 9; 156 | dir = 4; 157 | } 158 | } 159 | } 160 | 161 | Coord p(corner->coord.x + dx, corner->coord.y + dy); 162 | U32 cell = getCell(p.x, p.y); 163 | while (cell == wall) { 164 | p.x += dx; 165 | p.y += dy; 166 | cell = getCell(p.x, p.y); 167 | } 168 | 169 | Corner pc = getCorner(p, cell, dir); 170 | Coord prc = getRegion(&pc); 171 | Region& pr = m_regions[prc.y * m_regionCount.x + prc.x]; 172 | 173 | for (Corner* cp : pr.corners) { 174 | if (*cp == pc) { 175 | corner->right = cp; 176 | cp->left = corner; 177 | break; 178 | } 179 | } 180 | 181 | Coord r = getRegion(corner); 182 | if (dx) { 183 | for (int x = r.x; x != prc.x; x += dx) 184 | m_regions[r.y * m_regionCount.x + x + dx].corners.push(corner); 185 | } 186 | else { 187 | for (int y = r.y; y != prc.y; y += dy) 188 | m_regions[(y + dy) * m_regionCount.x + r.x].corners.push(corner); 189 | } 190 | } 191 | } 192 | 193 | WallTracing::~WallTracing() { 194 | int rc = m_regionCount.x * m_regionCount.y; 195 | for (int i = 0; i < rc; i++) 196 | m_regions[i].~Region(); 197 | } 198 | 199 | void WallTracing::addObstacle(Circle o) { 200 | nook::Size hs = map()->size() / 2; 201 | Point2 bl(o.pos.x - o.radius + hs.width, o.pos.y - o.radius + hs.height); 202 | Point2 tr(o.pos.x + o.radius + hs.width, o.pos.y + o.radius + hs.height); 203 | bl /= RegionSize; 204 | tr /= RegionSize; 205 | 206 | Obstacle* ob = m_obstaclePool.alloc(); 207 | ob->pos = o.pos; 208 | ob->radius = o.radius; 209 | ob->checked = m_curCheck; 210 | ob->request = m_curRequest; 211 | 212 | for (int y = bl.y; y <= tr.y; y++) 213 | for (int x = bl.x; x <= tr.x; x++) 214 | m_regions[y * m_regionCount.x + x].obstacles.push(ob); 215 | } 216 | 217 | void WallTracing::removeObstacle(Circle o) { 218 | nook::Size hs = map()->size() / 2; 219 | Point2 bl(o.pos.x - o.radius + hs.width, o.pos.y - o.radius + hs.height); 220 | Point2 tr(o.pos.x + o.radius + hs.width, o.pos.y + o.radius + hs.height); 221 | bl /= RegionSize; 222 | tr /= RegionSize; 223 | 224 | Obstacle* ob = nullptr; 225 | Region& r = m_regions[bl.y * m_regionCount.x + bl.x]; 226 | for (Obstacle* ro : r.obstacles) 227 | if (ro->pos == o.pos) { 228 | ob = ro; 229 | break; 230 | } 231 | ASSERT(ob); 232 | 233 | for (int y = bl.y; y <= tr.y; y++) 234 | for (int x = bl.x; x <= tr.x; x++) 235 | m_regions[y * m_regionCount.x + x].obstacles.remove(ob); 236 | m_obstaclePool.free(ob); 237 | } 238 | 239 | void WallTracing::find(vec2 start, vec2 end, float radius, Array& path) { 240 | m_curCheck++; 241 | m_queue.clear(); 242 | m_path.clear(); 243 | m_nextPool.clear(); 244 | m_curRadius = radius; 245 | m_end = end; 246 | m_iter = kMaxIter; 247 | 248 | m_curObstacle = getObstacle(start); 249 | m_lastObstacle = nullptr; 250 | m_endObstacle = findObstacle(end); 251 | if (m_endObstacle == &m_dummyObstacle) { 252 | m_lastObstacle = findObstacle(end, radius); 253 | if (m_lastObstacle != &m_dummyObstacle) { 254 | vec2 v = end - m_lastObstacle->pos; 255 | v *= 10.0f / v.length(); 256 | m_lastOLine = m_lastObstacle->pos + v; 257 | } 258 | } 259 | 260 | m_best = nullptr; 261 | 262 | Next col = findCollision(start, end, true); 263 | if (col.type) { 264 | if (col.type == 2) 265 | col.obstacle->checked = m_curCheck; 266 | 267 | float h = heuristics(end, col.pos); 268 | float p = col.cost + h; 269 | 270 | Next* left = m_nextPool.alloc(); 271 | left->type = col.type; 272 | left->dir = 1; 273 | left->last = col.last; 274 | left->cost = col.cost; 275 | left->pos = col.pos; 276 | left->from = nullptr; 277 | left->corner = col.corner; 278 | m_queue.insert(left, p); 279 | 280 | Next* right = m_nextPool.alloc(); 281 | right->type = col.type; 282 | right->dir = 2; 283 | right->last = col.last; 284 | right->cost = col.cost; 285 | right->pos = col.pos; 286 | right->from = nullptr; 287 | right->corner = col.corner; 288 | m_queue.insert(right, p); 289 | 290 | m_best = left; 291 | m_bestH = h; 292 | } 293 | else 294 | m_path.push(end); 295 | 296 | while (m_queue.count()) { 297 | Next* r = m_queue.pop(); 298 | if (r->last) { 299 | m_best = r; 300 | break; 301 | } 302 | if (!m_iter) { 303 | debugLog("max iter") 304 | break; 305 | } 306 | m_iter--; 307 | 308 | if (r->type == 1) { 309 | Corner* c = r->corner; 310 | vec2 cpos = c->pos + c->normal * radius; 311 | if (r->pos == cpos) { 312 | pushNextCorner(r, r->dir == 1 ? c->left : c->right); 313 | 314 | vec2 to = end - cpos; 315 | vec2 d = r->dir == 1 ? vec2(c->normal.y, -c->normal.x) : vec2(-c->normal.y, c->normal.x); 316 | if (c->outer && to.x * d.x >= 0.0f && to.y * d.y >= 0.0f) { 317 | c->request = m_curRequest + 1; 318 | c->left->request = m_curRequest + 1; 319 | pushNextCollision(r); 320 | } 321 | } 322 | else { 323 | if (r->dir == 1) { 324 | c->checked = m_curCheck - 1; 325 | pushNextCorner(r, c); 326 | } 327 | else 328 | pushNextCorner(r, c->right); 329 | } 330 | } 331 | else 332 | pushNextObstacle(r); 333 | } 334 | 335 | while (m_best) { 336 | m_path.push(m_best->pos); 337 | m_best = m_best->from; 338 | } 339 | m_path.push(start); 340 | 341 | List::Node* node = m_path.root()->prev; 342 | for (int i = m_path.size() - 2; i > 0; i--) { 343 | vec2 from = node->elem; 344 | vec2 to = node->prev->prev->elem; 345 | float dist = (from - to).length(); 346 | Next col = findCollision(from, to, true); 347 | if (col.cost < dist - 0.001f) 348 | node = node->prev; 349 | else 350 | m_path.remove(node->prev); 351 | } 352 | 353 | if (m_path.size() == 2 && m_path.first() == m_path.last()) 354 | m_path.remove(m_path.root()->prev); 355 | 356 | for (vec2 p : m_path) 357 | path.push(p); 358 | } 359 | 360 | WallTracing::Corner WallTracing::getCorner(Coord coord, U32 cell, int dir) { 361 | Corner corner; 362 | corner.coord = coord; 363 | switch (cell) { 364 | case 1: // ▜ 365 | corner.normal = nook::vec2(-1.0f, -1.0f); 366 | corner.outer = false; 367 | break; 368 | case 2: // ▛ 369 | corner.normal = nook::vec2(1.0f, -1.0f); 370 | corner.outer = false; 371 | break; 372 | case 4: // ▙ 373 | corner.normal = nook::vec2(1.0f, 1.0f); 374 | corner.outer = false; 375 | break; 376 | case 8: // ▟ 377 | corner.normal = nook::vec2(-1.0f, 1.0f); 378 | corner.outer = false; 379 | break; 380 | case 5: // ▚ 381 | corner.normal = dir & 9 ? nook::vec2(-1.0f, -1.0f) : nook::vec2(1.0f, 1.0f); 382 | corner.outer = false; 383 | break; 384 | case 10: // ▞ 385 | corner.normal = dir & 5 ? nook::vec2(-1.0f, 1.0f) : nook::vec2(1.0f, -1.0f); 386 | corner.outer = false; 387 | break; 388 | case 7: // ┛ 389 | corner.normal = nook::vec2(1.0f, -1.0f); 390 | corner.outer = true; 391 | break; 392 | case 11: // ┗ 393 | corner.normal = nook::vec2(-1.0f, -1.0f); 394 | corner.outer = true; 395 | break; 396 | case 13: // ┏ 397 | corner.normal = nook::vec2(-1.0f, 1.0f); 398 | corner.outer = true; 399 | break; 400 | case 14: // ┓ 401 | corner.normal = nook::vec2(1.0f, 1.0f); 402 | corner.outer = true; 403 | break; 404 | default: 405 | break; 406 | } 407 | return corner; 408 | } 409 | 410 | WallTracing::Obstacle* WallTracing::getObstacle(vec2 pos) { 411 | nook::Size hs = map()->size() / 2; 412 | U32 rx = (U32)(pos.x + hs.width) / RegionSize; 413 | U32 ry = (U32)(pos.y + hs.height) / RegionSize; 414 | Region& r = m_regions[ry * m_regionCount.x + rx]; 415 | 416 | for (Obstacle* ro : r.obstacles) 417 | if (ro->pos == pos) 418 | return ro; 419 | return &m_dummyObstacle; 420 | } 421 | 422 | WallTracing::Obstacle* WallTracing::findObstacle(vec2 pos) { 423 | nook::Size hs = map()->size() / 2; 424 | U32 rx = (U32)(pos.x + hs.width) / RegionSize; 425 | U32 ry = (U32)(pos.y + hs.height) / RegionSize; 426 | Region& r = m_regions[ry * m_regionCount.x + rx]; 427 | 428 | for (Obstacle* ro : r.obstacles) 429 | if ((ro->pos - pos).length() <= ro->radius) 430 | return ro; 431 | return &m_dummyObstacle; 432 | } 433 | 434 | WallTracing::Obstacle* WallTracing::findObstacle(vec2 pos, float radius) { 435 | nook::Size hs = map()->size() / 2; 436 | U32 rx = (U32)(pos.x + hs.width) / RegionSize; 437 | U32 ry = (U32)(pos.y + hs.height) / RegionSize; 438 | Region& r = m_regions[ry * m_regionCount.x + rx]; 439 | 440 | for (Obstacle* ro : r.obstacles) { 441 | if (ro == m_curObstacle) 442 | continue; 443 | if ((ro->pos - pos).length() <= (ro->radius + radius) * kr2) 444 | return ro; 445 | } 446 | return &m_dummyObstacle; 447 | } 448 | 449 | void WallTracing::pushNextCorner(Next* n, Corner* nc) { 450 | if (nc->checked == m_curCheck) 451 | return; 452 | 453 | vec2 cpos = nc->pos + nc->normal * m_curRadius; 454 | Next col = findCollision(n->pos, cpos, false); 455 | 456 | if (col.type) { // can be only obstacle 457 | if (col.obstacle->checked != m_curCheck) { 458 | col.obstacle->checked = m_curCheck; 459 | 460 | float h = heuristics(col.pos, m_end); 461 | float cost = n->cost + col.cost; 462 | 463 | Next* next = m_nextPool.alloc(); 464 | next->type = 2; 465 | next->dir = n->dir; 466 | next->last = col.last; 467 | next->cost = cost; 468 | next->pos = col.pos; 469 | next->from = n; 470 | next->obstacle = col.obstacle; 471 | m_queue.insert(next, h + cost); 472 | 473 | if (h < m_bestH) { 474 | m_bestH = h; 475 | m_best = next; 476 | } 477 | } 478 | } 479 | else { 480 | nc->checked = m_curCheck; 481 | 482 | float h = heuristics(cpos, m_end); 483 | float cost = n->cost + col.cost; 484 | 485 | Next* next = m_nextPool.alloc(); 486 | next->type = 1; 487 | next->dir = n->dir; 488 | next->last = false; 489 | next->cost = cost; 490 | next->pos = cpos; 491 | next->from = n; 492 | next->corner = nc; 493 | m_queue.insert(next, h + cost); 494 | 495 | if (h < m_bestH) { 496 | m_bestH = h; 497 | m_best = next; 498 | } 499 | } 500 | } 501 | 502 | void WallTracing::pushNextObstacle(Next* n) { 503 | Obstacle* o = n->obstacle; 504 | o->request = m_curRequest + 1; 505 | vec2 cp = o->pos - n->pos; 506 | vec2 ne = m_end - n->pos; 507 | 508 | float d2 = cp.length2(); 509 | float r = o->radius + m_curRadius + 0.01f; 510 | float r2 = r * r; 511 | float tr = std::sqrt(r2 * (kr2 * kr2 - 1.0f)); 512 | float s = (int)(n->dir & 2) - 1; 513 | vec2 dir; 514 | 515 | if (d2 <= r2) { 516 | float trr = tr / r * s; 517 | dir = vec2(cp.y * trr, -cp.x * trr); 518 | } 519 | else { 520 | float t2 = d2 - r2; 521 | float t = std::sqrt(t2); 522 | float rd2 = 1.0f / d2; 523 | vec2 tcp = cp * t; 524 | vec2 rcp = cp * r; 525 | float ux = (tcp.x + rcp.y * s) * rd2; 526 | float uy = (tcp.y - rcp.x * s) * rd2; 527 | dir = vec2(ux, uy) * (t + tr); 528 | } 529 | 530 | if (dir.cross(ne) * s <= 0.0f) { 531 | pushNextCollision(n); 532 | } 533 | else { 534 | vec2 to = n->pos + dir; 535 | bool last = false; 536 | if (o == m_lastObstacle) 537 | last = lineIntersect(n->pos, to, o->pos, m_lastOLine, to); 538 | 539 | Next col = findCollision(n->pos, to, true); 540 | if (col.type == 1) { 541 | U32 ch = n->dir == 1 ? col.corner->checked : col.corner->right->checked; 542 | if (ch == m_curCheck) 543 | return; 544 | 545 | float h = heuristics(m_end, col.pos); 546 | float cost = n->cost + col.cost; 547 | 548 | Next* next = m_nextPool.alloc(); 549 | next->type = 1; 550 | next->dir = n->dir; 551 | next->last = col.last; 552 | next->cost = cost; 553 | next->pos = col.pos; 554 | next->from = n; 555 | next->obstacle = col.obstacle; 556 | m_queue.insert(next, h + cost); 557 | 558 | if (h < m_bestH) { 559 | m_bestH = h; 560 | m_best = next; 561 | } 562 | } 563 | else if (col.type == 2) { 564 | if (col.obstacle->checked == m_curCheck) 565 | return; 566 | col.obstacle->checked = m_curCheck; 567 | 568 | float h = heuristics(m_end, col.pos); 569 | float cost = n->cost + col.cost; 570 | 571 | Next* next = m_nextPool.alloc(); 572 | next->type = 2; 573 | next->dir = n->dir; 574 | next->last = col.last; 575 | next->cost = cost; 576 | next->pos = col.pos; 577 | next->from = n; 578 | next->obstacle = col.obstacle; 579 | m_queue.insert(next, h + cost); 580 | 581 | if (h < m_bestH) { 582 | m_bestH = h; 583 | m_best = next; 584 | } 585 | } 586 | else { 587 | float h = heuristics(m_end, to); 588 | float cost = n->cost + col.cost; 589 | 590 | Next* next = m_nextPool.alloc(); 591 | next->type = 2; 592 | next->dir = n->dir; 593 | next->last = last; 594 | next->cost = cost; 595 | next->pos = to; 596 | next->from = n; 597 | next->obstacle = o; 598 | m_queue.insert(next, h + cost); 599 | 600 | if (h < m_bestH) { 601 | m_bestH = h; 602 | m_best = next; 603 | } 604 | } 605 | } 606 | } 607 | 608 | void WallTracing::pushNextCollision(Next* n) { 609 | Next col = findCollision(n->pos, m_end, true); 610 | if (col.type == 1) { 611 | float h = heuristics(m_end, col.pos); 612 | float cost = n->cost + col.cost; 613 | float p = cost + h; 614 | 615 | if (col.corner->checked != m_curCheck) { 616 | Next* left = m_nextPool.alloc(); 617 | left->type = 1; 618 | left->dir = 1; 619 | left->last = col.last; 620 | left->cost = cost; 621 | left->pos = col.pos; 622 | left->from = n; 623 | left->corner = col.corner; 624 | m_queue.insert(left, p); 625 | 626 | if (h < m_bestH) { 627 | m_best = left; 628 | m_bestH = h; 629 | } 630 | } 631 | if (col.corner->right->checked != m_curCheck) { 632 | Next* right = m_nextPool.alloc(); 633 | right->type = 1; 634 | right->dir = 2; 635 | right->last = col.last; 636 | right->cost = cost; 637 | right->pos = col.pos; 638 | right->from = n; 639 | right->corner = col.corner; 640 | m_queue.insert(right, p); 641 | 642 | if (h < m_bestH) { 643 | m_best = right; 644 | m_bestH = h; 645 | } 646 | } 647 | } 648 | else if (col.type == 2) { 649 | if (col.obstacle->checked != m_curCheck) { 650 | float h = heuristics(m_end, col.pos); 651 | float cost = n->cost + col.cost; 652 | float p = cost + h; 653 | 654 | Next* left = m_nextPool.alloc(); 655 | left->type = col.type; 656 | left->dir = 1; 657 | left->last = col.last; 658 | left->cost = cost; 659 | left->pos = col.pos; 660 | left->from = n; 661 | left->obstacle = col.obstacle; 662 | m_queue.insert(left, p); 663 | 664 | Next* right = m_nextPool.alloc(); 665 | right->type = col.type; 666 | right->dir = 2; 667 | right->last = col.last; 668 | right->cost = cost; 669 | right->pos = col.pos; 670 | right->from = n; 671 | right->obstacle = col.obstacle; 672 | m_queue.insert(right, p); 673 | 674 | if (h < m_bestH) { 675 | m_best = left; 676 | m_bestH = h; 677 | } 678 | } 679 | } 680 | else { 681 | Next* next = m_nextPool.alloc(); 682 | next->last = true; 683 | next->pos = m_end; 684 | next->from = n; 685 | m_queue.insert(next, n->cost + col.cost); 686 | } 687 | } 688 | 689 | WallTracing::Next WallTracing::findCollision(vec2 from, vec2 to, bool checkCorners) { 690 | m_curRequest++; 691 | m_curObstacle->request = m_curRequest; 692 | 693 | Next col; 694 | col.type = 0; 695 | col.pos = to; 696 | col.last = false; 697 | col.cost = (from - to).length2(); 698 | vec2 dir = to - from; 699 | 700 | nook::Size hs = map()->size() / 2; 701 | Point2 bl, tr; 702 | if (from.x < to.x) { 703 | bl.x = from.x - m_curRadius + hs.width; 704 | tr.x = to.x + m_curRadius + hs.width; 705 | } 706 | else { 707 | bl.x = to.x - m_curRadius + hs.width; 708 | tr.x = from.x + m_curRadius + hs.width; 709 | } 710 | if (from.y < to.y) { 711 | bl.y = from.y - m_curRadius + hs.height; 712 | tr.y = to.y + m_curRadius + hs.height; 713 | } 714 | else { 715 | bl.y = to.y - m_curRadius + hs.height; 716 | tr.y = from.y + m_curRadius + hs.height; 717 | } 718 | bl /= RegionSize; 719 | tr /= RegionSize; 720 | 721 | if (checkCorners) { 722 | for (int y = bl.y; y <= tr.y; y++) 723 | for (int x = bl.x; x <= tr.x; x++) { 724 | Region& r = m_regions[y * m_regionCount.x + x]; 725 | for (Corner* c : r.corners) 726 | if (c->request != m_curRequest) { 727 | c->request = m_curRequest; 728 | 729 | Corner* right = c->right; 730 | vec2 l1 = c->pos + c->normal * m_curRadius; 731 | vec2 l2 = right->pos + right->normal * m_curRadius; 732 | vec2 p; 733 | 734 | if (from == l1 || from == l2) { 735 | if (from == l2) { 736 | right->request = m_curRequest; 737 | c = right; 738 | } 739 | else 740 | c->left->request = m_curRequest; 741 | 742 | if (c->outer) { 743 | if (dir.x * c->normal.x < 0.0f && dir.y * c->normal.y < 0.0f) { 744 | col.type = 1; 745 | col.cost = 0.0f; 746 | col.pos = from; 747 | col.corner = c; 748 | return col; 749 | } 750 | else 751 | continue; 752 | } 753 | else { 754 | if (dir.x * c->normal.x < 0.0f || dir.y * c->normal.y < 0.0f) { 755 | col.type = 1; 756 | col.cost = 0.0f; 757 | col.pos = from; 758 | col.corner = c; 759 | return col; 760 | } 761 | else 762 | continue; 763 | } 764 | } 765 | 766 | if (lineIntersect(from, to, l1, l2, p)) { 767 | if (p == from) { 768 | if (c->coord.x == right->coord.x) { 769 | if (c->normal.x * dir.x >= 0.0f) 770 | continue; 771 | } 772 | else { 773 | if (c->normal.y * dir.y >= 0.0f) 774 | continue; 775 | } 776 | col.type = 1; 777 | col.cost = 0.0f; 778 | col.pos = from; 779 | col.corner = c; 780 | return col; 781 | } 782 | 783 | float dist2 = (p - from).length2(); 784 | if (dist2 < col.cost) { 785 | col.type = 1; 786 | col.cost = dist2; 787 | col.pos = p; 788 | col.corner = c; 789 | } 790 | } 791 | } 792 | } 793 | } 794 | 795 | for (int y = bl.y; y <= tr.y; y++) 796 | for (int x = bl.x; x <= tr.x; x++) { 797 | Region& r = m_regions[y * m_regionCount.x + x]; 798 | for (Obstacle* o : r.obstacles) { 799 | if (o->request != m_curRequest) { 800 | o->request = m_curRequest; 801 | 802 | Circle c(o->pos, o->radius + m_curRadius); 803 | vec2 p; 804 | if (circleLineIntersectOutside(c, from, to, p)) { 805 | if (p == from) { 806 | vec2 vc = c.pos - from; 807 | if (dir.dot(vc) <= 0.0f) 808 | continue; 809 | col.type = 2; 810 | col.cost = 0.0f; 811 | col.last = o == m_endObstacle; 812 | col.pos = from; 813 | col.obstacle = o; 814 | return col; 815 | } 816 | 817 | float dist2 = (p - from).length2(); 818 | if (dist2 < col.cost) { 819 | col.type = 2; 820 | col.cost = dist2; 821 | col.pos = p; 822 | col.obstacle = o; 823 | } 824 | } 825 | } 826 | } 827 | } 828 | 829 | col.cost = std::sqrt(col.cost); 830 | 831 | if (col.type == 2) { 832 | float rd = (col.obstacle->radius + m_curRadius) * (kr2 - 1.0f); 833 | if (col.cost > rd) 834 | col.pos += (from - col.pos) * (rd / col.cost); 835 | col.last = col.obstacle == m_endObstacle; 836 | } 837 | 838 | return col; 839 | } 840 | -------------------------------------------------------------------------------- /src/WallTracing.hpp: -------------------------------------------------------------------------------- 1 | 2 | #pragma once 3 | 4 | #include "Coord.hpp" 5 | 6 | class WallTracing { 7 | public: 8 | WallTracing(); 9 | ~WallTracing(); 10 | 11 | void addObstacle(nook::Circle o); 12 | void removeObstacle(nook::Circle o); 13 | 14 | void find(nook::vec2 start, nook::vec2 end, float radius, nook::Array& path); 15 | 16 | private: 17 | static constexpr int RegionSize = 4; 18 | 19 | struct Corner { 20 | nook::U32 checked; 21 | nook::U32 request; 22 | 23 | nook::vec2 pos; 24 | nook::vec2 normal; 25 | Coord coord; 26 | bool outer; 27 | 28 | Corner* left; 29 | Corner* right; 30 | 31 | bool operator==(const Corner& c) { return coord == c.coord && normal == c.normal; } 32 | }; 33 | 34 | struct Obstacle { 35 | nook::U32 checked; 36 | nook::U32 request; 37 | 38 | nook::vec2 pos; 39 | float radius; 40 | }; 41 | 42 | struct Region { 43 | nook::List1 corners; 44 | nook::List1 obstacles; 45 | }; 46 | 47 | struct Next { 48 | nook::U8 type; // 1-corner, 2-obstacle 49 | nook::U8 dir; // 1-left, 2-right 50 | bool last; 51 | float cost; 52 | nook::vec2 pos; 53 | Next* from; 54 | union { 55 | Corner* corner; 56 | Obstacle* obstacle; 57 | }; 58 | }; 59 | 60 | Coord getRegion(Corner* c) { 61 | int rx = c->coord.x / RegionSize; 62 | int ry = c->coord.y / RegionSize; 63 | rx -= (c->coord.x % RegionSize == 0) & (c->outer ^ (c->normal.x < 0)); 64 | ry -= (c->coord.y % RegionSize == 0) & (c->outer ^ (c->normal.y < 0)); 65 | return Coord(rx, ry); 66 | } 67 | 68 | // dir: 1-left. 2-right, 4-up, 8-down 69 | Corner getCorner(Coord coord, nook::U32 cell, int dir); 70 | 71 | Obstacle* getObstacle(nook::vec2 pos); 72 | Obstacle* findObstacle(nook::vec2 pos); 73 | Obstacle* findObstacle(nook::vec2 pos, float radius); 74 | nook::vec2 getLeftObstacle(nook::vec2 from, nook::Circle o); 75 | nook::vec2 getRightObstacle(nook::vec2 from, nook::Circle o); 76 | void pushNextCorner(Next* n, Corner* nc); 77 | void pushNextObstacle(Next* n); 78 | void pushNextCollision(Next* n); 79 | 80 | Next findCollision(nook::vec2 from, nook::vec2 to, bool checkCorners); 81 | 82 | nook::Size m_size; 83 | nook::Point2 m_regionCount; 84 | Region* m_regions; 85 | nook::PagePool m_cornerPool; 86 | nook::PagePool m_obstaclePool; 87 | nook::PagePool m_nextPool; 88 | nook::PriorityQueue m_queue; 89 | nook::List m_path; 90 | 91 | nook::U32 m_curCheck; 92 | nook::U32 m_curRequest; 93 | 94 | Obstacle m_dummyObstacle; 95 | Obstacle* m_curObstacle; 96 | Obstacle* m_endObstacle; 97 | Obstacle* m_lastObstacle; 98 | nook::vec2 m_lastOLine; 99 | 100 | Next* m_best; 101 | float m_bestH; 102 | float m_curRadius; 103 | int m_iter; 104 | nook::vec2 m_end; 105 | }; 106 | -------------------------------------------------------------------------------- /src/geometry.hpp: -------------------------------------------------------------------------------- 1 | 2 | namespace nook { 3 | 4 | enum class IntersectResult { 5 | Outside, 6 | Inside, 7 | Intersecting 8 | }; 9 | 10 | struct Point2 { 11 | S32 x; 12 | S32 y; 13 | 14 | Point2() = default; 15 | Point2(const Point2&) = default; 16 | Point2(S32 x, S32 y) : x(x), y(y) {} 17 | Point2(const vec2& p) : x(p.x), y(p.y) {} 18 | void set(S32 x, S32 y) { Point2::x = x; Point2::y = y; } 19 | 20 | bool operator==(Point2 p) { return x == p.x && y == p.y; } 21 | bool operator!=(Point2 p) { return x != p.x || y != p.y; } 22 | 23 | Point2& operator=(const Point2& p) = default; 24 | Point2& operator=(const vec2& p) { x = p.x; y = p.y; return *this; } 25 | 26 | Point2& operator+=(const Point2& p) { x += p.x; y += p.y; return *this; } 27 | Point2& operator-=(const Point2& p) { x -= p.x; y -= p.y; return *this; } 28 | Point2& operator+=(int d) { x += d; y += d; return *this; } 29 | Point2& operator-=(int d) { x -= d; y -= d; return *this; } 30 | Point2& operator*=(int d) { x *= d; y *= d; return *this; } 31 | Point2& operator/=(int d) { x /= d; y /= d; return *this; } 32 | }; 33 | 34 | struct Point3 { 35 | S32 x; 36 | S32 y; 37 | S32 z; 38 | 39 | Point3() {} 40 | Point3(S32 x, S32 y, S32 z) : x(x), y(y), z(z) {} 41 | void set(S32 x, S32 y, S32 z) { Point3::x = x; Point3::y = y; Point3::z = z; } 42 | }; 43 | 44 | struct Size { 45 | S32 width; 46 | S32 height; 47 | 48 | Size() {} 49 | Size(S32 w, S32 h) { width = w; height = h; } 50 | void set(S32 w, S32 h) { width = w; height = h; } 51 | 52 | bool operator==(const Size& size) { return width == size.width && height == size.height; } 53 | bool operator!=(const Size& size) { return width != size.width || height != size.height; } 54 | Size& operator=(int i) { width = height = i; return *this; } 55 | Size& operator+=(int d) { width += d; height += d; return *this; } 56 | Size& operator-=(int d) { width -= d; height -= d; return *this; } 57 | Size& operator+=(Size s) { width += s.width; height += s.height; return *this; } 58 | Size& operator-=(Size s) { width -= s.width; height -= s.height; return *this; } 59 | Size& operator*=(int d) { width *= d; height *= d; return *this; } 60 | Size& operator/=(int d) { width /= d; height /= d; return *this; } 61 | }; 62 | 63 | inline Size operator+(Size sz, int d); 64 | inline Size operator-(Size sz, int d); 65 | inline Size operator+(Size s1, Size s2); 66 | inline Size operator-(Size s1, Size s2); 67 | inline Size operator*(Size sz, int d); 68 | inline Size operator/(Size sz, int d); 69 | 70 | struct Circle { 71 | vec2 pos; 72 | float radius; 73 | 74 | Circle() = default; 75 | Circle(const vec2& pos, float radius) : pos(pos), radius(radius) {} 76 | }; 77 | 78 | struct Rect { 79 | S32 x; 80 | S32 y; 81 | S32 width; 82 | S32 height; 83 | 84 | Rect() {} 85 | Rect(S32 x, S32 y, S32 width, S32 height) : x(x), y(y), width(width), height(height) {} 86 | 87 | void set(S32 x, S32 y, S32 width, S32 height); 88 | bool contains(const Rect& rect) const; 89 | }; 90 | 91 | struct Rectf { 92 | vec2 origin; 93 | vec2 end; 94 | 95 | Rectf() {} 96 | Rectf(float x, float y, float endX, float endY) : origin(x, y), end(endX, endY) {} 97 | Rectf(const vec2& origin, const vec2& end) : origin(origin), end(end) {} 98 | 99 | void set(float x, float y, float endX, float endY); 100 | 101 | bool intersects(const Rectf& rect) const; 102 | bool contains(vec2 p) const; 103 | bool contains(const Rectf& rect) const; 104 | bool contains(const Circle& circle) const; 105 | }; 106 | 107 | struct Ray { 108 | vec3 origin; 109 | vec3 dir; 110 | vec3 inv_dir; 111 | int sign[3]; 112 | 113 | Ray() = default; 114 | Ray(const vec3& origin, const vec3& dir); 115 | }; 116 | 117 | struct Box { 118 | vec3 bounds[2]; 119 | 120 | Box() = default; 121 | Box(const vec3& bmin, const vec3& bmax); 122 | 123 | vec3 center() const { return (bounds[0] + bounds[1]) * 0.5f; } 124 | vec2 centerXZ() const { return vec2((bounds[0].x + bounds[1].x) * 0.5f, (bounds[0].z + bounds[1].z) * 0.5f); } 125 | Rectf boundsXZ() const { return Rectf(bounds[0].x, bounds[0].z, bounds[1].x, bounds[1].z); } 126 | 127 | bool intersects(const Ray& ray, float t0, float t1) const; 128 | }; 129 | 130 | struct Triangle { 131 | vec3 vert[3]; 132 | 133 | Triangle() = default; 134 | Triangle(const vec3& v1, const vec3& v2, const vec3& v3); 135 | 136 | const vec3& operator[](int i) const { return vert[i]; } 137 | vec3& operator[](int i) { return vert[i]; } 138 | 139 | vec3 barycentric(vec3 p) const; 140 | float getY(vec2 p) const; 141 | bool intersects(const Ray& ray, float& t, float& u, float& v) const; 142 | }; 143 | 144 | struct Sphere { 145 | vec3 pos; 146 | float radius; 147 | 148 | Sphere() = default; 149 | Sphere(const vec3& pos, float radius) : pos(pos), radius(radius) {} 150 | 151 | void combine(const vec3& p); 152 | void combine(const Sphere& s); 153 | bool intersects(const Ray& ray) const; 154 | 155 | static Sphere fromPoints(vec3* pts, U32 count); 156 | }; 157 | 158 | struct Geometry { 159 | enum class Type { 160 | Box, 161 | Sphere, 162 | } type; 163 | 164 | union { 165 | Box box; 166 | Sphere sphere; 167 | }; 168 | 169 | void setBox(const Box& box); 170 | void setSphere(const vec3& pos, float radius); 171 | 172 | }; 173 | 174 | inline float sign(vec2 p1, vec2 p2, vec2 p3); 175 | inline bool pointInTriangle(vec2 pt, vec2 v1, vec2 v2, vec2 v3); 176 | inline vec2 pointToLine(vec2 p, vec2 l1, vec2 l2); 177 | inline bool lineIntersect(vec2 p0, vec2 p1, vec2 p2, vec2 p3, vec2& i); 178 | inline bool circleLineIntersect(const Circle& c, vec2 l1, vec2 l2); 179 | 180 | inline void frustumPlanes(const mat4& m, vec4 planes[6]); 181 | inline IntersectResult planeAABBIntersect(const vec4& plane, const Box& box); 182 | inline IntersectResult frustumAABBIntersect(const vec4 planes[6], const Box& box); 183 | inline bool frustumSphereIntersect(const vec4 planes[6], const Sphere& sphere); 184 | 185 | 186 | #pragma mark - Implementation 187 | 188 | inline Size operator+(Size sz, int d) { 189 | return sz += d; 190 | } 191 | 192 | inline Size operator-(Size sz, int d) { 193 | return sz -= d; 194 | } 195 | 196 | inline Size operator+(Size s1, Size s2) { 197 | return s1 += s2; 198 | } 199 | 200 | inline Size operator-(Size s1, Size s2) { 201 | return s1 -= s2; 202 | } 203 | 204 | inline Size operator*(Size sz, int d) { 205 | return sz *= d; 206 | } 207 | 208 | inline Size operator/(Size sz, int d) { 209 | return sz /= d; 210 | } 211 | 212 | inline void Rect::set(S32 x, S32 y, S32 width, S32 height) { 213 | Rect::x = x; 214 | Rect::y = y; 215 | Rect::width = width; 216 | Rect::height = height; 217 | } 218 | 219 | inline bool Rect::contains(const Rect& rect) const { 220 | return (rect.x >= x) && (rect.y >= y) && (rect.x + rect.width <= x + width) && 221 | (rect.y + rect.height <= y + height); 222 | } 223 | 224 | inline void Rectf::set(float x, float y, float endX, float endY) { 225 | origin.set(x, y); 226 | end.set(endX, endY); 227 | } 228 | 229 | inline bool Rectf::intersects(const Rectf& rect) const { 230 | return origin.x <= rect.end.x && rect.origin.x <= end.x && origin.y <= rect.end.y && rect.origin.y <= end.y; 231 | } 232 | 233 | inline bool Rectf::contains(vec2 p) const { 234 | return origin.x <= p.x && end.x >= p.x && origin.y <= p.y && end.y >= p.y; 235 | } 236 | 237 | inline bool Rectf::contains(const Rectf& rect) const { 238 | return origin.x <= rect.origin.x && origin.y <= rect.origin.y && end.x >= rect.end.x && end.y >= rect.end.y; 239 | } 240 | 241 | inline bool Rectf::contains(const Circle& circle) const { 242 | return contains(Rectf(circle.pos - circle.radius, circle.pos + circle.radius)); 243 | } 244 | 245 | inline Ray::Ray(const vec3& origin, const vec3& dir) { 246 | Ray::origin = origin; 247 | Ray::dir = dir; 248 | inv_dir = 1.0f / dir; 249 | sign[0] = dir.x < 0.0f; 250 | sign[1] = dir.y < 0.0f; 251 | sign[2] = dir.z < 0.0f; 252 | } 253 | 254 | inline Box::Box(const vec3& bmin, const vec3& bmax) { 255 | bounds[0] = bmin; 256 | bounds[1] = bmax; 257 | } 258 | 259 | inline bool Box::intersects(const Ray& ray, float t0, float t1) const { 260 | float tmin, tmax, tymin, tymax, tzmin, tzmax; 261 | tmin = (bounds[ray.sign[0]].x - ray.origin.x) * ray.inv_dir.x; 262 | tmax = (bounds[1 - ray.sign[0]].x - ray.origin.x) * ray.inv_dir.x; 263 | tymin = (bounds[ray.sign[1]].y - ray.origin.y) * ray.inv_dir.y; 264 | tymax = (bounds[1 - ray.sign[1]].y - ray.origin.y) * ray.inv_dir.y; 265 | if ((tmin > tymax) || (tymin > tmax)) 266 | return false; 267 | if (tymin > tmin) 268 | tmin = tymin; 269 | if (tymax < tmax) 270 | tmax = tymax; 271 | tzmin = (bounds[ray.sign[2]].z - ray.origin.z) * ray.inv_dir.z; 272 | tzmax = (bounds[1 - ray.sign[2]].z - ray.origin.z) * ray.inv_dir.z; 273 | if ((tmin > tzmax) || (tzmin > tmax)) 274 | return false; 275 | if (tzmin > tmin) 276 | tmin = tzmin; 277 | if (tzmax < tmax) 278 | tmax = tzmax; 279 | return ((tmin < t1) && (tmax > t0)); 280 | } 281 | 282 | inline Triangle::Triangle(const vec3& v1, const vec3& v2, const vec3& v3) { 283 | vert[0] = v1; 284 | vert[1] = v2; 285 | vert[2] = v3; 286 | } 287 | 288 | inline vec3 Triangle::barycentric(vec3 p) const { 289 | vec3 v0 = vert[1] - vert[0]; 290 | vec3 v1 = vert[2] - vert[0]; 291 | vec3 v2 = p - vert[0]; 292 | float d00 = v0.dot(v0); 293 | float d01 = v0.dot(v1); 294 | float d11 = v1.dot(v1); 295 | float d20 = v2.dot(v0); 296 | float d21 = v2.dot(v1); 297 | float invDenom = 1.0f / (d00 * d11 - d01 * d01); 298 | float v = (d11 * d20 - d01 * d21) * invDenom; 299 | float w = (d00 * d21 - d01 * d20) * invDenom; 300 | float u = 1.0f - v - w; 301 | return vec3(v, w, u); 302 | } 303 | 304 | inline float Triangle::getY(vec2 p) const { 305 | float invDet = 1.0f / ((vert[1].z - vert[2].z) * (vert[0].x - vert[2].x) + 306 | (vert[2].x - vert[1].x) * (vert[0].z - vert[2].z)); 307 | 308 | float l1 = ((vert[1].z - vert[2].z) * (p.x - vert[2].x) + (vert[2].x - vert[1].x) * (p.y - vert[2].z)) * invDet; 309 | float l2 = ((vert[2].z - vert[0].z) * (p.x - vert[2].x) + (vert[0].x - vert[2].x) * (p.y - vert[2].z)) * invDet; 310 | float l3 = 1.0f - l1 - l2; 311 | 312 | return l1 * vert[0].y + l2 * vert[1].y + l3 * vert[2].y; 313 | } 314 | 315 | inline bool Triangle::intersects(const Ray& ray, float& t, float& u, float& v) const { 316 | vec3 edge1, edge2, tvec, pvec, qvec; 317 | float det, inv_det; 318 | 319 | edge1 = vert[1] - vert[0]; 320 | edge2 = vert[2] - vert[0]; 321 | pvec = ray.dir.cross(edge2); 322 | det = edge1.dot(pvec); 323 | tvec = ray.origin - vert[0]; 324 | inv_det = 1.0f / det; 325 | 326 | // if (det > 0.00001f) { //CW 327 | // u = tvec.dot(pvec); 328 | // if (u < 0.0f || u > det) 329 | // return false; 330 | // qvec = tvec.cross(edge1); 331 | // v = ray.dir.dot(qvec); 332 | // if (v < 0.0f || u + v > det) 333 | // return false; 334 | // } 335 | // else 336 | if (det < -0.00001f) { //CCW 337 | u = tvec.dot(pvec); 338 | if (u > 0.0f || u < det) 339 | return false; 340 | qvec = tvec.cross(edge1); 341 | v = ray.dir.dot(qvec) ; 342 | if (v > 0.0f || u + v < det) 343 | return false; 344 | } 345 | else 346 | return false; 347 | 348 | t = edge2.dot(qvec) * inv_det; 349 | u *= inv_det; 350 | v *= inv_det; 351 | return true; 352 | } 353 | 354 | inline void Sphere::combine(const vec3& p) { 355 | vec3 v = p - pos; 356 | float d = v.length(); 357 | if (d > radius) { 358 | float a = (d - radius) * 0.5f; 359 | pos += v * (a / d); 360 | radius += a; 361 | } 362 | } 363 | 364 | inline void Sphere::combine(const Sphere& s) { 365 | const Sphere& maxSphere = s.radius > radius ? s : *this; 366 | const Sphere& minSphere = s.radius > radius ? *this : s; 367 | 368 | float newRadius = (maxSphere.radius + minSphere.radius + vec3(minSphere.pos - maxSphere.pos).length()) * 0.5f; 369 | if (newRadius <= maxSphere.radius) { 370 | pos = maxSphere.pos; 371 | radius = maxSphere.radius; 372 | } 373 | else { 374 | vec3 norm = (minSphere.pos - maxSphere.pos).normalized(); 375 | pos = maxSphere.pos + norm * (newRadius - maxSphere.radius); 376 | radius = newRadius; 377 | } 378 | } 379 | 380 | inline bool Sphere::intersects(const nook::Ray& ray) const { 381 | vec3 l = pos - ray.origin; 382 | float s = l.dot(ray.dir); 383 | float l2 = l.dot(l); 384 | float r2 = radius * radius; 385 | if (s < 0 && l2 > r2) 386 | return false; 387 | float m2 = l2 - s * s; 388 | return m2 <= r2; 389 | } 390 | 391 | inline Sphere Sphere::fromPoints(vec3* pts, U32 count) { 392 | vec3* end = pts + count; 393 | vec3 a = *pts; 394 | vec3 b = *pts; 395 | float d2 = 0.0f; 396 | for (vec3* p = pts + 1; p != end; p++) { 397 | float pd = (*p - a).length2(); 398 | if (pd > d2) { 399 | d2 = pd; 400 | b = *p; 401 | } 402 | } 403 | for (vec3* p = pts; p != end; p++) { 404 | float pd = (*p - b).length2(); 405 | if (pd > d2) { 406 | d2 = pd; 407 | a = *p; 408 | } 409 | } 410 | vec3 pos = (a + b) * 0.5f; 411 | float r = sqrt(d2) * 0.5f; 412 | float r2 = r * r; 413 | for (vec3* p = pts; p != end; p++) { 414 | vec3 v = *p - pos; 415 | float d2 = v.length2(); 416 | if (d2 > r2) { 417 | float d = sqrt(d2); 418 | float a = (d - r) * 0.5f; 419 | pos += v * (a / d); 420 | r += a; 421 | r2 = r * r; 422 | } 423 | } 424 | return Sphere(pos, r); 425 | } 426 | 427 | inline void Geometry::setBox(const Box& box) { 428 | type = Type::Box; 429 | Geometry::box = box; 430 | } 431 | 432 | inline void Geometry::setSphere(const vec3& pos, float radius) { 433 | type = Type::Sphere; 434 | sphere.pos = pos; 435 | sphere.radius = radius; 436 | } 437 | 438 | // >0 is left 439 | inline float sign(vec2 p, vec2 v1, vec2 v2) { 440 | return (p.x - v2.x) * (v1.y - v2.y) - (v1.x - v2.x) * (p.y - v2.y); 441 | } 442 | 443 | inline bool pointInTriangle(vec2 pt, vec2 v1, vec2 v2, vec2 v3) { 444 | bool b1, b2, b3; 445 | b1 = sign(pt, v1, v2) < 0.0f; 446 | b2 = sign(pt, v2, v3) < 0.0f; 447 | b3 = sign(pt, v3, v1) < 0.0f; 448 | return ((b1 == b2) && (b2 == b3)); 449 | } 450 | 451 | inline vec2 pointToLine(vec2 p, vec2 l1, vec2 l2) { 452 | vec2 v = l2 - l1; 453 | vec2 w = p - l1; 454 | 455 | float c1 = w.dot(v); 456 | if (c1 <= 0) 457 | return l1 - p; 458 | 459 | float c2 = v.dot(v); 460 | if (c2 <= c1) 461 | return l2 - p; 462 | 463 | float b = c1 / c2; 464 | vec2 pb = l1 + b * v; 465 | return pb - p; 466 | } 467 | 468 | inline void frustumPlanes(const mat4& m, vec4 planes[6]) { 469 | planes[0] = (m.row(3) + m.row(0)).normalize(); // left 470 | planes[1] = (m.row(3) - m.row(0)).normalize(); // right 471 | planes[2] = (m.row(3) + m.row(1)).normalize(); // bottom 472 | planes[3] = (m.row(3) - m.row(1)).normalize(); // top 473 | planes[4] = (m.row(3) + m.row(2)).normalize(); // near 474 | planes[5] = (m.row(3) - m.row(2)).normalize(); // far 475 | } 476 | 477 | inline bool lineIntersect(vec2 p0, vec2 p1, vec2 p2, vec2 p3, vec2& i) { 478 | vec2 s1 = p1 - p0; 479 | vec2 s2 = p3 - p2; 480 | float d = -s2.x * s1.y + s1.x * s2.y; 481 | if (d == 0.0f) 482 | return false; 483 | d = 1.0f / d; 484 | float s = (-s1.y * (p0.x - p2.x) + s1.x * (p0.y - p2.y)) * d; 485 | float t = ( s2.x * (p0.y - p2.y) - s2.y * (p0.x - p2.x)) * d; 486 | if (s >= 0.0f && s <= 1.0f && t >= 0.0f && t <= 1.0f) { 487 | i.x = p0.x + (t * s1.x); 488 | i.y = p0.y + (t * s1.y); 489 | return true; 490 | } 491 | return false; 492 | } 493 | 494 | // works only when l1 is outside 495 | inline bool circleLineIntersectOutside(const Circle& circle, vec2 l1, vec2 l2, vec2& i) { 496 | vec2 d = l2 - l1; 497 | vec2 f = l1 - circle.pos; 498 | float a = d.length2(); 499 | float b = 2.0f * f.dot(d); 500 | float c = f.length2() - circle.radius * circle.radius; 501 | 502 | float dis = b * b - 4.0f * a * c; 503 | if (dis < 0.0f) 504 | return false; 505 | 506 | dis = std::sqrt(dis); 507 | float t = (-b - dis) / (a * 2.0f); 508 | if (t >= 0.0f && t <= 1.0f) { 509 | i = l1 + d * t; 510 | return true; 511 | } 512 | return false; 513 | } 514 | 515 | inline bool circleLineIntersect(const Circle& circle, vec2 l1, vec2 l2, vec2& i) { 516 | vec2 d = l2 - l1; 517 | vec2 f = l1 - circle.pos; 518 | float a = d.length2(); 519 | float b = 2.0f * f.dot(d); 520 | float c = f.length2() - circle.radius * circle.radius; 521 | 522 | float dis = b * b - 4.0f * a * c; 523 | if (dis < 0.0f) 524 | return false; 525 | 526 | dis = std::sqrt(dis); 527 | a = 0.5f / a; 528 | float t1 = (-b - dis) * a; 529 | float t2 = (-b + dis) * a; 530 | 531 | if (t1 >= 0.0f && t1 <= 1.0f) { 532 | i = l1 + d * t1; 533 | return true; 534 | } 535 | if (t2 >= 0.0f && t2 <= 1.0f) { 536 | i = l1 + d * t2; 537 | return true; 538 | } 539 | return false; 540 | } 541 | 542 | inline IntersectResult planeAABBIntersect(const vec4& plane, const Box& box) { 543 | int sx = plane.x > 0.0f; 544 | int sy = plane.y > 0.0f; 545 | int sz = plane.z > 0.0f; 546 | 547 | float dot = plane.x * box.bounds[sx].x + plane.y * box.bounds[sy].y + plane.z * box.bounds[sz].z; 548 | if (dot < -plane.w) 549 | return IntersectResult::Outside; 550 | 551 | float dot2 = plane.x * box.bounds[1-sx].x + plane.y * box.bounds[1-sy].y + plane.z * box.bounds[1-sz].z; 552 | if (dot2 < -plane.w) 553 | return IntersectResult::Intersecting; 554 | 555 | return IntersectResult::Inside; 556 | } 557 | 558 | inline IntersectResult frustumAABBIntersect(const vec4 planes[6], const Box& box) { 559 | IntersectResult result = IntersectResult::Inside; 560 | for (int i = 0; i < 6; i++) { 561 | const vec4& plane = planes[i]; 562 | int sx = plane.x > 0.0f; 563 | int sy = plane.y > 0.0f; 564 | int sz = plane.z > 0.0f; 565 | 566 | float dot = plane.x * box.bounds[sx].x + plane.y * box.bounds[sy].y + plane.z * box.bounds[sz].z; 567 | if (dot < -plane.w) 568 | return IntersectResult::Outside; 569 | 570 | float dot2 = plane.x * box.bounds[1-sx].x + plane.y * box.bounds[1-sy].y + plane.z * box.bounds[1-sz].z; 571 | if (dot2 < -plane.w) 572 | result = IntersectResult::Intersecting; 573 | } 574 | return result; 575 | } 576 | 577 | inline bool frustumSphereIntersect(const vec4 planes[6], const Sphere& sphere) { 578 | for (int i = 0; i < 6; i++) { 579 | const vec4& plane = planes[i]; 580 | float side = sphere.pos.dot(plane) + plane.w; 581 | if (side < -sphere.radius) 582 | return false; 583 | } 584 | return true; 585 | } 586 | 587 | } 588 | --------------------------------------------------------------------------------