src/Terrain/Intersection.cpp

   1 /*
   2 Copyright_License {
   3
   4   XCSoar Glide Computer - http://www.xcsoar.org/
   5   Copyright (C) 2000-2013 The XCSoar Project
   6   A detailed list of copyright holders can be found in the file "AUTHORS".
   7
   8   This program is free software; you can redistribute it and/or
   9   modify it under the terms of the GNU General Public License
  10   as published by the Free Software Foundation; either version 2
  11   of the License, or (at your option) any later version.
  12
  13   This program is distributed in the hope that it will be useful,
  14   but WITHOUT ANY WARRANTY; without even the implied warranty of
  15   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  16   GNU General Public License for more details.
  17
  18   You should have received a copy of the GNU General Public License
  19   along with this program; if not, write to the Free Software
  20   Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
  21 }
  22 */
  23
  24 #include "RasterTileCache.hpp"
  25 #include "Terrain/RasterLocation.hpp"
  26
  27 #include <string.h>
  28 #include <stdlib.h>
  29 #include <algorithm>
  30
  31 //#define DEBUG_TILE
  32 #ifdef DEBUG_TILE
  33 #include <stdio.h>
  34 #endif
  35
  36 /**
  37  * Replaces "water" samples with 0.
  38  */
  39 constexpr
  40 static int
  41 ReplaceWater0(short h)
  42 {
  43   return RasterBuffer::IsWater(h) ? 0 : h;
  44 }
  45
  46 bool
  47 RasterTileCache::FirstIntersection(const int x0, const int y0,
  48                                    const int x1, const int y1,
  49                                    int h_origin,
  50                                    int h_dest,
  51                                    const int slope_fact, const int h_ceiling,
  52                                    const int h_safety,
  53                                    RasterLocation &_location, int &_h,
  54                                    const bool can_climb) const
  55 {
  56   RasterLocation location(x0, y0);
  57   if (location.x >= width || location.y >= height)
  58     // origin is outside overall bounds
  59     return false;
  60
  61   const short h_origin2 = GetFieldDirect(x0, y0).first;
  62   if (RasterBuffer::IsInvalid(h_origin2)) {
  63     _location = location;
  64     _h = h_origin;
  65     return true;
  66   }
  67
  68   if (!RasterBuffer::IsSpecial(h_origin2))
  69     h_origin = std::max(h_origin, (int)h_origin2);
  70
  71   h_dest = std::max(h_dest, h_origin);
  72
  73   // line algorithm parameters
  74   const int dx = abs(x1-x0);
  75   const int dy = abs(y1-y0);
  76   int err = dx-dy;
  77   const int sx = (x0 < x1)? 1: -1;
  78   const int sy = (y0 < y1)? 1: -1;
  79
  80   // max number of steps to walk
  81   const int max_steps = (dx+dy);
  82   // calculate number of fine steps to produce a step on the overview field
  83   const int step_fine = std::max(1, max_steps >> INTERSECT_BITS);
  84   // number of steps for update to the overview map
  85   const int step_coarse = std::max(1<< OVERVIEW_BITS, step_fine);
  86
  87   // number of steps to be cleared after climbing over obstruction
  88   const int intersect_steps = 32;
  89
  90   // counter for steps to reach next position to be checked on the field.
  91   unsigned step_counter = 0;
  92   // total counter of fine steps
  93   int total_steps = 0;
  94
  95   // number of steps since intersection
  96   int intersect_counter = 0;
  97
  98 #ifdef DEBUG_TILE
  99   printf("# max steps %d\n", max_steps);
 100   printf("# step coarse %d\n", step_coarse);
 101   printf("# step fine %d\n", step_fine);
 102 #endif
 103
 104   // early exit if origin is too high (should not occur)
 105   if (h_origin> h_ceiling) {
 106 #ifdef DEBUG_TILE
 107     printf("# fint start above ceiling %d %d\n", h_origin, h_ceiling);
 108 #endif
 109     _location = location;
 110     _h = h_origin;
 111     return true;
 112   }
 113
 114 #ifdef DEBUG_TILE
 115   printf("# fint width %d height %d\n", width, height);
 116 #endif
 117
 118   // location of last point within ceiling limit that doesnt intersect
 119   RasterLocation last_clear_location = location;
 120   int last_clear_h = h_origin;
 121
 122   while (true) {
 123
 124     if (!step_counter) {
 125
 126       if (location.x >= width || location.y >= height)
 127         break; // outside bounds
 128
 129       const auto field_direct = GetFieldDirect(location.x, location.y);
 130       if (RasterBuffer::IsInvalid(field_direct.first))
 131         break;
 132
 133       const int h_terrain = ReplaceWater0(field_direct.first) + h_safety;
 134       step_counter = field_direct.second ? step_fine : step_coarse;
 135
 136       // calculate height of glide so far
 137       const int dh = (total_steps * slope_fact) >> RASTER_SLOPE_FACT;
 138
 139       // current aircraft height
 140       int h_int = dh + h_origin;
 141       if (can_climb) {
 142         h_int = std::min(h_int, h_dest);
 143       }
 144
 145 #ifdef DEBUG_TILE
 146       printf("%d %d %d %d %d # fint\n", location.x, location.y, h_int, h_terrain, h_ceiling);
 147 #endif
 148
 149       // this point has intersected if aircraft is below terrain height
 150       const bool this_intersecting = (h_int< h_terrain);
 151
 152       if (this_intersecting) {
 153         intersect_counter = 1;
 154
 155         // when intersecting, consider origin to have started higher
 156         const int h_jump = h_terrain - h_int;
 157         h_origin += h_jump;
 158
 159         if (can_climb) {
 160           // if intersecting beyond desired destination height, allow dest height
 161           // to be increased
 162           if (h_terrain> h_dest)
 163             h_dest = h_terrain;
 164         } else {
 165           // if can't climb, must jump so path is pure glide
 166           h_dest += h_jump;
 167         }
 168         h_int = h_terrain;
 169
 170       }
 171
 172       if (h_int > h_ceiling) {
 173         _location = last_clear_location;
 174         _h = last_clear_h;
 175 #ifdef DEBUG_TILE
 176         printf("# fint reach ceiling\n");
 177 #endif
 178         return true; // reached ceiling
 179       }
 180
 181       if (!this_intersecting) {
 182         if (intersect_counter) {
 183           intersect_counter+= step_counter;
 184
 185           // was intersecting, now cleared.
 186           // exit with small height above terrain
 187 #ifdef DEBUG_TILE
 188           printf("# fint int->clear\n");
 189 #endif
 190           if (intersect_counter >= intersect_steps) {
 191             _location = location;
 192             _h = h_int;
 193             return true;
 194           }
 195         } else {
 196           last_clear_location = location;
 197           last_clear_h = h_int;
 198         }
 199       }
 200     }
 201
 202     if (!intersect_counter && (total_steps == max_steps)) {
 203 #ifdef DEBUG_TILE
 204       printf("# fint cleared\n");
 205 #endif
 206       return false;
 207     }
 208
 209     const int e2 = 2*err;
 210     if (e2 > -dy) {
 211       err -= dy;
 212       location.x += sx;
 213       if (step_counter)
 214         step_counter--;
 215       total_steps++;
 216     }
 217     if (e2 < dx) {
 218       err += dx;
 219       location.y += sy;
 220       if (step_counter)
 221         step_counter--;
 222       total_steps++;
 223     }
 224   }
 225
 226   // early exit due to inability to find clearance after intersecting
 227   if (intersect_counter) {
 228     _location = last_clear_location;
 229     _h = last_clear_h;
 230 #ifdef DEBUG_TILE
 231     printf("# fint early exit\n");
 232 #endif
 233     return true;
 234   }
 235   return false;
 236 }
 237
 238 inline std::pair<short, bool>
 239 RasterTileCache::GetFieldDirect(const unsigned px, const unsigned py) const
 240 {
 241   assert(px < width);
 242   assert(py < height);
 243
 244   const RasterTile &tile = tiles.Get(px / tile_width, py / tile_height);
 245   if (tile.IsEnabled())
 246     return std::make_pair(tile.GetHeight(px, py), true);
 247
 248   // still not found, so go to overview
 249
 250   // The overview might not cover the whole tile, if width or height are not
 251   // a multiple of 2^OVERVIEW_BITS.
 252   unsigned x_overview = px >> OVERVIEW_BITS;
 253   unsigned y_overview = py >> OVERVIEW_BITS;
 254   assert(x_overview <= overview.GetWidth());
 255   assert(y_overview <= overview.GetHeight());
 256
 257   if (x_overview == overview.GetWidth())
 258     x_overview--;
 259   if (y_overview == overview.GetHeight())
 260     y_overview--;
 261
 262   return std::make_pair(overview.Get(x_overview, y_overview), false);
 263 }
 264
 265 RasterLocation
 266 RasterTileCache::Intersection(const int x0, const int y0,
 267                               const int x1, const int y1,
 268                               const int h_origin,
 269                               const int slope_fact) const
 270 {
 271   RasterLocation location(x0, y0);
 272
 273   if (location.x >= width || location.y >= height)
 274     // origin is outside overall bounds
 275     return location;
 276
 277   // line algorithm parameters
 278   const int dx = abs(x1-x0);
 279   const int dy = abs(y1-y0);
 280   int err = dx-dy;
 281   const int sx = (x0 < x1)? 1: -1;
 282   const int sy = (y0 < y1)? 1: -1;
 283
 284   // max number of steps to walk
 285   const int max_steps = (dx+dy);
 286   // calculate number of fine steps to produce a step on the overview field
 287
 288   // step size at selected refinement level
 289   const int refine_step = max_steps >> 5;
 290
 291   // number of steps for update to the fine map
 292   const int step_fine = std::max(1, refine_step);
 293   // number of steps for update to the overview map
 294   const int step_coarse = std::max(1<< OVERVIEW_BITS, step_fine);
 295
 296   // counter for steps to reach next position to be checked on the field.
 297   unsigned step_counter = 0;
 298   // total counter of fine steps
 299   int total_steps = 0;
 300
 301 #ifdef DEBUG_TILE
 302   printf("# max steps %d\n", max_steps);
 303   printf("# step coarse %d\n", step_coarse);
 304   printf("# step fine %d\n", step_fine);
 305 #endif
 306
 307   RasterLocation last_clear_location = location;
 308   int last_clear_h = h_origin;
 309
 310   while (true) {
 311
 312     if (!step_counter) {
 313
 314       if (location.x >= width || location.y >= height)
 315         break; // outside bounds
 316
 317       const auto field_direct = GetFieldDirect(location.x, location.y);
 318       if (RasterBuffer::IsInvalid(field_direct.first))
 319         break;
 320
 321       const int h_terrain = ReplaceWater0(field_direct.first);
 322       step_counter = field_direct.second ? step_fine : step_coarse;
 323
 324       // calculate height of glide so far
 325       const int dh = (total_steps * slope_fact) >> RASTER_SLOPE_FACT;
 326
 327       // current aircraft height
 328       const int h_int = h_origin - dh;
 329
 330       if (h_int < h_terrain) {
 331         if (refine_step<3) // can't refine any further
 332           return RasterLocation(last_clear_location.x, last_clear_location.y);
 333
 334         // refine solution
 335         return Intersection(last_clear_location.x, last_clear_location.y,
 336                             location.x, location.y, last_clear_h, slope_fact);
 337       }
 338
 339       if (h_int <= 0)
 340         break; // reached max range
 341
 342       last_clear_location = location;
 343       last_clear_h = h_int;
 344     }
 345
 346     if (total_steps > max_steps)
 347       break;
 348
 349     const int e2 = 2*err;
 350     if (e2 > -dy) {
 351       err -= dy;
 352       location.x += sx;
 353       if (step_counter>0)
 354         step_counter--;
 355       total_steps++;
 356     }
 357     if (e2 < dx) {
 358       err += dx;
 359       location.y += sy;
 360       if (step_counter>0)
 361         step_counter--;
 362       total_steps++;
 363     }
 364   }
 365
 366   // if we reached invalid terrain, assume we can hit MSL
 367   return RasterLocation(x1, y1);
 368 }