1 ////////////////////////////////////////////////////////////////////////////////
2 //
3 // Copyright (c) 2008 The Regents of the University of California
4 //
5 // This file is part of Qbox
6 //
7 // Qbox is distributed under the terms of the GNU General Public License
8 // as published by the Free Software Foundation, either version 2 of
9 // the License, or (at your option) any later version.
10 // See the file COPYING in the root directory of this distribution
11 // or <http://www.gnu.org/licenses/>.
12 //
13 ////////////////////////////////////////////////////////////////////////////////
14 //
15 // DistanceConstraint.cpp
16 //
17 ////////////////////////////////////////////////////////////////////////////////
18
19 #include "DistanceConstraint.h"
20 #include "AtomSet.h"
21 #include "Atom.h"
22 #include "Species.h"
23 #include <cassert>
24 #include <cmath>
25 #include <iostream>
26 #include <iomanip>
27 using namespace std;
28
29 ////////////////////////////////////////////////////////////////////////////////
setup(const AtomSet & atoms)30 void DistanceConstraint::setup(const AtomSet& atoms)
31 {
32 // find position in tau array corresponding to atom name1
33 is1_ = atoms.is(name1_);
34 ia1_ = atoms.ia(name1_);
35 assert(is1_>=0);
36 assert(ia1_>=0);
37 m1_ = atoms.species_list[is1_]->mass() * 1822.89;
38 assert(m1_>0.0);
39 m1_inv_ = 1.0/m1_;
40
41 is2_ = atoms.is(name2_);
42 ia2_ = atoms.ia(name2_);
43 assert(is2_>=0);
44 assert(ia2_>=0);
45 m2_ = atoms.species_list[is2_]->mass() * 1822.89;
46 assert(m2_>0.0);
47 m2_inv_ = 1.0/m2_;
48 }
49
50 ////////////////////////////////////////////////////////////////////////////////
update(double dt)51 void DistanceConstraint::update(double dt)
52 {
53 // cout << " DistanceConstraint::update" << endl;
54 if ( distance_ + velocity_ * dt > 0.0 )
55 distance_ += velocity_ * dt;
56 }
57
58 ////////////////////////////////////////////////////////////////////////////////
enforce_r(const vector<vector<double>> & r0,vector<vector<double>> & rp) const59 bool DistanceConstraint::enforce_r(const vector<vector<double> > &r0,
60 vector<vector<double> > &rp) const
61 {
62 const double* pr1 = &r0[is1_][3*ia1_];
63 const double* pr2 = &r0[is2_][3*ia2_];
64 D3vector r1(pr1);
65 D3vector r2(pr2);
66 double* pr1p = &rp[is1_][3*ia1_];
67 double* pr2p = &rp[is2_][3*ia2_];
68 D3vector r1p(pr1p);
69 D3vector r2p(pr2p);
70
71 // compute gradient at r
72 D3vector r12(r1-r2);
73 D3vector g1,g2;
74 g1 = 2.0 * r12;
75 g2 = -g1;
76 double ng = g1*g1 + g2*g2;
77 assert(ng>=0.0);
78
79 // compute gradient at rp
80 D3vector r12p(r1p-r2p);
81 D3vector g1p,g2p;
82 g1p = 2.0 * r12p;
83 g2p = -g1p;
84 const double ngp = g1p*g1p + g2p*g2p;
85 assert(ngp>=0.0);
86
87 // test alignment of the gradient at r and at rp
88 // compute scalar product of normalized gradients
89 const double sp = ( g1*g1p + g2*g2p ) / sqrt( ng * ngp );
90 if ( fabs(sp) < 0.5*sqrt(3.0) )
91 {
92 g1 = g1p;
93 g2 = g2p;
94 #if DEBUG_CONSTRAINTS
95 cout << " g and gp nearly orthogonal, use gp only" << endl;
96 #endif
97 }
98
99 const double sigma = r12p*r12p - distance_*distance_;
100 #if DEBUG_CONSTRAINTS
101 cout << " DistanceConstraint::enforce_r: " << name1_ << " " << name2_ << endl;
102 cout << " DistanceConstraint::enforce_r: r1 = " << r1 << endl;
103 cout << " DistanceConstraint::enforce_r: r2 = " << r2 << endl;
104 cout << " DistanceConstraint::enforce_r: tol = " << tol_ << endl;
105 cout << " DistanceConstraint::enforce_r: err = " << sigma << endl;
106 cout << " DistanceConstraint::enforce_r: g1 = " << g1 << endl;
107 cout << " DistanceConstraint::enforce_r: g2 = " << g2 << endl;
108 cout << " DistanceConstraint::enforce_r: g1p = " << g1p << endl;
109 cout << " DistanceConstraint::enforce_r: g2p = " << g2p << endl;
110 #endif
111 if ( fabs(sigma) < tol_ ) return true;
112
113 // make one shake iteration
114 const double den = m1_inv_ * g1 * g1p + m2_inv_ * g2 * g2p;
115
116 const double lambda = -sigma / den;
117
118 pr1p[0] += m1_inv_ * lambda * g1.x;
119 pr1p[1] += m1_inv_ * lambda * g1.y;
120 pr1p[2] += m1_inv_ * lambda * g1.z;
121
122 pr2p[0] += m2_inv_ * lambda * g2.x;
123 pr2p[1] += m2_inv_ * lambda * g2.y;
124 pr2p[2] += m2_inv_ * lambda * g2.z;
125
126 return false;
127 }
128
129 ////////////////////////////////////////////////////////////////////////////////
enforce_v(const vector<vector<double>> & r0,vector<vector<double>> & v0) const130 bool DistanceConstraint::enforce_v(const vector<vector<double> > &r0,
131 vector<vector<double> > &v0) const
132 {
133 const double* pr1 = &r0[is1_][3*ia1_];
134 const double* pr2 = &r0[is2_][3*ia2_];
135 D3vector r1(pr1);
136 D3vector r2(pr2);
137 double* pv1 = &v0[is1_][3*ia1_];
138 double* pv2 = &v0[is2_][3*ia2_];
139 D3vector v1(pv1);
140 D3vector v2(pv2);
141
142 // compute gradient at r
143 D3vector r12(r1-r2);
144 D3vector g1,g2;
145 g1 = 2.0 * r12;
146 g2 = -g1;
147 const double norm2 = g1*g1 + g2*g2;
148 assert(norm2>=0.0);
149
150 // if the gradient is too small, do not attempt correction
151 if ( norm2 < 1.e-6 ) return true;
152 const double proj = v1 * g1 + v2 * g2;
153 const double err = fabs(proj)/sqrt(norm2);
154 #if DEBUG_CONSTRAINTS
155 cout << " DistanceConstraint::enforce_v: " << name1_ << " " << name2_ << endl;
156 cout << " DistanceConstraint::enforce_v: r1 = " << r1 << endl;
157 cout << " DistanceConstraint::enforce_v: r2 = " << r2 << endl;
158 cout << " DistanceConstraint::enforce_v: v1 = "
159 << v1[0] << " " << v1[1] << " " << v1[2] << endl;
160 cout << " DistanceConstraint::enforce_v: v2 = "
161 << v2[0] << " " << v2[1] << " " << v2[2] << endl;
162 cout << " DistanceConstraint::enforce_v: tol = " << tol_ << endl;
163 cout << " DistanceConstraint::enforce_v: err = " << err
164 << endl;
165 #endif
166 if ( err < tol_ ) return true;
167
168 // make one shake iteration
169
170 const double den = m1_inv_ * g1 * g1 +
171 m2_inv_ * g2 * g2;
172 assert(den>0.0);
173 const double eta = -proj / den;
174
175 pv1[0] += m1_inv_ * eta * g1.x;
176 pv1[1] += m1_inv_ * eta * g1.y;
177 pv1[2] += m1_inv_ * eta * g1.z;
178
179 pv2[0] += m2_inv_ * eta * g2.x;
180 pv2[1] += m2_inv_ * eta * g2.y;
181 pv2[2] += m2_inv_ * eta * g2.z;
182
183 return false;
184 }
185
186 ////////////////////////////////////////////////////////////////////////////////
compute_force(const vector<vector<double>> & r0,const vector<vector<double>> & f)187 void DistanceConstraint::compute_force(const vector<vector<double> > &r0,
188 const vector<vector<double> > &f)
189 {
190 const double* pr1 = &r0[is1_][3*ia1_];
191 const double* pr2 = &r0[is2_][3*ia2_];
192 D3vector r1(pr1);
193 D3vector r2(pr2);
194 const double* pf1 = &f[is1_][3*ia1_];
195 const double* pf2 = &f[is2_][3*ia2_];
196 D3vector f1(pf1);
197 D3vector f2(pf2);
198
199 // compute gradient at r
200 D3vector r12(r1-r2);
201 D3vector g1,g2;
202 g1 = 2.0 * r12;
203 g2 = -g1;
204 const double norm2 = g1*g1;
205 assert(norm2>=0.0);
206 if ( norm2 == 0.0 )
207 {
208 force_ = 0.0;
209 return;
210 }
211 const double norm = sqrt(norm2);
212
213 D3vector e1(g1/norm);
214 D3vector e2(-e1);
215
216 const double proj1 = f1*e1;
217 const double proj2 = f2*e2;
218
219 // A positive force on the constraint tends to increase the distance
220 force_ = 0.5*(proj1+proj2);
221 }
222
223 ////////////////////////////////////////////////////////////////////////////////
print(ostream & os)224 ostream& DistanceConstraint::print( ostream &os )
225 {
226 os.setf(ios::left,ios::adjustfield);
227 os << " <constraint name=\"" << name();
228 os << "\" type=\"" << type();
229 os << "\" atoms=\"" << name1_ << " " << name2_ << "\"\n";
230 os.setf(ios::fixed,ios::floatfield);
231 os.setf(ios::right,ios::adjustfield);
232 os << " velocity=\"" << setprecision(6) << velocity_ << "\"";
233 os << " weight=\"" << setprecision(6) << weight_ << "\">\n";
234 os << " <value> " << setprecision(6) << distance_ << " </value>";
235 os << " <force> " << setprecision(6) << force_ << " </force>\n";
236 os << " </constraint>";
237 return os;
238 }
239