/*--
Cat lbxgl;Entity;Query
Form
LBXGL_Entity *LBXGL_Entity_QueryLineNext(LBXGL_Entity *ents, \
double *start, double *end);
Description
Query entities which intersect with the line defined by start and end.
Returns the first entity which intersects with the line.
Returns NULL if none match or the list is empty.
--*/
LBXGL_Entity *LBXGL_Entity_QueryLineNext(LBXGL_Entity *ents,
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;
while(cur)
{
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))
return(cur);
cur=cur->next;
}
return(NULL);
}
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 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;
}
int zgesv_idrs(
const size_t n,
// A is a function which multiplies the matrix by the first argument
// and returns the result in the second. The second argument must
// be manually cleared. The third parameter is user data, passed in
// through Adata.
void (*A)(const std::complex<double>*, std::complex<double>*, void*),
std::complex<double>* b,
std::complex<double>* x,
// Optional parameters
void *Adata = NULL,
size_t maxit = 0, // default is min(2*n,1000)
const size_t s = 4,
const double tol = 1e-8,
bool x_initialized = false,
// P is a precondition which simply solves P*x' = x,
// where x i the first argument. The second parameter is user data,
// which is passed in through Pdata.
void (*P)(std::complex<double>*, void*) = NULL,
void *Pdata = NULL,
double angle = 0.7
){
double normb = vecnorm(n, b);
if(0 == normb){
for(size_t i = 0; i < n; ++i){ x[i] = 0; }
return 0;
}
const double tolb = tol*normb; // compute tolerance
// Set initial x
if(!x_initialized){
for(size_t i = 0; i < n; ++i){ x[i] = 0; }
}
std::complex<double> *r = new std::complex<double>[n];
A(x,r,Adata);
for(size_t i = 0; i < n; ++i){ r[i] = b[i]-r[i]; }
double normr = vecnorm(n, r);
// Now, r = b-A*x
std::complex<double> *Q = new std::complex<double>[n*s];
{ // set up shadow space
for(size_t j = 0; j < s; ++j){
for(size_t i = 0; i < n; ++i){
Q[i+j*n] = (double)rand()/(double)RAND_MAX - 0.5;
}
}
// Orthogonalize Q
orth(n, s, Q);
}
std::complex<double> *G = new std::complex<double>[n*s];
std::complex<double> *U = new std::complex<double>[n*s];
std::complex<double> *M = new std::complex<double>[s*s];
std::complex<double> *Mcopy = new std::complex<double>[s*s];
size_t *pivots = new size_t[s];
for(size_t j = 0; j < s; ++j){
for(size_t i = 0; i < n; ++i){
G[i+j*n] = 0;
U[i+j*n] = 0;
}
for(size_t i = 0; i < s; ++i){
if(i == j){
M[i+j*s] = 1;
}else{
M[i+j*s] = 0;
}
}
}
std::complex<double> *f = new std::complex<double>[s];
std::complex<double> *c = new std::complex<double>[s];
std::complex<double> *v = new std::complex<double>[n];
std::complex<double> *t = new std::complex<double>[n];
size_t iter = 0;
std::complex<double> om = 1;
if(0 == maxit){
maxit = 2*n;
if(1000 < maxit){ maxit = 1000; }
}
int ret = 0;
while(normr > tolb && iter < maxit){
std::cout << "iter = " << iter << std::endl;
// generate RHS for small system
for(size_t j = 0; j < s; ++j){
std::complex<double> sum = 0;
for(size_t i = 0; i < n; ++i){
sum += r[i] * std::conj(Q[i+j*n]);
}
f[j] = sum;
}
for(size_t k = 0; k < s; ++k){
// solve small systems of M(k:s,k:s)*c(k:s) = f(k:s)
{
// Copy over stuff for a destructive LU solve in Mcopy
for(size_t j = k; j < s; ++j){
for(size_t i = k; i < s; ++i){
Mcopy[i+j*s] = M[i+j*s];
}
c[j] = f[j];
}
// Perform LU solve...
lu(s-k, s-k, s, &Mcopy[k+k*s], pivots);
lu_solve(s-k, s-k, s, &Mcopy[k+k*s], pivots, &c[k]);
}
// v = r - G(:,k:s)*c;
for(size_t i = 0; i < n; ++i){
std::complex<double> sum = 0;
for(size_t j = k; j < s; ++j){
sum += G[i+j*n]*c[j];
}
v[i] = r[i] - sum;
}
if(NULL != P){
P(v, Pdata);
}
//U(:,k) = U(:,k:s)*c + om*v;
for(size_t i = 0; i < n; ++i){
std::complex<double> sum = 0;
for(size_t j = k; j < s; ++j){
sum += U[i+j*n]*c[j];
}
U[i+k*n] = sum + om*v[i];
}
//G(:,k) = A*U(:,k);
A(&U[0+k*n], &G[0+k*n], Adata);
// Bi-Orthogonalise the new basis vectors
for(size_t j = 0; j < k; ++j){
std::complex<double> alpha = 0;
for(size_t i = 0; i < n; ++i){
alpha += std::conj(Q[i+j*n])*G[i+k*n];
}
alpha /= M[j+j*s];
for(size_t i = 0; i < n; ++i){
G[i+k*n] -= alpha*G[i+j*n];
}
for(size_t i = 0; i < n; ++i){
U[i+k*n] -= alpha*U[i+j*n];
}
}
// New column of M = (Q'*G)' (first k-1 entries are zero)
for(size_t j = k; j < s; ++j){
std::complex<double> sum = 0;
for(size_t i = 0; i < n; ++i){
sum += G[i+k*n]*std::conj(Q[i+j*n]);
}
M[j+k*s] = sum;
}
// Make r orthogonal to p_i, i = 1..k
std::complex<double> beta = f[k]/M[k+k*s];
for(size_t i = 0; i < n; ++i){
r[i] -= beta*G[i+k*n];
}
for(size_t i = 0; i < n; ++i){
x[i] += beta*U[i+k*n];
}
++iter;
normr = vecnorm(n, r);
if(normr < tolb || iter == maxit){ break; }
// New f = Q'*r (first k components are zero)
for(size_t j = k+1; j < s; ++j){
f[j] -= beta*M[j+k*s];
}
} // end k loop
// If we break'd out of the inner loop, do so again
if(normr < tolb){ break; }
// Now we have sufficient vectors in G_j to compute residual in G_j+1
// Note: r is already perpendicular to Q so v = r
for(size_t i = 0; i < n; ++i){ v[i] = r[i]; }
if(NULL != P){
P(v, Pdata);
}
A(v, t, Adata);
{ // compute new omega
double norms = vecnorm(n, r), normt = vecnorm(n, t);
std::complex<double> ts = 0;
for(size_t i = 0; i < n; ++i){
ts += std::conj(t[i])*r[i];
}
double rho = std::abs(ts/(normt*norms));
om = ts/(normt*normt);
if(rho < angle){
om *= angle/rho;
}
}
for(size_t i = 0; i < n; ++i){ r[i] -= om*t[i]; }
for(size_t i = 0; i < n; ++i){ x[i] += om*v[i]; }
normr = vecnorm(n, r);
++iter;
}
delete [] r;
delete [] G;
delete [] U;
delete [] M;
delete [] Mcopy;
delete [] f;
delete [] c;
delete [] v;
delete [] t;
return ret;
}