Linear algebra package

SolveLinearSystem

bool SolveLinearSystem(int n, float *m, float *r)
{
    float *rowNormalizer = new float[n];
    bool result = false;
    
    // Calculate a normalizer for each row
    for (int i = 0; i < n; i++)
    {
        const float *entry = m + i;
        float maxvalue = 0.0F;
        
        for (int j = 0; j < n; j++)
        {
            float value = fabs(*entry);
            if (value > maxvalue) maxvalue = value;
            entry += n;
        }
        
        if (maxvalue == 0.0F) goto exit; // Singular
        rowNormalizer[i] = 1.0F / maxvalue;
    }
    
    // Perform elimination one column at a time
    for (int j = 0; j < n - 1; j++)
    {
        // Find pivot element
        int pivotRow = -1;
        float maxvalue = 0.0F;
        for (int i = j; i < n; i++)
        {
            float p = fabs(m[j * n + i]) * rowNormalizer[i];
            if (p > maxvalue)
            {
                maxvalue = p;
                pivotRow = i;
            }
        }
        
        if (pivotRow != j)
        {
            if (pivotRow == -1) goto exit; // Singular
            
            // Exchange rows
            for (int k = 0; k < n; k++)
            {
                float temp = m[k * n + j];
                m[k * n + j] = m[k * n + pivotRow];
                m[k * n + pivotRow] = temp;
            }
            
            float temp = r[j];
            r[j] = r[pivotRow];
            r[pivotRow] = temp;
            
            rowNormalizer[pivotRow] = rowNormalizer[j];
        }
        
        float denom = 1.0F / m[j * n + j];
        for (int i = j + 1; i < n; i++)
        {
            float factor = m[j * n + i] * denom;
            r[i] -= r[j] * factor;
            for (int k = 0; k < n; k++) m[k * n + i] -= m[k * n + j] * factor;
        }
    }
    
    // Perform backward substitution
    for (int i = n - 1; i >= 0; i--)
    {
        float sum = r[i];
        for (int k = i + 1; k < n; k++) sum -= m[k * n + i] * r[k];
        r[i] = sum / m[i * n + i];
    }

    result = true;
    
    exit:
    delete[] rowNormalizer;
    return (result);
}

LUDecompose

bool LUDecompose(int n, float *m,
    unsigned short *index, float *detSign)
{
    float *rowNormalizer = new float[n];
    float exchangeParity = 1.0F;
    bool result = false;
    
    // Calculate a normalizer for each row
    for (int i = 0; i < n; i++)
    {
        const float *entry = m + i;
        float maxvalue = 0.0F;
        
        for (int j = 0; j < n; j++)
        {
            float value = fabs(*entry);
            if (value > maxvalue) maxvalue = value;
            entry += n;
        }
        
        if (maxvalue == 0.0F) goto exit; // Singular
        rowNormalizer[i] = 1.0F / maxvalue;
        index[i] = i;
    }
    
    // Perform decomposition
    for (int j = 0; j < n; j++)
    {
        for (int i = 1; i < j; i++)
        {
            // Evaluate Equation (14.15)
            float sum = m[j * n + i];
            for (int k = 0; k < i; k++) sum -= m[k * n + i] * m[j * n + k];
            m[j * n + i] = sum;
        }
        
        // Find pivot element
        int pivotRow = -1;
        float maxvalue = 0.0F;
        for (int i = j; i < n; i++)
        {
            // Evaluate Equation (14.17)
            float sum = m[j * n + i];
            for (int k = 0; k < j; k++) sum -= m[k * n + i] * m[j * n + k];
            m[j * n + i] = sum;
            
            sum = fabs(sum) * rowNormalizer[i];
            if (sum > maxvalue)
            {
                maxvalue = sum;
                pivotRow = i;
            }
        }
        
        if (pivotRow != j)
        {
            if (pivotRow == -1) goto exit; // Singular
            
            // Exchange rows
            for (int k = 0; k < n; k++)
            {
                float temp = m[k * n + j];
                m[k * n + j] = m[k * n + pivotRow];
                m[k * n + pivotRow] = temp;
            }
            
            unsigned short temp = index[j];
            index[j] = index[pivotRow];
            index[pivotRow] = temp;
            
            rowNormalizer[pivotRow] = rowNormalizer[j];
            exchangeParity = -exchangeParity;
        }
        
        // Divide by pivot element
        if (j != n - 1)
        {
            float denom = 1.0F / m[j * n + j];
            for (int i = j + 1; i < n; i++) m[j * n + i] *= denom;
        }
    }
    
    if (detSign) *detSign = exchangeParity;
    result = true;

    exit:
    delete[] rowNormalizer;
    return (result);
}

LUBacksubstitute

void LUBacksubstitute(int n, const float *d,
    const unsigned short *index, const float *r, float *x)
{
    for (int i = 0; i < n; i++) x[i] = r[index[i]];
    
    // Perform forward substitution for Ly = r
    for (int i = 0; i < n; i++)
    {
        float sum = x[i];
        for (int k = 0; k < i; k++) sum -= d[k * n + i] * x[k];
        x[i] = sum;
    }
    
    // Perform backward substitution for Ux = y
    for (int i = n - 1; i >= 0; i--)
    {
        float sum = x[i];
        for (int k = i + 1; k < n; k++) sum -= d[k * n + i] * x[k];
        x[i] = sum / d[i * n + i];
    }
}

LURefineSolution

void LURefineSolution(int n, const float *m,
    const float *d, const unsigned short *index,
    const float *r, float *x)
{
    float *t = new float[n];
    
    for (int i = 0; i < n; i++)
    {
        double q = -r[i];
        for (int k = 0; k < n; k++) q += m[k * n + i] * x[k];
        t[i] = (float) q;
    }
    
    LUBacksubstitute(n, d, index, t, t);
    for (int i = 0; i < n; i++) x[i] -= t[i];
    
    delete[] t;
}

SolveTridiagonalSystem

void SolveTridiagonalSystem(int n,
    const float *a, const float *b, const float *c,
    const float *r, float *x)
{
    // Allocate temporary storage for c[i]/beta[i]
    float *t = new float[n - 1];

    float recipBeta = 1.0F / b[0];
    x[0] = r[0] * recipBeta;

    for (int i = 1; i < n; i++)
    {
        t[i - 1] = c[i - 1] * recipBeta; 
        recipBeta = 1.0F / (b[i] - a[i] * t[i - 1]);
        x[i] = (r[i] - a[i] * x[i - 1]) * recipBeta;
    }

    for (int i = n - 2; i >= 0; i--) x[i] -= t[i] * x[i + 1];

    delete[] t;
}

CalculateEigensystem

const float epsilon = 1.0e-10F;
const int maxSweeps = 32;


struct Matrix3D
{
    float n[3][3];

    float& operator()(int i, int j)
    {
        return (n[j][i]);
    }

    const float& operator()(int i, int j) const
    {
        return (n[j][i]);
    }

    void SetIdentity(void)
    {
        n[0][0] = n[1][1] = n[2][2] = 1.0F;
        n[0][1] = n[0][2] = n[1][0] = n[1][2] = n[2][0] = n[2][1] = 0.0F;
    }
};


void CalculateEigensystem(const Matrix3D& m, float *lambda, Matrix3D& r)
{
    float m11 = m(0,0);
    float m12 = m(0,1);
    float m13 = m(0,2);
    float m22 = m(1,1);
    float m23 = m(1,2);
    float m33 = m(2,2);
    
    r.SetIdentity();
    for (int a = 0; a < maxSweeps; a++)
    {
        // Exit if off-diagonal entries small enough
        if ((Fabs(m12) < epsilon) && (Fabs(m13) < epsilon) &&
            (Fabs(m23) < epsilon)) break;
        
        // Annihilate (1,2) entry
        if (m12 != 0.0F)
        {
            float u = (m22 - m11) * 0.5F / m12;
            float u2 = u * u;
            float u2p1 = u2 + 1.0F;
            float t = (u2p1 != u2) ?
                ((u < 0.0F) ? -1.0F : 1.0F) * (sqrt(u2p1) - fabs(u)) : 0.5F / u;
            float c = 1.0F / sqrt(t * t + 1.0F);
            float s = c * t;
            
            m11 -= t * m12;
            m22 += t * m12;
            m12 = 0.0F;
            
            float temp = c * m13 - s * m23;
            m23 = s * m13 + c * m23;
            m13 = temp;
            
            for (int i = 0; i < 3; i++)
            {
                float temp = c * r(i,0) - s * r(i,1);
                r(i,1) = s * r(i,0) + c * r(i,1);
                r(i,0) = temp;
            }
        }
        
        // Annihilate (1,3) entry
        if (m13 != 0.0F)
        {
            float u = (m33 - m11) * 0.5F / m13;
            float u2 = u * u;
            float u2p1 = u2 + 1.0F;
            float t = (u2p1 != u2) ?
                ((u < 0.0F) ? -1.0F : 1.0F) * (sqrt(u2p1) - fabs(u)) : 0.5F / u;
            float c = 1.0F / sqrt(t * t + 1.0F);
            float s = c * t;
            
            m11 -= t * m13;
            m33 += t * m13;
            m13 = 0.0F;
            
            float temp = c * m12 - s * m23;
            m23 = s * m12 + c * m23;
            m12 = temp;
            
            for (int i = 0; i < 3; i++)
            {
                float temp = c * r(i,0) - s * r(i,2);
                r(i,2) = s * r(i,0) + c * r(i,2);
                r(i,0) = temp;
            }
        }
        
        // Annihilate (2,3) entry
        if (m23 != 0.0F)
        {
            float u = (m33 - m22) * 0.5F / m23;
            float u2 = u * u;
            float u2p1 = u2 + 1.0F;
            float t = (u2p1 != u2) ?
                ((u < 0.0F) ? -1.0F : 1.0F) * (sqrt(u2p1) - fabs(u)) : 0.5F / u;
            float c = 1.0F / sqrt(t * t + 1.0F);
            float s = c * t;
            
            m22 -= t * m23;
            m33 += t * m23;
            m23 = 0.0F;
            
            float temp = c * m12 - s * m13;
            m13 = s * m12 + c * m13;
            m12 = temp;
            
            for (int i = 0; i < 3; i++)
            {
                float temp = c * r(i,1) - s * r(i,2);
                r(i,2) = s * r(i,1) + c * r(i,2);
                r(i,1) = temp;
            }
        }
    }
    
    lambda[0] = m11;
    lambda[1] = m22;
    lambda[2] = m33;
}