Category : C Source Code
Archive   : GRAFGEM1.ZIP
Filename : MATRIXOR.C

/*
Matrix Orthogonalization
Eric Raible
from "Graphics Gems", Academic Press, 1990
*/

/*
* Reorthogonalize matrix R - that is find an orthogonal matrix that is
* "close" to R by computing an approximation to the orthogonal matrix
*
* T -1/2
* RC = R(R R)
* T -1
* [RC is orthogonal because (RC) = (RC) ]
* -1/2
* To compute C, we evaluate the Taylor expansion of F(x) = (I + x)
* (where x = C - I) about x=0.
* This gives C = I - (1/2)x + (3/8)x^2 - (5/16)x^3 + ...
*/

#include "GraphicsGems.h"

static float coef[10] = /* From mathematica */
{ 1, -1/2., 3/8., -5/16., 35/128., -63/256.,
231/1024., -429/2048., 6435/32768., -12155/65536. };

MATRIX_reorthogonalize (R, limit)
Matrix R;
{
Matrix I, Temp, X, X_power, Sum;
int power;

limit = MAX(limit, 10);

MATRIX_transpose (R, Temp); /* Rt */
MATRIX_multiply (Temp, R, Temp); /* RtR */
MATRIX_identify (I);
MATRIX_subtract (Temp, I, X); /* RtR - I */
MATRIX_identify (X_power); /* X^0 */
MATRIX_identify (Sum); /* coef[0] * X^0 */

for (power = 1; power < limit; ++power)
{
MATRIX_multiply (X_power, X, X_power);
MATRIX_constant_multiply (coef[power], X_power, Temp);
MATRIX_add (Sum, Temp, Sum);
}

MATRIX_multiply (R, Sum, R);
}