ViewVC Help
View File | Revision Log | Show Annotations | View Changeset | Root Listing
root/group/trunk/OOPSE/libmdtools/DirectionalAtom.cpp
(Generate patch)

Comparing trunk/OOPSE/libmdtools/DirectionalAtom.cpp (file contents):
Revision 378 by mmeineke, Fri Mar 21 17:42:12 2003 UTC vs.
Revision 878 by gezelter, Fri Dec 12 15:42:13 2003 UTC

# Line 1 | Line 1
1 < #include <cmath>
1 > #include <math.h>
2  
3   #include "Atom.hpp"
4 + #include "simError.h"
5  
5 – double* Atom::pos; // the position array
6 – double* Atom::vel; // the velocity array
7 – double* Atom::frc; // the forc array
8 – double* Atom::trq; // the torque vector  ( space fixed )
9 – double* Atom::Amat; // the rotation matrix
10 – double* Atom::mu;   // the array of dipole moments
11 – double* Atom::ul;   // the lab frame unit directional vector
6  
7 +
8 + void DirectionalAtom::zeroForces() {
9 +  if( hasCoords ){
10 +    frc[offsetX] = 0.0;
11 +    frc[offsetY] = 0.0;
12 +    frc[offsetZ] = 0.0;
13 +    
14 +    trq[offsetX] = 0.0;
15 +    trq[offsetY] = 0.0;
16 +    trq[offsetZ] = 0.0;
17 +  }
18 +  else{
19 +    
20 +    sprintf( painCave.errMsg,
21 +             "Attempt to zero frc and trq for atom %d before coords set.\n",
22 +             index );
23 +    painCave.isFatal = 1;
24 +    simError();
25 +  }
26 + }
27 +
28 + void DirectionalAtom::setCoords(void){
29 +
30 +  if( myConfig->isAllocated() ){
31 +
32 +    myConfig->getAtomPointers( index,
33 +                     &pos,
34 +                     &vel,
35 +                     &frc,
36 +                     &trq,
37 +                     &Amat,
38 +                     &mu,  
39 +                     &ul );
40 +  }
41 +  else{
42 +    sprintf( painCave.errMsg,
43 +             "Attempted to set Atom %d  coordinates with an unallocated "
44 +             "SimState object.\n", index );
45 +    painCave.isFatal = 1;
46 +    simError();
47 +  }
48 +
49 +  hasCoords = true;
50 +
51 +  *mu = myMu;
52 +
53 + }
54 +
55 + double DirectionalAtom::getMu( void ) {
56 +
57 +  if( hasCoords ){
58 +    return *mu;
59 +  }
60 +  else{
61 +    return myMu;
62 +  }
63 + }
64 +
65 + void DirectionalAtom::setMu( double the_mu ) {
66 +
67 +  if( hasCoords ){
68 +    *mu = the_mu;
69 +    myMu = the_mu;
70 +  }
71 +  else{
72 +    myMu = the_mu;
73 +  }
74 + }
75 +
76   void DirectionalAtom::setA( double the_A[3][3] ){
14 –  
15 –  Amat[Axx] = the_A[0][0]; Amat[Axy] = the_A[0][1]; Amat[Axz] = the_A[0][2];
16 –  Amat[Ayx] = the_A[1][0]; Amat[Ayy] = the_A[1][1]; Amat[Ayz] = the_A[1][2];
17 –  Amat[Azx] = the_A[2][0]; Amat[Azy] = the_A[2][1]; Amat[Azz] = the_A[2][2];
77  
78 <  this->updateU();  
78 >  if( hasCoords ){
79 >    Amat[Axx] = the_A[0][0]; Amat[Axy] = the_A[0][1]; Amat[Axz] = the_A[0][2];
80 >    Amat[Ayx] = the_A[1][0]; Amat[Ayy] = the_A[1][1]; Amat[Ayz] = the_A[1][2];
81 >    Amat[Azx] = the_A[2][0]; Amat[Azy] = the_A[2][1]; Amat[Azz] = the_A[2][2];
82 >    
83 >    this->updateU();  
84 >  }
85 >  else{
86 >    
87 >    sprintf( painCave.errMsg,
88 >             "Attempt to set Amat for atom %d before coords set.\n",
89 >             index );
90 >    painCave.isFatal = 1;
91 >    simError();
92 >  }
93   }
94  
95   void DirectionalAtom::setI( double the_I[3][3] ){
# Line 30 | Line 103 | void DirectionalAtom::setQ( double the_q[4] ){
103  
104    double q0Sqr, q1Sqr, q2Sqr, q3Sqr;
105  
106 <  q0Sqr = the_q[0] * the_q[0];
107 <  q1Sqr = the_q[1] * the_q[1];
108 <  q2Sqr = the_q[2] * the_q[2];
109 <  q3Sqr = the_q[3] * the_q[3];
110 <  
106 >  if( hasCoords ){
107 >    q0Sqr = the_q[0] * the_q[0];
108 >    q1Sqr = the_q[1] * the_q[1];
109 >    q2Sqr = the_q[2] * the_q[2];
110 >    q3Sqr = the_q[3] * the_q[3];
111 >    
112 >    
113 >    Amat[Axx] = q0Sqr + q1Sqr - q2Sqr - q3Sqr;
114 >    Amat[Axy] = 2.0 * ( the_q[1] * the_q[2] + the_q[0] * the_q[3] );
115 >    Amat[Axz] = 2.0 * ( the_q[1] * the_q[3] - the_q[0] * the_q[2] );
116 >    
117 >    Amat[Ayx] = 2.0 * ( the_q[1] * the_q[2] - the_q[0] * the_q[3] );
118 >    Amat[Ayy] = q0Sqr - q1Sqr + q2Sqr - q3Sqr;
119 >    Amat[Ayz] = 2.0 * ( the_q[2] * the_q[3] + the_q[0] * the_q[1] );
120 >    
121 >    Amat[Azx] = 2.0 * ( the_q[1] * the_q[3] + the_q[0] * the_q[2] );
122 >    Amat[Azy] = 2.0 * ( the_q[2] * the_q[3] - the_q[0] * the_q[1] );
123 >    Amat[Azz] = q0Sqr - q1Sqr -q2Sqr +q3Sqr;
124 >    
125 >    this->updateU();
126 >  }
127 >  else{
128 >    
129 >    sprintf( painCave.errMsg,
130 >             "Attempt to set Q for atom %d before coords set.\n",
131 >             index );
132 >    painCave.isFatal = 1;
133 >    simError();
134 >  }
135  
39 –  Amat[Axx] = q0Sqr + q1Sqr - q2Sqr - q3Sqr;
40 –  Amat[Axy] = 2.0 * ( the_q[1] * the_q[2] + the_q[0] * the_q[3] );
41 –  Amat[Axz] = 2.0 * ( the_q[1] * the_q[3] - the_q[0] * the_q[2] );
42 –  
43 –  Amat[Ayx] = 2.0 * ( the_q[1] * the_q[2] - the_q[0] * the_q[3] );
44 –  Amat[Ayy] = q0Sqr - q1Sqr + q2Sqr - q3Sqr;
45 –  Amat[Ayz] = 2.0 * ( the_q[2] * the_q[3] + the_q[0] * the_q[1] );
46 –
47 –  Amat[Azx] = 2.0 * ( the_q[1] * the_q[3] + the_q[0] * the_q[2] );
48 –  Amat[Azy] = 2.0 * ( the_q[2] * the_q[3] - the_q[0] * the_q[1] );
49 –  Amat[Azz] = q0Sqr - q1Sqr -q2Sqr +q3Sqr;
50 –
51 –  this->updateU();
136   }
137  
138   void DirectionalAtom::getA( double the_A[3][3] ){
139    
140 <  the_A[0][0] = Amat[Axx];
141 <  the_A[0][1] = Amat[Axy];
142 <  the_A[0][2] = Amat[Axz];
140 >  if( hasCoords ){
141 >    the_A[0][0] = Amat[Axx];
142 >    the_A[0][1] = Amat[Axy];
143 >    the_A[0][2] = Amat[Axz];
144 >    
145 >    the_A[1][0] = Amat[Ayx];
146 >    the_A[1][1] = Amat[Ayy];
147 >    the_A[1][2] = Amat[Ayz];
148 >    
149 >    the_A[2][0] = Amat[Azx];
150 >    the_A[2][1] = Amat[Azy];
151 >    the_A[2][2] = Amat[Azz];
152 >  }
153 >  else{
154 >    
155 >    sprintf( painCave.errMsg,
156 >             "Attempt to get Amat for atom %d before coords set.\n",
157 >             index );
158 >    painCave.isFatal = 1;
159 >    simError();
160 >  }
161  
162 <  the_A[1][0] = Amat[Ayx];
61 <  the_A[1][1] = Amat[Ayy];
62 <  the_A[1][2] = Amat[Ayz];
162 > }
163  
164 <  the_A[2][0] = Amat[Azx];
165 <  the_A[2][1] = Amat[Azy];
166 <  the_A[2][2] = Amat[Azz];
164 > void DirectionalAtom::printAmatIndex( void ){
165 >
166 >  if( hasCoords ){
167 >    std::cerr << "Atom[" << index << "] index =>\n"
168 >              << "[ " << Axx << ", " << Axy << ", " << Axz << " ]\n"
169 >              << "[ " << Ayx << ", " << Ayy << ", " << Ayz << " ]\n"
170 >              << "[ " << Azx << ", " << Azy << ", " << Azz << " ]\n";
171 >  }
172 >  else{
173 >    
174 >    sprintf( painCave.errMsg,
175 >             "Attempt to print Amat indices for atom %d before coords set.\n",
176 >             index );
177 >    painCave.isFatal = 1;
178 >    simError();
179 >  }
180   }
181  
182  
# Line 81 | Line 194 | void DirectionalAtom::getQ( double q[4] ){
194    double t, s;
195    double ad1, ad2, ad3;
196  
197 <  t = Amat[Axx] + Amat[Ayy] + Amat[Azz] + 1.0;
85 <  if( t > 0.0 ){
197 >  if( hasCoords ){
198      
199 <    s = 0.5 / sqrt( t );
200 <    q[0] = 0.25 / s;
89 <    q[1] = (Amat[Ayz] - Amat[Azy]) * s;
90 <    q[2] = (Amat[Azx] - Amat[Axz]) * s;
91 <    q[3] = (Amat[Axy] - Amat[Ayx]) * s;
92 <  }
93 <  else{
94 <    
95 <    ad1 = fabs( Amat[Axx] );
96 <    ad2 = fabs( Amat[Ayy] );
97 <    ad3 = fabs( Amat[Azz] );
98 <
99 <    if( ad1 >= ad2 && ad1 >= ad3 ){
199 >    t = Amat[Axx] + Amat[Ayy] + Amat[Azz] + 1.0;
200 >    if( t > 0.0 ){
201        
202 <      s = 2.0 * sqrt( 1.0 + Amat[Axx] - Amat[Ayy] - Amat[Azz] );
203 <      q[0] = (Amat[Ayz] + Amat[Azy]) / s;
204 <      q[1] = 0.5 / s;
205 <      q[2] = (Amat[Axy] + Amat[Ayx]) / s;
206 <      q[3] = (Amat[Axz] + Amat[Azx]) / s;
202 >      s = 0.5 / sqrt( t );
203 >      q[0] = 0.25 / s;
204 >      q[1] = (Amat[Ayz] - Amat[Azy]) * s;
205 >      q[2] = (Amat[Azx] - Amat[Axz]) * s;
206 >      q[3] = (Amat[Axy] - Amat[Ayx]) * s;
207      }
107 –    else if( ad2 >= ad1 && ad2 >= ad3 ){
108 –
109 –      s = sqrt( 1.0 + Amat[Ayy] - Amat[Axx] - Amat[Azz] ) * 2.0;
110 –      q[0] = (Amat[Axz] + Amat[Azx]) / s;
111 –      q[1] = (Amat[Axy] + Amat[Ayx]) / s;
112 –      q[2] = 0.5 / s;
113 –      q[3] = (Amat[Ayz] + Amat[Azy]) / s;
114 –    }
208      else{
209        
210 <      s = sqrt( 1.0 + Amat[Azz] - Amat[Axx] - Amat[Ayy] ) * 2.0;
211 <      q[0] = (Amat[Axy] + Amat[Ayx]) / s;
212 <      q[1] = (Amat[Axz] + Amat[Azx]) / s;
213 <      q[2] = (Amat[Ayz] + Amat[Azy]) / s;
214 <      q[3] = 0.5 / s;
210 >      ad1 = fabs( Amat[Axx] );
211 >      ad2 = fabs( Amat[Ayy] );
212 >      ad3 = fabs( Amat[Azz] );
213 >      
214 >      if( ad1 >= ad2 && ad1 >= ad3 ){
215 >        
216 >        s = 2.0 * sqrt( 1.0 + Amat[Axx] - Amat[Ayy] - Amat[Azz] );
217 >        q[0] = (Amat[Ayz] + Amat[Azy]) / s;
218 >        q[1] = 0.5 / s;
219 >        q[2] = (Amat[Axy] + Amat[Ayx]) / s;
220 >        q[3] = (Amat[Axz] + Amat[Azx]) / s;
221 >      }
222 >      else if( ad2 >= ad1 && ad2 >= ad3 ){
223 >        
224 >        s = sqrt( 1.0 + Amat[Ayy] - Amat[Axx] - Amat[Azz] ) * 2.0;
225 >        q[0] = (Amat[Axz] + Amat[Azx]) / s;
226 >        q[1] = (Amat[Axy] + Amat[Ayx]) / s;
227 >        q[2] = 0.5 / s;
228 >        q[3] = (Amat[Ayz] + Amat[Azy]) / s;
229 >      }
230 >      else{
231 >        
232 >        s = sqrt( 1.0 + Amat[Azz] - Amat[Axx] - Amat[Ayy] ) * 2.0;
233 >        q[0] = (Amat[Axy] + Amat[Ayx]) / s;
234 >        q[1] = (Amat[Axz] + Amat[Azx]) / s;
235 >        q[2] = (Amat[Ayz] + Amat[Azy]) / s;
236 >        q[3] = 0.5 / s;
237 >      }
238      }
239    }
240 +  else{
241 +    
242 +    sprintf( painCave.errMsg,
243 +             "Attempt to get Q for atom %d before coords set.\n",
244 +             index );
245 +    painCave.isFatal = 1;
246 +    simError();
247 +  }
248   }
249  
250  
251   void DirectionalAtom::setEuler( double phi, double theta, double psi ){
252    
253 <  Amat[Axx] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi));
254 <  Amat[Axy] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi));
255 <  Amat[Axz] = sin(theta) * sin(psi);
256 <  
257 <  Amat[Ayx] = -(cos(phi) * sin(psi)) - (sin(phi) * cos(theta) * cos(psi));
258 <  Amat[Ayy] = -(sin(phi) * sin(psi)) + (cos(phi) * cos(theta) * cos(psi));
259 <  Amat[Ayz] = sin(theta) * cos(psi);
260 <
261 <  Amat[Azx] = sin(phi) * sin(theta);
262 <  Amat[Azy] = -cos(phi) * sin(theta);
263 <  Amat[Azz] = cos(theta);
264 <
265 <  this->updateU();
253 >  if( hasCoords ){
254 >    Amat[Axx] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi));
255 >    Amat[Axy] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi));
256 >    Amat[Axz] = sin(theta) * sin(psi);
257 >    
258 >    Amat[Ayx] = -(cos(phi) * sin(psi)) - (sin(phi) * cos(theta) * cos(psi));
259 >    Amat[Ayy] = -(sin(phi) * sin(psi)) + (cos(phi) * cos(theta) * cos(psi));
260 >    Amat[Ayz] = sin(theta) * cos(psi);
261 >    
262 >    Amat[Azx] = sin(phi) * sin(theta);
263 >    Amat[Azy] = -cos(phi) * sin(theta);
264 >    Amat[Azz] = cos(theta);
265 >    
266 >    this->updateU();
267 >  }
268 >  else{
269 >    
270 >    sprintf( painCave.errMsg,
271 >             "Attempt to set Euler angles for atom %d before coords set.\n",
272 >             index );
273 >    painCave.isFatal = 1;
274 >    simError();
275 >  }
276   }
277  
278  
# Line 146 | Line 280 | void DirectionalAtom::lab2Body( double r[3] ){
280  
281    double rl[3]; // the lab frame vector
282    
283 <  rl[0] = r[0];
284 <  rl[1] = r[1];
285 <  rl[2] = r[2];
283 >  if( hasCoords ){
284 >    rl[0] = r[0];
285 >    rl[1] = r[1];
286 >    rl[2] = r[2];
287 >    
288 >    r[0] = (Amat[Axx] * rl[0]) + (Amat[Axy] * rl[1]) + (Amat[Axz] * rl[2]);
289 >    r[1] = (Amat[Ayx] * rl[0]) + (Amat[Ayy] * rl[1]) + (Amat[Ayz] * rl[2]);
290 >    r[2] = (Amat[Azx] * rl[0]) + (Amat[Azy] * rl[1]) + (Amat[Azz] * rl[2]);
291 >  }
292 >  else{
293 >    
294 >    sprintf( painCave.errMsg,
295 >             "Attempt to convert lab2body for atom %d before coords set.\n",
296 >             index );
297 >    painCave.isFatal = 1;
298 >    simError();
299 >  }
300  
153 –  r[0] = (Amat[Axx] * rl[0]) + (Amat[Axy] * rl[1]) + (Amat[Axz] * rl[2]);
154 –  r[1] = (Amat[Ayx] * rl[0]) + (Amat[Ayy] * rl[1]) + (Amat[Ayz] * rl[2]);
155 –  r[2] = (Amat[Azx] * rl[0]) + (Amat[Azy] * rl[1]) + (Amat[Azz] * rl[2]);
301   }
302  
303   void DirectionalAtom::body2Lab( double r[3] ){
304  
305    double rb[3]; // the body frame vector
306    
307 <  rb[0] = r[0];
308 <  rb[1] = r[1];
309 <  rb[2] = r[2];
310 <
311 <  r[0] = (Amat[Axx] * rb[0]) + (Amat[Ayx] * rb[1]) + (Amat[Azx] * rb[2]);
312 <  r[1] = (Amat[Axy] * rb[0]) + (Amat[Ayy] * rb[1]) + (Amat[Azy] * rb[2]);
313 <  r[2] = (Amat[Axz] * rb[0]) + (Amat[Ayz] * rb[1]) + (Amat[Azz] * rb[2]);
307 >  if( hasCoords ){
308 >    rb[0] = r[0];
309 >    rb[1] = r[1];
310 >    rb[2] = r[2];
311 >    
312 >    r[0] = (Amat[Axx] * rb[0]) + (Amat[Ayx] * rb[1]) + (Amat[Azx] * rb[2]);
313 >    r[1] = (Amat[Axy] * rb[0]) + (Amat[Ayy] * rb[1]) + (Amat[Azy] * rb[2]);
314 >    r[2] = (Amat[Axz] * rb[0]) + (Amat[Ayz] * rb[1]) + (Amat[Azz] * rb[2]);
315 >  }
316 >  else{
317 >    
318 >    sprintf( painCave.errMsg,
319 >             "Attempt to convert body2lab for atom %d before coords set.\n",
320 >             index );
321 >    painCave.isFatal = 1;
322 >    simError();
323 >  }
324   }
325  
326   void DirectionalAtom::updateU( void ){
327  
328 <  ul[offsetX] = (Amat[Axx] * sux) + (Amat[Ayx] * suy) + (Amat[Azx] * suz);
329 <  ul[offsetY] = (Amat[Axy] * sux) + (Amat[Ayy] * suy) + (Amat[Azy] * suz);
330 <  ul[offsetZ] = (Amat[Axz] * sux) + (Amat[Ayz] * suy) + (Amat[Azz] * suz);
328 >  if( hasCoords ){
329 >    ul[offsetX] = (Amat[Axx] * sux) + (Amat[Ayx] * suy) + (Amat[Azx] * suz);
330 >    ul[offsetY] = (Amat[Axy] * sux) + (Amat[Ayy] * suy) + (Amat[Azy] * suz);
331 >    ul[offsetZ] = (Amat[Axz] * sux) + (Amat[Ayz] * suy) + (Amat[Azz] * suz);
332 >  }
333 >  else{
334 >    
335 >    sprintf( painCave.errMsg,
336 >             "Attempt to updateU for atom %d before coords set.\n",
337 >             index );
338 >    painCave.isFatal = 1;
339 >    simError();
340 >  }
341   }
342  
343 + void DirectionalAtom::getJ( double theJ[3] ){
344 +  
345 +  theJ[0] = jx;
346 +  theJ[1] = jy;
347 +  theJ[2] = jz;
348 + }
349 +
350 + void DirectionalAtom::setJ( double theJ[3] ){
351 +  
352 +  jx = theJ[0];
353 +  jy = theJ[1];
354 +  jz = theJ[2];
355 + }
356 +
357 + void DirectionalAtom::getTrq( double theT[3] ){
358 +  
359 +  if( hasCoords ){
360 +    theT[0] = trq[offsetX];
361 +    theT[1] = trq[offsetY];
362 +    theT[2] = trq[offsetZ];
363 +  }
364 +  else{
365 +    
366 +    sprintf( painCave.errMsg,
367 +             "Attempt to get Trq for atom %d before coords set.\n",
368 +             index );
369 +    painCave.isFatal = 1;
370 +    simError();
371 +  }
372 + }
373 +
374 + void DirectionalAtom::addTrq( double theT[3] ){
375 +  
376 +  if( hasCoords ){
377 +    trq[offsetX] += theT[0];
378 +    trq[offsetY] += theT[1];
379 +    trq[offsetZ] += theT[2];
380 +  }
381 +  else{
382 +    
383 +    sprintf( painCave.errMsg,
384 +             "Attempt to add Trq for atom %d before coords set.\n",
385 +             index );
386 +    painCave.isFatal = 1;
387 +    simError();
388 +  }
389 + }
390 +
391 +
392 + void DirectionalAtom::getI( double the_I[3][3] ){
393 +  
394 +  the_I[0][0] = Ixx;
395 +  the_I[0][1] = Ixy;
396 +  the_I[0][2] = Ixz;
397 +
398 +  the_I[1][0] = Iyx;
399 +  the_I[1][1] = Iyy;
400 +  the_I[1][2] = Iyz;
401 +
402 +  the_I[2][0] = Izx;
403 +  the_I[2][1] = Izy;
404 +  the_I[2][2] = Izz;
405 + }
406 +
407 + void DirectionalAtom::getGrad( double grad[6] ) {
408 +
409 +  double myEuler[3];
410 +  double phi, theta, psi;
411 +  double cphi, sphi, ctheta, stheta;
412 +  double ephi[3];
413 +  double etheta[3];
414 +  double epsi[3];
415 +
416 +  this->getEulerAngles(myEuler);
417 +
418 +  phi = myEuler[0];
419 +  theta = myEuler[1];
420 +  psi = myEuler[2];
421 +
422 +  cphi = cos(phi);
423 +  sphi = sin(phi);
424 +  ctheta = cos(theta);
425 +  stheta = sin(theta);
426 +
427 +  // get unit vectors along the phi, theta and psi rotation axes
428 +
429 +  ephi[0] = 0.0;
430 +  ephi[1] = 0.0;
431 +  ephi[2] = 1.0;
432 +  etheta[0] = -sphi;
433 +  etheta[1] = cphi;
434 +  etheta[2] = 0.0;
435 +  epsi[0] = ctheta * cphi;
436 +  epsi[1] = ctheta * sphi;
437 +  epsi[2] = -stheta;
438 +  
439 +  for (int j = 0 ; j<3; j++)
440 +    grad[j] = frc[j];
441 +
442 +  for (int j = 0; j < 3; j++ ) {
443 +    
444 +    grad[3] += trq[j]*ephi[j];
445 +    grad[4] += trq[j]*etheta[j];
446 +    grad[5] += trq[j]*epsi[j];
447 +    
448 +  }
449 +
450 + }
451 +
452 +
453 + void DirectionalAtom::getEulerAngles(double myEuler[3]) {
454 +
455 +  // getEulerAngles computes a set of Euler angle values consistent
456 +  // with an input rotation matrix.  They are returned in the following
457 +  // order:
458 +  //  myEuler[0] = phi;
459 +  //  myEuler[1] = theta;
460 +  //  myEuler[2] = psi;
461 +  
462 +  double phi,theta,psi,eps;
463 +  double pi;
464 +  double cphi,ctheta,cpsi;
465 +  double sphi,stheta,spsi;
466 +  double b[3];
467 +  int flip[3];
468 +
469 +  // set the tolerance for Euler angles and rotation elements
470 +  
471 +  eps = 1.0e-8;
472 +    
473 +  // get a trial value of theta from a single rotation element
474 +  
475 +  theta = asin(min(1.0,max(-1.0,-Amat[Axz])));
476 +  ctheta = cos(theta);
477 +  stheta = -Amat[Axz];
478 +  
479 +  // set the phi/psi difference when theta is either 90 or -90
480 +  
481 +  if (fabs(ctheta) <= eps) {
482 +    phi = 0.0;
483 +    if (fabs(Amat[Azx]) < eps) {
484 +      psi = asin(min(1.0,max(-1.0,-Amat[Ayx]/Amat[Axz])));
485 +    } else {
486 +      if (fabs(Amat[Ayx]) < eps) {
487 +        psi = acos(min(1.0,max(-1.0,-Amat[Azx]/Amat[Axz])));
488 +      } else {
489 +        psi = atan(Amat[Ayx]/Amat[Azx]);
490 +      }    
491 +    }
492 +  }
493 +
494 +  // set the phi and psi values for all other theta values
495 +  
496 +  else {
497 +    if (fabs(Amat[Axx]) < eps) {
498 +      phi = asin(min(1.0,max(-1.0,Amat[Axy]/ctheta)));
499 +    } else {
500 +      if (fabs(Amat[Axy]) < eps) {
501 +        phi = acos(min(1.0,max(-1.0,Amat[Axx]/ctheta)));
502 +      } else {
503 +        phi = atan(Amat[Axy]/Amat[Axx]);
504 +      }
505 +    }
506 +    if (fabs(Amat[Azz]) < eps) {
507 +      psi = asin(min(1.0,max(-1.0,Amat[Ayz]/ctheta)));
508 +    } else {
509 +      if (fabs(Amat[Ayz]) < eps) {
510 +        psi = acos(min(1.0,max(-1.0,Amat[Azz]/ctheta)));
511 +      }
512 +      psi = atan(Amat[Ayz]/Amat[Azz]);
513 +    }
514 +  }
515 +
516 +  // find sine and cosine of the trial phi and psi values
517 +
518 +  cphi = cos(phi);
519 +  sphi = sin(phi);
520 +  cpsi = cos(psi);
521 +  spsi = sin(psi);
522 +
523 +  // reconstruct the diagonal of the rotation matrix
524 +
525 +  b[0] = ctheta * cphi;
526 +  b[1] = spsi*stheta*sphi + cpsi*cphi;
527 +  b[2] = ctheta * cpsi;
528 +
529 +  // compare the correct matrix diagonal to rebuilt diagonal
530 +
531 +  for (int i = 0; i < 3; i++) {
532 +    flip[i] = 0;
533 +    if (fabs(Amat[3*i + i] - b[i]) > eps)  flip[i] = 1;
534 +  }
535 +
536 +  // alter Euler angles to get correct rotation matrix values
537 +  
538 +  if (flip[0] && flip[1]) phi = phi - copysign(M_PI,phi);
539 +  if (flip[0] && flip[2]) theta = -theta + copysign(M_PI, theta);
540 +  if (flip[1] && flip[2]) psi = psi - copysign(M_PI, psi);
541 +
542 +  myEuler[0] = phi;
543 +  myEuler[1] = theta;
544 +  myEuler[2] = psi;
545 +
546 +  return;
547 + }
548 +
549 + double DirectionalAtom::max(double x, double  y) {  
550 +  return (x > y) ? x : y;
551 + }
552 +
553 + double DirectionalAtom::min(double x, double  y) {  
554 +  return (x > y) ? y : x;
555 + }

Diff Legend

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