From da5d5c92034f446b3227a48466cafe6c6a2db0f5 Mon Sep 17 00:00:00 2001
From: Peter Barker <pbarker@barker.dropbear.id.au>
Date: Tue, 28 Aug 2018 22:42:40 +1000
Subject: [PATCH] AP_Math: add tests for Vector2

---
 libraries/AP_Math/tests/test_vector2.cpp | 61 ++++++++++++++++++++++++
 libraries/AP_Math/vector2.cpp            |  2 +
 libraries/AP_Math/vector2.h              |  3 +-
 3 files changed, 65 insertions(+), 1 deletion(-)

diff --git a/libraries/AP_Math/tests/test_vector2.cpp b/libraries/AP_Math/tests/test_vector2.cpp
index 28ddf47420..44ae19a16b 100644
--- a/libraries/AP_Math/tests/test_vector2.cpp
+++ b/libraries/AP_Math/tests/test_vector2.cpp
@@ -15,4 +15,65 @@ TEST(Vector2Test, IsEqual)
     EXPECT_TRUE(v_float1 == v_float1);
 }
 
+TEST(Vector2Test, angle)
+{
+    EXPECT_FLOAT_EQ(M_PI/2, Vector2f(0, 1).angle());
+    EXPECT_FLOAT_EQ(M_PI/4, Vector2f(1, 1).angle());
+    EXPECT_FLOAT_EQ(0.0f, Vector2f(1, 0).angle());
+    EXPECT_FLOAT_EQ(M_PI*5/4, Vector2f(-1, -1).angle());
+    EXPECT_FLOAT_EQ(M_PI*5/4, Vector2f(-5, -5).angle());
+}
+
+TEST(Vector2Test, length)
+{
+    EXPECT_FLOAT_EQ(25, Vector2f(3, 4).length_squared());
+}
+
+TEST(Vector2Test, normalized)
+{
+    EXPECT_EQ(Vector2f(sqrtf(2)/2, sqrtf(2)/2), Vector2f(5, 5).normalized());
+    EXPECT_EQ(Vector2f(3, 3).normalized(), Vector2f(5, 5).normalized());
+    EXPECT_EQ(Vector2f(-3, 3).normalized(), Vector2f(-5, 5).normalized());
+    EXPECT_NE(Vector2f(-3, 3).normalized(), Vector2f(5, 5).normalized());
+}
+
+TEST(Vector2Test, reflect)
+{
+    Vector2f reflected1 = Vector2f(3, 8);
+    reflected1.reflect(Vector2f(0, 1));
+    EXPECT_EQ(reflected1, Vector2f(-3, 8));
+
+    // colinear vectors
+    Vector2f reflected2 = Vector2f(3, 3);
+    reflected2.reflect(Vector2f(1, 1));
+    EXPECT_EQ(reflected2, Vector2f(3, 3));
+
+    // orthogonal vectors
+    Vector2f reflected3 = Vector2f(3, 3);
+    reflected3.reflect(Vector2f(1, -1));
+    EXPECT_EQ(reflected3, Vector2f(-3, -3));
+}
+
+TEST(Vector2Test, closest_point)
+{
+    // closest_point is (p, v,w)
+
+    // the silly case:
+    EXPECT_EQ((Vector2f{0, 0}),
+              (Vector2f::closest_point(Vector2f{0, 0}, Vector2f{0, 0}, Vector2f{0, 0})));
+    // on line:
+    EXPECT_EQ((Vector2f{0, 0}),
+              (Vector2f::closest_point(Vector2f{0, 0}, Vector2f{0, 0}, Vector2f{1, 1})));
+    EXPECT_EQ((Vector2f{5, 5}),
+              (Vector2f::closest_point(Vector2f{5, 5}, Vector2f{0, 0}, Vector2f{5, 5})));
+    // on line but not segment:
+    EXPECT_EQ((Vector2f{5, 5}),
+              (Vector2f::closest_point(Vector2f{6, 6}, Vector2f{0, 0}, Vector2f{5, 5})));
+
+    EXPECT_EQ((Vector2f{0.5, 0.5}),
+              (Vector2f::closest_point(Vector2f{1,0}, Vector2f{0, 0}, Vector2f{5, 5})));
+    EXPECT_EQ((Vector2f{0, 1}),
+              (Vector2f::closest_point(Vector2f{0,0}, Vector2f{-1, 1}, Vector2f{1, 1})));
+}
+
 AP_GTEST_MAIN()
diff --git a/libraries/AP_Math/vector2.cpp b/libraries/AP_Math/vector2.cpp
index bec3dfdca9..500bffdd44 100644
--- a/libraries/AP_Math/vector2.cpp
+++ b/libraries/AP_Math/vector2.cpp
@@ -445,6 +445,8 @@ template float Vector2<float>::closest_distance_between_line_and_point(const Vec
 template float Vector2<float>::closest_distance_between_line_and_point_squared(const Vector2<float> &w1, const Vector2<float> &w2, const Vector2<float> &p);
 template float Vector2<float>::closest_distance_between_lines_squared(const Vector2<float> &a1,const Vector2<float> &a2,const Vector2<float> &b1,const Vector2<float> &b2);
 
+template void Vector2<float>::reflect(const Vector2<float> &n);
+
 template bool Vector2<long>::operator ==(const Vector2<long> &v) const;
 
 // define for int
diff --git a/libraries/AP_Math/vector2.h b/libraries/AP_Math/vector2.h
index e28d55aa52..01cf270490 100644
--- a/libraries/AP_Math/vector2.h
+++ b/libraries/AP_Math/vector2.h
@@ -95,7 +95,8 @@ struct Vector2
     // returns 0 if the vectors are parallel, and M_PI if they are antiparallel
     float angle(const Vector2<T> &v2) const;
 
-    // computes the angle of this vector in radians, from -Pi to Pi
+    // computes the angle of this vector in radians, from 0 to 2pi,
+    // from a unit vector(1,0); a (1,1) vector's angle is +M_PI/4
     float angle(void) const;
 
     // check if any elements are NAN