g2_spline.cpp

#include "g2_spline.h"
#include <math.h>
#include <boost/bind.hpp>
#include <iostream>

CG2Spline::CG2Spline()
{
  this->start.x=0.0;
  this->start.y=0.0;
  this->start.curvature=0.0;
  this->start.heading=0.0;
  this->end.x=0.0;
  this->end.y=0.0;
  this->end.curvature=0.0;
  this->end.heading=0.0;
  memset(this->x_coeff,0,6*sizeof(double));
  memset(this->y_coeff,0,6*sizeof(double));
  this->generated=false;
  this->x=NULL;
  this->y=NULL;
  this->length=NULL;
  this->num_points=0;
  this->resolution=DEFAULT_RESOLUTION;
  // initialize optimization objects
  this->curvature_grad.set_function(boost::bind(&CG2Spline::curvature_pow2,this,_1));
  this->curvature_grad.set_function_der(boost::bind(&CG2Spline::curvature_pow2_der,this,_1));
  this->curvature_grad.set_coord_constraints(1.0,0.0);
  this->curvature_computed=false;
  this->curvature_value=0.0;
  this->curvature_der_grad.set_function(boost::bind(&CG2Spline::curvature_der_pow2,this,_1));
  this->curvature_der_grad.set_function_der(boost::bind(&CG2Spline::curvature_der_pow2_der,this,_1));
  this->curvature_der_grad.set_coord_constraints(1.0,0.0);
  this->curvature_der_computed=false;
  this->curvature_der_value=0.0;
  this->distance_grad.set_function(boost::bind(&CG2Spline::distance_pow2,this,_1));
  this->distance_grad.set_function_der(boost::bind(&CG2Spline::distance_pow2_der,this,_1));
  this->distance_grad.set_coord_constraints(1.0,0.0);
}

CG2Spline::CG2Spline(const CG2Spline &obj)
{
  this->start.x=obj.start.x;
  this->start.y=obj.start.y;
  this->start.curvature=obj.start.curvature;
  this->start.heading=obj.start.heading;
  this->end.x=obj.end.x;
  this->end.y=obj.end.y;
  this->end.curvature=obj.end.curvature;
  this->end.heading=obj.end.heading;
  memcpy(this->x_coeff,obj.x_coeff,6*sizeof(double));
  memcpy(this->y_coeff,obj.y_coeff,6*sizeof(double));
  this->generated=obj.generated;
  this->num_points=obj.num_points;
  if(this->num_points!=0)
  {
    this->x=new double[this->num_points];
    std::memcpy(this->x,obj.x,this->num_points*sizeof(double));
    this->y=new double[this->num_points];
    std::memcpy(this->y,obj.y,this->num_points*sizeof(double));
    this->length= new double[this->num_points];
    std::memcpy(this->length,obj.length,this->num_points*sizeof(double));
  }
  else
  {
    this->x=NULL;
    this->y=NULL;
    this->length=NULL;
  }
  this->resolution=obj.resolution;
  // initialize optimization objects
  this->curvature_grad.set_function(boost::bind(&CG2Spline::curvature_pow2,this,_1));
  this->curvature_grad.set_function_der(boost::bind(&CG2Spline::curvature_pow2_der,this,_1));
  this->curvature_grad.set_coord_constraints(1.0,0.0);
  this->curvature_computed=false;
  this->curvature_value=0.0;
  this->curvature_der_grad.set_function(boost::bind(&CG2Spline::curvature_der_pow2,this,_1));
  this->curvature_der_grad.set_function_der(boost::bind(&CG2Spline::curvature_der_pow2_der,this,_1));
  this->curvature_der_grad.set_coord_constraints(1.0,0.0);
  this->curvature_der_computed=false;
  this->curvature_der_value=0.0;
  this->distance_grad.set_function(boost::bind(&CG2Spline::distance_pow2,this,_1));
  this->distance_grad.set_function_der(boost::bind(&CG2Spline::distance_pow2_der,this,_1));
  this->distance_grad.set_coord_constraints(1.0,0.0);
}

CG2Spline::CG2Spline(TPoint &start,TPoint &end)
{
  this->start=start;
  this->end=end;
  memset(this->x_coeff,0,6*sizeof(double));
  memset(this->y_coeff,0,6*sizeof(double));
  this->generated=false;
  this->x=NULL;
  this->y=NULL;
  this->length=NULL;
  this->num_points=0;
  this->resolution=DEFAULT_RESOLUTION;
  // initialize optimization objects
  this->curvature_grad.set_function(boost::bind(&CG2Spline::curvature_pow2,this,_1));
  this->curvature_grad.set_function_der(boost::bind(&CG2Spline::curvature_pow2_der,this,_1));
  this->curvature_grad.set_coord_constraints(1.0,0.0);
  this->curvature_computed=false;
  this->curvature_value=0.0;
  this->curvature_der_grad.set_function(boost::bind(&CG2Spline::curvature_der_pow2,this,_1));
  this->curvature_der_grad.set_function_der(boost::bind(&CG2Spline::curvature_der_pow2_der,this,_1));
  this->curvature_der_grad.set_coord_constraints(1.0,0.0);
  this->curvature_der_computed=false;
  this->curvature_der_value=0.0;
  this->distance_grad.set_function(boost::bind(&CG2Spline::distance_pow2,this,_1));
  this->distance_grad.set_function_der(boost::bind(&CG2Spline::distance_pow2_der,this,_1));
  this->distance_grad.set_coord_constraints(1.0,0.0);
}

void CG2Spline::normalize_angle(double &angle)
{
  if(angle>3.14159)
    angle-=2*3.14159;
  else if(angle<-3.14159)
    angle+=2*3.14159;
}

double CG2Spline::find_parameter(double length)
{
  unsigned int index;

  index=length/this->resolution;
  
  if(this->length[index]-length>this->resolution)
    while(index>0 && this->length[index]-length>this->resolution)
      index--;
  else
    while(index<this->num_points && this->length[index]<length)
      index++;

  return ((double)index/(double)this->num_points);
}

TPoint CG2Spline::evaluate_parameter(double u)
{
  double d_x,dd_x,d_y,dd_y,pow_u;
  TPoint point;

  if(!this->generated)
    this->generate(DEFAULT_RESOLUTION);
  if(u==0.0)
  {
    point.x=this->x_coeff[0];
    point.y=this->y_coeff[0];
    d_x=this->x_coeff[1];
    d_y=this->y_coeff[1];
    dd_x=2.0*this->x_coeff[2];
    dd_y=2.0*this->y_coeff[2];
  }
  else
  {
    point.x=this->x_coeff[0];
    point.y=this->y_coeff[0];
    d_x=this->x_coeff[1];
    d_y=this->y_coeff[1];
    dd_x=2.0*this->x_coeff[2];
    dd_y=2.0*this->y_coeff[2];
    pow_u=u;
    point.x+=this->x_coeff[1]*pow_u;
    point.y+=this->y_coeff[1]*pow_u;
    d_x+=2.0*this->x_coeff[2]*pow_u;
    d_y+=2.0*this->y_coeff[2]*pow_u;
    dd_x+=6.0*this->x_coeff[3]*pow_u;
    dd_y+=6.0*this->y_coeff[3]*pow_u;
    pow_u*=u;
    point.x+=this->x_coeff[2]*pow_u;
    point.y+=this->y_coeff[2]*pow_u;
    d_x+=3.0*this->x_coeff[3]*pow_u;
    d_y+=3.0*this->y_coeff[3]*pow_u;
    dd_x+=12.0*this->x_coeff[4]*pow_u;
    dd_y+=12.0*this->y_coeff[4]*pow_u;
    pow_u*=u;
    point.x+=this->x_coeff[3]*pow_u;
    point.y+=this->y_coeff[3]*pow_u;
    d_x+=4.0*this->x_coeff[4]*pow_u;
    d_y+=4.0*this->y_coeff[4]*pow_u;
    dd_x+=20.0*this->x_coeff[5]*pow_u;
    dd_y+=20.0*this->y_coeff[5]*pow_u;
    pow_u*=u;
    point.x+=this->x_coeff[4]*pow_u;
    point.y+=this->y_coeff[4]*pow_u;
    d_x+=5.0*this->x_coeff[5]*pow_u;
    d_y+=5.0*this->y_coeff[5]*pow_u;
    pow_u*=u;
    point.x+=this->x_coeff[5]*pow_u;
    point.y+=this->y_coeff[5]*pow_u;
  }
  // compute heading
  point.heading=atan2(d_y,d_x);
  this->normalize_angle(point.heading);
  point.curvature=(d_x*dd_y-dd_x*d_y)/(sqrt(pow(d_x*d_x+d_y*d_y,3)));

  return point;
}

double CG2Spline::get_max_sample_point(boost::function<double (double)> p_function)
{
  unsigned int i,max_coord;
  double max_value=-std::numeric_limits<double>::max(),value;
  double coord;

  for(i=0,coord=0.0;i<5;i++,coord+=0.25)
  {
    value=p_function(coord);
    if(value>max_value)
    {
      max_value=value;
      max_coord=i;
    }
  }

  return max_coord*0.25;
}

double CG2Spline::get_min_sample_point(boost::function<double (double)> p_function)
{
  unsigned int i,min_coord;
  double min_value=std::numeric_limits<double>::max(),value;
  double coord;

  for(i=0,coord=0.0;i<5;i++,coord+=0.25)
  {
    value=p_function(coord);
    if(value<min_value)
    {
      min_value=value;
      min_coord=i;
    }
  }

  return min_coord*0.25;
}

double CG2Spline::curvature_pow2(double u)
{
  double a,b,c,d,k_pow2; 
  double pow_u;
  
  a=this->x_coeff[1];
  b=this->y_coeff[1];
  c=this->x_coeff[2]*2.0;
  d=this->y_coeff[2]*2.0;
  pow_u=u;
  a+=(pow_u*this->x_coeff[2]*2.0);
  b+=(pow_u*this->y_coeff[2]*2.0);
  c+=(pow_u*this->x_coeff[3]*6.0);
  d+=(pow_u*this->y_coeff[3]*6.0);
  pow_u*=u;
  a+=(pow_u*this->x_coeff[3]*3.0);
  b+=(pow_u*this->y_coeff[3]*3.0);
  c+=(pow_u*this->x_coeff[4]*12.0);
  d+=(pow_u*this->y_coeff[4]*12.0);
  pow_u*=u;
  a+=(pow_u*this->x_coeff[4]*4.0);
  b+=(pow_u*this->y_coeff[4]*4.0);
  c+=(pow_u*this->x_coeff[5]*20.0);
  d+=(pow_u*this->y_coeff[5]*20.0);
  pow_u*=u;
  a+=(pow_u*this->x_coeff[5]*5.0);
  b+=(pow_u*this->y_coeff[5]*5.0);

  k_pow2=pow(((a*d)-(c*b)),2.0)/(pow(((a*a)+(b*b)),3.0));
    
  return k_pow2;
}

double CG2Spline::curvature_pow2_der(double u)
{
  double a,b,c,d,e,f,k_pow2_der; 
  double AB2,ADdot,CBdot,ABdot,ADCB;  
  double pow_u;
  
  a=this->x_coeff[1];
  b=this->y_coeff[1];
  c=this->x_coeff[2]*2.0;
  d=this->y_coeff[2]*2.0;
  e=this->x_coeff[3]*6.0;
  f=this->y_coeff[3]*6.0;
  pow_u=u;
  a+=(pow_u*this->x_coeff[2]*2.0);
  b+=(pow_u*this->y_coeff[2]*2.0);
  c+=(pow_u*this->x_coeff[3]*6.0);
  d+=(pow_u*this->y_coeff[3]*6.0);
  e+=(pow_u*this->x_coeff[4]*24.0);
  f+=(pow_u*this->y_coeff[4]*24.0);
  pow_u*=u;
  a+=(pow_u*this->x_coeff[3]*3.0);
  b+=(pow_u*this->y_coeff[3]*3.0);
  c+=(pow_u*this->x_coeff[4]*12.0);
  d+=(pow_u*this->y_coeff[4]*12.0);
  e+=(pow_u*this->x_coeff[5]*60.0);
  f+=(pow_u*this->y_coeff[5]*60.0);
  pow_u*=u;
  a+=(pow_u*this->x_coeff[4]*4.0);
  b+=(pow_u*this->y_coeff[4]*4.0);
  c+=(pow_u*this->x_coeff[5]*20.0);
  d+=(pow_u*this->y_coeff[5]*20.0);
  pow_u*=u;
  a+=(pow_u*this->x_coeff[5]*5.0);
  b+=(pow_u*this->y_coeff[5]*5.0);

  AB2=(a*a)+(b*b);
  ADdot=(c*d)+(a*f);
  CBdot=(e*b)+(c*d);
  ABdot=(a*c)+(b*d);
  ADCB=(a*d)-(c*b);
  
  k_pow2_der=(2.0*ADCB*(((ADdot-CBdot)*AB2)-(3.0*(ADCB*ABdot))))/AB2; 
      
  return k_pow2_der;
}

double CG2Spline::curvature_der_pow2(double u)
{
  double AB2,ADdot,CBdot,ABdot,ADCB;  
  double a,b,c,d,e,f,k_der_pow2; 
  double pow_u;
  
  a=this->x_coeff[1];
  b=this->y_coeff[1];
  c=this->x_coeff[2]*2.0;
  d=this->y_coeff[2]*2.0;
  e=this->x_coeff[3]*6.0;
  f=this->y_coeff[3]*6.0;
  pow_u=u;
  a+=(pow_u*this->x_coeff[2]*2.0);
  b+=(pow_u*this->y_coeff[2]*2.0);
  c+=(pow_u*this->x_coeff[3]*6.0);
  d+=(pow_u*this->y_coeff[3]*6.0);
  e+=(pow_u*this->x_coeff[4]*24.0); 
  f+=(pow_u*this->y_coeff[4]*24.0);
  pow_u*=u;
  a+=(pow_u*this->x_coeff[3]*3.0);
  b+=(pow_u*this->y_coeff[3]*3.0);
  c+=(pow_u*this->x_coeff[4]*12.0);
  d+=(pow_u*this->y_coeff[4]*12.0);
  e+=(pow_u*this->x_coeff[5]*60.0);
  f+=(pow_u*this->y_coeff[5]*60.0);
  pow_u*=u;
  a+=(pow_u*this->x_coeff[4]*4.0);
  b+=(pow_u*this->y_coeff[4]*4.0);
  c+=(pow_u*this->x_coeff[5]*20.0);
  d+=(pow_u*this->y_coeff[5]*20.0);
  pow_u*=u;
  a+=(pow_u*this->x_coeff[5]*5.0);
  b+=(pow_u*this->y_coeff[5]*5.0);
    
  AB2=(a*a)+(b*b);
  ADdot=(c*d)+(a*f);
  CBdot=(e*b)+(c*d);
  ABdot=(a*c)+(b*d);
  ADCB=(a*d)-(c*b);
  
  k_der_pow2=pow((((ADdot-CBdot)*AB2)-(3.0*ADCB*ABdot)),2.0)/pow(AB2,5.0);
      
  return k_der_pow2;	
}

double CG2Spline::curvature_der_pow2_der(double u)
{
  double T1,T2,T3,T4,T5,T6,T7,T8;
  double M1,M2,M3,M4,M5,M6,M7; 
  double AB2,ABdot,ADCB,FA_EB;  
  double a,b,c,d,e,f,k_der_pow2_der;
  double pow_u;
  
  a=this->x_coeff[1];
  b=this->y_coeff[1];
  c=this->x_coeff[2]*2.0;
  d=this->y_coeff[2]*2.0;
  e=this->x_coeff[3]*6.0;
  f=this->y_coeff[3]*6.0;
  pow_u=u;
  a+=(pow_u*this->x_coeff[2]*2.0);
  b+=(pow_u*this->y_coeff[2]*2.0);
  c+=(pow_u*this->x_coeff[3]*6.0);
  d+=(pow_u*this->y_coeff[3]*6.0);
  e+=(pow_u*this->x_coeff[4]*24.0);
  f+=(pow_u*this->y_coeff[4]*24.0);
  pow_u*=u;
  a+=(pow_u*this->x_coeff[3]*3.0);
  b+=(pow_u*this->y_coeff[3]*3.0);
  c+=(pow_u*this->x_coeff[4]*12.0);
  d+=(pow_u*this->y_coeff[4]*12.0);
  e+=(pow_u*this->x_coeff[5]*60.0);
  f+=(pow_u*this->y_coeff[5]*60.0);
  pow_u*=u;
  a+=(pow_u*this->x_coeff[4]*4.0);
  b+=(pow_u*this->y_coeff[4]*4.0);
  c+=(pow_u*this->x_coeff[5]*20.0);
  d+=(pow_u*this->y_coeff[5]*20.0);
  pow_u*=u;
  a+=(pow_u*this->x_coeff[5]*5.0);
  b+=(pow_u*this->y_coeff[5]*5.0);

  AB2=(a*a)+(b*b);
  ABdot=(a*c)+(b*d);
  ADCB=(a*d)-(c*b);
  FA_EB=(f*a)-(e*b);
  
  M1=AB2;
  M2=(AB2*FA_EB)-(ABdot*3.0*ADCB);
  M3=2.0*ABdot;
  M4=FA_EB;
  M5=3.0*(ADCB);
  M6=AB2;
  M7=ABdot;
  
  T1=((24.0*this->x_coeff[4])+(120.0*u*this->x_coeff[5]))*b;
  T2=((24.0*this->y_coeff[4])+(120.0*u*this->y_coeff[5]))*a;
  T3=f*c;
  T4=e*d;
  T5=f*a;
  T6=e*b;
  T7=pow(c,2.0)+pow(d,2.0)+(e*a)+(f*b);
  T8=M3*pow(M2,2.0);
    
  k_der_pow2_der=((M2*(M6*(T2-T1+T3-T4)+(M3*M4)-M7*3.0*(T5-T6)-M5*T7)*2.0)/pow(M1,5.0))-((5.0*T8)/pow(M1,6.0));
      
  return k_der_pow2_der;
}

double CG2Spline::distance(double u)
{
  double px,py,d;
  double pow_u;

  px=this->x_coeff[0];
  py=this->y_coeff[0];
  pow_u=u;
  px+=(this->x_coeff[1]*pow_u);
  py+=(this->y_coeff[1]*pow_u);
  pow_u*=u;
  px+=(this->x_coeff[2]*pow_u);
  py+=(this->y_coeff[2]*pow_u);
  pow_u*=u;
  px+=(this->x_coeff[3]*pow_u);
  py+=(this->y_coeff[3]*pow_u);
  pow_u*=u;
  px+=(this->x_coeff[4]*pow_u);
  py+=(this->y_coeff[4]*pow_u);
  pow_u*=u;
  px+=(this->x_coeff[5]*pow_u);
  py+=(this->y_coeff[5]*pow_u);

  d=pow(pow((px-this->dist_x),2.0)+pow((py-this->dist_y),2.0),0.5);
  
  return d;
}

double CG2Spline::distance_pow2(double u)
{
  double px,py,d_pow2;
  double pow_u;

  px=this->x_coeff[0];
  py=this->y_coeff[0];
  pow_u=u;
  px+=(this->x_coeff[1]*pow_u);
  py+=(this->y_coeff[1]*pow_u);
  pow_u*=u;
  px+=(this->x_coeff[2]*pow_u);
  py+=(this->y_coeff[2]*pow_u);
  pow_u*=u;
  px+=(this->x_coeff[3]*pow_u);
  py+=(this->y_coeff[3]*pow_u);
  pow_u*=u;
  px+=(this->x_coeff[4]*pow_u);
  py+=(this->y_coeff[4]*pow_u);
  pow_u*=u;
  px+=(this->x_coeff[5]*pow_u);
  py+=(this->y_coeff[5]*pow_u);

  d_pow2=pow((px-this->dist_x),2.0)+pow((py-this->dist_y),2.0);

  return d_pow2;
}

double CG2Spline::distance_pow2_der(double u)
{
  double px,py,px_dot,py_dot,d_pow2_der;
  double pow_u;

  px=this->x_coeff[0];
  py=this->y_coeff[0];
  px_dot=this->x_coeff[1];
  py_dot=this->y_coeff[1];
  pow_u=u;
  px+=(this->x_coeff[1]*pow_u);
  py+=(this->y_coeff[1]*pow_u);
  px_dot+=pow_u*this->x_coeff[2]*2.0;
  py_dot+=pow_u*this->y_coeff[2]*2.0;
  pow_u*=u;
  px+=(this->x_coeff[2]*pow_u);
  py+=(this->y_coeff[2]*pow_u);
  px_dot+=pow_u*this->x_coeff[3]*3.0;
  py_dot+=pow_u*this->y_coeff[3]*3.0;
  pow_u*=u;
  px+=(this->x_coeff[3]*pow_u);
  py+=(this->y_coeff[3]*pow_u);
  px_dot+=pow_u*this->x_coeff[4]*4.0;
  py_dot+=pow_u*this->y_coeff[4]*4.0;
  pow_u*=u;
  px+=(this->x_coeff[4]*pow_u);
  py+=(this->y_coeff[4]*pow_u);
  px_dot+=pow_u*this->x_coeff[5]*5.0;
  py_dot+=pow_u*this->y_coeff[5]*5.0;
  pow_u*=u;
  px+=(this->x_coeff[5]*pow_u);
  py+=(this->y_coeff[5]*pow_u);

  d_pow2_der=(2.0*(px-this->dist_x)*px_dot)+(2.0*(py-this->dist_y)*py_dot);

  return d_pow2_der;
}

double CG2Spline::get_resolution(void)
{
  return this->resolution;
}

void CG2Spline::set_start_point(const TPoint &point)
{
  this->start=point;
  memset(this->x_coeff,0,6*sizeof(double));
  memset(this->y_coeff,0,6*sizeof(double));
  this->generated=false;
  this->curvature_computed=false;
  this->curvature_der_computed=false;
  if(this->x!=NULL)
  {
    delete[] this->x;
    this->x=NULL;
  }
  if(this->y!=NULL)
  {
    delete[] this->y;
    this->y=NULL;
  }
  if(this->length!=NULL)
  {
    delete[] this->length;
    this->length=NULL;
  }
}

void CG2Spline::get_start_point(TPoint &point)
{
  point=this->start;
}

void CG2Spline::set_end_point(const TPoint &point)
{
  this->end=point;
  memset(this->x_coeff,0,6*sizeof(double));
  memset(this->y_coeff,0,6*sizeof(double));
  this->generated=false;
  this->curvature_computed=false;
  this->curvature_der_computed=false;
  if(this->x!=NULL)
  {
    delete[] this->x;
    this->x=NULL;
  }
  if(this->y!=NULL)
  {
    delete[] this->y;
    this->y=NULL;
  }
  if(this->length!=NULL)
  {
    delete[] this->length;
    this->length=NULL;
  }
}

void CG2Spline::get_end_point(TPoint &point)
{
  point=this->end;
}

void CG2Spline::generate(double resolution,unsigned int iterations)
{
  double n1_c_start,n1_2_k_s_start,n1_s_start,n1_2_k_c_start;
  double n2_c_end,n2_2_k_s_end,n2_s_end,n2_2_k_c_end;
  double u,pow_u,new_length,last_length;
  unsigned int i;

  this->resolution=resolution;
  last_length=0.0;
  new_length=sqrt(pow(this->end.x-this->start.x,2)+pow(this->end.y-this->start.y,2));
  if(new_length>this->resolution)
  {
    while(iterations>0 && fabs(new_length-last_length)>this->resolution)
    {
      last_length=new_length;
      this->num_points=ceil(last_length/resolution);
      n1_c_start=last_length*cos(this->start.heading);
      n1_2_k_s_start=pow(last_length,2)*this->start.curvature*sin(this->start.heading);
      n2_c_end=last_length*cos(this->end.heading);
      n2_2_k_s_end=pow(last_length,2)*this->end.curvature*sin(this->end.heading);
      this->x_coeff[0]=this->start.x;
      this->x_coeff[1]=n1_c_start;
      this->x_coeff[2]=-0.5*n1_2_k_s_start;
      this->x_coeff[3]=10.0*(this->end.x-this->start.x)-6.0*n1_c_start-4.0*n2_c_end+1.5*n1_2_k_s_start-0.5*n2_2_k_s_end;
      this->x_coeff[4]=-15.0*(this->end.x-this->start.x)+8.0*n1_c_start+7.0*n2_c_end-1.5*n1_2_k_s_start+n2_2_k_s_end;
      this->x_coeff[5]=6.0*(this->end.x-this->start.x)-3.0*n1_c_start-3.0*n2_c_end+0.5*n1_2_k_s_start-0.5*n2_2_k_s_end;
      n1_s_start=last_length*sin(this->start.heading);
      n1_2_k_c_start=pow(last_length,2)*this->start.curvature*cos(this->start.heading);
      n2_s_end=last_length*sin(this->end.heading);
      n2_2_k_c_end=pow(last_length,2)*this->end.curvature*cos(this->end.heading);
      this->y_coeff[0]=this->start.y;
      this->y_coeff[1]=n1_s_start;
      this->y_coeff[2]=0.5*n1_2_k_c_start;
      this->y_coeff[3]=10.0*(this->end.y-this->start.y)-6.0*n1_s_start-4.0*n2_s_end-1.5*n1_2_k_c_start+0.5*n2_2_k_c_end;
      this->y_coeff[4]=-15*(this->end.y-this->start.y)+8.0*n1_s_start+7.0*n2_s_end+1.5*n1_2_k_c_start-n2_2_k_c_end;
      this->y_coeff[5]=6.0*(this->end.y-this->start.y)-3.0*n1_s_start-3.0*n2_s_end-0.5*n1_2_k_c_start+0.5*n2_2_k_c_end;
      new_length=0.0;
      if(this->x!=NULL)
        delete[] this->x;
      this->x=new double[num_points];
      if(this->y!=NULL)
        delete[] this->y;
      this->y=new double[num_points];
      if(this->length!=NULL)
        delete[] this->length;
      this->length=new double[num_points];
      this->x[0]=this->x_coeff[0];
      this->y[0]=this->y_coeff[0];
      this->length[0]=0.0;
      for(i=1,u=(1.0/(num_points-1));i<num_points;i++,u+=(1.0/(num_points-1)))
      {
        this->x[i]=this->x_coeff[0];
        this->y[i]=this->y_coeff[0];
        pow_u=u;
        this->x[i]+=this->x_coeff[1]*pow_u;
        this->y[i]+=this->y_coeff[1]*pow_u;
        pow_u*=u;
        this->x[i]+=this->x_coeff[2]*pow_u;
        this->y[i]+=this->y_coeff[2]*pow_u;
        pow_u*=u;
        this->x[i]+=this->x_coeff[3]*pow_u;
        this->y[i]+=this->y_coeff[3]*pow_u;
        pow_u*=u;
        this->x[i]+=this->x_coeff[4]*pow_u;
        this->y[i]+=this->y_coeff[4]*pow_u;
        pow_u*=u;
        this->x[i]+=this->x_coeff[5]*pow_u;
        this->y[i]+=this->y_coeff[5]*pow_u;
        new_length+=sqrt(pow(this->x[i]-this->x[i-1],2)+pow(this->y[i]-this->y[i-1],2));
        this->length[i]=new_length;
      }
      iterations--;
    }
  }
  else
  {
    this->num_points=1;
    if(this->x!=NULL)
      delete[] this->x;
    this->x=new double[num_points];
    x[0]=this->start.x;
    if(this->y!=NULL)
      delete[] this->y;
    this->y=new double[num_points];
    y[0]=this->start.y;
    if(this->length!=NULL)
      delete[] this->length;
    this->length=new double[num_points];
    this->length[0]=0.0;
  }
  this->generated=true;
}

void CG2Spline::generate(double resolution,double length,unsigned int iterations)
{
  double n1_c_start,n1_2_k_s_start,n1_s_start,n1_2_k_c_start;
  double n2_c_end,n2_2_k_s_end,n2_s_end,n2_2_k_c_end;
  double u,pow_u,new_length,last_length;
  unsigned int i;

  this->resolution=resolution;
  last_length=0.0;
  new_length=length;
  if(new_length>this->resolution)
  {
    while(iterations>0 && fabs(new_length-last_length)>this->resolution)
    {
      last_length=new_length;
      this->num_points=ceil(last_length/resolution);
      n1_c_start=last_length*cos(this->start.heading);
      n1_2_k_s_start=pow(last_length,2)*this->start.curvature*sin(this->start.heading);
      n2_c_end=last_length*cos(this->end.heading);
      n2_2_k_s_end=pow(last_length,2)*this->end.curvature*sin(this->end.heading);
      this->x_coeff[0]=this->start.x;
      this->x_coeff[1]=n1_c_start;
      this->x_coeff[2]=-0.5*n1_2_k_s_start;
      this->x_coeff[3]=10.0*(this->end.x-this->start.x)-6.0*n1_c_start-4.0*n2_c_end+1.5*n1_2_k_s_start-0.5*n2_2_k_s_end;
      this->x_coeff[4]=-15.0*(this->end.x-this->start.x)+8.0*n1_c_start+7.0*n2_c_end-1.5*n1_2_k_s_start+n2_2_k_s_end;
      this->x_coeff[5]=6.0*(this->end.x-this->start.x)-3.0*n1_c_start-3.0*n2_c_end+0.5*n1_2_k_s_start-0.5*n2_2_k_s_end;
      n1_s_start=last_length*sin(this->start.heading);
      n1_2_k_c_start=pow(last_length,2)*this->start.curvature*cos(this->start.heading);
      n2_s_end=last_length*sin(this->end.heading);
      n2_2_k_c_end=pow(last_length,2)*this->end.curvature*cos(this->end.heading);
      this->y_coeff[0]=this->start.y;
      this->y_coeff[1]=n1_s_start;
      this->y_coeff[2]=0.5*n1_2_k_c_start;
      this->y_coeff[3]=10.0*(this->end.y-this->start.y)-6.0*n1_s_start-4.0*n2_s_end-1.5*n1_2_k_c_start+0.5*n2_2_k_c_end;
      this->y_coeff[4]=-15*(this->end.y-this->start.y)+8.0*n1_s_start+7.0*n2_s_end+1.5*n1_2_k_c_start-n2_2_k_c_end;
      this->y_coeff[5]=6.0*(this->end.y-this->start.y)-3.0*n1_s_start-3.0*n2_s_end-0.5*n1_2_k_c_start+0.5*n2_2_k_c_end;
      new_length=0.0;
      if(this->x!=NULL)
        delete[] this->x;
      this->x=new double[num_points];
      if(this->y!=NULL)
        delete[] this->y;
      this->y=new double[num_points];
      if(this->length!=NULL)
        delete[] this->length;
      this->length=new double[num_points];
      this->x[0]=this->x_coeff[0];
      this->y[0]=this->y_coeff[0];
      this->length[0]=0.0;
      for(i=1,u=(1.0/(num_points-1));i<num_points;i++,u+=(1.0/(num_points-1)))
      {
        this->x[i]=this->x_coeff[0];
        this->y[i]=this->y_coeff[0];
        pow_u=u;
        this->x[i]+=this->x_coeff[1]*pow_u;
        this->y[i]+=this->y_coeff[1]*pow_u;
        pow_u*=u;
        this->x[i]+=this->x_coeff[2]*pow_u;
        this->y[i]+=this->y_coeff[2]*pow_u;
        pow_u*=u;
        this->x[i]+=this->x_coeff[3]*pow_u;
        this->y[i]+=this->y_coeff[3]*pow_u;
        pow_u*=u;
        this->x[i]+=this->x_coeff[4]*pow_u;
        this->y[i]+=this->y_coeff[4]*pow_u;
        pow_u*=u;
        this->x[i]+=this->x_coeff[5]*pow_u;
        this->y[i]+=this->y_coeff[5]*pow_u;
        new_length+=sqrt(pow(this->x[i]-this->x[i-1],2)+pow(this->y[i]-this->y[i-1],2));
        this->length[i]=new_length;
      }
      iterations--;
    }
  }
  else
  {
    this->num_points=1;
    if(this->x!=NULL)
      delete[] this->x;
    this->x=new double[num_points];
    x[0]=this->start.x;
    if(this->y!=NULL)
      delete[] this->y;
    this->y=new double[num_points];
    y[0]=this->start.y;
    if(this->length!=NULL)
      delete[] this->length;
    this->length=new double[num_points];
    this->length[0]=0.0;
  }
  this->generated=true;
}

void CG2Spline::generate(double &resolution,double n1,double n2, double n3, double n4)
{
  double n1_c_start,n1_2_k_s_start,n1_s_start,n1_2_k_c_start;
  double n3_c_start,n3_s_start;
  double n2_c_end,n2_2_k_s_end,n2_s_end,n2_2_k_c_end;
  double n4_c_end,n4_s_end;
  double u,pow_u,length,new_length;
  unsigned int i;

  this->resolution=resolution;
  length=sqrt(pow(this->end.x-this->start.x,2)+pow(this->end.y-this->start.y,2));
  if(length>this->resolution)
  {
    this->num_points=ceil(length/resolution);
    n1_c_start=n1*cos(this->start.heading);
    n3_c_start=n3*cos(this->start.heading);
    n1_2_k_s_start=pow(n1,2)*this->start.curvature*sin(this->start.heading);
    n2_c_end=n2*cos(this->end.heading);
    n4_c_end=n4*cos(this->end.heading);
    n2_2_k_s_end=pow(n2,2)*this->end.curvature*sin(this->end.heading);
    this->x_coeff[0]=this->start.x;
    this->x_coeff[1]=n1_c_start;
    this->x_coeff[2]=0.5*n3_c_start-0.5*n1_2_k_s_start;
    this->x_coeff[3]=10.0*(this->end.x-this->start.x)-6.0*n1_c_start-1.5*n3_c_start-4.0*n2_c_end+0.5*n4_c_end+1.5*n1_2_k_s_start-0.5*n2_2_k_s_end;
    this->x_coeff[4]=-15.0*(this->end.x-this->start.x)+8.0*n1_c_start+1.5*n3_c_start+7.0*n2_c_end-n4_c_end-1.5*n1_2_k_s_start+n2_2_k_s_end;
    this->x_coeff[5]=6.0*(this->end.x-this->start.x)-3.0*n1_c_start-0.5*n3_c_start-3.0*n2_c_end+0.5*n4_c_end+0.5*n1_2_k_s_start-0.5*n2_2_k_s_end;
    n1_s_start=n1*sin(this->start.heading);
    n3_s_start=n3*sin(this->start.heading);
    n1_2_k_c_start=pow(n1,2)*this->start.curvature*cos(this->start.heading);
    n2_s_end=n2*sin(this->end.heading);
    n4_s_end=n4*sin(this->end.heading);
    n2_2_k_c_end=pow(n2,2)*this->end.curvature*cos(this->end.heading);
    this->y_coeff[0]=this->start.y;
    this->y_coeff[1]=n1_s_start;
    this->y_coeff[2]=0.5*n3_s_start+0.5*n1_2_k_c_start;
    this->y_coeff[3]=10.0*(this->end.y-this->start.y)-6.0*n1_s_start-1.5*n3_s_start-4.0*n2_s_end+0.5*n4_s_end-1.5*n1_2_k_c_start+0.5*n2_2_k_c_end;
    this->y_coeff[4]=-15*(this->end.y-this->start.y)+8.0*n1_s_start+1.5*n3_s_start+7.0*n2_s_end-n4_s_end+1.5*n1_2_k_c_start-n2_2_k_c_end;
    this->y_coeff[5]=6.0*(this->end.y-this->start.y)-3.0*n1_s_start-0.5*n3_s_start-3.0*n2_s_end+0.5*n4_s_end-0.5*n1_2_k_c_start+0.5*n2_2_k_c_end;
    if(this->x!=NULL)
      delete[] this->x;
    this->x=new double[num_points];
    if(this->y!=NULL)
      delete[] this->y;
    this->y=new double[num_points];
    if(this->length!=NULL)
      delete[] this->length;
    this->length=new double[num_points];
    this->x[0]=this->x_coeff[0];
    this->y[0]=this->y_coeff[0];
    this->length[0]=0.0;
    new_length=0.0;
    for(i=1,u=(1.0/(num_points-1));i<num_points;i++,u+=(1.0/(num_points-1)))
    {
      this->x[i]=this->x_coeff[0];
      this->y[i]=this->y_coeff[0];
      pow_u=u;
      this->x[i]+=this->x_coeff[1]*pow_u;
      this->y[i]+=this->y_coeff[1]*pow_u;
      pow_u*=u;
      this->x[i]+=this->x_coeff[2]*pow_u;
      this->y[i]+=this->y_coeff[2]*pow_u;
      pow_u*=u;
      this->x[i]+=this->x_coeff[3]*pow_u;
      this->y[i]+=this->y_coeff[3]*pow_u;
      pow_u*=u;
      this->x[i]+=this->x_coeff[4]*pow_u;
      this->y[i]+=this->y_coeff[4]*pow_u;
      pow_u*=u;
      this->x[i]+=this->x_coeff[5]*pow_u;
      this->y[i]+=this->y_coeff[5]*pow_u;
      new_length+=sqrt(pow(this->x[i]-this->x[i-1],2)+pow(this->y[i]-this->y[i-1],2));
      this->length[i]=new_length;
    }
    this->resolution=new_length/num_points;
    resolution=this->resolution;
  }
  else
  {
    this->num_points=1;
    if(this->x!=NULL)
      delete[] this->x;
    this->x=new double[num_points];
    x[0]=this->start.x;
    if(this->y!=NULL)
      delete[] this->y;
    this->y=new double[num_points];
    y[0]=this->start.y;
    if(this->length!=NULL)
      delete[] this->length;
    this->length=new double[num_points];
    this->length[0]=0.0;
  }
  this->generated=true;
}

TPoint CG2Spline::evaluate(double length)
{
  return this->evaluate_parameter(this->find_parameter(length));
}

void CG2Spline::evaluate_all(std::vector<double> &x, std::vector<double> &y,std::vector<double> &curvature,std::vector<double> &heading)
{
  double pow_u,u;
  unsigned int i=0;
  double d_x,dd_x,d_y,dd_y;

  if(!this->generated)
    this->generate(DEFAULT_RESOLUTION);
  x.resize(this->num_points);
  y.resize(this->num_points);
  curvature.resize(this->num_points);
  heading.resize(this->num_points);
  for(i=0,u=0.0;i<num_points;i++,u+=(1.0/(num_points-1)))
  {
    x[i]=this->x[i];
    y[i]=this->y[i];
    d_x=this->x_coeff[1];
    d_y=this->y_coeff[1];
    dd_x=2.0*this->x_coeff[2];
    dd_y=2.0*this->y_coeff[2];
    pow_u=u;
    d_x+=2.0*this->x_coeff[2]*pow_u;
    d_y+=2.0*this->y_coeff[2]*pow_u;
    dd_x+=6.0*this->x_coeff[3]*pow_u;
    dd_y+=6.0*this->y_coeff[3]*pow_u;
    pow_u*=u;
    d_x+=3.0*this->x_coeff[3]*pow_u;
    d_y+=3.0*this->y_coeff[3]*pow_u;
    dd_x+=12.0*this->x_coeff[4]*pow_u;
    dd_y+=12.0*this->y_coeff[4]*pow_u;
    pow_u*=u;
    d_x+=4.0*this->x_coeff[4]*pow_u;
    d_y+=4.0*this->y_coeff[4]*pow_u;
    dd_x+=20.0*this->x_coeff[5]*pow_u;
    dd_y+=20.0*this->y_coeff[5]*pow_u;
    pow_u*=u;
    d_x+=5.0*this->x_coeff[5]*pow_u;
    d_y+=5.0*this->y_coeff[5]*pow_u;
    // compute heading
    heading[i]=atan2(d_y,d_x);
    this->normalize_angle(heading[i]);
    curvature[i]=(d_x*dd_y-dd_x*d_y)/(sqrt(pow(d_x*d_x+d_y*d_y,3)));    
  }
}

double CG2Spline::get_length(void)
{
  if(!this->generated)
    this->generate(DEFAULT_RESOLUTION);
  if(this->num_points>0)
    return this->length[this->num_points-1];
  else
    return 0.0;
} 

double CG2Spline::get_max_curvature(double max_error,unsigned int max_iter)
{
  double start_point;
  bool beyond_limits;

  if(this->curvature_computed)
    return this->curvature_value;
  else
  {
    start_point=this->get_max_sample_point(boost::bind(&CG2Spline::curvature_pow2,this,_1));
    if(!this->curvature_grad.compute(max_error,max_iter,start_point,beyond_limits,true))
    {
      std::cout << "Gradient: " << max_error << "," << max_iter << "," << beyond_limits << std::endl;
      return std::numeric_limits<double>::max();
    }
    else
    {
      this->curvature_computed=true;
      this->curvature_value=this->curvature(this->curvature_grad.get_coordinate());
      return this->curvature_value;
    } 
  }
}

double CG2Spline::get_max_curvature_der(double max_error,unsigned int max_iter)
{
  double start_point;
  bool beyond_limits;

  if(this->curvature_der_computed)
    return this->curvature_der_value;
  else
  {
    start_point=this->get_max_sample_point(boost::bind(&CG2Spline::curvature_der_pow2,this,_1));  
    if(!this->curvature_der_grad.compute(max_error,max_iter,start_point,beyond_limits,true))
      return std::numeric_limits<double>::max();
    else
    {
      this->curvature_der_computed=true;
      this->curvature_der_value=this->curvature_der(this->curvature_der_grad.get_coordinate());
      return this->curvature_der_value;
    }
  }
}

double CG2Spline::find_closest_point(double x, double y, TPoint &point,double max_error,unsigned int max_iter)
{
  double start_point;
  bool beyond_limits;

  this->dist_x=x;
  this->dist_y=y;
  start_point=this->get_min_sample_point(boost::bind(&CG2Spline::distance_pow2,this,_1));  
  if(!this->distance_grad.compute(max_error,max_iter,start_point,beyond_limits,false))
    return std::numeric_limits<double>::max();
  else
  {
    if(!beyond_limits)
    {
      point=this->evaluate_parameter(this->distance_grad.get_coordinate());
      if(this->num_points>0)
        return this->length[(unsigned int)ceil(this->distance_grad.get_coordinate()*(this->num_points-1))];
      else
        return 0.0;
    }
    else
      return std::numeric_limits<double>::max();
  }  
}

double CG2Spline::find_closest_point(double x, double y,double max_error,unsigned int max_iter)
{
  double start_point;
  bool beyond_limits;

  this->dist_x=x;
  this->dist_y=y;
  start_point=this->get_min_sample_point(boost::bind(&CG2Spline::distance_pow2,this,_1));  
  if(!this->distance_grad.compute(max_error,max_iter,start_point,beyond_limits,false))
    return std::numeric_limits<double>::max();
  else
  {
    if(!beyond_limits)
    {
      if(this->num_points)
        return this->length[(unsigned int)ceil(this->distance_grad.get_coordinate()*(this->num_points-1))];
      else
        return 0.0;
    }
    else
      return std::numeric_limits<double>::max();
  }
}

void CG2Spline::get_part(CG2Spline *spline,double start_length, double end_length)
{
  if(start_length==0.0)
    spline->set_start_point(this->start);
  else
    spline->set_start_point(this->evaluate(start_length));
  if(end_length==-1.0)
    spline->set_end_point(this->end);
  else
    spline->set_end_point(this->evaluate(end_length));
  spline->generate(this->resolution);
}

double CG2Spline::curvature(double u)
{
  double a,b,c,d,k; 
  double pow_u;

  if(u>0.0) 
  {
    a=this->x_coeff[1];
    b=this->y_coeff[1];
    c=this->x_coeff[2]*2.0;
    d=this->y_coeff[2]*2.0;
    pow_u=u;
    a+=(pow_u*this->x_coeff[2]*2.0);
    b+=(pow_u*this->y_coeff[2]*2.0);
    c+=(pow_u*this->x_coeff[3]*6.0);
    d+=(pow_u*this->y_coeff[3]*6.0);
    pow_u*=u;
    a+=(pow_u*this->x_coeff[3]*3.0);
    b+=(pow_u*this->y_coeff[3]*3.0);
    c+=(pow_u*this->x_coeff[4]*12.0);
    d+=(pow_u*this->y_coeff[4]*12.0);
    pow_u*=u;
    a+=(pow_u*this->x_coeff[4]*4.0);
    b+=(pow_u*this->y_coeff[4]*4.0);
    c+=(pow_u*this->x_coeff[5]*20.0);
    d+=(pow_u*this->y_coeff[5]*20.0);
    pow_u*=u;
    a+=(pow_u*this->x_coeff[5]*5.0);
    b+=(pow_u*this->y_coeff[5]*5.0);
  
    k=((a*d)-(c*b))/(pow(((a*a)+(b*b)),(1.5)));
  }
  else
  {
    a=this->x_coeff[1];
    b=this->y_coeff[1];
    c=this->x_coeff[2]*2.0;
    d=this->y_coeff[2]*2.0;

    k=((a*d)-(c*b))/(pow(((a*a)+(b*b)),(1.5)));
  }
    
  return k;
}

double CG2Spline::curvature_der(double u)
{
  double AB2,ADdot,CBdot,ABdot,ADCB;
  double a,b,c,d,e,f,k_der;
  double pow_u;

  if(u>0.0)
  {
    a=this->x_coeff[1];
    b=this->y_coeff[1];
    c=this->x_coeff[2]*2.0;
    d=this->y_coeff[2]*2.0;
    e=this->x_coeff[3]*6.0;
    f=this->y_coeff[3]*6.0;
    pow_u=u;
    a+=(pow_u*this->x_coeff[2]*2.0);
    b+=(pow_u*this->y_coeff[2]*2.0);
    c+=(pow_u*this->x_coeff[3]*6.0);
    d+=(pow_u*this->y_coeff[3]*6.0);
    e+=(pow_u*this->x_coeff[4]*24.0);
    f+=(pow_u*this->y_coeff[4]*24.0);
    pow_u*=u;
    a+=(pow_u*this->x_coeff[3]*3.0);
    b+=(pow_u*this->y_coeff[3]*3.0);
    c+=(pow_u*this->x_coeff[4]*12.0);
    d+=(pow_u*this->y_coeff[4]*12.0);
    e+=(pow_u*this->x_coeff[5]*60.0);
    f+=(pow_u*this->y_coeff[5]*60.0);
    pow_u*=u;
    a+=(pow_u*this->x_coeff[4]*4.0);
    b+=(pow_u*this->y_coeff[4]*4.0);
    c+=(pow_u*this->x_coeff[5]*20.0);
    d+=(pow_u*this->y_coeff[5]*20.0);
    pow_u*=u;
    a+=(pow_u*this->x_coeff[5]*5.0);
    b+=(pow_u*this->y_coeff[5]*5.0);
  
    AB2=(a*a)+(b*b);
    ADdot=(c*d)+(a*f);
    CBdot=(e*b)+(c*d);
    ABdot=(a*c)+(b*d);
    ADCB=(a*d)-(c*b);
 
    k_der=(((ADdot-CBdot)*AB2)-(3.0*ADCB*ABdot))/pow(AB2,2.5);
  } 
  else
  {
    a=this->x_coeff[1];
    b=this->y_coeff[1];
    c=this->x_coeff[2]*2.0;
    d=this->y_coeff[2]*2.0;
    e=this->x_coeff[3]*6.0;
    f=this->y_coeff[3]*6.0;

    AB2=(a*a)+(b*b);
    ADdot=(c*d)+(a*f);
    CBdot=(e*b)+(c*d);
    ABdot=(a*c)+(b*d);
    ADCB=(a*d)-(c*b);

    k_der=(((ADdot-CBdot)*AB2)-(3.0*ADCB*ABdot))/pow(AB2,2.5);
  }

  return k_der;
}

CG2Spline& CG2Spline::operator=(const CG2Spline &obj)
{
  if(this!=&obj)
  {
    this->start.x=obj.start.x;
    this->start.y=obj.start.y;
    this->start.curvature=obj.start.curvature;
    this->start.heading=obj.start.heading;
    this->end.x=obj.end.x;
    this->end.y=obj.end.y;
    this->end.curvature=obj.end.curvature;
    this->end.heading=obj.end.heading;
    memcpy(this->x_coeff,obj.x_coeff,6*sizeof(double));
    memcpy(this->y_coeff,obj.y_coeff,6*sizeof(double));
    this->generated=obj.generated;
    this->num_points=obj.num_points;
    if(this->x!=NULL)
    {
      delete[] this->x;
      this->x=NULL;
    }
    if(this->y!=NULL)
    {
      delete[] this->x;
      this->x=NULL;
    }
    if(this->length!=NULL)
    {
      delete[] this->length;
      this->length=NULL;
    }
    if(this->num_points!=0)
    {
      this->x=new double[this->num_points];
      std::memcpy(this->x,obj.x,this->num_points*sizeof(double));
      this->y=new double[this->num_points];
      std::memcpy(this->y,obj.y,this->num_points*sizeof(double));
      this->length=new double[this->num_points];
      std::memcpy(this->length,obj.length,this->num_points*sizeof(double));
    }
    this->resolution=obj.resolution;
  }
  
  return *this;
}

CG2Spline::~CG2Spline()
{
  if(this->x!=NULL)
  {
    delete[] this->x;
    this->x=NULL;
  }
  if(this->y!=NULL)
  {
    delete[] this->y;
    this->y=NULL;
  }
  if(this->length!=NULL)
  {
    delete[] this->length;
    this->length=NULL;
  }
}