opt.cpp

# include <Siv3D.hpp> // OpenSiv3D v0.6.6

double safeDistance = 5.0;
double minDelta = 0.1;

struct Node;

struct Shape
{
	bool isSelected = false;

	Polygon polygon;
	Polygon semiSafePolygon;
	Polygon safePolygon;

	Array<std::shared_ptr<Node>> ownNodes;
	std::shared_ptr<Node> piercePoint;
};

struct Node
{
	bool isReflect = false;
	Vec2 position;
	Array<std::shared_ptr<Node>> connectedNodes;
	std::shared_ptr<Shape> shape;
	std::shared_ptr<Node> nodeTo;
	std::shared_ptr<Node> nodeFr;

	double cost1;
	double cost2;
	std::shared_ptr<Node> pathFr1;
	std::shared_ptr<Node> pathFr2;
};

struct Reflect
{
	std::shared_ptr<Node> nodeFr;
	std::shared_ptr<Node> nodeRe;
	std::shared_ptr<Node> nodeTo;
};

Array<std::shared_ptr<Shape>> shapes;
Array<std::shared_ptr<Node>> nodes;
Array<std::shared_ptr<Node>> route;
Array<Reflect> reflects;
Array<int> orders;

void makeCostMap(std::shared_ptr<Node> dst, std::shared_ptr<Shape> via)
{
	for (const auto& n : nodes)
	{
		n->cost1 = 0.0;
		n->cost2 = 0.0;
		n->pathFr1 = nullptr;
		n->pathFr2 = nullptr;
	}
	Array<std::shared_ptr<Node>> queue;
	queue.push_back(dst);
	dst->pathFr1 = dst;
	while (!queue.empty())
	{
		auto n = queue.front();
		for (const auto& other : n->connectedNodes)
		{
			bool pushFlag = false;
			if (n->pathFr1)
			{
					if (!other->pathFr1 || other->cost1 > n->cost1 + n->position.distanceFrom(other->position))
					{
						other->cost1 = n->cost1 + n->position.distanceFrom(other->position);
						other->pathFr1 = n;
						pushFlag = true;
					}
				if (!other->isReflect)
				{
				if (n->shape == via && (!other->pathFr2 || other->cost2 > n->cost1 + n->position.distanceFrom(other->position)))
				{
					other->cost2 = n->cost1 + n->position.distanceFrom(other->position);
					other->pathFr2 = n;
					pushFlag = true;
				}
				}
			}
			if (n->pathFr2)
			{
					if (!other->pathFr2 || other->cost2 > n->cost2 + n->position.distanceFrom(other->position))
					{
						other->cost2 = n->cost2 + n->position.distanceFrom(other->position);
						other->pathFr2 = n;
						pushFlag = true;
					}
				
			}

			if (pushFlag)
			{
				queue.push_back(other);
			}
		}
		queue.pop_front();
	}
}

void makeCostMapNoVia(std::shared_ptr<Node> dst)
{
	for (const auto& n : nodes)
	{
		n->cost1 = 0.0;
		n->pathFr1 = nullptr;
	}
	Array<std::shared_ptr<Node>> queue;
	queue.push_back(dst);
	dst->pathFr1 = dst;
	while (!queue.empty())
	{
		auto n = queue.front();
		for (const auto& other : n->connectedNodes)
		{
			if (!other->pathFr1 || other->cost1 > n->cost1 + n->position.distanceFrom(other->position))
			{
				other->cost1 = n->cost1 + n->position.distanceFrom(other->position);
				other->pathFr1 = n;
				queue.push_back(other);
			}
		}
		queue.pop_front();
	}
}

void optimizePiercePoint(int n = 2)
{
	for (const auto& s : shapes)
		s->piercePoint = s->ownNodes.front();

	for (int j = 0; j < n; ++j)
	{
		for (int i = 0; i < orders.size(); ++i)
		{
			std::shared_ptr<Node> fr = i == 0 ? nodes.front() : shapes[orders[i - 1]]->piercePoint;
			std::shared_ptr<Node> to = i == orders.size() - 1 ? nodes.front() : shapes[orders[i + 1]]->piercePoint;

			makeCostMap(fr, shapes[orders[i]]);

			bool isArrivedPoint = false;
			for (auto n = to; n != fr; )
			{
				if (n->shape == shapes[orders[i]])
				{
					isArrivedPoint = true;
					shapes[orders[i]]->piercePoint = n;
				}

				if (isArrivedPoint)
				{
					n = n->pathFr1;
					route.emplace_back(n);
				}
				else
				{
					n = n->pathFr2;
				}
			}
		}
	}
}

double getCurrentLength()
{
	double cost = 0.0;
	for (int i = 0; i < orders.size() + 1; ++i)
	{
		std::shared_ptr<Node> fr = (i == 0) ? nodes.front() : shapes[orders[i - 1]]->piercePoint;
		std::shared_ptr<Node> to = (i == orders.size()) ? nodes.front() : shapes[orders[i]]->piercePoint;

		makeCostMapNoVia(fr);

		for (auto n = to; n != fr; )
		{
			cost += n->position.distanceFrom(n->pathFr1->position);
			n = n->pathFr1;
		}
	}

	return cost;
}

std::unordered_map<int, int> costMap;

int getKey(Array<int> _orders)
{
	int val = 0;
	int mul = 1;
	for (int i = 0; i < orders.size(); ++i)
	{
		val += mul * _orders[i];
		mul *= orders.size();
	}

	return val;
}

int getCost(Array<int> _orders)
{
	int key = getKey(_orders);

	if (costMap.find(key) == costMap.end())
	{

		orders = _orders;

		optimizePiercePoint();
		double cost = getCurrentLength();

		costMap[key] = cost;
		return cost;
	}
	else
	{
		return costMap[key];
	}
}

void optimizeOrders()
{
	costMap.clear();

	const int maxK = 4;
	int k = 2;
	double bestCost = getCost(orders);
	Array<int> bestOrders = orders;

	while (k <= maxK)
	{
		Array<int> list(orders.size());
		const int size = list.size();
		for (int i = 0; i < size; ++i)
			list[size - i - 1] = i;

		bool superFlag = false;
		while (true)
		{
			bool flag = true;
			for (int i = 0; i < size; ++i)
				for (int j = i + 1; j < size; ++j)
					if (list[i] == list[j]) { flag = false; break; }

			if (flag)
			{
				int num = 0;
				for (int i = 0; i < size; ++i)
					if (bestOrders[i] != list[i])
						++num;

				if (num > 0 && num <= k)
				{
					for (int i = 0; i < size; ++i)
						orders[i] = list[i];

					double cost = getCost(orders);
					if (cost < bestCost)
					{
						//Console << orders;

						//Console << cost;
						bestOrders = orders;
						bestCost = cost;
						k = 2;
						break;
					}
				}
			}

			++list[0];
			for (int i = 0; i < size - 1; ++i)
			{
				if (list[i] == size)
				{
					list[i] = 0;
					++list[i + 1];
				}
			}
			if (list.back() == size)
			{
				++k;
				break;
			}
		}
	}

	optimizePiercePoint(20);

	//Console << getCurrentLength();
}

void build()
{
	// clear
	nodes.clear();
	route.clear();
	reflects.clear();
	orders.resize(shapes.size());
	for (int i = 0; i < orders.size(); ++i)
		orders[i] = i;

	nodes.reserve(100000);

	// rebuild
	for (const auto& s : shapes)
	{
		s->safePolygon = s->polygon.calculateRoundBuffer(safeDistance);
		s->semiSafePolygon = s->polygon.calculateRoundBuffer(safeDistance - minDelta);
		s->safePolygon = s->polygon.calculateBuffer(safeDistance);
		s->semiSafePolygon = s->polygon.calculateBuffer(safeDistance - minDelta);
		s->ownNodes.clear();
		s->piercePoint = nullptr;
	}

	for (const auto& s1 : shapes)
		for (const auto& s2 : shapes)
			if (s1 != s2 && s1->safePolygon.intersects(s2->safePolygon)) return;

	// set origin
	{
		const auto n = nodes.emplace_back(std::make_shared<Node>());
		n->position = Vec2::Zero();
	}

	// making nodes
	for (const auto& s : shapes)
	{
		for (const auto& p : s->safePolygon.outer())
		{
			const auto n = nodes.emplace_back(std::make_shared<Node>());

			s->ownNodes.emplace_back(n);
			n->shape = s;
			n->position = p;
		}

		s->piercePoint = s->ownNodes[0];

		const auto size = s->ownNodes.size();
		for (int i = 0; i < size; ++i)
		{
			s->ownNodes[i]->nodeFr = s->ownNodes[(i - 1 + size) % size];
			s->ownNodes[i]->nodeTo = s->ownNodes[(i + 1) % size];
		}
	}

	// Connect
	for (const auto& n1 : nodes)
	{
		for (const auto& n2 : nodes)
		{
			if (n1 != n2)
			{
				const auto l = Line(n1->position, n2->position);

				bool flag = true;
				for (const auto& s : shapes)
				{
					if (s->semiSafePolygon.intersects(l))
					{
						flag = false;
						break;
					}
				}

				if (flag)
				{
					n1->connectedNodes.emplace_back(n2);
				}
			}
		}
	}

	// Reflect
	if (true)
	{
		const int nMax = nodes.size();
		for (int i = 0; i < nMax; ++i)
		{
			for (int j = i; j < nMax; ++j)
			{
				const auto& n1 = nodes[i];
				const auto& n2 = nodes[j];

				for (const auto& s : shapes)
				{
					const auto outer = s->safePolygon.outer();
					for (int i = 0; i < outer.size(); ++i)
					{
						const Line line(outer[i], outer[(i + 1) % outer.size()]);

						const double v = (n2->position - line.begin).dot(line.vector().normalized());
						const auto p = line.begin + line.vector().setLength(v);

						Line crossLine(n1->position, n2->position + (p - n2->position) * 2);

						if (auto result = crossLine.intersectsAtPrecise(line))
						{
							const Line l1(n1->position, result.value());
							const Line l2(n2->position, result.value());

							if (result.value().distanceFrom(line.begin) <= minDelta * 2.0) continue;
							if (result.value().distanceFrom(line.end) <= minDelta * 2.0) continue;

							bool flag = true;
							for (const auto& s : shapes)
							{
								if (s->semiSafePolygon.intersects(l1) || s->semiSafePolygon.intersects(l2))
								{
									flag = false;
									break;
								}
							}

							if (flag)
							{
								const auto n = nodes.emplace_back(std::make_shared<Node>());
								n->position = result.value();
								n->shape = s;
								n->isReflect = true;
								s->ownNodes.emplace_back(n);

								n1->connectedNodes.emplace_back(n);
								n2->connectedNodes.emplace_back(n);
								n->connectedNodes.emplace_back(n1);
								n->connectedNodes.emplace_back(n2);

								auto& r = reflects.emplace_back();
								r.nodeFr = n1;
								r.nodeTo = n2;
								r.nodeRe = n;
							}
						}
					}
				}
			}
		}
	}

	// make route
	//optimizePiercePoint();

	optimizeOrders();

	route.clear();
	for (int i = 0; i < orders.size() + 1; ++i)
	{
		std::shared_ptr<Node> fr = (i == 0) ? nodes.front() : shapes[orders[i - 1]]->piercePoint;
		std::shared_ptr<Node> to = (i == orders.size()) ? nodes.front() : shapes[orders[i]]->piercePoint;

		makeCostMapNoVia(fr);

		Array<std::shared_ptr<Node>> temp;

		for (auto n = to; n != fr; )
		{
			//temp.emplace_back(n);
			n = n->pathFr1;
			temp.emplace_back(n);
		}

		for (auto t : temp.reversed())
			route.emplace_back(t);
	}
	route.emplace_back(nodes.front());
	ClearPrint();

	Print << U"経路長は" << getCost(orders);
}

void Main()
{
	Window::SetStyle(WindowStyle::Sizable);

	{
		const auto s = shapes.emplace_back(std::make_shared<Shape>());
		s->polygon = Shape2D::Star(100).asPolygon().movedBy(200, 0);
	}
	{
		const auto s = shapes.emplace_back(std::make_shared<Shape>());
		s->polygon = Shape2D::Star(100).asPolygon().movedBy(200, 200);
	}
	{
		const auto s = shapes.emplace_back(std::make_shared<Shape>());
		s->polygon = Shape2D::Star(100).asPolygon().movedBy(0, 150);
	}
	{
		const auto s = shapes.emplace_back(std::make_shared<Shape>());
		s->polygon = Shape2D::Hexagon(100).asPolygon().movedBy(400, 100);
	}
	{
		const auto s = shapes.emplace_back(std::make_shared<Shape>());
		s->polygon = Rect(-200, -400, 200, 200).asPolygon().movedBy(0, 150);
	}

	build();

	Camera2D camera;

	const Font font(64);

	while (System::Update())
	{
		camera.update();
		const  auto t = camera.createTransformer();

		for (const auto& s : shapes)
		{
			s->safePolygon.draw(ColorF(Palette::Green, 0.5));
			s->polygon.drawFrame(2, Palette::Skyblue);

			if (s->semiSafePolygon.leftClicked())
				s->isSelected = true;

			if (!MouseL.pressed() && s->isSelected)
			{
				s->isSelected = false;
				build();
			}

			if (s->isSelected)
				s->polygon.moveBy(Cursor::DeltaF());
		}
		for (const auto& ref : reflects)
		{
			Line(ref.nodeFr->position, ref.nodeRe->position).draw(2, ColorF(Palette::Yellow, 0.5));
			Line(ref.nodeTo->position, ref.nodeRe->position).draw(2, ColorF(Palette::Yellow, 0.5));
			Circle(ref.nodeRe->position, 2).draw(Palette::Yellow);
		}
		for (const auto& n : nodes)
		{
			for (const auto& cn : n->connectedNodes)
				Line(n->position, cn->position).draw(1, ColorF(Palette::White, 0.25));

			/*if (n->pathFr1)
				Circle(n->position, 8).draw(Palette::Blue);
			if (n->pathFr2)
				Circle(n->position, 5).draw(Palette::Red);

			if (n->pathFr1)
				Line(n->position, n->pathFr1->position)
				.stretched(-5)
				.movedBy(Line(n->position, n->pathFr2->position).vector().setLength(1).rotated(Math::HalfPi))
				.drawArrow(2, SizeF(5, 5), ColorF(Palette::Blue, 0.75));
			if (n->pathFr2)
				Line(n->position, n->pathFr2->position)
				.stretched(-5)
				.movedBy(Line(n->position, n->pathFr2->position).vector().setLength(-1).rotated(Math::HalfPi))
				.drawArrow(2, SizeF(5, 5), ColorF(Palette::Red, 0.75));*/
		}

		for (const auto& s : shapes)
		{
			if (s->piercePoint)
				Circle(s->piercePoint->position, 4).draw(Palette::Red);

			for (const auto& n : s->ownNodes)
			{
				Circle(n->position, 2).draw(Palette::Blue);


			}
		}

		for (int i = 0; i < int(route.size()) - 1; ++i)
		{
			Line(route[i]->position, route[i + 1]->position).stretched(-4).drawArrow(4, SizeF(8, 8), Palette::Red);
		}

		for (int i = 0; i < orders.size(); ++i)
			font(i).drawAt(shapes[orders[i]]->polygon.centroid());

		Circle(Vec2(0, 0), 5).draw(Palette::Blue);
	}
}