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.
branches/development/src/primitives/RigidBody.cpp (file contents), Revision 1842 by gezelter, Tue Jan 29 19:10:04 2013 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. Redistributions of source code must retain the above copyright
10 + *    notice, this list of conditions and the following disclaimer.
11 + *
12 + * 2. Redistributions in binary form must reproduce the above copyright
13 + *    notice, this list of conditions and the following disclaimer in the
14 + *    documentation and/or other materials provided with the
15 + *    distribution.
16 + *
17 + * This software is provided "AS IS," without a warranty of any
18 + * kind. All express or implied conditions, representations and
19 + * warranties, including any implied warranty of merchantability,
20 + * fitness for a particular purpose or non-infringement, are hereby
21 + * excluded.  The University of Notre Dame and its licensors shall not
22 + * be liable for any damages suffered by licensee as a result of
23 + * using, modifying or distributing the software or its
24 + * derivatives. In no event will the University of Notre Dame or its
25 + * licensors be liable for any lost revenue, profit or data, or for
26 + * direct, indirect, special, consequential, incidental or punitive
27 + * damages, however caused and regardless of the theory of liability,
28 + * arising out of the use of or inability to use software, even if the
29 + * University of Notre Dame has been advised of the possibility of
30 + * such damages.
31 + *
32 + * SUPPORT OPEN SCIENCE!  If you use OpenMD or its source code in your
33 + * research, please cite the appropriate papers when you publish your
34 + * work.  Good starting points are:
35 + *                                                                      
36 + * [1]  Meineke, et al., J. Comp. Chem. 26, 252-271 (2005).            
37 + * [2]  Fennell & Gezelter, J. Chem. Phys. 124, 234104 (2006).          
38 + * [3]  Sun, Lin & Gezelter, J. Chem. Phys. 128, 24107 (2008).          
39 + * [4]  Kuang & Gezelter,  J. Chem. Phys. 133, 164101 (2010).
40 + * [5]  Vardeman, Stocker & Gezelter, J. Chem. Theory Comput. 7, 834 (2011).
41 + */
42 + #include <algorithm>
43   #include <math.h>
44 < #include "RigidBody.hpp"
45 < #include "DirectionalAtom.hpp"
46 < #include "simError.h"
47 < #include "MatVec3.h"
48 <
49 < RigidBody::RigidBody() : StuntDouble() {
50 <  objType = OT_RIGIDBODY;
9 <  is_linear = false;
10 <  linear_axis =  -1;
11 <  momIntTol = 1e-6;
12 < }
13 <
14 < RigidBody::~RigidBody() {
15 < }
16 <
17 < void RigidBody::addAtom(Atom* at, AtomStamp* ats) {
18 <
19 <  vec3 coords;
20 <  vec3 euler;
21 <  mat3x3 Atmp;
22 <
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();
44 > #include "primitives/RigidBody.hpp"
45 > #include "utils/simError.h"
46 > #include "utils/NumericConstant.hpp"
47 > namespace OpenMD {
48 >  
49 >  RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData),
50 >                           inertiaTensor_(0.0){    
51    }
52    
53 <  coords[0] = ats->getPosX();
54 <  coords[1] = ats->getPosY();
55 <  coords[2] = ats->getPosZ();
56 <
57 <  refCoords.push_back(coords);
53 >  void RigidBody::setPrevA(const RotMat3x3d& a) {
54 >    ((snapshotMan_->getPrevSnapshot())->*storage_).aMat[localIndex_] = a;
55 >    
56 >    for (unsigned int i = 0 ; i < atoms_.size(); ++i){
57 >      if (atoms_[i]->isDirectional()) {
58 >        atoms_[i]->setPrevA(refOrients_[i].transpose() * a);
59 >      }
60 >    }
61 >    
62 >  }
63    
64 <  if (at->isDirectional()) {  
64 >  
65 >  void RigidBody::setA(const RotMat3x3d& a) {
66 >    ((snapshotMan_->getCurrentSnapshot())->*storage_).aMat[localIndex_] = a;
67  
68 <    if( !ats->haveOrientation() ){
69 <      sprintf( painCave.errMsg,
70 <               "RigidBody error.\n"
71 <               "\tAtom %s does not have an orientation specified.\n"
72 <               "\tThis means RigidBody cannot set up reference orientations.\n",
73 <               ats->getType() );
74 <      painCave.isFatal = 1;
75 <      simError();
76 <    }    
68 >    for (unsigned int i = 0 ; i < atoms_.size(); ++i){
69 >      if (atoms_[i]->isDirectional()) {
70 >        atoms_[i]->setA(refOrients_[i].transpose() * a);
71 >      }
72 >    }
73 >  }    
74 >  
75 >  void RigidBody::setA(const RotMat3x3d& a, int snapshotNo) {
76 >    ((snapshotMan_->getSnapshot(snapshotNo))->*storage_).aMat[localIndex_] = a;
77 >        
78 >    for (unsigned int i = 0 ; i < atoms_.size(); ++i){
79 >      if (atoms_[i]->isDirectional()) {
80 >        atoms_[i]->setA(refOrients_[i].transpose() * a, snapshotNo);
81 >      }
82 >    }
83      
84 <    euler[0] = ats->getEulerPhi();
85 <    euler[1] = ats->getEulerTheta();
86 <    euler[2] = ats->getEulerPsi();
84 >  }  
85 >  
86 >  Mat3x3d RigidBody::getI() {
87 >    return inertiaTensor_;
88 >  }    
89 >  
90 >  std::vector<RealType> RigidBody::getGrad() {
91 >    std::vector<RealType> grad(6, 0.0);
92 >    Vector3d force;
93 >    Vector3d torque;
94 >    Vector3d myEuler;
95 >    RealType phi, theta;
96 >    // RealType psi;
97 >    RealType cphi, sphi, ctheta, stheta;
98 >    Vector3d ephi;
99 >    Vector3d etheta;
100 >    Vector3d epsi;
101      
102 <    doEulerToRotMat(euler, Atmp);
102 >    force = getFrc();
103 >    torque =getTrq();
104 >    myEuler = getA().toEulerAngles();
105      
106 <    refOrients.push_back(Atmp);
106 >    phi = myEuler[0];
107 >    theta = myEuler[1];
108 >    // psi = myEuler[2];
109      
110 <  }
111 < }
112 <
113 < void RigidBody::getPos(double theP[3]){
65 <  for (int i = 0; i < 3 ; i++)
66 <    theP[i] = pos[i];
67 < }      
68 <
69 < void RigidBody::setPos(double theP[3]){
70 <  for (int i = 0; i < 3 ; i++)
71 <    pos[i] = theP[i];
72 < }      
73 <
74 < void RigidBody::getVel(double theV[3]){
75 <  for (int i = 0; i < 3 ; i++)
76 <    theV[i] = vel[i];
77 < }      
78 <
79 < void RigidBody::setVel(double theV[3]){
80 <  for (int i = 0; i < 3 ; i++)
81 <    vel[i] = theV[i];
82 < }      
83 <
84 < void RigidBody::getFrc(double theF[3]){
85 <  for (int i = 0; i < 3 ; i++)
86 <    theF[i] = frc[i];
87 < }      
88 <
89 < void RigidBody::addFrc(double theF[3]){
90 <  for (int i = 0; i < 3 ; i++)
91 <    frc[i] += theF[i];
92 < }    
93 <
94 < void RigidBody::zeroForces() {
95 <
96 <  for (int i = 0; i < 3; i++) {
97 <    frc[i] = 0.0;
98 <    trq[i] = 0.0;
99 <  }
100 <
101 < }
102 <
103 < void RigidBody::setEuler( double phi, double theta, double psi ){
104 <  
105 <    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);
110 >    cphi = cos(phi);
111 >    sphi = sin(phi);
112 >    ctheta = cos(theta);
113 >    stheta = sin(theta);
114      
115 <    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);
115 >    // get unit vectors along the phi, theta and psi rotation axes
116      
117 <    A[2][0] = sin(phi) * sin(theta);
118 <    A[2][1] = -cos(phi) * sin(theta);
119 <    A[2][2] = cos(theta);
116 <
117 < }
118 <
119 < void RigidBody::getQ( double q[4] ){
120 <  
121 <  double t, s;
122 <  double ad1, ad2, ad3;
117 >    ephi[0] = 0.0;
118 >    ephi[1] = 0.0;
119 >    ephi[2] = 1.0;
120      
121 <  t = A[0][0] + A[1][1] + A[2][2] + 1.0;
122 <  if( t > 0.0 ){
121 >    //etheta[0] = -sphi;
122 >    //etheta[1] =  cphi;
123 >    //etheta[2] =  0.0;
124      
125 <    s = 0.5 / sqrt( t );
126 <    q[0] = 0.25 / s;
127 <    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{
125 >    etheta[0] = cphi;
126 >    etheta[1] = sphi;
127 >    etheta[2] =  0.0;
128      
129 <    ad1 = fabs( A[0][0] );
130 <    ad2 = fabs( A[1][1] );
131 <    ad3 = fabs( A[2][2] );
129 >    epsi[0] = stheta * cphi;
130 >    epsi[1] = stheta * sphi;
131 >    epsi[2] = ctheta;
132      
133 <    if( ad1 >= ad2 && ad1 >= ad3 ){
133 >    //gradient is equal to -force
134 >    for (int j = 0 ; j<3; j++)
135 >      grad[j] = -force[j];
136 >    
137 >    for (int j = 0; j < 3; j++ ) {
138        
139 <      s = 2.0 * sqrt( 1.0 + A[0][0] - A[1][1] - A[2][2] );
140 <      q[0] = (A[1][2] + A[2][1]) / s;
141 <      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 ){
139 >      grad[3] += torque[j]*ephi[j];
140 >      grad[4] += torque[j]*etheta[j];
141 >      grad[5] += torque[j]*epsi[j];
142        
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;
143      }
144 <    else{
144 >    
145 >    return grad;
146 >  }    
147 >  
148 >  void RigidBody::accept(BaseVisitor* v) {
149 >    v->visit(this);
150 >  }    
151 >
152 >  /**@todo need modification */
153 >  void  RigidBody::calcRefCoords() {
154 >    RealType mtmp;
155 >    Vector3d refCOM(0.0);
156 >    mass_ = 0.0;
157 >    for (std::size_t i = 0; i < atoms_.size(); ++i) {
158 >      mtmp = atoms_[i]->getMass();
159 >      mass_ += mtmp;
160 >      refCOM += refCoords_[i]*mtmp;
161 >    }
162 >    refCOM /= mass_;
163 >    
164 >    // Next, move the origin of the reference coordinate system to the COM:
165 >    for (std::size_t i = 0; i < atoms_.size(); ++i) {
166 >      refCoords_[i] -= refCOM;
167 >    }
168 >
169 >    // Moment of Inertia calculation
170 >    Mat3x3d Itmp(0.0);    
171 >    for (std::size_t i = 0; i < atoms_.size(); i++) {
172 >      Mat3x3d IAtom(0.0);  
173 >      mtmp = atoms_[i]->getMass();
174 >      IAtom -= outProduct(refCoords_[i], refCoords_[i]) * mtmp;
175 >      RealType r2 = refCoords_[i].lengthSquare();
176 >      IAtom(0, 0) += mtmp * r2;
177 >      IAtom(1, 1) += mtmp * r2;
178 >      IAtom(2, 2) += mtmp * r2;
179 >      Itmp += IAtom;
180        
181 <      s = sqrt( 1.0 + A[2][2] - A[0][0] - A[1][1] ) * 2.0;
182 <      q[0] = (A[0][1] + A[1][0]) / s;
183 <      q[1] = (A[0][2] + A[2][0]) / s;
184 <      q[2] = (A[1][2] + A[2][1]) / s;
161 <      q[3] = 0.5 / s;
181 >      //project the inertial moment of directional atoms into this rigid body
182 >      if (atoms_[i]->isDirectional()) {
183 >        Itmp += refOrients_[i].transpose() * atoms_[i]->getI() * refOrients_[i];
184 >      }
185      }
163  }
164 }
186  
187 < void RigidBody::setQ( double the_q[4] ){
187 >    //    std::cout << Itmp << std::endl;
188  
189 <  double q0Sqr, q1Sqr, q2Sqr, q3Sqr;
190 <  
191 <  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;  
189 >    //diagonalize
190 >    Vector3d evals;
191 >    Mat3x3d::diagonalize(Itmp, evals, sU_);
192  
193 < }
193 >    // zero out I and then fill the diagonals with the moments of inertia:
194 >    inertiaTensor_(0, 0) = evals[0];
195 >    inertiaTensor_(1, 1) = evals[1];
196 >    inertiaTensor_(2, 2) = evals[2];
197 >        
198 >    int nLinearAxis = 0;
199 >    for (int i = 0; i < 3; i++) {    
200 >      if (fabs(evals[i]) < OpenMD::epsilon) {
201 >        linear_ = true;
202 >        linearAxis_ = i;
203 >        ++ nLinearAxis;
204 >      }
205 >    }
206  
207 < void RigidBody::getA( double the_A[3][3] ){
207 >    if (nLinearAxis > 1) {
208 >      sprintf( painCave.errMsg,
209 >               "RigidBody error.\n"
210 >               "\tOpenMD found more than one axis in this rigid body with a vanishing \n"
211 >               "\tmoment of inertia.  This can happen in one of three ways:\n"
212 >               "\t 1) Only one atom was specified, or \n"
213 >               "\t 2) All atoms were specified at the same location, or\n"
214 >               "\t 3) The programmers did something stupid.\n"
215 >               "\tIt is silly to use a rigid body to describe this situation.  Be smarter.\n"
216 >               );
217 >      painCave.isFatal = 1;
218 >      simError();
219 >    }
220    
221 <  for (int i = 0; i < 3; i++)
192 <    for (int j = 0; j < 3; j++)
193 <      the_A[i][j] = A[i][j];
221 >  }
222  
223 < }
223 >  void  RigidBody::calcForcesAndTorques() {
224 >    Vector3d afrc;
225 >    Vector3d atrq;
226 >    Vector3d apos;
227 >    Vector3d rpos;
228 >    Vector3d frc(0.0);
229 >    Vector3d trq(0.0);
230 >    Vector3d ef(0.0);
231 >    Vector3d pos = this->getPos();
232  
233 < void RigidBody::setA( double the_A[3][3] ){
233 >    int sl = ((snapshotMan_->getCurrentSnapshot())->*storage_).getStorageLayout();
234  
235 <  for (int i = 0; i < 3; i++)
200 <    for (int j = 0; j < 3; j++)
201 <      A[i][j] = the_A[i][j];
202 <  
203 < }
235 >    for (unsigned int i = 0; i < atoms_.size(); i++) {
236  
237 < void RigidBody::getJ( double theJ[3] ){
238 <  
239 <  for (int i = 0; i < 3; i++)
240 <    theJ[i] = ji[i];
237 >      afrc = atoms_[i]->getFrc();
238 >      apos = atoms_[i]->getPos();
239 >      rpos = apos - pos;
240 >        
241 >      frc += afrc;
242  
243 < }
243 >      trq[0] += rpos[1]*afrc[2] - rpos[2]*afrc[1];
244 >      trq[1] += rpos[2]*afrc[0] - rpos[0]*afrc[2];
245 >      trq[2] += rpos[0]*afrc[1] - rpos[1]*afrc[0];
246  
247 < void RigidBody::setJ( double theJ[3] ){
248 <  
214 <  for (int i = 0; i < 3; i++)
215 <    ji[i] = theJ[i];
247 >      // If the atom has a torque associated with it, then we also need to
248 >      // migrate the torques onto the center of mass:
249  
250 < }
250 >      if (atoms_[i]->isDirectional()) {
251 >        atrq = atoms_[i]->getTrq();
252 >        trq += atrq;
253 >      }
254  
255 < void RigidBody::getTrq(double theT[3]){
256 <  for (int i = 0; i < 3 ; i++)
257 <    theT[i] = trq[i];
258 < }      
255 >      if (sl & DataStorage::dslElectricField) {
256 >        ef += atoms_[i]->getElectricField();
257 >      }
258 >    }        
259 >    addFrc(frc);
260 >    addTrq(trq);    
261  
262 < void RigidBody::addTrq(double theT[3]){
263 <  for (int i = 0; i < 3 ; i++)
264 <    trq[i] += theT[i];
265 < }      
262 >    if (sl & DataStorage::dslElectricField) {
263 >      ef /= atoms_.size();
264 >      setElectricField(ef);
265 >    }
266  
267 < void RigidBody::getI( double the_I[3][3] ){  
267 >  }
268  
269 <    for (int i = 0; i < 3; i++)
270 <      for (int j = 0; j < 3; j++)
271 <        the_I[i][j] = I[i][j];
269 >  Mat3x3d RigidBody::calcForcesAndTorquesAndVirial() {
270 >    Vector3d afrc;
271 >    Vector3d atrq;
272 >    Vector3d apos;
273 >    Vector3d rpos;
274 >    Vector3d dfrc;
275 >    Vector3d frc(0.0);
276 >    Vector3d trq(0.0);
277 >    Vector3d ef(0.0);
278 >    
279 >    Vector3d pos = this->getPos();
280 >    Mat3x3d tau_(0.0);
281  
282 < }
282 >    int sl = ((snapshotMan_->getCurrentSnapshot())->*storage_).getStorageLayout();
283  
284 < void RigidBody::lab2Body( double r[3] ){
284 >    for (unsigned int i = 0; i < atoms_.size(); i++) {
285 >      
286 >      afrc = atoms_[i]->getFrc();
287 >      apos = atoms_[i]->getPos();
288 >      rpos = apos - pos;
289 >        
290 >      frc += afrc;
291  
292 <  double rl[3]; // the lab frame vector
292 >      trq[0] += rpos[1]*afrc[2] - rpos[2]*afrc[1];
293 >      trq[1] += rpos[2]*afrc[0] - rpos[0]*afrc[2];
294 >      trq[2] += rpos[0]*afrc[1] - rpos[1]*afrc[0];
295 >
296 >      // If the atom has a torque associated with it, then we also need to
297 >      // migrate the torques onto the center of mass:
298 >
299 >      if (atoms_[i]->isDirectional()) {
300 >        atrq = atoms_[i]->getTrq();
301 >        trq += atrq;
302 >      }
303 >      if (sl & DataStorage::dslElectricField) {
304 >        ef += atoms_[i]->getElectricField();
305 >      }
306 >      
307 >      tau_(0,0) -= rpos[0]*afrc[0];
308 >      tau_(0,1) -= rpos[0]*afrc[1];
309 >      tau_(0,2) -= rpos[0]*afrc[2];
310 >      tau_(1,0) -= rpos[1]*afrc[0];
311 >      tau_(1,1) -= rpos[1]*afrc[1];
312 >      tau_(1,2) -= rpos[1]*afrc[2];
313 >      tau_(2,0) -= rpos[2]*afrc[0];
314 >      tau_(2,1) -= rpos[2]*afrc[1];
315 >      tau_(2,2) -= rpos[2]*afrc[2];
316 >
317 >    }
318 >    addFrc(frc);
319 >    addTrq(trq);
320 >
321 >    if (sl & DataStorage::dslElectricField) {
322 >      ef /= atoms_.size();
323 >      setElectricField(ef);
324 >    }
325 >
326 >    return tau_;
327 >  }
328 >
329 >  void  RigidBody::updateAtoms() {
330 >    unsigned int i;
331 >    Vector3d ref;
332 >    Vector3d apos;
333 >    DirectionalAtom* dAtom;
334 >    Vector3d pos = getPos();
335 >    RotMat3x3d a = getA();
336 >    
337 >    for (i = 0; i < atoms_.size(); i++) {
338 >    
339 >      ref = body2Lab(refCoords_[i]);
340 >
341 >      apos = pos + ref;
342 >
343 >      atoms_[i]->setPos(apos);
344 >
345 >      if (atoms_[i]->isDirectional()) {
346 >          
347 >        dAtom = (DirectionalAtom *) atoms_[i];
348 >        dAtom->setA(refOrients_[i].transpose() * a);
349 >      }
350 >
351 >    }
352    
353 <  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]);
353 >  }
354  
249 }
355  
356 < void RigidBody::body2Lab( double r[3] ){
356 >  void  RigidBody::updateAtoms(int frame) {
357 >    unsigned int i;
358 >    Vector3d ref;
359 >    Vector3d apos;
360 >    DirectionalAtom* dAtom;
361 >    Vector3d pos = getPos(frame);
362 >    RotMat3x3d a = getA(frame);
363 >    
364 >    for (i = 0; i < atoms_.size(); i++) {
365 >    
366 >      ref = body2Lab(refCoords_[i], frame);
367  
368 <  double rb[3]; // the body frame vector
368 >      apos = pos + ref;
369 >
370 >      atoms_[i]->setPos(apos, frame);
371 >
372 >      if (atoms_[i]->isDirectional()) {
373 >          
374 >        dAtom = (DirectionalAtom *) atoms_[i];
375 >        dAtom->setA(refOrients_[i].transpose() * a, frame);
376 >      }
377 >
378 >    }
379    
380 <  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]);
380 >  }
381  
382 < }
382 >  void RigidBody::updateAtomVel() {
383 >    Mat3x3d skewMat;;
384  
385 < double RigidBody::getZangle( ){
386 <    return zAngle;
267 < }
385 >    Vector3d ji = getJ();
386 >    Mat3x3d I =  getI();
387  
388 < void RigidBody::setZangle( double zAng ){
389 <    zAngle = zAng;
390 < }
388 >    skewMat(0, 0) =0;
389 >    skewMat(0, 1) = ji[2] /I(2, 2);
390 >    skewMat(0, 2) = -ji[1] /I(1, 1);
391 >
392 >    skewMat(1, 0) = -ji[2] /I(2, 2);
393 >    skewMat(1, 1) = 0;
394 >    skewMat(1, 2) = ji[0]/I(0, 0);
395 >
396 >    skewMat(2, 0) =ji[1] /I(1, 1);
397 >    skewMat(2, 1) = -ji[0]/I(0, 0);
398 >    skewMat(2, 2) = 0;
399 >
400 >    Mat3x3d mat = (getA() * skewMat).transpose();
401 >    Vector3d rbVel = getVel();
402 >
403 >
404 >    Vector3d velRot;        
405 >    for (unsigned int i = 0 ; i < refCoords_.size(); ++i) {
406 >      atoms_[i]->setVel(rbVel + mat * refCoords_[i]);
407 >    }
408 >
409 >  }
410 >
411 >  void RigidBody::updateAtomVel(int frame) {
412 >    Mat3x3d skewMat;;
413 >
414 >    Vector3d ji = getJ(frame);
415 >    Mat3x3d I =  getI();
416 >
417 >    skewMat(0, 0) =0;
418 >    skewMat(0, 1) = ji[2] /I(2, 2);
419 >    skewMat(0, 2) = -ji[1] /I(1, 1);
420 >
421 >    skewMat(1, 0) = -ji[2] /I(2, 2);
422 >    skewMat(1, 1) = 0;
423 >    skewMat(1, 2) = ji[0]/I(0, 0);
424 >
425 >    skewMat(2, 0) =ji[1] /I(1, 1);
426 >    skewMat(2, 1) = -ji[0]/I(0, 0);
427 >    skewMat(2, 2) = 0;
428  
429 < void RigidBody::addZangle( double zAng ){
430 <    zAngle += zAng;
275 < }
429 >    Mat3x3d mat = (getA(frame) * skewMat).transpose();
430 >    Vector3d rbVel = getVel(frame);
431  
277 void RigidBody::calcRefCoords( ) {
432  
433 <  int i,j,k, it, n_linear_coords;
434 <  double mtmp;
435 <  vec3 apos;
436 <  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;
433 >    Vector3d velRot;        
434 >    for (unsigned int i = 0 ; i < refCoords_.size(); ++i) {
435 >      atoms_[i]->setVel(rbVel + mat * refCoords_[i], frame);
436 >    }
437  
438 <  // 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;
438 >  }
439  
440 <    apos = refCoords[i];
441 <    
442 <    for(j = 0; j < 3; j++) {
443 <      refCOM[j] += apos[j]*mtmp;    
440 >        
441 >
442 >  bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
443 >    if (index < atoms_.size()) {
444 >
445 >      Vector3d ref = body2Lab(refCoords_[index]);
446 >      pos = getPos() + ref;
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      }    
453    }
305  
306  for(j = 0; j < 3; j++)
307    refCOM[j] /= mass;
454  
455 < // Next, move the origin of the reference coordinate system to the COM:
456 <
457 <  for (i = 0; i < myAtoms.size(); i++) {
458 <    apos = refCoords[i];
459 <    for (j=0; j < 3; j++) {
460 <      apos[j] = apos[j] - refCOM[j];
455 >  bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) {
456 >    std::vector<Atom*>::iterator i;
457 >    i = std::find(atoms_.begin(), atoms_.end(), atom);
458 >    if (i != atoms_.end()) {
459 >      //RigidBody class makes sure refCoords_ and atoms_ match each other
460 >      Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]);
461 >      pos = getPos() + ref;
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      }
316    refCoords[i] = apos;
468    }
469 +  bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {
470  
471 < // Moment of Inertia calculation
471 >    //velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
472  
473 <  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++) {
473 >    if (index < atoms_.size()) {
474  
475 <    mtmp = myAtoms[it]->getMass();
476 <    ptmp = refCoords[it];
477 <    r= norm3(ptmp.vec);
478 <    r2 = r*r;
479 <    
332 <    for (i = 0; i < 3; i++) {
333 <      for (j = 0; j < 3; j++) {
334 <        
335 <        if (i==j) Itmp[i][j] += mtmp * r2;
475 >      Vector3d velRot;
476 >      Mat3x3d skewMat;;
477 >      Vector3d ref = refCoords_[index];
478 >      Vector3d ji = getJ();
479 >      Mat3x3d I =  getI();
480  
481 <        Itmp[i][j] -= mtmp * ptmp.vec[i]*ptmp.vec[j];
482 <      }
483 <    }
340 <  }
341 <  
342 <  diagonalize3x3(Itmp, evals, sU);
343 <  
344 <  // zero out I and then fill the diagonals with the moments of inertia:
481 >      skewMat(0, 0) =0;
482 >      skewMat(0, 1) = ji[2] /I(2, 2);
483 >      skewMat(0, 2) = -ji[1] /I(1, 1);
484  
485 <  n_linear_coords = 0;
485 >      skewMat(1, 0) = -ji[2] /I(2, 2);
486 >      skewMat(1, 1) = 0;
487 >      skewMat(1, 2) = ji[0]/I(0, 0);
488  
489 <  for (i = 0; i < 3; i++) {
490 <    for (j = 0; j < 3; j++) {
491 <      I[i][j] = 0.0;  
351 <    }
352 <    I[i][i] = evals[i];
489 >      skewMat(2, 0) =ji[1] /I(1, 1);
490 >      skewMat(2, 1) = -ji[0]/I(0, 0);
491 >      skewMat(2, 2) = 0;
492  
493 <    if (fabs(evals[i]) < momIntTol) {
355 <      is_linear = true;
356 <      n_linear_coords++;
357 <      linear_axis = i;
358 <    }
359 <  }
493 >      velRot = (getA() * skewMat).transpose() * ref;
494  
495 <  if (n_linear_coords > 1) {
496 <          sprintf( painCave.errMsg,
497 <               "RigidBody error.\n"
498 <               "\tOOPSE found more than one axis in this rigid body with a vanishing \n"
499 <               "\tmoment of inertia.  This can happen in one of three ways:\n"
500 <               "\t 1) Only one atom was specified, or \n"
501 <               "\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];
495 >      vel =getVel() + velRot;
496 >      return true;
497 >        
498 >    } else {
499 >      std::cerr << index << " is an invalid index, current rigid body contains "
500 >                << atoms_.size() << "atoms" << std::endl;
501 >      return false;
502      }
382    len = sqrt(len);
383    for (j = 0; j < 3; j++) {
384      sU[i][j] /= len;
385    }
503    }
387 }
504  
505 < void RigidBody::doEulerToRotMat(vec3 &euler, mat3x3 &myA ){
505 >  bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) {
506  
507 <  double phi, theta, psi;
508 <  
509 <  phi = euler[0];
510 <  theta = euler[1];
511 <  psi = euler[2];
512 <  
513 <  myA[0][0] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi));
514 <  myA[0][1] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi));
515 <  myA[0][2] = sin(theta) * sin(psi);
516 <  
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);
507 >    std::vector<Atom*>::iterator i;
508 >    i = std::find(atoms_.begin(), atoms_.end(), atom);
509 >    if (i != atoms_.end()) {
510 >      return getAtomVel(vel, i - atoms_.begin());
511 >    } else {
512 >      std::cerr << "Atom " << atom->getGlobalIndex()
513 >                <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;    
514 >      return false;
515 >    }    
516 >  }
517  
518 < }
518 >  bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) {
519 >    if (index < atoms_.size()) {
520  
521 < void RigidBody::calcForcesAndTorques() {
522 <
523 <  // Convert Atomic forces and torques to total forces and torques:
524 <  int i, j;
525 <  double apos[3];
526 <  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];
521 >      coor = refCoords_[index];
522 >      return true;
523 >    } else {
524 >      std::cerr << index << " is an invalid index, current rigid body contains "
525 >                << atoms_.size() << "atoms" << std::endl;
526 >      return false;
527      }
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];
528  
529 <    // If the atom has a torque associated with it, then we also need to
437 <    // migrate the torques onto the center of mass:
529 >  }
530  
531 <    if (myAtoms[i]->isDirectional()) {
532 <
533 <      myAtoms[i]->getTrq(atrq);
534 <      
535 <      for (j=0; j<3; j++)
536 <        trq[j] += atrq[j];
531 >  bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) {
532 >    std::vector<Atom*>::iterator i;
533 >    i = std::find(atoms_.begin(), atoms_.end(), atom);
534 >    if (i != atoms_.end()) {
535 >      //RigidBody class makes sure refCoords_ and atoms_ match each other
536 >      coor = refCoords_[i - atoms_.begin()];
537 >      return true;
538 >    } else {
539 >      std::cerr << "Atom " << atom->getGlobalIndex()
540 >                <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;    
541 >      return false;
542      }
543 +
544    }
545  
448  // Convert Torque to Body-fixed coordinates:
449  // (Actually, on second thought, don't.  Integrator does this now.)
450  // lab2Body(trq);
546  
547 < }
547 >  void RigidBody::addAtom(Atom* at, AtomStamp* ats) {
548  
549 < void RigidBody::updateAtoms() {
550 <  int i, j;
456 <  vec3 ref;
457 <  double apos[3];
458 <  DirectionalAtom* dAtom;
549 >    Vector3d coords;
550 >    Vector3d euler;
551    
460  for (i = 0; i < myAtoms.size(); i++) {
461    
462    ref = refCoords[i];
552  
553 <    body2Lab(ref.vec);
554 <    
555 <    for (j = 0; j<3; j++)
556 <      apos[j] = pos[j] + ref.vec[j];
557 <    
558 <    myAtoms[i]->setPos(apos);
559 <    
560 <    if (myAtoms[i]->isDirectional()) {
561 <      
562 <      dAtom = (DirectionalAtom *) myAtoms[i];
474 <      dAtom->rotateBy( A );
475 <      
553 >    atoms_.push_back(at);
554 >
555 >    if( !ats->havePosition() ){
556 >      sprintf( painCave.errMsg,
557 >               "RigidBody error.\n"
558 >               "\tAtom %s does not have a position specified.\n"
559 >               "\tThis means RigidBody cannot set up reference coordinates.\n",
560 >               ats->getType().c_str() );
561 >      painCave.isFatal = 1;
562 >      simError();
563      }
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];
564    
565 <  this->getEulerAngles(myEuler);
565 >    coords[0] = ats->getPosX();
566 >    coords[1] = ats->getPosY();
567 >    coords[2] = ats->getPosZ();
568  
569 <  phi = myEuler[0];
492 <  theta = myEuler[1];
493 <  psi = myEuler[2];
569 >    refCoords_.push_back(coords);
570  
571 <  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;
571 >    RotMat3x3d identMat = RotMat3x3d::identity();
572    
573 <  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];
573 >    if (at->isDirectional()) {  
574  
575 <  grad[3] = 0.0;
576 <  grad[4] = 0.0;
577 <  grad[5] = 0.0;
578 <  
579 <  for (int j = 0; j < 3; j++ ) {
575 >      if( !ats->haveOrientation() ){
576 >        sprintf( painCave.errMsg,
577 >                 "RigidBody error.\n"
578 >                 "\tAtom %s does not have an orientation specified.\n"
579 >                 "\tThis means RigidBody cannot set up reference orientations.\n",
580 >                 ats->getType().c_str() );
581 >        painCave.isFatal = 1;
582 >        simError();
583 >      }    
584      
585 <    grad[3] += trq[j]*ephi[j];
586 <    grad[4] += trq[j]*etheta[j];
587 <    grad[5] += trq[j]*epsi[j];
526 <    
527 <  }
528 <  
529 < }
585 >      euler[0] = ats->getEulerPhi() * NumericConstant::PI /180.0;
586 >      euler[1] = ats->getEulerTheta() * NumericConstant::PI /180.0;
587 >      euler[2] = ats->getEulerPsi() * NumericConstant::PI /180.0;
588  
589 < /**
590 <  * 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++) {
589 >      RotMat3x3d Atmp(euler);
590 >      refOrients_.push_back(Atmp);
591      
592 <    mtmp = myAtoms[i]->getMass();    
593 <    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;
592 >    }else {
593 >      refOrients_.push_back(identMat);
594      }
632    
633  }
595    
596 <  for(j = 0; j < 3; j++) {
636 <    pos[j] /= mass;
637 <    vel[j] /= mass;
596 >  
597    }
598  
599   }
600  
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

Comparing:
trunk/src/primitives/RigidBody.cpp (property svn:keywords), Revision 2 by gezelter, Fri Sep 24 04:16:43 2004 UTC vs.
branches/development/src/primitives/RigidBody.cpp (property svn:keywords), Revision 1842 by gezelter, Tue Jan 29 19:10:04 2013 UTC

# Line 0 | Line 1
1 + Author Id Revision Date

Diff Legend

Removed lines
+ Added lines
< Changed lines
> Changed lines