diff --git a/LibCarla/source/carla/geom/Mesh.cpp b/LibCarla/source/carla/geom/Mesh.cpp index 5f192f770..c30918a95 100644 --- a/LibCarla/source/carla/geom/Mesh.cpp +++ b/LibCarla/source/carla/geom/Mesh.cpp @@ -9,6 +9,8 @@ #include #include #include +#include +#include #include @@ -266,6 +268,9 @@ namespace geom { return _indexes; } + std::vector& Mesh::GetIndexes() { + return _indexes; + } size_t Mesh::GetIndexesNum() const { return _indexes.size(); } @@ -282,6 +287,62 @@ namespace geom { return _vertices.size(); } + Mesh& Mesh::ConcatMesh(const Mesh& rhs, int num_vertices_to_link) { + + if (!rhs.IsValid()) + { + return *this += rhs; + } + const size_t v_num = GetVerticesNum(); + const size_t i_num = GetIndexesNum(); + + _vertices.insert( + _vertices.end(), + rhs.GetVertices().begin(), + rhs.GetVertices().end()); + + _normals.insert( + _normals.end(), + rhs.GetNormals().begin(), + rhs.GetNormals().end()); + + const int vertex_to_start_concating = v_num - num_vertices_to_link; + + for( int i = 1; i < num_vertices_to_link; ++i ) + { + _indexes.push_back( vertex_to_start_concating + i ); + _indexes.push_back( vertex_to_start_concating + i + 1 ); + _indexes.push_back( v_num + i ); + + _indexes.push_back( vertex_to_start_concating + i + 1); + _indexes.push_back( v_num + i + 1); + _indexes.push_back( v_num + i); + } + + std::transform( + rhs.GetIndexes().begin(), + rhs.GetIndexes().end(), + std::back_inserter(_indexes), + [=](size_t index) {return index + v_num; }); + + _uvs.insert( + _uvs.end(), + rhs.GetUVs().begin(), + rhs.GetUVs().end()); + + std::transform( + rhs.GetMaterials().begin(), + rhs.GetMaterials().end(), + std::back_inserter(_materials), + [=](MeshMaterial mat) { + mat.index_start += i_num; + mat.index_end += i_num; + return mat; + }); + + return *this; + } + Mesh &Mesh::operator+=(const Mesh &rhs) { const size_t v_num = GetVerticesNum(); const size_t i_num = GetIndexesNum(); diff --git a/LibCarla/source/carla/geom/Mesh.h b/LibCarla/source/carla/geom/Mesh.h index e5882c760..326b0d561 100644 --- a/LibCarla/source/carla/geom/Mesh.h +++ b/LibCarla/source/carla/geom/Mesh.h @@ -131,7 +131,9 @@ namespace geom { const std::vector &GetNormals() const; - const std::vector &GetIndexes() const; + const std::vector& GetIndexes() const; + + std::vector &GetIndexes(); size_t GetIndexesNum() const; @@ -142,6 +144,9 @@ namespace geom { /// Returns the index of the last added vertex (number of vertices). size_t GetLastVertexIndex() const; + /// Merges two meshes into a single mesh + Mesh& ConcatMesh(const Mesh& rhs, int num_vertices_to_link); + /// Merges two meshes into a single mesh Mesh &operator+=(const Mesh &rhs); diff --git a/LibCarla/source/carla/road/Map.cpp b/LibCarla/source/carla/road/Map.cpp index d0abdc74d..9a937bb02 100644 --- a/LibCarla/source/carla/road/Map.cpp +++ b/LibCarla/source/carla/road/Map.cpp @@ -7,6 +7,7 @@ #include "carla/road/Map.h" #include "carla/Exception.h" #include "carla/geom/Math.h" +#include "carla/geom/Vector3D.h" #include "carla/road/MeshFactory.h" #include "carla/road/element/LaneCrossingCalculator.h" #include "carla/road/element/RoadInfoCrosswalk.h" @@ -17,6 +18,8 @@ #include "carla/road/element/RoadInfoMarkRecord.h" #include "carla/road/element/RoadInfoSignal.h" +#include "simplify/Simplify.h" + #include #include #include @@ -1040,6 +1043,7 @@ namespace road { return out_mesh; } + std::vector> Map::GenerateChunkedMesh( const rpc::OpendriveGenerationParameters& params) const { geom::MeshFactory mesh_factory(params); @@ -1124,6 +1128,161 @@ namespace road { return result; } + std::map>> + Map::GenerateOrderedChunkedMesh( const rpc::OpendriveGenerationParameters& params) const + { + + geom::MeshFactory mesh_factory(params); + std::map>> out_mesh_list; + std::unordered_map junction_map; + + float simplificationrate = params.simplification_percentage * 0.01f; + for (auto &&pair : _data.GetRoads()) + { + const auto &road = pair.second; + if (!road.IsJunction()) { + mesh_factory.GenerateAllOrderedWithMaxLen(road, out_mesh_list); + } + } + + for (auto& current_mesh_vector : out_mesh_list) + { + if (current_mesh_vector.first != road::Lane::LaneType::Driving) + { + continue; + } + + for (std::unique_ptr& current_mesh : current_mesh_vector.second) + { + + if (!current_mesh->IsValid()) + { + continue; + } + + for (carla::geom::Vector3D& current_vertex : current_mesh->GetVertices()) + { + current_vertex.z = GetZPosInDeformation(current_vertex.x, current_vertex.y); + } + + for (carla::geom::Vector3D& current_vertex : current_mesh->GetVertices()) + { + Simplify::Vertex v; + v.p.x = current_vertex.x; + v.p.y = current_vertex.y; + v.p.z = current_vertex.z; + Simplify::vertices.push_back(v); + } + + for (int i = 0; i < current_mesh->GetIndexes().size() - 2; i += 3) + { + Simplify::Triangle t; + t.material = 0; + auto indices = current_mesh->GetIndexes(); + t.v[0] = (indices[i]) - 1; + t.v[1] = (indices[i + 1]) - 1; + t.v[2] = (indices[i + 2]) - 1; + + if (i >= current_mesh->GetIndexes().size()) { + std::cout << "Not right number of Indexes Index: " << i << " Indices size: " << current_mesh->GetIndexes().size() << std::endl; + } + + Simplify::triangles.push_back(t); + } + + // Reduce to the X% of the polys + float target_size = Simplify::triangles.size(); + Simplify::simplify_mesh( (target_size * simplificationrate) ); + + current_mesh->GetVertices().clear(); + current_mesh->GetIndexes().clear(); + for (Simplify::Vertex& current_vertex : Simplify::vertices) + { + carla::geom::Vector3D v; + v.x = current_vertex.p.x; + v.y = current_vertex.p.y; + v.z = current_vertex.p.z; + current_mesh->AddVertex(v); + } + + for (int i = 0; i < Simplify::triangles.size(); ++i) + { + for (int j = 0; j < 3; ++j) { + current_mesh->GetIndexes().push_back((Simplify::triangles[i].v[j]) + 1); + } + } + Simplify::vertices.clear(); + Simplify::triangles.clear(); + } + } + + + // Generate roads within junctions and smooth them + for (const auto &junc_pair : _data.GetJunctions()) { + const auto &junction = junc_pair.second; + std::vector> lane_meshes; + std::vector> sidewalk_lane_meshes; + for(const auto &connection_pair : junction.GetConnections()) { + const auto &connection = connection_pair.second; + const auto &road = _data.GetRoads().at(connection.connecting_road); + for (auto &&lane_section : road.GetLaneSections()) { + for (auto &&lane_pair : lane_section.GetLanes()) { + const auto &lane = lane_pair.second; + if (lane.GetType() != road::Lane::LaneType::Sidewalk) { + lane_meshes.push_back(mesh_factory.Generate(lane)); + } else { + sidewalk_lane_meshes.push_back(mesh_factory.Generate(lane)); + } + } + } + } + + if(params.smooth_junctions) { + std::unique_ptr merged_mesh = mesh_factory.MergeAndSmooth(lane_meshes); + std::unique_ptr sidewalk_mesh = std::make_unique(); + for(auto& lane : sidewalk_lane_meshes) { + *sidewalk_mesh += *lane; + } + + for (carla::geom::Vector3D& current_vertex : merged_mesh->GetVertices()) + { + current_vertex.z = GetZPosInDeformation(current_vertex.x, current_vertex.y); + } + for (carla::geom::Vector3D& current_vertex : sidewalk_mesh->GetVertices()) + { + current_vertex.z = GetZPosInDeformation(current_vertex.x, current_vertex.y); + } + + out_mesh_list[road::Lane::LaneType::Driving].push_back(std::move(merged_mesh)); + out_mesh_list[road::Lane::LaneType::Sidewalk].push_back(std::move(sidewalk_mesh)); + } else { + std::unique_ptr junction_mesh = std::make_unique(); + std::unique_ptr sidewalk_mesh = std::make_unique(); + for(auto& lane : lane_meshes) { + *junction_mesh += *lane; + } + for(auto& lane : sidewalk_lane_meshes) { + *sidewalk_mesh += *lane; + } + + for (carla::geom::Vector3D& current_vertex : junction_mesh->GetVertices()) + { + current_vertex.z = GetZPosInDeformation(current_vertex.x, current_vertex.y); + } + for (carla::geom::Vector3D& current_vertex : sidewalk_mesh->GetVertices()) + { + current_vertex.z = GetZPosInDeformation(current_vertex.x, current_vertex.y); + } + out_mesh_list[road::Lane::LaneType::Driving].push_back(std::move(junction_mesh)); + out_mesh_list[road::Lane::LaneType::Sidewalk].push_back(std::move(sidewalk_mesh)); + } + } + + + return out_mesh_list; + } + + geom::Mesh Map::GetAllCrosswalkMesh() const { geom::Mesh out_mesh; @@ -1160,5 +1319,24 @@ namespace road { return out_mesh; } + float Map::GetZPosInDeformation(float posx, float posy) const + { + // Amplitud + const float A1 = 0.3f; + const float A2 = 0.5f; + // Fases + const float F1 = 100.0; + const float F2 = -1500.0; + // Modifiers + const float Kx1 = 0.035f; + const float Kx2 = 0.02f; + + const float Ky1 = -0.08f; + const float Ky2 = 0.05f; + + return A1 * sin((Kx1 * posx + Ky1 * posy + F1)) + + A2 * sin((Kx2 * posx + Ky2 * posy + F2)); + } + } // namespace road } // namespace carla diff --git a/LibCarla/source/carla/road/Map.h b/LibCarla/source/carla/road/Map.h index 680ad731e..c27f311ae 100644 --- a/LibCarla/source/carla/road/Map.h +++ b/LibCarla/source/carla/road/Map.h @@ -159,6 +159,9 @@ namespace road { std::vector> GenerateChunkedMesh( const rpc::OpendriveGenerationParameters& params) const; + std::map>> + GenerateOrderedChunkedMesh( const rpc::OpendriveGenerationParameters& params) const; + /// Buids a mesh of all crosswalks based on the OpenDRIVE geom::Mesh GetAllCrosswalkMesh() const; @@ -201,6 +204,8 @@ private: geom::Transform ¤t_transform, Waypoint ¤t_waypoint, Waypoint &next_waypoint); + + float GetZPosInDeformation(float posx, float posy) const; }; } // namespace road diff --git a/LibCarla/source/carla/road/MeshFactory.cpp b/LibCarla/source/carla/road/MeshFactory.cpp index c8f665adc..1c21c4355 100644 --- a/LibCarla/source/carla/road/MeshFactory.cpp +++ b/LibCarla/source/carla/road/MeshFactory.cpp @@ -19,6 +19,7 @@ namespace geom { road_param.max_road_len = static_cast(params.max_road_length); road_param.extra_lane_width = static_cast(params.additional_width); road_param.wall_height = static_cast(params.wall_height); + road_param.vertex_width_resolution = static_cast(params.vertex_width_resolution); } /// We use this epsilon to shift the waypoints away from the edges of the lane @@ -48,6 +49,12 @@ namespace geom { return Generate(lane, s_start, s_end); } + std::unique_ptr MeshFactory::GenerateTesselated(const road::Lane& lane) const { + const double s_start = lane.GetDistance() + EPSILON; + const double s_end = lane.GetDistance() + lane.GetLength() - EPSILON; + return GenerateTesselated(lane, s_start, s_end); + } + std::unique_ptr MeshFactory::Generate( const road::Lane &lane, const double s_start, const double s_end) const { RELEASE_ASSERT(road_param.resolution > 0.0); @@ -63,13 +70,16 @@ namespace geom { double s_current = s_start; std::vector vertices; + if (lane.IsStraight()) { // Mesh optimization: If the lane is straight just add vertices at the // begining and at the end of it const auto edges = lane.GetCornerPositions(s_current, road_param.extra_lane_width); vertices.push_back(edges.first); vertices.push_back(edges.second); - } else { + } + else + { // Iterate over the lane's 's' and store the vertices based on it's width do { // Get the location of the edges of the current lane at the current waypoint @@ -98,6 +108,103 @@ namespace geom { return std::make_unique(out_mesh); } + std::unique_ptr MeshFactory::GenerateTesselated( + const road::Lane& lane, const double s_start, const double s_end) const { + RELEASE_ASSERT(road_param.resolution > 0.0); + DEBUG_ASSERT(s_start >= 0.0); + DEBUG_ASSERT(s_end <= lane.GetDistance() + lane.GetLength()); + DEBUG_ASSERT(s_end >= EPSILON); + DEBUG_ASSERT(s_start < s_end); + // The lane with lane_id 0 have no physical representation in OpenDRIVE + Mesh out_mesh; + if (lane.GetId() == 0) { + return std::make_unique(out_mesh); + } + double s_current = s_start; + + std::vector vertices; + // Ensure minimum vertices in width are two + const int vertices_in_width = road_param.vertex_width_resolution >= 2 ? road_param.vertex_width_resolution : 2; + const int segments_number = vertices_in_width - 1; + + // Iterate over the lane's 's' and store the vertices based on it's width + do { + // Get the location of the edges of the current lane at the current waypoint + std::pair edges = lane.GetCornerPositions(s_current, road_param.extra_lane_width); + const geom::Vector3D segments_size = ( edges.second - edges.first ) / segments_number; + geom::Vector3D current_vertex = edges.first; + + for (int i = 0; i < vertices_in_width; ++i) + { + vertices.push_back(current_vertex); + current_vertex = current_vertex + segments_size; + } + // Update the current waypoint's "s" + s_current += road_param.resolution; + } while (s_current < s_end); + + // This ensures the mesh is constant and have no gaps between roads, + // adding geometry at the very end of the lane + + if (s_end - (s_current - road_param.resolution) > EPSILON) { + const auto edges = lane.GetCornerPositions(s_end - MESH_EPSILON, road_param.extra_lane_width); + const geom::Vector3D segments_size = (edges.second - edges.first) / segments_number; + geom::Vector3D current_vertex = edges.first; + + for (int i = 0; i < vertices_in_width; ++i) + { + vertices.push_back(current_vertex); + current_vertex = current_vertex + segments_size; + } + } + + out_mesh.AddVertices(vertices); + + // Add the adient material, create the strip and close the material + out_mesh.AddMaterial( + lane.GetType() == road::Lane::LaneType::Sidewalk ? "sidewalk" : "road"); + + const int number_of_rows = (vertices.size() / vertices_in_width); + + for (int i = 0; i < (number_of_rows - 1); ++i) + { + for (int j = 0; j < vertices_in_width - 1; ++j) + { + out_mesh.AddIndex( j + ( i * vertices_in_width ) + 1); + out_mesh.AddIndex( ( j + 1 ) + ( i * vertices_in_width ) + 1); + out_mesh.AddIndex( j + ( ( i + 1 ) * vertices_in_width ) + 1); + + out_mesh.AddIndex( ( j + 1 ) + ( i * vertices_in_width ) + 1); + out_mesh.AddIndex( ( j + 1 ) + ( ( i + 1 ) * vertices_in_width ) + 1); + out_mesh.AddIndex( j + ( ( i + 1 ) * vertices_in_width ) + 1); + } + } + + out_mesh.EndMaterial(); + return std::make_unique(out_mesh); + } + + + void MeshFactory::GenerateLaneSectionOrdered( + const road::LaneSection &lane_section, + std::map>>& result) + const + { + const int vertices_in_width = road_param.vertex_width_resolution >= 2 ? road_param.vertex_width_resolution : 2; + for (auto &&lane_pair : lane_section.GetLanes()) + { + Mesh out_mesh = *GenerateTesselated(lane_pair.second); + if( result[lane_pair.second.GetType()].empty() ) + { + result[lane_pair.second.GetType()].push_back(std::make_unique(out_mesh)); + } + else + { + (result[lane_pair.second.GetType()][0])->ConcatMesh(out_mesh, vertices_in_width); + } + } + } + std::unique_ptr MeshFactory::GenerateWalls(const road::LaneSection &lane_section) const { Mesh out_mesh; @@ -263,6 +370,62 @@ namespace geom { return mesh_uptr_list; } +std::map>> MeshFactory::GenerateOrderedWithMaxLen( + const road::Road &road) const { + std::map>> mesh_uptr_list; + for (auto &&lane_section : road.GetLaneSections()) { + std::map>> section_uptr_list = GenerateOrderedWithMaxLen(lane_section); + mesh_uptr_list.insert( + std::make_move_iterator(section_uptr_list.begin()), + std::make_move_iterator(section_uptr_list.end())); + } + return mesh_uptr_list; + } + + std::map>> MeshFactory::GenerateOrderedWithMaxLen( + const road::LaneSection &lane_section) const { + const int vertices_in_width = road_param.vertex_width_resolution >= 2 ? road_param.vertex_width_resolution : 2; + + std::map>> mesh_uptr_list; + + if (lane_section.GetLength() < road_param.max_road_len) { + GenerateLaneSectionOrdered(lane_section, mesh_uptr_list); + } else { + double s_current = lane_section.GetDistance() + EPSILON; + const double s_end = lane_section.GetDistance() + lane_section.GetLength() - EPSILON; + while(s_current + road_param.max_road_len < s_end) { + const auto s_until = s_current + road_param.max_road_len; + for (auto &&lane_pair : lane_section.GetLanes()) { + Mesh lane_section_mesh = *GenerateTesselated(lane_pair.second, s_current, s_until); + if( mesh_uptr_list[lane_pair.second.GetType()].empty() ) + { + mesh_uptr_list[lane_pair.second.GetType()].push_back(std::make_unique(lane_section_mesh)); + } + else + { + (mesh_uptr_list[lane_pair.second.GetType()][0])->ConcatMesh(lane_section_mesh, vertices_in_width); + } + + } + s_current = s_until; + } + if (s_end - s_current > EPSILON) { + for (auto &&lane_pair : lane_section.GetLanes()) { + Mesh lane_section_mesh = *GenerateTesselated(lane_pair.second, s_current, s_end); + if( mesh_uptr_list[lane_pair.second.GetType()].empty() ) + { + mesh_uptr_list[lane_pair.second.GetType()].push_back(std::make_unique(lane_section_mesh)); + } + else + { + (mesh_uptr_list[lane_pair.second.GetType()][0])->ConcatMesh(lane_section_mesh, vertices_in_width); + } + } + } + } + return mesh_uptr_list; + } + std::vector> MeshFactory::GenerateWallsWithMaxLen( const road::Road &road) const { std::vector> mesh_uptr_list; @@ -352,6 +515,21 @@ namespace geom { return mesh_uptr_list; } +void MeshFactory::GenerateAllOrderedWithMaxLen( + const road::Road &road, + std::map>>& roads + ) const + { + // Get road meshes + std::map>> result = GenerateOrderedWithMaxLen(road); + for (auto &pair_map : result) + { + std::vector>& origin = roads[pair_map.first]; + std::vector>& source = pair_map.second; + std::move(source.begin(), source.end(), std::back_inserter(origin)); + } + } + struct VertexWeight { Mesh::vertex_type* vertex; double weight; diff --git a/LibCarla/source/carla/road/MeshFactory.h b/LibCarla/source/carla/road/MeshFactory.h index 417302b76..d86debcf1 100644 --- a/LibCarla/source/carla/road/MeshFactory.h +++ b/LibCarla/source/carla/road/MeshFactory.h @@ -41,9 +41,19 @@ namespace geom { std::unique_ptr Generate( const road::Lane &lane, const double s_start, const double s_end) const; + /// Generates a mesh that defines a lane from a given s start and end with bigger tesselation + std::unique_ptr GenerateTesselated( + const road::Lane& lane, const double s_start, const double s_end) const; + /// Generates a mesh that defines the whole lane std::unique_ptr Generate(const road::Lane &lane) const; + /// Generates a mesh that defines the whole lane with bigger tesselation + std::unique_ptr GenerateTesselated(const road::Lane& lane) const; + + /// Generates a mesh that defines a lane section + void GenerateLaneSectionOrdered(const road::LaneSection &lane_section, + std::map>>& result ) const; // -- Walls -- /// Genrates a mesh representing a wall on the road corners to avoid @@ -68,6 +78,14 @@ namespace geom { std::vector> GenerateWithMaxLen( const road::LaneSection &lane_section) const; + /// Generates a list of meshes that defines a road with a maximum length + std::map>> GenerateOrderedWithMaxLen( + const road::Road &road) const; + + /// Generates a list of meshes that defines a lane_section with a maximum length + std::map>> GenerateOrderedWithMaxLen( + const road::LaneSection &lane_section) const; + /// Generates a list of meshes that defines a road safety wall with a maximum length std::vector> GenerateWallsWithMaxLen( const road::Road &road) const; @@ -82,6 +100,10 @@ namespace geom { std::vector> GenerateAllWithMaxLen( const road::Road &road) const; + + void GenerateAllOrderedWithMaxLen(const road::Road &road, + std::map>>& roads) const; + std::unique_ptr MergeAndSmooth(std::vector> &lane_meshes) const; // ========================================================================= @@ -94,6 +116,7 @@ namespace geom { float max_road_len = 50.0f; float extra_lane_width = 1.0f; float wall_height = 0.6f; + float vertex_width_resolution = 4.0f; // Road mesh smoothness: float max_weight_distance = 5.0f; float same_lane_weight_multiplier = 2.0f; diff --git a/LibCarla/source/carla/rpc/OpendriveGenerationParameters.h b/LibCarla/source/carla/rpc/OpendriveGenerationParameters.h index 832842a39..582668425 100644 --- a/LibCarla/source/carla/rpc/OpendriveGenerationParameters.h +++ b/LibCarla/source/carla/rpc/OpendriveGenerationParameters.h @@ -35,6 +35,8 @@ namespace rpc { double max_road_length = 50.0; double wall_height = 1.0; double additional_width = 0.6; + double vertex_width_resolution = 4.0f; + float simplification_percentage = 20.0f; bool smooth_junctions = true; bool enable_mesh_visibility = true; bool enable_pedestrian_navigation = true; diff --git a/LibCarla/source/third-party/simplify/Simplify.h b/LibCarla/source/third-party/simplify/Simplify.h new file mode 100644 index 000000000..410726187 --- /dev/null +++ b/LibCarla/source/third-party/simplify/Simplify.h @@ -0,0 +1,1163 @@ +///////////////////////////////////////////// +// +// Mesh Simplification Tutorial +// +// (C) by Sven Forstmann in 2014 +// +// License : MIT +// http://opensource.org/licenses/MIT +// +// https://github.com/sp4cerat/Fast-Quadric-Mesh-Simplification +// +// 5/2016: Chris Rorden created minimal version for OSX/Linux/Windows compile + +#include +#include +#include + +#include + +#include +#include +#include +#include +#include +#include +#include //FLT_EPSILON, DBL_EPSILON + +#define loopi(start_l, end_l) for (int i = start_l; i < end_l; ++i) +#define loopi(start_l, end_l) for (int i = start_l; i < end_l; ++i) +#define loopj(start_l, end_l) for (int j = start_l; j < end_l; ++j) +#define loopk(start_l, end_l) for (int k = start_l; k < end_l; ++k) + +struct vector3 +{ + double x, y, z; +}; + +struct vec3f +{ + double x, y, z; + + inline vec3f(void) {} + + // inline vec3f operator =( vector3 a ) + // { vec3f b ; b.x = a.x; b.y = a.y; b.z = a.z; return b;} + + inline vec3f(vector3 a) + { + x = a.x; + y = a.y; + z = a.z; + } + + inline vec3f(const double X, const double Y, const double Z) + { + x = X; + y = Y; + z = Z; + } + + inline vec3f operator+(const vec3f &a) const + { + return vec3f(x + a.x, y + a.y, z + a.z); + } + + inline vec3f operator+=(const vec3f &a) const + { + return vec3f(x + a.x, y + a.y, z + a.z); + } + + inline vec3f operator*(const double a) const + { + return vec3f(x * a, y * a, z * a); + } + + inline vec3f operator*(const vec3f a) const + { + return vec3f(x * a.x, y * a.y, z * a.z); + } + + inline vec3f v3() const + { + return vec3f(x, y, z); + } + + inline vec3f operator=(const vector3 a) + { + x = a.x; + y = a.y; + z = a.z; + return *this; + } + + inline vec3f operator=(const vec3f a) + { + x = a.x; + y = a.y; + z = a.z; + return *this; + } + + inline vec3f operator/(const vec3f a) const + { + return vec3f(x / a.x, y / a.y, z / a.z); + } + + inline vec3f operator-(const vec3f &a) const + { + return vec3f(x - a.x, y - a.y, z - a.z); + } + + inline vec3f operator/(const double a) const + { + return vec3f(x / a, y / a, z / a); + } + + inline double dot(const vec3f &a) const + { + return a.x * x + a.y * y + a.z * z; + } + + inline vec3f cross(const vec3f &a, const vec3f &b) + { + x = a.y * b.z - a.z * b.y; + y = a.z * b.x - a.x * b.z; + z = a.x * b.y - a.y * b.x; + return *this; + } + + inline double angle(const vec3f &v) + { + vec3f a = v, b = *this; + double dot = v.x * x + v.y * y + v.z * z; + double len = a.length() * b.length(); + if (len == 0) + len = 0.00001f; + double input = dot / len; + if (input < -1) + input = -1; + if (input > 1) + input = 1; + return (double)acos(input); + } + + inline double angle2(const vec3f &v, const vec3f &w) + { + vec3f a = v, b = *this; + double dot = a.x * b.x + a.y * b.y + a.z * b.z; + double len = a.length() * b.length(); + if (len == 0) + len = 1; + + vec3f plane; + plane.cross(b, w); + + if (plane.x * a.x + plane.y * a.y + plane.z * a.z > 0) + return (double)-acos(dot / len); + + return (double)acos(dot / len); + } + + inline vec3f rot_x(double a) + { + double yy = cos(a) * y + sin(a) * z; + double zz = cos(a) * z - sin(a) * y; + y = yy; + z = zz; + return *this; + } + inline vec3f rot_y(double a) + { + double xx = cos(-a) * x + sin(-a) * z; + double zz = cos(-a) * z - sin(-a) * x; + x = xx; + z = zz; + return *this; + } + inline void clamp(double min, double max) + { + if (x < min) + x = min; + if (y < min) + y = min; + if (z < min) + z = min; + if (x > max) + x = max; + if (y > max) + y = max; + if (z > max) + z = max; + } + inline vec3f rot_z(double a) + { + double yy = cos(a) * y + sin(a) * x; + double xx = cos(a) * x - sin(a) * y; + y = yy; + x = xx; + return *this; + } + inline vec3f invert() + { + x = -x; + y = -y; + z = -z; + return *this; + } + inline vec3f frac() + { + return vec3f( + x - double(int(x)), + y - double(int(y)), + z - double(int(z))); + } + + inline vec3f integer() + { + return vec3f( + double(int(x)), + double(int(y)), + double(int(z))); + } + + inline double length() const + { + return (double)sqrt(x * x + y * y + z * z); + } + + inline vec3f normalize(double desired_length = 1) + { + double square = sqrt(x * x + y * y + z * z); + /* + if (square <= 0.00001f ) + { + x=1;y=0;z=0; + return *this; + }*/ + // double len = desired_length / square; + x /= square; + y /= square; + z /= square; + + return *this; + } + static vec3f normalize(vec3f a); + + static void random_init(); + static double random_double(); + static vec3f random(); + + static int random_number; + + double random_double_01(double a) + { + double rnf = a * 14.434252 + a * 364.2343 + a * 4213.45352 + a * 2341.43255 + a * 254341.43535 + a * 223454341.3523534245 + 23453.423412; + int rni = ((int)rnf) % 100000; + return double(rni) / (100000.0f - 1.0f); + } + + vec3f random01_fxyz() + { + x = (double)random_double_01(x); + y = (double)random_double_01(y); + z = (double)random_double_01(z); + return *this; + } +}; + +vec3f barycentric(const vec3f &p, const vec3f &a, const vec3f &b, const vec3f &c) +{ + vec3f v0 = b - a; + vec3f v1 = c - a; + vec3f v2 = p - a; + double d00 = v0.dot(v0); + double d01 = v0.dot(v1); + double d11 = v1.dot(v1); + double d20 = v2.dot(v0); + double d21 = v2.dot(v1); + double denom = d00 * d11 - d01 * d01; + double v = (d11 * d20 - d01 * d21) / denom; + double w = (d00 * d21 - d01 * d20) / denom; + double u = 1.0 - v - w; + return vec3f(u, v, w); +} + +vec3f interpolate(const vec3f &p, const vec3f &a, const vec3f &b, const vec3f &c, const vec3f attrs[3]) +{ + vec3f bary = barycentric(p, a, b, c); + vec3f out = vec3f(0, 0, 0); + out = out + attrs[0] * bary.x; + out = out + attrs[1] * bary.y; + out = out + attrs[2] * bary.z; + return out; +} + +double min(double v1, double v2) +{ + return fmin(v1, v2); +} + +class SymetricMatrix +{ + +public: + // Constructor + + SymetricMatrix(double c = 0) { loopi(0, 10) m[i] = c; } + + SymetricMatrix(double m11, double m12, double m13, double m14, + double m22, double m23, double m24, + double m33, double m34, + double m44) + { + m[0] = m11; + m[1] = m12; + m[2] = m13; + m[3] = m14; + m[4] = m22; + m[5] = m23; + m[6] = m24; + m[7] = m33; + m[8] = m34; + m[9] = m44; + } + + // Make plane + + SymetricMatrix(double a, double b, double c, double d) + { + m[0] = a * a; + m[1] = a * b; + m[2] = a * c; + m[3] = a * d; + m[4] = b * b; + m[5] = b * c; + m[6] = b * d; + m[7] = c * c; + m[8] = c * d; + m[9] = d * d; + } + + double operator[](int c) const { return m[c]; } + + // Determinant + + double det(int a11, int a12, int a13, + int a21, int a22, int a23, + int a31, int a32, int a33) + { + double det = m[a11] * m[a22] * m[a33] + m[a13] * m[a21] * m[a32] + m[a12] * m[a23] * m[a31] - m[a13] * m[a22] * m[a31] - m[a11] * m[a23] * m[a32] - m[a12] * m[a21] * m[a33]; + return det; + } + + const SymetricMatrix operator+(const SymetricMatrix &n) const + { + return SymetricMatrix(m[0] + n[0], m[1] + n[1], m[2] + n[2], m[3] + n[3], + m[4] + n[4], m[5] + n[5], m[6] + n[6], + m[7] + n[7], m[8] + n[8], + m[9] + n[9]); + } + + SymetricMatrix &operator+=(const SymetricMatrix &n) + { + m[0] += n[0]; + m[1] += n[1]; + m[2] += n[2]; + m[3] += n[3]; + m[4] += n[4]; + m[5] += n[5]; + m[6] += n[6]; + m[7] += n[7]; + m[8] += n[8]; + m[9] += n[9]; + return *this; + } + + double m[10]; +}; +/////////////////////////////////////////// + +namespace Simplify +{ + // Global Variables & Strctures + enum Attributes + { + NONE, + NORMAL = 2, + TEXCOORD = 4, + COLOR = 8 + }; + struct Triangle + { + int v[3]; + double err[4]; + int deleted, dirty, attr; + vec3f n; + vec3f uvs[3]; + int material; + }; + struct Vertex + { + vec3f p; + int tstart, tcount; + SymetricMatrix q; + int border; + }; + struct Ref + { + int tid, tvertex; + }; + std::vector triangles; + std::vector vertices; + std::vector refs; + std::string mtllib; + std::vector materials; + + // Helper functions + + double vertex_error(SymetricMatrix q, double x, double y, double z); + double calculate_error(int id_v1, int id_v2, vec3f &p_result); + bool flipped(vec3f p, int i0, int i1, Vertex &v0, Vertex &v1, std::vector &deleted); + void update_uvs(int i0, const Vertex &v, const vec3f &p, std::vector &deleted); + void update_triangles(int i0, Vertex &v, std::vector &deleted, int &deleted_triangles); + void update_mesh(int iteration); + void compact_mesh(); + // + // Main simplification function + // + // target_count : target nr. of triangles + // agressiveness : sharpness to increase the threshold. + // 5..8 are good numbers + // more iterations yield higher quality + // + + void simplify_mesh(int target_count, double agressiveness = 7, bool verbose = false) + { + // init + loopi(0, triangles.size()) + { + triangles[i].deleted = 0; + } + + // main iteration loop + int deleted_triangles = 0; + std::vector deleted0, deleted1; + int triangle_count = triangles.size(); + // int iteration = 0; + // loop(iteration,0,100) + for (int iteration = 0; iteration < 100; iteration++) + { + if (triangle_count - deleted_triangles <= target_count) + break; + + // update mesh once in a while + if (iteration % 5 == 0) + { + update_mesh(iteration); + } + + // clear dirty flag + loopi(0, triangles.size()) triangles[i].dirty = 0; + + // + // All triangles with edges below the threshold will be removed + // + // The following numbers works well for most models. + // If it does not, try to adjust the 3 parameters + // + double threshold = 0.000000001 * pow(double(iteration + 3), agressiveness); + + // target number of triangles reached ? Then break + if ((verbose) && (iteration % 5 == 0)) + { + printf("iteration %d - triangles %d threshold %g\n", iteration, triangle_count - deleted_triangles, threshold); + } + + // remove vertices & mark deleted triangles + loopi(0, triangles.size()) + { + Triangle &t = triangles[i]; + if (t.err[3] > threshold) + continue; + if (t.deleted) + continue; + if (t.dirty) + continue; + + loopj(0, 3) if (t.err[j] < threshold) + { + + int i0 = t.v[j]; + Vertex &v0 = vertices[i0]; + int i1 = t.v[(j + 1) % 3]; + Vertex &v1 = vertices[i1]; + // Border check + if (v0.border || v1.border) + continue; + + // Compute vertex to collapse to + vec3f p; + calculate_error(i0, i1, p); + deleted0.resize(v0.tcount); // normals temporarily + deleted1.resize(v1.tcount); // normals temporarily + // don't remove if flipped + if (flipped(p, i0, i1, v0, v1, deleted0)) + continue; + + if (flipped(p, i1, i0, v1, v0, deleted1)) + continue; + + if ((t.attr & TEXCOORD) == TEXCOORD) + { + update_uvs(i0, v0, p, deleted0); + update_uvs(i0, v1, p, deleted1); + } + + // not flipped, so remove edge + v0.p = p; + v0.q = v1.q + v0.q; + int tstart = refs.size(); + + update_triangles(i0, v0, deleted0, deleted_triangles); + update_triangles(i0, v1, deleted1, deleted_triangles); + + int tcount = refs.size() - tstart; + + if (tcount <= v0.tcount) + { + // save ram + if (tcount) + memcpy(&refs[v0.tstart], &refs[tstart], tcount * sizeof(Ref)); + } + else + // append + v0.tstart = tstart; + + v0.tcount = tcount; + break; + } + // done? + if (triangle_count - deleted_triangles <= target_count) + break; + } + } + // clean up mesh + compact_mesh(); + } // simplify_mesh() + + void simplify_mesh_lossless(bool verbose = false) + { + // init + loopi(0, triangles.size()) triangles[i].deleted = 0; + + // main iteration loop + int deleted_triangles = 0; + std::vector deleted0, deleted1; + int triangle_count = triangles.size(); + // int iteration = 0; + // loop(iteration,0,100) + for (int iteration = 0; iteration < 9999; iteration++) + { + // update mesh constantly + update_mesh(iteration); + // clear dirty flag + loopi(0, triangles.size()) triangles[i].dirty = 0; + // + // All triangles with edges below the threshold will be removed + // + // The following numbers works well for most models. + // If it does not, try to adjust the 3 parameters + // + double threshold = DBL_EPSILON; // 1.0E-3 EPS; + if (verbose) + { + printf("lossless iteration %d\n", iteration); + } + + // remove vertices & mark deleted triangles + loopi(0, triangles.size()) + { + Triangle &t = triangles[i]; + if (t.err[3] > threshold) + continue; + if (t.deleted) + continue; + if (t.dirty) + continue; + + loopj(0, 3) if (t.err[j] < threshold) + { + int i0 = t.v[j]; + Vertex &v0 = vertices[i0]; + int i1 = t.v[(j + 1) % 3]; + Vertex &v1 = vertices[i1]; + + // Border check + if (v0.border != v1.border) + continue; + + // Compute vertex to collapse to + vec3f p; + calculate_error(i0, i1, p); + + deleted0.resize(v0.tcount); // normals temporarily + deleted1.resize(v1.tcount); // normals temporarily + + // don't remove if flipped + if (flipped(p, i0, i1, v0, v1, deleted0)) + continue; + if (flipped(p, i1, i0, v1, v0, deleted1)) + continue; + + if ((t.attr & TEXCOORD) == TEXCOORD) + { + update_uvs(i0, v0, p, deleted0); + update_uvs(i0, v1, p, deleted1); + } + + // not flipped, so remove edge + v0.p = p; + v0.q = v1.q + v0.q; + int tstart = refs.size(); + + update_triangles(i0, v0, deleted0, deleted_triangles); + update_triangles(i0, v1, deleted1, deleted_triangles); + + int tcount = refs.size() - tstart; + + if (tcount <= v0.tcount) + { + // save ram + if (tcount) + memcpy(&refs[v0.tstart], &refs[tstart], tcount * sizeof(Ref)); + } + else + // append + v0.tstart = tstart; + + v0.tcount = tcount; + break; + } + } + if (deleted_triangles <= 0) + break; + deleted_triangles = 0; + } // for each iteration + // clean up mesh + compact_mesh(); + } // simplify_mesh_lossless() + + // Check if a triangle flips when this edge is removed + + bool flipped(vec3f p, int i0, int i1, Vertex &v0, Vertex &v1, std::vector &deleted) + { + + loopk(0, v0.tcount) + { + Triangle &t = triangles[refs[v0.tstart + k].tid]; + if (t.deleted) + continue; + + int s = refs[v0.tstart + k].tvertex; + int id1 = t.v[(s + 1) % 3]; + int id2 = t.v[(s + 2) % 3]; + + if (id1 == i1 || id2 == i1) // delete ? + { + + deleted[k] = 1; + continue; + } + vec3f d1 = vertices[id1].p - p; + d1.normalize(); + vec3f d2 = vertices[id2].p - p; + d2.normalize(); + if (fabs(d1.dot(d2)) > 0.999) + return true; + vec3f n; + n.cross(d1, d2); + n.normalize(); + deleted[k] = 0; + if (n.dot(t.n) < 0.2) + return true; + } + return false; + } + + // update_uvs + + void update_uvs(int i0, const Vertex &v, const vec3f &p, std::vector &deleted) + { + loopk(0, v.tcount) + { + Ref &r = refs[v.tstart + k]; + Triangle &t = triangles[r.tid]; + if (t.deleted) + continue; + if (deleted[k]) + continue; + vec3f p1 = vertices[t.v[0]].p; + vec3f p2 = vertices[t.v[1]].p; + vec3f p3 = vertices[t.v[2]].p; + t.uvs[r.tvertex] = interpolate(p, p1, p2, p3, t.uvs); + } + } + + // Update triangle connections and edge error after a edge is collapsed + + void update_triangles(int i0, Vertex &v, std::vector &deleted, int &deleted_triangles) + { + vec3f p; + loopk(0, v.tcount) + { + Ref &r = refs[v.tstart + k]; + Triangle &t = triangles[r.tid]; + if (t.deleted) + continue; + if (deleted[k]) + { + t.deleted = 1; + deleted_triangles++; + continue; + } + t.v[r.tvertex] = i0; + t.dirty = 1; + t.err[0] = calculate_error(t.v[0], t.v[1], p); + t.err[1] = calculate_error(t.v[1], t.v[2], p); + t.err[2] = calculate_error(t.v[2], t.v[0], p); + t.err[3] = min(t.err[0], min(t.err[1], t.err[2])); + refs.push_back(r); + } + } + + // compact triangles, compute edge error and build reference list + + void update_mesh(int iteration) + { + if (iteration > 0) // compact triangles + { + int dst = 0; + loopi(0, triangles.size()) if (!triangles[i].deleted) + { + triangles[dst++] = triangles[i]; + } + triangles.resize(dst); + } + // + + // Init Reference ID list + loopi(0, vertices.size()) + { + vertices[i].tstart = 0; + vertices[i].tcount = 0; + } + + loopi(0, triangles.size()) + { + Triangle &t = triangles[i]; + loopj(0, 3) vertices[t.v[j]].tcount++; + } + int tstart = 0; + loopi(0, vertices.size()) + { + Vertex &v = vertices[i]; + v.tstart = tstart; + tstart += v.tcount; + v.tcount = 0; + } + + // Write References + refs.resize(triangles.size() * 3); + loopi(0, triangles.size()) + { + Triangle &t = triangles[i]; + loopj(0, 3) + { + Vertex &v = vertices[t.v[j]]; + refs[v.tstart + v.tcount].tid = i; + refs[v.tstart + v.tcount].tvertex = j; + v.tcount++; + } + } + + // Init Quadrics by Plane & Edge Errors + // + // required at the beginning ( iteration == 0 ) + // recomputing during the simplification is not required, + // but mostly improves the result for closed meshes + // + if (iteration == 0) + { + // Identify boundary : vertices[].border=0,1 + + std::vector vcount, vids; + + loopi(0, vertices.size()) + vertices[i] + .border = 0; + + loopi(0, vertices.size()) + { + Vertex &v = vertices[i]; + vcount.clear(); + vids.clear(); + loopj(0, v.tcount) + { + int k = refs[v.tstart + j].tid; + Triangle &t = triangles[k]; + loopk(0, 3) + { + int ofs = 0, id = t.v[k]; + while (ofs < vcount.size()) + { + if (vids[ofs] == id) + break; + ofs++; + } + if (ofs == vcount.size()) + { + vcount.push_back(1); + vids.push_back(id); + } + else + vcount[ofs]++; + } + } + loopj(0, vcount.size()) if (vcount[j] == 1) + vertices[vids[j]] + .border = 1; + } + // initialize errors + loopi(0, vertices.size()) + vertices[i] + .q = SymetricMatrix(0.0); + + loopi(0, triangles.size()) + { + Triangle &t = triangles[i]; + vec3f n, p[3]; + loopj(0, 3) p[j] = vertices[t.v[j]].p; + n.cross(p[1] - p[0], p[2] - p[0]); + n.normalize(); + t.n = n; + loopj(0, 3) vertices[t.v[j]].q = + vertices[t.v[j]].q + SymetricMatrix(n.x, n.y, n.z, -n.dot(p[0])); + } + loopi(0, triangles.size()) + { + // Calc Edge Error + Triangle &t = triangles[i]; + vec3f p; + loopj(0, 3) t.err[j] = calculate_error(t.v[j], t.v[(j + 1) % 3], p); + t.err[3] = min(t.err[0], min(t.err[1], t.err[2])); + } + } + } + + // Finally compact mesh before exiting + + void compact_mesh() + { + int dst = 0; + loopi(0, vertices.size()) + { + vertices[i].tcount = 0; + } + loopi(0, triangles.size()) if (!triangles[i].deleted) + { + Triangle &t = triangles[i]; + triangles[dst++] = t; + loopj(0, 3) vertices[t.v[j]].tcount = 1; + } + triangles.resize(dst); + dst = 0; + loopi(0, vertices.size()) if (vertices[i].tcount) + { + vertices[i].tstart = dst; + vertices[dst].p = vertices[i].p; + dst++; + } + loopi(0, triangles.size()) + { + Triangle &t = triangles[i]; + loopj(0, 3) t.v[j] = vertices[t.v[j]].tstart; + } + vertices.resize(dst); + } + + // Error between vertex and Quadric + + double vertex_error(SymetricMatrix q, double x, double y, double z) + { + return q[0] * x * x + 2 * q[1] * x * y + 2 * q[2] * x * z + 2 * q[3] * x + q[4] * y * y + 2 * q[5] * y * z + 2 * q[6] * y + q[7] * z * z + 2 * q[8] * z + q[9]; + } + + // Error for one edge + + double calculate_error(int id_v1, int id_v2, vec3f &p_result) + { + // compute interpolated vertex + + SymetricMatrix q = vertices[id_v1].q + vertices[id_v2].q; + bool border = vertices[id_v1].border & vertices[id_v2].border; + double error = 0; + double det = q.det(0, 1, 2, 1, 4, 5, 2, 5, 7); + if (det != 0 && !border) + { + + // q_delta is invertible + p_result.x = -1 / det * (q.det(1, 2, 3, 4, 5, 6, 5, 7, 8)); // vx = A41/det(q_delta) + p_result.y = 1 / det * (q.det(0, 2, 3, 1, 5, 6, 2, 7, 8)); // vy = A42/det(q_delta) + p_result.z = -1 / det * (q.det(0, 1, 3, 1, 4, 6, 2, 5, 8)); // vz = A43/det(q_delta) + + error = vertex_error(q, p_result.x, p_result.y, p_result.z); + } + else + { + // det = 0 -> try to find best result + vec3f p1 = vertices[id_v1].p; + vec3f p2 = vertices[id_v2].p; + vec3f p3 = (p1 + p2) / 2; + double error1 = vertex_error(q, p1.x, p1.y, p1.z); + double error2 = vertex_error(q, p2.x, p2.y, p2.z); + double error3 = vertex_error(q, p3.x, p3.y, p3.z); + error = min(error1, min(error2, error3)); + if (error1 == error) + p_result = p1; + if (error2 == error) + p_result = p2; + if (error3 == error) + p_result = p3; + } + return error; + } + + char *trimwhitespace(char *str) + { + char *end; + + // Trim leading space + while (isspace((unsigned char)*str)) + str++; + + if (*str == 0) // All spaces? + return str; + + // Trim trailing space + end = str + strlen(str) - 1; + while (end > str && isspace((unsigned char)*end)) + end--; + + // Write new null terminator + *(end + 1) = 0; + + return str; + } + + // Option : Load OBJ + void load_obj(const char *filename, bool process_uv = false) + { + vertices.clear(); + triangles.clear(); + // printf ( "Loading Objects %s ... \n",filename); + FILE *fn; + if (filename == NULL) + return; + if ((char)filename[0] == 0) + return; + if ((fn = fopen(filename, "rb")) == NULL) + { + printf("File %s not found!\n", filename); + return; + } + char line[1000]; + memset(line, 0, 1000); + int vertex_cnt = 0; + int material = -1; + std::map material_map; + std::vector uvs; + std::vector> uvMap; + + while (fgets(line, 1000, fn) != NULL) + { + Vertex v; + vec3f uv; + + if (strncmp(line, "mtllib", 6) == 0) + { + mtllib = trimwhitespace(&line[7]); + } + if (strncmp(line, "usemtl", 6) == 0) + { + std::string usemtl = trimwhitespace(&line[7]); + if (material_map.find(usemtl) == material_map.end()) + { + material_map[usemtl] = materials.size(); + materials.push_back(usemtl); + } + material = material_map[usemtl]; + } + + if (line[0] == 'v' && line[1] == 't') + { + if (line[2] == ' ') + if (sscanf(line, "vt %lf %lf", + &uv.x, &uv.y) == 2) + { + uv.z = 0; + uvs.push_back(uv); + } + else if (sscanf(line, "vt %lf %lf %lf", + &uv.x, &uv.y, &uv.z) == 3) + { + uvs.push_back(uv); + } + } + else if (line[0] == 'v') + { + if (line[1] == ' ') + if (sscanf(line, "v %lf %lf %lf", + &v.p.x, &v.p.y, &v.p.z) == 3) + { + vertices.push_back(v); + } + } + int integers[9]; + if (line[0] == 'f') + { + Triangle t; + bool tri_ok = false; + bool has_uv = false; + + if (sscanf(line, "f %d %d %d", + &integers[0], &integers[1], &integers[2]) == 3) + { + tri_ok = true; + } + else if (sscanf(line, "f %d// %d// %d//", + &integers[0], &integers[1], &integers[2]) == 3) + { + tri_ok = true; + } + else if (sscanf(line, "f %d//%d %d//%d %d//%d", + &integers[0], &integers[3], + &integers[1], &integers[4], + &integers[2], &integers[5]) == 6) + { + tri_ok = true; + } + else if (sscanf(line, "f %d/%d/%d %d/%d/%d %d/%d/%d", + &integers[0], &integers[6], &integers[3], + &integers[1], &integers[7], &integers[4], + &integers[2], &integers[8], &integers[5]) == 9) + { + tri_ok = true; + has_uv = true; + } + else // Add Support for v/vt only meshes + if (sscanf(line, "f %d/%d %d/%d %d/%d", + &integers[0], &integers[6], + &integers[1], &integers[7], + &integers[2], &integers[8]) == 6) + { + tri_ok = true; + has_uv = true; + } + else + { + printf("unrecognized sequence\n"); + printf("%s\n", line); + while (1) + ; + } + if (tri_ok) + { + t.v[0] = integers[0] - 1 - vertex_cnt; + t.v[1] = integers[1] - 1 - vertex_cnt; + t.v[2] = integers[2] - 1 - vertex_cnt; + t.attr = 0; + + if (process_uv && has_uv) + { + std::vector indices; + indices.push_back(integers[6] - 1 - vertex_cnt); + indices.push_back(integers[7] - 1 - vertex_cnt); + indices.push_back(integers[8] - 1 - vertex_cnt); + uvMap.push_back(indices); + t.attr |= TEXCOORD; + } + + t.material = material; + // geo.triangles.push_back ( tri ); + triangles.push_back(t); + // state_before = state; + // state ='f'; + } + } + } + + if (process_uv && uvs.size()) + { + loopi(0, triangles.size()) + { + loopj(0, 3) + triangles[i] + .uvs[j] = uvs[uvMap[i][j]]; + } + } + + fclose(fn); + + // printf("load_obj: vertices = %lu, triangles = %lu, uvs = %lu\n", vertices.size(), triangles.size(), uvs.size() ); + } // load_obj() + + // Optional : Store as OBJ + + void write_obj(const char *filename) + { + FILE *file = fopen(filename, "w"); + int cur_material = -1; + bool has_uv = (triangles.size() && (triangles[0].attr & TEXCOORD) == TEXCOORD); + + if (!file) + { + printf("write_obj: can't write data file \"%s\".\n", filename); + exit(0); + } + if (!mtllib.empty()) + { + fprintf(file, "mtllib %s\n", mtllib.c_str()); + } + loopi(0, vertices.size()) + { + // fprintf(file, "v %lf %lf %lf\n", vertices[i].p.x,vertices[i].p.y,vertices[i].p.z); + fprintf(file, "v %g %g %g\n", vertices[i].p.x, vertices[i].p.y, vertices[i].p.z); // more compact: remove trailing zeros + } + if (has_uv) + { + loopi(0, triangles.size()) if (!triangles[i].deleted) + { + fprintf(file, "vt %g %g\n", triangles[i].uvs[0].x, triangles[i].uvs[0].y); + fprintf(file, "vt %g %g\n", triangles[i].uvs[1].x, triangles[i].uvs[1].y); + fprintf(file, "vt %g %g\n", triangles[i].uvs[2].x, triangles[i].uvs[2].y); + } + } + int uv = 1; + loopi(0, triangles.size()) if (!triangles[i].deleted) + { + if (has_uv) + { + fprintf(file, "f %d/%d %d/%d %d/%d\n", triangles[i].v[0] + 1, uv, triangles[i].v[1] + 1, uv + 1, triangles[i].v[2] + 1, uv + 2); + uv += 3; + } + else + { + fprintf(file, "f %d %d %d\n", triangles[i].v[0] + 1, triangles[i].v[1] + 1, triangles[i].v[2] + 1); + } + // fprintf(file, "f %d// %d// %d//\n", triangles[i].v[0]+1, triangles[i].v[1]+1, triangles[i].v[2]+1); //more compact: remove trailing zeros + } + fclose(file); + } +}; +/////////////////////////////////////////// diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Carla.Build.cs b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Carla.Build.cs index d7685bbe3..78fc0190b 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Carla.Build.cs +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Carla.Build.cs @@ -72,7 +72,8 @@ public class Carla : ModuleRules "RenderCore", "RHI", "Renderer", - "ProceduralMeshComponent" + "ProceduralMeshComponent", + "MeshDescription" // ... add other public dependencies that you statically link with here ... } ); @@ -94,6 +95,8 @@ public class Carla : ModuleRules "CoreUObject", "Engine", "Foliage", + "HTTP", + "StaticMeshDescription", "ImageWriteQueue", "Json", "JsonUtilities", @@ -201,6 +204,13 @@ public class Carla : ModuleRules AddDllDependency(Path.Combine(LibCarlaInstallPath, "dll"), "ChronoModels_robot.dll"); bUseRTTI = true; } + + //OsmToODR + PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", "sqlite3.lib")); + PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", "xerces-c_3.lib")); + PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", "proj.lib")); + PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", "osm2odr.lib")); + PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", "zlibstatic.lib")); } else { @@ -219,6 +229,7 @@ public class Carla : ModuleRules AddDynamicLibrary(Path.Combine(LibCarlaInstallPath, "lib", "libChronoEngine_vehicle.so")); AddDynamicLibrary(Path.Combine(LibCarlaInstallPath, "lib", "libChronoModels_vehicle.so")); AddDynamicLibrary(Path.Combine(LibCarlaInstallPath, "lib", "libChronoModels_robot.so")); + bUseRTTI = true; } @@ -295,7 +306,15 @@ public class Carla : ModuleRules PublicAdditionalLibraries.Add("stdc++"); PublicAdditionalLibraries.Add("/usr/lib/x86_64-linux-gnu/libpython3.9.so"); } - + + + //OsmToODR + PublicAdditionalLibraries.Add("/usr/lib/x86_64-linux-gnu/libc.so"); + PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", "libsqlite3.so")); + PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", "libxerces-c.a")); + PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", "libproj.a")); + PublicAdditionalLibraries.Add(Path.Combine(LibCarlaInstallPath, "lib", "libosm2odr.a")); + } bEnableExceptions = true; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Online/CustomFileDownloader.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Online/CustomFileDownloader.cpp new file mode 100644 index 000000000..f3731cb56 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Online/CustomFileDownloader.cpp @@ -0,0 +1,128 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). This work is licensed under the terms of the MIT license. For a copy, see . + +#undef CreateDirectory + +#include "Online/CustomFileDownloader.h" +#include "HttpModule.h" +#include "Http.h" +#include "Misc/FileHelper.h" + +#include + +void UCustomFileDownloader::ConvertOSMInOpenDrive(FString FilePath) +{ + IPlatformFile &FileManager = FPlatformFileManager::Get().GetPlatformFile(); + + FString FileContent; + // Always first check if the file that you want to manipulate exist. + if (FileManager.FileExists(*FilePath)) + { + // We use the LoadFileToString to load the file into + if (FFileHelper::LoadFileToString(FileContent, *FilePath, FFileHelper::EHashOptions::None)) + { + UE_LOG(LogCarla, Warning, TEXT("FileManipulation: Text From File: %s"), *FilePath); + } + else + { + UE_LOG(LogCarla, Warning, TEXT("FileManipulation: Did not load text from file")); + } + } + else + { + UE_LOG(LogCarla, Warning, TEXT("File: %s does not exist"), *FilePath); + return; + } + + std::string OsmFile = std::string(TCHAR_TO_UTF8(*FileContent)); + std::string OpenDriveFile = osm2odr::ConvertOSMToOpenDRIVE(OsmFile); + + FilePath.RemoveFromEnd(".osm", ESearchCase::Type::IgnoreCase); + FilePath += ".xodr"; + + UE_LOG(LogCarla, Warning, TEXT("File: %s does not exist"), *FilePath); + + // We use the LoadFileToString to load the file into + if (FFileHelper::SaveStringToFile(FString(OpenDriveFile.c_str()), *FilePath)) + { + UE_LOG(LogCarla, Warning, TEXT("FileManipulation: Sucsesfuly Written: \"%s\" to the text file"), *FilePath); + } + else + { + UE_LOG(LogCarla, Warning, TEXT("FileManipulation: Failed to write FString to file.")); + } +} + +void UCustomFileDownloader::StartDownload() +{ + UE_LOG(LogCarla, Log, TEXT("FHttpDownloader CREATED")); + FHttpDownloader *Download = new FHttpDownloader("GET", Url, ResultFileName, DownloadDelegate); + Download->Run(); +} + +FHttpDownloader::FHttpDownloader(const FString &InVerb, const FString &InUrl, const FString &InFilename, FDownloadComplete &Delegate) + : Verb(InVerb), Url(InUrl), Filename(InFilename), DelegateToCall(Delegate) +{ +} + +void FHttpDownloader::Run(void) +{ + UE_LOG(LogCarla, Log, TEXT("Starting download [%s] Url=[%s]"), *Verb, *Url); + TSharedPtr Request = FHttpModule::Get().CreateRequest(); + Request->OnProcessRequestComplete().BindRaw(this, &FHttpDownloader::RequestComplete); + Request->SetURL(Url); + Request->SetVerb(Verb); + Request->ProcessRequest(); +} + +void FHttpDownloader::RequestComplete(FHttpRequestPtr HttpRequest, FHttpResponsePtr HttpResponse, bool bSucceeded) +{ + if (!HttpResponse.IsValid()) + { + UE_LOG(LogCarla, Log, TEXT("Download failed. NULL response")); + } + else + { + // If we do not get success responses codes we do not do anything + if (HttpResponse->GetResponseCode() < 200 || 300 <= HttpResponse->GetResponseCode()) + { + UE_LOG(LogCarla, Error, TEXT("Error during download [%s] Url=[%s] Response=[%d]"), + *HttpRequest->GetVerb(), + *HttpRequest->GetURL(), + HttpResponse->GetResponseCode()); + delete this; + return; + } + + UE_LOG(LogCarla, Log, TEXT("Completed download [%s] Url=[%s] Response=[%d]"), + *HttpRequest->GetVerb(), + *HttpRequest->GetURL(), + HttpResponse->GetResponseCode()); + + HttpRequest->OnProcessRequestComplete().Unbind(); + + FString CurrentFile = FPaths::ProjectContentDir() + "CustomMaps/" + Filename + "/"; + + // We will use this FileManager to deal with the file. + IPlatformFile &FileManager = FPlatformFileManager::Get().GetPlatformFile(); + if (!FileManager.DirectoryExists(*CurrentFile)) + { + FileManager.CreateDirectory(*CurrentFile); + } + CurrentFile += Filename + ".osm"; + + FString StringToWrite = HttpResponse->GetContentAsString(); + + // We use the LoadFileToString to load the file into + if (FFileHelper::SaveStringToFile(StringToWrite, *CurrentFile, FFileHelper::EEncodingOptions::ForceUTF8WithoutBOM)) + { + UE_LOG(LogCarla, Warning, TEXT("FileManipulation: Sucsesfuly Written ")); + } + else + { + UE_LOG(LogCarla, Warning, TEXT("FileManipulation: Failed to write FString to file.")); + } + } + DelegateToCall.ExecuteIfBound(); + + delete this; +} diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Online/CustomFileDownloader.h b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Online/CustomFileDownloader.h new file mode 100644 index 000000000..9df10bd41 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Online/CustomFileDownloader.h @@ -0,0 +1,63 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). This work is licensed under the terms of the MIT license. For a copy, see . + +#pragma once + +#include "CoreMinimal.h" +#include "Interfaces/IHttpRequest.h" +#include "CustomFileDownloader.generated.h" +/** + * + */ + +DECLARE_DELEGATE(FDownloadComplete) + +UCLASS(Blueprintable) +class CARLA_API UCustomFileDownloader : public UObject +{ + GENERATED_BODY() +public: + UFUNCTION(BlueprintCallable) + void StartDownload(); + UFUNCTION(BlueprintCallable) + void ConvertOSMInOpenDrive(FString FilePath); + + FString ResultFileName; + + FString Url; + + FDownloadComplete DownloadDelegate; + +private: + void RequestComplete(FHttpRequestPtr HttpRequest, FHttpResponsePtr HttpResponse, bool bSucceeded); + + FString Payload; +}; + +class FHttpDownloader +{ +public: + /** + * + * @param Verb - verb to use for request (GET,POST,DELETE,etc) + * @param Url - url address to connect to + */ + FHttpDownloader( const FString& InVerb, const FString& InUrl, const FString& InFilename, FDownloadComplete& Delegate ); + + // Kick off the Http request and wait for delegate to be called + void Run(void); + + /** + * Delegate called when the request completes + * + * @param HttpRequest - object that started/processed the request + * @param HttpResponse - optional response object if request completed + * @param bSucceeded - true if Url connection was made and response was received + */ + void RequestComplete(FHttpRequestPtr HttpRequest, FHttpResponsePtr HttpResponse, bool bSucceeded); + +private: + FString Verb; + FString Url; + FString Filename; + FDownloadComplete DelegateToCall; +}; diff --git a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CarlaSettingsDelegate.cpp b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CarlaSettingsDelegate.cpp index 92f99265b..4843e1d1c 100644 --- a/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CarlaSettingsDelegate.cpp +++ b/Unreal/CarlaUE4/Plugins/Carla/Source/Carla/Settings/CarlaSettingsDelegate.cpp @@ -3,7 +3,7 @@ #include "Carla/Game/CarlaGameInstance.h" #include "Carla/Settings/CarlaSettings.h" - +#include "Carla/Game/CarlaGameInstance.h" #include "Async.h" #include "Components/StaticMeshComponent.h" #include "Engine/DirectionalLight.h" diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/CarlaTools.uplugin b/Unreal/CarlaUE4/Plugins/CarlaTools/CarlaTools.uplugin index 83d5474fc..245d378f3 100644 --- a/Unreal/CarlaUE4/Plugins/CarlaTools/CarlaTools.uplugin +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/CarlaTools.uplugin @@ -33,6 +33,10 @@ { "Name": "PhysXVehicles", "Enabled": true + }, + { + "Name": "ProceduralMeshComponent", + "Enabled": true } ] } \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Content/MapGenerator/UWG_Opendrivetomap.uasset b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/MapGenerator/UWG_Opendrivetomap.uasset new file mode 100644 index 000000000..c61d776f9 Binary files /dev/null and b/Unreal/CarlaUE4/Plugins/CarlaTools/Content/MapGenerator/UWG_Opendrivetomap.uasset differ diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/CarlaTools.Build.cs b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/CarlaTools.Build.cs index bf9fad9ba..49886089e 100644 --- a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/CarlaTools.Build.cs +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/CarlaTools.Build.cs @@ -39,7 +39,11 @@ public class CarlaTools : ModuleRules PublicDependencyModuleNames.AddRange( new string[] { - "Core" + "Core", + "ProceduralMeshComponent", + "MeshDescription", + "RawMesh", + "AssetTools" // ... add other public dependencies that you statically link with here ... } ); diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/OpenDriveToMap.cpp b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/OpenDriveToMap.cpp new file mode 100644 index 000000000..5e4571a92 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Private/OpenDriveToMap.cpp @@ -0,0 +1,504 @@ +// Copyright (c) 2023 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). This work is licensed under the terms of the MIT license. For a copy, see . + +#include "OpenDriveToMap.h" +#include "Components/Button.h" +#include "DesktopPlatform/Public/IDesktopPlatform.h" +#include "DesktopPlatform/Public/DesktopPlatformModule.h" +#include "Misc/FileHelper.h" +#include "Runtime/Core/Public/Async/ParallelFor.h" + +#include "Carla/Game/CarlaStatics.h" +#include "Traffic/TrafficLightManager.h" +#include "Online/CustomFileDownloader.h" +#include "Util/ProceduralCustomMesh.h" + +#include "OpenDrive/OpenDriveGenerator.h" + +#include +#include +#include +#include +#include +#include + +#include "Engine/Classes/Interfaces/Interface_CollisionDataProvider.h" +#include "PhysicsCore/Public/BodySetupEnums.h" +#include "RawMesh.h" +#include "AssetRegistryModule.h" +#include "Engine/StaticMesh.h" +#include "Engine/StaticMeshActor.h" +#include "MeshDescription.h" +#include "ProceduralMeshConversion.h" + +FString LaneTypeToFString(carla::road::Lane::LaneType LaneType) +{ + switch (LaneType) + { + case carla::road::Lane::LaneType::Driving: + return FString("Driving"); + break; + case carla::road::Lane::LaneType::Stop: + return FString("Stop"); + break; + case carla::road::Lane::LaneType::Shoulder: + return FString("Shoulder"); + break; + case carla::road::Lane::LaneType::Biking: + return FString("Biking"); + break; + case carla::road::Lane::LaneType::Sidewalk: + return FString("Sidewalk"); + break; + case carla::road::Lane::LaneType::Border: + return FString("Border"); + break; + case carla::road::Lane::LaneType::Restricted: + return FString("Restricted"); + break; + case carla::road::Lane::LaneType::Parking: + return FString("Parking"); + break; + case carla::road::Lane::LaneType::Bidirectional: + return FString("Bidirectional"); + break; + case carla::road::Lane::LaneType::Median: + return FString("Median"); + break; + case carla::road::Lane::LaneType::Special1: + return FString("Special1"); + break; + case carla::road::Lane::LaneType::Special2: + return FString("Special2"); + break; + case carla::road::Lane::LaneType::Special3: + return FString("Special3"); + break; + case carla::road::Lane::LaneType::RoadWorks: + return FString("RoadWorks"); + break; + case carla::road::Lane::LaneType::Tram: + return FString("Tram"); + break; + case carla::road::Lane::LaneType::Rail: + return FString("Rail"); + break; + case carla::road::Lane::LaneType::Entry: + return FString("Entry"); + break; + case carla::road::Lane::LaneType::Exit: + return FString("Exit"); + break; + case carla::road::Lane::LaneType::OffRamp: + return FString("OffRamp"); + break; + case carla::road::Lane::LaneType::OnRamp: + return FString("OnRamp"); + break; + case carla::road::Lane::LaneType::Any: + return FString("Any"); + break; + } + + return FString("Empty"); +} + +void UOpenDriveToMap::ConvertOSMInOpenDrive() +{ + FilePath = FPaths::ProjectContentDir() + "CustomMaps/" + MapName + "/" + MapName + ".osm"; + FileDownloader->ConvertOSMInOpenDrive( FilePath ); + FilePath.RemoveFromEnd(".osm", ESearchCase::Type::IgnoreCase); + FilePath += ".xodr"; + + LoadMap(); +} + +void UOpenDriveToMap::NativeConstruct() +{ + if( !IsValid(FileDownloader) ){ + FileDownloader = NewObject(); + } +} + +void UOpenDriveToMap::NativeDestruct() +{ + Super::NativeDestruct(); + if( IsValid(FileDownloader) ){ + // UObjects are not being destroyed, they are collected by GarbageCollector + // Should we force garbage collection here? + } +} + +void UOpenDriveToMap::CreateMap() +{ + if( MapName.IsEmpty() ) + { + UE_LOG(LogCarlaToolsMapGenerator, Error, TEXT("Map Name Is Empty") ); + return; + } + if ( !IsValid(FileDownloader) ) + { + FileDownloader = NewObject(); + } + FileDownloader->ResultFileName = MapName; + FileDownloader->Url = Url; + FileDownloader->DownloadDelegate.BindUObject( this, &UOpenDriveToMap::ConvertOSMInOpenDrive ); + FileDownloader->StartDownload(); + + RoadType.Empty(); + RoadMesh.Empty(); + MeshesToSpawn.Empty(); + ActorMeshList.Empty(); +} + +void UOpenDriveToMap::OpenFileDialog() +{ + TArray OutFileNames; + void* ParentWindowPtr = FSlateApplication::Get().GetActiveTopLevelWindow()->GetNativeWindow()->GetOSWindowHandle(); + IDesktopPlatform* DesktopPlatform = FDesktopPlatformModule::Get(); + if (DesktopPlatform) + { + DesktopPlatform->OpenFileDialog(ParentWindowPtr, "Select xodr file", FPaths::ProjectDir(), FString(""), ".xodr", 1, OutFileNames); + } + for(FString& CurrentString : OutFileNames) + { + FilePath = CurrentString; + UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT("FileObtained %s"), *CurrentString ); + } +} + +void UOpenDriveToMap::LoadMap() +{ + FString FileContent; + UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT("UOpenDriveToMap::LoadMap(): File to load %s"), *FilePath ); + FFileHelper::LoadFileToString(FileContent, *FilePath); + std::string opendrive_xml = carla::rpc::FromLongFString(FileContent); + boost::optional CarlaMap = carla::opendrive::OpenDriveParser::Load(opendrive_xml); + + if (!CarlaMap.has_value()) + { + UE_LOG(LogCarlaToolsMapGenerator, Error, TEXT("Invalid Map")); + }else + { + UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT("Valid Map loaded")); + } + MapName = FPaths::GetCleanFilename(FilePath); + MapName.RemoveFromEnd(".xodr", ESearchCase::Type::IgnoreCase); + UE_LOG(LogCarlaToolsMapGenerator, Warning, TEXT("MapName %s"), *MapName); + + GenerateAll(CarlaMap); +} + +void UOpenDriveToMap::GenerateAll(const boost::optional& CarlaMap ) +{ + if (!CarlaMap.has_value()) + { + UE_LOG(LogCarlaToolsMapGenerator, Error, TEXT("Invalid Map")); + }else + { + GenerateRoadMesh(CarlaMap); + GenerateSpawnPoints(CarlaMap); + } +} + +void UOpenDriveToMap::GenerateRoadMesh( const boost::optional& CarlaMap ) +{ + opg_parameters.vertex_distance = 0.5f; + opg_parameters.vertex_width_resolution = 8.0f; + opg_parameters.simplification_percentage = 15.0f; + + double start = FPlatformTime::Seconds(); + const auto Meshes = CarlaMap->GenerateOrderedChunkedMesh(opg_parameters); + double end = FPlatformTime::Seconds(); + UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT(" GenerateOrderedChunkedMesh code executed in %f seconds. Simplification percentage is %f"), end - start, opg_parameters.simplification_percentage); + + start = FPlatformTime::Seconds(); + for (const auto &PairMap : Meshes) + { + for( const auto &Mesh : PairMap.second ) + { + if (!Mesh->GetVertices().size()) + { + continue; + } + if (!Mesh->IsValid()) { + continue; + } + + AProceduralMeshActor* TempActor = GetWorld()->SpawnActor(); + UProceduralMeshComponent *TempPMC = TempActor->MeshComponent; + TempPMC->bUseAsyncCooking = true; + TempPMC->bUseComplexAsSimpleCollision = true; + TempPMC->SetCollisionEnabled(ECollisionEnabled::QueryAndPhysics); + + const FProceduralCustomMesh MeshData = *Mesh; + TempPMC->CreateMeshSection_LinearColor( + 0, + MeshData.Vertices, + MeshData.Triangles, + MeshData.Normals, + TArray(), // UV0 + TArray(), // VertexColor + TArray(), // Tangents + true); // Create collision + ActorMeshList.Add(TempActor); + + RoadType.Add(LaneTypeToFString(PairMap.first)); + RoadMesh.Add(TempPMC); + } + } + + end = FPlatformTime::Seconds(); + UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT("Mesh spawnning and translation code executed in %f seconds."), end - start); +} + +void UOpenDriveToMap::GenerateSpawnPoints( const boost::optional& CarlaMap ) +{ + float SpawnersHeight = 300.f; + const auto Waypoints = CarlaMap->GenerateWaypointsOnRoadEntries(); + for (const auto &Wp : Waypoints) + { + const FTransform Trans = CarlaMap->ComputeTransform(Wp); + AVehicleSpawnPoint *Spawner = GetWorld()->SpawnActor(); + Spawner->SetActorRotation(Trans.GetRotation()); + Spawner->SetActorLocation(Trans.GetTranslation() + FVector(0.f, 0.f, SpawnersHeight)); + } +} + +UStaticMesh* UOpenDriveToMap::CreateStaticMeshAsset( UProceduralMeshComponent* ProcMeshComp, int32 MeshIndex, FString FolderName ) +{ + FMeshDescription MeshDescription = BuildMeshDescription(ProcMeshComp); + + IPlatformFile& PlatformFile = FPlatformFileManager::Get().GetPlatformFile(); + + // If we got some valid data. + if (MeshDescription.Polygons().Num() > 0) + { + FString MeshName = *(FolderName + FString::FromInt(MeshIndex) ); + FString PackageName = "/Game/CustomMaps/" + MapName + "/Static/" + FolderName + "/" + MeshName; + + if( !PlatformFile.DirectoryExists(*PackageName) ) + { + PlatformFile.CreateDirectory(*PackageName); + } + + // Then find/create it. + UPackage* Package = CreatePackage(*PackageName); + check(Package); + // Create StaticMesh object + UStaticMesh* StaticMesh = NewObject(Package, *MeshName, RF_Public | RF_Standalone); + StaticMesh->InitResources(); + + StaticMesh->LightingGuid = FGuid::NewGuid(); + + // Add source to new StaticMesh + FStaticMeshSourceModel& SrcModel = StaticMesh->AddSourceModel(); + SrcModel.BuildSettings.bRecomputeNormals = false; + SrcModel.BuildSettings.bRecomputeTangents = false; + SrcModel.BuildSettings.bRemoveDegenerates = false; + SrcModel.BuildSettings.bUseHighPrecisionTangentBasis = false; + SrcModel.BuildSettings.bUseFullPrecisionUVs = false; + SrcModel.BuildSettings.bGenerateLightmapUVs = true; + SrcModel.BuildSettings.SrcLightmapIndex = 0; + SrcModel.BuildSettings.DstLightmapIndex = 1; + StaticMesh->CreateMeshDescription(0, MoveTemp(MeshDescription)); + StaticMesh->CommitMeshDescription(0); + + //// SIMPLE COLLISION + if (!ProcMeshComp->bUseComplexAsSimpleCollision ) + { + StaticMesh->CreateBodySetup(); + UBodySetup* NewBodySetup = StaticMesh->BodySetup; + NewBodySetup->BodySetupGuid = FGuid::NewGuid(); + NewBodySetup->AggGeom.ConvexElems = ProcMeshComp->ProcMeshBodySetup->AggGeom.ConvexElems; + NewBodySetup->bGenerateMirroredCollision = false; + NewBodySetup->bDoubleSidedGeometry = true; + NewBodySetup->CollisionTraceFlag = CTF_UseDefault; + NewBodySetup->CreatePhysicsMeshes(); + } + + //// MATERIALS + TSet UniqueMaterials; + const int32 NumSections = ProcMeshComp->GetNumSections(); + for (int32 SectionIdx = 0; SectionIdx < NumSections; SectionIdx++) + { + FProcMeshSection *ProcSection = + ProcMeshComp->GetProcMeshSection(SectionIdx); + UMaterialInterface *Material = ProcMeshComp->GetMaterial(SectionIdx); + UniqueMaterials.Add(Material); + } + // Copy materials to new mesh + for (auto* Material : UniqueMaterials) + { + StaticMesh->StaticMaterials.Add(FStaticMaterial(Material)); + } + + //Set the Imported version before calling the build + StaticMesh->ImportVersion = EImportStaticMeshVersion::LastVersion; + StaticMesh->Build(false); + StaticMesh->PostEditChange(); + + // Notify asset registry of new asset + FAssetRegistryModule::AssetCreated(StaticMesh); + UPackage::SavePackage(Package, StaticMesh, EObjectFlags::RF_Public | EObjectFlags::RF_Standalone, *MeshName, GError, nullptr, true, true, SAVE_NoError); + return StaticMesh; + } + return nullptr; +} + +TArray UOpenDriveToMap::CreateStaticMeshAssets() +{ + double start = FPlatformTime::Seconds(); + double end = FPlatformTime::Seconds(); + + IPlatformFile& PlatformFile = FPlatformFileManager::Get().GetPlatformFile(); + + TArray StaticMeshes; + + double BuildMeshDescriptionTime = 0.0f; + double PackgaesCreatingTime = 0.0f; + double MeshInitTime = 0.0f; + double MatAndCollInitTime = 0.0f; + double MeshBuildTime = 0.0f; + double PackSaveTime = 0.0f; + + + for (int i = 0; i < RoadMesh.Num(); ++i) + { + FString MeshName = RoadType[i] + FString::FromInt(i); + FString PackageName = "/Game/CustomMaps/" + MapName + "/Static/" + RoadType[i] + "/" + MeshName; + + if (!PlatformFile.DirectoryExists(*PackageName)) + { + PlatformFile.CreateDirectory(*PackageName); + } + + UProceduralMeshComponent* ProcMeshComp = RoadMesh[i]; + start = FPlatformTime::Seconds(); + FMeshDescription MeshDescription = BuildMeshDescription(ProcMeshComp); + end = FPlatformTime::Seconds(); + BuildMeshDescriptionTime += end - start; + // If we got some valid data. + if (MeshDescription.Polygons().Num() > 0) + { + start = FPlatformTime::Seconds(); + // Then find/create it. + UPackage* Package = CreatePackage(*PackageName); + check(Package); + end = FPlatformTime::Seconds(); + PackgaesCreatingTime += end - start; + + start = FPlatformTime::Seconds(); + + // Create StaticMesh object + UStaticMesh* CurrentStaticMesh = NewObject(Package, *MeshName, RF_Public | RF_Standalone); + CurrentStaticMesh->InitResources(); + + CurrentStaticMesh->LightingGuid = FGuid::NewGuid(); + + // Add source to new StaticMesh + FStaticMeshSourceModel& SrcModel = CurrentStaticMesh->AddSourceModel(); + SrcModel.BuildSettings.bRecomputeNormals = false; + SrcModel.BuildSettings.bRecomputeTangents = false; + SrcModel.BuildSettings.bRemoveDegenerates = false; + SrcModel.BuildSettings.bUseHighPrecisionTangentBasis = false; + SrcModel.BuildSettings.bUseFullPrecisionUVs = false; + SrcModel.BuildSettings.bGenerateLightmapUVs = true; + SrcModel.BuildSettings.SrcLightmapIndex = 0; + SrcModel.BuildSettings.DstLightmapIndex = 1; + CurrentStaticMesh->CreateMeshDescription(0, MoveTemp(MeshDescription)); + CurrentStaticMesh->CommitMeshDescription(0); + + end = FPlatformTime::Seconds(); + MeshInitTime += end - start; + start = FPlatformTime::Seconds(); + + //// SIMPLE COLLISION + if (!ProcMeshComp->bUseComplexAsSimpleCollision) + { + CurrentStaticMesh->CreateBodySetup(); + UBodySetup* NewBodySetup = CurrentStaticMesh->BodySetup; + NewBodySetup->BodySetupGuid = FGuid::NewGuid(); + NewBodySetup->AggGeom.ConvexElems = ProcMeshComp->ProcMeshBodySetup->AggGeom.ConvexElems; + NewBodySetup->bGenerateMirroredCollision = false; + NewBodySetup->bDoubleSidedGeometry = true; + NewBodySetup->CollisionTraceFlag = CTF_UseDefault; + NewBodySetup->CreatePhysicsMeshes(); + } + + //// MATERIALS + TSet UniqueMaterials; + const int32 NumSections = ProcMeshComp->GetNumSections(); + for (int32 SectionIdx = 0; SectionIdx < NumSections; SectionIdx++) + { + FProcMeshSection* ProcSection = + ProcMeshComp->GetProcMeshSection(SectionIdx); + UMaterialInterface* Material = ProcMeshComp->GetMaterial(SectionIdx); + UniqueMaterials.Add(Material); + } + // Copy materials to new mesh + for (auto* Material : UniqueMaterials) + { + CurrentStaticMesh->StaticMaterials.Add(FStaticMaterial(Material)); + } + + end = FPlatformTime::Seconds(); + MatAndCollInitTime += end - start; + start = FPlatformTime::Seconds(); + //Set the Imported version before calling the build + CurrentStaticMesh->ImportVersion = EImportStaticMeshVersion::LastVersion; + CurrentStaticMesh->Build(false); + CurrentStaticMesh->PostEditChange(); + + end = FPlatformTime::Seconds(); + MeshBuildTime += end - start; + start = FPlatformTime::Seconds(); + + FString MeshName = *(RoadType[i] + FString::FromInt(i)); + // Notify asset registry of new asset + FAssetRegistryModule::AssetCreated(CurrentStaticMesh); + UPackage::SavePackage(Package, CurrentStaticMesh, EObjectFlags::RF_Public | EObjectFlags::RF_Standalone, *MeshName, GError, nullptr, true, true, SAVE_NoError); + + end = FPlatformTime::Seconds(); + PackSaveTime += end - start; + + + StaticMeshes.Add( CurrentStaticMesh ); + } + } + UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT(" UOpenDriveToMap::CreateStaticMeshAssets total time in BuildMeshDescriptionTime %f. Time per mesh %f"), BuildMeshDescriptionTime, BuildMeshDescriptionTime/ RoadMesh.Num()); + UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT(" UOpenDriveToMap::CreateStaticMeshAssets total time in PackgaesCreatingTime %f. Time per mesh %f"), PackgaesCreatingTime, PackgaesCreatingTime / RoadMesh.Num()); + UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT(" UOpenDriveToMap::CreateStaticMeshAssets total time in MeshInitTime %f. Time per mesh %f"), MeshInitTime, MeshInitTime / RoadMesh.Num()); + UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT(" UOpenDriveToMap::CreateStaticMeshAssets total time in MatAndCollInitTime %f. Time per mesh %f"), MatAndCollInitTime, MatAndCollInitTime / RoadMesh.Num()); + UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT(" UOpenDriveToMap::CreateStaticMeshAssets total time in MeshBuildTime %f. Time per mesh %f"), MeshBuildTime, MeshBuildTime / RoadMesh.Num()); + UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT(" UOpenDriveToMap::CreateStaticMeshAssets total time in PackSaveTime %f. Time per mesh %f"), PackSaveTime, PackSaveTime / RoadMesh.Num()); + return StaticMeshes; +} + +void UOpenDriveToMap::SaveMap() +{ + double start = FPlatformTime::Seconds(); + + MeshesToSpawn = CreateStaticMeshAssets(); + + double end = FPlatformTime::Seconds(); + UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT(" Meshes created static mesh code executed in %f seconds."), end - start); + + for (auto CurrentActor : ActorMeshList) + { + CurrentActor->Destroy(); + } + + start = FPlatformTime::Seconds(); + + for (auto CurrentMesh : MeshesToSpawn) + { + AStaticMeshActor* TempActor = GetWorld()->SpawnActor(); + // Build mesh from source + TempActor->GetStaticMeshComponent()->SetStaticMesh(CurrentMesh); + TempActor->SetActorLabel(FString("SM_") + CurrentMesh->GetName()); + } + + end = FPlatformTime::Seconds(); + UE_LOG(LogCarlaToolsMapGenerator, Log, TEXT(" Spawning Static Meshes code executed in %f seconds."), end - start); + +} \ No newline at end of file diff --git a/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/OpenDriveToMap.h b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/OpenDriveToMap.h new file mode 100644 index 000000000..a527798d6 --- /dev/null +++ b/Unreal/CarlaUE4/Plugins/CarlaTools/Source/CarlaTools/Public/OpenDriveToMap.h @@ -0,0 +1,83 @@ +// Copyright (c) 2017 Computer Vision Center (CVC) at the Universitat Autonoma de Barcelona (UAB). This work is licensed under the terms of the MIT license. For a copy, see . + +#pragma once + +#include "CoreMinimal.h" +#include "Blueprint/UserWidget.h" +#include "ProceduralMeshComponent.h" + +#include +#include +#include +#include + +#include "OpenDriveToMap.generated.h" + + +class UProceduralMeshComponent; +class UCustomFileDownloader; +/** + * + */ +UCLASS() +class CARLATOOLS_API UOpenDriveToMap : public UUserWidget +{ + GENERATED_BODY() + +public: + + UFUNCTION() + void ConvertOSMInOpenDrive(); + + UPROPERTY( meta = (BindWidget) ) + class UButton* StartButton; + + UPROPERTY(meta = (BindWidget)) + class UButton* SaveButton; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category="File") + FString FilePath; + + UPROPERTY(EditAnywhere, BlueprintReadWrite, Category="File") + FString MapName; + + UPROPERTY( EditAnywhere, BlueprintReadWrite, Category="Settings" ) + FString Url; + +protected: + virtual void NativeConstruct() override; + virtual void NativeDestruct() override; + + UFUNCTION( BlueprintCallable ) + void CreateMap(); + + UFUNCTION( BlueprintCallable ) + void SaveMap(); +private: + + UFUNCTION() + void OpenFileDialog(); + + UFUNCTION() + void LoadMap(); + + void GenerateAll(const boost::optional& CarlaMap); + void GenerateRoadMesh(const boost::optional& CarlaMap); + void GenerateSpawnPoints(const boost::optional& CarlaMap); + + carla::rpc::OpendriveGenerationParameters opg_parameters; + + UStaticMesh* CreateStaticMeshAsset(UProceduralMeshComponent* ProcMeshComp, int32 MeshIndex, FString FolderName); + TArray CreateStaticMeshAssets(); + + UPROPERTY() + UCustomFileDownloader* FileDownloader; + UPROPERTY() + TArray ActorMeshList; + UPROPERTY() + TArray MeshesToSpawn; + UPROPERTY() + TArray RoadType; + UPROPERTY() + TArray RoadMesh; +}; diff --git a/Util/BuildTools/BuildOSM2ODR.bat b/Util/BuildTools/BuildOSM2ODR.bat index 47d0aeb9e..030de270c 100644 --- a/Util/BuildTools/BuildOSM2ODR.bat +++ b/Util/BuildTools/BuildOSM2ODR.bat @@ -69,6 +69,8 @@ rem set OSM2ODR_VSPROJECT_PATH=%INSTALLATION_DIR:/=\%osm2odr-visualstudio\ set OSM2ODR_SOURCE_PATH=%INSTALLATION_DIR:/=\%om2odr-source\ set OSM2ODR_INSTALL_PATH=%ROOT_PATH:/=\%PythonAPI\carla\dependencies\ +set OSM2ODR__SERVER_INSTALL_PATH=%ROOT_PATH:/=\%Unreal\CarlaUE4\Plugins\Carla\CarlaDependencies +set CARLA_DEPENDENCIES_FOLDER=%ROOT_PATH:/=\%Unreal\CarlaUE4\Plugins\Carla\CarlaDependencies\ if %REMOVE_INTERMEDIATE% == true ( rem Remove directories @@ -107,6 +109,9 @@ if %BUILD_OSM2ODR% == true ( cmake --build . --config Release --target install | findstr /V "Up-to-date:" if %errorlevel% neq 0 goto error_install + + copy %OSM2ODR_INSTALL_PATH%\lib\osm2odr.lib %CARLA_DEPENDENCIES_FOLDER%\lib + copy %OSM2ODR_INSTALL_PATH%\include\OSM2ODR.h %CARLA_DEPENDENCIES_FOLDER%\include ) goto success diff --git a/Util/BuildTools/BuildOSM2ODR.sh b/Util/BuildTools/BuildOSM2ODR.sh index 9e709a88d..5dfe1b6ec 100755 --- a/Util/BuildTools/BuildOSM2ODR.sh +++ b/Util/BuildTools/BuildOSM2ODR.sh @@ -105,6 +105,25 @@ if ${BUILD_OSM2ODR} ; then ninja osm2odr ninja install + mkdir -p ${OSM2ODR_SERVER_BUILD_FOLDER} + cd ${OSM2ODR_SERVER_BUILD_FOLDER} + + LLVM_BASENAME=llvm-8.0 + LLVM_INCLUDE="$UE4_ROOT/Engine/Source/ThirdParty/Linux/LibCxx/include/c++/v1" + LLVM_LIBPATH="$UE4_ROOT/Engine/Source/ThirdParty/Linux/LibCxx/lib/Linux/x86_64-unknown-linux-gnu" + + cmake ${OSM2ODR_SOURCE_FOLDER} \ + -G "Eclipse CDT4 - Ninja" \ + -DCMAKE_CXX_FLAGS="-fPIC -std=c++14 -stdlib=libc++ -I${LLVM_INCLUDE} -L${LLVM_LIBPATH}" \ + -DCMAKE_INSTALL_PREFIX=${LIBCARLA_INSTALL_SERVER_FOLDER} \ + -DPROJ_INCLUDE_DIR=${CARLA_BUILD_FOLDER}/proj-install-server/include \ + -DPROJ_LIBRARY=${CARLA_BUILD_FOLDER}/proj-install-server/lib/libproj.a \ + -DXercesC_INCLUDE_DIR=${CARLA_BUILD_FOLDER}/xerces-c-3.2.3-install-server/include \ + -DXercesC_LIBRARY=${CARLA_BUILD_FOLDER}/xerces-c-3.2.3-install-server/lib/libxerces-c.a + + ninja osm2odr + ninja install + fi -log "Success!" +log " OSM2ODR Success!" diff --git a/Util/BuildTools/Linux.mk b/Util/BuildTools/Linux.mk index bbaa11cd7..87a0992bc 100644 --- a/Util/BuildTools/Linux.mk +++ b/Util/BuildTools/Linux.mk @@ -3,7 +3,7 @@ default: help help: @less ${CARLA_BUILD_TOOLS_FOLDER}/Linux.mk.help -launch: LibCarla.server.release +launch: LibCarla.server.release osm2odr @${CARLA_BUILD_TOOLS_FOLDER}/BuildCarlaUE4.sh --build --launch $(ARGS) launch-only: @@ -79,7 +79,7 @@ examples: run-examples: @for D in ${CARLA_EXAMPLES_FOLDER}/*; do [ -d "$${D}" ] && make -C $${D} run.only; done -CarlaUE4Editor: LibCarla.server.release +CarlaUE4Editor: LibCarla.server.release osm2odr @${CARLA_BUILD_TOOLS_FOLDER}/BuildCarlaUE4.sh --build $(ARGS) .PHONY: PythonAPI diff --git a/Util/BuildTools/Setup.bat b/Util/BuildTools/Setup.bat index 533d6aace..7e025c509 100644 --- a/Util/BuildTools/Setup.bat +++ b/Util/BuildTools/Setup.bat @@ -190,15 +190,16 @@ echo %FILE_N% Installing Xercesc... call "%INSTALLERS_DIR%install_xercesc.bat"^ --build-dir "%INSTALLATION_DIR%" copy %INSTALLATION_DIR%\xerces-c-3.2.3-install\lib\xerces-c_3.lib %CARLA_PYTHON_DEPENDENCIES%\lib +copy %INSTALLATION_DIR%\xerces-c-3.2.3-install\lib\xerces-c_3.lib %CARLA_DEPENDENCIES_FOLDER%\lib rem ============================================================================ rem -- Download and install Sqlite3 -------------------------------------------- rem ============================================================================ - echo %FILE_N% Installing Sqlite3 call "%INSTALLERS_DIR%install_sqlite3.bat"^ --build-dir "%INSTALLATION_DIR%" copy %INSTALLATION_DIR%\sqlite3-install\lib\sqlite3.lib %CARLA_PYTHON_DEPENDENCIES%\lib +copy %INSTALLATION_DIR%\sqlite3-install\lib\sqlite3.lib %CARLA_DEPENDENCIES_FOLDER%\lib rem ============================================================================ rem -- Download and install PROJ -------------------------------------------- @@ -208,6 +209,7 @@ echo %FILE_N% Installing PROJ call "%INSTALLERS_DIR%install_proj.bat"^ --build-dir "%INSTALLATION_DIR%" copy %INSTALLATION_DIR%\proj-install\lib\proj.lib %CARLA_PYTHON_DEPENDENCIES%\lib +copy %INSTALLATION_DIR%\proj-install\lib\proj.lib %CARLA_DEPENDENCIES_FOLDER%\lib rem ============================================================================ rem -- Download and install Eigen ---------------------------------------------- diff --git a/Util/BuildTools/Setup.sh b/Util/BuildTools/Setup.sh index b464c52b5..f04508d63 100755 --- a/Util/BuildTools/Setup.sh +++ b/Util/BuildTools/Setup.sh @@ -380,9 +380,11 @@ XERCESC_REPO=https://archive.apache.org/dist/xerces/c/3/sources/xerces-c-${XERCE XERCESC_SRC_DIR=${XERCESC_BASENAME}-source XERCESC_INSTALL_DIR=${XERCESC_BASENAME}-install +XERCESC_INSTALL_SERVER_DIR=${XERCESC_BASENAME}-install-server XERCESC_LIB=${XERCESC_INSTALL_DIR}/lib/libxerces-c.a +XERCESC_SERVER_LIB=${XERCESC_INSTALL_SERVER_DIR}/lib/libxerces-c.a -if [[ -d ${XERCESC_INSTALL_DIR} ]] ; then +if [[ -d ${XERCESC_INSTALL_DIR} && -d ${XERCESC_INSTALL_SERVER_DIR} ]] ; then log "Xerces-c already installed." else log "Retrieving xerces-c." @@ -415,6 +417,23 @@ else popd >/dev/null + mkdir -p ${XERCESC_INSTALL_SERVER_DIR} + + pushd ${XERCESC_SRC_DIR}/build >/dev/null + + cmake -G "Ninja" \ + -DCMAKE_CXX_FLAGS="-std=c++14 -stdlib=libc++ -fPIC -w -I${LLVM_INCLUDE} -L${LLVM_LIBPATH}" \ + -DCMAKE_INSTALL_PREFIX="../../${XERCESC_INSTALL_SERVER_DIR}" \ + -DCMAKE_BUILD_TYPE=Release \ + -DBUILD_SHARED_LIBS=OFF \ + -Dtranscoder=gnuiconv \ + -Dnetwork=OFF \ + .. + ninja + ninja install + + popd >/dev/null + rm -Rf ${XERCESC_BASENAME}.tar.gz rm -Rf ${XERCESC_SRC_DIR} fi @@ -422,6 +441,9 @@ fi mkdir -p ${LIBCARLA_INSTALL_CLIENT_FOLDER}/lib/ cp ${XERCESC_LIB} ${LIBCARLA_INSTALL_CLIENT_FOLDER}/lib/ +mkdir -p ${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/ +cp -p ${XERCESC_SERVER_LIB} ${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/ + # ============================================================================== # -- Get Eigen headers 3.1.0 (CARLA dependency) ------------------------------------- # ============================================================================== @@ -545,6 +567,7 @@ SQLITE_INSTALL_DIR=sqlite-install SQLITE_INCLUDE_DIR=${PWD}/${SQLITE_INSTALL_DIR}/include SQLITE_LIB=${PWD}/${SQLITE_INSTALL_DIR}/lib/libsqlite3.a +SQLITE_FULL_LIB=${PWD}/${SQLITE_INSTALL_DIR}/lib/ SQLITE_EXE=${PWD}/${SQLITE_INSTALL_DIR}/bin/sqlite3 if [[ -d ${SQLITE_INSTALL_DIR} ]] ; then @@ -561,7 +584,7 @@ else pushd ${SQLITE_SOURCE_DIR} >/dev/null - export CFLAGS="-fPIC" + export CFLAGS="-fPIC -w" ./configure --prefix=${PWD}/../sqlite-install/ make make install @@ -575,6 +598,9 @@ fi mkdir -p ${LIBCARLA_INSTALL_CLIENT_FOLDER}/lib/ cp ${SQLITE_LIB} ${LIBCARLA_INSTALL_CLIENT_FOLDER}/lib/ +mkdir -p ${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/ +cp -p -r ${SQLITE_FULL_LIB} ${LIBCARLA_INSTALL_SERVER_FOLDER} + # ============================================================================== # -- Get and compile PROJ ------------------------------------------------------ # ============================================================================== @@ -585,10 +611,13 @@ PROJ_REPO=https://download.osgeo.org/proj/${PROJ_VERSION}.tar.gz PROJ_TAR=${PROJ_VERSION}.tar.gz PROJ_SRC_DIR=proj-src PROJ_INSTALL_DIR=proj-install +PROJ_INSTALL_SERVER_DIR=proj-install-server PROJ_INSTALL_DIR_FULL=${PWD}/${PROJ_INSTALL_DIR} +PROJ_INSTALL_SERVER_DIR_FULL=${PWD}/${PROJ_INSTALL_SERVER_DIR} PROJ_LIB=${PROJ_INSTALL_DIR_FULL}/lib/libproj.a +PROJ_SERVER_LIB=${PROJ_INSTALL_SERVER_DIR_FULL}/lib/libproj.a -if [[ -d ${PROJ_INSTALL_DIR} ]] ; then +if [[ -d ${PROJ_INSTALL_DIR} && -d ${PROJ_INSTALL_SERVER_DIR_FULL} ]] ; then log "PROJ already installed." else log "Retrieving PROJ" @@ -598,8 +627,8 @@ else tar -xzf ${PROJ_TAR} mv ${PROJ_VERSION} ${PROJ_SRC_DIR} - mkdir ${PROJ_SRC_DIR}/build - mkdir ${PROJ_INSTALL_DIR} + mkdir -p ${PROJ_SRC_DIR}/build + mkdir -p ${PROJ_INSTALL_DIR} pushd ${PROJ_SRC_DIR}/build >/dev/null @@ -617,6 +646,24 @@ else popd >/dev/null + mkdir -p ${PROJ_INSTALL_SERVER_DIR} + + pushd ${PROJ_SRC_DIR}/build >/dev/null + + cmake -G "Ninja" .. \ + -DCMAKE_CXX_FLAGS="-std=c++14 -fPIC -stdlib=libc++ -I${LLVM_INCLUDE} -Wl,-L${LLVM_LIBPATH}" \ + -DSQLITE3_INCLUDE_DIR=${SQLITE_INCLUDE_DIR} -DSQLITE3_LIBRARY=${SQLITE_LIB} \ + -DEXE_SQLITE3=${SQLITE_EXE} \ + -DENABLE_TIFF=OFF -DENABLE_CURL=OFF -DBUILD_SHARED_LIBS=OFF -DBUILD_PROJSYNC=OFF \ + -DCMAKE_BUILD_TYPE=Release -DBUILD_PROJINFO=OFF \ + -DBUILD_CCT=OFF -DBUILD_CS2CS=OFF -DBUILD_GEOD=OFF -DBUILD_GIE=OFF \ + -DBUILD_PROJ=OFF -DBUILD_TESTING=OFF \ + -DCMAKE_INSTALL_PREFIX=${PROJ_INSTALL_SERVER_DIR_FULL} + ninja + ninja install + + popd >/dev/null + rm -Rf ${PROJ_TAR} rm -Rf ${PROJ_SRC_DIR} @@ -624,6 +671,9 @@ fi cp ${PROJ_LIB} ${LIBCARLA_INSTALL_CLIENT_FOLDER}/lib/ +mkdir -p ${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/ +cp -p ${PROJ_SERVER_LIB} ${LIBCARLA_INSTALL_SERVER_FOLDER}/lib/ + # ============================================================================== # -- Get and compile patchelf -------------------------------------------------- # ============================================================================== diff --git a/Util/BuildTools/Vars.mk b/Util/BuildTools/Vars.mk index 538708d4a..4b57a1f3e 100644 --- a/Util/BuildTools/Vars.mk +++ b/Util/BuildTools/Vars.mk @@ -21,6 +21,7 @@ LIBCARLA_INSTALL_SERVER_FOLDER=${CARLAUE4_PLUGIN_ROOT_FOLDER}/CarlaDependencies LIBCARLA_INSTALL_CLIENT_FOLDER=${CARLA_PYTHONAPI_SOURCE_FOLDER}/dependencies OSM2ODR_BUILD_FOLDER=${CARLA_BUILD_FOLDER}/libosm2dr-build +OSM2ODR_SERVER_BUILD_FOLDER=${CARLA_BUILD_FOLDER}/libosm2dr-build-server OSM2ODR_SOURCE_FOLDER=${CARLA_BUILD_FOLDER}/libosm2dr-source CARLAUE4_PLUGIN_DEPS_FOLDER=${CARLAUE4_PLUGIN_ROOT_FOLDER}/CarlaDependencies diff --git a/Util/BuildTools/Windows.mk b/Util/BuildTools/Windows.mk index 6437f9aae..c604a06ed 100644 --- a/Util/BuildTools/Windows.mk +++ b/Util/BuildTools/Windows.mk @@ -16,7 +16,7 @@ help: import: server @"${CARLA_BUILD_TOOLS_FOLDER}/Import.py" $(ARGS) -CarlaUE4Editor: LibCarla +CarlaUE4Editor: LibCarla osm2odr @"${CARLA_BUILD_TOOLS_FOLDER}/BuildCarlaUE4.bat" --build $(ARGS) launch: CarlaUE4Editor diff --git a/Util/InstallersWin/install_zlib.bat b/Util/InstallersWin/install_zlib.bat index 173ec0489..f418293d3 100644 --- a/Util/InstallersWin/install_zlib.bat +++ b/Util/InstallersWin/install_zlib.bat @@ -172,6 +172,7 @@ rem ============================================================================ :good_exit echo %FILE_N% Exiting... rem A return value used for checking for errors + copy %ZLIB_INSTALL_DIR%\lib\zlibstatic.lib %CARLA_DEPENDENCIES_FOLDER%\lib endlocal & set install_zlib=%ZLIB_INSTALL_DIR% exit /b 0