Skip to content
Snippets Groups Projects
Commit 644af5b8 authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Added functions to normalize angles and compute angle differences.

parent 0faf9bc6
No related branches found
No related tags found
No related merge requests found
Hide whitespace changes
Inline Side-by-side
OpenRoadEd/create_junction_road.cpp
+ 43
− 21
View file @ 644af5b8
......@@ -71,8 +71,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
{
predecessor->GetGeometryBlock(0)->GetCoords(0,start_x,start_y,original_start_heading);
start_heading=original_start_heading+3.14158;
if(start_heading>2*3.14158)
start_heading-=2*3.14158;
start_heading=normalize_angle(start_heading);
}
else
{
......@@ -81,8 +80,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
if(predecessor->GetSuccessor()->GetElementType()=="junction" && predecessor->GetSuccessor()->GetElementId()==junction->GetId())
{
predecessor->GetLastGeometryBlock()->GetLastCoords(s,start_x,start_y,original_start_heading);
if(original_start_heading>2*3.14158)
original_start_heading-=2*3.14158;
original_start_heading=normalize_angle(original_start_heading);
start_heading=original_start_heading;
}
else
......@@ -116,7 +114,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
}
// get lane offset
lane_start_offset=0.0;
if(fabs(original_start_heading-start_heading)<0.01)
if(diff_angle(original_start_heading,start_heading)<0.01)
{
for(unsigned int i=0;i<lanes->size();i++)
{
......@@ -152,8 +150,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
if(successor->GetPredecessor()->GetElementType()=="junction" && successor->GetPredecessor()->GetElementId()==junction->GetId())
{
successor->GetGeometryBlock(0)->GetCoords(0,end_x,end_y,original_end_heading);
if(original_end_heading>2*3.14158)
original_end_heading-=2*3.14158;
original_end_heading=normalize_angle(original_end_heading);
end_heading=original_end_heading;
}
else
......@@ -164,8 +161,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
{
successor->GetLastGeometryBlock()->GetLastCoords(s,end_x,end_y,original_end_heading);
end_heading=original_end_heading+3.14158;
if(end_heading>2*3.14158)
end_heading-=2*3.14158;
end_heading=normalize_angle(end_heading);
}
else
{
......@@ -188,7 +184,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
// get lane offset
lanes=successor->GetLaneSection(0)->GetLaneVector();
lane_end_offset=0.0;
if(fabs(original_end_heading-end_heading)<0.01)
if(diff_angle(original_end_heading,end_heading)<0.01)
{
for(unsigned int i=0;i<lanes->size();i++)
{
......@@ -221,7 +217,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
// compute geometry
// LINE
if(fabs(start_heading-1.5707)<0.01 || fabs(start_heading-4.71238898)<0.01)
if(diff_angle(start_heading,1.5707)<0.01 || diff_angle(start_heading,4.71238898)<0.01)
{
line1=start_x;
line2=end_x;
......@@ -231,7 +227,7 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
line1=start_y-tan(start_heading)*start_x;
line2=end_y-tan(start_heading)*end_x;
}
if(fabs(line2-line1)<0.01 && fabs(start_heading-end_heading)<0.01)
if(fabs(line2-line1)<0.01 && diff_angle(start_heading,end_heading)<0.01)
{
if(road==NULL)
{
......@@ -245,11 +241,11 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
road->SetRoadName(name.str());
road->SetRoadId(id.str());
road->SetRoadJunction(junction->GetId());
if(predecessor->GetPredecessor()->GetElementType()=="junction")
if(predecessor->GetPredecessor()->GetElementType()=="junction" && predecessor->GetPredecessor()->GetElementId()==junction->GetId())
road->SetPredecessor("road", connection->GetIncomingRoad(), "start");
else
road->SetPredecessor("road", connection->GetIncomingRoad(), "end");
if(successor->GetPredecessor()->GetElementType()=="junction")
if(successor->GetPredecessor()->GetElementType()=="junction" && successor->GetPredecessor()->GetElementId()==junction->GetId())
road->SetSuccessor("road",connection->GetConnectingRoad(), "start");
else
road->SetSuccessor("road",connection->GetConnectingRoad(), "end");
......@@ -341,11 +337,8 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
new_geometry.AddGeometryLine(s,check_x,check_y,check_heading,end_length);
new_geometry.GetLastCoords(s,check_x,check_y,check_heading);
}
std::cout << end_heading << "," << check_heading << std::endl;
if(check_heading>2*3.14159)
check_heading-=2*3.14159;
std::cout << end_heading << "," << check_heading << std::endl;
if(fabs(end_x-check_x)>0.1 || fabs(end_y-check_y)>0.1 || fabs(end_heading-check_heading)>0.1)
check_heading=normalize_angle(check_heading);
if(fabs(end_x-check_x)>0.1 || fabs(end_y-check_y)>0.1 || diff_angle(end_heading,check_heading)>0.1)
{
std::cout << "Warning: Impossible to create connecting road from " << connection->GetIncomingRoad() << " lane " << lane_link->GetFrom() << " to " << connection->GetConnectingRoad() << " lane " << lane_link->GetTo() << std::endl;
return -1;
......@@ -362,11 +355,11 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
road->SetRoadName(name.str());
road->SetRoadId(id.str());
road->SetRoadJunction(junction->GetId());
if(predecessor->GetPredecessor()->GetElementType()=="junction")
if(predecessor->GetPredecessor()->GetElementType()=="junction" && predecessor->GetPredecessor()->GetElementId()==junction->GetId())
road->SetPredecessor("road", connection->GetIncomingRoad(), "start");
else
road->SetPredecessor("road", connection->GetIncomingRoad(), "end");
if(successor->GetPredecessor()->GetElementType()=="junction")
if(successor->GetPredecessor()->GetElementType()=="junction" && successor->GetPredecessor()->GetElementId()==junction->GetId())
road->SetSuccessor("road",connection->GetConnectingRoad(), "start");
else
road->SetSuccessor("road",connection->GetConnectingRoad(), "end");
......@@ -451,3 +444,32 @@ unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,uns
return road_index;
}
double normalize_angle(double angle)
{
double norm_angle;
if(angle>=2*3.14159)
norm_angle=angle-2*3.14159;
else if(angle<0.0)
norm_angle=angle+2*3.14159;
else
norm_angle=angle;
return norm_angle;
}
double diff_angle(double angle1,double angle2)
{
double diff1,diff2;
diff1=fabs(angle1-angle2);
if(angle1<angle2)
diff2=fabs(angle1+2*3.14159-angle2);
else
diff2=fabs(angle1-angle2-2*3.14159);
return std::min(diff1,diff2);
}
OpenRoadEd/create_junction_road.h
+ 2
− 0
View file @ 644af5b8
......@@ -6,5 +6,7 @@
#include "RoadTree.h"
unsigned int create_junction_road(RoadTree *road_tree, OpenDrive *open_drive,unsigned int junction_index,unsigned int connection_index,unsigned int lane_link_index);
double normalize_angle(double angle);
double diff_angle(double angle1,double angle2);
#endif
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment