From d7c5918396e2ad9c6c686864f538978522be432e Mon Sep 17 00:00:00 2001
From: FrogAi <91348155+FrogAi@users.noreply.github.com>
Date: Sun, 9 Aug 2026 19:51:55 -0700
Subject: [PATCH] Keep curvature geometry finite

---
 math/curvature.go | 37 ++++++++++++++++++++++++-------------
 math/position.go  |  9 +++++++--
 2 files changed, 31 insertions(+), 15 deletions(-)

diff --git a/math/curvature.go b/math/curvature.go
index bdfc9d9..623f5f7 100644
--- a/math/curvature.go
+++ b/math/curvature.go
@@ -10,13 +10,9 @@ 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))))
+	lengthA := a.distanceTo(b)
+	lengthB := a.distanceTo(c)
+	lengthC := b.distanceTo(c)
 
 	lengthProd := lengthA * lengthB * lengthC
 	if lengthProd == 0 {
@@ -24,14 +20,29 @@ func CalculateCurvature(a Position, b Position, c Position) Curvature {
 	}
 
 	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)
+	x, y, z := lengthA, lengthB, lengthC
+	if x < y {
+		x, y = y, x
+	}
+	if x < z {
+		x, z = z, x
+	}
+	if y < z {
+		y, z = z, y
+	}
+
+	areaProduct := (x + (y + z)) * (z - (x - y)) * (z + (x - y)) * (x + (y - z))
+	if areaProduct <= 0 {
+		res.ArcLength = lengthB
+		return res
+	}
+
+	area := 0.25 * m.Sqrt(areaProduct)
+	res.Curvature = 4 * area / lengthProd
 
-	res.ArcLength = radius * res.Angle
+	res.Angle = 2 * m.Asin(m.Min(1, lengthB*res.Curvature/2))
+	res.ArcLength = res.Angle / res.Curvature
 
 	return res
 }
diff --git a/math/position.go b/math/position.go
index 3a7063b..3eb5a6a 100644
--- a/math/position.go
+++ b/math/position.go
@@ -36,13 +36,18 @@ func (p *Position) Lon() float64 {
 	return p.longitudeDeg
 }
 
-func (p *Position) DistanceTo(end Position) float32 {
+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 = m.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
+}
+
+func (p *Position) DistanceTo(end Position) float32 {
+	return float32(p.distanceTo(end)) // in metres
 }
 
 func (p *Position) Subtract(other Position) Position {