std::list<Point> Simulation::BresenhamLine(const double x1, const double y1, const double z1, const double x2, const double y2, const double z2)
{
	std::list<Point> lst;
	double d, dx, dy, ai, bi, xi, yi;
	double x = x1, y = y1, dd = 0.2;
	double dz;
	if (x1 < x2)
	{
		xi = dd;
		dx = x2 - x1;
	}
	else
	{
		xi = -dd;
		dx = x1 - x2;
	}
	if (y1 < y2)
	{
		yi = dd;
		dy = y2 - y1;
	}
	else
	{
		yi = -dd;
		dy = y1 - y2;
	}
	dz = (z2 - z1)/std::fmax(dy, dx);
	lst.push_back(Point(x, y, z1));
	int iterator = 0;
	if (dx > dy)
	{
		ai = (dy - dx) * 2;
		bi = dy * 2;
		d = bi - dx;
		while (!EpsilonEquals(x,x2,dd))
		{
			iterator++;
			if (d >= 0)
			{
				x += xi;
				y += yi;
				d += ai;
			}
			else
			{
				d += bi;
				x += xi;
			}
			lst.push_back(Point(x, y, z1 + iterator * dz));
		}
	}
	else
	{
		ai = (dx - dy) * 2;
		bi = dx * 2;
		d = bi - dy;
		while (!EpsilonEquals(y, y2, dd))
		{
			iterator++;
			if (d >= 0)
			{
				x += xi;
				y += yi;
				d += ai;
			}
			else
			{
				d += bi;
				y += yi;
			}
			lst.push_back(Point(x, y, z1 + iterator * dz));
		}
	}
	return lst;
}