LU_HELPER.cpp

Go to the documentation of this file.

#include "LU_HELPER.h"

int isNonsingular (sLU LU_) {
      for (int j = 0; j < 3; j++) {
         if (LU_.values[j][j] == 0)
            return 0;
      }
      return 1;
}

sLU computeLU( float a[3][3])
{
    sLU result;
    int m=3;
    int n=3;

    //float LU_[3][3];
    for (int i = 0; i < m; i++) {
            result.piv[i] = i;
            for (int j = 0; j < n; j++) result.values[i][j]=a[i][j];
      }

      result.pivsign = 1;
      //Real *LUrowi = 0;;
      //Array1D<Real> LUcolj(m);
      //float *LUrowi = 0;
      float LUcolj[3];

      // Outer loop.

      for (int j = 0; j < n; j++) {

         // Make a copy of the j-th column to localize references.

         for (int i = 0; i < m; i++) {
            LUcolj[i] = result.values[i][j];
         }

         // Apply previous transformations.

         for (int i = 0; i < m; i++) {
             //float LUrowi[3];
             //LUrowi = result.values[i];

            // Most of the time is spent in the following dot product.

            int kmax = min(i,j);
            double s = 0.0;
            for (int k = 0; k < kmax; k++) {
               s += result.values[i][k]*LUcolj[k];
            }

            result.values[i][j] = LUcolj[i] -= s;
         }
   
         // Find pivot and exchange if necessary.

         int p = j;
         for (int i = j+1; i < m; i++) {
            if (abs(LUcolj[i]) > abs(LUcolj[p])) {
               p = i;
            }
         }
         if (p != j) {
            int k=0;
            for (k = 0; k < n; k++) {
               double t = result.values[p][k]; 
               result.values[p][k] = result.values[j][k]; 
               result.values[j][k] = t;
            }
            k = result.piv[p]; 
            result.piv[p] = result.piv[j]; 
            result.piv[j] = k;
            result.pivsign = -result.pivsign;
         }

         // Compute multipliers.
         
         if ((j < m) && (result.values[j][j] != 0.0)) {
            for (int i = j+1; i < m; i++) {
               result.values[i][j] /= result.values[j][j];
            }
         }
      }

      return result;
}

void solveLU3x3(sLU& A, float x[3], float b[3])
{
  //TNT::Array1D<float> jamaB = TNT::Array1D<float>(3, &b[0]);
  //TNT::Array1D<float> jamaX = A.solve(jamaB);

    
  // Solve A, B

    {
      if (!isNonsingular(A)) {
        x[0]=0.0f;
        x[1]=0.0f;
        x[2]=0.0f;
        return;
      }


      //Array1D<Real> Ax = permute_copy(b, piv);
      float Ax[3];

    // permute copy: b , A.piv
    {
         for (int i = 0; i < 3; i++) 
               Ax[i] = b[A.piv[i]];
    }

      // Solve L*Y = B(piv)
      for (int k = 0; k < 3; k++) {
         for (int i = k+1; i < 3; i++) {
               Ax[i] -= Ax[k]*A.values[i][k];
            }
         }
      
      // Solve U*X = Y;
      for (int k = 2; k >= 0; k--) {
            Ax[k] /= A.values[k][k];
            for (int i = 0; i < k; i++) 
                Ax[i] -= Ax[k]*A.values[i][k];
      }
     

        x[0] = Ax[0];
        x[1] = Ax[1];
        x[2] = Ax[2];
      return;
    }
}