Keep curvature calculations finite for valid road geometry

math/curvature.go

@@ -10,28 +10,38 @@ type Curvature struct {
 }
 
 func CalculateCurvature(a Position, b Position, c Position) Curvature {
-	lengthA := a.DistanceTo(b)
-	lengthB := a.DistanceTo(c)
-	lengthC := b.DistanceTo(c)
-
-	sp := (lengthA + lengthB + lengthC) / 2
-
-	area := float32(m.Sqrt(float64(sp * (sp - lengthA) * (sp - lengthB) * (sp - lengthC))))
-
-	lengthProd := lengthA * lengthB * lengthC
-	if lengthProd == 0 {
+	distanceAB := a.distanceTo(b)
+	distanceAC := a.distanceTo(c)
+	distanceBC := b.distanceTo(c)
+	distanceProduct := distanceAB * distanceAC * distanceBC
+	if distanceProduct == 0 {
 		return Curvature{Pos: b}
 	}
 
-	res := Curvature{Pos: b}
-	res.Curvature = float64((4 * area) / lengthProd)
-	radius := 1.0 / res.Curvature
-
-	num := (m.Pow(radius, 2)*2 - m.Pow(float64(lengthB), 2))
-	den := (2 * m.Pow(radius, 2))
-	res.Angle = m.Acos(num / den)
+	longestSide, middleSide, shortestSide := distanceAB, distanceAC, distanceBC
+	if longestSide < middleSide {
+		longestSide, middleSide = middleSide, longestSide
+	}
+	if longestSide < shortestSide {
+		longestSide, shortestSide = shortestSide, longestSide
+	}
+	if middleSide < shortestSide {
+		middleSide, shortestSide = shortestSide, middleSide
+	}
 
-	res.ArcLength = radius * res.Angle
+	// Sorted-side Heron arithmetic reduces cancellation for nearly straight roads.
+	areaProduct := (longestSide + (middleSide + shortestSide)) *
+		(shortestSide - (longestSide - middleSide)) *
+		(shortestSide + (longestSide - middleSide)) *
+		(longestSide + (middleSide - shortestSide))
+	res := Curvature{Pos: b}
+	res.Curvature = m.Sqrt(max(0, areaProduct)) / distanceProduct
+	if res.Curvature == 0 {
+		res.ArcLength = distanceAC
+		return res
+	}
 
+	res.Angle = 2 * m.Asin(min(1, distanceAC*res.Curvature/2))
+	res.ArcLength = res.Angle / res.Curvature
 	return res
 }

math/position.go

@@ -37,12 +37,17 @@ func (p *Position) Lon() float64 {
 }
 
 func (p *Position) DistanceTo(end Position) float32 {
+	return float32(p.distanceTo(end))
+}
+
+func (p *Position) distanceTo(end Position) float64 {
 	latDiff := end.LatRad() - p.LatRad()
 	lonDiff := end.LonRad() - p.LonRad()
 	a := m.Pow(m.Sin(latDiff/2), 2) + m.Cos(p.LatRad())*m.Cos(end.LatRad())*m.Pow(m.Sin(lonDiff/2), 2)
+	a = min(1, a)
 	c := 2 * m.Atan2(m.Sqrt(a), m.Sqrt(1-a))
 
-	return float32(ms.R * c) // in metres
+	return ms.R * c // in metres
 }
 
 func (p *Position) Subtract(other Position) Position {