229 lines
8.8 KiB
Dart
229 lines
8.8 KiB
Dart
import 'dart:math' as math;
|
|
import '../models/landmark.dart';
|
|
|
|
class ResectionCalculator {
|
|
static const double earthRadiusKm = 6371.0;
|
|
static const double magneticDeclinationJordan = 5.5; // +5.5° East avg in Jordan
|
|
|
|
/// Calculates true azimuth from magnetic compass heading.
|
|
/// When Magnetic North is East of True North (Jordan ~ +5.5° East),
|
|
/// True Grid Azimuth = (Magnetic Heading - Declination).
|
|
static double getTrueAzimuth(double magneticHeadingDeg, [double customOffset = 5.5]) {
|
|
return (magneticHeadingDeg - customOffset + 360.0) % 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;
|
|
|
|
// Base angular GDOP for 2 landmarks based on cut angle (sin(det)) and landmark distances
|
|
final cutAngleSin = det.abs();
|
|
final distADeg = math.sqrt(math.pow((xIntersect - xA), 2) + math.pow((yIntersect - yA), 2));
|
|
final distBDeg = math.sqrt(math.pow((xIntersect - xB), 2) + math.pow((yIntersect - yB), 2));
|
|
final approxDistMeters = ((distADeg + distBDeg) / 2.0) * 111320.0;
|
|
|
|
// Compass measurement uncertainty (~1.5 deg = ~0.026 rad) scaled by GDOP (1 / sin(cutAngle))
|
|
double estimatedAccuracy = math.max(10.0, (approxDistMeters * 0.026) / math.max(0.2, cutAngleSin));
|
|
|
|
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;
|
|
// Solve intersection of A and C
|
|
final detAC = sinA * cosC - cosA * sinC;
|
|
|
|
if (detBC.abs() >= 0.0001 && detAC.abs() >= 0.0001) {
|
|
final dxBC = xC - xB;
|
|
final dyBC = yC - yB;
|
|
final tB = (dxBC * cosC - dyBC * sinC) / detBC;
|
|
final xBC = xB + tB * sinB;
|
|
final yBC = yB + tB * cosB;
|
|
|
|
final dxAC = xC - xA;
|
|
final dyAC = yC - yA;
|
|
final tAc = (dxAC * cosC - dyAC * sinC) / detAC;
|
|
final xAC = xA + tAc * sinA;
|
|
final yAC = yA + tAc * cosA;
|
|
|
|
// Centroid of the classic military "Triangle of Error" (مثلث الخطأ)
|
|
final xCentroid = (xIntersect + xBC + xAC) / 3.0;
|
|
final yCentroid = (yIntersect + yBC + yAC) / 3.0;
|
|
|
|
calcLng = xCentroid / cosLat;
|
|
calcLat = yCentroid;
|
|
|
|
// Compute actual radius of Triangle of Error in meters
|
|
final d1 = math.sqrt(math.pow(xIntersect - xCentroid, 2) + math.pow(yIntersect - yCentroid, 2));
|
|
final d2 = math.sqrt(math.pow(xBC - xCentroid, 2) + math.pow(yBC - yCentroid, 2));
|
|
final d3 = math.sqrt(math.pow(xAC - xCentroid, 2) + math.pow(yAC - yCentroid, 2));
|
|
final triangleRadiusMeters = ((d1 + d2 + d3) / 3.0) * 111320.0;
|
|
|
|
// Estimated accuracy is bound by triangle size + residual optical error
|
|
estimatedAccuracy = math.max(5.0, double.parse(triangleRadiusMeters.toStringAsFixed(1)));
|
|
}
|
|
}
|
|
|
|
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;
|
|
}
|
|
|
|
/// Great-circle distance between two points in meters.
|
|
static double haversineDistance(double lat1, double lon1, double lat2, double lon2) {
|
|
return haversineDistanceKm(lat1, lon1, lat2, lon2) * 1000.0;
|
|
}
|
|
|
|
/// Calculate forward azimuth/bearing in degrees (0-360°) from point 1 to point 2.
|
|
static double calculateBearing(double lat1, double lon1, double lat2, double lon2) {
|
|
final dLon = (lon2 - lon1) * (math.pi / 180.0);
|
|
final lat1Rad = lat1 * (math.pi / 180.0);
|
|
final lat2Rad = lat2 * (math.pi / 180.0);
|
|
|
|
final y = math.sin(dLon) * math.cos(lat2Rad);
|
|
final x = math.cos(lat1Rad) * math.sin(lat2Rad) -
|
|
math.sin(lat1Rad) * math.cos(lat2Rad) * math.cos(dLon);
|
|
final bearingRad = math.atan2(y, x);
|
|
return (bearingRad * (180.0 / math.pi) + 360.0) % 360.0;
|
|
}
|
|
|
|
/// Single Landmark Rangefinder (Parallax Triangulation from baseline step-off)
|
|
/// حساب المسافة والإحداثيات من معلم جغرافي وحيد بالتحرك على خط أساس
|
|
///
|
|
/// [landmark]: المعلم الجغرافي المرصود
|
|
/// [azimuth1Deg]: زاوية الرصد الأولى (بالدرجات)
|
|
/// [azimuth2Deg]: زاوية الرصد الثانية بعد التحرك (بالدرجات)
|
|
/// [baselineMeters]: مسافة التحرك العمودي على خط النظر (مثلاً 10م، 11م، 20م، 50م)
|
|
static ResectionResult? calculateSingleLandmarkPolar({
|
|
required TacticalLandmark landmark,
|
|
required double azimuth1Deg,
|
|
required double azimuth2Deg,
|
|
required double baselineMeters,
|
|
}) {
|
|
final trueAzimuth1 = getTrueAzimuth(azimuth1Deg);
|
|
final trueAzimuth2 = getTrueAzimuth(azimuth2Deg);
|
|
|
|
// Angular parallax difference in degrees
|
|
var deltaAngleDeg = (trueAzimuth2 - trueAzimuth1).abs();
|
|
if (deltaAngleDeg > 180.0) {
|
|
deltaAngleDeg = 360.0 - deltaAngleDeg;
|
|
}
|
|
|
|
if (deltaAngleDeg < 0.05) {
|
|
return null;
|
|
}
|
|
|
|
final deltaAngleRad = deltaAngleDeg * (math.pi / 180.0);
|
|
|
|
// Exact trigonometric range: D = Baseline / tan(deltaAngle)
|
|
final distanceMeters = baselineMeters / math.tan(deltaAngleRad);
|
|
final distanceKm = distanceMeters / 1000.0;
|
|
|
|
// Project observer position from Landmark along Back-Azimuth
|
|
final backBearingDeg = (trueAzimuth1 + 180.0) % 360.0;
|
|
final backBearingRad = backBearingDeg * (math.pi / 180.0);
|
|
|
|
final latRad = landmark.lat * (math.pi / 180.0);
|
|
final angularDist = distanceKm / earthRadiusKm;
|
|
|
|
final obsLatRad = math.asin(
|
|
math.sin(latRad) * math.cos(angularDist) +
|
|
math.cos(latRad) * math.sin(angularDist) * math.cos(backBearingRad)
|
|
);
|
|
|
|
final obsLngRad = (landmark.lng * (math.pi / 180.0)) +
|
|
math.atan2(
|
|
math.sin(backBearingRad) * math.sin(angularDist) * math.cos(latRad),
|
|
math.cos(angularDist) - math.sin(latRad) * math.sin(obsLatRad)
|
|
);
|
|
|
|
final calcLat = obsLatRad * (180.0 / math.pi);
|
|
final calcLng = obsLngRad * (180.0 / math.pi);
|
|
|
|
final obs = ResectionObservation(
|
|
landmark: landmark,
|
|
observedAzimuthDeg: azimuth1Deg,
|
|
trueAzimuthDeg: trueAzimuth1,
|
|
);
|
|
|
|
return ResectionResult(
|
|
lat: double.parse(calcLat.toStringAsFixed(6)),
|
|
lng: double.parse(calcLng.toStringAsFixed(6)),
|
|
estimatedAccuracyMeters: math.max(15.0, distanceMeters * 0.03),
|
|
observations: [obs],
|
|
distanceToLandmarksKm: {landmark.id: distanceKm},
|
|
);
|
|
}
|
|
}
|