mrdivide.cpp

Go to the documentation of this file.

/*
 * mrdivide.cpp
 *
 * Code generation for function 'mrdivide'
 *
 * 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 "mrdivide.h"
#include "mldivide.h"
#include "expm.h"
#include "Optimal_affine_tracking_3d16_fast_realtime_rtwutil.h"

/* Type Definitions */

/* Named Constants */

/* Variable Declarations */

/* Variable Definitions */

/* Function Declarations */
static real_T b_eml_xnrm2(int32_T n, const real_T x[12], int32_T ix0);
static real_T eml_xnrm2(int32_T n, const real_T x[12], int32_T ix0);

/* Function Definitions */
static real_T b_eml_xnrm2(int32_T n, const real_T x[12], int32_T ix0)
{
  real_T y;
  real_T scale;
  int32_T kend;
  int32_T k;
  real_T absxk;
  real_T t;
  y = 0.0;
  if (n == 1) {
    y = fabs(x[ix0 - 1]);
  } else {
    scale = 2.2250738585072014E-308;
    kend = (ix0 + n) - 1;
    for (k = ix0; k <= kend; k++) {
      absxk = fabs(x[k - 1]);
      if (absxk > scale) {
        t = scale / absxk;
        y = 1.0 + y * t * t;
        scale = absxk;
      } else {
        t = absxk / scale;
        y += t * t;
      }
    }

    y = scale * sqrt(y);
  }

  return y;
}

static real_T eml_xnrm2(int32_T n, const real_T x[12], int32_T ix0)
{
  real_T y;
  real_T scale;
  int32_T kend;
  int32_T k;
  real_T absxk;
  real_T t;
  y = 0.0;
  if (n == 1) {
    y = fabs(x[ix0 - 1]);
  } else {
    scale = 2.2250738585072014E-308;
    kend = (ix0 + n) - 1;
    for (k = ix0; k <= kend; k++) {
      absxk = fabs(x[k - 1]);
      if (absxk > scale) {
        t = scale / absxk;
        y = 1.0 + y * t * t;
        scale = absxk;
      } else {
        t = absxk / scale;
        y += t * t;
      }
    }

    y = scale * sqrt(y);
  }

  return y;
}

void b_mrdivide(const creal_T A[4], const creal_T B[4], creal_T y[4])
{
  creal_T b_A[4];
  creal_T b_B[4];
  int32_T r1;
  int32_T r2;
  creal_T a21;
  creal_T a22;
  creal_T Y[4];
  int32_T k;
  creal_T c_B;
  for (r1 = 0; r1 < 2; r1++) {
    for (r2 = 0; r2 < 2; r2++) {
      b_A[r2 + (r1 << 1)].re = B[r1 + (r2 << 1)].re;
      b_A[r2 + (r1 << 1)].im = -B[r1 + (r2 << 1)].im;
      b_B[r2 + (r1 << 1)].re = A[r1 + (r2 << 1)].re;
      b_B[r2 + (r1 << 1)].im = -A[r1 + (r2 << 1)].im;
    }
  }

  if (fabs(b_A[1].re) + fabs(b_A[1].im) > fabs(b_A[0].re) + fabs(b_A[0].im)) {
    r1 = 1;
    r2 = 0;
  } else {
    r1 = 0;
    r2 = 1;
  }

  a21 = c_eml_div(b_A[r2], b_A[r1]);
  a22.re = b_A[2 + r2].re - (a21.re * b_A[2 + r1].re - a21.im * b_A[2 + r1].im);
  a22.im = b_A[2 + r2].im - (a21.re * b_A[2 + r1].im + a21.im * b_A[2 + r1].re);
  for (k = 0; k < 2; k++) {
    c_B.re = b_B[r2 + (k << 1)].re - (b_B[r1 + (k << 1)].re * a21.re - b_B[r1 +
                                      (k << 1)].im * a21.im);
    c_B.im = b_B[r2 + (k << 1)].im - (b_B[r1 + (k << 1)].re * a21.im + b_B[r1 +
                                      (k << 1)].im * a21.re);
    Y[1 + (k << 1)] = c_eml_div(c_B, a22);
    c_B.re = b_B[r1 + (k << 1)].re - (Y[1 + (k << 1)].re * b_A[2 + r1].re - Y[1
      + (k << 1)].im * b_A[2 + r1].im);
    c_B.im = b_B[r1 + (k << 1)].im - (Y[1 + (k << 1)].re * b_A[2 + r1].im + Y[1
      + (k << 1)].im * b_A[2 + r1].re);
    Y[k << 1] = c_eml_div(c_B, b_A[r1]);
  }

  for (r1 = 0; r1 < 2; r1++) {
    for (r2 = 0; r2 < 2; r2++) {
      y[r2 + (r1 << 1)].re = Y[r1 + (r2 << 1)].re;
      y[r2 + (r1 << 1)].im = -Y[r1 + (r2 << 1)].im;
    }
  }
}

void mrdivide(const real_T A[12], const real_T B[12], real_T y[9])
{
  real_T tau[3];
  real_T b_A[12];
  real_T b_B[12];
  int8_T jpvt[3];
  real_T work[3];
  int32_T i4;
  int32_T nmip1;
  real_T vn1[3];
  real_T vn2[3];
  int32_T k;
  int32_T j;
  real_T b_y;
  real_T wj;
  real_T rankR;
  real_T t;
  int32_T i;
  int32_T i_i;
  int32_T pvt;
  int32_T ix;
  int32_T iy;
  int32_T i_ip1;
  int32_T lastv;
  boolean_T exitg2;
  int32_T exitg1;
  real_T Y[9];
  for (i4 = 0; i4 < 3; i4++) {
    for (nmip1 = 0; nmip1 < 4; nmip1++) {
      b_A[nmip1 + (i4 << 2)] = B[i4 + 3 * nmip1];
      b_B[nmip1 + (i4 << 2)] = A[i4 + 3 * nmip1];
    }

    jpvt[i4] = (int8_T)(1 + i4);
    work[i4] = 0.0;
  }

  k = 1;
  for (j = 0; j < 3; j++) {
    b_y = 0.0;
    wj = 2.2250738585072014E-308;
    for (nmip1 = k; nmip1 <= k + 3; nmip1++) {
      rankR = fabs(b_A[nmip1 - 1]);
      if (rankR > wj) {
        t = wj / rankR;
        b_y = 1.0 + b_y * t * t;
        wj = rankR;
      } else {
        t = rankR / wj;
        b_y += t * t;
      }
    }

    b_y = wj * sqrt(b_y);
    vn1[j] = b_y;
    vn2[j] = vn1[j];
    k += 4;
  }

  for (i = 0; i < 3; i++) {
    i_i = i + (i << 2);
    nmip1 = 3 - i;
    pvt = 0;
    if (nmip1 > 1) {
      ix = i;
      wj = fabs(vn1[i]);
      for (k = 2; k <= nmip1; k++) {
        ix++;
        rankR = fabs(vn1[ix]);
        if (rankR > wj) {
          pvt = k - 1;
          wj = rankR;
        }
      }
    }

    pvt += i;
    if (pvt + 1 != i + 1) {
      ix = pvt << 2;
      iy = i << 2;
      for (k = 0; k < 4; k++) {
        wj = b_A[ix];
        b_A[ix] = b_A[iy];
        b_A[iy] = wj;
        ix++;
        iy++;
      }

      nmip1 = jpvt[pvt];
      jpvt[pvt] = jpvt[i];
      jpvt[i] = (int8_T)nmip1;
      vn1[pvt] = vn1[i];
      vn2[pvt] = vn2[i];
    }

    nmip1 = i_i + 2;
    t = b_A[i_i];
    rankR = 0.0;
    wj = eml_xnrm2(3 - i, b_A, nmip1);
    if (wj != 0.0) {
      wj = rt_hypotd_snf(fabs(b_A[i_i]), fabs(wj));
      if (b_A[i_i] >= 0.0) {
        wj = -wj;
      }

      if (fabs(wj) < 1.0020841800044864E-292) {
        pvt = 0;
        do {
          pvt++;
          i4 = (nmip1 - i) + 2;
          for (k = nmip1; k <= i4; k++) {
            b_A[k - 1] *= 9.9792015476736E+291;
          }

          wj *= 9.9792015476736E+291;
          t *= 9.9792015476736E+291;
        } while (!(fabs(wj) >= 1.0020841800044864E-292));

        wj = rt_hypotd_snf(fabs(t), fabs(eml_xnrm2(3 - i, b_A, nmip1)));
        if (t >= 0.0) {
          wj = -wj;
        }

        rankR = (wj - t) / wj;
        t = 1.0 / (t - wj);
        i4 = (nmip1 - i) + 2;
        while (nmip1 <= i4) {
          b_A[nmip1 - 1] *= t;
          nmip1++;
        }

        for (k = 1; k <= pvt; k++) {
          wj *= 1.0020841800044864E-292;
        }

        t = wj;
      } else {
        rankR = (wj - b_A[i_i]) / wj;
        t = 1.0 / (b_A[i_i] - wj);
        i4 = (nmip1 - i) + 2;
        while (nmip1 <= i4) {
          b_A[nmip1 - 1] *= t;
          nmip1++;
        }

        t = wj;
      }
    }

    tau[i] = rankR;
    b_A[i_i] = t;
    if (i + 1 < 3) {
      t = b_A[i_i];
      b_A[i_i] = 1.0;
      i_ip1 = (i + ((i + 1) << 2)) + 1;
      if (tau[i] != 0.0) {
        lastv = 3 - i;
        nmip1 = (i_i - i) + 3;
        while ((lastv + 1 > 0) && (b_A[nmip1] == 0.0)) {
          lastv--;
          nmip1--;
        }

        k = 2 - i;
        exitg2 = FALSE;
        while ((exitg2 == 0U) && (k > 0)) {
          nmip1 = i_ip1 + ((k - 1) << 2);
          j = nmip1;
          do {
            exitg1 = 0;
            if (j <= nmip1 + lastv) {
              if (b_A[j - 1] != 0.0) {
                exitg1 = 1;
              } else {
                j++;
              }
            } else {
              k--;
              exitg1 = 2;
            }
          } while (exitg1 == 0U);

          if (exitg1 == 1U) {
            exitg2 = TRUE;
          }
        }
      } else {
        lastv = -1;
        k = 0;
      }

      if (lastv + 1 > 0) {
        if (k == 0) {
        } else {
          nmip1 = k - 1;
          for (iy = 1; iy <= nmip1 + 1; iy++) {
            work[iy - 1] = 0.0;
          }

          iy = 0;
          i4 = i_ip1 + (nmip1 << 2);
          for (pvt = i_ip1; pvt <= i4; pvt += 4) {
            ix = i_i;
            wj = 0.0;
            nmip1 = (pvt + lastv) + 1;
            for (j = pvt; j <= nmip1 - 1; j++) {
              wj += b_A[j - 1] * b_A[ix];
              ix++;
            }

            work[iy] += wj;
            iy++;
          }
        }

        if (-tau[i] == 0.0) {
        } else {
          nmip1 = 0;
          for (j = 1; j <= k; j++) {
            if (work[nmip1] != 0.0) {
              wj = work[nmip1] * -tau[i];
              ix = i_i;
              i4 = lastv + i_ip1;
              for (pvt = i_ip1; pvt <= i4; pvt++) {
                b_A[pvt - 1] += b_A[ix] * wj;
                ix++;
              }
            }

            nmip1++;
            i_ip1 += 4;
          }
        }
      }

      b_A[i_i] = t;
    }

    for (j = i + 1; j + 1 < 4; j++) {
      if (vn1[j] != 0.0) {
        rankR = fabs(b_A[i + (j << 2)]) / vn1[j];
        b_y = rankR * rankR;
        rankR = 1.0 - rankR * rankR;
        if (1.0 - b_y < 0.0) {
          rankR = 0.0;
        }

        wj = vn1[j] / vn2[j];
        if (rankR * (wj * wj) <= 1.4901161193847656E-8) {
          vn1[j] = b_eml_xnrm2(3 - i, b_A, (i + (j << 2)) + 2);
          vn2[j] = vn1[j];
        } else {
          vn1[j] *= sqrt(rankR);
        }
      }
    }
  }

  rankR = 0.0;
  k = 0;
  while ((k < 3) && (!(fabs(b_A[k + (k << 2)]) <= 4.0 * fabs(b_A[0]) *
                       2.2204460492503131E-16))) {
    rankR++;
    k++;
  }

  memset(&Y[0], 0, 9U * sizeof(real_T));
  for (j = 0; j < 3; j++) {
    if (tau[j] != 0.0) {
      for (k = 0; k < 3; k++) {
        wj = b_B[j + (k << 2)];
        for (i = 0; i <= 2 - j; i++) {
          nmip1 = (j + i) + 1;
          wj += b_A[nmip1 + (j << 2)] * b_B[nmip1 + (k << 2)];
        }

        wj *= tau[j];
        if (wj != 0.0) {
          b_B[j + (k << 2)] -= wj;
          for (i = 0; i <= 2 - j; i++) {
            nmip1 = (j + i) + 1;
            b_B[nmip1 + (k << 2)] -= b_A[nmip1 + (j << 2)] * wj;
          }
        }
      }
    }
  }

  for (k = 0; k < 3; k++) {
    for (i = 0; i <= (int32_T)rankR - 1; i++) {
      Y[(jpvt[i] + 3 * k) - 1] = b_B[i + (k << 2)];
    }

    for (j = 0; j <= (int32_T)-(1.0 + (-1.0 - rankR)) - 1; j++) {
      wj = rankR + -(real_T)j;
      Y[(jpvt[(int32_T)wj - 1] + 3 * k) - 1] /= b_A[((int32_T)wj + (((int32_T)wj
        - 1) << 2)) - 1];
      for (i = 0; i <= (int32_T)wj - 2; i++) {
        Y[(jpvt[i] + 3 * k) - 1] -= Y[(jpvt[(int32_T)wj - 1] + 3 * k) - 1] *
          b_A[i + (((int32_T)wj - 1) << 2)];
      }
    }
  }

  for (i4 = 0; i4 < 3; i4++) {
    for (nmip1 = 0; nmip1 < 3; nmip1++) {
      y[nmip1 + 3 * i4] = Y[i4 + 3 * nmip1];
    }
  }
}

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