text: /* Produced by CVXGEN, 2019-04-30 09:43:33 -0400.  */
  /* CVXGEN is Copyright (C) 2006-2017 Jacob Mattingley, jem@cvxgen.com. */
  /* The code in this file is Copyright (C) 2006-2017 Jacob Mattingley. */
  /* CVXGEN, or solvers produced by CVXGEN, cannot be used for commercial */
  /* applications without prior written permission from Jacob Mattingley. */
  
  /* Filename: matrix_support.c. */
  /* Description: Support functions for matrix multiplication and vector filling. */
  #include "IKCG_solver.h"
  void IKCG_multbymA(double *lhs, double *rhs) {
    lhs[0] = -rhs[0]*(-(1-IKCG_params.hard[0]))-rhs[36]*(IKCG_params.J_1[0])-rhs[37]*(IKCG_params.J_1[1])-rhs[38]*(IKCG_params.J_1[2])-rhs[39]*(IKCG_params.J_1[3])-rhs[40]*(IKCG_params.J_1[4])-rhs[41]*(IKCG_params.J_1[5])-rhs[42]*(IKCG_params.J_1[6])-rhs[43]*(IKCG_params.J_1[7])-rhs[44]*(IKCG_params.J_1[8])-rhs[45]*(IKCG_params.J_1[9])-rhs[46]*(IKCG_params.J_1[10])-rhs[47]*(IKCG_params.J_1[11])-rhs[48]*(IKCG_params.J_1[12])-rhs[49]*(IKCG_params.J_1[13])-rhs[50]*(IKCG_params.J_1[14])-rhs[51]*(IKCG_params.J_1[15])-rhs[52]*(IKCG_params.J_1[16])-rhs[53]*(IKCG_params.J_1[17])-rhs[54]*(IKCG_params.J_1[18])-rhs[55]*(IKCG_params.J_1[19])-rhs[56]*(IKCG_params.J_1[20])-rhs[57]*(IKCG_params.J_1[21])-rhs[58]*(IKCG_params.J_1[22])-rhs[59]*(IKCG_params.J_1[23])-rhs[60]*(IKCG_params.J_1[24])-rhs[61]*(IKCG_params.J_1[25])-rhs[62]*(IKCG_params.J_1[26])-rhs[63]*(IKCG_params.J_1[27])-rhs[64]*(IKCG_params.J_1[28])-rhs[65]*(IKCG_params.J_1[29])-rhs[66]*(IKCG_params.J_1[30])-rhs[67]*(IKCG_params.J_1[31])-rhs[68]*(IKCG_params.J_1[32])-rhs[69]*(IKCG_params.J_1[33])-rhs[70]*(IKCG_params.J_1[34])-rhs[71]*(IKCG_params.J_1[35])-rhs[72]*(IKCG_params.J_1[36])-rhs[73]*(IKCG_params.J_1[37]);
    lhs[1] = -rhs[1]*(-(1-IKCG_params.hard[1]))-rhs[36]*(IKCG_params.J_2[0])-rhs[37]*(IKCG_params.J_2[1])-rhs[38]*(IKCG_params.J_2[2])-rhs[39]*(IKCG_params.J_2[3])-rhs[40]*(IKCG_params.J_2[4])-rhs[41]*(IKCG_params.J_2[5])-rhs[42]*(IKCG_params.J_2[6])-rhs[43]*(IKCG_params.J_2[7])-rhs[44]*(IKCG_params.J_2[8])-rhs[45]*(IKCG_params.J_2[9])-rhs[46]*(IKCG_params.J_2[10])-rhs[47]*(IKCG_params.J_2[11])-rhs[48]*(IKCG_params.J_2[12])-rhs[49]*(IKCG_params.J_2[13])-rhs[50]*(IKCG_params.J_2[14])-rhs[51]*(IKCG_params.J_2[15])-rhs[52]*(IKCG_params.J_2[16])-rhs[53]*(IKCG_params.J_2[17])-rhs[54]*(IKCG_params.J_2[18])-rhs[55]*(IKCG_params.J_2[19])-rhs[56]*(IKCG_params.J_2[20])-rhs[57]*(IKCG_params.J_2[21])-rhs[58]*(IKCG_params.J_2[22])-rhs[59]*(IKCG_params.J_2[23])-rhs[60]*(IKCG_params.J_2[24])-rhs[61]*(IKCG_params.J_2[25])-rhs[62]*(IKCG_params.J_2[26])-rhs[63]*(IKCG_params.J_2[27])-rhs[64]*(IKCG_params.J_2[28])-rhs[65]*(IKCG_params.J_2[29])-rhs[66]*(IKCG_params.J_2[30])-rhs[67]*(IKCG_params.J_2[31])-rhs[68]*(IKCG_params.J_2[32])-rhs[69]*(IKCG_params.J_2[33])-rhs[70]*(IKCG_params.J_2[34])-rhs[71]*(IKCG_params.J_2[35])-rhs[72]*(IKCG_params.J_2[36])-rhs[73]*(IKCG_params.J_2[37]);
    lhs[2] = -rhs[2]*(-(1-IKCG_params.hard[2]))-rhs[36]*(IKCG_params.J_3[0])-rhs[37]*(IKCG_params.J_3[1])-rhs[38]*(IKCG_params.J_3[2])-rhs[39]*(IKCG_params.J_3[3])-rhs[40]*(IKCG_params.J_3[4])-rhs[41]*(IKCG_params.J_3[5])-rhs[42]*(IKCG_params.J_3[6])-rhs[43]*(IKCG_params.J_3[7])-rhs[44]*(IKCG_params.J_3[8])-rhs[45]*(IKCG_params.J_3[9])-rhs[46]*(IKCG_params.J_3[10])-rhs[47]*(IKCG_params.J_3[11])-rhs[48]*(IKCG_params.J_3[12])-rhs[49]*(IKCG_params.J_3[13])-rhs[50]*(IKCG_params.J_3[14])-rhs[51]*(IKCG_params.J_3[15])-rhs[52]*(IKCG_params.J_3[16])-rhs[53]*(IKCG_params.J_3[17])-rhs[54]*(IKCG_params.J_3[18])-rhs[55]*(IKCG_params.J_3[19])-rhs[56]*(IKCG_params.J_3[20])-rhs[57]*(IKCG_params.J_3[21])-rhs[58]*(IKCG_params.J_3[22])-rhs[59]*(IKCG_params.J_3[23])-rhs[60]*(IKCG_params.J_3[24])-rhs[61]*(IKCG_params.J_3[25])-rhs[62]*(IKCG_params.J_3[26])-rhs[63]*(IKCG_params.J_3[27])-rhs[64]*(IKCG_params.J_3[28])-rhs[65]*(IKCG_params.J_3[29])-rhs[66]*(IKCG_params.J_3[30])-rhs[67]*(IKCG_params.J_3[31])-rhs[68]*(IKCG_params.J_3[32])-rhs[69]*(IKCG_params.J_3[33])-rhs[70]*(IKCG_params.J_3[34])-rhs[71]*(IKCG_params.J_3[35])-rhs[72]*(IKCG_params.J_3[36])-rhs[73]*(IKCG_params.J_3[37]);
    lhs[3] = -rhs[3]*(-(1-IKCG_params.hard[3]))-rhs[39]*(IKCG_params.J_4[3])-rhs[40]*(IKCG_params.J_4[4])-rhs[41]*(IKCG_params.J_4[5]);
    lhs[4] = -rhs[4]*(-(1-IKCG_params.hard[4]))-rhs[39]*(IKCG_params.J_5[3])-rhs[40]*(IKCG_params.J_5[4])-rhs[41]*(IKCG_params.J_5[5]);
    lhs[5] = -rhs[5]*(-(1-IKCG_params.hard[5]))-rhs[39]*(IKCG_params.J_6[3])-rhs[40]*(IKCG_params.J_6[4])-rhs[41]*(IKCG_params.J_6[5]);
    lhs[6] = -rhs[6]*(-(1-IKCG_params.hard[6]))-rhs[36]*(IKCG_params.J_7[0])-rhs[37]*(IKCG_params.J_7[1])-rhs[38]*(IKCG_params.J_7[2])-rhs[39]*(IKCG_params.J_7[3])-rhs[40]*(IKCG_params.J_7[4])-rhs[41]*(IKCG_params.J_7[5])-rhs[48]*(IKCG_params.J_7[12])-rhs[49]*(IKCG_params.J_7[13])-rhs[50]*(IKCG_params.J_7[14])-rhs[51]*(IKCG_params.J_7[15])-rhs[52]*(IKCG_params.J_7[16])-rhs[53]*(IKCG_params.J_7[17]);
    lhs[7] = -rhs[7]*(-(1-IKCG_params.hard[7]))-rhs[36]*(IKCG_params.J_8[0])-rhs[37]*(IKCG_params.J_8[1])-rhs[38]*(IKCG_params.J_8[2])-rhs[39]*(IKCG_params.J_8[3])-rhs[40]*(IKCG_params.J_8[4])-rhs[41]*(IKCG_params.J_8[5])-rhs[48]*(IKCG_params.J_8[12])-rhs[49]*(IKCG_params.J_8[13])-rhs[50]*(IKCG_params.J_8[14])-rhs[51]*(IKCG_params.J_8[15])-rhs[52]*(IKCG_params.J_8[16])-rhs[53]*(IKCG_params.J_8[17]);
    lhs[8] = -rhs[8]*(-(1-IKCG_params.hard[8]))-rhs[36]*(IKCG_params.J_9[0])-rhs[37]*(IKCG_params.J_9[1])-rhs[38]*(IKCG_params.J_9[2])-rhs[39]*(IKCG_params.J_9[3])-rhs[40]*(IKCG_params.J_9[4])-rhs[41]*(IKCG_params.J_9[5])-rhs[48]*(IKCG_params.J_9[12])-rhs[49]*(IKCG_params.J_9[13])-rhs[50]*(IKCG_params.J_9[14])-rhs[51]*(IKCG_params.J_9[15])-rhs[52]*(IKCG_params.J_9[16])-rhs[53]*(IKCG_params.J_9[17]);
    lhs[9] = -rhs[9]*(-(1-IKCG_params.hard[9]))-rhs[39]*(IKCG_params.J_10[3])-rhs[40]*(IKCG_params.J_10[4])-rhs[41]*(IKCG_params.J_10[5])-rhs[48]*(IKCG_params.J_10[12])-rhs[49]*(IKCG_params.J_10[13])-rhs[50]*(IKCG_params.J_10[14])-rhs[51]*(IKCG_params.J_10[15])-rhs[52]*(IKCG_params.J_10[16])-rhs[53]*(IKCG_params.J_10[17]);
    lhs[10] = -rhs[10]*(-(1-IKCG_params.hard[10]))-rhs[39]*(IKCG_params.J_11[3])-rhs[40]*(IKCG_params.J_11[4])-rhs[41]*(IKCG_params.J_11[5])-rhs[48]*(IKCG_params.J_11[12])-rhs[49]*(IKCG_params.J_11[13])-rhs[50]*(IKCG_params.J_11[14])-rhs[51]*(IKCG_params.J_11[15])-rhs[52]*(IKCG_params.J_11[16])-rhs[53]*(IKCG_params.J_11[17]);
    lhs[11] = -rhs[11]*(-(1-IKCG_params.hard[11]))-rhs[39]*(IKCG_params.J_12[3])-rhs[40]*(IKCG_params.J_12[4])-rhs[41]*(IKCG_params.J_12[5])-rhs[48]*(IKCG_params.J_12[12])-rhs[49]*(IKCG_params.J_12[13])-rhs[50]*(IKCG_params.J_12[14])-rhs[51]*(IKCG_params.J_12[15])-rhs[52]*(IKCG_params.J_12[16])-rhs[53]*(IKCG_params.J_12[17]);
    lhs[12] = -rhs[12]*(-(1-IKCG_params.hard[12]))-rhs[36]*(IKCG_params.J_13[0])-rhs[37]*(IKCG_params.J_13[1])-rhs[38]*(IKCG_params.J_13[2])-rhs[39]*(IKCG_params.J_13[3])-rhs[40]*(IKCG_params.J_13[4])-rhs[41]*(IKCG_params.J_13[5])-rhs[54]*(IKCG_params.J_13[18])-rhs[55]*(IKCG_params.J_13[19])-rhs[56]*(IKCG_params.J_13[20])-rhs[57]*(IKCG_params.J_13[21])-rhs[58]*(IKCG_params.J_13[22])-rhs[59]*(IKCG_params.J_13[23]);
    lhs[13] = -rhs[13]*(-(1-IKCG_params.hard[13]))-rhs[36]*(IKCG_params.J_14[0])-rhs[37]*(IKCG_params.J_14[1])-rhs[38]*(IKCG_params.J_14[2])-rhs[39]*(IKCG_params.J_14[3])-rhs[40]*(IKCG_params.J_14[4])-rhs[41]*(IKCG_params.J_14[5])-rhs[54]*(IKCG_params.J_14[18])-rhs[55]*(IKCG_params.J_14[19])-rhs[56]*(IKCG_params.J_14[20])-rhs[57]*(IKCG_params.J_14[21])-rhs[58]*(IKCG_params.J_14[22])-rhs[59]*(IKCG_params.J_14[23]);
    lhs[14] = -rhs[14]*(-(1-IKCG_params.hard[14]))-rhs[36]*(IKCG_params.J_15[0])-rhs[37]*(IKCG_params.J_15[1])-rhs[38]*(IKCG_params.J_15[2])-rhs[39]*(IKCG_params.J_15[3])-rhs[40]*(IKCG_params.J_15[4])-rhs[41]*(IKCG_params.J_15[5])-rhs[54]*(IKCG_params.J_15[18])-rhs[55]*(IKCG_params.J_15[19])-rhs[56]*(IKCG_params.J_15[20])-rhs[57]*(IKCG_params.J_15[21])-rhs[58]*(IKCG_params.J_15[22])-rhs[59]*(IKCG_params.J_15[23]);
    lhs[15] = -rhs[15]*(-(1-IKCG_params.hard[15]))-rhs[39]*(IKCG_params.J_16[3])-rhs[40]*(IKCG_params.J_16[4])-rhs[41]*(IKCG_params.J_16[5])-rhs[54]*(IKCG_params.J_16[18])-rhs[55]*(IKCG_params.J_16[19])-rhs[56]*(IKCG_params.J_16[20])-rhs[57]*(IKCG_params.J_16[21])-rhs[58]*(IKCG_params.J_16[22])-rhs[59]*(IKCG_params.J_16[23]);
    lhs[16] = -rhs[16]*(-(1-IKCG_params.hard[16]))-rhs[39]*(IKCG_params.J_17[3])-rhs[40]*(IKCG_params.J_17[4])-rhs[41]*(IKCG_params.J_17[5])-rhs[54]*(IKCG_params.J_17[18])-rhs[55]*(IKCG_params.J_17[19])-rhs[56]*(IKCG_params.J_17[20])-rhs[57]*(IKCG_params.J_17[21])-rhs[58]*(IKCG_params.J_17[22])-rhs[59]*(IKCG_params.J_17[23]);
    lhs[17] = -rhs[17]*(-(1-IKCG_params.hard[17]))-rhs[39]*(IKCG_params.J_18[3])-rhs[40]*(IKCG_params.J_18[4])-rhs[41]*(IKCG_params.J_18[5])-rhs[54]*(IKCG_params.J_18[18])-rhs[55]*(IKCG_params.J_18[19])-rhs[56]*(IKCG_params.J_18[20])-rhs[57]*(IKCG_params.J_18[21])-rhs[58]*(IKCG_params.J_18[22])-rhs[59]*(IKCG_params.J_18[23]);
    lhs[18] = -rhs[18]*(-(1-IKCG_params.hard[18]))-rhs[36]*(IKCG_params.J_19[0])-rhs[37]*(IKCG_params.J_19[1])-rhs[38]*(IKCG_params.J_19[2])-rhs[39]*(IKCG_params.J_19[3])-rhs[40]*(IKCG_params.J_19[4])-rhs[41]*(IKCG_params.J_19[5])-rhs[42]*(IKCG_params.J_19[6])-rhs[43]*(IKCG_params.J_19[7])-rhs[44]*(IKCG_params.J_19[8])-rhs[60]*(IKCG_params.J_19[24])-rhs[61]*(IKCG_params.J_19[25])-rhs[62]*(IKCG_params.J_19[26])-rhs[63]*(IKCG_params.J_19[27])-rhs[64]*(IKCG_params.J_19[28])-rhs[65]*(IKCG_params.J_19[29])-rhs[66]*(IKCG_params.J_19[30]);
    lhs[19] = -rhs[19]*(-(1-IKCG_params.hard[19]))-rhs[36]*(IKCG_params.J_20[0])-rhs[37]*(IKCG_params.J_20[1])-rhs[38]*(IKCG_params.J_20[2])-rhs[39]*(IKCG_params.J_20[3])-rhs[40]*(IKCG_params.J_20[4])-rhs[41]*(IKCG_params.J_20[5])-rhs[42]*(IKCG_params.J_20[6])-rhs[43]*(IKCG_params.J_20[7])-rhs[44]*(IKCG_params.J_20[8])-rhs[60]*(IKCG_params.J_20[24])-rhs[61]*(IKCG_params.J_20[25])-rhs[62]*(IKCG_params.J_20[26])-rhs[63]*(IKCG_params.J_20[27])-rhs[64]*(IKCG_params.J_20[28])-rhs[65]*(IKCG_params.J_20[29])-rhs[66]*(IKCG_params.J_20[30]);
    lhs[20] = -rhs[20]*(-(1-IKCG_params.hard[20]))-rhs[36]*(IKCG_params.J_21[0])-rhs[37]*(IKCG_params.J_21[1])-rhs[38]*(IKCG_params.J_21[2])-rhs[39]*(IKCG_params.J_21[3])-rhs[40]*(IKCG_params.J_21[4])-rhs[41]*(IKCG_params.J_21[5])-rhs[42]*(IKCG_params.J_21[6])-rhs[43]*(IKCG_params.J_21[7])-rhs[44]*(IKCG_params.J_21[8])-rhs[60]*(IKCG_params.J_21[24])-rhs[61]*(IKCG_params.J_21[25])-rhs[62]*(IKCG_params.J_21[26])-rhs[63]*(IKCG_params.J_21[27])-rhs[64]*(IKCG_params.J_21[28])-rhs[65]*(IKCG_params.J_21[29])-rhs[66]*(IKCG_params.J_21[30]);
    lhs[21] = -rhs[21]*(-(1-IKCG_params.hard[21]))-rhs[39]*(IKCG_params.J_22[3])-rhs[40]*(IKCG_params.J_22[4])-rhs[41]*(IKCG_params.J_22[5])-rhs[42]*(IKCG_params.J_22[6])-rhs[43]*(IKCG_params.J_22[7])-rhs[44]*(IKCG_params.J_22[8])-rhs[60]*(IKCG_params.J_22[24])-rhs[61]*(IKCG_params.J_22[25])-rhs[62]*(IKCG_params.J_22[26])-rhs[63]*(IKCG_params.J_22[27])-rhs[64]*(IKCG_params.J_22[28])-rhs[65]*(IKCG_params.J_22[29])-rhs[66]*(IKCG_params.J_22[30]);
    lhs[22] = -rhs[22]*(-(1-IKCG_params.hard[22]))-rhs[39]*(IKCG_params.J_23[3])-rhs[40]*(IKCG_params.J_23[4])-rhs[41]*(IKCG_params.J_23[5])-rhs[42]*(IKCG_params.J_23[6])-rhs[43]*(IKCG_params.J_23[7])-rhs[44]*(IKCG_params.J_23[8])-rhs[60]*(IKCG_params.J_23[24])-rhs[61]*(IKCG_params.J_23[25])-rhs[62]*(IKCG_params.J_23[26])-rhs[63]*(IKCG_params.J_23[27])-rhs[64]*(IKCG_params.J_23[28])-rhs[65]*(IKCG_params.J_23[29])-rhs[66]*(IKCG_params.J_23[30]);
    lhs[23] = -rhs[23]*(-(1-IKCG_params.hard[23]))-rhs[39]*(IKCG_params.J_24[3])-rhs[40]*(IKCG_params.J_24[4])-rhs[41]*(IKCG_params.J_24[5])-rhs[42]*(IKCG_params.J_24[6])-rhs[43]*(IKCG_params.J_24[7])-rhs[44]*(IKCG_params.J_24[8])-rhs[60]*(IKCG_params.J_24[24])-rhs[61]*(IKCG_params.J_24[25])-rhs[62]*(IKCG_params.J_24[26])-rhs[63]*(IKCG_params.J_24[27])-rhs[64]*(IKCG_params.J_24[28])-rhs[65]*(IKCG_params.J_24[29])-rhs[66]*(IKCG_params.J_24[30]);
    lhs[24] = -rhs[24]*(-(1-IKCG_params.hard[24]))-rhs[36]*(IKCG_params.J_25[0])-rhs[37]*(IKCG_params.J_25[1])-rhs[38]*(IKCG_params.J_25[2])-rhs[39]*(IKCG_params.J_25[3])-rhs[40]*(IKCG_params.J_25[4])-rhs[41]*(IKCG_params.J_25[5])-rhs[42]*(IKCG_params.J_25[6])-rhs[43]*(IKCG_params.J_25[7])-rhs[44]*(IKCG_params.J_25[8])-rhs[67]*(IKCG_params.J_25[31])-rhs[68]*(IKCG_params.J_25[32])-rhs[69]*(IKCG_params.J_25[33])-rhs[70]*(IKCG_params.J_25[34])-rhs[71]*(IKCG_params.J_25[35])-rhs[72]*(IKCG_params.J_25[36])-rhs[73]*(IKCG_params.J_25[37]);
    lhs[25] = -rhs[25]*(-(1-IKCG_params.hard[25]))-rhs[36]*(IKCG_params.J_26[0])-rhs[37]*(IKCG_params.J_26[1])-rhs[38]*(IKCG_params.J_26[2])-rhs[39]*(IKCG_params.J_26[3])-rhs[40]*(IKCG_params.J_26[4])-rhs[41]*(IKCG_params.J_26[5])-rhs[42]*(IKCG_params.J_26[6])-rhs[43]*(IKCG_params.J_26[7])-rhs[44]*(IKCG_params.J_26[8])-rhs[67]*(IKCG_params.J_26[31])-rhs[68]*(IKCG_params.J_26[32])-rhs[69]*(IKCG_params.J_26[33])-rhs[70]*(IKCG_params.J_26[34])-rhs[71]*(IKCG_params.J_26[35])-rhs[72]*(IKCG_params.J_26[36])-rhs[73]*(IKCG_params.J_26[37]);
    lhs[26] = -rhs[26]*(-(1-IKCG_params.hard[26]))-rhs[36]*(IKCG_params.J_27[0])-rhs[37]*(IKCG_params.J_27[1])-rhs[38]*(IKCG_params.J_27[2])-rhs[39]*(IKCG_params.J_27[3])-rhs[40]*(IKCG_params.J_27[4])-rhs[41]*(IKCG_params.J_27[5])-rhs[42]*(IKCG_params.J_27[6])-rhs[43]*(IKCG_params.J_27[7])-rhs[44]*(IKCG_params.J_27[8])-rhs[67]*(IKCG_params.J_27[31])-rhs[68]*(IKCG_params.J_27[32])-rhs[69]*(IKCG_params.J_27[33])-rhs[70]*(IKCG_params.J_27[34])-rhs[71]*(IKCG_params.J_27[35])-rhs[72]*(IKCG_params.J_27[36])-rhs[73]*(IKCG_params.J_27[37]);
    lhs[27] = -rhs[27]*(-(1-IKCG_params.hard[27]))-rhs[39]*(IKCG_params.J_28[3])-rhs[40]*(IKCG_params.J_28[4])-rhs[41]*(IKCG_params.J_28[5])-rhs[42]*(IKCG_params.J_28[6])-rhs[43]*(IKCG_params.J_28[7])-rhs[44]*(IKCG_params.J_28[8])-rhs[67]*(IKCG_params.J_28[31])-rhs[68]*(IKCG_params.J_28[32])-rhs[69]*(IKCG_params.J_28[33])-rhs[70]*(IKCG_params.J_28[34])-rhs[71]*(IKCG_params.J_28[35])-rhs[72]*(IKCG_params.J_28[36])-rhs[73]*(IKCG_params.J_28[37]);
    lhs[28] = -rhs[28]*(-(1-IKCG_params.hard[28]))-rhs[39]*(IKCG_params.J_29[3])-rhs[40]*(IKCG_params.J_29[4])-rhs[41]*(IKCG_params.J_29[5])-rhs[42]*(IKCG_params.J_29[6])-rhs[43]*(IKCG_params.J_29[7])-rhs[44]*(IKCG_params.J_29[8])-rhs[67]*(IKCG_params.J_29[31])-rhs[68]*(IKCG_params.J_29[32])-rhs[69]*(IKCG_params.J_29[33])-rhs[70]*(IKCG_params.J_29[34])-rhs[71]*(IKCG_params.J_29[35])-rhs[72]*(IKCG_params.J_29[36])-rhs[73]*(IKCG_params.J_29[37]);
    lhs[29] = -rhs[29]*(-(1-IKCG_params.hard[29]))-rhs[39]*(IKCG_params.J_30[3])-rhs[40]*(IKCG_params.J_30[4])-rhs[41]*(IKCG_params.J_30[5])-rhs[42]*(IKCG_params.J_30[6])-rhs[43]*(IKCG_params.J_30[7])-rhs[44]*(IKCG_params.J_30[8])-rhs[67]*(IKCG_params.J_30[31])-rhs[68]*(IKCG_params.J_30[32])-rhs[69]*(IKCG_params.J_30[33])-rhs[70]*(IKCG_params.J_30[34])-rhs[71]*(IKCG_params.J_30[35])-rhs[72]*(IKCG_params.J_30[36])-rhs[73]*(IKCG_params.J_30[37]);
    lhs[30] = -rhs[30]*(-(1-IKCG_params.hard[30]))-rhs[39]*(IKCG_params.J_31[3])-rhs[40]*(IKCG_params.J_31[4])-rhs[41]*(IKCG_params.J_31[5])-rhs[42]*(IKCG_params.J_31[6])-rhs[43]*(IKCG_params.J_31[7])-rhs[44]*(IKCG_params.J_31[8]);
    lhs[31] = -rhs[31]*(-(1-IKCG_params.hard[31]))-rhs[39]*(IKCG_params.J_32[3])-rhs[40]*(IKCG_params.J_32[4])-rhs[41]*(IKCG_params.J_32[5])-rhs[42]*(IKCG_params.J_32[6])-rhs[43]*(IKCG_params.J_32[7])-rhs[44]*(IKCG_params.J_32[8]);
    lhs[32] = -rhs[32]*(-(1-IKCG_params.hard[32]))-rhs[39]*(IKCG_params.J_33[3])-rhs[40]*(IKCG_params.J_33[4])-rhs[41]*(IKCG_params.J_33[5])-rhs[42]*(IKCG_params.J_33[6])-rhs[43]*(IKCG_params.J_33[7])-rhs[44]*(IKCG_params.J_33[8]);
    lhs[33] = -rhs[33]*(-(1-IKCG_params.hard[33]))-rhs[39]*(IKCG_params.J_34[3])-rhs[40]*(IKCG_params.J_34[4])-rhs[41]*(IKCG_params.J_34[5])-rhs[42]*(IKCG_params.J_34[6])-rhs[43]*(IKCG_params.J_34[7])-rhs[44]*(IKCG_params.J_34[8])-rhs[45]*(IKCG_params.J_34[9])-rhs[46]*(IKCG_params.J_34[10])-rhs[47]*(IKCG_params.J_34[11]);
    lhs[34] = -rhs[34]*(-(1-IKCG_params.hard[34]))-rhs[39]*(IKCG_params.J_35[3])-rhs[40]*(IKCG_params.J_35[4])-rhs[41]*(IKCG_params.J_35[5])-rhs[42]*(IKCG_params.J_35[6])-rhs[43]*(IKCG_params.J_35[7])-rhs[44]*(IKCG_params.J_35[8])-rhs[45]*(IKCG_params.J_35[9])-rhs[46]*(IKCG_params.J_35[10])-rhs[47]*(IKCG_params.J_35[11]);
    lhs[35] = -rhs[35]*(-(1-IKCG_params.hard[35]))-rhs[39]*(IKCG_params.J_36[3])-rhs[40]*(IKCG_params.J_36[4])-rhs[41]*(IKCG_params.J_36[5])-rhs[42]*(IKCG_params.J_36[6])-rhs[43]*(IKCG_params.J_36[7])-rhs[44]*(IKCG_params.J_36[8])-rhs[45]*(IKCG_params.J_36[9])-rhs[46]*(IKCG_params.J_36[10])-rhs[47]*(IKCG_params.J_36[11]);
  }
  void IKCG_multbymAT(double *lhs, double *rhs) {
    lhs[0] = -rhs[0]*(-(1-IKCG_params.hard[0]));
    lhs[1] = -rhs[1]*(-(1-IKCG_params.hard[1]));
    lhs[2] = -rhs[2]*(-(1-IKCG_params.hard[2]));
    lhs[3] = -rhs[3]*(-(1-IKCG_params.hard[3]));
    lhs[4] = -rhs[4]*(-(1-IKCG_params.hard[4]));
    lhs[5] = -rhs[5]*(-(1-IKCG_params.hard[5]));
    lhs[6] = -rhs[6]*(-(1-IKCG_params.hard[6]));
    lhs[7] = -rhs[7]*(-(1-IKCG_params.hard[7]));
    lhs[8] = -rhs[8]*(-(1-IKCG_params.hard[8]));
    lhs[9] = -rhs[9]*(-(1-IKCG_params.hard[9]));
    lhs[10] = -rhs[10]*(-(1-IKCG_params.hard[10]));
    lhs[11] = -rhs[11]*(-(1-IKCG_params.hard[11]));
    lhs[12] = -rhs[12]*(-(1-IKCG_params.hard[12]));
    lhs[13] = -rhs[13]*(-(1-IKCG_params.hard[13]));
    lhs[14] = -rhs[14]*(-(1-IKCG_params.hard[14]));
    lhs[15] = -rhs[15]*(-(1-IKCG_params.hard[15]));
    lhs[16] = -rhs[16]*(-(1-IKCG_params.hard[16]));
    lhs[17] = -rhs[17]*(-(1-IKCG_params.hard[17]));
    lhs[18] = -rhs[18]*(-(1-IKCG_params.hard[18]));
    lhs[19] = -rhs[19]*(-(1-IKCG_params.hard[19]));
    lhs[20] = -rhs[20]*(-(1-IKCG_params.hard[20]));
    lhs[21] = -rhs[21]*(-(1-IKCG_params.hard[21]));
    lhs[22] = -rhs[22]*(-(1-IKCG_params.hard[22]));
    lhs[23] = -rhs[23]*(-(1-IKCG_params.hard[23]));
    lhs[24] = -rhs[24]*(-(1-IKCG_params.hard[24]));
    lhs[25] = -rhs[25]*(-(1-IKCG_params.hard[25]));
    lhs[26] = -rhs[26]*(-(1-IKCG_params.hard[26]));
    lhs[27] = -rhs[27]*(-(1-IKCG_params.hard[27]));
    lhs[28] = -rhs[28]*(-(1-IKCG_params.hard[28]));
    lhs[29] = -rhs[29]*(-(1-IKCG_params.hard[29]));
    lhs[30] = -rhs[30]*(-(1-IKCG_params.hard[30]));
    lhs[31] = -rhs[31]*(-(1-IKCG_params.hard[31]));
    lhs[32] = -rhs[32]*(-(1-IKCG_params.hard[32]));
    lhs[33] = -rhs[33]*(-(1-IKCG_params.hard[33]));
    lhs[34] = -rhs[34]*(-(1-IKCG_params.hard[34]));
    lhs[35] = -rhs[35]*(-(1-IKCG_params.hard[35]));
    lhs[36] = -rhs[0]*(IKCG_params.J_1[0])-rhs[1]*(IKCG_params.J_2[0])-rhs[2]*(IKCG_params.J_3[0])-rhs[6]*(IKCG_params.J_7[0])-rhs[7]*(IKCG_params.J_8[0])-rhs[8]*(IKCG_params.J_9[0])-rhs[12]*(IKCG_params.J_13[0])-rhs[13]*(IKCG_params.J_14[0])-rhs[14]*(IKCG_params.J_15[0])-rhs[18]*(IKCG_params.J_19[0])-rhs[19]*(IKCG_params.J_20[0])-rhs[20]*(IKCG_params.J_21[0])-rhs[24]*(IKCG_params.J_25[0])-rhs[25]*(IKCG_params.J_26[0])-rhs[26]*(IKCG_params.J_27[0]);
    lhs[37] = -rhs[0]*(IKCG_params.J_1[1])-rhs[1]*(IKCG_params.J_2[1])-rhs[2]*(IKCG_params.J_3[1])-rhs[6]*(IKCG_params.J_7[1])-rhs[7]*(IKCG_params.J_8[1])-rhs[8]*(IKCG_params.J_9[1])-rhs[12]*(IKCG_params.J_13[1])-rhs[13]*(IKCG_params.J_14[1])-rhs[14]*(IKCG_params.J_15[1])-rhs[18]*(IKCG_params.J_19[1])-rhs[19]*(IKCG_params.J_20[1])-rhs[20]*(IKCG_params.J_21[1])-rhs[24]*(IKCG_params.J_25[1])-rhs[25]*(IKCG_params.J_26[1])-rhs[26]*(IKCG_params.J_27[1]);
    lhs[38] = -rhs[0]*(IKCG_params.J_1[2])-rhs[1]*(IKCG_params.J_2[2])-rhs[2]*(IKCG_params.J_3[2])-rhs[6]*(IKCG_params.J_7[2])-rhs[7]*(IKCG_params.J_8[2])-rhs[8]*(IKCG_params.J_9[2])-rhs[12]*(IKCG_params.J_13[2])-rhs[13]*(IKCG_params.J_14[2])-rhs[14]*(IKCG_params.J_15[2])-rhs[18]*(IKCG_params.J_19[2])-rhs[19]*(IKCG_params.J_20[2])-rhs[20]*(IKCG_params.J_21[2])-rhs[24]*(IKCG_params.J_25[2])-rhs[25]*(IKCG_params.J_26[2])-rhs[26]*(IKCG_params.J_27[2]);
    lhs[39] = -rhs[0]*(IKCG_params.J_1[3])-rhs[1]*(IKCG_params.J_2[3])-rhs[2]*(IKCG_params.J_3[3])-rhs[3]*(IKCG_params.J_4[3])-rhs[4]*(IKCG_params.J_5[3])-rhs[5]*(IKCG_params.J_6[3])-rhs[6]*(IKCG_params.J_7[3])-rhs[7]*(IKCG_params.J_8[3])-rhs[8]*(IKCG_params.J_9[3])-rhs[9]*(IKCG_params.J_10[3])-rhs[10]*(IKCG_params.J_11[3])-rhs[11]*(IKCG_params.J_12[3])-rhs[12]*(IKCG_params.J_13[3])-rhs[13]*(IKCG_params.J_14[3])-rhs[14]*(IKCG_params.J_15[3])-rhs[15]*(IKCG_params.J_16[3])-rhs[16]*(IKCG_params.J_17[3])-rhs[17]*(IKCG_params.J_18[3])-rhs[18]*(IKCG_params.J_19[3])-rhs[19]*(IKCG_params.J_20[3])-rhs[20]*(IKCG_params.J_21[3])-rhs[21]*(IKCG_params.J_22[3])-rhs[22]*(IKCG_params.J_23[3])-rhs[23]*(IKCG_params.J_24[3])-rhs[24]*(IKCG_params.J_25[3])-rhs[25]*(IKCG_params.J_26[3])-rhs[26]*(IKCG_params.J_27[3])-rhs[27]*(IKCG_params.J_28[3])-rhs[28]*(IKCG_params.J_29[3])-rhs[29]*(IKCG_params.J_30[3])-rhs[30]*(IKCG_params.J_31[3])-rhs[31]*(IKCG_params.J_32[3])-rhs[32]*(IKCG_params.J_33[3])-rhs[33]*(IKCG_params.J_34[3])-rhs[34]*(IKCG_params.J_35[3])-rhs[35]*(IKCG_params.J_36[3]);
    lhs[40] = -rhs[0]*(IKCG_params.J_1[4])-rhs[1]*(IKCG_params.J_2[4])-rhs[2]*(IKCG_params.J_3[4])-rhs[3]*(IKCG_params.J_4[4])-rhs[4]*(IKCG_params.J_5[4])-rhs[5]*(IKCG_params.J_6[4])-rhs[6]*(IKCG_params.J_7[4])-rhs[7]*(IKCG_params.J_8[4])-rhs[8]*(IKCG_params.J_9[4])-rhs[9]*(IKCG_params.J_10[4])-rhs[10]*(IKCG_params.J_11[4])-rhs[11]*(IKCG_params.J_12[4])-rhs[12]*(IKCG_params.J_13[4])-rhs[13]*(IKCG_params.J_14[4])-rhs[14]*(IKCG_params.J_15[4])-rhs[15]*(IKCG_params.J_16[4])-rhs[16]*(IKCG_params.J_17[4])-rhs[17]*(IKCG_params.J_18[4])-rhs[18]*(IKCG_params.J_19[4])-rhs[19]*(IKCG_params.J_20[4])-rhs[20]*(IKCG_params.J_21[4])-rhs[21]*(IKCG_params.J_22[4])-rhs[22]*(IKCG_params.J_23[4])-rhs[23]*(IKCG_params.J_24[4])-rhs[24]*(IKCG_params.J_25[4])-rhs[25]*(IKCG_params.J_26[4])-rhs[26]*(IKCG_params.J_27[4])-rhs[27]*(IKCG_params.J_28[4])-rhs[28]*(IKCG_params.J_29[4])-rhs[29]*(IKCG_params.J_30[4])-rhs[30]*(IKCG_params.J_31[4])-rhs[31]*(IKCG_params.J_32[4])-rhs[32]*(IKCG_params.J_33[4])-rhs[33]*(IKCG_params.J_34[4])-rhs[34]*(IKCG_params.J_35[4])-rhs[35]*(IKCG_params.J_36[4]);
    lhs[41] = -rhs[0]*(IKCG_params.J_1[5])-rhs[1]*(IKCG_params.J_2[5])-rhs[2]*(IKCG_params.J_3[5])-rhs[3]*(IKCG_params.J_4[5])-rhs[4]*(IKCG_params.J_5[5])-rhs[5]*(IKCG_params.J_6[5])-rhs[6]*(IKCG_params.J_7[5])-rhs[7]*(IKCG_params.J_8[5])-rhs[8]*(IKCG_params.J_9[5])-rhs[9]*(IKCG_params.J_10[5])-rhs[10]*(IKCG_params.J_11[5])-rhs[11]*(IKCG_params.J_12[5])-rhs[12]*(IKCG_params.J_13[5])-rhs[13]*(IKCG_params.J_14[5])-rhs[14]*(IKCG_params.J_15[5])-rhs[15]*(IKCG_params.J_16[5])-rhs[16]*(IKCG_params.J_17[5])-rhs[17]*(IKCG_params.J_18[5])-rhs[18]*(IKCG_params.J_19[5])-rhs[19]*(IKCG_params.J_20[5])-rhs[20]*(IKCG_params.J_21[5])-rhs[21]*(IKCG_params.J_22[5])-rhs[22]*(IKCG_params.J_23[5])-rhs[23]*(IKCG_params.J_24[5])-rhs[24]*(IKCG_params.J_25[5])-rhs[25]*(IKCG_params.J_26[5])-rhs[26]*(IKCG_params.J_27[5])-rhs[27]*(IKCG_params.J_28[5])-rhs[28]*(IKCG_params.J_29[5])-rhs[29]*(IKCG_params.J_30[5])-rhs[30]*(IKCG_params.J_31[5])-rhs[31]*(IKCG_params.J_32[5])-rhs[32]*(IKCG_params.J_33[5])-rhs[33]*(IKCG_params.J_34[5])-rhs[34]*(IKCG_params.J_35[5])-rhs[35]*(IKCG_params.J_36[5]);
    lhs[42] = -rhs[0]*(IKCG_params.J_1[6])-rhs[1]*(IKCG_params.J_2[6])-rhs[2]*(IKCG_params.J_3[6])-rhs[18]*(IKCG_params.J_19[6])-rhs[19]*(IKCG_params.J_20[6])-rhs[20]*(IKCG_params.J_21[6])-rhs[21]*(IKCG_params.J_22[6])-rhs[22]*(IKCG_params.J_23[6])-rhs[23]*(IKCG_params.J_24[6])-rhs[24]*(IKCG_params.J_25[6])-rhs[25]*(IKCG_params.J_26[6])-rhs[26]*(IKCG_params.J_27[6])-rhs[27]*(IKCG_params.J_28[6])-rhs[28]*(IKCG_params.J_29[6])-rhs[29]*(IKCG_params.J_30[6])-rhs[30]*(IKCG_params.J_31[6])-rhs[31]*(IKCG_params.J_32[6])-rhs[32]*(IKCG_params.J_33[6])-rhs[33]*(IKCG_params.J_34[6])-rhs[34]*(IKCG_params.J_35[6])-rhs[35]*(IKCG_params.J_36[6]);
    lhs[43] = -rhs[0]*(IKCG_params.J_1[7])-rhs[1]*(IKCG_params.J_2[7])-rhs[2]*(IKCG_params.J_3[7])-rhs[18]*(IKCG_params.J_19[7])-rhs[19]*(IKCG_params.J_20[7])-rhs[20]*(IKCG_params.J_21[7])-rhs[21]*(IKCG_params.J_22[7])-rhs[22]*(IKCG_params.J_23[7])-rhs[23]*(IKCG_params.J_24[7])-rhs[24]*(IKCG_params.J_25[7])-rhs[25]*(IKCG_params.J_26[7])-rhs[26]*(IKCG_params.J_27[7])-rhs[27]*(IKCG_params.J_28[7])-rhs[28]*(IKCG_params.J_29[7])-rhs[29]*(IKCG_params.J_30[7])-rhs[30]*(IKCG_params.J_31[7])-rhs[31]*(IKCG_params.J_32[7])-rhs[32]*(IKCG_params.J_33[7])-rhs[33]*(IKCG_params.J_34[7])-rhs[34]*(IKCG_params.J_35[7])-rhs[35]*(IKCG_params.J_36[7]);
    lhs[44] = -rhs[0]*(IKCG_params.J_1[8])-rhs[1]*(IKCG_params.J_2[8])-rhs[2]*(IKCG_params.J_3[8])-rhs[18]*(IKCG_params.J_19[8])-rhs[19]*(IKCG_params.J_20[8])-rhs[20]*(IKCG_params.J_21[8])-rhs[21]*(IKCG_params.J_22[8])-rhs[22]*(IKCG_params.J_23[8])-rhs[23]*(IKCG_params.J_24[8])-rhs[24]*(IKCG_params.J_25[8])-rhs[25]*(IKCG_params.J_26[8])-rhs[26]*(IKCG_params.J_27[8])-rhs[27]*(IKCG_params.J_28[8])-rhs[28]*(IKCG_params.J_29[8])-rhs[29]*(IKCG_params.J_30[8])-rhs[30]*(IKCG_params.J_31[8])-rhs[31]*(IKCG_params.J_32[8])-rhs[32]*(IKCG_params.J_33[8])-rhs[33]*(IKCG_params.J_34[8])-rhs[34]*(IKCG_params.J_35[8])-rhs[35]*(IKCG_params.J_36[8]);
    lhs[45] = -rhs[0]*(IKCG_params.J_1[9])-rhs[1]*(IKCG_params.J_2[9])-rhs[2]*(IKCG_params.J_3[9])-rhs[33]*(IKCG_params.J_34[9])-rhs[34]*(IKCG_params.J_35[9])-rhs[35]*(IKCG_params.J_36[9]);
    lhs[46] = -rhs[0]*(IKCG_params.J_1[10])-rhs[1]*(IKCG_params.J_2[10])-rhs[2]*(IKCG_params.J_3[10])-rhs[33]*(IKCG_params.J_34[10])-rhs[34]*(IKCG_params.J_35[10])-rhs[35]*(IKCG_params.J_36[10]);
    lhs[47] = -rhs[0]*(IKCG_params.J_1[11])-rhs[1]*(IKCG_params.J_2[11])-rhs[2]*(IKCG_params.J_3[11])-rhs[33]*(IKCG_params.J_34[11])-rhs[34]*(IKCG_params.J_35[11])-rhs[35]*(IKCG_params.J_36[11]);
    lhs[48] = -rhs[0]*(IKCG_params.J_1[12])-rhs[1]*(IKCG_params.J_2[12])-rhs[2]*(IKCG_params.J_3[12])-rhs[6]*(IKCG_params.J_7[12])-rhs[7]*(IKCG_params.J_8[12])-rhs[8]*(IKCG_params.J_9[12])-rhs[9]*(IKCG_params.J_10[12])-rhs[10]*(IKCG_params.J_11[12])-rhs[11]*(IKCG_params.J_12[12]);
    lhs[49] = -rhs[0]*(IKCG_params.J_1[13])-rhs[1]*(IKCG_params.J_2[13])-rhs[2]*(IKCG_params.J_3[13])-rhs[6]*(IKCG_params.J_7[13])-rhs[7]*(IKCG_params.J_8[13])-rhs[8]*(IKCG_params.J_9[13])-rhs[9]*(IKCG_params.J_10[13])-rhs[10]*(IKCG_params.J_11[13])-rhs[11]*(IKCG_params.J_12[13]);
    lhs[50] = -rhs[0]*(IKCG_params.J_1[14])-rhs[1]*(IKCG_params.J_2[14])-rhs[2]*(IKCG_params.J_3[14])-rhs[6]*(IKCG_params.J_7[14])-rhs[7]*(IKCG_params.J_8[14])-rhs[8]*(IKCG_params.J_9[14])-rhs[9]*(IKCG_params.J_10[14])-rhs[10]*(IKCG_params.J_11[14])-rhs[11]*(IKCG_params.J_12[14]);
    lhs[51] = -rhs[0]*(IKCG_params.J_1[15])-rhs[1]*(IKCG_params.J_2[15])-rhs[2]*(IKCG_params.J_3[15])-rhs[6]*(IKCG_params.J_7[15])-rhs[7]*(IKCG_params.J_8[15])-rhs[8]*(IKCG_params.J_9[15])-rhs[9]*(IKCG_params.J_10[15])-rhs[10]*(IKCG_params.J_11[15])-rhs[11]*(IKCG_params.J_12[15]);
    lhs[52] = -rhs[0]*(IKCG_params.J_1[16])-rhs[1]*(IKCG_params.J_2[16])-rhs[2]*(IKCG_params.J_3[16])-rhs[6]*(IKCG_params.J_7[16])-rhs[7]*(IKCG_params.J_8[16])-rhs[8]*(IKCG_params.J_9[16])-rhs[9]*(IKCG_params.J_10[16])-rhs[10]*(IKCG_params.J_11[16])-rhs[11]*(IKCG_params.J_12[16]);
    lhs[53] = -rhs[0]*(IKCG_params.J_1[17])-rhs[1]*(IKCG_params.J_2[17])-rhs[2]*(IKCG_params.J_3[17])-rhs[6]*(IKCG_params.J_7[17])-rhs[7]*(IKCG_params.J_8[17])-rhs[8]*(IKCG_params.J_9[17])-rhs[9]*(IKCG_params.J_10[17])-rhs[10]*(IKCG_params.J_11[17])-rhs[11]*(IKCG_params.J_12[17]);
    lhs[54] = -rhs[0]*(IKCG_params.J_1[18])-rhs[1]*(IKCG_params.J_2[18])-rhs[2]*(IKCG_params.J_3[18])-rhs[12]*(IKCG_params.J_13[18])-rhs[13]*(IKCG_params.J_14[18])-rhs[14]*(IKCG_params.J_15[18])-rhs[15]*(IKCG_params.J_16[18])-rhs[16]*(IKCG_params.J_17[18])-rhs[17]*(IKCG_params.J_18[18]);
    lhs[55] = -rhs[0]*(IKCG_params.J_1[19])-rhs[1]*(IKCG_params.J_2[19])-rhs[2]*(IKCG_params.J_3[19])-rhs[12]*(IKCG_params.J_13[19])-rhs[13]*(IKCG_params.J_14[19])-rhs[14]*(IKCG_params.J_15[19])-rhs[15]*(IKCG_params.J_16[19])-rhs[16]*(IKCG_params.J_17[19])-rhs[17]*(IKCG_params.J_18[19]);
    lhs[56] = -rhs[0]*(IKCG_params.J_1[20])-rhs[1]*(IKCG_params.J_2[20])-rhs[2]*(IKCG_params.J_3[20])-rhs[12]*(IKCG_params.J_13[20])-rhs[13]*(IKCG_params.J_14[20])-rhs[14]*(IKCG_params.J_15[20])-rhs[15]*(IKCG_params.J_16[20])-rhs[16]*(IKCG_params.J_17[20])-rhs[17]*(IKCG_params.J_18[20]);
    lhs[57] = -rhs[0]*(IKCG_params.J_1[21])-rhs[1]*(IKCG_params.J_2[21])-rhs[2]*(IKCG_params.J_3[21])-rhs[12]*(IKCG_params.J_13[21])-rhs[13]*(IKCG_params.J_14[21])-rhs[14]*(IKCG_params.J_15[21])-rhs[15]*(IKCG_params.J_16[21])-rhs[16]*(IKCG_params.J_17[21])-rhs[17]*(IKCG_params.J_18[21]);
    lhs[58] = -rhs[0]*(IKCG_params.J_1[22])-rhs[1]*(IKCG_params.J_2[22])-rhs[2]*(IKCG_params.J_3[22])-rhs[12]*(IKCG_params.J_13[22])-rhs[13]*(IKCG_params.J_14[22])-rhs[14]*(IKCG_params.J_15[22])-rhs[15]*(IKCG_params.J_16[22])-rhs[16]*(IKCG_params.J_17[22])-rhs[17]*(IKCG_params.J_18[22]);
    lhs[59] = -rhs[0]*(IKCG_params.J_1[23])-rhs[1]*(IKCG_params.J_2[23])-rhs[2]*(IKCG_params.J_3[23])-rhs[12]*(IKCG_params.J_13[23])-rhs[13]*(IKCG_params.J_14[23])-rhs[14]*(IKCG_params.J_15[23])-rhs[15]*(IKCG_params.J_16[23])-rhs[16]*(IKCG_params.J_17[23])-rhs[17]*(IKCG_params.J_18[23]);
    lhs[60] = -rhs[0]*(IKCG_params.J_1[24])-rhs[1]*(IKCG_params.J_2[24])-rhs[2]*(IKCG_params.J_3[24])-rhs[18]*(IKCG_params.J_19[24])-rhs[19]*(IKCG_params.J_20[24])-rhs[20]*(IKCG_params.J_21[24])-rhs[21]*(IKCG_params.J_22[24])-rhs[22]*(IKCG_params.J_23[24])-rhs[23]*(IKCG_params.J_24[24]);
    lhs[61] = -rhs[0]*(IKCG_params.J_1[25])-rhs[1]*(IKCG_params.J_2[25])-rhs[2]*(IKCG_params.J_3[25])-rhs[18]*(IKCG_params.J_19[25])-rhs[19]*(IKCG_params.J_20[25])-rhs[20]*(IKCG_params.J_21[25])-rhs[21]*(IKCG_params.J_22[25])-rhs[22]*(IKCG_params.J_23[25])-rhs[23]*(IKCG_params.J_24[25]);
    lhs[62] = -rhs[0]*(IKCG_params.J_1[26])-rhs[1]*(IKCG_params.J_2[26])-rhs[2]*(IKCG_params.J_3[26])-rhs[18]*(IKCG_params.J_19[26])-rhs[19]*(IKCG_params.J_20[26])-rhs[20]*(IKCG_params.J_21[26])-rhs[21]*(IKCG_params.J_22[26])-rhs[22]*(IKCG_params.J_23[26])-rhs[23]*(IKCG_params.J_24[26]);
    lhs[63] = -rhs[0]*(IKCG_params.J_1[27])-rhs[1]*(IKCG_params.J_2[27])-rhs[2]*(IKCG_params.J_3[27])-rhs[18]*(IKCG_params.J_19[27])-rhs[19]*(IKCG_params.J_20[27])-rhs[20]*(IKCG_params.J_21[27])-rhs[21]*(IKCG_params.J_22[27])-rhs[22]*(IKCG_params.J_23[27])-rhs[23]*(IKCG_params.J_24[27]);
    lhs[64] = -rhs[0]*(IKCG_params.J_1[28])-rhs[1]*(IKCG_params.J_2[28])-rhs[2]*(IKCG_params.J_3[28])-rhs[18]*(IKCG_params.J_19[28])-rhs[19]*(IKCG_params.J_20[28])-rhs[20]*(IKCG_params.J_21[28])-rhs[21]*(IKCG_params.J_22[28])-rhs[22]*(IKCG_params.J_23[28])-rhs[23]*(IKCG_params.J_24[28]);
    lhs[65] = -rhs[0]*(IKCG_params.J_1[29])-rhs[1]*(IKCG_params.J_2[29])-rhs[2]*(IKCG_params.J_3[29])-rhs[18]*(IKCG_params.J_19[29])-rhs[19]*(IKCG_params.J_20[29])-rhs[20]*(IKCG_params.J_21[29])-rhs[21]*(IKCG_params.J_22[29])-rhs[22]*(IKCG_params.J_23[29])-rhs[23]*(IKCG_params.J_24[29]);
    lhs[66] = -rhs[0]*(IKCG_params.J_1[30])-rhs[1]*(IKCG_params.J_2[30])-rhs[2]*(IKCG_params.J_3[30])-rhs[18]*(IKCG_params.J_19[30])-rhs[19]*(IKCG_params.J_20[30])-rhs[20]*(IKCG_params.J_21[30])-rhs[21]*(IKCG_params.J_22[30])-rhs[22]*(IKCG_params.J_23[30])-rhs[23]*(IKCG_params.J_24[30]);
    lhs[67] = -rhs[0]*(IKCG_params.J_1[31])-rhs[1]*(IKCG_params.J_2[31])-rhs[2]*(IKCG_params.J_3[31])-rhs[24]*(IKCG_params.J_25[31])-rhs[25]*(IKCG_params.J_26[31])-rhs[26]*(IKCG_params.J_27[31])-rhs[27]*(IKCG_params.J_28[31])-rhs[28]*(IKCG_params.J_29[31])-rhs[29]*(IKCG_params.J_30[31]);
    lhs[68] = -rhs[0]*(IKCG_params.J_1[32])-rhs[1]*(IKCG_params.J_2[32])-rhs[2]*(IKCG_params.J_3[32])-rhs[24]*(IKCG_params.J_25[32])-rhs[25]*(IKCG_params.J_26[32])-rhs[26]*(IKCG_params.J_27[32])-rhs[27]*(IKCG_params.J_28[32])-rhs[28]*(IKCG_params.J_29[32])-rhs[29]*(IKCG_params.J_30[32]);
    lhs[69] = -rhs[0]*(IKCG_params.J_1[33])-rhs[1]*(IKCG_params.J_2[33])-rhs[2]*(IKCG_params.J_3[33])-rhs[24]*(IKCG_params.J_25[33])-rhs[25]*(IKCG_params.J_26[33])-rhs[26]*(IKCG_params.J_27[33])-rhs[27]*(IKCG_params.J_28[33])-rhs[28]*(IKCG_params.J_29[33])-rhs[29]*(IKCG_params.J_30[33]);
    lhs[70] = -rhs[0]*(IKCG_params.J_1[34])-rhs[1]*(IKCG_params.J_2[34])-rhs[2]*(IKCG_params.J_3[34])-rhs[24]*(IKCG_params.J_25[34])-rhs[25]*(IKCG_params.J_26[34])-rhs[26]*(IKCG_params.J_27[34])-rhs[27]*(IKCG_params.J_28[34])-rhs[28]*(IKCG_params.J_29[34])-rhs[29]*(IKCG_params.J_30[34]);
    lhs[71] = -rhs[0]*(IKCG_params.J_1[35])-rhs[1]*(IKCG_params.J_2[35])-rhs[2]*(IKCG_params.J_3[35])-rhs[24]*(IKCG_params.J_25[35])-rhs[25]*(IKCG_params.J_26[35])-rhs[26]*(IKCG_params.J_27[35])-rhs[27]*(IKCG_params.J_28[35])-rhs[28]*(IKCG_params.J_29[35])-rhs[29]*(IKCG_params.J_30[35]);
    lhs[72] = -rhs[0]*(IKCG_params.J_1[36])-rhs[1]*(IKCG_params.J_2[36])-rhs[2]*(IKCG_params.J_3[36])-rhs[24]*(IKCG_params.J_25[36])-rhs[25]*(IKCG_params.J_26[36])-rhs[26]*(IKCG_params.J_27[36])-rhs[27]*(IKCG_params.J_28[36])-rhs[28]*(IKCG_params.J_29[36])-rhs[29]*(IKCG_params.J_30[36]);
    lhs[73] = -rhs[0]*(IKCG_params.J_1[37])-rhs[1]*(IKCG_params.J_2[37])-rhs[2]*(IKCG_params.J_3[37])-rhs[24]*(IKCG_params.J_25[37])-rhs[25]*(IKCG_params.J_26[37])-rhs[26]*(IKCG_params.J_27[37])-rhs[27]*(IKCG_params.J_28[37])-rhs[28]*(IKCG_params.J_29[37])-rhs[29]*(IKCG_params.J_30[37]);
  }
  void IKCG_multbymG(double *lhs, double *rhs) {
    lhs[0] = -rhs[42]*(-1);
    lhs[1] = -rhs[43]*(-1);
    lhs[2] = -rhs[44]*(-1);
    lhs[3] = -rhs[45]*(-1);
    lhs[4] = -rhs[46]*(-1);
    lhs[5] = -rhs[47]*(-1);
    lhs[6] = -rhs[48]*(-1);
    lhs[7] = -rhs[49]*(-1);
    lhs[8] = -rhs[50]*(-1);
    lhs[9] = -rhs[51]*(-1);
    lhs[10] = -rhs[52]*(-1);
    lhs[11] = -rhs[53]*(-1);
    lhs[12] = -rhs[54]*(-1);
    lhs[13] = -rhs[55]*(-1);
    lhs[14] = -rhs[56]*(-1);
    lhs[15] = -rhs[57]*(-1);
    lhs[16] = -rhs[58]*(-1);
    lhs[17] = -rhs[59]*(-1);
    lhs[18] = -rhs[60]*(-1);
    lhs[19] = -rhs[61]*(-1);
    lhs[20] = -rhs[62]*(-1);
    lhs[21] = -rhs[63]*(-1);
    lhs[22] = -rhs[64]*(-1);
    lhs[23] = -rhs[65]*(-1);
    lhs[24] = -rhs[66]*(-1);
    lhs[25] = -rhs[67]*(-1);
    lhs[26] = -rhs[68]*(-1);
    lhs[27] = -rhs[69]*(-1);
    lhs[28] = -rhs[70]*(-1);
    lhs[29] = -rhs[71]*(-1);
    lhs[30] = -rhs[72]*(-1);
    lhs[31] = -rhs[73]*(-1);
    lhs[32] = -rhs[42]*(1);
    lhs[33] = -rhs[43]*(1);
    lhs[34] = -rhs[44]*(1);
    lhs[35] = -rhs[45]*(1);
    lhs[36] = -rhs[46]*(1);
    lhs[37] = -rhs[47]*(1);
    lhs[38] = -rhs[48]*(1);
    lhs[39] = -rhs[49]*(1);
    lhs[40] = -rhs[50]*(1);
    lhs[41] = -rhs[51]*(1);
    lhs[42] = -rhs[52]*(1);
    lhs[43] = -rhs[53]*(1);
    lhs[44] = -rhs[54]*(1);
    lhs[45] = -rhs[55]*(1);
    lhs[46] = -rhs[56]*(1);
    lhs[47] = -rhs[57]*(1);
    lhs[48] = -rhs[58]*(1);
    lhs[49] = -rhs[59]*(1);
    lhs[50] = -rhs[60]*(1);
    lhs[51] = -rhs[61]*(1);
    lhs[52] = -rhs[62]*(1);
    lhs[53] = -rhs[63]*(1);
    lhs[54] = -rhs[64]*(1);
    lhs[55] = -rhs[65]*(1);
    lhs[56] = -rhs[66]*(1);
    lhs[57] = -rhs[67]*(1);
    lhs[58] = -rhs[68]*(1);
    lhs[59] = -rhs[69]*(1);
    lhs[60] = -rhs[70]*(1);
    lhs[61] = -rhs[71]*(1);
    lhs[62] = -rhs[72]*(1);
    lhs[63] = -rhs[73]*(1);
  }
  void IKCG_multbymGT(double *lhs, double *rhs) {
    lhs[0] = 0;
    lhs[1] = 0;
    lhs[2] = 0;
    lhs[3] = 0;
    lhs[4] = 0;
    lhs[5] = 0;
    lhs[6] = 0;
    lhs[7] = 0;
    lhs[8] = 0;
    lhs[9] = 0;
    lhs[10] = 0;
    lhs[11] = 0;
    lhs[12] = 0;
    lhs[13] = 0;
    lhs[14] = 0;
    lhs[15] = 0;
    lhs[16] = 0;
    lhs[17] = 0;
    lhs[18] = 0;
    lhs[19] = 0;
    lhs[20] = 0;
    lhs[21] = 0;
    lhs[22] = 0;
    lhs[23] = 0;
    lhs[24] = 0;
    lhs[25] = 0;
    lhs[26] = 0;
    lhs[27] = 0;
    lhs[28] = 0;
    lhs[29] = 0;
    lhs[30] = 0;
    lhs[31] = 0;
    lhs[32] = 0;
    lhs[33] = 0;
    lhs[34] = 0;
    lhs[35] = 0;
    lhs[36] = 0;
    lhs[37] = 0;
    lhs[38] = 0;
    lhs[39] = 0;
    lhs[40] = 0;
    lhs[41] = 0;
    lhs[42] = -rhs[0]*(-1)-rhs[32]*(1);
    lhs[43] = -rhs[1]*(-1)-rhs[33]*(1);
    lhs[44] = -rhs[2]*(-1)-rhs[34]*(1);
    lhs[45] = -rhs[3]*(-1)-rhs[35]*(1);
    lhs[46] = -rhs[4]*(-1)-rhs[36]*(1);
    lhs[47] = -rhs[5]*(-1)-rhs[37]*(1);
    lhs[48] = -rhs[6]*(-1)-rhs[38]*(1);
    lhs[49] = -rhs[7]*(-1)-rhs[39]*(1);
    lhs[50] = -rhs[8]*(-1)-rhs[40]*(1);
    lhs[51] = -rhs[9]*(-1)-rhs[41]*(1);
    lhs[52] = -rhs[10]*(-1)-rhs[42]*(1);
    lhs[53] = -rhs[11]*(-1)-rhs[43]*(1);
    lhs[54] = -rhs[12]*(-1)-rhs[44]*(1);
    lhs[55] = -rhs[13]*(-1)-rhs[45]*(1);
    lhs[56] = -rhs[14]*(-1)-rhs[46]*(1);
    lhs[57] = -rhs[15]*(-1)-rhs[47]*(1);
    lhs[58] = -rhs[16]*(-1)-rhs[48]*(1);
    lhs[59] = -rhs[17]*(-1)-rhs[49]*(1);
    lhs[60] = -rhs[18]*(-1)-rhs[50]*(1);
    lhs[61] = -rhs[19]*(-1)-rhs[51]*(1);
    lhs[62] = -rhs[20]*(-1)-rhs[52]*(1);
    lhs[63] = -rhs[21]*(-1)-rhs[53]*(1);
    lhs[64] = -rhs[22]*(-1)-rhs[54]*(1);
    lhs[65] = -rhs[23]*(-1)-rhs[55]*(1);
    lhs[66] = -rhs[24]*(-1)-rhs[56]*(1);
    lhs[67] = -rhs[25]*(-1)-rhs[57]*(1);
    lhs[68] = -rhs[26]*(-1)-rhs[58]*(1);
    lhs[69] = -rhs[27]*(-1)-rhs[59]*(1);
    lhs[70] = -rhs[28]*(-1)-rhs[60]*(1);
    lhs[71] = -rhs[29]*(-1)-rhs[61]*(1);
    lhs[72] = -rhs[30]*(-1)-rhs[62]*(1);
    lhs[73] = -rhs[31]*(-1)-rhs[63]*(1);
  }
  void IKCG_multbyP(double *lhs, double *rhs) {
    /* TODO use the fact that P is symmetric? */
    /* TODO check doubling / half factor etc. */
    lhs[0] = rhs[0]*(2*IKCG_params.slack[0]);
    lhs[1] = rhs[1]*(2*IKCG_params.slack[1]);
    lhs[2] = rhs[2]*(2*IKCG_params.slack[2]);
    lhs[3] = rhs[3]*(2*IKCG_params.slack[3]);
    lhs[4] = rhs[4]*(2*IKCG_params.slack[4]);
    lhs[5] = rhs[5]*(2*IKCG_params.slack[5]);
    lhs[6] = rhs[6]*(2*IKCG_params.slack[6]);
    lhs[7] = rhs[7]*(2*IKCG_params.slack[7]);
    lhs[8] = rhs[8]*(2*IKCG_params.slack[8]);
    lhs[9] = rhs[9]*(2*IKCG_params.slack[9]);
    lhs[10] = rhs[10]*(2*IKCG_params.slack[10]);
    lhs[11] = rhs[11]*(2*IKCG_params.slack[11]);
    lhs[12] = rhs[12]*(2*IKCG_params.slack[12]);
    lhs[13] = rhs[13]*(2*IKCG_params.slack[13]);
    lhs[14] = rhs[14]*(2*IKCG_params.slack[14]);
    lhs[15] = rhs[15]*(2*IKCG_params.slack[15]);
    lhs[16] = rhs[16]*(2*IKCG_params.slack[16]);
    lhs[17] = rhs[17]*(2*IKCG_params.slack[17]);
    lhs[18] = rhs[18]*(2*IKCG_params.slack[18]);
    lhs[19] = rhs[19]*(2*IKCG_params.slack[19]);
    lhs[20] = rhs[20]*(2*IKCG_params.slack[20]);
    lhs[21] = rhs[21]*(2*IKCG_params.slack[21]);
    lhs[22] = rhs[22]*(2*IKCG_params.slack[22]);
    lhs[23] = rhs[23]*(2*IKCG_params.slack[23]);
    lhs[24] = rhs[24]*(2*IKCG_params.slack[24]);
    lhs[25] = rhs[25]*(2*IKCG_params.slack[25]);
    lhs[26] = rhs[26]*(2*IKCG_params.slack[26]);
    lhs[27] = rhs[27]*(2*IKCG_params.slack[27]);
    lhs[28] = rhs[28]*(2*IKCG_params.slack[28]);
    lhs[29] = rhs[29]*(2*IKCG_params.slack[29]);
    lhs[30] = rhs[30]*(2*IKCG_params.slack[30]);
    lhs[31] = rhs[31]*(2*IKCG_params.slack[31]);
    lhs[32] = rhs[32]*(2*IKCG_params.slack[32]);
    lhs[33] = rhs[33]*(2*IKCG_params.slack[33]);
    lhs[34] = rhs[34]*(2*IKCG_params.slack[34]);
    lhs[35] = rhs[35]*(2*IKCG_params.slack[35]);
    lhs[36] = rhs[36]*(2*IKCG_params.damping[0]);
    lhs[37] = rhs[37]*(2*IKCG_params.damping[1]);
    lhs[38] = rhs[38]*(2*IKCG_params.damping[2]);
    lhs[39] = rhs[39]*(2*IKCG_params.damping[3]);
    lhs[40] = rhs[40]*(2*IKCG_params.damping[4]);
    lhs[41] = rhs[41]*(2*IKCG_params.damping[5]);
    lhs[42] = rhs[42]*(2*IKCG_params.damping[6]);
    lhs[43] = rhs[43]*(2*IKCG_params.damping[7]);
    lhs[44] = rhs[44]*(2*IKCG_params.damping[8]);
    lhs[45] = rhs[45]*(2*IKCG_params.damping[9]);
    lhs[46] = rhs[46]*(2*IKCG_params.damping[10]);
    lhs[47] = rhs[47]*(2*IKCG_params.damping[11]);
    lhs[48] = rhs[48]*(2*IKCG_params.damping[12]);
    lhs[49] = rhs[49]*(2*IKCG_params.damping[13]);
    lhs[50] = rhs[50]*(2*IKCG_params.damping[14]);
    lhs[51] = rhs[51]*(2*IKCG_params.damping[15]);
    lhs[52] = rhs[52]*(2*IKCG_params.damping[16]);
    lhs[53] = rhs[53]*(2*IKCG_params.damping[17]);
    lhs[54] = rhs[54]*(2*IKCG_params.damping[18]);
    lhs[55] = rhs[55]*(2*IKCG_params.damping[19]);
    lhs[56] = rhs[56]*(2*IKCG_params.damping[20]);
    lhs[57] = rhs[57]*(2*IKCG_params.damping[21]);
    lhs[58] = rhs[58]*(2*IKCG_params.damping[22]);
    lhs[59] = rhs[59]*(2*IKCG_params.damping[23]);
    lhs[60] = rhs[60]*(2*IKCG_params.damping[24]);
    lhs[61] = rhs[61]*(2*IKCG_params.damping[25]);
    lhs[62] = rhs[62]*(2*IKCG_params.damping[26]);
    lhs[63] = rhs[63]*(2*IKCG_params.damping[27]);
    lhs[64] = rhs[64]*(2*IKCG_params.damping[28]);
    lhs[65] = rhs[65]*(2*IKCG_params.damping[29]);
    lhs[66] = rhs[66]*(2*IKCG_params.damping[30]);
    lhs[67] = rhs[67]*(2*IKCG_params.damping[31]);
    lhs[68] = rhs[68]*(2*IKCG_params.damping[32]);
    lhs[69] = rhs[69]*(2*IKCG_params.damping[33]);
    lhs[70] = rhs[70]*(2*IKCG_params.damping[34]);
    lhs[71] = rhs[71]*(2*IKCG_params.damping[35]);
    lhs[72] = rhs[72]*(2*IKCG_params.damping[36]);
    lhs[73] = rhs[73]*(2*IKCG_params.damping[37]);
  }
  void IKCG_fillq(void) {
    IKCG_work.q[0] = 0;
    IKCG_work.q[1] = 0;
    IKCG_work.q[2] = 0;
    IKCG_work.q[3] = 0;
    IKCG_work.q[4] = 0;
    IKCG_work.q[5] = 0;
    IKCG_work.q[6] = 0;
    IKCG_work.q[7] = 0;
    IKCG_work.q[8] = 0;
    IKCG_work.q[9] = 0;
    IKCG_work.q[10] = 0;
    IKCG_work.q[11] = 0;
    IKCG_work.q[12] = 0;
    IKCG_work.q[13] = 0;
    IKCG_work.q[14] = 0;
    IKCG_work.q[15] = 0;
    IKCG_work.q[16] = 0;
    IKCG_work.q[17] = 0;
    IKCG_work.q[18] = 0;
    IKCG_work.q[19] = 0;
    IKCG_work.q[20] = 0;
    IKCG_work.q[21] = 0;
    IKCG_work.q[22] = 0;
    IKCG_work.q[23] = 0;
    IKCG_work.q[24] = 0;
    IKCG_work.q[25] = 0;
    IKCG_work.q[26] = 0;
    IKCG_work.q[27] = 0;
    IKCG_work.q[28] = 0;
    IKCG_work.q[29] = 0;
    IKCG_work.q[30] = 0;
    IKCG_work.q[31] = 0;
    IKCG_work.q[32] = 0;
    IKCG_work.q[33] = 0;
    IKCG_work.q[34] = 0;
    IKCG_work.q[35] = 0;
    IKCG_work.q[36] = 2*IKCG_params.damping[0]*IKCG_params.qref[0];
    IKCG_work.q[37] = 2*IKCG_params.damping[1]*IKCG_params.qref[1];
    IKCG_work.q[38] = 2*IKCG_params.damping[2]*IKCG_params.qref[2];
    IKCG_work.q[39] = 2*IKCG_params.damping[3]*IKCG_params.qref[3];
    IKCG_work.q[40] = 2*IKCG_params.damping[4]*IKCG_params.qref[4];
    IKCG_work.q[41] = 2*IKCG_params.damping[5]*IKCG_params.qref[5];
    IKCG_work.q[42] = 2*IKCG_params.damping[6]*IKCG_params.qref[6];
    IKCG_work.q[43] = 2*IKCG_params.damping[7]*IKCG_params.qref[7];
    IKCG_work.q[44] = 2*IKCG_params.damping[8]*IKCG_params.qref[8];
    IKCG_work.q[45] = 2*IKCG_params.damping[9]*IKCG_params.qref[9];
    IKCG_work.q[46] = 2*IKCG_params.damping[10]*IKCG_params.qref[10];
    IKCG_work.q[47] = 2*IKCG_params.damping[11]*IKCG_params.qref[11];
    IKCG_work.q[48] = 2*IKCG_params.damping[12]*IKCG_params.qref[12];
    IKCG_work.q[49] = 2*IKCG_params.damping[13]*IKCG_params.qref[13];
    IKCG_work.q[50] = 2*IKCG_params.damping[14]*IKCG_params.qref[14];
    IKCG_work.q[51] = 2*IKCG_params.damping[15]*IKCG_params.qref[15];
    IKCG_work.q[52] = 2*IKCG_params.damping[16]*IKCG_params.qref[16];
    IKCG_work.q[53] = 2*IKCG_params.damping[17]*IKCG_params.qref[17];
    IKCG_work.q[54] = 2*IKCG_params.damping[18]*IKCG_params.qref[18];
    IKCG_work.q[55] = 2*IKCG_params.damping[19]*IKCG_params.qref[19];
    IKCG_work.q[56] = 2*IKCG_params.damping[20]*IKCG_params.qref[20];
    IKCG_work.q[57] = 2*IKCG_params.damping[21]*IKCG_params.qref[21];
    IKCG_work.q[58] = 2*IKCG_params.damping[22]*IKCG_params.qref[22];
    IKCG_work.q[59] = 2*IKCG_params.damping[23]*IKCG_params.qref[23];
    IKCG_work.q[60] = 2*IKCG_params.damping[24]*IKCG_params.qref[24];
    IKCG_work.q[61] = 2*IKCG_params.damping[25]*IKCG_params.qref[25];
    IKCG_work.q[62] = 2*IKCG_params.damping[26]*IKCG_params.qref[26];
    IKCG_work.q[63] = 2*IKCG_params.damping[27]*IKCG_params.qref[27];
    IKCG_work.q[64] = 2*IKCG_params.damping[28]*IKCG_params.qref[28];
    IKCG_work.q[65] = 2*IKCG_params.damping[29]*IKCG_params.qref[29];
    IKCG_work.q[66] = 2*IKCG_params.damping[30]*IKCG_params.qref[30];
    IKCG_work.q[67] = 2*IKCG_params.damping[31]*IKCG_params.qref[31];
    IKCG_work.q[68] = 2*IKCG_params.damping[32]*IKCG_params.qref[32];
    IKCG_work.q[69] = 2*IKCG_params.damping[33]*IKCG_params.qref[33];
    IKCG_work.q[70] = 2*IKCG_params.damping[34]*IKCG_params.qref[34];
    IKCG_work.q[71] = 2*IKCG_params.damping[35]*IKCG_params.qref[35];
    IKCG_work.q[72] = 2*IKCG_params.damping[36]*IKCG_params.qref[36];
    IKCG_work.q[73] = 2*IKCG_params.damping[37]*IKCG_params.qref[37];
  }
  void IKCG_fillh(void) {
    IKCG_work.h[0] = -IKCG_params.qlow[0];
    IKCG_work.h[1] = -IKCG_params.qlow[1];
    IKCG_work.h[2] = -IKCG_params.qlow[2];
    IKCG_work.h[3] = -IKCG_params.qlow[3];
    IKCG_work.h[4] = -IKCG_params.qlow[4];
    IKCG_work.h[5] = -IKCG_params.qlow[5];
    IKCG_work.h[6] = -IKCG_params.qlow[6];
    IKCG_work.h[7] = -IKCG_params.qlow[7];
    IKCG_work.h[8] = -IKCG_params.qlow[8];
    IKCG_work.h[9] = -IKCG_params.qlow[9];
    IKCG_work.h[10] = -IKCG_params.qlow[10];
    IKCG_work.h[11] = -IKCG_params.qlow[11];
    IKCG_work.h[12] = -IKCG_params.qlow[12];
    IKCG_work.h[13] = -IKCG_params.qlow[13];
    IKCG_work.h[14] = -IKCG_params.qlow[14];
    IKCG_work.h[15] = -IKCG_params.qlow[15];
    IKCG_work.h[16] = -IKCG_params.qlow[16];
    IKCG_work.h[17] = -IKCG_params.qlow[17];
    IKCG_work.h[18] = -IKCG_params.qlow[18];
    IKCG_work.h[19] = -IKCG_params.qlow[19];
    IKCG_work.h[20] = -IKCG_params.qlow[20];
    IKCG_work.h[21] = -IKCG_params.qlow[21];
    IKCG_work.h[22] = -IKCG_params.qlow[22];
    IKCG_work.h[23] = -IKCG_params.qlow[23];
    IKCG_work.h[24] = -IKCG_params.qlow[24];
    IKCG_work.h[25] = -IKCG_params.qlow[25];
    IKCG_work.h[26] = -IKCG_params.qlow[26];
    IKCG_work.h[27] = -IKCG_params.qlow[27];
    IKCG_work.h[28] = -IKCG_params.qlow[28];
    IKCG_work.h[29] = -IKCG_params.qlow[29];
    IKCG_work.h[30] = -IKCG_params.qlow[30];
    IKCG_work.h[31] = -IKCG_params.qlow[31];
    IKCG_work.h[32] = IKCG_params.qup[0];
    IKCG_work.h[33] = IKCG_params.qup[1];
    IKCG_work.h[34] = IKCG_params.qup[2];
    IKCG_work.h[35] = IKCG_params.qup[3];
    IKCG_work.h[36] = IKCG_params.qup[4];
    IKCG_work.h[37] = IKCG_params.qup[5];
    IKCG_work.h[38] = IKCG_params.qup[6];
    IKCG_work.h[39] = IKCG_params.qup[7];
    IKCG_work.h[40] = IKCG_params.qup[8];
    IKCG_work.h[41] = IKCG_params.qup[9];
    IKCG_work.h[42] = IKCG_params.qup[10];
    IKCG_work.h[43] = IKCG_params.qup[11];
    IKCG_work.h[44] = IKCG_params.qup[12];
    IKCG_work.h[45] = IKCG_params.qup[13];
    IKCG_work.h[46] = IKCG_params.qup[14];
    IKCG_work.h[47] = IKCG_params.qup[15];
    IKCG_work.h[48] = IKCG_params.qup[16];
    IKCG_work.h[49] = IKCG_params.qup[17];
    IKCG_work.h[50] = IKCG_params.qup[18];
    IKCG_work.h[51] = IKCG_params.qup[19];
    IKCG_work.h[52] = IKCG_params.qup[20];
    IKCG_work.h[53] = IKCG_params.qup[21];
    IKCG_work.h[54] = IKCG_params.qup[22];
    IKCG_work.h[55] = IKCG_params.qup[23];
    IKCG_work.h[56] = IKCG_params.qup[24];
    IKCG_work.h[57] = IKCG_params.qup[25];
    IKCG_work.h[58] = IKCG_params.qup[26];
    IKCG_work.h[59] = IKCG_params.qup[27];
    IKCG_work.h[60] = IKCG_params.qup[28];
    IKCG_work.h[61] = IKCG_params.qup[29];
    IKCG_work.h[62] = IKCG_params.qup[30];
    IKCG_work.h[63] = IKCG_params.qup[31];
  }
  void IKCG_fillb(void) {
    IKCG_work.b[0] = IKCG_params.dx[0];
    IKCG_work.b[1] = IKCG_params.dx[1];
    IKCG_work.b[2] = IKCG_params.dx[2];
    IKCG_work.b[3] = IKCG_params.dx[3];
    IKCG_work.b[4] = IKCG_params.dx[4];
    IKCG_work.b[5] = IKCG_params.dx[5];
    IKCG_work.b[6] = IKCG_params.dx[6];
    IKCG_work.b[7] = IKCG_params.dx[7];
    IKCG_work.b[8] = IKCG_params.dx[8];
    IKCG_work.b[9] = IKCG_params.dx[9];
    IKCG_work.b[10] = IKCG_params.dx[10];
    IKCG_work.b[11] = IKCG_params.dx[11];
    IKCG_work.b[12] = IKCG_params.dx[12];
    IKCG_work.b[13] = IKCG_params.dx[13];
    IKCG_work.b[14] = IKCG_params.dx[14];
    IKCG_work.b[15] = IKCG_params.dx[15];
    IKCG_work.b[16] = IKCG_params.dx[16];
    IKCG_work.b[17] = IKCG_params.dx[17];
    IKCG_work.b[18] = IKCG_params.dx[18];
    IKCG_work.b[19] = IKCG_params.dx[19];
    IKCG_work.b[20] = IKCG_params.dx[20];
    IKCG_work.b[21] = IKCG_params.dx[21];
    IKCG_work.b[22] = IKCG_params.dx[22];
    IKCG_work.b[23] = IKCG_params.dx[23];
    IKCG_work.b[24] = IKCG_params.dx[24];
    IKCG_work.b[25] = IKCG_params.dx[25];
    IKCG_work.b[26] = IKCG_params.dx[26];
    IKCG_work.b[27] = IKCG_params.dx[27];
    IKCG_work.b[28] = IKCG_params.dx[28];
    IKCG_work.b[29] = IKCG_params.dx[29];
    IKCG_work.b[30] = IKCG_params.dx[30];
    IKCG_work.b[31] = IKCG_params.dx[31];
    IKCG_work.b[32] = IKCG_params.dx[32];
    IKCG_work.b[33] = IKCG_params.dx[33];
    IKCG_work.b[34] = IKCG_params.dx[34];
    IKCG_work.b[35] = IKCG_params.dx[35];
  }
  void IKCG_pre_ops(void) {
    IKCG_work.quad_568441778176[0] = IKCG_params.qref[0]*IKCG_params.damping[0]*IKCG_params.qref[0]+IKCG_params.qref[1]*IKCG_params.damping[1]*IKCG_params.qref[1]+IKCG_params.qref[2]*IKCG_params.damping[2]*IKCG_params.qref[2]+IKCG_params.qref[3]*IKCG_params.damping[3]*IKCG_params.qref[3]+IKCG_params.qref[4]*IKCG_params.damping[4]*IKCG_params.qref[4]+IKCG_params.qref[5]*IKCG_params.damping[5]*IKCG_params.qref[5]+IKCG_params.qref[6]*IKCG_params.damping[6]*IKCG_params.qref[6]+IKCG_params.qref[7]*IKCG_params.damping[7]*IKCG_params.qref[7]+IKCG_params.qref[8]*IKCG_params.damping[8]*IKCG_params.qref[8]+IKCG_params.qref[9]*IKCG_params.damping[9]*IKCG_params.qref[9]+IKCG_params.qref[10]*IKCG_params.damping[10]*IKCG_params.qref[10]+IKCG_params.qref[11]*IKCG_params.damping[11]*IKCG_params.qref[11]+IKCG_params.qref[12]*IKCG_params.damping[12]*IKCG_params.qref[12]+IKCG_params.qref[13]*IKCG_params.damping[13]*IKCG_params.qref[13]+IKCG_params.qref[14]*IKCG_params.damping[14]*IKCG_params.qref[14]+IKCG_params.qref[15]*IKCG_params.damping[15]*IKCG_params.qref[15]+IKCG_params.qref[16]*IKCG_params.damping[16]*IKCG_params.qref[16]+IKCG_params.qref[17]*IKCG_params.damping[17]*IKCG_params.qref[17]+IKCG_params.qref[18]*IKCG_params.damping[18]*IKCG_params.qref[18]+IKCG_params.qref[19]*IKCG_params.damping[19]*IKCG_params.qref[19]+IKCG_params.qref[20]*IKCG_params.damping[20]*IKCG_params.qref[20]+IKCG_params.qref[21]*IKCG_params.damping[21]*IKCG_params.qref[21]+IKCG_params.qref[22]*IKCG_params.damping[22]*IKCG_params.qref[22]+IKCG_params.qref[23]*IKCG_params.damping[23]*IKCG_params.qref[23]+IKCG_params.qref[24]*IKCG_params.damping[24]*IKCG_params.qref[24]+IKCG_params.qref[25]*IKCG_params.damping[25]*IKCG_params.qref[25]+IKCG_params.qref[26]*IKCG_params.damping[26]*IKCG_params.qref[26]+IKCG_params.qref[27]*IKCG_params.damping[27]*IKCG_params.qref[27]+IKCG_params.qref[28]*IKCG_params.damping[28]*IKCG_params.qref[28]+IKCG_params.qref[29]*IKCG_params.damping[29]*IKCG_params.qref[29]+IKCG_params.qref[30]*IKCG_params.damping[30]*IKCG_params.qref[30]+IKCG_params.qref[31]*IKCG_params.damping[31]*IKCG_params.qref[31]+IKCG_params.qref[32]*IKCG_params.damping[32]*IKCG_params.qref[32]+IKCG_params.qref[33]*IKCG_params.damping[33]*IKCG_params.qref[33]+IKCG_params.qref[34]*IKCG_params.damping[34]*IKCG_params.qref[34]+IKCG_params.qref[35]*IKCG_params.damping[35]*IKCG_params.qref[35]+IKCG_params.qref[36]*IKCG_params.damping[36]*IKCG_params.qref[36]+IKCG_params.qref[37]*IKCG_params.damping[37]*IKCG_params.qref[37];
  }
  