# | Line 1 | Line 1 | |
---|---|---|
1 | /* | |
2 | < | * Copyright (C) 2000-2004 Object Oriented Parallel Simulation Engine (OOPSE) project |
2 | > | * Copyright (C) 2000-2009 The Open Molecular Dynamics Engine (OpenMD) project |
3 | * | |
4 | < | * Contact: oopse@oopse.org |
4 | > | * Contact: gezelter@openscience.org |
5 | * | |
6 | * This program is free software; you can redistribute it and/or | |
7 | * modify it under the terms of the GNU Lesser General Public License | |
# | Line 25 | Line 25 | |
25 | ||
26 | #include "primitives/RigidBody.hpp" | |
27 | ||
28 | < | namespace oopse { |
28 | > | namespace OpenMD { |
29 | ||
30 | < | RigidBody::RigidBody() : objType_(otRigidBody), storage_(&Snapshot::rigidbodyData){ |
30 | > | RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData){ |
31 | ||
32 | } | |
33 | ||
34 | void RigidBody::setPrevA(const RotMat3x3d& a) { | |
35 | < | (snapshotMan_->getPrevSnapshot())->storage_->aMat[localIndex_] = a; |
36 | < | (snapshotMan_->getPrevSnapshot())->storage_->unitVector[localIndex_] = a.inverse() * sU_.getColum(2); |
35 | > | ((snapshotMan_->getPrevSnapshot())->*storage_).aMat[localIndex_] = a; |
36 | > | ((snapshotMan_->getPrevSnapshot())->*storage_).unitVector[localIndex_] = a.inverse() * sU_.getColumn(2); |
37 | > | |
38 | > | std::vector<Atom*>::iterator i; |
39 | > | for (i = atoms_.begin(); i != atoms_.end(); ++i) { |
40 | > | if ((*i)->isDirectional()) { |
41 | > | (*i)->setPrevA(a * (*i)->getPrevA()); |
42 | > | } |
43 | > | } |
44 | > | |
45 | } | |
46 | ||
47 | ||
48 | void RigidBody::setA(const RotMat3x3d& a) { | |
49 | < | (snapshotMan_->getCurrentSnapshot())->storage_->aMat[localIndex_] = a; |
50 | < | (snapshotMan_->getCurrentSnapshot())->storage_->unitVector[localIndex_] = a.inverse() * sU_.getColum(2); |
49 | > | ((snapshotMan_->getCurrentSnapshot())->*storage_).aMat[localIndex_] = a; |
50 | > | ((snapshotMan_->getCurrentSnapshot())->*storage_).unitVector[localIndex_] = a.inverse() * sU_.getColumn(2); |
51 | > | |
52 | > | std::vector<Atom*>::iterator i; |
53 | > | for (i = atoms_.begin(); i != atoms_.end(); ++i) { |
54 | > | if ((*i)->isDirectional()) { |
55 | > | (*i)->setA(a * (*i)->getA()); |
56 | > | } |
57 | > | } |
58 | } | |
59 | ||
60 | void RigidBody::setA(const RotMat3x3d& a, int snapshotNo) { | |
61 | < | (snapshotMan_->getSnapshot(snapshotNo))->storage_->aMat[localIndex_] = a; |
62 | < | (snapshotMan_->getSnapshot(snapshotNo))->storage_->unitVector[localIndex_] = a.inverse() * sU_.getColum(2); |
61 | > | ((snapshotMan_->getSnapshot(snapshotNo))->*storage_).aMat[localIndex_] = a; |
62 | > | ((snapshotMan_->getSnapshot(snapshotNo))->*storage_).unitVector[localIndex_] = a.inverse() * sU_.getColumn(2); |
63 | > | |
64 | > | std::vector<Atom*>::iterator i; |
65 | > | for (i = atoms_.begin(); i != atoms_.end(); ++i) { |
66 | > | if ((*i)->isDirectional()) { |
67 | > | (*i)->setA(a * (*i)->getA(snapshotNo), snapshotNo); |
68 | > | } |
69 | > | } |
70 | > | |
71 | } | |
72 | ||
73 | + | void DirectionalAtom::setUnitFrameFromEuler(double phi, double theta, double psi) { |
74 | + | sU_.setupRotMat(phi,theta,psi); |
75 | + | } |
76 | + | |
77 | Mat3x3d RigidBody::getI() { | |
78 | return inertiaTensor_; | |
79 | } | |
80 | ||
54 | – | void RigidBody::setI(Mat3x3d& I) { |
55 | – | inertiaTensor_ = I; |
56 | – | } |
57 | – | |
81 | std::vector<double> RigidBody::getGrad() { | |
82 | vector<double> grad(6, 0.0); | |
83 | Vector3d force; | |
# | Line 112 | Line 135 | void RigidBody::accept(BaseVisitor* v) { | |
135 | v->visit(this); | |
136 | } | |
137 | ||
138 | + | void RigidBody::calcRefCoords() { |
139 | + | /* |
140 | + | double mtmp; |
141 | + | vec3 apos; |
142 | + | double refCOM[3]; |
143 | + | vec3 ptmp; |
144 | + | double Itmp[3][3]; |
145 | + | double evals[3]; |
146 | + | double evects[3][3]; |
147 | + | double r, r2, len; |
148 | + | |
149 | + | // First, find the center of mass: |
150 | + | |
151 | + | mass = 0.0; |
152 | + | for (j=0; j<3; j++) |
153 | + | refCOM[j] = 0.0; |
154 | + | |
155 | + | for (i = 0; i < atoms_.size(); i++) { |
156 | + | mtmp = atoms_[i]->getMass(); |
157 | + | mass += mtmp; |
158 | + | |
159 | + | apos = refCoords[i]; |
160 | + | |
161 | + | for(j = 0; j < 3; j++) { |
162 | + | refCOM[j] += apos[j]*mtmp; |
163 | + | } |
164 | + | } |
165 | + | |
166 | + | for(j = 0; j < 3; j++) |
167 | + | refCOM[j] /= mass; |
168 | + | |
169 | + | // Next, move the origin of the reference coordinate system to the COM: |
170 | + | |
171 | + | for (i = 0; i < atoms_.size(); i++) { |
172 | + | apos = refCoords[i]; |
173 | + | for (j=0; j < 3; j++) { |
174 | + | apos[j] = apos[j] - refCOM[j]; |
175 | + | } |
176 | + | refCoords[i] = apos; |
177 | + | } |
178 | + | |
179 | + | // Moment of Inertia calculation |
180 | + | |
181 | + | for (i = 0; i < 3; i++) |
182 | + | for (j = 0; j < 3; j++) |
183 | + | Itmp[i][j] = 0.0; |
184 | + | |
185 | + | for (it = 0; it < atoms_.size(); it++) { |
186 | + | |
187 | + | mtmp = atoms_[it]->getMass(); |
188 | + | ptmp = refCoords[it]; |
189 | + | r= norm3(ptmp.vec); |
190 | + | r2 = r*r; |
191 | + | |
192 | + | for (i = 0; i < 3; i++) { |
193 | + | for (j = 0; j < 3; j++) { |
194 | + | |
195 | + | if (i==j) Itmp[i][j] += mtmp * r2; |
196 | + | |
197 | + | Itmp[i][j] -= mtmp * ptmp.vec[i]*ptmp.vec[j]; |
198 | + | } |
199 | + | } |
200 | + | } |
201 | + | |
202 | + | diagonalize3x3(Itmp, evals, sU); |
203 | + | |
204 | + | // zero out I and then fill the diagonals with the moments of inertia: |
205 | + | |
206 | + | n_linear_coords = 0; |
207 | + | |
208 | + | for (i = 0; i < 3; i++) { |
209 | + | for (j = 0; j < 3; j++) { |
210 | + | I[i][j] = 0.0; |
211 | + | } |
212 | + | I[i][i] = evals[i]; |
213 | + | |
214 | + | if (fabs(evals[i]) < momIntTol) { |
215 | + | is_linear = true; |
216 | + | n_linear_coords++; |
217 | + | linear_axis = i; |
218 | + | } |
219 | + | } |
220 | + | |
221 | + | if (n_linear_coords > 1) { |
222 | + | sprintf( painCave.errMsg, |
223 | + | "RigidBody error.\n" |
224 | + | "\tOpenMD found more than one axis in this rigid body with a vanishing \n" |
225 | + | "\tmoment of inertia. This can happen in one of three ways:\n" |
226 | + | "\t 1) Only one atom was specified, or \n" |
227 | + | "\t 2) All atoms were specified at the same location, or\n" |
228 | + | "\t 3) The programmers did something stupid.\n" |
229 | + | "\tIt is silly to use a rigid body to describe this situation. Be smarter.\n" |
230 | + | ); |
231 | + | painCave.isFatal = 1; |
232 | + | simError(); |
233 | + | } |
234 | + | |
235 | + | // renormalize column vectors: |
236 | + | |
237 | + | for (i=0; i < 3; i++) { |
238 | + | len = 0.0; |
239 | + | for (j = 0; j < 3; j++) { |
240 | + | len += sU[i][j]*sU[i][j]; |
241 | + | } |
242 | + | len = sqrt(len); |
243 | + | for (j = 0; j < 3; j++) { |
244 | + | sU[i][j] /= len; |
245 | + | } |
246 | + | } |
247 | + | */ |
248 | } | |
249 | ||
250 | + | void RigidBody::calcForcesAndTorques() { |
251 | + | unsigned int i; |
252 | + | unsigned int j; |
253 | + | //Vector3d apos; |
254 | + | Vector3d afrc; |
255 | + | Vector3d atrq; |
256 | + | Vector3d rpos; |
257 | + | Vector3d frc; |
258 | + | Vector3d trq; |
259 | + | //Vector3d pos; |
260 | + | |
261 | + | zeroForces(); |
262 | + | |
263 | + | //pos = getPos(); |
264 | + | frc = getFrc(); |
265 | + | trq = getTrq(); |
266 | + | |
267 | + | for (i = 0; i < atoms_.size(); i++) { |
268 | + | |
269 | + | afrc = atoms_[i]->getFrc(); |
270 | + | |
271 | + | //apos = atoms_[i]->getPos(apos); |
272 | + | //rpos = apos - pos; |
273 | + | rpos = refCoords_[i]; |
274 | + | |
275 | + | frc += afrc; |
276 | + | |
277 | + | trq[0] += rpos[1]*afrc[2] - rpos[2]*afrc[1]; |
278 | + | trq[1] += rpos[2]*afrc[0] - rpos[0]*afrc[2]; |
279 | + | trq[2] += rpos[0]*afrc[1] - rpos[1]*afrc[0]; |
280 | + | |
281 | + | // If the atom has a torque associated with it, then we also need to |
282 | + | // migrate the torques onto the center of mass: |
283 | + | |
284 | + | if (atoms_[i]->isDirectional()) { |
285 | + | atrq = atoms_[i]->getTrq(); |
286 | + | trq += atrq; |
287 | + | } |
288 | + | |
289 | + | } |
290 | + | |
291 | + | setFrc(frc); |
292 | + | setTrq(trq); |
293 | + | |
294 | + | } |
295 | + | |
296 | + | void RigidBody::updateAtoms() { |
297 | + | unsigned int i; |
298 | + | unsigned int j; |
299 | + | Vector3d ref; |
300 | + | Vector3d apos; |
301 | + | DirectionalAtom* dAtom; |
302 | + | Vector3d pos = getPos(); |
303 | + | RotMat3x3d A = getA(); |
304 | + | |
305 | + | for (i = 0; i < atoms_.size(); i++) { |
306 | + | |
307 | + | ref = body2Lab(refCoords_[i]); |
308 | + | |
309 | + | apos = pos + ref; |
310 | + | |
311 | + | atoms_[i]->setPos(apos); |
312 | + | |
313 | + | if (atoms_[i]->isDirectional()) { |
314 | + | |
315 | + | dAtom = (DirectionalAtom *) atoms_[i]; |
316 | + | dAtom->rotateBy( A ); |
317 | + | } |
318 | + | |
319 | + | } |
320 | + | |
321 | + | } |
322 | + | |
323 | + | |
324 | + | bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) { |
325 | + | if (index < atoms_.size()) { |
326 | + | |
327 | + | Vector3d ref = body2Lab(refCoords_[index]); |
328 | + | pos = getPos() + ref; |
329 | + | return true; |
330 | + | } else { |
331 | + | std::cerr << index << " is an invalid index, current rigid body contains " |
332 | + | << atoms_.size() << "atoms" << std::endl; |
333 | + | return false; |
334 | + | } |
335 | + | } |
336 | + | |
337 | + | bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) { |
338 | + | std::vector<Atom*>::iterator i; |
339 | + | i = find(atoms_.begin(), atoms_.end(), atom); |
340 | + | if (i != atoms_.end()) { |
341 | + | //RigidBody class makes sure refCoords_ and atoms_ match each other |
342 | + | Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]); |
343 | + | pos = getPos() + ref; |
344 | + | return true; |
345 | + | } else { |
346 | + | std::cerr << "Atom " << atom->getGlobalIndex() |
347 | + | <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl; |
348 | + | return false; |
349 | + | } |
350 | + | } |
351 | + | bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) { |
352 | + | |
353 | + | //velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$ |
354 | + | |
355 | + | if (index < atoms_.size()) { |
356 | + | |
357 | + | Vector3d velRot; |
358 | + | Mat3x3d skewMat;; |
359 | + | Vector3d ref = refCoords_[index]; |
360 | + | Vector3d ji = getJ(); |
361 | + | Mat3x3d I = getI(); |
362 | + | |
363 | + | skewMat(0, 0) =0; |
364 | + | skewMat(0, 1) = ji[2] /I(2, 2); |
365 | + | skewMat(0, 2) = -ji[1] /I(1, 1); |
366 | + | |
367 | + | skewMat(1, 0) = -ji[2] /I(2, 2); |
368 | + | skewMat(1, 1) = 0; |
369 | + | skewMat(1, 2) = ji[0]/I(0, 0); |
370 | + | |
371 | + | skewMat(2, 0) =ji[1] /I(1, 1); |
372 | + | skewMat(2, 1) = -ji[0]/I(0, 0); |
373 | + | skewMat(2, 2) = 0; |
374 | + | |
375 | + | velRot = (getA() * skewMat).transpose() * ref; |
376 | + | |
377 | + | vel =getVel() + velRot; |
378 | + | return true; |
379 | + | |
380 | + | } else { |
381 | + | std::cerr << index << " is an invalid index, current rigid body contains " |
382 | + | << atoms_.size() << "atoms" << std::endl; |
383 | + | return false; |
384 | + | } |
385 | + | } |
386 | + | |
387 | + | bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) { |
388 | + | |
389 | + | std::vector<Atom*>::iterator i; |
390 | + | i = find(atoms_.begin(), atoms_.end(), atom); |
391 | + | if (i != atoms_.end()) { |
392 | + | return getAtomVel(vel, i - atoms_.begin()); |
393 | + | } else { |
394 | + | std::cerr << "Atom " << atom->getGlobalIndex() |
395 | + | <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl; |
396 | + | return false; |
397 | + | } |
398 | + | } |
399 | + | |
400 | + | bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) { |
401 | + | if (index < atoms_.size()) { |
402 | + | |
403 | + | coor = refCoords_[index]; |
404 | + | return true; |
405 | + | } else { |
406 | + | std::cerr << index << " is an invalid index, current rigid body contains " |
407 | + | << atoms_.size() << "atoms" << std::endl; |
408 | + | return false; |
409 | + | } |
410 | + | |
411 | + | } |
412 | + | |
413 | + | bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) { |
414 | + | std::vector<Atom*>::iterator i; |
415 | + | i = find(atoms_.begin(), atoms_.end(), atom); |
416 | + | if (i != atoms_.end()) { |
417 | + | //RigidBody class makes sure refCoords_ and atoms_ match each other |
418 | + | coor = refCoords_[i - atoms_.begin()]; |
419 | + | return true; |
420 | + | } else { |
421 | + | std::cerr << "Atom " << atom->getGlobalIndex() |
422 | + | <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl; |
423 | + | return false; |
424 | + | } |
425 | + | |
426 | + | } |
427 | + | |
428 | + | } |
429 | + |
– | Removed lines |
+ | Added lines |
< | Changed lines |
> | Changed lines |