paraboloid_no_slip.cc

#include <cmath>
#include <Eigen/Dense>
#include <gsl/gsl_errno.h>
#include "paraboloid_no_slip.h"

namespace Mechanics { namespace Rattleback { namespace Paraboloid {
namespace NoSlip {
  int ode(double t, const double x[], double dxdt[], void *params)
  {
    using namespace Eigen;
    parameters *p = static_cast<parameters *>(params);
    static const double a = p->a,
                        b = p->b,
                        c = p->c,
                        d = p->d,
                        e = p->e,
                        m = p->m,
                        Ixx = p->Ixx,
                        Iyy = p->Iyy,
                        Izz = p->Izz,
                        Ixy = p->Ixy,
                        Iyz = p->Iyz,
                        Ixz = p->Ixz,
                        g = p->g,
                        s = p->s;
    double z[77];
    Matrix3d M_dyn;
    Vector3d f_dyn;
    Vector3d udot;
    FullPivHouseholderQR<Matrix3d> dec;

    // Intermediate variables for ODE function
    z[0] = sin(x[2]);
    z[1] = cos(x[2]);
    z[2] = pow(a, 2);
    z[3] = cos(x[0]);
    z[4] = sin(x[0]);
    z[5] = sin(x[1]);
    z[6] = pow(b, 2);
    z[7] = cos(x[1]);
    z[8] = -x[0 + 5];
    z[9] = -x[2 + 5];
    z[11] = g*m;
    z[12] = -x[1 + 5];
    z[13] = x[0 + 5]*z[0];
    z[14] = x[2 + 5]*z[1];
    z[15] = 1.0/z[7];
    dxdt[0] = z[15]*(-z[13] + z[14]);
    dxdt[1] = x[0 + 5]*z[1] + x[2 + 5]*z[0];
    dxdt[2] = x[1 + 5] + (z[13] - z[14])*tan(x[1]);
    z[10] = dxdt[2]/2;
    z[16] = 1.0/z[1];
    z[17] = pow(z[5], 2);
    z[18] = pow(z[0], 2);
    z[19] = z[6]/2;
    z[20] = dxdt[1]*z[16];
    z[21] = z[2]/2;
    z[22] = pow(z[16], 2);
    z[23] = pow(z[22], 3.0/2.0);
    z[24] = pow(z[15], 2);
    z[25] = z[1]*z[3];
    z[26] = z[15]*z[5];
    z[27] = dxdt[2]*z[23];
    z[28] = z[0]*z[3];
    z[29] = z[1]*z[7];
    z[30] = z[1]*z[4];
    z[31] = z[0]*z[7];
    z[32] = z[16]*z[20];
    z[33] = z[0]*z[16];
    z[34] = z[0]*z[4];
    z[35] = Ixz*x[0 + 5] + Iyz*x[1 + 5] + Izz*x[2 + 5];
    z[36] = Ixy*x[0 + 5] + Iyy*x[1 + 5] + Iyz*x[2 + 5];
    z[37] = Ixx*x[0 + 5] + Ixy*x[1 + 5] + Ixz*x[2 + 5];
    z[38] = z[17]*z[24];
    z[39] = d + z[21]*z[33];
    z[40] = -z[39];
    z[41] = z[18]*z[2]*z[22];
    z[42] = e - z[16]*z[19]*z[26];
    z[43] = z[16]*z[19]*z[26];
    z[44] = -e + z[43];
    z[45] = z[40];
    z[46] = x[2 + 5]*z[39];
    z[47] = m*z[39];
    z[48] = x[0 + 5]*z[42];
    z[49] = z[12]*z[39];
    z[50] = m*z[42];
    z[51] = m*z[44];
    z[52] = z[42]*z[9];
    z[53] = -z[10]*z[2] - z[10]*z[41];
    z[54] = z[10]*z[2] + z[10]*z[41];
    z[55] = x[2 + 5]*z[54];
    z[56] = z[12]*z[54];
    z[57] = -Ixy - z[40]*z[50];
    z[58] = z[44]*z[8] + z[49];
    z[59] = c - z[18]*z[21]*z[22]/2 - z[19]*z[22]*z[38]/2;
    z[60] = z[18]*z[21]*z[22]/2 + z[19]*z[22]*z[38]/2;
    z[61] = -c + z[60];
    z[62] = x[1 + 5]*z[61];
    z[63] = m*z[61];
    z[64] = z[0]*z[10]*z[22]*z[26]*z[6] + z[19]*z[20]*z[38] + z[19]*z[20];
    z[65] = -Ixz - z[47]*z[59];
    z[66] = -z[64];
    z[67] = -Iyz - z[51]*z[61];
    z[68] = z[66];
    z[69] = z[12]*z[59] + z[52];
    z[70] = z[45]*z[9] + z[61]*z[8];
    z[71] = z[64]*z[7];
    z[72] = -z[0]*z[10]*z[23]*z[38]*z[6] - z[10]*pow(z[18], 3.0/2.0)*z[2]*z[23] - z[10]*z[2]*z[33] - sqrt(z[17])*z[19]*sqrt(z[24])*z[32]*z[38] - z[19]*z[26]*z[32];
    z[73] = z[0]*z[10]*z[23]*z[38]*z[6] + z[10]*pow(z[18], 3.0/2.0)*z[2]*z[23] + z[10]*z[2]*z[33] + sqrt(z[17])*z[19]*sqrt(z[24])*z[32]*z[38] + z[19]*z[26]*z[32];
    z[74] = x[1 + 5]*z[73];
    z[75] = z[73]*z[8];
    z[76] = z[55] + z[75];

    // Kinematic differential equations
    dxdt[3] = -z[4]*z[71] + z[53]*(z[25] - z[34]*z[5]) + z[72]*(z[28] + z[30]*z[5]);
    dxdt[4] = z[3]*z[71] + z[53]*(z[28]*z[5] + z[30]) + z[72]*(-z[25]*z[5] + z[34]);

    // Mass matrix
    M_dyn(0, 0) = -Ixx - m*pow(z[59], 2) - z[42]*z[50];
    M_dyn(0, 1) = z[57];
    M_dyn(0, 2) = z[65];
    M_dyn(1, 0) = z[57];
    M_dyn(1, 1) = -Iyy - m*pow(z[40], 2) - z[61]*z[63];
    M_dyn(1, 2) = z[67];
    M_dyn(2, 0) = z[65];
    M_dyn(2, 1) = z[67];
    M_dyn(2, 2) = -Izz - z[39]*z[47] - z[44]*z[51];

    // Right hand side of dynamic equations
    f_dyn(0) = m*z[59]*(x[2 + 5]*z[69] + z[76] + z[8]*(z[48] + z[49])) + s*x[0 + 5] + x[1 + 5]*z[35] - x[2 + 5]*z[36] - z[11]*(z[29]*z[42] + z[5]*z[59]) + z[50]*(x[0 + 5]*z[66] + x[0 + 5]*(x[0 + 5]*z[59] + z[46]) + z[12]*(-x[1 + 5]*z[59] + z[52]) + z[56]);
    f_dyn(1) = m*z[45]*(x[0 + 5]*z[68] + x[0 + 5]*z[70] + z[12]*(z[52] + z[62]) + z[56]) + s*x[1 + 5] - x[0 + 5]*z[35] + x[2 + 5]*z[37] - z[11]*(z[29]*z[45] - z[31]*z[61]) + z[63]*(x[1 + 5]*(x[1 + 5]*z[45] + z[48]) + z[68]*z[9] + z[74] + z[9]*(-x[2 + 5]*z[45] + z[61]*z[8]));
    f_dyn(2) = s*x[2 + 5] + x[0 + 5]*z[36] - x[1 + 5]*z[37] - z[11]*(-z[31]*z[44] + z[39]*z[5]) + z[47]*(x[2 + 5]*(x[2 + 5]*z[44] + z[62]) + z[76] + z[8]*(-x[0 + 5]*z[44] + z[49])) + z[51]*(x[1 + 5]*z[58] + z[66]*z[9] + z[74] + z[9]*(-x[0 + 5]*z[61] + z[46]));

    // Solve M_dyn*du/dt = f_dyn for du/dt
    dec.compute(M_dyn);
    udot = dec.solve(f_dyn);
    dxdt[5] = udot(0);
    dxdt[6] = udot(1);
    dxdt[7] = udot(2);
    return GSL_SUCCESS;
  }

  int jacobian(double t, const double x[], double dfdx[], double dfdt[], void *params)
  {
    using namespace Eigen;
    parameters *p = static_cast<parameters *>(params);
    const double a = p->a,
                     b = p->b,
                     c = p->c,
                     d = p->d,
                     e = p->e,
                     m = p->m,
                     Ixx = p->Ixx,
                     Iyy = p->Iyy,
                     Izz = p->Izz,
                     Ixy = p->Ixy,
                     Iyz = p->Iyz,
                     Ixz = p->Ixz,
                     g = p->g,
                     s = p->s;
    dfdt[0] = dfdt[1] = dfdt[2] = dfdt[3] = dfdt[4] = dfdt[5] = dfdt[6] = dfdt[7] = 0.0;
    Matrix<double, 8, 8> J; 
    Matrix<double, 3, 3> M_dyn;
    double z[356];
    double dxdt[8];

    // Compute dxdt
    ode(t, x, dxdt, params);

    // Intermediate quantities needed for Jacobian matrix
    z[0] = cos(x[1]);
    z[1] = sin(x[2]);
    z[2] = cos(x[2]);
    z[3] = tan(x[1]);
    z[4] = sin(x[1]);
    z[5] = pow(a, 2);
    z[6] = pow(b, 2);
    z[7] = sin(x[0]);
    z[8] = cos(x[0]);
    z[9] = pow(x[0 + 5], 2);
    z[10] = Izz*x[2 + 5];
    z[11] = Ixy*x[2 + 5];
    z[12] = Iyy*x[1 + 5];
    z[13] = Ixx*x[0 + 5];
    z[14] = Iyz*x[0 + 5];
    z[15] = Ixz*x[1 + 5];
    z[16] = -x[0 + 5];
    z[17] = -x[2 + 5];
    z[18] = -x[1 + 5];
    z[19] = -dxdt[2];
    z[20] = g*m;
    z[21] = Ixy*x[0 + 5];
    z[22] = x[0 + 5]*x[1 + 5];
    z[23] = -m;
    z[24] = 2*x[0 + 5];
    z[25] = 2*x[1 + 5];
    z[26] = Iyz*x[2 + 5];
    z[27] = m*x[0 + 5];
    z[28] = Ixz*x[2 + 5];
    z[29] = 1.0/z[0];
    z[30] = x[0 + 5]*z[1];
    z[31] = x[2 + 5]*z[2];
    z[32] = 1.0/z[2];
    z[33] = pow(z[4], 2);
    z[34] = pow(z[33], 3.0/2.0);
    z[35] = pow(z[1], 2);
    z[36] = pow(z[35], 3.0/2.0);
    z[37] = z[6]/2;
    z[38] = z[5]/2;
    z[39] = dxdt[1]*z[32];
    z[40] = dxdt[2]*z[5];
    z[41] = m*z[29];
    z[42] = dxdt[1]*z[6];
    z[43] = pow(z[32], 2);
    z[44] = pow(z[29], 2);
    z[45] = pow(z[43], 3.0/2.0);
    z[46] = pow(z[44], 3.0/2.0);
    z[47] = pow(z[3], 2) + 1;
    z[48] = dxdt[2]*z[38];
    z[49] = pow(z[45], 4.0/3.0);
    z[50] = z[1]*z[7];
    z[51] = z[2]*z[3];
    z[52] = z[4]*z[8];
    z[53] = z[0]*z[2];
    z[54] = z[1]*z[3];
    z[55] = z[1]*z[32];
    z[56] = z[0]*z[8];
    z[57] = z[1]*z[4];
    z[58] = dxdt[2]*z[45];
    z[59] = z[1]*z[6];
    z[60] = dxdt[2]*z[35];
    z[61] = z[2]*z[4];
    z[62] = z[0]*z[1];
    z[63] = z[29]*z[6];
    z[64] = z[0]*z[7];
    z[65] = -z[29];
    z[66] = z[32]*z[39];
    z[67] = z[39]*z[43];
    z[68] = 2*z[43];
    z[69] = -z[30] + z[31];
    z[70] = z[30] - z[31];
    z[71] = z[37]*z[4];
    z[72] = z[29]*z[32];
    z[73] = z[1]*z[43];
    z[74] = 3*z[49]/2;
    z[75] = -z[43];
    z[76] = z[68]/8;
    z[77] = z[1]*z[45];
    z[78] = x[0 + 5]*z[2] + x[2 + 5]*z[1];
    z[79] = z[2]*z[52];
    z[80] = z[37]*z[39];
    z[81] = z[34]*z[46];
    z[82] = z[36]*z[45];
    z[83] = z[33]*z[44];
    z[84] = z[35]*z[43];
    z[85] = z[38]*z[55];
    z[86] = -z[80];
    z[87] = d + z[85];
    z[88] = -z[87];
    z[89] = z[48]*z[55];
    z[90] = z[3]*z[78];
    z[91] = z[32]*z[83];
    z[92] = z[2]*z[8] - z[4]*z[50];
    z[93] = z[50] - z[79];
    z[94] = e - z[71]*z[72];
    z[95] = x[2 + 5]*z[87];
    z[96] = z[71]*z[72];
    z[97] = -e + z[96];
    z[98] = x[1 + 5]*z[87];
    z[99] = z[19]*z[85];
    z[100] = z[88];
    z[101] = m*z[87];
    z[102] = z[47]*z[70];
    z[103] = z[1]*z[8] + z[61]*z[7];
    z[104] = -z[103];
    z[105] = z[1]*z[52] + z[2]*z[7];
    z[106] = x[2 + 5]*z[94];
    z[107] = z[48]*z[84];
    z[108] = z[100];
    z[109] = x[0 + 5]*z[94];
    z[110] = x[1 + 5]*z[94];
    z[111] = z[48]*z[82];
    z[112] = z[18]*z[87];
    z[113] = m*z[94];
    z[114] = m*z[97];
    z[115] = 2*z[94];
    z[116] = -z[94];
    z[117] = z[108];
    z[118] = z[17]*z[94];
    z[119] = z[29]*z[66]*z[71];
    z[120] = z[19]*z[38]*z[82];
    z[121] = z[18]*z[97];
    z[122] = z[16]*z[97];
    z[123] = -z[108];
    z[124] = -z[104];
    z[125] = m*z[108];
    z[126] = -z[38]*z[84] - z[38];
    z[127] = z[38]*z[84] + z[38];
    z[128] = z[108]*z[17];
    z[129] = z[80]*z[83];
    z[130] = z[65]*z[66]*z[71];
    z[131] = z[108]*z[27];
    z[132] = m*z[117];
    z[133] = x[2 + 5]*z[127];
    z[134] = x[2 + 5]*z[29]*z[37]*z[43]*z[57];
    z[135] = 2*z[24]*z[29]*z[37]*z[57]*z[76];
    z[136] = z[126];
    z[137] = z[83]*z[86];
    z[138] = z[37]*z[66]*z[81];
    z[139] = dxdt[2]*z[29]*z[37]*z[43]*z[57];
    z[140] = -z[107] - z[48];
    z[141] = z[107] + z[48];
    z[142] = z[19]*z[29]*z[37]*z[43]*z[57];
    z[143] = -z[138];
    z[144] = x[2 + 5]*z[141];
    z[145] = z[1]*z[37]*z[58]*z[83];
    z[146] = z[32]*z[37] + z[37]*z[91];
    z[147] = -z[40]*z[55] - z[40]*z[82];
    z[148] = -z[146];
    z[149] = z[40]*z[55] + z[40]*z[82];
    z[150] = z[141]*z[18];
    z[151] = z[19]*z[37]*z[77]*z[83];
    z[152] = z[1]*z[140];
    z[153] = z[148];
    z[154] = x[2 + 5]*z[149];
    z[155] = x[1 + 5]*z[149];
    z[156] = z[153];
    z[157] = z[109] - z[98];
    z[158] = -Ixy - z[113]*z[88];
    z[159] = -z[156];
    z[160] = z[153]*z[27];
    z[161] = z[146]*z[64];
    z[162] = -z[159];
    z[163] = -c + 2*z[35]*z[38]*z[76] + 2*z[37]*z[76]*z[83];
    z[164] = 2*z[35]*z[38]*z[76] + 2*z[37]*z[76]*z[83];
    z[165] = z[16]*(z[109] + z[112]);
    z[166] = c - z[164];
    z[167] = -z[29]*z[43]*z[71] - z[37]*z[43]*z[81];
    z[168] = z[29]*z[43]*z[71] + z[37]*z[43]*z[81];
    z[169] = x[1 + 5]*z[163];
    z[170] = x[2 + 5]*z[163];
    z[171] = x[0 + 5]*z[163];
    z[172] = m*z[163];
    z[173] = m*z[166];
    z[174] = 2*z[163];
    z[175] = x[0 + 5]*z[166];
    z[176] = -z[163];
    z[177] = z[167];
    z[178] = z[166]*z[18];
    z[179] = z[16]*z[163];
    z[180] = x[1 + 5]*z[168];
    z[181] = m*z[168];
    z[182] = -z[167];
    z[183] = z[37]*z[75]*z[81] + z[43]*z[65]*z[71];
    z[184] = x[0 + 5]*x[2 + 5]*z[168];
    z[185] = -x[1 + 5]*z[127] - z[135];
    z[186] = z[185];
    z[187] = m*z[185];
    z[188] = -z[135] - z[136]*z[18];
    z[189] = -z[37]*z[77]*z[83] - z[38]*z[82] - z[85];
    z[190] = z[37]*z[77]*z[83] + z[38]*z[82] + z[85];
    z[191] = -z[171] + z[95];
    z[192] = z[129] + z[139] + z[80];
    z[193] = -z[106] + z[169];
    z[194] = z[189];
    z[195] = -z[192];
    z[196] = x[0 + 5]*z[190];
    z[197] = -Ixz - z[101]*z[166];
    z[198] = -z[189];
    z[199] = x[0 + 5]*z[195];
    z[200] = z[195];
    z[201] = z[118] + z[169];
    z[202] = -Iyz - z[114]*z[163];
    z[203] = z[137] + z[142] + z[86];
    z[204] = z[118] + z[178];
    z[205] = z[17]*z[195];
    z[206] = z[101]*z[16]*z[168];
    z[207] = z[117]*z[17] + z[179];
    z[208] = z[113]*z[185];
    z[209] = z[208];
    z[210] = z[125]*z[185];
    z[211] = -x[2 + 5]*z[148] + z[180];
    z[212] = -x[2 + 5]*z[153] + z[180];
    z[213] = m*z[212];
    z[214] = z[133] - z[196];
    z[215] = dxdt[2]*z[37]*z[73]*z[83] + dxdt[2]*z[37]*z[73] + z[39]*z[4]*z[63] + z[39]*z[6]*z[81];
    z[216] = -z[215];
    z[217] = x[1 + 5]*z[190] + z[134];
    z[218] = m*z[214];
    z[219] = z[134] + z[18]*z[194];
    z[220] = z[133] - z[16]*z[194];
    z[221] = z[216];
    z[222] = z[136]*z[17] - z[196];
    z[223] = m*z[217];
    z[224] = z[162]*z[17] + z[177]*z[18];
    z[225] = dxdt[2]*z[96] + z[129]*z[55] + z[35]*z[4]*z[58]*z[63] + z[55]*z[80];
    z[226] = -z[225];
    z[227] = z[226];
    z[228] = z[103]*z[167] - z[161];
    z[229] = z[146]*z[56] + z[167]*z[93];
    z[230] = z[101]*z[214];
    z[231] = z[114]*z[217];
    z[232] = -z[111] - z[119] - z[138] - z[145] - z[89];
    z[233] = z[111] + z[119] + z[138] + z[145] + z[89];
    z[234] = x[1 + 5]*z[233];
    z[235] = z[16]*z[233];
    z[236] = -3*z[138]*pow(z[34], 1.0/3.0)*pow(z[46], 1.0/3.0) - 4*z[37]*z[66]*z[83] - z[37]*z[66] - z[57]*z[58]*z[63] - z[58]*z[59]*z[81];
    z[237] = 3*z[138]*pow(z[34], 1.0/3.0)*pow(z[46], 1.0/3.0) + 4*z[37]*z[66]*z[83] + z[37]*z[66] + z[57]*z[58]*z[63] + z[58]*z[59]*z[81];
    z[238] = x[1 + 5]*z[237];
    z[239] = z[172]*z[212];
    z[240] = z[163]*z[214]*z[23];
    z[241] = z[172]*z[217];
    z[242] = z[120] + z[130] + z[143] + z[151] + z[98] + z[99];
    z[243] = z[103]*z[189] + z[126]*z[92] - z[43]*z[50]*z[71];
    z[244] = z[148]*z[27]*z[94] + z[16]*z[168]*z[173];
    z[245] = z[105]*z[126] + z[189]*z[93] + z[37]*z[52]*z[73];
    z[246] = z[144] + z[235];
    z[247] = -dxdt[2]*z[37]*z[43]*z[83] - 16*z[35]*z[48]*z[76] - 2*pow(z[36], 4.0/3.0)*z[48]*z[74] - 2*z[37]*z[60]*z[74]*z[83] - z[48] - z[57]*z[63]*z[67] - z[59]*z[67]*z[81];
    z[248] = dxdt[2]*z[37]*z[43]*z[83] + 16*z[35]*z[48]*z[76] + 2*pow(z[36], 4.0/3.0)*z[48]*z[74] + 2*z[37]*z[60]*z[74]*z[83] + z[48] + z[57]*z[63]*z[67] + z[59]*z[67]*z[81];
    z[249] = x[1 + 5]*z[248];
    z[250] = z[206] + z[211]*z[23]*z[94];
    z[251] = z[184] + z[238];
    z[252] = z[205] + z[234];
    z[253] = z[210] + z[241];
    z[254] = z[230] + z[231];
    z[255] = z[217]*z[23]*z[94] + z[230];

    // Entries of Jacobian matrix
    J(0, 0) = 0;
    J(0, 1) = z[4]*z[44]*z[69];
    J(0, 2) = -z[29]*z[78];
    J(0, 3) = 0;
    J(0, 4) = 0;
    J(0, 5) = z[1]*z[65];
    J(0, 6) = 0;
    J(0, 7) = z[2]*z[29];
    J(1, 0) = 0;
    J(1, 1) = 0;
    J(1, 2) = z[69];
    J(1, 3) = 0;
    J(1, 4) = 0;
    J(1, 5) = z[2];
    J(1, 6) = 0;
    J(1, 7) = z[1];
    J(2, 0) = 0;
    J(2, 1) = z[102];
    J(2, 2) = z[90];
    J(2, 3) = 0;
    J(2, 4) = 0;
    J(2, 5) = z[54];
    J(2, 6) = 1;
    J(2, 7) = -z[51];
    J(3, 0) = -z[105]*z[140] - z[192]*z[56] + z[232]*(-z[50] + z[79]);
    J(3, 1) = -z[0]*z[140]*z[50] + z[102]*z[243] + z[103]*z[236] + z[192]*z[4]*z[7] - z[215]*z[64] + z[232]*z[53]*z[7];
    J(3, 2) = z[104]*z[140] + z[124]*z[247] + z[147]*z[92] - z[225]*z[64] + z[232]*z[92] + z[69]*(z[124]*z[167] - z[161]) + z[90]*(z[124]*z[189] + z[126]*z[92] + z[50]*z[71]*z[75]);
    J(3, 3) = 0;
    J(3, 4) = 0;
    J(3, 5) = z[2]*z[228] + z[243]*z[54];
    J(3, 6) = z[243];
    J(3, 7) = z[1]*z[228] - z[243]*z[51];
    J(4, 0) = z[103]*z[232] + z[140]*z[92] - z[192]*z[64];
    J(4, 1) = z[102]*z[245] + z[152]*z[56] - z[192]*z[52] + z[215]*z[56] - z[232]*z[53]*z[8] + z[236]*z[93];
    J(4, 2) = z[105]*z[147] + z[105]*z[232] - z[140]*z[93] + z[225]*z[56] + z[229]*z[69] + z[245]*z[90] + z[247]*z[93];
    J(4, 3) = 0;
    J(4, 4) = 0;
    J(4, 5) = z[2]*z[229] + z[245]*z[54];
    J(4, 6) = z[245];
    J(4, 7) = z[1]*z[229] - z[245]*z[51];
    J(5, 0) = 0;
    J(5, 1) = m*z[162]*(x[0 + 5]*(z[179] + z[95]) - x[1 + 5]*z[141] - x[1 + 5]*z[193] + z[199]) + m*z[177]*(x[2 + 5]*z[193] + z[165] + z[246]) + z[102]*(z[209] + z[240]) + z[113]*(x[0 + 5]*z[216] + z[177]*z[9] + z[18]*(-x[1 + 5]*z[177] + z[162]*z[17])) + z[163]*z[23]*(x[2 + 5]*z[224] + z[16]*z[237] - z[162]*z[9]) - z[20]*(z[0]*z[176] + z[116]*z[61] + z[162]*z[53] + z[177]*z[4]);
    J(5, 2) = m*z[194]*(x[2 + 5]*z[204] + z[165] + z[246]) + z[113]*(x[0 + 5]*z[220] + x[0 + 5]*z[226] - x[1 + 5]*z[219] - z[155]) + z[173]*(x[2 + 5]*z[219] + z[154] + z[16]*z[186] + z[16]*z[248]) - z[20]*(z[116]*z[62] + z[194]*z[4] - z[55]*z[71]) + z[244]*z[69] - z[37]*z[41]*z[43]*z[57]*(x[0 + 5]*(x[0 + 5]*(c - 2*z[35]*z[38]*z[76] - z[6]*z[76]*z[83]) + z[95]) + x[0 + 5]*(-2*dxdt[2]*z[29]*z[4]*z[59]*z[76] - z[39]*z[6]*z[83]/2 - z[39]*z[6]/2) + z[150] + z[18]*(-x[1 + 5]*(c - 2*z[35]*z[38]*z[76] - z[6]*z[76]*z[83]) + z[17]*(e - z[32]*z[4]*z[63]/2))) + z[90]*(z[113]*z[186] + z[173]*z[220]);
    J(5, 3) = 0;
    J(5, 4) = 0;
    J(5, 5) = s - z[11] + z[113]*(z[166]*z[24] + z[203] + z[95]) + z[15] + z[173]*(z[115]*z[16] + z[242]) + z[2]*z[244] + z[54]*(z[173]*z[214] + z[209]);
    J(5, 6) = Ixz*x[0 + 5] - Iyy*x[2 + 5] + Iyz*z[25] + z[10] + z[113]*(z[106] + z[174]*z[18] + z[19]*z[38]*z[84] + z[19]*z[38]) + z[163]*z[23]*(-x[0 + 5]*z[88] + z[170]) + z[209] + z[240];
    J(5, 7) = Izz*x[1 + 5] + z[1]*(z[148]*z[27]*z[94] - z[166]*z[168]*z[27]) + z[113]*(x[0 + 5]*z[87] + z[110]) - z[12] + z[173]*(z[115]*z[17] + z[141] + z[178]) - z[21] - 2*z[26] - z[51]*(z[173]*z[214] + z[209]);
    J(6, 0) = 0;
    J(6, 1) = z[102]*z[253] + z[125]*(x[0 + 5]*z[221] - z[168]*z[9] + z[18]*(z[153]*z[17] + z[180])) + z[172]*(-x[2 + 5]*z[221] + z[153]*z[22] + z[251]) + z[181]*(x[1 + 5]*(x[1 + 5]*z[108] + z[109]) + z[17]*(x[2 + 5]*z[123] + z[179]) + z[252]) - z[20]*(z[123]*z[61] + z[163]*z[57] - z[168]*z[62]);
    J(6, 2) = m*z[136]*(x[0 + 5]*z[200] + x[0 + 5]*z[207] + z[150] + z[18]*z[201]) + m*z[190]*(x[1 + 5]*(x[1 + 5]*z[117] + z[109]) + z[17]*z[200] + z[17]*(-x[2 + 5]*z[117] + z[179]) + z[234]) + z[132]*(x[0 + 5]*z[222] + x[0 + 5]*z[227] + z[149]*z[18] + z[18]*z[217]) + z[172]*(x[1 + 5]*z[188] + z[17]*z[227] + z[17]*(-x[2 + 5]*z[136] - z[196]) + z[249]) - z[20]*(-z[117]*z[62] + z[136]*z[53] + z[176]*z[53] - z[190]*z[62]) + z[69]*(z[117]*z[160] + z[239]) + z[90]*(z[132]*z[188] + z[241]);
    J(6, 3) = 0;
    J(6, 4) = 0;
    J(6, 5) = Ixx*x[2 + 5] - Ixz*z[24] - Iyz*x[1 + 5] - z[10] + z[125]*(z[128] + z[16]*z[174] + z[203]) + z[172]*(z[110] + z[170]) + z[2]*(z[131]*z[153] + z[239]) + z[253]*z[54];
    J(6, 6) = s + z[11] + z[125]*(z[106] - z[141] - z[163]*z[25]) - z[14] + z[172]*(z[108]*z[25] + z[109] + z[233]) + z[253];
    J(6, 7) = Ixy*x[1 + 5] - Izz*x[0 + 5] + z[1]*(z[131]*z[153] + z[239]) + z[125]*(z[108]*z[16] + z[121]) + z[13] + z[172]*(2*x[2 + 5]*z[108] + z[171] + z[192]) - z[253]*z[51] + 2*z[28];
    J(7, 0) = 0;
    J(7, 1) = z[101]*(-x[0 + 5]*z[237] + x[2 + 5]*(-x[2 + 5]*z[156] + z[180]) - z[156]*z[9]) + z[102]*z[254] + z[114]*(z[156]*z[22] + z[17]*z[216] + z[251]) + z[156]*z[23]*(x[1 + 5]*(z[112] + z[122]) + z[17]*z[191] + z[252]) - z[20]*(z[0]*z[87] + z[156]*z[62] + z[57]*z[97]);
    J(7, 2) = m*z[127]*(x[2 + 5]*z[201] + z[157]*z[16] + z[246]) + z[101]*(-x[0 + 5]*z[186] - x[0 + 5]*z[248] + x[2 + 5]*z[217] + z[154]) - z[20]*(z[127]*z[4] + z[35]*z[71]*z[75] + z[53]*z[94]) + z[23]*z[94]*(x[1 + 5]*z[186] + z[17]*z[214] + z[17]*z[226] + z[249]) + z[250]*z[69] + z[255]*z[90] + z[37]*z[41]*z[43]*z[57]*(x[1 + 5]*(x[0 + 5]*(e - z[32]*z[4]*z[63]/2) - z[98]) + x[1 + 5]*(z[111] + z[4]*z[63]*z[66]/2 + z[58]*z[59]*z[83]/2 + z[6]*z[66]*z[81]/2 + z[89]) - x[2 + 5]*(-x[0 + 5]*(-c + 2*z[35]*z[38]*z[76] + z[6]*z[76]*z[83]) + z[95]) - x[2 + 5]*(-2*dxdt[2]*z[29]*z[4]*z[59]*z[76] - z[39]*z[6]*z[83]/2 - z[39]*z[6]/2));
    J(7, 3) = 0;
    J(7, 4) = 0;
    J(7, 5) = -Ixx*x[1 + 5] + z[101]*(z[24]*z[97] + z[242]) + z[114]*(z[121] + z[166]*z[17]) + z[12] + z[2]*(z[114]*z[211] + z[206]) + 2*z[21] + z[254]*z[54] + z[26];
    J(7, 6) = -Ixy*z[25] + Iyy*x[0 + 5] + z[100]*z[214]*z[23] + z[100]*z[23]*(z[100]*z[16] + z[170]) + z[114]*(z[100]*z[25] + z[122] + z[233]) - z[13] + z[231] - z[28];
    J(7, 7) = s + z[1]*z[250] + z[101]*(-z[106] + z[141] + z[201]) + z[14] - z[15] + z[23]*z[94]*(2*z[17]*z[87] + z[171] + z[192]) - z[255]*z[51];

    // Entries of Mass matrix
    M_dyn(0, 0) = -Ixx - z[113]*z[94] - z[166]*z[173];
    M_dyn(0, 1) = z[158];
    M_dyn(0, 2) = z[197];
    M_dyn(1, 0) = z[158];
    M_dyn(1, 1) = -Iyy - m*pow(z[88], 2) - z[163]*z[172];
    M_dyn(1, 2) = z[202];
    M_dyn(2, 0) = z[197];
    M_dyn(2, 1) = z[202];
    M_dyn(2, 2) = -Izz - z[101]*z[87] - z[114]*z[97];

    J.bottomRows(3) = M_dyn.fullPivLu().solve(J.bottomRows(3));

    for (int i = 0; i < 8; ++i)
      for (int j = 0; j < 8; ++j)
        dfdx[8*i + j] = J(i, j);
    
    return GSL_SUCCESS;
  }

  void outputs(simdata *sd, parameters *p)
  {
    // Compute dxdt
    ode(sd->t, sd->x, sd->dxdt, static_cast<void *>(p));

    // Pointers to state and state derivative vectors
    const double * const x = sd->x;
    const double * const dxdt = sd->dxdt;

    // Parameters
    const double a = p->a,
                     b = p->b,
                     c = p->c,
                     d = p->d,
                     e = p->e,
                     m = p->m,
                     Ixx = p->Ixx,
                     Iyy = p->Iyy,
                     Izz = p->Izz,
                     Ixy = p->Ixy,
                     Iyz = p->Iyz,
                     Ixz = p->Ixz,
                     g = p->g;
    double z[69];

    // Output quantites (evaluated at each output time-step)
    z[0] = sin(x[2]);
    z[1] = pow(b, 2);
    z[2] = cos(x[2]);
    z[3] = cos(x[1]);
    z[4] = sin(x[1]);
    z[5] = pow(a, 2);
    z[6] = -x[0 + 5];
    z[7] = -x[1 + 5];
    z[8] = m*dxdt[2 + 5];
    z[9] = g*m;
    z[10] = -x[2 + 5];
    z[11] = m*dxdt[0 + 5];
    z[12] = m*dxdt[1 + 5];
    z[13] = 1.0/z[2];
    z[14] = pow(z[4], 2);
    z[15] = pow(z[0], 2);
    z[16] = 1.0/z[3];
    z[17] = z[1]/2;
    z[18] = m*z[0];
    z[19] = m*z[4];
    z[20] = dxdt[1]*z[13];
    z[21] = z[5]/2;
    z[22] = pow(z[16], 2);
    z[23] = pow(z[13], 2);
    z[24] = pow(z[23], 3.0/2.0);
    z[25] = z[16]*z[4];
    z[26] = dxdt[2]*z[24];
    z[27] = z[2]*z[3];
    z[28] = dxdt[2]*z[23];
    z[29] = z[13]*z[20];
    z[30] = z[0]*z[13];
    z[31] = z[23]/4;
    z[32] = z[14]*z[22];
    z[33] = -d - z[21]*z[30];
    z[34] = d + z[21]*z[30];
    z[35] = e - z[13]*z[17]*z[25];
    z[36] = x[1 + 5]*z[33];
    z[37] = x[0 + 5]*z[35];
    z[38] = z[10]*z[33];
    z[39] = z[35]*z[4];
    z[40] = z[0]*z[3]*z[34];
    z[41] = dxdt[2]*z[21] + z[15]*z[21]*z[28];
    z[42] = z[41]*z[7];
    z[43] = -z[33]*z[7] + z[37];
    z[44] = -c + 2*z[15]*z[21]*z[31] + 2*z[17]*z[31]*z[32];
    z[45] = -z[11]*z[35] - z[12]*z[33];
    z[46] = x[1 + 5]*z[44];
    z[47] = m*z[44];
    z[48] = z[44]*z[6];
    z[49] = z[27]*z[44];
    z[50] = -x[0 + 5]*z[44] + z[38];
    z[51] = z[38] + z[48];
    z[52] = -z[0]*z[17]*z[25]*z[28] - z[17]*z[20]*z[32] - z[17]*z[20];
    z[53] = z[10]*z[35] + z[46];
    z[54] = x[0 + 5]*z[52];
    z[55] = z[11]*z[44] + z[33]*z[8];
    z[56] = x[2 + 5]*z[52];
    z[57] = z[53]*z[7];
    z[58] = -z[12]*z[44] + z[35]*z[8];
    z[59] = z[39] + z[49];
    z[60] = z[0]*z[58];
    z[61] = dxdt[2]*z[21]*z[30] + z[0]*z[17]*z[26]*z[32] + sqrt(z[14])*z[17]*sqrt(z[22])*z[29]*z[32] + pow(z[15], 3.0/2.0)*z[21]*z[26] + z[17]*z[25]*z[29];
    z[62] = x[1 + 5]*z[61];
    z[63] = z[42] + z[54] + z[57];
    z[64] = 0.5*m*(pow(x[2 + 5]*z[34] + z[48], 2) + pow(-x[2 + 5]*z[35] + z[46], 2) + pow(z[34]*z[7] + z[37], 2)) + 0.5*x[0 + 5]*(Ixx*x[0 + 5] + Ixy*x[1 + 5] + Ixz*x[2 + 5]) + 0.5*x[1 + 5]*(Ixy*x[0 + 5] + Iyy*x[1 + 5] + Iyz*x[2 + 5]) + 0.5*x[2 + 5]*(Ixz*x[0 + 5] + Iyz*x[1 + 5] + Izz*x[2 + 5]);
    z[65] = x[0 + 5]*z[50] + z[63];
    z[66] = m*z[65];
    z[67] = x[2 + 5]*z[41] + x[2 + 5]*z[53] + z[43]*z[6] + z[6]*z[61];
    z[68] = x[1 + 5]*z[43] - x[2 + 5]*z[50] - z[56] + z[62];

    // Contact forces
    sd->CF[0] = m*z[2]*(x[1 + 5]*(z[36] + z[37]) + z[10]*z[52] + z[10]*(-x[2 + 5]*z[33] + z[48]) + z[62]) - z[0]*z[45] + z[18]*(x[0 + 5]*z[51] + z[63]) - z[2]*z[58];
    sd->CF[1] = m*z[3]*z[67] + z[18]*z[4]*z[68] - z[19]*z[2]*z[65] + z[2]*z[4]*z[45] - z[3]*z[55] - z[4]*z[60];
    sd->CF[2] = -z[18]*z[3]*z[68] + z[19]*z[67] - z[27]*z[45] + z[27]*z[66] + z[3]*z[60] - z[4]*z[55] - z[9];

    // Mechanical energy
    sd->ke = z[64];
    sd->pe = -z[9]*(-z[40] + z[59]);
    sd->te = sd->ke + sd->pe;
    // Tilt of Rattleback with respect to vertical
    sd->delta = acos(z[27]);
    

    // State time derivatives
    for (int i = 0; i < 8; ++i)
      sd->dxdt[i] = dxdt[i];
  }
}}}}