# | 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 42 | Line 42 | |
42 | #include <math.h> | |
43 | #include "primitives/RigidBody.hpp" | |
44 | #include "utils/simError.h" | |
45 | + | #include "utils/NumericConstant.hpp" |
46 | namespace oopse { | |
47 | ||
48 | < | RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData), inertiaTensor_(0.0){ |
48 | > | RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData), inertiaTensor_(0.0){ |
49 | ||
50 | < | } |
50 | > | } |
51 | ||
52 | < | void RigidBody::setPrevA(const RotMat3x3d& a) { |
52 | > | void RigidBody::setPrevA(const RotMat3x3d& a) { |
53 | ((snapshotMan_->getPrevSnapshot())->*storage_).aMat[localIndex_] = a; | |
54 | //((snapshotMan_->getPrevSnapshot())->*storage_).electroFrame[localIndex_] = a.transpose() * sU_; | |
55 | ||
56 | for (int i =0 ; i < atoms_.size(); ++i){ | |
57 | < | if (atoms_[i]->isDirectional()) { |
58 | < | atoms_[i]->setPrevA(a * refOrients_[i]); |
59 | < | } |
57 | > | if (atoms_[i]->isDirectional()) { |
58 | > | atoms_[i]->setPrevA(a * refOrients_[i]); |
59 | > | } |
60 | } | |
61 | ||
62 | < | } |
62 | > | } |
63 | ||
64 | ||
65 | < | void RigidBody::setA(const RotMat3x3d& a) { |
65 | > | void RigidBody::setA(const RotMat3x3d& a) { |
66 | ((snapshotMan_->getCurrentSnapshot())->*storage_).aMat[localIndex_] = a; | |
67 | //((snapshotMan_->getCurrentSnapshot())->*storage_).electroFrame[localIndex_] = a.transpose() * sU_; | |
68 | ||
69 | for (int i =0 ; i < atoms_.size(); ++i){ | |
70 | < | if (atoms_[i]->isDirectional()) { |
71 | < | atoms_[i]->setA(a * refOrients_[i]); |
72 | < | } |
70 | > | if (atoms_[i]->isDirectional()) { |
71 | > | atoms_[i]->setA(a * refOrients_[i]); |
72 | > | } |
73 | } | |
74 | < | } |
74 | > | } |
75 | ||
76 | < | void RigidBody::setA(const RotMat3x3d& a, int snapshotNo) { |
76 | > | void RigidBody::setA(const RotMat3x3d& a, int snapshotNo) { |
77 | ((snapshotMan_->getSnapshot(snapshotNo))->*storage_).aMat[localIndex_] = a; | |
78 | //((snapshotMan_->getSnapshot(snapshotNo))->*storage_).electroFrame[localIndex_] = a.transpose() * sU_; | |
79 | ||
80 | for (int i =0 ; i < atoms_.size(); ++i){ | |
81 | < | if (atoms_[i]->isDirectional()) { |
82 | < | atoms_[i]->setA(a * refOrients_[i], snapshotNo); |
83 | < | } |
81 | > | if (atoms_[i]->isDirectional()) { |
82 | > | atoms_[i]->setA(a * refOrients_[i], snapshotNo); |
83 | > | } |
84 | } | |
85 | ||
86 | < | } |
86 | > | } |
87 | ||
88 | < | Mat3x3d RigidBody::getI() { |
88 | > | Mat3x3d RigidBody::getI() { |
89 | return inertiaTensor_; | |
90 | < | } |
90 | > | } |
91 | ||
92 | < | std::vector<double> RigidBody::getGrad() { |
93 | < | std::vector<double> grad(6, 0.0); |
92 | > | std::vector<double> RigidBody::getGrad() { |
93 | > | std::vector<double> grad(6, 0.0); |
94 | Vector3d force; | |
95 | Vector3d torque; | |
96 | Vector3d myEuler; | |
# | Line 128 | Line 129 | std::vector<double> RigidBody::getGrad() { | |
129 | ||
130 | //gradient is equal to -force | |
131 | for (int j = 0 ; j<3; j++) | |
132 | < | grad[j] = -force[j]; |
132 | > | grad[j] = -force[j]; |
133 | ||
134 | for (int j = 0; j < 3; j++ ) { | |
135 | ||
136 | < | grad[3] += torque[j]*ephi[j]; |
137 | < | grad[4] += torque[j]*etheta[j]; |
138 | < | grad[5] += torque[j]*epsi[j]; |
136 | > | grad[3] += torque[j]*ephi[j]; |
137 | > | grad[4] += torque[j]*etheta[j]; |
138 | > | grad[5] += torque[j]*epsi[j]; |
139 | ||
140 | } | |
141 | ||
142 | return grad; | |
143 | < | } |
143 | > | } |
144 | ||
145 | < | void RigidBody::accept(BaseVisitor* v) { |
145 | > | void RigidBody::accept(BaseVisitor* v) { |
146 | v->visit(this); | |
147 | < | } |
147 | > | } |
148 | ||
149 | < | /**@todo need modification */ |
150 | < | void RigidBody::calcRefCoords() { |
149 | > | /**@todo need modification */ |
150 | > | void RigidBody::calcRefCoords() { |
151 | double mtmp; | |
152 | Vector3d refCOM(0.0); | |
153 | mass_ = 0.0; | |
154 | for (std::size_t i = 0; i < atoms_.size(); ++i) { | |
155 | < | mtmp = atoms_[i]->getMass(); |
156 | < | mass_ += mtmp; |
157 | < | refCOM += refCoords_[i]*mtmp; |
155 | > | mtmp = atoms_[i]->getMass(); |
156 | > | mass_ += mtmp; |
157 | > | refCOM += refCoords_[i]*mtmp; |
158 | } | |
159 | refCOM /= mass_; | |
160 | ||
161 | // Next, move the origin of the reference coordinate system to the COM: | |
162 | for (std::size_t i = 0; i < atoms_.size(); ++i) { | |
163 | < | refCoords_[i] -= refCOM; |
163 | > | refCoords_[i] -= refCOM; |
164 | } | |
165 | ||
166 | < | // Moment of Inertia calculation |
166 | > | // Moment of Inertia calculation |
167 | Mat3x3d Itmp(0.0); | |
168 | ||
169 | for (std::size_t i = 0; i < atoms_.size(); i++) { | |
170 | < | mtmp = atoms_[i]->getMass(); |
171 | < | Itmp -= outProduct(refCoords_[i], refCoords_[i]) * mtmp; |
172 | < | double r2 = refCoords_[i].lengthSquare(); |
173 | < | Itmp(0, 0) += mtmp * r2; |
174 | < | Itmp(1, 1) += mtmp * r2; |
175 | < | Itmp(2, 2) += mtmp * r2; |
170 | > | mtmp = atoms_[i]->getMass(); |
171 | > | Itmp -= outProduct(refCoords_[i], refCoords_[i]) * mtmp; |
172 | > | double r2 = refCoords_[i].lengthSquare(); |
173 | > | Itmp(0, 0) += mtmp * r2; |
174 | > | Itmp(1, 1) += mtmp * r2; |
175 | > | Itmp(2, 2) += mtmp * r2; |
176 | } | |
177 | ||
178 | //project the inertial moment of directional atoms into this rigid body | |
179 | for (std::size_t i = 0; i < atoms_.size(); i++) { | |
180 | < | if (atoms_[i]->isDirectional()) { |
181 | < | RectMatrix<double, 3, 3> Iproject = refOrients_[i].transpose() * atoms_[i]->getI(); |
182 | < | Itmp(0, 0) += Iproject(0, 0); |
182 | < | Itmp(1, 1) += Iproject(1, 1); |
183 | < | Itmp(2, 2) += Iproject(2, 2); |
184 | < | } |
180 | > | if (atoms_[i]->isDirectional()) { |
181 | > | Itmp += refOrients_[i].transpose() * atoms_[i]->getI() * refOrients_[i]; |
182 | > | } |
183 | } | |
184 | ||
185 | //diagonalize | |
# | 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]) < oopse::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 | > | "\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(); |
215 | } | |
216 | ||
217 | < | } |
217 | > | } |
218 | ||
219 | < | void RigidBody::calcForcesAndTorques() { |
219 | > | void RigidBody::calcForcesAndTorques() { |
220 | Vector3d afrc; | |
221 | Vector3d atrq; | |
222 | Vector3d apos; | |
# | Line 228 | Line 226 | void RigidBody::calcForcesAndTorques() { | |
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 | } | |
248 | ||
249 | setFrc(frc); | |
250 | setTrq(trq); | |
251 | ||
252 | < | } |
252 | > | } |
253 | ||
254 | < | void RigidBody::updateAtoms() { |
254 | > | void RigidBody::updateAtoms() { |
255 | unsigned int i; | |
256 | Vector3d ref; | |
257 | Vector3d apos; | |
# | Line 263 | Line 261 | void RigidBody::updateAtoms() { | |
261 | ||
262 | for (i = 0; i < atoms_.size(); i++) { | |
263 | ||
264 | < | ref = body2Lab(refCoords_[i]); |
264 | > | ref = body2Lab(refCoords_[i]); |
265 | ||
266 | < | apos = pos + ref; |
266 | > | apos = pos + ref; |
267 | ||
268 | < | atoms_[i]->setPos(apos); |
268 | > | atoms_[i]->setPos(apos); |
269 | ||
270 | < | if (atoms_[i]->isDirectional()) { |
270 | > | if (atoms_[i]->isDirectional()) { |
271 | ||
272 | < | dAtom = (DirectionalAtom *) atoms_[i]; |
273 | < | dAtom->setA(a * refOrients_[i]); |
274 | < | //dAtom->rotateBy( A ); |
277 | < | } |
272 | > | dAtom = (DirectionalAtom *) atoms_[i]; |
273 | > | dAtom->setA(refOrients_[i] * a); |
274 | > | } |
275 | ||
276 | } | |
277 | ||
278 | < | } |
278 | > | } |
279 | ||
280 | ||
281 | < | bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) { |
282 | < | if (index < atoms_.size()) { |
281 | > | void RigidBody::updateAtoms(int frame) { |
282 | > | unsigned int i; |
283 | > | Vector3d ref; |
284 | > | Vector3d apos; |
285 | > | DirectionalAtom* dAtom; |
286 | > | Vector3d pos = getPos(frame); |
287 | > | RotMat3x3d a = getA(frame); |
288 | > | |
289 | > | for (i = 0; i < atoms_.size(); i++) { |
290 | > | |
291 | > | ref = body2Lab(refCoords_[i], frame); |
292 | ||
293 | < | Vector3d ref = body2Lab(refCoords_[index]); |
288 | < | pos = getPos() + ref; |
289 | < | return true; |
290 | < | } else { |
291 | < | std::cerr << index << " is an invalid index, current rigid body contains " |
292 | < | << atoms_.size() << "atoms" << std::endl; |
293 | < | return false; |
294 | < | } |
295 | < | } |
293 | > | apos = pos + ref; |
294 | ||
295 | < | bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) { |
296 | < | std::vector<Atom*>::iterator i; |
297 | < | i = std::find(atoms_.begin(), atoms_.end(), atom); |
298 | < | if (i != atoms_.end()) { |
299 | < | //RigidBody class makes sure refCoords_ and atoms_ match each other |
300 | < | Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]); |
301 | < | pos = getPos() + ref; |
302 | < | return true; |
305 | < | } else { |
306 | < | std::cerr << "Atom " << atom->getGlobalIndex() |
307 | < | <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl; |
308 | < | return false; |
295 | > | atoms_[i]->setPos(apos, frame); |
296 | > | |
297 | > | if (atoms_[i]->isDirectional()) { |
298 | > | |
299 | > | dAtom = (DirectionalAtom *) atoms_[i]; |
300 | > | dAtom->setA(refOrients_[i] * a, frame); |
301 | > | } |
302 | > | |
303 | } | |
304 | < | } |
305 | < | bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) { |
304 | > | |
305 | > | } |
306 | ||
307 | < | //velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$ |
307 | > | void RigidBody::updateAtomVel() { |
308 | > | Mat3x3d skewMat;; |
309 | ||
310 | < | if (index < atoms_.size()) { |
310 | > | Vector3d ji = getJ(); |
311 | > | Mat3x3d I = getI(); |
312 | ||
313 | < | Vector3d velRot; |
314 | < | Mat3x3d skewMat;; |
315 | < | Vector3d ref = refCoords_[index]; |
320 | < | Vector3d ji = getJ(); |
321 | < | Mat3x3d I = getI(); |
313 | > | skewMat(0, 0) =0; |
314 | > | skewMat(0, 1) = ji[2] /I(2, 2); |
315 | > | skewMat(0, 2) = -ji[1] /I(1, 1); |
316 | ||
317 | < | skewMat(0, 0) =0; |
318 | < | skewMat(0, 1) = ji[2] /I(2, 2); |
319 | < | skewMat(0, 2) = -ji[1] /I(1, 1); |
317 | > | skewMat(1, 0) = -ji[2] /I(2, 2); |
318 | > | skewMat(1, 1) = 0; |
319 | > | skewMat(1, 2) = ji[0]/I(0, 0); |
320 | ||
321 | < | skewMat(1, 0) = -ji[2] /I(2, 2); |
322 | < | skewMat(1, 1) = 0; |
323 | < | skewMat(1, 2) = ji[0]/I(0, 0); |
321 | > | skewMat(2, 0) =ji[1] /I(1, 1); |
322 | > | skewMat(2, 1) = -ji[0]/I(0, 0); |
323 | > | skewMat(2, 2) = 0; |
324 | ||
325 | < | skewMat(2, 0) =ji[1] /I(1, 1); |
326 | < | skewMat(2, 1) = -ji[0]/I(0, 0); |
333 | < | skewMat(2, 2) = 0; |
325 | > | Mat3x3d mat = (getA() * skewMat).transpose(); |
326 | > | Vector3d rbVel = getVel(); |
327 | ||
335 | – | velRot = (getA() * skewMat).transpose() * ref; |
328 | ||
329 | < | vel =getVel() + velRot; |
330 | < | return true; |
329 | > | Vector3d velRot; |
330 | > | for (int i =0 ; i < refCoords_.size(); ++i) { |
331 | > | atoms_[i]->setVel(rbVel + mat * refCoords_[i]); |
332 | > | } |
333 | > | |
334 | > | } |
335 | > | |
336 | > | void RigidBody::updateAtomVel(int frame) { |
337 | > | Mat3x3d skewMat;; |
338 | > | |
339 | > | Vector3d ji = getJ(frame); |
340 | > | Mat3x3d I = getI(); |
341 | > | |
342 | > | skewMat(0, 0) =0; |
343 | > | skewMat(0, 1) = ji[2] /I(2, 2); |
344 | > | skewMat(0, 2) = -ji[1] /I(1, 1); |
345 | > | |
346 | > | skewMat(1, 0) = -ji[2] /I(2, 2); |
347 | > | skewMat(1, 1) = 0; |
348 | > | skewMat(1, 2) = ji[0]/I(0, 0); |
349 | > | |
350 | > | skewMat(2, 0) =ji[1] /I(1, 1); |
351 | > | skewMat(2, 1) = -ji[0]/I(0, 0); |
352 | > | skewMat(2, 2) = 0; |
353 | > | |
354 | > | Mat3x3d mat = (getA(frame) * skewMat).transpose(); |
355 | > | Vector3d rbVel = getVel(frame); |
356 | > | |
357 | > | |
358 | > | Vector3d velRot; |
359 | > | for (int i =0 ; i < refCoords_.size(); ++i) { |
360 | > | atoms_[i]->setVel(rbVel + mat * refCoords_[i], frame); |
361 | > | } |
362 | > | |
363 | > | } |
364 | > | |
365 | ||
366 | + | |
367 | + | bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) { |
368 | + | if (index < atoms_.size()) { |
369 | + | |
370 | + | Vector3d ref = body2Lab(refCoords_[index]); |
371 | + | pos = getPos() + ref; |
372 | + | return true; |
373 | } else { | |
374 | < | std::cerr << index << " is an invalid index, current rigid body contains " |
375 | < | << atoms_.size() << "atoms" << std::endl; |
376 | < | return false; |
374 | > | std::cerr << index << " is an invalid index, current rigid body contains " |
375 | > | << atoms_.size() << "atoms" << std::endl; |
376 | > | return false; |
377 | > | } |
378 | > | } |
379 | > | |
380 | > | bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) { |
381 | > | std::vector<Atom*>::iterator i; |
382 | > | i = std::find(atoms_.begin(), atoms_.end(), atom); |
383 | > | if (i != atoms_.end()) { |
384 | > | //RigidBody class makes sure refCoords_ and atoms_ match each other |
385 | > | Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]); |
386 | > | pos = getPos() + ref; |
387 | > | return true; |
388 | > | } else { |
389 | > | std::cerr << "Atom " << atom->getGlobalIndex() |
390 | > | <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl; |
391 | > | return false; |
392 | } | |
393 | < | } |
393 | > | } |
394 | > | bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) { |
395 | ||
396 | < | bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) { |
396 | > | //velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$ |
397 | ||
398 | + | if (index < atoms_.size()) { |
399 | + | |
400 | + | Vector3d velRot; |
401 | + | Mat3x3d skewMat;; |
402 | + | Vector3d ref = refCoords_[index]; |
403 | + | Vector3d ji = getJ(); |
404 | + | Mat3x3d I = getI(); |
405 | + | |
406 | + | skewMat(0, 0) =0; |
407 | + | skewMat(0, 1) = ji[2] /I(2, 2); |
408 | + | skewMat(0, 2) = -ji[1] /I(1, 1); |
409 | + | |
410 | + | skewMat(1, 0) = -ji[2] /I(2, 2); |
411 | + | skewMat(1, 1) = 0; |
412 | + | skewMat(1, 2) = ji[0]/I(0, 0); |
413 | + | |
414 | + | skewMat(2, 0) =ji[1] /I(1, 1); |
415 | + | skewMat(2, 1) = -ji[0]/I(0, 0); |
416 | + | skewMat(2, 2) = 0; |
417 | + | |
418 | + | velRot = (getA() * skewMat).transpose() * ref; |
419 | + | |
420 | + | vel =getVel() + velRot; |
421 | + | return true; |
422 | + | |
423 | + | } else { |
424 | + | std::cerr << index << " is an invalid index, current rigid body contains " |
425 | + | << atoms_.size() << "atoms" << std::endl; |
426 | + | return false; |
427 | + | } |
428 | + | } |
429 | + | |
430 | + | bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) { |
431 | + | |
432 | std::vector<Atom*>::iterator i; | |
433 | i = std::find(atoms_.begin(), atoms_.end(), atom); | |
434 | if (i != atoms_.end()) { | |
435 | < | return getAtomVel(vel, i - atoms_.begin()); |
435 | > | return getAtomVel(vel, i - atoms_.begin()); |
436 | } else { | |
437 | < | std::cerr << "Atom " << atom->getGlobalIndex() |
438 | < | <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl; |
439 | < | return false; |
437 | > | std::cerr << "Atom " << atom->getGlobalIndex() |
438 | > | <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl; |
439 | > | return false; |
440 | } | |
441 | < | } |
441 | > | } |
442 | ||
443 | < | bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) { |
443 | > | bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) { |
444 | if (index < atoms_.size()) { | |
445 | ||
446 | < | coor = refCoords_[index]; |
447 | < | return true; |
446 | > | coor = refCoords_[index]; |
447 | > | return true; |
448 | } else { | |
449 | < | std::cerr << index << " is an invalid index, current rigid body contains " |
450 | < | << atoms_.size() << "atoms" << std::endl; |
451 | < | return false; |
449 | > | std::cerr << index << " is an invalid index, current rigid body contains " |
450 | > | << atoms_.size() << "atoms" << std::endl; |
451 | > | return false; |
452 | } | |
453 | ||
454 | < | } |
454 | > | } |
455 | ||
456 | < | bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) { |
456 | > | bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) { |
457 | std::vector<Atom*>::iterator i; | |
458 | i = std::find(atoms_.begin(), atoms_.end(), atom); | |
459 | if (i != atoms_.end()) { | |
460 | < | //RigidBody class makes sure refCoords_ and atoms_ match each other |
461 | < | coor = refCoords_[i - atoms_.begin()]; |
462 | < | return true; |
460 | > | //RigidBody class makes sure refCoords_ and atoms_ match each other |
461 | > | coor = refCoords_[i - atoms_.begin()]; |
462 | > | return true; |
463 | } else { | |
464 | < | std::cerr << "Atom " << atom->getGlobalIndex() |
465 | < | <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl; |
466 | < | return false; |
464 | > | std::cerr << "Atom " << atom->getGlobalIndex() |
465 | > | <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl; |
466 | > | return false; |
467 | } | |
468 | ||
469 | < | } |
469 | > | } |
470 | ||
471 | ||
472 | < | void RigidBody::addAtom(Atom* at, AtomStamp* ats) { |
472 | > | void RigidBody::addAtom(Atom* at, AtomStamp* ats) { |
473 | ||
474 | < | Vector3d coords; |
475 | < | Vector3d euler; |
474 | > | Vector3d coords; |
475 | > | Vector3d euler; |
476 | ||
477 | ||
478 | < | atoms_.push_back(at); |
478 | > | atoms_.push_back(at); |
479 | ||
480 | < | if( !ats->havePosition() ){ |
481 | < | sprintf( painCave.errMsg, |
482 | < | "RigidBody error.\n" |
483 | < | "\tAtom %s does not have a position specified.\n" |
484 | < | "\tThis means RigidBody cannot set up reference coordinates.\n", |
485 | < | ats->getType() ); |
486 | < | painCave.isFatal = 1; |
487 | < | simError(); |
488 | < | } |
480 | > | if( !ats->havePosition() ){ |
481 | > | sprintf( painCave.errMsg, |
482 | > | "RigidBody error.\n" |
483 | > | "\tAtom %s does not have a position specified.\n" |
484 | > | "\tThis means RigidBody cannot set up reference coordinates.\n", |
485 | > | ats->getType() ); |
486 | > | painCave.isFatal = 1; |
487 | > | simError(); |
488 | > | } |
489 | ||
490 | < | coords[0] = ats->getPosX(); |
491 | < | coords[1] = ats->getPosY(); |
492 | < | coords[2] = ats->getPosZ(); |
490 | > | coords[0] = ats->getPosX(); |
491 | > | coords[1] = ats->getPosY(); |
492 | > | coords[2] = ats->getPosZ(); |
493 | ||
494 | < | refCoords_.push_back(coords); |
494 | > | refCoords_.push_back(coords); |
495 | ||
496 | < | RotMat3x3d identMat = RotMat3x3d::identity(); |
496 | > | RotMat3x3d identMat = RotMat3x3d::identity(); |
497 | ||
498 | < | if (at->isDirectional()) { |
498 | > | if (at->isDirectional()) { |
499 | ||
500 | < | if( !ats->haveOrientation() ){ |
501 | < | sprintf( painCave.errMsg, |
502 | < | "RigidBody error.\n" |
503 | < | "\tAtom %s does not have an orientation specified.\n" |
504 | < | "\tThis means RigidBody cannot set up reference orientations.\n", |
505 | < | ats->getType() ); |
506 | < | painCave.isFatal = 1; |
507 | < | simError(); |
508 | < | } |
500 | > | if( !ats->haveOrientation() ){ |
501 | > | sprintf( painCave.errMsg, |
502 | > | "RigidBody error.\n" |
503 | > | "\tAtom %s does not have an orientation specified.\n" |
504 | > | "\tThis means RigidBody cannot set up reference orientations.\n", |
505 | > | ats->getType() ); |
506 | > | painCave.isFatal = 1; |
507 | > | simError(); |
508 | > | } |
509 | ||
510 | < | euler[0] = ats->getEulerPhi(); |
511 | < | euler[1] = ats->getEulerTheta(); |
512 | < | euler[2] = ats->getEulerPsi(); |
510 | > | euler[0] = ats->getEulerPhi() * NumericConstant::PI /180.0; |
511 | > | euler[1] = ats->getEulerTheta() * NumericConstant::PI /180.0; |
512 | > | euler[2] = ats->getEulerPsi() * NumericConstant::PI /180.0; |
513 | ||
514 | < | RotMat3x3d Atmp(euler); |
515 | < | refOrients_.push_back(Atmp); |
514 | > | RotMat3x3d Atmp(euler); |
515 | > | refOrients_.push_back(Atmp); |
516 | ||
517 | < | }else { |
518 | < | refOrients_.push_back(identMat); |
519 | < | } |
517 | > | }else { |
518 | > | refOrients_.push_back(identMat); |
519 | > | } |
520 | ||
521 | ||
522 | < | } |
522 | > | } |
523 | ||
524 | } | |
525 |
– | Removed lines |
+ | Added lines |
< | Changed lines |
> | Changed lines |