Untitled

typedef struct rneaReturn{
  int tau_0;
  int tau_1;
  int tau_2;
  int tau_3;
  int tau_4;
  int tau_5;
} rneaReturn;


rneaReturn rnea(int32_t qcos_0, int32_t qcos_1, int32_t qcos_2, int32_t qcos_3, int32_t qcos_4, int32_t qcos_5, int32_t qsin_0, int32_t qsin_1, int32_t qsin_2, int32_t qsin_3, int32_t qsin_4, int32_t qsin_5, int32_t v_0, int32_t v_1, int32_t v_2, int32_t v_3, int32_t v_4, int32_t v_5, int32_t a_0, int32_t a_1, int32_t a_2, int32_t a_3, int32_t a_4, int32_t a_5) {
  int32_t limi_translation_0_0 = 0;
  int32_t limi_translation_0_1 = 0;
  int32_t limi_translation_0_2 = 715112055;
  int32_t limi_translation_1_0 = 0;
  int32_t limi_translation_1_1 = 0;
  int32_t limi_translation_1_2 = 0;
  int32_t limi_translation_2_0 = 0;
  int32_t limi_translation_2_1 = -678604833;
  int32_t limi_translation_2_2 = 0;
  int32_t limi_translation_3_0 = 177167401;
  int32_t limi_translation_3_1 = 0;
  int32_t limi_translation_3_2 = 0;
  int32_t limi_translation_4_0 = -177167401;
  int32_t limi_translation_4_1 = 824633721;
  int32_t limi_translation_4_2 = 0;
  int32_t limi_translation_5_0 = 0;
  int32_t limi_translation_5_1 = 0;
  int32_t limi_translation_5_2 = 0;
  int32_t data_v_0_0 = 0;
  int32_t data_v_0_1 = 0;
  int32_t data_v_0_2 = 0;
  int32_t data_v_0_3 = 0;
  int32_t data_v_0_4 = 0;
  int32_t data_v_0_5 = 0;
  int32_t data_v_1_0 = 0;
  int32_t data_v_1_1 = 0;
  int32_t data_v_1_2 = 0;
  int32_t data_v_1_3 = 0;
  int32_t data_v_1_4 = 0;
  int32_t data_v_1_5 = 0;
  int32_t data_v_2_0 = 0;
  int32_t data_v_2_1 = 0;
  int32_t data_v_2_2 = 0;
  int32_t data_v_2_3 = 0;
  int32_t data_v_2_4 = 0;
  int32_t data_v_2_5 = 0;
  int32_t data_v_3_0 = 0;
  int32_t data_v_3_1 = 0;
  int32_t data_v_3_2 = 0;
  int32_t data_v_3_3 = 0;
  int32_t data_v_3_4 = 0;
  int32_t data_v_3_5 = 0;
  int32_t data_v_4_0 = 0;
  int32_t data_v_4_1 = 0;
  int32_t data_v_4_2 = 0;
  int32_t data_v_4_3 = 0;
  int32_t data_v_4_4 = 0;
  int32_t data_v_4_5 = 0;
  int32_t data_v_5_0 = 0;
  int32_t data_v_5_1 = 0;
  int32_t data_v_5_2 = 0;
  int32_t data_v_5_3 = 0;
  int32_t data_v_5_4 = 0;
  int32_t data_v_5_5 = 0;
  int32_t lever_0_0 = 8321499;
  int32_t lever_0_1 = 4468913;
  int32_t lever_0_2 = -102263172;
  int32_t lever_1_0 = -6745247;
  int32_t lever_1_1 = -61675731;
  int32_t lever_1_2 = 7505455;
  int32_t lever_2_0 = 59094455;
  int32_t lever_2_1 = 84293028;
  int32_t lever_2_2 = -142811958;
  int32_t lever_3_0 = -114181706;
  int32_t lever_3_1 = 224238095;
  int32_t lever_3_2 = 58957016;
  int32_t lever_4_0 = -25668873;
  int32_t lever_4_1 = 88186416;
  int32_t lever_4_2 = -82542829;
  int32_t lever_5_0 = 129168994;
  int32_t lever_5_1 = -30316027;
  int32_t lever_5_2 = -22585086;
  int32_t masses_0 = 1334307826;
  int32_t masses_1 = 1389263006;
  int32_t masses_2 = 1733343574;
  int32_t masses_3 = 1926236461;
  int32_t masses_4 = 1316349494;
  int32_t masses_5 = 1789455174;
  int32_t inertia_0_0_0 = 1510475573;
  int32_t inertia_0_0_1 = -298501;
  int32_t inertia_0_0_2 = 14542759;
  int32_t inertia_0_1_0 = -298501;
  int32_t inertia_0_1_1 = 1517433421;
  int32_t inertia_0_1_2 = 41165114;
  int32_t inertia_0_2_0 = 14542759;
  int32_t inertia_0_2_1 = 41165114;
  int32_t inertia_0_2_2 = 19578608;
  int32_t inertia_1_0_0 = 17098265;
  int32_t inertia_1_0_1 = -8428874;
  int32_t inertia_1_0_2 = 22020297;
  int32_t inertia_1_1_0 = -8428874;
  int32_t inertia_1_1_1 = 60365765;
  int32_t inertia_1_1_2 = 1511828;
  int32_t inertia_1_2_0 = 22020297;
  int32_t inertia_1_2_1 = 1511828;
  int32_t inertia_1_2_2 = 55823837;
  int32_t inertia_2_0_0 = 79976586;
  int32_t inertia_2_0_1 = -10224170;
  int32_t inertia_2_0_2 = -24472724;
  int32_t inertia_2_1_0 = -10224170;
  int32_t inertia_2_1_1 = 77642271;
  int32_t inertia_2_1_2 = -27498529;
  int32_t inertia_2_2_0 = -24472724;
  int32_t inertia_2_2_1 = -27498529;
  int32_t inertia_2_2_2 = 23257248;
  int32_t inertia_3_0_0 = 55518895;
  int32_t inertia_3_0_1 = 16741783;
  int32_t inertia_3_0_2 = -2860449;
  int32_t inertia_3_1_0 = 16741783;
  int32_t inertia_3_1_1 = 41987600;
  int32_t inertia_3_1_2 = 18556406;
  int32_t inertia_3_2_0 = -2860449;
  int32_t inertia_3_2_1 = 18556406;
  int32_t inertia_3_2_2 = 60823179;
  int32_t inertia_4_0_0 = 76340896;
  int32_t inertia_4_0_1 = -4546223;
  int32_t inertia_4_0_2 = -8669392;
  int32_t inertia_4_1_0 = -4546223;
  int32_t inertia_4_1_1 = 63294933;
  int32_t inertia_4_1_2 = 491774;
  int32_t inertia_4_2_0 = -8669392;
  int32_t inertia_4_2_1 = 491774;
  int32_t inertia_4_2_2 = 18526341;
  int32_t inertia_5_0_0 = 4217658;
  int32_t inertia_5_0_1 = 234076;
  int32_t inertia_5_0_2 = -2486787;
  int32_t inertia_5_1_0 = 234076;
  int32_t inertia_5_1_1 = 9350144;
  int32_t inertia_5_1_2 = 732292;
  int32_t inertia_5_2_0 = -2486787;
  int32_t inertia_5_2_1 = 732292;
  int32_t inertia_5_2_2 = 11667279;
  int32_t parent_v_0 = 0;
  int32_t parent_v_1 = 0;
  int32_t parent_v_2 = 0;
  int32_t parent_v_3 = 0;
  int32_t parent_v_4 = 0;
  int32_t parent_v_5 = 0;
  int32_t parent_a_gf_0 = 0;
  int32_t parent_a_gf_1 = 0;
  int32_t parent_a_gf_2 = 1316675912;
  int32_t parent_a_gf_3 = 0;
  int32_t parent_a_gf_4 = 0;
  int32_t parent_a_gf_5 = 0;
  int32_t new_v_0 = 0;
  int32_t new_v_1 = 0;
  int32_t new_v_2 = 0;
  int32_t new_v_3 = 0;
  int32_t new_v_4 = 0;
  int32_t new_v_5 = 0;
  int32_t new_a_gf_0 = 0;
  int32_t new_a_gf_1 = 0;
  int32_t new_a_gf_2 = 0;
  int32_t new_a_gf_3 = 0;
  int32_t new_a_gf_4 = 0;
  int32_t new_a_gf_5 = 0;
  int32_t new_h_0 = 0;
  int32_t new_h_1 = 0;
  int32_t new_h_2 = 0;
  int32_t new_h_3 = 0;
  int32_t new_h_4 = 0;
  int32_t new_h_5 = 0;
  int32_t new_f_0 = 0;
  int32_t new_f_1 = 0;
  int32_t new_f_2 = 0;
  int32_t new_f_3 = 0;
  int32_t new_f_4 = 0;
  int32_t new_f_5 = 0;
  int32_t rotation_matrix_0_0_0 = qcos_0;
  int32_t rotation_matrix_0_0_1 = -(qsin_0);
  int32_t rotation_matrix_0_0_2 = 0;
  int32_t rotation_matrix_0_1_0 = qsin_0;
  int32_t rotation_matrix_0_1_1 = qcos_0;
  int32_t rotation_matrix_0_1_2 = 0;
  int32_t rotation_matrix_0_2_0 = 0;
  int32_t rotation_matrix_0_2_1 = 0;
  int32_t rotation_matrix_0_2_2 = 1073741824;
  int32_t limi_rotation_0_0_0 = rotation_matrix_0_0_0;
  int32_t limi_rotation_0_0_1 = rotation_matrix_0_0_1;
  int32_t limi_rotation_0_0_2 = 0;
  int32_t limi_rotation_0_1_0 = rotation_matrix_0_1_0;
  int32_t limi_rotation_0_1_1 = rotation_matrix_0_1_1;
  int32_t limi_rotation_0_1_2 = 0;
  int32_t limi_rotation_0_2_0 = 0;
  int32_t limi_rotation_0_2_1 = 0;
  int32_t limi_rotation_0_2_2 = 1073741824;
  int32_t v_linear_0_0 = data_v_0_0;
  int32_t v_linear_0_1 = data_v_0_1;
  int32_t v_linear_0_2 = data_v_0_2;
  int32_t v_angular_0_0 = data_v_0_3;
  int32_t v_angular_0_1 = data_v_0_4;
  int32_t v_angular_0_2 = v_0;
  int32_t parent_v_linear_0_0 = parent_v_0;
  int32_t parent_v_linear_0_1 = parent_v_1;
  int32_t parent_v_linear_0_2 = parent_v_2;
  int32_t parent_v_angular_0_0 = parent_v_3;
  int32_t parent_v_angular_0_1 = parent_v_4;
  int32_t parent_v_angular_0_2 = parent_v_5;
  int32_t parent_a_gf_linear_0_0 = parent_a_gf_0;
  int32_t parent_a_gf_linear_0_1 = parent_a_gf_1;
  int32_t parent_a_gf_linear_0_2 = parent_a_gf_2;
  int32_t parent_a_gf_angular_0_0 = parent_a_gf_3;
  int32_t parent_a_gf_angular_0_1 = parent_a_gf_4;
  int32_t parent_a_gf_angular_0_2 = parent_a_gf_5;
  int32_t minus_m_w_0 = -(v_0);
  int32_t _tmp = -(minus_m_w_0);
  int32_t alpha_cross1_linear_0 = (int32_t) ((((int64_t) (_tmp) * (int64_t) (v_linear_0_1)) >> 28));
  int32_t alpha_cross2_linear_0 = (int32_t) ((((int64_t) (minus_m_w_0) * (int64_t) (v_linear_0_0)) >> 28));
  int32_t alpha_cross_linear_0_0 = alpha_cross1_linear_0;
  int32_t alpha_cross_linear_0_1 = alpha_cross2_linear_0;
  int32_t alpha_cross_linear_0_2 = 0;
  int32_t _tmp1 = -(minus_m_w_0);
  int32_t alpha_cross1_angular_0 = (int32_t) ((((int64_t) (_tmp1) * (int64_t) (v_angular_0_1)) >> 28));
  int32_t alpha_cross2_angular_0 = (int32_t) ((((int64_t) (minus_m_w_0) * (int64_t) (v_angular_0_0)) >> 28));
  int32_t alpha_cross_angular_0_0 = alpha_cross1_angular_0;
  int32_t alpha_cross_angular_0_1 = alpha_cross2_angular_0;
  int32_t alpha_cross_angular_0_2 = 0;
  int32_t r_1_new_a_gf_0_0 = alpha_cross_linear_0_0;
  int32_t r_1_new_a_gf_0_1 = alpha_cross_linear_0_1;
  int32_t r_1_new_a_gf_0_2 = alpha_cross_linear_0_2;
  int32_t r_1_new_a_gf_0_3 = alpha_cross_angular_0_0;
  int32_t r_1_new_a_gf_0_4 = alpha_cross_angular_0_1;
  int32_t r_1_new_a_gf_0_5 = alpha_cross_angular_0_2;
  int32_t new_a_gf_up1_0 = (a_0 + (r_1_new_a_gf_0_5 >> 3));
  int32_t new_a_gf2_linear_0_0 = r_1_new_a_gf_0_0;
  int32_t new_a_gf2_linear_0_1 = r_1_new_a_gf_0_1;
  int32_t new_a_gf2_linear_0_2 = r_1_new_a_gf_0_2;
  int32_t new_a_gf2_angular_0_0 = r_1_new_a_gf_0_3;
  int32_t new_a_gf2_angular_0_1 = r_1_new_a_gf_0_4;
  int32_t new_a_gf2_angular_0_2 = new_a_gf_up1_0;
  int32_t _tmp2 = (int32_t) ((((int64_t) (limi_translation_0_0) * (int64_t) (parent_a_gf_angular_0_1)) >> 31));
  int32_t _tmp3 = (int32_t) ((((int64_t) (limi_translation_0_1) * (int64_t) (parent_a_gf_angular_0_0)) >> 31));
  int32_t actInv1_0_0 = (_tmp2 - _tmp3);
  int32_t _tmp4 = (int32_t) ((((int64_t) (limi_translation_0_2) * (int64_t) (parent_a_gf_angular_0_0)) >> 31));
  int32_t _tmp5 = (int32_t) ((((int64_t) (limi_translation_0_0) * (int64_t) (parent_a_gf_angular_0_2)) >> 31));
  int32_t actInv1_0_1 = (_tmp4 - _tmp5);
  int32_t _tmp6 = (int32_t) ((((int64_t) (limi_translation_0_0) * (int64_t) (parent_a_gf_angular_0_1)) >> 31));
  int32_t _tmp7 = (int32_t) ((((int64_t) (limi_translation_0_1) * (int64_t) (parent_a_gf_angular_0_0)) >> 31));
  int32_t actInv1_0_2 = (_tmp6 - _tmp7);
  int32_t actInv2_0_0 = (parent_a_gf_linear_0_0 - actInv1_0_0);
  int32_t actInv2_0_1 = (parent_a_gf_linear_0_1 - actInv1_0_1);
  int32_t actInv2_0_2 = (parent_a_gf_linear_0_2 - (actInv1_0_2 >> 4));
  int32_t actInv3_0_0_0 = limi_rotation_0_0_0;
  int32_t actInv3_0_1_0 = limi_rotation_0_0_1;
  int32_t actInv3_0_2_0 = limi_rotation_0_0_2;
  int32_t actInv3_0_0_1 = limi_rotation_0_1_0;
  int32_t actInv3_0_1_1 = limi_rotation_0_1_1;
  int32_t actInv3_0_2_1 = limi_rotation_0_1_2;
  int32_t actInv3_0_0_2 = limi_rotation_0_2_0;
  int32_t actInv3_0_1_2 = limi_rotation_0_2_1;
  int32_t actInv3_0_2_2 = limi_rotation_0_2_2;
  int32_t _tmp8 = (int32_t) ((((int64_t) (actInv3_0_0_0) * (int64_t) (actInv2_0_0)) >> 30));
  int32_t _tmp9 = (int32_t) ((((int64_t) (actInv3_0_0_1) * (int64_t) (actInv2_0_1)) >> 30));
  int32_t _tmp10 = (_tmp8 + _tmp9);
  int32_t _tmp11 = (int32_t) ((((int64_t) (actInv3_0_0_2) * (int64_t) (actInv2_0_2)) >> 27));
  int32_t actInv4_0_0 = (_tmp10 + _tmp11);
  int32_t _tmp12 = (int32_t) ((((int64_t) (actInv3_0_1_0) * (int64_t) (actInv2_0_0)) >> 30));
  int32_t _tmp13 = (int32_t) ((((int64_t) (actInv3_0_1_1) * (int64_t) (actInv2_0_1)) >> 30));
  int32_t _tmp14 = (_tmp12 + _tmp13);
  int32_t _tmp15 = (int32_t) ((((int64_t) (actInv3_0_1_2) * (int64_t) (actInv2_0_2)) >> 27));
  int32_t actInv4_0_1 = (_tmp14 + _tmp15);
  int32_t _tmp16 = (int32_t) ((((int64_t) (actInv3_0_2_0) * (int64_t) (actInv2_0_0)) >> 31));
  int32_t _tmp17 = (int32_t) ((((int64_t) (actInv3_0_2_1) * (int64_t) (actInv2_0_1)) >> 31));
  int32_t _tmp18 = (_tmp16 + _tmp17);
  int32_t _tmp19 = (int32_t) ((((int64_t) (actInv3_0_2_2) * (int64_t) (actInv2_0_2)) >> 30));
  int32_t actInv4_0_2 = ((_tmp18 >> 4) + _tmp19);
  int32_t act_inv_linear_0_0 = (new_a_gf2_linear_0_0 + actInv4_0_0);
  int32_t act_inv_linear_0_1 = (new_a_gf2_linear_0_1 + actInv4_0_1);
  int32_t act_inv_linear_0_2 = ((new_a_gf2_linear_0_2 >> 4) + actInv4_0_2);
  int32_t _tmp20 = (int32_t) ((((int64_t) (actInv3_0_0_0) * (int64_t) (parent_a_gf_angular_0_0)) >> 30));
  int32_t _tmp21 = (int32_t) ((((int64_t) (actInv3_0_0_1) * (int64_t) (parent_a_gf_angular_0_1)) >> 30));
  int32_t _tmp22 = (_tmp20 + _tmp21);
  int32_t _tmp23 = (int32_t) ((((int64_t) (actInv3_0_0_2) * (int64_t) (parent_a_gf_angular_0_2)) >> 31));
  int32_t actInv5_0_0 = (_tmp22 + _tmp23);
  int32_t _tmp24 = (int32_t) ((((int64_t) (actInv3_0_1_0) * (int64_t) (parent_a_gf_angular_0_0)) >> 30));
  int32_t _tmp25 = (int32_t) ((((int64_t) (actInv3_0_1_1) * (int64_t) (parent_a_gf_angular_0_1)) >> 30));
  int32_t _tmp26 = (_tmp24 + _tmp25);
  int32_t _tmp27 = (int32_t) ((((int64_t) (actInv3_0_1_2) * (int64_t) (parent_a_gf_angular_0_2)) >> 31));
  int32_t actInv5_0_1 = (_tmp26 + _tmp27);
  int32_t _tmp28 = (int32_t) ((((int64_t) (actInv3_0_2_0) * (int64_t) (parent_a_gf_angular_0_0)) >> 31));
  int32_t _tmp29 = (int32_t) ((((int64_t) (actInv3_0_2_1) * (int64_t) (parent_a_gf_angular_0_1)) >> 31));
  int32_t _tmp30 = (_tmp28 + _tmp29);
  int32_t _tmp31 = (int32_t) ((((int64_t) (actInv3_0_2_2) * (int64_t) (parent_a_gf_angular_0_2)) >> 30));
  int32_t actInv5_0_2 = (_tmp30 + _tmp31);
  int32_t act_inv_angular_0_0 = (new_a_gf2_angular_0_0 + actInv5_0_0);
  int32_t act_inv_angular_0_1 = (new_a_gf2_angular_0_1 + actInv5_0_1);
  int32_t act_inv_angular_0_2 = (new_a_gf2_angular_0_2 + (actInv5_0_2 >> 3));
  int32_t new_a_gf_up3_0_0 = act_inv_linear_0_0;
  int32_t new_a_gf_up3_0_1 = act_inv_linear_0_1;
  int32_t new_a_gf_up3_0_2 = act_inv_linear_0_2;
  int32_t new_a_gf_up3_0_3 = act_inv_angular_0_0;
  int32_t new_a_gf_up3_0_4 = act_inv_angular_0_1;
  int32_t new_a_gf_up3_0_5 = act_inv_angular_0_2;
  int32_t _tmp32 = (int32_t) ((((int64_t) (lever_0_0) * (int64_t) (v_angular_0_1)) >> 31));
  int32_t _tmp33 = (int32_t) ((((int64_t) (lever_0_1) * (int64_t) (v_angular_0_0)) >> 31));
  int32_t h_linear_1_0_0 = (_tmp32 - _tmp33);
  int32_t _tmp34 = (int32_t) ((((int64_t) (lever_0_2) * (int64_t) (v_angular_0_0)) >> 31));
  int32_t _tmp35 = (int32_t) ((((int64_t) (lever_0_0) * (int64_t) (v_angular_0_2)) >> 28));
  int32_t h_linear_1_0_1 = (_tmp34 - _tmp35);
  int32_t _tmp36 = (int32_t) ((((int64_t) (lever_0_0) * (int64_t) (v_angular_0_1)) >> 31));
  int32_t _tmp37 = (int32_t) ((((int64_t) (lever_0_1) * (int64_t) (v_angular_0_0)) >> 31));
  int32_t h_linear_1_0_2 = (_tmp36 - _tmp37);
  int32_t h_linear_2_0_0 = (v_linear_0_0 - h_linear_1_0_0);
  int32_t h_linear_2_0_1 = (v_linear_0_1 - h_linear_1_0_1);
  int32_t h_linear_2_0_2 = (v_linear_0_2 - h_linear_1_0_2);
  int32_t h_linear_0_0 = (int32_t) ((((int64_t) (masses_0) * (int64_t) (h_linear_2_0_0)) >> 28));
  int32_t h_linear_0_1 = (int32_t) ((((int64_t) (masses_0) * (int64_t) (h_linear_2_0_1)) >> 28));
  int32_t h_linear_0_2 = (int32_t) ((((int64_t) (masses_0) * (int64_t) (h_linear_2_0_2)) >> 28));
  int32_t r_2_ = (int32_t) ((((int64_t) (inertia_0_0_0) * (int64_t) (v_angular_0_0)) >> 31));
  int32_t r_3_ = (int32_t) ((((int64_t) (inertia_0_0_1) * (int64_t) (v_angular_0_1)) >> 31));
  int32_t r_4_ = (int32_t) ((((int64_t) (inertia_0_0_2) * (int64_t) (v_angular_0_2)) >> 28));
  int32_t r_5_ = (int32_t) ((((int64_t) (inertia_0_0_1) * (int64_t) (v_angular_0_0)) >> 31));
  int32_t r_6_ = (int32_t) ((((int64_t) (inertia_0_1_1) * (int64_t) (v_angular_0_1)) >> 31));
  int32_t r_7_ = (int32_t) ((((int64_t) (inertia_0_1_2) * (int64_t) (v_angular_0_2)) >> 28));
  int32_t r_8_ = (int32_t) ((((int64_t) (inertia_0_0_2) * (int64_t) (v_angular_0_0)) >> 31));
  int32_t r_9_ = (int32_t) ((((int64_t) (inertia_0_1_2) * (int64_t) (v_angular_0_1)) >> 31));
  int32_t r_10_ = (int32_t) ((((int64_t) (inertia_0_2_2) * (int64_t) (v_angular_0_2)) >> 28));
  int32_t rhsMult1_temp_0 = (r_2_ + r_3_);
  int32_t rhsMult1_0 = (rhsMult1_temp_0 + r_4_);
  int32_t rhsMult2_temp_0 = (r_5_ + r_6_);
  int32_t rhsMult2_0 = (rhsMult2_temp_0 + r_7_);
  int32_t rhsMult3_temp_0 = (r_8_ + r_9_);
  int32_t rhsMult3_0 = (rhsMult3_temp_0 + r_10_);
  int32_t rhsMult_0_0 = rhsMult1_0;
  int32_t rhsMult_0_1 = rhsMult2_0;
  int32_t rhsMult_0_2 = rhsMult3_0;
  int32_t h_angular_first_0_0 = rhsMult_0_0;
  int32_t h_angular_first_0_1 = rhsMult_0_1;
  int32_t h_angular_first_0_2 = rhsMult_0_2;
  int32_t _tmp38 = (int32_t) ((((int64_t) (lever_0_0) * (int64_t) (h_linear_0_1)) >> 31));
  int32_t _tmp39 = (int32_t) ((((int64_t) (lever_0_1) * (int64_t) (h_linear_0_0)) >> 31));
  int32_t h_angular_1_0_0 = (_tmp38 - _tmp39);
  int32_t _tmp40 = (int32_t) ((((int64_t) (lever_0_2) * (int64_t) (h_linear_0_0)) >> 31));
  int32_t _tmp41 = (int32_t) ((((int64_t) (lever_0_0) * (int64_t) (h_linear_0_2)) >> 31));
  int32_t h_angular_1_0_1 = (_tmp40 - _tmp41);
  int32_t _tmp42 = (int32_t) ((((int64_t) (lever_0_0) * (int64_t) (h_linear_0_1)) >> 31));
  int32_t _tmp43 = (int32_t) ((((int64_t) (lever_0_1) * (int64_t) (h_linear_0_0)) >> 31));
  int32_t h_angular_1_0_2 = (_tmp42 - _tmp43);
  int32_t h_angular_0_0 = (h_angular_first_0_0 + h_angular_1_0_0);
  int32_t h_angular_0_1 = (h_angular_first_0_1 + h_angular_1_0_1);
  int32_t h_angular_0_2 = (h_angular_first_0_2 + h_angular_1_0_2);
  int32_t _tmp44 = (int32_t) ((((int64_t) (lever_0_0) * (int64_t) (act_inv_angular_0_1)) >> 31));
  int32_t _tmp45 = (int32_t) ((((int64_t) (lever_0_1) * (int64_t) (act_inv_angular_0_0)) >> 31));
  int32_t f_linear_1_0_0 = (_tmp44 - _tmp45);
  int32_t _tmp46 = (int32_t) ((((int64_t) (lever_0_2) * (int64_t) (act_inv_angular_0_0)) >> 31));
  int32_t _tmp47 = (int32_t) ((((int64_t) (lever_0_0) * (int64_t) (act_inv_angular_0_2)) >> 28));
  int32_t f_linear_1_0_1 = (_tmp46 - _tmp47);
  int32_t _tmp48 = (int32_t) ((((int64_t) (lever_0_0) * (int64_t) (act_inv_angular_0_1)) >> 31));
  int32_t _tmp49 = (int32_t) ((((int64_t) (lever_0_1) * (int64_t) (act_inv_angular_0_0)) >> 31));
  int32_t f_linear_1_0_2 = (_tmp48 - _tmp49);
  int32_t f_linear_2_0_0 = (act_inv_linear_0_0 - f_linear_1_0_0);
  int32_t f_linear_2_0_1 = (act_inv_linear_0_1 - f_linear_1_0_1);
  int32_t f_linear_2_0_2 = (act_inv_linear_0_2 - (f_linear_1_0_2 >> 4));
  int32_t f_linear_3_0_0 = (int32_t) ((((int64_t) (masses_0) * (int64_t) (f_linear_2_0_0)) >> 28));
  int32_t f_linear_3_0_1 = (int32_t) ((((int64_t) (masses_0) * (int64_t) (f_linear_2_0_1)) >> 28));
  int32_t f_linear_3_0_2 = (int32_t) ((((int64_t) (masses_0) * (int64_t) (f_linear_2_0_2)) >> 30));
  int32_t r_11_ = (int32_t) ((((int64_t) (inertia_0_0_0) * (int64_t) (act_inv_angular_0_0)) >> 31));
  int32_t r_12_ = (int32_t) ((((int64_t) (inertia_0_0_1) * (int64_t) (act_inv_angular_0_1)) >> 31));
  int32_t r_13_ = (int32_t) ((((int64_t) (inertia_0_0_2) * (int64_t) (act_inv_angular_0_2)) >> 28));
  int32_t r_14_ = (int32_t) ((((int64_t) (inertia_0_0_1) * (int64_t) (act_inv_angular_0_0)) >> 31));
  int32_t r_15_ = (int32_t) ((((int64_t) (inertia_0_1_1) * (int64_t) (act_inv_angular_0_1)) >> 31));
  int32_t r_16_ = (int32_t) ((((int64_t) (inertia_0_1_2) * (int64_t) (act_inv_angular_0_2)) >> 28));
  int32_t r_17_ = (int32_t) ((((int64_t) (inertia_0_0_2) * (int64_t) (act_inv_angular_0_0)) >> 31));
  int32_t r_18_ = (int32_t) ((((int64_t) (inertia_0_1_2) * (int64_t) (act_inv_angular_0_1)) >> 31));
  int32_t r_19_ = (int32_t) ((((int64_t) (inertia_0_2_2) * (int64_t) (act_inv_angular_0_2)) >> 28));
  int32_t r_20_rhsMult1_temp_0 = (r_11_ + r_12_);
  int32_t r_21_rhsMult1_0 = (r_20_rhsMult1_temp_0 + r_13_);
  int32_t r_22_rhsMult2_temp_0 = (r_14_ + r_15_);
  int32_t r_23_rhsMult2_0 = (r_22_rhsMult2_temp_0 + r_16_);
  int32_t r_24_rhsMult3_temp_0 = (r_17_ + r_18_);
  int32_t r_25_rhsMult3_0 = (r_24_rhsMult3_temp_0 + r_19_);
  int32_t r_26_rhsMult_0_0 = r_21_rhsMult1_0;
  int32_t r_26_rhsMult_0_1 = r_23_rhsMult2_0;
  int32_t r_26_rhsMult_0_2 = r_25_rhsMult3_0;
  int32_t f_angular_first_0_0 = r_26_rhsMult_0_0;
  int32_t f_angular_first_0_1 = r_26_rhsMult_0_1;
  int32_t f_angular_first_0_2 = r_26_rhsMult_0_2;
  int32_t _tmp50 = (int32_t) ((((int64_t) (lever_0_0) * (int64_t) (f_linear_3_0_1)) >> 31));
  int32_t _tmp51 = (int32_t) ((((int64_t) (lever_0_1) * (int64_t) (f_linear_3_0_0)) >> 31));
  int32_t f_angular_1_0_0 = (_tmp50 - _tmp51);
  int32_t _tmp52 = (int32_t) ((((int64_t) (lever_0_2) * (int64_t) (f_linear_3_0_0)) >> 31));
  int32_t _tmp53 = (int32_t) ((((int64_t) (lever_0_0) * (int64_t) (f_linear_3_0_2)) >> 25));
  int32_t f_angular_1_0_1 = (_tmp52 - _tmp53);
  int32_t _tmp54 = (int32_t) ((((int64_t) (lever_0_0) * (int64_t) (f_linear_3_0_1)) >> 31));
  int32_t _tmp55 = (int32_t) ((((int64_t) (lever_0_1) * (int64_t) (f_linear_3_0_0)) >> 31));
  int32_t f_angular_1_0_2 = (_tmp54 - _tmp55);
  int32_t f_angular_2_0_0 = (f_angular_first_0_0 + f_angular_1_0_0);
  int32_t f_angular_2_0_1 = (f_angular_first_0_1 + f_angular_1_0_1);
  int32_t f_angular_2_0_2 = (f_angular_first_0_2 + f_angular_1_0_2);
  int32_t _tmp56 = (int32_t) ((((int64_t) (v_angular_0_0) * (int64_t) (h_linear_0_1)) >> 31));
  int32_t _tmp57 = (int32_t) ((((int64_t) (v_angular_0_1) * (int64_t) (h_linear_0_0)) >> 31));
  int32_t f_linear_4_temp_0_0 = (_tmp56 - _tmp57);
  int32_t _tmp58 = (int32_t) ((((int64_t) (v_angular_0_2) * (int64_t) (h_linear_0_0)) >> 28));
  int32_t _tmp59 = (int32_t) ((((int64_t) (v_angular_0_0) * (int64_t) (h_linear_0_2)) >> 31));
  int32_t f_linear_4_temp_0_1 = (_tmp58 - _tmp59);
  int32_t _tmp60 = (int32_t) ((((int64_t) (v_angular_0_0) * (int64_t) (h_linear_0_1)) >> 31));
  int32_t _tmp61 = (int32_t) ((((int64_t) (v_angular_0_1) * (int64_t) (h_linear_0_0)) >> 31));
  int32_t f_linear_4_temp_0_2 = (_tmp60 - _tmp61);
  int32_t f_linear_4_0_0 = (f_linear_3_0_0 + f_linear_4_temp_0_0);
  int32_t f_linear_4_0_1 = (f_linear_3_0_1 + f_linear_4_temp_0_1);
  int32_t f_linear_4_0_2 = (f_linear_3_0_2 + (f_linear_4_temp_0_2 >> 6));
  int32_t _tmp62 = (int32_t) ((((int64_t) (v_angular_0_0) * (int64_t) (h_angular_first_0_1)) >> 31));
  int32_t _tmp63 = (int32_t) ((((int64_t) (v_angular_0_1) * (int64_t) (h_angular_first_0_0)) >> 31));
  int32_t f_angular_3_temp_0_0 = (_tmp62 - _tmp63);
  int32_t _tmp64 = (int32_t) ((((int64_t) (v_angular_0_2) * (int64_t) (h_angular_first_0_0)) >> 28));
  int32_t _tmp65 = (int32_t) ((((int64_t) (v_angular_0_0) * (int64_t) (h_angular_first_0_2)) >> 31));
  int32_t f_angular_3_temp_0_1 = (_tmp64 - _tmp65);
  int32_t _tmp66 = (int32_t) ((((int64_t) (v_angular_0_0) * (int64_t) (h_angular_first_0_1)) >> 31));
  int32_t _tmp67 = (int32_t) ((((int64_t) (v_angular_0_1) * (int64_t) (h_angular_first_0_0)) >> 31));
  int32_t f_angular_3_temp_0_2 = (_tmp66 - _tmp67);
  int32_t f_angular_3_0_0 = (f_angular_2_0_0 + f_angular_3_temp_0_0);
  int32_t f_angular_3_0_1 = (f_angular_2_0_1 + f_angular_3_temp_0_1);
  int32_t f_angular_3_0_2 = (f_angular_2_0_2 + f_angular_3_temp_0_2);
  int32_t _tmp68 = (int32_t) ((((int64_t) (v_linear_0_0) * (int64_t) (h_linear_0_1)) >> 31));
  int32_t _tmp69 = (int32_t) ((((int64_t) (v_linear_0_1) * (int64_t) (h_linear_0_0)) >> 31));
  int32_t f_angular_4_temp_0_0 = (_tmp68 - _tmp69);
  int32_t _tmp70 = (int32_t) ((((int64_t) (v_linear_0_2) * (int64_t) (h_linear_0_0)) >> 31));
  int32_t _tmp71 = (int32_t) ((((int64_t) (v_linear_0_0) * (int64_t) (h_linear_0_2)) >> 31));
  int32_t f_angular_4_temp_0_1 = (_tmp70 - _tmp71);
  int32_t _tmp72 = (int32_t) ((((int64_t) (v_linear_0_0) * (int64_t) (h_linear_0_1)) >> 31));
  int32_t _tmp73 = (int32_t) ((((int64_t) (v_linear_0_1) * (int64_t) (h_linear_0_0)) >> 31));
  int32_t f_angular_4_temp_0_2 = (_tmp72 - _tmp73);
  int32_t f_angular_4_0_0 = (f_angular_3_0_0 + f_angular_4_temp_0_0);
  int32_t f_angular_4_0_1 = (f_angular_3_0_1 + f_angular_4_temp_0_1);
  int32_t f_angular_4_0_2 = (f_angular_3_0_2 + f_angular_4_temp_0_2);
  int32_t h_rnea_firstpass0_0 = h_linear_0_0;
  int32_t h_rnea_firstpass0_1 = h_linear_0_1;
  int32_t h_rnea_firstpass0_2 = h_linear_0_2;
  int32_t h_rnea_firstpass0_3 = h_angular_0_0;
  int32_t h_rnea_firstpass0_4 = h_angular_0_1;
  int32_t h_rnea_firstpass0_5 = h_angular_0_2;
  int32_t f_rnea_firstpass0_0 = f_linear_4_0_0;
  int32_t f_rnea_firstpass0_1 = f_linear_4_0_1;
  int32_t f_rnea_firstpass0_2 = f_linear_4_0_2;
  int32_t f_rnea_firstpass0_3 = f_angular_4_0_0;
  int32_t f_rnea_firstpass0_4 = f_angular_4_0_1;
  int32_t f_rnea_firstpass0_5 = f_angular_4_0_2;
  int32_t r_27_new_v_0_0 = v_linear_0_0;
  int32_t r_27_new_v_0_1 = v_linear_0_1;
  int32_t r_27_new_v_0_2 = v_linear_0_2;
  int32_t r_27_new_v_0_3 = v_angular_0_0;
  int32_t r_27_new_v_0_4 = v_angular_0_1;
  int32_t r_27_new_v_0_5 = v_angular_0_2;
  int32_t rotation_matrix_1_0_0 = qcos_1;
  int32_t rotation_matrix_1_0_1 = -(qsin_1);
  int32_t rotation_matrix_1_0_2 = 0;
  int32_t rotation_matrix_1_1_0 = qsin_1;
  int32_t rotation_matrix_1_1_1 = qcos_1;
  int32_t rotation_matrix_1_1_2 = 0;
  int32_t rotation_matrix_1_2_0 = 0;
  int32_t rotation_matrix_1_2_1 = 0;
  int32_t rotation_matrix_1_2_2 = 1073741824;
  int32_t limi_rotation_1_0_0 = rotation_matrix_1_0_0;
  int32_t limi_rotation_1_0_1 = rotation_matrix_1_0_1;
  int32_t limi_rotation_1_0_2 = 0;
  int32_t limi_rotation_1_1_0 = 0;
  int32_t limi_rotation_1_1_1 = 0;
  int32_t limi_rotation_1_1_2 = 1073741824;
  int32_t limi_rotation_1_2_0 = -(rotation_matrix_1_1_0);
  int32_t limi_rotation_1_2_1 = -(rotation_matrix_1_1_1);
  int32_t limi_rotation_1_2_2 = 0;
  int32_t v_linear_1_0 = data_v_1_0;
  int32_t v_linear_1_1 = data_v_1_1;
  int32_t v_linear_1_2 = data_v_1_2;
  int32_t v_angular_1_0 = data_v_1_3;
  int32_t v_angular_1_1 = data_v_1_4;
  int32_t v_angular_1_2 = v_1;
  int32_t parent_v_linear_1_0 = r_27_new_v_0_0;
  int32_t parent_v_linear_1_1 = r_27_new_v_0_1;
  int32_t parent_v_linear_1_2 = r_27_new_v_0_2;
  int32_t parent_v_angular_1_0 = r_27_new_v_0_3;
  int32_t parent_v_angular_1_1 = r_27_new_v_0_4;
  int32_t parent_v_angular_1_2 = r_27_new_v_0_5;
  int32_t parent_a_gf_linear_1_0 = new_a_gf_up3_0_0;
  int32_t parent_a_gf_linear_1_1 = new_a_gf_up3_0_1;
  int32_t parent_a_gf_linear_1_2 = new_a_gf_up3_0_2;
  int32_t parent_a_gf_angular_1_0 = new_a_gf_up3_0_3;
  int32_t parent_a_gf_angular_1_1 = new_a_gf_up3_0_4;
  int32_t parent_a_gf_angular_1_2 = new_a_gf_up3_0_5;
  int32_t _tmp74 = (int32_t) ((((int64_t) (limi_translation_1_0) * (int64_t) (parent_v_angular_1_1)) >> 31));
  int32_t _tmp75 = (int32_t) ((((int64_t) (limi_translation_1_1) * (int64_t) (parent_v_angular_1_0)) >> 31));
  int32_t actInv1_1_0 = (_tmp74 - _tmp75);
  int32_t _tmp76 = (int32_t) ((((int64_t) (limi_translation_1_2) * (int64_t) (parent_v_angular_1_0)) >> 31));
  int32_t _tmp77 = (int32_t) ((((int64_t) (limi_translation_1_0) * (int64_t) (parent_v_angular_1_2)) >> 28));
  int32_t actInv1_1_1 = (_tmp76 - _tmp77);
  int32_t _tmp78 = (int32_t) ((((int64_t) (limi_translation_1_0) * (int64_t) (parent_v_angular_1_1)) >> 31));
  int32_t _tmp79 = (int32_t) ((((int64_t) (limi_translation_1_1) * (int64_t) (parent_v_angular_1_0)) >> 31));
  int32_t actInv1_1_2 = (_tmp78 - _tmp79);
  int32_t actInv2_1_0 = (parent_v_linear_1_0 - actInv1_1_0);
  int32_t actInv2_1_1 = (parent_v_linear_1_1 - actInv1_1_1);
  int32_t actInv2_1_2 = (parent_v_linear_1_2 - actInv1_1_2);
  int32_t actInv3_1_0_0 = limi_rotation_1_0_0;
  int32_t actInv3_1_1_0 = limi_rotation_1_0_1;
  int32_t actInv3_1_2_0 = limi_rotation_1_0_2;
  int32_t actInv3_1_0_1 = limi_rotation_1_1_0;
  int32_t actInv3_1_1_1 = limi_rotation_1_1_1;
  int32_t actInv3_1_2_1 = limi_rotation_1_1_2;
  int32_t actInv3_1_0_2 = limi_rotation_1_2_0;
  int32_t actInv3_1_1_2 = limi_rotation_1_2_1;
  int32_t actInv3_1_2_2 = limi_rotation_1_2_2;
  int32_t _tmp80 = (int32_t) ((((int64_t) (actInv3_1_0_0) * (int64_t) (actInv2_1_0)) >> 30));
  int32_t _tmp81 = (int32_t) ((((int64_t) (actInv3_1_0_1) * (int64_t) (actInv2_1_1)) >> 31));
  int32_t _tmp82 = (_tmp80 + _tmp81);
  int32_t _tmp83 = (int32_t) ((((int64_t) (actInv3_1_0_2) * (int64_t) (actInv2_1_2)) >> 30));
  int32_t actInv4_1_0 = (_tmp82 + _tmp83);
  int32_t _tmp84 = (int32_t) ((((int64_t) (actInv3_1_1_0) * (int64_t) (actInv2_1_0)) >> 30));
  int32_t _tmp85 = (int32_t) ((((int64_t) (actInv3_1_1_1) * (int64_t) (actInv2_1_1)) >> 31));
  int32_t _tmp86 = (_tmp84 + _tmp85);
  int32_t _tmp87 = (int32_t) ((((int64_t) (actInv3_1_1_2) * (int64_t) (actInv2_1_2)) >> 30));
  int32_t actInv4_1_1 = (_tmp86 + _tmp87);
  int32_t _tmp88 = (int32_t) ((((int64_t) (actInv3_1_2_0) * (int64_t) (actInv2_1_0)) >> 31));
  int32_t _tmp89 = (int32_t) ((((int64_t) (actInv3_1_2_1) * (int64_t) (actInv2_1_1)) >> 30));
  int32_t _tmp90 = (_tmp88 + _tmp89);
  int32_t _tmp91 = (int32_t) ((((int64_t) (actInv3_1_2_2) * (int64_t) (actInv2_1_2)) >> 31));
  int32_t actInv4_1_2 = (_tmp90 + _tmp91);
  int32_t act_inv_linear_1_0 = (v_linear_1_0 + actInv4_1_0);
  int32_t act_inv_linear_1_1 = (v_linear_1_1 + actInv4_1_1);
  int32_t act_inv_linear_1_2 = (v_linear_1_2 + actInv4_1_2);
  int32_t _tmp92 = (int32_t) ((((int64_t) (actInv3_1_0_0) * (int64_t) (parent_v_angular_1_0)) >> 30));
  int32_t _tmp93 = (int32_t) ((((int64_t) (actInv3_1_0_1) * (int64_t) (parent_v_angular_1_1)) >> 31));
  int32_t _tmp94 = (_tmp92 + _tmp93);
  int32_t _tmp95 = (int32_t) ((((int64_t) (actInv3_1_0_2) * (int64_t) (parent_v_angular_1_2)) >> 30));
  int32_t actInv5_1_0 = ((_tmp94 >> 3) + _tmp95);
  int32_t _tmp96 = (int32_t) ((((int64_t) (actInv3_1_1_0) * (int64_t) (parent_v_angular_1_0)) >> 30));
  int32_t _tmp97 = (int32_t) ((((int64_t) (actInv3_1_1_1) * (int64_t) (parent_v_angular_1_1)) >> 31));
  int32_t _tmp98 = (_tmp96 + _tmp97);
  int32_t _tmp99 = (int32_t) ((((int64_t) (actInv3_1_1_2) * (int64_t) (parent_v_angular_1_2)) >> 30));
  int32_t actInv5_1_1 = ((_tmp98 >> 3) + _tmp99);
  int32_t _tmp100 = (int32_t) ((((int64_t) (actInv3_1_2_0) * (int64_t) (parent_v_angular_1_0)) >> 31));
  int32_t _tmp101 = (int32_t) ((((int64_t) (actInv3_1_2_1) * (int64_t) (parent_v_angular_1_1)) >> 30));
  int32_t _tmp102 = (_tmp100 + _tmp101);
  int32_t _tmp103 = (int32_t) ((((int64_t) (actInv3_1_2_2) * (int64_t) (parent_v_angular_1_2)) >> 28));
  int32_t actInv5_1_2 = (_tmp102 + _tmp103);
  int32_t act_inv_angular_1_0 = ((v_angular_1_0 >> 3) + actInv5_1_0);
  int32_t act_inv_angular_1_1 = ((v_angular_1_1 >> 3) + actInv5_1_1);
  int32_t act_inv_angular_1_2 = (v_angular_1_2 + (actInv5_1_2 >> 3));
  int32_t minus_m_w_1 = -(v_1);
  int32_t _tmp104 = -(minus_m_w_1);
  int32_t alpha_cross1_linear_1 = (int32_t) ((((int64_t) (_tmp104) * (int64_t) (act_inv_linear_1_1)) >> 28));
  int32_t alpha_cross2_linear_1 = (int32_t) ((((int64_t) (minus_m_w_1) * (int64_t) (act_inv_linear_1_0)) >> 28));
  int32_t alpha_cross_linear_1_0 = alpha_cross1_linear_1;
  int32_t alpha_cross_linear_1_1 = alpha_cross2_linear_1;
  int32_t alpha_cross_linear_1_2 = 0;
  int32_t _tmp105 = -(minus_m_w_1);
  int32_t alpha_cross1_angular_1 = (int32_t) ((((int64_t) (_tmp105) * (int64_t) (act_inv_angular_1_1)) >> 30));
  int32_t alpha_cross2_angular_1 = (int32_t) ((((int64_t) (minus_m_w_1) * (int64_t) (act_inv_angular_1_0)) >> 30));
  int32_t alpha_cross_angular_1_0 = alpha_cross1_angular_1;
  int32_t alpha_cross_angular_1_1 = alpha_cross2_angular_1;
  int32_t alpha_cross_angular_1_2 = 0;
  int32_t r_28_new_a_gf_1_0 = alpha_cross_linear_1_0;
  int32_t r_28_new_a_gf_1_1 = alpha_cross_linear_1_1;
  int32_t r_28_new_a_gf_1_2 = alpha_cross_linear_1_2;
  int32_t r_28_new_a_gf_1_3 = alpha_cross_angular_1_0;
  int32_t r_28_new_a_gf_1_4 = alpha_cross_angular_1_1;
  int32_t r_28_new_a_gf_1_5 = alpha_cross_angular_1_2;
  int32_t new_a_gf_up1_1 = (a_1 + (r_28_new_a_gf_1_5 >> 3));
  int32_t new_a_gf2_linear_1_0 = r_28_new_a_gf_1_0;
  int32_t new_a_gf2_linear_1_1 = r_28_new_a_gf_1_1;
  int32_t new_a_gf2_linear_1_2 = r_28_new_a_gf_1_2;
  int32_t new_a_gf2_angular_1_0 = r_28_new_a_gf_1_3;
  int32_t new_a_gf2_angular_1_1 = r_28_new_a_gf_1_4;
  int32_t new_a_gf2_angular_1_2 = new_a_gf_up1_1;
  int32_t _tmp106 = (int32_t) ((((int64_t) (limi_translation_1_0) * (int64_t) (parent_a_gf_angular_1_1)) >> 31));
  int32_t _tmp107 = (int32_t) ((((int64_t) (limi_translation_1_1) * (int64_t) (parent_a_gf_angular_1_0)) >> 31));
  int32_t r_29_actInv1_1_0 = (_tmp106 - _tmp107);
  int32_t _tmp108 = (int32_t) ((((int64_t) (limi_translation_1_2) * (int64_t) (parent_a_gf_angular_1_0)) >> 31));
  int32_t _tmp109 = (int32_t) ((((int64_t) (limi_translation_1_0) * (int64_t) (parent_a_gf_angular_1_2)) >> 28));
  int32_t r_29_actInv1_1_1 = (_tmp108 - _tmp109);
  int32_t _tmp110 = (int32_t) ((((int64_t) (limi_translation_1_0) * (int64_t) (parent_a_gf_angular_1_1)) >> 31));
  int32_t _tmp111 = (int32_t) ((((int64_t) (limi_translation_1_1) * (int64_t) (parent_a_gf_angular_1_0)) >> 31));
  int32_t r_29_actInv1_1_2 = (_tmp110 - _tmp111);
  int32_t r_30_actInv2_1_0 = (parent_a_gf_linear_1_0 - r_29_actInv1_1_0);
  int32_t r_30_actInv2_1_1 = (parent_a_gf_linear_1_1 - r_29_actInv1_1_1);
  int32_t r_30_actInv2_1_2 = (parent_a_gf_linear_1_2 - (r_29_actInv1_1_2 >> 4));
  int32_t r_31_actInv3_1_0_0 = limi_rotation_1_0_0;
  int32_t r_31_actInv3_1_1_0 = limi_rotation_1_0_1;
  int32_t r_31_actInv3_1_2_0 = limi_rotation_1_0_2;
  int32_t r_31_actInv3_1_0_1 = limi_rotation_1_1_0;
  int32_t r_31_actInv3_1_1_1 = limi_rotation_1_1_1;
  int32_t r_31_actInv3_1_2_1 = limi_rotation_1_1_2;
  int32_t r_31_actInv3_1_0_2 = limi_rotation_1_2_0;
  int32_t r_31_actInv3_1_1_2 = limi_rotation_1_2_1;
  int32_t r_31_actInv3_1_2_2 = limi_rotation_1_2_2;
  int32_t _tmp112 = (int32_t) ((((int64_t) (r_31_actInv3_1_0_0) * (int64_t) (r_30_actInv2_1_0)) >> 30));
  int32_t _tmp113 = (int32_t) ((((int64_t) (r_31_actInv3_1_0_1) * (int64_t) (r_30_actInv2_1_1)) >> 31));
  int32_t _tmp114 = (_tmp112 + _tmp113);
  int32_t _tmp115 = (int32_t) ((((int64_t) (r_31_actInv3_1_0_2) * (int64_t) (r_30_actInv2_1_2)) >> 30));
  int32_t r_32_actInv4_1_0 = ((_tmp114 >> 4) + _tmp115);
  int32_t _tmp116 = (int32_t) ((((int64_t) (r_31_actInv3_1_1_0) * (int64_t) (r_30_actInv2_1_0)) >> 30));
  int32_t _tmp117 = (int32_t) ((((int64_t) (r_31_actInv3_1_1_1) * (int64_t) (r_30_actInv2_1_1)) >> 31));
  int32_t _tmp118 = (_tmp116 + _tmp117);
  int32_t _tmp119 = (int32_t) ((((int64_t) (r_31_actInv3_1_1_2) * (int64_t) (r_30_actInv2_1_2)) >> 30));
  int32_t r_32_actInv4_1_1 = ((_tmp118 >> 4) + _tmp119);
  int32_t _tmp120 = (int32_t) ((((int64_t) (r_31_actInv3_1_2_0) * (int64_t) (r_30_actInv2_1_0)) >> 31));
  int32_t _tmp121 = (int32_t) ((((int64_t) (r_31_actInv3_1_2_1) * (int64_t) (r_30_actInv2_1_1)) >> 30));
  int32_t _tmp122 = (_tmp120 + _tmp121);
  int32_t _tmp123 = (int32_t) ((((int64_t) (r_31_actInv3_1_2_2) * (int64_t) (r_30_actInv2_1_2)) >> 27));
  int32_t r_32_actInv4_1_2 = (_tmp122 + _tmp123);
  int32_t r_33_act_inv_linear_1_0 = ((new_a_gf2_linear_1_0 >> 4) + r_32_actInv4_1_0);
  int32_t r_33_act_inv_linear_1_1 = ((new_a_gf2_linear_1_1 >> 4) + r_32_actInv4_1_1);
  int32_t r_33_act_inv_linear_1_2 = (new_a_gf2_linear_1_2 + r_32_actInv4_1_2);
  int32_t _tmp124 = (int32_t) ((((int64_t) (r_31_actInv3_1_0_0) * (int64_t) (parent_a_gf_angular_1_0)) >> 30));
  int32_t _tmp125 = (int32_t) ((((int64_t) (r_31_actInv3_1_0_1) * (int64_t) (parent_a_gf_angular_1_1)) >> 31));
  int32_t _tmp126 = (_tmp124 + _tmp125);
  int32_t _tmp127 = (int32_t) ((((int64_t) (r_31_actInv3_1_0_2) * (int64_t) (parent_a_gf_angular_1_2)) >> 30));
  int32_t r_34_actInv5_1_0 = ((_tmp126 >> 3) + _tmp127);
  int32_t _tmp128 = (int32_t) ((((int64_t) (r_31_actInv3_1_1_0) * (int64_t) (parent_a_gf_angular_1_0)) >> 30));
  int32_t _tmp129 = (int32_t) ((((int64_t) (r_31_actInv3_1_1_1) * (int64_t) (parent_a_gf_angular_1_1)) >> 31));
  int32_t _tmp130 = (_tmp128 + _tmp129);
  int32_t _tmp131 = (int32_t) ((((int64_t) (r_31_actInv3_1_1_2) * (int64_t) (parent_a_gf_angular_1_2)) >> 30));
  int32_t r_34_actInv5_1_1 = ((_tmp130 >> 3) + _tmp131);
  int32_t _tmp132 = (int32_t) ((((int64_t) (r_31_actInv3_1_2_0) * (int64_t) (parent_a_gf_angular_1_0)) >> 31));
  int32_t _tmp133 = (int32_t) ((((int64_t) (r_31_actInv3_1_2_1) * (int64_t) (parent_a_gf_angular_1_1)) >> 30));
  int32_t _tmp134 = (_tmp132 + _tmp133);
  int32_t _tmp135 = (int32_t) ((((int64_t) (r_31_actInv3_1_2_2) * (int64_t) (parent_a_gf_angular_1_2)) >> 28));
  int32_t r_34_actInv5_1_2 = (_tmp134 + _tmp135);
  int32_t r_35_act_inv_angular_1_0 = (new_a_gf2_angular_1_0 + (r_34_actInv5_1_0 >> 2));
  int32_t r_35_act_inv_angular_1_1 = (new_a_gf2_angular_1_1 + (r_34_actInv5_1_1 >> 2));
  int32_t r_35_act_inv_angular_1_2 = (new_a_gf2_angular_1_2 + (r_34_actInv5_1_2 >> 3));
  int32_t new_a_gf_up3_1_0 = r_33_act_inv_linear_1_0;
  int32_t new_a_gf_up3_1_1 = r_33_act_inv_linear_1_1;
  int32_t new_a_gf_up3_1_2 = r_33_act_inv_linear_1_2;
  int32_t new_a_gf_up3_1_3 = r_35_act_inv_angular_1_0;
  int32_t new_a_gf_up3_1_4 = r_35_act_inv_angular_1_1;
  int32_t new_a_gf_up3_1_5 = r_35_act_inv_angular_1_2;
  int32_t _tmp136 = (int32_t) ((((int64_t) (lever_1_0) * (int64_t) (act_inv_angular_1_1)) >> 28));
  int32_t _tmp137 = (int32_t) ((((int64_t) (lever_1_1) * (int64_t) (act_inv_angular_1_0)) >> 28));
  int32_t h_linear_1_1_0 = (_tmp136 - _tmp137);
  int32_t _tmp138 = (int32_t) ((((int64_t) (lever_1_2) * (int64_t) (act_inv_angular_1_0)) >> 28));
  int32_t _tmp139 = (int32_t) ((((int64_t) (lever_1_0) * (int64_t) (act_inv_angular_1_2)) >> 28));
  int32_t h_linear_1_1_1 = (_tmp138 - _tmp139);
  int32_t _tmp140 = (int32_t) ((((int64_t) (lever_1_0) * (int64_t) (act_inv_angular_1_1)) >> 28));
  int32_t _tmp141 = (int32_t) ((((int64_t) (lever_1_1) * (int64_t) (act_inv_angular_1_0)) >> 28));
  int32_t h_linear_1_1_2 = (_tmp140 - _tmp141);
  int32_t h_linear_2_1_0 = (act_inv_linear_1_0 - h_linear_1_1_0);
  int32_t h_linear_2_1_1 = (act_inv_linear_1_1 - h_linear_1_1_1);
  int32_t h_linear_2_1_2 = (act_inv_linear_1_2 - h_linear_1_1_2);
  int32_t h_linear_1_0 = (int32_t) ((((int64_t) (masses_1) * (int64_t) (h_linear_2_1_0)) >> 31));
  int32_t h_linear_1_1 = (int32_t) ((((int64_t) (masses_1) * (int64_t) (h_linear_2_1_1)) >> 31));
  int32_t h_linear_1_2 = (int32_t) ((((int64_t) (masses_1) * (int64_t) (h_linear_2_1_2)) >> 31));
  int32_t r_36_ = (int32_t) ((((int64_t) (inertia_1_0_0) * (int64_t) (act_inv_angular_1_0)) >> 28));
  int32_t r_37_ = (int32_t) ((((int64_t) (inertia_1_0_1) * (int64_t) (act_inv_angular_1_1)) >> 28));
  int32_t r_38_ = (int32_t) ((((int64_t) (inertia_1_0_2) * (int64_t) (act_inv_angular_1_2)) >> 28));
  int32_t r_39_ = (int32_t) ((((int64_t) (inertia_1_0_1) * (int64_t) (act_inv_angular_1_0)) >> 28));
  int32_t r_40_ = (int32_t) ((((int64_t) (inertia_1_1_1) * (int64_t) (act_inv_angular_1_1)) >> 28));
  int32_t r_41_ = (int32_t) ((((int64_t) (inertia_1_1_2) * (int64_t) (act_inv_angular_1_2)) >> 28));
  int32_t r_42_ = (int32_t) ((((int64_t) (inertia_1_0_2) * (int64_t) (act_inv_angular_1_0)) >> 28));
  int32_t r_43_ = (int32_t) ((((int64_t) (inertia_1_1_2) * (int64_t) (act_inv_angular_1_1)) >> 28));
  int32_t r_44_ = (int32_t) ((((int64_t) (inertia_1_2_2) * (int64_t) (act_inv_angular_1_2)) >> 28));
  int32_t rhsMult1_temp_1 = (r_36_ + r_37_);
  int32_t rhsMult1_1 = (rhsMult1_temp_1 + r_38_);
  int32_t rhsMult2_temp_1 = (r_39_ + r_40_);
  int32_t rhsMult2_1 = (rhsMult2_temp_1 + r_41_);
  int32_t rhsMult3_temp_1 = (r_42_ + r_43_);
  int32_t rhsMult3_1 = (rhsMult3_temp_1 + r_44_);
  int32_t rhsMult_1_0 = rhsMult1_1;
  int32_t rhsMult_1_1 = rhsMult2_1;
  int32_t rhsMult_1_2 = rhsMult3_1;
  int32_t h_angular_first_1_0 = rhsMult_1_0;
  int32_t h_angular_first_1_1 = rhsMult_1_1;
  int32_t h_angular_first_1_2 = rhsMult_1_2;
  int32_t _tmp142 = (int32_t) ((((int64_t) (lever_1_0) * (int64_t) (h_linear_1_1)) >> 31));
  int32_t _tmp143 = (int32_t) ((((int64_t) (lever_1_1) * (int64_t) (h_linear_1_0)) >> 31));
  int32_t h_angular_1_1_0 = (_tmp142 - _tmp143);
  int32_t _tmp144 = (int32_t) ((((int64_t) (lever_1_2) * (int64_t) (h_linear_1_0)) >> 31));
  int32_t _tmp145 = (int32_t) ((((int64_t) (lever_1_0) * (int64_t) (h_linear_1_2)) >> 31));
  int32_t h_angular_1_1_1 = (_tmp144 - _tmp145);
  int32_t _tmp146 = (int32_t) ((((int64_t) (lever_1_0) * (int64_t) (h_linear_1_1)) >> 31));
  int32_t _tmp147 = (int32_t) ((((int64_t) (lever_1_1) * (int64_t) (h_linear_1_0)) >> 31));
  int32_t h_angular_1_1_2 = (_tmp146 - _tmp147);
  int32_t h_angular_1_0 = (h_angular_first_1_0 + h_angular_1_1_0);
  int32_t h_angular_1_1 = (h_angular_first_1_1 + h_angular_1_1_1);
  int32_t h_angular_1_2 = (h_angular_first_1_2 + h_angular_1_1_2);
  int32_t _tmp148 = (int32_t) ((((int64_t) (lever_1_0) * (int64_t) (r_35_act_inv_angular_1_1)) >> 26));
  int32_t _tmp149 = (int32_t) ((((int64_t) (lever_1_1) * (int64_t) (r_35_act_inv_angular_1_0)) >> 26));
  int32_t f_linear_1_1_0 = (_tmp148 - _tmp149);
  int32_t _tmp150 = (int32_t) ((((int64_t) (lever_1_2) * (int64_t) (r_35_act_inv_angular_1_0)) >> 26));
  int32_t _tmp151 = (int32_t) ((((int64_t) (lever_1_0) * (int64_t) (r_35_act_inv_angular_1_2)) >> 28));
  int32_t f_linear_1_1_1 = (_tmp150 - _tmp151);
  int32_t _tmp152 = (int32_t) ((((int64_t) (lever_1_0) * (int64_t) (r_35_act_inv_angular_1_1)) >> 26));
  int32_t _tmp153 = (int32_t) ((((int64_t) (lever_1_1) * (int64_t) (r_35_act_inv_angular_1_0)) >> 26));
  int32_t f_linear_1_1_2 = (_tmp152 - _tmp153);
  int32_t f_linear_2_1_0 = (r_33_act_inv_linear_1_0 - (f_linear_1_1_0 >> 4));
  int32_t f_linear_2_1_1 = (r_33_act_inv_linear_1_1 - (f_linear_1_1_1 >> 4));
  int32_t f_linear_2_1_2 = (r_33_act_inv_linear_1_2 - f_linear_1_1_2);
  int32_t f_linear_3_1_0 = (int32_t) ((((int64_t) (masses_1) * (int64_t) (f_linear_2_1_0)) >> 30));
  int32_t f_linear_3_1_1 = (int32_t) ((((int64_t) (masses_1) * (int64_t) (f_linear_2_1_1)) >> 30));
  int32_t f_linear_3_1_2 = (int32_t) ((((int64_t) (masses_1) * (int64_t) (f_linear_2_1_2)) >> 31));
  int32_t r_45_ = (int32_t) ((((int64_t) (inertia_1_0_0) * (int64_t) (r_35_act_inv_angular_1_0)) >> 26));
  int32_t r_46_ = (int32_t) ((((int64_t) (inertia_1_0_1) * (int64_t) (r_35_act_inv_angular_1_1)) >> 26));
  int32_t r_47_ = (int32_t) ((((int64_t) (inertia_1_0_2) * (int64_t) (r_35_act_inv_angular_1_2)) >> 28));
  int32_t r_48_ = (int32_t) ((((int64_t) (inertia_1_0_1) * (int64_t) (r_35_act_inv_angular_1_0)) >> 26));
  int32_t r_49_ = (int32_t) ((((int64_t) (inertia_1_1_1) * (int64_t) (r_35_act_inv_angular_1_1)) >> 26));
  int32_t r_50_ = (int32_t) ((((int64_t) (inertia_1_1_2) * (int64_t) (r_35_act_inv_angular_1_2)) >> 28));
  int32_t r_51_ = (int32_t) ((((int64_t) (inertia_1_0_2) * (int64_t) (r_35_act_inv_angular_1_0)) >> 26));
  int32_t r_52_ = (int32_t) ((((int64_t) (inertia_1_1_2) * (int64_t) (r_35_act_inv_angular_1_1)) >> 26));
  int32_t r_53_ = (int32_t) ((((int64_t) (inertia_1_2_2) * (int64_t) (r_35_act_inv_angular_1_2)) >> 28));
  int32_t r_54_rhsMult1_temp_1 = (r_45_ + r_46_);
  int32_t r_55_rhsMult1_1 = (r_54_rhsMult1_temp_1 + r_47_);
  int32_t r_56_rhsMult2_temp_1 = (r_48_ + r_49_);
  int32_t r_57_rhsMult2_1 = (r_56_rhsMult2_temp_1 + r_50_);
  int32_t r_58_rhsMult3_temp_1 = (r_51_ + r_52_);
  int32_t r_59_rhsMult3_1 = (r_58_rhsMult3_temp_1 + r_53_);
  int32_t r_60_rhsMult_1_0 = r_55_rhsMult1_1;
  int32_t r_60_rhsMult_1_1 = r_57_rhsMult2_1;
  int32_t r_60_rhsMult_1_2 = r_59_rhsMult3_1;
  int32_t f_angular_first_1_0 = r_60_rhsMult_1_0;
  int32_t f_angular_first_1_1 = r_60_rhsMult_1_1;
  int32_t f_angular_first_1_2 = r_60_rhsMult_1_2;
  int32_t _tmp154 = (int32_t) ((((int64_t) (lever_1_0) * (int64_t) (f_linear_3_1_1)) >> 28));
  int32_t _tmp155 = (int32_t) ((((int64_t) (lever_1_1) * (int64_t) (f_linear_3_1_0)) >> 28));
  int32_t f_angular_1_1_0 = (_tmp154 - _tmp155);
  int32_t _tmp156 = (int32_t) ((((int64_t) (lever_1_2) * (int64_t) (f_linear_3_1_0)) >> 28));
  int32_t _tmp157 = (int32_t) ((((int64_t) (lever_1_0) * (int64_t) (f_linear_3_1_2)) >> 31));
  int32_t f_angular_1_1_1 = (_tmp156 - _tmp157);
  int32_t _tmp158 = (int32_t) ((((int64_t) (lever_1_0) * (int64_t) (f_linear_3_1_1)) >> 28));
  int32_t _tmp159 = (int32_t) ((((int64_t) (lever_1_1) * (int64_t) (f_linear_3_1_0)) >> 28));
  int32_t f_angular_1_1_2 = (_tmp158 - _tmp159);
  int32_t f_angular_2_1_0 = (f_angular_first_1_0 + f_angular_1_1_0);
  int32_t f_angular_2_1_1 = (f_angular_first_1_1 + f_angular_1_1_1);
  int32_t f_angular_2_1_2 = (f_angular_first_1_2 + f_angular_1_1_2);
  int32_t _tmp160 = (int32_t) ((((int64_t) (act_inv_angular_1_0) * (int64_t) (h_linear_1_1)) >> 28));
  int32_t _tmp161 = (int32_t) ((((int64_t) (act_inv_angular_1_1) * (int64_t) (h_linear_1_0)) >> 28));
  int32_t f_linear_4_temp_1_0 = (_tmp160 - _tmp161);
  int32_t _tmp162 = (int32_t) ((((int64_t) (act_inv_angular_1_2) * (int64_t) (h_linear_1_0)) >> 28));
  int32_t _tmp163 = (int32_t) ((((int64_t) (act_inv_angular_1_0) * (int64_t) (h_linear_1_2)) >> 28));
  int32_t f_linear_4_temp_1_1 = ((_tmp162 - _tmp163) >> 1);
  int32_t _tmp164 = (int32_t) ((((int64_t) (act_inv_angular_1_0) * (int64_t) (h_linear_1_1)) >> 28));
  int32_t _tmp165 = (int32_t) ((((int64_t) (act_inv_angular_1_1) * (int64_t) (h_linear_1_0)) >> 28));
  int32_t f_linear_4_temp_1_2 = (_tmp164 - _tmp165);
  int32_t f_linear_4_1_0 = (f_linear_3_1_0 + (f_linear_4_temp_1_0 >> 3));
  int32_t f_linear_4_1_1 = (f_linear_3_1_1 + (f_linear_4_temp_1_1 >> 2));
  int32_t f_linear_4_1_2 = ((f_linear_3_1_2 + f_linear_4_temp_1_2) >> 1);
  int32_t _tmp166 = (int32_t) ((((int64_t) (act_inv_angular_1_0) * (int64_t) (h_angular_first_1_1)) >> 28));
  int32_t _tmp167 = (int32_t) ((((int64_t) (act_inv_angular_1_1) * (int64_t) (h_angular_first_1_0)) >> 28));
  int32_t f_angular_3_temp_1_0 = ((_tmp166 - _tmp167) >> 1);
  int32_t _tmp168 = (int32_t) ((((int64_t) (act_inv_angular_1_2) * (int64_t) (h_angular_first_1_0)) >> 28));
  int32_t _tmp169 = (int32_t) ((((int64_t) (act_inv_angular_1_0) * (int64_t) (h_angular_first_1_2)) >> 28));
  int32_t f_angular_3_temp_1_1 = ((_tmp168 - _tmp169) >> 1);
  int32_t _tmp170 = (int32_t) ((((int64_t) (act_inv_angular_1_0) * (int64_t) (h_angular_first_1_1)) >> 28));
  int32_t _tmp171 = (int32_t) ((((int64_t) (act_inv_angular_1_1) * (int64_t) (h_angular_first_1_0)) >> 28));
  int32_t f_angular_3_temp_1_2 = ((_tmp170 - _tmp171) >> 1);
  int32_t f_angular_3_1_0 = (((f_angular_2_1_0 >> 1) + f_angular_3_temp_1_0) >> 1);
  int32_t f_angular_3_1_1 = (((f_angular_2_1_1 >> 1) + f_angular_3_temp_1_1) >> 1);
  int32_t f_angular_3_1_2 = (((f_angular_2_1_2 >> 1) + f_angular_3_temp_1_2) >> 1);
  int32_t _tmp172 = (int32_t) ((((int64_t) (act_inv_linear_1_0) * (int64_t) (h_linear_1_1)) >> 31));
  int32_t _tmp173 = (int32_t) ((((int64_t) (act_inv_linear_1_1) * (int64_t) (h_linear_1_0)) >> 31));
  int32_t f_angular_4_temp_1_0 = (_tmp172 - _tmp173);
  int32_t _tmp174 = (int32_t) ((((int64_t) (act_inv_linear_1_2) * (int64_t) (h_linear_1_0)) >> 31));
  int32_t _tmp175 = (int32_t) ((((int64_t) (act_inv_linear_1_0) * (int64_t) (h_linear_1_2)) >> 31));
  int32_t f_angular_4_temp_1_1 = (_tmp174 - _tmp175);
  int32_t _tmp176 = (int32_t) ((((int64_t) (act_inv_linear_1_0) * (int64_t) (h_linear_1_1)) >> 31));
  int32_t _tmp177 = (int32_t) ((((int64_t) (act_inv_linear_1_1) * (int64_t) (h_linear_1_0)) >> 31));
  int32_t f_angular_4_temp_1_2 = (_tmp176 - _tmp177);
  int32_t f_angular_4_1_0 = (f_angular_3_1_0 + (f_angular_4_temp_1_0 >> 2));
  int32_t f_angular_4_1_1 = (f_angular_3_1_1 + (f_angular_4_temp_1_1 >> 2));
  int32_t f_angular_4_1_2 = (f_angular_3_1_2 + (f_angular_4_temp_1_2 >> 2));
  int32_t h_rnea_firstpass1_0 = h_linear_1_0;
  int32_t h_rnea_firstpass1_1 = h_linear_1_1;
  int32_t h_rnea_firstpass1_2 = h_linear_1_2;
  int32_t h_rnea_firstpass1_3 = h_angular_1_0;
  int32_t h_rnea_firstpass1_4 = h_angular_1_1;
  int32_t h_rnea_firstpass1_5 = h_angular_1_2;
  int32_t f_rnea_firstpass1_0 = f_linear_4_1_0;
  int32_t f_rnea_firstpass1_1 = f_linear_4_1_1;
  int32_t f_rnea_firstpass1_2 = f_linear_4_1_2;
  int32_t f_rnea_firstpass1_3 = f_angular_4_1_0;
  int32_t f_rnea_firstpass1_4 = f_angular_4_1_1;
  int32_t f_rnea_firstpass1_5 = f_angular_4_1_2;
  int32_t r_61_new_v_1_0 = act_inv_linear_1_0;
  int32_t r_61_new_v_1_1 = act_inv_linear_1_1;
  int32_t r_61_new_v_1_2 = act_inv_linear_1_2;
  int32_t r_61_new_v_1_3 = act_inv_angular_1_0;
  int32_t r_61_new_v_1_4 = act_inv_angular_1_1;
  int32_t r_61_new_v_1_5 = act_inv_angular_1_2;
  int32_t rotation_matrix_2_0_0 = qcos_2;
  int32_t rotation_matrix_2_0_1 = -(qsin_2);
  int32_t rotation_matrix_2_0_2 = 0;
  int32_t rotation_matrix_2_1_0 = qsin_2;
  int32_t rotation_matrix_2_1_1 = qcos_2;
  int32_t rotation_matrix_2_1_2 = 0;
  int32_t rotation_matrix_2_2_0 = 0;
  int32_t rotation_matrix_2_2_1 = 0;
  int32_t rotation_matrix_2_2_2 = 1073741824;
  int32_t limi_rotation_2_0_0 = rotation_matrix_2_0_0;
  int32_t limi_rotation_2_0_1 = rotation_matrix_2_0_1;
  int32_t limi_rotation_2_0_2 = 0;
  int32_t limi_rotation_2_1_0 = 0;
  int32_t limi_rotation_2_1_1 = 0;
  int32_t limi_rotation_2_1_2 = -1073741825;
  int32_t limi_rotation_2_2_0 = rotation_matrix_2_1_0;
  int32_t limi_rotation_2_2_1 = rotation_matrix_2_1_1;
  int32_t limi_rotation_2_2_2 = 0;
  int32_t v_linear_2_0 = data_v_2_0;
  int32_t v_linear_2_1 = data_v_2_1;
  int32_t v_linear_2_2 = data_v_2_2;
  int32_t v_angular_2_0 = data_v_2_3;
  int32_t v_angular_2_1 = data_v_2_4;
  int32_t v_angular_2_2 = v_2;
  int32_t parent_v_linear_2_0 = r_61_new_v_1_0;
  int32_t parent_v_linear_2_1 = r_61_new_v_1_1;
  int32_t parent_v_linear_2_2 = r_61_new_v_1_2;
  int32_t parent_v_angular_2_0 = r_61_new_v_1_3;
  int32_t parent_v_angular_2_1 = r_61_new_v_1_4;
  int32_t parent_v_angular_2_2 = r_61_new_v_1_5;
  int32_t parent_a_gf_linear_2_0 = new_a_gf_up3_1_0;
  int32_t parent_a_gf_linear_2_1 = new_a_gf_up3_1_1;
  int32_t parent_a_gf_linear_2_2 = new_a_gf_up3_1_2;
  int32_t parent_a_gf_angular_2_0 = new_a_gf_up3_1_3;
  int32_t parent_a_gf_angular_2_1 = new_a_gf_up3_1_4;
  int32_t parent_a_gf_angular_2_2 = new_a_gf_up3_1_5;
  int32_t _tmp178 = (int32_t) ((((int64_t) (limi_translation_2_0) * (int64_t) (parent_v_angular_2_1)) >> 28));
  int32_t _tmp179 = (int32_t) ((((int64_t) (limi_translation_2_1) * (int64_t) (parent_v_angular_2_0)) >> 29));
  int32_t actInv1_2_0 = ((_tmp178 >> 1) - _tmp179);
  int32_t _tmp180 = (int32_t) ((((int64_t) (limi_translation_2_2) * (int64_t) (parent_v_angular_2_0)) >> 28));
  int32_t _tmp181 = (int32_t) ((((int64_t) (limi_translation_2_0) * (int64_t) (parent_v_angular_2_2)) >> 28));
  int32_t actInv1_2_1 = (_tmp180 - _tmp181);
  int32_t _tmp182 = (int32_t) ((((int64_t) (limi_translation_2_0) * (int64_t) (parent_v_angular_2_1)) >> 28));
  int32_t _tmp183 = (int32_t) ((((int64_t) (limi_translation_2_1) * (int64_t) (parent_v_angular_2_0)) >> 29));
  int32_t actInv1_2_2 = ((_tmp182 >> 1) - _tmp183);
  int32_t actInv2_2_0 = ((parent_v_linear_2_0 >> 1) - actInv1_2_0);
  int32_t actInv2_2_1 = (parent_v_linear_2_1 - actInv1_2_1);
  int32_t actInv2_2_2 = ((parent_v_linear_2_2 >> 1) - actInv1_2_2);
  int32_t actInv3_2_0_0 = limi_rotation_2_0_0;
  int32_t actInv3_2_1_0 = limi_rotation_2_0_1;
  int32_t actInv3_2_2_0 = limi_rotation_2_0_2;
  int32_t actInv3_2_0_1 = limi_rotation_2_1_0;
  int32_t actInv3_2_1_1 = limi_rotation_2_1_1;
  int32_t actInv3_2_2_1 = limi_rotation_2_1_2;
  int32_t actInv3_2_0_2 = limi_rotation_2_2_0;
  int32_t actInv3_2_1_2 = limi_rotation_2_2_1;
  int32_t actInv3_2_2_2 = limi_rotation_2_2_2;
  int32_t _tmp184 = (int32_t) ((((int64_t) (actInv3_2_0_0) * (int64_t) (actInv2_2_0)) >> 30));
  int32_t _tmp185 = (int32_t) ((((int64_t) (actInv3_2_0_1) * (int64_t) (actInv2_2_1)) >> 31));
  int32_t _tmp186 = (_tmp184 + (_tmp185 >> 1));
  int32_t _tmp187 = (int32_t) ((((int64_t) (actInv3_2_0_2) * (int64_t) (actInv2_2_2)) >> 30));
  int32_t actInv4_2_0 = ((_tmp186 + _tmp187) >> 1);
  int32_t _tmp188 = (int32_t) ((((int64_t) (actInv3_2_1_0) * (int64_t) (actInv2_2_0)) >> 30));
  int32_t _tmp189 = (int32_t) ((((int64_t) (actInv3_2_1_1) * (int64_t) (actInv2_2_1)) >> 31));
  int32_t _tmp190 = (_tmp188 + (_tmp189 >> 1));
  int32_t _tmp191 = (int32_t) ((((int64_t) (actInv3_2_1_2) * (int64_t) (actInv2_2_2)) >> 30));
  int32_t actInv4_2_1 = ((_tmp190 + _tmp191) >> 1);
  int32_t _tmp192 = (int32_t) ((((int64_t) (actInv3_2_2_0) * (int64_t) (actInv2_2_0)) >> 30));
  int32_t _tmp193 = (int32_t) ((((int64_t) (actInv3_2_2_1) * (int64_t) (actInv2_2_1)) >> 30));
  int32_t _tmp194 = (_tmp192 + _tmp193);
  int32_t _tmp195 = (int32_t) ((((int64_t) (actInv3_2_2_2) * (int64_t) (actInv2_2_2)) >> 30));
  int32_t actInv4_2_2 = (_tmp194 + _tmp195);
  int32_t act_inv_linear_2_0 = ((v_linear_2_0 >> 2) + actInv4_2_0);
  int32_t act_inv_linear_2_1 = ((v_linear_2_1 >> 2) + actInv4_2_1);
  int32_t act_inv_linear_2_2 = (v_linear_2_2 + actInv4_2_2);
  int32_t _tmp196 = (int32_t) ((((int64_t) (actInv3_2_0_0) * (int64_t) (parent_v_angular_2_0)) >> 30));
  int32_t _tmp197 = (int32_t) ((((int64_t) (actInv3_2_0_1) * (int64_t) (parent_v_angular_2_1)) >> 28));
  int32_t _tmp198 = (_tmp196 + (_tmp197 >> 3));
  int32_t _tmp199 = (int32_t) ((((int64_t) (actInv3_2_0_2) * (int64_t) (parent_v_angular_2_2)) >> 30));
  int32_t actInv5_2_0 = ((_tmp198 + _tmp199) >> 1);
  int32_t _tmp200 = (int32_t) ((((int64_t) (actInv3_2_1_0) * (int64_t) (parent_v_angular_2_0)) >> 30));
  int32_t _tmp201 = (int32_t) ((((int64_t) (actInv3_2_1_1) * (int64_t) (parent_v_angular_2_1)) >> 28));
  int32_t _tmp202 = (_tmp200 + (_tmp201 >> 3));
  int32_t _tmp203 = (int32_t) ((((int64_t) (actInv3_2_1_2) * (int64_t) (parent_v_angular_2_2)) >> 30));
  int32_t actInv5_2_1 = ((_tmp202 + _tmp203) >> 1);
  int32_t _tmp204 = (int32_t) ((((int64_t) (actInv3_2_2_0) * (int64_t) (parent_v_angular_2_0)) >> 28));
  int32_t _tmp205 = (int32_t) ((((int64_t) (actInv3_2_2_1) * (int64_t) (parent_v_angular_2_1)) >> 30));
  int32_t _tmp206 = ((_tmp204 >> 3) + _tmp205);
  int32_t _tmp207 = (int32_t) ((((int64_t) (actInv3_2_2_2) * (int64_t) (parent_v_angular_2_2)) >> 28));
  int32_t actInv5_2_2 = (_tmp206 + (_tmp207 >> 3));
  int32_t act_inv_angular_2_0 = ((v_angular_2_0 >> 4) + actInv5_2_0);
  int32_t act_inv_angular_2_1 = ((v_angular_2_1 >> 4) + actInv5_2_1);
  int32_t act_inv_angular_2_2 = ((v_angular_2_2 + actInv5_2_2) >> 1);
  int32_t minus_m_w_2 = -(v_2);
  int32_t _tmp208 = -(minus_m_w_2);
  int32_t alpha_cross1_linear_2 = (int32_t) ((((int64_t) (_tmp208) * (int64_t) (act_inv_linear_2_1)) >> 30));
  int32_t alpha_cross2_linear_2 = (int32_t) ((((int64_t) (minus_m_w_2) * (int64_t) (act_inv_linear_2_0)) >> 30));
  int32_t alpha_cross_linear_2_0 = alpha_cross1_linear_2;
  int32_t alpha_cross_linear_2_1 = alpha_cross2_linear_2;
  int32_t alpha_cross_linear_2_2 = 0;
  int32_t _tmp209 = -(minus_m_w_2);
  int32_t alpha_cross1_angular_2 = (int32_t) ((((int64_t) (_tmp209) * (int64_t) (act_inv_angular_2_1)) >> 30));
  int32_t alpha_cross2_angular_2 = (int32_t) ((((int64_t) (minus_m_w_2) * (int64_t) (act_inv_angular_2_0)) >> 30));
  int32_t alpha_cross_angular_2_0 = alpha_cross1_angular_2;
  int32_t alpha_cross_angular_2_1 = alpha_cross2_angular_2;
  int32_t alpha_cross_angular_2_2 = 0;
  int32_t r_62_new_a_gf_2_0 = alpha_cross_linear_2_0;
  int32_t r_62_new_a_gf_2_1 = alpha_cross_linear_2_1;
  int32_t r_62_new_a_gf_2_2 = alpha_cross_linear_2_2;
  int32_t r_62_new_a_gf_2_3 = alpha_cross_angular_2_0;
  int32_t r_62_new_a_gf_2_4 = alpha_cross_angular_2_1;
  int32_t r_62_new_a_gf_2_5 = alpha_cross_angular_2_2;
  int32_t new_a_gf_up1_2 = (a_2 + (r_62_new_a_gf_2_5 >> 3));
  int32_t new_a_gf2_linear_2_0 = r_62_new_a_gf_2_0;
  int32_t new_a_gf2_linear_2_1 = r_62_new_a_gf_2_1;
  int32_t new_a_gf2_linear_2_2 = r_62_new_a_gf_2_2;
  int32_t new_a_gf2_angular_2_0 = r_62_new_a_gf_2_3;
  int32_t new_a_gf2_angular_2_1 = r_62_new_a_gf_2_4;
  int32_t new_a_gf2_angular_2_2 = new_a_gf_up1_2;
  int32_t _tmp210 = (int32_t) ((((int64_t) (limi_translation_2_0) * (int64_t) (parent_a_gf_angular_2_1)) >> 26));
  int32_t _tmp211 = (int32_t) ((((int64_t) (limi_translation_2_1) * (int64_t) (parent_a_gf_angular_2_0)) >> 30));
  int32_t r_63_actInv1_2_0 = ((_tmp210 >> 4) - _tmp211);
  int32_t _tmp212 = (int32_t) ((((int64_t) (limi_translation_2_2) * (int64_t) (parent_a_gf_angular_2_0)) >> 26));
  int32_t _tmp213 = (int32_t) ((((int64_t) (limi_translation_2_0) * (int64_t) (parent_a_gf_angular_2_2)) >> 28));
  int32_t r_63_actInv1_2_1 = (_tmp212 - _tmp213);
  int32_t _tmp214 = (int32_t) ((((int64_t) (limi_translation_2_0) * (int64_t) (parent_a_gf_angular_2_1)) >> 26));
  int32_t _tmp215 = (int32_t) ((((int64_t) (limi_translation_2_1) * (int64_t) (parent_a_gf_angular_2_0)) >> 30));
  int32_t r_63_actInv1_2_2 = ((_tmp214 >> 4) - _tmp215);
  int32_t r_64_actInv2_2_0 = ((parent_a_gf_linear_2_0 - r_63_actInv1_2_0) >> 1);
  int32_t r_64_actInv2_2_1 = (parent_a_gf_linear_2_1 - (r_63_actInv1_2_1 >> 4));
  int32_t r_64_actInv2_2_2 = ((parent_a_gf_linear_2_2 >> 4) - r_63_actInv1_2_2);
  int32_t r_65_actInv3_2_0_0 = limi_rotation_2_0_0;
  int32_t r_65_actInv3_2_1_0 = limi_rotation_2_0_1;
  int32_t r_65_actInv3_2_2_0 = limi_rotation_2_0_2;
  int32_t r_65_actInv3_2_0_1 = limi_rotation_2_1_0;
  int32_t r_65_actInv3_2_1_1 = limi_rotation_2_1_1;
  int32_t r_65_actInv3_2_2_1 = limi_rotation_2_1_2;
  int32_t r_65_actInv3_2_0_2 = limi_rotation_2_2_0;
  int32_t r_65_actInv3_2_1_2 = limi_rotation_2_2_1;
  int32_t r_65_actInv3_2_2_2 = limi_rotation_2_2_2;
  int32_t _tmp216 = (int32_t) ((((int64_t) (r_65_actInv3_2_0_0) * (int64_t) (r_64_actInv2_2_0)) >> 30));
  int32_t _tmp217 = (int32_t) ((((int64_t) (r_65_actInv3_2_0_1) * (int64_t) (r_64_actInv2_2_1)) >> 27));
  int32_t _tmp218 = (_tmp216 + (_tmp217 >> 5));
  int32_t _tmp219 = (int32_t) ((((int64_t) (r_65_actInv3_2_0_2) * (int64_t) (r_64_actInv2_2_2)) >> 30));
  int32_t r_66_actInv4_2_0 = (_tmp218 + (_tmp219 >> 1));
  int32_t _tmp220 = (int32_t) ((((int64_t) (r_65_actInv3_2_1_0) * (int64_t) (r_64_actInv2_2_0)) >> 30));
  int32_t _tmp221 = (int32_t) ((((int64_t) (r_65_actInv3_2_1_1) * (int64_t) (r_64_actInv2_2_1)) >> 27));
  int32_t _tmp222 = (_tmp220 + (_tmp221 >> 5));
  int32_t _tmp223 = (int32_t) ((((int64_t) (r_65_actInv3_2_1_2) * (int64_t) (r_64_actInv2_2_2)) >> 30));
  int32_t r_66_actInv4_2_1 = (_tmp222 + (_tmp223 >> 1));
  int32_t _tmp224 = (int32_t) ((((int64_t) (r_65_actInv3_2_2_0) * (int64_t) (r_64_actInv2_2_0)) >> 26));
  int32_t _tmp225 = (int32_t) ((((int64_t) (r_65_actInv3_2_2_1) * (int64_t) (r_64_actInv2_2_1)) >> 30));
  int32_t _tmp226 = ((_tmp224 >> 4) + _tmp225);
  int32_t _tmp227 = (int32_t) ((((int64_t) (r_65_actInv3_2_2_2) * (int64_t) (r_64_actInv2_2_2)) >> 27));
  int32_t r_66_actInv4_2_2 = (_tmp226 + (_tmp227 >> 4));
  int32_t r_67_act_inv_linear_2_0 = (((new_a_gf2_linear_2_0 >> 1) + r_66_actInv4_2_0) >> 1);
  int32_t r_67_act_inv_linear_2_1 = (((new_a_gf2_linear_2_1 >> 1) + r_66_actInv4_2_1) >> 1);
  int32_t r_67_act_inv_linear_2_2 = ((new_a_gf2_linear_2_2 >> 4) + r_66_actInv4_2_2);
  int32_t _tmp228 = (int32_t) ((((int64_t) (r_65_actInv3_2_0_0) * (int64_t) (parent_a_gf_angular_2_0)) >> 30));
  int32_t _tmp229 = (int32_t) ((((int64_t) (r_65_actInv3_2_0_1) * (int64_t) (parent_a_gf_angular_2_1)) >> 26));
  int32_t _tmp230 = (_tmp228 + (_tmp229 >> 5));
  int32_t _tmp231 = (int32_t) ((((int64_t) (r_65_actInv3_2_0_2) * (int64_t) (parent_a_gf_angular_2_2)) >> 30));
  int32_t r_68_actInv5_2_0 = ((_tmp230 + (_tmp231 >> 2)) >> 1);
  int32_t _tmp232 = (int32_t) ((((int64_t) (r_65_actInv3_2_1_0) * (int64_t) (parent_a_gf_angular_2_0)) >> 30));
  int32_t _tmp233 = (int32_t) ((((int64_t) (r_65_actInv3_2_1_1) * (int64_t) (parent_a_gf_angular_2_1)) >> 26));
  int32_t _tmp234 = (_tmp232 + (_tmp233 >> 5));
  int32_t _tmp235 = (int32_t) ((((int64_t) (r_65_actInv3_2_1_2) * (int64_t) (parent_a_gf_angular_2_2)) >> 30));
  int32_t r_68_actInv5_2_1 = ((_tmp234 + (_tmp235 >> 2)) >> 1);
  int32_t _tmp236 = (int32_t) ((((int64_t) (r_65_actInv3_2_2_0) * (int64_t) (parent_a_gf_angular_2_0)) >> 26));
  int32_t _tmp237 = (int32_t) ((((int64_t) (r_65_actInv3_2_2_1) * (int64_t) (parent_a_gf_angular_2_1)) >> 30));
  int32_t _tmp238 = ((_tmp236 >> 5) + _tmp237);
  int32_t _tmp239 = (int32_t) ((((int64_t) (r_65_actInv3_2_2_2) * (int64_t) (parent_a_gf_angular_2_2)) >> 28));
  int32_t r_68_actInv5_2_2 = (_tmp238 + (_tmp239 >> 5));
  int32_t r_69_act_inv_angular_2_0 = ((new_a_gf2_angular_2_0 + r_68_actInv5_2_0) >> 1);
  int32_t r_69_act_inv_angular_2_1 = ((new_a_gf2_angular_2_1 + r_68_actInv5_2_1) >> 1);
  int32_t r_69_act_inv_angular_2_2 = (((new_a_gf2_angular_2_2 >> 2) + r_68_actInv5_2_2) >> 1);
  int32_t new_a_gf_up3_2_0 = r_67_act_inv_linear_2_0;
  int32_t new_a_gf_up3_2_1 = r_67_act_inv_linear_2_1;
  int32_t new_a_gf_up3_2_2 = r_67_act_inv_linear_2_2;
  int32_t new_a_gf_up3_2_3 = r_69_act_inv_angular_2_0;
  int32_t new_a_gf_up3_2_4 = r_69_act_inv_angular_2_1;
  int32_t new_a_gf_up3_2_5 = r_69_act_inv_angular_2_2;
  int32_t _tmp240 = (int32_t) ((((int64_t) (lever_2_0) * (int64_t) (act_inv_angular_2_1)) >> 27));
  int32_t _tmp241 = (int32_t) ((((int64_t) (lever_2_1) * (int64_t) (act_inv_angular_2_0)) >> 27));
  int32_t r_70_h_linear_1_2_0 = (_tmp240 - _tmp241);
  int32_t _tmp242 = (int32_t) ((((int64_t) (lever_2_2) * (int64_t) (act_inv_angular_2_0)) >> 27));
  int32_t _tmp243 = (int32_t) ((((int64_t) (lever_2_0) * (int64_t) (act_inv_angular_2_2)) >> 27));
  int32_t r_70_h_linear_1_2_1 = (_tmp242 - _tmp243);
  int32_t _tmp244 = (int32_t) ((((int64_t) (lever_2_0) * (int64_t) (act_inv_angular_2_1)) >> 27));
  int32_t _tmp245 = (int32_t) ((((int64_t) (lever_2_1) * (int64_t) (act_inv_angular_2_0)) >> 27));
  int32_t r_70_h_linear_1_2_2 = (_tmp244 - _tmp245);
  int32_t h_linear_2_2_0 = (act_inv_linear_2_0 - (r_70_h_linear_1_2_0 >> 2));
  int32_t h_linear_2_2_1 = ((act_inv_linear_2_1 - (r_70_h_linear_1_2_1 >> 2)) >> 1);
  int32_t h_linear_2_2_2 = (act_inv_linear_2_2 - r_70_h_linear_1_2_2);
  int32_t h_linear_2_0 = (int32_t) ((((int64_t) (masses_2) * (int64_t) (h_linear_2_2_0)) >> 31));
  int32_t h_linear_2_1 = (int32_t) ((((int64_t) (masses_2) * (int64_t) (h_linear_2_2_1)) >> 30));
  int32_t h_linear_2_2 = (int32_t) ((((int64_t) (masses_2) * (int64_t) (h_linear_2_2_2)) >> 31));
  int32_t r_71_ = (int32_t) ((((int64_t) (inertia_2_0_0) * (int64_t) (act_inv_angular_2_0)) >> 27));
  int32_t r_72_ = (int32_t) ((((int64_t) (inertia_2_0_1) * (int64_t) (act_inv_angular_2_1)) >> 27));
  int32_t r_73_ = (int32_t) ((((int64_t) (inertia_2_0_2) * (int64_t) (act_inv_angular_2_2)) >> 27));
  int32_t r_74_ = (int32_t) ((((int64_t) (inertia_2_0_1) * (int64_t) (act_inv_angular_2_0)) >> 27));
  int32_t r_75_ = (int32_t) ((((int64_t) (inertia_2_1_1) * (int64_t) (act_inv_angular_2_1)) >> 27));
  int32_t r_76_ = (int32_t) ((((int64_t) (inertia_2_1_2) * (int64_t) (act_inv_angular_2_2)) >> 27));
  int32_t r_77_ = (int32_t) ((((int64_t) (inertia_2_0_2) * (int64_t) (act_inv_angular_2_0)) >> 27));
  int32_t r_78_ = (int32_t) ((((int64_t) (inertia_2_1_2) * (int64_t) (act_inv_angular_2_1)) >> 27));
  int32_t r_79_ = (int32_t) ((((int64_t) (inertia_2_2_2) * (int64_t) (act_inv_angular_2_2)) >> 27));
  int32_t rhsMult1_temp_2 = (r_71_ + r_72_);
  int32_t rhsMult1_2 = (rhsMult1_temp_2 + r_73_);
  int32_t rhsMult2_temp_2 = (r_74_ + r_75_);
  int32_t rhsMult2_2 = (rhsMult2_temp_2 + r_76_);
  int32_t rhsMult3_temp_2 = (r_77_ + r_78_);
  int32_t rhsMult3_2 = (rhsMult3_temp_2 + r_79_);
  int32_t rhsMult_2_0 = rhsMult1_2;
  int32_t rhsMult_2_1 = rhsMult2_2;
  int32_t rhsMult_2_2 = rhsMult3_2;
  int32_t h_angular_first_2_0 = rhsMult_2_0;
  int32_t h_angular_first_2_1 = rhsMult_2_1;
  int32_t h_angular_first_2_2 = rhsMult_2_2;
  int32_t _tmp246 = (int32_t) ((((int64_t) (lever_2_0) * (int64_t) (h_linear_2_1)) >> 27));
  int32_t _tmp247 = (int32_t) ((((int64_t) (lever_2_1) * (int64_t) (h_linear_2_0)) >> 27));
  int32_t r_80_h_angular_1_2_0 = (_tmp246 - _tmp247);
  int32_t _tmp248 = (int32_t) ((((int64_t) (lever_2_2) * (int64_t) (h_linear_2_0)) >> 27));
  int32_t _tmp249 = (int32_t) ((((int64_t) (lever_2_0) * (int64_t) (h_linear_2_2)) >> 29));
  int32_t r_80_h_angular_1_2_1 = (_tmp248 - _tmp249);
  int32_t _tmp250 = (int32_t) ((((int64_t) (lever_2_0) * (int64_t) (h_linear_2_1)) >> 27));
  int32_t _tmp251 = (int32_t) ((((int64_t) (lever_2_1) * (int64_t) (h_linear_2_0)) >> 27));
  int32_t r_80_h_angular_1_2_2 = (_tmp250 - _tmp251);
  int32_t h_angular_2_0 = ((h_angular_first_2_0 + r_80_h_angular_1_2_0) >> 1);
  int32_t h_angular_2_1 = ((h_angular_first_2_1 + r_80_h_angular_1_2_1) >> 1);
  int32_t h_angular_2_2 = ((h_angular_first_2_2 + r_80_h_angular_1_2_2) >> 1);
  int32_t _tmp252 = (int32_t) ((((int64_t) (lever_2_0) * (int64_t) (r_69_act_inv_angular_2_1)) >> 26));
  int32_t _tmp253 = (int32_t) ((((int64_t) (lever_2_1) * (int64_t) (r_69_act_inv_angular_2_0)) >> 26));
  int32_t f_linear_1_2_0 = ((_tmp252 - _tmp253) >> 1);
  int32_t _tmp254 = (int32_t) ((((int64_t) (lever_2_2) * (int64_t) (r_69_act_inv_angular_2_0)) >> 27));
  int32_t _tmp255 = (int32_t) ((((int64_t) (lever_2_0) * (int64_t) (r_69_act_inv_angular_2_2)) >> 25));
  int32_t f_linear_1_2_1 = (_tmp254 - (_tmp255 >> 3));
  int32_t _tmp256 = (int32_t) ((((int64_t) (lever_2_0) * (int64_t) (r_69_act_inv_angular_2_1)) >> 26));
  int32_t _tmp257 = (int32_t) ((((int64_t) (lever_2_1) * (int64_t) (r_69_act_inv_angular_2_0)) >> 26));
  int32_t f_linear_1_2_2 = ((_tmp256 - _tmp257) >> 1);
  int32_t f_linear_2_2_0 = (r_67_act_inv_linear_2_0 - (f_linear_1_2_0 >> 3));
  int32_t f_linear_2_2_1 = (r_67_act_inv_linear_2_1 - (f_linear_1_2_1 >> 3));
  int32_t f_linear_2_2_2 = (r_67_act_inv_linear_2_2 - (f_linear_1_2_2 >> 1));
  int32_t f_linear_3_2_0 = (int32_t) ((((int64_t) (masses_2) * (int64_t) (f_linear_2_2_0)) >> 31));
  int32_t f_linear_3_2_1 = (int32_t) ((((int64_t) (masses_2) * (int64_t) (f_linear_2_2_1)) >> 31));
  int32_t f_linear_3_2_2 = (int32_t) ((((int64_t) (masses_2) * (int64_t) (f_linear_2_2_2)) >> 31));
  int32_t r_81_ = (int32_t) ((((int64_t) (inertia_2_0_0) * (int64_t) (r_69_act_inv_angular_2_0)) >> 26));
  int32_t r_82_ = (int32_t) ((((int64_t) (inertia_2_0_1) * (int64_t) (r_69_act_inv_angular_2_1)) >> 24));
  int32_t r_83_ = (int32_t) ((((int64_t) (inertia_2_0_2) * (int64_t) (r_69_act_inv_angular_2_2)) >> 25));
  int32_t r_84_ = (int32_t) ((((int64_t) (inertia_2_0_1) * (int64_t) (r_69_act_inv_angular_2_0)) >> 24));
  int32_t r_85_ = (int32_t) ((((int64_t) (inertia_2_1_1) * (int64_t) (r_69_act_inv_angular_2_1)) >> 26));
  int32_t r_86_ = (int32_t) ((((int64_t) (inertia_2_1_2) * (int64_t) (r_69_act_inv_angular_2_2)) >> 25));
  int32_t r_87_ = (int32_t) ((((int64_t) (inertia_2_0_2) * (int64_t) (r_69_act_inv_angular_2_0)) >> 24));
  int32_t r_88_ = (int32_t) ((((int64_t) (inertia_2_1_2) * (int64_t) (r_69_act_inv_angular_2_1)) >> 25));
  int32_t r_89_ = (int32_t) ((((int64_t) (inertia_2_2_2) * (int64_t) (r_69_act_inv_angular_2_2)) >> 25));
  int32_t r_90_rhsMult1_temp_2 = (r_81_ + (r_82_ >> 2));
  int32_t r_91_rhsMult1_2 = (r_90_rhsMult1_temp_2 + (r_83_ >> 2));
  int32_t r_92_rhsMult2_temp_2 = ((r_84_ >> 2) + r_85_);
  int32_t r_93_rhsMult2_2 = (r_92_rhsMult2_temp_2 + (r_86_ >> 2));
  int32_t r_94_rhsMult3_temp_2 = (((r_87_ >> 1) + r_88_) >> 1);
  int32_t r_95_rhsMult3_2 = (r_94_rhsMult3_temp_2 + (r_89_ >> 2));
  int32_t r_96_rhsMult_2_0 = r_91_rhsMult1_2;
  int32_t r_96_rhsMult_2_1 = r_93_rhsMult2_2;
  int32_t r_96_rhsMult_2_2 = r_95_rhsMult3_2;
  int32_t f_angular_first_2_0 = r_96_rhsMult_2_0;
  int32_t f_angular_first_2_1 = r_96_rhsMult_2_1;
  int32_t f_angular_first_2_2 = r_96_rhsMult_2_2;
  int32_t _tmp258 = (int32_t) ((((int64_t) (lever_2_0) * (int64_t) (f_linear_3_2_1)) >> 26));
  int32_t _tmp259 = (int32_t) ((((int64_t) (lever_2_1) * (int64_t) (f_linear_3_2_0)) >> 26));
  int32_t f_angular_1_2_0 = ((_tmp258 - _tmp259) >> 1);
  int32_t _tmp260 = (int32_t) ((((int64_t) (lever_2_2) * (int64_t) (f_linear_3_2_0)) >> 27));
  int32_t _tmp261 = (int32_t) ((((int64_t) (lever_2_0) * (int64_t) (f_linear_3_2_2)) >> 26));
  int32_t f_angular_1_2_1 = (_tmp260 - (_tmp261 >> 3));
  int32_t _tmp262 = (int32_t) ((((int64_t) (lever_2_0) * (int64_t) (f_linear_3_2_1)) >> 26));
  int32_t _tmp263 = (int32_t) ((((int64_t) (lever_2_1) * (int64_t) (f_linear_3_2_0)) >> 26));
  int32_t f_angular_1_2_2 = ((_tmp262 - _tmp263) >> 1);
  int32_t f_angular_2_2_0 = ((f_angular_first_2_0 >> 2) + f_angular_1_2_0);
  int32_t f_angular_2_2_1 = (((f_angular_first_2_1 >> 2) + f_angular_1_2_1) >> 1);
  int32_t f_angular_2_2_2 = ((f_angular_first_2_2 >> 2) + f_angular_1_2_2);
  int32_t _tmp264 = (int32_t) ((((int64_t) (act_inv_angular_2_0) * (int64_t) (h_linear_2_1)) >> 31));
  int32_t _tmp265 = (int32_t) ((((int64_t) (act_inv_angular_2_1) * (int64_t) (h_linear_2_0)) >> 30));
  int32_t f_linear_4_temp_2_0 = (_tmp264 - (_tmp265 >> 1));
  int32_t _tmp266 = (int32_t) ((((int64_t) (act_inv_angular_2_2) * (int64_t) (h_linear_2_0)) >> 30));
  int32_t _tmp267 = (int32_t) ((((int64_t) (act_inv_angular_2_0) * (int64_t) (h_linear_2_2)) >> 30));
  int32_t f_linear_4_temp_2_1 = ((_tmp266 - (_tmp267 >> 2)) >> 1);
  int32_t _tmp268 = (int32_t) ((((int64_t) (act_inv_angular_2_0) * (int64_t) (h_linear_2_1)) >> 31));
  int32_t _tmp269 = (int32_t) ((((int64_t) (act_inv_angular_2_1) * (int64_t) (h_linear_2_0)) >> 30));
  int32_t f_linear_4_temp_2_2 = (_tmp268 - (_tmp269 >> 1));
  int32_t f_linear_4_2_0 = ((f_linear_3_2_0 + f_linear_4_temp_2_0) >> 1);
  int32_t f_linear_4_2_1 = ((f_linear_3_2_1 + f_linear_4_temp_2_1) >> 1);
  int32_t f_linear_4_2_2 = (((f_linear_3_2_2 >> 2) + f_linear_4_temp_2_2) >> 1);
  int32_t _tmp270 = (int32_t) ((((int64_t) (act_inv_angular_2_0) * (int64_t) (h_angular_first_2_1)) >> 30));
  int32_t _tmp271 = (int32_t) ((((int64_t) (act_inv_angular_2_1) * (int64_t) (h_angular_first_2_0)) >> 30));
  int32_t f_angular_3_temp_2_0 = ((_tmp270 - _tmp271) >> 1);
  int32_t _tmp272 = (int32_t) ((((int64_t) (act_inv_angular_2_2) * (int64_t) (h_angular_first_2_0)) >> 30));
  int32_t _tmp273 = (int32_t) ((((int64_t) (act_inv_angular_2_0) * (int64_t) (h_angular_first_2_2)) >> 29));
  int32_t f_angular_3_temp_2_1 = ((_tmp272 - (_tmp273 >> 1)) >> 1);
  int32_t _tmp274 = (int32_t) ((((int64_t) (act_inv_angular_2_0) * (int64_t) (h_angular_first_2_1)) >> 30));
  int32_t _tmp275 = (int32_t) ((((int64_t) (act_inv_angular_2_1) * (int64_t) (h_angular_first_2_0)) >> 30));
  int32_t f_angular_3_temp_2_2 = ((_tmp274 - _tmp275) >> 1);
  int32_t f_angular_3_2_0 = ((f_angular_2_2_0 + f_angular_3_temp_2_0) >> 1);
  int32_t f_angular_3_2_1 = (f_angular_2_2_1 + (f_angular_3_temp_2_1 >> 1));
  int32_t f_angular_3_2_2 = ((f_angular_2_2_2 + f_angular_3_temp_2_2) >> 1);
  int32_t _tmp276 = (int32_t) ((((int64_t) (act_inv_linear_2_0) * (int64_t) (h_linear_2_1)) >> 31));
  int32_t _tmp277 = (int32_t) ((((int64_t) (act_inv_linear_2_1) * (int64_t) (h_linear_2_0)) >> 31));
  int32_t f_angular_4_temp_2_0 = ((_tmp276 - _tmp277) >> 1);
  int32_t _tmp278 = (int32_t) ((((int64_t) (act_inv_linear_2_2) * (int64_t) (h_linear_2_0)) >> 27));
  int32_t _tmp279 = (int32_t) ((((int64_t) (act_inv_linear_2_0) * (int64_t) (h_linear_2_2)) >> 30));
  int32_t f_angular_4_temp_2_1 = ((_tmp278 >> 3) - _tmp279);
  int32_t _tmp280 = (int32_t) ((((int64_t) (act_inv_linear_2_0) * (int64_t) (h_linear_2_1)) >> 31));
  int32_t _tmp281 = (int32_t) ((((int64_t) (act_inv_linear_2_1) * (int64_t) (h_linear_2_0)) >> 31));
  int32_t f_angular_4_temp_2_2 = ((_tmp280 - _tmp281) >> 1);
  int32_t f_angular_4_2_0 = ((f_angular_3_2_0 >> 2) + f_angular_4_temp_2_0);
  int32_t f_angular_4_2_1 = (f_angular_3_2_1 + (f_angular_4_temp_2_1 >> 2));
  int32_t f_angular_4_2_2 = ((f_angular_3_2_2 >> 2) + f_angular_4_temp_2_2);
  int32_t h_rnea_firstpass2_0 = h_linear_2_0;
  int32_t h_rnea_firstpass2_1 = h_linear_2_1;
  int32_t h_rnea_firstpass2_2 = h_linear_2_2;
  int32_t h_rnea_firstpass2_3 = h_angular_2_0;
  int32_t h_rnea_firstpass2_4 = h_angular_2_1;
  int32_t h_rnea_firstpass2_5 = h_angular_2_2;
  int32_t f_rnea_firstpass2_0 = f_linear_4_2_0;
  int32_t f_rnea_firstpass2_1 = f_linear_4_2_1;
  int32_t f_rnea_firstpass2_2 = f_linear_4_2_2;
  int32_t f_rnea_firstpass2_3 = f_angular_4_2_0;
  int32_t f_rnea_firstpass2_4 = f_angular_4_2_1;
  int32_t f_rnea_firstpass2_5 = f_angular_4_2_2;
  int32_t r_97_new_v_2_0 = act_inv_linear_2_0;
  int32_t r_97_new_v_2_1 = act_inv_linear_2_1;
  int32_t r_97_new_v_2_2 = act_inv_linear_2_2;
  int32_t r_97_new_v_2_3 = act_inv_angular_2_0;
  int32_t r_97_new_v_2_4 = act_inv_angular_2_1;
  int32_t r_97_new_v_2_5 = act_inv_angular_2_2;
  int32_t rotation_matrix_3_0_0 = qcos_3;
  int32_t rotation_matrix_3_0_1 = -(qsin_3);
  int32_t rotation_matrix_3_0_2 = 0;
  int32_t rotation_matrix_3_1_0 = qsin_3;
  int32_t rotation_matrix_3_1_1 = qcos_3;
  int32_t rotation_matrix_3_1_2 = 0;
  int32_t rotation_matrix_3_2_0 = 0;
  int32_t rotation_matrix_3_2_1 = 0;
  int32_t rotation_matrix_3_2_2 = 1073741824;
  int32_t limi_rotation_3_0_0 = rotation_matrix_3_0_0;
  int32_t limi_rotation_3_0_1 = rotation_matrix_3_0_1;
  int32_t limi_rotation_3_0_2 = 0;
  int32_t limi_rotation_3_1_0 = 0;
  int32_t limi_rotation_3_1_1 = 0;
  int32_t limi_rotation_3_1_2 = -1073741825;
  int32_t limi_rotation_3_2_0 = rotation_matrix_3_1_0;
  int32_t limi_rotation_3_2_1 = rotation_matrix_3_1_1;
  int32_t limi_rotation_3_2_2 = 0;
  int32_t v_linear_3_0 = data_v_3_0;
  int32_t v_linear_3_1 = data_v_3_1;
  int32_t v_linear_3_2 = data_v_3_2;
  int32_t v_angular_3_0 = data_v_3_3;
  int32_t v_angular_3_1 = data_v_3_4;
  int32_t v_angular_3_2 = v_3;
  int32_t parent_v_linear_3_0 = r_97_new_v_2_0;
  int32_t parent_v_linear_3_1 = r_97_new_v_2_1;
  int32_t parent_v_linear_3_2 = r_97_new_v_2_2;
  int32_t parent_v_angular_3_0 = r_97_new_v_2_3;
  int32_t parent_v_angular_3_1 = r_97_new_v_2_4;
  int32_t parent_v_angular_3_2 = r_97_new_v_2_5;
  int32_t parent_a_gf_linear_3_0 = new_a_gf_up3_2_0;
  int32_t parent_a_gf_linear_3_1 = new_a_gf_up3_2_1;
  int32_t parent_a_gf_linear_3_2 = new_a_gf_up3_2_2;
  int32_t parent_a_gf_angular_3_0 = new_a_gf_up3_2_3;
  int32_t parent_a_gf_angular_3_1 = new_a_gf_up3_2_4;
  int32_t parent_a_gf_angular_3_2 = new_a_gf_up3_2_5;
  int32_t _tmp282 = (int32_t) ((((int64_t) (limi_translation_3_0) * (int64_t) (parent_v_angular_3_1)) >> 27));
  int32_t _tmp283 = (int32_t) ((((int64_t) (limi_translation_3_1) * (int64_t) (parent_v_angular_3_0)) >> 27));
  int32_t actInv1_3_0 = (_tmp282 - _tmp283);
  int32_t _tmp284 = (int32_t) ((((int64_t) (limi_translation_3_2) * (int64_t) (parent_v_angular_3_0)) >> 27));
  int32_t _tmp285 = (int32_t) ((((int64_t) (limi_translation_3_0) * (int64_t) (parent_v_angular_3_2)) >> 27));
  int32_t actInv1_3_1 = (_tmp284 - _tmp285);
  int32_t _tmp286 = (int32_t) ((((int64_t) (limi_translation_3_0) * (int64_t) (parent_v_angular_3_1)) >> 27));
  int32_t _tmp287 = (int32_t) ((((int64_t) (limi_translation_3_1) * (int64_t) (parent_v_angular_3_0)) >> 27));
  int32_t actInv1_3_2 = (_tmp286 - _tmp287);
  int32_t actInv2_3_0 = (parent_v_linear_3_0 - (actInv1_3_0 >> 2));
  int32_t actInv2_3_1 = (parent_v_linear_3_1 - (actInv1_3_1 >> 2));
  int32_t actInv2_3_2 = (parent_v_linear_3_2 - actInv1_3_2);
  int32_t actInv3_3_0_0 = limi_rotation_3_0_0;
  int32_t actInv3_3_1_0 = limi_rotation_3_0_1;
  int32_t actInv3_3_2_0 = limi_rotation_3_0_2;
  int32_t actInv3_3_0_1 = limi_rotation_3_1_0;
  int32_t actInv3_3_1_1 = limi_rotation_3_1_1;
  int32_t actInv3_3_2_1 = limi_rotation_3_1_2;
  int32_t actInv3_3_0_2 = limi_rotation_3_2_0;
  int32_t actInv3_3_1_2 = limi_rotation_3_2_1;
  int32_t actInv3_3_2_2 = limi_rotation_3_2_2;
  int32_t _tmp288 = (int32_t) ((((int64_t) (actInv3_3_0_0) * (int64_t) (actInv2_3_0)) >> 30));
  int32_t _tmp289 = (int32_t) ((((int64_t) (actInv3_3_0_1) * (int64_t) (actInv2_3_1)) >> 29));
  int32_t _tmp290 = (_tmp288 + (_tmp289 >> 2));
  int32_t _tmp291 = (int32_t) ((((int64_t) (actInv3_3_0_2) * (int64_t) (actInv2_3_2)) >> 30));
  int32_t actInv4_3_0 = ((_tmp290 + (_tmp291 >> 2)) >> 1);
  int32_t _tmp292 = (int32_t) ((((int64_t) (actInv3_3_1_0) * (int64_t) (actInv2_3_0)) >> 30));
  int32_t _tmp293 = (int32_t) ((((int64_t) (actInv3_3_1_1) * (int64_t) (actInv2_3_1)) >> 29));
  int32_t _tmp294 = (_tmp292 + (_tmp293 >> 2));
  int32_t _tmp295 = (int32_t) ((((int64_t) (actInv3_3_1_2) * (int64_t) (actInv2_3_2)) >> 30));
  int32_t actInv4_3_1 = ((_tmp294 + (_tmp295 >> 2)) >> 1);
  int32_t _tmp296 = (int32_t) ((((int64_t) (actInv3_3_2_0) * (int64_t) (actInv2_3_0)) >> 29));
  int32_t _tmp297 = (int32_t) ((((int64_t) (actInv3_3_2_1) * (int64_t) (actInv2_3_1)) >> 30));
  int32_t _tmp298 = ((_tmp296 >> 2) + _tmp297);
  int32_t _tmp299 = (int32_t) ((((int64_t) (actInv3_3_2_2) * (int64_t) (actInv2_3_2)) >> 31));
  int32_t actInv4_3_2 = (_tmp298 + (_tmp299 >> 2));
  int32_t act_inv_linear_3_0 = ((v_linear_3_0 >> 3) + actInv4_3_0);
  int32_t act_inv_linear_3_1 = ((v_linear_3_1 >> 3) + actInv4_3_1);
  int32_t act_inv_linear_3_2 = ((v_linear_3_2 >> 2) + actInv4_3_2);
  int32_t _tmp300 = (int32_t) ((((int64_t) (actInv3_3_0_0) * (int64_t) (parent_v_angular_3_0)) >> 30));
  int32_t _tmp301 = (int32_t) ((((int64_t) (actInv3_3_0_1) * (int64_t) (parent_v_angular_3_1)) >> 27));
  int32_t _tmp302 = (_tmp300 + (_tmp301 >> 4));
  int32_t _tmp303 = (int32_t) ((((int64_t) (actInv3_3_0_2) * (int64_t) (parent_v_angular_3_2)) >> 30));
  int32_t actInv5_3_0 = ((_tmp302 + _tmp303) >> 1);
  int32_t _tmp304 = (int32_t) ((((int64_t) (actInv3_3_1_0) * (int64_t) (parent_v_angular_3_0)) >> 30));
  int32_t _tmp305 = (int32_t) ((((int64_t) (actInv3_3_1_1) * (int64_t) (parent_v_angular_3_1)) >> 27));
  int32_t _tmp306 = (_tmp304 + (_tmp305 >> 4));
  int32_t _tmp307 = (int32_t) ((((int64_t) (actInv3_3_1_2) * (int64_t) (parent_v_angular_3_2)) >> 30));
  int32_t actInv5_3_1 = ((_tmp306 + _tmp307) >> 1);
  int32_t _tmp308 = (int32_t) ((((int64_t) (actInv3_3_2_0) * (int64_t) (parent_v_angular_3_0)) >> 27));
  int32_t _tmp309 = (int32_t) ((((int64_t) (actInv3_3_2_1) * (int64_t) (parent_v_angular_3_1)) >> 30));
  int32_t _tmp310 = ((_tmp308 >> 4) + _tmp309);
  int32_t _tmp311 = (int32_t) ((((int64_t) (actInv3_3_2_2) * (int64_t) (parent_v_angular_3_2)) >> 27));
  int32_t actInv5_3_2 = (_tmp310 + (_tmp311 >> 4));
  int32_t act_inv_angular_3_0 = ((v_angular_3_0 >> 5) + actInv5_3_0);
  int32_t act_inv_angular_3_1 = ((v_angular_3_1 >> 5) + actInv5_3_1);
  int32_t act_inv_angular_3_2 = ((v_angular_3_2 >> 1) + actInv5_3_2);
  int32_t minus_m_w_3 = -(v_3);
  int32_t _tmp312 = -(minus_m_w_3);
  int32_t alpha_cross1_linear_3 = (int32_t) ((((int64_t) (_tmp312) * (int64_t) (act_inv_linear_3_1)) >> 30));
  int32_t alpha_cross2_linear_3 = (int32_t) ((((int64_t) (minus_m_w_3) * (int64_t) (act_inv_linear_3_0)) >> 30));
  int32_t alpha_cross_linear_3_0 = alpha_cross1_linear_3;
  int32_t alpha_cross_linear_3_1 = alpha_cross2_linear_3;
  int32_t alpha_cross_linear_3_2 = 0;
  int32_t _tmp313 = -(minus_m_w_3);
  int32_t alpha_cross1_angular_3 = (int32_t) ((((int64_t) (_tmp313) * (int64_t) (act_inv_angular_3_1)) >> 30));
  int32_t alpha_cross2_angular_3 = (int32_t) ((((int64_t) (minus_m_w_3) * (int64_t) (act_inv_angular_3_0)) >> 30));
  int32_t alpha_cross_angular_3_0 = alpha_cross1_angular_3;
  int32_t alpha_cross_angular_3_1 = alpha_cross2_angular_3;
  int32_t alpha_cross_angular_3_2 = 0;
  int32_t r_98_new_a_gf_3_0 = alpha_cross_linear_3_0;
  int32_t r_98_new_a_gf_3_1 = alpha_cross_linear_3_1;
  int32_t r_98_new_a_gf_3_2 = alpha_cross_linear_3_2;
  int32_t r_98_new_a_gf_3_3 = alpha_cross_angular_3_0;
  int32_t r_98_new_a_gf_3_4 = alpha_cross_angular_3_1;
  int32_t r_98_new_a_gf_3_5 = alpha_cross_angular_3_2;
  int32_t new_a_gf_up1_3 = (a_3 + (r_98_new_a_gf_3_5 >> 3));
  int32_t new_a_gf2_linear_3_0 = r_98_new_a_gf_3_0;
  int32_t new_a_gf2_linear_3_1 = r_98_new_a_gf_3_1;
  int32_t new_a_gf2_linear_3_2 = r_98_new_a_gf_3_2;
  int32_t new_a_gf2_angular_3_0 = r_98_new_a_gf_3_3;
  int32_t new_a_gf2_angular_3_1 = r_98_new_a_gf_3_4;
  int32_t new_a_gf2_angular_3_2 = new_a_gf_up1_3;
  int32_t _tmp314 = (int32_t) ((((int64_t) (limi_translation_3_0) * (int64_t) (parent_a_gf_angular_3_1)) >> 27));
  int32_t _tmp315 = (int32_t) ((((int64_t) (limi_translation_3_1) * (int64_t) (parent_a_gf_angular_3_0)) >> 24));
  int32_t r_99_actInv1_3_0 = (_tmp314 - (_tmp315 >> 3));
  int32_t _tmp316 = (int32_t) ((((int64_t) (limi_translation_3_2) * (int64_t) (parent_a_gf_angular_3_0)) >> 24));
  int32_t _tmp317 = (int32_t) ((((int64_t) (limi_translation_3_0) * (int64_t) (parent_a_gf_angular_3_2)) >> 27));
  int32_t r_99_actInv1_3_1 = ((_tmp316 >> 2) - _tmp317);
  int32_t _tmp318 = (int32_t) ((((int64_t) (limi_translation_3_0) * (int64_t) (parent_a_gf_angular_3_1)) >> 27));
  int32_t _tmp319 = (int32_t) ((((int64_t) (limi_translation_3_1) * (int64_t) (parent_a_gf_angular_3_0)) >> 24));
  int32_t r_99_actInv1_3_2 = (_tmp318 - (_tmp319 >> 3));
  int32_t r_100_actInv2_3_0 = (parent_a_gf_linear_3_0 - (r_99_actInv1_3_0 >> 3));
  int32_t r_100_actInv2_3_1 = (parent_a_gf_linear_3_1 - (r_99_actInv1_3_1 >> 4));
  int32_t r_100_actInv2_3_2 = ((parent_a_gf_linear_3_2 - (r_99_actInv1_3_2 >> 1)) >> 1);
  int32_t r_101_actInv3_3_0_0 = limi_rotation_3_0_0;
  int32_t r_101_actInv3_3_1_0 = limi_rotation_3_0_1;
  int32_t r_101_actInv3_3_2_0 = limi_rotation_3_0_2;
  int32_t r_101_actInv3_3_0_1 = limi_rotation_3_1_0;
  int32_t r_101_actInv3_3_1_1 = limi_rotation_3_1_1;
  int32_t r_101_actInv3_3_2_1 = limi_rotation_3_1_2;
  int32_t r_101_actInv3_3_0_2 = limi_rotation_3_2_0;
  int32_t r_101_actInv3_3_1_2 = limi_rotation_3_2_1;
  int32_t r_101_actInv3_3_2_2 = limi_rotation_3_2_2;
  int32_t _tmp320 = (int32_t) ((((int64_t) (r_101_actInv3_3_0_0) * (int64_t) (r_100_actInv2_3_0)) >> 30));
  int32_t _tmp321 = (int32_t) ((((int64_t) (r_101_actInv3_3_0_1) * (int64_t) (r_100_actInv2_3_1)) >> 25));
  int32_t _tmp322 = (_tmp320 + (_tmp321 >> 6));
  int32_t _tmp323 = (int32_t) ((((int64_t) (r_101_actInv3_3_0_2) * (int64_t) (r_100_actInv2_3_2)) >> 30));
  int32_t r_102_actInv4_3_0 = ((_tmp322 + (_tmp323 >> 1)) >> 1);
  int32_t _tmp324 = (int32_t) ((((int64_t) (r_101_actInv3_3_1_0) * (int64_t) (r_100_actInv2_3_0)) >> 30));
  int32_t _tmp325 = (int32_t) ((((int64_t) (r_101_actInv3_3_1_1) * (int64_t) (r_100_actInv2_3_1)) >> 25));
  int32_t _tmp326 = (_tmp324 + (_tmp325 >> 6));
  int32_t _tmp327 = (int32_t) ((((int64_t) (r_101_actInv3_3_1_2) * (int64_t) (r_100_actInv2_3_2)) >> 30));
  int32_t r_102_actInv4_3_1 = ((_tmp326 + (_tmp327 >> 1)) >> 1);
  int32_t _tmp328 = (int32_t) ((((int64_t) (r_101_actInv3_3_2_0) * (int64_t) (r_100_actInv2_3_0)) >> 25));
  int32_t _tmp329 = (int32_t) ((((int64_t) (r_101_actInv3_3_2_1) * (int64_t) (r_100_actInv2_3_1)) >> 30));
  int32_t _tmp330 = ((_tmp328 >> 6) + _tmp329);
  int32_t _tmp331 = (int32_t) ((((int64_t) (r_101_actInv3_3_2_2) * (int64_t) (r_100_actInv2_3_2)) >> 26));
  int32_t r_102_actInv4_3_2 = (_tmp330 + (_tmp331 >> 6));
  int32_t r_103_act_inv_linear_3_0 = ((new_a_gf2_linear_3_0 >> 2) + r_102_actInv4_3_0);
  int32_t r_103_act_inv_linear_3_1 = ((new_a_gf2_linear_3_1 >> 2) + r_102_actInv4_3_1);
  int32_t r_103_act_inv_linear_3_2 = ((new_a_gf2_linear_3_2 >> 6) + r_102_actInv4_3_2);
  int32_t _tmp332 = (int32_t) ((((int64_t) (r_101_actInv3_3_0_0) * (int64_t) (parent_a_gf_angular_3_0)) >> 30));
  int32_t _tmp333 = (int32_t) ((((int64_t) (r_101_actInv3_3_0_1) * (int64_t) (parent_a_gf_angular_3_1)) >> 24));
  int32_t _tmp334 = (_tmp332 + (_tmp333 >> 7));
  int32_t _tmp335 = (int32_t) ((((int64_t) (r_101_actInv3_3_0_2) * (int64_t) (parent_a_gf_angular_3_2)) >> 30));
  int32_t r_104_actInv5_3_0 = (_tmp334 + (_tmp335 >> 1));
  int32_t _tmp336 = (int32_t) ((((int64_t) (r_101_actInv3_3_1_0) * (int64_t) (parent_a_gf_angular_3_0)) >> 30));
  int32_t _tmp337 = (int32_t) ((((int64_t) (r_101_actInv3_3_1_1) * (int64_t) (parent_a_gf_angular_3_1)) >> 24));
  int32_t _tmp338 = (_tmp336 + (_tmp337 >> 7));
  int32_t _tmp339 = (int32_t) ((((int64_t) (r_101_actInv3_3_1_2) * (int64_t) (parent_a_gf_angular_3_2)) >> 30));
  int32_t r_104_actInv5_3_1 = (_tmp338 + (_tmp339 >> 1));
  int32_t _tmp340 = (int32_t) ((((int64_t) (r_101_actInv3_3_2_0) * (int64_t) (parent_a_gf_angular_3_0)) >> 24));
  int32_t _tmp341 = (int32_t) ((((int64_t) (r_101_actInv3_3_2_1) * (int64_t) (parent_a_gf_angular_3_1)) >> 30));
  int32_t _tmp342 = ((_tmp340 >> 7) + _tmp341);
  int32_t _tmp343 = (int32_t) ((((int64_t) (r_101_actInv3_3_2_2) * (int64_t) (parent_a_gf_angular_3_2)) >> 25));
  int32_t r_104_actInv5_3_2 = (_tmp342 + (_tmp343 >> 7));
  int32_t r_105_act_inv_angular_3_0 = ((new_a_gf2_angular_3_0 + r_104_actInv5_3_0) >> 1);
  int32_t r_105_act_inv_angular_3_1 = ((new_a_gf2_angular_3_1 + r_104_actInv5_3_1) >> 1);
  int32_t r_105_act_inv_angular_3_2 = ((new_a_gf2_angular_3_2 >> 4) + r_104_actInv5_3_2);
  int32_t new_a_gf_up3_3_0 = r_103_act_inv_linear_3_0;
  int32_t new_a_gf_up3_3_1 = r_103_act_inv_linear_3_1;
  int32_t new_a_gf_up3_3_2 = r_103_act_inv_linear_3_2;
  int32_t new_a_gf_up3_3_3 = r_105_act_inv_angular_3_0;
  int32_t new_a_gf_up3_3_4 = r_105_act_inv_angular_3_1;
  int32_t new_a_gf_up3_3_5 = r_105_act_inv_angular_3_2;
  int32_t _tmp344 = (int32_t) ((((int64_t) (lever_3_0) * (int64_t) (act_inv_angular_3_1)) >> 27));
  int32_t _tmp345 = (int32_t) ((((int64_t) (lever_3_1) * (int64_t) (act_inv_angular_3_0)) >> 28));
  int32_t h_linear_1_3_0 = ((_tmp344 >> 1) - _tmp345);
  int32_t _tmp346 = (int32_t) ((((int64_t) (lever_3_2) * (int64_t) (act_inv_angular_3_0)) >> 26));
  int32_t _tmp347 = (int32_t) ((((int64_t) (lever_3_0) * (int64_t) (act_inv_angular_3_2)) >> 27));
  int32_t h_linear_1_3_1 = ((_tmp346 - _tmp347) >> 1);
  int32_t _tmp348 = (int32_t) ((((int64_t) (lever_3_0) * (int64_t) (act_inv_angular_3_1)) >> 27));
  int32_t _tmp349 = (int32_t) ((((int64_t) (lever_3_1) * (int64_t) (act_inv_angular_3_0)) >> 28));
  int32_t h_linear_1_3_2 = ((_tmp348 >> 1) - _tmp349);
  int32_t h_linear_2_3_0 = (act_inv_linear_3_0 - (h_linear_1_3_0 >> 1));
  int32_t h_linear_2_3_1 = (act_inv_linear_3_1 - (h_linear_1_3_1 >> 2));
  int32_t h_linear_2_3_2 = ((act_inv_linear_3_2 - h_linear_1_3_2) >> 1);
  int32_t h_linear_3_0 = (int32_t) ((((int64_t) (masses_3) * (int64_t) (h_linear_2_3_0)) >> 31));
  int32_t h_linear_3_1 = (int32_t) ((((int64_t) (masses_3) * (int64_t) (h_linear_2_3_1)) >> 31));
  int32_t h_linear_3_2 = (int32_t) ((((int64_t) (masses_3) * (int64_t) (h_linear_2_3_2)) >> 31));
  int32_t r_106_ = (int32_t) ((((int64_t) (inertia_3_0_0) * (int64_t) (act_inv_angular_3_0)) >> 26));
  int32_t r_107_ = (int32_t) ((((int64_t) (inertia_3_0_1) * (int64_t) (act_inv_angular_3_1)) >> 26));
  int32_t r_108_ = (int32_t) ((((int64_t) (inertia_3_0_2) * (int64_t) (act_inv_angular_3_2)) >> 27));
  int32_t r_109_ = (int32_t) ((((int64_t) (inertia_3_0_1) * (int64_t) (act_inv_angular_3_0)) >> 26));
  int32_t r_110_ = (int32_t) ((((int64_t) (inertia_3_1_1) * (int64_t) (act_inv_angular_3_1)) >> 26));
  int32_t r_111_ = (int32_t) ((((int64_t) (inertia_3_1_2) * (int64_t) (act_inv_angular_3_2)) >> 27));
  int32_t r_112_ = (int32_t) ((((int64_t) (inertia_3_0_2) * (int64_t) (act_inv_angular_3_0)) >> 26));
  int32_t r_113_ = (int32_t) ((((int64_t) (inertia_3_1_2) * (int64_t) (act_inv_angular_3_1)) >> 26));
  int32_t r_114_ = (int32_t) ((((int64_t) (inertia_3_2_2) * (int64_t) (act_inv_angular_3_2)) >> 27));
  int32_t rhsMult1_temp_3 = (r_106_ + r_107_);
  int32_t rhsMult1_3 = (rhsMult1_temp_3 + r_108_);
  int32_t rhsMult2_temp_3 = (r_109_ + r_110_);
  int32_t rhsMult2_3 = (rhsMult2_temp_3 + r_111_);
  int32_t rhsMult3_temp_3 = (r_112_ + r_113_);
  int32_t rhsMult3_3 = (rhsMult3_temp_3 + r_114_);
  int32_t rhsMult_3_0 = rhsMult1_3;
  int32_t rhsMult_3_1 = rhsMult2_3;
  int32_t rhsMult_3_2 = rhsMult3_3;
  int32_t h_angular_first_3_0 = rhsMult_3_0;
  int32_t h_angular_first_3_1 = rhsMult_3_1;
  int32_t h_angular_first_3_2 = rhsMult_3_2;
  int32_t _tmp350 = (int32_t) ((((int64_t) (lever_3_0) * (int64_t) (h_linear_3_1)) >> 27));
  int32_t _tmp351 = (int32_t) ((((int64_t) (lever_3_1) * (int64_t) (h_linear_3_0)) >> 28));
  int32_t h_angular_1_3_0 = (((_tmp350 >> 1) - _tmp351) >> 1);
  int32_t _tmp352 = (int32_t) ((((int64_t) (lever_3_2) * (int64_t) (h_linear_3_0)) >> 26));
  int32_t _tmp353 = (int32_t) ((((int64_t) (lever_3_0) * (int64_t) (h_linear_3_2)) >> 27));
  int32_t h_angular_1_3_1 = (((_tmp352 >> 1) - _tmp353) >> 1);
  int32_t _tmp354 = (int32_t) ((((int64_t) (lever_3_0) * (int64_t) (h_linear_3_1)) >> 27));
  int32_t _tmp355 = (int32_t) ((((int64_t) (lever_3_1) * (int64_t) (h_linear_3_0)) >> 28));
  int32_t h_angular_1_3_2 = (((_tmp354 >> 1) - _tmp355) >> 1);
  int32_t h_angular_3_0 = ((h_angular_first_3_0 >> 3) + h_angular_1_3_0);
  int32_t h_angular_3_1 = ((h_angular_first_3_1 >> 2) + h_angular_1_3_1);
  int32_t h_angular_3_2 = ((h_angular_first_3_2 >> 3) + h_angular_1_3_2);
  int32_t _tmp356 = (int32_t) ((((int64_t) (lever_3_0) * (int64_t) (r_105_act_inv_angular_3_1)) >> 27));
  int32_t _tmp357 = (int32_t) ((((int64_t) (lever_3_1) * (int64_t) (r_105_act_inv_angular_3_0)) >> 28));
  int32_t f_linear_1_3_0 = (((_tmp356 >> 1) - _tmp357) >> 1);
  int32_t _tmp358 = (int32_t) ((((int64_t) (lever_3_2) * (int64_t) (r_105_act_inv_angular_3_0)) >> 26));
  int32_t _tmp359 = (int32_t) ((((int64_t) (lever_3_0) * (int64_t) (r_105_act_inv_angular_3_2)) >> 27));
  int32_t f_linear_1_3_1 = ((_tmp358 - _tmp359) >> 1);
  int32_t _tmp360 = (int32_t) ((((int64_t) (lever_3_0) * (int64_t) (r_105_act_inv_angular_3_1)) >> 27));
  int32_t _tmp361 = (int32_t) ((((int64_t) (lever_3_1) * (int64_t) (r_105_act_inv_angular_3_0)) >> 28));
  int32_t f_linear_1_3_2 = (((_tmp360 >> 1) - _tmp361) >> 1);
  int32_t f_linear_2_3_0 = (r_103_act_inv_linear_3_0 - (f_linear_1_3_0 >> 1));
  int32_t f_linear_2_3_1 = (r_103_act_inv_linear_3_1 - (f_linear_1_3_1 >> 3));
  int32_t f_linear_2_3_2 = ((r_103_act_inv_linear_3_2 - f_linear_1_3_2) >> 1);
  int32_t f_linear_3_3_0 = (int32_t) ((((int64_t) (masses_3) * (int64_t) (f_linear_2_3_0)) >> 31));
  int32_t f_linear_3_3_1 = (int32_t) ((((int64_t) (masses_3) * (int64_t) (f_linear_2_3_1)) >> 31));
  int32_t f_linear_3_3_2 = (int32_t) ((((int64_t) (masses_3) * (int64_t) (f_linear_2_3_2)) >> 31));
  int32_t r_115_ = (int32_t) ((((int64_t) (inertia_3_0_0) * (int64_t) (r_105_act_inv_angular_3_0)) >> 26));
  int32_t r_116_ = (int32_t) ((((int64_t) (inertia_3_0_1) * (int64_t) (r_105_act_inv_angular_3_1)) >> 24));
  int32_t r_117_ = (int32_t) ((((int64_t) (inertia_3_0_2) * (int64_t) (r_105_act_inv_angular_3_2)) >> 24));
  int32_t r_118_ = (int32_t) ((((int64_t) (inertia_3_0_1) * (int64_t) (r_105_act_inv_angular_3_0)) >> 24));
  int32_t r_119_ = (int32_t) ((((int64_t) (inertia_3_1_1) * (int64_t) (r_105_act_inv_angular_3_1)) >> 26));
  int32_t r_120_ = (int32_t) ((((int64_t) (inertia_3_1_2) * (int64_t) (r_105_act_inv_angular_3_2)) >> 24));
  int32_t r_121_ = (int32_t) ((((int64_t) (inertia_3_0_2) * (int64_t) (r_105_act_inv_angular_3_0)) >> 23));
  int32_t r_122_ = (int32_t) ((((int64_t) (inertia_3_1_2) * (int64_t) (r_105_act_inv_angular_3_1)) >> 24));
  int32_t r_123_ = (int32_t) ((((int64_t) (inertia_3_2_2) * (int64_t) (r_105_act_inv_angular_3_2)) >> 26));
  int32_t r_124_rhsMult1_temp_3 = (r_115_ + (r_116_ >> 2));
  int32_t r_125_rhsMult1_3 = (r_124_rhsMult1_temp_3 + (r_117_ >> 3));
  int32_t r_126_rhsMult2_temp_3 = ((r_118_ >> 2) + r_119_);
  int32_t r_127_rhsMult2_3 = (r_126_rhsMult2_temp_3 + (r_120_ >> 3));
  int32_t r_128_rhsMult3_temp_3 = (((r_121_ >> 1) + r_122_) >> 1);
  int32_t r_129_rhsMult3_3 = ((r_128_rhsMult3_temp_3 + r_123_) >> 1);
  int32_t r_130_rhsMult_3_0 = r_125_rhsMult1_3;
  int32_t r_130_rhsMult_3_1 = r_127_rhsMult2_3;
  int32_t r_130_rhsMult_3_2 = r_129_rhsMult3_3;
  int32_t f_angular_first_3_0 = r_130_rhsMult_3_0;
  int32_t f_angular_first_3_1 = r_130_rhsMult_3_1;
  int32_t f_angular_first_3_2 = r_130_rhsMult_3_2;
  int32_t _tmp362 = (int32_t) ((((int64_t) (lever_3_0) * (int64_t) (f_linear_3_3_1)) >> 27));
  int32_t _tmp363 = (int32_t) ((((int64_t) (lever_3_1) * (int64_t) (f_linear_3_3_0)) >> 28));
  int32_t f_angular_1_3_0 = (((_tmp362 >> 1) - _tmp363) >> 1);
  int32_t _tmp364 = (int32_t) ((((int64_t) (lever_3_2) * (int64_t) (f_linear_3_3_0)) >> 26));
  int32_t _tmp365 = (int32_t) ((((int64_t) (lever_3_0) * (int64_t) (f_linear_3_3_2)) >> 26));
  int32_t f_angular_1_3_1 = ((_tmp364 - _tmp365) >> 1);
  int32_t _tmp366 = (int32_t) ((((int64_t) (lever_3_0) * (int64_t) (f_linear_3_3_1)) >> 27));
  int32_t _tmp367 = (int32_t) ((((int64_t) (lever_3_1) * (int64_t) (f_linear_3_3_0)) >> 28));
  int32_t f_angular_1_3_2 = (((_tmp366 >> 1) - _tmp367) >> 1);
  int32_t f_angular_2_3_0 = ((f_angular_first_3_0 >> 4) + f_angular_1_3_0);
  int32_t f_angular_2_3_1 = (((f_angular_first_3_1 >> 2) + f_angular_1_3_1) >> 1);
  int32_t f_angular_2_3_2 = ((f_angular_first_3_2 >> 4) + f_angular_1_3_2);
  int32_t _tmp368 = (int32_t) ((((int64_t) (act_inv_angular_3_0) * (int64_t) (h_linear_3_1)) >> 30));
  int32_t _tmp369 = (int32_t) ((((int64_t) (act_inv_angular_3_1) * (int64_t) (h_linear_3_0)) >> 31));
  int32_t f_linear_4_temp_3_0 = ((_tmp368 >> 1) - _tmp369);
  int32_t _tmp370 = (int32_t) ((((int64_t) (act_inv_angular_3_2) * (int64_t) (h_linear_3_0)) >> 31));
  int32_t _tmp371 = (int32_t) ((((int64_t) (act_inv_angular_3_0) * (int64_t) (h_linear_3_2)) >> 31));
  int32_t f_linear_4_temp_3_1 = ((_tmp370 >> 1) - _tmp371);
  int32_t _tmp372 = (int32_t) ((((int64_t) (act_inv_angular_3_0) * (int64_t) (h_linear_3_1)) >> 30));
  int32_t _tmp373 = (int32_t) ((((int64_t) (act_inv_angular_3_1) * (int64_t) (h_linear_3_0)) >> 31));
  int32_t f_linear_4_temp_3_2 = ((_tmp372 >> 1) - _tmp373);
  int32_t f_linear_4_3_0 = (((f_linear_3_3_0 >> 1) + f_linear_4_temp_3_0) >> 1);
  int32_t f_linear_4_3_1 = (((f_linear_3_3_1 >> 1) + f_linear_4_temp_3_1) >> 1);
  int32_t f_linear_4_3_2 = (((f_linear_3_3_2 >> 1) + f_linear_4_temp_3_2) >> 1);
  int32_t _tmp374 = (int32_t) ((((int64_t) (act_inv_angular_3_0) * (int64_t) (h_angular_first_3_1)) >> 30));
  int32_t _tmp375 = (int32_t) ((((int64_t) (act_inv_angular_3_1) * (int64_t) (h_angular_first_3_0)) >> 30));
  int32_t f_angular_3_temp_3_0 = ((_tmp374 - _tmp375) >> 1);
  int32_t _tmp376 = (int32_t) ((((int64_t) (act_inv_angular_3_2) * (int64_t) (h_angular_first_3_0)) >> 31));
  int32_t _tmp377 = (int32_t) ((((int64_t) (act_inv_angular_3_0) * (int64_t) (h_angular_first_3_2)) >> 30));
  int32_t f_angular_3_temp_3_1 = ((_tmp376 - _tmp377) >> 1);
  int32_t _tmp378 = (int32_t) ((((int64_t) (act_inv_angular_3_0) * (int64_t) (h_angular_first_3_1)) >> 30));
  int32_t _tmp379 = (int32_t) ((((int64_t) (act_inv_angular_3_1) * (int64_t) (h_angular_first_3_0)) >> 30));
  int32_t f_angular_3_temp_3_2 = ((_tmp378 - _tmp379) >> 1);
  int32_t f_angular_3_3_0 = (f_angular_2_3_0 + (f_angular_3_temp_3_0 >> 2));
  int32_t f_angular_3_3_1 = (f_angular_2_3_1 + (f_angular_3_temp_3_1 >> 1));
  int32_t f_angular_3_3_2 = (f_angular_2_3_2 + (f_angular_3_temp_3_2 >> 2));
  int32_t _tmp380 = (int32_t) ((((int64_t) (act_inv_linear_3_0) * (int64_t) (h_linear_3_1)) >> 30));
  int32_t _tmp381 = (int32_t) ((((int64_t) (act_inv_linear_3_1) * (int64_t) (h_linear_3_0)) >> 31));
  int32_t f_angular_4_temp_3_0 = ((_tmp380 >> 1) - _tmp381);
  int32_t _tmp382 = (int32_t) ((((int64_t) (act_inv_linear_3_2) * (int64_t) (h_linear_3_0)) >> 31));
  int32_t _tmp383 = (int32_t) ((((int64_t) (act_inv_linear_3_0) * (int64_t) (h_linear_3_2)) >> 30));
  int32_t f_angular_4_temp_3_1 = ((_tmp382 - _tmp383) >> 1);
  int32_t _tmp384 = (int32_t) ((((int64_t) (act_inv_linear_3_0) * (int64_t) (h_linear_3_1)) >> 30));
  int32_t _tmp385 = (int32_t) ((((int64_t) (act_inv_linear_3_1) * (int64_t) (h_linear_3_0)) >> 31));
  int32_t f_angular_4_temp_3_2 = ((_tmp384 >> 1) - _tmp385);
  int32_t f_angular_4_3_0 = (((f_angular_3_3_0 >> 1) + f_angular_4_temp_3_0) >> 1);
  int32_t f_angular_4_3_1 = (((f_angular_3_3_1 >> 2) + f_angular_4_temp_3_1) >> 1);
  int32_t f_angular_4_3_2 = (((f_angular_3_3_2 >> 1) + f_angular_4_temp_3_2) >> 1);
  int32_t h_rnea_firstpass3_0 = h_linear_3_0;
  int32_t h_rnea_firstpass3_1 = h_linear_3_1;
  int32_t h_rnea_firstpass3_2 = h_linear_3_2;
  int32_t h_rnea_firstpass3_3 = h_angular_3_0;
  int32_t h_rnea_firstpass3_4 = h_angular_3_1;
  int32_t h_rnea_firstpass3_5 = h_angular_3_2;
  int32_t f_rnea_firstpass3_0 = f_linear_4_3_0;
  int32_t f_rnea_firstpass3_1 = f_linear_4_3_1;
  int32_t f_rnea_firstpass3_2 = f_linear_4_3_2;
  int32_t f_rnea_firstpass3_3 = f_angular_4_3_0;
  int32_t f_rnea_firstpass3_4 = f_angular_4_3_1;
  int32_t f_rnea_firstpass3_5 = f_angular_4_3_2;
  int32_t r_131_new_v_3_0 = act_inv_linear_3_0;
  int32_t r_131_new_v_3_1 = act_inv_linear_3_1;
  int32_t r_131_new_v_3_2 = act_inv_linear_3_2;
  int32_t r_131_new_v_3_3 = act_inv_angular_3_0;
  int32_t r_131_new_v_3_4 = act_inv_angular_3_1;
  int32_t r_131_new_v_3_5 = act_inv_angular_3_2;
  int32_t rotation_matrix_4_0_0 = qcos_4;
  int32_t rotation_matrix_4_0_1 = -(qsin_4);
  int32_t rotation_matrix_4_0_2 = 0;
  int32_t rotation_matrix_4_1_0 = qsin_4;
  int32_t rotation_matrix_4_1_1 = qcos_4;
  int32_t rotation_matrix_4_1_2 = 0;
  int32_t rotation_matrix_4_2_0 = 0;
  int32_t rotation_matrix_4_2_1 = 0;
  int32_t rotation_matrix_4_2_2 = 1073741824;
  int32_t limi_rotation_4_0_0 = rotation_matrix_4_0_0;
  int32_t limi_rotation_4_0_1 = rotation_matrix_4_0_1;
  int32_t limi_rotation_4_0_2 = 0;
  int32_t limi_rotation_4_1_0 = 0;
  int32_t limi_rotation_4_1_1 = 0;
  int32_t limi_rotation_4_1_2 = 1073741824;
  int32_t limi_rotation_4_2_0 = -(rotation_matrix_4_1_0);
  int32_t limi_rotation_4_2_1 = -(rotation_matrix_4_1_1);
  int32_t limi_rotation_4_2_2 = 0;
  int32_t v_linear_4_0 = data_v_4_0;
  int32_t v_linear_4_1 = data_v_4_1;
  int32_t v_linear_4_2 = data_v_4_2;
  int32_t v_angular_4_0 = data_v_4_3;
  int32_t v_angular_4_1 = data_v_4_4;
  int32_t v_angular_4_2 = v_4;
  int32_t parent_v_linear_4_0 = r_131_new_v_3_0;
  int32_t parent_v_linear_4_1 = r_131_new_v_3_1;
  int32_t parent_v_linear_4_2 = r_131_new_v_3_2;
  int32_t parent_v_angular_4_0 = r_131_new_v_3_3;
  int32_t parent_v_angular_4_1 = r_131_new_v_3_4;
  int32_t parent_v_angular_4_2 = r_131_new_v_3_5;
  int32_t parent_a_gf_linear_4_0 = new_a_gf_up3_3_0;
  int32_t parent_a_gf_linear_4_1 = new_a_gf_up3_3_1;
  int32_t parent_a_gf_linear_4_2 = new_a_gf_up3_3_2;
  int32_t parent_a_gf_angular_4_0 = new_a_gf_up3_3_3;
  int32_t parent_a_gf_angular_4_1 = new_a_gf_up3_3_4;
  int32_t parent_a_gf_angular_4_2 = new_a_gf_up3_3_5;
  int32_t _tmp386 = (int32_t) ((((int64_t) (limi_translation_4_0) * (int64_t) (parent_v_angular_4_1)) >> 27));
  int32_t _tmp387 = (int32_t) ((((int64_t) (limi_translation_4_1) * (int64_t) (parent_v_angular_4_0)) >> 29));
  int32_t actInv1_4_0 = (((_tmp386 >> 2) - _tmp387) >> 1);
  int32_t _tmp388 = (int32_t) ((((int64_t) (limi_translation_4_2) * (int64_t) (parent_v_angular_4_0)) >> 26));
  int32_t _tmp389 = (int32_t) ((((int64_t) (limi_translation_4_0) * (int64_t) (parent_v_angular_4_2)) >> 28));
  int32_t actInv1_4_1 = ((_tmp388 >> 1) - _tmp389);
  int32_t _tmp390 = (int32_t) ((((int64_t) (limi_translation_4_0) * (int64_t) (parent_v_angular_4_1)) >> 27));
  int32_t _tmp391 = (int32_t) ((((int64_t) (limi_translation_4_1) * (int64_t) (parent_v_angular_4_0)) >> 29));
  int32_t actInv1_4_2 = (((_tmp390 >> 2) - _tmp391) >> 1);
  int32_t actInv2_4_0 = ((parent_v_linear_4_0 >> 1) - actInv1_4_0);
  int32_t actInv2_4_1 = (parent_v_linear_4_1 - (actInv1_4_1 >> 2));
  int32_t actInv2_4_2 = ((parent_v_linear_4_2 >> 2) - actInv1_4_2);
  int32_t actInv3_4_0_0 = limi_rotation_4_0_0;
  int32_t actInv3_4_1_0 = limi_rotation_4_0_1;
  int32_t actInv3_4_2_0 = limi_rotation_4_0_2;
  int32_t actInv3_4_0_1 = limi_rotation_4_1_0;
  int32_t actInv3_4_1_1 = limi_rotation_4_1_1;
  int32_t actInv3_4_2_1 = limi_rotation_4_1_2;
  int32_t actInv3_4_0_2 = limi_rotation_4_2_0;
  int32_t actInv3_4_1_2 = limi_rotation_4_2_1;
  int32_t actInv3_4_2_2 = limi_rotation_4_2_2;
  int32_t _tmp392 = (int32_t) ((((int64_t) (actInv3_4_0_0) * (int64_t) (actInv2_4_0)) >> 30));
  int32_t _tmp393 = (int32_t) ((((int64_t) (actInv3_4_0_1) * (int64_t) (actInv2_4_1)) >> 28));
  int32_t _tmp394 = (_tmp392 + (_tmp393 >> 4));
  int32_t _tmp395 = (int32_t) ((((int64_t) (actInv3_4_0_2) * (int64_t) (actInv2_4_2)) >> 30));
  int32_t actInv4_4_0 = ((_tmp394 + _tmp395) >> 1);
  int32_t _tmp396 = (int32_t) ((((int64_t) (actInv3_4_1_0) * (int64_t) (actInv2_4_0)) >> 30));
  int32_t _tmp397 = (int32_t) ((((int64_t) (actInv3_4_1_1) * (int64_t) (actInv2_4_1)) >> 28));
  int32_t _tmp398 = (_tmp396 + (_tmp397 >> 4));
  int32_t _tmp399 = (int32_t) ((((int64_t) (actInv3_4_1_2) * (int64_t) (actInv2_4_2)) >> 30));
  int32_t actInv4_4_1 = ((_tmp398 + _tmp399) >> 1);
  int32_t _tmp400 = (int32_t) ((((int64_t) (actInv3_4_2_0) * (int64_t) (actInv2_4_0)) >> 27));
  int32_t _tmp401 = (int32_t) ((((int64_t) (actInv3_4_2_1) * (int64_t) (actInv2_4_1)) >> 30));
  int32_t _tmp402 = ((_tmp400 >> 3) + _tmp401);
  int32_t _tmp403 = (int32_t) ((((int64_t) (actInv3_4_2_2) * (int64_t) (actInv2_4_2)) >> 27));
  int32_t actInv4_4_2 = (_tmp402 + (_tmp403 >> 3));
  int32_t act_inv_linear_4_0 = ((v_linear_4_0 >> 5) + actInv4_4_0);
  int32_t act_inv_linear_4_1 = ((v_linear_4_1 >> 5) + actInv4_4_1);
  int32_t act_inv_linear_4_2 = ((v_linear_4_2 >> 3) + actInv4_4_2);
  int32_t _tmp404 = (int32_t) ((((int64_t) (actInv3_4_0_0) * (int64_t) (parent_v_angular_4_0)) >> 30));
  int32_t _tmp405 = (int32_t) ((((int64_t) (actInv3_4_0_1) * (int64_t) (parent_v_angular_4_1)) >> 26));
  int32_t _tmp406 = (_tmp404 + (_tmp405 >> 5));
  int32_t _tmp407 = (int32_t) ((((int64_t) (actInv3_4_0_2) * (int64_t) (parent_v_angular_4_2)) >> 30));
  int32_t actInv5_4_0 = ((_tmp406 + (_tmp407 >> 1)) >> 1);
  int32_t _tmp408 = (int32_t) ((((int64_t) (actInv3_4_1_0) * (int64_t) (parent_v_angular_4_0)) >> 30));
  int32_t _tmp409 = (int32_t) ((((int64_t) (actInv3_4_1_1) * (int64_t) (parent_v_angular_4_1)) >> 26));
  int32_t _tmp410 = (_tmp408 + (_tmp409 >> 5));
  int32_t _tmp411 = (int32_t) ((((int64_t) (actInv3_4_1_2) * (int64_t) (parent_v_angular_4_2)) >> 30));
  int32_t actInv5_4_1 = ((_tmp410 + (_tmp411 >> 1)) >> 1);
  int32_t _tmp412 = (int32_t) ((((int64_t) (actInv3_4_2_0) * (int64_t) (parent_v_angular_4_0)) >> 26));
  int32_t _tmp413 = (int32_t) ((((int64_t) (actInv3_4_2_1) * (int64_t) (parent_v_angular_4_1)) >> 30));
  int32_t _tmp414 = ((_tmp412 >> 5) + _tmp413);
  int32_t _tmp415 = (int32_t) ((((int64_t) (actInv3_4_2_2) * (int64_t) (parent_v_angular_4_2)) >> 27));
  int32_t actInv5_4_2 = (_tmp414 + (_tmp415 >> 5));
  int32_t act_inv_angular_4_0 = ((v_angular_4_0 >> 6) + actInv5_4_0);
  int32_t act_inv_angular_4_1 = ((v_angular_4_1 >> 6) + actInv5_4_1);
  int32_t act_inv_angular_4_2 = ((v_angular_4_2 >> 2) + actInv5_4_2);
  int32_t minus_m_w_4 = -(v_4);
  int32_t _tmp416 = -(minus_m_w_4);
  int32_t alpha_cross1_linear_4 = (int32_t) ((((int64_t) (_tmp416) * (int64_t) (act_inv_linear_4_1)) >> 31));
  int32_t alpha_cross2_linear_4 = (int32_t) ((((int64_t) (minus_m_w_4) * (int64_t) (act_inv_linear_4_0)) >> 31));
  int32_t alpha_cross_linear_4_0 = alpha_cross1_linear_4;
  int32_t alpha_cross_linear_4_1 = alpha_cross2_linear_4;
  int32_t alpha_cross_linear_4_2 = 0;
  int32_t _tmp417 = -(minus_m_w_4);
  int32_t alpha_cross1_angular_4 = (int32_t) ((((int64_t) (_tmp417) * (int64_t) (act_inv_angular_4_1)) >> 30));
  int32_t alpha_cross2_angular_4 = (int32_t) ((((int64_t) (minus_m_w_4) * (int64_t) (act_inv_angular_4_0)) >> 30));
  int32_t alpha_cross_angular_4_0 = alpha_cross1_angular_4;
  int32_t alpha_cross_angular_4_1 = alpha_cross2_angular_4;
  int32_t alpha_cross_angular_4_2 = 0;
  int32_t r_132_new_a_gf_4_0 = alpha_cross_linear_4_0;
  int32_t r_132_new_a_gf_4_1 = alpha_cross_linear_4_1;
  int32_t r_132_new_a_gf_4_2 = alpha_cross_linear_4_2;
  int32_t r_132_new_a_gf_4_3 = alpha_cross_angular_4_0;
  int32_t r_132_new_a_gf_4_4 = alpha_cross_angular_4_1;
  int32_t r_132_new_a_gf_4_5 = alpha_cross_angular_4_2;
  int32_t new_a_gf_up1_4 = (a_4 + (r_132_new_a_gf_4_5 >> 3));
  int32_t new_a_gf2_linear_4_0 = r_132_new_a_gf_4_0;
  int32_t new_a_gf2_linear_4_1 = r_132_new_a_gf_4_1;
  int32_t new_a_gf2_linear_4_2 = r_132_new_a_gf_4_2;
  int32_t new_a_gf2_angular_4_0 = r_132_new_a_gf_4_3;
  int32_t new_a_gf2_angular_4_1 = r_132_new_a_gf_4_4;
  int32_t new_a_gf2_angular_4_2 = new_a_gf_up1_4;
  int32_t _tmp418 = (int32_t) ((((int64_t) (limi_translation_4_0) * (int64_t) (parent_a_gf_angular_4_1)) >> 28));
  int32_t _tmp419 = (int32_t) ((((int64_t) (limi_translation_4_1) * (int64_t) (parent_a_gf_angular_4_0)) >> 30));
  int32_t r_133_actInv1_4_0 = ((_tmp418 >> 2) - _tmp419);
  int32_t _tmp420 = (int32_t) ((((int64_t) (limi_translation_4_2) * (int64_t) (parent_a_gf_angular_4_0)) >> 23));
  int32_t _tmp421 = (int32_t) ((((int64_t) (limi_translation_4_0) * (int64_t) (parent_a_gf_angular_4_2)) >> 27));
  int32_t r_133_actInv1_4_1 = ((_tmp420 >> 3) - _tmp421);
  int32_t _tmp422 = (int32_t) ((((int64_t) (limi_translation_4_0) * (int64_t) (parent_a_gf_angular_4_1)) >> 28));
  int32_t _tmp423 = (int32_t) ((((int64_t) (limi_translation_4_1) * (int64_t) (parent_a_gf_angular_4_0)) >> 30));
  int32_t r_133_actInv1_4_2 = ((_tmp422 >> 2) - _tmp423);
  int32_t r_134_actInv2_4_0 = ((parent_a_gf_linear_4_0 - r_133_actInv1_4_0) >> 1);
  int32_t r_134_actInv2_4_1 = (parent_a_gf_linear_4_1 - (r_133_actInv1_4_1 >> 4));
  int32_t r_134_actInv2_4_2 = (((parent_a_gf_linear_4_2 >> 1) - r_133_actInv1_4_2) >> 1);
  int32_t r_135_actInv3_4_0_0 = limi_rotation_4_0_0;
  int32_t r_135_actInv3_4_1_0 = limi_rotation_4_0_1;
  int32_t r_135_actInv3_4_2_0 = limi_rotation_4_0_2;
  int32_t r_135_actInv3_4_0_1 = limi_rotation_4_1_0;
  int32_t r_135_actInv3_4_1_1 = limi_rotation_4_1_1;
  int32_t r_135_actInv3_4_2_1 = limi_rotation_4_1_2;
  int32_t r_135_actInv3_4_0_2 = limi_rotation_4_2_0;
  int32_t r_135_actInv3_4_1_2 = limi_rotation_4_2_1;
  int32_t r_135_actInv3_4_2_2 = limi_rotation_4_2_2;
  int32_t _tmp424 = (int32_t) ((((int64_t) (r_135_actInv3_4_0_0) * (int64_t) (r_134_actInv2_4_0)) >> 30));
  int32_t _tmp425 = (int32_t) ((((int64_t) (r_135_actInv3_4_0_1) * (int64_t) (r_134_actInv2_4_1)) >> 24));
  int32_t _tmp426 = (_tmp424 + (_tmp425 >> 8));
  int32_t _tmp427 = (int32_t) ((((int64_t) (r_135_actInv3_4_0_2) * (int64_t) (r_134_actInv2_4_2)) >> 30));
  int32_t r_136_actInv4_4_0 = ((_tmp426 + _tmp427) >> 1);
  int32_t _tmp428 = (int32_t) ((((int64_t) (r_135_actInv3_4_1_0) * (int64_t) (r_134_actInv2_4_0)) >> 30));
  int32_t _tmp429 = (int32_t) ((((int64_t) (r_135_actInv3_4_1_1) * (int64_t) (r_134_actInv2_4_1)) >> 24));
  int32_t _tmp430 = (_tmp428 + (_tmp429 >> 8));
  int32_t _tmp431 = (int32_t) ((((int64_t) (r_135_actInv3_4_1_2) * (int64_t) (r_134_actInv2_4_2)) >> 30));
  int32_t r_136_actInv4_4_1 = ((_tmp430 + _tmp431) >> 1);
  int32_t _tmp432 = (int32_t) ((((int64_t) (r_135_actInv3_4_2_0) * (int64_t) (r_134_actInv2_4_0)) >> 23));
  int32_t _tmp433 = (int32_t) ((((int64_t) (r_135_actInv3_4_2_1) * (int64_t) (r_134_actInv2_4_1)) >> 30));
  int32_t _tmp434 = ((_tmp432 >> 7) + _tmp433);
  int32_t _tmp435 = (int32_t) ((((int64_t) (r_135_actInv3_4_2_2) * (int64_t) (r_134_actInv2_4_2)) >> 23));
  int32_t r_136_actInv4_4_2 = (_tmp434 + (_tmp435 >> 7));
  int32_t r_137_act_inv_linear_4_0 = ((new_a_gf2_linear_4_0 >> 1) + r_136_actInv4_4_0);
  int32_t r_137_act_inv_linear_4_1 = ((new_a_gf2_linear_4_1 >> 1) + r_136_actInv4_4_1);
  int32_t r_137_act_inv_linear_4_2 = ((new_a_gf2_linear_4_2 >> 7) + r_136_actInv4_4_2);
  int32_t _tmp436 = (int32_t) ((((int64_t) (r_135_actInv3_4_0_0) * (int64_t) (parent_a_gf_angular_4_0)) >> 30));
  int32_t _tmp437 = (int32_t) ((((int64_t) (r_135_actInv3_4_0_1) * (int64_t) (parent_a_gf_angular_4_1)) >> 23));
  int32_t _tmp438 = (_tmp436 + (_tmp437 >> 8));
  int32_t _tmp439 = (int32_t) ((((int64_t) (r_135_actInv3_4_0_2) * (int64_t) (parent_a_gf_angular_4_2)) >> 30));
  int32_t r_138_actInv5_4_0 = ((_tmp438 + (_tmp439 >> 1)) >> 1);
  int32_t _tmp440 = (int32_t) ((((int64_t) (r_135_actInv3_4_1_0) * (int64_t) (parent_a_gf_angular_4_0)) >> 30));
  int32_t _tmp441 = (int32_t) ((((int64_t) (r_135_actInv3_4_1_1) * (int64_t) (parent_a_gf_angular_4_1)) >> 23));
  int32_t _tmp442 = (_tmp440 + (_tmp441 >> 8));
  int32_t _tmp443 = (int32_t) ((((int64_t) (r_135_actInv3_4_1_2) * (int64_t) (parent_a_gf_angular_4_2)) >> 30));
  int32_t r_138_actInv5_4_1 = ((_tmp442 + (_tmp443 >> 1)) >> 1);
  int32_t _tmp444 = (int32_t) ((((int64_t) (r_135_actInv3_4_2_0) * (int64_t) (parent_a_gf_angular_4_0)) >> 23));
  int32_t _tmp445 = (int32_t) ((((int64_t) (r_135_actInv3_4_2_1) * (int64_t) (parent_a_gf_angular_4_1)) >> 30));
  int32_t _tmp446 = ((_tmp444 >> 8) + _tmp445);
  int32_t _tmp447 = (int32_t) ((((int64_t) (r_135_actInv3_4_2_2) * (int64_t) (parent_a_gf_angular_4_2)) >> 24));
  int32_t r_138_actInv5_4_2 = (_tmp446 + (_tmp447 >> 8));
  int32_t r_139_act_inv_angular_4_0 = ((new_a_gf2_angular_4_0 >> 1) + r_138_actInv5_4_0);
  int32_t r_139_act_inv_angular_4_1 = ((new_a_gf2_angular_4_1 >> 1) + r_138_actInv5_4_1);
  int32_t r_139_act_inv_angular_4_2 = ((new_a_gf2_angular_4_2 >> 5) + r_138_actInv5_4_2);
  int32_t new_a_gf_up3_4_0 = r_137_act_inv_linear_4_0;
  int32_t new_a_gf_up3_4_1 = r_137_act_inv_linear_4_1;
  int32_t new_a_gf_up3_4_2 = r_137_act_inv_linear_4_2;
  int32_t new_a_gf_up3_4_3 = r_139_act_inv_angular_4_0;
  int32_t new_a_gf_up3_4_4 = r_139_act_inv_angular_4_1;
  int32_t new_a_gf_up3_4_5 = r_139_act_inv_angular_4_2;
  int32_t _tmp448 = (int32_t) ((((int64_t) (lever_4_0) * (int64_t) (act_inv_angular_4_1)) >> 25));
  int32_t _tmp449 = (int32_t) ((((int64_t) (lever_4_1) * (int64_t) (act_inv_angular_4_0)) >> 26));
  int32_t h_linear_1_4_0 = ((_tmp448 >> 1) - _tmp449);
  int32_t _tmp450 = (int32_t) ((((int64_t) (lever_4_2) * (int64_t) (act_inv_angular_4_0)) >> 26));
  int32_t _tmp451 = (int32_t) ((((int64_t) (lever_4_0) * (int64_t) (act_inv_angular_4_2)) >> 26));
  int32_t h_linear_1_4_1 = (_tmp450 - (_tmp451 >> 1));
  int32_t _tmp452 = (int32_t) ((((int64_t) (lever_4_0) * (int64_t) (act_inv_angular_4_1)) >> 25));
  int32_t _tmp453 = (int32_t) ((((int64_t) (lever_4_1) * (int64_t) (act_inv_angular_4_0)) >> 26));
  int32_t h_linear_1_4_2 = ((_tmp452 >> 1) - _tmp453);
  int32_t h_linear_2_4_0 = (act_inv_linear_4_0 - (h_linear_1_4_0 >> 4));
  int32_t h_linear_2_4_1 = (act_inv_linear_4_1 - (h_linear_1_4_1 >> 4));
  int32_t h_linear_2_4_2 = (act_inv_linear_4_2 - (h_linear_1_4_2 >> 2));
  int32_t h_linear_4_0 = (int32_t) ((((int64_t) (masses_4) * (int64_t) (h_linear_2_4_0)) >> 31));
  int32_t h_linear_4_1 = (int32_t) ((((int64_t) (masses_4) * (int64_t) (h_linear_2_4_1)) >> 31));
  int32_t h_linear_4_2 = (int32_t) ((((int64_t) (masses_4) * (int64_t) (h_linear_2_4_2)) >> 31));
  int32_t r_140_ = (int32_t) ((((int64_t) (inertia_4_0_0) * (int64_t) (act_inv_angular_4_0)) >> 26));
  int32_t r_141_ = (int32_t) ((((int64_t) (inertia_4_0_1) * (int64_t) (act_inv_angular_4_1)) >> 25));
  int32_t r_142_ = (int32_t) ((((int64_t) (inertia_4_0_2) * (int64_t) (act_inv_angular_4_2)) >> 26));
  int32_t r_143_ = (int32_t) ((((int64_t) (inertia_4_0_1) * (int64_t) (act_inv_angular_4_0)) >> 25));
  int32_t r_144_ = (int32_t) ((((int64_t) (inertia_4_1_1) * (int64_t) (act_inv_angular_4_1)) >> 26));
  int32_t r_145_ = (int32_t) ((((int64_t) (inertia_4_1_2) * (int64_t) (act_inv_angular_4_2)) >> 26));
  int32_t r_146_ = (int32_t) ((((int64_t) (inertia_4_0_2) * (int64_t) (act_inv_angular_4_0)) >> 25));
  int32_t r_147_ = (int32_t) ((((int64_t) (inertia_4_1_2) * (int64_t) (act_inv_angular_4_1)) >> 25));
  int32_t r_148_ = (int32_t) ((((int64_t) (inertia_4_2_2) * (int64_t) (act_inv_angular_4_2)) >> 26));
  int32_t rhsMult1_temp_4 = (r_140_ + (r_141_ >> 1));
  int32_t rhsMult1_4 = (rhsMult1_temp_4 + (r_142_ >> 1));
  int32_t rhsMult2_temp_4 = ((r_143_ >> 1) + r_144_);
  int32_t rhsMult2_4 = (rhsMult2_temp_4 + (r_145_ >> 1));
  int32_t rhsMult3_temp_4 = (r_146_ + r_147_);
  int32_t rhsMult3_4 = (rhsMult3_temp_4 + r_148_);
  int32_t rhsMult_4_0 = rhsMult1_4;
  int32_t rhsMult_4_1 = rhsMult2_4;
  int32_t rhsMult_4_2 = rhsMult3_4;
  int32_t h_angular_first_4_0 = rhsMult_4_0;
  int32_t h_angular_first_4_1 = rhsMult_4_1;
  int32_t h_angular_first_4_2 = rhsMult_4_2;
  int32_t _tmp454 = (int32_t) ((((int64_t) (lever_4_0) * (int64_t) (h_linear_4_1)) >> 25));
  int32_t _tmp455 = (int32_t) ((((int64_t) (lever_4_1) * (int64_t) (h_linear_4_0)) >> 26));
  int32_t h_angular_1_4_0 = ((_tmp454 >> 1) - _tmp455);
  int32_t _tmp456 = (int32_t) ((((int64_t) (lever_4_2) * (int64_t) (h_linear_4_0)) >> 26));
  int32_t _tmp457 = (int32_t) ((((int64_t) (lever_4_0) * (int64_t) (h_linear_4_2)) >> 27));
  int32_t h_angular_1_4_1 = (_tmp456 - (_tmp457 >> 1));
  int32_t _tmp458 = (int32_t) ((((int64_t) (lever_4_0) * (int64_t) (h_linear_4_1)) >> 25));
  int32_t _tmp459 = (int32_t) ((((int64_t) (lever_4_1) * (int64_t) (h_linear_4_0)) >> 26));
  int32_t h_angular_1_4_2 = ((_tmp458 >> 1) - _tmp459);
  int32_t h_angular_4_0 = ((h_angular_first_4_0 + h_angular_1_4_0) >> 1);
  int32_t h_angular_4_1 = ((h_angular_first_4_1 + h_angular_1_4_1) >> 1);
  int32_t h_angular_4_2 = (((h_angular_first_4_2 >> 1) + h_angular_1_4_2) >> 1);
  int32_t _tmp460 = (int32_t) ((((int64_t) (lever_4_0) * (int64_t) (r_139_act_inv_angular_4_1)) >> 25));
  int32_t _tmp461 = (int32_t) ((((int64_t) (lever_4_1) * (int64_t) (r_139_act_inv_angular_4_0)) >> 27));
  int32_t f_linear_1_4_0 = ((_tmp460 >> 2) - _tmp461);
  int32_t _tmp462 = (int32_t) ((((int64_t) (lever_4_2) * (int64_t) (r_139_act_inv_angular_4_0)) >> 27));
  int32_t _tmp463 = (int32_t) ((((int64_t) (lever_4_0) * (int64_t) (r_139_act_inv_angular_4_2)) >> 25));
  int32_t f_linear_1_4_1 = (_tmp462 - (_tmp463 >> 3));
  int32_t _tmp464 = (int32_t) ((((int64_t) (lever_4_0) * (int64_t) (r_139_act_inv_angular_4_1)) >> 25));
  int32_t _tmp465 = (int32_t) ((((int64_t) (lever_4_1) * (int64_t) (r_139_act_inv_angular_4_0)) >> 27));
  int32_t f_linear_1_4_2 = ((_tmp464 >> 2) - _tmp465);
  int32_t f_linear_2_4_0 = (r_137_act_inv_linear_4_0 - (f_linear_1_4_0 >> 4));
  int32_t f_linear_2_4_1 = (r_137_act_inv_linear_4_1 - (f_linear_1_4_1 >> 4));
  int32_t f_linear_2_4_2 = (r_137_act_inv_linear_4_2 - (f_linear_1_4_2 >> 2));
  int32_t f_linear_3_4_0 = (int32_t) ((((int64_t) (masses_4) * (int64_t) (f_linear_2_4_0)) >> 31));
  int32_t f_linear_3_4_1 = (int32_t) ((((int64_t) (masses_4) * (int64_t) (f_linear_2_4_1)) >> 31));
  int32_t f_linear_3_4_2 = (int32_t) ((((int64_t) (masses_4) * (int64_t) (f_linear_2_4_2)) >> 31));
  int32_t r_149_ = (int32_t) ((((int64_t) (inertia_4_0_0) * (int64_t) (r_139_act_inv_angular_4_0)) >> 27));
  int32_t r_150_ = (int32_t) ((((int64_t) (inertia_4_0_1) * (int64_t) (r_139_act_inv_angular_4_1)) >> 23));
  int32_t r_151_ = (int32_t) ((((int64_t) (inertia_4_0_2) * (int64_t) (r_139_act_inv_angular_4_2)) >> 23));
  int32_t r_152_ = (int32_t) ((((int64_t) (inertia_4_0_1) * (int64_t) (r_139_act_inv_angular_4_0)) >> 23));
  int32_t r_153_ = (int32_t) ((((int64_t) (inertia_4_1_1) * (int64_t) (r_139_act_inv_angular_4_1)) >> 26));
  int32_t r_154_ = (int32_t) ((((int64_t) (inertia_4_1_2) * (int64_t) (r_139_act_inv_angular_4_2)) >> 23));
  int32_t r_155_ = (int32_t) ((((int64_t) (inertia_4_0_2) * (int64_t) (r_139_act_inv_angular_4_0)) >> 23));
  int32_t r_156_ = (int32_t) ((((int64_t) (inertia_4_1_2) * (int64_t) (r_139_act_inv_angular_4_1)) >> 22));
  int32_t r_157_ = (int32_t) ((((int64_t) (inertia_4_2_2) * (int64_t) (r_139_act_inv_angular_4_2)) >> 24));
  int32_t r_158_rhsMult1_temp_4 = (r_149_ + (r_150_ >> 4));
  int32_t r_159_rhsMult1_4 = (r_158_rhsMult1_temp_4 + (r_151_ >> 5));
  int32_t r_160_rhsMult2_temp_4 = ((r_152_ >> 3) + r_153_);
  int32_t r_161_rhsMult2_4 = (r_160_rhsMult2_temp_4 + (r_154_ >> 4));
  int32_t r_162_rhsMult3_temp_4 = ((r_155_ + (r_156_ >> 1)) >> 1);
  int32_t r_163_rhsMult3_4 = ((r_162_rhsMult3_temp_4 + (r_157_ >> 1)) >> 1);
  int32_t r_164_rhsMult_4_0 = r_159_rhsMult1_4;
  int32_t r_164_rhsMult_4_1 = r_161_rhsMult2_4;
  int32_t r_164_rhsMult_4_2 = r_163_rhsMult3_4;
  int32_t f_angular_first_4_0 = r_164_rhsMult_4_0;
  int32_t f_angular_first_4_1 = r_164_rhsMult_4_1;
  int32_t f_angular_first_4_2 = r_164_rhsMult_4_2;
  int32_t _tmp466 = (int32_t) ((((int64_t) (lever_4_0) * (int64_t) (f_linear_3_4_1)) >> 24));
  int32_t _tmp467 = (int32_t) ((((int64_t) (lever_4_1) * (int64_t) (f_linear_3_4_0)) >> 26));
  int32_t f_angular_1_4_0 = (((_tmp466 >> 2) - _tmp467) >> 1);
  int32_t _tmp468 = (int32_t) ((((int64_t) (lever_4_2) * (int64_t) (f_linear_3_4_0)) >> 26));
  int32_t _tmp469 = (int32_t) ((((int64_t) (lever_4_0) * (int64_t) (f_linear_3_4_2)) >> 24));
  int32_t f_angular_1_4_1 = (_tmp468 - (_tmp469 >> 4));
  int32_t _tmp470 = (int32_t) ((((int64_t) (lever_4_0) * (int64_t) (f_linear_3_4_1)) >> 24));
  int32_t _tmp471 = (int32_t) ((((int64_t) (lever_4_1) * (int64_t) (f_linear_3_4_0)) >> 26));
  int32_t f_angular_1_4_2 = (((_tmp470 >> 2) - _tmp471) >> 1);
  int32_t f_angular_2_4_0 = ((f_angular_first_4_0 >> 1) + f_angular_1_4_0);
  int32_t f_angular_2_4_1 = (((f_angular_first_4_1 >> 1) + f_angular_1_4_1) >> 1);
  int32_t f_angular_2_4_2 = ((f_angular_first_4_2 >> 3) + f_angular_1_4_2);
  int32_t _tmp472 = (int32_t) ((((int64_t) (act_inv_angular_4_0) * (int64_t) (h_linear_4_1)) >> 30));
  int32_t _tmp473 = (int32_t) ((((int64_t) (act_inv_angular_4_1) * (int64_t) (h_linear_4_0)) >> 30));
  int32_t f_linear_4_temp_4_0 = ((_tmp472 - _tmp473) >> 1);
  int32_t _tmp474 = (int32_t) ((((int64_t) (act_inv_angular_4_2) * (int64_t) (h_linear_4_0)) >> 30));
  int32_t _tmp475 = (int32_t) ((((int64_t) (act_inv_angular_4_0) * (int64_t) (h_linear_4_2)) >> 30));
  int32_t f_linear_4_temp_4_1 = ((_tmp474 - (_tmp475 >> 1)) >> 1);
  int32_t _tmp476 = (int32_t) ((((int64_t) (act_inv_angular_4_0) * (int64_t) (h_linear_4_1)) >> 30));
  int32_t _tmp477 = (int32_t) ((((int64_t) (act_inv_angular_4_1) * (int64_t) (h_linear_4_0)) >> 30));
  int32_t f_linear_4_temp_4_2 = ((_tmp476 - _tmp477) >> 1);
  int32_t f_linear_4_4_0 = ((f_linear_3_4_0 >> 2) + f_linear_4_temp_4_0);
  int32_t f_linear_4_4_1 = ((f_linear_3_4_1 >> 1) + f_linear_4_temp_4_1);
  int32_t f_linear_4_4_2 = ((f_linear_3_4_2 >> 4) + f_linear_4_temp_4_2);
  int32_t _tmp478 = (int32_t) ((((int64_t) (act_inv_angular_4_0) * (int64_t) (h_angular_first_4_1)) >> 30));
  int32_t _tmp479 = (int32_t) ((((int64_t) (act_inv_angular_4_1) * (int64_t) (h_angular_first_4_0)) >> 30));
  int32_t f_angular_3_temp_4_0 = ((_tmp478 - _tmp479) >> 1);
  int32_t _tmp480 = (int32_t) ((((int64_t) (act_inv_angular_4_2) * (int64_t) (h_angular_first_4_0)) >> 31));
  int32_t _tmp481 = (int32_t) ((((int64_t) (act_inv_angular_4_0) * (int64_t) (h_angular_first_4_2)) >> 29));
  int32_t f_angular_3_temp_4_1 = (_tmp480 - (_tmp481 >> 2));
  int32_t _tmp482 = (int32_t) ((((int64_t) (act_inv_angular_4_0) * (int64_t) (h_angular_first_4_1)) >> 30));
  int32_t _tmp483 = (int32_t) ((((int64_t) (act_inv_angular_4_1) * (int64_t) (h_angular_first_4_0)) >> 30));
  int32_t f_angular_3_temp_4_2 = ((_tmp482 - _tmp483) >> 1);
  int32_t f_angular_3_4_0 = (((f_angular_2_4_0 >> 1) + f_angular_3_temp_4_0) >> 1);
  int32_t f_angular_3_4_1 = ((f_angular_2_4_1 + f_angular_3_temp_4_1) >> 1);
  int32_t f_angular_3_4_2 = ((f_angular_2_4_2 >> 1) + f_angular_3_temp_4_2);
  int32_t _tmp484 = (int32_t) ((((int64_t) (act_inv_linear_4_0) * (int64_t) (h_linear_4_1)) >> 30));
  int32_t _tmp485 = (int32_t) ((((int64_t) (act_inv_linear_4_1) * (int64_t) (h_linear_4_0)) >> 30));
  int32_t f_angular_4_temp_4_0 = ((_tmp484 - _tmp485) >> 1);
  int32_t _tmp486 = (int32_t) ((((int64_t) (act_inv_linear_4_2) * (int64_t) (h_linear_4_0)) >> 30));
  int32_t _tmp487 = (int32_t) ((((int64_t) (act_inv_linear_4_0) * (int64_t) (h_linear_4_2)) >> 31));
  int32_t f_angular_4_temp_4_1 = ((_tmp486 >> 1) - _tmp487);
  int32_t _tmp488 = (int32_t) ((((int64_t) (act_inv_linear_4_0) * (int64_t) (h_linear_4_1)) >> 30));
  int32_t _tmp489 = (int32_t) ((((int64_t) (act_inv_linear_4_1) * (int64_t) (h_linear_4_0)) >> 30));
  int32_t f_angular_4_temp_4_2 = ((_tmp488 - _tmp489) >> 1);
  int32_t f_angular_4_4_0 = (((f_angular_3_4_0 >> 3) + f_angular_4_temp_4_0) >> 1);
  int32_t f_angular_4_4_1 = (((f_angular_3_4_1 >> 2) + f_angular_4_temp_4_1) >> 1);
  int32_t f_angular_4_4_2 = (((f_angular_3_4_2 >> 4) + f_angular_4_temp_4_2) >> 1);
  int32_t h_rnea_firstpass4_0 = h_linear_4_0;
  int32_t h_rnea_firstpass4_1 = h_linear_4_1;
  int32_t h_rnea_firstpass4_2 = h_linear_4_2;
  int32_t h_rnea_firstpass4_3 = h_angular_4_0;
  int32_t h_rnea_firstpass4_4 = h_angular_4_1;
  int32_t h_rnea_firstpass4_5 = h_angular_4_2;
  int32_t f_rnea_firstpass4_0 = f_linear_4_4_0;
  int32_t f_rnea_firstpass4_1 = f_linear_4_4_1;
  int32_t f_rnea_firstpass4_2 = f_linear_4_4_2;
  int32_t f_rnea_firstpass4_3 = f_angular_4_4_0;
  int32_t f_rnea_firstpass4_4 = f_angular_4_4_1;
  int32_t f_rnea_firstpass4_5 = f_angular_4_4_2;
  int32_t r_165_new_v_4_0 = act_inv_linear_4_0;
  int32_t r_165_new_v_4_1 = act_inv_linear_4_1;
  int32_t r_165_new_v_4_2 = act_inv_linear_4_2;
  int32_t r_165_new_v_4_3 = act_inv_angular_4_0;
  int32_t r_165_new_v_4_4 = act_inv_angular_4_1;
  int32_t r_165_new_v_4_5 = act_inv_angular_4_2;
  int32_t rotation_matrix_5_0_0 = qcos_5;
  int32_t rotation_matrix_5_0_1 = -(qsin_5);
  int32_t rotation_matrix_5_0_2 = 0;
  int32_t rotation_matrix_5_1_0 = qsin_5;
  int32_t rotation_matrix_5_1_1 = qcos_5;
  int32_t rotation_matrix_5_1_2 = 0;
  int32_t rotation_matrix_5_2_0 = 0;
  int32_t rotation_matrix_5_2_1 = 0;
  int32_t rotation_matrix_5_2_2 = 1073741824;
  int32_t limi_rotation_5_0_0 = rotation_matrix_5_0_0;
  int32_t limi_rotation_5_0_1 = rotation_matrix_5_0_1;
  int32_t limi_rotation_5_0_2 = 0;
  int32_t limi_rotation_5_1_0 = 0;
  int32_t limi_rotation_5_1_1 = 0;
  int32_t limi_rotation_5_1_2 = -1073741825;
  int32_t limi_rotation_5_2_0 = rotation_matrix_5_1_0;
  int32_t limi_rotation_5_2_1 = rotation_matrix_5_1_1;
  int32_t limi_rotation_5_2_2 = 0;
  int32_t v_linear_5_0 = data_v_5_0;
  int32_t v_linear_5_1 = data_v_5_1;
  int32_t v_linear_5_2 = data_v_5_2;
  int32_t v_angular_5_0 = data_v_5_3;
  int32_t v_angular_5_1 = data_v_5_4;
  int32_t v_angular_5_2 = v_5;
  int32_t parent_v_linear_5_0 = r_165_new_v_4_0;
  int32_t parent_v_linear_5_1 = r_165_new_v_4_1;
  int32_t parent_v_linear_5_2 = r_165_new_v_4_2;
  int32_t parent_v_angular_5_0 = r_165_new_v_4_3;
  int32_t parent_v_angular_5_1 = r_165_new_v_4_4;
  int32_t parent_v_angular_5_2 = r_165_new_v_4_5;
  int32_t parent_a_gf_linear_5_0 = new_a_gf_up3_4_0;
  int32_t parent_a_gf_linear_5_1 = new_a_gf_up3_4_1;
  int32_t parent_a_gf_linear_5_2 = new_a_gf_up3_4_2;
  int32_t parent_a_gf_angular_5_0 = new_a_gf_up3_4_3;
  int32_t parent_a_gf_angular_5_1 = new_a_gf_up3_4_4;
  int32_t parent_a_gf_angular_5_2 = new_a_gf_up3_4_5;
  int32_t _tmp490 = (int32_t) ((((int64_t) (limi_translation_5_0) * (int64_t) (parent_v_angular_5_1)) >> 25));
  int32_t _tmp491 = (int32_t) ((((int64_t) (limi_translation_5_1) * (int64_t) (parent_v_angular_5_0)) >> 25));
  int32_t actInv1_5_0 = (_tmp490 - _tmp491);
  int32_t _tmp492 = (int32_t) ((((int64_t) (limi_translation_5_2) * (int64_t) (parent_v_angular_5_0)) >> 25));
  int32_t _tmp493 = (int32_t) ((((int64_t) (limi_translation_5_0) * (int64_t) (parent_v_angular_5_2)) >> 26));
  int32_t actInv1_5_1 = (_tmp492 - _tmp493);
  int32_t _tmp494 = (int32_t) ((((int64_t) (limi_translation_5_0) * (int64_t) (parent_v_angular_5_1)) >> 25));
  int32_t _tmp495 = (int32_t) ((((int64_t) (limi_translation_5_1) * (int64_t) (parent_v_angular_5_0)) >> 25));
  int32_t actInv1_5_2 = (_tmp494 - _tmp495);
  int32_t actInv2_5_0 = (parent_v_linear_5_0 - (actInv1_5_0 >> 5));
  int32_t actInv2_5_1 = (parent_v_linear_5_1 - (actInv1_5_1 >> 5));
  int32_t actInv2_5_2 = (parent_v_linear_5_2 - (actInv1_5_2 >> 3));
  int32_t actInv3_5_0_0 = limi_rotation_5_0_0;
  int32_t actInv3_5_1_0 = limi_rotation_5_0_1;
  int32_t actInv3_5_2_0 = limi_rotation_5_0_2;
  int32_t actInv3_5_0_1 = limi_rotation_5_1_0;
  int32_t actInv3_5_1_1 = limi_rotation_5_1_1;
  int32_t actInv3_5_2_1 = limi_rotation_5_1_2;
  int32_t actInv3_5_0_2 = limi_rotation_5_2_0;
  int32_t actInv3_5_1_2 = limi_rotation_5_2_1;
  int32_t actInv3_5_2_2 = limi_rotation_5_2_2;
  int32_t _tmp496 = (int32_t) ((((int64_t) (actInv3_5_0_0) * (int64_t) (actInv2_5_0)) >> 30));
  int32_t _tmp497 = (int32_t) ((((int64_t) (actInv3_5_0_1) * (int64_t) (actInv2_5_1)) >> 26));
  int32_t _tmp498 = (_tmp496 + (_tmp497 >> 5));
  int32_t _tmp499 = (int32_t) ((((int64_t) (actInv3_5_0_2) * (int64_t) (actInv2_5_2)) >> 30));
  int32_t actInv4_5_0 = ((_tmp498 + (_tmp499 >> 2)) >> 1);
  int32_t _tmp500 = (int32_t) ((((int64_t) (actInv3_5_1_0) * (int64_t) (actInv2_5_0)) >> 30));
  int32_t _tmp501 = (int32_t) ((((int64_t) (actInv3_5_1_1) * (int64_t) (actInv2_5_1)) >> 26));
  int32_t _tmp502 = (_tmp500 + (_tmp501 >> 5));
  int32_t _tmp503 = (int32_t) ((((int64_t) (actInv3_5_1_2) * (int64_t) (actInv2_5_2)) >> 30));
  int32_t actInv4_5_1 = ((_tmp502 + (_tmp503 >> 2)) >> 1);
  int32_t _tmp504 = (int32_t) ((((int64_t) (actInv3_5_2_0) * (int64_t) (actInv2_5_0)) >> 26));
  int32_t _tmp505 = (int32_t) ((((int64_t) (actInv3_5_2_1) * (int64_t) (actInv2_5_1)) >> 30));
  int32_t _tmp506 = ((_tmp504 >> 5) + _tmp505);
  int32_t _tmp507 = (int32_t) ((((int64_t) (actInv3_5_2_2) * (int64_t) (actInv2_5_2)) >> 28));
  int32_t actInv4_5_2 = (_tmp506 + (_tmp507 >> 5));
  int32_t act_inv_linear_5_0 = ((v_linear_5_0 >> 6) + actInv4_5_0);
  int32_t act_inv_linear_5_1 = ((v_linear_5_1 >> 6) + actInv4_5_1);
  int32_t act_inv_linear_5_2 = ((v_linear_5_2 >> 5) + actInv4_5_2);
  int32_t _tmp508 = (int32_t) ((((int64_t) (actInv3_5_0_0) * (int64_t) (parent_v_angular_5_0)) >> 30));
  int32_t _tmp509 = (int32_t) ((((int64_t) (actInv3_5_0_1) * (int64_t) (parent_v_angular_5_1)) >> 25));
  int32_t _tmp510 = (_tmp508 + (_tmp509 >> 6));
  int32_t _tmp511 = (int32_t) ((((int64_t) (actInv3_5_0_2) * (int64_t) (parent_v_angular_5_2)) >> 30));
  int32_t actInv5_5_0 = (_tmp510 + (_tmp511 >> 1));
  int32_t _tmp512 = (int32_t) ((((int64_t) (actInv3_5_1_0) * (int64_t) (parent_v_angular_5_0)) >> 30));
  int32_t _tmp513 = (int32_t) ((((int64_t) (actInv3_5_1_1) * (int64_t) (parent_v_angular_5_1)) >> 25));
  int32_t _tmp514 = (_tmp512 + (_tmp513 >> 6));
  int32_t _tmp515 = (int32_t) ((((int64_t) (actInv3_5_1_2) * (int64_t) (parent_v_angular_5_2)) >> 30));
  int32_t actInv5_5_1 = (_tmp514 + (_tmp515 >> 1));
  int32_t _tmp516 = (int32_t) ((((int64_t) (actInv3_5_2_0) * (int64_t) (parent_v_angular_5_0)) >> 25));
  int32_t _tmp517 = (int32_t) ((((int64_t) (actInv3_5_2_1) * (int64_t) (parent_v_angular_5_1)) >> 30));
  int32_t _tmp518 = ((_tmp516 >> 6) + _tmp517);
  int32_t _tmp519 = (int32_t) ((((int64_t) (actInv3_5_2_2) * (int64_t) (parent_v_angular_5_2)) >> 26));
  int32_t actInv5_5_2 = (_tmp518 + (_tmp519 >> 6));
  int32_t act_inv_angular_5_0 = ((v_angular_5_0 >> 6) + actInv5_5_0);
  int32_t act_inv_angular_5_1 = ((v_angular_5_1 >> 6) + actInv5_5_1);
  int32_t act_inv_angular_5_2 = ((v_angular_5_2 >> 3) + actInv5_5_2);
  int32_t minus_m_w_5 = -(v_5);
  int32_t _tmp520 = -(minus_m_w_5);
  int32_t alpha_cross1_linear_5 = (int32_t) ((((int64_t) (_tmp520) * (int64_t) (act_inv_linear_5_1)) >> 30));
  int32_t alpha_cross2_linear_5 = (int32_t) ((((int64_t) (minus_m_w_5) * (int64_t) (act_inv_linear_5_0)) >> 30));
  int32_t alpha_cross_linear_5_0 = alpha_cross1_linear_5;
  int32_t alpha_cross_linear_5_1 = alpha_cross2_linear_5;
  int32_t alpha_cross_linear_5_2 = 0;
  int32_t _tmp521 = -(minus_m_w_5);
  int32_t alpha_cross1_angular_5 = (int32_t) ((((int64_t) (_tmp521) * (int64_t) (act_inv_angular_5_1)) >> 31));
  int32_t alpha_cross2_angular_5 = (int32_t) ((((int64_t) (minus_m_w_5) * (int64_t) (act_inv_angular_5_0)) >> 31));
  int32_t alpha_cross_angular_5_0 = alpha_cross1_angular_5;
  int32_t alpha_cross_angular_5_1 = alpha_cross2_angular_5;
  int32_t alpha_cross_angular_5_2 = 0;
  int32_t r_166_new_a_gf_5_0 = alpha_cross_linear_5_0;
  int32_t r_166_new_a_gf_5_1 = alpha_cross_linear_5_1;
  int32_t r_166_new_a_gf_5_2 = alpha_cross_linear_5_2;
  int32_t r_166_new_a_gf_5_3 = alpha_cross_angular_5_0;
  int32_t r_166_new_a_gf_5_4 = alpha_cross_angular_5_1;
  int32_t r_166_new_a_gf_5_5 = alpha_cross_angular_5_2;
  int32_t new_a_gf_up1_5 = (a_5 + (r_166_new_a_gf_5_5 >> 3));
  int32_t new_a_gf2_linear_5_0 = r_166_new_a_gf_5_0;
  int32_t new_a_gf2_linear_5_1 = r_166_new_a_gf_5_1;
  int32_t new_a_gf2_linear_5_2 = r_166_new_a_gf_5_2;
  int32_t new_a_gf2_angular_5_0 = r_166_new_a_gf_5_3;
  int32_t new_a_gf2_angular_5_1 = r_166_new_a_gf_5_4;
  int32_t new_a_gf2_angular_5_2 = new_a_gf_up1_5;
  int32_t _tmp522 = (int32_t) ((((int64_t) (limi_translation_5_0) * (int64_t) (parent_a_gf_angular_5_1)) >> 22));
  int32_t _tmp523 = (int32_t) ((((int64_t) (limi_translation_5_1) * (int64_t) (parent_a_gf_angular_5_0)) >> 22));
  int32_t r_167_actInv1_5_0 = (_tmp522 - _tmp523);
  int32_t _tmp524 = (int32_t) ((((int64_t) (limi_translation_5_2) * (int64_t) (parent_a_gf_angular_5_0)) >> 22));
  int32_t _tmp525 = (int32_t) ((((int64_t) (limi_translation_5_0) * (int64_t) (parent_a_gf_angular_5_2)) >> 23));
  int32_t r_167_actInv1_5_1 = (_tmp524 - _tmp525);
  int32_t _tmp526 = (int32_t) ((((int64_t) (limi_translation_5_0) * (int64_t) (parent_a_gf_angular_5_1)) >> 22));
  int32_t _tmp527 = (int32_t) ((((int64_t) (limi_translation_5_1) * (int64_t) (parent_a_gf_angular_5_0)) >> 22));
  int32_t r_167_actInv1_5_2 = (_tmp526 - _tmp527);
  int32_t r_168_actInv2_5_0 = (parent_a_gf_linear_5_0 - (r_167_actInv1_5_0 >> 9));
  int32_t r_168_actInv2_5_1 = (parent_a_gf_linear_5_1 - (r_167_actInv1_5_1 >> 9));
  int32_t r_168_actInv2_5_2 = (parent_a_gf_linear_5_2 - (r_167_actInv1_5_2 >> 7));
  int32_t r_169_actInv3_5_0_0 = limi_rotation_5_0_0;
  int32_t r_169_actInv3_5_1_0 = limi_rotation_5_0_1;
  int32_t r_169_actInv3_5_2_0 = limi_rotation_5_0_2;
  int32_t r_169_actInv3_5_0_1 = limi_rotation_5_1_0;
  int32_t r_169_actInv3_5_1_1 = limi_rotation_5_1_1;
  int32_t r_169_actInv3_5_2_1 = limi_rotation_5_1_2;
  int32_t r_169_actInv3_5_0_2 = limi_rotation_5_2_0;
  int32_t r_169_actInv3_5_1_2 = limi_rotation_5_2_1;
  int32_t r_169_actInv3_5_2_2 = limi_rotation_5_2_2;
  int32_t _tmp528 = (int32_t) ((((int64_t) (r_169_actInv3_5_0_0) * (int64_t) (r_168_actInv2_5_0)) >> 30));
  int32_t _tmp529 = (int32_t) ((((int64_t) (r_169_actInv3_5_0_1) * (int64_t) (r_168_actInv2_5_1)) >> 22));
  int32_t _tmp530 = (_tmp528 + (_tmp529 >> 9));
  int32_t _tmp531 = (int32_t) ((((int64_t) (r_169_actInv3_5_0_2) * (int64_t) (r_168_actInv2_5_2)) >> 30));
  int32_t r_170_actInv4_5_0 = ((_tmp530 + (_tmp531 >> 2)) >> 1);
  int32_t _tmp532 = (int32_t) ((((int64_t) (r_169_actInv3_5_1_0) * (int64_t) (r_168_actInv2_5_0)) >> 30));
  int32_t _tmp533 = (int32_t) ((((int64_t) (r_169_actInv3_5_1_1) * (int64_t) (r_168_actInv2_5_1)) >> 22));
  int32_t _tmp534 = (_tmp532 + (_tmp533 >> 9));
  int32_t _tmp535 = (int32_t) ((((int64_t) (r_169_actInv3_5_1_2) * (int64_t) (r_168_actInv2_5_2)) >> 30));
  int32_t r_170_actInv4_5_1 = ((_tmp534 + (_tmp535 >> 2)) >> 1);
  int32_t _tmp536 = (int32_t) ((((int64_t) (r_169_actInv3_5_2_0) * (int64_t) (r_168_actInv2_5_0)) >> 22));
  int32_t _tmp537 = (int32_t) ((((int64_t) (r_169_actInv3_5_2_1) * (int64_t) (r_168_actInv2_5_1)) >> 30));
  int32_t _tmp538 = ((_tmp536 >> 9) + _tmp537);
  int32_t _tmp539 = (int32_t) ((((int64_t) (r_169_actInv3_5_2_2) * (int64_t) (r_168_actInv2_5_2)) >> 24));
  int32_t r_170_actInv4_5_2 = (_tmp538 + (_tmp539 >> 9));
  int32_t r_171_act_inv_linear_5_0 = ((new_a_gf2_linear_5_0 >> 2) + r_170_actInv4_5_0);
  int32_t r_171_act_inv_linear_5_1 = ((new_a_gf2_linear_5_1 >> 2) + r_170_actInv4_5_1);
  int32_t r_171_act_inv_linear_5_2 = ((new_a_gf2_linear_5_2 >> 9) + r_170_actInv4_5_2);
  int32_t _tmp540 = (int32_t) ((((int64_t) (r_169_actInv3_5_0_0) * (int64_t) (parent_a_gf_angular_5_0)) >> 30));
  int32_t _tmp541 = (int32_t) ((((int64_t) (r_169_actInv3_5_0_1) * (int64_t) (parent_a_gf_angular_5_1)) >> 22));
  int32_t _tmp542 = (_tmp540 + (_tmp541 >> 9));
  int32_t _tmp543 = (int32_t) ((((int64_t) (r_169_actInv3_5_0_2) * (int64_t) (parent_a_gf_angular_5_2)) >> 30));
  int32_t r_172_actInv5_5_0 = ((_tmp542 + (_tmp543 >> 1)) >> 1);
  int32_t _tmp544 = (int32_t) ((((int64_t) (r_169_actInv3_5_1_0) * (int64_t) (parent_a_gf_angular_5_0)) >> 30));
  int32_t _tmp545 = (int32_t) ((((int64_t) (r_169_actInv3_5_1_1) * (int64_t) (parent_a_gf_angular_5_1)) >> 22));
  int32_t _tmp546 = (_tmp544 + (_tmp545 >> 9));
  int32_t _tmp547 = (int32_t) ((((int64_t) (r_169_actInv3_5_1_2) * (int64_t) (parent_a_gf_angular_5_2)) >> 30));
  int32_t r_172_actInv5_5_1 = ((_tmp546 + (_tmp547 >> 1)) >> 1);
  int32_t _tmp548 = (int32_t) ((((int64_t) (r_169_actInv3_5_2_0) * (int64_t) (parent_a_gf_angular_5_0)) >> 22));
  int32_t _tmp549 = (int32_t) ((((int64_t) (r_169_actInv3_5_2_1) * (int64_t) (parent_a_gf_angular_5_1)) >> 30));
  int32_t _tmp550 = ((_tmp548 >> 9) + _tmp549);
  int32_t _tmp551 = (int32_t) ((((int64_t) (r_169_actInv3_5_2_2) * (int64_t) (parent_a_gf_angular_5_2)) >> 23));
  int32_t r_172_actInv5_5_2 = (_tmp550 + (_tmp551 >> 9));
  int32_t r_173_act_inv_angular_5_0 = ((new_a_gf2_angular_5_0 >> 1) + r_172_actInv5_5_0);
  int32_t r_173_act_inv_angular_5_1 = ((new_a_gf2_angular_5_1 >> 1) + r_172_actInv5_5_1);
  int32_t r_173_act_inv_angular_5_2 = ((new_a_gf2_angular_5_2 >> 6) + r_172_actInv5_5_2);
  int32_t new_a_gf_up3_5_0 = r_171_act_inv_linear_5_0;
  int32_t new_a_gf_up3_5_1 = r_171_act_inv_linear_5_1;
  int32_t new_a_gf_up3_5_2 = r_171_act_inv_linear_5_2;
  int32_t new_a_gf_up3_5_3 = r_173_act_inv_angular_5_0;
  int32_t new_a_gf_up3_5_4 = r_173_act_inv_angular_5_1;
  int32_t new_a_gf_up3_5_5 = r_173_act_inv_angular_5_2;
  int32_t _tmp552 = (int32_t) ((((int64_t) (lever_5_0) * (int64_t) (act_inv_angular_5_1)) >> 27));
  int32_t _tmp553 = (int32_t) ((((int64_t) (lever_5_1) * (int64_t) (act_inv_angular_5_0)) >> 25));
  int32_t h_linear_1_5_0 = ((_tmp552 - (_tmp553 >> 2)) >> 1);
  int32_t _tmp554 = (int32_t) ((((int64_t) (lever_5_2) * (int64_t) (act_inv_angular_5_0)) >> 25));
  int32_t _tmp555 = (int32_t) ((((int64_t) (lever_5_0) * (int64_t) (act_inv_angular_5_2)) >> 27));
  int32_t h_linear_1_5_1 = ((_tmp554 >> 2) - _tmp555);
  int32_t _tmp556 = (int32_t) ((((int64_t) (lever_5_0) * (int64_t) (act_inv_angular_5_1)) >> 27));
  int32_t _tmp557 = (int32_t) ((((int64_t) (lever_5_1) * (int64_t) (act_inv_angular_5_0)) >> 25));
  int32_t h_linear_1_5_2 = ((_tmp556 - (_tmp557 >> 2)) >> 1);
  int32_t h_linear_2_5_0 = (act_inv_linear_5_0 - (h_linear_1_5_0 >> 3));
  int32_t h_linear_2_5_1 = (act_inv_linear_5_1 - (h_linear_1_5_1 >> 4));
  int32_t h_linear_2_5_2 = (act_inv_linear_5_2 - (h_linear_1_5_2 >> 2));
  int32_t h_linear_5_0 = (int32_t) ((((int64_t) (masses_5) * (int64_t) (h_linear_2_5_0)) >> 30));
  int32_t h_linear_5_1 = (int32_t) ((((int64_t) (masses_5) * (int64_t) (h_linear_2_5_1)) >> 30));
  int32_t h_linear_5_2 = (int32_t) ((((int64_t) (masses_5) * (int64_t) (h_linear_2_5_2)) >> 31));
  int32_t r_174_ = (int32_t) ((((int64_t) (inertia_5_0_0) * (int64_t) (act_inv_angular_5_0)) >> 25));
  int32_t r_175_ = (int32_t) ((((int64_t) (inertia_5_0_1) * (int64_t) (act_inv_angular_5_1)) >> 25));
  int32_t r_176_ = (int32_t) ((((int64_t) (inertia_5_0_2) * (int64_t) (act_inv_angular_5_2)) >> 25));
  int32_t r_177_ = (int32_t) ((((int64_t) (inertia_5_0_1) * (int64_t) (act_inv_angular_5_0)) >> 25));
  int32_t r_178_ = (int32_t) ((((int64_t) (inertia_5_1_1) * (int64_t) (act_inv_angular_5_1)) >> 25));
  int32_t r_179_ = (int32_t) ((((int64_t) (inertia_5_1_2) * (int64_t) (act_inv_angular_5_2)) >> 25));
  int32_t r_180_ = (int32_t) ((((int64_t) (inertia_5_0_2) * (int64_t) (act_inv_angular_5_0)) >> 25));
  int32_t r_181_ = (int32_t) ((((int64_t) (inertia_5_1_2) * (int64_t) (act_inv_angular_5_1)) >> 25));
  int32_t r_182_ = (int32_t) ((((int64_t) (inertia_5_2_2) * (int64_t) (act_inv_angular_5_2)) >> 25));
  int32_t rhsMult1_temp_5 = (r_174_ + r_175_);
  int32_t rhsMult1_5 = (rhsMult1_temp_5 + r_176_);
  int32_t rhsMult2_temp_5 = (r_177_ + r_178_);
  int32_t rhsMult2_5 = (rhsMult2_temp_5 + r_179_);
  int32_t rhsMult3_temp_5 = (r_180_ + r_181_);
  int32_t rhsMult3_5 = (rhsMult3_temp_5 + r_182_);
  int32_t rhsMult_5_0 = rhsMult1_5;
  int32_t rhsMult_5_1 = rhsMult2_5;
  int32_t rhsMult_5_2 = rhsMult3_5;
  int32_t h_angular_first_5_0 = rhsMult_5_0;
  int32_t h_angular_first_5_1 = rhsMult_5_1;
  int32_t h_angular_first_5_2 = rhsMult_5_2;
  int32_t _tmp558 = (int32_t) ((((int64_t) (lever_5_0) * (int64_t) (h_linear_5_1)) >> 27));
  int32_t _tmp559 = (int32_t) ((((int64_t) (lever_5_1) * (int64_t) (h_linear_5_0)) >> 25));
  int32_t h_angular_1_5_0 = ((_tmp558 - (_tmp559 >> 2)) >> 1);
  int32_t _tmp560 = (int32_t) ((((int64_t) (lever_5_2) * (int64_t) (h_linear_5_0)) >> 25));
  int32_t _tmp561 = (int32_t) ((((int64_t) (lever_5_0) * (int64_t) (h_linear_5_2)) >> 27));
  int32_t h_angular_1_5_1 = ((_tmp560 >> 2) - _tmp561);
  int32_t _tmp562 = (int32_t) ((((int64_t) (lever_5_0) * (int64_t) (h_linear_5_1)) >> 27));
  int32_t _tmp563 = (int32_t) ((((int64_t) (lever_5_1) * (int64_t) (h_linear_5_0)) >> 25));
  int32_t h_angular_1_5_2 = ((_tmp562 - (_tmp563 >> 2)) >> 1);
  int32_t h_angular_5_0 = ((h_angular_first_5_0 >> 3) + h_angular_1_5_0);
  int32_t h_angular_5_1 = (((h_angular_first_5_1 >> 2) + h_angular_1_5_1) >> 1);
  int32_t h_angular_5_2 = ((h_angular_first_5_2 >> 3) + h_angular_1_5_2);
  int32_t _tmp564 = (int32_t) ((((int64_t) (lever_5_0) * (int64_t) (r_173_act_inv_angular_5_1)) >> 27));
  int32_t _tmp565 = (int32_t) ((((int64_t) (lever_5_1) * (int64_t) (r_173_act_inv_angular_5_0)) >> 25));
  int32_t f_linear_1_5_0 = ((_tmp564 - (_tmp565 >> 2)) >> 1);
  int32_t _tmp566 = (int32_t) ((((int64_t) (lever_5_2) * (int64_t) (r_173_act_inv_angular_5_0)) >> 25));
  int32_t _tmp567 = (int32_t) ((((int64_t) (lever_5_0) * (int64_t) (r_173_act_inv_angular_5_2)) >> 27));
  int32_t f_linear_1_5_1 = (((_tmp566 >> 1) - _tmp567) >> 1);
  int32_t _tmp568 = (int32_t) ((((int64_t) (lever_5_0) * (int64_t) (r_173_act_inv_angular_5_1)) >> 27));
  int32_t _tmp569 = (int32_t) ((((int64_t) (lever_5_1) * (int64_t) (r_173_act_inv_angular_5_0)) >> 25));
  int32_t f_linear_1_5_2 = ((_tmp568 - (_tmp569 >> 2)) >> 1);
  int32_t f_linear_2_5_0 = (r_171_act_inv_linear_5_0 - (f_linear_1_5_0 >> 3));
  int32_t f_linear_2_5_1 = (r_171_act_inv_linear_5_1 - (f_linear_1_5_1 >> 4));
  int32_t f_linear_2_5_2 = ((r_171_act_inv_linear_5_2 - (f_linear_1_5_2 >> 2)) >> 1);
  int32_t f_linear_3_5_0 = (int32_t) ((((int64_t) (masses_5) * (int64_t) (f_linear_2_5_0)) >> 31));
  int32_t f_linear_3_5_1 = (int32_t) ((((int64_t) (masses_5) * (int64_t) (f_linear_2_5_1)) >> 31));
  int32_t f_linear_3_5_2 = (int32_t) ((((int64_t) (masses_5) * (int64_t) (f_linear_2_5_2)) >> 30));
  int32_t r_183_ = (int32_t) ((((int64_t) (inertia_5_0_0) * (int64_t) (r_173_act_inv_angular_5_0)) >> 22));
  int32_t r_184_ = (int32_t) ((((int64_t) (inertia_5_0_1) * (int64_t) (r_173_act_inv_angular_5_1)) >> 21));
  int32_t r_185_ = (int32_t) ((((int64_t) (inertia_5_0_2) * (int64_t) (r_173_act_inv_angular_5_2)) >> 22));
  int32_t r_186_ = (int32_t) ((((int64_t) (inertia_5_0_1) * (int64_t) (r_173_act_inv_angular_5_0)) >> 21));
  int32_t r_187_ = (int32_t) ((((int64_t) (inertia_5_1_1) * (int64_t) (r_173_act_inv_angular_5_1)) >> 24));
  int32_t r_188_ = (int32_t) ((((int64_t) (inertia_5_1_2) * (int64_t) (r_173_act_inv_angular_5_2)) >> 22));
  int32_t r_189_ = (int32_t) ((((int64_t) (inertia_5_0_2) * (int64_t) (r_173_act_inv_angular_5_0)) >> 22));
  int32_t r_190_ = (int32_t) ((((int64_t) (inertia_5_1_2) * (int64_t) (r_173_act_inv_angular_5_1)) >> 21));
  int32_t r_191_ = (int32_t) ((((int64_t) (inertia_5_2_2) * (int64_t) (r_173_act_inv_angular_5_2)) >> 24));
  int32_t r_192_rhsMult1_temp_5 = ((r_183_ + (r_184_ >> 1)) >> 1);
  int32_t r_193_rhsMult1_5 = (r_192_rhsMult1_temp_5 + (r_185_ >> 2));
  int32_t r_194_rhsMult2_temp_5 = ((r_186_ >> 3) + r_187_);
  int32_t r_195_rhsMult2_5 = (r_194_rhsMult2_temp_5 + (r_188_ >> 3));
  int32_t r_196_rhsMult3_temp_5 = (r_189_ + (r_190_ >> 1));
  int32_t r_197_rhsMult3_5 = (((r_196_rhsMult3_temp_5 >> 1) + r_191_) >> 1);
  int32_t r_198_rhsMult_5_0 = r_193_rhsMult1_5;
  int32_t r_198_rhsMult_5_1 = r_195_rhsMult2_5;
  int32_t r_198_rhsMult_5_2 = r_197_rhsMult3_5;
  int32_t f_angular_first_5_0 = r_198_rhsMult_5_0;
  int32_t f_angular_first_5_1 = r_198_rhsMult_5_1;
  int32_t f_angular_first_5_2 = r_198_rhsMult_5_2;
  int32_t _tmp570 = (int32_t) ((((int64_t) (lever_5_0) * (int64_t) (f_linear_3_5_1)) >> 27));
  int32_t _tmp571 = (int32_t) ((((int64_t) (lever_5_1) * (int64_t) (f_linear_3_5_0)) >> 25));
  int32_t f_angular_1_5_0 = (_tmp570 - (_tmp571 >> 2));
  int32_t _tmp572 = (int32_t) ((((int64_t) (lever_5_2) * (int64_t) (f_linear_3_5_0)) >> 24));
  int32_t _tmp573 = (int32_t) ((((int64_t) (lever_5_0) * (int64_t) (f_linear_3_5_2)) >> 27));
  int32_t f_angular_1_5_1 = (((_tmp572 >> 2) - _tmp573) >> 1);
  int32_t _tmp574 = (int32_t) ((((int64_t) (lever_5_0) * (int64_t) (f_linear_3_5_1)) >> 27));
  int32_t _tmp575 = (int32_t) ((((int64_t) (lever_5_1) * (int64_t) (f_linear_3_5_0)) >> 25));
  int32_t f_angular_1_5_2 = (_tmp574 - (_tmp575 >> 2));
  int32_t f_angular_2_5_0 = ((f_angular_first_5_0 >> 5) + f_angular_1_5_0);
  int32_t f_angular_2_5_1 = ((f_angular_first_5_1 >> 4) + f_angular_1_5_1);
  int32_t f_angular_2_5_2 = ((f_angular_first_5_2 >> 4) + f_angular_1_5_2);
  int32_t _tmp576 = (int32_t) ((((int64_t) (act_inv_angular_5_0) * (int64_t) (h_linear_5_1)) >> 31));
  int32_t _tmp577 = (int32_t) ((((int64_t) (act_inv_angular_5_1) * (int64_t) (h_linear_5_0)) >> 31));
  int32_t f_linear_4_temp_5_0 = ((_tmp576 - _tmp577) >> 1);
  int32_t _tmp578 = (int32_t) ((((int64_t) (act_inv_angular_5_2) * (int64_t) (h_linear_5_0)) >> 31));
  int32_t _tmp579 = (int32_t) ((((int64_t) (act_inv_angular_5_0) * (int64_t) (h_linear_5_2)) >> 31));
  int32_t f_linear_4_temp_5_1 = ((_tmp578 - _tmp579) >> 1);
  int32_t _tmp580 = (int32_t) ((((int64_t) (act_inv_angular_5_0) * (int64_t) (h_linear_5_1)) >> 31));
  int32_t _tmp581 = (int32_t) ((((int64_t) (act_inv_angular_5_1) * (int64_t) (h_linear_5_0)) >> 31));
  int32_t f_linear_4_temp_5_2 = ((_tmp580 - _tmp581) >> 1);
  int32_t f_linear_4_5_0 = (((f_linear_3_5_0 >> 2) + f_linear_4_temp_5_0) >> 1);
  int32_t f_linear_4_5_1 = ((f_linear_3_5_1 >> 2) + f_linear_4_temp_5_1);
  int32_t f_linear_4_5_2 = (((f_linear_3_5_2 >> 3) + f_linear_4_temp_5_2) >> 1);
  int32_t _tmp582 = (int32_t) ((((int64_t) (act_inv_angular_5_0) * (int64_t) (h_angular_first_5_1)) >> 30));
  int32_t _tmp583 = (int32_t) ((((int64_t) (act_inv_angular_5_1) * (int64_t) (h_angular_first_5_0)) >> 29));
  int32_t f_angular_3_temp_5_0 = (_tmp582 - (_tmp583 >> 1));
  int32_t _tmp584 = (int32_t) ((((int64_t) (act_inv_angular_5_2) * (int64_t) (h_angular_first_5_0)) >> 28));
  int32_t _tmp585 = (int32_t) ((((int64_t) (act_inv_angular_5_0) * (int64_t) (h_angular_first_5_2)) >> 30));
  int32_t f_angular_3_temp_5_1 = ((_tmp584 >> 2) - _tmp585);
  int32_t _tmp586 = (int32_t) ((((int64_t) (act_inv_angular_5_0) * (int64_t) (h_angular_first_5_1)) >> 30));
  int32_t _tmp587 = (int32_t) ((((int64_t) (act_inv_angular_5_1) * (int64_t) (h_angular_first_5_0)) >> 29));
  int32_t f_angular_3_temp_5_2 = (_tmp586 - (_tmp587 >> 1));
  int32_t f_angular_3_5_0 = ((f_angular_2_5_0 + (f_angular_3_temp_5_0 >> 2)) >> 1);
  int32_t f_angular_3_5_1 = (f_angular_2_5_1 + (f_angular_3_temp_5_1 >> 2));
  int32_t f_angular_3_5_2 = ((f_angular_2_5_2 + (f_angular_3_temp_5_2 >> 2)) >> 1);
  int32_t _tmp588 = (int32_t) ((((int64_t) (act_inv_linear_5_0) * (int64_t) (h_linear_5_1)) >> 30));
  int32_t _tmp589 = (int32_t) ((((int64_t) (act_inv_linear_5_1) * (int64_t) (h_linear_5_0)) >> 31));
  int32_t f_angular_4_temp_5_0 = (((_tmp588 >> 1) - _tmp589) >> 1);
  int32_t _tmp590 = (int32_t) ((((int64_t) (act_inv_linear_5_2) * (int64_t) (h_linear_5_0)) >> 31));
  int32_t _tmp591 = (int32_t) ((((int64_t) (act_inv_linear_5_0) * (int64_t) (h_linear_5_2)) >> 30));
  int32_t f_angular_4_temp_5_1 = ((_tmp590 - _tmp591) >> 1);
  int32_t _tmp592 = (int32_t) ((((int64_t) (act_inv_linear_5_0) * (int64_t) (h_linear_5_1)) >> 30));
  int32_t _tmp593 = (int32_t) ((((int64_t) (act_inv_linear_5_1) * (int64_t) (h_linear_5_0)) >> 31));
  int32_t f_angular_4_temp_5_2 = (((_tmp592 >> 1) - _tmp593) >> 1);
  int32_t f_angular_4_5_0 = ((f_angular_3_5_0 >> 5) + f_angular_4_temp_5_0);
  int32_t f_angular_4_5_1 = ((f_angular_3_5_1 >> 5) + f_angular_4_temp_5_1);
  int32_t f_angular_4_5_2 = ((f_angular_3_5_2 >> 5) + f_angular_4_temp_5_2);
  int32_t h_rnea_firstpass5_0 = h_linear_5_0;
  int32_t h_rnea_firstpass5_1 = h_linear_5_1;
  int32_t h_rnea_firstpass5_2 = h_linear_5_2;
  int32_t h_rnea_firstpass5_3 = h_angular_5_0;
  int32_t h_rnea_firstpass5_4 = h_angular_5_1;
  int32_t h_rnea_firstpass5_5 = h_angular_5_2;
  int32_t f_rnea_firstpass5_0 = f_linear_4_5_0;
  int32_t f_rnea_firstpass5_1 = f_linear_4_5_1;
  int32_t f_rnea_firstpass5_2 = f_linear_4_5_2;
  int32_t f_rnea_firstpass5_3 = f_angular_4_5_0;
  int32_t f_rnea_firstpass5_4 = f_angular_4_5_1;
  int32_t f_rnea_firstpass5_5 = f_angular_4_5_2;
  int32_t r_199_new_v_5_0 = act_inv_linear_5_0;
  int32_t r_199_new_v_5_1 = act_inv_linear_5_1;
  int32_t r_199_new_v_5_2 = act_inv_linear_5_2;
  int32_t r_199_new_v_5_3 = act_inv_angular_5_0;
  int32_t r_199_new_v_5_4 = act_inv_angular_5_1;
  int32_t r_199_new_v_5_5 = act_inv_angular_5_2;
  int32_t data_tau_temp_5 = f_rnea_firstpass5_5;
  int32_t f_linear_5_0 = f_rnea_firstpass5_0;
  int32_t f_linear_5_1 = f_rnea_firstpass5_1;
  int32_t f_linear_5_2 = f_rnea_firstpass5_2;
  int32_t f_angular_5_0 = f_rnea_firstpass5_3;
  int32_t f_angular_5_1 = f_rnea_firstpass5_4;
  int32_t f_angular_5_2 = f_rnea_firstpass5_5;
  int32_t _tmp594 = (int32_t) ((((int64_t) (limi_rotation_5_0_0) * (int64_t) (f_linear_5_0)) >> 30));
  int32_t _tmp595 = (int32_t) ((((int64_t) (limi_rotation_5_0_1) * (int64_t) (f_linear_5_1)) >> 30));
  int32_t _tmp596 = (_tmp594 + (_tmp595 >> 1));
  int32_t _tmp597 = (int32_t) ((((int64_t) (limi_rotation_5_0_2) * (int64_t) (f_linear_5_2)) >> 17));
  int32_t new_f_linear_5_0 = (_tmp596 + (_tmp597 >> 14));
  int32_t _tmp598 = (int32_t) ((((int64_t) (limi_rotation_5_1_0) * (int64_t) (f_linear_5_0)) >> 17));
  int32_t _tmp599 = (int32_t) ((((int64_t) (limi_rotation_5_1_1) * (int64_t) (f_linear_5_1)) >> 18));
  int32_t _tmp600 = (_tmp598 + _tmp599);
  int32_t _tmp601 = (int32_t) ((((int64_t) (limi_rotation_5_1_2) * (int64_t) (f_linear_5_2)) >> 30));
  int32_t new_f_linear_5_1 = ((_tmp600 >> 14) + _tmp601);
  int32_t _tmp602 = (int32_t) ((((int64_t) (limi_rotation_5_2_0) * (int64_t) (f_linear_5_0)) >> 30));
  int32_t _tmp603 = (int32_t) ((((int64_t) (limi_rotation_5_2_1) * (int64_t) (f_linear_5_1)) >> 30));
  int32_t _tmp604 = (_tmp602 + (_tmp603 >> 1));
  int32_t _tmp605 = (int32_t) ((((int64_t) (limi_rotation_5_2_2) * (int64_t) (f_linear_5_2)) >> 17));
  int32_t new_f_linear_5_2 = (_tmp604 + (_tmp605 >> 14));
  int32_t _tmp606 = (int32_t) ((((int64_t) (limi_rotation_5_0_0) * (int64_t) (f_angular_5_0)) >> 30));
  int32_t _tmp607 = (int32_t) ((((int64_t) (limi_rotation_5_0_1) * (int64_t) (f_angular_5_1)) >> 30));
  int32_t _tmp608 = (_tmp606 + (_tmp607 >> 1));
  int32_t _tmp609 = (int32_t) ((((int64_t) (limi_rotation_5_0_2) * (int64_t) (f_angular_5_2)) >> 18));
  int32_t new_f_angular_temp_5_0 = (_tmp608 + (_tmp609 >> 13));
  int32_t _tmp610 = (int32_t) ((((int64_t) (limi_rotation_5_1_0) * (int64_t) (f_angular_5_0)) >> 18));
  int32_t _tmp611 = (int32_t) ((((int64_t) (limi_rotation_5_1_1) * (int64_t) (f_angular_5_1)) >> 19));
  int32_t _tmp612 = (_tmp610 + _tmp611);
  int32_t _tmp613 = (int32_t) ((((int64_t) (limi_rotation_5_1_2) * (int64_t) (f_angular_5_2)) >> 30));
  int32_t new_f_angular_temp_5_1 = ((_tmp612 >> 13) + _tmp613);
  int32_t _tmp614 = (int32_t) ((((int64_t) (limi_rotation_5_2_0) * (int64_t) (f_angular_5_0)) >> 30));
  int32_t _tmp615 = (int32_t) ((((int64_t) (limi_rotation_5_2_1) * (int64_t) (f_angular_5_1)) >> 30));
  int32_t _tmp616 = (_tmp614 + (_tmp615 >> 1));
  int32_t _tmp617 = (int32_t) ((((int64_t) (limi_rotation_5_2_2) * (int64_t) (f_angular_5_2)) >> 18));
  int32_t new_f_angular_temp_5_2 = (_tmp616 + (_tmp617 >> 13));
  int32_t _tmp618 = (int32_t) ((((int64_t) (limi_translation_5_0) * (int64_t) (new_f_linear_5_1)) >> 17));
  int32_t _tmp619 = (int32_t) ((((int64_t) (limi_translation_5_1) * (int64_t) (new_f_linear_5_0)) >> 17));
  int32_t f_angular_cross_5_0 = (_tmp618 - _tmp619);
  int32_t _tmp620 = (int32_t) ((((int64_t) (limi_translation_5_2) * (int64_t) (new_f_linear_5_0)) >> 17));
  int32_t _tmp621 = (int32_t) ((((int64_t) (limi_translation_5_0) * (int64_t) (new_f_linear_5_2)) >> 17));
  int32_t f_angular_cross_5_1 = (_tmp620 - _tmp621);
  int32_t _tmp622 = (int32_t) ((((int64_t) (limi_translation_5_0) * (int64_t) (new_f_linear_5_1)) >> 17));
  int32_t _tmp623 = (int32_t) ((((int64_t) (limi_translation_5_1) * (int64_t) (new_f_linear_5_0)) >> 17));
  int32_t f_angular_cross_5_2 = (_tmp622 - _tmp623);
  int32_t new_f_angular_5_0 = (new_f_angular_temp_5_0 + (f_angular_cross_5_0 >> 13));
  int32_t new_f_angular_5_1 = (new_f_angular_temp_5_1 + (f_angular_cross_5_1 >> 13));
  int32_t new_f_angular_5_2 = (new_f_angular_temp_5_2 + (f_angular_cross_5_2 >> 13));
  int32_t r_200_new_f_5_0 = new_f_linear_5_0;
  int32_t r_200_new_f_5_1 = new_f_linear_5_1;
  int32_t r_200_new_f_5_2 = new_f_linear_5_2;
  int32_t r_200_new_f_5_3 = new_f_angular_5_0;
  int32_t r_200_new_f_5_4 = new_f_angular_5_1;
  int32_t r_200_new_f_5_5 = new_f_angular_5_2;
  int32_t new_data_f_parent_5_0 = (((f_rnea_firstpass4_0 >> 2) + r_200_new_f_5_0) >> 1);
  int32_t new_data_f_parent_5_1 = ((f_rnea_firstpass4_1 >> 3) + r_200_new_f_5_1);
  int32_t new_data_f_parent_5_2 = (((f_rnea_firstpass4_2 >> 2) + r_200_new_f_5_2) >> 1);
  int32_t new_data_f_parent_5_3 = (((f_rnea_firstpass4_3 >> 1) + r_200_new_f_5_3) >> 1);
  int32_t new_data_f_parent_5_4 = ((f_rnea_firstpass4_4 >> 3) + r_200_new_f_5_4);
  int32_t new_data_f_parent_5_5 = (((f_rnea_firstpass4_5 >> 1) + r_200_new_f_5_5) >> 1);
  int32_t data_tau_temp_4 = new_data_f_parent_5_5;
  int32_t f_linear_4_0 = new_data_f_parent_5_0;
  int32_t f_linear_4_1 = new_data_f_parent_5_1;
  int32_t f_linear_4_2 = new_data_f_parent_5_2;
  int32_t f_angular_4_0 = new_data_f_parent_5_3;
  int32_t f_angular_4_1 = new_data_f_parent_5_4;
  int32_t f_angular_4_2 = new_data_f_parent_5_5;
  int32_t _tmp624 = (int32_t) ((((int64_t) (limi_rotation_4_0_0) * (int64_t) (f_linear_4_0)) >> 30));
  int32_t _tmp625 = (int32_t) ((((int64_t) (limi_rotation_4_0_1) * (int64_t) (f_linear_4_1)) >> 30));
  int32_t _tmp626 = (_tmp624 + (_tmp625 >> 1));
  int32_t _tmp627 = (int32_t) ((((int64_t) (limi_rotation_4_0_2) * (int64_t) (f_linear_4_2)) >> 16));
  int32_t new_f_linear_4_0 = (_tmp626 + (_tmp627 >> 15));
  int32_t _tmp628 = (int32_t) ((((int64_t) (limi_rotation_4_1_0) * (int64_t) (f_linear_4_0)) >> 16));
  int32_t _tmp629 = (int32_t) ((((int64_t) (limi_rotation_4_1_1) * (int64_t) (f_linear_4_1)) >> 17));
  int32_t _tmp630 = (_tmp628 + _tmp629);
  int32_t _tmp631 = (int32_t) ((((int64_t) (limi_rotation_4_1_2) * (int64_t) (f_linear_4_2)) >> 30));
  int32_t new_f_linear_4_1 = ((_tmp630 >> 15) + _tmp631);
  int32_t _tmp632 = (int32_t) ((((int64_t) (limi_rotation_4_2_0) * (int64_t) (f_linear_4_0)) >> 30));
  int32_t _tmp633 = (int32_t) ((((int64_t) (limi_rotation_4_2_1) * (int64_t) (f_linear_4_1)) >> 30));
  int32_t _tmp634 = (_tmp632 + (_tmp633 >> 1));
  int32_t _tmp635 = (int32_t) ((((int64_t) (limi_rotation_4_2_2) * (int64_t) (f_linear_4_2)) >> 16));
  int32_t new_f_linear_4_2 = (_tmp634 + (_tmp635 >> 15));
  int32_t _tmp636 = (int32_t) ((((int64_t) (limi_rotation_4_0_0) * (int64_t) (f_angular_4_0)) >> 30));
  int32_t _tmp637 = (int32_t) ((((int64_t) (limi_rotation_4_0_1) * (int64_t) (f_angular_4_1)) >> 30));
  int32_t _tmp638 = (_tmp636 + (_tmp637 >> 1));
  int32_t _tmp639 = (int32_t) ((((int64_t) (limi_rotation_4_0_2) * (int64_t) (f_angular_4_2)) >> 17));
  int32_t new_f_angular_temp_4_0 = (_tmp638 + (_tmp639 >> 14));
  int32_t _tmp640 = (int32_t) ((((int64_t) (limi_rotation_4_1_0) * (int64_t) (f_angular_4_0)) >> 17));
  int32_t _tmp641 = (int32_t) ((((int64_t) (limi_rotation_4_1_1) * (int64_t) (f_angular_4_1)) >> 18));
  int32_t _tmp642 = (_tmp640 + _tmp641);
  int32_t _tmp643 = (int32_t) ((((int64_t) (limi_rotation_4_1_2) * (int64_t) (f_angular_4_2)) >> 30));
  int32_t new_f_angular_temp_4_1 = ((_tmp642 >> 14) + _tmp643);
  int32_t _tmp644 = (int32_t) ((((int64_t) (limi_rotation_4_2_0) * (int64_t) (f_angular_4_0)) >> 30));
  int32_t _tmp645 = (int32_t) ((((int64_t) (limi_rotation_4_2_1) * (int64_t) (f_angular_4_1)) >> 30));
  int32_t _tmp646 = (_tmp644 + (_tmp645 >> 1));
  int32_t _tmp647 = (int32_t) ((((int64_t) (limi_rotation_4_2_2) * (int64_t) (f_angular_4_2)) >> 17));
  int32_t new_f_angular_temp_4_2 = (_tmp646 + (_tmp647 >> 14));
  int32_t _tmp648 = (int32_t) ((((int64_t) (limi_translation_4_0) * (int64_t) (new_f_linear_4_1)) >> 27));
  int32_t _tmp649 = (int32_t) ((((int64_t) (limi_translation_4_1) * (int64_t) (new_f_linear_4_0)) >> 30));
  int32_t f_angular_cross_4_0 = ((_tmp648 >> 3) - _tmp649);
  int32_t _tmp650 = (int32_t) ((((int64_t) (limi_translation_4_2) * (int64_t) (new_f_linear_4_0)) >> 16));
  int32_t _tmp651 = (int32_t) ((((int64_t) (limi_translation_4_0) * (int64_t) (new_f_linear_4_2)) >> 28));
  int32_t f_angular_cross_4_1 = ((_tmp650 >> 12) - _tmp651);
  int32_t _tmp652 = (int32_t) ((((int64_t) (limi_translation_4_0) * (int64_t) (new_f_linear_4_1)) >> 27));
  int32_t _tmp653 = (int32_t) ((((int64_t) (limi_translation_4_1) * (int64_t) (new_f_linear_4_0)) >> 30));
  int32_t f_angular_cross_4_2 = ((_tmp652 >> 3) - _tmp653);
  int32_t new_f_angular_4_0 = ((new_f_angular_temp_4_0 + f_angular_cross_4_0) >> 1);
  int32_t new_f_angular_4_1 = (new_f_angular_temp_4_1 + (f_angular_cross_4_1 >> 2));
  int32_t new_f_angular_4_2 = ((new_f_angular_temp_4_2 + f_angular_cross_4_2) >> 1);
  int32_t r_201_new_f_4_0 = new_f_linear_4_0;
  int32_t r_201_new_f_4_1 = new_f_linear_4_1;
  int32_t r_201_new_f_4_2 = new_f_linear_4_2;
  int32_t r_201_new_f_4_3 = new_f_angular_4_0;
  int32_t r_201_new_f_4_4 = new_f_angular_4_1;
  int32_t r_201_new_f_4_5 = new_f_angular_4_2;
  int32_t new_data_f_parent_4_0 = ((f_rnea_firstpass3_0 >> 4) + r_201_new_f_4_0);
  int32_t new_data_f_parent_4_1 = ((f_rnea_firstpass3_1 >> 4) + r_201_new_f_4_1);
  int32_t new_data_f_parent_4_2 = ((f_rnea_firstpass3_2 >> 4) + r_201_new_f_4_2);
  int32_t new_data_f_parent_4_3 = ((f_rnea_firstpass3_3 >> 6) + r_201_new_f_4_3);
  int32_t new_data_f_parent_4_4 = ((f_rnea_firstpass3_4 >> 5) + r_201_new_f_4_4);
  int32_t new_data_f_parent_4_5 = ((f_rnea_firstpass3_5 >> 6) + r_201_new_f_4_5);
  int32_t data_tau_temp_3 = new_data_f_parent_4_5;
  int32_t f_linear_3_0 = new_data_f_parent_4_0;
  int32_t f_linear_3_1 = new_data_f_parent_4_1;
  int32_t f_linear_3_2 = new_data_f_parent_4_2;
  int32_t f_angular_3_0 = new_data_f_parent_4_3;
  int32_t f_angular_3_1 = new_data_f_parent_4_4;
  int32_t f_angular_3_2 = new_data_f_parent_4_5;
  int32_t _tmp654 = (int32_t) ((((int64_t) (limi_rotation_3_0_0) * (int64_t) (f_linear_3_0)) >> 30));
  int32_t _tmp655 = (int32_t) ((((int64_t) (limi_rotation_3_0_1) * (int64_t) (f_linear_3_1)) >> 30));
  int32_t _tmp656 = ((_tmp654 + _tmp655) >> 1);
  int32_t _tmp657 = (int32_t) ((((int64_t) (limi_rotation_3_0_2) * (int64_t) (f_linear_3_2)) >> 16));
  int32_t new_f_linear_3_0 = (_tmp656 + (_tmp657 >> 16));
  int32_t _tmp658 = (int32_t) ((((int64_t) (limi_rotation_3_1_0) * (int64_t) (f_linear_3_0)) >> 16));
  int32_t _tmp659 = (int32_t) ((((int64_t) (limi_rotation_3_1_1) * (int64_t) (f_linear_3_1)) >> 16));
  int32_t _tmp660 = (_tmp658 + _tmp659);
  int32_t _tmp661 = (int32_t) ((((int64_t) (limi_rotation_3_1_2) * (int64_t) (f_linear_3_2)) >> 30));
  int32_t new_f_linear_3_1 = ((_tmp660 >> 15) + _tmp661);
  int32_t _tmp662 = (int32_t) ((((int64_t) (limi_rotation_3_2_0) * (int64_t) (f_linear_3_0)) >> 30));
  int32_t _tmp663 = (int32_t) ((((int64_t) (limi_rotation_3_2_1) * (int64_t) (f_linear_3_1)) >> 30));
  int32_t _tmp664 = ((_tmp662 + _tmp663) >> 1);
  int32_t _tmp665 = (int32_t) ((((int64_t) (limi_rotation_3_2_2) * (int64_t) (f_linear_3_2)) >> 16));
  int32_t new_f_linear_3_2 = (_tmp664 + (_tmp665 >> 16));
  int32_t _tmp666 = (int32_t) ((((int64_t) (limi_rotation_3_0_0) * (int64_t) (f_angular_3_0)) >> 30));
  int32_t _tmp667 = (int32_t) ((((int64_t) (limi_rotation_3_0_1) * (int64_t) (f_angular_3_1)) >> 30));
  int32_t _tmp668 = ((_tmp666 + (_tmp667 >> 1)) >> 1);
  int32_t _tmp669 = (int32_t) ((((int64_t) (limi_rotation_3_0_2) * (int64_t) (f_angular_3_2)) >> 16));
  int32_t new_f_angular_temp_3_0 = (_tmp668 + (_tmp669 >> 16));
  int32_t _tmp670 = (int32_t) ((((int64_t) (limi_rotation_3_1_0) * (int64_t) (f_angular_3_0)) >> 16));
  int32_t _tmp671 = (int32_t) ((((int64_t) (limi_rotation_3_1_1) * (int64_t) (f_angular_3_1)) >> 17));
  int32_t _tmp672 = (_tmp670 + _tmp671);
  int32_t _tmp673 = (int32_t) ((((int64_t) (limi_rotation_3_1_2) * (int64_t) (f_angular_3_2)) >> 30));
  int32_t new_f_angular_temp_3_1 = ((_tmp672 >> 15) + _tmp673);
  int32_t _tmp674 = (int32_t) ((((int64_t) (limi_rotation_3_2_0) * (int64_t) (f_angular_3_0)) >> 30));
  int32_t _tmp675 = (int32_t) ((((int64_t) (limi_rotation_3_2_1) * (int64_t) (f_angular_3_1)) >> 30));
  int32_t _tmp676 = ((_tmp674 + (_tmp675 >> 1)) >> 1);
  int32_t _tmp677 = (int32_t) ((((int64_t) (limi_rotation_3_2_2) * (int64_t) (f_angular_3_2)) >> 16));
  int32_t new_f_angular_temp_3_2 = (_tmp676 + (_tmp677 >> 16));
  int32_t _tmp678 = (int32_t) ((((int64_t) (limi_translation_3_0) * (int64_t) (new_f_linear_3_1)) >> 28));
  int32_t _tmp679 = (int32_t) ((((int64_t) (limi_translation_3_1) * (int64_t) (new_f_linear_3_0)) >> 15));
  int32_t f_angular_cross_3_0 = (_tmp678 - (_tmp679 >> 12));
  int32_t _tmp680 = (int32_t) ((((int64_t) (limi_translation_3_2) * (int64_t) (new_f_linear_3_0)) >> 15));
  int32_t _tmp681 = (int32_t) ((((int64_t) (limi_translation_3_0) * (int64_t) (new_f_linear_3_2)) >> 28));
  int32_t f_angular_cross_3_1 = ((_tmp680 >> 13) - _tmp681);
  int32_t _tmp682 = (int32_t) ((((int64_t) (limi_translation_3_0) * (int64_t) (new_f_linear_3_1)) >> 28));
  int32_t _tmp683 = (int32_t) ((((int64_t) (limi_translation_3_1) * (int64_t) (new_f_linear_3_0)) >> 15));
  int32_t f_angular_cross_3_2 = (_tmp682 - (_tmp683 >> 12));
  int32_t new_f_angular_3_0 = (new_f_angular_temp_3_0 + (f_angular_cross_3_0 >> 4));
  int32_t new_f_angular_3_1 = (new_f_angular_temp_3_1 + (f_angular_cross_3_1 >> 2));
  int32_t new_f_angular_3_2 = (new_f_angular_temp_3_2 + (f_angular_cross_3_2 >> 4));
  int32_t r_202_new_f_3_0 = new_f_linear_3_0;
  int32_t r_202_new_f_3_1 = new_f_linear_3_1;
  int32_t r_202_new_f_3_2 = new_f_linear_3_2;
  int32_t r_202_new_f_3_3 = new_f_angular_3_0;
  int32_t r_202_new_f_3_4 = new_f_angular_3_1;
  int32_t r_202_new_f_3_5 = new_f_angular_3_2;
  int32_t new_data_f_parent_3_0 = ((f_rnea_firstpass2_0 >> 7) + r_202_new_f_3_0);
  int32_t new_data_f_parent_3_1 = ((f_rnea_firstpass2_1 >> 6) + r_202_new_f_3_1);
  int32_t new_data_f_parent_3_2 = ((f_rnea_firstpass2_2 >> 7) + r_202_new_f_3_2);
  int32_t new_data_f_parent_3_3 = ((f_rnea_firstpass2_3 >> 9) + r_202_new_f_3_3);
  int32_t new_data_f_parent_3_4 = ((f_rnea_firstpass2_4 >> 10) + r_202_new_f_3_4);
  int32_t new_data_f_parent_3_5 = ((f_rnea_firstpass2_5 >> 9) + r_202_new_f_3_5);
  int32_t data_tau_temp_2 = new_data_f_parent_3_5;
  int32_t f_linear_2_0 = new_data_f_parent_3_0;
  int32_t f_linear_2_1 = new_data_f_parent_3_1;
  int32_t f_linear_2_2 = new_data_f_parent_3_2;
  int32_t f_angular_2_0 = new_data_f_parent_3_3;
  int32_t f_angular_2_1 = new_data_f_parent_3_4;
  int32_t f_angular_2_2 = new_data_f_parent_3_5;
  int32_t _tmp684 = (int32_t) ((((int64_t) (limi_rotation_2_0_0) * (int64_t) (f_linear_2_0)) >> 30));
  int32_t _tmp685 = (int32_t) ((((int64_t) (limi_rotation_2_0_1) * (int64_t) (f_linear_2_1)) >> 30));
  int32_t _tmp686 = ((_tmp684 + (_tmp685 >> 1)) >> 1);
  int32_t _tmp687 = (int32_t) ((((int64_t) (limi_rotation_2_0_2) * (int64_t) (f_linear_2_2)) >> 15));
  int32_t new_f_linear_2_0 = (_tmp686 + (_tmp687 >> 17));
  int32_t _tmp688 = (int32_t) ((((int64_t) (limi_rotation_2_1_0) * (int64_t) (f_linear_2_0)) >> 15));
  int32_t _tmp689 = (int32_t) ((((int64_t) (limi_rotation_2_1_1) * (int64_t) (f_linear_2_1)) >> 16));
  int32_t _tmp690 = (_tmp688 + _tmp689);
  int32_t _tmp691 = (int32_t) ((((int64_t) (limi_rotation_2_1_2) * (int64_t) (f_linear_2_2)) >> 30));
  int32_t new_f_linear_2_1 = ((_tmp690 >> 16) + _tmp691);
  int32_t _tmp692 = (int32_t) ((((int64_t) (limi_rotation_2_2_0) * (int64_t) (f_linear_2_0)) >> 30));
  int32_t _tmp693 = (int32_t) ((((int64_t) (limi_rotation_2_2_1) * (int64_t) (f_linear_2_1)) >> 30));
  int32_t _tmp694 = ((_tmp692 + (_tmp693 >> 1)) >> 1);
  int32_t _tmp695 = (int32_t) ((((int64_t) (limi_rotation_2_2_2) * (int64_t) (f_linear_2_2)) >> 15));
  int32_t new_f_linear_2_2 = (_tmp694 + (_tmp695 >> 17));
  int32_t _tmp696 = (int32_t) ((((int64_t) (limi_rotation_2_0_0) * (int64_t) (f_angular_2_0)) >> 30));
  int32_t _tmp697 = (int32_t) ((((int64_t) (limi_rotation_2_0_1) * (int64_t) (f_angular_2_1)) >> 30));
  int32_t _tmp698 = ((_tmp696 + (_tmp697 >> 1)) >> 1);
  int32_t _tmp699 = (int32_t) ((((int64_t) (limi_rotation_2_0_2) * (int64_t) (f_angular_2_2)) >> 15));
  int32_t new_f_angular_temp_2_0 = (_tmp698 + (_tmp699 >> 17));
  int32_t _tmp700 = (int32_t) ((((int64_t) (limi_rotation_2_1_0) * (int64_t) (f_angular_2_0)) >> 15));
  int32_t _tmp701 = (int32_t) ((((int64_t) (limi_rotation_2_1_1) * (int64_t) (f_angular_2_1)) >> 16));
  int32_t _tmp702 = (_tmp700 + _tmp701);
  int32_t _tmp703 = (int32_t) ((((int64_t) (limi_rotation_2_1_2) * (int64_t) (f_angular_2_2)) >> 30));
  int32_t new_f_angular_temp_2_1 = ((_tmp702 >> 16) + _tmp703);
  int32_t _tmp704 = (int32_t) ((((int64_t) (limi_rotation_2_2_0) * (int64_t) (f_angular_2_0)) >> 30));
  int32_t _tmp705 = (int32_t) ((((int64_t) (limi_rotation_2_2_1) * (int64_t) (f_angular_2_1)) >> 30));
  int32_t _tmp706 = ((_tmp704 + (_tmp705 >> 1)) >> 1);
  int32_t _tmp707 = (int32_t) ((((int64_t) (limi_rotation_2_2_2) * (int64_t) (f_angular_2_2)) >> 15));
  int32_t new_f_angular_temp_2_2 = (_tmp706 + (_tmp707 >> 17));
  int32_t _tmp708 = (int32_t) ((((int64_t) (limi_translation_2_0) * (int64_t) (new_f_linear_2_1)) >> 15));
  int32_t _tmp709 = (int32_t) ((((int64_t) (limi_translation_2_1) * (int64_t) (new_f_linear_2_0)) >> 29));
  int32_t f_angular_cross_2_0 = ((_tmp708 >> 15) - _tmp709);
  int32_t _tmp710 = (int32_t) ((((int64_t) (limi_translation_2_2) * (int64_t) (new_f_linear_2_0)) >> 14));
  int32_t _tmp711 = (int32_t) ((((int64_t) (limi_translation_2_0) * (int64_t) (new_f_linear_2_2)) >> 14));
  int32_t f_angular_cross_2_1 = (_tmp710 - _tmp711);
  int32_t _tmp712 = (int32_t) ((((int64_t) (limi_translation_2_0) * (int64_t) (new_f_linear_2_1)) >> 15));
  int32_t _tmp713 = (int32_t) ((((int64_t) (limi_translation_2_1) * (int64_t) (new_f_linear_2_0)) >> 29));
  int32_t f_angular_cross_2_2 = ((_tmp712 >> 15) - _tmp713);
  int32_t new_f_angular_2_0 = (new_f_angular_temp_2_0 + (f_angular_cross_2_0 >> 2));
  int32_t new_f_angular_2_1 = (new_f_angular_temp_2_1 + (f_angular_cross_2_1 >> 16));
  int32_t new_f_angular_2_2 = (new_f_angular_temp_2_2 + (f_angular_cross_2_2 >> 2));
  int32_t r_203_new_f_2_0 = new_f_linear_2_0;
  int32_t r_203_new_f_2_1 = new_f_linear_2_1;
  int32_t r_203_new_f_2_2 = new_f_linear_2_2;
  int32_t r_203_new_f_2_3 = new_f_angular_2_0;
  int32_t r_203_new_f_2_4 = new_f_angular_2_1;
  int32_t r_203_new_f_2_5 = new_f_angular_2_2;
  int32_t new_data_f_parent_2_0 = ((f_rnea_firstpass1_0 >> 14) + r_203_new_f_2_0);
  int32_t new_data_f_parent_2_1 = ((f_rnea_firstpass1_1 >> 13) + r_203_new_f_2_1);
  int32_t new_data_f_parent_2_2 = ((f_rnea_firstpass1_2 >> 16) + r_203_new_f_2_2);
  int32_t new_data_f_parent_2_3 = ((f_rnea_firstpass1_3 >> 15) + r_203_new_f_2_3);
  int32_t new_data_f_parent_2_4 = ((f_rnea_firstpass1_4 >> 14) + r_203_new_f_2_4);
  int32_t new_data_f_parent_2_5 = ((f_rnea_firstpass1_5 >> 15) + r_203_new_f_2_5);
  int32_t data_tau_temp_1 = new_data_f_parent_2_5;
  int32_t f_linear_1_0 = new_data_f_parent_2_0;
  int32_t f_linear_1_1 = new_data_f_parent_2_1;
  int32_t f_linear_1_2 = new_data_f_parent_2_2;
  int32_t f_angular_1_0 = new_data_f_parent_2_3;
  int32_t f_angular_1_1 = new_data_f_parent_2_4;
  int32_t f_angular_1_2 = new_data_f_parent_2_5;
  int32_t _tmp714 = (int32_t) ((((int64_t) (limi_rotation_1_0_0) * (int64_t) (f_linear_1_0)) >> 30));
  int32_t _tmp715 = (int32_t) ((((int64_t) (limi_rotation_1_0_1) * (int64_t) (f_linear_1_1)) >> 30));
  int32_t _tmp716 = ((_tmp714 + (_tmp715 >> 1)) >> 1);
  int32_t _tmp717 = (int32_t) ((((int64_t) (limi_rotation_1_0_2) * (int64_t) (f_linear_1_2)) >> 14));
  int32_t new_f_linear_1_0 = (_tmp716 + (_tmp717 >> 18));
  int32_t _tmp718 = (int32_t) ((((int64_t) (limi_rotation_1_1_0) * (int64_t) (f_linear_1_0)) >> 14));
  int32_t _tmp719 = (int32_t) ((((int64_t) (limi_rotation_1_1_1) * (int64_t) (f_linear_1_1)) >> 15));
  int32_t _tmp720 = (_tmp718 + _tmp719);
  int32_t _tmp721 = (int32_t) ((((int64_t) (limi_rotation_1_1_2) * (int64_t) (f_linear_1_2)) >> 30));
  int32_t new_f_linear_1_1 = ((_tmp720 >> 17) + _tmp721);
  int32_t _tmp722 = (int32_t) ((((int64_t) (limi_rotation_1_2_0) * (int64_t) (f_linear_1_0)) >> 30));
  int32_t _tmp723 = (int32_t) ((((int64_t) (limi_rotation_1_2_1) * (int64_t) (f_linear_1_1)) >> 30));
  int32_t _tmp724 = ((_tmp722 + (_tmp723 >> 1)) >> 1);
  int32_t _tmp725 = (int32_t) ((((int64_t) (limi_rotation_1_2_2) * (int64_t) (f_linear_1_2)) >> 14));
  int32_t new_f_linear_1_2 = (_tmp724 + (_tmp725 >> 18));
  int32_t _tmp726 = (int32_t) ((((int64_t) (limi_rotation_1_0_0) * (int64_t) (f_angular_1_0)) >> 30));
  int32_t _tmp727 = (int32_t) ((((int64_t) (limi_rotation_1_0_1) * (int64_t) (f_angular_1_1)) >> 30));
  int32_t _tmp728 = ((_tmp726 + (_tmp727 >> 1)) >> 1);
  int32_t _tmp729 = (int32_t) ((((int64_t) (limi_rotation_1_0_2) * (int64_t) (f_angular_1_2)) >> 14));
  int32_t new_f_angular_temp_1_0 = (_tmp728 + (_tmp729 >> 18));
  int32_t _tmp730 = (int32_t) ((((int64_t) (limi_rotation_1_1_0) * (int64_t) (f_angular_1_0)) >> 14));
  int32_t _tmp731 = (int32_t) ((((int64_t) (limi_rotation_1_1_1) * (int64_t) (f_angular_1_1)) >> 15));
  int32_t _tmp732 = (_tmp730 + _tmp731);
  int32_t _tmp733 = (int32_t) ((((int64_t) (limi_rotation_1_1_2) * (int64_t) (f_angular_1_2)) >> 30));
  int32_t new_f_angular_temp_1_1 = ((_tmp732 >> 17) + _tmp733);
  int32_t _tmp734 = (int32_t) ((((int64_t) (limi_rotation_1_2_0) * (int64_t) (f_angular_1_0)) >> 30));
  int32_t _tmp735 = (int32_t) ((((int64_t) (limi_rotation_1_2_1) * (int64_t) (f_angular_1_1)) >> 30));
  int32_t _tmp736 = ((_tmp734 + (_tmp735 >> 1)) >> 1);
  int32_t _tmp737 = (int32_t) ((((int64_t) (limi_rotation_1_2_2) * (int64_t) (f_angular_1_2)) >> 14));
  int32_t new_f_angular_temp_1_2 = (_tmp736 + (_tmp737 >> 18));
  int32_t _tmp738 = (int32_t) ((((int64_t) (limi_translation_1_0) * (int64_t) (new_f_linear_1_1)) >> 14));
  int32_t _tmp739 = (int32_t) ((((int64_t) (limi_translation_1_1) * (int64_t) (new_f_linear_1_0)) >> 13));
  int32_t f_angular_cross_1_0 = (_tmp738 - _tmp739);
  int32_t _tmp740 = (int32_t) ((((int64_t) (limi_translation_1_2) * (int64_t) (new_f_linear_1_0)) >> 13));
  int32_t _tmp741 = (int32_t) ((((int64_t) (limi_translation_1_0) * (int64_t) (new_f_linear_1_2)) >> 13));
  int32_t f_angular_cross_1_1 = (_tmp740 - _tmp741);
  int32_t _tmp742 = (int32_t) ((((int64_t) (limi_translation_1_0) * (int64_t) (new_f_linear_1_1)) >> 14));
  int32_t _tmp743 = (int32_t) ((((int64_t) (limi_translation_1_1) * (int64_t) (new_f_linear_1_0)) >> 13));
  int32_t f_angular_cross_1_2 = (_tmp742 - _tmp743);
  int32_t new_f_angular_1_0 = (new_f_angular_temp_1_0 + (f_angular_cross_1_0 >> 18));
  int32_t new_f_angular_1_1 = (new_f_angular_temp_1_1 + (f_angular_cross_1_1 >> 17));
  int32_t new_f_angular_1_2 = (new_f_angular_temp_1_2 + (f_angular_cross_1_2 >> 18));
  int32_t r_204_new_f_1_0 = new_f_linear_1_0;
  int32_t r_204_new_f_1_1 = new_f_linear_1_1;
  int32_t r_204_new_f_1_2 = new_f_linear_1_2;
  int32_t r_204_new_f_1_3 = new_f_angular_1_0;
  int32_t r_204_new_f_1_4 = new_f_angular_1_1;
  int32_t r_204_new_f_1_5 = new_f_angular_1_2;
  int32_t new_data_f_parent_1_0 = ((f_rnea_firstpass0_0 >> 18) + r_204_new_f_1_0);
  int32_t new_data_f_parent_1_1 = ((f_rnea_firstpass0_1 >> 17) + r_204_new_f_1_1);
  int32_t new_data_f_parent_1_2 = ((f_rnea_firstpass0_2 >> 12) + r_204_new_f_1_2);
  int32_t new_data_f_parent_1_3 = ((f_rnea_firstpass0_3 >> 18) + r_204_new_f_1_3);
  int32_t new_data_f_parent_1_4 = ((f_rnea_firstpass0_4 >> 17) + r_204_new_f_1_4);
  int32_t new_data_f_parent_1_5 = ((f_rnea_firstpass0_5 >> 18) + r_204_new_f_1_5);
  int32_t data_tau_temp_0 = new_data_f_parent_1_5;
  int32_t data_tau_0 = data_tau_temp_0;
  int32_t data_tau_1 = data_tau_temp_1;
  int32_t data_tau_2 = data_tau_temp_2;
  int32_t data_tau_3 = data_tau_temp_3;
  int32_t data_tau_4 = data_tau_temp_4;
  int32_t data_tau_5 = data_tau_temp_5;

  return {data_tau_0, data_tau_1, data_tau_2, data_tau_3, data_tau_4, data_tau_5};
}