ofPoint Radar::get_next_vertex(int i) {
ofPoint a1 = get_aframe_point(i);
ofPoint a2 = get_aframe_point(i+1);
ofPoint a3 = get_aframe_point(i+2);
ofPoint p1 = intermediate_point(a1, a2, 2/3.0);
ofPoint p2 = intermediate_point(a2, a3, 1/3.0);
return intermediate_point(p1,p2,0.5);
}
void Radar::compute_bezier_from_aframe() {
// From the aframe points, compute the bezier segments
// place bezier structs into bezier_segments vector
bezier_segments.clear();
ofPoint start = get_next_vertex(-1);
for (int i = 0; i < aframe_points.size(); i++) {
ofPoint a1 = get_aframe_point(i);
ofPoint a2 = get_aframe_point(i+1);
ofPoint c1 = intermediate_point(a1, a2, 1/3.0);
ofPoint c2 = intermediate_point(a1, a2, 2/3.0);
ofPoint end = get_next_vertex(i);
bezier b = bezier();
b.start = start;
b.c1 = c1;
b.c2 = c2;
b.end = end;
bezier_segments.push_back(b);
start = end;
}
}
ofPoint rec_de_casteljau(vector<ofPoint> points, float t) {
// Recursively compute the point at t=... on a bezier curve (defined by
// the 4 points in the 'points' vector) using the de casteljau algorithm
//
// Essentially this means if t=0.1 find the point 10% along the bezier.
//
if (points.size() == 1)
return points[0];
vector<ofPoint> intermediates;
for (int i = 0; i < points.size()-1; i++) {
ofPoint p1 = points[i];
ofPoint p2 = points[i+1];
ofPoint p = intermediate_point(p1, p2, t);
intermediates.push_back(p);
}
return rec_de_casteljau(intermediates, t);
}
static int
conjugate_fr_iterate (void *vstate, gsl_multimin_function_fdf * fdf,
gsl_vector * x, double *f,
gsl_vector * gradient, gsl_vector * dx)
{
conjugate_fr_state_t *state = (conjugate_fr_state_t *) vstate;
gsl_vector *x1 = state->x1;
gsl_vector *dx1 = state->dx1;
gsl_vector *x2 = state->x2;
gsl_vector *p = state->p;
gsl_vector *g0 = state->g0;
double pnorm = state->pnorm;
double g0norm = state->g0norm;
double fa = *f, fb, fc;
double dir;
double stepa = 0.0, stepb, stepc = state->step, tol = state->tol;
double g1norm;
double pg;
if (pnorm == 0.0 || g0norm == 0.0)
{
gsl_vector_set_zero (dx);
return GSL_ENOPROG;
}
/* Determine which direction is downhill, +p or -p */
gsl_blas_ddot (p, gradient, &pg);
dir = (pg >= 0.0) ? +1.0 : -1.0;
/* Compute new trial point at x_c= x - step * p, where p is the
current direction */
take_step (x, p, stepc, dir / pnorm, x1, dx);
/* Evaluate function and gradient at new point xc */
fc = GSL_MULTIMIN_FN_EVAL_F (fdf, x1);
if (fc < fa)
{
/* Success, reduced the function value */
state->step = stepc * 2.0;
*f = fc;
gsl_vector_memcpy (x, x1);
GSL_MULTIMIN_FN_EVAL_DF (fdf, x1, gradient);
return GSL_SUCCESS;
}
#ifdef DEBUG
printf ("got stepc = %g fc = %g\n", stepc, fc);
#endif
/* Do a line minimisation in the region (xa,fa) (xc,fc) to find an
intermediate (xb,fb) satisifying fa > fb < fc. Choose an initial
xb based on parabolic interpolation */
intermediate_point (fdf, x, p, dir / pnorm, pg,
stepa, stepc, fa, fc, x1, dx1, gradient, &stepb, &fb);
if (stepb == 0.0)
{
return GSL_ENOPROG;
}
minimize (fdf, x, p, dir / pnorm,
stepa, stepb, stepc, fa, fb, fc, tol,
x1, dx1, x2, dx, gradient, &(state->step), f, &g1norm);
gsl_vector_memcpy (x, x2);
/* Choose a new conjugate direction for the next step */
state->iter = (state->iter + 1) % x->size;
if (state->iter == 0)
{
gsl_vector_memcpy (p, gradient);
state->pnorm = g1norm;
}
else
{
/* p' = g1 - beta * p */
double beta = -pow (g1norm / g0norm, 2.0);
gsl_blas_dscal (-beta, p);
gsl_blas_daxpy (1.0, gradient, p);
state->pnorm = gsl_blas_dnrm2 (p);
}
state->g0norm = g1norm;
gsl_vector_memcpy (g0, gradient);
#ifdef DEBUG
printf ("updated conjugate directions\n");
printf ("p: ");
gsl_vector_fprintf (stdout, p, "%g");
printf ("g: ");
gsl_vector_fprintf (stdout, gradient, "%g");
#endif
return GSL_SUCCESS;
}
static int
vector_bfgs_iterate (void *vstate, gsl_multimin_function_fdf * fdf,
gsl_vector * x, double *f,
gsl_vector * gradient, gsl_vector * dx)
{
vector_bfgs_state_t *state = (vector_bfgs_state_t *) vstate;
gsl_vector *x1 = state->x1;
gsl_vector *dx1 = state->dx1;
gsl_vector *x2 = state->x2;
gsl_vector *p = state->p;
gsl_vector *g0 = state->g0;
gsl_vector *x0 = state->x0;
double pnorm = state->pnorm;
double g0norm = state->g0norm;
double fa = *f, fb, fc;
double dir;
double stepa = 0.0, stepb, stepc = state->step, tol = state->tol;
double g1norm;
double pg;
if (pnorm == 0.0 || g0norm == 0.0)
{
gsl_vector_set_zero (dx);
return GSL_ENOPROG;
}
/* Determine which direction is downhill, +p or -p */
gsl_blas_ddot (p, gradient, &pg);
dir = (pg >= 0.0) ? +1.0 : -1.0;
/* Compute new trial point at x_c= x - step * p, where p is the
current direction */
take_step (x, p, stepc, dir / pnorm, x1, dx);
/* Evaluate function and gradient at new point xc */
fc = GSL_MULTIMIN_FN_EVAL_F (fdf, x1);
if (fc < fa)
{
/* Success, reduced the function value */
state->step = stepc * 2.0;
*f = fc;
gsl_vector_memcpy (x, x1);
GSL_MULTIMIN_FN_EVAL_DF (fdf, x1, gradient);
return GSL_SUCCESS;
}
#ifdef DEBUG
printf ("got stepc = %g fc = %g\n", stepc, fc);
#endif
/* Do a line minimisation in the region (xa,fa) (xc,fc) to find an
intermediate (xb,fb) satisifying fa > fb < fc. Choose an initial
xb based on parabolic interpolation */
intermediate_point (fdf, x, p, dir / pnorm, pg,
stepa, stepc, fa, fc, x1, dx1, gradient, &stepb, &fb);
if (stepb == 0.0)
{
return GSL_ENOPROG;
}
minimize (fdf, x, p, dir / pnorm,
stepa, stepb, stepc, fa, fb, fc, tol,
x1, dx1, x2, dx, gradient, &(state->step), f, &g1norm);
gsl_vector_memcpy (x, x2);
/* Choose a new direction for the next step */
state->iter = (state->iter + 1) % x->size;
if (state->iter == 0)
{
gsl_vector_memcpy (p, gradient);
state->pnorm = g1norm;
}
else
{
/* This is the BFGS update: */
/* p' = g1 - A dx - B dg */
/* A = - (1+ dg.dg/dx.dg) B + dg.g/dx.dg */
/* B = dx.g/dx.dg */
gsl_vector *dx0 = state->dx0;
gsl_vector *dg0 = state->dg0;
double dxg, dgg, dxdg, dgnorm, A, B;
/* dx0 = x - x0 */
gsl_vector_memcpy (dx0, x);
gsl_blas_daxpy (-1.0, x0, dx0);
/* dg0 = g - g0 */
gsl_vector_memcpy (dg0, gradient);
gsl_blas_daxpy (-1.0, g0, dg0);
gsl_blas_ddot (dx0, gradient, &dxg);
gsl_blas_ddot (dg0, gradient, &dgg);
gsl_blas_ddot (dx0, dg0, &dxdg);
dgnorm = gsl_blas_dnrm2 (dg0);
if (dxdg != 0)
{
B = dxg / dxdg;
A = -(1.0 + dgnorm * dgnorm / dxdg) * B + dgg / dxdg;
}
else
{
B = 0;
A = 0;
}
gsl_vector_memcpy (p, gradient);
gsl_blas_daxpy (-A, dx0, p);
gsl_blas_daxpy (-B, dg0, p);
state->pnorm = gsl_blas_dnrm2 (p);
}
gsl_vector_memcpy (g0, gradient);
gsl_vector_memcpy (x0, x);
state->g0norm = gsl_blas_dnrm2 (g0);
#ifdef DEBUG
printf ("updated directions\n");
printf ("p: ");
gsl_vector_fprintf (stdout, p, "%g");
printf ("g: ");
gsl_vector_fprintf (stdout, gradient, "%g");
#endif
return GSL_SUCCESS;
}
