SUMO - Simulation of Urban MObility
 All Data Structures Namespaces Files Functions Variables Typedefs Enumerations Enumerator Friends Macros Modules Pages
BiArc.cpp
Go to the documentation of this file.
1 #include "BiArc.h"
2 #include <stdexcept>
3 #include <cassert>
4 #include <cmath> // added for cygwin build 2015-06-10 MB
5 
7 {
8  double k1, k2, k3, k4;
9  double L1, L2, L3, L4;
10 
11  double t0 = params.start_angle;
12  double t2 = params.end_angle;
13  double tjoin;
14 
16 
17  //degenerate case
18  if (L<eA){
19  params.K1 = HUGE;
20  params.K2 = HUGE;
21  params.E = HUGE;
22  return;
23  }
24 
25  double psi = atan2(params.end_pt.y()-params.start_pt.y(),params.end_pt.x()-params.start_pt.x());
26  if (psi<0) psi+=2*M_PI; //psi = [0,2*pi)
27 
28  params.flag = 0;
29 
30  L1 = -10;
31  L2 = -10;
32 
33  // due to the 0-2*pi discontinuity even for perfect arcs,
34  // (psi-(t2+t0)/2)~{-pi,0, pi} for cocircular solutions
35  // if (abs(mod(psi -(t2+t0)/2, pi))<eA) % this condition is not correct
36 
37  if (fabs(psi-(t2+t0)/2)<eA || fabs(psi-(t2+t0)/2+M_PI)<eA || fabs(psi-(t2+t0)/2-M_PI)<eA){
38  if (fabs(fmod(psi-t0,2*M_PI))<eA){ // straight line (still need to check mod 2*pi)
39  k1 = 0;
40  k2 = 0;
41  L1 = L;
42  L2 = 0;
43  }
44  else { // single arc
45  k1 = -4*sin((3*t0+t2)/4 - psi)*cos((t2-t0)/4)/L;
46  k2 = 0;
47  L1 = compute_arclength(t0,t2,k1);
48  L2 = 0;
49  }
50  //record the solutions the parameters
51  params.L1 = L1;
52  params.L2 = L2;
53  params.K1 = k1;
54  params.K2 = k2;
55  params.E = 0;
57  return;
58  }
59  else {
60  params.flag = 1; //truly a biarc
61 
62  k1 = -4*sin((3*t0+t2)/4 - psi)*cos((t2-t0)/4)/L;
63  k2 = 4*sin((t0+3*t2)/4 - psi)*cos((t2-t0)/4)/L;
64 
65  if (fabs(k1)<eK){
66  k1 = 0;
67  L1 = L*sin((t2+t0)/2-psi)/sin((t2-t0)/2);
68  L2 = compute_arclength(t0,t2,k2);
69  }
70  else {
71  if (fabs(k2)<eK){
72  k2 = 0;
73  L2 = L*sin((t2+t0)/2-psi)/sin((t0-t2)/2);
74  L1 = compute_arclength(t0,t2,k1);
75  }
76  else {
77  // tjoin will be incorrect if k1~0 or k2~0
78  tjoin = compute_join_theta(k1,k2);
79  L1 = compute_arclength(t0,tjoin,k1);
80  L2 = compute_arclength(tjoin,t2,k2);
81  }
82  }
83 
84  // the other possible biarc
85  L3 = -10;
86  L4 = -10;
87 
88  k3 = 4*cos((3*t0+t2)/4 - psi)*sin((t2-t0)/4)/L;
89  k4 = 4*cos((t0+3*t2)/4 - psi)*sin((t2-t0)/4)/L;
90 
91  // since this solution picks the biarc with the bigger turn
92  // the curvature solutions can still be close to zero
93  if ( (fabs(k3)>eK || fabs(k4)>eK) && fabs(k4-k3)>eK){
94  if (fabs(k3)<eK){
95  k3 = 0;
96  L3 = L*sin((t2+t0)/2-psi)/sin((t2-t0)/2);
97  L4 = compute_arclength(t0,t2,k4);
98  }
99  else {
100  if (fabs(k4)<eK){
101  k4 = 0;
102  L4 = L*sin((t2+t0)/2-psi)/sin((t0-t2)/2);
103  L3 = compute_arclength(t0,t2,k3);
104  }
105  else {
106  tjoin = compute_join_theta(k3,k4);
107  L3 = compute_arclength(t0,tjoin,k3);
108  L4 = compute_arclength(tjoin,t2,k4);
109  }
110  }
111  }
112 
113  // choose the smaller one
114  // but due to the epsilon settings (eA and eK) we could still get an incorrect solution
115  // this could be caught by looking at the signs of Ls.
116 
117  if ((L1>0 && L2>0) && ((L3<0 || L4<0) || (L1+L2)<(L3+L4))){
118  //k1 and k2 are the correct solutions
119  params.L1 = L1;
120  params.L2 = L2;
121  params.K1 = k1;
122  params.K2 = k2;
123  params.E = (k2-k1)*(k2-k1);
124  }
125  else {
126  if (L3>0 && L4>0){
127  params.L1 = L3;
128  params.L2 = L4;
129  params.K1 = k3;
130  params.K2 = k4;
131  params.E = (k3-k4)*(k3-k4);
132  }
133  else {
134  //this should never happen
135  throw std::runtime_error("Could not compute biarc parameters");
136  }
137  }
138  }
140 }
141 
143 {
144  if (params.K1 != 0){
145  params.R1 = fabs(1/params.K1);
148  double dt = params.L1*params.K1;
151  }
152  else {
155  params.R1 = HUGE;
156  }
157 
158  if (params.K2 != 0){
159  params.R2 = fabs(1/params.K2);
162  }
163  else {
164  params.R2 = HUGE;
165  }
166 
167  params.dir1 = params.K1<0 ? -1 : 1;//sign(params.K1); //CCW=+1
168  params.dir2 = params.K2<0 ? -1 : 1;//sign(params.K2);
169 }
170 
171 /* ---------------- BiArc support Functions --------------------- */
172 
173 double BiArc::compute_join_theta(double k1, double k2)
174 {
175  // compute the theta at which the two arcs meet
176  double x0=params.start_pt.x();
177  double y0=params.start_pt.y();
178  double x2=params.end_pt.x();
179  double y2=params.end_pt.y();
180  double t0=params.start_angle;
181  double t2=params.end_angle;
182 
183  double sin_join_theta = (k1*k2*(x2-x0)+k2*sin(t0)-k1*sin(t2))/(k2-k1);
184  double cos_join_theta = (-k1*k2*(y2-y0)+k2*cos(t0)-k1*cos(t2))/(k2-k1);
185 
186  double join_theta = atan2(sin_join_theta, cos_join_theta);
187  if (join_theta<0) join_theta += 2*M_PI;
188 
189  return join_theta;
190 }
191 
192 double BiArc::compute_arclength(double t0, double t1, double k)
193 {
194  double num = (t1-t0);
195 
196  if (k<0 && (t1-t0)>0)
197  num = num-2*M_PI;
198  else if (k>0 && (t1-t0)<0)
199  num = num+2*M_PI;
200 
201  return num/k;
202 }
#define eA
Definition: BiArc.h:34
double K2
Definition: BiArc.h:50
void compute_other_stuff()
Definition: BiArc.cpp:142
void setY(coord_type ny)
Definition: points.h:100
#define M_PI
Definition: angles.h:37
int flag
Definition: BiArc.h:41
double E
Definition: BiArc.h:55
Point2D< double > mid_pt
Definition: BiArc.h:63
double L1
Definition: BiArc.h:52
double start_angle
Definition: BiArc.h:46
#define eK
Definition: BiArc.h:35
double euc_distance(Point2D< point_type1 > pt1, Point2D< point_type2 > pt2)
Definition: points.h:245
Point2D< double > start_pt
Definition: BiArc.h:43
coord_type x() const
Definition: points.h:96
void compute_biarc_params()
Definition: BiArc.cpp:6
double L2
Definition: BiArc.h:53
double R1
Definition: BiArc.h:57
double K1
Definition: BiArc.h:49
BiArcParams params
Definition: BiArc.h:168
Point2D< double > end_pt
Definition: BiArc.h:44
double R2
Definition: BiArc.h:58
double compute_arclength(double theta0, double theta2, double k)
Definition: BiArc.cpp:192
double compute_join_theta(double k1, double k2)
Definition: BiArc.cpp:173
double end_angle
Definition: BiArc.h:47
int dir2
Definition: BiArc.h:61
void setX(coord_type nx)
Definition: points.h:99
Point2D< double > center1
Definition: BiArc.h:64
coord_type y() const
Definition: points.h:97
int dir1
Definition: BiArc.h:60
Point2D< double > center2
Definition: BiArc.h:65