CARLA
geom/Transform.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/Location.h"
11 #include "carla/geom/Math.h"
12 #include "carla/geom/Rotation.h"
13 
14 #ifdef LIBCARLA_INCLUDED_FROM_UE4
15 #include "Math/Transform.h"
16 #endif // LIBCARLA_INCLUDED_FROM_UE4
17 
18 namespace carla {
19 namespace geom {
20 
21  class Transform {
22  public:
23 
24  // =========================================================================
25  // -- Public data members --------------------------------------------------
26  // =========================================================================
27 
29 
31 
32  MSGPACK_DEFINE_ARRAY(location, rotation);
33 
34  // =========================================================================
35  // -- Constructors ---------------------------------------------------------
36  // =========================================================================
37 
38  Transform() = default;
39 
40  Transform(const Location &in_location)
41  : location(in_location),
42  rotation() {}
43 
44  Transform(const Location &in_location, const Rotation &in_rotation)
45  : location(in_location),
46  rotation(in_rotation) {}
47 
48  // =========================================================================
49  // -- Other methods --------------------------------------------------------
50  // =========================================================================
51 
53  return rotation.GetForwardVector();
54  }
55 
57  return rotation.GetRightVector();
58  }
59 
61  return rotation.GetUpVector();
62  }
63 
64  /// Applies this transformation to @a in_point (first translation then rotation).
65  void TransformPoint(Vector3D &in_point) const {
66  auto out_point = in_point;
67  rotation.RotateVector(out_point); // First rotate
68  out_point += location; // Then translate
69  in_point = out_point;
70  }
71 
72  /// Applies this transformation to @a in_vector (rotation only).
73  void TransformVector(Vector3D &in_vector) const {
74  auto out_vector = in_vector;
75  rotation.RotateVector(out_vector); // First rotate
76  in_vector = out_vector;
77  }
78 
79  /// Applies the inverse of this transformation to @a in_point
80  void InverseTransformPoint(Vector3D &in_point) const {
81  auto out_point = in_point;
82  out_point -= location; // First translate inverse
83  rotation.InverseRotateVector(out_point); // Then rotate inverse
84  in_point = out_point;
85  }
86 
87  /// Computes the 4-matrix form of the transformation
88  std::array<float, 16> GetMatrix() const {
89  const float yaw = rotation.yaw;
90  const float cy = std::cos(Math::ToRadians(yaw));
91  const float sy = std::sin(Math::ToRadians(yaw));
92 
93  const float roll = rotation.roll;
94  const float cr = std::cos(Math::ToRadians(roll));
95  const float sr = std::sin(Math::ToRadians(roll));
96 
97  const float pitch = rotation.pitch;
98  const float cp = std::cos(Math::ToRadians(pitch));
99  const float sp = std::sin(Math::ToRadians(pitch));
100 
101  std::array<float, 16> transform = {
102  cp * cy, cy * sp * sr - sy * cr, -cy * sp * cr - sy * sr, location.x,
103  cp * sy, sy * sp * sr + cy * cr, -sy * sp * cr + cy * sr, location.y,
104  sp, -cp * sr, cp * cr, location.z,
105  0.0, 0.0, 0.0, 1.0};
106 
107  return transform;
108  }
109 
110  /// Computes the 4-matrix form of the inverse transformation
111  std::array<float, 16> GetInverseMatrix() const {
112  const float yaw = rotation.yaw;
113  const float cy = std::cos(Math::ToRadians(yaw));
114  const float sy = std::sin(Math::ToRadians(yaw));
115 
116  const float roll = rotation.roll;
117  const float cr = std::cos(Math::ToRadians(roll));
118  const float sr = std::sin(Math::ToRadians(roll));
119 
120  const float pitch = rotation.pitch;
121  const float cp = std::cos(Math::ToRadians(pitch));
122  const float sp = std::sin(Math::ToRadians(pitch));
123 
124  Vector3D a = {0.0f, 0.0f, 0.0f};
126 
127  std::array<float, 16> transform = {
128  cp * cy, cp * sy, sp, a.x,
129  cy * sp * sr - sy * cr, sy * sp * sr + cy * cr, -cp * sr, a.y,
130  -cy * sp * cr - sy * sr, -sy * sp * cr + cy * sr, cp * cr, a.z,
131  0.0f, 0.0f, 0.0f, 1.0};
132 
133  return transform;
134  }
135 
136  // =========================================================================
137  // -- Comparison operators -------------------------------------------------
138  // =========================================================================
139 
140  bool operator==(const Transform &rhs) const {
141  return (location == rhs.location) && (rotation == rhs.rotation);
142  }
143 
144  bool operator!=(const Transform &rhs) const {
145  return !(*this == rhs);
146  }
147 
148  // =========================================================================
149  // -- Conversions to UE4 types ---------------------------------------------
150  // =========================================================================
151 
152 #ifdef LIBCARLA_INCLUDED_FROM_UE4
153 
154  Transform(const FTransform &transform)
155  : Transform(Location(transform.GetLocation()), Rotation(transform.Rotator())) {}
156 
157  operator FTransform() const {
158  const FVector scale{1.0f, 1.0f, 1.0f};
159  return FTransform{FRotator(rotation), FVector(location), scale};
160  }
161 
162 #endif // LIBCARLA_INCLUDED_FROM_UE4
163  };
164 
165 } // namespace geom
166 } // namespace carla
Vector3D GetRightVector() const
Definition: Rotation.h:54
void InverseRotateVector(Vector3D &in_point) const
Definition: Rotation.h:96
std::array< float, 16 > GetInverseMatrix() const
Computes the 4-matrix form of the inverse transformation.
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:133
Transform(const Location &in_location)
Vector3D GetForwardVector() const
geom::Location Location
Definition: rpc/Location.h:14
bool operator==(const Transform &rhs) const
void TransformVector(Vector3D &in_vector) const
Applies this transformation to in_vector (rotation only).
Transform(const Location &in_location, const Rotation &in_rotation)
void TransformPoint(Vector3D &in_point) const
Applies this transformation to in_point (first translation then rotation).
std::array< float, 16 > GetMatrix() const
Computes the 4-matrix form of the transformation.
static constexpr T ToRadians(T deg)
Definition: Math.h:43
geom::Rotation Rotation
Definition: rpc/Transform.h:14
Vector3D GetUpVector() const
Vector3D GetForwardVector() const
Definition: Rotation.h:50
Vector3D GetRightVector() const
Vector3D GetUpVector() const
Definition: Rotation.h:58
bool operator!=(const Transform &rhs) const
void InverseTransformPoint(Vector3D &in_point) const
Applies the inverse of this transformation to in_point.
void RotateVector(Vector3D &in_point) const
Definition: Rotation.h:62
MSGPACK_DEFINE_ARRAY(location, rotation)