void orthonormalize( Matrix& S, Matrix& u ) {
if (S.size1() == 0 || S.size2() == 0) return;
int i, j, k;
int n = S.size1();
Vector eval( n );
Matrix evec( n, n );
u.reset( n, n );
u.set(0);
// diagonalize S, and then calculate 1/sqrt(S).
diagonalize( S, evec, eval);
for(i=0; i<n; i++)
eval[i] = 1.0/sqrt(eval[i]);
double sum;
for(i=0; i<n; i++)
for(j=0; j<n; j++) {
sum = 0;
for(k=0; k<n; k++)
sum += evec(j,k)*eval[k]*evec(i,k);
u(i,j) = sum;
}
}
Hand * EigenHandLoader::loadHand(World * w)
{
// Get the graspit root directory to compose the palm's path
QString graspitRoot = getenv("GRASPIT");
// Load the base hand with just the palm
Hand * h = static_cast<Hand *>(w->importRobot(graspitRoot + "/models/robots/Eigenhand Lite Model VRML/EigenhandBase.xml"));
// Get the diagonalized palm scaling matrix
std::vector<mat3> output;
diagonalize(output, hd->palmScale);
// Scale the palm
RobotTools::scalePalm(output[0], h);
// Load the kinematic chains for the hand
for(unsigned int i = 0; i < fdList.size(); ++i)
{
// Add the chain from the description
h->addChain(loadKinematicChain(h, *fdList[i], i));
// Set the location of the finger around the base
setBaseJointPosition(h, i);
}
// Set up the eigengrasps for the hand
setupEigengrasps(h);
// Test that the default pose does not have collisions. If it does, this hand is not usable.
if (!testDefaultCollisions(h))
return NULL;
// Set the hand's name appropriately.
h->setName(QString::number(hd->handID));
// Return the finished hand.
return h;
}
int main (int argc, char * argv[]) {
Protein protein1, protein2;
Atom* atom[2];
double cm[2][3]= {{0.0}};
double I[3][3] = {{0.0}};
double principal_axis[2][3][3], principal_moment[2][3];
double **x, **y, **R; /* aux vectors fro finding rotations */
double ** rot_coord;
double q[4], T[3], rmsd;
char pdbname1 [BUFFLEN] = "\0";
char pdbname2 [BUFFLEN] = "\0";
int no_atoms[2] = {0};
int i,j,a,l;
double diagonalize (double I[3][3], double evec[3][3], double evals[3]);
int mappedCoords2rotation (double **x, double **y, int no_vectors,
double q[4], double **R, double T[3], double *rmsd);
if ( argc < 2 ) {
printf ( "Usage: %s <pdbname1> <pdbname2>\n", argv[0] );
exit (1);
}
sprintf ( pdbname1, "%s", argv[1]);
sprintf ( pdbname2, "%s", argv[2]);
/*********************************************/
/* read in the two structures */
if ( read_pdb ( pdbname1, &protein1, '\0')) exit (1);
if ( read_pdb ( pdbname2, &protein2, '\0')) exit (1);
if (0) {
printf ("read in %20s, %s, length %d, atoms %d\n",
pdbname1, protein1.sequence[0].res_type, protein1.length, protein1.sequence[0].no_atoms);
printf ("read in %20s, %s, length %d, atoms %d\n",
pdbname2, protein2.sequence[0].res_type, protein2.length, protein2.sequence[0].no_atoms);
}
/*********************************************/
/* calculate moments of inertia for both */
atom[0] = protein1.sequence[0].atom;
no_atoms[0] = protein1.sequence[0].no_atoms;
atom[1] = protein2.sequence[0].atom;
no_atoms[1] = protein2.sequence[0].no_atoms;
/* sanity: */
for (l=0; l<2; l++) {
if (! no_atoms[l] ) {
fprintf (stderr, "no atoms in %s(?)\n", pdbname1);
exit (1);
}
}
for (l=0; l<2; l++) {
/*cm*/
for (a=0; a<no_atoms[l]; a++) {
for (i=0; i<3; i++) cm[l][i] += atom[l][a].coord[i];
}
for (i=0; i<3; i++) cm[l][i] /= no_atoms[l];
/* points in cm */
for (a=0; a<no_atoms[l]; a++) {
for (i=0; i<3; i++) atom[l][a].coord[i] -= cm[l][i];
}
/* moment of inertia tensor */
memset (I[0], 0.0, 3*3*sizeof(double));
for (a=0; a<no_atoms[l]; a++) {
for (i=0; i<3; i++) {
double d1 = atom[l][a].coord[(i+1)%3];
double d2 = atom[l][a].coord[(i+2)%3];
I[i][i] += d1*d1 + d2*d2;
for (j=0; j<3; j++) {
I[i][j] -= atom[l][a].coord[i]*atom[l][a].coord[j];
}
}
}
diagonalize (I, principal_axis[l], principal_moment[l]);
}
if ( ! (x=dmatrix(3,no_atoms[0]) ) ) exit (1);
if ( ! (y=dmatrix(3,no_atoms[1]) ) ) exit (1);
for (i=0; i<3; i++) {
for (j=0; j<3; j++) {
x[j][i] = principal_axis[0][i][j];
y[j][i] = principal_axis[1][i][j];
}
}
if ( ! (R=dmatrix(3,3) ) ) return 1; /* compiler is bugging me otherwise */
mappedCoords2rotation (x, y, 3, q, R, T, &rmsd);
/*******************************************/
/* output the tfm matrix */
if ( 0 ) {
for (i=0; i<3; i++) {
for (j=0; j<3; j++) {
printf (" %8.4lf", R[i][j]);
}
printf (" %8.4lf\n", cm[0][i]);
}
}
/* rotate (the first structure), move back to the old cm,
and output */
if ( ! (rot_coord=dmatrix(no_atoms[0],3) ) ) exit (1);
for (a=0; a<no_atoms[0]; a++) {
for (i=0; i<3; i++) {
rot_coord[a][i] = 0.0;
for (j=0; j<3; j++) {
rot_coord[a][i] += R[i][j]*atom[0][a].coord[j] ;
}
}
/* actually, moe it to the cm of the *other* molecule */
for (i=0; i<3; i++) rot_coord[a][i] += cm[1][i];
/* 123456789012345678901234567890*/
printf ("HETATM%5d %2s %3s %1c %4d %8.3f%8.3f%8.3f \n",
a, atom[0][a].type, protein1.sequence[0].res_type, 'L', 1,
rot_coord[a][0], rot_coord[a][1], rot_coord[a][2]);
}
return 0;
}
enum calcd run_calc(const BasisSet & basis, Settings & set, bool force) {
// Checkpoint file to load
std::string loadname=set.get_string("LoadChk");
if(stricmp(loadname,"")==0) {
// Nothing to load - run full calculation.
calculate(basis,set,force);
if(force)
return FULLCALC;
else
return ECALC;
}
// Was the calculation converged?
bool convd;
BasisSet oldbas;
{
Checkpoint chkpt(loadname,false);
chkpt.read("Converged",convd);
chkpt.read(oldbas);
}
if(!convd) {
// Old calculation was not converged - run full calculation
set.set_string("LoadName","");
calculate(basis,set,force);
if(force)
return FULLCALC;
else
return ECALC;
}
// Copy the checkpoint if necessary
std::string savename=set.get_string("SaveChk");
if(stricmp(savename,loadname)!=0) {
// Copy the file
std::ostringstream oss;
oss << "cp " << loadname << " " << savename;
int cp=system(oss.str().c_str());
if(cp) {
ERROR_INFO();
throw std::runtime_error("Failed to copy checkpoint file.\n");
}
}
// Strict integrals?
bool strictint(set.get_bool("StrictIntegrals"));
// Check if the basis set is different
if(!(basis==oldbas)) {
// Basis set is different or calculation was not converged - need
// to run calculation. Project Fock matrix to new basis and
// diagonalize to get new orbitals
{
// Open in write mode, don't truncate
Checkpoint chkpt(savename,true,false);
// Save basis
chkpt.write(basis);
// Overlap matrix
arma::mat S=basis.overlap();
chkpt.write("S",S);
arma::mat Sinvh=BasOrth(S,set);
chkpt.write("Sinvh",Sinvh);
// Restricted or unrestricted run?
bool restr;
chkpt.read("Restricted",restr);
chkpt.read(oldbas);
if(restr) {
rscf_t sol;
std::vector<double> occs;
chkpt.read("H",sol.H);
chkpt.read("occs",occs);
// Form new orbitals by diagonalizing H in new geometry
diagonalize(S,Sinvh,sol);
// Form new density
sol.P=form_density(sol.C,occs);
// Write basis set, orbitals and density
chkpt.write("C",sol.C);
chkpt.write("E",sol.E);
chkpt.write("P",sol.P);
} else {
uscf_t sol;
std::vector<double> occa, occb;
chkpt.read("Ha",sol.Ha);
chkpt.read("Hb",sol.Hb);
chkpt.read("occa",occa);
chkpt.read("occb",occb);
// Form new orbitals by diagonalizing H in new geometry
diagonalize(S,Sinvh,sol);
// Form new density
sol.Pa=form_density(sol.Ca,occa);
sol.Pb=form_density(sol.Cb,occb);
sol.P=sol.Pa+sol.Pb;
// Write basis set, orbitals and density
chkpt.write("Ca",sol.Ca);
chkpt.write("Cb",sol.Cb);
chkpt.write("Ea",sol.Ea);
chkpt.write("Eb",sol.Eb);
chkpt.write("Pa",sol.Pa);
chkpt.write("Pb",sol.Pb);
chkpt.write("P",sol.P);
}
// Calculation is now not converged
bool conv=false;
chkpt.write("Converged",conv);
}
// Now we can do the SCF calculation
calculate(basis,set,force);
if(force)
return FULLCALC;
else
return ECALC;
}
// Because we are here, the basis set is the same.
if(!force)
// No force required, can just read energy from file.
return NOCALC;
// Have converged energy, compute force.
// Open the checkpoint in write mode, don't truncate it
Checkpoint chkpt(savename,true,false);
// SCF structure
SCF solver(basis,set,chkpt);
// Do a plain Hartree-Fock calculation?
bool hf= (stricmp(set.get_string("Method"),"HF")==0);
bool rohf=(stricmp(set.get_string("Method"),"ROHF")==0);
dft_t dft;
if(!hf && !rohf) {
// Get exchange and correlation functionals as well as grid settings
dft=parse_dft(set,false);
}
// Force
arma::vec f;
double tol;
chkpt.read("tol",tol);
// Multiplicity
int mult=set.get_int("Multiplicity");
// Amount of electrons
int Nel=basis.Ztot()-set.get_int("Charge");
if(mult==1 && Nel%2==0 && !set.get_bool("ForcePol")) {
rscf_t sol;
chkpt.read("C",sol.C);
chkpt.read("E",sol.E);
chkpt.read("H",sol.H);
chkpt.read("P",sol.P);
std::vector<double> occs;
chkpt.read("occs",occs);
// Restricted case
if(hf) {
f=solver.force_RHF(sol,occs,FINETOL);
} else {
DFTGrid grid(&basis,set.get_bool("Verbose"),dft.lobatto);
DFTGrid nlgrid(&basis,set.get_bool("Verbose"),dft.lobatto);
// Fixed grid?
if(dft.adaptive)
grid.construct(sol.P,dft.gridtol,dft.x_func,dft.c_func);
else {
grid.construct(dft.nrad,dft.lmax,dft.x_func,dft.c_func,strictint);
if(dft.nl)
nlgrid.construct(dft.nlnrad,dft.nllmax,true,false,strictint,true);
}
f=solver.force_RDFT(sol,occs,dft,grid,nlgrid,FINETOL);
}
} else {
uscf_t sol;
chkpt.read("Ca",sol.Ca);
chkpt.read("Ea",sol.Ea);
chkpt.read("Ha",sol.Ha);
chkpt.read("Pa",sol.Pa);
chkpt.read("Cb",sol.Cb);
chkpt.read("Eb",sol.Eb);
chkpt.read("Hb",sol.Hb);
chkpt.read("Pb",sol.Pb);
chkpt.read("P",sol.P);
std::vector<double> occa, occb;
chkpt.read("occa",occa);
chkpt.read("occb",occb);
if(hf) {
f=solver.force_UHF(sol,occa,occb,tol);
} else if(rohf) {
int Nela, Nelb;
chkpt.read("Nel-a",Nela);
chkpt.read("Nel-b",Nelb);
throw std::runtime_error("Not implemented!\n");
// f=solver.force_ROHF(sol,Nela,Nelb,tol);
} else {
DFTGrid grid(&basis,set.get_bool("Verbose"),dft.lobatto);
DFTGrid nlgrid(&basis,set.get_bool("Verbose"),dft.lobatto);
// Fixed grid?
if(dft.adaptive)
grid.construct(sol.P,dft.gridtol,dft.x_func,dft.c_func);
else {
grid.construct(dft.nrad,dft.lmax,dft.x_func,dft.c_func,strictint);
if(dft.nl)
nlgrid.construct(dft.nlnrad,dft.nllmax,true,false,strictint,true);
}
f=solver.force_UDFT(sol,occa,occb,dft,grid,nlgrid,tol);
}
}
chkpt.write("Force",f);
chkpt.close();
return FORCECALC;
}
int main(){
int i, j, k, test, mode, mode1;
double zA[1+Q][1+m], zB[1+Q][1+m], zavg[1+m];
double movesize, gradsize, dlnLsize, maxsize, temp;
double crit_move, crit_grad, crit_dlnL, x[1+m];
double mlnlold, dmlnLold[1+m], H[1+m][1+m], K[1+m], S[1+m][1+m];
double rc, a[1+m], aold[1+m], alpha0[1+m];
double mlnl, dmlnL[1+m], alnLmax[1+m+1+1];
FILE *convergence;
getdata(zA, zB);
for(j=0;j<=m;j=j+1){
zavg[j] = 0.0;
}
for(i=1;i<=zA[0][0];i=i+1){
for(j=1;j<=m;j=j+1){
zavg[j] = zavg[j] + zA[i][j];
}
}
for(j=1;j<=m;j=j+1){
zavg[j] = zavg[j]/((double)zA[0][0]);
alpha0[j] = 0.0;
}
crit_move = 0.001;
crit_grad = 0.01;
crit_dlnL = 0.0001;
maxsize = 0.1;
/***** SCREEN FOR STARTING VALUES *****/
if(m == 1){
printf("\n GET INITIAL VALUES BY RANDOM NUMBERS");
for(i=1;i<=16;i=i+1){
a[1] = randomf(-2.0, 2.0);
a[0] = -a[1]*zavg[j];
mlnl = mlnL(a, zA, zB);
if( mlnl < mlnL(alpha0, zA, zB) ){
for(j=0;j<=1;j=j+1){
alpha0[j] = a[j];
printf("\n improvement %lf a: %.3f %.3f",
mlnl, a[0], a[1]);
}
}
}
}else{
printf("\n GET INITIAL VALUES FROM BEST SUB-MODEL");
convergence = fopen("initial.txt", "r");
for(i=0;i<=m;i=i+1){
fscanf(convergence, "%lf", &temp);
alpha0[i] = temp;
}
fclose(convergence);
}
printf("\n initial a: ");
for(j=0;j<=m;j=j+1){
a[j] = alpha0[j];
printf(" %.3f ", a[j]);
for(k=0;k<=m;k=k+1){
H[j][k] = 0.0;
}
H[j][j] = 1.0 + randomf(0.0, 0.1);
}
convergence = fopen("convergence.txt", "w");
test = 0;
for(i=1;test==0;i=i+1){
if(i != 1){
mlnlold = mlnl;
for(j=0;j<=m;j=j+1){
dmlnLold[j] = dmlnL[j];
}
}
mlnl = grad(a, zA, zB, dmlnL);
printf("\n dmlnL = %f %f", dmlnL[0], dmlnL[1]);
if(i!=1){
BFGS_update(x, dmlnL, dmlnLold, H);
}
diagonalize(H, S, K);
dlnLsize = dmlnL - dmlnLold;
gradsize = sqrt(dot(dmlnL, dmlnL));
fprintf(convergence,"\n %d %f %f", i, gradsize, mlnl);
printf("\n\n gradients: %d mlnL: %f gradsize: %f", i, mlnl, gradsize);
fflush(convergence);
fflush(stdout);
for(j=0;j<=m;j=j+1){
x[j] = 0.0;
for(k=0;k<=m;k=k+1){
x[j] = x[j] - (dot(S[k],dmlnL)/K[k])*S[k][j];
}
}
movesize = sqrt(dot(x,x));
printf("\n RAW MOVESIZE/(B): %f", movesize);
if(movesize > maxsize){
for(j=0;j<=m;j=j+1){
x[j] = x[j]*maxsize/movesize;
}
}
printf("\n NEW MOVESIZE/(B): %f ALPHA: ", sqrt(dot(x,x)));
for(j=0;j<=m;j=j+1){
aold[j] = a[j];
a[j] = a[j] + x[j];
printf(" %.3f", a[j]);
}
printf("\n CONVERGENCE TESTING");
printf("\n movesize:tolerance %f::%f", movesize, crit_move);
printf("\n gradsize:tolerance %f::%f", gradsize, crit_grad);
printf("\n deltaE:|tolerance| %f::%f", dlnLsize, crit_dlnL);
if(movesize < crit_move && gradsize < crit_grad){
test = 1;
}
if(movesize < crit_move && fabs(dlnLsize) < crit_dlnL){
test = 1;
}
if(fabs(dlnLsize) < crit_dlnL && gradsize < crit_grad){
test = 1;
}
if(test == 1){
printf("\n CONVERGED ");
for(j=0;j<=m;j=j+1){
alnLmax[j] = a[j];
}
alnLmax[m+1] = -mlnl;
}
}
fclose(convergence);
convergence = fopen("answer.txt", "w");
fprintf(convergence, "%f ", alnLmax[m+1]);
for(j=0;j<=m;j=j+1){
fprintf(convergence, "%f ", alnLmax[j]);
}
fclose(convergence);
printf("\n");
return 0;
}
