From b794a1ac4da49f17bd6b9d7fde5b63607e8a0f67 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Tue, 27 Aug 2019 16:19:40 +0200
Subject: [PATCH] Add Euclidean distance function

---
 src/psp.cc | 5 +++++
 1 file changed, 5 insertions(+)

diff --git a/src/psp.cc b/src/psp.cc
index e441068..edec2aa 100644
--- a/src/psp.cc
+++ b/src/psp.cc
@@ -55,6 +55,11 @@ void PSPlanner::gc_to_4()
         this->gc().h(angl_slot);
 }
 
+double edist(double x1, double y1, double x2, double y2)
+{
+        return sqrt(pow(x2 - x1, 2) + pow(y2 - y1, 2));
+}
+
 void PSPlanner::guess_gc()
 {
         double x = this->ps().x1();
-- 
2.39.2