Add a convenient vector wrapper
Change-Id: I9b620d065f69909e5be74cd0df802bf9e57eb4d0
diff --git a/aos/vision/math/vector_test.cc b/aos/vision/math/vector_test.cc
new file mode 100644
index 0000000..8c5244d
--- /dev/null
+++ b/aos/vision/math/vector_test.cc
@@ -0,0 +1,76 @@
+#include "aos/vision/math/vector.h"
+
+#include "gtest/gtest.h"
+
+namespace aos {
+namespace vision {
+namespace testing {
+
+class VectorTest : public ::testing::Test {
+ protected:
+ const Vector<3> vec1_{1.0, 1.0, 1.0};
+ const Vector<3> vec2_{2.0, 4.0, 6.0};
+ const Vector<3> vec5_{2.0, 2.0, 1.0};
+ const Vector<3> vec6_{1.0, 1.0, 1.0};
+};
+
+TEST_F(VectorTest, Equality) {
+ EXPECT_FALSE(vec1_ == vec2_);
+ EXPECT_TRUE(Vector<3>(2.0, 4.0, 6.0) == vec2_);
+}
+
+TEST_F(VectorTest, Addition) {
+ Vector<3> vec3 = vec1_ + vec2_;
+ EXPECT_EQ(3.0, vec3.x());
+ EXPECT_EQ(5.0, vec3.y());
+ EXPECT_EQ(7.0, vec3.z());
+}
+
+TEST_F(VectorTest, Multiplication) {
+ auto new_vec1 = vec1_;
+ new_vec1 *= 2.0;
+ EXPECT_EQ(2.0, new_vec1.x());
+ EXPECT_EQ(2.0, new_vec1.y());
+ EXPECT_EQ(2.0, new_vec1.z());
+
+ auto new_vec2 = new_vec1 * 2;
+ EXPECT_EQ(4.0, new_vec2.x());
+ EXPECT_EQ(4.0, new_vec2.y());
+ EXPECT_EQ(4.0, new_vec2.z());
+}
+
+TEST_F(VectorTest, Magnitude) {
+ const Vector<3> vec4(1.0, 1.0, 1.0);
+ EXPECT_NEAR(1.732, vec4.Mag(), 0.001);
+
+ EXPECT_NEAR(1.414, (vec5_ - vec6_).Mag(), 0.001);
+ EXPECT_NEAR(1.414, (vec6_ - vec5_).Mag(), 0.001);
+}
+
+TEST_F(VectorTest, Sign) {
+ const Vector<3> vec7 = vec5_ - vec6_;
+ const Vector<3> vec8 = vec6_ - vec5_;
+ EXPECT_EQ(1.0, vec7.x());
+ EXPECT_EQ(1.0, vec7.y());
+ EXPECT_EQ(0.0, vec7.z());
+ EXPECT_EQ(-1.0, vec8.x());
+ EXPECT_EQ(-1.0, vec8.y());
+ EXPECT_EQ(0.0, vec8.z());
+}
+
+TEST_F(VectorTest, Angle) {
+ const Vector<3> vec1(1.0, 0.0, 0.0);
+ const Vector<3> vec2(0.0, 1.0, 0.0);
+ EXPECT_NEAR(M_PI / 2, vec1.AngleTo(vec2), 0.0001);
+ const Vector<3> vec3 = Vector<3>(1.0, 1.0, 0.0);
+ EXPECT_NEAR(M_PI / 4, vec1.AngleTo(vec3), 0.0001);
+
+ const Vector<2> vec4(1, 1);
+ EXPECT_NEAR(M_PI / 4, vec4.AngleToZero(), 0.0001);
+ const Vector<2> vec5(0.5, 0.8660254037844386);
+ EXPECT_NEAR(M_PI / 3, vec5.AngleToZero(), 0.0001);
+}
+
+} // namespace testing
+} // namespace vision
+} // namespace aos