[p5,geom] Implement helper package for cubic bezier spline routing (#15)

internal/collectors/deque.go

@@ -0,0 +1,61 @@
+package collectors
+
+type Deque[T any] struct {
+	data []T
+	f    int // front index
+	b    int // back index
+}
+
+func NewDeque[T any](size int) *Deque[T] {
+	return &Deque[T]{
+		data: make([]T, size*2),
+		f:    size,
+		b:    size - 1,
+	}
+}
+
+func (d *Deque[_]) Len() int {
+	return d.b - d.f + 1
+}
+
+// PushFront pushes a new item to the front of the queue.
+// This grows the queue to the left toward the 0 index.
+func (d *Deque[T]) PushFront(x T) {
+	d.f--
+	d.data[d.f] = x
+}
+
+// PushBack pushes a new item to the back of the queue.
+// This grows the queue to the right toward Len()-1 index.
+func (d *Deque[T]) PushBack(x T) {
+	d.b++
+	d.data[d.b] = x
+}
+
+func (d *Deque[T]) PeekFront(i int) T {
+	return d.data[d.f+i-1]
+}
+
+func (d *Deque[T]) PeekBack(i int) T {
+	return d.data[d.b-i+1]
+}
+
+func (d *Deque[T]) PopFront() T {
+	i := d.f
+	d.f++
+	return d.data[i]
+}
+
+func (d *Deque[T]) PopBack() T {
+	i := d.b
+	d.b--
+	return d.data[i]
+}
+
+func (d *Deque[T]) Front() int {
+	return d.f
+}
+
+func (d *Deque[T]) Back() int {
+	return d.b
+}

internal/collectors/deque_test.go

@@ -0,0 +1,23 @@
+package collectors
+
+import (
+	"testing"
+
+	"github.com/stretchr/testify/assert"
+)
+
+func TestDeque(t *testing.T) {
+	deq := NewDeque[string](10)
+	deq.PushFront("p")
+	deq.PushFront("u")
+	deq.PushBack("v")
+	deq.PushFront("w1")
+	deq.PushFront("w2")
+	deq.PushBack("w3")
+	assert.Equal(t, 6, deq.Len())
+	assert.Equal(t, 6, deq.f)  // 10-4
+	assert.Equal(t, 11, deq.b) // 9+2
+	assert.Equal(t, "v", deq.data[10])
+	assert.Equal(t, "w2", deq.PopFront())
+	assert.Equal(t, "w3", deq.PopBack())
+}

internal/collectors/mat.go

@@ -0,0 +1,11 @@
+package collectors
+
+type Mat[T any] [][]T
+
+func NewMat[T any](n int) Mat[T] {
+	m := make([][]T, n)
+	for i := range m {
+		m[i] = make([]T, n)
+	}
+	return m
+}

internal/collectors/stack.go

@@ -0,0 +1,27 @@
+package collectors
+
+type stack[T any] struct {
+	ts []T
+}
+
+func NewStack[T any](n int) *stack[T] {
+	return &stack[T]{make([]T, 0, n)}
+}
+
+func (s *stack[T]) Push(vs ...T) {
+	s.ts = append(s.ts, vs...)
+}
+
+func (s *stack[T]) Pop() (t T) {
+	t = s.ts[s.Len()-1]
+	s.ts = s.ts[:s.Len()-1]
+	return
+}
+
+func (s *stack[T]) Peek(n int) T {
+	return s.ts[s.Len()-n]
+}
+
+func (s *stack[T]) Len() int {
+	return len(s.ts)
+}

internal/geom/orientation.go

@@ -0,0 +1,23 @@
+package geom
+
+const (
+	// counter-clockwise
+	ccw = iota - 1
+	// collinear
+	cln
+	// clockwise
+	cw
+)
+
+// calculates the determinant of the vector product of the three points, and determines the orientation.
+// NOTE: autog works with SVG-like coordinates so the inequalities are reversed
+func orientation(a, b, c P) int {
+	d := (b.X-a.X)*(c.Y-a.Y) - (b.Y-a.Y)*(c.X-a.X)
+	if d < 0 {
+		return ccw
+	}
+	if d > 0 {
+		return cw
+	}
+	return cln
+}

internal/geom/orientation_test.go

@@ -0,0 +1,61 @@
+package geom
+
+import (
+	"math"
+	"testing"
+
+	"github.com/stretchr/testify/assert"
+)
+
+func TestOrientation(t *testing.T) {
+	t.Run("success", func(t *testing.T) {
+		a, b, c := P{0, 0}, P{20, 20}, P{40, 20}
+		assert.Equal(t, ccw, orientation(a, b, c))
+		assert.Equal(t, cw, orientation(a, c, b))
+		assert.Equal(t, cw, orientation(c, b, a))
+		assert.Equal(t, ccw, orientation(c, a, b))
+	})
+
+	t.Run("success with small margin", func(t *testing.T) {
+		a, b, c := P{12.0, 4.78}, P{12.0000000001, 14.78}, P{12.0, 20}
+		assert.Equal(t, cw, orientation(a, b, c))
+		assert.Equal(t, ccw, orientation(a, c, b))
+	})
+
+	t.Run("collinear single point", func(t *testing.T) {
+		a := P{math.Pi, math.Cos(78.12)}
+		b, c := a, a
+		assert.Equal(t, cln, orientation(a, b, c))
+	})
+
+	t.Run("collinear two identical", func(t *testing.T) {
+		a := P{45.787232, 34.9829283}
+		c := P{12.8934, 65.9232}
+		b := a
+		assert.Equal(t, cln, orientation(a, b, c))
+	})
+
+	t.Run("collinear origin", func(t *testing.T) {
+		a, b, c := P{}, P{}, P{}
+		assert.Equal(t, cln, orientation(a, b, c))
+	})
+
+	t.Run("collinear parallel to x axis", func(t *testing.T) {
+		a, b, c := P{-8934.12, 50.566}, P{0, 50.566}, P{math.Sqrt(23), 50.566}
+		assert.Equal(t, cln, orientation(a, b, c))
+	})
+
+	t.Run("collinear parallel to y axis", func(t *testing.T) {
+		a, b, c := P{20.001, 50}, P{20.001, 75.346}, P{20.001, -0.00000001}
+		assert.Equal(t, cln, orientation(a, b, c))
+	})
+
+	t.Run("collinear with pos slope", func(t *testing.T) {
+		f := func(x float64) float64 {
+			return 4*x/5.0 + 7.58
+		}
+		x1, x2, x3 := 45.68, 0.012, -746.1
+		a, b, c := P{x1, f(x1)}, P{x2, f(x2)}, P{x3, f(x3)}
+		assert.Equal(t, cln, orientation(a, b, c))
+	})
+}

internal/geom/point.go

@@ -0,0 +1,70 @@
+package geom
+
+import (
+	"fmt"
+	"math"
+)
+
+// P represents a point on the plane
+type P struct {
+	X, Y float64
+}
+
+func (p P) String() string {
+	return fmt.Sprintf(`<circle r="4" cx="%.02f" cy="%.02f" fill="black"/>`, p.X, p.Y)
+}
+
+// could also be represented as a [2]float64 but there's essentially no difference except for readability: p.X, p.Y vs p[0], p[1]
+// otherwise both are comparable, take up 16 bytes and have meaningful zero values
+
+// adds p2 to p1 and returns a new point
+func addp(p1, p2 P) P {
+	return P{
+		p1.X + p2.X,
+		p1.Y + p2.Y,
+	}
+}
+
+// subtracts p2 from p1 and returns a new point
+func subp(p1, p2 P) P {
+	return P{
+		p1.X - p2.X,
+		p1.Y - p2.Y,
+	}
+}
+
+// multiplies p by a scalar and returns a new point
+func scalep(p P, c float64) P {
+	return P{
+		p.X * c,
+		p.Y * c,
+	}
+}
+
+// computes the dot product between p1 and p2
+func dotp(p1, p2 P) float64 {
+	return p1.X*p2.X + p1.Y*p2.Y
+}
+
+// computes the distance between p1 and p2
+func distp(p, q P) float64 {
+	return math.Hypot(q.X-p.X, q.Y-p.Y)
+}
+
+// computes the square distance between p1 and p2
+func sqdistp(p, q P) float64 {
+	return (q.X-p.X)*(q.X-p.X) + (q.Y-p.Y)*(q.Y-p.Y)
+}
+
+// normalizes the vector represented by this point (sets its length to 1)
+func norm(p P) P {
+	d := p.X*p.X + p.Y*p.Y
+	if d > epsilon2 {
+		d = math.Sqrt(d)
+		return P{
+			X: p.X / d,
+			Y: p.Y / d,
+		}
+	}
+	return p
+}

internal/geom/polygon.go

@@ -0,0 +1,66 @@
+package geom
+
+// Polygon represents a polygon on the plane
+type Polygon struct {
+	Points []P // counterclockwise list of points
+	r      int // index at which the right chain starts
+}
+
+func (p Polygon) Sides() []Segment {
+	barriers := []Segment{}
+	for i := 1; i < len(p.Points); i++ {
+		barriers = append(barriers, Segment{p.Points[i-1], p.Points[i]})
+	}
+	barriers = append(barriers, Segment{p.Points[len(p.Points)-1], p.Points[0]})
+	return barriers
+}
+
+func MergeRects(rects []Rect) Polygon {
+	np := len(rects) * 2 // number of polygon vertices
+
+	lps := make([]P, 0, np)
+	rps := make([]P, 0, np)
+
+	var prev Rect
+	for i, r := range rects {
+		if i == 0 {
+			lps = append(lps, r.TL)
+			rps = append(rps, P{r.BR.X, r.TL.Y})
+			prev = r
+			continue
+		}
+
+		if prev.TL.X != r.TL.X {
+			lps = append(lps, P{prev.TL.X, r.TL.Y}, r.TL)
+		} else {
+			lps = append(lps, r.TL)
+		}
+
+		if prev.BR.X != r.BR.X {
+			// rps will be iterated backwards
+			rps = append(rps, prev.BR, P{r.BR.X, prev.BR.Y})
+		} else {
+			rps = append(rps, prev.BR)
+		}
+
+		prev = r
+		i += 2
+	}
+	lps = append(lps, P{prev.TL.X, prev.BR.Y})
+	rps = append(rps, prev.BR)
+
+	points := make([]P, np*2)
+	i := 0
+	for i < len(lps) {
+		points[i] = lps[i]
+		i++
+	}
+	r := i
+	j := len(rps) - 1
+	for j >= 0 {
+		points[i] = rps[j]
+		i++
+		j--
+	}
+	return Polygon{points, r}
+}

internal/geom/polygon_test.go

@@ -0,0 +1,34 @@
+package geom
+
+import (
+	"testing"
+
+	"github.com/stretchr/testify/assert"
+)
+
+func TestMergeRects(t *testing.T) {
+	rects := []Rect{
+		{P{305, 1}, P{348, 52}},
+		{P{181, 52}, P{345, 103}},
+		{P{259, 103}, P{318, 154}},
+		{P{1, 154}, P{282, 205}},
+		{P{78, 205}, P{162, 256}},
+		{P{137, 256}, P{391, 307}},
+		{P{18, 307}, P{240, 358}},
+		{P{175, 358}, P{367, 409}},
+		{P{251, 409}, P{471, 460}},
+		{P{174, 460}, P{341, 511}},
+	}
+
+	poly := MergeRects(rects)
+
+	want := []P{
+		{305, 1}, {305, 52}, {181, 52}, {181, 103}, {259, 103}, {259, 154}, {1, 154}, {1, 205}, {78, 205}, {78, 256},
+		{137, 256}, {137, 307}, {18, 307}, {18, 358}, {175, 358}, {175, 409}, {251, 409}, {251, 460}, {174, 460}, {174, 511},
+		{341, 511}, {341, 460}, {471, 460}, {471, 409}, {367, 409}, {367, 358}, {240, 358}, {240, 307}, {391, 307}, {391, 256},
+		{162, 256}, {162, 205}, {282, 205}, {282, 154}, {318, 154}, {318, 103}, {345, 103}, {345, 52}, {348, 52}, {348, 1},
+	}
+	for i := range poly.Points {
+		assert.Equal(t, want[i], poly.Points[i])
+	}
+}