diff --git a/opendbc_repo/opendbc/car/hyundai/carcontroller.py b/opendbc_repo/opendbc/car/hyundai/carcontroller.py index 8035cd2f5..97de2af32 100644 --- a/opendbc_repo/opendbc/car/hyundai/carcontroller.py +++ b/opendbc_repo/opendbc/car/hyundai/carcontroller.py @@ -251,6 +251,7 @@ class CarController(CarControllerBase): self._ioniq_6_last_ipedal_regen_state_2 = 0 self._ioniq_6_last_buttons_counter = 0 self._ioniq_6_last_regen_control_counter = -1 + self._ioniq_6_regen_request_sent = False self._ioniq_6_last_gear = structs.CarState.GearShifter.unknown self._genesis_g90_long_tuning = GenesisG90LongitudinalTuningState() @@ -258,11 +259,13 @@ class CarController(CarControllerBase): self._ioniq_6_always_ipedal_pending = False self._ioniq_6_always_ipedal_press_remaining = 0 self._ioniq_6_always_ipedal_retry_frame = 0 + self._ioniq_6_regen_request_sent = False def _arm_ioniq_6_always_ipedal(self) -> None: self._ioniq_6_always_ipedal_pending = True self._ioniq_6_always_ipedal_press_remaining = 0 self._ioniq_6_always_ipedal_retry_frame = self.frame + self._ioniq_6_regen_request_sent = False def _update_ioniq_6_always_ipedal(self, CC, CS, starpilot_toggles): can_sends = [] @@ -313,6 +316,7 @@ class CarController(CarControllerBase): if self._ioniq_6_always_ipedal_press_remaining == 0 and self.frame >= self._ioniq_6_always_ipedal_retry_frame: self._ioniq_6_always_ipedal_press_remaining = IONIQ_6_IPEDAL_LATCH_PRESS_SEND_COUNT if (max_regen_state or ipedal_latch_pending) \ else IONIQ_6_IPEDAL_PRESS_SEND_COUNT + self._ioniq_6_regen_request_sent = False # Mirror the current stock counter after seeing the real CRUISE_BUTTONS frame land on the bus. # Sending the next counter early creates a paddle frame that gets followed by the stock no-paddle @@ -327,10 +331,12 @@ class CarController(CarControllerBase): self._ioniq_6_always_ipedal_retry_frame = self.frame + retry_wait_frames # The drivetrain latch uses a second HKG CAN-FD request path in addition to the left paddle bit. - # Once the cluster-facing regen state reaches max regen, mirror the live stock frame and only - # flip the request bytes that the physical paddle toggles for the final i-Pedal latch step. - if max_regen_state and has_regen_control_msg and regen_control_counter_changed: - can_sends.append(hyundaicanfd.create_ioniq_6_regen_control(self.packer, self.CP, self.CAN, regen_control_msg)) + # Mirror the next stock 0x25A frame once per retry burst instead of spamming it continuously. + if max_regen_state and has_regen_control_msg and regen_control_counter_changed and not self._ioniq_6_regen_request_sent: + request_tail = hyundaicanfd.get_ioniq_6_regen_control_request_tail(regen_control_msg) + if request_tail is not None: + can_sends.append(hyundaicanfd.create_ioniq_6_regen_control(self.packer, self.CP, self.CAN, regen_control_msg)) + self._ioniq_6_regen_request_sent = True self._ioniq_6_last_ipedal_regen_state = regen_state self._ioniq_6_last_ipedal_regen_state_2 = regen_state_2 diff --git a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py index 97893cc64..e63624496 100644 --- a/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py +++ b/opendbc_repo/opendbc/car/hyundai/hyundaicanfd.py @@ -164,8 +164,12 @@ IONIQ_6_CRUISE_BUTTONS_BASE_CHECKSUMS = ( ) IONIQ_6_CRUISE_BUTTONS_LEFT_PADDLE_CHECKSUM_XOR = 0x77 IONIQ_6_CRUISE_BUTTONS_RIGHT_PADDLE_CHECKSUM_XOR = 0xD4 -IONIQ_6_REGEN_CONTROL_REQ_BYTE24 = 0xC0 -IONIQ_6_REGEN_CONTROL_REQ_BYTE27 = 0x00 +IONIQ_6_REGEN_CONTROL_REQUEST_TAILS = { + # Stock manual left-paddle latch request seen on route 0000025e. + (0xA8, 0x0C, 0x12, 0x0E): (0xC0, 0x0C, 0x12, 0x00), + # Stock manual left-paddle latch request seen on route 00000260. + (0xA8, 0x0E, 0x07, 0x0E): (0x85, 0x0E, 0x07, 0x0C), +} def get_ioniq_6_cruise_buttons_next_counter(counter: int) -> int: @@ -202,14 +206,20 @@ def create_ioniq_6_paddle_buttons(packer, CP, CAN, cnt, left_paddle=False, right return address, bytes(dat), bus -def create_ioniq_6_regen_control(packer, CP, CAN, base_values, - req_byte24=IONIQ_6_REGEN_CONTROL_REQ_BYTE24, - req_byte27=IONIQ_6_REGEN_CONTROL_REQ_BYTE27): +def get_ioniq_6_regen_control_request_tail(base_values): + base_tail = tuple(int(base_values.get(f"BYTE{i}", 0)) for i in range(24, 28)) + return IONIQ_6_REGEN_CONTROL_REQUEST_TAILS.get(base_tail) + + +def create_ioniq_6_regen_control(packer, CP, CAN, base_values): + request_tail = get_ioniq_6_regen_control_request_tail(base_values) + if request_tail is None: + raise ValueError(f"Unsupported Ioniq 6 regen control tail: {tuple(int(base_values.get(f'BYTE{i}', 0)) for i in range(24, 28))}") + values = {k: v for k, v in base_values.items() if k != "CHECKSUM"} address = packer.dbc.name_to_msg["IONIQ_6_REGEN_CONTROL"].address dat = bytearray(packer.pack(address, values)) - dat[24] = req_byte24 - dat[27] = req_byte27 + dat[24:28] = bytes(request_tail) checksum = hkg_can_fd_checksum(address, None, dat) dat[0] = checksum & 0xFF diff --git a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py index 7606bd354..86452f949 100644 --- a/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py +++ b/opendbc_repo/opendbc/car/hyundai/tests/test_hyundai.py @@ -343,7 +343,11 @@ class TestHyundaiFingerprint: assert hyundaicanfd.get_ioniq_6_cruise_buttons_next_counter(13) == 14 assert hyundaicanfd.get_ioniq_6_cruise_buttons_next_counter(14) == 0 - def test_ioniq_6_regen_control_message_preserves_stock_frame_and_flips_only_ipedal_request_bytes(self): + @pytest.mark.parametrize(("stock_hex", "expected_tail"), [ + ("45421440801f000000000000a865170000a000000080c071a80c120e00000000", bytes.fromhex("c00c1200")), + ("47745840801f0000000005006e5e0e0000a000000080c071a80e070e00000000", bytes.fromhex("850e070c")), + ]) + def test_ioniq_6_regen_control_message_preserves_stock_frame_and_flips_only_ipedal_request_bytes(self, stock_hex, expected_tail): CP = CarParams.new_message() CP.carFingerprint = CAR.HYUNDAI_IONIQ_6 CP.flags = int(HyundaiFlags.CANFD | HyundaiFlags.CANFD_LKA_STEERING) @@ -351,14 +355,12 @@ class TestHyundaiFingerprint: packer = CANPacker(DBC[CP.carFingerprint][Bus.pt]) can_bus = CanBus(CP) parser = CANParser(DBC[CP.carFingerprint][Bus.pt], [("IONIQ_6_REGEN_CONTROL", 0)], can_bus.ECAN) - stock_dat = bytes.fromhex("45421440801f000000000000a865170000a000000080c071a80c120e00000000") + stock_dat = bytes.fromhex(stock_hex) parser.update([(1, [(0x25A, stock_dat, can_bus.ECAN)])]) msg = hyundaicanfd.create_ioniq_6_regen_control(packer, CP, can_bus, parser.vl["IONIQ_6_REGEN_CONTROL"]) assert msg[1][2:24] == stock_dat[2:24] - assert msg[1][24] == hyundaicanfd.IONIQ_6_REGEN_CONTROL_REQ_BYTE24 - assert msg[1][25:27] == stock_dat[25:27] - assert msg[1][27] == hyundaicanfd.IONIQ_6_REGEN_CONTROL_REQ_BYTE27 + assert msg[1][24:28] == expected_tail assert msg[1][28:] == stock_dat[28:] checksum = hyundaicanfd.hkg_can_fd_checksum(msg[0], None, bytearray(msg[1])) assert msg[1][0] | (msg[1][1] << 8) == checksum @@ -525,11 +527,18 @@ class TestHyundaiFingerprint: assert any(msg[0] == 0x1CF for msg in sends) regen_cmd = next(msg for msg in sends if msg[0] == 0x25A) - assert regen_cmd[1][24] == hyundaicanfd.IONIQ_6_REGEN_CONTROL_REQ_BYTE24 - assert regen_cmd[1][27] == hyundaicanfd.IONIQ_6_REGEN_CONTROL_REQ_BYTE27 + assert regen_cmd[1][24:28] == bytes.fromhex("c00c1200") checksum = hyundaicanfd.hkg_can_fd_checksum(regen_cmd[0], None, bytearray(regen_cmd[1])) assert regen_cmd[1][0] | (regen_cmd[1][1] << 8) == checksum + controller.frame = 1 + cs.buttons_counter = 6 + cs.ioniq_6_regen_control_msg = dict(cs.ioniq_6_regen_control_msg, COUNTER=0x15) + sends = controller._update_ioniq_6_always_ipedal(cc, cs, toggles) + + assert any(msg[0] == 0x1CF for msg in sends) + assert not any(msg[0] == 0x25A for msg in sends) + def test_ioniq_6_longitudinal_params_match_canfd_tune(self): toggles = get_test_toggles() CP = CarInterface.get_params(CAR.HYUNDAI_IONIQ_6, gen_empty_fingerprint(), [], True, False, False, toggles) diff --git a/opendbc_repo/opendbc/dbc/hyundai_canfd_generated.dbc b/opendbc_repo/opendbc/dbc/hyundai_canfd_generated.dbc index faed2e6f3..fb9b336c1 100644 --- a/opendbc_repo/opendbc/dbc/hyundai_canfd_generated.dbc +++ b/opendbc_repo/opendbc/dbc/hyundai_canfd_generated.dbc @@ -774,39 +774,6 @@ BO_ 593 RADAR_0x251: 16 FRONT_RADAR SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX -BO_ 602 IONIQ_6_REGEN_CONTROL: 32 XXX - SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX - SG_ COUNTER : 16|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE3 : 24|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE4 : 32|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE5 : 40|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE6 : 48|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE7 : 56|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE8 : 64|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE9 : 72|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE10 : 80|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE11 : 88|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE12 : 96|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE13 : 104|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE14 : 112|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE15 : 120|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE16 : 128|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE17 : 136|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE18 : 144|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE19 : 152|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE20 : 160|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE21 : 168|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE22 : 176|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE23 : 184|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE24 : 192|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE25 : 200|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE26 : 208|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE27 : 216|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE28 : 224|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE29 : 232|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE30 : 240|8@1+ (1,0) [0|255] "" XXX - SG_ BYTE31 : 248|8@1+ (1,0) [0|255] "" XXX - BO_ 687 HOD_FD_01_100ms: 8 XXX SG_ HOD_Dir_Status : 18|3@0+ (1,0) [0|7] "" XXX SG_ NEW_SIGNAL_2 : 32|8@1+ (1,0) [0|255] "" XXX @@ -856,9 +823,7 @@ BO_ 736 MANUAL_SPEED_LIMIT_ASSIST: 32 XXX SG_ MSLA_STATUS : 26|2@1+ (1,0) [0|3] "" XXX SG_ MSLA_ENABLED : 38|1@1+ (1,0) [0|1] "" XXX SG_ MAX_SPEED : 55|8@0+ (1,0) [0|255] "" XXX - SG_ EV_REGEN_STATE : 104|8@1+ (1,0) [0|255] "" XXX SG_ MAX_SPEED_COPY : 144|8@1+ (1,0) [0|255] "" XXX - SG_ EV_REGEN_STATE_2 : 184|8@1+ (1,0) [0|255] "" XXX BO_ 837 ADRV_0x345: 8 ADRV SG_ CHECKSUM : 0|16@1+ (1,0) [0|65535] "" XXX diff --git a/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h b/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h index ba89db604..0b817b05c 100644 --- a/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h +++ b/opendbc_repo/opendbc/safety/modes/hyundai_canfd.h @@ -82,6 +82,25 @@ static uint32_t hyundai_canfd_get_checksum(const CANPacket_t *msg) { return chksum; } +static bool hyundai_canfd_ioniq_6_regen_tail_allowed(const CANPacket_t *msg) { + const uint8_t stock24 = hyundai_canfd_last_regen_control[24]; + const uint8_t stock25 = hyundai_canfd_last_regen_control[25]; + const uint8_t stock26 = hyundai_canfd_last_regen_control[26]; + const uint8_t stock27 = hyundai_canfd_last_regen_control[27]; + + if ((stock24 == 0xA8U) && (stock25 == 0x0CU) && (stock26 == 0x12U) && (stock27 == 0x0EU)) { + return (msg->data[24] == 0xC0U) && (msg->data[25] == 0x0CU) && + (msg->data[26] == 0x12U) && (msg->data[27] == 0x00U); + } + + if ((stock24 == 0xA8U) && (stock25 == 0x0EU) && (stock26 == 0x07U) && (stock27 == 0x0EU)) { + return (msg->data[24] == 0x85U) && (msg->data[25] == 0x0EU) && + (msg->data[26] == 0x07U) && (msg->data[27] == 0x0CU); + } + + return false; +} + static void hyundai_canfd_rx_all_hook(const CANPacket_t *msg) { const unsigned pt_bus = hyundai_canfd_lka_steering ? 1U : 0U; @@ -273,14 +292,15 @@ static bool hyundai_canfd_tx_hook(const CANPacket_t *msg) { (GET_LEN(msg) == 32U) && hyundai_canfd_last_regen_control_seen && (hyundai_canfd_get_checksum(msg) == hyundai_common_canfd_compute_checksum(msg)) && - (msg->data[24] == 0xC0U) && - (msg->data[27] == 0x00U); + hyundai_canfd_ioniq_6_regen_tail_allowed(msg); if (allowed) { for (int i = 2; i < 32; i++) { - if ((i != 24) && (i != 27) && (msg->data[i] != hyundai_canfd_last_regen_control[i])) { - allowed = false; - break; + if ((i < 24) || (i > 27)) { + if (msg->data[i] != hyundai_canfd_last_regen_control[i]) { + allowed = false; + break; + } } } } diff --git a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py index 7313fc9a8..bcbc484fb 100755 --- a/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py +++ b/opendbc_repo/opendbc/safety/tests/test_hyundai_canfd.py @@ -440,7 +440,7 @@ class TestHyundaiCanfdLKASteeringEV(TestHyundaiCanfdBase): } return self.packer.make_can_msg_safety("CRUISE_BUTTONS", bus, values) - def _regen_control_msg(self, byte24=0xA8, byte27=0x0E, counter=0x14, bus=None): + def _regen_control_msg(self, byte24=0xA8, byte25=0x0C, byte26=0x12, byte27=0x0E, counter=0x14, bus=None): if bus is None: bus = self.PT_BUS values = { @@ -467,8 +467,8 @@ class TestHyundaiCanfdLKASteeringEV(TestHyundaiCanfdBase): "BYTE22": 0xC0, "BYTE23": 0x71, "BYTE24": byte24, - "BYTE25": 0x0C, - "BYTE26": 0x12, + "BYTE25": byte25, + "BYTE26": byte26, "BYTE27": byte27, "BYTE28": 0x00, "BYTE29": 0x00, @@ -522,10 +522,16 @@ class TestHyundaiCanfdLKASteeringEVAlwaysIPedal(TestHyundaiCanfdLKASteeringEV): self.safety.set_controls_allowed(False) self.assertTrue(self._tx(self._regen_control_msg(byte24=0xC0, byte27=0x00, counter=0x14))) + self.assertFalse(self._tx(self._regen_control_msg(byte24=0x85, byte25=0x0E, byte26=0x07, byte27=0x0C, counter=0x14))) self.assertFalse(self._tx(self._regen_control_msg(byte24=0xA8, byte27=0x00, counter=0x14))) self.assertFalse(self._tx(self._regen_control_msg(byte24=0xC0, byte27=0x0E, counter=0x14))) self.assertFalse(self._tx(self._regen_control_msg(byte24=0xC0, byte27=0x00, counter=0x15))) + stock_msg = self._regen_control_msg(byte24=0xA8, byte25=0x0E, byte26=0x07, byte27=0x0E, counter=0x58) + self._rx(stock_msg) + self.assertTrue(self._tx(self._regen_control_msg(byte24=0x85, byte25=0x0E, byte26=0x07, byte27=0x0C, counter=0x58))) + self.assertFalse(self._tx(self._regen_control_msg(byte24=0xC0, byte25=0x0E, byte26=0x07, byte27=0x00, counter=0x58))) + self.safety.set_controls_allowed(True) self.assertFalse(self._tx(self._regen_control_msg(byte24=0xC0, byte27=0x00, counter=0x14))) diff --git a/panda/board/drivers/can_common.h b/panda/board/drivers/can_common.h index 722e05943..7ec89f7ba 100644 --- a/panda/board/drivers/can_common.h +++ b/panda/board/drivers/can_common.h @@ -216,6 +216,22 @@ void ignition_can_hook(CANPacket_t *msg) { prev_counter_tesla = counter; } + // Tesla Model S pre-AP exception + if ((msg->addr == 0x101U) && (len == 3)) { + // Validate Tesla checksum/counter to avoid false positives on overlapping frames. + int counter = msg->data[1] & 0xFU; + int checksum = (((msg->addr & 0xFFU) + ((msg->addr >> 8U) & 0xFFU) + msg->data[0] + msg->data[1]) & 0xFFU); + + static int prev_counter_tesla_preap = -1; + if ((msg->data[2] == checksum) && (counter == ((prev_counter_tesla_preap + 1) % 16)) && (prev_counter_tesla_preap != -1)) { + // GTW_epasPowerMode=1 is DRIVE_ON, which is the only ignition source on pre-AP cars. + int power_mode = (msg->data[0] >> 3U) & 0xFU; + ignition_can = power_mode == 0x1U; + ignition_can_cnt = 0U; + } + prev_counter_tesla_preap = counter; + } + // Mazda exception if ((msg->addr == 0x9EU) && (len == 8)) { ignition_can = (msg->data[0] >> 5) == 0x6U; diff --git a/panda/board/obj/body_h7.bin.signed b/panda/board/obj/body_h7.bin.signed index b41969208..a5d1d2ea9 100644 Binary files a/panda/board/obj/body_h7.bin.signed and b/panda/board/obj/body_h7.bin.signed differ diff --git a/panda/board/obj/body_h7/bootstub.elf b/panda/board/obj/body_h7/bootstub.elf index 5f67ed83c..6cef5fc63 100755 Binary files a/panda/board/obj/body_h7/bootstub.elf and b/panda/board/obj/body_h7/bootstub.elf differ diff --git a/panda/board/obj/body_h7/main.bin b/panda/board/obj/body_h7/main.bin index 77f625eed..963077ccd 100755 Binary files a/panda/board/obj/body_h7/main.bin and b/panda/board/obj/body_h7/main.bin differ diff --git a/panda/board/obj/body_h7/main.elf b/panda/board/obj/body_h7/main.elf index a5f9ed364..e8e57a38c 100755 Binary files a/panda/board/obj/body_h7/main.elf and b/panda/board/obj/body_h7/main.elf differ diff --git a/panda/board/obj/bootstub.body_h7.bin b/panda/board/obj/bootstub.body_h7.bin index 8337269e0..162c3d1a7 100755 Binary files a/panda/board/obj/bootstub.body_h7.bin and b/panda/board/obj/bootstub.body_h7.bin differ diff --git a/panda/board/obj/bootstub.panda.bin b/panda/board/obj/bootstub.panda.bin index 75bfc2e02..714da20d1 100755 Binary files a/panda/board/obj/bootstub.panda.bin and b/panda/board/obj/bootstub.panda.bin differ diff --git a/panda/board/obj/bootstub.panda_h7.bin b/panda/board/obj/bootstub.panda_h7.bin index a46debfc2..5047381d8 100755 Binary files a/panda/board/obj/bootstub.panda_h7.bin and b/panda/board/obj/bootstub.panda_h7.bin differ diff --git a/panda/board/obj/bootstub.panda_h7_remote.bin b/panda/board/obj/bootstub.panda_h7_remote.bin index a46debfc2..5047381d8 100755 Binary files a/panda/board/obj/bootstub.panda_h7_remote.bin and b/panda/board/obj/bootstub.panda_h7_remote.bin differ diff --git a/panda/board/obj/bootstub.panda_jungle_h7.bin b/panda/board/obj/bootstub.panda_jungle_h7.bin index 2b2a2d6af..407ff73e8 100755 Binary files a/panda/board/obj/bootstub.panda_jungle_h7.bin and b/panda/board/obj/bootstub.panda_jungle_h7.bin differ diff --git a/panda/board/obj/bootstub.panda_remote.bin b/panda/board/obj/bootstub.panda_remote.bin index 75bfc2e02..714da20d1 100755 Binary files a/panda/board/obj/bootstub.panda_remote.bin and b/panda/board/obj/bootstub.panda_remote.bin differ diff --git a/panda/board/obj/gitversion.h b/panda/board/obj/gitversion.h index c7c757f4f..462f65018 100644 --- a/panda/board/obj/gitversion.h +++ b/panda/board/obj/gitversion.h @@ -1,2 +1,2 @@ extern const uint8_t gitversion[19]; -const uint8_t gitversion[19] = "DEV-2f74f933-DEBUG"; +const uint8_t gitversion[19] = "DEV-9b97124f-DEBUG"; diff --git a/panda/board/obj/panda.bin.signed b/panda/board/obj/panda.bin.signed index efb7c83a2..a8621bbc7 100644 Binary files a/panda/board/obj/panda.bin.signed and b/panda/board/obj/panda.bin.signed differ diff --git a/panda/board/obj/panda/bootstub.elf b/panda/board/obj/panda/bootstub.elf index a5f8e487f..f33350d60 100755 Binary files a/panda/board/obj/panda/bootstub.elf and b/panda/board/obj/panda/bootstub.elf differ diff --git a/panda/board/obj/panda/main.bin b/panda/board/obj/panda/main.bin index fdaa8e923..042acaf07 100755 Binary files a/panda/board/obj/panda/main.bin and b/panda/board/obj/panda/main.bin differ diff --git a/panda/board/obj/panda/main.elf b/panda/board/obj/panda/main.elf index d6ab660ed..8add8df67 100755 Binary files a/panda/board/obj/panda/main.elf and b/panda/board/obj/panda/main.elf differ diff --git a/panda/board/obj/panda_h7.bin.signed b/panda/board/obj/panda_h7.bin.signed index 3f68eaeee..32328987c 100644 Binary files a/panda/board/obj/panda_h7.bin.signed and b/panda/board/obj/panda_h7.bin.signed differ diff --git a/panda/board/obj/panda_h7/bootstub.elf b/panda/board/obj/panda_h7/bootstub.elf index 59bce629b..e09443cdd 100755 Binary files a/panda/board/obj/panda_h7/bootstub.elf and b/panda/board/obj/panda_h7/bootstub.elf differ diff --git a/panda/board/obj/panda_h7/main.bin b/panda/board/obj/panda_h7/main.bin index 6b1e28d7a..e054b3b52 100755 Binary files a/panda/board/obj/panda_h7/main.bin and b/panda/board/obj/panda_h7/main.bin differ diff --git a/panda/board/obj/panda_h7/main.elf b/panda/board/obj/panda_h7/main.elf index 00ff9e13b..2509127bc 100755 Binary files a/panda/board/obj/panda_h7/main.elf and b/panda/board/obj/panda_h7/main.elf differ diff --git a/panda/board/obj/panda_h7_remote.bin.signed b/panda/board/obj/panda_h7_remote.bin.signed index e040c29c1..53e13c030 100644 Binary files a/panda/board/obj/panda_h7_remote.bin.signed and b/panda/board/obj/panda_h7_remote.bin.signed differ diff --git a/panda/board/obj/panda_h7_remote/bootstub.elf b/panda/board/obj/panda_h7_remote/bootstub.elf index a61c202ff..e5f80ca34 100755 Binary files a/panda/board/obj/panda_h7_remote/bootstub.elf and b/panda/board/obj/panda_h7_remote/bootstub.elf differ diff --git a/panda/board/obj/panda_h7_remote/main.bin b/panda/board/obj/panda_h7_remote/main.bin index 06713114a..fef5be014 100755 Binary files a/panda/board/obj/panda_h7_remote/main.bin and b/panda/board/obj/panda_h7_remote/main.bin differ diff --git a/panda/board/obj/panda_h7_remote/main.elf b/panda/board/obj/panda_h7_remote/main.elf index 30ec994e8..9bb7919ab 100755 Binary files a/panda/board/obj/panda_h7_remote/main.elf and b/panda/board/obj/panda_h7_remote/main.elf differ diff --git a/panda/board/obj/panda_jungle_h7.bin.signed b/panda/board/obj/panda_jungle_h7.bin.signed index 307378ab2..37a33ce78 100644 Binary files a/panda/board/obj/panda_jungle_h7.bin.signed and b/panda/board/obj/panda_jungle_h7.bin.signed differ diff --git a/panda/board/obj/panda_jungle_h7/bootstub.elf b/panda/board/obj/panda_jungle_h7/bootstub.elf index fef7a82ce..cec4332a4 100755 Binary files a/panda/board/obj/panda_jungle_h7/bootstub.elf and b/panda/board/obj/panda_jungle_h7/bootstub.elf differ diff --git a/panda/board/obj/panda_jungle_h7/main.bin b/panda/board/obj/panda_jungle_h7/main.bin index c18289d33..e0371050a 100755 Binary files a/panda/board/obj/panda_jungle_h7/main.bin and b/panda/board/obj/panda_jungle_h7/main.bin differ diff --git a/panda/board/obj/panda_jungle_h7/main.elf b/panda/board/obj/panda_jungle_h7/main.elf index fec21519a..4f96cb291 100755 Binary files a/panda/board/obj/panda_jungle_h7/main.elf and b/panda/board/obj/panda_jungle_h7/main.elf differ diff --git a/panda/board/obj/panda_remote.bin.signed b/panda/board/obj/panda_remote.bin.signed index 9ba519aeb..17b71b38d 100644 Binary files a/panda/board/obj/panda_remote.bin.signed and b/panda/board/obj/panda_remote.bin.signed differ diff --git a/panda/board/obj/panda_remote/bootstub.elf b/panda/board/obj/panda_remote/bootstub.elf index 034a6cd5c..32e909937 100755 Binary files a/panda/board/obj/panda_remote/bootstub.elf and b/panda/board/obj/panda_remote/bootstub.elf differ diff --git a/panda/board/obj/panda_remote/main.bin b/panda/board/obj/panda_remote/main.bin index 72c690825..0047e1556 100755 Binary files a/panda/board/obj/panda_remote/main.bin and b/panda/board/obj/panda_remote/main.bin differ diff --git a/panda/board/obj/panda_remote/main.elf b/panda/board/obj/panda_remote/main.elf index 0c26008fa..f3d1cadab 100755 Binary files a/panda/board/obj/panda_remote/main.elf and b/panda/board/obj/panda_remote/main.elf differ diff --git a/panda/board/obj/version b/panda/board/obj/version index f0b84b966..4aa67d8c0 100644 --- a/panda/board/obj/version +++ b/panda/board/obj/version @@ -1 +1 @@ -DEV-2f74f933-DEBUG \ No newline at end of file +DEV-9b97124f-DEBUG \ No newline at end of file diff --git a/selfdrive/controls/lib/latcontrol_torque.py b/selfdrive/controls/lib/latcontrol_torque.py index 77b137315..2a8205aed 100644 --- a/selfdrive/controls/lib/latcontrol_torque.py +++ b/selfdrive/controls/lib/latcontrol_torque.py @@ -48,8 +48,8 @@ UNWIND_LAT_ACCEL_NEAR_ZERO = 0.3 MIN_LATERAL_CONTROL_SPEED = 0.3 CIVIC_BOSCH_MODIFIED_B_FIXED_FRICTION_THRESHOLD = 0.30 CIVIC_BOSCH_MODIFIED_B_LAT_ACCEL_FACTOR_MULT = 1.20 -CIVIC_BOSCH_MODIFIED_A_VARIANT_LAT_ACCEL_FACTOR_MULT = 0.98 -CIVIC_BOSCH_MODIFIED_B_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.14 +CIVIC_BOSCH_MODIFIED_A_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.00 +CIVIC_BOSCH_MODIFIED_B_VARIANT_LAT_ACCEL_FACTOR_MULT = 1.18 CIVIC_BOSCH_MODIFIED_B_TRANSITION_SPEED = 12.0 CIVIC_BOSCH_MODIFIED_B_PHASE_SCALE = 0.10 CIVIC_BOSCH_MODIFIED_B_FF_ONSET = 0.18 @@ -66,22 +66,26 @@ CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_LEFT = 0.02 CIVIC_BOSCH_MODIFIED_B_TURN_IN_FRICTION_BOOST_RIGHT = 0.00 CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_LEFT = 0.26 CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_RIGHT = 0.40 -CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_LEFT = 0.05 -CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_RIGHT = 0.02 -CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_LEFT = 0.04 -CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_RIGHT = 0.01 -CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.02 -CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.01 -CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_LEFT = 0.14 -CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_RIGHT = 0.22 +CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_LEFT = 0.00 +CIVIC_BOSCH_MODIFIED_A_VARIANT_FF_RESTORE_RIGHT = 0.00 +CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_LEFT = 0.00 +CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_BOOST_RIGHT = 0.00 +CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_LEFT = 0.06 +CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_RIGHT = 0.14 +CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.00 +CIVIC_BOSCH_MODIFIED_A_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.00 +CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.04 +CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.10 +CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_LEFT = 0.18 +CIVIC_BOSCH_MODIFIED_B_VARIANT_FF_REDUCTION_RIGHT = 0.28 CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_LEFT = 0.02 CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_BOOST_RIGHT = 0.00 -CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_LEFT = 0.38 -CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_RIGHT = 0.48 +CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_LEFT = 0.50 +CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_RIGHT = 0.62 CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_LEFT = 0.01 CIVIC_BOSCH_MODIFIED_B_VARIANT_TURN_IN_FRICTION_BOOST_RIGHT = 0.00 -CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.24 -CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.34 +CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT = 0.32 +CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT = 0.46 BOLT_2022_2023_CARS = ( GM_CAR.CHEVROLET_BOLT_ACC_2022_2023, @@ -248,21 +252,21 @@ IONIQ_6_FF_CUTOFF = 0.48 IONIQ_6_FF_CUTOFF_WIDTH = 0.12 IONIQ_6_TRANSITION_SPEED = 10.0 IONIQ_6_PHASE_SCALE = 0.10 -IONIQ_6_TURN_IN_BOOST_LEFT = 1.28 -IONIQ_6_TURN_IN_BOOST_RIGHT = 1.36 -IONIQ_6_UNWIND_TAPER_LEFT = 2.18 -IONIQ_6_UNWIND_TAPER_RIGHT = 5.05 +IONIQ_6_TURN_IN_BOOST_LEFT = 1.40 +IONIQ_6_TURN_IN_BOOST_RIGHT = 1.50 +IONIQ_6_UNWIND_TAPER_LEFT = 2.40 +IONIQ_6_UNWIND_TAPER_RIGHT = 5.40 IONIQ_6_FRICTION_MULT = 0.965 IONIQ_6_FRICTION_LAT_RISE = 0.20 IONIQ_6_FRICTION_JERK_RISE = 0.24 -IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.50 -IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.72 -IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 2.55 -IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 6.20 -IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.26 -IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.44 -IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 2.30 -IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 5.70 +IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_LEFT = 0.56 +IONIQ_6_TURN_IN_THRESHOLD_REDUCTION_RIGHT = 0.82 +IONIQ_6_UNWIND_THRESHOLD_INCREASE_LEFT = 2.80 +IONIQ_6_UNWIND_THRESHOLD_INCREASE_RIGHT = 6.70 +IONIQ_6_TURN_IN_FRICTION_BOOST_LEFT = 0.30 +IONIQ_6_TURN_IN_FRICTION_BOOST_RIGHT = 0.50 +IONIQ_6_UNWIND_FRICTION_REDUCTION_LEFT = 2.55 +IONIQ_6_UNWIND_FRICTION_REDUCTION_RIGHT = 6.10 IONIQ_6_CENTER_TAPER_MAX = 0.056 IONIQ_6_CENTER_TAPER_LAT = 0.22 IONIQ_6_CENTER_TAPER_LAT_WIDTH = 0.02 @@ -426,6 +430,11 @@ def get_civic_bosch_modified_b_ff_scale(desired_lateral_accel: float, desired_la CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_LEFT, CIVIC_BOSCH_MODIFIED_B_UNWIND_TAPER_RIGHT) * unwind_weight * (0.35 + 0.65 * low_speed_factor)) + if a_variant_active: + unwind_taper *= 1.0 - (_civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_LEFT, + CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_TAPER_RIGHT) * + unwind_weight * (0.35 + 0.65 * low_speed_factor)) if variant_active: unwind_taper *= 1.0 - (_civic_bosch_modified_b_side_value(desired_lateral_accel, CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_TAPER_LEFT, @@ -467,6 +476,11 @@ def get_civic_bosch_modified_b_friction_scale(v_ego: float, desired_lateral_acce CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_LEFT, CIVIC_BOSCH_MODIFIED_B_UNWIND_FRICTION_REDUCTION_RIGHT) * envelope * unwind_weight) + if a_variant_active: + friction_scale -= (_civic_bosch_modified_b_side_value(desired_lateral_accel, + CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT, + CIVIC_BOSCH_MODIFIED_A_VARIANT_UNWIND_FRICTION_REDUCTION_RIGHT) * + envelope * unwind_weight) if variant_active: friction_scale -= (_civic_bosch_modified_b_side_value(desired_lateral_accel, CIVIC_BOSCH_MODIFIED_B_VARIANT_UNWIND_FRICTION_REDUCTION_LEFT, diff --git a/selfdrive/controls/tests/test_latcontrol.py b/selfdrive/controls/tests/test_latcontrol.py index 1735a4915..85fd31bcb 100644 --- a/selfdrive/controls/tests/test_latcontrol.py +++ b/selfdrive/controls/tests/test_latcontrol.py @@ -432,13 +432,13 @@ class TestLatControl: monkeypatch.setattr(latcontrol_torque, "civic_bosch_modified_a_lateral_testing_ground_active", lambda: True) a_variant_controller = LatControlTorque(CP.as_reader(), CI, DT_CTRL) - assert a_variant_controller.torque_params.latAccelFactor == pytest.approx(3.0 * 1.20 * 0.98) + assert a_variant_controller.torque_params.latAccelFactor == pytest.approx(3.0 * 1.20) monkeypatch.setattr(latcontrol_torque, "civic_bosch_modified_a_lateral_testing_ground_active", lambda: False) monkeypatch.setattr(latcontrol_torque, "civic_bosch_modified_lateral_testing_ground_active", lambda: True) variant_controller = LatControlTorque(CP.as_reader(), CI, DT_CTRL) - assert variant_controller.torque_params.latAccelFactor == pytest.approx(3.0 * 1.20 * 1.14) + assert variant_controller.torque_params.latAccelFactor == pytest.approx(3.0 * 1.20 * 1.18) def test_modified_civic_b_torque_ff_scale_curve(self): steady_left = get_civic_bosch_modified_b_ff_scale(0.5, 0.0, 12.0) @@ -490,7 +490,9 @@ class TestLatControl: base_steady_right = get_civic_bosch_modified_b_ff_scale(-0.5, 0.0, 12.0) base_turn_in_left = get_civic_bosch_modified_b_ff_scale(0.5, 0.8, 12.0) base_turn_in_right = get_civic_bosch_modified_b_ff_scale(-0.5, -0.8, 12.0) - base_turn_in_left_friction = get_civic_bosch_modified_b_friction_scale(12.0, 0.5, 0.8) + base_unwind_left = get_civic_bosch_modified_b_ff_scale(0.5, -0.8, 12.0) + base_unwind_right = get_civic_bosch_modified_b_ff_scale(-0.5, 0.8, 12.0) + base_unwind_right_friction = get_civic_bosch_modified_b_friction_scale(12.0, -0.5, 0.8) monkeypatch.setattr(latcontrol_torque, "civic_bosch_modified_a_lateral_testing_ground_active", lambda: True) @@ -498,13 +500,17 @@ class TestLatControl: a_variant_steady_right = get_civic_bosch_modified_b_ff_scale(-0.5, 0.0, 12.0) a_variant_turn_in_left = get_civic_bosch_modified_b_ff_scale(0.5, 0.8, 12.0) a_variant_turn_in_right = get_civic_bosch_modified_b_ff_scale(-0.5, -0.8, 12.0) - a_variant_turn_in_left_friction = get_civic_bosch_modified_b_friction_scale(12.0, 0.5, 0.8) + a_variant_unwind_left = get_civic_bosch_modified_b_ff_scale(0.5, -0.8, 12.0) + a_variant_unwind_right = get_civic_bosch_modified_b_ff_scale(-0.5, 0.8, 12.0) + a_variant_unwind_right_friction = get_civic_bosch_modified_b_friction_scale(12.0, -0.5, 0.8) - assert a_variant_steady_left > base_steady_left - assert a_variant_steady_right > base_steady_right - assert a_variant_turn_in_left > base_turn_in_left - assert a_variant_turn_in_right > base_turn_in_right - assert a_variant_turn_in_left_friction > base_turn_in_left_friction + assert a_variant_steady_left == pytest.approx(base_steady_left) + assert a_variant_steady_right == pytest.approx(base_steady_right) + assert a_variant_turn_in_left == pytest.approx(base_turn_in_left) + assert a_variant_turn_in_right == pytest.approx(base_turn_in_right) + assert a_variant_unwind_left < base_unwind_left + assert a_variant_unwind_right < base_unwind_right + assert a_variant_unwind_right_friction < base_unwind_right_friction def test_kia_ev6_testing_ground_update_path(self, monkeypatch): controller, VM, CS, params, starpilot_toggles = self._build_torque_controller(HYUNDAI.KIA_EV6) diff --git a/selfdrive/locationd/models/generated/car.cpp b/selfdrive/locationd/models/generated/car.cpp index 9c78f10ef..99d82d97f 100644 --- a/selfdrive/locationd/models/generated/car.cpp +++ b/selfdrive/locationd/models/generated/car.cpp @@ -45,326 +45,326 @@ const static double MAHA_THRESH_31 = 3.8414588206941227; * * * This file is part of 'ekf' * ******************************************************************************/ -void err_fun(double *nom_x, double *delta_x, double *out_1461273597009064329) { - out_1461273597009064329[0] = delta_x[0] + nom_x[0]; - out_1461273597009064329[1] = delta_x[1] + nom_x[1]; - out_1461273597009064329[2] = delta_x[2] + nom_x[2]; - out_1461273597009064329[3] = delta_x[3] + nom_x[3]; - out_1461273597009064329[4] = delta_x[4] + nom_x[4]; - out_1461273597009064329[5] = delta_x[5] + nom_x[5]; - out_1461273597009064329[6] = delta_x[6] + nom_x[6]; - out_1461273597009064329[7] = delta_x[7] + nom_x[7]; - out_1461273597009064329[8] = delta_x[8] + nom_x[8]; +void err_fun(double *nom_x, double *delta_x, double *out_4611641411917091701) { + out_4611641411917091701[0] = delta_x[0] + nom_x[0]; + out_4611641411917091701[1] = delta_x[1] + nom_x[1]; + out_4611641411917091701[2] = delta_x[2] + nom_x[2]; + out_4611641411917091701[3] = delta_x[3] + nom_x[3]; + out_4611641411917091701[4] = delta_x[4] + nom_x[4]; + out_4611641411917091701[5] = delta_x[5] + nom_x[5]; + out_4611641411917091701[6] = delta_x[6] + nom_x[6]; + out_4611641411917091701[7] = delta_x[7] + nom_x[7]; + out_4611641411917091701[8] = delta_x[8] + nom_x[8]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_7289765730580897738) { - out_7289765730580897738[0] = -nom_x[0] + true_x[0]; - out_7289765730580897738[1] = -nom_x[1] + true_x[1]; - out_7289765730580897738[2] = -nom_x[2] + true_x[2]; - out_7289765730580897738[3] = -nom_x[3] + true_x[3]; - out_7289765730580897738[4] = -nom_x[4] + true_x[4]; - out_7289765730580897738[5] = -nom_x[5] + true_x[5]; - out_7289765730580897738[6] = -nom_x[6] + true_x[6]; - out_7289765730580897738[7] = -nom_x[7] + true_x[7]; - out_7289765730580897738[8] = -nom_x[8] + true_x[8]; +void inv_err_fun(double *nom_x, double *true_x, double *out_3968532722392907058) { + out_3968532722392907058[0] = -nom_x[0] + true_x[0]; + out_3968532722392907058[1] = -nom_x[1] + true_x[1]; + out_3968532722392907058[2] = -nom_x[2] + true_x[2]; + out_3968532722392907058[3] = -nom_x[3] + true_x[3]; + out_3968532722392907058[4] = -nom_x[4] + true_x[4]; + out_3968532722392907058[5] = -nom_x[5] + true_x[5]; + out_3968532722392907058[6] = -nom_x[6] + true_x[6]; + out_3968532722392907058[7] = -nom_x[7] + true_x[7]; + out_3968532722392907058[8] = -nom_x[8] + true_x[8]; } -void H_mod_fun(double *state, double *out_8304527178040913025) { - out_8304527178040913025[0] = 1.0; - out_8304527178040913025[1] = 0.0; - out_8304527178040913025[2] = 0.0; - out_8304527178040913025[3] = 0.0; - out_8304527178040913025[4] = 0.0; - out_8304527178040913025[5] = 0.0; - out_8304527178040913025[6] = 0.0; - out_8304527178040913025[7] = 0.0; - out_8304527178040913025[8] = 0.0; - out_8304527178040913025[9] = 0.0; - out_8304527178040913025[10] = 1.0; - out_8304527178040913025[11] = 0.0; - out_8304527178040913025[12] = 0.0; - out_8304527178040913025[13] = 0.0; - out_8304527178040913025[14] = 0.0; - out_8304527178040913025[15] = 0.0; - out_8304527178040913025[16] = 0.0; - out_8304527178040913025[17] = 0.0; - out_8304527178040913025[18] = 0.0; - out_8304527178040913025[19] = 0.0; - out_8304527178040913025[20] = 1.0; - out_8304527178040913025[21] = 0.0; - out_8304527178040913025[22] = 0.0; - out_8304527178040913025[23] = 0.0; - out_8304527178040913025[24] = 0.0; - out_8304527178040913025[25] = 0.0; - out_8304527178040913025[26] = 0.0; - out_8304527178040913025[27] = 0.0; - out_8304527178040913025[28] = 0.0; - out_8304527178040913025[29] = 0.0; - out_8304527178040913025[30] = 1.0; - out_8304527178040913025[31] = 0.0; - out_8304527178040913025[32] = 0.0; - out_8304527178040913025[33] = 0.0; - out_8304527178040913025[34] = 0.0; - out_8304527178040913025[35] = 0.0; - out_8304527178040913025[36] = 0.0; - out_8304527178040913025[37] = 0.0; - out_8304527178040913025[38] = 0.0; - out_8304527178040913025[39] = 0.0; - out_8304527178040913025[40] = 1.0; - out_8304527178040913025[41] = 0.0; - out_8304527178040913025[42] = 0.0; - out_8304527178040913025[43] = 0.0; - out_8304527178040913025[44] = 0.0; - out_8304527178040913025[45] = 0.0; - out_8304527178040913025[46] = 0.0; - out_8304527178040913025[47] = 0.0; - out_8304527178040913025[48] = 0.0; - out_8304527178040913025[49] = 0.0; - out_8304527178040913025[50] = 1.0; - out_8304527178040913025[51] = 0.0; - out_8304527178040913025[52] = 0.0; - out_8304527178040913025[53] = 0.0; - out_8304527178040913025[54] = 0.0; - out_8304527178040913025[55] = 0.0; - out_8304527178040913025[56] = 0.0; - out_8304527178040913025[57] = 0.0; - out_8304527178040913025[58] = 0.0; - out_8304527178040913025[59] = 0.0; - out_8304527178040913025[60] = 1.0; - out_8304527178040913025[61] = 0.0; - out_8304527178040913025[62] = 0.0; - out_8304527178040913025[63] = 0.0; - out_8304527178040913025[64] = 0.0; - out_8304527178040913025[65] = 0.0; - out_8304527178040913025[66] = 0.0; - out_8304527178040913025[67] = 0.0; - out_8304527178040913025[68] = 0.0; - out_8304527178040913025[69] = 0.0; - out_8304527178040913025[70] = 1.0; - out_8304527178040913025[71] = 0.0; - out_8304527178040913025[72] = 0.0; - out_8304527178040913025[73] = 0.0; - out_8304527178040913025[74] = 0.0; - out_8304527178040913025[75] = 0.0; - out_8304527178040913025[76] = 0.0; - out_8304527178040913025[77] = 0.0; - out_8304527178040913025[78] = 0.0; - out_8304527178040913025[79] = 0.0; - out_8304527178040913025[80] = 1.0; +void H_mod_fun(double *state, double *out_1232909156344202498) { + out_1232909156344202498[0] = 1.0; + out_1232909156344202498[1] = 0.0; + out_1232909156344202498[2] = 0.0; + out_1232909156344202498[3] = 0.0; + out_1232909156344202498[4] = 0.0; + out_1232909156344202498[5] = 0.0; + out_1232909156344202498[6] = 0.0; + out_1232909156344202498[7] = 0.0; + out_1232909156344202498[8] = 0.0; + out_1232909156344202498[9] = 0.0; + out_1232909156344202498[10] = 1.0; + out_1232909156344202498[11] = 0.0; + out_1232909156344202498[12] = 0.0; + out_1232909156344202498[13] = 0.0; + out_1232909156344202498[14] = 0.0; + out_1232909156344202498[15] = 0.0; + out_1232909156344202498[16] = 0.0; + out_1232909156344202498[17] = 0.0; + out_1232909156344202498[18] = 0.0; + out_1232909156344202498[19] = 0.0; + out_1232909156344202498[20] = 1.0; + out_1232909156344202498[21] = 0.0; + out_1232909156344202498[22] = 0.0; + out_1232909156344202498[23] = 0.0; + out_1232909156344202498[24] = 0.0; + out_1232909156344202498[25] = 0.0; + out_1232909156344202498[26] = 0.0; + out_1232909156344202498[27] = 0.0; + out_1232909156344202498[28] = 0.0; + out_1232909156344202498[29] = 0.0; + out_1232909156344202498[30] = 1.0; + out_1232909156344202498[31] = 0.0; + out_1232909156344202498[32] = 0.0; + out_1232909156344202498[33] = 0.0; + out_1232909156344202498[34] = 0.0; + out_1232909156344202498[35] = 0.0; + out_1232909156344202498[36] = 0.0; + out_1232909156344202498[37] = 0.0; + out_1232909156344202498[38] = 0.0; + out_1232909156344202498[39] = 0.0; + out_1232909156344202498[40] = 1.0; + out_1232909156344202498[41] = 0.0; + out_1232909156344202498[42] = 0.0; + out_1232909156344202498[43] = 0.0; + out_1232909156344202498[44] = 0.0; + out_1232909156344202498[45] = 0.0; + out_1232909156344202498[46] = 0.0; + out_1232909156344202498[47] = 0.0; + out_1232909156344202498[48] = 0.0; + out_1232909156344202498[49] = 0.0; + out_1232909156344202498[50] = 1.0; + out_1232909156344202498[51] = 0.0; + out_1232909156344202498[52] = 0.0; + out_1232909156344202498[53] = 0.0; + out_1232909156344202498[54] = 0.0; + out_1232909156344202498[55] = 0.0; + out_1232909156344202498[56] = 0.0; + out_1232909156344202498[57] = 0.0; + out_1232909156344202498[58] = 0.0; + out_1232909156344202498[59] = 0.0; + out_1232909156344202498[60] = 1.0; + out_1232909156344202498[61] = 0.0; + out_1232909156344202498[62] = 0.0; + out_1232909156344202498[63] = 0.0; + out_1232909156344202498[64] = 0.0; + out_1232909156344202498[65] = 0.0; + out_1232909156344202498[66] = 0.0; + out_1232909156344202498[67] = 0.0; + out_1232909156344202498[68] = 0.0; + out_1232909156344202498[69] = 0.0; + out_1232909156344202498[70] = 1.0; + out_1232909156344202498[71] = 0.0; + out_1232909156344202498[72] = 0.0; + out_1232909156344202498[73] = 0.0; + out_1232909156344202498[74] = 0.0; + out_1232909156344202498[75] = 0.0; + out_1232909156344202498[76] = 0.0; + out_1232909156344202498[77] = 0.0; + out_1232909156344202498[78] = 0.0; + out_1232909156344202498[79] = 0.0; + out_1232909156344202498[80] = 1.0; } -void f_fun(double *state, double dt, double *out_1355173870151422192) { - out_1355173870151422192[0] = state[0]; - out_1355173870151422192[1] = state[1]; - out_1355173870151422192[2] = state[2]; - out_1355173870151422192[3] = state[3]; - out_1355173870151422192[4] = state[4]; - out_1355173870151422192[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5]; - out_1355173870151422192[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6]; - out_1355173870151422192[7] = state[7]; - out_1355173870151422192[8] = state[8]; +void f_fun(double *state, double dt, double *out_241923337051056220) { + out_241923337051056220[0] = state[0]; + out_241923337051056220[1] = state[1]; + out_241923337051056220[2] = state[2]; + out_241923337051056220[3] = state[3]; + out_241923337051056220[4] = state[4]; + out_241923337051056220[5] = dt*((-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4]))*state[6] - 9.8100000000000005*state[8] + stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*state[1]) + (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*state[4])) + state[5]; + out_241923337051056220[6] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*state[4])) + state[6]; + out_241923337051056220[7] = state[7]; + out_241923337051056220[8] = state[8]; } -void F_fun(double *state, double dt, double *out_3219998669912464347) { - out_3219998669912464347[0] = 1; - out_3219998669912464347[1] = 0; - out_3219998669912464347[2] = 0; - out_3219998669912464347[3] = 0; - out_3219998669912464347[4] = 0; - out_3219998669912464347[5] = 0; - out_3219998669912464347[6] = 0; - out_3219998669912464347[7] = 0; - out_3219998669912464347[8] = 0; - out_3219998669912464347[9] = 0; - out_3219998669912464347[10] = 1; - out_3219998669912464347[11] = 0; - out_3219998669912464347[12] = 0; - out_3219998669912464347[13] = 0; - out_3219998669912464347[14] = 0; - out_3219998669912464347[15] = 0; - out_3219998669912464347[16] = 0; - out_3219998669912464347[17] = 0; - out_3219998669912464347[18] = 0; - out_3219998669912464347[19] = 0; - out_3219998669912464347[20] = 1; - out_3219998669912464347[21] = 0; - out_3219998669912464347[22] = 0; - out_3219998669912464347[23] = 0; - out_3219998669912464347[24] = 0; - out_3219998669912464347[25] = 0; - out_3219998669912464347[26] = 0; - out_3219998669912464347[27] = 0; - out_3219998669912464347[28] = 0; - out_3219998669912464347[29] = 0; - out_3219998669912464347[30] = 1; - out_3219998669912464347[31] = 0; - out_3219998669912464347[32] = 0; - out_3219998669912464347[33] = 0; - out_3219998669912464347[34] = 0; - out_3219998669912464347[35] = 0; - out_3219998669912464347[36] = 0; - out_3219998669912464347[37] = 0; - out_3219998669912464347[38] = 0; - out_3219998669912464347[39] = 0; - out_3219998669912464347[40] = 1; - out_3219998669912464347[41] = 0; - out_3219998669912464347[42] = 0; - out_3219998669912464347[43] = 0; - out_3219998669912464347[44] = 0; - out_3219998669912464347[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4])); - out_3219998669912464347[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); - out_3219998669912464347[47] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_3219998669912464347[48] = -dt*stiffness_front*state[0]/(mass*state[1]); - out_3219998669912464347[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2))); - out_3219998669912464347[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; - out_3219998669912464347[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); - out_3219998669912464347[52] = dt*stiffness_front*state[0]/(mass*state[1]); - out_3219998669912464347[53] = -9.8100000000000005*dt; - out_3219998669912464347[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4])); - out_3219998669912464347[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); - out_3219998669912464347[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3219998669912464347[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3219998669912464347[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2))); - out_3219998669912464347[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); - out_3219998669912464347[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1; - out_3219998669912464347[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); - out_3219998669912464347[62] = 0; - out_3219998669912464347[63] = 0; - out_3219998669912464347[64] = 0; - out_3219998669912464347[65] = 0; - out_3219998669912464347[66] = 0; - out_3219998669912464347[67] = 0; - out_3219998669912464347[68] = 0; - out_3219998669912464347[69] = 0; - out_3219998669912464347[70] = 1; - out_3219998669912464347[71] = 0; - out_3219998669912464347[72] = 0; - out_3219998669912464347[73] = 0; - out_3219998669912464347[74] = 0; - out_3219998669912464347[75] = 0; - out_3219998669912464347[76] = 0; - out_3219998669912464347[77] = 0; - out_3219998669912464347[78] = 0; - out_3219998669912464347[79] = 0; - out_3219998669912464347[80] = 1; +void F_fun(double *state, double dt, double *out_2984795699500747305) { + out_2984795699500747305[0] = 1; + out_2984795699500747305[1] = 0; + out_2984795699500747305[2] = 0; + out_2984795699500747305[3] = 0; + out_2984795699500747305[4] = 0; + out_2984795699500747305[5] = 0; + out_2984795699500747305[6] = 0; + out_2984795699500747305[7] = 0; + out_2984795699500747305[8] = 0; + out_2984795699500747305[9] = 0; + out_2984795699500747305[10] = 1; + out_2984795699500747305[11] = 0; + out_2984795699500747305[12] = 0; + out_2984795699500747305[13] = 0; + out_2984795699500747305[14] = 0; + out_2984795699500747305[15] = 0; + out_2984795699500747305[16] = 0; + out_2984795699500747305[17] = 0; + out_2984795699500747305[18] = 0; + out_2984795699500747305[19] = 0; + out_2984795699500747305[20] = 1; + out_2984795699500747305[21] = 0; + out_2984795699500747305[22] = 0; + out_2984795699500747305[23] = 0; + out_2984795699500747305[24] = 0; + out_2984795699500747305[25] = 0; + out_2984795699500747305[26] = 0; + out_2984795699500747305[27] = 0; + out_2984795699500747305[28] = 0; + out_2984795699500747305[29] = 0; + out_2984795699500747305[30] = 1; + out_2984795699500747305[31] = 0; + out_2984795699500747305[32] = 0; + out_2984795699500747305[33] = 0; + out_2984795699500747305[34] = 0; + out_2984795699500747305[35] = 0; + out_2984795699500747305[36] = 0; + out_2984795699500747305[37] = 0; + out_2984795699500747305[38] = 0; + out_2984795699500747305[39] = 0; + out_2984795699500747305[40] = 1; + out_2984795699500747305[41] = 0; + out_2984795699500747305[42] = 0; + out_2984795699500747305[43] = 0; + out_2984795699500747305[44] = 0; + out_2984795699500747305[45] = dt*(stiffness_front*(-state[2] - state[3] + state[7])/(mass*state[1]) + (-stiffness_front - stiffness_rear)*state[5]/(mass*state[4]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[6]/(mass*state[4])); + out_2984795699500747305[46] = -dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(mass*pow(state[1], 2)); + out_2984795699500747305[47] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2984795699500747305[48] = -dt*stiffness_front*state[0]/(mass*state[1]); + out_2984795699500747305[49] = dt*((-1 - (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*pow(state[4], 2)))*state[6] - (-stiffness_front*state[0] - stiffness_rear*state[0])*state[5]/(mass*pow(state[4], 2))); + out_2984795699500747305[50] = dt*(-stiffness_front*state[0] - stiffness_rear*state[0])/(mass*state[4]) + 1; + out_2984795699500747305[51] = dt*(-state[4] + (-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(mass*state[4])); + out_2984795699500747305[52] = dt*stiffness_front*state[0]/(mass*state[1]); + out_2984795699500747305[53] = -9.8100000000000005*dt; + out_2984795699500747305[54] = dt*(center_to_front*stiffness_front*(-state[2] - state[3] + state[7])/(rotational_inertia*state[1]) + (-center_to_front*stiffness_front + center_to_rear*stiffness_rear)*state[5]/(rotational_inertia*state[4]) + (-pow(center_to_front, 2)*stiffness_front - pow(center_to_rear, 2)*stiffness_rear)*state[6]/(rotational_inertia*state[4])); + out_2984795699500747305[55] = -center_to_front*dt*stiffness_front*(-state[2] - state[3] + state[7])*state[0]/(rotational_inertia*pow(state[1], 2)); + out_2984795699500747305[56] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2984795699500747305[57] = -center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2984795699500747305[58] = dt*(-(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])*state[5]/(rotational_inertia*pow(state[4], 2)) - (-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])*state[6]/(rotational_inertia*pow(state[4], 2))); + out_2984795699500747305[59] = dt*(-center_to_front*stiffness_front*state[0] + center_to_rear*stiffness_rear*state[0])/(rotational_inertia*state[4]); + out_2984795699500747305[60] = dt*(-pow(center_to_front, 2)*stiffness_front*state[0] - pow(center_to_rear, 2)*stiffness_rear*state[0])/(rotational_inertia*state[4]) + 1; + out_2984795699500747305[61] = center_to_front*dt*stiffness_front*state[0]/(rotational_inertia*state[1]); + out_2984795699500747305[62] = 0; + out_2984795699500747305[63] = 0; + out_2984795699500747305[64] = 0; + out_2984795699500747305[65] = 0; + out_2984795699500747305[66] = 0; + out_2984795699500747305[67] = 0; + out_2984795699500747305[68] = 0; + out_2984795699500747305[69] = 0; + out_2984795699500747305[70] = 1; + out_2984795699500747305[71] = 0; + out_2984795699500747305[72] = 0; + out_2984795699500747305[73] = 0; + out_2984795699500747305[74] = 0; + out_2984795699500747305[75] = 0; + out_2984795699500747305[76] = 0; + out_2984795699500747305[77] = 0; + out_2984795699500747305[78] = 0; + out_2984795699500747305[79] = 0; + out_2984795699500747305[80] = 1; } -void h_25(double *state, double *unused, double *out_7686598041594365016) { - out_7686598041594365016[0] = state[6]; +void h_25(double *state, double *unused, double *out_4848168669919202886) { + out_4848168669919202886[0] = state[6]; } -void H_25(double *state, double *unused, double *out_2220878862694778786) { - out_2220878862694778786[0] = 0; - out_2220878862694778786[1] = 0; - out_2220878862694778786[2] = 0; - out_2220878862694778786[3] = 0; - out_2220878862694778786[4] = 0; - out_2220878862694778786[5] = 0; - out_2220878862694778786[6] = 1; - out_2220878862694778786[7] = 0; - out_2220878862694778786[8] = 0; +void H_25(double *state, double *unused, double *out_7960279069568034739) { + out_7960279069568034739[0] = 0; + out_7960279069568034739[1] = 0; + out_7960279069568034739[2] = 0; + out_7960279069568034739[3] = 0; + out_7960279069568034739[4] = 0; + out_7960279069568034739[5] = 0; + out_7960279069568034739[6] = 1; + out_7960279069568034739[7] = 0; + out_7960279069568034739[8] = 0; } -void h_24(double *state, double *unused, double *out_3813839643785118531) { - out_3813839643785118531[0] = state[4]; - out_3813839643785118531[1] = state[5]; +void h_24(double *state, double *unused, double *out_5973332433336352466) { + out_5973332433336352466[0] = state[4]; + out_5973332433336352466[1] = state[5]; } -void H_24(double *state, double *unused, double *out_1337046736107544350) { - out_1337046736107544350[0] = 0; - out_1337046736107544350[1] = 0; - out_1337046736107544350[2] = 0; - out_1337046736107544350[3] = 0; - out_1337046736107544350[4] = 1; - out_1337046736107544350[5] = 0; - out_1337046736107544350[6] = 0; - out_1337046736107544350[7] = 0; - out_1337046736107544350[8] = 0; - out_1337046736107544350[9] = 0; - out_1337046736107544350[10] = 0; - out_1337046736107544350[11] = 0; - out_1337046736107544350[12] = 0; - out_1337046736107544350[13] = 0; - out_1337046736107544350[14] = 1; - out_1337046736107544350[15] = 0; - out_1337046736107544350[16] = 0; - out_1337046736107544350[17] = 0; +void H_24(double *state, double *unused, double *out_8313815405136017311) { + out_8313815405136017311[0] = 0; + out_8313815405136017311[1] = 0; + out_8313815405136017311[2] = 0; + out_8313815405136017311[3] = 0; + out_8313815405136017311[4] = 1; + out_8313815405136017311[5] = 0; + out_8313815405136017311[6] = 0; + out_8313815405136017311[7] = 0; + out_8313815405136017311[8] = 0; + out_8313815405136017311[9] = 0; + out_8313815405136017311[10] = 0; + out_8313815405136017311[11] = 0; + out_8313815405136017311[12] = 0; + out_8313815405136017311[13] = 0; + out_8313815405136017311[14] = 1; + out_8313815405136017311[15] = 0; + out_8313815405136017311[16] = 0; + out_8313815405136017311[17] = 0; } -void h_30(double *state, double *unused, double *out_2120268555396644716) { - out_2120268555396644716[0] = state[4]; +void h_30(double *state, double *unused, double *out_7443128207949688601) { + out_7443128207949688601[0] = state[4]; } -void H_30(double *state, double *unused, double *out_297454095812469841) { - out_297454095812469841[0] = 0; - out_297454095812469841[1] = 0; - out_297454095812469841[2] = 0; - out_297454095812469841[3] = 0; - out_297454095812469841[4] = 1; - out_297454095812469841[5] = 0; - out_297454095812469841[6] = 0; - out_297454095812469841[7] = 0; - out_297454095812469841[8] = 0; +void H_30(double *state, double *unused, double *out_5441946111060786112) { + out_5441946111060786112[0] = 0; + out_5441946111060786112[1] = 0; + out_5441946111060786112[2] = 0; + out_5441946111060786112[3] = 0; + out_5441946111060786112[4] = 1; + out_5441946111060786112[5] = 0; + out_5441946111060786112[6] = 0; + out_5441946111060786112[7] = 0; + out_5441946111060786112[8] = 0; } -void h_26(double *state, double *unused, double *out_2881404712264200929) { - out_2881404712264200929[0] = state[7]; +void h_26(double *state, double *unused, double *out_5333477160662303659) { + out_5333477160662303659[0] = state[7]; } -void H_26(double *state, double *unused, double *out_5962382181568835010) { - out_5962382181568835010[0] = 0; - out_5962382181568835010[1] = 0; - out_5962382181568835010[2] = 0; - out_5962382181568835010[3] = 0; - out_5962382181568835010[4] = 0; - out_5962382181568835010[5] = 0; - out_5962382181568835010[6] = 0; - out_5962382181568835010[7] = 1; - out_5962382181568835010[8] = 0; +void H_26(double *state, double *unused, double *out_6744961685267460653) { + out_6744961685267460653[0] = 0; + out_6744961685267460653[1] = 0; + out_6744961685267460653[2] = 0; + out_6744961685267460653[3] = 0; + out_6744961685267460653[4] = 0; + out_6744961685267460653[5] = 0; + out_6744961685267460653[6] = 0; + out_6744961685267460653[7] = 1; + out_6744961685267460653[8] = 0; } -void h_27(double *state, double *unused, double *out_901564640210476392) { - out_901564640210476392[0] = state[3]; +void h_27(double *state, double *unused, double *out_5010412721985507322) { + out_5010412721985507322[0] = state[3]; } -void H_27(double *state, double *unused, double *out_2521048166996413058) { - out_2521048166996413058[0] = 0; - out_2521048166996413058[1] = 0; - out_2521048166996413058[2] = 0; - out_2521048166996413058[3] = 1; - out_2521048166996413058[4] = 0; - out_2521048166996413058[5] = 0; - out_2521048166996413058[6] = 0; - out_2521048166996413058[7] = 0; - out_2521048166996413058[8] = 0; +void H_27(double *state, double *unused, double *out_8182362745197851896) { + out_8182362745197851896[0] = 0; + out_8182362745197851896[1] = 0; + out_8182362745197851896[2] = 0; + out_8182362745197851896[3] = 1; + out_8182362745197851896[4] = 0; + out_8182362745197851896[5] = 0; + out_8182362745197851896[6] = 0; + out_8182362745197851896[7] = 0; + out_8182362745197851896[8] = 0; } -void h_29(double *state, double *unused, double *out_952524754320502979) { - out_952524754320502979[0] = state[1]; +void h_29(double *state, double *unused, double *out_8399210544951165364) { + out_8399210544951165364[0] = state[1]; } -void H_29(double *state, double *unused, double *out_807685440126862025) { - out_807685440126862025[0] = 0; - out_807685440126862025[1] = 1; - out_807685440126862025[2] = 0; - out_807685440126862025[3] = 0; - out_807685440126862025[4] = 0; - out_807685440126862025[5] = 0; - out_807685440126862025[6] = 0; - out_807685440126862025[7] = 0; - out_807685440126862025[8] = 0; +void H_29(double *state, double *unused, double *out_6469000018328300863) { + out_6469000018328300863[0] = 0; + out_6469000018328300863[1] = 1; + out_6469000018328300863[2] = 0; + out_6469000018328300863[3] = 0; + out_6469000018328300863[4] = 0; + out_6469000018328300863[5] = 0; + out_6469000018328300863[6] = 0; + out_6469000018328300863[7] = 0; + out_6469000018328300863[8] = 0; } -void h_28(double *state, double *unused, double *out_8550909813521810734) { - out_8550909813521810734[0] = state[0]; +void h_28(double *state, double *unused, double *out_2638932451325827020) { + out_2638932451325827020[0] = state[0]; } -void H_28(double *state, double *unused, double *out_4274713576942668549) { - out_4274713576942668549[0] = 1; - out_4274713576942668549[1] = 0; - out_4274713576942668549[2] = 0; - out_4274713576942668549[3] = 0; - out_4274713576942668549[4] = 0; - out_4274713576942668549[5] = 0; - out_4274713576942668549[6] = 0; - out_4274713576942668549[7] = 0; - out_4274713576942668549[8] = 0; +void H_28(double *state, double *unused, double *out_8432630289893627114) { + out_8432630289893627114[0] = 1; + out_8432630289893627114[1] = 0; + out_8432630289893627114[2] = 0; + out_8432630289893627114[3] = 0; + out_8432630289893627114[4] = 0; + out_8432630289893627114[5] = 0; + out_8432630289893627114[6] = 0; + out_8432630289893627114[7] = 0; + out_8432630289893627114[8] = 0; } -void h_31(double *state, double *unused, double *out_7961792103878870905) { - out_7961792103878870905[0] = state[8]; +void h_31(double *state, double *unused, double *out_4572974607634696997) { + out_4572974607634696997[0] = state[8]; } -void H_31(double *state, double *unused, double *out_6588590283802186486) { - out_6588590283802186486[0] = 0; - out_6588590283802186486[1] = 0; - out_6588590283802186486[2] = 0; - out_6588590283802186486[3] = 0; - out_6588590283802186486[4] = 0; - out_6588590283802186486[5] = 0; - out_6588590283802186486[6] = 0; - out_6588590283802186486[7] = 0; - out_6588590283802186486[8] = 1; +void H_31(double *state, double *unused, double *out_6118753583034109177) { + out_6118753583034109177[0] = 0; + out_6118753583034109177[1] = 0; + out_6118753583034109177[2] = 0; + out_6118753583034109177[3] = 0; + out_6118753583034109177[4] = 0; + out_6118753583034109177[5] = 0; + out_6118753583034109177[6] = 0; + out_6118753583034109177[7] = 0; + out_6118753583034109177[8] = 1; } #include #include @@ -518,68 +518,68 @@ void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, doubl void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { update<1, 3, 0>(in_x, in_P, h_31, H_31, NULL, in_z, in_R, in_ea, MAHA_THRESH_31); } -void car_err_fun(double *nom_x, double *delta_x, double *out_1461273597009064329) { - err_fun(nom_x, delta_x, out_1461273597009064329); +void car_err_fun(double *nom_x, double *delta_x, double *out_4611641411917091701) { + err_fun(nom_x, delta_x, out_4611641411917091701); } -void car_inv_err_fun(double *nom_x, double *true_x, double *out_7289765730580897738) { - inv_err_fun(nom_x, true_x, out_7289765730580897738); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3968532722392907058) { + inv_err_fun(nom_x, true_x, out_3968532722392907058); } -void car_H_mod_fun(double *state, double *out_8304527178040913025) { - H_mod_fun(state, out_8304527178040913025); +void car_H_mod_fun(double *state, double *out_1232909156344202498) { + H_mod_fun(state, out_1232909156344202498); } -void car_f_fun(double *state, double dt, double *out_1355173870151422192) { - f_fun(state, dt, out_1355173870151422192); +void car_f_fun(double *state, double dt, double *out_241923337051056220) { + f_fun(state, dt, out_241923337051056220); } -void car_F_fun(double *state, double dt, double *out_3219998669912464347) { - F_fun(state, dt, out_3219998669912464347); +void car_F_fun(double *state, double dt, double *out_2984795699500747305) { + F_fun(state, dt, out_2984795699500747305); } -void car_h_25(double *state, double *unused, double *out_7686598041594365016) { - h_25(state, unused, out_7686598041594365016); +void car_h_25(double *state, double *unused, double *out_4848168669919202886) { + h_25(state, unused, out_4848168669919202886); } -void car_H_25(double *state, double *unused, double *out_2220878862694778786) { - H_25(state, unused, out_2220878862694778786); +void car_H_25(double *state, double *unused, double *out_7960279069568034739) { + H_25(state, unused, out_7960279069568034739); } -void car_h_24(double *state, double *unused, double *out_3813839643785118531) { - h_24(state, unused, out_3813839643785118531); +void car_h_24(double *state, double *unused, double *out_5973332433336352466) { + h_24(state, unused, out_5973332433336352466); } -void car_H_24(double *state, double *unused, double *out_1337046736107544350) { - H_24(state, unused, out_1337046736107544350); +void car_H_24(double *state, double *unused, double *out_8313815405136017311) { + H_24(state, unused, out_8313815405136017311); } -void car_h_30(double *state, double *unused, double *out_2120268555396644716) { - h_30(state, unused, out_2120268555396644716); +void car_h_30(double *state, double *unused, double *out_7443128207949688601) { + h_30(state, unused, out_7443128207949688601); } -void car_H_30(double *state, double *unused, double *out_297454095812469841) { - H_30(state, unused, out_297454095812469841); +void car_H_30(double *state, double *unused, double *out_5441946111060786112) { + H_30(state, unused, out_5441946111060786112); } -void car_h_26(double *state, double *unused, double *out_2881404712264200929) { - h_26(state, unused, out_2881404712264200929); +void car_h_26(double *state, double *unused, double *out_5333477160662303659) { + h_26(state, unused, out_5333477160662303659); } -void car_H_26(double *state, double *unused, double *out_5962382181568835010) { - H_26(state, unused, out_5962382181568835010); +void car_H_26(double *state, double *unused, double *out_6744961685267460653) { + H_26(state, unused, out_6744961685267460653); } -void car_h_27(double *state, double *unused, double *out_901564640210476392) { - h_27(state, unused, out_901564640210476392); +void car_h_27(double *state, double *unused, double *out_5010412721985507322) { + h_27(state, unused, out_5010412721985507322); } -void car_H_27(double *state, double *unused, double *out_2521048166996413058) { - H_27(state, unused, out_2521048166996413058); +void car_H_27(double *state, double *unused, double *out_8182362745197851896) { + H_27(state, unused, out_8182362745197851896); } -void car_h_29(double *state, double *unused, double *out_952524754320502979) { - h_29(state, unused, out_952524754320502979); +void car_h_29(double *state, double *unused, double *out_8399210544951165364) { + h_29(state, unused, out_8399210544951165364); } -void car_H_29(double *state, double *unused, double *out_807685440126862025) { - H_29(state, unused, out_807685440126862025); +void car_H_29(double *state, double *unused, double *out_6469000018328300863) { + H_29(state, unused, out_6469000018328300863); } -void car_h_28(double *state, double *unused, double *out_8550909813521810734) { - h_28(state, unused, out_8550909813521810734); +void car_h_28(double *state, double *unused, double *out_2638932451325827020) { + h_28(state, unused, out_2638932451325827020); } -void car_H_28(double *state, double *unused, double *out_4274713576942668549) { - H_28(state, unused, out_4274713576942668549); +void car_H_28(double *state, double *unused, double *out_8432630289893627114) { + H_28(state, unused, out_8432630289893627114); } -void car_h_31(double *state, double *unused, double *out_7961792103878870905) { - h_31(state, unused, out_7961792103878870905); +void car_h_31(double *state, double *unused, double *out_4572974607634696997) { + h_31(state, unused, out_4572974607634696997); } -void car_H_31(double *state, double *unused, double *out_6588590283802186486) { - H_31(state, unused, out_6588590283802186486); +void car_H_31(double *state, double *unused, double *out_6118753583034109177) { + H_31(state, unused, out_6118753583034109177); } void car_predict(double *in_x, double *in_P, double *in_Q, double dt) { predict(in_x, in_P, in_Q, dt); diff --git a/selfdrive/locationd/models/generated/car.h b/selfdrive/locationd/models/generated/car.h index 9b5b1538a..2c38541f2 100644 --- a/selfdrive/locationd/models/generated/car.h +++ b/selfdrive/locationd/models/generated/car.h @@ -9,27 +9,27 @@ void car_update_27(double *in_x, double *in_P, double *in_z, double *in_R, doubl void car_update_29(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void car_update_28(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void car_update_31(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); -void car_err_fun(double *nom_x, double *delta_x, double *out_1461273597009064329); -void car_inv_err_fun(double *nom_x, double *true_x, double *out_7289765730580897738); -void car_H_mod_fun(double *state, double *out_8304527178040913025); -void car_f_fun(double *state, double dt, double *out_1355173870151422192); -void car_F_fun(double *state, double dt, double *out_3219998669912464347); -void car_h_25(double *state, double *unused, double *out_7686598041594365016); -void car_H_25(double *state, double *unused, double *out_2220878862694778786); -void car_h_24(double *state, double *unused, double *out_3813839643785118531); -void car_H_24(double *state, double *unused, double *out_1337046736107544350); -void car_h_30(double *state, double *unused, double *out_2120268555396644716); -void car_H_30(double *state, double *unused, double *out_297454095812469841); -void car_h_26(double *state, double *unused, double *out_2881404712264200929); -void car_H_26(double *state, double *unused, double *out_5962382181568835010); -void car_h_27(double *state, double *unused, double *out_901564640210476392); -void car_H_27(double *state, double *unused, double *out_2521048166996413058); -void car_h_29(double *state, double *unused, double *out_952524754320502979); -void car_H_29(double *state, double *unused, double *out_807685440126862025); -void car_h_28(double *state, double *unused, double *out_8550909813521810734); -void car_H_28(double *state, double *unused, double *out_4274713576942668549); -void car_h_31(double *state, double *unused, double *out_7961792103878870905); -void car_H_31(double *state, double *unused, double *out_6588590283802186486); +void car_err_fun(double *nom_x, double *delta_x, double *out_4611641411917091701); +void car_inv_err_fun(double *nom_x, double *true_x, double *out_3968532722392907058); +void car_H_mod_fun(double *state, double *out_1232909156344202498); +void car_f_fun(double *state, double dt, double *out_241923337051056220); +void car_F_fun(double *state, double dt, double *out_2984795699500747305); +void car_h_25(double *state, double *unused, double *out_4848168669919202886); +void car_H_25(double *state, double *unused, double *out_7960279069568034739); +void car_h_24(double *state, double *unused, double *out_5973332433336352466); +void car_H_24(double *state, double *unused, double *out_8313815405136017311); +void car_h_30(double *state, double *unused, double *out_7443128207949688601); +void car_H_30(double *state, double *unused, double *out_5441946111060786112); +void car_h_26(double *state, double *unused, double *out_5333477160662303659); +void car_H_26(double *state, double *unused, double *out_6744961685267460653); +void car_h_27(double *state, double *unused, double *out_5010412721985507322); +void car_H_27(double *state, double *unused, double *out_8182362745197851896); +void car_h_29(double *state, double *unused, double *out_8399210544951165364); +void car_H_29(double *state, double *unused, double *out_6469000018328300863); +void car_h_28(double *state, double *unused, double *out_2638932451325827020); +void car_H_28(double *state, double *unused, double *out_8432630289893627114); +void car_h_31(double *state, double *unused, double *out_4572974607634696997); +void car_H_31(double *state, double *unused, double *out_6118753583034109177); void car_predict(double *in_x, double *in_P, double *in_Q, double dt); void car_set_mass(double x); void car_set_rotational_inertia(double x); diff --git a/selfdrive/locationd/models/generated/pose.cpp b/selfdrive/locationd/models/generated/pose.cpp index d8864232f..ac77c54ae 100644 --- a/selfdrive/locationd/models/generated/pose.cpp +++ b/selfdrive/locationd/models/generated/pose.cpp @@ -17,961 +17,961 @@ const static double MAHA_THRESH_14 = 7.814727903251177; * * * This file is part of 'ekf' * ******************************************************************************/ -void err_fun(double *nom_x, double *delta_x, double *out_7352766374165151156) { - out_7352766374165151156[0] = delta_x[0] + nom_x[0]; - out_7352766374165151156[1] = delta_x[1] + nom_x[1]; - out_7352766374165151156[2] = delta_x[2] + nom_x[2]; - out_7352766374165151156[3] = delta_x[3] + nom_x[3]; - out_7352766374165151156[4] = delta_x[4] + nom_x[4]; - out_7352766374165151156[5] = delta_x[5] + nom_x[5]; - out_7352766374165151156[6] = delta_x[6] + nom_x[6]; - out_7352766374165151156[7] = delta_x[7] + nom_x[7]; - out_7352766374165151156[8] = delta_x[8] + nom_x[8]; - out_7352766374165151156[9] = delta_x[9] + nom_x[9]; - out_7352766374165151156[10] = delta_x[10] + nom_x[10]; - out_7352766374165151156[11] = delta_x[11] + nom_x[11]; - out_7352766374165151156[12] = delta_x[12] + nom_x[12]; - out_7352766374165151156[13] = delta_x[13] + nom_x[13]; - out_7352766374165151156[14] = delta_x[14] + nom_x[14]; - out_7352766374165151156[15] = delta_x[15] + nom_x[15]; - out_7352766374165151156[16] = delta_x[16] + nom_x[16]; - out_7352766374165151156[17] = delta_x[17] + nom_x[17]; +void err_fun(double *nom_x, double *delta_x, double *out_8612662017984698055) { + out_8612662017984698055[0] = delta_x[0] + nom_x[0]; + out_8612662017984698055[1] = delta_x[1] + nom_x[1]; + out_8612662017984698055[2] = delta_x[2] + nom_x[2]; + out_8612662017984698055[3] = delta_x[3] + nom_x[3]; + out_8612662017984698055[4] = delta_x[4] + nom_x[4]; + out_8612662017984698055[5] = delta_x[5] + nom_x[5]; + out_8612662017984698055[6] = delta_x[6] + nom_x[6]; + out_8612662017984698055[7] = delta_x[7] + nom_x[7]; + out_8612662017984698055[8] = delta_x[8] + nom_x[8]; + out_8612662017984698055[9] = delta_x[9] + nom_x[9]; + out_8612662017984698055[10] = delta_x[10] + nom_x[10]; + out_8612662017984698055[11] = delta_x[11] + nom_x[11]; + out_8612662017984698055[12] = delta_x[12] + nom_x[12]; + out_8612662017984698055[13] = delta_x[13] + nom_x[13]; + out_8612662017984698055[14] = delta_x[14] + nom_x[14]; + out_8612662017984698055[15] = delta_x[15] + nom_x[15]; + out_8612662017984698055[16] = delta_x[16] + nom_x[16]; + out_8612662017984698055[17] = delta_x[17] + nom_x[17]; } -void inv_err_fun(double *nom_x, double *true_x, double *out_5943722919136625654) { - out_5943722919136625654[0] = -nom_x[0] + true_x[0]; - out_5943722919136625654[1] = -nom_x[1] + true_x[1]; - out_5943722919136625654[2] = -nom_x[2] + true_x[2]; - out_5943722919136625654[3] = -nom_x[3] + true_x[3]; - out_5943722919136625654[4] = -nom_x[4] + true_x[4]; - out_5943722919136625654[5] = -nom_x[5] + true_x[5]; - out_5943722919136625654[6] = -nom_x[6] + true_x[6]; - out_5943722919136625654[7] = -nom_x[7] + true_x[7]; - out_5943722919136625654[8] = -nom_x[8] + true_x[8]; - out_5943722919136625654[9] = -nom_x[9] + true_x[9]; - out_5943722919136625654[10] = -nom_x[10] + true_x[10]; - out_5943722919136625654[11] = -nom_x[11] + true_x[11]; - out_5943722919136625654[12] = -nom_x[12] + true_x[12]; - out_5943722919136625654[13] = -nom_x[13] + true_x[13]; - out_5943722919136625654[14] = -nom_x[14] + true_x[14]; - out_5943722919136625654[15] = -nom_x[15] + true_x[15]; - out_5943722919136625654[16] = -nom_x[16] + true_x[16]; - out_5943722919136625654[17] = -nom_x[17] + true_x[17]; +void inv_err_fun(double *nom_x, double *true_x, double *out_6583538545281744086) { + out_6583538545281744086[0] = -nom_x[0] + true_x[0]; + out_6583538545281744086[1] = -nom_x[1] + true_x[1]; + out_6583538545281744086[2] = -nom_x[2] + true_x[2]; + out_6583538545281744086[3] = -nom_x[3] + true_x[3]; + out_6583538545281744086[4] = -nom_x[4] + true_x[4]; + out_6583538545281744086[5] = -nom_x[5] + true_x[5]; + out_6583538545281744086[6] = -nom_x[6] + true_x[6]; + out_6583538545281744086[7] = -nom_x[7] + true_x[7]; + out_6583538545281744086[8] = -nom_x[8] + true_x[8]; + out_6583538545281744086[9] = -nom_x[9] + true_x[9]; + out_6583538545281744086[10] = -nom_x[10] + true_x[10]; + out_6583538545281744086[11] = -nom_x[11] + true_x[11]; + out_6583538545281744086[12] = -nom_x[12] + true_x[12]; + out_6583538545281744086[13] = -nom_x[13] + true_x[13]; + out_6583538545281744086[14] = -nom_x[14] + true_x[14]; + out_6583538545281744086[15] = -nom_x[15] + true_x[15]; + out_6583538545281744086[16] = -nom_x[16] + true_x[16]; + out_6583538545281744086[17] = -nom_x[17] + true_x[17]; } -void H_mod_fun(double *state, double *out_2863715751586478773) { - out_2863715751586478773[0] = 1.0; - out_2863715751586478773[1] = 0.0; - out_2863715751586478773[2] = 0.0; - out_2863715751586478773[3] = 0.0; - out_2863715751586478773[4] = 0.0; - out_2863715751586478773[5] = 0.0; - out_2863715751586478773[6] = 0.0; - out_2863715751586478773[7] = 0.0; - out_2863715751586478773[8] = 0.0; - out_2863715751586478773[9] = 0.0; - out_2863715751586478773[10] = 0.0; - out_2863715751586478773[11] = 0.0; - out_2863715751586478773[12] = 0.0; - out_2863715751586478773[13] = 0.0; - out_2863715751586478773[14] = 0.0; - out_2863715751586478773[15] = 0.0; - out_2863715751586478773[16] = 0.0; - out_2863715751586478773[17] = 0.0; - out_2863715751586478773[18] = 0.0; - out_2863715751586478773[19] = 1.0; - out_2863715751586478773[20] = 0.0; - out_2863715751586478773[21] = 0.0; - out_2863715751586478773[22] = 0.0; - out_2863715751586478773[23] = 0.0; - out_2863715751586478773[24] = 0.0; - out_2863715751586478773[25] = 0.0; - out_2863715751586478773[26] = 0.0; - out_2863715751586478773[27] = 0.0; - out_2863715751586478773[28] = 0.0; - out_2863715751586478773[29] = 0.0; - out_2863715751586478773[30] = 0.0; - out_2863715751586478773[31] = 0.0; - out_2863715751586478773[32] = 0.0; - out_2863715751586478773[33] = 0.0; - out_2863715751586478773[34] = 0.0; - out_2863715751586478773[35] = 0.0; - out_2863715751586478773[36] = 0.0; - out_2863715751586478773[37] = 0.0; - out_2863715751586478773[38] = 1.0; - out_2863715751586478773[39] = 0.0; - out_2863715751586478773[40] = 0.0; - out_2863715751586478773[41] = 0.0; - out_2863715751586478773[42] = 0.0; - out_2863715751586478773[43] = 0.0; - out_2863715751586478773[44] = 0.0; - out_2863715751586478773[45] = 0.0; - out_2863715751586478773[46] = 0.0; - out_2863715751586478773[47] = 0.0; - out_2863715751586478773[48] = 0.0; - out_2863715751586478773[49] = 0.0; - out_2863715751586478773[50] = 0.0; - out_2863715751586478773[51] = 0.0; - out_2863715751586478773[52] = 0.0; - out_2863715751586478773[53] = 0.0; - out_2863715751586478773[54] = 0.0; - out_2863715751586478773[55] = 0.0; - out_2863715751586478773[56] = 0.0; - out_2863715751586478773[57] = 1.0; - out_2863715751586478773[58] = 0.0; - out_2863715751586478773[59] = 0.0; - out_2863715751586478773[60] = 0.0; - out_2863715751586478773[61] = 0.0; - out_2863715751586478773[62] = 0.0; - out_2863715751586478773[63] = 0.0; - out_2863715751586478773[64] = 0.0; - out_2863715751586478773[65] = 0.0; - out_2863715751586478773[66] = 0.0; - out_2863715751586478773[67] = 0.0; - out_2863715751586478773[68] = 0.0; - out_2863715751586478773[69] = 0.0; - out_2863715751586478773[70] = 0.0; - out_2863715751586478773[71] = 0.0; - out_2863715751586478773[72] = 0.0; - out_2863715751586478773[73] = 0.0; - out_2863715751586478773[74] = 0.0; - out_2863715751586478773[75] = 0.0; - out_2863715751586478773[76] = 1.0; - out_2863715751586478773[77] = 0.0; - out_2863715751586478773[78] = 0.0; - out_2863715751586478773[79] = 0.0; - out_2863715751586478773[80] = 0.0; - out_2863715751586478773[81] = 0.0; - out_2863715751586478773[82] = 0.0; - out_2863715751586478773[83] = 0.0; - out_2863715751586478773[84] = 0.0; - out_2863715751586478773[85] = 0.0; - out_2863715751586478773[86] = 0.0; - out_2863715751586478773[87] = 0.0; - out_2863715751586478773[88] = 0.0; - out_2863715751586478773[89] = 0.0; - out_2863715751586478773[90] = 0.0; - out_2863715751586478773[91] = 0.0; - out_2863715751586478773[92] = 0.0; - out_2863715751586478773[93] = 0.0; - out_2863715751586478773[94] = 0.0; - out_2863715751586478773[95] = 1.0; - out_2863715751586478773[96] = 0.0; - out_2863715751586478773[97] = 0.0; - out_2863715751586478773[98] = 0.0; - out_2863715751586478773[99] = 0.0; - out_2863715751586478773[100] = 0.0; - out_2863715751586478773[101] = 0.0; - out_2863715751586478773[102] = 0.0; - out_2863715751586478773[103] = 0.0; - out_2863715751586478773[104] = 0.0; - out_2863715751586478773[105] = 0.0; - out_2863715751586478773[106] = 0.0; - out_2863715751586478773[107] = 0.0; - out_2863715751586478773[108] = 0.0; - out_2863715751586478773[109] = 0.0; - out_2863715751586478773[110] = 0.0; - out_2863715751586478773[111] = 0.0; - out_2863715751586478773[112] = 0.0; - out_2863715751586478773[113] = 0.0; - out_2863715751586478773[114] = 1.0; - out_2863715751586478773[115] = 0.0; - out_2863715751586478773[116] = 0.0; - out_2863715751586478773[117] = 0.0; - out_2863715751586478773[118] = 0.0; - out_2863715751586478773[119] = 0.0; - out_2863715751586478773[120] = 0.0; - out_2863715751586478773[121] = 0.0; - out_2863715751586478773[122] = 0.0; - out_2863715751586478773[123] = 0.0; - out_2863715751586478773[124] = 0.0; - out_2863715751586478773[125] = 0.0; - out_2863715751586478773[126] = 0.0; - out_2863715751586478773[127] = 0.0; - out_2863715751586478773[128] = 0.0; - out_2863715751586478773[129] = 0.0; - out_2863715751586478773[130] = 0.0; - out_2863715751586478773[131] = 0.0; - out_2863715751586478773[132] = 0.0; - out_2863715751586478773[133] = 1.0; - out_2863715751586478773[134] = 0.0; - out_2863715751586478773[135] = 0.0; - out_2863715751586478773[136] = 0.0; - out_2863715751586478773[137] = 0.0; - out_2863715751586478773[138] = 0.0; - out_2863715751586478773[139] = 0.0; - out_2863715751586478773[140] = 0.0; - out_2863715751586478773[141] = 0.0; - out_2863715751586478773[142] = 0.0; - out_2863715751586478773[143] = 0.0; - out_2863715751586478773[144] = 0.0; - out_2863715751586478773[145] = 0.0; - out_2863715751586478773[146] = 0.0; - out_2863715751586478773[147] = 0.0; - out_2863715751586478773[148] = 0.0; - out_2863715751586478773[149] = 0.0; - out_2863715751586478773[150] = 0.0; - out_2863715751586478773[151] = 0.0; - out_2863715751586478773[152] = 1.0; - out_2863715751586478773[153] = 0.0; - out_2863715751586478773[154] = 0.0; - out_2863715751586478773[155] = 0.0; - out_2863715751586478773[156] = 0.0; - out_2863715751586478773[157] = 0.0; - out_2863715751586478773[158] = 0.0; - out_2863715751586478773[159] = 0.0; - out_2863715751586478773[160] = 0.0; - out_2863715751586478773[161] = 0.0; - out_2863715751586478773[162] = 0.0; - out_2863715751586478773[163] = 0.0; - out_2863715751586478773[164] = 0.0; - out_2863715751586478773[165] = 0.0; - out_2863715751586478773[166] = 0.0; - out_2863715751586478773[167] = 0.0; - out_2863715751586478773[168] = 0.0; - out_2863715751586478773[169] = 0.0; - out_2863715751586478773[170] = 0.0; - out_2863715751586478773[171] = 1.0; - out_2863715751586478773[172] = 0.0; - out_2863715751586478773[173] = 0.0; - out_2863715751586478773[174] = 0.0; - out_2863715751586478773[175] = 0.0; - out_2863715751586478773[176] = 0.0; - out_2863715751586478773[177] = 0.0; - out_2863715751586478773[178] = 0.0; - out_2863715751586478773[179] = 0.0; - out_2863715751586478773[180] = 0.0; - out_2863715751586478773[181] = 0.0; - out_2863715751586478773[182] = 0.0; - out_2863715751586478773[183] = 0.0; - out_2863715751586478773[184] = 0.0; - out_2863715751586478773[185] = 0.0; - out_2863715751586478773[186] = 0.0; - out_2863715751586478773[187] = 0.0; - out_2863715751586478773[188] = 0.0; - out_2863715751586478773[189] = 0.0; - out_2863715751586478773[190] = 1.0; - out_2863715751586478773[191] = 0.0; - out_2863715751586478773[192] = 0.0; - out_2863715751586478773[193] = 0.0; - out_2863715751586478773[194] = 0.0; - out_2863715751586478773[195] = 0.0; - out_2863715751586478773[196] = 0.0; - out_2863715751586478773[197] = 0.0; - out_2863715751586478773[198] = 0.0; - out_2863715751586478773[199] = 0.0; - out_2863715751586478773[200] = 0.0; - out_2863715751586478773[201] = 0.0; - out_2863715751586478773[202] = 0.0; - out_2863715751586478773[203] = 0.0; - out_2863715751586478773[204] = 0.0; - out_2863715751586478773[205] = 0.0; - out_2863715751586478773[206] = 0.0; - out_2863715751586478773[207] = 0.0; - out_2863715751586478773[208] = 0.0; - out_2863715751586478773[209] = 1.0; - out_2863715751586478773[210] = 0.0; - out_2863715751586478773[211] = 0.0; - out_2863715751586478773[212] = 0.0; - out_2863715751586478773[213] = 0.0; - out_2863715751586478773[214] = 0.0; - out_2863715751586478773[215] = 0.0; - out_2863715751586478773[216] = 0.0; - out_2863715751586478773[217] = 0.0; - out_2863715751586478773[218] = 0.0; - out_2863715751586478773[219] = 0.0; - out_2863715751586478773[220] = 0.0; - out_2863715751586478773[221] = 0.0; - out_2863715751586478773[222] = 0.0; - out_2863715751586478773[223] = 0.0; - out_2863715751586478773[224] = 0.0; - out_2863715751586478773[225] = 0.0; - out_2863715751586478773[226] = 0.0; - out_2863715751586478773[227] = 0.0; - out_2863715751586478773[228] = 1.0; - out_2863715751586478773[229] = 0.0; - out_2863715751586478773[230] = 0.0; - out_2863715751586478773[231] = 0.0; - out_2863715751586478773[232] = 0.0; - out_2863715751586478773[233] = 0.0; - out_2863715751586478773[234] = 0.0; - out_2863715751586478773[235] = 0.0; - out_2863715751586478773[236] = 0.0; - out_2863715751586478773[237] = 0.0; - out_2863715751586478773[238] = 0.0; - out_2863715751586478773[239] = 0.0; - out_2863715751586478773[240] = 0.0; - out_2863715751586478773[241] = 0.0; - out_2863715751586478773[242] = 0.0; - out_2863715751586478773[243] = 0.0; - out_2863715751586478773[244] = 0.0; - out_2863715751586478773[245] = 0.0; - out_2863715751586478773[246] = 0.0; - out_2863715751586478773[247] = 1.0; - out_2863715751586478773[248] = 0.0; - out_2863715751586478773[249] = 0.0; - out_2863715751586478773[250] = 0.0; - out_2863715751586478773[251] = 0.0; - out_2863715751586478773[252] = 0.0; - out_2863715751586478773[253] = 0.0; - out_2863715751586478773[254] = 0.0; - out_2863715751586478773[255] = 0.0; - out_2863715751586478773[256] = 0.0; - out_2863715751586478773[257] = 0.0; - out_2863715751586478773[258] = 0.0; - out_2863715751586478773[259] = 0.0; - out_2863715751586478773[260] = 0.0; - out_2863715751586478773[261] = 0.0; - out_2863715751586478773[262] = 0.0; - out_2863715751586478773[263] = 0.0; - out_2863715751586478773[264] = 0.0; - out_2863715751586478773[265] = 0.0; - out_2863715751586478773[266] = 1.0; - out_2863715751586478773[267] = 0.0; - out_2863715751586478773[268] = 0.0; - out_2863715751586478773[269] = 0.0; - out_2863715751586478773[270] = 0.0; - out_2863715751586478773[271] = 0.0; - out_2863715751586478773[272] = 0.0; - out_2863715751586478773[273] = 0.0; - out_2863715751586478773[274] = 0.0; - out_2863715751586478773[275] = 0.0; - out_2863715751586478773[276] = 0.0; - out_2863715751586478773[277] = 0.0; - out_2863715751586478773[278] = 0.0; - out_2863715751586478773[279] = 0.0; - out_2863715751586478773[280] = 0.0; - out_2863715751586478773[281] = 0.0; - out_2863715751586478773[282] = 0.0; - out_2863715751586478773[283] = 0.0; - out_2863715751586478773[284] = 0.0; - out_2863715751586478773[285] = 1.0; - out_2863715751586478773[286] = 0.0; - out_2863715751586478773[287] = 0.0; - out_2863715751586478773[288] = 0.0; - out_2863715751586478773[289] = 0.0; - out_2863715751586478773[290] = 0.0; - out_2863715751586478773[291] = 0.0; - out_2863715751586478773[292] = 0.0; - out_2863715751586478773[293] = 0.0; - out_2863715751586478773[294] = 0.0; - out_2863715751586478773[295] = 0.0; - out_2863715751586478773[296] = 0.0; - out_2863715751586478773[297] = 0.0; - out_2863715751586478773[298] = 0.0; - out_2863715751586478773[299] = 0.0; - out_2863715751586478773[300] = 0.0; - out_2863715751586478773[301] = 0.0; - out_2863715751586478773[302] = 0.0; - out_2863715751586478773[303] = 0.0; - out_2863715751586478773[304] = 1.0; - out_2863715751586478773[305] = 0.0; - out_2863715751586478773[306] = 0.0; - out_2863715751586478773[307] = 0.0; - out_2863715751586478773[308] = 0.0; - out_2863715751586478773[309] = 0.0; - out_2863715751586478773[310] = 0.0; - out_2863715751586478773[311] = 0.0; - out_2863715751586478773[312] = 0.0; - out_2863715751586478773[313] = 0.0; - out_2863715751586478773[314] = 0.0; - out_2863715751586478773[315] = 0.0; - out_2863715751586478773[316] = 0.0; - out_2863715751586478773[317] = 0.0; - out_2863715751586478773[318] = 0.0; - out_2863715751586478773[319] = 0.0; - out_2863715751586478773[320] = 0.0; - out_2863715751586478773[321] = 0.0; - out_2863715751586478773[322] = 0.0; - out_2863715751586478773[323] = 1.0; +void H_mod_fun(double *state, double *out_7537065087244600898) { + out_7537065087244600898[0] = 1.0; + out_7537065087244600898[1] = 0.0; + out_7537065087244600898[2] = 0.0; + out_7537065087244600898[3] = 0.0; + out_7537065087244600898[4] = 0.0; + out_7537065087244600898[5] = 0.0; + out_7537065087244600898[6] = 0.0; + out_7537065087244600898[7] = 0.0; + out_7537065087244600898[8] = 0.0; + out_7537065087244600898[9] = 0.0; + out_7537065087244600898[10] = 0.0; + out_7537065087244600898[11] = 0.0; + out_7537065087244600898[12] = 0.0; + out_7537065087244600898[13] = 0.0; + out_7537065087244600898[14] = 0.0; + out_7537065087244600898[15] = 0.0; + out_7537065087244600898[16] = 0.0; + out_7537065087244600898[17] = 0.0; + out_7537065087244600898[18] = 0.0; + out_7537065087244600898[19] = 1.0; + out_7537065087244600898[20] = 0.0; + out_7537065087244600898[21] = 0.0; + out_7537065087244600898[22] = 0.0; + out_7537065087244600898[23] = 0.0; + out_7537065087244600898[24] = 0.0; + out_7537065087244600898[25] = 0.0; + out_7537065087244600898[26] = 0.0; + out_7537065087244600898[27] = 0.0; + out_7537065087244600898[28] = 0.0; + out_7537065087244600898[29] = 0.0; + out_7537065087244600898[30] = 0.0; + out_7537065087244600898[31] = 0.0; + out_7537065087244600898[32] = 0.0; + out_7537065087244600898[33] = 0.0; + out_7537065087244600898[34] = 0.0; + out_7537065087244600898[35] = 0.0; + out_7537065087244600898[36] = 0.0; + out_7537065087244600898[37] = 0.0; + out_7537065087244600898[38] = 1.0; + out_7537065087244600898[39] = 0.0; + out_7537065087244600898[40] = 0.0; + out_7537065087244600898[41] = 0.0; + out_7537065087244600898[42] = 0.0; + out_7537065087244600898[43] = 0.0; + out_7537065087244600898[44] = 0.0; + out_7537065087244600898[45] = 0.0; + out_7537065087244600898[46] = 0.0; + out_7537065087244600898[47] = 0.0; + out_7537065087244600898[48] = 0.0; + out_7537065087244600898[49] = 0.0; + out_7537065087244600898[50] = 0.0; + out_7537065087244600898[51] = 0.0; + out_7537065087244600898[52] = 0.0; + out_7537065087244600898[53] = 0.0; + out_7537065087244600898[54] = 0.0; + out_7537065087244600898[55] = 0.0; + out_7537065087244600898[56] = 0.0; + out_7537065087244600898[57] = 1.0; + out_7537065087244600898[58] = 0.0; + out_7537065087244600898[59] = 0.0; + out_7537065087244600898[60] = 0.0; + out_7537065087244600898[61] = 0.0; + out_7537065087244600898[62] = 0.0; + out_7537065087244600898[63] = 0.0; + out_7537065087244600898[64] = 0.0; + out_7537065087244600898[65] = 0.0; + out_7537065087244600898[66] = 0.0; + out_7537065087244600898[67] = 0.0; + out_7537065087244600898[68] = 0.0; + out_7537065087244600898[69] = 0.0; + out_7537065087244600898[70] = 0.0; + out_7537065087244600898[71] = 0.0; + out_7537065087244600898[72] = 0.0; + out_7537065087244600898[73] = 0.0; + out_7537065087244600898[74] = 0.0; + out_7537065087244600898[75] = 0.0; + out_7537065087244600898[76] = 1.0; + out_7537065087244600898[77] = 0.0; + out_7537065087244600898[78] = 0.0; + out_7537065087244600898[79] = 0.0; + out_7537065087244600898[80] = 0.0; + out_7537065087244600898[81] = 0.0; + out_7537065087244600898[82] = 0.0; + out_7537065087244600898[83] = 0.0; + out_7537065087244600898[84] = 0.0; + out_7537065087244600898[85] = 0.0; + out_7537065087244600898[86] = 0.0; + out_7537065087244600898[87] = 0.0; + out_7537065087244600898[88] = 0.0; + out_7537065087244600898[89] = 0.0; + out_7537065087244600898[90] = 0.0; + out_7537065087244600898[91] = 0.0; + out_7537065087244600898[92] = 0.0; + out_7537065087244600898[93] = 0.0; + out_7537065087244600898[94] = 0.0; + out_7537065087244600898[95] = 1.0; + out_7537065087244600898[96] = 0.0; + out_7537065087244600898[97] = 0.0; + out_7537065087244600898[98] = 0.0; + out_7537065087244600898[99] = 0.0; + out_7537065087244600898[100] = 0.0; + out_7537065087244600898[101] = 0.0; + out_7537065087244600898[102] = 0.0; + out_7537065087244600898[103] = 0.0; + out_7537065087244600898[104] = 0.0; + out_7537065087244600898[105] = 0.0; + out_7537065087244600898[106] = 0.0; + out_7537065087244600898[107] = 0.0; + out_7537065087244600898[108] = 0.0; + out_7537065087244600898[109] = 0.0; + out_7537065087244600898[110] = 0.0; + out_7537065087244600898[111] = 0.0; + out_7537065087244600898[112] = 0.0; + out_7537065087244600898[113] = 0.0; + out_7537065087244600898[114] = 1.0; + out_7537065087244600898[115] = 0.0; + out_7537065087244600898[116] = 0.0; + out_7537065087244600898[117] = 0.0; + out_7537065087244600898[118] = 0.0; + out_7537065087244600898[119] = 0.0; + out_7537065087244600898[120] = 0.0; + out_7537065087244600898[121] = 0.0; + out_7537065087244600898[122] = 0.0; + out_7537065087244600898[123] = 0.0; + out_7537065087244600898[124] = 0.0; + out_7537065087244600898[125] = 0.0; + out_7537065087244600898[126] = 0.0; + out_7537065087244600898[127] = 0.0; + out_7537065087244600898[128] = 0.0; + out_7537065087244600898[129] = 0.0; + out_7537065087244600898[130] = 0.0; + out_7537065087244600898[131] = 0.0; + out_7537065087244600898[132] = 0.0; + out_7537065087244600898[133] = 1.0; + out_7537065087244600898[134] = 0.0; + out_7537065087244600898[135] = 0.0; + out_7537065087244600898[136] = 0.0; + out_7537065087244600898[137] = 0.0; + out_7537065087244600898[138] = 0.0; + out_7537065087244600898[139] = 0.0; + out_7537065087244600898[140] = 0.0; + out_7537065087244600898[141] = 0.0; + out_7537065087244600898[142] = 0.0; + out_7537065087244600898[143] = 0.0; + out_7537065087244600898[144] = 0.0; + out_7537065087244600898[145] = 0.0; + out_7537065087244600898[146] = 0.0; + out_7537065087244600898[147] = 0.0; + out_7537065087244600898[148] = 0.0; + out_7537065087244600898[149] = 0.0; + out_7537065087244600898[150] = 0.0; + out_7537065087244600898[151] = 0.0; + out_7537065087244600898[152] = 1.0; + out_7537065087244600898[153] = 0.0; + out_7537065087244600898[154] = 0.0; + out_7537065087244600898[155] = 0.0; + out_7537065087244600898[156] = 0.0; + out_7537065087244600898[157] = 0.0; + out_7537065087244600898[158] = 0.0; + out_7537065087244600898[159] = 0.0; + out_7537065087244600898[160] = 0.0; + out_7537065087244600898[161] = 0.0; + out_7537065087244600898[162] = 0.0; + out_7537065087244600898[163] = 0.0; + out_7537065087244600898[164] = 0.0; + out_7537065087244600898[165] = 0.0; + out_7537065087244600898[166] = 0.0; + out_7537065087244600898[167] = 0.0; + out_7537065087244600898[168] = 0.0; + out_7537065087244600898[169] = 0.0; + out_7537065087244600898[170] = 0.0; + out_7537065087244600898[171] = 1.0; + out_7537065087244600898[172] = 0.0; + out_7537065087244600898[173] = 0.0; + out_7537065087244600898[174] = 0.0; + out_7537065087244600898[175] = 0.0; + out_7537065087244600898[176] = 0.0; + out_7537065087244600898[177] = 0.0; + out_7537065087244600898[178] = 0.0; + out_7537065087244600898[179] = 0.0; + out_7537065087244600898[180] = 0.0; + out_7537065087244600898[181] = 0.0; + out_7537065087244600898[182] = 0.0; + out_7537065087244600898[183] = 0.0; + out_7537065087244600898[184] = 0.0; + out_7537065087244600898[185] = 0.0; + out_7537065087244600898[186] = 0.0; + out_7537065087244600898[187] = 0.0; + out_7537065087244600898[188] = 0.0; + out_7537065087244600898[189] = 0.0; + out_7537065087244600898[190] = 1.0; + out_7537065087244600898[191] = 0.0; + out_7537065087244600898[192] = 0.0; + out_7537065087244600898[193] = 0.0; + out_7537065087244600898[194] = 0.0; + out_7537065087244600898[195] = 0.0; + out_7537065087244600898[196] = 0.0; + out_7537065087244600898[197] = 0.0; + out_7537065087244600898[198] = 0.0; + out_7537065087244600898[199] = 0.0; + out_7537065087244600898[200] = 0.0; + out_7537065087244600898[201] = 0.0; + out_7537065087244600898[202] = 0.0; + out_7537065087244600898[203] = 0.0; + out_7537065087244600898[204] = 0.0; + out_7537065087244600898[205] = 0.0; + out_7537065087244600898[206] = 0.0; + out_7537065087244600898[207] = 0.0; + out_7537065087244600898[208] = 0.0; + out_7537065087244600898[209] = 1.0; + out_7537065087244600898[210] = 0.0; + out_7537065087244600898[211] = 0.0; + out_7537065087244600898[212] = 0.0; + out_7537065087244600898[213] = 0.0; + out_7537065087244600898[214] = 0.0; + out_7537065087244600898[215] = 0.0; + out_7537065087244600898[216] = 0.0; + out_7537065087244600898[217] = 0.0; + out_7537065087244600898[218] = 0.0; + out_7537065087244600898[219] = 0.0; + out_7537065087244600898[220] = 0.0; + out_7537065087244600898[221] = 0.0; + out_7537065087244600898[222] = 0.0; + out_7537065087244600898[223] = 0.0; + out_7537065087244600898[224] = 0.0; + out_7537065087244600898[225] = 0.0; + out_7537065087244600898[226] = 0.0; + out_7537065087244600898[227] = 0.0; + out_7537065087244600898[228] = 1.0; + out_7537065087244600898[229] = 0.0; + out_7537065087244600898[230] = 0.0; + out_7537065087244600898[231] = 0.0; + out_7537065087244600898[232] = 0.0; + out_7537065087244600898[233] = 0.0; + out_7537065087244600898[234] = 0.0; + out_7537065087244600898[235] = 0.0; + out_7537065087244600898[236] = 0.0; + out_7537065087244600898[237] = 0.0; + out_7537065087244600898[238] = 0.0; + out_7537065087244600898[239] = 0.0; + out_7537065087244600898[240] = 0.0; + out_7537065087244600898[241] = 0.0; + out_7537065087244600898[242] = 0.0; + out_7537065087244600898[243] = 0.0; + out_7537065087244600898[244] = 0.0; + out_7537065087244600898[245] = 0.0; + out_7537065087244600898[246] = 0.0; + out_7537065087244600898[247] = 1.0; + out_7537065087244600898[248] = 0.0; + out_7537065087244600898[249] = 0.0; + out_7537065087244600898[250] = 0.0; + out_7537065087244600898[251] = 0.0; + out_7537065087244600898[252] = 0.0; + out_7537065087244600898[253] = 0.0; + out_7537065087244600898[254] = 0.0; + out_7537065087244600898[255] = 0.0; + out_7537065087244600898[256] = 0.0; + out_7537065087244600898[257] = 0.0; + out_7537065087244600898[258] = 0.0; + out_7537065087244600898[259] = 0.0; + out_7537065087244600898[260] = 0.0; + out_7537065087244600898[261] = 0.0; + out_7537065087244600898[262] = 0.0; + out_7537065087244600898[263] = 0.0; + out_7537065087244600898[264] = 0.0; + out_7537065087244600898[265] = 0.0; + out_7537065087244600898[266] = 1.0; + out_7537065087244600898[267] = 0.0; + out_7537065087244600898[268] = 0.0; + out_7537065087244600898[269] = 0.0; + out_7537065087244600898[270] = 0.0; + out_7537065087244600898[271] = 0.0; + out_7537065087244600898[272] = 0.0; + out_7537065087244600898[273] = 0.0; + out_7537065087244600898[274] = 0.0; + out_7537065087244600898[275] = 0.0; + out_7537065087244600898[276] = 0.0; + out_7537065087244600898[277] = 0.0; + out_7537065087244600898[278] = 0.0; + out_7537065087244600898[279] = 0.0; + out_7537065087244600898[280] = 0.0; + out_7537065087244600898[281] = 0.0; + out_7537065087244600898[282] = 0.0; + out_7537065087244600898[283] = 0.0; + out_7537065087244600898[284] = 0.0; + out_7537065087244600898[285] = 1.0; + out_7537065087244600898[286] = 0.0; + out_7537065087244600898[287] = 0.0; + out_7537065087244600898[288] = 0.0; + out_7537065087244600898[289] = 0.0; + out_7537065087244600898[290] = 0.0; + out_7537065087244600898[291] = 0.0; + out_7537065087244600898[292] = 0.0; + out_7537065087244600898[293] = 0.0; + out_7537065087244600898[294] = 0.0; + out_7537065087244600898[295] = 0.0; + out_7537065087244600898[296] = 0.0; + out_7537065087244600898[297] = 0.0; + out_7537065087244600898[298] = 0.0; + out_7537065087244600898[299] = 0.0; + out_7537065087244600898[300] = 0.0; + out_7537065087244600898[301] = 0.0; + out_7537065087244600898[302] = 0.0; + out_7537065087244600898[303] = 0.0; + out_7537065087244600898[304] = 1.0; + out_7537065087244600898[305] = 0.0; + out_7537065087244600898[306] = 0.0; + out_7537065087244600898[307] = 0.0; + out_7537065087244600898[308] = 0.0; + out_7537065087244600898[309] = 0.0; + out_7537065087244600898[310] = 0.0; + out_7537065087244600898[311] = 0.0; + out_7537065087244600898[312] = 0.0; + out_7537065087244600898[313] = 0.0; + out_7537065087244600898[314] = 0.0; + out_7537065087244600898[315] = 0.0; + out_7537065087244600898[316] = 0.0; + out_7537065087244600898[317] = 0.0; + out_7537065087244600898[318] = 0.0; + out_7537065087244600898[319] = 0.0; + out_7537065087244600898[320] = 0.0; + out_7537065087244600898[321] = 0.0; + out_7537065087244600898[322] = 0.0; + out_7537065087244600898[323] = 1.0; } -void f_fun(double *state, double dt, double *out_4163803070138990822) { - out_4163803070138990822[0] = atan2((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), -(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1])); - out_4163803070138990822[1] = asin(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8])); - out_4163803070138990822[2] = atan2(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), -(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2])); - out_4163803070138990822[3] = dt*state[12] + state[3]; - out_4163803070138990822[4] = dt*state[13] + state[4]; - out_4163803070138990822[5] = dt*state[14] + state[5]; - out_4163803070138990822[6] = state[6]; - out_4163803070138990822[7] = state[7]; - out_4163803070138990822[8] = state[8]; - out_4163803070138990822[9] = state[9]; - out_4163803070138990822[10] = state[10]; - out_4163803070138990822[11] = state[11]; - out_4163803070138990822[12] = state[12]; - out_4163803070138990822[13] = state[13]; - out_4163803070138990822[14] = state[14]; - out_4163803070138990822[15] = state[15]; - out_4163803070138990822[16] = state[16]; - out_4163803070138990822[17] = state[17]; +void f_fun(double *state, double dt, double *out_6289537546199484080) { + out_6289537546199484080[0] = atan2((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), -(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1])); + out_6289537546199484080[1] = asin(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8])); + out_6289537546199484080[2] = atan2(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), -(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2])); + out_6289537546199484080[3] = dt*state[12] + state[3]; + out_6289537546199484080[4] = dt*state[13] + state[4]; + out_6289537546199484080[5] = dt*state[14] + state[5]; + out_6289537546199484080[6] = state[6]; + out_6289537546199484080[7] = state[7]; + out_6289537546199484080[8] = state[8]; + out_6289537546199484080[9] = state[9]; + out_6289537546199484080[10] = state[10]; + out_6289537546199484080[11] = state[11]; + out_6289537546199484080[12] = state[12]; + out_6289537546199484080[13] = state[13]; + out_6289537546199484080[14] = state[14]; + out_6289537546199484080[15] = state[15]; + out_6289537546199484080[16] = state[16]; + out_6289537546199484080[17] = state[17]; } -void F_fun(double *state, double dt, double *out_3325085581577154625) { - out_3325085581577154625[0] = ((-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*cos(state[0])*cos(state[1]) - sin(state[0])*cos(dt*state[6])*cos(dt*state[7])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + ((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*cos(state[0])*cos(state[1]) - sin(dt*state[6])*sin(state[0])*cos(dt*state[7])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_3325085581577154625[1] = ((-sin(dt*state[6])*sin(dt*state[8]) - sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*cos(state[1]) - (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*sin(state[1]) - sin(state[1])*cos(dt*state[6])*cos(dt*state[7])*cos(state[0]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*sin(state[1]) + (-sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) + sin(dt*state[8])*cos(dt*state[6]))*cos(state[1]) - sin(dt*state[6])*sin(state[1])*cos(dt*state[7])*cos(state[0]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_3325085581577154625[2] = 0; - out_3325085581577154625[3] = 0; - out_3325085581577154625[4] = 0; - out_3325085581577154625[5] = 0; - out_3325085581577154625[6] = (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(dt*cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*sin(dt*state[8]) - dt*sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-dt*sin(dt*state[6])*cos(dt*state[8]) + dt*sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) - dt*cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (dt*sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_3325085581577154625[7] = (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[6])*sin(dt*state[7])*cos(state[0])*cos(state[1]) + dt*sin(dt*state[6])*sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) - dt*sin(dt*state[6])*sin(state[1])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[7])*cos(dt*state[6])*cos(state[0])*cos(state[1]) + dt*sin(dt*state[8])*sin(state[0])*cos(dt*state[6])*cos(dt*state[7])*cos(state[1]) - dt*sin(state[1])*cos(dt*state[6])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_3325085581577154625[8] = ((dt*sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + dt*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (dt*sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + ((dt*sin(dt*state[6])*sin(dt*state[8]) + dt*sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*cos(dt*state[8]) + dt*sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); - out_3325085581577154625[9] = 0; - out_3325085581577154625[10] = 0; - out_3325085581577154625[11] = 0; - out_3325085581577154625[12] = 0; - out_3325085581577154625[13] = 0; - out_3325085581577154625[14] = 0; - out_3325085581577154625[15] = 0; - out_3325085581577154625[16] = 0; - out_3325085581577154625[17] = 0; - out_3325085581577154625[18] = (-sin(dt*state[7])*sin(state[0])*cos(state[1]) - sin(dt*state[8])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_3325085581577154625[19] = (-sin(dt*state[7])*sin(state[1])*cos(state[0]) + sin(dt*state[8])*sin(state[0])*sin(state[1])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_3325085581577154625[20] = 0; - out_3325085581577154625[21] = 0; - out_3325085581577154625[22] = 0; - out_3325085581577154625[23] = 0; - out_3325085581577154625[24] = 0; - out_3325085581577154625[25] = (dt*sin(dt*state[7])*sin(dt*state[8])*sin(state[0])*cos(state[1]) - dt*sin(dt*state[7])*sin(state[1])*cos(dt*state[8]) + dt*cos(dt*state[7])*cos(state[0])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_3325085581577154625[26] = (-dt*sin(dt*state[8])*sin(state[1])*cos(dt*state[7]) - dt*sin(state[0])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); - out_3325085581577154625[27] = 0; - out_3325085581577154625[28] = 0; - out_3325085581577154625[29] = 0; - out_3325085581577154625[30] = 0; - out_3325085581577154625[31] = 0; - out_3325085581577154625[32] = 0; - out_3325085581577154625[33] = 0; - out_3325085581577154625[34] = 0; - out_3325085581577154625[35] = 0; - out_3325085581577154625[36] = ((sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[7]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[7]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_3325085581577154625[37] = (-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(-sin(dt*state[7])*sin(state[2])*cos(state[0])*cos(state[1]) + sin(dt*state[8])*sin(state[0])*sin(state[2])*cos(dt*state[7])*cos(state[1]) - sin(state[1])*sin(state[2])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*(-sin(dt*state[7])*cos(state[0])*cos(state[1])*cos(state[2]) + sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1])*cos(state[2]) - sin(state[1])*cos(dt*state[7])*cos(dt*state[8])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_3325085581577154625[38] = ((-sin(state[0])*sin(state[2]) - sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (-sin(state[0])*sin(state[1])*sin(state[2]) - cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_3325085581577154625[39] = 0; - out_3325085581577154625[40] = 0; - out_3325085581577154625[41] = 0; - out_3325085581577154625[42] = 0; - out_3325085581577154625[43] = (-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(dt*(sin(state[0])*cos(state[2]) - sin(state[1])*sin(state[2])*cos(state[0]))*cos(dt*state[7]) - dt*(sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[7])*sin(dt*state[8]) - dt*sin(dt*state[7])*sin(state[2])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*(dt*(-sin(state[0])*sin(state[2]) - sin(state[1])*cos(state[0])*cos(state[2]))*cos(dt*state[7]) - dt*(sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[7])*sin(dt*state[8]) - dt*sin(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_3325085581577154625[44] = (dt*(sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*cos(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*sin(state[2])*cos(dt*state[7])*cos(state[1]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + (dt*(sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*cos(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[7])*cos(state[1])*cos(state[2]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); - out_3325085581577154625[45] = 0; - out_3325085581577154625[46] = 0; - out_3325085581577154625[47] = 0; - out_3325085581577154625[48] = 0; - out_3325085581577154625[49] = 0; - out_3325085581577154625[50] = 0; - out_3325085581577154625[51] = 0; - out_3325085581577154625[52] = 0; - out_3325085581577154625[53] = 0; - out_3325085581577154625[54] = 0; - out_3325085581577154625[55] = 0; - out_3325085581577154625[56] = 0; - out_3325085581577154625[57] = 1; - out_3325085581577154625[58] = 0; - out_3325085581577154625[59] = 0; - out_3325085581577154625[60] = 0; - out_3325085581577154625[61] = 0; - out_3325085581577154625[62] = 0; - out_3325085581577154625[63] = 0; - out_3325085581577154625[64] = 0; - out_3325085581577154625[65] = 0; - out_3325085581577154625[66] = dt; - out_3325085581577154625[67] = 0; - out_3325085581577154625[68] = 0; - out_3325085581577154625[69] = 0; - out_3325085581577154625[70] = 0; - out_3325085581577154625[71] = 0; - out_3325085581577154625[72] = 0; - out_3325085581577154625[73] = 0; - out_3325085581577154625[74] = 0; - out_3325085581577154625[75] = 0; - out_3325085581577154625[76] = 1; - out_3325085581577154625[77] = 0; - out_3325085581577154625[78] = 0; - out_3325085581577154625[79] = 0; - out_3325085581577154625[80] = 0; - out_3325085581577154625[81] = 0; - out_3325085581577154625[82] = 0; - out_3325085581577154625[83] = 0; - out_3325085581577154625[84] = 0; - out_3325085581577154625[85] = dt; - out_3325085581577154625[86] = 0; - out_3325085581577154625[87] = 0; - out_3325085581577154625[88] = 0; - out_3325085581577154625[89] = 0; - out_3325085581577154625[90] = 0; - out_3325085581577154625[91] = 0; - out_3325085581577154625[92] = 0; - out_3325085581577154625[93] = 0; - out_3325085581577154625[94] = 0; - out_3325085581577154625[95] = 1; - out_3325085581577154625[96] = 0; - out_3325085581577154625[97] = 0; - out_3325085581577154625[98] = 0; - out_3325085581577154625[99] = 0; - out_3325085581577154625[100] = 0; - out_3325085581577154625[101] = 0; - out_3325085581577154625[102] = 0; - out_3325085581577154625[103] = 0; - out_3325085581577154625[104] = dt; - out_3325085581577154625[105] = 0; - out_3325085581577154625[106] = 0; - out_3325085581577154625[107] = 0; - out_3325085581577154625[108] = 0; - out_3325085581577154625[109] = 0; - out_3325085581577154625[110] = 0; - out_3325085581577154625[111] = 0; - out_3325085581577154625[112] = 0; - out_3325085581577154625[113] = 0; - out_3325085581577154625[114] = 1; - out_3325085581577154625[115] = 0; - out_3325085581577154625[116] = 0; - out_3325085581577154625[117] = 0; - out_3325085581577154625[118] = 0; - out_3325085581577154625[119] = 0; - out_3325085581577154625[120] = 0; - out_3325085581577154625[121] = 0; - out_3325085581577154625[122] = 0; - out_3325085581577154625[123] = 0; - out_3325085581577154625[124] = 0; - out_3325085581577154625[125] = 0; - out_3325085581577154625[126] = 0; - out_3325085581577154625[127] = 0; - out_3325085581577154625[128] = 0; - out_3325085581577154625[129] = 0; - out_3325085581577154625[130] = 0; - out_3325085581577154625[131] = 0; - out_3325085581577154625[132] = 0; - out_3325085581577154625[133] = 1; - out_3325085581577154625[134] = 0; - out_3325085581577154625[135] = 0; - out_3325085581577154625[136] = 0; - out_3325085581577154625[137] = 0; - out_3325085581577154625[138] = 0; - out_3325085581577154625[139] = 0; - out_3325085581577154625[140] = 0; - out_3325085581577154625[141] = 0; - out_3325085581577154625[142] = 0; - out_3325085581577154625[143] = 0; - out_3325085581577154625[144] = 0; - out_3325085581577154625[145] = 0; - out_3325085581577154625[146] = 0; - out_3325085581577154625[147] = 0; - out_3325085581577154625[148] = 0; - out_3325085581577154625[149] = 0; - out_3325085581577154625[150] = 0; - out_3325085581577154625[151] = 0; - out_3325085581577154625[152] = 1; - out_3325085581577154625[153] = 0; - out_3325085581577154625[154] = 0; - out_3325085581577154625[155] = 0; - out_3325085581577154625[156] = 0; - out_3325085581577154625[157] = 0; - out_3325085581577154625[158] = 0; - out_3325085581577154625[159] = 0; - out_3325085581577154625[160] = 0; - out_3325085581577154625[161] = 0; - out_3325085581577154625[162] = 0; - out_3325085581577154625[163] = 0; - out_3325085581577154625[164] = 0; - out_3325085581577154625[165] = 0; - out_3325085581577154625[166] = 0; - out_3325085581577154625[167] = 0; - out_3325085581577154625[168] = 0; - out_3325085581577154625[169] = 0; - out_3325085581577154625[170] = 0; - out_3325085581577154625[171] = 1; - out_3325085581577154625[172] = 0; - out_3325085581577154625[173] = 0; - out_3325085581577154625[174] = 0; - out_3325085581577154625[175] = 0; - out_3325085581577154625[176] = 0; - out_3325085581577154625[177] = 0; - out_3325085581577154625[178] = 0; - out_3325085581577154625[179] = 0; - out_3325085581577154625[180] = 0; - out_3325085581577154625[181] = 0; - out_3325085581577154625[182] = 0; - out_3325085581577154625[183] = 0; - out_3325085581577154625[184] = 0; - out_3325085581577154625[185] = 0; - out_3325085581577154625[186] = 0; - out_3325085581577154625[187] = 0; - out_3325085581577154625[188] = 0; - out_3325085581577154625[189] = 0; - out_3325085581577154625[190] = 1; - out_3325085581577154625[191] = 0; - out_3325085581577154625[192] = 0; - out_3325085581577154625[193] = 0; - out_3325085581577154625[194] = 0; - out_3325085581577154625[195] = 0; - out_3325085581577154625[196] = 0; - out_3325085581577154625[197] = 0; - out_3325085581577154625[198] = 0; - out_3325085581577154625[199] = 0; - out_3325085581577154625[200] = 0; - out_3325085581577154625[201] = 0; - out_3325085581577154625[202] = 0; - out_3325085581577154625[203] = 0; - out_3325085581577154625[204] = 0; - out_3325085581577154625[205] = 0; - out_3325085581577154625[206] = 0; - out_3325085581577154625[207] = 0; - out_3325085581577154625[208] = 0; - out_3325085581577154625[209] = 1; - out_3325085581577154625[210] = 0; - out_3325085581577154625[211] = 0; - out_3325085581577154625[212] = 0; - out_3325085581577154625[213] = 0; - out_3325085581577154625[214] = 0; - out_3325085581577154625[215] = 0; - out_3325085581577154625[216] = 0; - out_3325085581577154625[217] = 0; - out_3325085581577154625[218] = 0; - out_3325085581577154625[219] = 0; - out_3325085581577154625[220] = 0; - out_3325085581577154625[221] = 0; - out_3325085581577154625[222] = 0; - out_3325085581577154625[223] = 0; - out_3325085581577154625[224] = 0; - out_3325085581577154625[225] = 0; - out_3325085581577154625[226] = 0; - out_3325085581577154625[227] = 0; - out_3325085581577154625[228] = 1; - out_3325085581577154625[229] = 0; - out_3325085581577154625[230] = 0; - out_3325085581577154625[231] = 0; - out_3325085581577154625[232] = 0; - out_3325085581577154625[233] = 0; - out_3325085581577154625[234] = 0; - out_3325085581577154625[235] = 0; - out_3325085581577154625[236] = 0; - out_3325085581577154625[237] = 0; - out_3325085581577154625[238] = 0; - out_3325085581577154625[239] = 0; - out_3325085581577154625[240] = 0; - out_3325085581577154625[241] = 0; - out_3325085581577154625[242] = 0; - out_3325085581577154625[243] = 0; - out_3325085581577154625[244] = 0; - out_3325085581577154625[245] = 0; - out_3325085581577154625[246] = 0; - out_3325085581577154625[247] = 1; - out_3325085581577154625[248] = 0; - out_3325085581577154625[249] = 0; - out_3325085581577154625[250] = 0; - out_3325085581577154625[251] = 0; - out_3325085581577154625[252] = 0; - out_3325085581577154625[253] = 0; - out_3325085581577154625[254] = 0; - out_3325085581577154625[255] = 0; - out_3325085581577154625[256] = 0; - out_3325085581577154625[257] = 0; - out_3325085581577154625[258] = 0; - out_3325085581577154625[259] = 0; - out_3325085581577154625[260] = 0; - out_3325085581577154625[261] = 0; - out_3325085581577154625[262] = 0; - out_3325085581577154625[263] = 0; - out_3325085581577154625[264] = 0; - out_3325085581577154625[265] = 0; - out_3325085581577154625[266] = 1; - out_3325085581577154625[267] = 0; - out_3325085581577154625[268] = 0; - out_3325085581577154625[269] = 0; - out_3325085581577154625[270] = 0; - out_3325085581577154625[271] = 0; - out_3325085581577154625[272] = 0; - out_3325085581577154625[273] = 0; - out_3325085581577154625[274] = 0; - out_3325085581577154625[275] = 0; - out_3325085581577154625[276] = 0; - out_3325085581577154625[277] = 0; - out_3325085581577154625[278] = 0; - out_3325085581577154625[279] = 0; - out_3325085581577154625[280] = 0; - out_3325085581577154625[281] = 0; - out_3325085581577154625[282] = 0; - out_3325085581577154625[283] = 0; - out_3325085581577154625[284] = 0; - out_3325085581577154625[285] = 1; - out_3325085581577154625[286] = 0; - out_3325085581577154625[287] = 0; - out_3325085581577154625[288] = 0; - out_3325085581577154625[289] = 0; - out_3325085581577154625[290] = 0; - out_3325085581577154625[291] = 0; - out_3325085581577154625[292] = 0; - out_3325085581577154625[293] = 0; - out_3325085581577154625[294] = 0; - out_3325085581577154625[295] = 0; - out_3325085581577154625[296] = 0; - out_3325085581577154625[297] = 0; - out_3325085581577154625[298] = 0; - out_3325085581577154625[299] = 0; - out_3325085581577154625[300] = 0; - out_3325085581577154625[301] = 0; - out_3325085581577154625[302] = 0; - out_3325085581577154625[303] = 0; - out_3325085581577154625[304] = 1; - out_3325085581577154625[305] = 0; - out_3325085581577154625[306] = 0; - out_3325085581577154625[307] = 0; - out_3325085581577154625[308] = 0; - out_3325085581577154625[309] = 0; - out_3325085581577154625[310] = 0; - out_3325085581577154625[311] = 0; - out_3325085581577154625[312] = 0; - out_3325085581577154625[313] = 0; - out_3325085581577154625[314] = 0; - out_3325085581577154625[315] = 0; - out_3325085581577154625[316] = 0; - out_3325085581577154625[317] = 0; - out_3325085581577154625[318] = 0; - out_3325085581577154625[319] = 0; - out_3325085581577154625[320] = 0; - out_3325085581577154625[321] = 0; - out_3325085581577154625[322] = 0; - out_3325085581577154625[323] = 1; +void F_fun(double *state, double dt, double *out_3263235949829739422) { + out_3263235949829739422[0] = ((-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*cos(state[0])*cos(state[1]) - sin(state[0])*cos(dt*state[6])*cos(dt*state[7])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + ((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*cos(state[0])*cos(state[1]) - sin(dt*state[6])*sin(state[0])*cos(dt*state[7])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); + out_3263235949829739422[1] = ((-sin(dt*state[6])*sin(dt*state[8]) - sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*cos(state[1]) - (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*sin(state[1]) - sin(state[1])*cos(dt*state[6])*cos(dt*state[7])*cos(state[0]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*sin(state[1]) + (-sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) + sin(dt*state[8])*cos(dt*state[6]))*cos(state[1]) - sin(dt*state[6])*sin(state[1])*cos(dt*state[7])*cos(state[0]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); + out_3263235949829739422[2] = 0; + out_3263235949829739422[3] = 0; + out_3263235949829739422[4] = 0; + out_3263235949829739422[5] = 0; + out_3263235949829739422[6] = (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(dt*cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*sin(dt*state[8]) - dt*sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-dt*sin(dt*state[6])*cos(dt*state[8]) + dt*sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) - dt*cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (dt*sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); + out_3263235949829739422[7] = (-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[6])*sin(dt*state[7])*cos(state[0])*cos(state[1]) + dt*sin(dt*state[6])*sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) - dt*sin(dt*state[6])*sin(state[1])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + (-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))*(-dt*sin(dt*state[7])*cos(dt*state[6])*cos(state[0])*cos(state[1]) + dt*sin(dt*state[8])*sin(state[0])*cos(dt*state[6])*cos(dt*state[7])*cos(state[1]) - dt*sin(state[1])*cos(dt*state[6])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); + out_3263235949829739422[8] = ((dt*sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + dt*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (dt*sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]))*(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)) + ((dt*sin(dt*state[6])*sin(dt*state[8]) + dt*sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (-dt*sin(dt*state[6])*cos(dt*state[8]) + dt*sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]))*(-(sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) + (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) - sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/(pow(-(sin(dt*state[6])*sin(dt*state[8]) + sin(dt*state[7])*cos(dt*state[6])*cos(dt*state[8]))*sin(state[1]) + (-sin(dt*state[6])*cos(dt*state[8]) + sin(dt*state[7])*sin(dt*state[8])*cos(dt*state[6]))*sin(state[0])*cos(state[1]) + cos(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2) + pow((sin(dt*state[6])*sin(dt*state[7])*sin(dt*state[8]) + cos(dt*state[6])*cos(dt*state[8]))*sin(state[0])*cos(state[1]) - (sin(dt*state[6])*sin(dt*state[7])*cos(dt*state[8]) - sin(dt*state[8])*cos(dt*state[6]))*sin(state[1]) + sin(dt*state[6])*cos(dt*state[7])*cos(state[0])*cos(state[1]), 2)); + out_3263235949829739422[9] = 0; + out_3263235949829739422[10] = 0; + out_3263235949829739422[11] = 0; + out_3263235949829739422[12] = 0; + out_3263235949829739422[13] = 0; + out_3263235949829739422[14] = 0; + out_3263235949829739422[15] = 0; + out_3263235949829739422[16] = 0; + out_3263235949829739422[17] = 0; + out_3263235949829739422[18] = (-sin(dt*state[7])*sin(state[0])*cos(state[1]) - sin(dt*state[8])*cos(dt*state[7])*cos(state[0])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); + out_3263235949829739422[19] = (-sin(dt*state[7])*sin(state[1])*cos(state[0]) + sin(dt*state[8])*sin(state[0])*sin(state[1])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); + out_3263235949829739422[20] = 0; + out_3263235949829739422[21] = 0; + out_3263235949829739422[22] = 0; + out_3263235949829739422[23] = 0; + out_3263235949829739422[24] = 0; + out_3263235949829739422[25] = (dt*sin(dt*state[7])*sin(dt*state[8])*sin(state[0])*cos(state[1]) - dt*sin(dt*state[7])*sin(state[1])*cos(dt*state[8]) + dt*cos(dt*state[7])*cos(state[0])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); + out_3263235949829739422[26] = (-dt*sin(dt*state[8])*sin(state[1])*cos(dt*state[7]) - dt*sin(state[0])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/sqrt(1 - pow(sin(dt*state[7])*cos(state[0])*cos(state[1]) - sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1]) + sin(state[1])*cos(dt*state[7])*cos(dt*state[8]), 2)); + out_3263235949829739422[27] = 0; + out_3263235949829739422[28] = 0; + out_3263235949829739422[29] = 0; + out_3263235949829739422[30] = 0; + out_3263235949829739422[31] = 0; + out_3263235949829739422[32] = 0; + out_3263235949829739422[33] = 0; + out_3263235949829739422[34] = 0; + out_3263235949829739422[35] = 0; + out_3263235949829739422[36] = ((sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[7]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[7]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); + out_3263235949829739422[37] = (-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(-sin(dt*state[7])*sin(state[2])*cos(state[0])*cos(state[1]) + sin(dt*state[8])*sin(state[0])*sin(state[2])*cos(dt*state[7])*cos(state[1]) - sin(state[1])*sin(state[2])*cos(dt*state[7])*cos(dt*state[8]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*(-sin(dt*state[7])*cos(state[0])*cos(state[1])*cos(state[2]) + sin(dt*state[8])*sin(state[0])*cos(dt*state[7])*cos(state[1])*cos(state[2]) - sin(state[1])*cos(dt*state[7])*cos(dt*state[8])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); + out_3263235949829739422[38] = ((-sin(state[0])*sin(state[2]) - sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (-sin(state[0])*sin(state[1])*sin(state[2]) - cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); + out_3263235949829739422[39] = 0; + out_3263235949829739422[40] = 0; + out_3263235949829739422[41] = 0; + out_3263235949829739422[42] = 0; + out_3263235949829739422[43] = (-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))*(dt*(sin(state[0])*cos(state[2]) - sin(state[1])*sin(state[2])*cos(state[0]))*cos(dt*state[7]) - dt*(sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[7])*sin(dt*state[8]) - dt*sin(dt*state[7])*sin(state[2])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + ((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))*(dt*(-sin(state[0])*sin(state[2]) - sin(state[1])*cos(state[0])*cos(state[2]))*cos(dt*state[7]) - dt*(sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[7])*sin(dt*state[8]) - dt*sin(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); + out_3263235949829739422[44] = (dt*(sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*cos(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*sin(state[2])*cos(dt*state[7])*cos(state[1]))*(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)) + (dt*(sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*cos(dt*state[7])*cos(dt*state[8]) - dt*sin(dt*state[8])*cos(dt*state[7])*cos(state[1])*cos(state[2]))*((-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) - (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) - sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]))/(pow(-(sin(state[0])*sin(state[2]) + sin(state[1])*cos(state[0])*cos(state[2]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*cos(state[2]) - sin(state[2])*cos(state[0]))*sin(dt*state[8])*cos(dt*state[7]) + cos(dt*state[7])*cos(dt*state[8])*cos(state[1])*cos(state[2]), 2) + pow(-(-sin(state[0])*cos(state[2]) + sin(state[1])*sin(state[2])*cos(state[0]))*sin(dt*state[7]) + (sin(state[0])*sin(state[1])*sin(state[2]) + cos(state[0])*cos(state[2]))*sin(dt*state[8])*cos(dt*state[7]) + sin(state[2])*cos(dt*state[7])*cos(dt*state[8])*cos(state[1]), 2)); + out_3263235949829739422[45] = 0; + out_3263235949829739422[46] = 0; + out_3263235949829739422[47] = 0; + out_3263235949829739422[48] = 0; + out_3263235949829739422[49] = 0; + out_3263235949829739422[50] = 0; + out_3263235949829739422[51] = 0; + out_3263235949829739422[52] = 0; + out_3263235949829739422[53] = 0; + out_3263235949829739422[54] = 0; + out_3263235949829739422[55] = 0; + out_3263235949829739422[56] = 0; + out_3263235949829739422[57] = 1; + out_3263235949829739422[58] = 0; + out_3263235949829739422[59] = 0; + out_3263235949829739422[60] = 0; + out_3263235949829739422[61] = 0; + out_3263235949829739422[62] = 0; + out_3263235949829739422[63] = 0; + out_3263235949829739422[64] = 0; + out_3263235949829739422[65] = 0; + out_3263235949829739422[66] = dt; + out_3263235949829739422[67] = 0; + out_3263235949829739422[68] = 0; + out_3263235949829739422[69] = 0; + out_3263235949829739422[70] = 0; + out_3263235949829739422[71] = 0; + out_3263235949829739422[72] = 0; + out_3263235949829739422[73] = 0; + out_3263235949829739422[74] = 0; + out_3263235949829739422[75] = 0; + out_3263235949829739422[76] = 1; + out_3263235949829739422[77] = 0; + out_3263235949829739422[78] = 0; + out_3263235949829739422[79] = 0; + out_3263235949829739422[80] = 0; + out_3263235949829739422[81] = 0; + out_3263235949829739422[82] = 0; + out_3263235949829739422[83] = 0; + out_3263235949829739422[84] = 0; + out_3263235949829739422[85] = dt; + out_3263235949829739422[86] = 0; + out_3263235949829739422[87] = 0; + out_3263235949829739422[88] = 0; + out_3263235949829739422[89] = 0; + out_3263235949829739422[90] = 0; + out_3263235949829739422[91] = 0; + out_3263235949829739422[92] = 0; + out_3263235949829739422[93] = 0; + out_3263235949829739422[94] = 0; + out_3263235949829739422[95] = 1; + out_3263235949829739422[96] = 0; + out_3263235949829739422[97] = 0; + out_3263235949829739422[98] = 0; + out_3263235949829739422[99] = 0; + out_3263235949829739422[100] = 0; + out_3263235949829739422[101] = 0; + out_3263235949829739422[102] = 0; + out_3263235949829739422[103] = 0; + out_3263235949829739422[104] = dt; + out_3263235949829739422[105] = 0; + out_3263235949829739422[106] = 0; + out_3263235949829739422[107] = 0; + out_3263235949829739422[108] = 0; + out_3263235949829739422[109] = 0; + out_3263235949829739422[110] = 0; + out_3263235949829739422[111] = 0; + out_3263235949829739422[112] = 0; + out_3263235949829739422[113] = 0; + out_3263235949829739422[114] = 1; + out_3263235949829739422[115] = 0; + out_3263235949829739422[116] = 0; + out_3263235949829739422[117] = 0; + out_3263235949829739422[118] = 0; + out_3263235949829739422[119] = 0; + out_3263235949829739422[120] = 0; + out_3263235949829739422[121] = 0; + out_3263235949829739422[122] = 0; + out_3263235949829739422[123] = 0; + out_3263235949829739422[124] = 0; + out_3263235949829739422[125] = 0; + out_3263235949829739422[126] = 0; + out_3263235949829739422[127] = 0; + out_3263235949829739422[128] = 0; + out_3263235949829739422[129] = 0; + out_3263235949829739422[130] = 0; + out_3263235949829739422[131] = 0; + out_3263235949829739422[132] = 0; + out_3263235949829739422[133] = 1; + out_3263235949829739422[134] = 0; + out_3263235949829739422[135] = 0; + out_3263235949829739422[136] = 0; + out_3263235949829739422[137] = 0; + out_3263235949829739422[138] = 0; + out_3263235949829739422[139] = 0; + out_3263235949829739422[140] = 0; + out_3263235949829739422[141] = 0; + out_3263235949829739422[142] = 0; + out_3263235949829739422[143] = 0; + out_3263235949829739422[144] = 0; + out_3263235949829739422[145] = 0; + out_3263235949829739422[146] = 0; + out_3263235949829739422[147] = 0; + out_3263235949829739422[148] = 0; + out_3263235949829739422[149] = 0; + out_3263235949829739422[150] = 0; + out_3263235949829739422[151] = 0; + out_3263235949829739422[152] = 1; + out_3263235949829739422[153] = 0; + out_3263235949829739422[154] = 0; + out_3263235949829739422[155] = 0; + out_3263235949829739422[156] = 0; + out_3263235949829739422[157] = 0; + out_3263235949829739422[158] = 0; + out_3263235949829739422[159] = 0; + out_3263235949829739422[160] = 0; + out_3263235949829739422[161] = 0; + out_3263235949829739422[162] = 0; + out_3263235949829739422[163] = 0; + out_3263235949829739422[164] = 0; + out_3263235949829739422[165] = 0; + out_3263235949829739422[166] = 0; + out_3263235949829739422[167] = 0; + out_3263235949829739422[168] = 0; + out_3263235949829739422[169] = 0; + out_3263235949829739422[170] = 0; + out_3263235949829739422[171] = 1; + out_3263235949829739422[172] = 0; + out_3263235949829739422[173] = 0; + out_3263235949829739422[174] = 0; + out_3263235949829739422[175] = 0; + out_3263235949829739422[176] = 0; + out_3263235949829739422[177] = 0; + out_3263235949829739422[178] = 0; + out_3263235949829739422[179] = 0; + out_3263235949829739422[180] = 0; + out_3263235949829739422[181] = 0; + out_3263235949829739422[182] = 0; + out_3263235949829739422[183] = 0; + out_3263235949829739422[184] = 0; + out_3263235949829739422[185] = 0; + out_3263235949829739422[186] = 0; + out_3263235949829739422[187] = 0; + out_3263235949829739422[188] = 0; + out_3263235949829739422[189] = 0; + out_3263235949829739422[190] = 1; + out_3263235949829739422[191] = 0; + out_3263235949829739422[192] = 0; + out_3263235949829739422[193] = 0; + out_3263235949829739422[194] = 0; + out_3263235949829739422[195] = 0; + out_3263235949829739422[196] = 0; + out_3263235949829739422[197] = 0; + out_3263235949829739422[198] = 0; + out_3263235949829739422[199] = 0; + out_3263235949829739422[200] = 0; + out_3263235949829739422[201] = 0; + out_3263235949829739422[202] = 0; + out_3263235949829739422[203] = 0; + out_3263235949829739422[204] = 0; + out_3263235949829739422[205] = 0; + out_3263235949829739422[206] = 0; + out_3263235949829739422[207] = 0; + out_3263235949829739422[208] = 0; + out_3263235949829739422[209] = 1; + out_3263235949829739422[210] = 0; + out_3263235949829739422[211] = 0; + out_3263235949829739422[212] = 0; + out_3263235949829739422[213] = 0; + out_3263235949829739422[214] = 0; + out_3263235949829739422[215] = 0; + out_3263235949829739422[216] = 0; + out_3263235949829739422[217] = 0; + out_3263235949829739422[218] = 0; + out_3263235949829739422[219] = 0; + out_3263235949829739422[220] = 0; + out_3263235949829739422[221] = 0; + out_3263235949829739422[222] = 0; + out_3263235949829739422[223] = 0; + out_3263235949829739422[224] = 0; + out_3263235949829739422[225] = 0; + out_3263235949829739422[226] = 0; + out_3263235949829739422[227] = 0; + out_3263235949829739422[228] = 1; + out_3263235949829739422[229] = 0; + out_3263235949829739422[230] = 0; + out_3263235949829739422[231] = 0; + out_3263235949829739422[232] = 0; + out_3263235949829739422[233] = 0; + out_3263235949829739422[234] = 0; + out_3263235949829739422[235] = 0; + out_3263235949829739422[236] = 0; + out_3263235949829739422[237] = 0; + out_3263235949829739422[238] = 0; + out_3263235949829739422[239] = 0; + out_3263235949829739422[240] = 0; + out_3263235949829739422[241] = 0; + out_3263235949829739422[242] = 0; + out_3263235949829739422[243] = 0; + out_3263235949829739422[244] = 0; + out_3263235949829739422[245] = 0; + out_3263235949829739422[246] = 0; + out_3263235949829739422[247] = 1; + out_3263235949829739422[248] = 0; + out_3263235949829739422[249] = 0; + out_3263235949829739422[250] = 0; + out_3263235949829739422[251] = 0; + out_3263235949829739422[252] = 0; + out_3263235949829739422[253] = 0; + out_3263235949829739422[254] = 0; + out_3263235949829739422[255] = 0; + out_3263235949829739422[256] = 0; + out_3263235949829739422[257] = 0; + out_3263235949829739422[258] = 0; + out_3263235949829739422[259] = 0; + out_3263235949829739422[260] = 0; + out_3263235949829739422[261] = 0; + out_3263235949829739422[262] = 0; + out_3263235949829739422[263] = 0; + out_3263235949829739422[264] = 0; + out_3263235949829739422[265] = 0; + out_3263235949829739422[266] = 1; + out_3263235949829739422[267] = 0; + out_3263235949829739422[268] = 0; + out_3263235949829739422[269] = 0; + out_3263235949829739422[270] = 0; + out_3263235949829739422[271] = 0; + out_3263235949829739422[272] = 0; + out_3263235949829739422[273] = 0; + out_3263235949829739422[274] = 0; + out_3263235949829739422[275] = 0; + out_3263235949829739422[276] = 0; + out_3263235949829739422[277] = 0; + out_3263235949829739422[278] = 0; + out_3263235949829739422[279] = 0; + out_3263235949829739422[280] = 0; + out_3263235949829739422[281] = 0; + out_3263235949829739422[282] = 0; + out_3263235949829739422[283] = 0; + out_3263235949829739422[284] = 0; + out_3263235949829739422[285] = 1; + out_3263235949829739422[286] = 0; + out_3263235949829739422[287] = 0; + out_3263235949829739422[288] = 0; + out_3263235949829739422[289] = 0; + out_3263235949829739422[290] = 0; + out_3263235949829739422[291] = 0; + out_3263235949829739422[292] = 0; + out_3263235949829739422[293] = 0; + out_3263235949829739422[294] = 0; + out_3263235949829739422[295] = 0; + out_3263235949829739422[296] = 0; + out_3263235949829739422[297] = 0; + out_3263235949829739422[298] = 0; + out_3263235949829739422[299] = 0; + out_3263235949829739422[300] = 0; + out_3263235949829739422[301] = 0; + out_3263235949829739422[302] = 0; + out_3263235949829739422[303] = 0; + out_3263235949829739422[304] = 1; + out_3263235949829739422[305] = 0; + out_3263235949829739422[306] = 0; + out_3263235949829739422[307] = 0; + out_3263235949829739422[308] = 0; + out_3263235949829739422[309] = 0; + out_3263235949829739422[310] = 0; + out_3263235949829739422[311] = 0; + out_3263235949829739422[312] = 0; + out_3263235949829739422[313] = 0; + out_3263235949829739422[314] = 0; + out_3263235949829739422[315] = 0; + out_3263235949829739422[316] = 0; + out_3263235949829739422[317] = 0; + out_3263235949829739422[318] = 0; + out_3263235949829739422[319] = 0; + out_3263235949829739422[320] = 0; + out_3263235949829739422[321] = 0; + out_3263235949829739422[322] = 0; + out_3263235949829739422[323] = 1; } -void h_4(double *state, double *unused, double *out_1824245269356126422) { - out_1824245269356126422[0] = state[6] + state[9]; - out_1824245269356126422[1] = state[7] + state[10]; - out_1824245269356126422[2] = state[8] + state[11]; +void h_4(double *state, double *unused, double *out_6871661940623032085) { + out_6871661940623032085[0] = state[6] + state[9]; + out_6871661940623032085[1] = state[7] + state[10]; + out_6871661940623032085[2] = state[8] + state[11]; } -void H_4(double *state, double *unused, double *out_908091458257428799) { - out_908091458257428799[0] = 0; - out_908091458257428799[1] = 0; - out_908091458257428799[2] = 0; - out_908091458257428799[3] = 0; - out_908091458257428799[4] = 0; - out_908091458257428799[5] = 0; - out_908091458257428799[6] = 1; - out_908091458257428799[7] = 0; - out_908091458257428799[8] = 0; - out_908091458257428799[9] = 1; - out_908091458257428799[10] = 0; - out_908091458257428799[11] = 0; - out_908091458257428799[12] = 0; - out_908091458257428799[13] = 0; - out_908091458257428799[14] = 0; - out_908091458257428799[15] = 0; - out_908091458257428799[16] = 0; - out_908091458257428799[17] = 0; - out_908091458257428799[18] = 0; - out_908091458257428799[19] = 0; - out_908091458257428799[20] = 0; - out_908091458257428799[21] = 0; - out_908091458257428799[22] = 0; - out_908091458257428799[23] = 0; - out_908091458257428799[24] = 0; - out_908091458257428799[25] = 1; - out_908091458257428799[26] = 0; - out_908091458257428799[27] = 0; - out_908091458257428799[28] = 1; - out_908091458257428799[29] = 0; - out_908091458257428799[30] = 0; - out_908091458257428799[31] = 0; - out_908091458257428799[32] = 0; - out_908091458257428799[33] = 0; - out_908091458257428799[34] = 0; - out_908091458257428799[35] = 0; - out_908091458257428799[36] = 0; - out_908091458257428799[37] = 0; - out_908091458257428799[38] = 0; - out_908091458257428799[39] = 0; - out_908091458257428799[40] = 0; - out_908091458257428799[41] = 0; - out_908091458257428799[42] = 0; - out_908091458257428799[43] = 0; - out_908091458257428799[44] = 1; - out_908091458257428799[45] = 0; - out_908091458257428799[46] = 0; - out_908091458257428799[47] = 1; - out_908091458257428799[48] = 0; - out_908091458257428799[49] = 0; - out_908091458257428799[50] = 0; - out_908091458257428799[51] = 0; - out_908091458257428799[52] = 0; - out_908091458257428799[53] = 0; +void H_4(double *state, double *unused, double *out_5061597179712991997) { + out_5061597179712991997[0] = 0; + out_5061597179712991997[1] = 0; + out_5061597179712991997[2] = 0; + out_5061597179712991997[3] = 0; + out_5061597179712991997[4] = 0; + out_5061597179712991997[5] = 0; + out_5061597179712991997[6] = 1; + out_5061597179712991997[7] = 0; + out_5061597179712991997[8] = 0; + out_5061597179712991997[9] = 1; + out_5061597179712991997[10] = 0; + out_5061597179712991997[11] = 0; + out_5061597179712991997[12] = 0; + out_5061597179712991997[13] = 0; + out_5061597179712991997[14] = 0; + out_5061597179712991997[15] = 0; + out_5061597179712991997[16] = 0; + out_5061597179712991997[17] = 0; + out_5061597179712991997[18] = 0; + out_5061597179712991997[19] = 0; + out_5061597179712991997[20] = 0; + out_5061597179712991997[21] = 0; + out_5061597179712991997[22] = 0; + out_5061597179712991997[23] = 0; + out_5061597179712991997[24] = 0; + out_5061597179712991997[25] = 1; + out_5061597179712991997[26] = 0; + out_5061597179712991997[27] = 0; + out_5061597179712991997[28] = 1; + out_5061597179712991997[29] = 0; + out_5061597179712991997[30] = 0; + out_5061597179712991997[31] = 0; + out_5061597179712991997[32] = 0; + out_5061597179712991997[33] = 0; + out_5061597179712991997[34] = 0; + out_5061597179712991997[35] = 0; + out_5061597179712991997[36] = 0; + out_5061597179712991997[37] = 0; + out_5061597179712991997[38] = 0; + out_5061597179712991997[39] = 0; + out_5061597179712991997[40] = 0; + out_5061597179712991997[41] = 0; + out_5061597179712991997[42] = 0; + out_5061597179712991997[43] = 0; + out_5061597179712991997[44] = 1; + out_5061597179712991997[45] = 0; + out_5061597179712991997[46] = 0; + out_5061597179712991997[47] = 1; + out_5061597179712991997[48] = 0; + out_5061597179712991997[49] = 0; + out_5061597179712991997[50] = 0; + out_5061597179712991997[51] = 0; + out_5061597179712991997[52] = 0; + out_5061597179712991997[53] = 0; } -void h_10(double *state, double *unused, double *out_5992945801677381357) { - out_5992945801677381357[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; - out_5992945801677381357[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; - out_5992945801677381357[2] = -9.8100000000000005*cos(state[0])*cos(state[1]) - state[3]*state[7] + state[4]*state[6] + state[14] + state[17]; +void h_10(double *state, double *unused, double *out_974669649895256171) { + out_974669649895256171[0] = 9.8100000000000005*sin(state[1]) - state[4]*state[8] + state[5]*state[7] + state[12] + state[15]; + out_974669649895256171[1] = -9.8100000000000005*sin(state[0])*cos(state[1]) + state[3]*state[8] - state[5]*state[6] + state[13] + state[16]; + out_974669649895256171[2] = -9.8100000000000005*cos(state[0])*cos(state[1]) - state[3]*state[7] + state[4]*state[6] + state[14] + state[17]; } -void H_10(double *state, double *unused, double *out_1551783764236526040) { - out_1551783764236526040[0] = 0; - out_1551783764236526040[1] = 9.8100000000000005*cos(state[1]); - out_1551783764236526040[2] = 0; - out_1551783764236526040[3] = 0; - out_1551783764236526040[4] = -state[8]; - out_1551783764236526040[5] = state[7]; - out_1551783764236526040[6] = 0; - out_1551783764236526040[7] = state[5]; - out_1551783764236526040[8] = -state[4]; - out_1551783764236526040[9] = 0; - out_1551783764236526040[10] = 0; - out_1551783764236526040[11] = 0; - out_1551783764236526040[12] = 1; - out_1551783764236526040[13] = 0; - out_1551783764236526040[14] = 0; - out_1551783764236526040[15] = 1; - out_1551783764236526040[16] = 0; - out_1551783764236526040[17] = 0; - out_1551783764236526040[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); - out_1551783764236526040[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); - out_1551783764236526040[20] = 0; - out_1551783764236526040[21] = state[8]; - out_1551783764236526040[22] = 0; - out_1551783764236526040[23] = -state[6]; - out_1551783764236526040[24] = -state[5]; - out_1551783764236526040[25] = 0; - out_1551783764236526040[26] = state[3]; - out_1551783764236526040[27] = 0; - out_1551783764236526040[28] = 0; - out_1551783764236526040[29] = 0; - out_1551783764236526040[30] = 0; - out_1551783764236526040[31] = 1; - out_1551783764236526040[32] = 0; - out_1551783764236526040[33] = 0; - out_1551783764236526040[34] = 1; - out_1551783764236526040[35] = 0; - out_1551783764236526040[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); - out_1551783764236526040[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); - out_1551783764236526040[38] = 0; - out_1551783764236526040[39] = -state[7]; - out_1551783764236526040[40] = state[6]; - out_1551783764236526040[41] = 0; - out_1551783764236526040[42] = state[4]; - out_1551783764236526040[43] = -state[3]; - out_1551783764236526040[44] = 0; - out_1551783764236526040[45] = 0; - out_1551783764236526040[46] = 0; - out_1551783764236526040[47] = 0; - out_1551783764236526040[48] = 0; - out_1551783764236526040[49] = 0; - out_1551783764236526040[50] = 1; - out_1551783764236526040[51] = 0; - out_1551783764236526040[52] = 0; - out_1551783764236526040[53] = 1; +void H_10(double *state, double *unused, double *out_3862977066975629010) { + out_3862977066975629010[0] = 0; + out_3862977066975629010[1] = 9.8100000000000005*cos(state[1]); + out_3862977066975629010[2] = 0; + out_3862977066975629010[3] = 0; + out_3862977066975629010[4] = -state[8]; + out_3862977066975629010[5] = state[7]; + out_3862977066975629010[6] = 0; + out_3862977066975629010[7] = state[5]; + out_3862977066975629010[8] = -state[4]; + out_3862977066975629010[9] = 0; + out_3862977066975629010[10] = 0; + out_3862977066975629010[11] = 0; + out_3862977066975629010[12] = 1; + out_3862977066975629010[13] = 0; + out_3862977066975629010[14] = 0; + out_3862977066975629010[15] = 1; + out_3862977066975629010[16] = 0; + out_3862977066975629010[17] = 0; + out_3862977066975629010[18] = -9.8100000000000005*cos(state[0])*cos(state[1]); + out_3862977066975629010[19] = 9.8100000000000005*sin(state[0])*sin(state[1]); + out_3862977066975629010[20] = 0; + out_3862977066975629010[21] = state[8]; + out_3862977066975629010[22] = 0; + out_3862977066975629010[23] = -state[6]; + out_3862977066975629010[24] = -state[5]; + out_3862977066975629010[25] = 0; + out_3862977066975629010[26] = state[3]; + out_3862977066975629010[27] = 0; + out_3862977066975629010[28] = 0; + out_3862977066975629010[29] = 0; + out_3862977066975629010[30] = 0; + out_3862977066975629010[31] = 1; + out_3862977066975629010[32] = 0; + out_3862977066975629010[33] = 0; + out_3862977066975629010[34] = 1; + out_3862977066975629010[35] = 0; + out_3862977066975629010[36] = 9.8100000000000005*sin(state[0])*cos(state[1]); + out_3862977066975629010[37] = 9.8100000000000005*sin(state[1])*cos(state[0]); + out_3862977066975629010[38] = 0; + out_3862977066975629010[39] = -state[7]; + out_3862977066975629010[40] = state[6]; + out_3862977066975629010[41] = 0; + out_3862977066975629010[42] = state[4]; + out_3862977066975629010[43] = -state[3]; + out_3862977066975629010[44] = 0; + out_3862977066975629010[45] = 0; + out_3862977066975629010[46] = 0; + out_3862977066975629010[47] = 0; + out_3862977066975629010[48] = 0; + out_3862977066975629010[49] = 0; + out_3862977066975629010[50] = 1; + out_3862977066975629010[51] = 0; + out_3862977066975629010[52] = 0; + out_3862977066975629010[53] = 1; } -void h_13(double *state, double *unused, double *out_4650572533653388734) { - out_4650572533653388734[0] = state[3]; - out_4650572533653388734[1] = state[4]; - out_4650572533653388734[2] = state[5]; +void h_13(double *state, double *unused, double *out_2719357598944466940) { + out_2719357598944466940[0] = state[3]; + out_2719357598944466940[1] = state[4]; + out_2719357598944466940[2] = state[5]; } -void H_13(double *state, double *unused, double *out_1472693377939272903) { - out_1472693377939272903[0] = 0; - out_1472693377939272903[1] = 0; - out_1472693377939272903[2] = 0; - out_1472693377939272903[3] = 1; - out_1472693377939272903[4] = 0; - out_1472693377939272903[5] = 0; - out_1472693377939272903[6] = 0; - out_1472693377939272903[7] = 0; - out_1472693377939272903[8] = 0; - out_1472693377939272903[9] = 0; - out_1472693377939272903[10] = 0; - out_1472693377939272903[11] = 0; - out_1472693377939272903[12] = 0; - out_1472693377939272903[13] = 0; - out_1472693377939272903[14] = 0; - out_1472693377939272903[15] = 0; - out_1472693377939272903[16] = 0; - out_1472693377939272903[17] = 0; - out_1472693377939272903[18] = 0; - out_1472693377939272903[19] = 0; - out_1472693377939272903[20] = 0; - out_1472693377939272903[21] = 0; - out_1472693377939272903[22] = 1; - out_1472693377939272903[23] = 0; - out_1472693377939272903[24] = 0; - out_1472693377939272903[25] = 0; - out_1472693377939272903[26] = 0; - out_1472693377939272903[27] = 0; - out_1472693377939272903[28] = 0; - out_1472693377939272903[29] = 0; - out_1472693377939272903[30] = 0; - out_1472693377939272903[31] = 0; - out_1472693377939272903[32] = 0; - out_1472693377939272903[33] = 0; - out_1472693377939272903[34] = 0; - out_1472693377939272903[35] = 0; - out_1472693377939272903[36] = 0; - out_1472693377939272903[37] = 0; - out_1472693377939272903[38] = 0; - out_1472693377939272903[39] = 0; - out_1472693377939272903[40] = 0; - out_1472693377939272903[41] = 1; - out_1472693377939272903[42] = 0; - out_1472693377939272903[43] = 0; - out_1472693377939272903[44] = 0; - out_1472693377939272903[45] = 0; - out_1472693377939272903[46] = 0; - out_1472693377939272903[47] = 0; - out_1472693377939272903[48] = 0; - out_1472693377939272903[49] = 0; - out_1472693377939272903[50] = 0; - out_1472693377939272903[51] = 0; - out_1472693377939272903[52] = 0; - out_1472693377939272903[53] = 0; +void H_13(double *state, double *unused, double *out_8273871005045324798) { + out_8273871005045324798[0] = 0; + out_8273871005045324798[1] = 0; + out_8273871005045324798[2] = 0; + out_8273871005045324798[3] = 1; + out_8273871005045324798[4] = 0; + out_8273871005045324798[5] = 0; + out_8273871005045324798[6] = 0; + out_8273871005045324798[7] = 0; + out_8273871005045324798[8] = 0; + out_8273871005045324798[9] = 0; + out_8273871005045324798[10] = 0; + out_8273871005045324798[11] = 0; + out_8273871005045324798[12] = 0; + out_8273871005045324798[13] = 0; + out_8273871005045324798[14] = 0; + out_8273871005045324798[15] = 0; + out_8273871005045324798[16] = 0; + out_8273871005045324798[17] = 0; + out_8273871005045324798[18] = 0; + out_8273871005045324798[19] = 0; + out_8273871005045324798[20] = 0; + out_8273871005045324798[21] = 0; + out_8273871005045324798[22] = 1; + out_8273871005045324798[23] = 0; + out_8273871005045324798[24] = 0; + out_8273871005045324798[25] = 0; + out_8273871005045324798[26] = 0; + out_8273871005045324798[27] = 0; + out_8273871005045324798[28] = 0; + out_8273871005045324798[29] = 0; + out_8273871005045324798[30] = 0; + out_8273871005045324798[31] = 0; + out_8273871005045324798[32] = 0; + out_8273871005045324798[33] = 0; + out_8273871005045324798[34] = 0; + out_8273871005045324798[35] = 0; + out_8273871005045324798[36] = 0; + out_8273871005045324798[37] = 0; + out_8273871005045324798[38] = 0; + out_8273871005045324798[39] = 0; + out_8273871005045324798[40] = 0; + out_8273871005045324798[41] = 1; + out_8273871005045324798[42] = 0; + out_8273871005045324798[43] = 0; + out_8273871005045324798[44] = 0; + out_8273871005045324798[45] = 0; + out_8273871005045324798[46] = 0; + out_8273871005045324798[47] = 0; + out_8273871005045324798[48] = 0; + out_8273871005045324798[49] = 0; + out_8273871005045324798[50] = 0; + out_8273871005045324798[51] = 0; + out_8273871005045324798[52] = 0; + out_8273871005045324798[53] = 0; } -void h_14(double *state, double *unused, double *out_9091688560146400700) { - out_9091688560146400700[0] = state[6]; - out_9091688560146400700[1] = state[7]; - out_9091688560146400700[2] = state[8]; +void h_14(double *state, double *unused, double *out_5270922229605318979) { + out_5270922229605318979[0] = state[6]; + out_5270922229605318979[1] = state[7]; + out_5270922229605318979[2] = state[8]; } -void H_14(double *state, double *unused, double *out_2174696974037943497) { - out_2174696974037943497[0] = 0; - out_2174696974037943497[1] = 0; - out_2174696974037943497[2] = 0; - out_2174696974037943497[3] = 0; - out_2174696974037943497[4] = 0; - out_2174696974037943497[5] = 0; - out_2174696974037943497[6] = 1; - out_2174696974037943497[7] = 0; - out_2174696974037943497[8] = 0; - out_2174696974037943497[9] = 0; - out_2174696974037943497[10] = 0; - out_2174696974037943497[11] = 0; - out_2174696974037943497[12] = 0; - out_2174696974037943497[13] = 0; - out_2174696974037943497[14] = 0; - out_2174696974037943497[15] = 0; - out_2174696974037943497[16] = 0; - out_2174696974037943497[17] = 0; - out_2174696974037943497[18] = 0; - out_2174696974037943497[19] = 0; - out_2174696974037943497[20] = 0; - out_2174696974037943497[21] = 0; - out_2174696974037943497[22] = 0; - out_2174696974037943497[23] = 0; - out_2174696974037943497[24] = 0; - out_2174696974037943497[25] = 1; - out_2174696974037943497[26] = 0; - out_2174696974037943497[27] = 0; - out_2174696974037943497[28] = 0; - out_2174696974037943497[29] = 0; - out_2174696974037943497[30] = 0; - out_2174696974037943497[31] = 0; - out_2174696974037943497[32] = 0; - out_2174696974037943497[33] = 0; - out_2174696974037943497[34] = 0; - out_2174696974037943497[35] = 0; - out_2174696974037943497[36] = 0; - out_2174696974037943497[37] = 0; - out_2174696974037943497[38] = 0; - out_2174696974037943497[39] = 0; - out_2174696974037943497[40] = 0; - out_2174696974037943497[41] = 0; - out_2174696974037943497[42] = 0; - out_2174696974037943497[43] = 0; - out_2174696974037943497[44] = 1; - out_2174696974037943497[45] = 0; - out_2174696974037943497[46] = 0; - out_2174696974037943497[47] = 0; - out_2174696974037943497[48] = 0; - out_2174696974037943497[49] = 0; - out_2174696974037943497[50] = 0; - out_2174696974037943497[51] = 0; - out_2174696974037943497[52] = 0; - out_2174696974037943497[53] = 0; +void H_14(double *state, double *unused, double *out_9024838036052476526) { + out_9024838036052476526[0] = 0; + out_9024838036052476526[1] = 0; + out_9024838036052476526[2] = 0; + out_9024838036052476526[3] = 0; + out_9024838036052476526[4] = 0; + out_9024838036052476526[5] = 0; + out_9024838036052476526[6] = 1; + out_9024838036052476526[7] = 0; + out_9024838036052476526[8] = 0; + out_9024838036052476526[9] = 0; + out_9024838036052476526[10] = 0; + out_9024838036052476526[11] = 0; + out_9024838036052476526[12] = 0; + out_9024838036052476526[13] = 0; + out_9024838036052476526[14] = 0; + out_9024838036052476526[15] = 0; + out_9024838036052476526[16] = 0; + out_9024838036052476526[17] = 0; + out_9024838036052476526[18] = 0; + out_9024838036052476526[19] = 0; + out_9024838036052476526[20] = 0; + out_9024838036052476526[21] = 0; + out_9024838036052476526[22] = 0; + out_9024838036052476526[23] = 0; + out_9024838036052476526[24] = 0; + out_9024838036052476526[25] = 1; + out_9024838036052476526[26] = 0; + out_9024838036052476526[27] = 0; + out_9024838036052476526[28] = 0; + out_9024838036052476526[29] = 0; + out_9024838036052476526[30] = 0; + out_9024838036052476526[31] = 0; + out_9024838036052476526[32] = 0; + out_9024838036052476526[33] = 0; + out_9024838036052476526[34] = 0; + out_9024838036052476526[35] = 0; + out_9024838036052476526[36] = 0; + out_9024838036052476526[37] = 0; + out_9024838036052476526[38] = 0; + out_9024838036052476526[39] = 0; + out_9024838036052476526[40] = 0; + out_9024838036052476526[41] = 0; + out_9024838036052476526[42] = 0; + out_9024838036052476526[43] = 0; + out_9024838036052476526[44] = 1; + out_9024838036052476526[45] = 0; + out_9024838036052476526[46] = 0; + out_9024838036052476526[47] = 0; + out_9024838036052476526[48] = 0; + out_9024838036052476526[49] = 0; + out_9024838036052476526[50] = 0; + out_9024838036052476526[51] = 0; + out_9024838036052476526[52] = 0; + out_9024838036052476526[53] = 0; } #include #include @@ -1113,44 +1113,44 @@ void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, doub void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea) { update<3, 3, 0>(in_x, in_P, h_14, H_14, NULL, in_z, in_R, in_ea, MAHA_THRESH_14); } -void pose_err_fun(double *nom_x, double *delta_x, double *out_7352766374165151156) { - err_fun(nom_x, delta_x, out_7352766374165151156); +void pose_err_fun(double *nom_x, double *delta_x, double *out_8612662017984698055) { + err_fun(nom_x, delta_x, out_8612662017984698055); } -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5943722919136625654) { - inv_err_fun(nom_x, true_x, out_5943722919136625654); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6583538545281744086) { + inv_err_fun(nom_x, true_x, out_6583538545281744086); } -void pose_H_mod_fun(double *state, double *out_2863715751586478773) { - H_mod_fun(state, out_2863715751586478773); +void pose_H_mod_fun(double *state, double *out_7537065087244600898) { + H_mod_fun(state, out_7537065087244600898); } -void pose_f_fun(double *state, double dt, double *out_4163803070138990822) { - f_fun(state, dt, out_4163803070138990822); +void pose_f_fun(double *state, double dt, double *out_6289537546199484080) { + f_fun(state, dt, out_6289537546199484080); } -void pose_F_fun(double *state, double dt, double *out_3325085581577154625) { - F_fun(state, dt, out_3325085581577154625); +void pose_F_fun(double *state, double dt, double *out_3263235949829739422) { + F_fun(state, dt, out_3263235949829739422); } -void pose_h_4(double *state, double *unused, double *out_1824245269356126422) { - h_4(state, unused, out_1824245269356126422); +void pose_h_4(double *state, double *unused, double *out_6871661940623032085) { + h_4(state, unused, out_6871661940623032085); } -void pose_H_4(double *state, double *unused, double *out_908091458257428799) { - H_4(state, unused, out_908091458257428799); +void pose_H_4(double *state, double *unused, double *out_5061597179712991997) { + H_4(state, unused, out_5061597179712991997); } -void pose_h_10(double *state, double *unused, double *out_5992945801677381357) { - h_10(state, unused, out_5992945801677381357); +void pose_h_10(double *state, double *unused, double *out_974669649895256171) { + h_10(state, unused, out_974669649895256171); } -void pose_H_10(double *state, double *unused, double *out_1551783764236526040) { - H_10(state, unused, out_1551783764236526040); +void pose_H_10(double *state, double *unused, double *out_3862977066975629010) { + H_10(state, unused, out_3862977066975629010); } -void pose_h_13(double *state, double *unused, double *out_4650572533653388734) { - h_13(state, unused, out_4650572533653388734); +void pose_h_13(double *state, double *unused, double *out_2719357598944466940) { + h_13(state, unused, out_2719357598944466940); } -void pose_H_13(double *state, double *unused, double *out_1472693377939272903) { - H_13(state, unused, out_1472693377939272903); +void pose_H_13(double *state, double *unused, double *out_8273871005045324798) { + H_13(state, unused, out_8273871005045324798); } -void pose_h_14(double *state, double *unused, double *out_9091688560146400700) { - h_14(state, unused, out_9091688560146400700); +void pose_h_14(double *state, double *unused, double *out_5270922229605318979) { + h_14(state, unused, out_5270922229605318979); } -void pose_H_14(double *state, double *unused, double *out_2174696974037943497) { - H_14(state, unused, out_2174696974037943497); +void pose_H_14(double *state, double *unused, double *out_9024838036052476526) { + H_14(state, unused, out_9024838036052476526); } void pose_predict(double *in_x, double *in_P, double *in_Q, double dt) { predict(in_x, in_P, in_Q, dt); diff --git a/selfdrive/locationd/models/generated/pose.h b/selfdrive/locationd/models/generated/pose.h index 4c7cf1aa9..e544f8386 100644 --- a/selfdrive/locationd/models/generated/pose.h +++ b/selfdrive/locationd/models/generated/pose.h @@ -5,18 +5,18 @@ void pose_update_4(double *in_x, double *in_P, double *in_z, double *in_R, doubl void pose_update_10(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void pose_update_13(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); void pose_update_14(double *in_x, double *in_P, double *in_z, double *in_R, double *in_ea); -void pose_err_fun(double *nom_x, double *delta_x, double *out_7352766374165151156); -void pose_inv_err_fun(double *nom_x, double *true_x, double *out_5943722919136625654); -void pose_H_mod_fun(double *state, double *out_2863715751586478773); -void pose_f_fun(double *state, double dt, double *out_4163803070138990822); -void pose_F_fun(double *state, double dt, double *out_3325085581577154625); -void pose_h_4(double *state, double *unused, double *out_1824245269356126422); -void pose_H_4(double *state, double *unused, double *out_908091458257428799); -void pose_h_10(double *state, double *unused, double *out_5992945801677381357); -void pose_H_10(double *state, double *unused, double *out_1551783764236526040); -void pose_h_13(double *state, double *unused, double *out_4650572533653388734); -void pose_H_13(double *state, double *unused, double *out_1472693377939272903); -void pose_h_14(double *state, double *unused, double *out_9091688560146400700); -void pose_H_14(double *state, double *unused, double *out_2174696974037943497); +void pose_err_fun(double *nom_x, double *delta_x, double *out_8612662017984698055); +void pose_inv_err_fun(double *nom_x, double *true_x, double *out_6583538545281744086); +void pose_H_mod_fun(double *state, double *out_7537065087244600898); +void pose_f_fun(double *state, double dt, double *out_6289537546199484080); +void pose_F_fun(double *state, double dt, double *out_3263235949829739422); +void pose_h_4(double *state, double *unused, double *out_6871661940623032085); +void pose_H_4(double *state, double *unused, double *out_5061597179712991997); +void pose_h_10(double *state, double *unused, double *out_974669649895256171); +void pose_H_10(double *state, double *unused, double *out_3862977066975629010); +void pose_h_13(double *state, double *unused, double *out_2719357598944466940); +void pose_H_13(double *state, double *unused, double *out_8273871005045324798); +void pose_h_14(double *state, double *unused, double *out_5270922229605318979); +void pose_H_14(double *state, double *unused, double *out_9024838036052476526); void pose_predict(double *in_x, double *in_P, double *in_Q, double dt); } \ No newline at end of file diff --git a/selfdrive/ui/ui b/selfdrive/ui/ui index aceec4e86..5b4796f8e 100755 Binary files a/selfdrive/ui/ui and b/selfdrive/ui/ui differ