int update_simplex(double ** simplex, int dim, double fmax, double ** fx, int ihi, double * midpoint, double * line, double scale, Search_settings *sett, Aux_arrays *aux, double *nS, double **sigaa, double **sigbb){
int i, o, j, update = 0;
double * next = alloc_vector(dim);
double * fx2;
double sinalt, cosalt, sindelt, cosdelt;
for (i = 0; i < dim; i++) next[i] = midpoint[i] + scale * line[i];
sinalt = sin(next[3]);
cosalt = cos(next[3]);
sindelt = sin(next[2]);
cosdelt = cos(next[2]);
nS[0] = cosalt*cosdelt;
nS[1] = sinalt*cosdelt;
nS[2] = sindelt;
for (o = 0; o < sett->nifo; ++o){
modvir(sinalt, cosalt, sindelt, cosdelt,
sett->N, &ifo[o], aux, sigaa[o], sigbb[o]);
}
fx2 = Fstatnet(sett, next, nS, sigaa, sigbb);
if (fx2[5] < fmax){
for (i = 0; i < dim; i++) simplex[ihi][i] = next[i];
for (j = 0; j < 11; j++) fx[ihi][j] = fx2[j];
update = 1;
}
free_vector(next, dim);
return update;
}
static u16 global_add(struct global_state *gstate,
struct string *name, value val)
{
struct symbol *pos;
ivalue old_size, aindex;
GCCHECK(val);
GCPRO2(gstate, name);
old_size = vector_len(gstate->environment->values);
aindex = env_add_entry(gstate->environment, val);
if (vector_len(gstate->environment->values) != old_size) /* Increase mvars too */
{
struct vector *new_mvars = alloc_vector(vector_len(gstate->environment->values));
memcpy(new_mvars->data, gstate->mvars->data,
gstate->mvars->o.size - sizeof(struct obj));
gstate->mvars = new_mvars;
}
GCPOP(2);
gstate->mvars->data[aindex] = makeint(var_normal);
pos = table_add_fast(gstate->global, name, makeint(aindex));
SET_READONLY(pos); /* index of global vars never changes */
return aindex;
}
void add_empty_vector(interp_core_type *interp) {
object_type *obj=0;
obj=alloc_vector(interp, 0);
interp->added=obj;
}
double * amoeba(Search_settings *sett, Aux_arrays *aux, double *point, double *nS, double *res_max, int dim, double tol, double *pc2, double **sigaa, double **sigbb){
int ihi, ilo, inhi;
// ihi = ih[0], ilo = ih[1], inhi = ih[2];
int *ih;
int j, i;
static double NM_out[11];
double ** fx = alloc_matrix(dim + 1, 11);
double * midpoint = alloc_vector(dim);
double * line = alloc_vector(dim);
double ** simplex = make_simplex(point, dim, pc2);
evaluate_simplex(simplex, dim, fx, sett, aux, nS, sigaa, sigbb);
while (true)
{
ih = simplex_extremes(fx, dim);
ihi = ih[0];
ilo = ih[1];
inhi = ih[2];
simplex_bearings(simplex, dim, midpoint, line, ihi);
if(check_tol(fx[ihi][5], fx[ilo][5], tol)) break;
update_simplex(simplex, dim, fx[ihi][5], fx, ihi, midpoint, line, -1.0, sett, aux, nS, sigaa, sigbb);
if (fx[ihi][5] < fx[ilo][5]){
update_simplex(simplex, dim, fx[ihi][5], fx, ihi, midpoint, line, -2.0, sett, aux, nS, sigaa, sigbb);
}
else if (fx[ihi][5] > fx[inhi][5]){
if (!update_simplex(simplex, dim, fx[ihi][5], fx, ihi, midpoint, line, 0.5, sett, aux, nS, sigaa, sigbb)){
contract_simplex(simplex, dim, fx, ilo, ihi, sett, aux, nS, sigaa, sigbb);
}
}
}
for (j = 0; j < dim; j++) point[j] = simplex[ilo][j];
for (j = 0; j < 11; j++) NM_out[j] = fx[ilo][j];
free_matrix(fx, dim + 1, 10);
free_vector(midpoint, dim);
free_vector(line, dim);
free_matrix(simplex, dim + 1, dim);
/* free(fx);
free(midpoint);
free(line);
free(simplex);
*/
return NM_out;
}
vector_t* getBeta(const mxArray* array, size_t L) {
if (mxGetN(array) != L) {
mexErrMsgTxt("Invalid dimension of beta vector.");
}
vector_t* beta = alloc_vector(L);
copyVector(beta, array);
return beta;
}
lispobj
alloc_string(const char *str)
{
int len = strlen(str);
lispobj result = alloc_vector(type_SimpleString, len + 1, 8);
struct vector *vec = (struct vector *) PTR(result);
vec->length = make_fixnum(len);
strcpy((char *) vec->data, str);
return result;
}
struct vector *copy_vector(struct vector *v)
{
struct vector *newp;
uvalue size = vector_len(v);
GCPRO1(v);
newp = alloc_vector(size);
memcpy(newp->data, v->data, size * sizeof(*v->data));
GCPOP(1);
return newp;
}
struct global_state *new_global_state(struct machine_specification *machine)
/* Returns: A new global state for a motlle interpreter for machine
*/
{
struct global_state *gstate;
value tmp;
GCPRO1(machine);
gstate = (struct global_state *)allocate_record(type_vector, 8);
GCPRO1(gstate);
tmp = alloc_table(DEF_TABLE_SIZE); gstate->modules = tmp;
tmp = alloc_vector(GLOBAL_SIZE); gstate->mvars = tmp;
tmp = alloc_vector(GLOBAL_SIZE); gstate->types = tmp;
tmp = alloc_vector(GLOBAL_SIZE); gstate->names = tmp;
tmp = alloc_table(GLOBAL_SIZE); gstate->global = tmp;
tmp = alloc_table(DEF_TABLE_SIZE); gstate->gsymbols = tmp;
tmp = alloc_env(GLOBAL_SIZE); gstate->environment = tmp;
gstate->machine = machine;
GCPOP(2);
return gstate;
}
static struct vector *make_arg_types(function f)
{
if (f->varargs)
return NULL;
int i = 0;
for (vlist a = f->args; a; a = a->next)
++i;
struct vector *result = alloc_vector(i);
for (vlist a = f->args; a; a = a->next)
result->data[--i] = makeint(a->typeset);
result->o.flags |= OBJ_READONLY | OBJ_IMMUTABLE;
return result;
}
// NOTE: this is NOT an efficient operation. it is only used by the
// reader, and requires at least 1 and up to 3 garbage collections!
static value_t vector_grow(value_t v)
{
size_t i, s = vector_size(v);
size_t d = vector_grow_amt(s);
PUSH(v);
value_t newv = alloc_vector(s+d, 1);
v = Stack[SP-1];
for(i=0; i < s; i++)
vector_elt(newv, i) = vector_elt(v, i);
// use gc to rewrite references from the old vector to the new
Stack[SP-1] = newv;
if (s > 0) {
((size_t*)ptr(v))[0] |= 0x1;
vector_elt(v, 0) = newv;
gc(0);
}
return POP();
}
static value make_array(cstlist csts, fncode fn)
{
struct list *l;
struct vector *v;
uvalue size = 0, i;
cstlist scan;
for (scan = csts; scan; scan = scan->next) size++;
/* This intermediate step is necessary as v is IMMUTABLE
(so must be allocated after its contents) */
l = make_list(NULL, csts, 0, FALSE, fn);
GCPRO1(l);
v = alloc_vector(size);
SET_IMMUTABLE(v); SET_READONLY(v);
GCPOP(1);
for (i = 0; i < size; i++, l = l->cdr) v->data[i] = l->car;
return v;
}
static value_t fl_vector_alloc(value_t *args, u_int32_t nargs)
{
fixnum_t i;
value_t f, v;
if (nargs == 0)
lerror(ArgError, "vector.alloc: too few arguments");
i = (fixnum_t)toulong(args[0], "vector.alloc");
if (i < 0)
lerror(ArgError, "vector.alloc: invalid size");
if (nargs == 2)
f = args[1];
else
f = FL_UNSPECIFIED;
v = alloc_vector((unsigned)i, f==FL_UNSPECIFIED);
if (f != FL_UNSPECIFIED) {
int k;
for(k=0; k < i; k++)
vector_elt(v,k) = f;
}
return v;
}
static value make_array(cstlist csts)
{
ulong size = 0;
for (cstlist scan = csts; scan; scan = scan->next) size++;
struct vector *v = alloc_vector(size);
GCPRO1(v);
for (cstlist scan = csts; scan; scan = scan->next)
{
value val = make_constant(scan->cst);
assert(immutablep(val));
v->data[--size] = val;
}
UNGCPRO();
assert(size == 0);
v->o.flags |= OBJ_IMMUTABLE | OBJ_READONLY;
return v;
}
lispobj
alloc_string(const char *str)
{
int k;
int len = strlen(str);
lispobj result = alloc_vector(type_SimpleString, len + 1, 16);
struct vector *vec = (struct vector *) PTR(result);
unsigned short int *wide_char_data;
vec->length = make_fixnum(len);
wide_char_data = (unsigned short int*) vec->data;
for (k = 0; k < len; ++k) {
wide_char_data[k] = str[k] & 0xff;
}
#if 0
fprintf(stderr, "alloc-string: 0x%lx %d -> `%s'\n",
result, len, str);
#endif
return result;
}
/* parse and add a vector */
void add_vector(interp_core_type *interp) {
object_type *obj=0;
object_type *next=0;
uint64_t len=0;
uint64_t i=0;
/* figure out how long the list is */
len=list_length(interp, interp->added);
/* create a new vector */
obj=alloc_vector(interp, len);
next=interp->added;
while(next!=interp->empty_list) {
obj->value.vector.vector[i]=car(next);
next=cdr(next);
i++;
}
interp->added=obj;
}
int bp(char *matfile, char *vecfile, char *oldbase, char *newbase)
{
int i, j=0;
int m, n = 4; /* m and n are rows and columns of matrix A */
float sum, rms;
float **A, **Acp, **At;
float **U, **Ut;
float **V, **Vt;
float **S, **St, **Si;
float *Sv;
float *b, *bT, *db, *x, *dx, *x0;
float *Utb, *SiUtb, *VSiUtb;
float **UUt, **VVt, **VtV, **US, **SVt, **USVt;
float bperp, dbperp, bpar, dbpar, btemp;
FILE *fp, *fpNew, *fpOld;
/*
* Part I: Singular Value Decomposition of matrix A
*/
/* printf("Beginning singular value decomposition of matrix A...\n");*/
/* determine number of rows 'm' */
m = get_matrix_rows(matfile,vecfile);
/* establish matrix A and vector b */
b = alloc_vector(1, m);
db = alloc_vector(1, m);
bT = alloc_vector(1, m);
x = alloc_vector(1, n);
dx = alloc_vector(1, n);
x0 = alloc_vector(1, n);
A = matrix(1, m, 1, n);
Acp = matrix(1, m, 1, n);
At = matrix(1, n, 1, m);
/* establish decomposition matrices */
U = matrix(1, m, 1, n);
Ut = matrix(1, n, 1, m);
S = matrix(1, n, 1, n);
St = matrix(1, n, 1, n);
Si = matrix(1, n, 1, n);
V = matrix(1, n, 1, n);
Vt = matrix(1, n, 1, n);
/* establish product matrices */
UUt = matrix(1, n, 1, n);
VVt = matrix(1, n, 1, n);
VtV = matrix(1, n, 1, n);
US = matrix(1, m, 1, n);
SVt = matrix(1, n, 1, n);
/* establish SVD product matrices */
USVt = matrix(1, m, 1, n);
/* vector version of diagonal matrix S */
Sv = alloc_vector(1, m);
/* vector products */
Utb = alloc_vector(1, n);
SiUtb = alloc_vector(1, n);
VSiUtb = alloc_vector(1, n);
/* read matrix and vector from input files */
fp = FOPEN(matfile,"r");
for (i = 1; i <= m; i++) {
for (j = 1; j <= n; j++) {
fscanf(fp, "%f", &A[i][j]);
}
}
fclose(fp);
fp = FOPEN(vecfile, "r");
for (i = 1; i <= m; i++)
fscanf(fp, "%f", &b[i]);
fclose(fp);
/* copy A into Acp */
for (i = 1; i <= m; i++) {
for (j = 1; j <= n; j++) {
Acp[i][j] = A[i][j];
}
}
/* transpose A into At */
for (i = 1; i <= m; i++) {
for (j = 1; j <= n; j++) {
At[j][i] = A[i][j];
}
}
/* NR fn to decompose A = U x S x Vt, where U is written into A */
svdcmp(A, m, n, Sv, V);
/* copy Sv into the diagonal of S and St */
for (i = 1; i <= 4; i++)
St[i][i] = S[i][i] = Sv[i];
/* copy A into U where it belongs, copy Acp back into A */
for (i = 1; i <= m; i++) {
for (j = 1; j <= n; j++) {
U[i][j] = A[i][j];
A[i][j] = Acp[i][j];
}
}
/* establish Ut and Vt */
for (i = 1; i <= m; i++) {
for (j = 1; j <= n; j++) {
Ut[j][i] = U[i][j];
}
}
for (i = 1; i <= n; i++) {
for (j = 1; j <= n; j++) {
Vt[j][i] = V[i][j];
}
}
/* check that SVD of A == A */
matrix_multiply(U, S, US, m, n, n);
matrix_multiply(US, Vt, USVt, m, n, n);
for (i = 1; i <= m; i++) {
for (j = 1; j <= n; j++) {
if (fabs(A[i][j] - USVt[i][j]) > 1e-12) {
/* FIXME: This check needs to be examined and reintroduced */
/* Exit(" reconstruction of A from SVD failed"); */
}
}
}
/* invert S into Si, automatically fixing small singular values */
for (i = 1; i <= n; i++) {
if (fabs(S[i][i]) < 0.0) {
Exit("svdcmp() found a negative singular value");
}
if (S[i][i] < 1e-6) {
printf(" singular value %d = %f; auto-set inverse to zero\n", i, S[i][i]);
Si[i][i] = 0.0;
}
else {
Si[i][i] = 1.0 / S[i][i];
}
}
/* breathe sigh of relief having gotten through SVD */
/* printf("\nSVD of A is ok\n\n");*/
/*
* Part II: Solve for n-vector x0
*/
/* multiply matrix Ut x vector b = vector Utb */
for (i = 1; i <= n; i++) {
for (j = 1, sum = 0.0; j <= m; j++) {
sum += Ut[i][j] * b[j];
}
Utb[i] = sum;
}
/* multiply matrix Si x vector Utb = vector SiUtb */
for (i = 1; i <= n; i++) {
SiUtb[i] = Si[i][i] * Utb[i];
}
/* multiply matrix V x vector SiUtb = vector VSiUtb */
for (i = 1; i <= n; i++) {
for (j = 1, sum = 0.0; j <= n; j++) {
sum += V[i][j] * SiUtb[j];
}
VSiUtb[i] = sum;
}
/* copy VSiUtb into x0 */
for (i = 1; i <= n; i++) {
x0[i] = VSiUtb[i];
}
/* calculate A x x0 */
for (i = 1; i <= m; i++) {
for (j = 1, sum = 0.0; j <= n; j++) {
sum += A[i][j] * x0[j];
}
bT[i] = sum;
}
/*print_vector(bT, 1, m, "b check, compare with ...");
print_vector(b, 1, m, "b");
print_vector(x0, 1, n, "x0");*/
for (i = 1, sum = 0.0; i <= m; i++) {
sum += (bT[i] - b[i])*(bT[i] - b[i]);
}
rms = sqrt(sum/(float)(m));
if (!quietflag) printf(" RMS of b-reconstructed and b = %f\n\n", rms);
/* test for sign of deltas */
fpOld = FOPEN(oldbase,"r");
fscanf(fpOld, "%f %f %f %f %f", &bperp, &dbperp, &bpar, &dbpar, &btemp);
fclose(fpOld);
printf(" New Baseline: Normal: %f, delta: %f\n"
" Parallel: %f, delta: %f\n"
" Temporal: %f days\n\n", x0[1], x0[2], x0[3], x0[4], btemp);
if (logflag) {
sprintf(logbuf," New Baseline: Normal: %f, delta: %f\n"
" Parallel: %f, delta: %f\n"
" Temporal: %f days\n\n", x0[1], x0[2], x0[3], x0[4], btemp);
printLog(logbuf);
}
fpNew = FOPEN(newbase,"w");
fprintf(fpNew, "%14.7f %14.7f %14.7f %14.7f %14.7f\n",
x0[1], x0[2], x0[3], x0[4], btemp);
fclose(fpNew);
/* free memory */
free_vector(b,1,m);
free_vector(db,1,m);
free_vector(bT,1,m);
free_vector(x,1,m);
free_vector(dx,1,m);
free_vector(x0,1,m);
free_matrix(A,1,n,1,m);
free_matrix(Acp,1,n,1,m);
free_matrix(At,1,n,1,m);
free_matrix(U,1,m,1,n);
free_matrix(Ut,1,n,1,m);
free_matrix(S,1,n,1,n);
free_matrix(St,1,n,1,n);
free_matrix(Si,1,n,1,n);
free_matrix(V,1,n,1,n);
free_matrix(Vt,1,n,1,n);
free_matrix(UUt,1,n,1,n);
free_matrix(VVt,1,n,1,n);
free_matrix(VtV,1,n,1,n);
free_matrix(US,1,n,1,n);
free_matrix(SVt,1,n,1,n);
free_matrix(USVt,1,m,1,n);
free_vector(Sv,1,m);
free_vector(Utb,1,n);
free_vector(SiUtb,1,n);
free_vector(VSiUtb,1,n);
return(0);
}
int
main(void)
{
int plot;
int i, j;
float x, y;
float *rt, *ct;
float **m;
int xsize, ysize;
char buf[256];
FILE *fout;
/* Create a few standard test interfaces */
for (plot = 0; plot < NUM_PLOTS; plot++) {
xsize = (TheRange[plot].xmax - TheRange[plot].xmin) * ISOSAMPLES + 1;
ysize = (TheRange[plot].ymax - TheRange[plot].ymin) * ISOSAMPLES + 1;
rt = alloc_vector(0, xsize - 1);
ct = alloc_vector(0, ysize - 1);
m = matrix(0, xsize - 1, 0, ysize - 1);
for (y = TheRange[plot].ymin, j = 0; j < ysize; j++, y += 1.0 / (double) ISOSAMPLES) {
ct[j] = y;
}
for (x = TheRange[plot].xmin, i = 0; i < xsize; i++, x += 1.0 / (double) ISOSAMPLES) {
rt[i] = x;
for (y = TheRange[plot].ymin, j = 0; j < ysize; j++, y += 1.0 / (double) ISOSAMPLES) {
m[i][j] = function(plot, x, y);
}
}
sprintf(buf, "binary%d", plot + 1);
if (!(fout = fopen(buf, "wb")))
int_error(0, "Could not open file");
else {
fwrite_matrix(fout, m, 0, xsize - 1, 0, ysize - 1, rt, ct);
}
free_vector(rt, 0);
free_vector(ct, 0);
free_matrix(m, 0, xsize - 1, 0);
}
/* Show that it's ok to vary sampling rate, as long as x1<x2, y1<y2... */
xsize = (TheRange[plot].xmax - TheRange[plot].xmin) * ISOSAMPLES + 1;
ysize = (TheRange[plot].ymax - TheRange[plot].ymin) * ISOSAMPLES + 1;
rt = alloc_vector(0, xsize - 1);
ct = alloc_vector(0, ysize - 1);
m = matrix(0, xsize - 1, 0, ysize - 1);
for (y = TheRange[plot].ymin, j = 0; j < ysize; j++, y += 1.0 / (double) ISOSAMPLES) {
ct[j] = y > 0 ? 2 * y : y;
}
for (x = TheRange[plot].xmin, i = 0; i < xsize; i++, x += 1.0 / (double) ISOSAMPLES) {
rt[i] = x > 0 ? 2 * x : x;
for (y = TheRange[plot].ymin, j = 0; j < ysize; j++, y += 1.0 / (double) ISOSAMPLES) {
m[i][j] = function(plot, x, y);
}
}
sprintf(buf, "binary%d", plot + 1);
if (!(fout = fopen(buf, "wb")))
int_error(0, "Could not open file");
else {
fwrite_matrix(fout, m, 0, xsize - 1, 0, ysize - 1, rt, ct);
}
free_vector(rt, 0);
free_vector(ct, 0);
free_matrix(m, 0, xsize - 1, 0);
return EXIT_SUCCESS;
}
/* export functions for interface.py */
vector_t * _alloc_vector(int size) { return alloc_vector(size); }
