#ifndef PSP_H
#define PSP_H
+#include <tuple>
+
#include "bcar.h"
#include "pslot.h"
\param cc Current bicycle car.
\param ps Parking slot.
+\param gc Goal car.
*/
class PSPlanner {
private:
- BicycleCar *cc_ = new BicycleCar();
- ParkingSlot *ps_ = new ParkingSlot();
+ BicycleCar cc_;
+ BicycleCar gc_;
+ ParkingSlot ps_;
public:
+ /*! \brief Return `true` if there is collision.
+
+ If the parking slot `ps` collide with current car `cc`,
+ return `true`.
+
+ This method depends on `intersection` function that
+ returns `true` or `false` if two line segments collide.
+ Each line segment of current car `cc` (borders) is
+ checked to each line segment of parking slot `ps`
+ (parking slot lines).
+ */
+ bool collide();
+ /*! \brief Has current car `cc` left?
+
+ Return `true` if the current car `cc` left the parking
+ slot `ps`;
+ */
+ bool left();
+
+ // find entry
+ /*! \brief Find entry to the parking slot.
+ */
+ void fe();
+ /*! \brief Find entry to slot by reverse approach.
+
+ See `Vorobieva2015` for more information.
+ */
+ void fer();
+
// getters, setters
- BicycleCar *cc() { return this->cc_; }
- ParkingSlot *ps() { return this->ps_; }
+ BicycleCar &cc() { return this->cc_; }
+ BicycleCar &gc() { return this->gc_; }
+ ParkingSlot &ps() { return this->ps_; }
PSPlanner();
};
+/*! \brief Return intersection of two line segments.
+
+The output is tuple `std::tuple<bool, double, double>`, where the first
+value is true when there is an intersection and false otherwise. The
+second and third parameters in the return tuple are coordinates of the
+intersection.
+
+\see https://en.wikipedia.org/wiki/Line%E2%80%93line_intersection
+
+\param x1 First line segment first `x` coordinate.
+\param y1 First line segment first `y` coordinate.
+\param x2 First line segment second `x` coordinate.
+\param y2 First line segment second `y` coordinate.
+\param x3 Second line segment first `x` coordinate.
+\param y3 Second line segment first `y` coordinate.
+\param x4 Second line segment second `x` coordinate.
+\param y4 Second line segment second `y` coordinate.
+*/
+std::tuple<bool, double, double> intersect(
+ double x1, double y1,
+ double x2, double y2,
+ double x3, double y3,
+ double x4, double y4
+);
+
#endif /* PSP_H */
#include "psp.h"
+bool PSPlanner::collide()
+{
+ if(std::get<0>(intersect(
+ this->cc().lfx(), this->cc().lfy(),
+ this->cc().lrx(), this->cc().lry(),
+ this->ps().x1(), this->ps().y1(),
+ this->ps().x2(), this->ps().y2()
+ )))
+ return true;
+ if(std::get<0>(intersect(
+ this->cc().rfx(), this->cc().rfy(),
+ this->cc().rrx(), this->cc().rry(),
+ this->ps().x1(), this->ps().y1(),
+ this->ps().x2(), this->ps().y2()
+ )))
+ return true;
+ if(std::get<0>(intersect(
+ this->cc().lfx(), this->cc().lfy(),
+ this->cc().rfx(), this->cc().rfy(),
+ this->ps().x1(), this->ps().y1(),
+ this->ps().x2(), this->ps().y2()
+ )))
+ return true;
+ if(std::get<0>(intersect(
+ this->cc().lrx(), this->cc().lry(),
+ this->cc().rrx(), this->cc().rry(),
+ this->ps().x1(), this->ps().y1(),
+ this->ps().x2(), this->ps().y2()
+ )))
+ return true;
+ if(std::get<0>(intersect(
+ this->cc().lfx(), this->cc().lfy(),
+ this->cc().lrx(), this->cc().lry(),
+ this->ps().x2(), this->ps().y2(),
+ this->ps().x3(), this->ps().y3()
+ )))
+ return true;
+ if(std::get<0>(intersect(
+ this->cc().rfx(), this->cc().rfy(),
+ this->cc().rrx(), this->cc().rry(),
+ this->ps().x2(), this->ps().y2(),
+ this->ps().x3(), this->ps().y3()
+ )))
+ return true;
+ if(std::get<0>(intersect(
+ this->cc().lfx(), this->cc().lfy(),
+ this->cc().rfx(), this->cc().rfy(),
+ this->ps().x2(), this->ps().y2(),
+ this->ps().x3(), this->ps().y3()
+ )))
+ return true;
+ if(std::get<0>(intersect(
+ this->cc().lrx(), this->cc().lry(),
+ this->cc().rrx(), this->cc().rry(),
+ this->ps().x2(), this->ps().y2(),
+ this->ps().x3(), this->ps().y3()
+ )))
+ return true;
+ if(std::get<0>(intersect(
+ this->cc().lfx(), this->cc().lfy(),
+ this->cc().lrx(), this->cc().lry(),
+ this->ps().x3(), this->ps().y3(),
+ this->ps().x4(), this->ps().y4()
+ )))
+ return true;
+ if(std::get<0>(intersect(
+ this->cc().rfx(), this->cc().rfy(),
+ this->cc().rrx(), this->cc().rry(),
+ this->ps().x3(), this->ps().y3(),
+ this->ps().x4(), this->ps().y4()
+ )))
+ return true;
+ if(std::get<0>(intersect(
+ this->cc().lfx(), this->cc().lfy(),
+ this->cc().rfx(), this->cc().rfy(),
+ this->ps().x3(), this->ps().y3(),
+ this->ps().x4(), this->ps().y4()
+ )))
+ return true;
+ if(std::get<0>(intersect(
+ this->cc().lrx(), this->cc().lry(),
+ this->cc().rrx(), this->cc().rry(),
+ this->ps().x3(), this->ps().y3(),
+ this->ps().x4(), this->ps().y4()
+ )))
+ return true;
+ return false;
+}
+
+bool PSPlanner::left()
+{
+ double lfx = this->cc().lfx();
+ double lfy = this->cc().lfy();
+ double rfx = this->cc().rfx();
+ double rfy = this->cc().rfy();
+ double ccx = this->cc().x();
+ double ccy = this->cc().y();
+ double lfs = sgn(
+ (lfx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
+ - (lfy - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
+ );
+ double rfs = sgn(
+ (rfx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
+ - (rfy - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
+ );
+ double ccs = sgn(
+ (ccx - this->ps().x1()) * (this->ps().y4() - this->ps().y1())
+ - (ccy - this->ps().y1()) * (this->ps().x4() - this->ps().x1())
+ );
+ return lfs == rfs && lfs != ccs;
+}
+
+// find entry
+void PSPlanner::fe()
+{
+}
+
+void PSPlanner::fer()
+{
+ this->cc().st(this->cc().wb() / this->cc().mtr());
+ if (!this->ps().right())
+ this->cc().st(this->cc().st() * -1);
+ this->cc().sp(0.1);
+ while (!this->left()) {
+ while (!this->collide() && !this->left()) {
+ this->cc().next();
+ }
+ if (this->left() && !this->collide()) {
+ break;
+ } else {
+ this->cc().sp(this->cc().sp() * -1);
+ this->cc().next();
+ this->cc().st(this->cc().st() * -1);
+ }
+ }
+}
+
PSPlanner::PSPlanner()
{
}
+
+std::tuple<bool, double, double> intersect(
+ double x1, double y1,
+ double x2, double y2,
+ double x3, double y3,
+ double x4, double y4
+)
+{
+ double deno = (x1 - x2) * (y3 - y4) - (y1 - y2) * (x3 - x4);
+ if (deno == 0)
+ return std::make_tuple(false, 0, 0);
+ double t = (x1 - x3) * (y3 - y4) - (y1 - y3) * (x3 - x4);
+ t /= deno;
+ double u = (x1 - x2) * (y1 - y3) - (y1 - y2) * (x1 - x3);
+ u *= -1;
+ u /= deno;
+ if (t < 0 || t > 1 || u < 0 || u > 1)
+ return std::make_tuple(false, 0, 0);
+ return std::make_tuple(true, x1 + t * (x2 - x1), y1 + t * (y2 - y1));
+}
WVTEST_MAIN("parking slot planner basic test")
{
- PSPlanner *psp = new PSPlanner();
+ PSPlanner psp;
+ psp.ps().border(3, 3, 5, 3, 5, 8, 3, 8);
+ psp.gc().x(4);
+ psp.gc().y(4);
+ psp.gc().h(M_PI / 2);
+ psp.gc().mtr(10);
+ psp.gc().wb(2);
+ psp.gc().w(1);
+ psp.gc().l(3);
+ psp.gc().he(1.5);
+ psp.gc().df(2 + 0.5);
+ psp.gc().dr(0.5);
+ psp.cc() = BicycleCar(psp.gc());
+
+ // init orientation
+ WVPASS(!psp.collide());
+ WVPASSEQ_DOUBLE(psp.ps().heading(), psp.gc().h(), 0.00001);
+
+ // entry point found by reverse
+ WVPASS(!psp.left());
+ psp.fer();
+ WVPASS(psp.left());
+
+ // collide
+ auto tmpi1 = intersect(1, 1, 3, 3, 1, 3, 3, 1);
+ WVPASS(std::get<0>(tmpi1));
+ WVPASSEQ_DOUBLE(std::get<1>(tmpi1), 2, 0.00001);
+ WVPASSEQ_DOUBLE(std::get<2>(tmpi1), 2, 0.00001);
+ auto tmpi2 = intersect(1, 1, 1, 3, 3, 1, 3, 3);
+ WVPASS(!std::get<0>(tmpi2));
+ PSPlanner tpsp;
+ tpsp.ps().border(2, 3, 4, 3, 4, 8, 2, 8);
+ tpsp.gc() = BicycleCar(psp.gc());
+ tpsp.cc() = BicycleCar(tpsp.gc());
+ WVPASS(tpsp.collide());
+ WVPASS(!tpsp.left());
+ tpsp.ps().border(3, 4.1, 3, 2.1, 8, 2.1, 8, 4.1);
+ WVPASS(tpsp.left());
}
projects / hubacji1 / psp.git / commitdiff