Make pose range headings positive only

diff --git a/src/bcar.cc b/src/bcar.cc
index b3748dbe9ca0c3cbae6dce9c82a201ab796daa8e..90f2a9a7cfcec4193f0b048e12d869638f14e48a 100644 (file)
--- a/src/bcar.cc
+++ b/src/bcar.cc
@@ -340,6 +340,16 @@ PoseRange::set_xyh()
        li1.intersects_with(li2);
        this->x(li1.i1().x());
        this->y(li1.i1().y());
+       double bh = this->b();
+       while (bh < 0.0) {
+               bh += 2.0 * M_PI;
+       }
+       this->bp_.h(bh);
+       double eh = this->e();
+       while (eh < 0.0) {
+               eh += 2.0 * M_PI;
+       }
+       this->ep_.h(eh);
        this->h((this->b() + this->e()) / 2.0);
 }