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