ConstraintSet.C 15 KB
Newer Older
1 2 3 4 5
////////////////////////////////////////////////////////////////////////////////
//
// ConstraintSet.C
//
////////////////////////////////////////////////////////////////////////////////
6
// $Id: ConstraintSet.C,v 1.4 2008-02-12 05:39:19 fgygi Exp $
7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22

#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
23
bool ConstraintSet::define_constraint(AtomSet &atoms, int argc, char **argv)
24
{
25
  enum constraint_type { unknown, distance_type, multidistance_type,
26 27 28 29 30
                         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();
31

Francois Gygi committed
32 33 34 35 36 37 38
  // 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
39 40 41 42 43

  if ( argc < 2 )
  {
    if ( onpe0 )
    {
44
      cout << " Use: constraint define name distance name1 name2 distance [velocity]"
45
           << endl;
Francois Gygi committed
46
      cout << "      constraint define name angle name1 name2 name3 angle [velocity]"
47
           << endl;
Francois Gygi committed
48
      cout << "      constraint define name torsion name1 name2 name3 name4 angle"
49
           << " [velocity] "
50 51
           << endl;
    }
Francois Gygi committed
52
    return false;
53
  }
Francois Gygi committed
54 55
  const string constraint_type = argv[2];
  if ( constraint_type == "distance" )
56 57 58
  {
    type = distance_type;
  }
Francois Gygi committed
59
  else if ( constraint_type == "angle" )
60 61 62
  {
    type = angle_type;
  }
Francois Gygi committed
63
  else if ( constraint_type == "torsion" )
64 65 66 67 68
  {
    type = torsion_type;
  }
  else
  {
69
    if ( onpe0 )
70
      cout << " Incorrect constraint type " << constraint_type << endl;
71 72
    return false;
  }
73

74 75
  if ( type == distance_type )
  {
Francois Gygi committed
76 77
    // define name distance A B value
    // define name distance A B value velocity
78

Francois Gygi committed
79
    if ( argc < 7 || argc > 8 )
80
    {
81
      if ( onpe0 )
82 83
        cout << " Incorrect number of arguments for distance constraint"
             << endl;
84 85 86
      return false;
    }
    double distance, velocity=0.0, distance_tolerance=1.e-7;
Francois Gygi committed
87 88 89
    string name = argv[3];
    string name1 = argv[4];
    string name2 = argv[5];
90

91 92
    Atom *a1 = atoms.findAtom(name1);
    Atom *a2 = atoms.findAtom(name2);
93

94 95
    if ( a1 == 0 || a2 == 0 )
    {
Francois Gygi committed
96 97 98
      if ( onpe0 )
      {
        if ( a1 == 0 )
99
          cout << " ConstraintSet: could not find atom " << name1 << endl;
Francois Gygi committed
100
        if ( a2 == 0 )
101 102
          cout << " ConstraintSet: could not find atom " << name2 << endl;
        cout << " ConstraintSet: could not define constraint" << endl;
Francois Gygi committed
103 104
      }
      return false;
105 106 107
    }
    if ( name1 == name2 )
    {
108
      if ( onpe0 )
109 110
        cout << " ConstraintSet: cannot define distance constraint between "
             << name1 << " and " << name2 << endl;
Francois Gygi committed
111
      return false;
112
    }
113

Francois Gygi committed
114 115
    distance = atof(argv[6]);
    if ( argc == 8 )
116
    {
Francois Gygi committed
117
      velocity = atof(argv[7]);
118
    }
119

120 121 122
    if ( distance <= 0.0 )
    {
      if ( onpe0 )
123 124
        cout << " ConstraintSet: distance must be positive" << endl
             << " ConstraintSet: could not define constraint" << endl;
Francois Gygi committed
125
      return false;
126
    }
127

Francois Gygi committed
128
    // check if constraint is already defined
129 130 131 132 133 134 135 136 137 138 139
    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
140 141
      if ( pc->type() == "distance" && pc->name() == name )
        found = true;
142
    }
143

144 145 146
    if ( found )
    {
      if ( onpe0 )
147 148
        cout << " ConstraintSet: constraint is already defined:\n"
             << " cannot define constraint" << endl;
Francois Gygi committed
149
      return false;
150 151 152 153
    }
    else
    {
      DistanceConstraint *c =
Francois Gygi committed
154
        new DistanceConstraint(name,name1,name2,distance,
155
                               velocity,distance_tolerance);
156

157 158 159 160 161
      constraint_list.push_back(c);
    }
  }
  else if ( type == angle_type )
  {
Francois Gygi committed
162 163
    // constraint define name angle A B C value
    // constraint define name angle A B C value velocity
164

Francois Gygi committed
165
    if ( argc < 8  || argc > 9 )
166 167
    {
      if ( onpe0 )
168
        cout << " Incorrect number of arguments for angle constraint"
169 170 171
             << endl;
      return false;
    }
Francois Gygi committed
172 173 174 175
    string name = argv[3];
    string name1 = argv[4];
    string name2 = argv[5];
    string name3 = argv[6];
176

177 178 179
    Atom *a1 = atoms.findAtom(name1);
    Atom *a2 = atoms.findAtom(name2);
    Atom *a3 = atoms.findAtom(name3);
180

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

196 197
    if ( name1 == name2 || name1 == name3 || name2 == name3)
    {
198
      if ( onpe0 )
199 200
        cout << " ConstraintSet: cannot define angle constraint between "
             << name1 << " " << name2 << " and " << name3 << endl;
Francois Gygi committed
201
      return false;
202
    }
203

Francois Gygi committed
204 205 206
    const double angle = atof(argv[7]);
    double velocity = 0.0;
    if ( argc == 9 )
207
    {
Francois Gygi committed
208
      velocity = atof(argv[8]);
209
    }
210

211 212 213
    if ( angle < 0.0 || angle > 180.0 )
    {
      if ( onpe0 )
214 215
        cout << " ConstraintSet: angle must be in [0,180]" << endl
             << " ConstraintSet: could not define constraint" << endl;
Francois Gygi committed
216
      return false;
217
    }
218

219 220 221 222 223 224 225
    // 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);
226
      // check if a constraint (name1,name2,name3) or
227 228
      // (name3,name2,name1) is defined
      if ( pc->type() == "angle" &&
229 230
           ( pc->names(0) == name1 &&
             pc->names(1) == name2 &&
231
             pc->names(2) == name3) ||
232
           ( pc->names(0) == name3 &&
233 234 235 236
             pc->names(1) == name2 &&
             pc->names(2) == name1) )
         found = true;
    }
237

238 239 240
    if ( found )
    {
      if ( onpe0 )
241
        cout << " ConstraintSet:set_constraint: an angle constraint "
242 243
             << name1 << " " << name2 << " " << name3
             << " was found" << endl
244
             << " ConstraintSet: cannot define constraint" << endl;
Francois Gygi committed
245
      return false;
246 247 248 249
    }
    else
    {
      AngleConstraint *c =
Francois Gygi committed
250 251
      new AngleConstraint(name, name1,name2,name3,angle,
        velocity,angle_tolerance);
252 253 254 255 256
      constraint_list.push_back(c);
    }
  }
  else if ( type == torsion_type )
  {
Francois Gygi committed
257 258
    // constraint define name torsion A B C D angle
    // constraint define name torsion A B C D angle velocity
259

Francois Gygi committed
260
    if ( argc < 9  || argc > 10 )
261 262
    {
      if ( onpe0 )
263
        cout << " Incorrect number of arguments for torsion constraint"
264 265 266
             << endl;
      return false;
    }
Francois Gygi committed
267 268 269 270 271
    string name = argv[3];
    string name1 = argv[4];
    string name2 = argv[5];
    string name3 = argv[6];
    string name4 = argv[7];
272

273 274 275 276
    Atom *a1 = atoms.findAtom(name1);
    Atom *a2 = atoms.findAtom(name2);
    Atom *a3 = atoms.findAtom(name3);
    Atom *a4 = atoms.findAtom(name4);
277

278 279
    if ( a1 == 0 || a2 == 0 || a3 == 0 || a4 == 0 )
    {
Francois Gygi committed
280
      if ( onpe0 )
281
      {
Francois Gygi committed
282
        if ( a1 == 0 )
283
          cout << " ConstraintSet: could not find atom " << name1 << endl;
Francois Gygi committed
284
        if ( a2 == 0 )
285
          cout << " ConstraintSet: could not find atom " << name2 << endl;
Francois Gygi committed
286
        if ( a3 == 0 )
287
          cout << " ConstraintSet: could not find atom " << name3 << endl;
Francois Gygi committed
288
        if ( a4 == 0 )
289 290
          cout << " ConstraintSet: could not find atom " << name4 << endl;
        cout << " ConstraintSet: could not define constraint" << endl;
Francois Gygi committed
291 292
      }
      return false;
293 294 295 296
    }
    if ( name1 == name2 || name1 == name3 || name1 == name4 ||
         name2 == name3 || name2 == name4 || name3 == name4 )
    {
297
      if ( onpe0 )
298
        cout << " ConstraintSet: cannot define torsion constraint using "
299
             << name1 << " " << name2 << " " << name3 << " " << name4
300
             << endl;
Francois Gygi committed
301
      return false;
302
    }
303

Francois Gygi committed
304
    double angle = atof(argv[8]);
305 306 307 308 309
    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
310 311
    double velocity = 0.0;
    if ( argc == 10 )
312
    {
Francois Gygi committed
313
      velocity = atof(argv[9]);
314
    }
315

316 317 318 319 320 321 322
    // 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);
323
      // check if an equivalent constraint (name1,name2,name3,name4) or
324 325
      // (name4,name3,name2,name1) is defined
      if ( pc->type() == "angle" &&
326 327 328
           ( pc->names(0) == name1 &&
             pc->names(1) == name2 &&
             pc->names(2) == name3 &&
329
             pc->names(3) == name4) ||
330
           ( pc->names(0) == name4 &&
331 332 333 334 335
             pc->names(1) == name3 &&
             pc->names(2) == name2 &&
             pc->names(3) == name1) )
         found = true;
    }
336

337 338 339
    if ( found )
    {
      if ( onpe0 )
340
        cout << " ConstraintSet: a torsion constraint "
341 342
             << name1 << " " << name2 << " " << name3 << " " << name4
             << " is already defined" << endl
343
             << " ConstraintSet: cannot define constraint" << endl;
Francois Gygi committed
344
      return false;
345 346 347 348
    }
    else
    {
      TorsionConstraint *c =
Francois Gygi committed
349
      new TorsionConstraint(name,name1,name2,name3,name4,
350 351 352 353 354 355 356
                            angle,velocity,angle_tolerance);
      constraint_list.push_back(c);
    }
  }
  else
  {
    if ( onpe0 )
357
      cout << " ConstraintSet::set_constraint: internal error" << endl;
358 359
    return false;
  }
360

361 362 363 364
  return true;
}

////////////////////////////////////////////////////////////////////////////////
Francois Gygi committed
365
bool ConstraintSet::set_constraint(int argc, char **argv)
366 367
{
  const bool onpe0 = ctxt_.onpe0();
Francois Gygi committed
368 369 370 371 372 373 374 375 376 377 378
  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]);
379

Francois Gygi committed
380 381
    // check if constraint is already defined
  bool found = false;
382 383 384 385 386 387 388
  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
389
    {
390
      found = true;
Francois Gygi committed
391 392 393 394
      pc->set_value(value);
      if ( set_velocity ) pc->set_velocity(velocity);
    }
    i++;
395
  }
396 397 398 399

  if ( !found )
  {
    if ( onpe0 )
400
      cout << " ConstraintSet: no such constraint" << endl;
401 402
    return false;
  }
Francois Gygi committed
403 404 405 406 407 408 409 410 411 412 413 414 415
  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();

416 417 418 419 420 421 422 423 424 425 426 427 428
  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
429
    {
430 431 432 433 434 435 436 437 438 439
      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
440
    }
441
  }
442

Francois Gygi committed
443
  if ( !found )
444
  {
445
    if ( onpe0 ) cout << " No such constraint" << endl;
446 447 448 449 450 451 452 453
    return false;
  }
  return true;
}

////////////////////////////////////////////////////////////////////////////////
void ConstraintSet::list_constraints(ostream &os)
{
Francois Gygi committed
454
  if ( !constraint_list.empty() )
455
  {
Francois Gygi committed
456 457 458 459 460 461 462
    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;
463 464 465 466 467 468 469 470 471 472 473 474 475 476 477 478 479 480 481 482 483 484 485
  }
}

////////////////////////////////////////////////////////////////////////////////
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;
486
  while ( !done && (iter < constraints_maxiter) )
487 488 489 490 491 492 493 494 495 496
  {
    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++;
  }
497

498 499 500
  if ( !done )
  {
    if ( onpe0 )
501 502
      cout << " ConstraintSet: could not enforce position constraints in "
           << constraints_maxiter << " iterations" << endl;
503 504 505 506 507 508 509 510 511 512
  }
}

////////////////////////////////////////////////////////////////////////////////
void ConstraintSet::enforce_v(const vector<vector<double> > &r0,
                              vector<vector<double> > &v0)
{
  const bool onpe0 = ctxt_.onpe0();
  int iter = 0;
  bool done = false;
513
  while ( !done && (iter < constraints_maxiter) )
514 515 516 517 518 519 520 521 522
  {
    done = true;
    for ( int i = 0; i < constraint_list.size(); i++ )
    {
      bool b = constraint_list[i]->enforce_v(r0,v0);
      done &= b;
    }
    iter++;
  }
523

524 525 526
  if ( !done )
  {
    if ( onpe0 )
527 528
      cout << " ConstraintSet: could not enforce velocity constraints in "
           << constraints_maxiter << " iterations" << endl;
529 530 531 532
  }
}

////////////////////////////////////////////////////////////////////////////////
Francois Gygi committed
533 534
void ConstraintSet::compute_forces(const vector<vector<double> > &r0,
 const vector<vector<double> > &f)
535 536 537
{
  for ( int i = 0; i < constraint_list.size(); i++ )
  {
Francois Gygi committed
538
    constraint_list[i]->compute_force(r0,f);
539 540 541 542 543 544 545 546 547 548 549 550 551 552 553 554 555 556 557 558
  }
}

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