Last active
February 1, 2022 06:13
-
-
Save sthairno/12dcdf6257847551eba7b2e6b51b7b3d to your computer and use it in GitHub Desktop.
線(LineString)の特徴から図形(線, 円, ポリゴン, 三角形, 四角形)を検出するプログラム
This file contains bidirectional Unicode text that may be interpreted or compiled differently than what appears below. To review, open the file in an editor that reveals hidden Unicode characters.
Learn more about bidirectional Unicode characters
# 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); | |
} | |
} | |
} | |
} |
Sign up for free
to join this conversation on GitHub.
Already have an account?
Sign in to comment