Spiral morph toolpath

spiral_morph.cpp

#include <cmath>
#include <vector>
#include <iostream>

struct point_2
{
    double x;
    double y;

    point_2 operator-(const point_2& p) const {
        return {x-p.x, y-p.y};
    }
    point_2 operator+(const point_2& p) const {
        return {x+p.x, y+p.y};
    }
    point_2 operator*(double t) const {
        return {x*t, y*t};
    }
};
inline double distance(const point_2& a, const point_2& b) {
    return std::sqrt(std::pow(b.x - a.x, 2) + std::pow(b.y - a.y, 2));
}
point_2 lerp(const point_2& p0, const point_2& p1, double t) {
    return { (1-t)*p0.x + t*p1.x, (1-t)*p0.y + t*p1.y };
}

struct vector_2
{
    double x;
    double y;
};
inline double dot(const vector_2& a, const vector_2& b) {
    return a.x * b.x + a.y * b.y;
}

struct line_segment_2
{
    point_2 a;
    point_2 b;
};

// dead man's boost::optional
template <typename T>
struct optional
{
		bool valid = false;
		T value = {};

		optional() = default;
		optional(T v) : valid(true), value(v) {}

		explicit operator bool() { return valid; }
		T operator*() { return value; }
};

// TODO fixed intersection test WRT version in nc_tools repo
optional<point_2> intersects (const line_segment_2& l1, const line_segment_2& l2) {
    auto s1 = l1.b - l1.a;
    auto s2 = l2.b - l2.a;

    vector_2 ortho{s2.y, -s2.x};
    auto denom = dot({s1.x, s1.y}, ortho);
    if (std::abs(denom) < 0.000001)
        return {};

    auto u = l1.a - l2.a;
    auto s = ( s2.x * u.y - s2.y * u.x) / denom;
    auto t = (-s1.y * u.x + s1.x * u.y) / denom;

    if (s >= 0 && s <= 1 && t >= 0 && t <= 1)
        return { l2.a + (s2 * t) };

    return {};
}

// LAZY, considers only a single intersection point!!! (specalised for source inside polygon)
optional<point_2> intersects (const line_segment_2& l1, const std::vector<point_2>& polygon) {
		auto it = begin(polygon);
		auto p0 = *it++;
		while (it != end(polygon)) {
				auto p1 = *it++;
				if (auto p = intersects(l1, line_segment_2{p0, p1}))
						return p;
				p0 = p1;
		}
		return {};
}

point_2 centroid(const std::vector<point_2>& polygon) {
    point_2 c = {0, 0};
    double A = 0;
    auto it = begin(polygon);
    for (unsigned i = 0; i < polygon.size(); ++i) {
        auto p = *it++;
        if (it == end(polygon))
            it = begin(polygon);
        auto p1 = *it;
        c.x += (p.x + p1.x) * ((p.x * p1.y) - (p1.x * p.y));
        c.y += (p.y + p1.y) * ((p.x * p1.y) - (p1.x * p.y));
        A += ((p.x * p1.y) - (p1.x * p.y));
    }

    A /= 2.0;
    c.x /= 6*A;
    c.y /= 6*A;
    return c;
}

int main() {
	std::vector<point_2> polygon = { {37.98518, 2.14975}, {38.870249, 2.350096}, {39.74891, 2.576901}, {40.620372, 2.829959}, {41.483852, 3.109043}, {42.338572, 3.413902}, {43.183763, 3.744262}, {44.018664, 4.099825}, {44.842525, 4.480272}, {45.654603, 4.885259}, {46.454167, 5.314423}, {47.240499, 5.767377}, {48.012891, 6.243713}, {48.718596, 6.708707}, {45.211814, 13.722269}, {37.741827, 2.102289}, {37.98518, 2.14975} };
	// HACK for these polygon points!
	for (auto& p : polygon) {
		p.x -= 48.7186/2;
		p.y -= 13.7223/2;
	}

	auto c = centroid(polygon);

	double min_radius = 0.0;
	double max_radius = 0.0;
	{
		auto it = std::minmax_element(begin(polygon), end(polygon), [&c](const point_2& p0, const point_2& p1) -> bool {
			return distance(c, p0) < distance(c, p1);
		});

		min_radius = distance(c, *it.first);
		max_radius = distance(c, *it.second);
	}

	std::vector<point_2> toolpath;

	double radius = min_radius;
	double PI = 3.14159265359;
	double coil_gap = 0.3;
	double turn_theta = 2*PI * (radius / coil_gap);
	double rad_per_theta = radius / turn_theta;
	for (double theta = 0.1; theta < turn_theta; theta += 0.03) {
		double r = rad_per_theta * theta;
		point_2 p0 = {c.x + std::cos(theta) * r, c.y + std::sin(theta) * r};
		line_segment_2 ray;
		ray.a = c;
		ray.b = {c.x + std::cos(theta) * (max_radius+1), c.y + std::sin(theta) * (max_radius+1)}; // +1 == fudge
		auto p1 = intersects(ray, polygon);
		if (!p1) return 1;	// MUST intersect else not closed polygon.
		auto t = theta / turn_theta;
		auto p = lerp(p0, *p1, t);

		toolpath.push_back(p);
	}

	auto output_path = [](const std::vector<point_2>& path) {
		bool rapid_to_first = true;
		for (auto& p : path) {
				if (rapid_to_first) {
						std::cout << std::fixed << "M" << p.x << " " << p.y;
						rapid_to_first = false;
				}
				std::cout << std::fixed << "L" << p.x << " " << p.y;
		}
		std::cout << "\n";
	};

	output_path(toolpath);
	output_path(polygon);
}