PhysiGo/app/lib/navigation/utils/geometry_utils.dart

import 'dart:math';
import 'package:latlong2/latlong.dart';

class Point3D {
  final num x;
  final num y;
  final num z;
  static const _earthRadiusInMeter = 6371 * 1000;

  const Point3D({required this.x, required this.y, required this.z});

  // From https://stackoverflow.com/questions/1185408/converting-from-longitude-latitude-to-cartesian-coordinates
  // (because I forgot basic geometry...)
  factory Point3D.fromLatLng(LatLng point) {
    final latInRadian = point.latitude * pi / 180;
    final longInRadian = point.longitude * pi / 180;
    return Point3D(
      x: _earthRadiusInMeter * cos(latInRadian) * cos(longInRadian),
      y: _earthRadiusInMeter * cos(latInRadian) * sin(longInRadian),
      z: _earthRadiusInMeter * sin(latInRadian),
    );
  }

  Point3D operator +(Point3D other) {
    return Point3D(x: x + other.x, y: y + other.y, z: z + other.z);
  }

  Point3D operator -(Point3D other) {
    return Point3D(x: x - other.x, y: y - other.y, z: z - other.z);
  }

  Point3D operator *(num value) {
    return Point3D(x: x * value, y: y * value, z: z * value);
  }
}

class Segment {
  final LatLng start;
  final LatLng end;

  const Segment({required this.start, required this.end});
}

// Implementation found from StackOverflow answer https://stackoverflow.com/questions/849211/shortest-distance-between-a-point-and-a-line-segment
class DistanceUtils {
  /// Distance from [point] to [segment] in meter
  static num distToSegment(LatLng point, Segment segment) {
    final p = Point3D.fromLatLng(point);
    final v = Point3D.fromLatLng(segment.start);
    final w = Point3D.fromLatLng(segment.end);
    final segmentLength = _sqrDistBetweenPoints(v, w);

    // Case v == w
    if (segmentLength == 0) {
      return sqrt(_sqrDistBetweenPoints(p, v));
    }

    // If dotProduct is between 0 and 1, then the projected point is on the segment
    // and we take the distance between p and the projected point
    // Else, we take the distance between p and the nearer end
    final t = max(0, min(1, _dotProduct(p - v, w - v) / segmentLength));

    final projection = v + (w - v) * t;
    return sqrt(_sqrDistBetweenPoints(p, projection));
  }

  /// Distance between [point1] and [point2] in meter
  static num distBetweenTwoPoints(LatLng point1, LatLng point2) {
    final v = Point3D.fromLatLng(point1);
    final w = Point3D.fromLatLng(point2);

    return sqrt(_sqrDistBetweenPoints(v, w));
  }

  /// If parameter is between 0 and 1, the projected point is on the segment
  ///
  /// If parameter is greater than 1, the projected point is after the end of the segment
  ///
  /// If parameter is lesser than 0, the projected point is before the start of the segment
  static num getParameterFromProjection(LatLng point, Segment segment) {
    final p = Point3D.fromLatLng(point);
    final v = Point3D.fromLatLng(segment.start);
    final w = Point3D.fromLatLng(segment.end);
    final segmentLength = _sqrDistBetweenPoints(v, w);

    // Case v == w
    if (segmentLength == 0) {
      return 0;
    }

    return _dotProduct(p - v, w - v) / segmentLength;
  }

  static num _sqr(num x) {
    return x * x;
  }

  static num _sqrDistBetweenPoints(Point3D v, Point3D w) {
    return _sqr(v.x - w.x) + _sqr(v.y - w.y) + _sqr(v.z - w.z);
  }

  static num _dotProduct(Point3D v, Point3D w) {
    return (v.x * w.x) + (v.y * w.y) + (v.z * w.z);
  }
}