ConstraintSet.C 15.5 KB
Newer Older
1 2
////////////////////////////////////////////////////////////////////////////////
//
Francois Gygi committed
3 4 5 6 7 8 9 10 11 12 13 14
// Copyright (c) 2008 The Regents of the University of California
//
// This file is part of Qbox
//
// Qbox is distributed under the terms of the GNU General Public License 
// as published by the Free Software Foundation, either version 2 of 
// 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.5 2008-08-13 06:39:42 fgygi Exp $
19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34

#include "ConstraintSet.h"
#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>
using namespace std;

const int constraints_maxiter = 10;

////////////////////////////////////////////////////////////////////////////////
Francois Gygi committed
35
bool ConstraintSet::define_constraint(AtomSet &atoms, int argc, char **argv)
36
{
37
  enum constraint_type { unknown, distance_type, multidistance_type,
38 39 40 41 42
                         angle_type, torsion_type }
    type = unknown;
  const double distance_tolerance = 1.0e-7;
  const double angle_tolerance = 1.0e-4;
  const bool onpe0 = ctxt_.onpe0();
43

Francois Gygi committed
44 45 46 47 48 49 50
  // argv[0] == "constraint"
  // argv[1] == "define"
  // argv[2] == {"distance", "angle", "torsion"}
  // argv[3] == constraint name
  // argv[4-(5,6,7)] == atom names
  // argv[{5,6,7}] == {distance,angle,angle}
  // argv[{6,7,8}] == velocity
51 52 53 54 55

  if ( argc < 2 )
  {
    if ( onpe0 )
    {
56
      cout << " Use: constraint define name distance name1 name2 distance [velocity]"
57
           << endl;
Francois Gygi committed
58
      cout << "      constraint define name angle name1 name2 name3 angle [velocity]"
59
           << endl;
Francois Gygi committed
60
      cout << "      constraint define name torsion name1 name2 name3 name4 angle"
61
           << " [velocity] "
62 63
           << endl;
    }
Francois Gygi committed
64
    return false;
65
  }
Francois Gygi committed
66 67
  const string constraint_type = argv[2];
  if ( constraint_type == "distance" )
68 69 70
  {
    type = distance_type;
  }
Francois Gygi committed
71
  else if ( constraint_type == "angle" )
72 73 74
  {
    type = angle_type;
  }
Francois Gygi committed
75
  else if ( constraint_type == "torsion" )
76 77 78 79 80
  {
    type = torsion_type;
  }
  else
  {
81
    if ( onpe0 )
82
      cout << " Incorrect constraint type " << constraint_type << endl;
83 84
    return false;
  }
85

86 87
  if ( type == distance_type )
  {
Francois Gygi committed
88 89
    // define name distance A B value
    // define name distance A B value velocity
90

Francois Gygi committed
91
    if ( argc < 7 || argc > 8 )
92
    {
93
      if ( onpe0 )
94 95
        cout << " Incorrect number of arguments for distance constraint"
             << endl;
96 97 98
      return false;
    }
    double distance, velocity=0.0, distance_tolerance=1.e-7;
Francois Gygi committed
99 100 101
    string name = argv[3];
    string name1 = argv[4];
    string name2 = argv[5];
102

103 104
    Atom *a1 = atoms.findAtom(name1);
    Atom *a2 = atoms.findAtom(name2);
105

106 107
    if ( a1 == 0 || a2 == 0 )
    {
Francois Gygi committed
108 109 110
      if ( onpe0 )
      {
        if ( a1 == 0 )
111
          cout << " ConstraintSet: could not find atom " << name1 << endl;
Francois Gygi committed
112
        if ( a2 == 0 )
113 114
          cout << " ConstraintSet: could not find atom " << name2 << endl;
        cout << " ConstraintSet: could not define constraint" << endl;
Francois Gygi committed
115 116
      }
      return false;
117 118 119
    }
    if ( name1 == name2 )
    {
120
      if ( onpe0 )
121 122
        cout << " ConstraintSet: cannot define distance constraint between "
             << name1 << " and " << name2 << endl;
Francois Gygi committed
123
      return false;
124
    }
125

Francois Gygi committed
126 127
    distance = atof(argv[6]);
    if ( argc == 8 )
128
    {
Francois Gygi committed
129
      velocity = atof(argv[7]);
130
    }
131

132 133 134
    if ( distance <= 0.0 )
    {
      if ( onpe0 )
135 136
        cout << " ConstraintSet: distance must be positive" << endl
             << " ConstraintSet: could not define constraint" << endl;
Francois Gygi committed
137
      return false;
138
    }
139

Francois Gygi committed
140
    // check if constraint is already defined
141 142 143 144 145 146 147 148 149 150 151
    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
      if ( pc->type() == "distance" &&
           ( pc->names(0) == name1 && pc->names(1) == name2 ) ||
           ( pc->names(1) == name1 && pc->names(0) == name2 ) )
         found = true;
Francois Gygi committed
152 153
      if ( pc->type() == "distance" && pc->name() == name )
        found = true;
154
    }
155

156 157 158
    if ( found )
    {
      if ( onpe0 )
159 160
        cout << " ConstraintSet: constraint is already defined:\n"
             << " cannot define constraint" << endl;
Francois Gygi committed
161
      return false;
162 163 164 165
    }
    else
    {
      DistanceConstraint *c =
Francois Gygi committed
166
        new DistanceConstraint(name,name1,name2,distance,
167
                               velocity,distance_tolerance);
168

169 170 171 172 173
      constraint_list.push_back(c);
    }
  }
  else if ( type == angle_type )
  {
Francois Gygi committed
174 175
    // constraint define name angle A B C value
    // constraint define name angle A B C value velocity
176

Francois Gygi committed
177
    if ( argc < 8  || argc > 9 )
178 179
    {
      if ( onpe0 )
180
        cout << " Incorrect number of arguments for angle constraint"
181 182 183
             << endl;
      return false;
    }
Francois Gygi committed
184 185 186 187
    string name = argv[3];
    string name1 = argv[4];
    string name2 = argv[5];
    string name3 = argv[6];
188

189 190 191
    Atom *a1 = atoms.findAtom(name1);
    Atom *a2 = atoms.findAtom(name2);
    Atom *a3 = atoms.findAtom(name3);
192

193 194
    if ( a1 == 0 || a2 == 0 || a3 == 0 )
    {
195
      if ( onpe0 )
Francois Gygi committed
196 197
      {
        if ( a1 == 0 )
198
          cout << " ConstraintSet: could not find atom " << name1 << endl;
Francois Gygi committed
199
        if ( a2 == 0 )
200
          cout << " ConstraintSet: could not find atom " << name2 << endl;
Francois Gygi committed
201
        if ( a3 == 0 )
202 203
          cout << " ConstraintSet: could not find atom " << name3 << endl;
        cout << " ConstraintSet: could not define constraint" << endl;
Francois Gygi committed
204 205
      }
      return false;
206
    }
207

208 209
    if ( name1 == name2 || name1 == name3 || name2 == name3)
    {
210
      if ( onpe0 )
211 212
        cout << " ConstraintSet: cannot define angle constraint between "
             << name1 << " " << name2 << " and " << name3 << endl;
Francois Gygi committed
213
      return false;
214
    }
215

Francois Gygi committed
216 217 218
    const double angle = atof(argv[7]);
    double velocity = 0.0;
    if ( argc == 9 )
219
    {
Francois Gygi committed
220
      velocity = atof(argv[8]);
221
    }
222

223 224 225
    if ( angle < 0.0 || angle > 180.0 )
    {
      if ( onpe0 )
226 227
        cout << " ConstraintSet: angle must be in [0,180]" << endl
             << " ConstraintSet: could not define constraint" << endl;
Francois Gygi committed
228
      return false;
229
    }
230

231 232 233 234 235 236 237
    // 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);
238
      // check if a constraint (name1,name2,name3) or
239 240
      // (name3,name2,name1) is defined
      if ( pc->type() == "angle" &&
241 242
           ( pc->names(0) == name1 &&
             pc->names(1) == name2 &&
243
             pc->names(2) == name3) ||
244
           ( pc->names(0) == name3 &&
245 246 247 248
             pc->names(1) == name2 &&
             pc->names(2) == name1) )
         found = true;
    }
249

250 251 252
    if ( found )
    {
      if ( onpe0 )
253
        cout << " ConstraintSet:set_constraint: an angle constraint "
254 255
             << name1 << " " << name2 << " " << name3
             << " was found" << endl
256
             << " ConstraintSet: cannot define constraint" << endl;
Francois Gygi committed
257
      return false;
258 259 260 261
    }
    else
    {
      AngleConstraint *c =
Francois Gygi committed
262 263
      new AngleConstraint(name, name1,name2,name3,angle,
        velocity,angle_tolerance);
264 265 266 267 268
      constraint_list.push_back(c);
    }
  }
  else if ( type == torsion_type )
  {
Francois Gygi committed
269 270
    // constraint define name torsion A B C D angle
    // constraint define name torsion A B C D angle velocity
271

Francois Gygi committed
272
    if ( argc < 9  || argc > 10 )
273 274
    {
      if ( onpe0 )
275
        cout << " Incorrect number of arguments for torsion constraint"
276 277 278
             << endl;
      return false;
    }
Francois Gygi committed
279 280 281 282 283
    string name = argv[3];
    string name1 = argv[4];
    string name2 = argv[5];
    string name3 = argv[6];
    string name4 = argv[7];
284

285 286 287 288
    Atom *a1 = atoms.findAtom(name1);
    Atom *a2 = atoms.findAtom(name2);
    Atom *a3 = atoms.findAtom(name3);
    Atom *a4 = atoms.findAtom(name4);
289

290 291
    if ( a1 == 0 || a2 == 0 || a3 == 0 || a4 == 0 )
    {
Francois Gygi committed
292
      if ( onpe0 )
293
      {
Francois Gygi committed
294
        if ( a1 == 0 )
295
          cout << " ConstraintSet: could not find atom " << name1 << endl;
Francois Gygi committed
296
        if ( a2 == 0 )
297
          cout << " ConstraintSet: could not find atom " << name2 << endl;
Francois Gygi committed
298
        if ( a3 == 0 )
299
          cout << " ConstraintSet: could not find atom " << name3 << endl;
Francois Gygi committed
300
        if ( a4 == 0 )
301 302
          cout << " ConstraintSet: could not find atom " << name4 << endl;
        cout << " ConstraintSet: could not define constraint" << endl;
Francois Gygi committed
303 304
      }
      return false;
305 306 307 308
    }
    if ( name1 == name2 || name1 == name3 || name1 == name4 ||
         name2 == name3 || name2 == name4 || name3 == name4 )
    {
309
      if ( onpe0 )
310
        cout << " ConstraintSet: cannot define torsion constraint using "
311
             << name1 << " " << name2 << " " << name3 << " " << name4
312
             << endl;
Francois Gygi committed
313
      return false;
314
    }
315

Francois Gygi committed
316
    double angle = atof(argv[8]);
317 318 319 320 321
    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
322 323
    double velocity = 0.0;
    if ( argc == 10 )
324
    {
Francois Gygi committed
325
      velocity = atof(argv[9]);
326
    }
327

328 329 330 331 332 333 334
    // 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);
335
      // check if an equivalent constraint (name1,name2,name3,name4) or
336 337
      // (name4,name3,name2,name1) is defined
      if ( pc->type() == "angle" &&
338 339 340
           ( pc->names(0) == name1 &&
             pc->names(1) == name2 &&
             pc->names(2) == name3 &&
341
             pc->names(3) == name4) ||
342
           ( pc->names(0) == name4 &&
343 344 345 346 347
             pc->names(1) == name3 &&
             pc->names(2) == name2 &&
             pc->names(3) == name1) )
         found = true;
    }
348

349 350 351
    if ( found )
    {
      if ( onpe0 )
352
        cout << " ConstraintSet: a torsion constraint "
353 354
             << name1 << " " << name2 << " " << name3 << " " << name4
             << " is already defined" << endl
355
             << " ConstraintSet: cannot define constraint" << endl;
Francois Gygi committed
356
      return false;
357 358 359 360
    }
    else
    {
      TorsionConstraint *c =
Francois Gygi committed
361
      new TorsionConstraint(name,name1,name2,name3,name4,
362 363 364 365 366 367 368
                            angle,velocity,angle_tolerance);
      constraint_list.push_back(c);
    }
  }
  else
  {
    if ( onpe0 )
369
      cout << " ConstraintSet::set_constraint: internal error" << endl;
370 371
    return false;
  }
372

373 374 375 376
  return true;
}

////////////////////////////////////////////////////////////////////////////////
Francois Gygi committed
377
bool ConstraintSet::set_constraint(int argc, char **argv)
378 379
{
  const bool onpe0 = ctxt_.onpe0();
Francois Gygi committed
380 381 382 383 384 385 386 387 388 389 390
  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]);
391

Francois Gygi committed
392 393
    // check if constraint is already defined
  bool found = false;
394 395 396 397 398 399 400
  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
401
    {
402
      found = true;
Francois Gygi committed
403 404 405 406
      pc->set_value(value);
      if ( set_velocity ) pc->set_velocity(velocity);
    }
    i++;
407
  }
408 409 410 411

  if ( !found )
  {
    if ( onpe0 )
412
      cout << " ConstraintSet: no such constraint" << endl;
413 414
    return false;
  }
Francois Gygi committed
415 416 417 418 419 420 421 422 423 424 425 426 427
  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();

428 429 430 431 432 433 434 435 436 437 438 439 440
  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
441
    {
442 443 444 445 446 447 448 449 450 451
      found = true;
      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
452
    }
453
  }
454

Francois Gygi committed
455
  if ( !found )
456
  {
457
    if ( onpe0 ) cout << " No such constraint" << endl;
458 459 460 461 462 463 464 465
    return false;
  }
  return true;
}

////////////////////////////////////////////////////////////////////////////////
void ConstraintSet::list_constraints(ostream &os)
{
Francois Gygi committed
466
  if ( !constraint_list.empty() )
467
  {
Francois Gygi committed
468 469 470 471 472 473 474
    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;
475 476 477 478 479 480 481 482 483 484 485 486 487 488 489 490 491 492 493 494 495 496 497
  }
}

////////////////////////////////////////////////////////////////////////////////
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;
498
  while ( !done && (iter < constraints_maxiter) )
499 500 501 502 503 504 505 506 507 508
  {
    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++;
  }
509

510 511 512
  if ( !done )
  {
    if ( onpe0 )
513 514
      cout << " ConstraintSet: could not enforce position constraints in "
           << constraints_maxiter << " iterations" << endl;
515 516 517 518 519 520 521 522 523 524
  }
}

////////////////////////////////////////////////////////////////////////////////
void ConstraintSet::enforce_v(const vector<vector<double> > &r0,
                              vector<vector<double> > &v0)
{
  const bool onpe0 = ctxt_.onpe0();
  int iter = 0;
  bool done = false;
525
  while ( !done && (iter < constraints_maxiter) )
526 527 528 529 530 531 532 533 534
  {
    done = true;
    for ( int i = 0; i < constraint_list.size(); i++ )
    {
      bool b = constraint_list[i]->enforce_v(r0,v0);
      done &= b;
    }
    iter++;
  }
535

536 537 538
  if ( !done )
  {
    if ( onpe0 )
539 540
      cout << " ConstraintSet: could not enforce velocity constraints in "
           << constraints_maxiter << " iterations" << endl;
541 542 543 544
  }
}

////////////////////////////////////////////////////////////////////////////////
Francois Gygi committed
545 546
void ConstraintSet::compute_forces(const vector<vector<double> > &r0,
 const vector<vector<double> > &f)
547 548 549
{
  for ( int i = 0; i < constraint_list.size(); i++ )
  {
Francois Gygi committed
550
    constraint_list[i]->compute_force(r0,f);
551 552 553 554 555 556 557 558 559 560 561 562 563 564 565 566 567 568 569 570
  }
}

////////////////////////////////////////////////////////////////////////////////
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);
  }
}