1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
|
using Costasdev.Busurbano.Backend.Configuration;
using Costasdev.Busurbano.Backend.Types;
using Microsoft.Extensions.Options;
using ProjNet.CoordinateSystems;
using ProjNet.CoordinateSystems.Transformations;
using SysFile = System.IO.File;
namespace Costasdev.Busurbano.Backend.Services;
/// <summary>
/// Service for loading shapes and calculating remaining path from a given stop point
/// </summary>
public class ShapeTraversalService
{
private readonly AppConfiguration _configuration;
private readonly ILogger<ShapeTraversalService> _logger;
private readonly ICoordinateTransformation _transformation;
public ShapeTraversalService(IOptions<AppConfiguration> options, ILogger<ShapeTraversalService> logger)
{
_configuration = options.Value;
_logger = logger;
// Set up coordinate transformation from EPSG:25829 (meters) to EPSG:4326 (lat/lng)
var ctFactory = new CoordinateTransformationFactory();
var csFactory = new CoordinateSystemFactory();
// EPSG:25829 - ETRS89 / UTM zone 29N
var source = csFactory.CreateFromWkt(
"PROJCS[\"ETRS89 / UTM zone 29N\",GEOGCS[\"ETRS89\",DATUM[\"European_Terrestrial_Reference_System_1989\",SPHEROID[\"GRS 1980\",6378137,298.257222101,AUTHORITY[\"EPSG\",\"7019\"]],TOWGS84[0,0,0,0,0,0,0],AUTHORITY[\"EPSG\",\"6258\"]],PRIMEM[\"Greenwich\",0,AUTHORITY[\"EPSG\",\"8901\"]],UNIT[\"degree\",0.0174532925199433,AUTHORITY[\"EPSG\",\"9122\"]],AUTHORITY[\"EPSG\",\"4258\"]],PROJECTION[\"Transverse_Mercator\"],PARAMETER[\"latitude_of_origin\",0],PARAMETER[\"central_meridian\",-9],PARAMETER[\"scale_factor\",0.9996],PARAMETER[\"false_easting\",500000],PARAMETER[\"false_northing\",0],UNIT[\"metre\",1,AUTHORITY[\"EPSG\",\"9001\"]],AXIS[\"Easting\",EAST],AXIS[\"Northing\",NORTH],AUTHORITY[\"EPSG\",\"25829\"]]");
// EPSG:4326 - WGS84
var target = GeographicCoordinateSystem.WGS84;
_transformation = ctFactory.CreateFromCoordinateSystems(source, target);
}
/// <summary>
/// Loads a shape from disk
/// </summary>
public async Task<Shape?> LoadShapeAsync(string shapeId)
{
var file = Path.Combine(_configuration.ScheduleBasePath, "shapes", shapeId + ".pb");
if (!SysFile.Exists(file))
{
_logger.LogWarning("Shape file not found: {ShapeId}", shapeId);
return null;
}
try
{
var contents = await SysFile.ReadAllBytesAsync(file);
var shape = Shape.Parser.ParseFrom(contents);
return shape;
}
catch (Exception ex)
{
_logger.LogError(ex, "Error loading shape {ShapeId}", shapeId);
return null;
}
}
/// <summary>
/// Calculates the bus position by reverse-traversing the shape from the stop location
/// </summary>
/// <param name="shape">The shape points (in EPSG:25829 meters)</param>
/// <param name="stopLocation">The stop location (in EPSG:25829 meters)</param>
/// <param name="distanceMeters">Distance in meters from the stop to traverse backwards</param>
/// <returns>The lat/lng position of the bus, or null if not calculable</returns>
public Position? GetBusPosition(Shape shape, Epsg25829 stopLocation, int distanceMeters)
{
if (shape.Points.Count == 0 || distanceMeters <= 0)
{
return null;
}
// Find the closest point on the shape to the stop
int closestPointIndex = FindClosestPointIndex(shape.Points, stopLocation);
// Traverse backwards from the closest point to find the position at the given distance
var (busPoint, forwardPoint) = TraverseBackwards(shape.Points.ToArray(), closestPointIndex, distanceMeters);
if (busPoint == null)
{
return null;
}
// Compute orientation in EPSG:25829 (meters): 0°=North, 90°=East (azimuth)
var dx = forwardPoint.X - busPoint.X; // Easting difference
var dy = forwardPoint.Y - busPoint.Y; // Northing difference
var bearing = Math.Atan2(dx, dy) * 180.0 / Math.PI; // swap for 0° north
if (bearing < 0) bearing += 360.0;
// Transform from EPSG:25829 (meters) to EPSG:4326 (lat/lng)
var pos = TransformToLatLng(busPoint);
pos.OrientationDegrees = (int)Math.Round(bearing);
return pos;
}
/// <summary>
/// Traverses backwards along the shape from a starting point by the specified distance
/// </summary>
private (Epsg25829 point, Epsg25829 forward) TraverseBackwards(Epsg25829[] shapePoints, int startIndex, double distanceMeters)
{
if (startIndex <= 0)
{
// Already at the beginning, return the first point
var forwardIdx = Math.Min(1, shapePoints.Length - 1);
return (shapePoints[0], shapePoints[forwardIdx]);
}
double remainingDistance = distanceMeters;
int currentIndex = startIndex;
while (currentIndex > 0 && remainingDistance > 0)
{
var segmentDistance = CalculateDistance(shapePoints[currentIndex], shapePoints[currentIndex - 1]);
if (segmentDistance >= remainingDistance)
{
// The bus position is somewhere along this segment
// Interpolate between the two points
var ratio = remainingDistance / segmentDistance;
var interpolated = InterpolatePoint(shapePoints[currentIndex], shapePoints[currentIndex - 1], ratio);
// Forward direction is towards the stop (increasing index direction)
return (interpolated, shapePoints[currentIndex]);
}
remainingDistance -= segmentDistance;
currentIndex--;
}
// We've reached the beginning of the shape
var fwd = shapePoints[Math.Min(1, shapePoints.Length - 1)];
return (shapePoints[0], fwd);
}
/// <summary>
/// Interpolates a point between two points at a given ratio
/// </summary>
private Epsg25829 InterpolatePoint(Epsg25829 from, Epsg25829 to, double ratio)
{
return new Epsg25829
{
X = from.X + (to.X - from.X) * ratio,
Y = from.Y + (to.Y - from.Y) * ratio
};
}
/// <summary>
/// Finds the index of the closest point in the shape to the given location
/// </summary>
private int FindClosestPointIndex(IEnumerable<Epsg25829> shapePoints, Epsg25829 location)
{
var pointsArray = shapePoints.ToArray();
var minDistance = double.MaxValue;
var closestIndex = 0;
for (int i = 0; i < pointsArray.Length; i++)
{
var distance = CalculateDistance(pointsArray[i], location);
if (distance < minDistance)
{
minDistance = distance;
closestIndex = i;
}
}
return closestIndex;
}
/// <summary>
/// Calculates Euclidean distance between two points in meters
/// </summary>
private double CalculateDistance(Epsg25829 p1, Epsg25829 p2)
{
var dx = p1.X - p2.X;
var dy = p1.Y - p2.Y;
return Math.Sqrt(dx * dx + dy * dy);
}
/// <summary>
/// Transforms a point from EPSG:25829 (meters) to EPSG:4326 (lat/lng)
/// </summary>
private Position TransformToLatLng(Epsg25829 point)
{
var transformed = _transformation.MathTransform.Transform(new[] { point.X, point.Y });
return new Position
{
// Round to 6 decimals (~0.1m precision)
Longitude = Math.Round(transformed[0], 6),
Latitude = Math.Round(transformed[1], 6)
};
}
}
|
