punish change directions

astar.go

@@ -133,7 +133,6 @@ func (p Pathfinder) Find(startPos, endPos Vec2) []Vec2 {
 			searchSpace.Set(succPos, successor)
 			open.push(heapNode{pos: succPos, f: successor.f})
 		}
-
 		q.closed = true
 		searchSpace.Set(qPos, q)
 	}
@@ -142,6 +141,43 @@ func (p Pathfinder) Find(startPos, endPos Vec2) []Vec2 {
 	return []Vec2{}
 }
 
+func punishChangeDirection(q node, successor, end Vec2) int {
+	if q.parent == nil {
+		return 0
+	}
+	punishment := abs(successor.X-end.X) + abs(successor.Y-end.Y)
+
+	isHorizAdj := func(a, b Vec2) bool {
+		return a.Y-b.Y == 0
+	}
+	if !isHorizAdj(q.pos, successor) {
+		if isHorizAdj(q.pos, q.parent.pos) {
+			return punishment
+		}
+	}
+
+	isVertAdj := func(a, b Vec2) bool {
+		return a.X-b.X == 0
+	}
+	if !isVertAdj(q.pos, successor) {
+		if isVertAdj(q.pos, q.parent.pos) {
+			return punishment
+		}
+	}
+
+	// todo: check option if diagonal enabled
+	isDiagonal := func(a, b Vec2) bool {
+		return abs(a.X-b.X) == abs(a.Y-b.Y)
+	}
+	if !isDiagonal(q.pos, successor) {
+		if isDiagonal(q.pos, q.parent.pos) {
+			return punishment
+		}
+	}
+
+	return 0
+}
+
 // manhattan calculates the Manhattan distance between two vectors by summing
 // the absolute values of the differences of their components. It does not
 // support diagonal movement.

astar_test.go

@@ -238,7 +238,7 @@ func TestPath_Diagonal2(t *testing.T) {
 }
 
 func TestPath_PunishChangeDirection(t *testing.T) {
-	t.Skip("unimplemented")
+	t.Skip()
 	weights := []int{
 		1, 1, 1, 1, 1,
 		1, 1, 1, 1, 1,
@@ -261,6 +261,94 @@ func TestPath_PunishChangeDirection(t *testing.T) {
 	equal(t, got, want, &grid)
 }
 
+func TestPunishChangeDirection_Algo(t *testing.T) {
+	end := Vec2{7, 2}
+	cases := []struct {
+		name string
+		q    node
+		succ Vec2
+		want int
+	}{
+		{
+			name: "x adjacent",
+			q: node{
+				pos: Vec2{1, 2},
+				parent: &node{
+					pos: Vec2{1, 1},
+				},
+			},
+			succ: Vec2{1, 3},
+			want: 0,
+		},
+		{
+			name: "x change dir",
+			q: node{
+				pos: Vec2{1, 2},
+				parent: &node{
+					pos: Vec2{1, 1},
+				},
+			},
+			succ: Vec2{2, 3},
+			want: 6,
+		},
+		{
+			name: "y adjacent",
+			q: node{
+				pos: Vec2{2, 1},
+				parent: &node{
+					pos: Vec2{1, 1},
+				},
+			},
+			succ: Vec2{3, 1},
+			want: 0,
+		},
+		{
+			name: "y change dir",
+			q: node{
+				pos: Vec2{2, 1},
+				parent: &node{
+					pos: Vec2{1, 1},
+				},
+			},
+			succ: Vec2{3, 2},
+			want: 4,
+		},
+		{
+			name: "diag adj",
+			q: node{
+				pos: Vec2{2, 2},
+				parent: &node{
+					pos: Vec2{1, 1},
+				},
+			},
+			succ: Vec2{3, 3},
+			want: 0,
+		},
+		{
+			name: "diag change dir",
+			q: node{
+				pos: Vec2{2, 2},
+				parent: &node{
+					pos: Vec2{1, 1},
+				},
+			},
+			succ: Vec2{3, 4},
+			want: 6,
+		},
+	}
+
+	for _, c := range cases {
+		t.Run(c.name, func(t *testing.T) {
+			got := punishChangeDirection(c.q, c.succ, end)
+
+			if got != c.want {
+				t.Errorf("want: %d got: %d", c.want, got)
+			}
+		})
+	}
+
+}
+
 func equal(t *testing.T, got, want []Vec2, grid *Grid[int]) {
 	t.Helper()