ViewVC Help
View File | Revision Log | Show Annotations | View Changeset | Root Listing
root/group/trunk/OOPSE-4/src/primitives/RigidBody.cpp
(Generate patch)

Comparing trunk/OOPSE-4/src/primitives/RigidBody.cpp (file contents):
Revision 1490 by gezelter, Fri Sep 24 04:16:43 2004 UTC vs.
Revision 2204 by gezelter, Fri Apr 15 22:04:00 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;
122 <  double ad1, ad2, ad3;
123 <    
124 <  t = A[0][0] + A[1][1] + A[2][2] + 1.0;
125 <  if( t > 0.0 ){
126 <    
127 <    s = 0.5 / sqrt( t );
128 <    q[0] = 0.25 / s;
129 <    q[1] = (A[1][2] - A[2][1]) * s;
130 <    q[2] = (A[2][0] - A[0][2]) * s;
131 <    q[3] = (A[0][1] - A[1][0]) * s;
132 <  }
133 <  else{
134 <    
135 <    ad1 = fabs( A[0][0] );
136 <    ad2 = fabs( A[1][1] );
137 <    ad3 = fabs( A[2][2] );
138 <    
139 <    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;
146 <    }
147 <    else if( ad2 >= ad1 && ad2 >= ad3 ){
148 <      
149 <      s = sqrt( 1.0 + A[1][1] - A[0][0] - A[2][2] ) * 2.0;
150 <      q[0] = (A[0][2] + A[2][0]) / s;
151 <      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;
154 <    }
155 <    else{
156 <      
157 <      s = sqrt( 1.0 + A[2][2] - A[0][0] - A[1][1] ) * 2.0;
158 <      q[0] = (A[0][1] + A[1][0]) / s;
159 <      q[1] = (A[0][2] + A[2][0]) / s;
160 <      q[2] = (A[1][2] + A[2][1]) / s;
161 <      q[3] = 0.5 / s;
162 <    }
163 <  }
164 < }
130 >    //gradient is equal to -force
131 >    for (int j = 0 ; j<3; j++)
132 >      grad[j] = -force[j];
133  
134 < void RigidBody::setQ( double the_q[4] ){
134 >    for (int j = 0; j < 3; j++ ) {
135  
136 <  double q0Sqr, q1Sqr, q2Sqr, q3Sqr;
137 <  
138 <  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;  
136 >      grad[3] += torque[j]*ephi[j];
137 >      grad[4] += torque[j]*etheta[j];
138 >      grad[5] += torque[j]*epsi[j];
139  
140 < }
140 >    }
141 >    
142 >    return grad;
143 >  }    
144  
145 < void RigidBody::getA( double the_A[3][3] ){
146 <  
147 <  for (int i = 0; i < 3; i++)
192 <    for (int j = 0; j < 3; j++)
193 <      the_A[i][j] = A[i][j];
145 >  void RigidBody::accept(BaseVisitor* v) {
146 >    v->visit(this);
147 >  }    
148  
149 < }
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 >    refCOM /= mass_;
160  
161 < void RigidBody::setA( double the_A[3][3] ){
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  
166 <  for (int i = 0; i < 3; i++)
167 <    for (int j = 0; j < 3; j++)
201 <      A[i][j] = the_A[i][j];
202 <  
203 < }
204 <
205 < void RigidBody::getJ( double theJ[3] ){
166 >    // Moment of Inertia calculation
167 >    Mat3x3d Itmp(0.0);
168    
169 <  for (int i = 0; i < 3; i++)
170 <    theJ[i] = ji[i];
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;
176 >    }
177  
178 < }
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);
183 >        Itmp(1, 1) += Iproject(1, 1);
184 >        Itmp(2, 2) += Iproject(2, 2);
185 >      }
186 >    }
187  
188 < void RigidBody::setJ( double theJ[3] ){
188 >    //diagonalize
189 >    Vector3d evals;
190 >    Mat3x3d::diagonalize(Itmp, evals, sU_);
191 >
192 >    // zero out I and then fill the diagonals with the moments of inertia:
193 >    inertiaTensor_(0, 0) = evals[0];
194 >    inertiaTensor_(1, 1) = evals[1];
195 >    inertiaTensor_(2, 2) = evals[2];
196 >        
197 >    int nLinearAxis = 0;
198 >    for (int i = 0; i < 3; i++) {    
199 >      if (fabs(evals[i]) < oopse::epsilon) {
200 >        linear_ = true;
201 >        linearAxis_ = i;
202 >        ++ nLinearAxis;
203 >      }
204 >    }
205 >
206 >    if (nLinearAxis > 1) {
207 >      sprintf( painCave.errMsg,
208 >               "RigidBody error.\n"
209 >               "\tOOPSE found more than one axis in this rigid body with a vanishing \n"
210 >               "\tmoment of inertia.  This can happen in one of three ways:\n"
211 >               "\t 1) Only one atom was specified, or \n"
212 >               "\t 2) All atoms were specified at the same location, or\n"
213 >               "\t 3) The programmers did something stupid.\n"
214 >               "\tIt is silly to use a rigid body to describe this situation.  Be smarter.\n"
215 >               );
216 >      painCave.isFatal = 1;
217 >      simError();
218 >    }
219    
220 <  for (int i = 0; i < 3; i++)
215 <    ji[i] = theJ[i];
220 >  }
221  
222 < }
222 >  void  RigidBody::calcForcesAndTorques() {
223 >    Vector3d afrc;
224 >    Vector3d atrq;
225 >    Vector3d apos;
226 >    Vector3d rpos;
227 >    Vector3d frc(0.0);
228 >    Vector3d trq(0.0);
229 >    Vector3d pos = this->getPos();
230 >    for (int i = 0; i < atoms_.size(); i++) {
231  
232 < void RigidBody::getTrq(double theT[3]){
233 <  for (int i = 0; i < 3 ; i++)
234 <    theT[i] = trq[i];
235 < }      
232 >      afrc = atoms_[i]->getFrc();
233 >      apos = atoms_[i]->getPos();
234 >      rpos = apos - pos;
235 >        
236 >      frc += afrc;
237  
238 < void RigidBody::addTrq(double theT[3]){
239 <  for (int i = 0; i < 3 ; i++)
240 <    trq[i] += theT[i];
227 < }      
238 >      trq[0] += rpos[1]*afrc[2] - rpos[2]*afrc[1];
239 >      trq[1] += rpos[2]*afrc[0] - rpos[0]*afrc[2];
240 >      trq[2] += rpos[0]*afrc[1] - rpos[1]*afrc[0];
241  
242 < void RigidBody::getI( double the_I[3][3] ){  
242 >      // If the atom has a torque associated with it, then we also need to
243 >      // migrate the torques onto the center of mass:
244  
245 <    for (int i = 0; i < 3; i++)
246 <      for (int j = 0; j < 3; j++)
247 <        the_I[i][j] = I[i][j];
245 >      if (atoms_[i]->isDirectional()) {
246 >        atrq = atoms_[i]->getTrq();
247 >        trq += atrq;
248 >      }
249 >        
250 >    }
251 >    
252 >    setFrc(frc);
253 >    setTrq(trq);
254 >    
255 >  }
256  
257 < }
257 >  void  RigidBody::updateAtoms() {
258 >    unsigned int i;
259 >    Vector3d ref;
260 >    Vector3d apos;
261 >    DirectionalAtom* dAtom;
262 >    Vector3d pos = getPos();
263 >    RotMat3x3d a = getA();
264 >    
265 >    for (i = 0; i < atoms_.size(); i++) {
266 >    
267 >      ref = body2Lab(refCoords_[i]);
268  
269 < void RigidBody::lab2Body( double r[3] ){
269 >      apos = pos + ref;
270  
271 <  double rl[3]; // the lab frame vector
271 >      atoms_[i]->setPos(apos);
272 >
273 >      if (atoms_[i]->isDirectional()) {
274 >          
275 >        dAtom = (DirectionalAtom *) atoms_[i];
276 >        dAtom->setA(a * refOrients_[i]);
277 >        //dAtom->rotateBy( A );      
278 >      }
279 >
280 >    }
281    
282 <  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]);
282 >  }
283  
249 – }
284  
285 < void RigidBody::body2Lab( double r[3] ){
285 >  void  RigidBody::updateAtoms(int frame) {
286 >    unsigned int i;
287 >    Vector3d ref;
288 >    Vector3d apos;
289 >    DirectionalAtom* dAtom;
290 >    Vector3d pos = getPos(frame);
291 >    RotMat3x3d a = getA(frame);
292 >    
293 >    for (i = 0; i < atoms_.size(); i++) {
294 >    
295 >      ref = body2Lab(refCoords_[i], frame);
296  
297 <  double rb[3]; // the body frame vector
297 >      apos = pos + ref;
298 >
299 >      atoms_[i]->setPos(apos, frame);
300 >
301 >      if (atoms_[i]->isDirectional()) {
302 >          
303 >        dAtom = (DirectionalAtom *) atoms_[i];
304 >        dAtom->setA(a * refOrients_[i], frame);
305 >      }
306 >
307 >    }
308    
309 <  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]);
309 >  }
310  
311 < }
311 >  void RigidBody::updateAtomVel() {
312 >    Mat3x3d skewMat;;
313  
314 < double RigidBody::getZangle( ){
315 <    return zAngle;
267 < }
314 >    Vector3d ji = getJ();
315 >    Mat3x3d I =  getI();
316  
317 < void RigidBody::setZangle( double zAng ){
318 <    zAngle = zAng;
319 < }
317 >    skewMat(0, 0) =0;
318 >    skewMat(0, 1) = ji[2] /I(2, 2);
319 >    skewMat(0, 2) = -ji[1] /I(1, 1);
320  
321 < void RigidBody::addZangle( double zAng ){
322 <    zAngle += zAng;
323 < }
321 >    skewMat(1, 0) = -ji[2] /I(2, 2);
322 >    skewMat(1, 1) = 0;
323 >    skewMat(1, 2) = ji[0]/I(0, 0);
324  
325 < void RigidBody::calcRefCoords( ) {
325 >    skewMat(2, 0) =ji[1] /I(1, 1);
326 >    skewMat(2, 1) = -ji[0]/I(0, 0);
327 >    skewMat(2, 2) = 0;
328  
329 <  int i,j,k, it, n_linear_coords;
330 <  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;
329 >    Mat3x3d mat = (getA() * skewMat).transpose();
330 >    Vector3d rbVel = getVel();
331  
289 –  // First, find the center of mass:
290 –  
291 –  mass = 0.0;
292 –  for (j=0; j<3; j++)
293 –    refCOM[j] = 0.0;
294 –  
295 –  for (i = 0; i < myAtoms.size(); i++) {
296 –    mtmp = myAtoms[i]->getMass();
297 –    mass += mtmp;
332  
333 <    apos = refCoords[i];
334 <    
335 <    for(j = 0; j < 3; j++) {
336 <      refCOM[j] += apos[j]*mtmp;    
337 <    }    
338 <  }
339 <  
340 <  for(j = 0; j < 3; j++)
341 <    refCOM[j] /= mass;
333 >    Vector3d velRot;        
334 >    for (int i =0 ; i < refCoords_.size(); ++i) {
335 >      atoms_[i]->setVel(rbVel + mat * refCoords_[i]);
336 >    }
337 >
338 >  }
339 >
340 >  void RigidBody::updateAtomVel(int frame) {
341 >    Mat3x3d skewMat;;
342 >
343 >    Vector3d ji = getJ(frame);
344 >    Mat3x3d I =  getI();
345 >
346 >    skewMat(0, 0) =0;
347 >    skewMat(0, 1) = ji[2] /I(2, 2);
348 >    skewMat(0, 2) = -ji[1] /I(1, 1);
349 >
350 >    skewMat(1, 0) = -ji[2] /I(2, 2);
351 >    skewMat(1, 1) = 0;
352 >    skewMat(1, 2) = ji[0]/I(0, 0);
353 >
354 >    skewMat(2, 0) =ji[1] /I(1, 1);
355 >    skewMat(2, 1) = -ji[0]/I(0, 0);
356 >    skewMat(2, 2) = 0;
357 >
358 >    Mat3x3d mat = (getA(frame) * skewMat).transpose();
359 >    Vector3d rbVel = getVel(frame);
360  
309 – // Next, move the origin of the reference coordinate system to the COM:
361  
362 <  for (i = 0; i < myAtoms.size(); i++) {
363 <    apos = refCoords[i];
364 <    for (j=0; j < 3; j++) {
314 <      apos[j] = apos[j] - refCOM[j];
362 >    Vector3d velRot;        
363 >    for (int i =0 ; i < refCoords_.size(); ++i) {
364 >      atoms_[i]->setVel(rbVel + mat * refCoords_[i], frame);
365      }
366 <    refCoords[i] = apos;
366 >
367    }
368  
369 < // Moment of Inertia calculation
369 >        
370  
371 <  for (i = 0; i < 3; i++)
372 <    for (j = 0; j < 3; j++)
323 <      Itmp[i][j] = 0.0;  
324 <  
325 <  for (it = 0; it < myAtoms.size(); it++) {
371 >  bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
372 >    if (index < atoms_.size()) {
373  
374 <    mtmp = myAtoms[it]->getMass();
375 <    ptmp = refCoords[it];
376 <    r= norm3(ptmp.vec);
377 <    r2 = r*r;
378 <    
379 <    for (i = 0; i < 3; i++) {
380 <      for (j = 0; j < 3; j++) {
381 <        
382 <        if (i==j) Itmp[i][j] += mtmp * r2;
374 >      Vector3d ref = body2Lab(refCoords_[index]);
375 >      pos = getPos() + ref;
376 >      return true;
377 >    } else {
378 >      std::cerr << index << " is an invalid index, current rigid body contains "
379 >                << atoms_.size() << "atoms" << std::endl;
380 >      return false;
381 >    }    
382 >  }
383  
384 <        Itmp[i][j] -= mtmp * ptmp.vec[i]*ptmp.vec[j];
385 <      }
384 >  bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) {
385 >    std::vector<Atom*>::iterator i;
386 >    i = std::find(atoms_.begin(), atoms_.end(), atom);
387 >    if (i != atoms_.end()) {
388 >      //RigidBody class makes sure refCoords_ and atoms_ match each other
389 >      Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]);
390 >      pos = getPos() + ref;
391 >      return true;
392 >    } else {
393 >      std::cerr << "Atom " << atom->getGlobalIndex()
394 >                <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
395 >      return false;
396      }
397    }
398 <  
342 <  diagonalize3x3(Itmp, evals, sU);
343 <  
344 <  // zero out I and then fill the diagonals with the moments of inertia:
398 >  bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {
399  
400 <  n_linear_coords = 0;
400 >    //velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
401  
402 <  for (i = 0; i < 3; i++) {
349 <    for (j = 0; j < 3; j++) {
350 <      I[i][j] = 0.0;  
351 <    }
352 <    I[i][i] = evals[i];
402 >    if (index < atoms_.size()) {
403  
404 <    if (fabs(evals[i]) < momIntTol) {
405 <      is_linear = true;
406 <      n_linear_coords++;
407 <      linear_axis = i;
408 <    }
359 <  }
404 >      Vector3d velRot;
405 >      Mat3x3d skewMat;;
406 >      Vector3d ref = refCoords_[index];
407 >      Vector3d ji = getJ();
408 >      Mat3x3d I =  getI();
409  
410 <  if (n_linear_coords > 1) {
411 <          sprintf( painCave.errMsg,
412 <               "RigidBody error.\n"
364 <               "\tOOPSE found more than one axis in this rigid body with a vanishing \n"
365 <               "\tmoment of inertia.  This can happen in one of three ways:\n"
366 <               "\t 1) Only one atom was specified, or \n"
367 <               "\t 2) All atoms were specified at the same location, or\n"
368 <               "\t 3) The programmers did something stupid.\n"
369 <               "\tIt is silly to use a rigid body to describe this situation.  Be smarter.\n"
370 <               );
371 <      painCave.isFatal = 1;
372 <      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 <    }
382 <    len = sqrt(len);
383 <    for (j = 0; j < 3; j++) {
384 <      sU[i][j] /= len;
385 <    }
386 <  }
387 < }
410 >      skewMat(0, 0) =0;
411 >      skewMat(0, 1) = ji[2] /I(2, 2);
412 >      skewMat(0, 2) = -ji[1] /I(1, 1);
413  
414 < void RigidBody::doEulerToRotMat(vec3 &euler, mat3x3 &myA ){
414 >      skewMat(1, 0) = -ji[2] /I(2, 2);
415 >      skewMat(1, 1) = 0;
416 >      skewMat(1, 2) = ji[0]/I(0, 0);
417  
418 <  double phi, theta, psi;
419 <  
420 <  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);
418 >      skewMat(2, 0) =ji[1] /I(1, 1);
419 >      skewMat(2, 1) = -ji[0]/I(0, 0);
420 >      skewMat(2, 2) = 0;
421  
422 < }
422 >      velRot = (getA() * skewMat).transpose() * ref;
423  
424 < void RigidBody::calcForcesAndTorques() {
424 >      vel =getVel() + velRot;
425 >      return true;
426 >        
427 >    } else {
428 >      std::cerr << index << " is an invalid index, current rigid body contains "
429 >                << atoms_.size() << "atoms" << std::endl;
430 >      return false;
431 >    }
432 >  }
433  
434 <  // Convert Atomic forces and torques to total forces and torques:
414 <  int i, j;
415 <  double apos[3];
416 <  double afrc[3];
417 <  double atrq[3];
418 <  double rpos[3];
434 >  bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) {
435  
436 <  zeroForces();
437 <  
438 <  for (i = 0; i < myAtoms.size(); i++) {
436 >    std::vector<Atom*>::iterator i;
437 >    i = std::find(atoms_.begin(), atoms_.end(), atom);
438 >    if (i != atoms_.end()) {
439 >      return getAtomVel(vel, i - atoms_.begin());
440 >    } else {
441 >      std::cerr << "Atom " << atom->getGlobalIndex()
442 >                <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;    
443 >      return false;
444 >    }    
445 >  }
446  
447 <    myAtoms[i]->getPos(apos);
448 <    myAtoms[i]->getFrc(afrc);
447 >  bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) {
448 >    if (index < atoms_.size()) {
449  
450 <    for (j=0; j<3; j++) {
451 <      rpos[j] = apos[j] - pos[j];
452 <      frc[j] += afrc[j];
450 >      coor = refCoords_[index];
451 >      return true;
452 >    } else {
453 >      std::cerr << index << " is an invalid index, current rigid body contains "
454 >                << atoms_.size() << "atoms" << std::endl;
455 >      return false;
456      }
431 –    
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];
457  
458 <    // If the atom has a torque associated with it, then we also need to
437 <    // migrate the torques onto the center of mass:
458 >  }
459  
460 <    if (myAtoms[i]->isDirectional()) {
461 <
462 <      myAtoms[i]->getTrq(atrq);
463 <      
464 <      for (j=0; j<3; j++)
465 <        trq[j] += atrq[j];
460 >  bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) {
461 >    std::vector<Atom*>::iterator i;
462 >    i = std::find(atoms_.begin(), atoms_.end(), atom);
463 >    if (i != atoms_.end()) {
464 >      //RigidBody class makes sure refCoords_ and atoms_ match each other
465 >      coor = refCoords_[i - atoms_.begin()];
466 >      return true;
467 >    } else {
468 >      std::cerr << "Atom " << atom->getGlobalIndex()
469 >                <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;    
470 >      return false;
471      }
472 +
473    }
474  
448 –  // Convert Torque to Body-fixed coordinates:
449 –  // (Actually, on second thought, don't.  Integrator does this now.)
450 –  // lab2Body(trq);
475  
476 < }
476 >  void RigidBody::addAtom(Atom* at, AtomStamp* ats) {
477  
478 < void RigidBody::updateAtoms() {
479 <  int i, j;
456 <  vec3 ref;
457 <  double apos[3];
458 <  DirectionalAtom* dAtom;
478 >    Vector3d coords;
479 >    Vector3d euler;
480    
460 –  for (i = 0; i < myAtoms.size(); i++) {
461 –    
462 –    ref = refCoords[i];
481  
482 <    body2Lab(ref.vec);
483 <    
484 <    for (j = 0; j<3; j++)
485 <      apos[j] = pos[j] + ref.vec[j];
486 <    
487 <    myAtoms[i]->setPos(apos);
488 <    
489 <    if (myAtoms[i]->isDirectional()) {
490 <      
491 <      dAtom = (DirectionalAtom *) myAtoms[i];
474 <      dAtom->rotateBy( A );
475 <      
482 >    atoms_.push_back(at);
483 >
484 >    if( !ats->havePosition() ){
485 >      sprintf( painCave.errMsg,
486 >               "RigidBody error.\n"
487 >               "\tAtom %s does not have a position specified.\n"
488 >               "\tThis means RigidBody cannot set up reference coordinates.\n",
489 >               ats->getType() );
490 >      painCave.isFatal = 1;
491 >      simError();
492      }
477 –  }  
478 – }
479 –
480 – void RigidBody::getGrad( double grad[6] ) {
481 –
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];
493    
494 <  this->getEulerAngles(myEuler);
494 >    coords[0] = ats->getPosX();
495 >    coords[1] = ats->getPosY();
496 >    coords[2] = ats->getPosZ();
497  
498 <  phi = myEuler[0];
492 <  theta = myEuler[1];
493 <  psi = myEuler[2];
498 >    refCoords_.push_back(coords);
499  
500 <  cphi = cos(phi);
496 <  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;
500 >    RotMat3x3d identMat = RotMat3x3d::identity();
501    
502 <  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];
502 >    if (at->isDirectional()) {  
503  
504 <  grad[3] = 0.0;
505 <  grad[4] = 0.0;
506 <  grad[5] = 0.0;
507 <  
508 <  for (int j = 0; j < 3; j++ ) {
504 >      if( !ats->haveOrientation() ){
505 >        sprintf( painCave.errMsg,
506 >                 "RigidBody error.\n"
507 >                 "\tAtom %s does not have an orientation specified.\n"
508 >                 "\tThis means RigidBody cannot set up reference orientations.\n",
509 >                 ats->getType() );
510 >        painCave.isFatal = 1;
511 >        simError();
512 >      }    
513      
514 <    grad[3] += trq[j]*ephi[j];
515 <    grad[4] += trq[j]*etheta[j];
516 <    grad[5] += trq[j]*epsi[j];
526 <    
527 <  }
528 <  
529 < }
514 >      euler[0] = ats->getEulerPhi() * NumericConstant::PI /180.0;
515 >      euler[1] = ats->getEulerTheta() * NumericConstant::PI /180.0;
516 >      euler[2] = ats->getEulerPsi() * NumericConstant::PI /180.0;
517  
518 < /**
519 <  * 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]) {
540 <
541 <  // 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).
547 <  
548 <  
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;
559 <
560 <  theta = acos(min(1.0,max(-1.0,A[2][2])));
561 <  ctheta = A[2][2];
562 <  stheta = sqrt(1.0 - ctheta * ctheta);
563 <
564 <  // when sin(theta) is close to 0, we need to consider the
565 <  // 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++) {
518 >      RotMat3x3d Atmp(euler);
519 >      refOrients_.push_back(Atmp);
520      
521 <    mtmp = myAtoms[i]->getMass();    
522 <    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;
521 >    }else {
522 >      refOrients_.push_back(identMat);
523      }
632 –    
633 –  }
524    
525 <  for(j = 0; j < 3; j++) {
636 <    pos[j] /= mass;
637 <    vel[j] /= mass;
525 >  
526    }
527  
528   }
529  
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