// set_euler expects a vector containing the Euler angles in the format
// (c,b,a), where a is the angle of the first rotation, and c is the last.
// The current implementation uses XYZ convention (Z is the first rotation).
void Basis::set_euler(const Vector3& p_euler) {
real_t c, s;
c = Math::cos(p_euler.x);
s = Math::sin(p_euler.x);
Basis xmat(1.0,0.0,0.0,0.0,c,-s,0.0,s,c);
c = Math::cos(p_euler.y);
s = Math::sin(p_euler.y);
Basis ymat(c,0.0,s,0.0,1.0,0.0,-s,0.0,c);
c = Math::cos(p_euler.z);
s = Math::sin(p_euler.z);
Basis zmat(c,-s,0.0,s,c,0.0,0.0,0.0,1.0);
//optimizer will optimize away all this anyway
*this = xmat*(ymat*zmat);
}
std::vector<double> SparseSolverEigenCustom::solve_Ax_b(SparseMatrix<double> A, SparseVector<double> b, int numel, std::vector<int> & uidx, const std::vector<int> * labels, const std::vector<int> * seeds, int active_label)
{
// Custom solver
SparseVector<double> x = this->cgSolveSparse(A,b,iterations,tolerance);
std::vector<double> xmat(numel);
// All entries are non-zero except at seeds
for (SparseVector<double>::InnerIterator it(x); it; ++it)
{
xmat[uidx[it.index()]] = it.value();
}
for (size_t i=0; i<seeds->size(); i++)
{
if((*labels)[i] == active_label)
xmat[(*seeds)[i]] = 1.0;
else
xmat[(*seeds)[i]] = 0.0;
}
return xmat;
}
Results* join_clusters2_restart
(double *x,//array/matrix of data
SymNoDiag *W,//lower triangle of weight matrix
unsigned int Px,//problem size
double lambda,//starting point in regularization path
double join_thresh, //tolerance for equality of points
double opt_thresh, //tolerance for optimality
double lambda_factor,//increase of lambda after optimality
double smooth,//smoothing parameter
int maxit,
int linesearch_freq,//how often to do a linesearch? if 0, never. if
//n>0, do n-1 linesearch steps for every
//decreasing step size step. set this to 2 if
//unsure.
int linesearch_points,//how many points to check along the gradient
//direction. set to 10 if unsure.
int check_splits,
int target_cluster,
int verbose
){
unsigned int N = W->N;
//W->print();
double old_lambda=0;
std::vector<int> rows,rowsj;
std::vector<int>::iterator rowit,ri,rj;
std::list< std::vector<int> > clusters,tocheck;
std::list< std::vector<int> >::iterator it,cj;
unsigned int i,k,j;
int tried_restart;
for(i=0;i<N;i++){
rows.assign(1,i);
clusters.push_back(rows);
}
double *old_alpha = new double[N*Px];
double *alpha = new double[N*Px];
double *xbar = new double[N*Px];
double *dir = new double[N*Px];
for(i=0;i<N*Px;i++){
alpha[i]=xbar[i]=x[i];
}
Matrix amat(alpha,N,Px),xmat(x,N,Px);
SymNoDiag diffs(N);
diffs.calc_diffs(clusters,amat,nrm2);
//store initial trivial solution
Results *results = new Results(N,Px,opt_thresh);
if(target_cluster==0)results->add(alpha,0,0);
double weight,diff,step;
while(clusters.size()>1){
double grad=opt_thresh;
int iteration=1;
tried_restart=0;
//if we use the general (slower) algorithm for any weights, then
//split the clusters to individual points
if(check_splits){
clusters.clear();
//reassign original clusters
for(i=0;i<N;i++){
rows.assign(1,i);
clusters.push_back(rows);
}
//recopy original xbar
for(i=0;i<N*Px;i++){
xbar[i]=x[i];
}
}
while(grad>=opt_thresh){
//first calc gradients
grad = 0;
for(it=clusters.begin();it!=clusters.end();it++){
rows = *it;
i = rows[0];
for(k=0;k<Px;k++){
dir[i+k*N] = xbar[i+k*N] - alpha[i+k*N];
}
for(cj=clusters.begin();cj!=clusters.end();cj++){
if(it!=cj){
rowsj = *cj;
j=rowsj[0];
weight=0;
diff = *diffs(i,j);
if(diff!=0){
if(smooth!=0){
diff *= diff; //now squared l2 norm
diff += smooth; //add smoothing parameter under sqrt
diff = sqrt(diff);//put sqrt back
}
for(ri=rows.begin();ri!=rows.end();ri++){
for(rj=rowsj.begin();rj!=rowsj.end();rj++){
weight += W->getval(*ri,*rj);
}
}
//weight *= lambda / diff / ((double)(N-1)) / ((double)rows.size());
weight *= lambda / diff / ((double)rows.size());
for(k=0;k<Px;k++){
dir[i+k*N] += weight * (alpha[j+k*N]-alpha[i+k*N]);
}
}
}
}
grad += nrm2(Array(dir+i,N,Px));
}
//store this iteration
//results->add(alpha,lambda,grad);
//then take a step
if(linesearch_freq==0 || (iteration % linesearch_freq)==0 ){
//Decreasing step size
//TDH and pierre 18 jan 2011 try sqrt dec step size
step=1/((double)iteration);
//step=1/sqrt((double)iteration);
if(verbose>=2)printf("grad %f step %f it %d\n",grad,step,iteration);
take_step(clusters,alpha,dir,N,Px,step);
}else{
double cost_here,cost_step;
std::map<double,double> cost_steps;
std::map<double,double>::iterator step1,step2;
for(i=0;i<N*Px;i++)old_alpha[i]=alpha[i];//copy alpha
//compare current cost to cost after stepping in gradient direction
cost_here=cost_step=calc_cost(clusters,amat,xmat,W,diffs,lambda);
step = 0;
cost_steps.insert(std::pair<double,double>(cost_here,0));
while(cost_step<=cost_here){
take_step(clusters,alpha,dir,N,Px,1);
step += 1;
diffs.calc_diffs(clusters,amat,nrm2);
cost_step=calc_cost(clusters,amat,xmat,W,diffs,lambda);
if(verbose>=2)
printf("cost %.10f step %f cost_here %f\n",cost_step,step,cost_here);
cost_steps.insert(std::pair<double,double>(cost_step,step));
}
for(int cuts=0;cuts<linesearch_points;cuts++){
step1=step2=cost_steps.begin();
step2++;
step = (step1->second + step2->second)/2;
for(i=0;i<N*Px;i++){
alpha[i]=old_alpha[i];
}
take_step(clusters,alpha,dir,N,Px,step);
diffs.calc_diffs(clusters,amat,nrm2);
cost_step=calc_cost(clusters,amat,xmat,W,diffs,lambda);
if(verbose>=2)printf("cost %.10f step %f %d\n",cost_step,step,cuts);
cost_steps.insert(std::pair<double,double>(cost_step,step));
}
cost_steps.clear();
}
if(iteration++ > maxit){
if(tried_restart){
printf("max iteration %d exit\n",maxit);
delete old_alpha;
delete alpha;
delete xbar;
delete dir;
return results;
}else{
if(verbose>=1)printf("max iterations, trying restart from x\n");
tried_restart=1;
iteration=1;
for(i=0;i<N*Px;i++)alpha[i]=x[i];
}
}
//calculate differences
diffs.calc_diffs(clusters,amat,nrm2);
//check for joins
JoinPair tojoin;
while(dojoin(tojoin=check_clusters_thresh(&clusters,diffs,join_thresh))){
//if(verbose>=1)
// printf("join: %d %d\n",tojoin.first->front(),tojoin.second->front());
int ni=tojoin.first->size();
int nj=tojoin.second->size();
i=tojoin.first->front();
j=tojoin.second->front();
tojoin.first->insert(tojoin.first->end(),
tojoin.second->begin(),
tojoin.second->end());
for(k=0;k<Px;k++){
alpha[i+k*N] = (alpha[i+k*N]*ni + alpha[j+k*N]*nj)/(ni+nj);
xbar[i+k*N] = (xbar[i+k*N]*ni + xbar[j+k*N]*nj)/(ni+nj);
}
clusters.erase(tojoin.second);
iteration=1;
if(clusters.size()>1){
diffs.calc_diffs(clusters,amat,nrm2);//inefficient
}else{
grad=0;//so we can escape from the last optimization loop
}
}
}//while(grad>=opt_thresh)
if(verbose>=1)
printf("solution iteration %d lambda %f nclusters %d\n",
iteration,lambda,(int)clusters.size());
if(target_cluster == 0){
//for each cluster, there may be several points. we store the
//alpha value just in the row of the first point. thus here we
//copy this value to the other rows before copying the optimal
//alpha to results.
for(it=clusters.begin();it!=clusters.end();it++){
rows = *it;
if(rows.size()>1){
for(i=1;i<rows.size();i++){
for(k=0;k<Px;k++){
alpha[rows[i]+k*N] = alpha[rows[0]+k*N];
}
}
}
}
results->add(alpha,lambda,grad);
}
//haven't yet reached the target number of clusters, multiply
//lambda by lambda_factor and continue along the path
if((int)clusters.size()>target_cluster){
old_lambda=lambda;
lambda *= lambda_factor;
}
//if we have passed the target cluster number then decrease
//lambda and go look for it!
if((int)clusters.size()<target_cluster){
if(verbose>=1){
printf("missed target %d, going back for it\n",target_cluster);
}
lambda = (lambda+old_lambda)/2;
clusters.clear();
//reassign original clusters
for(i=0;i<N;i++){
rows.assign(1,i);
clusters.push_back(rows);
}
//recopy original xbar
for(i=0;i<N*Px;i++){
xbar[i]=x[i];
}
}
//this is the number of clusters that we were looking for,
//save and quit!
if((int)clusters.size()==target_cluster){
for(it=clusters.begin();it!=clusters.end();it++){
rows = *it;
if(rows.size()>1){
for(i=1;i<rows.size();i++){
for(k=0;k<Px;k++){
alpha[rows[i]+k*N] = alpha[rows[0]+k*N];
}
}
}
}
results->add(alpha,lambda,grad);
if(verbose>=1)printf("got target cluster %d exit\n",target_cluster);
delete old_alpha;
delete alpha;
delete xbar;
delete dir;
return results;
}
}
//TODO: consolidate cleanup... just use data structures that
//automatically clean themselves up when the function exits.
delete old_alpha;
delete alpha;
delete xbar;
delete dir;
return results;
}
void loadlgf(int mode)
{
FILE *ps;
char inname[150], temp[500], *pdchar;
char **signals;
short *signets;
objectptr *libobj;
genericptr *iolabel;
int i, sigs;
sscanf(_STR, "%149s", inname);
ps = fopen(inname, "r");
if (ps == NULL) {
sprintf(inname, "%s.lgf", _STR);
ps = fopen(inname, "r");
if (ps == NULL) {
sprintf(inname, "%s.lfo", _STR);
ps = fopen(inname, "r");
if (ps == NULL) {
Wprintf("Can't open LGF file %s", inname);
return;
}
}
}
/* for PostScript file, remove ".lgf" or ".lfo" (to be replaced with ".ps") */
if ((pdchar = strstr(inname, ".l")) != NULL) *pdchar = '\0';
Wprintf("Loaded file: %s", inname);
/* Make sure that LGF object library has been loaded by loading it now. */
if (NameToLibrary(LGF_LIB) < 0) {
int ilib;
strcpy(_STR, LGF_LIB);
ilib = createlibrary(FALSE);
loadlibrary(ilib);
}
/* Read header information */
if (fgets(temp, 149, ps) == NULL) {
Wprintf("Error: end of file.");
return;
}
for (pdchar = temp; *pdchar != '-' && *pdchar != '\n'; pdchar++);
if (*pdchar == '\n') {
Wprintf("Not an LGF file?");
return;
}
if (*(++pdchar) != '5') {
Wprintf("Don't know how to read version %c.", *pdchar);
return;
}
if (fgets(temp, 149, ps) == NULL) {
Wprintf("Error: end of file.");
return;
}
for (pdchar = temp; *pdchar != 'f' && *pdchar != '\n'; pdchar++);
for (; *pdchar != 's' && *pdchar != '\n'; pdchar++);
if (*pdchar == '\n') {
Wprintf("Something wrong with the LGF file?");
return;
}
/* Done with header. . . okay to clear current page now unless importing */
if (mode == 0) {
reset(topobject, NORMAL);
pagereset(areawin->page);
}
/* Set up filename and object (page) name */
xobjs.pagelist[areawin->page]->filename = (char *) realloc (
xobjs.pagelist[areawin->page]->filename, (strlen(inname) + 1) * sizeof(char));
strcpy(xobjs.pagelist[areawin->page]->filename, inname);
/* If the filename has a path component, use only the root */
if ((pdchar = strrchr(inname, '/')) != NULL)
sprintf(topobject->name, "%s", pdchar + 1);
else
sprintf(topobject->name, "%s", inname);
renamepage(areawin->page);
printname(topobject);
/* Read objects */
for(;;) {
char *lineptr, keyptr, tmpstring[256];
int dval;
short pvalx, pvaly, pvalx2, pvaly2;
if (fgets(temp, 499, ps) == NULL) break; /* End-Of-File */
/* ignore whitespace */
for (lineptr = temp; isspace(*lineptr) && *lineptr != '\n'; lineptr++);
if (*lineptr == '\n') continue; /* ignore blank lines */
switch(keyptr = *lineptr) {
case '#': /* comment */
break;
case 'n': /* nodes */
sscanf(++lineptr, "%d", &dval);
for (i = 0; i < dval; i++) {
do {
if (fgets(temp, 499, ps) == NULL) {
Wprintf("End of file in node section");
return;
}
for (lineptr = temp; isspace(*lineptr) && *lineptr != '\n'; lineptr++);
} while (*lineptr == '\n');
}
break;
case 's': /* signal names --- save for future reference */
sscanf(++lineptr, "%d", &sigs);
signals = (char **) malloc(sigs * sizeof(char *));
signets = (short *) malloc(sigs * sizeof(short));
for (i = 0; i < sigs; i++) {
do {
if (fgets(temp, 499, ps) == NULL) {
Wprintf("End of file in signal section");
return;
}
for (lineptr = temp; isspace(*lineptr) && *lineptr != '\n'; lineptr++);
} while (*lineptr == '\n');
sscanf(lineptr, "%hd %249s", &signets[i], tmpstring);
signals[i] = (char *)malloc((strlen(tmpstring) + 1) * sizeof(char));
sprintf(signals[i], "%s", tmpstring);
}
break;
case 'l': { /* labels */
labelptr *newlabel;
char *tstrp;
sscanf(++lineptr, "%d", &dval);
for (i = 0; i < dval; i++) {
do {
if (fgets(temp, 499, ps) == NULL) {
Wprintf("End of file in signal section");
return;
}
for (lineptr = temp; isspace(*lineptr) && *lineptr != '\n'; lineptr++);
} while (*lineptr == '\n');
/* Allocate label, and put node number into X value, to be replaced */
/* Flag it using an inappropriate rotation value (= 500) */
sscanf(lineptr, "%hd %hd", &pvalx, &pvaly);
/* Get rid of newline character, if any */
ridnewline(lineptr);
/* forward to the label part of the input line */
tstrp = lineptr - 1;
while (isdigit(*(++tstrp)));
while (isspace(*(++tstrp)));
while (isdigit(*(++tstrp)));
while (isspace(*(++tstrp)));
while (isdigit(*(++tstrp)));
while (isspace(*(++tstrp)));
if (tstrp != NULL) { /* could be a blank line */
stringpart *strptr;
NEW_LABEL(newlabel, topobject);
labeldefaults(*newlabel, False, xmat(pvalx), ymat(pvaly));
(*newlabel)->justify = TOP | NOTBOTTOM;
(*newlabel)->color = DEFAULTCOLOR;
(*newlabel)->string->data.font = 0;
strptr = makesegment(&((*newlabel)->string), NULL);
strptr->type = TEXT_STRING;
strptr->data.string = (char *)malloc(1 + strlen(tstrp));
strcpy(strptr->data.string, tstrp);
(*newlabel)->pin = NORMAL;
}
}}
break;
case 'w': { /* wires, implemented as single-segment polygons */
polyptr *newwire;
XPoint *tmppnts;
sscanf(++lineptr, "%d", &dval);
for (i = 0; i < dval; i++) {
do {
if (fgets(temp, 499, ps) == NULL) {
Wprintf("End of file in wire section");
return;
}
for (lineptr = temp; isspace(*lineptr) && *lineptr != '\n'; lineptr++);
} while (*lineptr == '\n');
/* Allocate wire */
NEW_POLY(newwire, topobject);
sscanf(lineptr, "%hd %hd %hd %hd", &pvalx, &pvaly, &pvalx2, &pvaly2);
(*newwire)->number = 2;
(*newwire)->points = (XPoint *)malloc(2 * sizeof(XPoint));
(*newwire)->width = 1.0;
(*newwire)->style = UNCLOSED;
(*newwire)->color = DEFAULTCOLOR;
(*newwire)->passed = NULL;
tmppnts = (*newwire)->points;
tmppnts->x = xmat(pvalx);
tmppnts->y = ymat(pvaly);
(++tmppnts)->x = xmat(pvalx2);
tmppnts->y = ymat(pvaly2);
}}
break;
case 'p': /* solder dot */
sscanf(++lineptr, "%d", &dval);
for (i = 0; i < dval; i++) {
do {
if (fgets(temp, 499, ps) == NULL) {
Wprintf("End of file in solder dot section");
return;
}
for (lineptr = temp; isspace(*lineptr) && *lineptr != '\n'; lineptr++);
} while (*lineptr == '\n');
/* Allocate arc */
sscanf(lineptr, "%hd %hd", &pvalx, &pvaly);
drawdot(xmat(pvalx), ymat(pvaly));
}
break;
case 'b': { /* boxes */
polyptr *newpoly;
pointlist newpoints;
sscanf(++lineptr, "%d", &dval);
for (i = 0; i < dval; i++) {
do {
if (fgets(temp, 499, ps) == NULL) {
Wprintf("End of file in box section");
return;
}
for (lineptr = temp; isspace(*lineptr) && *lineptr != '\n'; lineptr++);
} while (*lineptr == '\n');
NEW_POLY(newpoly, topobject);
(*newpoly)->style = DASHED;
(*newpoly)->color = DEFAULTCOLOR;
(*newpoly)->width = 1.0;
(*newpoly)->number = 4;
(*newpoly)->points = (pointlist) malloc(4 * sizeof(XPoint));
(*newpoly)->passed = NULL;
newpoints = (*newpoly)->points;
sscanf(lineptr, "%hd %hd %hd %hd", &pvalx, &pvaly, &pvalx2, &pvaly2);
newpoints->x = xmat(pvalx);
newpoints->y = ymat(pvaly);
(newpoints + 1)->x = xmat(pvalx2);
(newpoints + 2)->y = ymat(pvaly2);
(newpoints + 2)->x = (newpoints + 1)->x;
(newpoints + 3)->x = newpoints->x;
(newpoints + 1)->y = newpoints->y;
(newpoints + 3)->y = (newpoints + 2)->y;
}}
break;
case 'g': { /* gates */
objinstptr *newinst;
labelptr *newlabel;
int j, k, hval, flip;
sscanf(++lineptr, "%d", &dval);
for (i = 0; i < dval; i++) {
do {
if (fgets(temp, 499, ps) == NULL) {
Wprintf("End of file in gates section");
return;
}
for (lineptr = temp; *lineptr != '\n'; lineptr++); *lineptr = '\0';
for (lineptr = temp; isspace(*lineptr) && *lineptr != '\0'; lineptr++);
} while (*lineptr == '\0');
/* double loop through user libraries */
for (j = 0; j < xobjs.numlibs; j++) {
for (k = 0; k < xobjs.userlibs[j].number; k++) {
libobj = xobjs.userlibs[j].library + k;
if (!strcmp(lineptr, (*libobj)->name)) break;
}
if (k < xobjs.userlibs[j].number) break;
}
strcpy(tmpstring, lineptr);
/* read gate definition */
if (fgets(temp, 499, ps) == NULL) {
Wprintf("End of file during gate read");
return;
}
for (lineptr = temp; isspace(*lineptr) && *lineptr != '\n'; lineptr++);
if (j < xobjs.numlibs || k < xobjs.userlibs[xobjs.numlibs - 1].number) {
NEW_OBJINST(newinst, topobject);
sscanf(lineptr, "%hd %hd %hd %*d %*d %*d %d", &pvalx, &pvaly,
&pvalx2, &hval);
flip = (pvalx2 >= 4) ? 1 : 0;
if (!strcmp(tmpstring, "FROM")) flip = 1 - flip;
(*newinst)->position.x = xmat(pvalx);
(*newinst)->position.y = ymat(pvaly);
(*newinst)->scale = 1.0;
(*newinst)->color = DEFAULTCOLOR;
(*newinst)->params = NULL;
(*newinst)->passed = NULL;
if (pvalx2 & 0x01) pvalx2 ^= 0x02;
if (pvalx2 >= 4) (*newinst)->rotation = -(((pvalx2 - 4) * 90) + 1);
else (*newinst)->rotation = (pvalx2 * 90) + 1;
(*newinst)->thisobject = *libobj;
(*newinst)->bbox.lowerleft.x = (*libobj)->bbox.lowerleft.x;
(*newinst)->bbox.lowerleft.y = (*libobj)->bbox.lowerleft.y;
(*newinst)->bbox.width = (*libobj)->bbox.width;
(*newinst)->bbox.height = (*libobj)->bbox.height;
/* Add label to "TO" and "FROM" */
if (!strcmp(tmpstring, "FROM") || !strcmp(tmpstring, "TO")) {
int nval;
hval--;
fgets(temp, 499, ps);
sscanf(temp, "%d", &nval);
for (k = 0; k < sigs; k++)
if (signets[k] == nval) {
stringpart *strptr;
NEW_LABEL(newlabel, topobject);
/* reconnect newinst if displaced by realloc() */
newinst = (objinstptr *)(topobject->plist
+ topobject->parts - 2);
labeldefaults(*newlabel, False, (*newinst)->position.x,
(*newinst)->position.y);
(*newlabel)->color = DEFAULTCOLOR;
(*newlabel)->pin = LOCAL;
(*newlabel)->color = LOCALPINCOLOR;
if (!strcmp(tmpstring, "TO"))
(*newlabel)->position.x += ((flip) ? 48 : -48);
else
(*newlabel)->position.x += ((flip) ? 54 : -54);
(*newlabel)->justify = NOTBOTTOM;
if (flip) (*newlabel)->justify |= (RIGHT | NOTLEFT);
(*newlabel)->string->data.font = 0;
strptr = makesegment(&((*newlabel)->string), NULL);
strptr->type = TEXT_STRING;
strptr->data.string = (char *)malloc(1 + strlen(signals[k]));
strcpy(strptr->data.string, signals[k]);
break;
}
}
}
/* read through list of attributes */
else {
sscanf(lineptr, "%*d %*d %*d %*d %*d %*d %d", &hval);
Wprintf("No library object %s", tmpstring);
}
for (j = 0; j < hval + 1; j++) {
if (fgets(temp, 499, ps) == NULL) {
Wprintf("Unexpected end of file");
return;
}
}
/* read to next blank line */
do {
if (fgets(temp, 499, ps) == NULL) break;
for (lineptr = temp; isspace(*lineptr) && *lineptr != '\n'; lineptr++);
} while (*lineptr != '\n');
}}
break;
case 'h': { /* history */
int j, hval;
sscanf(++lineptr, "%d", &dval);
for (i = 0; i < dval; i++) {
do {
if (fgets(temp, 499, ps) == NULL) {
Wprintf("End of file in history section");
return;
}
for (lineptr = temp; isspace(*lineptr) && *lineptr != '\n'; lineptr++);
} while (*lineptr == '\n');
/* read through history */
sscanf(lineptr, "%*d %d", &hval);
for (j = 0; j < hval; j++)
if (fgets(temp, 499, ps) == NULL) {
Wprintf("Unexpected end of file");
return;
}
}}
break;
case '.': /* blank, don't use for EOF */
break;
default:
Wprintf("Don't understand statement '%c'", *lineptr);
break;
}
}
/* check for unattached labels and delete them */
for (iolabel = topobject->plist; iolabel < topobject->plist +
topobject->parts; iolabel++)
if (IS_LABEL(*iolabel)) {
if (TOLABEL(iolabel)->rotation == 500) {
genericptr *tmplabel;
free(TOLABEL(iolabel)->string);
free(*iolabel);
for (tmplabel = iolabel + 1; tmplabel < topobject->plist +
topobject->parts; tmplabel++) *(tmplabel - 1) = *tmplabel;
topobject->parts--;
iolabel--;
}
}
calcbbox(areawin->topinstance);
centerview(areawin->topinstance);
for (i = 0; i < sigs; i++) free(signals[i]);
free(signals);
free(signets);
}
