File angles.hpp

File List > include > nebula_decoders > nebula_decoders_common > angles.hpp
// Copyright 2024 TIER IV, Inc.
//
// 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.

#pragma once

#include <cmath>
#include <cstdint>

namespace nebula::drivers
{

struct Radians
{
  static constexpr double circle_modulus = 2 * M_PI;
};

template <uint32_t Subdivisions>
struct ScaledDegrees
{
  static constexpr double circle_modulus = 360 * Subdivisions;
};

using Degrees = ScaledDegrees<1>;
using DeciDegrees = ScaledDegrees<10>;
using CentiDegrees = ScaledDegrees<100>;
using MilliDegrees = ScaledDegrees<1000>;

template <typename T, typename AngleUnit>
struct AnglePair
{
  T azimuth;
  T elevation;
};

template <typename T, typename AngleUnit>
struct AngleRange
{
  T start;
  T end;

  [[nodiscard]] T extent() const
  {
    return (end < start) ? AngleUnit::circle_modulus - start + end : end - start;
  }
};

template <typename T, typename AngleUnit>
struct FieldOfView
{
  AngleRange<T, AngleUnit> azimuth;
  AngleRange<T, AngleUnit> elevation;
};

template <typename T>
bool angle_is_between(
  T start_angle, T end_angle, T angle, bool start_inclusive = true, bool end_inclusive = true)
{
  // Note: comments in this function refer to 360 degrees as the max_angle for simplicity, but the
  // logic works equally for other units, such as radians or centi-degrees.
  // 360-degree sector, angle coincides with boundary
  if (start_angle == end_angle && start_angle == angle) return start_inclusive || end_inclusive;

  // 360-degree sector, angle is fully inside
  if (start_angle == end_angle) return true;

  // Non-360-degree sector, angle coincides with boundary
  if (!start_inclusive && angle == start_angle) return false;
  if (!end_inclusive && angle == end_angle) return false;

  // Sector wraps around at 360/0
  if (end_angle < start_angle) return (angle <= end_angle) || (start_angle <= angle);

  // Sector does not wrap around
  return (start_angle <= angle) && (angle <= end_angle);
}

template <typename T>
T normalize_angle(T angle, T max_angle)
{
  T factor = std::floor((1.0 * angle) / max_angle);
  return angle - (factor * max_angle);
}

}  // namespace nebula::drivers