Commit bd3cd66e authored by Sergi Hernandez's avatar Sergi Hernandez
Browse files

Modified the generate functions of the spline to create a suingle point spline...

Modified the generate functions of the spline to create a suingle point spline when the length is 0 or the end point is further than the start point (invalid spline).
Modified the get_closest_point function to fail when the gradient goes beyond limits.
parent 0516dd8d
......@@ -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();
}
}
......
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment