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;
}
CCLib::SimpleCloud* ccGBLSensor::project(CCLib::GenericCloud* theCloud, int& errorCode, bool autoParameters/*false*/)
{
assert(theCloud);
CCLib::SimpleCloud* newCloud = new CCLib::SimpleCloud();
unsigned pointCount = theCloud->size();
if (!newCloud->reserve(pointCount) || !newCloud->enableScalarField()) //not enough memory
{
errorCode = -4;
delete newCloud;
return 0;
}
ScalarType maxDepth = 0;
theCloud->placeIteratorAtBegining();
{
for (unsigned i=0; i<pointCount; ++i)
{
const CCVector3 *P = theCloud->getNextPoint();
CCVector2 Q;
ScalarType depth;
projectPoint(*P,Q,depth);
newCloud->addPoint(CCVector3(Q.x,Q.y,0));
newCloud->setPointScalarValue(i,depth);
if (i != 0)
maxDepth = std::max(maxDepth,depth);
else
maxDepth = depth;
}
}
if (autoParameters)
{
//dimensions = theta, phi, 0
PointCoordinateType bbMin[3],bbMax[3];
newCloud->getBoundingBox(bbMin,bbMax);
setTheta(bbMin[0],bbMax[0]);
setPhi(bbMin[1],bbMax[1]);
setSensorRange(maxDepth);
}
//clear previous Z-buffer
{
if (m_depthBuffer.zBuff)
delete[] m_depthBuffer.zBuff;
m_depthBuffer.zBuff=0;
m_depthBuffer.width=0;
m_depthBuffer.height=0;
}
//init new Z-buffer
{
int width = static_cast<int>(ceil((thetaMax-thetaMin)/deltaTheta));
int height = static_cast<int>(ceil((phiMax-phiMin)/deltaPhi));
if (width*height == 0 || std::max(width,height) > 2048) //too small or... too big!
{
errorCode = -2;
delete newCloud;
return 0;
}
unsigned zBuffSize = width*height;
m_depthBuffer.zBuff = new ScalarType[zBuffSize];
if (!m_depthBuffer.zBuff) //not enough memory
{
errorCode = -4;
delete newCloud;
return 0;
}
m_depthBuffer.width = width;
m_depthBuffer.height = height;
memset(m_depthBuffer.zBuff,0,zBuffSize*sizeof(ScalarType));
}
//project points and accumulate them in Z-buffer
newCloud->placeIteratorAtBegining();
for (unsigned i=0; i<newCloud->size(); ++i)
{
const CCVector3 *P = newCloud->getNextPoint();
ScalarType depth = newCloud->getPointScalarValue(i);
int x = static_cast<int>(floor((P->x-thetaMin)/deltaTheta));
int y = static_cast<int>(floor((P->y-phiMin)/deltaPhi));
ScalarType& zBuf = m_depthBuffer.zBuff[y*m_depthBuffer.width+x];
zBuf = std::max(zBuf,depth);
}
errorCode = 0;
return newCloud;
}