diff --git a/sources/obstacles/circular_obstacle.cpp b/sources/obstacles/circular_obstacle.cpp new file mode 100644 index 0000000..a7e392e --- /dev/null +++ b/sources/obstacles/circular_obstacle.cpp @@ -0,0 +1,85 @@ +#include "obstacle.h" +#include "circular_obstacle.h" +#include "rectangular_obstacle.h" +#include "../struct/vector_2d.h" +#include +#include + +namespace kraken +{ + CircularObstacle::CircularObstacle(const Vector2D &pos, float radius) noexcept : + Obstacle(pos), radius_(radius), squared_radius_(radius * radius) + {} + + bool CircularObstacle::isInObstacle(const Vector2D &pos) const noexcept + { + 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 noexcept + { + 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_) + 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); + } + + void CircularObstacle::getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, + std::vector &vector_2d_list) const noexcept + { + 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 * static_cast(M_2_PI) / nbPoints) + + rotation_center_); + } + + bool CircularObstacle::operator==(const Obstacle &rhs) const noexcept + { + return Obstacle::operator==(rhs) && typeid(*this) == typeid(rhs) && + radius_ == static_cast(rhs).radius_; + } + + bool CircularObstacle::isColliding(const RectangularObstacle &obs) const noexcept + { + if (rotation_center_.squaredDistance(obs.getRotationCenter()) >= + (radius_ + obs.getHalfDiagonal()) * (radius_ + obs.getHalfDiagonal())) + return false; + return obs.squaredDistance(rotation_center_) < squared_radius_; + } + +#if DEBUG + + std::ostream &operator<<(std::ostream &strm, const CircularObstacle &o) + { + return strm << "CircularObstacle(" << o.rotation_center_ << "," << o.radius_ << ")" << std::endl; + } + +#endif + +} diff --git a/sources/obstacles/circular_obstacle.h b/sources/obstacles/circular_obstacle.h new file mode 100644 index 0000000..7107be2 --- /dev/null +++ b/sources/obstacles/circular_obstacle.h @@ -0,0 +1,37 @@ +#ifndef TESTS_CIRCULAR_OBSTACLE_H +#define TESTS_CIRCULAR_OBSTACLE_H + +#include "../struct/vector_2d.h" +#include "obstacle.h" + +#if DEBUG + +#include + +#endif + +namespace kraken +{ + class CircularObstacle : public Obstacle + { + public: + 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 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); +#endif + + protected: + float const radius_; + float const squared_radius_; + }; +} + +#endif //TESTS_CIRCULAR_OBSTACLE_H diff --git a/sources/obstacles/compound_obstacle.cpp b/sources/obstacles/compound_obstacle.cpp new file mode 100644 index 0000000..cbceafd --- /dev/null +++ b/sources/obstacles/compound_obstacle.cpp @@ -0,0 +1,69 @@ +#include "compound_obstacle.h" + +#include +#include + +namespace kraken +{ + CompoundObstacle::CompoundObstacle(const Vector2D &rotation_center, + std::vector obstacles_list) noexcept : + Obstacle(rotation_center), obstacles_list_(std::move(obstacles_list)) + { + } + + bool CompoundObstacle::isInObstacle(const Vector2D &pos) const noexcept + { + for (const auto &o : obstacles_list_) + { + if (o.isInObstacle(pos)) + return true; + } + + return false; + } + + float CompoundObstacle::squaredDistance(const Vector2D &pos) const + { + float min = std::numeric_limits::max(); + for (const auto &o : obstacles_list_) + { + min = std::min(min, o.squaredDistance(pos)); + if (min == 0) + break; + } + return min; + } + + void 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 Vector2D &point_a, const Vector2D &point_b) const noexcept + { + for (const auto &o : obstacles_list_) + { + if (o.isColliding(point_a, point_b)) + return true; + } + return false; + } + + bool CompoundObstacle::isColliding(const RectangularObstacle &obs) const noexcept + { + for (const auto &o : obstacles_list_) + { + if (o.isColliding(obs)) + return true; + } + return false; + } + + bool CompoundObstacle::operator==(const Obstacle &rhs) const noexcept + { + 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 new file mode 100644 index 0000000..b483a09 --- /dev/null +++ b/sources/obstacles/compound_obstacle.h @@ -0,0 +1,29 @@ +#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) 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 noexcept override; + bool isColliding(const RectangularObstacle &obs) const noexcept override; + + bool operator==(const Obstacle &rhs) const noexcept override; + + protected: + const std::vector obstacles_list_; + }; + +} +#endif //TESTS_COMPOUND_OBSTACLE_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 diff --git a/sources/obstacles/obstacle.cpp b/sources/obstacles/obstacle.cpp new file mode 100644 index 0000000..a003542 --- /dev/null +++ b/sources/obstacles/obstacle.cpp @@ -0,0 +1,18 @@ +#include "obstacle.h" + + +namespace kraken +{ + Obstacle::Obstacle(const Vector2D &rotation_center) noexcept : rotation_center_(rotation_center) + {} + + bool Obstacle::operator==(const Obstacle &rhs) const noexcept + { + 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 new file mode 100644 index 0000000..7fc080b --- /dev/null +++ b/sources/obstacles/obstacle.h @@ -0,0 +1,31 @@ +#ifndef TESTS_OBSTACLE_H +#define TESTS_OBSTACLE_H + +#include +#include "../struct/vector_2d.h" + +namespace kraken +{ + class RectangularObstacle; + + class Obstacle + { + 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, + std::vector &vector_2d_list) const = 0; + 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; + + Vector2D getRotationCenter() const noexcept; + protected: + Vector2D rotation_center_; + + }; +} + +#endif //TESTS_OBSTACLE_H diff --git a/sources/obstacles/rectangular_obstacle.cpp b/sources/obstacles/rectangular_obstacle.cpp new file mode 100644 index 0000000..e919261 --- /dev/null +++ b/sources/obstacles/rectangular_obstacle.cpp @@ -0,0 +1,219 @@ +#include "rectangular_obstacle.h" +#include +#include +#include + +namespace kraken +{ + RectangularObstacle::RectangularObstacle(const int &size_x, const int &size_y, const Vector2D &position, + 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) 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) 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()), + 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) + {} + + 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) noexcept + { + 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; + } + + void RectangularObstacle::getExpandedConvexHull(const float &expansion, const float &longestAllowedLength, + std::vector &vector_2d_list) const + { + 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 (const auto &point : corners) + vector_2d_list.push_back(point); + + for (auto i = 0; i < 4; i++) + { + 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 noexcept + { + auto position = toObstacleCoordinateSystem(v); + + if (position.getX() < left_bottom_corner_.getX() && position.getY() < left_bottom_corner_.getY()) + return position.squaredDistance(left_bottom_corner_); + + if (position.getX() < left_upper_corner_.getX() && position.getY() > left_upper_corner_.getY()) + return position.squaredDistance(left_upper_corner_); + + if (position.getX() > right_bottom_corner_.getX() && position.getY() < right_bottom_corner_.getY()) + return position.squaredDistance(right_bottom_corner_); + + if (position.getX() > right_upper_corner_.getX() && position.getY() > right_upper_corner_.getY()) + return position.squaredDistance(right_upper_corner_); + + if (position.getX() > right_upper_corner_.getX()) + return (position.getX() - right_upper_corner_.getX()) * (position.getX() - right_upper_corner_.getX()); + + if (position.getX() < left_bottom_corner_.getX()) + return (position.getX() - left_bottom_corner_.getX()) * (position.getX() - left_bottom_corner_.getX()); + + if (position.getY() > right_upper_corner_.getY()) + return (position.getY() - right_upper_corner_.getY()) * (position.getY() - right_upper_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 position the obstacle + return 0; + } + + bool RectangularObstacle::isInObstacle(const Vector2D &pos) const noexcept + { + 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 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_) + || 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 noexcept + { + 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 noexcept + { + return half_diagonal_; + } + + bool RectangularObstacle::operator==(const Obstacle &rhs) const noexcept + { + if (!Obstacle::operator==(rhs)) + return false; + + if (typeid(*this) != typeid(rhs)) + return false; + + 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_; + } + + 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 + { + auto min1 = std::min(a, b); + auto max1 = std::max(a, b); + + 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; + } + +#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/rectangular_obstacle.h b/sources/obstacles/rectangular_obstacle.h new file mode 100644 index 0000000..c866cf8 --- /dev/null +++ b/sources/obstacles/rectangular_obstacle.h @@ -0,0 +1,72 @@ +#ifndef TESTS_RECTANGULAR_OBSTACLE_H +#define TESTS_RECTANGULAR_OBSTACLE_H + +#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 = {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, const float &angle = 0) noexcept; + + 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 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; + + float getHalfDiagonal() const noexcept; + + bool operator==(const Obstacle &rhs) const noexcept override; + + protected: + 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, + const float &d2) const noexcept; + +#if DEBUG + friend std::ostream &operator<<(std::ostream &strm, const RectangularObstacle &o); +#endif + + protected: + float angle_; + float cos_; + float sin_; + + // these corners are in the obstacle coordinate system + 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_; + Vector2D left_upper_corner_rotate_; + Vector2D right_bottom_corner_rotate_; + Vector2D right_upper_corner_rotate_; + + Vector2D geometric_center_; + const float half_diagonal_; + }; +} + +#endif //TESTS_RECTANGULAR_OBSTACLE_H diff --git a/sources/struct/vector_2d.cpp b/sources/struct/vector_2d.cpp index c888ef7..9388f1f 100644 --- a/sources/struct/vector_2d.cpp +++ b/sources/struct/vector_2d.cpp @@ -1,102 +1,58 @@ -#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) + 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(const bool &do_symmetry) + Vector2D &Vector2D::Ysym(bool do_symmetry) noexcept { if (do_symmetry) y_ = -y_; return *this; } - Vector2D Vector2D::rotate(const float &angle, const Vector2D &rotation_center) const + Vector2D Vector2D::rotate(const 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(const float &angle, const Vector2D &rotation_center) + void Vector2D::rotate(const float &angle, const Vector2D &rotation_center) noexcept { float cos = std::cos(angle); float sin = std::sin(angle); @@ -105,7 +61,7 @@ namespace kraken x_ = tmp_x; } - Vector2D &Vector2D::rotate(const float &angle) + Vector2D &Vector2D::rotate(const float &angle) noexcept { float cos = std::cos(angle); float sin = std::sin(angle); @@ -115,7 +71,7 @@ namespace kraken return *this; } - Vector2D &Vector2D::rotate(const float &cos, const float &sin) + Vector2D &Vector2D::rotate(const float &cos, const float &sin) noexcept { assert(std::abs(1 - cos * cos - sin * sin) < 0.01f); float old_x = x_; @@ -144,33 +100,18 @@ 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; @@ -181,7 +122,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) noexcept { // Source : https://stackoverflow.com/questions/3746274/line-intersection-with-aabb-rectangle @@ -206,11 +147,9 @@ namespace kraken return true; } - Vector2D Vector2D::fromPolar(float radius, float angle) + Vector2D Vector2D::fromPolar(float radius, float angle) noexcept { - float x = std::cos(angle) * radius; - float y = std::sin(angle) * radius; - return Vector2D(x, y); + return {std::cos(angle) * radius, std::sin(angle) * radius}; } #if DEBUG @@ -218,5 +157,6 @@ namespace kraken { 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 a6990b7..99897df 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,29 +14,65 @@ 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); - bool operator==(const Vector2D &rhs) const; - bool operator!=(const Vector2D &rhs) const; - float dot(const Vector2D &other) const; - - float squaredDistance(const Vector2D &other) const; + 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_); + } + float distance(const Vector2D &other) const; - float distanceFast(const Vector2D &other) const; - Vector2D &Ysym(const bool &do_symmetry); - Vector2D rotate(const float &angle, const Vector2D &rotation_center) const; - void rotate(const float &angle, const Vector2D &rotation_center); - Vector2D &rotate(const float &angle); - Vector2D &rotate(const float &cos, const float &sin); + 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(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; float getFastArgument() const; - float squaredNorm() const; + constexpr float squaredNorm() const noexcept + { + return x_ * x_ + y_ * y_; + } + float norm() const; /** @@ -41,10 +80,17 @@ namespace kraken * @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); @@ -57,8 +103,11 @@ namespace kraken * @param pointB2 * @return */ - static bool segmentIntersection(Vector2D &pointA1, Vector2D &pointA2, Vector2D &pointB1, 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; + + static Vector2D fromPolar(float radius, float angle) noexcept; + protected: float x_; float y_; 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))); +}