線(LineString)の特徴から図形(線, 円, ポリゴン, 三角形, 四角形)を検出するプログラム

Main.cpp

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

struct Arc
{
	Vec2 center;

	double r;

	double startAngle;

	double angle;

	void draw(double innerThickness = 1.0, double outerThickness = 0.0, const ColorF& color = Palette::White)
	{
		Circle(center, r).drawArc(Math::HalfPi - startAngle, -angle, innerThickness, outerThickness, color);
	}
};

// 最小2乗法(LSM)のサンプル長さ
constexpr double LSMSampleLength = 100;

// 小さな曲率を直線に補正するときの最大値
constexpr double LineMaxCurvature = 0.003;

// 特徴検出時の弧の最小割合
double ArcMinPer = 0.1;

double lineLength = 0;

LineString line;

LineString posRadLine;
void GeneratePosRadLine()
{
	lineLength = 0;

	posRadLine.clear();
	if (line.size() <= 1)
	{
		return;
	}

	Vec2 prevPos = line.front();
	double prevDeg = 0;
	double degTotal = 0;
	for (auto idx : Iota(1, line.size()))
	{
		Vec2 newPos = line[idx];

		Vec2 diff = newPos - prevPos;
		double distance = newPos.distanceFrom(prevPos);
		double deg = Math::Atan2(-diff.y, diff.x);
		double degDiff = deg - prevDeg;

		// -2π~2π
		if (degDiff > Math::Pi)
		{
			// π~2π -> -π~0
			degDiff = degDiff - Math::TwoPi;
		}
		else if (degDiff < -Math::Pi)
		{
			// -2π~-π -> 0~π
			degDiff = Math::TwoPi + degDiff;
		}
		// -π~π

		degTotal += degDiff;
		lineLength += distance;

		posRadLine.emplace_back(Vec2{ lineLength, degTotal });

		prevPos = newPos;
		prevDeg = deg;
	}
}

LineString simplifiedLine;
void GenerateSimplifiedLine(double maxDistance)
{
	simplifiedLine = posRadLine.simplified(maxDistance);
}

Array<double> slopes;
// 最小2乗法で傾きを検出
double DetectSlope(size_t startIdx, size_t count)
{
	if (count < 2)
	{
		return QNaN<double>;
	}

	// Node::pos -> X
	// Node::degTotal -> Y

	auto begin = posRadLine.cbegin() + startIdx;
	auto last = begin + (count - 1);
	auto end = begin + count;

	// 合計(XY)
	Vec2 total{ 0, 0 };
	for (auto idx : Iota(count))
	{
		total += posRadLine[startIdx + idx];
	}

	// 平均(XY)
	Vec2 average = total / count;

	double varianceX = 0; // 分散(X)
	double covariance = 0; // 共分散
	for (auto idx : Iota(count))
	{
		// 偏差(XY)
		Vec2 deviation = posRadLine[startIdx + idx] - average;

		varianceX += deviation.x * deviation.x;
		covariance += deviation.x * deviation.y;
	}
	varianceX /= count;
	covariance /= count;

	// 傾きから角度を計算
	return std::atan(covariance / varianceX);
}

void UpdateSlopes()
{
	slopes.resize(posRadLine.size());
	for (auto [idx, slope] : IndexedRef(slopes))
	{
		size_t count = 0;
		for (auto nodeIdx : Iota(idx, posRadLine.size()))
		{
			count++;
			if (posRadLine[nodeIdx].x - posRadLine[idx].x >= LSMSampleLength)
			{
				break;
			}
		}
		slope = DetectSlope(idx, count);
	}
}

template<std::ranges::range SrcType>
void GenerateGraphLines(const SrcType& src, Array<LineString>& dst)
{
	Vec2 prevposRad;
	double centerY = 0;
	for (const Vec2& posRad : src)
	{
		Vec2 drawPos = posRad + Vec2{ 0, -centerY };

		dst.back().push_back(drawPos);

		if (drawPos.y > Math::Pi)
		{
			do
			{
				centerY += Math::TwoPi;
			} while (posRad.y > centerY + Math::Pi);
		}
		else if (drawPos.y < -Math::Pi)
		{
			do
			{
				centerY -= Math::TwoPi;
			} while (posRad.y < centerY - Math::Pi);
		}
		else
		{
			prevposRad = posRad;
			continue;
		}

		Vec2 prevDrawPos = prevposRad + Vec2{ 0, -centerY };
		drawPos = posRad + Vec2{ 0, -centerY };

		dst.emplace_back(LineString(
			{
				prevDrawPos,
				drawPos
			}
		));

		prevDrawPos = drawPos;
	}
}

Array<std::variant<Line, Arc>> features;
void GenerateShapes()
{
	ClearPrint();
	features.clear();

	if (simplifiedLine.size() <= 1)
	{
		return;
	}

	size_t posRadStartIdx = 0;
	for (auto simplifiedEndIdx : Iota(1, simplifiedLine.size()))
	{
		size_t posRadEndIdx = 0;
		for (auto posRadIdx : Iota(posRadStartIdx + 1, posRadLine.size()))
		{
			posRadEndIdx = posRadIdx;
			if (posRadLine[posRadIdx].x == simplifiedLine[simplifiedEndIdx].x)
			{
				break;
			}
		}

		// 位置,角度の差分

		Vec2 posRadDiff = posRadLine[posRadEndIdx] - posRadLine[posRadStartIdx];

		// 描画位置

		Vec2 drawStartPos = line[posRadStartIdx];
		Vec2 drawEndPos = line[posRadEndIdx];
		Vec2 drawDiff = drawEndPos - drawStartPos;

		if (std::abs(posRadDiff.y / posRadDiff.x) < LineMaxCurvature)
		{
			// 直線
			features.emplace_back(Line(drawStartPos, drawEndPos));
		}
		else
		{
			// 円弧

			// 最小長さチェック
			if (posRadDiff.x >= lineLength * ArcMinPer)
			{
				// 半径([+]->左,[-]->右)
				double r = posRadDiff.x / posRadDiff.y;

				// 円弧角度([+]->左,[-]->右)
				double angle = posRadDiff.y;

				// 半径 - 矢高
				double triangleHeight = r * Sin((Math::Pi - angle) / 2);
				Vec2 triangleBottomCenter = (drawStartPos + drawDiff / 2);

				// 円弧の描画位置

				Vec2 arcStartPos = drawStartPos;
				Vec2 arcCenterPos =
					triangleBottomCenter
					+ Vec2{ drawDiff.y, -drawDiff.x } *triangleHeight / drawDiff.length(); // 三角形の底辺に垂直, 長さ: triangleHeight
				Vec2 arcEndPos = drawEndPos;

				double arcStartAngle = Math::Atan2(-drawDiff.x, -drawDiff.y)
					+ (r < 0 ? Math::Pi : 0)
					- angle / 2;

				features.emplace_back(Arc
					{
						.center = arcCenterPos,
						.r = Abs(r),
						.startAngle = arcStartAngle,
						.angle = angle
					});
			}
		}

		posRadStartIdx = posRadEndIdx;
	}
}

Optional<Line> DetectLine(Array<std::variant<Line, Arc>>& features)
{
	if (features.size() != 1 ||
		not std::holds_alternative<Line>(features[0]))
	{
		return none;
	}

	return std::get<Line>(features[0]);
}

Optional<Circle> DetectCircle(Array<std::variant<Line, Arc>>& features)
{
	if (features.size() != 1 ||
		not std::holds_alternative<Arc>(features[0]))
	{
		return none;
	}

	auto& arc = std::get<Arc>(features[0]);

	if (std::abs(arc.angle) <= 270_deg)
	{
		return none;
	}

	return Circle{ arc.center, arc.r };
}

Array<Vec2> DetectVertices(Array<std::variant<Line, Arc>>& features)
{
	// 2つ以上辺が無いと多角形は作れないのでチェック
	if (features.size() < 3)
	{
		return { };
	}

	// 線分のリストを作成

	Array<Line> lineList(Arg::reserve = features.size());

	for (auto& feature : features)
	{
		// 直線であるかチェック
		if (not std::holds_alternative<Line>(feature))
		{
			return { };
		}

		// すべての線分の長さを2倍にする
		Line line = std::get<Line>(feature);
		Vec2 diff = line.end - line.begin;

		line.begin -= diff / 2;
		line.end += diff / 2;

		lineList.push_back(line);
	}

	// 頂点のリストを作成

	Array<Vec2> vertexList(Arg::reserve = features.size());

	for (auto [idx, line] : Indexed(lineList))
	{
		size_t prevIdx = idx == 0 ? lineList.size() - 1 : idx - 1;
		Line& prevLine = lineList[prevIdx];

		auto intersection = prevLine.intersectsAt(line);

		if (not intersection.has_value())
		{
			return { };
		}

		vertexList.push_back(*intersection);
	}

	return vertexList;
}

Optional<RectF> DetectRectF(Array<Vec2>& vertices, double maxAngle = 20_deg)
{
	if (vertices.size() != 4)
	{
		return none;
	}

	// 4つの辺の向きを0~3のグループに分類し、グループの番号が1ずつ増減しているか確認することで四角形を判別する

	// 線分のグループを検出
	// なし: -1, →: 0, ↑: 1, ←: 2, ↓: 3
	const auto detectLineGroup = [maxAngle](Vec2 start, Vec2 end) -> int8
	{
		Vec2 diff = end - start;
		double angle = Math::Atan2(-diff.y, diff.x);

		if (-maxAngle <= angle && angle <= maxAngle)
		{
			return 0;
		}

		if (Math::HalfPi - maxAngle <= angle && angle <= Math::HalfPi + maxAngle)
		{
			return 1;
		}

		if (Math::Pi - maxAngle <= angle || angle <= -Math::Pi + maxAngle)
		{
			return 2;
		}

		if (-Math::HalfPi - maxAngle <= angle && angle <= -Math::HalfPi + maxAngle)
		{
			return 3;
		}

		return -1;
	};

	const std::array<int8, 4> lineGroups = {
		detectLineGroup(vertices[0], vertices[1]),
		detectLineGroup(vertices[1], vertices[2]),
		detectLineGroup(vertices[2], vertices[3]),
		detectLineGroup(vertices[3], vertices[0]),
	};

	// -1が含まれていないか確認
	if (std::find(lineGroups.begin(), lineGroups.end(), -1) != lineGroups.end())
	{
		return none;
	}

	// 1ずつ増加/減少しているか確認
	int8 direction = (4 + lineGroups[1] - lineGroups[0]) % 4;
	if (direction == 3 || direction == 1)
	{
		for (int8 idx : Iota(1, lineGroups.size()))
		{
			int8 nextGroup = (lineGroups[idx] + direction) % 4;
			if (lineGroups[(idx + 1) % 4] != nextGroup)
			{
				return none;
			}
		}
	}
	else
	{
		return none;
	}

	// 四角形を作成
	Array<double> hLinePos(Arg::reserve = 2);
	Array<double> vLinePos(Arg::reserve = 2);
	for (size_t i : Iota(lineGroups.size()))
	{
		size_t startIdx = i;
		size_t endIdx = (startIdx + 1) % lineGroups.size();

		switch (lineGroups[i])
		{
			// 水平
		case 0:
		case 2:
			hLinePos.push_back((vertices[startIdx].y + vertices[endIdx].y) / 2);
			break;
			// 垂直
		case 1:
		case 3:
			vLinePos.push_back((vertices[startIdx].x + vertices[endIdx].x) / 2);
			break;
		}
	}
	hLinePos.sort();
	vLinePos.sort();

	return RectF{
		vLinePos[0],
		hLinePos[0],
		vLinePos[1] - vLinePos[0],
		hLinePos[1] - hLinePos[0]
	};
}

Optional<std::variant<Line, Circle, MultiPolygon, Triangle, RectF>> detectedShape;
void DetectShape()
{
	if (auto s = DetectLine(features))
	{
		detectedShape = *s;
		return;
	}

	if (auto s = DetectCircle(features))
	{
		detectedShape = *s;
		return;
	}

	if (auto vertices = DetectVertices(features))
	{
		if (vertices.size() == 3)
		{
			detectedShape = Triangle{ vertices[0], vertices[1], vertices[2] };
		}
		else if (auto rect = DetectRectF(vertices))
		{
			detectedShape = *rect;
		}
		else
		{
			detectedShape = MultiPolygon(Polygon::Correct(vertices));
		}
		return;
	}

	detectedShape = none;
}

void Main()
{
	Scene::SetBackground(ColorF(0.9));
	Window::Resize({ 1280, 720 });

	Rect topbarRect;
	Rect editorRect;
	Rect graphRect;

	bool drawing = false;

	double maxDistanceVal = 33;

	Font font(20);

	RasterizerState clipState = RasterizerState::Default2D;
	clipState.scissorEnable = true;

	while (System::Update())
	{
		ScopedRenderStates2D _(clipState);
		Graphics2D::SetScissorRect(Scene::Rect());

		topbarRect = { 0, 0, Scene::Width(), 54 };
		editorRect = { 0, 54, Scene::Width(), Scene::Height() - 54 - 200 };
		graphRect = { 0, Scene::Height() - 200, Scene::Width(), 200 };

		// 線処理

		if (drawing)
		{
			if (not MouseL.pressed())
			{
				GeneratePosRadLine();
				GenerateSimplifiedLine(ToRadians(maxDistanceVal));
				GenerateShapes();
				DetectShape();
				drawing = false;
			}
		}
		else
		{
			if (editorRect.leftClicked())
			{
				line.clear();
				line.push_back(Cursor::PosF());
				drawing = true;
			}
		}

		if (drawing)
		{
			auto newPos = Cursor::PosF();
			auto distance = newPos.distanceFrom(line.back());

			if (distance >= 3)
			{
				line.push_back(newPos);
			}
		}

		// 上部バー描画

		topbarRect.draw(Palette::White);

		// ノイズ追加

		if (SimpleGUI::Button(U"Add Noise", { 10, 10 }, unspecified, not line.empty() && not drawing))
		{
			Vec2 prevPos = line.front();
			for (auto& pos : line)
			{
				double distance = pos.distanceFrom(prevPos);

				if (distance != 0)
				{
					pos += RandomVec2() * 10 / distance;
				}

				prevPos = pos;
			}
			GeneratePosRadLine();
			GenerateSimplifiedLine(ToRadians(maxDistanceVal));
			GenerateShapes();
			DetectShape();
		}

		// 単純化しきい値変更

		if (SimpleGUI::Slider(U"Max Distance", maxDistanceVal, 10, 180, { 160, 10 }, 140, 180, not posRadLine.empty() && not drawing))
		{
			GenerateSimplifiedLine(ToRadians(maxDistanceVal));
			GenerateShapes();
			DetectShape();
		}
		font(U"{:.1f}°"_fmt(maxDistanceVal)).draw(Arg::rightCenter = Vec2{ 540, topbarRect.h / 2 }, Palette::Black);

		if (SimpleGUI::Slider(U"Arc Min%", ArcMinPer, 0.0, 0.5, { 560, 10 }, 100, 180, not posRadLine.empty() && not drawing))
		{
			GenerateSimplifiedLine(ToRadians(maxDistanceVal));
			GenerateShapes();
			DetectShape();
		}
		font(U"{:.2f}%"_fmt(ArcMinPer)).draw(Arg::rightCenter = Vec2{ 900, topbarRect.h / 2 }, Palette::Black);

		// 線,特徴,検出図形描画
		Graphics2D::SetScissorRect(editorRect);
		{
			// 線
			line.draw(4, Palette::Grey);

			// 特徴
			for (auto shape : features)
			{
				if (std::holds_alternative<Line>(shape))
				{
					std::get<Line>(shape).draw(3, Palette::Blue);
				}
				else
				{
					std::get<Arc>(shape).draw(1.5, 1.5, Palette::Blueviolet);
				}
			}

			// 検出図形
			if (detectedShape)
			{
				if (std::holds_alternative<Line>(*detectedShape))
				{
					std::get<Line>(*detectedShape).draw(4, Palette::Red);
				}
				else if (std::holds_alternative<Circle>(*detectedShape))
				{
					std::get<Circle>(*detectedShape).drawFrame(4, Palette::Red);
				}
				else if (std::holds_alternative<MultiPolygon>(*detectedShape))
				{
					std::get<MultiPolygon>(*detectedShape).drawFrame(4, Palette::Red);
				}
				else if (std::holds_alternative<Triangle>(*detectedShape))
				{
					std::get<Triangle>(*detectedShape).drawFrame(4, Palette::Red);
				}
				else if (std::holds_alternative<RectF>(*detectedShape))
				{
					std::get<RectF>(*detectedShape).drawFrame(4, Palette::Red);
				}
			}
		}
		Graphics2D::SetScissorRect(Scene::Rect());

		// グラフ描画

		graphRect.drawFrame(1, 0, Palette::Grey);
		Line(graphRect.leftCenter(), graphRect.rightCenter())
			.draw(2, Palette::Grey);

		font(U" π").draw(Arg::bottomLeft = graphRect.tl(), Palette::Grey);
		font(U" 0").draw(Arg::bottomLeft = graphRect.leftCenter(), Palette::Grey);
		font(U"-π").draw(Arg::bottomLeft = graphRect.bl(), Palette::Grey);

		Vec2 pivot{
			graphRect.x,
			graphRect.y + graphRect.h / 2
		};

		Vec2 scale{
			lineLength <= graphRect.w ? 1 : (graphRect.w / lineLength),
			-graphRect.h / Math::TwoPi
		};

		Array<LineString> graphLines0 = { LineString() };
		if (not posRadLine.isEmpty())
		{
			LineString posRadDiff(Arg::reserve = posRadLine.size());
			double prevRad = posRadLine[0].y;
			for (auto idx : Iota(1, posRadLine.size()))
			{
				auto [pos, rad] = posRadLine[idx];
				double diff = rad - prevRad;
				posRadDiff.push_back({ pos, diff });

				prevRad = rad;
			}
			GenerateGraphLines(posRadDiff, graphLines0);
			for (auto& str : graphLines0)
			{
				for (auto& v : str)
				{
					v = pivot + v * Vec2{ scale.x, -graphRect.h / Math::Pi };
				}
			}
		}

		Array<LineString> graphLines1 = { LineString() };
		GenerateGraphLines(posRadLine, graphLines1);
		for (auto& str : graphLines1)
		{
			for (auto& v : str)
			{
				v = pivot + v * scale;
			}
		}

		Array<LineString> graphLines2 = { LineString() };
		GenerateGraphLines(simplifiedLine.densified(Math::TwoPi), graphLines2);
		for (auto& str : graphLines2)
		{
			for (auto& v : str)
			{
				v = pivot + v * scale;
			}
		}

		Graphics2D::SetScissorRect(graphRect);
		{
			for (auto& graphLine : graphLines0)
			{
				graphLine.draw(3, Palette::Grey);
			}

			for (auto& graphLine : graphLines1)
			{
				graphLine.draw(3, Palette::Red);
			}

			for (auto& graphLine : graphLines2)
			{
				graphLine.draw(3, Palette::Blue);
			}
		}
		Graphics2D::SetScissorRect(Scene::Rect());

		// 特徴テキスト描画
		for (auto [idx, shape] : Indexed(features))
		{
			if (std::holds_alternative<Line>(shape))
			{
				auto& line = std::get<Line>(shape);

				font(U"[Line] ({:.1f}, {:.1f}) -> ({:.1f}, {:.1f})"_fmt(line.begin.x, line.begin.y, line.end.x, line.end.y))
					.draw(editorRect.tl() + Vec2{ 0, font.height() * idx }, Palette::Blue);
			}
			else
			{
				auto& arc = std::get<Arc>(shape);

				font(U"[Arc] Center:({:.1f}, {:.1f}), R:{:.1f}, Angle:{:.1f}"_fmt(arc.center.x, arc.center.y, arc.r, arc.angle))
					.draw(editorRect.tl() + Vec2{ 0, font.height() * idx }, Palette::Blueviolet);
			}
		}

		// 検出図形テキスト描画
		if (detectedShape)
		{
			if (std::holds_alternative<Line>(*detectedShape))
			{
				font(U"Line").draw(Arg::topRight = editorRect.tr(), Palette::Red);
			}
			else if (std::holds_alternative<Circle>(*detectedShape))
			{
				font(U"Circle").draw(Arg::topRight = editorRect.tr(), Palette::Red);
			}
			else if (std::holds_alternative<MultiPolygon>(*detectedShape))
			{
				font(U"Polygon").draw(Arg::topRight = editorRect.tr(), Palette::Red);
			}
			else if (std::holds_alternative<Triangle>(*detectedShape))
			{
				font(U"Triangle").draw(Arg::topRight = editorRect.tr(), Palette::Red);
			}
			else if (std::holds_alternative<RectF>(*detectedShape))
			{
				font(U"Rect").draw(Arg::topRight = editorRect.tr(), Palette::Red);
			}
		}
	}
}