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