add interpolate function

3 files changed, 87 insertions, 2 deletions

diff --git a/include/llmr/geometry/anchor.hpp b/include/llmr/geometry/anchor.hpp
new file mode 100644
index 0000000000..879213ab05
--- /dev/null
+++ b/include/llmr/geometry/anchor.hpp
@@ -0,0 +1,25 @@
+#ifndef LLMR_GEOMETRY_ANCHOR
+#define LLMR_GEOMETRY_ANCHOR
+
+#include <vector>
+
+namespace llmr {
+
+struct Anchor {
+    float x = 0.0f;
+    float y = 0.0f;
+    float angle = 0.0f;
+    float scale = 0.0f;
+    int segment = -1;
+
+    Anchor(float x, float y, float angle, float scale)
+        : x(x), y(y), angle(angle), scale(scale) {}
+    Anchor(float x, float y, float angle, float scale, int segment)
+        : x(x), y(y), angle(angle), scale(scale), segment(segment) {}
+};
+
+typedef std::vector<Anchor> Anchors;
+
+}
+
+#endif
\ No newline at end of file
diff --git a/include/llmr/geometry/interpolate.hpp b/include/llmr/geometry/interpolate.hpp
new file mode 100644
index 0000000000..bfc00a5134
--- /dev/null
+++ b/include/llmr/geometry/interpolate.hpp
@@ -0,0 +1,54 @@
+#ifndef LLMR_GEOMETRY_INTERPOLATE
+#define LLMR_GEOMETRY_INTERPOLATE
+
+#include <llmr/geometry/anchor.hpp>
+#include <llmr/util/math.hpp>
+
+namespace llmr {
+
+template <typename Vertices>
+Anchors interpolate(const Vertices& vertices, float spacing, float minScale = 0.0f, int start = 0) {
+
+    float distance = 0.0f;
+    float markedDistance = 0.0f;
+    int added = start;
+
+    Anchors points;
+
+    auto end = vertices.end() - 1;
+    int i = 0;
+    for (auto it = vertices.begin(); it != end; it++, i++) {
+        const auto& a = *(it),
+            b = *(it + 1);
+
+        float segmentDist = util::dist(a, b);
+        float angle = util::angle_to(b, a);
+
+        while (markedDistance + spacing < distance + segmentDist) {
+            markedDistance += spacing;
+
+            float t = (markedDistance - distance) / segmentDist,
+                x = util::interp(a.x, b.x, t),
+                y = util::interp(a.y, b.y, t),
+                s = added % 8 == 0 ? minScale :
+                    added % 4 == 0 ? 2 :
+                    added % 2 == 0 ? 4 :
+                    8;
+
+            if (x >= 0 && x < 4096 && y >= 0 && y < 4096) {
+                points.emplace_back(x, y, angle, s, i);
+            }
+
+            added++;
+        }
+
+        distance += segmentDist;
+    }
+
+    return points;
+}
+
+}
+
+#endif
+
diff --git a/include/llmr/util/math.hpp b/include/llmr/util/math.hpp
index bf47a3d073..b5f0ca2895 100644
--- a/include/llmr/util/math.hpp
+++ b/include/llmr/util/math.hpp
@@ -22,15 +22,21 @@ inline T min(T a, T b) {
 
 // Find the angle of the two vectors, solving the formula for the cross product
 // a x b = |a||b|sin(θ) for θ.
-inline double angle_between(double ax, double ay, double bx, double by) {
+template <typename T = double, typename S>
+inline T angle_between(S ax, S ay, S bx, S by) {
     return atan2((ax * by - ay * bx), ax * bx + ay * by);
 }
 
 template <typename T = double, typename S>
-inline double angle_between(const vec2<S>& a, const vec2<S>& b) {
+inline T angle_between(const vec2<S>& a, const vec2<S>& b) {
     return angle_between(a.x, a.y, b.x, b.y);
 }
 
+template <typename T = double, typename S>
+inline T angle_to(const vec2<S>& a, const vec2<S>& b) {
+    return atan2(a.y - b.y, a.x - b.x);
+}
+
 template <typename T, typename S1, typename S2>
 inline T interp(S1 a, S2 b, T t) {
     return (a * ((T)1 - t)) + (b * t);