Return heading in (-M_PI,+M_PI] interval

diff --git a/base/rrtnode.cc b/base/rrtnode.cc
index cba4c5514abfe14aa51b3a43349cdc6665ae66f9..8127f3d4cfd9d9194985f2b3e366fa1873544ab7 100644 (file)
--- 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