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(); } }