common_2d.hpp

Go to the documentation of this file.

// Copyright 2017-2021 the Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.
//
// Co-developed by Tier IV, Inc. and Apex.AI, Inc.
 
#ifndef GEOMETRY__COMMON_2D_HPP_
#define GEOMETRY__COMMON_2D_HPP_
 
#include <common/types.hpp>
#include <cmath>
#include <limits>
#include <stdexcept>
 
#include "autoware_auto_planning_msgs/msg/trajectory_point.hpp"
#include "geometry/interval.hpp"
 
using autoware::common::types::float32_t;
using autoware::common::types::bool8_t;
 
namespace comparison = autoware::common::helper_functions::comparisons;
 
namespace autoware
{
namespace common
{
namespace geometry
{
 
namespace point_adapter
{
template<typename PointT>
inline auto x_(const PointT & pt)
{
  return pt.x;
}
inline auto x_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt)
{
  return pt.pose.position.x;
}
template<typename PointT>
inline auto y_(const PointT & pt)
{
  return pt.y;
}
inline auto y_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt)
{
  return pt.pose.position.y;
}
template<typename PointT>
inline auto z_(const PointT & pt)
{
  return pt.z;
}
inline auto z_(const autoware_auto_planning_msgs::msg::TrajectoryPoint & pt)
{
  return pt.pose.position.z;
}
template<typename PointT>
inline auto & xr_(PointT & pt)
{
  return pt.x;
}
inline auto & xr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt)
{
  return pt.pose.position.x;
}
template<typename PointT>
inline auto & yr_(PointT & pt)
{
  return pt.y;
}
inline auto & yr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt)
{
  return pt.pose.position.y;
}
template<typename PointT>
inline auto & zr_(PointT & pt)
{
  return pt.z;
}
inline auto & zr_(autoware_auto_planning_msgs::msg::TrajectoryPoint & pt)
{
  return pt.pose.position.z;
}
}  // namespace point_adapter
 
namespace details
{
// Return the next iterator, cycling back to the beginning of the list if you hit the end
template<typename IT>
IT circular_next(const IT begin, const IT end, const IT current) noexcept
{
  auto next = std::next(current);
  if (end == next) {
    next = begin;
  }
  return next;
}
 
}  // namespace details
 
 
template<typename T1, typename T2, typename T3>
inline auto ccw(const T1 & pt, const T2 & q, const T3 & r)
{
  using point_adapter::x_;
  using point_adapter::y_;
 
  return (((x_(q) - x_(pt)) * (y_(r) - y_(pt))) - ((y_(q) - y_(pt)) * (x_(r) - x_(pt)))) <= 0.0F;
}
 
template<typename T1, typename T2>
inline auto cross_2d(const T1 & pt, const T2 & q)
{
  using point_adapter::x_;
  using point_adapter::y_;
  return (x_(pt) * y_(q)) - (y_(pt) * x_(q));
}
 
template<typename T1, typename T2>
inline auto dot_2d(const T1 & pt, const T2 & q)
{
  using point_adapter::x_;
  using point_adapter::y_;
  return (x_(pt) * x_(q)) + (y_(pt) * y_(q));
}
 
template<typename T>
T minus_2d(const T & p, const T & q)
{
  T r;
  using point_adapter::x_;
  using point_adapter::y_;
  point_adapter::xr_(r) = x_(p) - x_(q);
  point_adapter::yr_(r) = y_(p) - y_(q);
  return r;
}
 
template<typename T>
T minus_2d(const T & p)
{
  T r;
  point_adapter::xr_(r) = -point_adapter::x_(p);
  point_adapter::yr_(r) = -point_adapter::y_(p);
  return r;
}
template<typename T>
T plus_2d(const T & p, const T & q)
{
  T r;
  using point_adapter::x_;
  using point_adapter::y_;
  point_adapter::xr_(r) = x_(p) + x_(q);
  point_adapter::yr_(r) = y_(p) + y_(q);
  return r;
}
 
template<typename T>
T times_2d(const T & p, const float32_t a)
{
  T r;
  point_adapter::xr_(r) = static_cast<float32_t>(point_adapter::x_(p)) * a;
  point_adapter::yr_(r) = static_cast<float32_t>(point_adapter::y_(p)) * a;
  return r;
}
 
template<typename T>
inline T intersection_2d(const T & pt, const T & u, const T & q, const T & v)
{
  const float32_t num = cross_2d(minus_2d(pt, q), u);
  float32_t den = cross_2d(v, u);
  constexpr auto FEPS = std::numeric_limits<float32_t>::epsilon();
  if (fabsf(den) < FEPS) {
    if (fabsf(num) < FEPS) {
      // collinear case, anything is ok
      den = 1.0F;
    } else {
      // parallel case, no valid output
      throw std::runtime_error(
              "intersection_2d: no unique solution (either collinear or parallel)");
    }
  }
  return plus_2d(q, times_2d(v, num / den));
}
 
 
template<typename T>
inline void rotate_2d(T & pt, const float32_t cos_th, const float32_t sin_th)
{
  const float32_t x = point_adapter::x_(pt);
  const float32_t y = point_adapter::y_(pt);
  point_adapter::xr_(pt) = (cos_th * x) - (sin_th * y);
  point_adapter::yr_(pt) = (sin_th * x) + (cos_th * y);
}
 
template<typename T>
inline T rotate_2d(const T & pt, const float32_t th_rad)
{
  T q(pt);  // It's reasonable to expect a copy constructor
  const float32_t s = sinf(th_rad);
  const float32_t c = cosf(th_rad);
  rotate_2d(q, c, s);
  return q;
}
 
template<typename T>
inline T get_normal(const T & pt)
{
  T q(pt);
  point_adapter::xr_(q) = -point_adapter::y_(pt);
  point_adapter::yr_(q) = point_adapter::x_(pt);
  return q;
}
 
template<typename T>
inline auto norm_2d(const T & pt)
{
  return sqrtf(static_cast<float32_t>(dot_2d(pt, pt)));
}
 
template<typename T>
inline T closest_segment_point_2d(const T & p, const T & q, const T & r)
{
  const T qp = minus_2d(q, p);
  const float32_t len2 = static_cast<float32_t>(dot_2d(qp, qp));
  T ret = p;
  if (len2 > std::numeric_limits<float32_t>::epsilon()) {
    const Interval_f unit_interval(0.0f, 1.0f);
    const float32_t val = static_cast<float32_t>(dot_2d(minus_2d(r, p), qp)) / len2;
    const float32_t t = Interval_f::clamp_to(unit_interval, val);
    ret = plus_2d(p, times_2d(qp, t));
  }
  return ret;
}
//
//         Obtained by simplifying closest_segment_point_2d.
//         define a line
template<typename T>
inline T closest_line_point_2d(const T & p, const T & q, const T & r)
{
  const T qp = minus_2d(q, p);
  const float32_t len2 = dot_2d(qp, qp);
  T ret = p;
  if (len2 > std::numeric_limits<float32_t>::epsilon()) {
    const float32_t t = dot_2d(minus_2d(r, p), qp) / len2;
    ret = plus_2d(p, times_2d(qp, t));
  } else {
    throw std::runtime_error(
            "closet_line_point_2d: line ill-defined because given points coincide");
  }
  return ret;
}
 
template<typename T>
inline auto point_line_segment_distance_2d(const T & p, const T & q, const T & r)
{
  const T pq_r = minus_2d(closest_segment_point_2d(p, q, r), r);
  return norm_2d(pq_r);
}
 
template<typename T>
inline T make_unit_vector2d(float th)
{
  T r;
  point_adapter::xr_(r) = std::cos(th);
  point_adapter::yr_(r) = std::sin(th);
  return r;
}
 
template<typename OUT = float32_t, typename T1, typename T2>
inline OUT squared_distance_2d(const T1 & a, const T2 & b)
{
  const auto x = static_cast<OUT>(point_adapter::x_(a)) - static_cast<OUT>(point_adapter::x_(b));
  const auto y = static_cast<OUT>(point_adapter::y_(a)) - static_cast<OUT>(point_adapter::y_(b));
  return (x * x) + (y * y);
}
 
template<typename OUT = float32_t, typename T1, typename T2>
inline OUT distance_2d(const T1 & a, const T2 & b)
{
  return std::sqrt(squared_distance_2d<OUT>(a, b));
}
 
template<typename T>
inline auto check_point_position_to_line_2d(const T & p1, const T & p2, const T & q)
{
  return cross_2d(minus_2d(p2, p1), minus_2d(q, p1));
}
 
template<typename IT>
bool all_ordered(const IT begin, const IT end) noexcept
{
  // Short circuit: a line or point is always CCW or otherwise ill-defined
  if (std::distance(begin, end) <= 2U) {
    return true;
  }
  bool is_first_point_ccw = false;
  // Can't use std::all_of because that works directly on the values
  for (auto line_start = begin; line_start != end; ++line_start) {
    const auto line_end = details::circular_next(begin, end, line_start);
    const auto query_point = details::circular_next(begin, end, line_end);
    // Check if 3 points starting from current point are in counter clockwise direction
    const bool is_ccw = ccw(*line_start, *line_end, *query_point);
    if (is_ccw) {
      if (line_start == begin) {
        is_first_point_ccw = true;
      } else {
        if (!is_first_point_ccw) {
          return false;
        }
      }
    } else if (is_first_point_ccw) {
      return false;
    }
  }
  return true;
}
 
template<typename IT>
auto area_2d(const IT begin, const IT end) noexcept
{
  using point_adapter::x_;
  using point_adapter::y_;
  using T = decltype(x_(*begin));
  auto area = T{};  // zero initialization
  // use the approach of https://www.mathwords.com/a/area_convex_polygon.htm
  for (auto it = begin; it != end; ++it) {
    const auto next = details::circular_next(begin, end, it);
    area += x_(*it) * y_(*next);
    area -= x_(*next) * y_(*it);
  }
  return std::abs(T{0.5} *area);
}
 
template<typename IT>
auto area_checked_2d(const IT begin, const IT end)
{
  if (!all_ordered(begin, end)) {
    throw std::domain_error{"Cannot compute area: points are not ordered"};
  }
  return area_2d(begin, end);
}
 
template<typename IteratorType, typename PointType>
bool is_point_inside_polygon_2d(
  const IteratorType & start_it, const IteratorType & end_it, const PointType & p)
{
  std::int32_t winding_num = 0;
 
  for (IteratorType it = start_it; it != end_it; ++it) {
    auto next_it = std::next(it);
    if (next_it == end_it) {
      next_it = start_it;
    }
 
    if (comparison::abs_eq(point_adapter::y_(*it), point_adapter::y_(p), 1e-3F) &&
      comparison::abs_eq(point_adapter::x_(*it), point_adapter::x_(p), 1e-3F))
    {
      return true;
    }
 
    if (point_adapter::y_(*it) <= point_adapter::y_(p)) {
      // Upward crossing edge
      if (point_adapter::y_(*next_it) > point_adapter::y_(p)) {
        if (comparison::abs_gt(
            check_point_position_to_line_2d(*it, *next_it, p), 0.0F,
            1e-3F))
        {
          ++winding_num;
        } else {
          if (comparison::abs_eq_zero(
              check_point_position_to_line_2d(*it, *next_it, p),
              1e-3F))
          {
            return true;
          }
        }
      }
    } else {
      // Downward crossing edge
      if (point_adapter::y_(*next_it) <= point_adapter::y_(p)) {
        if (comparison::abs_lt(
            check_point_position_to_line_2d(*it, *next_it, p), 0.0F,
            1e-3F))
        {
          --winding_num;
        } else {
          if (comparison::abs_eq_zero(
              check_point_position_to_line_2d(*it, *next_it, p),
              1e-3F))
          {
            return true;
          }
        }
      }
    }
  }
 
  return winding_num != 0;
}
 
 
}  // namespace geometry
}  // namespace common
}  // namespace autoware
 
#endif  // GEOMETRY__COMMON_2D_HPP_