Mirror/ardupilot
5
0
Fork 0
mirror of https://github.com/ArduPilot/ardupilot synced 2025-01-05 07:28:29 -04:00
Code Issues Packages Projects Releases Wiki Activity

AP_Math: add template versions of Polygon_closest_distance_point()

Browse Source
This commit is contained in:
Andy Piper 2024-12-18 18:26:26 +00:00
parent 339f36897c
commit 854eacab92
3 changed files with 17 additions and 7 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

14
libraries/AP_Math/polygon.cpp
View File

@ -123,7 +123,8 @@ template bool Polygon_outside<int32_t>(const Vector2l &P, const Vector2l *V, uns
template bool Polygon_complete<int32_t>(const Vector2l *V, unsigned n);
template bool Polygon_outside<float>(const Vector2f &P, const Vector2f *V, unsigned n);
template bool Polygon_complete<float>(const Vector2f *V, unsigned n);
template float Polygon_closest_distance_point<float>(const Vector2f *V, unsigned N, const Vector2f &p);
template float Polygon_closest_distance_point<int32_t>(const Vector2l *V, unsigned N, const Vector2l &p);
/*
determine if the polygon of N verticies defined by points V is
@ -200,14 +201,15 @@ float Polygon_closest_distance_line(const Vector2f *V, unsigned N, const Vector2
return the closest distance that point p comes to an edge of closed
polygon V, defined by N points
*/
float Polygon_closest_distance_point(const Vector2f *V, unsigned N, const Vector2f &p)
template <typename T>
float Polygon_closest_distance_point(const Vector2<T> *V, unsigned N, const Vector2<T> &p)
{
float closest_sq = FLT_MAX;
T closest_sq = std::numeric_limits<T>::max();
for (uint8_t i=0; i<N-1; i++) {
const Vector2f &v1 = V[i];
const Vector2f &v2 = V[i+1];
const Vector2<T> &v1 = V[i];
const Vector2<T> &v2 = V[i+1];
float dist_sq = Vector2f::closest_distance_between_line_and_point_squared(v1, v2, p);
T dist_sq = Vector2<T>::closest_distance_between_line_and_point_squared(v1, v2, p);
if (dist_sq < closest_sq) {
closest_sq = dist_sq;
}

3
libraries/AP_Math/polygon.h
View File

@ -43,4 +43,5 @@ float Polygon_closest_distance_line(const Vector2f *V, unsigned N, const Vector2
return the closest distance that a point p comes to an edge of
closed polygon V, defined by N points
*/
float Polygon_closest_distance_point(const Vector2f *V, unsigned N, const Vector2f &p);
template <typename T>
float Polygon_closest_distance_point(const Vector2<T> *V, unsigned N, const Vector2<T> &p);

7
libraries/AP_Math/vector2.cpp
View File

@ -20,6 +20,13 @@
#include "AP_Math.h"
template float Vector2f::closest_distance_between_line_and_point_squared(const Vector2f &w1,
const Vector2f &w2,
const Vector2f &p);
template int32_t Vector2l::closest_distance_between_line_and_point_squared(const Vector2l &w1,
const Vector2l &w2,
const Vector2l &p);
template <typename T>
T Vector2<T>::length_squared() const
{