Geometry.hpp

Go to the documentation of this file.

// ----------------- BEGIN LICENSE BLOCK ---------------------------------
//
// Copyright (C) 2020-2022 Intel Corporation
//
// SPDX-License-Identifier: LGPL-2.1-only
//
// ----------------- END LICENSE BLOCK -----------------------------------
 
#pragma once
 
#include <boost/geometry.hpp>
#include <boost/geometry/geometries/geometries.hpp>
#include <boost/geometry/geometries/point_xy.hpp>
#include <boost/geometry/geometries/polygon.hpp>
#include <sstream>
#include "ad/physics/AngleOperation.hpp"
#include "ad/physics/Distance.hpp"
#include "ad/rss/state/HeadingRange.hpp"
#include "ad/rss/world/UnstructuredTrajectorySet.hpp"
 
namespace ad {
namespace rss {
namespace unstructured {
 
// Basic types
typedef boost::geometry::model::d2::point_xy<double> Point;
typedef boost::geometry::model::linestring<Point> Line;
typedef boost::geometry::model::polygon<Point, false> Polygon; // counterclockwise
typedef boost::geometry::model::multi_point<Point> MultiPoint;
 
inline Point toPoint(ad::physics::Distance2D const &distance)
{
  return Point(static_cast<double>(distance.x), static_cast<double>(distance.y));
}
 
inline Point toPoint(ad::physics::Distance const &distanceX, ad::physics::Distance const &distanceY)
{
  return Point(static_cast<double>(distanceX), static_cast<double>(distanceY));
}
 
inline ad::physics::Distance2D toDistance(Point const &point)
{
  ad::physics::Distance2D distance;
  distance.x = ad::physics::Distance(point.x());
  distance.y = ad::physics::Distance(point.y());
  return distance;
}
 
void toPolygon(world::UnstructuredTrajectorySet const &trajectorySet, Polygon &polygon);
 
void toTrajectorySet(Polygon const &polygon, world::UnstructuredTrajectorySet &trajectorySet);
 
bool isInsideHeadingRange(ad::physics::Angle const &angle, state::HeadingRange const &range);
 
bool getHeadingOverlap(state::HeadingRange const &a,
                       state::HeadingRange const &b,
                       std::vector<state::HeadingRange> &overlapRanges);
 
bool getHeadingOverlap(state::HeadingRange const &headingRange, std::vector<state::HeadingRange> &overlapRanges);
 
Point rotateAroundPoint(Point const &origin, Point const &relativePoint, ad::physics::Angle const &angle);
 
Point getPointOnCircle(Point const &origin, ad::physics::Distance const &radius, ad::physics::Angle const &angle);
 
Point getCircleOrigin(Point const &point, ad::physics::Distance const &radius, ad::physics::Angle const &angle);
 
template <typename T>
void calculateCircleArc(Point origin,
                        ad::physics::Distance const &radius,
                        ad::physics::Angle const &from,
                        ad::physics::Angle const &delta,
                        ad::physics::Angle const &stepWidth,
                        T &geometry)
{
  ad::physics::Angle currentAngle = from;
  ad::physics::Angle maxAngle = currentAngle + delta;
  while (currentAngle <= maxAngle)
  {
    boost::geometry::append(geometry, getPointOnCircle(origin, radius, currentAngle));
    currentAngle += stepWidth;
  }
  if (currentAngle - stepWidth != maxAngle)
  {
    boost::geometry::append(geometry, getPointOnCircle(origin, radius, maxAngle));
  }
}
 
bool collides(world::UnstructuredTrajectorySet const &trajectorySet1,
              world::UnstructuredTrajectorySet const &trajectorySet2);
 
bool combinePolygon(Polygon const &a, Polygon const &b, Polygon &result);
 
} // namespace unstructured
} // namespace rss
} // namespace ad
 
inline ad::rss::unstructured::Point operator+(ad::rss::unstructured::Point const &a,
                                              ad::rss::unstructured::Point const &b)
{
  auto result = a;
  boost::geometry::add_point(result, b);
  return result;
}
 
inline ad::rss::unstructured::Point operator-(ad::rss::unstructured::Point const &a,
                                              ad::rss::unstructured::Point const &b)
{
  auto result = a;
  boost::geometry::subtract_point(result, b);
  return result;
}
 
inline bool operator==(ad::rss::unstructured::Point const &a, ad::rss::unstructured::Point const &b)
{
  return (std::abs(a.x() - b.x()) <= std::numeric_limits<double>::epsilon())
    && (std::abs(a.y() - b.y()) <= std::numeric_limits<double>::epsilon());
}
 
inline bool operator!=(ad::rss::unstructured::Point const &a, ad::rss::unstructured::Point const &b)
{
  return !(a == b);
}
 
namespace std {
 
inline std::string to_string(ad::rss::unstructured::Point const &value)
{
  std::stringstream stream;
  stream << "[" << value.x() << "," << value.y() << "]";
  return stream.str();
}
 
inline std::string to_string(ad::rss::unstructured::Polygon const &value)
{
  std::stringstream stream;
  stream << "[";
  for (auto pt : value.outer())
  {
    stream << std::to_string(pt) << ",";
  }
  stream << "]";
  return stream.str();
}
 
inline std::string to_string(ad::rss::unstructured::Line value)
{
  std::stringstream stream;
  stream << "[";
  for (auto pt : value)
  {
    stream << "[" << pt.x() << "," << pt.y() << "],";
  }
  stream << "]";
  return stream.str();
}
} // namespace std