Update Colvars to version 2018-05-15
[namd.git] / colvars / src / colvartypes.cpp
1 // -*- c++ -*-
2
3 // This file is part of the Collective Variables module (Colvars).
4 // The original version of Colvars and its updates are located at:
5 // https://github.com/colvars/colvars
6 // Please update all Colvars source files before making any changes.
7 // If you wish to distribute your changes, please submit them to the
8 // Colvars repository at GitHub.
9
10 #include <cstdlib>
11 #include <cstring>
12
13 #include "colvarmodule.h"
14 #include "colvartypes.h"
15 #include "colvarparse.h"
16
17
18 bool      colvarmodule::rotation::monitor_crossings = false;
19 cvm::real colvarmodule::rotation::crossing_threshold = 1.0E-02;
20
21
22 /// Numerical recipes diagonalization
23 static int jacobi(cvm::real **a, cvm::real *d, cvm::real **v, int *nrot);
24
25 /// Eigenvector sort
26 static int eigsrt(cvm::real *d, cvm::real **v);
27
28 /// Transpose the matrix
29 static int transpose(cvm::real **v);
30
31
32
33 std::string cvm::rvector::to_simple_string() const
34 {
35   std::ostringstream os;
36   os.setf(std::ios::scientific, std::ios::floatfield);
37   os.precision(cvm::cv_prec);
38   os << x << " " << y << " " << z;
39   return os.str();
40 }
41
42
43 int cvm::rvector::from_simple_string(std::string const &s)
44 {
45   std::stringstream stream(s);
46   if ( !(stream >> x) ||
47        !(stream >> y) ||
48        !(stream >> z) ) {
49     return COLVARS_ERROR;
50   }
51   return COLVARS_OK;
52 }
53
54
55 std::ostream & operator << (std::ostream &os, colvarmodule::rvector const &v)
56 {
57   std::streamsize const w = os.width();
58   std::streamsize const p = os.precision();
59
60   os.width(2);
61   os << "( ";
62   os.width(w); os.precision(p);
63   os << v.x << " , ";
64   os.width(w); os.precision(p);
65   os << v.y << " , ";
66   os.width(w); os.precision(p);
67   os << v.z << " )";
68   return os;
69 }
70
71
72 std::istream & operator >> (std::istream &is, colvarmodule::rvector &v)
73 {
74   size_t const start_pos = is.tellg();
75   char sep;
76   if ( !(is >> sep) || !(sep == '(') ||
77        !(is >> v.x) || !(is >> sep)  || !(sep == ',') ||
78        !(is >> v.y) || !(is >> sep)  || !(sep == ',') ||
79        !(is >> v.z) || !(is >> sep)  || !(sep == ')') ) {
80     is.clear();
81     is.seekg(start_pos, std::ios::beg);
82     is.setstate(std::ios::failbit);
83     return is;
84   }
85   return is;
86 }
87
88 std::string cvm::quaternion::to_simple_string() const
89 {
90   std::ostringstream os;
91   os.setf(std::ios::scientific, std::ios::floatfield);
92   os.precision(cvm::cv_prec);
93   os << q0 << " " << q1 << " " << q2 << " " << q3;
94   return os.str();
95 }
96
97 int cvm::quaternion::from_simple_string(std::string const &s)
98 {
99   std::stringstream stream(s);
100   if ( !(stream >> q0) ||
101        !(stream >> q1) ||
102        !(stream >> q2) ||
103        !(stream >> q3) ) {
104     return COLVARS_ERROR;
105   }
106   return COLVARS_OK;
107 }
108
109 std::ostream & operator << (std::ostream &os, colvarmodule::quaternion const &q)
110 {
111   std::streamsize const w = os.width();
112   std::streamsize const p = os.precision();
113
114   os.width(2);
115   os << "( ";
116   os.width(w); os.precision(p);
117   os << q.q0 << " , ";
118   os.width(w); os.precision(p);
119   os << q.q1 << " , ";
120   os.width(w); os.precision(p);
121   os << q.q2 << " , ";
122   os.width(w); os.precision(p);
123   os << q.q3 << " )";
124   return os;
125 }
126
127
128 std::istream & operator >> (std::istream &is, colvarmodule::quaternion &q)
129 {
130   size_t const start_pos = is.tellg();
131
132   std::string euler("");
133
134   if ( (is >> euler) && (colvarparse::to_lower_cppstr(euler) ==
135                          std::string("euler")) ) {
136
137     // parse the Euler angles
138
139     char sep;
140     cvm::real phi, theta, psi;
141     if ( !(is >> sep)   || !(sep == '(') ||
142          !(is >> phi)   || !(is >> sep)  || !(sep == ',') ||
143          !(is >> theta) || !(is >> sep)  || !(sep == ',') ||
144          !(is >> psi)   || !(is >> sep)  || !(sep == ')') ) {
145       is.clear();
146       is.seekg(start_pos, std::ios::beg);
147       is.setstate(std::ios::failbit);
148       return is;
149     }
150
151     q = colvarmodule::quaternion(phi, theta, psi);
152
153   } else {
154
155     // parse the quaternion components
156
157     is.seekg(start_pos, std::ios::beg);
158     char sep;
159     if ( !(is >> sep)  || !(sep == '(') ||
160          !(is >> q.q0) || !(is >> sep)  || !(sep == ',') ||
161          !(is >> q.q1) || !(is >> sep)  || !(sep == ',') ||
162          !(is >> q.q2) || !(is >> sep)  || !(sep == ',') ||
163          !(is >> q.q3) || !(is >> sep)  || !(sep == ')') ) {
164       is.clear();
165       is.seekg(start_pos, std::ios::beg);
166       is.setstate(std::ios::failbit);
167       return is;
168     }
169   }
170
171   return is;
172 }
173
174
175 cvm::quaternion
176 cvm::quaternion::position_derivative_inner(cvm::rvector const &pos,
177                                             cvm::rvector const &vec) const
178 {
179   cvm::quaternion result(0.0, 0.0, 0.0, 0.0);
180
181
182   result.q0 =   2.0 * pos.x * q0 * vec.x
183                +2.0 * pos.y * q0 * vec.y
184                +2.0 * pos.z * q0 * vec.z
185
186                -2.0 * pos.y * q3 * vec.x
187                +2.0 * pos.z * q2 * vec.x
188
189                +2.0 * pos.x * q3 * vec.y
190                -2.0 * pos.z * q1 * vec.y
191
192                -2.0 * pos.x * q2 * vec.z
193                +2.0 * pos.y * q1 * vec.z;
194
195
196   result.q1 =  +2.0 * pos.x * q1 * vec.x
197                -2.0 * pos.y * q1 * vec.y
198                -2.0 * pos.z * q1 * vec.z
199
200                +2.0 * pos.y * q2 * vec.x
201                +2.0 * pos.z * q3 * vec.x
202
203                +2.0 * pos.x * q2 * vec.y
204                -2.0 * pos.z * q0 * vec.y
205
206                +2.0 * pos.x * q3 * vec.z
207                +2.0 * pos.y * q0 * vec.z;
208
209
210   result.q2 =  -2.0 * pos.x * q2 * vec.x
211                +2.0 * pos.y * q2 * vec.y
212                -2.0 * pos.z * q2 * vec.z
213
214                +2.0 * pos.y * q1 * vec.x
215                +2.0 * pos.z * q0 * vec.x
216
217                +2.0 * pos.x * q1 * vec.y
218                +2.0 * pos.z * q3 * vec.y
219
220                -2.0 * pos.x * q0 * vec.z
221                +2.0 * pos.y * q3 * vec.z;
222
223
224   result.q3 =  -2.0 * pos.x * q3 * vec.x
225                -2.0 * pos.y * q3 * vec.y
226                +2.0 * pos.z * q3 * vec.z
227
228                -2.0 * pos.y * q0 * vec.x
229                +2.0 * pos.z * q1 * vec.x
230
231                +2.0 * pos.x * q0 * vec.y
232                +2.0 * pos.z * q2 * vec.y
233
234                +2.0 * pos.x * q1 * vec.z
235                +2.0 * pos.y * q2 * vec.z;
236
237   return result;
238 }
239
240
241
242
243 // Calculate the optimal rotation between two groups, and implement it
244 // as a quaternion.  Uses the method documented in: Coutsias EA,
245 // Seok C, Dill KA.  Using quaternions to calculate RMSD.  J Comput
246 // Chem. 25(15):1849-57 (2004) DOI: 10.1002/jcc.20110 PubMed: 15376254
247
248 void colvarmodule::rotation::build_matrix(std::vector<cvm::atom_pos> const &pos1,
249                                           std::vector<cvm::atom_pos> const &pos2,
250                                           cvm::matrix2d<cvm::real>         &S)
251 {
252   // build the correlation matrix
253   C.resize(3, 3);
254   C.reset();
255   size_t i;
256   for (i = 0; i < pos1.size(); i++) {
257     C.xx() += pos1[i].x * pos2[i].x;
258     C.xy() += pos1[i].x * pos2[i].y;
259     C.xz() += pos1[i].x * pos2[i].z;
260     C.yx() += pos1[i].y * pos2[i].x;
261     C.yy() += pos1[i].y * pos2[i].y;
262     C.yz() += pos1[i].y * pos2[i].z;
263     C.zx() += pos1[i].z * pos2[i].x;
264     C.zy() += pos1[i].z * pos2[i].y;
265     C.zz() += pos1[i].z * pos2[i].z;
266   }
267
268   // build the "overlap" matrix, whose eigenvectors are stationary
269   // points of the RMSD in the space of rotations
270   S[0][0] =    C.xx() + C.yy() + C.zz();
271   S[1][0] =    C.yz() - C.zy();
272   S[0][1] = S[1][0];
273   S[2][0] =  - C.xz() + C.zx() ;
274   S[0][2] = S[2][0];
275   S[3][0] =    C.xy() - C.yx();
276   S[0][3] = S[3][0];
277   S[1][1] =    C.xx() - C.yy() - C.zz();
278   S[2][1] =    C.xy() + C.yx();
279   S[1][2] = S[2][1];
280   S[3][1] =    C.xz() + C.zx();
281   S[1][3] = S[3][1];
282   S[2][2] = - C.xx() + C.yy() - C.zz();
283   S[3][2] =   C.yz() + C.zy();
284   S[2][3] = S[3][2];
285   S[3][3] = - C.xx() - C.yy() + C.zz();
286 }
287
288
289 void colvarmodule::rotation::diagonalize_matrix(cvm::matrix2d<cvm::real> &S,
290                                                 cvm::vector1d<cvm::real> &S_eigval,
291                                                 cvm::matrix2d<cvm::real> &S_eigvec)
292 {
293   S_eigval.resize(4);
294   S_eigval.reset();
295   S_eigvec.resize(4,4);
296   S_eigvec.reset();
297
298   // diagonalize
299   int jac_nrot = 0;
300   if (jacobi(S.c_array(), S_eigval.c_array(), S_eigvec.c_array(), &jac_nrot) !=
301       COLVARS_OK) {
302     cvm::error("Too many iterations in routine jacobi.\n"
303                "This is usually the result of an ill-defined set of atoms for "
304                "rotational alignment (RMSD, rotateReference, etc).\n");
305   }
306   eigsrt(S_eigval.c_array(), S_eigvec.c_array());
307   // jacobi saves eigenvectors by columns
308   transpose(S_eigvec.c_array());
309
310   // normalize eigenvectors
311   for (size_t ie = 0; ie < 4; ie++) {
312     cvm::real norm2 = 0.0;
313     size_t i;
314     for (i = 0; i < 4; i++) {
315       norm2 += S_eigvec[ie][i] * S_eigvec[ie][i];
316     }
317     cvm::real const norm = std::sqrt(norm2);
318     for (i = 0; i < 4; i++) {
319       S_eigvec[ie][i] /= norm;
320     }
321   }
322 }
323
324
325 // Calculate the rotation, plus its derivatives
326
327 void colvarmodule::rotation::calc_optimal_rotation(std::vector<cvm::atom_pos> const &pos1,
328                                                    std::vector<cvm::atom_pos> const &pos2)
329 {
330   S.resize(4,4);
331   S.reset();
332
333   build_matrix(pos1, pos2, S);
334
335   S_backup.resize(4,4);
336   S_backup = S;
337
338   if (b_debug_gradients) {
339     cvm::log("S     = "+cvm::to_str(cvm::to_str(S_backup), cvm::cv_width, cvm::cv_prec)+"\n");
340   }
341
342   diagonalize_matrix(S, S_eigval, S_eigvec);
343
344   // eigenvalues and eigenvectors
345   cvm::real const L0 = S_eigval[0];
346   cvm::real const L1 = S_eigval[1];
347   cvm::real const L2 = S_eigval[2];
348   cvm::real const L3 = S_eigval[3];
349   cvm::quaternion const Q0(S_eigvec[0]);
350   cvm::quaternion const Q1(S_eigvec[1]);
351   cvm::quaternion const Q2(S_eigvec[2]);
352   cvm::quaternion const Q3(S_eigvec[3]);
353
354   lambda = L0;
355   q = Q0;
356
357   if (cvm::rotation::monitor_crossings) {
358     if (q_old.norm2() > 0.0) {
359       q.match(q_old);
360       if (q_old.inner(q) < (1.0 - crossing_threshold)) {
361         cvm::log("Warning: one molecular orientation has changed by more than "+
362                  cvm::to_str(crossing_threshold)+": discontinuous rotation ?\n");
363       }
364     }
365     q_old = q;
366   }
367
368   if (b_debug_gradients) {
369     cvm::log("L0 = "+cvm::to_str(L0, cvm::cv_width, cvm::cv_prec)+
370              ", Q0 = "+cvm::to_str(Q0, cvm::cv_width, cvm::cv_prec)+
371              ", Q0*Q0 = "+cvm::to_str(Q0.inner(Q0), cvm::cv_width, cvm::cv_prec)+
372              "\n");
373     cvm::log("L1 = "+cvm::to_str(L1, cvm::cv_width, cvm::cv_prec)+
374              ", Q1 = "+cvm::to_str(Q1, cvm::cv_width, cvm::cv_prec)+
375              ", Q0*Q1 = "+cvm::to_str(Q0.inner(Q1), cvm::cv_width, cvm::cv_prec)+
376              "\n");
377     cvm::log("L2 = "+cvm::to_str(L2, cvm::cv_width, cvm::cv_prec)+
378              ", Q2 = "+cvm::to_str(Q2, cvm::cv_width, cvm::cv_prec)+
379              ", Q0*Q2 = "+cvm::to_str(Q0.inner(Q2), cvm::cv_width, cvm::cv_prec)+
380              "\n");
381     cvm::log("L3 = "+cvm::to_str(L3, cvm::cv_width, cvm::cv_prec)+
382              ", Q3 = "+cvm::to_str(Q3, cvm::cv_width, cvm::cv_prec)+
383              ", Q0*Q3 = "+cvm::to_str(Q0.inner(Q3), cvm::cv_width, cvm::cv_prec)+
384              "\n");
385   }
386
387   // calculate derivatives of L0 and Q0 with respect to each atom in
388   // either group; note: if dS_1 is a null vector, nothing will be
389   // calculated
390   size_t ia;
391   for (ia = 0; ia < dS_1.size(); ia++) {
392
393     cvm::real const &a2x = pos2[ia].x;
394     cvm::real const &a2y = pos2[ia].y;
395     cvm::real const &a2z = pos2[ia].z;
396
397     cvm::matrix2d<cvm::rvector> &ds_1 = dS_1[ia];
398
399     // derivative of the S matrix
400     ds_1.reset();
401     ds_1[0][0].set( a2x,  a2y,  a2z);
402     ds_1[1][0].set( 0.0,  a2z, -a2y);
403     ds_1[0][1] = ds_1[1][0];
404     ds_1[2][0].set(-a2z,  0.0,  a2x);
405     ds_1[0][2] = ds_1[2][0];
406     ds_1[3][0].set( a2y, -a2x,  0.0);
407     ds_1[0][3] = ds_1[3][0];
408     ds_1[1][1].set( a2x, -a2y, -a2z);
409     ds_1[2][1].set( a2y,  a2x,  0.0);
410     ds_1[1][2] = ds_1[2][1];
411     ds_1[3][1].set( a2z,  0.0,  a2x);
412     ds_1[1][3] = ds_1[3][1];
413     ds_1[2][2].set(-a2x,  a2y, -a2z);
414     ds_1[3][2].set( 0.0,  a2z,  a2y);
415     ds_1[2][3] = ds_1[3][2];
416     ds_1[3][3].set(-a2x, -a2y,  a2z);
417
418     cvm::rvector                &dl0_1 = dL0_1[ia];
419     cvm::vector1d<cvm::rvector> &dq0_1 = dQ0_1[ia];
420
421     // matrix multiplications; derivatives of L_0 and Q_0 are
422     // calculated using Hellmann-Feynman theorem (i.e. exploiting the
423     // fact that the eigenvectors Q_i form an orthonormal basis)
424
425     dl0_1.reset();
426     for (size_t i = 0; i < 4; i++) {
427       for (size_t j = 0; j < 4; j++) {
428         dl0_1 += Q0[i] * ds_1[i][j] * Q0[j];
429       }
430     }
431
432     dq0_1.reset();
433     for (size_t p = 0; p < 4; p++) {
434       for (size_t i = 0; i < 4; i++) {
435         for (size_t j = 0; j < 4; j++) {
436           dq0_1[p] +=
437             (Q1[i] * ds_1[i][j] * Q0[j]) / (L0-L1) * Q1[p] +
438             (Q2[i] * ds_1[i][j] * Q0[j]) / (L0-L2) * Q2[p] +
439             (Q3[i] * ds_1[i][j] * Q0[j]) / (L0-L3) * Q3[p];
440         }
441       }
442     }
443   }
444
445   // do the same for the second group
446   for (ia = 0; ia < dS_2.size(); ia++) {
447
448     cvm::real const &a1x = pos1[ia].x;
449     cvm::real const &a1y = pos1[ia].y;
450     cvm::real const &a1z = pos1[ia].z;
451
452     cvm::matrix2d<cvm::rvector> &ds_2 = dS_2[ia];
453
454     ds_2.reset();
455     ds_2[0][0].set( a1x,  a1y,  a1z);
456     ds_2[1][0].set( 0.0, -a1z,  a1y);
457     ds_2[0][1] = ds_2[1][0];
458     ds_2[2][0].set( a1z,  0.0, -a1x);
459     ds_2[0][2] = ds_2[2][0];
460     ds_2[3][0].set(-a1y,  a1x,  0.0);
461     ds_2[0][3] = ds_2[3][0];
462     ds_2[1][1].set( a1x, -a1y, -a1z);
463     ds_2[2][1].set( a1y,  a1x,  0.0);
464     ds_2[1][2] = ds_2[2][1];
465     ds_2[3][1].set( a1z,  0.0,  a1x);
466     ds_2[1][3] = ds_2[3][1];
467     ds_2[2][2].set(-a1x,  a1y, -a1z);
468     ds_2[3][2].set( 0.0,  a1z,  a1y);
469     ds_2[2][3] = ds_2[3][2];
470     ds_2[3][3].set(-a1x, -a1y,  a1z);
471
472     cvm::rvector                &dl0_2 = dL0_2[ia];
473     cvm::vector1d<cvm::rvector> &dq0_2 = dQ0_2[ia];
474
475     dl0_2.reset();
476     for (size_t i = 0; i < 4; i++) {
477       for (size_t j = 0; j < 4; j++) {
478         dl0_2 += Q0[i] * ds_2[i][j] * Q0[j];
479       }
480     }
481
482     dq0_2.reset();
483     for (size_t p = 0; p < 4; p++) {
484       for (size_t i = 0; i < 4; i++) {
485         for (size_t j = 0; j < 4; j++) {
486           dq0_2[p] +=
487             (Q1[i] * ds_2[i][j] * Q0[j]) / (L0-L1) * Q1[p] +
488             (Q2[i] * ds_2[i][j] * Q0[j]) / (L0-L2) * Q2[p] +
489             (Q3[i] * ds_2[i][j] * Q0[j]) / (L0-L3) * Q3[p];
490         }
491       }
492     }
493
494     if (b_debug_gradients) {
495
496       cvm::matrix2d<cvm::real> S_new(4, 4);
497       cvm::vector1d<cvm::real> S_new_eigval(4);
498       cvm::matrix2d<cvm::real> S_new_eigvec(4, 4);
499
500       // make an infitesimal move along each cartesian coordinate of
501       // this atom, and solve again the eigenvector problem
502       for (size_t comp = 0; comp < 3; comp++) {
503
504         S_new = S_backup;
505         // diagonalize the new overlap matrix
506         for (size_t i = 0; i < 4; i++) {
507           for (size_t j = 0; j < 4; j++) {
508             S_new[i][j] +=
509               colvarmodule::debug_gradients_step_size * ds_2[i][j][comp];
510           }
511         }
512
513         //           cvm::log("S_new = "+cvm::to_str(cvm::to_str (S_new), cvm::cv_width, cvm::cv_prec)+"\n");
514
515         diagonalize_matrix(S_new, S_new_eigval, S_new_eigvec);
516
517         cvm::real const &L0_new = S_new_eigval[0];
518         cvm::quaternion const Q0_new(S_new_eigvec[0]);
519
520         cvm::real const DL0 = (dl0_2[comp]) * colvarmodule::debug_gradients_step_size;
521         cvm::quaternion const DQ0(dq0_2[0][comp] * colvarmodule::debug_gradients_step_size,
522                                   dq0_2[1][comp] * colvarmodule::debug_gradients_step_size,
523                                   dq0_2[2][comp] * colvarmodule::debug_gradients_step_size,
524                                   dq0_2[3][comp] * colvarmodule::debug_gradients_step_size);
525
526         cvm::log(  "|(l_0+dl_0) - l_0^new|/l_0 = "+
527                    cvm::to_str(std::fabs(L0+DL0 - L0_new)/L0, cvm::cv_width, cvm::cv_prec)+
528                    ", |(q_0+dq_0) - q_0^new| = "+
529                    cvm::to_str((Q0+DQ0 - Q0_new).norm(), cvm::cv_width, cvm::cv_prec)+
530                    "\n");
531       }
532     }
533   }
534 }
535
536
537
538 // Numerical Recipes routine for diagonalization
539
540 #define ROTATE(a,i,j,k,l) g=a[i][j]; \
541   h=a[k][l];                         \
542   a[i][j]=g-s*(h+g*tau);             \
543   a[k][l]=h+s*(g-h*tau);
544
545 #define n 4
546
547 int jacobi(cvm::real **a, cvm::real *d, cvm::real **v, int *nrot)
548 {
549   int j,iq,ip,i;
550   cvm::real tresh,theta,tau,t,sm,s,h,g,c;
551
552   cvm::vector1d<cvm::real> b(n);
553   cvm::vector1d<cvm::real> z(n);
554
555   for (ip=0;ip<n;ip++) {
556     for (iq=0;iq<n;iq++) {
557       v[ip][iq]=0.0;
558     }
559     v[ip][ip]=1.0;
560   }
561   for (ip=0;ip<n;ip++) {
562     b[ip]=d[ip]=a[ip][ip];
563     z[ip]=0.0;
564   }
565   *nrot=0;
566   for (i=0;i<=50;i++) {
567     sm=0.0;
568     for (ip=0;ip<n-1;ip++) {
569       for (iq=ip+1;iq<n;iq++)
570         sm += std::fabs(a[ip][iq]);
571     }
572     if (sm == 0.0) {
573       return COLVARS_OK;
574     }
575     if (i < 4)
576       tresh=0.2*sm/(n*n);
577     else
578       tresh=0.0;
579     for (ip=0;ip<n-1;ip++) {
580       for (iq=ip+1;iq<n;iq++) {
581         g=100.0*std::fabs(a[ip][iq]);
582         if (i > 4 && (cvm::real)(std::fabs(d[ip])+g) == (cvm::real)std::fabs(d[ip])
583             && (cvm::real)(std::fabs(d[iq])+g) == (cvm::real)std::fabs(d[iq]))
584           a[ip][iq]=0.0;
585         else if (std::fabs(a[ip][iq]) > tresh) {
586           h=d[iq]-d[ip];
587           if ((cvm::real)(std::fabs(h)+g) == (cvm::real)std::fabs(h))
588             t=(a[ip][iq])/h;
589           else {
590             theta=0.5*h/(a[ip][iq]);
591             t=1.0/(std::fabs(theta)+std::sqrt(1.0+theta*theta));
592             if (theta < 0.0) t = -t;
593           }
594           c=1.0/std::sqrt(1+t*t);
595           s=t*c;
596           tau=s/(1.0+c);
597           h=t*a[ip][iq];
598           z[ip] -= h;
599           z[iq] += h;
600           d[ip] -= h;
601           d[iq] += h;
602           a[ip][iq]=0.0;
603           for (j=0;j<=ip-1;j++) {
604             ROTATE(a,j,ip,j,iq)
605               }
606           for (j=ip+1;j<=iq-1;j++) {
607             ROTATE(a,ip,j,j,iq)
608               }
609           for (j=iq+1;j<n;j++) {
610             ROTATE(a,ip,j,iq,j)
611               }
612           for (j=0;j<n;j++) {
613             ROTATE(v,j,ip,j,iq)
614               }
615           ++(*nrot);
616         }
617       }
618     }
619     for (ip=0;ip<n;ip++) {
620       b[ip] += z[ip];
621       d[ip]=b[ip];
622       z[ip]=0.0;
623     }
624   }
625   return COLVARS_ERROR;
626 }
627
628
629 int eigsrt(cvm::real *d, cvm::real **v)
630 {
631   int k,j,i;
632   cvm::real p;
633
634   for (i=0;i<n;i++) {
635     p=d[k=i];
636     for (j=i+1;j<n;j++)
637       if (d[j] >= p) p=d[k=j];
638     if (k != i) {
639       d[k]=d[i];
640       d[i]=p;
641       for (j=0;j<n;j++) {
642         p=v[j][i];
643         v[j][i]=v[j][k];
644         v[j][k]=p;
645       }
646     }
647   }
648   return COLVARS_OK;
649 }
650
651
652 int transpose(cvm::real **v)
653 {
654   cvm::real p;
655   int i,j;
656   for (i=0;i<n;i++) {
657     for (j=i+1;j<n;j++) {
658       p=v[i][j];
659       v[i][j]=v[j][i];
660       v[j][i]=p;
661     }
662   }
663   return COLVARS_OK;
664 }
665
666 #undef n
667 #undef ROTATE