inline double EulerSpiral::compute_error(double k0, double L)
{
//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);
}
static int fair_distances(struct point *pt, struct point pts[], int npts, double epsilon)
{
int i;
pt->dists = calloc(npts, sizeof(double));
for (i = 0; i < npts; i++) {
if ((fabs(pt->x - pts[i].x) < POS_THRESHOLD * epsilon) ||
(fabs(pt->y - pts[i].y) < POS_THRESHOLD * epsilon)) {
//printf("points too close, discarding: (%.2lf, %.2lf) (%.2lf, %.2lf)\n", pt->x, pt->y, pts[i].x, pts[i].y);
return 0;
}
pt->dists[i] = euc_distance(pt->x, pts[i].x, pt->y, pts[i].y);
}
return 1;
}
// Computes the Euler spiral for the given params
//if the global lookup table is available, it looks up the ES params first and then optimizes them
//this should dramatically cut down in the time to optimize
void EulerSpiral::compute_es_params ()
{
//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 = 0;
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;
}
