* @brief add a step from pPath to destination, return 1 if successful, and update the frontier/visited nodes accordingly * * @param pathIndex index of the current path node * @param candidatePosition expected to be a neighbour of the current path node position * @param destinationPosition where we hope to end up * @return true if step successfully added, false if we ran out of nodes to u
| 236 | * @return true if step successfully added, false if we ran out of nodes to use |
| 237 | */ |
| 238 | bool ParentPath(uint16_t pathIndex, Point candidatePosition, Point destinationPosition) |
| 239 | { |
| 240 | PathNode &path = PathNodes[pathIndex]; |
| 241 | int nextG = path.g + CheckEqual(path.position(), candidatePosition); |
| 242 | |
| 243 | // 3 cases to consider |
| 244 | // case 1: (dx,dy) is already on the frontier |
| 245 | uint16_t dxdyIndex = GetNode1(candidatePosition); |
| 246 | if (dxdyIndex != PathNode::InvalidIndex) { |
| 247 | path.addChild(dxdyIndex); |
| 248 | PathNode &dxdy = PathNodes[dxdyIndex]; |
| 249 | if (nextG < dxdy.g) { |
| 250 | if (path_solid_pieces(path.position(), candidatePosition)) { |
| 251 | // we'll explore it later, just update |
| 252 | dxdy.parentIndex = pathIndex; |
| 253 | dxdy.g = nextG; |
| 254 | dxdy.f = nextG + dxdy.h; |
| 255 | } |
| 256 | } |
| 257 | } else { |
| 258 | // case 2: (dx,dy) was already visited |
| 259 | dxdyIndex = GetNode2(candidatePosition); |
| 260 | if (dxdyIndex != PathNode::InvalidIndex) { |
| 261 | path.addChild(dxdyIndex); |
| 262 | PathNode &dxdy = PathNodes[dxdyIndex]; |
| 263 | if (nextG < dxdy.g && path_solid_pieces(path.position(), candidatePosition)) { |
| 264 | // update the node |
| 265 | dxdy.parentIndex = pathIndex; |
| 266 | dxdy.g = nextG; |
| 267 | dxdy.f = nextG + dxdy.h; |
| 268 | // already explored, so re-update others starting from that node |
| 269 | SetCoords(dxdyIndex); |
| 270 | } |
| 271 | } else { |
| 272 | // case 3: (dx,dy) is totally new |
| 273 | dxdyIndex = NewStep(); |
| 274 | if (dxdyIndex == PathNode::InvalidIndex) |
| 275 | return false; |
| 276 | PathNode &dxdy = PathNodes[dxdyIndex]; |
| 277 | dxdy.parentIndex = pathIndex; |
| 278 | dxdy.g = nextG; |
| 279 | dxdy.h = GetHeuristicCost(candidatePosition, destinationPosition); |
| 280 | dxdy.f = nextG + dxdy.h; |
| 281 | dxdy.x = static_cast<int16_t>(candidatePosition.x); |
| 282 | dxdy.y = static_cast<int16_t>(candidatePosition.y); |
| 283 | // add it to the frontier |
| 284 | NextNode(dxdyIndex); |
| 285 | path.addChild(dxdyIndex); |
| 286 | } |
| 287 | } |
| 288 | return true; |
| 289 | } |
| 290 | |
| 291 | /** |
| 292 | * @brief perform a single step of A* bread-first search by trying to step in every possible direction from pPath with goal (x,y). Check each step with PosOk |
no test coverage detected