From 9989f29bf64d23b802049d086356223d2b46e9cb Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Mon, 4 Mar 2019 18:14:41 +0100
Subject: [PATCH] Add slot type getter

---
 decision_control/slotplanner.cc | 19 +++++++++++++++++++
 incl/slotplanner.h              | 12 ++++++++++++
 2 files changed, 31 insertions(+)

diff --git a/decision_control/slotplanner.cc b/decision_control/slotplanner.cc
index dff0fe4..bc4ff1b 100644
--- a/decision_control/slotplanner.cc
+++ b/decision_control/slotplanner.cc
@@ -40,6 +40,25 @@ PolygonObstacle &ParallelSlot::slot()
         return this->slot_;
 }
 
+SlotType ParallelSlot::slotType()
+{
+        if (!this->slotType_) {
+                float d1 = EDIST(
+                        this->slot().bnodes()[0],
+                        this->slot().bnodes()[1]
+                );
+                float d2 = EDIST(
+                        this->slot().bnodes()[1],
+                        this->slot().bnodes()[2]
+                );
+                if (d1 > d2)
+                        this->slotType_ = PERPENDICULAR;
+                else
+                        this->slotType_ = PARALLEL;
+        }
+        return this->slotType_;
+}
+
 // setter
 void ParallelSlot::DH(float dh)
 {
diff --git a/incl/slotplanner.h b/incl/slotplanner.h
index 04cb966..b1c75aa 100644
--- a/incl/slotplanner.h
+++ b/incl/slotplanner.h
@@ -22,11 +22,22 @@ along with I am car. If not, see <http://www.gnu.org/licenses/>.
 #include "bcar.h"
 #include "obstacle.h"
 
+#define EDIST(a, b) ({ __typeof__ (a) _a = (a); \
+                __typeof__ (b) _b = (b); \
+                pow(pow((_b)->x() - (_a)->x(), 2) + \
+                                pow((_b)->y() - (_a)->y(), 2), 0.5); })
+
+enum SlotType {
+        PARALLEL,
+        PERPENDICULAR
+};
+
 class ParallelSlot {
         private:
                 float DH_ = 0.01;
                 std::vector<std::vector<RRTNode *>> cusp_;
                 PolygonObstacle slot_;
+                SlotType slotType_;
         public:
                 ParallelSlot();
 
@@ -34,6 +45,7 @@ class ParallelSlot {
                 std::vector<std::vector<RRTNode *>> &cusp();
                 float DH() const;
                 PolygonObstacle &slot();
+		SlotType slotType();
 
                 // setter
                 void DH(float dh);
-- 
2.39.2