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 Cross(const Vector3D &a, const Vector3D &b) {
59  return Vector3D(a.y * b.z - a.z * b.y, a.z * b.x - a.x * b.z, a.x * b.y - a.y * b.x);
60  }
61 
62  static auto Dot(const Vector3D &a, const Vector3D &b) {
63  return a.x * b.x + a.y * b.y + a.z * b.z;
64  }
65 
66  static auto Dot2D(const Vector3D &a, const Vector3D &b) {
67  return a.x * b.x + a.y * b.y;
68  }
69 
70  static auto DistanceSquared(const Vector3D &a, const Vector3D &b) {
71  return Square(b.x - a.x) + Square(b.y - a.y) + Square(b.z - a.z);
72  }
73 
74  static auto DistanceSquared2D(const Vector3D &a, const Vector3D &b) {
75  return Square(b.x - a.x) + Square(b.y - a.y);
76  }
77 
78  static auto Distance(const Vector3D &a, const Vector3D &b) {
79  return std::sqrt(DistanceSquared(a, b));
80  }
81 
82  static auto Distance2D(const Vector3D &a, const Vector3D &b) {
83  return std::sqrt(DistanceSquared2D(a, b));
84  }
85 
86  /// Returns the angle between 2 vectors in radians
87  static double GetVectorAngle(const Vector3D &a, const Vector3D &b);
88 
89  /// Returns a pair containing:
90  /// - @b first: distance from v to p' where p' = p projected on segment
91  /// (w - v)
92  /// - @b second: Euclidean distance from p to p'
93  /// @param p point to calculate distance
94  /// @param v first point of the segment
95  /// @param w second point of the segment
96  static std::pair<float, float> DistanceSegmentToPoint(
97  const Vector3D &p,
98  const Vector3D &v,
99  const Vector3D &w);
100 
101  /// Returns a pair containing:
102  /// - @b first: distance across the arc from start_pos to p' where p' = p
103  /// projected on Arc
104  /// - @b second: Euclidean distance from p to p'
105  static std::pair<float, float> DistanceArcToPoint(
106  Vector3D p,
107  Vector3D start_pos,
108  float length,
109  float heading, // [radians]
110  float curvature);
111 
112  static Vector3D RotatePointOnOrigin2D(Vector3D p, float angle);
113 
114  /// Compute the unit vector pointing towards the X-axis of @a rotation.
115  static Vector3D GetForwardVector(const Rotation &rotation);
116 
117  /// Compute the unit vector pointing towards the Y-axis of @a rotation.
118  static Vector3D GetRightVector(const Rotation &rotation);
119 
120  /// Compute the unit vector pointing towards the Y-axis of @a rotation.
121  static Vector3D GetUpVector(const Rotation &rotation);
122 
123  // Helper function to generate a vector of consecutive integers from a to b
124  static std::vector<int> GenerateRange(int a, int b);
125 
126  };
127 } // namespace geom
128 } // namespace carla
static auto Dot2D(const Vector3D &a, const Vector3D &b)
Definition: Math.h:66
static auto Distance(const Vector3D &a, const Vector3D &b)
Definition: Math.h:78
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:133
static auto DistanceSquared(const Vector3D &a, const Vector3D &b)
Definition: Math.h:70
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
geom::Vector3D Vector3D
Definition: rpc/Vector3D.h:14
static auto DistanceSquared2D(const Vector3D &a, const Vector3D &b)
Definition: Math.h:74
static auto Distance2D(const Vector3D &a, const Vector3D &b)
Definition: Math.h:82
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:62
static constexpr T ToDegrees(T rad)
Definition: Math.h:37
static auto Cross(const Vector3D &a, const Vector3D &b)
Definition: Math.h:58
static Vector3D RotatePointOnOrigin2D(Vector3D p, float angle)
Definition: Math.cpp:111