bool cc2Point5DimEditor::RasterGrid::fillWith( ccGenericPointCloud* cloud,
unsigned char projectionDimension,
cc2Point5DimEditor::ProjectionType projectionType,
bool interpolateEmptyCells,
cc2Point5DimEditor::ProjectionType sfInterpolation/*=INVALID_PROJECTION_TYPE*/,
ccProgressDialog* progressDialog/*=0*/)
{
if (!cloud)
{
assert(false);
return false;
}
//current parameters
unsigned gridTotalSize = width * height;
//vertical dimension
const unsigned char Z = projectionDimension;
assert(Z >= 0 && Z <= 2);
const unsigned char X = Z == 2 ? 0 : Z +1;
const unsigned char Y = X == 2 ? 0 : X +1;
//do we need to interpolate scalar fields?
ccPointCloud* pc = cloud->isA(CC_TYPES::POINT_CLOUD) ? static_cast<ccPointCloud*>(cloud) : 0;
bool interpolateSF = (sfInterpolation != INVALID_PROJECTION_TYPE);
interpolateSF &= (pc && pc->hasScalarFields());
if (interpolateSF)
{
unsigned sfCount = pc->getNumberOfScalarFields();
bool memoryError = false;
size_t previousCount = scalarFields.size();
if (sfCount > previousCount)
{
try
{
scalarFields.resize(sfCount,0);
}
catch (const std::bad_alloc&)
{
//not enough memory
memoryError = true;
}
}
for (size_t i=previousCount; i<sfCount; ++i)
{
assert(scalarFields[i] == 0);
scalarFields[i] = new double[gridTotalSize];
if (!scalarFields[i])
{
//not enough memory
memoryError = true;
break;
}
}
if (memoryError)
{
ccLog::Warning(QString("[Rasterize] Failed to allocate memory for scalar fields!"));
}
}
//filling the grid
unsigned pointCount = cloud->size();
double gridMaxX = gridStep * width;
double gridMaxY = gridStep * height;
if (progressDialog)
{
progressDialog->setMethodTitle("Grid generation");
progressDialog->setInfo(qPrintable(QString("Points: %1\nCells: %2 x %3").arg(pointCount).arg(width).arg(height)));
progressDialog->start();
progressDialog->show();
QCoreApplication::processEvents();
}
CCLib::NormalizedProgress nProgress(progressDialog,pointCount);
for (unsigned n=0; n<pointCount; ++n)
{
const CCVector3* P = cloud->getPoint(n);
CCVector3d relativePos = CCVector3d::fromArray(P->u) - minCorner;
int i = static_cast<int>(relativePos.u[X]/gridStep);
int j = static_cast<int>(relativePos.u[Y]/gridStep);
//specific case: if we fall exactly on the max corner of the grid box
if (i == static_cast<int>(width) && relativePos.u[X] == gridMaxX)
--i;
if (j == static_cast<int>(height) && relativePos.u[Y] == gridMaxY)
--j;
//we skip points outside the box!
if ( i < 0 || i >= static_cast<int>(width)
|| j < 0 || j >= static_cast<int>(height) )
continue;
assert(i >= 0 && j >= 0);
RasterCell* aCell = data[j]+i;
unsigned& pointsInCell = aCell->nbPoints;
if (pointsInCell)
{
if (P->u[Z] < aCell->minHeight)
{
aCell->minHeight = P->u[Z];
if (projectionType == PROJ_MINIMUM_VALUE)
aCell->pointIndex = n;
}
else if (P->u[Z] > aCell->maxHeight)
{
aCell->maxHeight = P->u[Z];
if (projectionType == PROJ_MAXIMUM_VALUE)
aCell->pointIndex = n;
}
}
else
{
aCell->minHeight = aCell->maxHeight = P->u[Z];
aCell->pointIndex = n;
}
// Sum the points heights
aCell->avgHeight += P->u[Z];
aCell->stdDevHeight += static_cast<double>(P->u[Z])*P->u[Z];
//scalar fields
if (interpolateSF)
{
int pos = j*static_cast<int>(width)+i; //pos in 2D SF grid(s)
assert(pos < static_cast<int>(gridTotalSize));
for (size_t k=0; k<scalarFields.size(); ++k)
{
if (scalarFields[k])
{
CCLib::ScalarField* sf = pc->getScalarField(static_cast<unsigned>(k));
assert(sf);
ScalarType sfValue = sf->getValue(n);
ScalarType formerValue = static_cast<ScalarType>(scalarFields[k][pos]);
if (pointsInCell && ccScalarField::ValidValue(formerValue))
{
if (ccScalarField::ValidValue(sfValue))
{
switch (sfInterpolation)
{
case PROJ_MINIMUM_VALUE:
// keep the minimum value
scalarFields[k][pos] = std::min<double>(formerValue,sfValue);
break;
case PROJ_AVERAGE_VALUE:
//we sum all values (we will divide them later)
scalarFields[k][pos] += sfValue;
break;
case PROJ_MAXIMUM_VALUE:
// keep the maximum value
scalarFields[k][pos] = std::max<double>(formerValue,sfValue);
break;
default:
assert(false);
break;
}
}
}
else
{
//for the first (vaild) point, we simply have to store its SF value (in any case)
scalarFields[k][pos] = sfValue;
}
}
}
}
pointsInCell++;
if (!nProgress.oneStep())
{
//process cancelled by user
return false;
}
}
//update SF grids for 'average' cases
if (sfInterpolation == PROJ_AVERAGE_VALUE)
{
for (size_t k=0; k<scalarFields.size(); ++k)
{
if (scalarFields[k])
{
double* _gridSF = scalarFields[k];
for (unsigned j=0;j<height;++j)
{
RasterCell* cell = data[j];
for (unsigned i=0; i<width; ++i,++cell,++_gridSF)
{
if (cell->nbPoints > 1)
{
ScalarType s = static_cast<ScalarType>(*_gridSF);
if (ccScalarField::ValidValue(s)) //valid SF value
{
*_gridSF /= cell->nbPoints;
}
}
}
}
}
}
}
//update the main grid (average height and std.dev. computation + current 'height' value)
{
for (unsigned j=0; j<height; ++j)
{
RasterCell* cell = data[j];
for (unsigned i=0; i<width; ++i,++cell)
{
if (cell->nbPoints > 1)
{
cell->avgHeight /= cell->nbPoints;
cell->stdDevHeight = sqrt(fabs(cell->stdDevHeight/cell->nbPoints - cell->avgHeight*cell->avgHeight));
}
else
{
cell->stdDevHeight = 0;
}
if (cell->nbPoints != 0)
{
//set the right 'height' value
switch (projectionType)
{
case PROJ_MINIMUM_VALUE:
cell->h = cell->minHeight;
break;
case PROJ_AVERAGE_VALUE:
cell->h = cell->avgHeight;
break;
case PROJ_MAXIMUM_VALUE:
cell->h = cell->maxHeight;
break;
default:
assert(false);
break;
}
}
}
}
}
//compute the number of non empty cells
nonEmptyCellCount = 0;
{
for (unsigned i=0; i<height; ++i)
for (unsigned j=0; j<width; ++j)
if (data[i][j].nbPoints)
++nonEmptyCellCount;
}
//specific case: interpolate the empty cells
if (interpolateEmptyCells)
{
std::vector<CCVector2> the2DPoints;
if (nonEmptyCellCount < 3)
{
ccLog::Warning("[Rasterize] Not enough non-empty cells to interpolate!");
}
else if (nonEmptyCellCount < width * height) //otherwise it's useless!
{
try
{
the2DPoints.resize(nonEmptyCellCount);
}
catch (const std::bad_alloc&)
{
//out of memory
ccLog::Warning("[Rasterize] Not enough memory to interpolate empty cells!");
}
}
//fill 2D vector with non-empty cell indexes
if (!the2DPoints.empty())
{
unsigned index = 0;
for (unsigned j=0; j<height; ++j)
{
const RasterCell* cell = data[j];
for (unsigned i=0; i<width; ++i, ++cell)
{
if (cell->nbPoints)
{
//we only use the non-empty cells to interpolate
the2DPoints[index++] = CCVector2(static_cast<PointCoordinateType>(i),static_cast<PointCoordinateType>(j));
}
}
}
assert(index == nonEmptyCellCount);
//mesh the '2D' points
CCLib::Delaunay2dMesh delaunayMesh;
char errorStr[1024];
if (delaunayMesh.buildMesh(the2DPoints,0,errorStr))
{
unsigned triNum = delaunayMesh.size();
//now we are going to 'project' all triangles on the grid
delaunayMesh.placeIteratorAtBegining();
for (unsigned k=0; k<triNum; ++k)
{
const CCLib::VerticesIndexes* tsi = delaunayMesh.getNextTriangleVertIndexes();
//get the triangle bounding box (in grid coordinates)
int P[3][2];
int xMin = 0, yMin = 0, xMax = 0, yMax = 0;
{
for (unsigned j=0; j<3; ++j)
{
const CCVector2& P2D = the2DPoints[tsi->i[j]];
P[j][0] = static_cast<int>(P2D.x);
P[j][1] = static_cast<int>(P2D.y);
}
xMin = std::min(std::min(P[0][0],P[1][0]),P[2][0]);
yMin = std::min(std::min(P[0][1],P[1][1]),P[2][1]);
xMax = std::max(std::max(P[0][0],P[1][0]),P[2][0]);
yMax = std::max(std::max(P[0][1],P[1][1]),P[2][1]);
}
//now scan the cells
{
//pre-computation for barycentric coordinates
const double& valA = data[ P[0][1] ][ P[0][0] ].h;
const double& valB = data[ P[1][1] ][ P[1][0] ].h;
const double& valC = data[ P[2][1] ][ P[2][0] ].h;
int det = (P[1][1]-P[2][1])*(P[0][0]-P[2][0]) + (P[2][0]-P[1][0])*(P[0][1]-P[2][1]);
for (int j=yMin; j<=yMax; ++j)
{
RasterCell* cell = data[static_cast<unsigned>(j)];
for (int i=xMin; i<=xMax; ++i)
{
//if the cell is empty
if (!cell[i].nbPoints)
{
//we test if it's included or not in the current triangle
//Point Inclusion in Polygon Test (inspired from W. Randolph Franklin - WRF)
bool inside = false;
for (int ti=0; ti<3; ++ti)
{
const int* P1 = P[ti];
const int* P2 = P[(ti+1)%3];
if ((P2[1] <= j &&j < P1[1]) || (P1[1] <= j && j < P2[1]))
{
int t = (i-P2[0])*(P1[1]-P2[1])-(P1[0]-P2[0])*(j-P2[1]);
if (P1[1] < P2[1])
t = -t;
if (t < 0)
inside = !inside;
}
}
//can we interpolate?
if (inside)
{
double l1 = static_cast<double>((P[1][1]-P[2][1])*(i-P[2][0])+(P[2][0]-P[1][0])*(j-P[2][1]))/det;
double l2 = static_cast<double>((P[2][1]-P[0][1])*(i-P[2][0])+(P[0][0]-P[2][0])*(j-P[2][1]))/det;
double l3 = 1.0-l1-l2;
cell[i].h = l1 * valA + l2 * valB + l3 * valC;
assert(cell[i].h == cell[i].h);
//interpolate SFs as well!
for (size_t sfIndex=0; sfIndex<scalarFields.size(); ++sfIndex)
{
if (scalarFields[sfIndex])
{
double* gridSF = scalarFields[sfIndex];
const double& sfValA = gridSF[ P[0][0] + P[0][1]*width ];
const double& sfValB = gridSF[ P[1][0] + P[1][1]*width ];
const double& sfValC = gridSF[ P[2][0] + P[2][1]*width ];
gridSF[i + j*width] = l1 * sfValA + l2 * sfValB + l3 * sfValC;
}
}
}
}
}
}
}
}
}
else
{
ccLog::Warning(QString("[Rasterize] Empty cells interpolation failed: Triangle lib. said '%1'").arg(errorStr));
}
}
}
//computation of the average and extreme height values in the grid
{
minHeight = 0;
maxHeight = 0;
meanHeight = 0;
validCellCount = 0;
for (unsigned i=0; i<height; ++i)
{
for (unsigned j=0; j<width; ++j)
{
if (data[i][j].h == data[i][j].h) //valid height
{
double h = data[i][j].h;
if (validCellCount)
{
if (h < minHeight)
minHeight = h;
else if (h > maxHeight)
maxHeight = h;
meanHeight += h;
}
else
{
//first valid cell
meanHeight = minHeight = maxHeight = h;
}
++validCellCount;
}
}
}
meanHeight /= validCellCount;
}
setValid(true);
return true;
}
QSharedPointer<DistanceMapGenerationTool::Map> DistanceMapGenerationTool::CreateMap(ccPointCloud* cloud,
ccScalarField* sf,
const ccGLMatrix& cloudToSurface,
unsigned char revolutionAxisDim,
double xStep_rad,
double yStep,
double yMin,
double yMax,
bool spherical,
bool counterclockwise,
FillStrategyType fillStrategy,
EmptyCellFillOption emptyCellfillOption,
ccMainAppInterface* app/*=0*/)
{
assert(cloud && sf);
if (!cloud || !sf)
{
if (app)
app->dispToConsole(QString("[DistanceMapGenerationTool] Internal error: invalid input structures!"),ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return QSharedPointer<Map>(0);
}
//invalid parameters?
if (xStep_rad <= 0.0 || yStep <= 0.0 || yMax <= yMin || revolutionAxisDim > 2)
{
if (app)
app->dispToConsole(QString("[DistanceMapGenerationTool] Internal error: invalid grid parameters!"),ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return QSharedPointer<Map>(0);
}
unsigned count = cloud->size();
if (count == 0)
{
if (app)
app->dispToConsole(QString("[DistanceMapGenerationTool] Cloud is empty! Nothing to do!"),ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return QSharedPointer<Map>(0);
}
//revolution axis
const unsigned char Z = revolutionAxisDim;
//we deduce the 2 other ('horizontal') dimensions from the revolution axis
const unsigned char X = (Z < 2 ? Z+1 : 0);
const unsigned char Y = (X < 2 ? X+1 : 0);
//grid dimensions
unsigned xSteps = 0;
{
if (xStep_rad > 0)
xSteps = static_cast<unsigned>(ceil((2 * M_PI) / xStep_rad));
if (xSteps == 0)
{
if (app)
app->dispToConsole(QString("[DistanceMapGenerationTool] Invalid longitude step/boundaries! Can't generate a proper map!"),ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return QSharedPointer<Map>(0);
}
}
unsigned ySteps = 0;
{
if (yStep > 0)
ySteps = static_cast<unsigned>(ceil((yMax - yMin) / yStep));
if (ySteps == 0)
{
if (app)
app->dispToConsole(QString("[DistanceMapGenerationTool] Invalid latitude step/boundaries! Can't generate a proper map!"),ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return QSharedPointer<Map>(0);
}
}
unsigned cellCount = xSteps * ySteps;
if (app)
app->dispToConsole(QString("[DistanceMapGenerationTool] Projected map size: %1 x %2 (%3 cells)").arg(xSteps).arg(ySteps).arg(cellCount),ccMainAppInterface::STD_CONSOLE_MESSAGE);
//reserve memory for the output matrix
QSharedPointer<Map> grid(new Map);
try
{
grid->resize(cellCount);
}
catch (const std::bad_alloc&)
{
if (app)
app->dispToConsole(QString("[DistanceMapGenerationTool] Not enough memory!"),ccMainAppInterface::ERR_CONSOLE_MESSAGE);
return QSharedPointer<Map>(0);
}
//update grid info ("for the records")
grid->xSteps = xSteps;
grid->xMin = 0.0;
grid->xMax = 2.0 * M_PI;
grid->xStep = xStep_rad;
grid->ySteps = ySteps;
grid->yMin = yMin;
grid->yMax = yMax;
grid->yStep = yStep;
//motion direction
grid->counterclockwise = counterclockwise;
double ccw = (counterclockwise ? -1.0 : 1.0);
for (unsigned n=0; n<count; ++n)
{
//we skip invalid values
const ScalarType& val = sf->getValue(n);
if (!CCLib::ScalarField::ValidValue(val))
continue;
const CCVector3* P = cloud->getPoint(n);
CCVector3 relativePos = cloudToSurface * (*P);
//convert to cylindrical or spherical coordinates
double x = ccw * atan2(relativePos.u[X],relativePos.u[Y]); //longitude
if (x < 0.0)
x += 2.0 * M_PI;
double y = 0.0;
if (spherical)
{
y = ComputeLatitude_rad(relativePos.u[X],relativePos.u[Y],relativePos.u[Z]); //latitude between 0 and pi/2
}
else
{
y = relativePos.u[Z]; //height
}
int i = static_cast<int>((x-grid->xMin)/grid->xStep);
int j = static_cast<int>((y-grid->yMin)/grid->yStep);
//if we fall exactly on the max corner of the grid box
if (i == static_cast<int>(grid->xSteps))
--i;
if (j == static_cast<int>(grid->ySteps))
--j;
//we skip points outside the box!
if ( i < 0 || i >= static_cast<int>(grid->xSteps)
|| j < 0 || j >= static_cast<int>(grid->ySteps) )
{
continue;
}
assert(i >= 0 && j >= 0);
MapCell& cell = (*grid)[j*static_cast<int>(grid->xSteps)+i];
if (cell.count) //if there's already values projected in this cell
{
switch (fillStrategy)
{
case FILL_STRAT_MIN_DIST:
// Set the minimum SF value
if (val < cell.value)
cell.value = val;
break;
case FILL_STRAT_AVG_DIST:
// Sum the values
cell.value += static_cast<double>(val);
break;
case FILL_STRAT_MAX_DIST:
// Set the maximum SF value
if (val > cell.value)
cell.value = val;
break;
default:
assert(false);
break;
}
}
else
{
//for the first point, we simply have to store its associated value (whatever the case)
cell.value = val;
}
++cell.count;
//if (progressCb)
// progressCb->update(30.0 * (float)n / (float)cloud->size());
}
//we need to finish the average values computation
if (fillStrategy == FILL_STRAT_AVG_DIST)
{
MapCell* cell = &grid->at(0);
for (unsigned i=0; i<cellCount; ++i, ++cell)
if (cell->count > 1)
cell->value /= (double)cell->count;
}
//fill empty cells with zero?
if (emptyCellfillOption == FILL_WITH_ZERO)
{
MapCell* cell = &grid->at(0);
for (unsigned i=0; i<cellCount; ++i, ++cell)
{
if (cell->count == 0)
{
cell->value = 0.0;
cell->count = 1;
}
}
}
else if (emptyCellfillOption == FILL_INTERPOLATE)
{
//convert the non-empty cells to a 2D point cloud
unsigned fillCount = 0;
{
MapCell* cell = &grid->at(0);
for (unsigned i=0; i<cellCount; ++i, ++cell)
if (cell->count != 0)
++fillCount;
}
//do we really need to interpolate empty grid cells?
if (fillCount)
{
std::vector<CCVector2> the2DPoints;
try
{
the2DPoints.reserve(fillCount);
}
catch (...)
{
//out of memory
if (app)
app->dispToConsole(QString("[DistanceMapGenerationTool] Not enough memory to interpolate!"),ccMainAppInterface::ERR_CONSOLE_MESSAGE);
}
if (the2DPoints.capacity() == fillCount)
{
//fill 2D vector with non-empty cell indexes
{
const MapCell* cell = &grid->at(0);
for (unsigned j=0; j<grid->ySteps; ++j)
for (unsigned i=0; i<grid->xSteps; ++i, ++cell)
if (cell->count)
the2DPoints.push_back(CCVector2(static_cast<PointCoordinateType>(i),static_cast<PointCoordinateType>(j)));
}
//mesh the '2D' points
CCLib::Delaunay2dMesh* dm = new CCLib::Delaunay2dMesh();
char errorStr[1024];
if (!dm->buildMesh(the2DPoints,0,errorStr))
{
if (app)
app->dispToConsole(QString("[DistanceMapGenerationTool] Interpolation failed: Triangle lib. said '%1'").arg(errorStr),ccMainAppInterface::ERR_CONSOLE_MESSAGE);
}
else
{
unsigned triNum = dm->size();
MapCell* cells = &grid->at(0);
//now we are going to 'project' all triangles on the grid
dm->placeIteratorAtBegining();
for (unsigned k=0; k<triNum; ++k)
{
const CCLib::VerticesIndexes* tsi = dm->getNextTriangleVertIndexes();
//get the triangle bounding box (in grid coordinates)
int P[3][2];
int xMin=0,yMin=0,xMax=0,yMax=0;
{
for (unsigned j=0; j<3; ++j)
{
const CCVector2& P2D = the2DPoints[tsi->i[j]];
P[j][0] = static_cast<int>(P2D.x);
P[j][1] = static_cast<int>(P2D.y);
}
xMin = std::min(std::min(P[0][0],P[1][0]),P[2][0]);
yMin = std::min(std::min(P[0][1],P[1][1]),P[2][1]);
xMax = std::max(std::max(P[0][0],P[1][0]),P[2][0]);
yMax = std::max(std::max(P[0][1],P[1][1]),P[2][1]);
}
//now scan the cells
{
//pre-computation for barycentric coordinates
const double& valA = cells[P[0][0] + P[0][1] * grid->xSteps].value;
const double& valB = cells[P[1][0] + P[1][1] * grid->xSteps].value;
const double& valC = cells[P[2][0] + P[2][1] * grid->xSteps].value;
int det = (P[1][1]-P[2][1])*(P[0][0]-P[2][0]) + (P[2][0]-P[1][0])*(P[0][1]-P[2][1]);
for (int j=yMin; j<=yMax; ++j)
{
MapCell* cell = cells + static_cast<unsigned>(j)*grid->xSteps;
for (int i=xMin; i<=xMax; ++i)
{
//if the cell is empty
if (!cell[i].count)
{
//we test if it's included or not in the current triangle
//Point Inclusion in Polygon Test (inspired from W. Randolph Franklin - WRF)
bool inside = false;
for (int ti=0; ti<3; ++ti)
{
const int* P1 = P[ti];
const int* P2 = P[(ti+1)%3];
if ((P2[1] <= j &&j < P1[1]) || (P1[1] <= j && j < P2[1]))
{
int t = (i-P2[0])*(P1[1]-P2[1])-(P1[0]-P2[0])*(j-P2[1]);
if (P1[1] < P2[1])
t = -t;
if (t < 0)
inside = !inside;
}
}
//can we interpolate?
if (inside)
{
double l1 = static_cast<double>((P[1][1]-P[2][1])*(i-P[2][0])+(P[2][0]-P[1][0])*(j-P[2][1]))/det;
double l2 = static_cast<double>((P[2][1]-P[0][1])*(i-P[2][0])+(P[0][0]-P[2][0])*(j-P[2][1]))/det;
double l3 = 1.0-l1-l2;
cell[i].count = 1;
cell[i].value = l1 * valA + l2 * valB + l3 * valC;
}
}
}
}
}
}
}
delete dm;
dm = 0;
}
}
}
//update min and max values
{
const MapCell* cell = &grid->at(0);
grid->minVal = grid->maxVal = cell->value;
++cell;
for (unsigned i=1; i<cellCount; ++i, ++cell)
{
if (cell->value < grid->minVal)
grid->minVal = cell->value;
else if (cell->value > grid->maxVal)
grid->maxVal = cell->value;
}
}
//end of process
return grid;
}