ConstraintSet.C 17.8 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
// ConstraintSet.C
//
////////////////////////////////////////////////////////////////////////////////
Francois Gygi committed
18
// $Id: ConstraintSet.C,v 1.11 2010-02-20 23:13:02 fgygi Exp $
19 20

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

const int constraints_maxiter = 10;

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

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

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

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

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

174 175
    Atom *a1 = atoms.findAtom(name1);
    Atom *a2 = atoms.findAtom(name2);
176

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

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

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

Francois Gygi committed
211
    // check if constraint is already defined
212 213 214 215 216 217 218
    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
219 220 221 222
      if ( pc->type() == "distance" )
        found = ( pc->names(0) == name1 && pc->names(1) == name2 ) ||
                ( pc->names(1) == name1 && pc->names(0) == name2 ) ||
                ( pc->name() == name );
223
    }
224

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

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

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

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

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

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

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

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

300 301 302 303 304 305 306
    // 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);
307
      // check if a constraint (name1,name2,name3) or
308
      // (name3,name2,name1) is defined
309 310 311 312 313 314 315 316
      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 );
317
    }
318

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

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

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

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

Francois Gygi committed
385
    double angle = atof(argv[8]);
386 387 388 389 390
    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
391 392
    double velocity = 0.0;
    if ( argc == 10 )
393
    {
Francois Gygi committed
394
      velocity = atof(argv[9]);
395
    }
396

397 398 399 400 401 402 403
    // 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);
404
      // check if an equivalent constraint (name1,name2,name3,name4) or
405
      // (name4,name3,name2,name1) is defined
406 407 408 409 410 411 412 413 414 415
      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 );
416
    }
417

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

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

445 446 447 448
  return true;
}

////////////////////////////////////////////////////////////////////////////////
Francois Gygi committed
449
bool ConstraintSet::set_constraint(int argc, char **argv)
450 451
{
  const bool onpe0 = ctxt_.onpe0();
Francois Gygi committed
452 453 454 455 456 457 458 459 460 461 462
  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]);
463

Francois Gygi committed
464 465
    // check if constraint is already defined
  bool found = false;
466 467 468 469 470 471 472
  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
473
    {
474
      found = true;
Francois Gygi committed
475 476 477 478
      pc->set_value(value);
      if ( set_velocity ) pc->set_velocity(velocity);
    }
    i++;
479
  }
480 481 482 483

  if ( !found )
  {
    if ( onpe0 )
484
      cout << " ConstraintSet: no such constraint" << endl;
485 486
    return false;
  }
Francois Gygi committed
487 488 489 490 491 492 493 494 495 496 497 498 499
  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();

500 501 502 503 504 505 506 507 508 509 510 511 512
  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
513
    {
514
      found = true;
515 516 517 518

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

519 520 521 522 523 524 525 526 527
      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
528
    }
529
  }
530

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

////////////////////////////////////////////////////////////////////////////////
void ConstraintSet::list_constraints(ostream &os)
{
Francois Gygi committed
542
  if ( !constraint_list.empty() )
543
  {
Francois Gygi committed
544 545 546 547 548 549 550
    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;
551 552 553 554 555 556 557 558 559 560 561 562
  }
}

////////////////////////////////////////////////////////////////////////////////
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);
563
  atoms.sync_positions(rp);
564 565
  atoms.set_positions(rp);
  enforce_v(r0,v0);
566
  atoms.sync_velocities(v0);
567 568 569 570 571 572 573 574 575
  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;
576
  while ( !done && (iter < constraints_maxiter) )
577 578 579 580 581 582 583 584 585 586
  {
    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++;
  }
587

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

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

614 615 616
  if ( !done )
  {
    if ( onpe0 )
617 618
      cout << " ConstraintSet: could not enforce velocity constraints in "
           << constraints_maxiter << " iterations" << endl;
619 620 621 622
  }
}

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

////////////////////////////////////////////////////////////////////////////////
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
649 650 651 652 653 654 655 656 657

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