Add some basic point/angle related math utilities.
Change-Id: I64cf1b23c218525af9edb1cc1088cca8772b49d6
diff --git a/aos/util/BUILD b/aos/util/BUILD
index 902cb00..99ae87d 100644
--- a/aos/util/BUILD
+++ b/aos/util/BUILD
@@ -37,6 +37,23 @@
)
cc_library(
+ name = "math",
+ hdrs = ["math.h"],
+ deps = [
+ "//third_party/eigen",
+ ],
+)
+
+cc_test(
+ name = "math_test",
+ srcs = ["math_test.cc"],
+ deps = [
+ ":math",
+ "//aos/testing:googletest",
+ ],
+)
+
+cc_library(
name = "death_test_log_implementation",
hdrs = [
"death_test_log_implementation.h",
@@ -69,8 +86,8 @@
"phased_loop.h",
],
deps = [
- "//aos/time:time",
"//aos/logging",
+ "//aos/time",
],
)
@@ -80,8 +97,8 @@
"log_interval.h",
],
deps = [
- "//aos/time:time",
"//aos/logging",
+ "//aos/time",
],
)
@@ -129,8 +146,8 @@
"-lm",
],
deps = [
- "//aos/time:time",
"//aos/logging",
+ "//aos/time",
"//third_party/eigen",
],
)
@@ -216,7 +233,7 @@
"linked_list.h",
],
deps = [
- "//aos/transaction:transaction",
+ "//aos/transaction",
],
)
diff --git a/aos/util/math.h b/aos/util/math.h
new file mode 100644
index 0000000..5efc3c9
--- /dev/null
+++ b/aos/util/math.h
@@ -0,0 +1,70 @@
+#ifndef AOS_UTIL_MATH_H_
+#define AOS_UTIL_MATH_H_
+
+#include <cmath>
+
+#include "Eigen/Dense"
+
+namespace aos {
+namespace math {
+
+// Normalizes an angle to be in (-M_PI, M_PI]
+template <typename Scalar>
+constexpr Scalar NormalizeAngle(Scalar theta) {
+ // First clause takes care of getting theta into
+ // (-3 * M_PI, M_PI)
+ const int n_pi_pos = (theta + M_PI) / 2.0 / M_PI;
+ theta -= n_pi_pos * 2.0 * M_PI;
+ // Next we fix it to cut off the bottom half of the above
+ // range and bring us into (-M_PI, M_PI]
+ const int n_pi_neg = (theta - M_PI) / 2.0 / M_PI;
+ theta -= n_pi_neg * 2.0 * M_PI;
+ return theta;
+}
+
+// Calculate a - b and return the result in (-M_PI, M_PI]
+template <typename Scalar>
+constexpr Scalar DiffAngle(Scalar a, Scalar b) {
+ return NormalizeAngle(a - b);
+}
+
+// Returns whether points A, B, C are arranged in a counter-clockwise manner on
+// a 2-D plane.
+// Collinear points of any sort will cause this to return false.
+// Source: https://bryceboe.com/2006/10/23/line-segment-intersection-algorithm/
+// Mathod:
+// 3 points on a plane will form a triangle (unless they are collinear), e.g.:
+// A-------------------C
+// \ /
+// \ /
+// \ /
+// \ /
+// \ /
+// \ /
+// \ /
+// \ /
+// \ /
+// B
+// We are interested in whether A->B->C is the counter-clockwise direction
+// around the triangle (it is in this picture).
+// Essentially, we want to know whether the angle between A->B and A->C is
+// positive or negative.
+// For this, consider the cross-product, where we imagine a third z-axis
+// coming out of the page. The cross-product AB x AC will be positive if ABC
+// is counter-clockwise and negative if clockwise (and zero if collinear).
+// The z-component (which is the only non-zero component) of the cross-product
+// is AC.y * AB.x - AB.y * AC.x > 0, which turns into:
+// AC.y * AB.x > AB.y * AC.x
+// (C.y - A.y) * (B.x - A.x) > (B.y - A.y) * (C.x - A.x)
+// which is exactly what we have below.
+template <typename Scalar>
+constexpr bool PointsAreCCW(const Eigen::Ref<Eigen::Matrix<Scalar, 2, 1>> &A,
+ const Eigen::Ref<Eigen::Matrix<Scalar, 2, 1>> &B,
+ const Eigen::Ref<Eigen::Matrix<Scalar, 2, 1>> &C) {
+ return (C.y() - A.y()) * (B.x() - A.x()) > (B.y() - A.y()) * (C.x() - A.x());
+}
+
+} // namespace math
+} // namespace aos
+
+#endif // AOS_UTIL_MATH_H_
diff --git a/aos/util/math_test.cc b/aos/util/math_test.cc
new file mode 100644
index 0000000..a243e53
--- /dev/null
+++ b/aos/util/math_test.cc
@@ -0,0 +1,89 @@
+#include "aos/util/math.h"
+
+#include "gtest/gtest.h"
+
+namespace aos {
+namespace math {
+namespace testing {
+
+bool AngleEqual(double a1, double a2) {
+ double diff = a1 - a2;
+ return ::std::fmod(diff, 2 * M_PI) == 0.0;
+}
+
+bool AngleInBounds(double a) { return a <= M_PI && a > -M_PI; }
+
+// Check that something
+void ExpectNormalizesCorrectly(double a) {
+ double b = NormalizeAngle(a);
+ EXPECT_PRED2(AngleEqual, a, b);
+ EXPECT_PRED1(AngleInBounds, b);
+}
+
+void ExpectDiffsCorrectly(double a, double b) {
+ double diff = DiffAngle(a, b);
+
+ EXPECT_PRED1(AngleInBounds, diff) << "a: " << a << " b: " << b;
+ EXPECT_PRED2(AngleEqual, a, b + diff);
+}
+
+// Checks that normalizing positive and negative angles in and out of bounds
+// works correctly.
+TEST(MathTest, NormalizeAngleTest) {
+ ExpectNormalizesCorrectly(0.0);
+ ExpectNormalizesCorrectly(1.0);
+ ExpectNormalizesCorrectly(-1.0);
+ ExpectNormalizesCorrectly(M_PI);
+ ExpectNormalizesCorrectly(2.0 * M_PI);
+ ExpectNormalizesCorrectly(-2.0 * M_PI);
+ ExpectNormalizesCorrectly(100.0);
+ ExpectNormalizesCorrectly(-100.0);
+}
+
+// Checks that subtracting one angle from another works for various combinations
+// of angles.
+TEST(MathTest, DiffAngleTest) {
+ ExpectDiffsCorrectly(0.0, 0.0);
+ ExpectDiffsCorrectly(1.0, 0.0);
+ ExpectDiffsCorrectly(0.0, 1.0);
+ ExpectDiffsCorrectly(-1.0, 1.0);
+ ExpectDiffsCorrectly(100.0, 1.0);
+ ExpectDiffsCorrectly(-100.0, 1.0);
+ ExpectDiffsCorrectly(M_PI, M_PI);
+}
+
+// Check that points that are really counter-clockwise show up as
+// counter-clockwise, and that invalid orderings don't show up as CCW.
+TEST(MathTest, PointsAreCCWTest) {
+ Eigen::Vector2d a, b, c;
+ a << 0.0, 0.0;
+ b << 1.0, 0.0;
+ c << 0.0, 1.0;
+ // Because there are only six orderings we can just enumerate them all.
+ EXPECT_TRUE(PointsAreCCW<double>(a, b, c));
+ EXPECT_FALSE(PointsAreCCW<double>(a, c, b));
+ EXPECT_FALSE(PointsAreCCW<double>(b, a, c));
+ EXPECT_TRUE(PointsAreCCW<double>(b, c, a));
+ EXPECT_TRUE(PointsAreCCW<double>(c, a, b));
+ EXPECT_FALSE(PointsAreCCW<double>(c, b, a));
+}
+
+// Collinear points should always appear as non-counter-clockwise.
+TEST(MathTest, CollinearPointsAreCCWTest) {
+ // Create three collinear points along the X-axis and check that every
+ // combination is not CCW.
+ Eigen::Vector2d a, b, c;
+ a << 0.0, 0.0;
+ b << 1.0, 0.0;
+ c << 2.0, 0.0;
+ EXPECT_FALSE(PointsAreCCW<double>(a, b, c));
+ EXPECT_FALSE(PointsAreCCW<double>(a, c, b));
+ EXPECT_FALSE(PointsAreCCW<double>(b, a, c));
+ EXPECT_FALSE(PointsAreCCW<double>(b, c, a));
+ EXPECT_FALSE(PointsAreCCW<double>(c, a, b));
+ EXPECT_FALSE(PointsAreCCW<double>(c, b, a));
+}
+
+} // namespace testing
+} // namespace math
+} // namespace aos