From 37553941f11e85cfddf6a51af7bf24fba3180e4b Mon Sep 17 00:00:00 2001 From: PF Date: Sun, 20 May 2018 15:15:07 +0200 Subject: [PATCH 01/36] Obstacle abstract class --- sources/obstacles/obstacle.cpp | 15 +++++++++++++++ sources/obstacles/obstacle.h | 26 ++++++++++++++++++++++++++ 2 files changed, 41 insertions(+) create mode 100644 sources/obstacles/obstacle.cpp create mode 100644 sources/obstacles/obstacle.h diff --git a/sources/obstacles/obstacle.cpp b/sources/obstacles/obstacle.cpp new file mode 100644 index 0000000..95d6acb --- /dev/null +++ b/sources/obstacles/obstacle.cpp @@ -0,0 +1,15 @@ +#include "obstacle.h" + + +namespace kraken +{ + + Obstacle::Obstacle(const Vector2D &rotation_center) : rotation_center_(rotation_center) + { + } + + bool Obstacle::operator==(const Obstacle &rhs) const + { + return rotation_center_ == rhs.rotation_center_; + } +} \ No newline at end of file diff --git a/sources/obstacles/obstacle.h b/sources/obstacles/obstacle.h new file mode 100644 index 0000000..c55ff08 --- /dev/null +++ b/sources/obstacles/obstacle.h @@ -0,0 +1,26 @@ +#ifndef TESTS_OBSTACLE_H +#define TESTS_OBSTACLE_H + +#include "../struct/vector_2d.h" + +namespace kraken +{ + + class Obstacle + { + public: + virtual bool isInObstacle(Vector2D pos) = 0; + virtual float squaredDistance(Vector2D pos) = 0; + virtual Vector2D* getExpandedConvexHull(float expansion, float longestAllowedLength) = 0; + virtual bool isColliding(Vector2D point_a, Vector2D point_b) = 0; + Obstacle(const Vector2D &rotation_center); + bool operator==(const Obstacle &rhs) const; + //virtual bool isColliding(RectangularObstacle obs) + + protected: + Vector2D rotation_center_; + + }; +} + +#endif //TESTS_OBSTACLE_H From c0357643ca7f32d0bb7764017af09783c5329ad4 Mon Sep 17 00:00:00 2001 From: PF Date: Sun, 20 May 2018 16:40:49 +0200 Subject: [PATCH 02/36] =?UTF-8?q?Better=20using=20references=E2=80=A6=20(I?= =?UTF-8?q?'m=20a=20noob)?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- sources/obstacles/obstacle.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/sources/obstacles/obstacle.h b/sources/obstacles/obstacle.h index c55ff08..c42eba2 100644 --- a/sources/obstacles/obstacle.h +++ b/sources/obstacles/obstacle.h @@ -9,11 +9,11 @@ namespace kraken class Obstacle { public: - virtual bool isInObstacle(Vector2D pos) = 0; - virtual float squaredDistance(Vector2D pos) = 0; - virtual Vector2D* getExpandedConvexHull(float expansion, float longestAllowedLength) = 0; - virtual bool isColliding(Vector2D point_a, Vector2D point_b) = 0; Obstacle(const Vector2D &rotation_center); + virtual bool isInObstacle(const Vector2D &pos) const = 0; + virtual float squaredDistance(const Vector2D &pos) const = 0; + virtual Vector2D* getExpandedConvexHull(float expansion, float longestAllowedLength) const = 0; + virtual bool isColliding(const Vector2D &point_a, const Vector2D &point_b) const = 0; bool operator==(const Obstacle &rhs) const; //virtual bool isColliding(RectangularObstacle obs) From 614840bafeeecf22208cd6cadad09d9d0ea93c6b Mon Sep 17 00:00:00 2001 From: PF Date: Sun, 20 May 2018 17:15:04 +0200 Subject: [PATCH 03/36] Copy constructor for Vector2D --- sources/struct/vector_2d.cpp | 5 +++++ sources/struct/vector_2d.h | 1 + 2 files changed, 6 insertions(+) diff --git a/sources/struct/vector_2d.cpp b/sources/struct/vector_2d.cpp index 6e601df..7d7ff8a 100644 --- a/sources/struct/vector_2d.cpp +++ b/sources/struct/vector_2d.cpp @@ -16,6 +16,11 @@ namespace kraken } + Vector2D::Vector2D(const Vector2D &vector) : x_(vector.getX()), y_(vector.getY()) + { + + } + Vector2D Vector2D::operator+(const Vector2D &rhs) const { return Vector2D(x_ + rhs.x_, y_ + rhs.y_); diff --git a/sources/struct/vector_2d.h b/sources/struct/vector_2d.h index 4f7d639..0efc4e1 100644 --- a/sources/struct/vector_2d.h +++ b/sources/struct/vector_2d.h @@ -13,6 +13,7 @@ namespace kraken public: Vector2D(); Vector2D(const float &x, const float &y); + Vector2D(const Vector2D &vector); Vector2D operator+(const Vector2D &rhs) const; Vector2D &operator+=(const Vector2D &rhs); From 2b22ab555d61433841aaa34fdeee7579fbdd74e9 Mon Sep 17 00:00:00 2001 From: PF Date: Sun, 20 May 2018 17:15:19 +0200 Subject: [PATCH 04/36] CircularObstacle class --- sources/obstacles/circular_obstacle.cpp | 60 +++++++++++++++++++++++++ sources/obstacles/circular_obstacle.h | 25 +++++++++++ sources/obstacles/obstacle.h | 1 + 3 files changed, 86 insertions(+) create mode 100644 sources/obstacles/circular_obstacle.cpp create mode 100644 sources/obstacles/circular_obstacle.h diff --git a/sources/obstacles/circular_obstacle.cpp b/sources/obstacles/circular_obstacle.cpp new file mode 100644 index 0000000..8187b1c --- /dev/null +++ b/sources/obstacles/circular_obstacle.cpp @@ -0,0 +1,60 @@ +#include "obstacle.h" +#include "circular_obstacle.h" +#include "../struct/vector_2d.h" +#include + +namespace kraken +{ + CircularObstacle::CircularObstacle(const Vector2D &pos, float radius) : + Obstacle(pos), radius_(radius), squared_radius_(radius * radius) + { + } + + bool CircularObstacle::isInObstacle(const Vector2D &pos) const + { + return pos.squaredDistance(rotation_center_) <= squared_radius_; + } + + float CircularObstacle::squaredDistance(const Vector2D &pos) const + { + // TODO : is "distance" really necessary ? (it uses a std::sqrt computation) + float out = std::max(0.f, pos.distance(rotation_center_) - radius_); + return out * out; + } + + bool CircularObstacle::isColliding(const Vector2D &point_a, const Vector2D &point_b) const + { + const Vector2D point_c = rotation_center_; + const Vector2D ab = point_b - point_a; + const Vector2D ac = point_c - point_a; + float numerator = std::abs(ab.getX() * ac.getY() - ab.getY() * ac.getX()); + float denominator = ab.squaredNorm(); + float distance = numerator * numerator / denominator; + + // no collision with the line (AB) + if (distance > squared_radius_) + return false; + + // collision with the segment [AB] + float pscal1 = ab.dot(ac); + float pscal2 = -ab.dot(point_c - point_b); + if (pscal1 >= 0 && pscal2 >= 0) + return true; + + // check if A or B is in the circle + return isInObstacle(point_a) || isInObstacle(point_b); + } + + Vector2D *CircularObstacle::getExpandedConvexHull(float expansion, float longestAllowedLength) const + { + int nbPoints = std::ceil(M_2_PI * (radius_ + expansion) / longestAllowedLength); + if (nbPoints < 3) + nbPoints = 3; + + Vector2D* hull = new Vector2D[nbPoints]; + for(int i = 0; i < nbPoints; ++i) + hull[i] = Vector2D::fromPolar(expansion + radius_, i * M_2_PI / nbPoints) + rotation_center_; + + return hull; + } +} \ No newline at end of file diff --git a/sources/obstacles/circular_obstacle.h b/sources/obstacles/circular_obstacle.h new file mode 100644 index 0000000..7cfcf56 --- /dev/null +++ b/sources/obstacles/circular_obstacle.h @@ -0,0 +1,25 @@ +#ifndef TESTS_CIRCULAR_OBSTACLE_H +#define TESTS_CIRCULAR_OBSTACLE_H + +#include "../struct/vector_2d.h" +#include "obstacle.h" + +namespace kraken +{ + class CircularObstacle : public Obstacle + { + public: + CircularObstacle(const Vector2D &pos, float radius); + ~CircularObstacle() = 0; + bool isInObstacle(const Vector2D &pos) const; + float squaredDistance(const Vector2D &pos) const; + Vector2D* getExpandedConvexHull(float expansion, float longestAllowedLength) const; + bool isColliding(const Vector2D &point_a, const Vector2D &point_b) const; + + protected: + float const radius_; + float const squared_radius_; + }; +} + +#endif //TESTS_CIRCULAR_OBSTACLE_H diff --git a/sources/obstacles/obstacle.h b/sources/obstacles/obstacle.h index c42eba2..f82e21a 100644 --- a/sources/obstacles/obstacle.h +++ b/sources/obstacles/obstacle.h @@ -10,6 +10,7 @@ namespace kraken { public: Obstacle(const Vector2D &rotation_center); + virtual ~Obstacle() = 0; virtual bool isInObstacle(const Vector2D &pos) const = 0; virtual float squaredDistance(const Vector2D &pos) const = 0; virtual Vector2D* getExpandedConvexHull(float expansion, float longestAllowedLength) const = 0; From a932538b529d0e4befb21a8e018cca08a3b7f5f9 Mon Sep 17 00:00:00 2001 From: PF Date: Sun, 20 May 2018 17:18:32 +0200 Subject: [PATCH 05/36] Add operator<< in CircularObstacle class --- sources/obstacles/circular_obstacle.cpp | 8 ++++++++ sources/obstacles/circular_obstacle.h | 8 ++++++++ 2 files changed, 16 insertions(+) diff --git a/sources/obstacles/circular_obstacle.cpp b/sources/obstacles/circular_obstacle.cpp index 8187b1c..695a12e 100644 --- a/sources/obstacles/circular_obstacle.cpp +++ b/sources/obstacles/circular_obstacle.cpp @@ -57,4 +57,12 @@ namespace kraken return hull; } + +#if DEBUG + std::ostream &operator<<(std::ostream &strm, const CircularObstacle &o) + { + return strm << "CircularObstacle(" << o.rotation_center_ << "," << o.radius_ << ")" << std::endl; + } +#endif + } \ No newline at end of file diff --git a/sources/obstacles/circular_obstacle.h b/sources/obstacles/circular_obstacle.h index 7cfcf56..e0a2351 100644 --- a/sources/obstacles/circular_obstacle.h +++ b/sources/obstacles/circular_obstacle.h @@ -4,6 +4,10 @@ #include "../struct/vector_2d.h" #include "obstacle.h" +#if DEBUG +#include +#endif + namespace kraken { class CircularObstacle : public Obstacle @@ -16,6 +20,10 @@ namespace kraken Vector2D* getExpandedConvexHull(float expansion, float longestAllowedLength) const; bool isColliding(const Vector2D &point_a, const Vector2D &point_b) const; + #if DEBUG + friend std::ostream &operator<<(std::ostream &strm, const CircularObstacle &o); + #endif + protected: float const radius_; float const squared_radius_; From 92144ae627fc95ed9b7286c1bd76914efefcb01a Mon Sep 17 00:00:00 2001 From: PF Date: Sun, 20 May 2018 17:58:55 +0200 Subject: [PATCH 06/36] Copy constructor is implicit --- sources/struct/vector_2d.cpp | 5 ----- sources/struct/vector_2d.h | 1 - 2 files changed, 6 deletions(-) diff --git a/sources/struct/vector_2d.cpp b/sources/struct/vector_2d.cpp index 7d7ff8a..6e601df 100644 --- a/sources/struct/vector_2d.cpp +++ b/sources/struct/vector_2d.cpp @@ -16,11 +16,6 @@ namespace kraken } - Vector2D::Vector2D(const Vector2D &vector) : x_(vector.getX()), y_(vector.getY()) - { - - } - Vector2D Vector2D::operator+(const Vector2D &rhs) const { return Vector2D(x_ + rhs.x_, y_ + rhs.y_); diff --git a/sources/struct/vector_2d.h b/sources/struct/vector_2d.h index 0efc4e1..4f7d639 100644 --- a/sources/struct/vector_2d.h +++ b/sources/struct/vector_2d.h @@ -13,7 +13,6 @@ namespace kraken public: Vector2D(); Vector2D(const float &x, const float &y); - Vector2D(const Vector2D &vector); Vector2D operator+(const Vector2D &rhs) const; Vector2D &operator+=(const Vector2D &rhs); From 29222ec1f73d62bf8c563a7fb8560a4f4f343103 Mon Sep 17 00:00:00 2001 From: PF Date: Sun, 20 May 2018 17:59:16 +0200 Subject: [PATCH 07/36] Add override to circular_obstacle.h --- sources/obstacles/circular_obstacle.h | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) diff --git a/sources/obstacles/circular_obstacle.h b/sources/obstacles/circular_obstacle.h index e0a2351..27d7d29 100644 --- a/sources/obstacles/circular_obstacle.h +++ b/sources/obstacles/circular_obstacle.h @@ -15,10 +15,10 @@ namespace kraken public: CircularObstacle(const Vector2D &pos, float radius); ~CircularObstacle() = 0; - bool isInObstacle(const Vector2D &pos) const; - float squaredDistance(const Vector2D &pos) const; - Vector2D* getExpandedConvexHull(float expansion, float longestAllowedLength) const; - bool isColliding(const Vector2D &point_a, const Vector2D &point_b) const; + bool isInObstacle(const Vector2D &pos) const override; + float squaredDistance(const Vector2D &pos) const override; + Vector2D* getExpandedConvexHull(float expansion, float longestAllowedLength) const override; + bool isColliding(const Vector2D &point_a, const Vector2D &point_b) const override; #if DEBUG friend std::ostream &operator<<(std::ostream &strm, const CircularObstacle &o); From d680994ea58ae2a26f1bcb3e6b329c9fe32843fb Mon Sep 17 00:00:00 2001 From: PF Date: Sun, 20 May 2018 18:08:09 +0200 Subject: [PATCH 08/36] Style --- sources/obstacles/circular_obstacle.cpp | 6 ++++-- sources/obstacles/circular_obstacle.h | 8 +++++--- sources/obstacles/obstacle.h | 2 +- 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/sources/obstacles/circular_obstacle.cpp b/sources/obstacles/circular_obstacle.cpp index 695a12e..09ff076 100644 --- a/sources/obstacles/circular_obstacle.cpp +++ b/sources/obstacles/circular_obstacle.cpp @@ -51,18 +51,20 @@ namespace kraken if (nbPoints < 3) nbPoints = 3; - Vector2D* hull = new Vector2D[nbPoints]; - for(int i = 0; i < nbPoints; ++i) + Vector2D *hull = new Vector2D[nbPoints]; + for (int i = 0; i < nbPoints; ++i) hull[i] = Vector2D::fromPolar(expansion + radius_, i * M_2_PI / nbPoints) + rotation_center_; return hull; } #if DEBUG + std::ostream &operator<<(std::ostream &strm, const CircularObstacle &o) { return strm << "CircularObstacle(" << o.rotation_center_ << "," << o.radius_ << ")" << std::endl; } + #endif } \ No newline at end of file diff --git a/sources/obstacles/circular_obstacle.h b/sources/obstacles/circular_obstacle.h index 27d7d29..968f578 100644 --- a/sources/obstacles/circular_obstacle.h +++ b/sources/obstacles/circular_obstacle.h @@ -5,7 +5,9 @@ #include "obstacle.h" #if DEBUG + #include + #endif namespace kraken @@ -17,12 +19,12 @@ namespace kraken ~CircularObstacle() = 0; bool isInObstacle(const Vector2D &pos) const override; float squaredDistance(const Vector2D &pos) const override; - Vector2D* getExpandedConvexHull(float expansion, float longestAllowedLength) const override; + Vector2D *getExpandedConvexHull(float expansion, float longestAllowedLength) const override; bool isColliding(const Vector2D &point_a, const Vector2D &point_b) const override; - #if DEBUG +#if DEBUG friend std::ostream &operator<<(std::ostream &strm, const CircularObstacle &o); - #endif +#endif protected: float const radius_; diff --git a/sources/obstacles/obstacle.h b/sources/obstacles/obstacle.h index f82e21a..7b082a8 100644 --- a/sources/obstacles/obstacle.h +++ b/sources/obstacles/obstacle.h @@ -13,7 +13,7 @@ namespace kraken virtual ~Obstacle() = 0; virtual bool isInObstacle(const Vector2D &pos) const = 0; virtual float squaredDistance(const Vector2D &pos) const = 0; - virtual Vector2D* getExpandedConvexHull(float expansion, float longestAllowedLength) const = 0; + virtual Vector2D *getExpandedConvexHull(float expansion, float longestAllowedLength) const = 0; virtual bool isColliding(const Vector2D &point_a, const Vector2D &point_b) const = 0; bool operator==(const Obstacle &rhs) const; //virtual bool isColliding(RectangularObstacle obs) From b6bf67e8d12cb774089c88ad295e42fef3cea456 Mon Sep 17 00:00:00 2001 From: bourreP Date: Sun, 20 May 2018 18:53:00 +0200 Subject: [PATCH 09/36] no more pointers --- sources/obstacles/circular_obstacle.cpp | 10 +++++----- sources/obstacles/circular_obstacle.h | 2 +- sources/obstacles/obstacle.h | 3 ++- 3 files changed, 8 insertions(+), 7 deletions(-) diff --git a/sources/obstacles/circular_obstacle.cpp b/sources/obstacles/circular_obstacle.cpp index 09ff076..c75f048 100644 --- a/sources/obstacles/circular_obstacle.cpp +++ b/sources/obstacles/circular_obstacle.cpp @@ -45,17 +45,17 @@ namespace kraken return isInObstacle(point_a) || isInObstacle(point_b); } - Vector2D *CircularObstacle::getExpandedConvexHull(float expansion, float longestAllowedLength) const + void CircularObstacle::getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, std::vector &vector_2d_list) const { int nbPoints = std::ceil(M_2_PI * (radius_ + expansion) / longestAllowedLength); if (nbPoints < 3) nbPoints = 3; - Vector2D *hull = new Vector2D[nbPoints]; - for (int i = 0; i < nbPoints; ++i) - hull[i] = Vector2D::fromPolar(expansion + radius_, i * M_2_PI / nbPoints) + rotation_center_; + vector_2d_list.clear(); + vector_2d_list.resize(nbPoints); - return hull; + for (int i = 0; i < nbPoints; ++i) + vector_2d_list.push_back(Vector2D::fromPolar(expansion + radius_, i * M_2_PI / nbPoints) + rotation_center_); } #if DEBUG diff --git a/sources/obstacles/circular_obstacle.h b/sources/obstacles/circular_obstacle.h index 968f578..00a2378 100644 --- a/sources/obstacles/circular_obstacle.h +++ b/sources/obstacles/circular_obstacle.h @@ -19,7 +19,7 @@ namespace kraken ~CircularObstacle() = 0; bool isInObstacle(const Vector2D &pos) const override; float squaredDistance(const Vector2D &pos) const override; - Vector2D *getExpandedConvexHull(float expansion, float longestAllowedLength) const override; + void getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, std::vector &vector_2d_list) const override; bool isColliding(const Vector2D &point_a, const Vector2D &point_b) const override; #if DEBUG diff --git a/sources/obstacles/obstacle.h b/sources/obstacles/obstacle.h index 7b082a8..d512c7f 100644 --- a/sources/obstacles/obstacle.h +++ b/sources/obstacles/obstacle.h @@ -1,6 +1,7 @@ #ifndef TESTS_OBSTACLE_H #define TESTS_OBSTACLE_H +#include #include "../struct/vector_2d.h" namespace kraken @@ -13,7 +14,7 @@ namespace kraken virtual ~Obstacle() = 0; virtual bool isInObstacle(const Vector2D &pos) const = 0; virtual float squaredDistance(const Vector2D &pos) const = 0; - virtual Vector2D *getExpandedConvexHull(float expansion, float longestAllowedLength) const = 0; + virtual void getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, std::vector &vector_2d_list) const = 0; virtual bool isColliding(const Vector2D &point_a, const Vector2D &point_b) const = 0; bool operator==(const Obstacle &rhs) const; //virtual bool isColliding(RectangularObstacle obs) From 2910d7f9cb7422cdc7cab50cb103da3fea25062f Mon Sep 17 00:00:00 2001 From: Kayou Date: Mon, 21 May 2018 16:52:34 +0200 Subject: [PATCH 10/36] Fix virtual destructor --- sources/obstacles/circular_obstacle.h | 1 - sources/obstacles/obstacle.h | 2 +- 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/sources/obstacles/circular_obstacle.h b/sources/obstacles/circular_obstacle.h index 00a2378..19f1d13 100644 --- a/sources/obstacles/circular_obstacle.h +++ b/sources/obstacles/circular_obstacle.h @@ -16,7 +16,6 @@ namespace kraken { public: CircularObstacle(const Vector2D &pos, float radius); - ~CircularObstacle() = 0; bool isInObstacle(const Vector2D &pos) const override; float squaredDistance(const Vector2D &pos) const override; void getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, std::vector &vector_2d_list) const override; diff --git a/sources/obstacles/obstacle.h b/sources/obstacles/obstacle.h index d512c7f..2c19fa6 100644 --- a/sources/obstacles/obstacle.h +++ b/sources/obstacles/obstacle.h @@ -11,7 +11,7 @@ namespace kraken { public: Obstacle(const Vector2D &rotation_center); - virtual ~Obstacle() = 0; + virtual ~Obstacle() = default; virtual bool isInObstacle(const Vector2D &pos) const = 0; virtual float squaredDistance(const Vector2D &pos) const = 0; virtual void getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, std::vector &vector_2d_list) const = 0; From 3745205deca86a1f9dad61df52e7d69fc8472c4a Mon Sep 17 00:00:00 2001 From: PF Date: Wed, 23 May 2018 12:50:01 +0200 Subject: [PATCH 11/36] operator== --- sources/obstacles/circular_obstacle.cpp | 8 ++++++++ sources/obstacles/circular_obstacle.h | 1 + sources/obstacles/obstacle.h | 2 +- 3 files changed, 10 insertions(+), 1 deletion(-) diff --git a/sources/obstacles/circular_obstacle.cpp b/sources/obstacles/circular_obstacle.cpp index c75f048..f51c5a0 100644 --- a/sources/obstacles/circular_obstacle.cpp +++ b/sources/obstacles/circular_obstacle.cpp @@ -58,6 +58,14 @@ namespace kraken vector_2d_list.push_back(Vector2D::fromPolar(expansion + radius_, i * M_2_PI / nbPoints) + rotation_center_); } + bool CircularObstacle::operator==(const Obstacle &rhs) const + { + if(!Obstacle::operator==(rhs)) + return false; + + return radius_ == static_cast(rhs).radius_; + } + #if DEBUG std::ostream &operator<<(std::ostream &strm, const CircularObstacle &o) diff --git a/sources/obstacles/circular_obstacle.h b/sources/obstacles/circular_obstacle.h index 19f1d13..85e2fbf 100644 --- a/sources/obstacles/circular_obstacle.h +++ b/sources/obstacles/circular_obstacle.h @@ -20,6 +20,7 @@ namespace kraken float squaredDistance(const Vector2D &pos) const override; void getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, std::vector &vector_2d_list) const override; bool isColliding(const Vector2D &point_a, const Vector2D &point_b) const override; + bool operator==(const Obstacle &rhs) const override; #if DEBUG friend std::ostream &operator<<(std::ostream &strm, const CircularObstacle &o); diff --git a/sources/obstacles/obstacle.h b/sources/obstacles/obstacle.h index 2c19fa6..55d5ae7 100644 --- a/sources/obstacles/obstacle.h +++ b/sources/obstacles/obstacle.h @@ -16,7 +16,7 @@ namespace kraken virtual float squaredDistance(const Vector2D &pos) const = 0; virtual void getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, std::vector &vector_2d_list) const = 0; virtual bool isColliding(const Vector2D &point_a, const Vector2D &point_b) const = 0; - bool operator==(const Obstacle &rhs) const; + virtual bool operator==(const Obstacle &rhs) const; //virtual bool isColliding(RectangularObstacle obs) protected: From f4a9cba7a2fe304eb6f7b80b3c49a20863154bbc Mon Sep 17 00:00:00 2001 From: PF Date: Wed, 23 May 2018 13:51:46 +0200 Subject: [PATCH 12/36] Check the typeid before static_cast --- sources/obstacles/circular_obstacle.cpp | 6 +++++- 1 file changed, 5 insertions(+), 1 deletion(-) diff --git a/sources/obstacles/circular_obstacle.cpp b/sources/obstacles/circular_obstacle.cpp index f51c5a0..1c574b0 100644 --- a/sources/obstacles/circular_obstacle.cpp +++ b/sources/obstacles/circular_obstacle.cpp @@ -2,6 +2,7 @@ #include "circular_obstacle.h" #include "../struct/vector_2d.h" #include +#include namespace kraken { @@ -63,6 +64,9 @@ namespace kraken if(!Obstacle::operator==(rhs)) return false; + if(typeid(*this) != typeid(rhs)) + return false; + return radius_ == static_cast(rhs).radius_; } @@ -75,4 +79,4 @@ namespace kraken #endif -} \ No newline at end of file +} From a51a91031003beeb8cabc5557425f23749a17710 Mon Sep 17 00:00:00 2001 From: Kayou Date: Wed, 23 May 2018 17:13:11 +0200 Subject: [PATCH 13/36] Add constructor --- sources/obstacles/RectangularObstacle.cpp | 76 +++++++++++++++++++++++ sources/obstacles/RectangularObstacle.h | 44 +++++++++++++ sources/struct/vector_2d.cpp | 5 ++ sources/struct/vector_2d.h | 1 + 4 files changed, 126 insertions(+) create mode 100644 sources/obstacles/RectangularObstacle.cpp create mode 100644 sources/obstacles/RectangularObstacle.h diff --git a/sources/obstacles/RectangularObstacle.cpp b/sources/obstacles/RectangularObstacle.cpp new file mode 100644 index 0000000..0281530 --- /dev/null +++ b/sources/obstacles/RectangularObstacle.cpp @@ -0,0 +1,76 @@ +#include "RectangularObstacle.h" +#include + +namespace kraken +{ + + RectangularObstacle::RectangularObstacle(const Vector2D &position, const int &size_x, const int &size_y) : + RectangularObstacle(position, size_x, size_y, 0) + { + + } + + RectangularObstacle::RectangularObstacle(const Vector2D &position, const int &size_x, const int &size_y, + const float &angle) : + RectangularObstacle(position, size_x / 2, size_x / 2, size_y / 2, size_y / 2, angle) + { + + } + + RectangularObstacle::RectangularObstacle(const int &distance_to_front, const int &distance_to_back, + const int &distance_to_left, const int &distance_to_right) : + RectangularObstacle(distance_to_front, distance_to_back, distance_to_left, distance_to_right, 0) + { + + } + + RectangularObstacle::RectangularObstacle(const int &distance_to_front, const int &distance_to_back, + const int &distance_to_left, const int &distance_to_right, + const float &tilt_angle) : + RectangularObstacle(Vector2D(0, 0), distance_to_back, distance_to_front, distance_to_left, + distance_to_right, tilt_angle) + { + + } + + RectangularObstacle::RectangularObstacle(const Vector2D &position, const int &size_left_x, const int &size_right_x, + const int &size_up_y, const int &size_down_y, const float &angle) : + RectangularObstacle(position, Vector2D(size_right_x, size_up_y), Vector2D(-size_left_x, -size_down_y), + angle) + { + + } + + RectangularObstacle::RectangularObstacle(const Vector2D &position, const Vector2D &top_right_corner, + const Vector2D &bottom_left_corner, const float &angle) : + Obstacle(position), angle_(angle), cos_(std::cos(angle)), sin_(std::sin(angle)), + left_bottom_corner_(bottom_left_corner), + left_upper_corner_(bottom_left_corner.getX(), top_right_corner.getY()), + right_bottom_corner_(top_right_corner.getX(), bottom_left_corner.getY()), + right_upper_corner_(top_right_corner), + left_bottom_corner_rotate_(toTableCoordinateSystem(left_bottom_corner_)), + left_upper_corner_rotate_(toTableCoordinateSystem(left_upper_corner_)), + right_bottom_corner_rotate_(toTableCoordinateSystem(right_bottom_corner_)), + right_upper_corner_rotate_(toTableCoordinateSystem(right_upper_corner_)), + geometric_center_((right_bottom_corner_rotate_ + left_upper_corner_rotate_) * 0.5f), + half_diagonal_(top_right_corner.distance(bottom_left_corner) / 2.f) + { + + } + + Vector2D RectangularObstacle::toObstacleCoordinateSystem(const Vector2D &point) + { + Vector2D out; + out.setX(cos_ * (point.getX() - rotation_center_.getX()) + sin_ * (point.getY() - rotation_center_.getY())); + out.setY(-sin_ * (point.getX() - rotation_center_.getX()) + cos_ * (point.getY() - rotation_center_.getY())); + return out; + } + + Vector2D RectangularObstacle::toTableCoordinateSystem(const Vector2D &point) + { + Vector2D out; + out.setX(cos_ * point.getX() - sin_ * point.getY() + rotation_center_.getX()); + out.setY(sin_ * point.getX() + cos_ * point.getY() + rotation_center_.getY()); + return out; + } +} \ No newline at end of file diff --git a/sources/obstacles/RectangularObstacle.h b/sources/obstacles/RectangularObstacle.h new file mode 100644 index 0000000..87a8409 --- /dev/null +++ b/sources/obstacles/RectangularObstacle.h @@ -0,0 +1,44 @@ +#ifndef TESTS_RECTANGULAR_OBSTACLE_H +#define TESTS_RECTANGULAR_OBSTACLE_H + +#include "Obstacle.h" +#include "../struct/vector_2d.h" + +namespace kraken +{ + class RectangularObstacle : public Obstacle + { + public: + RectangularObstacle(const Vector2D &position, const int &size_x, const int &size_y); + RectangularObstacle(const Vector2D &position, const int &size_x, const int &size_y, const float &angle); + RectangularObstacle(const int &distance_to_front, const int &distance_to_back, const int &distance_to_left, const int &distance_to_right); + RectangularObstacle(const int &distance_to_front, const int &distance_to_back, const int &distance_to_left, const int &distance_to_right, const float &tilt_angle); + RectangularObstacle(const Vector2D &position, const int &size_left_x, const int &size_right_x, const int &size_up_y, const int &size_down_y, const float &angle); + RectangularObstacle(const Vector2D &position, const Vector2D &top_right_corner, const Vector2D &bottom_left_corner, const float &angle); + + protected: + Vector2D toObstacleCoordinateSystem(const Vector2D &point); + Vector2D toTableCoordinateSystem(const Vector2D &point); + protected: + float angle_; + float cos_; + float sin_; + + // these corners are in the obstacle coordinate system + Vector2D left_bottom_corner_; + Vector2D left_upper_corner_; + Vector2D right_bottom_corner_; + Vector2D right_upper_corner_; + + // these corners are in the table coordinate system + Vector2D left_bottom_corner_rotate_; + Vector2D left_upper_corner_rotate_; + Vector2D right_bottom_corner_rotate_; + Vector2D right_upper_corner_rotate_; + + Vector2D geometric_center_; + float half_diagonal_; + }; +} + +#endif //TESTS_RECTANGULAR_OBSTACLE_H diff --git a/sources/struct/vector_2d.cpp b/sources/struct/vector_2d.cpp index 6e601df..5694f18 100644 --- a/sources/struct/vector_2d.cpp +++ b/sources/struct/vector_2d.cpp @@ -40,6 +40,11 @@ namespace kraken return *this; } + Vector2D Vector2D::operator*(const float &d) const + { + return Vector2D(x_ * d, y_ * d); + } + Vector2D &Vector2D::operator*=(const float &d) { x_ *= d; diff --git a/sources/struct/vector_2d.h b/sources/struct/vector_2d.h index 4f7d639..1c70161 100644 --- a/sources/struct/vector_2d.h +++ b/sources/struct/vector_2d.h @@ -19,6 +19,7 @@ namespace kraken Vector2D operator-(const Vector2D &rhs) const; Vector2D &operator-=(const Vector2D &rhs); Vector2D &operator*=(const float &d); + Vector2D operator*(const float &d) const; bool operator==(const Vector2D &rhs) const; bool operator!=(const Vector2D &rhs) const; float dot(const Vector2D &other) const; From c801c28a112bd45003d4ae0c5a5a40ff331c9c58 Mon Sep 17 00:00:00 2001 From: Kayou Date: Wed, 23 May 2018 18:38:29 +0200 Subject: [PATCH 14/36] Implemente getExpandedConvexHull and squaredDistance --- sources/obstacles/RectangularObstacle.cpp | 82 +++++++++++++++++++++-- sources/obstacles/RectangularObstacle.h | 11 ++- 2 files changed, 87 insertions(+), 6 deletions(-) diff --git a/sources/obstacles/RectangularObstacle.cpp b/sources/obstacles/RectangularObstacle.cpp index 0281530..7d770a3 100644 --- a/sources/obstacles/RectangularObstacle.cpp +++ b/sources/obstacles/RectangularObstacle.cpp @@ -1,5 +1,6 @@ #include "RectangularObstacle.h" #include +#include namespace kraken { @@ -58,19 +59,92 @@ namespace kraken } - Vector2D RectangularObstacle::toObstacleCoordinateSystem(const Vector2D &point) + Vector2D RectangularObstacle::toObstacleCoordinateSystem(const Vector2D &point) const { Vector2D out; - out.setX(cos_ * (point.getX() - rotation_center_.getX()) + sin_ * (point.getY() - rotation_center_.getY())); - out.setY(-sin_ * (point.getX() - rotation_center_.getX()) + cos_ * (point.getY() - rotation_center_.getY())); + out.setX(getXToObstacleCoordinateSystem(point)); + out.setY(getYToObstacleCoordinateSystem(point)); return out; } - Vector2D RectangularObstacle::toTableCoordinateSystem(const Vector2D &point) + Vector2D RectangularObstacle::toTableCoordinateSystem(const Vector2D &point) const { Vector2D out; out.setX(cos_ * point.getX() - sin_ * point.getY() + rotation_center_.getX()); out.setY(sin_ * point.getX() + cos_ * point.getY() + rotation_center_.getY()); return out; } + + float RectangularObstacle::getXToObstacleCoordinateSystem(const Vector2D &point) const + { + return cos_ * (point.getX() - rotation_center_.getX()) + sin_ * (point.getY() - rotation_center_.getY()); + } + + float RectangularObstacle::getYToObstacleCoordinateSystem(const Vector2D &point) const + { + return -sin_ * (point.getX() - rotation_center_.getX()) + cos_ * (point.getY() - rotation_center_.getY()); + } + + void RectangularObstacle::getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, + std::vector &vector_2d_list) const + { + float coeff = expansion / half_diagonal_; + Vector2D corners[] = {(right_bottom_corner_rotate_ - geometric_center_) * coeff + right_bottom_corner_rotate_, + (right_upper_corner_rotate_ - geometric_center_) * coeff + right_upper_corner_rotate_, + (left_upper_corner_rotate_ - geometric_center_) * coeff + left_upper_corner_rotate_, + (left_bottom_corner_rotate_ - geometric_center_) * coeff + left_bottom_corner_rotate_}; + + vector_2d_list.clear(); + + for (auto const &point : point) + vector_2d_list.push_back(point); + + for (int i = 0; i < 4; i++) + { + Vector2D pointA = corners[i]; + Vector2D pointB = corners[(i + 1) % 4]; + float distance = pointA.distance(pointB); + int nbPoints = static_cast(std::ceil(distance / longestAllowedLength)); + float delta = distance / nbPoints; + for (int j = 1; j < nbPoints; j++) + vector_2d_list.push_back((pointB - pointA) * ((j * delta) / distance) + pointA); + } + } + + float RectangularObstacle::squaredDistance(const Vector2D &v) const + { + Vector2D in = toObstacleCoordinateSystem(v); + + if (in.getX() < left_bottom_corner_.getX() && in.getY() < left_bottom_corner_.getY()) + return in.squaredDistance(left_bottom_corner_); + + if (in.getX() < left_upper_corner_.getX() && in.getY() > left_upper_corner_.getY()) + return in.squaredDistance(left_upper_corner_); + + if (in.getX() > right_bottom_corner_.getX() && in.getY() < right_bottom_corner_.getY()) + return in.squaredDistance(right_bottom_corner_); + + if (in.getX() > right_upper_corner_.getX() && in.getY() > right_upper_corner_.getY()) + return in.squaredDistance(right_upper_corner_); + + if (in.getX() > right_upper_corner_.getX()) + return (in.getX() - right_upper_corner_.getX()) * (in.getX() - right_upper_corner_.getX()); + + if (in.getX() < left_bottom_corner_.getX()) + return (in.getX() - left_bottom_corner_.getX()) * (in.getX() - left_bottom_corner_.getX()); + + if (in.getY() > right_upper_corner_.getY()) + return (in.getY() - right_upper_corner_.getY()) * (in.getY() - right_upper_corner_.getY()); + + if (in.getY() < left_bottom_corner_.getY()) + return (in.getY() - left_bottom_corner_.getY()) * (in.getY() - left_bottom_corner_.getY()); + + // else, v is in the obstacle + return 0; + } + + float RectangularObstacle::getHalfDiagonal() const + { + return half_diagonal_; + } } \ No newline at end of file diff --git a/sources/obstacles/RectangularObstacle.h b/sources/obstacles/RectangularObstacle.h index 87a8409..0912aa6 100644 --- a/sources/obstacles/RectangularObstacle.h +++ b/sources/obstacles/RectangularObstacle.h @@ -16,9 +16,16 @@ namespace kraken RectangularObstacle(const Vector2D &position, const int &size_left_x, const int &size_right_x, const int &size_up_y, const int &size_down_y, const float &angle); RectangularObstacle(const Vector2D &position, const Vector2D &top_right_corner, const Vector2D &bottom_left_corner, const float &angle); + void getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, std::vector &vector_2d_list) const override; + float squaredDistance(const Vector2D &v) const override; + + float getHalfDiagonal() const; protected: - Vector2D toObstacleCoordinateSystem(const Vector2D &point); - Vector2D toTableCoordinateSystem(const Vector2D &point); + Vector2D toObstacleCoordinateSystem(const Vector2D &point) const; + Vector2D toTableCoordinateSystem(const Vector2D &point) const; + + float getXToObstacleCoordinateSystem(const Vector2D &point) const; + float getYToObstacleCoordinateSystem(const Vector2D &point) const; protected: float angle_; float cos_; From 98a476b08b7abb36d4b5be2d35f0b5c7e69d1f7f Mon Sep 17 00:00:00 2001 From: bourreP Date: Thu, 24 May 2018 01:11:19 +0200 Subject: [PATCH 15/36] Finish RectangularObstacle --- sources/obstacles/circular_obstacle.cpp | 23 +++-- sources/obstacles/circular_obstacle.h | 4 +- sources/obstacles/obstacle.cpp | 5 + sources/obstacles/obstacle.h | 7 +- ...rObstacle.cpp => rectangular_obstacle.cpp} | 93 ++++++++++++++++++- ...gularObstacle.h => rectangular_obstacle.h} | 31 ++++++- sources/struct/vector_2d.cpp | 2 +- sources/struct/vector_2d.h | 2 +- 8 files changed, 149 insertions(+), 18 deletions(-) rename sources/obstacles/{RectangularObstacle.cpp => rectangular_obstacle.cpp} (57%) rename sources/obstacles/{RectangularObstacle.h => rectangular_obstacle.h} (57%) diff --git a/sources/obstacles/circular_obstacle.cpp b/sources/obstacles/circular_obstacle.cpp index 1c574b0..0babd60 100644 --- a/sources/obstacles/circular_obstacle.cpp +++ b/sources/obstacles/circular_obstacle.cpp @@ -1,5 +1,6 @@ #include "obstacle.h" #include "circular_obstacle.h" +#include "rectangular_obstacle.h" #include "../struct/vector_2d.h" #include #include @@ -46,7 +47,8 @@ namespace kraken return isInObstacle(point_a) || isInObstacle(point_b); } - void CircularObstacle::getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, std::vector &vector_2d_list) const + void CircularObstacle::getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, + std::vector &vector_2d_list) const { int nbPoints = std::ceil(M_2_PI * (radius_ + expansion) / longestAllowedLength); if (nbPoints < 3) @@ -56,18 +58,27 @@ namespace kraken vector_2d_list.resize(nbPoints); for (int i = 0; i < nbPoints; ++i) - vector_2d_list.push_back(Vector2D::fromPolar(expansion + radius_, i * M_2_PI / nbPoints) + rotation_center_); + vector_2d_list.push_back( + Vector2D::fromPolar(expansion + radius_, i * M_2_PI / nbPoints) + rotation_center_); } bool CircularObstacle::operator==(const Obstacle &rhs) const { - if(!Obstacle::operator==(rhs)) + if (!Obstacle::operator==(rhs)) return false; - if(typeid(*this) != typeid(rhs)) + if (typeid(*this) != typeid(rhs)) return false; - - return radius_ == static_cast(rhs).radius_; + + return radius_ == static_cast(rhs).radius_; + } + + bool CircularObstacle::isColliding(const RectangularObstacle &obs) const + { + if (rotation_center_.squaredDistance(obs.getRotationCenter()) >= + (radius_ + obs.getHalfDiagonal()) * (radius_ + obs.getHalfDiagonal())) + return false; + return obs.squaredDistance(rotation_center_) < squared_radius_; } #if DEBUG diff --git a/sources/obstacles/circular_obstacle.h b/sources/obstacles/circular_obstacle.h index 85e2fbf..fcd17d0 100644 --- a/sources/obstacles/circular_obstacle.h +++ b/sources/obstacles/circular_obstacle.h @@ -18,8 +18,10 @@ namespace kraken CircularObstacle(const Vector2D &pos, float radius); bool isInObstacle(const Vector2D &pos) const override; float squaredDistance(const Vector2D &pos) const override; - void getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, std::vector &vector_2d_list) const override; + void getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, + std::vector &vector_2d_list) const override; bool isColliding(const Vector2D &point_a, const Vector2D &point_b) const override; + bool isColliding(const RectangularObstacle &obs) const override; bool operator==(const Obstacle &rhs) const override; #if DEBUG diff --git a/sources/obstacles/obstacle.cpp b/sources/obstacles/obstacle.cpp index 95d6acb..5ea5a69 100644 --- a/sources/obstacles/obstacle.cpp +++ b/sources/obstacles/obstacle.cpp @@ -12,4 +12,9 @@ namespace kraken { return rotation_center_ == rhs.rotation_center_; } + + const Vector2D &Obstacle::getRotationCenter() const + { + return rotation_center_; + } } \ No newline at end of file diff --git a/sources/obstacles/obstacle.h b/sources/obstacles/obstacle.h index 55d5ae7..58cc173 100644 --- a/sources/obstacles/obstacle.h +++ b/sources/obstacles/obstacle.h @@ -6,6 +6,7 @@ namespace kraken { + class RectangularObstacle; class Obstacle { @@ -14,11 +15,13 @@ namespace kraken virtual ~Obstacle() = default; virtual bool isInObstacle(const Vector2D &pos) const = 0; virtual float squaredDistance(const Vector2D &pos) const = 0; - virtual void getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, std::vector &vector_2d_list) const = 0; + virtual void getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, + std::vector &vector_2d_list) const = 0; virtual bool isColliding(const Vector2D &point_a, const Vector2D &point_b) const = 0; + virtual bool isColliding(const RectangularObstacle &obs) const = 0; virtual bool operator==(const Obstacle &rhs) const; - //virtual bool isColliding(RectangularObstacle obs) + const Vector2D &getRotationCenter() const; protected: Vector2D rotation_center_; diff --git a/sources/obstacles/RectangularObstacle.cpp b/sources/obstacles/rectangular_obstacle.cpp similarity index 57% rename from sources/obstacles/RectangularObstacle.cpp rename to sources/obstacles/rectangular_obstacle.cpp index 7d770a3..165c3d7 100644 --- a/sources/obstacles/RectangularObstacle.cpp +++ b/sources/obstacles/rectangular_obstacle.cpp @@ -1,4 +1,4 @@ -#include "RectangularObstacle.h" +#include "rectangular_obstacle.h" #include #include @@ -59,6 +59,26 @@ namespace kraken } + void RectangularObstacle::update(const Vector2D &position, const float &orientation) + { + update(position.getX(), position.getY(), orientation); + } + + void RectangularObstacle::update(const float &x, const float &y, const float &orientation) + { + rotation_center_.setX(x); + rotation_center_.setY(y); + angle_ = orientation; + cos_ = std::cos(angle_); + sin_ = std::sin(angle_); + left_bottom_corner_rotate_ = toTableCoordinateSystem(left_bottom_corner_); + left_upper_corner_rotate_ = toTableCoordinateSystem(left_upper_corner_); + right_bottom_corner_rotate_ = toTableCoordinateSystem(right_bottom_corner_); + right_upper_corner_rotate_ = toTableCoordinateSystem(right_upper_corner_); + + geometric_center_ = (right_bottom_corner_rotate_ + left_upper_corner_rotate_) * 0.5f; + } + Vector2D RectangularObstacle::toObstacleCoordinateSystem(const Vector2D &point) const { Vector2D out; @@ -96,7 +116,7 @@ namespace kraken vector_2d_list.clear(); - for (auto const &point : point) + for (auto const &point : corners) vector_2d_list.push_back(point); for (int i = 0; i < 4; i++) @@ -143,8 +163,77 @@ namespace kraken return 0; } + bool RectangularObstacle::isInObstacle(const Vector2D &pos) const + { + Vector2D in = toObstacleCoordinateSystem(pos); + return in.getX() >= left_bottom_corner_.getX() && in.getX() <= right_upper_corner_.getX() && + in.getY() >= left_bottom_corner_.getY() && in.getY() <= right_upper_corner_.getY(); + } + + bool RectangularObstacle::isColliding(const Vector2D &point_a, const Vector2D &point_b) const + { + if (Vector2D::segmentIntersection(point_a, point_b, left_bottom_corner_rotate_, left_upper_corner_rotate_) + || Vector2D::segmentIntersection(point_a, point_b, left_upper_corner_rotate_, right_upper_corner_rotate_) + || Vector2D::segmentIntersection(point_a, point_b, right_upper_corner_rotate_, right_bottom_corner_rotate_) + || Vector2D::segmentIntersection(point_a, point_b, right_bottom_corner_rotate_, left_bottom_corner_rotate_)) + return true; + + return isInObstacle(point_a) || isInObstacle(point_b); + } + + bool RectangularObstacle::isColliding(const RectangularObstacle &obs) const + { + if (rotation_center_.squaredDistance(obs.rotation_center_) >= + (half_diagonal_ + obs.half_diagonal_) * (half_diagonal_ + obs.half_diagonal_)) + return false; + + return !test_separation(left_bottom_corner_.getX(), right_bottom_corner_.getX(), + getXToObstacleCoordinateSystem(obs.left_bottom_corner_rotate_), + getXToObstacleCoordinateSystem(obs.left_upper_corner_rotate_), + getXToObstacleCoordinateSystem(obs.right_bottom_corner_rotate_), + getXToObstacleCoordinateSystem(obs.right_upper_corner_rotate_)) + && !test_separation(left_bottom_corner_.getY(), left_upper_corner_.getY(), + getYToObstacleCoordinateSystem(obs.left_bottom_corner_rotate_), + getYToObstacleCoordinateSystem(obs.left_upper_corner_rotate_), + getYToObstacleCoordinateSystem(obs.right_bottom_corner_rotate_), + getYToObstacleCoordinateSystem(obs.right_upper_corner_rotate_)) + && !test_separation(obs.left_bottom_corner_.getX(), obs.right_bottom_corner_.getX(), + obs.getXToObstacleCoordinateSystem(left_bottom_corner_rotate_), + obs.getXToObstacleCoordinateSystem(left_upper_corner_rotate_), + obs.getXToObstacleCoordinateSystem(right_bottom_corner_rotate_), + obs.getXToObstacleCoordinateSystem(right_upper_corner_rotate_)) + && !test_separation(obs.left_bottom_corner_.getY(), obs.left_upper_corner_.getY(), + obs.getYToObstacleCoordinateSystem(left_bottom_corner_rotate_), + obs.getYToObstacleCoordinateSystem(left_upper_corner_rotate_), + obs.getYToObstacleCoordinateSystem(right_bottom_corner_rotate_), + obs.getYToObstacleCoordinateSystem(right_upper_corner_rotate_)); + } + float RectangularObstacle::getHalfDiagonal() const { return half_diagonal_; } + + bool RectangularObstacle::test_separation(const float &a, const float &b, const float &a2, const float &b2, + const float &c2, const float &d2) const + { + float min1 = std::min(a, b); + float max1 = std::max(a, b); + + float min2 = std::min(std::min(a2, b2), std::min(c2, d2)); + float max2 = std::max(std::max(a2, b2), std::max(c2, d2)); + + return min1 > max2 || min2 > max1; + } + +#if DEBUG + + std::ostream &operator<<(std::ostream &strm, const RectangularObstacle &o) + { + return strm << "RectangularObstacle(" << o.left_bottom_corner_rotate_ << " " << o.right_bottom_corner_rotate_ + << " " << o.right_upper_corner_rotate_ << " " << o.left_upper_corner_rotate_ << ")" << std::endl; + } + +#endif + } \ No newline at end of file diff --git a/sources/obstacles/RectangularObstacle.h b/sources/obstacles/rectangular_obstacle.h similarity index 57% rename from sources/obstacles/RectangularObstacle.h rename to sources/obstacles/rectangular_obstacle.h index 0912aa6..f624449 100644 --- a/sources/obstacles/RectangularObstacle.h +++ b/sources/obstacles/rectangular_obstacle.h @@ -11,13 +11,25 @@ namespace kraken public: RectangularObstacle(const Vector2D &position, const int &size_x, const int &size_y); RectangularObstacle(const Vector2D &position, const int &size_x, const int &size_y, const float &angle); - RectangularObstacle(const int &distance_to_front, const int &distance_to_back, const int &distance_to_left, const int &distance_to_right); - RectangularObstacle(const int &distance_to_front, const int &distance_to_back, const int &distance_to_left, const int &distance_to_right, const float &tilt_angle); - RectangularObstacle(const Vector2D &position, const int &size_left_x, const int &size_right_x, const int &size_up_y, const int &size_down_y, const float &angle); - RectangularObstacle(const Vector2D &position, const Vector2D &top_right_corner, const Vector2D &bottom_left_corner, const float &angle); + RectangularObstacle(const int &distance_to_front, const int &distance_to_back, const int &distance_to_left, + const int &distance_to_right); + RectangularObstacle(const int &distance_to_front, const int &distancesquaredDistance_to_back, + const int &distance_to_left, + const int &distance_to_right, const float &tilt_angle); + RectangularObstacle(const Vector2D &position, const int &size_left_x, const int &size_right_x, + const int &size_up_y, const int &size_down_y, const float &angle); + RectangularObstacle(const Vector2D &position, const Vector2D &top_right_corner, + const Vector2D &bottom_left_corner, const float &angle); - void getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, std::vector &vector_2d_list) const override; + void update(const Vector2D &position, const float &orientation); + void update(const float &x, const float &y, const float &orientation); + + void getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, + std::vector &vector_2d_list) const override; float squaredDistance(const Vector2D &v) const override; + bool isInObstacle(const Vector2D &pos) const override; + bool isColliding(const Vector2D &point_a, const Vector2D &point_b) const override; + bool isColliding(const RectangularObstacle &obs) const override; float getHalfDiagonal() const; protected: @@ -26,6 +38,15 @@ namespace kraken float getXToObstacleCoordinateSystem(const Vector2D &point) const; float getYToObstacleCoordinateSystem(const Vector2D &point) const; + + private: + bool test_separation(const float &a, const float &b, const float &a2, const float &b2, const float &c2, + const float &d2) const; + +#if DEBUG + friend std::ostream &operator<<(std::ostream &strm, const RectangularObstacle &o); +#endif + protected: float angle_; float cos_; diff --git a/sources/struct/vector_2d.cpp b/sources/struct/vector_2d.cpp index 5694f18..d06a75d 100644 --- a/sources/struct/vector_2d.cpp +++ b/sources/struct/vector_2d.cpp @@ -186,7 +186,7 @@ namespace kraken y_ = y; } - bool Vector2D::segmentIntersection(Vector2D &point_A1, Vector2D &point_A2, Vector2D &point_B1, Vector2D &point_B2) + bool Vector2D::segmentIntersection(const Vector2D &point_A1, const Vector2D &point_A2, const Vector2D &point_B1, const Vector2D &point_B2) { // Source : https://stackoverflow.com/questions/3746274/line-intersection-with-aabb-rectangle diff --git a/sources/struct/vector_2d.h b/sources/struct/vector_2d.h index 1c70161..27c7614 100644 --- a/sources/struct/vector_2d.h +++ b/sources/struct/vector_2d.h @@ -58,7 +58,7 @@ namespace kraken * @param pointB2 * @return */ - static bool segmentIntersection(Vector2D &pointA1, Vector2D &pointA2, Vector2D &pointB1, Vector2D &pointB2); + static bool segmentIntersection(const Vector2D &pointA1, const Vector2D &pointA2, const Vector2D &pointB1, const Vector2D &pointB2); static Vector2D fromPolar(float radius, float angle); protected: float x_; From faae8508a4c6d3a0431689de0d168aa6ca3f8a57 Mon Sep 17 00:00:00 2001 From: bourreP Date: Thu, 24 May 2018 01:13:25 +0200 Subject: [PATCH 16/36] fix Include --- sources/obstacles/rectangular_obstacle.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sources/obstacles/rectangular_obstacle.h b/sources/obstacles/rectangular_obstacle.h index f624449..199fda0 100644 --- a/sources/obstacles/rectangular_obstacle.h +++ b/sources/obstacles/rectangular_obstacle.h @@ -1,7 +1,7 @@ #ifndef TESTS_RECTANGULAR_OBSTACLE_H #define TESTS_RECTANGULAR_OBSTACLE_H -#include "Obstacle.h" +#include "obstacle.h" #include "../struct/vector_2d.h" namespace kraken From 98a280e65c40568d0f68e9811199817473be1c2c Mon Sep 17 00:00:00 2001 From: Kayou Date: Thu, 24 May 2018 13:43:30 +0200 Subject: [PATCH 17/36] add compoundObstacle --- sources/obstacles/circular_obstacle.cpp | 3 -- sources/obstacles/compound_obstacle.cpp | 59 ++++++++++++++++++++++ sources/obstacles/compound_obstacle.h | 27 ++++++++++ sources/obstacles/rectangular_obstacle.cpp | 2 - 4 files changed, 86 insertions(+), 5 deletions(-) create mode 100644 sources/obstacles/compound_obstacle.cpp create mode 100644 sources/obstacles/compound_obstacle.h diff --git a/sources/obstacles/circular_obstacle.cpp b/sources/obstacles/circular_obstacle.cpp index 0babd60..440cdec 100644 --- a/sources/obstacles/circular_obstacle.cpp +++ b/sources/obstacles/circular_obstacle.cpp @@ -54,9 +54,6 @@ namespace kraken if (nbPoints < 3) nbPoints = 3; - vector_2d_list.clear(); - vector_2d_list.resize(nbPoints); - for (int i = 0; i < nbPoints; ++i) vector_2d_list.push_back( Vector2D::fromPolar(expansion + radius_, i * M_2_PI / nbPoints) + rotation_center_); diff --git a/sources/obstacles/compound_obstacle.cpp b/sources/obstacles/compound_obstacle.cpp new file mode 100644 index 0000000..6b28851 --- /dev/null +++ b/sources/obstacles/compound_obstacle.cpp @@ -0,0 +1,59 @@ +#include "compound_obstacle.h" + +#include + +kraken::CompoundObstacle::CompoundObstacle(const kraken::Vector2D &rotation_center, + std::vector obstacles_list) : + Obstacle(rotation_center), obstacles_list_(std::move(obstacles_list)) +{ +} + +bool kraken::CompoundObstacle::isInObstacle(const kraken::Vector2D &pos) const +{ + for (auto const &o : obstacles_list_) + { + if (o.isInObstacle(pos)) + return true; + } + + return false; +} + +float kraken::CompoundObstacle::squaredDistance(const kraken::Vector2D &pos) const +{ + float min = std::numeric_limits::max(); + for(auto const &o : obstacles_list_) + { + min = std::min(min, o.squaredDistance(pos)); + if(min == 0) + break; + } + return min; +} + +void kraken::CompoundObstacle::getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, + std::vector &vector_2d_list) const +{ + for(const auto &o : obstacles_list_) + o.getExpandedConvexHull(expansion, longestAllowedLength, vector_2d_list); +} + +bool kraken::CompoundObstacle::isColliding(const kraken::Vector2D &point_a, const kraken::Vector2D &point_b) const +{ + for(auto const &o : obstacles_list_) + { + if (o.isColliding(point_a, point_b)) + return true; + } + return false; +} + +bool kraken::CompoundObstacle::isColliding(const kraken::RectangularObstacle &obs) const +{ + for(auto const &o : obstacles_list_) + { + if (o.isColliding(obs)) + return true; + } + return false; +} diff --git a/sources/obstacles/compound_obstacle.h b/sources/obstacles/compound_obstacle.h new file mode 100644 index 0000000..403a90e --- /dev/null +++ b/sources/obstacles/compound_obstacle.h @@ -0,0 +1,27 @@ +#ifndef TESTS_COMPOUND_OBSTACLE_H +#define TESTS_COMPOUND_OBSTACLE_H + +#include +#include "obstacle.h" + +namespace kraken +{ + + class CompoundObstacle : public Obstacle + { + public: + CompoundObstacle(const Vector2D &rotation_center, std::vector obstacles_list); + + bool isInObstacle(const Vector2D &pos) const override; + float squaredDistance(const Vector2D &pos) const override; + void getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, + std::vector &vector_2d_list) const override; + bool isColliding(const Vector2D &point_a, const Vector2D &point_b) const override; + bool isColliding(const RectangularObstacle &obs) const override; + + protected: + const std::vector obstacles_list_; + }; + +} +#endif //TESTS_COMPOUND_OBSTACLE_H diff --git a/sources/obstacles/rectangular_obstacle.cpp b/sources/obstacles/rectangular_obstacle.cpp index 165c3d7..91c6629 100644 --- a/sources/obstacles/rectangular_obstacle.cpp +++ b/sources/obstacles/rectangular_obstacle.cpp @@ -114,8 +114,6 @@ namespace kraken (left_upper_corner_rotate_ - geometric_center_) * coeff + left_upper_corner_rotate_, (left_bottom_corner_rotate_ - geometric_center_) * coeff + left_bottom_corner_rotate_}; - vector_2d_list.clear(); - for (auto const &point : corners) vector_2d_list.push_back(point); From ba537cfcb00b043f60bbe282e6ff755f1203c837 Mon Sep 17 00:00:00 2001 From: PF Date: Fri, 25 May 2018 19:49:32 +0200 Subject: [PATCH 18/36] Use default values to simplify the construtors of RectangularObstacle --- sources/obstacles/rectangular_obstacle.cpp | 37 ++++------------------ sources/obstacles/rectangular_obstacle.h | 16 ++++------ 2 files changed, 13 insertions(+), 40 deletions(-) diff --git a/sources/obstacles/rectangular_obstacle.cpp b/sources/obstacles/rectangular_obstacle.cpp index 91c6629..18778f1 100644 --- a/sources/obstacles/rectangular_obstacle.cpp +++ b/sources/obstacles/rectangular_obstacle.cpp @@ -4,46 +4,23 @@ namespace kraken { - - RectangularObstacle::RectangularObstacle(const Vector2D &position, const int &size_x, const int &size_y) : - RectangularObstacle(position, size_x, size_y, 0) - { - - } - - RectangularObstacle::RectangularObstacle(const Vector2D &position, const int &size_x, const int &size_y, + RectangularObstacle::RectangularObstacle(const int &size_x, const int &size_y, const Vector2D &position, const float &angle) : - RectangularObstacle(position, size_x / 2, size_x / 2, size_y / 2, size_y / 2, angle) - { - - } - - RectangularObstacle::RectangularObstacle(const int &distance_to_front, const int &distance_to_back, - const int &distance_to_left, const int &distance_to_right) : - RectangularObstacle(distance_to_front, distance_to_back, distance_to_left, distance_to_right, 0) - { - - } - - RectangularObstacle::RectangularObstacle(const int &distance_to_front, const int &distance_to_back, - const int &distance_to_left, const int &distance_to_right, - const float &tilt_angle) : - RectangularObstacle(Vector2D(0, 0), distance_to_back, distance_to_front, distance_to_left, - distance_to_right, tilt_angle) + RectangularObstacle(size_x / 2, size_x / 2, size_y / 2, size_y / 2, position, angle) { } - RectangularObstacle::RectangularObstacle(const Vector2D &position, const int &size_left_x, const int &size_right_x, - const int &size_up_y, const int &size_down_y, const float &angle) : - RectangularObstacle(position, Vector2D(size_right_x, size_up_y), Vector2D(-size_left_x, -size_down_y), + RectangularObstacle::RectangularObstacle(const int &size_left_x, const int &size_right_x, const int &size_up_y, + const int &size_down_y, const Vector2D &position, const float &angle) : + RectangularObstacle(Vector2D(size_right_x, size_up_y), Vector2D(-size_left_x, -size_down_y), position, angle) { } - RectangularObstacle::RectangularObstacle(const Vector2D &position, const Vector2D &top_right_corner, - const Vector2D &bottom_left_corner, const float &angle) : + RectangularObstacle::RectangularObstacle(const Vector2D &top_right_corner, const Vector2D &bottom_left_corner, + const Vector2D &position, const float &angle) : Obstacle(position), angle_(angle), cos_(std::cos(angle)), sin_(std::sin(angle)), left_bottom_corner_(bottom_left_corner), left_upper_corner_(bottom_left_corner.getX(), top_right_corner.getY()), diff --git a/sources/obstacles/rectangular_obstacle.h b/sources/obstacles/rectangular_obstacle.h index 199fda0..0be44af 100644 --- a/sources/obstacles/rectangular_obstacle.h +++ b/sources/obstacles/rectangular_obstacle.h @@ -9,17 +9,13 @@ namespace kraken class RectangularObstacle : public Obstacle { public: - RectangularObstacle(const Vector2D &position, const int &size_x, const int &size_y); - RectangularObstacle(const Vector2D &position, const int &size_x, const int &size_y, const float &angle); + RectangularObstacle(const int &size_x, const int &size_y, const Vector2D &position = Vector2D(0, 0), + const float &angle = 0); RectangularObstacle(const int &distance_to_front, const int &distance_to_back, const int &distance_to_left, - const int &distance_to_right); - RectangularObstacle(const int &distance_to_front, const int &distancesquaredDistance_to_back, - const int &distance_to_left, - const int &distance_to_right, const float &tilt_angle); - RectangularObstacle(const Vector2D &position, const int &size_left_x, const int &size_right_x, - const int &size_up_y, const int &size_down_y, const float &angle); - RectangularObstacle(const Vector2D &position, const Vector2D &top_right_corner, - const Vector2D &bottom_left_corner, const float &angle); + const int &distance_to_right, const Vector2D &position = Vector2D(0, 0), + const float &tilt_angle = 0); + RectangularObstacle(const Vector2D &top_right_corner, const Vector2D &bottom_left_corner, + const Vector2D &position = Vector2D(0, 0), const float &angle = 0); void update(const Vector2D &position, const float &orientation); void update(const float &x, const float &y, const float &orientation); From 5f012c6e4cefe7a4ea8ceb5929229848f9c27a64 Mon Sep 17 00:00:00 2001 From: PF Date: Fri, 25 May 2018 19:56:04 +0200 Subject: [PATCH 19/36] operator== for RectangularObstacle --- sources/obstacles/rectangular_obstacle.cpp | 14 ++++++++++++++ sources/obstacles/rectangular_obstacle.h | 2 ++ 2 files changed, 16 insertions(+) diff --git a/sources/obstacles/rectangular_obstacle.cpp b/sources/obstacles/rectangular_obstacle.cpp index 18778f1..479001d 100644 --- a/sources/obstacles/rectangular_obstacle.cpp +++ b/sources/obstacles/rectangular_obstacle.cpp @@ -1,6 +1,7 @@ #include "rectangular_obstacle.h" #include #include +#include namespace kraken { @@ -189,6 +190,19 @@ namespace kraken return half_diagonal_; } + bool RectangularObstacle::operator==(const Obstacle &rhs) const + { + if (!Obstacle::operator==(rhs)) + return false; + + if (typeid(*this) != typeid(rhs)) + return false; + + RectangularObstacle ro_rhs = static_cast(rhs); + return angle_ == ro_rhs.angle_ && left_bottom_corner_ == ro_rhs.left_bottom_corner_ && + right_upper_corner_ == ro_rhs.right_upper_corner_; + } + bool RectangularObstacle::test_separation(const float &a, const float &b, const float &a2, const float &b2, const float &c2, const float &d2) const { diff --git a/sources/obstacles/rectangular_obstacle.h b/sources/obstacles/rectangular_obstacle.h index 0be44af..e8a6f60 100644 --- a/sources/obstacles/rectangular_obstacle.h +++ b/sources/obstacles/rectangular_obstacle.h @@ -28,6 +28,8 @@ namespace kraken bool isColliding(const RectangularObstacle &obs) const override; float getHalfDiagonal() const; + bool operator==(const Obstacle &rhs) const override; + protected: Vector2D toObstacleCoordinateSystem(const Vector2D &point) const; Vector2D toTableCoordinateSystem(const Vector2D &point) const; From be727fc64c3460aec58fd96108f44effc73290e0 Mon Sep 17 00:00:00 2001 From: PF Date: Fri, 25 May 2018 20:14:28 +0200 Subject: [PATCH 20/36] A bit better --- sources/obstacles/rectangular_obstacle.cpp | 11 +++-------- 1 file changed, 3 insertions(+), 8 deletions(-) diff --git a/sources/obstacles/rectangular_obstacle.cpp b/sources/obstacles/rectangular_obstacle.cpp index 479001d..dc4e211 100644 --- a/sources/obstacles/rectangular_obstacle.cpp +++ b/sources/obstacles/rectangular_obstacle.cpp @@ -59,18 +59,13 @@ namespace kraken Vector2D RectangularObstacle::toObstacleCoordinateSystem(const Vector2D &point) const { - Vector2D out; - out.setX(getXToObstacleCoordinateSystem(point)); - out.setY(getYToObstacleCoordinateSystem(point)); - return out; + return Vector2D(getXToObstacleCoordinateSystem(point), getYToObstacleCoordinateSystem(point)); } Vector2D RectangularObstacle::toTableCoordinateSystem(const Vector2D &point) const { - Vector2D out; - out.setX(cos_ * point.getX() - sin_ * point.getY() + rotation_center_.getX()); - out.setY(sin_ * point.getX() + cos_ * point.getY() + rotation_center_.getY()); - return out; + return Vector2D(cos_ * point.getX() - sin_ * point.getY() + rotation_center_.getX(), + sin_ * point.getX() + cos_ * point.getY() + rotation_center_.getY()); } float RectangularObstacle::getXToObstacleCoordinateSystem(const Vector2D &point) const From e79c081647a883a1a23894adee04ea80710ea8a3 Mon Sep 17 00:00:00 2001 From: PF Date: Fri, 25 May 2018 20:14:52 +0200 Subject: [PATCH 21/36] Compound_obstacle in kraken namespace and operator== --- sources/obstacles/compound_obstacle.cpp | 95 ++++++++++++++----------- sources/obstacles/compound_obstacle.h | 2 + 2 files changed, 57 insertions(+), 40 deletions(-) diff --git a/sources/obstacles/compound_obstacle.cpp b/sources/obstacles/compound_obstacle.cpp index 6b28851..e6be5b1 100644 --- a/sources/obstacles/compound_obstacle.cpp +++ b/sources/obstacles/compound_obstacle.cpp @@ -1,59 +1,74 @@ #include "compound_obstacle.h" #include +#include -kraken::CompoundObstacle::CompoundObstacle(const kraken::Vector2D &rotation_center, - std::vector obstacles_list) : - Obstacle(rotation_center), obstacles_list_(std::move(obstacles_list)) +namespace kraken { -} + CompoundObstacle::CompoundObstacle(const kraken::Vector2D &rotation_center, + std::vector obstacles_list) : + Obstacle(rotation_center), obstacles_list_(std::move(obstacles_list)) + { + } -bool kraken::CompoundObstacle::isInObstacle(const kraken::Vector2D &pos) const -{ - for (auto const &o : obstacles_list_) + bool CompoundObstacle::isInObstacle(const kraken::Vector2D &pos) const { - if (o.isInObstacle(pos)) - return true; + for (auto const &o : obstacles_list_) + { + if (o.isInObstacle(pos)) + return true; + } + + return false; } - return false; -} + float CompoundObstacle::squaredDistance(const kraken::Vector2D &pos) const + { + float min = std::numeric_limits::max(); + for (auto const &o : obstacles_list_) + { + min = std::min(min, o.squaredDistance(pos)); + if (min == 0) + break; + } + return min; + } -float kraken::CompoundObstacle::squaredDistance(const kraken::Vector2D &pos) const -{ - float min = std::numeric_limits::max(); - for(auto const &o : obstacles_list_) + void CompoundObstacle::getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, + std::vector &vector_2d_list) const { - min = std::min(min, o.squaredDistance(pos)); - if(min == 0) - break; + for (const auto &o : obstacles_list_) + o.getExpandedConvexHull(expansion, longestAllowedLength, vector_2d_list); } - return min; -} -void kraken::CompoundObstacle::getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, - std::vector &vector_2d_list) const -{ - for(const auto &o : obstacles_list_) - o.getExpandedConvexHull(expansion, longestAllowedLength, vector_2d_list); -} + bool CompoundObstacle::isColliding(const kraken::Vector2D &point_a, const kraken::Vector2D &point_b) const + { + for (auto const &o : obstacles_list_) + { + if (o.isColliding(point_a, point_b)) + return true; + } + return false; + } -bool kraken::CompoundObstacle::isColliding(const kraken::Vector2D &point_a, const kraken::Vector2D &point_b) const -{ - for(auto const &o : obstacles_list_) + bool CompoundObstacle::isColliding(const kraken::RectangularObstacle &obs) const { - if (o.isColliding(point_a, point_b)) - return true; + for (auto const &o : obstacles_list_) + { + if (o.isColliding(obs)) + return true; + } + return false; } - return false; -} -bool kraken::CompoundObstacle::isColliding(const kraken::RectangularObstacle &obs) const -{ - for(auto const &o : obstacles_list_) + bool CompoundObstacle::operator==(const Obstacle &rhs) const { - if (o.isColliding(obs)) - return true; + if (!Obstacle::operator==(rhs)) + return false; + + if (typeid(*this) != typeid(rhs)) + return false; + + return static_cast(rhs).obstacles_list_ == obstacles_list_; } - return false; -} +} \ No newline at end of file diff --git a/sources/obstacles/compound_obstacle.h b/sources/obstacles/compound_obstacle.h index 403a90e..ad810c5 100644 --- a/sources/obstacles/compound_obstacle.h +++ b/sources/obstacles/compound_obstacle.h @@ -19,6 +19,8 @@ namespace kraken bool isColliding(const Vector2D &point_a, const Vector2D &point_b) const override; bool isColliding(const RectangularObstacle &obs) const override; + bool operator==(const Obstacle &rhs) const override; + protected: const std::vector obstacles_list_; }; From 1bacac17454bc5cb26c687a0a38718b4a3e96753 Mon Sep 17 00:00:00 2001 From: PF Date: Fri, 25 May 2018 20:16:25 +0200 Subject: [PATCH 22/36] Remove superfluous kraken:: --- sources/obstacles/compound_obstacle.cpp | 14 +++++++------- sources/obstacles/compound_obstacle.h | 2 +- 2 files changed, 8 insertions(+), 8 deletions(-) diff --git a/sources/obstacles/compound_obstacle.cpp b/sources/obstacles/compound_obstacle.cpp index e6be5b1..1759a94 100644 --- a/sources/obstacles/compound_obstacle.cpp +++ b/sources/obstacles/compound_obstacle.cpp @@ -5,13 +5,13 @@ namespace kraken { - CompoundObstacle::CompoundObstacle(const kraken::Vector2D &rotation_center, - std::vector obstacles_list) : + CompoundObstacle::CompoundObstacle(const Vector2D &rotation_center, + std::vector obstacles_list) : Obstacle(rotation_center), obstacles_list_(std::move(obstacles_list)) { } - bool CompoundObstacle::isInObstacle(const kraken::Vector2D &pos) const + bool CompoundObstacle::isInObstacle(const Vector2D &pos) const { for (auto const &o : obstacles_list_) { @@ -22,7 +22,7 @@ namespace kraken return false; } - float CompoundObstacle::squaredDistance(const kraken::Vector2D &pos) const + float CompoundObstacle::squaredDistance(const Vector2D &pos) const { float min = std::numeric_limits::max(); for (auto const &o : obstacles_list_) @@ -35,13 +35,13 @@ namespace kraken } void CompoundObstacle::getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, - std::vector &vector_2d_list) const + std::vector &vector_2d_list) const { for (const auto &o : obstacles_list_) o.getExpandedConvexHull(expansion, longestAllowedLength, vector_2d_list); } - bool CompoundObstacle::isColliding(const kraken::Vector2D &point_a, const kraken::Vector2D &point_b) const + bool CompoundObstacle::isColliding(const Vector2D &point_a, const Vector2D &point_b) const { for (auto const &o : obstacles_list_) { @@ -51,7 +51,7 @@ namespace kraken return false; } - bool CompoundObstacle::isColliding(const kraken::RectangularObstacle &obs) const + bool CompoundObstacle::isColliding(const RectangularObstacle &obs) const { for (auto const &o : obstacles_list_) { diff --git a/sources/obstacles/compound_obstacle.h b/sources/obstacles/compound_obstacle.h index ad810c5..bf1ba8c 100644 --- a/sources/obstacles/compound_obstacle.h +++ b/sources/obstacles/compound_obstacle.h @@ -10,7 +10,7 @@ namespace kraken class CompoundObstacle : public Obstacle { public: - CompoundObstacle(const Vector2D &rotation_center, std::vector obstacles_list); + CompoundObstacle(const Vector2D &rotation_center, std::vector obstacles_list); bool isInObstacle(const Vector2D &pos) const override; float squaredDistance(const Vector2D &pos) const override; From 96de11d66843fd7edb368ce93c8ca46d3b40395d Mon Sep 17 00:00:00 2001 From: Kayou Date: Tue, 29 May 2018 11:56:54 +0200 Subject: [PATCH 23/36] add unit tests --- tests/020-test_obstacles.cpp | 50 ++++++++++++++++++++++++++++++++++++ 1 file changed, 50 insertions(+) create mode 100644 tests/020-test_obstacles.cpp diff --git a/tests/020-test_obstacles.cpp b/tests/020-test_obstacles.cpp new file mode 100644 index 0000000..d3c213d --- /dev/null +++ b/tests/020-test_obstacles.cpp @@ -0,0 +1,50 @@ +#include "catch.hpp" +#include "../sources/obstacles/circular_obstacle.h" +#include "../sources/obstacles/rectangular_obstacle.h" + +using namespace kraken; + +TEST_CASE("Circular Obstacle", "[obstacles]") +{ + CircularObstacle o(Vector2D(0, 0), 30); + REQUIRE(o.squaredDistance(Vector2D(10, 10)) == 0); + REQUIRE(o.squaredDistance(Vector2D(22, 22)) > 0); +} + +TEST_CASE("Rectangular Obstacle", "[obstacles]") +{ + { + RectangularObstacle o(30, 10); + REQUIRE(o.squaredDistance(Vector2D(13, -3)) == 0); + REQUIRE(o.squaredDistance(Vector2D(16, 0)) > 0); + REQUIRE(o.squaredDistance(Vector2D(-16, 0)) > 0); + REQUIRE(o.squaredDistance(Vector2D(0, 7)) > 0); + REQUIRE(o.squaredDistance(Vector2D(0, -7)) > 0); + } + + { + RectangularObstacle o(200, 200, Vector2D(0, 0), static_cast(M_PI / 8)); + REQUIRE(o.squaredDistance(Vector2D(0, 0)) == 0); + REQUIRE(o.squaredDistance(Vector2D(100, 0)) == 0); + REQUIRE(o.squaredDistance(Vector2D(0, -100)) == 0); + REQUIRE(o.squaredDistance(Vector2D(90, 90)) > 0); + REQUIRE(o.squaredDistance(Vector2D(-90, -90)) > 0); + REQUIRE(o.squaredDistance(Vector2D(54, 130)) == 0); + REQUIRE(o.squaredDistance(Vector2D(-54, -130)) == 0); + REQUIRE(o.squaredDistance(Vector2D(-54, 130)) > 0); + REQUIRE(o.squaredDistance(Vector2D(54, -130)) > 0); + REQUIRE(o.squaredDistance(Vector2D(-100, 100)) > 0); + } +} + +TEST_CASE("Rectangular Colliding", "[obstacles]") +{ + RectangularObstacle o(200, 200, Vector2D(1000, 1000), static_cast(M_PI / 8)); + REQUIRE(!o.isColliding(RectangularObstacle(10, 10, Vector2D(1200, 1000), 0))); + REQUIRE(!o.isColliding(RectangularObstacle(10, 10, Vector2D(800, 1000), 0))); + REQUIRE(!o.isColliding(RectangularObstacle(10, 10, Vector2D(0, 1200), 0))); + REQUIRE(!o.isColliding(RectangularObstacle(10, 10, Vector2D(0, 800), 0))); + REQUIRE(!o.isColliding(RectangularObstacle(20, 20, Vector2D(900, 1100), 0))); + REQUIRE(!o.isColliding(RectangularObstacle(40, 40, Vector2D(900, 1100), 0))); + REQUIRE(o.isColliding(RectangularObstacle(60, 60, Vector2D(900, 1100), 0))); +} From 5e8ca2bc518fa24dc9b98cd850014b6dd6059a9d Mon Sep 17 00:00:00 2001 From: Kayou Date: Tue, 12 Jun 2018 21:58:15 +0200 Subject: [PATCH 24/36] constexpr and noexcept for vector_2d --- sources/struct/vector_2d.cpp | 105 ++++---------------------------- sources/struct/vector_2d.h | 114 ++++++++++++++++++++++++++--------- 2 files changed, 98 insertions(+), 121 deletions(-) diff --git a/sources/struct/vector_2d.cpp b/sources/struct/vector_2d.cpp index d06a75d..5b560b2 100644 --- a/sources/struct/vector_2d.cpp +++ b/sources/struct/vector_2d.cpp @@ -1,107 +1,53 @@ -#include -#include #include #include "vector_2d.h" namespace kraken { - - Vector2D::Vector2D() : x_(0.f), y_(0.f) - { - - } - - Vector2D::Vector2D(const float &x, const float &y) : x_(x), y_(y) - { - - } - - Vector2D Vector2D::operator+(const Vector2D &rhs) const - { - return Vector2D(x_ + rhs.x_, y_ + rhs.y_); - } - - Vector2D &Vector2D::operator+=(const Vector2D &rhs) + Vector2D& Vector2D::operator+=(const Vector2D &rhs) noexcept { x_ += rhs.x_; y_ += rhs.y_; return *this; } - Vector2D Vector2D::operator-(const Vector2D &rhs) const - { - return Vector2D(x_ - rhs.x_, y_ - rhs.y_); - } - - Vector2D &Vector2D::operator-=(const Vector2D &rhs) + Vector2D& Vector2D::operator-=(const Vector2D &rhs) noexcept { x_ -= rhs.x_; y_ -= rhs.y_; return *this; } - Vector2D Vector2D::operator*(const float &d) const - { - return Vector2D(x_ * d, y_ * d); - } - - Vector2D &Vector2D::operator*=(const float &d) + Vector2D& Vector2D::operator*=(const float &d) noexcept { x_ *= d; y_ *= d; return *this; } - bool Vector2D::operator==(const Vector2D &rhs) const - { - return x_ == rhs.x_ && y_ == rhs.y_; - } - - bool Vector2D::operator!=(const Vector2D &rhs) const - { - return x_ != rhs.x_ || y_ != rhs.y_; - } - - float Vector2D::dot(const Vector2D &other) const - { - return x_ * other.x_ + y_ * other.y_; - } - - float Vector2D::squaredDistance(const Vector2D &other) const - { - float tmp_x = x_ - other.x_, tmp_y = y_ - other.y_; - return tmp_x * tmp_x + tmp_y * tmp_y; - } - - float Vector2D::distance(const Vector2D &other) const - { - return std::sqrt(squaredDistance(other)); - } - - float Vector2D::distanceFast(const Vector2D &other) const + float Vector2D::distanceFast(const Vector2D &other) const noexcept { float dx = std::abs(x_ - other.x_); float dy = std::abs(y_ - other.y_); return std::max(dx, dy) + 0.414f * std::min(dx, dy); } - Vector2D &Vector2D::Ysym(bool do_symmetry) + Vector2D &Vector2D::Ysym(bool do_symmetry) noexcept { if (do_symmetry) y_ = -y_; return *this; } - Vector2D Vector2D::rotate(float angle, const Vector2D &rotation_center) const + Vector2D Vector2D::rotate(float angle, const Vector2D &rotation_center) const noexcept { float cos = std::cos(angle); float sin = std::sin(angle); float x = cos * (x_ - rotation_center.x_) - sin * (y_ - rotation_center.y_) + rotation_center.x_; float y = sin * (x_ - rotation_center.x_) + cos * (y_ - rotation_center.y_) + rotation_center.y_; - return Vector2D(x, y); + return {x, y}; } - void Vector2D::rotate(float angle, const Vector2D &rotation_center) + void Vector2D::rotate(float angle, const Vector2D &rotation_center) noexcept { float cos = std::cos(angle); float sin = std::sin(angle); @@ -110,7 +56,7 @@ namespace kraken x_ = tmp_x; } - Vector2D &Vector2D::rotate(float angle) + Vector2D &Vector2D::rotate(float angle) noexcept { float cos = std::cos(angle); float sin = std::sin(angle); @@ -120,7 +66,7 @@ namespace kraken return *this; } - Vector2D &Vector2D::rotate(float cos, float sin) + Vector2D &Vector2D::rotate(float cos, float sin) noexcept { assert(std::abs(1 - cos * cos - sin * sin) < 0.01f); float old_x = x_; @@ -149,33 +95,13 @@ namespace kraken return static_cast(r); } - float Vector2D::squaredNorm() const - { - return x_ * x_ + y_ * y_; - } - - float Vector2D::norm() const - { - return std::sqrt(squaredNorm()); - } - - int Vector2D::distanceOctile(const Vector2D &other) const + int Vector2D::distanceOctile(const Vector2D &other) const noexcept { float dx = std::abs(x_ - other.x_); float dy = std::abs(y_ - other.y_); return static_cast(1000 * std::max(dx, dy) + 414 * std::min(dx, dy)); } - float Vector2D::getX() const - { - return x_; - } - - float Vector2D::getY() const - { - return y_; - } - void Vector2D::setX(float x) { x_ = x; @@ -186,7 +112,7 @@ namespace kraken y_ = y; } - bool Vector2D::segmentIntersection(const Vector2D &point_A1, const Vector2D &point_A2, const Vector2D &point_B1, const Vector2D &point_B2) + bool Vector2D::segmentIntersection(const Vector2D &point_A1, const Vector2D &point_A2, const Vector2D &point_B1, const Vector2D &point_B2) noexcept { // Source : https://stackoverflow.com/questions/3746274/line-intersection-with-aabb-rectangle @@ -211,13 +137,6 @@ namespace kraken return true; } - Vector2D Vector2D::fromPolar(float radius, float angle) - { - float x = std::cos(angle) * radius; - float y = std::sin(angle) * radius; - return Vector2D(x, y); - } - #if DEBUG std::ostream &operator<<(std::ostream &strm, const Vector2D &v) { diff --git a/sources/struct/vector_2d.h b/sources/struct/vector_2d.h index 27c7614..d787d01 100644 --- a/sources/struct/vector_2d.h +++ b/sources/struct/vector_2d.h @@ -2,8 +2,11 @@ #define KRAKEN_VECTOR_2D_H #if DEBUG + #include + #endif +#include namespace kraken { @@ -11,41 +14,90 @@ namespace kraken class Vector2D { public: - Vector2D(); - Vector2D(const float &x, const float &y); - - Vector2D operator+(const Vector2D &rhs) const; - Vector2D &operator+=(const Vector2D &rhs); - Vector2D operator-(const Vector2D &rhs) const; - Vector2D &operator-=(const Vector2D &rhs); - Vector2D &operator*=(const float &d); - Vector2D operator*(const float &d) const; - bool operator==(const Vector2D &rhs) const; - bool operator!=(const Vector2D &rhs) const; - float dot(const Vector2D &other) const; - - float squaredDistance(const Vector2D &other) const; - float distance(const Vector2D &other) const; - float distanceFast(const Vector2D &other) const; - Vector2D &Ysym(bool do_symmetry); - Vector2D rotate(float angle, const Vector2D &rotation_center) const; - void rotate(float angle, const Vector2D &rotation_center); - Vector2D &rotate(float angle); - Vector2D &rotate(float cos, float sin); + constexpr Vector2D() : x_(0.f), y_(0.f) + {} + + constexpr Vector2D(const float &x, const float &y) : x_(x), y_(y) + {} + + constexpr Vector2D operator+(const Vector2D &rhs) const noexcept + { + return {x_ + rhs.x_, y_ + rhs.y_}; + } + + constexpr Vector2D operator-(const Vector2D &rhs) const noexcept + { + return {x_ - rhs.x_, y_ - rhs.y_}; + } + + constexpr Vector2D operator*(const float &d) const noexcept + { + return {x_ * d, y_ * d}; + } + + constexpr bool operator==(const Vector2D &rhs) const noexcept + { + return x_ == rhs.x_ && y_ == rhs.y_; + } + + constexpr bool operator!=(const Vector2D &rhs) const noexcept + { + return x_ != rhs.x_ || y_ != rhs.y_; + } + + Vector2D &operator+=(const Vector2D &rhs) noexcept; + Vector2D &operator-=(const Vector2D &rhs) noexcept; + Vector2D &operator*=(const float &d) noexcept; + + constexpr float dot(const Vector2D &other) const noexcept + { + return x_ * other.x_ + y_ * other.y_; + } + + constexpr float squaredDistance(const Vector2D &other) const noexcept + { + return (x_ - other.x_) * (x_ - other.x_) + (y_ - other.y_) * (y_ - other.y_); + } + + constexpr float distance(const Vector2D &other) const + { + return std::sqrt(squaredDistance(other)); + } + + float distanceFast(const Vector2D &other) const noexcept; + Vector2D &Ysym(bool do_symmetry) noexcept; + Vector2D rotate(float angle, const Vector2D &rotation_center) const noexcept; + void rotate(float angle, const Vector2D &rotation_center) noexcept; + Vector2D &rotate(float angle) noexcept; + Vector2D &rotate(float cos, float sin) noexcept; float getArgument() const; float getFastArgument() const; - float squaredNorm() const; - float norm() const; + constexpr float squaredNorm() const noexcept + { + return x_ * x_ + y_ * y_; + } + + constexpr float norm() const + { + return std::sqrt(squaredNorm()); + } /** * The distance is in μm ! * @param other * @return */ - int distanceOctile(const Vector2D &other) const; + int distanceOctile(const Vector2D &other) const noexcept; + + constexpr float getX() const noexcept + { + return x_; + } - float getX() const; - float getY() const; + constexpr float getY() const noexcept + { + return y_; + } void setX(float x); void setY(float y); @@ -58,8 +110,14 @@ namespace kraken * @param pointB2 * @return */ - static bool segmentIntersection(const Vector2D &pointA1, const Vector2D &pointA2, const Vector2D &pointB1, const Vector2D &pointB2); - static Vector2D fromPolar(float radius, float angle); + static bool segmentIntersection(const Vector2D &point_A1, const Vector2D &point_A2, const Vector2D &point_B1, + const Vector2D &point_B2) noexcept; + + constexpr static Vector2D fromPolar(float radius, float angle) noexcept + { + return {std::cos(angle) * radius, std::sin(angle) * radius}; + } + protected: float x_; float y_; From 259e1c48d732e1fc6ebb874d0fa76333a0fc5193 Mon Sep 17 00:00:00 2001 From: Kayou Date: Tue, 12 Jun 2018 21:58:51 +0200 Subject: [PATCH 25/36] constexpr and noexcept for Obstacle --- sources/obstacles/obstacle.cpp | 13 +++---------- sources/obstacles/obstacle.h | 15 +++++++++------ 2 files changed, 12 insertions(+), 16 deletions(-) diff --git a/sources/obstacles/obstacle.cpp b/sources/obstacles/obstacle.cpp index 5ea5a69..07302d2 100644 --- a/sources/obstacles/obstacle.cpp +++ b/sources/obstacles/obstacle.cpp @@ -3,18 +3,11 @@ namespace kraken { + Obstacle::Obstacle(const Vector2D &rotation_center) noexcept : rotation_center_(rotation_center) + {} - Obstacle::Obstacle(const Vector2D &rotation_center) : rotation_center_(rotation_center) - { - } - - bool Obstacle::operator==(const Obstacle &rhs) const + bool Obstacle::operator==(const Obstacle &rhs) const noexcept { return rotation_center_ == rhs.rotation_center_; } - - const Vector2D &Obstacle::getRotationCenter() const - { - return rotation_center_; - } } \ No newline at end of file diff --git a/sources/obstacles/obstacle.h b/sources/obstacles/obstacle.h index 58cc173..7496c1d 100644 --- a/sources/obstacles/obstacle.h +++ b/sources/obstacles/obstacle.h @@ -11,17 +11,20 @@ namespace kraken class Obstacle { public: - Obstacle(const Vector2D &rotation_center); + Obstacle(const Vector2D &rotation_center) noexcept; virtual ~Obstacle() = default; - virtual bool isInObstacle(const Vector2D &pos) const = 0; + virtual bool isInObstacle(const Vector2D &pos) const noexcept = 0; virtual float squaredDistance(const Vector2D &pos) const = 0; virtual void getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, std::vector &vector_2d_list) const = 0; - virtual bool isColliding(const Vector2D &point_a, const Vector2D &point_b) const = 0; - virtual bool isColliding(const RectangularObstacle &obs) const = 0; - virtual bool operator==(const Obstacle &rhs) const; + virtual bool isColliding(const Vector2D &point_a, const Vector2D &point_b) const noexcept = 0; + virtual bool isColliding(const RectangularObstacle &obs) const noexcept = 0; + virtual bool operator==(const Obstacle &rhs) const noexcept; - const Vector2D &getRotationCenter() const; + constexpr Vector2D getRotationCenter() const noexcept + { + return rotation_center_; + } protected: Vector2D rotation_center_; From 9473903f774ddd69b62d74e0b3488e8bdc2b5311 Mon Sep 17 00:00:00 2001 From: Kayou Date: Tue, 12 Jun 2018 22:02:41 +0200 Subject: [PATCH 26/36] constexpr, noexcept and auto for circular_obstacle --- sources/obstacles/circular_obstacle.cpp | 41 +++++++++++-------------- sources/obstacles/circular_obstacle.h | 12 ++++---- 2 files changed, 24 insertions(+), 29 deletions(-) diff --git a/sources/obstacles/circular_obstacle.cpp b/sources/obstacles/circular_obstacle.cpp index 440cdec..a7e392e 100644 --- a/sources/obstacles/circular_obstacle.cpp +++ b/sources/obstacles/circular_obstacle.cpp @@ -7,12 +7,11 @@ namespace kraken { - CircularObstacle::CircularObstacle(const Vector2D &pos, float radius) : + CircularObstacle::CircularObstacle(const Vector2D &pos, float radius) noexcept : Obstacle(pos), radius_(radius), squared_radius_(radius * radius) - { - } + {} - bool CircularObstacle::isInObstacle(const Vector2D &pos) const + bool CircularObstacle::isInObstacle(const Vector2D &pos) const noexcept { return pos.squaredDistance(rotation_center_) <= squared_radius_; } @@ -24,14 +23,14 @@ namespace kraken return out * out; } - bool CircularObstacle::isColliding(const Vector2D &point_a, const Vector2D &point_b) const + bool CircularObstacle::isColliding(const Vector2D &point_a, const Vector2D &point_b) const noexcept { - const Vector2D point_c = rotation_center_; - const Vector2D ab = point_b - point_a; - const Vector2D ac = point_c - point_a; - float numerator = std::abs(ab.getX() * ac.getY() - ab.getY() * ac.getX()); - float denominator = ab.squaredNorm(); - float distance = numerator * numerator / denominator; + const auto point_c = rotation_center_; + const auto ab = point_b - point_a; + const auto ac = point_c - point_a; + auto numerator = std::abs(ab.getX() * ac.getY() - ab.getY() * ac.getX()); + auto denominator = ab.squaredNorm(); + auto distance = numerator * numerator / denominator; // no collision with the line (AB) if (distance > squared_radius_) @@ -48,29 +47,25 @@ namespace kraken } void CircularObstacle::getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, - std::vector &vector_2d_list) const + std::vector &vector_2d_list) const noexcept { - int nbPoints = std::ceil(M_2_PI * (radius_ + expansion) / longestAllowedLength); + auto nbPoints = static_cast(std::ceil(M_2_PI * (radius_ + expansion) / longestAllowedLength)); if (nbPoints < 3) nbPoints = 3; for (int i = 0; i < nbPoints; ++i) vector_2d_list.push_back( - Vector2D::fromPolar(expansion + radius_, i * M_2_PI / nbPoints) + rotation_center_); + Vector2D::fromPolar(expansion + radius_, i * static_cast(M_2_PI) / nbPoints) + + rotation_center_); } - bool CircularObstacle::operator==(const Obstacle &rhs) const + bool CircularObstacle::operator==(const Obstacle &rhs) const noexcept { - if (!Obstacle::operator==(rhs)) - return false; - - if (typeid(*this) != typeid(rhs)) - return false; - - return radius_ == static_cast(rhs).radius_; + return Obstacle::operator==(rhs) && typeid(*this) == typeid(rhs) && + radius_ == static_cast(rhs).radius_; } - bool CircularObstacle::isColliding(const RectangularObstacle &obs) const + bool CircularObstacle::isColliding(const RectangularObstacle &obs) const noexcept { if (rotation_center_.squaredDistance(obs.getRotationCenter()) >= (radius_ + obs.getHalfDiagonal()) * (radius_ + obs.getHalfDiagonal())) diff --git a/sources/obstacles/circular_obstacle.h b/sources/obstacles/circular_obstacle.h index fcd17d0..7107be2 100644 --- a/sources/obstacles/circular_obstacle.h +++ b/sources/obstacles/circular_obstacle.h @@ -15,14 +15,14 @@ namespace kraken class CircularObstacle : public Obstacle { public: - CircularObstacle(const Vector2D &pos, float radius); - bool isInObstacle(const Vector2D &pos) const override; + CircularObstacle(const Vector2D &pos, float radius) noexcept; + bool isInObstacle(const Vector2D &pos) const noexcept override; float squaredDistance(const Vector2D &pos) const override; void getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, - std::vector &vector_2d_list) const override; - bool isColliding(const Vector2D &point_a, const Vector2D &point_b) const override; - bool isColliding(const RectangularObstacle &obs) const override; - bool operator==(const Obstacle &rhs) const override; + std::vector &vector_2d_list) const noexcept override; + bool isColliding(const Vector2D &point_a, const Vector2D &point_b) const noexcept override; + bool isColliding(const RectangularObstacle &obs) const noexcept override; + bool operator==(const Obstacle &rhs) const noexcept override; #if DEBUG friend std::ostream &operator<<(std::ostream &strm, const CircularObstacle &o); From c5947a2b822697eeff5289ef3200f35ad8ff0f56 Mon Sep 17 00:00:00 2001 From: Kayou Date: Tue, 12 Jun 2018 22:03:33 +0200 Subject: [PATCH 27/36] constexpr, noexcept and auto for rectangular_obstacle --- sources/obstacles/rectangular_obstacle.cpp | 142 ++++++++------------- sources/obstacles/rectangular_obstacle.h | 69 ++++++---- 2 files changed, 101 insertions(+), 110 deletions(-) diff --git a/sources/obstacles/rectangular_obstacle.cpp b/sources/obstacles/rectangular_obstacle.cpp index dc4e211..a15b0c8 100644 --- a/sources/obstacles/rectangular_obstacle.cpp +++ b/sources/obstacles/rectangular_obstacle.cpp @@ -1,27 +1,24 @@ #include "rectangular_obstacle.h" -#include #include +#include #include namespace kraken { RectangularObstacle::RectangularObstacle(const int &size_x, const int &size_y, const Vector2D &position, - const float &angle) : + const float &angle) noexcept : RectangularObstacle(size_x / 2, size_x / 2, size_y / 2, size_y / 2, position, angle) - { - - } + {} RectangularObstacle::RectangularObstacle(const int &size_left_x, const int &size_right_x, const int &size_up_y, - const int &size_down_y, const Vector2D &position, const float &angle) : + const int &size_down_y, const Vector2D &position, + const float &angle) noexcept : RectangularObstacle(Vector2D(size_right_x, size_up_y), Vector2D(-size_left_x, -size_down_y), position, angle) - { - - } + {} RectangularObstacle::RectangularObstacle(const Vector2D &top_right_corner, const Vector2D &bottom_left_corner, - const Vector2D &position, const float &angle) : + const Vector2D &position, const float &angle) noexcept : Obstacle(position), angle_(angle), cos_(std::cos(angle)), sin_(std::sin(angle)), left_bottom_corner_(bottom_left_corner), left_upper_corner_(bottom_left_corner.getX(), top_right_corner.getY()), @@ -33,16 +30,14 @@ namespace kraken right_upper_corner_rotate_(toTableCoordinateSystem(right_upper_corner_)), geometric_center_((right_bottom_corner_rotate_ + left_upper_corner_rotate_) * 0.5f), half_diagonal_(top_right_corner.distance(bottom_left_corner) / 2.f) - { - - } + {} - void RectangularObstacle::update(const Vector2D &position, const float &orientation) + void RectangularObstacle::update(const Vector2D &position, const float &orientation) noexcept { update(position.getX(), position.getY(), orientation); } - void RectangularObstacle::update(const float &x, const float &y, const float &orientation) + void RectangularObstacle::update(const float &x, const float &y, const float &orientation) noexcept { rotation_center_.setX(x); rotation_center_.setY(y); @@ -57,91 +52,71 @@ namespace kraken geometric_center_ = (right_bottom_corner_rotate_ + left_upper_corner_rotate_) * 0.5f; } - Vector2D RectangularObstacle::toObstacleCoordinateSystem(const Vector2D &point) const - { - return Vector2D(getXToObstacleCoordinateSystem(point), getYToObstacleCoordinateSystem(point)); - } - - Vector2D RectangularObstacle::toTableCoordinateSystem(const Vector2D &point) const - { - return Vector2D(cos_ * point.getX() - sin_ * point.getY() + rotation_center_.getX(), - sin_ * point.getX() + cos_ * point.getY() + rotation_center_.getY()); - } - - float RectangularObstacle::getXToObstacleCoordinateSystem(const Vector2D &point) const - { - return cos_ * (point.getX() - rotation_center_.getX()) + sin_ * (point.getY() - rotation_center_.getY()); - } - - float RectangularObstacle::getYToObstacleCoordinateSystem(const Vector2D &point) const - { - return -sin_ * (point.getX() - rotation_center_.getX()) + cos_ * (point.getY() - rotation_center_.getY()); - } - void RectangularObstacle::getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, std::vector &vector_2d_list) const { - float coeff = expansion / half_diagonal_; - Vector2D corners[] = {(right_bottom_corner_rotate_ - geometric_center_) * coeff + right_bottom_corner_rotate_, - (right_upper_corner_rotate_ - geometric_center_) * coeff + right_upper_corner_rotate_, - (left_upper_corner_rotate_ - geometric_center_) * coeff + left_upper_corner_rotate_, - (left_bottom_corner_rotate_ - geometric_center_) * coeff + left_bottom_corner_rotate_}; + const auto coeff = expansion / half_diagonal_; + std::array corners = { + (right_bottom_corner_rotate_ - geometric_center_) * coeff + right_bottom_corner_rotate_, + (right_upper_corner_rotate_ - geometric_center_) * coeff + right_upper_corner_rotate_, + (left_upper_corner_rotate_ - geometric_center_) * coeff + left_upper_corner_rotate_, + (left_bottom_corner_rotate_ - geometric_center_) * coeff + left_bottom_corner_rotate_}; - for (auto const &point : corners) + for (const auto &point : corners) vector_2d_list.push_back(point); - for (int i = 0; i < 4; i++) + for (auto i = 0; i < 4; i++) { - Vector2D pointA = corners[i]; - Vector2D pointB = corners[(i + 1) % 4]; - float distance = pointA.distance(pointB); - int nbPoints = static_cast(std::ceil(distance / longestAllowedLength)); - float delta = distance / nbPoints; - for (int j = 1; j < nbPoints; j++) + const auto pointA = corners[i]; + const auto pointB = corners[(i + 1) % 4]; + auto distance = pointA.distance(pointB); + auto nbPoints = static_cast(std::ceil(distance / longestAllowedLength)); + auto delta = distance / nbPoints; + for (auto j = 1; j < nbPoints; j++) vector_2d_list.push_back((pointB - pointA) * ((j * delta) / distance) + pointA); } } - float RectangularObstacle::squaredDistance(const Vector2D &v) const + float RectangularObstacle::squaredDistance(const Vector2D &v) const noexcept { - Vector2D in = toObstacleCoordinateSystem(v); + auto position = toObstacleCoordinateSystem(v); - if (in.getX() < left_bottom_corner_.getX() && in.getY() < left_bottom_corner_.getY()) - return in.squaredDistance(left_bottom_corner_); + if (position.getX() < left_bottom_corner_.getX() && position.getY() < left_bottom_corner_.getY()) + return position.squaredDistance(left_bottom_corner_); - if (in.getX() < left_upper_corner_.getX() && in.getY() > left_upper_corner_.getY()) - return in.squaredDistance(left_upper_corner_); + if (position.getX() < left_upper_corner_.getX() && position.getY() > left_upper_corner_.getY()) + return position.squaredDistance(left_upper_corner_); - if (in.getX() > right_bottom_corner_.getX() && in.getY() < right_bottom_corner_.getY()) - return in.squaredDistance(right_bottom_corner_); + if (position.getX() > right_bottom_corner_.getX() && position.getY() < right_bottom_corner_.getY()) + return position.squaredDistance(right_bottom_corner_); - if (in.getX() > right_upper_corner_.getX() && in.getY() > right_upper_corner_.getY()) - return in.squaredDistance(right_upper_corner_); + if (position.getX() > right_upper_corner_.getX() && position.getY() > right_upper_corner_.getY()) + return position.squaredDistance(right_upper_corner_); - if (in.getX() > right_upper_corner_.getX()) - return (in.getX() - right_upper_corner_.getX()) * (in.getX() - right_upper_corner_.getX()); + if (position.getX() > right_upper_corner_.getX()) + return (position.getX() - right_upper_corner_.getX()) * (position.getX() - right_upper_corner_.getX()); - if (in.getX() < left_bottom_corner_.getX()) - return (in.getX() - left_bottom_corner_.getX()) * (in.getX() - left_bottom_corner_.getX()); + if (position.getX() < left_bottom_corner_.getX()) + return (position.getX() - left_bottom_corner_.getX()) * (position.getX() - left_bottom_corner_.getX()); - if (in.getY() > right_upper_corner_.getY()) - return (in.getY() - right_upper_corner_.getY()) * (in.getY() - right_upper_corner_.getY()); + if (position.getY() > right_upper_corner_.getY()) + return (position.getY() - right_upper_corner_.getY()) * (position.getY() - right_upper_corner_.getY()); - if (in.getY() < left_bottom_corner_.getY()) - return (in.getY() - left_bottom_corner_.getY()) * (in.getY() - left_bottom_corner_.getY()); + if (position.getY() < left_bottom_corner_.getY()) + return (position.getY() - left_bottom_corner_.getY()) * (position.getY() - left_bottom_corner_.getY()); - // else, v is in the obstacle + // else, v is position the obstacle return 0; } - bool RectangularObstacle::isInObstacle(const Vector2D &pos) const + bool RectangularObstacle::isInObstacle(const Vector2D &pos) const noexcept { - Vector2D in = toObstacleCoordinateSystem(pos); - return in.getX() >= left_bottom_corner_.getX() && in.getX() <= right_upper_corner_.getX() && - in.getY() >= left_bottom_corner_.getY() && in.getY() <= right_upper_corner_.getY(); + const auto position = toObstacleCoordinateSystem(pos); + return position.getX() >= left_bottom_corner_.getX() && position.getX() <= right_upper_corner_.getX() && + position.getY() >= left_bottom_corner_.getY() && position.getY() <= right_upper_corner_.getY(); } - bool RectangularObstacle::isColliding(const Vector2D &point_a, const Vector2D &point_b) const + bool RectangularObstacle::isColliding(const Vector2D &point_a, const Vector2D &point_b) const noexcept { if (Vector2D::segmentIntersection(point_a, point_b, left_bottom_corner_rotate_, left_upper_corner_rotate_) || Vector2D::segmentIntersection(point_a, point_b, left_upper_corner_rotate_, right_upper_corner_rotate_) @@ -152,7 +127,7 @@ namespace kraken return isInObstacle(point_a) || isInObstacle(point_b); } - bool RectangularObstacle::isColliding(const RectangularObstacle &obs) const + bool RectangularObstacle::isColliding(const RectangularObstacle &obs) const noexcept { if (rotation_center_.squaredDistance(obs.rotation_center_) >= (half_diagonal_ + obs.half_diagonal_) * (half_diagonal_ + obs.half_diagonal_)) @@ -180,12 +155,7 @@ namespace kraken obs.getYToObstacleCoordinateSystem(right_upper_corner_rotate_)); } - float RectangularObstacle::getHalfDiagonal() const - { - return half_diagonal_; - } - - bool RectangularObstacle::operator==(const Obstacle &rhs) const + bool RectangularObstacle::operator==(const Obstacle &rhs) const noexcept { if (!Obstacle::operator==(rhs)) return false; @@ -193,19 +163,19 @@ namespace kraken if (typeid(*this) != typeid(rhs)) return false; - RectangularObstacle ro_rhs = static_cast(rhs); + const auto ro_rhs = static_cast(rhs); return angle_ == ro_rhs.angle_ && left_bottom_corner_ == ro_rhs.left_bottom_corner_ && right_upper_corner_ == ro_rhs.right_upper_corner_; } bool RectangularObstacle::test_separation(const float &a, const float &b, const float &a2, const float &b2, - const float &c2, const float &d2) const + const float &c2, const float &d2) const noexcept { - float min1 = std::min(a, b); - float max1 = std::max(a, b); + auto min1 = std::min(a, b); + auto max1 = std::max(a, b); - float min2 = std::min(std::min(a2, b2), std::min(c2, d2)); - float max2 = std::max(std::max(a2, b2), std::max(c2, d2)); + auto min2 = std::min(std::min(a2, b2), std::min(c2, d2)); + auto max2 = std::max(std::max(a2, b2), std::max(c2, d2)); return min1 > max2 || min2 > max1; } diff --git a/sources/obstacles/rectangular_obstacle.h b/sources/obstacles/rectangular_obstacle.h index e8a6f60..6fc059a 100644 --- a/sources/obstacles/rectangular_obstacle.h +++ b/sources/obstacles/rectangular_obstacle.h @@ -3,43 +3,64 @@ #include "obstacle.h" #include "../struct/vector_2d.h" +#include namespace kraken { class RectangularObstacle : public Obstacle { public: - RectangularObstacle(const int &size_x, const int &size_y, const Vector2D &position = Vector2D(0, 0), - const float &angle = 0); - RectangularObstacle(const int &distance_to_front, const int &distance_to_back, const int &distance_to_left, - const int &distance_to_right, const Vector2D &position = Vector2D(0, 0), - const float &tilt_angle = 0); + RectangularObstacle(const int &size_x, const int &size_y, const Vector2D &position = {0, 0}, + const float &angle = 0) noexcept; + + RectangularObstacle(const int &size_left_x, const int &size_right_x, const int &size_up_y, + const int &size_down_y, const Vector2D &position, const float &angle = 0) noexcept; + RectangularObstacle(const Vector2D &top_right_corner, const Vector2D &bottom_left_corner, - const Vector2D &position = Vector2D(0, 0), const float &angle = 0); + const Vector2D &position, const float &angle = 0) noexcept; - void update(const Vector2D &position, const float &orientation); - void update(const float &x, const float &y, const float &orientation); + void update(const Vector2D &position, const float &orientation) noexcept; + void update(const float &x, const float &y, const float &orientation) noexcept; void getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, std::vector &vector_2d_list) const override; - float squaredDistance(const Vector2D &v) const override; - bool isInObstacle(const Vector2D &pos) const override; - bool isColliding(const Vector2D &point_a, const Vector2D &point_b) const override; - bool isColliding(const RectangularObstacle &obs) const override; + float squaredDistance(const Vector2D &v) const noexcept override; + bool isInObstacle(const Vector2D &pos) const noexcept override; + bool isColliding(const Vector2D &point_a, const Vector2D &point_b) const noexcept override; + bool isColliding(const RectangularObstacle &obs) const noexcept override; + + constexpr float getHalfDiagonal() const noexcept + { + return half_diagonal_; + } - float getHalfDiagonal() const; - bool operator==(const Obstacle &rhs) const override; + bool operator==(const Obstacle &rhs) const noexcept override; protected: - Vector2D toObstacleCoordinateSystem(const Vector2D &point) const; - Vector2D toTableCoordinateSystem(const Vector2D &point) const; + constexpr Vector2D toObstacleCoordinateSystem(const Vector2D &point) const noexcept + { + return {getXToObstacleCoordinateSystem(point), getYToObstacleCoordinateSystem(point)}; + } + + constexpr Vector2D toTableCoordinateSystem(const Vector2D &point) const noexcept + { + return {cos_ * point.getX() - sin_ * point.getY() + rotation_center_.getX(), + sin_ * point.getX() + cos_ * point.getY() + rotation_center_.getY()}; + } + + constexpr float getXToObstacleCoordinateSystem(const Vector2D &point) const + { + return cos_ * (point.getX() - rotation_center_.getX()) + sin_ * (point.getY() - rotation_center_.getY()); + } - float getXToObstacleCoordinateSystem(const Vector2D &point) const; - float getYToObstacleCoordinateSystem(const Vector2D &point) const; + constexpr float getYToObstacleCoordinateSystem(const Vector2D &point) const + { + return -sin_ * (point.getX() - rotation_center_.getX()) + cos_ * (point.getY() - rotation_center_.getY()); + } private: bool test_separation(const float &a, const float &b, const float &a2, const float &b2, const float &c2, - const float &d2) const; + const float &d2) const noexcept; #if DEBUG friend std::ostream &operator<<(std::ostream &strm, const RectangularObstacle &o); @@ -51,10 +72,10 @@ namespace kraken float sin_; // these corners are in the obstacle coordinate system - Vector2D left_bottom_corner_; - Vector2D left_upper_corner_; - Vector2D right_bottom_corner_; - Vector2D right_upper_corner_; + const Vector2D left_bottom_corner_; + const Vector2D left_upper_corner_; + const Vector2D right_bottom_corner_; + const Vector2D right_upper_corner_; // these corners are in the table coordinate system Vector2D left_bottom_corner_rotate_; @@ -63,7 +84,7 @@ namespace kraken Vector2D right_upper_corner_rotate_; Vector2D geometric_center_; - float half_diagonal_; + const float half_diagonal_; }; } From 5b6b18be229ccb6e51d74ae4432da685e6163e8f Mon Sep 17 00:00:00 2001 From: Kayou Date: Tue, 12 Jun 2018 22:04:05 +0200 Subject: [PATCH 28/36] constexpr, noexcept and auto for compound_obstacle --- sources/obstacles/compound_obstacle.cpp | 27 ++++++++++--------------- sources/obstacles/compound_obstacle.h | 10 ++++----- 2 files changed, 16 insertions(+), 21 deletions(-) diff --git a/sources/obstacles/compound_obstacle.cpp b/sources/obstacles/compound_obstacle.cpp index 1759a94..cbceafd 100644 --- a/sources/obstacles/compound_obstacle.cpp +++ b/sources/obstacles/compound_obstacle.cpp @@ -6,14 +6,14 @@ namespace kraken { CompoundObstacle::CompoundObstacle(const Vector2D &rotation_center, - std::vector obstacles_list) : + std::vector obstacles_list) noexcept : Obstacle(rotation_center), obstacles_list_(std::move(obstacles_list)) { } - bool CompoundObstacle::isInObstacle(const Vector2D &pos) const + bool CompoundObstacle::isInObstacle(const Vector2D &pos) const noexcept { - for (auto const &o : obstacles_list_) + for (const auto &o : obstacles_list_) { if (o.isInObstacle(pos)) return true; @@ -25,7 +25,7 @@ namespace kraken float CompoundObstacle::squaredDistance(const Vector2D &pos) const { float min = std::numeric_limits::max(); - for (auto const &o : obstacles_list_) + for (const auto &o : obstacles_list_) { min = std::min(min, o.squaredDistance(pos)); if (min == 0) @@ -41,9 +41,9 @@ namespace kraken o.getExpandedConvexHull(expansion, longestAllowedLength, vector_2d_list); } - bool CompoundObstacle::isColliding(const Vector2D &point_a, const Vector2D &point_b) const + bool CompoundObstacle::isColliding(const Vector2D &point_a, const Vector2D &point_b) const noexcept { - for (auto const &o : obstacles_list_) + for (const auto &o : obstacles_list_) { if (o.isColliding(point_a, point_b)) return true; @@ -51,9 +51,9 @@ namespace kraken return false; } - bool CompoundObstacle::isColliding(const RectangularObstacle &obs) const + bool CompoundObstacle::isColliding(const RectangularObstacle &obs) const noexcept { - for (auto const &o : obstacles_list_) + for (const auto &o : obstacles_list_) { if (o.isColliding(obs)) return true; @@ -61,14 +61,9 @@ namespace kraken return false; } - bool CompoundObstacle::operator==(const Obstacle &rhs) const + bool CompoundObstacle::operator==(const Obstacle &rhs) const noexcept { - if (!Obstacle::operator==(rhs)) - return false; - - if (typeid(*this) != typeid(rhs)) - return false; - - return static_cast(rhs).obstacles_list_ == obstacles_list_; + return Obstacle::operator==(rhs) && typeid(*this) == typeid(rhs) && + static_cast(rhs).obstacles_list_ == obstacles_list_; } } \ No newline at end of file diff --git a/sources/obstacles/compound_obstacle.h b/sources/obstacles/compound_obstacle.h index bf1ba8c..b483a09 100644 --- a/sources/obstacles/compound_obstacle.h +++ b/sources/obstacles/compound_obstacle.h @@ -10,16 +10,16 @@ namespace kraken class CompoundObstacle : public Obstacle { public: - CompoundObstacle(const Vector2D &rotation_center, std::vector obstacles_list); + CompoundObstacle(const Vector2D &rotation_center, std::vector obstacles_list) noexcept; - bool isInObstacle(const Vector2D &pos) const override; + bool isInObstacle(const Vector2D &pos) const noexcept override; float squaredDistance(const Vector2D &pos) const override; void getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, std::vector &vector_2d_list) const override; - bool isColliding(const Vector2D &point_a, const Vector2D &point_b) const override; - bool isColliding(const RectangularObstacle &obs) const override; + bool isColliding(const Vector2D &point_a, const Vector2D &point_b) const noexcept override; + bool isColliding(const RectangularObstacle &obs) const noexcept override; - bool operator==(const Obstacle &rhs) const override; + bool operator==(const Obstacle &rhs) const noexcept override; protected: const std::vector obstacles_list_; From 487966ff214d3d64e820466bb01ce4e04d3d4730 Mon Sep 17 00:00:00 2001 From: Kayou Date: Tue, 12 Jun 2018 22:15:52 +0200 Subject: [PATCH 29/36] fix merge --- sources/struct/vector_2d.h | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/sources/struct/vector_2d.h b/sources/struct/vector_2d.h index 6a3e3a8..8ce1f2d 100644 --- a/sources/struct/vector_2d.h +++ b/sources/struct/vector_2d.h @@ -66,8 +66,8 @@ namespace kraken float distanceFast(const Vector2D &other) const noexcept; Vector2D &Ysym(bool do_symmetry) noexcept; - Vector2D rotate(const float angle, const Vector2D &rotation_center) const noexcept; - void rotate(float angle, const Vector2D &rotation_center) noexcept; + Vector2D rotate(const float &angle, const Vector2D &rotation_center) const noexcept; + void rotate(const float &angle, const Vector2D &rotation_center) noexcept; Vector2D &rotate(const float &angle) noexcept; Vector2D &rotate(const float &cos, const float &sin) noexcept; float getArgument() const; From 4a1d35d3676d77ff634fc3625a314d7f6138271b Mon Sep 17 00:00:00 2001 From: Kayou Date: Tue, 12 Jun 2018 22:35:53 +0200 Subject: [PATCH 30/36] sqrt, sin and cos are not constexpr in each implementations --- sources/struct/vector_2d.cpp | 16 ++++++++++++++++ sources/struct/vector_2d.h | 16 +++------------- 2 files changed, 19 insertions(+), 13 deletions(-) diff --git a/sources/struct/vector_2d.cpp b/sources/struct/vector_2d.cpp index 24e2276..9388f1f 100644 --- a/sources/struct/vector_2d.cpp +++ b/sources/struct/vector_2d.cpp @@ -24,6 +24,11 @@ namespace kraken return *this; } + float Vector2D::distance(const Vector2D &other) const + { + return std::sqrt(squaredDistance(other)); + } + float Vector2D::distanceFast(const Vector2D &other) const noexcept { float dx = std::abs(x_ - other.x_); @@ -95,6 +100,11 @@ namespace kraken return static_cast(r); } + float Vector2D::norm() const + { + return std::sqrt(squaredNorm()); + } + int Vector2D::distanceOctile(const Vector2D &other) const noexcept { float dx = std::abs(x_ - other.x_); @@ -137,10 +147,16 @@ namespace kraken return true; } + Vector2D Vector2D::fromPolar(float radius, float angle) noexcept + { + return {std::cos(angle) * radius, std::sin(angle) * radius}; + } + #if DEBUG std::ostream &operator<<(std::ostream &strm, const Vector2D &v) { return strm << "Vector2D(" << v.x_ << "," << v.y_ << ")" << std::endl; } + #endif } \ No newline at end of file diff --git a/sources/struct/vector_2d.h b/sources/struct/vector_2d.h index 8ce1f2d..99897df 100644 --- a/sources/struct/vector_2d.h +++ b/sources/struct/vector_2d.h @@ -59,11 +59,7 @@ namespace kraken return (x_ - other.x_) * (x_ - other.x_) + (y_ - other.y_) * (y_ - other.y_); } - constexpr float distance(const Vector2D &other) const - { - return std::sqrt(squaredDistance(other)); - } - + float distance(const Vector2D &other) const; float distanceFast(const Vector2D &other) const noexcept; Vector2D &Ysym(bool do_symmetry) noexcept; Vector2D rotate(const float &angle, const Vector2D &rotation_center) const noexcept; @@ -77,10 +73,7 @@ namespace kraken return x_ * x_ + y_ * y_; } - constexpr float norm() const - { - return std::sqrt(squaredNorm()); - } + float norm() const; /** * The distance is in μm ! @@ -113,10 +106,7 @@ namespace kraken static bool segmentIntersection(const Vector2D &point_A1, const Vector2D &point_A2, const Vector2D &point_B1, const Vector2D &point_B2) noexcept; - constexpr static Vector2D fromPolar(float radius, float angle) noexcept - { - return {std::cos(angle) * radius, std::sin(angle) * radius}; - } + static Vector2D fromPolar(float radius, float angle) noexcept; protected: float x_; From 7177b01ba687046b7192ecae7fdae06865e630ff Mon Sep 17 00:00:00 2001 From: Kayou Date: Tue, 12 Jun 2018 22:54:50 +0200 Subject: [PATCH 31/36] Obstacle constructor is explicit --- sources/obstacles/obstacle.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sources/obstacles/obstacle.h b/sources/obstacles/obstacle.h index 7496c1d..c168534 100644 --- a/sources/obstacles/obstacle.h +++ b/sources/obstacles/obstacle.h @@ -11,7 +11,7 @@ namespace kraken class Obstacle { public: - Obstacle(const Vector2D &rotation_center) noexcept; + explicit Obstacle(const Vector2D &rotation_center) noexcept; virtual ~Obstacle() = default; virtual bool isInObstacle(const Vector2D &pos) const noexcept = 0; virtual float squaredDistance(const Vector2D &pos) const = 0; From b3eaac235d92c6af906584d0a63e255ebcba5a2a Mon Sep 17 00:00:00 2001 From: Kayou Date: Tue, 12 Jun 2018 23:25:20 +0200 Subject: [PATCH 32/36] non-default destructor for Obstacle --- sources/obstacles/obstacle.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/sources/obstacles/obstacle.h b/sources/obstacles/obstacle.h index c168534..c6455d7 100644 --- a/sources/obstacles/obstacle.h +++ b/sources/obstacles/obstacle.h @@ -12,7 +12,7 @@ namespace kraken { public: explicit Obstacle(const Vector2D &rotation_center) noexcept; - virtual ~Obstacle() = default; + virtual ~Obstacle() {}; virtual bool isInObstacle(const Vector2D &pos) const noexcept = 0; virtual float squaredDistance(const Vector2D &pos) const = 0; virtual void getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, From cb144598eb05b09ab348d065bb11e611c62bf928 Mon Sep 17 00:00:00 2001 From: Kayou Date: Tue, 12 Jun 2018 23:27:06 +0200 Subject: [PATCH 33/36] non-default destructor for Obstacle --- sources/obstacles/obstacle.h | 1 - 1 file changed, 1 deletion(-) diff --git a/sources/obstacles/obstacle.h b/sources/obstacles/obstacle.h index c6455d7..f28b4f5 100644 --- a/sources/obstacles/obstacle.h +++ b/sources/obstacles/obstacle.h @@ -12,7 +12,6 @@ namespace kraken { public: explicit Obstacle(const Vector2D &rotation_center) noexcept; - virtual ~Obstacle() {}; virtual bool isInObstacle(const Vector2D &pos) const noexcept = 0; virtual float squaredDistance(const Vector2D &pos) const = 0; virtual void getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, From fc8598a6a31a4f5672225191a586dec3a21a5535 Mon Sep 17 00:00:00 2001 From: Kayou Date: Tue, 12 Jun 2018 23:34:02 +0200 Subject: [PATCH 34/36] remove constexpr in Obstacle for gcc --- sources/obstacles/obstacle.cpp | 5 +++++ sources/obstacles/obstacle.h | 6 ++---- 2 files changed, 7 insertions(+), 4 deletions(-) diff --git a/sources/obstacles/obstacle.cpp b/sources/obstacles/obstacle.cpp index 07302d2..a003542 100644 --- a/sources/obstacles/obstacle.cpp +++ b/sources/obstacles/obstacle.cpp @@ -10,4 +10,9 @@ namespace kraken { return rotation_center_ == rhs.rotation_center_; } + + Vector2D Obstacle::getRotationCenter() const noexcept + { + return rotation_center_; + } } \ No newline at end of file diff --git a/sources/obstacles/obstacle.h b/sources/obstacles/obstacle.h index f28b4f5..7fc080b 100644 --- a/sources/obstacles/obstacle.h +++ b/sources/obstacles/obstacle.h @@ -12,6 +12,7 @@ namespace kraken { public: explicit Obstacle(const Vector2D &rotation_center) noexcept; + virtual ~Obstacle() = default; virtual bool isInObstacle(const Vector2D &pos) const noexcept = 0; virtual float squaredDistance(const Vector2D &pos) const = 0; virtual void getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, @@ -20,10 +21,7 @@ namespace kraken virtual bool isColliding(const RectangularObstacle &obs) const noexcept = 0; virtual bool operator==(const Obstacle &rhs) const noexcept; - constexpr Vector2D getRotationCenter() const noexcept - { - return rotation_center_; - } + Vector2D getRotationCenter() const noexcept; protected: Vector2D rotation_center_; From e2e560254a10575405f569901c7f6d5f0c0f3196 Mon Sep 17 00:00:00 2001 From: Kayou Date: Tue, 12 Jun 2018 23:49:01 +0200 Subject: [PATCH 35/36] fine! I'll not use constexpr! --- sources/obstacles/rectangular_obstacle.cpp | 26 +++++++++++++++++++ sources/obstacles/rectangular_obstacle.h | 29 ++++------------------ 2 files changed, 31 insertions(+), 24 deletions(-) diff --git a/sources/obstacles/rectangular_obstacle.cpp b/sources/obstacles/rectangular_obstacle.cpp index a15b0c8..e919261 100644 --- a/sources/obstacles/rectangular_obstacle.cpp +++ b/sources/obstacles/rectangular_obstacle.cpp @@ -155,6 +155,11 @@ namespace kraken obs.getYToObstacleCoordinateSystem(right_upper_corner_rotate_)); } + float RectangularObstacle::getHalfDiagonal() const noexcept + { + return half_diagonal_; + } + bool RectangularObstacle::operator==(const Obstacle &rhs) const noexcept { if (!Obstacle::operator==(rhs)) @@ -168,6 +173,27 @@ namespace kraken right_upper_corner_ == ro_rhs.right_upper_corner_; } + Vector2D RectangularObstacle::toObstacleCoordinateSystem(const Vector2D &point) const noexcept + { + return {getXToObstacleCoordinateSystem(point), getYToObstacleCoordinateSystem(point)}; + } + + Vector2D RectangularObstacle::toTableCoordinateSystem(const Vector2D &point) const noexcept + { + return {cos_ * point.getX() - sin_ * point.getY() + rotation_center_.getX(), + sin_ * point.getX() + cos_ * point.getY() + rotation_center_.getY()}; + } + + float RectangularObstacle::getXToObstacleCoordinateSystem(const Vector2D &point) const noexcept + { + return cos_ * (point.getX() - rotation_center_.getX()) + sin_ * (point.getY() - rotation_center_.getY()); + } + + float RectangularObstacle::getYToObstacleCoordinateSystem(const Vector2D &point) const noexcept + { + return -sin_ * (point.getX() - rotation_center_.getX()) + cos_ * (point.getY() - rotation_center_.getY()); + } + bool RectangularObstacle::test_separation(const float &a, const float &b, const float &a2, const float &b2, const float &c2, const float &d2) const noexcept { diff --git a/sources/obstacles/rectangular_obstacle.h b/sources/obstacles/rectangular_obstacle.h index 6fc059a..c866cf8 100644 --- a/sources/obstacles/rectangular_obstacle.h +++ b/sources/obstacles/rectangular_obstacle.h @@ -29,34 +29,15 @@ namespace kraken bool isColliding(const Vector2D &point_a, const Vector2D &point_b) const noexcept override; bool isColliding(const RectangularObstacle &obs) const noexcept override; - constexpr float getHalfDiagonal() const noexcept - { - return half_diagonal_; - } + float getHalfDiagonal() const noexcept; bool operator==(const Obstacle &rhs) const noexcept override; protected: - constexpr Vector2D toObstacleCoordinateSystem(const Vector2D &point) const noexcept - { - return {getXToObstacleCoordinateSystem(point), getYToObstacleCoordinateSystem(point)}; - } - - constexpr Vector2D toTableCoordinateSystem(const Vector2D &point) const noexcept - { - return {cos_ * point.getX() - sin_ * point.getY() + rotation_center_.getX(), - sin_ * point.getX() + cos_ * point.getY() + rotation_center_.getY()}; - } - - constexpr float getXToObstacleCoordinateSystem(const Vector2D &point) const - { - return cos_ * (point.getX() - rotation_center_.getX()) + sin_ * (point.getY() - rotation_center_.getY()); - } - - constexpr float getYToObstacleCoordinateSystem(const Vector2D &point) const - { - return -sin_ * (point.getX() - rotation_center_.getX()) + cos_ * (point.getY() - rotation_center_.getY()); - } + Vector2D toObstacleCoordinateSystem(const Vector2D &point) const noexcept; + Vector2D toTableCoordinateSystem(const Vector2D &point) const noexcept; + float getXToObstacleCoordinateSystem(const Vector2D &point) const noexcept; + float getYToObstacleCoordinateSystem(const Vector2D &point) const noexcept; private: bool test_separation(const float &a, const float &b, const float &a2, const float &b2, const float &c2, From 9880df4a31925287fb5b15913c0869ba0ccf6903 Mon Sep 17 00:00:00 2001 From: Kayou Date: Thu, 28 Jun 2018 17:21:10 +0200 Subject: [PATCH 36/36] Add StaticObstacles --- .../obstacles/container/static_obstacles.cpp | 46 +++++++++++++++++++ .../obstacles/container/static_obstacles.h | 32 +++++++++++++ 2 files changed, 78 insertions(+) create mode 100644 sources/obstacles/container/static_obstacles.cpp create mode 100644 sources/obstacles/container/static_obstacles.h diff --git a/sources/obstacles/container/static_obstacles.cpp b/sources/obstacles/container/static_obstacles.cpp new file mode 100644 index 0000000..024421f --- /dev/null +++ b/sources/obstacles/container/static_obstacles.cpp @@ -0,0 +1,46 @@ +#include "static_obstacles.h" + +namespace kraken +{ + + StaticObstacles::StaticObstacles(Vector2D bottom_left_corner, Vector2D top_right_corner) : bottom_left_corner_{ + bottom_left_corner}, top_right_corner_{top_right_corner} + { + + } + + StaticObstacles::StaticObstacles(Vector2D bottom_left_corner, Vector2D top_right_corner, + std::vector> o) + : bottom_left_corner_{bottom_left_corner}, top_right_corner_{top_right_corner}, + obstacles_{std::move(o)} + { + + } + + void StaticObstacles::add(std::shared_ptr o) + { + obstacles_.emplace_back(std::move(o)); + } + + std::vector> StaticObstacles::getObstacles() const noexcept + { + return obstacles_; + } + + Vector2D StaticObstacles::getBottomLeftCorner() const noexcept + { + return bottom_left_corner_; + } + + Vector2D StaticObstacles::getTopRightCorner() const noexcept + { + return top_right_corner_; + } + + bool StaticObstacles::isInsiderSearchDomain(const Vector2D &point) const noexcept + { + return point.getX() >= bottom_left_corner_.getX() && point.getX() <= top_right_corner_.getX() && + point.getY() >= bottom_left_corner_.getY() && point.getY() <= top_right_corner_.getY(); + } +} + diff --git a/sources/obstacles/container/static_obstacles.h b/sources/obstacles/container/static_obstacles.h new file mode 100644 index 0000000..50ad855 --- /dev/null +++ b/sources/obstacles/container/static_obstacles.h @@ -0,0 +1,32 @@ +#ifndef TESTS_STATICOBSTACLES_H +#define TESTS_STATICOBSTACLES_H + +#include +#include +#include "../../struct/vector_2d.h" +#include "../obstacle.h" + +namespace kraken +{ + + class StaticObstacles + { + public: + StaticObstacles(Vector2D bottom_left_corner, Vector2D top_right_corner); + StaticObstacles(Vector2D bottom_left_corner, Vector2D top_right_corner, std::vector> o); + void add(std::shared_ptr o); + std::vector> getObstacles() const noexcept; + Vector2D getBottomLeftCorner() const noexcept; + Vector2D getTopRightCorner() const noexcept; + bool isInsiderSearchDomain(const Vector2D& point) const noexcept; + + private: + Vector2D bottom_left_corner_; + Vector2D top_right_corner_; + std::vector> obstacles_; + }; + +} + + +#endif //TESTS_STATICOBSTACLES_H