BiArc Class Reference

#include <BiArc.h>

Detailed Description

Definition at line 165 of file BiArc.h.


Public Member Functions

 BiArc (Point2D< double > start_pt, double start_angle, Point2D< double > end_pt, double end_angle)
 BiArc ()
double compute_arclength (double theta0, double theta2, double k)
void compute_biarc_params ()
double compute_join_theta (double k1, double k2)
void compute_other_stuff ()
void set_end_params (Point2D< double > end_pt, double end_angle)
void set_start_params (Point2D< double > start_pt, double start_angle)
 ~BiArc ()

Data Fields

BiArcParams params

Constructor & Destructor Documentation

BiArc::BiArc (  )  [inline]

Definition at line 170 of file BiArc.h.

00170 {}

BiArc::BiArc ( Point2D< double >  start_pt,
double  start_angle,
Point2D< double >  end_pt,
double  end_angle 
) [inline]

Definition at line 171 of file BiArc.h.

References angle0To2Pi(), compute_biarc_params(), BiArcParams::end_angle, BiArcParams::end_pt, params, BiArcParams::start_angle, and BiArcParams::start_pt.

00172   {
00173     params.start_pt = start_pt;
00174     params.start_angle = angle0To2Pi(start_angle);
00175 
00176       params.end_pt = end_pt;
00177     params.end_angle = angle0To2Pi(end_angle);
00178 
00179     //since we have all the parameters, we might as well compute it
00180     compute_biarc_params();
00181   }

BiArc::~BiArc (  )  [inline]

Definition at line 183 of file BiArc.h.

00183 {}

Member Function Documentation

double BiArc::compute_arclength ( double  theta0,
double  theta2,
double  k 
)

Definition at line 190 of file BiArc.cpp.

References M_PI.

Referenced by compute_biarc_params().

00191 {
00192   double num = (t1-t0);
00193 
00194   if (k<0 && (t1-t0)>0)
00195      num = num-2*M_PI;
00196   else if (k>0 && (t1-t0)<0)
00197      num = num+2*M_PI;
00198 
00199   return num/k;
00200 }

void BiArc::compute_biarc_params ( void   ) 

Definition at line 4 of file BiArc.cpp.

References compute_arclength(), compute_join_theta(), compute_other_stuff(), BiArcParams::E, eA, eK, BiArcParams::end_angle, BiArcParams::end_pt, euc_distance(), BiArcParams::flag, BiArcParams::K1, BiArcParams::K2, BiArcParams::L1, BiArcParams::L2, M_PI, params, BiArcParams::start_angle, BiArcParams::start_pt, Point2D< coord_type >::x(), and Point2D< coord_type >::y().

Referenced by BiArc(), and EulerSpiral::compute_es_params().

00005 {
00006   double k1, k2, k3, k4;
00007   double L1, L2, L3, L4;
00008 
00009   double t0 = params.start_angle;
00010   double t2 = params.end_angle;
00011   double tjoin;
00012 
00013   double L = euc_distance(params.start_pt, params.end_pt);
00014 
00015   //degenerate case
00016   if (L<eA){
00017     params.K1 = HUGE;
00018     params.K2 = HUGE;
00019     params.E = HUGE;
00020     return;
00021   }
00022 
00023   double psi = atan2(params.end_pt.y()-params.start_pt.y(),params.end_pt.x()-params.start_pt.x());
00024   if (psi<0) psi+=2*M_PI; //psi = [0,2*pi)
00025 
00026   params.flag = 0;
00027 
00028   L1 = -10;
00029   L2 = -10;
00030 
00031   // due to the 0-2*pi discontinuity even for perfect arcs,
00032   // (psi-(t2+t0)/2)~{-pi,0, pi} for cocircular solutions
00033   // if (abs(mod(psi -(t2+t0)/2, pi))<eA) % this condition is not correct
00034 
00035   if (fabs(psi-(t2+t0)/2)<eA || fabs(psi-(t2+t0)/2+M_PI)<eA || fabs(psi-(t2+t0)/2-M_PI)<eA){
00036     if (fabs(fmod(psi-t0,2*M_PI))<eA){    // straight line (still need to check mod 2*pi)
00037       k1 = 0;
00038       k2 = 0;
00039       L1 = L;
00040       L2 = 0;
00041     } 
00042     else {    // single arc
00043       k1 = -4*sin((3*t0+t2)/4 - psi)*cos((t2-t0)/4)/L;
00044       k2 = 0;
00045       L1 = compute_arclength(t0,t2,k1);
00046       L2 = 0;
00047     }
00048     //record the solutions the parameters
00049     params.L1 = L1;
00050     params.L2 = L2;
00051     params.K1 = k1;
00052     params.K2 = k2;
00053     params.E = 0;
00054     compute_other_stuff();
00055     return;
00056   } 
00057   else {
00058     params.flag = 1;   //truly a biarc
00059 
00060     k1 = -4*sin((3*t0+t2)/4 - psi)*cos((t2-t0)/4)/L;
00061     k2 = 4*sin((t0+3*t2)/4 - psi)*cos((t2-t0)/4)/L;
00062 
00063     if (fabs(k1)<eK){
00064       k1 = 0;
00065       L1 = L*sin((t2+t0)/2-psi)/sin((t2-t0)/2);
00066       L2 = compute_arclength(t0,t2,k2);
00067     }
00068     else {
00069       if (fabs(k2)<eK){
00070         k2 = 0;
00071         L2 = L*sin((t2+t0)/2-psi)/sin((t0-t2)/2);
00072         L1 = compute_arclength(t0,t2,k1);
00073       }
00074       else {
00075         // tjoin will be incorrect if k1~0 or k2~0
00076         tjoin = compute_join_theta(k1,k2);
00077         L1 = compute_arclength(t0,tjoin,k1);
00078         L2 = compute_arclength(tjoin,t2,k2);
00079       }
00080     }
00081 
00082     // the other possible biarc
00083     L3 = -10;
00084     L4 = -10;
00085 
00086     k3 = 4*cos((3*t0+t2)/4 - psi)*sin((t2-t0)/4)/L;
00087     k4 = 4*cos((t0+3*t2)/4 - psi)*sin((t2-t0)/4)/L;
00088 
00089     // since this solution picks the biarc with the bigger turn
00090     // the curvature solutions can still be close to zero
00091     if ( (fabs(k3)>eK || fabs(k4)>eK) && fabs(k4-k3)>eK){
00092       if (fabs(k3)<eK){
00093         k3 = 0;
00094         L3 = L*sin((t2+t0)/2-psi)/sin((t2-t0)/2);
00095         L4 = compute_arclength(t0,t2,k4);
00096       }
00097       else {
00098         if (fabs(k4)<eK){
00099           k4 = 0;
00100           L4 = L*sin((t2+t0)/2-psi)/sin((t0-t2)/2);
00101           L3 = compute_arclength(t0,t2,k3);
00102         }
00103         else {
00104           tjoin = compute_join_theta(k3,k4);
00105           L3 = compute_arclength(t0,tjoin,k3);
00106           L4 = compute_arclength(tjoin,t2,k4);
00107         }
00108       }
00109     }
00110 
00111     // choose the smaller one
00112     // but due to the epsilon settings (eA and eK) we could still get an incorrect solution
00113     // this could be caught by looking at the signs of Ls.
00114 
00115     if ((L1>0 && L2>0) && ((L3<0 || L4<0) || (L1+L2)<(L3+L4))){
00116       //k1 and k2 are the correct solutions
00117       params.L1 = L1;
00118       params.L2 = L2;
00119       params.K1 = k1;
00120       params.K2 = k2;
00121       params.E = (k2-k1)*(k2-k1); 
00122     }
00123     else { 
00124       if (L3>0 && L4>0){
00125         params.L1 = L3;
00126         params.L2 = L4;
00127         params.K1 = k3;
00128         params.K2 = k4;
00129         params.E = (k3-k4)*(k3-k4);
00130       }
00131       else {
00132         //this should never happen
00133         assert(false);
00134       }
00135     }
00136   }
00137   compute_other_stuff();
00138 }

double BiArc::compute_join_theta ( double  k1,
double  k2 
)

Definition at line 171 of file BiArc.cpp.

References BiArcParams::end_angle, BiArcParams::end_pt, M_PI, params, BiArcParams::start_angle, BiArcParams::start_pt, Point2D< coord_type >::x(), and Point2D< coord_type >::y().

Referenced by compute_biarc_params().

00172 {
00173   // compute the theta at which the two arcs meet
00174   double x0=params.start_pt.x();
00175   double y0=params.start_pt.y();
00176   double x2=params.end_pt.x();
00177   double y2=params.end_pt.y();
00178   double t0=params.start_angle;
00179   double t2=params.end_angle;
00180 
00181   double sin_join_theta = (k1*k2*(x2-x0)+k2*sin(t0)-k1*sin(t2))/(k2-k1);
00182   double cos_join_theta = (-k1*k2*(y2-y0)+k2*cos(t0)-k1*cos(t2))/(k2-k1);
00183 
00184   double join_theta = atan2(sin_join_theta, cos_join_theta);
00185   if (join_theta<0) join_theta += 2*M_PI;
00186 
00187   return join_theta;
00188 }

void BiArc::compute_other_stuff ( void   ) 

Definition at line 140 of file BiArc.cpp.

References BiArcParams::center1, BiArcParams::center2, BiArcParams::dir1, BiArcParams::dir2, BiArcParams::end_angle, BiArcParams::end_pt, BiArcParams::K1, BiArcParams::K2, BiArcParams::L1, M_PI, BiArcParams::mid_pt, params, BiArcParams::R1, BiArcParams::R2, Point2D< coord_type >::setX(), Point2D< coord_type >::setY(), BiArcParams::start_angle, BiArcParams::start_pt, Point2D< coord_type >::x(), and Point2D< coord_type >::y().

Referenced by compute_biarc_params().

00141 {
00142   if (params.K1 != 0){
00143     params.R1 = fabs(1/params.K1);
00144     params.center1.setX(params.start_pt.x() - cos(params.start_angle-M_PI/2)/params.K1);
00145     params.center1.setY(params.start_pt.y() - sin(params.start_angle-M_PI/2)/params.K1);
00146     double dt = params.L1*params.K1;
00147     params.mid_pt.setX(params.center1.x() - cos(params.start_angle+M_PI/2+dt)/params.K1);
00148     params.mid_pt.setY(params.center1.y() - sin(params.start_angle+M_PI/2+dt)/params.K1);
00149   }
00150   else {
00151     params.mid_pt.setX(params.start_pt.x() + cos(params.start_angle)*params.L1);
00152     params.mid_pt.setY(params.start_pt.y() + sin(params.start_angle)*params.L1);
00153     params.R1 = HUGE;
00154   }
00155 
00156   if (params.K2 != 0){
00157     params.R2 = fabs(1/params.K2);
00158     params.center2.setX(params.end_pt.x() - cos(params.end_angle-M_PI/2)/params.K2);
00159     params.center2.setY(params.end_pt.y() - sin(params.end_angle-M_PI/2)/params.K2);
00160   }
00161   else {
00162     params.R2 = HUGE;
00163   }
00164 
00165   params.dir1 = params.K1<0 ? -1. : 1.;//sign(params.K1); //CCW=+1
00166   params.dir2 = params.K2<0 ? -1. : 1.;//sign(params.K2);
00167 }

void BiArc::set_end_params ( Point2D< double >  end_pt,
double  end_angle 
) [inline]

Definition at line 196 of file BiArc.h.

References angle0To2Pi(), BiArcParams::end_angle, BiArcParams::end_pt, and params.

Referenced by EulerSpiral::compute_es_params().

00197   {
00198     params.end_pt = end_pt;
00199     params.end_angle = angle0To2Pi(end_angle);
00200   }

void BiArc::set_start_params ( Point2D< double >  start_pt,
double  start_angle 
) [inline]

Definition at line 190 of file BiArc.h.

References angle0To2Pi(), params, BiArcParams::start_angle, and BiArcParams::start_pt.

Referenced by EulerSpiral::compute_es_params().

00191   {
00192     params.start_pt = start_pt;
00193     params.start_angle = angle0To2Pi(start_angle);
00194   }

Field Documentation

BiArcParams BiArc::params

Definition at line 168 of file BiArc.h.

Referenced by BiArc(), compute_biarc_params(), EulerSpiral::compute_es_params(), compute_join_theta(), compute_other_stuff(), set_end_params(), and set_start_params().

The documentation for this class was generated from the following files:
Generated on Wed May 5 00:06:40 2010 for Sumo - Simulation of Urban MObility by  doxygen 1.5.6