position.h

/*******************************************************************************
 * Copyright (c) 2021, Bayerische Motoren Werke Aktiengesellschaft (BMW AG)
 *
 * This program and the accompanying materials are made
 * available under the terms of the Eclipse Public License 2.0
 * which is available at https://www.eclipse.org/legal/epl-2.0/
 *
 * SPDX-License-Identifier: EPL-2.0
 *******************************************************************************/

//-----------------------------------------------------------------------------
/** @file  position.h */
//-----------------------------------------------------------------------------

#ifndef MANTLEAPI_COMMON_POSITION_H
#define MANTLEAPI_COMMON_POSITION_H

#include <MantleAPI/Common/floating_point_helper.h>
#include <MantleAPI/Common/vector.h>
#include <units.h>

#include <cmath>
#include <cstdint>
#include <string>
#include <variant>

namespace mantle_api
{
struct OpenDriveRoadPosition
{
  /// @brief RoadId as defined in OpenDRIVE
  std::string road{};
  /// @brief Offset in s direction w.r.t. road, unit: [m]
  units::length::meter_t s_offset{0.0};
  /// @brief Offset in t direction w.r.t. reference line of road, unit: [m]
  units::length::meter_t t_offset{0.0};
};

struct OpenDriveLanePosition
{
  /// @brief RoadId as defined in OpenDRIVE
  std::string road{};
  /// @brief LaneId as defined in OpenDRIVE (e.g. -1 for right lane)
  std::int32_t lane{0};
  /// @brief Offset in s direction w.r.t lane (same as road), unit: [m]
  units::length::meter_t s_offset{0.0};
  /// @brief Offset in t direction w.r.t center line of lane, unit: [m]
  units::length::meter_t t_offset{0.0};
};

struct LatLonPosition
{
  /// @brief GPS latitude, unit: [rad]
  units::angle::radian_t latitude{0.0};
  /// @brief GPS longitude, unit: [rad]
  units::angle::radian_t longitude{0.0};
};

using Position = std::variant<OpenDriveRoadPosition, OpenDriveLanePosition, LatLonPosition, Vec3<units::length::meter_t>>;

/// @brief Equality comparison for OpenDriveRoadPosition.
///
/// **Attention** Floating-point comparision may require tweaks in precision.
inline bool operator==(const OpenDriveRoadPosition& lhs, const OpenDriveRoadPosition& rhs) noexcept
{
  return lhs.road == rhs.road &&
         IsEqual(lhs.s_offset, rhs.s_offset) &&
         IsEqual(lhs.t_offset, rhs.t_offset);
}

inline bool operator!=(const OpenDriveRoadPosition& lhs, const OpenDriveRoadPosition& rhs) noexcept
{
  return !(lhs == rhs);
}

/// @brief Equality comparison for OpenDriveLanePosition.
///
/// **Attention** Floating-point comparision may require tweaks in precision.
inline bool operator==(const OpenDriveLanePosition& lhs, const OpenDriveLanePosition& rhs) noexcept
{
  return lhs.road == rhs.road &&
         lhs.lane == rhs.lane &&
         IsEqual(lhs.s_offset, rhs.s_offset) &&
         IsEqual(lhs.t_offset, rhs.t_offset);
}

inline bool operator!=(const OpenDriveLanePosition& lhs, const OpenDriveLanePosition& rhs) noexcept
{
  return !(lhs == rhs);
}

/// @brief Equality comparison for LatLonPosition.
///
/// **Attention** Floating-point comparision may require tweaks in precision.
inline bool operator==(const LatLonPosition& lhs, const LatLonPosition& rhs) noexcept
{
  return IsEqual(lhs.latitude, rhs.latitude) && IsEqual(lhs.longitude, rhs.longitude);
}

inline bool operator!=(const LatLonPosition& lhs, const LatLonPosition& rhs) noexcept
{
  return !(lhs == rhs);
}

}  // namespace mantle_api
#endif  // MANTLEAPI_COMMON_POSITION_H