Add forward vector to rotations and transforms

CHANGELOG.md

@@ -2,6 +2,7 @@
 
   * Added point transformation functionality for LibCarla and PythonAPI
   * Added "sensor_tick" attribute to sensors (cameras and lidars) to specify the capture rate in seconds
+  * Added "get_forward_vector()" to rotation and transform, retrieves the unit vector on the rotation's X-axis
   * Added support for Deepin in PythonAPI's setup.py
 
 ## CARLA 0.9.2

Docs/python_api.md

@@ -243,6 +243,7 @@ Static presets
 - `pitch`
 - `yaw`
 - `roll`
+- `get_forward_vector()`
 - `__eq__(other)`
 - `__ne__(other)`
 
@@ -250,10 +251,10 @@ Static presets
 
 - `location`
 - `rotation`
+- `transform(geom_object)`
+- `get_forward_vector()`
 - `__eq__(other)`
 - `__ne__(other)`
-- `transform_point`
-- `transform_point_list`
 
 ## `carla.BoundingBox`
 

LibCarla/source/carla/geom/Math.cpp

@@ -6,6 +6,8 @@
 
 #include "carla/geom/Math.h"
 
+#include "carla/geom/Rotation.h"
+
 namespace carla {
 namespace geom {
 
@@ -127,5 +129,13 @@ namespace geom {
            transf_p.x >= -extent.x && transf_p.y >= -extent.y;
   }
 
+  Vector3D Math::GetForwardVector(const Rotation &rotation) {
+    const float cp = std::cos(to_radians(rotation.pitch));
+    const float sp = std::sin(to_radians(rotation.pitch));
+    const float cy = std::cos(to_radians(rotation.yaw));
+    const float sy = std::sin(to_radians(rotation.yaw));
+    return {cy * cp, sy * cp, sp};
+  }
+
 } // namespace geom
 } // namespace carla

LibCarla/source/carla/geom/Math.h

@@ -15,6 +15,8 @@
 namespace carla {
 namespace geom {
 
+  class Rotation;
+
   class Math {
   public:
 
@@ -99,6 +101,9 @@ namespace geom {
         const Vector3D &,
         double, // [radians]
         const Vector3D &);
+
+    /// Compute the unit vector pointing towards the X-axis of @a rotation.
+    static Vector3D GetForwardVector(const Rotation &rotation);
   };
 
 } // namespace geom

LibCarla/source/carla/geom/Rotation.h

@@ -7,6 +7,8 @@
 #pragma once
 
 #include "carla/MsgPack.h"
+#include "carla/geom/Math.h"
+#include "carla/geom/Vector3D.h"
 
 #ifdef LIBCARLA_INCLUDED_FROM_UE4
 #  include "Math/Rotator.h"
@@ -41,6 +43,14 @@ namespace geom {
         yaw(y),
         roll(r) {}
 
+    // =========================================================================
+    // -- Other methods --------------------------------------------------------
+    // =========================================================================
+
+    Vector3D GetForwardVector() const {
+      return Math::GetForwardVector(*this);
+    }
+
     // =========================================================================
     // -- Comparison operators -------------------------------------------------
     // =========================================================================

LibCarla/source/carla/geom/Transform.h

@@ -45,6 +45,10 @@ namespace geom {
     // -- Other methods --------------------------------------------------------
     // =========================================================================
 
+    Vector3D GetForwardVector() const {
+      return rotation.GetForwardVector();
+    }
+
     void TransformPoint(Vector3D &in_point) const {
       // Rotate
       double cy = cos(Math::to_radians(rotation.yaw));

LibCarla/source/test/test_geom.cpp

@@ -11,6 +11,17 @@
 #include <carla/geom/Transform.h>
 #include <limits>
 
+namespace carla {
+namespace geom {
+
+  std::ostream &operator<<(std::ostream &out, const Vector3D &vector3D) {
+    out << "{x=" << vector3D.x << ", y=" << vector3D.y << ", z=" << vector3D.z << '}';
+    return out;
+  }
+
+} // namespace geom
+} // namespace carla
+
 using namespace carla::geom;
 
 TEST(geom, single_point_no_transform) {
@@ -76,7 +87,6 @@ TEST(geom, single_point_translation_and_rotation) {
   ASSERT_NEAR(point.z, result_point.z, error);
 }
 
-
 TEST(geom, distance) {
   constexpr double error = .01;
   ASSERT_NEAR(Math::Distance({0, 0, 0}, {0, 0, 0}), 0.0, error);
@@ -138,6 +148,27 @@ TEST(geom, nearest_point_segment) {
   }
 }
 
+TEST(geom, forward_vector) {
+  auto compare = [](Rotation rotation, Vector3D expected) {
+    constexpr float eps = 2.0f * std::numeric_limits<float>::epsilon();
+    auto result = rotation.GetForwardVector();
+    EXPECT_TRUE(
+            (std::abs(expected.x - result.x) < eps) &&
+            (std::abs(expected.y - result.y) < eps) &&
+            (std::abs(expected.z - result.z) < eps))
+        << "result   = " << result << '\n'
+        << "expected = " << expected;
+  };
+  //        pitch     yaw    roll       x     y     z
+  compare({  0.0f,   0.0f,   0.0f}, {1.0f, 0.0f, 0.0f});
+  compare({  0.0f,   0.0f, 123.0f}, {1.0f, 0.0f, 0.0f});
+  compare({360.0f, 360.0f,   0.0f}, {1.0f, 0.0f, 0.0f});
+  compare({  0.0f,  90.0f,   0.0f}, {0.0f, 1.0f, 0.0f});
+  compare({  0.0f, -90.0f,   0.0f}, {0.0f,-1.0f, 0.0f});
+  compare({ 90.0f,   0.0f,   0.0f}, {0.0f, 0.0f, 1.0f});
+  compare({180.0f, -90.0f,   0.0f}, {0.0f, 1.0f, 0.0f});
+}
+
 TEST(geom, point_in_rectangle) {
   ASSERT_TRUE(Math::PointInRectangle(
       Vector3D(0, 0, 0), Vector3D(1, 1, 0), 0, Vector3D(0, 0, 0)));

PythonAPI/source/libcarla/Geom.cpp

@@ -100,24 +100,25 @@ class_<cg::Location, bases<cg::Vector3D>>("Location")
     .def_readwrite("pitch", &cg::Rotation::pitch)
     .def_readwrite("yaw", &cg::Rotation::yaw)
     .def_readwrite("roll", &cg::Rotation::roll)
+    .def("get_forward_vector", &cg::Rotation::GetForwardVector)
     .def("__eq__", &cg::Rotation::operator==)
     .def("__ne__", &cg::Rotation::operator!=)
     .def(self_ns::str(self_ns::self))
   ;
 
   class_<cg::Transform>("Transform")
-
     .def(init<cg::Location, cg::Rotation>(
         (arg("location")=cg::Location(), arg("rotation")=cg::Rotation())))
     .def_readwrite("location", &cg::Transform::location)
     .def_readwrite("rotation", &cg::Transform::rotation)
-    .def("__eq__", &cg::Transform::operator==)
-    .def("__ne__", &cg::Transform::operator!=)
     .def("transform", &TransformList)
     .def("transform", +[](const cg::Transform &self, cg::Vector3D &location) {
       self.TransformPoint(location);
       return location;
     }, arg("in_point"))
+    .def("get_forward_vector", &cg::Transform::GetForwardVector)
+    .def("__eq__", &cg::Transform::operator==)
+    .def("__ne__", &cg::Transform::operator!=)
     .def(self_ns::str(self_ns::self))
   ;
 

PythonAPI/source/libcarla/libcarla.cpp

@@ -119,12 +119,12 @@ static auto MakeCallback(boost::python::object callback) {
   };
 }
 
+#include "Geom.cpp"
 #include "Actor.cpp"
 #include "Blueprint.cpp"
 #include "Client.cpp"
 #include "Control.cpp"
 #include "Exception.cpp"
-#include "Geom.cpp"
 #include "Map.cpp"
 #include "Sensor.cpp"
 #include "SensorData.cpp"