File: calculateTransformations.c

    1   /*
    2    * Academic License - for use in teaching, academic research, and meeting
    3    * course requirements at degree granting institutions only.  Not for
    4    * government, commercial, or other organizational use.
    5    *
    6    * calculateTransformations.c
    7    *
    8    * Code generation for function 'calculateTransformations'
    9    *
   10    */
   11   
   12   /* Include files */
   13   #include "rt_nonfinite.h"
   14   #include "calculateTransformations.h"
   15   
   16   /* Function Declarations */
   17   static double rt_atan2d_snf(double u0, double u1);
   18   
   19   /* Function Definitions */
   20   static double rt_atan2d_snf(double u0, double u1)
   21   {
   22     double y;
   23     int b_u0;
   24     int b_u1;
   25     if (rtIsNaN(u0) || rtIsNaN(u1)) {
   26       y = rtNaN;
   27     } else if (rtIsInf(u0) && rtIsInf(u1)) {
   28       if (u0 > 0.0) {
   29         b_u0 = 1;
   30       } else {
   31         b_u0 = -1;
   32       }
   33   
   34       if (u1 > 0.0) {
   35         b_u1 = 1;
   36       } else {
   37         b_u1 = -1;
   38       }
   39   
   40       y = atan2(b_u0, b_u1);
   41     } else if (u1 == 0.0) {
   42       if (u0 > 0.0) {
   43         y = RT_PI / 2.0;
   44       } else if (u0 < 0.0) {
   45         y = -(RT_PI / 2.0);
   46       } else {
   47         y = 0.0;
   48       }
   49     } else {
   50       y = atan2(u0, u1);
   51     }
   52   
   53     return y;
   54   }
   55   
   56   void calculateTransformations(const double q1[5], const double q2[5], const
   57     double eta_marker[6], const double g_marker2front[16], double
   58     controlCenterLink, double g_If[16], double R_If[9], double eta_f[6], double
   59     g_Ib[16], double R_Ib[9], double eta_b[6], double q[8], double g_joints_home
   60     [144], double g_moduleCM[144], double g_thrusters[112])
   61   {
   62     double cphi;
   63     double sphi;
   64     double cth;
   65     double sth;
   66     double cpsi;
   67     double spsi;
   68     double b_cpsi[16];
   69     int k;
   70     static const signed char iv0[4] = { 0, 0, 0, 1 };
   71   
   72     int i0;
   73     int i1;
   74     double g_joints[128];
   75     double g_center2back[16];
   76     int i;
   77     boolean_T b_bool;
   78     static const char joint_ax[8] = { 'Z', 'Y', 'Z', 'Y', 'Z', 'Y', 'Z', 'Y' };
   79   
   80     double cos_angle;
   81     double sin_angle;
   82     double g_front2back[16];
   83     static const signed char iv1[3] = { 0, 1, 0 };
   84   
   85     static const double joint_tr[27] = { 0.62, 0.1, 0.59, 0.1, 0.8, 0.1, 0.59, 0.1,
   86       0.37, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
   87       0.0, 0.0, 0.0, 0.0 };
   88   
   89     signed char I[9];
   90     static const signed char iv2[3] = { 0, 0, 1 };
   91   
   92     static const double g_ne[16] = { 1.0, 0.0, 0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
   93       0.0, 1.0, 0.0, 0.37, 0.0, 0.0, 1.0 };
   94   
   95     static const signed char iv3[9] = { 1, 0, 0, 0, 1, 0, 0, 0, 1 };
   96   
   97     static const double module_cm[27] = { 0.31, 0.05, 0.295, 0.05, 0.4, 0.05,
   98       0.295, 0.05, 0.185, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0,
   99       0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
  100   
  101     static const char thruster_ax[7] = { 'Z', 'Y', 'Z', 'X', 'X', 'Y', 'Z' };
  102   
  103     static const double rotZ[9] = { 6.123233995736766E-17, 1.0, 0.0, -1.0,
  104       6.123233995736766E-17, 0.0, 0.0, 0.0, 1.0 };
  105   
  106     static const double thruster_pos[21] = { 0.24, 0.35000000000000003, 0.24, 0.53,
  107       0.53, 0.24, 0.35000000000000003, 0.0, 0.0, 0.0, 0.15, -0.15, 0.0, 0.0, 0.0,
  108       0.0, 0.0, 0.0, 0.0, 0.0, 0.0 };
  109   
  110     static const double rotY[9] = { 6.123233995736766E-17, 0.0, 1.0, 0.0, 1.0, 0.0,
  111       -1.0, 0.0, 6.123233995736766E-17 };
  112   
  113     static const signed char iv4[7] = { 3, 3, 5, 5, 5, 7, 7 };
  114   
  115     double b_g_joints_home[9];
  116     double c_g_joints_home[3];
  117   
  118     /*  [EELY] Specific code for Eely */
  119     /*  Measured: Position and Euler angles for the markers, eta_marker */
  120     /*  R = Rzyx(phi,theta,psi) computes the Euler angle */
  121     /*  rotation matrix R in SO(3) using the zyx convention */
  122     /*  */
  123     /*  Author:   Thor I. Fossen */
  124     /*  Date:     14th June 2001 */
  125     /*  Revisions:  */
  126     /*  ________________________________________________________________ */
  127     /*  */
  128     /*  MSS GNC is a Matlab toolbox for guidance, navigation and control. */
  129     /*  The toolbox is part of the Marine Systems Simulator (MSS). */
  130     /*  */
  131     /*  Copyright (C) 2008 Thor I. Fossen and Tristan Perez */
  132     /*   */
  133     /*  This program is free software: you can redistribute it and/or modify */
  134     /*  it under the terms of the GNU General Public License as published by */
  135     /*  the Free Software Foundation, either version 3 of the License, or */
  136     /*  (at your option) any later version. */
  137     /*   */
  138     /*  This program is distributed in the hope that it will be useful, but */
  139     /*  WITHOUT ANY WARRANTY; without even the implied warranty of */
  140     /*  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the  */
  141     /*  GNU General Public License for more details. */
  142     /*   */
  143     /*  You should have received a copy of the GNU General Public License */
  144     /*  along with this program.  If not, see <http://www.gnu.org/licenses/>. */
  145     /*   */
  146     /*  E-mail: contact@marinecontrol.org */
  147     /*  URL:    <http://www.marinecontrol.org> */
  148     cphi = cos(eta_marker[3]);
  149     sphi = sin(eta_marker[3]);
  150     cth = cos(eta_marker[4]);
  151     sth = sin(eta_marker[4]);
  152     cpsi = cos(eta_marker[5]);
  153     spsi = sin(eta_marker[5]);
  154     b_cpsi[0] = cpsi * cth;
  155     b_cpsi[4] = -spsi * cphi + cpsi * sth * sphi;
  156     b_cpsi[8] = spsi * sphi + cpsi * cphi * sth;
  157     b_cpsi[1] = spsi * cth;
  158     b_cpsi[5] = cpsi * cphi + sphi * sth * spsi;
  159     b_cpsi[9] = -cpsi * sphi + sth * spsi * cphi;
  160     b_cpsi[2] = -sth;
  161     b_cpsi[6] = cth * sphi;
  162     b_cpsi[10] = cth * cphi;
  163     for (k = 0; k < 3; k++) {
  164       b_cpsi[12 + k] = eta_marker[k];
  165     }
  166   
  167     for (k = 0; k < 4; k++) {
  168       b_cpsi[3 + (k << 2)] = iv0[k];
  169     }
  170   
  171     for (k = 0; k < 4; k++) {
  172       for (i0 = 0; i0 < 4; i0++) {
  173         g_If[k + (i0 << 2)] = 0.0;
  174         for (i1 = 0; i1 < 4; i1++) {
  175           g_If[k + (i0 << 2)] += b_cpsi[k + (i1 << 2)] * g_marker2front[i1 + (i0 <<
  176             2)];
  177         }
  178       }
  179     }
  180   
  181     for (k = 0; k < 3; k++) {
  182       for (i0 = 0; i0 < 3; i0++) {
  183         R_If[i0 + 3 * k] = g_If[i0 + (k << 2)];
  184       }
  185   
  186       eta_f[k] = g_If[12 + k];
  187     }
  188   
  189     eta_f[3] = rt_atan2d_snf(g_If[6], g_If[10]);
  190     eta_f[4] = -asin(g_If[2]);
  191     eta_f[5] = rt_atan2d_snf(g_If[1], g_If[0]);
  192   
  193     /*  Code to convert the joint angle inputs q1 and q2 from the Eely robot */
  194     /*  to the definition used in this code by reorganizing the vector elements. */
  195     q[0] = q2[3] * 3.1415926535897931 / 180.0;
  196     q[1] = q1[3] * 3.1415926535897931 / 180.0;
  197     q[2] = q2[2] * 3.1415926535897931 / 180.0;
  198     q[3] = q1[2] * 3.1415926535897931 / 180.0;
  199     q[4] = q2[1] * 3.1415926535897931 / 180.0;
  200     q[5] = q1[1] * 3.1415926535897931 / 180.0;
  201     q[6] = q2[4] * 3.1415926535897931 / 180.0;
  202     q[7] = q1[4] * 3.1415926535897931 / 180.0;
  203   
  204     /* % TRANSFORMATIONS */
  205     /*  Calculate the sequential transformations from base frame to the joint */
  206     /*  frames */
  207     /*  Calculates the transformation matrices from  */
  208     /*  the USM base frame, F_b = F_0, to the joint frames F_i (i=1..n) */
  209     /*  From the tail module and forward */
  210     /*  JOINT FRAMES */
  211     memset(&g_joints[0], 0, sizeof(double) << 7);
  212   
  213     /*  size = num_joint+1 to include F_e */
  214     for (i = 0; i < 8; i++) {
  215       b_bool = false;
  216       if (joint_ax[i] != 'Y') {
  217       } else {
  218         b_bool = true;
  219       }
  220   
  221       if (b_bool) {
  222         cos_angle = cos(q[i]);
  223         sin_angle = sin(q[i]);
  224         g_front2back[0] = cos_angle;
  225         g_front2back[4] = 0.0;
  226         g_front2back[8] = sin_angle;
  227         g_front2back[2] = -sin_angle;
  228         g_front2back[6] = 0.0;
  229         g_front2back[10] = cos_angle;
  230         for (k = 0; k < 3; k++) {
  231           g_front2back[1 + (k << 2)] = iv1[k];
  232           g_front2back[12 + k] = joint_tr[i + 9 * k];
  233         }
  234   
  235         for (k = 0; k < 4; k++) {
  236           g_front2back[3 + (k << 2)] = iv0[k];
  237         }
  238   
  239         for (k = 0; k < 9; k++) {
  240           I[k] = 0;
  241         }
  242   
  243         for (k = 0; k < 3; k++) {
  244           I[k + 3 * k] = 1;
  245         }
  246   
  247         for (k = 0; k < 3; k++) {
  248           for (i0 = 0; i0 < 3; i0++) {
  249             g_center2back[i0 + (k << 2)] = I[i0 + 3 * k];
  250           }
  251   
  252           g_center2back[12 + k] = joint_tr[i + 9 * k];
  253         }
  254   
  255         for (k = 0; k < 4; k++) {
  256           g_center2back[3 + (k << 2)] = iv0[k];
  257         }
  258       } else {
  259         b_bool = false;
  260         if (joint_ax[i] != 'Z') {
  261         } else {
  262           b_bool = true;
  263         }
  264   
  265         if (b_bool) {
  266           cos_angle = cos(q[i]);
  267           sin_angle = sin(q[i]);
  268           g_front2back[0] = cos_angle;
  269           g_front2back[4] = -sin_angle;
  270           g_front2back[8] = 0.0;
  271           g_front2back[1] = sin_angle;
  272           g_front2back[5] = cos_angle;
  273           g_front2back[9] = 0.0;
  274           for (k = 0; k < 3; k++) {
  275             g_front2back[2 + (k << 2)] = iv2[k];
  276             g_front2back[12 + k] = joint_tr[i + 9 * k];
  277           }
  278   
  279           for (k = 0; k < 4; k++) {
  280             g_front2back[3 + (k << 2)] = iv0[k];
  281           }
  282   
  283           for (k = 0; k < 9; k++) {
  284             I[k] = 0;
  285           }
  286   
  287           for (k = 0; k < 3; k++) {
  288             I[k + 3 * k] = 1;
  289           }
  290   
  291           for (k = 0; k < 3; k++) {
  292             for (i0 = 0; i0 < 3; i0++) {
  293               g_center2back[i0 + (k << 2)] = I[i0 + 3 * k];
  294             }
  295   
  296             g_center2back[12 + k] = joint_tr[i + 9 * k];
  297           }
  298   
  299           for (k = 0; k < 4; k++) {
  300             g_center2back[3 + (k << 2)] = iv0[k];
  301           }
  302         } else {
  303           memset(&g_front2back[0], 0, sizeof(double) << 4);
  304           for (k = 0; k < 4; k++) {
  305             g_front2back[k + (k << 2)] = 1.0;
  306           }
  307   
  308           memset(&g_center2back[0], 0, sizeof(double) << 4);
  309           for (k = 0; k < 4; k++) {
  310             g_center2back[k + (k << 2)] = 1.0;
  311           }
  312         }
  313       }
  314   
  315       if (1 + i == 1) {
  316         for (k = 0; k < 4; k++) {
  317           for (i0 = 0; i0 < 4; i0++) {
  318             g_joints[(i0 + (k << 2)) + (i << 4)] = g_front2back[i0 + (k << 2)];
  319             g_joints_home[(i0 + (k << 2)) + (i << 4)] = g_center2back[i0 + (k << 2)];
  320           }
  321         }
  322       } else {
  323         for (k = 0; k < 4; k++) {
  324           for (i0 = 0; i0 < 4; i0++) {
  325             b_cpsi[k + (i0 << 2)] = 0.0;
  326             for (i1 = 0; i1 < 4; i1++) {
  327               b_cpsi[k + (i0 << 2)] += g_joints[(k + (i1 << 2)) + ((i - 1) << 4)] *
  328                 g_front2back[i1 + (i0 << 2)];
  329             }
  330           }
  331         }
  332   
  333         for (k = 0; k < 4; k++) {
  334           for (i0 = 0; i0 < 4; i0++) {
  335             g_joints[(i0 + (k << 2)) + (i << 4)] = b_cpsi[i0 + (k << 2)];
  336           }
  337         }
  338   
  339         for (k = 0; k < 4; k++) {
  340           for (i0 = 0; i0 < 4; i0++) {
  341             g_joints_home[(k + (i0 << 2)) + (i << 4)] = 0.0;
  342             for (i1 = 0; i1 < 4; i1++) {
  343               g_joints_home[(k + (i0 << 2)) + (i << 4)] += g_joints[(k + (i1 << 2))
  344                 + ((i - 1) << 4)] * g_center2back[i1 + (i0 << 2)];
  345             }
  346           }
  347         }
  348       }
  349     }
  350   
  351     for (k = 0; k < 4; k++) {
  352       for (i0 = 0; i0 < 4; i0++) {
  353         g_joints_home[128 + (k + (i0 << 2))] = 0.0;
  354         for (i1 = 0; i1 < 4; i1++) {
  355           g_joints_home[128 + (k + (i0 << 2))] += g_joints[112 + (k + (i1 << 2))] *
  356             g_ne[i1 + (i0 << 2)];
  357         }
  358       }
  359     }
  360   
  361     /*  Calculate transformations for the CM of each module */
  362     /*  Calculating the transformations from the USM base frame, F_b = F_0, */
  363     /*  to the MODULE CENTER OF MASS FRAMES, F_CM_i */
  364     /*  From the tail module and forward */
  365     for (i = 0; i < 9; i++) {
  366       for (k = 0; k < 3; k++) {
  367         for (i0 = 0; i0 < 3; i0++) {
  368           g_front2back[i0 + (k << 2)] = iv3[i0 + 3 * k];
  369         }
  370   
  371         g_front2back[12 + k] = module_cm[i + 9 * k];
  372       }
  373   
  374       for (k = 0; k < 4; k++) {
  375         g_front2back[3 + (k << 2)] = iv0[k];
  376       }
  377   
  378       if (1 + i == 1) {
  379         for (k = 0; k < 4; k++) {
  380           for (i0 = 0; i0 < 4; i0++) {
  381             g_moduleCM[(i0 + (k << 2)) + (i << 4)] = g_front2back[i0 + (k << 2)];
  382           }
  383         }
  384       } else {
  385         for (k = 0; k < 4; k++) {
  386           for (i0 = 0; i0 < 4; i0++) {
  387             g_moduleCM[(k + (i0 << 2)) + (i << 4)] = 0.0;
  388             for (i1 = 0; i1 < 4; i1++) {
  389               g_moduleCM[(k + (i0 << 2)) + (i << 4)] += g_joints[(k + (i1 << 2)) +
  390                 ((i - 1) << 4)] * g_front2back[i1 + (i0 << 2)];
  391             }
  392           }
  393         }
  394       }
  395     }
  396   
  397     /*  Calculate transformations for the thrusters */
  398     /*  Calculating the transformations from the back frame to the thruster */
  399     /*  frames */
  400     /*  From the tail module and forward */
  401     /*  THRUSTER FRAMES */
  402     for (i = 0; i < 7; i++) {
  403       b_bool = false;
  404       if (thruster_ax[i] != 'Y') {
  405       } else {
  406         b_bool = true;
  407       }
  408   
  409       if (b_bool) {
  410         k = 0;
  411       } else {
  412         b_bool = false;
  413         if (thruster_ax[i] != 'Z') {
  414         } else {
  415           b_bool = true;
  416         }
  417   
  418         if (b_bool) {
  419           k = 1;
  420         } else {
  421           k = -1;
  422         }
  423       }
  424   
  425       switch (k) {
  426        case 0:
  427         for (k = 0; k < 3; k++) {
  428           for (i0 = 0; i0 < 3; i0++) {
  429             g_front2back[i0 + (k << 2)] = rotZ[i0 + 3 * k];
  430           }
  431   
  432           g_front2back[12 + k] = thruster_pos[i + 7 * k];
  433         }
  434   
  435         for (k = 0; k < 4; k++) {
  436           g_front2back[3 + (k << 2)] = iv0[k];
  437         }
  438         break;
  439   
  440        case 1:
  441         for (k = 0; k < 3; k++) {
  442           for (i0 = 0; i0 < 3; i0++) {
  443             g_front2back[i0 + (k << 2)] = rotY[i0 + 3 * k];
  444           }
  445   
  446           g_front2back[12 + k] = thruster_pos[i + 7 * k];
  447         }
  448   
  449         for (k = 0; k < 4; k++) {
  450           g_front2back[3 + (k << 2)] = iv0[k];
  451         }
  452         break;
  453   
  454        default:
  455         memset(&g_front2back[0], 0, sizeof(double) << 4);
  456         for (k = 0; k < 4; k++) {
  457           g_front2back[k + (k << 2)] = 1.0;
  458         }
  459         break;
  460       }
  461   
  462       for (k = 0; k < 4; k++) {
  463         for (i0 = 0; i0 < 4; i0++) {
  464           g_center2back[i0 + (k << 2)] = g_joints[(i0 + (k << 2)) + ((iv4[i] - 2) <<
  465             4)];
  466         }
  467       }
  468   
  469       for (k = 0; k < 4; k++) {
  470         for (i0 = 0; i0 < 4; i0++) {
  471           g_thrusters[(k + (i0 << 2)) + (i << 4)] = 0.0;
  472           for (i1 = 0; i1 < 4; i1++) {
  473             g_thrusters[(k + (i0 << 2)) + (i << 4)] += g_center2back[k + (i1 << 2)]
  474               * g_front2back[i1 + (i0 << 2)];
  475           }
  476         }
  477       }
  478     }
  479   
  480     /*  Calculate the transformation from the front end to the back end */
  481     for (k = 0; k < 3; k++) {
  482       for (i0 = 0; i0 < 3; i0++) {
  483         b_g_joints_home[i0 + 3 * k] = -g_joints_home[128 + (k + (i0 << 2))];
  484       }
  485     }
  486   
  487     for (k = 0; k < 3; k++) {
  488       c_g_joints_home[k] = 0.0;
  489       for (i0 = 0; i0 < 3; i0++) {
  490         g_front2back[i0 + (k << 2)] = g_joints_home[128 + (k + (i0 << 2))];
  491         c_g_joints_home[k] += b_g_joints_home[k + 3 * i0] * g_joints_home[140 + i0];
  492       }
  493   
  494       g_front2back[12 + k] = c_g_joints_home[k];
  495     }
  496   
  497     for (k = 0; k < 4; k++) {
  498       g_front2back[3 + (k << 2)] = iv0[k];
  499     }
  500   
  501     /*  Transformation from inertial frame to base frame */
  502     if (controlCenterLink != 0.0) {
  503       /*  Calculate the transformation from the back end to the center module */
  504       /*  (module number 5) */
  505       for (k = 0; k < 3; k++) {
  506         for (i0 = 0; i0 < 3; i0++) {
  507           b_g_joints_home[i0 + 3 * k] = -g_moduleCM[64 + (k + (i0 << 2))];
  508         }
  509       }
  510   
  511       for (k = 0; k < 3; k++) {
  512         c_g_joints_home[k] = 0.0;
  513         for (i0 = 0; i0 < 3; i0++) {
  514           g_center2back[i0 + (k << 2)] = g_moduleCM[64 + (k + (i0 << 2))];
  515           c_g_joints_home[k] += b_g_joints_home[k + 3 * i0] * g_moduleCM[76 + i0];
  516         }
  517   
  518         g_center2back[12 + k] = c_g_joints_home[k];
  519       }
  520   
  521       for (k = 0; k < 4; k++) {
  522         g_center2back[3 + (k << 2)] = iv0[k];
  523         for (i0 = 0; i0 < 4; i0++) {
  524           b_cpsi[k + (i0 << 2)] = 0.0;
  525           for (i1 = 0; i1 < 4; i1++) {
  526             b_cpsi[k + (i0 << 2)] += g_If[k + (i1 << 2)] * g_front2back[i1 + (i0 <<
  527               2)];
  528           }
  529         }
  530   
  531         for (i0 = 0; i0 < 4; i0++) {
  532           g_Ib[k + (i0 << 2)] = 0.0;
  533           for (i1 = 0; i1 < 4; i1++) {
  534             g_Ib[k + (i0 << 2)] += b_cpsi[k + (i1 << 2)] * g_moduleCM[64 + (i1 +
  535               (i0 << 2))];
  536           }
  537         }
  538       }
  539   
  540       for (i = 0; i < 7; i++) {
  541         for (k = 0; k < 4; k++) {
  542           for (i0 = 0; i0 < 4; i0++) {
  543             b_cpsi[k + (i0 << 2)] = 0.0;
  544             for (i1 = 0; i1 < 4; i1++) {
  545               b_cpsi[k + (i0 << 2)] += g_center2back[k + (i1 << 2)] * g_thrusters
  546                 [(i1 + (i0 << 2)) + (i << 4)];
  547             }
  548           }
  549         }
  550   
  551         for (k = 0; k < 4; k++) {
  552           for (i0 = 0; i0 < 4; i0++) {
  553             g_thrusters[(i0 + (k << 2)) + (i << 4)] = b_cpsi[i0 + (k << 2)];
  554           }
  555         }
  556       }
  557   
  558       for (i = 0; i < 9; i++) {
  559         for (k = 0; k < 4; k++) {
  560           for (i0 = 0; i0 < 4; i0++) {
  561             b_cpsi[k + (i0 << 2)] = 0.0;
  562             for (i1 = 0; i1 < 4; i1++) {
  563               b_cpsi[k + (i0 << 2)] += g_center2back[k + (i1 << 2)] * g_moduleCM
  564                 [(i1 + (i0 << 2)) + (i << 4)];
  565             }
  566           }
  567         }
  568   
  569         for (k = 0; k < 4; k++) {
  570           for (i0 = 0; i0 < 4; i0++) {
  571             g_moduleCM[(i0 + (k << 2)) + (i << 4)] = b_cpsi[i0 + (k << 2)];
  572           }
  573         }
  574       }
  575     } else {
  576       for (k = 0; k < 4; k++) {
  577         for (i0 = 0; i0 < 4; i0++) {
  578           g_Ib[k + (i0 << 2)] = 0.0;
  579           for (i1 = 0; i1 < 4; i1++) {
  580             g_Ib[k + (i0 << 2)] += g_If[k + (i1 << 2)] * g_front2back[i1 + (i0 <<
  581               2)];
  582           }
  583         }
  584       }
  585     }
  586   
  587     for (k = 0; k < 3; k++) {
  588       for (i0 = 0; i0 < 3; i0++) {
  589         R_Ib[i0 + 3 * k] = g_Ib[i0 + (k << 2)];
  590       }
  591   
  592       eta_b[k] = g_Ib[12 + k];
  593     }
  594   
  595     eta_b[3] = rt_atan2d_snf(g_Ib[6], g_Ib[10]);
  596     eta_b[4] = -asin(g_Ib[2]);
  597     eta_b[5] = rt_atan2d_snf(g_Ib[1], g_Ib[0]);
  598   }
  599   
  600   /* End of code generation (calculateTransformations.c) */
  601