-
Notifications
You must be signed in to change notification settings - Fork 0
/
T_dt_dt_f.c
16010 lines (16009 loc) · 495 KB
/
T_dt_dt_f.c
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
521
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
560
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
640
641
642
643
644
645
646
647
648
649
650
651
652
653
654
655
656
657
658
659
660
661
662
663
664
665
666
667
668
669
670
671
672
673
674
675
676
677
678
679
680
681
682
683
684
685
686
687
688
689
690
691
692
693
694
695
696
697
698
699
700
701
702
703
704
705
706
707
708
709
710
711
712
713
714
715
716
717
718
719
720
721
722
723
724
725
726
727
728
729
730
731
732
733
734
735
736
737
738
739
740
741
742
743
744
745
746
747
748
749
750
751
752
753
754
755
756
757
758
759
760
761
762
763
764
765
766
767
768
769
770
771
772
773
774
775
776
777
778
779
780
781
782
783
784
785
786
787
788
789
790
791
792
793
794
795
796
797
798
799
800
801
802
803
804
805
806
807
808
809
810
811
812
813
814
815
816
817
818
819
820
821
822
823
824
825
826
827
828
829
830
831
832
833
834
835
836
837
838
839
840
841
842
843
844
845
846
847
848
849
850
851
852
853
854
855
856
857
858
859
860
861
862
863
864
865
866
867
868
869
870
871
872
873
874
875
876
877
878
879
880
881
882
883
884
885
886
887
888
889
890
891
892
893
894
895
896
897
898
899
900
901
902
903
904
905
906
907
908
909
910
911
912
913
914
915
916
917
918
919
920
921
922
923
924
925
926
927
928
929
930
931
932
933
934
935
936
937
938
939
940
941
942
943
944
945
946
947
948
949
950
951
952
953
954
955
956
957
958
959
960
961
962
963
964
965
966
967
968
969
970
971
972
973
974
975
976
977
978
979
980
981
982
983
984
985
986
987
988
989
990
991
992
993
994
995
996
997
998
999
1000
#include <stdio.h>
#include <math.h>
#include "parameters.h"
void T_dt_dt_f(double* state_var, double* T_dt_dt)
{
double theta=state_var[0];
double phi=state_var[1];
double psi=state_var[2];
double X=state_var[3];
double Y=state_var[4];
double Z=state_var[5];
double p10_b=state_var[6];
double p9_b=state_var[7];
double p8_b=state_var[8];
double p7_b=state_var[9];
double p6_b=state_var[10];
double p5_b=state_var[11];
double p4_b=state_var[12];
double p3_b=state_var[13];
double p2_b=state_var[14];
double p1_b=state_var[15];
double q1_b=state_var[16];
double q2_b=state_var[17];
double q3_b=state_var[18];
double q4_b=state_var[19];
double q5_b=state_var[20];
double q6_b=state_var[21];
double q7_b=state_var[22];
double q8_b=state_var[23];
double q9_b=state_var[24];
double q10_b=state_var[25];
double p10_b_dot=state_var[26];
double p9_b_dot=state_var[27];
double p8_b_dot=state_var[28];
double p7_b_dot=state_var[29];
double p6_b_dot=state_var[30];
double p5_b_dot=state_var[31];
double p4_b_dot=state_var[32];
double p3_b_dot=state_var[33];
double p2_b_dot=state_var[34];
double p1_b_dot=state_var[35];
double q1_b_dot=state_var[36];
double q2_b_dot=state_var[37];
double q3_b_dot=state_var[38];
double q4_b_dot=state_var[39];
double q5_b_dot=state_var[40];
double q6_b_dot=state_var[41];
double q7_b_dot=state_var[42];
double q8_b_dot=state_var[43];
double q9_b_dot=state_var[44];
double q10_b_dot=state_var[45];
double p10_t=state_var[46];
double p9_t=state_var[47];
double p8_t=state_var[48];
double p7_t=state_var[49];
double p6_t=state_var[50];
double p5_t=state_var[51];
double p4_t=state_var[52];
double p3_t=state_var[53];
double p2_t=state_var[54];
double p1_t=state_var[55];
double q1_t=state_var[56];
double q2_t=state_var[57];
double q3_t=state_var[58];
double q4_t=state_var[59];
double q5_t=state_var[60];
double q6_t=state_var[61];
double q7_t=state_var[62];
double q8_t=state_var[63];
double q9_t=state_var[64];
double q10_t=state_var[65];
double p10_i=state_var[66];
double p9_i=state_var[67];
double p8_i=state_var[68];
double p7_i=state_var[69];
double p6_i=state_var[70];
double p5_i=state_var[71];
double p4_i=state_var[72];
double p3_i=state_var[73];
double p2_i=state_var[74];
double p1_i=state_var[75];
double q1_i=state_var[76];
double q2_i=state_var[77];
double q3_i=state_var[78];
double q4_i=state_var[79];
double q5_i=state_var[80];
double q6_i=state_var[81];
double q7_i=state_var[82];
double q8_i=state_var[83];
double q9_i=state_var[84];
double q10_i=state_var[85];
double p10_i_dot=state_var[86];
double p9_i_dot=state_var[87];
double p8_i_dot=state_var[88];
double p7_i_dot=state_var[89];
double p6_i_dot=state_var[90];
double p5_i_dot=state_var[91];
double p4_i_dot=state_var[92];
double p3_i_dot=state_var[93];
double p2_i_dot=state_var[94];
double p1_i_dot=state_var[95];
double q1_i_dot=state_var[96];
double q2_i_dot=state_var[97];
double q3_i_dot=state_var[98];
double q4_i_dot=state_var[99];
double q5_i_dot=state_var[100];
double q6_i_dot=state_var[101];
double q7_i_dot=state_var[102];
double q8_i_dot=state_var[103];
double q9_i_dot=state_var[104];
double q10_i_dot=state_var[105];
double theta_dt=state_var[106];
double phi_dt=state_var[107];
double psi_dt=state_var[108];
double X_dt=state_var[109];
double Y_dt=state_var[110];
double Z_dt=state_var[111];
double p10_b_dt=state_var[112];
double p9_b_dt=state_var[113];
double p8_b_dt=state_var[114];
double p7_b_dt=state_var[115];
double p6_b_dt=state_var[116];
double p5_b_dt=state_var[117];
double p4_b_dt=state_var[118];
double p3_b_dt=state_var[119];
double p2_b_dt=state_var[120];
double p1_b_dt=state_var[121];
double q1_b_dt=state_var[122];
double q2_b_dt=state_var[123];
double q3_b_dt=state_var[124];
double q4_b_dt=state_var[125];
double q5_b_dt=state_var[126];
double q6_b_dt=state_var[127];
double q7_b_dt=state_var[128];
double q8_b_dt=state_var[129];
double q9_b_dt=state_var[130];
double q10_b_dt=state_var[131];
double p10_b_dot_dt=state_var[132];
double p9_b_dot_dt=state_var[133];
double p8_b_dot_dt=state_var[134];
double p7_b_dot_dt=state_var[135];
double p6_b_dot_dt=state_var[136];
double p5_b_dot_dt=state_var[137];
double p4_b_dot_dt=state_var[138];
double p3_b_dot_dt=state_var[139];
double p2_b_dot_dt=state_var[140];
double p1_b_dot_dt=state_var[141];
double q1_b_dot_dt=state_var[142];
double q2_b_dot_dt=state_var[143];
double q3_b_dot_dt=state_var[144];
double q4_b_dot_dt=state_var[145];
double q5_b_dot_dt=state_var[146];
double q6_b_dot_dt=state_var[147];
double q7_b_dot_dt=state_var[148];
double q8_b_dot_dt=state_var[149];
double q9_b_dot_dt=state_var[150];
double q10_b_dot_dt=state_var[151];
double p10_t_dt=state_var[152];
double p9_t_dt=state_var[153];
double p8_t_dt=state_var[154];
double p7_t_dt=state_var[155];
double p6_t_dt=state_var[156];
double p5_t_dt=state_var[157];
double p4_t_dt=state_var[158];
double p3_t_dt=state_var[159];
double p2_t_dt=state_var[160];
double p1_t_dt=state_var[161];
double q1_t_dt=state_var[162];
double q2_t_dt=state_var[163];
double q3_t_dt=state_var[164];
double q4_t_dt=state_var[165];
double q5_t_dt=state_var[166];
double q6_t_dt=state_var[167];
double q7_t_dt=state_var[168];
double q8_t_dt=state_var[169];
double q9_t_dt=state_var[170];
double q10_t_dt=state_var[171];
double p10_i_dt=state_var[172];
double p9_i_dt=state_var[173];
double p8_i_dt=state_var[174];
double p7_i_dt=state_var[175];
double p6_i_dt=state_var[176];
double p5_i_dt=state_var[177];
double p4_i_dt=state_var[178];
double p3_i_dt=state_var[179];
double p2_i_dt=state_var[180];
double p1_i_dt=state_var[181];
double q1_i_dt=state_var[182];
double q2_i_dt=state_var[183];
double q3_i_dt=state_var[184];
double q4_i_dt=state_var[185];
double q5_i_dt=state_var[186];
double q6_i_dt=state_var[187];
double q7_i_dt=state_var[188];
double q8_i_dt=state_var[189];
double q9_i_dt=state_var[190];
double q10_i_dt=state_var[191];
double p10_i_dot_dt=state_var[192];
double p9_i_dot_dt=state_var[193];
double p8_i_dot_dt=state_var[194];
double p7_i_dot_dt=state_var[195];
double p6_i_dot_dt=state_var[196];
double p5_i_dot_dt=state_var[197];
double p4_i_dot_dt=state_var[198];
double p3_i_dot_dt=state_var[199];
double p2_i_dot_dt=state_var[200];
double p1_i_dot_dt=state_var[201];
double q1_i_dot_dt=state_var[202];
double q2_i_dot_dt=state_var[203];
double q3_i_dot_dt=state_var[204];
double q4_i_dot_dt=state_var[205];
double q5_i_dot_dt=state_var[206];
double q6_i_dot_dt=state_var[207];
double q7_i_dot_dt=state_var[208];
double q8_i_dot_dt=state_var[209];
double q9_i_dot_dt=state_var[210];
double q10_i_dot_dt=state_var[211];
double x0 = sin(phi);
double x1 = pow(x0, 2);
double x2 = MOI*x1;
double x3 = cos(phi);
double x4 = pow(x3, 2);
double x5 = MOI*x4;
double x6 = pow(L, 3);
double x7 = c*m;
double x8 = x6*x7;
double x9 = (2000.0/3.0)*x8;
double x10 = L*x7;
double x11 = (9.0/35.0)*x10;
double x12 = p9_b*x11;
double x13 = p10_b*x12;
double x14 = p10_i*p9_i;
double x15 = x1*x11;
double x16 = x14*x15;
double x17 = x11*x4;
double x18 = p2_b*x11;
double x19 = p1_b*x18;
double x20 = p1_i*p2_i;
double x21 = x15*x20;
double x22 = p3_b*x18;
double x23 = p2_i*p3_i;
double x24 = x15*x23;
double x25 = p4_b*x11;
double x26 = p3_b*x25;
double x27 = p3_i*p4_i;
double x28 = x15*x27;
double x29 = p5_b*x25;
double x30 = p4_i*p5_i;
double x31 = x15*x30;
double x32 = p6_b*x11;
double x33 = p5_b*x32;
double x34 = p5_i*p6_i;
double x35 = x15*x34;
double x36 = p7_b*x32;
double x37 = p6_i*p7_i;
double x38 = x15*x37;
double x39 = p7_b*x11;
double x40 = p8_b*x39;
double x41 = p7_i*p8_i;
double x42 = x15*x41;
double x43 = p8_b*x12;
double x44 = p8_i*p9_i;
double x45 = x15*x44;
double x46 = q9_b*x11;
double x47 = q10_b*x46;
double x48 = q10_i*q9_i;
double x49 = x15*x48;
double x50 = q2_b*x11;
double x51 = q1_b*x50;
double x52 = q1_i*q2_i;
double x53 = x15*x52;
double x54 = q3_b*x50;
double x55 = q2_i*q3_i;
double x56 = x15*x55;
double x57 = q4_b*x11;
double x58 = q3_b*x57;
double x59 = q3_i*q4_i;
double x60 = x15*x59;
double x61 = q5_b*x57;
double x62 = q4_i*q5_i;
double x63 = x15*x62;
double x64 = q6_b*x11;
double x65 = q5_b*x64;
double x66 = q5_i*q6_i;
double x67 = x15*x66;
double x68 = q7_b*x64;
double x69 = q6_i*q7_i;
double x70 = x15*x69;
double x71 = q7_b*x11;
double x72 = q8_b*x71;
double x73 = q7_i*q8_i;
double x74 = x15*x73;
double x75 = q8_b*x46;
double x76 = q8_i*q9_i;
double x77 = x15*x76;
double x78 = (13.0/35.0)*x10;
double x79 = pow(p10_b, 2)*x78;
double x80 = pow(p10_i, 2)*x78;
double x81 = x1*x80;
double x82 = x4*x80;
double x83 = (26.0/35.0)*x10;
double x84 = pow(p1_b, 2)*x83;
double x85 = pow(p1_i, 2)*x83;
double x86 = x1*x85;
double x87 = x4*x85;
double x88 = pow(p2_b, 2)*x83;
double x89 = pow(p2_i, 2)*x83;
double x90 = x1*x89;
double x91 = x4*x89;
double x92 = pow(p3_b, 2)*x83;
double x93 = pow(p3_i, 2)*x83;
double x94 = x1*x93;
double x95 = x4*x93;
double x96 = pow(p4_b, 2)*x83;
double x97 = pow(p4_i, 2)*x83;
double x98 = x1*x97;
double x99 = x4*x97;
double x100 = pow(p5_b, 2)*x83;
double x101 = pow(p5_i, 2)*x83;
double x102 = x1*x101;
double x103 = x101*x4;
double x104 = pow(p6_b, 2)*x83;
double x105 = pow(p6_i, 2)*x83;
double x106 = x1*x105;
double x107 = x105*x4;
double x108 = pow(p7_b, 2)*x83;
double x109 = pow(p7_i, 2)*x83;
double x110 = x1*x109;
double x111 = x109*x4;
double x112 = pow(p8_b, 2)*x83;
double x113 = pow(p8_i, 2)*x83;
double x114 = x1*x113;
double x115 = x113*x4;
double x116 = pow(p9_b, 2)*x83;
double x117 = pow(p9_i, 2)*x83;
double x118 = x1*x117;
double x119 = x117*x4;
double x120 = pow(q10_b, 2)*x78;
double x121 = pow(q10_i, 2)*x78;
double x122 = x1*x121;
double x123 = x121*x4;
double x124 = pow(q1_b, 2)*x83;
double x125 = pow(q1_i, 2)*x83;
double x126 = x1*x125;
double x127 = x125*x4;
double x128 = pow(q2_b, 2)*x83;
double x129 = pow(q2_i, 2)*x83;
double x130 = x1*x129;
double x131 = x129*x4;
double x132 = pow(q3_b, 2)*x83;
double x133 = pow(q3_i, 2)*x83;
double x134 = x1*x133;
double x135 = x133*x4;
double x136 = pow(q4_b, 2)*x83;
double x137 = pow(q4_i, 2)*x83;
double x138 = x1*x137;
double x139 = x137*x4;
double x140 = pow(q5_b, 2)*x83;
double x141 = pow(q5_i, 2)*x83;
double x142 = x1*x141;
double x143 = x141*x4;
double x144 = pow(q6_b, 2)*x83;
double x145 = pow(q6_i, 2)*x83;
double x146 = x1*x145;
double x147 = x145*x4;
double x148 = pow(q7_b, 2)*x83;
double x149 = pow(q7_i, 2)*x83;
double x150 = x1*x149;
double x151 = x149*x4;
double x152 = pow(q8_b, 2)*x83;
double x153 = pow(q8_i, 2)*x83;
double x154 = x1*x153;
double x155 = x153*x4;
double x156 = pow(q9_b, 2)*x83;
double x157 = pow(q9_i, 2)*x83;
double x158 = x1*x157;
double x159 = x157*x4;
double x160 = pow(L, 2);
double x161 = m*x160;
double x162 = c*x161;
double x163 = x0*x162;
double x164 = x163*x3;
double x165 = (97.0/10.0)*x164;
double x166 = p10_b*x165;
double x167 = 2*x163;
double x168 = x167*x3;
double x169 = p1_b*x168;
double x170 = 4*x163;
double x171 = x170*x3;
double x172 = p2_b*x171;
double x173 = 6*x163;
double x174 = x173*x3;
double x175 = p3_b*x174;
double x176 = 8*x163;
double x177 = x176*x3;
double x178 = p4_b*x177;
double x179 = 10*x164;
double x180 = p5_b*x179;
double x181 = 12*x164;
double x182 = p6_b*x181;
double x183 = 14*x164;
double x184 = p7_b*x183;
double x185 = 16*x164;
double x186 = p8_b*x185;
double x187 = 18*x164;
double x188 = p9_b*x187;
double x189 = q10_b*x165;
double x190 = q1_b*x168;
double x191 = q2_b*x171;
double x192 = q3_b*x174;
double x193 = q4_b*x177;
double x194 = q5_b*x179;
double x195 = q6_b*x181;
double x196 = q7_b*x183;
double x197 = q8_b*x185;
double x198 = q9_b*x187;
double x199 = p10_b_dot*x0;
double x200 = x3*x8;
double x201 = (8.0/5.0)*x200;
double x202 = x199*x201;
double x203 = p1_b_dot*x0;
double x204 = (2.0/15.0)*x200;
double x205 = x203*x204;
double x206 = x0*x204;
double x207 = p2_b_dot*x206;
double x208 = p3_b_dot*x206;
double x209 = p4_b_dot*x206;
double x210 = p5_b_dot*x206;
double x211 = p6_b_dot*x206;
double x212 = p7_b_dot*x206;
double x213 = p8_b_dot*x206;
double x214 = p9_b_dot*x206;
double x215 = q10_b_dot*x0;
double x216 = x201*x215;
double x217 = q1_b_dot*x206;
double x218 = q2_b_dot*x206;
double x219 = q3_b_dot*x206;
double x220 = q4_b_dot*x206;
double x221 = q5_b_dot*x206;
double x222 = q6_b_dot*x206;
double x223 = q7_b_dot*x206;
double x224 = q8_b_dot*x206;
double x225 = q9_b_dot*x206;
double x226 = (11.0/105.0)*x162;
double x227 = p10_b*p10_b_dot;
double x228 = x226*x227;
double x229 = (13.0/210.0)*x162;
double x230 = p9_b_dot*x229;
double x231 = p10_b*x230;
double x232 = p9_b*x229;
double x233 = p10_b_dot*x232;
double x234 = p10_i_dot*x226;
double x235 = p10_i*x1;
double x236 = x234*x235;
double x237 = p10_i*x4;
double x238 = p10_i*p9_i_dot;
double x239 = x1*x229;
double x240 = x238*x239;
double x241 = x229*x4;
double x242 = p10_i_dot*p9_i;
double x243 = x239*x242;
double x244 = p1_b*x162;
double x245 = (13.0/210.0)*p2_b_dot;
double x246 = x244*x245;
double x247 = p2_b*x229;
double x248 = p1_b_dot*x247;
double x249 = p1_i*p2_i_dot;
double x250 = x239*x249;
double x251 = p1_i_dot*p2_i;
double x252 = x239*x251;
double x253 = p3_b_dot*x247;
double x254 = p3_b*x229;
double x255 = p2_b_dot*x254;
double x256 = p2_i*p3_i_dot;
double x257 = x239*x256;
double x258 = p2_i_dot*p3_i;
double x259 = x239*x258;
double x260 = p4_b_dot*x254;
double x261 = p4_b*x229;
double x262 = p3_b_dot*x261;
double x263 = p3_i*p4_i_dot;
double x264 = x239*x263;
double x265 = p3_i_dot*p4_i;
double x266 = x239*x265;
double x267 = p5_b_dot*x261;
double x268 = p5_b*x229;
double x269 = p4_b_dot*x268;
double x270 = p4_i*p5_i_dot;
double x271 = x239*x270;
double x272 = p4_i_dot*p5_i;
double x273 = x239*x272;
double x274 = p6_b_dot*x268;
double x275 = p6_b*x229;
double x276 = p5_b_dot*x275;
double x277 = p5_i*p6_i_dot;
double x278 = x239*x277;
double x279 = p5_i_dot*p6_i;
double x280 = x239*x279;
double x281 = p7_b_dot*x275;
double x282 = p7_b*x229;
double x283 = p6_b_dot*x282;
double x284 = p6_i*p7_i_dot;
double x285 = x239*x284;
double x286 = p6_i_dot*p7_i;
double x287 = x239*x286;
double x288 = p8_b_dot*x282;
double x289 = p7_b_dot*p8_b;
double x290 = x229*x289;
double x291 = p7_i*p8_i_dot;
double x292 = x239*x291;
double x293 = p7_i_dot*p8_i;
double x294 = x239*x293;
double x295 = p8_b*x230;
double x296 = p8_b_dot*x232;
double x297 = p8_i*p9_i_dot;
double x298 = x239*x297;
double x299 = p8_i_dot*p9_i;
double x300 = x239*x299;
double x301 = q10_b*q10_b_dot;
double x302 = x226*x301;
double x303 = q9_b_dot*x229;
double x304 = q10_b*x303;
double x305 = q9_b*x229;
double x306 = q10_b_dot*x305;
double x307 = q10_i*x226;
double x308 = q10_i_dot*x1;
double x309 = x307*x308;
double x310 = q10_i_dot*x4;
double x311 = q10_i*q9_i_dot;
double x312 = x239*x311;
double x313 = q10_i_dot*q9_i;
double x314 = x239*x313;
double x315 = q1_b*x162;
double x316 = (13.0/210.0)*q2_b_dot;
double x317 = x315*x316;
double x318 = q2_b*x229;
double x319 = q1_b_dot*x318;
double x320 = q1_i*q2_i_dot;
double x321 = x239*x320;
double x322 = q1_i_dot*q2_i;
double x323 = x239*x322;
double x324 = q3_b_dot*x318;
double x325 = q3_b*x229;
double x326 = q2_b_dot*x325;
double x327 = q2_i*q3_i_dot;
double x328 = x239*x327;
double x329 = q2_i_dot*q3_i;
double x330 = x239*x329;
double x331 = q4_b_dot*x325;
double x332 = q4_b*x229;
double x333 = q3_b_dot*x332;
double x334 = q3_i*q4_i_dot;
double x335 = x239*x334;
double x336 = q3_i_dot*q4_i;
double x337 = x239*x336;
double x338 = q5_b_dot*x332;
double x339 = q5_b*x229;
double x340 = q4_b_dot*x339;
double x341 = q4_i*q5_i_dot;
double x342 = x239*x341;
double x343 = q4_i_dot*q5_i;
double x344 = x239*x343;
double x345 = q6_b_dot*x339;
double x346 = q6_b*x229;
double x347 = q5_b_dot*x346;
double x348 = q5_i*q6_i_dot;
double x349 = x239*x348;
double x350 = q5_i_dot*q6_i;
double x351 = x239*x350;
double x352 = q7_b_dot*x346;
double x353 = q7_b*x229;
double x354 = q6_b_dot*x353;
double x355 = q6_i*q7_i_dot;
double x356 = x239*x355;
double x357 = q6_i_dot*q7_i;
double x358 = x239*x357;
double x359 = q8_b_dot*x353;
double x360 = q7_b_dot*q8_b;
double x361 = x229*x360;
double x362 = q7_i*q8_i_dot;
double x363 = x239*x362;
double x364 = q7_i_dot*q8_i;
double x365 = x239*x364;
double x366 = q8_b*x303;
double x367 = q8_b_dot*x305;
double x368 = q8_i*q9_i_dot;
double x369 = x239*x368;
double x370 = q8_i_dot*q9_i;
double x371 = x239*x370;
double x372 = (1.0/70.0)*x8;
double x373 = p9_b_dot*x372;
double x374 = p10_b_dot*x373;
double x375 = p10_i_dot*p9_i_dot;
double x376 = x1*x372;
double x377 = x375*x376;
double x378 = x372*x4;
double x379 = p2_b_dot*x372;
double x380 = p1_b_dot*x379;
double x381 = p1_i_dot*p2_i_dot;
double x382 = x376*x381;
double x383 = p3_b_dot*x379;
double x384 = p2_i_dot*p3_i_dot;
double x385 = x376*x384;
double x386 = p4_b_dot*x372;
double x387 = p3_b_dot*x386;
double x388 = p3_i_dot*p4_i_dot;
double x389 = x376*x388;
double x390 = p5_b_dot*x386;
double x391 = p4_i_dot*p5_i_dot;
double x392 = x376*x391;
double x393 = p6_b_dot*x372;
double x394 = p5_b_dot*x393;
double x395 = p5_i_dot*p6_i_dot;
double x396 = x376*x395;
double x397 = p7_b_dot*x393;
double x398 = p6_i_dot*p7_i_dot;
double x399 = x376*x398;
double x400 = p7_b_dot*x372;
double x401 = p8_b_dot*x400;
double x402 = p7_i_dot*p8_i_dot;
double x403 = x376*x402;
double x404 = p8_b_dot*x373;
double x405 = p8_i_dot*p9_i_dot;
double x406 = x376*x405;
double x407 = q9_b_dot*x372;
double x408 = q10_b_dot*x407;
double x409 = q9_i_dot*x372;
double x410 = x308*x409;
double x411 = q2_b_dot*x372;
double x412 = q1_b_dot*x411;
double x413 = q1_i_dot*q2_i_dot;
double x414 = x376*x413;
double x415 = q3_b_dot*x411;
double x416 = q2_i_dot*q3_i_dot;
double x417 = x376*x416;
double x418 = q4_b_dot*x372;
double x419 = q3_b_dot*x418;
double x420 = q3_i_dot*q4_i_dot;
double x421 = x376*x420;
double x422 = q5_b_dot*x418;
double x423 = q4_i_dot*q5_i_dot;
double x424 = x376*x423;
double x425 = q6_b_dot*x372;
double x426 = q5_b_dot*x425;
double x427 = q5_i_dot*q6_i_dot;
double x428 = x376*x427;
double x429 = q7_b_dot*x425;
double x430 = q6_i_dot*q7_i_dot;
double x431 = x376*x430;
double x432 = q7_b_dot*x372;
double x433 = q8_b_dot*x432;
double x434 = q7_i_dot*q8_i_dot;
double x435 = x376*x434;
double x436 = q8_b_dot*x407;
double x437 = q8_i_dot*q9_i_dot;
double x438 = x376*x437;
double x439 = (1.0/105.0)*x8;
double x440 = pow(p10_b_dot, 2)*x439;
double x441 = pow(p10_i_dot, 2)*x439;
double x442 = x1*x441;
double x443 = x4*x441;
double x444 = (2.0/105.0)*x8;
double x445 = pow(p1_b_dot, 2)*x444;
double x446 = pow(p1_i_dot, 2)*x444;
double x447 = x1*x446;
double x448 = x4*x446;
double x449 = pow(p2_b_dot, 2)*x444;
double x450 = pow(p2_i_dot, 2)*x444;
double x451 = x1*x450;
double x452 = x4*x450;
double x453 = pow(p3_b_dot, 2)*x444;
double x454 = pow(p3_i_dot, 2)*x444;
double x455 = x1*x454;
double x456 = x4*x454;
double x457 = pow(p4_b_dot, 2)*x444;
double x458 = pow(p4_i_dot, 2)*x444;
double x459 = x1*x458;
double x460 = x4*x458;
double x461 = pow(p5_b_dot, 2)*x444;
double x462 = pow(p5_i_dot, 2)*x444;
double x463 = x1*x462;
double x464 = x4*x462;
double x465 = pow(p6_b_dot, 2)*x444;
double x466 = pow(p6_i_dot, 2)*x444;
double x467 = x1*x466;
double x468 = x4*x466;
double x469 = pow(p7_b_dot, 2)*x444;
double x470 = pow(p7_i_dot, 2)*x444;
double x471 = x1*x470;
double x472 = x4*x470;
double x473 = pow(p8_b_dot, 2)*x444;
double x474 = pow(p8_i_dot, 2)*x444;
double x475 = x1*x474;
double x476 = x4*x474;
double x477 = pow(p9_b_dot, 2)*x444;
double x478 = pow(p9_i_dot, 2)*x444;
double x479 = x1*x478;
double x480 = x4*x478;
double x481 = pow(q10_b_dot, 2)*x439;
double x482 = pow(q10_i_dot, 2)*x439;
double x483 = x1*x482;
double x484 = x4*x482;
double x485 = pow(q1_b_dot, 2)*x444;
double x486 = pow(q1_i_dot, 2)*x444;
double x487 = x1*x486;
double x488 = x4*x486;
double x489 = pow(q2_b_dot, 2)*x444;
double x490 = pow(q2_i_dot, 2)*x444;
double x491 = x1*x490;
double x492 = x4*x490;
double x493 = pow(q3_b_dot, 2)*x444;
double x494 = pow(q3_i_dot, 2)*x444;
double x495 = x1*x494;
double x496 = x4*x494;
double x497 = pow(q4_b_dot, 2)*x444;
double x498 = pow(q4_i_dot, 2)*x444;
double x499 = x1*x498;
double x500 = x4*x498;
double x501 = pow(q5_b_dot, 2)*x444;
double x502 = pow(q5_i_dot, 2)*x444;
double x503 = x1*x502;
double x504 = x4*x502;
double x505 = pow(q6_b_dot, 2)*x444;
double x506 = pow(q6_i_dot, 2)*x444;
double x507 = x1*x506;
double x508 = x4*x506;
double x509 = pow(q7_b_dot, 2)*x444;
double x510 = pow(q7_i_dot, 2)*x444;
double x511 = x1*x510;
double x512 = x4*x510;
double x513 = pow(q8_b_dot, 2)*x444;
double x514 = pow(q8_i_dot, 2)*x444;
double x515 = x1*x514;
double x516 = x4*x514;
double x517 = pow(q9_b_dot, 2)*x444;
double x518 = pow(q9_i_dot, 2)*x444;
double x519 = x1*x518;
double x520 = x4*x518;
double x521 = x162*x3;
double x522 = x0*x78;
double x523 = p10_i*x522;
double x524 = p10_b*x523;
double x525 = (9.0/70.0)*x10;
double x526 = x0*x525;
double x527 = p9_i*x526;
double x528 = p10_b*x527;
double x529 = p9_b*x526;
double x530 = p10_i*x529;
double x531 = x0*x83;
double x532 = p1_i*x531;
double x533 = p1_b*x532;
double x534 = p2_i*x526;
double x535 = p1_b*x534;
double x536 = p2_b*x526;
double x537 = p1_i*x536;
double x538 = p2_i*x531;
double x539 = p2_b*x538;
double x540 = p3_i*x536;
double x541 = p3_b*x534;
double x542 = p3_i*x531;
double x543 = p3_b*x542;
double x544 = p4_i*x526;
double x545 = p3_b*x544;
double x546 = p4_b*x526;
double x547 = p3_i*x546;
double x548 = p4_i*x531;
double x549 = p4_b*x548;
double x550 = p5_i*x546;
double x551 = p5_b*x544;
double x552 = p5_i*x531;
double x553 = p5_b*x552;
double x554 = p6_i*x526;
double x555 = p5_b*x554;
double x556 = p6_b*x526;
double x557 = p5_i*x556;
double x558 = p6_i*x531;
double x559 = p6_b*x558;
double x560 = p7_i*x556;
double x561 = p7_b*x554;
double x562 = p7_i*x531;
double x563 = p7_b*x562;
double x564 = p8_i*x526;
double x565 = p7_b*x564;
double x566 = p7_i*x526;
double x567 = p8_b*x566;
double x568 = p8_i*x531;
double x569 = p8_b*x568;
double x570 = p8_b*x527;
double x571 = p8_i*x529;
double x572 = p9_i*x531;
double x573 = p9_b*x572;
double x574 = q10_i*x522;
double x575 = q10_b*x574;
double x576 = q9_i*x526;
double x577 = q10_b*x576;
double x578 = q9_b*x526;
double x579 = q10_i*x578;
double x580 = q1_i*x531;
double x581 = q1_b*x580;
double x582 = q2_i*x526;
double x583 = q1_b*x582;
double x584 = q2_b*x526;
double x585 = q1_i*x584;
double x586 = q2_i*x531;
double x587 = q2_b*x586;
double x588 = q3_i*x584;
double x589 = q3_b*x582;
double x590 = q3_i*x531;
double x591 = q3_b*x590;
double x592 = q4_i*x526;
double x593 = q3_b*x592;
double x594 = q4_b*x526;
double x595 = q3_i*x594;
double x596 = q4_i*x531;
double x597 = q4_b*x596;
double x598 = q5_i*x594;
double x599 = q5_b*x592;
double x600 = q5_i*x531;
double x601 = q5_b*x600;
double x602 = q6_i*x526;
double x603 = q5_b*x602;
double x604 = q6_b*x526;
double x605 = q5_i*x604;
double x606 = q6_i*x531;
double x607 = q6_b*x606;
double x608 = q7_i*x604;
double x609 = q7_b*x602;
double x610 = q7_i*x531;
double x611 = q7_b*x610;
double x612 = q8_i*x526;
double x613 = q7_b*x612;
double x614 = q7_i*x526;
double x615 = q8_b*x614;
double x616 = q8_i*x531;
double x617 = q8_b*x616;
double x618 = q8_b*x576;
double x619 = q8_i*x578;
double x620 = q9_i*x531;
double x621 = q9_b*x620;
double x622 = (97.0/20.0)*x162;
double x623 = x3*x622;
double x624 = 2*x162;
double x625 = x3*x624;
double x626 = 3*x162;
double x627 = x3*x626;
double x628 = 4*x162;
double x629 = x3*x628;
double x630 = 5*x162;
double x631 = x3*x630;
double x632 = 6*x162;
double x633 = x3*x632;
double x634 = 7*x162;
double x635 = x3*x634;
double x636 = 8*x162;
double x637 = x3*x636;
double x638 = 9*x162;
double x639 = x3*x638;
double x640 = (4.0/5.0)*x8;
double x641 = x3*x640;
double x642 = (1.0/15.0)*x8;
double x643 = x3*x642;
double x644 = (11.0/210.0)*x163;
double x645 = p10_b*p10_i_dot;
double x646 = x644*x645;
double x647 = (13.0/420.0)*x163;
double x648 = p10_b*p9_i_dot;
double x649 = x647*x648;
double x650 = p10_b_dot*p10_i;
double x651 = x644*x650;
double x652 = p9_i*x647;
double x653 = p10_b_dot*x652;
double x654 = p9_b_dot*x647;
double x655 = p10_i*x654;
double x656 = p9_b*x647;
double x657 = p10_i_dot*x656;
double x658 = p2_i_dot*x647;
double x659 = p1_b*x658;
double x660 = p2_i*x647;
double x661 = p1_b_dot*x660;
double x662 = p2_b_dot*x647;
double x663 = p1_i*x662;
double x664 = p2_b*x647;
double x665 = p1_i_dot*x664;
double x666 = p3_i_dot*x664;
double x667 = p3_i*x662;
double x668 = p3_b_dot*x660;
double x669 = p3_b*x658;
double x670 = p4_i_dot*x647;
double x671 = p3_b*x670;
double x672 = p4_i*x647;
double x673 = p3_b_dot*x672;
double x674 = p4_b_dot*x647;
double x675 = p3_i*x674;
double x676 = p4_b*x647;
double x677 = p3_i_dot*x676;
double x678 = p5_i_dot*x676;
double x679 = p5_i*x674;
double x680 = p5_b_dot*x672;
double x681 = p5_b*x670;
double x682 = p6_i_dot*x647;
double x683 = p5_b*x682;
double x684 = p6_i*x647;
double x685 = p5_b_dot*x684;
double x686 = p6_b_dot*x647;
double x687 = p5_i*x686;
double x688 = p6_b*x647;
double x689 = p5_i_dot*x688;
double x690 = p7_i_dot*x688;
double x691 = p7_i*x686;
double x692 = p7_b_dot*x684;
double x693 = p7_b*x682;
double x694 = p7_b*p8_i_dot;
double x695 = x647*x694;
double x696 = p7_b_dot*p8_i;
double x697 = x647*x696;
double x698 = p7_i*p8_b_dot;
double x699 = x647*x698;
double x700 = p8_b*x647;
double x701 = p7_i_dot*x700;
double x702 = p9_i_dot*x700;
double x703 = p8_b_dot*x652;
double x704 = p8_i*x654;
double x705 = p8_i_dot*x656;
double x706 = q10_b*q10_i_dot;
double x707 = x644*x706;
double x708 = q9_i_dot*x647;
double x709 = q10_b*x708;
double x710 = q10_b_dot*q10_i;
double x711 = x644*x710;
double x712 = q9_i*x647;
double x713 = q10_b_dot*x712;
double x714 = q9_b_dot*x647;
double x715 = q10_i*x714;
double x716 = q9_b*x647;
double x717 = q10_i_dot*x716;
double x718 = q2_i_dot*x647;
double x719 = q1_b*x718;
double x720 = q2_i*x647;
double x721 = q1_b_dot*x720;
double x722 = q2_b_dot*x647;
double x723 = q1_i*x722;
double x724 = q2_b*x647;
double x725 = q1_i_dot*x724;
double x726 = q3_i_dot*x724;
double x727 = q3_i*x722;
double x728 = q3_b_dot*x720;
double x729 = q3_b*x718;
double x730 = q4_i_dot*x647;
double x731 = q3_b*x730;
double x732 = q4_i*x647;
double x733 = q3_b_dot*x732;
double x734 = q4_b_dot*x647;
double x735 = q3_i*x734;
double x736 = q4_b*x647;
double x737 = q3_i_dot*x736;
double x738 = q5_i_dot*x736;
double x739 = q5_i*x734;
double x740 = q5_b_dot*x732;
double x741 = q5_b*x730;
double x742 = q6_i_dot*x647;
double x743 = q5_b*x742;
double x744 = q6_i*x647;
double x745 = q5_b_dot*x744;
double x746 = q6_b_dot*x647;
double x747 = q5_i*x746;
double x748 = q6_b*x647;
double x749 = q5_i_dot*x748;
double x750 = q7_i_dot*x748;
double x751 = q7_i*x746;
double x752 = q7_b_dot*x744;
double x753 = q7_b*x742;
double x754 = q7_b*q8_i_dot;
double x755 = x647*x754;
double x756 = q7_b_dot*q8_i;
double x757 = x647*x756;
double x758 = q7_i*q8_b_dot;
double x759 = x647*x758;
double x760 = q7_i_dot*q8_b;
double x761 = x647*x760;
double x762 = q8_b*x708;
double x763 = q8_b_dot*x712;
double x764 = q8_i*x714;
double x765 = q8_i_dot*x716;
double x766 = p10_i_dot*x439;
double x767 = x199*x766;
double x768 = (1.0/140.0)*x8;
double x769 = p9_i_dot*x768;
double x770 = x199*x769;
double x771 = x0*x768;
double x772 = p9_b_dot*x771;
double x773 = p10_i_dot*x772;
double x774 = p1_i_dot*x444;
double x775 = x203*x774;
double x776 = x203*x768;
double x777 = p2_i_dot*x776;
double x778 = p2_b_dot*x771;
double x779 = p1_i_dot*x778;
double x780 = x0*x444;
double x781 = p2_i_dot*x780;