AP_Math: add Vector2f perpendicular

Add closest_distance_between_radial_and_point function

libraries/AP_Math/tests/test_closest_distance_between_radial_and_point.cpp

@@ -0,0 +1,49 @@
+#include <AP_gtest.h>
+
+#include <AP_Math/vector2.h>
+
+#define TEST_DISTANCE_BOTH(line_segment_x,line_segment_y, point_x, point_y, expected_length) \
+    do {                                                                \
+        {                                                               \
+            Vector2f line_segment = Vector2f(line_segment_x, line_segment_y); \
+            Vector2f point = Vector2f(point_x, point_y);                \
+            float result = Vector2<float>::closest_distance_between_radial_and_point( \
+                line_segment,                                           \
+                point                                                   \
+                );                                                      \
+            EXPECT_FLOAT_EQ(result, expected_length);                   \
+        }                                                               \
+    } while (false)
+
+
+TEST(ThreatTests, Distance)
+{
+
+    TEST_DISTANCE_BOTH( 0, 0,  0, 0, 0);
+    TEST_DISTANCE_BOTH( 0, 0,  0, 1, 1);
+
+    TEST_DISTANCE_BOTH( 1, 1,  1, 0, sqrt(0.5));
+    TEST_DISTANCE_BOTH(-1,-1, -1, 0, sqrt(0.5));
+
+    TEST_DISTANCE_BOTH( 3, 1,  3, 0, 0.94868332);
+    TEST_DISTANCE_BOTH( 1, 3,  0, 3, 0.94868332);
+    TEST_DISTANCE_BOTH(-1, 3,  0, 3, 0.94868332);
+    TEST_DISTANCE_BOTH( 1,-3,  0,-3, 0.94868332);
+    TEST_DISTANCE_BOTH(-1,-3,  0,-3, 0.94868332);
+
+    TEST_DISTANCE_BOTH( 2, 2,  1, 1, 0.0);
+    TEST_DISTANCE_BOTH( 2, 2,  3, 3, sqrt(2));
+    TEST_DISTANCE_BOTH( 2, 2,  2, 2, 0);
+    TEST_DISTANCE_BOTH( 0, 0,  1, 1, sqrt(2));
+    TEST_DISTANCE_BOTH( 0, 0,  1, 1, sqrt(2));
+
+    TEST_DISTANCE_BOTH( 0, 0,  1, 1, sqrt(2));
+    TEST_DISTANCE_BOTH( 1, 1,  0, 3, sqrt(5));
+}
+
+AP_GTEST_MAIN()
+
+
+int hal = 0; // bizarrely, this fixes an undefined-symbol error but doesn't raise a type exception.  Yay.
+
+

libraries/AP_Math/tests/test_perpendicular.cpp

@@ -0,0 +1,36 @@
+#include <AP_gtest.h>
+
+#include <AP_Math/vector2.h>
+
+#define EXPECT_VECTOR2F_EQ(v1, v2)              \
+    do {                                        \
+        EXPECT_FLOAT_EQ(v1[0], v2[0]);          \
+        EXPECT_FLOAT_EQ(v1[1], v2[1]);          \
+    } while (false);
+
+#define PERP_TEST(px,py, vx,vy, ex,ey)              \
+    do {                                                \
+        Vector2f p(px,py);                           \
+        Vector2f v(vx,vy);                           \
+        Vector2f expected(ex,ey);                       \
+        Vector2f result;                                \
+        result = Vector2f::perpendicular(p, v);   \
+        EXPECT_VECTOR2F_EQ(result, expected);           \
+    } while (false)
+
+TEST(ThreatTests, Distance)
+{
+
+    PERP_TEST( 0.0f,0.0f, 0.0f,0.0f, 0.0f,0.0f);
+    PERP_TEST( 0.0f,0.0f, 1.0f,0.0f, 0.0f,-1.0f);
+    PERP_TEST( 2.0f,0.0f, 1.0f,0.0f, 0.0f,-1.0f);
+    PERP_TEST( 0.0f,2.0f, 1.0f,0.0f, 0.0f,1.0f);
+
+    PERP_TEST( 0.0f,2.0f, 1.0f,2.0f, -2.0f,1.0f);
+    PERP_TEST( 2.0f,0.0f, 1.0f,2.0f, 2.0f,-1.0f);
+}
+
+AP_GTEST_MAIN()
+
+
+int hal = 0; // bizarrely, this fixes an undefined-symbol error but doesn't raise a type exception.  Yay.

libraries/AP_Math/vector2.h

@@ -161,6 +161,35 @@ struct Vector2
     {
         return v * (*this * v)/(v*v);
     }
+
+    // given a position p1 and a velocity v1 produce a vector
+    // perpendicular to v1 maximising distance from p1
+    static Vector2<T> perpendicular(const Vector2<T> &pos_delta, const Vector2<T> &v1)
+    {
+        Vector2<T> perpendicular1 = Vector2<T>(-v1[1], v1[0]);
+        Vector2<T> perpendicular2 = Vector2<T>(v1[1], -v1[0]);
+        T d1 = perpendicular1 * pos_delta;
+        T d2 = perpendicular2 * pos_delta;
+        if (d1 > d2) {
+            return perpendicular1;
+        }
+        return perpendicular2;
+    }
+
+    // thanks to grumdrig (http://stackoverflow.com/questions/849211/shortest-distance-between-a-point-and-a-line-segment)
+    // w defines a line segment from the origin
+    // p is a point
+    // returns the closest distance between the radial and the point
+    static float closest_distance_between_radial_and_point(const Vector2<T> &w,
+                                                           const Vector2<T> &p)
+    {
+        const float len = w.length_squared();
+        if (len < FLT_EPSILON) return p.length();
+        const float t = fmax(0, fmin(1, (p*w) / len));
+        const Vector2<T> projection = w * t;
+        return (p-projection).length();
+    }
+
 };
 
 typedef Vector2<int16_t>        Vector2i;