chol.cpp

Go to the documentation of this file.

/*
 * chol.cpp
 *
 * Code generation for function 'chol'
 *
 * C source code generated on: Wed Jul 24 16:11:35 2013
 *
 */

/* Include files */
#include "rt_nonfinite.h"
#include "Optimal_affine_tracking_3d16_fast_realtime.h"
#include "chol.h"

/* Type Definitions */

/* Named Constants */

/* Variable Declarations */

/* Variable Definitions */

/* Function Declarations */

/* Function Definitions */
void chol(real_T A[36])
{
  int32_T info;
  int32_T colj;
  int32_T j;
  boolean_T exitg1;
  int32_T jm1;
  int32_T jj;
  real_T ajj;
  int32_T ix;
  int32_T iy;
  int32_T coljp1;
  int32_T i29;
  int32_T iac;
  real_T c;
  int32_T i30;
  int32_T ia;
  info = 0;
  colj = 1;
  j = 1;
  exitg1 = FALSE;
  while ((exitg1 == 0U) && (j < 7)) {
    jm1 = j - 1;
    jj = (colj + jm1) - 1;
    ajj = 0.0;
    if (jm1 < 1) {
    } else {
      ix = colj;
      iy = colj;
      for (coljp1 = 1; coljp1 <= jm1; coljp1++) {
        ajj += A[ix - 1] * A[iy - 1];
        ix++;
        iy++;
      }
    }

    ajj = A[jj] - ajj;
    if (ajj > 0.0) {
      ajj = sqrt(ajj);
      A[jj] = ajj;
      if (j < 6) {
        jj += 7;
        coljp1 = colj + 6;
        if (jm1 == 0) {
        } else {
          iy = jj - 1;
          i29 = coljp1 + 6 * (5 - j);
          for (iac = coljp1; iac <= i29; iac += 6) {
            ix = colj;
            c = 0.0;
            i30 = iac + jm1;
            for (ia = iac; ia <= i30 - 1; ia++) {
              c += A[ia - 1] * A[ix - 1];
              ix++;
            }

            A[iy] += -c;
            iy += 6;
          }
        }

        ajj = 1.0 / ajj;
        i29 = jj + 6 * (5 - j);
        while (jj <= i29) {
          A[jj - 1] *= ajj;
          jj += 6;
        }

        colj = coljp1;
      }

      j++;
    } else {
      A[jj] = ajj;
      info = j;
      exitg1 = TRUE;
    }
  }

  if (info == 0) {
    jj = 6;
  } else {
    jj = info - 1;
  }

  for (j = 0; j + 1 <= jj; j++) {
    for (coljp1 = j + 1; coljp1 + 1 <= jj; coljp1++) {
      A[coljp1 + 6 * j] = 0.0;
    }
  }
}

/* End of code generation (chol.cpp) */