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;
}
Example #2
0
/**
 * 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();
}
Example #3
0
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;
}
Example #4
0
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;
}
Example #5
0
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;
}
Example #7
0
    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));
        }
    }
Example #8
0
 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();
}
Example #10
0
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;
}
Example #11
0
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;
}
Example #12
0
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;
}
Example #13
0
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);
	}
}
Example #16
0
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;
    }
}
Example #19
0
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));
    }
}
Example #21
0
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);
}
Example #22
0
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);
}
Example #23
0
//-----------------------------------------------------------------------------
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);
}
Example #24
0
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));
        }
    }
}
Example #28
0
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();
}
Example #30
0
 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