173 |
|
|
174 |
|
void RigidBody::calcRefCoords( ) { |
175 |
|
|
176 |
< |
int i, j, it, n_linear_coords; |
176 |
> |
int i, j, it, n_linear_coords, pAxis, maxAxis, midAxis; |
177 |
|
double mtmp; |
178 |
|
vec3 apos; |
179 |
|
double refCOM[3]; |
180 |
|
vec3 ptmp; |
181 |
|
double Itmp[3][3]; |
182 |
+ |
double pAxisMat[3][3], pAxisRotMat[3][3]; |
183 |
|
double evals[3]; |
184 |
+ |
double prePos[3], rotPos[3]; |
185 |
|
double r, r2, len; |
186 |
+ |
double iMat[3][3]; |
187 |
|
|
188 |
|
// First, find the center of mass: |
189 |
|
|
270 |
|
exit(-1); |
271 |
|
} |
272 |
|
|
273 |
+ |
//sort and reorder the moment axes |
274 |
+ |
if (evals[0] < evals[1] && evals[0] < evals[2]) |
275 |
+ |
pAxis = 0; |
276 |
+ |
else if (evals[1] < evals[2]) |
277 |
+ |
pAxis = 1; |
278 |
+ |
else |
279 |
+ |
pAxis = 2; |
280 |
+ |
|
281 |
+ |
if (evals[0] > evals[1] && evals[0] > evals[2]) |
282 |
+ |
maxAxis = 0; |
283 |
+ |
else if (evals[1] > evals[2]) |
284 |
+ |
maxAxis = 1; |
285 |
+ |
else |
286 |
+ |
maxAxis = 2; |
287 |
+ |
|
288 |
+ |
midAxis = 0; |
289 |
+ |
if (midAxis == pAxis || midAxis == pAxis) |
290 |
+ |
midAxis = 1; |
291 |
+ |
if (midAxis == pAxis || midAxis == pAxis) |
292 |
+ |
midAxis = 2; |
293 |
+ |
|
294 |
+ |
if (pAxis != maxAxis){ |
295 |
+ |
//zero out our matrices |
296 |
+ |
for (i=0; i<3; i++){ |
297 |
+ |
for (j=0; j<3; j++) { |
298 |
+ |
pAxisMat[i][j] = 0.0; |
299 |
+ |
pAxisRotMat[i][j] = 0.0; |
300 |
+ |
} |
301 |
+ |
} |
302 |
+ |
|
303 |
+ |
//let z be the smallest and x be the largest eigenvalue axes |
304 |
+ |
for (i=0; i<3; i++){ |
305 |
+ |
pAxisMat[i][2] = I[i][pAxis]; |
306 |
+ |
pAxisMat[i][1] = I[i][midAxis]; |
307 |
+ |
pAxisMat[i][0] = I[i][maxAxis]; |
308 |
+ |
} |
309 |
+ |
|
310 |
+ |
//calculate the proper rotation matrix |
311 |
+ |
transposeMat3(pAxisMat, pAxisRotMat); |
312 |
+ |
|
313 |
+ |
//rotate the rigid body to the principle axis frame |
314 |
+ |
for (i = 0; i < myAtoms.size(); i++) { |
315 |
+ |
apos = refCoords[i]; |
316 |
+ |
for (j=0; j<3; j++) |
317 |
+ |
prePos[j] = apos[j]; |
318 |
+ |
|
319 |
+ |
matVecMul3(pAxisRotMat, prePos, rotPos); |
320 |
+ |
|
321 |
+ |
for (j=0; j < 3; j++) |
322 |
+ |
apos[j] = rotPos[j]; |
323 |
+ |
|
324 |
+ |
refCoords[i] = apos; |
325 |
+ |
} |
326 |
+ |
|
327 |
+ |
//the lab and the body frame match up at this point, so A = Identity Matrix |
328 |
+ |
for (i=0; i<3; i++){ |
329 |
+ |
for (j=0; j<3; j++){ |
330 |
+ |
if (i == j) |
331 |
+ |
iMat[i][j] = 1.0; |
332 |
+ |
else |
333 |
+ |
iMat[i][j] = 0.0; |
334 |
+ |
} |
335 |
+ |
} |
336 |
+ |
setA(iMat); |
337 |
+ |
} |
338 |
+ |
|
339 |
|
// renormalize column vectors: |
340 |
|
|
341 |
|
for (i=0; i < 3; i++) { |