bool
RasterTileCache::FirstIntersection(const int x0, const int y0,
const int x1, const int y1,
int h_origin,
int h_dest,
const int slope_fact, const int h_ceiling,
const int h_safety,
RasterLocation &_location, int &_h,
const bool can_climb) const
{
RasterLocation location(x0, y0);
if (!IsInside(location))
// origin is outside overall bounds
return false;
const TerrainHeight h_origin2 = GetFieldDirect(x0, y0).first;
if (h_origin2.IsInvalid()) {
_location = location;
_h = h_origin;
return true;
}
if (!h_origin2.IsSpecial())
h_origin = std::max(h_origin, (int)h_origin2.GetValue());
h_dest = std::max(h_dest, h_origin);
// line algorithm parameters
const int dx = abs(x1-x0);
const int dy = abs(y1-y0);
int err = dx-dy;
const int sx = (x0 < x1)? 1: -1;
const int sy = (y0 < y1)? 1: -1;
// max number of steps to walk
const int max_steps = (dx+dy);
// calculate number of fine steps to produce a step on the overview field
const int step_fine = std::max(1, max_steps >> INTERSECT_BITS);
// number of steps for update to the overview map
const int step_coarse = std::max(1<< OVERVIEW_BITS, step_fine);
// number of steps to be cleared after climbing over obstruction
const int intersect_steps = 32;
// counter for steps to reach next position to be checked on the field.
unsigned step_counter = 0;
// total counter of fine steps
int total_steps = 0;
// number of steps since intersection
int intersect_counter = 0;
#ifdef DEBUG_TILE
printf("# max steps %d\n", max_steps);
printf("# step coarse %d\n", step_coarse);
printf("# step fine %d\n", step_fine);
#endif
// early exit if origin is too high (should not occur)
if (h_origin> h_ceiling) {
#ifdef DEBUG_TILE
printf("# fint start above ceiling %d %d\n", h_origin, h_ceiling);
#endif
_location = location;
_h = h_origin;
return true;
}
#ifdef DEBUG_TILE
printf("# fint width %d height %d\n", width, height);
#endif
// location of last point within ceiling limit that doesnt intersect
RasterLocation last_clear_location = location;
int last_clear_h = h_origin;
while (true) {
if (!step_counter) {
if (!IsInside(location))
break; // outside bounds
const auto field_direct = GetFieldDirect(location.x, location.y);
if (field_direct.first.IsInvalid())
break;
const int h_terrain = field_direct.first.GetValueOr0() + h_safety;
step_counter = field_direct.second ? step_fine : step_coarse;
// calculate height of glide so far
const int dh = (total_steps * slope_fact) >> RASTER_SLOPE_FACT;
// current aircraft height
int h_int = dh + h_origin;
if (can_climb) {
h_int = std::min(h_int, h_dest);
}
#ifdef DEBUG_TILE
printf("%d %d %d %d %d # fint\n", location.x, location.y, h_int, h_terrain, h_ceiling);
#endif
// this point has intersected if aircraft is below terrain height
const bool this_intersecting = (h_int< h_terrain);
if (this_intersecting) {
intersect_counter = 1;
// when intersecting, consider origin to have started higher
const int h_jump = h_terrain - h_int;
h_origin += h_jump;
if (can_climb) {
// if intersecting beyond desired destination height, allow dest height
// to be increased
if (h_terrain> h_dest)
h_dest = h_terrain;
} else {
// if can't climb, must jump so path is pure glide
h_dest += h_jump;
}
h_int = h_terrain;
}
if (h_int > h_ceiling) {
_location = last_clear_location;
_h = last_clear_h;
#ifdef DEBUG_TILE
printf("# fint reach ceiling\n");
#endif
return true; // reached ceiling
}
if (!this_intersecting) {
if (intersect_counter) {
intersect_counter+= step_counter;
// was intersecting, now cleared.
// exit with small height above terrain
#ifdef DEBUG_TILE
printf("# fint int->clear\n");
#endif
if (intersect_counter >= intersect_steps) {
_location = location;
_h = h_int;
return true;
}
} else {
last_clear_location = location;
last_clear_h = h_int;
}
}
}
if (!intersect_counter && (total_steps == max_steps)) {
#ifdef DEBUG_TILE
printf("# fint cleared\n");
#endif
return false;
}
const int e2 = 2*err;
if (e2 > -dy) {
err -= dy;
location.x += sx;
if (step_counter)
step_counter--;
total_steps++;
}
if (e2 < dx) {
err += dx;
location.y += sy;
if (step_counter)
step_counter--;
total_steps++;
}
}
// early exit due to inability to find clearance after intersecting
if (intersect_counter) {
_location = last_clear_location;
_h = last_clear_h;
#ifdef DEBUG_TILE
printf("# fint early exit\n");
#endif
return true;
}
return false;
}
bool
RasterTileCache::FirstIntersection(int x0, int y0,
int x1, int y1,
short h_origin,
short h_dest,
const int slope_fact, const short h_ceiling,
const short h_safety,
unsigned& _x, unsigned& _y, short &_h,
const bool can_climb) const
{
if (((unsigned)x0 >= width) || ((unsigned)y0 >= height))
// origin is outside overall bounds
return false;
// remember index of active tile, so we dont need to scan each each time
// (unless the guess fails)
int tile_index = -1;
h_origin = std::max(h_origin, GetFieldDirect(x0, y0, tile_index));
h_dest = std::max(h_dest, h_origin);
// line algorithm parameters
const int dx = abs(x1-x0);
const int dy = abs(y1-y0);
int err = dx-dy;
const int sx = (x0 < x1)? 1: -1;
const int sy = (y0 < y1)? 1: -1;
// max number of steps to walk
const int max_steps = (dx+dy);
// calculate number of fine steps to produce a step on the overview field
const int step_fine = std::max(1, max_steps >> INTERSECT_BITS);
// number of steps for update to the overview map
const int step_coarse = std::max(1<< OVERVIEW_BITS, step_fine);
// number of steps to be cleared after climbing over obstruction
const int intersect_steps = 32;
// counter for steps to reach next position to be checked on the field.
unsigned step_counter = 0;
// total counter of fine steps
int total_steps = 0;
// number of steps since intersection
int intersect_counter = 0;
#ifdef DEBUG_TILE
printf("# max steps %d\n", max_steps);
printf("# step coarse %d\n", step_coarse);
printf("# step fine %d\n", step_fine);
#endif
// early exit if origin is too high (should not occur)
if (h_origin> h_ceiling) {
#ifdef DEBUG_TILE
printf("# fint start above ceiling %d %d\n", h_origin, h_ceiling);
#endif
_x = (unsigned)x0;
_y = (unsigned)y0;
_h = h_origin;
return true;
}
#ifdef DEBUG_TILE
printf("# fint width %d height %d\n", width, height);
#endif
short h_terrain = 1;
unsigned x_int= x0, y_int= y0;
short h_int= h_origin;
// location of last point within ceiling limit that doesnt intersect
unsigned last_clear_x = x0;
unsigned last_clear_y = y0;
short last_clear_h = h_origin;
while (h_terrain>=0) {
if (!step_counter) {
if ((_x >= width) || (_y >= height))
break; // outside bounds
h_terrain = GetFieldDirect(x_int, y_int, tile_index)+h_safety;
step_counter = tile_index<0? step_coarse: step_fine;
// calculate height of glide so far
const short dh = (short)((total_steps*slope_fact)>>RASTER_SLOPE_FACT);
// current aircraft height
h_int = dh + h_origin;
if (can_climb) {
h_int = std::min(h_int, h_dest);
}
#ifdef DEBUG_TILE
printf("%d %d %d %d %d # fint\n", x_int, y_int, h_int, h_terrain, h_ceiling);
#endif
// this point has intersected if aircraft is below terrain height
const bool this_intersecting = (h_int< h_terrain);
if (this_intersecting) {
intersect_counter = 1;
// when intersecting, consider origin to have started higher
const short h_jump = h_terrain - h_int;
h_origin += h_jump;
if (can_climb) {
// if intersecting beyond desired destination height, allow dest height
// to be increased
if (h_terrain> h_dest)
h_dest = h_terrain;
} else {
// if can't climb, must jump so path is pure glide
h_dest += h_jump;
}
h_int = h_terrain;
}
if (h_int > h_ceiling) {
_x = last_clear_x;
_y = last_clear_y;
_h = last_clear_h;
#ifdef DEBUG_TILE
printf("# fint reach ceiling\n");
#endif
return true; // reached ceiling
}
if (!this_intersecting) {
if (intersect_counter) {
intersect_counter+= step_counter;
// was intersecting, now cleared.
// exit with small height above terrain
#ifdef DEBUG_TILE
printf("# fint int->clear\n");
#endif
if (intersect_counter >= intersect_steps) {
_x = x_int;
_y = y_int;
_h = h_int;
return true;
}
} else {
last_clear_x = x_int;
last_clear_y = y_int;
last_clear_h = h_int;
}
}
}
if (!intersect_counter && (total_steps == max_steps)) {
#ifdef DEBUG_TILE
printf("# fint cleared\n");
#endif
return false;
}
const int e2 = 2*err;
if (e2 > -dy) {
err -= dy;
x_int += sx;
if (step_counter)
step_counter--;
total_steps++;
}
if (e2 < dx) {
err += dx;
y_int += sy;
if (step_counter)
step_counter--;
total_steps++;
}
}
// early exit due to inability to find clearance after intersecting
if (intersect_counter) {
_x = last_clear_x;
_y = last_clear_y;
_h = last_clear_h;
#ifdef DEBUG_TILE
printf("# fint early exit\n");
#endif
return true;
}
return false;
}
SignedRasterLocation
RasterTileCache::Intersection(const int x0, const int y0,
const int x1, const int y1,
const int h_origin,
const int slope_fact) const
{
SignedRasterLocation location(x0, y0);
if (!IsInside(location))
// origin is outside overall bounds
return {-1, -1};
// line algorithm parameters
const int dx = abs(x1-x0);
const int dy = abs(y1-y0);
int err = dx-dy;
const int sx = (x0 < x1)? 1: -1;
const int sy = (y0 < y1)? 1: -1;
// max number of steps to walk
const int max_steps = (dx+dy);
// calculate number of fine steps to produce a step on the overview field
// step size at selected refinement level
const int refine_step = max_steps >> 5;
// number of steps for update to the fine map
const int step_fine = std::max(1, refine_step);
// number of steps for update to the overview map
const int step_coarse = std::max(1<< OVERVIEW_BITS, step_fine);
// counter for steps to reach next position to be checked on the field.
unsigned step_counter = 0;
// total counter of fine steps
int total_steps = 0;
#ifdef DEBUG_TILE
printf("# max steps %d\n", max_steps);
printf("# step coarse %d\n", step_coarse);
printf("# step fine %d\n", step_fine);
#endif
RasterLocation last_clear_location = location;
int last_clear_h = h_origin;
while (true) {
if (!step_counter) {
if (!IsInside(location))
break;
const auto field_direct = GetFieldDirect(location.x, location.y);
if (field_direct.first.IsInvalid())
break;
const int h_terrain = field_direct.first.GetValueOr0();
step_counter = field_direct.second ? step_fine : step_coarse;
// calculate height of glide so far
const int dh = (total_steps * slope_fact) >> RASTER_SLOPE_FACT;
// current aircraft height
const int h_int = h_origin - dh;
if (h_int < h_terrain) {
if (refine_step<3) // can't refine any further
return RasterLocation(last_clear_location.x, last_clear_location.y);
// refine solution
return Intersection(last_clear_location.x, last_clear_location.y,
location.x, location.y, last_clear_h, slope_fact);
}
if (h_int <= 0)
break; // reached max range
last_clear_location = location;
last_clear_h = h_int;
}
if (total_steps > max_steps)
break;
const int e2 = 2*err;
if (e2 > -dy) {
err -= dy;
location.x += sx;
if (step_counter>0)
step_counter--;
total_steps++;
}
if (e2 < dx) {
err += dx;
location.y += sy;
if (step_counter>0)
step_counter--;
total_steps++;
}
}
// if we reached invalid terrain, assume we can hit MSL
return {-1, -1};
}
RasterLocation
RasterTileCache::Intersection(int x0, int y0,
int x1, int y1,
short h_origin,
const int slope_fact) const
{
unsigned _x = (unsigned)x0;
unsigned _y = (unsigned)y0;
if ((_x >= width) || (_y >= height)) {
// origin is outside overall bounds
return RasterLocation(_x, _y);
}
// remember index of active tile, so we dont need to scan each each time
// (unless the guess fails)
int tile_index = -1;
// line algorithm parameters
const int dx = abs(x1-x0);
const int dy = abs(y1-y0);
int err = dx-dy;
const int sx = (x0 < x1)? 1: -1;
const int sy = (y0 < y1)? 1: -1;
// max number of steps to walk
const int max_steps = (dx+dy);
// calculate number of fine steps to produce a step on the overview field
// step size at selected refinement level
const int refine_step = max_steps >> 5;
// number of steps for update to the fine map
const int step_fine = std::max(1, refine_step);
// number of steps for update to the overview map
const int step_coarse = std::max(1<< OVERVIEW_BITS, step_fine);
// counter for steps to reach next position to be checked on the field.
unsigned step_counter = 0;
// total counter of fine steps
int total_steps = 0;
#ifdef DEBUG_TILE
printf("# max steps %d\n", max_steps);
printf("# step coarse %d\n", step_coarse);
printf("# step fine %d\n", step_fine);
#endif
short h_int= h_origin;
short h_terrain = 1;
unsigned last_clear_x = _x;
unsigned last_clear_y = _y;
short last_clear_h = h_int;
while (h_terrain>=0) {
if (!step_counter) {
if ((_x >= width) || (_y >= height))
break; // outside bounds
h_terrain = GetFieldDirect(_x, _y, tile_index);
step_counter = tile_index<0? step_coarse: step_fine;
// calculate height of glide so far
const short dh = (short)((total_steps*slope_fact)>>RASTER_SLOPE_FACT);
// current aircraft height
h_int = h_origin-dh;
if (h_int < h_terrain) {
if (refine_step<3) // can't refine any further
return RasterLocation(last_clear_x, last_clear_y);
// refine solution
return Intersection(last_clear_x, last_clear_y, _x, _y, last_clear_h, slope_fact);
}
if (h_int <= 0)
break; // reached max range
last_clear_x = _x;
last_clear_y = _y;
last_clear_h = h_int;
}
if (total_steps > max_steps)
break;
const int e2 = 2*err;
if (e2 > -dy) {
err -= dy;
_x += sx;
if (step_counter>0)
step_counter--;
total_steps++;
}
if (e2 < dx) {
err += dx;
_y += sy;
if (step_counter>0)
step_counter--;
total_steps++;
}
}
// if we reached invalid terrain, assume we can hit MSL
return RasterLocation(x1, y1);
}
