spline.h 3.26 KB
Newer Older
1

Arun Das's avatar
Arun Das committed
2
/*******************************************************************************
Arun Das's avatar
Arun Das committed
3
4
5
6
7
8
9
10
 * 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
 *******************************************************************************/
Arun Das's avatar
Arun Das committed
11
12
13
14
15

//-----------------------------------------------------------------------------
/** @file  spline.h */
//-----------------------------------------------------------------------------

Arun Das's avatar
Arun Das committed
16
17
#ifndef MANTLEAPI_COMMON_SPLINE_H
#define MANTLEAPI_COMMON_SPLINE_H
Arun Das's avatar
Arun Das committed
18
19

#include <MantleAPI/Common/floating_point_helper.h>
20
#include <MantleAPI/Common/orientation.h>
21
#include <MantleAPI/Common/time_utils.h>
22
#include <MantleAPI/Common/vector.h>
23
#include <units.h>
Arun Das's avatar
Arun Das committed
24

25
26
27
#include <tuple>

using namespace units;
Arun Das's avatar
Arun Das committed
28

29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
namespace units
{
UNIT_ADD(jerk,
         meters_per_second_cubed,
         meters_per_second_cubed,
         mps_cu,
         compound_unit<velocity::meters_per_second, inverse<squared<time::seconds>>>)

namespace category
{
typedef base_unit<detail::meter_ratio<1>, std::ratio<0>, std::ratio<-3>> jerk_unit;
}

UNIT_ADD_CATEGORY_TRAIT(jerk)

UNIT_ADD(jerk_acceleration,
         meters_per_second_to_the_power_of_four,
         meters_per_second_to_the_power_of_four,
         mps_pow4,
         compound_unit<velocity::meters_per_second, inverse<cubed<time::seconds>>>)

namespace category
{
typedef base_unit<detail::meter_ratio<1>, std::ratio<0>, std::ratio<-4>> jerk_acceleration_unit;
}

UNIT_ADD_CATEGORY_TRAIT(jerk_acceleration)

}  // namespace units

Arun Das's avatar
Arun Das committed
59
60
namespace mantle_api
{
61
template <typename T>
62
63
struct SplineSegment
{
René Paris's avatar
René Paris committed
64
65
66
67
  Vec3<T> a;
  Vec3<T> b;
  Vec3<T> c;
  Vec3<T> d;
68
69
};

70
template <typename T, class = typename std::enable_if_t<traits::is_unit<T>::value>>
Arun Das's avatar
Arun Das committed
71
72
struct SplineSection
{
René Paris's avatar
René Paris committed
73
74
75
76
  Time start_time{0};
  Time end_time{0};
  /// @brief Represents the polynomial.
  ///
77
  /// The tuple stores in format \f$[a_3, a_2, a_1, a_0]\f$ for a polynomial in form
René Paris's avatar
René Paris committed
78
79
80
  /// \f[
  ///   P(x) = \sum_{i=0}^{3} a_{i} x^{i} = a_3 x^3 + a_2 x^2 + a_1 x + a_0
  /// \f]
81
82
83
84
85
86
87
88
89
  std::tuple<unit_t<compound_unit<T, inverse<cubed<time::second>>>>,
             unit_t<compound_unit<T, inverse<squared<time::second>>>>,
             unit_t<compound_unit<T, inverse<time::second>>>,
             unit_t<T>>
      polynomial{
          unit_t<compound_unit<T, inverse<cubed<time::second>>>>(0),
          unit_t<compound_unit<T, inverse<squared<time::second>>>>(0),
          unit_t<compound_unit<T, inverse<time::second>>>(0),
          unit_t<T>(0)};
Arun Das's avatar
Arun Das committed
90
91
92
};

/// @brief Equality comparison for SplineSection.
93
template <typename T, class = typename std::enable_if_t<units::traits::is_unit<T>::value>>
94
inline bool operator==(const SplineSection<T>& lhs, const SplineSection<T>& rhs) noexcept
Arun Das's avatar
Arun Das committed
95
{
96
  return lhs.start_time == rhs.start_time && lhs.end_time == rhs.end_time && IsEqual(std::get<0>(lhs.polynomial), std::get<0>(rhs.polynomial)) && IsEqual(std::get<1>(lhs.polynomial), std::get<1>(rhs.polynomial)) && IsEqual(std::get<2>(lhs.polynomial), std::get<2>(rhs.polynomial)) && IsEqual(std::get<3>(lhs.polynomial), std::get<3>(rhs.polynomial));
Arun Das's avatar
Arun Das committed
97
98
99
100
}

}  // namespace mantle_api

Arun Das's avatar
Arun Das committed
101
#endif  // MANTLEAPI_COMMON_SPLINE_H