262 lines
10 KiB
Swift
262 lines
10 KiB
Swift
import CoreLocation
|
|
|
|
extension Array where Element == CLLocation {
|
|
|
|
/**
|
|
Sample the locations using a given time interval.
|
|
*/
|
|
func samplePeriodically(at interval: TimeInterval) -> [CLLocation] {
|
|
guard interval > 0 else { return [] }
|
|
guard let start = first, let end = last else { return self }
|
|
let totalTime = end.timestamp.timeIntervalSince(start.timestamp)
|
|
let numberOfSamples = Int((totalTime / interval).rounded(.up))
|
|
return periodicSamples(interval: interval, numberOfSamples: numberOfSamples)
|
|
}
|
|
|
|
/**
|
|
Sample the locations at a fixed period determined by the number of desired sampels
|
|
*/
|
|
func samplePeriodically(numberOfSamples: Int) -> [CLLocation] {
|
|
guard numberOfSamples > 0 else { return [] }
|
|
guard let start = first, let end = last else { return self }
|
|
let totalTime = end.timestamp.timeIntervalSince(start.timestamp)
|
|
let timeInterval = totalTime / TimeInterval(count - 1)
|
|
return periodicSamples(interval: timeInterval, numberOfSamples: numberOfSamples)
|
|
}
|
|
|
|
private func periodicSamples(interval: TimeInterval, numberOfSamples: Int) -> [CLLocation] {
|
|
guard let start = first else { return [] }
|
|
var currentIndex = 0
|
|
var currentTime = start.timestamp
|
|
|
|
var samples = [start]
|
|
for _ in 1..<numberOfSamples {
|
|
currentTime = currentTime.addingTimeInterval(interval)
|
|
while true {
|
|
let nextIndex = currentIndex + 1
|
|
if nextIndex >= count { break }
|
|
let nextTime = self[nextIndex].timestamp
|
|
if nextTime > currentTime { break }
|
|
currentIndex += 1
|
|
}
|
|
if currentIndex + 1 == count {
|
|
samples.append(self[currentIndex])
|
|
} else {
|
|
let before = self[currentIndex]
|
|
let after = self[currentIndex + 1]
|
|
let interpolated = before.interpolate(currentTime, to: after)
|
|
samples.append(interpolated)
|
|
}
|
|
}
|
|
return samples
|
|
}
|
|
|
|
/// Computes path length by moving along center-to-center lines, intersecting uncertainty spheres
|
|
func minimumTraveledDistance3D() -> CLLocationDistance {
|
|
guard count > 1 else { return 0 }
|
|
|
|
// Remove the uncertainty radius of the first location
|
|
var current = self.first!
|
|
var totalDistance: CLLocationDistance = -current.uncertaintyRadius3D
|
|
for next in self[1...] {
|
|
let (movement, point) = current.minimumDistance(to: next)
|
|
current = point
|
|
totalDistance += movement
|
|
}
|
|
return totalDistance
|
|
}
|
|
|
|
/// Calculates the minimum possible ascended altitude (meters),
|
|
/// considering vertical accuracy as an uncertainty interval.
|
|
func minimumAscendedAltitude() -> CLLocationDistance {
|
|
guard let first = self.first else { return 0 }
|
|
|
|
// Start with the highest possible value of the first point
|
|
var currentAltitude = first.altitude + first.verticalAccuracy
|
|
var ascended: CLLocationDistance = 0
|
|
|
|
for next in self.dropFirst() {
|
|
let newMin = next.altitude - next.verticalAccuracy
|
|
let newMax = next.altitude + next.verticalAccuracy
|
|
|
|
if newMin > currentAltitude {
|
|
// Lower bound must be adjusted
|
|
ascended += newMin - currentAltitude
|
|
currentAltitude = newMin
|
|
} else if newMax < currentAltitude {
|
|
// Upper bound must be adjusted
|
|
currentAltitude = newMax
|
|
}
|
|
}
|
|
return ascended
|
|
}
|
|
|
|
/// Calculates the minimum possible ascended altitude (meters),
|
|
/// considering a given vertical accuracy threshold
|
|
func minimumAscendedAltitude(threshold: CLLocationDistance) -> CLLocationDistance {
|
|
guard let first = self.first else { return 0 }
|
|
|
|
// Start with the highest possible value of the first point
|
|
var currentAltitude = first.altitude + threshold
|
|
var ascended: CLLocationDistance = 0
|
|
|
|
for next in self.dropFirst() {
|
|
let newMin = next.altitude - threshold
|
|
let newMax = next.altitude + threshold
|
|
|
|
if newMin > currentAltitude {
|
|
// Lower bound must be adjusted
|
|
ascended += newMin - currentAltitude
|
|
currentAltitude = newMin
|
|
} else if newMax < currentAltitude {
|
|
// Upper bound must be adjusted
|
|
currentAltitude = newMax
|
|
}
|
|
}
|
|
return ascended
|
|
}
|
|
|
|
func interpolateAltitudes(
|
|
from startDate: Date,
|
|
to endDate: Date
|
|
) -> [CLLocation] {
|
|
|
|
// Ensure valid range
|
|
guard startDate < endDate else { return self }
|
|
|
|
// Find first and last locations in the window
|
|
guard
|
|
let startLocation = first(where: { $0.timestamp >= startDate }),
|
|
let endLocation = last(where: { $0.timestamp <= endDate })
|
|
else {
|
|
return self // No valid range found
|
|
}
|
|
|
|
let startAltitude = startLocation.altitude
|
|
let endAltitude = endLocation.altitude
|
|
let duration = endDate.timeIntervalSince(startDate)
|
|
|
|
return map { loc in
|
|
let t = loc.timestamp.timeIntervalSince1970
|
|
|
|
if loc.timestamp >= startDate && loc.timestamp <= endDate {
|
|
let progress = (loc.timestamp.timeIntervalSince(startDate)) / duration
|
|
let newAltitude = startAltitude + progress * (endAltitude - startAltitude)
|
|
|
|
return CLLocation(
|
|
coordinate: loc.coordinate,
|
|
altitude: newAltitude,
|
|
horizontalAccuracy: loc.horizontalAccuracy,
|
|
verticalAccuracy: loc.verticalAccuracy,
|
|
course: loc.course,
|
|
speed: loc.speed,
|
|
timestamp: loc.timestamp
|
|
)
|
|
} else {
|
|
return loc // outside window, unchanged
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
extension CLLocation {
|
|
|
|
/// Combined uncertainty sphere radius (meters) from horizontal+vertical accuracy
|
|
var uncertaintyRadius3D: CLLocationDistance {
|
|
let h = max(0, horizontalAccuracy)
|
|
let v = max(0, verticalAccuracy)
|
|
return sqrt(h * h + v * v)
|
|
}
|
|
|
|
func verticalDistance(from other: CLLocation) -> CLLocationDistance {
|
|
abs(self.altitude - other.altitude)
|
|
}
|
|
|
|
func minimumDistance(to other: CLLocation) -> (distance: CLLocationDistance, point: CLLocation) {
|
|
let horizontalDistance = distance(from: other)
|
|
let horizontalMovement = Swift.max(0, horizontalDistance - Swift.max(0, other.horizontalAccuracy))
|
|
|
|
let latitude: CLLocationDegrees
|
|
let longitude: CLLocationDegrees
|
|
if horizontalDistance == 0 || horizontalMovement == 0 {
|
|
latitude = coordinate.latitude
|
|
longitude = coordinate.longitude
|
|
} else {
|
|
let horizontalRatio = horizontalMovement / horizontalDistance
|
|
latitude = coordinate.latitude.move(horizontalRatio, to: other.coordinate.latitude)
|
|
longitude = coordinate.longitude.move(horizontalRatio, to: other.coordinate.longitude)
|
|
}
|
|
|
|
let verticalDistance = verticalDistance(from: other)
|
|
let verticalMovement = Swift.max(0, verticalDistance - Swift.max(0, other.verticalAccuracy))
|
|
|
|
let altitude: CLLocationDistance
|
|
if verticalDistance == 0 || verticalMovement == 0 {
|
|
altitude = self.altitude
|
|
} else {
|
|
let verticalRatio = verticalMovement / verticalDistance
|
|
altitude = self.altitude.move(verticalRatio, to: other.altitude)
|
|
}
|
|
|
|
let movement = sqrt(horizontalMovement * horizontalMovement + verticalMovement * verticalMovement)
|
|
let point = CLLocation(
|
|
coordinate: .init(latitude: latitude, longitude: longitude),
|
|
altitude: altitude,
|
|
horizontalAccuracy: 0,
|
|
verticalAccuracy: 0,
|
|
timestamp: other.timestamp
|
|
)
|
|
return (movement, point)
|
|
}
|
|
|
|
func interpolate(_ time: Date, to other: CLLocation) -> CLLocation {
|
|
if self.timestamp > other.timestamp {
|
|
return other.interpolate(time, to: self)
|
|
}
|
|
let totalDuration = other.timestamp.timeIntervalSince(self.timestamp)
|
|
if totalDuration == 0 { return move(0.5, to: other) }
|
|
let ratio = time.timeIntervalSince(self.timestamp) / totalDuration
|
|
return move(ratio, to: other)
|
|
}
|
|
|
|
func move(_ ratio: Double, to other: CLLocation) -> CLLocation {
|
|
if ratio <= 0 { return self }
|
|
if ratio >= 1 { return other }
|
|
|
|
let time = timestamp.addingTimeInterval(other.timestamp.timeIntervalSince(timestamp) * ratio)
|
|
|
|
return CLLocation(
|
|
coordinate: .init(
|
|
latitude: coordinate.latitude.move(ratio, to: other.coordinate.latitude),
|
|
longitude: coordinate.longitude.move(ratio, to: other.coordinate.longitude)),
|
|
altitude: altitude.move(ratio, to: other.altitude),
|
|
horizontalAccuracy: move(from: horizontalAccuracy, to: other.horizontalAccuracy, by: ratio),
|
|
verticalAccuracy: move(from: verticalAccuracy, to: other.verticalAccuracy, by: ratio),
|
|
course: move(from: course, to: other.course, by: ratio),
|
|
courseAccuracy: move(from: courseAccuracy, to: other.courseAccuracy, by: ratio),
|
|
speed: move(from: speed, to: other.speed, by: ratio),
|
|
speedAccuracy: move(from: speedAccuracy, to: other.speedAccuracy, by: ratio),
|
|
timestamp: time)
|
|
}
|
|
|
|
private func move(from source: Double, to other: Double, by ratio: Double) -> Double {
|
|
if source == -1 {
|
|
return other
|
|
}
|
|
if other == -1 {
|
|
return source
|
|
}
|
|
return source.move(ratio, to: other)
|
|
}
|
|
}
|
|
|
|
extension Double {
|
|
|
|
/**
|
|
Move to a different value by the given ratio of their distance.
|
|
*/
|
|
func move(_ ratio: Double, to other: Double) -> Double {
|
|
self + (other - self) * ratio
|
|
}
|
|
}
|