Refactor pose range

diff --git a/incl/bcar.hh b/incl/bcar.hh
index 5370934e011dc4586a9cee57415dc29fbe06a998..22e57d4ca2f7efb268e24b0aadcd8b3dcf956888 100644 (file)
--- a/incl/bcar.hh
+++ b/incl/bcar.hh
@@ -156,21 +156,21 @@ public:
 
 class PoseRange : public virtual Pose {
 private:
-       double e_ = 0.0;
-       using Pose::h;
+       Pose bp_;
+       Pose ep_;
+       void set_xyh();
 public:
+       PoseRange(Pose bp, Pose ep);
+
+       Pose bp() const;
+       Pose ep() const;
+
        /*! Get heading's begin in the interval [-pi, +pi] radians. */
        double b() const;
 
-       /*! Set heading's begin in radians. It's recomputed to [-pi, +pi]. */
-       void b(double b);
-
        /*! Get heading's end in the interval [-pi, +pi] radians. */
        double e() const;
 
-       /*! Set heading's end in radians. It's recomputed to [-pi, +pi]. */
-       void e(double e);
-
        void rotate(Point const& c, double const angl);
 
        void reflect(Line const& li);

diff --git a/src/bcar.cc b/src/bcar.cc
index 93f017e7cacde2038ae0cf57e3092c163c659473..d64f27fe29d07e6fa51b4c26bc430f92537ce035 100644 (file)
--- a/src/bcar.cc
+++ b/src/bcar.cc
@@ -309,49 +309,73 @@ operator<<(std::ostream& out, Pose const& p)
        return out;
 }
 
-double
-PoseRange::b() const
+void
+PoseRange::set_xyh()
+{
+       double clen = 10.0;
+       double bpbx = this->bp_.x() - clen * cos(this->bp_.h());
+       double bpby = this->bp_.y() - clen * sin(this->bp_.h());
+       double bpfx = this->bp_.x() + clen * cos(this->bp_.h());
+       double bpfy = this->bp_.y() + clen * sin(this->bp_.h());
+       Line li1(Point(bpbx, bpby), Point(bpfx, bpfy));
+       double epbx = this->ep_.x() - clen * cos(this->ep_.h());
+       double epby = this->ep_.y() - clen * sin(this->ep_.h());
+       double epfx = this->ep_.x() + clen * cos(this->ep_.h());
+       double epfy = this->ep_.y() + clen * sin(this->ep_.h());
+       Line li2(Point(epbx, epby), Point(epfx, epfy));
+       li1.intersects_with(li2);
+       this->x(li1.i1().x());
+       this->y(li1.i1().y());
+       this->h((this->b() + this->e()) / 2.0);
+}
+
+PoseRange::PoseRange(Pose bp, Pose ep) : bp_(bp), ep_(ep)
+{
+       if (this->bp_ == this->ep_) {
+               this->set_pose(this->ep_);
+       } else {
+               this->set_xyh();
+       }
+}
+
+Pose
+PoseRange::bp() const
 {
-       return this->h();
+       return this->bp_;
 }
 
-void
-PoseRange::b(double b)
+Pose
+PoseRange::ep() const
 {
-       this->h(b);
+       return this->ep_;
 }
 
 double
-PoseRange::e() const
+PoseRange::b() const
 {
-       return this->e_;
+       return std::min(this->bp_.h(), this->ep_.h());
 }
 
-void
-PoseRange::e(double e)
+double
+PoseRange::e() const
 {
-       while (e < -M_PI) {
-               e += 2 * M_PI;
-       }
-       while (e > +M_PI) {
-               e -= 2 * M_PI;
-       }
-       this->e_ = e;
+       return std::max(this->bp_.h(), this->ep_.h());
 }
 
 void
 PoseRange::rotate(Point const& c, double const angl)
 {
-       Pose::rotate(c, angl);
-       this->e(this->e() + angl);
+       this->bp_.rotate(c, angl);
+       this->ep_.rotate(c, angl);
+       this->set_xyh();
 }
 
 void
 PoseRange::reflect(Line const& li)
 {
-       Pose::reflect(li);
-       double dh = li.h() - this->e();
-       this->e(this->e() + 2.0 * dh);
+       this->bp_.reflect(li);
+       this->ep_.reflect(li);
+       this->set_xyh();
 }
 
 std::ostream&
@@ -497,24 +521,18 @@ CarMove::st(double st)
 bool
 BicycleCar::drivable(Pose const& p) const
 {
-       PoseRange pr;
-       pr.x(p.x());
-       pr.y(p.y());
-       pr.b(p.h());
-       pr.e(p.h());
-       return this->drivable(pr);
+       return this->drivable(PoseRange(p, p));
 }
 
 bool
 BicycleCar::drivable(PoseRange const& p) const
 {
-       double h = (p.b() + p.e()) / 2.0;
        double a_1 = atan2(p.y() - this->y(), p.x() - this->x()) - this->h();
        while (a_1 < -M_PI)
                a_1 += 2 * M_PI;
        while (a_1 > +M_PI)
                a_1 -= 2 * M_PI;
-       double h_d = h - this->h();
+       double h_d = p.h() - this->h();
        while (h_d < -M_PI)
                h_d += 2 * M_PI;
        while (h_d > +M_PI)
@@ -525,9 +543,9 @@ BicycleCar::drivable(PoseRange const& p) const
        } else if (0 < a_1 && a_1 <= M_PI/2) { // left front
                BicycleCar z(*this); // zone border
                z.h(p.e());
-               h_d = h - this->h();
+               h_d = p.h() - this->h();
                z.rotate(this->ccl(), h_d);
-               // assert z.h() == h
+               // assert z.h() == p.h()
                if (p.y() == z.y() && p.x() == z.x()) // p on zone border
                        return true;
                a_2 = atan2(p.y() - z.y(), p.x() - z.x());
@@ -540,9 +558,9 @@ BicycleCar::drivable(PoseRange const& p) const
        } else if (M_PI/2 < a_1 && a_1 <= M_PI) { // left rear
                BicycleCar z(*this); // zone border
                z.h(p.e());
-               h_d = h - this->h();
+               h_d = p.h() - this->h();
                z.rotate(this->ccl(), h_d);
-               // assert z.h() == h
+               // assert z.h() == p.h()
                if (p.y() == z.y() && p.x() == z.x()) // p on zone border
                        return true;
                a_2 = atan2(p.y() - z.y(), p.x() - z.x());
@@ -556,9 +574,9 @@ BicycleCar::drivable(PoseRange const& p) const
        } else if (0 > a_1 && a_1 >= -M_PI/2) { // right front
                BicycleCar z(*this); // zone border
                z.h(p.b());
-               h_d = h - this->h();
+               h_d = p.h() - this->h();
                z.rotate(this->ccr(), h_d);
-               // assert z.h() == h
+               // assert z.h() == p.h()
                if (p.y() == z.y() && p.x() == z.x()) // p on zone border
                        return true;
                a_2 = atan2(p.y() - z.y(), p.x() - z.x());
@@ -571,9 +589,9 @@ BicycleCar::drivable(PoseRange const& p) const
        } else if (-M_PI/2 > a_1 && a_1 >= -M_PI) { // right rear
                BicycleCar z(*this); // zone border
                z.h(p.b());
-               h_d = h - this->h();
+               h_d = p.h() - this->h();
                z.rotate(this->ccr(), h_d);
-               // assert z.h() == h
+               // assert z.h() == p.h()
                if (p.y() == z.y() && p.x() == z.x()) // p on zone border
                        return true;
                a_2 = atan2(p.y() - z.y(), p.x() - z.x());

diff --git a/src/pslot.cc b/src/pslot.cc
index 5f25efd52d703256bf8ad632c3756381e9f674e1..6966377bf8999107a36f3b24645fbdeacccf6b71 100644 (file)
--- a/src/pslot.cc
+++ b/src/pslot.cc
@@ -251,34 +251,15 @@ ParkingSlot::fe(BicycleCar c)
                }
        }
        if (entries.size() == 0) {
-               return PoseRange();
+               return PoseRange(Pose(0.0, 0.0, 0.0), Pose(0.0, 0.0, 0.0));
        }
        if (entries.size() == 1) {
-               PoseRange pr;
-               pr.x(entries.front().front().x());
-               pr.y(entries.front().front().y());
-               pr.b(entries.front().front().h());
-               pr.e(entries.front().front().h());
-               return pr;
+               auto f = entries.front().front();
+               return PoseRange(f, f);
        }
        auto& c1 = entries.front().front();
        auto& c2 = entries.back().front();
-       double b = std::min(c1.h(), c2.h());
-       double e = std::max(c1.h(), c2.h());
-       clen = c.len();
-       Point b1(c1.x() - clen * cos(c1.h()), c1.y() - clen * sin(c1.h()));
-       Point b2(c2.x() - clen * cos(c2.h()), c2.y() - clen * sin(c2.h()));
-       Point e1(c1.x() + clen * cos(c1.h()), c1.y() + clen * sin(c1.h()));
-       Point e2(c2.x() + clen * cos(c2.h()), c2.y() + clen * sin(c2.h()));
-       Line li1(b1, e1);
-       Line li2(b2, e2);
-       li1.intersects_with(li2);
-       PoseRange pr;
-       pr.x(li1.i1().x());
-       pr.y(li1.i1().y());
-       pr.b(b);
-       pr.e(e);
-       return pr;
+       return PoseRange(c1, c2);
 }
 
 PoseRange