OpenCV Triangulation

pathfinding.cpp

#include <iostream>
#include <string>
#include <vector>
#include <array>
#include <list>
#include <cmath>

#include <opencv2/opencv.hpp>

#include "cvwindow.h"
#include "hsv_filter_window.h"

// PATH
// RGB(82,47,51)
// HUE(229,65,61)

using ContourBuffer = std::vector<std::vector<cv::Point>>;
using Polygon = std::vector<cv::Point>;

struct Triangle
{
	Triangle(cv::Point p1, cv::Point p2, cv::Point p3) :
		points{p1, p2, p3}
	{
	}

	Triangle(const std::array<cv::Point, 3>& points) :
		points(points)
	{
	}

	Triangle() : 
		points{}
	{
	}

	bool hasConcaveVertex() const
	{
		return false;
	}


	std::array<cv::Point, 3> points;
};

static void filter(cv::Mat& out, cv::Mat& src, cv::Scalar min, cv::Scalar max)
{
	cv::cvtColor(src, out, cv::COLOR_BGR2HSV);
	cv::inRange(src, min, max, out);
}

static std::vector<Polygon> getPolygonsFromContours(const ContourBuffer& contours)
{
	std::vector<Polygon> polygons;

	for (const auto& contour : contours)
	{
		Polygon polygon;
		cv::approxPolyDP(contour, polygon, cv::arcLength(cv::Mat(contour), true) * 0.01, true);

		polygons.push_back(polygon);
	}

	return polygons;
}

static void drawPolygons(cv::Mat& out, const std::vector<Polygon>& polygons)
{
	for (const auto& polygon : polygons)
	{
		for (const auto& point : polygon)
		{
			cv::circle(out, point, 3, cv::Scalar(0, 255, 0), 2);
		}
	}
}

static std::array<cv::Point, 3> getAdjacentPoints(std::list<cv::Point>::iterator& ear, std::list<cv::Point>& points)
{
	std::array<cv::Point, 3> ret_points;
	cv::Point p1, p2;

	if (ear == points.begin())
	{
		p1 = points.back();
		p2 = *std::next(ear);
	}
	else if (ear == points.end() || std::next(ear) == points.end())
	{
		p1 = *std::prev(ear);
		p2 = points.front();
	}
	else
	{
		p1 = *std::prev(ear);
		p2 = *std::next(ear);
	}

	ret_points[0] = p1;
	ret_points[1] = *ear;
	ret_points[2] = p2;

	return ret_points;
}

static bool isConvex(std::list<cv::Point>::iterator& pi, std::list<cv::Point>& list)
{
	auto points = getAdjacentPoints(pi, list);

	// get the two direction vectors from P[i-1] -> P[i+1]
	auto d1 = points[1] - points[0];
	auto d2 = points[2] - points[1];

	auto cross = d1.cross(d2);

	// negative is convex
	return std::signbit(cross);
}

static bool isConcave(std::list<cv::Point>::iterator& pi, std::list<cv::Point>& list)
{
	return !isConvex(pi, list);
}

static Triangle getTriangle(std::list<cv::Point>::iterator& ear, std::list<cv::Point>& points)
{
	return Triangle(getAdjacentPoints(ear, points));
}

/*
	http://stackoverflow.com/questions/13300904/determine-whether-point-lies-inside-triangle
*/
static bool pointInTriangle(const Triangle& triangle, const cv::Point& p)
{
	auto A = triangle.points[0];
	auto B = triangle.points[1];
	auto C = triangle.points[2];

	float alpha = (float)((B.y - C.y)*(p.x - C.x) + (C.x - B.x)*(p.y - C.y)) /
		(float)((B.y - C.y)*(A.x - C.x) + (C.x - B.x)*(A.y - C.y));
	float beta = (float)((C.y - A.y)*(p.x - C.x) + (A.x - C.x)*(p.y - C.y)) /
		(float)((B.y - C.y)*(A.x - C.x) + (C.x - B.x)*(A.y - C.y));
	float gamma = 1.0f - alpha - beta;

	return alpha > 0 && beta > 0 && gamma > 0;

}

static std::list<cv::Point>::iterator findEar(std::list<cv::Point>& points)
{
	auto not_found = true;

	auto pi = points.begin();

	while (not_found)
	{
		if (isConvex(pi, points))
		{
			auto triangle = getTriangle(pi, points);
			bool contains_concave_vertex = false;

			for (auto iter = points.begin(); iter != points.end(); ++iter)
			{
				if (iter != pi && isConcave(iter, points) && pointInTriangle(triangle, *iter))
				{
					contains_concave_vertex = true;
				}
			}

			if (!contains_concave_vertex)
			{
				not_found = false;
			}
		}

		if (not_found) 
		{
			pi++;
			if (pi == points.end())
				pi = points.begin();
		}
	}

	return pi;
}


/**
	Triangulate via ear clipping
*/
static std::vector<Triangle> triangulatePolygon(const Polygon& polygon)
{
	std::vector<Triangle> triangles;
	std::list<cv::Point> points(polygon.begin(), polygon.end());
	points.unique();

	while (points.size() > 3)
	{
		// find a ear in the polygon
		auto ear = findEar(points);

		triangles.push_back(getTriangle(ear, points));

		// remove the ear from the list
		points.erase(ear);
	}

	// push last ear
	triangles.push_back(getTriangle(++points.begin(), points));

	return triangles;
}

static void drawTriangles(cv::Mat& out, const std::vector<Triangle>& triangles)
{
	for (const auto& triangle : triangles)
	{
		cv::line(out, triangle.points[0], triangle.points[1], cv::Scalar(255), 1);
		cv::line(out, triangle.points[1], triangle.points[2], cv::Scalar(255), 2);
		cv::line(out, triangle.points[2], triangle.points[0], cv::Scalar(255), 2);
	}
}

int main(int argc, char *argv[])
{
	CVWindow src_window("source");
	CVWindow output_window("output");

	cv::Mat image = cv::imread("simple-maze.png");
	src_window.show(image);

	cv::Mat path;
	filter(path, image, cv::Scalar(0, 1, 0), cv::Scalar(255, 255, 255));

	ContourBuffer contours;
	std::vector<cv::Vec4i> hierarchy;

	cv::Mat canny;

	cv::Canny(path, canny, 100, 200, 3);
	cv::findContours(
		canny, contours, hierarchy,
		CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE,
		cv::Point(0, 0)
	);

	auto polygons = getPolygonsFromContours(contours);
	
	for (const auto& polygon : polygons)
	{
		auto triangles = triangulatePolygon(polygon);
		drawTriangles(image, triangles);
	}

	output_window.show(image);

	cv::waitKey();

	return 0;
}