static void calcul_reflec(t_env *rt, t_vector *n, t_vector *tmp_reflect,
t_vector *ray)
{
if (rt->object[rt->i2].name != PLANE)
*n = vecsub(&rt->object[rt->i2].center, &rt->inter);
else
*n = rt->object[rt->i2].center;
normalize(n);
if ((rt->disk_s == 2 && rt->object[rt->i2].name == L_SPHERE) ||
(rt->disk_cy == 2 && rt->object[rt->i2].name == L_CYLINDER))
{
n->x = 0;
n->y = 1;
n->z = 0;
}
if (rt->disk_cy == 3 && rt->object[rt->i2].name == L_CYLINDER)
{
n->x = 0;
n->y = -1;
n->z = 0;
}
tmp_reflect->x = ray->x;
tmp_reflect->y = ray->y;
tmp_reflect->z = ray->z;
ray->x = -2 * n->x * vecdot(n, tmp_reflect) + tmp_reflect->x;
ray->y = -2 * n->y * vecdot(n, tmp_reflect) + tmp_reflect->y;
ray->z = -2 * n->z * vecdot(n, tmp_reflect) + tmp_reflect->z;
}
t_vec3 vecreflect(t_vec3 *i, t_vec3 *n)
{
t_vec3 v;
v.x = i->x - 2.0 * vecdot(n, i) * n->x;
v.y = i->y - 2.0 * vecdot(n, i) * n->y;
v.z = i->z - 2.0 * vecdot(n, i) * n->z;
return (v);
}
/*============================================================================
* Test routines.
*==========================================================================*/
int main(int argc, char **argv)
{
/*=======================================================================
* Vector operation tests
*=====================================================================*/
{ /* vecdot */
float X[3] = {1.0, 2.0, 3.0};
float Y[3] = {3.0, 2.0, 1.0};
assert(vecdot(X, Y, 3) == 10.0);
}
{ /* vecdot */
float X[3] = {10.0};
float Y[3] = {13.0};
assert(vecdot(X, Y, 1) == 130.0);
}
{ /* veclen */
float X[3] = {3.0, 4.0};
assert(veclen(X, 2) == 5.0);
}
{ /* vecmul */
float X[5] = {1.0, 2.0, 3.0, 4.0, 5.0};
float Y[5] = {2.0, 4.0, 6.0, 8.0, 10.0};
vecmul(X, 2, 5, X);
assert(vecveceq(X, Y, 5));
}
{ /* vecdiv */
float X[5] = {2.0, 4.0, 6.0, 8.0, 10.0};
float Y[5] = {1.0, 2.0, 3.0, 4.0, 5.0};
vecdiv(X, 2, 5, X);
assert(vecveceq(X, Y, 5));
}
{ /* vecvecsub */
float X[5] = {1.0, 2.0, 3.0, 4.0, 5.0};
float Y[5] = {2.0, 4.0, 6.0, 8.0, 10.0};
float Z[5] = {-1.0, -2.0, -3.0, -4.0, -5.0};
vecvecsub(X, Y, 5, X);
assert(vecveceq(X, Z, 5));
}
{ /* vecvecadd */
float X[5] = {1.0, 2.0, 3.0, 4.0, 5.0};
float Y[5] = {2.0, 4.0, 6.0, 8.0, 10.0};
float Z[5] = {3.0, 6.0, 9.0, 12.0, 15.0};
vecvecadd(X, Y, 5, X);
assert(vecveceq(X, Z, 5));
}
return 0;
}
static t_vector calcul_normal2(t_env *rt, t_figure object, t_vector light_ray,
t_vector n)
{
t_vector tmp;
t_vector tmp2;
if (object.name == TRIANGLE || object.name == QUADRILATERAL
|| object.name == CUBE)
{
tmp = vecsub(&object.a, &object.b);
tmp2 = vecsub(&object.a, &object.c);
normalize(&tmp);
normalize(&tmp2);
n = vecprod(&tmp, &tmp2);
}
if (object.name == ELLIPSOIDE)
vecscale(&n, 0.5);
if (object.name == TORUS)
n = normal_torus(rt->inter, object);
if (object.name == PARABOL)
n = normale_parab(rt->inter);
rt->angle = vecdot(&n, &light_ray) / (sqrt(light_ray.x * light_ray.x
+ light_ray.y * light_ray.y + light_ray.z * light_ray.z) * sqrt(n.x
* n.x + n.y * n.y + n.z * n.z));
return (n);
}
void crf1dc_beta_score(crf1d_context_t* ctx)
{
int i, t;
floatval_t *cur = NULL;
floatval_t *row = ctx->row;
const floatval_t *next = NULL, *state = NULL, *trans = NULL;
const int T = ctx->num_items;
const int L = ctx->num_labels;
const floatval_t *scale = &ctx->scale_factor[T-1];
/* Compute the beta scores at (T-1, *). */
cur = BETA_SCORE(ctx, T-1);
vecset(cur, *scale, L);
--scale;
/* Compute the beta scores at (t, *). */
for (t = T-2;0 <= t;--t) {
cur = BETA_SCORE(ctx, t);
next = BETA_SCORE(ctx, t+1);
state = EXP_STATE_SCORE(ctx, t+1);
veccopy(row, next, L);
vecmul(row, state, L);
/* Compute the beta score at (t, i). */
for (i = 0;i < L;++i) {
trans = EXP_TRANS_SCORE(ctx, i);
cur[i] = vecdot(trans, row, L);
}
vecscale(cur, *scale, L);
--scale;
}
}
/*--
Cat lbxgl;Entity;Query
Form
int LBXGL_Entity_QueryLine(LBXGL_Entity *ents, \
LBXGL_Entity **array, int max, \
double *start, double *end);
Description
Query entities which intersect with the line defined by start and end.
array is used to hold the query results, and max is the maximal \
number of entities that can be placed in array (at least max+1 \
entries should exist within array to handle the trailing NULL).
This returns the number of entities placed in array.
--*/
int LBXGL_Entity_QueryLine(LBXGL_Entity *ents,
LBXGL_Entity **array, int max,
double *start, double *end)
{
LBXGL_Entity *cur;
int i, j;
double *corg, *cmins, *cmaxs;
double crad, dx, dy, dz, dl, de;
double dir[3], px[3], py[3];
double rorg[3];
for(i=0; i<3; i++)dir[i]=end[i]-start[i];
vecnorm(dir);
perp_vector(dir, px);
veccross(dir, px, py);
for(i=0; i<3; i++)rorg[i]=end[i]-start[i];
de=vecdot(rorg, dir);
cur=ents;
i=0;
while(cur && (i<max))
{
corg=LBXGL_Entity_GetProperty(cur, "origin");
if(!corg)
{
cur=cur->next;
continue;
}
for(j=0; j<3; j++)rorg[j]=corg[j]-start[j];
dx=vecdot(rorg, px);
dy=vecdot(rorg, py);
dz=vecdot(rorg, dir);
dl=sqrt((dx*dx)+(dy*dy));
crad=LBXGL_Entity_GetRadius(cur);
if((dl<=crad) && (dz>=0) && (dz<=de))
{
array[i++]=cur;
array[i]=NULL;
}
cur=cur->next;
}
return(i);
}
void normalize(t_vector *v1)
{
double vdist;
vdist = sqrt(vecdot(v1, v1));
v1->x /= vdist;
v1->y /= vdist;
v1->z /= vdist;
}
static inline float computeDenominator(float invMass, float invInertia, const vec3* offset, const vec3* norm)
{
// If you apply an impulse of 1.0f in the direction of 'norm'
// at position specified by 'offset' then the point will change
// velocity by the amount calculated here
vec3 cross;
veccross(&cross, offset, norm);
vecscale(&cross, &cross, invInertia);
veccross(&cross, &cross, offset);
return vecdot(norm, &cross) + invMass;
}
static void doCorrection3(Chassis* c, const vec3* worldOffset, const vec3* axis, float requiredVelocityChange, float linearRatio)
{
const float u = 1.f - linearRatio;
// Add half to the linear
vec3 tmp;
vecaddscale(&c->vel, &c->vel, axis, requiredVelocityChange*linearRatio);
// Add the other half to the angular
vec3 cross;
veccross(&cross, worldOffset, axis);
veccross(&cross, &cross, worldOffset);
float angularResponse = vecdot(axis, &cross);
float angVelChange = requiredVelocityChange / angularResponse;
vec3 impulse;
vecscale(&impulse, axis, angVelChange*u);
veccross(&tmp, worldOffset, &impulse);
vecadd(&c->angVel, &c->angVel, &tmp);
}
static inline int perp_vector(double *dir, double *dst)
{
double tvec[3];
double l;
int i, j;
for(i=0; i<3; i++)
{
for(j=0; j<3; j++)tvec[j]=0;
tvec[i]=1;
l=vecdot(tvec, dir);
for(j=0; j<3; j++)tvec[j]-=l*tvec[j];
if(vecnorm(tvec))
{
for(j=0; j<3; j++)dst[j]=tvec[j];
return(0);
}
}
return(0);
}
int line_search_morethuente(
int n,
T *x,
T *f,
T *g,
T *s,
T *stp,
const T* xp,
const T* gp,
T *wa,
callback_data_t<T> *cd,
const lbfgs_parameter_t *param
)
{
int count = 0;
int brackt, stage1, uinfo = 0;
T dg;
T stx, fx, dgx;
T sty, fy, dgy;
T fxm, dgxm, fym, dgym, fm, dgm;
T finit, ftest1, dginit, dgtest;
T width, prev_width;
T stmin, stmax;
/* Check the input parameters for errors. */
if (*stp <= 0.) {
return LBFGSERR_INVALIDPARAMETERS;
}
/* Compute the initial gradient in the search direction. */
vecdot(&dginit, g, s, n);
/* Make sure that s points to a descent direction. */
if (0 < dginit) {
return LBFGSERR_INCREASEGRADIENT;
}
/* Initialize local variables. */
brackt = 0;
stage1 = 1;
finit = *f;
dgtest = param->ftol * dginit;
width = param->max_step - param->min_step;
prev_width = 2.0 * width;
/*
The variables stx, fx, dgx contain the values of the step,
function, and directional derivative at the best step.
The variables sty, fy, dgy contain the value of the step,
function, and derivative at the other endpoint of
the interval of uncertainty.
The variables stp, f, dg contain the values of the step,
function, and derivative at the current step.
*/
stx = sty = 0.;
fx = fy = finit;
dgx = dgy = dginit;
for (;;) {
/*
Set the minimum and maximum steps to correspond to the
present interval of uncertainty.
*/
if (brackt) {
stmin = min2(stx, sty);
stmax = max2(stx, sty);
} else {
stmin = stx;
stmax = *stp + 4.0 * (*stp - stx);
}
/* Clip the step in the range of [stpmin, stpmax]. */
if (*stp < param->min_step) *stp = param->min_step;
if (param->max_step < *stp) *stp = param->max_step;
/*
If an unusual termination is to occur then let
stp be the lowest point obtained so far.
*/
if ((brackt && ((*stp <= stmin || stmax <= *stp) || param->max_linesearch <= count + 1 || uinfo != 0)) || (brackt && (stmax - stmin <= param->xtol * stmax))) {
*stp = stx;
}
/*
Compute the current value of x:
x <- x + (*stp) * s.
*/
veccpy(x, xp, n);
vecadd(x, s, *stp, n);
/* Evaluate the function and gradient values. */
*f = cd->proc_evaluate(cd->instance, x, g, cd->n, *stp);
vecdot(&dg, g, s, n);
ftest1 = finit + *stp * dgtest;
++count;
/* Test for errors and convergence. */
if (brackt && ((*stp <= stmin || stmax <= *stp) || uinfo != 0)) {
/* Rounding errors prevent further progress. */
return LBFGSERR_ROUNDING_ERROR;
}
if (*stp == param->max_step && *f <= ftest1 && dg <= dgtest) {
/* The step is the maximum value. */
return LBFGSERR_MAXIMUMSTEP;
}
if (*stp == param->min_step && (ftest1 < *f || dgtest <= dg)) {
/* The step is the minimum value. */
return LBFGSERR_MINIMUMSTEP;
}
if (brackt && (stmax - stmin) <= param->xtol * stmax) {
/* Relative width of the interval of uncertainty is at most xtol. */
return LBFGSERR_WIDTHTOOSMALL;
}
if (param->max_linesearch <= count) {
/* Maximum number of iteration. */
return LBFGSERR_MAXIMUMLINESEARCH;
}
if (*f <= ftest1 && fabs(dg) <= param->gtol * (-dginit)) {
/* The sufficient decrease condition and the directional derivative condition hold. */
return count;
}
/*
In the first stage we seek a step for which the modified
function has a nonpositive value and nonnegative derivative.
*/
if (stage1 && *f <= ftest1 && min2(param->ftol, param->gtol) * dginit <= dg) {
stage1 = 0;
}
/*
A modified function is used to predict the step only if
we have not obtained a step for which the modified
function has a nonpositive function value and nonnegative
derivative, and if a lower function value has been
obtained but the decrease is not sufficient.
*/
if (stage1 && ftest1 < *f && *f <= fx) {
/* Define the modified function and derivative values. */
fm = *f - *stp * dgtest;
fxm = fx - stx * dgtest;
fym = fy - sty * dgtest;
dgm = dg - dgtest;
dgxm = dgx - dgtest;
dgym = dgy - dgtest;
/*
Call update_trial_interval() to update the interval of
uncertainty and to compute the new step.
*/
uinfo = update_trial_interval(
&stx, &fxm, &dgxm,
&sty, &fym, &dgym,
stp, &fm, &dgm,
stmin, stmax, &brackt
);
/* Reset the function and gradient values for f. */
fx = fxm + stx * dgtest;
fy = fym + sty * dgtest;
dgx = dgxm + dgtest;
dgy = dgym + dgtest;
} else {
/*
Call update_trial_interval() to update the interval of
uncertainty and to compute the new step.
*/
uinfo = update_trial_interval(
&stx, &fx, &dgx,
&sty, &fy, &dgy,
stp, f, &dg,
stmin, stmax, &brackt
);
}
/*
Force a sufficient decrease in the interval of uncertainty.
*/
if (brackt) {
if (0.66 * prev_width <= fabs(sty - stx)) {
*stp = stx + 0.5 * (sty - stx);
}
prev_width = width;
width = fabs(sty - stx);
}
}
return LBFGSERR_LOGICERROR;
}
int line_search_backtracking(
int n,
T *x,
T *f,
T *g,
T *s,
T *stp,
const T* xp,
const T* gp,
T *wp,
callback_data_t<T>*cd,
const lbfgs_parameter_t *param
)
{
int count = 0;
T width, dg;
T finit, dginit = 0., dgtest;
const T dec = 0.5, inc = 2.1;
/* Check the input parameters for errors. */
if (*stp <= 0.) {
return LBFGSERR_INVALIDPARAMETERS;
}
/* Compute the initial gradient in the search direction. */
vecdot(&dginit, g, s, n);
/* Make sure that s points to a descent direction. */
if (0 < dginit) {
return LBFGSERR_INCREASEGRADIENT;
}
/* The initial value of the objective function. */
finit = *f;
dgtest = param->ftol * dginit;
for (;;) {
veccpy(x, xp, n);
vecadd(x, s, *stp, n);
/* Evaluate the function and gradient values. */
*f = cd->proc_evaluate(cd->instance, x, g, cd->n, *stp);
++count;
if (*f > finit + *stp * dgtest) {
width = dec;
} else {
/* The sufficient decrease condition (Armijo condition). */
if (param->linesearch == LBFGS_LINESEARCH_BACKTRACKING_ARMIJO) {
/* Exit with the Armijo condition. */
return count;
}
/* Check the Wolfe condition. */
vecdot(&dg, g, s, n);
if (dg < param->wolfe * dginit) {
width = inc;
} else {
if(param->linesearch == LBFGS_LINESEARCH_BACKTRACKING_WOLFE) {
/* Exit with the regular Wolfe condition. */
return count;
}
/* Check the strong Wolfe condition. */
if(dg > -param->wolfe * dginit) {
width = dec;
} else {
/* Exit with the strong Wolfe condition. */
return count;
}
}
}
if (*stp < param->min_step) {
/* The step is the minimum value. */
return LBFGSERR_MINIMUMSTEP;
}
if (*stp > param->max_step) {
/* The step is the maximum value. */
return LBFGSERR_MAXIMUMSTEP;
}
if (param->max_linesearch <= count) {
/* Maximum number of iteration. */
return LBFGSERR_MAXIMUMLINESEARCH;
}
(*stp) *= width;
}
}
int lbfgs(
int n,
T *x,
T *ptr_fx,
typename FuncWrapper<T>::lbfgs_evaluate_t proc_evaluate,
typename FuncWrapper<T>::lbfgs_progress_t proc_progress,
void *instance,
lbfgs_parameter_t *_param
)
{
int ret;
int i, j, k, ls, end, bound;
T step;
/* Constant parameters and their default values. */
lbfgs_parameter_t param = (_param != NULL) ? (*_param) : _defparam;
const int m = param.m;
T *xp = NULL;
T *g = NULL, *gp = NULL, *pg = NULL;
T *d = NULL, *w = NULL, *pf = NULL;
iteration_data_t<T> *lm = NULL;
iteration_data_t<T>*it = NULL;
T ys, yy;
T xnorm, gnorm, beta;
T fx = 0.;
T rate = 0.;
typename LineSearchWrapper<T>::line_search_proc linesearch = line_search_morethuente;
/* Construct a callback data. */
callback_data_t<T> cd;
cd.n = n;
cd.instance = instance;
cd.proc_evaluate = proc_evaluate;
cd.proc_progress = proc_progress;
#if defined(USE_SSE) && (defined(__SSE__) || defined(__SSE2__))
/* Round out the number of variables. */
n = round_out_variables(n);
#endif/*defined(USE_SSE)*/
/* Check the input parameters for errors. */
if (n <= 0) {
return LBFGSERR_INVALID_N;
}
#if defined(USE_SSE) && (defined(__SSE__) || defined(__SSE2__))
if (n % 8 != 0) {
return LBFGSERR_INVALID_N_SSE;
}
if ((uintptr_t)(const void*)x % 16 != 0) {
return LBFGSERR_INVALID_X_SSE;
}
#endif/*defined(USE_SSE)*/
if (param.epsilon < 0.) {
return LBFGSERR_INVALID_EPSILON;
}
if (param.past < 0) {
return LBFGSERR_INVALID_TESTPERIOD;
}
if (param.delta < 0.) {
return LBFGSERR_INVALID_DELTA;
}
if (param.min_step < 0.) {
return LBFGSERR_INVALID_MINSTEP;
}
if (param.max_step < param.min_step) {
return LBFGSERR_INVALID_MAXSTEP;
}
if (param.ftol < 0.) {
return LBFGSERR_INVALID_FTOL;
}
if (param.linesearch == LBFGS_LINESEARCH_BACKTRACKING_WOLFE ||
param.linesearch == LBFGS_LINESEARCH_BACKTRACKING_STRONG_WOLFE) {
if (param.wolfe <= param.ftol || 1. <= param.wolfe) {
return LBFGSERR_INVALID_WOLFE;
}
}
if (param.gtol < 0.) {
return LBFGSERR_INVALID_GTOL;
}
if (param.xtol < 0.) {
return LBFGSERR_INVALID_XTOL;
}
if (param.max_linesearch <= 0) {
return LBFGSERR_INVALID_MAXLINESEARCH;
}
if (param.orthantwise_c < 0.) {
return LBFGSERR_INVALID_ORTHANTWISE;
}
if (param.orthantwise_start < 0 || n < param.orthantwise_start) {
return LBFGSERR_INVALID_ORTHANTWISE_START;
}
if (param.orthantwise_end < 0) {
param.orthantwise_end = n;
}
if (n < param.orthantwise_end) {
return LBFGSERR_INVALID_ORTHANTWISE_END;
}
if (param.orthantwise_c != 0.) {
switch (param.linesearch) {
case LBFGS_LINESEARCH_BACKTRACKING:
linesearch = line_search_backtracking_owlqn;
break;
default:
/* Only the backtracking method is available. */
return LBFGSERR_INVALID_LINESEARCH;
}
} else {
switch (param.linesearch) {
case LBFGS_LINESEARCH_MORETHUENTE:
linesearch = line_search_morethuente;
break;
case LBFGS_LINESEARCH_BACKTRACKING_ARMIJO:
case LBFGS_LINESEARCH_BACKTRACKING_WOLFE:
case LBFGS_LINESEARCH_BACKTRACKING_STRONG_WOLFE:
linesearch = line_search_backtracking;
break;
default:
return LBFGSERR_INVALID_LINESEARCH;
}
}
/* Allocate working space. */
xp = (T*)vecalloc(n * sizeof(T));
g = (T*)vecalloc(n * sizeof(T));
gp = (T*)vecalloc(n * sizeof(T));
d = (T*)vecalloc(n * sizeof(T));
w = (T*)vecalloc(n * sizeof(T));
if (xp == NULL || g == NULL || gp == NULL || d == NULL || w == NULL) {
ret = LBFGSERR_OUTOFMEMORY;
goto lbfgs_exit;
}
if (param.orthantwise_c != 0.) {
/* Allocate working space for OW-LQN. */
pg = (T*)vecalloc(n * sizeof(T));
if (pg == NULL) {
ret = LBFGSERR_OUTOFMEMORY;
goto lbfgs_exit;
}
}
/* Allocate limited memory storage. */
lm = (iteration_data_t<T>*)vecalloc(m * sizeof(iteration_data_t<T>));
if (lm == NULL) {
ret = LBFGSERR_OUTOFMEMORY;
goto lbfgs_exit;
}
/* Initialize the limited memory. */
for (i = 0;i < m;++i) {
it = &lm[i];
it->alpha = 0;
it->ys = 0;
it->s = (T*)vecalloc(n * sizeof(T));
it->y = (T*)vecalloc(n * sizeof(T));
if (it->s == NULL || it->y == NULL) {
ret = LBFGSERR_OUTOFMEMORY;
goto lbfgs_exit;
}
}
/* Allocate an array for storing previous values of the objective function. */
if (0 < param.past) {
pf = (T*)vecalloc(param.past * sizeof(T));
}
/* Evaluate the function value and its gradient. */
fx = cd.proc_evaluate(cd.instance, x, g, cd.n, 0);
if (0. != param.orthantwise_c) {
/* Compute the L1 norm of the variable and add it to the object value. */
xnorm = owlqn_x1norm(x, param.orthantwise_start, param.orthantwise_end);
fx += xnorm * param.orthantwise_c;
owlqn_pseudo_gradient(
pg, x, g, n,
T(param.orthantwise_c), param.orthantwise_start, param.orthantwise_end
);
}
/* Store the initial value of the objective function. */
if (pf != NULL) {
pf[0] = fx;
}
/*
Compute the direction;
we assume the initial hessian matrix H_0 as the identity matrix.
*/
if (param.orthantwise_c == 0.) {
vecncpy(d, g, n);
} else {
vecncpy(d, pg, n);
}
/*
Make sure that the initial variables are not a minimizer.
*/
vec2norm(&xnorm, x, n);
if (param.orthantwise_c == 0.) {
vec2norm(&gnorm, g, n);
} else {
vec2norm(&gnorm, pg, n);
}
if (xnorm < 1.0) xnorm = 1.0;
if (gnorm / xnorm <= param.epsilon) {
ret = LBFGS_ALREADY_MINIMIZED;
goto lbfgs_exit;
}
/* Compute the initial step:
step = 1.0 / sqrt(vecdot(d, d, n))
*/
vec2norminv(&step, d, n);
k = 1;
end = 0;
for (;;) {
/* Store the current position and gradient vectors. */
veccpy(xp, x, n);
veccpy(gp, g, n);
/* Search for an optimal step. */
if (param.orthantwise_c == 0.) {
ls = linesearch(n, x, &fx, g, d, &step, xp, gp, w, &cd, ¶m);
} else {
ls = linesearch(n, x, &fx, g, d, &step, xp, pg, w, &cd, ¶m);
owlqn_pseudo_gradient(
pg, x, g, n,
T(param.orthantwise_c), param.orthantwise_start, param.orthantwise_end
);
}
if (ls < 0) {
/* Revert to the previous point. */
veccpy(x, xp, n);
veccpy(g, gp, n);
ret = ls;
goto lbfgs_exit;
}
/* Compute x and g norms. */
vec2norm(&xnorm, x, n);
if (param.orthantwise_c == 0.) {
vec2norm(&gnorm, g, n);
} else {
vec2norm(&gnorm, pg, n);
}
/* Report the progress. */
if (cd.proc_progress) {
if ((ret = cd.proc_progress(cd.instance, x, g, fx, xnorm, gnorm, step, cd.n, k, ls))) {
goto lbfgs_exit;
}
}
/*
Convergence test.
The criterion is given by the following formula:
|g(x)| / \max(1, |x|) < \epsilon
*/
if (xnorm < 1.0) xnorm = 1.0;
if (gnorm / xnorm <= param.epsilon) {
/* Convergence. */
ret = LBFGS_SUCCESS;
break;
}
/*
Test for stopping criterion.
The criterion is given by the following formula:
(f(past_x) - f(x)) / f(x) < \delta
*/
if (pf != NULL) {
/* We don't test the stopping criterion while k < past. */
if (param.past <= k) {
/* Compute the relative improvement from the past. */
rate = (pf[k % param.past] - fx) / fx;
/* The stopping criterion. */
if (rate < param.delta) {
ret = LBFGS_STOP;
break;
}
}
/* Store the current value of the objective function. */
pf[k % param.past] = fx;
}
if (param.max_iterations != 0 && param.max_iterations < k+1) {
/* Maximum number of iterations. */
ret = LBFGSERR_MAXIMUMITERATION;
break;
}
/*
Update vectors s and y:
s_{k+1} = x_{k+1} - x_{k} = \step * d_{k}.
y_{k+1} = g_{k+1} - g_{k}.
*/
it = &lm[end];
vecdiff(it->s, x, xp, n);
vecdiff(it->y, g, gp, n);
/*
Compute scalars ys and yy:
ys = y^t \cdot s = 1 / \rho.
yy = y^t \cdot y.
Notice that yy is used for scaling the hessian matrix H_0 (Cholesky factor).
*/
vecdot(&ys, it->y, it->s, n);
vecdot(&yy, it->y, it->y, n);
it->ys = ys;
/*
Recursive formula to compute dir = -(H \cdot g).
This is described in page 779 of:
Jorge Nocedal.
Updating Quasi-Newton Matrices with Limited Storage.
Mathematics of Computation, Vol. 35, No. 151,
pp. 773--782, 1980.
*/
bound = (m <= k) ? m : k;
++k;
end = (end + 1) % m;
/* Compute the steepest direction. */
if (param.orthantwise_c == 0.) {
/* Compute the negative of gradients. */
vecncpy(d, g, n);
} else {
vecncpy(d, pg, n);
}
j = end;
for (i = 0;i < bound;++i) {
j = (j + m - 1) % m; /* if (--j == -1) j = m-1; */
it = &lm[j];
/* \alpha_{j} = \rho_{j} s^{t}_{j} \cdot q_{k+1}. */
vecdot(&it->alpha, it->s, d, n);
it->alpha /= it->ys;
/* q_{i} = q_{i+1} - \alpha_{i} y_{i}. */
vecadd(d, it->y, -it->alpha, n);
}
vecscale(d, ys / yy, n);
for (i = 0;i < bound;++i) {
it = &lm[j];
/* \beta_{j} = \rho_{j} y^t_{j} \cdot \gamma_{i}. */
vecdot(&beta, it->y, d, n);
beta /= it->ys;
/* \gamma_{i+1} = \gamma_{i} + (\alpha_{j} - \beta_{j}) s_{j}. */
vecadd(d, it->s, it->alpha - beta, n);
j = (j + 1) % m; /* if (++j == m) j = 0; */
}
/*
Constrain the search direction for orthant-wise updates.
*/
if (param.orthantwise_c != 0.) {
for (i = param.orthantwise_start;i < param.orthantwise_end;++i) {
if (d[i] * pg[i] >= 0) {
d[i] = 0;
}
}
}
/*
Now the search direction d is ready. We try step = 1 first.
*/
step = 1.0;
}
lbfgs_exit:
/* Return the final value of the objective function. */
if (ptr_fx != NULL) {
*ptr_fx = fx;
}
vecfree(pf);
/* Free memory blocks used by this function. */
if (lm != NULL) {
for (i = 0;i < m;++i) {
vecfree(lm[i].s);
vecfree(lm[i].y);
}
vecfree(lm);
}
vecfree(pg);
vecfree(w);
vecfree(d);
vecfree(gp);
vecfree(g);
vecfree(xp);
return ret;
}
int cg(
int n,
double* x,
double* pfx,
lbfgs_evaluate_t evaluate,
lbfgs_progress_t progress,
void* instance,
const lbfgs_parameter_t* _param
) {
static const double RHO = 0.01;
static const double SIG = 0.5;
static const double INT = 0.1;
static const double EXT = 3.0;
static const double RATIO = 100.0;
int ret;
int k, ls_count, ls_success, ls_failed = 0, n_evaluate = 0;
lbfgs_parameter_t param = (_param) ? (*_param) : default_param;
double f0, f1, f2 = 0.0, f3, d1, d2, d3, z1, z2 = 0.0, z3, limit, A, B, C;
double xnorm, gnorm, rate;
double* df0, *df1, *df2, *s, *x0;
double* pf = 0;
if (progress == 0) {
progress = default_lbfgs_progress;
}
if (n <= 0) {
return LBFGSERR_INVALID_N;
}
if (param.epsilon < 0.0) {
return LBFGSERR_INVALID_EPSILON;
}
if (param.past < 0) {
return LBFGSERR_INVALID_TESTPERIOD;
}
if (param.delta < 0.0) {
return LBFGSERR_INVALID_DELTA;
}
if (param.max_linesearch <= 0) {
return LBFGSERR_INVALID_MAXLINESEARCH;
}
df0 = vecalloc(n);
df1 = vecalloc(n);
df2 = vecalloc(n);
s = vecalloc(n);
x0 = vecalloc(n);
if (param.past > 0) {
pf = vecalloc((size_t)param.past);
}
f1 = evaluate(instance, n, x, df1, 0);
n_evaluate++;
if (pf) {
pf[0] = f1;
}
vec2norm(&xnorm, x, n);
vec2norm(&gnorm, df1, n);
if (xnorm < 1.0) {
xnorm = 1.0;
}
if (gnorm / xnorm <= param.epsilon) {
ret = LBFGS_ALREADY_MINIMIZED;
goto cg_exit;
}
vecncpy(s, df1, n);
vecdot(&d1, s, s, n);
d1 = -d1;
/**
* Compute the initial step z1:
*/
z1 = 1.0 / (1.0 - d1);
k = 1;
for (;;) {
/* Store the current position and gradient vectors. */
f0 = f1;
veccpy(x0, x, n);
veccpy(df0, df1, n);
/* update x using current step: x=x+z1*s */
vecadd(x, s, z1, n);
f2 = evaluate(instance, n, x, df2, 0);
n_evaluate++;
vecdot(&d2, df2, s, n);
/* set point 3 equal to point 1 */
f3 = f1;
d3 = d1;
z3 = -z1;
/* begin line search */
ls_success = 0;
ls_count = 0;
limit = -1.0;
for (;;) {
while (f2 > f1 + RHO * z1 * d1 || d2 > -SIG * d1) {
limit = z1;
if (f2 > f1) {
/* quadratic fit */
z2 = z3 - (0.5 * d3 * z3 * z3) / (d3 * z3 + f2 - f3);
} else {
/* cubic fit */
A = 6 * (f2 - f3) / z3 + 3 * (d2 + d3);
B = 3 * (f3 - f2) - z3 * (d3 + 2 * d2);
z2 = (sqrt(B * B - A * d2 * z3 * z3) - B) / A;
}
if (isinf(z2) || isnan(z2)) {
/* if we had a numerical problem then bisect */
z2 = z3 / 2.0;
}
/* don't accept too close to limits */
z2 = max2(min2(z2, INT* z3), (1.0 - INT) * z3);
/* update step and x */
z1 = z1 + z2;
vecadd(x, s, z2, n);
f2 = evaluate(instance, n, x, df2, 0);
n_evaluate++;
ls_count++;
vecdot(&d2, df2, s, n);
z3 = z3 - z2;
}
if (f2 > f1 + z1 * RHO * d1 || d2 > -SIG * d1) {
/* a line search failure */
break;
} else if (d2 > SIG * d1) {
/* a line search success */
ls_success = 1;
break;
} else if (ls_count >= param.max_linesearch) {
ret = LBFGSERR_MAXIMUMLINESEARCH;
goto cg_exit;
}
/* cubic extrapolation */
A = 6.0 * (f2 - f3) / z3 + 3.0 * (d2 + d3);
B = 3.0 * (f3 - f2) - z3 * (d3 + 2 * d2);
z2 = -d2 * z3 * z3 / (B + sqrt(B * B - A * d2 * z3 * z3));
/* adjust current step z2 for many cases */
if (isnan(z2) || isinf(z2) || z2 < 0.0) {
if (limit < -0.5) {
z2 = z1 * (EXT - 1.0);
} else {
z2 = (limit - z1) / 2.0;
}
} else if (limit > -0.5 && z2 + z1 > limit) {
z2 = (limit - z1) / 2.0;
} else if (limit < -0.5 && z2 + z1 > z1 * EXT) {
z2 = z1 * (EXT - 1.0);
} else if (z2 < -z3 * INT) {
z2 = -z3 * INT;
} else if (limit > -0.5 && z2 < (limit - z1) * (1.0 - INT)) {
z2 = (limit - z1) * (1.0 - INT);
}
/* set point 3 equal to point 2 */
f3 = f2;
d3 = d2;
z3 = -z2;
z1 = z1 + z2;
vecadd(x, s, z2, n);
f2 = evaluate(instance, n, x, df2, 0);
n_evaluate++;
ls_count++;
vecdot(&d2, df2, s, n);
}
if (ls_success) {
vec2norm(&xnorm, x, n);
vec2norm(&gnorm, df2, n);
if ((ret = progress(instance, n, x, df2, f2, xnorm, gnorm, z2, k, n_evaluate)) != 0) {
ret = LBFGSERR_CANCELED;
break;
}
if (xnorm < 1.0) {
xnorm = 1.0;
}
if (gnorm / xnorm <= param.epsilon) {
ret = LBFGS_CONVERGENCE;
break;
}
if (pf) {
if (param.past <= k) {
rate = (pf[k % param.past] - f2) / f2;
if (rate < param.delta) {
ret = LBFGS_CONVERGENCE_DELTA;
break;
}
}
pf[k % param.past] = f2;
}
if (param.max_iterations != 0 && param.max_iterations < k + 1) {
ret = LBFGSERR_MAXIMUMITERATION;
break;
}
k++;
f1 = f2;
/**
* Polack-Ribiere direction
* s = (df2'*df2-df1'*df2)/(df1'*df1)*s - df2
*/
vecdot(&A, df2, df2, n);
vecdot(&B, df1, df2, n);
vecdot(&C, df1, df1, n);
vecscale(s, (A - B) / C, n);
vecadd(s, df2, -1.0, n);
vecswap(df1, df2, n);
vecdot(&d2, df1, s, n);
if (d2 > 0) {
vecncpy(s, df1, n);
vecdot(&d2, s, s, n);
d2 = -d2;
}
z1 = z1 * min2(RATIO, d1 / (d2 - DBL_MIN));
d1 = d2;
ls_failed = 0;
} else {
/* restore previous point */
f1 = f0;
veccpy(x, x0, n);
veccpy(df1, df0, n);
if (ls_failed) {
/* line search failed twice */
ret = LBFGSERR_LINE_SEARCH_FAILED;
break;
}
vecswap(df1, df2, n);
vecncpy(s, df1, n);/* try steepest */
vecdot(&d1, s, s, n);
d1 = -d1;
z1 = 1.0 / (1.0 - d1);
ls_failed = 1;
}
}
cg_exit:
if (pfx) {
*pfx = f2;
}
vecfree(pf);
vecfree(x0);
vecfree(s);
vecfree(df2);
vecfree(df1);
vecfree(df0);
return ret;
}
int lbfgs(
int n,
double* x,
double* pfx,
lbfgs_evaluate_t evaluate,
lbfgs_progress_t progress,
void* instance,
const lbfgs_parameter_t* _param
) {
int ret;
int i, j, k, ls, end, bound, n_evaluate = 0;
int enalbe_owlqn;
double step;
lbfgs_parameter_t param = (_param) ? (*_param) : default_param;
const int m = param.m;
double* xp;
double* g, *gp, *pg = 0;
double* d, *w, *pf = 0;
iteration_data_t* lm = 0, *it = 0;
double ys, yy;
double xnorm, gnorm, rate, beta;
double fx;
line_search_proc_t linesearch = line_search_morethuente;
callback_data_t cd;
cd.n = n;
cd.instance = instance;
cd.evaluate = evaluate;
cd.progress = (progress) ? progress : default_lbfgs_progress;
/* Check the input parameters for errors. */
if (n <= 0) {
return LBFGSERR_INVALID_N;
}
if (param.epsilon < 0.0) {
return LBFGSERR_INVALID_EPSILON;
}
if (param.past < 0) {
return LBFGSERR_INVALID_TESTPERIOD;
}
if (param.delta < 0.0) {
return LBFGSERR_INVALID_DELTA;
}
if (param.min_step < 0.0) {
return LBFGSERR_INVALID_MINSTEP;
}
if (param.max_step < param.min_step) {
return LBFGSERR_INVALID_MAXSTEP;
}
if (param.ftol < 0.0) {
return LBFGSERR_INVALID_FTOL;
}
if (param.linesearch == LBFGS_LINESEARCH_BACKTRACKING_WOLFE ||
param.linesearch == LBFGS_LINESEARCH_BACKTRACKING_STRONG_WOLFE) {
if (param.wolfe <= param.ftol || 1. <= param.wolfe) {
return LBFGSERR_INVALID_WOLFE;
}
}
if (param.gtol < 0.0) {
return LBFGSERR_INVALID_GTOL;
}
if (param.xtol < 0.0) {
return LBFGSERR_INVALID_XTOL;
}
if (param.max_linesearch <= 0) {
return LBFGSERR_INVALID_MAXLINESEARCH;
}
if (param.orthantwise_c < 0.0) {
return LBFGSERR_INVALID_ORTHANTWISE;
}
if (param.orthantwise_start < 0 || param.orthantwise_start > n) {
return LBFGSERR_INVALID_ORTHANTWISE_START;
}
if (param.orthantwise_end < 0) {
param.orthantwise_end = n;
}
if (param.orthantwise_end > n) {
return LBFGSERR_INVALID_ORTHANTWISE_END;
}
enalbe_owlqn = (param.orthantwise_c != 0.0);
if (enalbe_owlqn) {
switch (param.linesearch) {
case LBFGS_LINESEARCH_BACKTRACKING_WOLFE:
linesearch = line_search_backtracking_owlqn;
break;
default:
/* Only the backtracking method is available. */
return LBFGSERR_INVALID_LINESEARCH;
}
} else {
switch (param.linesearch) {
case LBFGS_LINESEARCH_MORETHUENTE:
linesearch = line_search_morethuente;
break;
case LBFGS_LINESEARCH_BACKTRACKING_ARMIJO:
case LBFGS_LINESEARCH_BACKTRACKING_WOLFE:
case LBFGS_LINESEARCH_BACKTRACKING_STRONG_WOLFE:
linesearch = line_search_backtracking;
break;
default:
return LBFGSERR_INVALID_LINESEARCH;
}
}
/* Allocate working space. */
xp = vecalloc(n);
g = vecalloc(n);
gp = vecalloc(n);
d = vecalloc(n);
w = vecalloc(n);
/* Allocate pseudo gradient. */
if (enalbe_owlqn) {
pg = vecalloc(n);
}
/* Allocate and initialize the limited memory storage. */
lm = (iteration_data_t*)xalloc(m * sizeof(iteration_data_t));
for (i = 0; i < m; i++) {
it = &lm[i];
it->alpha = 0.0;
it->s = vecalloc(n);
it->y = vecalloc(n);
it->ys = 0.0;
}
/* Allocate an array for storing previous values of the objective function. */
if (param.past > 0) {
pf = vecalloc((size_t)param.past);
}
fx = cd.evaluate(cd.instance, cd.n, x, g, 0);
n_evaluate++;
if (enalbe_owlqn) {
xnorm = owlqn_x1norm(x, param.orthantwise_start, param.orthantwise_end);
fx += xnorm * param.orthantwise_c;
owlqn_pseudo_gradient(
pg, x, g, n,
param.orthantwise_c, param.orthantwise_start, param.orthantwise_end);
}
/* Store the initial value of the objective function. */
if (pf) {
pf[0] = fx;
}
/**
* Compute the direction.
* we assume the initial hessian matrix H_0 as the identity matrix.
*/
if (!enalbe_owlqn) {
vecncpy(d, g, n);
} else {
vecncpy(d, pg, n);
}
/**
* Make sure that the initial variables are not a minimizer.
*/
vec2norm(&xnorm, x, n);
if (!enalbe_owlqn) {
vec2norm(&gnorm, g, n);
} else {
vec2norm(&gnorm, pg, n);
}
if (xnorm < 1.0) {
xnorm = 1.0;
}
if (gnorm / xnorm <= param.epsilon) {
ret = LBFGS_ALREADY_MINIMIZED;
goto lbfgs_exit;
}
/**
* Compute the initial step:
* step = 1.0 / ||d||
*/
vec2norminv(&step, d, n);
k = 1;
end = 0;
for (;;) {
/* Store the current position and gradient vectors. */
veccpy(xp, x, n);
veccpy(gp, g, n);
/* Search for an optimal step. */
if (!enalbe_owlqn) {
ls = linesearch(n, x, &fx, g, d, &step, xp, gp, w, &cd, ¶m);
} else {
ls = linesearch(n, x, &fx, g, d, &step, xp, pg, w, &cd, ¶m);
owlqn_pseudo_gradient(
pg, x, g, n,
param.orthantwise_c, param.orthantwise_start, param.orthantwise_end
);
}
if (ls < 0) {
/* Revert to the previous point. */
veccpy(x, xp, n);
veccpy(g, gp, n);
ret = ls;
break;
}
n_evaluate += ls;
/* Compute x and g norms. */
vec2norm(&xnorm, x, n);
if (!enalbe_owlqn) {
vec2norm(&gnorm, g, n);
} else {
vec2norm(&gnorm, pg, n);
}
/* Report the progress. */
if ((ret = cd.progress(cd.instance, cd.n, x, g, fx, xnorm, gnorm, step, k, n_evaluate)) != 0) {
ret = LBFGSERR_CANCELED;
break;
}
/* Convergence test. */
if (xnorm < 1.0) {
xnorm = 1.0;
}
if (gnorm / xnorm <= param.epsilon) {
ret = LBFGS_CONVERGENCE;
break;
}
/* Stopping criterion test. */
if (pf) {
/* We don't test the stopping criterion while k < past. */
if (param.past <= k) {
/* Compute the relative improvement from the past. */
rate = (pf[k % param.past] - fx) / fx;
/* The stopping criterion. */
if (rate < param.delta) {
ret = LBFGS_CONVERGENCE_DELTA;
break;
}
}
/* Store the current value of the objective function. */
pf[k % param.past] = fx;
}
if (param.max_iterations != 0 && param.max_iterations < k + 1) {
ret = LBFGSERR_MAXIMUMITERATION;
break;
}
/**
* Update s and y:
* s_{k+1} = x_{k+1} - x_{k} = step * d_{k}
* y_{k+1} = g_{k+1} - g_{k}
*/
it = &lm[end];
vecdiff(it->s, x, xp, n);
vecdiff(it->y, g, gp, n);
/**
* Compute scalars ys and yy:
* ys = y^t s = 1 / \rho
* yy = y^t y
* Notice that yy is used for scaling the hessian matrix H_0 (Cholesky factor).
*/
vecdot(&ys, it->y, it->s, n);
vecdot(&yy, it->y, it->y, n);
it->ys = ys;
/**
* Recursive formula to compute d = -(H g).
* This is described in page 779 of:
* Jorge Nocedal.
* Updating Quasi-Newton Matrices with Limited Storage.
* Mathematics of Computation, Vol. 35, No. 151,
* pp. 773--782, 1980.
*/
bound = (m <= k) ? m : k;
k++;
end = (end + 1) % m;
/* Compute the steepest direction. */
/* Compute the negative of (pseudo) gradient. */
if (!enalbe_owlqn) {
vecncpy(d, g, n);
} else {
vecncpy(d, pg, n);
}
j = end;
for (i = 0; i < bound; i++) {
j = (j + m - 1) % m; /* if (--j == -1) j = m-1; */
it = &lm[j];
/* \alpha_{j} = \rho_{j} s^{t}_{j} q_{k+1} */
vecdot(&it->alpha, it->s, d, n);
it->alpha /= it->ys;
/* q_{i} = q_{i+1} - \alpha_{i} y_{i} */
vecadd(d, it->y, -it->alpha, n);
}
vecscale(d, ys / yy, n);
for (i = 0; i < bound; i++) {
it = &lm[j];
/* \beta_{j} = \rho_{j} y^t_{j} \gamma_{i} */
vecdot(&beta, it->y, d, n);
beta /= it->ys;
/* \gamma_{i+1} = \gamma_{i} + (\alpha_{j} - \beta_{j}) s_{j} */
vecadd(d, it->s, it->alpha - beta, n);
j = (j + 1) % m; /* if (++j == m) j = 0; */
}
/* Constrain the search direction for orthant-wise updates. */
if (enalbe_owlqn) {
owlqn_contrain_line_search(d, pg, param.orthantwise_start, param.orthantwise_end);
}
/* Now the search direction d is ready. We try step = 1 first. */
step = 1.0;
}
lbfgs_exit:
/* Return the final value of the objective function. */
if (pfx) {
*pfx = fx;
}
vecfree(pf);
if (lm != 0) {
for (i = 0; i < m; i++) {
vecfree(lm[i].s);
vecfree(lm[i].y);
}
xfree(lm);
}
vecfree(pg);
vecfree(w);
vecfree(d);
vecfree(gp);
vecfree(g);
vecfree(xp);
return ret;
}
static int l2sgd(
encoder_t *gm,
dataset_t *trainset,
dataset_t *testset,
floatval_t *w,
logging_t *lg,
const int N,
const floatval_t t0,
const floatval_t lambda,
const int num_epochs,
int calibration,
int period,
const floatval_t epsilon,
floatval_t *ptr_loss
)
{
int i, epoch, ret = 0;
floatval_t t = 0;
floatval_t loss = 0, sum_loss = 0;
floatval_t best_sum_loss = DBL_MAX;
floatval_t eta, gain, decay = 1.;
floatval_t improvement = 0.;
floatval_t norm2 = 0.;
floatval_t *pf = NULL;
floatval_t *best_w = NULL;
clock_t clk_prev, clk_begin = clock();
const int K = gm->num_features;
if (!calibration) {
pf = (floatval_t*)malloc(sizeof(floatval_t) * period);
best_w = (floatval_t*)calloc(K, sizeof(floatval_t));
if (pf == NULL || best_w == NULL) {
ret = CRFSUITEERR_OUTOFMEMORY;
goto error_exit;
}
}
/* Initialize the feature weights. */
vecset(w, 0, K);
/* Loop for epochs. */
for (epoch = 1;epoch <= num_epochs;++epoch) {
clk_prev = clock();
if (!calibration) {
logging(lg, "***** Epoch #%d *****\n", epoch);
/* Shuffle the training instances. */
dataset_shuffle(trainset);
}
/* Loop for instances. */
sum_loss = 0.;
for (i = 0;i < N;++i) {
const crfsuite_instance_t *inst = dataset_get(trainset, i);
/* Update various factors. */
eta = 1 / (lambda * (t0 + t));
decay *= (1.0 - eta * lambda);
gain = eta / decay;
/* Compute the loss and gradients for the instance. */
gm->set_weights(gm, w, decay);
gm->set_instance(gm, inst);
gm->objective_and_gradients(gm, &loss, w, gain);
sum_loss += loss;
++t;
}
/* Terminate when the loss is abnormal (NaN, -Inf, +Inf). */
if (!isfinite(loss)) {
logging(lg, "ERROR: overflow loss\n");
ret = CRFSUITEERR_OVERFLOW;
sum_loss = loss;
goto error_exit;
}
/* Scale the feature weights. */
vecscale(w, decay, K);
decay = 1.;
/* Include the L2 norm of feature weights to the objective. */
/* The factor N is necessary because lambda = 2 * C / N. */
norm2 = vecdot(w, w, K);
sum_loss += 0.5 * lambda * norm2 * N;
/* One epoch finished. */
if (!calibration) {
/* Check if the current epoch is the best. */
if (sum_loss < best_sum_loss) {
/* Store the feature weights to best_w. */
best_sum_loss = sum_loss;
veccopy(best_w, w, K);
}
/* We don't test the stopping criterion while period < epoch. */
if (period < epoch) {
improvement = (pf[(epoch-1) % period] - sum_loss) / sum_loss;
} else {
improvement = epsilon;
}
/* Store the current value of the objective function. */
pf[(epoch-1) % period] = sum_loss;
logging(lg, "Loss: %f\n", sum_loss);
if (period < epoch) {
logging(lg, "Improvement ratio: %f\n", improvement);
}
logging(lg, "Feature L2-norm: %f\n", sqrt(norm2));
logging(lg, "Learning rate (eta): %f\n", eta);
logging(lg, "Total number of feature updates: %.0f\n", t);
logging(lg, "Seconds required for this iteration: %.3f\n", (clock() - clk_prev) / (double)CLOCKS_PER_SEC);
/* Holdout evaluation if necessary. */
if (testset != NULL) {
holdout_evaluation(gm, testset, w, lg);
}
logging(lg, "\n");
/* Check for the stopping criterion. */
if (improvement < epsilon) {
ret = 0;
break;
}
}
}
/* Output the optimization result. */
if (!calibration) {
if (ret == 0) {
if (epoch < num_epochs) {
logging(lg, "SGD terminated with the stopping criteria\n");
} else {
logging(lg, "SGD terminated with the maximum number of iterations\n");
}
} else {
logging(lg, "SGD terminated with error code (%d)\n", ret);
}
}
/* Restore the best weights. */
if (best_w != NULL) {
sum_loss = best_sum_loss;
veccopy(w, best_w, K);
}
error_exit:
free(best_w);
free(pf);
if (ptr_loss != NULL) {
*ptr_loss = sum_loss;
}
return ret;
}
static int line_search_backtracking(
int n,
lbfgsfloatval_t *x,
lbfgsfloatval_t *f,
lbfgsfloatval_t *g,
lbfgsfloatval_t *s,
lbfgsfloatval_t *stp,
lbfgsfloatval_t *xp,
callback_data_t *cd,
const lbfgs_parameter_t *param
)
{
int i, ret = 0, count = 0;
lbfgsfloatval_t width = 0.5, norm = 0.;
lbfgsfloatval_t finit, dginit = 0., dgtest;
/* Check the input parameters for errors. */
if (*stp <= 0.) {
return LBFGSERR_INVALIDPARAMETERS;
}
/* Compute the initial gradient in the search direction. */
if (param->orthantwise_c != 0.) {
/* Compute the negative of gradients. */
for (i = 0;i < param->orthantwise_start;++i) {
dginit += s[i] * g[i];
}
/* Use psuedo-gradients for orthant-wise updates. */
for (i = param->orthantwise_start;i < n;++i) {
/* Notice that:
(-s[i] < 0) <==> (g[i] < -param->orthantwise_c)
(-s[i] > 0) <==> (param->orthantwise_c < g[i])
as the result of the lbfgs() function for orthant-wise updates.
*/
if (s[i] != 0.) {
if (x[i] < 0.) {
/* Differentiable. */
dginit += s[i] * (g[i] - param->orthantwise_c);
} else if (0. < x[i]) {
/* Differentiable. */
dginit += s[i] * (g[i] + param->orthantwise_c);
} else if (s[i] < 0.) {
/* Take the left partial derivative. */
dginit += s[i] * (g[i] - param->orthantwise_c);
} else if (0. < s[i]) {
/* Take the right partial derivative. */
dginit += s[i] * (g[i] + param->orthantwise_c);
}
}
}
} else {
vecdot(&dginit, g, s, n);
}
/* Make sure that s points to a descent direction. */
if (0 < dginit) {
return LBFGSERR_INCREASEGRADIENT;
}
/* The initial value of the objective function. */
finit = *f;
dgtest = param->ftol * dginit;
/* Copy the value of x to the work area. */
veccpy(xp, x, n);
for (;;) {
veccpy(x, xp, n);
vecadd(x, s, *stp, n);
if (param->orthantwise_c != 0.) {
/* The current point is projected onto the orthant of the initial one. */
for (i = param->orthantwise_start;i < n;++i) {
if (x[i] * xp[i] < 0.) {
x[i] = 0.;
}
}
}
/* Evaluate the function and gradient values. */
*f = cd->proc_evaluate(cd->instance, x, g, cd->n, *stp);
if (0. < param->orthantwise_c) {
/* Compute L1-regularization factor and add it to the object value. */
norm = 0.;
for (i = param->orthantwise_start;i < n;++i) {
norm += fabs(x[i]);
}
*f += norm * param->orthantwise_c;
}
++count;
if (*f <= finit + *stp * dgtest) {
/* The sufficient decrease condition. */
return count;
}
if (*stp < param->min_step) {
/* The step is the minimum value. */
ret = LBFGSERR_MINIMUMSTEP;
break;
}
if (param->max_linesearch <= count) {
/* Maximum number of iteration. */
ret = LBFGSERR_MAXIMUMLINESEARCH;
break;
}
(*stp) *= width;
}
/* Revert to the previous position. */
veccpy(x, xp, n);
return ret;
}
int lbfgs(
int n,
lbfgsfloatval_t *x,
lbfgsfloatval_t *ptr_fx,
lbfgs_evaluate_t proc_evaluate,
lbfgs_progress_t proc_progress,
void *instance,
lbfgs_parameter_t *_param
)
{
int ret;
int i, j, k, ls, end, bound;
lbfgsfloatval_t step;
/* Constant parameters and their default values. */
const lbfgs_parameter_t* param = (_param != NULL) ? _param : &_defparam;
const int m = param->m;
lbfgsfloatval_t *xp = NULL, *g = NULL, *gp = NULL, *d = NULL, *w = NULL;
iteration_data_t *lm = NULL, *it = NULL;
lbfgsfloatval_t ys, yy;
lbfgsfloatval_t norm, xnorm, gnorm, beta;
lbfgsfloatval_t fx = 0.;
line_search_proc linesearch = line_search_morethuente;
/* Construct a callback data. */
callback_data_t cd;
cd.n = n;
cd.instance = instance;
cd.proc_evaluate = proc_evaluate;
cd.proc_progress = proc_progress;
#if defined(USE_SSE) && (defined(__SSE__) || defined(__SSE2__))
/* Round out the number of variables. */
n = round_out_variables(n);
#endif/*defined(USE_SSE)*/
/* Check the input parameters for errors. */
if (n <= 0) {
return LBFGSERR_INVALID_N;
}
#if defined(USE_SSE) && (defined(__SSE__) || defined(__SSE2__))
if (n % 8 != 0) {
return LBFGSERR_INVALID_N_SSE;
}
if (((unsigned short)x & 0x000F) != 0) {
return LBFGSERR_INVALID_X_SSE;
}
#endif/*defined(USE_SSE)*/
if (param->min_step < 0.) {
return LBFGSERR_INVALID_MINSTEP;
}
if (param->max_step < param->min_step) {
return LBFGSERR_INVALID_MAXSTEP;
}
if (param->ftol < 0.) {
return LBFGSERR_INVALID_FTOL;
}
if (param->gtol < 0.) {
return LBFGSERR_INVALID_GTOL;
}
if (param->xtol < 0.) {
return LBFGSERR_INVALID_XTOL;
}
if (param->max_linesearch <= 0) {
return LBFGSERR_INVALID_MAXLINESEARCH;
}
if (param->orthantwise_c < 0.) {
return LBFGSERR_INVALID_ORTHANTWISE;
}
if (param->orthantwise_start < 0 || n < param->orthantwise_start) {
return LBFGSERR_INVALID_ORTHANTWISE_START;
}
switch (param->linesearch) {
case LBFGS_LINESEARCH_MORETHUENTE:
linesearch = line_search_morethuente;
break;
case LBFGS_LINESEARCH_BACKTRACKING:
linesearch = line_search_backtracking;
break;
default:
return LBFGSERR_INVALID_LINESEARCH;
}
/* Allocate working space. */
xp = (lbfgsfloatval_t*)vecalloc(n * sizeof(lbfgsfloatval_t));
g = (lbfgsfloatval_t*)vecalloc(n * sizeof(lbfgsfloatval_t));
gp = (lbfgsfloatval_t*)vecalloc(n * sizeof(lbfgsfloatval_t));
d = (lbfgsfloatval_t*)vecalloc(n * sizeof(lbfgsfloatval_t));
w = (lbfgsfloatval_t*)vecalloc(n * sizeof(lbfgsfloatval_t));
if (xp == NULL || g == NULL || gp == NULL || d == NULL || w == NULL) {
ret = LBFGSERR_OUTOFMEMORY;
goto lbfgs_exit;
}
/* Allocate limited memory storage. */
lm = (iteration_data_t*)vecalloc(m * sizeof(iteration_data_t));
if (lm == NULL) {
ret = LBFGSERR_OUTOFMEMORY;
goto lbfgs_exit;
}
/* Initialize the limited memory. */
for (i = 0;i < m;++i) {
it = &lm[i];
it->alpha = 0;
it->ys = 0;
it->s = (lbfgsfloatval_t*)vecalloc(n * sizeof(lbfgsfloatval_t));
it->y = (lbfgsfloatval_t*)vecalloc(n * sizeof(lbfgsfloatval_t));
if (it->s == NULL || it->y == NULL) {
ret = LBFGSERR_OUTOFMEMORY;
goto lbfgs_exit;
}
}
/* Evaluate the function value and its gradient. */
fx = cd.proc_evaluate(cd.instance, x, g, cd.n, 0);
if (0. < param->orthantwise_c) {
/* Compute L1-regularization factor and add it to the object value. */
norm = 0.;
for (i = param->orthantwise_start;i < n;++i) {
norm += fabs(x[i]);
}
fx += norm * param->orthantwise_c;
}
/* We assume the initial hessian matrix H_0 as the identity matrix. */
if (param->orthantwise_c == 0.) {
vecncpy(d, g, n);
} else {
/* Compute the negative of gradients. */
for (i = 0;i < param->orthantwise_start;++i) {
d[i] = -g[i];
}
/* Compute the negative of psuedo-gradients. */
for (i = param->orthantwise_start;i < n;++i) {
if (x[i] < 0.) {
/* Differentiable. */
d[i] = -g[i] + param->orthantwise_c;
} else if (0. < x[i]) {
/* Differentiable. */
d[i] = -g[i] - param->orthantwise_c;
} else {
if (g[i] < -param->orthantwise_c) {
/* Take the right partial derivative. */
d[i] = -g[i] - param->orthantwise_c;
} else if (param->orthantwise_c < g[i]) {
/* Take the left partial derivative. */
d[i] = -g[i] + param->orthantwise_c;
} else {
d[i] = 0.;
}
}
}
}
/*
Make sure that the initial variables are not a minimizer.
*/
vecnorm(&gnorm, g, n);
vecnorm(&xnorm, x, n);
if (xnorm < 1.0) xnorm = 1.0;
if (gnorm / xnorm <= param->epsilon) {
ret = LBFGS_ALREADY_MINIMIZED;
goto lbfgs_exit;
}
/* Compute the initial step:
step = 1.0 / sqrt(vecdot(d, d, n))
*/
vecrnorm(&step, d, n);
k = 1;
end = 0;
for (;;) {
/* Store the current position and gradient vectors. */
veccpy(xp, x, n);
veccpy(gp, g, n);
/* Search for an optimal step. */
ls = linesearch(n, x, &fx, g, d, &step, w, &cd, param);
if (ls < 0) {
ret = ls;
goto lbfgs_exit;
}
/* Compute x and g norms. */
vecnorm(&gnorm, g, n);
vecnorm(&xnorm, x, n);
/* Report the progress. */
if (cd.proc_progress) {
if (ret = cd.proc_progress(cd.instance, x, g, fx, xnorm, gnorm, step, cd.n, k, ls)) {
goto lbfgs_exit;
}
}
/*
Convergence test.
The criterion is given by the following formula:
|g(x)| / \max(1, |x|) < \epsilon
*/
if (xnorm < 1.0) xnorm = 1.0;
if (gnorm / xnorm <= param->epsilon) {
/* Convergence. */
ret = LBFGS_SUCCESS;
break;
}
if (param->max_iterations != 0 && param->max_iterations < k+1) {
/* Maximum number of iterations. */
ret = LBFGSERR_MAXIMUMITERATION;
break;
}
/*
Update vectors s and y:
s_{k+1} = x_{k+1} - x_{k} = \step * d_{k}.
y_{k+1} = g_{k+1} - g_{k}.
*/
it = &lm[end];
vecdiff(it->s, x, xp, n);
vecdiff(it->y, g, gp, n);
/*
Compute scalars ys and yy:
ys = y^t \cdot s = 1 / \rho.
yy = y^t \cdot y.
Notice that yy is used for scaling the hessian matrix H_0 (Cholesky factor).
*/
vecdot(&ys, it->y, it->s, n);
vecdot(&yy, it->y, it->y, n);
it->ys = ys;
/*
Recursive formula to compute dir = -(H \cdot g).
This is described in page 779 of:
Jorge Nocedal.
Updating Quasi-Newton Matrices with Limited Storage.
Mathematics of Computation, Vol. 35, No. 151,
pp. 773--782, 1980.
*/
bound = (m <= k) ? m : k;
++k;
end = (end + 1) % m;
if (param->orthantwise_c == 0.) {
/* Compute the negative of gradients. */
vecncpy(d, g, n);
} else {
/* Compute the negative of gradients. */
for (i = 0;i < param->orthantwise_start;++i) {
d[i] = -g[i];
}
/* Compute the negative of psuedo-gradients. */
for (i = param->orthantwise_start;i < n;++i) {
if (x[i] < 0.) {
/* Differentiable. */
d[i] = -g[i] + param->orthantwise_c;
} else if (0. < x[i]) {
/* Differentiable. */
d[i] = -g[i] - param->orthantwise_c;
} else {
if (g[i] < -param->orthantwise_c) {
/* Take the right partial derivative. */
d[i] = -g[i] - param->orthantwise_c;
} else if (param->orthantwise_c < g[i]) {
/* Take the left partial derivative. */
d[i] = -g[i] + param->orthantwise_c;
} else {
d[i] = 0.;
}
}
}
/* Store the steepest direction.*/
veccpy(w, d, n);
}
j = end;
for (i = 0;i < bound;++i) {
j = (j + m - 1) % m; /* if (--j == -1) j = m-1; */
it = &lm[j];
/* \alpha_{j} = \rho_{j} s^{t}_{j} \cdot q_{k+1}. */
vecdot(&it->alpha, it->s, d, n);
it->alpha /= it->ys;
/* q_{i} = q_{i+1} - \alpha_{i} y_{i}. */
vecadd(d, it->y, -it->alpha, n);
}
vecscale(d, ys / yy, n);
for (i = 0;i < bound;++i) {
it = &lm[j];
/* \beta_{j} = \rho_{j} y^t_{j} \cdot \gamma_{i}. */
vecdot(&beta, it->y, d, n);
beta /= it->ys;
/* \gamma_{i+1} = \gamma_{i} + (\alpha_{j} - \beta_{j}) s_{j}. */
vecadd(d, it->s, it->alpha - beta, n);
j = (j + 1) % m; /* if (++j == m) j = 0; */
}
/*
Constrain the search direction for orthant-wise updates.
*/
if (param->orthantwise_c != 0.) {
for (i = param->orthantwise_start;i < n;++i) {
if (d[i] * w[i] <= 0) {
d[i] = 0;
}
}
}
/*
Now the search direction d is ready. We try step = 1 first.
*/
step = 1.0;
}
lbfgs_exit:
/* Return the final value of the objective function. */
if (ptr_fx != NULL) {
*ptr_fx = fx;
}
/* Free memory blocks used by this function. */
if (lm != NULL) {
for (i = 0;i < m;++i) {
vecfree(lm[i].s);
vecfree(lm[i].y);
}
vecfree(lm);
}
vecfree(w);
vecfree(d);
vecfree(gp);
vecfree(g);
vecfree(xp);
return ret;
}
/* returns the Euclidean norm of vector x */
double norm ( double * x, int D ) {
return sqrt(vecdot(x,x,D));
}
void do_eigen_projections(int nev, float *M,
float **models, float *mean, int num_models,
eigen_t *eigen, int dim)
{
int i, m;
struct
{
float *proj;
float best_dot;
float best_dot_index;
char symbol;
} proj_model[2];
assert(models != NULL);
assert(mean != NULL);
assert(eigen != NULL);
assert(dim > 0);
assert(num_models > 0);
assert(0 < nev && nev <= dim);
proj_model[0].proj = (float *)malloc(dim * sizeof(float));
proj_model[1].proj = (float *)malloc(dim * sizeof(float));
proj_model[0].symbol = '+';
proj_model[1].symbol = '-';
if (!opt_multi_out) fprintf(out, "#BEGIN ENSEM\n");
for (i=0; i < nev; i++)
{
/*================================================================
* Find the models that have the largest and smallest projections
* onto the current eigenvector.
*==============================================================*/
proj_model[0].best_dot = 0;
proj_model[0].best_dot_index = -1;
proj_model[1].best_dot = 0;
proj_model[1].best_dot_index = -1;
for (m=0; m < num_models; m++)
{
float dot = vecdot(models[m], eigen[i].rvec, dim);
if (dot >= 0 && dot >= proj_model[0].best_dot)
{
proj_model[0].best_dot = dot;
proj_model[0].best_dot_index = m;
vecmul(eigen[i].rvec, dot, dim, proj_model[0].proj);
}
if (dot <= 0 && dot <= proj_model[1].best_dot)
{
proj_model[1].best_dot = dot;
proj_model[1].best_dot_index = m;
vecmul(eigen[i].rvec, dot, dim, proj_model[1].proj);
}
}
if (opt_interp_proj)
{
if (proj_model[0].best_dot_index != -1
&& proj_model[1].best_dot_index != -1)
{
int j;
float *newp = (float *)malloc(dim * sizeof(float));
float *step = (float *)malloc(dim * sizeof(float));
#define NSTEPS 20
#define OVERSHOOT 20
vecvecsub(proj_model[1].proj, proj_model[0].proj, dim, step);
vecdiv(step, NSTEPS, dim, step);
for (j=0; j < NSTEPS+OVERSHOOT; j++)
{
vecmul(step, j, dim, newp);
vecvecadd(proj_model[0].proj, newp, dim, newp);
undo_scale(newp, dim, M);
vecvecadd(newp, mean, dim, newp);
multi_out_write_ev_model((i+1)*100 + j, 'p', 0, 0,
newp, dim);
}
free(newp);
free(step);
#undef NSTEPS
#undef OVERSHOOT
}
}
else
{
/*================================================================
* Write out the new models
*==============================================================*/
for (m=0; m < 2; m++)
{
if (proj_model[m].best_dot_index != -1)
{
float proj_len_before_scale
= veclen(proj_model[m].proj, dim);
undo_scale(proj_model[m].proj, dim, M);
/* add back mean */
vecvecadd(proj_model[m].proj,mean, dim, proj_model[m].proj);
multi_out_write_ev_model(i,
proj_model[m].symbol,
proj_len_before_scale,
proj_model[m].best_dot,
proj_model[m].proj, dim);
}
}
}
}
if (!opt_multi_out)
fprintf(out, "#END ENSEM\n");
free(proj_model[0].proj);
free(proj_model[1].proj);
}
static void vehicleSubTick(Chassis* c, float dt)
{
if (g_step==0) return;
if (g_step&1) g_step = 0;
vec3* chassisPos = &c->pose.v[3].v3;
vec3* x = &c->pose.v[0].v3;
vec3* y = &c->pose.v[1].v3;
vec3* z = &c->pose.v[2].v3;
// This bit is done by the physics engine
if(1)
{
vecaddscale(chassisPos, chassisPos, &c->vel, dt);
mtx rot;
matrixRotateByVelocity(&rot, &c->pose, &c->angVel, dt);
matrixCopy33(&c->pose, &rot);
}
// Damp
vecscale(&c->vel, &c->vel, expf(-dt*1.f));
vecscale(&c->angVel, &c->angVel, expf(-dt*1.f));
if (fabsf(c->angVel.x)<0.01f) c->angVel.x = 0.f;
if (fabsf(c->angVel.y)<0.01f) c->angVel.y = 0.f;
if (fabsf(c->angVel.z)<0.01f) c->angVel.z = 0.f;
ClampedImpulse frictionImpulse[numWheels][2];
c->steer = g_steer;
//g_steer *= expf(-dt*3.f);
//g_speed *= expf(-dt*3.f);
static float latf = 10.f;
static float angSpeed = 0.f;
if (g_handBrake>0.f)
{
g_handBrake *= expf(-4.f*dt);
g_speed *= expf(-4.f*dt);
if (g_handBrake < 0.1f)
{
g_handBrake = 0.f;
}
}
// Prepare
for (int i=0; i<numWheels; i++)
{
Suspension* s = c->suspension[i];
Wheel* w = s->wheel;
// Calculate the world position and offset of the suspension point
vec3mtx33mulvec3(&s->worldOffset, &c->pose, &s->offset);
vec3mtx43mulvec3(&s->worldDefaultPos, &c->pose, &s->offset);
w->pos = s->worldDefaultPos;
vec3 pointVel = getPointVel(c, &s->worldOffset);
vecadd(&w->vel, &w->vel, &pointVel);
float maxFriction0 = 2.0f * dt * c->mass * gravity * (1.f/(float)numWheels);
clampedImpulseInit(&frictionImpulse[i][0], maxFriction0);
float latfriction = 10.f;
float newAngSpeed = vecdot(z, &c->angVel);
float changeAngSpeed = (newAngSpeed - angSpeed)/dt;
angSpeed = newAngSpeed;
printf("changeAngSpeed = %f\n", changeAngSpeed);
float speed = fabsf(vecdot(y, &c->vel));
const float base = 0.5f;
if (g_speed>=0 && i>=2)
{
latfriction = 1.f*expf(-5.f*g_handBrake) + base;
// latfriction = 1.f*expf(-2.f*fabsf(speed*changeAngSpeed)) + base;
// if (angSpeed*g_steer < -0.1f)
// {
// latfriction = base;
// }
//if (g_steer == 0.f)
//{
// latf += (10.f - latf) * (1.f - exp(-0.1f*dt));
//}
//else
//{
// latf += (0.1f - latf) * (1.f - exp(-10.f*dt));
//}
//latfriction = latf;
}
else
{
latfriction = 10.f;
}
float maxFriction1 = latfriction * dt * c->mass * gravity * (1.f/(float)numWheels);
clampedImpulseInit(&frictionImpulse[i][1], maxFriction1);
vecset(&s->hitNorm, 0.f, 0.f, 1.f);
float steer = w->maxSteer*c->steer * (1.f + 0.3f*s->offset.x*c->steer);
vecscale(&w->wheelAxis, x, cosf(steer));
vecsubscale(&w->wheelAxis, &w->wheelAxis, y, sinf(steer));
w->frictionDir[0];
veccross(&w->frictionDir[0], z, &w->wheelAxis);
w->frictionDir[1] = w->wheelAxis;
w->angSpeed = -40.f*g_speed;
}
//=============
// VERBOSE
//=============
#define verbose false
#define dump if (verbose) printf
dump("==========================================\n");
dump("START ITERATION\n");
dump("==========================================\n");
float solverERP = numIterations>1 ? 0.1f : 1.f;
float changeSolverERP = numIterations>1 ? (1.f - solverERP)/(0.01f+ (float)(numIterations-1)) : 0.f;
for (int repeat=0; repeat<numIterations; repeat++)
{
dump(" == Start Iter == \n");
for (int i=0; i<numWheels; i++)
{
Suspension* s = c->suspension[i];
Wheel* w = s->wheel;
const bool axisError = true;
const bool friction = true;
// Friction
if (friction)
{
vec3 lateralVel;
vecaddscale(&lateralVel, &w->vel, &s->hitNorm, -vecdot(&s->hitNorm, &w->vel));
vecaddscale(&lateralVel, &lateralVel, &w->frictionDir[0], +w->angSpeed * w->radius);
{
int dir = 0;
float v = vecdot(&lateralVel, &w->frictionDir[dir]);
float denom = 1.f/w->mass + w->radius*w->radius*w->invInertia;
float impulse = clampedImpulseApply(&frictionImpulse[i][dir], - solverERP * v / denom);
vec3 impulseV;
vecscale(&impulseV, &w->frictionDir[dir], impulse);
vecaddscale(&w->vel, &w->vel, &impulseV, 1.f/w->mass);
w->angSpeed = w->angSpeed + (impulse * w->radius * w->invInertia);
}
if (1)
{
int dir=1;
float v = vecdot(&lateralVel, &w->frictionDir[dir]);
float denom = 1.f/w->mass;
float impulse = clampedImpulseApply(&frictionImpulse[i][dir], - solverERP * v / denom);
vec3 impulseV;
vecscale(&impulseV, &w->frictionDir[dir], impulse);
vecaddscale(&w->vel, &w->vel, &impulseV, 1.f/w->mass);
}
//dump("gound collision errorV = %f, vel of wheel after = %f\n", penetration, vecdot(&w->vel, &s->hitNorm));
}
if (axisError) // Axis Error
{
vec3 offset;
vecsub(&offset, &w->pos, chassisPos);
vec3 pointvel = getPointVel(c, &offset);
vec3 error;
vecsub(&error, &pointvel, &w->vel);
vecaddscale(&error, &error, z, -vecdot(&error, z));
vec3 norm;
if (vecsizesq(&error)>0.001f)
{
dump("axis error %f\n", vecsize(&error));
vecnormalise(&norm, &error);
float denom = computeDenominator(1.f/c->mass, 1.f/c->inertia, &offset, &norm) + 1.f/w->mass;
vecscale(&error, &error, -solverERP/denom);
addImpulseAtOffset(&c->vel, &c->angVel, 1.f/c->mass, 1.f/c->inertia, &offset, &error);
vecaddscale(&w->vel, &w->vel, &error, -solverERP/w->mass);
}
//dump("axis error vel of wheel after = %f, inline = %f\n", vecdot(&w->vel, &s->hitNorm), vecdot(&w->vel, &s->axis));
}
}
solverERP += changeSolverERP;
}
for (int i=0; i<numWheels; i++)
{
Suspension* s = c->suspension[i];
Wheel* w = s->wheel;
vec3 pointVel = getPointVel(c, s);
// Convert suspension wheel speed back to car space
vecsub(&w->vel, &w->vel, &pointVel);
}
}
static int line_search_morethuente(
int n,
lbfgsfloatval_t *x,
lbfgsfloatval_t *f,
lbfgsfloatval_t *g,
lbfgsfloatval_t *s,
lbfgsfloatval_t *stp,
lbfgsfloatval_t *wa,
callback_data_t *cd,
const lbfgs_parameter_t *param
)
{
int i, count = 0;
int brackt, stage1, uinfo = 0;
lbfgsfloatval_t dg, norm;
lbfgsfloatval_t stx, fx, dgx;
lbfgsfloatval_t sty, fy, dgy;
lbfgsfloatval_t fxm, dgxm, fym, dgym, fm, dgm;
lbfgsfloatval_t finit, ftest1, dginit, dgtest;
lbfgsfloatval_t width, prev_width;
lbfgsfloatval_t stmin, stmax;
/* Check the input parameters for errors. */
if (*stp <= 0.) {
return LBFGSERR_INVALIDPARAMETERS;
}
/* Compute the initial gradient in the search direction. */
if (param->orthantwise_c != 0.) {
dginit = 0.;
for (i = 0;i < param->orthantwise_start;++i) {
dginit += s[i] * g[i];
}
/* Use psuedo-gradients for orthant-wise updates. */
for (i = param->orthantwise_start;i < n;++i) {
/* Notice that:
(-s[i] < 0) <==> (g[i] < -param->orthantwise_c)
(-s[i] > 0) <==> (param->orthantwise_c < g[i])
as the result of the lbfgs() function for orthant-wise updates.
*/
if (s[i] != 0.) {
if (x[i] < 0.) {
/* Differentiable. */
dginit += s[i] * (g[i] - param->orthantwise_c);
} else if (0. < x[i]) {
/* Differentiable. */
dginit += s[i] * (g[i] + param->orthantwise_c);
} else if (s[i] < 0.) {
/* Take the left partial derivative. */
dginit += s[i] * (g[i] - param->orthantwise_c);
} else if (0. < s[i]) {
/* Take the right partial derivative. */
dginit += s[i] * (g[i] + param->orthantwise_c);
}
}
}
} else {
vecdot(&dginit, g, s, n);
}
/* Make sure that s points to a descent direction. */
if (0 < dginit) {
return LBFGSERR_INCREASEGRADIENT;
}
/* Initialize local variables. */
brackt = 0;
stage1 = 1;
finit = *f;
dgtest = param->ftol * dginit;
width = param->max_step - param->min_step;
prev_width = 2.0 * width;
/* Copy the value of x to the work area. */
veccpy(wa, x, n);
/*
The variables stx, fx, dgx contain the values of the step,
function, and directional derivative at the best step.
The variables sty, fy, dgy contain the value of the step,
function, and derivative at the other endpoint of
the interval of uncertainty.
The variables stp, f, dg contain the values of the step,
function, and derivative at the current step.
*/
stx = sty = 0.;
fx = fy = finit;
dgx = dgy = dginit;
for (;;) {
/*
Set the minimum and maximum steps to correspond to the
present interval of uncertainty.
*/
if (brackt) {
stmin = min2(stx, sty);
stmax = max2(stx, sty);
} else {
stmin = stx;
stmax = *stp + 4.0 * (*stp - stx);
}
/* Clip the step in the range of [stpmin, stpmax]. */
if (*stp < param->min_step) *stp = param->min_step;
if (param->max_step < *stp) *stp = param->max_step;
/*
If an unusual termination is to occur then let
stp be the lowest point obtained so far.
*/
if ((brackt && ((*stp <= stmin || stmax <= *stp) || param->max_linesearch <= count + 1 || uinfo != 0)) || (brackt && (stmax - stmin <= param->xtol * stmax))) {
*stp = stx;
}
/*
Compute the current value of x:
x <- x + (*stp) * s.
*/
veccpy(x, wa, n);
vecadd(x, s, *stp, n);
if (param->orthantwise_c != 0.) {
/* The current point is projected onto the orthant of the previous one. */
for (i = param->orthantwise_start;i < n;++i) {
if (x[i] * wa[i] < 0.) {
x[i] = 0.;
}
}
}
/* Evaluate the function and gradient values. */
*f = cd->proc_evaluate(cd->instance, x, g, cd->n, *stp);
if (0. < param->orthantwise_c) {
/* Compute L1-regularization factor and add it to the object value. */
norm = 0.;
for (i = param->orthantwise_start;i < n;++i) {
norm += fabs(x[i]);
}
*f += norm * param->orthantwise_c;
dg = 0.;
for (i = 0;i < param->orthantwise_start;++i) {
dg += s[i] * g[i];
}
/* Use psuedo-gradients for orthant-wise updates. */
for (i = param->orthantwise_start;i < n;++i) {
if (x[i] < 0.) {
/* Differentiable. */
dg += s[i] * (g[i] - param->orthantwise_c);
} else if (0. < x[i]) {
/* Differentiable. */
dg += s[i] * (g[i] + param->orthantwise_c);
} else {
if (g[i] < -param->orthantwise_c) {
/* Take the right partial derivative. */
dg += s[i] * (g[i] + param->orthantwise_c);
} else if (param->orthantwise_c < g[i]) {
/* Take the left partial derivative. */
dg += s[i] * (g[i] - param->orthantwise_c);
} else {
/* dg += 0.; */
}
}
}
} else {
vecdot(&dg, g, s, n);
}
ftest1 = finit + *stp * dgtest;
++count;
/* Test for errors and convergence. */
if (brackt && ((*stp <= stmin || stmax <= *stp) || uinfo != 0)) {
/* Rounding errors prevent further progress. */
return LBFGSERR_ROUNDING_ERROR;
}
if (*stp == param->max_step && *f <= ftest1 && dg <= dgtest) {
/* The step is the maximum value. */
return LBFGSERR_MAXIMUMSTEP;
}
if (*stp == param->min_step && (ftest1 < *f || dgtest <= dg)) {
/* The step is the minimum value. */
return LBFGSERR_MINIMUMSTEP;
}
if (brackt && (stmax - stmin) <= param->xtol * stmax) {
/* Relative width of the interval of uncertainty is at most xtol. */
return LBFGSERR_WIDTHTOOSMALL;
}
if (param->max_linesearch <= count) {
/* Maximum number of iteration. */
return LBFGSERR_MAXIMUMLINESEARCH;
}
if (*f <= ftest1 && fabs(dg) <= param->gtol * (-dginit)) {
/* The sufficient decrease condition and the directional derivative condition hold. */
return count;
}
/*
In the first stage we seek a step for which the modified
function has a nonpositive value and nonnegative derivative.
*/
if (stage1 && *f <= ftest1 && min2(param->ftol, param->gtol) * dginit <= dg) {
stage1 = 0;
}
/*
A modified function is used to predict the step only if
we have not obtained a step for which the modified
function has a nonpositive function value and nonnegative
derivative, and if a lower function value has been
obtained but the decrease is not sufficient.
*/
if (stage1 && ftest1 < *f && *f <= fx) {
/* Define the modified function and derivative values. */
fm = *f - *stp * dgtest;
fxm = fx - stx * dgtest;
fym = fy - sty * dgtest;
dgm = dg - dgtest;
dgxm = dgx - dgtest;
dgym = dgy - dgtest;
/*
Call update_trial_interval() to update the interval of
uncertainty and to compute the new step.
*/
uinfo = update_trial_interval(
&stx, &fxm, &dgxm,
&sty, &fym, &dgym,
stp, &fm, &dgm,
stmin, stmax, &brackt
);
/* Reset the function and gradient values for f. */
fx = fxm + stx * dgtest;
fy = fym + sty * dgtest;
dgx = dgxm + dgtest;
dgy = dgym + dgtest;
} else {
/*
Call update_trial_interval() to update the interval of
uncertainty and to compute the new step.
*/
uinfo = update_trial_interval(
&stx, &fx, &dgx,
&sty, &fy, &dgy,
stp, f, &dg,
stmin, stmax, &brackt
);
}
/*
Force a sufficient decrease in the interval of uncertainty.
*/
if (brackt) {
if (0.66 * prev_width <= fabs(sty - stx)) {
*stp = stx + 0.5 * (sty - stx);
}
prev_width = width;
width = fabs(sty - stx);
}
}
return LBFGSERR_LOGICERROR;
}
static floatval_t
l2sgd_calibration(
encoder_t *gm,
dataset_t *ds,
floatval_t *w,
logging_t *lg,
const training_option_t* opt
)
{
int i, s;
int dec = 0, ok, trials = 1;
int num = opt->calibration_candidates;
clock_t clk_begin = clock();
floatval_t loss = 0.;
floatval_t init_loss = 0.;
floatval_t best_loss = DBL_MAX;
floatval_t eta = opt->calibration_eta;
floatval_t best_eta = opt->calibration_eta;
const int N = ds->num_instances;
const int S = MIN(N, opt->calibration_samples);
const int K = gm->num_features;
const floatval_t init_eta = opt->calibration_eta;
const floatval_t rate = opt->calibration_rate;
const floatval_t lambda = opt->lambda;
logging(lg, "Calibrating the learning rate (eta)\n");
logging(lg, "calibration.eta: %f\n", eta);
logging(lg, "calibration.rate: %f\n", rate);
logging(lg, "calibration.samples: %d\n", S);
logging(lg, "calibration.candidates: %d\n", num);
logging(lg, "calibration.max_trials: %d\n", opt->calibration_max_trials);
/* Initialize a permutation that shuffles the instances. */
dataset_shuffle(ds);
/* Initialize feature weights as zero. */
vecset(w, 0, K);
/* Compute the initial loss. */
gm->set_weights(gm, w, 1.);
init_loss = 0;
for (i = 0;i < S;++i) {
floatval_t score;
const crfsuite_instance_t *inst = dataset_get(ds, i);
gm->set_instance(gm, inst);
gm->score(gm, inst->labels, &score);
init_loss -= score;
gm->partition_factor(gm, &score);
init_loss += score;
}
init_loss += 0.5 * lambda * vecdot(w, w, K) * N;
logging(lg, "Initial loss: %f\n", init_loss);
while (num > 0 || !dec) {
logging(lg, "Trial #%d (eta = %f): ", trials, eta);
/* Perform SGD for one epoch. */
l2sgd(
gm,
ds,
NULL,
w,
lg,
S, 1.0 / (lambda * eta), lambda, 1, 1, 1, 0., &loss);
/* Make sure that the learning rate decreases the log-likelihood. */
ok = isfinite(loss) && (loss < init_loss);
if (ok) {
logging(lg, "%f\n", loss);
--num;
} else {
logging(lg, "%f (worse)\n", loss);
}
if (isfinite(loss) && loss < best_loss) {
best_loss = loss;
best_eta = eta;
}
if (!dec) {
if (ok && 0 < num) {
eta *= rate;
} else {
dec = 1;
num = opt->calibration_candidates;
eta = init_eta / rate;
}
} else {
eta /= rate;
}
++trials;
if (opt->calibration_max_trials <= trials) {
break;
}
}
eta = best_eta;
logging(lg, "Best learning rate (eta): %f\n", eta);
logging(lg, "Seconds required: %.3f\n", (clock() - clk_begin) / (double)CLOCKS_PER_SEC);
logging(lg, "\n");
return 1.0 / (lambda * eta);
}