From 744a2d47e0c03ef2e22d3c43432454f5c7291886 Mon Sep 17 00:00:00 2001 From: Marc Garcia Puig Date: Fri, 7 Feb 2020 12:44:21 +0100 Subject: [PATCH] Fixed style --- CHANGELOG.md | 1 + LibCarla/cmake/client/CMakeLists.txt | 6 +-- LibCarla/source/carla/geom/CubicPolynomial.h | 50 ++++++++--------- LibCarla/source/carla/geom/Math.cpp | 32 +++++------ LibCarla/source/carla/geom/Math.h | 1 + LibCarla/source/carla/geom/Rtree.h | 57 +++++++++++--------- LibCarla/source/carla/road/Map.h | 21 +++++--- Util/BuildTools/Setup.sh | 8 +-- 8 files changed, 97 insertions(+), 79 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index ad0f71ea5..14c5beff1 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -4,6 +4,7 @@ * Vehicles get destroyed when they are stuck in Traffic Manager * Implemented intersection anticipation algorithm in Traffic Manager * Added support for new geometry: `spiral`, `poly3`, and `paramPoly3` + * Improved `get_waypoint(location)` performance * New weather system: night time, fog, rain ripples, and now wind affects vegetation and rain (not car physics) * Fixed Low/Epic quality settings transition * Enabled Mesh distance fields diff --git a/LibCarla/cmake/client/CMakeLists.txt b/LibCarla/cmake/client/CMakeLists.txt index 38312a2a4..a18c69645 100644 --- a/LibCarla/cmake/client/CMakeLists.txt +++ b/LibCarla/cmake/client/CMakeLists.txt @@ -198,11 +198,11 @@ file(GLOB libcarla_cephes_sources set(libcarla_sources "${libcarla_sources};${libcarla_cephes_sources}") install(FILES ${libcarla_cephes_sources} DESTINATION include/cephes) -file(GLOB libcarla_odrSpiral_sources +file(GLOB libcarla_odr_spiral_sources "${libcarla_source_thirdparty_path}/odrSpiral/*.cpp" "${libcarla_source_thirdparty_path}/odrSpiral/*.h") -set(libcarla_sources "${libcarla_sources};${libcarla_odrSpiral_sources}") -install(FILES ${libcarla_odrSpiral_sources} DESTINATION include/odrSpiral) +set(libcarla_sources "${libcarla_sources};${libcarla_odr_spiral_sources}") +install(FILES ${libcarla_odr_spiral_sources} DESTINATION include/odrSpiral) file(GLOB libcarla_moodycamel_sources "${libcarla_source_thirdparty_path}/moodycamel/*.h") diff --git a/LibCarla/source/carla/geom/CubicPolynomial.h b/LibCarla/source/carla/geom/CubicPolynomial.h index 1e1fb6b9d..72951c074 100644 --- a/LibCarla/source/carla/geom/CubicPolynomial.h +++ b/LibCarla/source/carla/geom/CubicPolynomial.h @@ -29,24 +29,24 @@ namespace geom { CubicPolynomial(const CubicPolynomial &) = default; CubicPolynomial( - const value_type &a, - const value_type &b, - const value_type &c, - const value_type &d) + const value_type &a, + const value_type &b, + const value_type &c, + const value_type &d) : _v{ {a, b, c, d} }, - _s(0.0) {} + _s(0.0) {} CubicPolynomial( - const value_type &a, - const value_type &b, - const value_type &c, - const value_type &d, - const value_type &s) // lateral offset + const value_type &a, + const value_type &b, + const value_type &c, + const value_type &d, + const value_type &s) // lateral offset : _v{ {a - b * s + c * s * s - d * s * s * s, - b - 2 * c * s + 3 * d * s * s, - c - 3 * d * s, - d} }, - _s(s) {} + b - 2 * c * s + 3 * d * s * s, + c - 3 * d * s, + d} }, + _s(s) {} // ========================================================================= // -- Getters -------------------------------------------------------------- @@ -77,11 +77,11 @@ namespace geom { // ========================================================================= void Set( - const value_type &a, - const value_type &b, - const value_type &c, - const value_type &d, - const value_type &s) { // lateral offset + const value_type &a, + const value_type &b, + const value_type &c, + const value_type &d, + const value_type &s) { // lateral offset _v = { a - b * s + c * s * s - d * s * s * s, b - 2 * c * s + 3 * d * s * s, c - 3 * d * s, @@ -90,12 +90,12 @@ namespace geom { } void Set( - const value_type &a, - const value_type &b, - const value_type &c, - const value_type &d) { - _v = {{a, b, c, d}}; - _s = (0.0); + const value_type &a, + const value_type &b, + const value_type &c, + const value_type &d) { + _v = {a, b, c, d}; + _s = 0.0; } // ========================================================================= diff --git a/LibCarla/source/carla/geom/Math.cpp b/LibCarla/source/carla/geom/Math.cpp index dbd0a306e..85d367775 100644 --- a/LibCarla/source/carla/geom/Math.cpp +++ b/LibCarla/source/carla/geom/Math.cpp @@ -16,9 +16,9 @@ namespace geom { } std::pair Math::DistanceSegmentToPoint( - const Vector3D &p, - const Vector3D &v, - const Vector3D &w) { + const Vector3D &p, + const Vector3D &v, + const Vector3D &w) { const float l2 = DistanceSquared2D(v, w); const float l = std::sqrt(l2); if (l2 == 0.0f) { @@ -31,11 +31,11 @@ namespace geom { } std::pair Math::DistanceArcToPoint( - Vector3D p, - Vector3D start_pos, - const float length, - float heading, // [radians] - float curvature) { + Vector3D p, + Vector3D start_pos, + const float length, + float heading, // [radians] + float curvature) { /// @todo: Because Unreal's coordinates, hacky way to correct /// the -y, this must be changed in the future @@ -91,21 +91,21 @@ namespace geom { DEBUG_ASSERT(angle >= 0.0f); if (angle <= last_point_angle) { return std::make_pair( - angle * radius, - Distance2D(intersection, rotated_p)); + angle * radius, + Distance2D(intersection, rotated_p)); } // find the nearest point, start or end to intersection const float start_dist = Distance2D(Vector3D(), rotated_p); const Vector3D end_pos( - radius * std::cos(last_point_angle - pi_half), - radius * std::sin(last_point_angle - pi_half) + circ_center.y, - 0.0f); + radius * std::cos(last_point_angle - pi_half), + radius * std::sin(last_point_angle - pi_half) + circ_center.y, + 0.0f); const float end_dist = Distance2D(end_pos, rotated_p); return (start_dist < end_dist) ? - std::make_pair(0.0f, start_dist) : - std::make_pair(length, end_dist); + std::make_pair(0.0f, start_dist) : + std::make_pair(length, end_dist); } Vector3D Math::RotatePointOnOrigin2D(Vector3D p, float angle) { @@ -119,7 +119,7 @@ namespace geom { const float sp = std::sin(ToRadians(rotation.pitch)); const float cy = std::cos(ToRadians(rotation.yaw)); const float sy = std::sin(ToRadians(rotation.yaw)); - return {cy *cp, sy *cp, sp}; + return {cy * cp, sy * cp, sp}; } } // namespace geom diff --git a/LibCarla/source/carla/geom/Math.h b/LibCarla/source/carla/geom/Math.h index 765bf7841..745644e03 100644 --- a/LibCarla/source/carla/geom/Math.h +++ b/LibCarla/source/carla/geom/Math.h @@ -79,6 +79,7 @@ namespace geom { return std::sqrt(DistanceSquared2D(a, b)); } + /// Returns the angle between 2 vectors in radians static double GetVectorAngle(const Vector3D &a, const Vector3D &b); /// Returns a pair containing: diff --git a/LibCarla/source/carla/geom/Rtree.h b/LibCarla/source/carla/geom/Rtree.h index a24f2a2f8..e5d4dbc3f 100644 --- a/LibCarla/source/carla/geom/Rtree.h +++ b/LibCarla/source/carla/geom/Rtree.h @@ -1,4 +1,4 @@ -// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma +// Copyright (c) 2020 Computer Vision Center (CVC) at the Universitat Autonoma // de Barcelona (UAB). // // This work is licensed under the terms of the MIT license. @@ -15,9 +15,9 @@ namespace carla { namespace geom { - // Rtree class working with 3D point clouds. - // Asociates a T element with a 3D point - // Useful to perform fast k-NN searches + /// Rtree class working with 3D point clouds. + /// Asociates a T element with a 3D point + /// Useful to perform fast k-NN searches template class PointCloudRtree { public: @@ -36,11 +36,12 @@ namespace geom { void InsertElements(const std::vector &elements) { _rtree.insert(elements.begin(), elements.end()); } - // Return nearest neighbors with a user defined filter. - // The filter reveices as an argument a TreeElement value and needs to - // return a bool to accept or reject the value - // [&](Rtree::TreeElement const &element){if (IsOk(element)) return true; - // else return false;} + + /// Return nearest neighbors with a user defined filter. + /// The filter reveices as an argument a TreeElement value and needs to + /// return a bool to accept or reject the value + /// [&](Rtree::TreeElement const &element){if (IsOk(element)) return true; + /// else return false;} template std::vector GetNearestNeighboursWithFilter(const BPoint &point, Filter filter, size_t number_neighbours = 1) const { @@ -50,12 +51,14 @@ namespace geom { std::back_inserter(query_result)); return query_result; } + std::vector GetNearestNeighbours(const BPoint &point, size_t number_neighbours = 1) const { std::vector query_result; _rtree.query(boost::geometry::index::nearest(point, static_cast(number_neighbours)), std::back_inserter(query_result)); return query_result; } + size_t GetTreeSize() const { return _rtree.size(); } @@ -66,9 +69,9 @@ namespace geom { }; - // Rtree class working with 3D segment clouds. - // Stores a pair of T elements (one for each end of the segment) - // Useful to perform fast k-NN searches. + /// Rtree class working with 3D segment clouds. + /// Stores a pair of T elements (one for each end of the segment) + /// Useful to perform fast k-NN searches. template class SegmentCloudRtree { public: @@ -80,6 +83,7 @@ namespace geom { void InsertElement(const BSegment &segment, const T &element_start, const T &element_end) { _rtree.insert(std::make_pair(segment, std::make_pair(element_start, element_end))); } + void InsertElement(const TreeElement &element) { _rtree.insert(element); } @@ -87,25 +91,30 @@ namespace geom { void InsertElements(const std::vector &elements) { _rtree.insert(elements.begin(), elements.end()); } - // Return nearest neighbors with a user defined filter. - // The filter reveices as an argument a TreeElement value and needs to - // return a bool to accept or reject the value - // [&](Rtree::TreeElement const &element){if (IsOk(element)) return true; - // else return false;} + + /// Return nearest neighbors with a user defined filter. + /// The filter reveices as an argument a TreeElement value and needs to + /// return a bool to accept or reject the value + /// [&](Rtree::TreeElement const &element){if (IsOk(element)) return true; + /// else return false;} template - std::vector GetNearestNeighboursWithFilter(const BPoint &point, Filter filter, + std::vector GetNearestNeighboursWithFilter( + const BPoint &point, + Filter filter, size_t number_neighbours = 1) const { std::vector query_result; - _rtree.query(boost::geometry::index::nearest(point, - static_cast(number_neighbours)) && boost::geometry::index::satisfies(filter), - std::back_inserter(query_result)); + _rtree.query( + boost::geometry::index::nearest(point, static_cast(number_neighbours)) && + boost::geometry::index::satisfies(filter), + std::back_inserter(query_result)); return query_result; } + std::vector GetNearestNeighbours(const BPoint &point, size_t number_neighbours = 1) const { std::vector query_result; - _rtree.query(boost::geometry::index::nearest(point, - static_cast(number_neighbours)), - std::back_inserter(query_result)); + _rtree.query( + boost::geometry::index::nearest(point, static_cast(number_neighbours)), + std::back_inserter(query_result)); return query_result; } diff --git a/LibCarla/source/carla/road/Map.h b/LibCarla/source/carla/road/Map.h index d841504b1..497ba06f7 100644 --- a/LibCarla/source/carla/road/Map.h +++ b/LibCarla/source/carla/road/Map.h @@ -140,13 +140,20 @@ private: Rtree _rtree; void CreateRtree(); - //Helper Functions for constructing the rtree element list - void AddElementToRtree(std::vector &rtree_elements, - geom::Transform ¤t_transform, geom::Transform &next_transform, - Waypoint ¤t_waypoint, Waypoint &next_waypoint); - void AddElementToRtreeAndUpdateTransforms(std::vector &rtree_elements, - geom::Transform ¤t_transform, Waypoint ¤t_waypoint, - Waypoint &next_waypoint); + + /// Helper Functions for constructing the rtree element list + void AddElementToRtree( + std::vector &rtree_elements, + geom::Transform ¤t_transform, + geom::Transform &next_transform, + Waypoint ¤t_waypoint, + Waypoint &next_waypoint); + + void AddElementToRtreeAndUpdateTransforms( + std::vector &rtree_elements, + geom::Transform ¤t_transform, + Waypoint ¤t_waypoint, + Waypoint &next_waypoint); }; } // namespace road diff --git a/Util/BuildTools/Setup.sh b/Util/BuildTools/Setup.sh index 0e98d27f8..202da873d 100755 --- a/Util/BuildTools/Setup.sh +++ b/Util/BuildTools/Setup.sh @@ -91,8 +91,8 @@ else mkdir -p ${BOOST_BASENAME}-install/include mv ${BOOST_PACKAGE_BASENAME} ${BOOST_BASENAME}-source # Boost patch for exception handling - cp ${CARLA_BUILD_FOLDER}/../Util/BoostFiles/rational.hpp ${BOOST_BASENAME}-source/boost/rational.hpp - cp ${CARLA_BUILD_FOLDER}/../Util/BoostFiles/read.hpp ${BOOST_BASENAME}-source/boost/geometry/io/wkt/read.hpp + cp "${CARLA_BUILD_FOLDER}/../Util/BoostFiles/rational.hpp ${BOOST_BASENAME}-source/boost/rational.hpp" + cp "${CARLA_BUILD_FOLDER}/../Util/BoostFiles/read.hpp ${BOOST_BASENAME}-source/boost/geometry/io/wkt/read.hpp" # --- pushd ${BOOST_BASENAME}-source >/dev/null @@ -160,8 +160,8 @@ else fi # Boost patch for exception handling -cp ${CARLA_BUILD_FOLDER}/../Util/BoostFiles/rational.hpp ${BOOST_BASENAME}-install/include/boost/rational.hpp -cp ${CARLA_BUILD_FOLDER}/../Util/BoostFiles/read.hpp ${BOOST_BASENAME}-install/include/boost/geometry/io/wkt/read.hpp +cp "${CARLA_BUILD_FOLDER}/../Util/BoostFiles/rational.hpp ${BOOST_BASENAME}-install/include/boost/rational.hpp" +cp "${CARLA_BUILD_FOLDER}/../Util/BoostFiles/read.hpp ${BOOST_BASENAME}-install/include/boost/geometry/io/wkt/read.hpp" # --- unset BOOST_BASENAME