1 /*
2 Math3D.cpp
3
4 3D math routines
5
6 Modified and added to for MacMolPlt by Brett Bode 8-94
7 Added Euler angle routines and separated out math routines 4/98
8
9 Portions by:
10 Author: Michael Chen, Human Interface Group / ATG
11 Copyright � 1991-1993 Apple Computer, Inc. All rights reserved.
12 Part of Virtual Sphere Sample Code Release v1.1
13
14 */
15 #include "Globals.h"
16 #include "Geometries.h"
17 #include "Math3D.h"
18 #include "GlobalExceptions.h"
19
InitRotationMatrix(Matrix4D RotationM)20 void InitRotationMatrix(Matrix4D RotationM)
21 {
22 // Init to the identity matrix
23 for (int i=0; i<4; i++) {
24 for (int j=0; j<4; j++)
25 RotationM[i][j] = 0.0;
26 RotationM[i][i] = 1.0;
27 }
28 }
29
30 /*-------------------------------------
31 Rotate3DPt
32
33 Simple routine to rotate a 3D point
34 -------------------------------------*/
Rotate3DPt(const Matrix4D rotationMatrix,const CPoint3D & incoord,CPoint3D * outcoord)35 void Rotate3DPt(const Matrix4D rotationMatrix, const CPoint3D & incoord, CPoint3D *outcoord)
36 {
37 (*outcoord).x = ((incoord.x)*rotationMatrix[0][0] +
38 ( incoord.y)*rotationMatrix[1][0] +
39 ( incoord.z)*rotationMatrix[2][0])
40 + rotationMatrix[3][0];
41 (*outcoord).y = ((incoord.x)*rotationMatrix[0][1] +
42 ( incoord.y)*rotationMatrix[1][1] +
43 ( incoord.z)*rotationMatrix[2][1])
44 + rotationMatrix[3][1];
45 (*outcoord).z = ((incoord.x)*rotationMatrix[0][2] +
46 ( incoord.y)*rotationMatrix[1][2] +
47 ( incoord.z)*rotationMatrix[2][2])
48 + rotationMatrix[3][2];
49 return ;
50 }
51 /*-------------------------------------
52 Rotate3DOffset
53
54 Simple routine to rotate a 3D offset ignoring the translation part of the rotation Matrix
55 Useful for the normal mode vectors which are just offset from the atom center. Thus the
56 translation is already taken into account in the atom center.
57 -------------------------------------*/
Rotate3DOffset(const Matrix4D rotationMatrix,const CPoint3D & incoord,CPoint3D * outcoord)58 void Rotate3DOffset(const Matrix4D rotationMatrix, const CPoint3D & incoord, CPoint3D *outcoord)
59 {
60 (*outcoord).x = ((incoord.x)*rotationMatrix[0][0] +
61 ( incoord.y)*rotationMatrix[1][0] +
62 ( incoord.z)*rotationMatrix[2][0]);
63 (*outcoord).y = ((incoord.x)*rotationMatrix[0][1] +
64 ( incoord.y)*rotationMatrix[1][1] +
65 ( incoord.z)*rotationMatrix[2][1]);
66 (*outcoord).z = ((incoord.x)*rotationMatrix[0][2] +
67 ( incoord.y)*rotationMatrix[1][2] +
68 ( incoord.z)*rotationMatrix[2][2]);
69 }
SortzBuffer(const CPoint3D coord[],long zBuffer[],long natoms)70 void SortzBuffer(const CPoint3D coord[], long zBuffer[], long natoms) {
71 int itemp, iatm;
72 bool done=false;
73
74 do {
75 done = true;
76 for (iatm=0; iatm<(natoms-1); iatm++) {
77 if ((coord[zBuffer[iatm]]).z>(coord[zBuffer[iatm+1]]).z) {
78 itemp = zBuffer[iatm];
79 zBuffer[iatm] = zBuffer[iatm+1];
80 zBuffer[iatm+1] = itemp;
81 done = false;
82 }
83 }
84 natoms--; // The max is now at the end so we don't need to check it again
85 } while (!done);
86 } /* SortzBuffer */
BackRotate3DOffset(const Matrix4D rotMatrix,const CPoint3D * in,CPoint3D * out)87 void BackRotate3DOffset(const Matrix4D rotMatrix, const CPoint3D * in, CPoint3D * out) {
88 out->x = (in->x * rotMatrix[0][0] + in->y * rotMatrix[0][1] + in->z * rotMatrix[0][2]);
89 out->y = (in->x * rotMatrix[1][0] + in->y * rotMatrix[1][1] + in->z * rotMatrix[1][2]);
90 out->z = (in->x * rotMatrix[2][0] + in->y * rotMatrix[2][1] + in->z * rotMatrix[2][2]);
91 }
92 //Code converted from :
93 // METHOD: NOTES OF J. APPLEQUIST, 10/16/79 (JA-H-146-148).
94 // REFERENCE: H. GOLDSTEIN, "CLASSICAL MECHANICS," P. 107.
MatrixToEulerAngles(Matrix4D R,float * phi,float * psi,float * theta)95 void MatrixToEulerAngles(Matrix4D R, float * phi, float * psi, float * theta) {
96 if ((1.0-fabs(R[2][2]))>1.0e-5) { //if theta is not 0 or 180
97 double sinThetaSquared = 1.0 - R[2][2] * R[2][2];
98 //use formulas for sin/cos (psi +/- phi) to get psi and phi
99 double cSum = -(R[1][2]*R[2][1] + R[0][2]*R[2][0])/sinThetaSquared;
100 double sSum = (R[0][2]*R[2][1] - R[1][2]*R[2][0])/sinThetaSquared;
101 double cDiff = (-R[1][2]*R[2][1] + R[0][2]*R[2][0])/sinThetaSquared;
102 double sDiff = (R[0][2]*R[2][1] + R[1][2]*R[2][0])/sinThetaSquared;
103
104 if (cSum > 1.0) cSum = 1.0;
105 else if (cSum < -1.0) cSum = -1.0;
106
107 double PhiPlusPsi = acos(cSum);
108 if (sin(PhiPlusPsi)*sSum < 0.0) PhiPlusPsi *= -1.0;
109
110 if (cDiff > 1.0) cDiff = 1.0;
111 else if (cDiff < -1.0) cDiff = -1.0;
112
113 double PhiMinusPsi = acos(cDiff);
114 if (sin(PhiMinusPsi)*sDiff < 0.0) PhiMinusPsi *= -1.0;
115
116 *psi = (float) ((PhiPlusPsi + PhiMinusPsi) * 0.5);
117 *phi = (float) ((PhiPlusPsi - PhiMinusPsi) * 0.5);
118 } else { //otherwise we really only have 2 angles
119 if (R[0][0] > 1.0) R[0][0] = 1.0;
120 else if (R[0][0] < -1.0) R[0][0] = -1.0;
121
122 *psi = (float) acos(R[0][0]);
123 if (sin(*psi)*R[1][0] < 0.0) *psi *= -1.0;
124 *phi = 0.0;
125 }
126 double sin2;
127 if (fabs(*psi)-1.0e-5 <= 0.0)
128 sin2 = -R[1][2]/cos(*psi);
129 else
130 sin2 = R[0][2]/sin(*psi);
131
132 if (R[2][2] > 1.0) R[2][2] = 1.0;
133 else if (R[2][2] < -1.0) R[2][2] = -1.0;
134
135 *theta = (float) acos(R[2][2]);
136 if (sin(*theta)*sin2 < 0.0) *theta *= -1.0;
137
138 double RadToDegree = 180.0/acos(-1.0);
139 *phi *= RadToDegree;
140 *psi *= RadToDegree;
141 *theta *= RadToDegree;
142 }
143 //reference: "Foundations of Mathematical Physics" by Sadri Hassani pg 32.
EulerAnglesToMatrix(Matrix4D rotMatrix,float phi,float psi,float theta)144 void EulerAnglesToMatrix(Matrix4D rotMatrix, float phi, float psi, float theta) {
145 double degreeToRad = acos(-1.0)/180.0;
146 double cosPhi = cos(phi*degreeToRad);
147 double sinPhi = sin(phi*degreeToRad);
148 double cosPsi = cos(psi*degreeToRad);
149 double sinPsi = sin(psi*degreeToRad);
150 double cosTheta = cos(theta*degreeToRad);
151 double sinTheta = sin(theta*degreeToRad);
152
153 rotMatrix[0][0] = (float) (cosPsi*cosPhi-sinPsi*cosTheta*sinPhi);
154 rotMatrix[0][1] = (float) (-cosPsi*sinPhi-sinPsi*cosTheta*cosPhi);
155 rotMatrix[0][2] = (float) (sinPsi*sinTheta);
156 rotMatrix[1][0] = (float) (sinPsi*cosPhi+cosPsi*cosTheta*sinPhi);
157 rotMatrix[1][1] = (float) (-sinPsi*sinPhi+cosPsi*cosTheta*cosPhi);
158 rotMatrix[1][2] = (float) (-cosPsi*sinTheta);
159 rotMatrix[2][0] = (float) (sinTheta*sinPhi);
160 rotMatrix[2][1] = (float) (sinTheta*cosPhi);
161 rotMatrix[2][2] = (float) (cosTheta);
162 }
163
164
165 /*=================================================================================================
166 CopyMatrix
167 -------------------------------------------------------------------------------------------------*/
168 typedef struct { /* Struct to make matrix copying more efficient */
169 Matrix4D a;
170 } MatrixAsStruct;
171
CopyMatrix(const Matrix4D fromMatrix,Matrix4D toMatrix)172 void CopyMatrix (const Matrix4D fromMatrix, Matrix4D toMatrix)
173 {
174 * (MatrixAsStruct *) toMatrix = * (MatrixAsStruct *) fromMatrix;
175 }
176
177 /*=================================================================================================
178 CrossProduct3D
179
180 Returns the right-handed cross-product of a and b in c
181 -------------------------------------------------------------------------------------------------*/
CrossProduct3D(const CPoint3D * a,const CPoint3D * b,CPoint3D * aCrossB)182 void CrossProduct3D (const CPoint3D *a, const CPoint3D *b, CPoint3D *aCrossB)
183 {
184 aCrossB->x = a->y * b->z - a->z * b->y;
185 aCrossB->y = a->z * b->x - a->x * b->z;
186 aCrossB->z = a->x * b->y - a->y * b->x;
187 }
188
189 /*=================================================================================================
190 DotProduct3D
191
192 Returns the dot-product of a and b
193 -------------------------------------------------------------------------------------------------*/
DotProduct3D(const CPoint3D * a,const CPoint3D * b)194 float DotProduct3D (const CPoint3D *a, const CPoint3D *b)
195 {
196 return (a->x*b->x + a->y*b->y + a->z*b->z);
197 }
198
DeterminantMatrix(const Matrix4D A)199 float DeterminantMatrix(const Matrix4D A) {
200 return A[0][0]*(A[1][1]*A[2][2] - A[1][2]*A[2][1]) - A[0][1]*(A[1][0]*A[2][2] - A[1][2]*A[2][0])
201 + A[0][2]*(A[1][0]*A[2][1] - A[1][1]*A[2][0]);
202 }
203
InverseMatrix(const Matrix4D A,Matrix4D AInverse)204 void InverseMatrix(const Matrix4D A, Matrix4D AInverse) {
205 float DetA = DeterminantMatrix(A);
206
207 AInverse[0][0] = (A[1][1]*A[2][2]-A[1][2]*A[2][1])/DetA;
208 AInverse[0][1] = (A[0][2]*A[2][1]-A[0][1]*A[2][2])/DetA;
209 AInverse[0][2] = (A[0][1]*A[1][2]-A[0][2]*A[1][1])/DetA;
210 AInverse[1][0] = (A[1][2]*A[2][0]-A[1][0]*A[2][2])/DetA;
211 AInverse[1][1] = (A[0][0]*A[2][2]-A[0][2]*A[2][0])/DetA;
212 AInverse[1][2] = (A[0][2]*A[1][0]-A[0][0]*A[1][2])/DetA;
213 AInverse[2][0] = (A[1][0]*A[2][1]-A[1][1]*A[2][0])/DetA;
214 AInverse[2][1] = (A[0][1]*A[2][0]-A[0][0]*A[2][1])/DetA;
215 AInverse[2][2] = (A[0][0]*A[1][1]-A[0][1]*A[1][0])/DetA;
216 AInverse[0][3] = AInverse[1][3] = AInverse[2][3] = 0.0;
217 AInverse[3][0] = -1.0f*A[3][0];
218 AInverse[3][1] = -1.0f*A[3][1];
219 AInverse[3][2] = -1.0f*A[3][2];
220 AInverse[3][3] = A[3][3];
221 }
222 /*=================================================================================================
223 MultiplyMatrix
224
225 Matrix multiplication c = a x b
226 -------------------------------------------------------------------------------------------------*/
MultiplyMatrix(Matrix4D a,Matrix4D b,Matrix4D aTimesB)227 void MultiplyMatrix (Matrix4D a, Matrix4D b, Matrix4D aTimesB)
228 {
229 long i, j, k;
230
231 for (i=3; i>=0; i--) {
232 for (j=3; j>=0; j--) {
233 float sum = 0.0;
234 for (k=3; k>=0; k--) {
235 sum += a[i][k] * b[k][j];
236 }
237 aTimesB[i][j]= sum;
238 }
239 }
240 }
241
242 /*=================================================================================================
243 Normalize3D
244
245 Returns the normalized vector
246 -------------------------------------------------------------------------------------------------*/
Normalize3D(CPoint3D * v)247 void Normalize3D (CPoint3D *v)
248 {
249 double length;
250
251 length = sqrt ((double) (v->x * v->x + v->y * v->y + v->z * v->z));
252 if (length > 0.0) {
253 v->x /= length;
254 v->y /= length;
255 v->z /= length;
256 } else {
257 /* Vector is zero. Probably should set some error */
258 v->x = v->y = v->z = 0;
259 /* DebugMessage ("\p Normalize3D: zero vector!");*/
260 }
261 }
262
263 /*=================================================================================================
264 SetRotationMatrix
265
266 Computes a rotation matrix that would map (rotate) vectors op onto oq.
267 The rotation is about an axis perpendicular to op and oq.
268 Note this routine won't work if op or oq are zero vectors, or if they
269 are parallel or antiparallel to each other. Note the implementation below
270 is easy to read but is not done in the most efficient way. It can be
271 sped up by reusing some of the temporary results.
272
273 Modification of Michael Pique's formula in
274 Graphics Gems Vol. 1. Andrew Glassner, Ed. Addison-Wesley.
275 -------------------------------------------------------------------------------------------------*/
276 #include <iostream>
SetRotationMatrix(Matrix4D rotationMatrix,const CPoint3D * op,const CPoint3D * oq)277 void SetRotationMatrix (Matrix4D rotationMatrix, const CPoint3D *op, const CPoint3D *oq) {
278
279 float s, c;
280 CPoint3D a;
281
282 CrossProduct3D(op, oq, &a);
283 s = a.Magnitude();
284 c = DotProduct3D(op, oq);
285
286 // The cross product will be invalid if the vectors are parallel or
287 // antiparallel. But, we can still make this rotation work...
288 if (s < 1e-6f) {
289
290 // If the two vectors are antiparallel, we can just choose an
291 // arbitrary vector orthogonal the vectors and use it as the axis to
292 // rotate one vector around to the other.
293 if (c < 0.0f) {
294 OrthoVector(*op, a);
295 s = a.Magnitude();
296 }
297
298 // If the vectors are parallel, we just need an identity transform
299 // since the vectors are already aligned.
300 else {
301 InitRotationMatrix(rotationMatrix);
302 return;
303 }
304 }
305
306 Normalize3D(&a);
307 float degrees = (float) (acos(c) * 180.0 / kPi);
308 RotateAroundAxis(rotationMatrix, a, degrees);
309 }
310
RotateAroundAxis(Matrix4D m,const CPoint3D & axis,float degrees)311 void RotateAroundAxis(Matrix4D m, const CPoint3D& axis, float degrees) {
312
313 float radians;
314 float cosine;
315 float sine;
316 float cos_rec;
317
318 radians = (float)(degrees * kPi / 180.0);
319
320 sine = (float) sin(radians);
321 cosine = (float) cos(radians);
322 cos_rec = 1.0f - cosine;
323
324 m[0][0] = cos_rec * axis.x * axis.x + cosine;
325 m[1][0] = cos_rec * axis.x * axis.y - sine * axis.z;
326 m[2][0] = cos_rec * axis.x * axis.z + sine * axis.y;
327
328 m[0][1] = cos_rec * axis.y * axis.x + sine * axis.z;
329 m[1][1] = cos_rec * axis.y * axis.y + cosine;
330 m[2][1] = cos_rec * axis.y * axis.z - sine * axis.x;
331
332 m[0][2] = cos_rec * axis.z * axis.x - sine * axis.y;
333 m[1][2] = cos_rec * axis.z * axis.y + sine * axis.x;
334 m[2][2] = cos_rec * axis.z * axis.z + cosine;
335
336 m[3][0] = m[3][1] = m[3][2] = m[0][3] = m[1][3] = m[2][3] = 0.0f;
337 m[3][3] = 1.0f;
338
339 }
340
341 //Generate a rotation matrix to rotate the x-y plane to the plane defined by vectors op and oq
SetPlaneRotation(Matrix4D rotationMatrix,const CPoint3D & op,const CPoint3D & oq)342 void SetPlaneRotation(Matrix4D rotationMatrix, const CPoint3D & op, const CPoint3D & oq) {
343 CPoint3D Vector1, Vector2, Vector3;
344 Matrix4D A;
345
346 Vector1 = op;
347 Vector2 = oq;
348
349 float length = Vector1.Magnitude();
350 if (length <= 0.0) return; //2 points with the same coordinates
351 Vector1.x /= length;
352 Vector1.y /= length;
353 Vector1.z /= length;
354 length = Vector2.Magnitude();
355 if (length <= 0.0) return;
356 Vector2.x /= length;
357 Vector2.y /= length;
358 Vector2.z /= length;
359 float V1DotV2 = DotProduct3D(&Vector2, &Vector1);
360 //Make sure the vectors are not parallel or anti-parallel
361 if (fabs(V1DotV2) >= 1.0) return;
362 Vector2.x -= V1DotV2*Vector1.x;
363 Vector2.y -= V1DotV2*Vector1.y;
364 Vector2.z -= V1DotV2*Vector1.z;
365 Normalize3D(&Vector2);
366 CrossProduct3D (&Vector1, &Vector2, &Vector3);
367 Normalize3D(&Vector3);
368
369 A[0][0] = Vector1.x;
370 A[1][0] = Vector1.y;
371 A[2][0] = Vector1.z;
372 A[0][3] = 0.0;
373 A[0][1] = Vector2.x;
374 A[1][1] = Vector2.y;
375 A[2][1] = Vector2.z;
376 A[1][3] = 0.0;
377 A[0][2] = Vector3.x;
378 A[1][2] = Vector3.y;
379 A[2][2] = Vector3.z;
380 A[2][3] = A[3][0] = A[3][1] = A[3][2] = 0.0;
381 A[3][3] = 1.0;
382
383 InverseMatrix(A, rotationMatrix);
384 }
BackRotate3DPt(Matrix4D rotationMatrix,CPoint3D incoord,CPoint3D * outcoord)385 void BackRotate3DPt(Matrix4D rotationMatrix, CPoint3D incoord, CPoint3D *outcoord)
386 {
387 incoord.x += rotationMatrix[3][0];
388 incoord.y += rotationMatrix[3][1];
389 incoord.z += rotationMatrix[3][2];
390 (*outcoord).x = ((incoord.x)*rotationMatrix[0][0] +
391 ( incoord.y)*rotationMatrix[1][0] +
392 ( incoord.z)*rotationMatrix[2][0]);
393 (*outcoord).y = ((incoord.x)*rotationMatrix[0][1] +
394 ( incoord.y)*rotationMatrix[1][1] +
395 ( incoord.z)*rotationMatrix[2][1]);
396 (*outcoord).z = ((incoord.x)*rotationMatrix[0][2] +
397 ( incoord.y)*rotationMatrix[1][2] +
398 ( incoord.z)*rotationMatrix[2][2]);
399 return ;
400 }
401 /*=========================================================================
402 * OrthogonalizeRotationMatrix
403 *
404 * Orthogonalizes a pure rotation matrix (this is not checked!).
405 * Assumes the "y-axis" (2nd row) of the matrix is "correct"
406 * and make 1st and 3rd row orthogonal to it.
407 * Assumes matrix is already close to orthogonal.
408 *-------------------------------------------------------------------------*/
OrthogonalizeRotationMatrix(Matrix4D matrix)409 void OrthogonalizeRotationMatrix (Matrix4D matrix)
410 {
411 CPoint3D xAxis, yAxis, zAxis;
412
413 yAxis.x = matrix[1][0];
414 yAxis.y = matrix[1][1];
415 yAxis.z = matrix[1][2];
416 zAxis.x = matrix[2][0];
417 zAxis.y = matrix[2][1];
418 zAxis.z = matrix[2][2];
419
420 Normalize3D (&yAxis);
421 UnitCrossProduct3D (&yAxis, &zAxis, &xAxis);
422 UnitCrossProduct3D (&xAxis, &yAxis, &zAxis);
423
424 matrix[0][0] = xAxis.x;
425 matrix[0][1] = xAxis.y;
426 matrix[0][2] = xAxis.z;
427
428 matrix[1][0] = yAxis.x;
429 matrix[1][1] = yAxis.y;
430 matrix[1][2] = yAxis.z;
431
432 matrix[2][0] = zAxis.x;
433 matrix[2][1] = zAxis.y;
434 matrix[2][2] = zAxis.z;
435 }
436
437 /*=================================================================================================
438 UnitCrossProduct3D
439
440 Returns the normalized right-handed cross product of a and b in c
441 -------------------------------------------------------------------------------------------------*/
UnitCrossProduct3D(const CPoint3D * a,const CPoint3D * b,CPoint3D * aCrossB)442 void UnitCrossProduct3D (const CPoint3D *a, const CPoint3D *b, CPoint3D *aCrossB)
443 {
444 CrossProduct3D (a, b, aCrossB);
445 Normalize3D (aCrossB);
446 }
447
SymmetricJacobiDiagonalization(double * SymMatrix,double * EigenVectors,double * EigenValues,long NumVectors,long Dimension)448 void SymmetricJacobiDiagonalization(double * SymMatrix, double * EigenVectors,
449 double * EigenValues, long NumVectors, long Dimension) {
450 long i;
451
452 long MLength = NumVectors * Dimension;
453 for (i=0; i<MLength; i++) EigenVectors[i] = 0.0;
454 for (i=0; i<NumVectors; i++) EigenVectors[i+i*Dimension] = 1.0;
455
456 JacobiDiagonalization(SymMatrix, EigenVectors, Dimension, NumVectors, 0,
457 Dimension);
458 for (i=0; i<NumVectors; i++) //Copy over the eigenvalues
459 EigenValues[i] = SymMatrix[((i+1+(i+1)*(i+1))/2) - 1];
460
461 SortEigenValues(EigenVectors, EigenValues, Dimension);
462 }
463 //Matrix is a triangular matrix of dimension Dimension which will be diagonalized
464 //in place. The results are stored in EigenVectors and EigenValues. NumVectors is the
465 //number of eigenvectors to solve for and start indicates the first vector to consider
466 //for rotation, end is the last vector to consider for rotation.
JacobiDiagonalization(double * Matrix,double * EigenVectors,long Dimension,long,long Start,long End)467 void JacobiDiagonalization(double * Matrix, double * EigenVectors, long Dimension,
468 long /*NumVectors*/, long Start, long End) {
469
470 const double root2=sqrt((double)2.0);
471 long ieaa=0, ieab=0, i;
472 double tt=0.0;
473 double * big = new double[Dimension];
474 long * jbig = new long[Dimension];
475
476 if (!big || !jbig) throw MemoryError();
477
478 double eps = 64.0*EPSLON(1.0);
479
480 // loop over the columns (k) of the triangular matrix to determine the largest
481 // off-diagonal elements in row(i)
482 for (i=0; i<Dimension; i++) {
483 big[i] = 0.0;
484 jbig[i] = 0;
485 if ((i>0)&&(i>=Start)) {
486 long ii = (i*(i+1))/2;
487 long j = MIN(i, End);
488 for (long k=0; k<j; k++) {
489 if (fabs(big[i]) < fabs(Matrix[ii+k])) {
490 big[i] = Matrix[ii+k];
491 jbig[i] = k;
492 }
493 }
494 }
495 }
496 // 2x2 Jacobi iterations begin here
497 long MaxIt = MAX((Dimension*Dimension+Dimension)/2, 500);
498 long Iter = 0;
499 double SD;
500 long jj, j, it;
501 while (true) {
502 Iter++;
503 //Find the smallest diagonal element
504 SD = 1.05;
505 jj = -1;
506 for (j=0; j<Dimension; j++) {
507 jj += j + 1;
508 SD = MIN(SD, fabs(Matrix[jj]));
509 }
510 double test = MAX(eps, (1.0e-12)*MAX(SD, 3.0e-06));
511 //Find the largest off-diagonal element
512 double T = 0.0;
513 long I1 = MAX(1, Start);
514 long IB = I1;
515 for (i=I1; i<Dimension; i++) {
516 if (T<fabs(big[i])) {
517 T = fabs(big[i]);
518 IB = i;
519 }
520 }
521 //Test for convergance, then determine rotation
522 if (T<test) break;
523
524 if (Iter > MaxIt) {
525 delete [] big;
526 delete [] jbig;
527 throw DataError();
528 }
529
530 long IA = jbig[IB];
531 long IAA = IA*(IA+1)/2;
532 long IBB = IB*(IB+1)/2;
533 double Dif = Matrix[IAA+IA]-Matrix[IBB+IB];
534 double SX, CX, T2X2, T2X25;
535 if (fabs(Dif)>((4.0e-16)*T)) {
536 T2X2 = big[IB]/Dif;
537 T2X25 = T2X2*T2X2;
538 if (T2X25 > 2.0e-16) {
539 if (T2X25 > 8.0e-09) {
540 if (T2X25 > 3.0e-06) {
541 T = 0.25/sqrt(0.25 + T2X25);
542 CX = sqrt(0.5 + T);
543 SX = sqrt(0.5-T)*((T2X2>=0.0)?1.0:-1.0);
544 } else {
545 CX = 1.0 + T2X25*(T2X25*1.375 - 0.5);
546 SX = T2X2*(1.0 + T2X25*(T2X25*3.875 - 1.5));
547 }
548 } else {
549 SX = T2X2*(1.0 - 1.5*T2X25);
550 CX = 1.0 - 0.5*T2X25;
551 }
552 } else {
553 CX = 1.0;
554 SX = T2X2;
555 }
556 } else {
557 SX = CX = root2;
558 }
559 long IEAR = IAA;
560 long IEBR = IBB;
561 for (long ir=0; ir<Dimension; ir++) {
562 T = Matrix[IEAR]*SX;
563 Matrix[IEAR] = Matrix[IEAR]*CX + Matrix[IEBR]*SX;
564 Matrix[IEBR] = T - Matrix[IEBR]*CX;
565 if ((ir-IA)>=0) {
566 if ((ir-IA)==0) {
567 tt = Matrix[IEBR];
568 ieaa = IEAR;
569 ieab = IEBR;
570 Matrix[IEBR] = big[IB];
571 IEAR += ir;
572 if (jbig[ir] == 0) goto l220;
573 } else {
574 T = Matrix[IEAR];
575 it = IA;
576 IEAR += ir;
577 if ((ir-IB)==0) {
578 Matrix[ieaa] = Matrix[ieaa]*CX + Matrix[ieab]*SX;
579 Matrix[ieab] = tt*CX + Matrix[IEBR]*SX;
580 Matrix[IEBR] = tt*SX - Matrix[IEBR]*CX;
581 IEBR += ir;
582 } else {
583 if ((ir-IB)>0) {
584 if ((fabs(T) < fabs(Matrix[IEBR]))&&
585 (IB <= End)) {
586 T = Matrix[IEBR];
587 it = IB;
588 }
589 IEBR += ir;
590 }
591 if (fabs(T) >= fabs(big[ir])) {
592 big[ir] = T;
593 jbig[ir] = it;
594 goto l220;
595 }
596 if ((IA != jbig[ir]) && (IB != jbig[ir])) goto l220;
597 }
598 }
599 long kq = IEAR-ir-IA;
600 big[ir] = 0.0;
601 long ir1 = MIN(ir, End);
602 for (i=0; i<ir1; i++) {
603 long k = kq+i;
604 if (fabs(big[ir]) < fabs(Matrix[k])) {
605 big[ir] = Matrix[k];
606 jbig[ir] = i;
607 }
608 }
609 }
610 l220: IEAR++;
611 IEBR++;
612 }
613 for (i=0; i<Dimension; i++) {
614 double T1 = EigenVectors[(i*Dimension)+IA]*CX + EigenVectors[IB+(i*Dimension)]*SX;
615 double T2 = EigenVectors[IA+(i*Dimension)]*SX - EigenVectors[IB+(i*Dimension)]*CX;
616 EigenVectors[IA+(i*Dimension)] = T1;
617 EigenVectors[IB+(i*Dimension)] = T2;
618 }
619 }
620
621 delete [] big;
622 delete [] jbig;
623 }
624
SortEigenValues(double * EigenVectors,double * EigenValues,long Dimension)625 void SortEigenValues(double * EigenVectors, double * EigenValues, long Dimension) {
626 //Sort the eigenvectors in order of accending eigenvalues
627 long i, j, jj;
628 for (i=0; i<Dimension; i++) {
629 jj = i;
630 for (j=i; j<Dimension; j++) {
631 if (EigenValues[j] < EigenValues[jj]) jj = j;
632 }
633 if (jj != i) {
634 double tempValue = EigenValues[jj];
635 EigenValues[jj] = EigenValues[i];
636 EigenValues[i] = tempValue;
637 for (j=0; j<Dimension; j++) {
638 tempValue = EigenVectors[jj+j*Dimension];
639 EigenVectors[jj+j*Dimension] = EigenVectors[i+j*Dimension];
640 EigenVectors[i+j*Dimension] = tempValue;
641 }
642 }
643 }
644 }
EPSLON(double x)645 double EPSLON(double x) {
646 /*C*
647 C* AUTHORS -
648 C* THIS ROUTINE WAS TAKEN FROM EISPACK EDITION 3 DATED 4/6/83
649 C* THIS VERSION IS BY S. T. ELBERT, AMES LABORATORY-USDOE NOV 1986
650 C*
651 C* PURPOSE -
652 C* ESTIMATE UNIT ROUNDOFF IN QUANTITIES OF SIZE X.
653 C*
654 C* ON ENTRY -
655 C* X - WORKING PRECISION REAL
656 C* VALUES TO FIND EPSLON FOR
657 C*
658 C* ON EXIT -
659 C* EPSLON - WORKING PRECISION REAL
660 C* SMALLEST POSITIVE VALUE SUCH THAT X+EPSLON .NE. ZERO
661 C*
662 C* QUALIFICATIONS -
663 C* THIS ROUTINE SHOULD PERFORM PROPERLY ON ALL SYSTEMS
664 C* SATISFYING THE FOLLOWING TWO ASSUMPTIONS,
665 C* 1. THE BASE USED IN REPRESENTING FLOATING POINT
666 C* NUMBERS IS NOT A POWER OF THREE.
667 C* 2. THE QUANTITY A IN STATEMENT 10 IS REPRESENTED TO
668 C* THE ACCURACY USED IN FLOATING POINT VARIABLES
669 C* THAT ARE STORED IN MEMORY.
670 C* THE STATEMENT NUMBER 10 AND THE GO TO 10 ARE INTENDED TO
671 C* FORCE OPTIMIZING COMPILERS TO GENERATE CODE SATISFYING
672 C* ASSUMPTION 2.
673 C* UNDER THESE ASSUMPTIONS, IT SHOULD BE TRUE THAT,
674 C* A IS NOT EXACTLY EQUAL TO FOUR-THIRDS,
675 C* B HAS A ZERO FOR ITS LAST BIT OR DIGIT,
676 C* C IS NOT EXACTLY EQUAL TO ONE,
677 C* EPS MEASURES THE SEPARATION OF 1.0 FROM
678 C* THE NEXT LARGER FLOATING POINT NUMBER.
679 C* THE DEVELOPERS OF EISPACK WOULD APPRECIATE BEING INFORMED
680 C* ABOUT ANY SYSTEMS WHERE THESE ASSUMPTIONS DO NOT HOLD.
681 C*
682 C* DIFFERENCES FROM EISPACK 3 -
683 C* USE IS MADE OF PARAMETER STATEMENTS AND INTRINSIC FUNCTIONS
684 C* --NO EXECUTEABLE CODE CHANGES--
685 C*
686 C* NOTE -
687 C* QUESTIONS AND COMMENTS CONCERNING EISPACK SHOULD BE DIRECTED TO
688 C* B. S. GARBOW, APPLIED MATH. DIVISION, ARGONNE NATIONAL LAB.
689 C
690 */
691 double A,B,C,EPS, result;
692 const double zero=0, one=1.0, three=3.0, four=4.0;
693
694 A = four/three;
695 do {
696 B = A - one;
697 C = B + B + B;
698 EPS = fabs(C - one);
699 } while (EPS == zero);
700 result = EPS*fabs(x);
701
702 return result;
703 }
704
ProjectToPlane(const CPoint3D & normal,const CPoint3D & origin,CPoint3D & pt)705 void ProjectToPlane(const CPoint3D& normal, const CPoint3D& origin, CPoint3D& pt) {
706
707 float dist;
708 CPoint3D vec;
709
710 vec = pt - origin;
711 dist = DotProduct3D(&normal, &vec);
712 pt = pt - normal * dist;
713
714 }
715
OrthoVector(const CPoint3D & base_vec,CPoint3D & ortho_vec)716 void OrthoVector(const CPoint3D& base_vec, CPoint3D& ortho_vec) {
717
718 // This function finds a vector orthogonal to another. There are
719 // infinitely many choices, but we choose one that can be calculated
720 // simply. It's assumed that base_vec is non-zero. The returned
721 // ortho_vec is normalized.
722
723 ortho_vec = CPoint3D(1.0f, 1.0f, 1.0f);
724
725 if (base_vec.x != 0.0f) {
726 ortho_vec.x = -(base_vec.y + base_vec.z) / base_vec.x;
727 }
728
729 else if (base_vec.y != 0.0f) {
730 ortho_vec.y = -(base_vec.x + base_vec.z) / base_vec.y;
731 }
732
733 else if (base_vec.z != 0.0f) {
734 ortho_vec.z = -(base_vec.x + base_vec.y) / base_vec.z;
735 }
736
737 Normalize3D(&ortho_vec);
738
739 }
740