core/src/main/kotlin/ai/neuon/utility/distance/Distance.kt

@@ -1,10 +1,6 @@
 package ai.neuon.utility.distance
 
-import kotlin.math.abs
+import kotlin.math.*
-import kotlin.math.asin
-import kotlin.math.cos
-import kotlin.math.sin
-import kotlin.math.sqrt
 
 object Distance {
     /**
@@ -26,4 +22,31 @@ object Distance {
         )
         return abs(distance)
     }
+
+    /**
+     * @return 3D distance between 2 points in kilometers.
+     *
+     * @param lat1 Latitude of point 1
+     * @param lng1 Longitude of point 1
+     * @param alt1 Altitude of point 1 in meters (nullable)
+     * @param lat2 Latitude of point 2
+     * @param lng2 Longitude of point 2
+     * @param alt2 Altitude of point 2 in meters (nullable)
+     */
+    fun measure3D(
+        lat1: Double, lng1: Double, alt1: Double?,
+        lat2: Double, lng2: Double, alt2: Double?,
+    ): Double {
+
+        val horizontalKm = measure2Points(lat1, lng1, lat2, lng2) // existing 2D function
+
+        val verticalM = if (alt1 != null && alt2 != null) {
+            kotlin.math.abs(alt2 - alt1)
+        } else 0.0
+
+        val horizontalM = horizontalKm * 1000
+        val distanceM = sqrt(horizontalM * horizontalM + verticalM * verticalM)
+
+        return distanceM / 1000 // return km
+    }
 }