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