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;
}
/**
* Calculates the points where the meshes merge.
* @param MeshList * meshList The list of meshes whose points must be calculated.
*/
void
Polyhedron::getPoints(MeshList * meshList)
{
MeshList::iterator i1, i2, i3;
PointList vert; /* Set a Vertices list. */
PointList::iterator vit;
Point * temp;
/* Going through the list of meshes and calculating the vertices of the polyhedron. */
for (i1 = meshList->begin(); i1 != meshList->end(); i1++) {
vert.clear();
for (i2 = meshList->begin(); i2 != meshList->end(); i2++) {
for (i3 = meshList->begin(); i3 != meshList->end(); i3++) {
/* Getting the vertices of the face only if all the meshes are different. */
if (i1 != i2 && i1 != i3 && i2 != i3) {
temp = Mesh::intersection(*(*i1), *(*i2), *(*i3));
if (temp != NULL) {
vert.push_back(temp);
vertices.push_back(temp);
}
}
}
}
faces.push_back(new Face(&vertices, &(*i1)->normal));
}
getOrigin();
}
void Puzzle::slidePiece(int x1, int y1, int x2, int y2) {
int count;
PointList slidePoints;
slidePoints.resize(320);
x1 += _pieceInfo[_puzzlePiece].offX;
y1 += _pieceInfo[_puzzlePiece].offY;
count = pathLine(slidePoints, 0, Point(x1, y1),
Point(x2 + _pieceInfo[_puzzlePiece].offX, y2 + _pieceInfo[_puzzlePiece].offY));
if (count > 1) {
int factor = count / 4;
_sliding = true;
if (!factor)
factor++;
for (int i = 1; i < count; i += factor) {
_slidePointX = slidePoints[i].x;
_slidePointY = slidePoints[i].y;
_vm->_render->drawScene();
_vm->_system->delayMillis(10);
}
_sliding = false;
}
_pieceInfo[_puzzlePiece].curX = x2;
_pieceInfo[_puzzlePiece].curY = y2;
}
PointList Board::getOtherBombermans() const {
PointList rslt;
rslt.splice(rslt.end(), findAll(Element(LL("OTHER_BOMBERMAN"))));
rslt.splice(rslt.end(), findAll(Element(LL("OTHER_BOMB_BOMBERMAN"))));
rslt.splice(rslt.end(), findAll(Element(LL("OTHER_DEAD_BOMBERMAN"))));
return rslt;
}
Vector ShootRay (Reta R, double Relative) {
PointList *P;
Point *hit;
Vector Target;
Vector color;
Vector Normal;
Vector Zero (0,0,0);
Reta S;
if (Relative<reflection)
return Zero;
RaysShooted++;
P=new PointList;
scene->Intersect (R,P);
hit=P->First ();
if (hit!=NULL) {
Target=R.O+R.R*hit->t;
Normal=hit->owner->Normal (Target);
color=lightlist->Shade (Target,Normal,scene);
delete P;
if (hit->owner->surface->GetKs()!=0.0) {
S.O=Target+Normal*epsilon;
S.R=R.R-Normal*(R.R*Normal)*2.0;
return
hit->owner->surface->Apply
(color,ShootRay (S,Relative*hit->owner->surface->GetKs()));
}
else
return hit->owner->surface->Apply (color,Zero);
}
else {
delete P;
return Zero;
}
}
double AverageIgnoreNullAnalysis::analyze(const PointList &values) const
{
if(values.isEmpty())
{
return 0;
}
const double sum = AbstractAnalysis::listSum(values);
int length = 0;
foreach(const Point value, values.points())
{
if(value != 0)
{
length++;
}
}
if(length == 0)
{
return 0.0;
}
const double result = sum / static_cast<double>(length);
return result;
}
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 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();
}
bool copy_block(Tile* tiles, const Block* block) {
const p2i& p = block->position;
int xp = (p.x - START_X + SQUARE_SIZE / 2) / SQUARE_SIZE;
int yp = (p.y - START_Y + SQUARE_SIZE / 2) / SQUARE_SIZE;
if (is_block_available(tiles, xp, yp)) {
for (int i = 0; i < 4; ++i) {
int cx = xp + MARK_STEPS[i * 2];
int cy = yp + MARK_STEPS[i * 2 + 1];
uint32_t idx = get_tiles_index(cx, cy);
Tile& t = tiles[idx];
t.state.set(BIT_MARKED);
t.color = block->colors[i];
PointList list;
check(tiles, cx, cy, -1, list, true);
list.add(cx, cy);
if (list.size() > 2) {
//LOG << "connected";
t.state.set(BIT_COHERENT);
for (size_t j = 0; j < list.size(); ++j) {
const p2i& p = list.get(j);
//LOG << j << " = " << p.x << " " << p.y;
set_state(tiles, p.x, p.y, BIT_COHERENT);
}
}
}
determineEdges(tiles);
return true;
}
else {
//LOG << "Block is not available";
}
return false;
}
T_Point vectorFromEigenArray(const PointList& array)
{
T_Point res;
for(int i =0;i<array.cols();++i)
res.push_back(array.col(i));
return res;
}
bool CollisionManager::isIntersectsPointPolygon(ICollisionHull* point, ICollisionHull* polygon)
{
PointCollisionHull* pointCH = dynamic_cast<PointCollisionHull*>(point);
PoligonCollisionHull* poligonCH = dynamic_cast<PoligonCollisionHull*>(polygon);
Vector3 position = pointCH->getPosition();
PointList points = poligonCH->getPoints();
int i0, i1;
float A, B, C, D;
Vector3 P0, P1;
for(int i = 0; i < points.size(); i++)
{
i0 = i;
i1 = (i == (points.size() - 1)) ? 0 : i + 1;
P0 = points[i0];
P1 = points[i1];
A = P0._y - P1._y;
B = P1._x - P0._x;
C = (P0._x * P1._y) - (P1._x * P0._y);
D = (A * position._x) + (B * position._y) + C;
if(D > 0)
{
return false;
}
}
return true;
}
void SplineTrack::get_endpoints(PointList& output) const
{
output.push_back(origin);
if (abs(delta.x) > 0 || abs(delta.y) > 0)
output.push_back(
make_point(origin.x + delta.x, origin.y + delta.y));
}
void Planner::addReading(Pose p)
{
PointList points = willBeExplored(p);
for(PointList::iterator i = points.begin(); i < points.end(); ++i)
{
mCoverageMap->setData(*i, VISIBLE);
}
}
void Planner::setCoverageMap(PointList points, char value)
{
PointList::iterator i;
for(i = points.begin(); i < points.end(); i++)
{
mCoverageMap->setData(*i, value);
}
}
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 pushToFarPlane(PointList& points)
{
for(PointList::iterator itr=points.begin();
itr!=points.end();
++itr)
{
itr->second.z() = 1.0f;
}
}
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;
}
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 handleDual(Segment_2 s, std::vector<PointList>& polylines)
{
Point_2 ss = s.source();
Point_2 st = s.target();
PointList points;
points.push_back(ss);
points.push_back(st);
polylines.push_back(points);
}
void handleDual(Segment_2 s, std::vector<PointList>& polylines)
{
Point_2 ss = s.source();
Point_2 st = s.target();
std::cerr << "segment " << ss << " " << st << std::endl; // str << s;
PointList points;
points.push_back(ss);
points.push_back(st);
polylines.push_back(points);
}
//-----------------------------------------------------------------------------
void CDrawContext::drawPolygon (const CPoint* pPoints, int32_t numberOfPoints, const CDrawStyle drawStyle)
{
assert (numberOfPoints < 0);
PointList list (static_cast<uint32_t> (numberOfPoints));
for (int32_t i = 0; i < numberOfPoints; i++)
{
list.push_back (pPoints[i]);
}
drawPolygon (list, drawStyle);
}
PointList Board::findAll(Element el) const {
PointList rslt;
for (auto i = 0; i < size*size; ++i) {
Point pt = xyl.getXY(i);
if (isAt(pt.getX(), pt.getY(), el)) {
rslt.push_back(pt);
}
}
return rslt;
}
// copyVertexListToPointList a vector for Vec3 into a vector of Point's.
void copyVertexListToPointList(const VertexList& in,PointList& out)
{
out.reserve(in.size());
for(VertexList::const_iterator itr=in.begin();
itr!=in.end();
++itr)
{
out.push_back(Point(0,*itr));
}
}
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);
}
}
void computePlanes(const PointList& front, const PointList& back, Polytope::PlaneList& planeList)
{
for(unsigned int i=0;i<front.size();++i)
{
unsigned int i_1 = (i+1)%front.size(); // do the mod to wrap the index round back to the start.
if (!(front[i].first & front[i_1].first))
{
planeList.push_back(Plane(front[i].second,front[i_1].second,back[i].second));
}
}
}
ScreenCalibrator::PointList ScreenCalibrator::readTotalstationSurveyFile(const char* fileName,const char* tag) const
{
/* Open the CSV input file: */
IO::TokenSource tok(Vrui::openFile(fileName));
tok.setPunctuation(",\n");
tok.setQuotes("\"");
tok.skipWs();
/* Read point records until the end of file: */
PointList result;
unsigned int line=2;
while(!tok.eof())
{
/* Read the point coordinates: */
Point p;
for(int i=0;i<3;++i)
{
if(i>0)
{
tok.readNextToken();
if(!tok.isToken(","))
Misc::throwStdErr("MeasureEnvironment::MeasureEnvironment: Format error in input file %s",fileName);
}
p[i]=Scalar(atof(tok.readNextToken()));
}
tok.readNextToken();
if(!tok.isToken(","))
Misc::throwStdErr("MeasureEnvironment::MeasureEnvironment: Format error in input file %s",fileName);
/* Read the point tag: */
tok.readNextToken();
if(tok.isCaseToken(tag))
{
/* Store the point: */
result.push_back(p);
}
tok.readNextToken();
if(!tok.isToken("\n"))
Misc::throwStdErr("MeasureEnvironment::MeasureEnvironment: Format error in input file %s",fileName);
++line;
}
/* Cull duplicate points from the point list: */
size_t numDupes=cullDuplicates(result,Scalar(0.05));
if(numDupes>0)
std::cout<<"ScreenCalibrator::readTotalstationSurveyFile: "<<numDupes<<" duplicate points culled from input file"<<std::endl;
return result;
}
void ColoredPolygon::UpdatePoints(const PointList &points)
{
_dots.resize(points.size());
for (uint i = 0; i < points.size(); ++i)
{
_dots[i].pos = points[i];
}
CalcWidthAndHeight();
GenerateTriangles();
_dotUnderCursor.clear();
_selectedDots.clear();
InitCorners();
}
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