double distance(struct position a, struct position b,
const region_map_t* regs) {
// find a region that fits a.
bool a_in_reg = false;
bool b_in_reg = false;
int r;
for (r=0; r<SIZE(*regs); ++r) {
if (within(a, GET(*regs,r))) {
a_in_reg = true;
}
if (within(b, GET(*regs,r))) {
b_in_reg = true;
}
if (within(a, GET(*regs,r)) && within(b, GET(*regs,r))) {
return rawDistance(a,b);
}
}
/* if (!a_in_reg) {
iprintf ("%7d WARNING(a): (%4.1f, %4.1f, %4.1f) is not in any region.\n", warn_count,
a.x, a.y, a.z);
warn_count++;
}
if (!b_in_reg) {
iprintf ("%7d WARNING(b): (%4.1f, %4.1f, %4.1f) is not in any region.\n", warn_count,
b.x, b.y, b.z);
warn_count++;
}
*/
return INFINITY;
}
bool makeWaypointTable(waypoint_vec_t *dest,
const region_map_t* regs,
int nwaypoints,
struct waypoint_init* inits) {
struct waypoint init = {{0.0, 0.0}, 0, {INFINITY, INFINITY}};
assert(dest);
assert(inits);
if (nwaypoints > NR_POINTS)
return false;
int i;
for (i=0; i<NR_POINTS; ++i)
init.distances[i] = INFINITY;
CLEAR(*dest);
int w;
for (w=0; w<nwaypoints; w++) {
PUSH_BACK(*dest, init);
GET(*dest,w).pos = inits[w].p;
GET(*dest,w).flags = inits[w].flags;
int r;
int offset;
for (r=0; r<nwaypoints; ++r) {
int r_local = r;
if (r > 31) {
offset = 0;
r_local -= 32;
} else {
offset = 1;
}
if(inits[w].reachable_bitmap[offset] & (1 << r_local)) {
// iprintf (" %s: bit %d of 0x%x is set, connecting to %s\n",
// inits[w].comment, r, inits[w].reachable_bitmap, inits[r].comment);
GET(*dest,w).distances[r] = rawDistance(GET(*dest,w).pos, inits[r].p);
}
else {
GET(*dest, w).distances[r] = INFINITY;
}
// all routing is now explicit.
/*else {
GET(*dest,w).distances[r] = distance(GET(*dest,w).pos, inits[r].p, regs);
}*/
}
}
return true;
}
int Map::rawDistance(int locX, int locY, int destX, int destY)
{
return rawDistance(locY * width() + locX, destY * width() + destX);
}
