EulerSpiral Class Reference

#include <euler.h>

---

Detailed Description

Definition at line 139 of file euler.h.

Public Member Functions
Point2D< double >	compute_end_pt (double k0, double gamma, double L, bool bNormalized=false)
Point2D< double >	compute_end_pt (double arclength, bool bNormalized=false)
double	compute_error (double k0, double L)
void	compute_es_params ()
Point2D< double >	compute_es_point (EulerSpiralParams &es_params, double arclength, bool bNormalized=false)
void	computeSpiral (std::vector< Point2D< double > > &spiral, double ds=0, int NPts=0)
	EulerSpiral (Point2D< double > start_pt, double start_angle, double k0, double gamma, double L)
	EulerSpiral (Point2D< double > start_pt, double start_angle, Point2D< double > end_pt, double end_angle)
	EulerSpiral ()
Point2D< double >	get_fresnel_integral (double value)
void	set_end_params (Point2D< double > end_pt, double end_angle)
void	set_params (Point2D< double > start_pt, double start_angle, Point2D< double > end_pt, double end_angle)
void	set_start_params (Point2D< double > start_pt, double start_angle)
Data Fields
EulerSpiralParams	params
std::vector< Point2D< double > >	pts
Private Attributes
BiArc	_bi_arc_estimate

---

Constructor & Destructor Documentation

EulerSpiral::EulerSpiral

(

)

[inline]

Definition at line 148 of file euler.h.

{};

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

Definition at line 151 of file euler.h.

References angle0To2Pi(), compute_es_params(), EulerSpiralParams::end_angle, EulerSpiralParams::end_pt, params, EulerSpiralParams::start_angle, and EulerSpiralParams::start_pt.

  {
    params.start_pt = start_pt;
    params.start_angle = angle0To2Pi(start_angle);

    params.end_pt = end_pt;
    params.end_angle = angle0To2Pi(end_angle);

    //since we have all the parameters, we might as well compute it
    compute_es_params();
  }

EulerSpiral::EulerSpiral	(	Point2D< double >	start_pt,
		double	start_angle,
		double	k0,
		double	gamma,
		double	L
	)			[inline]

Definition at line 164 of file euler.h.

References angle0To2Pi(), compute_end_pt(), compute_es_params(), EulerSpiralParams::end_angle, EulerSpiralParams::end_pt, params, EulerSpiralParams::start_angle, and EulerSpiralParams::start_pt.

  {
    params.start_pt = start_pt;
    params.start_angle = angle0To2Pi(start_angle);
    params.end_pt = compute_end_pt(k0, gamma, L);
    params.end_angle = start_angle + 0.5*gamma*L*L + k0*L;

    //since we have all the parameters, we might as well compute it
    compute_es_params();
  }

---

Member Function Documentation

Point2D< double > EulerSpiral::compute_end_pt	(	double	k0,
		double	gamma,
		double	L,
		bool	bNormalized = false
	)

Definition at line 400 of file euler.cpp.

References CCW(), eGamma, eK, get_fresnel_integral(), Point2D< coord_type >::getX(), Point2D< coord_type >::getY(), M_PI, params, EulerSpiralParams::psi, Point2D< coord_type >::setX(), Point2D< coord_type >::setY(), EulerSpiralParams::start_angle, and EulerSpiralParams::start_pt.

{
  Point2D<double> start_pt;
  Point2D<double> end_pt;

  double theta;

  if (bNormalized){
    start_pt = Point2D<double>(0,0);
    theta = CCW(params.psi, params.start_angle);
  }
  else {
    start_pt = params.start_pt;
    theta = params.start_angle;
  }

  if (L==0)
    return start_pt;

  if (fabs(gamma)<eGamma) 
  { 
    if (fabs(k0)<eK)
    {
      //straight line
      end_pt.setX(start_pt.getX()+L*cos(theta));
      end_pt.setY(start_pt.getY()+L*sin(theta));
    }
    else 
    { 
      //circle
      double const_term = 1.0/k0;
      end_pt.setX(start_pt.getX()+const_term*(sin(k0*L+theta)-sin(theta)));
      end_pt.setY(start_pt.getY()-const_term*(cos(k0*L+theta)-cos(theta))); 
    }
    return end_pt;
  }

  double const1 = sqrt(M_PI*fabs(gamma));
  double const2 = sqrt(M_PI/fabs(gamma));

  Point2D<double> fresnel1 = get_fresnel_integral((k0+gamma*L)/const1);
  Point2D<double> fresnel2 = get_fresnel_integral(k0/const1);

  double C = (fresnel1.getX() - fresnel2.getX());
  if(gamma<0) {
      C *= -1.;
  }
  double S = fresnel1.getY() - fresnel2.getY();

  double cos_term = cos(theta-((k0*k0)/(2.0*gamma)));
  double sin_term = sin(theta-((k0*k0)/(2.0*gamma))); 
  
  end_pt.setX(start_pt.getX() + const2*(C*cos_term - S*sin_term));
  end_pt.setY(start_pt.getY() + const2*(C*sin_term + S*cos_term));

  return end_pt;
}

Point2D< double > EulerSpiral::compute_end_pt	(	double	arclength,
		bool	bNormalized = false
	)

Definition at line 395 of file euler.cpp.

References EulerSpiralParams::gamma, EulerSpiralParams::K0, and params.

Referenced by compute_error(), compute_es_point(), computeSpiral(), and EulerSpiral().

{
  return compute_end_pt(params.K0, params.gamma, arclength, bNormalized);
}

double EulerSpiral::compute_error	(	double	k0,
		double	L
	)			[inline]

Definition at line 458 of file euler.cpp.

References compute_end_pt(), euc_distance(), params, and EulerSpiralParams::turningAngle.

Referenced by compute_es_params().

{
  //assumes normalized parameters

  //compute the endpoint of the Euler spiral with the given intrinsic parameters
  double gamma = 2*(params.turningAngle - k0*L)/(L*L);
  Point2D<double> cur_end_pt = compute_end_pt(k0, gamma, L, true);

  //the error is the distance between the current end point and the desired end point
  return euc_distance(Point2D<double>(1,0), cur_end_pt);
}

void EulerSpiral::compute_es_params

(

)

Definition at line 288 of file euler.cpp.

References _bi_arc_estimate, angle0To2Pi(), BiArc::compute_biarc_params(), compute_error(), eError, EulerSpiralParams::end_angle, EulerSpiralParams::end_pt, EulerSpiralParams::error, euc_distance(), EulerSpiralParams::gamma, EulerSpiralParams::K0, BiArcParams::K1, EulerSpiralParams::K2, BiArcParams::K2, EulerSpiralParams::L, BiArcParams::L(), BiArcParams::L1, BiArcParams::L2, MAX_NUM_ITERATIONS, MIN2(), BiArc::params, params, EulerSpiralParams::psi, BiArc::set_end_params(), BiArc::set_start_params(), EulerSpiralParams::start_angle, EulerSpiralParams::start_pt, EulerSpiralParams::turningAngle, Point2D< coord_type >::x(), and Point2D< coord_type >::y().

Referenced by EulerSpiral().

{
  //compute scaling distance
  double d = euc_distance(params.start_pt, params.end_pt);
  params.psi = angle0To2Pi(atan2(params.end_pt.y()-params.start_pt.y(),params.end_pt.x()-params.start_pt.x()));

  //degeneracy check
  if (d<eError)
    return; 

  //first compute a biarc estimate
  _bi_arc_estimate.set_start_params(params.start_pt, params.start_angle);
  _bi_arc_estimate.set_end_params(params.end_pt, params.end_angle);
  _bi_arc_estimate.compute_biarc_params();

  //get the total turning angle::This is an important parameter because
  //it defines the one solution out of many possible solutions
  params.turningAngle = _bi_arc_estimate.params.K1*_bi_arc_estimate.params.L1 +
                 _bi_arc_estimate.params.K2*_bi_arc_estimate.params.L2;

  //From here on, normlize the parameters and use these to perform the optimization

  double k0_init_est = _bi_arc_estimate.params.K1*d;
  double L_init_est = _bi_arc_estimate.params.L()/d;
  double dstep = 0.1;

  //Alternately, we can get the initial values from the lookup table and perform 
  //the optimization from there
  //double k0_init_est = globalEulerSpiralLookupTable->get_globalEulerSpiralLookupTable()->k0(CCW(params.psi, params.start_angle), CCW(params.psi, params.end_angle));
  //double L_init_est = globalEulerSpiralLookupTable->get_globalEulerSpiralLookupTable()->L(CCW(params.psi, params.start_angle), CCW(params.psi, params.end_angle));
  //double dstep = globalEulerSpiralLookupTable->get_globalEulerSpiralLookupTable()->dt()/4;

  //then perform a simple gradient descent to find the real solution
  double error = compute_error(k0_init_est, L_init_est);
  double prev_error = error;
  
  double k0 = k0_init_est;
  double L = L_init_est;

  double e1, e2, e3, e4;

  for (int i=0;i<MAX_NUM_ITERATIONS;i++)
  {
     if (error<eError)
        break;
    
     e1 = compute_error(k0 + dstep, L);
     e2 = compute_error(k0 - dstep, L);
     e3 = compute_error(k0, L + dstep);
     if (L>dstep)  e4 = compute_error(k0, L - dstep);
    
     error = MIN2(MIN2(e1,e2),MIN2(e3,e4));
    
     if (error>prev_error)
     {
       dstep = dstep/2;
       continue;
     }
    
     if    (error==e1)  k0 = k0 + dstep;
     else if (error==e2) k0 = k0 - dstep;
     else if (error==e3) L = L + dstep;
     else if (error==e4) L = L - dstep;
      
     prev_error = error;
  }

  //store the parameters
  params.K0 = k0/d;
  params.L = L*d;
  params.gamma = 2*(params.turningAngle - k0*L)/(L*L)/(d*d);
  params.K2 = (k0 + params.gamma*L)/d;
  params.error = error;
}

Point2D< double > EulerSpiral::compute_es_point	(	EulerSpiralParams &	es_params,
		double	arclength,
		bool	bNormalized = false
	)

Definition at line 389 of file euler.cpp.

References compute_end_pt(), EulerSpiralParams::gamma, EulerSpiralParams::K0, and params.

{
  params = es_params;
  return compute_end_pt(params.K0, params.gamma, arclength, bNormalized);
}

void EulerSpiral::computeSpiral	(	std::vector< Point2D< double > > &	spiral,
		double	ds = 0,
		int	NPts = 0
	)

Definition at line 364 of file euler.cpp.

References compute_end_pt(), EulerSpiralParams::end_pt, EulerSpiralParams::L, params, and EulerSpiralParams::start_pt.

Referenced by NIImporter_OpenDrive::geomFromSpiral().

{
  if (ds==0 && NPts==0){
    //use default values
    NPts = 100;
    ds = params.L/NPts;
  }

  spiral.clear();
  spiral.push_back(params.start_pt);

  double s=ds;
  if (NPts == 0) 
    NPts = (int) (params.L / ds);
  for (int i=1; i<NPts; i++,s+=ds){
    Point2D<double> cur_pt = compute_end_pt(s);
    spiral.push_back(cur_pt);
  }
  spiral.push_back(params.end_pt);
}

Point2D< double > EulerSpiral::get_fresnel_integral

(

double

value

)

Definition at line 482 of file euler.cpp.

References EPS, FPMIN, M_PI, MAXIT, Point2D< coord_type >::setX(), Point2D< coord_type >::setY(), sign, and XMIN.

Referenced by compute_end_pt().

{
  bool odd;
  int k,n;
  double a,ax,fact,pix2,sign,sum,sumc,sums,term,test;

  std::complex<double> b,cc,d,h,del,cs;
  Point2D<double> result;

  ax=fabs(x);
  if (ax < sqrt(FPMIN)) 
  { 
    //Special case: avoid failure of convergence test because of undeflow.
    result.setY(0.0);
    result.setX(ax);
  }
  else {
    if (ax <= XMIN)
    {
      // Evaluate both series simultaneously.
      sum=sums=0.0;
      sumc=ax;
      sign=1.0;
      fact=(M_PI/2.0)*ax*ax;
      odd=true;
      term=ax;
      n=3;

      for (k=1;k<=MAXIT;k++)
      { 
        term *= fact/k;
        sum  += sign*term/n;
        test=fabs(sum)*EPS;
        if (odd) 
        {
          sign = -sign;
          sums=sum;
          sum=sumc;
        }
        else {
          sumc=sum;
          sum=sums;
        }

        if (term < test) break;
        odd=!odd;
        n +=2;
      }

      if (k > MAXIT) 
        std::cout << "series failed in fresnel" << std::endl;

      result.setY(sums); 
      result.setX(sumc);
    }
    else {
      // Evaluate continued fraction by modified Lentz's method
      pix2=M_PI*ax*ax;
      b   = std::complex<double>(1.0,-pix2);
      cc  = std::complex<double>(1.0/FPMIN,0.0);
      d=h = std::complex<double>(1.0,0.0)/b;
      n = -1;

      for (k=2;k<=MAXIT;k++) 
      {
        n +=2;
        a = -n*(n+1);
        b= b+std::complex<double>(4.0,0.0);
        d=(std::complex<double>(1.0,0.0)/((a*d)+b));

        //Denominators cannot be zero
        cc=(b+(std::complex<double>(a,0.0)/cc));

        del=(cc*d);
        h=h*del;
        if ((fabs(del.real()-1.0)+fabs(del.imag())) < EPS)
          break;
      }
      if (k > MAXIT) 
        std::cout << "cf failed in frenel" << std::endl;

      h=std::complex<double>(ax,-ax)*h;
      cs=std::complex<double>(0.5,0.5)*(std::complex<double>(1.0,0.0) - std::complex<double>(cos(0.5*pix2),sin(0.5*pix2))*h );

      result.setX(cs.real());
      result.setY(cs.imag());
    }
  }

  if (x<0){ //use antisymmetry
    result = -1*result;  
  }

  return result;
}

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

Definition at line 181 of file euler.h.

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

  {
    params.end_pt = end_pt;
    params.end_angle = angle0To2Pi(end_angle);
  }

void EulerSpiral::set_params	(	Point2D< double >	start_pt,
		double	start_angle,
		Point2D< double >	end_pt,
		double	end_angle
	)			[inline]

Definition at line 187 of file euler.h.

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

  {
    params.start_pt = start_pt;
    params.start_angle = angle0To2Pi(start_angle);

    params.end_pt = end_pt;
    params.end_angle = angle0To2Pi(end_angle);
  }

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

Definition at line 175 of file euler.h.

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

  {
    params.start_pt = start_pt;
    params.start_angle = angle0To2Pi(start_angle);
  }

---

Field Documentation

BiArc EulerSpiral::_bi_arc_estimate [private]

Definition at line 142 of file euler.h.

Referenced by compute_es_params().

EulerSpiralParams EulerSpiral::params

Definition at line 145 of file euler.h.

Referenced by compute_end_pt(), compute_error(), compute_es_params(), compute_es_point(), computeSpiral(), EulerSpiral(), set_end_params(), set_params(), and set_start_params().

std::vector<Point2D<double> > EulerSpiral::pts

Definition at line 146 of file euler.h.

---

The documentation for this class was generated from the following files:

• euler.h
• euler.cpp