bool Cloud2CloudDist::Compute(const Cloth& cloth,
const wl::PointCloud& pc,
double class_threshold,
std::vector<int>& groundIndexes,
std::vector<int>& offGroundIndexes,
unsigned N/*=3*/)
{
try
{
std::list<Point_d> points_2d;
std::map<std::string, double >mapstring;
// maping coordinates xy->z to query the height value of each point
for (int i = 0; i < cloth.getSize(); i++)
{
const Particle& particle = cloth.getParticleByIndex(i);
std::ostringstream ostrx, ostrz;
ostrx << particle.pos.x;
ostrz << particle.pos.z;
mapstring.insert(std::pair<std::string, double>(ostrx.str() + ostrz.str(), particle.pos.y));
points_2d.push_back(Point_d(particle.pos.x, particle.pos.z));
}
Tree tree(points_2d.begin(), points_2d.end());
// step two query the nearest point of cloth for each terrain point
for (int i = 0; i < pc.size(); i++)
{
Point_d query(pc[i].x, pc[i].z);
Neighbor_search search(tree, query, N);
double search_min = 0;
for (Neighbor_search::iterator it = search.begin(); it != search.end(); it++)
{
std::ostringstream ostrx, ostrz;
ostrx << it->first.x();
ostrz << it->first.y();
double y = mapstring[ostrx.str() + ostrz.str()];
search_min = search_min + y / N;
//if (y > search_min)
//{
// search_min = y;
//}
}
if (std::fabs(search_min - 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;
}
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;
}