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