Add collide functions, ut

diff --git a/api/bcar.h b/api/bcar.h
index 82c2672aea3a4cd87536854c3ca974425bb8b46c..fc58b0b226493974d271b8d6ecb9e83e4c9f80ba 100644 (file)
--- a/api/bcar.h
+++ b/api/bcar.h
@@ -2,6 +2,8 @@
 #define BCAR_H
 
 #include <ostream>
+#include <tuple>
+#include <vector>
 
 /*! \brief Bicycle car basic class.
 
@@ -157,4 +159,41 @@ class BicycleCar {
                 }
 };
 
+/*! \brief Is `x, y` coordinate in polynom `poly`?
+
+Return `true` if `x, y` coordinate is inside of polynom `poly`.
+
+\see https://en.wikipedia.org/wiki/Even%E2%80%93odd_rule
+
+\param x Horizontal coordinate.
+\param y Vertical coordinate.
+\param poly The vector of coordinates.
+*/
+bool inside(double x, double y, std::vector<std::tuple<double, double>> poly);
+
+/*! \brief Return intersection of two line segments.
+
+The output is tuple `std::tuple<bool, double, double>`, where the first
+value is true when there is an intersection and false otherwise. The
+second and third parameters in the return tuple are coordinates of the
+intersection.
+
+\see https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
+
+\param x1 First line segment first `x` coordinate.
+\param y1 First line segment first `y` coordinate.
+\param x2 First line segment second `x` coordinate.
+\param y2 First line segment second `y` coordinate.
+\param x3 Second line segment first `x` coordinate.
+\param y3 Second line segment first `y` coordinate.
+\param x4 Second line segment second `x` coordinate.
+\param y4 Second line segment second `y` coordinate.
+*/
+std::tuple<bool, double, double> intersect(
+        double x1, double y1,
+        double x2, double y2,
+        double x3, double y3,
+        double x4, double y4
+);
+
 #endif /* BCAR_H */

diff --git a/src/bcar.cc b/src/bcar.cc
index 53a3a23a4a66f83a6a8d4ec77bb303031c2118d5..e3c9a6f0bd7f72e405759fca6350dd3e952a8683 100644 (file)
--- a/src/bcar.cc
+++ b/src/bcar.cc
@@ -167,3 +167,44 @@ void BicycleCar::next()
 BicycleCar::BicycleCar()
 {
 }
+
+bool inside(double x, double y, std::vector<std::tuple<double, double>> poly)
+{
+        unsigned int i = 0;
+        unsigned int j = 3;
+        bool inside = false;
+        for (i = 0; i < 4; i++) {
+                if (
+                        (std::get<1>(poly[i]) > y) != (std::get<1>(poly[j]) > y)
+                        && (
+                                x < std::get<0>(poly[i])
+                                + (std::get<0>(poly[j]) - std::get<0>(poly[i]))
+                                * (y - std::get<1>(poly[i]))
+                                / (std::get<1>(poly[j]) - std::get<1>(poly[i]))
+                        )
+                )
+                        inside = !inside;
+                j = i;
+        }
+        return inside;
+}
+
+std::tuple<bool, double, double> intersect(
+        double x1, double y1,
+        double x2, double y2,
+        double x3, double y3,
+        double x4, double y4
+)
+{
+        double deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
+        if (deno == 0)
+                return std::make_tuple(false, 0, 0);
+        double t = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
+        t /= deno;
+        double u = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
+        u *= -1;
+        u /= deno;
+        if (t < 0 || t > 1 || u < 0 || u > 1)
+                return std::make_tuple(false, 0, 0);
+        return std::make_tuple(true, x1 + t * (x2 - x1), y1 + t * (y2 - y1));
+}

diff --git a/ut/bcar.t.cc b/ut/bcar.t.cc
index e19ce393d7c41eb21a49a8e8d322841e39270248..db985bb376ba287d50048f4b672ba6a1a63116c9 100644 (file)
--- a/ut/bcar.t.cc
+++ b/ut/bcar.t.cc
@@ -81,3 +81,20 @@ WVTEST_MAIN("bcar basic geometry")
         bc.next();
         WVPASSEQ_DOUBLE(-0.2, bc.st(), 0.00001);
 }
+
+WVTEST_MAIN("test collide functions")
+{
+        std::vector<std::tuple<double, double>> slot;
+        slot.push_back(std::make_tuple(1, 1));
+        slot.push_back(std::make_tuple(1, 3));
+        slot.push_back(std::make_tuple(3, 3));
+        slot.push_back(std::make_tuple(3, 1));
+        WVPASS(inside(2, 2, slot));
+        WVPASS(!inside(4, 4, slot));
+        auto tmpi1 = intersect(1, 1, 3, 3, 1, 3, 3, 1);
+        WVPASS(std::get<0>(tmpi1));
+        WVPASSEQ_DOUBLE(std::get<1>(tmpi1), 2, 0.00001);
+        WVPASSEQ_DOUBLE(std::get<2>(tmpi1), 2, 0.00001);
+        auto tmpi2 = intersect(1, 1, 1, 3, 3, 1, 3, 3);
+        WVPASS(!std::get<0>(tmpi2));
+}