Array2D CombinationWithDuplicatedElementsByNonRecursion(Array1D array, int K)
{
Array2D result;
if (array.empty() || K <= 0) { return result; }
if (array.size() < K) {
result.push_back(array); /** @warning maybe exclude it */
return result;
}
std::sort(array.begin(), array.end());
result.push_back(Array1D());
int last = array[0], opt_result_num = 1;
for (int i = 0; i < array.size(); i++) {
if (array[i] != last) {
last = array[i];
opt_result_num = result.size();
}
Array2D::size_type result_size = result.size();
for (int j = result_size - 1; j >= result_size - opt_result_num; j--) {
result.push_back(result[j]);
result.back().push_back(array[i]);
}
}
return result;
}
static inline void TerrainProcessor(F func, const Array2D<T> &elevations, const float zscale, Array2D<float> &output){
if(elevations.getCellLengthX()!=elevations.getCellLengthY())
RDLOG_WARN<<"Cell X and Y dimensions are not equal!";
output.resize(elevations);
ProgressBar progress;
progress.start(elevations.size());
#pragma omp parallel for
for(int y=0;y<elevations.height();y++){
progress.update(y*elevations.width());
for(int x=0;x<elevations.width();x++)
if(elevations.isNoData(x,y))
output(x,y) = output.noData();
else
output(x,y) = func(elevations,x,y,zscale);
}
RDLOG_TIME_USE<<"Wall-time = "<<progress.stop();
}
void FindFlats(
const Array2D<T> &elevations,
Array2D<int8_t> &flats
){
flats.resize(elevations);
flats.setNoData(FLAT_NO_DATA);
ProgressBar progress;
progress.start( elevations.size() );
#pragma omp parallel for
for(int y=0;y<elevations.height();y++)
for(int x=0;x<elevations.width();x++){
if(elevations.isNoData(x,y)){
flats(x,y) = FLAT_NO_DATA;
continue;
}
if(elevations.isEdgeCell(x,y)){
flats(x,y) = NOT_A_FLAT;
continue;
}
//We'll now assume that the cell is a flat unless proven otherwise
flats(x,y) = IS_A_FLAT;
for(int n=1;n<=8;n++){
const int nx = x+dx[n];
const int ny = y+dy[n];
if(elevations(nx,ny)<elevations(x,y) || elevations.isNoData(nx,ny)){
flats(x,y) = NOT_A_FLAT;
break;
}
}
//We handled the base case just above the for loop
}
RDLOG_TIME_USE<<"Succeeded in = "<<progress.stop()<<" s";
}
void dinf_flow_directions(const Array2D<T> &elevations, Array2D<float> &flowdirs){
ProgressBar progress;
std::cerr<<"\nA Dinf Flow Directions"<<std::endl;
std::cerr<<"C Tarboton, D.G. 1997. A new method for the determination of flow directions and upslope areas in grid digital elevation models. Water Resources Research. Vol. 33. pp 309-319."<<std::endl;
std::cerr<<"p Setting up the Dinf flow directions matrix..."<<std::endl;
flowdirs.resize(elevations);
flowdirs.setNoData(dinf_NO_DATA);
flowdirs.setAll(NO_FLOW);
std::cerr<<"p Calculating Dinf flow directions..."<<std::endl;
progress.start( elevations.size() );
#pragma omp parallel for
for(int y=0;y<elevations.height();y++){
progress.update( y*elevations.width() );
for(int x=0;x<elevations.width();x++)
if(elevations(x,y)==elevations.noData())
flowdirs(x,y) = flowdirs.noData();
else
flowdirs(x,y) = dinf_FlowDir(elevations,x,y);
}
std::cerr<<"t Succeeded in = "<<progress.stop()<<" s"<<std::endl;
}
void dinf_upslope_area(
const Array2D<T> &flowdirs,
Array2D<U> &area
){
Array2D<int8_t> dependency;
std::queue<GridCell> sources;
ProgressBar progress;
std::cerr<<"\nA D-infinity Upslope Area"<<std::endl;
std::cerr<<"C Tarboton, D.G. 1997. A new method for the determination of flow directions and upslope areas in grid digital elevation models. Water Resources Research. Vol. 33. pp 309-319."<<std::endl;
std::cerr<<"p Setting up the dependency matrix..."<<std::endl;
dependency.resize(flowdirs);
dependency.setAll(0);
std::cerr<<"p Setting up the area matrix..."<<std::endl;
area.resize(flowdirs);
area.setAll(0);
area.setNoData(dinf_NO_DATA);
bool has_cells_without_flow_directions=false;
std::cerr<<"p Calculating dependency matrix & setting noData() cells..."<<std::endl;
progress.start( flowdirs.size() );
///////////////////////
//Calculate the number of "dependencies" each cell has. That is, count the
//number of cells which flow into each cell.
#pragma omp parallel for reduction(|:has_cells_without_flow_directions)
for(int y=0;y<flowdirs.height();y++){
progress.update( y*flowdirs.width() );
for(int x=0;x<flowdirs.width();x++){
//If the flow direction of the cell is NoData, mark its area as NoData
if(flowdirs.isNoData(x,y)){
area(x,y) = area.noData();
dependency(x,y) = 9; //TODO: This is an unnecessary safety precaution. This prevents the cell from ever being enqueued (an unnecessary safe guard? TODO)
continue; //Only necessary if there are bugs below (TODO)
}
//If the cell has no flow direction, note that so we can warn the user
if(flowdirs(x,y)==NO_FLOW){
has_cells_without_flow_directions=true;
continue;
}
//TODO: More explanation of what's going on here
int n_high, n_low;
int nhx,nhy,nlx,nly;
where_do_i_flow(flowdirs(x,y),n_high,n_low);
nhx=x+dinf_dx[n_high];
nhy=y+dinf_dy[n_high];
if(n_low!=-1){
nlx = x+dinf_dx[n_low];
nly = y+dinf_dy[n_low];
}
if( n_low!=-1 && flowdirs.inGrid(nlx,nly) && flowdirs(nlx,nly)!=flowdirs.noData() )
dependency(nlx,nly)++;
if( flowdirs.inGrid(nhx,nhy) && flowdirs(nhx,nhy)!=flowdirs.noData() )
dependency(nhx,nhy)++;
}
}
std::cerr<<"t Succeeded in = "<<progress.stop()<<" s"<<std::endl;
if(has_cells_without_flow_directions)
std::cerr<<"W \033[91mNot all cells had defined flow directions! This implies that there will be digital dams!\033[39m"<<std::endl;
///////////////////////
//Find those cells which have no dependencies. These are the places to start
//the flow accumulation calculation.
std::cerr<<"p Locating source cells..."<<std::endl;
progress.start( flowdirs.size() );
for(int y=0;y<flowdirs.height();y++){
progress.update( y*flowdirs.width() );
for(int x=0;x<flowdirs.width();x++)
if(flowdirs(x,y)==flowdirs.noData())
continue;
else if(flowdirs(x,y)==NO_FLOW)
continue;
else if(dependency(x,y)==0)
sources.emplace(x,y);
}
std::cerr<<"t Source cells located in = "<<progress.stop()<<" s"<<std::endl;
///////////////////////
//Calculate the flow accumulation by "pouring" a cell's flow accumulation
//value into the cells below it, as indicated by the D-infinite flow routing
//method.
std::cerr<<"p Calculating up-slope areas..."<<std::endl;
progress.start( flowdirs.numDataCells() );
long int ccount=0;
while(sources.size()>0){
auto c = sources.front();
sources.pop();
progress.update(ccount++);
if(flowdirs.isNoData(c.x,c.y)) //TODO: This line shouldn't be necessary since NoData's do not get added below
continue;
area(c.x,c.y)+=1;
if(flowdirs(c.x,c.y)==NO_FLOW)
continue;
int n_high,n_low,nhx,nhy,nlx,nly;
where_do_i_flow(flowdirs(c.x,c.y),n_high,n_low);
nhx = c.x+dinf_dx[n_high];
nhy = c.y+dinf_dy[n_high];
float phigh,plow;
area_proportion(flowdirs(c.x,c.y), n_high, n_low, phigh, plow);
if(flowdirs.inGrid(nhx,nhy) && flowdirs(nhx,nhy)!=flowdirs.noData())
area(nhx,nhy)+=area(c.x,c.y)*phigh;
if(n_low!=-1){
nlx = c.x+dinf_dx[n_low];
nly = c.y+dinf_dy[n_low];
if(flowdirs.inGrid(nlx,nly) && flowdirs(nlx,nly)!=flowdirs.noData()){
area(nlx,nly)+=area(c.x,c.y)*plow;
if((--dependency(nlx,nly))==0)
sources.emplace(nlx,nly);
}
}
if( flowdirs.inGrid(nhx,nhy) && flowdirs(nhx,nhy)!=flowdirs.noData() && (--dependency(nhx,nhy))==0)
sources.emplace(nhx,nhy);
}
std::cerr<<"p Succeeded in = "<<progress.stop()<<" s"<<std::endl;
}
bool NavMeshGenerator::handleBuild(const Array2D<int>& tab)
{
cleanup();
//
// Step 1. Initialize build config.
//
// Init build configuration from GUI
memset(&m_cfg, 0, sizeof(m_cfg));
const int voxels_per_tile = 3; //3
m_cfg.width = tab.size().x*3*voxels_per_tile;
m_cfg.height = tab.size().x*3*voxels_per_tile;
m_cfg.cs = pixels_per_tile/float(voxels_per_tile);
m_cfg.walkableRadius = voxels_per_tile == 1 ? 0 : 1;
m_cfg.maxEdgeLen = 0;//20; //
m_cfg.maxSimplificationError = 0.f; // 0 or 1, no need because we are working on tiles
m_cfg.minRegionArea = 64;//(int)rcSqr(m_regionMinSize); // Note: area = size*size
m_cfg.mergeRegionArea = 10000;//(int)rcSqr(m_regionMergeSize); // Note: area = size*size
m_cfg.maxVertsPerPoly = 4;//(int)m_vertsPerPoly;
m_cfg.ch = 0.2f; // < height info, not used
m_cfg.detailSampleDist = 20.f; // < height info, not used
m_cfg.detailSampleMaxError = 0.2f; // < height info, not used
m_cfg.walkableSlopeAngle = 0; // < height info, not used
m_cfg.walkableHeight = 0; // < height info, not used
m_cfg.walkableClimb = 0; // < height info, not used
// Set the area where the navigation will be build.
m_cfg.bmin[0] = 0;
m_cfg.bmin[1] = 0;
m_cfg.bmin[2] = 0;
m_cfg.bmax[0] = tab.size().x*3*pixels_per_tile;
m_cfg.bmax[1] = 3;
m_cfg.bmax[2] = tab.size().y*3*pixels_per_tile;
// Reset build times gathering.
m_ctx->resetTimers();
// Start the build process.
m_ctx->startTimer(RC_TIMER_TOTAL);
m_ctx->log(RC_LOG_PROGRESS, "Building navigation:");
m_ctx->log(RC_LOG_PROGRESS, " - %d x %d cells", m_cfg.width, m_cfg.height);
//
// Step 2. Create Heightfield
//
// Allocate voxel heightfield
m_solid = rcAllocHeightfield();
if (!m_solid)
{
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'solid'.");
return false;
}
if (!rcCreateHeightfield(m_ctx, *m_solid, m_cfg.width, m_cfg.height, m_cfg.bmin, m_cfg.bmax, m_cfg.cs, m_cfg.ch))
{
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not create solid heightfield.");
return false;
}
for(int i = 0; i < m_solid->width; ++i) {
for(int j = 0; j < m_solid->height; ++j) {
bool colide = tab(int(i*tab.size().x/m_solid->width) , int(j*tab.size().y/m_solid->height)) != 0 ;
rcAddSpan(NULL,
*m_solid, i, j, 0,
colide ? 10: 0, colide ? RC_NULL_AREA : RC_WALKABLE_AREA, 1);
}
}
//
// Step 3. Filter walkables surfaces.
//----> Not done for 2D
//
// Step 4. Partition walkable surface to simple regions.
//
// Compact the heightfield so that it is faster to handle from now on.
// This will result more cache coherent data as well as the neighbours
// between walkable cells will be calculated.
m_chf = rcAllocCompactHeightfield();
if (!m_chf)
{
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'chf'.");
return false;
}
if (!rcBuildCompactHeightfield(m_ctx, m_cfg.walkableHeight, m_cfg.walkableClimb, *m_solid, *m_chf))
{
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build compact data.");
return false;
}
if (!m_keepInterResults)
{
rcFreeHeightField(m_solid);
m_solid = 0;
}
// Erode the walkable area by agent radius.
if (!rcErodeWalkableArea(m_ctx, m_cfg.walkableRadius, *m_chf))
{
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not erode.");
return false;
}
// (Optional) Mark areas.
/*const ConvexVolume* vols = m_geom->getConvexVolumes();
for (int i = 0; i < m_geom->getConvexVolumeCount(); ++i)
rcMarkConvexPolyArea(m_ctx, vols[i].verts, vols[i].nverts, vols[i].hmin, vols[i].hmax, (unsigned char)vols[i].area, *m_chf);
*/
// Partition the heightfield so that we can use simple algorithm later to triangulate the walkable areas.
// There are 3 martitioning methods, each with some pros and cons:
// 1) Watershed partitioning
// - the classic Recast partitioning
// - creates the nicest tessellation
// - usually slowest
// - partitions the heightfield into nice regions without holes or overlaps
// - the are some corner cases where this method creates produces holes and overlaps
// - holes may appear when a small obstacles is close to large open area (triangulation can handle this)
// - overlaps may occur if you have narrow spiral corridors (i.e stairs), this make triangulation to fail
// * generally the best choice if you precompute the nacmesh, use this if you have large open areas
// 2) Monotone partioning
// - fastest
// - partitions the heightfield into regions without holes and overlaps (guaranteed)
// - creates long thin polygons, which sometimes causes paths with detours
// * use this if you want fast navmesh generation
// 3) Layer partitoining
// - quite fast
// - partitions the heighfield into non-overlapping regions
// - relies on the triangulation code to cope with holes (thus slower than monotone partitioning)
// - produces better triangles than monotone partitioning
// - does not have the corner cases of watershed partitioning
// - can be slow and create a bit ugly tessellation (still better than monotone)
// if you have large open areas with small obstacles (not a problem if you use tiles)
// * good choice to use for tiled navmesh with medium and small sized tiles
if (true) //m_partitionType == SAMPLE_PARTITION_WATERSHED)
{
// Prepare for region partitioning, by calculating distance field along the walkable surface.
if (!rcBuildDistanceField(m_ctx, *m_chf))
{
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build distance field.");
return false;
}
// Partition the walkable surface into simple regions without holes.
if (!rcBuildRegions(m_ctx, *m_chf, 0, m_cfg.minRegionArea, m_cfg.mergeRegionArea))
{
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build watershed regions.");
return false;
}
}
else if (false)//m_partitionType == SAMPLE_PARTITION_MONOTONE)
{
// Partition the walkable surface into simple regions without holes.
// Monotone partitioning does not need distancefield.
if (!rcBuildRegionsMonotone(m_ctx, *m_chf, 0, m_cfg.minRegionArea, m_cfg.mergeRegionArea))
{
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build monotone regions.");
return false;
}
}
else // SAMPLE_PARTITION_LAYERS
{
// Partition the walkable surface into simple regions without holes.
if (!rcBuildLayerRegions(m_ctx, *m_chf, 0, m_cfg.minRegionArea))
{
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build layer regions.");
return false;
}
}
//
// Step 5. Trace and simplify region contours.
//
// Create contours.
m_cset = rcAllocContourSet();
if (!m_cset)
{
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'cset'.");
return false;
}
if (!rcBuildContours(m_ctx, *m_chf, m_cfg.maxSimplificationError, m_cfg.maxEdgeLen, *m_cset))
{
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not create contours.");
return false;
}
//
// Step 6. Build polygons mesh from contours.
//
// Build polygon navmesh from the contours.
m_pmesh = rcAllocPolyMesh();
if (!m_pmesh)
{
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'pmesh'.");
return false;
}
if (!rcBuildPolyMesh(m_ctx, *m_cset, m_cfg.maxVertsPerPoly, *m_pmesh))
{
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not triangulate contours.");
return false;
}
//
// Step 7. Create detail mesh which allows to access approximate height on each polygon.
//
m_dmesh = rcAllocPolyMeshDetail();
if (!m_dmesh)
{
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Out of memory 'pmdtl'.");
return false;
}
if (!rcBuildPolyMeshDetail(m_ctx, *m_pmesh, *m_chf, m_cfg.detailSampleDist, m_cfg.detailSampleMaxError, *m_dmesh))
{
m_ctx->log(RC_LOG_ERROR, "buildNavigation: Could not build detail mesh.");
return false;
}
if (!m_keepInterResults)
{
rcFreeCompactHeightfield(m_chf);
m_chf = 0;
rcFreeContourSet(m_cset);
m_cset = 0;
}
// At this point the navigation mesh data is ready, you can access it from m_pmesh.
// See duDebugDrawPolyMesh or dtCreateNavMeshData as examples how to access the data.
//
// (Optional) Step 8. Create Detour data from Recast poly mesh.
//
// The GUI may allow more max points per polygon than Detour can handle.
// Only build the detour navmesh if we do not exceed the limit.
if (m_cfg.maxVertsPerPoly <= DT_VERTS_PER_POLYGON)
{
unsigned char* navData = 0;
int navDataSize = 0;
// Update poly flags from areas.
for (int i = 0; i < m_pmesh->npolys; ++i)
{
//if (m_pmesh->areas[i] == RC_WALKABLE_AREA)
// m_pmesh->areas[i] = SAMPLE_POLYAREA_GROUND;
if (m_pmesh->areas[i] == RC_WALKABLE_AREA)
{
m_pmesh->flags[i] = 1;
}
else {
m_pmesh->flags[i] = 0;
}
}
dtNavMeshCreateParams params;
memset(¶ms, 0, sizeof(params));
params.verts = m_pmesh->verts;
params.vertCount = m_pmesh->nverts;
params.polys = m_pmesh->polys;
params.polyAreas = m_pmesh->areas;
params.polyFlags = m_pmesh->flags;
params.polyCount = m_pmesh->npolys;
params.nvp = m_pmesh->nvp;
params.detailMeshes = m_dmesh->meshes;
params.detailVerts = m_dmesh->verts;
params.detailVertsCount = m_dmesh->nverts;
params.detailTris = m_dmesh->tris;
params.detailTriCount = m_dmesh->ntris;
params.offMeshConCount = 0;
/*
unused since offMeshConCount is null
params.offMeshConVerts = m_geom->getOffMeshConnectionVerts();
params.offMeshConRad = m_geom->getOffMeshConnectionRads();
params.offMeshConDir = m_geom->getOffMeshConnectionDirs();
params.offMeshConAreas = m_geom->getOffMeshConnectionAreas();
params.offMeshConFlags = m_geom->getOffMeshConnectionFlags();
params.offMeshConUserID = m_geom->getOffMeshConnectionId();
*/
params.walkableHeight = m_agentHeight;
params.walkableRadius = m_agentRadius;
params.walkableClimb = m_agentMaxClimb;
rcVcopy(params.bmin, m_pmesh->bmin);
rcVcopy(params.bmax, m_pmesh->bmax);
params.cs = m_cfg.cs;
params.ch = m_cfg.ch;
params.buildBvTree = true;
if (!dtCreateNavMeshData(¶ms, &navData, &navDataSize))
{
m_ctx->log(RC_LOG_ERROR, "Could not build Detour navmesh.");
return false;
}
m_navMesh = dtAllocNavMesh();
if (!m_navMesh)
{
dtFree(navData);
m_ctx->log(RC_LOG_ERROR, "Could not create Detour navmesh");
return false;
}
dtStatus status;
status = m_navMesh->init(navData, navDataSize, DT_TILE_FREE_DATA);
if (dtStatusFailed(status))
{
dtFree(navData);
m_ctx->log(RC_LOG_ERROR, "Could not init Detour navmesh");
return false;
}
status = m_navQuery->init(m_navMesh, 2048);
if (dtStatusFailed(status))
{
m_ctx->log(RC_LOG_ERROR, "Could not init Detour navmesh query");
return false;
}
}
m_ctx->stopTimer(RC_TIMER_TOTAL);
// Show performance stats.
m_ctx->log(RC_LOG_PROGRESS, ">> Polymesh: %d vertices %d polygons", m_pmesh->nverts, m_pmesh->npolys);
return true;
}
void FM_Freeman(
const Array2D<E> &elevations,
Array3D<float> &props,
const double xparam
){
RDLOG_ALG_NAME<<"Freeman (1991) Flow Accumulation (aka MFD, MD8)";
RDLOG_CITATION<<"Freeman, T.G., 1991. Calculating catchment area with divergent flow based on a regular grid. Computers & Geosciences 17, 413–422.";
RDLOG_CONFIG<<"p = "<<xparam;
props.setAll(NO_FLOW_GEN);
props.setNoData(NO_DATA_GEN);
ProgressBar progress;
progress.start(elevations.size());
#pragma omp parallel for collapse(2)
for(int y=0;y<elevations.height();y++)
for(int x=0;x<elevations.width();x++){
++progress;
if(elevations.isNoData(x,y)){
props(x,y,0) = NO_DATA_GEN;
continue;
}
if(elevations.isEdgeCell(x,y))
continue;
const E e = elevations(x,y);
double C = 0;
for(int n=1;n<=8;n++){
const int nx = x+dx[n];
const int ny = y+dy[n];
if(!elevations.inGrid(nx,ny))
continue;
if(elevations.isNoData(nx,ny)) //TODO: Don't I want water to drain this way?
continue;
const E ne = elevations(nx,ny);
if(ne<e){
const double rise = e-ne;
const double run = dr[n];
const double grad = rise/run;
const auto cval = std::pow(grad,xparam);
props(x,y,n) = cval;
C += cval;
}
}
if(C>0){
props(x,y,0) = HAS_FLOW_GEN;
C = 1/C; //TODO
for(int n=1;n<=8;n++){
auto &this_por = props(x,y,n);
if(this_por>0)
this_por *= C;
else
this_por = 0;
}
}
}
progress.stop();
}
void Garbrecht_CombineGradients(Array2D<T> &elevations, const Array2D<int32_t> &inc1, const Array2D<int32_t> &inc2, float epsilon){
std::cerr<<"Combining the gradients..."<<std::endl;
for(unsigned int i=0;i<elevations.size();i++)
elevations(i) += (inc1(i)+inc2(i))*epsilon;
}
