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++)
61 for (i = 0; i < 3; i++)
66 for (j = 0; j < 3; j++)
68 *(pAij++) = *(pBij++);
82 for (i = 0; i < rc; i++)
86 for (j = 0; j < rc; j++)
101 for (i = 0; i < 3; i++)
105 for (j = 0; j < 3; j++)
119 for (i = 0; i < 3; i++)
123 for (j = 0; j < 3; j++)
138 for (i = 0; i < 3; i++)
142 for (j = 0; j < 3; j++)
156 float fB11B22mB12B12;
157 float fB12B02mB01B22;
158 float fB01B12mB11B02;
162 fB11B22mB12B12 = B[1][1] * B[2][2] - B[1][2] * B[1][2];
163 fB12B02mB01B22 = B[1][2] * B[0][2] - B[0][1] * B[2][2];
164 fB01B12mB11B02 = B[0][1] * B[1][2] - B[1][1] * B[0][2];
167 ftmp = B[0][0] * fB11B22mB12B12 + B[0][1] * fB12B02mB01B22 + B[0][2] * fB01B12mB11B02;
173 A[0][0] = fB11B22mB12B12 * ftmp;
174 A[1][0] = A[0][1] = fB12B02mB01B22 * ftmp;
175 A[2][0] = A[0][2] = fB01B12mB11B02 * ftmp;
176 A[1][1] = (B[0][0] * B[2][2] - B[0][2] * B[0][2]) * ftmp;
177 A[2][1] = A[1][2] = (B[0][2] * B[0][1] - B[0][0] * B[1][2]) * ftmp;
178 A[2][2] = (B[0][0] * B[1][1] - B[0][1] * B[0][1]) * ftmp;
191 return (A[
X][
X] * (A[
Y][
Y] * A[
Z][
Z] - A[
Y][
Z] * A[
Z][
Y]) +
192 A[
X][Y] * (A[Y][
Z] * A[
Z][
X] - A[Y][
X] * A[
Z][
Z]) +
193 A[
X][Z] * (A[Y][
X] * A[Z][Y] - A[Y][Y] * A[Z][
X]));
206 #define NITERATIONS 15
209 float cot2phi, tanhalfphi, tanphi, sinphi, cosphi;
222 for (ir = 0; ir < n; ir++)
225 for (ic = 0; ic < n; ic++)
228 eigvec[ir][ic] = 0.0F;
232 eigvec[ir][ir] = 1.0F;
235 eigval[ir] = A[ir][ir];
245 for (ir = 0; ir < n - 1; ir++)
248 for (ic = ir + 1; ic < n; ic++)
251 residue += fabsf(A[ir][ic]);
259 for (ir = 0; ir < n - 1; ir++)
262 for (ic = ir + 1; ic < n; ic++)
265 if (fabsf(A[ir][ic]) > 0.0F)
268 cot2phi = 0.5F * (eigval[ic] - eigval[ir]) / (A[ir][ic]);
271 tanphi = 1.0F / (fabsf(cot2phi) + sqrtf(1.0F + cot2phi * cot2phi));
278 cosphi = 1.0F / sqrtf(1.0F + tanphi * tanphi);
279 sinphi = tanphi * cosphi;
282 tanhalfphi = sinphi / (1.0F + cosphi);
285 ftmp = tanphi * A[ir][ic];
297 for (j = 0; j < n; j++)
300 ftmp = eigvec[j][ir];
302 eigvec[j][ir] = ftmp - sinphi * (eigvec[j][ic] + tanhalfphi * ftmp);
304 eigvec[j][ic] = eigvec[j][ic] + sinphi * (ftmp - tanhalfphi * eigvec[j][ic]);
308 for (j = 0; j <= ir - 1; j++)
313 A[j][ir] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp);
315 A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]);
317 for (j = ir + 1; j <= ic - 1; j++)
322 A[ir][j] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp);
324 A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]);
326 for (j = ic + 1; j < n; j++)
331 A[ir][j] = ftmp - sinphi * (A[ic][j] + tanhalfphi * ftmp);
333 A[ic][j] = A[ic][j] + sinphi * (ftmp - tanhalfphi * A[ic][j]);
339 }
while ((residue > 0.0F) && (ctr++ <
NITERATIONS));
356 #define NITERATIONS 15
359 float cot2phi, tanhalfphi, tanphi, sinphi, cosphi;
372 for (ir = 0; ir < n; ir++)
375 for (ic = 0; ic < n; ic++)
378 eigvec[ir][ic] = 0.0F;
382 eigvec[ir][ir] = 1.0F;
385 eigval[ir] = A[ir][ir];
395 for (ir = 0; ir < n - 1; ir++)
398 for (ic = ir + 1; ic < n; ic++)
401 residue += fabsf(A[ir][ic]);
409 for (ir = 0; ir < n - 1; ir++)
412 for (ic = ir + 1; ic < n; ic++)
415 if (fabsf(A[ir][ic]) > 0.0F)
418 cot2phi = 0.5F * (eigval[ic] - eigval[ir]) / (A[ir][ic]);
421 tanphi = 1.0F / (fabsf(cot2phi) + sqrtf(1.0F + cot2phi * cot2phi));
428 cosphi = 1.0F / sqrtf(1.0F + tanphi * tanphi);
429 sinphi = tanphi * cosphi;
432 tanhalfphi = sinphi / (1.0F + cosphi);
435 ftmp = tanphi * A[ir][ic];
447 for (j = 0; j < n; j++)
450 ftmp = eigvec[j][ir];
452 eigvec[j][ir] = ftmp - sinphi * (eigvec[j][ic] + tanhalfphi * ftmp);
454 eigvec[j][ic] = eigvec[j][ic] + sinphi * (ftmp - tanhalfphi * eigvec[j][ic]);
458 for (j = 0; j <= ir - 1; j++)
463 A[j][ir] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp);
465 A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]);
467 for (j = ir + 1; j <= ic - 1; j++)
472 A[ir][j] = ftmp - sinphi * (A[j][ic] + tanhalfphi * ftmp);
474 A[j][ic] = A[j][ic] + sinphi * (ftmp - tanhalfphi * A[j][ic]);
476 for (j = ic + 1; j < n; j++)
481 A[ir][j] = ftmp - sinphi * (A[ic][j] + tanhalfphi * ftmp);
483 A[ic][j] = A[ic][j] + sinphi * (ftmp - tanhalfphi * A[ic][j]);
489 }
while ((residue > 0.0F) && (ctr++ <
NITERATIONS));
503 int8 iPivotRow, iPivotCol;
506 iPivotRow = iPivotCol = 0;
512 for (j = 0; j < isize; j++)
518 for (i = 0; i < isize; i++)
523 for (j = 0; j < isize; j++)
529 for (k = 0; k < isize; k++)
535 if (fabsf(A[j][k]) >= largest)
540 largest = (float) fabsf(A[iPivotRow][iPivotCol]);
543 else if (iPivot[k] > 1)
557 if (iPivotRow != iPivotCol)
560 for (l = 0; l < isize; l++)
563 ftmp = A[iPivotRow][l];
564 A[iPivotRow][l] = A[iPivotCol][l];
565 A[iPivotCol][l] = ftmp;
570 iRowInd[i] = iPivotRow;
571 iColInd[i] = iPivotCol;
574 if (A[iPivotCol][iPivotCol] == 0.0F)
583 recippiv = 1.0F / A[iPivotCol][iPivotCol];
585 A[iPivotCol][iPivotCol] = 1.0F;
588 for (l = 0; l < isize; l++)
590 if (A[iPivotCol][l] != 0.0F)
591 A[iPivotCol][l] *= recippiv;
594 for (m = 0; m < isize; m++)
599 scaling = A[m][iPivotCol];
601 A[m][iPivotCol] = 0.0F;
603 for (l = 0; l < isize; l++)
605 if ((A[iPivotCol][l] != 0.0F) && (scaling != 0.0F))
606 A[m][l] -= A[iPivotCol][l] * scaling;
613 for (l = isize - 1; l >= 0; l--)
623 for (k = 0; k < isize; k++)
641 ftmp = sqrtf(A[
X][
X] * A[
X][
X] + A[
Y][
X] * A[
Y][
X] + A[
Z][
X] * A[
Z][
X]);
654 A[
Y][
X] = A[
Z][
X] = 0.0F;
658 ftmp = A[
X][
X] * A[
X][
Y] + A[
Y][
X] * A[
Y][
Y] + A[
Z][
X] * A[
Z][
Y];
659 A[
X][
Y] -= ftmp * A[
X][
X];
660 A[
Y][
Y] -= ftmp * A[
Y][
X];
661 A[
Z][
Y] -= ftmp * A[
Z][
X];
664 ftmp = sqrtf(A[X][
Y] * A[X][
Y] + A[
Y][
Y] * A[
Y][
Y] + A[
Z][
Y] * A[
Z][
Y]);
677 A[
X][
Y] = A[
Z][
Y] = 0.0F;
681 A[
X][
Z] = A[
Y][
X] * A[
Z][
Y] - A[
Z][
X] * A[
Y][
Y];
682 A[
Y][
Z] = A[
Z][
X] * A[
X][
Y] - A[
X][
X] * A[
Z][
Y];
683 A[
Z][
Z] = A[
X][
X] * A[
Y][
Y] - A[
Y][
X] * A[
X][
Y];
void fmatrixAeqInvA(float *A[], int8 iColInd[], int8 iRowInd[], int8 iPivot[], int8 isize, int8 *pierror)
void f3x3matrixAeqB(float A[][3], float B[][3])
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])
void f3x3matrixAeqI(float A[][3])
void eigencompute4(float A[][4], float eigval[], float eigvec[][4], int8 n)
void f3x3matrixAeqScalar(float A[][3], float Scalar)
void fmatrixAeqRenormRotA(float A[][3])
void eigencompute10(float A[][10], float eigval[], float eigvec[][10], int8 n)