CARLA
Math.cpp
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 #include "carla/geom/Math.h"
8 
9 #include "carla/geom/Rotation.h"
10 
11 namespace carla {
12 namespace geom {
13 
14  double Math::GetVectorAngle(const Vector3D &a, const Vector3D &b) {
15  return std::acos(Dot(a, b) / (a.Length() * b.Length()));
16  }
17 
18  std::pair<float, float> Math::DistanceSegmentToPoint(
19  const Vector3D &p,
20  const Vector3D &v,
21  const Vector3D &w) {
22  const float l2 = DistanceSquared2D(v, w);
23  const float l = std::sqrt(l2);
24  if (l2 == 0.0f) {
25  return std::make_pair(0.0f, Distance2D(v, p));
26  }
27  const float dot_p_w = Dot2D(p - v, w - v);
28  const float t = Clamp(dot_p_w / l2);
29  const Vector3D projection = v + t * (w - v);
30  return std::make_pair(t * l, Distance2D(projection, p));
31  }
32 
33  std::pair<float, float> Math::DistanceArcToPoint(
34  Vector3D p,
35  Vector3D start_pos,
36  const float length,
37  float heading, // [radians]
38  float curvature) {
39 
40  /// @todo: Because Unreal's coordinates, hacky way to correct
41  /// the -y, this must be changed in the future
42  p.y = -p.y;
43  start_pos.y = -start_pos.y;
44  heading = -heading;
45  curvature = -curvature;
46 
47  // since this algorithm is working for positive curvatures,
48  // and we are only calculating distances, we can invert the y
49  // axis (along with the curvature and the heading), so if the
50  // curvature is negative, the algorithm will work as expected
51  if (curvature < 0.0f) {
52  p.y = -p.y;
53  start_pos.y = -start_pos.y;
54  heading = -heading;
55  curvature = -curvature;
56  }
57 
58  // transport point relative to the arc starting poistion and rotation
59  const Vector3D rotated_p(RotatePointOnOrigin2D(p - start_pos, -heading));
60 
61  const float radius = 1.0f / curvature;
62  const Vector3D circ_center(0.0f, radius, 0.0f);
63 
64  // check if the point is in the center of the circle, so we know p
65  // is in the same distance of every possible point in the arc
66  if (rotated_p == circ_center) {
67  return std::make_pair(0.0f, radius);
68  }
69 
70  // find intersection position using the unit vector from the center
71  // of the circle to the point and multiplying by the radius
72  const Vector3D intersection = ((rotated_p - circ_center).MakeUnitVector() * radius) + circ_center;
73 
74  // use the arc length to calculate the angle in the last point of it
75  // circumference of a circle = 2 * PI * r
76  // last_point_angle = (length / circumference) * 2 * PI
77  // so last_point_angle = length / radius
78  const float last_point_angle = length / radius;
79 
80  constexpr float pi_half = Pi<float>() / 2.0f;
81 
82  // move the point relative to the center of the circle and find
83  // the angle between the point and the center of coords in rad
84  float angle = std::atan2(intersection.y - radius, intersection.x) + pi_half;
85 
86  if (angle < 0.0f) {
87  angle += Pi<float>() * 2.0f;
88  }
89 
90  // see if the angle is between 0 and last_point_angle
91  DEBUG_ASSERT(angle >= 0.0f);
92  if (angle <= last_point_angle) {
93  return std::make_pair(
94  angle * radius,
95  Distance2D(intersection, rotated_p));
96  }
97 
98  // find the nearest point, start or end to intersection
99  const float start_dist = Distance2D(Vector3D(), rotated_p);
100 
101  const Vector3D end_pos(
102  radius * std::cos(last_point_angle - pi_half),
103  radius * std::sin(last_point_angle - pi_half) + circ_center.y,
104  0.0f);
105  const float end_dist = Distance2D(end_pos, rotated_p);
106  return (start_dist < end_dist) ?
107  std::make_pair(0.0f, start_dist) :
108  std::make_pair(length, end_dist);
109  }
110 
112  const float s = std::sin(angle);
113  const float c = std::cos(angle);
114  return Vector3D(p.x * c - p.y * s, p.x * s + p.y * c, 0.0f);
115  }
116 
118  const float cp = std::cos(ToRadians(rotation.pitch));
119  const float sp = std::sin(ToRadians(rotation.pitch));
120  const float cy = std::cos(ToRadians(rotation.yaw));
121  const float sy = std::sin(ToRadians(rotation.yaw));
122  return {cy * cp, sy * cp, sp};
123  }
124 
126  const float cy = std::cos(ToRadians(rotation.yaw));
127  const float sy = std::sin(ToRadians(rotation.yaw));
128  const float cr = std::cos(ToRadians(rotation.roll));
129  const float sr = std::sin(ToRadians(rotation.roll));
130  const float cp = std::cos(ToRadians(rotation.pitch));
131  const float sp = std::sin(ToRadians(rotation.pitch));
132  return {
133  cy * sp * sr - sy * cr,
134  sy * sp * sr + cy * cr,
135  -cp * sr};
136  }
137 
139  const float cy = std::cos(ToRadians(rotation.yaw));
140  const float sy = std::sin(ToRadians(rotation.yaw));
141  const float cr = std::cos(ToRadians(rotation.roll));
142  const float sr = std::sin(ToRadians(rotation.roll));
143  const float cp = std::cos(ToRadians(rotation.pitch));
144  const float sp = std::sin(ToRadians(rotation.pitch));
145  return {
146  -cy * sp * cr - sy * sr,
147  -sy * sp * cr + cy * sr,
148  -cp * cr};
149  }
150 
151  std::vector<int> Math::GenerateRange(int a, int b) {
152  std::vector<int> result;
153  if (a < b) {
154  for(int i = a; i <= b; ++i) {
155  result.push_back(i);
156  }
157  } else {
158  for(int i = a; i >= b; --i) {
159  result.push_back(i);
160  }
161  }
162  return result;
163  }
164 
165 } // namespace geom
166 } // namespace carla
static auto Dot2D(const Vector3D &a, const Vector3D &b)
Definition: Math.h:62
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 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 Vector3D GetRightVector(const Rotation &rotation)
Compute the unit vector pointing towards the Y-axis of rotation.
Definition: Math.cpp:125
float Length() const
Definition: geom/Vector3D.h:49
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
#define DEBUG_ASSERT(predicate)
Definition: Debug.h:66
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
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 Vector3D RotatePointOnOrigin2D(Vector3D p, float angle)
Definition: Math.cpp:111