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_IPOSITION_H
#define MANTLEAPI_COMMON_IPOSITION_H

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

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

namespace mantle_api
{

struct ReferencedObject
{
    std::int32_t road{0};
    std::int32_t lane{0};
};

struct OpenDrivePosition
{
    ReferencedObject referenced_object{};

    /// @brief Offset in s direction, unit: [m]
    units::length::meter_t s_offset{0.0};
    /// @brief Offset in t direction, 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<OpenDrivePosition, LatLonPosition, Vec3<units::length::meter_t>>;

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

inline bool operator!=(const OpenDrivePosition& lhs, const OpenDrivePosition& 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_IPOSITION_H