diff --git a/src/g2_spline.cpp b/src/g2_spline.cpp
index 5709b60c69db30232591fc6cdceefc1b9aec2d24..9f21ff78878ac5c743a1560be2ec4f9090d98d49 100644
--- a/src/g2_spline.cpp
+++ b/src/g2_spline.cpp
@@ -592,66 +592,85 @@ void CG2Spline::generate(double resolution,unsigned int iterations)
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));
- while(iterations>0 && fabs(new_length-last_length)>this->resolution)
+ if(new_length>this->resolution)
{
- last_length=new_length;
- this->num_points=ceil(last_length/resolution);
- l_c_start=last_length*cos(this->start.heading);
- l2_k_s_start=pow(last_length,2)*this->start.curvature*sin(this->start.heading);
- l_c_end=last_length*cos(this->end.heading);
- l2_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]=l_c_start;
- this->x_coeff[2]=-0.5*l2_k_s_start;
- this->x_coeff[3]=10.0*(this->end.x-this->start.x)-6.0*l_c_start-4.0*l_c_end+1.5*l2_k_s_start-0.5*l2_k_s_end;
- this->x_coeff[4]=-15.0*(this->end.x-this->start.x)+8.0*l_c_start+7.0*l_c_end-1.5*l2_k_s_start+l2_k_s_end;
- this->x_coeff[5]=6.0*(this->end.x-this->start.x)-3.0*l_c_start-3.0*l_c_end+0.5*l2_k_s_start-0.5*l2_k_s_end;
- l_s_start=last_length*sin(this->start.heading);
- l2_k_c_start=pow(last_length,2)*this->start.curvature*cos(this->start.heading);
- l_s_end=last_length*sin(this->end.heading);
- l2_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]=l_s_start;
- this->y_coeff[2]=0.5*l2_k_c_start;
- this->y_coeff[3]=10.0*(this->end.y-this->start.y)-6.0*l_s_start-4.0*l_s_end-1.5*l2_k_c_start+0.5*l2_k_c_end;
- this->y_coeff[4]=-15*(this->end.y-this->start.y)+8.0*l_s_start+7.0*l_s_end+1.5*l2_k_c_start-l2_k_c_end;
- this->y_coeff[5]=6.0*(this->end.y-this->start.y)-3.0*l_s_start-3.0*l_s_end-0.5*l2_k_c_start+0.5*l2_k_c_end;
- new_length=0.0;
+ while(iterations>0 && fabs(new_length-last_length)>this->resolution)
+ {
+ last_length=new_length;
+ this->num_points=ceil(last_length/resolution);
+ l_c_start=last_length*cos(this->start.heading);
+ l2_k_s_start=pow(last_length,2)*this->start.curvature*sin(this->start.heading);
+ l_c_end=last_length*cos(this->end.heading);
+ l2_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]=l_c_start;
+ this->x_coeff[2]=-0.5*l2_k_s_start;
+ this->x_coeff[3]=10.0*(this->end.x-this->start.x)-6.0*l_c_start-4.0*l_c_end+1.5*l2_k_s_start-0.5*l2_k_s_end;
+ this->x_coeff[4]=-15.0*(this->end.x-this->start.x)+8.0*l_c_start+7.0*l_c_end-1.5*l2_k_s_start+l2_k_s_end;
+ this->x_coeff[5]=6.0*(this->end.x-this->start.x)-3.0*l_c_start-3.0*l_c_end+0.5*l2_k_s_start-0.5*l2_k_s_end;
+ l_s_start=last_length*sin(this->start.heading);
+ l2_k_c_start=pow(last_length,2)*this->start.curvature*cos(this->start.heading);
+ l_s_end=last_length*sin(this->end.heading);
+ l2_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]=l_s_start;
+ this->y_coeff[2]=0.5*l2_k_c_start;
+ this->y_coeff[3]=10.0*(this->end.y-this->start.y)-6.0*l_s_start-4.0*l_s_end-1.5*l2_k_c_start+0.5*l2_k_c_end;
+ this->y_coeff[4]=-15*(this->end.y-this->start.y)+8.0*l_s_start+7.0*l_s_end+1.5*l2_k_c_start-l2_k_c_end;
+ this->y_coeff[5]=6.0*(this->end.y-this->start.y)-3.0*l_s_start-3.0*l_s_end-0.5*l2_k_c_start+0.5*l2_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->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--;
}
this->generated=true;
}
@@ -666,69 +685,87 @@ void CG2Spline::generate(double resolution,double length,unsigned int iterations
this->resolution=resolution;
last_length=0.0;
new_length=length;
- while(iterations>0 && fabs(new_length-last_length)>this->resolution)
+ if(new_length>this->resolution)
{
- last_length=new_length;
- this->num_points=ceil(last_length/resolution);
- l_c_start=last_length*cos(this->start.heading);
- l2_k_s_start=pow(last_length,2)*this->start.curvature*sin(this->start.heading);
- l_c_end=last_length*cos(this->end.heading);
- l2_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]=l_c_start;
- this->x_coeff[2]=-0.5*l2_k_s_start;
- this->x_coeff[3]=10.0*(this->end.x-this->start.x)-6.0*l_c_start-4.0*l_c_end+1.5*l2_k_s_start-0.5*l2_k_s_end;
- this->x_coeff[4]=-15.0*(this->end.x-this->start.x)+8.0*l_c_start+7.0*l_c_end-1.5*l2_k_s_start+l2_k_s_end;
- this->x_coeff[5]=6.0*(this->end.x-this->start.x)-3.0*l_c_start-3.0*l_c_end+0.5*l2_k_s_start-0.5*l2_k_s_end;
- l_s_start=last_length*sin(this->start.heading);
- l2_k_c_start=pow(last_length,2)*this->start.curvature*cos(this->start.heading);
- l_s_end=last_length*sin(this->end.heading);
- l2_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]=l_s_start;
- this->y_coeff[2]=0.5*l2_k_c_start;
- this->y_coeff[3]=10.0*(this->end.y-this->start.y)-6.0*l_s_start-4.0*l_s_end-1.5*l2_k_c_start+0.5*l2_k_c_end;
- this->y_coeff[4]=-15*(this->end.y-this->start.y)+8.0*l_s_start+7.0*l_s_end+1.5*l2_k_c_start-l2_k_c_end;
- this->y_coeff[5]=6.0*(this->end.y-this->start.y)-3.0*l_s_start-3.0*l_s_end-0.5*l2_k_c_start+0.5*l2_k_c_end;
- new_length=0.0;
+ while(iterations>0 && fabs(new_length-last_length)>this->resolution)
+ {
+ last_length=new_length;
+ this->num_points=ceil(last_length/resolution);
+ l_c_start=last_length*cos(this->start.heading);
+ l2_k_s_start=pow(last_length,2)*this->start.curvature*sin(this->start.heading);
+ l_c_end=last_length*cos(this->end.heading);
+ l2_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]=l_c_start;
+ this->x_coeff[2]=-0.5*l2_k_s_start;
+ this->x_coeff[3]=10.0*(this->end.x-this->start.x)-6.0*l_c_start-4.0*l_c_end+1.5*l2_k_s_start-0.5*l2_k_s_end;
+ this->x_coeff[4]=-15.0*(this->end.x-this->start.x)+8.0*l_c_start+7.0*l_c_end-1.5*l2_k_s_start+l2_k_s_end;
+ this->x_coeff[5]=6.0*(this->end.x-this->start.x)-3.0*l_c_start-3.0*l_c_end+0.5*l2_k_s_start-0.5*l2_k_s_end;
+ l_s_start=last_length*sin(this->start.heading);
+ l2_k_c_start=pow(last_length,2)*this->start.curvature*cos(this->start.heading);
+ l_s_end=last_length*sin(this->end.heading);
+ l2_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]=l_s_start;
+ this->y_coeff[2]=0.5*l2_k_c_start;
+ this->y_coeff[3]=10.0*(this->end.y-this->start.y)-6.0*l_s_start-4.0*l_s_end-1.5*l2_k_c_start+0.5*l2_k_c_end;
+ this->y_coeff[4]=-15*(this->end.y-this->start.y)+8.0*l_s_start+7.0*l_s_end+1.5*l2_k_c_start-l2_k_c_end;
+ this->y_coeff[5]=6.0*(this->end.y-this->start.y)-3.0*l_s_start-3.0*l_s_end-0.5*l2_k_c_start+0.5*l2_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->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--;
}
this->generated=true;
-
}
TPoint CG2Spline::evaluate(double length)
@@ -794,13 +831,14 @@ double CG2Spline::get_length(void)
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,true))
+ if(!this->curvature_grad.compute(max_error,max_iter,start_point,beyond_limits,true))
return std::numeric_limits<double>::max();
else
{
@@ -814,13 +852,14 @@ double CG2Spline::get_max_curvature(double max_error,unsigned int max_iter)
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,true))
+ if(!this->curvature_der_grad.compute(max_error,max_iter,start_point,beyond_limits,true))
return std::numeric_limits<double>::max();
else
{
@@ -834,37 +873,49 @@ double CG2Spline::get_max_curvature_der(double max_error,unsigned int max_iter)
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,false))
+ if(!this->distance_grad.compute(max_error,max_iter,start_point,beyond_limits,false))
return std::numeric_limits<double>::max();
else
{
- 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))];
+ 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 0.0;
+ 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,false))
+ if(!this->distance_grad.compute(max_error,max_iter,start_point,beyond_limits,false))
return std::numeric_limits<double>::max();
else
{
- if(this->num_points)
- return this->length[(unsigned int)ceil(this->distance_grad.get_coordinate()*(this->num_points-1))];
+ 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 0.0;
+ return std::numeric_limits<double>::max();
}
}