svd.cpp

Go to the documentation of this file.

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

/* Type Definitions */

/* Named Constants */

/* Variable Declarations */

/* Variable Definitions */

/* Function Declarations */
static void b_eml_xaxpy(int32_T n, real_T a, const real_T x[16], int32_T ix0,
  real_T y[4], int32_T iy0);
static void b_eml_xscal(int32_T n, real_T a, real_T x[4], int32_T ix0);
static void c_eml_xaxpy(int32_T n, real_T a, const real_T x[4], int32_T ix0,
  real_T y[16], int32_T iy0);
static real_T c_eml_xnrm2(int32_T n, const real_T x[16], int32_T ix0);
static real_T d_eml_xnrm2(int32_T n, const real_T x[4], int32_T ix0);
static real_T eml_div(real_T x, real_T y);
static void eml_xaxpy(int32_T n, real_T a, int32_T ix0, real_T y[16], int32_T
                      iy0);
static real_T eml_xdotc(int32_T n, const real_T x[16], int32_T ix0, const real_T
  y[16], int32_T iy0);
static void eml_xgesvd(const real_T A[16], real_T U[16], real_T S[4], real_T V
  [16]);
static void eml_xrot(real_T x[16], int32_T ix0, int32_T iy0, real_T c, real_T s);
static void eml_xrotg(real_T *a, real_T *b, real_T *c, real_T *s);
static void eml_xscal(int32_T n, real_T a, real_T x[16], int32_T ix0);
static void eml_xswap(real_T x[16], int32_T ix0, int32_T iy0);

/* Function Definitions */
static void b_eml_xaxpy(int32_T n, real_T a, const real_T x[16], int32_T ix0,
  real_T y[4], int32_T iy0)
{
  int32_T ix;
  int32_T iy;
  int32_T k;
  if (a == 0.0) {
  } else {
    ix = ix0 - 1;
    iy = iy0 - 1;
    for (k = 0; k <= n - 1; k++) {
      y[iy] += a * x[ix];
      ix++;
      iy++;
    }
  }
}

static void b_eml_xscal(int32_T n, real_T a, real_T x[4], int32_T ix0)
{
  int32_T i28;
  int32_T k;
  i28 = (ix0 + n) - 1;
  for (k = ix0; k <= i28; k++) {
    x[k - 1] *= a;
  }
}

static void c_eml_xaxpy(int32_T n, real_T a, const real_T x[4], int32_T ix0,
  real_T y[16], int32_T iy0)
{
  int32_T ix;
  int32_T iy;
  int32_T k;
  if (a == 0.0) {
  } else {
    ix = ix0 - 1;
    iy = iy0 - 1;
    for (k = 0; k <= n - 1; k++) {
      y[iy] += a * x[ix];
      ix++;
      iy++;
    }
  }
}

static real_T c_eml_xnrm2(int32_T n, const real_T x[16], int32_T ix0)
{
  real_T y;
  real_T scale;
  int32_T kend;
  int32_T k;
  real_T absxk;
  real_T t;
  y = 0.0;
  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;
    }
  }

  return scale * sqrt(y);
}

static real_T d_eml_xnrm2(int32_T n, const real_T x[4], int32_T ix0)
{
  real_T y;
  real_T scale;
  int32_T kend;
  int32_T k;
  real_T absxk;
  real_T t;
  y = 0.0;
  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;
    }
  }

  return scale * sqrt(y);
}

static real_T eml_div(real_T x, real_T y)
{
  return x / y;
}

static void eml_xaxpy(int32_T n, real_T a, int32_T ix0, real_T y[16], int32_T
                      iy0)
{
  int32_T ix;
  int32_T iy;
  int32_T k;
  if ((n < 1) || (a == 0.0)) {
  } else {
    ix = ix0 - 1;
    iy = iy0 - 1;
    for (k = 0; k <= n - 1; k++) {
      y[iy] += a * y[ix];
      ix++;
      iy++;
    }
  }
}

static real_T eml_xdotc(int32_T n, const real_T x[16], int32_T ix0, const real_T
  y[16], int32_T iy0)
{
  real_T d;
  int32_T ix;
  int32_T iy;
  int32_T k;
  d = 0.0;
  if (n < 1) {
  } else {
    ix = ix0;
    iy = iy0;
    for (k = 1; k <= n; k++) {
      d += x[ix - 1] * y[iy - 1];
      ix++;
      iy++;
    }
  }

  return d;
}

static void eml_xgesvd(const real_T A[16], real_T U[16], real_T S[4], real_T V
  [16])
{
  real_T b_A[16];
  real_T s[4];
  real_T e[4];
  real_T work[4];
  int32_T i;
  int32_T q;
  int32_T qp1;
  int32_T mm1;
  real_T ztest0;
  int32_T iter;
  int32_T qs;
  int32_T m;
  real_T rt;
  real_T ztest;
  real_T tiny;
  real_T snorm;
  boolean_T exitg3;
  boolean_T exitg2;
  real_T sn;
  real_T varargin_1[5];
  boolean_T exitg1;
  real_T sqds;
  real_T b;
  memcpy(&b_A[0], &A[0], sizeof(real_T) << 4);
  for (i = 0; i < 4; i++) {
    s[i] = 0.0;
    e[i] = 0.0;
    work[i] = 0.0;
  }

  for (i = 0; i < 16; i++) {
    U[i] = 0.0;
    V[i] = 0.0;
  }

  for (q = 0; q < 3; q++) {
    qp1 = q + 2;
    mm1 = q + (q << 2);
    i = 4 - q;
    ztest0 = c_eml_xnrm2(i, b_A, mm1 + 1);
    if (ztest0 > 0.0) {
      if (b_A[mm1] < 0.0) {
        ztest0 = -ztest0;
      }

      s[q] = ztest0;
      eml_xscal(i, eml_div(1.0, s[q]), b_A, mm1 + 1);
      b_A[mm1]++;
      s[q] = -s[q];
    } else {
      s[q] = 0.0;
    }

    for (iter = qp1; iter < 5; iter++) {
      qs = q + ((iter - 1) << 2);
      if (s[q] != 0.0) {
        eml_xaxpy(i, -eml_div(eml_xdotc(i, b_A, mm1 + 1, b_A, qs + 1), b_A[q +
                              (q << 2)]), mm1 + 1, b_A, qs + 1);
      }

      e[iter - 1] = b_A[qs];
    }

    for (i = q; i + 1 < 5; i++) {
      U[i + (q << 2)] = b_A[i + (q << 2)];
    }

    if (q + 1 <= 2) {
      ztest0 = d_eml_xnrm2(3 - q, e, qp1);
      if (ztest0 == 0.0) {
        e[q] = 0.0;
      } else {
        if (e[qp1 - 1] < 0.0) {
          ztest0 = -ztest0;
        }

        e[q] = ztest0;
        b_eml_xscal(3 - q, eml_div(1.0, e[q]), e, qp1);
        e[qp1 - 1]++;
      }

      e[q] = -e[q];
      if (e[q] != 0.0) {
        for (i = qp1; i < 5; i++) {
          work[i - 1] = 0.0;
        }

        for (iter = qp1; iter < 5; iter++) {
          b_eml_xaxpy(3 - q, e[iter - 1], b_A, qp1 + ((iter - 1) << 2), work,
                      qp1);
        }

        for (iter = qp1; iter < 5; iter++) {
          c_eml_xaxpy(3 - q, eml_div(-e[iter - 1], e[qp1 - 1]), work, qp1, b_A,
                      qp1 + ((iter - 1) << 2));
        }
      }

      while (qp1 < 5) {
        V[(qp1 + (q << 2)) - 1] = e[qp1 - 1];
        qp1++;
      }
    }
  }

  m = 4;
  s[3] = b_A[15];
  e[2] = b_A[14];
  e[3] = 0.0;
  for (i = 0; i < 4; i++) {
    U[12 + i] = 0.0;
  }

  U[15] = 1.0;
  for (q = 2; q > -1; q += -1) {
    mm1 = q + (q << 2);
    if (s[q] != 0.0) {
      for (iter = q + 1; iter + 1 < 5; iter++) {
        qs = (q + (iter << 2)) + 1;
        eml_xaxpy(4 - q, -eml_div(eml_xdotc(4 - q, U, mm1 + 1, U, qs), U[mm1]),
                  mm1 + 1, U, qs);
      }

      for (i = q; i + 1 < 5; i++) {
        U[i + (q << 2)] = -U[i + (q << 2)];
      }

      U[mm1]++;
      for (i = 1; i <= q; i++) {
        U[(i + (q << 2)) - 1] = 0.0;
      }
    } else {
      for (i = 0; i < 4; i++) {
        U[i + (q << 2)] = 0.0;
      }

      U[mm1] = 1.0;
    }
  }

  for (q = 3; q > -1; q += -1) {
    if ((q + 1 <= 2) && (e[q] != 0.0)) {
      qp1 = q + 2;
      i = qp1 + (q << 2);
      for (iter = qp1; iter < 5; iter++) {
        qs = qp1 + ((iter - 1) << 2);
        eml_xaxpy(3 - q, -eml_div(eml_xdotc(3 - q, V, i, V, qs), V[i - 1]), i, V,
                  qs);
      }
    }

    for (i = 0; i < 4; i++) {
      V[i + (q << 2)] = 0.0;
    }

    V[q + (q << 2)] = 1.0;
  }

  for (q = 0; q < 4; q++) {
    ztest0 = e[q];
    if (s[q] != 0.0) {
      rt = fabs(s[q]);
      ztest = eml_div(s[q], rt);
      s[q] = rt;
      if (q + 1 < 4) {
        ztest0 = eml_div(e[q], ztest);
      }

      eml_xscal(4, ztest, U, (q << 2) + 1);
    }

    if ((q + 1 < 4) && (ztest0 != 0.0)) {
      rt = fabs(ztest0);
      ztest = eml_div(rt, ztest0);
      ztest0 = rt;
      s[q + 1] *= ztest;
      eml_xscal(4, ztest, V, ((q + 1) << 2) + 1);
    }

    e[q] = ztest0;
  }

  iter = 0;
  tiny = eml_div(2.2250738585072014E-308, 2.2204460492503131E-16);
  snorm = 0.0;
  for (i = 0; i < 4; i++) {
    ztest0 = fabs(s[i]);
    ztest = fabs(e[i]);
    if ((ztest0 >= ztest) || rtIsNaN(ztest)) {
      ztest = ztest0;
    }

    if ((snorm >= ztest) || rtIsNaN(ztest)) {
    } else {
      snorm = ztest;
    }
  }

  while ((m > 0) && (!(iter >= 75))) {
    q = m - 1;
    exitg3 = FALSE;
    while (!((exitg3 == 1U) || (q == 0))) {
      ztest0 = fabs(e[q - 1]);
      if ((ztest0 <= 2.2204460492503131E-16 * (fabs(s[q - 1]) + fabs(s[q]))) ||
          (ztest0 <= tiny) || ((iter > 20) && (ztest0 <= 2.2204460492503131E-16 *
            snorm))) {
        e[q - 1] = 0.0;
        exitg3 = TRUE;
      } else {
        q--;
      }
    }

    if (q == m - 1) {
      i = 4;
    } else {
      qs = m;
      i = m;
      exitg2 = FALSE;
      while ((exitg2 == 0U) && (i >= q)) {
        qs = i;
        if (i == q) {
          exitg2 = TRUE;
        } else {
          ztest0 = 0.0;
          if (i < m) {
            ztest0 = fabs(e[i - 1]);
          }

          if (i > q + 1) {
            ztest0 += fabs(e[i - 2]);
          }

          ztest = fabs(s[i - 1]);
          if ((ztest <= 2.2204460492503131E-16 * ztest0) || (ztest <= tiny)) {
            s[i - 1] = 0.0;
            exitg2 = TRUE;
          } else {
            i--;
          }
        }
      }

      if (qs == q) {
        i = 3;
      } else if (qs == m) {
        i = 1;
      } else {
        i = 2;
        q = qs;
      }
    }

    switch (i) {
     case 1:
      ztest = e[m - 2];
      e[m - 2] = 0.0;
      for (qs = m - 2; qs + 1 >= q + 1; qs--) {
        ztest0 = s[qs];
        eml_xrotg(&ztest0, &ztest, &rt, &sn);
        s[qs] = ztest0;
        if (qs + 1 > q + 1) {
          i = qs - 1;
          ztest = -sn * e[i];
          e[i] *= rt;
        }

        eml_xrot(V, (qs << 2) + 1, ((m - 1) << 2) + 1, rt, sn);
      }
      break;

     case 2:
      i = q - 1;
      ztest = e[i];
      e[i] = 0.0;
      while (q + 1 <= m) {
        eml_xrotg(&s[q], &ztest, &rt, &sn);
        ztest = -sn * e[q];
        e[q] *= rt;
        eml_xrot(U, (q << 2) + 1, (i << 2) + 1, rt, sn);
        q++;
      }
      break;

     case 3:
      mm1 = m - 2;
      varargin_1[0] = fabs(s[m - 1]);
      varargin_1[1] = fabs(s[mm1]);
      varargin_1[2] = fabs(e[mm1]);
      varargin_1[3] = fabs(s[q]);
      varargin_1[4] = fabs(e[q]);
      i = 1;
      sn = varargin_1[0];
      if (rtIsNaN(varargin_1[0])) {
        qs = 2;
        exitg1 = FALSE;
        while ((exitg1 == 0U) && (qs < 6)) {
          i = qs;
          if (!rtIsNaN(varargin_1[qs - 1])) {
            sn = varargin_1[qs - 1];
            exitg1 = TRUE;
          } else {
            qs++;
          }
        }
      }

      if (i < 5) {
        while (i + 1 < 6) {
          if (varargin_1[i] > sn) {
            sn = varargin_1[i];
          }

          i++;
        }
      }

      rt = eml_div(s[m - 1], sn);
      ztest0 = eml_div(s[mm1], sn);
      ztest = eml_div(e[mm1], sn);
      sqds = eml_div(s[q], sn);
      b = eml_div((ztest0 + rt) * (ztest0 - rt) + ztest * ztest, 2.0);
      ztest0 = rt * ztest;
      ztest0 *= ztest0;
      ztest = 0.0;
      if ((b != 0.0) || (ztest0 != 0.0)) {
        ztest = sqrt(b * b + ztest0);
        if (b < 0.0) {
          ztest = -ztest;
        }

        ztest = eml_div(ztest0, b + ztest);
      }

      ztest += (sqds + rt) * (sqds - rt);
      ztest0 = sqds * eml_div(e[q], sn);
      for (qs = q; qs + 1 <= mm1 + 1; qs++) {
        i = qs + 1;
        eml_xrotg(&ztest, &ztest0, &rt, &sn);
        if (qs + 1 > q + 1) {
          e[qs - 1] = ztest;
        }

        ztest0 = rt * s[qs];
        ztest = sn * e[qs];
        e[qs] = rt * e[qs] - sn * s[qs];
        b = s[i];
        s[i] *= rt;
        eml_xrot(V, (qs << 2) + 1, ((qs + 1) << 2) + 1, rt, sn);
        s[qs] = ztest0 + ztest;
        ztest0 = sn * b;
        eml_xrotg(&s[qs], &ztest0, &rt, &sn);
        ztest = rt * e[qs] + sn * s[i];
        s[i] = -sn * e[qs] + rt * s[i];
        ztest0 = sn * e[i];
        e[i] *= rt;
        eml_xrot(U, (qs << 2) + 1, ((qs + 1) << 2) + 1, rt, sn);
      }

      e[m - 2] = ztest;
      iter++;
      break;

     default:
      if (s[q] < 0.0) {
        s[q] = -s[q];
        eml_xscal(4, -1.0, V, (q << 2) + 1);
      }

      qp1 = q + 1;
      while ((q + 1 < 4) && (s[q] < s[qp1])) {
        rt = s[q];
        s[q] = s[qp1];
        s[qp1] = rt;
        eml_xswap(V, (q << 2) + 1, ((q + 1) << 2) + 1);
        eml_xswap(U, (q << 2) + 1, ((q + 1) << 2) + 1);
        q = qp1;
        qp1++;
      }

      iter = 0;
      m--;
      break;
    }
  }

  for (qs = 0; qs < 4; qs++) {
    S[qs] = s[qs];
  }
}

static void eml_xrot(real_T x[16], int32_T ix0, int32_T iy0, real_T c, real_T s)
{
  int32_T ix;
  int32_T iy;
  int32_T k;
  real_T y;
  real_T b_y;
  ix = ix0 - 1;
  iy = iy0 - 1;
  for (k = 0; k < 4; k++) {
    y = c * x[ix];
    b_y = s * x[iy];
    x[iy] = c * x[iy] - s * x[ix];
    x[ix] = y + b_y;
    iy++;
    ix++;
  }
}

static void eml_xrotg(real_T *a, real_T *b, real_T *c, real_T *s)
{
  real_T roe;
  real_T absa;
  real_T absb;
  real_T scale;
  real_T ads;
  real_T bds;
  roe = *b;
  absa = fabs(*a);
  absb = fabs(*b);
  if (absa > absb) {
    roe = *a;
  }

  scale = absa + absb;
  if (scale == 0.0) {
    *s = 0.0;
    *c = 1.0;
    ads = 0.0;
    scale = 0.0;
  } else {
    ads = absa / scale;
    bds = absb / scale;
    ads = scale * sqrt(ads * ads + bds * bds);
    if (roe < 0.0) {
      ads = -ads;
    }

    *c = *a / ads;
    *s = *b / ads;
    if (absa > absb) {
      scale = *s;
    } else if (*c != 0.0) {
      scale = 1.0 / *c;
    } else {
      scale = 1.0;
    }
  }

  *a = ads;
  *b = scale;
}

static void eml_xscal(int32_T n, real_T a, real_T x[16], int32_T ix0)
{
  int32_T i27;
  int32_T k;
  i27 = (ix0 + n) - 1;
  for (k = ix0; k <= i27; k++) {
    x[k - 1] *= a;
  }
}

static void eml_xswap(real_T x[16], int32_T ix0, int32_T iy0)
{
  int32_T ix;
  int32_T iy;
  int32_T k;
  real_T temp;
  ix = ix0 - 1;
  iy = iy0 - 1;
  for (k = 0; k < 4; k++) {
    temp = x[ix];
    x[ix] = x[iy];
    x[iy] = temp;
    ix++;
    iy++;
  }
}

void svd(const real_T A[16], real_T U[16], real_T S[16], real_T V[16])
{
  real_T s[4];
  int32_T k;
  eml_xgesvd(A, U, s, V);
  memset(&S[0], 0, sizeof(real_T) << 4);
  for (k = 0; k < 4; k++) {
    S[k + (k << 2)] = s[k];
  }
}

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