void Obstacles::add(RectFloat rect) {
int polygonIndex = findEmptyPolygon();
if (polygonIndex < 0) {
return;
}
rect.expand(12.0f);
rect.trunc_2_decimals();
Polygon &poly = _polygons[polygonIndex];
poly.rect = rect;
poly.vertices[0] = Vector2(rect.x0, rect.y0);
poly.vertexType[0] = TOP_LEFT;
poly.vertices[1] = Vector2(rect.x1, rect.y0);
poly.vertexType[1] = TOP_RIGHT;
poly.vertices[2] = Vector2(rect.x1, rect.y1);
poly.vertexType[2] = BOTTOM_RIGHT;
poly.vertices[3] = Vector2(rect.x0, rect.y1);
poly.vertexType[3] = BOTTOM_LEFT;
poly.isPresent = true;
poly.verticeCount = 4;
restart:
for (int i = 0; i < kPolygonCount; ++i) {
Polygon &polyA = _polygons[i];
if (!polyA.isPresent) {
continue;
}
for (int j = i+1; j < kPolygonCount; ++j) {
Polygon &polyB = _polygons[j];
if (!polyB.isPresent) {
continue;
}
if (!overlaps(polyA.rect, polyB.rect)) {
continue;
}
if (mergePolygons(polyA, polyB)) {
goto restart;
}
}
}
}
void PCPolyhedron::addToModel(const PCPtr& nuCloud)
{
PCModelObject::addToModel(nuCloud);
/// 1 - Detectar caras de la nueva nube
/// 2 - Descartar caras que no pertenecen a la caja (caras de la mano)
/// 3 - Matchear caras de la nube anterior con las nuevas caras
/// 4 - Estimar caras ocultas (?)
PCPtr trimmedCloud = getHalo(this->getvMin(),this->getvMax(),0.05,nuCloud);
saveCloud("nucloud.pcd",*nuCloud);
saveCloud("trimmed.pcd",*trimmedCloud);
pcPolygonsMutex.lock();
vector<PCPolygonPtr> prevPcPolygons = pcpolygons;
//Detecto nuevas caras
vector<PCPolygonPtr> nuevos = detectPolygons(trimmedCloud,Constants::OBJECT_PLANE_TOLERANCE(),Constants::CLOUD_VOXEL_SIZE * 2,false);
//Matching de las caras detectadas con las anteriores
nuevos = mergePolygons(nuevos);
//Estimación de caras no visibles
bool estimationOK;
vector<PCPolygonPtr> estimated = estimateHiddenPolygons(nuevos,estimationOK);
bool valid = true;
if(estimationOK)
{
for(int i = 0; i < nuevos.size(); i++)
nuevos.at(i)->setEstimated(false);
//Actualizo los nuevos polygons si no hubo errores
nuevos.insert(nuevos.end(),estimated.begin(),estimated.end());
pcpolygons = nuevos;
//Unifico vertices
unifyVertexs();
isvalid = validate();
}
//Chequeo volumenes
if(IsFeatureActive(FEATURE_INVALIDATE_OBJECT_BY_VOLUME))
{
float inVol = computeVolume(trimmedCloud);
if(getVolume() < inVol * Constants::OBJECT_VOLUME_TOLERANCE)
{
cout << "[ INVALID VOLUME ]" << endl;
cout << "need: " << inVol * 0.6 << " --- has: " << getVolume() << endl;
isvalid = false;
}
}
if(estimationOK && isvalid)
{
polygonsCache.clear();
for (vector<PCPolygonPtr>::iterator p = pcpolygons.begin(); p != pcpolygons.end(); ++p)
{
polygonsCache.push_back((*p)->getPolygonModelObject());
}
}
else
{
pcpolygons = prevPcPolygons;
//Si hay errores en la estimación, rollback de los pcpolygons mergeados
for(int i = 0; i < pcpolygons.size(); i++)
pcpolygons.at(i)->rollBackMatching();
if(!isvalid)
{
//Necesito unificar los vertices despues del rollback
unifyVertexs();
}
isvalid = false;
}
//Seteo nueva nube
PCPtr finalCloud (new PC());
for(int i = 0; i < pcpolygons.size(); i ++)
{
*finalCloud += *pcpolygons.at(i)->getCloud();
saveCloud("pol" + ofToString(pcpolygons.at(i)->getPolygonModelObject()->getName()) + ".pcd", *pcpolygons.at(i)->getCloud());
}
saveCloud("finalCloud" + ofToString(getId()) + ".pcd", *finalCloud);
setCloud(finalCloud);
pcPolygonsMutex.unlock();
}
//-----------------------------------------------------------------------
void ConvexBody::mergePolygons( void )
{
// Merge all polygons that lay in the same plane as one big polygon.
// A convex body does not have two separate regions (separated by polygons
// with different normals) where the same normal occurs, so we can simply
// search all similar normals of a polygon. Two different options are
// possible when the normals fit:
// - the two polygons are neighbors
// - the two polygons aren't neighbors (but a third, fourth,.. polygon lays
// in between)
// Signals if the body holds polygons which aren't neighbors but have the same
// normal. That means another step has to be processed.
bool bDirty = false;
for ( size_t iPolyA = 0; iPolyA < getPolygonCount(); ++iPolyA )
{
// ??
OgreAssert( iPolyA >= 0, "strange..." );
for ( size_t iPolyB = iPolyA+1; iPolyB < getPolygonCount(); ++iPolyB )
{
const Vector3& n1 = getNormal( iPolyA );
const Vector3& n2 = getNormal( iPolyB );
// if the normals point into the same direction
if ( n1.directionEquals( n2, Radian( Degree( 0.00001 ) ) ) )
{
// indicates if a neighbor has been found and joined
bool bFound = false;
// search the two fitting vertices (if there are any) for the common edge
const size_t numVerticesA = getVertexCount( iPolyA );
for ( size_t iVertexA = 0; iVertexA < numVerticesA; ++iVertexA )
{
const size_t numVerticesB = getVertexCount( iPolyB );
for ( size_t iVertexB = 0; iVertexB < numVerticesB; ++iVertexB )
{
const Vector3& aCurrent = getVertex( iPolyA, iVertexA );
const Vector3& aNext = getVertex( iPolyA, (iVertexA + 1) % getVertexCount( iPolyA ) );
const Vector3& bCurrent = getVertex( iPolyB, iVertexB );
const Vector3& bNext = getVertex( iPolyB, (iVertexB + 1) % getVertexCount( iPolyB ) );
// if the edge is the same the current vertex of A has to be equal to the next of B and the other
// way round
if ( aCurrent.positionEquals(bNext) &&
bCurrent.positionEquals(aNext))
{
// polygons are neighbors, assemble new one
Polygon *pNew = allocatePolygon();
// insert all vertices of A up to the join (including the common vertex, ignoring
// whether the first vertex of A may be a shared vertex)
for ( size_t i = 0; i <= iVertexA; ++i )
{
pNew->insertVertex( getVertex( iPolyA, i%numVerticesA ) );
}
// insert all vertices of B _after_ the join to the end
for ( size_t i = iVertexB + 2; i < numVerticesB; ++i )
{
pNew->insertVertex( getVertex( iPolyB, i ) );
}
// insert all vertices of B from the beginning up to the join (including the common vertex
// and excluding the first vertex if the first is part of the shared edge)
for ( size_t i = 0; i <= iVertexB; ++i )
{
pNew->insertVertex( getVertex( iPolyB, i%numVerticesB ) );
}
// insert all vertices of A _after_ the join to the end
for ( size_t i = iVertexA + 2; i < numVerticesA; ++i )
{
pNew->insertVertex( getVertex( iPolyA, i ) );
}
// in case there are double vertices (in special cases), remove them
for ( size_t i = 0; i < pNew->getVertexCount(); ++i )
{
const Vector3& a = pNew->getVertex( i );
const Vector3& b = pNew->getVertex( (i + 1) % pNew->getVertexCount() );
// if the two vertices are the same...
if (a.positionEquals(b))
{
// remove a
pNew->deleteVertex( i );
// decrement counter
--i;
}
}
// delete the two old ones
OgreAssert( iPolyA != iPolyB, "PolyA and polyB are the same!" );
// polyB is always higher than polyA, so delete polyB first
deletePolygon( iPolyB );
deletePolygon( iPolyA );
// continue with next (current is deleted, so don't jump to the next after the next)
--iPolyA;
--iPolyB;
// insert new polygon
insertPolygon( pNew );
bFound = true;
break;
}
}
if ( bFound )
{
break;
}
}
if ( bFound == false )
{
// there are two polygons available with the same normal direction, but they
// could not be merged into one single because of no shared edge
bDirty = true;
break;
}
}
}
}
// recursion to merge the previous non-neighbors
if ( bDirty )
{
mergePolygons();
}
}
