33 #define CORRUPTMATRIX 0.001F // column vector modulus limit for rotation matrix
41 for (i = 0; i < 3; i++)
45 for (j = 0; j < 3; j++)
62 for (i = 0; i < rc; i++)
66 for (j = 0; j < rc; j++)
81 for (i = 0; i < 3; i++)
85 for (j = 0; j < 3; j++)
99 for (i = 0; i < 3; i++)
103 for (j = 0; j < 3; j++)
118 for (i = 0; i < 3; i++)
122 for (j = 0; j < 3; j++)
136 float fB11B22mB12B12;
137 float fB12B02mB01B22;
138 float fB01B12mB11B02;
142 fB11B22mB12B12 = B[1][1] * B[2][2] - B[1][2] * B[1][2];
143 fB12B02mB01B22 = B[1][2] * B[0][2] - B[0][1] * B[2][2];
144 fB01B12mB11B02 = B[0][1] * B[1][2] - B[1][1] * B[0][2];
147 ftmp = B[0][0] * fB11B22mB12B12 + B[0][1] * fB12B02mB01B22 + B[0][2] * fB01B12mB11B02;
153 A[0][0] = fB11B22mB12B12 * ftmp;
154 A[1][0] = A[0][1] = fB12B02mB01B22 * ftmp;
155 A[2][0] = A[0][2] = fB01B12mB11B02 * ftmp;
156 A[1][1] = (B[0][0] * B[2][2] - B[0][2] * B[0][2]) * ftmp;
157 A[2][1] = A[1][2] = (B[0][2] * B[0][1] - B[0][0] * B[1][2]) * ftmp;
158 A[2][2] = (B[0][0] * B[1][1] - B[0][1] * B[0][1]) * ftmp;
171 return (A[
X][
X] * (A[
Y][
Y] * A[
Z][
Z] - A[
Y][
Z] * A[
Z][
Y]) +
172 A[
X][Y] * (A[Y][
Z] * A[
Z][
X] - A[Y][
X] * A[
Z][
Z]) +
173 A[
X][Z] * (A[Y][
X] * A[Z][Y] - A[Y][Y] * A[Z][
X]));
185 #define NITERATIONS 15
188 float cot2phi, tanhalfphi, tanphi, sinphi, cosphi;
201 for (ir = 0; ir < n; ir++)
204 for (ic = 0; ic < n; ic++)
207 eigvec[ir][ic] = 0.0F;
211 eigvec[ir][ir] = 1.0F;
214 eigval[ir] = A[ir][ir];
224 for (ir = 0; ir < n - 1; ir++)
227 for (ic = ir + 1; ic < n; ic++)
230 residue += fabs(A[ir][ic]);
238 for (ir = 0; ir < n - 1; ir++)
241 for (ic = ir + 1; ic < n; ic++)
244 if (fabs(A[ir][ic]) > 0.0F)
247 cot2phi = 0.5F * (eigval[ic] - eigval[ir]) / (A[ir][ic]);
250 tanphi = 1.0F / (fabs(cot2phi) + sqrtf(1.0F + cot2phi * cot2phi));
257 cosphi = 1.0F / sqrtf(1.0F + tanphi * tanphi);
258 sinphi = tanphi * cosphi;
261 tanhalfphi = sinphi / (1.0F + cosphi);
264 ftmp = tanphi * A[ir][ic];
276 for (j = 0; j < n; j++)
279 ftmp = eigvec[j][ir];
281 eigvec[j][ir] = ftmp - sinphi * (eigvec[j][ic] + tanhalfphi * ftmp);
283 eigvec[j][ic] = eigvec[j][ic] + sinphi * (ftmp - tanhalfphi * eigvec[j][ic]);
287 for (j = 0; j <= ir - 1; j++)
292 A[j][ir] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp);
294 A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]);
296 for (j = ir + 1; j <= ic - 1; j++)
301 A[ir][j] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp);
303 A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]);
305 for (j = ic + 1; j < n; j++)
310 A[ir][j] = ftmp - sinphi * (A[ic][j] + tanhalfphi * ftmp);
312 A[ic][j] = A[ic][j] + sinphi * (ftmp - tanhalfphi * A[ic][j]);
318 }
while ((residue > 0.0F) && (ctr++ <
NITERATIONS));
332 int8 iPivotRow, iPivotCol;
335 iPivotRow = iPivotCol = 0;
338 for (j = 0; j < isize; j++)
344 for (i = 0; i < isize; i++)
349 for (j = 0; j < isize; j++)
355 for (k = 0; k < isize; k++)
361 if (fabs(A[j][k]) >= largest)
366 largest = (float) fabs(A[iPivotRow][iPivotCol]);
369 else if (iPivot[k] > 1)
382 if (iPivotRow != iPivotCol)
385 for (l = 0; l < isize; l++)
388 ftmp = A[iPivotRow][l];
389 A[iPivotRow][l] = A[iPivotCol][l];
390 A[iPivotCol][l] = ftmp;
395 iRowInd[i] = iPivotRow;
396 iColInd[i] = iPivotCol;
399 if (A[iPivotCol][iPivotCol] == 0.0F)
407 recippiv = 1.0F / A[iPivotCol][iPivotCol];
409 A[iPivotCol][iPivotCol] = 1.0F;
412 for (l = 0; l < isize; l++)
414 A[iPivotCol][l] *= recippiv;
417 for (m = 0; m < isize; m++)
422 scaling = A[m][iPivotCol];
424 A[m][iPivotCol] = 0.0F;
426 for (l = 0; l < isize; l++)
428 A[m][l] -= A[iPivotCol][l] * scaling;
435 for (l = isize - 1; l >= 0; l--)
445 for (k = 0; k < isize; k++)
463 ftmp = sqrtf(A[
X][
X] * A[
X][
X] + A[
Y][
X] * A[
Y][
X] + A[
Z][
X] * A[
Z][
X]);
476 A[
Y][
X] = A[
Z][
X] = 0.0F;
480 ftmp = A[
X][
X] * A[
X][
Y] + A[
Y][
X] * A[
Y][
Y] + A[
Z][
X] * A[
Z][
Y];
481 A[
X][
Y] -= ftmp * A[
X][
X];
482 A[
Y][
Y] -= ftmp * A[
Y][
X];
483 A[
Z][
Y] -= ftmp * A[
Z][
X];
486 ftmp = sqrtf(A[X][
Y] * A[X][
Y] + A[
Y][
Y] * A[
Y][
Y] + A[
Z][
Y] * A[
Z][
Y]);
499 A[
X][
Y] = A[
Z][
Y] = 0.0F;
503 A[
X][
Z] = A[
Y][
X] * A[
Z][
Y] - A[
Z][
X] * A[
Y][
Y];
504 A[
Y][
Z] = A[
Z][
X] * A[
X][
Y] - A[
X][
X] * A[
Z][
Y];
505 A[
Z][
Z] = A[
X][
X] * A[
Y][
Y] - A[
Y][
X] * A[
X][
Y];
void eigencompute(float A[][10], float eigval[], float eigvec[][10], int8 n)
void f3x3matrixAeqMinusA(float A[][3])
void f3x3matrixAeqAxScalar(float A[][3], float Scalar)
float f3x3matrixDetA(float A[][3])
void fmatrixAeqI(float *A[], int16 rc)
void f3x3matrixAeqInvSymB(float A[][3], float B[][3])
short int16
This defines int16 as short.
void f3x3matrixAeqI(float A[][3])
void f3x3matrixAeqScalar(float A[][3], float Scalar)
void fmatrixAeqRenormRotA(float A[][3])
void fmatrixAeqInvA(float *A[], int8 iColInd[], int8 iRowInd[], int8 iPivot[], int8 isize)