Add pose range constructor

[hubacji1/bcar.git] / src / bcar.cc

diff --git a/src/bcar.cc b/src/bcar.cc
index d0d40a146b5abba0184d26d0cd87fcf1482bf233..673ec198853baafcfec05e29f46e2b25c9dfc007 100644 (file)
--- a/src/bcar.cc
+++ b/src/bcar.cc
@@ -108,12 +108,28 @@ Point::rotate(Point const& c, double const angl)
        this->y(ny + c.y());
 }
 
+void
+Point::reflect(Line const& li)
+{
+       this->rotate(li.b(), -li.h());
+       this->y_ -= li.b().y();
+       this->y_ *= -1.0;
+       this->y_ += li.b().y();
+       this->rotate(li.b(), li.h());
+}
+
 double
 Point::edist(Point const& p) const
 {
        return sqrt(pow(p.x() - this->x_, 2.0) + pow(p.y() - this->y_, 2.0));
 }
 
+bool
+Point::operator==(Point const& p)
+{
+       return this->x() == p.x() && this->y() == p.y();
+}
+
 std::ostream&
 operator<<(std::ostream& out, Point const& p)
 {
@@ -272,6 +288,20 @@ Pose::rotate(Point const& c, double const angl)
        this->h(this->h() + angl);
 }
 
+void
+Pose::reflect(Line const& li)
+{
+       Point::reflect(li);
+       double dh = li.h() - this->h();
+       this->h(this->h() + 2.0 * dh);
+}
+
+bool
+Pose::operator==(Pose const& p)
+{
+       return this->x() == p.x() && this->y() == p.y() && this->h() == p.h();
+}
+
 std::ostream&
 operator<<(std::ostream& out, Pose const& p)
 {
@@ -279,41 +309,78 @@ 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();
+       }
+}
+
+PoseRange::PoseRange(double x, double y, double b, double e)
+               : PoseRange(Pose(x, y, b), Pose(x, y, e))
 {
-       return this->h();
 }
 
-void
-PoseRange::b(double b)
+Pose
+PoseRange::bp() const
+{
+       return this->bp_;
+}
+
+Pose
+PoseRange::ep() const
+{
+       return this->ep_;
+}
+
+double
+PoseRange::b() const
 {
-       this->h(b);
+       return std::min(this->bp_.h(), this->ep_.h());
 }
 
 double
 PoseRange::e() const
 {
-       return this->e_;
+       return std::max(this->bp_.h(), this->ep_.h());
 }
 
 void
-PoseRange::e(double e)
+PoseRange::rotate(Point const& c, double const angl)
 {
-       while (e < -M_PI) {
-               e += 2 * M_PI;
-       }
-       while (e > +M_PI) {
-               e -= 2 * M_PI;
-       }
-       this->e_ = e;
+       this->bp_.rotate(c, angl);
+       this->ep_.rotate(c, angl);
+       this->set_xyh();
 }
 
 void
-PoseRange::rotate(Point const& c, double const angl)
+PoseRange::reflect(Line const& li)
 {
-       Pose::rotate(c, angl);
-       this->e(this->e() + angl);
+       this->bp_.reflect(li);
+       this->ep_.reflect(li);
+       this->set_xyh();
 }
 
 std::ostream&
@@ -459,24 +526,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)
@@ -487,9 +548,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());
@@ -502,9 +563,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());
@@ -518,9 +579,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());
@@ -533,9 +594,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());