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 "geometry/interval.hpp"
 
using autoware::common::types::float32_t;
using autoware::common::types::bool8_t;
 
namespace autoware
{
namespace common
{
namespace geometry
{
 
namespace point_adapter
{
template<typename PointT>
inline auto x_(const PointT & pt)
{
  return pt.x;
}
template<typename PointT>
inline auto y_(const PointT & pt)
{
  return pt.y;
}
template<typename PointT>
inline auto z_(const PointT & pt)
{
  return pt.z;
}
template<typename PointT>
inline auto & xr_(PointT & pt)
{
  return pt.x;
}
template<typename PointT>
inline auto & yr_(PointT & pt)
{
  return pt.y;
}
template<typename PointT>
inline auto & zr_(PointT & pt)
{
  return pt.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) = point_adapter::x_(p) * a;
  point_adapter::yr_(r) = 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(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 = 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 = 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 T>
inline bool is_left_of_line_2d(const T & p1, const T & p2, const T & q)
{
  return cross_2d(minus_2d(p2, p1), minus_2d(q, p1)) > 0.0F;
}
 
template<typename IT>
bool all_ccw(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;
  }
  // 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);
    if (!is_left_of_line_2d(*line_start, *line_end, *query_point)) {
      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 T{0.5} *area;
}
 
template<typename IT>
auto area_checked_2d(const IT begin, const IT end)
{
  if (!all_ccw(begin, end)) {
    throw std::domain_error{"Cannot compute area: points are not CCW"};
  }
  return area_2d(begin, end);
}
 
}  // namespace geometry
}  // namespace common
}  // namespace autoware
 
#endif  // GEOMETRY__COMMON_2D_HPP_