Add parking slot constructor

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

diff --git a/src/pslot.cc b/src/pslot.cc
index e0526d979a45822bf1ac0662092e311c82336382..8cf1ad950e5ab70bd93a772e2c38c69603ca5d5d 100644 (file)
--- a/src/pslot.cc
+++ b/src/pslot.cc
@@ -1,59 +1,117 @@
+#include <cassert>
 #include <cmath>
-#include "pslot.h"
+#include "pslot.hh"
 
-// slot info
-double ParkingSlot::heading() const
+namespace bcar {
+
+ParkingSlot::ParkingSlot(Point p, double h, double W, double L) :
+               border_({p,
+                       Point(p.x() + W * cos(h - M_PI / 2.0),
+                               p.y() + W * sin(h - M_PI / 2.0)),
+                       Point(p.x() + W * cos(h - M_PI / 2.0) + L * cos(h),
+                               p.y() + W * sin(h - M_PI / 2.0) + L * sin(h)),
+                       Point(p.x() + L * cos(h), p.y() + L * sin(h))}),
+               entry_(border_[0], border_[3]),
+               rear_(border_[0], border_[1]),
+               curb_(border_[1], border_[2]),
+               front_(border_[2], border_[3])
+{
+}
+
+ParkingSlot::ParkingSlot(double lrx, double lry, double rrx, double rry,
+               double rfx, double rfy, double lfx, double lfy) :
+                       border_({Point(lrx, lry), Point(rrx, rry),
+                               Point(rfx, rfy), Point(lfx, lfy)}),
+                       entry_(border_[0], border_[3]),
+                       rear_(border_[0], border_[1]),
+                       curb_(border_[1], border_[2]),
+                       front_(border_[2], border_[3])
+{
+}
+
+double
+ParkingSlot::lfx() const
+{
+       return this->border_[3].x();
+}
+
+double
+ParkingSlot::lfy() const
+{
+       return this->border_[3].y();
+}
+
+double
+ParkingSlot::lrx() const
+{
+       return this->border_[0].x();
+}
+
+double
+ParkingSlot::lry() const
 {
-        return atan2(this->y4() - this->y1(), this->x4() - this->x1());
+       return this->border_[0].y();
 }
 
-bool ParkingSlot::parallel() const
+double
+ParkingSlot::rrx() const
 {
-        double d1 = sqrt(
-                pow(this->x2() - this->x1(), 2)
-                + pow(this->y2() - this->y1(), 2)
-        );
-        double d2 = sqrt(
-                pow(this->x3() - this->x2(), 2)
-                + pow(this->y3() - this->y2(), 2)
-        );
-        if (d1 < d2)
-                return true;
-        else
-                return false;
+       return this->border_[1].x();
 }
 
-bool ParkingSlot::right() const
+double
+ParkingSlot::rry() const
 {
-        if (sgn(
-                (this->x2() - this->x1()) * (this->y4() - this->y1())
-                - (this->y2() - this->y1()) * (this->x4() - this->x1())
-        ) < 0)
-                return false;
-        else
-                return true;
+       return this->border_[1].y();
 }
 
-//getters, setters
-void ParkingSlot::set_slot(
-        double x,
-        double y,
-        double h,
-        double w,
-        double l
-)
+double
+ParkingSlot::rfx() const
 {
-        double x1 = x + w/2 * cos(h - M_PI/2);
-        double y1 = y + w/2 * sin(h - M_PI/2);
-        double x2 = x + l * cos(h) + w/2 * cos(h - M_PI/2);
-        double y2 = y + l * sin(h) + w/2 * sin(h - M_PI/2);
-        double x3 = x + l * cos(h) + w/2 * cos(h + M_PI/2);
-        double y3 = y + l * sin(h) + w/2 * sin(h + M_PI/2);
-        double x4 = x + w/2 * cos(h + M_PI/2);
-        double y4 = y + w/2 * sin(h + M_PI/2);
-        this->border(x1, y1, x2, y2, x3, y3, x4, y4);
+       return this->border_[2].x();
 }
 
-ParkingSlot::ParkingSlot()
+double
+ParkingSlot::rfy() const
 {
+       return this->border_[2].y();
 }
+
+double
+ParkingSlot::h() const
+{
+       return atan2(this->lfy() - this->lry(), this->lfx() - this->lrx());
+}
+
+bool
+ParkingSlot::parallel() const
+{
+       return this->entry_.len() > this->rear_.len();
+}
+
+bool
+ParkingSlot::right() const
+{
+       return this->border_[1].on_right_side_of(this->entry_);
+}
+
+void
+ParkingSlot::swap_side()
+{
+       this->border_[1].rotate(this->border_[0], M_PI);
+       this->border_[2].rotate(this->border_[3], M_PI);
+}
+
+std::ostream&
+operator<<(std::ostream& o, ParkingSlot const& s)
+{
+       o << "[";
+       o << s.border_[0] << ",";
+       o << s.border_[1] << ",";
+       o << s.border_[2] << ",";
+       o << s.border_[3];
+       o << "]";
+       return o;
+}
+
+} // namespace bcar