262 lines
7.8 KiB
Go
262 lines
7.8 KiB
Go
package main
|
|
|
|
// import (
|
|
// "container/heap"
|
|
// "time"
|
|
|
|
// "git.tornberg.me/go-gtfs/pkg/types"
|
|
// )
|
|
|
|
// // CostFactors defines the parameters for the cost function in A* search.
|
|
// type CostFactors struct {
|
|
// TransferPenalty time.Duration
|
|
// MaxTransfers int
|
|
// MaxWaitBetweenTrips time.Duration
|
|
// MaxTravelDuration time.Duration
|
|
// MaxDetourFactor float64
|
|
// }
|
|
|
|
// // Heuristic function estimates the cost from a node to the goal.
|
|
// // For pathfinding on a map, this is typically the straight-line distance.
|
|
// type Heuristic func(from, to *types.Stop) time.Duration
|
|
|
|
// // AStarPlanner uses the A* algorithm to find routes.
|
|
// type AStarPlanner struct {
|
|
// *TripPlanner
|
|
// CostFactors CostFactors
|
|
// Heuristic Heuristic
|
|
// }
|
|
|
|
// // NewAStarPlanner creates a new planner that uses the A* algorithm.
|
|
// func NewAStarPlanner(tp *TripPlanner, factors CostFactors, heuristic Heuristic) *AStarPlanner {
|
|
// return &AStarPlanner{
|
|
// TripPlanner: tp,
|
|
// CostFactors: factors,
|
|
// Heuristic: heuristic,
|
|
// }
|
|
// }
|
|
|
|
// // findRoute implements a time-aware A* algorithm for routing.
|
|
// func (p *AStarPlanner) findRoute(start, end string, when time.Time) *Route {
|
|
// startStop := p.GetStop(start)
|
|
// if startStop == nil {
|
|
// return nil
|
|
// }
|
|
// goalStop := p.GetStop(end)
|
|
// if goalStop == nil {
|
|
// return nil
|
|
// }
|
|
// maxAllowedDistance := haversine(startStop.StopLat, startStop.StopLon, goalStop.StopLat, goalStop.StopLon) * p.CostFactors.MaxDetourFactor
|
|
|
|
// arrival := make(map[string]time.Time)
|
|
// cost := make(map[string]time.Time)
|
|
// prev := make(map[string]PathInfo)
|
|
// pq := &priorityQueue{}
|
|
// heap.Init(pq)
|
|
|
|
// arrival[start] = when
|
|
// cost[start] = when
|
|
// prev[start] = PathInfo{Prev: "", TripID: "", DepartureTime: when, Transfers: 0, LastTrip: "", WaitDuration: 0}
|
|
// heap.Push(pq, &pqItem{Stop: start, Cost: when})
|
|
|
|
// for pq.Len() > 0 {
|
|
// item := heap.Pop(pq).(*pqItem)
|
|
// current := item.Stop
|
|
// if storedCost, ok := cost[current]; !ok || item.Cost.After(storedCost) {
|
|
// continue
|
|
// }
|
|
|
|
// currentArrival, ok := arrival[current]
|
|
// if !ok || currentArrival.IsZero() {
|
|
// continue
|
|
// }
|
|
|
|
// if current == end {
|
|
// // Reconstruct path
|
|
// return reconstructPath(p.TripPlanner, prev, start, end, arrival)
|
|
// }
|
|
|
|
// currentStop := p.GetStop(current)
|
|
// if currentStop == nil {
|
|
// continue
|
|
// }
|
|
|
|
// for _, edge := range p.graph[current] {
|
|
// if edge.To == current {
|
|
// continue
|
|
// }
|
|
// if info, ok := prev[current]; ok && info.Prev == edge.To && info.TripID == edge.TripID {
|
|
// continue
|
|
// }
|
|
// nextStop := p.GetStop(edge.To)
|
|
// if nextStop == nil {
|
|
// continue
|
|
// }
|
|
|
|
// distanceToGoal := haversine(nextStop.StopLat, nextStop.StopLon, goalStop.StopLat, goalStop.StopLon)
|
|
// if distanceToGoal > maxAllowedDistance {
|
|
// continue
|
|
// }
|
|
|
|
// var arrivalTime time.Time
|
|
// var departureTime time.Time
|
|
// var waitDuration time.Duration
|
|
|
|
// if edge.TripID == "transfer" {
|
|
// waitDuration = time.Duration(edge.Time) * time.Second
|
|
// if waitDuration > p.CostFactors.MaxWaitBetweenTrips {
|
|
// continue
|
|
// }
|
|
// arrivalTime = currentArrival.Add(waitDuration)
|
|
// departureTime = arrivalTime
|
|
// } else {
|
|
// depSec := edge.DepartureTime
|
|
// day := currentArrival.Truncate(24 * time.Hour)
|
|
// departure := day.Add(time.Duration(depSec) * time.Second)
|
|
// if departure.Before(currentArrival) {
|
|
// departure = departure.Add(24 * time.Hour)
|
|
// }
|
|
// if departure.After(currentArrival) || departure.Equal(currentArrival) {
|
|
// arrivalTime = departure.Add(time.Duration(edge.Time) * time.Second)
|
|
// departureTime = departure
|
|
// waitDuration = departureTime.Sub(currentArrival)
|
|
// if waitDuration > p.CostFactors.MaxWaitBetweenTrips {
|
|
// continue
|
|
// }
|
|
// } else {
|
|
// continue
|
|
// }
|
|
// }
|
|
|
|
// if arrivalTime.Sub(when) > p.CostFactors.MaxTravelDuration {
|
|
// continue
|
|
// }
|
|
|
|
// currentTransfers := prev[current].Transfers
|
|
// lastTrip := prev[current].LastTrip
|
|
// newTransfers := currentTransfers
|
|
// var newLastTrip string
|
|
// if edge.TripID == "transfer" {
|
|
// newLastTrip = lastTrip
|
|
// } else {
|
|
// newLastTrip = edge.TripID
|
|
// if lastTrip != "" && lastTrip != edge.TripID {
|
|
// newTransfers++
|
|
// }
|
|
// }
|
|
// if newTransfers > p.CostFactors.MaxTransfers {
|
|
// continue
|
|
// }
|
|
|
|
// // A* cost calculation: g(n) + h(n)
|
|
// // g(n) is the actual cost from the start, which is the arrival time with penalties.
|
|
// gCost := arrivalTime
|
|
// if edge.TripID != "transfer" && lastTrip != "" && lastTrip != edge.TripID {
|
|
// gCost = gCost.Add(p.CostFactors.TransferPenalty)
|
|
// }
|
|
|
|
// // h(n) is the heuristic cost from the current node to the goal.
|
|
// hCost := p.Heuristic(nextStop, goalStop)
|
|
// fCost := gCost.Add(hCost)
|
|
|
|
// existingCost, hasCost := cost[edge.To]
|
|
// existingArrival := arrival[edge.To]
|
|
// existingInfo, havePrev := prev[edge.To]
|
|
// shouldRelax := !hasCost || existingCost.IsZero() || fCost.Before(existingCost)
|
|
// if !shouldRelax && fCost.Equal(existingCost) {
|
|
// if existingArrival.IsZero() || arrivalTime.Before(existingArrival) {
|
|
// shouldRelax = true
|
|
// } else if havePrev && arrivalTime.Equal(existingArrival) {
|
|
// if newTransfers < existingInfo.Transfers {
|
|
// shouldRelax = true
|
|
// } else if waitDuration < existingInfo.WaitDuration {
|
|
// shouldRelax = true
|
|
// }
|
|
// }
|
|
// }
|
|
// if shouldRelax {
|
|
// ancestor := current
|
|
// createsCycle := false
|
|
// for ancestor != "" {
|
|
// if ancestor == edge.To {
|
|
// createsCycle = true
|
|
// break
|
|
// }
|
|
// info, ok := prev[ancestor]
|
|
// if !ok {
|
|
// break
|
|
// }
|
|
// ancestor = info.Prev
|
|
// }
|
|
// if createsCycle {
|
|
// continue
|
|
// }
|
|
// arrival[edge.To] = arrivalTime
|
|
// cost[edge.To] = fCost
|
|
// prev[edge.To] = PathInfo{Prev: current, TripID: edge.TripID, DepartureTime: departureTime, Transfers: newTransfers, LastTrip: newLastTrip, WaitDuration: waitDuration}
|
|
// heap.Push(pq, &pqItem{Stop: edge.To, Cost: fCost})
|
|
// }
|
|
// }
|
|
// }
|
|
// return nil
|
|
// }
|
|
|
|
// func reconstructPath(tp *TripPlanner, prev map[string]PathInfo, start, end string, arrival map[string]time.Time) *Route {
|
|
// stopsPath := []string{}
|
|
// current := end
|
|
// visited := make(map[string]bool)
|
|
// for current != "" {
|
|
// if visited[current] {
|
|
// break
|
|
// }
|
|
// visited[current] = true
|
|
// stopsPath = append([]string{current}, stopsPath...)
|
|
// if current == start {
|
|
// break
|
|
// }
|
|
// info, ok := prev[current]
|
|
// if !ok || info.Prev == "" {
|
|
// break
|
|
// }
|
|
// current = info.Prev
|
|
// }
|
|
// legs := []Leg{}
|
|
// var currentLeg *Leg
|
|
// for i := 0; i < len(stopsPath)-1; i++ {
|
|
// from := stopsPath[i]
|
|
// to := stopsPath[i+1]
|
|
// tripID := prev[to].TripID
|
|
// if tripID != "transfer" {
|
|
// if currentLeg == nil || currentLeg.TripID != tripID {
|
|
// if currentLeg != nil {
|
|
// legs = append(legs, *currentLeg)
|
|
// }
|
|
// trip := tp.GetTrip(tripID)
|
|
// route := tp.GetRoute(trip.RouteId)
|
|
// currentLeg = &Leg{
|
|
// TripID: tripID,
|
|
// From: from,
|
|
// FromStop: tp.GetStop(from),
|
|
// Trip: trip,
|
|
// Agency: tp.GetAgency(route.AgencyID),
|
|
// Route: route,
|
|
// Stops: []string{from},
|
|
// DepartureTime: prev[to].DepartureTime,
|
|
// }
|
|
// }
|
|
// currentLeg.To = to
|
|
// currentLeg.ToStop = tp.GetStop(to)
|
|
// currentLeg.Stops = append(currentLeg.Stops, to)
|
|
// currentLeg.ArrivalTime = arrival[to]
|
|
// }
|
|
// }
|
|
// if currentLeg != nil {
|
|
// currentLeg.To = stopsPath[len(stopsPath)-1]
|
|
// currentLeg.ToStop = tp.GetStop(currentLeg.To)
|
|
// currentLeg.ArrivalTime = arrival[stopsPath[len(stopsPath)-1]]
|
|
// legs = append(legs, *currentLeg)
|
|
// }
|
|
|
|
// return &Route{Legs: legs}
|
|
// }
|