1 |
gezelter |
829 |
#include <math.h> |
2 |
mmeineke |
377 |
|
3 |
|
|
#include "Atom.hpp" |
4 |
gezelter |
1097 |
#include "DirectionalAtom.hpp" |
5 |
mmeineke |
670 |
#include "simError.h" |
6 |
gezelter |
1097 |
#include "MatVec3.h" |
7 |
mmeineke |
377 |
|
8 |
mmeineke |
670 |
void DirectionalAtom::zeroForces() { |
9 |
|
|
if( hasCoords ){ |
10 |
gezelter |
1097 |
|
11 |
|
|
Atom::zeroForces(); |
12 |
mmeineke |
670 |
|
13 |
|
|
trq[offsetX] = 0.0; |
14 |
|
|
trq[offsetY] = 0.0; |
15 |
|
|
trq[offsetZ] = 0.0; |
16 |
|
|
} |
17 |
|
|
else{ |
18 |
|
|
|
19 |
|
|
sprintf( painCave.errMsg, |
20 |
|
|
"Attempt to zero frc and trq for atom %d before coords set.\n", |
21 |
|
|
index ); |
22 |
|
|
painCave.isFatal = 1; |
23 |
|
|
simError(); |
24 |
|
|
} |
25 |
|
|
} |
26 |
mmeineke |
377 |
|
27 |
tim |
689 |
void DirectionalAtom::setCoords(void){ |
28 |
mmeineke |
414 |
|
29 |
tim |
689 |
if( myConfig->isAllocated() ){ |
30 |
|
|
|
31 |
|
|
myConfig->getAtomPointers( index, |
32 |
|
|
&pos, |
33 |
|
|
&vel, |
34 |
|
|
&frc, |
35 |
|
|
&trq, |
36 |
|
|
&Amat, |
37 |
|
|
&mu, |
38 |
tim |
1452 |
&ul, |
39 |
|
|
&quat); |
40 |
mmeineke |
670 |
} |
41 |
|
|
else{ |
42 |
|
|
sprintf( painCave.errMsg, |
43 |
tim |
689 |
"Attempted to set Atom %d coordinates with an unallocated " |
44 |
mmeineke |
787 |
"SimState object.\n", index ); |
45 |
mmeineke |
670 |
painCave.isFatal = 1; |
46 |
|
|
simError(); |
47 |
|
|
} |
48 |
tim |
689 |
|
49 |
|
|
hasCoords = true; |
50 |
|
|
|
51 |
|
|
} |
52 |
|
|
|
53 |
mmeineke |
377 |
void DirectionalAtom::setA( double the_A[3][3] ){ |
54 |
|
|
|
55 |
mmeineke |
670 |
if( hasCoords ){ |
56 |
|
|
Amat[Axx] = the_A[0][0]; Amat[Axy] = the_A[0][1]; Amat[Axz] = the_A[0][2]; |
57 |
|
|
Amat[Ayx] = the_A[1][0]; Amat[Ayy] = the_A[1][1]; Amat[Ayz] = the_A[1][2]; |
58 |
|
|
Amat[Azx] = the_A[2][0]; Amat[Azy] = the_A[2][1]; Amat[Azz] = the_A[2][2]; |
59 |
|
|
|
60 |
|
|
this->updateU(); |
61 |
|
|
} |
62 |
|
|
else{ |
63 |
|
|
|
64 |
|
|
sprintf( painCave.errMsg, |
65 |
|
|
"Attempt to set Amat for atom %d before coords set.\n", |
66 |
|
|
index ); |
67 |
|
|
painCave.isFatal = 1; |
68 |
|
|
simError(); |
69 |
|
|
} |
70 |
mmeineke |
377 |
} |
71 |
|
|
|
72 |
gezelter |
1097 |
void DirectionalAtom::setI( double the_I[3][3] ){ |
73 |
mmeineke |
377 |
|
74 |
|
|
Ixx = the_I[0][0]; Ixy = the_I[0][1]; Ixz = the_I[0][2]; |
75 |
|
|
Iyx = the_I[1][0]; Iyy = the_I[1][1]; Iyz = the_I[1][2]; |
76 |
|
|
Izx = the_I[2][0]; Izy = the_I[2][1]; Izz = the_I[2][2]; |
77 |
|
|
} |
78 |
|
|
|
79 |
|
|
void DirectionalAtom::setQ( double the_q[4] ){ |
80 |
|
|
|
81 |
|
|
double q0Sqr, q1Sqr, q2Sqr, q3Sqr; |
82 |
|
|
|
83 |
mmeineke |
670 |
if( hasCoords ){ |
84 |
|
|
q0Sqr = the_q[0] * the_q[0]; |
85 |
|
|
q1Sqr = the_q[1] * the_q[1]; |
86 |
|
|
q2Sqr = the_q[2] * the_q[2]; |
87 |
|
|
q3Sqr = the_q[3] * the_q[3]; |
88 |
|
|
|
89 |
|
|
|
90 |
|
|
Amat[Axx] = q0Sqr + q1Sqr - q2Sqr - q3Sqr; |
91 |
|
|
Amat[Axy] = 2.0 * ( the_q[1] * the_q[2] + the_q[0] * the_q[3] ); |
92 |
|
|
Amat[Axz] = 2.0 * ( the_q[1] * the_q[3] - the_q[0] * the_q[2] ); |
93 |
|
|
|
94 |
|
|
Amat[Ayx] = 2.0 * ( the_q[1] * the_q[2] - the_q[0] * the_q[3] ); |
95 |
|
|
Amat[Ayy] = q0Sqr - q1Sqr + q2Sqr - q3Sqr; |
96 |
|
|
Amat[Ayz] = 2.0 * ( the_q[2] * the_q[3] + the_q[0] * the_q[1] ); |
97 |
|
|
|
98 |
|
|
Amat[Azx] = 2.0 * ( the_q[1] * the_q[3] + the_q[0] * the_q[2] ); |
99 |
|
|
Amat[Azy] = 2.0 * ( the_q[2] * the_q[3] - the_q[0] * the_q[1] ); |
100 |
|
|
Amat[Azz] = q0Sqr - q1Sqr -q2Sqr +q3Sqr; |
101 |
|
|
|
102 |
|
|
this->updateU(); |
103 |
|
|
} |
104 |
|
|
else{ |
105 |
|
|
|
106 |
|
|
sprintf( painCave.errMsg, |
107 |
|
|
"Attempt to set Q for atom %d before coords set.\n", |
108 |
|
|
index ); |
109 |
|
|
painCave.isFatal = 1; |
110 |
|
|
simError(); |
111 |
|
|
} |
112 |
mmeineke |
377 |
|
113 |
|
|
} |
114 |
|
|
|
115 |
|
|
void DirectionalAtom::getA( double the_A[3][3] ){ |
116 |
|
|
|
117 |
mmeineke |
670 |
if( hasCoords ){ |
118 |
|
|
the_A[0][0] = Amat[Axx]; |
119 |
|
|
the_A[0][1] = Amat[Axy]; |
120 |
|
|
the_A[0][2] = Amat[Axz]; |
121 |
|
|
|
122 |
|
|
the_A[1][0] = Amat[Ayx]; |
123 |
|
|
the_A[1][1] = Amat[Ayy]; |
124 |
|
|
the_A[1][2] = Amat[Ayz]; |
125 |
|
|
|
126 |
|
|
the_A[2][0] = Amat[Azx]; |
127 |
|
|
the_A[2][1] = Amat[Azy]; |
128 |
|
|
the_A[2][2] = Amat[Azz]; |
129 |
|
|
} |
130 |
|
|
else{ |
131 |
|
|
|
132 |
|
|
sprintf( painCave.errMsg, |
133 |
|
|
"Attempt to get Amat for atom %d before coords set.\n", |
134 |
|
|
index ); |
135 |
|
|
painCave.isFatal = 1; |
136 |
|
|
simError(); |
137 |
|
|
} |
138 |
mmeineke |
377 |
|
139 |
|
|
} |
140 |
|
|
|
141 |
mmeineke |
597 |
void DirectionalAtom::printAmatIndex( void ){ |
142 |
mmeineke |
377 |
|
143 |
mmeineke |
670 |
if( hasCoords ){ |
144 |
|
|
std::cerr << "Atom[" << index << "] index =>\n" |
145 |
|
|
<< "[ " << Axx << ", " << Axy << ", " << Axz << " ]\n" |
146 |
|
|
<< "[ " << Ayx << ", " << Ayy << ", " << Ayz << " ]\n" |
147 |
|
|
<< "[ " << Azx << ", " << Azy << ", " << Azz << " ]\n"; |
148 |
|
|
} |
149 |
|
|
else{ |
150 |
|
|
|
151 |
|
|
sprintf( painCave.errMsg, |
152 |
|
|
"Attempt to print Amat indices for atom %d before coords set.\n", |
153 |
|
|
index ); |
154 |
|
|
painCave.isFatal = 1; |
155 |
|
|
simError(); |
156 |
|
|
} |
157 |
mmeineke |
597 |
} |
158 |
|
|
|
159 |
|
|
|
160 |
mmeineke |
377 |
void DirectionalAtom::getU( double the_u[3] ){ |
161 |
|
|
|
162 |
gezelter |
1097 |
the_u[0] = sU[2][0]; |
163 |
|
|
the_u[1] = sU[2][1]; |
164 |
|
|
the_u[2] = sU[2][2]; |
165 |
|
|
|
166 |
mmeineke |
377 |
this->body2Lab( the_u ); |
167 |
|
|
} |
168 |
|
|
|
169 |
|
|
void DirectionalAtom::getQ( double q[4] ){ |
170 |
|
|
|
171 |
|
|
double t, s; |
172 |
|
|
double ad1, ad2, ad3; |
173 |
|
|
|
174 |
mmeineke |
670 |
if( hasCoords ){ |
175 |
mmeineke |
377 |
|
176 |
mmeineke |
670 |
t = Amat[Axx] + Amat[Ayy] + Amat[Azz] + 1.0; |
177 |
|
|
if( t > 0.0 ){ |
178 |
mmeineke |
377 |
|
179 |
mmeineke |
670 |
s = 0.5 / sqrt( t ); |
180 |
|
|
q[0] = 0.25 / s; |
181 |
|
|
q[1] = (Amat[Ayz] - Amat[Azy]) * s; |
182 |
|
|
q[2] = (Amat[Azx] - Amat[Axz]) * s; |
183 |
|
|
q[3] = (Amat[Axy] - Amat[Ayx]) * s; |
184 |
mmeineke |
377 |
} |
185 |
|
|
else{ |
186 |
|
|
|
187 |
mmeineke |
670 |
ad1 = fabs( Amat[Axx] ); |
188 |
|
|
ad2 = fabs( Amat[Ayy] ); |
189 |
|
|
ad3 = fabs( Amat[Azz] ); |
190 |
|
|
|
191 |
|
|
if( ad1 >= ad2 && ad1 >= ad3 ){ |
192 |
|
|
|
193 |
|
|
s = 2.0 * sqrt( 1.0 + Amat[Axx] - Amat[Ayy] - Amat[Azz] ); |
194 |
|
|
q[0] = (Amat[Ayz] + Amat[Azy]) / s; |
195 |
|
|
q[1] = 0.5 / s; |
196 |
|
|
q[2] = (Amat[Axy] + Amat[Ayx]) / s; |
197 |
|
|
q[3] = (Amat[Axz] + Amat[Azx]) / s; |
198 |
|
|
} |
199 |
|
|
else if( ad2 >= ad1 && ad2 >= ad3 ){ |
200 |
|
|
|
201 |
|
|
s = sqrt( 1.0 + Amat[Ayy] - Amat[Axx] - Amat[Azz] ) * 2.0; |
202 |
|
|
q[0] = (Amat[Axz] + Amat[Azx]) / s; |
203 |
|
|
q[1] = (Amat[Axy] + Amat[Ayx]) / s; |
204 |
|
|
q[2] = 0.5 / s; |
205 |
|
|
q[3] = (Amat[Ayz] + Amat[Azy]) / s; |
206 |
|
|
} |
207 |
|
|
else{ |
208 |
|
|
|
209 |
|
|
s = sqrt( 1.0 + Amat[Azz] - Amat[Axx] - Amat[Ayy] ) * 2.0; |
210 |
|
|
q[0] = (Amat[Axy] + Amat[Ayx]) / s; |
211 |
|
|
q[1] = (Amat[Axz] + Amat[Azx]) / s; |
212 |
|
|
q[2] = (Amat[Ayz] + Amat[Azy]) / s; |
213 |
|
|
q[3] = 0.5 / s; |
214 |
|
|
} |
215 |
mmeineke |
377 |
} |
216 |
|
|
} |
217 |
mmeineke |
670 |
else{ |
218 |
|
|
|
219 |
|
|
sprintf( painCave.errMsg, |
220 |
|
|
"Attempt to get Q for atom %d before coords set.\n", |
221 |
|
|
index ); |
222 |
|
|
painCave.isFatal = 1; |
223 |
|
|
simError(); |
224 |
|
|
} |
225 |
mmeineke |
377 |
} |
226 |
|
|
|
227 |
gezelter |
1097 |
void DirectionalAtom::setUnitFrameFromEuler(double phi, |
228 |
|
|
double theta, |
229 |
|
|
double psi) { |
230 |
mmeineke |
377 |
|
231 |
gezelter |
1097 |
double myA[3][3]; |
232 |
|
|
double uFrame[3][3]; |
233 |
|
|
double len; |
234 |
|
|
int i, j; |
235 |
|
|
|
236 |
|
|
myA[0][0] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi)); |
237 |
|
|
myA[0][1] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi)); |
238 |
|
|
myA[0][2] = sin(theta) * sin(psi); |
239 |
|
|
|
240 |
|
|
myA[1][0] = -(cos(phi) * sin(psi)) - (sin(phi) * cos(theta) * cos(psi)); |
241 |
|
|
myA[1][1] = -(sin(phi) * sin(psi)) + (cos(phi) * cos(theta) * cos(psi)); |
242 |
|
|
myA[1][2] = sin(theta) * cos(psi); |
243 |
|
|
|
244 |
|
|
myA[2][0] = sin(phi) * sin(theta); |
245 |
|
|
myA[2][1] = -cos(phi) * sin(theta); |
246 |
|
|
myA[2][2] = cos(theta); |
247 |
|
|
|
248 |
|
|
// Make the unit Frame: |
249 |
|
|
|
250 |
|
|
for (i=0; i < 3; i++) |
251 |
|
|
for (j=0; j < 3; j++) |
252 |
|
|
uFrame[i][j] = 0.0; |
253 |
|
|
|
254 |
|
|
for (i=0; i < 3; i++) |
255 |
|
|
uFrame[i][i] = 1.0; |
256 |
|
|
|
257 |
|
|
// rotate by the given rotation matrix: |
258 |
|
|
|
259 |
|
|
matMul3(myA, uFrame, sU); |
260 |
|
|
|
261 |
|
|
// renormalize column vectors: |
262 |
|
|
|
263 |
|
|
for (i=0; i < 3; i++) { |
264 |
|
|
len = 0.0; |
265 |
|
|
for (j = 0; j < 3; j++) { |
266 |
|
|
len += sU[i][j]*sU[i][j]; |
267 |
|
|
} |
268 |
|
|
len = sqrt(len); |
269 |
|
|
for (j = 0; j < 3; j++) { |
270 |
|
|
sU[i][j] /= len; |
271 |
|
|
} |
272 |
|
|
} |
273 |
|
|
|
274 |
|
|
// sU now contains the coordinates of the 'special' frame; |
275 |
|
|
|
276 |
|
|
} |
277 |
|
|
|
278 |
mmeineke |
377 |
void DirectionalAtom::setEuler( double phi, double theta, double psi ){ |
279 |
|
|
|
280 |
mmeineke |
670 |
if( hasCoords ){ |
281 |
|
|
Amat[Axx] = (cos(phi) * cos(psi)) - (sin(phi) * cos(theta) * sin(psi)); |
282 |
|
|
Amat[Axy] = (sin(phi) * cos(psi)) + (cos(phi) * cos(theta) * sin(psi)); |
283 |
|
|
Amat[Axz] = sin(theta) * sin(psi); |
284 |
|
|
|
285 |
|
|
Amat[Ayx] = -(cos(phi) * sin(psi)) - (sin(phi) * cos(theta) * cos(psi)); |
286 |
|
|
Amat[Ayy] = -(sin(phi) * sin(psi)) + (cos(phi) * cos(theta) * cos(psi)); |
287 |
|
|
Amat[Ayz] = sin(theta) * cos(psi); |
288 |
|
|
|
289 |
|
|
Amat[Azx] = sin(phi) * sin(theta); |
290 |
|
|
Amat[Azy] = -cos(phi) * sin(theta); |
291 |
|
|
Amat[Azz] = cos(theta); |
292 |
|
|
|
293 |
|
|
this->updateU(); |
294 |
|
|
} |
295 |
|
|
else{ |
296 |
|
|
|
297 |
|
|
sprintf( painCave.errMsg, |
298 |
|
|
"Attempt to set Euler angles for atom %d before coords set.\n", |
299 |
|
|
index ); |
300 |
|
|
painCave.isFatal = 1; |
301 |
|
|
simError(); |
302 |
|
|
} |
303 |
mmeineke |
377 |
} |
304 |
|
|
|
305 |
|
|
|
306 |
|
|
void DirectionalAtom::lab2Body( double r[3] ){ |
307 |
|
|
|
308 |
|
|
double rl[3]; // the lab frame vector |
309 |
|
|
|
310 |
mmeineke |
670 |
if( hasCoords ){ |
311 |
|
|
rl[0] = r[0]; |
312 |
|
|
rl[1] = r[1]; |
313 |
|
|
rl[2] = r[2]; |
314 |
|
|
|
315 |
|
|
r[0] = (Amat[Axx] * rl[0]) + (Amat[Axy] * rl[1]) + (Amat[Axz] * rl[2]); |
316 |
|
|
r[1] = (Amat[Ayx] * rl[0]) + (Amat[Ayy] * rl[1]) + (Amat[Ayz] * rl[2]); |
317 |
|
|
r[2] = (Amat[Azx] * rl[0]) + (Amat[Azy] * rl[1]) + (Amat[Azz] * rl[2]); |
318 |
|
|
} |
319 |
|
|
else{ |
320 |
|
|
|
321 |
|
|
sprintf( painCave.errMsg, |
322 |
|
|
"Attempt to convert lab2body for atom %d before coords set.\n", |
323 |
|
|
index ); |
324 |
|
|
painCave.isFatal = 1; |
325 |
|
|
simError(); |
326 |
|
|
} |
327 |
mmeineke |
377 |
|
328 |
|
|
} |
329 |
|
|
|
330 |
gezelter |
1097 |
void DirectionalAtom::rotateBy( double by_A[3][3]) { |
331 |
|
|
|
332 |
|
|
// Check this |
333 |
|
|
|
334 |
|
|
double r00, r01, r02, r10, r11, r12, r20, r21, r22; |
335 |
|
|
|
336 |
|
|
if( hasCoords ){ |
337 |
|
|
|
338 |
|
|
r00 = by_A[0][0]*Amat[Axx] + by_A[0][1]*Amat[Ayx] + by_A[0][2]*Amat[Azx]; |
339 |
|
|
r01 = by_A[0][0]*Amat[Axy] + by_A[0][1]*Amat[Ayy] + by_A[0][2]*Amat[Azy]; |
340 |
|
|
r02 = by_A[0][0]*Amat[Axz] + by_A[0][1]*Amat[Ayz] + by_A[0][2]*Amat[Azz]; |
341 |
|
|
|
342 |
|
|
r10 = by_A[1][0]*Amat[Axx] + by_A[1][1]*Amat[Ayx] + by_A[1][2]*Amat[Azx]; |
343 |
|
|
r11 = by_A[1][0]*Amat[Axy] + by_A[1][1]*Amat[Ayy] + by_A[1][2]*Amat[Azy]; |
344 |
|
|
r12 = by_A[1][0]*Amat[Axz] + by_A[1][1]*Amat[Ayz] + by_A[1][2]*Amat[Azz]; |
345 |
|
|
|
346 |
|
|
r20 = by_A[2][0]*Amat[Axx] + by_A[2][1]*Amat[Ayx] + by_A[2][2]*Amat[Azx]; |
347 |
|
|
r21 = by_A[2][0]*Amat[Axy] + by_A[2][1]*Amat[Ayy] + by_A[2][2]*Amat[Azy]; |
348 |
|
|
r22 = by_A[2][0]*Amat[Axz] + by_A[2][1]*Amat[Ayz] + by_A[2][2]*Amat[Azz]; |
349 |
|
|
|
350 |
|
|
Amat[Axx] = r00; Amat[Axy] = r01; Amat[Axz] = r02; |
351 |
|
|
Amat[Ayx] = r10; Amat[Ayy] = r11; Amat[Ayz] = r12; |
352 |
|
|
Amat[Azx] = r20; Amat[Azy] = r21; Amat[Azz] = r22; |
353 |
|
|
|
354 |
|
|
} |
355 |
|
|
else{ |
356 |
|
|
|
357 |
|
|
sprintf( painCave.errMsg, |
358 |
|
|
"Attempt to rotate frame for atom %d before coords set.\n", |
359 |
|
|
index ); |
360 |
|
|
painCave.isFatal = 1; |
361 |
|
|
simError(); |
362 |
|
|
} |
363 |
|
|
|
364 |
|
|
} |
365 |
|
|
|
366 |
|
|
|
367 |
mmeineke |
377 |
void DirectionalAtom::body2Lab( double r[3] ){ |
368 |
|
|
|
369 |
|
|
double rb[3]; // the body frame vector |
370 |
|
|
|
371 |
mmeineke |
670 |
if( hasCoords ){ |
372 |
|
|
rb[0] = r[0]; |
373 |
|
|
rb[1] = r[1]; |
374 |
|
|
rb[2] = r[2]; |
375 |
|
|
|
376 |
|
|
r[0] = (Amat[Axx] * rb[0]) + (Amat[Ayx] * rb[1]) + (Amat[Azx] * rb[2]); |
377 |
|
|
r[1] = (Amat[Axy] * rb[0]) + (Amat[Ayy] * rb[1]) + (Amat[Azy] * rb[2]); |
378 |
|
|
r[2] = (Amat[Axz] * rb[0]) + (Amat[Ayz] * rb[1]) + (Amat[Azz] * rb[2]); |
379 |
|
|
} |
380 |
|
|
else{ |
381 |
|
|
|
382 |
|
|
sprintf( painCave.errMsg, |
383 |
|
|
"Attempt to convert body2lab for atom %d before coords set.\n", |
384 |
|
|
index ); |
385 |
|
|
painCave.isFatal = 1; |
386 |
|
|
simError(); |
387 |
|
|
} |
388 |
mmeineke |
377 |
} |
389 |
|
|
|
390 |
|
|
void DirectionalAtom::updateU( void ){ |
391 |
|
|
|
392 |
mmeineke |
670 |
if( hasCoords ){ |
393 |
gezelter |
1097 |
ul[offsetX] = (Amat[Axx] * sU[2][0]) + |
394 |
|
|
(Amat[Ayx] * sU[2][1]) + (Amat[Azx] * sU[2][2]); |
395 |
|
|
ul[offsetY] = (Amat[Axy] * sU[2][0]) + |
396 |
|
|
(Amat[Ayy] * sU[2][1]) + (Amat[Azy] * sU[2][2]); |
397 |
|
|
ul[offsetZ] = (Amat[Axz] * sU[2][0]) + |
398 |
|
|
(Amat[Ayz] * sU[2][1]) + (Amat[Azz] * sU[2][2]); |
399 |
mmeineke |
670 |
} |
400 |
|
|
else{ |
401 |
|
|
|
402 |
|
|
sprintf( painCave.errMsg, |
403 |
|
|
"Attempt to updateU for atom %d before coords set.\n", |
404 |
|
|
index ); |
405 |
|
|
painCave.isFatal = 1; |
406 |
|
|
simError(); |
407 |
|
|
} |
408 |
mmeineke |
377 |
} |
409 |
|
|
|
410 |
mmeineke |
599 |
void DirectionalAtom::getJ( double theJ[3] ){ |
411 |
|
|
|
412 |
|
|
theJ[0] = jx; |
413 |
|
|
theJ[1] = jy; |
414 |
|
|
theJ[2] = jz; |
415 |
|
|
} |
416 |
|
|
|
417 |
|
|
void DirectionalAtom::setJ( double theJ[3] ){ |
418 |
|
|
|
419 |
|
|
jx = theJ[0]; |
420 |
|
|
jy = theJ[1]; |
421 |
|
|
jz = theJ[2]; |
422 |
|
|
} |
423 |
|
|
|
424 |
|
|
void DirectionalAtom::getTrq( double theT[3] ){ |
425 |
|
|
|
426 |
mmeineke |
670 |
if( hasCoords ){ |
427 |
|
|
theT[0] = trq[offsetX]; |
428 |
|
|
theT[1] = trq[offsetY]; |
429 |
|
|
theT[2] = trq[offsetZ]; |
430 |
|
|
} |
431 |
|
|
else{ |
432 |
|
|
|
433 |
|
|
sprintf( painCave.errMsg, |
434 |
|
|
"Attempt to get Trq for atom %d before coords set.\n", |
435 |
|
|
index ); |
436 |
|
|
painCave.isFatal = 1; |
437 |
|
|
simError(); |
438 |
|
|
} |
439 |
mmeineke |
599 |
} |
440 |
|
|
|
441 |
tim |
1452 |
void DirectionalAtom::setTrq( double theT[3] ){ |
442 |
|
|
|
443 |
|
|
if( hasCoords ){ |
444 |
|
|
trq[offsetX] = theT[0]; |
445 |
|
|
trq[offsetY] = theT[1]; |
446 |
|
|
trq[offsetZ] = theT[2]; |
447 |
|
|
} |
448 |
|
|
else{ |
449 |
|
|
|
450 |
|
|
sprintf( painCave.errMsg, |
451 |
|
|
"Attempt to add Trq for atom %d before coords set.\n", |
452 |
|
|
index ); |
453 |
|
|
painCave.isFatal = 1; |
454 |
|
|
simError(); |
455 |
|
|
} |
456 |
|
|
} |
457 |
|
|
|
458 |
mmeineke |
599 |
void DirectionalAtom::addTrq( double theT[3] ){ |
459 |
|
|
|
460 |
mmeineke |
670 |
if( hasCoords ){ |
461 |
|
|
trq[offsetX] += theT[0]; |
462 |
|
|
trq[offsetY] += theT[1]; |
463 |
|
|
trq[offsetZ] += theT[2]; |
464 |
|
|
} |
465 |
|
|
else{ |
466 |
|
|
|
467 |
|
|
sprintf( painCave.errMsg, |
468 |
|
|
"Attempt to add Trq for atom %d before coords set.\n", |
469 |
|
|
index ); |
470 |
|
|
painCave.isFatal = 1; |
471 |
|
|
simError(); |
472 |
|
|
} |
473 |
mmeineke |
599 |
} |
474 |
|
|
|
475 |
|
|
|
476 |
|
|
void DirectionalAtom::getI( double the_I[3][3] ){ |
477 |
|
|
|
478 |
|
|
the_I[0][0] = Ixx; |
479 |
|
|
the_I[0][1] = Ixy; |
480 |
|
|
the_I[0][2] = Ixz; |
481 |
|
|
|
482 |
|
|
the_I[1][0] = Iyx; |
483 |
|
|
the_I[1][1] = Iyy; |
484 |
|
|
the_I[1][2] = Iyz; |
485 |
|
|
|
486 |
|
|
the_I[2][0] = Izx; |
487 |
|
|
the_I[2][1] = Izy; |
488 |
|
|
the_I[2][2] = Izz; |
489 |
|
|
} |
490 |
gezelter |
878 |
|
491 |
|
|
void DirectionalAtom::getGrad( double grad[6] ) { |
492 |
|
|
|
493 |
|
|
double myEuler[3]; |
494 |
|
|
double phi, theta, psi; |
495 |
|
|
double cphi, sphi, ctheta, stheta; |
496 |
|
|
double ephi[3]; |
497 |
|
|
double etheta[3]; |
498 |
|
|
double epsi[3]; |
499 |
|
|
|
500 |
|
|
this->getEulerAngles(myEuler); |
501 |
|
|
|
502 |
|
|
phi = myEuler[0]; |
503 |
|
|
theta = myEuler[1]; |
504 |
|
|
psi = myEuler[2]; |
505 |
|
|
|
506 |
|
|
cphi = cos(phi); |
507 |
|
|
sphi = sin(phi); |
508 |
|
|
ctheta = cos(theta); |
509 |
|
|
stheta = sin(theta); |
510 |
|
|
|
511 |
|
|
// get unit vectors along the phi, theta and psi rotation axes |
512 |
|
|
|
513 |
|
|
ephi[0] = 0.0; |
514 |
|
|
ephi[1] = 0.0; |
515 |
|
|
ephi[2] = 1.0; |
516 |
tim |
1046 |
|
517 |
|
|
etheta[0] = cphi; |
518 |
|
|
etheta[1] = sphi; |
519 |
gezelter |
878 |
etheta[2] = 0.0; |
520 |
|
|
|
521 |
tim |
1046 |
epsi[0] = stheta * cphi; |
522 |
|
|
epsi[1] = stheta * sphi; |
523 |
|
|
epsi[2] = ctheta; |
524 |
|
|
|
525 |
gezelter |
878 |
for (int j = 0 ; j<3; j++) |
526 |
|
|
grad[j] = frc[j]; |
527 |
|
|
|
528 |
tim |
1046 |
grad[3] = 0; |
529 |
|
|
grad[4] = 0; |
530 |
|
|
grad[5] = 0; |
531 |
|
|
|
532 |
gezelter |
878 |
for (int j = 0; j < 3; j++ ) { |
533 |
|
|
|
534 |
|
|
grad[3] += trq[j]*ephi[j]; |
535 |
|
|
grad[4] += trq[j]*etheta[j]; |
536 |
|
|
grad[5] += trq[j]*epsi[j]; |
537 |
|
|
|
538 |
|
|
} |
539 |
|
|
|
540 |
|
|
} |
541 |
|
|
|
542 |
tim |
1046 |
/** |
543 |
|
|
* getEulerAngles computes a set of Euler angle values consistent |
544 |
|
|
* with an input rotation matrix. They are returned in the following |
545 |
|
|
* order: |
546 |
|
|
* myEuler[0] = phi; |
547 |
|
|
* myEuler[1] = theta; |
548 |
|
|
* myEuler[2] = psi; |
549 |
|
|
*/ |
550 |
gezelter |
878 |
void DirectionalAtom::getEulerAngles(double myEuler[3]) { |
551 |
|
|
|
552 |
tim |
1046 |
// We use so-called "x-convention", which is the most common definition. |
553 |
|
|
// In this convention, the rotation given by Euler angles (phi, theta, psi), where the first |
554 |
|
|
// rotation is by an angle phi about the z-axis, the second is by an angle |
555 |
|
|
// theta (0 <= theta <= 180)about the x-axis, and thethird is by an angle psi about the |
556 |
|
|
//z-axis (again). |
557 |
gezelter |
878 |
|
558 |
tim |
1046 |
|
559 |
gezelter |
878 |
double phi,theta,psi,eps; |
560 |
gezelter |
1252 |
double ctheta,stheta; |
561 |
gezelter |
878 |
|
562 |
|
|
// set the tolerance for Euler angles and rotation elements |
563 |
|
|
|
564 |
|
|
eps = 1.0e-8; |
565 |
|
|
|
566 |
tim |
1046 |
theta = acos(min(1.0,max(-1.0,Amat[Azz]))); |
567 |
|
|
ctheta = Amat[Azz]; |
568 |
|
|
stheta = sqrt(1.0 - ctheta * ctheta); |
569 |
|
|
|
570 |
|
|
// when sin(theta) is close to 0, we need to consider singularity |
571 |
|
|
// In this case, we can assign an arbitary value to phi (or psi), and then determine |
572 |
|
|
// the psi (or phi) or vice-versa. We'll assume that phi always gets the rotation, and psi is 0 |
573 |
|
|
// in cases of singularity. |
574 |
|
|
// we use atan2 instead of atan, since atan2 will give us -Pi to Pi. |
575 |
|
|
// Since 0 <= theta <= 180, sin(theta) will be always non-negative. Therefore, it never |
576 |
|
|
// change the sign of both of the parameters passed to atan2. |
577 |
gezelter |
878 |
|
578 |
tim |
1046 |
if (fabs(stheta) <= eps){ |
579 |
|
|
psi = 0.0; |
580 |
|
|
phi = atan2(-Amat[Ayx], Amat[Axx]); |
581 |
gezelter |
878 |
} |
582 |
tim |
1046 |
// we only have one unique solution |
583 |
|
|
else{ |
584 |
|
|
phi = atan2(Amat[Azx], -Amat[Azy]); |
585 |
|
|
psi = atan2(Amat[Axz], Amat[Ayz]); |
586 |
|
|
} |
587 |
gezelter |
878 |
|
588 |
tim |
1046 |
//wrap phi and psi, make sure they are in the range from 0 to 2*Pi |
589 |
|
|
//if (phi < 0) |
590 |
|
|
// phi += M_PI; |
591 |
gezelter |
878 |
|
592 |
tim |
1046 |
//if (psi < 0) |
593 |
|
|
// psi += M_PI; |
594 |
gezelter |
878 |
|
595 |
|
|
myEuler[0] = phi; |
596 |
|
|
myEuler[1] = theta; |
597 |
|
|
myEuler[2] = psi; |
598 |
tim |
1046 |
|
599 |
gezelter |
878 |
return; |
600 |
|
|
} |
601 |
|
|
|
602 |
chrisfen |
1187 |
double DirectionalAtom::getZangle( ){ |
603 |
|
|
|
604 |
|
|
if( hasCoords ){ |
605 |
|
|
return zAngle; |
606 |
|
|
} |
607 |
|
|
else{ |
608 |
|
|
|
609 |
|
|
sprintf( painCave.errMsg, |
610 |
|
|
"Attempt to get zAngle for atom %d before coords set.\n", |
611 |
|
|
index ); |
612 |
|
|
painCave.isFatal = 1; |
613 |
|
|
simError(); |
614 |
|
|
return 0; |
615 |
|
|
} |
616 |
|
|
} |
617 |
|
|
|
618 |
|
|
void DirectionalAtom::setZangle( double zAng ){ |
619 |
|
|
|
620 |
|
|
if( hasCoords ){ |
621 |
|
|
zAngle = zAng; |
622 |
|
|
} |
623 |
|
|
else{ |
624 |
|
|
|
625 |
|
|
sprintf( painCave.errMsg, |
626 |
|
|
"Attempt to set zAngle for atom %d before coords set.\n", |
627 |
|
|
index ); |
628 |
|
|
painCave.isFatal = 1; |
629 |
|
|
simError(); |
630 |
|
|
} |
631 |
|
|
} |
632 |
|
|
|
633 |
|
|
void DirectionalAtom::addZangle( double zAng ){ |
634 |
|
|
|
635 |
|
|
if( hasCoords ){ |
636 |
|
|
zAngle += zAng; |
637 |
|
|
} |
638 |
|
|
else{ |
639 |
|
|
|
640 |
|
|
sprintf( painCave.errMsg, |
641 |
|
|
"Attempt to add zAngle to atom %d before coords set.\n", |
642 |
|
|
index ); |
643 |
|
|
painCave.isFatal = 1; |
644 |
|
|
simError(); |
645 |
|
|
} |
646 |
|
|
} |
647 |
|
|
|
648 |
gezelter |
878 |
double DirectionalAtom::max(double x, double y) { |
649 |
|
|
return (x > y) ? x : y; |
650 |
|
|
} |
651 |
|
|
|
652 |
|
|
double DirectionalAtom::min(double x, double y) { |
653 |
|
|
return (x > y) ? y : x; |
654 |
|
|
} |