From e2983cab4b0f30fba47ce2940d5f76c385ba1795 Mon Sep 17 00:00:00 2001
From: Jiri Vlasak <hubacji1@fel.cvut.cz>
Date: Wed, 3 Apr 2019 15:24:02 +0200
Subject: [PATCH] Return heading in (-M_PI,+M_PI] interval

---
 base/rrtnode.cc | 7 ++++++-
 1 file changed, 6 insertions(+), 1 deletion(-)

diff --git a/base/rrtnode.cc b/base/rrtnode.cc
index cba4c55..8127f3d 100644
--- a/base/rrtnode.cc
+++ b/base/rrtnode.cc
@@ -71,7 +71,12 @@ float RRTNode::y() const
 
 float RRTNode::h() const
 {
-        return this->h_;
+        float h = this->h_;
+        while (h > M_PI)
+                h -= 2 * M_PI;
+        while (h <= -M_PI)
+                h += 2 * M_PI;
+        return h;
 }
 
 float RRTNode::t() const
-- 
2.39.2