common_2d.hpp

Go to the documentation of this file.

// Copyright 2017-2019 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

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;
}

}  // namespace geometry
}  // namespace common
}  // namespace autoware

#endif  // GEOMETRY__COMMON_2D_HPP_