mldivide.cpp

Go to the documentation of this file.

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

/* Type Definitions */

/* Named Constants */

/* Variable Declarations */

/* Variable Definitions */

/* Function Declarations */

/* Function Definitions */
void b_mldivide(const real_T A[9], const real_T B[9], real_T Y[9])
{
  real_T b_A[9];
  int32_T r1;
  int32_T r2;
  int32_T r3;
  real_T maxval;
  real_T a21;
  int32_T rtemp;
  memcpy(&b_A[0], &A[0], 9U * sizeof(real_T));
  r1 = 0;
  r2 = 1;
  r3 = 2;
  maxval = fabs(A[0]);
  a21 = fabs(A[1]);
  if (a21 > maxval) {
    maxval = a21;
    r1 = 1;
    r2 = 0;
  }

  if (fabs(A[2]) > maxval) {
    r1 = 2;
    r2 = 1;
    r3 = 0;
  }

  b_A[r2] = A[r2] / A[r1];
  b_A[r3] /= b_A[r1];
  b_A[3 + r2] -= b_A[r2] * b_A[3 + r1];
  b_A[3 + r3] -= b_A[r3] * b_A[3 + r1];
  b_A[6 + r2] -= b_A[r2] * b_A[6 + r1];
  b_A[6 + r3] -= b_A[r3] * b_A[6 + r1];
  if (fabs(b_A[3 + r3]) > fabs(b_A[3 + r2])) {
    rtemp = r2;
    r2 = r3;
    r3 = rtemp;
  }

  b_A[3 + r3] /= b_A[3 + r2];
  b_A[6 + r3] -= b_A[3 + r3] * b_A[6 + r2];
  for (rtemp = 0; rtemp < 3; rtemp++) {
    Y[3 * rtemp] = B[r1 + 3 * rtemp];
    Y[1 + 3 * rtemp] = B[r2 + 3 * rtemp] - Y[3 * rtemp] * b_A[r2];
    Y[2 + 3 * rtemp] = (B[r3 + 3 * rtemp] - Y[3 * rtemp] * b_A[r3]) - Y[1 + 3 *
      rtemp] * b_A[3 + r3];
    Y[2 + 3 * rtemp] /= b_A[6 + r3];
    Y[3 * rtemp] -= Y[2 + 3 * rtemp] * b_A[6 + r1];
    Y[1 + 3 * rtemp] -= Y[2 + 3 * rtemp] * b_A[6 + r2];
    Y[1 + 3 * rtemp] /= b_A[3 + r2];
    Y[3 * rtemp] -= Y[1 + 3 * rtemp] * b_A[3 + r1];
    Y[3 * rtemp] /= b_A[r1];
  }
}

creal_T c_eml_div(const creal_T x, const creal_T y)
{
  creal_T z;
  real_T brm;
  real_T bim;
  real_T d;
  if (y.im == 0.0) {
    if (x.im == 0.0) {
      z.re = x.re / y.re;
      z.im = 0.0;
    } else if (x.re == 0.0) {
      z.re = 0.0;
      z.im = x.im / y.re;
    } else {
      z.re = x.re / y.re;
      z.im = x.im / y.re;
    }
  } else if (y.re == 0.0) {
    if (x.re == 0.0) {
      z.re = x.im / y.im;
      z.im = 0.0;
    } else if (x.im == 0.0) {
      z.re = 0.0;
      z.im = -(x.re / y.im);
    } else {
      z.re = x.im / y.im;
      z.im = -(x.re / y.im);
    }
  } else {
    brm = fabs(y.re);
    bim = fabs(y.im);
    if (brm > bim) {
      bim = y.im / y.re;
      d = y.re + bim * y.im;
      z.re = (x.re + bim * x.im) / d;
      z.im = (x.im - bim * x.re) / d;
    } else if (bim == brm) {
      bim = y.re > 0.0 ? 0.5 : -0.5;
      d = y.im > 0.0 ? 0.5 : -0.5;
      z.re = (x.re * bim + x.im * d) / brm;
      z.im = (x.im * bim - x.re * d) / brm;
    } else {
      bim = y.re / y.im;
      d = y.im + bim * y.re;
      z.re = (bim * x.re + x.im) / d;
      z.im = (bim * x.im - x.re) / d;
    }
  }

  return z;
}

void c_mldivide(const real_T A[4], const real_T B[4], real_T Y[4])
{
  int32_T r1;
  int32_T r2;
  real_T a21;
  real_T a22;
  int32_T k;
  if (fabs(A[1]) > fabs(A[0])) {
    r1 = 1;
    r2 = 0;
  } else {
    r1 = 0;
    r2 = 1;
  }

  a21 = A[r2] / A[r1];
  a22 = A[2 + r2] - a21 * A[2 + r1];
  for (k = 0; k < 2; k++) {
    Y[1 + (k << 1)] = (B[r2 + (k << 1)] - B[r1 + (k << 1)] * a21) / a22;
    Y[k << 1] = (B[r1 + (k << 1)] - Y[1 + (k << 1)] * A[2 + r1]) / A[r1];
  }
}

void mldivide(const creal_T A[9], const creal_T B[9], creal_T Y[9])
{
  creal_T b_A[9];
  int32_T r1;
  int32_T r2;
  int32_T r3;
  real_T maxval;
  real_T a21;
  int32_T rtemp;
  memcpy(&b_A[0], &A[0], 9U * sizeof(creal_T));
  r1 = 0;
  r2 = 1;
  r3 = 2;
  maxval = fabs(A[0].re) + fabs(A[0].im);
  a21 = fabs(A[1].re) + fabs(A[1].im);
  if (a21 > maxval) {
    maxval = a21;
    r1 = 1;
    r2 = 0;
  }

  if (fabs(A[2].re) + fabs(A[2].im) > maxval) {
    r1 = 2;
    r2 = 1;
    r3 = 0;
  }

  b_A[r2] = c_eml_div(A[r2], A[r1]);
  b_A[r3] = c_eml_div(b_A[r3], b_A[r1]);
  maxval = b_A[r2].re * b_A[3 + r1].im + b_A[r2].im * b_A[3 + r1].re;
  b_A[3 + r2].re -= b_A[r2].re * b_A[3 + r1].re - b_A[r2].im * b_A[3 + r1].im;
  b_A[3 + r2].im -= maxval;
  maxval = b_A[r3].re * b_A[3 + r1].im + b_A[r3].im * b_A[3 + r1].re;
  b_A[3 + r3].re -= b_A[r3].re * b_A[3 + r1].re - b_A[r3].im * b_A[3 + r1].im;
  b_A[3 + r3].im -= maxval;
  maxval = b_A[r2].re * b_A[6 + r1].im + b_A[r2].im * b_A[6 + r1].re;
  b_A[6 + r2].re -= b_A[r2].re * b_A[6 + r1].re - b_A[r2].im * b_A[6 + r1].im;
  b_A[6 + r2].im -= maxval;
  maxval = b_A[r3].re * b_A[6 + r1].im + b_A[r3].im * b_A[6 + r1].re;
  b_A[6 + r3].re -= b_A[r3].re * b_A[6 + r1].re - b_A[r3].im * b_A[6 + r1].im;
  b_A[6 + r3].im -= maxval;
  if (fabs(b_A[3 + r3].re) + fabs(b_A[3 + r3].im) > fabs(b_A[3 + r2].re) + fabs
      (b_A[3 + r2].im)) {
    rtemp = r2;
    r2 = r3;
    r3 = rtemp;
  }

  b_A[3 + r3] = c_eml_div(b_A[3 + r3], b_A[3 + r2]);
  maxval = b_A[3 + r3].re * b_A[6 + r2].im + b_A[3 + r3].im * b_A[6 + r2].re;
  b_A[6 + r3].re -= b_A[3 + r3].re * b_A[6 + r2].re - b_A[3 + r3].im * b_A[6 +
    r2].im;
  b_A[6 + r3].im -= maxval;
  for (rtemp = 0; rtemp < 3; rtemp++) {
    Y[3 * rtemp] = B[r1 + 3 * rtemp];
    maxval = Y[3 * rtemp].re * b_A[r2].im + Y[3 * rtemp].im * b_A[r2].re;
    Y[1 + 3 * rtemp].re = B[r2 + 3 * rtemp].re - (Y[3 * rtemp].re * b_A[r2].re -
      Y[3 * rtemp].im * b_A[r2].im);
    Y[1 + 3 * rtemp].im = B[r2 + 3 * rtemp].im - maxval;
    maxval = Y[3 * rtemp].re * b_A[r3].im + Y[3 * rtemp].im * b_A[r3].re;
    a21 = Y[1 + 3 * rtemp].re * b_A[3 + r3].im + Y[1 + 3 * rtemp].im * b_A[3 +
      r3].re;
    Y[2 + 3 * rtemp].re = (B[r3 + 3 * rtemp].re - (Y[3 * rtemp].re * b_A[r3].re
      - Y[3 * rtemp].im * b_A[r3].im)) - (Y[1 + 3 * rtemp].re * b_A[3 + r3].re -
      Y[1 + 3 * rtemp].im * b_A[3 + r3].im);
    Y[2 + 3 * rtemp].im = (B[r3 + 3 * rtemp].im - maxval) - a21;
    Y[2 + 3 * rtemp] = c_eml_div(Y[2 + 3 * rtemp], b_A[6 + r3]);
    maxval = Y[2 + 3 * rtemp].re * b_A[6 + r1].im + Y[2 + 3 * rtemp].im * b_A[6
      + r1].re;
    Y[3 * rtemp].re -= Y[2 + 3 * rtemp].re * b_A[6 + r1].re - Y[2 + 3 * rtemp].
      im * b_A[6 + r1].im;
    Y[3 * rtemp].im -= maxval;
    maxval = Y[2 + 3 * rtemp].re * b_A[6 + r2].im + Y[2 + 3 * rtemp].im * b_A[6
      + r2].re;
    Y[1 + 3 * rtemp].re -= Y[2 + 3 * rtemp].re * b_A[6 + r2].re - Y[2 + 3 *
      rtemp].im * b_A[6 + r2].im;
    Y[1 + 3 * rtemp].im -= maxval;
    Y[1 + 3 * rtemp] = c_eml_div(Y[1 + 3 * rtemp], b_A[3 + r2]);
    maxval = Y[1 + 3 * rtemp].re * b_A[3 + r1].im + Y[1 + 3 * rtemp].im * b_A[3
      + r1].re;
    Y[3 * rtemp].re -= Y[1 + 3 * rtemp].re * b_A[3 + r1].re - Y[1 + 3 * rtemp].
      im * b_A[3 + r1].im;
    Y[3 * rtemp].im -= maxval;
    Y[3 * rtemp] = c_eml_div(Y[3 * rtemp], b_A[r1]);
  }
}

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