| 542 | |
| 543 | |
| 544 | void DynamicEDT3D::commitAndColorize(bool updateRealDist) { |
| 545 | // ADD NEW OBSTACLES |
| 546 | for (unsigned int i=0; i<addList.size(); i++) { |
| 547 | INTPOINT3D p = addList[i]; |
| 548 | int x = p.x; |
| 549 | int y = p.y; |
| 550 | int z = p.z; |
| 551 | dataCell c = data[x][y][z]; |
| 552 | |
| 553 | if(c.queueing != fwQueued){ |
| 554 | if (updateRealDist) c.dist = 0; |
| 555 | c.sqdist = 0; |
| 556 | c.obstX = x; |
| 557 | c.obstY = y; |
| 558 | c.obstZ = z; |
| 559 | c.queueing = fwQueued; |
| 560 | data[x][y][z] = c; |
| 561 | open.push(0, INTPOINT3D(x,y,z)); |
| 562 | } |
| 563 | } |
| 564 | |
| 565 | // REMOVE OLD OBSTACLES |
| 566 | for (unsigned int i=0; i<removeList.size(); i++) { |
| 567 | INTPOINT3D p = removeList[i]; |
| 568 | int x = p.x; |
| 569 | int y = p.y; |
| 570 | int z = p.z; |
| 571 | dataCell c = data[x][y][z]; |
| 572 | |
| 573 | if (isOccupied(x,y,z,c)==true) continue; // obstacle was removed and reinserted |
| 574 | open.push(0, INTPOINT3D(x,y,z)); |
| 575 | if (updateRealDist) c.dist = maxDist; |
| 576 | c.sqdist = maxDist_squared; |
| 577 | c.needsRaise = true; |
| 578 | data[x][y][z] = c; |
| 579 | } |
| 580 | removeList.clear(); |
| 581 | addList.clear(); |
| 582 | } |
| 583 | |
| 584 | bool DynamicEDT3D::isOccupied(int x, int y, int z) const { |
| 585 | dataCell c = data[x][y][z]; |