p71924506
/
carla
mirror of https://github.com/carla-simulator/carla.git
1
0
Fork 0
Code Issues Packages Projects Releases Wiki Activity

Style fix

This commit is contained in:
Axel 2020-02-07 13:03:27 +01:00 committed by Marc Garcia Puig
parent 744a2d47e0
commit dd4e1a7733
2 changed files with 99 additions and 80 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

99
LibCarla/source/carla/road/Map.cpp
View File

@ -328,10 +328,7 @@ namespace road {
// compute the tangent of the laneOffset // compute the tangent of the laneOffset
const auto lane_offset_info = road.GetInfo<RoadInfoLaneOffset>(waypoint.s); const auto lane_offset_info = road.GetInfo<RoadInfoLaneOffset>(waypoint.s);
float lane_offset_tangent = 0; const auto lane_offset_tangent = static_cast<float>(lane_offset_info->GetPolynomial().Tangent(waypoint.s));
if (lane_offset_info) {
lane_offset_tangent = static_cast<float>(lane_offset_info->GetPolynomial().Tangent(waypoint.s));
}
lane_tangent -= lane_offset_tangent; lane_tangent -= lane_offset_tangent;
@ -339,10 +336,9 @@ namespace road {
lane_tangent *= -1; lane_tangent *= -1;
geom::Rotation rot( geom::Rotation rot(
geom::Math::ToDegrees(static_cast<float>(dp.pitch)), geom::Math::ToDegrees(static_cast<float>(dp.pitch)),
geom::Math::ToDegrees(-static_cast<float>(dp.tangent)), // Unreal's Y geom::Math::ToDegrees(-static_cast<float>(dp.tangent)), // Unreal's Y axis hack
// axis hack 0.0f);
0.0f);
dp.ApplyLateralOffset(lane_width); dp.ApplyLateralOffset(lane_width);
@ -448,8 +444,7 @@ namespace road {
// move perpendicular ('t') // move perpendicular ('t')
geom::Transform pivot = base; geom::Transform pivot = base;
pivot.rotation.yaw -= geom::Math::ToDegrees<float>(static_cast<float>(crosswalk->GetHeading())); pivot.rotation.yaw -= geom::Math::ToDegrees<float>(static_cast<float>(crosswalk->GetHeading()));
pivot.rotation.yaw -= 90; // move perpendicular to 's' for the pivot.rotation.yaw -= 90; // move perpendicular to 's' for the lateral offset
// lateral offset
geom::Vector3D v(static_cast<float>(crosswalk->GetT()), 0.0f, 0.0f); geom::Vector3D v(static_cast<float>(crosswalk->GetT()), 0.0f, 0.0f);
pivot.TransformPoint(v); pivot.TransformPoint(v);
// restore pivot position and orientation // restore pivot position and orientation
@ -461,13 +456,11 @@ namespace road {
for (auto corner : crosswalk->GetPoints()) { for (auto corner : crosswalk->GetPoints()) {
geom::Vector3D v2(static_cast<float>(corner.u), static_cast<float>(corner.v), geom::Vector3D v2(static_cast<float>(corner.u), static_cast<float>(corner.v),
static_cast<float>(corner.z)); static_cast<float>(corner.z));
// set the width larger to contact with the sidewalk (in case they // set the width larger to contact with the sidewalk (in case they have gutter area)
// have gutter area) if (corner.u < 0)
if (corner.u < 0) {
v2.x -= 1.0f; v2.x -= 1.0f;
} else { else
v2.x += 1.0f; v2.x += 1.0f;
}
pivot.TransformPoint(v2); pivot.TransformPoint(v2);
result.push_back(v2); result.push_back(v2);
} }
@ -628,7 +621,7 @@ namespace road {
for (const auto &lane : lane_section.GetLanes()) { for (const auto &lane : lane_section.GetLanes()) {
// add only the right (negative) lanes // add only the right (negative) lanes
if (lane.first < 0 && if (lane.first < 0 &&
static_cast<uint32_t>(lane.second.GetType()) & static_cast<uint32_t>(lane_type)) { static_cast<uint32_t>(lane.second.GetType()) & static_cast<uint32_t>(lane_type)) {
result.emplace_back(Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), 0.0 }); result.emplace_back(Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), 0.0 });
} }
} }
@ -639,9 +632,9 @@ namespace road {
for (const auto &lane : lane_section.GetLanes()) { for (const auto &lane : lane_section.GetLanes()) {
// add only the left (positive) lanes // add only the left (positive) lanes
if (lane.first > 0 && if (lane.first > 0 &&
static_cast<uint32_t>(lane.second.GetType()) & static_cast<uint32_t>(lane_type)) { static_cast<uint32_t>(lane.second.GetType()) & static_cast<uint32_t>(lane_type)) {
result.emplace_back(Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), result.emplace_back(
road_len }); Waypoint{ road.GetId(), lane_section.GetId(), lane.second.GetId(), road_len });
} }
} }
} }
@ -711,9 +704,12 @@ namespace road {
// Adds a new element to the rtree element list using the position of the // Adds a new element to the rtree element list using the position of the
// waypoints // waypoints
// both ends of the segment // both ends of the segment
void Map::AddElementToRtree(std::vector<Rtree::TreeElement> &rtree_elements, void Map::AddElementToRtree(
geom::Transform &current_transform, geom::Transform &next_transform, std::vector<Rtree::TreeElement> &rtree_elements,
Waypoint & current_waypoint, Waypoint & next_waypoint) { geom::Transform &current_transform,
geom::Transform &next_transform,
Waypoint & current_waypoint,
Waypoint & next_waypoint) {
Rtree::BPoint init = Rtree::BPoint init =
Rtree::BPoint( Rtree::BPoint(
current_transform.location.x, current_transform.location.x,
@ -728,10 +724,12 @@ namespace road {
std::make_pair(current_waypoint, next_waypoint))); std::make_pair(current_waypoint, next_waypoint)));
} }
// Adds a new element to the rtree element list using the position of the // Adds a new element to the rtree element list using the position of the
// waypoints // waypoints, both ends of the segment
// both ends of the segment void Map::AddElementToRtreeAndUpdateTransforms(
void Map::AddElementToRtreeAndUpdateTransforms(std::vector<Rtree::TreeElement> &rtree_elements, std::vector<Rtree::TreeElement> &rtree_elements,
geom::Transform &current_transform, Waypoint & current_waypoint, Waypoint & next_waypoint) { geom::Transform &current_transform,
Waypoint & current_waypoint,
Waypoint & next_waypoint) {
geom::Transform next_transform = ComputeTransform(next_waypoint); geom::Transform next_transform = ComputeTransform(next_waypoint);
AddElementToRtree(rtree_elements, current_transform, next_transform, AddElementToRtree(rtree_elements, current_transform, next_transform,
current_waypoint, next_waypoint); current_waypoint, next_waypoint);
@ -741,7 +739,10 @@ namespace road {
// returns the remaining length of the geometry depending on the lane // returns the remaining length of the geometry depending on the lane
// direction // direction
double GetRemainingLength(const Lane &lane, double geometry_start_s, double geometry_end_s, double GetRemainingLength(
const Lane &lane,
double geometry_start_s,
double geometry_end_s,
double current_s) { double current_s) {
if (lane.GetId() < 0) { if (lane.GetId() < 0) {
return (geometry_end_s - current_s); return (geometry_end_s - current_s);
@ -786,7 +787,8 @@ namespace road {
if (IsLineStraight(road, lane, geometry->GetGeometry().GetType())) { if (IsLineStraight(road, lane, geometry->GetGeometry().GetType())) {
double delta_s = min_delta_s; double delta_s = min_delta_s;
double remaining_length = double remaining_length =
GetRemainingLength(lane, geometry_start_s, geometry_end_s, current_waypoint.s) - epsilon; GetRemainingLength(lane, geometry_start_s, geometry_end_s, current_waypoint.s);
remaining_length -= epsilon;
delta_s = remaining_length; delta_s = remaining_length;
if (delta_s < epsilon) { if (delta_s < epsilon) {
break; break;
@ -796,38 +798,56 @@ namespace road {
RELEASE_ASSERT(next.front().road_id == current_waypoint.road_id); RELEASE_ASSERT(next.front().road_id == current_waypoint.road_id);
auto next_waypoint = next.front(); auto next_waypoint = next.front();
AddElementToRtreeAndUpdateTransforms(rtree_elements, current_transform, current_waypoint, AddElementToRtreeAndUpdateTransforms(
next_waypoint); rtree_elements,
current_transform,
current_waypoint,
next_waypoint);
} else { } else {
auto next_waypoint = current_waypoint; auto next_waypoint = current_waypoint;
// Loop until the end of the geometry // Loop until the end of the geometry
while (true) { while (true) {
double delta_s = min_delta_s; double delta_s = min_delta_s;
double remaining_length = double remaining_length =
GetRemainingLength(lane, geometry_start_s, geometry_end_s, next_waypoint.s) - epsilon; GetRemainingLength(
lane,
geometry_start_s,
geometry_end_s,
next_waypoint.s);
remaining_length -= epsilon;
delta_s = std::min(delta_s, remaining_length); delta_s = std::min(delta_s, remaining_length);
if (delta_s < epsilon) { if (delta_s < epsilon) {
AddElementToRtreeAndUpdateTransforms(rtree_elements, current_transform, current_waypoint, AddElementToRtreeAndUpdateTransforms(
next_waypoint); rtree_elements,
current_transform,
current_waypoint,
next_waypoint);
break; break;
} }
auto next = GetNext(next_waypoint, delta_s); auto next = GetNext(next_waypoint, delta_s);
if (next.size() != 1 || if (next.size() != 1 ||
current_waypoint.road_id != next.front().road_id) { current_waypoint.road_id != next.front().road_id) {
AddElementToRtreeAndUpdateTransforms(rtree_elements, current_transform, current_waypoint, AddElementToRtreeAndUpdateTransforms(
next_waypoint); rtree_elements,
current_transform,
current_waypoint,
next_waypoint);
break; break;
} }
next_waypoint = next.front(); next_waypoint = next.front();
geom::Transform next_transform = ComputeTransform(next_waypoint); geom::Transform next_transform = ComputeTransform(next_waypoint);
double angle = geom::Math::GetVectorAngle(current_transform.GetForwardVector(), double angle = geom::Math::GetVectorAngle(
next_transform.GetForwardVector()); current_transform.GetForwardVector(), next_transform.GetForwardVector());
if (abs(angle) > angle_threshold) { if (abs(angle) > angle_threshold) {
AddElementToRtree(rtree_elements, current_transform, next_transform, AddElementToRtree(
current_waypoint, next_waypoint); rtree_elements,
current_transform,
next_transform,
current_waypoint,
next_waypoint);
current_waypoint = next_waypoint; current_waypoint = next_waypoint;
current_transform = next_transform; current_transform = next_transform;
} }
@ -841,7 +861,6 @@ namespace road {
} }
} }
} }
_rtree.InsertElements(rtree_elements); _rtree.InsertElements(rtree_elements);
} }

80
LibCarla/source/carla/road/element/Geometry.cpp
View File

@ -188,47 +188,47 @@ namespace element {
} }
void GeometryParamPoly3::PreComputeSpline() { void GeometryParamPoly3::PreComputeSpline() {
size_t number_intervals = 1000; size_t number_intervals = 1000;
double delta_p = 1.0 / number_intervals; double delta_p = 1.0 / number_intervals;
if (_arcLength) { if (_arcLength) {
delta_p *= _length; delta_p *= _length;
} }
double param_p = 0; double param_p = 0;
double current_s = 0; double current_s = 0;
double last_u = 0; double last_u = 0;
double last_v = 0; double last_v = 0;
double last_s = 0; double last_s = 0;
RtreeValue last_val; RtreeValue last_val;
for(size_t i = 0; i < number_intervals; ++i) { for(size_t i = 0; i < number_intervals; ++i) {
double current_u = _polyU.Evaluate(param_p); double current_u = _polyU.Evaluate(param_p);
double current_v = _polyV.Evaluate(param_p); double current_v = _polyV.Evaluate(param_p);
double du = current_u - last_u; double du = current_u - last_u;
double dv = current_v - last_v; double dv = current_v - last_v;
double ds = sqrt(du * du + dv * dv); double ds = sqrt(du * du + dv * dv);
current_s += ds; current_s += ds;
double current_t_u = _polyU.Tangent(param_p); double current_t_u = _polyU.Tangent(param_p);
double current_t_v = _polyV.Tangent(param_p); double current_t_v = _polyV.Tangent(param_p);
RtreeValue current_val{ RtreeValue current_val{
current_u, current_u,
current_v, current_v,
current_s, current_s,
current_t_u, current_t_u,
current_t_v }; current_t_v };
Rtree::BPoint p1(static_cast<float>(last_s)); Rtree::BPoint p1(static_cast<float>(last_s));
Rtree::BPoint p2(static_cast<float>(current_s)); Rtree::BPoint p2(static_cast<float>(current_s));
_rtree.InsertElement(Rtree::BSegment(p1, p2), last_val, current_val); _rtree.InsertElement(Rtree::BSegment(p1, p2), last_val, current_val);
last_u = current_u; last_u = current_u;
last_v = current_v; last_v = current_v;
last_s = current_s; last_s = current_s;
last_val = current_val; last_val = current_val;
param_p += delta_p; param_p += delta_p;
if(current_s > _length){ if(current_s > _length){
break; break;
}
} }
}
} }
} // namespace element } // namespace element
} // namespace road } // namespace road