long double zbrent(long double (func)(long double, long double, long double,
long double, int), long double par1, long double par2, long double es,
int n,long double x1, long double x2, long double tol)
{
int iter;
long double a=x1,b=x2,c=x2,d,e,min1,min2;
long double fa=(func)(a,par1,par2,es,n);
long double fb=(func)(b,par1,par2,es,n),fc,p,q,r,s,tol1,xm;
if ((fa > 0.0 && fb > 0.0) || (fa < 0.0 && fb < 0.0)){
return -1.0;
}
fc=fb;
for (iter=1;iter<=ITMAX;iter++) {
if ((fb > 0.0 && fc > 0.0) || (fb < 0.0 && fc < 0.0)) {
c=a;
fc=fa;
e=d=b-a;
}
if (fabsl(fc) < fabsl(fb)) {
a=b;
b=c;
c=a;
fa=fb;
fb=fc;
fc=fa;
}
tol1=2.0*EPS*fabsl(b)+0.5*tol;
xm=0.5*(c-b);
if (fabsl(xm) <= tol1 || fb == 0.0) {
return b;
}
if (fabsl(e) >= tol1 && fabsl(fa) > fabsl(fb)) {
s=fb/fa;
if (a == c) {
p=2.0*xm*s;
q=1.0-s;
} else {
q=fa/fc;
r=fb/fc;
p=s*(2.0*xm*q*(q-r)-(b-a)*(r-1.0));
q=(q-1.0)*(r-1.0)*(s-1.0);
}
if (p > 0.0) q = -q;
p=fabsl(p);
min1=3.0*xm*q-fabsl(tol1*q);
min2=fabsl(e*q);
if (2.0*p < (min1 < min2 ? min1 : min2)) {
e=d;
d=p/q;
} else {
d=xm;
e=d;
}
}
else {
d=xm;
e=d;
}
a=b;
fa=fb;
if (fabsl(d) > tol1)
b += d;
else
b += SIGN(tol1,xm);
fb=(*func)(b,par1,par2,es,n);
}
nrerror("Maximum number of iterations exceeded in zbrent");
return 0.0;
}
TSIL_REAL BrentMin (TSIL_REAL ax,
TSIL_REAL bx,
TSIL_REAL cx,
TSIL_REAL (*f)(TSIL_REAL),
TSIL_REAL tol,
TSIL_REAL *xmin)
{
int iter;
TSIL_REAL a,b,d,etemp,fu,fv,fw,fx,p,q,r,tol1,tol2,u,v,w,x,xm;
TSIL_REAL e = 0.0;
a = (ax < cx ? ax : cx);
b = (ax > cx ? ax : cx);
x = w = v = bx;
fw = fv = fx = (*f)(x);
for (iter=1; iter<=ITMAX; iter++) {
xm = 0.5L*(a + b);
tol2 = 2.0L*(tol1 = tol*TSIL_FABS(x) + ZEPS);
if (TSIL_FABS(x - xm) <= (tol2 - 0.5*(b-a))) {
*xmin = x;
/* printf("Brent: %d evaluations\n", iter); */
return fx;
}
if (TSIL_FABS(e) > tol1) {
r = (x - w)*(fx - fv);
q = (x - v)*(fx - fw);
p = (x - v)*q - (x - w)*r;
q = 2.0L*(q - r);
if (q > 0.0) p = -p;
q = TSIL_FABS(q);
etemp = e;
e = d;
if (TSIL_FABS(p) >= TSIL_FABS(0.5L*q*etemp) ||
p <= q*(a - x) ||
p >= q*(b - x))
d = CGOLD*(e = (x >= xm ? a-x : b-x));
else {
d = p/q;
u = x + d;
if (u-a < tol2 || b-u < tol2)
d = SIGN(tol1, xm-x);
}
}
else {
d = CGOLD*(e = (x >= xm ? a-x: b-x));
}
u = (TSIL_FABS(d) >= tol1 ? x+d : x + SIGN(tol1,d));
fu = (*f)(u);
if (fu <= fx) {
if (u >= x) a = x; else b = x;
SHFT(v,w,x,u) ;
SHFT(fv,fw,fx,fu) ;
}
else {
if (u < x) a = u; else b = u;
if (fu <= fw || w == x) {
v = w;
w = u;
fv = fw;
fw = fu;
}
else if (fu <= fv || v == x || v == w) {
v = u;
fv = fu;
}
}
}
TSIL_Error ("Brent", "Too many iterations", 42);
*xmin = x;
return fx;
}
double zbrent_err(double (*func)(double x, void *para, size_t N), double x1, double x2, double tol, void *para, size_t N)
// Using Brent’s method, find the root of a function func known to lie between x1 and x2. The root, returned as zbrent, will be refined until its accuracy is tol.
{
//struct my_params *fp=(struct my_params *)para;
//printf ("zbrent_err: %lf %lf\n", fp->a, fp->b);
int iter;
double a=x1,b=x2,c=x2,d,e,min1,min2;
double fa=(*func)(a, para, N),fb=(*func)(b, para, N),fc,p,q,r,s,tol1,xm;
if ((fa > 0.0 && fb > 0.0) || (fa < 0.0 && fb < 0.0))
puts ("Root must be bracketed in zbrent\n");
fc=fb;
for (iter=1;iter<=ITMAX;iter++)
{
if ((fb > 0.0 && fc > 0.0) || (fb < 0.0 && fc < 0.0))
{
c=a; // Rename a, b, c and adjust bounding interval d.
fc=fa;
e=d=b-a;
}
if (fabs(fc) < fabs(fb))
{
a=b;
b=c;
c=a;
fa=fb;
fb=fc;
fc=fa;
}
tol1=2.0*EPS*fabs(b)+0.5*tol; // Convergence check.
xm=0.5*(c-b);
if (fabs(xm) <= tol1 || fb == 0.0) return b;
if (fabs(e) >= tol1 && fabs(fa) > fabs(fb))
{
s=fb/fa; // Attempt inverse quadratic interpolation.
if (a == c)
{
p=2.0*xm*s;
q=1.0-s;
}
else
{
q=fa/fc;
r=fb/fc;
p=s*(2.0*xm*q*(q-r)-(b-a)*(r-1.0));
q=(q-1.0)*(r-1.0)*(s-1.0);
}
if (p > 0.0) q = -q; // Check whether in bounds.
p=fabs(p);
min1=3.0*xm*q-fabs(tol1*q);
min2=fabs(e*q);
if (2.0*p < (min1 < min2 ? min1 : min2))
{
e=d; // Accept interpolation.
d=p/q;
}
else
{
d=xm; // Interpolation failed, use bisection.
e=d;
}
}
else // Bounds decreasing too slowly, use bisection.
{
d=xm;
e=d;
}
a=b; // Move last best guess to a.
fa=fb;
if (fabs(d) > tol1) // Evaluate new trial root.
b += d;
else
b += SIGN(tol1,xm);
fb=(*func)(b, para, N);
}
puts ("Maximum number of iterations exceeded in zbrent\n");
return 0.0;
}
int main(int argc, char *argv[])
{
unsigned long p = 0xAA0000AA;
unsigned long q = 0x00550055;
unsigned long r = 0x00570057;
char buf[0x1000];
char *pre1, *pre2, *pre3, *res;
int pos, cpos, cmp, ccmp;
cp_bit_sequence *a = cp_bit_sequence_create(7, (unsigned char *) &p);
cp_bit_sequence *b = cp_bit_sequence_create(23, (unsigned char *) &q);
cp_bit_sequence *c = cp_bit_sequence_create(23, (unsigned char *) &r);
pre1 = cp_bit_sequence_to_string(a);
pre2 = cp_bit_sequence_to_string(b);
pre3 = cp_bit_sequence_to_string(c);
printf("comparing sequences b [%s] and c [%s]\n", pre2, pre3);
for (pos = 0; pos < c->length; pos++)
if (pre2[pos] != pre3[pos]) break;
cmp = pre2[pos] - pre3[pos];
printf("expected: b %c c, difference at bit %d\n", cmp > 0 ? '>' : '<', pos + 1);
ccmp = cp_bit_sequence_cmp(b, c, &cpos);
#define SIGN(p) ((p) > 0 ? 1 : -1)
printf("actual: b %c c, difference at bit %d - %s\n", cmp > 0 ? '>' : '<', cpos + 1,
(cpos == pos && SIGN(cmp) == SIGN(ccmp) ? "success" : "error"));
cp_bit_sequence_destroy(c);
free(pre3);
strcpy(buf, pre1);
strcat(buf, pre2);
cp_bit_sequence_cat(a, b);
res = cp_bit_sequence_to_string(a);
printf("testing [%s] + [%s]\n", pre1, pre2);
printf("expected: [%s]\n", buf);
printf("actual : [%s] - %s\n", res, (strcmp(res, buf) == 0) ? "success" : "error");
free(pre1); free(pre2); free(res);
cp_bit_sequence_cat(b, a);
pre1 = cp_bit_sequence_to_string(b);
pre2 = cp_bit_sequence_to_string(a);
strcpy(buf, pre1);
strcat(buf, pre2);
cp_bit_sequence_cat(b, a);
res = cp_bit_sequence_to_string(b);
printf("testing [%s] + [%s]\n", pre1, pre2);
printf("expected: [%s]\n", buf);
printf("actual : [%s] - %s\n", res, (strcmp(res, buf) == 0) ? "success" : "error");
free(pre1); free(pre2);
cp_bit_sequence_destroy(a);
printf("\ntesting shift left\n");
pre1 = res;
while (1)
{
pos = (b->length / 2) - 1;
if (pos <= 0) break;
a = cp_bit_sequence_dup(b);
cp_bit_sequence_shift_left(a, pos);
pre2 = cp_bit_sequence_to_string(a);
printf("testing [%s] << %d\n", pre1, pos);
pre1 = &pre1[pos];
printf("expected [%s]\n", pre1);
printf("actual [%s] - %s\n", pre2, (strcmp(pre1, pre2) == 0) ? "success" : "error");
cp_bit_sequence_destroy(b);
free(pre2);
b = a;
}
free(res);
cp_bit_sequence_destroy(b);
printf("\ntesting initialization from string\n");
pre1 = "10101010101010101000111010101011101000011100101010101010101111000111000110001";
a = cstr_to_bit_sequence(pre1);
pre2 = cp_bit_sequence_to_string(a);
printf("testing [%s]\n", pre1);
printf("expected [%s]\n", pre1);
printf("actual [%s] - %s\n", pre2, (strcmp(pre1, pre2) == 0) ? "success" : "error");
free(pre2);
pre2 = "101";
b = cstr_to_bit_sequence(pre2);
printf("testing compare: [%s], [%s]\n", pre1, pre2);
cmp = cp_bit_sequence_cmp(a, b, &pos);
printf("expected: a > b , pos 3\n");
printf("actual: cmp %s 0, pos %d - %s\n", cmp > 0 ? ">" : cmp < 0 ? "<" : "==", pos, (cmp > 0 && pos == 3) ? "success" : "error");
cp_bit_sequence_destroy(b);
pre2 = "100";
b = cstr_to_bit_sequence(pre2);
printf("testing compare: [%s], [%s]\n", pre2, pre1);
cmp = cp_bit_sequence_cmp(a, b, &pos);
printf("expected: cmp > 0, pos 2\n");
printf("actual: cmp %s 0, pos %d - %s\n", cmp > 0 ? ">" : cmp < 0 ? "<" : "==", pos, (cmp > 0 && pos == 2) ? "success" : "error");
cp_bit_sequence_destroy(a);
cp_bit_sequence_destroy(b);
test_short_strings();
return 0;
}
int svdcmp(float **a, int m, int n, float w[], float **v)
{// return 0 if result bad
float pythag(float a, float b);
int flag,i,its,j,jj,k,l,nm;
float anorm,c,f,g,h,s,scale,x,y,z,*rv1;
int res = 1;
float volatile temp;
rv1=vector(1,n);
g=scale=anorm=0.0;
for (i=1;i<=n;i++) {
l=i+1;
rv1[i]=scale*g;
g=s=scale=0.0;
if (i <= m) {
for (k=i;k<=m;k++) scale += fabs(a[k][i]);
if (scale) {
for (k=i;k<=m;k++) {
a[k][i] /= scale;
s += a[k][i]*a[k][i];
}
f=a[i][i];
g = -SIGN(sqrt(s),f);
h=f*g-s;
a[i][i]=f-g;
for (j=l;j<=n;j++) {
for (s=0.0,k=i;k<=m;k++) s += a[k][i]*a[k][j];
f=s/h;
for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
}
for (k=i;k<=m;k++) a[k][i] *= scale;
}
}
w[i]=scale *g;
g=s=scale=0.0;
if (i <= m && i != n) {
for (k=l;k<=n;k++) scale += fabs(a[i][k]);
if (scale) {
for (k=l;k<=n;k++) {
a[i][k] /= scale;
s += a[i][k]*a[i][k];
}
f=a[i][l];
g = -SIGN(sqrt(s),f);
h=f*g-s;
a[i][l]=f-g;
for (k=l;k<=n;k++) rv1[k]=a[i][k]/h;
for (j=l;j<=m;j++) {
for (s=0.0,k=l;k<=n;k++) s += a[j][k]*a[i][k];
for (k=l;k<=n;k++) a[j][k] += s*rv1[k];
}
for (k=l;k<=n;k++) a[i][k] *= scale;
}
}
anorm=FMAX(anorm,(fabs(w[i])+fabs(rv1[i])));
}
for (i=n;i>=1;i--) {
if (i < n) {
if (g) {
for (j=l;j<=n;j++)
v[j][i]=(a[i][j]/a[i][l])/g;
for (j=l;j<=n;j++) {
for (s=0.0,k=l;k<=n;k++) s += a[i][k]*v[k][j];
for (k=l;k<=n;k++) v[k][j] += s*v[k][i];
}
}
for (j=l;j<=n;j++) v[i][j]=v[j][i]=0.0;
}
v[i][i]=1.0;
g=rv1[i];
l=i;
}
for (i=IMIN(m,n);i>=1;i--) {
l=i+1;
g=w[i];
for (j=l;j<=n;j++) a[i][j]=0.0;
if (g) {
g=1.0/g;
for (j=l;j<=n;j++) {
for (s=0.0,k=l;k<=m;k++) s += a[k][i]*a[k][j];
f=(s/a[i][i])*g;
for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
}
for (j=i;j<=m;j++) a[j][i] *= g;
} else for (j=i;j<=m;j++) a[j][i]=0.0;
++a[i][i];
}
for (k=n;k>=1;k--) {
for (its=1;its<=30;its++) {
flag=1;
for (l=k;l>=1;l--) {
nm=l-1;
temp= (fabs(rv1[l])+anorm);
if (temp == anorm) {
flag=0;
break;
}
temp= (fabs(w[nm])+anorm);
if (temp == anorm) break;
}
if (flag) {
c=0.0;
s=1.0;
for (i=l;i<=k;i++) {
f=s*rv1[i];
rv1[i]=c*rv1[i];
temp= (fabs(f)+anorm);
if (temp == anorm) break;
g=w[i];
h=pythag(f,g);
w[i]=h;
h=1.0/h;
c=g*h;
s = -f*h;
for (j=1;j<=m;j++) {
y=a[j][nm];
z=a[j][i];
a[j][nm]=y*c+z*s;
a[j][i]=z*c-y*s;
}
}
}
z=w[k];
if (l == k) {
if (z < 0.0) {
w[k] = -z;
for (j=1;j<=n;j++) v[j][k] = -v[j][k];
}
break;
}
if (its == 40) {
cout<<"\nSVDCMP():no convergence in 40 svdcmp iterations"<<endl;
nrerror("no convergence in 40 svdcmp iterations",0);
//for (j=1;j<=m;j++) //by XDR
//for (jj=1;jj<i=n;jj++)
// v[j][jj]=0;
// free_vector(rv1,1,n);
//return 0; // result bad
res = 0;
}
x=w[l];
nm=k-1;
y=w[nm];
g=rv1[nm];
h=rv1[k];
f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
g=pythag(f,1.0);
f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
c=s=1.0;
for (j=l;j<=nm;j++) {
i=j+1;
g=rv1[i];
y=w[i];
h=s*g;
g=c*g;
z=pythag(f,h);
rv1[j]=z;
c=f/z;
s=h/z;
f=x*c+g*s;
g = g*c-x*s;
h=y*s;
y *= c;
for (jj=1;jj<=n;jj++) {
x=v[jj][j];
z=v[jj][i];
v[jj][j]=x*c+z*s;
v[jj][i]=z*c-x*s;
}
z=pythag(f,h);
w[j]=z;
if (z) {
z=1.0/z;
c=f*z;
s=h*z;
}
f=c*g+s*y;
x=c*y-s*g;
for (jj=1;jj<=m;jj++) {
y=a[jj][j];
z=a[jj][i];
a[jj][j]=y*c+z*s;
a[jj][i]=z*c-y*s;
}
}
rv1[l]=0.0;
rv1[k]=f;
w[k]=x;
}
}
free_vector(rv1,1,n);
return res; // good defined result when res = 1
}
cholmod_sparse *CHOLMOD(copy)
(
/* ---- input ---- */
cholmod_sparse *A, /* matrix to copy */
int stype, /* requested stype of C */
int mode, /* >0: numerical, 0: pattern, <0: pattern (no diag) */
/* --------------- */
cholmod_common *Common
)
{
cholmod_sparse *C ;
Int nrow, ncol, up, lo, values, diag, astype ;
/* ---------------------------------------------------------------------- */
/* check inputs */
/* ---------------------------------------------------------------------- */
RETURN_IF_NULL_COMMON (NULL) ;
RETURN_IF_NULL (A, NULL) ;
values = (mode > 0) && (A->xtype != CHOLMOD_PATTERN) ;
RETURN_IF_XTYPE_INVALID (A, CHOLMOD_PATTERN,
values ? CHOLMOD_REAL : CHOLMOD_ZOMPLEX, NULL) ;
nrow = A->nrow ;
ncol = A->ncol ;
if ((stype || A->stype) && nrow != ncol)
{
/* inputs invalid */
ERROR (CHOLMOD_INVALID, "matrix invalid") ;
return (NULL) ;
}
Common->status = CHOLMOD_OK ;
/* ---------------------------------------------------------------------- */
/* allocate workspace */
/* ---------------------------------------------------------------------- */
CHOLMOD(allocate_work) (0, MAX (nrow,ncol), 0, Common) ;
if (Common->status < CHOLMOD_OK)
{
/* out of memory */
return (NULL) ;
}
/* ---------------------------------------------------------------------- */
/* get inputs */
/* ---------------------------------------------------------------------- */
diag = (mode >= 0) ;
astype = SIGN (A->stype) ;
stype = SIGN (stype) ;
up = (astype > 0) ;
lo = (astype < 0) ;
/* ---------------------------------------------------------------------- */
/* copy the matrix */
/* ---------------------------------------------------------------------- */
if (astype == stype)
{
/* ------------------------------------------------------------------ */
/* symmetry of A and C are the same */
/* ------------------------------------------------------------------ */
/* copy A into C, keeping the same symmetry. If A is symmetric
* entries in the ignored part of A are not copied into C */
C = CHOLMOD(band) (A, -nrow, ncol, mode, Common) ;
}
else if (!astype)
{
/* ------------------------------------------------------------------ */
/* convert unsymmetric matrix A into a symmetric matrix C */
/* ------------------------------------------------------------------ */
if (stype > 0)
{
/* C = triu (A) */
C = CHOLMOD(band) (A, 0, ncol, mode, Common) ;
}
else
{
/* C = tril (A) */
C = CHOLMOD(band) (A, -nrow, 0, mode, Common) ;
}
if (Common->status < CHOLMOD_OK)
{
/* out of memory */
return (NULL) ;
}
C->stype = stype ;
}
else if (astype == -stype)
{
/* ------------------------------------------------------------------ */
/* transpose a symmetric matrix */
/* ------------------------------------------------------------------ */
/* converting upper to lower or lower to upper */
/* workspace: Iwork (nrow) */
C = CHOLMOD(transpose) (A, values, Common) ;
if (!diag)
{
/* remove diagonal, if requested */
CHOLMOD(band_inplace) (-nrow, ncol, -1, C, Common) ;
}
}
else
{
/* ------------------------------------------------------------------ */
/* create an unsymmetric copy of a symmetric matrix */
/* ------------------------------------------------------------------ */
C = copy_sym_to_unsym (A, mode, Common) ;
}
/* ---------------------------------------------------------------------- */
/* return if error */
/* ---------------------------------------------------------------------- */
if (Common->status < CHOLMOD_OK)
{
/* out of memory */
return (NULL) ;
}
/* ---------------------------------------------------------------------- */
/* return the result */
/* ---------------------------------------------------------------------- */
DEBUG (diag = CHOLMOD(dump_sparse) (C, "copy", Common)) ;
PRINT1 (("mode %d nnzdiag "ID"\n", mode, diag)) ;
ASSERT (IMPLIES (mode < 0, diag == 0)) ;
return (C) ;
}
void svdcmp(double** a, int m, int n, double* w, double** v)
{
int flag,i,its,j,jj,k,l,nm;
double c,f,h,s,x,y,z;
double anorm=0.0,g=0.0,scale=0.0;
double *rv1;
void nrerror();
if (m < n) ntrerror("SVDCMP: You must augment A with extra zero rows");
rv1=allocVect(n);
for (i=1;i<=n;i++) {
l=i+1;
rv1[i]=scale*g;
g=s=scale=0.0;
if (i <= m) {
for (k=i;k<=m;k++) scale += fabs(a[k][i]);
if (scale) {
for (k=i;k<=m;k++) {
a[k][i] /= scale;
s += a[k][i]*a[k][i];
}
f=a[i][i];
g = -SIGN(sqrt(s),f);
h=f*g-s;
a[i][i]=f-g;
if (i != n) {
for (j=l;j<=n;j++) {
for (s=0.0,k=i;k<=m;k++) s += a[k][i]*a[k][j];
f=s/h;
for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
}
}
for (k=i;k<=m;k++) a[k][i] *= scale;
}
}
w[i]=scale*g;
g=s=scale=0.0;
if (i <= m && i != n) {
for (k=l;k<=n;k++) scale += fabs(a[i][k]);
if (scale) {
for (k=l;k<=n;k++) {
a[i][k] /= scale;
s += a[i][k]*a[i][k];
}
f=a[i][l];
g = -SIGN(sqrt(s),f);
h=f*g-s;
a[i][l]=f-g;
for (k=l;k<=n;k++) rv1[k]=a[i][k]/h;
if (i != m) {
for (j=l;j<=m;j++) {
for (s=0.0,k=l;k<=n;k++) s += a[j][k]*a[i][k];
for (k=l;k<=n;k++) a[j][k] += s*rv1[k];
}
}
for (k=l;k<=n;k++) a[i][k] *= scale;
}
}
anorm=MAX(anorm,(fabs(w[i])+fabs(rv1[i])));
}
for (i=n;i>=1;i--) {
if (i < n) {
if (g) {
for (j=l;j<=n;j++)
v[j][i]=(a[i][j]/a[i][l])/g;
for (j=l;j<=n;j++) {
for (s=0.0,k=l;k<=n;k++) s += a[i][k]*v[k][j];
for (k=l;k<=n;k++) v[k][j] += s*v[k][i];
}
}
for (j=l;j<=n;j++) v[i][j]=v[j][i]=0.0;
}
v[i][i]=1.0;
g=rv1[i];
l=i;
}
for (i=n;i>=1;i--) {
l=i+1;
g=w[i];
if (i < n)
for (j=l;j<=n;j++) a[i][j]=0.0;
if (g) {
g=1.0/g;
if (i != n) {
for (j=l;j<=n;j++) {
for (s=0.0,k=l;k<=m;k++) s += a[k][i]*a[k][j];
f=(s/a[i][i])*g;
for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
}
}
for (j=i;j<=m;j++) a[j][i] *= g;
} else {
for (j=i;j<=m;j++) a[j][i]=0.0;
}
++a[i][i];
}
for (k=n;k>=1;k--) {
for (its=1;its<=30;its++) {
flag=1;
for (l=k;l>=1;l--) {
nm=l-1;
if (fabs(rv1[l])+anorm == anorm) {
flag=0;
break;
}
if (fabs(w[nm])+anorm == anorm) break;
}
if (flag) {
c=0.0;
s=1.0;
for (i=l;i<=k;i++) {
f=s*rv1[i];
if (fabs(f)+anorm != anorm) {
g=w[i];
h=PYTHAG(f,g);
w[i]=h;
h=1.0/h;
c=g*h;
s=(-f*h);
for (j=1;j<=m;j++) {
y=a[j][nm];
z=a[j][i];
a[j][nm]=y*c+z*s;
a[j][i]=z*c-y*s;
}
}
}
}
z=w[k];
if (l == k) {
if (z < 0.0) {
w[k] = -z;
for (j=1;j<=n;j++) v[j][k]=(-v[j][k]);
}
break;
}
if (its == 30) ntrerror("No convergence in 30 SVDCMP iterations");
x=w[l];
nm=k-1;
y=w[nm];
g=rv1[nm];
h=rv1[k];
f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
g=PYTHAG(f,1.0);
f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
c=s=1.0;
for (j=l;j<=nm;j++) {
i=j+1;
g=rv1[i];
y=w[i];
h=s*g;
g=c*g;
z=PYTHAG(f,h);
rv1[j]=z;
c=f/z;
s=h/z;
f=x*c+g*s;
g=g*c-x*s;
h=y*s;
y=y*c;
for (jj=1;jj<=n;jj++) {
x=v[jj][j];
z=v[jj][i];
v[jj][j]=x*c+z*s;
v[jj][i]=z*c-x*s;
}
z=PYTHAG(f,h);
w[j]=z;
if (z) {
z=1.0/z;
c=f*z;
s=h*z;
}
f=(c*g)+(s*y);
x=(c*y)-(s*g);
for (jj=1;jj<=m;jj++) {
y=a[jj][j];
z=a[jj][i];
a[jj][j]=y*c+z*s;
a[jj][i]=z*c-y*s;
}
}
rv1[l]=0.0;
rv1[k]=f;
w[k]=x;
}
}
freeVect(rv1);
}
void getlsampthresh(int N, double *t, double *mag, double *sig, double period, int harm_specsigflag, FILE *signalfile, int Nsubharm, int Nharm, double minPer, double thresh, double *ampthresh_scale, double *amp, int use_orig_ls)
{
/* This routine takes a light curve and a period, it fits a fourier series to the light curve to determine the signal, which is subtracted from the light curve, it then scales the signal and adds it back to the light curve and calculates the lomb-scargle probability at the specified period. It repeats, adjusting the scale until the LS probability is above the thresh-hold at which point the threshhold scaling factor will be returned. If the original light curve does not pass the signal, a negative factor will be returned.
Embedded in this routine is the zbrent algorithm (see Numerical Recipes in C) to find the zero of LS-Prob = thresh.
*/
double getlsampthresh_func(double amp_scale, int N, double *t, double *mag_orig, double *sig, double *mag_signal, double *mag_tmp, double period, int Nsubharm, int Nharm, double *subharmA, double *subharmB, double *harmA, double *harmB, double fundA, double fundB, double minPer, double thresh, int use_orig_ls);
int i;
double *subharmA, *subharmB, *harmA, *harmB, fundA, fundB, meanval, dum1, dum2;
double *t_tmp, *mag_tmp, *sig_tmp, *mag_signal, *mag_orig, val0, val1;
int iter;
double a, b, c, d, e, min1, min2, fa, fb, fc, p, q, r, s, tol1, xm, tol;
char *line;
size_t line_size = MAXLEN;
if((t_tmp = (double *) malloc(N * sizeof(double))) == NULL ||
(mag_tmp = (double *) malloc(N * sizeof(double))) == NULL ||
(sig_tmp = (double *) malloc(N * sizeof(double))) == NULL ||
(mag_signal = (double *) malloc(N * sizeof(double))) == NULL ||
(mag_orig = (double *) malloc(N * sizeof(double))) == NULL ||
(subharmA = (double *) malloc((Nsubharm + 1) * sizeof(double))) == NULL ||
(subharmB = (double *) malloc((Nsubharm + 1) * sizeof(double))) == NULL ||
(harmA = (double *) malloc((Nharm + 1) * sizeof(double))) == NULL ||
(harmB = (double *) malloc((Nharm + 1) * sizeof(double))) == NULL)
error(ERR_MEMALLOC);
/* First Get the signal */
/* Read the signal in from the file if we're doing that */
if(harm_specsigflag)
{
line = malloc(line_size);
rewind(signalfile);
val0 = 0.;
for(i=0;i<N;i++)
{
gnu_getline(&line,&line_size,signalfile);
sscanf(line,"%lf %lf %lf",&dum1,&dum2,&mag_signal[i]);
val0 += mag[i];
}
val0 = val0 / (double) N;
for(i=0;i<N;i++)
{
mag_signal[i] -= val0;
mag_orig[i] = mag[i] - mag_signal[i];
}
fclose(signalfile);
free(line);
}
/* Otherwise fit a fourier series at the period to compute the signal */
else
{
for(i=0;i<N;i++)
{
t_tmp[i] = t[i]; mag_tmp[i] = mag[i]; sig_tmp[i] = sig[i];
}
dokillharms(N, t_tmp, mag_tmp, sig_tmp, 1, &period, Nsubharm, Nharm, &subharmA, &subharmB, &harmA, &harmB, &fundA, &fundB, &meanval, 0, NULL, amp, 0, KILLHARM_OUTTYPE_DEFAULT, -1.);
for(i=0;i<N;i++)
{
mag_signal[i] = mag[i] - mag_tmp[i];
mag_orig[i] = mag_tmp[i];
}
}
/* Check to see if no signal still lies above the threshhold, or if the original signal lies below the threshhold */
val0 = getlsampthresh_func(0., N, t, mag_orig, sig, mag_signal, mag_tmp, period, Nsubharm, Nharm, subharmA, subharmB, harmA, harmB, fundA, fundB, minPer, thresh, use_orig_ls);
val1 = getlsampthresh_func(1., N, t, mag_orig, sig, mag_signal, mag_tmp, period, Nsubharm, Nharm, subharmA, subharmB, harmA, harmB, fundA, fundB, minPer, thresh, use_orig_ls);
if(val0 < 0)
{
*ampthresh_scale = 0.;
*amp = (*amp)*(*ampthresh_scale);
free(t_tmp);
free(mag_tmp);
free(sig_tmp);
free(mag_signal);
free(mag_orig);
free(subharmA);
free(subharmB);
free(harmA);
free(harmB);
return;
}
if(val1 > 0)
{
*ampthresh_scale = -1.;
*amp = (*amp)*(*ampthresh_scale);
free(t_tmp);
free(mag_tmp);
free(sig_tmp);
free(mag_signal);
free(mag_orig);
free(subharmA);
free(subharmB);
free(harmA);
free(harmB);
return;
}
a = 0.;
b = 1.;
tol = 1.0e-5;
/* Now start the zbrent loop */
fa = val0; fb = val1;
fc = fb;
for (iter=1; iter<=ITMAX; iter++) {
if ((fb > 0.0 && fc > 0.0) || (fb < 0.0 && fc < 0.0)) {
c = a;
fc = fa;
e=d=b-a;
}
if(fabs(fc) < fabs(fb)) {
a = b; b = c; c = a; fa = fb; fb = fc; fc = fa;
}
tol1 = 2.0*EPS*fabs(b)+0.5*tol;
xm=0.5*(c-b);
if (fabs(xm) <= tol1 || fb == 0.0)
{
*ampthresh_scale = b;
*amp = (*amp)*(*ampthresh_scale);
free(t_tmp);
free(mag_tmp);
free(sig_tmp);
free(mag_signal);
free(mag_orig);
free(subharmA);
free(subharmB);
free(harmA);
free(harmB);
return;
}
if (fabs(e) >= tol1 && fabs(fa) > fabs(fb)) {
s = fb/fa;
if (a == c) {
p = 2.0*xm*s;
q = 1.0 - s;
} else {
q = fa/fc;
r = fb/fc;
p = s*(2.0*xm*q*(q-r)-(b-a)*(r-1.0));
q = (q-1.0)*(r-1.0)*(s-1.0);
}
if (p > 0.0) q = -q;
p = fabs(p);
min1 = 3.0*xm*q - fabs(tol1*q);
min2 = fabs(e*q);
if (2.0*p < (min1 < min2 ? min1 : min2)) {
e=d;
d=p/q;
} else {
d=xm;
e=d;
}
} else {
d=xm;
e=d;
}
a=b;
fa=fb;
if (fabs(d) > tol1)
b += d;
else
b += SIGN(tol1,xm);
fb = getlsampthresh_func(b, N, t, mag_orig, sig, mag_signal, mag_tmp, period, Nsubharm, Nharm, subharmA, subharmB, harmA, harmB, fundA, fundB, minPer, thresh, use_orig_ls);
}
/* Too many iterations - set ampthresh_scale to -2. */
*ampthresh_scale = -2.;
*amp = (*amp)*(*ampthresh_scale);
free(t_tmp);
free(mag_tmp);
free(sig_tmp);
free(mag_signal);
free(mag_orig);
free(subharmA);
free(subharmB);
free(harmA);
free(harmB);
return;
}
static void
underflow (mpfr_exp_t e)
{
mpfr_t x, y, z1, z2;
mpfr_exp_t emin;
int i, k;
int prec;
int rnd;
int div;
int inex1, inex2;
unsigned int flags1, flags2;
/* Test mul_2si(x, e - k), div_2si(x, k - e) and div_2ui(x, k - e)
* with emin = e, x = 1 + i/16, i in { -1, 0, 1 }, and k = 1 to 4,
* by comparing the result with the one of a simple division.
*/
emin = mpfr_get_emin ();
set_emin (e);
mpfr_inits2 (8, x, y, (mpfr_ptr) 0);
for (i = 15; i <= 17; i++)
{
inex1 = mpfr_set_ui_2exp (x, i, -4, MPFR_RNDN);
MPFR_ASSERTN (inex1 == 0);
for (prec = 6; prec >= 3; prec -= 3)
{
mpfr_inits2 (prec, z1, z2, (mpfr_ptr) 0);
RND_LOOP (rnd)
for (k = 1; k <= 4; k++)
{
/* The following one is assumed to be correct. */
inex1 = mpfr_mul_2si (y, x, e, MPFR_RNDN);
MPFR_ASSERTN (inex1 == 0);
inex1 = mpfr_set_ui (z1, 1 << k, MPFR_RNDN);
MPFR_ASSERTN (inex1 == 0);
mpfr_clear_flags ();
/* Do not use mpfr_div_ui to avoid the optimization
by mpfr_div_2si. */
inex1 = mpfr_div (z1, y, z1, (mpfr_rnd_t) rnd);
flags1 = __gmpfr_flags;
for (div = 0; div <= 2; div++)
{
mpfr_clear_flags ();
inex2 = div == 0 ?
mpfr_mul_2si (z2, x, e - k, (mpfr_rnd_t) rnd) : div == 1 ?
mpfr_div_2si (z2, x, k - e, (mpfr_rnd_t) rnd) :
mpfr_div_2ui (z2, x, k - e, (mpfr_rnd_t) rnd);
flags2 = __gmpfr_flags;
if (flags1 == flags2 && SAME_SIGN (inex1, inex2) &&
mpfr_equal_p (z1, z2))
continue;
printf ("Error in underflow(");
if (e == MPFR_EMIN_MIN)
printf ("MPFR_EMIN_MIN");
else if (e == emin)
printf ("default emin");
else if (e >= LONG_MIN)
printf ("%ld", (long) e);
else
printf ("<LONG_MIN");
printf (") with mpfr_%s,\nx = %d/16, prec = %d, k = %d, "
"%s\n", div == 0 ? "mul_2si" : div == 1 ?
"div_2si" : "div_2ui", i, prec, k,
mpfr_print_rnd_mode ((mpfr_rnd_t) rnd));
printf ("Expected ");
mpfr_out_str (stdout, 16, 0, z1, MPFR_RNDN);
printf (", inex = %d, flags = %u\n", SIGN (inex1), flags1);
printf ("Got ");
mpfr_out_str (stdout, 16, 0, z2, MPFR_RNDN);
printf (", inex = %d, flags = %u\n", SIGN (inex2), flags2);
exit (1);
} /* div */
} /* k */
mpfr_clears (z1, z2, (mpfr_ptr) 0);
} /* prec */
} /* i */
mpfr_clears (x, y, (mpfr_ptr) 0);
set_emin (emin);
}
/*
Обрабатывает один отсчёт сигнала:
1) расчитывает выход фильтра
2) адаптирует коэффициенты фильтра
*/
NUM rnlms_func(rnlms_data_hnd f, NUM far_, NUM near_, NUM *err, NUM *output)
{
// size_t i;
NUM Psi;
NUM mediana;
f->norma += SQR(near_) - SQR(CB_get_first_elem(f->sig)) ;// f->norma == X*X'
CB_push_elem(f->sig, near_);
*output = convolution_CB_and_vector(f->sig, f->coeff); //выход фильтра
*err = far_ - *output;
if (f->opt & OPT_INHIBIT_ADAPTATION)
return *err;
//CB_push_elem(f->err_buf, *err);
MediatorInsert(f->err_buf, *err);
// NUM mediana = CB_mediana(f->err_buf);
mediana = MediatorMedian(f->err_buf);
// {
// NUM tmp[3];
// memcpy(tmp, f->err_buf->data, sizeof(NUM)*f->param.ERR_BUF_LEN);
// qsort(tmp, f->param.ERR_BUF_LEN, sizeof(NUM), compare_NUM);
// fprintf(stderr, "%f %f\n", *err, mediana);
// }
if (3 * NUM_abs(mediana) > *err) //FIXME почему 3 ???
Psi = *err * (f->param.ALPHA);
else
Psi = 0.5f * mediana * (f->param.ALPHA) * SIGN(*err);
// NUM tmp = Psi/(f->param.BETTA+f->norma);
CB_multiple_and_add(f->sig, Psi/(f->param.BETTA+f->norma), &(f->coeff));
/* for (i =0; i<f->param.len; ++i) */
/* { */
/* NUM x_i = CB_get_elem(f->sig, i); */
/* f->coeff[i] += Psi*(x_i/(f->param.BETTA+f->norma)); */
/* } */
// f->DELTA = f->MEMORY_FACTOR * f->DELTA + (1 - (f->MEMORY_FACTOR)) * MIN(SQR(*err)/(f->BETTA + f->norma), f->DELTA);
if (!(f->opt & OPT_DISABLE_NONLINEAR_PROCESSING))
{
// CB_push_elem(f->err_signal, SQR(*err/65535));
CB_push_elem(f->err_signal, SQR(*err));
double pwr = convolution_CB_and_vector(f->err_signal, level_measurement_filter_coeff);
double level = (+10 * log10(pwr/LEVEL_MEASUREMENT_FILTER_LEN) - 83.11858286715683);
fprintf(stderr, "level : %g\n", level);
if (level < f->param.NLP_thresold)
*err=0;
}
return *err;
}
/*-----------------------------------------------------------------*/
int LoadSuesTable(char *fname, int col1, int log10flag,
int **pplutindex, double **pplog10p) {
FSENV *fsenv;
char tmpstr[2000], tmpstr2[2000], segname[2000], hemi[3];
FILE *fp;
int nitems, segindex;
char *item;
double v;
memset(hemi,'\0',3);
fsenv = FSENVgetenv();
fp = fopen(fname,"r");
if (fp == NULL) {
printf("ERROR: could not open%s\n",fname);
exit(1);
}
// Count the number of items
nitems = 0;
while (1) {
if (fgets(tmpstr,2000-1,fp) == NULL) break;
nitems++;
}
fclose(fp);
printf("nitems %d\n",nitems);
*pplutindex = (int *) calloc(nitems,sizeof(int));
*pplog10p = (double *) calloc(nitems,sizeof(double));
fp = fopen(fname,"r");
nitems = 0;
while (1) {
if (fgets(tmpstr,2000-1,fp) == NULL) break;
memset(tmpstr2,'\0',2000);
sscanf(tmpstr,"%s",tmpstr2);
memcpy(hemi,tmpstr2,2);
sprintf(segname,"ctx-%2s-%s",hemi,&(tmpstr2[3]));
CTABfindName(fsenv->ctab, segname, &segindex);
if (segindex < 0) {
printf("ERROR: reading %s, cannot find %s in color table\n",
fname,segname);
printf("%s",tmpstr);
exit(1);
}
item = gdfGetNthItemFromString(tmpstr,col1-1);
if (item == NULL) {
printf("ERROR: reading col %d from %s\n",col1,fname);
exit(1);
}
sscanf(item,"%lf",&v);
if (log10flag) v = -SIGN(v)*log10(fabs(v));
printf("%2d %4d %20s %6.4lf\n",nitems+1,segindex,segname,v);
(*pplutindex)[nitems] = segindex;
(*pplog10p)[nitems] = v;
nitems++;
free(item);
}
fclose(fp);
FSENVfree(&fsenv);
return(nitems);
}
void latlon2_(double *alat1, double *alon1, double *delta, double *azi, double *alat2, double *alon2) {
double alat, alatr, alon, b, c, coslat, dlon;
double r13, sinlat, x1, x2, x3;
/* changed for ellipticity of earth
* changed use of *alat1 and *alat2
*/
double esq, alat3;
esq=(1.0-1.0/298.25)*(1.0-1.0/298.25);
alat3=atan(tan(*alat1*DEG_TO_RAD)*esq)*RAD_TO_DEG;
/* Convert a geographical location to geocentric cartesian
* coordinates, assuming a spherical earth
*/
alat = 90.0 - *delta;
alon = 180.0 - *azi;
r13 = cos(DEG_TO_RAD*alat);
/* x1: Axis 1 intersects equator at 0 deg longitude
* x2: Axis 2 intersects equator at 90 deg longitude
* x3: Axis 3 intersects north pole
*/
x1 = r13*sin(DEG_TO_RAD*alon);
x2 = sin(DEG_TO_RAD*alat);
x3 = r13*cos(DEG_TO_RAD*alon);
/* Rotate in cartesian coordinates. The cartesian coordinate system
* is most easily described in geographic terms. The origin is at
* the Earth's center. Rotation by alat1 degrees southward, about
* the 1-axis.
*/
alatr = (90.0-alat3)/RAD_TO_DEG;
sinlat = sin(alatr);
coslat = cos(alatr);
b = x2;
c = x3;
x2 = b*coslat - c*sinlat;
x3 = b*sinlat + c*coslat;
/* Convert geocentric cartesian coordinates to a geographical
* location, assuming a spherical earth
*/
r13 = sqrt(x3*x3 + x1*x1);
dlon = RAD_TO_DEG*atan2(x1, x3);
/* changed for ellipticity of earth
* *alat2 = RAD_TO_DEG*atan2(x2, r13);
*/
alat3= atan2(x2, r13);
*alat2=RAD_TO_DEG * atan(tan(alat3)/esq);
*alon2 = *alon1 + dlon;
if (fabs(*alon2) > 180.0)
*alon2 = SIGN((360.0-fabs(*alon2)), *alon2);
}
void AkFrac::setNumDen(qint64 num, qint64 den)
{
bool changed = false;
if (!den) {
if (this->d->m_num != 0) {
this->d->m_num = 0;
changed = true;
emit this->numChanged();
}
if (this->d->m_den != 0) {
this->d->m_den = 0;
changed = true;
emit this->denChanged();
}
if (this->d->m_isValid != false) {
this->d->m_isValid = false;
changed = true;
emit this->isValidChanged();
}
if (changed) {
emit this->valueChanged();
emit this->stringChanged();
}
return;
}
num = SIGN(den) * num;
den = qAbs(den);
AkFracPrivate::reduce(&num, &den);
if (this->d->m_num != num) {
this->d->m_num = num;
changed = true;
emit this->numChanged();
}
if (this->d->m_den != den) {
this->d->m_den = den;
changed = true;
emit this->denChanged();
}
if (this->d->m_isValid != true) {
this->d->m_isValid = true;
changed = true;
emit this->isValidChanged();
}
if (changed) {
emit this->valueChanged();
emit this->stringChanged();
}
}
std::vector<double> FrameToVec7(const cv::Mat frame)
{
// [0]-[2]: translation xyz
// [3]-[6]: quaternion wxyz
std::vector<double> pose(7, 0.0);
double r11 = frame.at<double>(0,0);
double r12 = frame.at<double>(0,1);
double r13 = frame.at<double>(0,2);
double r21 = frame.at<double>(1,0);
double r22 = frame.at<double>(1,1);
double r23 = frame.at<double>(1,2);
double r31 = frame.at<double>(2,0);
double r32 = frame.at<double>(2,1);
double r33 = frame.at<double>(2,2);
double qw = ( r11 + r22 + r33 + 1.0) / 4.0;
double qx = ( r11 - r22 - r33 + 1.0) / 4.0;
double qy = (-r11 + r22 - r33 + 1.0) / 4.0;
double qz = (-r11 - r22 + r33 + 1.0) / 4.0;
if(qw < 0.0f) qw = 0.0;
if(qx < 0.0f) qx = 0.0;
if(qy < 0.0f) qy = 0.0;
if(qz < 0.0f) qz = 0.0;
qw = std::sqrt(qw);
qx = std::sqrt(qx);
qy = std::sqrt(qy);
qz = std::sqrt(qz);
if(qw >= qx && qw >= qy && qw >= qz)
{
qw *= +1.0;
qx *= SIGN(r32 - r23);
qy *= SIGN(r13 - r31);
qz *= SIGN(r21 - r12);
}
else if(qx >= qw && qx >= qy && qx >= qz)
{
qw *= SIGN(r32 - r23);
qx *= +1.0;
qy *= SIGN(r21 + r12);
qz *= SIGN(r13 + r31);
}
else if(qy >= qw && qy >= qx && qy >= qz)
{
qw *= SIGN(r13 - r31);
qx *= SIGN(r21 + r12);
qy *= +1.0;
qz *= SIGN(r32 + r23);
}
else if(qz >= qw && qz >= qx && qz >= qy)
{
qw *= SIGN(r21 - r12);
qx *= SIGN(r31 + r13);
qy *= SIGN(r32 + r23);
qz *= +1.0;
}
else
{
printf("coding error\n");
}
double r = std::sqrt(qw*qw + qx*qx + qy*qy + qz*qz);
qw /= r;
qx /= r;
qy /= r;
qz /= r;
pose[3] = qw;
pose[4] = qx;
pose[5] = qy;
pose[6] = qz;
// Translation
pose[0] = frame.at<double>(0,3);
pose[1] = frame.at<double>(1,3);
pose[2] = frame.at<double>(2,3);
return pose;
}
void CCCatmullRomSprite::updateAtlas() {
// populate points
populatePoints(m_controlPoints, m_points);
// clear
m_atlas->removeAllQuads();
m_segmentQuadIndices.clear();
// basic check, at least we need two points
int pc = m_points.getCount();
if(pc < 2)
return;
// append a point to avoid losting last segment
CCPoint pLast0 = m_points.getPointAt(pc - 1);
CCPoint pLast1 = m_points.getPointAt(pc - 2);
CCPoint pAppend = ccpAdd(pLast0, ccpSub(pLast0, pLast1));
m_points.addPoint(pAppend);
pc++;
// first two points
CCPoint p0 = m_points.getPointAt(0);
CCPoint p1 = m_points.getPointAt(1);
// half width of pattern
float halfWidth = m_patternWidth / 2;
// bl and br of first quad
CCPoint bl, tl;
{
CCPoint v01 = ccpSub(p1, p0);
float r01 = ccpToAngle(v01);
bl.x = p0.x + halfWidth * sinf(r01);
bl.y = p0.y - halfWidth * cosf(r01);
tl.x = p0.x - halfWidth * sinf(r01);
tl.y = p0.y + halfWidth * cosf(r01);
}
// current length
float texStartP = 0;
float texEndP;
float headPos = 0;
// populate quads
int segIndex = 0;
for(int i = 2; i < pc; i++) {
// save quad index
if(m_segmentPointIndices[segIndex] == i - 2) {
m_segmentQuadIndices.push_back(m_atlas->getTotalQuads());
segIndex++;
}
// third point
CCPoint p2 = m_points.getPointAt(i);
// 3 vectors, from 0 to 1, from 1 to 2, from 1 to 0
CCPoint v01 = ccpSub(p1, p0);
CCPoint v12 = ccpSub(p2, p1);
CCPoint v10 = ccpSub(p0, p1);
// angle in center of v01 and v12, then rotate 90 degrees
float r = (ccpToAngle(v01) + ccpToAngle(v12)) / 2 - M_PI_2;
// the vector of center divider
CCPoint m = ccp(cosf(r), sinf(r));
// angle between center and v10
float rm01 = ccpToAngle(m) - ccpToAngle(v10);
// the actual base width of joint
// but we must prevent the base width to be too large
float s = sinf(rm01);
float w = MAX_FLOAT;
if(s != 0)
w = fabsf(halfWidth / s);
w = MIN(halfWidth * 2, w);
// a corner situation when v01 is in third quadrant and v12 is in fourth quadrant
if(v01.x < 0 && SIGN(v01.x) == SIGN(v12.x) && SIGN(v01.y) != SIGN(v12.y)) {
r = M_PI + r;
}
// populate tl and tr
CCPoint br, tr;
br.x = p1.x + w * cosf(r);
br.y = p1.y + w * sinf(r);
tr.x = p1.x - w * cosf(r);
tr.y = p1.y - w * sinf(r);
// calculate texcoords pencentage
float segLen = ccpLength(v01);
float remainLen = segLen;
float initVerP = 0;
float stepVerP = m_patternLength / segLen;
while(remainLen > m_patternLength) {
texEndP = texStartP;
float p = (1 - texStartP) / (1 - texStartP + texEndP);
populateQuad(bl, br, tl, tr, texStartP, 1, initVerP, initVerP + stepVerP * p);
populateQuad(bl, br, tl, tr, 0, texEndP, initVerP + stepVerP * p, initVerP + stepVerP);
initVerP += stepVerP;
// cut pattern length
remainLen -= m_patternLength;
}
// remaining length
headPos += segLen;
headPos = fmodf(headPos, m_patternLength);
texEndP = headPos / m_patternLength;
stepVerP = remainLen / segLen;
if(texEndP <= texStartP) {
float p = (1 - texStartP) / (1 - texStartP + texEndP);
populateQuad(bl, br, tl, tr, texStartP, 1, initVerP, initVerP + stepVerP * p);
populateQuad(bl, br, tl, tr, 0, texEndP, initVerP + stepVerP * p, initVerP + stepVerP);
} else {
populateQuad(bl, br, tl, tr, texStartP, texEndP, initVerP, 1);
}
// move forward
p0 = p1;
p1 = p2;
bl = br;
tl = tr;
texStartP = texEndP;
}
}
//
// Do single unary node folding
//
// Returns a new node representing the result.
//
TIntermTyped* TIntermConstantUnion::fold(TOperator op, const TType& returnType) const
{
// First, size the result, which is mostly the same as the argument's size,
// but not always, and classify what is componentwise.
// Also, eliminate cases that can't be compile-time constant.
int resultSize;
bool componentWise = true;
int objectSize = getType().computeNumComponents();
switch (op) {
case EOpDeterminant:
case EOpAny:
case EOpAll:
case EOpLength:
componentWise = false;
resultSize = 1;
break;
case EOpEmitStreamVertex:
case EOpEndStreamPrimitive:
// These don't actually fold
return 0;
case EOpPackSnorm2x16:
case EOpPackUnorm2x16:
case EOpPackHalf2x16:
componentWise = false;
resultSize = 1;
break;
case EOpUnpackSnorm2x16:
case EOpUnpackUnorm2x16:
case EOpUnpackHalf2x16:
componentWise = false;
resultSize = 2;
break;
case EOpNormalize:
componentWise = false;
resultSize = objectSize;
break;
default:
resultSize = objectSize;
break;
}
// Set up for processing
TConstUnionArray newConstArray(resultSize);
const TConstUnionArray& unionArray = getConstArray();
// Process non-component-wise operations
switch (op) {
case EOpLength:
case EOpNormalize:
{
double sum = 0;
for (int i = 0; i < objectSize; i++)
sum += unionArray[i].getDConst() * unionArray[i].getDConst();
double length = sqrt(sum);
if (op == EOpLength)
newConstArray[0].setDConst(length);
else {
for (int i = 0; i < objectSize; i++)
newConstArray[i].setDConst(unionArray[i].getDConst() / length);
}
break;
}
// TODO: 3.0 Functionality: unary constant folding: the rest of the ops have to be fleshed out
case EOpPackSnorm2x16:
case EOpPackUnorm2x16:
case EOpPackHalf2x16:
case EOpUnpackSnorm2x16:
case EOpUnpackUnorm2x16:
case EOpUnpackHalf2x16:
case EOpDeterminant:
case EOpMatrixInverse:
case EOpTranspose:
case EOpAny:
case EOpAll:
return 0;
default:
assert(componentWise);
break;
}
// Turn off the componentwise loop
if (! componentWise)
objectSize = 0;
// Process component-wise operations
for (int i = 0; i < objectSize; i++) {
switch (op) {
case EOpNegative:
switch (getType().getBasicType()) {
case EbtDouble:
case EbtFloat: newConstArray[i].setDConst(-unionArray[i].getDConst()); break;
case EbtInt: newConstArray[i].setIConst(-unionArray[i].getIConst()); break;
case EbtUint: newConstArray[i].setUConst(static_cast<unsigned int>(-static_cast<int>(unionArray[i].getUConst()))); break;
default:
return 0;
}
break;
case EOpLogicalNot:
case EOpVectorLogicalNot:
switch (getType().getBasicType()) {
case EbtBool: newConstArray[i].setBConst(!unionArray[i].getBConst()); break;
default:
return 0;
}
break;
case EOpBitwiseNot:
newConstArray[i] = ~unionArray[i];
break;
case EOpRadians:
newConstArray[i].setDConst(unionArray[i].getDConst() * pi / 180.0);
break;
case EOpDegrees:
newConstArray[i].setDConst(unionArray[i].getDConst() * 180.0 / pi);
break;
case EOpSin:
newConstArray[i].setDConst(sin(unionArray[i].getDConst()));
break;
case EOpCos:
newConstArray[i].setDConst(cos(unionArray[i].getDConst()));
break;
case EOpTan:
newConstArray[i].setDConst(tan(unionArray[i].getDConst()));
break;
case EOpAsin:
newConstArray[i].setDConst(asin(unionArray[i].getDConst()));
break;
case EOpAcos:
newConstArray[i].setDConst(acos(unionArray[i].getDConst()));
break;
case EOpAtan:
newConstArray[i].setDConst(atan(unionArray[i].getDConst()));
break;
case EOpDPdx:
case EOpDPdy:
case EOpFwidth:
case EOpDPdxFine:
case EOpDPdyFine:
case EOpFwidthFine:
case EOpDPdxCoarse:
case EOpDPdyCoarse:
case EOpFwidthCoarse:
// The derivatives are all mandated to create a constant 0.
newConstArray[i].setDConst(0.0);
break;
case EOpExp:
newConstArray[i].setDConst(exp(unionArray[i].getDConst()));
break;
case EOpLog:
newConstArray[i].setDConst(log(unionArray[i].getDConst()));
break;
case EOpExp2:
{
const double inv_log2_e = 0.69314718055994530941723212145818;
newConstArray[i].setDConst(exp(unionArray[i].getDConst() * inv_log2_e));
break;
}
case EOpLog2:
{
const double log2_e = 1.4426950408889634073599246810019;
newConstArray[i].setDConst(log2_e * log(unionArray[i].getDConst()));
break;
}
case EOpSqrt:
newConstArray[i].setDConst(sqrt(unionArray[i].getDConst()));
break;
case EOpInverseSqrt:
newConstArray[i].setDConst(1.0 / sqrt(unionArray[i].getDConst()));
break;
case EOpAbs:
if (unionArray[i].getType() == EbtDouble)
newConstArray[i].setDConst(fabs(unionArray[i].getDConst()));
else if (unionArray[i].getType() == EbtInt)
newConstArray[i].setIConst(abs(unionArray[i].getIConst()));
else
newConstArray[i] = unionArray[i];
break;
case EOpSign:
#define SIGN(X) (X == 0 ? 0 : (X < 0 ? -1 : 1))
if (unionArray[i].getType() == EbtDouble)
newConstArray[i].setDConst(SIGN(unionArray[i].getDConst()));
else
newConstArray[i].setIConst(SIGN(unionArray[i].getIConst()));
break;
case EOpFloor:
newConstArray[i].setDConst(floor(unionArray[i].getDConst()));
break;
case EOpTrunc:
if (unionArray[i].getDConst() > 0)
newConstArray[i].setDConst(floor(unionArray[i].getDConst()));
else
newConstArray[i].setDConst(ceil(unionArray[i].getDConst()));
break;
case EOpRound:
newConstArray[i].setDConst(floor(0.5 + unionArray[i].getDConst()));
break;
case EOpRoundEven:
{
double flr = floor(unionArray[i].getDConst());
bool even = flr / 2.0 == floor(flr / 2.0);
double rounded = even ? ceil(unionArray[i].getDConst() - 0.5) : floor(unionArray[i].getDConst() + 0.5);
newConstArray[i].setDConst(rounded);
break;
}
case EOpCeil:
newConstArray[i].setDConst(ceil(unionArray[i].getDConst()));
break;
case EOpFract:
{
double x = unionArray[i].getDConst();
newConstArray[i].setDConst(x - floor(x));
break;
}
case EOpIsNan:
{
newConstArray[i].setBConst(isNan(unionArray[i].getDConst()));
break;
}
case EOpIsInf:
{
newConstArray[i].setBConst(isInf(unionArray[i].getDConst()));
break;
}
// TODO: 3.0 Functionality: unary constant folding: the rest of the ops have to be fleshed out
case EOpSinh:
case EOpCosh:
case EOpTanh:
case EOpAsinh:
case EOpAcosh:
case EOpAtanh:
case EOpFloatBitsToInt:
case EOpFloatBitsToUint:
case EOpIntBitsToFloat:
case EOpUintBitsToFloat:
default:
return 0;
}
}
TIntermConstantUnion *newNode = new TIntermConstantUnion(newConstArray, returnType);
newNode->getWritableType().getQualifier().storage = EvqConst;
newNode->setLoc(getLoc());
return newNode;
}
static void tqli( float d[3], float e[3], float z[3][3] ) {
int m,l,iter,i,k;
float s,r,p,g,f,dd,c,b;
for(i=2;i<=3;i++)
e[i-2]=e[i-1]; /* convenient to renumber the elements of e */
e[2]=0.0;
for(l=1;l<=3;l++)
{
iter=0;
do
{
for(m=l;m<=2;m++)
{
/*
** Look for a single small subdiagonal element
** to split the matrix.
*/
dd=fabs(d[m-1])+fabs(d[m]);
if(fabs(e[m-1])+dd == dd)
break;
}
if(m!=l)
{
if(iter++ == 30)
{
printf("\nToo many iterations in TQLI");
}
g=(d[l]-d[l-1])/(2.0f*e[l-1]); /* form shift */
r=sqrt((g*g)+1.0f);
g=d[m-1]-d[l-1]+e[l-1]/(g+SIGN(r,g)); /* this is dm-ks */
s=c=1.0;
p=0.0;
for(i=m-1;i>=l;i--)
{
/*
** A plane rotation as in the original
** QL, followed by Givens rotations to
** restore tridiagonal form.
*/
f=s*e[i-1];
b=c*e[i-1];
if(fabs(f) >= fabs(g))
{
c=g/f;
r=sqrt((c*c)+1.0f);
e[i]=f*r;
c*=(s=1.0f/r);
}
else
{
s=f/g;
r=sqrt((s*s)+1.0f);
e[i]=g*r;
s*=(c=1.0f/r);
}
g=d[i]-p;
r=(d[i-1]-g)*s+2.0f*c*b;
p=s*r;
d[i]=g+p;
g=c*r-b;
for(k=1;k<=3;k++)
{
/*
** Form eigenvectors
*/
f=z[k-1][i];
z[k-1][i]=s*z[k-1][i-1]+c*f;
z[k-1][i-1]=c*z[k-1][i-1]-s*f;
}
}
d[l-1]=d[l-1]-p;
e[l-1]=g;
e[m-1]=0.0f;
}
}while(m != l);
}
}
void Grnn::Trainer::bracket(Grnn& grnn, double& a, double& b, double& c)
{
double fa = error(grnn, a);
double fb = error(grnn, b);
if (fb > fa)
{
SWP(a, b);
SWP(fa, fb);
}
c = b + GOLD*(b - a);
double fc = error(grnn, c);
double fu;
while (fb > fc)
{
double r = (b - a)*(fb - fc);
double q = (b - c)*(fb - fa);
double u = b - (q*(b - c) - r*(b - a))/
(2.0*SIGN(FMAX(fabs(q - r), TINY), q - r));
double ulim = b + GLIMIT*(c - b);
if(a > 5.0 && b > 5.0 && c > 5.0)
{
a = b;
b = u;
fa = fb;
fb = fu;
return;
}
if ((b - u)*(u - c) > 0.0)
{
fu = error(grnn, u);
if (fu < fc)
{
a = b;
b = u;
fa = fb;
fb = fu;
return;
}
else if (fu > fb)
{
c = u;
fc = fu;
return;
}
u = c + GOLD*(c - b);
fu = error(grnn, u);
}
else if ((c - u)*(u - ulim) > 0.0)
{
fu = error(grnn, u);
if (fu < fc)
{
double temp;
temp = c + GOLD*(c - b);
SHFT(b, c, u, temp);
temp = error(grnn, u);
SHFT(fb, fc, fu, temp);
}
}
else if ((u - ulim)*(ulim - c) >= 0.0)
{
u = ulim;
fu = error(grnn, u);
}
else
{
u = c + GOLD*(c - b);
fu = error(grnn, u);
}
SHFT(a, b, c, u);
SHFT(fa, fb, fc, fu);
}
}
static cholmod_sparse *copy_sym_to_unsym
(
/* ---- input ---- */
cholmod_sparse *A, /* matrix to copy */
int mode, /* >0: numerical, 0: pattern, <0: pattern (no diag)
* -2: pattern only, no diagonal, add 50% + n extra
* space to C */
/* --------------- */
cholmod_common *Common
)
{
double aij ;
double *Ax, *Cx ;
Int *Ap, *Ai, *Anz, *Cp, *Ci, *Wj, *Iwork ;
cholmod_sparse *C ;
Int nrow, ncol, nz, packed, j, p, pend, i, pc, up, lo, values, diag,
astype, extra ;
/* ---------------------------------------------------------------------- */
/* get inputs */
/* ---------------------------------------------------------------------- */
nrow = A->nrow ;
ncol = A->ncol ;
Ap = A->p ;
Anz = A->nz ;
Ai = A->i ;
Ax = A->x ;
packed = A->packed ;
values = (mode > 0) && (A->xtype != CHOLMOD_PATTERN) ;
diag = (mode >= 0) ;
astype = SIGN (A->stype) ;
up = (astype > 0) ;
lo = (astype < 0) ;
ASSERT (astype != 0) ;
/* ---------------------------------------------------------------------- */
/* create an unsymmetric copy of a symmetric matrix */
/* ---------------------------------------------------------------------- */
Iwork = Common->Iwork ;
Wj = Iwork ; /* size ncol (i/i/l) */
/* In MATLAB notation, for converting a symmetric/upper matrix:
* U = triu (A) ;
* L = tril (U',-1) ;
* C = L + U ;
*
* For converting a symmetric/lower matrix to unsymmetric:
* L = tril (A) ;
* U = triu (L',1) ;
* C = L + U ;
*/
ASSERT (up || lo) ;
PRINT1 (("copy: convert symmetric to unsym\n")) ;
/* count the number of entries in each column of C */
for (j = 0 ; j < ncol ; j++)
{
Wj [j] = 0 ;
}
for (j = 0 ; j < ncol ; j++)
{
p = Ap [j] ;
pend = (packed) ? (Ap [j+1]) : (p + Anz [j]) ;
for ( ; p < pend ; p++)
{
i = Ai [p] ;
if (i == j)
{
/* the diagonal entry A(i,i) will appear just once
* (unless it is excluded with mode < 0) */
if (diag)
{
Wj [j]++ ;
}
}
else if ((up && i < j) || (lo && i > j))
{
/* upper case: A(i,j) is in the strictly upper part;
* A(j,i) will be added to the strictly lower part of C.
* lower case is the opposite. */
Wj [j]++ ;
Wj [i]++ ;
}
}
}
nz = 0 ;
for (j = 0 ; j < ncol ; j++)
{
nz += Wj [j] ;
}
extra = (mode == -2) ? (nz/2 + ncol) : 0 ;
/* allocate C. C is sorted if and only if A is sorted */
C = CHOLMOD(allocate_sparse) (nrow, ncol, nz + extra, A->sorted, TRUE, 0,
values ? A->xtype : CHOLMOD_PATTERN, Common) ;
if (Common->status < CHOLMOD_OK)
{
return (NULL) ;
}
Cp = C->p ;
Ci = C->i ;
Cx = C->x ;
/* construct the column pointers for C */
p = 0 ;
for (j = 0 ; j < ncol ; j++)
{
Cp [j] = p ;
p += Wj [j] ;
}
Cp [ncol] = p ;
for (j = 0 ; j < ncol ; j++)
{
Wj [j] = Cp [j] ;
}
/* construct C */
if (values)
{
/* pattern and values */
ASSERT (diag) ;
for (j = 0 ; j < ncol ; j++)
{
p = Ap [j] ;
pend = (packed) ? (Ap [j+1]) : (p + Anz [j]) ;
for ( ; p < pend ; p++)
{
i = Ai [p] ;
aij = Ax [p] ;
if (i == j)
{
/* add diagonal entry A(i,i) to column i */
pc = Wj [i]++ ;
Ci [pc] = i ;
Cx [pc] = aij ;
}
else if ((up && i < j) || (lo && i > j))
{
/* add A(i,j) to column j */
pc = Wj [j]++ ;
Ci [pc] = i ;
Cx [pc] = aij ;
/* add A(j,i) to column i */
pc = Wj [i]++ ;
Ci [pc] = j ;
Cx [pc] = aij ;
}
}
}
}
else
{
/* pattern only, possibly excluding the diagonal */
for (j = 0 ; j < ncol ; j++)
{
p = Ap [j] ;
pend = (packed) ? (Ap [j+1]) : (p + Anz [j]) ;
for ( ; p < pend ; p++)
{
i = Ai [p] ;
if (i == j)
{
/* add diagonal entry A(i,i) to column i
* (unless it is excluded with mode < 0) */
if (diag)
{
Ci [Wj [i]++] = i ;
}
}
else if ((up && i < j) || (lo && i > j))
{
/* add A(i,j) to column j */
Ci [Wj [j]++] = i ;
/* add A(j,i) to column i */
Ci [Wj [i]++] = j ;
}
}
}
}
/* ---------------------------------------------------------------------- */
/* return the result */
/* ---------------------------------------------------------------------- */
DEBUG (i = CHOLMOD(dump_sparse) (C, "copy_sym_to_unsym", Common)) ;
PRINT1 (("mode %d nnzdiag "ID"\n", mode, i)) ;
ASSERT (IMPLIES (mode < 0, i == 0)) ;
return (C) ;
}
void Grnn::Trainer::trainBrent(Grnn& grnn)
{
double a = tolerance();
double b = grnn.bandwidth();
double x = 0.5*(a + b);
bracket(grnn, a, b, x);
if (a > b)
SWP(a, b);
double fx = error(grnn, x);
double w = x, fw = fx;
double v = x, fv = fx;
double d = 0.0, u, e = 0.0;
for(unsigned iter = 1; iter <= maxIter(); ++iter)
{
double xm = 0.5*(a + b);
double tol1 = tolerance()*fabs(x) + ZEPS;
double tol2 = 2.0*(tol1);
if (fabs(x - xm) <= (tol2 - 0.5*(b - a)))
break;
if (fabs(e) > tol1)
{
double p = (x-v)*(x-v)*(fx-fw) - (x-w)*(x-w)*(fx-fv);
double q = 2.0*((x-v)*(fx-fw) - (x-w)*(fx-fv));
if (q > 0.0)
p = -p;
q = fabs(q);
double etemp = e;
e = d;
if (fabs(p) >= fabs(0.5*q*etemp) ||
p <= q*(a - x) ||
p >= q*(b - x))
{
if (x >= xm)
e = a - x;
else
e = b - x;
d = CGOLD*(e);
}
else
{
d = p/q;
u = x + d;
if (u - a < tol2 || b - u < tol2)
d = SIGN(tol1, xm - x);
}
}
else
{
if (x >= xm)
e = a - x;
else
e = b - x;
d = CGOLD *(e);
}
double u;
if(fabs(d) >= tol1)
u = x + d;
else
u = x + SIGN(tol1, d);
double fu = error(grnn, u);
if (fu <= fx)
{
if (u >= x)
a = x;
else
b = x;
SHFT(v, w, x, u);
SHFT(fv, fw, fx, fu);
}
else
{
if (u < x)
a = u;
else
b = u;
if (fu <= fw || w == x)
{
v = w;
w = u;
fv = fw;
fw = fu;
}
else if (fu <= fv || v == x || v == w)
{
v = u;
fv = fu;
}
}
}
grnn.setBandwidth(x);
}
int
DCtrCurv(CKTcircuit *ckt, int restart)
/* forced restart flag */
{
TRCV *job = (TRCV *) ckt->CKTcurJob;
int i;
double *temp;
int converged;
int rcode;
int vcode;
int icode;
int j;
int error;
IFuid varUid;
IFuid *nameList;
int numNames;
int firstTime=1;
static runDesc *plot = NULL;
#ifdef WANT_SENSE2
long save;
#ifdef SENSDEBUG
if(ckt->CKTsenInfo && (ckt->CKTsenInfo->SENmode&DCSEN) ){
printf("\nDC Sensitivity Results\n\n");
CKTsenPrint(ckt);
}
#endif /* SENSDEBUG */
#endif
rcode = CKTtypelook("Resistor");
vcode = CKTtypelook("Vsource");
icode = CKTtypelook("Isource");
if (!restart && job->TRCVnestState >= 0) {
/* continuing */
i = job->TRCVnestState;
/* resume to work? saj*/
error = SPfrontEnd->OUTpBeginPlot (NULL, NULL,
NULL,
NULL, 0,
666, NULL, 666,
&plot);
goto resume;
}
ckt->CKTtime = 0;
ckt->CKTdelta = job->TRCVvStep[0];
ckt->CKTmode = (ckt->CKTmode & MODEUIC) | MODEDCTRANCURVE | MODEINITJCT ;
ckt->CKTorder=1;
/* Save the state of the circuit */
for(i=0;i<7;i++) {
ckt->CKTdeltaOld[i]=ckt->CKTdelta;
}
for (i = 0; i <= job->TRCVnestLevel; i++) {
if(rcode >= 0) {
/* resistances are in this version, so use them */
RESinstance *here;
RESmodel *model;
for(model = (RESmodel *)ckt->CKThead[rcode];model != NULL;
model=model->RESnextModel){
for(here=model->RESinstances;here!=NULL;
here=here->RESnextInstance) {
if (here->RESname == job->TRCVvName[i]) {
job->TRCVvElt[i] = (GENinstance *)here;
job->TRCVvSave[i] = here->RESresist;
job->TRCVgSave[i] = here->RESresGiven;
job->TRCVvType[i] = rcode;
here->RESresist = job->TRCVvStart[i];
here->RESresGiven = 1;
CKTtemp(ckt);
goto found;
}
}
}
}
if(vcode >= 0) {
/* voltage sources are in this version, so use them */
VSRCinstance *here;
VSRCmodel *model;
for(model = (VSRCmodel *)ckt->CKThead[vcode];model != NULL;
model=model->VSRCnextModel){
for(here=model->VSRCinstances;here!=NULL;
here=here->VSRCnextInstance) {
if (here->VSRCname == job->TRCVvName[i]) {
job->TRCVvElt[i] = (GENinstance *)here;
job->TRCVvSave[i] = here->VSRCdcValue;
job->TRCVgSave[i] = here->VSRCdcGiven;
job->TRCVvType[i] = vcode;
here->VSRCdcValue = job->TRCVvStart[i];
here->VSRCdcGiven = 1;
goto found;
}
}
}
}
if(icode >= 0 ) {
/* current sources are in this version, so use them */
ISRCinstance *here;
ISRCmodel *model;
for(model= (ISRCmodel *)ckt->CKThead[icode];model != NULL;
model=model->ISRCnextModel){
for(here=model->ISRCinstances;here!=NULL;
here=here->ISRCnextInstance) {
if(here->ISRCname == job->TRCVvName[i]) {
job->TRCVvElt[i] = (GENinstance *)here;
job->TRCVvSave[i] = here->ISRCdcValue;
job->TRCVgSave[i] = here->ISRCdcGiven;
job->TRCVvType[i] = icode;
here->ISRCdcValue = job->TRCVvStart[i];
here->ISRCdcGiven = 1;
goto found;
}
}
}
}
if (!strcmp(job->TRCVvName[i], "temp"))
{
job->TRCVvSave[i] = ckt->CKTtemp; /* Saves the old circuit temperature */
job->TRCVvType[i] = TEMP_CODE; /* Set the sweep type code */
ckt->CKTtemp = job->TRCVvStart[i] + CONSTCtoK; /* Set the new circuit temp */
CKTtemp(ckt);
if (expr_w_temper)
inp_evaluate_temper();
goto found;
}
SPfrontEnd->IFerror (ERR_FATAL,
"DCtrCurv: source / resistor %s not in circuit", &(job->TRCVvName[i]));
return(E_NODEV);
found:;
}
#ifdef HAS_PROGREP
actval = job->TRCVvStart[job->TRCVnestLevel];
actdiff = job->TRCVvStart[job->TRCVnestLevel] - job->TRCVvStop[job->TRCVnestLevel];
#endif
#ifdef XSPICE
/* gtri - add - wbk - 12/19/90 - Add IPC stuff and anal_init and anal_type */
/* Tell the beginPlot routine what mode we're in */
g_ipc.anal_type = IPC_ANAL_DCTRCURVE;
/* Tell the code models what mode we're in */
g_mif_info.circuit.anal_type = MIF_DC;
g_mif_info.circuit.anal_init = MIF_TRUE;
/* gtri - end - wbk */
#endif
i--; /* PN: This seems to do nothing ??? */
error = CKTnames(ckt,&numNames,&nameList);
if(error) return(error);
if (job->TRCVvType[i] == vcode)
SPfrontEnd->IFnewUid (ckt, &varUid, NULL, "v-sweep", UID_OTHER, NULL);
else {
if (job->TRCVvType[i] == icode)
SPfrontEnd->IFnewUid (ckt, &varUid, NULL, "i-sweep", UID_OTHER, NULL);
else {
if (job->TRCVvType[i] == TEMP_CODE)
SPfrontEnd->IFnewUid (ckt, &varUid, NULL, "temp-sweep", UID_OTHER, NULL);
else {
if (job->TRCVvType[i] == rcode)
SPfrontEnd->IFnewUid (ckt, &varUid, NULL, "res-sweep", UID_OTHER, NULL);
else
SPfrontEnd->IFnewUid (ckt, &varUid, NULL, "?-sweep", UID_OTHER, NULL);
} /* icode */
} /* TEMP_CODE */
} /* rcode*/
error = SPfrontEnd->OUTpBeginPlot (ckt, ckt->CKTcurJob,
ckt->CKTcurJob->JOBname,
varUid, IF_REAL,
numNames, nameList, IF_REAL,
&plot);
tfree(nameList);
if(error) return(error);
/* initialize CKTsoaCheck `warn' counters */
if (ckt->CKTsoaCheck)
error = CKTsoaInit();
/* now have finished the initialization - can start doing hard part */
i = 0;
resume:
for(;;) {
if (job->TRCVvType[i] == vcode) { /* voltage source */
if((((VSRCinstance*)(job->TRCVvElt[i]))->VSRCdcValue) *
SIGN(1.0, job->TRCVvStep[i]) -
SIGN(1.0, job->TRCVvStep[i]) * job->TRCVvStop[i] >
DBL_EPSILON*1e+03)
{
i++ ;
firstTime=1;
ckt->CKTmode = (ckt->CKTmode & MODEUIC) |
MODEDCTRANCURVE | MODEINITJCT ;
if (i > job->TRCVnestLevel) break;
goto nextstep;
}
} else if (job->TRCVvType[i] == icode) { /* current source */
if((((ISRCinstance*)(job->TRCVvElt[i]))->ISRCdcValue) *
SIGN(1.0, job->TRCVvStep[i]) -
SIGN(1.0, job->TRCVvStep[i]) * job->TRCVvStop[i] >
DBL_EPSILON*1e+03)
{
i++ ;
firstTime=1;
ckt->CKTmode = (ckt->CKTmode & MODEUIC) |
MODEDCTRANCURVE | MODEINITJCT ;
if (i > job->TRCVnestLevel) break;
goto nextstep;
}
} else if (job->TRCVvType[i] == rcode) { /* resistance */
if((((RESinstance*)(job->TRCVvElt[i]))->RESresist) *
SIGN(1.0, job->TRCVvStep[i]) -
SIGN(1.0, job->TRCVvStep[i]) * job->TRCVvStop[i]
> DBL_EPSILON*1e+03)
{
i++ ;
firstTime=1;
ckt->CKTmode = (ckt->CKTmode & MODEUIC) |
MODEDCTRANCURVE | MODEINITJCT ;
if (i > job->TRCVnestLevel) break;
goto nextstep;
}
} else if (job->TRCVvType[i] == TEMP_CODE) { /* temp sweep */
if(((ckt->CKTtemp) - CONSTCtoK) * SIGN(1.0, job->TRCVvStep[i]) -
SIGN(1.0, job->TRCVvStep[i]) * job->TRCVvStop[i] >
DBL_EPSILON*1e+03)
{
i++ ;
firstTime=1;
ckt->CKTmode = (ckt->CKTmode & MODEUIC) |
MODEDCTRANCURVE | MODEINITJCT ;
if (i > job->TRCVnestLevel) break;
goto nextstep;
}
} /* else not possible */
while (i > 0) {
/* init(i); */
i--;
if (job->TRCVvType[i] == vcode) { /* voltage source */
((VSRCinstance *)(job->TRCVvElt[i]))->VSRCdcValue =
job->TRCVvStart[i];
} else if (job->TRCVvType[i] == icode) { /* current source */
((ISRCinstance *)(job->TRCVvElt[i]))->ISRCdcValue =
job->TRCVvStart[i];
} else if (job->TRCVvType[i] == TEMP_CODE) {
ckt->CKTtemp = job->TRCVvStart[i] + CONSTCtoK;
CKTtemp(ckt);
if (expr_w_temper)
inp_evaluate_temper();
} else if (job->TRCVvType[i] == rcode) {
((RESinstance *)(job->TRCVvElt[i]))->RESresist =
job->TRCVvStart[i];
((RESinstance *)(job->TRCVvElt[i]))->RESconduct =
1/(((RESinstance *)(job->TRCVvElt[i]))->RESresist);
/* Note: changing the resistance does nothing */
/* changing the conductance 1/r instead */
DEVices[rcode]->DEVload(job->TRCVvElt[i]->GENmodPtr, ckt);
/*
* RESload(job->TRCVvElt[i]->GENmodPtr, ckt);
*/
} /* else not possible */
}
/* Rotate state vectors. */
temp = ckt->CKTstates[ckt->CKTmaxOrder+1];
for(j=ckt->CKTmaxOrder;j>=0;j--) {
ckt->CKTstates[j+1] = ckt->CKTstates[j];
}
ckt->CKTstate0 = temp;
/* do operation */
#ifdef XSPICE
/* gtri - begin - wbk - Do EVTop if event instances exist */
if(ckt->evt->counts.num_insts == 0) {
/* If no event-driven instances, do what SPICE normally does */
#endif
converged = NIiter(ckt,ckt->CKTdcTrcvMaxIter);
if(converged != 0) {
converged = CKTop(ckt,
(ckt->CKTmode&MODEUIC)|MODEDCTRANCURVE | MODEINITJCT,
(ckt->CKTmode&MODEUIC)|MODEDCTRANCURVE | MODEINITFLOAT,
ckt->CKTdcMaxIter);
if(converged != 0) {
return(converged);
}
}
#ifdef XSPICE
}
else {
/* else do new algorithm */
/* first get the current step in the analysis */
if (job->TRCVvType[0] == vcode) {
g_mif_info.circuit.evt_step =
((VSRCinstance *)(job->TRCVvElt[i]))->VSRCdcValue ;
} else if (job->TRCVvType[0] == icode) {
g_mif_info.circuit.evt_step =
((ISRCinstance *)(job->TRCVvElt[i]))->ISRCdcValue ;
} else if (job->TRCVvType[0] == rcode) {
g_mif_info.circuit.evt_step =
((RESinstance*)(job->TRCVvElt[i]->GENmodPtr))->RESresist;
} else if (job->TRCVvType[0] == TEMP_CODE) {
g_mif_info.circuit.evt_step =
ckt->CKTtemp - CONSTCtoK;
}
/* if first time through, call EVTop immediately and save event results */
if(firstTime) {
converged = EVTop(ckt,
(ckt->CKTmode & MODEUIC) | MODEDCTRANCURVE | MODEINITJCT,
(ckt->CKTmode & MODEUIC) | MODEDCTRANCURVE | MODEINITFLOAT,
ckt->CKTdcMaxIter,
MIF_TRUE);
EVTdump(ckt, IPC_ANAL_DCOP, g_mif_info.circuit.evt_step);
EVTop_save(ckt, MIF_FALSE, g_mif_info.circuit.evt_step);
if(converged != 0)
return(converged);
}
/* else, call NIiter first with mode = MODEINITPRED */
/* to attempt quick analog solution. Then call all hybrids and call */
/* EVTop only if event outputs have changed, or if non-converged */
else {
converged = NIiter(ckt,ckt->CKTdcTrcvMaxIter);
EVTcall_hybrids(ckt);
if((converged != 0) || (ckt->evt->queue.output.num_changed != 0)) {
converged = EVTop(ckt,
(ckt->CKTmode & MODEUIC) | MODEDCTRANCURVE | MODEINITJCT,
(ckt->CKTmode & MODEUIC) | MODEDCTRANCURVE | MODEINITFLOAT,
ckt->CKTdcMaxIter,
MIF_FALSE);
EVTdump(ckt, IPC_ANAL_DCTRCURVE, g_mif_info.circuit.evt_step);
EVTop_save(ckt, MIF_FALSE, g_mif_info.circuit.evt_step);
if(converged != 0)
return(converged);
}
}
}
/* gtri - end - wbk - Do EVTop if event instances exist */
#endif
ckt->CKTmode = (ckt->CKTmode&MODEUIC) | MODEDCTRANCURVE | MODEINITPRED ;
if (job->TRCVvType[0] == vcode) {
ckt->CKTtime = ((VSRCinstance *)(job->TRCVvElt[i]))->VSRCdcValue ;
} else if (job->TRCVvType[0] == icode) {
ckt->CKTtime = ((ISRCinstance *)(job->TRCVvElt[i]))->ISRCdcValue ;
} else if (job->TRCVvType[0] == rcode) {
ckt->CKTtime = ((RESinstance *)(job->TRCVvElt[i]))->RESresist;
}
/* PN Temp sweep */
else
{
ckt->CKTtime = ckt->CKTtemp - CONSTCtoK ;
}
#ifdef XSPICE
/* gtri - add - wbk - 12/19/90 - Add IPC stuff */
/* If first time through, call CKTdump to output Operating Point info */
/* for Mspice compatibility */
if(g_ipc.enabled && firstTime) {
ipc_send_dcop_prefix();
CKTdump(ckt, 0.0, plot);
ipc_send_dcop_suffix();
}
/* gtri - end - wbk */
#endif
#ifdef WANT_SENSE2
/*
if(!ckt->CKTsenInfo) printf("sensitivity structure does not exist\n");
*/
if(ckt->CKTsenInfo && (ckt->CKTsenInfo->SENmode&DCSEN) ){
int senmode;
#ifdef SENSDEBUG
if (job->TRCVvType[i] == vcode) { /* voltage source */
printf("Voltage Source Value : %.5e V\n",
((VSRCinstance*) (job->TRCVvElt[i]))->VSRCdcValue);
}
if (job->TRCVvType[i] == icode) { /* current source */
printf("Current Source Value : %.5e A\n",
((ISRCinstance*)(job->TRCVvElt[i]))->ISRCdcValue);
}
if (job->TRCVvType[i] == rcode) { /* resistance */
printf("Current Resistance Value : %.5e Ohm\n",
((RESinstance*)(job->TRCVvElt[i]->GENmodPtr))->RESresist);
}
if (job->TRCVvType[i] == TEMP_CODE) { /* Temperature */
printf("Current Circuit Temperature : %.5e C\n",
ckt->CKTtemp - CONSTCtoK);
}
#endif /* SENSDEBUG */
senmode = ckt->CKTsenInfo->SENmode;
save = ckt->CKTmode;
ckt->CKTsenInfo->SENmode = DCSEN;
error = CKTsenDCtran(ckt);
if (error)
return(error);
ckt->CKTmode = save;
ckt->CKTsenInfo->SENmode = senmode;
}
#endif
#ifdef XSPICE
/* gtri - modify - wbk - 12/19/90 - Send IPC delimiters */
if(g_ipc.enabled)
ipc_send_data_prefix(ckt->CKTtime);
#endif
CKTdump(ckt,ckt->CKTtime,plot);
if (ckt->CKTsoaCheck)
error = CKTsoaCheck(ckt);
#ifdef XSPICE
if(g_ipc.enabled)
ipc_send_data_suffix();
/* gtri - end - wbk */
#endif
if(firstTime) {
firstTime=0;
bcopy(ckt->CKTstate0, ckt->CKTstate1,
(size_t) ckt->CKTnumStates * sizeof(double));
}
nextstep:;
if (job->TRCVvType[i] == vcode) { /* voltage source */
((VSRCinstance*)(job->TRCVvElt[i]))->VSRCdcValue +=
job->TRCVvStep[i];
} else if (job->TRCVvType[i] == icode) { /* current source */
((ISRCinstance*)(job->TRCVvElt[i]))->ISRCdcValue +=
job->TRCVvStep[i];
} else if (job->TRCVvType[i] == rcode) { /* resistance */
((RESinstance*)(job->TRCVvElt[i]))->RESresist +=
job->TRCVvStep[i];
/* This code should update resistance and conductance */
((RESinstance*)(job->TRCVvElt[i]))->RESconduct =
1/(((RESinstance*)(job->TRCVvElt[i]))->RESresist);
DEVices[rcode]->DEVload(job->TRCVvElt[i]->GENmodPtr, ckt);
/*
* RESload(job->TRCVvElt[i]->GENmodPtr, ckt);
*/
}
/* PN Temp Sweep - serban */
else if (job->TRCVvType[i] == TEMP_CODE)
{
ckt->CKTtemp += job->TRCVvStep[i];
CKTtemp(ckt);
if (expr_w_temper)
inp_evaluate_temper();
} /* else not possible */
if(SPfrontEnd->IFpauseTest()) {
/* user asked us to pause, so save state */
job->TRCVnestState = i;
return(E_PAUSE);
}
#ifdef HAS_PROGREP
if (i == job->TRCVnestLevel) {
actval += job->TRCVvStep[job->TRCVnestLevel];
SetAnalyse( "dc", abs((int)(actval * 1000. / actdiff)));
}
#endif
}
/* all done, lets put everything back */
for (i = 0; i <= job->TRCVnestLevel; i++) {
if (job->TRCVvType[i] == vcode) { /* voltage source */
((VSRCinstance*)(job->TRCVvElt[i]))->VSRCdcValue =
job->TRCVvSave[i];
((VSRCinstance*)(job->TRCVvElt[i]))->VSRCdcGiven = (job->TRCVgSave[i] != 0);
} else if (job->TRCVvType[i] == icode) /*current source */ {
((ISRCinstance*)(job->TRCVvElt[i]))->ISRCdcValue =
job->TRCVvSave[i];
((ISRCinstance*)(job->TRCVvElt[i]))->ISRCdcGiven = (job->TRCVgSave[i] != 0);
} else if (job->TRCVvType[i] == rcode) /* Resistance */ {
((RESinstance*)(job->TRCVvElt[i]))->RESresist =
job->TRCVvSave[i];
/* We restore both resistance and conductance */
((RESinstance*)(job->TRCVvElt[i]))->RESconduct =
1/(((RESinstance*)(job->TRCVvElt[i]))->RESresist);
((RESinstance*)(job->TRCVvElt[i]))->RESresGiven = (job->TRCVgSave[i] != 0);
DEVices[rcode]->DEVload(job->TRCVvElt[i]->GENmodPtr, ckt);
/*
* RESload(job->TRCVvElt[i]->GENmodPtr, ckt);
*/
}
else if (job->TRCVvType[i] == TEMP_CODE) {
ckt->CKTtemp = job->TRCVvSave[i];
CKTtemp(ckt);
if (expr_w_temper)
inp_evaluate_temper();
} /* else not possible */
}
SPfrontEnd->OUTendPlot (plot);
return(OK);
}
double ROOT_FIND(double x1, double x2, double (*func)(double), double tol){
int iter;
double a=x1,b=x2,c=x2,d,e,min1,min2;
double fa=(*func)(a),fb=(*func)(b),fc,p,q,r,s,tol1,xm;
if ((fa > 0.0 && fb > 0.0) || (fa < 0.0 && fb < 0.0))
MATHERR("Root must be bracketed in ROOT_FIND", 00);
fc=fb;
for (iter=1;iter<=ITMAX;iter++){
if ((fb > 0.0 && fc > 0.0) || (fb < 0.0 && fc < 0.0)){
c=a;
fc=fa;
e=d=b-a;
}
if (fabs(fc) < fabs(fb)) {
a=b;
b=c;
c=a;
fa=fb;
fb=fc;
fc=fa;
}
tol1=2.0*EPS*fabs(b)+0.5*tol;
xm=0.5*(c-b);
if (fabs(xm) <= tol1 || fb == 0.0) return b;
if (fabs(e) >= tol1 && fabs(fa) > fabs(fb)) {
s=fb/fa;
if (a == c) {
p=2.0*xm*s;
q=1.0-s;
} else {
q=fa/fc;
r=fb/fc;
p=s*(2.0*xm*q*(q-r)-(b-a)*(r-1.0));
q=(q-1.0)*(r-1.0)*(s-1.0);
}
if (p > 0.0) q = -q;
p=fabs(p);
min1=3.0*xm*q-fabs(tol1*q);
min2=fabs(e*q);
if (2.0*p < (min1 < min2 ? min1 : min2)) {
e=d;
d=p/q;
} else {
d=xm;
e=d;
}
} else {
d=xm;
e=d;
}
a=b;
fa=fb;
if (fabs(d) > tol1)
b += d;
else
b += SIGN(tol1,xm);
fb=(*func)(b);
}
MATHERR("Maximum number of iterations exceeded in zbrent", 00);
return 0.0;
}
int URKinematics::inverse(const double* T, double* q_sols, double q6_des) {
int num_sols = 0;
double T02 = -*T; T++; double T00 = *T; T++; double T01 = *T; T++; double T03 = -*T; T++;
double T12 = -*T; T++; double T10 = *T; T++; double T11 = *T; T++; double T13 = -*T; T++;
double T22 = *T; T++; double T20 = -*T; T++; double T21 = -*T; T++; double T23 = *T;
////////////////////////////// shoulder rotate joint (q1) //////////////////////////////
double q1[2];
{
double A = d6*T12 - T13;
double B = d6*T02 - T03;
double R = A*A + B*B;
if(fabs(A) < ZERO_THRESH) {
double div;
if(fabs(fabs(d4) - fabs(B)) < ZERO_THRESH)
div = -SIGN(d4)*SIGN(B);
else
div = -d4/B;
double arcsin = asin(div);
if(fabs(arcsin) < ZERO_THRESH)
arcsin = 0.0;
if(arcsin < 0.0)
q1[0] = arcsin + 2.0*PI;
else
q1[0] = arcsin;
q1[1] = PI - arcsin;
}
else if(fabs(B) < ZERO_THRESH) {
double div;
if(fabs(fabs(d4) - fabs(A)) < ZERO_THRESH)
div = SIGN(d4)*SIGN(A);
else
div = d4/A;
double arccos = acos(div);
q1[0] = arccos;
q1[1] = 2.0*PI - arccos;
}
else if(d4*d4 > R) {
return num_sols;
}
else {
double arccos = acos(d4 / sqrt(R)) ;
double arctan = atan2(-B, A);
double pos = arccos + arctan;
double neg = -arccos + arctan;
if(fabs(pos) < ZERO_THRESH)
pos = 0.0;
if(fabs(neg) < ZERO_THRESH)
neg = 0.0;
if(neg >= 0.0)
q1[1] = neg;
else
q1[1] = 2.0*PI + neg;
if(pos >= 0.0)
q1[0] = pos;
else
q1[0] = 2.0*PI + pos;
}
}
////////////////////////////////////////////////////////////////////////////////
////////////////////////////// wrist 2 joint (q5) //////////////////////////////
double q5[2][2];
{
for(int i=0;i<2;i++) {
double numer = (T03*sin(q1[i]) - T13*cos(q1[i])-d4);
double div;
if(fabs(fabs(numer) - fabs(d6)) < ZERO_THRESH)
div = SIGN(numer) * SIGN(d6);
else
div = numer / d6;
double arccos = acos(div);
q5[i][0] = arccos;
q5[i][1] = 2.0*PI - arccos;
}
}
////////////////////////////////////////////////////////////////////////////////
{
for(int i=0;i<2;i++) {
for(int j=0;j<2;j++) {
double c1 = cos(q1[i]), s1 = sin(q1[i]);
double c5 = cos(q5[i][j]), s5 = sin(q5[i][j]);
double q6;
////////////////////////////// wrist 3 joint (q6) //////////////////////////////
if(fabs(s5) < ZERO_THRESH)
q6 = q6_des;
else {
q6 = atan2(SIGN(s5)*-(T01*s1 - T11*c1),
SIGN(s5)*(T00*s1 - T10*c1));
if(fabs(q6) < ZERO_THRESH)
q6 = 0.0;
if(q6 < 0.0)
q6 += 2.0*PI;
}
////////////////////////////////////////////////////////////////////////////////
double q2[2], q3[2], q4[2];
///////////////////////////// RRR joints (q2,q3,q4) ////////////////////////////
double c6 = cos(q6), s6 = sin(q6);
double x04x = -s5*(T02*c1 + T12*s1) - c5*(s6*(T01*c1 + T11*s1) - c6*(T00*c1 + T10*s1));
double x04y = c5*(T20*c6 - T21*s6) - T22*s5;
double p13x = d5*(s6*(T00*c1 + T10*s1) + c6*(T01*c1 + T11*s1)) - d6*(T02*c1 + T12*s1) +
T03*c1 + T13*s1;
double p13y = T23 - d1 - d6*T22 + d5*(T21*c6 + T20*s6);
double c3 = (p13x*p13x + p13y*p13y - a2*a2 - a3*a3) / (2.0*a2*a3);
if(fabs(fabs(c3) - 1.0) < ZERO_THRESH)
c3 = SIGN(c3);
else if(fabs(c3) > 1.0) {
// TODO NO SOLUTION
continue;
}
double arccos = acos(c3);
q3[0] = arccos;
q3[1] = 2.0*PI - arccos;
double denom = a2*a2 + a3*a3 + 2*a2*a3*c3;
double s3 = sin(arccos);
double A = (a2 + a3*c3), B = a3*s3;
q2[0] = atan2((A*p13y - B*p13x) / denom, (A*p13x + B*p13y) / denom);
q2[1] = atan2((A*p13y + B*p13x) / denom, (A*p13x - B*p13y) / denom);
double c23_0 = cos(q2[0]+q3[0]);
double s23_0 = sin(q2[0]+q3[0]);
double c23_1 = cos(q2[1]+q3[1]);
double s23_1 = sin(q2[1]+q3[1]);
q4[0] = atan2(c23_0*x04y - s23_0*x04x, x04x*c23_0 + x04y*s23_0);
q4[1] = atan2(c23_1*x04y - s23_1*x04x, x04x*c23_1 + x04y*s23_1);
////////////////////////////////////////////////////////////////////////////////
for(int k=0;k<2;k++) {
if(fabs(q2[k]) < ZERO_THRESH)
q2[k] = 0.0;
else if(q2[k] < 0.0) q2[k] += 2.0*PI;
if(fabs(q4[k]) < ZERO_THRESH)
q4[k] = 0.0;
else if(q4[k] < 0.0) q4[k] += 2.0*PI;
q_sols[num_sols*6+0] = q1[i]; q_sols[num_sols*6+1] = q2[k];
q_sols[num_sols*6+2] = q3[k]; q_sols[num_sols*6+3] = q4[k];
q_sols[num_sols*6+4] = q5[i][j]; q_sols[num_sols*6+5] = q6;
num_sols++;
}
}
}
}
return num_sols;
}
/*
Given matrix a[m][n], m>=n, using svd decomposition a = p d q' to get
p[m][n], diag d[n] and q[n][n].
*/
void svd(int m, int n, double **a, double **p, double *d, double **q)
{
int flag, i, its, j, jj, k, l, nm, nm1 = n - 1, mm1 = m - 1;
double c, f, h, s, x, y, z;
double anorm = 0, g = 0, scale = 0;
//double *r = tvector_alloc(0, n, double);
double *r = (double*)malloc(sizeof(double)*n);
for (i = 0; i < m; i++)
for (j = 0; j < n; j++)
p[i][j] = a[i][j];
//for (i = m; i < n; i++)
// p[i][j] = 0;
/* Householder reduction to bidigonal form */
for (i = 0; i < n; i++)
{
l = i + 1;
r[i] = scale * g;
g = s = scale = 0.0;
if (i < m)
{
for (k = i; k < m; k++)
scale += fabs(p[k][i]);
if (scale)
{
for (k = i; k < m; k++)
{
p[k][i] /= scale;
s += p[k][i] * p[k][i];
}
f = p[i][i];
g = -SIGN(sqrt(s), f);
h = f * g - s;
p[i][i] = f - g;
if (i != nm1)
{
for (j = l; j < n; j++)
{
for (s = 0.0, k = i; k < m; k++)
s += p[k][i] * p[k][j];
f = s / h;
for (k = i; k < m; k++)
p[k][j] += f * p[k][i];
}
}
for (k = i; k < m; k++)
p[k][i] *= scale;
}
}
d[i] = scale * g;
g = s = scale = 0.0;
if (i < m && i != nm1)
{
for (k = l; k < n; k++)
scale += fabs(p[i][k]);
if (scale)
{
for (k = l; k < n; k++)
{
p[i][k] /= scale;
s += p[i][k] * p[i][k];
}
f = p[i][l];
g = -SIGN(sqrt(s), f);
h = f * g - s;
p[i][l] = f - g;
for (k = l; k < n; k++)
r[k] = p[i][k] / h;
if (i != mm1)
{
for (j = l; j < m; j++)
{
for (s = 0.0, k = l; k < n; k++)
s += p[j][k] * p[i][k];
for (k = l; k < n; k++)
p[j][k] += s * r[k];
}
}
for (k = l; k < n; k++)
p[i][k] *= scale;
}
}
anorm = MAX(anorm, fabs(d[i]) + fabs(r[i]));
}
/* Accumulation of right-hand transformations */
for (i = n - 1; i >= 0; i--)
{
if (i < nm1)
{
if (g)
{
for (j = l; j < n; j++)
q[j][i] = (p[i][j] / p[i][l]) / g;
for (j = l; j < n; j++)
{
for (s = 0.0, k = l; k < n; k++)
s += p[i][k] * q[k][j];
for (k = l; k < n; k++)
q[k][j] += s * q[k][i];
}
}
for (j = l; j < n; j++)
q[i][j] = q[j][i] = 0.0;
}
q[i][i] = 1.0;
g = r[i];
l = i;
}
/* Accumulation of left-hand transformations */
for (i = n - 1; i >= 0; i--)
{
l = i + 1;
g = d[i];
if (i < nm1)
for (j = l; j < n; j++)
p[i][j] = 0.0;
if (g)
{
g = 1.0 / g;
if (i != nm1)
{
for (j = l; j < n; j++)
{
for (s = 0.0, k = l; k < m; k++)
s += p[k][i] * p[k][j];
f = (s / p[i][i]) * g;
for (k = i; k < m; k++)
p[k][j] += f * p[k][i];
}
}
for (j = i; j < m; j++)
p[j][i] *= g;
} else
for (j = i; j < m; j++)
p[j][i] = 0.0;
++p[i][i];
}
/* diagonalization of the bidigonal form */
for (k = n - 1; k >= 0; k--)
{ /* loop over singlar values */
for (its = 0; its < 30; its++)
{ /* loop over allowed iterations */
flag = 1;
for (l = k; l >= 0; l--)
{ /* test for splitting */
nm = l - 1; /* note that r[l] is always
* zero */
if (fabs(r[l]) + anorm == anorm)
{
flag = 0;
break;
}
if (fabs(d[nm]) + anorm == anorm)
break;
}
if (flag)
{
c = 0.0; /* cancellation of r[l], if
* l>1 */
s = 1.0;
for (i = l; i <= k; i++)
{
f = s * r[i];
if (fabs(f) + anorm != anorm)
{
g = d[i];
h = radius(f, g);
d[i] = h;
h = 1.0 / h;
c = g * h;
s = (-f * h);
for (j = 0; j < m; j++)
{
y = p[j][nm];
z = p[j][i];
p[j][nm] = y * c + z * s;
p[j][i] = z * c - y * s;
}
}
}
}
z = d[k];
if (l == k)
{ /* convergence */
if (z < 0.0)
{
d[k] = -z;
for (j = 0; j < n; j++)
q[j][k] = (-q[j][k]);
}
break;
}
if (its == 30)
{
//error("svd: No convergence in 30 svd iterations", non_fatal);
return;
}
x = d[l]; /* shift from bottom 2-by-2 minor */
nm = k - 1;
y = d[nm];
g = r[nm];
h = r[k];
f = ((y - z) * (y + z) + (g - h) * (g + h)) / (2.0 * h * y);
g = radius(f, 1.0);
/* next QR transformation */
f = ((x - z) * (x + z) + h * ((y / (f + SIGN(g, f))) - h)) / x;
c = s = 1.0;
for (j = l; j <= nm; j++)
{
i = j + 1;
g = r[i];
y = d[i];
h = s * g;
g = c * g;
z = radius(f, h);
r[j] = z;
c = f / z;
s = h / z;
f = x * c + g * s;
g = g * c - x * s;
h = y * s;
y = y * c;
for (jj = 0; jj < n; jj++)
{
x = q[jj][j];
z = q[jj][i];
q[jj][j] = x * c + z * s;
q[jj][i] = z * c - x * s;
}
z = radius(f, h);
d[j] = z; /* rotation can be arbitrary
* id z=0 */
if (z)
{
z = 1.0 / z;
c = f * z;
s = h * z;
}
f = (c * g) + (s * y);
x = (c * y) - (s * g);
for (jj = 0; jj < m; jj++)
{
y = p[jj][j];
z = p[jj][i];
p[jj][j] = y * c + z * s;
p[jj][i] = z * c - y * s;
}
}
r[l] = 0.0;
r[k] = f;
d[k] = x;
}
}
free(r);
// dhli add: the original code does not sort the eigen value
// should do that and change the eigen vector accordingly
}
int32_t svdcmp_c(int32_t m, double* a, double* w, double* v) {
// C port of PLINK stats.cpp svdcmp().
// Note that this function is NOT thread-safe, due to the buffer allocated
// from the workspace stack. Pass in a preallocated buffer if that's not
// okay.
unsigned char* bigstack_mark = g_bigstack_base;
int32_t n = m;
int32_t flag;
int32_t l = 0; // suppress compile warning
int32_t i,its,j,jj,k,nm;
double anorm,c,f,g,h,s,scale,x,y,z;
double volatile temp;
double* rv1;
if (bigstack_alloc_d(m, &rv1)) {
return -1;
}
g=scale=anorm=0.0;
for (i=0;i<n;i++) {
l=i+2;
rv1[i]=scale*g;
g=s=scale=0.0;
if (i < m) {
for (k=i;k<m;k++) scale += fabs(a[k * m + i]);
if (scale != 0.0) {
for (k=i;k<m;k++) {
a[k * m + i] /= scale;
s += a[k * m + i]*a[k * m + i];
}
f=a[i * m + i];
g = -SIGN(sqrt(s),f);
h=f*g-s;
a[i * m + i]=f-g;
for (j=l-1;j<n;j++) {
for (s=0.0,k=i;k<m;k++) s += a[k * m + i]*a[k * m + j];
f=s/h;
for (k=i;k<m;k++) a[k * m + j] += f*a[k * m + i];
}
for (k=i;k<m;k++) a[k * m + i] *= scale;
}
}
w[i]=scale *g;
g=s=scale=0.0;
if (i+1 <= m && i+1 != n) {
for (k=l-1;k<n;k++) scale += fabs(a[i * m + k]);
if (scale != 0.0) {
for (k=l-1;k<n;k++) {
a[i * m + k] /= scale;
s += a[i * m + k]*a[i * m + k];
}
f=a[i * m + l-1];
g = -SIGN(sqrt(s),f);
h=f*g-s;
a[i * m + l-1]=f-g;
for (k=l-1;k<n;k++) rv1[k]=a[i * m + k]/h;
for (j=l-1;j<m;j++) {
for (s=0.0,k=l-1;k<n;k++) s += a[j * m + k]*a[i * m + k];
for (k=l-1;k<n;k++) a[j * m + k] += s*rv1[k];
}
for (k=l-1;k<n;k++) a[i * m + k] *= scale;
}
}
anorm=MAXV(anorm,(fabs(w[i])+fabs(rv1[i])));
}
for (i=n-1;i>=0;i--) {
if (i < n-1) {
if (g != 0.0) {
for (j=l;j<n;j++)
v[j * m + i]=(a[i * m + j]/a[i * m + l])/g;
for (j=l;j<n;j++) {
for (s=0.0,k=l;k<n;k++) s += a[i * m + k]*v[k * m + j];
for (k=l;k<n;k++) v[k * m + j] += s*v[k * m + i];
}
}
for (j=l;j<n;j++) v[i * m + j]=v[j * m + i]=0.0;
}
v[i * m + i]=1.0;
g=rv1[i];
l=i;
}
for (i=MINV(m,n)-1;i>=0;i--) {
l=i+1;
g=w[i];
for (j=l;j<n;j++) a[i * m + j]=0.0;
if (g != 0.0) {
g=1.0/g;
for (j=l;j<n;j++) {
for (s=0.0,k=l;k<m;k++) s += a[k * m + i]*a[k * m + j];
f=(s/a[i * m + i])*g;
for (k=i;k<m;k++) a[k * m + j] += f*a[k * m + i];
}
for (j=i;j<m;j++) a[j * m + i] *= g;
} else for (j=i;j<m;j++) a[j * m + i]=0.0;
++a[i * m + i];
}
for (k=n-1;k>=0;k--) {
for (its=0;its<30;its++) {
flag=1;
for (l=k;l>=0;l--) {
nm=l-1;
temp=fabs(rv1[l])+anorm;
if (temp == anorm) {
flag=0;
break;
}
temp=fabs(w[nm])+anorm;
if (temp == anorm) break;
}
if (flag) {
c=0.0;
s=1.0;
for (i=l;i<k+1;i++) {
f=s*rv1[i];
rv1[i]=c*rv1[i];
temp = fabs(f)+anorm;
if (temp == anorm) break;
g=w[i];
h=pythag(f,g);
w[i]=h;
h=1.0/h;
c=g*h;
s = -f*h;
for (j=0;j<m;j++) {
y=a[j * m + nm];
z=a[j * m + i];
a[j * m + nm]=y*c+z*s;
a[j * m + i]=z*c-y*s;
}
}
}
z=w[k];
if (l == k) {
if (z < 0.0) {
w[k] = -z;
for (j=0;j<n;j++) v[j * m + k] = -v[j * m + k];
}
break;
}
if (its == 29)
return 0; // cannot converge: multi-collinearity?
x=w[l];
nm=k-1;
y=w[nm];
g=rv1[nm];
h=rv1[k];
f=((y-z)*(y+z)+(g-h)*(g+h))/(2.0*h*y);
g=pythag(f,1.0);
f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
c=s=1.0;
for (j=l;j<=nm;j++) {
i=j+1;
g=rv1[i];
y=w[i];
h=s*g;
g=c*g;
z=pythag(f,h);
rv1[j]=z;
c=f/z;
s=h/z;
f=x*c+g*s;
g=g*c-x*s;
h=y*s;
y *= c;
for (jj=0;jj<n;jj++) {
x=v[jj * m + j];
z=v[jj * m + i];
v[jj * m + j]=x*c+z*s;
v[jj * m + i]=z*c-x*s;
}
z=pythag(f,h);
w[j]=z;
if (z) {
z=1.0/z;
c=f*z;
s=h*z;
}
f=c*g+s*y;
x=c*y-s*g;
for (jj=0;jj<m;jj++) {
y=a[jj * m + j];
z=a[jj * m + i];
a[jj * m + j]=y*c+z*s;
a[jj * m + i]=z*c-y*s;
}
}
rv1[l]=0.0;
rv1[k]=f;
w[k]=x;
}
}
bigstack_reset(bigstack_mark);
return 1;
}
/* Coefficients : x^4 + C[0] x^3 + C[1] x^2 + C[2] x + C[3] */
size_t quarticRoots(
double a, double b, double c, double d, double roots[4] )
{
double h,h1,h2,H, g,g1,g2,G, n, m, en, em, y;
double cubic[3]; /* Cubic and quadratic coefficients */
int i, nr;
/* Find the a real root of a certain cubic */
cubic[0] = -2.0*b;
cubic[1] = b*b + a*c - 4*d;
cubic[2] = c*c - a*b*c + a*a*d;
nr = cubicRoots( cubic[0], cubic[1], cubic[2], roots );
if( nr == 1 ) {
y = PolishRoot( 3, cubic[0], cubic[1], cubic[2], 0.0, roots[0] );
} else {
if( b < 0 && d < 0 ) {
y = PolishRoot( 3, cubic[0], cubic[1], cubic[2], 0.0, roots[2] );
} else {
y = PolishRoot( 3, cubic[0], cubic[1], cubic[2], 0.0, roots[0] );
}
}
g1 = a/2.0;
h1 = (b-y)/2.0;
if( y < 0 ) {
n = a*a - 4*y;
if( n <= 0 ) {
return 0;
}
g2 = sqrt(n);
if( g2 == 0 ) {
return 0;
}
h2 = (a*((b-y)/2.0) - c) / g2;
g2 /= 2.0;
} else if( y > 0 && d > 0 && b < 0 ) {
m = (b-y)*(b-y) - 4*d;
if( m <= 0 ) {
return 0;
}
h2 = sqrt(m);
if( h2 == 0 ) {
return 0;
}
g2 = (a*h1 - c) / h2;
h2 /= 2.0;
} else {
n = a*a - 4*y;
m = (b-y)*(b-y) - 4*d;
en = b*b + 2.*fabs(b*y) + y*y + 4*fabs(d);
em = a*a + 4.*fabs(y);
if( m*en > n*em ) { /* use m */
if( m <= 0 ) {
return 0;
}
h2 = sqrt(m);
if( h2 == 0 ) {
return 0;
}
g2 = (a*h1 - c) / h2;
h2 /= 2.0;
} else { /* use n */
if (n <= 0) {
return 0;
}
g2 = sqrt(n);
if (g2 == 0) {
return 0;
}
h2 = (a*( (b-y)/2.0) - c) / g2;
g2 /= 2.0;
}
}
if( SIGN(g1) == SIGN(g2) ) {
G = g1 + g2;
g = (G==0) ? g1-g2 : y/G;
} else {
g = g1 - g2;
G = (g == 0) ? g1+g2 : y/g;
}
if( SIGN(h1) == SIGN(h2) ) {
H = h1+h2;
h = (H == 0) ? h1-h2 : d/H;
} else {
h = h1 - h2;
H = (h == 0) ? h1+h2 : d/h;
}
nr = quadraticRoots( 1.0, G, H, roots );
nr += quadraticRoots( 1.0, g, h, roots+nr );
for( i=0; i < nr; ++i ) {
roots[i] = PolishRoot( 4, a, b, c, d, roots[i] );
}
/* remove non-roots */
// fprintf(stderr,"REMOVE NONROOTS %d\n",nr);
for ( i=0; i < nr; i++ ) {
double r;
r = (((roots[i]+a)*roots[i]+b)*roots[i]+c)*roots[i]+d;
// fprintf(stderr,"root %d is %g\n",i,r);
if ( fabs(r)>1e-4 ) {
roots[i] = roots[nr-1];
nr--; i--;
}
}
return nr;
}
int svdcmp( double **a, int m,int n, double *w,double **v)
{
int flag,i,its,j,jj,k,ii=0,nm=0;
SVD_FLOAT c,f,h,s,x,y,z;
SVD_FLOAT anorm=0.0,g=0.0,scale=0.0;
if (m < n) return -1; // must augment A with extra zero rows
assert(n==3);
SVD_FLOAT rv1[3]; // was: rv1=G_alloc_vector(n);
n--;
m--;
for (i=0;i<=n;i++) {
ii=i+1;
rv1[i]=scale*g;
g=s=scale=0.0;
if (i <= m) {
for (k=i;k<=m;k++) scale += (SVD_FLOAT)fabs(a[k][i]);
if (scale) {
for (k=i;k<=m;k++) {
a[k][i] /= (SVD_FLOAT)(scale);
s += a[k][i]*a[k][i];
}
f=a[i][i];
g = -SIGN(sqrt(s),f);
h=f*g-s;
a[i][i]=(SVD_FLOAT)(f-g);
if (i != n) {
for (j=ii;j<=n;j++) {
for (s=0.0,k=i;k<=m;k++) s += a[k][i]*a[k][j];
f=s/h;
for (k=i;k<=m;k++) a[k][j] += (SVD_FLOAT)(f)*a[k][i];
}
}
for (k=i;k<=m;k++) a[k][i] *= (SVD_FLOAT)(scale);
}
}
w[i]=scale*g;
g=s=scale=0.0;
if (i <= m && i != n) {
for (k=ii;k<=n;k++) scale += fabs(a[i][k]);
if (scale) {
for (k=ii;k<=n;k++) {
a[i][k] /= scale;
s += a[i][k]*a[i][k];
}
f=a[i][ii];
g = -SIGN(sqrt(s),f);
h=f*g-s;
a[i][ii]=f-g;
for (k=ii;k<=n;k++) rv1[k]=a[i][k]/h;
if (i != m) {
for (j=ii;j<=m;j++) {
for (s=0.0,k=ii;k<=n;k++) s += a[j][k]*a[i][k];
for (k=ii;k<=n;k++) a[j][k] += s*rv1[k];
}
}
for (k=ii;k<=n;k++) a[i][k] *= scale;
}
}
anorm=MAX(anorm,(fabs(w[i])+fabs(rv1[i])));
}
for (i=n;i>=0;i--) {
if (i < n) {
if (g) {
for (j=ii;j<=n;j++)
v[j][i]=(a[i][j]/a[i][ii])/g;
for (j=ii;j<=n;j++) {
for (s=0.0,k=ii;k<=n;k++) s += a[i][k]*v[k][j];
for (k=ii;k<=n;k++) v[k][j] += s*v[k][i];
}
}
for (j=ii;j<=n;j++) v[i][j]=v[j][i]=0.0;
}
v[i][i]=1.0;
g=rv1[i];
ii=i;
}
for (i=n;i>=0;i--) {
ii=i+1;
g=w[i];
if (i < n)
for (j=ii;j<=n;j++) a[i][j]=0.0;
if (g) {
g=SVD_FLOAT(1.0)/g;
if (i != n) {
for (j=ii;j<=n;j++) {
for (s=0.0,k=ii;k<=m;k++) s += a[k][i]*a[k][j];
f=(s/a[i][i])*g;
for (k=i;k<=m;k++) a[k][j] += f*a[k][i];
}
}
for (j=i;j<=m;j++) a[j][i] *= g;
} else {
for (j=i;j<=m;j++) a[j][i]=0.0;
}
++a[i][i];
}
for (k=n;k>=0;k--) {
for (its=1;its<=30;its++) {
flag=1;
for (ii=k;ii>=0;ii--) {
nm=ii-1;
if (fabs(rv1[ii])+anorm == anorm) {
flag=0;
break;
}
if (fabs(w[nm])+anorm == anorm) break;
}
if (flag) {
c=0.0;
s=1.0;
for (i=ii;i<=k;i++) {
f=s*rv1[i];
if (fabs(f)+anorm != anorm) {
g=w[i];
h=PYTHAG(f,g);
w[i]=h;
h=SVD_FLOAT(1.0)/h;
c=g*h;
s=(-f*h);
for (j=0;j<=m;j++) {
y=a[j][nm];
z=a[j][i];
a[j][nm]=y*c+z*s;
a[j][i]=z*c-y*s;
}
}
}
}
z=w[k];
if (ii == k) {
if (z < 0.0) {
w[k] = -z;
for (j=0;j<=n;j++) v[j][k]=(-v[j][k]);
}
break;
}
if (its == 30) return -2; /*No convergence in 30 SVDCMP iterations*/
x=w[ii];
nm=k-1;
y=w[nm];
g=rv1[nm];
h=rv1[k];
f=((y-z)*(y+z)+(g-h)*(g+h))/(SVD_FLOAT(2.0)*h*y);
g=PYTHAG(f,SVD_FLOAT(1.0));
f=((x-z)*(x+z)+h*((y/(f+SIGN(g,f)))-h))/x;
c=s=SVD_FLOAT(1.0);
for (j=ii;j<=nm;j++) {
i=j+1;
g=rv1[i];
y=w[i];
h=s*g;
g=c*g;
z=PYTHAG(f,h);
rv1[j]=z;
c=f/z;
s=h/z;
f=x*c+g*s;
g=g*c-x*s;
h=y*s;
y=y*c;
for (jj=0;jj<=n;jj++) {
x=v[jj][j];
z=v[jj][i];
v[jj][j]=x*c+z*s;
v[jj][i]=z*c-x*s;
}
z=PYTHAG(f,h);
w[j]=z;
if (z) {
z=SVD_FLOAT(1.0)/z;
c=f*z;
s=h*z;
}
f=(c*g)+(s*y);
x=(c*y)-(s*g);
for (jj=0;jj<=m;jj++) {
y=a[jj][j];
z=a[jj][i];
a[jj][j]=y*c+z*s;
a[jj][i]=z*c-y*s;
}
}
rv1[ii]=SVD_FLOAT(0.0);
rv1[k]=f;
w[k]=x;
}
}
//G_free_vector(rv1); // no longer used, rv1 is now on stack
return 0;
}
Real NewtonSolver::line_search(Real tol,
Real last_residual,
Real ¤t_residual,
NumericVector<Number> &newton_iterate,
const NumericVector<Number> &linear_solution)
{
// Take a full step if we got a residual reduction or if we
// aren't substepping
if ((current_residual < last_residual) ||
(!require_residual_reduction &&
(!require_finite_residual || !libmesh_isnan(current_residual))))
return 1.;
// The residual vector
NumericVector<Number> &rhs = *(_system.rhs);
Real ax = 0.; // First abscissa, don't take negative steps
Real cx = 1.; // Second abscissa, don't extrapolate steps
// Find bx, a step length that gives lower residual than ax or cx
Real bx = 1.;
while (libmesh_isnan(current_residual) ||
(current_residual > last_residual &&
require_residual_reduction))
{
// Reduce step size to 1/2, 1/4, etc.
Real substepdivision;
if (brent_line_search && !libmesh_isnan(current_residual))
{
substepdivision = std::min(0.5, last_residual/current_residual);
substepdivision = std::max(substepdivision, tol*2.);
}
else
substepdivision = 0.5;
newton_iterate.add (bx * (1.-substepdivision),
linear_solution);
newton_iterate.close();
bx *= substepdivision;
if (verbose)
libMesh::out << " Shrinking Newton step to "
<< bx << std::endl;
// Check residual with fractional Newton step
_system.assembly (true, false);
rhs.close();
current_residual = rhs.l2_norm();
if (verbose)
libMesh::out << " Current Residual: "
<< current_residual << std::endl;
if (bx/2. < minsteplength &&
(libmesh_isnan(current_residual) ||
(current_residual > last_residual)))
{
libMesh::out << "Inexact Newton step FAILED at step "
<< _outer_iterations << std::endl;
if (!continue_after_backtrack_failure)
{
libmesh_convergence_failure();
}
else
{
libMesh::out << "Continuing anyway ..." << std::endl;
_solve_result = DiffSolver::DIVERGED_BACKTRACKING_FAILURE;
return bx;
}
}
} // end while (current_residual > last_residual)
// Now return that reduced-residual step, or use Brent's method to
// find a more optimal step.
if (!brent_line_search)
return bx;
// Brent's method adapted from Numerical Recipes in C, ch. 10.2
Real e = 0.;
Real x = bx, w = bx, v = bx;
// Residuals at bx
Real fx = current_residual,
fw = current_residual,
fv = current_residual;
// Max iterations for Brent's method loop
const unsigned int max_i = 20;
// for golden ratio steps
const Real golden_ratio = 1.-(std::sqrt(5.)-1.)/2.;
for (unsigned int i=1; i <= max_i; i++)
{
Real xm = (ax+cx)*0.5;
Real tol1 = tol * std::abs(x) + tol*tol;
Real tol2 = 2.0 * tol1;
// Test if we're done
if (std::abs(x-xm) <= (tol2 - 0.5 * (cx - ax)))
return x;
Real d;
// Construct a parabolic fit
if (std::abs(e) > tol1)
{
Real r = (x-w)*(fx-fv);
Real q = (x-v)*(fx-fw);
Real p = (x-v)*q-(x-w)*r;
q = 2. * (q-r);
if (q > 0.)
p = -p;
else
q = std::abs(q);
if (std::abs(p) >= std::abs(0.5*q*e) ||
p <= q * (ax-x) ||
p >= q * (cx-x))
{
// Take a golden section step
e = x >= xm ? ax-x : cx-x;
d = golden_ratio * e;
}
else
{
// Take a parabolic fit step
d = p/q;
if (x+d-ax < tol2 || cx-(x+d) < tol2)
d = SIGN(tol1, xm - x);
}
}
else
{
// Take a golden section step
e = x >= xm ? ax-x : cx-x;
d = golden_ratio * e;
}
Real u = std::abs(d) >= tol1 ? x+d : x + SIGN(tol1,d);
// Assemble the residual at the new steplength u
newton_iterate.add (bx - u, linear_solution);
newton_iterate.close();
bx = u;
if (verbose)
libMesh::out << " Shrinking Newton step to "
<< bx << std::endl;
_system.assembly (true, false);
rhs.close();
Real fu = current_residual = rhs.l2_norm();
if (verbose)
libMesh::out << " Current Residual: "
<< fu << std::endl;
if (fu <= fx)
{
if (u >= x)
ax = x;
else
cx = x;
v = w; w = x; x = u;
fv = fw; fw = fx; fx = fu;
}
else
{
if (u < x)
ax = u;
else
cx = u;
if (fu <= fw || w == x)
{
v = w; w = u;
fv = fw; fw = fu;
}
else if (fu <= fv || v == x || v == w)
{
v = u;
fv = fu;
}
}
}
if (!quiet)
libMesh::out << "Warning! Too many iterations used in Brent line search!"
<< std::endl;
return bx;
}
int dbt_cmp_val(dbt_val_p _vp, db_val_t* _v)
{
int _l, _n;
if(!_vp && !_v)
return 0;
if(!_v)
return 1;
if(!_vp)
return -1;
if(_vp->nul && _v->nul)
return 0;
if(_v->nul)
return 1;
if(_vp->nul)
return -1;
switch(VAL_TYPE(_v))
{
case DB1_INT:
return (_vp->val.int_val<_v->val.int_val)?-1:
(_vp->val.int_val>_v->val.int_val)?1:0;
case DB1_BIGINT:
LM_ERR("BIGINT not supported\n");
return -1;
case DB1_DOUBLE:
return (_vp->val.double_val<_v->val.double_val)?-1:
(_vp->val.double_val>_v->val.double_val)?1:0;
case DB1_DATETIME:
return (_vp->val.int_val<_v->val.time_val)?-1:
(_vp->val.int_val>_v->val.time_val)?1:0;
case DB1_STRING:
_l = strlen(_v->val.string_val);
_l = (_l>_vp->val.str_val.len)?_vp->val.str_val.len:_l;
_n = strncasecmp(_vp->val.str_val.s, _v->val.string_val, _l);
if(_n)
return SIGN(_n);
if(_vp->val.str_val.len == strlen(_v->val.string_val))
return 0;
if(_l==_vp->val.str_val.len)
return -1;
return 1;
case DB1_STR:
_l = _v->val.str_val.len;
_l = (_l>_vp->val.str_val.len)?_vp->val.str_val.len:_l;
_n = strncasecmp(_vp->val.str_val.s, _v->val.str_val.s, _l);
if(_n)
return SIGN(_n);
if(_vp->val.str_val.len == _v->val.str_val.len)
return 0;
if(_l==_vp->val.str_val.len)
return -1;
return 1;
case DB1_BLOB:
_l = _v->val.blob_val.len;
_l = (_l>_vp->val.str_val.len)?_vp->val.str_val.len:_l;
_n = strncasecmp(_vp->val.str_val.s, _v->val.blob_val.s, _l);
if(_n)
return SIGN(_n);
if(_vp->val.str_val.len == _v->val.blob_val.len)
return 0;
if(_l==_vp->val.str_val.len)
return -1;
return 1;
case DB1_BITMAP:
return (_vp->val.int_val<_v->val.bitmap_val)?-1:
(_vp->val.int_val>_v->val.bitmap_val)?1:0;
default:
LM_ERR("invalid datatype %d\n", VAL_TYPE(_v));
return -2;
}
return -2;
}
void draw_line( int x1, int y1, int x2, int y2, char value ) {
#if 0
if ( x1 == x2 ) {
//Draw vertical line
unsigned char i;
for ( i = y1; i <= y2; i++ ) {
draw_char( x1, i, value );
}
}
else if ( y1 == y2 ) {
//Draw horizontal line
unsigned char i;
for ( i = x1; i <= x2; i++ ) {
draw_char( i, y1, value );
}
}
else {
// Figure out octant
unsigned int oct;
float g = ( (float)y2 - y1 ) / ( (float)x2 - x1 );
if ( x2 > x1 ) {
if ( g > 1 ) {
oct = 1;
}
else if ( g <= 1 && g > 0 ) {
oct = 0;
}
else if ( g <= 0 && g > -1 ) {
oct = 7;
}
else {
oct = 6;
}
}
else {
if ( g > 1 ) {
oct = 5;
}
else if ( g <= 1 && g > 0 ) {
oct = 4;
}
else if ( g <= 0 && g > -1 ) {
oct = 3;
}
else {
oct = 2;
}
}
// Translate octants to settings
unsigned int useX;
switch ( oct ) {
case 0:
case 3:
case 4:
case 7:
useX = 1;
break;
case 1:
case 2:
case 5:
case 6:
useX = 0;
break;
}
unsigned int i1, i, i2, c;
float dx = (float)x2 - x1, dy = (float)y2 - y1, m;
if ( useX ) {
m = dy / dx;
c = y1; i1 = x1; i2 = x2;
}
else {
m = dx / dy;
c = x1; i1 = y1; i2 = y2;
}
int flipM;
switch ( oct ) {
case 0:
case 1:
case 2:
case 7:
flipM = 1;
break;
case 3:
case 4:
case 5:
case 6:
flipM = -1;
break;
}
// Perform the actual loop
float err = 0.0;
for ( i = i1; ( i2 > i1 ) ? i <= i2 : i >= i2; ( i2 > i1 ) ? i++ : i-- ) {
( useX ) ? draw_char( i, c, value ) : draw_char( c, i, value );
err += ABS( m );
if ( err > 0.5f ) {
( SIGN( m*flipM ) > 0 ) ? c++ : c--;
err -= 1.0f;
}
}
}
#else
if ( x1 == x2 ) {
// Draw vertical line
for ( int i = y1; ( y2 > y1 ) ? i <= y2 : i >= y2; ( y2 > y1 ) ? i++ : i-- ) {
draw_char( x1, i, value );
}
}
else if ( y1 == y2 ) {
// Draw horizontal line
for ( int i = x1; ( x2 > x1 ) ? i <= x2 : i >= x2; ( x2 > x1 ) ? i++ : i-- ) {
draw_char( i, y1, value );
}
}
else {
// Get Bresenhaming...
float dx = x2-x1;
float dy = y2-y1;
float err = 0.0;
float derr = ABS( dy/dx );
for ( int x = x1, y = y1; ( dx > 0 ) ? x <= x2 : x >= x2; ( dx > 0 ) ? x++ : x-- ) {
draw_char( x, y, value );
err += derr;
while ( err >= 0.5 && ( ( dy > 0 ) ? y <= y2 : y >= y2 ) ) {
draw_char( x, y, value );
y += ( dy > 0 ) - ( dy < 0 );
err -= 1.0;
}
}
}
#endif
}