//
//  Academic License - for use in teaching, academic research, and meeting
//  course requirements at degree granting institutions only.  Not for
//  government, commercial, or other organizational use.
//
//  MatrixExp6.cpp
//
//  Code generation for function 'MatrixExp6'
//


// Include files
#include "MatrixExp6.h"
#include "calculateGravityForce.h"
#include "calculateGravityForce_data.h"
#include "mwmathutil.h"
#include "rt_nonfinite.h"
#include <string.h>

// Function Definitions
void MatrixExp6(const real_T se3mat[16], real_T T[16])
{
  real_T scale;
  real_T absxk;
  real_T t;
  real_T x_tmp;
  int8_T b_I[9];
  real_T x[3];
  real_T b_x_tmp;
  real_T omgmat[9];
  real_T R[9];
  real_T b_omgmat[9];

  //  *** CHAPTER 3: RIGID-BODY MOTIONS ***
  //  Takes a se(3) representation of exponential coordinates.
  //  Returns a T matrix in SE(3) that is achieved by traveling along/about the  
  //  screw axis S for a distance theta from an initial configuration T = I.
  //  Example Input:
  //
  //  clear; clc;
  //  se3mat = [ 0,      0,       0,      0;
  //           0,      0, -1.5708, 2.3562;
  //           0, 1.5708,       0, 2.3562;
  //           0,      0,       0,      0]
  //  T = MatrixExp6(se3mat)
  //
  //  Output:
  //  T =
  //     1.0000         0         0         0
  //          0    0.0000   -1.0000   -0.0000
  //          0    1.0000    0.0000    3.0000
  //          0         0         0    1.0000
  //  *** CHAPTER 3: RIGID-BODY MOTIONS ***
  //  Takes a 3x3 skew-symmetric matrix (an element of so(3)).
  //  Returns the corresponding 3-vector (angular velocity).
  //  Example Input:
  //
  //  clear; clc;
  //  so3mat = [[0, -3, 2]; [3, 0, -1]; [-2, 1, 0]];
  //  omg = so3ToVec(so3mat)
  //
  //  Output:
  //  omg =
  //      1
  //      2
  //      3
  scale = 3.3121686421112381E-170;
  absxk = muDoubleScalarAbs(se3mat[6]);
  if (absxk > 3.3121686421112381E-170) {
    x_tmp = 1.0;
    scale = absxk;
  } else {
    t = absxk / 3.3121686421112381E-170;
    x_tmp = t * t;
  }

  absxk = muDoubleScalarAbs(se3mat[8]);
  if (absxk > scale) {
    t = scale / absxk;
    x_tmp = x_tmp * t * t + 1.0;
    scale = absxk;
  } else {
    t = absxk / scale;
    x_tmp += t * t;
  }

  absxk = muDoubleScalarAbs(se3mat[1]);
  if (absxk > scale) {
    t = scale / absxk;
    x_tmp = x_tmp * t * t + 1.0;
    scale = absxk;
  } else {
    t = absxk / scale;
    x_tmp += t * t;
  }

  x_tmp = scale * muDoubleScalarSqrt(x_tmp);

  //  *** BASIC HELPER FUNCTIONS ***
  //  Takes a scalar.
  //  Checks if the scalar is small enough to be neglected.
  //  Example Input:
  //
  //  clear; clc;
  //  near = -1e-7;
  //  judge = NearZero(near)
  //
  //  Output:
  //  judge =
  //      1
  if (x_tmp < 1.0E-6) {
    int32_T i;
    for (i = 0; i < 9; i++) {
      b_I[i] = 0;
    }

    b_I[0] = 1;
    b_I[4] = 1;
    b_I[8] = 1;
    for (i = 0; i < 3; i++) {
      int32_T k;
      k = i << 2;
      T[k] = b_I[3 * i];
      T[k + 1] = b_I[3 * i + 1];
      T[k + 2] = b_I[3 * i + 2];
      T[i + 12] = se3mat[i + 12];
    }

    T[3] = 0.0;
    T[7] = 0.0;
    T[11] = 0.0;
    T[15] = 1.0;
  } else {
    int32_T i;
    int32_T k;
    real_T a;
    real_T b_a;
    int32_T omgmat_tmp;

    //  *** CHAPTER 3: RIGID-BODY MOTIONS ***
    //  Takes A 3-vector of exponential coordinates for rotation.
    //  Returns the unit rotation axis omghat and the corresponding rotation
    //  angle theta.
    //  Example Input:
    //
    //  clear; clc;
    //  expc3 = [1; 2; 3];
    //  [omghat, theta] = AxisAng3(expc3)
    //
    //  Output:
    //  omghat =
    //     0.2673
    //     0.5345
    //     0.8018
    //  theta =
    //     3.7417
    a = 1.0 - muDoubleScalarCos(x_tmp);
    b_a = x_tmp - muDoubleScalarSin(x_tmp);

    //  *** CHAPTER 3: RIGID-BODY MOTIONS ***
    //  Takes a 3x3 so(3) representation of exponential coordinates.
    //  Returns R in SO(3) that is achieved by rotating about omghat by theta
    //  from an initial orientation R = I.
    //  Example Input:
    //
    //  clear; clc;
    //  so3mat = [[0, -3, 2]; [3, 0, -1]; [-2, 1, 0]];
    //  R = MatrixExp3(so3mat)
    //
    //  Output:
    //  R =
    //    -0.6949    0.7135    0.0893
    //    -0.1920   -0.3038    0.9332
    //     0.6930    0.6313    0.3481
    //  *** CHAPTER 3: RIGID-BODY MOTIONS ***
    //  Takes a 3x3 skew-symmetric matrix (an element of so(3)).
    //  Returns the corresponding 3-vector (angular velocity).
    //  Example Input:
    //
    //  clear; clc;
    //  so3mat = [[0, -3, 2]; [3, 0, -1]; [-2, 1, 0]];
    //  omg = so3ToVec(so3mat)
    //
    //  Output:
    //  omg =
    //      1
    //      2
    //      3
    x[0] = se3mat[6];
    x[1] = se3mat[8];
    x[2] = se3mat[1];
    b_x_tmp = 0.0;
    scale = 3.3121686421112381E-170;
    for (k = 0; k < 3; k++) {
      omgmat_tmp = k << 2;
      omgmat[3 * k] = se3mat[omgmat_tmp] / x_tmp;
      omgmat[3 * k + 1] = se3mat[omgmat_tmp + 1] / x_tmp;
      omgmat[3 * k + 2] = se3mat[omgmat_tmp + 2] / x_tmp;
      absxk = muDoubleScalarAbs(x[k]);
      if (absxk > scale) {
        t = scale / absxk;
        b_x_tmp = b_x_tmp * t * t + 1.0;
        scale = absxk;
      } else {
        t = absxk / scale;
        b_x_tmp += t * t;
      }
    }

    b_x_tmp = scale * muDoubleScalarSqrt(b_x_tmp);

    //  *** BASIC HELPER FUNCTIONS ***
    //  Takes a scalar.
    //  Checks if the scalar is small enough to be neglected.
    //  Example Input:
    //
    //  clear; clc;
    //  near = -1e-7;
    //  judge = NearZero(near)
    //
    //  Output:
    //  judge =
    //      1
    if (b_x_tmp < 1.0E-6) {
      memset(&R[0], 0, 9U * sizeof(real_T));
      R[0] = 1.0;
      R[4] = 1.0;
      R[8] = 1.0;
    } else {
      //  *** CHAPTER 3: RIGID-BODY MOTIONS ***
      //  Takes A 3-vector of exponential coordinates for rotation.
      //  Returns the unit rotation axis omghat and the corresponding rotation
      //  angle theta.
      //  Example Input:
      //
      //  clear; clc;
      //  expc3 = [1; 2; 3];
      //  [omghat, theta] = AxisAng3(expc3)
      //
      //  Output:
      //  omghat =
      //     0.2673
      //     0.5345
      //     0.8018
      //  theta =
      //     3.7417
      for (i = 0; i < 3; i++) {
        omgmat_tmp = i << 2;
        b_omgmat[3 * i] = se3mat[omgmat_tmp] / b_x_tmp;
        b_omgmat[3 * i + 1] = se3mat[omgmat_tmp + 1] / b_x_tmp;
        b_omgmat[3 * i + 2] = se3mat[omgmat_tmp + 2] / b_x_tmp;
      }

      absxk = muDoubleScalarSin(b_x_tmp);
      scale = muDoubleScalarCos(b_x_tmp);
      for (i = 0; i < 9; i++) {
        b_I[i] = 0;
      }

      b_I[0] = 1;
      b_I[4] = 1;
      b_I[8] = 1;
      for (i = 0; i < 3; i++) {
        for (omgmat_tmp = 0; omgmat_tmp < 3; omgmat_tmp++) {
          k = i + 3 * omgmat_tmp;
          R[k] = (static_cast<real_T>(b_I[k]) + absxk * b_omgmat[k]) + (((1.0 -
            scale) * b_omgmat[i] * b_omgmat[3 * omgmat_tmp] + (1.0 - scale) *
            b_omgmat[i + 3] * b_omgmat[3 * omgmat_tmp + 1]) + (1.0 - scale) *
            b_omgmat[i + 6] * b_omgmat[3 * omgmat_tmp + 2]);
        }
      }
    }

    for (i = 0; i < 3; i++) {
      scale = 0.0;
      for (omgmat_tmp = 0; omgmat_tmp < 3; omgmat_tmp++) {
        k = i + 3 * omgmat_tmp;
        scale += ((static_cast<real_T>(iv[k]) * x_tmp + a * omgmat[k]) + ((b_a *
                    omgmat[i] * omgmat[3 * omgmat_tmp] + b_a * omgmat[i + 3] *
                    omgmat[3 * omgmat_tmp + 1]) + b_a * omgmat[i + 6] * omgmat[3
                   * omgmat_tmp + 2])) * se3mat[omgmat_tmp + 12];
        T[omgmat_tmp + (i << 2)] = R[omgmat_tmp + 3 * i];
      }

      T[i + 12] = scale / x_tmp;
    }

    T[3] = 0.0;
    T[7] = 0.0;
    T[11] = 0.0;
    T[15] = 1.0;
  }
}

// End of code generation (MatrixExp6.cpp)