diff options
| -rw-r--r-- | Makefile | 2 | ||||
| -rw-r--r-- | geom.hpp | 46 | ||||
| -rw-r--r-- | problem.cpp | 35 | ||||
| -rw-r--r-- | problem.hpp | 35 |
4 files changed, 91 insertions, 27 deletions
@@ -1,4 +1,4 @@ BIN=pathfind $(BIN): *.cpp *.hpp - g++ -o $@ *.cpp -lm -O2 + g++ -o $@ *.cpp -lm -O2 -std=c++11 @@ -2,6 +2,9 @@ #include <math.h> +#define EPSILON 1e-6 +#define abs(x) ((x)<0?-(x):(x)) + struct vec { double x, y; @@ -10,6 +13,9 @@ struct vec { double norm() const { return sqrt(x*x + y*y); } + double sqnorm() const { + return x*x + y*y; + } double angle() const { double xx = x / norm(); @@ -23,33 +29,20 @@ struct vec { } bool is_nil() const { - return x == 0 && y == 0; - } - - vec operator+ (const vec& o) const { - return vec(x + o.x, y + o.y); - } - vec operator- (const vec& o) const { - return vec(x - o.x, y - o.y); - } - vec operator* (double m) const { - return vec(m * x, m * y); - } - vec operator/ (double m) const { - return vec(m / x, m / y); + return sqnorm() < EPSILON; } static vec from_polar(double r, double theta) { return vec(r * cos(theta), r * sin(theta)); } - static vec dot(vec a, vec b) { // dot product (produit scalaire) + static double dot(vec a, vec b) { // dot product (produit scalaire) return a.x * b.x + a.y * b.y; } - static vec cross(vec a, vec b) { // cross product (déterminant 2x2) + static double cross(vec a, vec b) { // cross product (déterminant 2x2) return a.x * b.y - a.y * b.x; } - static vec angle(vec a, vec b) { // oriented angle between two vectors + static double angle(vec a, vec b) { // oriented angle between two vectors if (a.is_nil() || b.is_nil()) return 0; float cos = dot(a.normalize(), b.normalize()); if (cos <= -1) return M_PI; @@ -62,6 +55,13 @@ struct vec { } }; +inline vec operator+(const vec& a, const vec& b) { return vec(a.x+b.x, a.y+b.y); } +inline vec operator-(const vec& a, const vec& b) { return vec(a.x-b.x, a.y-b.y); } +inline vec operator-(const vec& a) { return vec(-a.x, -a.y); } +inline vec operator*(double a, const vec& v) { return vec(a*v.x, a*v.y); } +inline vec operator*(const vec& v, double a) { return vec(a*v.x, a*v.y); } +inline vec operator/(const vec& v, double a) { return vec(v.x/a, v.y/a); } + struct line { // Line defined by ax + by + c = 0 double a, b, c; @@ -74,12 +74,12 @@ struct line { } bool on_line(vec p) const { - return a * p.x + b * p.y + c == 0; + return a * p.x + b * p.y + c < EPSILON; } double dist(vec p) const { // calculate distance from p to the line - return abs(a*p.x+b*p.y+c)/sqrt(a*a+b*b); + return abs(a*p.x + b*p.y + c) / sqrt(a*a + b*b); } vec proj(vec p) const { @@ -106,7 +106,7 @@ struct segment { } double dist(vec p) const { - + //TODO return 1; } }; @@ -118,8 +118,8 @@ struct circle { circle(double x, double y, double rr) : c(x, y), r(rr) {} circle(vec cc, double rr) : c(cc), r(rr) {} - bool on_circle(vec.p) const { - return ((p - c).norm() == r); + bool on_circle(vec p) const { + return ((p - c).norm() - r < EPSILON); } double dist(vec p) const { @@ -133,7 +133,7 @@ struct circpoint { double theta; circpoint(circle cc, double th) : c(cc), theta(th) {} - circpoint(vec cc, double rr, double th : c(cc, rr), theta(th) {} + circpoint(vec cc, double rr, double th) : c(cc, rr), theta(th) {} vec pos() const { return c.c + vec(c.r * cos(theta), c.r * sin(theta)); diff --git a/problem.cpp b/problem.cpp new file mode 100644 index 0000000..c196879 --- /dev/null +++ b/problem.cpp @@ -0,0 +1,35 @@ +#include "problem.hpp" + +using namespace std; + +// ===================================== // +// IMPLEMENTATION FOR CLASS HILARE_A_MVT // +// ===================================== // + +double hilare_a_mvt::length() { + // returns length traveled by the car + return domega * (center - from.pos()).norm(); +} + +bool hilare_a_mvt::intersects(const obstacle &o) const { + // TODO + return false; +} + +bool hilare_a_mvt::intersects(const problem &p) const { + for (auto i = p.map.begin(); i != p.map.end(); i++) { + if (intersects(*i)) return true; + } + return false; +} + +// ================================= // +// IMPLEMENTATION FOR CLASS SOLUTION // +// ================================= // + +solution solution::direct_sol(const hilare_a &pos_a, const hilare_a &pos_b) { + // TODO: try different possibilities and chose the shortest one + return solution(vector<hilare_a_mvt>()); +} + +/* vim: set ts=4 sw=4 tw=0 noet :*/ diff --git a/problem.hpp b/problem.hpp index 10adaf8..7097e0c 100644 --- a/problem.hpp +++ b/problem.hpp @@ -33,11 +33,40 @@ struct problem { hilare_a begin_pos, end_pos; }; +struct hilare_a_mvt { + // Describes an elementary movement : rotate car and run on a circle + + // Hilare se déplace sur un arc de cercle. Le chariot donne la contrainte + // par rapport à la droite sur laquelle se place le centre de ce cercle + // (c'est la droite perpendiculaire à dir_trolley() passant par pos_trolley()) + + // deux étapes dans le mouvement : + // - bien orienter la voiture (c'est l'angle dtheta_before) + // - avancer/reculer sur le cercle (c'est l'angle domega) + + hilare_a from, to; + vec center; + double dtheta_before; // rotation de la voiture sur elle-même avant + double domega; // angle parcouru sur le cercle de centre center + + double length(); // length of a movement + + bool intersects(const obstacle& o) const; // intersects an obstacle ? + bool intersects(const problem &p) const; // intersects any obstacle on the map ? +}; + struct solution { - std::vector<hilare_a> movement; + std::vector<hilare_a_mvt> movement; + solution(const std::vector<hilare_a_mvt> &m) : movement(m) {} + + // simple direct solution from A to B, takes into account no obstacles + static solution direct_sol(const hilare_a &pos_a, const hilare_a &pos_b); + + // check if a solution intersects an obstacle of problem + bool intersects(const problem &p) const; - // TODO : décrire mieux un mouvement entre deux points (donner - // le centre de rotation, l'angle, etc.) + // calculate a solution + static solution make_solution(const problem &p); }; |