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

Comparing:
trunk/OOPSE-3.0/src/primitives/RigidBody.cpp (file contents), Revision 1492 by tim, Fri Sep 24 16:27:58 2004 UTC vs.
branches/new_design/OOPSE-3.0/src/primitives/RigidBody.cpp (file contents), Revision 1738 by tim, Sat Nov 13 05:08:12 2004 UTC

# Line 1 | Line 1
1 < #include <math.h>
1 > /*
2 > * Copyright (C) 2000-2004  Object Oriented Parallel Simulation Engine (OOPSE) project
3 > *
4 > * Contact: oopse@oopse.org
5 > *
6 > * This program is free software; you can redistribute it and/or
7 > * modify it under the terms of the GNU Lesser General Public License
8 > * as published by the Free Software Foundation; either version 2.1
9 > * of the License, or (at your option) any later version.
10 > * All we ask is that proper credit is given for our work, which includes
11 > * - but is not limited to - adding the above copyright notice to the beginning
12 > * of your source code files, and to any copyright notice that you may distribute
13 > * with programs based on this work.
14 > *
15 > * This program is distributed in the hope that it will be useful,
16 > * but WITHOUT ANY WARRANTY; without even the implied warranty of
17 > * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
18 > * GNU Lesser General Public License for more details.
19 > *
20 > * You should have received a copy of the GNU Lesser General Public License
21 > * along with this program; if not, write to the Free Software
22 > * Foundation, Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
23 > *
24 > */
25 >
26   #include "primitives/RigidBody.hpp"
3 – #include "primitives/DirectionalAtom.hpp"
4 – #include "utils/simError.h"
5 – #include "math/MatVec3.h"
27  
28 < RigidBody::RigidBody() : StuntDouble() {
8 <  objType = OT_RIGIDBODY;
9 <  is_linear = false;
10 <  linear_axis =  -1;
11 <  momIntTol = 1e-6;
12 < }
28 > namespace oopse {
29  
30 < RigidBody::~RigidBody() {
30 > RigidBody::RigidBody() : StuntDouble(otRigidBody, &Snapshot::rigidbodyData){
31 >
32   }
33  
34 < void RigidBody::addAtom(Atom* at, AtomStamp* ats) {
34 > void RigidBody::setPrevA(const RotMat3x3d& a) {
35 >    ((snapshotMan_->getPrevSnapshot())->*storage_).aMat[localIndex_] = a;
36 >    ((snapshotMan_->getPrevSnapshot())->*storage_).unitVector[localIndex_] = a.inverse() * sU_.getColum(2);
37  
38 <  vec3 coords;
39 <  vec3 euler;
40 <  mat3x3 Atmp;
38 >    std::vector<Atom*>::iterator i;
39 >    for (i = atoms_.begin(); i != atoms_.end(); ++i) {
40 >        if ((*i)->isDirectional()) {
41 >            (*i)->setPrevA(a * (*i)->getPrevA());
42 >        }
43 >    }
44  
45 <  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();
33 <  }
34 <  
35 <  coords[0] = ats->getPosX();
36 <  coords[1] = ats->getPosY();
37 <  coords[2] = ats->getPosZ();
45 > }
46  
47 <  refCoords.push_back(coords);
48 <  
49 <  if (at->isDirectional()) {  
47 >      
48 > void RigidBody::setA(const RotMat3x3d& a) {
49 >    ((snapshotMan_->getCurrentSnapshot())->*storage_).aMat[localIndex_] = a;
50 >    ((snapshotMan_->getCurrentSnapshot())->*storage_).unitVector[localIndex_] = a.inverse() * sU_.getColum(2);
51  
52 <    if( !ats->haveOrientation() ){
53 <      sprintf( painCave.errMsg,
54 <               "RigidBody error.\n"
55 <               "\tAtom %s does not have an orientation specified.\n"
56 <               "\tThis means RigidBody cannot set up reference orientations.\n",
57 <               ats->getType() );
58 <      painCave.isFatal = 1;
50 <      simError();
51 <    }    
52 >    std::vector<Atom*>::iterator i;
53 >    for (i = atoms_.begin(); i != atoms_.end(); ++i) {
54 >        if ((*i)->isDirectional()) {
55 >            (*i)->setA(a * (*i)->getA());
56 >        }
57 >    }
58 > }    
59      
60 <    euler[0] = ats->getEulerPhi();
61 <    euler[1] = ats->getEulerTheta();
62 <    euler[2] = ats->getEulerPsi();
63 <    
64 <    doEulerToRotMat(euler, Atmp);
65 <    
66 <    refOrients.push_back(Atmp);
67 <    
68 <  }
60 > void RigidBody::setA(const RotMat3x3d& a, int snapshotNo) {
61 >    ((snapshotMan_->getSnapshot(snapshotNo))->*storage_).aMat[localIndex_] = a;
62 >    ((snapshotMan_->getSnapshot(snapshotNo))->*storage_).unitVector[localIndex_] = a.inverse() * sU_.getColum(2);    
63 >
64 >    std::vector<Atom*>::iterator i;
65 >    for (i = atoms_.begin(); i != atoms_.end(); ++i) {
66 >        if ((*i)->isDirectional()) {
67 >            (*i)->setA(a * (*i)->getA(snapshotNo), snapshotNo);
68 >        }
69 >    }
70 >
71 > }  
72 >
73 > void  DirectionalAtom::setUnitFrameFromEuler(double phi, double theta, double psi) {
74 >    sU_.setupRotMat(phi,theta,psi);
75   }
76  
77 < void RigidBody::getPos(double theP[3]){
78 <  for (int i = 0; i < 3 ; i++)
79 <    theP[i] = pos[i];
67 < }      
77 > Mat3x3d RigidBody::getI() {
78 >    return inertiaTensor_;
79 > }    
80  
81 < void RigidBody::setPos(double theP[3]){
82 <  for (int i = 0; i < 3 ; i++)
83 <    pos[i] = theP[i];
84 < }      
81 > std::vector<double> RigidBody::getGrad() {
82 >    vector<double> grad(6, 0.0);
83 >    Vector3d force;
84 >    Vector3d torque;
85 >    Vector3d myEuler;
86 >    double phi, theta, psi;
87 >    double cphi, sphi, ctheta, stheta;
88 >    Vector3d ephi;
89 >    Vector3d etheta;
90 >    Vector3d epsi;
91  
92 < void RigidBody::getVel(double theV[3]){
93 <  for (int i = 0; i < 3 ; i++)
94 <    theV[i] = vel[i];
77 < }      
92 >    force = getFrc();
93 >    torque =getTrq();
94 >    myEuler = getA().toEulerAngles();
95  
96 < void RigidBody::setVel(double theV[3]){
97 <  for (int i = 0; i < 3 ; i++)
98 <    vel[i] = theV[i];
82 < }      
96 >    phi = myEuler[0];
97 >    theta = myEuler[1];
98 >    psi = myEuler[2];
99  
100 < void RigidBody::getFrc(double theF[3]){
101 <  for (int i = 0; i < 3 ; i++)
102 <    theF[i] = frc[i];
103 < }      
100 >    cphi = cos(phi);
101 >    sphi = sin(phi);
102 >    ctheta = cos(theta);
103 >    stheta = sin(theta);
104  
105 < void RigidBody::addFrc(double theF[3]){
90 <  for (int i = 0; i < 3 ; i++)
91 <    frc[i] += theF[i];
92 < }    
105 >    // get unit vectors along the phi, theta and psi rotation axes
106  
107 < void RigidBody::zeroForces() {
107 >    ephi[0] = 0.0;
108 >    ephi[1] = 0.0;
109 >    ephi[2] = 1.0;
110  
111 <  for (int i = 0; i < 3; i++) {
112 <    frc[i] = 0.0;
113 <    trq[i] = 0.0;
99 <  }
111 >    etheta[0] = cphi;
112 >    etheta[1] = sphi;
113 >    etheta[2] = 0.0;
114  
115 < }
115 >    epsi[0] = stheta * cphi;
116 >    epsi[1] = stheta * sphi;
117 >    epsi[2] = ctheta;
118  
119 < void RigidBody::setEuler( double phi, double theta, double psi ){
120 <  
121 <    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);
119 >    //gradient is equal to -force
120 >    for (int j = 0 ; j<3; j++)
121 >        grad[j] = -force[j];
122  
123 < }
123 >    for (int j = 0; j < 3; j++ ) {
124  
125 < void RigidBody::getQ( double q[4] ){
126 <  
127 <  double t, s;
128 <  double ad1, ad2, ad3;
125 >        grad[3] += torque[j]*ephi[j];
126 >        grad[4] += torque[j]*etheta[j];
127 >        grad[5] += torque[j]*epsi[j];
128 >
129 >    }
130      
131 <  t = A[0][0] + A[1][1] + A[2][2] + 1.0;
132 <  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 < }
131 >    return grad;
132 > }    
133  
134 < void RigidBody::setQ( double the_q[4] ){
134 > void RigidBody::accept(BaseVisitor* v) {
135 >    v->visit(this);
136 > }    
137  
138 <  double q0Sqr, q1Sqr, q2Sqr, q3Sqr;
139 <  
170 <  q0Sqr = the_q[0] * the_q[0];
171 <  q1Sqr = the_q[1] * the_q[1];
172 <  q2Sqr = the_q[2] * the_q[2];
173 <  q3Sqr = the_q[3] * the_q[3];
174 <  
175 <  A[0][0] = q0Sqr + q1Sqr - q2Sqr - q3Sqr;
176 <  A[0][1] = 2.0 * ( the_q[1] * the_q[2] + the_q[0] * the_q[3] );
177 <  A[0][2] = 2.0 * ( the_q[1] * the_q[3] - the_q[0] * the_q[2] );
178 <  
179 <  A[1][0] = 2.0 * ( the_q[1] * the_q[2] - the_q[0] * the_q[3] );
180 <  A[1][1] = q0Sqr - q1Sqr + q2Sqr - q3Sqr;
181 <  A[1][2] = 2.0 * ( the_q[2] * the_q[3] + the_q[0] * the_q[1] );
182 <  
183 <  A[2][0] = 2.0 * ( the_q[1] * the_q[3] + the_q[0] * the_q[2] );
184 <  A[2][1] = 2.0 * ( the_q[2] * the_q[3] - the_q[0] * the_q[1] );
185 <  A[2][2] = q0Sqr - q1Sqr -q2Sqr +q3Sqr;  
186 <
187 < }
188 <
189 < void RigidBody::getA( double the_A[3][3] ){
190 <  
191 <  for (int i = 0; i < 3; i++)
192 <    for (int j = 0; j < 3; j++)
193 <      the_A[i][j] = A[i][j];
194 <
195 < }
196 <
197 < void RigidBody::setA( double the_A[3][3] ){
198 <
199 <  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 < }
204 <
205 < void RigidBody::getJ( double theJ[3] ){
206 <  
207 <  for (int i = 0; i < 3; i++)
208 <    theJ[i] = ji[i];
209 <
210 < }
211 <
212 < void RigidBody::setJ( double theJ[3] ){
213 <  
214 <  for (int i = 0; i < 3; i++)
215 <    ji[i] = theJ[i];
216 <
217 < }
218 <
219 < void RigidBody::getTrq(double theT[3]){
220 <  for (int i = 0; i < 3 ; i++)
221 <    theT[i] = trq[i];
222 < }      
223 <
224 < void RigidBody::addTrq(double theT[3]){
225 <  for (int i = 0; i < 3 ; i++)
226 <    trq[i] += theT[i];
227 < }      
228 <
229 < void RigidBody::getI( double the_I[3][3] ){  
230 <
231 <    for (int i = 0; i < 3; i++)
232 <      for (int j = 0; j < 3; j++)
233 <        the_I[i][j] = I[i][j];
234 <
235 < }
236 <
237 < void RigidBody::lab2Body( double r[3] ){
238 <
239 <  double rl[3]; // the lab frame vector
240 <  
241 <  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]);
248 <
249 < }
250 <
251 < void RigidBody::body2Lab( double r[3] ){
252 <
253 <  double rb[3]; // the body frame vector
254 <  
255 <  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]);
262 <
263 < }
264 <
265 < double RigidBody::getZangle( ){
266 <    return zAngle;
267 < }
268 <
269 < void RigidBody::setZangle( double zAng ){
270 <    zAngle = zAng;
271 < }
272 <
273 < void RigidBody::addZangle( double zAng ){
274 <    zAngle += zAng;
275 < }
276 <
277 < void RigidBody::calcRefCoords( ) {
278 <
279 <  int i,j,k, it, n_linear_coords;
138 > void  RigidBody::calcRefCoords() {
139 >  /*
140    double mtmp;
141    vec3 apos;
142    double refCOM[3];
# Line 292 | Line 152 | void RigidBody::calcRefCoords( ) {
152    for (j=0; j<3; j++)
153      refCOM[j] = 0.0;
154    
155 <  for (i = 0; i < myAtoms.size(); i++) {
156 <    mtmp = myAtoms[i]->getMass();
155 >  for (i = 0; i < atoms_.size(); i++) {
156 >    mtmp = atoms_[i]->getMass();
157      mass += mtmp;
158  
159      apos = refCoords[i];
# Line 308 | Line 168 | void RigidBody::calcRefCoords( ) {
168  
169   // Next, move the origin of the reference coordinate system to the COM:
170  
171 <  for (i = 0; i < myAtoms.size(); i++) {
171 >  for (i = 0; i < atoms_.size(); i++) {
172      apos = refCoords[i];
173      for (j=0; j < 3; j++) {
174        apos[j] = apos[j] - refCOM[j];
# Line 322 | Line 182 | void RigidBody::calcRefCoords( ) {
182      for (j = 0; j < 3; j++)
183        Itmp[i][j] = 0.0;  
184    
185 <  for (it = 0; it < myAtoms.size(); it++) {
185 >  for (it = 0; it < atoms_.size(); it++) {
186  
187 <    mtmp = myAtoms[it]->getMass();
187 >    mtmp = atoms_[it]->getMass();
188      ptmp = refCoords[it];
189      r= norm3(ptmp.vec);
190      r2 = r*r;
# Line 384 | Line 244 | void RigidBody::calcRefCoords( ) {
244        sU[i][j] /= len;
245      }
246    }
247 +  */
248   }
249  
250 < void RigidBody::doEulerToRotMat(vec3 &euler, mat3x3 &myA ){
250 > void  RigidBody::calcForcesAndTorques() {
251 >    unsigned int i;
252 >    unsigned int j;
253 >    Vector3d afrc;
254 >    Vector3d atrq;
255 >    Vector3d rpos;
256 >    Vector3d frc;
257 >    Vector3d trq;
258  
391 –  double phi, theta, psi;
392 –  
393 –  phi = euler[0];
394 –  theta = euler[1];
395 –  psi = euler[2];
396 –  
397 –  myA[0][0] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi));
398 –  myA[0][1] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi));
399 –  myA[0][2] = sin(theta) * sin(psi);
400 –  
401 –  myA[1][0] = -(cos(phi) * sin(psi)) - (sin(phi) * cos(theta) * cos(psi));
402 –  myA[1][1] = -(sin(phi) * sin(psi)) + (cos(phi) * cos(theta) * cos(psi));
403 –  myA[1][2] = sin(theta) * cos(psi);
404 –  
405 –  myA[2][0] = sin(phi) * sin(theta);
406 –  myA[2][1] = -cos(phi) * sin(theta);
407 –  myA[2][2] = cos(theta);
259  
260 < }
260 >    zeroForces();
261 >    
262 >    frc = getFrc();
263 >    trq = getTrq();
264  
265 < void RigidBody::calcForcesAndTorques() {
265 >    for (i = 0; i < atoms_.size(); i++) {
266  
267 <  // 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];
267 >        afrc = atoms_[i]->getFrc();
268  
269 <  zeroForces();
270 <  
271 <  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];
430 <    }
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];
269 >        rpos = refCoords_[i];
270 >        
271 >        frc += afrc;
272  
273 <    // If the atom has a torque associated with it, then we also need to
274 <    // migrate the torques onto the center of mass:
273 >        trq[0] += rpos[1]*afrc[2] - rpos[2]*afrc[1];
274 >        trq[1] += rpos[2]*afrc[0] - rpos[0]*afrc[2];
275 >        trq[2] += rpos[0]*afrc[1] - rpos[1]*afrc[0];
276  
277 <    if (myAtoms[i]->isDirectional()) {
277 >        // If the atom has a torque associated with it, then we also need to
278 >        // migrate the torques onto the center of mass:
279  
280 <      myAtoms[i]->getTrq(atrq);
281 <      
282 <      for (j=0; j<3; j++)
283 <        trq[j] += atrq[j];
280 >        if (atoms_[i]->isDirectional()) {
281 >            atrq = atoms_[i]->getTrq();
282 >            trq += atrq;
283 >        }
284 >        
285      }
446 –  }
447 –
448 –  // Convert Torque to Body-fixed coordinates:
449 –  // (Actually, on second thought, don't.  Integrator does this now.)
450 –  // lab2Body(trq);
451 –
452 – }
453 –
454 – void RigidBody::updateAtoms() {
455 –  int i, j;
456 –  vec3 ref;
457 –  double apos[3];
458 –  DirectionalAtom* dAtom;
459 –  
460 –  for (i = 0; i < myAtoms.size(); i++) {
461 –    
462 –    ref = refCoords[i];
463 –
464 –    body2Lab(ref.vec);
286      
287 <    for (j = 0; j<3; j++)
288 <      apos[j] = pos[j] + ref.vec[j];
287 >    setFrc(frc);
288 >    setTrq(trq);
289      
469 –    myAtoms[i]->setPos(apos);
470 –    
471 –    if (myAtoms[i]->isDirectional()) {
472 –      
473 –      dAtom = (DirectionalAtom *) myAtoms[i];
474 –      dAtom->rotateBy( A );
475 –      
476 –    }
477 –  }  
290   }
291  
292 < void RigidBody::getGrad( double grad[6] ) {
292 > void  RigidBody::updateAtoms() {
293 >    unsigned int i;
294 >    unsigned int j;
295 >    Vector3d ref;
296 >    Vector3d apos;
297 >    DirectionalAtom* dAtom;
298 >    Vector3d pos = getPos();
299 >    RotMat3x3d A = getA();
300 >    
301 >    for (i = 0; i < atoms_.size(); i++) {
302 >    
303 >        ref = body2Lab(refCoords_[i]);
304  
305 <  double myEuler[3];
483 <  double phi, theta, psi;
484 <  double cphi, sphi, ctheta, stheta;
485 <  double ephi[3];
486 <  double etheta[3];
487 <  double epsi[3];
488 <  
489 <  this->getEulerAngles(myEuler);
305 >        apos = pos + ref;
306  
307 <  phi = myEuler[0];
492 <  theta = myEuler[1];
493 <  psi = myEuler[2];
307 >        atoms_[i]->setPos(apos);
308  
309 <  cphi = cos(phi);
310 <  sphi = sin(phi);
311 <  ctheta = cos(theta);
312 <  stheta = sin(theta);
309 >        if (atoms_[i]->isDirectional()) {
310 >          
311 >          dAtom = (DirectionalAtom *) atoms_[i];
312 >          dAtom->rotateBy( A );      
313 >        }
314  
315 <  // 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;
315 >    }
316    
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++ ) {
522 –    
523 –    grad[3] += trq[j]*ephi[j];
524 –    grad[4] += trq[j]*etheta[j];
525 –    grad[5] += trq[j]*epsi[j];
526 –    
527 –  }
528 –  
317   }
318  
531 – /**
532 –  * getEulerAngles computes a set of Euler angle values consistent
533 –  * with an input rotation matrix.  They are returned in the following
534 –  * order:
535 –  *  myEuler[0] = phi;
536 –  *  myEuler[1] = theta;
537 –  *  myEuler[2] = psi;
538 – */
539 – void RigidBody::getEulerAngles(double myEuler[3]) {
319  
320 <  // We use so-called "x-convention", which is the most common
321 <  // 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;
320 > bool RigidBody::getAtomPos(Vector3d& pos, unsigned int index) {
321 >    if (index < atoms_.size()) {
322  
323 <  theta = acos(min(1.0,max(-1.0,A[2][2])));
324 <  ctheta = A[2][2];
325 <  stheta = sqrt(1.0 - ctheta * ctheta);
326 <
327 <  // when sin(theta) is close to 0, we need to consider the
328 <  // possibility of a singularity. In this case, we can assign an
329 <  // arbitary value to phi (or psi), and then determine the psi (or
330 <  // 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;
323 >        Vector3d ref = body2Lab(refCoords_[index]);
324 >        pos = getPos() + ref;
325 >        return true;
326 >    } else {
327 >        std::cerr << index << " is an invalid index, current rigid body contains "
328 >                      << atoms_.size() << "atoms" << std::endl;
329 >        return false;
330 >    }    
331   }
332  
333 < double RigidBody::max(double x, double  y) {  
334 <  return (x > y) ? x : y;
333 > bool RigidBody::getAtomPos(Vector3d& pos, Atom* atom) {
334 >    std::vector<Atom*>::iterator i;
335 >    i = find(atoms_.begin(), atoms_.end(), atom);
336 >    if (i != atoms_.end()) {
337 >        //RigidBody class makes sure refCoords_ and atoms_ match each other
338 >        Vector3d ref = body2Lab(refCoords_[i - atoms_.begin()]);
339 >        pos = getPos() + ref;
340 >        return true;
341 >    } else {
342 >        std::cerr << "Atom " << atom->getGlobalIndex()
343 >                      <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;
344 >        return false;
345 >    }
346   }
347 + bool RigidBody::getAtomVel(Vector3d& vel, unsigned int index) {
348  
349 < double RigidBody::min(double x, double  y) {  
603 <  return (x > y) ? y : x;
604 < }
349 >    //velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
350  
351 < 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++) {
621 <    
622 <    mtmp = myAtoms[i]->getMass();    
623 <    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;
631 <    }
632 <    
633 <  }
634 <  
635 <  for(j = 0; j < 3; j++) {
636 <    pos[j] /= mass;
637 <    vel[j] /= mass;
638 <  }
351 >    if (index < atoms_.size()) {
352  
353 < }
353 >        Vector3d velRot;
354 >        Mat3x3d skewMat;;
355 >        Vector3d ref = refCoords_[index];
356 >        Vector3d ji = getJ();
357 >        Mat3x3d I =  getI();
358  
359 < void RigidBody::accept(BaseVisitor* v){
360 <  vector<Atom*>::iterator atomIter;
361 <  v->visit(this);
359 >        skewMat(0, 0) =0;
360 >        skewMat(0, 1) = ji[2] /I(2, 2);
361 >        skewMat(0, 2) = -ji[1] /I(1, 1);
362  
363 <  //for(atomIter = myAtoms.begin(); atomIter != myAtoms.end(); ++atomIter)
364 <  //  (*atomIter)->accept(v);
365 < }
649 < void RigidBody::getAtomRefCoor(double pos[3], int index){
650 <  vec3 ref;
363 >        skewMat(1, 0) = -ji[2] /I(2, 2);
364 >        skewMat(1, 1) = 0;
365 >        skewMat(1, 2) = ji[0]/I(0, 0);
366  
367 <  ref = refCoords[index];
368 <  pos[0] = ref[0];
369 <  pos[1] = ref[1];
655 <  pos[2] = ref[2];
656 <  
657 < }
367 >        skewMat(2, 0) =ji[1] /I(1, 1);
368 >        skewMat(2, 1) = -ji[0]/I(0, 0);
369 >        skewMat(2, 2) = 0;
370  
371 +        velRot = (getA() * skewMat).transpose() * ref;
372  
373 < void RigidBody::getAtomPos(double theP[3], int index){
374 <  vec3 ref;
373 >        vel =getVel() + velRot;
374 >        return true;
375 >        
376 >    } else {
377 >        std::cerr << index << " is an invalid index, current rigid body contains "
378 >                      << atoms_.size() << "atoms" << std::endl;
379 >        return false;
380 >    }
381 > }
382  
383 <  if (index >= myAtoms.size())
664 <    cerr << index << " is an invalid index, current rigid body contains " << myAtoms.size() << "atoms" << endl;
383 > bool RigidBody::getAtomVel(Vector3d& vel, Atom* atom) {
384  
385 <  ref = refCoords[index];
386 <  body2Lab(ref.vec);
387 <  
388 <  theP[0] = pos[0] + ref[0];
389 <  theP[1] = pos[1] + ref[1];
390 <  theP[2] = pos[2] + ref[2];
385 >    std::vector<Atom*>::iterator i;
386 >    i = find(atoms_.begin(), atoms_.end(), atom);
387 >    if (i != atoms_.end()) {
388 >        return getAtomVel(vel, i - atoms_.begin());
389 >    } else {
390 >        std::cerr << "Atom " << atom->getGlobalIndex()
391 >                      <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;    
392 >        return false;
393 >    }    
394   }
395  
396 + bool RigidBody::getAtomRefCoor(Vector3d& coor, unsigned int index) {
397 +    if (index < atoms_.size()) {
398  
399 < void RigidBody::getAtomVel(double theV[3], int index){
400 <  vec3 ref;
401 <  double velRot[3];
402 <  double skewMat[3][3];
403 <  double aSkewMat[3][3];
404 <  double aSkewTransMat[3][3];
405 <  
682 <  //velRot = $(A\cdot skew(I^{-1}j))^{T}refCoor$
399 >        coor = refCoords_[index];
400 >        return true;
401 >    } else {
402 >        std::cerr << index << " is an invalid index, current rigid body contains "
403 >                      << atoms_.size() << "atoms" << std::endl;
404 >        return false;
405 >    }
406  
407 <  if (index >= myAtoms.size())
685 <    cerr << index << " is an invalid index, current rigid body contains " << myAtoms.size() << "atoms" << endl;
407 > }
408  
409 <  ref = refCoords[index];
409 > bool RigidBody::getAtomRefCoor(Vector3d& coor, Atom* atom) {
410 >    std::vector<Atom*>::iterator i;
411 >    i = find(atoms_.begin(), atoms_.end(), atom);
412 >    if (i != atoms_.end()) {
413 >        //RigidBody class makes sure refCoords_ and atoms_ match each other
414 >        coor = refCoords_[i - atoms_.begin()];
415 >        return true;
416 >    } else {
417 >        std::cerr << "Atom " << atom->getGlobalIndex()
418 >                      <<" does not belong to Rigid body "<< getGlobalIndex() << std::endl;    
419 >        return false;
420 >    }
421  
422 <  skewMat[0][0] =0;
690 <  skewMat[0][1] = ji[2] /I[2][2];
691 <  skewMat[0][2] = -ji[1] /I[1][1];
422 > }
423  
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];
424   }
425  
711 –

Diff Legend

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