bool Cloud2CloudDist::Compute( const Cloth& cloth,
const wl::PointCloud& pc,
double class_threshold,
std::vector<int>& groundIndexes,
std::vector<int>& offGroundIndexes,
unsigned N/*=3*/)
{
CCLib::SimpleCloud particlePoints;
if (!particlePoints.reserve(static_cast<unsigned>(cloth.getSize())))
{
//not enough memory
return false;
}
for (int i = 0; i < cloth.getSize(); i++)
{
const Particle& particle = cloth.getParticleByIndex(i);
particlePoints.addPoint(CCVector3(static_cast<PointCoordinateType>(particle.pos.x), 0, static_cast<PointCoordinateType>(particle.pos.z)));
}
CCLib::SimpleCloud pcPoints;
if ( !pcPoints.reserve(static_cast<unsigned>(pc.size()))
|| !pcPoints.enableScalarField())
{
//not enough memory
return false;
}
for (size_t i = 0; i < pc.size(); i++)
{
const wl::Point& P = pc[i];
pcPoints.addPoint(CCVector3(P.x, 0, P.z));
}
try
{
//we spatially 'synchronize' the cloud and particles octrees
CCLib::DgmOctree *cloudOctree = 0, *particleOctree = 0;
CCLib::DistanceComputationTools::SOReturnCode soCode =
CCLib::DistanceComputationTools::synchronizeOctrees
(
&particlePoints,
&pcPoints,
particleOctree,
cloudOctree,
0,
0
);
if (soCode != CCLib::DistanceComputationTools::SYNCHRONIZED && soCode != CCLib::DistanceComputationTools::DISJOINT)
{
//not enough memory (or invalid input)
return false;
}
//additional parameters
void* additionalParameters[] = {(void*)(&cloth),
(void*)(particleOctree),
(void*)(&N)
};
int octreeLevel = particleOctree->findBestLevelForAGivenPopulationPerCell(std::max<unsigned>(2, N));
int result = cloudOctree->executeFunctionForAllCellsAtLevel(
octreeLevel,
ComputeMeanNeighborAltitude,
additionalParameters,
true,
0,
"Rasterization",
QThread::idealThreadCount());
delete cloudOctree;
cloudOctree = 0;
delete particleOctree;
particleOctree = 0;
if (result == 0)
{
//something went wrong
return false;
}
//now classify the points
for (unsigned i = 0; i < pcPoints.size(); ++i)
{
if (std::fabs(pcPoints.getPointScalarValue(i) - pc[i].y) < class_threshold)
{
groundIndexes.push_back(i);
}
else
{
offGroundIndexes.push_back(i);
}
}
}
catch (const std::bad_alloc&)
{
//not enough memory
return false;
}
return true;
}
int ccFastMarchingForNormsDirection::ResolveNormsDirectionByFrontPropagation( ccPointCloud* theCloud,
NormsIndexesTableType* theNorms,
uchar octreeLevel,
CCLib::GenericProgressCallback* progressCb,
CCLib::DgmOctree* inputOctree)
{
assert(theCloud);
unsigned numberOfPoints = theCloud->size();
if (numberOfPoints == 0)
return -1;
//we compute the octree if none is provided
CCLib::DgmOctree* theOctree = inputOctree;
if (!theOctree)
{
theOctree = new CCLib::DgmOctree(theCloud);
if (theOctree->build(progressCb)<1)
{
delete theOctree;
return -2;
}
}
//temporary SF
int oldSfIdx = theCloud->getCurrentDisplayedScalarFieldIndex();
int sfIdx = theCloud->getScalarFieldIndexByName("FM_Propagation");
if (sfIdx < 0)
sfIdx = theCloud->addScalarField("FM_Propagation");
if (sfIdx >= 0)
{
theCloud->setCurrentScalarField(sfIdx);
}
else
{
ccLog::Warning("[ccFastMarchingForNormsDirection] Couldn't create temporary scalar field! Not enough memory?");
if (!inputOctree)
delete theOctree;
return -3;
}
if (!theCloud->enableScalarField())
{
ccLog::Warning("[ccFastMarchingForNormsDirection] Couldn't enable temporary scalar field! Not enough memory?");
theCloud->deleteScalarField(sfIdx);
theCloud->setCurrentScalarField(oldSfIdx);
if (!inputOctree)
delete theOctree;
return -4;
}
//flags indicating if each point has been processed or not
GenericChunkedArray<1,uchar>* resolved = new GenericChunkedArray<1,uchar>();
if (!resolved->resize(numberOfPoints,true,0)) //defaultResolvedValue = 0
{
ccLog::Warning("[ccFastMarchingForNormsDirection] Not enough memory!");
theCloud->deleteScalarField(sfIdx);
theCloud->setCurrentScalarField(oldSfIdx);
if (!inputOctree)
delete theOctree;
resolved->release();
return -5;
}
//Fast Marching propagation
ccFastMarchingForNormsDirection fm;
int result = fm.init(theCloud,theNorms,theOctree,octreeLevel);
if (result < 0)
{
ccLog::Error("[ccFastMarchingForNormsDirection] Something went wrong during initialization...");
theCloud->deleteScalarField(sfIdx);
theCloud->setCurrentScalarField(oldSfIdx);
resolved->release();
if (!inputOctree)
delete theOctree;
return -6;
}
//progress notification
if (progressCb)
{
progressCb->reset();
progressCb->setMethodTitle("Norms direction");
progressCb->setInfo(qPrintable(QString("Octree level: %1\nPoints: %2").arg(octreeLevel).arg(numberOfPoints)));
progressCb->start();
}
const int octreeWidth = (1<<octreeLevel)-1;
//enable 26-connectivity
//fm.setExtendedConnectivity(true);
//while non-processed points remain...
unsigned resolvedPoints = 0;
int lastProcessedPoint = -1;
while (true)
{
//find the next non-processed point
do
{
++lastProcessedPoint;
}
while (lastProcessedPoint < static_cast<int>(numberOfPoints) && resolved->getValue(lastProcessedPoint) != 0);
//all points have been processed? Then we can stop.
if (lastProcessedPoint == static_cast<int>(numberOfPoints))
break;
//we start the propagation from this point
//its corresponding cell in fact ;)
const CCVector3 *thePoint = theCloud->getPoint(lastProcessedPoint);
int pos[3];
theOctree->getTheCellPosWhichIncludesThePoint(thePoint,pos,octreeLevel);
//clipping (in case the octree is not 'complete')
pos[0] = std::min(octreeWidth,pos[0]);
pos[1] = std::min(octreeWidth,pos[1]);
pos[2] = std::min(octreeWidth,pos[2]);
//set corresponding FM cell as 'seed'
fm.setSeedCell(pos);
//launch propagation
int propagationResult = fm.propagate();
//if it's a success
if (propagationResult >= 0)
{
//compute the number of points processed during this pass
unsigned count = fm.updateResolvedTable(theCloud,*resolved,theNorms);
if (count != 0)
{
resolvedPoints += count;
if (progressCb)
progressCb->update(static_cast<float>(resolvedPoints)/static_cast<float>(numberOfPoints)*100.0f);
}
fm.cleanLastPropagation();
}
else
{
ccLog::Error("An error occurred during front propagation! Process cancelled...");
break;
}
}
if (progressCb)
progressCb->stop();
resolved->release();
resolved = 0;
if (!inputOctree)
delete theOctree;
theCloud->showNormals(true);
#ifdef _DEBUG
theCloud->setCurrentDisplayedScalarField(sfIdx);
theCloud->getCurrentDisplayedScalarField()->computeMinAndMax();
theCloud->showSF(true);
#else
theCloud->deleteScalarField(sfIdx);
theCloud->setCurrentScalarField(oldSfIdx);
#endif
return 0;
}
bool ccMinimumSpanningTreeForNormsDirection::Process( ccPointCloud* cloud,
CCLib::GenericProgressCallback* progressCb/*=0*/,
CCLib::DgmOctree* _octree/*=0*/)
{
assert(cloud);
if (!cloud->hasNormals())
{
ccLog::Warning(QString("Cloud '%1' has no normals!").arg(cloud->getName()));
return false;
}
//ask for parameter
bool ok;
#ifdef MST_USE_KNN
unsigned kNN = static_cast<unsigned>(QInputDialog::getInt(0,"Neighborhood size", "Neighbors", 6, 1, 1000, 1, &ok));
if (!ok)
return false;
#else
PointCoordinateType radius = static_cast<PointCoordinateType>(QInputDialog::getDouble(0,"Neighborhood radius", "radius", cloud->getOwnBB().getDiagNorm() * 0.01, 0, 1.0e9, 6, &ok));
if (!ok)
return false;
#endif
//build octree if necessary
CCLib::DgmOctree* octree = _octree;
if (!octree)
{
octree = new CCLib::DgmOctree(cloud);
if (octree->build(progressCb) <= 0)
{
ccLog::Warning(QString("Failed to compute octree on cloud '%1'").arg(cloud->getName()));
delete octree;
return false;
}
}
#ifdef MST_USE_KNN
uchar level = octree->findBestLevelForAGivenPopulationPerCell(kNN*2);
#else
uchar level = octree->findBestLevelForAGivenNeighbourhoodSizeExtraction(radius);
#endif
bool result = true;
try
{
Graph graph;
if (!graph.reserve(cloud->size()))
{
//not enough memory!
result = false;
}
//parameters
void* additionalParameters[3] = { reinterpret_cast<void*>(&graph),
reinterpret_cast<void*>(cloud),
#ifdef MST_USE_KNN
reinterpret_cast<void*>(&kNN)
#else
reinterpret_cast<void*>(&radius)
#endif
};
if (octree->executeFunctionForAllCellsAtLevel( level,
&ComputeMSTGraphAtLevel,
additionalParameters,
false, //not compatible with parallel strategies!
progressCb,
"Build Spanning Tree") == 0)
{
//something went wrong
ccLog::Warning(QString("Failed to compute Spanning Tree on cloud '%1'").arg(cloud->getName()));
result = false;
}
else
{
if (!ResolveNormalsWithMST(cloud,graph,progressCb))
{
//something went wrong
ccLog::Warning(QString("Failed to compute Minimum Spanning Tree on cloud '%1'").arg(cloud->getName()));
result = false;
}
}
}
catch(...)
{
ccLog::Error(QString("Process failed on cloud '%1'").arg(cloud->getName()));
result = false;
}
if (octree && !_octree)
{
delete octree;
octree = 0;
}
return result;
}
int ccFastMarchingForNormsDirection::ResolveNormsDirectionByFrontPropagation(ccPointCloud* theCloud,
NormsIndexesTableType* theNorms,
uchar octreeLevel,
CCLib::GenericProgressCallback* progressCb,
CCLib::DgmOctree* _theOctree)
{
assert(theCloud);
int i,numberOfPoints = theCloud->size();
if (numberOfPoints<1)
return -2;
//on calcule l'octree si besoin
CCLib::DgmOctree* theOctree = _theOctree;
if (!theOctree)
{
theOctree = new CCLib::DgmOctree(theCloud);
if (theOctree->build(progressCb)<1)
{
delete theOctree;
return -3;
}
}
//temporaire
int oldSfIdx = theCloud->getCurrentInScalarFieldIndex();
int sfIdx = theCloud->getScalarFieldIndexByName("FM_Propagation");
if (sfIdx<0)
sfIdx=theCloud->addScalarField("FM_Propagation",true);
if (sfIdx>=0)
theCloud->setCurrentScalarField(sfIdx);
else
{
ccConsole::Warning("[ccFastMarchingForNormsDirection] Couldn't create temporary scalar field! Not enough memory?");
if (!_theOctree)
delete theOctree;
return -5;
}
theCloud->enableScalarField();
//vecteur indiquant si le point a été traité
GenericChunkedArray<1,uchar>* resolved = new GenericChunkedArray<1,uchar>();
resolved->resize(numberOfPoints,true,0); //defaultResolvedValue=0
//on va faire la propagation avec l'algorithme de Fast Marching
ccFastMarchingForNormsDirection* fm = new ccFastMarchingForNormsDirection();
int result = fm->init(theCloud,theNorms,theOctree,octreeLevel);
if (result<0)
{
ccConsole::Error("[ccFastMarchingForNormsDirection] Something went wrong during initialization ...\n");
theCloud->deleteScalarField(sfIdx);
theCloud->setCurrentScalarField(oldSfIdx);
if (!_theOctree)
delete theOctree;
delete fm;
return -4;
}
int resolvedPoints=0;
if (progressCb)
{
progressCb->reset();
progressCb->setMethodTitle("Norms direction");
char buffer[256];
sprintf(buffer,"Octree level: %i\nNumber of points: %i",octreeLevel,numberOfPoints);
progressCb->setInfo(buffer);
progressCb->start();
}
int octreeLength = (1<<octreeLevel)-1;
while (true)
{
//on cherche un point non encore traité
resolved->placeIteratorAtBegining();
for (i=0;i<numberOfPoints;++i)
{
if (resolved->getCurrentValue()==0)
break;
resolved->forwardIterator();
}
//si tous les points ont été traités, on peut arréter !
if (i==numberOfPoints)
break;
//on lance la propagation à partir du point trouvé
const CCVector3 *thePoint = theCloud->getPoint(i);
int pos[3];
theOctree->getTheCellPosWhichIncludesThePoint(thePoint,pos,octreeLevel);
//clipping (important !)
pos[0] = std::min(octreeLength,pos[0]);
pos[1] = std::min(octreeLength,pos[1]);
pos[2] = std::min(octreeLength,pos[2]);
fm->setSeedCell(pos);
int result = fm->propagate();
//si la propagation s'est bien passée
if (result>=0)
{
int count = fm->updateResolvedTable(theCloud,*resolved,theNorms);
if (progressCb)
{
if (count>=0)
{
resolvedPoints += count;
progressCb->update(float(resolvedPoints)/float(numberOfPoints)*100.0);
}
}
fm->endPropagation();
}
}
if (progressCb)
progressCb->stop();
delete fm;
resolved->release();
resolved=0;
if (!_theOctree)
delete theOctree;
theCloud->deleteScalarField(sfIdx);
theCloud->setCurrentScalarField(oldSfIdx);
return 0;
}
int FastMarchingForFacetExtraction::ExtractPlanarFacets( ccPointCloud* theCloud,
unsigned char octreeLevel,
ScalarType maxError,
CCLib::DistanceComputationTools::ERROR_MEASURES errorMeasure,
bool useRetroProjectionError/*=true*/,
CCLib::GenericProgressCallback* progressCb/*=0*/,
CCLib::DgmOctree* _theOctree/*=0*/)
{
assert(theCloud);
unsigned numberOfPoints = theCloud->size();
if (numberOfPoints == 0)
return -1;
if (!theCloud->getCurrentOutScalarField())
return -2;
if (progressCb)
{
//just spawn the dialog so that we can see the
//octree computation (in case the CPU charge prevents
//the dialog from being shown)
progressCb->start();
QApplication::processEvents();
}
//we compute the octree if none is provided
CCLib::DgmOctree* theOctree = _theOctree;
if (!theOctree)
{
theOctree = new CCLib::DgmOctree(theCloud);
if (theOctree->build(progressCb)<1)
{
delete theOctree;
return -3;
}
}
if (progressCb)
{
if (progressCb->textCanBeEdited())
{
progressCb->setMethodTitle("Fast Marching for facets extraction");
progressCb->setInfo("Initializing...");
}
progressCb->start();
QApplication::processEvents();
}
if (!theCloud->enableScalarField())
{
ccLog::Warning("[FastMarchingForFacetExtraction] Couldn't enable scalar field! Not enough memory?");
if (!_theOctree)
delete theOctree;
return -4;
}
//raz SF values
{
for (unsigned i=0; i!=theCloud->size(); ++i)
theCloud->setPointScalarValue(i,0);
}
//flags indicating if each point has been processed or not
GenericChunkedArray<1,unsigned char>* flags = new GenericChunkedArray<1,unsigned char>();
if (!flags->resize(numberOfPoints,true,0)) //defaultFlagValue = 0
{
ccLog::Warning("[FastMarchingForFacetExtraction] Not enough memory!");
if (!_theOctree)
delete theOctree;
flags->release();
return -5;
}
//Fast Marching propagation
FastMarchingForFacetExtraction fm;
int result = fm.init( theCloud,
theOctree,
octreeLevel,
maxError,
errorMeasure,
useRetroProjectionError,
progressCb);
if (result < 0)
{
ccLog::Error("[FastMarchingForFacetExtraction] Something went wrong during initialization...");
flags->release();
if (!_theOctree)
delete theOctree;
return -6;
}
//progress notification
if (progressCb)
{
progressCb->update(0);
if (progressCb->textCanBeEdited())
{
progressCb->setMethodTitle("Facets extraction");
progressCb->setInfo(qPrintable(QString("Octree level: %1\nPoints: %2").arg(octreeLevel).arg(numberOfPoints)));
}
progressCb->start();
QApplication::processEvents();
}
const int octreeWidth = (1<<octreeLevel)-1;
//enable 26-connectivity mode
//fm.setExtendedConnectivity(true);
//while non-processed points remain...
unsigned resolvedPoints = 0;
int lastProcessedPoint = -1;
unsigned facetIndex = 0;
while (true)
{
//find the next non-processed point
do
{
++lastProcessedPoint;
}
while (lastProcessedPoint < static_cast<int>(numberOfPoints) && flags->getValue(lastProcessedPoint) != 0);
//all points have been processed? Then we can stop.
if (lastProcessedPoint == static_cast<int>(numberOfPoints))
break;
//we start the propagation from this point
//its corresponding cell in fact ;)
const CCVector3 *thePoint = theCloud->getPoint(lastProcessedPoint);
Tuple3i pos;
theOctree->getTheCellPosWhichIncludesThePoint(thePoint, pos, octreeLevel);
//clipping (in case the octree is not 'complete')
pos.x = std::min(octreeWidth, pos.x);
pos.y = std::min(octreeWidth, pos.y);
pos.z = std::min(octreeWidth, pos.z);
//set corresponding FM cell as 'seed'
if (!fm.setSeedCell(pos))
{
//an error occurred?!
//result = -7;
//break;
continue;
}
//launch propagation
int propagationResult = fm.propagate();
//compute the number of points processed during this pass
unsigned count = fm.updateFlagsTable(theCloud,*flags,propagationResult >= 0 ? ++facetIndex : 0); //0 = invalid facet index
if (count != 0)
{
resolvedPoints += count;
if (progressCb)
{
if (progressCb->isCancelRequested())
{
result = -7;
break;
}
progressCb->update(static_cast<float>(resolvedPoints)/static_cast<float>(numberOfPoints)*100.0f);
}
}
fm.cleanLastPropagation();
}
if (progressCb)
progressCb->stop();
flags->release();
flags = 0;
if (!_theOctree)
delete theOctree;
return result;
}