ReferenceCCMAAlgorithm.cpp 21.4 KB
Newer Older
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30

/* Portions copyright (c) 2006-2009 Stanford University and Simbios.
 * Contributors: Peter Eastman, Pande Group
 *
 * Permission is hereby granted, free of charge, to any person obtaining
 * a copy of this software and associated documentation files (the
 * "Software"), to deal in the Software without restriction, including
 * without limitation the rights to use, copy, modify, merge, publish,
 * distribute, sublicense, and/or sell copies of the Software, and to
 * permit persons to whom the Software is furnished to do so, subject
 * to the following conditions:
 *
 * The above copyright notice and this permission notice shall be included
 * in all copies or substantial portions of the Software.
 *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS
 * OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF
 * MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT.
 * IN NO EVENT SHALL THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE
 * LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION
 * OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION
 * WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE.
 */

#include <string.h>
#include <sstream>

#include "../SimTKUtilities/SimTKOpenMMCommon.h"
#include "../SimTKUtilities/SimTKOpenMMUtilities.h"
#include "../SimTKUtilities/SimTKOpenMMLog.h"
31
#include "ReferenceCCMAAlgorithm.h"
32
#include "ReferenceDynamics.h"
33
#include "quern.h"
34
35
36
37
#include "openmm/Vec3.h"
#include <map>

using std::map;
38
using std::pair;
39
40
41
using std::vector;
using std::set;
using OpenMM::Vec3;
42
using OpenMM::RealVec;
43
44
45

/**---------------------------------------------------------------------------------------

46
   ReferenceCCMAAlgorithm constructor
47

48
49
50
51
52
53
54
         @param numberOfAtoms    number of atoms
         @param numberOfConstraints      number of constraints
         @param atomIndices              atom indices for contraints
         @param distance                 distances for constraints
         @param masses                   atom masses
         @param angles                   angle force field terms
         @param tolerance                constraint tolerance
55
56
57

   --------------------------------------------------------------------------------------- */

58
ReferenceCCMAAlgorithm::ReferenceCCMAAlgorithm( int numberOfAtoms,
59
                                                  int numberOfConstraints,
60
61
                                                  const vector<pair<int, int> >& atomIndices,
                                                  const vector<RealOpenMM>& distance,
62
                                                  vector<RealOpenMM>& masses,
63
                                                  vector<AngleInfo>& angles,
64
65
66
67
                                                  RealOpenMM tolerance){

   // ---------------------------------------------------------------------------------------

68
   // static const char* methodName = "\nReferenceCCMAAlgorithm::ReferenceCCMAAlgorithm";
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90

   static const RealOpenMM zero        =  0.0;
   static const RealOpenMM one         =  1.0;
   static const RealOpenMM two         =  2.0;
   static const RealOpenMM three       =  3.0;
   static const RealOpenMM oneM        = -1.0;

   static const int threeI             =  3;

   // ---------------------------------------------------------------------------------------

   _numberOfConstraints        = numberOfConstraints;
   _atomIndices                = atomIndices;
   _distance                   = distance;

   _maximumNumberOfIterations  = 150;
   _tolerance                  = tolerance;
   _hasInitializedMasses       = false;

   // work arrays

   if (_numberOfConstraints > 0) {
91
       _r_ij.resize(numberOfConstraints);
92
93
94
95
       _d_ij2                      = SimTKOpenMMUtilities::allocateOneDRealOpenMMArray( numberOfConstraints, NULL, 1, zero, "dij_2" );
       _distanceTolerance          = SimTKOpenMMUtilities::allocateOneDRealOpenMMArray( numberOfConstraints, NULL, 1, zero, "distanceTolerance" );
       _reducedMasses              = SimTKOpenMMUtilities::allocateOneDRealOpenMMArray( numberOfConstraints, NULL, 1, zero, "reducedMasses" );
   }
96
97
98
99
100
101
102
103
104
105
106
107
108
   if (numberOfConstraints > 0)
   {
       // Compute the constraint coupling matrix

       vector<vector<int> > atomAngles(numberOfAtoms);
       for (int i = 0; i < (int) angles.size(); i++)
           atomAngles[angles[i].atom2].push_back(i);
       vector<vector<pair<int, double> > > matrix(numberOfConstraints);
       for (int j = 0; j < numberOfConstraints; j++) {
           for (int k = 0; k < numberOfConstraints; k++) {
               if (j == k) {
                   matrix[j].push_back(pair<int, double>(j, 1.0));
                   continue;
109
               }
110
               double scale;
111
112
113
114
               int atomj0 = _atomIndices[j].first;
               int atomj1 = _atomIndices[j].second;
               int atomk0 = _atomIndices[k].first;
               int atomk1 = _atomIndices[k].second;
115
116
               RealOpenMM invMass0 = one/masses[atomj0];
               RealOpenMM invMass1 = one/masses[atomj1];
117
               int atoma, atomb, atomc;
118
119
               if (atomj0 == atomk0) {
                   atoma = atomj1;
120
121
                   atomb = atomj0;
                   atomc = atomk1;
122
123
124
125
                   scale = invMass0/(invMass0+invMass1);
               }
               else if (atomj1 == atomk1) {
                   atoma = atomj0;
126
127
                   atomb = atomj1;
                   atomc = atomk0;
128
129
130
131
                   scale = invMass1/(invMass0+invMass1);
               }
               else if (atomj0 == atomk1) {
                   atoma = atomj1;
132
133
                   atomb = atomj0;
                   atomc = atomk0;
134
135
136
137
                   scale = invMass0/(invMass0+invMass1);
               }
               else if (atomj1 == atomk0) {
                   atoma = atomj0;
138
139
                   atomb = atomj1;
                   atomc = atomk1;
140
141
                   scale = invMass1/(invMass0+invMass1);
               }
142
               else
143
144
                   continue; // These constraints are not connected.

145
               // Look for a third constraint forming a triangle with these two.
146

147
148
               bool foundConstraint = false;
               for (int other = 0; other < numberOfConstraints; other++) {
149
                   if ((_atomIndices[other].first == atoma && _atomIndices[other].second == atomc) || (_atomIndices[other].first == atomc && _atomIndices[other].second == atoma)) {
150
151
                       double d1 = _distance[j];
                       double d2 = _distance[k];
152
                       double d3 = _distance[other];
153
154
                       matrix[j].push_back(pair<int, double>(k, scale*(d1*d1+d2*d2-d3*d3)/(2.0*d1*d2)));
                       foundConstraint = true;
155
156
157
                       break;
                   }
               }
158
159
160
161
162
163
164
165
166
167
168
169
               if (!foundConstraint) {
                   // We didn't find one, so look for an angle force field term.

                   const vector<int>& angleCandidates = atomAngles[atomb];
                   for (vector<int>::const_iterator iter = angleCandidates.begin(); iter != angleCandidates.end(); iter++) {
                       const AngleInfo& angle = angles[*iter];
                       if ((angle.atom1 == atoma && angle.atom3 == atomc) || (angle.atom3 == atoma && angle.atom1 == atomc)) {
                           matrix[j].push_back(pair<int, double>(k, scale*cos(angle.angle)));
                           break;
                       }
                   }
               }
170
171
172
           }
       }

173
174
175
176
177
178
179
180
181
182
183
       // Invert it using QR.

       vector<int> matrixRowStart;
       vector<int> matrixColIndex;
       vector<double> matrixValue;
       for (int i = 0; i < numberOfConstraints; i++) {
           matrixRowStart.push_back(matrixValue.size());
           for (int j = 0; j < (int) matrix[i].size(); j++) {
               pair<int, double> element = matrix[i][j];
               matrixColIndex.push_back(element.first);
               matrixValue.push_back(element.second);
184
185
           }
       }
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
       matrixRowStart.push_back(matrixValue.size());
       int *qRowStart, *qColIndex, *rRowStart, *rColIndex;
       double *qValue, *rValue;
       QUERN_compute_qr(numberOfConstraints, numberOfConstraints, &matrixRowStart[0], &matrixColIndex[0], &matrixValue[0], NULL,
               &qRowStart, &qColIndex, &qValue, &rRowStart, &rColIndex, &rValue);
       vector<double> rhs(numberOfConstraints);
       _matrix.resize(numberOfConstraints);
       for (int i = 0; i < numberOfConstraints; i++) {
           // Extract column i of the inverse matrix.

           for (int j = 0; j < numberOfConstraints; j++)
               rhs[j] = (i == j ? 1.0 : 0.0);
           QUERN_multiply_with_q_transpose(numberOfConstraints, qRowStart, qColIndex, qValue, &rhs[0]);
           QUERN_solve_with_r(numberOfConstraints, rRowStart, rColIndex, rValue, &rhs[0], &rhs[0]);
           for (int j = 0; j < numberOfConstraints; j++) {
               double value = rhs[j]*_distance[i]/_distance[j];
202
               if (FABS((RealOpenMM)value) > 0.02)
203
204
205
206
207
208
                   _matrix[j].push_back(pair<int, RealOpenMM>(i, (RealOpenMM) value));
           }
       }
       QUERN_free_result(qRowStart, qColIndex, qValue);
       QUERN_free_result(rRowStart, rColIndex, rValue);
   }
209
210
211
212
}

/**---------------------------------------------------------------------------------------

213
   ReferenceCCMAAlgorithm destructor
214
215
216

   --------------------------------------------------------------------------------------- */

217
ReferenceCCMAAlgorithm::~ReferenceCCMAAlgorithm( ){
218
219
220

   // ---------------------------------------------------------------------------------------

221
   // static const char* methodName = "\nReferenceCCMAAlgorithm::~ReferenceCCMAAlgorithm";
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239

   // ---------------------------------------------------------------------------------------

    if (_numberOfConstraints > 0) {
       SimTKOpenMMUtilities::freeOneDRealOpenMMArray( _d_ij2, "d_ij2" );
       SimTKOpenMMUtilities::freeOneDRealOpenMMArray( _distanceTolerance, "distanceTolerance" );
       SimTKOpenMMUtilities::freeOneDRealOpenMMArray( _reducedMasses, "reducedMasses" );
    }
}

/**---------------------------------------------------------------------------------------

   Get number of constraints

   @return number of constraints

   --------------------------------------------------------------------------------------- */

240
int ReferenceCCMAAlgorithm::getNumberOfConstraints( void ) const {
241
242
243

   // ---------------------------------------------------------------------------------------

244
   // static const char* methodName = "\nReferenceCCMAAlgorithm::getNumberOfConstraints";
245
246
247
248
249
250
251
252
253
254
255
256
257
258

   // ---------------------------------------------------------------------------------------

   return _numberOfConstraints;
}

/**---------------------------------------------------------------------------------------

   Get maximum number of iterations

   @return maximum number of iterations

   --------------------------------------------------------------------------------------- */

259
int ReferenceCCMAAlgorithm::getMaximumNumberOfIterations( void ) const {
260
261
262

   // ---------------------------------------------------------------------------------------

263
   // static const char* methodName = "\nReferenceCCMAAlgorithm::getMaximumNumberOfIterations";
264
265
266
267
268
269
270
271
272
273
274
275
276
277

   // ---------------------------------------------------------------------------------------

   return _maximumNumberOfIterations;
}

/**---------------------------------------------------------------------------------------

   Set maximum number of iterations

   @param maximumNumberOfIterations   new maximum number of iterations

   --------------------------------------------------------------------------------------- */

278
void ReferenceCCMAAlgorithm::setMaximumNumberOfIterations( int maximumNumberOfIterations ){
279
280
281

   // ---------------------------------------------------------------------------------------

282
   // static const char* methodName = "\nReferenceCCMAAlgorithm::setMaximumNumberOfIterations";
283
284
285
286
287
288
289
290
291
292
293
294
295
296

   // ---------------------------------------------------------------------------------------

   _maximumNumberOfIterations = maximumNumberOfIterations;
}

/**---------------------------------------------------------------------------------------

   Get tolerance

   @return tolerance

   --------------------------------------------------------------------------------------- */

297
RealOpenMM ReferenceCCMAAlgorithm::getTolerance( void ) const {
298
299
300

   // ---------------------------------------------------------------------------------------

301
   // static const char* methodName = "\nReferenceCCMAAlgorithm::getTolerance";
302
303
304
305
306
307
308
309
310
311
312
313
314
315

   // ---------------------------------------------------------------------------------------

   return _tolerance;
}

/**---------------------------------------------------------------------------------------

   Set tolerance

   @param tolerance new tolerance

   --------------------------------------------------------------------------------------- */

316
void ReferenceCCMAAlgorithm::setTolerance( RealOpenMM tolerance ){
317
318
319
320


   // ---------------------------------------------------------------------------------------

321
   // static const char* methodName = "\nReferenceCCMAAlgorithm::setTolerance";
322
323
324
325
326
327
328
329

   // ---------------------------------------------------------------------------------------

   _tolerance = tolerance;;
}

/**---------------------------------------------------------------------------------------

330
   Apply CCMA algorithm
331
332
333
334
335
336

   @param numberOfAtoms    number of atoms
   @param atomCoordinates  atom coordinates
   @param atomCoordinatesP atom coordinates prime
   @param inverseMasses    1/mass

337
338
   @return SimTKOpenMMCommon::DefaultReturn if converge; else
    return SimTKOpenMMCommon::ErrorReturn
339
340
341

   --------------------------------------------------------------------------------------- */

342
343
344
int ReferenceCCMAAlgorithm::apply( int numberOfAtoms, vector<RealVec>& atomCoordinates,
                                         vector<RealVec>& atomCoordinatesP,
                                         vector<RealOpenMM>& inverseMasses ){
345
    return applyConstraints(numberOfAtoms, atomCoordinates, atomCoordinatesP, inverseMasses, false);
346
347
348
349
350
}

/**---------------------------------------------------------------------------------------

   Apply constraint algorithm to velocities.
351

352
353
354
355
356
357
358
359
360
361
362
363
   @param numberOfAtoms    number of atoms
   @param atomCoordinates  atom coordinates
   @param velocities       atom velocities
   @param inverseMasses    1/mass

   @return SimTKOpenMMCommon::DefaultReturn if converge; else
    return SimTKOpenMMCommon::ErrorReturn

   --------------------------------------------------------------------------------------- */

int ReferenceCCMAAlgorithm::applyToVelocities(int numberOfAtoms, std::vector<OpenMM::RealVec>& atomCoordinates,
               std::vector<OpenMM::RealVec>& velocities, std::vector<RealOpenMM>& inverseMasses) {
364
    return applyConstraints(numberOfAtoms, atomCoordinates, velocities, inverseMasses, true);
365
366
367
368
369
}

int ReferenceCCMAAlgorithm::applyConstraints(int numberOfAtoms, vector<RealVec>& atomCoordinates,
                                         vector<RealVec>& atomCoordinatesP,
                                         vector<RealOpenMM>& inverseMasses, bool constrainingVelocities){
370
371
   // ---------------------------------------------------------------------------------------

372
   static const char* methodName = "\nReferenceCCMAAlgorithm::apply";
373
374
375
376
377
378
379
380
381
382
383
384

   static const RealOpenMM zero        =  0.0;
   static const RealOpenMM one         =  1.0;
   static const RealOpenMM two         =  2.0;
   static const RealOpenMM half        =  0.5;

   static const RealOpenMM epsilon6    = (RealOpenMM) 1.0e-06;

   // ---------------------------------------------------------------------------------------

   // temp arrays

385
   vector<RealVec>& r_ij               = _r_ij;
386
387
388
389
390
391
392
393
   RealOpenMM* d_ij2                   = _d_ij2;
   RealOpenMM* reducedMasses           = _reducedMasses;

   // calculate reduced masses on 1st pass

   if( !_hasInitializedMasses ){
      _hasInitializedMasses = true;
      for( int ii = 0; ii < _numberOfConstraints; ii++ ){
394
395
         int atomI          = _atomIndices[ii].first;
         int atomJ          = _atomIndices[ii].second;
396
397
398
399
400
401
402
403
404
         reducedMasses[ii]  = half/( inverseMasses[atomI] + inverseMasses[atomJ] );
      }
   }

   // setup: r_ij for each (i,j) constraint

   RealOpenMM tolerance     = getTolerance();
              tolerance    *= two;
   for( int ii = 0; ii < _numberOfConstraints; ii++ ){
405
406
      int atomI   = _atomIndices[ii].first;
      int atomJ   = _atomIndices[ii].second;
407
408
      r_ij[ii] = atomCoordinates[atomI] - atomCoordinates[atomJ];
      d_ij2[ii] = r_ij[ii].dot(r_ij[ii]);
409
410
411
412
413
414
415
416
   }
   RealOpenMM lowerTol = one-two*getTolerance()+getTolerance()*getTolerance();
   RealOpenMM upperTol = one+two*getTolerance()+getTolerance()*getTolerance();

   // main loop

   int iterations           = 0;
   int numberConverged      = 0;
417
418
   vector<RealOpenMM> constraintDelta(_numberOfConstraints);
   vector<RealOpenMM> tempDelta(_numberOfConstraints);
419
   while (iterations < getMaximumNumberOfIterations()) {
420
421
422
      numberConverged  = 0;
      for( int ii = 0; ii < _numberOfConstraints; ii++ ){

423
424
         int atomI   = _atomIndices[ii].first;
         int atomJ   = _atomIndices[ii].second;
425

426
427
428
429
         RealVec rp_ij = atomCoordinatesP[atomI] - atomCoordinatesP[atomJ];
         if (constrainingVelocities) {
             RealOpenMM rrpr = rp_ij.dot(r_ij[ii]);
             constraintDelta[ii] = -2*reducedMasses[ii]*rrpr/d_ij2[ii];
430
             if (fabs(constraintDelta[ii]) <= getTolerance())
431
                numberConverged++;
432
         }
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
         else {
             RealOpenMM rp2  = rp_ij.dot(rp_ij);
             RealOpenMM dist2 = _distance[ii]*_distance[ii];
             RealOpenMM diff = dist2 - rp2;
             constraintDelta[ii] = zero;
             RealOpenMM rrpr = DOT3(  rp_ij, r_ij[ii] );
             if( rrpr <  d_ij2[ii]*epsilon6 ){
                 std::stringstream message;
                 message << iterations <<" "<<atomI<<" "<<atomJ<< " Error: sign of rrpr < 0?\n";
                 SimTKOpenMMLog::printMessage( message );
             } else {
                 constraintDelta[ii] = reducedMasses[ii]*diff/rrpr;
             }
             if (rp2 >= lowerTol*dist2 && rp2 <= upperTol*dist2)
                numberConverged++;
448
449
         }
      }
450
451
452
      if( numberConverged == _numberOfConstraints )
         break;
      iterations++;
453

454
455
456
457
458
      if (_matrix.size() > 0) {
          for (int i = 0; i < _numberOfConstraints; i++) {
              RealOpenMM sum = 0.0;
              for (int j = 0; j < (int) _matrix[i].size(); j++) {
                  pair<int, RealOpenMM> element = _matrix[i][j];
459
                  sum += element.second*constraintDelta[element.first];
460
              }
461
              tempDelta[i] = sum;
462
          }
463
          constraintDelta = tempDelta;
464
465
466
      }
      for( int ii = 0; ii < _numberOfConstraints; ii++ ){

467
468
         int atomI   = _atomIndices[ii].first;
         int atomJ   = _atomIndices[ii].second;
469
470
471
         RealVec dr = r_ij[ii]*constraintDelta[ii];
         atomCoordinatesP[atomI] += dr*inverseMasses[atomI];
         atomCoordinatesP[atomJ] -= dr*inverseMasses[atomJ];
472
473
474
      }
   }

475
   return (numberConverged == _numberOfConstraints ? SimTKOpenMMCommon::DefaultReturn : SimTKOpenMMCommon::ErrorReturn);
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490

}

/**---------------------------------------------------------------------------------------

   Report any violated constriants

   @param numberOfAtoms    number of atoms
   @param atomCoordinates  atom coordinates
   @param message          report

   @return number of violated constraints

   --------------------------------------------------------------------------------------- */

491
int ReferenceCCMAAlgorithm::reportCCMA( int numberOfAtoms, vector<RealVec>& atomCoordinates,
492
493
494
495
                                          std::stringstream& message ){

   // ---------------------------------------------------------------------------------------

496
   static const char* methodName = "\nReferenceCCMAAlgorithm::reportCCMA";
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515

   static const RealOpenMM zero        =  0.0;
   static const RealOpenMM one         =  1.0;
   static const RealOpenMM two         =  2.0;
   static const RealOpenMM three       =  3.0;
   static const RealOpenMM oneM        = -1.0;
   static const RealOpenMM half        =  0.5;

   // ---------------------------------------------------------------------------------------

   int numberOfConstraints = getNumberOfConstraints();

   // loop over constraints calculating distance and comparing to
   // expected distance -- report any contraints that are violated

   int numberConverged  = 0;
   RealOpenMM tolerance = getTolerance();
   for( int ii = 0; ii < _numberOfConstraints; ii++ ){

516
517
      int atomI   = _atomIndices[ii].first;
      int atomJ   = _atomIndices[ii].second;
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538

      RealOpenMM rp2  = zero;
      for( int jj = 0; jj < 3; jj++ ){
         rp2 += (atomCoordinates[atomI][jj] - atomCoordinates[atomJ][jj])*(atomCoordinates[atomI][jj] - atomCoordinates[atomJ][jj]);
      }
      RealOpenMM diff = FABS( rp2 - (_distance[ii]*_distance[ii]) );
      if( diff > tolerance ){
         message << ii << " constraint violated: " << atomI << " " << atomJ << "] d=" << SQRT( rp2 ) << " " << rp2 << " d0=" << _distance[ii];
         message << " diff=" << diff;
         message << " [" << atomCoordinates[atomI][0] << " " << atomCoordinates[atomI][1] << " " << atomCoordinates[atomI][2] << "] ";
         message << " [" << atomCoordinates[atomJ][0] << " " << atomCoordinates[atomJ][1] << " " << atomCoordinates[atomJ][2] << "] ";
         message << "\n";
      } else {
         numberConverged++;
      }
   }

   return (numberOfConstraints-numberConverged);

}