ConstraintSet.C 17.7 KB
Newer Older
1 2
////////////////////////////////////////////////////////////////////////////////
//
Francois Gygi committed
3 4 5 6
// Copyright (c) 2008 The Regents of the University of California
//
// This file is part of Qbox
//
Francois Gygi committed
7 8
// Qbox is distributed under the terms of the GNU General Public License
// as published by the Free Software Foundation, either version 2 of
Francois Gygi committed
9 10 11 12 13 14
// the License, or (at your option) any later version.
// See the file COPYING in the root directory of this distribution
// or <http://www.gnu.org/licenses/>.
//
////////////////////////////////////////////////////////////////////////////////
//
15 16 17 18 19
// ConstraintSet.C
//
////////////////////////////////////////////////////////////////////////////////

#include "ConstraintSet.h"
20
#include "PositionConstraint.h"
21 22 23 24 25 26 27 28
#include "DistanceConstraint.h"
#include "AngleConstraint.h"
#include "TorsionConstraint.h"
#include "Atom.h"
#include "AtomSet.h"
#include "Context.h"
#include <iostream>
#include <iomanip>
29
#include <cstdlib> // atof
30 31
using namespace std;

32
const int constraints_maxiter = 50;
33

34 35 36 37 38 39 40
////////////////////////////////////////////////////////////////////////////////
ConstraintSet::~ConstraintSet(void)
{
  for ( int ic = 0; ic < constraint_list.size(); ic++ )
    delete constraint_list[ic];
}

41
////////////////////////////////////////////////////////////////////////////////
Francois Gygi committed
42
bool ConstraintSet::define_constraint(AtomSet &atoms, int argc, char **argv)
43
{
44
  enum constraint_type { unknown, position_type, distance_type,
45 46
                         angle_type, torsion_type }
    type = unknown;
47
  const double position_tolerance = 1.0e-7;
48 49 50
  const double distance_tolerance = 1.0e-7;
  const double angle_tolerance = 1.0e-4;
  const bool onpe0 = ctxt_.onpe0();
51

Francois Gygi committed
52 53
  // argv[0] == "constraint"
  // argv[1] == "define"
54
  // argv[2] == {"position", "distance", "angle", "torsion"}
Francois Gygi committed
55 56 57 58
  // argv[3] == constraint name
  // argv[4-(5,6,7)] == atom names
  // argv[{5,6,7}] == {distance,angle,angle}
  // argv[{6,7,8}] == velocity
59 60 61 62 63

  if ( argc < 2 )
  {
    if ( onpe0 )
    {
64
      cout << " Use: constraint define position constraint_name atom_name"
65
           << endl;
66 67
      cout << " Use: constraint define distance constraint_name "
           << "atom_name1 atom_name2 distance_value [velocity]"
68
           << endl;
69 70
      cout << "      constraint define angle constraint_name "
           << "name1 name2 name3 angle_value [velocity]"
71
           << endl;
72 73
      cout << "      constraint define torsion constraint_name "
           << "name1 name2 name3 name4 angle_value"
74
           << " [velocity] "
75 76
           << endl;
    }
Francois Gygi committed
77
    return false;
78
  }
Francois Gygi committed
79
  const string constraint_type = argv[2];
80 81 82 83 84
  if ( constraint_type == "position" )
  {
    type = position_type;
  }
  else if ( constraint_type == "distance" )
85 86 87
  {
    type = distance_type;
  }
Francois Gygi committed
88
  else if ( constraint_type == "angle" )
89 90 91
  {
    type = angle_type;
  }
Francois Gygi committed
92
  else if ( constraint_type == "torsion" )
93 94 95 96 97
  {
    type = torsion_type;
  }
  else
  {
98
    if ( onpe0 )
99
      cout << " Incorrect constraint type " << constraint_type << endl;
100 101
    return false;
  }
102

103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155
  if ( type == position_type )
  {
    // define position name A

    if ( argc != 5 )
    {
      if ( onpe0 )
        cout << " Incorrect number of arguments for position constraint"
             << endl;
      return false;
    }
    string name = argv[3];
    string name1 = argv[4];

    Atom *a1 = atoms.findAtom(name1);

    if ( a1 == 0 )
    {
      if ( onpe0 )
      {
        cout << " ConstraintSet: could not find atom " << name1 << endl;
        cout << " ConstraintSet: could not define constraint" << endl;
      }
      return false;
    }

    // check if constraint is already defined
    bool found = false;
    Constraint *pc = 0;
    for ( int i = 0; i < constraint_list.size(); i++ )
    {
      pc = constraint_list[i];
      assert(pc != 0);
      // check if a constraint with same name or with same atom is defined
      if ( pc->type() == "position" )
        found = ( pc->name() == name ) || ( pc->names(0) == name1 );
    }

    if ( found )
    {
      if ( onpe0 )
        cout << " ConstraintSet: constraint is already defined:\n"
             << " cannot define constraint" << endl;
      return false;
    }
    else
    {
      PositionConstraint *c =
        new PositionConstraint(name,name1,position_tolerance);
      constraint_list.push_back(c);
    }
  }
  else if ( type == distance_type )
156
  {
157 158
    // define distance name A B value
    // define distance name A B value velocity
159

Francois Gygi committed
160
    if ( argc < 7 || argc > 8 )
161
    {
162
      if ( onpe0 )
163 164
        cout << " Incorrect number of arguments for distance constraint"
             << endl;
165 166
      return false;
    }
167
    double distance, velocity=0.0;
Francois Gygi committed
168 169 170
    string name = argv[3];
    string name1 = argv[4];
    string name2 = argv[5];
171

172 173
    Atom *a1 = atoms.findAtom(name1);
    Atom *a2 = atoms.findAtom(name2);
174

175 176
    if ( a1 == 0 || a2 == 0 )
    {
Francois Gygi committed
177 178 179
      if ( onpe0 )
      {
        if ( a1 == 0 )
180
          cout << " ConstraintSet: could not find atom " << name1 << endl;
Francois Gygi committed
181
        if ( a2 == 0 )
182 183
          cout << " ConstraintSet: could not find atom " << name2 << endl;
        cout << " ConstraintSet: could not define constraint" << endl;
Francois Gygi committed
184 185
      }
      return false;
186 187 188
    }
    if ( name1 == name2 )
    {
189
      if ( onpe0 )
190 191
        cout << " ConstraintSet: cannot define distance constraint between "
             << name1 << " and " << name2 << endl;
Francois Gygi committed
192
      return false;
193
    }
194

Francois Gygi committed
195 196
    distance = atof(argv[6]);
    if ( argc == 8 )
197
    {
Francois Gygi committed
198
      velocity = atof(argv[7]);
199
    }
200

201 202 203
    if ( distance <= 0.0 )
    {
      if ( onpe0 )
204 205
        cout << " ConstraintSet: distance must be positive" << endl
             << " ConstraintSet: could not define constraint" << endl;
Francois Gygi committed
206
      return false;
207
    }
208

Francois Gygi committed
209
    // check if constraint is already defined
210 211 212 213 214 215 216
    bool found = false;
    Constraint *pc = 0;
    for ( int i = 0; i < constraint_list.size(); i++ )
    {
      pc = constraint_list[i];
      assert(pc != 0);
      // check if a constraint (name1,name2) or (name2,name1) is defined
217 218 219 220
      if ( pc->type() == "distance" )
        found = ( pc->names(0) == name1 && pc->names(1) == name2 ) ||
                ( pc->names(1) == name1 && pc->names(0) == name2 ) ||
                ( pc->name() == name );
221
    }
222

223 224 225
    if ( found )
    {
      if ( onpe0 )
226 227
        cout << " ConstraintSet: constraint is already defined:\n"
             << " cannot define constraint" << endl;
Francois Gygi committed
228
      return false;
229 230 231 232
    }
    else
    {
      DistanceConstraint *c =
Francois Gygi committed
233
        new DistanceConstraint(name,name1,name2,distance,
234
                               velocity,distance_tolerance);
235

236 237 238 239 240
      constraint_list.push_back(c);
    }
  }
  else if ( type == angle_type )
  {
241 242
    // constraint define angle name A B C value
    // constraint define angle name A B C value velocity
243

Francois Gygi committed
244
    if ( argc < 8  || argc > 9 )
245 246
    {
      if ( onpe0 )
247
        cout << " Incorrect number of arguments for angle constraint"
248 249 250
             << endl;
      return false;
    }
Francois Gygi committed
251 252 253 254
    string name = argv[3];
    string name1 = argv[4];
    string name2 = argv[5];
    string name3 = argv[6];
255

256 257 258
    Atom *a1 = atoms.findAtom(name1);
    Atom *a2 = atoms.findAtom(name2);
    Atom *a3 = atoms.findAtom(name3);
259

260 261
    if ( a1 == 0 || a2 == 0 || a3 == 0 )
    {
262
      if ( onpe0 )
Francois Gygi committed
263 264
      {
        if ( a1 == 0 )
265
          cout << " ConstraintSet: could not find atom " << name1 << endl;
Francois Gygi committed
266
        if ( a2 == 0 )
267
          cout << " ConstraintSet: could not find atom " << name2 << endl;
Francois Gygi committed
268
        if ( a3 == 0 )
269 270
          cout << " ConstraintSet: could not find atom " << name3 << endl;
        cout << " ConstraintSet: could not define constraint" << endl;
Francois Gygi committed
271 272
      }
      return false;
273
    }
274

275 276
    if ( name1 == name2 || name1 == name3 || name2 == name3)
    {
277
      if ( onpe0 )
278 279
        cout << " ConstraintSet: cannot define angle constraint between "
             << name1 << " " << name2 << " and " << name3 << endl;
Francois Gygi committed
280
      return false;
281
    }
282

Francois Gygi committed
283 284 285
    const double angle = atof(argv[7]);
    double velocity = 0.0;
    if ( argc == 9 )
286
    {
Francois Gygi committed
287
      velocity = atof(argv[8]);
288
    }
289

290 291 292
    if ( angle < 0.0 || angle > 180.0 )
    {
      if ( onpe0 )
293 294
        cout << " ConstraintSet: angle must be in [0,180]" << endl
             << " ConstraintSet: could not define constraint" << endl;
Francois Gygi committed
295
      return false;
296
    }
297

298 299 300 301 302 303 304
    // check if equivalent constraint is already defined
    bool found = false;
    Constraint *pc = 0;
    for ( int i = 0; i < constraint_list.size(); i++ )
    {
      pc = constraint_list[i];
      assert(pc != 0);
305
      // check if a constraint (name1,name2,name3) or
306
      // (name3,name2,name1) is defined
307 308 309 310 311 312 313 314
      if ( pc->type() == "angle" )
        found = ( pc->names(0) == name1 &&
                  pc->names(1) == name2 &&
                  pc->names(2) == name3 ) ||
                ( pc->names(0) == name3 &&
                  pc->names(1) == name2 &&
                  pc->names(2) == name1 ) ||
                ( pc->name() == name );
315
    }
316

317 318 319
    if ( found )
    {
      if ( onpe0 )
320
        cout << " ConstraintSet:set_constraint: an angle constraint "
321 322
             << name1 << " " << name2 << " " << name3
             << " was found" << endl
323
             << " ConstraintSet: cannot define constraint" << endl;
Francois Gygi committed
324
      return false;
325 326 327 328
    }
    else
    {
      AngleConstraint *c =
Francois Gygi committed
329 330
      new AngleConstraint(name, name1,name2,name3,angle,
        velocity,angle_tolerance);
331 332 333 334 335
      constraint_list.push_back(c);
    }
  }
  else if ( type == torsion_type )
  {
336 337
    // constraint define torsion name A B C D angle
    // constraint define torsion name A B C D angle velocity
338

Francois Gygi committed
339
    if ( argc < 9  || argc > 10 )
340 341
    {
      if ( onpe0 )
342
        cout << " Incorrect number of arguments for torsion constraint"
343 344 345
             << endl;
      return false;
    }
Francois Gygi committed
346 347 348 349 350
    string name = argv[3];
    string name1 = argv[4];
    string name2 = argv[5];
    string name3 = argv[6];
    string name4 = argv[7];
351

352 353 354 355
    Atom *a1 = atoms.findAtom(name1);
    Atom *a2 = atoms.findAtom(name2);
    Atom *a3 = atoms.findAtom(name3);
    Atom *a4 = atoms.findAtom(name4);
356

357 358
    if ( a1 == 0 || a2 == 0 || a3 == 0 || a4 == 0 )
    {
Francois Gygi committed
359
      if ( onpe0 )
360
      {
Francois Gygi committed
361
        if ( a1 == 0 )
362
          cout << " ConstraintSet: could not find atom " << name1 << endl;
Francois Gygi committed
363
        if ( a2 == 0 )
364
          cout << " ConstraintSet: could not find atom " << name2 << endl;
Francois Gygi committed
365
        if ( a3 == 0 )
366
          cout << " ConstraintSet: could not find atom " << name3 << endl;
Francois Gygi committed
367
        if ( a4 == 0 )
368 369
          cout << " ConstraintSet: could not find atom " << name4 << endl;
        cout << " ConstraintSet: could not define constraint" << endl;
Francois Gygi committed
370 371
      }
      return false;
372 373 374 375
    }
    if ( name1 == name2 || name1 == name3 || name1 == name4 ||
         name2 == name3 || name2 == name4 || name3 == name4 )
    {
376
      if ( onpe0 )
377
        cout << " ConstraintSet: cannot define torsion constraint using "
378
             << name1 << " " << name2 << " " << name3 << " " << name4
379
             << endl;
Francois Gygi committed
380
      return false;
381
    }
382

Francois Gygi committed
383
    double angle = atof(argv[8]);
384 385 386 387 388
    if ( angle > 180.0 )
      while ( angle > 180.0 ) angle -= 360.0;
    else if ( angle < -180.0 )
      while ( angle < -180.0 ) angle += 360.0;

Francois Gygi committed
389 390
    double velocity = 0.0;
    if ( argc == 10 )
391
    {
Francois Gygi committed
392
      velocity = atof(argv[9]);
393
    }
394

395 396 397 398 399 400 401
    // check if equivalent constraint is already defined
    bool found = false;
    Constraint *pc = 0;
    for ( int i = 0; i < constraint_list.size(); i++ )
    {
      pc = constraint_list[i];
      assert(pc != 0);
402
      // check if an equivalent constraint (name1,name2,name3,name4) or
403
      // (name4,name3,name2,name1) is defined
404 405 406 407 408 409 410 411 412 413
      if ( pc->type() == "angle" )
        found = ( pc->names(0) == name1 &&
                  pc->names(1) == name2 &&
                  pc->names(2) == name3 &&
                  pc->names(3) == name4 ) ||
                ( pc->names(0) == name4 &&
                  pc->names(1) == name3 &&
                  pc->names(2) == name2 &&
                  pc->names(3) == name1 ) ||
                ( pc->name() == name );
414
    }
415

416 417 418
    if ( found )
    {
      if ( onpe0 )
419
        cout << " ConstraintSet: a torsion constraint "
420 421
             << name1 << " " << name2 << " " << name3 << " " << name4
             << " is already defined" << endl
422
             << " ConstraintSet: cannot define constraint" << endl;
Francois Gygi committed
423
      return false;
424 425 426 427
    }
    else
    {
      TorsionConstraint *c =
Francois Gygi committed
428
      new TorsionConstraint(name,name1,name2,name3,name4,
429 430 431 432 433 434 435
                            angle,velocity,angle_tolerance);
      constraint_list.push_back(c);
    }
  }
  else
  {
    if ( onpe0 )
436
      cout << " ConstraintSet::set_constraint: internal error" << endl;
437 438
    return false;
  }
439

440 441 442
  // update total number of blocked degrees of freedom
  ndofs_ += constraint_list.back()->dofs();

443 444 445 446
  return true;
}

////////////////////////////////////////////////////////////////////////////////
Francois Gygi committed
447
bool ConstraintSet::set_constraint(int argc, char **argv)
448 449
{
  const bool onpe0 = ctxt_.onpe0();
Francois Gygi committed
450 451 452 453 454 455 456 457 458 459 460
  assert(argc==4||argc==5);
  // argv[0] == "constraint"
  // argv[1] == "set"
  // argv[2] == constraint_name
  // argv[3] == value
  // argv[4] (optional) == velocity
  string name = argv[2];
  const double value = atof(argv[3]);
  double velocity = 0.0;
  const bool set_velocity = ( argc == 5 );
  if ( set_velocity ) velocity = atof(argv[4]);
461

Francois Gygi committed
462 463
    // check if constraint is already defined
  bool found = false;
464 465 466 467 468 469 470
  vector<Constraint*>::iterator i = constraint_list.begin();
  while ( !found && i != constraint_list.end() )
  {
    Constraint *pc = *i;
    assert(pc != 0);

    if ( pc->name() == name )
Francois Gygi committed
471
    {
472
      found = true;
Francois Gygi committed
473 474 475 476
      pc->set_value(value);
      if ( set_velocity ) pc->set_velocity(velocity);
    }
    i++;
477
  }
478 479 480 481

  if ( !found )
  {
    if ( onpe0 )
482
      cout << " ConstraintSet: no such constraint" << endl;
483 484
    return false;
  }
Francois Gygi committed
485 486 487 488 489 490 491 492 493 494 495 496 497
  return true;
}

////////////////////////////////////////////////////////////////////////////////
bool ConstraintSet::delete_constraint(int argc, char **argv)
{
  assert(argc==3);
  // argv[0] == "constraint"
  // argv[1] == "delete"
  // argv[2] == constraint_name
  string name = argv[2];
  const bool onpe0 = ctxt_.onpe0();

498 499 500 501 502 503 504 505 506 507 508 509 510
  bool found = false;
  // note next loop in reverse: avoid use of invalidated iterators
  // after erase operation

  vector<Constraint*>::iterator i = constraint_list.begin();
  while ( !found && i != constraint_list.end() )
  {
    Constraint *pc = *i;
    assert(pc != 0);

    // note structure of if else test to avoid incrementing
    // invalidated iterator after erase (see Meyers STL, p.45)
    if ( pc->name() == name )
Francois Gygi committed
511
    {
512
      found = true;
513 514 515 516

      // update total number of blocked degrees of freedom
      ndofs_ -= pc->dofs();

517 518 519 520 521 522 523 524 525
      delete pc;

      // remove constraint pointer from the list
      // note: iterator is incremented before erasing, remains valid
      constraint_list.erase(i++);
    }
    else
    {
      i++;
Francois Gygi committed
526
    }
527
  }
528

Francois Gygi committed
529
  if ( !found )
530
  {
531
    if ( onpe0 ) cout << " No such constraint" << endl;
532 533 534 535 536 537 538 539
    return false;
  }
  return true;
}

////////////////////////////////////////////////////////////////////////////////
void ConstraintSet::list_constraints(ostream &os)
{
Francois Gygi committed
540
  if ( !constraint_list.empty() )
541
  {
Francois Gygi committed
542 543 544 545 546 547 548
    os << " <constraint_set>" << endl;
    for ( int i = 0; i < constraint_list.size(); i++ )
    {
      Constraint *c = constraint_list[i];
      os << *c << endl;
    }
    os << " </constraint_set>" << endl;
549 550 551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570 571
  }
}

////////////////////////////////////////////////////////////////////////////////
void ConstraintSet::enforce(AtomSet& atoms)
{
  vector<vector<double> > r0,rp,v0;
  setup(atoms);
  atoms.get_positions(r0);
  rp=r0;
  atoms.get_velocities(v0);
  enforce_r(r0,rp);
  atoms.set_positions(rp);
  enforce_v(r0,v0);
  atoms.set_velocities(v0);
}
////////////////////////////////////////////////////////////////////////////////
void ConstraintSet::enforce_r(const vector<vector<double> > &r0,
                              vector<vector<double> > &rp)
{
  const bool onpe0 = ctxt_.onpe0();
  int iter = 0;
  bool done = false;
572
  while ( !done && (iter < constraints_maxiter) )
573 574 575 576 577 578 579 580 581 582
  {
    done = true;
    for ( int i = 0; i < constraint_list.size(); i++ )
    {
      Constraint *c = constraint_list[i];
      bool b = c->enforce_r(r0,rp);
      done &= b;
    }
    iter++;
  }
583

584 585 586
  if ( !done )
  {
    if ( onpe0 )
587 588
      cout << " ConstraintSet: could not enforce position constraints in "
           << constraints_maxiter << " iterations" << endl;
589 590 591 592 593 594 595 596 597 598
  }
}

////////////////////////////////////////////////////////////////////////////////
void ConstraintSet::enforce_v(const vector<vector<double> > &r0,
                              vector<vector<double> > &v0)
{
  const bool onpe0 = ctxt_.onpe0();
  int iter = 0;
  bool done = false;
599
  while ( !done && (iter < constraints_maxiter) )
600 601 602 603 604 605 606 607 608
  {
    done = true;
    for ( int i = 0; i < constraint_list.size(); i++ )
    {
      bool b = constraint_list[i]->enforce_v(r0,v0);
      done &= b;
    }
    iter++;
  }
609

610 611 612
  if ( !done )
  {
    if ( onpe0 )
613 614
      cout << " ConstraintSet: could not enforce velocity constraints in "
           << constraints_maxiter << " iterations" << endl;
615 616 617 618
  }
}

////////////////////////////////////////////////////////////////////////////////
Francois Gygi committed
619 620
void ConstraintSet::compute_forces(const vector<vector<double> > &r0,
 const vector<vector<double> > &f)
621 622 623
{
  for ( int i = 0; i < constraint_list.size(); i++ )
  {
Francois Gygi committed
624
    constraint_list[i]->compute_force(r0,f);
625 626 627 628 629 630 631 632 633 634 635 636 637 638 639 640 641 642 643 644
  }
}

////////////////////////////////////////////////////////////////////////////////
void ConstraintSet::update_constraints(double dt)
{
  for ( int i = 0; i < constraint_list.size(); i++ )
  {
    constraint_list[i]->update(dt);
  }
}

////////////////////////////////////////////////////////////////////////////////
void ConstraintSet::setup(AtomSet& atoms)
{
  for ( int i = 0; i < constraint_list.size(); i++ )
  {
    constraint_list[i]->setup(atoms);
  }
}
Francois Gygi committed
645 646 647 648 649 650 651 652 653

////////////////////////////////////////////////////////////////////////////////
void ConstraintSet::reset(void)
{
  for ( int i = 0; i < constraint_list.size(); i++ )
    delete constraint_list[i];
  ndofs_ = 0;
  constraint_list.resize(0);
}