CARLA
Math.h
Go to the documentation of this file.
1 // Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma
2 // de Barcelona (UAB).
3 //
4 // This work is licensed under the terms of the MIT license.
5 // For a copy, see <https://opensource.org/licenses/MIT>.
6 
7 #pragma once
8 
9 #include "carla/Debug.h"
10 #include "carla/geom/Vector3D.h"
11 
12 #include <cmath>
13 #include <type_traits>
14 #include <utility>
15 
16 namespace carla {
17 namespace geom {
18 
19  class Rotation;
20 
21  class Math {
22  public:
23 
24  template <typename T>
25  static constexpr T Pi() {
26  static_assert(std::is_floating_point<T>::value, "type must be floating point");
27  return static_cast<T>(3.14159265358979323846264338327950288);
28  }
29 
30  template <typename T>
31  static constexpr T Pi2() {
32  static_assert(std::is_floating_point<T>::value, "type must be floating point");
33  return static_cast<T>(static_cast<T>(2) * Pi<T>());
34  }
35 
36  template <typename T>
37  static constexpr T ToDegrees(T rad) {
38  static_assert(std::is_floating_point<T>::value, "type must be floating point");
39  return rad * (T(180.0) / Pi<T>());
40  }
41 
42  template <typename T>
43  static constexpr T ToRadians(T deg) {
44  static_assert(std::is_floating_point<T>::value, "type must be floating point");
45  return deg * (Pi<T>() / T(180.0));
46  }
47 
48  template <typename T>
49  static T Clamp(T a, T min = T(0), T max = T(1)) {
50  return std::min(std::max(a, min), max);
51  }
52 
53  template <typename T>
54  static T Square(const T &a) {
55  return a * a;
56  }
57 
58  static auto Dot(const Vector3D &a, const Vector3D &b) {
59  return a.x * b.x + a.y * b.y + a.z * b.z;
60  }
61 
62  static auto Dot2D(const Vector3D &a, const Vector3D &b) {
63  return a.x * b.x + a.y * b.y;
64  }
65 
66  static auto DistanceSquared(const Vector3D &a, const Vector3D &b) {
67  return Square(b.x - a.x) + Square(b.y - a.y) + Square(b.z - a.z);
68  }
69 
70  static auto DistanceSquared2D(const Vector3D &a, const Vector3D &b) {
71  return Square(b.x - a.x) + Square(b.y - a.y);
72  }
73 
74  static auto Distance(const Vector3D &a, const Vector3D &b) {
75  return std::sqrt(DistanceSquared(a, b));
76  }
77 
78  static auto Distance2D(const Vector3D &a, const Vector3D &b) {
79  return std::sqrt(DistanceSquared2D(a, b));
80  }
81 
82  /// Returns the angle between 2 vectors in radians
83  static double GetVectorAngle(const Vector3D &a, const Vector3D &b);
84 
85  /// Returns a pair containing:
86  /// - @b first: distance from v to p' where p' = p projected on segment
87  /// (w - v)
88  /// - @b second: Euclidean distance from p to p'
89  /// @param p point to calculate distance
90  /// @param v first point of the segment
91  /// @param w second point of the segment
92  static std::pair<float, float> DistanceSegmentToPoint(
93  const Vector3D &p,
94  const Vector3D &v,
95  const Vector3D &w);
96 
97  /// Returns a pair containing:
98  /// - @b first: distance across the arc from start_pos to p' where p' = p
99  /// projected on Arc
100  /// - @b second: Euclidean distance from p to p'
101  static std::pair<float, float> DistanceArcToPoint(
102  Vector3D p,
103  Vector3D start_pos,
104  float length,
105  float heading, // [radians]
106  float curvature);
107 
108  static Vector3D RotatePointOnOrigin2D(Vector3D p, float angle);
109 
110  /// Compute the unit vector pointing towards the X-axis of @a rotation.
111  static Vector3D GetForwardVector(const Rotation &rotation);
112 
113  /// Compute the unit vector pointing towards the Y-axis of @a rotation.
114  static Vector3D GetRightVector(const Rotation &rotation);
115 
116  /// Compute the unit vector pointing towards the Y-axis of @a rotation.
117  static Vector3D GetUpVector(const Rotation &rotation);
118 
119  // Helper function to generate a vector of consecutive integers from a to b
120  static std::vector<int> GenerateRange(int a, int b);
121 
122  };
123 } // namespace geom
124 } // namespace carla
static auto Dot2D(const Vector3D &a, const Vector3D &b)
Definition: Math.h:62
static auto Distance(const Vector3D &a, const Vector3D &b)
Definition: Math.h:74
static Vector3D GetUpVector(const Rotation &rotation)
Compute the unit vector pointing towards the Y-axis of rotation.
Definition: Math.cpp:138
static std::pair< float, float > DistanceSegmentToPoint(const Vector3D &p, const Vector3D &v, const Vector3D &w)
Returns a pair containing:
Definition: Math.cpp:18
static constexpr T Pi()
Definition: Math.h:25
static T Square(const T &a)
Definition: Math.h:54
static Vector3D GetForwardVector(const Rotation &rotation)
Compute the unit vector pointing towards the X-axis of rotation.
Definition: Math.cpp:117
static T Clamp(T a, T min=T(0), T max=T(1))
Definition: Math.h:49
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:99
static auto DistanceSquared(const Vector3D &a, const Vector3D &b)
Definition: Math.h:66
static Vector3D GetRightVector(const Rotation &rotation)
Compute the unit vector pointing towards the Y-axis of rotation.
Definition: Math.cpp:125
static std::pair< float, float > DistanceArcToPoint(Vector3D p, Vector3D start_pos, float length, float heading, float curvature)
Returns a pair containing:
Definition: Math.cpp:33
static auto DistanceSquared2D(const Vector3D &a, const Vector3D &b)
Definition: Math.h:70
static auto Distance2D(const Vector3D &a, const Vector3D &b)
Definition: Math.h:78
static constexpr T ToRadians(T deg)
Definition: Math.h:43
geom::Rotation Rotation
Definition: rpc/Transform.h:14
static constexpr T Pi2()
Definition: Math.h:31
static double GetVectorAngle(const Vector3D &a, const Vector3D &b)
Returns the angle between 2 vectors in radians.
Definition: Math.cpp:14
static std::vector< int > GenerateRange(int a, int b)
Definition: Math.cpp:151
static auto Dot(const Vector3D &a, const Vector3D &b)
Definition: Math.h:58
static constexpr T ToDegrees(T rad)
Definition: Math.h:37
static Vector3D RotatePointOnOrigin2D(Vector3D p, float angle)
Definition: Math.cpp:111