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 the inverse of this transformation to @a in_point
73  void InverseTransformPoint(Vector3D &in_point) const {
74  auto out_point = in_point;
75  out_point -= location; // First translate inverse
76  rotation.InverseRotateVector(out_point); // Then rotate inverse
77  in_point = out_point;
78  }
79 
80  /// Computes the 4-matrix form of the transformation
81  std::array<float, 16> GetMatrix() const {
82  const float yaw = rotation.yaw;
83  const float cy = std::cos(Math::ToRadians(yaw));
84  const float sy = std::sin(Math::ToRadians(yaw));
85 
86  const float roll = rotation.roll;
87  const float cr = std::cos(Math::ToRadians(roll));
88  const float sr = std::sin(Math::ToRadians(roll));
89 
90  const float pitch = rotation.pitch;
91  const float cp = std::cos(Math::ToRadians(pitch));
92  const float sp = std::sin(Math::ToRadians(pitch));
93 
94  std::array<float, 16> transform = {
95  cp * cy, cy * sp * sr - sy * cr, -cy * sp * cr - sy * sr, location.x,
96  cp * sy, sy * sp * sr + cy * cr, -sy * sp * cr + cy * sr, location.y,
97  sp, -cp * sr, cp * cr, location.z,
98  0.0, 0.0, 0.0, 1.0};
99 
100  return transform;
101  }
102 
103  /// Computes the 4-matrix form of the inverse transformation
104  std::array<float, 16> GetInverseMatrix() const {
105  const float yaw = rotation.yaw;
106  const float cy = std::cos(Math::ToRadians(yaw));
107  const float sy = std::sin(Math::ToRadians(yaw));
108 
109  const float roll = rotation.roll;
110  const float cr = std::cos(Math::ToRadians(roll));
111  const float sr = std::sin(Math::ToRadians(roll));
112 
113  const float pitch = rotation.pitch;
114  const float cp = std::cos(Math::ToRadians(pitch));
115  const float sp = std::sin(Math::ToRadians(pitch));
116 
117  Vector3D a = {0.0f, 0.0f, 0.0f};
119 
120  std::array<float, 16> transform = {
121  cp * cy, cp * sy, sp, a.x,
122  cy * sp * sr - sy * cr, sy * sp * sr + cy * cr, -cp * sr, a.y,
123  -cy * sp * cr - sy * sr, -sy * sp * cr + cy * sr, cp * cr, a.z,
124  0.0f, 0.0f, 0.0f, 1.0};
125 
126  return transform;
127  }
128 
129  // =========================================================================
130  // -- Comparison operators -------------------------------------------------
131  // =========================================================================
132 
133  bool operator==(const Transform &rhs) const {
134  return (location == rhs.location) && (rotation == rhs.rotation);
135  }
136 
137  bool operator!=(const Transform &rhs) const {
138  return !(*this == rhs);
139  }
140 
141  // =========================================================================
142  // -- Conversions to UE4 types ---------------------------------------------
143  // =========================================================================
144 
145 #ifdef LIBCARLA_INCLUDED_FROM_UE4
146 
147  Transform(const FTransform &transform)
148  : Transform(Location(transform.GetLocation()), Rotation(transform.Rotator())) {}
149 
150  operator FTransform() const {
151  const FVector scale{1.0f, 1.0f, 1.0f};
152  return FTransform{FRotator(rotation), FVector(location), scale};
153  }
154 
155 #endif // LIBCARLA_INCLUDED_FROM_UE4
156  };
157 
158 } // namespace geom
159 } // 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:99
Transform(const Location &in_location)
Vector3D GetForwardVector() const
geom::Location Location
Definition: rpc/Location.h:14
bool operator==(const Transform &rhs) const
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)