ViewVC Help
View File | Revision Log | Show Annotations | View Changeset | Root Listing
root/OpenMD/branches/development/src/primitives/RigidBody.cpp
(Generate patch)

Comparing trunk/src/primitives/RigidBody.cpp (file contents):
Revision 2 by gezelter, Fri Sep 24 04:16:43 2004 UTC vs.
Revision 695 by chrisfen, Sun Oct 23 21:08:08 2005 UTC

# Line 1 | Line 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
5 + * non-exclusive, royalty free, license to use, modify and
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
19 + *    notice, this list of conditions and the following disclaimer.
20 + *
21 + * 3. Redistributions in binary form must reproduce the above copyright
22 + *    notice, this list of conditions and the following disclaimer in the
23 + *    documentation and/or other materials provided with the
24 + *    distribution.
25 + *
26 + * This software is provided "AS IS," without a warranty of any
27 + * kind. All express or implied conditions, representations and
28 + * warranties, including any implied warranty of merchantability,
29 + * fitness for a particular purpose or non-infringement, are hereby
30 + * excluded.  The University of Notre Dame and its licensors shall not
31 + * be liable for any damages suffered by licensee as a result of
32 + * using, modifying or distributing the software or its
33 + * derivatives. In no event will the University of Notre Dame or its
34 + * licensors be liable for any lost revenue, profit or data, or for
35 + * direct, indirect, special, consequential, incidental or punitive
36 + * damages, however caused and regardless of the theory of liability,
37 + * arising out of the use of or inability to use software, even if the
38 + * University of Notre Dame has been advised of the possibility of
39 + * such damages.
40 + */
41 + #include <algorithm>
42   #include <math.h>
43 < #include "RigidBody.hpp"
44 < #include "DirectionalAtom.hpp"
45 < #include "simError.h"
46 < #include "MatVec3.h"
43 > #include "primitives/RigidBody.hpp"
44 > #include "utils/simError.h"
45 > #include "utils/NumericConstant.hpp"
46 > namespace oopse {
47  
48 < RigidBody::RigidBody() : StuntDouble() {
8 <  objType = OT_RIGIDBODY;
9 <  is_linear = false;
10 <  linear_axis =  -1;
11 <  momIntTol = 1e-6;
12 < }
48 >  RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData), inertiaTensor_(0.0){
49  
50 < RigidBody::~RigidBody() {
15 < }
50 >  }
51  
52 < void RigidBody::addAtom(Atom* at, AtomStamp* ats) {
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 <  vec3 coords;
57 <  vec3 euler;
58 <  mat3x3 Atmp;
56 >    for (int i =0 ; i < atoms_.size(); ++i){
57 >      if (atoms_[i]->isDirectional()) {
58 >        atoms_[i]->setPrevA(a * refOrients_[i]);
59 >      }
60 >    }
61  
23 –  myAtoms.push_back(at);
24 –
25 –  if( !ats->havePosition() ){
26 –    sprintf( painCave.errMsg,
27 –             "RigidBody error.\n"
28 –             "\tAtom %s does not have a position specified.\n"
29 –             "\tThis means RigidBody cannot set up reference coordinates.\n",
30 –             ats->getType() );
31 –    painCave.isFatal = 1;
32 –    simError();
62    }
34 –  
35 –  coords[0] = ats->getPosX();
36 –  coords[1] = ats->getPosY();
37 –  coords[2] = ats->getPosZ();
63  
64 <  refCoords.push_back(coords);
65 <  
66 <  if (at->isDirectional()) {  
64 >      
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 <    if( !ats->haveOrientation() ){
70 <      sprintf( painCave.errMsg,
71 <               "RigidBody error.\n"
72 <               "\tAtom %s does not have an orientation specified.\n"
73 <               "\tThis means RigidBody cannot set up reference orientations.\n",
74 <               ats->getType() );
49 <      painCave.isFatal = 1;
50 <      simError();
51 <    }    
69 >    for (int i =0 ; i < atoms_.size(); ++i){
70 >      if (atoms_[i]->isDirectional()) {
71 >        atoms_[i]->setA(a * refOrients_[i]);
72 >      }
73 >    }
74 >  }    
75      
76 <    euler[0] = ats->getEulerPhi();
77 <    euler[1] = ats->getEulerTheta();
78 <    euler[2] = ats->getEulerPsi();
56 <    
57 <    doEulerToRotMat(euler, Atmp);
58 <    
59 <    refOrients.push_back(Atmp);
60 <    
61 <  }
62 < }
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 < void RigidBody::getPos(double theP[3]){
81 <  for (int i = 0; i < 3 ; i++)
82 <    theP[i] = pos[i];
83 < }      
80 >    for (int i =0 ; i < atoms_.size(); ++i){
81 >      if (atoms_[i]->isDirectional()) {
82 >        atoms_[i]->setA(a * refOrients_[i], snapshotNo);
83 >      }
84 >    }
85  
86 < void RigidBody::setPos(double theP[3]){
70 <  for (int i = 0; i < 3 ; i++)
71 <    pos[i] = theP[i];
72 < }      
86 >  }  
87  
88 < void RigidBody::getVel(double theV[3]){
89 <  for (int i = 0; i < 3 ; i++)
90 <    theV[i] = vel[i];
77 < }      
88 >  Mat3x3d RigidBody::getI() {
89 >    return inertiaTensor_;
90 >  }    
91  
92 < void RigidBody::setVel(double theV[3]){
93 <  for (int i = 0; i < 3 ; i++)
94 <    vel[i] = theV[i];
95 < }      
92 >  std::vector<double> RigidBody::getGrad() {
93 >    std::vector<double> grad(6, 0.0);
94 >    Vector3d force;
95 >    Vector3d torque;
96 >    Vector3d myEuler;
97 >    double phi, theta, psi;
98 >    double cphi, sphi, ctheta, stheta;
99 >    Vector3d ephi;
100 >    Vector3d etheta;
101 >    Vector3d epsi;
102  
103 < void RigidBody::getFrc(double theF[3]){
104 <  for (int i = 0; i < 3 ; i++)
105 <    theF[i] = frc[i];
87 < }      
103 >    force = getFrc();
104 >    torque =getTrq();
105 >    myEuler = getA().toEulerAngles();
106  
107 < void RigidBody::addFrc(double theF[3]){
108 <  for (int i = 0; i < 3 ; i++)
109 <    frc[i] += theF[i];
92 < }    
107 >    phi = myEuler[0];
108 >    theta = myEuler[1];
109 >    psi = myEuler[2];
110  
111 < void RigidBody::zeroForces() {
111 >    cphi = cos(phi);
112 >    sphi = sin(phi);
113 >    ctheta = cos(theta);
114 >    stheta = sin(theta);
115  
116 <  for (int i = 0; i < 3; i++) {
97 <    frc[i] = 0.0;
98 <    trq[i] = 0.0;
99 <  }
116 >    // get unit vectors along the phi, theta and psi rotation axes
117  
118 < }
118 >    ephi[0] = 0.0;
119 >    ephi[1] = 0.0;
120 >    ephi[2] = 1.0;
121  
122 < void RigidBody::setEuler( double phi, double theta, double psi ){
123 <  
124 <    A[0][0] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi));
106 <    A[0][1] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi));
107 <    A[0][2] = sin(theta) * sin(psi);
108 <    
109 <    A[1][0] = -(cos(phi) * sin(psi)) - (sin(phi) * cos(theta) * cos(psi));
110 <    A[1][1] = -(sin(phi) * sin(psi)) + (cos(phi) * cos(theta) * cos(psi));
111 <    A[1][2] = sin(theta) * cos(psi);
112 <    
113 <    A[2][0] = sin(phi) * sin(theta);
114 <    A[2][1] = -cos(phi) * sin(theta);
115 <    A[2][2] = cos(theta);
122 >    etheta[0] = cphi;
123 >    etheta[1] = sphi;
124 >    etheta[2] = 0.0;
125  
126 < }
126 >    epsi[0] = stheta * cphi;
127 >    epsi[1] = stheta * sphi;
128 >    epsi[2] = ctheta;
129  
130 < void RigidBody::getQ( double q[4] ){
131 <  
132 <  double t, s;
133 <  double ad1, ad2, ad3;
130 >    //gradient is equal to -force
131 >    for (int j = 0 ; j<3; 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];
139 >
140 >    }
141      
142 <  t = A[0][0] + A[1][1] + A[2][2] + 1.0;
143 <  if( t > 0.0 ){
144 <    
145 <    s = 0.5 / sqrt( t );
146 <    q[0] = 0.25 / s;
147 <    q[1] = (A[1][2] - A[2][1]) * s;
148 <    q[2] = (A[2][0] - A[0][2]) * s;
149 <    q[3] = (A[0][1] - A[1][0]) * s;
150 <  }
151 <  else{
152 <    
153 <    ad1 = fabs( A[0][0] );
154 <    ad2 = fabs( A[1][1] );
155 <    ad3 = fabs( A[2][2] );
156 <    
157 <    if( ad1 >= ad2 && ad1 >= ad3 ){
140 <      
141 <      s = 2.0 * sqrt( 1.0 + A[0][0] - A[1][1] - A[2][2] );
142 <      q[0] = (A[1][2] + A[2][1]) / s;
143 <      q[1] = 0.5 / s;
144 <      q[2] = (A[0][1] + A[1][0]) / s;
145 <      q[3] = (A[0][2] + A[2][0]) / s;
142 >    return grad;
143 >  }    
144 >
145 >  void RigidBody::accept(BaseVisitor* v) {
146 >    v->visit(this);
147 >  }    
148 >
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;
158      }
159 <    else if( ad2 >= ad1 && ad2 >= ad3 ){
160 <      
161 <      s = sqrt( 1.0 + A[1][1] - A[0][0] - A[2][2] ) * 2.0;
162 <      q[0] = (A[0][2] + A[2][0]) / s;
163 <      q[1] = (A[0][1] + A[1][0]) / s;
152 <      q[2] = 0.5 / s;
153 <      q[3] = (A[1][2] + A[2][1]) / s;
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;
164      }
165 <    else{
166 <      
167 <      s = sqrt( 1.0 + A[2][2] - A[0][0] - A[1][1] ) * 2.0;
168 <      q[0] = (A[0][1] + A[1][0]) / s;
169 <      q[1] = (A[0][2] + A[2][0]) / s;
170 <      q[2] = (A[1][2] + A[2][1]) / s;
171 <      q[3] = 0.5 / s;
165 >
166 >    // Moment of Inertia calculation
167 >    Mat3x3d Itmp(0.0);    
168 >    for (std::size_t i = 0; i < atoms_.size(); i++) {
169 >      Mat3x3d IAtom(0.0);  
170 >      mtmp = atoms_[i]->getMass();
171 >      IAtom -= outProduct(refCoords_[i], refCoords_[i]) * mtmp;
172 >      double r2 = refCoords_[i].lengthSquare();
173 >      IAtom(0, 0) += mtmp * r2;
174 >      IAtom(1, 1) += mtmp * r2;
175 >      IAtom(2, 2) += mtmp * r2;
176 >      Itmp += IAtom;
177 >
178 >      //project the inertial moment of directional atoms into this rigid body
179 >      if (atoms_[i]->isDirectional()) {
180 >        Itmp += refOrients_[i].transpose() * atoms_[i]->getI() * refOrients_[i];
181 >      }
182      }
163 –  }
164 – }
183  
184 < void RigidBody::setQ( double the_q[4] ){
184 >    //    std::cout << Itmp << std::endl;
185  
186 <  double q0Sqr, q1Sqr, q2Sqr, q3Sqr;
187 <  
188 <  q0Sqr = the_q[0] * the_q[0];
171 <  q1Sqr = the_q[1] * the_q[1];
172 <  q2Sqr = the_q[2] * the_q[2];
173 <  q3Sqr = the_q[3] * the_q[3];
174 <  
175 <  A[0][0] = q0Sqr + q1Sqr - q2Sqr - q3Sqr;
176 <  A[0][1] = 2.0 * ( the_q[1] * the_q[2] + the_q[0] * the_q[3] );
177 <  A[0][2] = 2.0 * ( the_q[1] * the_q[3] - the_q[0] * the_q[2] );
178 <  
179 <  A[1][0] = 2.0 * ( the_q[1] * the_q[2] - the_q[0] * the_q[3] );
180 <  A[1][1] = q0Sqr - q1Sqr + q2Sqr - q3Sqr;
181 <  A[1][2] = 2.0 * ( the_q[2] * the_q[3] + the_q[0] * the_q[1] );
182 <  
183 <  A[2][0] = 2.0 * ( the_q[1] * the_q[3] + the_q[0] * the_q[2] );
184 <  A[2][1] = 2.0 * ( the_q[2] * the_q[3] - the_q[0] * the_q[1] );
185 <  A[2][2] = q0Sqr - q1Sqr -q2Sqr +q3Sqr;  
186 >    //diagonalize
187 >    Vector3d evals;
188 >    Mat3x3d::diagonalize(Itmp, evals, sU_);
189  
190 < }
190 >    // zero out I and then fill the diagonals with the moments of inertia:
191 >    inertiaTensor_(0, 0) = evals[0];
192 >    inertiaTensor_(1, 1) = evals[1];
193 >    inertiaTensor_(2, 2) = evals[2];
194 >        
195 >    int nLinearAxis = 0;
196 >    for (int i = 0; i < 3; i++) {    
197 >      if (fabs(evals[i]) < oopse::epsilon) {
198 >        linear_ = true;
199 >        linearAxis_ = i;
200 >        ++ nLinearAxis;
201 >      }
202 >    }
203  
204 < void RigidBody::getA( double the_A[3][3] ){
204 >    if (nLinearAxis > 1) {
205 >      sprintf( painCave.errMsg,
206 >               "RigidBody error.\n"
207 >               "\tOOPSE found more than one axis in this rigid body with a vanishing \n"
208 >               "\tmoment of inertia.  This can happen in one of three ways:\n"
209 >               "\t 1) Only one atom was specified, or \n"
210 >               "\t 2) All atoms were specified at the same location, or\n"
211 >               "\t 3) The programmers did something stupid.\n"
212 >               "\tIt is silly to use a rigid body to describe this situation.  Be smarter.\n"
213 >               );
214 >      painCave.isFatal = 1;
215 >      simError();
216 >    }
217    
218 <  for (int i = 0; i < 3; i++)
192 <    for (int j = 0; j < 3; j++)
193 <      the_A[i][j] = A[i][j];
218 >  }
219  
220 < }
220 >  void  RigidBody::calcForcesAndTorques() {
221 >    Vector3d afrc;
222 >    Vector3d atrq;
223 >    Vector3d apos;
224 >    Vector3d rpos;
225 >    Vector3d frc(0.0);
226 >    Vector3d trq(0.0);
227 >    Vector3d pos = this->getPos();
228 >    for (int i = 0; i < atoms_.size(); i++) {
229  
230 < void RigidBody::setA( double the_A[3][3] ){
230 >      afrc = atoms_[i]->getFrc();
231 >      apos = atoms_[i]->getPos();
232 >      rpos = apos - pos;
233 >        
234 >      frc += afrc;
235  
236 <  for (int i = 0; i < 3; i++)
237 <    for (int j = 0; j < 3; j++)
238 <      A[i][j] = the_A[i][j];
202 <  
203 < }
236 >      trq[0] += rpos[1]*afrc[2] - rpos[2]*afrc[1];
237 >      trq[1] += rpos[2]*afrc[0] - rpos[0]*afrc[2];
238 >      trq[2] += rpos[0]*afrc[1] - rpos[1]*afrc[0];
239  
240 < void RigidBody::getJ( double theJ[3] ){
241 <  
207 <  for (int i = 0; i < 3; i++)
208 <    theJ[i] = ji[i];
240 >      // If the atom has a torque associated with it, then we also need to
241 >      // migrate the torques onto the center of mass:
242  
243 < }
243 >      if (atoms_[i]->isDirectional()) {
244 >        atrq = atoms_[i]->getTrq();
245 >        trq += atrq;
246 >      }
247 >        
248 >    }
249 >    
250 >    setFrc(frc);
251 >    setTrq(trq);
252 >    
253 >  }
254  
255 < void RigidBody::setJ( double theJ[3] ){
256 <  
257 <  for (int i = 0; i < 3; i++)
258 <    ji[i] = theJ[i];
255 >  void  RigidBody::updateAtoms() {
256 >    unsigned int i;
257 >    Vector3d ref;
258 >    Vector3d apos;
259 >    DirectionalAtom* dAtom;
260 >    Vector3d pos = getPos();
261 >    RotMat3x3d a = getA();
262 >    
263 >    for (i = 0; i < atoms_.size(); i++) {
264 >    
265 >      ref = body2Lab(refCoords_[i]);
266  
267 < }
267 >      apos = pos + ref;
268  
269 < void RigidBody::getTrq(double theT[3]){
220 <  for (int i = 0; i < 3 ; i++)
221 <    theT[i] = trq[i];
222 < }      
269 >      atoms_[i]->setPos(apos);
270  
271 < void RigidBody::addTrq(double theT[3]){
272 <  for (int i = 0; i < 3 ; i++)
273 <    trq[i] += theT[i];
274 < }      
271 >      if (atoms_[i]->isDirectional()) {
272 >          
273 >        dAtom = (DirectionalAtom *) atoms_[i];
274 >        dAtom->setA(refOrients_[i] * a);
275 >      }
276  
277 < void RigidBody::getI( double the_I[3][3] ){  
277 >    }
278 >  
279 >  }
280  
231 –    for (int i = 0; i < 3; i++)
232 –      for (int j = 0; j < 3; j++)
233 –        the_I[i][j] = I[i][j];
281  
282 < }
282 >  void  RigidBody::updateAtoms(int frame) {
283 >    unsigned int i;
284 >    Vector3d ref;
285 >    Vector3d apos;
286 >    DirectionalAtom* dAtom;
287 >    Vector3d pos = getPos(frame);
288 >    RotMat3x3d a = getA(frame);
289 >    
290 >    for (i = 0; i < atoms_.size(); i++) {
291 >    
292 >      ref = body2Lab(refCoords_[i], frame);
293  
294 < void RigidBody::lab2Body( double r[3] ){
294 >      apos = pos + ref;
295  
296 <  double rl[3]; // the lab frame vector
296 >      atoms_[i]->setPos(apos, frame);
297 >
298 >      if (atoms_[i]->isDirectional()) {
299 >          
300 >        dAtom = (DirectionalAtom *) atoms_[i];
301 >        dAtom->setA(refOrients_[i] * a, frame);
302 >      }
303 >
304 >    }
305    
306 <  rl[0] = r[0];
242 <  rl[1] = r[1];
243 <  rl[2] = r[2];
244 <  
245 <  r[0] = (A[0][0] * rl[0]) + (A[0][1] * rl[1]) + (A[0][2] * rl[2]);
246 <  r[1] = (A[1][0] * rl[0]) + (A[1][1] * rl[1]) + (A[1][2] * rl[2]);
247 <  r[2] = (A[2][0] * rl[0]) + (A[2][1] * rl[1]) + (A[2][2] * rl[2]);
306 >  }
307  
308 < }
308 >  void RigidBody::updateAtomVel() {
309 >    Mat3x3d skewMat;;
310  
311 < void RigidBody::body2Lab( double r[3] ){
311 >    Vector3d ji = getJ();
312 >    Mat3x3d I =  getI();
313  
314 <  double rb[3]; // the body frame vector
315 <  
316 <  rb[0] = r[0];
256 <  rb[1] = r[1];
257 <  rb[2] = r[2];
258 <  
259 <  r[0] = (A[0][0] * rb[0]) + (A[1][0] * rb[1]) + (A[2][0] * rb[2]);
260 <  r[1] = (A[0][1] * rb[0]) + (A[1][1] * rb[1]) + (A[2][1] * rb[2]);
261 <  r[2] = (A[0][2] * rb[0]) + (A[1][2] * rb[1]) + (A[2][2] * rb[2]);
314 >    skewMat(0, 0) =0;
315 >    skewMat(0, 1) = ji[2] /I(2, 2);
316 >    skewMat(0, 2) = -ji[1] /I(1, 1);
317  
318 < }
318 >    skewMat(1, 0) = -ji[2] /I(2, 2);
319 >    skewMat(1, 1) = 0;
320 >    skewMat(1, 2) = ji[0]/I(0, 0);
321  
322 < double RigidBody::getZangle( ){
323 <    return zAngle;
324 < }
322 >    skewMat(2, 0) =ji[1] /I(1, 1);
323 >    skewMat(2, 1) = -ji[0]/I(0, 0);
324 >    skewMat(2, 2) = 0;
325  
326 < void RigidBody::setZangle( double zAng ){
327 <    zAngle = zAng;
271 < }
326 >    Mat3x3d mat = (getA() * skewMat).transpose();
327 >    Vector3d rbVel = getVel();
328  
273 – void RigidBody::addZangle( double zAng ){
274 –    zAngle += zAng;
275 – }
329  
330 < void RigidBody::calcRefCoords( ) {
330 >    Vector3d velRot;        
331 >    for (int i =0 ; i < refCoords_.size(); ++i) {
332 >      atoms_[i]->setVel(rbVel + mat * refCoords_[i]);
333 >    }
334  
335 <  int i,j,k, it, n_linear_coords;
280 <  double mtmp;
281 <  vec3 apos;
282 <  double refCOM[3];
283 <  vec3 ptmp;
284 <  double Itmp[3][3];
285 <  double evals[3];
286 <  double evects[3][3];
287 <  double r, r2, len;
335 >  }
336  
337 <  // First, find the center of mass:
338 <  
339 <  mass = 0.0;
340 <  for (j=0; j<3; j++)
341 <    refCOM[j] = 0.0;
342 <  
343 <  for (i = 0; i < myAtoms.size(); i++) {
344 <    mtmp = myAtoms[i]->getMass();
345 <    mass += mtmp;
337 >  void RigidBody::updateAtomVel(int frame) {
338 >    Mat3x3d skewMat;;
339 >
340 >    Vector3d ji = getJ(frame);
341 >    Mat3x3d I =  getI();
342 >
343 >    skewMat(0, 0) =0;
344 >    skewMat(0, 1) = ji[2] /I(2, 2);
345 >    skewMat(0, 2) = -ji[1] /I(1, 1);
346 >
347 >    skewMat(1, 0) = -ji[2] /I(2, 2);
348 >    skewMat(1, 1) = 0;
349 >    skewMat(1, 2) = ji[0]/I(0, 0);
350 >
351 >    skewMat(2, 0) =ji[1] /I(1, 1);
352 >    skewMat(2, 1) = -ji[0]/I(0, 0);
353 >    skewMat(2, 2) = 0;
354 >
355 >    Mat3x3d mat = (getA(frame) * skewMat).transpose();
356 >    Vector3d rbVel = getVel(frame);
357 >
358 >
359 >    Vector3d velRot;        
360 >    for (int i =0 ; i < refCoords_.size(); ++i) {
361 >      atoms_[i]->setVel(rbVel + mat * refCoords_[i], frame);
362 >    }
363  
299 –    apos = refCoords[i];
300 –    
301 –    for(j = 0; j < 3; j++) {
302 –      refCOM[j] += apos[j]*mtmp;    
303 –    }    
364    }
305 –  
306 –  for(j = 0; j < 3; j++)
307 –    refCOM[j] /= mass;
365  
309 – // Next, move the origin of the reference coordinate system to the COM:
310 –
311 –  for (i = 0; i < myAtoms.size(); i++) {
312 –    apos = refCoords[i];
313 –    for (j=0; j < 3; j++) {
314 –      apos[j] = apos[j] - refCOM[j];
315 –    }
316 –    refCoords[i] = apos;
317 –  }
318 –
319 – // Moment of Inertia calculation
320 –
321 –  for (i = 0; i < 3; i++)
322 –    for (j = 0; j < 3; j++)
323 –      Itmp[i][j] = 0.0;  
324 –  
325 –  for (it = 0; it < myAtoms.size(); it++) {
326 –
327 –    mtmp = myAtoms[it]->getMass();
328 –    ptmp = refCoords[it];
329 –    r= norm3(ptmp.vec);
330 –    r2 = r*r;
331 –    
332 –    for (i = 0; i < 3; i++) {
333 –      for (j = 0; j < 3; j++) {
366          
335 –        if (i==j) Itmp[i][j] += mtmp * r2;
367  
368 <        Itmp[i][j] -= mtmp * ptmp.vec[i]*ptmp.vec[j];
369 <      }
339 <    }
340 <  }
341 <  
342 <  diagonalize3x3(Itmp, evals, sU);
343 <  
344 <  // zero out I and then fill the diagonals with the moments of inertia:
368 >  bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
369 >    if (index < atoms_.size()) {
370  
371 <  n_linear_coords = 0;
372 <
373 <  for (i = 0; i < 3; i++) {
374 <    for (j = 0; j < 3; j++) {
375 <      I[i][j] = 0.0;  
376 <    }
377 <    I[i][i] = evals[i];
378 <
354 <    if (fabs(evals[i]) < momIntTol) {
355 <      is_linear = true;
356 <      n_linear_coords++;
357 <      linear_axis = i;
358 <    }
371 >      Vector3d ref = body2Lab(refCoords_[index]);
372 >      pos = getPos() + ref;
373 >      return true;
374 >    } else {
375 >      std::cerr << index << " is an invalid index, current rigid body contains "
376 >                << atoms_.size() << "atoms" << std::endl;
377 >      return false;
378 >    }    
379    }
380  
381 <  if (n_linear_coords > 1) {
382 <          sprintf( painCave.errMsg,
383 <               "RigidBody error.\n"
384 <               "\tOOPSE found more than one axis in this rigid body with a vanishing \n"
385 <               "\tmoment of inertia.  This can happen in one of three ways:\n"
386 <               "\t 1) Only one atom was specified, or \n"
387 <               "\t 2) All atoms were specified at the same location, or\n"
388 <               "\t 3) The programmers did something stupid.\n"
389 <               "\tIt is silly to use a rigid body to describe this situation.  Be smarter.\n"
390 <               );
391 <      painCave.isFatal = 1;
392 <      simError();
373 <  }
374 <  
375 <  // renormalize column vectors:
376 <  
377 <  for (i=0; i < 3; i++) {
378 <    len = 0.0;
379 <    for (j = 0; j < 3; j++) {
380 <      len += sU[i][j]*sU[i][j];
381 >  bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) {
382 >    std::vector<Atom*>::iterator i;
383 >    i = std::find(atoms_.begin(), atoms_.end(), atom);
384 >    if (i != atoms_.end()) {
385 >      //RigidBody class makes sure refCoords_ and atoms_ match each other
386 >      Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]);
387 >      pos = getPos() + ref;
388 >      return true;
389 >    } else {
390 >      std::cerr << "Atom " << atom->getGlobalIndex()
391 >                <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
392 >      return false;
393      }
382 –    len = sqrt(len);
383 –    for (j = 0; j < 3; j++) {
384 –      sU[i][j] /= len;
385 –    }
394    }
395 < }
395 >  bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {
396  
397 < void RigidBody::doEulerToRotMat(vec3 &euler, mat3x3 &myA ){
397 >    //velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
398  
399 <  double phi, theta, psi;
392 <  
393 <  phi = euler[0];
394 <  theta = euler[1];
395 <  psi = euler[2];
396 <  
397 <  myA[0][0] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi));
398 <  myA[0][1] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi));
399 <  myA[0][2] = sin(theta) * sin(psi);
400 <  
401 <  myA[1][0] = -(cos(phi) * sin(psi)) - (sin(phi) * cos(theta) * cos(psi));
402 <  myA[1][1] = -(sin(phi) * sin(psi)) + (cos(phi) * cos(theta) * cos(psi));
403 <  myA[1][2] = sin(theta) * cos(psi);
404 <  
405 <  myA[2][0] = sin(phi) * sin(theta);
406 <  myA[2][1] = -cos(phi) * sin(theta);
407 <  myA[2][2] = cos(theta);
399 >    if (index < atoms_.size()) {
400  
401 < }
401 >      Vector3d velRot;
402 >      Mat3x3d skewMat;;
403 >      Vector3d ref = refCoords_[index];
404 >      Vector3d ji = getJ();
405 >      Mat3x3d I =  getI();
406  
407 < void RigidBody::calcForcesAndTorques() {
407 >      skewMat(0, 0) =0;
408 >      skewMat(0, 1) = ji[2] /I(2, 2);
409 >      skewMat(0, 2) = -ji[1] /I(1, 1);
410  
411 <  // Convert Atomic forces and torques to total forces and torques:
412 <  int i, j;
413 <  double apos[3];
416 <  double afrc[3];
417 <  double atrq[3];
418 <  double rpos[3];
411 >      skewMat(1, 0) = -ji[2] /I(2, 2);
412 >      skewMat(1, 1) = 0;
413 >      skewMat(1, 2) = ji[0]/I(0, 0);
414  
415 <  zeroForces();
416 <  
417 <  for (i = 0; i < myAtoms.size(); i++) {
415 >      skewMat(2, 0) =ji[1] /I(1, 1);
416 >      skewMat(2, 1) = -ji[0]/I(0, 0);
417 >      skewMat(2, 2) = 0;
418  
419 <    myAtoms[i]->getPos(apos);
425 <    myAtoms[i]->getFrc(afrc);
419 >      velRot = (getA() * skewMat).transpose() * ref;
420  
421 <    for (j=0; j<3; j++) {
422 <      rpos[j] = apos[j] - pos[j];
423 <      frc[j] += afrc[j];
421 >      vel =getVel() + velRot;
422 >      return true;
423 >        
424 >    } else {
425 >      std::cerr << index << " is an invalid index, current rigid body contains "
426 >                << atoms_.size() << "atoms" << std::endl;
427 >      return false;
428      }
429 <    
432 <    trq[0] += rpos[1]*afrc[2] - rpos[2]*afrc[1];
433 <    trq[1] += rpos[2]*afrc[0] - rpos[0]*afrc[2];
434 <    trq[2] += rpos[0]*afrc[1] - rpos[1]*afrc[0];
429 >  }
430  
431 <    // If the atom has a torque associated with it, then we also need to
437 <    // migrate the torques onto the center of mass:
431 >  bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) {
432  
433 <    if (myAtoms[i]->isDirectional()) {
434 <
435 <      myAtoms[i]->getTrq(atrq);
436 <      
437 <      for (j=0; j<3; j++)
438 <        trq[j] += atrq[j];
439 <    }
433 >    std::vector<Atom*>::iterator i;
434 >    i = std::find(atoms_.begin(), atoms_.end(), atom);
435 >    if (i != atoms_.end()) {
436 >      return getAtomVel(vel, i - atoms_.begin());
437 >    } else {
438 >      std::cerr << "Atom " << atom->getGlobalIndex()
439 >                <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;    
440 >      return false;
441 >    }    
442    }
443  
444 <  // Convert Torque to Body-fixed coordinates:
445 <  // (Actually, on second thought, don't.  Integrator does this now.)
450 <  // lab2Body(trq);
444 >  bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) {
445 >    if (index < atoms_.size()) {
446  
447 < }
447 >      coor = refCoords_[index];
448 >      return true;
449 >    } else {
450 >      std::cerr << index << " is an invalid index, current rigid body contains "
451 >                << atoms_.size() << "atoms" << std::endl;
452 >      return false;
453 >    }
454  
455 < void RigidBody::updateAtoms() {
455 <  int i, j;
456 <  vec3 ref;
457 <  double apos[3];
458 <  DirectionalAtom* dAtom;
459 <  
460 <  for (i = 0; i < myAtoms.size(); i++) {
461 <    
462 <    ref = refCoords[i];
455 >  }
456  
457 <    body2Lab(ref.vec);
458 <    
459 <    for (j = 0; j<3; j++)
460 <      apos[j] = pos[j] + ref.vec[j];
461 <    
462 <    myAtoms[i]->setPos(apos);
463 <    
464 <    if (myAtoms[i]->isDirectional()) {
465 <      
466 <      dAtom = (DirectionalAtom *) myAtoms[i];
467 <      dAtom->rotateBy( A );
475 <      
457 >  bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) {
458 >    std::vector<Atom*>::iterator i;
459 >    i = std::find(atoms_.begin(), atoms_.end(), atom);
460 >    if (i != atoms_.end()) {
461 >      //RigidBody class makes sure refCoords_ and atoms_ match each other
462 >      coor = refCoords_[i - atoms_.begin()];
463 >      return true;
464 >    } else {
465 >      std::cerr << "Atom " << atom->getGlobalIndex()
466 >                <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;    
467 >      return false;
468      }
477 –  }  
478 – }
469  
470 < void RigidBody::getGrad( double grad[6] ) {
470 >  }
471  
482 –  double myEuler[3];
483 –  double phi, theta, psi;
484 –  double cphi, sphi, ctheta, stheta;
485 –  double ephi[3];
486 –  double etheta[3];
487 –  double epsi[3];
488 –  
489 –  this->getEulerAngles(myEuler);
472  
473 <  phi = myEuler[0];
492 <  theta = myEuler[1];
493 <  psi = myEuler[2];
473 >  void RigidBody::addAtom(Atom* at, AtomStamp* ats) {
474  
475 <  cphi = cos(phi);
476 <  sphi = sin(phi);
497 <  ctheta = cos(theta);
498 <  stheta = sin(theta);
499 <
500 <  // get unit vectors along the phi, theta and psi rotation axes
501 <
502 <  ephi[0] = 0.0;
503 <  ephi[1] = 0.0;
504 <  ephi[2] = 1.0;
505 <
506 <  etheta[0] = cphi;
507 <  etheta[1] = sphi;
508 <  etheta[2] = 0.0;
475 >    Vector3d coords;
476 >    Vector3d euler;
477    
510 –  epsi[0] = stheta * cphi;
511 –  epsi[1] = stheta * sphi;
512 –  epsi[2] = ctheta;
513 –  
514 –  for (int j = 0 ; j<3; j++)
515 –    grad[j] = frc[j];
478  
479 <  grad[3] = 0.0;
480 <  grad[4] = 0.0;
481 <  grad[5] = 0.0;
479 >    atoms_.push_back(at);
480 >
481 >    if( !ats->havePosition() ){
482 >      sprintf( painCave.errMsg,
483 >               "RigidBody error.\n"
484 >               "\tAtom %s does not have a position specified.\n"
485 >               "\tThis means RigidBody cannot set up reference coordinates.\n",
486 >               ats->getType() );
487 >      painCave.isFatal = 1;
488 >      simError();
489 >    }
490    
491 <  for (int j = 0; j < 3; j++ ) {
492 <    
493 <    grad[3] += trq[j]*ephi[j];
524 <    grad[4] += trq[j]*etheta[j];
525 <    grad[5] += trq[j]*epsi[j];
526 <    
527 <  }
528 <  
529 < }
491 >    coords[0] = ats->getPosX();
492 >    coords[1] = ats->getPosY();
493 >    coords[2] = ats->getPosZ();
494  
495 < /**
532 <  * getEulerAngles computes a set of Euler angle values consistent
533 <  * with an input rotation matrix.  They are returned in the following
534 <  * order:
535 <  *  myEuler[0] = phi;
536 <  *  myEuler[1] = theta;
537 <  *  myEuler[2] = psi;
538 < */
539 < void RigidBody::getEulerAngles(double myEuler[3]) {
495 >    refCoords_.push_back(coords);
496  
497 <  // We use so-called "x-convention", which is the most common
542 <  // definition.  In this convention, the rotation given by Euler
543 <  // angles (phi, theta, psi), where the first rotation is by an angle
544 <  // phi about the z-axis, the second is by an angle theta (0 <= theta
545 <  // <= 180) about the x-axis, and the third is by an angle psi about
546 <  // the z-axis (again).
497 >    RotMat3x3d identMat = RotMat3x3d::identity();
498    
499 <  
549 <  double phi,theta,psi,eps;
550 <  double pi;
551 <  double cphi,ctheta,cpsi;
552 <  double sphi,stheta,spsi;
553 <  double b[3];
554 <  int flip[3];
555 <  
556 <  // set the tolerance for Euler angles and rotation elements
557 <  
558 <  eps = 1.0e-8;
499 >    if (at->isDirectional()) {  
500  
501 <  theta = acos(min(1.0,max(-1.0,A[2][2])));
502 <  ctheta = A[2][2];
503 <  stheta = sqrt(1.0 - ctheta * ctheta);
501 >      if( !ats->haveOrientation() ){
502 >        sprintf( painCave.errMsg,
503 >                 "RigidBody error.\n"
504 >                 "\tAtom %s does not have an orientation specified.\n"
505 >                 "\tThis means RigidBody cannot set up reference orientations.\n",
506 >                 ats->getType() );
507 >        painCave.isFatal = 1;
508 >        simError();
509 >      }    
510 >    
511 >      euler[0] = ats->getEulerPhi() * NumericConstant::PI /180.0;
512 >      euler[1] = ats->getEulerTheta() * NumericConstant::PI /180.0;
513 >      euler[2] = ats->getEulerPsi() * NumericConstant::PI /180.0;
514  
515 <  // when sin(theta) is close to 0, we need to consider the
516 <  // possibility of a singularity. In this case, we can assign an
566 <  // arbitary value to phi (or psi), and then determine the psi (or
567 <  // phi) or vice-versa.  We'll assume that phi always gets the
568 <  // rotation, and psi is 0 in cases of singularity.  we use atan2
569 <  // instead of atan, since atan2 will give us -Pi to Pi.  Since 0 <=
570 <  // theta <= 180, sin(theta) will be always non-negative. Therefore,
571 <  // it never changes the sign of both of the parameters passed to
572 <  // atan2.
573 <  
574 <  if (fabs(stheta) <= eps){
575 <    psi = 0.0;
576 <    phi = atan2(-A[1][0], A[0][0]);  
577 <  }
578 <  // we only have one unique solution
579 <  else{    
580 <    phi = atan2(A[2][0], -A[2][1]);
581 <    psi = atan2(A[0][2], A[1][2]);
582 <  }
583 <  
584 <  //wrap phi and psi, make sure they are in the range from 0 to 2*Pi
585 <  //if (phi < 0)
586 <  //  phi += M_PI;
587 <  
588 <  //if (psi < 0)
589 <  //  psi += M_PI;
590 <  
591 <  myEuler[0] = phi;
592 <  myEuler[1] = theta;
593 <  myEuler[2] = psi;
594 <  
595 <  return;
596 < }
597 <
598 < double RigidBody::max(double x, double  y) {  
599 <  return (x > y) ? x : y;
600 < }
601 <
602 < double RigidBody::min(double x, double  y) {  
603 <  return (x > y) ? y : x;
604 < }
605 <
606 < void RigidBody::findCOM() {
607 <  
608 <  size_t i;
609 <  int j;
610 <  double mtmp;
611 <  double ptmp[3];
612 <  double vtmp[3];
613 <  
614 <  for(j = 0; j < 3; j++) {
615 <    pos[j] = 0.0;
616 <    vel[j] = 0.0;
617 <  }
618 <  mass = 0.0;
619 <  
620 <  for (i = 0; i < myAtoms.size(); i++) {
515 >      RotMat3x3d Atmp(euler);
516 >      refOrients_.push_back(Atmp);
517      
518 <    mtmp = myAtoms[i]->getMass();    
519 <    myAtoms[i]->getPos(ptmp);
624 <    myAtoms[i]->getVel(vtmp);
625 <    
626 <    mass += mtmp;
627 <    
628 <    for(j = 0; j < 3; j++) {
629 <      pos[j] += ptmp[j]*mtmp;
630 <      vel[j] += vtmp[j]*mtmp;
518 >    }else {
519 >      refOrients_.push_back(identMat);
520      }
632 –    
633 –  }
521    
522 <  for(j = 0; j < 3; j++) {
636 <    pos[j] /= mass;
637 <    vel[j] /= mass;
522 >  
523    }
524  
525   }
526  
642 – void RigidBody::accept(BaseVisitor* v){
643 –  vector<Atom*>::iterator atomIter;
644 –  v->visit(this);
645 –
646 –  //for(atomIter = myAtoms.begin(); atomIter != myAtoms.end(); ++atomIter)
647 –  //  (*atomIter)->accept(v);
648 – }
649 – void RigidBody::getAtomRefCoor(double pos[3], int index){
650 –  vec3 ref;
651 –
652 –  ref = refCoords[index];
653 –  pos[0] = ref[0];
654 –  pos[1] = ref[1];
655 –  pos[2] = ref[2];
656 –  
657 – }
658 –
659 –
660 – void RigidBody::getAtomPos(double theP[3], int index){
661 –  vec3 ref;
662 –
663 –  if (index >= myAtoms.size())
664 –    cerr << index << " is an invalid index, current rigid body contains " << myAtoms.size() << "atoms" << endl;
665 –
666 –  ref = refCoords[index];
667 –  body2Lab(ref.vec);
668 –  
669 –  theP[0] = pos[0] + ref[0];
670 –  theP[1] = pos[1] + ref[1];
671 –  theP[2] = pos[2] + ref[2];
672 – }
673 –
674 –
675 – void RigidBody::getAtomVel(double theV[3], int index){
676 –  vec3 ref;
677 –  double velRot[3];
678 –  double skewMat[3][3];
679 –  double aSkewMat[3][3];
680 –  double aSkewTransMat[3][3];
681 –  
682 –  //velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
683 –
684 –  if (index >= myAtoms.size())
685 –    cerr << index << " is an invalid index, current rigid body contains " << myAtoms.size() << "atoms" << endl;
686 –
687 –  ref = refCoords[index];
688 –
689 –  skewMat[0][0] =0;
690 –  skewMat[0][1] = ji[2] /I[2][2];
691 –  skewMat[0][2] = -ji[1] /I[1][1];
692 –
693 –  skewMat[1][0] = -ji[2] /I[2][2];
694 –  skewMat[1][1] = 0;
695 –  skewMat[1][2] = ji[0]/I[0][0];
696 –
697 –  skewMat[2][0] =ji[1] /I[1][1];
698 –  skewMat[2][1] = -ji[0]/I[0][0];
699 –  skewMat[2][2] = 0;
700 –  
701 –  matMul3(A, skewMat, aSkewMat);
702 –
703 –  transposeMat3(aSkewMat, aSkewTransMat);
704 –
705 –  matVecMul3(aSkewTransMat, ref.vec, velRot);
706 –  theV[0] = vel[0] + velRot[0];
707 –  theV[1] = vel[1] + velRot[1];
708 –  theV[2] = vel[2] + velRot[2];
709 – }
710 –
711 –

Diff Legend

– Removed lines
+ Added lines
< Changed lines
> Changed lines