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