/*!
\brief Add linestring to a ring (private)
\param[in,out] papoRing list of rings
\param poLine pointer to linestring to be added to a ring
\return TRUE on success
\return FALSE on failure
*/
bool IVFKDataBlock::AppendLineToRing(PointListArray *papoRing, const OGRLineString *poLine, bool bNewRing)
{
OGRPoint *poFirst, *poLast;
OGRPoint *poFirstNew, *poLastNew;
OGRPoint pt;
PointList poList;
/* OGRLineString -> PointList */
for (int i = 0; i < poLine->getNumPoints(); i++) {
poLine->getPoint(i, &pt);
poList.push_back(pt);
}
/* create new ring */
if (bNewRing) {
papoRing->push_back(new PointList(poList));
return TRUE;
}
poFirstNew = &(poList.front());
poLastNew = &(poList.back());
for (PointListArray::const_iterator i = papoRing->begin(), e = papoRing->end();
i != e; ++i) {
PointList *ring = (*i);
poFirst = &(ring->front());
poLast = &(ring->back());
if (!poFirst || !poLast || poLine->getNumPoints() < 2)
return FALSE;
if (poFirstNew->Equals(poLast)) {
/* forward, skip first point */
ring->insert(ring->end(), poList.begin()+1, poList.end());
return TRUE;
}
if (poFirstNew->Equals(poFirst)) {
/* backward, skip last point */
ring->insert(ring->begin(), poList.rbegin(), poList.rend()-1);
return TRUE;
}
if (poLastNew->Equals(poLast)) {
/* backward, skip first point */
ring->insert(ring->end(), poList.rbegin()+1, poList.rend());
return TRUE;
}
if (poLastNew->Equals(poFirst)) {
/* forward, skip last point */
ring->insert(ring->begin(), poList.begin(), poList.end()-1);
return TRUE;
}
}
return FALSE;
}
GLuint CreateSplat(GLuint quadVao, PointList positions)
{
const int Size = 64;
Surface surface = CreateVolume(Size, Size, Size);
GLuint program = CreateProgram("Splat.VS", "Splat.GS", "Splat.FS");
glUseProgram(program);
GLint inverseSize = glGetUniformLocation(program, "InverseSize");
glUniform1f(inverseSize, 1.0f / (float) Size);
const float innerScale = 0.4f;
GLint inverseVariance = glGetUniformLocation(program, "InverseVariance");
glUniform1f(inverseVariance, -1.0f / (2.0f * innerScale * innerScale));
GLint normalizationConstant = glGetUniformLocation(program, "NormalizationConstant");
glUniform1f(normalizationConstant, 1.0f / std::pow(std::sqrt(TwoPi) * innerScale, 3.0f));
glBindFramebuffer(GL_FRAMEBUFFER, surface.FboHandle);
glBindTexture(GL_TEXTURE_3D, 0);
glViewport(0, 0, Size, Size);
glBindVertexArray(quadVao);
glEnable(GL_BLEND);
glBlendFunc(GL_ONE, GL_ONE);
PointList::const_iterator i = positions.begin();
for (; i != positions.end(); ++i) {
PointList::const_iterator next = i;
if (++next == positions.end())
next = positions.begin();
VectorMath::Vector3 velocity = (*next - *i);
GLint center = glGetUniformLocation(program, "Center");
glUniform4f(center, i->getX(), i->getY(), i->getZ(), 0);
GLint color = glGetUniformLocation(program, "Color");
glUniform3fv(color, 1, (float*) &velocity);
glDrawArraysInstanced(GL_TRIANGLE_STRIP, 0, 4, Size);
}
PezCheckCondition(GL_NO_ERROR == glGetError(), "Unable to create splat.");
glViewport(0, 0, PEZ_VIEWPORT_WIDTH, PEZ_VIEWPORT_HEIGHT);
glDisable(GL_BLEND);
return surface.TextureHandle[0];
}
FrontierList Planner::getFrontiers(GridMap* map, GridPoint start)
{
// Initialization
mFrontierCellCount = 0;
mFrontierCount = 0;
GridMap plan = GridMap(map->getWidth(), map->getHeight());
FrontierList result;
// Initialize the queue with the robot position
Queue queue;
queue.insert(Entry(0, start));
plan.setData(start, VISIBLE);
// Do full search with weightless Dijkstra-Algorithm
while(!queue.empty())
{
// Get the nearest cell from the queue
Queue::iterator next = queue.begin();
int distance = next->first;
GridPoint point = next->second;
queue.erase(next);
// Add neighbors
bool isFrontier = false;
PointList neighbors = getNeighbors(point, false);
char c = 0;
for(PointList::const_iterator cell = neighbors.begin(); cell < neighbors.end(); cell++)
{
if(map->getData(*cell,c) && c == UNKNOWN)
{
plan.setData(*cell, OBSTACLE);
isFrontier = true;
break;
}
if(map->getData(*cell, c) && c == VISIBLE &&
plan.getData(*cell, c) && c == UNKNOWN)
{
queue.insert(Entry(distance+1, *cell));
plan.setData(*cell, VISIBLE);
}
}
if(isFrontier)
{
result.push_back(getFrontier(map, &plan, point));
}
}
// Set result message and return the point list
if(result.size() > 0)
{
mStatus = SUCCESS;
sprintf(mStatusMessage, "Found %d frontiers with %d frontier cells.", mFrontierCount, mFrontierCellCount);
}else
{
mStatus = NO_GOAL;
sprintf(mStatusMessage, "No reachable frontiers found.");
}
return result;
}
void RenderCallback()
{
// Clear buffers
glClearColor(1.0f,1.0f,1.0f,1.0f);
glClear(GL_COLOR_BUFFER_BIT | GL_DEPTH_BUFFER_BIT);
glPointSize(1.0);
glColor3f(0,0,0);
glLineWidth(2.0f);
int i, j;
glBegin(GL_LINES);
//渲染凸多边形P
for( i = gN - 1, j = 0 ; j < gN ; i = j, j += 1 )
{
glVertex2f(gP[i].x,gP[i].y);
glVertex2f(gP[j].x,gP[j].y);
}
//渲染凸多边形Q
for( i = gM - 1, j = 0 ; j < gM ; i = j, j += 1 )
{
glVertex2f(gQ[i].x,gQ[i].y);
glVertex2f(gQ[j].x,gQ[j].y);
}
PointList::iterator it;
for( it = gInterList.begin() ; it != gInterList.end() ; it ++ )
{
glVertex2f((*it).x,(*it).y - 20);
glVertex2f((*it).x,(*it).y + 20);
}
glEnd();
glutSwapBuffers();
}
void draw_hand_trace(sf::RenderWindow& window,
const PointList& pointList,
const sf::Color& color,
const float depthScale)
{
if (pointList.size() < 2) { return; }
const float thickness = 4;
auto it = pointList.begin();
astra::Vector2i previousPoint = *it;
while (it != pointList.end())
{
const astra::Vector2i currentPoint = *it;
++it;
const sf::Vector2f p1((previousPoint.x + .5f) * depthScale,
(previousPoint.y + .5f) * depthScale);
const sf::Vector2f p2((currentPoint.x + .5f) * depthScale,
(currentPoint.y + .5f) * depthScale);
previousPoint = currentPoint;
window.draw(sfLine(p1, p2, color, thickness));
}
}
void Dataset<D, ELEM_TYPE>::load(const PointList& newPoints)
{
// Pre-allocate memory in one sys call
m_points.reserve(m_points.size() + newPoints.size());
// Append given points to end of current point list
m_points.insert(m_points.end(), newPoints.begin(), newPoints.end());
}
void Planner::setCoverageMap(PointList points, char value)
{
PointList::iterator i;
for(i = points.begin(); i < points.end(); i++)
{
mCoverageMap->setData(*i, value);
}
}
void handleDual(Hyperbola_segment_2 hs, std::vector<PointList>& polylines)
{
PointList p;
hs.generate_points(p);
PointList points;
points.insert(points.end(), p.begin(), p.end());
polylines.push_back(points);
}
void handleDual(Hyperbola_ray_2 hr, Iso_rectangle_2 crect, std::vector<PointList>& polylines)
{
PointList p;
hr.generate_points(p);
PointList points;
points.insert(points.end(), p.begin(), p.end());
polylines.push_back(points);
}
void Planner::addReading(Pose p)
{
PointList points = willBeExplored(p);
for(PointList::iterator i = points.begin(); i < points.end(); ++i)
{
mCoverageMap->setData(*i, VISIBLE);
}
}
void pushToFarPlane(PointList& points)
{
for(PointList::iterator itr=points.begin();
itr!=points.end();
++itr)
{
itr->second.z() = 1.0f;
}
}
void transform(const PointList& in,PointList& out,const osg::Matrix& matrix)
{
for(PointList::const_iterator itr=in.begin();
itr!=in.end();
++itr)
{
out.push_back(Point(itr->first,itr->second * matrix));
}
}
void transform(PointList& points,const osg::Matrix& matrix)
{
for(PointList::iterator itr=points.begin();
itr!=points.end();
++itr)
{
itr->second = itr->second*matrix;
}
}
PointList Board::removeDuplicates(PointList lst) const {
PointList res;
for (auto pt : lst) {
if (std::find(res.begin(), res.end(), pt) == res.end()) {
res.push_back(pt);
}
}
return res;
}
PointList::PointList (const PointList& pointList):
KKQueue<Point> (true)
{
PointList::const_iterator idx;
for (idx = pointList.begin (); idx != pointList.end (); idx++)
{
PushOnBack (new Point (*(*idx)));
}
}
void copyPointListToVertexList(const PointList& in,VertexList& out)
{
out.reserve(in.size());
for(PointList::const_iterator itr=in.begin();
itr!=in.end();
++itr)
{
out.push_back(itr->second);
}
}
ScreenCalibrator::PickResult ScreenCalibrator::pickPoint(const Ray& queryRay) const
{
/* Create a ray picker: */
Geometry::RayPicker<Scalar,3> picker(queryRay,Scalar(Vrui::getRayPickCosine()));
/* Process all points: */
for(PointList::const_iterator pIt=trackingPoints.begin();pIt!=trackingPoints.end();++pIt)
picker(*pIt);
for(PointList::const_iterator pIt=floorPoints.begin();pIt!=floorPoints.end();++pIt)
picker(*pIt);
for(PointList::const_iterator pIt=screenPoints.begin();pIt!=screenPoints.end();++pIt)
picker(*pIt);
for(PointList::const_iterator pIt=ballPoints.begin();pIt!=ballPoints.end();++pIt)
picker(*pIt);
/* Return the index of the picked point: */
if(picker.havePickedPoint())
return picker.getPickIndex();
else
return ~PickResult(0);
}
void handleDual(Hyperbola_ray_2 hr, Iso_rectangle_2 crect, std::vector<PointList>& polylines)
{
std::cerr << "hyperbola ray" << std::endl; // hr.draw(str);
PointList p;
hr.generate_points(p);
PointList points;
points.insert(points.end(), p.begin(), p.end());
polylines.push_back(points);
for (unsigned int i = 0; i < p.size() - 1; i++) {
Segment_2 seg(p[i], p[i+1]);
// doing nothing here
}
}
boost::optional<Point> fastFilterPointList(
const PointList& pointList,
std::vector<Point>& newPointList) const
{
assert(!pointList.empty());
newPointList.reserve(pointList.size() - 1);
std::copy(
++pointList.begin(),
pointList.end(),
std::back_inserter(newPointList)
);
return boost::optional<Point>(pointList.front());
} // fastFilterFunctorList
void handleDual(Hyperbola_segment_2 hs, std::vector<PointList>& polylines)
{
std::cerr << "hyperbola segment" << std::endl; //hs.draw(str);
PointList p;
hs.generate_points(p);
std::cerr << "# hyperbola points: " << p.size() << std::endl;
PointList points;
points.insert(points.end(), p.begin(), p.end());
polylines.push_back(points);
for (unsigned int i = 0; i < p.size() - 1; i++) {
Segment_2 seg(p[i], p[i+1]);
// doing nothing here
}
}
// True if a track segment could be placed in its present location
bool Editor::can_place_track(ITrackSegmentPtr track)
{
PointList covered;
track->get_endpoints(covered);
track->get_covers(covered);
for (auto it = covered.begin(); it != covered.end(); ++it) {
if (map->is_valid_track(*it)) {
warn() << "Cannot place track here";
return false;
}
}
return true;
}
PointList ClusteredDatasetGenerator::generate(unsigned int numDimensions,
const std::vector<ClusterSpecification>& clusters) const
{
PointList points;
RandomPointGenerator rpGenerator;
for (std::vector<ClusterSpecification>::const_iterator it = clusters.begin();
(it != clusters.end()); it++)
{
PointList clusterPoints = rpGenerator.generatePointsInRegion(
numDimensions, it->region(), it->numPoints());
points.insert(points.end(), clusterPoints.begin(), clusterPoints.end());
}
return points;
}
bool Planner::isFrontierCell(GridMap* map, GridPoint point) const
{
char c = 0;
if(map->getData(point, c) && c != VISIBLE)
return false;
PointList neighbors = getNeighbors(point, true);
for(PointList::const_iterator cell = neighbors.begin(); cell < neighbors.end(); cell++) {
char c = 0;
if(map->getData(*cell, c) && c == UNKNOWN) {
return true;
}
}
return false;
}
Pose Planner::getCoverageTarget(Pose start)
{
GridPoint startPoint;
startPoint.x = start.x;
startPoint.y = start.y;
PointList fCells = getFrontierCells(mCoverageMap, startPoint);
PointList::const_iterator p;
Pose target;
for(p = fCells.begin(); p < fCells.end(); p++)
{
target.x = p->x;
target.y = p->y;
LOG_DEBUG_S << "possible goals: " << target.x << "/" << target.y;
if(p->distance > 20 && p->distance < 30) break;
}
return target;
}
PointList Board::getFutureBlasts() const {
PointList bombs = getBombs();
bombs.splice(bombs.end(), findAll(Element(LL("OTHER_BOMB_BOMBERMAN"))));
bombs.splice(bombs.end(), findAll(Element(LL("BOMB_BOMBERMAN"))));
PointList rslt;
PointList walls = getWalls();
for (auto bmb : bombs) {
rslt.push_back(bmb);
PointList bombSurrs = bmb.getSurrounds(size);
for (auto surr : bombSurrs) {
if (std::find(walls.begin(), walls.end(), surr) == walls.end()) {
rslt.push_back(surr);
}
}
}
return removeDuplicates(rslt);
}
PointList Planner::getFrontier(GridMap* map, GridMap* plan, GridPoint start)
{
// Mark the cell as "already added to a frontier" by setting
// the value in the plan to 2.
PointList frontier;
mFrontierCount++;
// Initialize the queue with the first frontier cell
Queue queue;
queue.insert(Entry(0, start));
plan->setData(start, OBSTACLE);
// Do full search with weightless Dijkstra-Algorithm
while(!queue.empty())
{
// Get the nearest cell from the queue
Queue::iterator next = queue.begin();
int distance = next->first;
GridPoint point = next->second;
queue.erase(next);
// Add it to the frontier
frontier.push_back(point);
mFrontierCellCount++;
// Add all adjacent frontier cells to the queue
PointList neighbors = getNeighbors(point, true);
char c = 0;
for(PointList::const_iterator cell = neighbors.begin(); cell < neighbors.end(); cell++)
{
if(plan->getData(*cell, c) && c != OBSTACLE && isFrontierCell(map, *cell))
{
plan->setData(*cell, OBSTACLE);
queue.insert(Entry(distance+1, *cell));
}
}
}
return frontier;
}
// clip the convex hull 'in' to plane to generate a clipped convex hull 'out'
// return true if points remain after clipping.
unsigned int clip(const Plane& plane,const PointList& in, PointList& out,unsigned int planeMask)
{
std::vector<float> distance;
distance.reserve(in.size());
for(PointList::const_iterator itr=in.begin();
itr!=in.end();
++itr)
{
distance.push_back(plane.distance(itr->second));
}
out.clear();
for(unsigned int i=0;i<in.size();++i)
{
unsigned int i_1 = (i+1)%in.size(); // do the mod to wrap the index round back to the start.
if (distance[i]>=0.0f)
{
out.push_back(in[i]);
if (distance[i_1]<0.0f)
{
unsigned int mask = (in[i].first & in[i_1].first) | planeMask;
float r = distance[i_1]/(distance[i_1]-distance[i]);
out.push_back(Point(mask,in[i].second*r+in[i_1].second*(1.0f-r)));
}
}
else if (distance[i_1]>0.0f)
{
unsigned int mask = (in[i].first & in[i_1].first) | planeMask;
float r = distance[i_1]/(distance[i_1]-distance[i]);
out.push_back(Point(mask,in[i].second*r+in[i_1].second*(1.0f-r)));
}
}
return out.size();
}
/*!
\brief Load geometry (polygon BUD/PAR layers)
\return number of invalid features
*/
int VFKDataBlockDB::LoadGeometryPolygon()
{
int nInvalidNoLines, nInvalidNoRings, nGeometries, nBridges;
int rowId, nCount, nCountMax;
size_t nLines;
GIntBig iFID;
bool bIsPar, bNewRing, bFound;
CPLString osSQL;
const char *vrColumn[2];
GUIntBig vrValue[2];
GUIntBig id, idOb;
sqlite3_stmt *hStmt;
VFKReaderDB *poReader;
VFKDataBlockDB *poDataBlockLines1, *poDataBlockLines2;
VFKFeatureDB *poFeature;
VFKFeatureDBList poLineList;
/* first is to be considered as exterior */
PointListArray poRingList;
std::vector<OGRLinearRing *> poLinearRingList;
OGRPolygon ogrPolygon;
OGRLinearRing *poOgrRing;
nInvalidNoLines = nInvalidNoRings = nGeometries = 0;
poReader = (VFKReaderDB*) m_poReader;
if (EQUAL (m_pszName, "PAR")) {
poDataBlockLines1 = (VFKDataBlockDB *) m_poReader->GetDataBlock("HP");
poDataBlockLines2 = poDataBlockLines1;
bIsPar = TRUE;
}
else {
poDataBlockLines1 = (VFKDataBlockDB *) m_poReader->GetDataBlock("OB");
poDataBlockLines2 = (VFKDataBlockDB *) m_poReader->GetDataBlock("SBP");
bIsPar = FALSE;
}
if (NULL == poDataBlockLines1) {
CPLError(CE_Warning, CPLE_FileIO,
"Data block %s not found. Unable to build geometry for %s.",
bIsPar ? "HP" : "OB", m_pszName);
return -1;
}
if (NULL == poDataBlockLines2) {
CPLError(CE_Warning, CPLE_FileIO,
"Data block %s not found. Unable to build geometry for %s.",
"SBP", m_pszName);
return -1;
}
poDataBlockLines1->LoadGeometry();
poDataBlockLines2->LoadGeometry();
if (LoadGeometryFromDB()) /* try to load geometry from DB */
return 0;
if (bIsPar) {
vrColumn[0] = "PAR_ID_1";
vrColumn[1] = "PAR_ID_2";
}
else {
vrColumn[0] = "OB_ID";
vrColumn[1] = "PORADOVE_CISLO_BODU";
vrValue[1] = 1;
}
osSQL.Printf("SELECT ID,%s,rowid FROM %s", FID_COLUMN, m_pszName);
if (poReader->IsSpatial())
poReader->ExecuteSQL("BEGIN");
std::vector<VFKDbValue> record;
record.push_back(VFKDbValue(DT_BIGINT));
record.push_back(VFKDbValue(DT_BIGINT));
record.push_back(VFKDbValue(DT_INT));
poReader->PrepareStatement(osSQL.c_str());
while(poReader->ExecuteSQL(record) == OGRERR_NONE) {
nBridges = 0;
/* read values */
id = static_cast<GIntBig> (record[0]);
iFID = static_cast<GIntBig> (record[1]);
rowId = static_cast<int> (record[2]);
poFeature = (VFKFeatureDB *) GetFeatureByIndex(rowId - 1);
CPLAssert(NULL != poFeature && poFeature->GetFID() == iFID);
if (bIsPar) {
vrValue[0] = vrValue[1] = id;
poLineList = poDataBlockLines1->GetFeatures(vrColumn, vrValue, 2);
}
else {
VFKFeatureDB *poLineSbp;
std::vector<VFKFeatureDB *> poLineListOb;
sqlite3_stmt *hStmtOb;
osSQL.Printf("SELECT ID FROM %s WHERE BUD_ID = " CPL_FRMT_GUIB,
poDataBlockLines1->GetName(), id);
if (poReader->IsSpatial()) {
CPLString osColumn;
osColumn.Printf(" AND %s IS NULL", GEOM_COLUMN);
osSQL += osColumn;
}
std::vector<VFKDbValue> record1;
record1.push_back(VFKDbValue(DT_BIGINT));
poReader->PrepareStatement(osSQL.c_str(), 1);
while(poReader->ExecuteSQL(record, 1) == OGRERR_NONE) {
idOb = static_cast<GIntBig> (record[0]);
vrValue[0] = idOb;
poLineSbp = poDataBlockLines2->GetFeature(vrColumn, vrValue, 2);
if (poLineSbp)
poLineList.push_back(poLineSbp);
}
}
nLines = poLineList.size();
if (nLines < 1) {
CPLDebug("OGR-VFK",
"%s: unable to collect rings for polygon fid = %ld (no lines)",
m_pszName, iFID);
nInvalidNoLines++;
continue;
}
/* clear */
ogrPolygon.empty();
poRingList.clear();
/* collect rings from lines */
bFound = FALSE;
nCount = 0;
nCountMax = static_cast<int>(nLines) * 2;
while (poLineList.size() > 0 && nCount < nCountMax) {
bNewRing = !bFound ? TRUE : FALSE;
bFound = FALSE;
int i = 1;
for (VFKFeatureDBList::iterator iHp = poLineList.begin(), eHp = poLineList.end();
iHp != eHp; ++iHp, ++i) {
const OGRLineString *pLine = (OGRLineString *) (*iHp)->GetGeometry();
if (pLine && AppendLineToRing(&poRingList, pLine, bNewRing)) {
bFound = TRUE;
poLineList.erase(iHp);
break;
}
}
nCount++;
}
CPLDebug("OGR-VFK", "%s: fid = %ld nlines = %d -> nrings = %d", m_pszName,
iFID, (int)nLines, (int)poRingList.size());
if (poLineList.size() > 0) {
CPLDebug("OGR-VFK",
"%s: unable to collect rings for polygon fid = %ld",
m_pszName, iFID);
nInvalidNoRings++;
continue;
}
/* build rings */
poLinearRingList.clear();
int i = 1;
for (PointListArray::const_iterator iRing = poRingList.begin(), eRing = poRingList.end();
iRing != eRing; ++iRing) {
OGRPoint *poPoint;
PointList *poList = *iRing;
poLinearRingList.push_back(new OGRLinearRing());
poOgrRing = poLinearRingList.back();
CPLAssert(NULL != poOgrRing);
for (PointList::iterator iPoint = poList->begin(), ePoint = poList->end();
iPoint != ePoint; ++iPoint) {
poPoint = &(*iPoint);
poOgrRing->addPoint(poPoint);
}
i++;
}
/* find exterior ring */
if (poLinearRingList.size() > 1) {
double dArea, dMaxArea;
std::vector<OGRLinearRing *>::iterator exteriorRing;
exteriorRing = poLinearRingList.begin();
dMaxArea = -1.;
for (std::vector<OGRLinearRing *>::iterator iRing = poLinearRingList.begin(),
eRing = poLinearRingList.end(); iRing != eRing; ++iRing) {
poOgrRing = *iRing;
if (!IsRingClosed(poOgrRing))
continue; /* skip unclosed rings */
dArea = poOgrRing->get_Area();
if (dArea > dMaxArea) {
dMaxArea = dArea;
exteriorRing = iRing;
}
}
if (exteriorRing != poLinearRingList.begin()) {
std::swap(*poLinearRingList.begin(), *exteriorRing);
}
}
/* build polygon from rings */
for (std::vector<OGRLinearRing *>::iterator iRing = poLinearRingList.begin(),
eRing = poLinearRingList.end(); iRing != eRing; ++iRing) {
poOgrRing = *iRing;
/* check if ring is closed */
if (IsRingClosed(poOgrRing)) {
ogrPolygon.addRing(poOgrRing);
}
else {
if (poOgrRing->getNumPoints() == 2) {
CPLDebug("OGR-VFK", "%s: Polygon (fid = %ld) bridge removed",
m_pszName, iFID);
nBridges++;
}
else {
CPLDebug("OGR-VFK",
"%s: Polygon (fid = %ld) unclosed ring skipped",
m_pszName, iFID);
}
}
delete poOgrRing;
*iRing = NULL;
}
/* set polygon */
ogrPolygon.setCoordinateDimension(2); /* force 2D */
if (ogrPolygon.getNumInteriorRings() + nBridges != (int) poLinearRingList.size() - 1 ||
!poFeature->SetGeometry(&ogrPolygon)) {
nInvalidNoRings++;
continue;
}
/* store also geometry in DB */
if (poReader->IsSpatial() &&
SaveGeometryToDB(&ogrPolygon, rowId) != OGRERR_FAILURE)
nGeometries++;
}
/* free ring list */
for (PointListArray::iterator iRing = poRingList.begin(), eRing = poRingList.end();
iRing != eRing; ++iRing) {
delete (*iRing);
*iRing = NULL;
}
CPLDebug("OGR-VFK", "%s: nolines = %d norings = %d",
m_pszName, nInvalidNoLines, nInvalidNoRings);
/* update number of geometries in VFK_DB_TABLE table */
UpdateVfkBlocks(nGeometries);
if (poReader->IsSpatial())
poReader->ExecuteSQL("COMMIT");
return nInvalidNoLines + nInvalidNoRings;
}
/*!
\brief Load geometry (polygon BUD/PAR layers)
\return number of invalid features
*/
int VFKDataBlock::LoadGeometryPolygon()
{
long nInvalid;
bool bIsPar, bNewRing, bFound;
GUIntBig id, idOb;
int nCount, nCountMax;
int idxId, idxPar1, idxPar2, idxBud, idxOb, idxIdOb;
VFKFeature *poFeature;
VFKDataBlock *poDataBlockLines1, *poDataBlockLines2;
VFKFeatureList poLineList;
PointListArray poRingList; /* first is to be considered as exterior */
OGRLinearRing ogrRing;
OGRPolygon ogrPolygon;
idxPar1 = idxPar2 = idxBud = idxOb = idxIdOb = 0;
nInvalid = 0;
if (EQUAL (m_pszName, "PAR")) {
poDataBlockLines1 = (VFKDataBlock *) m_poReader->GetDataBlock("HP");
poDataBlockLines2 = poDataBlockLines1;
bIsPar = TRUE;
}
else {
poDataBlockLines1 = (VFKDataBlock *) m_poReader->GetDataBlock("OB");
poDataBlockLines2 = (VFKDataBlock *) m_poReader->GetDataBlock("SBP");
bIsPar = FALSE;
}
if (NULL == poDataBlockLines1 || NULL == poDataBlockLines2) {
CPLError(CE_Failure, CPLE_NotSupported,
"Data block %s not found.\n", m_pszName);
return nInvalid;
}
poDataBlockLines1->LoadGeometry();
poDataBlockLines2->LoadGeometry();
idxId = GetPropertyIndex("ID");
if (idxId < 0) {
CPLError(CE_Failure, CPLE_NotSupported,
"Corrupted data (%s).\n", m_pszName);
return nInvalid;
}
if (bIsPar) {
idxPar1 = poDataBlockLines1->GetPropertyIndex("PAR_ID_1");
idxPar2 = poDataBlockLines1->GetPropertyIndex("PAR_ID_2");
if (idxPar1 < 0 || idxPar2 < 0) {
CPLError(CE_Failure, CPLE_NotSupported,
"Corrupted data (%s).\n", m_pszName);
return nInvalid;
}
}
else { /* BUD */
idxIdOb = poDataBlockLines1->GetPropertyIndex("ID");
idxBud = poDataBlockLines1->GetPropertyIndex("BUD_ID");
idxOb = poDataBlockLines2->GetPropertyIndex("OB_ID");
if (idxIdOb < 0 || idxBud < 0 || idxOb < 0) {
CPLError(CE_Failure, CPLE_NotSupported,
"Corrupted data (%s).\n", m_pszName);
return nInvalid;
}
}
for (int i = 0; i < ((IVFKDataBlock *) this)->GetFeatureCount(); i++) {
poFeature = (VFKFeature *) GetFeatureByIndex(i);
id = strtoul(poFeature->GetProperty(idxId)->GetValueS(), NULL, 0);
if (bIsPar) {
poLineList = poDataBlockLines1->GetFeatures(idxPar1, idxPar2, id);
}
else {
VFKFeature *poLineOb, *poLineSbp;
std::vector<VFKFeature *> poLineListOb;
poLineListOb = poDataBlockLines1->GetFeatures(idxBud, id);
for (std::vector<VFKFeature *>::const_iterator iOb = poLineListOb.begin(), eOb = poLineListOb.end();
iOb != eOb; ++iOb) {
poLineOb = (*iOb);
idOb = strtoul(poLineOb->GetProperty(idxIdOb)->GetValueS(), NULL, 0);
poLineSbp = poDataBlockLines2->GetFeature(idxOb, idOb);
if (poLineSbp)
poLineList.push_back(poLineSbp);
}
}
if (poLineList.size() < 1)
continue;
/* clear */
ogrPolygon.empty();
poRingList.clear();
/* collect rings (points) */
bFound = FALSE;
nCount = 0;
nCountMax = poLineList.size() * 2;
while (poLineList.size() > 0 && nCount < nCountMax) {
bNewRing = !bFound ? TRUE : FALSE;
bFound = FALSE;
for (VFKFeatureList::iterator iHp = poLineList.begin(), eHp = poLineList.end();
iHp != eHp; ++iHp) {
const OGRLineString *pLine = (OGRLineString *) (*iHp)->GetGeometry();
if (pLine && AppendLineToRing(&poRingList, pLine, bNewRing)) {
bFound = TRUE;
poLineList.erase(iHp);
break;
}
}
nCount++;
}
/* create rings */
for (PointListArray::const_iterator iRing = poRingList.begin(), eRing = poRingList.end();
iRing != eRing; ++iRing) {
PointList *poList = *iRing;
ogrRing.empty();
for (PointList::iterator iPoint = poList->begin(), ePoint = poList->end();
iPoint != ePoint; ++iPoint) {
ogrRing.addPoint(&(*iPoint));
}
ogrPolygon.addRing(&ogrRing);
}
/* set polygon */
ogrPolygon.setCoordinateDimension(2); /* force 2D */
if (!poFeature->SetGeometry(&ogrPolygon))
nInvalid++;
}
/* free ring list */
for (PointListArray::iterator iRing = poRingList.begin(),
eRing = poRingList.end(); iRing != eRing; ++iRing) {
delete (*iRing);
*iRing = NULL;
}
poDataBlockLines1->ResetReading();
poDataBlockLines2->ResetReading();
return nInvalid;
}
// adapted from http://www.cgal.org/Manual/beta/examples/Surface_reconstruction_points_3/poisson_reconstruction_example.cpp
Mesh poissonSurface(const Mat ipoints, const Mat normals)
{
// Poisson options
FT sm_angle = 20.0; // Min triangle angle in degrees.
FT sm_radius = 300; // Max triangle size w.r.t. point set average spacing.
FT sm_distance = 0.375; // Surface Approximation error w.r.t. point set average spacing.
PointList points;
points.reserve(ipoints.rows);
float min = 0, max = 0;
for (int i=0; i<ipoints.rows; i++) {
float const *p = ipoints.ptr<float const>(i), *n = normals.ptr<float const>(i);
points.push_back(Point_with_normal(Point(p[0]/p[3], p[1]/p[3], p[2]/p[3]), Vector(n[0], n[1], n[2])));
if (p[0]/p[3] > max) max = p[0]/p[3];
if (p[0]/p[3] < min) min = p[0]/p[3];
}
// Creates implicit function from the read points using the default solver.
// Note: this method requires an iterator over points
// + property maps to access each point's position and normal.
// The position property map can be omitted here as we use iterators over Point_3 elements.
Poisson_reconstruction_function function(points.begin(), points.end(), CGAL::make_normal_of_point_with_normal_pmap(points.begin()) );
// Computes the Poisson indicator function f() at each vertex of the triangulation.
bool success = function.compute_implicit_function();
assert(success);
#ifdef TEST_BUILD
printf("implicit function ready. Meshing...\n");
#endif
// Computes average spacing
FT average_spacing = CGAL::compute_average_spacing(points.begin(), points.end(), 6 /* knn = 1 ring */);
// Gets one point inside the implicit surface
// and computes implicit function bounding sphere radius.
Point inner_point = function.get_inner_point();
Sphere bsphere = function.bounding_sphere();
FT radius = std::sqrt(bsphere.squared_radius());
// Defines the implicit surface: requires defining a
// conservative bounding sphere centered at inner point.
FT sm_sphere_radius = 5.0 * radius;
FT sm_dichotomy_error = sm_distance*average_spacing/1000.0; // Dichotomy error must be << sm_distance
Surface_3 surface(function, Sphere(inner_point,sm_sphere_radius*sm_sphere_radius), sm_dichotomy_error/sm_sphere_radius);
// Defines surface mesh generation criteria
// parameters: min triangle angle (degrees), max triangle size, approximation error
CGAL::Surface_mesh_default_criteria_3<STr> criteria(sm_angle, sm_radius*average_spacing, sm_distance*average_spacing);
// Generates surface mesh with manifold option
STr tr; // 3D Delaunay triangulation for surface mesh generation
C2t3 c2t3(tr); // 2D complex in 3D Delaunay triangulation
// parameters: reconstructed mesh, implicit surface, meshing criteria, 'do not require manifold mesh'
CGAL::make_surface_mesh(c2t3, surface, criteria, CGAL::Non_manifold_tag());
assert(tr.number_of_vertices() > 0);
// Search structure: index of the vertex at an exactly given position
std::map<Point, int> vertexIndices;
// Extract all vertices of the resulting mesh
Mat vertices(tr.number_of_vertices(), 4, CV_32FC1), faces(c2t3.number_of_facets(), 3, CV_32SC1);
#ifdef TEST_BUILD
printf("%i vertices, %i facets. Converting...\n", vertices.rows, faces.rows);
#endif
{ int i=0;
for (C2t3::Vertex_iterator it=c2t3.vertices_begin(); it!=c2t3.vertices_end(); it++, i++) {
float *vertex = vertices.ptr<float>(i);
Point p = it->point();
vertex[0] = p.x();
vertex[1] = p.y();
vertex[2] = p.z();
vertex[3] = 1;
vertexIndices[p] = i;
}
vertices.resize(i);
}
// Extract all faces of the resulting mesh and calculate the index of each of their vertices
{ int i=0;
for (C2t3::Facet_iterator it=c2t3.facets_begin(); it!=c2t3.facets_end(); it++, i++) {
Cell cell = *it->first;
int vertex_excluded = it->second;
int32_t* outfacet = faces.ptr<int32_t>(i);
// a little magic so that face normals are oriented outside
char sign = (function(cell.vertex(vertex_excluded)->point()) > 0) ? 1 : 2; // 2 = -1 (mod 3)
for (char j = vertex_excluded + 1; j < vertex_excluded + 4; j++) {
outfacet[(sign*j)%3] = vertexIndices[cell.vertex(j%4)->point()];
}
}
}
return Mesh(vertices, faces);
}