StarPilot/mapd_repo/maps/way_geometry_test.go

package maps

import (
	"math"
	"testing"

	"capnproto.org/go/capnp/v3"
	"pfeifer.dev/mapd/cereal/log"
	"pfeifer.dev/mapd/cereal/offline"
	m "pfeifer.dev/mapd/math"
)

func TestBearingAlignmentIsAnUndirectedNonnegativePenalty(t *testing.T) {
	// Roads are represented in either node order. Reversing the same segment
	// or wrapping a GPS heading must not turn a crossing into a score bonus.
	start := m.NewPosition(35.15, -97.9)
	for _, delta := range [][2]float64{{0.01, 0}, {0, 0.01}, {-0.01, 0}, {0, -0.01}, {-0.01, -0.01}} {
		end := m.NewPosition(start.Lat()+delta[0], start.Lon()+delta[1])
		_, seg, err := capnp.NewMessage(capnp.SingleSegment(nil))
		if err != nil {
			t.Fatal(err)
		}
		raw, err := offline.NewRootWay(seg)
		if err != nil {
			t.Fatal(err)
		}
		nodes, err := raw.NewNodes(2)
		if err != nil {
			t.Fatal(err)
		}
		nodes.At(0).SetLatitude(start.Lat())
		nodes.At(0).SetLongitude(start.Lon())
		nodes.At(1).SetLatitude(end.Lat())
		nodes.At(1).SetLongitude(end.Lon())
		way := NewWay(raw)
		location, err := log.NewGpsLocationData(seg)
		if err != nil {
			t.Fatal(err)
		}
		location.SetLatitude((start.Lat() + end.Lat()) / 2)
		location.SetLongitude((start.Lon() + end.Lon()) / 2)
		vec := start.VectorTo(end)
		bearing := vec.Bearing()
		for _, relative := range []float64{0, 30, 90, 150, 180, 270} {
			heading := math.Mod(bearing*180/math.Pi+relative+360, 360)
			location.SetBearingDeg(float32(heading))
			got, err := way.BearingAlignment(location)
			if err != nil {
				t.Fatal(err)
			}
			want := math.Abs(math.Sin(relative * math.Pi / 180))
			if got < 0 || got > 1 || math.Abs(float64(got)-want) > 1e-6 {
				t.Errorf("segment delta=%v heading=%f: alignment=%f want=%f", delta, heading, got, want)
			}
		}
	}
}