1

I have an array of latitude and longitudes making an entire route from origin to destination, but I'm really struggling to calculate all the curvatures for the entire rail route, including having left turns as a negative value and right turns as positive (or visa-versa, whichever is easier).

I also have code that gives me a bearing and also the difference between two bearings but that's as far as I've got (code below).

From setting off at the green marker from the image below, travelling west (left), there is:

  • Slight curve to the right
  • A sharper turn to the right
  • A turn to the left
  • Smaller turn to the left
  • Mostly straight
  • A sharp left
  • A bit of straight
  • Followed by an immediate sharp right

Essentially, it's calculating the radius of the turn for each of the turns in the above list/picture.

enter image description here

Desired Output object

public EdgeCurve[] Curves {get; set;}

public class EdgeCurve
{
    public double StartLatitude {get;s et;}
    public double StartLongitude {get; set;}

    public double EndLatitude {get; set;}
    public double EndLongitude {get; set;}

    public double Curve {get; set;}

    //Optional
    public double Distance {get;set;}
}

Bearing Code

public static class BearingHelper
{

    /// <summary>
    /// Angle between two points, where North is 0 and South is 180.
    /// </summary>
    /// <param name="lat0">Latitude for point A.</param>
    /// <param name="lon0">Longitude for point A.</param>
    /// <param name="lat1">Latitude for point B.</param>
    /// <param name="lon1">Longitude for point B.</param>
    /// <returns>Angle between points A and B, in degrees.</returns>
    public static double Degrees(double lat0, double lon0, double lat1, double lon1)
    {
        var dlon = (lon1 - lon0).ToRadians();
        var dphi = Math.Log(Math.Tan(lat1.ToRadians() / 2 + PI_d4) / Math.Tan(lat0.ToRadians() / 2 + PI_d4));

        if (Math.Abs(dlon) > Math.PI)
        {
            dlon = dlon > 0 ? -(PI_m2 - dlon) : (PI_m2 + dlon);
        }

        return (Math.Atan2(dlon, dphi).ToDegrees() + 360) % 360;
    }

    public static double AbsDifference(double bearing0, double bearing1)
    {
        double diff = Math.Abs(bearing0 - bearing1);

        if (diff > 180)
            diff -= 360;

        return Math.Abs(diff);
    }
}

public static class CoordinateMath
{
    /// <summary>
    /// PI multiplied by 2.
    /// </summary>
    public const double PI_m2 = 6.28318530718;

    /// <summary>
    /// PI divided by 4.
    /// </summary>
    public const double PI_d4 = 0.78539816339;

    /// <summary>
    /// Convert degrees to radians.
    /// </summary>
    public static double ToRadians(this double deg) => deg * 0.01745329252;

    /// <summary>
    /// Convert radians to degrees.
    /// </summary>
    public static double ToDegrees(this double rad) => rad * 57.295779513;
}

Array of Lat/Longs

[
  {
    "Latitude": 53.743647,
    "Longitude": -0.347531
  },
  {
    "Latitude": 53.743635,
    "Longitude": -0.348089
  },
  {
    "Latitude": 53.74365,
    "Longitude": -0.348648
  },
  {
    "Latitude": 53.74369,
    "Longitude": -0.349114
  },
  {
    "Latitude": 53.743754,
    "Longitude": -0.3496
  },
  {
    "Latitude": 53.743867,
    "Longitude": -0.35029
  },
  {
    "Latitude": 53.743941,
    "Longitude": -0.350679
  },
  {
    "Latitude": 53.744063,
    "Longitude": -0.351096
  },
  {
    "Latitude": 53.744518,
    "Longitude": -0.352272
  },
  {
    "Latitude": 53.744557,
    "Longitude": -0.352369
  },
  {
    "Latitude": 53.744888,
    "Longitude": -0.353199
  },
  {
    "Latitude": 53.744925,
    "Longitude": -0.353296
  },
  {
    "Latitude": 53.745078,
    "Longitude": -0.353724
  },
  {
    "Latitude": 53.745176,
    "Longitude": -0.354035
  },
  {
    "Latitude": 53.745339,
    "Longitude": -0.354672
  },
  {
    "Latitude": 53.745494,
    "Longitude": -0.355461
  },
  {
    "Latitude": 53.745591,
    "Longitude": -0.356196
  },
  {
    "Latitude": 53.745644,
    "Longitude": -0.356898
  },
  {
    "Latitude": 53.74566,
    "Longitude": -0.357678
  },
  {
    "Latitude": 53.745633,
    "Longitude": -0.358616
  },
  {
    "Latitude": 53.745577,
    "Longitude": -0.359361
  },
  {
    "Latitude": 53.745521,
    "Longitude": -0.359924
  },
  {
    "Latitude": 53.745439,
    "Longitude": -0.360563
  },
  {
    "Latitude": 53.745344,
    "Longitude": -0.361198
  },
  {
    "Latitude": 53.745192,
    "Longitude": -0.362056
  },
  {
    "Latitude": 53.745051,
    "Longitude": -0.362748
  },
  {
    "Latitude": 53.744811,
    "Longitude": -0.363821
  },
  {
    "Latitude": 53.744539,
    "Longitude": -0.364885
  },
  {
    "Latitude": 53.74428,
    "Longitude": -0.365809
  },
  {
    "Latitude": 53.744023,
    "Longitude": -0.366656
  },
  {
    "Latitude": 53.743862,
    "Longitude": -0.367141
  },
  {
    "Latitude": 53.743723,
    "Longitude": -0.367545
  },
  {
    "Latitude": 53.7434,
    "Longitude": -0.368412
  },
  {
    "Latitude": 53.743096,
    "Longitude": -0.369159
  },
  {
    "Latitude": 53.742528,
    "Longitude": -0.370427
  },
  {
    "Latitude": 53.742197,
    "Longitude": -0.371099
  },
  {
    "Latitude": 53.741909,
    "Longitude": -0.371663
  },
  {
    "Latitude": 53.741679,
    "Longitude": -0.372077
  },
  {
    "Latitude": 53.741462,
    "Longitude": -0.372498
  },
  {
    "Latitude": 53.74131,
    "Longitude": -0.372765
  },
  {
    "Latitude": 53.737842,
    "Longitude": -0.379223
  },
  {
    "Latitude": 53.737396,
    "Longitude": -0.380011
  },
  {
    "Latitude": 53.737152,
    "Longitude": -0.380388
  },
  {
    "Latitude": 53.737006,
    "Longitude": -0.380587
  },
  {
    "Latitude": 53.736736,
    "Longitude": -0.380912
  },
  {
    "Latitude": 53.736466,
    "Longitude": -0.381183
  },
  {
    "Latitude": 53.736294,
    "Longitude": -0.381331
  },
  {
    "Latitude": 53.736187,
    "Longitude": -0.381417
  },
  {
    "Latitude": 53.735827,
    "Longitude": -0.381659
  },
  {
    "Latitude": 53.735664,
    "Longitude": -0.381751
  },
  {
    "Latitude": 53.735378,
    "Longitude": -0.381882
  },
  {
    "Latitude": 53.735185,
    "Longitude": -0.381952
  },
  {
    "Latitude": 53.734976,
    "Longitude": -0.382014
  },
  {
    "Latitude": 53.734575,
    "Longitude": -0.382077
  },
  {
    "Latitude": 53.734372,
    "Longitude": -0.382083
  },
  {
    "Latitude": 53.734044,
    "Longitude": -0.382055
  },
  {
    "Latitude": 53.733148,
    "Longitude": -0.381884
  },
  {
    "Latitude": 53.732913,
    "Longitude": -0.381838
  },
  {
    "Latitude": 53.732526,
    "Longitude": -0.381762
  },
  {
    "Latitude": 53.732156,
    "Longitude": -0.381722
  },
  {
    "Latitude": 53.731976,
    "Longitude": -0.381718
  },
  {
    "Latitude": 53.731854,
    "Longitude": -0.381728
  }
]
Liam
  • 439
  • 1
  • 4
  • 26
  • What does a curve of e.g. 2.0 or -5.7 mean though? Are you essentiall;y asking for something like https://stackoverflow.com/questions/7811761/smoothing-a-2d-line-from-an-array-of-points – Caius Jard Aug 20 '21 at 16:03
  • Hi, that link is about smoothing out lines if nodes are too close together, which is completely different. 2.0 would mean that it's turning in one direction and -5.7 would mean it's turning in the other direction. I've uploaded a better picture. – Liam Aug 20 '21 at 16:11
  • But what significance the magnitude? If I write a code that declares your curve to be -2312375.234234, what does it mean ? As is, I don't think your question can (or should) be answered, because it's basically "here's an incomplete spec, please do my work for me". Smoothing an array of line segments into a set of curves is not "completely different", it's actually what you're apparently asking for – Caius Jard Aug 20 '21 at 16:20
  • When you say smoothing of lines, I think of Douglas-Peucker algorithm, where you remove redundant nodes in an array. What I'm asking for is.. if a train is moving along a track and it starts to turn right, what is the angle between when it started to turn right and either going straight again or turns left. I just don't know the maths or pseudo code that's required and without that, how does someone know where to look. I'm a software developer not a mathematician so it's more difficult to write the requirements. – Liam Aug 20 '21 at 16:39
  • I suppose, algorithmically, if you're standing at any given point N then you should calculate the heading of the next point N+1 with reference to the line from the previous point N-1 to the current. You will then know several things: whether the next point is a change of heading - if not then you're on a constant curve, if so then b) whether the change is more the right or more to the left and hence (based on whether you were already going right or left) whether it's steeper or lesser, and whether it represents a flip from going-right to going-left. Then maybe just collapse your sequences – Caius Jard Aug 20 '21 at 16:45
  • according to some rule, so that you have 5 "right, getting tighter" in a row, becoming 4 "right, no change" becoming 2 "right, getting wider", becoming 4 "left getting tighter" ... and you collapse the repetition of the nubmers leaving jsut the description in quotes as "the type of curve" .. you don't even need to draw curves.. you're just having 100 points, making 99 descriptions and then removing repetitions of the same description. If you want to remove minor left/right/left/right flips to "mostly straight" quantize the heading/bearing so that e.g. ~0.1 flips are rounded to 0 – Caius Jard Aug 20 '21 at 16:47

1 Answers1

1

Assuming you are satisfied with modeling your path as a sequence of line segments, you can simply calculate the curvature by calculating the signed angle between neighboring line segments.

in pseudo code

for(var i = 1; i < points.Length - 1; i++){
    var a = (points[i] - points[i-1]).Normalized;
    var b = (points[i+1] - points[i]).Normalized;
    // calculate signed angle between vectors
    angle = atan2( a.x*b.y - a.y*b.x, a.x*b.x + a.y*b.y );
}

Code for calculating signed angle from finding signed angle between vectors

If you want to have a smooth interpolation, like a Bézier spline, you can still create it, split it into shorter line segments and use the method above. But if you want a single curvature value for a smooth segment you need to define what that value should be. since smooth functions will have a continuous range of curvatures.

Note that this assumes planar coordinates, if you have lat/long you might want to re-project to a planar coordinate system, potentially one for each line segment pair.

JonasH
  • 28,608
  • 2
  • 10
  • 23