# | 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; | |
53 | – | //((snapshotMan_->getPrevSnapshot())->*storage_).electroFrame[localIndex_] = a.transpose() * sU_; |
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 | < | } |
61 | > | } |
62 | ||
63 | ||
64 | < | void RigidBody::setA(const RotMat3x3d& a) { |
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 | < | } |
72 | > | } |
73 | ||
74 | < | void RigidBody::setA(const RotMat3x3d& a, int snapshotNo) { |
74 | > | void RigidBody::setA(const RotMat3x3d& a, int snapshotNo) { |
75 | ((snapshotMan_->getSnapshot(snapshotNo))->*storage_).aMat[localIndex_] = a; | |
76 | //((snapshotMan_->getSnapshot(snapshotNo))->*storage_).electroFrame[localIndex_] = a.transpose() * sU_; | |
77 | ||
78 | for (int i =0 ; i < atoms_.size(); ++i){ | |
79 | < | if (atoms_[i]->isDirectional()) { |
80 | < | atoms_[i]->setA(a * refOrients_[i], snapshotNo); |
81 | < | } |
79 | > | if (atoms_[i]->isDirectional()) { |
80 | > | atoms_[i]->setA(refOrients_[i].transpose() * a, snapshotNo); |
81 | > | } |
82 | } | |
83 | ||
84 | < | } |
84 | > | } |
85 | ||
86 | < | Mat3x3d RigidBody::getI() { |
86 | > | Mat3x3d RigidBody::getI() { |
87 | return inertiaTensor_; | |
88 | < | } |
88 | > | } |
89 | ||
90 | < | std::vector<double> RigidBody::getGrad() { |
91 | < | std::vector<double> grad(6, 0.0); |
90 | > | std::vector<RealType> RigidBody::getGrad() { |
91 | > | std::vector<RealType> grad(6, 0.0); |
92 | Vector3d force; | |
93 | Vector3d torque; | |
94 | Vector3d myEuler; | |
95 | < | double phi, theta, psi; |
96 | < | double cphi, sphi, ctheta, stheta; |
95 | > | RealType phi, theta, psi; |
96 | > | RealType cphi, sphi, ctheta, stheta; |
97 | Vector3d ephi; | |
98 | Vector3d etheta; | |
99 | Vector3d epsi; | |
# | Line 128 | Line 127 | std::vector<double> RigidBody::getGrad() { | |
127 | ||
128 | //gradient is equal to -force | |
129 | for (int j = 0 ; j<3; j++) | |
130 | < | grad[j] = -force[j]; |
130 | > | grad[j] = -force[j]; |
131 | ||
132 | for (int j = 0; j < 3; j++ ) { | |
133 | ||
134 | < | grad[3] += torque[j]*ephi[j]; |
135 | < | grad[4] += torque[j]*etheta[j]; |
136 | < | grad[5] += torque[j]*epsi[j]; |
134 | > | grad[3] += torque[j]*ephi[j]; |
135 | > | grad[4] += torque[j]*etheta[j]; |
136 | > | grad[5] += torque[j]*epsi[j]; |
137 | ||
138 | } | |
139 | ||
140 | return grad; | |
141 | < | } |
141 | > | } |
142 | ||
143 | < | void RigidBody::accept(BaseVisitor* v) { |
143 | > | void RigidBody::accept(BaseVisitor* v) { |
144 | v->visit(this); | |
145 | < | } |
145 | > | } |
146 | ||
147 | < | /**@todo need modification */ |
148 | < | void RigidBody::calcRefCoords() { |
149 | < | double mtmp; |
147 | > | /**@todo need modification */ |
148 | > | void RigidBody::calcRefCoords() { |
149 | > | RealType mtmp; |
150 | Vector3d refCOM(0.0); | |
151 | mass_ = 0.0; | |
152 | for (std::size_t i = 0; i < atoms_.size(); ++i) { | |
153 | < | mtmp = atoms_[i]->getMass(); |
154 | < | mass_ += mtmp; |
155 | < | refCOM += refCoords_[i]*mtmp; |
153 | > | mtmp = atoms_[i]->getMass(); |
154 | > | mass_ += mtmp; |
155 | > | refCOM += refCoords_[i]*mtmp; |
156 | } | |
157 | refCOM /= mass_; | |
158 | ||
159 | // Next, move the origin of the reference coordinate system to the COM: | |
160 | for (std::size_t i = 0; i < atoms_.size(); ++i) { | |
161 | < | refCoords_[i] -= refCOM; |
161 | > | refCoords_[i] -= refCOM; |
162 | } | |
163 | ||
164 | < | // Moment of Inertia calculation |
165 | < | Mat3x3d Itmp(0.0); |
167 | < | |
164 | > | // Moment of Inertia calculation |
165 | > | Mat3x3d Itmp(0.0); |
166 | for (std::size_t i = 0; i < atoms_.size(); i++) { | |
167 | < | mtmp = atoms_[i]->getMass(); |
168 | < | Itmp -= outProduct(refCoords_[i], refCoords_[i]) * mtmp; |
169 | < | double r2 = refCoords_[i].lengthSquare(); |
170 | < | Itmp(0, 0) += mtmp * r2; |
171 | < | Itmp(1, 1) += mtmp * r2; |
172 | < | Itmp(2, 2) += mtmp * r2; |
173 | < | } |
167 | > | Mat3x3d IAtom(0.0); |
168 | > | mtmp = atoms_[i]->getMass(); |
169 | > | IAtom -= outProduct(refCoords_[i], refCoords_[i]) * mtmp; |
170 | > | RealType r2 = refCoords_[i].lengthSquare(); |
171 | > | IAtom(0, 0) += mtmp * r2; |
172 | > | IAtom(1, 1) += mtmp * r2; |
173 | > | IAtom(2, 2) += mtmp * r2; |
174 | > | Itmp += IAtom; |
175 | ||
176 | < | //project the inertial moment of directional atoms into this rigid body |
177 | < | for (std::size_t i = 0; i < atoms_.size(); i++) { |
178 | < | if (atoms_[i]->isDirectional()) { |
179 | < | 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 | < | } |
176 | > | //project the inertial moment of directional atoms into this rigid body |
177 | > | if (atoms_[i]->isDirectional()) { |
178 | > | Itmp += refOrients_[i].transpose() * atoms_[i]->getI() * refOrients_[i]; |
179 | > | } |
180 | } | |
181 | ||
182 | + | // std::cout << Itmp << std::endl; |
183 | + | |
184 | //diagonalize | |
185 | Vector3d evals; | |
186 | Mat3x3d::diagonalize(Itmp, evals, sU_); | |
# | Line 195 | Line 192 | void RigidBody::calcRefCoords() { | |
192 | ||
193 | int nLinearAxis = 0; | |
194 | for (int i = 0; i < 3; i++) { | |
195 | < | if (fabs(evals[i]) < oopse::epsilon) { |
196 | < | linear_ = true; |
197 | < | linearAxis_ = i; |
198 | < | ++ nLinearAxis; |
199 | < | } |
195 | > | if (fabs(evals[i]) < oopse::epsilon) { |
196 | > | linear_ = true; |
197 | > | linearAxis_ = i; |
198 | > | ++ nLinearAxis; |
199 | > | } |
200 | } | |
201 | ||
202 | if (nLinearAxis > 1) { | |
203 | < | sprintf( painCave.errMsg, |
204 | < | "RigidBody error.\n" |
205 | < | "\tOOPSE found more than one axis in this rigid body with a vanishing \n" |
206 | < | "\tmoment of inertia. This can happen in one of three ways:\n" |
207 | < | "\t 1) Only one atom was specified, or \n" |
208 | < | "\t 2) All atoms were specified at the same location, or\n" |
209 | < | "\t 3) The programmers did something stupid.\n" |
210 | < | "\tIt is silly to use a rigid body to describe this situation. Be smarter.\n" |
211 | < | ); |
212 | < | painCave.isFatal = 1; |
213 | < | simError(); |
203 | > | sprintf( painCave.errMsg, |
204 | > | "RigidBody error.\n" |
205 | > | "\tOOPSE found more than one axis in this rigid body with a vanishing \n" |
206 | > | "\tmoment of inertia. This can happen in one of three ways:\n" |
207 | > | "\t 1) Only one atom was specified, or \n" |
208 | > | "\t 2) All atoms were specified at the same location, or\n" |
209 | > | "\t 3) The programmers did something stupid.\n" |
210 | > | "\tIt is silly to use a rigid body to describe this situation. Be smarter.\n" |
211 | > | ); |
212 | > | painCave.isFatal = 1; |
213 | > | simError(); |
214 | } | |
215 | ||
216 | < | } |
216 | > | } |
217 | ||
218 | < | void RigidBody::calcForcesAndTorques() { |
218 | > | void RigidBody::calcForcesAndTorques() { |
219 | Vector3d afrc; | |
220 | Vector3d atrq; | |
221 | Vector3d apos; | |
# | Line 228 | Line 225 | void RigidBody::calcForcesAndTorques() { | |
225 | Vector3d pos = this->getPos(); | |
226 | for (int i = 0; i < atoms_.size(); i++) { | |
227 | ||
228 | < | afrc = atoms_[i]->getFrc(); |
229 | < | apos = atoms_[i]->getPos(); |
230 | < | rpos = apos - pos; |
228 | > | afrc = atoms_[i]->getFrc(); |
229 | > | apos = atoms_[i]->getPos(); |
230 | > | rpos = apos - pos; |
231 | ||
232 | < | frc += afrc; |
232 | > | frc += afrc; |
233 | ||
234 | < | trq[0] += rpos[1]*afrc[2] - rpos[2]*afrc[1]; |
235 | < | trq[1] += rpos[2]*afrc[0] - rpos[0]*afrc[2]; |
236 | < | trq[2] += rpos[0]*afrc[1] - rpos[1]*afrc[0]; |
234 | > | trq[0] += rpos[1]*afrc[2] - rpos[2]*afrc[1]; |
235 | > | trq[1] += rpos[2]*afrc[0] - rpos[0]*afrc[2]; |
236 | > | trq[2] += rpos[0]*afrc[1] - rpos[1]*afrc[0]; |
237 | ||
238 | < | // If the atom has a torque associated with it, then we also need to |
239 | < | // migrate the torques onto the center of mass: |
238 | > | // If the atom has a torque associated with it, then we also need to |
239 | > | // migrate the torques onto the center of mass: |
240 | ||
241 | < | if (atoms_[i]->isDirectional()) { |
242 | < | atrq = atoms_[i]->getTrq(); |
243 | < | trq += atrq; |
244 | < | } |
241 | > | if (atoms_[i]->isDirectional()) { |
242 | > | atrq = atoms_[i]->getTrq(); |
243 | > | trq += atrq; |
244 | > | } |
245 | ||
246 | } | |
247 | ||
248 | < | setFrc(frc); |
249 | < | setTrq(trq); |
248 | > | addFrc(frc); |
249 | > | addTrq(trq); |
250 | ||
251 | < | } |
251 | > | } |
252 | ||
253 | < | void RigidBody::updateAtoms() { |
253 | > | void RigidBody::updateAtoms() { |
254 | unsigned int i; | |
255 | Vector3d ref; | |
256 | Vector3d apos; | |
# | Line 263 | Line 260 | void RigidBody::updateAtoms() { | |
260 | ||
261 | for (i = 0; i < atoms_.size(); i++) { | |
262 | ||
263 | < | ref = body2Lab(refCoords_[i]); |
263 | > | ref = body2Lab(refCoords_[i]); |
264 | ||
265 | < | apos = pos + ref; |
265 | > | apos = pos + ref; |
266 | ||
267 | < | atoms_[i]->setPos(apos); |
267 | > | atoms_[i]->setPos(apos); |
268 | ||
269 | < | if (atoms_[i]->isDirectional()) { |
269 | > | if (atoms_[i]->isDirectional()) { |
270 | ||
271 | < | dAtom = (DirectionalAtom *) atoms_[i]; |
272 | < | dAtom->setA(a * refOrients_[i]); |
273 | < | //dAtom->rotateBy( A ); |
277 | < | } |
271 | > | dAtom = (DirectionalAtom *) atoms_[i]; |
272 | > | dAtom->setA(refOrients_[i].transpose() * a); |
273 | > | } |
274 | ||
275 | } | |
276 | ||
277 | < | } |
277 | > | } |
278 | ||
279 | ||
280 | < | void RigidBody::updateAtoms(int frame) { |
280 | > | void RigidBody::updateAtoms(int frame) { |
281 | unsigned int i; | |
282 | Vector3d ref; | |
283 | Vector3d apos; | |
# | Line 291 | Line 287 | void RigidBody::updateAtoms(int frame) { | |
287 | ||
288 | for (i = 0; i < atoms_.size(); i++) { | |
289 | ||
290 | < | ref = body2Lab(refCoords_[i], frame); |
290 | > | ref = body2Lab(refCoords_[i], frame); |
291 | ||
292 | < | apos = pos + ref; |
292 | > | apos = pos + ref; |
293 | ||
294 | < | atoms_[i]->setPos(apos, frame); |
294 | > | atoms_[i]->setPos(apos, frame); |
295 | ||
296 | < | if (atoms_[i]->isDirectional()) { |
296 | > | if (atoms_[i]->isDirectional()) { |
297 | ||
298 | < | dAtom = (DirectionalAtom *) atoms_[i]; |
299 | < | dAtom->setA(a * refOrients_[i], frame); |
300 | < | } |
298 | > | dAtom = (DirectionalAtom *) atoms_[i]; |
299 | > | dAtom->setA(refOrients_[i].transpose() * a, frame); |
300 | > | } |
301 | ||
302 | } | |
303 | ||
304 | < | } |
304 | > | } |
305 | ||
306 | < | void RigidBody::updateAtomVel() { |
306 | > | void RigidBody::updateAtomVel() { |
307 | Mat3x3d skewMat;; | |
308 | ||
309 | Vector3d ji = getJ(); | |
# | Line 331 | Line 327 | void RigidBody::updateAtomVel() { | |
327 | ||
328 | Vector3d velRot; | |
329 | for (int i =0 ; i < refCoords_.size(); ++i) { | |
330 | < | atoms_[i]->setVel(rbVel + mat * refCoords_[i]); |
330 | > | atoms_[i]->setVel(rbVel + mat * refCoords_[i]); |
331 | } | |
332 | ||
333 | < | } |
333 | > | } |
334 | ||
335 | < | void RigidBody::updateAtomVel(int frame) { |
335 | > | void RigidBody::updateAtomVel(int frame) { |
336 | Mat3x3d skewMat;; | |
337 | ||
338 | Vector3d ji = getJ(frame); | |
# | Line 360 | Line 356 | void RigidBody::updateAtomVel(int frame) { | |
356 | ||
357 | Vector3d velRot; | |
358 | for (int i =0 ; i < refCoords_.size(); ++i) { | |
359 | < | atoms_[i]->setVel(rbVel + mat * refCoords_[i], frame); |
359 | > | atoms_[i]->setVel(rbVel + mat * refCoords_[i], frame); |
360 | } | |
361 | ||
362 | < | } |
362 | > | } |
363 | ||
364 | ||
365 | ||
366 | < | bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) { |
366 | > | bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) { |
367 | if (index < atoms_.size()) { | |
368 | ||
369 | < | Vector3d ref = body2Lab(refCoords_[index]); |
370 | < | pos = getPos() + ref; |
371 | < | return true; |
369 | > | Vector3d ref = body2Lab(refCoords_[index]); |
370 | > | pos = getPos() + ref; |
371 | > | return true; |
372 | } else { | |
373 | < | std::cerr << index << " is an invalid index, current rigid body contains " |
374 | < | << atoms_.size() << "atoms" << std::endl; |
375 | < | return false; |
373 | > | std::cerr << index << " is an invalid index, current rigid body contains " |
374 | > | << atoms_.size() << "atoms" << std::endl; |
375 | > | return false; |
376 | } | |
377 | < | } |
377 | > | } |
378 | ||
379 | < | bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) { |
379 | > | bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) { |
380 | std::vector<Atom*>::iterator i; | |
381 | i = std::find(atoms_.begin(), atoms_.end(), atom); | |
382 | if (i != atoms_.end()) { | |
383 | < | //RigidBody class makes sure refCoords_ and atoms_ match each other |
384 | < | Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]); |
385 | < | pos = getPos() + ref; |
386 | < | return true; |
383 | > | //RigidBody class makes sure refCoords_ and atoms_ match each other |
384 | > | Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]); |
385 | > | pos = getPos() + ref; |
386 | > | return true; |
387 | } else { | |
388 | < | std::cerr << "Atom " << atom->getGlobalIndex() |
389 | < | <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl; |
390 | < | return false; |
388 | > | std::cerr << "Atom " << atom->getGlobalIndex() |
389 | > | <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl; |
390 | > | return false; |
391 | } | |
392 | < | } |
393 | < | bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) { |
392 | > | } |
393 | > | bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) { |
394 | ||
395 | //velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$ | |
396 | ||
397 | if (index < atoms_.size()) { | |
398 | ||
399 | < | Vector3d velRot; |
400 | < | Mat3x3d skewMat;; |
401 | < | Vector3d ref = refCoords_[index]; |
402 | < | Vector3d ji = getJ(); |
403 | < | Mat3x3d I = getI(); |
399 | > | Vector3d velRot; |
400 | > | Mat3x3d skewMat;; |
401 | > | Vector3d ref = refCoords_[index]; |
402 | > | Vector3d ji = getJ(); |
403 | > | Mat3x3d I = getI(); |
404 | ||
405 | < | skewMat(0, 0) =0; |
406 | < | skewMat(0, 1) = ji[2] /I(2, 2); |
407 | < | skewMat(0, 2) = -ji[1] /I(1, 1); |
405 | > | skewMat(0, 0) =0; |
406 | > | skewMat(0, 1) = ji[2] /I(2, 2); |
407 | > | skewMat(0, 2) = -ji[1] /I(1, 1); |
408 | ||
409 | < | skewMat(1, 0) = -ji[2] /I(2, 2); |
410 | < | skewMat(1, 1) = 0; |
411 | < | skewMat(1, 2) = ji[0]/I(0, 0); |
409 | > | skewMat(1, 0) = -ji[2] /I(2, 2); |
410 | > | skewMat(1, 1) = 0; |
411 | > | skewMat(1, 2) = ji[0]/I(0, 0); |
412 | ||
413 | < | skewMat(2, 0) =ji[1] /I(1, 1); |
414 | < | skewMat(2, 1) = -ji[0]/I(0, 0); |
415 | < | skewMat(2, 2) = 0; |
413 | > | skewMat(2, 0) =ji[1] /I(1, 1); |
414 | > | skewMat(2, 1) = -ji[0]/I(0, 0); |
415 | > | skewMat(2, 2) = 0; |
416 | ||
417 | < | velRot = (getA() * skewMat).transpose() * ref; |
417 | > | velRot = (getA() * skewMat).transpose() * ref; |
418 | ||
419 | < | vel =getVel() + velRot; |
420 | < | return true; |
419 | > | vel =getVel() + velRot; |
420 | > | return true; |
421 | ||
422 | } else { | |
423 | < | std::cerr << index << " is an invalid index, current rigid body contains " |
424 | < | << atoms_.size() << "atoms" << std::endl; |
425 | < | return false; |
423 | > | std::cerr << index << " is an invalid index, current rigid body contains " |
424 | > | << atoms_.size() << "atoms" << std::endl; |
425 | > | return false; |
426 | } | |
427 | < | } |
427 | > | } |
428 | ||
429 | < | bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) { |
429 | > | bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) { |
430 | ||
431 | std::vector<Atom*>::iterator i; | |
432 | i = std::find(atoms_.begin(), atoms_.end(), atom); | |
433 | if (i != atoms_.end()) { | |
434 | < | return getAtomVel(vel, i - atoms_.begin()); |
434 | > | return getAtomVel(vel, i - atoms_.begin()); |
435 | } else { | |
436 | < | std::cerr << "Atom " << atom->getGlobalIndex() |
437 | < | <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl; |
438 | < | return false; |
436 | > | std::cerr << "Atom " << atom->getGlobalIndex() |
437 | > | <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl; |
438 | > | return false; |
439 | } | |
440 | < | } |
440 | > | } |
441 | ||
442 | < | bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) { |
442 | > | bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) { |
443 | if (index < atoms_.size()) { | |
444 | ||
445 | < | coor = refCoords_[index]; |
446 | < | return true; |
445 | > | coor = refCoords_[index]; |
446 | > | return true; |
447 | } else { | |
448 | < | std::cerr << index << " is an invalid index, current rigid body contains " |
449 | < | << atoms_.size() << "atoms" << std::endl; |
450 | < | return false; |
448 | > | std::cerr << index << " is an invalid index, current rigid body contains " |
449 | > | << atoms_.size() << "atoms" << std::endl; |
450 | > | return false; |
451 | } | |
452 | ||
453 | < | } |
453 | > | } |
454 | ||
455 | < | bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) { |
455 | > | bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) { |
456 | std::vector<Atom*>::iterator i; | |
457 | i = std::find(atoms_.begin(), atoms_.end(), atom); | |
458 | if (i != atoms_.end()) { | |
459 | < | //RigidBody class makes sure refCoords_ and atoms_ match each other |
460 | < | coor = refCoords_[i - atoms_.begin()]; |
461 | < | return true; |
459 | > | //RigidBody class makes sure refCoords_ and atoms_ match each other |
460 | > | coor = refCoords_[i - atoms_.begin()]; |
461 | > | return true; |
462 | } else { | |
463 | < | std::cerr << "Atom " << atom->getGlobalIndex() |
464 | < | <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl; |
465 | < | return false; |
463 | > | std::cerr << "Atom " << atom->getGlobalIndex() |
464 | > | <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl; |
465 | > | return false; |
466 | } | |
467 | ||
468 | < | } |
468 | > | } |
469 | ||
470 | ||
471 | < | void RigidBody::addAtom(Atom* at, AtomStamp* ats) { |
471 | > | void RigidBody::addAtom(Atom* at, AtomStamp* ats) { |
472 | ||
473 | < | Vector3d coords; |
474 | < | Vector3d euler; |
473 | > | Vector3d coords; |
474 | > | Vector3d euler; |
475 | ||
476 | ||
477 | < | atoms_.push_back(at); |
477 | > | atoms_.push_back(at); |
478 | ||
479 | < | if( !ats->havePosition() ){ |
480 | < | sprintf( painCave.errMsg, |
481 | < | "RigidBody error.\n" |
482 | < | "\tAtom %s does not have a position specified.\n" |
483 | < | "\tThis means RigidBody cannot set up reference coordinates.\n", |
484 | < | ats->getType() ); |
485 | < | painCave.isFatal = 1; |
486 | < | simError(); |
487 | < | } |
479 | > | if( !ats->havePosition() ){ |
480 | > | sprintf( painCave.errMsg, |
481 | > | "RigidBody error.\n" |
482 | > | "\tAtom %s does not have a position specified.\n" |
483 | > | "\tThis means RigidBody cannot set up reference coordinates.\n", |
484 | > | ats->getType().c_str() ); |
485 | > | painCave.isFatal = 1; |
486 | > | simError(); |
487 | > | } |
488 | ||
489 | < | coords[0] = ats->getPosX(); |
490 | < | coords[1] = ats->getPosY(); |
491 | < | coords[2] = ats->getPosZ(); |
489 | > | coords[0] = ats->getPosX(); |
490 | > | coords[1] = ats->getPosY(); |
491 | > | coords[2] = ats->getPosZ(); |
492 | ||
493 | < | refCoords_.push_back(coords); |
493 | > | refCoords_.push_back(coords); |
494 | ||
495 | < | RotMat3x3d identMat = RotMat3x3d::identity(); |
495 | > | RotMat3x3d identMat = RotMat3x3d::identity(); |
496 | ||
497 | < | if (at->isDirectional()) { |
497 | > | if (at->isDirectional()) { |
498 | ||
499 | < | if( !ats->haveOrientation() ){ |
500 | < | sprintf( painCave.errMsg, |
501 | < | "RigidBody error.\n" |
502 | < | "\tAtom %s does not have an orientation specified.\n" |
503 | < | "\tThis means RigidBody cannot set up reference orientations.\n", |
504 | < | ats->getType() ); |
505 | < | painCave.isFatal = 1; |
506 | < | simError(); |
507 | < | } |
499 | > | if( !ats->haveOrientation() ){ |
500 | > | sprintf( painCave.errMsg, |
501 | > | "RigidBody error.\n" |
502 | > | "\tAtom %s does not have an orientation specified.\n" |
503 | > | "\tThis means RigidBody cannot set up reference orientations.\n", |
504 | > | ats->getType().c_str() ); |
505 | > | painCave.isFatal = 1; |
506 | > | simError(); |
507 | > | } |
508 | ||
509 | < | euler[0] = ats->getEulerPhi(); |
510 | < | euler[1] = ats->getEulerTheta(); |
511 | < | euler[2] = ats->getEulerPsi(); |
509 | > | euler[0] = ats->getEulerPhi() * NumericConstant::PI /180.0; |
510 | > | euler[1] = ats->getEulerTheta() * NumericConstant::PI /180.0; |
511 | > | euler[2] = ats->getEulerPsi() * NumericConstant::PI /180.0; |
512 | ||
513 | < | RotMat3x3d Atmp(euler); |
514 | < | refOrients_.push_back(Atmp); |
513 | > | RotMat3x3d Atmp(euler); |
514 | > | refOrients_.push_back(Atmp); |
515 | ||
516 | < | }else { |
517 | < | refOrients_.push_back(identMat); |
518 | < | } |
516 | > | }else { |
517 | > | refOrients_.push_back(identMat); |
518 | > | } |
519 | ||
520 | ||
521 | < | } |
521 | > | } |
522 | ||
523 | } | |
524 |
– | Removed lines |
+ | Added lines |
< | Changed lines |
> | Changed lines |