CARLA
geom/Vector3D.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 
11 #include <cmath>
12 #include <limits>
13 
14 namespace carla {
15 namespace geom {
16 
17  class Vector3D {
18  public:
19 
20  // =========================================================================
21  // -- Public data members --------------------------------------------------
22  // =========================================================================
23 
24  float x = 0.0f;
25 
26  float y = 0.0f;
27 
28  float z = 0.0f;
29 
30  // =========================================================================
31  // -- Constructors ---------------------------------------------------------
32  // =========================================================================
33 
34  Vector3D() = default;
35 
36  Vector3D(float ix, float iy, float iz)
37  : x(ix),
38  y(iy),
39  z(iz) {}
40 
41  // =========================================================================
42  // -- Other methods --------------------------------------------------------
43  // =========================================================================
44 
45  float SquaredLength() const {
46  return x * x + y * y + z * z;
47  }
48 
49  float Length() const {
50  return std::sqrt(SquaredLength());
51  }
52 
53  float SquaredLength2D() const {
54  return x * x + y * y;
55  }
56 
57  float Length2D() const {
58  return std::sqrt(SquaredLength2D());
59  }
60 
61  Vector3D Abs() const {
62  return Vector3D(abs(x), abs(y), abs(z));
63  }
64 
66  const float length = Length();
67  DEVELOPMENT_ASSERT(length > 2.0f * std::numeric_limits<float>::epsilon());
68  const float k = 1.0f / length;
69  return Vector3D(x * k, y * k, z * k);
70  }
71 
72  Vector3D MakeSafeUnitVector(const float epsilon) const {
73  const float length = Length();
74  const float k = (length > std::max(epsilon, 0.0f)) ? (1.0f / length) : 1.0f;
75  return Vector3D(x * k, y * k, z * k);
76  }
77 
78  // =========================================================================
79  // -- Arithmetic operators -------------------------------------------------
80  // =========================================================================
81 
82  Vector3D &operator+=(const Vector3D &rhs) {
83  x += rhs.x;
84  y += rhs.y;
85  z += rhs.z;
86  return *this;
87  }
88 
89  friend Vector3D operator+(Vector3D lhs, const Vector3D &rhs) {
90  lhs += rhs;
91  return lhs;
92  }
93 
94  Vector3D &operator-=(const Vector3D &rhs) {
95  x -= rhs.x;
96  y -= rhs.y;
97  z -= rhs.z;
98  return *this;
99  }
100 
101  friend Vector3D operator-(Vector3D lhs, const Vector3D &rhs) {
102  lhs -= rhs;
103  return lhs;
104  }
105 
106  Vector3D& operator-=(const float f) {
107  x -= f;
108  y -= f;
109  z -= f;
110  return *this;
111  }
112 
113  Vector3D &operator*=(float rhs) {
114  x *= rhs;
115  y *= rhs;
116  z *= rhs;
117  return *this;
118  }
119 
120  friend Vector3D operator*(Vector3D lhs, float rhs) {
121  lhs *= rhs;
122  return lhs;
123  }
124 
125  friend Vector3D operator*(float lhs, Vector3D rhs) {
126  rhs *= lhs;
127  return rhs;
128  }
129 
130  Vector3D &operator/=(float rhs) {
131  x /= rhs;
132  y /= rhs;
133  z /= rhs;
134  return *this;
135  }
136 
137  friend Vector3D operator/(Vector3D lhs, float rhs) {
138  lhs /= rhs;
139  return lhs;
140  }
141 
142  friend Vector3D operator/(float lhs, Vector3D rhs) {
143  rhs /= lhs;
144  return rhs;
145  }
146 
147  // =========================================================================
148  // -- Comparison operators -------------------------------------------------
149  // =========================================================================
150 
151  bool operator==(const Vector3D &rhs) const {
152  return (x == rhs.x) && (y == rhs.y) && (z == rhs.z);
153  }
154 
155  bool operator!=(const Vector3D &rhs) const {
156  return !(*this == rhs);
157  }
158 
159  // =========================================================================
160  // -- Conversions to UE4 types ---------------------------------------------
161  // =========================================================================
162 
163 #ifdef LIBCARLA_INCLUDED_FROM_UE4
164 
165  /// These 2 methods are explicitly deleted to avoid creating them by other users,
166  /// unlike locations, some vectors have units and some don't, by removing
167  /// these methods we found several places were the conversion from cm to m was missing
168  Vector3D(const FVector &v) = delete;
169  Vector3D& operator=(const FVector &rhs) = delete;
170 
171  /// Return a Vector3D converted from centimeters to meters.
172  Vector3D ToMeters() const {
173  return *this * 1e-2f;
174  }
175 
176  /// Return a Vector3D converted from meters to centimeters.
177  Vector3D ToCentimeters() const {
178  return *this * 1e2f;
179  }
180 
181  FVector ToFVector() const {
182  return FVector{x, y, z};
183  }
184 
185 #endif // LIBCARLA_INCLUDED_FROM_UE4
186 
187  // =========================================================================
188  /// @todo The following is copy-pasted from MSGPACK_DEFINE_ARRAY.
189  /// This is a workaround for an issue in msgpack library. The
190  /// MSGPACK_DEFINE_ARRAY macro is shadowing our `z` variable.
191  /// https://github.com/msgpack/msgpack-c/issues/709
192  // =========================================================================
193  template <typename Packer>
194  void msgpack_pack(Packer& pk) const
195  {
196  clmdep_msgpack::type::make_define_array(x, y, z).msgpack_pack(pk);
197  }
198  void msgpack_unpack(clmdep_msgpack::object const& o)
199  {
200  clmdep_msgpack::type::make_define_array(x, y, z).msgpack_unpack(o);
201  }
202  template <typename MSGPACK_OBJECT>
203  void msgpack_object(MSGPACK_OBJECT* o, clmdep_msgpack::zone& sneaky_variable_that_shadows_z) const
204  {
205  clmdep_msgpack::type::make_define_array(x, y, z).msgpack_object(o, sneaky_variable_that_shadows_z);
206  }
207  // =========================================================================
208  };
209 
210 } // namespace geom
211 } // namespace carla
Vector3D & operator*=(float rhs)
friend Vector3D operator*(Vector3D lhs, float rhs)
void msgpack_pack(Packer &pk) const
void msgpack_unpack(clmdep_msgpack::object const &o)
Vector3D & operator/=(float rhs)
friend Vector3D operator+(Vector3D lhs, const Vector3D &rhs)
Definition: geom/Vector3D.h:89
float SquaredLength() const
Definition: geom/Vector3D.h:45
This file contains definitions of common data structures used in traffic manager. ...
Definition: Carla.cpp:133
float Length2D() const
Definition: geom/Vector3D.h:57
Vector3D Abs() const
Definition: geom/Vector3D.h:61
void msgpack_object(MSGPACK_OBJECT *o, clmdep_msgpack::zone &sneaky_variable_that_shadows_z) const
float Length() const
Definition: geom/Vector3D.h:49
bool operator!=(const Vector3D &rhs) const
Vector3D(float ix, float iy, float iz)
Definition: geom/Vector3D.h:36
friend Vector3D operator*(float lhs, Vector3D rhs)
friend Vector3D operator-(Vector3D lhs, const Vector3D &rhs)
#define DEVELOPMENT_ASSERT(pred)
Definition: Debug.h:82
Vector3D & operator-=(const Vector3D &rhs)
Definition: geom/Vector3D.h:94
friend Vector3D operator/(Vector3D lhs, float rhs)
bool operator==(const Vector3D &rhs) const
float SquaredLength2D() const
Definition: geom/Vector3D.h:53
Vector3D MakeUnitVector() const
Definition: geom/Vector3D.h:65
Vector3D & operator+=(const Vector3D &rhs)
Definition: geom/Vector3D.h:82
friend Vector3D operator/(float lhs, Vector3D rhs)
Vector3D MakeSafeUnitVector(const float epsilon) const
Definition: geom/Vector3D.h:72
Vector3D & operator-=(const float f)