AngleConstraint.C 12.4 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
//  AngleConstraint.C
//
////////////////////////////////////////////////////////////////////////////////
Francois Gygi committed
18
// $Id: AngleConstraint.C,v 1.6 2008-09-08 15:56:17 fgygi Exp $
19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40

#include "AngleConstraint.h"
#include "AtomSet.h"
#include "Atom.h"
#include "Species.h"
#include <cassert>
#include <cmath>
#include <iostream>
#include <iomanip>
using namespace std;

////////////////////////////////////////////////////////////////////////////////
void AngleConstraint::setup(const AtomSet& atoms)
{
  // find position in tau array corresponding to atom name1
  is1_ = atoms.is(name1_);
  ia1_ = atoms.ia(name1_);
  assert(is1_>=0);
  assert(ia1_>=0);
  m1_    = atoms.species_list[is1_]->mass() * 1822.89;
  assert(m1_>0.0);
  m1_inv_ = 1.0 / m1_;
41

42 43 44 45 46 47 48
  is2_ = atoms.is(name2_);
  ia2_ = atoms.ia(name2_);
  assert(is2_>=0);
  assert(ia2_>=0);
  m2_    = atoms.species_list[is2_]->mass() * 1822.89;
  assert(m2_>0.0);
  m2_inv_ = 1.0 / m2_;
49

50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77
  is3_ = atoms.is(name3_);
  ia3_ = atoms.ia(name3_);
  assert(is3_>=0);
  assert(ia3_>=0);
  m3_    = atoms.species_list[is3_]->mass() * 1822.89;
  assert(m3_>0.0);
  m3_inv_ = 1.0 / m3_;
}

////////////////////////////////////////////////////////////////////////////////
void AngleConstraint::update(double dt)
{
  //cout << " AngleConstraint::update" << endl;
  angle_ += velocity_ * dt;
  if ( angle_ <   0.0 ) angle_ =   0.0;
  if ( angle_ > 180.0 ) angle_ = 180.0;
}

////////////////////////////////////////////////////////////////////////////////
bool AngleConstraint::enforce_r(const vector<vector<double> > &r0,
vector<vector<double> > &rp) const
{
  const double* pr1 = &r0[is1_][3*ia1_];
  const double* pr2 = &r0[is2_][3*ia2_];
  const double* pr3 = &r0[is3_][3*ia3_];
  double* pr1p  = &rp[is1_][3*ia1_];
  double* pr2p  = &rp[is2_][3*ia2_];
  double* pr3p  = &rp[is3_][3*ia3_];
78

79 80 81 82 83 84 85 86
  D3vector r1(pr1);
  D3vector r2(pr2);
  D3vector r3(pr3);
  D3vector g1,g2,g3;
  grad_sigma(r1,r2,r3,g1,g2,g3);
  const double a = bond_angle(r1,r2,r3);
  double ng = g1*g1 + g2*g2 + g3*g3;
  assert(ng>=0.0);
87

88 89 90 91 92 93 94 95
  D3vector r1p(pr1p);
  D3vector r2p(pr2p);
  D3vector r3p(pr3p);
  D3vector g1p,g2p,g3p;
  grad_sigma(r1p,r2p,r3p,g1p,g2p,g3p);
  const double ap = bond_angle(r1p,r2p,r3p);
  const double ngp = g1p*g1p + g2p*g2p + g3p*g3p;
  assert(ngp>=0.0);
96

97 98 99 100 101 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
  const double err = fabs(ap-angle_);

  if ( ng == 0.0 )
  {
    // gradient is ill defined at r and is likely to change
    // rapidly between r and rp, invalidating the Taylor expansion
    // use the gradient at rp for the correction
    if ( ngp == 0.0 )
    {
      // gradient is undefined at r and rp
      // Insufficient information to correct rp
      return err < tol_;
    }
    else
    {
      // gradient is defined at rp but not at r
      // use gradient at rp only
      g1 = g1p;
      g2 = g2p;
      g3 = g3p;
      ng = ngp;
    }
  }
  else
  {
    // gradient is defined at r
    if ( ngp == 0.0 )
    {
      // gradient is defined at r but not at rp
      return err < tol_;
    }
    else
    {
      // gradient is defined at r and rp
      // no action necessary
    }
  }
134

135 136 137
  // test alignment of the gradient at r and at rp
  // compute scalar product of normalized gradients
  const double sp = ( g1*g1p + g2*g2p + g3*g3p ) / sqrt( ng * ngp );
138

139 140
  if ( fabs(sp) < 0.5*sqrt(3.0) )
  {
141 142 143
    g1 = g1p;
    g2 = g2p;
    g3 = g3p;
144
  }
145 146

  const double den = m1_inv_ * g1 * g1p +
147 148
                     m2_inv_ * g2 * g2p +
                     m3_inv_ * g3 * g3p;
149

150
#if DEBUG_CONSTRAINTS
151
  cout << " AngleConstraint::enforce_r: "
152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171
       << name1_ << " " << name2_ << " " << name3_
       << " angle = " << ap << endl;
  cout << " AngleConstraint::enforce_r:  r1  = " << r1 << endl;
  cout << " AngleConstraint::enforce_r:  r2  = " << r2 << endl;
  cout << " AngleConstraint::enforce_r:  r3  = " << r3 << endl;
  cout << " AngleConstraint::enforce_r: r1p  = " << r1p << endl;
  cout << " AngleConstraint::enforce_r: r2p  = " << r2p << endl;
  cout << " AngleConstraint::enforce_r: r3p  = " << r3p << endl;
  cout << " AngleConstraint::enforce_r: ap = " << ap << endl;
  cout << " AngleConstraint::enforce_r: err = " << err << endl;
  cout << " AngleConstraint::enforce_r: tol = " << tol_ << endl;
  cout << " AngleConstraint::enforce_r: g1  = " << g1 << endl;
  cout << " AngleConstraint::enforce_r: g2  = " << g2 << endl;
  cout << " AngleConstraint::enforce_r: g3  = " << g3 << endl;
  cout << " AngleConstraint::enforce_r: g1p = " << g1p << endl;
  cout << " AngleConstraint::enforce_r: g2p = " << g2p << endl;
  cout << " AngleConstraint::enforce_r: g3p = " << g3p << endl;
  cout << " AngleConstraint::enforce_r: den = " << den << endl;
#endif
  if ( err < tol_ ) return true;
172

173
  // make one shake iteration
174

175
  assert(fabs(den)>0.0);
176

177
  const double lambda = - sigma(r1p,r2p,r3p) / den;
178

179 180 181
  pr1p[0] += m1_inv_ * lambda * g1.x;
  pr1p[1] += m1_inv_ * lambda * g1.y;
  pr1p[2] += m1_inv_ * lambda * g1.z;
182

183 184 185
  pr2p[0] += m2_inv_ * lambda * g2.x;
  pr2p[1] += m2_inv_ * lambda * g2.y;
  pr2p[2] += m2_inv_ * lambda * g2.z;
186

187 188 189
  pr3p[0] += m3_inv_ * lambda * g3.x;
  pr3p[1] += m3_inv_ * lambda * g3.y;
  pr3p[2] += m3_inv_ * lambda * g3.z;
190

191 192 193 194 195 196 197 198 199 200 201 202 203
  return false;
}

////////////////////////////////////////////////////////////////////////////////
bool AngleConstraint::enforce_v(const vector<vector<double> > &r0,
vector<vector<double> > &v0) const
{
  const double* pr1 = &r0[is1_][3*ia1_];
  const double* pr2 = &r0[is2_][3*ia2_];
  const double* pr3 = &r0[is3_][3*ia3_];
  double* pv1 = &v0[is1_][3*ia1_];
  double* pv2 = &v0[is2_][3*ia2_];
  double* pv3 = &v0[is3_][3*ia3_];
204

205 206 207
  D3vector r1(pr1);
  D3vector r2(pr2);
  D3vector r3(pr3);
208

209
  D3vector g1,g2,g3;
210

211
  grad_sigma(r1,r2,r3,g1,g2,g3);
212

213 214 215
  D3vector v1(pv1);
  D3vector v2(pv2);
  D3vector v3(pv3);
216

217 218
  const double proj = v1*g1 + v2*g2 + v3*g3;
  const double norm2 = g1*g1 + g2*g2 + g3*g3;
219

220 221 222 223
  // if the gradient is too small, do not attempt correction
  if ( norm2 < 1.e-6 ) return true;

  const double err = fabs(proj)/sqrt(norm2);
224

225
#if DEBUG_CONSTRAINTS
226
  cout << " AngleConstraint::enforce_v: "
227 228 229 230 231 232 233 234
       << name1_ << " " << name2_ << " " << name3_ << endl;
  cout << " AngleConstraint::enforce_v: tol = " << tol_ << endl;
  cout << " AngleConstraint::enforce_v: err = " << err << endl;
  cout << " AngleConstraint::enforce_v: g1  = " << g1 << endl;
  cout << " AngleConstraint::enforce_v: g2  = " << g2 << endl;
  cout << " AngleConstraint::enforce_v: g3  = " << g3 << endl;
#endif
  if ( err < tol_ ) return true;
235

236
  // make one shake iteration
237
  const double den = m1_inv_ * g1 * g1 +
238 239 240
                     m2_inv_ * g2 * g2 +
                     m3_inv_ * g3 * g3;
  assert(den>0.0);
241

242
  const double eta = -proj / den;
243

244 245 246
  pv1[0] += m1_inv_ * eta * g1.x;
  pv1[1] += m1_inv_ * eta * g1.y;
  pv1[2] += m1_inv_ * eta * g1.z;
247

248 249 250
  pv2[0] += m2_inv_ * eta * g2.x;
  pv2[1] += m2_inv_ * eta * g2.y;
  pv2[2] += m2_inv_ * eta * g2.z;
251

252 253 254
  pv3[0] += m3_inv_ * eta * g3.x;
  pv3[1] += m3_inv_ * eta * g3.y;
  pv3[2] += m3_inv_ * eta * g3.z;
255

256 257 258 259
  return false;
}

////////////////////////////////////////////////////////////////////////////////
Francois Gygi committed
260 261
void AngleConstraint::compute_force(const vector<vector<double> > &r0,
 const vector<vector<double> > &f)
262 263 264 265
{
  const double* pr1 = &r0[is1_][3*ia1_];
  const double* pr2 = &r0[is2_][3*ia2_];
  const double* pr3 = &r0[is3_][3*ia3_];
Francois Gygi committed
266 267 268
  const double* pf1 = &f[is1_][3*ia1_];
  const double* pf2 = &f[is2_][3*ia2_];
  const double* pf3 = &f[is3_][3*ia3_];
269

270 271 272 273
  D3vector r1(pr1);
  D3vector r2(pr2);
  D3vector r3(pr3);
  D3vector g1,g2,g3;
274

275
  grad_sigma(r1,r2,r3,g1,g2,g3);
276

Francois Gygi committed
277 278 279
  D3vector f1(pf1);
  D3vector f2(pf2);
  D3vector f3(pf3);
280

281 282
  const double norm2 = g1*g1 + g2*g2 + g3*g3;
  if ( norm2 == 0.0 )
Francois Gygi committed
283 284 285 286
  {
    force_ = 0.0;
    return;
  }
287

Francois Gygi committed
288 289
  const double proj = f1*g1 + f2*g2 + f3*g3;
  force_ = -proj/norm2;
290

Francois Gygi committed
291
  // compute weight
292
  const double z = m1_inv_ * g1 * g1 +
Francois Gygi committed
293 294 295 296 297 298 299 300 301 302
                   m2_inv_ * g2 * g2 +
                   m3_inv_ * g3 * g3;
  assert(z > 0.0);
  weight_ = 1.0 / sqrt(z);
#if DEBUG_CONSTRAINTS
  // check value of z
  const double r12s = norm(r1-r2);
  const double r32s = norm(r3-r2);
  const double fac = 180.0/M_PI;
  const double cos_theta = normalized(r1-r2)*normalized(r3-r2);
303
  const double zcheck = fac*fac *
Francois Gygi committed
304 305
    ( m1_inv_ / r12s +
      m2_inv_ * ( (r12s+r32s-2*sqrt(r12s*r32s)*cos_theta) /(r12s*r32s) ) +
306
      m3_inv_ / r32s
Francois Gygi committed
307
    );
308
  cout << " AngleConstraint: z=" << z << " zcheck=" << zcheck << endl;
Francois Gygi committed
309
#endif
310 311 312 313 314 315 316 317 318 319
}

////////////////////////////////////////////////////////////////////////////////
double AngleConstraint::sigma(D3vector a, D3vector b, D3vector c) const
{
  // compute the constraint function
  return bond_angle(a,b,c) - angle_;
}

////////////////////////////////////////////////////////////////////////////////
320
void AngleConstraint::grad_sigma(const D3vector &r1, const D3vector &r2,
321 322 323 324 325
                const D3vector &r3,
                D3vector &g1, D3vector &g2, D3vector &g3) const
{
  D3vector r12(r1-r2);
  D3vector r32(r3-r2);
326 327
  assert(norm2(r12) > 0.0);
  assert(norm2(r32) > 0.0);
328 329 330
  D3vector e12(normalized(r12));
  D3vector e32(normalized(r32));
  const double ss = length(e12^e32);
331

332 333 334 335 336 337 338 339 340 341 342 343 344 345 346
  // use simplified expression if the angle is smaller than 0.2 degrees
  if ( ss < sin(0.2*(M_PI/180.0) ) )
  {
    const double eps = 1.e-8;
    if ( ss < eps )
    {
      g1 = D3vector(0.0,0.0,0.0);
      g2 = D3vector(0.0,0.0,0.0);
      g3 = D3vector(0.0,0.0,0.0);
      //cout << " ======== grad_sigma returning zero gradient" << endl;
    }
    else
    {
      // choose direction e as e12+e32
      D3vector e(e12+e32);
347
      assert(norm2(e)>0.0);
348 349 350 351 352 353 354 355 356 357 358 359 360
      e = normalized(e);
      const double r12_inv = 1.0/length(r12);
      const double r32_inv = 1.0/length(r32);
      g1 = -(180.0/M_PI) * r12_inv * e;
      g2 =  (180.0/M_PI) * ( r12_inv + r32_inv ) * e;
      g3 = -(180.0/M_PI) * r32_inv * e;
      //cout << " ========= grad_sigma returning e12+e32 e =" << e << endl;
    }
  }
  else
  {
    // angle is large enough. Use finite differences
    //cout << " ========= grad_sigma using finite differences" << endl;
361

362 363 364
    const double r12_inv = 1.0/length(r12);
    const double r32_inv = 1.0/length(r32);
    const double a = bond_angle(r1,r2,r3);
365

366 367 368 369 370 371
    const double l12 = length(r1-r2);
    const double l32 = length(r3-r2);
    // displacement h causes changes in angle of less than 0.05 degrees
    const double h = 0.05 * (M_PI/180.0) * min(l12,l32);
    const double fac = 0.5 / h;
    D3vector dx(h,0,0), dy(0,h,0), dz(0,0,h);
372

373
    // compute gradient at r
374

375 376 377
    g1.x = fac * ( sigma(r1+dx,r2,r3) - sigma(r1-dx,r2,r3) );
    g1.y = fac * ( sigma(r1+dy,r2,r3) - sigma(r1-dy,r2,r3) );
    g1.z = fac * ( sigma(r1+dz,r2,r3) - sigma(r1-dz,r2,r3) );
378

379 380 381
    g2.x = fac * ( sigma(r1,r2+dx,r3) - sigma(r1,r2-dx,r3) );
    g2.y = fac * ( sigma(r1,r2+dy,r3) - sigma(r1,r2-dy,r3) );
    g2.z = fac * ( sigma(r1,r2+dz,r3) - sigma(r1,r2-dz,r3) );
382

383 384 385
    g3.x = fac * ( sigma(r1,r2,r3+dx) - sigma(r1,r2,r3-dx) );
    g3.y = fac * ( sigma(r1,r2,r3+dy) - sigma(r1,r2,r3-dy) );
    g3.z = fac * ( sigma(r1,r2,r3+dz) - sigma(r1,r2,r3-dz) );
386
  }
387
}
388

389 390 391 392 393 394
////////////////////////////////////////////////////////////////////////////////
double AngleConstraint::bond_angle(D3vector a, D3vector b, D3vector c) const
{
  // compute the bond angle defined by vectors a,b,c
  D3vector e12(normalized(a-b));
  D3vector e32(normalized(c-b));
395 396 397
  const double ss = length(e12^e32);
  const double cc = e12*e32;
  double an = (180.0/M_PI) * atan2(ss,cc);
398 399 400 401 402 403 404
  return an;
}

////////////////////////////////////////////////////////////////////////////////
ostream& AngleConstraint::print( ostream &os )
{
  os.setf(ios::left,ios::adjustfield);
Francois Gygi committed
405 406 407 408
  os << " <constraint name=\"" << name();
  os << "\" type=\"" << type();
  os << "\" atoms=\"" << name1_ << " ";
  os << name2_ << " " << name3_ << "\"\n";
409 410
  os.setf(ios::fixed,ios::floatfield);
  os.setf(ios::right,ios::adjustfield);
Francois Gygi committed
411 412 413 414
  os << "  value=\"" << setprecision(6) << angle_;
  os << "\" velocity=\"" << setprecision(6) << velocity_ << "\"\n";
  os << "  force=\"" << setprecision(6) << force_;
  os << "\" weight=\"" << setprecision(6) << weight_ << "\"/>";
415 416
  return os;
}