CARLA
Rotation.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/MsgPack.h"
10 #include "carla/geom/Math.h"
11 #include "carla/geom/Vector3D.h"
12 
13 #ifdef LIBCARLA_INCLUDED_FROM_UE4
14 # include "Math/Rotator.h"
15 #endif // LIBCARLA_INCLUDED_FROM_UE4
16 
17 namespace carla {
18 namespace geom {
19 
20  class Rotation {
21  public:
22 
23  // =========================================================================
24  // -- Public data members --------------------------------------------------
25  // =========================================================================
26 
27  float pitch = 0.0f;
28 
29  float yaw = 0.0f;
30 
31  float roll = 0.0f;
32 
33  MSGPACK_DEFINE_ARRAY(pitch, yaw, roll);
34 
35  // =========================================================================
36  // -- Constructors ---------------------------------------------------------
37  // =========================================================================
38 
39  Rotation() = default;
40 
41  Rotation(float p, float y, float r)
42  : pitch(p),
43  yaw(y),
44  roll(r) {}
45 
46  // =========================================================================
47  // -- Other methods --------------------------------------------------------
48  // =========================================================================
49 
51  return Math::GetForwardVector(*this);
52  }
53 
55  return Math::GetRightVector(*this);
56  }
57 
59  return Math::GetUpVector(*this);
60  }
61 
62  void RotateVector(Vector3D &in_point) const {
63  // Rotates Rz(yaw) * Ry(pitch) * Rx(roll) = first x, then y, then z.
64  const float cy = std::cos(Math::ToRadians(yaw));
65  const float sy = std::sin(Math::ToRadians(yaw));
66  const float cr = std::cos(Math::ToRadians(roll));
67  const float sr = std::sin(Math::ToRadians(roll));
68  const float cp = std::cos(Math::ToRadians(pitch));
69  const float sp = std::sin(Math::ToRadians(pitch));
70 
71  Vector3D out_point;
72  out_point.x =
73  in_point.x * (cp * cy) +
74  in_point.y * (cy * sp * sr - sy * cr) +
75  in_point.z * (-cy * sp * cr - sy * sr);
76 
77  out_point.y =
78  in_point.x * (cp * sy) +
79  in_point.y * (sy * sp * sr + cy * cr) +
80  in_point.z * (-sy * sp * cr + cy * sr);
81 
82  out_point.z =
83  in_point.x * (sp) +
84  in_point.y * (-cp * sr) +
85  in_point.z * (cp * cr);
86 
87  in_point = out_point;
88  }
89 
90  Vector3D RotateVector(const Vector3D& in_point) const {
91  Vector3D out_point = in_point;
92  RotateVector(out_point);
93  return out_point;
94  }
95 
96  void InverseRotateVector(Vector3D &in_point) const {
97  // Applies the transposed of the matrix used in RotateVector function,
98  // which is the rotation inverse.
99  const float cy = std::cos(Math::ToRadians(yaw));
100  const float sy = std::sin(Math::ToRadians(yaw));
101  const float cr = std::cos(Math::ToRadians(roll));
102  const float sr = std::sin(Math::ToRadians(roll));
103  const float cp = std::cos(Math::ToRadians(pitch));
104  const float sp = std::sin(Math::ToRadians(pitch));
105 
106  Vector3D out_point;
107  out_point.x =
108  in_point.x * (cp * cy) +
109  in_point.y * (cp * sy) +
110  in_point.z * (sp);
111 
112  out_point.y =
113  in_point.x * (cy * sp * sr - sy * cr) +
114  in_point.y * (sy * sp * sr + cy * cr) +
115  in_point.z * (-cp * sr);
116 
117  out_point.z =
118  in_point.x * (-cy * sp * cr - sy * sr) +
119  in_point.y * (-sy * sp * cr + cy * sr) +
120  in_point.z * (cp * cr);
121 
122  in_point = out_point;
123  }
124 
125  // =========================================================================
126  // -- Comparison operators -------------------------------------------------
127  // =========================================================================
128 
129  bool operator==(const Rotation &rhs) const {
130  return (pitch == rhs.pitch) && (yaw == rhs.yaw) && (roll == rhs.roll);
131  }
132 
133  bool operator!=(const Rotation &rhs) const {
134  return !(*this == rhs);
135  }
136 
137  // =========================================================================
138  // -- Conversions to UE4 types ---------------------------------------------
139  // =========================================================================
140 
141 #ifdef LIBCARLA_INCLUDED_FROM_UE4
142 
143  Rotation(const FRotator &rotator)
144  : Rotation(rotator.Pitch, rotator.Yaw, rotator.Roll) {}
145 
146  operator FRotator() const {
147  return FRotator{pitch, yaw, roll};
148  }
149 
150 #endif // LIBCARLA_INCLUDED_FROM_UE4
151  };
152 
153 } // namespace geom
154 } // namespace carla
Vector3D GetRightVector() const
Definition: Rotation.h:54
void InverseRotateVector(Vector3D &in_point) const
Definition: Rotation.h:96
static Vector3D GetUpVector(const Rotation &rotation)
Compute the unit vector pointing towards the Y-axis of rotation.
Definition: Math.cpp:138
Vector3D RotateVector(const Vector3D &in_point) const
Definition: Rotation.h:90
static Vector3D GetForwardVector(const Rotation &rotation)
Compute the unit vector pointing towards the X-axis of rotation.
Definition: Math.cpp:117
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
bool operator==(const Rotation &rhs) const
Definition: Rotation.h:129
bool operator!=(const Rotation &rhs) const
Definition: Rotation.h:133
static constexpr T ToRadians(T deg)
Definition: Math.h:43
Vector3D GetForwardVector() const
Definition: Rotation.h:50
Rotation(float p, float y, float r)
Definition: Rotation.h:41
MSGPACK_DEFINE_ARRAY(pitch, yaw, roll)
Vector3D GetUpVector() const
Definition: Rotation.h:58
void RotateVector(Vector3D &in_point) const
Definition: Rotation.h:62