Skip to content

Commit

Permalink
AP_Math: WIP polygon tests
Browse files Browse the repository at this point in the history
  • Loading branch information
@andyp1per
andyp1per committed Dec 20, 2024
1 parent 1bca12a commit fe7a31c
Show file tree
Hide file tree
Showing 2 changed files with 161 additions and 23 deletions.
130 changes: 110 additions & 20 deletions libraries/AP_Math/polygon.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -118,26 +118,6 @@ bool Polygon_complete(const Vector2<T> *V, unsigned n)
return (n >= 4 && V[n-1] == V[0]);
}

/*
return the closest distance that point p comes to an edge of closed
polygon V, defined by N points
*/
template <typename T>
float Polygon_closest_distance_point(const Vector2<T> *V, unsigned N, const Vector2<T> &p)
{
T closest_sq = std::numeric_limits<T>::max();
for (uint8_t i=0; i<N-1; i++) {
const Vector2<T> &v1 = V[i];
const Vector2<T> &v2 = V[i+1];

T dist_sq = Vector2<T>::closest_distance_between_line_and_point_squared(v1, v2, p);
if (dist_sq < closest_sq) {
closest_sq = dist_sq;
}
}
return sqrtF(closest_sq);
}

// Necessary to avoid linker errors
template bool Polygon_outside<int32_t>(const Vector2l &P, const Vector2l *V, unsigned n);
template bool Polygon_complete<int32_t>(const Vector2l *V, unsigned n);
Expand Down Expand Up @@ -216,3 +196,113 @@ float Polygon_closest_distance_line(const Vector2f *V, unsigned N, const Vector2
}
return sqrtf(closest_sq);
}

#include <cmath>
#include <iostream>

const double EARTH_RADIUS_METERS = 6378137.0; // Earth's radius in meters (WGS84)

// Converts 1e7 degrees to radians
double toRadians(long degrees_1e7) {
return (degrees_1e7 / 1e7) * M_PI / 180.0;
}

// Converts (latitude, longitude) in radians to 3D Cartesian coordinates on a unit sphere
void latLonToCartesian(double latRad, double lonRad, double &x, double &y, double &z) {
x = std::cos(latRad) * std::cos(lonRad);
y = std::cos(latRad) * std::sin(lonRad);
z = std::sin(latRad);
}

// Haversine distance between two points given in 1e7 degrees
double haversineDistance(long lat1_1e7, long lon1_1e7, long lat2_1e7, long lon2_1e7) {
double lat1 = toRadians(lat1_1e7);
double lon1 = toRadians(lon1_1e7);
double lat2 = toRadians(lat2_1e7);
double lon2 = toRadians(lon2_1e7);

double dLat = lat2 - lat1;
double dLon = lon2 - lon1;

double a = std::sin(dLat / 2) * std::sin(dLat / 2) +
std::cos(lat1) * std::cos(lat2) * std::sin(dLon / 2) * std::sin(dLon / 2);
double c = 2 * std::atan2(std::sqrt(a), std::sqrt(1 - a));
return EARTH_RADIUS_METERS * c;
}

// Compute closest distance from a point to a line segment on a sphere
double closestDistanceToLineSegment(long pointLat_1e7, long pointLon_1e7,
long lineStartLat_1e7, long lineStartLon_1e7,
long lineEndLat_1e7, long lineEndLon_1e7) {
// Convert lat/lon to 3D Cartesian coordinates on a unit sphere
double x1, y1, z1, x2, y2, z2, xp, yp, zp;
latLonToCartesian(toRadians(lineStartLat_1e7), toRadians(lineStartLon_1e7), x1, y1, z1);
latLonToCartesian(toRadians(lineEndLat_1e7), toRadians(lineEndLon_1e7), x2, y2, z2);
latLonToCartesian(toRadians(pointLat_1e7), toRadians(pointLon_1e7), xp, yp, zp);

// Compute vector from lineStart to lineEnd (v) and from lineStart to point (w)
double vx = x2 - x1;
double vy = y2 - y1;
double vz = z2 - z1;
double wx = xp - x1;
double wy = yp - y1;
double wz = zp - z1;

// Project w onto v
double dot_vw = vx * wx + vy * wy + vz * wz;
double dot_vv = vx * vx + vy * vy + vz * vz;
double t = (dot_vv != 0) ? dot_vw / dot_vv : -1;

double closestX, closestY, closestZ;
if (t < 0) {
// Closest to line start
closestX = x1;
closestY = y1;
closestZ = z1;
} else if (t > 1) {
// Closest to line end
closestX = x2;
closestY = y2;
closestZ = z2;
} else {
// Closest point is on the line
closestX = x1 + t * vx;
closestY = y1 + t * vy;
closestZ = z1 + t * vz;
}

// Convert closest point back to lat/lon (reverse cartesian)
double norm = std::sqrt(closestX * closestX + closestY * closestY + closestZ * closestZ);
double closestLat = std::asin(closestZ / norm); // Latitude from z-coordinate
double closestLon = std::atan2(closestY, closestX); // Longitude from x and y

// Convert back to 1e7 degrees
long closestLat_1e7 = static_cast<long>((closestLat * 180.0 / M_PI) * 1e7);
long closestLon_1e7 = static_cast<long>((closestLon * 180.0 / M_PI) * 1e7);

// Calculate the Haversine distance from the point to the closest point on the line
return haversineDistance(pointLat_1e7, pointLon_1e7, closestLat_1e7, closestLon_1e7);
}

/*
return the closest distance that point p comes to an edge of closed
polygon V, defined by N points
*/
template <typename T>
float Polygon_closest_distance_point(const Vector2<T> *V, unsigned N, const Vector2<T> &p)
{
T closest = std::numeric_limits<T>::max();
for (uint8_t i=0; i<N-1; i++) {
const Vector2<T> &v1 = V[i];
const Vector2<T> &v2 = V[i+1];

double distance = closestDistanceToLineSegment(p.x, p.y,
v1.x, v1.y,
v2.x, v2.y);
::printf("%f\n", distance);
if (distance < closest) {
closest = distance;
}
}
return closest;
}
54 changes: 51 additions & 3 deletions libraries/AP_Math/tests/test_polygon.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -276,9 +276,9 @@ static const struct {
Vector2l point;
bool outside;
} OBC_test_points[] = {
{ Vector2l(-266398870, 1518220000), true },
{ Vector2l(-266418700, 1518709260), false },
{ Vector2l(-350000000, 1490000000), true },
{ Vector2l(-266398870, 1518220000), true, },
{ Vector2l(-266418700, 1518709260), false, },
{ Vector2l(-350000000, 1490000000), true, },
{ Vector2l(0, 0), true },
{ Vector2l(-265768150, 1518408250), false },
{ Vector2l(-265774060, 1518405860), true },
Expand All @@ -304,6 +304,54 @@ TEST(Polygon, obc)
TEST_POLYGON_POINTS(OBC_boundary, OBC_test_points);
}

#define TEST_POLYGON_DISTANCE_POINTS(POLYGON, TEST_POINTS) \
do { \
for (uint32_t i = 0; i < ARRAY_SIZE(TEST_POINTS); i++) { \
EXPECT_EQ(TEST_POINTS[i].distance, \
Polygon_closest_distance_point(POLYGON, \
ARRAY_SIZE(POLYGON),\
TEST_POINTS[i].point)); \
} \
} while(0)

// Center of London (Charing Cross)
const int32_t CENTER_LAT = static_cast<int32_t>(51.5085 * 1E7); // 515085000
const int32_t CENTER_LON = static_cast<int32_t>(-0.1257 * 1E7); // -1257000

// Bounding box coordinates (in 1E7 degrees)
const int32_t NORTH_WEST_LAT = static_cast<int32_t>((51.5085 + 0.0006732) * 1E7); // 515092732
const int32_t NORTH_WEST_LON = static_cast<int32_t>((-0.1257 - 0.0010776) * 1E7); // -1267776

const int32_t NORTH_EAST_LAT = static_cast<int32_t>((51.5085 + 0.0006732) * 1E7); // 515092732
const int32_t NORTH_EAST_LON = static_cast<int32_t>((-0.1257 + 0.0010776) * 1E7); // -1246224

const int32_t SOUTH_WEST_LAT = static_cast<int32_t>((51.5085 - 0.0006732) * 1E7); // 515080268
const int32_t SOUTH_WEST_LON = static_cast<int32_t>((-0.1257 - 0.0010776) * 1E7); // -1267776

const int32_t SOUTH_EAST_LAT = static_cast<int32_t>((51.5085 - 0.0006732) * 1E7); // 515080268
const int32_t SOUTH_EAST_LON = static_cast<int32_t>((-0.1257 + 0.0010776) * 1E7); // -1246224

// Array of coordinates in [latitude, longitude] pairs for each corner
static const Vector2l London_boundary[] {
Vector2l(NORTH_WEST_LAT, NORTH_WEST_LON), // Northwest corner
Vector2l(NORTH_EAST_LAT, NORTH_EAST_LON), // Northeast corner
Vector2l(SOUTH_WEST_LAT, SOUTH_WEST_LON), // Southwest corner
Vector2l(SOUTH_EAST_LAT, SOUTH_EAST_LON), // Southeast corner
Vector2l(NORTH_WEST_LAT, NORTH_WEST_LON), // Northwest corner
};

static const struct {
Vector2l point;
float distance;
} London_test_points[] = {
{ Vector2l(CENTER_LAT, CENTER_LON), 6737U, },
};

TEST(Polygon, London_distance)
{
TEST_POLYGON_DISTANCE_POINTS(London_boundary, London_test_points);
}

static const Vector2f PROX_boundary[] = {
Vector2f{938.315063f,388.662872f},
Vector2f{545.622803f,1317.25f},
Expand Down

0 comments on commit fe7a31c

Please sign in to comment.