117 lines
4.0 KiB
Dart
117 lines
4.0 KiB
Dart
import 'dart:math' as math;
|
|
import '../models/landmark.dart';
|
|
|
|
class ResectionCalculator {
|
|
static const double earthRadiusKm = 6371.0;
|
|
static const double magneticDeclinationJordan = 4.8; // +4.8° East avg in Jordan
|
|
|
|
/// Calculates true azimuth from magnetic compass heading.
|
|
static double getTrueAzimuth(double magneticHeadingDeg) {
|
|
return (magneticHeadingDeg + magneticDeclinationJordan) % 360.0;
|
|
}
|
|
|
|
/// Calculates observer position from 2 or more landmark observations using triangulation resection.
|
|
static ResectionResult? calculatePosition(List<ResectionObservation> observations) {
|
|
if (observations.length < 2) return null;
|
|
|
|
// Use first two observations for base intersection
|
|
final obsA = observations[0];
|
|
final obsB = observations[1];
|
|
|
|
final backBearingA = ((obsA.trueAzimuthDeg + 180.0) % 360.0) * (math.pi / 180.0);
|
|
final backBearingB = ((obsB.trueAzimuthDeg + 180.0) % 360.0) * (math.pi / 180.0);
|
|
|
|
final latA = obsA.landmark.lat;
|
|
final lngA = obsA.landmark.lng;
|
|
final latB = obsB.landmark.lat;
|
|
final lngB = obsB.landmark.lng;
|
|
|
|
final latMidRad = ((latA + latB) / 2.0) * (math.pi / 180.0);
|
|
final cosLat = math.cos(latMidRad);
|
|
|
|
final xA = lngA * cosLat;
|
|
final yA = latA;
|
|
final xB = lngB * cosLat;
|
|
final yB = latB;
|
|
|
|
final sinA = math.sin(backBearingA);
|
|
final cosA = math.cos(backBearingA);
|
|
final sinB = math.sin(backBearingB);
|
|
final cosB = math.cos(backBearingB);
|
|
|
|
final det = sinA * cosB - cosA * sinB;
|
|
if (det.abs() < 0.0001) {
|
|
// Lines are parallel or collinear
|
|
return null;
|
|
}
|
|
|
|
final dx = xB - xA;
|
|
final dy = yB - yA;
|
|
final tA = (dx * cosB - dy * sinB) / det;
|
|
|
|
final xIntersect = xA + tA * sinA;
|
|
final yIntersect = yA + tA * cosA;
|
|
|
|
var calcLng = xIntersect / cosLat;
|
|
var calcLat = yIntersect;
|
|
|
|
// If 3rd landmark observation exists, calculate centroid / weighted least-squares refinement
|
|
double estimatedAccuracy = 50.0; // Base 50m accuracy for 2 landmarks
|
|
|
|
if (observations.length >= 3) {
|
|
final obsC = observations[2];
|
|
final backBearingC = ((obsC.trueAzimuthDeg + 180.0) % 360.0) * (math.pi / 180.0);
|
|
final latC = obsC.landmark.lat;
|
|
final lngC = obsC.landmark.lng;
|
|
final xC = lngC * cosLat;
|
|
final yC = latC;
|
|
final sinC = math.sin(backBearingC);
|
|
final cosC = math.cos(backBearingC);
|
|
|
|
// Solve intersection of B and C
|
|
final detBC = sinB * cosC - cosB * sinC;
|
|
if (detBC.abs() >= 0.0001) {
|
|
final dxBC = xC - xB;
|
|
final dyBC = yC - yB;
|
|
final tB = (dxBC * cosC - dyBC * sinC) / detBC;
|
|
final xIntersectBC = xB + tB * sinB;
|
|
final yIntersectBC = yB + tB * cosB;
|
|
|
|
// Weighted centroid average of the triangle of error
|
|
calcLng = ((xIntersect + xIntersectBC) / 2.0) / cosLat;
|
|
calcLat = (yIntersect + yIntersectBC) / 2.0;
|
|
|
|
// 3 sightings tighten the error ellipse to ~20-30 meters
|
|
estimatedAccuracy = 25.0;
|
|
}
|
|
}
|
|
|
|
final distances = <String, double>{};
|
|
for (final obs in observations) {
|
|
final d = haversineDistanceKm(calcLat, calcLng, obs.landmark.lat, obs.landmark.lng);
|
|
distances[obs.landmark.id] = d;
|
|
}
|
|
|
|
return ResectionResult(
|
|
lat: double.parse(calcLat.toStringAsFixed(6)),
|
|
lng: double.parse(calcLng.toStringAsFixed(6)),
|
|
estimatedAccuracyMeters: estimatedAccuracy,
|
|
observations: observations,
|
|
distanceToLandmarksKm: distances,
|
|
);
|
|
}
|
|
|
|
/// Haversine formula to compute great-circle distance between two GPS coordinates in kilometers.
|
|
static double haversineDistanceKm(double lat1, double lon1, double lat2, double lon2) {
|
|
final dLat = (lat2 - lat1) * (math.pi / 180.0);
|
|
final dLon = (lon2 - lon1) * (math.pi / 180.0);
|
|
final a = math.sin(dLat / 2) * math.sin(dLat / 2) +
|
|
math.cos(lat1 * (math.pi / 180.0)) *
|
|
math.cos(lat2 * (math.pi / 180.0)) *
|
|
math.sin(dLon / 2) *
|
|
math.sin(dLon / 2);
|
|
final c = 2 * math.atan2(math.sqrt(a), math.sqrt(1 - a));
|
|
return earthRadiusKm * c;
|
|
}
|
|
}
|