Implement asension of ladders

bot/path/movement.go

@@ -58,6 +58,7 @@ var allMovements = []Movement{TraverseNorth, TraverseSouth, TraverseEast, Traver
 	DropNorth, DropSouth, DropEast, DropWest,
 	AscendNorth, AscendSouth, AscendEast, AscendWest,
 	DescendLadder, DescendLadderNorth, DescendLadderSouth, DescendLadderEast, DescendLadderWest,
+	AscendLadder,
 }
 
 // Valid movement values.
@@ -84,6 +85,7 @@ const (
 	DescendLadderSouth
 	DescendLadderEast
 	DescendLadderWest
+	AscendLadder
 )
 
 func (m Movement) Possible(nav *Nav, x, y, z int, from V3, previous Movement) bool {
@@ -125,12 +127,10 @@ func (m Movement) Possible(nav *Nav, x, y, z int, from V3, previous Movement) bo
 			AirLikeBlock(nav.World.GetBlockStatus(from.X, from.Y+1, from.Z)) &&
 			AirLikeBlock(nav.World.GetBlockStatus(from.X, from.Y+2, from.Z))
 
-	case DescendLadder:
+	case DescendLadder, AscendLadder:
 		if bID := nav.World.GetBlockStatus(x, y+1, z); !AirLikeBlock(bID) && !IsLadder(bID) {
 			return false
 		}
-		bID := nav.World.GetBlockStatus(x, y, z)
-		fmt.Println(bID, IsLadder(bID))
 		return IsLadder(nav.World.GetBlockStatus(x, y, z))
 
 	case DescendLadderNorth, DescendLadderSouth, DescendLadderEast, DescendLadderWest:
@@ -139,8 +139,6 @@ func (m Movement) Possible(nav *Nav, x, y, z int, from V3, previous Movement) bo
 				return false
 			}
 		}
-		bID := nav.World.GetBlockStatus(x, y, z)
-		fmt.Println(bID, IsLadder(bID))
 		return IsLadder(nav.World.GetBlockStatus(x, y, z))
 
 	default:
@@ -161,6 +159,8 @@ func (m Movement) Offset() (x, y, z int) {
 	case TraverseWest:
 		return West.Offset()
 
+	case AscendLadder:
+		return 0, 1, 0
 	case DescendLadder:
 		return 0, -1, 0
 	case DropNorth, DescendLadderNorth:
@@ -211,6 +211,8 @@ func (m Movement) BaseCost() float64 {
 		return 1.5
 	case DescendLadder:
 		return 1.2
+	case AscendLadder:
+		return 1.5
 	default:
 		panic(m)
 	}
@@ -266,6 +268,9 @@ func (m Movement) String() string {
 		return "descend-ladder-east"
 	case DescendLadderWest:
 		return "descend-ladder-west"
+
+	case AscendLadder:
+		return "ascend-ladder"
 	default:
 		panic(m)
 	}

bot/path/path.go

@@ -95,7 +95,8 @@ func (t Tile) PathNeighbors() []astar.Pather {
 	return possibles
 }
 
-func (t Tile) Inputs(deltaPos, vel Point) Inputs {
+func (t Tile) Inputs(pos, deltaPos, vel Point) Inputs {
+
 	// Sufficient for simple movements.
 	at := math.Atan2(-deltaPos.X, -deltaPos.Z)
 	out := Inputs{
@@ -104,6 +105,30 @@ func (t Tile) Inputs(deltaPos, vel Point) Inputs {
 	}
 
 	switch t.Movement {
+	case DescendLadder, DescendLadderEast, DescendLadderWest, DescendLadderNorth, DescendLadderSouth:
+		// Deadzone the throttle to prevent an accidental ascend.
+		if dist2 := math.Sqrt(deltaPos.X*deltaPos.X + deltaPos.Z*deltaPos.Z); dist2 < (0.22 * 0.22 * 2) {
+			out.ThrottleX, out.ThrottleZ = 0, 0
+		}
+
+	case AscendLadder:
+		dist2 := math.Sqrt(deltaPos.X*deltaPos.X + deltaPos.Z*deltaPos.Z)
+		bStateID := t.Nav.World.GetBlockStatus(t.Pos.X, t.Pos.Y, t.Pos.Z)
+
+		if x, _, z := LadderDirection(bStateID).Offset(); dist2 > (0.9*0.9) && deltaPos.Y < 0 {
+			pos.X -= (0.55 * float64(x))
+			pos.Z -= (0.55 * float64(z))
+		} else {
+			pos.X += (0.55 * float64(x))
+			pos.Z += (0.55 * float64(z))
+		}
+
+		at = math.Atan2(-pos.X+float64(t.Pos.X)+0.5, -pos.Z+float64(t.Pos.Z)+0.5)
+		out = Inputs{
+			ThrottleX: math.Sin(at),
+			ThrottleZ: math.Cos(at),
+		}
+
 	case AscendNorth, AscendSouth, AscendEast, AscendWest:
 		dist2 := math.Sqrt(deltaPos.X*deltaPos.X + deltaPos.Z*deltaPos.Z)
 		out.Jump = dist2 < 1.75 && deltaPos.Y < -0.81
@@ -119,8 +144,11 @@ func (t Tile) Inputs(deltaPos, vel Point) Inputs {
 
 func (t Tile) IsComplete(d Point) bool {
 	switch t.Movement {
-	case DescendLadder, DescendLadderNorth, DescendLadderSouth, DescendLadderWest, DescendLadderEast, DropNorth, DropSouth, DropEast, DropWest:
+	case DescendLadder, DescendLadderNorth, DescendLadderSouth, DescendLadderWest, DescendLadderEast,
-		return (d.X*d.X+d.Z*d.Z) < (0.1*0.1*0.13) && d.Y <= 0.05
+		DropNorth, DropSouth, DropEast, DropWest:
+		return (d.X*d.X+d.Z*d.Z) < (2*0.2*0.25) && d.Y <= 0.05
+	case AscendLadder:
+		return d.Y >= 0
 	}
 
 	return (d.X*d.X+d.Z*d.Z) < (0.18*0.18) && d.Y >= -0.01 && d.Y <= 0.08