Add Euclidean distance function

diff --git a/src/psp.cc b/src/psp.cc
index e4410682baa9ff5f6dbab0570031153947242e4d..edec2aa16e4934bc9e7119b736e5bfbcdd74076b 100644 (file)
--- 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();