# | Line 1 | Line 1 | |
---|---|---|
1 | < | /* |
1 | > | /* |
2 | * Copyright (c) 2005 The University of Notre Dame. All Rights Reserved. | |
3 | * | |
4 | * The University of Notre Dame grants you ("Licensee") a | |
# | Line 6 | Line 6 | |
6 | * redistribute this software in source and binary code form, provided | |
7 | * that the following conditions are met: | |
8 | * | |
9 | < | * 1. Acknowledgement of the program authors must be made in any |
10 | < | * publication of scientific results based in part on use of the |
11 | < | * program. An acceptable form of acknowledgement is citation of |
12 | < | * the article in which the program was described (Matthew |
13 | < | * A. Meineke, Charles F. Vardeman II, Teng Lin, Christopher |
14 | < | * J. Fennell and J. Daniel Gezelter, "OOPSE: An Object-Oriented |
15 | < | * Parallel Simulation Engine for Molecular Dynamics," |
16 | < | * J. Comput. Chem. 26, pp. 252-271 (2005)) |
17 | < | * |
18 | < | * 2. Redistributions of source code must retain the above copyright |
9 | > | * 1. Redistributions of source code must retain the above copyright |
10 | * notice, this list of conditions and the following disclaimer. | |
11 | * | |
12 | < | * 3. Redistributions in binary form must reproduce the above copyright |
12 | > | * 2. Redistributions in binary form must reproduce the above copyright |
13 | * notice, this list of conditions and the following disclaimer in the | |
14 | * documentation and/or other materials provided with the | |
15 | * distribution. | |
# | Line 37 | Line 28 | |
28 | * arising out of the use of or inability to use software, even if the | |
29 | * University of Notre Dame has been advised of the possibility of | |
30 | * such damages. | |
31 | + | * |
32 | + | * SUPPORT OPEN SCIENCE! If you use OpenMD or its source code in your |
33 | + | * research, please cite the appropriate papers when you publish your |
34 | + | * work. Good starting points are: |
35 | + | * |
36 | + | * [1] Meineke, et al., J. Comp. Chem. 26, 252-271 (2005). |
37 | + | * [2] Fennell & Gezelter, J. Chem. Phys. 124, 234104 (2006). |
38 | + | * [3] Sun, Lin & Gezelter, J. Chem. Phys. 128, 24107 (2008). |
39 | + | * [4] Vardeman & Gezelter, in progress (2009). |
40 | */ | |
41 | #include <algorithm> | |
42 | #include <math.h> | |
43 | #include "primitives/RigidBody.hpp" | |
44 | #include "utils/simError.h" | |
45 | < | namespace oopse { |
46 | < | |
47 | < | RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData), inertiaTensor_(0.0){ |
48 | < | |
49 | < | } |
50 | < | |
51 | < | void RigidBody::setPrevA(const RotMat3x3d& a) { |
45 | > | #include "utils/NumericConstant.hpp" |
46 | > | namespace OpenMD { |
47 | > | |
48 | > | RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData), |
49 | > | inertiaTensor_(0.0){ |
50 | > | } |
51 | > | |
52 | > | void RigidBody::setPrevA(const RotMat3x3d& a) { |
53 | ((snapshotMan_->getPrevSnapshot())->*storage_).aMat[localIndex_] = a; | |
54 | < | //((snapshotMan_->getPrevSnapshot())->*storage_).electroFrame[localIndex_] = a.transpose() * sU_; |
54 | < | |
54 | > | |
55 | for (int i =0 ; i < atoms_.size(); ++i){ | |
56 | < | if (atoms_[i]->isDirectional()) { |
57 | < | atoms_[i]->setPrevA(a * refOrients_[i]); |
58 | < | } |
56 | > | if (atoms_[i]->isDirectional()) { |
57 | > | atoms_[i]->setPrevA(refOrients_[i].transpose() * a); |
58 | > | } |
59 | } | |
60 | < | |
61 | < | } |
62 | < | |
63 | < | |
64 | < | void RigidBody::setA(const RotMat3x3d& a) { |
60 | > | |
61 | > | } |
62 | > | |
63 | > | |
64 | > | void RigidBody::setA(const RotMat3x3d& a) { |
65 | ((snapshotMan_->getCurrentSnapshot())->*storage_).aMat[localIndex_] = a; | |
66 | – | //((snapshotMan_->getCurrentSnapshot())->*storage_).electroFrame[localIndex_] = a.transpose() * sU_; |
66 | ||
67 | for (int i =0 ; i < atoms_.size(); ++i){ | |
68 | < | if (atoms_[i]->isDirectional()) { |
69 | < | atoms_[i]->setA(a * refOrients_[i]); |
70 | < | } |
68 | > | if (atoms_[i]->isDirectional()) { |
69 | > | atoms_[i]->setA(refOrients_[i].transpose() * a); |
70 | > | } |
71 | } | |
72 | < | } |
73 | < | |
74 | < | void RigidBody::setA(const RotMat3x3d& a, int snapshotNo) { |
72 | > | } |
73 | > | |
74 | > | void RigidBody::setA(const RotMat3x3d& a, int snapshotNo) { |
75 | ((snapshotMan_->getSnapshot(snapshotNo))->*storage_).aMat[localIndex_] = a; | |
76 | + | |
77 | //((snapshotMan_->getSnapshot(snapshotNo))->*storage_).electroFrame[localIndex_] = a.transpose() * sU_; | |
78 | < | |
78 | > | |
79 | for (int i =0 ; i < atoms_.size(); ++i){ | |
80 | < | if (atoms_[i]->isDirectional()) { |
81 | < | atoms_[i]->setA(a * refOrients_[i], snapshotNo); |
82 | < | } |
80 | > | if (atoms_[i]->isDirectional()) { |
81 | > | atoms_[i]->setA(refOrients_[i].transpose() * a, snapshotNo); |
82 | > | } |
83 | } | |
84 | < | |
85 | < | } |
86 | < | |
87 | < | Mat3x3d RigidBody::getI() { |
84 | > | |
85 | > | } |
86 | > | |
87 | > | Mat3x3d RigidBody::getI() { |
88 | return inertiaTensor_; | |
89 | < | } |
90 | < | |
91 | < | std::vector<double> RigidBody::getGrad() { |
92 | < | std::vector<double> grad(6, 0.0); |
89 | > | } |
90 | > | |
91 | > | std::vector<RealType> RigidBody::getGrad() { |
92 | > | std::vector<RealType> grad(6, 0.0); |
93 | Vector3d force; | |
94 | Vector3d torque; | |
95 | Vector3d myEuler; | |
96 | < | double phi, theta, psi; |
97 | < | double cphi, sphi, ctheta, stheta; |
96 | > | RealType phi, theta, psi; |
97 | > | RealType cphi, sphi, ctheta, stheta; |
98 | Vector3d ephi; | |
99 | Vector3d etheta; | |
100 | Vector3d epsi; | |
101 | < | |
101 | > | |
102 | force = getFrc(); | |
103 | torque =getTrq(); | |
104 | myEuler = getA().toEulerAngles(); | |
105 | < | |
105 | > | |
106 | phi = myEuler[0]; | |
107 | theta = myEuler[1]; | |
108 | psi = myEuler[2]; | |
109 | < | |
109 | > | |
110 | cphi = cos(phi); | |
111 | sphi = sin(phi); | |
112 | ctheta = cos(theta); | |
113 | stheta = sin(theta); | |
114 | < | |
114 | > | |
115 | // get unit vectors along the phi, theta and psi rotation axes | |
116 | < | |
116 | > | |
117 | ephi[0] = 0.0; | |
118 | ephi[1] = 0.0; | |
119 | ephi[2] = 1.0; | |
120 | < | |
121 | < | etheta[0] = cphi; |
122 | < | etheta[1] = sphi; |
123 | < | etheta[2] = 0.0; |
124 | < | |
120 | > | |
121 | > | etheta[0] = -sphi; |
122 | > | etheta[1] = cphi; |
123 | > | etheta[2] = 0.0; |
124 | > | |
125 | epsi[0] = stheta * cphi; | |
126 | epsi[1] = stheta * sphi; | |
127 | epsi[2] = ctheta; | |
128 | < | |
128 | > | |
129 | //gradient is equal to -force | |
130 | for (int j = 0 ; j<3; j++) | |
131 | < | grad[j] = -force[j]; |
132 | < | |
131 | > | grad[j] = -force[j]; |
132 | > | |
133 | for (int j = 0; j < 3; j++ ) { | |
134 | < | |
135 | < | grad[3] += torque[j]*ephi[j]; |
136 | < | grad[4] += torque[j]*etheta[j]; |
137 | < | grad[5] += torque[j]*epsi[j]; |
138 | < | |
134 | > | |
135 | > | grad[3] += torque[j]*ephi[j]; |
136 | > | grad[4] += torque[j]*etheta[j]; |
137 | > | grad[5] += torque[j]*epsi[j]; |
138 | > | |
139 | } | |
140 | ||
141 | return grad; | |
142 | < | } |
143 | < | |
144 | < | void RigidBody::accept(BaseVisitor* v) { |
142 | > | } |
143 | > | |
144 | > | void RigidBody::accept(BaseVisitor* v) { |
145 | v->visit(this); | |
146 | < | } |
146 | > | } |
147 | ||
148 | < | /**@todo need modification */ |
149 | < | void RigidBody::calcRefCoords() { |
150 | < | double mtmp; |
148 | > | /**@todo need modification */ |
149 | > | void RigidBody::calcRefCoords() { |
150 | > | RealType mtmp; |
151 | Vector3d refCOM(0.0); | |
152 | mass_ = 0.0; | |
153 | for (std::size_t i = 0; i < atoms_.size(); ++i) { | |
154 | < | mtmp = atoms_[i]->getMass(); |
155 | < | mass_ += mtmp; |
156 | < | refCOM += refCoords_[i]*mtmp; |
154 | > | mtmp = atoms_[i]->getMass(); |
155 | > | mass_ += mtmp; |
156 | > | refCOM += refCoords_[i]*mtmp; |
157 | } | |
158 | refCOM /= mass_; | |
159 | < | |
159 | > | |
160 | // Next, move the origin of the reference coordinate system to the COM: | |
161 | for (std::size_t i = 0; i < atoms_.size(); ++i) { | |
162 | < | refCoords_[i] -= refCOM; |
162 | > | refCoords_[i] -= refCOM; |
163 | } | |
164 | ||
165 | < | // Moment of Inertia calculation |
166 | < | Mat3x3d Itmp(0.0); |
167 | < | |
165 | > | // Moment of Inertia calculation |
166 | > | Mat3x3d Itmp(0.0); |
167 | for (std::size_t i = 0; i < atoms_.size(); i++) { | |
168 | < | mtmp = atoms_[i]->getMass(); |
169 | < | Itmp -= outProduct(refCoords_[i], refCoords_[i]) * mtmp; |
170 | < | double r2 = refCoords_[i].lengthSquare(); |
171 | < | Itmp(0, 0) += mtmp * r2; |
172 | < | Itmp(1, 1) += mtmp * r2; |
173 | < | Itmp(2, 2) += mtmp * r2; |
168 | > | Mat3x3d IAtom(0.0); |
169 | > | mtmp = atoms_[i]->getMass(); |
170 | > | IAtom -= outProduct(refCoords_[i], refCoords_[i]) * mtmp; |
171 | > | RealType r2 = refCoords_[i].lengthSquare(); |
172 | > | IAtom(0, 0) += mtmp * r2; |
173 | > | IAtom(1, 1) += mtmp * r2; |
174 | > | IAtom(2, 2) += mtmp * r2; |
175 | > | Itmp += IAtom; |
176 | > | |
177 | > | //project the inertial moment of directional atoms into this rigid body |
178 | > | if (atoms_[i]->isDirectional()) { |
179 | > | Itmp += refOrients_[i].transpose() * atoms_[i]->getI() * refOrients_[i]; |
180 | > | } |
181 | } | |
182 | ||
183 | < | //project the inertial moment of directional atoms into this rigid body |
178 | < | for (std::size_t i = 0; i < atoms_.size(); i++) { |
179 | < | if (atoms_[i]->isDirectional()) { |
180 | < | RectMatrix<double, 3, 3> Iproject = refOrients_[i].transpose() * atoms_[i]->getI(); |
181 | < | Itmp(0, 0) += Iproject(0, 0); |
182 | < | Itmp(1, 1) += Iproject(1, 1); |
183 | < | Itmp(2, 2) += Iproject(2, 2); |
184 | < | } |
185 | < | } |
183 | > | // std::cout << Itmp << std::endl; |
184 | ||
185 | //diagonalize | |
186 | Vector3d evals; | |
# | Line 195 | Line 193 | void RigidBody::calcRefCoords() { | |
193 | ||
194 | int nLinearAxis = 0; | |
195 | for (int i = 0; i < 3; i++) { | |
196 | < | if (fabs(evals[i]) < oopse::epsilon) { |
197 | < | linear_ = true; |
198 | < | linearAxis_ = i; |
199 | < | ++ nLinearAxis; |
200 | < | } |
196 | > | if (fabs(evals[i]) < OpenMD::epsilon) { |
197 | > | linear_ = true; |
198 | > | linearAxis_ = i; |
199 | > | ++ nLinearAxis; |
200 | > | } |
201 | } | |
202 | ||
203 | if (nLinearAxis > 1) { | |
204 | < | sprintf( painCave.errMsg, |
205 | < | "RigidBody error.\n" |
206 | < | "\tOOPSE found more than one axis in this rigid body with a vanishing \n" |
207 | < | "\tmoment of inertia. This can happen in one of three ways:\n" |
208 | < | "\t 1) Only one atom was specified, or \n" |
209 | < | "\t 2) All atoms were specified at the same location, or\n" |
210 | < | "\t 3) The programmers did something stupid.\n" |
211 | < | "\tIt is silly to use a rigid body to describe this situation. Be smarter.\n" |
212 | < | ); |
213 | < | painCave.isFatal = 1; |
214 | < | simError(); |
204 | > | sprintf( painCave.errMsg, |
205 | > | "RigidBody error.\n" |
206 | > | "\tOpenMD found more than one axis in this rigid body with a vanishing \n" |
207 | > | "\tmoment of inertia. This can happen in one of three ways:\n" |
208 | > | "\t 1) Only one atom was specified, or \n" |
209 | > | "\t 2) All atoms were specified at the same location, or\n" |
210 | > | "\t 3) The programmers did something stupid.\n" |
211 | > | "\tIt is silly to use a rigid body to describe this situation. Be smarter.\n" |
212 | > | ); |
213 | > | painCave.isFatal = 1; |
214 | > | simError(); |
215 | } | |
216 | ||
217 | < | } |
217 | > | } |
218 | ||
219 | < | void RigidBody::calcForcesAndTorques() { |
219 | > | void RigidBody::calcForcesAndTorques() { |
220 | Vector3d afrc; | |
221 | Vector3d atrq; | |
222 | Vector3d apos; | |
223 | Vector3d rpos; | |
224 | Vector3d frc(0.0); | |
225 | < | Vector3d trq(0.0); |
225 | > | Vector3d trq(0.0); |
226 | Vector3d pos = this->getPos(); | |
227 | for (int i = 0; i < atoms_.size(); i++) { | |
228 | ||
229 | < | afrc = atoms_[i]->getFrc(); |
230 | < | apos = atoms_[i]->getPos(); |
231 | < | rpos = apos - pos; |
229 | > | afrc = atoms_[i]->getFrc(); |
230 | > | apos = atoms_[i]->getPos(); |
231 | > | rpos = apos - pos; |
232 | ||
233 | < | frc += afrc; |
233 | > | frc += afrc; |
234 | ||
235 | < | trq[0] += rpos[1]*afrc[2] - rpos[2]*afrc[1]; |
236 | < | trq[1] += rpos[2]*afrc[0] - rpos[0]*afrc[2]; |
237 | < | trq[2] += rpos[0]*afrc[1] - rpos[1]*afrc[0]; |
235 | > | trq[0] += rpos[1]*afrc[2] - rpos[2]*afrc[1]; |
236 | > | trq[1] += rpos[2]*afrc[0] - rpos[0]*afrc[2]; |
237 | > | trq[2] += rpos[0]*afrc[1] - rpos[1]*afrc[0]; |
238 | ||
239 | < | // If the atom has a torque associated with it, then we also need to |
240 | < | // migrate the torques onto the center of mass: |
239 | > | // If the atom has a torque associated with it, then we also need to |
240 | > | // migrate the torques onto the center of mass: |
241 | ||
242 | < | if (atoms_[i]->isDirectional()) { |
243 | < | atrq = atoms_[i]->getTrq(); |
244 | < | trq += atrq; |
245 | < | } |
242 | > | if (atoms_[i]->isDirectional()) { |
243 | > | atrq = atoms_[i]->getTrq(); |
244 | > | trq += atrq; |
245 | > | } |
246 | > | } |
247 | > | addFrc(frc); |
248 | > | addTrq(trq); |
249 | > | } |
250 | > | |
251 | > | Mat3x3d RigidBody::calcForcesAndTorquesAndVirial() { |
252 | > | Vector3d afrc; |
253 | > | Vector3d atrq; |
254 | > | Vector3d apos; |
255 | > | Vector3d rpos; |
256 | > | Vector3d dfrc; |
257 | > | Vector3d frc(0.0); |
258 | > | Vector3d trq(0.0); |
259 | > | Vector3d pos = this->getPos(); |
260 | > | Mat3x3d tau_(0.0); |
261 | > | |
262 | > | for (int i = 0; i < atoms_.size(); i++) { |
263 | > | |
264 | > | afrc = atoms_[i]->getFrc(); |
265 | > | apos = atoms_[i]->getPos(); |
266 | > | rpos = apos - pos; |
267 | ||
268 | + | frc += afrc; |
269 | + | |
270 | + | trq[0] += rpos[1]*afrc[2] - rpos[2]*afrc[1]; |
271 | + | trq[1] += rpos[2]*afrc[0] - rpos[0]*afrc[2]; |
272 | + | trq[2] += rpos[0]*afrc[1] - rpos[1]*afrc[0]; |
273 | + | |
274 | + | // If the atom has a torque associated with it, then we also need to |
275 | + | // migrate the torques onto the center of mass: |
276 | + | |
277 | + | if (atoms_[i]->isDirectional()) { |
278 | + | atrq = atoms_[i]->getTrq(); |
279 | + | trq += atrq; |
280 | + | } |
281 | + | |
282 | + | tau_(0,0) -= rpos[0]*afrc[0]; |
283 | + | tau_(0,1) -= rpos[0]*afrc[1]; |
284 | + | tau_(0,2) -= rpos[0]*afrc[2]; |
285 | + | tau_(1,0) -= rpos[1]*afrc[0]; |
286 | + | tau_(1,1) -= rpos[1]*afrc[1]; |
287 | + | tau_(1,2) -= rpos[1]*afrc[2]; |
288 | + | tau_(2,0) -= rpos[2]*afrc[0]; |
289 | + | tau_(2,1) -= rpos[2]*afrc[1]; |
290 | + | tau_(2,2) -= rpos[2]*afrc[2]; |
291 | + | |
292 | } | |
293 | < | |
294 | < | setFrc(frc); |
295 | < | setTrq(trq); |
296 | < | |
254 | < | } |
293 | > | addFrc(frc); |
294 | > | addTrq(trq); |
295 | > | return tau_; |
296 | > | } |
297 | ||
298 | < | void RigidBody::updateAtoms() { |
298 | > | void RigidBody::updateAtoms() { |
299 | unsigned int i; | |
300 | Vector3d ref; | |
301 | Vector3d apos; | |
# | Line 263 | Line 305 | void RigidBody::updateAtoms() { | |
305 | ||
306 | for (i = 0; i < atoms_.size(); i++) { | |
307 | ||
308 | < | ref = body2Lab(refCoords_[i]); |
308 | > | ref = body2Lab(refCoords_[i]); |
309 | ||
310 | < | apos = pos + ref; |
310 | > | apos = pos + ref; |
311 | ||
312 | < | atoms_[i]->setPos(apos); |
312 | > | atoms_[i]->setPos(apos); |
313 | ||
314 | < | if (atoms_[i]->isDirectional()) { |
314 | > | if (atoms_[i]->isDirectional()) { |
315 | ||
316 | < | dAtom = (DirectionalAtom *) atoms_[i]; |
317 | < | dAtom->setA(a * refOrients_[i]); |
318 | < | //dAtom->rotateBy( A ); |
277 | < | } |
316 | > | dAtom = (DirectionalAtom *) atoms_[i]; |
317 | > | dAtom->setA(refOrients_[i].transpose() * a); |
318 | > | } |
319 | ||
320 | } | |
321 | ||
322 | < | } |
322 | > | } |
323 | ||
324 | ||
325 | < | bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) { |
325 | > | void RigidBody::updateAtoms(int frame) { |
326 | > | unsigned int i; |
327 | > | Vector3d ref; |
328 | > | Vector3d apos; |
329 | > | DirectionalAtom* dAtom; |
330 | > | Vector3d pos = getPos(frame); |
331 | > | RotMat3x3d a = getA(frame); |
332 | > | |
333 | > | for (i = 0; i < atoms_.size(); i++) { |
334 | > | |
335 | > | ref = body2Lab(refCoords_[i], frame); |
336 | > | |
337 | > | apos = pos + ref; |
338 | > | |
339 | > | atoms_[i]->setPos(apos, frame); |
340 | > | |
341 | > | if (atoms_[i]->isDirectional()) { |
342 | > | |
343 | > | dAtom = (DirectionalAtom *) atoms_[i]; |
344 | > | dAtom->setA(refOrients_[i].transpose() * a, frame); |
345 | > | } |
346 | > | |
347 | > | } |
348 | > | |
349 | > | } |
350 | > | |
351 | > | void RigidBody::updateAtomVel() { |
352 | > | Mat3x3d skewMat;; |
353 | > | |
354 | > | Vector3d ji = getJ(); |
355 | > | Mat3x3d I = getI(); |
356 | > | |
357 | > | skewMat(0, 0) =0; |
358 | > | skewMat(0, 1) = ji[2] /I(2, 2); |
359 | > | skewMat(0, 2) = -ji[1] /I(1, 1); |
360 | > | |
361 | > | skewMat(1, 0) = -ji[2] /I(2, 2); |
362 | > | skewMat(1, 1) = 0; |
363 | > | skewMat(1, 2) = ji[0]/I(0, 0); |
364 | > | |
365 | > | skewMat(2, 0) =ji[1] /I(1, 1); |
366 | > | skewMat(2, 1) = -ji[0]/I(0, 0); |
367 | > | skewMat(2, 2) = 0; |
368 | > | |
369 | > | Mat3x3d mat = (getA() * skewMat).transpose(); |
370 | > | Vector3d rbVel = getVel(); |
371 | > | |
372 | > | |
373 | > | Vector3d velRot; |
374 | > | for (int i =0 ; i < refCoords_.size(); ++i) { |
375 | > | atoms_[i]->setVel(rbVel + mat * refCoords_[i]); |
376 | > | } |
377 | > | |
378 | > | } |
379 | > | |
380 | > | void RigidBody::updateAtomVel(int frame) { |
381 | > | Mat3x3d skewMat;; |
382 | > | |
383 | > | Vector3d ji = getJ(frame); |
384 | > | Mat3x3d I = getI(); |
385 | > | |
386 | > | skewMat(0, 0) =0; |
387 | > | skewMat(0, 1) = ji[2] /I(2, 2); |
388 | > | skewMat(0, 2) = -ji[1] /I(1, 1); |
389 | > | |
390 | > | skewMat(1, 0) = -ji[2] /I(2, 2); |
391 | > | skewMat(1, 1) = 0; |
392 | > | skewMat(1, 2) = ji[0]/I(0, 0); |
393 | > | |
394 | > | skewMat(2, 0) =ji[1] /I(1, 1); |
395 | > | skewMat(2, 1) = -ji[0]/I(0, 0); |
396 | > | skewMat(2, 2) = 0; |
397 | > | |
398 | > | Mat3x3d mat = (getA(frame) * skewMat).transpose(); |
399 | > | Vector3d rbVel = getVel(frame); |
400 | > | |
401 | > | |
402 | > | Vector3d velRot; |
403 | > | for (int i =0 ; i < refCoords_.size(); ++i) { |
404 | > | atoms_[i]->setVel(rbVel + mat * refCoords_[i], frame); |
405 | > | } |
406 | > | |
407 | > | } |
408 | > | |
409 | > | |
410 | > | |
411 | > | bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) { |
412 | if (index < atoms_.size()) { | |
413 | ||
414 | < | Vector3d ref = body2Lab(refCoords_[index]); |
415 | < | pos = getPos() + ref; |
416 | < | return true; |
414 | > | Vector3d ref = body2Lab(refCoords_[index]); |
415 | > | pos = getPos() + ref; |
416 | > | return true; |
417 | } else { | |
418 | < | std::cerr << index << " is an invalid index, current rigid body contains " |
419 | < | << atoms_.size() << "atoms" << std::endl; |
420 | < | return false; |
418 | > | std::cerr << index << " is an invalid index, current rigid body contains " |
419 | > | << atoms_.size() << "atoms" << std::endl; |
420 | > | return false; |
421 | } | |
422 | < | } |
422 | > | } |
423 | ||
424 | < | bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) { |
424 | > | bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) { |
425 | std::vector<Atom*>::iterator i; | |
426 | i = std::find(atoms_.begin(), atoms_.end(), atom); | |
427 | if (i != atoms_.end()) { | |
428 | < | //RigidBody class makes sure refCoords_ and atoms_ match each other |
429 | < | Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]); |
430 | < | pos = getPos() + ref; |
431 | < | return true; |
428 | > | //RigidBody class makes sure refCoords_ and atoms_ match each other |
429 | > | Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]); |
430 | > | pos = getPos() + ref; |
431 | > | return true; |
432 | } else { | |
433 | < | std::cerr << "Atom " << atom->getGlobalIndex() |
434 | < | <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl; |
435 | < | return false; |
433 | > | std::cerr << "Atom " << atom->getGlobalIndex() |
434 | > | <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl; |
435 | > | return false; |
436 | } | |
437 | < | } |
438 | < | bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) { |
437 | > | } |
438 | > | bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) { |
439 | ||
440 | //velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$ | |
441 | ||
442 | if (index < atoms_.size()) { | |
443 | ||
444 | < | Vector3d velRot; |
445 | < | Mat3x3d skewMat;; |
446 | < | Vector3d ref = refCoords_[index]; |
447 | < | Vector3d ji = getJ(); |
448 | < | Mat3x3d I = getI(); |
444 | > | Vector3d velRot; |
445 | > | Mat3x3d skewMat;; |
446 | > | Vector3d ref = refCoords_[index]; |
447 | > | Vector3d ji = getJ(); |
448 | > | Mat3x3d I = getI(); |
449 | ||
450 | < | skewMat(0, 0) =0; |
451 | < | skewMat(0, 1) = ji[2] /I(2, 2); |
452 | < | skewMat(0, 2) = -ji[1] /I(1, 1); |
450 | > | skewMat(0, 0) =0; |
451 | > | skewMat(0, 1) = ji[2] /I(2, 2); |
452 | > | skewMat(0, 2) = -ji[1] /I(1, 1); |
453 | ||
454 | < | skewMat(1, 0) = -ji[2] /I(2, 2); |
455 | < | skewMat(1, 1) = 0; |
456 | < | skewMat(1, 2) = ji[0]/I(0, 0); |
454 | > | skewMat(1, 0) = -ji[2] /I(2, 2); |
455 | > | skewMat(1, 1) = 0; |
456 | > | skewMat(1, 2) = ji[0]/I(0, 0); |
457 | ||
458 | < | skewMat(2, 0) =ji[1] /I(1, 1); |
459 | < | skewMat(2, 1) = -ji[0]/I(0, 0); |
460 | < | skewMat(2, 2) = 0; |
458 | > | skewMat(2, 0) =ji[1] /I(1, 1); |
459 | > | skewMat(2, 1) = -ji[0]/I(0, 0); |
460 | > | skewMat(2, 2) = 0; |
461 | ||
462 | < | velRot = (getA() * skewMat).transpose() * ref; |
462 | > | velRot = (getA() * skewMat).transpose() * ref; |
463 | ||
464 | < | vel =getVel() + velRot; |
465 | < | return true; |
464 | > | vel =getVel() + velRot; |
465 | > | return true; |
466 | ||
467 | } else { | |
468 | < | std::cerr << index << " is an invalid index, current rigid body contains " |
469 | < | << atoms_.size() << "atoms" << std::endl; |
470 | < | return false; |
468 | > | std::cerr << index << " is an invalid index, current rigid body contains " |
469 | > | << atoms_.size() << "atoms" << std::endl; |
470 | > | return false; |
471 | } | |
472 | < | } |
472 | > | } |
473 | ||
474 | < | bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) { |
474 | > | bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) { |
475 | ||
476 | std::vector<Atom*>::iterator i; | |
477 | i = std::find(atoms_.begin(), atoms_.end(), atom); | |
478 | if (i != atoms_.end()) { | |
479 | < | return getAtomVel(vel, i - atoms_.begin()); |
479 | > | return getAtomVel(vel, i - atoms_.begin()); |
480 | } else { | |
481 | < | std::cerr << "Atom " << atom->getGlobalIndex() |
482 | < | <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl; |
483 | < | return false; |
481 | > | std::cerr << "Atom " << atom->getGlobalIndex() |
482 | > | <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl; |
483 | > | return false; |
484 | } | |
485 | < | } |
485 | > | } |
486 | ||
487 | < | bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) { |
487 | > | bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) { |
488 | if (index < atoms_.size()) { | |
489 | ||
490 | < | coor = refCoords_[index]; |
491 | < | return true; |
490 | > | coor = refCoords_[index]; |
491 | > | return true; |
492 | } else { | |
493 | < | std::cerr << index << " is an invalid index, current rigid body contains " |
494 | < | << atoms_.size() << "atoms" << std::endl; |
495 | < | return false; |
493 | > | std::cerr << index << " is an invalid index, current rigid body contains " |
494 | > | << atoms_.size() << "atoms" << std::endl; |
495 | > | return false; |
496 | } | |
497 | ||
498 | < | } |
498 | > | } |
499 | ||
500 | < | bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) { |
500 | > | bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) { |
501 | std::vector<Atom*>::iterator i; | |
502 | i = std::find(atoms_.begin(), atoms_.end(), atom); | |
503 | if (i != atoms_.end()) { | |
504 | < | //RigidBody class makes sure refCoords_ and atoms_ match each other |
505 | < | coor = refCoords_[i - atoms_.begin()]; |
506 | < | return true; |
504 | > | //RigidBody class makes sure refCoords_ and atoms_ match each other |
505 | > | coor = refCoords_[i - atoms_.begin()]; |
506 | > | return true; |
507 | } else { | |
508 | < | std::cerr << "Atom " << atom->getGlobalIndex() |
509 | < | <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl; |
510 | < | return false; |
508 | > | std::cerr << "Atom " << atom->getGlobalIndex() |
509 | > | <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl; |
510 | > | return false; |
511 | } | |
512 | ||
513 | < | } |
513 | > | } |
514 | ||
515 | ||
516 | < | void RigidBody::addAtom(Atom* at, AtomStamp* ats) { |
516 | > | void RigidBody::addAtom(Atom* at, AtomStamp* ats) { |
517 | ||
518 | < | Vector3d coords; |
519 | < | Vector3d euler; |
518 | > | Vector3d coords; |
519 | > | Vector3d euler; |
520 | ||
521 | ||
522 | < | atoms_.push_back(at); |
522 | > | atoms_.push_back(at); |
523 | ||
524 | < | if( !ats->havePosition() ){ |
525 | < | sprintf( painCave.errMsg, |
526 | < | "RigidBody error.\n" |
527 | < | "\tAtom %s does not have a position specified.\n" |
528 | < | "\tThis means RigidBody cannot set up reference coordinates.\n", |
529 | < | ats->getType() ); |
530 | < | painCave.isFatal = 1; |
531 | < | simError(); |
532 | < | } |
524 | > | if( !ats->havePosition() ){ |
525 | > | sprintf( painCave.errMsg, |
526 | > | "RigidBody error.\n" |
527 | > | "\tAtom %s does not have a position specified.\n" |
528 | > | "\tThis means RigidBody cannot set up reference coordinates.\n", |
529 | > | ats->getType().c_str() ); |
530 | > | painCave.isFatal = 1; |
531 | > | simError(); |
532 | > | } |
533 | ||
534 | < | coords[0] = ats->getPosX(); |
535 | < | coords[1] = ats->getPosY(); |
536 | < | coords[2] = ats->getPosZ(); |
534 | > | coords[0] = ats->getPosX(); |
535 | > | coords[1] = ats->getPosY(); |
536 | > | coords[2] = ats->getPosZ(); |
537 | ||
538 | < | refCoords_.push_back(coords); |
538 | > | refCoords_.push_back(coords); |
539 | ||
540 | < | RotMat3x3d identMat = RotMat3x3d::identity(); |
540 | > | RotMat3x3d identMat = RotMat3x3d::identity(); |
541 | ||
542 | < | if (at->isDirectional()) { |
542 | > | if (at->isDirectional()) { |
543 | ||
544 | < | if( !ats->haveOrientation() ){ |
545 | < | sprintf( painCave.errMsg, |
546 | < | "RigidBody error.\n" |
547 | < | "\tAtom %s does not have an orientation specified.\n" |
548 | < | "\tThis means RigidBody cannot set up reference orientations.\n", |
549 | < | ats->getType() ); |
550 | < | painCave.isFatal = 1; |
551 | < | simError(); |
552 | < | } |
544 | > | if( !ats->haveOrientation() ){ |
545 | > | sprintf( painCave.errMsg, |
546 | > | "RigidBody error.\n" |
547 | > | "\tAtom %s does not have an orientation specified.\n" |
548 | > | "\tThis means RigidBody cannot set up reference orientations.\n", |
549 | > | ats->getType().c_str() ); |
550 | > | painCave.isFatal = 1; |
551 | > | simError(); |
552 | > | } |
553 | ||
554 | < | euler[0] = ats->getEulerPhi(); |
555 | < | euler[1] = ats->getEulerTheta(); |
556 | < | euler[2] = ats->getEulerPsi(); |
554 | > | euler[0] = ats->getEulerPhi() * NumericConstant::PI /180.0; |
555 | > | euler[1] = ats->getEulerTheta() * NumericConstant::PI /180.0; |
556 | > | euler[2] = ats->getEulerPsi() * NumericConstant::PI /180.0; |
557 | ||
558 | < | RotMat3x3d Atmp(euler); |
559 | < | refOrients_.push_back(Atmp); |
558 | > | RotMat3x3d Atmp(euler); |
559 | > | refOrients_.push_back(Atmp); |
560 | ||
561 | < | }else { |
562 | < | refOrients_.push_back(identMat); |
563 | < | } |
561 | > | }else { |
562 | > | refOrients_.push_back(identMat); |
563 | > | } |
564 | ||
565 | ||
566 | < | } |
566 | > | } |
567 | ||
568 | } | |
569 |
– | Removed lines |
+ | Added lines |
< | Changed lines |
> | Changed lines |