diff --git a/include/osm/osm_junction.h b/include/osm/osm_junction.h
index 234ebfd86523eda758c4bed332cc8374e3a123c2..470d9a6f328b636e0303e3b79400fa4879f295b5 100644
--- a/include/osm/osm_junction.h
+++ b/include/osm/osm_junction.h
@@ -5,20 +5,32 @@
#include "osm/osm_node.h"
#include "osm/osm_road_segment.h"
+#include "g2_spline.h"
+
#include <Eigen/Dense>
#include <vector>
+typedef struct{
+ const COSMWay *in_way;
+ int in_lane_id;
+ const COSMWay *out_way;
+ int out_lane_id;
+ CG2Spline *geometry;
+}TOSMLink;
+
class COSMJunction
{
friend class COSMMap;
friend class COSMNode;
friend class COSMWay;
+ friend class COSMRoadSegment;
private:
std::vector<COSMRoadSegment *> in_roads;
std::vector<COSMRoadSegment *> out_roads;
COSMNode *parent_node;
std::vector<std::vector<Eigen::MatrixXi>> connections;
+ std::vector<TOSMLink> links;
protected:
void create_FF_connectivity_matrix(const COSMWay &in_way,const COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int num_out_lanes,Eigen::MatrixXi &matrix);
void create_FB_connectivity_matrix(const COSMWay &in_way,const COSMWay &out_way,unsigned int in_lane_index,unsigned int out_lane_index,unsigned int num_in_lanes,unsigned int num_out_lanes,unsigned int total_out_lanes,Eigen::MatrixXi &matrix);
@@ -27,10 +39,13 @@ class COSMJunction
void create_initial_connectivity(void);
void apply_node_restrictions(void);
void apply_way_restrictions(void);
- void compute_distances(void);
+ void create_links(void);
+ bool add_link(TOSMLink &new_link);
+ void generate_geometry(void);
public:
COSMJunction(COSMNode *node);
const COSMNode &get_parent_node(void) const;
+ void get_trajectory(std::vector<double> &x, std::vector<double> &y, std::vector<double> &heading, std::vector<double> &curvature);
friend std::ostream& operator<<(std::ostream& out, COSMJunction &junction);
~COSMJunction();
};
diff --git a/src/osm/osm_junction.cpp b/src/osm/osm_junction.cpp
index d72191b9ce1a5f58c44196ef0dd0b5964172a1b2..55b0da5ba066fe3f02faf38e846c1720e4c46af1 100644
--- a/src/osm/osm_junction.cpp
+++ b/src/osm/osm_junction.cpp
@@ -30,6 +30,8 @@ COSMJunction::COSMJunction(COSMNode *node)
}
this->create_initial_connectivity();
this->apply_node_restrictions();
+ this->apply_way_restrictions();
+ this->create_links();
/*
for(unsigned int i=0;i<this->in_roads.size();i++)
{
@@ -57,7 +59,7 @@ void COSMJunction::create_FF_connectivity_matrix(const COSMWay &in_way,const COS
// std::cout << "LEFT" << std::endl;
if((num_in_lanes-in_lane_index-1)==(num_out_lanes-out_lane_index-1))
matrix(in_lane_index,out_lane_index)=1;
- else if((out_lane_index>=num_in_lanes) && in_lane_index==0)
+ else if(((num_out_lanes-out_lane_index-1)>=num_in_lanes) && in_lane_index==0)
matrix(in_lane_index,out_lane_index)=1;
}
else if(in_way.is_node_right(*check_node,true))
@@ -89,7 +91,7 @@ void COSMJunction::create_FB_connectivity_matrix(const COSMWay &in_way,const COS
if(in_way.is_node_left(*check_node,true))
{
// std::cout << "LEFT" << std::endl;
- if(in_lane_index==(out_lane_index-(total_out_lanes-num_out_lanes)))
+ if((num_in_lanes-in_lane_index-1)==(out_lane_index-(total_out_lanes-num_out_lanes)))
matrix(in_lane_index,out_lane_index)=1;
else if(((out_lane_index-(total_out_lanes-num_out_lanes))>=num_in_lanes) && in_lane_index==0)
matrix(in_lane_index,out_lane_index)=1;
@@ -197,8 +199,9 @@ void COSMJunction::create_initial_connectivity(void)
{
for(unsigned int l=0;l<out_way.get_num_lanes();l++)
{
- if(in_way.get_id()==out_way.get_id())
+ if(in_way.get_id()!=out_way.get_id())
{
+ /*
if(out_way.is_node_first(this->parent_node->id))
{
if(k>=in_way.get_num_forward_lanes() && l<out_way.get_num_forward_lanes())
@@ -222,6 +225,7 @@ void COSMJunction::create_initial_connectivity(void)
}
else
{
+ */
if(in_way.is_one_way())
{
if(out_way.is_one_way())
@@ -300,16 +304,16 @@ void COSMJunction::apply_node_restrictions(void)
for(unsigned int k=0;k<this->parent_node->get_num_restrictions();k++)
{
const COSMRestriction &res=this->parent_node->get_restriction(k);
- switch(res.get_action())
+ for(unsigned int i=0;i<this->in_roads.size();i++)
{
- case RESTRICTION_NO:
-// std::cout << "Restriction: NO" << std::endl;
- for(unsigned int i=0;i<this->in_roads.size();i++)
+ const COSMWay &in_way=this->in_roads[i]->get_parent_way();
+ for(unsigned int j=0;j<this->out_roads.size();j++)
+ {
+ const COSMWay &out_way=this->out_roads[j]->get_parent_way();
+ switch(res.get_action())
{
- const COSMWay &in_way=this->in_roads[i]->get_parent_way();
- for(unsigned int j=0;j<this->out_roads.size();j++)
- {
- const COSMWay &out_way=this->out_roads[j]->get_parent_way();
+ case RESTRICTION_NO:
+// std::cout << "Restriction: NO" << std::endl;
if(out_way.is_node_first(this->parent_node->id))
check_node=&out_way.get_next_node_by_id(this->parent_node->id);
else
@@ -372,32 +376,19 @@ void COSMJunction::apply_node_restrictions(void)
default:
break;
}
- }
- }
- break;
- case RESTRICTION_ONLY:
-// std::cout << "Restriction: ONLY" << std::endl;
- for(unsigned int i=0;i<this->in_roads.size();i++)
- {
- const COSMWay &in_way=this->in_roads[i]->get_parent_way();
- for(unsigned int j=0;j<this->out_roads.size();j++)
- {
- const COSMWay &out_way=this->out_roads[j]->get_parent_way();
+ break;
+ case RESTRICTION_ONLY:
+// std::cout << "Restriction: ONLY" << std::endl;
switch(res.get_type())
{
case RESTRICTION_LEFT_TURN:
// std::cout << "Restriction: LEFT_TURN" << std::endl;
- if(in_way.get_id()==out_way.get_id())
- {
-// std::cout << "removing connection in node " << this->parent_node->id << std::endl;
- this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
- }
- else if(res.get_from_way().get_id()==in_way.get_id())
+ if(res.get_from_way().get_id()==in_way.get_id())
{
if(out_way.is_node_first(this->parent_node->id))
check_node=&out_way.get_next_node_by_id(this->parent_node->id);
- else
- check_node=&out_way.get_prev_node_by_id(this->parent_node->id);
+ else
+ check_node=&out_way.get_prev_node_by_id(this->parent_node->id);
if(in_way.is_node_last(this->parent_node->id))
{
if(!in_way.is_node_left(*check_node,true))
@@ -410,29 +401,6 @@ void COSMJunction::apply_node_restrictions(void)
{
if(!in_way.is_node_left(*check_node,false))
{
-// std::cout << "removing connection in node " << this->parent_node->id << std::endl;
- this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
- }
- }
- }
- else if(res.get_to_way().get_id()==out_way.get_id())
- {
- if(in_way.is_node_first(this->parent_node->id))
- check_node=&in_way.get_next_node_by_id(this->parent_node->id);
- else
- check_node=&in_way.get_prev_node_by_id(this->parent_node->id);
- if(out_way.is_node_last(this->parent_node->id))
- {
- if(!out_way.is_node_right(*check_node,true))
- {
-// std::cout << "removing connection in node " << this->parent_node->id << std::endl;
- this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
- }
- }
- else
- {
- if(!out_way.is_node_right(*check_node,false))
- {
// std::cout << "removing connection in node " << this->parent_node->id << std::endl;
this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
}
@@ -441,120 +409,98 @@ void COSMJunction::apply_node_restrictions(void)
break;
case RESTRICTION_RIGHT_TURN:
// std::cout << "Restriction: RIGHT_TURN" << std::endl;
- if(in_way.get_id()==out_way.get_id())
+ if(in_way.is_one_way())
{
-// std::cout << "removing connection in node " << this->parent_node->id << std::endl;
- this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
- }
- else if(res.get_from_way().get_id()==in_way.get_id())
- {
- if(out_way.is_node_first(this->parent_node->id))
- check_node=&out_way.get_next_node_by_id(this->parent_node->id);
- else
- check_node=&out_way.get_prev_node_by_id(this->parent_node->id);
- if(in_way.is_node_last(this->parent_node->id))
+ if(out_way.is_one_way())
{
- if(!in_way.is_node_right(*check_node,true))
+ if(res.get_from_way().get_id()==in_way.get_id() && res.get_to_way().get_id()==out_way.get_id())
{
-// std::cout << "removing connection in node " << this->parent_node->id << std::endl;
- this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+ if(out_way.is_node_first(this->parent_node->id))
+ check_node=&out_way.get_next_node_by_id(this->parent_node->id);
+ else
+ check_node=&out_way.get_prev_node_by_id(this->parent_node->id);
+ if(!in_way.is_node_right(*check_node,true))
+ this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
}
}
else
{
- if(!in_way.is_node_right(*check_node,false))
+ if(res.get_from_way().get_id()==in_way.get_id())
{
-// std::cout << "removing connection in node " << this->parent_node->id << std::endl;
- this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+ if(out_way.is_node_first(this->parent_node->id))
+ check_node=&out_way.get_next_node_by_id(this->parent_node->id);
+ else
+ check_node=&out_way.get_prev_node_by_id(this->parent_node->id);
+ if(!in_way.is_node_right(*check_node,true))
+ this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
}
}
}
- else if(res.get_to_way().get_id()==out_way.get_id())
+ else
{
- if(in_way.is_node_first(this->parent_node->id))
- check_node=&in_way.get_next_node_by_id(this->parent_node->id);
- else
- check_node=&in_way.get_prev_node_by_id(this->parent_node->id);
- if(out_way.is_node_last(this->parent_node->id))
- {
- if(!out_way.is_node_left(*check_node,true))
- {
-// std::cout << "removing connection in node " << this->parent_node->id << std::endl;
- this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
- }
- }
- else
+ if(res.get_from_way().get_id()==in_way.get_id())
{
- if(!out_way.is_node_left(*check_node,false))
- {
-// std::cout << "removing connection in node " << this->parent_node->id << std::endl;
+ if(out_way.is_node_first(this->parent_node->id))
+ check_node=&out_way.get_next_node_by_id(this->parent_node->id);
+ else
+ check_node=&out_way.get_prev_node_by_id(this->parent_node->id);
+ if(!in_way.is_node_right(*check_node,true))
this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
- }
}
}
break;
case RESTRICTION_STRAIGHT_ON:
// std::cout << "Restriction: STRAIGHT_ON" << std::endl;
- if(in_way.get_id()==out_way.get_id())
- {
-// std::cout << "removing connection in node " << this->parent_node->id << std::endl;
- this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
- }
- else if(res.get_from_way().get_id()==in_way.get_id())
+ if(in_way.is_one_way())
{
- if(out_way.is_node_first(this->parent_node->id))
- check_node=&out_way.get_next_node_by_id(this->parent_node->id);
- else
- check_node=&out_way.get_prev_node_by_id(this->parent_node->id);
- if(in_way.is_node_last(this->parent_node->id))
+ if(out_way.is_one_way())
{
- if(in_way.is_node_right(*check_node,true) || in_way.is_node_left(*check_node,true))
+ if(res.get_from_way().get_id()==in_way.get_id() && res.get_to_way().get_id()==out_way.get_id())
{
-// std::cout << "removing connection in node " << this->parent_node->id << std::endl;
- this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
+ if(out_way.is_node_first(this->parent_node->id))
+ check_node=&out_way.get_next_node_by_id(this->parent_node->id);
+ else
+ check_node=&out_way.get_prev_node_by_id(this->parent_node->id);
+ if(in_way.is_node_right(*check_node,true) || in_way.is_node_left(*check_node,true))
+ this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
}
}
else
{
- if(in_way.is_node_right(*check_node,false) || in_way.is_node_left(*check_node,false))
- {
-// std::cout << "removing connection in node " << this->parent_node->id << std::endl;
+ if(out_way.is_node_first(this->parent_node->id))
+ check_node=&out_way.get_next_node_by_id(this->parent_node->id);
+ else
+ check_node=&out_way.get_prev_node_by_id(this->parent_node->id);
+ if(in_way.is_node_right(*check_node,true) || in_way.is_node_left(*check_node,true))
this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
- }
}
}
- else if(res.get_to_way().get_id()==out_way.get_id())
- {
+ else
+ {
+ if(out_way.is_node_first(this->parent_node->id))
+ check_node=&out_way.get_next_node_by_id(this->parent_node->id);
+ else
+ check_node=&out_way.get_prev_node_by_id(this->parent_node->id);
if(in_way.is_node_first(this->parent_node->id))
- check_node=&in_way.get_next_node_by_id(this->parent_node->id);
- else
- check_node=&in_way.get_prev_node_by_id(this->parent_node->id);
- if(out_way.is_node_last(this->parent_node->id))
{
- if(out_way.is_node_right(*check_node,true) || out_way.is_node_left(*check_node,true))
- {
-// std::cout << "removing connection in node " << this->parent_node->id << std::endl;
+ if(in_way.is_node_right(*check_node,true) || in_way.is_node_left(*check_node,true))
this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
- }
}
else
{
- if(out_way.is_node_right(*check_node,false) || out_way.is_node_left(*check_node,false))
- {
-// std::cout << "removing connection in node " << this->parent_node->id << std::endl;
+ if(in_way.is_node_right(*check_node,false) || in_way.is_node_left(*check_node,false))
this->connections[i][j]=Eigen::MatrixXi::Zero(in_way.get_num_lanes(),out_way.get_num_lanes());
- }
}
}
break;
default:
break;
}
- }
+ break;
+ default:
+ break;
}
- break;
- default:
- break;
+ }
}
}
}
@@ -564,51 +510,211 @@ void COSMJunction::apply_way_restrictions(void)
}
-void COSMJunction::compute_distances(void)
+void COSMJunction::create_links(void)
{
- double angle,lane_width_in,lane_width_out,radius,dist,dist_offset;
- unsigned int num_lanes_in,num_lanes_out;
+ TOSMLink new_link;
+ const COSMNode *node1,*node3;
+ double in_lateral_offset,out_lateral_offset,length1,length2;
+ double angle,in_radius,out_radius,in_dist,out_dist;
+ bool left=false,right=false;
- for(unsigned int i=0;i<this->in_roads.size();i++)
+ for(unsigned int i=0;i<this->connections.size();i++)
{
const COSMWay &in_way=this->in_roads[i]->get_parent_way();
- const COSMNode &node1=in_way.get_segment_start_node(LAST_SEGMENT);
- num_lanes_in=in_way.get_num_lanes();
- lane_width_in=in_way.get_lane_width();
- for(unsigned int j=0;j<this->out_roads.size();j++)
+ for(unsigned int j=0;j<this->connections[i].size();j++)
{
const COSMWay &out_way=this->out_roads[j]->get_parent_way();
- if(in_way.get_id()==out_way.get_id())// don't allow U turn
- continue;
- const COSMNode &node2=out_way.get_segment_end_node(FIRST_SEGMENT);
- angle=this->parent_node->compute_angle(node1,node2);
- num_lanes_out=out_way.get_num_lanes();
- lane_width_out=out_way.get_lane_width();
- if(num_lanes_out<num_lanes_in)
- {
- radius=DEFAULT_MIN_RADIUS+(lane_width_in*(num_lanes_in/2.0-0.5))/sin(angle/2.0);
- dist=radius*cos(angle/2.0);
- dist_offset=((num_lanes_in/2.0-num_lanes_out/2.0)*lane_width_in)*cos(angle-3.14159);
- this->out_roads[i]->set_start_distance(dist+dist_offset);
- this->in_roads[j]->set_end_distance(dist);
- }
- else
+ for(unsigned int k=0;k<in_way.get_num_lanes();k++)
{
- radius=DEFAULT_MIN_RADIUS+(lane_width_out*(num_lanes_out/2.0-0.5))/sin(angle/2.0);
- dist=radius*cos(angle/2.0);
- dist_offset=((num_lanes_out/2.0-num_lanes_in/2.0)*lane_width_out)*cos(angle-3.14159);
- this->out_roads[i]->set_start_distance(dist);
- this->in_roads[j]->set_end_distance(dist+dist_offset);
+ for(unsigned int l=0;l<out_way.get_num_lanes();l++)
+ {
+ if(this->connections[i][j](k,l)==1)// link exists
+ {
+ new_link.in_way=&in_way;
+ new_link.out_way=&out_way;
+ new_link.in_lane_id=k;
+ new_link.out_lane_id=l;
+ new_link.geometry=NULL;
+ in_lateral_offset=(k+0.5-((double)in_way.get_num_lanes())/2.0)*in_way.get_lane_width();
+ out_lateral_offset=(l+0.5-((double)out_way.get_num_lanes())/2.0)*out_way.get_lane_width();
+// std::cout << "lateral offset: " << in_lateral_offset << "," << out_lateral_offset << std::endl;
+ if(out_way.is_node_first(this->parent_node->get_id()))
+ node3=&out_way.get_segment_end_node(FIRST_SEGMENT);
+ else
+ node3=&out_way.get_segment_start_node(LAST_SEGMENT);
+ if(in_way.is_node_first(this->parent_node->get_id()))
+ {
+ node1=&in_way.get_segment_end_node(FIRST_SEGMENT);
+ left=in_way.is_node_left(*node3,false,0.5);
+ right=in_way.is_node_right(*node3,false,0.5);
+ }
+ else
+ {
+ node1=&in_way.get_segment_start_node(LAST_SEGMENT);
+ left=in_way.is_node_left(*node3,true,0.5);
+ right=in_way.is_node_right(*node3,true,0.5);
+ }
+ angle=this->parent_node->compute_angle(*node1,*node3);
+ if(right)//right
+ {
+ if(in_way.is_node_first(this->parent_node->get_id()))
+ in_radius=DEFAULT_MIN_RADIUS+in_lateral_offset;
+ else
+ in_radius=DEFAULT_MIN_RADIUS-in_lateral_offset;
+ if(out_way.is_node_first(this->parent_node->get_id()))
+ out_radius=DEFAULT_MIN_RADIUS+out_lateral_offset;
+ else
+ out_radius=DEFAULT_MIN_RADIUS-out_lateral_offset;
+ in_dist=fabs(in_radius/tan(angle/2.0));
+ out_dist=fabs(out_radius/tan(angle/2.0));
+ }
+ else if(left) //left
+ {
+ if(in_way.is_node_first(this->parent_node->get_id()))
+ in_radius=DEFAULT_MIN_RADIUS-in_lateral_offset;
+ else
+ in_radius=DEFAULT_MIN_RADIUS+in_lateral_offset;
+ if(out_way.is_node_first(this->parent_node->get_id()))
+ out_radius=DEFAULT_MIN_RADIUS-out_lateral_offset;
+ else
+ out_radius=DEFAULT_MIN_RADIUS+out_lateral_offset;
+ in_dist=fabs(in_radius/tan(angle/2.0));
+ out_dist=fabs(out_radius/tan(angle/2.0));
+ }
+ else
+ {
+ length1=this->parent_node->compute_distance(*node1);
+ length2=this->parent_node->compute_distance(*node3);
+ in_dist=std::min(length1,length2)/2.0;
+ if(in_dist>DEFAULT_MIN_RADIUS)
+ in_dist=DEFAULT_MIN_RADIUS;
+ out_dist=in_dist;
+ }
+ if(this->add_link(new_link))
+ {
+ if(in_way.is_node_first(this->parent_node->get_id()))
+ in_way.road_segment->set_start_distance(in_dist);
+ else
+ in_way.road_segment->set_end_distance(in_dist);
+ if(out_way.is_node_first(this->parent_node->get_id()))
+ out_way.road_segment->set_start_distance(out_dist);
+ else
+ out_way.road_segment->set_end_distance(out_dist);
+ }
+ }
+ }
}
}
}
}
+bool COSMJunction::add_link(TOSMLink &new_link)
+{
+ for(unsigned int i=0;i<this->links.size();i++)
+ {
+ if(new_link.in_way->get_id()==this->links[i].in_way->get_id() && new_link.in_lane_id==this->links[i].in_lane_id && new_link.out_way->get_id()==this->links[i].out_way->get_id() && new_link.out_lane_id==this->links[i].out_lane_id)
+ return false;
+ }
+ this->links.push_back(new_link);
+
+ return true;
+}
+
+void COSMJunction::generate_geometry(void)
+{
+ const COSMNode *start_node,*end_node;
+ TPoint start_point,end_point;
+ double x_offset,y_offset,x,y,heading;
+
+ for(unsigned int i=0;i<this->links.size();i++)
+ {
+ y_offset=(this->links[i].in_lane_id+0.5-((double)this->links[i].in_way->get_num_lanes())/2.0)*this->links[i].in_way->get_lane_width();
+ if(this->links[i].in_way->is_node_first(this->parent_node->id))
+ {
+ start_node=&this->links[i].in_way->get_segment_start_node(FIRST_SEGMENT);
+ end_node=&this->links[i].in_way->get_segment_end_node(FIRST_SEGMENT);
+ start_node->get_location(x,y);
+ heading=start_node->compute_heading(*end_node);
+ x_offset=this->links[i].in_way->get_road_segment().get_start_distance();
+ start_point.x=x+x_offset*cos(heading)-y_offset*sin(heading);
+ start_point.y=y+x_offset*sin(heading)+y_offset*cos(heading);
+ heading+=3.14159;
+ if(heading>3.14159)
+ heading-=2.0*3.14159;
+ else if(heading<-3.14159)
+ heading+=2.0*3.14159;
+ start_point.heading=heading;
+ start_point.curvature=0.0;
+ }
+ else
+ {
+ start_node=&this->links[i].in_way->get_segment_start_node(LAST_SEGMENT);
+ end_node=&this->links[i].in_way->get_segment_end_node(LAST_SEGMENT);
+ end_node->get_location(x,y);
+ heading=start_node->compute_heading(*end_node);
+ x_offset=-this->links[i].in_way->get_road_segment().get_end_distance();
+ start_point.x=x+x_offset*cos(heading)-y_offset*sin(heading);
+ start_point.y=y+x_offset*sin(heading)+y_offset*cos(heading);
+ start_point.heading=heading;
+ start_point.curvature=0.0;
+ }
+ y_offset=(this->links[i].out_lane_id+0.5-((double)this->links[i].out_way->get_num_lanes())/2.0)*this->links[i].out_way->get_lane_width();
+ if(this->links[i].out_way->is_node_first(this->parent_node->id))
+ {
+ start_node=&this->links[i].out_way->get_segment_start_node(FIRST_SEGMENT);
+ end_node=&this->links[i].out_way->get_segment_end_node(FIRST_SEGMENT);
+ start_node->get_location(x,y);
+ heading=start_node->compute_heading(*end_node);
+ x_offset=this->links[i].out_way->get_road_segment().get_start_distance();
+ end_point.x=x+x_offset*cos(heading)-y_offset*sin(heading);
+ end_point.y=y+x_offset*sin(heading)+y_offset*cos(heading);
+ end_point.heading=heading;
+ end_point.curvature=0.0;
+ }
+ else
+ {
+ start_node=&this->links[i].out_way->get_segment_start_node(LAST_SEGMENT);
+ end_node=&this->links[i].out_way->get_segment_end_node(LAST_SEGMENT);
+ end_node->get_location(x,y);
+ heading=start_node->compute_heading(*end_node);
+ x_offset=-this->links[i].out_way->get_road_segment().get_end_distance();
+ end_point.x=x+x_offset*cos(heading)-y_offset*sin(heading);
+ end_point.y=y+x_offset*sin(heading)+y_offset*cos(heading);
+ heading+=3.14159;
+ if(heading>3.14159)
+ heading-=2.0*3.14159;
+ else if(heading<-3.14159)
+ heading+=2.0*3.14159;
+ end_point.heading=heading;
+ end_point.curvature=0.0;
+ }
+ this->links[i].geometry=new CG2Spline(start_point,end_point);
+ }
+}
+
const COSMNode &COSMJunction::get_parent_node(void) const
{
return *this->parent_node;
}
+void COSMJunction::get_trajectory(std::vector<double> &x, std::vector<double> &y, std::vector<double> &heading, std::vector<double> &curvature)
+{
+ std::vector<double> new_x,new_y,new_heading,new_curvature;
+
+ x.clear();
+ y.clear();
+ heading.clear();
+ curvature.clear();
+ for(unsigned int i=0;i<this->links.size();i++)
+ {
+ this->links[i].geometry->evaluate_all(new_x,new_y,new_curvature,new_heading);
+ x.insert(x.end(),new_x.begin(),new_x.end());
+ y.insert(y.end(),new_y.begin(),new_y.end());
+ heading.insert(heading.end(),new_heading.begin(),new_heading.end());
+ curvature.insert(curvature.end(),new_curvature.begin(),new_curvature.end());
+ }
+}
+
std::ostream& operator<<(std::ostream& out, COSMJunction &junction)
{
out << " ID: " << junction.parent_node->get_id() << std::endl;
@@ -627,4 +733,13 @@ COSMJunction::~COSMJunction()
this->parent_node=NULL;
this->in_roads.clear();
this->out_roads.clear();
+ for(unsigned int i=0;i<this->links.size();i++)
+ {
+ if(this->links[i].geometry!=NULL)
+ {
+ delete this->links[i].geometry;
+ this->links[i].geometry=NULL;
+ }
+ }
+ this->links.clear();
}