From df9afb74b20f79522be973b74f4d06a71b09c76e Mon Sep 17 00:00:00 2001 From: Alec Krawciw Date: Mon, 29 Jul 2024 16:16:26 -0400 Subject: [PATCH] Clean-up MPC * Remove cbit route checker. * Change corridor slider to use meters. * Add error checking to pose graph relaxation. * Improve pose interpolation to be in the lie algebra. * Update ui colours to match the slider bar. * Improve speed scheduling at the start of the path * Modify RViz to show poses along MPC solution. * Make all chain accessors const * Refactor cbit to start and stop based on state machine events. * Change the planning thread creation * Remove duplication of getChainInfo * Use the timestamp of the last command being sent out instead of time of point cloud. * Add Path typename rename 2d vectors. * Change MPC to use casadi as solver instead of STEAM * Removing some old config parameters * Refactor helper MPC functions into mpc_common * Remove old speed scheduler code * Move to fork of casadi for compilation reasons * Make the angle_weight for pa calculation a config parameter * Modify cmake to force a header export on every build * Add Ouster Jackal configs + add additional visualization/publications for external navigation * add localization index to PathInforForExternalNavigation * Fix error in evaluating so2 error * Change default corridor settings. Increase minimum to 20cm * tuning changes, log addition, timestamp fix * update jackal visualize flags * Add profiling options. * tune mpc params * Modify preprocessing to reduce computation time * Updated configs to match new MPC * lmprove the multi-threading shutdown behaviour. * add in angle threshold for route completion. Switch ouster jackal to cyclonedds * Update configs to set new angle threshold --------- Co-authored-by: James Han --- Dockerfile | 15 +- .../bumblebee_grizzly_learned_features.yaml | 494 +- config/hdl64_grizzly_default.yaml | 54 +- config/honeycomb_grizzly_default.yaml | 76 +- config/honeycomb_grizzly_learned.yaml | 350 - config/nerian_warthog_default.yaml | 81 +- config/ouster_jackal_default.yaml | 396 + config/ouster_warthog_default.yaml | 72 +- launch/offline_ouster_jackal.launch.yaml | 46 + ...line_bumblebee_learned_grizzly.launch.yaml | 10 +- launch/online_honeycomb_grizzly.launch.yaml | 6 +- launch/online_ouster_jackal.launch.yaml | 45 + main/src/deps/steam | 2 +- .../vtr-gui/src/components/graph/GraphMap.js | 5 +- .../src/components/tools/AnnotateSlider.js | 18 +- .../preprocessing/preprocessing_module.hpp | 2 +- .../modules/odometry/odometry_icp_module.cpp | 21 +- .../preprocessing/preprocessing_module.cpp | 12 +- main/src/vtr_navigation/launch/vtr.launch.py | 1 + .../vtr_navigation/src/graph_map_server.cpp | 10 +- main/src/vtr_navigation/src/navigator.cpp | 4 +- main/src/vtr_path_planning/CMakeLists.txt | 30 +- .../include/vtr_path_planning/cbit/cbit.hpp | 62 +- .../vtr_path_planning/cbit/cbit_config.hpp | 2 - .../vtr_path_planning/cbit/cbit_costmap.hpp | 2 +- .../cbit/cbit_path_planner.hpp | 24 +- .../vtr_path_planning/cbit/generate_pq.hpp | 7 +- .../include/vtr_path_planning/cbit/utils.hpp | 27 + .../cbit/visualization_utils.hpp | 29 +- .../mpc/custom_loss_functions.hpp | 60 - .../mpc/lateral_error_evaluators.hpp | 99 - .../vtr_path_planning/mpc/mpc_common.hpp | 69 + .../mpc/mpc_path_planner.hpp | 113 +- .../vtr_path_planning/mpc/mpc_solver.hpp | 162 + .../mpc/scalar_log_barrier_evaluator.hpp | 227 - .../vtr_path_planning/mpc/speed_scheduler.hpp | 22 +- main/src/vtr_path_planning/package.xml | 1 + .../scripts/export_unicycle.py | 9 + .../scripts/run_unicycle_mpc.py | 215 + .../scripts/simulation_code.py | 183 + .../scripts/unicycle_solver.py | 177 + main/src/vtr_path_planning/src/cbit/cbit.cpp | 464 +- .../src/cbit/cbit_path_planner.cpp | 130 +- .../src/cbit/generate_pq.cpp | 11 - main/src/vtr_path_planning/src/cbit/utils.cpp | 17 +- .../src/cbit/visualization_utils.cpp | 108 +- .../src/mpc/lateral_error_evaluator.cpp | 127 - .../vtr_path_planning/src/mpc/mpc_common.cpp | 287 + .../src/mpc/mpc_path_planner.cpp | 653 +- .../vtr_path_planning/src/mpc/mpc_solver.cpp | 41825 ++++++++++++++++ .../src/mpc/speed_scheduler.cpp | 73 +- .../test/closed_loop/unicycle_tests.cpp | 559 + .../src/vtr_path_planning_msgs/CMakeLists.txt | 31 + .../msg/PathInfoForExternalNavigation.msg | 23 + main/src/vtr_path_planning_msgs/package.xml | 26 + .../path/localization_chain.hpp | 11 +- .../include/vtr_pose_graph/path/path.hpp | 61 +- .../vtr_tactic/include/vtr_tactic/tactic.hpp | 1 + main/src/vtr_tactic/src/tactic.cpp | 12 +- .../test/tactic/test_query_cache.cpp | 2 +- main/src/vtr_tactic_msgs/msg/EnvInfo.msg | 2 +- .../vtr_torch/modules/torch_module.hpp | 2 - .../src/vtr_torch/include/vtr_torch/types.hpp | 2 +- rviz/ouster.rviz | 119 +- 64 files changed, 44969 insertions(+), 2817 deletions(-) delete mode 100644 config/honeycomb_grizzly_learned.yaml create mode 100644 config/ouster_jackal_default.yaml create mode 100644 launch/offline_ouster_jackal.launch.yaml create mode 100644 launch/online_ouster_jackal.launch.yaml delete mode 100644 main/src/vtr_path_planning/include/vtr_path_planning/mpc/custom_loss_functions.hpp delete mode 100644 main/src/vtr_path_planning/include/vtr_path_planning/mpc/lateral_error_evaluators.hpp create mode 100644 main/src/vtr_path_planning/include/vtr_path_planning/mpc/mpc_common.hpp create mode 100644 main/src/vtr_path_planning/include/vtr_path_planning/mpc/mpc_solver.hpp delete mode 100644 main/src/vtr_path_planning/include/vtr_path_planning/mpc/scalar_log_barrier_evaluator.hpp create mode 100755 main/src/vtr_path_planning/scripts/export_unicycle.py create mode 100644 main/src/vtr_path_planning/scripts/run_unicycle_mpc.py create mode 100644 main/src/vtr_path_planning/scripts/simulation_code.py create mode 100644 main/src/vtr_path_planning/scripts/unicycle_solver.py delete mode 100644 main/src/vtr_path_planning/src/mpc/lateral_error_evaluator.cpp create mode 100644 main/src/vtr_path_planning/src/mpc/mpc_common.cpp create mode 100644 main/src/vtr_path_planning/src/mpc/mpc_solver.cpp create mode 100644 main/src/vtr_path_planning/test/closed_loop/unicycle_tests.cpp create mode 100644 main/src/vtr_path_planning_msgs/CMakeLists.txt create mode 100644 main/src/vtr_path_planning_msgs/msg/PathInfoForExternalNavigation.msg create mode 100644 main/src/vtr_path_planning_msgs/package.xml diff --git a/Dockerfile b/Dockerfile index 99395247e..03b9f37a4 100644 --- a/Dockerfile +++ b/Dockerfile @@ -169,13 +169,22 @@ RUN rm libtorch.zip ENV TORCH_LIB=/opt/torch/libtorch ENV LD_LIBRARY_PATH=$TORCH_LIB/lib:${LD_LIBRARY_PATH:+:${LD_LIBRARY_PATH}} ENV CMAKE_PREFIX_PATH=$TORCH_LIB:$CMAKE_PREFIX_PATH + +RUN apt install swig liblapack-dev libmetis-dev -y -q --install-recommends +RUN mkdir -p ${HOMEDIR}/.casadi && cd ${HOMEDIR}/.casadi \ + && git clone https://github.com/utiasASRL/casadi.git . +RUN cd ${HOMEDIR}/.casadi \ + && mkdir -p build && cd build \ + && cmake build -DWITH_PYTHON=ON -DWITH_PYTHON3=ON -DWITH_IPOPT=ON -DWITH_BUILD_IPOPT=ON -DWITH_BUILD_REQUIRED=ON -DWITH_SELFCONTAINED=ON .. \ + && make -j${NUMPROC} install +ENV PYTHONPATH=${PYTHONPATH}:/usr/local +ENV LD_LIBRARY_PATH=/usr/local/casadi:${LD_LIBRARY_PATH} + ENV NVIDIA_DRIVER_CAPABILITIES compute,utility,graphics -RUN apt install htop +RUN apt install -q -y vim htop -# Install vim -RUN apt update && apt install -q -y vim ## Switch to specified user USER ${USERID}:${GROUPID} diff --git a/config/bumblebee_grizzly_learned_features.yaml b/config/bumblebee_grizzly_learned_features.yaml index 482874ddd..06030f89e 100644 --- a/config/bumblebee_grizzly_learned_features.yaml +++ b/config/bumblebee_grizzly_learned_features.yaml @@ -29,55 +29,10 @@ "tactic.pipeline" ] - # control frame from the urdf - robot_frame: base_link - # camera related - camera_frame: front_xb3 - camera_left_topic: /xb3/RGB/left_image - camera_right_topic: /xb3/RGB/right_image - - ############ map projection configuration ############ - map_projection: - origin_lat: 43.78220 - origin_lng: -79.4661 - origin_theta: 1.3 # positive == clockwise - scale: 1.0 - - ############ tactic configuration ############ - tactic: - enable_parallelization: true - extrapolate_odometry: true # path tracker - localization_only_keyframe: true # EDIT BY SHERRY - localization_skippable: true #false # false - save_odometry_result: true - save_localization_result: true - - default_loc_cov: [1.0, 0.5, 0.5, 0.25, 0.25, 0.5] - chain: - min_cusp_distance: 1.5 - angle_weight: 7.0 - search_depth: 20 - search_back_depth: 10 - distance_warning: 3.0 - live_mem: - enable: true - lookahead_distance: 100 - window_size: 10 - map_mem: - enable: true - lookahead_distance: 15 - vertex_life_span: 10 - - visualize: true - vis_loc_path_offset: [0., 0., 0.] - ############ pipeline configuration ############ pipeline: type: stereo - preprocessing: ["adapter","extraction", "triangulation"] - odometry: ["recall", "matcher", "ransac", "keyframe_optimize", "vertex_test"] - bundle_adjustment: ["recall", "steam"] localization: [ "recall", @@ -90,78 +45,15 @@ "steam", ] - preprocessing: - # conversion+extraction module - adapter: - type: stereo.adapter - distortion: [0.0, 0.0, 0.0, 0.0, 0.0] - intrinsic: - - 388.425 - - 0.0 - - 254.002 - - 0.0 - - 388.425 - - 197.32 - - 0.0 - - 0.0 - - 1.0 - - #zed2i - # - 371.88385009765625 - # - 0.0 - # - 433.3143005371094 - # - 0.0 - # - 377.7867736816406 - # - 252.2572021484375 - # - 0.0 - # - 0.0 - # - 1.0 - baseline: 0.24 #0.12 - - extraction: type: stereo.conversion_extraction visualize: true - - - # specify which conversions to do between different image formats from: - # RGB_TO_GRAYSCALE, RGB_TO_COLOR_CONSTANT - conversions: ["RGB_TO_GRAYSCALE", "RGB_TO_COLOR_CONSTANT"] - color_constant: - weights: [0.43] - histogram_equalization: false - visualize_raw_features: false - extractor: type: ASRL_GPU_SURF visualize_disparity: false use_learned: true channels: ["grayscale", "cc_0.430000"] - surf: - threshold: 0.000001 - upright_flag: true - nOctaves: 4 - nIntervals: 4 - initialScale: 1.5 - edgeScale: 1.5 - l1: 2.0 # 3.f/1.5f - l2: 3.333333 # 5.f/1.5f - l3: 2.0 # 3.f/1.5f - l4: 0.666667 # 1.f/1.5f - initialStep: 1 - targetFeatures: 1000 - detector_threads_x: 16 - detector_threads_y: 16 - regions_horizontal: 16 - regions_vertical: 16 - regions_target: 1000 - stereoDisparityMinimum: 0.0 - stereoDisparityMaximum: 64.0 - stereoCorrelationThreshold: 0.79 - stereoYTolerance: 0.9 - stereoScaleTolerance: 0.9 - learned: # we're providing the surf settings (don't change this param, use a different file) @@ -173,188 +65,9 @@ stereoDisparityMinimum: 0.1 stereoDisparityMaximum: 100.0 - triangulation: - type: image_triangulation - visualize: false - min_triangulation_depth: 0.01 - max_triangulation_depth: 500.0 - - odometry: - recall: - type: landmark_recall - - landmark_source: live - - matcher: - type: asrl_stereo_matcher - - check_laplacian_bit: true - check_octave: true - check_response: true - min_response_ratio: 0.2 - matching_pixel_thresh: 400 - tight_matching_pixel_thresh: 20 - - # !!! The covariance is currently set to identity * dt*dt, - # So the sigma will be dt, where dt is the time difference - # between the last keyframe and the live frame. - # This is ugly, but it is mirroring the mel matcher for - # simplicity, since we are just trying to get things working. - # in the case that we're not extrapolating and using identity, - # the covariance is multiplied by 4 (dt is doubled). - # This is not currently configurable. - # e.g. 0.4 is 400 ms while extrapolating, and 200 ms with identity - tight_matching_x_sigma: 0.6 # in s - tight_matching_y_sigma: 0.6 # in s - tight_matching_theta_sigma: 0.6 # in s - - use_pixel_variance: true - prediction_method: se3 - max_point_depth: 500.0 - descriptor_thresh: 0.1 - parallel_threads: 8 - visualize_feature_matches: true - min_matches: 1 - - ransac: - type: stereo_ransac - - rig: "front_xb3" - channel: "grayscale" - - enable: true - iterations: 2000 - flavor: Vanilla - sigma: 3.5 - threshold: 5.0 - early_stop_ratio: 0.8 - early_stop_min_inliers: 200 - visualize_ransac_inliers: true - use_migrated_points: false - min_inliers: 15 - - mask_depth: 200.0 - mask_depth_inlier_count: 0 - use_covariance: false - is_odometry: true - - keyframe_optimize: - type: keyframe_optimization - - solver_type: "DoglegGaussNewton" - loss_function: "DCS" - - verbose: true - iterations: 5 - absoluteCostThreshold: 0.0 - absoluteCostChangeThreshold: .0001 - relativeCostChangeThreshold: .0001 - ratioThresholdShrink: 0.25 - ratioThresholdGrow: 0.75 - shrinkCoeff: 0.5 - growCoeff: 3.0 - maxShrinkSteps: 50 - - perform_planarity_check: false - plane_distance: 20.0 - - min_point_depth: 0.0 - max_point_depth: 200.0 - backtrackMultiplier: 0.5 - maxBacktrackSteps: 10 - - disable_solver: false # Set true to do everything but run optimise() - - use_T_q_m_prior: false - - ## sample and save trajectory results to disk - save_trajectory: false - - # Smoothing based on vehicle dynamics, - # tune these for your robot in your robot-specific config - # E.g. a nonholonomic ground vehicle on rugged terrain: - trajectory_smoothing: true - ## smoothing factor - ## zero mean white noise on acceleration - ## one-sigma standard deviation for linear acceleration (m/s/s) - lin_acc_std_dev_x: 10.0 ## lifted from max acceleration in grizzly launch - lin_acc_std_dev_y: 10.0 ## grizzly does not strafe very fast - lin_acc_std_dev_z: 10.0 ## neither does it jump quickly, except for going over bumps - ## one-sigma standard devitation for angular acceleration (rad/s/s0) - ang_acc_std_dev_x: 1.0 # roll rate, should be low - ang_acc_std_dev_y: 1.0 # pitch rate, - ang_acc_std_dev_z: 1.0 # yaw rate, approx 2 deg - - # Mean for the velocity prior (0 mean is probably what you want) - velocity_prior: false - lin_vel_mean_x: 4.0 - lin_vel_mean_y: 0.0 - lin_vel_mean_z: 0.0 - ang_vel_mean_x: 0.0 - ang_vel_mean_y: 0.0 - ang_vel_mean_z: 0.0 - - ## standard deviation for velocity prior - ## linear velocity m/s - lin_vel_std_dev_x: 8.0 ## the grizzly is usually within +- 3.0 m/s - lin_vel_std_dev_y: 3.0 - lin_vel_std_dev_z: 0.5 - ## angular velocity rad/s - ang_vel_std_dev_x: 0.5 - ang_vel_std_dev_y: 0.5 - ang_vel_std_dev_z: 0.2 - - pose_prior_enable: false - use_migrated_points: false - is_odometry: true - - vertex_test: - type: simple_vertex_creation_test - - min_distance: 0.05 - min_creation_distance: 0.3 - max_creation_distance: 2.0 - rotation_threshold_min: 3.0 - rotation_threshold_max: 20.0 - match_threshold_min_count: 50 - match_threshold_fail_count: 15 - + + localization: - recall: - type: landmark_recall - landmark_source: live - - sub_map_extraction: - type: sub_map_extraction - - temporal_min_depth: 5 # use at least a depth of 5 - temporal_max_depth: 10 # vertices to search for scaled sigma - search_spatially: true - sigma_scale: 3.0 - angle_weight: 5.0 - - tod_recognition: - # the module type to configure (don't change this param) - type: timeofday_recognition - - # Whether to print out debug logs - verbose: true - - # The number of experiences to recommend for localization - num_desired_experiences: 8 - - # Whether or not we make the recommendation and restrict the experiences - in_the_loop: true - - # The weight to convert time-of-day difference to a distance - # probably just leave this at 1.0, and adjust the rest - time_of_day_weight: 1.0 - - # The weight to convert total time difference to a distance - # 1.0/24.0 means 1 day is worth 1 time-of-day hour - # 1.0/(7.0*24.0) means 1 week is worth 1 time-of-day-hour - total_time_weight: 0.00595 # 1.0/(7.0*24.0) - experience_triage: type: experience_triage @@ -362,9 +75,6 @@ only_privileged: true in_the_loop: true - migration: - type: landmark_migration - matcher: type: mel_matcher min_response_ratio: 0.0 #0.2 @@ -376,202 +86,4 @@ match_on_gpu: true #EDIT use_learned_features: true - ransac: - # the module type (don't change this param) - type: stereo_ransac - - enable: true - iterations: 6000 - flavor: Vanilla - sigma: 3.5 - threshold: 10.0 - early_stop_ratio: 1.0 - early_stop_min_inliers: 400 - visualize_ransac_inliers: true - use_migrated_points: true - min_inliers: 6 - - mask_depth: 1000.0 - mask_depth_inlier_count: 0 - use_covariance: false - is_odometry: false - - steam: - type: keyframe_optimization - - solver_type: "DoglegGaussNewton" - loss_function: "DCS" - - verbose: false - iterations: 15 - absoluteCostThreshold: 0.0 - absoluteCostChangeThreshold: .0001 - relativeCostChangeThreshold: .0001 - ratioThresholdShrink: 0.25 - ratioThresholdGrow: 0.75 - shrinkCoeff: 0.5 - growCoeff: 3.0 - maxShrinkSteps: 50 - max_point_depth: 1000.0 - backtrackMultiplier: 0.5 - maxBacktrackSteps: 10 - - disable_solver: false # Set true to do everything but run optimise() - - use_T_q_m_prior: true - - save_trajectory: false - trajectory_smoothing: false - velocity_prior: false - pose_prior_enable: true - use_migrated_points: true - is_odometry: false - - bundle_adjustment: - recall: - type: stereo_windowed_recall - window_size: 5 - - steam: - type: stereo_window_optimization - - enable: true - solver_type: "DoglegGaussNewton" - loss_function: "DCS" - verbose: false - iterations: 3 - - absoluteCostThreshold: 0.0 - absoluteCostChangeThreshold: .01 - relativeCostChangeThreshold: .01 - ratioThresholdShrink: 0.25 - ratioThresholdGrow: 0.75 - shrinkCoeff: 0.5 - growCoeff: 3.0 - maxShrinkSteps: 50 - - perform_planarity_check: false - plane_distance: 20.0 - - min_point_depth: 1.0 - max_point_depth: 800.0 - - backtrackMultiplier: 0.5 - maxBacktrackSteps: 10 - - disable_solver: false # Set true to do everything but run optimise() - - use_T_q_m_prior: false - - ## sample and save trajectory results to disk - save_trajectory: false - - trajectory_smoothing: true - ## smoothing factor - ## zero mean white noise on acceleration - ## one-sigma standard deviation for linear acceleration (m/s/s) - lin_acc_std_dev_x: 0.25 ## lifted from max acceleration in grizzly launch - lin_acc_std_dev_y: 0.001 ## grizzly does not strafe very fast - lin_acc_std_dev_z: 0.1 ## neither does it jump quickly, except for going over bumps - ## one-sigma standard devitation for angular acceleration (rad/s/s0) - ang_acc_std_dev_x: 0.001 # roll rate, should be low - ang_acc_std_dev_y: 0.001 # pitch rate, - ang_acc_std_dev_z: 0.003 # yaw rate, approx 2 deg - - velocity_prior: false - lin_vel_mean_x: 0.0 - lin_vel_mean_y: 0.0 - lin_vel_mean_z: 0.0 - ang_vel_mean_x: 0.0 - ang_vel_mean_y: 0.0 - ang_vel_mean_z: 0.0 - - ## standard deviation for velocity prior - ## linear velocity m/s - lin_vel_std_dev_x: 3.0 ## the grizzly is usually within +- 3.0 m/s - lin_vel_std_dev_y: 0.02 - lin_vel_std_dev_z: 0.02 - ## angular velocity rad/s - ang_vel_std_dev_x: 0.001 - ang_vel_std_dev_y: 0.001 - ang_vel_std_dev_z: 0.003 - - depth_prior_enable: true - depth_prior_weight: 1000.0 - - path_planning: - type: cbit - control_period: 100 # ms - cbit: - obstacle_avoidance: false - obs_padding: 0.0 - curv_to_euclid_discretization: 10 - sliding_window_width: 12.0 - sliding_window_freespace_padding: 0.5 - corridor_resolution: 0.2 - state_update_freq: 2.0 - update_state: true - rand_seed: 1 - - # Planner Tuning Params - initial_samples: 400 - batch_samples: 200 - pre_seed_resolution: 0.5 - alpha: 0.5 - q_max: 2.5 - frame_interval: 50 - iter_max: 10000000 - eta: 1.0 - rad_m_exhange: 1.00 - initial_exp_rad: 1.25 - extrapolation: false - incremental_plotting: false - plotting: true - costmap: - costmap_filter_value: 0.01 - costmap_history: 30 - - speed_scheduler: - planar_curv_weight: 2.50 - profile_curv_weight: 0.5 - eop_weight: 1.0 - min_vel: 0.5 - - mpc: - # Controller Params - extrapolate_robot_pose: true - mpc_verbosity: false - homotopy_guided_mpc: false - horizon_steps: 20 - horizon_step_size: 0.3 - forward_vel: 1.0 - max_lin_vel: 1.5 - max_ang_vel: 1.25 - robot_linear_velocity_scale: 1.1 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration - robot_angular_velocity_scale: 1.0 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration - - vehicle_model: "unicycle" - - # Cost Function Covariance Matrices - pose_error_cov: [100.0, 100.0, 1000.0, 1000.0, 1000.0, 200.0] - vel_error_cov: [25.0, 10.0] - acc_error_cov: [10.0, 10.0] - kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] - lat_error_cov: [20.0] - - # Cost Function Covariance Matrices (Tracking Backup) - #pose_error_cov: [100.0, 100.0, 1000.0, 1000.0, 1000.0, 200.0] - #vel_error_cov: [30.0, 30.0] # was [0.1 2.0] # No longer using this cost term - #acc_error_cov: [10.0, 10.0] - #kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] - #lat_error_cov: [10.0] - - # Cost Function Weights - pose_error_weight: 1.0 - vel_error_weight: 1.0 - acc_error_weight: 1.0 - kin_error_weight: 1.0 - lat_error_weight: 0.01 - - # Misc - command_history_length: 100 + \ No newline at end of file diff --git a/config/hdl64_grizzly_default.yaml b/config/hdl64_grizzly_default.yaml index 6b6e3a6d6..74fda6ad9 100644 --- a/config/hdl64_grizzly_default.yaml +++ b/config/hdl64_grizzly_default.yaml @@ -299,15 +299,14 @@ path_planning: type: cbit control_period: 100 # ms - cbit: obstacle_avoidance: false obs_padding: 0.0 curv_to_euclid_discretization: 10 sliding_window_width: 12.0 sliding_window_freespace_padding: 0.5 - corridor_resolution: 0.1 - state_update_freq: 1.0 + corridor_resolution: 0.2 + state_update_freq: 2.0 update_state: true rand_seed: 1 @@ -315,10 +314,9 @@ initial_samples: 400 batch_samples: 200 pre_seed_resolution: 0.5 - alpha: 0.5 + alpha: 0.25 q_max: 2.5 frame_interval: 50 - iter_max: 10000000 eta: 1.0 rad_m_exhange: 1.00 initial_exp_rad: 0.75 @@ -327,49 +325,27 @@ plotting: true costmap: costmap_filter_value: 0.01 - costmap_history: 30 - + costmap_history: 15 # Note I dont think im actually using this one anymore, am using the one in the change detection v3 now + speed_scheduler: - planar_curv_weight: 2.50 - profile_curv_weight: 0.5 + planar_curv_weight: 0.0 + profile_curv_weight: 0.0 eop_weight: 1.0 min_vel: 0.5 - mpc: # Controller Params - extrapolate_robot_pose: true + extrapolate_robot_pose: false mpc_verbosity: false - homotopy_guided_mpc: false - horizon_steps: 20 - horizon_step_size: 0.3 - forward_vel: 0.75 - max_lin_vel: 1.25 - max_ang_vel: 0.75 - robot_linear_velocity_scale: 1.1 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration + forward_vel: 1.0 + max_lin_vel: 1.5 + max_ang_vel: 0.5 + max_ang_acc: 5.0 + ang_alpha: 0.811 + lin_alpha: 0.0 + robot_linear_velocity_scale: 1.0 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration robot_angular_velocity_scale: 1.0 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration vehicle_model: "unicycle" - # Cost Function Covariance Matrices - pose_error_cov: [100.0, 100.0, 1000.0, 1000.0, 1000.0, 200.0] - vel_error_cov: [25.0, 10.0] - acc_error_cov: [10.0, 10.0] - kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] - lat_error_cov: [20.0] - - # Cost Function Covariance Matrices (Tracking Backup) - #pose_error_cov: [100.0, 100.0, 1000.0, 1000.0, 1000.0, 200.0] - #vel_error_cov: [30.0, 30.0] # was [0.1 2.0] # No longer using this cost term - #acc_error_cov: [10.0, 10.0] - #kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] - #lat_error_cov: [10.0] - - # Cost Function Weights - pose_error_weight: 1.0 - vel_error_weight: 1.0 - acc_error_weight: 1.0 - kin_error_weight: 1.0 - lat_error_weight: 0.01 - # Misc command_history_length: 100 diff --git a/config/honeycomb_grizzly_default.yaml b/config/honeycomb_grizzly_default.yaml index 4c2931ca8..eb7cdc15e 100644 --- a/config/honeycomb_grizzly_default.yaml +++ b/config/honeycomb_grizzly_default.yaml @@ -3,7 +3,7 @@ log_to_file: true log_debug: true log_enabled: - #- navigation + - navigation #- navigation.graph_map_server #- navigation.command # tactic @@ -13,21 +13,20 @@ #- tactic.module.live_mem_manager #- tactic.module.graph_mem_manager #- mission.state_machine - - mission.server + #- mission.server # planner manager - #- cbit.control + - path_planning + - path_planning.cbit + - path_planning.cbit_planner + - cbit.path_planning + #- cbit_planner.path_planning #- cbit.debug - #- cbit.path_planning - #- cbit.obstacle_filtering - #- grizzly_controller_tests.cbit - # mpc - #- mpc.speed_scheduler + - mpc.solver + - mpc.speed_scheduler + #- mpc.cost_function + #- mpc.cbit #- mpc.debug - #- mpc.solver - # path planner - #- cbit_planner.path_planning - # pose graph - #- pose_graph + - cbit.control # lidar pipeline #- lidar.pipeline #- lidar.honeycomb_converter @@ -60,7 +59,9 @@ task_queue_num_threads: 1 task_queue_size: -1 - route_completion_translation_threshold: 0.65 + route_completion_translation_threshold: 0.15 + route_completion_angle_threshold: 0.1 + chain: min_cusp_distance: 1.5 @@ -312,61 +313,38 @@ initial_samples: 400 batch_samples: 200 pre_seed_resolution: 0.5 - alpha: 0.5 + alpha: 0.25 q_max: 2.5 frame_interval: 50 - iter_max: 10000000 eta: 1.0 rad_m_exhange: 1.00 - initial_exp_rad: 1.25 + initial_exp_rad: 0.75 extrapolation: false incremental_plotting: false plotting: true costmap: costmap_filter_value: 0.01 - costmap_history: 30 - + costmap_history: 15 # Note I dont think im actually using this one anymore, am using the one in the change detection v3 now + speed_scheduler: - planar_curv_weight: 2.50 - profile_curv_weight: 0.5 + planar_curv_weight: 0.0 + profile_curv_weight: 0.0 eop_weight: 1.0 min_vel: 0.5 - mpc: # Controller Params - extrapolate_robot_pose: true + extrapolate_robot_pose: false mpc_verbosity: false - homotopy_guided_mpc: false - horizon_steps: 20 - horizon_step_size: 0.3 forward_vel: 1.0 max_lin_vel: 1.5 - max_ang_vel: 1.25 - robot_linear_velocity_scale: 1.1 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration + max_ang_vel: 0.5 + max_ang_acc: 5.0 + ang_alpha: 0.811 + lin_alpha: 0.0 + robot_linear_velocity_scale: 1.0 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration robot_angular_velocity_scale: 1.0 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration vehicle_model: "unicycle" - # Cost Function Covariance Matrices - pose_error_cov: [100.0, 100.0, 1000.0, 1000.0, 1000.0, 200.0] - vel_error_cov: [25.0, 10.0] - acc_error_cov: [10.0, 10.0] - kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] - lat_error_cov: [20.0] - - # Cost Function Covariance Matrices (Tracking Backup) - #pose_error_cov: [100.0, 100.0, 1000.0, 1000.0, 1000.0, 200.0] - #vel_error_cov: [30.0, 30.0] # was [0.1 2.0] # No longer using this cost term - #acc_error_cov: [10.0, 10.0] - #kin_error_cov: [0.001, 0.001, 0.001, 0.001, 0.001, 0.001] - #lat_error_cov: [10.0] - - # Cost Function Weights - pose_error_weight: 1.0 - vel_error_weight: 1.0 - acc_error_weight: 1.0 - kin_error_weight: 1.0 - lat_error_weight: 0.01 - # Misc command_history_length: 100 diff --git a/config/honeycomb_grizzly_learned.yaml b/config/honeycomb_grizzly_learned.yaml deleted file mode 100644 index c8d27d75d..000000000 --- a/config/honeycomb_grizzly_learned.yaml +++ /dev/null @@ -1,350 +0,0 @@ -/**: - ros__parameters: - log_to_file: true - log_debug: true - log_enabled: - #- navigation - - navigation.graph_map_server - - navigation.command - # tactic - - tactic - - tactic.pipeline - - tactic.module - - tactic.module.live_mem_manager - - tactic.module.graph_mem_manager - - torch - # path planner - # "path_planning", - # "path_planning.teb", - # "path_planning.cbit", - # "path_planning.cbit_planner", - # "mpc.cbit", - # "mpc_debug.cbit", - # mission planner - - mission.server - - mission.state_machine - # pose graph - - pose_graph - # pipeline specific - # "lidar.pipeline", - # "lidar.honeycomb_converter", - # "lidar.preprocessing", - # "lidar.odometry_icp", - # "lidar.odometry_map_maintenance", - # "lidar.vertex_test", - # "lidar.localization_map_recall", - # "lidar.localization_icp", - # "lidar.intra_exp_merging", - # "lidar.dynamic_detection", - # "lidar.inter_exp_merging", - # "lidar.change_detection", - # "lidar.ground_extraction", - # "lidar.obstacle_detection", - # "lidar.terrain_assessment", - robot_frame: base_link - env_info_topic: env_info - lidar_frame: honeycomb - lidar_topic: /points - graph_map: - origin_lat: 43.7822 - origin_lng: -79.4661 - origin_theta: 1.3 - scale: 1.0 - tactic: - enable_parallelization: true - preprocessing_skippable: false - odometry_mapping_skippable: false - localization_skippable: false - task_queue_num_threads: 1 - task_queue_size: -1 - route_completion_translation_threshold: 2.0 - chain: - min_cusp_distance: 1.5 - angle_weight: 7.0 - search_depth: 5 - search_back_depth: 10 - distance_warning: 5.0 - save_odometry_result: true - save_localization_result: true - visualize: true - rviz_loc_path_offset: - - 0.0 - - 0.0 - - 0.0 - pipeline: - type: lidar - preprocessing: - - conversion - - filtering - odometry: - - icp - - mapping - - vertex_test - - intra_exp_merging - - dynamic_detection - - inter_exp_merging - - memory - - network - localization: - - recall - - icp - - safe_corridor - - change_detection_sync - - memory - submap_translation_threshold: 1.5 - submap_rotation_threshold: 30.0 - preprocessing: - conversion: - type: lidar.honeycomb_converter_v2 - visualize: true - filtering: - type: lidar.preprocessing - num_threads: 8 - crop_range: 40.0 - frame_voxel_size: 0.2 - vertical_angle_res: 0.0132645 - polar_r_scale: 2.0 - r_scale: 4.0 - h_scale: 1.54 - num_sample1: 10000 - min_norm_score1: 0.95 - num_sample2: 10000 - min_norm_score2: 0.2 - min_normal_estimate_dist: 1.0 - max_normal_estimate_angle: 0.44 - cluster_num_sample: 10000 - visualize: true - odometry: - network: - type: torch.sample - use_gpu: true - abs_filepath: false - filepath: sample_model.pt - icp: - type: lidar.odometry_icp - use_trajectory_estimation: true - traj_num_extra_states: 0 - traj_lock_prev_pose: false - traj_lock_prev_vel: false - traj_qc_diag: - - 1.0 - - 0.1 - - 0.1 - - 0.1 - - 0.1 - - 1.0 - num_threads: 8 - first_num_steps: 2 - initial_max_iter: 4 - initial_max_pairing_dist: 1.5 - initial_max_planar_dist: 1.0 - refined_max_iter: 50 - refined_max_pairing_dist: 1.0 - refined_max_planar_dist: 0.3 - averaging_num_steps: 2 - verbose: false - max_iterations: 1 - min_matched_ratio: 0.5 - visualize: true - mapping: - type: lidar.odometry_map_maintenance_v2 - map_voxel_size: 0.2 - crop_range_front: 40.0 - back_over_front_ratio: 0.5 - point_life_time: 20.0 - visualize: true - vertex_test: - type: lidar.vertex_test - max_translation: 0.3 - max_rotation: 10.0 - intra_exp_merging: - type: lidar.intra_exp_merging_v2 - depth: 6.0 - map_voxel_size: 0.2 - crop_range_front: 40.0 - back_over_front_ratio: 0.5 - visualize: true - dynamic_detection: - type: lidar.dynamic_detection - depth: 12.0 - horizontal_resolution: 0.041 - vertical_resolution: 0.026 - max_num_observations: 2000 - min_num_observations: 4 - dynamic_threshold: 0.3 - visualize: true - inter_exp_merging: - type: lidar.inter_exp_merging_v2 - map_voxel_size: 0.2 - max_num_experiences: 128 - distance_threshold: 0.6 - planar_threshold: 0.2 - normal_threshold: 0.8 - dynamic_obs_threshold: 1 - visualize: true - memory: - type: live_mem_manager - window_size: 5 - localization: - recall: - type: lidar.localization_map_recall - map_version: pointmap - visualize: true - icp: - type: lidar.localization_icp - use_pose_prior: true - num_threads: 8 - first_num_steps: 2 - initial_max_iter: 4 - initial_max_pairing_dist: 1.5 - initial_max_planar_dist: 1.0 - refined_max_iter: 50 - refined_max_pairing_dist: 1.0 - refined_max_planar_dist: 0.3 - averaging_num_steps: 2 - verbose: false - max_iterations: 1 - min_matched_ratio: 0.3 - safe_corridor: - type: lidar.safe_corridor - lookahead_distance: 5.0 - corridor_width: 3.5 - influence_distance: 1.0 - resolution: 0.25 - size_x: 16.0 - size_y: 8.0 - visualize: true - change_detection_sync: - type: lidar.change_detection_v3 - detection_range: 8.0 - search_radius: 0.25 - negprob_threshold: 0.1 - use_prior: true - alpha0: 3.0 - beta0: 0.03 - use_support_filtering: true - support_radius: 0.25 - support_variance: 0.05 - support_threshold: 2.5 - influence_distance: 0.55 - minimum_distance: 0.35 - resolution: 0.25 - size_x: 16.0 - size_y: 8.0 - visualize: true - memory: - type: graph_mem_manager - vertex_life_span: 5 - window_size: 3 - obstacle_detection: - type: lidar.obstacle_detection - z_min: 0.5 - z_max: 2.0 - resolution: 0.6 - size_x: 40.0 - size_y: 20.0 - run_async: true - visualize: true - ground_extraction: - type: lidar.ground_extraction - z_offset: 0.2 - alpha: 0.035 - tolerance: 0.25 - Tm: 0.3 - Tm_small: 0.1 - Tb: 0.5 - Trmse: 0.1 - Tdprev: 1.0 - rmin: 2.0 - num_bins_small: 30.0 - bin_size_small: 0.5 - num_bins_large: 10.0 - bin_size_large: 1.0 - resolution: 0.6 - size_x: 40.0 - size_y: 20.0 - run_async: true - visualize: true - terrain_assessment: - type: lidar.terrain_assessment - lookahead_distance: 15.0 - corridor_width: 1.0 - search_radius: 1.0 - resolution: 0.5 - size_x: 40.0 - size_y: 20.0 - run_online: false - run_async: true - visualize: true - path_planning: - type: cbit - control_period: 33 - teb: - visualize: true - extrapolate: false - extrapolation_timeout: 2.0 - lookahead_distance: 8.5 - robot_model: circular - robot_radius: 0.5 - map_frame: planning frame - enable_homotopy_class_planning: true - free_goal_vel: true - max_vel_x: 0.6 - max_vel_y: 0.0 - max_vel_theta: 0.3 - min_turning_radius: 3 - weight_viapoint: 0.5 - weight_costmap: 100.0 - cbit: - obs_padding: 0.0 - curv_to_euclid_discretization: 20 - sliding_window_width: 10.0 - sliding_window_freespace_padding: 0.5 - corridor_resolution: 0.05 - state_update_freq: 0.5 - update_state: true - rand_seed: 1 - initial_samples: 250 - batch_samples: 100 - pre_seed_resolution: 0.5 - alpha: 0.5 - q_max: 2.5 - frame_interval: 50 - iter_max: 10000000 - eta: 1.1 - rad_m_exhange: 1.0 - initial_exp_rad: 0.75 - extrapolation: false - incremental_plotting: false - plotting: true - costmap: - costmap_filter_value: 0.01 - costmap_history: 15 - mpc: - horizon_steps: 40 - horizon_step_size: 0.1 - forward_vel: 1.0 - max_lin_vel: 1.25 - max_ang_vel: 0.75 - vehicle_model: unicycle - pose_error_cov: - - 0.5 - - 0.5 - - 0.5 - - 10.0 - - 10.0 - - 10.0 - vel_error_cov: - - 0.1 - - 1.0 - acc_error_cov: - - 0.1 - - 0.75 - kin_error_cov: - - 0.001 - - 0.001 - - 0.001 - - 0.001 - - 0.001 - - 0.001 - command_history_length: 100 diff --git a/config/nerian_warthog_default.yaml b/config/nerian_warthog_default.yaml index e453f256f..8dd0684b3 100644 --- a/config/nerian_warthog_default.yaml +++ b/config/nerian_warthog_default.yaml @@ -5,30 +5,35 @@ log_to_file: true log_debug: true log_enabled: - [ - "navigation", - "map_projector", - "tactic", - "tactic.module", - "tactic.module.live_mem_manager", - "tactic.module.graph_mem_manager", - "mission_planning_server", - "mission.state_machine", - "pose_graph", - "stereo.pipeline", + #"navigation", + #"map_projector", + #"tactic", + #"tactic.module", + #"tactic.module.live_mem_manager", + #"tactic.module.graph_mem_manager", + #"mission_planning_server", + #"mission.state_machine", + #"pose_graph", + #"stereo.pipeline", #"stereo.preprocessing", #"stereo.recall", #"stereo.matcher", #"stereo.keyframe_optimization", #"stereo.bundle_adjustment", - "stereo.mel_matcher", + - stereo.mel_matcher #"stereo.odometry", - "stereo.tod", + - stereo.tod #"stereo.vertex_test", #"odometry", - "tactic.pipeline", - "preprocessing" - ] + #"tactic.pipeline", + #"preprocessing" + - mpc.solver + - mpc.speed_scheduler + #- mpc.cost_function + #- mpc.cbit + #- mpc.debug + - cbit.control + # control frame from the urdf robot_frame: base_link @@ -98,11 +103,11 @@ target_width: 512 distortion: [0.0, 0.0, 0.0, 0.0, 0.0] intrinsic: - - 488.1484 + - 488.1067 - 0.0 - - 253.6597 + - 252.4117 - 0.0 - - 488.1484 + - 488.1067 - 175.043 - 0.0 - 0.0 @@ -510,51 +515,35 @@ alpha: 0.25 q_max: 2.5 frame_interval: 50 - iter_max: 10000000 eta: 1.0 rad_m_exhange: 1.00 - initial_exp_rad: 1.25 + initial_exp_rad: 0.75 extrapolation: false incremental_plotting: false plotting: true costmap: costmap_filter_value: 0.01 - costmap_history: 30 - + costmap_history: 15 # Note I dont think im actually using this one anymore, am using the one in the change detection v3 now + speed_scheduler: - planar_curv_weight: 2.50 - profile_curv_weight: 0.5 + planar_curv_weight: 0.0 + profile_curv_weight: 0.0 eop_weight: 1.0 min_vel: 0.5 - mpc: # Controller Params - extrapolate_robot_pose: true + extrapolate_robot_pose: false mpc_verbosity: false - homotopy_guided_mpc: false - horizon_steps: 20 - horizon_step_size: 0.3 forward_vel: 1.0 max_lin_vel: 1.5 - max_ang_vel: 1.25 + max_ang_vel: 0.5 + max_ang_acc: 5.0 + ang_alpha: 0.811 + lin_alpha: 0.0 robot_linear_velocity_scale: 1.0 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration robot_angular_velocity_scale: 1.0 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration vehicle_model: "unicycle" - # Cost Function Covariance Matrices - pose_error_cov: [10.0, 10.0, 100.0, 100.0, 100.0, 20.0] - vel_error_cov: [20.0, 30.0] - acc_error_cov: [20.0, 20.0] - kin_error_cov: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01] - lat_error_cov: [2.0] - - # Cost Function Weights - pose_error_weight: 1.0 - vel_error_weight: 1.5 - acc_error_weight: 1.5 - kin_error_weight: 1.0 - lat_error_weight: 0.01 - # Misc - command_history_length: 100 + command_history_length: 100 \ No newline at end of file diff --git a/config/ouster_jackal_default.yaml b/config/ouster_jackal_default.yaml new file mode 100644 index 000000000..e9b4b08dd --- /dev/null +++ b/config/ouster_jackal_default.yaml @@ -0,0 +1,396 @@ +/**: + ros__parameters: + log_to_file: true + log_debug: true + log_enabled: + - navigation + #- navigation.graph_map_server + #- navigation.command + # tactic + #- tactic + #- tactic.pipeline + - tactic.module # GOOD speed on to check + #- tactic.module.live_mem_manager + #- tactic.module.graph_mem_manager + #- mission.state_machine + - mission.server + # - navigator + # planner manager + - cbit.control + # - cbit.debug + # - cbit.path_planning + # - cbit.visualization + #- cbit.obstacle_filtering + #- grizzly_controller_tests.cbit + # mpc + # - mpc.speed_scheduler + # - mpc.debug + #- mpc.solver + # path planner + # - cbit_planner.path_planning + # pose graph + #- pose_graph + # lidar pipeline + #- lidar.pipeline + #- lidar.honeycomb_converter + # - lidar.preprocessing + #- lidar.odometry_icp + #- lidar.odometry_map_maintenance + #- lidar.vertex_test + #- lidar.localization_map_recall + #- lidar.localization_icp + #- lidar.intra_exp_merging + #- lidar.dynamic_detection + #- lidar.inter_exp_merging + #- lidar.ground_extraction + #- lidar.obstacle_detection + #- lidar.terrain_assessment + # control frame from the urdf + robot_frame: os_sensor #base_link + env_info_topic: env_info + lidar_frame: os_lidar + lidar_topic: /ouster/points + graph_projection: + origin_lat: 43.7822 + origin_lng: -79.4661 + origin_theta: 1.3 + scale: 1.0 + tactic: + enable_parallelization: true + preprocessing_skippable: false + odometry_mapping_skippable: false + localization_skippable: false + task_queue_num_threads: 1 + task_queue_size: -1 + + route_completion_translation_threshold: 0.3 # tactic.cpp route completed. # for RL this has to be smaller than the RL threshold so that we can get the localization results and stuff + route_completion_angle_threshold: 0.5 #0.26 + + chain: + min_cusp_distance: 1.5 + angle_weight: 0.25 + search_depth: 5 + search_back_depth: 5 + distance_warning: 5.0 + alpha: 0.25 # the one in genreate_pq is not used. + save_odometry_result: true + save_localization_result: true + visualize: true + rviz_loc_path_offset: + - 0.0 + - 0.0 + - 0.0 + pipeline: + type: lidar + preprocessing: + - conversion + - filtering + odometry: + - icp + - mapping + - vertex_test + # - intra_exp_merging + # - dynamic_detection + # - inter_exp_merging + - memory + localization: + - recall + - icp + - safe_corridor + # - change_detection_sync + - memory + submap_translation_threshold: 0.275 + submap_rotation_threshold: 15.0 + preprocessing: + conversion: + type: lidar.ouster_converter + visualize: false # JH: could add additional filters here? + filter_warthog: true # look at code: need to be true to filter + filter_z_min: -0.5 + filter_z_max: 2.5 + filter_radius: 0.5 # was 0.2 + + filtering: + type: lidar.preprocessing #lidar.preprocessing_v2 + num_threads: 8 + crop_range: 20.0 + + frame_voxel_size: 0.3 #0.3 # grid subsampling voxel size + + vertical_angle_res: 0.0122718463 # (pi/2)/128 for OS0 128 + polar_r_scale: 2.0 # polar_r_scale x vertical_angle_res = nearest neighbor search radius for normal computation + r_scale: 2.5 # scale down point range by this value after taking log, whatever works + h_scale: 0.5 # scale down yaw(phi) by this value so that vertical resolution ~= horizontal resolution, horizontal resolution is 360/1024 ~= 0.3516 degree, so 0.3516 / (90/128) = 0.5 + + num_sample1: 2500 #12500 # max number of sample after filtering based on planarity + min_norm_score1: 0.95 # min planarity score + + num_sample2: 2500 #12500 # max number of sample after filtering based on planarity + min_norm_score2: 0.2 # 0.2 is when the incident angle 5/12 * pi + min_normal_estimate_dist: 1.0 # minimum distance to estimate normal in meters + max_normal_estimate_angle: 0.44 # must <1/2, this value will be timed by M_PI + + cluster_num_sample: 2500 #12500 # maxnumber of sample after removing isolated points + + visualize: false + odometry: + icp: + type: lidar.odometry_icp + + # continuous time estimation + use_trajectory_estimation: false + traj_num_extra_states: 0 + traj_lock_prev_pose: false + traj_lock_prev_vel: false + traj_qc_diag: + - 1.0 + - 0.1 + - 0.1 + - 0.1 + - 0.1 + - 1.0 + num_threads: 8 + first_num_steps: 2 + initial_max_iter: 4 + initial_max_pairing_dist: 1.5 + initial_max_planar_dist: 1.0 + refined_max_iter: 50 + refined_max_pairing_dist: 1.0 + refined_max_planar_dist: 0.3 + averaging_num_steps: 2 + verbose: false + max_iterations: 1 + min_matched_ratio: 0.5 + visualize: false + mapping: + type: lidar.odometry_map_maintenance_v2 + + map_voxel_size: 0.25 + + crop_range_front: 40.0 + back_over_front_ratio: 0.5 + point_life_time: 20.0 + visualize: false + vertex_test: + type: lidar.vertex_test + + max_translation: 0.3 # JH: Alec mentioned something about if this is too small, weird things will happen + max_rotation: 15.0 + intra_exp_merging: + type: lidar.intra_exp_merging_v2 + depth: 6.0 + + map_voxel_size: 0.3 + + crop_range_front: 40.0 + back_over_front_ratio: 0.5 + visualize: false + dynamic_detection: + type: lidar.dynamic_detection + depth: 12.0 + + horizontal_resolution: 0.00613592315 # 2pi / 1024 + vertical_resolution: 0.0122718463 # pi/2 / 128 + max_num_observations: 10000 + min_num_observations: 4 + dynamic_threshold: 0.3 + visualize: false + inter_exp_merging: + type: "lidar.inter_exp_merging_v2" + + map_voxel_size: 0.25 + max_num_experiences: 128 + distance_threshold: 0.6 + planar_threshold: 0.2 + normal_threshold: 0.8 + dynamic_obs_threshold: 1 + visualize: false + memory: + type: live_mem_manager + window_size: 5 + localization: + recall: + type: lidar.localization_map_recall + map_version: pointmap + visualize: false + icp: + type: lidar.localization_icp + use_pose_prior: true + num_threads: 8 + first_num_steps: 2 + initial_max_iter: 4 + initial_max_pairing_dist: 1.5 + initial_max_planar_dist: 1.0 + refined_max_iter: 50 + refined_max_pairing_dist: 1.0 + refined_max_planar_dist: 0.3 + averaging_num_steps: 2 + verbose: false + max_iterations: 1 + min_matched_ratio: 0.3 + safe_corridor: + type: lidar.safe_corridor + lookahead_distance: 5.0 + corridor_width: 0.5 + influence_distance: 1.0 + resolution: 0.25 + size_x: 16.0 + size_y: 8.0 + visualize: false + change_detection_sync: + type: lidar.change_detection_v3 + detection_range: 8.0 + search_radius: 0.25 + + negprob_threshold: 0.25 # was 0.015 # -1.86 without prior, 0.015 with prior + use_prior: true + alpha0: 3.0 + beta0: 0.03 + use_support_filtering: true + support_radius: 0.25 + support_variance: 0.05 + support_threshold: 2.5 + + influence_distance: 0.5 # Note that the total distance where grid cells have values > 0 is min dist + influence dist, not influence dist! + minimum_distance: 0.8 + + # cost map + costmap_history_size: 15 # Keep between 3 and 30, used for temporal filtering + resolution: 0.25 + size_x: 16.0 + size_y: 8.0 + visualize: false + change_detection: + type: lidar.change_detection_v2 + detection_range: 8 + search_radius: 0.25 + negprob_threshold: 0.1 + use_prior: true + alpha0: 3.0 + beta0: 0.03 + use_support_filtering: true + support_radius: 0.25 + support_variance: 0.05 + support_threshold: 2.5 + resolution: 0.5 + size_x: 16.0 + size_y: 8.0 + run_online: false + run_async: true + visualize: false + save_module_result: false + memory: + type: graph_mem_manager + vertex_life_span: 5 + window_size: 3 + obstacle_detection: + type: lidar.obstacle_detection + z_min: 0.5 + z_max: 2.0 + resolution: 0.6 + size_x: 40.0 + size_y: 20.0 + run_async: true + visualize: false + ground_extraction: + type: lidar.ground_extraction + z_offset: 0.2 + alpha: 0.035 + tolerance: 0.25 + Tm: 0.3 + Tm_small: 0.1 + Tb: 0.5 + Trmse: 0.1 + Tdprev: 1.0 + rmin: 2.0 + num_bins_small: 30.0 + bin_size_small: 0.5 + num_bins_large: 10.0 + bin_size_large: 1.0 + resolution: 0.6 + size_x: 40.0 + size_y: 20.0 + run_async: true + visualize: false + terrain_assessment: + type: lidar.terrain_assessment + lookahead_distance: 15.0 + corridor_width: 1.0 + search_radius: 1.0 + resolution: 0.5 + size_x: 40.0 + size_y: 20.0 + run_online: false + run_async: true + visualize: false + + path_planning: + type: cbit + control_period: 100 # ms + cbit: + obstacle_avoidance: false + obs_padding: 0.0 + curv_to_euclid_discretization: 10 + sliding_window_width: 12.0 + sliding_window_freespace_padding: 0.5 + corridor_resolution: 0.2 + state_update_freq: 2.0 + update_state: true + rand_seed: 1 + + # Planner Tuning Params + initial_samples: 400 + batch_samples: 200 + pre_seed_resolution: 0.5 + alpha: 0.25 + q_max: 2.5 + frame_interval: 50 + iter_max: 10000000 + eta: 1.0 + rad_m_exhange: 1.00 + initial_exp_rad: 1.00 + extrapolation: false + incremental_plotting: false + plotting: true + costmap: + costmap_filter_value: 0.01 + costmap_history: 15 + + speed_scheduler: + planar_curv_weight: 2.50 + profile_curv_weight: 0.5 + eop_weight: 1.0 + min_vel: 1.2 #0.5 + + mpc: + # Controller Params + extrapolate_robot_pose: true + mpc_verbosity: false + homotopy_guided_mpc: false + horizon_steps: 20 + horizon_step_size: 0.25 # JH: I think this is planning time step which I need to be 0.25s for DRL model. + forward_vel: 0.75 + max_lin_vel: 1.1 #1.0 + max_ang_vel: 1.0 + robot_linear_velocity_scale: 1.0 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration + robot_angular_velocity_scale: 1.0 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration + + vehicle_model: "unicycle" + + # JH: I think Alec changed this to just rollout which removes kinematic constraints as part of optimization + # Cost Function Covariance Matrices + pose_error_cov: [10.0, 10.0, 100.0, 100.0, 100.0, 20.0] + vel_error_cov: [20.0, 30.0] + acc_error_cov: [20.0, 20.0] + kin_error_cov: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01] + lat_error_cov: [2.0] + + # Cost Function Weights + pose_error_weight: 1.0 + vel_error_weight: 1.5 + acc_error_weight: 1.5 + kin_error_weight: 1.0 + lat_error_weight: 0.01 + + # Misc + command_history_length: 100 diff --git a/config/ouster_warthog_default.yaml b/config/ouster_warthog_default.yaml index 978653ec2..443a180d3 100644 --- a/config/ouster_warthog_default.yaml +++ b/config/ouster_warthog_default.yaml @@ -10,18 +10,24 @@ # tactic #- tactic - - tactic.pipeline - - tactic.eop + #- tactic.pipeline + #- tactic.eop #- tactic.module #- tactic.module.live_mem_manager #- tactic.module.graph_mem_manager - # path planner + # path planner - path_planning - path_planning.cbit - #- path_planning.cbit_planner - - mpc.cbit - - mpc_debug + - path_planning.cbit_planner + - cbit.path_planning + #- cbit_planner.path_planning + #- cbit.debug + - mpc.solver + - mpc.speed_scheduler + #- mpc.cost_function + #- mpc.cbit + #- mpc.debug - cbit.control #- obstacle detection.cbit #- grizzly_controller_tests.cbit @@ -29,13 +35,13 @@ # mission planner #- mission.server - #- mission.state_machine + - mission.state_machine # pose graph #- pose_graph # lidar pipeline - - lidar.pipeline + #- lidar.pipeline #- lidar.preprocessing #- lidar.ouster_converter #- lidar.odometry_icp @@ -64,18 +70,20 @@ enable_parallelization: true preprocessing_skippable: false odometry_mapping_skippable: false - localization_skippable: false + localization_skippable: true task_queue_num_threads: 1 task_queue_size: -1 - route_completion_translation_threshold: 0.1 + route_completion_translation_threshold: 0.2 + route_completion_angle_threshold: 0.1 chain: - min_cusp_distance: 1.5 - angle_weight: 7.0 + min_cusp_distance: 0.5 + angle_weight: 1.0 search_depth: 5 search_back_depth: 10 distance_warning: 5.0 + alpha: 1.0 save_odometry_result: true save_localization_result: true visualize: true @@ -92,9 +100,9 @@ - icp - mapping - vertex_test - - intra_exp_merging - - dynamic_detection - - inter_exp_merging + #- intra_exp_merging + #- dynamic_detection + #- inter_exp_merging - memory localization: - recall @@ -174,7 +182,7 @@ crop_range_front: 40.0 back_over_front_ratio: 0.5 point_life_time: 20.0 - visualize: true + visualize: false vertex_test: type: lidar.vertex_test @@ -310,7 +318,7 @@ visualize: true path_planning: - type: cbit # cbit for obstacle free path tracker, cbit.lidar for obstacle avoidance version + type: cbit control_period: 100 # ms cbit: obstacle_avoidance: false @@ -330,7 +338,6 @@ alpha: 0.25 q_max: 2.5 frame_interval: 50 - iter_max: 10000000 eta: 1.0 rad_m_exhange: 1.00 initial_exp_rad: 0.75 @@ -341,34 +348,25 @@ costmap_filter_value: 0.01 costmap_history: 15 # Note I dont think im actually using this one anymore, am using the one in the change detection v3 now + speed_scheduler: + planar_curv_weight: 0.0 + profile_curv_weight: 0.0 + eop_weight: 1.0 + min_vel: 0.5 mpc: # Controller Params - extrapolate_robot_pose: true + extrapolate_robot_pose: false mpc_verbosity: false - homotopy_guided_mpc: false - horizon_steps: 20 - horizon_step_size: 0.3 forward_vel: 1.0 max_lin_vel: 1.5 - max_ang_vel: 1.25 + max_ang_vel: 0.5 + max_ang_acc: 5.0 + ang_alpha: 0.811 + lin_alpha: 0.0 robot_linear_velocity_scale: 1.0 # Used to scale the output twist linear velocity messages by some constant factor to compensate internal low level control miscalibration robot_angular_velocity_scale: 1.0 # Used to scale the output twist angular velocity messages by some constant factor to compensate internal low level control miscalibration vehicle_model: "unicycle" - # Cost Function Covariance Matrices - pose_error_cov: [10.0, 10.0, 100.0, 100.0, 100.0, 5.0] - vel_error_cov: [20.0, 30.0] - acc_error_cov: [20.0, 20.0] - kin_error_cov: [0.01, 0.01, 0.01, 0.01, 0.01, 0.01] - lat_error_cov: [2.0] - - # Cost Function Weights - pose_error_weight: 1.0 - vel_error_weight: 1.5 - acc_error_weight: 1.5 - kin_error_weight: 1.0 - lat_error_weight: 0.01 - # Misc command_history_length: 100 diff --git a/launch/offline_ouster_jackal.launch.yaml b/launch/offline_ouster_jackal.launch.yaml new file mode 100644 index 000000000..1f899693a --- /dev/null +++ b/launch/offline_ouster_jackal.launch.yaml @@ -0,0 +1,46 @@ +## Offline Lidar VTR3 (using datasets) +session_name: vtr_offline_ouster_jackal + +environment: + CYCLONEDDS_URI: ${VTRSRC}/config/cyclonedds.default.xml # custom dds configs + # ROS_DOMAIN_ID: "1" # set this to a unique number when multiple ROS2 dependent system running on the same network + +start_directory: ${VTRTEMP} + +suppress_history: false + +windows: + - window_name: vtr_navigation_system + layout: main-vertical + shell_command_before: + - source /opt/ros/humble/setup.bash + && source ${VTRSRC}/main/install/setup.bash + - ros2 run vtr_gui setup_server --ros-args -r __ns:=/vtr + + panes: + - > + ros2 launch vtr_navigation vtr.launch.py + base_params:=ouster_jackal_default.yaml + data_dir:=${VTRTEMP}/lidar + model_dir:=${VTRMODELS} + start_new_graph:=true + use_sim_time:=true + planner:="cbit" + + - window_name: vtr_gui + layout: main-vertical + shell_command_before: + - source ${VTRSRC}/main/install/setup.bash + panes: + - ros2 run vtr_gui socket_client --ros-args -r __ns:=/vtr + - ros2 run vtr_gui socket_server --ros-args -r __ns:=/vtr + - ros2 run vtr_gui web_server --ros-args -r __ns:=/vtr + # - firefox --new-window "localhost:5200" # the webpage has to wait for everything above + + - window_name: rviz2 + layout: main-vertical + shell_command_before: + - source /opt/ros/humble/setup.bash + panes: + - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/ouster.rviz + # - ros2 run rqt_reconfigure rqt_reconfigure diff --git a/launch/online_bumblebee_learned_grizzly.launch.yaml b/launch/online_bumblebee_learned_grizzly.launch.yaml index 7dbbb5326..a4d10c163 100644 --- a/launch/online_bumblebee_learned_grizzly.launch.yaml +++ b/launch/online_bumblebee_learned_grizzly.launch.yaml @@ -1,9 +1,8 @@ -## Online Lidar VTR3 -session_name: vtr_offline_bumblebee_grizzly +## Online Camera VTR3 +session_name: vtr_nline_bumblebee_grizzly_learned environment: - CYCLONEDDS_URI: ${VTRSRC}/config/cyclonedds.default.xml # custom dds configs - # ROS_DOMAIN_ID: "1" # set this to a unique number when multiple ROS2 dependent system running on the same network + ROS_DOMAIN_ID: "13" # set this to a unique number when multiple ROS2 dependent system running on the same network start_directory: ${VTRTEMP} @@ -18,7 +17,8 @@ windows: panes: - > ros2 launch vtr_navigation vtr.launch.py - base_params:=bumblebee_grizzly_learned_features.yaml + base_params:=bumblebee_grizzly_default.yaml + override_params:=bumblebee_grizzly_learned_features.yaml start_new_graph:=false use_sim_time:=false diff --git a/launch/online_honeycomb_grizzly.launch.yaml b/launch/online_honeycomb_grizzly.launch.yaml index 55634b472..7c9ede3bd 100644 --- a/launch/online_honeycomb_grizzly.launch.yaml +++ b/launch/online_honeycomb_grizzly.launch.yaml @@ -2,8 +2,10 @@ session_name: vtr_online_honeycomb_grizzly environment: - CYCLONEDDS_URI: ${VTRSRC}/config/cyclonedds.default.xml # custom dds configs - # ROS_DOMAIN_ID: "1" # set this to a unique number when multiple ROS2 dependent system running on the same network + RMW_IMPLEMENTATION: rmw_fastrtps_cpp + FASTRTPS_DEFAULT_PROFILES_FILE: ${VTRSRC}/config/vtr_rtps.xml + ROS_DOMAIN_ID: "13" # set this to a unique number when multiple ROS2 processes running on the same network + start_directory: ${VTRTEMP} suppress_history: false diff --git a/launch/online_ouster_jackal.launch.yaml b/launch/online_ouster_jackal.launch.yaml new file mode 100644 index 000000000..4e4739bbb --- /dev/null +++ b/launch/online_ouster_jackal.launch.yaml @@ -0,0 +1,45 @@ +session_name: vtr_online_ouster_jackal + +environment: + RMW_IMPLEMENTATION: rmw_cyclonedds_cpp # CYCLONEDDS_URI: ${VTRSRC}/config/cyclonedds.default.xml # custom dds configs + ROS_DOMAIN_ID: "0" # set this to a unique number when multiple ROS2 dependent system running on the same network + +start_directory: ${VTRTEMP} + +suppress_history: false + +windows: + - window_name: vtr_navigation_system + layout: main-vertical + shell_command_before: + - source /opt/ros/humble/setup.bash + && source ${VTRSRC}/main/install/setup.bash + - ros2 run vtr_gui setup_server --ros-args -r __ns:=/vtr + panes: + - > + ros2 launch vtr_navigation vtr.launch.py + base_params:=ouster_jackal_default.yaml + start_new_graph:=false + use_sim_time:=false + planner:="cbit" + +# - htop # monitor hardware usage + + - window_name: vtr_gui + layout: main-vertical + shell_command_before: + - source /opt/ros/humble/setup.bash + && source ${VTRSRC}/main/install/setup.bash + panes: + - ros2 run vtr_gui socket_client --ros-args -r __ns:=/vtr + - ros2 run vtr_gui socket_server --ros-args -r __ns:=/vtr + - ros2 run vtr_gui web_server --ros-args -r __ns:=/vtr + # - firefox --new-window "localhost:5200" # the webpage has to wait for everything above + + - window_name: rviz2 + layout: main-vertical + shell_command_before: + - source /opt/ros/humble/setup.bash + panes: + - ros2 run rviz2 rviz2 -d ${VTRSRC}/rviz/ouster.rviz + # - ros2 run rqt_reconfigure rqt_reconfigure diff --git a/main/src/deps/steam b/main/src/deps/steam index e49af4800..f5017d9f3 160000 --- a/main/src/deps/steam +++ b/main/src/deps/steam @@ -1 +1 @@ -Subproject commit e49af4800029c0d77efdfed01d6ff75aeb8fce96 +Subproject commit f5017d9f3e019524ee3bbaec87af3c53181d2037 diff --git a/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/graph/GraphMap.js b/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/graph/GraphMap.js index 6d5e26505..5342515d9 100644 --- a/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/graph/GraphMap.js +++ b/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/graph/GraphMap.js @@ -46,7 +46,7 @@ import AddIcon from "@mui/icons-material/Add"; import DeleteIcon from "@mui/icons-material/Delete"; /// pose graph constants -const ROUTE_TYPE_COLOR = ["#f44336", "#ff9800", "#ffeb3b", "#4caf50", "#00bcd4", "#2196f3", "#9c27b0"]; +const ROUTE_TYPE_COLOR = ["#ff0000", "#fd4a18", "#ffa500", "#fecd29", "#ffff00", "#6aaf1e", "#008000"]; const GRAPH_OPACITY = 0.9; const GRAPH_WEIGHT = 7; @@ -503,11 +503,12 @@ class GraphMap extends React.Component { route2Polyline(route) { // fixed_routes format: [{type: 0, ids: [id, ...]}, ...] let color = ROUTE_TYPE_COLOR[route.type % ROUTE_TYPE_COLOR.length]; + let width = route.type + 2 let latlngs = route.ids.map((id) => { let v = this.id2vertex.get(id); return [v.lat, v.lng]; }); - let polyline = L.polyline(latlngs, { color: color, opacity: GRAPH_OPACITY, weight: GRAPH_WEIGHT, pane: "graph" }); + let polyline = L.polyline(latlngs, { color: color, opacity: GRAPH_OPACITY, weight: width, pane: "graph" }); polyline.addTo(this.map); return polyline; } diff --git a/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/tools/AnnotateSlider.js b/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/tools/AnnotateSlider.js index 1b21bbdb5..3218c3980 100644 --- a/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/tools/AnnotateSlider.js +++ b/main/src/vtr_gui/vtr_gui/vtr-gui/src/components/tools/AnnotateSlider.js @@ -20,13 +20,13 @@ import React from "react"; import { Box, Slider, Paper } from "@mui/material"; const marks = [ - { value: 0, label: "Type 1" }, - { value: 1, label: "Type 2" }, - { value: 2, label: "Type 3" }, - { value: 3, label: "Type 4" }, - { value: 4, label: "Type 5" }, - { value: 5, label: "Type 6" }, - { value: 6, label: "Type 7" }, + { value: 0, label: "0.2 m" }, + { value: 1, label: "0.5 m" }, + { value: 2, label: "1.0 m" }, + { value: 3, label: "1.5 m" }, + { value: 4, label: "2.0 m" }, + { value: 5, label: "2.5 m" }, + { value: 6, label: "10.0 m" }, ]; function valueLabelFormat(value) { @@ -51,7 +51,7 @@ class AnnotateSlider extends React.Component { left: "50%", transform: "translate(-50%, -50%)", opacity: 0.5, - background: "linear-gradient(90deg, red, orange, yellow, green, blue, indigo, violet)", + background: "linear-gradient(90deg, red, orange, yellow, green)", width: 550, height: 50, zIndex: 1000, @@ -70,7 +70,7 @@ class AnnotateSlider extends React.Component { > value(); *qdata.w_m_r_in_r_odo = (T_r_m_query * T_r_m_prev.inverse()).vec() / (query_time - prev_time).seconds(); } + *qdata.w_v_r_in_r_odo = *qdata.w_m_r_in_r_odo; + *qdata.T_r_v_odo = T_r_m_icp * sliding_map_odo.T_vertex_this().inverse(); *qdata.T_r_m_odo = T_r_m_eval->value(); *qdata.timestamp_odo = query_stamp; -//#if 1 -// CLOG(WARNING, "lidar.odometry_icp") << "T_m_r is: " << qdata.T_r_m_odo->inverse().vec().transpose(); -// CLOG(WARNING, "lidar.odometry_icp") << "w_m_r_in_r is: " << qdata.w_m_r_in_r_odo->transpose(); -//#endif - // - /// \todo double check validity when no vertex has been created - *qdata.T_r_v_odo = T_r_m_icp * sliding_map_odo.T_vertex_this().inverse(); - - // Temporary modification by Jordy to test calibration of hte grizzly controller - //CLOG(DEBUG, "grizzly_controller_tests.cbit") << "The Odometry Velocity is: " << *qdata.w_m_r_in_r_odo * -1; - CLOG(DEBUG, "grizzly_controller_tests.cbit") << "Odom Linear Velocity: " << (*qdata.w_m_r_in_r_odo * -1)(0); - CLOG(DEBUG, "grizzly_controller_tests.cbit") << "Odom Angular Velocity: " << (*qdata.w_m_r_in_r_odo * -1)(5); - // End of Jordy's Modifications - - /// \todo double check that we can indeed treat m same as v for velocity - if (config_->use_trajectory_estimation) - *qdata.w_v_r_in_r_odo = *qdata.w_m_r_in_r_odo; - // + *qdata.odo_success = true; } else { CLOG(WARNING, "lidar.odometry_icp") diff --git a/main/src/vtr_lidar/src/modules/preprocessing/preprocessing_module.cpp b/main/src/vtr_lidar/src/modules/preprocessing/preprocessing_module.cpp index 87e6840b9..fe145138a 100644 --- a/main/src/vtr_lidar/src/modules/preprocessing/preprocessing_module.cpp +++ b/main/src/vtr_lidar/src/modules/preprocessing/preprocessing_module.cpp @@ -121,8 +121,6 @@ void PreprocessingModule::run_(QueryCache &qdata0, OutputCache &, auto filtered_point_cloud = std::make_shared>(*point_cloud); - auto nn_downsampled_cloud = - std::make_shared>(*point_cloud); /// Range cropping { @@ -143,7 +141,14 @@ void PreprocessingModule::run_(QueryCache &qdata0, OutputCache &, // Get subsampling of the frame in carthesian coordinates voxelDownsample(*filtered_point_cloud, config_->frame_voxel_size); - voxelDownsample(*nn_downsampled_cloud, config_->nn_voxel_size); + + //Secondary input for change detection + if (config_->nn_voxel_size > 0) { + auto nn_downsampled_cloud = + std::make_shared>(*point_cloud); + voxelDownsample(*nn_downsampled_cloud, config_->nn_voxel_size); + qdata.nn_point_cloud = nn_downsampled_cloud; + } CLOG(DEBUG, "lidar.preprocessing") << "grid subsampled point cloud size: " << filtered_point_cloud->size(); @@ -262,7 +267,6 @@ void PreprocessingModule::run_(QueryCache &qdata0, OutputCache &, /// Output qdata.preprocessed_point_cloud = filtered_point_cloud; - qdata.nn_point_cloud = nn_downsampled_cloud; } } // namespace lidar diff --git a/main/src/vtr_navigation/launch/vtr.launch.py b/main/src/vtr_navigation/launch/vtr.launch.py index dbc00a8bb..5770f2e63 100644 --- a/main/src/vtr_navigation/launch/vtr.launch.py +++ b/main/src/vtr_navigation/launch/vtr.launch.py @@ -20,6 +20,7 @@ def generate_launch_description(): "executable": 'vtr_navigation', "output": 'screen', #"prefix": 'xterm -e gdb -ex run --args', + #"prefix": 'valgrind --tool=callgrind', } return LaunchDescription([ diff --git a/main/src/vtr_navigation/src/graph_map_server.cpp b/main/src/vtr_navigation/src/graph_map_server.cpp index b907ed94a..8ef88fd76 100644 --- a/main/src/vtr_navigation/src/graph_map_server.cpp +++ b/main/src/vtr_navigation/src/graph_map_server.cpp @@ -389,9 +389,13 @@ void GraphMapServer::optimizeGraph(const tactic::GraphBase::Ptr& priv_graph) { std::make_shared>(cov); optimizer.addFactor(relaxation_factor); - // udpates the tf map - using SolverType = steam::DoglegGaussNewtonSolver; - optimizer.optimize(); + try { + // udpates the tf map + using SolverType = steam::DoglegGaussNewtonSolver; + optimizer.optimize(); + } catch (steam::unsuccessful_step &e) { + CLOG(WARNING, "navigation.graph_map_server") << "Pose graph relaxation for visualization failed. Falling back back to initial config."; + } // update the graph state vertices and idx map auto& vertices = graph_state_.vertices; diff --git a/main/src/vtr_navigation/src/navigator.cpp b/main/src/vtr_navigation/src/navigator.cpp index 40fdb4e23..e30767921 100644 --- a/main/src/vtr_navigation/src/navigator.cpp +++ b/main/src/vtr_navigation/src/navigator.cpp @@ -263,7 +263,7 @@ void Navigator::lidarCallback( /// Discard old frames if our queue is too big if (queue_.size() > max_queue_size_) { CLOG(WARNING, "navigation") - << "Dropping old pointcloud message because the queue is full."; + << "Dropping pointcloud message " << *queue_.front()->stamp << " because the queue is full."; queue_.pop(); } @@ -297,7 +297,7 @@ void Navigator::cameraCallback( /// Discard old frames if our queue is too big if (queue_.size() > max_queue_size_) { CLOG(WARNING, "navigation") - << "Dropping old images because the queue is full."; + << "Dropping old image " << *queue_.front()->stamp << " because the queue is full."; queue_.pop(); } diff --git a/main/src/vtr_path_planning/CMakeLists.txt b/main/src/vtr_path_planning/CMakeLists.txt index 9359a2f5e..819b58ba9 100644 --- a/main/src/vtr_path_planning/CMakeLists.txt +++ b/main/src/vtr_path_planning/CMakeLists.txt @@ -4,6 +4,7 @@ project(vtr_path_planning) ## Common setup for vtr packages include("${CMAKE_CURRENT_LIST_DIR}/../vtr_common/vtr_include.cmake") +# add_compile_options(-g -Og) # Find dependencies find_package(ament_cmake REQUIRED) @@ -15,17 +16,19 @@ find_package(tf2_ros REQUIRED) find_package(tf2_eigen REQUIRED) find_package(lgmath REQUIRED) -find_package(steam REQUIRED) +find_package(casadi REQUIRED) find_package(vtr_common REQUIRED) find_package(vtr_logging REQUIRED) find_package(vtr_tactic REQUIRED) +find_package(vtr_path_planning_msgs REQUIRED) # # find python libraries find_package(Python3 COMPONENTS Interpreter Development NumPy REQUIRED) find_package(PythonLibs 3.0 REQUIRED) + # C++ Libraries include_directories( PUBLIC @@ -40,17 +43,30 @@ add_library(${PROJECT_NAME}_base ${SRC}) ament_target_dependencies(${PROJECT_NAME}_base geometry_msgs vtr_common vtr_logging vtr_tactic + vtr_path_planning_msgs ) +add_custom_command(OUTPUT "include/vtr_path_planning/mpc/mpc_solver.hpp" + COMMAND "${PYTHON_EXECUTABLE}" "${CMAKE_CURRENT_SOURCE_DIR}/scripts/export_unicycle.py" + DEPENDS "scripts/unicycle_solver.py") + +add_custom_target(generate_header ALL +DEPENDS "include/vtr_path_planning/mpc/mpc_solver.hpp") + file(GLOB_RECURSE SRC src/mpc/*.cpp) add_library(${PROJECT_NAME}_mpc ${SRC}) target_link_libraries(${PROJECT_NAME}_mpc ${PROJECT_NAME}_base) +target_link_libraries(${PROJECT_NAME}_mpc casadi) ament_target_dependencies(${PROJECT_NAME}_mpc rclcpp tf2 tf2_ros tf2_eigen - lgmath steam Python3 + lgmath Python3 casadi + vtr_path_planning_msgs ) +add_library(solve_unicycle_mpc SHARED src/mpc/mpc_solver.cpp) +add_dependencies(solve_unicycle_mpc generate_header) +target_link_libraries(solve_unicycle_mpc casadi) find_package(matplotlib_cpp REQUIRED) @@ -64,13 +80,15 @@ target_link_libraries(${PROJECT_NAME}_cbit ${PYTHON_LIBRARIES}) ament_target_dependencies(${PROJECT_NAME}_cbit rclcpp tf2 tf2_ros tf2_eigen Python3 - lgmath steam PythonLibs matplotlib_cpp + lgmath PythonLibs matplotlib_cpp + vtr_path_planning_msgs ) ament_export_targets(export_${PROJECT_NAME} HAS_LIBRARY_TARGET) ament_export_dependencies( rclcpp tf2 tf2_ros tf2_eigen geometry_msgs matplotlib_cpp - vtr_common vtr_logging vtr_tactic + vtr_common vtr_logging vtr_tactic casadi + vtr_path_planning_msgs ) install( @@ -83,6 +101,7 @@ install( ${PROJECT_NAME}_base ${PROJECT_NAME}_mpc ${PROJECT_NAME}_cbit + solve_unicycle_mpc EXPORT export_${PROJECT_NAME} LIBRARY DESTINATION lib ARCHIVE DESTINATION lib @@ -94,6 +113,9 @@ install( if(BUILD_TESTING) find_package(ament_cmake_gtest REQUIRED) + ament_add_gtest(test_unicycle test/closed_loop/unicycle_tests.cpp) + target_link_libraries(test_unicycle ${PROJECT_NAME}_cbit) + # Linting find_package(ament_lint_auto REQUIRED) ament_lint_auto_find_test_dependencies() # Lint based on linter test_depend in package.xml diff --git a/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit.hpp b/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit.hpp index 6f1c03254..be1acedde 100644 --- a/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit.hpp +++ b/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit.hpp @@ -44,6 +44,7 @@ #include "vtr_path_planning/cbit/cbit_costmap.hpp" #include "vtr_path_planning/cbit/visualization_utils.hpp" #include "vtr_path_planning/mpc/speed_scheduler.hpp" +#include "vtr_path_planning/mpc/mpc_path_planner.hpp" #include "steam.hpp" @@ -87,8 +88,6 @@ class CBIT : public BasePathPlanner { double pre_seed_resolution = 0.5; double alpha = 0.5; double q_max = 2.5; - int frame_interval = 50; - int iter_max = 10000000; double eta = 1.1; double rad_m_exhange = 1.00; double initial_exp_rad = 1.00; @@ -108,31 +107,16 @@ class CBIT : public BasePathPlanner { bool obstacle_avoidance = false; bool extrapolate_robot_pose = true; bool mpc_verbosity = false; - bool homotopy_guided_mpc = false; - int horizon_steps = 10; - double horizon_step_size = 0.5; double forward_vel = 0.75; double max_lin_vel = 1.25; double max_ang_vel = 0.75; + double max_lin_acc = 10.0; + double max_ang_acc = 10.0; double robot_linear_velocity_scale = 1.0; double robot_angular_velocity_scale = 1.0; // Add unicycle model param - // Covariance tuning weights - Eigen::Matrix pose_error_cov = Eigen::Matrix::Zero(); - Eigen::Matrix vel_error_cov = Eigen::Matrix::Zero(); - Eigen::Matrix acc_error_cov = Eigen::Matrix::Zero(); - Eigen::Matrix kin_error_cov = Eigen::Matrix::Zero(); - Eigen::Matrix lat_error_cov = Eigen::Matrix::Zero(); - - // MPC weight params: - double pose_error_weight = 1.0; - double vel_error_weight = 1.0; - double acc_error_weight = 1.0; - double kin_error_weight = 1.0; - double lat_error_weight = 0.01; - // Misc int command_history_length = 100; @@ -143,10 +127,6 @@ class CBIT : public BasePathPlanner { static Ptr fromROS(const rclcpp::Node::SharedPtr& node, const std::string& prefix = "path_planning"); - // Subscription for parameter change - rclcpp::AsyncParametersClient::SharedPtr ros_parameters_client; - rclcpp::Subscription::SharedPtr - ros_parameter_event_sub; }; CBIT(const Config::ConstPtr& config, @@ -154,30 +134,21 @@ class CBIT : public BasePathPlanner { const Callback::Ptr& callback); ~CBIT() override; + void setRunning(const bool running) override; + protected: void initializeRoute(RobotState& robot_state); Command computeCommand(RobotState& robot_state) override; - - protected: - struct ChainInfo { - tactic::Timestamp stamp; - Eigen::Matrix w_p_r_in_r; - // planning frame is the current localization frame - tactic::EdgeTransform T_p_r; // T_planning_robot - tactic::EdgeTransform T_w_p; // T_world_planning - tactic::EdgeTransform T_w_v_odo; // T_planning_robot - tactic::EdgeTransform T_r_v_odo; // T_world_planning - unsigned curr_sid; - }; - /** \brief Retrieve information for planning from localization chain */ - ChainInfo getChainInfo(RobotState& robot_state); - - //private: // I think we need to make all of these protected so the derived lidarcbit class can access them + private: const Config::ConstPtr config_; CBITConfig cbit_config; VTR_REGISTER_PATH_PLANNER_DEC_TYPE(CBIT); + std::shared_ptr planner_ptr_; + + CasadiUnicycleMPC solver_; + // Pointers to the output path std::vector cbit_path; std::shared_ptr> cbit_path_ptr; @@ -198,23 +169,25 @@ class CBIT : public BasePathPlanner { std::shared_ptr visualization_ptr; unsigned int prev_costmap_sid = 0; - tactic::Timestamp prev_stamp; + tactic::Timestamp prev_cost_stamp_; // Store the previously applied velocity and a sliding window history of MPC results - Eigen::Matrix applied_vel; - std::vector> vel_history; + Eigen::Vector2d applied_vel_; + std::vector vel_history; + tactic::Timestamp prev_vel_stamp_; //create vector to store the robots path for visualization purposes std::vector robot_poses; // Create costmap pointer object - std::shared_ptr costmap_ptr = std::make_shared (); + std::shared_ptr costmap_ptr = std::make_shared(); private: void process_cbit(); std::thread process_thread_cbit_; void stop_cbit(); + Command computeCommand_(RobotState& robot_state); private: /** \brief shared memory that stores the current robot state */ @@ -223,5 +196,8 @@ class CBIT : public BasePathPlanner { }; +// Helper function for post-processing and saturating the velocity command +Eigen::Vector2d saturateVel(const Eigen::Vector2d& applied_vel, double v_lim, double w_lim); + } // namespace path_planning } // namespace vtr \ No newline at end of file diff --git a/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit_config.hpp b/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit_config.hpp index b0f15803e..b72d5e4e7 100644 --- a/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit_config.hpp +++ b/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit_config.hpp @@ -46,8 +46,6 @@ class CBITConfig { double pre_seed_resolution = 0.5; double alpha = 0.5; double q_max = 2.5; - int frame_interval = 50; - int iter_max = 2000; double eta = 1.1; double rad_m_exhange = 1.00; double initial_exp_rad = 1.00; diff --git a/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit_costmap.hpp b/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit_costmap.hpp index dac9530ae..7602ce58f 100644 --- a/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit_costmap.hpp +++ b/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit_costmap.hpp @@ -38,7 +38,7 @@ class CBITCostmap { std::vector, float>> obs_map_vect; //std::unique_ptr T_r_costmap_ptr; - // We also want to store a pointer to the current transform from the robot to the costmap + // We also want to store the current transform from the robot to the costmap vtr::tactic::EdgeTransform T_c_w; // For storing a history of the transforms for temporal filtering diff --git a/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit_path_planner.hpp b/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit_path_planner.hpp index 9ec111928..7862db90f 100644 --- a/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit_path_planner.hpp +++ b/main/src/vtr_path_planning/include/vtr_path_planning/cbit/cbit_path_planner.hpp @@ -41,15 +41,8 @@ // Note long term, this class should probably be inherited by the base path planner -// enum for path traversal direction: - using namespace vtr; -enum PathDirection -{ - PATH_DIRECTION_REVERSE = -1, - PATH_DIRECTION_FORWARD = 1 -}; class CBITPlanner { public: @@ -63,7 +56,7 @@ class CBITPlanner { double sample_box_width; double dynamic_window_width; Tree tree; - std::vector> samples; + Node::Path samples; // Repair mode variables bool repair_mode = false; // Flag for whether or not we should resume the planner in repair mode to update the tree following a state update @@ -78,7 +71,7 @@ class CBITPlanner { std::unique_ptr new_state; // For storing the Output Path - std::shared_ptr> cbit_path_ptr; + std::shared_ptr cbit_path_ptr; // Flag that tells the cbit.cpp planning interface to stop the mpc if there is no current cbit solution std::shared_ptr valid_solution_ptr; @@ -92,8 +85,11 @@ class CBITPlanner { // Costmap pointer std::shared_ptr cbit_costmap_ptr; - CBITPlanner(CBITConfig conf_in, std::shared_ptr path_in, vtr::path_planning::BasePathPlanner::RobotState& robot_state, std::shared_ptr> path_ptr, std::shared_ptr costmap_ptr, std::shared_ptr corridor_ptr, std::shared_ptrsolution_ptr, std::shared_ptrwidth_ptr, PathDirection path_direction); + CBITPlanner(CBITConfig conf_in, std::shared_ptr path_in, vtr::path_planning::BasePathPlanner::RobotState& robot_state, std::shared_ptr> path_ptr, std::shared_ptr costmap_ptr, std::shared_ptr corridor_ptr, std::shared_ptrsolution_ptr, std::shared_ptrwidth_ptr); + void plan(); + void stopPlanning(); + void resetPlanner(); protected: struct ChainInfo { vtr::tactic::Timestamp stamp; @@ -103,18 +99,14 @@ class CBITPlanner { vtr::tactic::EdgeTransform T_w_p; // T_world_planning unsigned curr_sid; }; - /** \brief Retrieve information for planning from localization chain */ - ChainInfo getChainInfo(vtr::path_planning::BasePathPlanner::RobotState& robot_state); - private: void InitializePlanningSpace(); - void Planning(vtr::path_planning::BasePathPlanner::RobotState& robot_state, std::shared_ptr costmap_ptr, std::shared_ptr corridor_ptr, PathDirection path_direction); - void ResetPlanner(); std::shared_ptr UpdateStateSID(size_t SID, vtr::tactic::EdgeTransform T_p_r); std::vector> SampleBox(int m); std::vector> SampleFreeSpace(int m); double BestVertexQueueValue(); double BestEdgeQueueValue(); + vtr::path_planning::BasePathPlanner::RobotState& robot_state_; std::shared_ptr BestInVertexQueue(); void ExpandVertex(std::shared_ptr v); std::tuple, std::shared_ptr> BestInEdgeQueue(); @@ -131,4 +123,6 @@ class CBITPlanner { bool costmap_col_tight(Node node); bool discrete_collision(std::vector> obs, double discretization, Node start, Node end); std::shared_ptr col_check_path_v2(double max_lookahead_p); + + mutable bool planning_active_ = false; }; \ No newline at end of file diff --git a/main/src/vtr_path_planning/include/vtr_path_planning/cbit/generate_pq.hpp b/main/src/vtr_path_planning/include/vtr_path_planning/cbit/generate_pq.hpp index 8bd886d7a..fc5ea1c3c 100644 --- a/main/src/vtr_path_planning/include/vtr_path_planning/cbit/generate_pq.hpp +++ b/main/src/vtr_path_planning/include/vtr_path_planning/cbit/generate_pq.hpp @@ -21,8 +21,6 @@ #include #include -#include -#include #include #include @@ -32,6 +30,7 @@ #pragma once + // TODO: ROC singularity region handling class CBITPath { @@ -44,7 +43,6 @@ class CBITPath { std::vector sid; // store the sid value of the transform from the teach path CBITPath(CBITConfig config, std::vector initial_path); // constructor; need to feed this the path CBITPath() = default; - Pose interp_pose(double p_in); // Function to interpolate a pose from the teach path given a p value double delta_p_calc(Pose start_pose, Pose end_pose, double alpha); // Function for computing delta p intervals in p,q space // Internal function declarations private: @@ -52,9 +50,6 @@ class CBITPath { Eigen::Spline spline_path_xz_yz(const std::vector &input_path); // Processes the input discrete path into a cubic spline double radius_of_curvature(double dist, Eigen::Spline spline); // Calculates the radius of curvature using the spline at a given distance along the spline - - // Actually I think ill just do this in the constructor for now - //std::vector ProcessPath(std::vector disc_path); // Function for assigning p distance values for each euclidean point in pre-processing }; // Class for storing the dynamic corridor information diff --git a/main/src/vtr_path_planning/include/vtr_path_planning/cbit/utils.hpp b/main/src/vtr_path_planning/include/vtr_path_planning/cbit/utils.hpp index 795c949aa..341c91198 100644 --- a/main/src/vtr_path_planning/include/vtr_path_planning/cbit/utils.hpp +++ b/main/src/vtr_path_planning/include/vtr_path_planning/cbit/utils.hpp @@ -26,12 +26,37 @@ #include #include +#include "vtr_tactic/types.hpp" + #pragma once +namespace vtr::path_planning { + + struct ChainInfo { + tactic::Timestamp stamp; + Eigen::Matrix w_p_r_in_r; + // planning frame is the current localization frame + tactic::EdgeTransform T_p_r; // T_planning_robot + tactic::EdgeTransform T_w_p; // T_world_planning + tactic::EdgeTransform T_w_v_odo; // T_planning_robot + tactic::EdgeTransform T_r_v_odo; // T_world_planning + unsigned curr_sid; + }; + + /** \brief Retrieve information for planning from localization chain */ + ChainInfo getChainInfo(const tactic::LocalizationChain& robot_state); +} // namespace vtr::path_planning + + + + // Some useful Classes class Pose { public: + using Ptr = std::shared_ptr; + using Path = std::vector; + double x; double y; double z; @@ -60,6 +85,8 @@ class Pose { class Node { public: using Ptr = std::shared_ptr; + using Path = std::vector; + using Edge = std::tuple; double p; double q; diff --git a/main/src/vtr_path_planning/include/vtr_path_planning/cbit/visualization_utils.hpp b/main/src/vtr_path_planning/include/vtr_path_planning/cbit/visualization_utils.hpp index a4ac3341d..6f828bd05 100644 --- a/main/src/vtr_path_planning/include/vtr_path_planning/cbit/visualization_utils.hpp +++ b/main/src/vtr_path_planning/include/vtr_path_planning/cbit/visualization_utils.hpp @@ -29,6 +29,8 @@ #include #include #include "tf2_ros/transform_broadcaster.h" +#include +#include "std_msgs/msg/header.hpp" #include "vtr_tactic/tactic.hpp" #include "vtr_logging/logging.hpp" @@ -42,16 +44,22 @@ class VisualizationUtils { VisualizationUtils(); VisualizationUtils(rclcpp::Node::SharedPtr node); void visualize( - const tactic::Timestamp& stamp, - const tactic::EdgeTransform& T_w_p, - const tactic::EdgeTransform& T_p_r, - const tactic::EdgeTransform& T_p_r_extp_mpc, - const std::vector& mpc_prediction, - const std::vector& robot_prediction, - const std::vector& tracking_pose_vec, - const std::vector& homotopy_pose_vec, - const std::shared_ptr> cbit_path_ptr, - const std::shared_ptr corridor_ptr); + const tactic::Timestamp& stamp, + const tactic::EdgeTransform& T_w_p, + const tactic::EdgeTransform& T_p_r, + const tactic::EdgeTransform& T_p_r_extp_mpc, + const tactic::EdgeTransform& T_w_r, + const std::vector& mpc_prediction, + const std::vector& mpc_velocities, + const std::vector& robot_prediction, + const std::vector& tracking_pose_vec, + const std::vector& homotopy_pose_vec, + const std::shared_ptr> cbit_path_ptr, + const std::shared_ptr corridor_ptr, + const lgmath::se3::Transformation& T_w_p_interpolated_closest_to_robot, + const double& state_p, + const std::shared_ptr global_path_ptr, + unsigned closest_node_idx); private: std::shared_ptr tf_bc_; @@ -62,6 +70,7 @@ class VisualizationUtils { rclcpp::Publisher::SharedPtr corridor_pub_r_; rclcpp::Publisher::SharedPtr ref_pose_pub_tracking_; rclcpp::Publisher::SharedPtr ref_pose_pub_homotopy_; + rclcpp::Publisher::SharedPtr path_info_for_external_navigation_pub_; }; } // namespace path_planning diff --git a/main/src/vtr_path_planning/include/vtr_path_planning/mpc/custom_loss_functions.hpp b/main/src/vtr_path_planning/include/vtr_path_planning/mpc/custom_loss_functions.hpp deleted file mode 100644 index 4373ce1c1..000000000 --- a/main/src/vtr_path_planning/include/vtr_path_planning/mpc/custom_loss_functions.hpp +++ /dev/null @@ -1,60 +0,0 @@ -// Copyright 2021, Autonomous Space Robotics Lab (ASRL) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/** - * \file custom_loss_functions.hpp - * \author Jordy Sehn, Autonomous Space Robotics Lab (ASRL) - */ - -#pragma once - -#include "steam/problem/loss_func/base_loss_func.hpp" - -namespace steam { - -/** \brief 'L2' loss function with different weights: TODO long term make a version of this class which is more generic and can dynamically set the weight */ -class L2WeightedLossFunc : public BaseLossFunc { - private: - double weight_val = 1.0; - - public: - /** \brief Convenience typedefs */ - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - - static Ptr MakeShared(double weight_input) { return std::make_shared(weight_input); } - - /** \brief Constructor */ - //L2WeightedLossFunc() = default; - - - L2WeightedLossFunc(double weight_input) - { - weight_val = weight_input; - } - - /** \brief Cost function (basic evaluation of the loss function) */ - double cost(double whitened_error_norm) const override { - return 0.5 * whitened_error_norm * whitened_error_norm; - } - - /** - * \brief Weight for iteratively reweighted least-squares (influence function - * div. by error) - */ - double weight(double) const override { return weight_val; } - -}; - -} // namespace steam \ No newline at end of file diff --git a/main/src/vtr_path_planning/include/vtr_path_planning/mpc/lateral_error_evaluators.hpp b/main/src/vtr_path_planning/include/vtr_path_planning/mpc/lateral_error_evaluators.hpp deleted file mode 100644 index 3e85ce126..000000000 --- a/main/src/vtr_path_planning/include/vtr_path_planning/mpc/lateral_error_evaluators.hpp +++ /dev/null @@ -1,99 +0,0 @@ -// Copyright 2021, Autonomous Space Robotics Lab (ASRL) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/** - * \file lateral_error_evaluators.hpp - * \author Jordy Sehn, Autonomous Space Robotics Lab (ASRL) - */ - -#pragma once - -#include "steam/evaluable/state_var.hpp" - -namespace steam { - -class LateralErrorEvaluatorLeft : public Evaluable> { - public: - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - - using InType = Eigen::Vector4d; - using OutType = Eigen::Matrix; - - static Ptr MakeShared(const Evaluable::ConstPtr& pt, - const InType& meas_pt); - LateralErrorEvaluatorLeft(const Evaluable::ConstPtr& pt, - const InType& meas_pt); - - bool active() const override; - void getRelatedVarKeys(KeySet& keys) const override; - - OutType value() const override; - Node::Ptr forward() const override; - void backward(const Eigen::MatrixXd& lhs, const Node::Ptr& node, - Jacobians& jacs) const override; - - private: - /** \brief Transform evaluable */ - const Evaluable::ConstPtr pt_; - /** \brief Landmark state variable */ - const InType meas_pt_; - // constants - Eigen::Matrix D_ = Eigen::Matrix::Zero(); -}; - -LateralErrorEvaluatorLeft::Ptr homo_point_error_left( - const Evaluable::ConstPtr& pt, - const LateralErrorEvaluatorLeft::InType& meas_pt); - - - - - -class LateralErrorEvaluatorRight : public Evaluable> { - public: - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - - using InType = Eigen::Vector4d; - using OutType = Eigen::Matrix; - - static Ptr MakeShared(const Evaluable::ConstPtr& pt, - const InType& meas_pt); - LateralErrorEvaluatorRight(const Evaluable::ConstPtr& pt, - const InType& meas_pt); - - bool active() const override; - void getRelatedVarKeys(KeySet& keys) const override; - - OutType value() const override; - Node::Ptr forward() const override; - void backward(const Eigen::MatrixXd& lhs, const Node::Ptr& node, - Jacobians& jacs) const override; - - private: - /** \brief Transform evaluable */ - const Evaluable::ConstPtr pt_; - /** \brief Landmark state variable */ - const InType meas_pt_; - // constants - Eigen::Matrix D_ = Eigen::Matrix::Zero(); -}; - -LateralErrorEvaluatorRight::Ptr homo_point_error_right( - const Evaluable::ConstPtr& pt, - const LateralErrorEvaluatorRight::InType& meas_pt); - - -} // namespace steam diff --git a/main/src/vtr_path_planning/include/vtr_path_planning/mpc/mpc_common.hpp b/main/src/vtr_path_planning/include/vtr_path_planning/mpc/mpc_common.hpp new file mode 100644 index 000000000..0ab61abc1 --- /dev/null +++ b/main/src/vtr_path_planning/include/vtr_path_planning/mpc/mpc_common.hpp @@ -0,0 +1,69 @@ +// Copyright 2024, Autonomous Space Robotics Lab (ASRL) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** + * \file mpc_common.hpp + * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) + */ +#pragma once +#include +#include + + +namespace vtr::path_planning +{ + +std::vector tf_to_global(const lgmath::se3::Transformation& T); +tactic::EdgeTransform tf_from_global(double x, double y, double theta); + + +struct CurvatureInfo { + Eigen::Vector3d center; + double radius; + + inline double curvature() const { + return 1 / radius; + } + + static CurvatureInfo fromTransform(const lgmath::se3::Transformation &T_12); +}; + +struct PoseResultHomotopy +{ + std::vector poses; + std::vector barrier_q_max; + std::vector barrier_q_min; +}; + +// Declaring helper functions + +// Helper function for generating reference measurements poses from a discrete path to use for tracking the path at a desired forward velocity +PoseResultHomotopy generateHomotopyReference(const std::vector &rolled_out_poses, tactic::LocalizationChain::Ptr); +PoseResultHomotopy generateHomotopyReference(const std::vector& rolled_out_p, tactic::LocalizationChain::Ptr chain); +lgmath::se3::Transformation interpolatedPose(double p, const tactic::LocalizationChain::Ptr chain); + +using Segment = std::pair; +Segment findClosestSegment(const lgmath::se3::Transformation& T_wr, const tactic::LocalizationChain::Ptr chain, unsigned sid_start=0); +Segment findClosestSegment(const double p, const tactic::LocalizationChain::Ptr chain, unsigned sid_start=0); + +lgmath::se3::Transformation interpolatePath(const lgmath::se3::Transformation& T_wr, + const lgmath::se3::Transformation& seq_start, const lgmath::se3::Transformation& seq_end, + double& interp); +double findRobotP(const lgmath::se3::Transformation& T_wr, tactic::LocalizationChain::Ptr chain); + +template int sgn(T val) { + return (T(0) < val) - (val < T(0)); +} + +} \ No newline at end of file diff --git a/main/src/vtr_path_planning/include/vtr_path_planning/mpc/mpc_path_planner.hpp b/main/src/vtr_path_planning/include/vtr_path_planning/mpc/mpc_path_planner.hpp index a0e4bc36b..2b8c8fe0b 100644 --- a/main/src/vtr_path_planning/include/vtr_path_planning/mpc/mpc_path_planner.hpp +++ b/main/src/vtr_path_planning/include/vtr_path_planning/mpc/mpc_path_planner.hpp @@ -16,81 +16,48 @@ * \file mpc_path_planner.hpp * \author Jordy Sehn, Autonomous Space Robotics Lab (ASRL) */ - -#include "vtr_path_planning/cbit/cbit.hpp" -#include "steam.hpp" - #pragma once -struct MPCConfig -{ - Eigen::Matrix previous_vel; - lgmath::se3::Transformation T0; - std::vector tracking_reference_poses; - std::vector homotopy_reference_poses; - std::vector barrier_q_left; - std::vector barrier_q_right; - int K; - double DT; - double VF; - Eigen::Matrix lat_noise_vect; - Eigen::Matrix pose_noise_vect; - Eigen::Matrix vel_noise_vect; - Eigen::Matrix accel_noise_vect; - Eigen::Matrix kin_noise_vect; - bool point_stabilization; - double pose_error_weight; - double vel_error_weight; - double acc_error_weight; - double kin_error_weight; - double lat_error_weight; - bool verbosity; - bool homotopy_mode; -}; - -struct MPCResult -{ - Eigen::Matrix applied_vel; - std::vector mpc_poses; -}; - -struct PoseResultTracking -{ - std::vector poses; - bool point_stabilization; - std::vector p_interp_vec; - std::vector q_interp_vec; -}; +#include +#include + +namespace vtr::path_planning { +class CasadiUnicycleMPC { +public: + PTR_TYPEDEFS(CasadiUnicycleMPC); + using DM = casadi::DM; + + struct Config { + // These values are defined the python code and exported + // TODO add an automatic way to keep the code in sync + static constexpr int nStates = 3; + static constexpr int nControl = 2; + static constexpr double alpha = 0.4; + static constexpr int N = 15; + static constexpr double DT = 0.25; + DM previous_vel{nControl, 1}; + DM T0{nStates, 1}; + std::vector reference_poses; + std::vector up_barrier_q; + std::vector low_barrier_q; + double VF = 0.0; + DM vel_max{nControl, 1}; + }; + + + CasadiUnicycleMPC(bool verbose=false, casadi::Dict iopt_config={ + { "max_iter", 2000 }, + { "acceptable_tol", 1e-8 } , + {"acceptable_obj_change_tol", 1e-6} + }); + + std::map solve(const Config& mpcConf); + + +private: + casadi::Function solve_mpc; + std::map arg_; -struct PoseResultHomotopy -{ - std::vector poses; - bool point_stabilization; - std::vector barrier_q_left; - std::vector barrier_q_right; }; -struct InterpResult -{ - lgmath::se3::Transformation pose; - double p_interp; - double q_interp; -}; - - -// Declaring helper functions - -// Primary optimization function: Takes in the input configurations and the extrapolated robot pose, outputs a vector for the velocity to apply and the predicted horizon -struct MPCResult SolveMPC(const MPCConfig& config); - -// Helper function for generating reference measurements poses from a discrete path to use for tracking the path at a desired forward velocity -struct PoseResultTracking GenerateTrackingReference(std::shared_ptr> cbit_path_ptr, std::tuple robot_pose, int K, double DT, double VF); - -// Helper function for generating reference measurements poses from a discrete path to use for tracking the path at a desired forward velocity -struct PoseResultHomotopy GenerateHomotopyReference(std::shared_ptr global_path_ptr, std::shared_ptr corridor_ptr, std::tuple robot_pose, const std::vector &p_interp_vec); - -// Helper function for post-processing and saturating the velocity command -Eigen::Matrix SaturateVel(Eigen::Matrix applied_vel, double v_lim, double w_lim); - -// Helper function in Generate Reference Meas which interpolates a Transformation measurement gen the cbit_path and the desired measurements p value along the path -struct InterpResult InterpolatePose(double p_meas, std::vector cbit_p, std::vector cbit_path); \ No newline at end of file +} //namespace vtr::path_planning \ No newline at end of file diff --git a/main/src/vtr_path_planning/include/vtr_path_planning/mpc/mpc_solver.hpp b/main/src/vtr_path_planning/include/vtr_path_planning/mpc/mpc_solver.hpp new file mode 100644 index 000000000..ab90084f5 --- /dev/null +++ b/main/src/vtr_path_planning/include/vtr_path_planning/mpc/mpc_solver.hpp @@ -0,0 +1,162 @@ +/* This file was automatically generated by CasADi 3.6.5. + * It consists of: + * 1) content generated by CasADi runtime: not copyrighted + * 2) template code copied from CasADi source: permissively licensed (MIT-0) + * 3) user code: owned by the user + * + */ +#ifndef casadi_real +#define casadi_real double +#endif + +#ifndef casadi_int +#define casadi_int long long int +#endif + +extern "C" int nlp(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem); +extern "C" int nlp_alloc_mem(void); +extern "C" int nlp_init_mem(int mem); +extern "C" void nlp_free_mem(int mem); +extern "C" int nlp_checkout(void); +extern "C" void nlp_release(int mem); +extern "C" void nlp_incref(void); +extern "C" void nlp_decref(void); +extern "C" casadi_int nlp_n_in(void); +extern "C" casadi_int nlp_n_out(void); +extern "C" casadi_real nlp_default_in(casadi_int i); +extern "C" const char* nlp_name_in(casadi_int i); +extern "C" const char* nlp_name_out(casadi_int i); +extern "C" const casadi_int* nlp_sparsity_in(casadi_int i); +extern "C" const casadi_int* nlp_sparsity_out(casadi_int i); +extern "C" int nlp_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w); +extern "C" int nlp_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w); +#define nlp_SZ_ARG 2 +#define nlp_SZ_RES 2 +#define nlp_SZ_IW 0 +#define nlp_SZ_W 138 +extern "C" int nlp_f(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem); +extern "C" int nlp_f_alloc_mem(void); +extern "C" int nlp_f_init_mem(int mem); +extern "C" void nlp_f_free_mem(int mem); +extern "C" int nlp_f_checkout(void); +extern "C" void nlp_f_release(int mem); +extern "C" void nlp_f_incref(void); +extern "C" void nlp_f_decref(void); +extern "C" casadi_int nlp_f_n_in(void); +extern "C" casadi_int nlp_f_n_out(void); +extern "C" casadi_real nlp_f_default_in(casadi_int i); +extern "C" const char* nlp_f_name_in(casadi_int i); +extern "C" const char* nlp_f_name_out(casadi_int i); +extern "C" const casadi_int* nlp_f_sparsity_in(casadi_int i); +extern "C" const casadi_int* nlp_f_sparsity_out(casadi_int i); +extern "C" int nlp_f_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w); +extern "C" int nlp_f_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w); +#define nlp_f_SZ_ARG 2 +#define nlp_f_SZ_RES 1 +#define nlp_f_SZ_IW 0 +#define nlp_f_SZ_W 37 +extern "C" int nlp_g(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem); +extern "C" int nlp_g_alloc_mem(void); +extern "C" int nlp_g_init_mem(int mem); +extern "C" void nlp_g_free_mem(int mem); +extern "C" int nlp_g_checkout(void); +extern "C" void nlp_g_release(int mem); +extern "C" void nlp_g_incref(void); +extern "C" void nlp_g_decref(void); +extern "C" casadi_int nlp_g_n_in(void); +extern "C" casadi_int nlp_g_n_out(void); +extern "C" casadi_real nlp_g_default_in(casadi_int i); +extern "C" const char* nlp_g_name_in(casadi_int i); +extern "C" const char* nlp_g_name_out(casadi_int i); +extern "C" const casadi_int* nlp_g_sparsity_in(casadi_int i); +extern "C" const casadi_int* nlp_g_sparsity_out(casadi_int i); +extern "C" int nlp_g_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w); +extern "C" int nlp_g_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w); +#define nlp_g_SZ_ARG 2 +#define nlp_g_SZ_RES 1 +#define nlp_g_SZ_IW 0 +#define nlp_g_SZ_W 47 +extern "C" int nlp_grad(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem); +extern "C" int nlp_grad_alloc_mem(void); +extern "C" int nlp_grad_init_mem(int mem); +extern "C" void nlp_grad_free_mem(int mem); +extern "C" int nlp_grad_checkout(void); +extern "C" void nlp_grad_release(int mem); +extern "C" void nlp_grad_incref(void); +extern "C" void nlp_grad_decref(void); +extern "C" casadi_int nlp_grad_n_in(void); +extern "C" casadi_int nlp_grad_n_out(void); +extern "C" casadi_real nlp_grad_default_in(casadi_int i); +extern "C" const char* nlp_grad_name_in(casadi_int i); +extern "C" const char* nlp_grad_name_out(casadi_int i); +extern "C" const casadi_int* nlp_grad_sparsity_in(casadi_int i); +extern "C" const casadi_int* nlp_grad_sparsity_out(casadi_int i); +extern "C" int nlp_grad_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w); +extern "C" int nlp_grad_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w); +#define nlp_grad_SZ_ARG 4 +#define nlp_grad_SZ_RES 4 +#define nlp_grad_SZ_IW 0 +#define nlp_grad_SZ_W 896 +extern "C" int nlp_grad_f(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem); +extern "C" int nlp_grad_f_alloc_mem(void); +extern "C" int nlp_grad_f_init_mem(int mem); +extern "C" void nlp_grad_f_free_mem(int mem); +extern "C" int nlp_grad_f_checkout(void); +extern "C" void nlp_grad_f_release(int mem); +extern "C" void nlp_grad_f_incref(void); +extern "C" void nlp_grad_f_decref(void); +extern "C" casadi_int nlp_grad_f_n_in(void); +extern "C" casadi_int nlp_grad_f_n_out(void); +extern "C" casadi_real nlp_grad_f_default_in(casadi_int i); +extern "C" const char* nlp_grad_f_name_in(casadi_int i); +extern "C" const char* nlp_grad_f_name_out(casadi_int i); +extern "C" const casadi_int* nlp_grad_f_sparsity_in(casadi_int i); +extern "C" const casadi_int* nlp_grad_f_sparsity_out(casadi_int i); +extern "C" int nlp_grad_f_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w); +extern "C" int nlp_grad_f_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w); +#define nlp_grad_f_SZ_ARG 2 +#define nlp_grad_f_SZ_RES 2 +#define nlp_grad_f_SZ_IW 0 +#define nlp_grad_f_SZ_W 394 +extern "C" int nlp_hess_l(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem); +extern "C" int nlp_hess_l_alloc_mem(void); +extern "C" int nlp_hess_l_init_mem(int mem); +extern "C" void nlp_hess_l_free_mem(int mem); +extern "C" int nlp_hess_l_checkout(void); +extern "C" void nlp_hess_l_release(int mem); +extern "C" void nlp_hess_l_incref(void); +extern "C" void nlp_hess_l_decref(void); +extern "C" casadi_int nlp_hess_l_n_in(void); +extern "C" casadi_int nlp_hess_l_n_out(void); +extern "C" casadi_real nlp_hess_l_default_in(casadi_int i); +extern "C" const char* nlp_hess_l_name_in(casadi_int i); +extern "C" const char* nlp_hess_l_name_out(casadi_int i); +extern "C" const casadi_int* nlp_hess_l_sparsity_in(casadi_int i); +extern "C" const casadi_int* nlp_hess_l_sparsity_out(casadi_int i); +extern "C" int nlp_hess_l_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w); +extern "C" int nlp_hess_l_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w); +#define nlp_hess_l_SZ_ARG 4 +#define nlp_hess_l_SZ_RES 1 +#define nlp_hess_l_SZ_IW 0 +#define nlp_hess_l_SZ_W 826 +extern "C" int nlp_jac_g(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem); +extern "C" int nlp_jac_g_alloc_mem(void); +extern "C" int nlp_jac_g_init_mem(int mem); +extern "C" void nlp_jac_g_free_mem(int mem); +extern "C" int nlp_jac_g_checkout(void); +extern "C" void nlp_jac_g_release(int mem); +extern "C" void nlp_jac_g_incref(void); +extern "C" void nlp_jac_g_decref(void); +extern "C" casadi_int nlp_jac_g_n_in(void); +extern "C" casadi_int nlp_jac_g_n_out(void); +extern "C" casadi_real nlp_jac_g_default_in(casadi_int i); +extern "C" const char* nlp_jac_g_name_in(casadi_int i); +extern "C" const char* nlp_jac_g_name_out(casadi_int i); +extern "C" const casadi_int* nlp_jac_g_sparsity_in(casadi_int i); +extern "C" const casadi_int* nlp_jac_g_sparsity_out(casadi_int i); +extern "C" int nlp_jac_g_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w); +extern "C" int nlp_jac_g_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w); +#define nlp_jac_g_SZ_ARG 2 +#define nlp_jac_g_SZ_RES 2 +#define nlp_jac_g_SZ_IW 0 +#define nlp_jac_g_SZ_W 369 diff --git a/main/src/vtr_path_planning/include/vtr_path_planning/mpc/scalar_log_barrier_evaluator.hpp b/main/src/vtr_path_planning/include/vtr_path_planning/mpc/scalar_log_barrier_evaluator.hpp deleted file mode 100644 index 06cce077c..000000000 --- a/main/src/vtr_path_planning/include/vtr_path_planning/mpc/scalar_log_barrier_evaluator.hpp +++ /dev/null @@ -1,227 +0,0 @@ -// Copyright 2021, Autonomous Space Robotics Lab (ASRL) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/** - * \file scalar_log_barrier_evaluator.hpp - * \author Jordy Sehn, Autonomous Space Robotics Lab (ASRL) - */ - -#pragma once - -#include - -#include "steam/evaluable/evaluable.hpp" - -namespace steam { -namespace vspace { - -template -class ScalarLogBarrierEvaluator : public Evaluable> { - public: - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - - using InType = Eigen::Matrix; - using OutType = Eigen::Matrix; - - static Ptr MakeShared(const typename Evaluable::ConstPtr& v); - ScalarLogBarrierEvaluator(const typename Evaluable::ConstPtr& v); - - bool active() const override; - using KeySet = typename Evaluable::KeySet; - void getRelatedVarKeys(KeySet& keys) const override; - - OutType value() const override; - typename Node::Ptr forward() const override; - void backward(const Eigen::MatrixXd& lhs, - const typename Node::Ptr& node, - Jacobians& jacs) const override; - - private: - const typename Evaluable::ConstPtr v_; -}; - -// clang-format off -template -typename ScalarLogBarrierEvaluator::Ptr slogbar( - const typename Evaluable::InType>::ConstPtr& v); -// clang-format on - -} // namespace vspace -} // namespace steam - -namespace steam { -namespace vspace { - -template -auto ScalarLogBarrierEvaluator::MakeShared( - const typename Evaluable::ConstPtr& v) -> Ptr { - return std::make_shared(v); -} - -template -ScalarLogBarrierEvaluator::ScalarLogBarrierEvaluator( - const typename Evaluable::ConstPtr& v) - : v_(v) {} - -template -bool ScalarLogBarrierEvaluator::active() const { - return v_->active(); -} - -template -void ScalarLogBarrierEvaluator::getRelatedVarKeys(KeySet& keys) const { - v_->getRelatedVarKeys(keys); -} - -template -auto ScalarLogBarrierEvaluator::value() const -> OutType { - auto v_intermed = v_->value(); - v_intermed[0] = (v_->value()[0]); // TODO. need to validate this, not 100% sure this is the best/correct way to do this log - return v_intermed; -} - -template -auto ScalarLogBarrierEvaluator::forward() const -> typename Node::Ptr { - const auto child = v_->forward(); - auto value = child->value(); - value[0] = log((child->value())[0]); // TODO. need to validate this, not 100% sure this is the best/correct way to do this log - //const auto value = log((child->value()[0])); - const auto node = Node::MakeShared(value); - node->addChild(child); - return node; -} - -template -void ScalarLogBarrierEvaluator::backward(const Eigen::MatrixXd& lhs, - const typename Node::Ptr& node, - Jacobians& jacs) const { - const auto child = std::static_pointer_cast>(node->at(0)); - if (v_->active()) { - v_->backward(lhs.inverse(), child, jacs); - } -} - -// clang-format off -template -typename ScalarLogBarrierEvaluator::Ptr slogbar( - const typename Evaluable::InType>::ConstPtr& v) { - return ScalarLogBarrierEvaluator::MakeShared(v); -} -// clang-format on - - - - - - - -// Inverse barrier term (EXPERIMENTAL) -template -class ScalarInverseBarrierEvaluator : public Evaluable> { - public: - using Ptr = std::shared_ptr; - using ConstPtr = std::shared_ptr; - - using InType = Eigen::Matrix; - using OutType = Eigen::Matrix; - - static Ptr MakeShared(const typename Evaluable::ConstPtr& v); - ScalarInverseBarrierEvaluator(const typename Evaluable::ConstPtr& v); - - bool active() const override; - using KeySet = typename Evaluable::KeySet; - void getRelatedVarKeys(KeySet& keys) const override; - - OutType value() const override; - typename Node::Ptr forward() const override; - void backward(const Eigen::MatrixXd& lhs, - const typename Node::Ptr& node, - Jacobians& jacs) const override; - - private: - const typename Evaluable::ConstPtr v_; -}; - -// clang-format off -template -typename ScalarInverseBarrierEvaluator::Ptr sinvbar( - const typename Evaluable::InType>::ConstPtr& v); -// clang-format on - -} // namespace vspace -} // namespace steam - -namespace steam { -namespace vspace { - -template -auto ScalarInverseBarrierEvaluator::MakeShared( - const typename Evaluable::ConstPtr& v) -> Ptr { - return std::make_shared(v); -} - -template -ScalarInverseBarrierEvaluator::ScalarInverseBarrierEvaluator( - const typename Evaluable::ConstPtr& v) - : v_(v) {} - -template -bool ScalarInverseBarrierEvaluator::active() const { - return v_->active(); -} - -template -void ScalarInverseBarrierEvaluator::getRelatedVarKeys(KeySet& keys) const { - v_->getRelatedVarKeys(keys); -} - -template -auto ScalarInverseBarrierEvaluator::value() const -> OutType { - auto v_intermed = v_->value(); - v_intermed[0] = (v_->value()[0]); // TODO. need to validate this, not 100% sure this is the best/correct way to do this log - return v_intermed; -} - -template -auto ScalarInverseBarrierEvaluator::forward() const -> typename Node::Ptr { - const auto child = v_->forward(); - auto value = child->value(); - value[0] = 1.0 / (child->value())[0]; // TODO. need to validate this, not 100% sure this is the best/correct way to do this inv - //const auto value = log((child->value()[0])); - const auto node = Node::MakeShared(value); - node->addChild(child); - return node; -} - -template -void ScalarInverseBarrierEvaluator::backward(const Eigen::MatrixXd& lhs, - const typename Node::Ptr& node, - Jacobians& jacs) const { - const auto child = std::static_pointer_cast>(node->at(0)); - if (v_->active()) { - v_->backward(-1.0 * (lhs * lhs).inverse(), child, jacs); - } -} - -// clang-format off -template -typename ScalarInverseBarrierEvaluator::Ptr sinvbar( - const typename Evaluable::InType>::ConstPtr& v) { - return ScalarInverseBarrierEvaluator::MakeShared(v); -} -// clang-format on - -} // namespace vspace -} // namespace steam \ No newline at end of file diff --git a/main/src/vtr_path_planning/include/vtr_path_planning/mpc/speed_scheduler.hpp b/main/src/vtr_path_planning/include/vtr_path_planning/mpc/speed_scheduler.hpp index a9c074858..a0c469420 100644 --- a/main/src/vtr_path_planning/include/vtr_path_planning/mpc/speed_scheduler.hpp +++ b/main/src/vtr_path_planning/include/vtr_path_planning/mpc/speed_scheduler.hpp @@ -17,11 +17,25 @@ * \author Jordy Sehn, Autonomous Space Robotics Lab (ASRL) */ - #pragma once #include -#include "vtr_logging/logging.hpp" -#include "vtr_path_planning/cbit/generate_pq.hpp" +#include +#include + + +namespace vtr::path_planning +{ +struct SpeedSchedConfig { + double target_vel = 1; + double min_vel = 0; + + double planar_curv_weight = 0; + double profile_curv_weight = 0; + double eop_weight = 0; + unsigned horizon_steps = 1; +}; -double ScheduleSpeed(const std::vector& disc_path_curvature_xy, const std::vector& disc_path_curvature_xz_yz, double VF, unsigned curr_sid, double planar_curv_weight, double profile_curv_weight, double eop_weight, double horizon_step_size, double min_vel); +double ScheduleSpeed(tactic::LocalizationChain::Ptr chain, const SpeedSchedConfig& params); + +} // namespace vtr::path_planning \ No newline at end of file diff --git a/main/src/vtr_path_planning/package.xml b/main/src/vtr_path_planning/package.xml index d568c4974..0f2360028 100644 --- a/main/src/vtr_path_planning/package.xml +++ b/main/src/vtr_path_planning/package.xml @@ -16,6 +16,7 @@ tf2_eigen vtr_common + vtr_path_planning_msgs vtr_logging vtr_tactic diff --git a/main/src/vtr_path_planning/scripts/export_unicycle.py b/main/src/vtr_path_planning/scripts/export_unicycle.py new file mode 100755 index 000000000..be699f520 --- /dev/null +++ b/main/src/vtr_path_planning/scripts/export_unicycle.py @@ -0,0 +1,9 @@ +#! /usr/bin/python3 + +from unicycle_solver import solver +import shutil +import os + +solver.generate_dependencies("mpc_solver.cpp", {"cpp": True, "with_header": True}) +shutil.move('mpc_solver.cpp', os.getenv("VTRSRC") + '/main/src/vtr_path_planning/src/mpc/mpc_solver.cpp') +shutil.move('mpc_solver.h', os.getenv("VTRSRC") + '/main/src/vtr_path_planning/include/vtr_path_planning/mpc/mpc_solver.hpp') \ No newline at end of file diff --git a/main/src/vtr_path_planning/scripts/run_unicycle_mpc.py b/main/src/vtr_path_planning/scripts/run_unicycle_mpc.py new file mode 100644 index 000000000..160b52dc4 --- /dev/null +++ b/main/src/vtr_path_planning/scripts/run_unicycle_mpc.py @@ -0,0 +1,215 @@ +from time import time +import casadi as ca +import numpy as np +import matplotlib.pyplot as plt +from simulation_code import simulate_path_tracking + +from unicycle_solver import solver, motion_model, alpha, N, step_horizon, n_states, n_controls + + +# specs +x_init = 0.5 +y_init = 0.2 +theta_init = 0 + +v_max = 1.5 +v_min = -1.5 +w_max = 0.5 +w_min = -0.5 +v_ref = 0.5*v_max + +lin_acc_max = 1.00 +ang_acc_max = 0.5 + + +sim_time = 100 # simulation time + + +state_init = ca.DM([x_init, y_init, theta_init]) # initial state + +def shift_timestep(step, t0, state_init, u, f, last_u): + f_value = f(state_init, u[:, 0], last_u) + next_state = ca.DM.full(state_init + (step * f_value)) + t0 = t0 + step + u0 = ca.horzcat( + u[:, 1:], + ca.reshape(u[:, -1], -1, 1) + ) + + return t0, next_state, u0 + + +def DM2Arr(dm): + return np.array(dm.full()) + +path_x = np.linspace(0, 30, 10000) +path_y = 0.5*path_x + np.sin(2*np.pi*path_x/10) +path_mat = np.zeros((np.size(path_x), 3)) +path_mat[:, 0] = path_x +path_mat[:, 1] = path_y +path_mat[:, 2] = np.arctan2(np.gradient(path_y), np.gradient(path_x)) +path_p = np.zeros_like(path_x) +for i in range(1, np.size(path_p)): + path_p[i] = path_p[i-1] + np.hypot(path_x[i] - path_x[i-1], path_y[i] - path_y[i-1]) + 0.25*np.abs(path_mat[i, 2] - path_mat[i-1, 2]) + + +lbx = ca.DM.zeros((n_states*(N+1) + n_controls*N, 1)) +ubx = ca.DM.zeros((n_states*(N+1) + n_controls*N, 1)) + +lbx[0: n_states*(N+1): n_states] = -ca.inf # X lower bound +lbx[1: n_states*(N+1): n_states] = -ca.inf # Y lower bound +lbx[2: n_states*(N+1): n_states] = -ca.inf # theta lower bound + +ubx[0: n_states*(N+1): n_states] = ca.inf # X upper bound +ubx[1: n_states*(N+1): n_states] = ca.inf # Y upper bound +ubx[2: n_states*(N+1): n_states] = ca.inf # theta upper bound + +lbx[n_states*(N+1)::2] = v_min # v lower bound for all V +ubx[n_states*(N+1)::2] = v_max # v upper bound for all V +lbx[n_states*(N+1)+1::2] = w_min # v upper bound for all V +ubx[n_states*(N+1)+1::2] = w_max # v upper bound for all V + +#Motion Model Constraints +lbg = ca.DM.zeros((n_states*(N+1) + N, 1)) # constraints lower bound +ubg = ca.DM.zeros((n_states*(N+1) + N, 1)) # constraints upper bound + +#Corridor Width constraints +lbg[n_states*(N+1):n_states*(N+1)+N] = -0.5 +ubg[n_states*(N+1):n_states*(N+1)+N] = 0.5 + +#Acceleration constraints +# lbg[n_states*(N+1)+N::2] = -lin_acc_max * step_horizon +# ubg[n_states*(N+1)+N::2] = lin_acc_max * step_horizon +# lbg[n_states*(N+1)+N+1::2] = -ang_acc_max * step_horizon +# ubg[n_states*(N+1)+N+1::2] = ang_acc_max * step_horizon + +print(lbx.shape) + +args = { + 'lbg': lbg, + 'ubg': ubg, + 'lbx': lbx, + 'ubx': ubx +} + +t0 = 0 + +t = ca.DM(t0) + +u0 = 0.1*ca.DM.ones((n_controls, N)) # initial control +X0 = ca.repmat(state_init, 1, N+1) # initial state full + + +mpc_iter = 0 +cat_states = DM2Arr(X0) +cat_controls = DM2Arr(u0[:, 0]) +times = np.array([[0]]) +last_u = u0[:, 0] +state_target = path_mat[-1, :] + +############################################################################### + +if __name__ == '__main__': + main_loop = time() # return time in sec + p_state = 0 + while (ca.norm_2(state_init - state_target) > 2e-1) and (mpc_iter * step_horizon < sim_time): + t1 = time() + + args['p'] = ca.vertcat( + state_init, # current state + ) + + for i in range(1, N+1): + p_target = p_state + v_ref * step_horizon * i + p_idx = np.argmin(np.abs(path_p - p_target)) + args['p'] = ca.vertcat(args['p'], + ca.DM(path_mat[p_idx, :])) + + state_target = ca.DM(path_mat[p_idx, :]) + + args['p'] = ca.vertcat(args['p'], + ca.DM(last_u,)) + + u0 = ca.DM.zeros((n_controls, N)) # initial control + X0 = ca.repmat(state_init, 1, N+1) # initial state full + + # optimization variable current state + args['x0'] = ca.vertcat( + ca.reshape(X0, n_states*(N+1), 1), + ca.reshape(u0, n_controls*N, 1) + ) + + sol = solver( + x0=args['x0'], + lbx=args['lbx'], + ubx=args['ubx'], + lbg=args['lbg'], + ubg=args['ubg'], + p=args['p'] + ) + if not solver.stats()["success"]: + print(solver.stats()['return_status']) + print(solver.stats()) + break + + + u = ca.reshape(sol['x'][n_states * (N + 1):], n_controls, N) + # print(u) + + X0 = ca.reshape(sol['x'][: n_states * (N+1)], n_states, N+1) + + cat_states = np.dstack(( + cat_states, + DM2Arr(X0) + )) + + cat_controls = np.hstack(( + cat_controls, + DM2Arr(u[:, 0]) + )) + t = np.vstack(( + t, + t0 + )) + + t0, state_init, u0 = shift_timestep(step_horizon, t0, state_init, u, motion_model, last_u) + last_u = ca.vertcat(u[0, 0], (1-alpha) * last_u[1] + alpha * u[1, 0]) + + closest_idx = np.argmin(np.linalg.norm(path_mat - state_init.T, axis=1)) + + p_state = path_p[closest_idx] + + + # print(X0) + X0 = ca.horzcat( + X0[:, 1:], + ca.reshape(X0[:, -1], -1, 1) + ) + + # xx ... + t2 = time() + # print(t2-t1) + times = np.vstack(( + times, + t2-t1 + )) + + mpc_iter = mpc_iter + 1 + # if mpc_iter == 10: + # break + + + main_loop_time = time() + ss_error = ca.norm_2(state_init - state_target) + + print('\n\n') + print('Total time: ', main_loop_time - main_loop) + print('avg iteration time: ', np.array(times).mean() * 1000, 'ms') + print('final error: ', ss_error) + + plt.plot(cat_controls[0]) + plt.plot(cat_controls[1]) + + + # simulate + simulate_path_tracking(cat_states, cat_controls, times, step_horizon, N, path_mat, save=False) diff --git a/main/src/vtr_path_planning/scripts/simulation_code.py b/main/src/vtr_path_planning/scripts/simulation_code.py new file mode 100644 index 000000000..d331a8014 --- /dev/null +++ b/main/src/vtr_path_planning/scripts/simulation_code.py @@ -0,0 +1,183 @@ +import numpy as np +from numpy import sin, cos, pi +import matplotlib.pyplot as plt +from matplotlib import animation +from time import time + + +def simulate_point_stab(cat_states, cat_controls, t, step_horizon, N, reference, save=False): + def create_triangle(state=[0,0,0], h=1, w=0.5, update=False): + x, y, th = state + triangle = np.array([ + [h, 0 ], + [0, w/2], + [0, -w/2], + [h, 0 ] + ]).T + rotation_matrix = np.array([ + [cos(th), -sin(th)], + [sin(th), cos(th)] + ]) + + coords = np.array([[x, y]]) + (rotation_matrix @ triangle).T + if update == True: + return coords + else: + return coords[:3, :] + + def init(): + return path, horizon, current_state, target_state, + + def animate(i): + # get variables + x = cat_states[0, 0, i] + y = cat_states[1, 0, i] + th = cat_states[2, 0, i] + + # update path + if i == 0: + path.set_data(np.array([]), np.array([])) + x_new = np.hstack((path.get_xdata(), x)) + y_new = np.hstack((path.get_ydata(), y)) + path.set_data(x_new, y_new) + + # update horizon + x_new = cat_states[0, :, i] + y_new = cat_states[1, :, i] + horizon.set_data(x_new, y_new) + + # update current_state + current_state.set_xy(create_triangle([x, y, th], update=True)) + + # update target_state + # xy = target_state.get_xy() + # target_state.set_xy(xy) + + return path, horizon, current_state, target_state, + + # create figure and axes + fig, ax = plt.subplots(figsize=(6, 6)) + min_scale = min(reference[0], reference[1], reference[3], reference[4]) - 2 + max_scale = max(reference[0], reference[1], reference[3], reference[4]) + 2 + ax.set_xlim(left = min_scale, right = max_scale) + ax.set_ylim(bottom = min_scale, top = max_scale) + + # create lines: + # path + path, = ax.plot([], [], 'k', linewidth=2) + # horizon + horizon, = ax.plot([], [], 'x-g', alpha=0.5) + # current_state + current_triangle = create_triangle(cat_states[:3, 0, 0]) + current_state = ax.fill(current_triangle[:, 0], current_triangle[:, 1], color='r') + current_state = current_state[0] + # target_state + target_triangle = create_triangle(reference[3:]) + target_state = ax.fill(target_triangle[:, 0], target_triangle[:, 1], color='b') + target_state = target_state[0] + + sim = animation.FuncAnimation( + fig=fig, + func=animate, + init_func=init, + frames=len(t), + interval=step_horizon*100, + blit=True, + repeat=True + ) + plt.show() + + if save == True: + sim.save('./animation' + str(time()) +'.gif', writer='ffmpeg', fps=30) + + return + + +def simulate_path_tracking(cat_states, cat_controls, t, step_horizon, N, reference_path, save=False): + def create_triangle(state=[0,0,0], h=1, w=0.5, update=False): + x, y, th = state + triangle = np.array([ + [h, 0 ], + [0, w/2], + [0, -w/2], + [h, 0 ] + ]).T + rotation_matrix = np.array([ + [cos(th), -sin(th)], + [sin(th), cos(th)] + ]) + + coords = np.array([[x, y]]) + (rotation_matrix @ triangle).T + if update == True: + return coords + else: + return coords[:3, :] + + def init(): + return path, horizon, current_state, target_state, + + def animate(i): + # get variables + x = cat_states[0, 0, i] + y = cat_states[1, 0, i] + th = cat_states[2, 0, i] + + # update path + if i == 0: + path.set_data(np.array([]), np.array([])) + x_new = np.hstack((path.get_xdata(), x)) + y_new = np.hstack((path.get_ydata(), y)) + path.set_data(x_new, y_new) + + # update horizon + x_new = cat_states[0, :, i] + y_new = cat_states[1, :, i] + horizon.set_data(x_new, y_new) + + # update current_state + current_state.set_xy(create_triangle([x, y, th], update=True)) + + # update target_state + # xy = target_state.get_xy() + # target_state.set_xy(xy) + + return path, horizon, current_state, target_state, + + # create figure and axes + fig, ax = plt.subplots(figsize=(6, 6)) + min_scale = np.min(reference_path[:, :2]) - 2 + max_scale = np.max(reference_path[:, :2]) + 2 + ax.set_xlim(left = min_scale, right = max_scale) + ax.set_ylim(bottom = min_scale, top = max_scale) + + # create lines: + # path + path, = ax.plot([], [], 'k', linewidth=2) + # horizon + horizon, = ax.plot([], [], 'x-g', alpha=0.5) + # current_state + current_triangle = create_triangle(cat_states[:3, 0, 0]) + current_state = ax.fill(current_triangle[:, 0], current_triangle[:, 1], color='r') + current_state = current_state[0] + # target_state + target_triangle = create_triangle(reference_path[-1]) + target_state = ax.fill(target_triangle[:, 0], target_triangle[:, 1], color='b') + target_state = target_state[0] + ref_path, = ax.plot(reference_path[:, 0], reference_path[:, 1], color='g', label="Ref Path") + + sim = animation.FuncAnimation( + fig=fig, + func=animate, + init_func=init, + frames=len(t), + interval=step_horizon*100, + blit=True, + repeat=True + ) + plt.show() + + if save == True: + sim.save('./animation' + str(time()) +'.gif', writer='ffmpeg', fps=30) + + return + diff --git a/main/src/vtr_path_planning/scripts/unicycle_solver.py b/main/src/vtr_path_planning/scripts/unicycle_solver.py new file mode 100644 index 000000000..351e0b203 --- /dev/null +++ b/main/src/vtr_path_planning/scripts/unicycle_solver.py @@ -0,0 +1,177 @@ +import sys +sys.dont_write_bytecode = True + +import casadi as ca +from casadi import sin, cos, pi + +#Compile Time Constants (Could use params to set!) + +# Pose Covariance +Q_x = 10 +Q_y = 10 +Q_theta = 1 +# Command Covariance +R1 = 1.0 #0.1 +R2 = 1.0 #0.1 + +# Acceleration Cost Covariance +Acc_R1 = 0.1 +Acc_R2 = 0.5 #0.01 + +step_horizon = 0.25 # time between steps in seconds +N = 15 # number of look ahead steps + +# The first order lag weighting for the angular velocity +alpha = 0.4 + +# state symbolic variables +x = ca.SX.sym('x') +y = ca.SX.sym('y') +theta = ca.SX.sym('theta') +states = ca.vertcat( + x, + y, + theta +) +n_states = states.numel() + +rot_2d_z = ca.vertcat( + ca.horzcat(cos(theta), -sin(theta)), + ca.horzcat(sin(theta), cos(theta)) + ) + +# control symbolic variables +v = ca.SX.sym('v') +omega = ca.SX.sym('omega') +controls = ca.vertcat( + v, + omega +) +n_controls = controls.numel() + +# matrix containing all states over all time steps +1 (each column is a state vector) +X = ca.SX.sym('X', n_states, N + 1) + +# matrix containing all control actions over all time steps (each column is an action vector) +U = ca.SX.sym('U', n_controls, N) + +last_v = ca.SX.sym('last_v') +last_omega = ca.SX.sym('last_omega') +last_controls = ca.vertcat( + last_v, + last_omega +) + +# column vector for storing initial state and target states + initial velocity +P = ca.SX.sym('P', n_states * (N+1) + n_controls) +measured_velo = P[-2:] + +# state weights matrix (Q_X, Q_Y, Q_THETA) +Q = ca.diagcat(Q_x, Q_y) + +# controls weights matrix +R = ca.diagcat(R1, R2) + +#Acceleration weith matrix +R_acc = ca.diagcat(Acc_R1, Acc_R2) + + +RHS = ca.vertcat(v*cos(theta), v*sin(theta), last_omega*alpha + (1-alpha)*omega) +motion_model = ca.Function('motion_model', [states, controls, last_controls], [RHS]) +# RHS_angle = ca.vertcat(x + v/omega*sin(theta + omega*step_horizon) - v/omega*sin(theta), +# y + v/omega*cos(theta) - v/omega*cos(theta + omega*step_horizon), +# theta + omega*step_horizon) + +# RHS_straight = ca.vertcat(x + v*cos(theta)*step_horizon, +# y + v*sin(theta)*step_horizon, +# theta) +# straight_motion = ca.Function('straight_mm', [states, controls], [RHS_straight]) +# curved_motion = ca.Function('curved_mm', [states, controls], [RHS_angle]) +# if_motion_model = ca.Function.if_else('motion_model_cond', curved_motion, straight_motion) +# motion_model = ca.Function("motion_model", [states, controls], [if_motion_model(ca.fabs(omega) > 1e-4, states, controls)]) +# motion_model = curved_motion + +theta_to_so2 = ca.Function('theta2rotm', [theta], [rot_2d_z]) + +cost_fn = 0 # cost function +g = X[:, 0] - P[:n_states] # constraints in the equation + + +def so2_error(ref, current): + rel_m = theta_to_so2(ref).T @ theta_to_so2(current) + return ca.atan2(rel_m[1, 0], rel_m[0, 0]) + +#for initial +k = 0 +st = X[:, k] +con = U[:, k] +last_vel = measured_velo +st_next = X[:, k+1] +cost_fn = cost_fn \ + + (st_next[:2] - P[n_states*(k+1):n_states*(k+2)-1]).T @ Q @ (st_next[:2] - P[n_states*(k+1):n_states*(k+2)-1]) \ + + con.T @ R @ con \ + + so2_error(P[n_states*(k+1) + 2], st_next[2]) * Q_theta * so2_error(P[n_states*(k+1) + 2], st_next[2]) +k1 = motion_model(st, con, last_vel) +k2 = motion_model(st + step_horizon/2*k1, con, last_vel) +k3 = motion_model(st + step_horizon/2*k2, con, last_vel) +k4 = motion_model(st + step_horizon * k3, con, last_vel) +st_next_RK4 = st + (step_horizon / 6) * (k1 + 2 * k2 + 2 * k3 + k4) +# st_next_int = motion_model(st, con) +g = ca.vertcat(g, st_next[:2] - st_next_RK4[:2]) +g = ca.vertcat(g, so2_error(st_next[2], st_next_RK4[2])) + +# runge kutta +for k in range(1, N): + st = X[:, k] + st_next = X[:, k+1] + + con = U[:, k] + last_vel = ca.vertcat(U[0, k-1], (1 - alpha) * U[1, k-1] + alpha * last_vel[1]) + cost_fn = cost_fn \ + + (st_next[:2] - P[n_states*(k+1):n_states*(k+2)-1]).T @ Q @ (st_next[:2] - P[n_states*(k+1):n_states*(k+2)-1]) \ + + con.T @ R @ con \ + + so2_error(P[n_states*(k+1) + 2], st_next[2]) * Q_theta * so2_error(P[n_states*(k+1) + 2], st_next[2]) + + k1 = motion_model(st, con, last_vel) + k2 = motion_model(st + step_horizon/2*k1, con, last_vel) + k3 = motion_model(st + step_horizon/2*k2, con, last_vel) + k4 = motion_model(st + step_horizon * k3, con, last_vel) + st_next_RK4 = st + (step_horizon / 6) * (k1 + 2 * k2 + 2 * k3 + k4) + # st_next_int = motion_model(st, con) + + g = ca.vertcat(g, st_next[:2] - st_next_RK4[:2]) + g = ca.vertcat(g, so2_error(st_next[2], st_next_RK4[2])) + + +for k in range(N): + theta_k = P[n_states*(k+1) + 2] + g = ca.vertcat(g, ca.vertcat(-sin(theta_k), cos(theta_k)).T @ (X[:2, k] - P[n_states*(k+1): n_states*(k+1)+2])) + +#Acceleration constraints +cost_fn += (U[:, 0] - measured_velo).T @ R_acc @ (U[:, 0] - measured_velo) +for k in range(1, N): + cost_fn += (U[:, k] - U[:, k-1]).T @ R_acc @ (U[:, k] - U[:, k-1]) + + +OPT_variables = ca.vertcat( + X.reshape((-1, 1)), # Example: 3x11 ---> 33x1 where 3=states, 11=N+1 + U.reshape((-1, 1)) +) +nlp_prob = { + 'f': cost_fn, + 'x': OPT_variables, + 'g': g, + 'p': P +} + +opts = { + 'ipopt': { + 'max_iter': 2000, + 'print_level': 0, + 'acceptable_tol': 1e-5, + 'acceptable_obj_change_tol': 1e-4 + }, + 'print_time': 0 +} + +solver = ca.nlpsol('solve_unicycle_mpc', 'ipopt', nlp_prob, opts) diff --git a/main/src/vtr_path_planning/src/cbit/cbit.cpp b/main/src/vtr_path_planning/src/cbit/cbit.cpp index 12572545a..c87ed43f6 100644 --- a/main/src/vtr_path_planning/src/cbit/cbit.cpp +++ b/main/src/vtr_path_planning/src/cbit/cbit.cpp @@ -18,7 +18,6 @@ */ #include "vtr_path_planning/cbit/cbit.hpp" -#include "vtr_path_planning/mpc/mpc_path_planner.hpp" #include #include @@ -60,8 +59,6 @@ auto CBIT::Config::fromROS(const rclcpp::Node::SharedPtr& node, const std::strin config->pre_seed_resolution = node->declare_parameter(prefix + ".cbit.pre_seed_resolution", config->pre_seed_resolution); config->alpha = node->declare_parameter(prefix + ".cbit.alpha", config->alpha); config->q_max = node->declare_parameter(prefix + ".cbit.q_max", config->q_max); - config->frame_interval = node->declare_parameter(prefix + ".cbit.frame_interval", config->frame_interval); // going to get rid of this - config->iter_max = node->declare_parameter(prefix + ".cbit.iter_max", config->iter_max); // going to get rid of this config->eta = node->declare_parameter(prefix + ".cbit.eta", config->eta); config->rad_m_exhange = node->declare_parameter(prefix + ".rad_m_exhange", config->rad_m_exhange); config->initial_exp_rad = node->declare_parameter(prefix + ".cbit.initial_exp_rad", config->initial_exp_rad); @@ -78,34 +75,14 @@ auto CBIT::Config::fromROS(const rclcpp::Node::SharedPtr& node, const std::strin // CONTROLLER PARAMS config->extrapolate_robot_pose = node->declare_parameter(prefix + ".mpc.extrapolate_robot_pose", config->extrapolate_robot_pose); config->mpc_verbosity = node->declare_parameter(prefix + ".mpc.mpc_verbosity", config->mpc_verbosity); - config->homotopy_guided_mpc = node->declare_parameter(prefix + ".mpc.homotopy_guided_mpc", config->homotopy_guided_mpc); - config->horizon_steps = node->declare_parameter(prefix + ".mpc.horizon_steps", config->horizon_steps); - config->horizon_step_size = node->declare_parameter(prefix + ".mpc.horizon_step_size", config->horizon_step_size); config->forward_vel = node->declare_parameter(prefix + ".mpc.forward_vel", config->forward_vel); config->max_lin_vel = node->declare_parameter(prefix + ".mpc.max_lin_vel", config->max_lin_vel); config->max_ang_vel = node->declare_parameter(prefix + ".mpc.max_ang_vel", config->max_ang_vel); + config->max_lin_acc = node->declare_parameter(prefix + ".mpc.max_lin_acc", config->max_lin_acc); + config->max_ang_acc = node->declare_parameter(prefix + ".mpc.max_ang_acc", config->max_ang_acc); config->robot_linear_velocity_scale = node->declare_parameter(prefix + ".mpc.robot_linear_velocity_scale", config->robot_linear_velocity_scale); config->robot_angular_velocity_scale = node->declare_parameter(prefix + ".mpc.robot_angular_velocity_scale", config->robot_angular_velocity_scale); - // MPC COST FUNCTION COVARIANCE - const auto pose_error_diag = node->declare_parameter>(prefix + ".mpc.pose_error_cov", std::vector()); - config->pose_error_cov.diagonal() << pose_error_diag[0], pose_error_diag[1], pose_error_diag[2], pose_error_diag[3], pose_error_diag[4], pose_error_diag[5]; - const auto vel_error_diag = node->declare_parameter>(prefix + ".mpc.vel_error_cov", std::vector()); - config->vel_error_cov.diagonal() << vel_error_diag[0], vel_error_diag[1]; - const auto acc_error_diag = node->declare_parameter>(prefix + ".mpc.acc_error_cov", std::vector()); - config->acc_error_cov.diagonal() << acc_error_diag[0], acc_error_diag[1]; - const auto kin_error_diag = node->declare_parameter>(prefix + ".mpc.kin_error_cov", std::vector()); - config->kin_error_cov.diagonal() << kin_error_diag[0], kin_error_diag[1], kin_error_diag[2], kin_error_diag[3], kin_error_diag[4], kin_error_diag[5]; - const auto lat_error_diag = node->declare_parameter>(prefix + ".mpc.lat_error_cov", std::vector()); - config->lat_error_cov.diagonal() << lat_error_diag[0]; - - // MPC COST FUNCTION WEIGHTS - config->pose_error_weight = node->declare_parameter(prefix + ".mpc.pose_error_weight", config->pose_error_weight); - config->vel_error_weight = node->declare_parameter(prefix + ".mpc.vel_error_weight", config->vel_error_weight); - config->acc_error_weight = node->declare_parameter(prefix + ".mpc.acc_error_weight", config->acc_error_weight); - config->kin_error_weight = node->declare_parameter(prefix + ".mpc.kin_error_weight", config->kin_error_weight); - config->lat_error_weight = node->declare_parameter(prefix + ".mpc.lat_error_weight", config->lat_error_weight); - // MISC config->command_history_length = node->declare_parameter(prefix + ".mpc.command_history_length", config->command_history_length); @@ -117,23 +94,14 @@ auto CBIT::Config::fromROS(const rclcpp::Node::SharedPtr& node, const std::strin } - - - - - // Declare class as inherited from the BasePathPlanner CBIT::CBIT(const Config::ConstPtr& config, const RobotState::Ptr& robot_state, const Callback::Ptr& callback) - : BasePathPlanner(config, robot_state, callback), config_(config) { + : BasePathPlanner(config, robot_state, callback), config_(config), solver_{config_->mpc_verbosity} { CLOG(INFO, "cbit.path_planning") << "Constructing the CBIT Class"; robot_state_ = robot_state; const auto node = robot_state->node.ptr(); - // Initialize the shared pointer to the output of the planner - cbit_path_ptr = std::make_shared> (cbit_path); - valid_solution_ptr = std::make_shared (false); - q_max_ptr = std::make_shared (config->q_max); // Create visualizer and its corresponding pointer: VisualizationUtils visualization_utils(node); @@ -156,8 +124,6 @@ CBIT::CBIT(const Config::ConstPtr& config, cbit_config.pre_seed_resolution = config->pre_seed_resolution; cbit_config.alpha = config->alpha; cbit_config.q_max = config->q_max; - cbit_config.frame_interval = config->frame_interval; - cbit_config.iter_max = config->iter_max; cbit_config.eta = config->eta; cbit_config.rad_m_exhange = config->rad_m_exhange; cbit_config.initial_exp_rad = config->initial_exp_rad; @@ -167,26 +133,26 @@ CBIT::CBIT(const Config::ConstPtr& config, cbit_config.incremental_plotting = config->incremental_plotting; cbit_config.plotting = config->plotting; CLOG(INFO, "cbit.path_planning") << "Successfully Constructed the CBIT Class"; +} - - // Initialize the current velocity state and a vector for storing a history of velocity commands applied - applied_vel << 0, - 0; - vel_history.reserve(config_->command_history_length); - for (int i = 0; i < config_->command_history_length; i++) - { - vel_history.push_back(applied_vel); +void CBIT::setRunning(const bool running) { + //Starting up + if (running_ == false && running == true) { + CLOG(INFO, "cbit.path_planning") << "Initializing CBIT Route"; + initializeRoute(*robot_state_); + CLOG(INFO, "cbit.path_planning") << "CBIT Plan Completed"; + } else if (running_ == true && running == false) { + CLOG(INFO, "cbit.path_planning") << "Stopping CBIT Planning"; + planner_ptr_->stopPlanning(); + if (process_thread_cbit_.joinable()) process_thread_cbit_.join(); + thread_count_--; + planner_ptr_->resetPlanner(); + // planner_ptr_.reset(); + CLOG(INFO, "cbit.path_planning") << "Stopped CBIT Planning"; } - - thread_count_ = 2; - process_thread_cbit_ = std::thread(&CBIT::process_cbit, this); + BasePathPlanner::setRunning(running); } - - - - - CBIT::~CBIT() { stop_cbit(); } void CBIT::stop_cbit() { @@ -218,14 +184,11 @@ void CBIT::process_cbit() { return; } - // Planner should not require the thread lock to execute lock.unlock(); // Note we need to run the above first so that the lidarcbit class can be constructed before calling initializeroute (so it can be overrided correctly) - CLOG(INFO, "cbit.path_planning") << "Initializing CBIT Route"; - initializeRoute(*robot_state_); - CLOG(INFO, "cbit.path_planning") << "CBIT Plan Completed"; + } } @@ -236,10 +199,21 @@ void CBIT::process_cbit() { void CBIT::initializeRoute(RobotState& robot_state) { auto& chain = *robot_state.chain; - // Wait until the chain becomes localized - while (!chain.isLocalized()) + + // Initialize the shared pointer to the output of the planner + cbit_path_ptr = std::make_shared> (cbit_path); + valid_solution_ptr = std::make_shared (false); + q_max_ptr = std::make_shared (config_->q_max); + + // Initialize the current velocity state and a vector for storing a history of velocity commands applied + applied_vel_ << 0, + 0; + vel_history.reserve(config_->command_history_length); + for (int i = 0; i < config_->command_history_length; i++) { + vel_history.push_back(applied_vel_); } + robot_poses.clear(); lgmath::se3::TransformationWithCovariance teach_frame; std::tuple se3_vector; @@ -256,94 +230,95 @@ void CBIT::initializeRoute(RobotState& robot_state) { } - // Using two consecutive poses on the path, we need to try to determine which direction the repeat is going before we generate the pq space: - const auto chain_info = getChainInfo(robot_state); - auto [stamp, w_p_r_in_r, T_p_r, T_w_p, T_w_v_odo, T_r_v_odo, curr_sid] = chain_info; - auto world_frame_pose = T2xyzrpy(T_w_p * T_p_r); - auto first_pose = euclid_path_vec[0]; - auto second_pose = euclid_path_vec[1]; - auto path_yaw = std::atan2((second_pose.y-first_pose.y),(second_pose.x-first_pose.x)); - auto pose_graph_yaw = std::get<5>(world_frame_pose); - - // Logic for determining the forward/reverse sign: - PathDirection path_direction; //1.0 = forward planning, -1.0 = reverse planning - if (abs((abs(path_yaw) - abs(pose_graph_yaw))) > 1.57075) - { - path_direction = PATH_DIRECTION_REVERSE; - } - else - { - path_direction = PATH_DIRECTION_FORWARD; - } - - CLOG(INFO, "cbit.path_planning") << "The path repeat direction is:" << path_direction; CLOG(INFO, "cbit.path_planning") << "Trying to create global path"; // Create the path class object (Path preprocessing) - CBITPath global_path(cbit_config, euclid_path_vec); - // Make a pointer to this path - global_path_ptr = std::make_shared(global_path); + global_path_ptr = std::make_shared(cbit_config, euclid_path_vec); CLOG(INFO, "cbit.path_planning") << "Teach Path has been pre-processed. Attempting to initialize the dynamic corridor"; // Initialize the dynamic corridor - CBITCorridor corridor(cbit_config, global_path_ptr); - // Make a pointer to the corridor - corridor_ptr = std::make_shared(corridor); + corridor_ptr = std::make_shared(cbit_config, global_path_ptr); CLOG(INFO, "cbit.path_planning") << "Corridor generated successfully. Attempting to instantiate the planner"; // Instantiate the planner - CBITPlanner cbit(cbit_config, global_path_ptr, robot_state, cbit_path_ptr, costmap_ptr, corridor_ptr, valid_solution_ptr, q_max_ptr, path_direction); + planner_ptr_ = std::make_shared(cbit_config, global_path_ptr, robot_state, cbit_path_ptr, costmap_ptr, corridor_ptr, valid_solution_ptr, q_max_ptr); CLOG(INFO, "cbit.path_planning") << "Planner successfully created and resolved"; + thread_count_++; + process_thread_cbit_ = std::thread(&CBITPlanner::plan, planner_ptr_); + } +auto CBIT::computeCommand(RobotState& robot_state) -> Command { + auto raw_command = computeCommand_(robot_state); + + Eigen::Vector2d output_vel = {raw_command.linear.x, raw_command.angular.z}; + // Apply robot motor controller calibration scaling factors if applicable + output_vel(0) = output_vel(0) * config_->robot_linear_velocity_scale; + output_vel(1) = output_vel(1) * config_->robot_angular_velocity_scale; + // If required, saturate the output velocity commands based on the configuration limits + CLOG(DEBUG, "cbit.control") << "Saturating the velocity command if required"; + Eigen::Vector2d saturated_vel = saturateVel(output_vel, config_->max_lin_vel, config_->max_ang_vel); + CLOG(INFO, "cbit.control") << "The Saturated linear velocity is: " << saturated_vel(0) << " The angular vel is: " << saturated_vel(1); + + Command command; + command.linear.x = saturated_vel(0); + command.angular.z = saturated_vel(1); + prev_vel_stamp_ = now(); + applied_vel_ = saturated_vel; + + // Store the result in memory so we can use previous state values to re-initialize and extrapolate the robot pose in subsequent iterations + vel_history.erase(vel_history.begin()); + vel_history.push_back(applied_vel_); + + CLOG(INFO, "cbit.control") + << "Final control command: [" << command.linear.x << ", " + << command.linear.y << ", " << command.linear.z << ", " + << command.angular.x << ", " << command.angular.y << ", " + << command.angular.z << "] for timestamp: " << prev_cost_stamp_; + + return command; +} // Generate twist commands to track the planned local path (function is called at the control rate) -auto CBIT::computeCommand(RobotState& robot_state) -> Command { - auto& chain = *robot_state.chain; - if (!chain.isLocalized()) { +auto CBIT::computeCommand_(RobotState& robot_state) -> Command { + auto& chain = robot_state.chain.ptr(); + if (!chain->isLocalized()) { CLOG(WARNING, "cbit.control") << "Robot is not localized, commanding the robot to stop"; - applied_vel << 0.0, 0.0; - // Update history: - vel_history.erase(vel_history.begin()); - vel_history.push_back(applied_vel); return Command(); } - // retrieve the transorm info from the localization chain for the current robot state - const auto chain_info = getChainInfo(robot_state); - auto [stamp, w_p_r_in_r, T_p_r, T_w_p, T_w_v_odo, T_r_v_odo, curr_sid] = chain_info; + // retrieve the transform info from the localization chain for the current robot state + const auto [stamp, w_p_r_in_r, T_p_r, T_w_p, T_w_v_odo, T_r_v_odo, curr_sid] = getChainInfo(*chain); + // Store the current robot state in the robot state path so it can be visualized + auto T_w_r = T_w_p * T_p_r; + robot_poses.push_back(T_w_r); // Handling Dynamic Corridor Widths: // Retrieve the terrain type (corresponds to maximum planning width) - if (curr_sid <= chain.size()-2) + if (curr_sid <= chain->size() - 2) { CLOG(DEBUG, "cbit.control") << "Trying to query the graph vertex type for SID: " << curr_sid; - int vertex_type = chain.query_terrain_type(curr_sid); - CLOG(DEBUG, "cbit.control") << "GUI Vertex Type is: " << (vertex_type + 1); // we may want to look ahead a few frames instead - - // Type 1 in GUI is default and returns the value 0. This should correspond to 2.5m maximum lateral deviation. - // Each subsequent type maps to Type # - 1, i.e. Type 2 path = 1, Type 3 = 2, and so on. + int vertex_type = chain->query_terrain_type(curr_sid); // Calculate q_max width for planner - *q_max_ptr = 2.5 - (0.5 * vertex_type); - // Note: Minimum should never go to exactly zero or this could cause issues in the planner (0.01 is fine) - // TODO: Update the type values in the GUI to reflect this change + *q_max_ptr = pose_graph::BasicPathBase::terrian_type_corridor_width(vertex_type); + CLOG(DEBUG, "cbit.control") << "Vertex Corridor Width is: " << *q_max_ptr; // we may want to look ahead a few frames instead + } // Retrieve the latest obstacle costmap - bool obstacle_avoidance = config_->obstacle_avoidance; - if ((prev_stamp != stamp) && (obstacle_avoidance == true)) + if ((prev_cost_stamp_ != stamp) && (config_->obstacle_avoidance == true)) { std::lock_guard lock(robot_state.obsMapMutex); // Read or use output.obs_map safely auto obs_map = robot_state.obs_map; const auto costmap_sid = robot_state.costmap_sid; - const auto T_start_vertex = chain.pose(costmap_sid); + const auto T_start_vertex = chain->pose(costmap_sid); costmap_ptr->grid_resolution = robot_state.grid_resolution; CLOG(DEBUG, "cbit.obstacle_filtering") << "The size of the map is: " << obs_map.size(); @@ -366,11 +341,12 @@ auto CBIT::computeCommand(RobotState& robot_state) -> Command { // After that point, we then do a sliding window using shift operations, moving out the oldest map and appending the newest one else { + //TODO looks like this shold use a list, it looks like it's just overwriting the last one. costmap_ptr->obs_map_vect[config_->costmap_history-1] = obs_map; costmap_ptr->T_c_w_vect[config_->costmap_history-1] = costmap_ptr->T_c_w ; } } - prev_stamp = stamp; + prev_cost_stamp_ = stamp; // END OF OBSTACLE PERCEPTION UPDATES // Make sure there is a valid solution, else stop the robot @@ -382,247 +358,119 @@ auto CBIT::computeCommand(RobotState& robot_state) -> Command { // START OF MPC CODE // Dont proceed to mpc control unless we have a valid plan to follow from BIT*, else return a 0 velocity command to stop and wait - if ((*cbit_path_ptr).size() != 0) + if (cbit_path_ptr->size() != 0) { - CLOG(DEBUG, "cbit.debug") << "History of the Robot Velocities:" << vel_history; + + CasadiUnicycleMPC::Config mpcConfig; + mpcConfig.vel_max = {config_->max_lin_vel, config_->max_ang_vel}; + // Initializations from config - bool extrapolate_robot_pose = config_->extrapolate_robot_pose; - bool mpc_verbosity = config_->mpc_verbosity; - bool homotopy_guided_mpc = config_->homotopy_guided_mpc; - int K = config_->horizon_steps; // Horizon steps - double DT = config_->horizon_step_size; // Horizon step size - double VF = config_->forward_vel; // Desired Forward velocity set-point for the robot. MPC will try to maintain this rate while balancing other constraints // Schedule speed based on path curvatures + other factors - VF = ScheduleSpeed(global_path_ptr->disc_path_curvature_xy, global_path_ptr->disc_path_curvature_xz_yz, VF, curr_sid, config_->planar_curv_weight, config_->profile_curv_weight, config_->eop_weight, config_->horizon_step_size, config_->min_vel); - - // Grab the current MPC configurations - // Pose Covariance Weights - Eigen::Matrix pose_noise_vect; - pose_noise_vect = config_->pose_error_cov; - - // Disturbance Velocity Covariance - Eigen::Matrix vel_noise_vect; - vel_noise_vect = config_->vel_error_cov; - - // Acceleration Tuning - Eigen::Matrix accel_noise_vect; - accel_noise_vect = config_->acc_error_cov; - - // Kinematics Covariance Weights (should be weighted quite heavily (smaller is higher because its covariance)) - Eigen::Matrix kin_noise_vect; - kin_noise_vect = config_->kin_error_cov; - - // Lateral Covariance Weights (should be weighted quite heavily (smaller is higher because its covariance)) - Eigen::Matrix lat_noise_vect; - lat_noise_vect = config_->lat_error_cov; - - // Cost term weights - double pose_error_weight = config_->pose_error_weight; - double vel_error_weight = config_->vel_error_weight; - double acc_error_weight = config_->acc_error_weight; - double kin_error_weight = config_->kin_error_weight; - double lat_error_weight = config_->lat_error_weight; - - + // TODO refactor to accept the chain and use the curvature of the links + mpcConfig.VF = ScheduleSpeed(chain, {config_->forward_vel, config_->min_vel, config_->planar_curv_weight, config_->profile_curv_weight, config_->eop_weight, 7}); + // EXTRAPOLATING ROBOT POSE INTO THE FUTURE TO COMPENSATE FOR SYSTEM DELAYS + // Removing for now. I'm not sure this is a good idea with noisy velocity estimates auto T_p_r_extp = T_p_r; - auto T_p_r_prop = T_p_r; - if (extrapolate_robot_pose == true) - { + if (config_->extrapolate_robot_pose) { const auto curr_time = now(); // always in nanoseconds - const auto dt = static_cast(curr_time - stamp) * 1e-9; - - // Check the time past since the last state update was received - // Go back through the vel_history to approximately dt seconds in the past - // Start applying each of the applied velocities sequentially - double control_period = config_->control_period / 1000.0; // control period is given by user in ms in the config - for (int i=std::floor(dt / control_period); i > 0; i--) - { - w_p_r_in_r(0) = -1* vel_history[vel_history.size()-(i+1)][0]; - w_p_r_in_r(1) = 0.0; - w_p_r_in_r(2) = 0.0; - w_p_r_in_r(3) = 0.0; - w_p_r_in_r(4) = 0.0; - w_p_r_in_r(5) = -1* vel_history[vel_history.size()-(i+1)][1]; - CLOG(DEBUG, "cbit.debug") << "Robot velocity Used for Extrapolation: " << -w_p_r_in_r.transpose() << std::endl; - - Eigen::Matrix xi_p_r_in_r(control_period * w_p_r_in_r); - T_p_r_prop = T_p_r_prop * tactic::EdgeTransform(xi_p_r_in_r).inverse(); - + auto dt = static_cast(curr_time - stamp) * 1e-9 - 0.05; + if (fabs(dt) > 0.25) { + CLOG(WARNING, "cbit") << "Pose extrapolation was requested but the time delta is " << dt << "s.\n" + << "Ignoring extrapolation requestion. Check your time sync!"; + dt = 0; } - // Apply the final partial period velocity - w_p_r_in_r(0) = -1* vel_history.back()[0]; - w_p_r_in_r(1) = 0.0; - w_p_r_in_r(2) = 0.0; - w_p_r_in_r(3) = 0.0; - w_p_r_in_r(4) = 0.0; - w_p_r_in_r(5) = -1* vel_history.back()[1]; - CLOG(DEBUG, "cbit.debug") << "Robot velocity Used for Extrapolation: " << -w_p_r_in_r.transpose() << std::endl; - Eigen::Matrix xi_p_r_in_r((dt - (std::floor(dt / control_period) * control_period)) * w_p_r_in_r); - T_p_r_prop = T_p_r_prop * tactic::EdgeTransform(xi_p_r_in_r).inverse(); - CLOG(DEBUG, "cbit.debug") << "The final time period is: " << (dt - (std::floor(dt / control_period) * control_period)); - T_p_r_extp = T_p_r_prop; + + CLOG(DEBUG, "cbit.debug") << "Robot velocity Used for Extrapolation: " << -w_p_r_in_r.transpose() << " dt: " << dt << std::endl; + Eigen::Matrix xi_p_r_in_r(-dt * w_p_r_in_r); + T_p_r_extp = T_p_r * tactic::EdgeTransform(xi_p_r_in_r); CLOG(DEBUG, "cbit.debug") << "New extrapolated pose:" << T_p_r_extp; } - lgmath::se3::Transformation T0 = lgmath::se3::Transformation(T_w_p * T_p_r_extp); - - //Convert to x,y,z,roll, pitch, yaw - std::tuple robot_pose = T2xyzrpy(T0); - CLOG(DEBUG, "cbit.control") << "The Current Robot Pose (from planning) is - x: " << std::get<0>(robot_pose) << " y: " << std::get<1>(robot_pose) << " yaw: " << std::get<5>(robot_pose); - - CLOG(DEBUG, "cbit.control") << "The Current Robot State Transform is: : " << T0; - // Need to also invert the robot state to make it T_vi instead of T_iv as this is how the MPC problem is structured - lgmath::se3::Transformation T0_inv = T0.inverse(); - CLOG(DEBUG, "cbit.control") << "The Inverted Current Robot State Using Direct Robot Values is: " << T0_inv; - // END OF ROBOT POSE EXTRAPOLATION - + lgmath::se3::Transformation T0 = T_p_r_extp; + mpcConfig.T0 = tf_to_global(T0); + CLOG(DEBUG, "cbit.control") << "Last velocity " << w_p_r_in_r << " with stamp " << stamp; - // Calculate which T_ref poses to used based on the current path solution - CLOG(INFO, "cbit.control") << "Attempting to generate T_ref poses"; - auto ref_tracking_result = GenerateTrackingReference(cbit_path_ptr, robot_pose, K, DT, VF); - auto tracking_poses = ref_tracking_result.poses; - //bool point_stabilization = ref_tracking_result.point_stabilization; // No need to do both tracking and homotopy point stabilization - std::vector p_interp_vec = ref_tracking_result.p_interp_vec; - std::vector q_interp_vec = ref_tracking_result.q_interp_vec; + double state_p = findRobotP(T_w_p * T_p_r_extp, chain); - // Synchronized Tracking/Teach Reference Poses For Homotopy Guided MPC: - auto ref_homotopy_result = GenerateHomotopyReference(global_path_ptr, corridor_ptr, robot_pose, p_interp_vec); - auto homotopy_poses = ref_homotopy_result.poses; - bool point_stabilization = ref_homotopy_result.point_stabilization; - std::vector barrier_q_left = ref_homotopy_result.barrier_q_left; - std::vector barrier_q_right = ref_homotopy_result.barrier_q_right; - - std::vector tracking_pose_vec; - for (size_t i = 0; i < tracking_poses.size(); i++) { - tracking_pose_vec.push_back(tracking_poses[i].inverse()); + std::vector p_rollout; + for(int j = 1; j < mpcConfig.N+1; j++){ + p_rollout.push_back(state_p + j*mpcConfig.VF*mpcConfig.DT); } - std::vector homotopy_pose_vec; - for (size_t i = 0; i < homotopy_poses.size(); i++) { - homotopy_pose_vec.push_back(homotopy_poses[i].inverse()); + mpcConfig.reference_poses.clear(); + auto referenceInfo = generateHomotopyReference(p_rollout, chain); + for(const auto& Tf : referenceInfo.poses) { + mpcConfig.reference_poses.push_back(tf_to_global(T_w_p.inverse() * Tf)); + CLOG(DEBUG, "test") << "Target " << tf_to_global(T_w_p.inverse() * Tf); } - // Generate the mpc configuration structure: - MPCConfig mpc_config; - mpc_config.previous_vel = applied_vel; - mpc_config.T0 = T0; - mpc_config.tracking_reference_poses = tracking_poses; - if (homotopy_guided_mpc == true) - { - mpc_config.homotopy_reference_poses = homotopy_poses; - } - else - { - mpc_config.homotopy_reference_poses = tracking_poses; // if not using homotopy guided mpc, set the homotopy reference poses to be the tracking poses - } - mpc_config.barrier_q_left = barrier_q_left; - mpc_config.barrier_q_right = barrier_q_right; - mpc_config.K = K; - mpc_config.DT = DT; - mpc_config.VF = VF; - mpc_config.lat_noise_vect = lat_noise_vect; - mpc_config.pose_noise_vect = pose_noise_vect; - mpc_config.vel_noise_vect = vel_noise_vect; - mpc_config.accel_noise_vect = accel_noise_vect; - mpc_config.kin_noise_vect = kin_noise_vect; - mpc_config.point_stabilization = point_stabilization; - mpc_config.pose_error_weight = pose_error_weight; - mpc_config.vel_error_weight = vel_error_weight; - mpc_config.acc_error_weight = acc_error_weight; - mpc_config.kin_error_weight = kin_error_weight; - mpc_config.lat_error_weight = lat_error_weight; - mpc_config.verbosity = mpc_verbosity; - mpc_config.homotopy_mode = homotopy_guided_mpc; - - // Create and solve the STEAM optimization problem + mpcConfig.up_barrier_q = referenceInfo.barrier_q_max; + mpcConfig.low_barrier_q = referenceInfo.barrier_q_min; + + mpcConfig.previous_vel = {-w_p_r_in_r(0, 0), -w_p_r_in_r(5, 0)}; + + + // Create and solve the casadi optimization problem std::vector mpc_poses; - try - { + // return the computed velocity command for the first time step + Command command; + std::vector mpc_velocities; + try { CLOG(INFO, "cbit.control") << "Attempting to solve the MPC problem"; - auto MPCResult = SolveMPC(mpc_config); - applied_vel = MPCResult.applied_vel; // note dont re-declare applied vel here - mpc_poses = MPCResult.mpc_poses; + auto mpc_res = solver_.solve(mpcConfig); + + for(int i = 0; i < mpc_res["pose"].columns(); i++) { + const auto& pose_i = mpc_res["pose"](casadi::Slice(), i).get_elements(); + mpc_poses.push_back(T_w_p * tf_from_global(pose_i[0], pose_i[1], pose_i[2])); + } + CLOG(INFO, "cbit.control") << "Successfully solved MPC problem"; - } - catch(steam::unsuccessful_step &e) - { - CLOG(WARNING, "cbit.control") << "STEAM Optimization Failed; Commanding to Stop the Vehicle"; - applied_vel(0) = 0.0; - applied_vel(1) = 0.0; - } + const auto& mpc_vel_vec = mpc_res["vel"](casadi::Slice(), 0).get_elements(); - CLOG(INFO, "cbit.control") << "The linear velocity is: " << applied_vel(0) << " The angular vel is: " << applied_vel(1); + command.linear.x = mpc_vel_vec[0]; + command.angular.z = mpc_vel_vec[1]; - // Apply robot motor controller calibration scaling factors if applicable - applied_vel(0) = applied_vel(0) * config_->robot_linear_velocity_scale; - applied_vel(1) = applied_vel(1) * config_->robot_angular_velocity_scale; + // Get all the mpc velocities + for (int i = 0; i < mpc_res["vel"].columns(); i++) { + const auto& vel_i = mpc_res["vel"](casadi::Slice(), i).get_elements(); + mpc_velocities.emplace_back(vel_i[0], vel_i[1]); + } - // If required, saturate the output velocity commands based on the configuration limits - CLOG(INFO, "cbit.control") << "Saturating the velocity command if required"; - Eigen::Matrix saturated_vel = SaturateVel(applied_vel, config_->max_lin_vel, config_->max_ang_vel); - CLOG(INFO, "cbit.control") << "The Saturated linear velocity is: " << saturated_vel(0) << " The angular vel is: " << saturated_vel(1); - - // Store the result in memory so we can use previous state values to re-initialize and extrapolate the robot pose in subsequent iterations - vel_history.erase(vel_history.begin()); - vel_history.push_back(saturated_vel); + } catch(std::exception &e) { + CLOG(WARNING, "cbit.control") << "casadi failed! " << e.what() << " Commanding to Stop the Vehicle"; + return Command(); + } - // Store the current robot state in the robot state path so it can be visualized - robot_poses.push_back(T_w_p * T_p_r); - // visualize the outputs - visualization_ptr->visualize(stamp, T_w_p, T_p_r, T_p_r_extp, mpc_poses, robot_poses, tracking_pose_vec, homotopy_pose_vec, cbit_path_ptr, corridor_ptr); + CLOG(INFO, "cbit.control") << "The linear velocity is: " << command.linear.x << " The angular vel is: " << command.angular.z; - // return the computed velocity command for the first time step - Command command; - command.linear.x = saturated_vel(0); - command.angular.z = saturated_vel(1); - - // Flagged log messages to test calibration of the grizzly controller (or any future robot) - CLOG(DEBUG, "grizzly_controller_tests.cbit") << "Twist Linear Velocity: " << saturated_vel(0); - CLOG(DEBUG, "grizzly_controller_tests.cbit") << "Twist Angular Velocity: " << saturated_vel(1); + // grab elements for visualization + lgmath::se3::Transformation T_w_p_interpolated_closest_to_robot = interpolatedPose(state_p, chain); - - CLOG(INFO, "cbit.control") - << "Final control command: [" << command.linear.x << ", " - << command.linear.y << ", " << command.linear.z << ", " - << command.angular.x << ", " << command.angular.y << ", " - << command.angular.z << "]"; + // visualize the outputs + visualization_ptr->visualize(stamp, T_w_p, T_p_r, T_p_r_extp, T_w_r, mpc_poses, mpc_velocities, robot_poses, referenceInfo.poses, referenceInfo.poses, cbit_path_ptr, corridor_ptr, T_w_p_interpolated_closest_to_robot, state_p, global_path_ptr, curr_sid); return command; } // Otherwise stop the robot - else - { + else { CLOG(INFO, "cbit.control") << "There is not a valid plan yet, returning zero velocity commands"; - applied_vel << 0.0, 0.0; - vel_history.erase(vel_history.begin()); - vel_history.push_back(applied_vel); return Command(); } } -// Function for grabbing the robots velocity in planning frame, transform of robot into planning frame, and transform of planning frame to world frame -auto CBIT::getChainInfo(RobotState& robot_state) -> ChainInfo { - auto& chain = *robot_state.chain; - auto lock = chain.guard(); - const auto stamp = chain.leaf_stamp(); - const auto w_p_r_in_r = chain.leaf_velocity(); - const auto T_p_r = chain.T_leaf_trunk().inverse(); - const auto T_w_p = chain.T_start_trunk(); - const auto T_w_v_odo = chain.T_start_petiole(); - const auto T_r_v_odo = chain.T_leaf_petiole(); - const auto curr_sid = chain.trunkSequenceId(); - return ChainInfo{stamp, w_p_r_in_r, T_p_r, T_w_p, T_w_v_odo, T_r_v_odo, curr_sid}; +// Simple function for checking that the current output velocity command is saturated between our mechanical velocity limits +Eigen::Vector2d saturateVel(const Eigen::Vector2d& applied_vel, double v_lim, double w_lim) { + return {std::clamp(applied_vel(0), -v_lim, v_lim), std::clamp(applied_vel(1), -w_lim, w_lim)}; } - } // namespace path_planning } // namespace vtr \ No newline at end of file diff --git a/main/src/vtr_path_planning/src/cbit/cbit_path_planner.cpp b/main/src/vtr_path_planning/src/cbit/cbit_path_planner.cpp index 32193ebe9..34e12dd6d 100644 --- a/main/src/vtr_path_planning/src/cbit/cbit_path_planner.cpp +++ b/main/src/vtr_path_planning/src/cbit/cbit_path_planner.cpp @@ -35,20 +35,9 @@ inline std::tuple T2xyzrpy( } -auto CBITPlanner::getChainInfo(BasePathPlanner::RobotState& robot_state) -> ChainInfo { - auto& chain = *robot_state.chain; - auto lock = chain.guard(); - const auto stamp = chain.leaf_stamp(); - const auto w_p_r_in_r = chain.leaf_velocity(); - const auto T_p_r = chain.T_leaf_trunk().inverse(); - const auto T_w_p = chain.T_start_trunk(); - const auto curr_sid = chain.trunkSequenceId(); - return ChainInfo{stamp, w_p_r_in_r, T_p_r, T_w_p, curr_sid}; -} - // Class Constructor: -CBITPlanner::CBITPlanner(CBITConfig conf_in, std::shared_ptr path_in, BasePathPlanner::RobotState& robot_state, std::shared_ptr> path_ptr, std::shared_ptr costmap_ptr, std::shared_ptr corridor_ptr, std::shared_ptr solution_ptr, std::shared_ptr width_ptr, PathDirection path_direction) -{ +CBITPlanner::CBITPlanner(CBITConfig conf_in, std::shared_ptr path_in, BasePathPlanner::RobotState& robot_state, std::shared_ptr> path_ptr, std::shared_ptr costmap_ptr, std::shared_ptr corridor_ptr, std::shared_ptr solution_ptr, std::shared_ptr width_ptr) +: robot_state_{robot_state} { // Setting random seed srand((unsigned int)time(NULL)); @@ -58,13 +47,6 @@ CBITPlanner::CBITPlanner(CBITConfig conf_in, std::shared_ptr path_in, valid_solution_ptr = solution_ptr; q_max_ptr = width_ptr; - // Before beginning the planning phase, we need to wait for the robot to localize, and then update the goal state - auto& chain = *robot_state.chain; - do - { - CLOG(WARNING, "cbit_planner.path_planning") << "Robot is not localized, Planner is waiting to start"; - } while (chain.isLocalized() == 0); - CLOG(INFO, "cbit_planner.path_planning") << "Planner is trying to initialize"; conf = conf_in; @@ -79,7 +61,6 @@ CBITPlanner::CBITPlanner(CBITConfig conf_in, std::shared_ptr path_in, //initialize_plot(); InitializePlanningSpace(); - Planning(robot_state, costmap_ptr, corridor_ptr, path_direction); } void CBITPlanner::InitializePlanningSpace() @@ -118,8 +99,13 @@ void CBITPlanner::InitializePlanningSpace() } // If we ever exit the planner due to a fault, we will do a hard reset, everything but the current robot_state (p_goal) and the inputs will be reinitialized -void CBITPlanner::ResetPlanner() +void CBITPlanner::stopPlanning() { + planning_active_ = false; +} + +void CBITPlanner::resetPlanner() { + stopPlanning(); tree.V.clear(); tree.E.clear(); tree.QV2.clear(); @@ -136,13 +122,14 @@ void CBITPlanner::ResetPlanner() // Main planning function -void CBITPlanner::Planning(BasePathPlanner::RobotState& robot_state, std::shared_ptr costmap_ptr, std::shared_ptr corridor_ptr, PathDirection path_direction) -{ +void CBITPlanner::plan() { double prev_path_cost = INFINITY; int compute_time = 0; int vertex_rej_prob = 100; int dyn_batch_samples = conf.batch_samples; + planning_active_ = true; + // Grab the amount of time in ms between robot state updates int control_period_ms = (1.0 / conf.state_update_freq) * 1000.0; auto state_update_time = std::chrono::high_resolution_clock::now() + std::chrono::milliseconds(control_period_ms); @@ -158,7 +145,7 @@ void CBITPlanner::Planning(BasePathPlanner::RobotState& robot_state, std::shared bool localization_flag = true; // Set the fact we are localized if we make it to this point - for (int k = 0; k < conf.iter_max; k++) + for (int k = 0; planning_active_; k++) { // Check whether a robot state update should be applied // We only update the state if A: we have first found a valid initial solution, and B: if the current time has elapsed the control period @@ -172,9 +159,8 @@ void CBITPlanner::Planning(BasePathPlanner::RobotState& robot_state, std::shared state_update_time = std::chrono::high_resolution_clock::now() + std::chrono::milliseconds(control_period_ms); // get the euclidean robot state in the world frame from vt&r - auto& chain = *robot_state.chain; - if (chain.isLocalized() == 0) - { + auto& chain = *robot_state_.chain; + if (chain.isLocalized() == 0) { // If we ever become unlocalized, I think we just need to break, then set a flag to exit the outer loop // This also triggers after we reach end of path and effectively shuts down the planner localization_flag = false; @@ -184,9 +170,9 @@ void CBITPlanner::Planning(BasePathPlanner::RobotState& robot_state, std::shared std::tuple robot_pose; - const auto chain_info = getChainInfo(robot_state); - auto [stamp, w_p_r_in_r, T_p_r, T_w_p, curr_sid] = chain_info; - robot_pose= T2xyzrpy(T_w_p * T_p_r); + const auto [stamp, w_p_r_in_r, T_p_r, T_w_p, T_w_v_odo, T_r_v_odo, curr_sid] = getChainInfo(chain); + + robot_pose = T2xyzrpy(T_w_p * T_p_r); CLOG(INFO, "cbit_planner.path_planning") << "Displaying Current Robot Transform: " << T_p_r; Pose se3_robot_pose = Pose(std::get<0>(robot_pose),(std::get<1>(robot_pose)),std::get<2>(robot_pose),std::get<3>(robot_pose),std::get<4>(robot_pose),std::get<5>(robot_pose)); @@ -318,7 +304,7 @@ void CBITPlanner::Planning(BasePathPlanner::RobotState& robot_state, std::shared m = dyn_batch_samples; // Extract the solution - std::tuple, std::vector> curv_path = ExtractPath(robot_state, costmap_ptr); + std::tuple, std::vector> curv_path = ExtractPath(robot_state_, cbit_costmap_ptr); path_x = std::get<0>(curv_path); // p coordinates of the current path path_y = std::get<1>(curv_path); // q coordinates of the current path @@ -394,9 +380,8 @@ void CBITPlanner::Planning(BasePathPlanner::RobotState& robot_state, std::shared double p_val = p_goal->p; for (int i = 0; i < (pre_seeds-1); i++) { - Node node((p_val+p_step), 0); - samples.push_back(std::make_shared (node)); - p_val = p_val + p_step; + samples.push_back(std::make_shared(p_val + p_step , 0)); + p_val += p_step; } } @@ -419,14 +404,13 @@ void CBITPlanner::Planning(BasePathPlanner::RobotState& robot_state, std::shared // Resample if (p_goal->g_T_weighted < INFINITY) { - std::vector> new_samples = SampleBox(m); + Node::Path new_samples = SampleBox(m); samples.insert(samples.end(), new_samples.begin(), new_samples.end()); CLOG(INFO, "cbit_planner.path_planning") << "Sampling Box"; } - else { - std::vector> new_samples = SampleFreeSpace(m); + Node::Path new_samples = SampleFreeSpace(m); samples.insert(samples.end(), new_samples.begin(), new_samples.end()); CLOG(INFO, "cbit_planner.path_planning") << "Sample Free Space"; } @@ -457,12 +441,12 @@ void CBITPlanner::Planning(BasePathPlanner::RobotState& robot_state, std::shared // if we have no 1st batch solution (we are in the first iteration or have just reset), add the whole tree to the queue if (k == 0) { - tree.QV2.insert(std::pair>((tree.V[i]->g_T_weighted + h_estimated_admissible(*tree.V[i], *p_goal)), tree.V[i])); + tree.QV2.insert(std::pair((tree.V[i]->g_T_weighted + h_estimated_admissible(*tree.V[i], *p_goal)), tree.V[i])); } // Otherwise, only add the portions of the tree within the sliding window to avoid processing preseeded vertices which are already optimal else if (((vertex_rej_prob / random_integer) >= 1.0)) // for some reason using the lookahead queue doesnt work reliably for collisions, not sure why, need to investigate { - tree.QV2.insert(std::pair>((tree.V[i]->g_T_weighted + h_estimated_admissible(*tree.V[i], *p_goal)), tree.V[i])); + tree.QV2.insert(std::pair((tree.V[i]->g_T_weighted + h_estimated_admissible(*tree.V[i], *p_goal)), tree.V[i])); } } @@ -494,7 +478,7 @@ void CBITPlanner::Planning(BasePathPlanner::RobotState& robot_state, std::shared // Generate prospective nodes - std::tuple, std::shared_ptr> prospective_edge = BestInEdgeQueue(); + Node::Edge prospective_edge = BestInEdgeQueue(); std::shared_ptr vm = std::get<0>(prospective_edge); std::shared_ptr xm = std::get<1>(prospective_edge); @@ -576,7 +560,7 @@ void CBITPlanner::Planning(BasePathPlanner::RobotState& robot_state, std::shared xm->parent = vm; tree.V.push_back(xm); - tree.QV2.insert(std::pair>(xm->g_T_weighted + h_estimated_admissible(*xm, *p_goal), xm)); + tree.QV2.insert(std::pair(xm->g_T_weighted + h_estimated_admissible(*xm, *p_goal), xm)); } stop_bench_time = std::chrono::high_resolution_clock::now(); duration_bench = std::chrono::duration_cast(stop_bench_time - start_bench_time); @@ -624,21 +608,16 @@ void CBITPlanner::Planning(BasePathPlanner::RobotState& robot_state, std::shared } // End of main planning for loop - // Exiting cleanly: - // If we make it here and are no longer localized, we've reached end of path and should clear the bitstar path from memory - if (localization_flag == false) - { - CLOG(ERROR, "cbit_planner.path_planning") << "Reached End of Plan, Exiting cleanly"; - (*cbit_path_ptr).clear(); - } + CLOG(INFO, "cbit_planner.path_planning") << "Reached End of Plan, Exiting cleanly"; + cbit_path_ptr->clear(); } // End of main planning function -std::vector> CBITPlanner::SampleBox(int m) +Node::Path CBITPlanner::SampleBox(int m) { // Create a vector to store the pointers to the new samples we are going to generate. - std::vector> new_samples; + Node::Path new_samples; // Initialize sample box parameters @@ -683,7 +662,7 @@ std::vector> CBITPlanner::SampleBox(int m) } else { - new_samples.push_back(std::make_shared (node)); + new_samples.push_back(std::make_shared(node)); ind++; } } @@ -692,13 +671,13 @@ std::vector> CBITPlanner::SampleBox(int m) -std::vector> CBITPlanner::SampleFreeSpace(int m) +Node::Path CBITPlanner::SampleFreeSpace(int m) { - std::vector> new_samples; + Node::Path new_samples; double p_max = p_goal->p + dynamic_window_width + conf.sliding_window_freespace_padding; double p_zero = p_goal->p - conf.sliding_window_freespace_padding; - double q_max = (*q_max_ptr); + double q_max = *q_max_ptr; int ind = 0; while (ind < m) @@ -731,10 +710,9 @@ std::vector> CBITPlanner::SampleFreeSpace(int m) double p_val = p_zero; for (int i = 0; i < (pre_seeds-1); i++) { - Node node((p_val+p_step), 0); - new_samples.push_back(std::make_shared (node)); + new_samples.push_back(std::make_shared(p_val+p_step, 0.0)); - p_val = p_val + p_step; + p_val += p_step; } } return new_samples; @@ -777,23 +755,23 @@ void CBITPlanner::Prune(double c_best, double c_best_weighted) { if (f_estimated(*samples[i], *p_start, *p_goal, conf.alpha) < cost_threshold)// Also handles inf flagged values { - samples_pruned.push_back(std::shared_ptr (samples[i])); + samples_pruned.push_back(samples[i]); } } // We also check the tree and add samples for unconnected vertices back to the sample set - std::vector> vertex_pruned; + std::vector vertex_pruned; int vertices_size = tree.V.size(); for (size_t i = 0; i < vertices_size; i++) { if (tree.V[i]->g_T_weighted == INFINITY) { - samples_pruned.push_back(std::shared_ptr (tree.V[i])); + samples_pruned.push_back(tree.V[i]); } if ((f_estimated(*tree.V[i], *p_start, *p_goal, conf.alpha) <= cost_threshold) && (tree.V[i]->g_T_weighted < INFINITY)) { - vertex_pruned.push_back(std::shared_ptr (tree.V[i])); + vertex_pruned.push_back(tree.V[i]); } } // After we do both the above loops update the sample vector @@ -802,13 +780,13 @@ void CBITPlanner::Prune(double c_best, double c_best_weighted) // Similar Prune of the Edges - std::vector, std::shared_ptr>> edge_pruned; + std::vector> edge_pruned; for (size_t i = 0; i (tree.E[i]), *p_start, *p_goal, conf.alpha) <= cost_threshold) && (f_estimated(*std::get<1>(tree.E[i]), *p_start, *p_goal, conf.alpha) <= cost_threshold)) { - edge_pruned.push_back(std::tuple, std::shared_ptr> (tree.E[i])); + edge_pruned.push_back(std::tuple (tree.E[i])); } } tree.E = edge_pruned; @@ -932,7 +910,7 @@ Node::Ptr CBITPlanner::UpdateStateSID(size_t SID, vtr::tactic::EdgeTransform T_p double test_err; Node new_state_pq; - for (double p = (*p_goal).p; p < ((*p_goal).p + conf.roc_lookahead); p += (1.0 / (conf.roc_lookahead * conf.curv_to_euclid_discretization))) + for (double p = p_goal->p; p < (p_goal->p + conf.roc_lookahead); p += (1.0 / (conf.roc_lookahead * conf.curv_to_euclid_discretization))) { test_pt = curve_to_euclid(Node(p,q_min)); double dx = test_pt.p - new_state->x; @@ -941,14 +919,12 @@ Node::Ptr CBITPlanner::UpdateStateSID(size_t SID, vtr::tactic::EdgeTransform T_p if (test_err < pose_err) { pose_err = test_err; - new_state_pq = Node(p,q_min); + new_state_pq = Node(p, q_min); } } // Now update the goal and its cost to come: - - std::shared_ptr new_state_pq_ptr = std::make_shared(new_state_pq); - p_goal = new_state_pq_ptr; + p_goal = std::make_shared(new_state_pq); p_goal->g_T = INFINITY; p_goal->g_T_weighted = INFINITY; @@ -980,7 +956,7 @@ double CBITPlanner::BestEdgeQueueValue() } // Using multimaps to accomplish the same thing: - std::multimap, std::shared_ptr>>::iterator itr = tree.QE2.begin(); + std::multimap>::iterator itr = tree.QE2.begin(); double min_edge_cost = itr -> first; return min_edge_cost; @@ -1037,20 +1013,20 @@ void CBITPlanner::ExpandVertex(std::shared_ptr v) } -std::tuple, std::shared_ptr> CBITPlanner::BestInEdgeQueue() +Node::Edge CBITPlanner::BestInEdgeQueue() { if (tree.QE2.size() == 0) // need to handle a case where the return path is 100% optimal in which case things get stuck and need ot be flagged to break { CLOG(DEBUG, "cbit_planner.path_planning") << "Edge Queue is Empty, Solution Could Not be Improved This Batch"; repair_mode = true; - return std::tuple, std::shared_ptr> {NULL, NULL}; + return Node::Edge {NULL, NULL}; } // Equivalent code using multimaps: std::multimap, std::shared_ptr>>::iterator itr = tree.QE2.begin(); - std::tuple, std::shared_ptr> edge_tuple = itr -> second; tree.QE2.erase(itr); - return edge_tuple; + + return itr->second; } @@ -1080,7 +1056,7 @@ std::tuple, std::vector> CBITPlanner::ExtractPath(Ba // Function to convert the resulting p,q path into a finely discretized euclidean path -std::vector CBITPlanner::ExtractEuclidPath() +Pose::Path CBITPlanner::ExtractEuclidPath() { Node node = *p_goal; @@ -1093,7 +1069,7 @@ std::vector CBITPlanner::ExtractEuclidPath() std::vector path_q; std::vector p = global_path->p; - std::vector disc_path = global_path->disc_path; + Pose::Path disc_path = global_path->disc_path; std::vector node_list = {node}; @@ -1171,7 +1147,7 @@ std::shared_ptr CBITPlanner::col_check_path_v2(double max_lookahead_p) { // Generate path to collision check - std::vector> curv_path; + Node::Path curv_path; std::shared_ptr node_ptr = p_goal; curv_path.push_back(node_ptr); diff --git a/main/src/vtr_path_planning/src/cbit/generate_pq.cpp b/main/src/vtr_path_planning/src/cbit/generate_pq.cpp index 07944e9ce..2c06ed7e6 100644 --- a/main/src/vtr_path_planning/src/cbit/generate_pq.cpp +++ b/main/src/vtr_path_planning/src/cbit/generate_pq.cpp @@ -67,17 +67,6 @@ CBITPath::CBITPath(CBITConfig config, std::vector initial_path) } -Pose CBITPath::interp_pose(double p_in) -{ - int p_iter = 0; - while (p_in > this->p[p_iter]) - { - p_iter = p_iter+1; - } - Pose interpolated_pose(this->disc_path[p_iter].x, this->disc_path[p_iter].y, this->disc_path[p_iter].z, this->disc_path[p_iter].roll, this->disc_path[p_iter].pitch, this->disc_path[p_iter].yaw); - return interpolated_pose; -} - // Calculating the distance between se(3) poses including a heading contribution double CBITPath::delta_p_calc(Pose start_pose, Pose end_pose, double alpha) diff --git a/main/src/vtr_path_planning/src/cbit/utils.cpp b/main/src/vtr_path_planning/src/cbit/utils.cpp index b0f3da8b8..e53734e30 100644 --- a/main/src/vtr_path_planning/src/cbit/utils.cpp +++ b/main/src/vtr_path_planning/src/cbit/utils.cpp @@ -135,7 +135,7 @@ int bisection(std::vector array, double value) int jm; while ((ju - jl) > 1) { - jm = (ju + jl) >> 1; // Compute midpoint using bitshit + jm = (ju + jl) >> 1; // Compute midpoint using bitshift if (value >= array[jm]) { jl = jm; // Replace either lower limit @@ -187,4 +187,19 @@ std::vector linspace(double start_in, double end_in, int num_in) linspaced.push_back(end); // I want to ensure that start and end // are exactly the same as the input return linspaced; +} + +namespace vtr::path_planning { + // Function for grabbing the robot's velocity in planning frame, transform of robot into planning frame, and transform of planning frame to world frame +ChainInfo getChainInfo(const tactic::LocalizationChain& chain) { + auto lock = chain.guard(); + const auto stamp = chain.leaf_stamp(); + const auto w_p_r_in_r = chain.leaf_velocity(); + const auto T_p_r = chain.T_leaf_trunk().inverse(); + const auto T_w_p = chain.T_start_trunk(); + const auto T_w_v_odo = chain.T_start_petiole(); + const auto T_r_v_odo = chain.T_leaf_petiole(); + const auto curr_sid = chain.trunkSequenceId(); + return ChainInfo{stamp, w_p_r_in_r, T_p_r, T_w_p, T_w_v_odo, T_r_v_odo, curr_sid}; +} } \ No newline at end of file diff --git a/main/src/vtr_path_planning/src/cbit/visualization_utils.cpp b/main/src/vtr_path_planning/src/cbit/visualization_utils.cpp index ad50f13e2..9b2f19d07 100644 --- a/main/src/vtr_path_planning/src/cbit/visualization_utils.cpp +++ b/main/src/vtr_path_planning/src/cbit/visualization_utils.cpp @@ -35,6 +35,7 @@ VisualizationUtils::VisualizationUtils(rclcpp::Node::SharedPtr node) { corridor_pub_r_ = node->create_publisher("corridor_path_right", 10); ref_pose_pub_tracking_ = node->create_publisher("mpc_ref_pose_array_tracking", 10); ref_pose_pub_homotopy_ = node->create_publisher("mpc_ref_pose_array_homotopy", 10); + path_info_for_external_navigation_pub_ = node->create_publisher("path_info_for_external_navigation", 10); } void VisualizationUtils::visualize( @@ -42,13 +43,102 @@ void VisualizationUtils::visualize( const tactic::EdgeTransform& T_w_p, const tactic::EdgeTransform& T_p_r, const tactic::EdgeTransform& T_p_r_extp_mpc, + const tactic::EdgeTransform& T_w_r, const std::vector& mpc_prediction, + const std::vector& mpc_velocities, const std::vector& robot_prediction, const std::vector& tracking_pose_vec, const std::vector& homotopy_pose_vec, const std::shared_ptr> cbit_path_ptr, - const std::shared_ptr corridor_ptr) + const std::shared_ptr corridor_ptr, + const lgmath::se3::Transformation& T_w_p_interpolated_closest_to_robot, + const double& state_p, + const std::shared_ptr global_path_ptr, + unsigned closest_node_idx) { + + { + vtr_path_planning_msgs::msg::PathInfoForExternalNavigation path_info_for_external_navigation_msg; + // path_info_msg.timestamp = stamp; + path_info_for_external_navigation_msg.header.stamp = rclcpp::Time(stamp); + path_info_for_external_navigation_msg.p_value = state_p; // Ensure closest_node_id is defined elsewhere + + // Set robot_pose + const auto T_w_r_matrix = T_w_r.matrix(); + double robot_x = T_w_r_matrix(0, 3); + double robot_y = T_w_r_matrix(1, 3); + double robot_th = atan2(T_w_r_matrix(1, 0), T_w_r_matrix(0, 0)); + + path_info_for_external_navigation_msg.robot_pose[0] = robot_x; + path_info_for_external_navigation_msg.robot_pose[1] = robot_y; + path_info_for_external_navigation_msg.robot_pose[2] = robot_th; + + // Reference path in robot frame + std::vector path = global_path_ptr->disc_path; // Assuming disc_path is a member of cbit_path_ptr + + Eigen::MatrixXd path_mat(path.size(), 2); + Eigen::MatrixXd th_mat(path.size(), 1); + for (size_t i = 0; i < path.size(); i++) { + path_mat(i, 0) = path[i].x; + path_mat(i, 1) = path[i].y; + th_mat(i, 0) = path[i].yaw; + } + + // Construct rotation matrix C + Eigen::Matrix2d C; + C << cos(robot_th), -sin(robot_th), sin(robot_th), cos(robot_th); + Eigen::MatrixXd path_mat_rot = (path_mat.rowwise() - Eigen::Vector2d(robot_x, robot_y).transpose()) * C.transpose(); + Eigen::MatrixXd th_mat_rot = th_mat.array() - robot_th; // Element-wise subtraction + + // Assign path to message + path_info_for_external_navigation_msg.path.resize(path.size() * 4); // Resize to accommodate all elements + + for (size_t i = 0; i < path.size(); i++) { + path_info_for_external_navigation_msg.path[i * 4] = path_mat_rot(i, 0); // x + path_info_for_external_navigation_msg.path[i * 4 + 1] = path_mat_rot(i, 1); // y + path_info_for_external_navigation_msg.path[i * 4 + 2] = cos(th_mat_rot(i, 0)); // dx + path_info_for_external_navigation_msg.path[i * 4 + 3] = sin(th_mat_rot(i, 0)); // dy + } + + // Set mpc_solution + path_info_for_external_navigation_msg.mpc_solution.resize(mpc_velocities.size() * 2); // Resize to accommodate all elements + for (int i = 0; i < mpc_velocities.size(); i++) { + path_info_for_external_navigation_msg.mpc_solution[i * 2] = mpc_velocities[i](0); // v + path_info_for_external_navigation_msg.mpc_solution[i * 2 + 1] = mpc_velocities[i](1); // w + } + + path_info_for_external_navigation_msg.closest_node_idx = closest_node_idx; + + // closest interpolated node's pose + path_info_for_external_navigation_msg.closest_interpolated_node_pose[0] = T_w_p_interpolated_closest_to_robot.matrix()(0, 3); + path_info_for_external_navigation_msg.closest_interpolated_node_pose[1] = T_w_p_interpolated_closest_to_robot.matrix()(1, 3); + path_info_for_external_navigation_msg.closest_interpolated_node_pose[2] = atan2(T_w_p_interpolated_closest_to_robot.matrix()(1, 0), T_w_p_interpolated_closest_to_robot.matrix()(0, 0)); + + // Compute distance and th offset to closest node + lgmath::se3::Transformation T_p_interpolated_closest_node_r = T_w_p_interpolated_closest_to_robot.inverse() * T_w_r; + const auto T_p_interpolated_closest_node_r_matrix = T_p_interpolated_closest_node_r.matrix(); + double distance_to_closest_interpolated_node = sqrt(pow(T_p_interpolated_closest_node_r_matrix(0, 3), 2) + pow(T_p_interpolated_closest_node_r_matrix(1, 3), 2)); + path_info_for_external_navigation_msg.distance_to_closest_interpolated_node = distance_to_closest_interpolated_node; + + double theta_offset_to_closest_interpolated_node = atan2(T_p_interpolated_closest_node_r_matrix(1, 0), T_p_interpolated_closest_node_r_matrix(0, 0)); + // note that this is already wrapped to [-pi, pi] by atan2 calc + path_info_for_external_navigation_msg.theta_offset_to_closest_interpolated_node = theta_offset_to_closest_interpolated_node; + + // Compute distance to goal node + const auto& goal_node_pose = global_path_ptr->disc_path.back(); + double goal_node_x = goal_node_pose.x; + double goal_node_y = goal_node_pose.y; + double distance_to_goal_node = sqrt(pow(goal_node_x - robot_x, 2) + pow(goal_node_y - robot_y, 2)); + path_info_for_external_navigation_msg.distance_to_goal_node = distance_to_goal_node; + + // Compute theta offset to goal node + double theta_offset_to_goal_node = goal_node_pose.yaw - robot_th; + path_info_for_external_navigation_msg.theta_offset_to_goal_node = theta_offset_to_goal_node; + + path_info_for_external_navigation_pub_->publish(path_info_for_external_navigation_msg); + CLOG(INFO, "cbit.visualization") << "Published path info for external navigation"; + } + /// Publish the current frame for planning { Eigen::Affine3d T(T_w_p.matrix()); @@ -125,7 +215,7 @@ void VisualizationUtils::visualize( for (unsigned i = 0; i < (*cbit_path_ptr).size(); ++i) { auto& pose = poses.emplace_back(); - //pose.pose = tf2::toMsg(Eigen::Affine3d(T_p_i_vec[i].matrix())); // Example for how to grab the transform from a transform with covariance data type + // pose.pose = tf2::toMsg(Eigen::Affine3d(T_p_i_vec[i].matrix())); // Example for how to grab the transform from a transform with covariance data type test_pose.position.x = (*cbit_path_ptr)[i].x; test_pose.position.y = (*cbit_path_ptr)[i].y; test_pose.position.z = (*cbit_path_ptr)[i].z; @@ -193,13 +283,8 @@ void VisualizationUtils::visualize( // fill the PoseArray with some sample poses for (size_t i = 0; i < tracking_pose_vec.size(); i++) { - geometry_msgs::msg::Pose pose; auto T1 = tracking_pose_vec[i].matrix(); - pose.position.x = T1(0,3); - pose.position.y = T1(1,3);; - pose.position.z = T1(2,3);; - pose.orientation.w = 1.0; - pose_array_msg.poses.push_back(pose); + pose_array_msg.poses.push_back(tf2::toMsg(Eigen::Affine3d(T1))); } ref_pose_pub_tracking_->publish(pose_array_msg); } @@ -212,13 +297,8 @@ void VisualizationUtils::visualize( // fill the PoseArray with some sample poses for (size_t i = 0; i < homotopy_pose_vec.size(); i++) { - geometry_msgs::msg::Pose pose; auto T2 = homotopy_pose_vec[i].matrix(); - pose.position.x = T2(0,3); - pose.position.y = T2(1,3);; - pose.position.z = T2(2,3);; - pose.orientation.w = 1.0; - pose_array_msg.poses.push_back(pose); + pose_array_msg.poses.push_back(tf2::toMsg(Eigen::Affine3d(T2))); } ref_pose_pub_homotopy_->publish(pose_array_msg); } diff --git a/main/src/vtr_path_planning/src/mpc/lateral_error_evaluator.cpp b/main/src/vtr_path_planning/src/mpc/lateral_error_evaluator.cpp deleted file mode 100644 index c519b837f..000000000 --- a/main/src/vtr_path_planning/src/mpc/lateral_error_evaluator.cpp +++ /dev/null @@ -1,127 +0,0 @@ -// Copyright 2021, Autonomous Space Robotics Lab (ASRL) -// -// Licensed under the Apache License, Version 2.0 (the "License"); -// you may not use this file except in compliance with the License. -// You may obtain a copy of the License at -// -// http://www.apache.org/licenses/LICENSE-2.0 -// -// Unless required by applicable law or agreed to in writing, software -// distributed under the License is distributed on an "AS IS" BASIS, -// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -// See the License for the specific language governing permissions and -// limitations under the License. - -/** - * \file lateral_error_evaluator.cpp - * \author Jordy Sehn, Autonomous Space Robotics Lab (ASRL) - */ - -#include "vtr_path_planning/mpc/lateral_error_evaluators.hpp" -#include - -namespace steam { - -auto LateralErrorEvaluatorRight::MakeShared(const Evaluable::ConstPtr& pt, - const InType& meas_pt) -> Ptr { - return std::make_shared(pt, meas_pt); -} - -LateralErrorEvaluatorRight::LateralErrorEvaluatorRight( - const Evaluable::ConstPtr& pt, const InType& meas_pt) - : pt_(pt), meas_pt_(meas_pt) { - //D_.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); - D_(0,1) = 1; - -} - -bool LateralErrorEvaluatorRight::active() const { return pt_->active(); } - -void LateralErrorEvaluatorRight::getRelatedVarKeys(KeySet& keys) const { - pt_->getRelatedVarKeys(keys); -} - -auto LateralErrorEvaluatorRight::value() const -> OutType { - return D_ * (pt_->value() - meas_pt_); -} - -auto LateralErrorEvaluatorRight::forward() const -> Node::Ptr { - const auto child = pt_->forward(); - const auto value = D_ * (child->value() - meas_pt_); - // Debug check for Barrier Constraint - //if (value <= 0.0) - //{ - // std::cout << "WARNING: MPC Crossed Lateral Barrier Constraints!" << value << std::endl; - //} - const auto node = Node::MakeShared(value); - node->addChild(child); - return node; -} - -void LateralErrorEvaluatorRight::backward(const Eigen::MatrixXd& lhs, - const Node::Ptr& node, - Jacobians& jacs) const { - if (pt_->active()) { - const auto child = std::static_pointer_cast>(node->at(0)); - Eigen::MatrixXd new_lhs = lhs * D_; - pt_->backward(new_lhs, child, jacs); - } -} - -LateralErrorEvaluatorRight::Ptr homo_point_error_right( - const Evaluable::ConstPtr& pt, - const LateralErrorEvaluatorRight::InType& meas_pt) { - return LateralErrorEvaluatorRight::MakeShared(pt, meas_pt); -} - - - - -auto LateralErrorEvaluatorLeft::MakeShared(const Evaluable::ConstPtr& pt, - const InType& meas_pt) -> Ptr { - return std::make_shared(pt, meas_pt); -} - -LateralErrorEvaluatorLeft::LateralErrorEvaluatorLeft( - const Evaluable::ConstPtr& pt, const InType& meas_pt) - : pt_(pt), meas_pt_(meas_pt) { - //D_.block<3, 3>(0, 0) = Eigen::Matrix3d::Identity(); - D_(0,1) = 1; - -} - -bool LateralErrorEvaluatorLeft::active() const { return pt_->active(); } - -void LateralErrorEvaluatorLeft::getRelatedVarKeys(KeySet& keys) const { - pt_->getRelatedVarKeys(keys); -} - -auto LateralErrorEvaluatorLeft::value() const -> OutType { - return D_ * (meas_pt_ - pt_->value()); -} - -auto LateralErrorEvaluatorLeft::forward() const -> Node::Ptr { - const auto child = pt_->forward(); - const auto value = D_ * (meas_pt_ - child->value()); - const auto node = Node::MakeShared(value); - node->addChild(child); - return node; -} - -void LateralErrorEvaluatorLeft::backward(const Eigen::MatrixXd& lhs, - const Node::Ptr& node, - Jacobians& jacs) const { - if (pt_->active()) { - const auto child = std::static_pointer_cast>(node->at(0)); - Eigen::MatrixXd new_lhs = -lhs * D_; - pt_->backward(new_lhs, child, jacs); - } -} - -LateralErrorEvaluatorLeft::Ptr homo_point_error_left( - const Evaluable::ConstPtr& pt, - const LateralErrorEvaluatorLeft::InType& meas_pt) { - return LateralErrorEvaluatorLeft::MakeShared(pt, meas_pt); -} - -} // namespace steam \ No newline at end of file diff --git a/main/src/vtr_path_planning/src/mpc/mpc_common.cpp b/main/src/vtr_path_planning/src/mpc/mpc_common.cpp new file mode 100644 index 000000000..89cad3bb6 --- /dev/null +++ b/main/src/vtr_path_planning/src/mpc/mpc_common.cpp @@ -0,0 +1,287 @@ +// Copyright 2024, Autonomous Space Robotics Lab (ASRL) +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/** + * \file mpc_common.cpp + * \author Alec Krawciw, Autonomous Space Robotics Lab (ASRL) + */ + +#include "vtr_path_planning/mpc/mpc_common.hpp" +#include + + + +namespace vtr::path_planning { +tactic::EdgeTransform tf_from_global(double x, double y, + double theta) { + auto rotm = lgmath::so3::vec2rot({0, 0, theta}); + Eigen::Vector3d final_pose{x, y, 0}; + return tactic::EdgeTransform(rotm, -rotm.transpose() * final_pose); +} + +CurvatureInfo CurvatureInfo::fromTransform(const lgmath::se3::Transformation& T) { + // Note that this is only along a relative path with an origin at 0,0 + // Using the base tf is still required to move into the world frame + auto aang = lgmath::so3::rot2vec(T.C_ba()); + double roc = T.r_ab_inb().norm() / 2 / (sin(aang(2) / 2) + sgn(sin(aang(2) / 2)) * 1e-6); + + Eigen::Matrix3d rotm_perp; + rotm_perp << 0.0, -sgn(roc), 0.0, sgn(roc), 0.0, 0.0, 0.0, 0.0, 1.0; + + auto dist = T.r_ab_inb().norm(); + auto lin_dir = T.r_ab_inb() / dist; + + Eigen::Vector3d coc = T.r_ab_inb() / 2 + sqrt(roc * roc - dist * dist / 4) * + rotm_perp * lin_dir; + return {coc, roc}; +} + + +Segment findClosestSegment(const lgmath::se3::Transformation& T_wr, const tactic::LocalizationChain::Ptr chain, unsigned sid_start) { + + double best_distance = std::numeric_limits::max(); + double max_distance = -1.; + unsigned best_sid = sid_start; + const unsigned end_sid = std::min(sid_start + 20 + 1, + unsigned(chain->size())); + + // Explicit casting to avoid numerical underflow when near the beginning of + // the chain + const unsigned begin_sid = unsigned(std::max(int(sid_start) - 5, 0)); + + // Find the closest vertex to the input + for (auto path_it = chain->begin(begin_sid); unsigned(path_it) < end_sid; + ++path_it) { + + // Calculate the distance + double distance = (T_wr.inverse() * chain->pose(path_it)).r_ab_inb().norm(); + // CLOG(DEBUG, "mpc.cost_function") << "Dist: " << distance << " sid: " << unsigned(path_it); + + // Record the best distance + max_distance = std::max(distance, max_distance); + if (distance < best_distance) { + best_distance = distance; + best_sid = unsigned(path_it); + } + + // This block detects direction switches, and prevents searching across them + if (unsigned(path_it) < end_sid - 1) { + Eigen::Matrix vec_prev_cur = path_it->T().vec(); + Eigen::Matrix vec_cur_next = (path_it + 1)->T().vec(); + // + means they are in the same direction (note the negative at the front + // to invert one of them) + double r_dot = vec_prev_cur.head<3>().dot(vec_cur_next.head<3>()); + // + means they are in the same direction + double C_dot = vec_prev_cur.tail<3>().dot(vec_cur_next.tail<3>()); + // combine the translation and rotation components using the angle weight + double T_dot = r_dot + 0.25 * C_dot; + // If this is negative, they are in the 'opposite direction', and we're at + // a cusp + if (T_dot < 0) { + if (unsigned(path_it) <= sid_start) { + CLOG(DEBUG, "cbit.debug") << "Direction switch behind reset"; + best_distance = std::numeric_limits::max(); + max_distance = -1.; + } else { + CLOG(DEBUG, "cbit.debug") << "Direction switch ahead break"; + break; + } + } + } + } + + //Handle end of path exceptions + if(best_sid == 0) + return std::make_pair(best_sid, best_sid + 1); + if(best_sid == chain->size() - 1) + return std::make_pair(best_sid - 1, best_sid); + + auto curr_dir = (chain->pose(best_sid).inverse() * T_wr).r_ab_inb(); + auto next_dir = (chain->pose(best_sid).inverse() * chain->pose(best_sid + 1)).r_ab_inb(); + + if(curr_dir.dot(next_dir) > 0) + return std::make_pair(best_sid, best_sid + 1); + else + return std::make_pair(best_sid - 1, best_sid); + } + +Segment findClosestSegment(const double p, const tactic::LocalizationChain::Ptr chain, unsigned sid_start) { + + double best_distance = std::numeric_limits::max(); + double max_distance = -1.; + unsigned best_sid = sid_start; + const unsigned end_sid = std::min(sid_start + 20 + 1, + unsigned(chain->size())); + + // Explicit casting to avoid numerical underflow when near the beginning of + // the chain + const unsigned begin_sid = unsigned(std::max(int(sid_start) - 5, 0)); + + // Find the closest vertex to the input + for (auto path_it = chain->begin(begin_sid); unsigned(path_it) < end_sid; + ++path_it) { + + // Calculate the distance + double distance = abs(p - chain->p(path_it)); + // CLOG(DEBUG, "mpc.cost_function") << "Dist: " << distance << " sid: " << unsigned(path_it); + + // Record the best distance + max_distance = std::max(distance, max_distance); + if (distance < best_distance) { + best_distance = distance; + best_sid = unsigned(path_it); + } + } + + //Handle end of path exceptions + if(best_sid == 0) + return std::make_pair(best_sid, best_sid + 1); + if(best_sid == chain->size() - 1) + return std::make_pair(best_sid - 1, best_sid); + + if(p - chain->p(best_sid) > 0) + return std::make_pair(best_sid, best_sid + 1); + else + return std::make_pair(best_sid - 1, best_sid); + } + + +double findRobotP(const lgmath::se3::Transformation& T_wr, const tactic::LocalizationChain::Ptr chain) { + double state_interp = 0; + auto segment = findClosestSegment(T_wr, chain, chain->trunkSequenceId()); + auto path_ref = interpolatePath(T_wr, chain->pose(segment.first), chain->pose(segment.second), state_interp); + return chain->p(segment.first) + state_interp * (chain->p(segment.second) - chain->p(segment.first)); +} + +lgmath::se3::Transformation interpolatePoses(const double interp, + const lgmath::se3::Transformation& seq_start, const lgmath::se3::Transformation& seq_end) { + const lgmath::se3::Transformation edge = seq_start.inverse() * seq_end; + return seq_start * lgmath::se3::Transformation(interp * edge.vec(), 0); +} + +lgmath::se3::Transformation interpolatePath(const lgmath::se3::Transformation& T_wr, + const lgmath::se3::Transformation& seq_start, const lgmath::se3::Transformation& seq_end, + double& interp) { + const lgmath::se3::Transformation edge = seq_start.inverse() * seq_end; + const auto& [coc, roc] = CurvatureInfo::fromTransform(edge); + Eigen::Vector4d coc_h{0, 0, 0, 1}; + + if (abs(roc) < 30) { + coc_h.head<3>() = coc; + + coc_h = seq_start.matrix() * coc_h; + + const auto interp_ang = + acos(std::clamp((T_wr.r_ab_inb() - coc_h.head<3>()) + .normalized() + .dot((seq_start.r_ab_inb() - coc_h.head<3>()).normalized()), -1.0, 1.0)); + + const auto interp_full = + acos(std::clamp((seq_end.r_ab_inb() - coc_h.head<3>()) + .normalized() + .dot((seq_start.r_ab_inb() - coc_h.head<3>()).normalized()), -1.0, 1.0)); + + interp = interp_ang / interp_full; + } else { + const auto edge_vec = seq_end.r_ab_inb() - seq_start.r_ab_inb(); + interp = (T_wr.r_ab_inb() - seq_start.r_ab_inb()).dot(edge_vec.normalized()) / edge_vec.norm(); + } + + // interp = std::clamp(interp, 0.0, 1.0); + if (abs(interp) - std::clamp(interp, 0.0, 1.0) > 0.001) { + CLOG(WARNING, "test") << "Extrapolating beyond path segment. Interp " << interp; + CLOG(DEBUG, "test") << "CoC is " << coc << " RoC is " << roc; + CLOG(DEBUG, "test") << "Position is " << T_wr.matrix(); + CLOG(DEBUG, "test") << "Start is " << seq_start.matrix(); + CLOG(DEBUG, "test") << "End is " << seq_end.matrix(); + CLOG(DEBUG, "test") << "Center is " << coc_h.head<3>(); + } + if (std::isnan(interp)) { + throw std::runtime_error("ERROR NAN FOUND!!!!!"); + } + return interpolatePoses(interp, seq_start, seq_end); +} + + +// For generating VT&R teach path poses used in the corridor mpc (new version which directly uses the interpolated p measurements from the cbit path trajectory tracking) +PoseResultHomotopy generateHomotopyReference(const std::vector& rolled_out_poses, tactic::LocalizationChain::Ptr chain) { + + // Initialize vectors storing the barrier values: + std::vector barrier_q_left; + std::vector barrier_q_right; + + std::vector tracking_reference_poses; + + unsigned last_sid = chain->trunkSequenceId(); + + // Iterate through the interpolated p_measurements and make interpolate euclidean poses from the teach path + for (const auto& T_wrk : rolled_out_poses) { + Segment closestSegment = findClosestSegment(T_wrk, chain, last_sid); + last_sid = closestSegment.first; + + double interp; + auto interpTf = interpolatePath(T_wrk, chain->pose(closestSegment.first), chain->pose(closestSegment.second), interp); + + // add to measurement vector + tracking_reference_poses.push_back(interpTf); + + // Find the corresponding left and right barrier q values to pass to the mpc + auto width1 = pose_graph::BasicPathBase::terrian_type_corridor_width(chain->query_terrain_type(closestSegment.first)); + auto width2 = pose_graph::BasicPathBase::terrian_type_corridor_width(chain->query_terrain_type(closestSegment.second)); + barrier_q_left.push_back((1-interp) * width1 + interp * width2); + barrier_q_right.push_back((1-interp) * width1 + interp * width2); + } + + return {tracking_reference_poses, barrier_q_left, barrier_q_right}; +} + +// For generating VT&R teach path poses used in the corridor mpc (new version which directly uses the interpolated p measurements from the cbit path trajectory tracking) +PoseResultHomotopy generateHomotopyReference(const std::vector& rolled_out_p, tactic::LocalizationChain::Ptr chain) { + + // Initialize vectors storing the barrier values: + std::vector barrier_q_max; + std::vector barrier_q_min; + + std::vector tracking_reference_poses; + + unsigned last_sid = chain->trunkSequenceId(); + + // Iterate through the interpolated p_measurements and make interpolate euclidean poses from the teach path + for (const auto& p_target : rolled_out_p) { + Segment closestSegment = findClosestSegment(p_target, chain, last_sid); + last_sid = closestSegment.first; + + double interp = std::clamp((p_target - chain->p(closestSegment.first)) / (chain->p(closestSegment.second) - chain->p(closestSegment.first)), 0.0, 1.0); + auto interpTf = interpolatePoses(interp, chain->pose(closestSegment.first), chain->pose(closestSegment.second)); + + // add to measurement vector + tracking_reference_poses.push_back(interpTf); + + // Find the corresponding left and right barrier q values to pass to the mpc + auto width1 = pose_graph::BasicPathBase::terrian_type_corridor_width(chain->query_terrain_type(closestSegment.first)); + auto width2 = pose_graph::BasicPathBase::terrian_type_corridor_width(chain->query_terrain_type(closestSegment.second)); + barrier_q_max.push_back((1-interp) * width1 + interp * width2); + barrier_q_min.push_back(-(1-interp) * width1 - interp * width2); + } + + return {tracking_reference_poses, barrier_q_max, barrier_q_min}; +} + +lgmath::se3::Transformation interpolatedPose(double p, const tactic::LocalizationChain::Ptr chain) { + Segment closestSegment = findClosestSegment(p, chain, chain->trunkSequenceId()); + double interp = std::clamp((p - chain->p(closestSegment.first)) / (chain->p(closestSegment.second) - chain->p(closestSegment.first)), 0.0, 1.0); + return interpolatePoses(interp, chain->pose(closestSegment.first), chain->pose(closestSegment.second)); +} + +} \ No newline at end of file diff --git a/main/src/vtr_path_planning/src/mpc/mpc_path_planner.cpp b/main/src/vtr_path_planning/src/mpc/mpc_path_planner.cpp index a22aa9cc7..f09430b11 100644 --- a/main/src/vtr_path_planning/src/mpc/mpc_path_planner.cpp +++ b/main/src/vtr_path_planning/src/mpc/mpc_path_planner.cpp @@ -17,612 +17,77 @@ * \author Jordy Sehn, Autonomous Space Robotics Lab (ASRL) */ -#include "vtr_path_planning/mpc/mpc_path_planner.hpp" -#include "vtr_path_planning/mpc/lateral_error_evaluators.hpp" -#include "vtr_path_planning/mpc/custom_loss_functions.hpp" -#include "vtr_path_planning/mpc/scalar_log_barrier_evaluator.hpp" - - - -// Main MPC problem solve function - TODO: dump all these arguments into an mpc config class -struct MPCResult SolveMPC(const MPCConfig& config) -{ - // Access configuration parameters from the config structure - Eigen::Matrix previous_vel = config.previous_vel; - lgmath::se3::Transformation T0 = config.T0; - std::vector homotopy_reference_poses = config.homotopy_reference_poses; - std::vector tracking_reference_poses = config.tracking_reference_poses; - std::vector barrier_q_left = config.barrier_q_left; - std::vector barrier_q_right = config.barrier_q_right; - int K = config.K; - double DT = config.DT; - double VF = config.VF; - Eigen::Matrix lat_noise_vect = config.lat_noise_vect; - Eigen::Matrix pose_noise_vect = config.pose_noise_vect; - Eigen::Matrix vel_noise_vect = config.vel_noise_vect; - Eigen::Matrix accel_noise_vect = config.accel_noise_vect; - Eigen::Matrix kin_noise_vect = config.kin_noise_vect; - bool point_stabilization = config.point_stabilization; - double pose_error_weight = config.pose_error_weight; - double vel_error_weight = config.vel_error_weight; - double acc_error_weight = config.acc_error_weight; - double kin_error_weight = config.kin_error_weight; - double lat_error_weight = config.lat_error_weight; - bool verbosity = config.verbosity; - bool homotopy_mode = config.homotopy_mode; - - // Conduct an MPC Iteration given current configurations - - // Velocity set-points (desired forward velocity and angular velocity), here we set a static forward target velocity, and try to minimize rotations (0rad/sec) - Eigen::Matrix v_ref; - v_ref << VF, - 0; - - - // Kinematic projection Matrix for Unicycle Model (note its -1's because our varpi lie algebra vector is of a weird frame) - Eigen::Matrix P_tran; - P_tran << -1, 0, - 0, 0, - 0, 0, - 0, 0, - 0, 0, - 0, -1; - - // Lateral constraint projection matrices (Experimental) - Eigen::Matrix I_4; // In steam, the homopoint vars automatically add the 4th row 1, so representing I_4 just needs the 3 zeros - I_4 << 0, - 0, - 0; - - - // Setup shared loss functions and noise models for all cost terms - //const auto sharedLossFunc = steam::L2LossFunc::MakeShared(); // The default L2 loss function weights all cost terms with a value of 1.0 (not using this one anymore) - - // The custom L2WeightedLossFunc allows you to dynamically set the weights of cost terms by providing the value as an argument - const auto poseLossFunc = steam::L2WeightedLossFunc::MakeShared(pose_error_weight); - const auto velLossFunc = steam::L2WeightedLossFunc::MakeShared(vel_error_weight); - const auto accelLossFunc = steam::L2WeightedLossFunc::MakeShared(acc_error_weight); - const auto kinLossFunc = steam::L2WeightedLossFunc::MakeShared(kin_error_weight); - const auto latLossFunc = steam::L2WeightedLossFunc::MakeShared(lat_error_weight); - - // Cost term Noise Covariance Initialization - const auto sharedPoseNoiseModel = steam::StaticNoiseModel<6>::MakeShared(pose_noise_vect); - const auto sharedVelNoiseModel = steam::StaticNoiseModel<2>::MakeShared(vel_noise_vect); - const auto sharedAccelNoiseModel = steam::StaticNoiseModel<2>::MakeShared(accel_noise_vect); - const auto sharedKinNoiseModel = steam::StaticNoiseModel<6>::MakeShared(kin_noise_vect); - const auto sharedLatNoiseModel = steam::StaticNoiseModel<1>::MakeShared(lat_noise_vect); - - - // Generate STEAM States for the velocity vector and SE3 state transforms - std::vector pose_states; - std::vector> vel_states; - - // Invert the extrapolated robot state and use this as the state initialization - lgmath::se3::Transformation T0_inv = T0.inverse(); - Eigen::Vector2d v0(VF, 0.0); - - // Push back the initial states (current robot state) - pose_states.push_back(T0_inv); - vel_states.push_back(v0); - - // Set the remaining states using a warm start from the cbit solution - for (int i=1; i < K; i++) - { - pose_states.push_back(tracking_reference_poses[i]); // New initialization - use the reference measurements from the cbit solution as our initialization - the first one is the same as our initial state - vel_states.push_back(v0); - } - - // Create STEAM states - std::vector pose_state_vars; - std::vector::Ptr> vel_state_vars; - std::vector measurement_vars; // This one is for storing measurements as locked evaluators for barrier constraints - - // Create a locked state var for the 4th column of the identity matrix (used in state constraint) - steam::stereo::HomoPointStateVar::Ptr I_4_eval = steam::stereo::HomoPointStateVar::MakeShared(I_4); - I_4_eval->locked() = true; - - // Create STEAM variables - for (int i = 0; i < K; i++) - { - pose_state_vars.push_back(steam::se3::SE3StateVar::MakeShared(pose_states[i])); - vel_state_vars.push_back(steam::vspace::VSpaceStateVar<2>::MakeShared(vel_states[i])); - } - - // Lock the first (current robot) state from being modified during the optimization - pose_state_vars[0]->locked() = true; - - - - // Setup the optimization problem - steam::OptimizationProblem opt_problem; - for (int i=1; i 0) - { - const auto pose_error_func = steam::se3::SE3ErrorEvaluator::MakeShared(pose_state_vars[i], homotopy_reference_poses[i]); - auto dynamicposeLossFunc = steam::L2WeightedLossFunc::MakeShared(dynamic_pose_error_weight); - const auto pose_cost_term = steam::WeightedLeastSqCostTerm<6>::MakeShared(pose_error_func, sharedPoseNoiseModel, dynamicposeLossFunc); - opt_problem.addCostTerm(pose_cost_term); - //dynamic_pose_error_weight = dynamic_pose_error_weight * 0.95; - } - - // Kinematic constraints (softened but penalized heavily) - if (i < (K-1)) - { - const auto lhs = steam::se3::ComposeInverseEvaluator::MakeShared(pose_state_vars[i+1], pose_state_vars[i]); - const auto vel_proj = steam::vspace::MatrixMultEvaluator<6,2>::MakeShared(vel_state_vars[i], P_tran); // TODO, I guess this version of steam doesnt have this one, will need to do it myself - const auto scaled_vel_proj = steam::vspace::ScalarMultEvaluator<6>::MakeShared(vel_proj, DT); - const auto rhs = steam::se3::ExpMapEvaluator::MakeShared(scaled_vel_proj); - const auto kin_error_func = steam::se3::LogMapEvaluator::MakeShared(steam::se3::ComposeInverseEvaluator::MakeShared(lhs, rhs)); - const auto kin_cost_term = steam::WeightedLeastSqCostTerm<6>::MakeShared(kin_error_func, sharedKinNoiseModel, kinLossFunc); - opt_problem.addCostTerm(kin_cost_term); - - // Non-Zero Velocity Penalty (penalty of non resting control effort helps with point stabilization) - // Only add this cost term if we are not in point stabilization mode (end of path) - - - if (point_stabilization == false) { - const auto vel_cost_term = steam::WeightedLeastSqCostTerm<2>::MakeShared(vel_state_vars[i], sharedVelNoiseModel, velLossFunc); - opt_problem.addCostTerm(vel_cost_term); - } - - - // Acceleration Constraints - if (i == 0) { - // On the first iteration, we need to use an error with the previously applied control command state - const auto accel_cost_term = steam::WeightedLeastSqCostTerm<2>::MakeShared(steam::vspace::VSpaceErrorEvaluator<2>::MakeShared(vel_state_vars[i], previous_vel), sharedAccelNoiseModel, accelLossFunc); - opt_problem.addCostTerm(accel_cost_term); - } else { - // Subsequent iterations we make an error between consecutive velocities. We penalize large changes in velocity between time steps - const auto accel_diff = steam::vspace::AdditionEvaluator<2>::MakeShared(vel_state_vars[i], steam::vspace::NegationEvaluator<2>::MakeShared(vel_state_vars[i-1])); - const auto accel_cost_term = steam::WeightedLeastSqCostTerm<2>::MakeShared(accel_diff, sharedAccelNoiseModel, accelLossFunc); - opt_problem.addCostTerm(accel_cost_term); - } - } - - // Laterial Barrier State Constraints (only when using homotopy guided MPC) - if (i >= 0) - { - // Generate a locked transform evaluator to store the current measurement for state constraints - // The reason we make it a variable and lock it is so we can use the built in steam evaluators which require evaluable inputs - measurement_vars.push_back(steam::se3::SE3StateVar::MakeShared(homotopy_reference_poses[i])); - measurement_vars[i]->locked() = true; - - // Take the compose inverse of the locked measurement w.r.t the state transforms - const auto compose_inv = steam::se3::ComposeInverseEvaluator::MakeShared(measurement_vars[i], pose_state_vars[i]); - - // Use the ComposeLandmarkEvaluator to right multiply the 4th column of the identity matrix to create a 4x1 homogenous point vector with lat,lon,alt error components - const auto error_vec = steam::stereo::ComposeLandmarkEvaluator::MakeShared(compose_inv, I_4_eval); - - // Build lateral barrier terms by querying the current cbit corridor - Eigen::Matrix barrier_right; - barrier_right << 0, - barrier_q_right[i], - 0, - 1; - Eigen::Matrix barrier_left; - barrier_left << 0.0, - barrier_q_left[i], - 0, - 1; - - CLOG(DEBUG, "mpc.debug") << "Left Barrier for this meas is: " << barrier_q_left[i]; - CLOG(DEBUG, "mpc.debug") << "Right Barrier for tis meas is: " << barrier_q_right[i]; - - // compute the lateral error using a custom Homogenous point error STEAM evaluator - const auto lat_error_right = steam::LateralErrorEvaluatorRight::MakeShared(error_vec, barrier_right); // TODO, rename this evaluator to something else - const auto lat_error_left = steam::LateralErrorEvaluatorLeft::MakeShared(error_vec, barrier_left); - - // Previously used Log barriers, however due to instability switch to using inverse squared barriers - //const auto lat_barrier_right = steam::vspace::ScalarLogBarrierEvaluator<1>::MakeShared(lat_error_right); - //const auto lat_barrier_left = steam::vspace::ScalarLogBarrierEvaluator<1>::MakeShared(lat_error_left); - - // For each side of the barrier, compute a scalar inverse barrier term to penalize being close to the bound - const auto lat_barrier_right = steam::vspace::ScalarInverseBarrierEvaluator<1>::MakeShared(lat_error_right); - const auto lat_barrier_left = steam::vspace::ScalarInverseBarrierEvaluator<1>::MakeShared(lat_error_left); - - // Generate least square cost terms and add them to the optimization problem - const auto lat_cost_term_right = steam::WeightedLeastSqCostTerm<1>::MakeShared(lat_barrier_right, sharedLatNoiseModel, latLossFunc); - const auto lat_cost_term_left = steam::WeightedLeastSqCostTerm<1>::MakeShared(lat_barrier_left, sharedLatNoiseModel, latLossFunc); - - // If using homotopy class based control, apply barrier constraints. Else ignore them (more stable but potentially more aggressive) - if (homotopy_mode == true) - { - opt_problem.addCostTerm(lat_cost_term_right); - opt_problem.addCostTerm(lat_cost_term_left); - } - } - } - - // Solve the optimization problem with GuassNewton solver - //using SolverType = steam::GaussNewtonSolver; // Old solver, does not have back stepping capability - //using SolverType = steam::LineSearchGaussNewtonSolver; - using SolverType = steam::DoglegGaussNewtonSolver; - - // Initialize solver parameters - SolverType::Params params; - params.verbose = verbosity; // Makes the output display for debug when true - // params.relative_cost_change_threshold = 1e-4; - params.max_iterations = 100; - params.absolute_cost_change_threshold = 1e-2; - //params.backtrack_multiplier = 0.95; // Line Search Specific Params, will fail to build if using GaussNewtonSolver - //params.max_backtrack_steps = 1000; // Line Search Specific Params, will fail to build if using GaussNewtonSolver - - SolverType solver(opt_problem, params); - - double initial_cost = opt_problem.cost(); - // Check the cost, disregard the result if it is unreasonable (i.e if its higher then the initial cost) - CLOG(DEBUG, "mpc.solver") << "The Initial Solution Cost is:" << initial_cost; - +#include - // Solve the optimization problem - solver.optimize(); - - double final_cost = opt_problem.cost(); - // Check the cost, disregard the result if it is unreasonable (i.e if its higher then the initial cost) - CLOG(DEBUG, "mpc.solver") << "The Final Solution Cost is:" << final_cost; - - if (final_cost > initial_cost) - { - CLOG(ERROR, "mpc.solver") << "The final cost was > initial cost, something went wrong. Commanding the vehicle to stop"; - Eigen::Matrix bad_cost_vel; - - bad_cost_vel(0) = 0.0; - bad_cost_vel(1) = 0.0; - - // Return the mpc_poses as all being the robots current pose (not moving across the horizon as we should be stopped) - std::vector mpc_poses; - for (size_t i = 0; i < pose_state_vars.size(); i++) - { - mpc_poses.push_back(T0); - } - return {bad_cost_vel, mpc_poses}; - } - - // DEBUG: Display the prediction horizon results - /* - CLOG(DEBUG, "mpc.solver") << "Trying to Display the Optimization Results for the isolated MPC"; - CLOG(DEBUG, "mpc.solver") << "The First State is: " << pose_state_vars[0]->value().inverse(); - CLOG(DEBUG, "mpc.solver") << "The Second State is: " << pose_state_vars[1]->value().inverse(); - CLOG(DEBUG, "mpc.solver") << "The Third State is: " << pose_state_vars[2]->value().inverse(); - CLOG(DEBUG, "mpc.solver") << "The Forth State is: " << pose_state_vars[3]->value().inverse(); - CLOG(DEBUG, "mpc.solver") << "The Fifth State is: " << pose_state_vars[4]->value().inverse(); - CLOG(DEBUG, "mpc.solver") << "The Sixth State is: " << pose_state_vars[5]->value().inverse(); - CLOG(DEBUG, "mpc.solver") << "The Seventh State is: " << pose_state_vars[6]->value().inverse(); - CLOG(DEBUG, "mpc.solver") << "The Eighth State is: " << pose_state_vars[7]->value().inverse(); - CLOG(DEBUG, "mpc.solver") << "The Ninth State is: " << pose_state_vars[8]->value().inverse(); - CLOG(DEBUG, "mpc.solver") << "The Tenth State is: " << pose_state_vars[9]->value().inverse(); - CLOG(DEBUG, "mpc.solver") << "The First Velocity is: " << vel_state_vars[0]->value(); - CLOG(DEBUG, "mpc.solver") << "The Second Velocity is: " << vel_state_vars[1]->value(); - CLOG(DEBUG, "mpc.solver") << "The Third Velocity is: " << vel_state_vars[2]->value(); - CLOG(DEBUG, "mpc.solver") << "The Forth Velocity is: " << vel_state_vars[3]->value(); - CLOG(DEBUG, "mpc.solver") << "The Fifth Velocity is: " << vel_state_vars[4]->value(); - CLOG(DEBUG, "mpc.solver") << "The Sixth Velocity is: " << vel_state_vars[5]->value(); - CLOG(DEBUG, "mpc.solver") << "The Seventh Velocity is: " << vel_state_vars[6]->value(); - CLOG(DEBUG, "mpc.solver") << "The Eighth Velocity is: " << vel_state_vars[7]->value(); - CLOG(DEBUG, "mpc.solver") << "The Ninth Velocity is: " << vel_state_vars[8]->value(); - CLOG(DEBUG, "mpc.solver") << "The Tenth Velocity is: " << vel_state_vars[9]->value(); - CLOG(DEBUG, "mpc.solver") << "Linear Component to Return is: " << (vel_state_vars[0]->value())[0]; - CLOG(DEBUG, "mpc.solver") << "Angular Component to Return is: " << (vel_state_vars[0]->value())[1]; - */ - - // Store the velocity command to apply - Eigen::Matrix applied_vel = vel_state_vars[0]->value(); - - // First check if any of the values are nan, if so we return a zero velocity and flag the error - Eigen::Matrix nan_vel; - if (std::isnan(applied_vel(0)) || std::isnan(applied_vel(1))) - { - CLOG(ERROR, "mpc.solver") << "NAN values detected, mpc optimization failed. Returning zero velocities"; - nan_vel(0) = 0.0; - nan_vel(1) = 0.0; - - if (verbosity) { - throw std::runtime_error("NAN values detected in MPC! Crashing for debug!"); - } - - // if we do detect nans, return the mpc_poses as all being the robots current pose (not moving across the horizon as we should be stopped) - std::vector mpc_poses; - for (size_t i = 0; i < pose_state_vars.size(); i++) - { - mpc_poses.push_back(T0); - } - return {nan_vel, mpc_poses}; - } - // if no nan values, return the applied velocity and mpc pose predictions as normal - else - { - // Store the sequence of resulting mpc prediction horizon poses for visualization - std::vector mpc_poses; - for (size_t i = 0; i < pose_state_vars.size(); i++) - { - mpc_poses.push_back(pose_state_vars[i]->value().inverse()); - } - - // Return the resulting structure - return {applied_vel, mpc_poses}; - } -} - - -// helper function for computing the optimization reference poses T_ref based on the current path solution -// This is specifically for the tracking mpc, but is also used to generate the warm start poses for the corridor mpc -struct PoseResultTracking GenerateTrackingReference(std::shared_ptr> cbit_path_ptr, std::tuple robot_pose, int K, double DT, double VF) -{ - - // Save a copy of the current path solution to work on - auto cbit_path = *cbit_path_ptr; - - // PSEUDO CODE: - // 1. Find the closest point on the cbit path to the current state of the robot - // 2. Using K, DT, VF, we generate a vector of "p" values that we want to create Euclidean Poses for (we know these up front) - // 3. After we have our starting closest point on the path, assign that point a p value of 0. Compute the p values for each subsequent point in the lookahead window - // 4. using the desired p values, and the known p values, interpolate a, x,y,z,yaw, value each measurement - // 5. Create the proper measurement transform for each measurement and get it ready for the using with the optimization problem - - // Limiting the size of the cbit path based on the sliding window and then assigning p values - double lookahead_dist = 0.0; - double p_dist = 0.0; - - double min_dist = INFINITY; - double new_dist; - double dx; - double dy; - double p_correction = 0.0; - bool min_flag = true; - - std::vector cbit_p; - cbit_p.reserve(cbit_path.size()); - cbit_p.push_back(0.0); - for (size_t i = 0; i < (cbit_path.size()-2); i++) // the last value of vector is size()-1, so second to last will be size-2 - { - // calculate the p value for the point - p_dist = sqrt((((cbit_path)[i].x - (cbit_path)[i+1].x) * ((cbit_path)[i].x - (cbit_path)[i+1].x)) + (((cbit_path)[i].y - (cbit_path)[i+1].y) * ((cbit_path)[i].y - (cbit_path)[i+1].y))); - lookahead_dist = lookahead_dist + p_dist; - cbit_p.push_back(lookahead_dist); - - // Keep track of the closest point to the robot state - dx = (cbit_path)[i].x - std::get<0>(robot_pose); - dy = (cbit_path)[i].y - std::get<1>(robot_pose); - new_dist = sqrt((dx * dx) + (dy * dy)); - if ((new_dist < min_dist) && (min_flag == true)) - { - p_correction = lookahead_dist; - min_dist = new_dist; - } - else - { - if (new_dist > min_dist + 0.1) - { - min_flag = false; - } - } - - // Stop once we get about 12m ahead of the robot (magic number for now, but this is just a conservative estimate of any reasonable lookahead window and mpc horizon) - if (lookahead_dist > 12.0) - { - break; - } - } - - // Determine the p_values we need for our measurement horizon, corrected for the p value of the closest point on the path to the current robot state - std::vector p_meas_vec; - std::vector homotopy_reference_poses; - std::vector p_interp_vec; - std::vector q_interp_vec; - - p_meas_vec.reserve(K); - for (int i = 0; i < K; i++) - { - - p_meas_vec.push_back((i * DT * VF) + p_correction); - } - - // Iterate through the p_measurements and interpolate euclidean poses from the cbit_path and the corresponding cbit_p values - // Note this could probably just be combined in the previous loop too - bool point_stabilization = false; - for (size_t i = 0; i < p_meas_vec.size(); i++) - { - // handle end of path case: - // if the p meas we would have needed exceeds the final measurement pose, set it equal to our last p value in the path - // This will cause the intepolation to return the final cbit_path pose for all poses past this point - - if (p_meas_vec[i] > cbit_p[cbit_p.size()-1]) - { - p_meas_vec[i] = cbit_p[cbit_p.size()-1]; - point_stabilization = true; // Enable point stabilization configs - CLOG(INFO, "mpc.solver") << "Approaching End of Path, Converting MPC to Point Stabilization Problem"; - } - struct InterpResult interp_pose = InterpolatePose(p_meas_vec[i], cbit_p, cbit_path); +#include "vtr_path_planning/mpc/mpc_path_planner.hpp" - // add to measurement vector - homotopy_reference_poses.push_back(interp_pose.pose); - p_interp_vec.push_back(interp_pose.p_interp); - q_interp_vec.push_back(interp_pose.q_interp); - } - return {homotopy_reference_poses, point_stabilization, p_interp_vec, q_interp_vec}; +namespace vtr::path_planning { +CasadiUnicycleMPC::CasadiUnicycleMPC( bool verbose, casadi::Dict ipopt_opts){ + casadi::Dict opts; + if (!verbose) { + opts["print_time"] = 0; + ipopt_opts["print_level"] = 0; + } + opts["ipopt"] = ipopt_opts; + solve_mpc = nlpsol("solver", "ipopt", "libsolve_unicycle_mpc.so", opts); } +std::map CasadiUnicycleMPC::solve(const Config& mpcConf) { + using namespace casadi; + std::map arg; + + arg["lbx"] = DM::zeros(mpcConf.nStates*(mpcConf.N+1) + mpcConf.nControl*mpcConf.N, 1); + arg["ubx"] = DM::zeros(mpcConf.nStates*(mpcConf.N+1) + mpcConf.nControl*mpcConf.N, 1); + arg["lbx"].set(-DM::inf(), true, Slice(0, mpcConf.nStates*(mpcConf.N+1))); + arg["ubx"].set(DM::inf(), true, Slice(0, mpcConf.nStates*(mpcConf.N+1))); + + // Velocity constraints + arg["ubx"].set(mpcConf.vel_max(Slice(0)), true, Slice(mpcConf.nStates*(mpcConf.N+1), mpcConf.nStates*(mpcConf.N+1) + mpcConf.nControl*mpcConf.N, 2)); + arg["ubx"].set(mpcConf.vel_max(Slice(1)), true, Slice(mpcConf.nStates*(mpcConf.N+1)+1, mpcConf.nStates*(mpcConf.N+1) + mpcConf.nControl*mpcConf.N, 2)); + arg["lbx"].set(-mpcConf.vel_max(Slice(0)), true, Slice(mpcConf.nStates*(mpcConf.N+1), mpcConf.nStates*(mpcConf.N+1) + mpcConf.nControl*mpcConf.N, 2)); + arg["lbx"].set(-mpcConf.vel_max(Slice(1)), true, Slice(mpcConf.nStates*(mpcConf.N+1)+1, mpcConf.nStates*(mpcConf.N+1) + mpcConf.nControl*mpcConf.N, 2)); + + arg["lbg"] = DM::zeros(mpcConf.nStates*(mpcConf.N+1) + mpcConf.N, 1); + arg["ubg"] = DM::zeros(mpcConf.nStates*(mpcConf.N+1) + mpcConf.N, 1); + + if (mpcConf.up_barrier_q.size() > 0 && mpcConf.low_barrier_q.size() > 0) { + arg["ubg"].set(DM(mpcConf.up_barrier_q), true, Slice(mpcConf.nStates*(mpcConf.N+1), mpcConf.nStates*(mpcConf.N+1) + mpcConf.N)); + arg["lbg"].set(DM(mpcConf.low_barrier_q), true, Slice(mpcConf.nStates*(mpcConf.N+1), mpcConf.nStates*(mpcConf.N+1) + mpcConf.N)); + } else { + arg["ubg"].set(DM::inf(), true, Slice(mpcConf.nStates*(mpcConf.N+1), mpcConf.nStates*(mpcConf.N+1) + mpcConf.N)); + arg["lbg"].set(-DM::inf(), true, Slice(mpcConf.nStates*(mpcConf.N+1), mpcConf.nStates*(mpcConf.N+1) + mpcConf.N)); + } + arg["x0"] = reshape(repmat(mpcConf.T0, 1, mpcConf.N+1), mpcConf.nStates*(mpcConf.N+1), 1); + arg["x0"] = vertcat(arg["x0"], DM::zeros(mpcConf.nControl* mpcConf.N, 1)); -// For generating VT&R teach path poses used in the corridor mpc (new version which directly uses the interpolated p measurements from the cbit path trajectory tracking) -struct PoseResultHomotopy GenerateHomotopyReference(std::shared_ptr global_path_ptr, std::shared_ptr corridor_ptr, std::tuple robot_pose, const std::vector &p_interp_vec) { - // Set point stabilization, but just note if we use this function in the cbit.cpp file we need to use the Tracking reference pose point stabilization instead - bool point_stabilization = false; - - // Initialize vectors storing the barrier values: - std::vector barrier_q_left; - std::vector barrier_q_right; - - // load the teach path - std::vector teach_path = global_path_ptr->disc_path; - std::vector teach_path_p = global_path_ptr->p; - - - std::vector tracking_reference_poses; - - // Iterate through the interpolated p_measurements and make interpolate euclidean poses from the teach path - for (size_t i = 0; i < p_interp_vec.size(); i++) - { - - struct InterpResult interp_pose = InterpolatePose(p_interp_vec[i], teach_path_p, teach_path); - - // add to measurement vector - tracking_reference_poses.push_back(interp_pose.pose); + arg["p"] = mpcConf.T0; + for(int i = 0; i < mpcConf.N; i++) { + arg["p"] = vertcat(arg["p"], + mpcConf.reference_poses.at(i)); + } + arg["p"] = vertcat(arg["p"], mpcConf.previous_vel); - // Find the corresponding left and right barrier q values to pass to the mpc + auto res = solve_mpc(arg); + auto stats = solve_mpc.stats(); - // The corridor_ptr points to the stored barrier values for the entire teach trajectory (0,p_len) - // To find the corresponding values, we just need to query the corridor_ptr given the current sid_p + p_meas_vec[i], and return the q values for that bin - double p_query = p_interp_vec[i]; - // this isnt the most efficient way of doing this, but it should be fine, we really only need to run this loop 10-20 times and the size is likely less then 1000 each - int p_ind = 0; - while (corridor_ptr->p_bins[p_ind] <= p_query) - { - p_ind++; - } - barrier_q_left.push_back(corridor_ptr->q_left[p_ind-1]); - barrier_q_right.push_back(corridor_ptr->q_right[p_ind-1]); - } + if(stats["success"].as_bool() == false) { + CLOG(WARNING, "mpc.solver") << "Casadi error: " << stats["return_status"]; + // throw std::logic_error("Casadi was unable to find a feasible solution. Barrier constraint likely violated"); + } + + std::map output; + output["pose"] = reshape(res["x"](Slice(0, mpcConf.nStates * (mpcConf.N + 1))), mpcConf.nStates, mpcConf.N + 1); + output["vel"] = reshape(res["x"](Slice(mpcConf.nStates * (mpcConf.N + 1), mpcConf.nStates * (mpcConf.N + 1) + mpcConf.nControl * mpcConf.N)), mpcConf.nControl, mpcConf.N); - return {tracking_reference_poses, point_stabilization, barrier_q_left, barrier_q_right}; + return output; } -// function takes in the cbit path solution with a vector defining the p axis of the path, and then a desired p_meas -// Then tries to output a euclidean pose interpolated for the desired p_meas. -struct InterpResult InterpolatePose(double p_val, std::vector cbit_p, std::vector cbit_path) -{ - // Find the lower bound of the p values - for (size_t i = 0; i < cbit_p.size(); i++) - { - if (cbit_p[i] < p_val) - { - continue; - } - else - { - double p_lower; - double p_upper; - Pose pose_lower; - Pose pose_upper; - // Handle potential for seg faults when p_val is before the cbit_p first value - if (i == 0) - { - // means we need to back extrapolate - p_lower = cbit_p[i]; - p_upper = cbit_p[i+1]; - pose_lower = cbit_path[i]; - pose_upper = cbit_path[i+1]; - } - else - { - p_lower = cbit_p[i-1]; - p_upper = cbit_p[i]; - pose_lower = cbit_path[i-1]; - pose_upper = cbit_path[i]; - } - - - double x_int = pose_lower.x + ((p_val - p_lower) / (p_upper - p_lower)) * (pose_upper.x - pose_lower.x); - double y_int = pose_lower.y + ((p_val - p_lower) / (p_upper - p_lower)) * (pose_upper.y - pose_lower.y); - double z_int = pose_lower.z + ((p_val - p_lower) / (p_upper - p_lower)) * (pose_upper.z - pose_lower.z); - - // For yaw we need to be abit careful about sign and angle wrap around - // Derive the yaw by creating the vector connecting the pose_upp and pose_lower pts - // TODO: There is a problem here for reverse planning, will need to rotate the yaw 180 degrees in that case. - // For normal forward planning this is fine though - - // This interpolation if we do have yaw available (when the input path is the teach path as it is for corridor mpc) - double yaw_int; - //yaw_int = std::atan2((pose_upper.y - pose_lower.y), (pose_upper.x - pose_lower.x)); - if ((pose_lower.yaw == 0.0) && (pose_upper.yaw == 0.0)) - { - // Yaw interpolation when we dont have yaw available explicitly (i.e from cbit path euclid conversion) - yaw_int = std::atan2((pose_upper.y - pose_lower.y), (pose_upper.x - pose_lower.x)); - } - else - { - yaw_int = pose_lower.yaw; - } - - - // we also want to interpolate p and q values based on the original p,q from the cbit_path. We use this afterwards for finding appropriate corridor mpc - // reference poses on the teach path - double p_int = pose_lower.p + ((p_val - p_lower) / (p_upper - p_lower)) * (pose_upper.p - pose_lower.p); - double q_int = pose_lower.q + ((p_val - p_lower) / (p_upper - p_lower)) * (pose_upper.q - pose_lower.q); - - // Build the transformation matrix - Eigen::Matrix4d T_ref; - T_ref << std::cos(yaw_int),-1*std::sin(yaw_int),0, x_int, - std::sin(yaw_int), std::cos(yaw_int),0, y_int, - 0, 0, 1, z_int, - 0, 0, 0, 1; - T_ref = T_ref.inverse().eval(); - - lgmath::se3::Transformation meas = lgmath::se3::Transformation(T_ref); - - CLOG(DEBUG, "mpc.debug") << "The measurement Euclidean state is - x: " << x_int << " y: " << y_int << " z: " << z_int << " yaw: " << yaw_int; - CLOG(DEBUG, "mpc.debug") << "The measurement P,Q value is - p: " << p_int << " q: " << q_int; - return {meas, p_int, q_int}; - } - } +std::vector tf_to_global(const lgmath::se3::Transformation& T) { + auto aang = lgmath::so3::rot2vec(T.C_ba()); + return {T.r_ab_inb()[0], T.r_ab_inb()[1], aang[2]}; } -// Simple function for checking that the current output velocity command is saturated between our mechanical velocity limits -Eigen::Matrix SaturateVel(Eigen::Matrix applied_vel, double v_lim, double w_lim) -{ - double command_lin_x; - double command_ang_z; - Eigen::Matrix saturated_vel; - - - if (abs(applied_vel(0)) >= v_lim) { - if (applied_vel(0) > 0.0) - { - command_lin_x = v_lim; - } - else if (applied_vel(0) < 0.0) - { - command_lin_x = -1.0* v_lim; - } else { - command_lin_x = 0; - } - } else { - command_lin_x = applied_vel(0) ; - } - - if (abs(applied_vel(1)) >= w_lim) - { - if (applied_vel(1) > 0.0) - { - command_ang_z = w_lim; - } - else if (applied_vel(1) < 0.0) - { - command_ang_z = -1.0 * w_lim; - } else { - command_ang_z = 0; - } - } else { - command_ang_z = applied_vel(1) ; - } - - saturated_vel << command_lin_x, command_ang_z; - return saturated_vel; -} \ No newline at end of file +} //namespace vtr::path_planning \ No newline at end of file diff --git a/main/src/vtr_path_planning/src/mpc/mpc_solver.cpp b/main/src/vtr_path_planning/src/mpc/mpc_solver.cpp new file mode 100644 index 000000000..bff0c2c72 --- /dev/null +++ b/main/src/vtr_path_planning/src/mpc/mpc_solver.cpp @@ -0,0 +1,41825 @@ +/* This file was automatically generated by CasADi 3.6.5. + * It consists of: + * 1) content generated by CasADi runtime: not copyrighted + * 2) template code copied from CasADi source: permissively licensed (MIT-0) + * 3) user code: owned by the user + * + */ +/* How to prefix internal symbols */ +#ifdef CASADI_CODEGEN_PREFIX + #define CASADI_NAMESPACE_CONCAT(NS, ID) _CASADI_NAMESPACE_CONCAT(NS, ID) + #define _CASADI_NAMESPACE_CONCAT(NS, ID) NS ## ID + #define CASADI_PREFIX(ID) CASADI_NAMESPACE_CONCAT(CODEGEN_PREFIX, ID) +#else + #define CASADI_PREFIX(ID) mpc_solver_ ## ID +#endif + +#include + +#ifndef casadi_real +#define casadi_real double +#endif + +#ifndef casadi_int +#define casadi_int long long int +#endif + +/* Add prefix to internal symbols */ +#define casadi_f0 CASADI_PREFIX(f0) +#define casadi_f1 CASADI_PREFIX(f1) +#define casadi_f2 CASADI_PREFIX(f2) +#define casadi_f3 CASADI_PREFIX(f3) +#define casadi_f4 CASADI_PREFIX(f4) +#define casadi_f5 CASADI_PREFIX(f5) +#define casadi_f6 CASADI_PREFIX(f6) +#define casadi_s0 CASADI_PREFIX(s0) +#define casadi_s1 CASADI_PREFIX(s1) +#define casadi_s2 CASADI_PREFIX(s2) +#define casadi_s3 CASADI_PREFIX(s3) +#define casadi_s4 CASADI_PREFIX(s4) +#define casadi_s5 CASADI_PREFIX(s5) +#define casadi_sq CASADI_PREFIX(sq) + +/* Symbol visibility in DLLs */ +#ifndef CASADI_SYMBOL_EXPORT + #if defined(_WIN32) || defined(__WIN32__) || defined(__CYGWIN__) + #if defined(STATIC_LINKED) + #define CASADI_SYMBOL_EXPORT + #else + #define CASADI_SYMBOL_EXPORT __declspec(dllexport) + #endif + #elif defined(__GNUC__) && defined(GCC_HASCLASSVISIBILITY) + #define CASADI_SYMBOL_EXPORT __attribute__ ((visibility ("default"))) + #else + #define CASADI_SYMBOL_EXPORT + #endif +#endif + +casadi_real casadi_sq(casadi_real x) { return x*x;} + +static const casadi_int casadi_s0[82] = {78, 1, 0, 78, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62, 63, 64, 65, 66, 67, 68, 69, 70, 71, 72, 73, 74, 75, 76, 77}; +static const casadi_int casadi_s1[54] = {50, 1, 0, 50, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49}; +static const casadi_int casadi_s2[5] = {1, 1, 0, 1, 0}; +static const casadi_int casadi_s3[67] = {63, 1, 0, 63, 0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 48, 49, 50, 51, 52, 53, 54, 55, 56, 57, 58, 59, 60, 61, 62}; +static const casadi_int casadi_s4[561] = {78, 78, 0, 0, 0, 1, 2, 3, 5, 6, 7, 9, 10, 11, 13, 14, 15, 17, 18, 19, 21, 22, 23, 25, 26, 27, 29, 30, 31, 33, 34, 35, 37, 38, 39, 41, 42, 43, 45, 46, 47, 49, 50, 51, 53, 54, 55, 57, 58, 59, 61, 63, 81, 85, 103, 108, 126, 132, 150, 157, 175, 183, 201, 210, 228, 238, 256, 267, 285, 297, 315, 328, 346, 360, 378, 393, 411, 427, 445, 462, 480, 2, 3, 4, 2, 5, 6, 7, 5, 8, 9, 10, 8, 11, 12, 13, 11, 14, 15, 16, 14, 17, 18, 19, 17, 20, 21, 22, 20, 23, 24, 25, 23, 26, 27, 28, 26, 29, 30, 31, 29, 32, 33, 34, 32, 35, 36, 37, 35, 38, 39, 40, 38, 41, 42, 43, 41, 44, 45, 46, 44, 47, 2, 48, 2, 5, 8, 11, 14, 17, 20, 23, 26, 29, 32, 35, 38, 41, 44, 47, 48, 49, 5, 48, 49, 50, 5, 8, 11, 14, 17, 20, 23, 26, 29, 32, 35, 38, 41, 44, 47, 49, 50, 51, 8, 49, 50, 51, 52, 8, 11, 14, 17, 20, 23, 26, 29, 32, 35, 38, 41, 44, 47, 49, 51, 52, 53, 11, 49, 51, 52, 53, 54, 11, 14, 17, 20, 23, 26, 29, 32, 35, 38, 41, 44, 47, 49, 51, 53, 54, 55, 14, 49, 51, 53, 54, 55, 56, 14, 17, 20, 23, 26, 29, 32, 35, 38, 41, 44, 47, 49, 51, 53, 55, 56, 57, 17, 49, 51, 53, 55, 56, 57, 58, 17, 20, 23, 26, 29, 32, 35, 38, 41, 44, 47, 49, 51, 53, 55, 57, 58, 59, 20, 49, 51, 53, 55, 57, 58, 59, 60, 20, 23, 26, 29, 32, 35, 38, 41, 44, 47, 49, 51, 53, 55, 57, 59, 60, 61, 23, 49, 51, 53, 55, 57, 59, 60, 61, 62, 23, 26, 29, 32, 35, 38, 41, 44, 47, 49, 51, 53, 55, 57, 59, 61, 62, 63, 26, 49, 51, 53, 55, 57, 59, 61, 62, 63, 64, 26, 29, 32, 35, 38, 41, 44, 47, 49, 51, 53, 55, 57, 59, 61, 63, 64, 65, 29, 49, 51, 53, 55, 57, 59, 61, 63, 64, 65, 66, 29, 32, 35, 38, 41, 44, 47, 49, 51, 53, 55, 57, 59, 61, 63, 65, 66, 67, 32, 49, 51, 53, 55, 57, 59, 61, 63, 65, 66, 67, 68, 32, 35, 38, 41, 44, 47, 49, 51, 53, 55, 57, 59, 61, 63, 65, 67, 68, 69, 35, 49, 51, 53, 55, 57, 59, 61, 63, 65, 67, 68, 69, 70, 35, 38, 41, 44, 47, 49, 51, 53, 55, 57, 59, 61, 63, 65, 67, 69, 70, 71, 38, 49, 51, 53, 55, 57, 59, 61, 63, 65, 67, 69, 70, 71, 72, 38, 41, 44, 47, 49, 51, 53, 55, 57, 59, 61, 63, 65, 67, 69, 71, 72, 73, 41, 49, 51, 53, 55, 57, 59, 61, 63, 65, 67, 69, 71, 72, 73, 74, 41, 44, 47, 49, 51, 53, 55, 57, 59, 61, 63, 65, 67, 69, 71, 73, 74, 75, 44, 49, 51, 53, 55, 57, 59, 61, 63, 65, 67, 69, 71, 73, 74, 75, 76, 44, 47, 49, 51, 53, 55, 57, 59, 61, 63, 65, 67, 69, 71, 73, 75, 76, 77}; +static const casadi_int casadi_s5[624] = {63, 78, 0, 3, 6, 10, 13, 16, 20, 23, 26, 30, 33, 36, 40, 43, 46, 50, 53, 56, 60, 63, 66, 70, 73, 76, 80, 83, 86, 90, 93, 96, 100, 103, 106, 110, 113, 116, 120, 123, 126, 130, 133, 136, 140, 143, 146, 150, 151, 152, 153, 155, 200, 202, 244, 246, 285, 287, 323, 325, 358, 360, 390, 392, 419, 421, 445, 447, 468, 470, 488, 490, 505, 507, 519, 521, 530, 532, 538, 540, 543, 0, 3, 48, 1, 4, 48, 2, 3, 4, 5, 3, 6, 49, 4, 7, 49, 5, 6, 7, 8, 6, 9, 50, 7, 10, 50, 8, 9, 10, 11, 9, 12, 51, 10, 13, 51, 11, 12, 13, 14, 12, 15, 52, 13, 16, 52, 14, 15, 16, 17, 15, 18, 53, 16, 19, 53, 17, 18, 19, 20, 18, 21, 54, 19, 22, 54, 20, 21, 22, 23, 21, 24, 55, 22, 25, 55, 23, 24, 25, 26, 24, 27, 56, 25, 28, 56, 26, 27, 28, 29, 27, 30, 57, 28, 31, 57, 29, 30, 31, 32, 30, 33, 58, 31, 34, 58, 32, 33, 34, 35, 33, 36, 59, 34, 37, 59, 35, 36, 37, 38, 36, 39, 60, 37, 40, 60, 38, 39, 40, 41, 39, 42, 61, 40, 43, 61, 41, 42, 43, 44, 42, 45, 62, 43, 46, 62, 44, 45, 46, 47, 45, 46, 47, 3, 4, 3, 4, 5, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 6, 7, 6, 7, 8, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 9, 10, 9, 10, 11, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 12, 13, 12, 13, 14, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 15, 16, 15, 16, 17, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 18, 19, 18, 19, 20, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 21, 22, 21, 22, 23, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 24, 25, 24, 25, 26, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 27, 28, 27, 28, 29, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 30, 31, 30, 31, 32, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 33, 34, 33, 34, 35, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 36, 37, 36, 37, 38, 39, 40, 41, 42, 43, 44, 45, 46, 47, 39, 40, 39, 40, 41, 42, 43, 44, 45, 46, 47, 42, 43, 42, 43, 44, 45, 46, 47, 45, 46, 45, 46, 47}; + +/* nlp:(x[78],p[50])->(f,g[63]) */ +static int casadi_f0(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_real a0, a1, a10, a100, a101, a102, a103, a104, a105, a106, a107, a108, a109, a11, a110, a111, a112, a113, a114, a115, a116, a117, a118, a119, a12, a120, a121, a122, a123, a124, a125, a126, a127, a128, a129, a13, a130, a131, a132, a133, a134, a135, a136, a137, a14, a15, a16, a17, a18, a19, a2, a20, a21, a22, a23, a24, a25, a26, a27, a28, a29, a3, a30, a31, a32, a33, a34, a35, a36, a37, a38, a39, a4, a40, a41, a42, a43, a44, a45, a46, a47, a48, a49, a5, a50, a51, a52, a53, a54, a55, a56, a57, a58, a59, a6, a60, a61, a62, a63, a64, a65, a66, a67, a68, a69, a7, a70, a71, a72, a73, a74, a75, a76, a77, a78, a79, a8, a80, a81, a82, a83, a84, a85, a86, a87, a88, a89, a9, a90, a91, a92, a93, a94, a95, a96, a97, a98, a99; + a0=10.; + a1=arg[0]? arg[0][3] : 0; + a2=arg[1]? arg[1][3] : 0; + a3=(a1-a2); + a3=(a0*a3); + a4=(a1-a2); + a3=(a3*a4); + a4=arg[0]? arg[0][4] : 0; + a5=arg[1]? arg[1][4] : 0; + a6=(a4-a5); + a6=(a0*a6); + a7=(a4-a5); + a6=(a6*a7); + a3=(a3+a6); + a6=arg[0]? arg[0][48] : 0; + a7=casadi_sq(a6); + a8=arg[0]? arg[0][49] : 0; + a9=casadi_sq(a8); + a7=(a7+a9); + a3=(a3+a7); + a7=arg[1]? arg[1][5] : 0; + a9=cos(a7); + a10=arg[0]? arg[0][5] : 0; + a11=sin(a10); + a9=(a9*a11); + a12=sin(a7); + a13=cos(a10); + a12=(a12*a13); + a9=(a9-a12); + a12=cos(a7); + a12=(a12*a13); + a13=sin(a7); + a13=(a13*a11); + a12=(a12+a13); + a9=atan2(a9,a12); + a12=cos(a7); + a13=sin(a10); + a12=(a12*a13); + a11=sin(a7); + a14=cos(a10); + a11=(a11*a14); + a12=(a12-a11); + a11=cos(a7); + a11=(a11*a14); + a14=sin(a7); + a14=(a14*a13); + a11=(a11+a14); + a12=atan2(a12,a11); + a9=(a9*a12); + a3=(a3+a9); + a9=arg[0]? arg[0][6] : 0; + a12=arg[1]? arg[1][6] : 0; + a11=(a9-a12); + a11=(a0*a11); + a14=(a9-a12); + a11=(a11*a14); + a14=arg[0]? arg[0][7] : 0; + a13=arg[1]? arg[1][7] : 0; + a15=(a14-a13); + a15=(a0*a15); + a16=(a14-a13); + a15=(a15*a16); + a11=(a11+a15); + a3=(a3+a11); + a11=arg[0]? arg[0][50] : 0; + a15=casadi_sq(a11); + a16=arg[0]? arg[0][51] : 0; + a17=casadi_sq(a16); + a15=(a15+a17); + a3=(a3+a15); + a15=arg[1]? arg[1][8] : 0; + a17=cos(a15); + a18=arg[0]? arg[0][8] : 0; + a19=sin(a18); + a17=(a17*a19); + a20=sin(a15); + a21=cos(a18); + a20=(a20*a21); + a17=(a17-a20); + a20=cos(a15); + a20=(a20*a21); + a21=sin(a15); + a21=(a21*a19); + a20=(a20+a21); + a17=atan2(a17,a20); + a20=cos(a15); + a21=sin(a18); + a20=(a20*a21); + a19=sin(a15); + a22=cos(a18); + a19=(a19*a22); + a20=(a20-a19); + a19=cos(a15); + a19=(a19*a22); + a22=sin(a15); + a22=(a22*a21); + a19=(a19+a22); + a20=atan2(a20,a19); + a17=(a17*a20); + a3=(a3+a17); + a17=arg[0]? arg[0][9] : 0; + a20=arg[1]? arg[1][9] : 0; + a19=(a17-a20); + a19=(a0*a19); + a22=(a17-a20); + a19=(a19*a22); + a22=arg[0]? arg[0][10] : 0; + a21=arg[1]? arg[1][10] : 0; + a23=(a22-a21); + a23=(a0*a23); + a24=(a22-a21); + a23=(a23*a24); + a19=(a19+a23); + a3=(a3+a19); + a19=arg[0]? arg[0][52] : 0; + a23=casadi_sq(a19); + a24=arg[0]? arg[0][53] : 0; + a25=casadi_sq(a24); + a23=(a23+a25); + a3=(a3+a23); + a23=arg[1]? arg[1][11] : 0; + a25=cos(a23); + a26=arg[0]? arg[0][11] : 0; + a27=sin(a26); + a25=(a25*a27); + a28=sin(a23); + a29=cos(a26); + a28=(a28*a29); + a25=(a25-a28); + a28=cos(a23); + a28=(a28*a29); + a29=sin(a23); + a29=(a29*a27); + a28=(a28+a29); + a25=atan2(a25,a28); + a28=cos(a23); + a29=sin(a26); + a28=(a28*a29); + a27=sin(a23); + a30=cos(a26); + a27=(a27*a30); + a28=(a28-a27); + a27=cos(a23); + a27=(a27*a30); + a30=sin(a23); + a30=(a30*a29); + a27=(a27+a30); + a28=atan2(a28,a27); + a25=(a25*a28); + a3=(a3+a25); + a25=arg[0]? arg[0][12] : 0; + a28=arg[1]? arg[1][12] : 0; + a27=(a25-a28); + a27=(a0*a27); + a30=(a25-a28); + a27=(a27*a30); + a30=arg[0]? arg[0][13] : 0; + a29=arg[1]? arg[1][13] : 0; + a31=(a30-a29); + a31=(a0*a31); + a32=(a30-a29); + a31=(a31*a32); + a27=(a27+a31); + a3=(a3+a27); + a27=arg[0]? arg[0][54] : 0; + a31=casadi_sq(a27); + a32=arg[0]? arg[0][55] : 0; + a33=casadi_sq(a32); + a31=(a31+a33); + a3=(a3+a31); + a31=arg[1]? arg[1][14] : 0; + a33=cos(a31); + a34=arg[0]? arg[0][14] : 0; + a35=sin(a34); + a33=(a33*a35); + a36=sin(a31); + a37=cos(a34); + a36=(a36*a37); + a33=(a33-a36); + a36=cos(a31); + a36=(a36*a37); + a37=sin(a31); + a37=(a37*a35); + a36=(a36+a37); + a33=atan2(a33,a36); + a36=cos(a31); + a37=sin(a34); + a36=(a36*a37); + a35=sin(a31); + a38=cos(a34); + a35=(a35*a38); + a36=(a36-a35); + a35=cos(a31); + a35=(a35*a38); + a38=sin(a31); + a38=(a38*a37); + a35=(a35+a38); + a36=atan2(a36,a35); + a33=(a33*a36); + a3=(a3+a33); + a33=arg[0]? arg[0][15] : 0; + a36=arg[1]? arg[1][15] : 0; + a35=(a33-a36); + a35=(a0*a35); + a38=(a33-a36); + a35=(a35*a38); + a38=arg[0]? arg[0][16] : 0; + a37=arg[1]? arg[1][16] : 0; + a39=(a38-a37); + a39=(a0*a39); + a40=(a38-a37); + a39=(a39*a40); + a35=(a35+a39); + a3=(a3+a35); + a35=arg[0]? arg[0][56] : 0; + a39=casadi_sq(a35); + a40=arg[0]? arg[0][57] : 0; + a41=casadi_sq(a40); + a39=(a39+a41); + a3=(a3+a39); + a39=arg[1]? arg[1][17] : 0; + a41=cos(a39); + a42=arg[0]? arg[0][17] : 0; + a43=sin(a42); + a41=(a41*a43); + a44=sin(a39); + a45=cos(a42); + a44=(a44*a45); + a41=(a41-a44); + a44=cos(a39); + a44=(a44*a45); + a45=sin(a39); + a45=(a45*a43); + a44=(a44+a45); + a41=atan2(a41,a44); + a44=cos(a39); + a45=sin(a42); + a44=(a44*a45); + a43=sin(a39); + a46=cos(a42); + a43=(a43*a46); + a44=(a44-a43); + a43=cos(a39); + a43=(a43*a46); + a46=sin(a39); + a46=(a46*a45); + a43=(a43+a46); + a44=atan2(a44,a43); + a41=(a41*a44); + a3=(a3+a41); + a41=arg[0]? arg[0][18] : 0; + a44=arg[1]? arg[1][18] : 0; + a43=(a41-a44); + a43=(a0*a43); + a46=(a41-a44); + a43=(a43*a46); + a46=arg[0]? arg[0][19] : 0; + a45=arg[1]? arg[1][19] : 0; + a47=(a46-a45); + a47=(a0*a47); + a48=(a46-a45); + a47=(a47*a48); + a43=(a43+a47); + a3=(a3+a43); + a43=arg[0]? arg[0][58] : 0; + a47=casadi_sq(a43); + a48=arg[0]? arg[0][59] : 0; + a49=casadi_sq(a48); + a47=(a47+a49); + a3=(a3+a47); + a47=arg[1]? arg[1][20] : 0; + a49=cos(a47); + a50=arg[0]? arg[0][20] : 0; + a51=sin(a50); + a49=(a49*a51); + a52=sin(a47); + a53=cos(a50); + a52=(a52*a53); + a49=(a49-a52); + a52=cos(a47); + a52=(a52*a53); + a53=sin(a47); + a53=(a53*a51); + a52=(a52+a53); + a49=atan2(a49,a52); + a52=cos(a47); + a53=sin(a50); + a52=(a52*a53); + a51=sin(a47); + a54=cos(a50); + a51=(a51*a54); + a52=(a52-a51); + a51=cos(a47); + a51=(a51*a54); + a54=sin(a47); + a54=(a54*a53); + a51=(a51+a54); + a52=atan2(a52,a51); + a49=(a49*a52); + a3=(a3+a49); + a49=arg[0]? arg[0][21] : 0; + a52=arg[1]? arg[1][21] : 0; + a51=(a49-a52); + a51=(a0*a51); + a54=(a49-a52); + a51=(a51*a54); + a54=arg[0]? arg[0][22] : 0; + a53=arg[1]? arg[1][22] : 0; + a55=(a54-a53); + a55=(a0*a55); + a56=(a54-a53); + a55=(a55*a56); + a51=(a51+a55); + a3=(a3+a51); + a51=arg[0]? arg[0][60] : 0; + a55=casadi_sq(a51); + a56=arg[0]? arg[0][61] : 0; + a57=casadi_sq(a56); + a55=(a55+a57); + a3=(a3+a55); + a55=arg[1]? arg[1][23] : 0; + a57=cos(a55); + a58=arg[0]? arg[0][23] : 0; + a59=sin(a58); + a57=(a57*a59); + a60=sin(a55); + a61=cos(a58); + a60=(a60*a61); + a57=(a57-a60); + a60=cos(a55); + a60=(a60*a61); + a61=sin(a55); + a61=(a61*a59); + a60=(a60+a61); + a57=atan2(a57,a60); + a60=cos(a55); + a61=sin(a58); + a60=(a60*a61); + a59=sin(a55); + a62=cos(a58); + a59=(a59*a62); + a60=(a60-a59); + a59=cos(a55); + a59=(a59*a62); + a62=sin(a55); + a62=(a62*a61); + a59=(a59+a62); + a60=atan2(a60,a59); + a57=(a57*a60); + a3=(a3+a57); + a57=arg[0]? arg[0][24] : 0; + a60=arg[1]? arg[1][24] : 0; + a59=(a57-a60); + a59=(a0*a59); + a62=(a57-a60); + a59=(a59*a62); + a62=arg[0]? arg[0][25] : 0; + a61=arg[1]? arg[1][25] : 0; + a63=(a62-a61); + a63=(a0*a63); + a64=(a62-a61); + a63=(a63*a64); + a59=(a59+a63); + a3=(a3+a59); + a59=arg[0]? arg[0][62] : 0; + a63=casadi_sq(a59); + a64=arg[0]? arg[0][63] : 0; + a65=casadi_sq(a64); + a63=(a63+a65); + a3=(a3+a63); + a63=arg[1]? arg[1][26] : 0; + a65=cos(a63); + a66=arg[0]? arg[0][26] : 0; + a67=sin(a66); + a65=(a65*a67); + a68=sin(a63); + a69=cos(a66); + a68=(a68*a69); + a65=(a65-a68); + a68=cos(a63); + a68=(a68*a69); + a69=sin(a63); + a69=(a69*a67); + a68=(a68+a69); + a65=atan2(a65,a68); + a68=cos(a63); + a69=sin(a66); + a68=(a68*a69); + a67=sin(a63); + a70=cos(a66); + a67=(a67*a70); + a68=(a68-a67); + a67=cos(a63); + a67=(a67*a70); + a70=sin(a63); + a70=(a70*a69); + a67=(a67+a70); + a68=atan2(a68,a67); + a65=(a65*a68); + a3=(a3+a65); + a65=arg[0]? arg[0][27] : 0; + a68=arg[1]? arg[1][27] : 0; + a67=(a65-a68); + a67=(a0*a67); + a70=(a65-a68); + a67=(a67*a70); + a70=arg[0]? arg[0][28] : 0; + a69=arg[1]? arg[1][28] : 0; + a71=(a70-a69); + a71=(a0*a71); + a72=(a70-a69); + a71=(a71*a72); + a67=(a67+a71); + a3=(a3+a67); + a67=arg[0]? arg[0][64] : 0; + a71=casadi_sq(a67); + a72=arg[0]? arg[0][65] : 0; + a73=casadi_sq(a72); + a71=(a71+a73); + a3=(a3+a71); + a71=arg[1]? arg[1][29] : 0; + a73=cos(a71); + a74=arg[0]? arg[0][29] : 0; + a75=sin(a74); + a73=(a73*a75); + a76=sin(a71); + a77=cos(a74); + a76=(a76*a77); + a73=(a73-a76); + a76=cos(a71); + a76=(a76*a77); + a77=sin(a71); + a77=(a77*a75); + a76=(a76+a77); + a73=atan2(a73,a76); + a76=cos(a71); + a77=sin(a74); + a76=(a76*a77); + a75=sin(a71); + a78=cos(a74); + a75=(a75*a78); + a76=(a76-a75); + a75=cos(a71); + a75=(a75*a78); + a78=sin(a71); + a78=(a78*a77); + a75=(a75+a78); + a76=atan2(a76,a75); + a73=(a73*a76); + a3=(a3+a73); + a73=arg[0]? arg[0][30] : 0; + a76=arg[1]? arg[1][30] : 0; + a75=(a73-a76); + a75=(a0*a75); + a78=(a73-a76); + a75=(a75*a78); + a78=arg[0]? arg[0][31] : 0; + a77=arg[1]? arg[1][31] : 0; + a79=(a78-a77); + a79=(a0*a79); + a80=(a78-a77); + a79=(a79*a80); + a75=(a75+a79); + a3=(a3+a75); + a75=arg[0]? arg[0][66] : 0; + a79=casadi_sq(a75); + a80=arg[0]? arg[0][67] : 0; + a81=casadi_sq(a80); + a79=(a79+a81); + a3=(a3+a79); + a79=arg[1]? arg[1][32] : 0; + a81=cos(a79); + a82=arg[0]? arg[0][32] : 0; + a83=sin(a82); + a81=(a81*a83); + a84=sin(a79); + a85=cos(a82); + a84=(a84*a85); + a81=(a81-a84); + a84=cos(a79); + a84=(a84*a85); + a85=sin(a79); + a85=(a85*a83); + a84=(a84+a85); + a81=atan2(a81,a84); + a84=cos(a79); + a85=sin(a82); + a84=(a84*a85); + a83=sin(a79); + a86=cos(a82); + a83=(a83*a86); + a84=(a84-a83); + a83=cos(a79); + a83=(a83*a86); + a86=sin(a79); + a86=(a86*a85); + a83=(a83+a86); + a84=atan2(a84,a83); + a81=(a81*a84); + a3=(a3+a81); + a81=arg[0]? arg[0][33] : 0; + a84=arg[1]? arg[1][33] : 0; + a83=(a81-a84); + a83=(a0*a83); + a86=(a81-a84); + a83=(a83*a86); + a86=arg[0]? arg[0][34] : 0; + a85=arg[1]? arg[1][34] : 0; + a87=(a86-a85); + a87=(a0*a87); + a88=(a86-a85); + a87=(a87*a88); + a83=(a83+a87); + a3=(a3+a83); + a83=arg[0]? arg[0][68] : 0; + a87=casadi_sq(a83); + a88=arg[0]? arg[0][69] : 0; + a89=casadi_sq(a88); + a87=(a87+a89); + a3=(a3+a87); + a87=arg[1]? arg[1][35] : 0; + a89=cos(a87); + a90=arg[0]? arg[0][35] : 0; + a91=sin(a90); + a89=(a89*a91); + a92=sin(a87); + a93=cos(a90); + a92=(a92*a93); + a89=(a89-a92); + a92=cos(a87); + a92=(a92*a93); + a93=sin(a87); + a93=(a93*a91); + a92=(a92+a93); + a89=atan2(a89,a92); + a92=cos(a87); + a93=sin(a90); + a92=(a92*a93); + a91=sin(a87); + a94=cos(a90); + a91=(a91*a94); + a92=(a92-a91); + a91=cos(a87); + a91=(a91*a94); + a94=sin(a87); + a94=(a94*a93); + a91=(a91+a94); + a92=atan2(a92,a91); + a89=(a89*a92); + a3=(a3+a89); + a89=arg[0]? arg[0][36] : 0; + a92=arg[1]? arg[1][36] : 0; + a91=(a89-a92); + a91=(a0*a91); + a94=(a89-a92); + a91=(a91*a94); + a94=arg[0]? arg[0][37] : 0; + a93=arg[1]? arg[1][37] : 0; + a95=(a94-a93); + a95=(a0*a95); + a96=(a94-a93); + a95=(a95*a96); + a91=(a91+a95); + a3=(a3+a91); + a91=arg[0]? arg[0][70] : 0; + a95=casadi_sq(a91); + a96=arg[0]? arg[0][71] : 0; + a97=casadi_sq(a96); + a95=(a95+a97); + a3=(a3+a95); + a95=arg[1]? arg[1][38] : 0; + a97=cos(a95); + a98=arg[0]? arg[0][38] : 0; + a99=sin(a98); + a97=(a97*a99); + a100=sin(a95); + a101=cos(a98); + a100=(a100*a101); + a97=(a97-a100); + a100=cos(a95); + a100=(a100*a101); + a101=sin(a95); + a101=(a101*a99); + a100=(a100+a101); + a97=atan2(a97,a100); + a100=cos(a95); + a101=sin(a98); + a100=(a100*a101); + a99=sin(a95); + a102=cos(a98); + a99=(a99*a102); + a100=(a100-a99); + a99=cos(a95); + a99=(a99*a102); + a102=sin(a95); + a102=(a102*a101); + a99=(a99+a102); + a100=atan2(a100,a99); + a97=(a97*a100); + a3=(a3+a97); + a97=arg[0]? arg[0][39] : 0; + a100=arg[1]? arg[1][39] : 0; + a99=(a97-a100); + a99=(a0*a99); + a102=(a97-a100); + a99=(a99*a102); + a102=arg[0]? arg[0][40] : 0; + a101=arg[1]? arg[1][40] : 0; + a103=(a102-a101); + a103=(a0*a103); + a104=(a102-a101); + a103=(a103*a104); + a99=(a99+a103); + a3=(a3+a99); + a99=arg[0]? arg[0][72] : 0; + a103=casadi_sq(a99); + a104=arg[0]? arg[0][73] : 0; + a105=casadi_sq(a104); + a103=(a103+a105); + a3=(a3+a103); + a103=arg[1]? arg[1][41] : 0; + a105=cos(a103); + a106=arg[0]? arg[0][41] : 0; + a107=sin(a106); + a105=(a105*a107); + a108=sin(a103); + a109=cos(a106); + a108=(a108*a109); + a105=(a105-a108); + a108=cos(a103); + a108=(a108*a109); + a109=sin(a103); + a109=(a109*a107); + a108=(a108+a109); + a105=atan2(a105,a108); + a108=cos(a103); + a109=sin(a106); + a108=(a108*a109); + a107=sin(a103); + a110=cos(a106); + a107=(a107*a110); + a108=(a108-a107); + a107=cos(a103); + a107=(a107*a110); + a110=sin(a103); + a110=(a110*a109); + a107=(a107+a110); + a108=atan2(a108,a107); + a105=(a105*a108); + a3=(a3+a105); + a105=arg[0]? arg[0][42] : 0; + a108=arg[1]? arg[1][42] : 0; + a107=(a105-a108); + a107=(a0*a107); + a110=(a105-a108); + a107=(a107*a110); + a110=arg[0]? arg[0][43] : 0; + a109=arg[1]? arg[1][43] : 0; + a111=(a110-a109); + a111=(a0*a111); + a112=(a110-a109); + a111=(a111*a112); + a107=(a107+a111); + a3=(a3+a107); + a107=arg[0]? arg[0][74] : 0; + a111=casadi_sq(a107); + a112=arg[0]? arg[0][75] : 0; + a113=casadi_sq(a112); + a111=(a111+a113); + a3=(a3+a111); + a111=arg[1]? arg[1][44] : 0; + a113=cos(a111); + a114=arg[0]? arg[0][44] : 0; + a115=sin(a114); + a113=(a113*a115); + a116=sin(a111); + a117=cos(a114); + a116=(a116*a117); + a113=(a113-a116); + a116=cos(a111); + a116=(a116*a117); + a117=sin(a111); + a117=(a117*a115); + a116=(a116+a117); + a113=atan2(a113,a116); + a116=cos(a111); + a117=sin(a114); + a116=(a116*a117); + a115=sin(a111); + a118=cos(a114); + a115=(a115*a118); + a116=(a116-a115); + a115=cos(a111); + a115=(a115*a118); + a118=sin(a111); + a118=(a118*a117); + a115=(a115+a118); + a116=atan2(a116,a115); + a113=(a113*a116); + a3=(a3+a113); + a113=arg[0]? arg[0][45] : 0; + a116=arg[1]? arg[1][45] : 0; + a115=(a113-a116); + a115=(a0*a115); + a118=(a113-a116); + a115=(a115*a118); + a118=arg[0]? arg[0][46] : 0; + a117=arg[1]? arg[1][46] : 0; + a119=(a118-a117); + a0=(a0*a119); + a119=(a118-a117); + a0=(a0*a119); + a115=(a115+a0); + a3=(a3+a115); + a115=arg[0]? arg[0][76] : 0; + a0=casadi_sq(a115); + a119=arg[0]? arg[0][77] : 0; + a120=casadi_sq(a119); + a0=(a0+a120); + a3=(a3+a0); + a0=arg[1]? arg[1][47] : 0; + a120=cos(a0); + a121=arg[0]? arg[0][47] : 0; + a122=sin(a121); + a120=(a120*a122); + a123=sin(a0); + a124=cos(a121); + a123=(a123*a124); + a120=(a120-a123); + a123=cos(a0); + a123=(a123*a124); + a124=sin(a0); + a124=(a124*a122); + a123=(a123+a124); + a120=atan2(a120,a123); + a123=cos(a0); + a124=sin(a121); + a123=(a123*a124); + a122=sin(a0); + a125=cos(a121); + a122=(a122*a125); + a123=(a123-a122); + a122=cos(a0); + a122=(a122*a125); + a125=sin(a0); + a125=(a125*a124); + a122=(a122+a125); + a123=atan2(a123,a122); + a120=(a120*a123); + a3=(a3+a120); + a120=2.; + a123=arg[1]? arg[1][48] : 0; + a122=(a6-a123); + a122=(a120*a122); + a123=(a6-a123); + a122=(a122*a123); + a123=5.0000000000000000e-01; + a125=arg[1]? arg[1][49] : 0; + a124=(a8-a125); + a124=(a123*a124); + a126=(a8-a125); + a124=(a124*a126); + a122=(a122+a124); + a3=(a3+a122); + a122=(a11-a6); + a122=(a120*a122); + a124=(a11-a6); + a122=(a122*a124); + a124=(a16-a8); + a124=(a123*a124); + a126=(a16-a8); + a124=(a124*a126); + a122=(a122+a124); + a3=(a3+a122); + a122=(a19-a11); + a122=(a120*a122); + a124=(a19-a11); + a122=(a122*a124); + a124=(a24-a16); + a124=(a123*a124); + a126=(a24-a16); + a124=(a124*a126); + a122=(a122+a124); + a3=(a3+a122); + a122=(a27-a19); + a122=(a120*a122); + a124=(a27-a19); + a122=(a122*a124); + a124=(a32-a24); + a124=(a123*a124); + a126=(a32-a24); + a124=(a124*a126); + a122=(a122+a124); + a3=(a3+a122); + a122=(a35-a27); + a122=(a120*a122); + a124=(a35-a27); + a122=(a122*a124); + a124=(a40-a32); + a124=(a123*a124); + a126=(a40-a32); + a124=(a124*a126); + a122=(a122+a124); + a3=(a3+a122); + a122=(a43-a35); + a122=(a120*a122); + a124=(a43-a35); + a122=(a122*a124); + a124=(a48-a40); + a124=(a123*a124); + a126=(a48-a40); + a124=(a124*a126); + a122=(a122+a124); + a3=(a3+a122); + a122=(a51-a43); + a122=(a120*a122); + a124=(a51-a43); + a122=(a122*a124); + a124=(a56-a48); + a124=(a123*a124); + a126=(a56-a48); + a124=(a124*a126); + a122=(a122+a124); + a3=(a3+a122); + a122=(a59-a51); + a122=(a120*a122); + a124=(a59-a51); + a122=(a122*a124); + a124=(a64-a56); + a124=(a123*a124); + a126=(a64-a56); + a124=(a124*a126); + a122=(a122+a124); + a3=(a3+a122); + a122=(a67-a59); + a122=(a120*a122); + a124=(a67-a59); + a122=(a122*a124); + a124=(a72-a64); + a124=(a123*a124); + a126=(a72-a64); + a124=(a124*a126); + a122=(a122+a124); + a3=(a3+a122); + a122=(a75-a67); + a122=(a120*a122); + a124=(a75-a67); + a122=(a122*a124); + a124=(a80-a72); + a124=(a123*a124); + a126=(a80-a72); + a124=(a124*a126); + a122=(a122+a124); + a3=(a3+a122); + a122=(a83-a75); + a122=(a120*a122); + a124=(a83-a75); + a122=(a122*a124); + a124=(a88-a80); + a124=(a123*a124); + a126=(a88-a80); + a124=(a124*a126); + a122=(a122+a124); + a3=(a3+a122); + a122=(a91-a83); + a122=(a120*a122); + a124=(a91-a83); + a122=(a122*a124); + a124=(a96-a88); + a124=(a123*a124); + a126=(a96-a88); + a124=(a124*a126); + a122=(a122+a124); + a3=(a3+a122); + a122=(a99-a91); + a122=(a120*a122); + a124=(a99-a91); + a122=(a122*a124); + a124=(a104-a96); + a124=(a123*a124); + a126=(a104-a96); + a124=(a124*a126); + a122=(a122+a124); + a3=(a3+a122); + a122=(a107-a99); + a122=(a120*a122); + a124=(a107-a99); + a122=(a122*a124); + a124=(a112-a104); + a124=(a123*a124); + a126=(a112-a104); + a124=(a124*a126); + a122=(a122+a124); + a3=(a3+a122); + a122=(a115-a107); + a122=(a120*a122); + a124=(a115-a107); + a122=(a122*a124); + a124=(a119-a112); + a123=(a123*a124); + a124=(a119-a112); + a123=(a123*a124); + a122=(a122+a123); + a3=(a3+a122); + if (res[0]!=0) res[0][0]=a3; + a3=arg[0]? arg[0][0] : 0; + a122=arg[1]? arg[1][0] : 0; + a122=(a3-a122); + if (res[1]!=0) res[1][0]=a122; + a122=arg[0]? arg[0][1] : 0; + a123=arg[1]? arg[1][1] : 0; + a123=(a122-a123); + if (res[1]!=0) res[1][1]=a123; + a123=arg[0]? arg[0][2] : 0; + a124=arg[1]? arg[1][2] : 0; + a124=(a123-a124); + if (res[1]!=0) res[1][2]=a124; + a124=4.1666666666666664e-02; + a126=cos(a123); + a126=(a6*a126); + a127=1.2500000000000000e-01; + a128=4.0000000000000002e-01; + a129=(a128*a125); + a130=5.9999999999999998e-01; + a131=(a130*a8); + a129=(a129+a131); + a131=(a127*a129); + a131=(a123+a131); + a132=cos(a131); + a132=(a6*a132); + a132=(a120*a132); + a126=(a126+a132); + a132=(a128*a125); + a133=(a130*a8); + a132=(a132+a133); + a133=(a127*a132); + a133=(a123+a133); + a134=cos(a133); + a134=(a6*a134); + a134=(a120*a134); + a126=(a126+a134); + a134=2.5000000000000000e-01; + a135=(a128*a125); + a136=(a130*a8); + a135=(a135+a136); + a136=(a134*a135); + a136=(a123+a136); + a137=cos(a136); + a137=(a6*a137); + a126=(a126+a137); + a126=(a124*a126); + a126=(a3+a126); + a126=(a1-a126); + if (res[1]!=0) res[1][3]=a126; + a126=sin(a123); + a126=(a6*a126); + a131=sin(a131); + a131=(a6*a131); + a131=(a120*a131); + a126=(a126+a131); + a133=sin(a133); + a133=(a6*a133); + a133=(a120*a133); + a126=(a126+a133); + a136=sin(a136); + a6=(a6*a136); + a126=(a126+a6); + a126=(a124*a126); + a126=(a122+a126); + a126=(a4-a126); + if (res[1]!=0) res[1][4]=a126; + a126=cos(a10); + a132=(a120*a132); + a129=(a129+a132); + a135=(a120*a135); + a129=(a129+a135); + a135=(a128*a125); + a132=(a130*a8); + a135=(a135+a132); + a129=(a129+a135); + a129=(a124*a129); + a123=(a123+a129); + a129=sin(a123); + a126=(a126*a129); + a135=sin(a10); + a123=cos(a123); + a135=(a135*a123); + a126=(a126-a135); + a135=cos(a10); + a135=(a135*a123); + a123=sin(a10); + a123=(a123*a129); + a135=(a135+a123); + a126=atan2(a126,a135); + if (res[1]!=0) res[1][5]=a126; + a126=cos(a10); + a126=(a11*a126); + a8=(a130*a8); + a125=(a128*a125); + a8=(a8+a125); + a125=(a128*a8); + a135=(a130*a16); + a125=(a125+a135); + a135=(a127*a125); + a135=(a10+a135); + a123=cos(a135); + a123=(a11*a123); + a123=(a120*a123); + a126=(a126+a123); + a123=(a128*a8); + a129=(a130*a16); + a123=(a123+a129); + a129=(a127*a123); + a129=(a10+a129); + a132=cos(a129); + a132=(a11*a132); + a132=(a120*a132); + a126=(a126+a132); + a132=(a128*a8); + a6=(a130*a16); + a132=(a132+a6); + a6=(a134*a132); + a6=(a10+a6); + a136=cos(a6); + a136=(a11*a136); + a126=(a126+a136); + a126=(a124*a126); + a126=(a1+a126); + a126=(a9-a126); + if (res[1]!=0) res[1][6]=a126; + a126=sin(a10); + a126=(a11*a126); + a135=sin(a135); + a135=(a11*a135); + a135=(a120*a135); + a126=(a126+a135); + a129=sin(a129); + a129=(a11*a129); + a129=(a120*a129); + a126=(a126+a129); + a6=sin(a6); + a11=(a11*a6); + a126=(a126+a11); + a126=(a124*a126); + a126=(a4+a126); + a126=(a14-a126); + if (res[1]!=0) res[1][7]=a126; + a126=cos(a18); + a123=(a120*a123); + a125=(a125+a123); + a132=(a120*a132); + a125=(a125+a132); + a132=(a128*a8); + a123=(a130*a16); + a132=(a132+a123); + a125=(a125+a132); + a125=(a124*a125); + a10=(a10+a125); + a125=sin(a10); + a126=(a126*a125); + a132=sin(a18); + a10=cos(a10); + a132=(a132*a10); + a126=(a126-a132); + a132=cos(a18); + a132=(a132*a10); + a10=sin(a18); + a10=(a10*a125); + a132=(a132+a10); + a126=atan2(a126,a132); + if (res[1]!=0) res[1][8]=a126; + a126=cos(a18); + a126=(a19*a126); + a16=(a130*a16); + a8=(a128*a8); + a16=(a16+a8); + a8=(a128*a16); + a132=(a130*a24); + a8=(a8+a132); + a132=(a127*a8); + a132=(a18+a132); + a10=cos(a132); + a10=(a19*a10); + a10=(a120*a10); + a126=(a126+a10); + a10=(a128*a16); + a125=(a130*a24); + a10=(a10+a125); + a125=(a127*a10); + a125=(a18+a125); + a123=cos(a125); + a123=(a19*a123); + a123=(a120*a123); + a126=(a126+a123); + a123=(a128*a16); + a11=(a130*a24); + a123=(a123+a11); + a11=(a134*a123); + a11=(a18+a11); + a6=cos(a11); + a6=(a19*a6); + a126=(a126+a6); + a126=(a124*a126); + a126=(a9+a126); + a126=(a17-a126); + if (res[1]!=0) res[1][9]=a126; + a126=sin(a18); + a126=(a19*a126); + a132=sin(a132); + a132=(a19*a132); + a132=(a120*a132); + a126=(a126+a132); + a125=sin(a125); + a125=(a19*a125); + a125=(a120*a125); + a126=(a126+a125); + a11=sin(a11); + a19=(a19*a11); + a126=(a126+a19); + a126=(a124*a126); + a126=(a14+a126); + a126=(a22-a126); + if (res[1]!=0) res[1][10]=a126; + a126=cos(a26); + a10=(a120*a10); + a8=(a8+a10); + a123=(a120*a123); + a8=(a8+a123); + a123=(a128*a16); + a10=(a130*a24); + a123=(a123+a10); + a8=(a8+a123); + a8=(a124*a8); + a18=(a18+a8); + a8=sin(a18); + a126=(a126*a8); + a123=sin(a26); + a18=cos(a18); + a123=(a123*a18); + a126=(a126-a123); + a123=cos(a26); + a123=(a123*a18); + a18=sin(a26); + a18=(a18*a8); + a123=(a123+a18); + a126=atan2(a126,a123); + if (res[1]!=0) res[1][11]=a126; + a126=cos(a26); + a126=(a27*a126); + a24=(a130*a24); + a16=(a128*a16); + a24=(a24+a16); + a16=(a128*a24); + a123=(a130*a32); + a16=(a16+a123); + a123=(a127*a16); + a123=(a26+a123); + a18=cos(a123); + a18=(a27*a18); + a18=(a120*a18); + a126=(a126+a18); + a18=(a128*a24); + a8=(a130*a32); + a18=(a18+a8); + a8=(a127*a18); + a8=(a26+a8); + a10=cos(a8); + a10=(a27*a10); + a10=(a120*a10); + a126=(a126+a10); + a10=(a128*a24); + a19=(a130*a32); + a10=(a10+a19); + a19=(a134*a10); + a19=(a26+a19); + a11=cos(a19); + a11=(a27*a11); + a126=(a126+a11); + a126=(a124*a126); + a126=(a17+a126); + a126=(a25-a126); + if (res[1]!=0) res[1][12]=a126; + a126=sin(a26); + a126=(a27*a126); + a123=sin(a123); + a123=(a27*a123); + a123=(a120*a123); + a126=(a126+a123); + a8=sin(a8); + a8=(a27*a8); + a8=(a120*a8); + a126=(a126+a8); + a19=sin(a19); + a27=(a27*a19); + a126=(a126+a27); + a126=(a124*a126); + a126=(a22+a126); + a126=(a30-a126); + if (res[1]!=0) res[1][13]=a126; + a126=cos(a34); + a18=(a120*a18); + a16=(a16+a18); + a10=(a120*a10); + a16=(a16+a10); + a10=(a128*a24); + a18=(a130*a32); + a10=(a10+a18); + a16=(a16+a10); + a16=(a124*a16); + a26=(a26+a16); + a16=sin(a26); + a126=(a126*a16); + a10=sin(a34); + a26=cos(a26); + a10=(a10*a26); + a126=(a126-a10); + a10=cos(a34); + a10=(a10*a26); + a26=sin(a34); + a26=(a26*a16); + a10=(a10+a26); + a126=atan2(a126,a10); + if (res[1]!=0) res[1][14]=a126; + a126=cos(a34); + a126=(a35*a126); + a32=(a130*a32); + a24=(a128*a24); + a32=(a32+a24); + a24=(a128*a32); + a10=(a130*a40); + a24=(a24+a10); + a10=(a127*a24); + a10=(a34+a10); + a26=cos(a10); + a26=(a35*a26); + a26=(a120*a26); + a126=(a126+a26); + a26=(a128*a32); + a16=(a130*a40); + a26=(a26+a16); + a16=(a127*a26); + a16=(a34+a16); + a18=cos(a16); + a18=(a35*a18); + a18=(a120*a18); + a126=(a126+a18); + a18=(a128*a32); + a27=(a130*a40); + a18=(a18+a27); + a27=(a134*a18); + a27=(a34+a27); + a19=cos(a27); + a19=(a35*a19); + a126=(a126+a19); + a126=(a124*a126); + a126=(a25+a126); + a126=(a33-a126); + if (res[1]!=0) res[1][15]=a126; + a126=sin(a34); + a126=(a35*a126); + a10=sin(a10); + a10=(a35*a10); + a10=(a120*a10); + a126=(a126+a10); + a16=sin(a16); + a16=(a35*a16); + a16=(a120*a16); + a126=(a126+a16); + a27=sin(a27); + a35=(a35*a27); + a126=(a126+a35); + a126=(a124*a126); + a126=(a30+a126); + a126=(a38-a126); + if (res[1]!=0) res[1][16]=a126; + a126=cos(a42); + a26=(a120*a26); + a24=(a24+a26); + a18=(a120*a18); + a24=(a24+a18); + a18=(a128*a32); + a26=(a130*a40); + a18=(a18+a26); + a24=(a24+a18); + a24=(a124*a24); + a34=(a34+a24); + a24=sin(a34); + a126=(a126*a24); + a18=sin(a42); + a34=cos(a34); + a18=(a18*a34); + a126=(a126-a18); + a18=cos(a42); + a18=(a18*a34); + a34=sin(a42); + a34=(a34*a24); + a18=(a18+a34); + a126=atan2(a126,a18); + if (res[1]!=0) res[1][17]=a126; + a126=cos(a42); + a126=(a43*a126); + a40=(a130*a40); + a32=(a128*a32); + a40=(a40+a32); + a32=(a128*a40); + a18=(a130*a48); + a32=(a32+a18); + a18=(a127*a32); + a18=(a42+a18); + a34=cos(a18); + a34=(a43*a34); + a34=(a120*a34); + a126=(a126+a34); + a34=(a128*a40); + a24=(a130*a48); + a34=(a34+a24); + a24=(a127*a34); + a24=(a42+a24); + a26=cos(a24); + a26=(a43*a26); + a26=(a120*a26); + a126=(a126+a26); + a26=(a128*a40); + a35=(a130*a48); + a26=(a26+a35); + a35=(a134*a26); + a35=(a42+a35); + a27=cos(a35); + a27=(a43*a27); + a126=(a126+a27); + a126=(a124*a126); + a126=(a33+a126); + a126=(a41-a126); + if (res[1]!=0) res[1][18]=a126; + a126=sin(a42); + a126=(a43*a126); + a18=sin(a18); + a18=(a43*a18); + a18=(a120*a18); + a126=(a126+a18); + a24=sin(a24); + a24=(a43*a24); + a24=(a120*a24); + a126=(a126+a24); + a35=sin(a35); + a43=(a43*a35); + a126=(a126+a43); + a126=(a124*a126); + a126=(a38+a126); + a126=(a46-a126); + if (res[1]!=0) res[1][19]=a126; + a126=cos(a50); + a34=(a120*a34); + a32=(a32+a34); + a26=(a120*a26); + a32=(a32+a26); + a26=(a128*a40); + a34=(a130*a48); + a26=(a26+a34); + a32=(a32+a26); + a32=(a124*a32); + a42=(a42+a32); + a32=sin(a42); + a126=(a126*a32); + a26=sin(a50); + a42=cos(a42); + a26=(a26*a42); + a126=(a126-a26); + a26=cos(a50); + a26=(a26*a42); + a42=sin(a50); + a42=(a42*a32); + a26=(a26+a42); + a126=atan2(a126,a26); + if (res[1]!=0) res[1][20]=a126; + a126=cos(a50); + a126=(a51*a126); + a48=(a130*a48); + a40=(a128*a40); + a48=(a48+a40); + a40=(a128*a48); + a26=(a130*a56); + a40=(a40+a26); + a26=(a127*a40); + a26=(a50+a26); + a42=cos(a26); + a42=(a51*a42); + a42=(a120*a42); + a126=(a126+a42); + a42=(a128*a48); + a32=(a130*a56); + a42=(a42+a32); + a32=(a127*a42); + a32=(a50+a32); + a34=cos(a32); + a34=(a51*a34); + a34=(a120*a34); + a126=(a126+a34); + a34=(a128*a48); + a43=(a130*a56); + a34=(a34+a43); + a43=(a134*a34); + a43=(a50+a43); + a35=cos(a43); + a35=(a51*a35); + a126=(a126+a35); + a126=(a124*a126); + a126=(a41+a126); + a126=(a49-a126); + if (res[1]!=0) res[1][21]=a126; + a126=sin(a50); + a126=(a51*a126); + a26=sin(a26); + a26=(a51*a26); + a26=(a120*a26); + a126=(a126+a26); + a32=sin(a32); + a32=(a51*a32); + a32=(a120*a32); + a126=(a126+a32); + a43=sin(a43); + a51=(a51*a43); + a126=(a126+a51); + a126=(a124*a126); + a126=(a46+a126); + a126=(a54-a126); + if (res[1]!=0) res[1][22]=a126; + a126=cos(a58); + a42=(a120*a42); + a40=(a40+a42); + a34=(a120*a34); + a40=(a40+a34); + a34=(a128*a48); + a42=(a130*a56); + a34=(a34+a42); + a40=(a40+a34); + a40=(a124*a40); + a50=(a50+a40); + a40=sin(a50); + a126=(a126*a40); + a34=sin(a58); + a50=cos(a50); + a34=(a34*a50); + a126=(a126-a34); + a34=cos(a58); + a34=(a34*a50); + a50=sin(a58); + a50=(a50*a40); + a34=(a34+a50); + a126=atan2(a126,a34); + if (res[1]!=0) res[1][23]=a126; + a126=cos(a58); + a126=(a59*a126); + a56=(a130*a56); + a48=(a128*a48); + a56=(a56+a48); + a48=(a128*a56); + a34=(a130*a64); + a48=(a48+a34); + a34=(a127*a48); + a34=(a58+a34); + a50=cos(a34); + a50=(a59*a50); + a50=(a120*a50); + a126=(a126+a50); + a50=(a128*a56); + a40=(a130*a64); + a50=(a50+a40); + a40=(a127*a50); + a40=(a58+a40); + a42=cos(a40); + a42=(a59*a42); + a42=(a120*a42); + a126=(a126+a42); + a42=(a128*a56); + a51=(a130*a64); + a42=(a42+a51); + a51=(a134*a42); + a51=(a58+a51); + a43=cos(a51); + a43=(a59*a43); + a126=(a126+a43); + a126=(a124*a126); + a126=(a49+a126); + a126=(a57-a126); + if (res[1]!=0) res[1][24]=a126; + a126=sin(a58); + a126=(a59*a126); + a34=sin(a34); + a34=(a59*a34); + a34=(a120*a34); + a126=(a126+a34); + a40=sin(a40); + a40=(a59*a40); + a40=(a120*a40); + a126=(a126+a40); + a51=sin(a51); + a59=(a59*a51); + a126=(a126+a59); + a126=(a124*a126); + a126=(a54+a126); + a126=(a62-a126); + if (res[1]!=0) res[1][25]=a126; + a126=cos(a66); + a50=(a120*a50); + a48=(a48+a50); + a42=(a120*a42); + a48=(a48+a42); + a42=(a128*a56); + a50=(a130*a64); + a42=(a42+a50); + a48=(a48+a42); + a48=(a124*a48); + a58=(a58+a48); + a48=sin(a58); + a126=(a126*a48); + a42=sin(a66); + a58=cos(a58); + a42=(a42*a58); + a126=(a126-a42); + a42=cos(a66); + a42=(a42*a58); + a58=sin(a66); + a58=(a58*a48); + a42=(a42+a58); + a126=atan2(a126,a42); + if (res[1]!=0) res[1][26]=a126; + a126=cos(a66); + a126=(a67*a126); + a64=(a130*a64); + a56=(a128*a56); + a64=(a64+a56); + a56=(a128*a64); + a42=(a130*a72); + a56=(a56+a42); + a42=(a127*a56); + a42=(a66+a42); + a58=cos(a42); + a58=(a67*a58); + a58=(a120*a58); + a126=(a126+a58); + a58=(a128*a64); + a48=(a130*a72); + a58=(a58+a48); + a48=(a127*a58); + a48=(a66+a48); + a50=cos(a48); + a50=(a67*a50); + a50=(a120*a50); + a126=(a126+a50); + a50=(a128*a64); + a59=(a130*a72); + a50=(a50+a59); + a59=(a134*a50); + a59=(a66+a59); + a51=cos(a59); + a51=(a67*a51); + a126=(a126+a51); + a126=(a124*a126); + a126=(a57+a126); + a126=(a65-a126); + if (res[1]!=0) res[1][27]=a126; + a126=sin(a66); + a126=(a67*a126); + a42=sin(a42); + a42=(a67*a42); + a42=(a120*a42); + a126=(a126+a42); + a48=sin(a48); + a48=(a67*a48); + a48=(a120*a48); + a126=(a126+a48); + a59=sin(a59); + a67=(a67*a59); + a126=(a126+a67); + a126=(a124*a126); + a126=(a62+a126); + a126=(a70-a126); + if (res[1]!=0) res[1][28]=a126; + a126=cos(a74); + a58=(a120*a58); + a56=(a56+a58); + a50=(a120*a50); + a56=(a56+a50); + a50=(a128*a64); + a58=(a130*a72); + a50=(a50+a58); + a56=(a56+a50); + a56=(a124*a56); + a66=(a66+a56); + a56=sin(a66); + a126=(a126*a56); + a50=sin(a74); + a66=cos(a66); + a50=(a50*a66); + a126=(a126-a50); + a50=cos(a74); + a50=(a50*a66); + a66=sin(a74); + a66=(a66*a56); + a50=(a50+a66); + a126=atan2(a126,a50); + if (res[1]!=0) res[1][29]=a126; + a126=cos(a74); + a126=(a75*a126); + a72=(a130*a72); + a64=(a128*a64); + a72=(a72+a64); + a64=(a128*a72); + a50=(a130*a80); + a64=(a64+a50); + a50=(a127*a64); + a50=(a74+a50); + a66=cos(a50); + a66=(a75*a66); + a66=(a120*a66); + a126=(a126+a66); + a66=(a128*a72); + a56=(a130*a80); + a66=(a66+a56); + a56=(a127*a66); + a56=(a74+a56); + a58=cos(a56); + a58=(a75*a58); + a58=(a120*a58); + a126=(a126+a58); + a58=(a128*a72); + a67=(a130*a80); + a58=(a58+a67); + a67=(a134*a58); + a67=(a74+a67); + a59=cos(a67); + a59=(a75*a59); + a126=(a126+a59); + a126=(a124*a126); + a126=(a65+a126); + a126=(a73-a126); + if (res[1]!=0) res[1][30]=a126; + a126=sin(a74); + a126=(a75*a126); + a50=sin(a50); + a50=(a75*a50); + a50=(a120*a50); + a126=(a126+a50); + a56=sin(a56); + a56=(a75*a56); + a56=(a120*a56); + a126=(a126+a56); + a67=sin(a67); + a75=(a75*a67); + a126=(a126+a75); + a126=(a124*a126); + a126=(a70+a126); + a126=(a78-a126); + if (res[1]!=0) res[1][31]=a126; + a126=cos(a82); + a66=(a120*a66); + a64=(a64+a66); + a58=(a120*a58); + a64=(a64+a58); + a58=(a128*a72); + a66=(a130*a80); + a58=(a58+a66); + a64=(a64+a58); + a64=(a124*a64); + a74=(a74+a64); + a64=sin(a74); + a126=(a126*a64); + a58=sin(a82); + a74=cos(a74); + a58=(a58*a74); + a126=(a126-a58); + a58=cos(a82); + a58=(a58*a74); + a74=sin(a82); + a74=(a74*a64); + a58=(a58+a74); + a126=atan2(a126,a58); + if (res[1]!=0) res[1][32]=a126; + a126=cos(a82); + a126=(a83*a126); + a80=(a130*a80); + a72=(a128*a72); + a80=(a80+a72); + a72=(a128*a80); + a58=(a130*a88); + a72=(a72+a58); + a58=(a127*a72); + a58=(a82+a58); + a74=cos(a58); + a74=(a83*a74); + a74=(a120*a74); + a126=(a126+a74); + a74=(a128*a80); + a64=(a130*a88); + a74=(a74+a64); + a64=(a127*a74); + a64=(a82+a64); + a66=cos(a64); + a66=(a83*a66); + a66=(a120*a66); + a126=(a126+a66); + a66=(a128*a80); + a75=(a130*a88); + a66=(a66+a75); + a75=(a134*a66); + a75=(a82+a75); + a67=cos(a75); + a67=(a83*a67); + a126=(a126+a67); + a126=(a124*a126); + a126=(a73+a126); + a126=(a81-a126); + if (res[1]!=0) res[1][33]=a126; + a126=sin(a82); + a126=(a83*a126); + a58=sin(a58); + a58=(a83*a58); + a58=(a120*a58); + a126=(a126+a58); + a64=sin(a64); + a64=(a83*a64); + a64=(a120*a64); + a126=(a126+a64); + a75=sin(a75); + a83=(a83*a75); + a126=(a126+a83); + a126=(a124*a126); + a126=(a78+a126); + a126=(a86-a126); + if (res[1]!=0) res[1][34]=a126; + a126=cos(a90); + a74=(a120*a74); + a72=(a72+a74); + a66=(a120*a66); + a72=(a72+a66); + a66=(a128*a80); + a74=(a130*a88); + a66=(a66+a74); + a72=(a72+a66); + a72=(a124*a72); + a82=(a82+a72); + a72=sin(a82); + a126=(a126*a72); + a66=sin(a90); + a82=cos(a82); + a66=(a66*a82); + a126=(a126-a66); + a66=cos(a90); + a66=(a66*a82); + a82=sin(a90); + a82=(a82*a72); + a66=(a66+a82); + a126=atan2(a126,a66); + if (res[1]!=0) res[1][35]=a126; + a126=cos(a90); + a126=(a91*a126); + a88=(a130*a88); + a80=(a128*a80); + a88=(a88+a80); + a80=(a128*a88); + a66=(a130*a96); + a80=(a80+a66); + a66=(a127*a80); + a66=(a90+a66); + a82=cos(a66); + a82=(a91*a82); + a82=(a120*a82); + a126=(a126+a82); + a82=(a128*a88); + a72=(a130*a96); + a82=(a82+a72); + a72=(a127*a82); + a72=(a90+a72); + a74=cos(a72); + a74=(a91*a74); + a74=(a120*a74); + a126=(a126+a74); + a74=(a128*a88); + a83=(a130*a96); + a74=(a74+a83); + a83=(a134*a74); + a83=(a90+a83); + a75=cos(a83); + a75=(a91*a75); + a126=(a126+a75); + a126=(a124*a126); + a126=(a81+a126); + a126=(a89-a126); + if (res[1]!=0) res[1][36]=a126; + a126=sin(a90); + a126=(a91*a126); + a66=sin(a66); + a66=(a91*a66); + a66=(a120*a66); + a126=(a126+a66); + a72=sin(a72); + a72=(a91*a72); + a72=(a120*a72); + a126=(a126+a72); + a83=sin(a83); + a91=(a91*a83); + a126=(a126+a91); + a126=(a124*a126); + a126=(a86+a126); + a126=(a94-a126); + if (res[1]!=0) res[1][37]=a126; + a126=cos(a98); + a82=(a120*a82); + a80=(a80+a82); + a74=(a120*a74); + a80=(a80+a74); + a74=(a128*a88); + a82=(a130*a96); + a74=(a74+a82); + a80=(a80+a74); + a80=(a124*a80); + a90=(a90+a80); + a80=sin(a90); + a126=(a126*a80); + a74=sin(a98); + a90=cos(a90); + a74=(a74*a90); + a126=(a126-a74); + a74=cos(a98); + a74=(a74*a90); + a90=sin(a98); + a90=(a90*a80); + a74=(a74+a90); + a126=atan2(a126,a74); + if (res[1]!=0) res[1][38]=a126; + a126=cos(a98); + a126=(a99*a126); + a96=(a130*a96); + a88=(a128*a88); + a96=(a96+a88); + a88=(a128*a96); + a74=(a130*a104); + a88=(a88+a74); + a74=(a127*a88); + a74=(a98+a74); + a90=cos(a74); + a90=(a99*a90); + a90=(a120*a90); + a126=(a126+a90); + a90=(a128*a96); + a80=(a130*a104); + a90=(a90+a80); + a80=(a127*a90); + a80=(a98+a80); + a82=cos(a80); + a82=(a99*a82); + a82=(a120*a82); + a126=(a126+a82); + a82=(a128*a96); + a91=(a130*a104); + a82=(a82+a91); + a91=(a134*a82); + a91=(a98+a91); + a83=cos(a91); + a83=(a99*a83); + a126=(a126+a83); + a126=(a124*a126); + a126=(a89+a126); + a126=(a97-a126); + if (res[1]!=0) res[1][39]=a126; + a126=sin(a98); + a126=(a99*a126); + a74=sin(a74); + a74=(a99*a74); + a74=(a120*a74); + a126=(a126+a74); + a80=sin(a80); + a80=(a99*a80); + a80=(a120*a80); + a126=(a126+a80); + a91=sin(a91); + a99=(a99*a91); + a126=(a126+a99); + a126=(a124*a126); + a126=(a94+a126); + a126=(a102-a126); + if (res[1]!=0) res[1][40]=a126; + a126=cos(a106); + a90=(a120*a90); + a88=(a88+a90); + a82=(a120*a82); + a88=(a88+a82); + a82=(a128*a96); + a90=(a130*a104); + a82=(a82+a90); + a88=(a88+a82); + a88=(a124*a88); + a98=(a98+a88); + a88=sin(a98); + a126=(a126*a88); + a82=sin(a106); + a98=cos(a98); + a82=(a82*a98); + a126=(a126-a82); + a82=cos(a106); + a82=(a82*a98); + a98=sin(a106); + a98=(a98*a88); + a82=(a82+a98); + a126=atan2(a126,a82); + if (res[1]!=0) res[1][41]=a126; + a126=cos(a106); + a126=(a107*a126); + a104=(a130*a104); + a96=(a128*a96); + a104=(a104+a96); + a96=(a128*a104); + a82=(a130*a112); + a96=(a96+a82); + a82=(a127*a96); + a82=(a106+a82); + a98=cos(a82); + a98=(a107*a98); + a98=(a120*a98); + a126=(a126+a98); + a98=(a128*a104); + a88=(a130*a112); + a98=(a98+a88); + a88=(a127*a98); + a88=(a106+a88); + a90=cos(a88); + a90=(a107*a90); + a90=(a120*a90); + a126=(a126+a90); + a90=(a128*a104); + a99=(a130*a112); + a90=(a90+a99); + a99=(a134*a90); + a99=(a106+a99); + a91=cos(a99); + a91=(a107*a91); + a126=(a126+a91); + a126=(a124*a126); + a126=(a97+a126); + a126=(a105-a126); + if (res[1]!=0) res[1][42]=a126; + a126=sin(a106); + a126=(a107*a126); + a82=sin(a82); + a82=(a107*a82); + a82=(a120*a82); + a126=(a126+a82); + a88=sin(a88); + a88=(a107*a88); + a88=(a120*a88); + a126=(a126+a88); + a99=sin(a99); + a107=(a107*a99); + a126=(a126+a107); + a126=(a124*a126); + a126=(a102+a126); + a126=(a110-a126); + if (res[1]!=0) res[1][43]=a126; + a126=cos(a114); + a98=(a120*a98); + a96=(a96+a98); + a90=(a120*a90); + a96=(a96+a90); + a90=(a128*a104); + a98=(a130*a112); + a90=(a90+a98); + a96=(a96+a90); + a96=(a124*a96); + a106=(a106+a96); + a96=sin(a106); + a126=(a126*a96); + a90=sin(a114); + a106=cos(a106); + a90=(a90*a106); + a126=(a126-a90); + a90=cos(a114); + a90=(a90*a106); + a106=sin(a114); + a106=(a106*a96); + a90=(a90+a106); + a126=atan2(a126,a90); + if (res[1]!=0) res[1][44]=a126; + a126=cos(a114); + a126=(a115*a126); + a112=(a130*a112); + a104=(a128*a104); + a112=(a112+a104); + a104=(a128*a112); + a90=(a130*a119); + a104=(a104+a90); + a90=(a127*a104); + a90=(a114+a90); + a106=cos(a90); + a106=(a115*a106); + a106=(a120*a106); + a126=(a126+a106); + a106=(a128*a112); + a96=(a130*a119); + a106=(a106+a96); + a127=(a127*a106); + a127=(a114+a127); + a96=cos(a127); + a96=(a115*a96); + a96=(a120*a96); + a126=(a126+a96); + a96=(a128*a112); + a98=(a130*a119); + a96=(a96+a98); + a134=(a134*a96); + a134=(a114+a134); + a98=cos(a134); + a98=(a115*a98); + a126=(a126+a98); + a126=(a124*a126); + a126=(a105+a126); + a113=(a113-a126); + if (res[1]!=0) res[1][45]=a113; + a113=sin(a114); + a113=(a115*a113); + a90=sin(a90); + a90=(a115*a90); + a90=(a120*a90); + a113=(a113+a90); + a127=sin(a127); + a127=(a115*a127); + a127=(a120*a127); + a113=(a113+a127); + a134=sin(a134); + a115=(a115*a134); + a113=(a113+a115); + a113=(a124*a113); + a113=(a110+a113); + a118=(a118-a113); + if (res[1]!=0) res[1][46]=a118; + a118=cos(a121); + a106=(a120*a106); + a104=(a104+a106); + a120=(a120*a96); + a104=(a104+a120); + a128=(a128*a112); + a130=(a130*a119); + a128=(a128+a130); + a104=(a104+a128); + a124=(a124*a104); + a114=(a114+a124); + a124=sin(a114); + a118=(a118*a124); + a104=sin(a121); + a114=cos(a114); + a104=(a104*a114); + a118=(a118-a104); + a104=cos(a121); + a104=(a104*a114); + a121=sin(a121); + a121=(a121*a124); + a104=(a104+a121); + a118=atan2(a118,a104); + if (res[1]!=0) res[1][47]=a118; + a118=cos(a7); + a122=(a122-a5); + a118=(a118*a122); + a7=sin(a7); + a3=(a3-a2); + a7=(a7*a3); + a118=(a118-a7); + if (res[1]!=0) res[1][48]=a118; + a118=cos(a15); + a4=(a4-a13); + a118=(a118*a4); + a15=sin(a15); + a1=(a1-a12); + a15=(a15*a1); + a118=(a118-a15); + if (res[1]!=0) res[1][49]=a118; + a118=cos(a23); + a14=(a14-a21); + a118=(a118*a14); + a23=sin(a23); + a9=(a9-a20); + a23=(a23*a9); + a118=(a118-a23); + if (res[1]!=0) res[1][50]=a118; + a118=cos(a31); + a22=(a22-a29); + a118=(a118*a22); + a31=sin(a31); + a17=(a17-a28); + a31=(a31*a17); + a118=(a118-a31); + if (res[1]!=0) res[1][51]=a118; + a118=cos(a39); + a30=(a30-a37); + a118=(a118*a30); + a39=sin(a39); + a25=(a25-a36); + a39=(a39*a25); + a118=(a118-a39); + if (res[1]!=0) res[1][52]=a118; + a118=cos(a47); + a38=(a38-a45); + a118=(a118*a38); + a47=sin(a47); + a33=(a33-a44); + a47=(a47*a33); + a118=(a118-a47); + if (res[1]!=0) res[1][53]=a118; + a118=cos(a55); + a46=(a46-a53); + a118=(a118*a46); + a55=sin(a55); + a41=(a41-a52); + a55=(a55*a41); + a118=(a118-a55); + if (res[1]!=0) res[1][54]=a118; + a118=cos(a63); + a54=(a54-a61); + a118=(a118*a54); + a63=sin(a63); + a49=(a49-a60); + a63=(a63*a49); + a118=(a118-a63); + if (res[1]!=0) res[1][55]=a118; + a118=cos(a71); + a62=(a62-a69); + a118=(a118*a62); + a71=sin(a71); + a57=(a57-a68); + a71=(a71*a57); + a118=(a118-a71); + if (res[1]!=0) res[1][56]=a118; + a118=cos(a79); + a70=(a70-a77); + a118=(a118*a70); + a79=sin(a79); + a65=(a65-a76); + a79=(a79*a65); + a118=(a118-a79); + if (res[1]!=0) res[1][57]=a118; + a118=cos(a87); + a78=(a78-a85); + a118=(a118*a78); + a87=sin(a87); + a73=(a73-a84); + a87=(a87*a73); + a118=(a118-a87); + if (res[1]!=0) res[1][58]=a118; + a118=cos(a95); + a86=(a86-a93); + a118=(a118*a86); + a95=sin(a95); + a81=(a81-a92); + a95=(a95*a81); + a118=(a118-a95); + if (res[1]!=0) res[1][59]=a118; + a118=cos(a103); + a94=(a94-a101); + a118=(a118*a94); + a103=sin(a103); + a89=(a89-a100); + a103=(a103*a89); + a118=(a118-a103); + if (res[1]!=0) res[1][60]=a118; + a118=cos(a111); + a102=(a102-a109); + a118=(a118*a102); + a111=sin(a111); + a97=(a97-a108); + a111=(a111*a97); + a118=(a118-a111); + if (res[1]!=0) res[1][61]=a118; + a118=cos(a0); + a110=(a110-a117); + a118=(a118*a110); + a0=sin(a0); + a105=(a105-a116); + a0=(a0*a105); + a118=(a118-a0); + if (res[1]!=0) res[1][62]=a118; + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f0(arg, res, iw, w, mem); +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_alloc_mem(void) { + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_init_mem(int mem) { + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_free_mem(int mem) { +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_checkout(void) { + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_release(int mem) { +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_incref(void) { +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_decref(void) { +} + +extern "C" CASADI_SYMBOL_EXPORT casadi_int nlp_n_in(void) { return 2;} + +extern "C" CASADI_SYMBOL_EXPORT casadi_int nlp_n_out(void) { return 2;} + +extern "C" CASADI_SYMBOL_EXPORT casadi_real nlp_default_in(casadi_int i) { + switch (i) { + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const char* nlp_name_in(casadi_int i) { + switch (i) { + case 0: return "x"; + case 1: return "p"; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const char* nlp_name_out(casadi_int i) { + switch (i) { + case 0: return "f"; + case 1: return "g"; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const casadi_int* nlp_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s1; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const casadi_int* nlp_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s2; + case 1: return casadi_s3; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 2; + if (sz_res) *sz_res = 2; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 0; + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 2*sizeof(const casadi_real*); + if (sz_res) *sz_res = 2*sizeof(casadi_real*); + if (sz_iw) *sz_iw = 0*sizeof(casadi_int); + if (sz_w) *sz_w = 0*sizeof(casadi_real); + return 0; +} + +/* nlp_f:(x[78],p[50])->(f) */ +static int casadi_f1(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_real a0, a1, a10, a11, a12, a13, a14, a15, a16, a17, a18, a19, a2, a20, a21, a22, a23, a24, a25, a26, a27, a28, a29, a3, a30, a31, a32, a33, a34, a35, a36, a4, a5, a6, a7, a8, a9; + a0=10.; + a1=arg[0]? arg[0][3] : 0; + a2=arg[1]? arg[1][3] : 0; + a3=(a1-a2); + a3=(a0*a3); + a1=(a1-a2); + a3=(a3*a1); + a1=arg[0]? arg[0][4] : 0; + a2=arg[1]? arg[1][4] : 0; + a4=(a1-a2); + a4=(a0*a4); + a1=(a1-a2); + a4=(a4*a1); + a3=(a3+a4); + a4=arg[0]? arg[0][48] : 0; + a1=casadi_sq(a4); + a2=arg[0]? arg[0][49] : 0; + a5=casadi_sq(a2); + a1=(a1+a5); + a3=(a3+a1); + a1=arg[1]? arg[1][5] : 0; + a5=cos(a1); + a6=arg[0]? arg[0][5] : 0; + a7=sin(a6); + a5=(a5*a7); + a8=sin(a1); + a9=cos(a6); + a8=(a8*a9); + a5=(a5-a8); + a8=cos(a1); + a8=(a8*a9); + a9=sin(a1); + a9=(a9*a7); + a8=(a8+a9); + a5=atan2(a5,a8); + a8=cos(a1); + a9=sin(a6); + a8=(a8*a9); + a7=sin(a1); + a6=cos(a6); + a7=(a7*a6); + a8=(a8-a7); + a7=cos(a1); + a7=(a7*a6); + a1=sin(a1); + a1=(a1*a9); + a7=(a7+a1); + a8=atan2(a8,a7); + a5=(a5*a8); + a3=(a3+a5); + a5=arg[0]? arg[0][6] : 0; + a8=arg[1]? arg[1][6] : 0; + a7=(a5-a8); + a7=(a0*a7); + a5=(a5-a8); + a7=(a7*a5); + a5=arg[0]? arg[0][7] : 0; + a8=arg[1]? arg[1][7] : 0; + a1=(a5-a8); + a1=(a0*a1); + a5=(a5-a8); + a1=(a1*a5); + a7=(a7+a1); + a3=(a3+a7); + a7=arg[0]? arg[0][50] : 0; + a1=casadi_sq(a7); + a5=arg[0]? arg[0][51] : 0; + a8=casadi_sq(a5); + a1=(a1+a8); + a3=(a3+a1); + a1=arg[1]? arg[1][8] : 0; + a8=cos(a1); + a9=arg[0]? arg[0][8] : 0; + a6=sin(a9); + a8=(a8*a6); + a10=sin(a1); + a11=cos(a9); + a10=(a10*a11); + a8=(a8-a10); + a10=cos(a1); + a10=(a10*a11); + a11=sin(a1); + a11=(a11*a6); + a10=(a10+a11); + a8=atan2(a8,a10); + a10=cos(a1); + a11=sin(a9); + a10=(a10*a11); + a6=sin(a1); + a9=cos(a9); + a6=(a6*a9); + a10=(a10-a6); + a6=cos(a1); + a6=(a6*a9); + a1=sin(a1); + a1=(a1*a11); + a6=(a6+a1); + a10=atan2(a10,a6); + a8=(a8*a10); + a3=(a3+a8); + a8=arg[0]? arg[0][9] : 0; + a10=arg[1]? arg[1][9] : 0; + a6=(a8-a10); + a6=(a0*a6); + a8=(a8-a10); + a6=(a6*a8); + a8=arg[0]? arg[0][10] : 0; + a10=arg[1]? arg[1][10] : 0; + a1=(a8-a10); + a1=(a0*a1); + a8=(a8-a10); + a1=(a1*a8); + a6=(a6+a1); + a3=(a3+a6); + a6=arg[0]? arg[0][52] : 0; + a1=casadi_sq(a6); + a8=arg[0]? arg[0][53] : 0; + a10=casadi_sq(a8); + a1=(a1+a10); + a3=(a3+a1); + a1=arg[1]? arg[1][11] : 0; + a10=cos(a1); + a11=arg[0]? arg[0][11] : 0; + a9=sin(a11); + a10=(a10*a9); + a12=sin(a1); + a13=cos(a11); + a12=(a12*a13); + a10=(a10-a12); + a12=cos(a1); + a12=(a12*a13); + a13=sin(a1); + a13=(a13*a9); + a12=(a12+a13); + a10=atan2(a10,a12); + a12=cos(a1); + a13=sin(a11); + a12=(a12*a13); + a9=sin(a1); + a11=cos(a11); + a9=(a9*a11); + a12=(a12-a9); + a9=cos(a1); + a9=(a9*a11); + a1=sin(a1); + a1=(a1*a13); + a9=(a9+a1); + a12=atan2(a12,a9); + a10=(a10*a12); + a3=(a3+a10); + a10=arg[0]? arg[0][12] : 0; + a12=arg[1]? arg[1][12] : 0; + a9=(a10-a12); + a9=(a0*a9); + a10=(a10-a12); + a9=(a9*a10); + a10=arg[0]? arg[0][13] : 0; + a12=arg[1]? arg[1][13] : 0; + a1=(a10-a12); + a1=(a0*a1); + a10=(a10-a12); + a1=(a1*a10); + a9=(a9+a1); + a3=(a3+a9); + a9=arg[0]? arg[0][54] : 0; + a1=casadi_sq(a9); + a10=arg[0]? arg[0][55] : 0; + a12=casadi_sq(a10); + a1=(a1+a12); + a3=(a3+a1); + a1=arg[1]? arg[1][14] : 0; + a12=cos(a1); + a13=arg[0]? arg[0][14] : 0; + a11=sin(a13); + a12=(a12*a11); + a14=sin(a1); + a15=cos(a13); + a14=(a14*a15); + a12=(a12-a14); + a14=cos(a1); + a14=(a14*a15); + a15=sin(a1); + a15=(a15*a11); + a14=(a14+a15); + a12=atan2(a12,a14); + a14=cos(a1); + a15=sin(a13); + a14=(a14*a15); + a11=sin(a1); + a13=cos(a13); + a11=(a11*a13); + a14=(a14-a11); + a11=cos(a1); + a11=(a11*a13); + a1=sin(a1); + a1=(a1*a15); + a11=(a11+a1); + a14=atan2(a14,a11); + a12=(a12*a14); + a3=(a3+a12); + a12=arg[0]? arg[0][15] : 0; + a14=arg[1]? arg[1][15] : 0; + a11=(a12-a14); + a11=(a0*a11); + a12=(a12-a14); + a11=(a11*a12); + a12=arg[0]? arg[0][16] : 0; + a14=arg[1]? arg[1][16] : 0; + a1=(a12-a14); + a1=(a0*a1); + a12=(a12-a14); + a1=(a1*a12); + a11=(a11+a1); + a3=(a3+a11); + a11=arg[0]? arg[0][56] : 0; + a1=casadi_sq(a11); + a12=arg[0]? arg[0][57] : 0; + a14=casadi_sq(a12); + a1=(a1+a14); + a3=(a3+a1); + a1=arg[1]? arg[1][17] : 0; + a14=cos(a1); + a15=arg[0]? arg[0][17] : 0; + a13=sin(a15); + a14=(a14*a13); + a16=sin(a1); + a17=cos(a15); + a16=(a16*a17); + a14=(a14-a16); + a16=cos(a1); + a16=(a16*a17); + a17=sin(a1); + a17=(a17*a13); + a16=(a16+a17); + a14=atan2(a14,a16); + a16=cos(a1); + a17=sin(a15); + a16=(a16*a17); + a13=sin(a1); + a15=cos(a15); + a13=(a13*a15); + a16=(a16-a13); + a13=cos(a1); + a13=(a13*a15); + a1=sin(a1); + a1=(a1*a17); + a13=(a13+a1); + a16=atan2(a16,a13); + a14=(a14*a16); + a3=(a3+a14); + a14=arg[0]? arg[0][18] : 0; + a16=arg[1]? arg[1][18] : 0; + a13=(a14-a16); + a13=(a0*a13); + a14=(a14-a16); + a13=(a13*a14); + a14=arg[0]? arg[0][19] : 0; + a16=arg[1]? arg[1][19] : 0; + a1=(a14-a16); + a1=(a0*a1); + a14=(a14-a16); + a1=(a1*a14); + a13=(a13+a1); + a3=(a3+a13); + a13=arg[0]? arg[0][58] : 0; + a1=casadi_sq(a13); + a14=arg[0]? arg[0][59] : 0; + a16=casadi_sq(a14); + a1=(a1+a16); + a3=(a3+a1); + a1=arg[1]? arg[1][20] : 0; + a16=cos(a1); + a17=arg[0]? arg[0][20] : 0; + a15=sin(a17); + a16=(a16*a15); + a18=sin(a1); + a19=cos(a17); + a18=(a18*a19); + a16=(a16-a18); + a18=cos(a1); + a18=(a18*a19); + a19=sin(a1); + a19=(a19*a15); + a18=(a18+a19); + a16=atan2(a16,a18); + a18=cos(a1); + a19=sin(a17); + a18=(a18*a19); + a15=sin(a1); + a17=cos(a17); + a15=(a15*a17); + a18=(a18-a15); + a15=cos(a1); + a15=(a15*a17); + a1=sin(a1); + a1=(a1*a19); + a15=(a15+a1); + a18=atan2(a18,a15); + a16=(a16*a18); + a3=(a3+a16); + a16=arg[0]? arg[0][21] : 0; + a18=arg[1]? arg[1][21] : 0; + a15=(a16-a18); + a15=(a0*a15); + a16=(a16-a18); + a15=(a15*a16); + a16=arg[0]? arg[0][22] : 0; + a18=arg[1]? arg[1][22] : 0; + a1=(a16-a18); + a1=(a0*a1); + a16=(a16-a18); + a1=(a1*a16); + a15=(a15+a1); + a3=(a3+a15); + a15=arg[0]? arg[0][60] : 0; + a1=casadi_sq(a15); + a16=arg[0]? arg[0][61] : 0; + a18=casadi_sq(a16); + a1=(a1+a18); + a3=(a3+a1); + a1=arg[1]? arg[1][23] : 0; + a18=cos(a1); + a19=arg[0]? arg[0][23] : 0; + a17=sin(a19); + a18=(a18*a17); + a20=sin(a1); + a21=cos(a19); + a20=(a20*a21); + a18=(a18-a20); + a20=cos(a1); + a20=(a20*a21); + a21=sin(a1); + a21=(a21*a17); + a20=(a20+a21); + a18=atan2(a18,a20); + a20=cos(a1); + a21=sin(a19); + a20=(a20*a21); + a17=sin(a1); + a19=cos(a19); + a17=(a17*a19); + a20=(a20-a17); + a17=cos(a1); + a17=(a17*a19); + a1=sin(a1); + a1=(a1*a21); + a17=(a17+a1); + a20=atan2(a20,a17); + a18=(a18*a20); + a3=(a3+a18); + a18=arg[0]? arg[0][24] : 0; + a20=arg[1]? arg[1][24] : 0; + a17=(a18-a20); + a17=(a0*a17); + a18=(a18-a20); + a17=(a17*a18); + a18=arg[0]? arg[0][25] : 0; + a20=arg[1]? arg[1][25] : 0; + a1=(a18-a20); + a1=(a0*a1); + a18=(a18-a20); + a1=(a1*a18); + a17=(a17+a1); + a3=(a3+a17); + a17=arg[0]? arg[0][62] : 0; + a1=casadi_sq(a17); + a18=arg[0]? arg[0][63] : 0; + a20=casadi_sq(a18); + a1=(a1+a20); + a3=(a3+a1); + a1=arg[1]? arg[1][26] : 0; + a20=cos(a1); + a21=arg[0]? arg[0][26] : 0; + a19=sin(a21); + a20=(a20*a19); + a22=sin(a1); + a23=cos(a21); + a22=(a22*a23); + a20=(a20-a22); + a22=cos(a1); + a22=(a22*a23); + a23=sin(a1); + a23=(a23*a19); + a22=(a22+a23); + a20=atan2(a20,a22); + a22=cos(a1); + a23=sin(a21); + a22=(a22*a23); + a19=sin(a1); + a21=cos(a21); + a19=(a19*a21); + a22=(a22-a19); + a19=cos(a1); + a19=(a19*a21); + a1=sin(a1); + a1=(a1*a23); + a19=(a19+a1); + a22=atan2(a22,a19); + a20=(a20*a22); + a3=(a3+a20); + a20=arg[0]? arg[0][27] : 0; + a22=arg[1]? arg[1][27] : 0; + a19=(a20-a22); + a19=(a0*a19); + a20=(a20-a22); + a19=(a19*a20); + a20=arg[0]? arg[0][28] : 0; + a22=arg[1]? arg[1][28] : 0; + a1=(a20-a22); + a1=(a0*a1); + a20=(a20-a22); + a1=(a1*a20); + a19=(a19+a1); + a3=(a3+a19); + a19=arg[0]? arg[0][64] : 0; + a1=casadi_sq(a19); + a20=arg[0]? arg[0][65] : 0; + a22=casadi_sq(a20); + a1=(a1+a22); + a3=(a3+a1); + a1=arg[1]? arg[1][29] : 0; + a22=cos(a1); + a23=arg[0]? arg[0][29] : 0; + a21=sin(a23); + a22=(a22*a21); + a24=sin(a1); + a25=cos(a23); + a24=(a24*a25); + a22=(a22-a24); + a24=cos(a1); + a24=(a24*a25); + a25=sin(a1); + a25=(a25*a21); + a24=(a24+a25); + a22=atan2(a22,a24); + a24=cos(a1); + a25=sin(a23); + a24=(a24*a25); + a21=sin(a1); + a23=cos(a23); + a21=(a21*a23); + a24=(a24-a21); + a21=cos(a1); + a21=(a21*a23); + a1=sin(a1); + a1=(a1*a25); + a21=(a21+a1); + a24=atan2(a24,a21); + a22=(a22*a24); + a3=(a3+a22); + a22=arg[0]? arg[0][30] : 0; + a24=arg[1]? arg[1][30] : 0; + a21=(a22-a24); + a21=(a0*a21); + a22=(a22-a24); + a21=(a21*a22); + a22=arg[0]? arg[0][31] : 0; + a24=arg[1]? arg[1][31] : 0; + a1=(a22-a24); + a1=(a0*a1); + a22=(a22-a24); + a1=(a1*a22); + a21=(a21+a1); + a3=(a3+a21); + a21=arg[0]? arg[0][66] : 0; + a1=casadi_sq(a21); + a22=arg[0]? arg[0][67] : 0; + a24=casadi_sq(a22); + a1=(a1+a24); + a3=(a3+a1); + a1=arg[1]? arg[1][32] : 0; + a24=cos(a1); + a25=arg[0]? arg[0][32] : 0; + a23=sin(a25); + a24=(a24*a23); + a26=sin(a1); + a27=cos(a25); + a26=(a26*a27); + a24=(a24-a26); + a26=cos(a1); + a26=(a26*a27); + a27=sin(a1); + a27=(a27*a23); + a26=(a26+a27); + a24=atan2(a24,a26); + a26=cos(a1); + a27=sin(a25); + a26=(a26*a27); + a23=sin(a1); + a25=cos(a25); + a23=(a23*a25); + a26=(a26-a23); + a23=cos(a1); + a23=(a23*a25); + a1=sin(a1); + a1=(a1*a27); + a23=(a23+a1); + a26=atan2(a26,a23); + a24=(a24*a26); + a3=(a3+a24); + a24=arg[0]? arg[0][33] : 0; + a26=arg[1]? arg[1][33] : 0; + a23=(a24-a26); + a23=(a0*a23); + a24=(a24-a26); + a23=(a23*a24); + a24=arg[0]? arg[0][34] : 0; + a26=arg[1]? arg[1][34] : 0; + a1=(a24-a26); + a1=(a0*a1); + a24=(a24-a26); + a1=(a1*a24); + a23=(a23+a1); + a3=(a3+a23); + a23=arg[0]? arg[0][68] : 0; + a1=casadi_sq(a23); + a24=arg[0]? arg[0][69] : 0; + a26=casadi_sq(a24); + a1=(a1+a26); + a3=(a3+a1); + a1=arg[1]? arg[1][35] : 0; + a26=cos(a1); + a27=arg[0]? arg[0][35] : 0; + a25=sin(a27); + a26=(a26*a25); + a28=sin(a1); + a29=cos(a27); + a28=(a28*a29); + a26=(a26-a28); + a28=cos(a1); + a28=(a28*a29); + a29=sin(a1); + a29=(a29*a25); + a28=(a28+a29); + a26=atan2(a26,a28); + a28=cos(a1); + a29=sin(a27); + a28=(a28*a29); + a25=sin(a1); + a27=cos(a27); + a25=(a25*a27); + a28=(a28-a25); + a25=cos(a1); + a25=(a25*a27); + a1=sin(a1); + a1=(a1*a29); + a25=(a25+a1); + a28=atan2(a28,a25); + a26=(a26*a28); + a3=(a3+a26); + a26=arg[0]? arg[0][36] : 0; + a28=arg[1]? arg[1][36] : 0; + a25=(a26-a28); + a25=(a0*a25); + a26=(a26-a28); + a25=(a25*a26); + a26=arg[0]? arg[0][37] : 0; + a28=arg[1]? arg[1][37] : 0; + a1=(a26-a28); + a1=(a0*a1); + a26=(a26-a28); + a1=(a1*a26); + a25=(a25+a1); + a3=(a3+a25); + a25=arg[0]? arg[0][70] : 0; + a1=casadi_sq(a25); + a26=arg[0]? arg[0][71] : 0; + a28=casadi_sq(a26); + a1=(a1+a28); + a3=(a3+a1); + a1=arg[1]? arg[1][38] : 0; + a28=cos(a1); + a29=arg[0]? arg[0][38] : 0; + a27=sin(a29); + a28=(a28*a27); + a30=sin(a1); + a31=cos(a29); + a30=(a30*a31); + a28=(a28-a30); + a30=cos(a1); + a30=(a30*a31); + a31=sin(a1); + a31=(a31*a27); + a30=(a30+a31); + a28=atan2(a28,a30); + a30=cos(a1); + a31=sin(a29); + a30=(a30*a31); + a27=sin(a1); + a29=cos(a29); + a27=(a27*a29); + a30=(a30-a27); + a27=cos(a1); + a27=(a27*a29); + a1=sin(a1); + a1=(a1*a31); + a27=(a27+a1); + a30=atan2(a30,a27); + a28=(a28*a30); + a3=(a3+a28); + a28=arg[0]? arg[0][39] : 0; + a30=arg[1]? arg[1][39] : 0; + a27=(a28-a30); + a27=(a0*a27); + a28=(a28-a30); + a27=(a27*a28); + a28=arg[0]? arg[0][40] : 0; + a30=arg[1]? arg[1][40] : 0; + a1=(a28-a30); + a1=(a0*a1); + a28=(a28-a30); + a1=(a1*a28); + a27=(a27+a1); + a3=(a3+a27); + a27=arg[0]? arg[0][72] : 0; + a1=casadi_sq(a27); + a28=arg[0]? arg[0][73] : 0; + a30=casadi_sq(a28); + a1=(a1+a30); + a3=(a3+a1); + a1=arg[1]? arg[1][41] : 0; + a30=cos(a1); + a31=arg[0]? arg[0][41] : 0; + a29=sin(a31); + a30=(a30*a29); + a32=sin(a1); + a33=cos(a31); + a32=(a32*a33); + a30=(a30-a32); + a32=cos(a1); + a32=(a32*a33); + a33=sin(a1); + a33=(a33*a29); + a32=(a32+a33); + a30=atan2(a30,a32); + a32=cos(a1); + a33=sin(a31); + a32=(a32*a33); + a29=sin(a1); + a31=cos(a31); + a29=(a29*a31); + a32=(a32-a29); + a29=cos(a1); + a29=(a29*a31); + a1=sin(a1); + a1=(a1*a33); + a29=(a29+a1); + a32=atan2(a32,a29); + a30=(a30*a32); + a3=(a3+a30); + a30=arg[0]? arg[0][42] : 0; + a32=arg[1]? arg[1][42] : 0; + a29=(a30-a32); + a29=(a0*a29); + a30=(a30-a32); + a29=(a29*a30); + a30=arg[0]? arg[0][43] : 0; + a32=arg[1]? arg[1][43] : 0; + a1=(a30-a32); + a1=(a0*a1); + a30=(a30-a32); + a1=(a1*a30); + a29=(a29+a1); + a3=(a3+a29); + a29=arg[0]? arg[0][74] : 0; + a1=casadi_sq(a29); + a30=arg[0]? arg[0][75] : 0; + a32=casadi_sq(a30); + a1=(a1+a32); + a3=(a3+a1); + a1=arg[1]? arg[1][44] : 0; + a32=cos(a1); + a33=arg[0]? arg[0][44] : 0; + a31=sin(a33); + a32=(a32*a31); + a34=sin(a1); + a35=cos(a33); + a34=(a34*a35); + a32=(a32-a34); + a34=cos(a1); + a34=(a34*a35); + a35=sin(a1); + a35=(a35*a31); + a34=(a34+a35); + a32=atan2(a32,a34); + a34=cos(a1); + a35=sin(a33); + a34=(a34*a35); + a31=sin(a1); + a33=cos(a33); + a31=(a31*a33); + a34=(a34-a31); + a31=cos(a1); + a31=(a31*a33); + a1=sin(a1); + a1=(a1*a35); + a31=(a31+a1); + a34=atan2(a34,a31); + a32=(a32*a34); + a3=(a3+a32); + a32=arg[0]? arg[0][45] : 0; + a34=arg[1]? arg[1][45] : 0; + a31=(a32-a34); + a31=(a0*a31); + a32=(a32-a34); + a31=(a31*a32); + a32=arg[0]? arg[0][46] : 0; + a34=arg[1]? arg[1][46] : 0; + a1=(a32-a34); + a0=(a0*a1); + a32=(a32-a34); + a0=(a0*a32); + a31=(a31+a0); + a3=(a3+a31); + a31=arg[0]? arg[0][76] : 0; + a0=casadi_sq(a31); + a32=arg[0]? arg[0][77] : 0; + a34=casadi_sq(a32); + a0=(a0+a34); + a3=(a3+a0); + a0=arg[1]? arg[1][47] : 0; + a34=cos(a0); + a1=arg[0]? arg[0][47] : 0; + a35=sin(a1); + a34=(a34*a35); + a33=sin(a0); + a36=cos(a1); + a33=(a33*a36); + a34=(a34-a33); + a33=cos(a0); + a33=(a33*a36); + a36=sin(a0); + a36=(a36*a35); + a33=(a33+a36); + a34=atan2(a34,a33); + a33=cos(a0); + a36=sin(a1); + a33=(a33*a36); + a35=sin(a0); + a1=cos(a1); + a35=(a35*a1); + a33=(a33-a35); + a35=cos(a0); + a35=(a35*a1); + a0=sin(a0); + a0=(a0*a36); + a35=(a35+a0); + a33=atan2(a33,a35); + a34=(a34*a33); + a3=(a3+a34); + a34=2.; + a33=arg[1]? arg[1][48] : 0; + a35=(a4-a33); + a35=(a34*a35); + a33=(a4-a33); + a35=(a35*a33); + a33=5.0000000000000000e-01; + a0=arg[1]? arg[1][49] : 0; + a36=(a2-a0); + a36=(a33*a36); + a0=(a2-a0); + a36=(a36*a0); + a35=(a35+a36); + a3=(a3+a35); + a35=(a7-a4); + a35=(a34*a35); + a4=(a7-a4); + a35=(a35*a4); + a4=(a5-a2); + a4=(a33*a4); + a2=(a5-a2); + a4=(a4*a2); + a35=(a35+a4); + a3=(a3+a35); + a35=(a6-a7); + a35=(a34*a35); + a7=(a6-a7); + a35=(a35*a7); + a7=(a8-a5); + a7=(a33*a7); + a5=(a8-a5); + a7=(a7*a5); + a35=(a35+a7); + a3=(a3+a35); + a35=(a9-a6); + a35=(a34*a35); + a6=(a9-a6); + a35=(a35*a6); + a6=(a10-a8); + a6=(a33*a6); + a8=(a10-a8); + a6=(a6*a8); + a35=(a35+a6); + a3=(a3+a35); + a35=(a11-a9); + a35=(a34*a35); + a9=(a11-a9); + a35=(a35*a9); + a9=(a12-a10); + a9=(a33*a9); + a10=(a12-a10); + a9=(a9*a10); + a35=(a35+a9); + a3=(a3+a35); + a35=(a13-a11); + a35=(a34*a35); + a11=(a13-a11); + a35=(a35*a11); + a11=(a14-a12); + a11=(a33*a11); + a12=(a14-a12); + a11=(a11*a12); + a35=(a35+a11); + a3=(a3+a35); + a35=(a15-a13); + a35=(a34*a35); + a13=(a15-a13); + a35=(a35*a13); + a13=(a16-a14); + a13=(a33*a13); + a14=(a16-a14); + a13=(a13*a14); + a35=(a35+a13); + a3=(a3+a35); + a35=(a17-a15); + a35=(a34*a35); + a15=(a17-a15); + a35=(a35*a15); + a15=(a18-a16); + a15=(a33*a15); + a16=(a18-a16); + a15=(a15*a16); + a35=(a35+a15); + a3=(a3+a35); + a35=(a19-a17); + a35=(a34*a35); + a17=(a19-a17); + a35=(a35*a17); + a17=(a20-a18); + a17=(a33*a17); + a18=(a20-a18); + a17=(a17*a18); + a35=(a35+a17); + a3=(a3+a35); + a35=(a21-a19); + a35=(a34*a35); + a19=(a21-a19); + a35=(a35*a19); + a19=(a22-a20); + a19=(a33*a19); + a20=(a22-a20); + a19=(a19*a20); + a35=(a35+a19); + a3=(a3+a35); + a35=(a23-a21); + a35=(a34*a35); + a21=(a23-a21); + a35=(a35*a21); + a21=(a24-a22); + a21=(a33*a21); + a22=(a24-a22); + a21=(a21*a22); + a35=(a35+a21); + a3=(a3+a35); + a35=(a25-a23); + a35=(a34*a35); + a23=(a25-a23); + a35=(a35*a23); + a23=(a26-a24); + a23=(a33*a23); + a24=(a26-a24); + a23=(a23*a24); + a35=(a35+a23); + a3=(a3+a35); + a35=(a27-a25); + a35=(a34*a35); + a25=(a27-a25); + a35=(a35*a25); + a25=(a28-a26); + a25=(a33*a25); + a26=(a28-a26); + a25=(a25*a26); + a35=(a35+a25); + a3=(a3+a35); + a35=(a29-a27); + a35=(a34*a35); + a27=(a29-a27); + a35=(a35*a27); + a27=(a30-a28); + a27=(a33*a27); + a28=(a30-a28); + a27=(a27*a28); + a35=(a35+a27); + a3=(a3+a35); + a35=(a31-a29); + a34=(a34*a35); + a31=(a31-a29); + a34=(a34*a31); + a31=(a32-a30); + a33=(a33*a31); + a32=(a32-a30); + a33=(a33*a32); + a34=(a34+a33); + a3=(a3+a34); + if (res[0]!=0) res[0][0]=a3; + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_f(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f1(arg, res, iw, w, mem); +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_f_alloc_mem(void) { + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_f_init_mem(int mem) { + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_f_free_mem(int mem) { +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_f_checkout(void) { + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_f_release(int mem) { +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_f_incref(void) { +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_f_decref(void) { +} + +extern "C" CASADI_SYMBOL_EXPORT casadi_int nlp_f_n_in(void) { return 2;} + +extern "C" CASADI_SYMBOL_EXPORT casadi_int nlp_f_n_out(void) { return 1;} + +extern "C" CASADI_SYMBOL_EXPORT casadi_real nlp_f_default_in(casadi_int i) { + switch (i) { + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const char* nlp_f_name_in(casadi_int i) { + switch (i) { + case 0: return "x"; + case 1: return "p"; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const char* nlp_f_name_out(casadi_int i) { + switch (i) { + case 0: return "f"; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const casadi_int* nlp_f_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s1; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const casadi_int* nlp_f_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s2; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_f_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 2; + if (sz_res) *sz_res = 1; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 0; + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_f_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 2*sizeof(const casadi_real*); + if (sz_res) *sz_res = 1*sizeof(casadi_real*); + if (sz_iw) *sz_iw = 0*sizeof(casadi_int); + if (sz_w) *sz_w = 0*sizeof(casadi_real); + return 0; +} + +/* nlp_g:(x[78],p[50])->(g[63]) */ +static int casadi_f2(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_real a0, a1, a10, a11, a12, a13, a14, a15, a16, a17, a18, a19, a2, a20, a21, a22, a23, a24, a25, a26, a27, a28, a29, a3, a30, a31, a32, a33, a34, a35, a36, a37, a38, a39, a4, a40, a41, a42, a43, a44, a45, a46, a5, a6, a7, a8, a9; + a0=arg[0]? arg[0][0] : 0; + a1=arg[1]? arg[1][0] : 0; + a1=(a0-a1); + if (res[0]!=0) res[0][0]=a1; + a1=arg[0]? arg[0][1] : 0; + a2=arg[1]? arg[1][1] : 0; + a2=(a1-a2); + if (res[0]!=0) res[0][1]=a2; + a2=arg[0]? arg[0][2] : 0; + a3=arg[1]? arg[1][2] : 0; + a3=(a2-a3); + if (res[0]!=0) res[0][2]=a3; + a3=arg[0]? arg[0][3] : 0; + a4=4.1666666666666664e-02; + a5=arg[0]? arg[0][48] : 0; + a6=cos(a2); + a6=(a5*a6); + a7=2.; + a8=1.2500000000000000e-01; + a9=4.0000000000000002e-01; + a10=arg[1]? arg[1][49] : 0; + a11=(a9*a10); + a12=5.9999999999999998e-01; + a13=arg[0]? arg[0][49] : 0; + a14=(a12*a13); + a11=(a11+a14); + a14=(a8*a11); + a14=(a2+a14); + a15=cos(a14); + a15=(a5*a15); + a15=(a7*a15); + a6=(a6+a15); + a15=(a9*a10); + a16=(a12*a13); + a15=(a15+a16); + a16=(a8*a15); + a16=(a2+a16); + a17=cos(a16); + a17=(a5*a17); + a17=(a7*a17); + a6=(a6+a17); + a17=2.5000000000000000e-01; + a18=(a9*a10); + a19=(a12*a13); + a18=(a18+a19); + a19=(a17*a18); + a19=(a2+a19); + a20=cos(a19); + a20=(a5*a20); + a6=(a6+a20); + a6=(a4*a6); + a6=(a0+a6); + a6=(a3-a6); + if (res[0]!=0) res[0][3]=a6; + a6=arg[0]? arg[0][4] : 0; + a20=sin(a2); + a20=(a5*a20); + a14=sin(a14); + a14=(a5*a14); + a14=(a7*a14); + a20=(a20+a14); + a16=sin(a16); + a16=(a5*a16); + a16=(a7*a16); + a20=(a20+a16); + a19=sin(a19); + a5=(a5*a19); + a20=(a20+a5); + a20=(a4*a20); + a20=(a1+a20); + a20=(a6-a20); + if (res[0]!=0) res[0][4]=a20; + a20=arg[0]? arg[0][5] : 0; + a5=cos(a20); + a15=(a7*a15); + a11=(a11+a15); + a18=(a7*a18); + a11=(a11+a18); + a18=(a9*a10); + a15=(a12*a13); + a18=(a18+a15); + a11=(a11+a18); + a11=(a4*a11); + a2=(a2+a11); + a11=sin(a2); + a5=(a5*a11); + a18=sin(a20); + a2=cos(a2); + a18=(a18*a2); + a5=(a5-a18); + a18=cos(a20); + a18=(a18*a2); + a2=sin(a20); + a2=(a2*a11); + a18=(a18+a2); + a5=atan2(a5,a18); + if (res[0]!=0) res[0][5]=a5; + a5=arg[0]? arg[0][6] : 0; + a18=arg[0]? arg[0][50] : 0; + a2=cos(a20); + a2=(a18*a2); + a13=(a12*a13); + a10=(a9*a10); + a13=(a13+a10); + a10=(a9*a13); + a11=arg[0]? arg[0][51] : 0; + a15=(a12*a11); + a10=(a10+a15); + a15=(a8*a10); + a15=(a20+a15); + a19=cos(a15); + a19=(a18*a19); + a19=(a7*a19); + a2=(a2+a19); + a19=(a9*a13); + a16=(a12*a11); + a19=(a19+a16); + a16=(a8*a19); + a16=(a20+a16); + a14=cos(a16); + a14=(a18*a14); + a14=(a7*a14); + a2=(a2+a14); + a14=(a9*a13); + a21=(a12*a11); + a14=(a14+a21); + a21=(a17*a14); + a21=(a20+a21); + a22=cos(a21); + a22=(a18*a22); + a2=(a2+a22); + a2=(a4*a2); + a2=(a3+a2); + a2=(a5-a2); + if (res[0]!=0) res[0][6]=a2; + a2=arg[0]? arg[0][7] : 0; + a22=sin(a20); + a22=(a18*a22); + a15=sin(a15); + a15=(a18*a15); + a15=(a7*a15); + a22=(a22+a15); + a16=sin(a16); + a16=(a18*a16); + a16=(a7*a16); + a22=(a22+a16); + a21=sin(a21); + a18=(a18*a21); + a22=(a22+a18); + a22=(a4*a22); + a22=(a6+a22); + a22=(a2-a22); + if (res[0]!=0) res[0][7]=a22; + a22=arg[0]? arg[0][8] : 0; + a18=cos(a22); + a19=(a7*a19); + a10=(a10+a19); + a14=(a7*a14); + a10=(a10+a14); + a14=(a9*a13); + a19=(a12*a11); + a14=(a14+a19); + a10=(a10+a14); + a10=(a4*a10); + a20=(a20+a10); + a10=sin(a20); + a18=(a18*a10); + a14=sin(a22); + a20=cos(a20); + a14=(a14*a20); + a18=(a18-a14); + a14=cos(a22); + a14=(a14*a20); + a20=sin(a22); + a20=(a20*a10); + a14=(a14+a20); + a18=atan2(a18,a14); + if (res[0]!=0) res[0][8]=a18; + a18=arg[0]? arg[0][9] : 0; + a14=arg[0]? arg[0][52] : 0; + a20=cos(a22); + a20=(a14*a20); + a11=(a12*a11); + a13=(a9*a13); + a11=(a11+a13); + a13=(a9*a11); + a10=arg[0]? arg[0][53] : 0; + a19=(a12*a10); + a13=(a13+a19); + a19=(a8*a13); + a19=(a22+a19); + a21=cos(a19); + a21=(a14*a21); + a21=(a7*a21); + a20=(a20+a21); + a21=(a9*a11); + a16=(a12*a10); + a21=(a21+a16); + a16=(a8*a21); + a16=(a22+a16); + a15=cos(a16); + a15=(a14*a15); + a15=(a7*a15); + a20=(a20+a15); + a15=(a9*a11); + a23=(a12*a10); + a15=(a15+a23); + a23=(a17*a15); + a23=(a22+a23); + a24=cos(a23); + a24=(a14*a24); + a20=(a20+a24); + a20=(a4*a20); + a20=(a5+a20); + a20=(a18-a20); + if (res[0]!=0) res[0][9]=a20; + a20=arg[0]? arg[0][10] : 0; + a24=sin(a22); + a24=(a14*a24); + a19=sin(a19); + a19=(a14*a19); + a19=(a7*a19); + a24=(a24+a19); + a16=sin(a16); + a16=(a14*a16); + a16=(a7*a16); + a24=(a24+a16); + a23=sin(a23); + a14=(a14*a23); + a24=(a24+a14); + a24=(a4*a24); + a24=(a2+a24); + a24=(a20-a24); + if (res[0]!=0) res[0][10]=a24; + a24=arg[0]? arg[0][11] : 0; + a14=cos(a24); + a21=(a7*a21); + a13=(a13+a21); + a15=(a7*a15); + a13=(a13+a15); + a15=(a9*a11); + a21=(a12*a10); + a15=(a15+a21); + a13=(a13+a15); + a13=(a4*a13); + a22=(a22+a13); + a13=sin(a22); + a14=(a14*a13); + a15=sin(a24); + a22=cos(a22); + a15=(a15*a22); + a14=(a14-a15); + a15=cos(a24); + a15=(a15*a22); + a22=sin(a24); + a22=(a22*a13); + a15=(a15+a22); + a14=atan2(a14,a15); + if (res[0]!=0) res[0][11]=a14; + a14=arg[0]? arg[0][12] : 0; + a15=arg[0]? arg[0][54] : 0; + a22=cos(a24); + a22=(a15*a22); + a10=(a12*a10); + a11=(a9*a11); + a10=(a10+a11); + a11=(a9*a10); + a13=arg[0]? arg[0][55] : 0; + a21=(a12*a13); + a11=(a11+a21); + a21=(a8*a11); + a21=(a24+a21); + a23=cos(a21); + a23=(a15*a23); + a23=(a7*a23); + a22=(a22+a23); + a23=(a9*a10); + a16=(a12*a13); + a23=(a23+a16); + a16=(a8*a23); + a16=(a24+a16); + a19=cos(a16); + a19=(a15*a19); + a19=(a7*a19); + a22=(a22+a19); + a19=(a9*a10); + a25=(a12*a13); + a19=(a19+a25); + a25=(a17*a19); + a25=(a24+a25); + a26=cos(a25); + a26=(a15*a26); + a22=(a22+a26); + a22=(a4*a22); + a22=(a18+a22); + a22=(a14-a22); + if (res[0]!=0) res[0][12]=a22; + a22=arg[0]? arg[0][13] : 0; + a26=sin(a24); + a26=(a15*a26); + a21=sin(a21); + a21=(a15*a21); + a21=(a7*a21); + a26=(a26+a21); + a16=sin(a16); + a16=(a15*a16); + a16=(a7*a16); + a26=(a26+a16); + a25=sin(a25); + a15=(a15*a25); + a26=(a26+a15); + a26=(a4*a26); + a26=(a20+a26); + a26=(a22-a26); + if (res[0]!=0) res[0][13]=a26; + a26=arg[0]? arg[0][14] : 0; + a15=cos(a26); + a23=(a7*a23); + a11=(a11+a23); + a19=(a7*a19); + a11=(a11+a19); + a19=(a9*a10); + a23=(a12*a13); + a19=(a19+a23); + a11=(a11+a19); + a11=(a4*a11); + a24=(a24+a11); + a11=sin(a24); + a15=(a15*a11); + a19=sin(a26); + a24=cos(a24); + a19=(a19*a24); + a15=(a15-a19); + a19=cos(a26); + a19=(a19*a24); + a24=sin(a26); + a24=(a24*a11); + a19=(a19+a24); + a15=atan2(a15,a19); + if (res[0]!=0) res[0][14]=a15; + a15=arg[0]? arg[0][15] : 0; + a19=arg[0]? arg[0][56] : 0; + a24=cos(a26); + a24=(a19*a24); + a13=(a12*a13); + a10=(a9*a10); + a13=(a13+a10); + a10=(a9*a13); + a11=arg[0]? arg[0][57] : 0; + a23=(a12*a11); + a10=(a10+a23); + a23=(a8*a10); + a23=(a26+a23); + a25=cos(a23); + a25=(a19*a25); + a25=(a7*a25); + a24=(a24+a25); + a25=(a9*a13); + a16=(a12*a11); + a25=(a25+a16); + a16=(a8*a25); + a16=(a26+a16); + a21=cos(a16); + a21=(a19*a21); + a21=(a7*a21); + a24=(a24+a21); + a21=(a9*a13); + a27=(a12*a11); + a21=(a21+a27); + a27=(a17*a21); + a27=(a26+a27); + a28=cos(a27); + a28=(a19*a28); + a24=(a24+a28); + a24=(a4*a24); + a24=(a14+a24); + a24=(a15-a24); + if (res[0]!=0) res[0][15]=a24; + a24=arg[0]? arg[0][16] : 0; + a28=sin(a26); + a28=(a19*a28); + a23=sin(a23); + a23=(a19*a23); + a23=(a7*a23); + a28=(a28+a23); + a16=sin(a16); + a16=(a19*a16); + a16=(a7*a16); + a28=(a28+a16); + a27=sin(a27); + a19=(a19*a27); + a28=(a28+a19); + a28=(a4*a28); + a28=(a22+a28); + a28=(a24-a28); + if (res[0]!=0) res[0][16]=a28; + a28=arg[0]? arg[0][17] : 0; + a19=cos(a28); + a25=(a7*a25); + a10=(a10+a25); + a21=(a7*a21); + a10=(a10+a21); + a21=(a9*a13); + a25=(a12*a11); + a21=(a21+a25); + a10=(a10+a21); + a10=(a4*a10); + a26=(a26+a10); + a10=sin(a26); + a19=(a19*a10); + a21=sin(a28); + a26=cos(a26); + a21=(a21*a26); + a19=(a19-a21); + a21=cos(a28); + a21=(a21*a26); + a26=sin(a28); + a26=(a26*a10); + a21=(a21+a26); + a19=atan2(a19,a21); + if (res[0]!=0) res[0][17]=a19; + a19=arg[0]? arg[0][18] : 0; + a21=arg[0]? arg[0][58] : 0; + a26=cos(a28); + a26=(a21*a26); + a11=(a12*a11); + a13=(a9*a13); + a11=(a11+a13); + a13=(a9*a11); + a10=arg[0]? arg[0][59] : 0; + a25=(a12*a10); + a13=(a13+a25); + a25=(a8*a13); + a25=(a28+a25); + a27=cos(a25); + a27=(a21*a27); + a27=(a7*a27); + a26=(a26+a27); + a27=(a9*a11); + a16=(a12*a10); + a27=(a27+a16); + a16=(a8*a27); + a16=(a28+a16); + a23=cos(a16); + a23=(a21*a23); + a23=(a7*a23); + a26=(a26+a23); + a23=(a9*a11); + a29=(a12*a10); + a23=(a23+a29); + a29=(a17*a23); + a29=(a28+a29); + a30=cos(a29); + a30=(a21*a30); + a26=(a26+a30); + a26=(a4*a26); + a26=(a15+a26); + a26=(a19-a26); + if (res[0]!=0) res[0][18]=a26; + a26=arg[0]? arg[0][19] : 0; + a30=sin(a28); + a30=(a21*a30); + a25=sin(a25); + a25=(a21*a25); + a25=(a7*a25); + a30=(a30+a25); + a16=sin(a16); + a16=(a21*a16); + a16=(a7*a16); + a30=(a30+a16); + a29=sin(a29); + a21=(a21*a29); + a30=(a30+a21); + a30=(a4*a30); + a30=(a24+a30); + a30=(a26-a30); + if (res[0]!=0) res[0][19]=a30; + a30=arg[0]? arg[0][20] : 0; + a21=cos(a30); + a27=(a7*a27); + a13=(a13+a27); + a23=(a7*a23); + a13=(a13+a23); + a23=(a9*a11); + a27=(a12*a10); + a23=(a23+a27); + a13=(a13+a23); + a13=(a4*a13); + a28=(a28+a13); + a13=sin(a28); + a21=(a21*a13); + a23=sin(a30); + a28=cos(a28); + a23=(a23*a28); + a21=(a21-a23); + a23=cos(a30); + a23=(a23*a28); + a28=sin(a30); + a28=(a28*a13); + a23=(a23+a28); + a21=atan2(a21,a23); + if (res[0]!=0) res[0][20]=a21; + a21=arg[0]? arg[0][21] : 0; + a23=arg[0]? arg[0][60] : 0; + a28=cos(a30); + a28=(a23*a28); + a10=(a12*a10); + a11=(a9*a11); + a10=(a10+a11); + a11=(a9*a10); + a13=arg[0]? arg[0][61] : 0; + a27=(a12*a13); + a11=(a11+a27); + a27=(a8*a11); + a27=(a30+a27); + a29=cos(a27); + a29=(a23*a29); + a29=(a7*a29); + a28=(a28+a29); + a29=(a9*a10); + a16=(a12*a13); + a29=(a29+a16); + a16=(a8*a29); + a16=(a30+a16); + a25=cos(a16); + a25=(a23*a25); + a25=(a7*a25); + a28=(a28+a25); + a25=(a9*a10); + a31=(a12*a13); + a25=(a25+a31); + a31=(a17*a25); + a31=(a30+a31); + a32=cos(a31); + a32=(a23*a32); + a28=(a28+a32); + a28=(a4*a28); + a28=(a19+a28); + a28=(a21-a28); + if (res[0]!=0) res[0][21]=a28; + a28=arg[0]? arg[0][22] : 0; + a32=sin(a30); + a32=(a23*a32); + a27=sin(a27); + a27=(a23*a27); + a27=(a7*a27); + a32=(a32+a27); + a16=sin(a16); + a16=(a23*a16); + a16=(a7*a16); + a32=(a32+a16); + a31=sin(a31); + a23=(a23*a31); + a32=(a32+a23); + a32=(a4*a32); + a32=(a26+a32); + a32=(a28-a32); + if (res[0]!=0) res[0][22]=a32; + a32=arg[0]? arg[0][23] : 0; + a23=cos(a32); + a29=(a7*a29); + a11=(a11+a29); + a25=(a7*a25); + a11=(a11+a25); + a25=(a9*a10); + a29=(a12*a13); + a25=(a25+a29); + a11=(a11+a25); + a11=(a4*a11); + a30=(a30+a11); + a11=sin(a30); + a23=(a23*a11); + a25=sin(a32); + a30=cos(a30); + a25=(a25*a30); + a23=(a23-a25); + a25=cos(a32); + a25=(a25*a30); + a30=sin(a32); + a30=(a30*a11); + a25=(a25+a30); + a23=atan2(a23,a25); + if (res[0]!=0) res[0][23]=a23; + a23=arg[0]? arg[0][24] : 0; + a25=arg[0]? arg[0][62] : 0; + a30=cos(a32); + a30=(a25*a30); + a13=(a12*a13); + a10=(a9*a10); + a13=(a13+a10); + a10=(a9*a13); + a11=arg[0]? arg[0][63] : 0; + a29=(a12*a11); + a10=(a10+a29); + a29=(a8*a10); + a29=(a32+a29); + a31=cos(a29); + a31=(a25*a31); + a31=(a7*a31); + a30=(a30+a31); + a31=(a9*a13); + a16=(a12*a11); + a31=(a31+a16); + a16=(a8*a31); + a16=(a32+a16); + a27=cos(a16); + a27=(a25*a27); + a27=(a7*a27); + a30=(a30+a27); + a27=(a9*a13); + a33=(a12*a11); + a27=(a27+a33); + a33=(a17*a27); + a33=(a32+a33); + a34=cos(a33); + a34=(a25*a34); + a30=(a30+a34); + a30=(a4*a30); + a30=(a21+a30); + a30=(a23-a30); + if (res[0]!=0) res[0][24]=a30; + a30=arg[0]? arg[0][25] : 0; + a34=sin(a32); + a34=(a25*a34); + a29=sin(a29); + a29=(a25*a29); + a29=(a7*a29); + a34=(a34+a29); + a16=sin(a16); + a16=(a25*a16); + a16=(a7*a16); + a34=(a34+a16); + a33=sin(a33); + a25=(a25*a33); + a34=(a34+a25); + a34=(a4*a34); + a34=(a28+a34); + a34=(a30-a34); + if (res[0]!=0) res[0][25]=a34; + a34=arg[0]? arg[0][26] : 0; + a25=cos(a34); + a31=(a7*a31); + a10=(a10+a31); + a27=(a7*a27); + a10=(a10+a27); + a27=(a9*a13); + a31=(a12*a11); + a27=(a27+a31); + a10=(a10+a27); + a10=(a4*a10); + a32=(a32+a10); + a10=sin(a32); + a25=(a25*a10); + a27=sin(a34); + a32=cos(a32); + a27=(a27*a32); + a25=(a25-a27); + a27=cos(a34); + a27=(a27*a32); + a32=sin(a34); + a32=(a32*a10); + a27=(a27+a32); + a25=atan2(a25,a27); + if (res[0]!=0) res[0][26]=a25; + a25=arg[0]? arg[0][27] : 0; + a27=arg[0]? arg[0][64] : 0; + a32=cos(a34); + a32=(a27*a32); + a11=(a12*a11); + a13=(a9*a13); + a11=(a11+a13); + a13=(a9*a11); + a10=arg[0]? arg[0][65] : 0; + a31=(a12*a10); + a13=(a13+a31); + a31=(a8*a13); + a31=(a34+a31); + a33=cos(a31); + a33=(a27*a33); + a33=(a7*a33); + a32=(a32+a33); + a33=(a9*a11); + a16=(a12*a10); + a33=(a33+a16); + a16=(a8*a33); + a16=(a34+a16); + a29=cos(a16); + a29=(a27*a29); + a29=(a7*a29); + a32=(a32+a29); + a29=(a9*a11); + a35=(a12*a10); + a29=(a29+a35); + a35=(a17*a29); + a35=(a34+a35); + a36=cos(a35); + a36=(a27*a36); + a32=(a32+a36); + a32=(a4*a32); + a32=(a23+a32); + a32=(a25-a32); + if (res[0]!=0) res[0][27]=a32; + a32=arg[0]? arg[0][28] : 0; + a36=sin(a34); + a36=(a27*a36); + a31=sin(a31); + a31=(a27*a31); + a31=(a7*a31); + a36=(a36+a31); + a16=sin(a16); + a16=(a27*a16); + a16=(a7*a16); + a36=(a36+a16); + a35=sin(a35); + a27=(a27*a35); + a36=(a36+a27); + a36=(a4*a36); + a36=(a30+a36); + a36=(a32-a36); + if (res[0]!=0) res[0][28]=a36; + a36=arg[0]? arg[0][29] : 0; + a27=cos(a36); + a33=(a7*a33); + a13=(a13+a33); + a29=(a7*a29); + a13=(a13+a29); + a29=(a9*a11); + a33=(a12*a10); + a29=(a29+a33); + a13=(a13+a29); + a13=(a4*a13); + a34=(a34+a13); + a13=sin(a34); + a27=(a27*a13); + a29=sin(a36); + a34=cos(a34); + a29=(a29*a34); + a27=(a27-a29); + a29=cos(a36); + a29=(a29*a34); + a34=sin(a36); + a34=(a34*a13); + a29=(a29+a34); + a27=atan2(a27,a29); + if (res[0]!=0) res[0][29]=a27; + a27=arg[0]? arg[0][30] : 0; + a29=arg[0]? arg[0][66] : 0; + a34=cos(a36); + a34=(a29*a34); + a10=(a12*a10); + a11=(a9*a11); + a10=(a10+a11); + a11=(a9*a10); + a13=arg[0]? arg[0][67] : 0; + a33=(a12*a13); + a11=(a11+a33); + a33=(a8*a11); + a33=(a36+a33); + a35=cos(a33); + a35=(a29*a35); + a35=(a7*a35); + a34=(a34+a35); + a35=(a9*a10); + a16=(a12*a13); + a35=(a35+a16); + a16=(a8*a35); + a16=(a36+a16); + a31=cos(a16); + a31=(a29*a31); + a31=(a7*a31); + a34=(a34+a31); + a31=(a9*a10); + a37=(a12*a13); + a31=(a31+a37); + a37=(a17*a31); + a37=(a36+a37); + a38=cos(a37); + a38=(a29*a38); + a34=(a34+a38); + a34=(a4*a34); + a34=(a25+a34); + a34=(a27-a34); + if (res[0]!=0) res[0][30]=a34; + a34=arg[0]? arg[0][31] : 0; + a38=sin(a36); + a38=(a29*a38); + a33=sin(a33); + a33=(a29*a33); + a33=(a7*a33); + a38=(a38+a33); + a16=sin(a16); + a16=(a29*a16); + a16=(a7*a16); + a38=(a38+a16); + a37=sin(a37); + a29=(a29*a37); + a38=(a38+a29); + a38=(a4*a38); + a38=(a32+a38); + a38=(a34-a38); + if (res[0]!=0) res[0][31]=a38; + a38=arg[0]? arg[0][32] : 0; + a29=cos(a38); + a35=(a7*a35); + a11=(a11+a35); + a31=(a7*a31); + a11=(a11+a31); + a31=(a9*a10); + a35=(a12*a13); + a31=(a31+a35); + a11=(a11+a31); + a11=(a4*a11); + a36=(a36+a11); + a11=sin(a36); + a29=(a29*a11); + a31=sin(a38); + a36=cos(a36); + a31=(a31*a36); + a29=(a29-a31); + a31=cos(a38); + a31=(a31*a36); + a36=sin(a38); + a36=(a36*a11); + a31=(a31+a36); + a29=atan2(a29,a31); + if (res[0]!=0) res[0][32]=a29; + a29=arg[0]? arg[0][33] : 0; + a31=arg[0]? arg[0][68] : 0; + a36=cos(a38); + a36=(a31*a36); + a13=(a12*a13); + a10=(a9*a10); + a13=(a13+a10); + a10=(a9*a13); + a11=arg[0]? arg[0][69] : 0; + a35=(a12*a11); + a10=(a10+a35); + a35=(a8*a10); + a35=(a38+a35); + a37=cos(a35); + a37=(a31*a37); + a37=(a7*a37); + a36=(a36+a37); + a37=(a9*a13); + a16=(a12*a11); + a37=(a37+a16); + a16=(a8*a37); + a16=(a38+a16); + a33=cos(a16); + a33=(a31*a33); + a33=(a7*a33); + a36=(a36+a33); + a33=(a9*a13); + a39=(a12*a11); + a33=(a33+a39); + a39=(a17*a33); + a39=(a38+a39); + a40=cos(a39); + a40=(a31*a40); + a36=(a36+a40); + a36=(a4*a36); + a36=(a27+a36); + a36=(a29-a36); + if (res[0]!=0) res[0][33]=a36; + a36=arg[0]? arg[0][34] : 0; + a40=sin(a38); + a40=(a31*a40); + a35=sin(a35); + a35=(a31*a35); + a35=(a7*a35); + a40=(a40+a35); + a16=sin(a16); + a16=(a31*a16); + a16=(a7*a16); + a40=(a40+a16); + a39=sin(a39); + a31=(a31*a39); + a40=(a40+a31); + a40=(a4*a40); + a40=(a34+a40); + a40=(a36-a40); + if (res[0]!=0) res[0][34]=a40; + a40=arg[0]? arg[0][35] : 0; + a31=cos(a40); + a37=(a7*a37); + a10=(a10+a37); + a33=(a7*a33); + a10=(a10+a33); + a33=(a9*a13); + a37=(a12*a11); + a33=(a33+a37); + a10=(a10+a33); + a10=(a4*a10); + a38=(a38+a10); + a10=sin(a38); + a31=(a31*a10); + a33=sin(a40); + a38=cos(a38); + a33=(a33*a38); + a31=(a31-a33); + a33=cos(a40); + a33=(a33*a38); + a38=sin(a40); + a38=(a38*a10); + a33=(a33+a38); + a31=atan2(a31,a33); + if (res[0]!=0) res[0][35]=a31; + a31=arg[0]? arg[0][36] : 0; + a33=arg[0]? arg[0][70] : 0; + a38=cos(a40); + a38=(a33*a38); + a11=(a12*a11); + a13=(a9*a13); + a11=(a11+a13); + a13=(a9*a11); + a10=arg[0]? arg[0][71] : 0; + a37=(a12*a10); + a13=(a13+a37); + a37=(a8*a13); + a37=(a40+a37); + a39=cos(a37); + a39=(a33*a39); + a39=(a7*a39); + a38=(a38+a39); + a39=(a9*a11); + a16=(a12*a10); + a39=(a39+a16); + a16=(a8*a39); + a16=(a40+a16); + a35=cos(a16); + a35=(a33*a35); + a35=(a7*a35); + a38=(a38+a35); + a35=(a9*a11); + a41=(a12*a10); + a35=(a35+a41); + a41=(a17*a35); + a41=(a40+a41); + a42=cos(a41); + a42=(a33*a42); + a38=(a38+a42); + a38=(a4*a38); + a38=(a29+a38); + a38=(a31-a38); + if (res[0]!=0) res[0][36]=a38; + a38=arg[0]? arg[0][37] : 0; + a42=sin(a40); + a42=(a33*a42); + a37=sin(a37); + a37=(a33*a37); + a37=(a7*a37); + a42=(a42+a37); + a16=sin(a16); + a16=(a33*a16); + a16=(a7*a16); + a42=(a42+a16); + a41=sin(a41); + a33=(a33*a41); + a42=(a42+a33); + a42=(a4*a42); + a42=(a36+a42); + a42=(a38-a42); + if (res[0]!=0) res[0][37]=a42; + a42=arg[0]? arg[0][38] : 0; + a33=cos(a42); + a39=(a7*a39); + a13=(a13+a39); + a35=(a7*a35); + a13=(a13+a35); + a35=(a9*a11); + a39=(a12*a10); + a35=(a35+a39); + a13=(a13+a35); + a13=(a4*a13); + a40=(a40+a13); + a13=sin(a40); + a33=(a33*a13); + a35=sin(a42); + a40=cos(a40); + a35=(a35*a40); + a33=(a33-a35); + a35=cos(a42); + a35=(a35*a40); + a40=sin(a42); + a40=(a40*a13); + a35=(a35+a40); + a33=atan2(a33,a35); + if (res[0]!=0) res[0][38]=a33; + a33=arg[0]? arg[0][39] : 0; + a35=arg[0]? arg[0][72] : 0; + a40=cos(a42); + a40=(a35*a40); + a10=(a12*a10); + a11=(a9*a11); + a10=(a10+a11); + a11=(a9*a10); + a13=arg[0]? arg[0][73] : 0; + a39=(a12*a13); + a11=(a11+a39); + a39=(a8*a11); + a39=(a42+a39); + a41=cos(a39); + a41=(a35*a41); + a41=(a7*a41); + a40=(a40+a41); + a41=(a9*a10); + a16=(a12*a13); + a41=(a41+a16); + a16=(a8*a41); + a16=(a42+a16); + a37=cos(a16); + a37=(a35*a37); + a37=(a7*a37); + a40=(a40+a37); + a37=(a9*a10); + a43=(a12*a13); + a37=(a37+a43); + a43=(a17*a37); + a43=(a42+a43); + a44=cos(a43); + a44=(a35*a44); + a40=(a40+a44); + a40=(a4*a40); + a40=(a31+a40); + a40=(a33-a40); + if (res[0]!=0) res[0][39]=a40; + a40=arg[0]? arg[0][40] : 0; + a44=sin(a42); + a44=(a35*a44); + a39=sin(a39); + a39=(a35*a39); + a39=(a7*a39); + a44=(a44+a39); + a16=sin(a16); + a16=(a35*a16); + a16=(a7*a16); + a44=(a44+a16); + a43=sin(a43); + a35=(a35*a43); + a44=(a44+a35); + a44=(a4*a44); + a44=(a38+a44); + a44=(a40-a44); + if (res[0]!=0) res[0][40]=a44; + a44=arg[0]? arg[0][41] : 0; + a35=cos(a44); + a41=(a7*a41); + a11=(a11+a41); + a37=(a7*a37); + a11=(a11+a37); + a37=(a9*a10); + a41=(a12*a13); + a37=(a37+a41); + a11=(a11+a37); + a11=(a4*a11); + a42=(a42+a11); + a11=sin(a42); + a35=(a35*a11); + a37=sin(a44); + a42=cos(a42); + a37=(a37*a42); + a35=(a35-a37); + a37=cos(a44); + a37=(a37*a42); + a42=sin(a44); + a42=(a42*a11); + a37=(a37+a42); + a35=atan2(a35,a37); + if (res[0]!=0) res[0][41]=a35; + a35=arg[0]? arg[0][42] : 0; + a37=arg[0]? arg[0][74] : 0; + a42=cos(a44); + a42=(a37*a42); + a13=(a12*a13); + a10=(a9*a10); + a13=(a13+a10); + a10=(a9*a13); + a11=arg[0]? arg[0][75] : 0; + a41=(a12*a11); + a10=(a10+a41); + a41=(a8*a10); + a41=(a44+a41); + a43=cos(a41); + a43=(a37*a43); + a43=(a7*a43); + a42=(a42+a43); + a43=(a9*a13); + a16=(a12*a11); + a43=(a43+a16); + a16=(a8*a43); + a16=(a44+a16); + a39=cos(a16); + a39=(a37*a39); + a39=(a7*a39); + a42=(a42+a39); + a39=(a9*a13); + a45=(a12*a11); + a39=(a39+a45); + a45=(a17*a39); + a45=(a44+a45); + a46=cos(a45); + a46=(a37*a46); + a42=(a42+a46); + a42=(a4*a42); + a42=(a33+a42); + a42=(a35-a42); + if (res[0]!=0) res[0][42]=a42; + a42=arg[0]? arg[0][43] : 0; + a46=sin(a44); + a46=(a37*a46); + a41=sin(a41); + a41=(a37*a41); + a41=(a7*a41); + a46=(a46+a41); + a16=sin(a16); + a16=(a37*a16); + a16=(a7*a16); + a46=(a46+a16); + a45=sin(a45); + a37=(a37*a45); + a46=(a46+a37); + a46=(a4*a46); + a46=(a40+a46); + a46=(a42-a46); + if (res[0]!=0) res[0][43]=a46; + a46=arg[0]? arg[0][44] : 0; + a37=cos(a46); + a43=(a7*a43); + a10=(a10+a43); + a39=(a7*a39); + a10=(a10+a39); + a39=(a9*a13); + a43=(a12*a11); + a39=(a39+a43); + a10=(a10+a39); + a10=(a4*a10); + a44=(a44+a10); + a10=sin(a44); + a37=(a37*a10); + a39=sin(a46); + a44=cos(a44); + a39=(a39*a44); + a37=(a37-a39); + a39=cos(a46); + a39=(a39*a44); + a44=sin(a46); + a44=(a44*a10); + a39=(a39+a44); + a37=atan2(a37,a39); + if (res[0]!=0) res[0][44]=a37; + a37=arg[0]? arg[0][45] : 0; + a39=arg[0]? arg[0][76] : 0; + a44=cos(a46); + a44=(a39*a44); + a11=(a12*a11); + a13=(a9*a13); + a11=(a11+a13); + a13=(a9*a11); + a10=arg[0]? arg[0][77] : 0; + a43=(a12*a10); + a13=(a13+a43); + a43=(a8*a13); + a43=(a46+a43); + a45=cos(a43); + a45=(a39*a45); + a45=(a7*a45); + a44=(a44+a45); + a45=(a9*a11); + a16=(a12*a10); + a45=(a45+a16); + a8=(a8*a45); + a8=(a46+a8); + a16=cos(a8); + a16=(a39*a16); + a16=(a7*a16); + a44=(a44+a16); + a16=(a9*a11); + a41=(a12*a10); + a16=(a16+a41); + a17=(a17*a16); + a17=(a46+a17); + a41=cos(a17); + a41=(a39*a41); + a44=(a44+a41); + a44=(a4*a44); + a44=(a35+a44); + a37=(a37-a44); + if (res[0]!=0) res[0][45]=a37; + a37=arg[0]? arg[0][46] : 0; + a44=sin(a46); + a44=(a39*a44); + a43=sin(a43); + a43=(a39*a43); + a43=(a7*a43); + a44=(a44+a43); + a8=sin(a8); + a8=(a39*a8); + a8=(a7*a8); + a44=(a44+a8); + a17=sin(a17); + a39=(a39*a17); + a44=(a44+a39); + a44=(a4*a44); + a44=(a42+a44); + a37=(a37-a44); + if (res[0]!=0) res[0][46]=a37; + a37=arg[0]? arg[0][47] : 0; + a44=cos(a37); + a45=(a7*a45); + a13=(a13+a45); + a7=(a7*a16); + a13=(a13+a7); + a9=(a9*a11); + a12=(a12*a10); + a9=(a9+a12); + a13=(a13+a9); + a4=(a4*a13); + a46=(a46+a4); + a4=sin(a46); + a44=(a44*a4); + a13=sin(a37); + a46=cos(a46); + a13=(a13*a46); + a44=(a44-a13); + a13=cos(a37); + a13=(a13*a46); + a37=sin(a37); + a37=(a37*a4); + a13=(a13+a37); + a44=atan2(a44,a13); + if (res[0]!=0) res[0][47]=a44; + a44=arg[1]? arg[1][5] : 0; + a13=cos(a44); + a37=arg[1]? arg[1][4] : 0; + a1=(a1-a37); + a13=(a13*a1); + a44=sin(a44); + a1=arg[1]? arg[1][3] : 0; + a0=(a0-a1); + a44=(a44*a0); + a13=(a13-a44); + if (res[0]!=0) res[0][48]=a13; + a13=arg[1]? arg[1][8] : 0; + a44=cos(a13); + a0=arg[1]? arg[1][7] : 0; + a6=(a6-a0); + a44=(a44*a6); + a13=sin(a13); + a6=arg[1]? arg[1][6] : 0; + a3=(a3-a6); + a13=(a13*a3); + a44=(a44-a13); + if (res[0]!=0) res[0][49]=a44; + a44=arg[1]? arg[1][11] : 0; + a13=cos(a44); + a3=arg[1]? arg[1][10] : 0; + a2=(a2-a3); + a13=(a13*a2); + a44=sin(a44); + a2=arg[1]? arg[1][9] : 0; + a5=(a5-a2); + a44=(a44*a5); + a13=(a13-a44); + if (res[0]!=0) res[0][50]=a13; + a13=arg[1]? arg[1][14] : 0; + a44=cos(a13); + a5=arg[1]? arg[1][13] : 0; + a20=(a20-a5); + a44=(a44*a20); + a13=sin(a13); + a20=arg[1]? arg[1][12] : 0; + a18=(a18-a20); + a13=(a13*a18); + a44=(a44-a13); + if (res[0]!=0) res[0][51]=a44; + a44=arg[1]? arg[1][17] : 0; + a13=cos(a44); + a18=arg[1]? arg[1][16] : 0; + a22=(a22-a18); + a13=(a13*a22); + a44=sin(a44); + a22=arg[1]? arg[1][15] : 0; + a14=(a14-a22); + a44=(a44*a14); + a13=(a13-a44); + if (res[0]!=0) res[0][52]=a13; + a13=arg[1]? arg[1][20] : 0; + a44=cos(a13); + a14=arg[1]? arg[1][19] : 0; + a24=(a24-a14); + a44=(a44*a24); + a13=sin(a13); + a24=arg[1]? arg[1][18] : 0; + a15=(a15-a24); + a13=(a13*a15); + a44=(a44-a13); + if (res[0]!=0) res[0][53]=a44; + a44=arg[1]? arg[1][23] : 0; + a13=cos(a44); + a15=arg[1]? arg[1][22] : 0; + a26=(a26-a15); + a13=(a13*a26); + a44=sin(a44); + a26=arg[1]? arg[1][21] : 0; + a19=(a19-a26); + a44=(a44*a19); + a13=(a13-a44); + if (res[0]!=0) res[0][54]=a13; + a13=arg[1]? arg[1][26] : 0; + a44=cos(a13); + a19=arg[1]? arg[1][25] : 0; + a28=(a28-a19); + a44=(a44*a28); + a13=sin(a13); + a28=arg[1]? arg[1][24] : 0; + a21=(a21-a28); + a13=(a13*a21); + a44=(a44-a13); + if (res[0]!=0) res[0][55]=a44; + a44=arg[1]? arg[1][29] : 0; + a13=cos(a44); + a21=arg[1]? arg[1][28] : 0; + a30=(a30-a21); + a13=(a13*a30); + a44=sin(a44); + a30=arg[1]? arg[1][27] : 0; + a23=(a23-a30); + a44=(a44*a23); + a13=(a13-a44); + if (res[0]!=0) res[0][56]=a13; + a13=arg[1]? arg[1][32] : 0; + a44=cos(a13); + a23=arg[1]? arg[1][31] : 0; + a32=(a32-a23); + a44=(a44*a32); + a13=sin(a13); + a32=arg[1]? arg[1][30] : 0; + a25=(a25-a32); + a13=(a13*a25); + a44=(a44-a13); + if (res[0]!=0) res[0][57]=a44; + a44=arg[1]? arg[1][35] : 0; + a13=cos(a44); + a25=arg[1]? arg[1][34] : 0; + a34=(a34-a25); + a13=(a13*a34); + a44=sin(a44); + a34=arg[1]? arg[1][33] : 0; + a27=(a27-a34); + a44=(a44*a27); + a13=(a13-a44); + if (res[0]!=0) res[0][58]=a13; + a13=arg[1]? arg[1][38] : 0; + a44=cos(a13); + a27=arg[1]? arg[1][37] : 0; + a36=(a36-a27); + a44=(a44*a36); + a13=sin(a13); + a36=arg[1]? arg[1][36] : 0; + a29=(a29-a36); + a13=(a13*a29); + a44=(a44-a13); + if (res[0]!=0) res[0][59]=a44; + a44=arg[1]? arg[1][41] : 0; + a13=cos(a44); + a29=arg[1]? arg[1][40] : 0; + a38=(a38-a29); + a13=(a13*a38); + a44=sin(a44); + a38=arg[1]? arg[1][39] : 0; + a31=(a31-a38); + a44=(a44*a31); + a13=(a13-a44); + if (res[0]!=0) res[0][60]=a13; + a13=arg[1]? arg[1][44] : 0; + a44=cos(a13); + a31=arg[1]? arg[1][43] : 0; + a40=(a40-a31); + a44=(a44*a40); + a13=sin(a13); + a40=arg[1]? arg[1][42] : 0; + a33=(a33-a40); + a13=(a13*a33); + a44=(a44-a13); + if (res[0]!=0) res[0][61]=a44; + a44=arg[1]? arg[1][47] : 0; + a13=cos(a44); + a33=arg[1]? arg[1][46] : 0; + a42=(a42-a33); + a13=(a13*a42); + a44=sin(a44); + a42=arg[1]? arg[1][45] : 0; + a35=(a35-a42); + a44=(a44*a35); + a13=(a13-a44); + if (res[0]!=0) res[0][62]=a13; + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_g(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f2(arg, res, iw, w, mem); +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_g_alloc_mem(void) { + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_g_init_mem(int mem) { + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_g_free_mem(int mem) { +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_g_checkout(void) { + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_g_release(int mem) { +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_g_incref(void) { +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_g_decref(void) { +} + +extern "C" CASADI_SYMBOL_EXPORT casadi_int nlp_g_n_in(void) { return 2;} + +extern "C" CASADI_SYMBOL_EXPORT casadi_int nlp_g_n_out(void) { return 1;} + +extern "C" CASADI_SYMBOL_EXPORT casadi_real nlp_g_default_in(casadi_int i) { + switch (i) { + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const char* nlp_g_name_in(casadi_int i) { + switch (i) { + case 0: return "x"; + case 1: return "p"; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const char* nlp_g_name_out(casadi_int i) { + switch (i) { + case 0: return "g"; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const casadi_int* nlp_g_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s1; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const casadi_int* nlp_g_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s3; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_g_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 2; + if (sz_res) *sz_res = 1; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 0; + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_g_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 2*sizeof(const casadi_real*); + if (sz_res) *sz_res = 1*sizeof(casadi_real*); + if (sz_iw) *sz_iw = 0*sizeof(casadi_int); + if (sz_w) *sz_w = 0*sizeof(casadi_real); + return 0; +} + +/* nlp_grad:(x[78],p[50],lam_f,lam_g[63])->(f,g[63],grad_gamma_x[78],grad_gamma_p[50]) */ +static int casadi_f3(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_real a0, a1, a10, a100, a101, a102, a103, a104, a105, a106, a107, a108, a109, a11, a110, a111, a112, a113, a114, a115, a116, a117, a118, a119, a12, a120, a121, a122, a123, a124, a125, a126, a127, a128, a129, a13, a130, a131, a132, a133, a134, a135, a136, a137, a138, a139, a14, a140, a141, a142, a143, a144, a145, a146, a147, a148, a149, a15, a150, a151, a152, a153, a154, a155, a156, a157, a158, a159, a16, a160, a161, a162, a163, a164, a165, a166, a167, a168, a169, a17, a170, a171, a172, a173, a174, a175, a176, a177, a178, a179, a18, a180, a181, a182, a183, a184, a185, a186, a187, a188, a189, a19, a190, a191, a192, a193, a194, a195, a196, a197, a198, a199, a2, a20, a200, a201, a202, a203, a204, a205, a206, a207, a208, a209, a21, a210, a211, a212, a213, a214, a215, a216, a217, a218, a219, a22, a220, a221, a222, a223, a224, a225, a226, a227, a228, a229, a23, a230, a231, a232, a233, a234, a235, a236, a237, a238, a239, a24, a240, a241, a242, a243, a244, a245, a246, a247, a248, a249, a25, a250, a251, a252, a253, a254, a255, a256, a257, a258, a259, a26, a260, a261, a262, a263, a264, a265, a266, a267, a268, a269, a27, a270, a271, a272, a273, a274, a275, a276, a277, a278, a279, a28, a280, a281, a282, a283, a284, a285, a286, a287, a288, a289, a29, a290, a291, a292, a293, a294, a295, a296, a297, a298, a299, a3, a30, a300, a301, a302, a303, a304, a305, a306, a307, a308, a309, a31, a310, a311, a312, a313, a314, a315, a316, a317, a318, a319, a32, a320, a321, a322, a323, a324, a325, a326, a327, a328, a329, a33, a330, a331, a332, a333, a334, a335, a336, a337, a338, a339, a34, a340, a341, a342, a343, a344, a345, a346, a347, a348, a349, a35, a350, a351, a352, a353, a354, a355, a356, a357, a358, a359, a36, a360, a361, a362, a363, a364, a365, a366, a367, a368, a369, a37, a370, a371, a372, a373, a374, a375, a376, a377, a378, a379, a38, a380, a381, a382, a383, a384, a385, a386, a387, a388, a389, a39, a390, a391, a392, a393, a394, a395, a396, a397, a398, a399, a4, a40, a400, a401, a402, a403, a404, a405, a406, a407, a408, a409, a41, a410, a411, a412, a413, a414, a415, a416, a417, a418, a419, a42, a420, a421, a422, a423, a424, a425, a426, a427, a428, a429, a43, a430, a431, a432, a433, a434, a435, a436, a437, a438, a439, a44, a440, a441, a442, a443, a444, a445, a446, a447, a448, a449, a45, a450, a451, a452, a453, a454, a455, a456, a457, a458, a459, a46, a460, a461, a462, a463, a464, a465, a466, a467, a468, a469, a47, a470, a471, a472, a473, a474, a475, a476, a477, a478, a479, a48, a480, a481, a482, a483, a484, a485, a486, a487, a488, a489, a49, a490, a491, a492, a493, a494, a495, a496, a497, a498, a499, a5, a50, a500, a501, a502, a503, a504, a505, a506, a507, a508, a509, a51, a510, a511, a512, a513, a514, a515, a516, a517, a518, a519, a52, a520, a521, a522, a523, a524, a525, a526, a527, a528, a529, a53, a530, a531, a532, a533, a534, a535, a536, a537, a538, a539, a54, a540, a541, a542, a543, a544, a545, a546, a547, a548, a549, a55, a550, a551, a552, a553, a554, a555, a556, a557, a558, a559, a56, a560, a561, a562, a563, a564, a565, a566, a567, a568, a569, a57, a570, a571, a572, a573, a574, a575, a576, a577, a578, a579, a58, a580, a581, a582, a583, a584, a585, a586, a587, a588, a589, a59, a590, a591, a592, a593, a594, a595, a596, a597, a598, a599, a6, a60, a600, a601, a602, a603, a604, a605, a606, a607, a608, a609, a61, a610, a611, a612, a613, a614, a615, a616, a617, a618, a619, a62, a620, a621, a622, a623, a624, a625, a626, a627, a628, a629, a63, a630, a631, a632, a633, a634, a635, a636, a637, a638, a639, a64, a640, a641, a642, a643, a644, a645, a646, a647, a648, a649, a65, a650, a651, a652, a653, a654, a655, a656, a657, a658, a659, a66, a660, a661, a662, a663, a664, a665, a666, a667, a668, a669, a67, a670, a671, a672, a673, a674, a675, a676, a677, a678, a679, a68, a680, a681, a682, a683, a684, a685, a686, a687, a688, a689, a69, a690, a691, a692, a693, a694, a695, a696, a697, a698, a699, a7, a70, a700, a701, a702, a703, a704, a705, a706, a707, a708, a709, a71, a710, a711, a712, a713, a714, a715, a716, a717, a718, a719, a72, a720, a721, a722, a723, a724, a725, a726, a727, a728, a729, a73, a730, a731, a732, a733, a734, a735, a736, a737, a738, a739, a74, a740, a741, a742, a743, a744, a745, a746, a747, a748, a749, a75, a750, a751, a752, a753, a754, a755, a756, a757, a758, a759, a76, a760, a761, a762, a763, a764, a765, a766, a767, a768, a769, a77, a770, a771, a772, a773, a774, a775, a776, a777, a778, a779, a78, a780, a781, a782, a783, a784, a785, a786, a787, a788, a789, a79, a790, a791, a792, a793, a794, a795, a796, a797, a798, a799, a8, a80, a800, a801, a802, a803, a804, a805, a806, a807, a808, a809, a81, a810, a811, a812, a813, a814, a815, a816, a817, a818, a819, a82, a820, a821, a822, a823, a824, a825, a826, a827, a828, a829, a83, a830, a831, a832, a833, a834, a835, a836, a837, a838, a839, a84, a840, a841, a842, a843, a844, a845, a846, a847, a848, a849, a85, a850, a851, a852, a853, a854, a855, a856, a857, a858, a859, a86, a860, a861, a862, a863, a864, a865, a866, a867, a868, a869, a87, a870, a871, a872, a873, a874, a875, a876, a877, a878, a879, a88, a880, a881, a882, a883, a884, a885, a886, a887, a888, a889, a89, a890, a891, a892, a893, a894, a895, a9, a90, a91, a92, a93, a94, a95, a96, a97, a98, a99; + a0=10.; + a1=arg[0]? arg[0][3] : 0; + a2=arg[1]? arg[1][3] : 0; + a3=(a1-a2); + a3=(a0*a3); + a4=(a1-a2); + a5=(a3*a4); + a6=arg[0]? arg[0][4] : 0; + a7=arg[1]? arg[1][4] : 0; + a8=(a6-a7); + a8=(a0*a8); + a9=(a6-a7); + a10=(a8*a9); + a5=(a5+a10); + a10=arg[0]? arg[0][48] : 0; + a11=casadi_sq(a10); + a12=arg[0]? arg[0][49] : 0; + a13=casadi_sq(a12); + a11=(a11+a13); + a5=(a5+a11); + a11=arg[1]? arg[1][5] : 0; + a13=cos(a11); + a14=arg[0]? arg[0][5] : 0; + a15=sin(a14); + a16=(a13*a15); + a17=sin(a11); + a18=cos(a14); + a19=(a17*a18); + a16=(a16-a19); + a19=cos(a11); + a20=(a19*a18); + a21=sin(a11); + a22=(a21*a15); + a20=(a20+a22); + a22=atan2(a16,a20); + a23=cos(a11); + a24=sin(a14); + a25=(a23*a24); + a26=sin(a11); + a27=cos(a14); + a28=(a26*a27); + a25=(a25-a28); + a28=cos(a11); + a29=(a28*a27); + a30=sin(a11); + a31=(a30*a24); + a29=(a29+a31); + a31=atan2(a25,a29); + a32=(a22*a31); + a5=(a5+a32); + a32=arg[0]? arg[0][6] : 0; + a33=arg[1]? arg[1][6] : 0; + a34=(a32-a33); + a34=(a0*a34); + a35=(a32-a33); + a36=(a34*a35); + a37=arg[0]? arg[0][7] : 0; + a38=arg[1]? arg[1][7] : 0; + a39=(a37-a38); + a39=(a0*a39); + a40=(a37-a38); + a41=(a39*a40); + a36=(a36+a41); + a5=(a5+a36); + a36=arg[0]? arg[0][50] : 0; + a41=casadi_sq(a36); + a42=arg[0]? arg[0][51] : 0; + a43=casadi_sq(a42); + a41=(a41+a43); + a5=(a5+a41); + a41=arg[1]? arg[1][8] : 0; + a43=cos(a41); + a44=arg[0]? arg[0][8] : 0; + a45=sin(a44); + a46=(a43*a45); + a47=sin(a41); + a48=cos(a44); + a49=(a47*a48); + a46=(a46-a49); + a49=cos(a41); + a50=(a49*a48); + a51=sin(a41); + a52=(a51*a45); + a50=(a50+a52); + a52=atan2(a46,a50); + a53=cos(a41); + a54=sin(a44); + a55=(a53*a54); + a56=sin(a41); + a57=cos(a44); + a58=(a56*a57); + a55=(a55-a58); + a58=cos(a41); + a59=(a58*a57); + a60=sin(a41); + a61=(a60*a54); + a59=(a59+a61); + a61=atan2(a55,a59); + a62=(a52*a61); + a5=(a5+a62); + a62=arg[0]? arg[0][9] : 0; + a63=arg[1]? arg[1][9] : 0; + a64=(a62-a63); + a64=(a0*a64); + a65=(a62-a63); + a66=(a64*a65); + a67=arg[0]? arg[0][10] : 0; + a68=arg[1]? arg[1][10] : 0; + a69=(a67-a68); + a69=(a0*a69); + a70=(a67-a68); + a71=(a69*a70); + a66=(a66+a71); + a5=(a5+a66); + a66=arg[0]? arg[0][52] : 0; + a71=casadi_sq(a66); + a72=arg[0]? arg[0][53] : 0; + a73=casadi_sq(a72); + a71=(a71+a73); + a5=(a5+a71); + a71=arg[1]? arg[1][11] : 0; + a73=cos(a71); + a74=arg[0]? arg[0][11] : 0; + a75=sin(a74); + a76=(a73*a75); + a77=sin(a71); + a78=cos(a74); + a79=(a77*a78); + a76=(a76-a79); + a79=cos(a71); + a80=(a79*a78); + a81=sin(a71); + a82=(a81*a75); + a80=(a80+a82); + a82=atan2(a76,a80); + a83=cos(a71); + a84=sin(a74); + a85=(a83*a84); + a86=sin(a71); + a87=cos(a74); + a88=(a86*a87); + a85=(a85-a88); + a88=cos(a71); + a89=(a88*a87); + a90=sin(a71); + a91=(a90*a84); + a89=(a89+a91); + a91=atan2(a85,a89); + a92=(a82*a91); + a5=(a5+a92); + a92=arg[0]? arg[0][12] : 0; + a93=arg[1]? arg[1][12] : 0; + a94=(a92-a93); + a94=(a0*a94); + a95=(a92-a93); + a96=(a94*a95); + a97=arg[0]? arg[0][13] : 0; + a98=arg[1]? arg[1][13] : 0; + a99=(a97-a98); + a99=(a0*a99); + a100=(a97-a98); + a101=(a99*a100); + a96=(a96+a101); + a5=(a5+a96); + a96=arg[0]? arg[0][54] : 0; + a101=casadi_sq(a96); + a102=arg[0]? arg[0][55] : 0; + a103=casadi_sq(a102); + a101=(a101+a103); + a5=(a5+a101); + a101=arg[1]? arg[1][14] : 0; + a103=cos(a101); + a104=arg[0]? arg[0][14] : 0; + a105=sin(a104); + a106=(a103*a105); + a107=sin(a101); + a108=cos(a104); + a109=(a107*a108); + a106=(a106-a109); + a109=cos(a101); + a110=(a109*a108); + a111=sin(a101); + a112=(a111*a105); + a110=(a110+a112); + a112=atan2(a106,a110); + a113=cos(a101); + a114=sin(a104); + a115=(a113*a114); + a116=sin(a101); + a117=cos(a104); + a118=(a116*a117); + a115=(a115-a118); + a118=cos(a101); + a119=(a118*a117); + a120=sin(a101); + a121=(a120*a114); + a119=(a119+a121); + a121=atan2(a115,a119); + a122=(a112*a121); + a5=(a5+a122); + a122=arg[0]? arg[0][15] : 0; + a123=arg[1]? arg[1][15] : 0; + a124=(a122-a123); + a124=(a0*a124); + a125=(a122-a123); + a126=(a124*a125); + a127=arg[0]? arg[0][16] : 0; + a128=arg[1]? arg[1][16] : 0; + a129=(a127-a128); + a129=(a0*a129); + a130=(a127-a128); + a131=(a129*a130); + a126=(a126+a131); + a5=(a5+a126); + a126=arg[0]? arg[0][56] : 0; + a131=casadi_sq(a126); + a132=arg[0]? arg[0][57] : 0; + a133=casadi_sq(a132); + a131=(a131+a133); + a5=(a5+a131); + a131=arg[1]? arg[1][17] : 0; + a133=cos(a131); + a134=arg[0]? arg[0][17] : 0; + a135=sin(a134); + a136=(a133*a135); + a137=sin(a131); + a138=cos(a134); + a139=(a137*a138); + a136=(a136-a139); + a139=cos(a131); + a140=(a139*a138); + a141=sin(a131); + a142=(a141*a135); + a140=(a140+a142); + a142=atan2(a136,a140); + a143=cos(a131); + a144=sin(a134); + a145=(a143*a144); + a146=sin(a131); + a147=cos(a134); + a148=(a146*a147); + a145=(a145-a148); + a148=cos(a131); + a149=(a148*a147); + a150=sin(a131); + a151=(a150*a144); + a149=(a149+a151); + a151=atan2(a145,a149); + a152=(a142*a151); + a5=(a5+a152); + a152=arg[0]? arg[0][18] : 0; + a153=arg[1]? arg[1][18] : 0; + a154=(a152-a153); + a154=(a0*a154); + a155=(a152-a153); + a156=(a154*a155); + a157=arg[0]? arg[0][19] : 0; + a158=arg[1]? arg[1][19] : 0; + a159=(a157-a158); + a159=(a0*a159); + a160=(a157-a158); + a161=(a159*a160); + a156=(a156+a161); + a5=(a5+a156); + a156=arg[0]? arg[0][58] : 0; + a161=casadi_sq(a156); + a162=arg[0]? arg[0][59] : 0; + a163=casadi_sq(a162); + a161=(a161+a163); + a5=(a5+a161); + a161=arg[1]? arg[1][20] : 0; + a163=cos(a161); + a164=arg[0]? arg[0][20] : 0; + a165=sin(a164); + a166=(a163*a165); + a167=sin(a161); + a168=cos(a164); + a169=(a167*a168); + a166=(a166-a169); + a169=cos(a161); + a170=(a169*a168); + a171=sin(a161); + a172=(a171*a165); + a170=(a170+a172); + a172=atan2(a166,a170); + a173=cos(a161); + a174=sin(a164); + a175=(a173*a174); + a176=sin(a161); + a177=cos(a164); + a178=(a176*a177); + a175=(a175-a178); + a178=cos(a161); + a179=(a178*a177); + a180=sin(a161); + a181=(a180*a174); + a179=(a179+a181); + a181=atan2(a175,a179); + a182=(a172*a181); + a5=(a5+a182); + a182=arg[0]? arg[0][21] : 0; + a183=arg[1]? arg[1][21] : 0; + a184=(a182-a183); + a184=(a0*a184); + a185=(a182-a183); + a186=(a184*a185); + a187=arg[0]? arg[0][22] : 0; + a188=arg[1]? arg[1][22] : 0; + a189=(a187-a188); + a189=(a0*a189); + a190=(a187-a188); + a191=(a189*a190); + a186=(a186+a191); + a5=(a5+a186); + a186=arg[0]? arg[0][60] : 0; + a191=casadi_sq(a186); + a192=arg[0]? arg[0][61] : 0; + a193=casadi_sq(a192); + a191=(a191+a193); + a5=(a5+a191); + a191=arg[1]? arg[1][23] : 0; + a193=cos(a191); + a194=arg[0]? arg[0][23] : 0; + a195=sin(a194); + a196=(a193*a195); + a197=sin(a191); + a198=cos(a194); + a199=(a197*a198); + a196=(a196-a199); + a199=cos(a191); + a200=(a199*a198); + a201=sin(a191); + a202=(a201*a195); + a200=(a200+a202); + a202=atan2(a196,a200); + a203=cos(a191); + a204=sin(a194); + a205=(a203*a204); + a206=sin(a191); + a207=cos(a194); + a208=(a206*a207); + a205=(a205-a208); + a208=cos(a191); + a209=(a208*a207); + a210=sin(a191); + a211=(a210*a204); + a209=(a209+a211); + a211=atan2(a205,a209); + a212=(a202*a211); + a5=(a5+a212); + a212=arg[0]? arg[0][24] : 0; + a213=arg[1]? arg[1][24] : 0; + a214=(a212-a213); + a214=(a0*a214); + a215=(a212-a213); + a216=(a214*a215); + a217=arg[0]? arg[0][25] : 0; + a218=arg[1]? arg[1][25] : 0; + a219=(a217-a218); + a219=(a0*a219); + a220=(a217-a218); + a221=(a219*a220); + a216=(a216+a221); + a5=(a5+a216); + a216=arg[0]? arg[0][62] : 0; + a221=casadi_sq(a216); + a222=arg[0]? arg[0][63] : 0; + a223=casadi_sq(a222); + a221=(a221+a223); + a5=(a5+a221); + a221=arg[1]? arg[1][26] : 0; + a223=cos(a221); + a224=arg[0]? arg[0][26] : 0; + a225=sin(a224); + a226=(a223*a225); + a227=sin(a221); + a228=cos(a224); + a229=(a227*a228); + a226=(a226-a229); + a229=cos(a221); + a230=(a229*a228); + a231=sin(a221); + a232=(a231*a225); + a230=(a230+a232); + a232=atan2(a226,a230); + a233=cos(a221); + a234=sin(a224); + a235=(a233*a234); + a236=sin(a221); + a237=cos(a224); + a238=(a236*a237); + a235=(a235-a238); + a238=cos(a221); + a239=(a238*a237); + a240=sin(a221); + a241=(a240*a234); + a239=(a239+a241); + a241=atan2(a235,a239); + a242=(a232*a241); + a5=(a5+a242); + a242=arg[0]? arg[0][27] : 0; + a243=arg[1]? arg[1][27] : 0; + a244=(a242-a243); + a244=(a0*a244); + a245=(a242-a243); + a246=(a244*a245); + a247=arg[0]? arg[0][28] : 0; + a248=arg[1]? arg[1][28] : 0; + a249=(a247-a248); + a249=(a0*a249); + a250=(a247-a248); + a251=(a249*a250); + a246=(a246+a251); + a5=(a5+a246); + a246=arg[0]? arg[0][64] : 0; + a251=casadi_sq(a246); + a252=arg[0]? arg[0][65] : 0; + a253=casadi_sq(a252); + a251=(a251+a253); + a5=(a5+a251); + a251=arg[1]? arg[1][29] : 0; + a253=cos(a251); + a254=arg[0]? arg[0][29] : 0; + a255=sin(a254); + a256=(a253*a255); + a257=sin(a251); + a258=cos(a254); + a259=(a257*a258); + a256=(a256-a259); + a259=cos(a251); + a260=(a259*a258); + a261=sin(a251); + a262=(a261*a255); + a260=(a260+a262); + a262=atan2(a256,a260); + a263=cos(a251); + a264=sin(a254); + a265=(a263*a264); + a266=sin(a251); + a267=cos(a254); + a268=(a266*a267); + a265=(a265-a268); + a268=cos(a251); + a269=(a268*a267); + a270=sin(a251); + a271=(a270*a264); + a269=(a269+a271); + a271=atan2(a265,a269); + a272=(a262*a271); + a5=(a5+a272); + a272=arg[0]? arg[0][30] : 0; + a273=arg[1]? arg[1][30] : 0; + a274=(a272-a273); + a274=(a0*a274); + a275=(a272-a273); + a276=(a274*a275); + a277=arg[0]? arg[0][31] : 0; + a278=arg[1]? arg[1][31] : 0; + a279=(a277-a278); + a279=(a0*a279); + a280=(a277-a278); + a281=(a279*a280); + a276=(a276+a281); + a5=(a5+a276); + a276=arg[0]? arg[0][66] : 0; + a281=casadi_sq(a276); + a282=arg[0]? arg[0][67] : 0; + a283=casadi_sq(a282); + a281=(a281+a283); + a5=(a5+a281); + a281=arg[1]? arg[1][32] : 0; + a283=cos(a281); + a284=arg[0]? arg[0][32] : 0; + a285=sin(a284); + a286=(a283*a285); + a287=sin(a281); + a288=cos(a284); + a289=(a287*a288); + a286=(a286-a289); + a289=cos(a281); + a290=(a289*a288); + a291=sin(a281); + a292=(a291*a285); + a290=(a290+a292); + a292=atan2(a286,a290); + a293=cos(a281); + a294=sin(a284); + a295=(a293*a294); + a296=sin(a281); + a297=cos(a284); + a298=(a296*a297); + a295=(a295-a298); + a298=cos(a281); + a299=(a298*a297); + a300=sin(a281); + a301=(a300*a294); + a299=(a299+a301); + a301=atan2(a295,a299); + a302=(a292*a301); + a5=(a5+a302); + a302=arg[0]? arg[0][33] : 0; + a303=arg[1]? arg[1][33] : 0; + a304=(a302-a303); + a304=(a0*a304); + a305=(a302-a303); + a306=(a304*a305); + a307=arg[0]? arg[0][34] : 0; + a308=arg[1]? arg[1][34] : 0; + a309=(a307-a308); + a309=(a0*a309); + a310=(a307-a308); + a311=(a309*a310); + a306=(a306+a311); + a5=(a5+a306); + a306=arg[0]? arg[0][68] : 0; + a311=casadi_sq(a306); + a312=arg[0]? arg[0][69] : 0; + a313=casadi_sq(a312); + a311=(a311+a313); + a5=(a5+a311); + a311=arg[1]? arg[1][35] : 0; + a313=cos(a311); + a314=arg[0]? arg[0][35] : 0; + a315=sin(a314); + a316=(a313*a315); + a317=sin(a311); + a318=cos(a314); + a319=(a317*a318); + a316=(a316-a319); + a319=cos(a311); + a320=(a319*a318); + a321=sin(a311); + a322=(a321*a315); + a320=(a320+a322); + a322=atan2(a316,a320); + a323=cos(a311); + a324=sin(a314); + a325=(a323*a324); + a326=sin(a311); + a327=cos(a314); + a328=(a326*a327); + a325=(a325-a328); + a328=cos(a311); + a329=(a328*a327); + a330=sin(a311); + a331=(a330*a324); + a329=(a329+a331); + a331=atan2(a325,a329); + a332=(a322*a331); + a5=(a5+a332); + a332=arg[0]? arg[0][36] : 0; + a333=arg[1]? arg[1][36] : 0; + a334=(a332-a333); + a334=(a0*a334); + a335=(a332-a333); + a336=(a334*a335); + a337=arg[0]? arg[0][37] : 0; + a338=arg[1]? arg[1][37] : 0; + a339=(a337-a338); + a339=(a0*a339); + a340=(a337-a338); + a341=(a339*a340); + a336=(a336+a341); + a5=(a5+a336); + a336=arg[0]? arg[0][70] : 0; + a341=casadi_sq(a336); + a342=arg[0]? arg[0][71] : 0; + a343=casadi_sq(a342); + a341=(a341+a343); + a5=(a5+a341); + a341=arg[1]? arg[1][38] : 0; + a343=cos(a341); + a344=arg[0]? arg[0][38] : 0; + a345=sin(a344); + a346=(a343*a345); + a347=sin(a341); + a348=cos(a344); + a349=(a347*a348); + a346=(a346-a349); + a349=cos(a341); + a350=(a349*a348); + a351=sin(a341); + a352=(a351*a345); + a350=(a350+a352); + a352=atan2(a346,a350); + a353=cos(a341); + a354=sin(a344); + a355=(a353*a354); + a356=sin(a341); + a357=cos(a344); + a358=(a356*a357); + a355=(a355-a358); + a358=cos(a341); + a359=(a358*a357); + a360=sin(a341); + a361=(a360*a354); + a359=(a359+a361); + a361=atan2(a355,a359); + a362=(a352*a361); + a5=(a5+a362); + a362=arg[0]? arg[0][39] : 0; + a363=arg[1]? arg[1][39] : 0; + a364=(a362-a363); + a364=(a0*a364); + a365=(a362-a363); + a366=(a364*a365); + a367=arg[0]? arg[0][40] : 0; + a368=arg[1]? arg[1][40] : 0; + a369=(a367-a368); + a369=(a0*a369); + a370=(a367-a368); + a371=(a369*a370); + a366=(a366+a371); + a5=(a5+a366); + a366=arg[0]? arg[0][72] : 0; + a371=casadi_sq(a366); + a372=arg[0]? arg[0][73] : 0; + a373=casadi_sq(a372); + a371=(a371+a373); + a5=(a5+a371); + a371=arg[1]? arg[1][41] : 0; + a373=cos(a371); + a374=arg[0]? arg[0][41] : 0; + a375=sin(a374); + a376=(a373*a375); + a377=sin(a371); + a378=cos(a374); + a379=(a377*a378); + a376=(a376-a379); + a379=cos(a371); + a380=(a379*a378); + a381=sin(a371); + a382=(a381*a375); + a380=(a380+a382); + a382=atan2(a376,a380); + a383=cos(a371); + a384=sin(a374); + a385=(a383*a384); + a386=sin(a371); + a387=cos(a374); + a388=(a386*a387); + a385=(a385-a388); + a388=cos(a371); + a389=(a388*a387); + a390=sin(a371); + a391=(a390*a384); + a389=(a389+a391); + a391=atan2(a385,a389); + a392=(a382*a391); + a5=(a5+a392); + a392=arg[0]? arg[0][42] : 0; + a393=arg[1]? arg[1][42] : 0; + a394=(a392-a393); + a394=(a0*a394); + a395=(a392-a393); + a396=(a394*a395); + a397=arg[0]? arg[0][43] : 0; + a398=arg[1]? arg[1][43] : 0; + a399=(a397-a398); + a399=(a0*a399); + a400=(a397-a398); + a401=(a399*a400); + a396=(a396+a401); + a5=(a5+a396); + a396=arg[0]? arg[0][74] : 0; + a401=casadi_sq(a396); + a402=arg[0]? arg[0][75] : 0; + a403=casadi_sq(a402); + a401=(a401+a403); + a5=(a5+a401); + a401=arg[1]? arg[1][44] : 0; + a403=cos(a401); + a404=arg[0]? arg[0][44] : 0; + a405=sin(a404); + a406=(a403*a405); + a407=sin(a401); + a408=cos(a404); + a409=(a407*a408); + a406=(a406-a409); + a409=cos(a401); + a410=(a409*a408); + a411=sin(a401); + a412=(a411*a405); + a410=(a410+a412); + a412=atan2(a406,a410); + a413=cos(a401); + a414=sin(a404); + a415=(a413*a414); + a416=sin(a401); + a417=cos(a404); + a418=(a416*a417); + a415=(a415-a418); + a418=cos(a401); + a419=(a418*a417); + a420=sin(a401); + a421=(a420*a414); + a419=(a419+a421); + a421=atan2(a415,a419); + a422=(a412*a421); + a5=(a5+a422); + a422=arg[0]? arg[0][45] : 0; + a423=arg[1]? arg[1][45] : 0; + a424=(a422-a423); + a424=(a0*a424); + a425=(a422-a423); + a426=(a424*a425); + a427=arg[0]? arg[0][46] : 0; + a428=arg[1]? arg[1][46] : 0; + a429=(a427-a428); + a429=(a0*a429); + a430=(a427-a428); + a431=(a429*a430); + a426=(a426+a431); + a5=(a5+a426); + a426=arg[0]? arg[0][76] : 0; + a431=casadi_sq(a426); + a432=arg[0]? arg[0][77] : 0; + a433=casadi_sq(a432); + a431=(a431+a433); + a5=(a5+a431); + a431=arg[1]? arg[1][47] : 0; + a433=cos(a431); + a434=arg[0]? arg[0][47] : 0; + a435=sin(a434); + a436=(a433*a435); + a437=sin(a431); + a438=cos(a434); + a439=(a437*a438); + a436=(a436-a439); + a439=cos(a431); + a440=(a439*a438); + a441=sin(a431); + a442=(a441*a435); + a440=(a440+a442); + a442=atan2(a436,a440); + a443=cos(a431); + a444=sin(a434); + a445=(a443*a444); + a446=sin(a431); + a447=cos(a434); + a448=(a446*a447); + a445=(a445-a448); + a448=cos(a431); + a449=(a448*a447); + a450=sin(a431); + a451=(a450*a444); + a449=(a449+a451); + a451=atan2(a445,a449); + a452=(a442*a451); + a5=(a5+a452); + a452=2.; + a453=arg[1]? arg[1][48] : 0; + a454=(a10-a453); + a454=(a452*a454); + a453=(a10-a453); + a455=(a454*a453); + a456=5.0000000000000000e-01; + a457=arg[1]? arg[1][49] : 0; + a458=(a12-a457); + a458=(a456*a458); + a459=(a12-a457); + a460=(a458*a459); + a455=(a455+a460); + a5=(a5+a455); + a455=(a36-a10); + a455=(a452*a455); + a460=(a36-a10); + a461=(a455*a460); + a462=(a42-a12); + a462=(a456*a462); + a463=(a42-a12); + a464=(a462*a463); + a461=(a461+a464); + a5=(a5+a461); + a461=(a66-a36); + a461=(a452*a461); + a464=(a66-a36); + a465=(a461*a464); + a466=(a72-a42); + a466=(a456*a466); + a467=(a72-a42); + a468=(a466*a467); + a465=(a465+a468); + a5=(a5+a465); + a465=(a96-a66); + a465=(a452*a465); + a468=(a96-a66); + a469=(a465*a468); + a470=(a102-a72); + a470=(a456*a470); + a471=(a102-a72); + a472=(a470*a471); + a469=(a469+a472); + a5=(a5+a469); + a469=(a126-a96); + a469=(a452*a469); + a472=(a126-a96); + a473=(a469*a472); + a474=(a132-a102); + a474=(a456*a474); + a475=(a132-a102); + a476=(a474*a475); + a473=(a473+a476); + a5=(a5+a473); + a473=(a156-a126); + a473=(a452*a473); + a476=(a156-a126); + a477=(a473*a476); + a478=(a162-a132); + a478=(a456*a478); + a479=(a162-a132); + a480=(a478*a479); + a477=(a477+a480); + a5=(a5+a477); + a477=(a186-a156); + a477=(a452*a477); + a480=(a186-a156); + a481=(a477*a480); + a482=(a192-a162); + a482=(a456*a482); + a483=(a192-a162); + a484=(a482*a483); + a481=(a481+a484); + a5=(a5+a481); + a481=(a216-a186); + a481=(a452*a481); + a484=(a216-a186); + a485=(a481*a484); + a486=(a222-a192); + a486=(a456*a486); + a487=(a222-a192); + a488=(a486*a487); + a485=(a485+a488); + a5=(a5+a485); + a485=(a246-a216); + a485=(a452*a485); + a488=(a246-a216); + a489=(a485*a488); + a490=(a252-a222); + a490=(a456*a490); + a491=(a252-a222); + a492=(a490*a491); + a489=(a489+a492); + a5=(a5+a489); + a489=(a276-a246); + a489=(a452*a489); + a492=(a276-a246); + a493=(a489*a492); + a494=(a282-a252); + a494=(a456*a494); + a495=(a282-a252); + a496=(a494*a495); + a493=(a493+a496); + a5=(a5+a493); + a493=(a306-a276); + a493=(a452*a493); + a496=(a306-a276); + a497=(a493*a496); + a498=(a312-a282); + a498=(a456*a498); + a499=(a312-a282); + a500=(a498*a499); + a497=(a497+a500); + a5=(a5+a497); + a497=(a336-a306); + a497=(a452*a497); + a500=(a336-a306); + a501=(a497*a500); + a502=(a342-a312); + a502=(a456*a502); + a503=(a342-a312); + a504=(a502*a503); + a501=(a501+a504); + a5=(a5+a501); + a501=(a366-a336); + a501=(a452*a501); + a504=(a366-a336); + a505=(a501*a504); + a506=(a372-a342); + a506=(a456*a506); + a507=(a372-a342); + a508=(a506*a507); + a505=(a505+a508); + a5=(a5+a505); + a505=(a396-a366); + a505=(a452*a505); + a508=(a396-a366); + a509=(a505*a508); + a510=(a402-a372); + a510=(a456*a510); + a511=(a402-a372); + a512=(a510*a511); + a509=(a509+a512); + a5=(a5+a509); + a509=(a426-a396); + a509=(a452*a509); + a512=(a426-a396); + a513=(a509*a512); + a514=(a432-a402); + a514=(a456*a514); + a515=(a432-a402); + a516=(a514*a515); + a513=(a513+a516); + a5=(a5+a513); + if (res[0]!=0) res[0][0]=a5; + a5=arg[0]? arg[0][0] : 0; + a513=arg[1]? arg[1][0] : 0; + a513=(a5-a513); + if (res[1]!=0) res[1][0]=a513; + a513=arg[0]? arg[0][1] : 0; + a516=arg[1]? arg[1][1] : 0; + a516=(a513-a516); + if (res[1]!=0) res[1][1]=a516; + a516=arg[0]? arg[0][2] : 0; + a517=arg[1]? arg[1][2] : 0; + a517=(a516-a517); + if (res[1]!=0) res[1][2]=a517; + a517=4.1666666666666664e-02; + a518=cos(a516); + a519=(a10*a518); + a520=1.2500000000000000e-01; + a521=4.0000000000000002e-01; + a522=(a521*a457); + a523=5.9999999999999998e-01; + a524=(a523*a12); + a522=(a522+a524); + a524=(a520*a522); + a524=(a516+a524); + a525=cos(a524); + a526=(a10*a525); + a526=(a452*a526); + a519=(a519+a526); + a526=(a521*a457); + a527=(a523*a12); + a526=(a526+a527); + a527=(a520*a526); + a527=(a516+a527); + a528=cos(a527); + a529=(a10*a528); + a529=(a452*a529); + a519=(a519+a529); + a529=2.5000000000000000e-01; + a530=(a521*a457); + a531=(a523*a12); + a530=(a530+a531); + a531=(a529*a530); + a531=(a516+a531); + a532=cos(a531); + a533=(a10*a532); + a519=(a519+a533); + a519=(a517*a519); + a519=(a5+a519); + a519=(a1-a519); + if (res[1]!=0) res[1][3]=a519; + a519=sin(a516); + a533=(a10*a519); + a534=sin(a524); + a535=(a10*a534); + a535=(a452*a535); + a533=(a533+a535); + a535=sin(a527); + a536=(a10*a535); + a536=(a452*a536); + a533=(a533+a536); + a536=sin(a531); + a537=(a10*a536); + a533=(a533+a537); + a533=(a517*a533); + a533=(a513+a533); + a533=(a6-a533); + if (res[1]!=0) res[1][4]=a533; + a533=cos(a14); + a526=(a452*a526); + a522=(a522+a526); + a530=(a452*a530); + a522=(a522+a530); + a530=(a521*a457); + a526=(a523*a12); + a530=(a530+a526); + a522=(a522+a530); + a522=(a517*a522); + a522=(a516+a522); + a530=sin(a522); + a526=(a533*a530); + a537=sin(a14); + a538=cos(a522); + a539=(a537*a538); + a526=(a526-a539); + a539=cos(a14); + a540=(a539*a538); + a541=sin(a14); + a542=(a541*a530); + a540=(a540+a542); + a542=atan2(a526,a540); + if (res[1]!=0) res[1][5]=a542; + a542=cos(a14); + a543=(a36*a542); + a544=(a523*a12); + a457=(a521*a457); + a544=(a544+a457); + a457=(a521*a544); + a545=(a523*a42); + a457=(a457+a545); + a545=(a520*a457); + a545=(a14+a545); + a546=cos(a545); + a547=(a36*a546); + a547=(a452*a547); + a543=(a543+a547); + a547=(a521*a544); + a548=(a523*a42); + a547=(a547+a548); + a548=(a520*a547); + a548=(a14+a548); + a549=cos(a548); + a550=(a36*a549); + a550=(a452*a550); + a543=(a543+a550); + a550=(a521*a544); + a551=(a523*a42); + a550=(a550+a551); + a551=(a529*a550); + a551=(a14+a551); + a552=cos(a551); + a553=(a36*a552); + a543=(a543+a553); + a543=(a517*a543); + a543=(a1+a543); + a543=(a32-a543); + if (res[1]!=0) res[1][6]=a543; + a543=sin(a14); + a553=(a36*a543); + a554=sin(a545); + a555=(a36*a554); + a555=(a452*a555); + a553=(a553+a555); + a555=sin(a548); + a556=(a36*a555); + a556=(a452*a556); + a553=(a553+a556); + a556=sin(a551); + a557=(a36*a556); + a553=(a553+a557); + a553=(a517*a553); + a553=(a6+a553); + a553=(a37-a553); + if (res[1]!=0) res[1][7]=a553; + a553=cos(a44); + a547=(a452*a547); + a457=(a457+a547); + a550=(a452*a550); + a457=(a457+a550); + a550=(a521*a544); + a547=(a523*a42); + a550=(a550+a547); + a457=(a457+a550); + a457=(a517*a457); + a457=(a14+a457); + a550=sin(a457); + a547=(a553*a550); + a557=sin(a44); + a558=cos(a457); + a559=(a557*a558); + a547=(a547-a559); + a559=cos(a44); + a560=(a559*a558); + a561=sin(a44); + a562=(a561*a550); + a560=(a560+a562); + a562=atan2(a547,a560); + if (res[1]!=0) res[1][8]=a562; + a562=cos(a44); + a563=(a66*a562); + a564=(a523*a42); + a544=(a521*a544); + a564=(a564+a544); + a544=(a521*a564); + a565=(a523*a72); + a544=(a544+a565); + a565=(a520*a544); + a565=(a44+a565); + a566=cos(a565); + a567=(a66*a566); + a567=(a452*a567); + a563=(a563+a567); + a567=(a521*a564); + a568=(a523*a72); + a567=(a567+a568); + a568=(a520*a567); + a568=(a44+a568); + a569=cos(a568); + a570=(a66*a569); + a570=(a452*a570); + a563=(a563+a570); + a570=(a521*a564); + a571=(a523*a72); + a570=(a570+a571); + a571=(a529*a570); + a571=(a44+a571); + a572=cos(a571); + a573=(a66*a572); + a563=(a563+a573); + a563=(a517*a563); + a563=(a32+a563); + a563=(a62-a563); + if (res[1]!=0) res[1][9]=a563; + a563=sin(a44); + a573=(a66*a563); + a574=sin(a565); + a575=(a66*a574); + a575=(a452*a575); + a573=(a573+a575); + a575=sin(a568); + a576=(a66*a575); + a576=(a452*a576); + a573=(a573+a576); + a576=sin(a571); + a577=(a66*a576); + a573=(a573+a577); + a573=(a517*a573); + a573=(a37+a573); + a573=(a67-a573); + if (res[1]!=0) res[1][10]=a573; + a573=cos(a74); + a567=(a452*a567); + a544=(a544+a567); + a570=(a452*a570); + a544=(a544+a570); + a570=(a521*a564); + a567=(a523*a72); + a570=(a570+a567); + a544=(a544+a570); + a544=(a517*a544); + a544=(a44+a544); + a570=sin(a544); + a567=(a573*a570); + a577=sin(a74); + a578=cos(a544); + a579=(a577*a578); + a567=(a567-a579); + a579=cos(a74); + a580=(a579*a578); + a581=sin(a74); + a582=(a581*a570); + a580=(a580+a582); + a582=atan2(a567,a580); + if (res[1]!=0) res[1][11]=a582; + a582=cos(a74); + a583=(a96*a582); + a584=(a523*a72); + a564=(a521*a564); + a584=(a584+a564); + a564=(a521*a584); + a585=(a523*a102); + a564=(a564+a585); + a585=(a520*a564); + a585=(a74+a585); + a586=cos(a585); + a587=(a96*a586); + a587=(a452*a587); + a583=(a583+a587); + a587=(a521*a584); + a588=(a523*a102); + a587=(a587+a588); + a588=(a520*a587); + a588=(a74+a588); + a589=cos(a588); + a590=(a96*a589); + a590=(a452*a590); + a583=(a583+a590); + a590=(a521*a584); + a591=(a523*a102); + a590=(a590+a591); + a591=(a529*a590); + a591=(a74+a591); + a592=cos(a591); + a593=(a96*a592); + a583=(a583+a593); + a583=(a517*a583); + a583=(a62+a583); + a583=(a92-a583); + if (res[1]!=0) res[1][12]=a583; + a583=sin(a74); + a593=(a96*a583); + a594=sin(a585); + a595=(a96*a594); + a595=(a452*a595); + a593=(a593+a595); + a595=sin(a588); + a596=(a96*a595); + a596=(a452*a596); + a593=(a593+a596); + a596=sin(a591); + a597=(a96*a596); + a593=(a593+a597); + a593=(a517*a593); + a593=(a67+a593); + a593=(a97-a593); + if (res[1]!=0) res[1][13]=a593; + a593=cos(a104); + a587=(a452*a587); + a564=(a564+a587); + a590=(a452*a590); + a564=(a564+a590); + a590=(a521*a584); + a587=(a523*a102); + a590=(a590+a587); + a564=(a564+a590); + a564=(a517*a564); + a564=(a74+a564); + a590=sin(a564); + a587=(a593*a590); + a597=sin(a104); + a598=cos(a564); + a599=(a597*a598); + a587=(a587-a599); + a599=cos(a104); + a600=(a599*a598); + a601=sin(a104); + a602=(a601*a590); + a600=(a600+a602); + a602=atan2(a587,a600); + if (res[1]!=0) res[1][14]=a602; + a602=cos(a104); + a603=(a126*a602); + a604=(a523*a102); + a584=(a521*a584); + a604=(a604+a584); + a584=(a521*a604); + a605=(a523*a132); + a584=(a584+a605); + a605=(a520*a584); + a605=(a104+a605); + a606=cos(a605); + a607=(a126*a606); + a607=(a452*a607); + a603=(a603+a607); + a607=(a521*a604); + a608=(a523*a132); + a607=(a607+a608); + a608=(a520*a607); + a608=(a104+a608); + a609=cos(a608); + a610=(a126*a609); + a610=(a452*a610); + a603=(a603+a610); + a610=(a521*a604); + a611=(a523*a132); + a610=(a610+a611); + a611=(a529*a610); + a611=(a104+a611); + a612=cos(a611); + a613=(a126*a612); + a603=(a603+a613); + a603=(a517*a603); + a603=(a92+a603); + a603=(a122-a603); + if (res[1]!=0) res[1][15]=a603; + a603=sin(a104); + a613=(a126*a603); + a614=sin(a605); + a615=(a126*a614); + a615=(a452*a615); + a613=(a613+a615); + a615=sin(a608); + a616=(a126*a615); + a616=(a452*a616); + a613=(a613+a616); + a616=sin(a611); + a617=(a126*a616); + a613=(a613+a617); + a613=(a517*a613); + a613=(a97+a613); + a613=(a127-a613); + if (res[1]!=0) res[1][16]=a613; + a613=cos(a134); + a607=(a452*a607); + a584=(a584+a607); + a610=(a452*a610); + a584=(a584+a610); + a610=(a521*a604); + a607=(a523*a132); + a610=(a610+a607); + a584=(a584+a610); + a584=(a517*a584); + a584=(a104+a584); + a610=sin(a584); + a607=(a613*a610); + a617=sin(a134); + a618=cos(a584); + a619=(a617*a618); + a607=(a607-a619); + a619=cos(a134); + a620=(a619*a618); + a621=sin(a134); + a622=(a621*a610); + a620=(a620+a622); + a622=atan2(a607,a620); + if (res[1]!=0) res[1][17]=a622; + a622=cos(a134); + a623=(a156*a622); + a624=(a523*a132); + a604=(a521*a604); + a624=(a624+a604); + a604=(a521*a624); + a625=(a523*a162); + a604=(a604+a625); + a625=(a520*a604); + a625=(a134+a625); + a626=cos(a625); + a627=(a156*a626); + a627=(a452*a627); + a623=(a623+a627); + a627=(a521*a624); + a628=(a523*a162); + a627=(a627+a628); + a628=(a520*a627); + a628=(a134+a628); + a629=cos(a628); + a630=(a156*a629); + a630=(a452*a630); + a623=(a623+a630); + a630=(a521*a624); + a631=(a523*a162); + a630=(a630+a631); + a631=(a529*a630); + a631=(a134+a631); + a632=cos(a631); + a633=(a156*a632); + a623=(a623+a633); + a623=(a517*a623); + a623=(a122+a623); + a623=(a152-a623); + if (res[1]!=0) res[1][18]=a623; + a623=sin(a134); + a633=(a156*a623); + a634=sin(a625); + a635=(a156*a634); + a635=(a452*a635); + a633=(a633+a635); + a635=sin(a628); + a636=(a156*a635); + a636=(a452*a636); + a633=(a633+a636); + a636=sin(a631); + a637=(a156*a636); + a633=(a633+a637); + a633=(a517*a633); + a633=(a127+a633); + a633=(a157-a633); + if (res[1]!=0) res[1][19]=a633; + a633=cos(a164); + a627=(a452*a627); + a604=(a604+a627); + a630=(a452*a630); + a604=(a604+a630); + a630=(a521*a624); + a627=(a523*a162); + a630=(a630+a627); + a604=(a604+a630); + a604=(a517*a604); + a604=(a134+a604); + a630=sin(a604); + a627=(a633*a630); + a637=sin(a164); + a638=cos(a604); + a639=(a637*a638); + a627=(a627-a639); + a639=cos(a164); + a640=(a639*a638); + a641=sin(a164); + a642=(a641*a630); + a640=(a640+a642); + a642=atan2(a627,a640); + if (res[1]!=0) res[1][20]=a642; + a642=cos(a164); + a643=(a186*a642); + a644=(a523*a162); + a624=(a521*a624); + a644=(a644+a624); + a624=(a521*a644); + a645=(a523*a192); + a624=(a624+a645); + a645=(a520*a624); + a645=(a164+a645); + a646=cos(a645); + a647=(a186*a646); + a647=(a452*a647); + a643=(a643+a647); + a647=(a521*a644); + a648=(a523*a192); + a647=(a647+a648); + a648=(a520*a647); + a648=(a164+a648); + a649=cos(a648); + a650=(a186*a649); + a650=(a452*a650); + a643=(a643+a650); + a650=(a521*a644); + a651=(a523*a192); + a650=(a650+a651); + a651=(a529*a650); + a651=(a164+a651); + a652=cos(a651); + a653=(a186*a652); + a643=(a643+a653); + a643=(a517*a643); + a643=(a152+a643); + a643=(a182-a643); + if (res[1]!=0) res[1][21]=a643; + a643=sin(a164); + a653=(a186*a643); + a654=sin(a645); + a655=(a186*a654); + a655=(a452*a655); + a653=(a653+a655); + a655=sin(a648); + a656=(a186*a655); + a656=(a452*a656); + a653=(a653+a656); + a656=sin(a651); + a657=(a186*a656); + a653=(a653+a657); + a653=(a517*a653); + a653=(a157+a653); + a653=(a187-a653); + if (res[1]!=0) res[1][22]=a653; + a653=cos(a194); + a647=(a452*a647); + a624=(a624+a647); + a650=(a452*a650); + a624=(a624+a650); + a650=(a521*a644); + a647=(a523*a192); + a650=(a650+a647); + a624=(a624+a650); + a624=(a517*a624); + a624=(a164+a624); + a650=sin(a624); + a647=(a653*a650); + a657=sin(a194); + a658=cos(a624); + a659=(a657*a658); + a647=(a647-a659); + a659=cos(a194); + a660=(a659*a658); + a661=sin(a194); + a662=(a661*a650); + a660=(a660+a662); + a662=atan2(a647,a660); + if (res[1]!=0) res[1][23]=a662; + a662=cos(a194); + a663=(a216*a662); + a664=(a523*a192); + a644=(a521*a644); + a664=(a664+a644); + a644=(a521*a664); + a665=(a523*a222); + a644=(a644+a665); + a665=(a520*a644); + a665=(a194+a665); + a666=cos(a665); + a667=(a216*a666); + a667=(a452*a667); + a663=(a663+a667); + a667=(a521*a664); + a668=(a523*a222); + a667=(a667+a668); + a668=(a520*a667); + a668=(a194+a668); + a669=cos(a668); + a670=(a216*a669); + a670=(a452*a670); + a663=(a663+a670); + a670=(a521*a664); + a671=(a523*a222); + a670=(a670+a671); + a671=(a529*a670); + a671=(a194+a671); + a672=cos(a671); + a673=(a216*a672); + a663=(a663+a673); + a663=(a517*a663); + a663=(a182+a663); + a663=(a212-a663); + if (res[1]!=0) res[1][24]=a663; + a663=sin(a194); + a673=(a216*a663); + a674=sin(a665); + a675=(a216*a674); + a675=(a452*a675); + a673=(a673+a675); + a675=sin(a668); + a676=(a216*a675); + a676=(a452*a676); + a673=(a673+a676); + a676=sin(a671); + a677=(a216*a676); + a673=(a673+a677); + a673=(a517*a673); + a673=(a187+a673); + a673=(a217-a673); + if (res[1]!=0) res[1][25]=a673; + a673=cos(a224); + a667=(a452*a667); + a644=(a644+a667); + a670=(a452*a670); + a644=(a644+a670); + a670=(a521*a664); + a667=(a523*a222); + a670=(a670+a667); + a644=(a644+a670); + a644=(a517*a644); + a644=(a194+a644); + a670=sin(a644); + a667=(a673*a670); + a677=sin(a224); + a678=cos(a644); + a679=(a677*a678); + a667=(a667-a679); + a679=cos(a224); + a680=(a679*a678); + a681=sin(a224); + a682=(a681*a670); + a680=(a680+a682); + a682=atan2(a667,a680); + if (res[1]!=0) res[1][26]=a682; + a682=cos(a224); + a683=(a246*a682); + a684=(a523*a222); + a664=(a521*a664); + a684=(a684+a664); + a664=(a521*a684); + a685=(a523*a252); + a664=(a664+a685); + a685=(a520*a664); + a685=(a224+a685); + a686=cos(a685); + a687=(a246*a686); + a687=(a452*a687); + a683=(a683+a687); + a687=(a521*a684); + a688=(a523*a252); + a687=(a687+a688); + a688=(a520*a687); + a688=(a224+a688); + a689=cos(a688); + a690=(a246*a689); + a690=(a452*a690); + a683=(a683+a690); + a690=(a521*a684); + a691=(a523*a252); + a690=(a690+a691); + a691=(a529*a690); + a691=(a224+a691); + a692=cos(a691); + a693=(a246*a692); + a683=(a683+a693); + a683=(a517*a683); + a683=(a212+a683); + a683=(a242-a683); + if (res[1]!=0) res[1][27]=a683; + a683=sin(a224); + a693=(a246*a683); + a694=sin(a685); + a695=(a246*a694); + a695=(a452*a695); + a693=(a693+a695); + a695=sin(a688); + a696=(a246*a695); + a696=(a452*a696); + a693=(a693+a696); + a696=sin(a691); + a697=(a246*a696); + a693=(a693+a697); + a693=(a517*a693); + a693=(a217+a693); + a693=(a247-a693); + if (res[1]!=0) res[1][28]=a693; + a693=cos(a254); + a687=(a452*a687); + a664=(a664+a687); + a690=(a452*a690); + a664=(a664+a690); + a690=(a521*a684); + a687=(a523*a252); + a690=(a690+a687); + a664=(a664+a690); + a664=(a517*a664); + a664=(a224+a664); + a690=sin(a664); + a687=(a693*a690); + a697=sin(a254); + a698=cos(a664); + a699=(a697*a698); + a687=(a687-a699); + a699=cos(a254); + a700=(a699*a698); + a701=sin(a254); + a702=(a701*a690); + a700=(a700+a702); + a702=atan2(a687,a700); + if (res[1]!=0) res[1][29]=a702; + a702=cos(a254); + a703=(a276*a702); + a704=(a523*a252); + a684=(a521*a684); + a704=(a704+a684); + a684=(a521*a704); + a705=(a523*a282); + a684=(a684+a705); + a705=(a520*a684); + a705=(a254+a705); + a706=cos(a705); + a707=(a276*a706); + a707=(a452*a707); + a703=(a703+a707); + a707=(a521*a704); + a708=(a523*a282); + a707=(a707+a708); + a708=(a520*a707); + a708=(a254+a708); + a709=cos(a708); + a710=(a276*a709); + a710=(a452*a710); + a703=(a703+a710); + a710=(a521*a704); + a711=(a523*a282); + a710=(a710+a711); + a711=(a529*a710); + a711=(a254+a711); + a712=cos(a711); + a713=(a276*a712); + a703=(a703+a713); + a703=(a517*a703); + a703=(a242+a703); + a703=(a272-a703); + if (res[1]!=0) res[1][30]=a703; + a703=sin(a254); + a713=(a276*a703); + a714=sin(a705); + a715=(a276*a714); + a715=(a452*a715); + a713=(a713+a715); + a715=sin(a708); + a716=(a276*a715); + a716=(a452*a716); + a713=(a713+a716); + a716=sin(a711); + a717=(a276*a716); + a713=(a713+a717); + a713=(a517*a713); + a713=(a247+a713); + a713=(a277-a713); + if (res[1]!=0) res[1][31]=a713; + a713=cos(a284); + a707=(a452*a707); + a684=(a684+a707); + a710=(a452*a710); + a684=(a684+a710); + a710=(a521*a704); + a707=(a523*a282); + a710=(a710+a707); + a684=(a684+a710); + a684=(a517*a684); + a684=(a254+a684); + a710=sin(a684); + a707=(a713*a710); + a717=sin(a284); + a718=cos(a684); + a719=(a717*a718); + a707=(a707-a719); + a719=cos(a284); + a720=(a719*a718); + a721=sin(a284); + a722=(a721*a710); + a720=(a720+a722); + a722=atan2(a707,a720); + if (res[1]!=0) res[1][32]=a722; + a722=cos(a284); + a723=(a306*a722); + a724=(a523*a282); + a704=(a521*a704); + a724=(a724+a704); + a704=(a521*a724); + a725=(a523*a312); + a704=(a704+a725); + a725=(a520*a704); + a725=(a284+a725); + a726=cos(a725); + a727=(a306*a726); + a727=(a452*a727); + a723=(a723+a727); + a727=(a521*a724); + a728=(a523*a312); + a727=(a727+a728); + a728=(a520*a727); + a728=(a284+a728); + a729=cos(a728); + a730=(a306*a729); + a730=(a452*a730); + a723=(a723+a730); + a730=(a521*a724); + a731=(a523*a312); + a730=(a730+a731); + a731=(a529*a730); + a731=(a284+a731); + a732=cos(a731); + a733=(a306*a732); + a723=(a723+a733); + a723=(a517*a723); + a723=(a272+a723); + a723=(a302-a723); + if (res[1]!=0) res[1][33]=a723; + a723=sin(a284); + a733=(a306*a723); + a734=sin(a725); + a735=(a306*a734); + a735=(a452*a735); + a733=(a733+a735); + a735=sin(a728); + a736=(a306*a735); + a736=(a452*a736); + a733=(a733+a736); + a736=sin(a731); + a737=(a306*a736); + a733=(a733+a737); + a733=(a517*a733); + a733=(a277+a733); + a733=(a307-a733); + if (res[1]!=0) res[1][34]=a733; + a733=cos(a314); + a727=(a452*a727); + a704=(a704+a727); + a730=(a452*a730); + a704=(a704+a730); + a730=(a521*a724); + a727=(a523*a312); + a730=(a730+a727); + a704=(a704+a730); + a704=(a517*a704); + a704=(a284+a704); + a730=sin(a704); + a727=(a733*a730); + a737=sin(a314); + a738=cos(a704); + a739=(a737*a738); + a727=(a727-a739); + a739=cos(a314); + a740=(a739*a738); + a741=sin(a314); + a742=(a741*a730); + a740=(a740+a742); + a742=atan2(a727,a740); + if (res[1]!=0) res[1][35]=a742; + a742=cos(a314); + a743=(a336*a742); + a744=(a523*a312); + a724=(a521*a724); + a744=(a744+a724); + a724=(a521*a744); + a745=(a523*a342); + a724=(a724+a745); + a745=(a520*a724); + a745=(a314+a745); + a746=cos(a745); + a747=(a336*a746); + a747=(a452*a747); + a743=(a743+a747); + a747=(a521*a744); + a748=(a523*a342); + a747=(a747+a748); + a748=(a520*a747); + a748=(a314+a748); + a749=cos(a748); + a750=(a336*a749); + a750=(a452*a750); + a743=(a743+a750); + a750=(a521*a744); + a751=(a523*a342); + a750=(a750+a751); + a751=(a529*a750); + a751=(a314+a751); + a752=cos(a751); + a753=(a336*a752); + a743=(a743+a753); + a743=(a517*a743); + a743=(a302+a743); + a743=(a332-a743); + if (res[1]!=0) res[1][36]=a743; + a743=sin(a314); + a753=(a336*a743); + a754=sin(a745); + a755=(a336*a754); + a755=(a452*a755); + a753=(a753+a755); + a755=sin(a748); + a756=(a336*a755); + a756=(a452*a756); + a753=(a753+a756); + a756=sin(a751); + a757=(a336*a756); + a753=(a753+a757); + a753=(a517*a753); + a753=(a307+a753); + a753=(a337-a753); + if (res[1]!=0) res[1][37]=a753; + a753=cos(a344); + a747=(a452*a747); + a724=(a724+a747); + a750=(a452*a750); + a724=(a724+a750); + a750=(a521*a744); + a747=(a523*a342); + a750=(a750+a747); + a724=(a724+a750); + a724=(a517*a724); + a724=(a314+a724); + a750=sin(a724); + a747=(a753*a750); + a757=sin(a344); + a758=cos(a724); + a759=(a757*a758); + a747=(a747-a759); + a759=cos(a344); + a760=(a759*a758); + a761=sin(a344); + a762=(a761*a750); + a760=(a760+a762); + a762=atan2(a747,a760); + if (res[1]!=0) res[1][38]=a762; + a762=cos(a344); + a763=(a366*a762); + a764=(a523*a342); + a744=(a521*a744); + a764=(a764+a744); + a744=(a521*a764); + a765=(a523*a372); + a744=(a744+a765); + a765=(a520*a744); + a765=(a344+a765); + a766=cos(a765); + a767=(a366*a766); + a767=(a452*a767); + a763=(a763+a767); + a767=(a521*a764); + a768=(a523*a372); + a767=(a767+a768); + a768=(a520*a767); + a768=(a344+a768); + a769=cos(a768); + a770=(a366*a769); + a770=(a452*a770); + a763=(a763+a770); + a770=(a521*a764); + a771=(a523*a372); + a770=(a770+a771); + a771=(a529*a770); + a771=(a344+a771); + a772=cos(a771); + a773=(a366*a772); + a763=(a763+a773); + a763=(a517*a763); + a763=(a332+a763); + a763=(a362-a763); + if (res[1]!=0) res[1][39]=a763; + a763=sin(a344); + a773=(a366*a763); + a774=sin(a765); + a775=(a366*a774); + a775=(a452*a775); + a773=(a773+a775); + a775=sin(a768); + a776=(a366*a775); + a776=(a452*a776); + a773=(a773+a776); + a776=sin(a771); + a777=(a366*a776); + a773=(a773+a777); + a773=(a517*a773); + a773=(a337+a773); + a773=(a367-a773); + if (res[1]!=0) res[1][40]=a773; + a773=cos(a374); + a767=(a452*a767); + a744=(a744+a767); + a770=(a452*a770); + a744=(a744+a770); + a770=(a521*a764); + a767=(a523*a372); + a770=(a770+a767); + a744=(a744+a770); + a744=(a517*a744); + a744=(a344+a744); + a770=sin(a744); + a767=(a773*a770); + a777=sin(a374); + a778=cos(a744); + a779=(a777*a778); + a767=(a767-a779); + a779=cos(a374); + a780=(a779*a778); + a781=sin(a374); + a782=(a781*a770); + a780=(a780+a782); + a782=atan2(a767,a780); + if (res[1]!=0) res[1][41]=a782; + a782=cos(a374); + a783=(a396*a782); + a784=(a523*a372); + a764=(a521*a764); + a784=(a784+a764); + a764=(a521*a784); + a785=(a523*a402); + a764=(a764+a785); + a785=(a520*a764); + a785=(a374+a785); + a786=cos(a785); + a787=(a396*a786); + a787=(a452*a787); + a783=(a783+a787); + a787=(a521*a784); + a788=(a523*a402); + a787=(a787+a788); + a788=(a520*a787); + a788=(a374+a788); + a789=cos(a788); + a790=(a396*a789); + a790=(a452*a790); + a783=(a783+a790); + a790=(a521*a784); + a791=(a523*a402); + a790=(a790+a791); + a791=(a529*a790); + a791=(a374+a791); + a792=cos(a791); + a793=(a396*a792); + a783=(a783+a793); + a783=(a517*a783); + a783=(a362+a783); + a783=(a392-a783); + if (res[1]!=0) res[1][42]=a783; + a783=sin(a374); + a793=(a396*a783); + a794=sin(a785); + a795=(a396*a794); + a795=(a452*a795); + a793=(a793+a795); + a795=sin(a788); + a796=(a396*a795); + a796=(a452*a796); + a793=(a793+a796); + a796=sin(a791); + a797=(a396*a796); + a793=(a793+a797); + a793=(a517*a793); + a793=(a367+a793); + a793=(a397-a793); + if (res[1]!=0) res[1][43]=a793; + a793=cos(a404); + a787=(a452*a787); + a764=(a764+a787); + a790=(a452*a790); + a764=(a764+a790); + a790=(a521*a784); + a787=(a523*a402); + a790=(a790+a787); + a764=(a764+a790); + a764=(a517*a764); + a764=(a374+a764); + a790=sin(a764); + a787=(a793*a790); + a797=sin(a404); + a798=cos(a764); + a799=(a797*a798); + a787=(a787-a799); + a799=cos(a404); + a800=(a799*a798); + a801=sin(a404); + a802=(a801*a790); + a800=(a800+a802); + a802=atan2(a787,a800); + if (res[1]!=0) res[1][44]=a802; + a802=cos(a404); + a803=(a426*a802); + a804=(a523*a402); + a784=(a521*a784); + a804=(a804+a784); + a784=(a521*a804); + a805=(a523*a432); + a784=(a784+a805); + a805=(a520*a784); + a805=(a404+a805); + a806=cos(a805); + a807=(a426*a806); + a807=(a452*a807); + a803=(a803+a807); + a807=(a521*a804); + a808=(a523*a432); + a807=(a807+a808); + a808=(a520*a807); + a808=(a404+a808); + a809=cos(a808); + a810=(a426*a809); + a810=(a452*a810); + a803=(a803+a810); + a810=(a521*a804); + a811=(a523*a432); + a810=(a810+a811); + a811=(a529*a810); + a811=(a404+a811); + a812=cos(a811); + a813=(a426*a812); + a803=(a803+a813); + a803=(a517*a803); + a803=(a392+a803); + a422=(a422-a803); + if (res[1]!=0) res[1][45]=a422; + a422=sin(a404); + a803=(a426*a422); + a813=sin(a805); + a814=(a426*a813); + a814=(a452*a814); + a803=(a803+a814); + a814=sin(a808); + a815=(a426*a814); + a815=(a452*a815); + a803=(a803+a815); + a815=sin(a811); + a816=(a426*a815); + a803=(a803+a816); + a803=(a517*a803); + a803=(a397+a803); + a427=(a427-a803); + if (res[1]!=0) res[1][46]=a427; + a427=cos(a434); + a807=(a452*a807); + a784=(a784+a807); + a810=(a452*a810); + a784=(a784+a810); + a804=(a521*a804); + a810=(a523*a432); + a804=(a804+a810); + a784=(a784+a804); + a784=(a517*a784); + a784=(a404+a784); + a804=sin(a784); + a810=(a427*a804); + a807=sin(a434); + a803=cos(a784); + a816=(a807*a803); + a810=(a810-a816); + a816=cos(a434); + a817=(a816*a803); + a818=sin(a434); + a819=(a818*a804); + a817=(a817+a819); + a819=atan2(a810,a817); + if (res[1]!=0) res[1][47]=a819; + a819=cos(a11); + a513=(a513-a7); + a7=(a819*a513); + a820=sin(a11); + a5=(a5-a2); + a2=(a820*a5); + a7=(a7-a2); + if (res[1]!=0) res[1][48]=a7; + a7=cos(a41); + a6=(a6-a38); + a38=(a7*a6); + a2=sin(a41); + a1=(a1-a33); + a33=(a2*a1); + a38=(a38-a33); + if (res[1]!=0) res[1][49]=a38; + a38=cos(a71); + a37=(a37-a68); + a68=(a38*a37); + a33=sin(a71); + a32=(a32-a63); + a63=(a33*a32); + a68=(a68-a63); + if (res[1]!=0) res[1][50]=a68; + a68=cos(a101); + a67=(a67-a98); + a98=(a68*a67); + a63=sin(a101); + a62=(a62-a93); + a93=(a63*a62); + a98=(a98-a93); + if (res[1]!=0) res[1][51]=a98; + a98=cos(a131); + a97=(a97-a128); + a128=(a98*a97); + a93=sin(a131); + a92=(a92-a123); + a123=(a93*a92); + a128=(a128-a123); + if (res[1]!=0) res[1][52]=a128; + a128=cos(a161); + a127=(a127-a158); + a158=(a128*a127); + a123=sin(a161); + a122=(a122-a153); + a153=(a123*a122); + a158=(a158-a153); + if (res[1]!=0) res[1][53]=a158; + a158=cos(a191); + a157=(a157-a188); + a188=(a158*a157); + a153=sin(a191); + a152=(a152-a183); + a183=(a153*a152); + a188=(a188-a183); + if (res[1]!=0) res[1][54]=a188; + a188=cos(a221); + a187=(a187-a218); + a218=(a188*a187); + a183=sin(a221); + a182=(a182-a213); + a213=(a183*a182); + a218=(a218-a213); + if (res[1]!=0) res[1][55]=a218; + a218=cos(a251); + a217=(a217-a248); + a248=(a218*a217); + a213=sin(a251); + a212=(a212-a243); + a243=(a213*a212); + a248=(a248-a243); + if (res[1]!=0) res[1][56]=a248; + a248=cos(a281); + a247=(a247-a278); + a278=(a248*a247); + a243=sin(a281); + a242=(a242-a273); + a273=(a243*a242); + a278=(a278-a273); + if (res[1]!=0) res[1][57]=a278; + a278=cos(a311); + a277=(a277-a308); + a308=(a278*a277); + a273=sin(a311); + a272=(a272-a303); + a303=(a273*a272); + a308=(a308-a303); + if (res[1]!=0) res[1][58]=a308; + a308=cos(a341); + a307=(a307-a338); + a338=(a308*a307); + a303=sin(a341); + a302=(a302-a333); + a333=(a303*a302); + a338=(a338-a333); + if (res[1]!=0) res[1][59]=a338; + a338=cos(a371); + a337=(a337-a368); + a368=(a338*a337); + a333=sin(a371); + a332=(a332-a363); + a363=(a333*a332); + a368=(a368-a363); + if (res[1]!=0) res[1][60]=a368; + a368=cos(a401); + a367=(a367-a398); + a398=(a368*a367); + a363=sin(a401); + a362=(a362-a393); + a393=(a363*a362); + a398=(a398-a393); + if (res[1]!=0) res[1][61]=a398; + a398=cos(a431); + a397=(a397-a428); + a428=(a398*a397); + a393=sin(a431); + a392=(a392-a423); + a423=(a393*a392); + a428=(a428-a423); + if (res[1]!=0) res[1][62]=a428; + a428=arg[3]? arg[3][0] : 0; + a423=arg[3]? arg[3][48] : 0; + a821=(a820*a423); + a822=arg[3]? arg[3][3] : 0; + a821=(a821+a822); + a821=(a428-a821); + if (res[2]!=0) res[2][0]=a821; + a821=(a819*a423); + a823=arg[3]? arg[3][4] : 0; + a821=(a821-a823); + a824=arg[3]? arg[3][1] : 0; + a821=(a821+a824); + if (res[2]!=0) res[2][1]=a821; + a821=sin(a522); + a825=casadi_sq(a526); + a826=casadi_sq(a540); + a825=(a825+a826); + a826=(a526/a825); + a827=arg[3]? arg[3][5] : 0; + a826=(a826*a827); + a828=(a539*a826); + a825=(a540/a825); + a825=(a825*a827); + a829=(a537*a825); + a828=(a828+a829); + a821=(a821*a828); + a828=cos(a522); + a829=(a533*a825); + a830=(a541*a826); + a829=(a829-a830); + a828=(a828*a829); + a821=(a821+a828); + a828=cos(a516); + a829=(a517*a823); + a830=(a10*a829); + a828=(a828*a830); + a828=(a821-a828); + a830=sin(a531); + a831=(a517*a822); + a832=(a10*a831); + a830=(a830*a832); + a832=cos(a531); + a833=(a10*a829); + a832=(a832*a833); + a830=(a830-a832); + a828=(a828+a830); + a832=sin(a527); + a833=(a452*a831); + a834=(a10*a833); + a832=(a832*a834); + a834=cos(a527); + a835=(a452*a829); + a836=(a10*a835); + a834=(a834*a836); + a832=(a832-a834); + a828=(a828+a832); + a834=sin(a524); + a836=(a452*a831); + a837=(a10*a836); + a834=(a834*a837); + a837=cos(a524); + a838=(a452*a829); + a839=(a10*a838); + a837=(a837*a839); + a834=(a834-a837); + a828=(a828+a834); + a516=sin(a516); + a837=(a10*a831); + a516=(a516*a837); + a828=(a828+a516); + a516=arg[3]? arg[3][2] : 0; + a828=(a828+a516); + if (res[2]!=0) res[2][2]=a828; + a828=arg[3]? arg[3][49] : 0; + a837=(a2*a828); + a839=arg[3]? arg[3][6] : 0; + a837=(a837+a839); + a837=(a822-a837); + a840=arg[2]? arg[2][0] : 0; + a841=(a3*a840); + a837=(a837+a841); + a841=(a4*a840); + a841=(a0*a841); + a837=(a837+a841); + if (res[2]!=0) res[2][3]=a837; + a837=(a7*a828); + a841=arg[3]? arg[3][7] : 0; + a837=(a837-a841); + a837=(a837+a823); + a842=(a8*a840); + a837=(a837+a842); + a842=(a9*a840); + a842=(a0*a842); + a837=(a837+a842); + if (res[2]!=0) res[2][4]=a837; + a837=sin(a457); + a842=casadi_sq(a547); + a843=casadi_sq(a560); + a842=(a842+a843); + a843=(a547/a842); + a844=arg[3]? arg[3][8] : 0; + a843=(a843*a844); + a845=(a559*a843); + a842=(a560/a842); + a842=(a842*a844); + a846=(a557*a842); + a845=(a845+a846); + a837=(a837*a845); + a845=cos(a457); + a846=(a553*a842); + a847=(a561*a843); + a846=(a846-a847); + a845=(a845*a846); + a837=(a837+a845); + a845=cos(a14); + a846=(a517*a841); + a847=(a36*a846); + a845=(a845*a847); + a845=(a837-a845); + a847=sin(a551); + a848=(a517*a839); + a849=(a36*a848); + a847=(a847*a849); + a849=cos(a551); + a850=(a36*a846); + a849=(a849*a850); + a847=(a847-a849); + a845=(a845+a847); + a849=sin(a548); + a850=(a452*a848); + a851=(a36*a850); + a849=(a849*a851); + a851=cos(a548); + a852=(a452*a846); + a853=(a36*a852); + a851=(a851*a853); + a849=(a849-a851); + a845=(a845+a849); + a851=sin(a545); + a853=(a452*a848); + a854=(a36*a853); + a851=(a851*a854); + a854=cos(a545); + a855=(a452*a846); + a856=(a36*a855); + a854=(a854*a856); + a851=(a851-a854); + a845=(a845+a851); + a854=sin(a14); + a856=(a36*a848); + a854=(a854*a856); + a845=(a845+a854); + a854=cos(a14); + a856=(a530*a826); + a854=(a854*a856); + a845=(a845-a854); + a854=sin(a14); + a826=(a538*a826); + a854=(a854*a826); + a845=(a845+a854); + a854=cos(a14); + a538=(a538*a825); + a854=(a854*a538); + a845=(a845-a854); + a854=sin(a14); + a530=(a530*a825); + a854=(a854*a530); + a845=(a845-a854); + a854=sin(a14); + a530=casadi_sq(a25); + a825=casadi_sq(a29); + a530=(a530+a825); + a825=(a25/a530); + a538=(a22*a840); + a825=(a825*a538); + a28=(a28*a825); + a530=(a29/a530); + a530=(a530*a538); + a26=(a26*a530); + a28=(a28+a26); + a854=(a854*a28); + a845=(a845+a854); + a854=cos(a14); + a23=(a23*a530); + a30=(a30*a825); + a23=(a23-a30); + a854=(a854*a23); + a845=(a845+a854); + a854=sin(a14); + a23=casadi_sq(a16); + a30=casadi_sq(a20); + a23=(a23+a30); + a30=(a16/a23); + a825=(a31*a840); + a30=(a30*a825); + a19=(a19*a30); + a23=(a20/a23); + a23=(a23*a825); + a17=(a17*a23); + a19=(a19+a17); + a854=(a854*a19); + a845=(a845+a854); + a14=cos(a14); + a13=(a13*a23); + a21=(a21*a30); + a13=(a13-a21); + a14=(a14*a13); + a845=(a845+a14); + if (res[2]!=0) res[2][5]=a845; + a845=arg[3]? arg[3][50] : 0; + a14=(a33*a845); + a13=arg[3]? arg[3][9] : 0; + a14=(a14+a13); + a14=(a839-a14); + a21=(a34*a840); + a14=(a14+a21); + a21=(a35*a840); + a21=(a0*a21); + a14=(a14+a21); + if (res[2]!=0) res[2][6]=a14; + a14=(a38*a845); + a21=arg[3]? arg[3][10] : 0; + a14=(a14-a21); + a14=(a14+a841); + a30=(a39*a840); + a14=(a14+a30); + a30=(a40*a840); + a30=(a0*a30); + a14=(a14+a30); + if (res[2]!=0) res[2][7]=a14; + a14=sin(a544); + a30=casadi_sq(a567); + a23=casadi_sq(a580); + a30=(a30+a23); + a23=(a567/a30); + a854=arg[3]? arg[3][11] : 0; + a23=(a23*a854); + a19=(a579*a23); + a30=(a580/a30); + a30=(a30*a854); + a17=(a577*a30); + a19=(a19+a17); + a14=(a14*a19); + a19=cos(a544); + a17=(a573*a30); + a825=(a581*a23); + a17=(a17-a825); + a19=(a19*a17); + a14=(a14+a19); + a19=cos(a44); + a17=(a517*a21); + a825=(a66*a17); + a19=(a19*a825); + a19=(a14-a19); + a825=sin(a571); + a530=(a517*a13); + a28=(a66*a530); + a825=(a825*a28); + a28=cos(a571); + a26=(a66*a17); + a28=(a28*a26); + a825=(a825-a28); + a19=(a19+a825); + a28=sin(a568); + a26=(a452*a530); + a538=(a66*a26); + a28=(a28*a538); + a538=cos(a568); + a826=(a452*a17); + a856=(a66*a826); + a538=(a538*a856); + a28=(a28-a538); + a19=(a19+a28); + a538=sin(a565); + a856=(a452*a530); + a857=(a66*a856); + a538=(a538*a857); + a857=cos(a565); + a858=(a452*a17); + a859=(a66*a858); + a857=(a857*a859); + a538=(a538-a857); + a19=(a19+a538); + a857=sin(a44); + a859=(a66*a530); + a857=(a857*a859); + a19=(a19+a857); + a857=cos(a44); + a859=(a550*a843); + a857=(a857*a859); + a19=(a19-a857); + a857=sin(a44); + a843=(a558*a843); + a857=(a857*a843); + a19=(a19+a857); + a857=cos(a44); + a558=(a558*a842); + a857=(a857*a558); + a19=(a19-a857); + a857=sin(a44); + a550=(a550*a842); + a857=(a857*a550); + a19=(a19-a857); + a857=sin(a44); + a550=casadi_sq(a55); + a842=casadi_sq(a59); + a550=(a550+a842); + a842=(a55/a550); + a558=(a52*a840); + a842=(a842*a558); + a58=(a58*a842); + a550=(a59/a550); + a550=(a550*a558); + a56=(a56*a550); + a58=(a58+a56); + a857=(a857*a58); + a19=(a19+a857); + a857=cos(a44); + a53=(a53*a550); + a60=(a60*a842); + a53=(a53-a60); + a857=(a857*a53); + a19=(a19+a857); + a857=sin(a44); + a53=casadi_sq(a46); + a60=casadi_sq(a50); + a53=(a53+a60); + a60=(a46/a53); + a842=(a61*a840); + a60=(a60*a842); + a49=(a49*a60); + a53=(a50/a53); + a53=(a53*a842); + a47=(a47*a53); + a49=(a49+a47); + a857=(a857*a49); + a19=(a19+a857); + a44=cos(a44); + a43=(a43*a53); + a51=(a51*a60); + a43=(a43-a51); + a44=(a44*a43); + a19=(a19+a44); + if (res[2]!=0) res[2][8]=a19; + a19=arg[3]? arg[3][51] : 0; + a44=(a63*a19); + a43=arg[3]? arg[3][12] : 0; + a44=(a44+a43); + a44=(a13-a44); + a51=(a64*a840); + a44=(a44+a51); + a51=(a65*a840); + a51=(a0*a51); + a44=(a44+a51); + if (res[2]!=0) res[2][9]=a44; + a44=(a68*a19); + a51=arg[3]? arg[3][13] : 0; + a44=(a44-a51); + a44=(a44+a21); + a60=(a69*a840); + a44=(a44+a60); + a60=(a70*a840); + a60=(a0*a60); + a44=(a44+a60); + if (res[2]!=0) res[2][10]=a44; + a44=sin(a564); + a60=casadi_sq(a587); + a53=casadi_sq(a600); + a60=(a60+a53); + a53=(a587/a60); + a857=arg[3]? arg[3][14] : 0; + a53=(a53*a857); + a49=(a599*a53); + a60=(a600/a60); + a60=(a60*a857); + a47=(a597*a60); + a49=(a49+a47); + a44=(a44*a49); + a49=cos(a564); + a47=(a593*a60); + a842=(a601*a53); + a47=(a47-a842); + a49=(a49*a47); + a44=(a44+a49); + a49=cos(a74); + a47=(a517*a51); + a842=(a96*a47); + a49=(a49*a842); + a49=(a44-a49); + a842=sin(a591); + a550=(a517*a43); + a58=(a96*a550); + a842=(a842*a58); + a58=cos(a591); + a56=(a96*a47); + a58=(a58*a56); + a842=(a842-a58); + a49=(a49+a842); + a58=sin(a588); + a56=(a452*a550); + a558=(a96*a56); + a58=(a58*a558); + a558=cos(a588); + a843=(a452*a47); + a859=(a96*a843); + a558=(a558*a859); + a58=(a58-a558); + a49=(a49+a58); + a558=sin(a585); + a859=(a452*a550); + a860=(a96*a859); + a558=(a558*a860); + a860=cos(a585); + a861=(a452*a47); + a862=(a96*a861); + a860=(a860*a862); + a558=(a558-a860); + a49=(a49+a558); + a860=sin(a74); + a862=(a96*a550); + a860=(a860*a862); + a49=(a49+a860); + a860=cos(a74); + a862=(a570*a23); + a860=(a860*a862); + a49=(a49-a860); + a860=sin(a74); + a23=(a578*a23); + a860=(a860*a23); + a49=(a49+a860); + a860=cos(a74); + a578=(a578*a30); + a860=(a860*a578); + a49=(a49-a860); + a860=sin(a74); + a570=(a570*a30); + a860=(a860*a570); + a49=(a49-a860); + a860=sin(a74); + a570=casadi_sq(a85); + a30=casadi_sq(a89); + a570=(a570+a30); + a30=(a85/a570); + a578=(a82*a840); + a30=(a30*a578); + a88=(a88*a30); + a570=(a89/a570); + a570=(a570*a578); + a86=(a86*a570); + a88=(a88+a86); + a860=(a860*a88); + a49=(a49+a860); + a860=cos(a74); + a83=(a83*a570); + a90=(a90*a30); + a83=(a83-a90); + a860=(a860*a83); + a49=(a49+a860); + a860=sin(a74); + a83=casadi_sq(a76); + a90=casadi_sq(a80); + a83=(a83+a90); + a90=(a76/a83); + a30=(a91*a840); + a90=(a90*a30); + a79=(a79*a90); + a83=(a80/a83); + a83=(a83*a30); + a77=(a77*a83); + a79=(a79+a77); + a860=(a860*a79); + a49=(a49+a860); + a74=cos(a74); + a73=(a73*a83); + a81=(a81*a90); + a73=(a73-a81); + a74=(a74*a73); + a49=(a49+a74); + if (res[2]!=0) res[2][11]=a49; + a49=arg[3]? arg[3][52] : 0; + a74=(a93*a49); + a73=arg[3]? arg[3][15] : 0; + a74=(a74+a73); + a74=(a43-a74); + a81=(a94*a840); + a74=(a74+a81); + a81=(a95*a840); + a81=(a0*a81); + a74=(a74+a81); + if (res[2]!=0) res[2][12]=a74; + a74=(a98*a49); + a81=arg[3]? arg[3][16] : 0; + a74=(a74-a81); + a74=(a74+a51); + a90=(a99*a840); + a74=(a74+a90); + a90=(a100*a840); + a90=(a0*a90); + a74=(a74+a90); + if (res[2]!=0) res[2][13]=a74; + a74=sin(a584); + a90=casadi_sq(a607); + a83=casadi_sq(a620); + a90=(a90+a83); + a83=(a607/a90); + a860=arg[3]? arg[3][17] : 0; + a83=(a83*a860); + a79=(a619*a83); + a90=(a620/a90); + a90=(a90*a860); + a77=(a617*a90); + a79=(a79+a77); + a74=(a74*a79); + a79=cos(a584); + a77=(a613*a90); + a30=(a621*a83); + a77=(a77-a30); + a79=(a79*a77); + a74=(a74+a79); + a79=cos(a104); + a77=(a517*a81); + a30=(a126*a77); + a79=(a79*a30); + a79=(a74-a79); + a30=sin(a611); + a570=(a517*a73); + a88=(a126*a570); + a30=(a30*a88); + a88=cos(a611); + a86=(a126*a77); + a88=(a88*a86); + a30=(a30-a88); + a79=(a79+a30); + a88=sin(a608); + a86=(a452*a570); + a578=(a126*a86); + a88=(a88*a578); + a578=cos(a608); + a23=(a452*a77); + a862=(a126*a23); + a578=(a578*a862); + a88=(a88-a578); + a79=(a79+a88); + a578=sin(a605); + a862=(a452*a570); + a863=(a126*a862); + a578=(a578*a863); + a863=cos(a605); + a864=(a452*a77); + a865=(a126*a864); + a863=(a863*a865); + a578=(a578-a863); + a79=(a79+a578); + a863=sin(a104); + a865=(a126*a570); + a863=(a863*a865); + a79=(a79+a863); + a863=cos(a104); + a865=(a590*a53); + a863=(a863*a865); + a79=(a79-a863); + a863=sin(a104); + a53=(a598*a53); + a863=(a863*a53); + a79=(a79+a863); + a863=cos(a104); + a598=(a598*a60); + a863=(a863*a598); + a79=(a79-a863); + a863=sin(a104); + a590=(a590*a60); + a863=(a863*a590); + a79=(a79-a863); + a863=sin(a104); + a590=casadi_sq(a115); + a60=casadi_sq(a119); + a590=(a590+a60); + a60=(a115/a590); + a598=(a112*a840); + a60=(a60*a598); + a118=(a118*a60); + a590=(a119/a590); + a590=(a590*a598); + a116=(a116*a590); + a118=(a118+a116); + a863=(a863*a118); + a79=(a79+a863); + a863=cos(a104); + a113=(a113*a590); + a120=(a120*a60); + a113=(a113-a120); + a863=(a863*a113); + a79=(a79+a863); + a863=sin(a104); + a113=casadi_sq(a106); + a120=casadi_sq(a110); + a113=(a113+a120); + a120=(a106/a113); + a60=(a121*a840); + a120=(a120*a60); + a109=(a109*a120); + a113=(a110/a113); + a113=(a113*a60); + a107=(a107*a113); + a109=(a109+a107); + a863=(a863*a109); + a79=(a79+a863); + a104=cos(a104); + a103=(a103*a113); + a111=(a111*a120); + a103=(a103-a111); + a104=(a104*a103); + a79=(a79+a104); + if (res[2]!=0) res[2][14]=a79; + a79=arg[3]? arg[3][53] : 0; + a104=(a123*a79); + a103=arg[3]? arg[3][18] : 0; + a104=(a104+a103); + a104=(a73-a104); + a111=(a124*a840); + a104=(a104+a111); + a111=(a125*a840); + a111=(a0*a111); + a104=(a104+a111); + if (res[2]!=0) res[2][15]=a104; + a104=(a128*a79); + a111=arg[3]? arg[3][19] : 0; + a104=(a104-a111); + a104=(a104+a81); + a120=(a129*a840); + a104=(a104+a120); + a120=(a130*a840); + a120=(a0*a120); + a104=(a104+a120); + if (res[2]!=0) res[2][16]=a104; + a104=sin(a604); + a120=casadi_sq(a627); + a113=casadi_sq(a640); + a120=(a120+a113); + a113=(a627/a120); + a863=arg[3]? arg[3][20] : 0; + a113=(a113*a863); + a109=(a639*a113); + a120=(a640/a120); + a120=(a120*a863); + a107=(a637*a120); + a109=(a109+a107); + a104=(a104*a109); + a109=cos(a604); + a107=(a633*a120); + a60=(a641*a113); + a107=(a107-a60); + a109=(a109*a107); + a104=(a104+a109); + a109=cos(a134); + a107=(a517*a111); + a60=(a156*a107); + a109=(a109*a60); + a109=(a104-a109); + a60=sin(a631); + a590=(a517*a103); + a118=(a156*a590); + a60=(a60*a118); + a118=cos(a631); + a116=(a156*a107); + a118=(a118*a116); + a60=(a60-a118); + a109=(a109+a60); + a118=sin(a628); + a116=(a452*a590); + a598=(a156*a116); + a118=(a118*a598); + a598=cos(a628); + a53=(a452*a107); + a865=(a156*a53); + a598=(a598*a865); + a118=(a118-a598); + a109=(a109+a118); + a598=sin(a625); + a865=(a452*a590); + a866=(a156*a865); + a598=(a598*a866); + a866=cos(a625); + a867=(a452*a107); + a868=(a156*a867); + a866=(a866*a868); + a598=(a598-a866); + a109=(a109+a598); + a866=sin(a134); + a868=(a156*a590); + a866=(a866*a868); + a109=(a109+a866); + a866=cos(a134); + a868=(a610*a83); + a866=(a866*a868); + a109=(a109-a866); + a866=sin(a134); + a83=(a618*a83); + a866=(a866*a83); + a109=(a109+a866); + a866=cos(a134); + a618=(a618*a90); + a866=(a866*a618); + a109=(a109-a866); + a866=sin(a134); + a610=(a610*a90); + a866=(a866*a610); + a109=(a109-a866); + a866=sin(a134); + a610=casadi_sq(a145); + a90=casadi_sq(a149); + a610=(a610+a90); + a90=(a145/a610); + a618=(a142*a840); + a90=(a90*a618); + a148=(a148*a90); + a610=(a149/a610); + a610=(a610*a618); + a146=(a146*a610); + a148=(a148+a146); + a866=(a866*a148); + a109=(a109+a866); + a866=cos(a134); + a143=(a143*a610); + a150=(a150*a90); + a143=(a143-a150); + a866=(a866*a143); + a109=(a109+a866); + a866=sin(a134); + a143=casadi_sq(a136); + a150=casadi_sq(a140); + a143=(a143+a150); + a150=(a136/a143); + a90=(a151*a840); + a150=(a150*a90); + a139=(a139*a150); + a143=(a140/a143); + a143=(a143*a90); + a137=(a137*a143); + a139=(a139+a137); + a866=(a866*a139); + a109=(a109+a866); + a134=cos(a134); + a133=(a133*a143); + a141=(a141*a150); + a133=(a133-a141); + a134=(a134*a133); + a109=(a109+a134); + if (res[2]!=0) res[2][17]=a109; + a109=arg[3]? arg[3][54] : 0; + a134=(a153*a109); + a133=arg[3]? arg[3][21] : 0; + a134=(a134+a133); + a134=(a103-a134); + a141=(a154*a840); + a134=(a134+a141); + a141=(a155*a840); + a141=(a0*a141); + a134=(a134+a141); + if (res[2]!=0) res[2][18]=a134; + a134=(a158*a109); + a141=arg[3]? arg[3][22] : 0; + a134=(a134-a141); + a134=(a134+a111); + a150=(a159*a840); + a134=(a134+a150); + a150=(a160*a840); + a150=(a0*a150); + a134=(a134+a150); + if (res[2]!=0) res[2][19]=a134; + a134=sin(a624); + a150=casadi_sq(a647); + a143=casadi_sq(a660); + a150=(a150+a143); + a143=(a647/a150); + a866=arg[3]? arg[3][23] : 0; + a143=(a143*a866); + a139=(a659*a143); + a150=(a660/a150); + a150=(a150*a866); + a137=(a657*a150); + a139=(a139+a137); + a134=(a134*a139); + a139=cos(a624); + a137=(a653*a150); + a90=(a661*a143); + a137=(a137-a90); + a139=(a139*a137); + a134=(a134+a139); + a139=cos(a164); + a137=(a517*a141); + a90=(a186*a137); + a139=(a139*a90); + a139=(a134-a139); + a90=sin(a651); + a610=(a517*a133); + a148=(a186*a610); + a90=(a90*a148); + a148=cos(a651); + a146=(a186*a137); + a148=(a148*a146); + a90=(a90-a148); + a139=(a139+a90); + a148=sin(a648); + a146=(a452*a610); + a618=(a186*a146); + a148=(a148*a618); + a618=cos(a648); + a83=(a452*a137); + a868=(a186*a83); + a618=(a618*a868); + a148=(a148-a618); + a139=(a139+a148); + a618=sin(a645); + a868=(a452*a610); + a869=(a186*a868); + a618=(a618*a869); + a869=cos(a645); + a870=(a452*a137); + a871=(a186*a870); + a869=(a869*a871); + a618=(a618-a869); + a139=(a139+a618); + a869=sin(a164); + a871=(a186*a610); + a869=(a869*a871); + a139=(a139+a869); + a869=cos(a164); + a871=(a630*a113); + a869=(a869*a871); + a139=(a139-a869); + a869=sin(a164); + a113=(a638*a113); + a869=(a869*a113); + a139=(a139+a869); + a869=cos(a164); + a638=(a638*a120); + a869=(a869*a638); + a139=(a139-a869); + a869=sin(a164); + a630=(a630*a120); + a869=(a869*a630); + a139=(a139-a869); + a869=sin(a164); + a630=casadi_sq(a175); + a120=casadi_sq(a179); + a630=(a630+a120); + a120=(a175/a630); + a638=(a172*a840); + a120=(a120*a638); + a178=(a178*a120); + a630=(a179/a630); + a630=(a630*a638); + a176=(a176*a630); + a178=(a178+a176); + a869=(a869*a178); + a139=(a139+a869); + a869=cos(a164); + a173=(a173*a630); + a180=(a180*a120); + a173=(a173-a180); + a869=(a869*a173); + a139=(a139+a869); + a869=sin(a164); + a173=casadi_sq(a166); + a180=casadi_sq(a170); + a173=(a173+a180); + a180=(a166/a173); + a120=(a181*a840); + a180=(a180*a120); + a169=(a169*a180); + a173=(a170/a173); + a173=(a173*a120); + a167=(a167*a173); + a169=(a169+a167); + a869=(a869*a169); + a139=(a139+a869); + a164=cos(a164); + a163=(a163*a173); + a171=(a171*a180); + a163=(a163-a171); + a164=(a164*a163); + a139=(a139+a164); + if (res[2]!=0) res[2][20]=a139; + a139=arg[3]? arg[3][55] : 0; + a164=(a183*a139); + a163=arg[3]? arg[3][24] : 0; + a164=(a164+a163); + a164=(a133-a164); + a171=(a184*a840); + a164=(a164+a171); + a171=(a185*a840); + a171=(a0*a171); + a164=(a164+a171); + if (res[2]!=0) res[2][21]=a164; + a164=(a188*a139); + a171=arg[3]? arg[3][25] : 0; + a164=(a164-a171); + a164=(a164+a141); + a180=(a189*a840); + a164=(a164+a180); + a180=(a190*a840); + a180=(a0*a180); + a164=(a164+a180); + if (res[2]!=0) res[2][22]=a164; + a164=sin(a644); + a180=casadi_sq(a667); + a173=casadi_sq(a680); + a180=(a180+a173); + a173=(a667/a180); + a869=arg[3]? arg[3][26] : 0; + a173=(a173*a869); + a169=(a679*a173); + a180=(a680/a180); + a180=(a180*a869); + a167=(a677*a180); + a169=(a169+a167); + a164=(a164*a169); + a169=cos(a644); + a167=(a673*a180); + a120=(a681*a173); + a167=(a167-a120); + a169=(a169*a167); + a164=(a164+a169); + a169=cos(a194); + a167=(a517*a171); + a120=(a216*a167); + a169=(a169*a120); + a169=(a164-a169); + a120=sin(a671); + a630=(a517*a163); + a178=(a216*a630); + a120=(a120*a178); + a178=cos(a671); + a176=(a216*a167); + a178=(a178*a176); + a120=(a120-a178); + a169=(a169+a120); + a178=sin(a668); + a176=(a452*a630); + a638=(a216*a176); + a178=(a178*a638); + a638=cos(a668); + a113=(a452*a167); + a871=(a216*a113); + a638=(a638*a871); + a178=(a178-a638); + a169=(a169+a178); + a638=sin(a665); + a871=(a452*a630); + a872=(a216*a871); + a638=(a638*a872); + a872=cos(a665); + a873=(a452*a167); + a874=(a216*a873); + a872=(a872*a874); + a638=(a638-a872); + a169=(a169+a638); + a872=sin(a194); + a874=(a216*a630); + a872=(a872*a874); + a169=(a169+a872); + a872=cos(a194); + a874=(a650*a143); + a872=(a872*a874); + a169=(a169-a872); + a872=sin(a194); + a143=(a658*a143); + a872=(a872*a143); + a169=(a169+a872); + a872=cos(a194); + a658=(a658*a150); + a872=(a872*a658); + a169=(a169-a872); + a872=sin(a194); + a650=(a650*a150); + a872=(a872*a650); + a169=(a169-a872); + a872=sin(a194); + a650=casadi_sq(a205); + a150=casadi_sq(a209); + a650=(a650+a150); + a150=(a205/a650); + a658=(a202*a840); + a150=(a150*a658); + a208=(a208*a150); + a650=(a209/a650); + a650=(a650*a658); + a206=(a206*a650); + a208=(a208+a206); + a872=(a872*a208); + a169=(a169+a872); + a872=cos(a194); + a203=(a203*a650); + a210=(a210*a150); + a203=(a203-a210); + a872=(a872*a203); + a169=(a169+a872); + a872=sin(a194); + a203=casadi_sq(a196); + a210=casadi_sq(a200); + a203=(a203+a210); + a210=(a196/a203); + a150=(a211*a840); + a210=(a210*a150); + a199=(a199*a210); + a203=(a200/a203); + a203=(a203*a150); + a197=(a197*a203); + a199=(a199+a197); + a872=(a872*a199); + a169=(a169+a872); + a194=cos(a194); + a193=(a193*a203); + a201=(a201*a210); + a193=(a193-a201); + a194=(a194*a193); + a169=(a169+a194); + if (res[2]!=0) res[2][23]=a169; + a169=arg[3]? arg[3][56] : 0; + a194=(a213*a169); + a193=arg[3]? arg[3][27] : 0; + a194=(a194+a193); + a194=(a163-a194); + a201=(a214*a840); + a194=(a194+a201); + a201=(a215*a840); + a201=(a0*a201); + a194=(a194+a201); + if (res[2]!=0) res[2][24]=a194; + a194=(a218*a169); + a201=arg[3]? arg[3][28] : 0; + a194=(a194-a201); + a194=(a194+a171); + a210=(a219*a840); + a194=(a194+a210); + a210=(a220*a840); + a210=(a0*a210); + a194=(a194+a210); + if (res[2]!=0) res[2][25]=a194; + a194=sin(a664); + a210=casadi_sq(a687); + a203=casadi_sq(a700); + a210=(a210+a203); + a203=(a687/a210); + a872=arg[3]? arg[3][29] : 0; + a203=(a203*a872); + a199=(a699*a203); + a210=(a700/a210); + a210=(a210*a872); + a197=(a697*a210); + a199=(a199+a197); + a194=(a194*a199); + a199=cos(a664); + a197=(a693*a210); + a150=(a701*a203); + a197=(a197-a150); + a199=(a199*a197); + a194=(a194+a199); + a199=cos(a224); + a197=(a517*a201); + a150=(a246*a197); + a199=(a199*a150); + a199=(a194-a199); + a150=sin(a691); + a650=(a517*a193); + a208=(a246*a650); + a150=(a150*a208); + a208=cos(a691); + a206=(a246*a197); + a208=(a208*a206); + a150=(a150-a208); + a199=(a199+a150); + a208=sin(a688); + a206=(a452*a650); + a658=(a246*a206); + a208=(a208*a658); + a658=cos(a688); + a143=(a452*a197); + a874=(a246*a143); + a658=(a658*a874); + a208=(a208-a658); + a199=(a199+a208); + a658=sin(a685); + a874=(a452*a650); + a875=(a246*a874); + a658=(a658*a875); + a875=cos(a685); + a876=(a452*a197); + a877=(a246*a876); + a875=(a875*a877); + a658=(a658-a875); + a199=(a199+a658); + a875=sin(a224); + a877=(a246*a650); + a875=(a875*a877); + a199=(a199+a875); + a875=cos(a224); + a877=(a670*a173); + a875=(a875*a877); + a199=(a199-a875); + a875=sin(a224); + a173=(a678*a173); + a875=(a875*a173); + a199=(a199+a875); + a875=cos(a224); + a678=(a678*a180); + a875=(a875*a678); + a199=(a199-a875); + a875=sin(a224); + a670=(a670*a180); + a875=(a875*a670); + a199=(a199-a875); + a875=sin(a224); + a670=casadi_sq(a235); + a180=casadi_sq(a239); + a670=(a670+a180); + a180=(a235/a670); + a678=(a232*a840); + a180=(a180*a678); + a238=(a238*a180); + a670=(a239/a670); + a670=(a670*a678); + a236=(a236*a670); + a238=(a238+a236); + a875=(a875*a238); + a199=(a199+a875); + a875=cos(a224); + a233=(a233*a670); + a240=(a240*a180); + a233=(a233-a240); + a875=(a875*a233); + a199=(a199+a875); + a875=sin(a224); + a233=casadi_sq(a226); + a240=casadi_sq(a230); + a233=(a233+a240); + a240=(a226/a233); + a180=(a241*a840); + a240=(a240*a180); + a229=(a229*a240); + a233=(a230/a233); + a233=(a233*a180); + a227=(a227*a233); + a229=(a229+a227); + a875=(a875*a229); + a199=(a199+a875); + a224=cos(a224); + a223=(a223*a233); + a231=(a231*a240); + a223=(a223-a231); + a224=(a224*a223); + a199=(a199+a224); + if (res[2]!=0) res[2][26]=a199; + a199=arg[3]? arg[3][57] : 0; + a224=(a243*a199); + a223=arg[3]? arg[3][30] : 0; + a224=(a224+a223); + a224=(a193-a224); + a231=(a244*a840); + a224=(a224+a231); + a231=(a245*a840); + a231=(a0*a231); + a224=(a224+a231); + if (res[2]!=0) res[2][27]=a224; + a224=(a248*a199); + a231=arg[3]? arg[3][31] : 0; + a224=(a224-a231); + a224=(a224+a201); + a240=(a249*a840); + a224=(a224+a240); + a240=(a250*a840); + a240=(a0*a240); + a224=(a224+a240); + if (res[2]!=0) res[2][28]=a224; + a224=sin(a684); + a240=casadi_sq(a707); + a233=casadi_sq(a720); + a240=(a240+a233); + a233=(a707/a240); + a875=arg[3]? arg[3][32] : 0; + a233=(a233*a875); + a229=(a719*a233); + a240=(a720/a240); + a240=(a240*a875); + a227=(a717*a240); + a229=(a229+a227); + a224=(a224*a229); + a229=cos(a684); + a227=(a713*a240); + a180=(a721*a233); + a227=(a227-a180); + a229=(a229*a227); + a224=(a224+a229); + a229=cos(a254); + a227=(a517*a231); + a180=(a276*a227); + a229=(a229*a180); + a229=(a224-a229); + a180=sin(a711); + a670=(a517*a223); + a238=(a276*a670); + a180=(a180*a238); + a238=cos(a711); + a236=(a276*a227); + a238=(a238*a236); + a180=(a180-a238); + a229=(a229+a180); + a238=sin(a708); + a236=(a452*a670); + a678=(a276*a236); + a238=(a238*a678); + a678=cos(a708); + a173=(a452*a227); + a877=(a276*a173); + a678=(a678*a877); + a238=(a238-a678); + a229=(a229+a238); + a678=sin(a705); + a877=(a452*a670); + a878=(a276*a877); + a678=(a678*a878); + a878=cos(a705); + a879=(a452*a227); + a880=(a276*a879); + a878=(a878*a880); + a678=(a678-a878); + a229=(a229+a678); + a878=sin(a254); + a880=(a276*a670); + a878=(a878*a880); + a229=(a229+a878); + a878=cos(a254); + a880=(a690*a203); + a878=(a878*a880); + a229=(a229-a878); + a878=sin(a254); + a203=(a698*a203); + a878=(a878*a203); + a229=(a229+a878); + a878=cos(a254); + a698=(a698*a210); + a878=(a878*a698); + a229=(a229-a878); + a878=sin(a254); + a690=(a690*a210); + a878=(a878*a690); + a229=(a229-a878); + a878=sin(a254); + a690=casadi_sq(a265); + a210=casadi_sq(a269); + a690=(a690+a210); + a210=(a265/a690); + a698=(a262*a840); + a210=(a210*a698); + a268=(a268*a210); + a690=(a269/a690); + a690=(a690*a698); + a266=(a266*a690); + a268=(a268+a266); + a878=(a878*a268); + a229=(a229+a878); + a878=cos(a254); + a263=(a263*a690); + a270=(a270*a210); + a263=(a263-a270); + a878=(a878*a263); + a229=(a229+a878); + a878=sin(a254); + a263=casadi_sq(a256); + a270=casadi_sq(a260); + a263=(a263+a270); + a270=(a256/a263); + a210=(a271*a840); + a270=(a270*a210); + a259=(a259*a270); + a263=(a260/a263); + a263=(a263*a210); + a257=(a257*a263); + a259=(a259+a257); + a878=(a878*a259); + a229=(a229+a878); + a254=cos(a254); + a253=(a253*a263); + a261=(a261*a270); + a253=(a253-a261); + a254=(a254*a253); + a229=(a229+a254); + if (res[2]!=0) res[2][29]=a229; + a229=arg[3]? arg[3][58] : 0; + a254=(a273*a229); + a253=arg[3]? arg[3][33] : 0; + a254=(a254+a253); + a254=(a223-a254); + a261=(a274*a840); + a254=(a254+a261); + a261=(a275*a840); + a261=(a0*a261); + a254=(a254+a261); + if (res[2]!=0) res[2][30]=a254; + a254=(a278*a229); + a261=arg[3]? arg[3][34] : 0; + a254=(a254-a261); + a254=(a254+a231); + a270=(a279*a840); + a254=(a254+a270); + a270=(a280*a840); + a270=(a0*a270); + a254=(a254+a270); + if (res[2]!=0) res[2][31]=a254; + a254=sin(a704); + a270=casadi_sq(a727); + a263=casadi_sq(a740); + a270=(a270+a263); + a263=(a727/a270); + a878=arg[3]? arg[3][35] : 0; + a263=(a263*a878); + a259=(a739*a263); + a270=(a740/a270); + a270=(a270*a878); + a257=(a737*a270); + a259=(a259+a257); + a254=(a254*a259); + a259=cos(a704); + a257=(a733*a270); + a210=(a741*a263); + a257=(a257-a210); + a259=(a259*a257); + a254=(a254+a259); + a259=cos(a284); + a257=(a517*a261); + a210=(a306*a257); + a259=(a259*a210); + a259=(a254-a259); + a210=sin(a731); + a690=(a517*a253); + a268=(a306*a690); + a210=(a210*a268); + a268=cos(a731); + a266=(a306*a257); + a268=(a268*a266); + a210=(a210-a268); + a259=(a259+a210); + a268=sin(a728); + a266=(a452*a690); + a698=(a306*a266); + a268=(a268*a698); + a698=cos(a728); + a203=(a452*a257); + a880=(a306*a203); + a698=(a698*a880); + a268=(a268-a698); + a259=(a259+a268); + a698=sin(a725); + a880=(a452*a690); + a881=(a306*a880); + a698=(a698*a881); + a881=cos(a725); + a882=(a452*a257); + a883=(a306*a882); + a881=(a881*a883); + a698=(a698-a881); + a259=(a259+a698); + a881=sin(a284); + a883=(a306*a690); + a881=(a881*a883); + a259=(a259+a881); + a881=cos(a284); + a883=(a710*a233); + a881=(a881*a883); + a259=(a259-a881); + a881=sin(a284); + a233=(a718*a233); + a881=(a881*a233); + a259=(a259+a881); + a881=cos(a284); + a718=(a718*a240); + a881=(a881*a718); + a259=(a259-a881); + a881=sin(a284); + a710=(a710*a240); + a881=(a881*a710); + a259=(a259-a881); + a881=sin(a284); + a710=casadi_sq(a295); + a240=casadi_sq(a299); + a710=(a710+a240); + a240=(a295/a710); + a718=(a292*a840); + a240=(a240*a718); + a298=(a298*a240); + a710=(a299/a710); + a710=(a710*a718); + a296=(a296*a710); + a298=(a298+a296); + a881=(a881*a298); + a259=(a259+a881); + a881=cos(a284); + a293=(a293*a710); + a300=(a300*a240); + a293=(a293-a300); + a881=(a881*a293); + a259=(a259+a881); + a881=sin(a284); + a293=casadi_sq(a286); + a300=casadi_sq(a290); + a293=(a293+a300); + a300=(a286/a293); + a240=(a301*a840); + a300=(a300*a240); + a289=(a289*a300); + a293=(a290/a293); + a293=(a293*a240); + a287=(a287*a293); + a289=(a289+a287); + a881=(a881*a289); + a259=(a259+a881); + a284=cos(a284); + a283=(a283*a293); + a291=(a291*a300); + a283=(a283-a291); + a284=(a284*a283); + a259=(a259+a284); + if (res[2]!=0) res[2][32]=a259; + a259=arg[3]? arg[3][59] : 0; + a284=(a303*a259); + a283=arg[3]? arg[3][36] : 0; + a284=(a284+a283); + a284=(a253-a284); + a291=(a304*a840); + a284=(a284+a291); + a291=(a305*a840); + a291=(a0*a291); + a284=(a284+a291); + if (res[2]!=0) res[2][33]=a284; + a284=(a308*a259); + a291=arg[3]? arg[3][37] : 0; + a284=(a284-a291); + a284=(a284+a261); + a300=(a309*a840); + a284=(a284+a300); + a300=(a310*a840); + a300=(a0*a300); + a284=(a284+a300); + if (res[2]!=0) res[2][34]=a284; + a284=sin(a724); + a300=casadi_sq(a747); + a293=casadi_sq(a760); + a300=(a300+a293); + a293=(a747/a300); + a881=arg[3]? arg[3][38] : 0; + a293=(a293*a881); + a289=(a759*a293); + a300=(a760/a300); + a300=(a300*a881); + a287=(a757*a300); + a289=(a289+a287); + a284=(a284*a289); + a289=cos(a724); + a287=(a753*a300); + a240=(a761*a293); + a287=(a287-a240); + a289=(a289*a287); + a284=(a284+a289); + a289=cos(a314); + a287=(a517*a291); + a240=(a336*a287); + a289=(a289*a240); + a289=(a284-a289); + a240=sin(a751); + a710=(a517*a283); + a298=(a336*a710); + a240=(a240*a298); + a298=cos(a751); + a296=(a336*a287); + a298=(a298*a296); + a240=(a240-a298); + a289=(a289+a240); + a298=sin(a748); + a296=(a452*a710); + a718=(a336*a296); + a298=(a298*a718); + a718=cos(a748); + a233=(a452*a287); + a883=(a336*a233); + a718=(a718*a883); + a298=(a298-a718); + a289=(a289+a298); + a718=sin(a745); + a883=(a452*a710); + a884=(a336*a883); + a718=(a718*a884); + a884=cos(a745); + a885=(a452*a287); + a886=(a336*a885); + a884=(a884*a886); + a718=(a718-a884); + a289=(a289+a718); + a884=sin(a314); + a886=(a336*a710); + a884=(a884*a886); + a289=(a289+a884); + a884=cos(a314); + a886=(a730*a263); + a884=(a884*a886); + a289=(a289-a884); + a884=sin(a314); + a263=(a738*a263); + a884=(a884*a263); + a289=(a289+a884); + a884=cos(a314); + a738=(a738*a270); + a884=(a884*a738); + a289=(a289-a884); + a884=sin(a314); + a730=(a730*a270); + a884=(a884*a730); + a289=(a289-a884); + a884=sin(a314); + a730=casadi_sq(a325); + a270=casadi_sq(a329); + a730=(a730+a270); + a270=(a325/a730); + a738=(a322*a840); + a270=(a270*a738); + a328=(a328*a270); + a730=(a329/a730); + a730=(a730*a738); + a326=(a326*a730); + a328=(a328+a326); + a884=(a884*a328); + a289=(a289+a884); + a884=cos(a314); + a323=(a323*a730); + a330=(a330*a270); + a323=(a323-a330); + a884=(a884*a323); + a289=(a289+a884); + a884=sin(a314); + a323=casadi_sq(a316); + a330=casadi_sq(a320); + a323=(a323+a330); + a330=(a316/a323); + a270=(a331*a840); + a330=(a330*a270); + a319=(a319*a330); + a323=(a320/a323); + a323=(a323*a270); + a317=(a317*a323); + a319=(a319+a317); + a884=(a884*a319); + a289=(a289+a884); + a314=cos(a314); + a313=(a313*a323); + a321=(a321*a330); + a313=(a313-a321); + a314=(a314*a313); + a289=(a289+a314); + if (res[2]!=0) res[2][35]=a289; + a289=arg[3]? arg[3][60] : 0; + a314=(a333*a289); + a313=arg[3]? arg[3][39] : 0; + a314=(a314+a313); + a314=(a283-a314); + a321=(a334*a840); + a314=(a314+a321); + a321=(a335*a840); + a321=(a0*a321); + a314=(a314+a321); + if (res[2]!=0) res[2][36]=a314; + a314=(a338*a289); + a321=arg[3]? arg[3][40] : 0; + a314=(a314-a321); + a314=(a314+a291); + a330=(a339*a840); + a314=(a314+a330); + a330=(a340*a840); + a330=(a0*a330); + a314=(a314+a330); + if (res[2]!=0) res[2][37]=a314; + a314=sin(a744); + a330=casadi_sq(a767); + a323=casadi_sq(a780); + a330=(a330+a323); + a323=(a767/a330); + a884=arg[3]? arg[3][41] : 0; + a323=(a323*a884); + a319=(a779*a323); + a330=(a780/a330); + a330=(a330*a884); + a317=(a777*a330); + a319=(a319+a317); + a314=(a314*a319); + a319=cos(a744); + a317=(a773*a330); + a270=(a781*a323); + a317=(a317-a270); + a319=(a319*a317); + a314=(a314+a319); + a319=cos(a344); + a317=(a517*a321); + a270=(a366*a317); + a319=(a319*a270); + a319=(a314-a319); + a270=sin(a771); + a730=(a517*a313); + a328=(a366*a730); + a270=(a270*a328); + a328=cos(a771); + a326=(a366*a317); + a328=(a328*a326); + a270=(a270-a328); + a319=(a319+a270); + a328=sin(a768); + a326=(a452*a730); + a738=(a366*a326); + a328=(a328*a738); + a738=cos(a768); + a263=(a452*a317); + a886=(a366*a263); + a738=(a738*a886); + a328=(a328-a738); + a319=(a319+a328); + a738=sin(a765); + a886=(a452*a730); + a887=(a366*a886); + a738=(a738*a887); + a887=cos(a765); + a888=(a452*a317); + a889=(a366*a888); + a887=(a887*a889); + a738=(a738-a887); + a319=(a319+a738); + a887=sin(a344); + a889=(a366*a730); + a887=(a887*a889); + a319=(a319+a887); + a887=cos(a344); + a889=(a750*a293); + a887=(a887*a889); + a319=(a319-a887); + a887=sin(a344); + a293=(a758*a293); + a887=(a887*a293); + a319=(a319+a887); + a887=cos(a344); + a758=(a758*a300); + a887=(a887*a758); + a319=(a319-a887); + a887=sin(a344); + a750=(a750*a300); + a887=(a887*a750); + a319=(a319-a887); + a887=sin(a344); + a750=casadi_sq(a355); + a300=casadi_sq(a359); + a750=(a750+a300); + a300=(a355/a750); + a758=(a352*a840); + a300=(a300*a758); + a358=(a358*a300); + a750=(a359/a750); + a750=(a750*a758); + a356=(a356*a750); + a358=(a358+a356); + a887=(a887*a358); + a319=(a319+a887); + a887=cos(a344); + a353=(a353*a750); + a360=(a360*a300); + a353=(a353-a360); + a887=(a887*a353); + a319=(a319+a887); + a887=sin(a344); + a353=casadi_sq(a346); + a360=casadi_sq(a350); + a353=(a353+a360); + a360=(a346/a353); + a300=(a361*a840); + a360=(a360*a300); + a349=(a349*a360); + a353=(a350/a353); + a353=(a353*a300); + a347=(a347*a353); + a349=(a349+a347); + a887=(a887*a349); + a319=(a319+a887); + a344=cos(a344); + a343=(a343*a353); + a351=(a351*a360); + a343=(a343-a351); + a344=(a344*a343); + a319=(a319+a344); + if (res[2]!=0) res[2][38]=a319; + a319=arg[3]? arg[3][61] : 0; + a344=(a363*a319); + a343=arg[3]? arg[3][42] : 0; + a344=(a344+a343); + a344=(a313-a344); + a351=(a364*a840); + a344=(a344+a351); + a351=(a365*a840); + a351=(a0*a351); + a344=(a344+a351); + if (res[2]!=0) res[2][39]=a344; + a344=(a368*a319); + a351=arg[3]? arg[3][43] : 0; + a344=(a344-a351); + a344=(a344+a321); + a360=(a369*a840); + a344=(a344+a360); + a360=(a370*a840); + a360=(a0*a360); + a344=(a344+a360); + if (res[2]!=0) res[2][40]=a344; + a344=sin(a764); + a360=casadi_sq(a787); + a353=casadi_sq(a800); + a360=(a360+a353); + a353=(a787/a360); + a887=arg[3]? arg[3][44] : 0; + a353=(a353*a887); + a349=(a799*a353); + a360=(a800/a360); + a360=(a360*a887); + a347=(a797*a360); + a349=(a349+a347); + a344=(a344*a349); + a349=cos(a764); + a347=(a793*a360); + a300=(a801*a353); + a347=(a347-a300); + a349=(a349*a347); + a344=(a344+a349); + a349=cos(a374); + a347=(a517*a351); + a300=(a396*a347); + a349=(a349*a300); + a349=(a344-a349); + a300=sin(a791); + a750=(a517*a343); + a358=(a396*a750); + a300=(a300*a358); + a358=cos(a791); + a356=(a396*a347); + a358=(a358*a356); + a300=(a300-a358); + a349=(a349+a300); + a358=sin(a788); + a356=(a452*a750); + a758=(a396*a356); + a358=(a358*a758); + a758=cos(a788); + a293=(a452*a347); + a889=(a396*a293); + a758=(a758*a889); + a358=(a358-a758); + a349=(a349+a358); + a758=sin(a785); + a889=(a452*a750); + a890=(a396*a889); + a758=(a758*a890); + a890=cos(a785); + a891=(a452*a347); + a892=(a396*a891); + a890=(a890*a892); + a758=(a758-a890); + a349=(a349+a758); + a890=sin(a374); + a892=(a396*a750); + a890=(a890*a892); + a349=(a349+a890); + a890=cos(a374); + a892=(a770*a323); + a890=(a890*a892); + a349=(a349-a890); + a890=sin(a374); + a323=(a778*a323); + a890=(a890*a323); + a349=(a349+a890); + a890=cos(a374); + a778=(a778*a330); + a890=(a890*a778); + a349=(a349-a890); + a890=sin(a374); + a770=(a770*a330); + a890=(a890*a770); + a349=(a349-a890); + a890=sin(a374); + a770=casadi_sq(a385); + a330=casadi_sq(a389); + a770=(a770+a330); + a330=(a385/a770); + a778=(a382*a840); + a330=(a330*a778); + a388=(a388*a330); + a770=(a389/a770); + a770=(a770*a778); + a386=(a386*a770); + a388=(a388+a386); + a890=(a890*a388); + a349=(a349+a890); + a890=cos(a374); + a383=(a383*a770); + a390=(a390*a330); + a383=(a383-a390); + a890=(a890*a383); + a349=(a349+a890); + a890=sin(a374); + a383=casadi_sq(a376); + a390=casadi_sq(a380); + a383=(a383+a390); + a390=(a376/a383); + a330=(a391*a840); + a390=(a390*a330); + a379=(a379*a390); + a383=(a380/a383); + a383=(a383*a330); + a377=(a377*a383); + a379=(a379+a377); + a890=(a890*a379); + a349=(a349+a890); + a374=cos(a374); + a373=(a373*a383); + a381=(a381*a390); + a373=(a373-a381); + a374=(a374*a373); + a349=(a349+a374); + if (res[2]!=0) res[2][41]=a349; + a349=arg[3]? arg[3][62] : 0; + a374=(a393*a349); + a373=arg[3]? arg[3][45] : 0; + a374=(a374+a373); + a374=(a343-a374); + a381=(a394*a840); + a374=(a374+a381); + a381=(a395*a840); + a381=(a0*a381); + a374=(a374+a381); + if (res[2]!=0) res[2][42]=a374; + a374=(a398*a349); + a381=arg[3]? arg[3][46] : 0; + a374=(a374-a381); + a374=(a374+a351); + a390=(a399*a840); + a374=(a374+a390); + a390=(a400*a840); + a390=(a0*a390); + a374=(a374+a390); + if (res[2]!=0) res[2][43]=a374; + a374=sin(a784); + a390=casadi_sq(a810); + a383=casadi_sq(a817); + a390=(a390+a383); + a383=(a810/a390); + a890=arg[3]? arg[3][47] : 0; + a383=(a383*a890); + a379=(a816*a383); + a390=(a817/a390); + a390=(a390*a890); + a377=(a807*a390); + a379=(a379+a377); + a374=(a374*a379); + a379=cos(a784); + a377=(a427*a390); + a330=(a818*a383); + a377=(a377-a330); + a379=(a379*a377); + a374=(a374+a379); + a379=cos(a404); + a377=(a517*a381); + a330=(a426*a377); + a379=(a379*a330); + a379=(a374-a379); + a330=sin(a811); + a770=(a517*a373); + a388=(a426*a770); + a330=(a330*a388); + a388=cos(a811); + a386=(a426*a377); + a388=(a388*a386); + a330=(a330-a388); + a379=(a379+a330); + a388=sin(a808); + a386=(a452*a770); + a778=(a426*a386); + a388=(a388*a778); + a778=cos(a808); + a323=(a452*a377); + a892=(a426*a323); + a778=(a778*a892); + a388=(a388-a778); + a379=(a379+a388); + a778=sin(a805); + a892=(a452*a770); + a893=(a426*a892); + a778=(a778*a893); + a893=cos(a805); + a894=(a452*a377); + a895=(a426*a894); + a893=(a893*a895); + a778=(a778-a893); + a379=(a379+a778); + a893=sin(a404); + a895=(a426*a770); + a893=(a893*a895); + a379=(a379+a893); + a893=cos(a404); + a895=(a790*a353); + a893=(a893*a895); + a379=(a379-a893); + a893=sin(a404); + a353=(a798*a353); + a893=(a893*a353); + a379=(a379+a893); + a893=cos(a404); + a798=(a798*a360); + a893=(a893*a798); + a379=(a379-a893); + a893=sin(a404); + a790=(a790*a360); + a893=(a893*a790); + a379=(a379-a893); + a893=sin(a404); + a790=casadi_sq(a415); + a360=casadi_sq(a419); + a790=(a790+a360); + a360=(a415/a790); + a798=(a412*a840); + a360=(a360*a798); + a418=(a418*a360); + a790=(a419/a790); + a790=(a790*a798); + a416=(a416*a790); + a418=(a418+a416); + a893=(a893*a418); + a379=(a379+a893); + a893=cos(a404); + a413=(a413*a790); + a420=(a420*a360); + a413=(a413-a420); + a893=(a893*a413); + a379=(a379+a893); + a893=sin(a404); + a413=casadi_sq(a406); + a420=casadi_sq(a410); + a413=(a413+a420); + a420=(a406/a413); + a360=(a421*a840); + a420=(a420*a360); + a409=(a409*a420); + a413=(a410/a413); + a413=(a413*a360); + a407=(a407*a413); + a409=(a409+a407); + a893=(a893*a409); + a379=(a379+a893); + a404=cos(a404); + a403=(a403*a413); + a411=(a411*a420); + a403=(a403-a411); + a404=(a404*a403); + a379=(a379+a404); + if (res[2]!=0) res[2][44]=a379; + a379=(a424*a840); + a379=(a373+a379); + a404=(a425*a840); + a404=(a0*a404); + a379=(a379+a404); + if (res[2]!=0) res[2][45]=a379; + a379=(a429*a840); + a379=(a381+a379); + a404=(a430*a840); + a404=(a0*a404); + a379=(a379+a404); + if (res[2]!=0) res[2][46]=a379; + a379=sin(a434); + a404=(a803*a383); + a379=(a379*a404); + a404=cos(a434); + a383=(a804*a383); + a404=(a404*a383); + a379=(a379-a404); + a404=cos(a434); + a803=(a803*a390); + a404=(a404*a803); + a379=(a379-a404); + a404=sin(a434); + a804=(a804*a390); + a404=(a404*a804); + a379=(a379-a404); + a404=sin(a434); + a804=casadi_sq(a445); + a390=casadi_sq(a449); + a804=(a804+a390); + a390=(a445/a804); + a803=(a442*a840); + a390=(a390*a803); + a448=(a448*a390); + a804=(a449/a804); + a804=(a804*a803); + a446=(a446*a804); + a448=(a448+a446); + a404=(a404*a448); + a379=(a379+a404); + a404=cos(a434); + a443=(a443*a804); + a450=(a450*a390); + a443=(a443-a450); + a404=(a404*a443); + a379=(a379+a404); + a404=sin(a434); + a443=casadi_sq(a436); + a450=casadi_sq(a440); + a443=(a443+a450); + a450=(a436/a443); + a390=(a451*a840); + a450=(a450*a390); + a439=(a439*a450); + a443=(a440/a443); + a443=(a443*a390); + a437=(a437*a443); + a439=(a439+a437); + a404=(a404*a439); + a379=(a379+a404); + a434=cos(a434); + a433=(a433*a443); + a441=(a441*a450); + a433=(a433-a441); + a434=(a434*a433); + a379=(a379+a434); + if (res[2]!=0) res[2][47]=a379; + a379=(a454*a840); + a536=(a536*a829); + a535=(a535*a835); + a536=(a536+a535); + a534=(a534*a838); + a536=(a536+a534); + a519=(a519*a829); + a536=(a536+a519); + a532=(a532*a831); + a536=(a536+a532); + a528=(a528*a833); + a536=(a536+a528); + a525=(a525*a836); + a536=(a536+a525); + a518=(a518*a831); + a536=(a536+a518); + a455=(a455*a840); + a536=(a536+a455); + a460=(a460*a840); + a460=(a452*a460); + a536=(a536+a460); + a379=(a379-a536); + a536=(a453*a840); + a536=(a452*a536); + a379=(a379+a536); + a536=(a10+a10); + a536=(a536*a840); + a379=(a379+a536); + if (res[2]!=0) res[2][48]=a379; + a374=(a517*a374); + a379=(a521*a374); + a536=(a452*a374); + a330=(a529*a330); + a536=(a536+a330); + a330=(a521*a536); + a379=(a379+a330); + a330=(a452*a374); + a388=(a520*a388); + a330=(a330+a388); + a388=(a521*a330); + a379=(a379+a388); + a778=(a520*a778); + a778=(a374+a778); + a388=(a521*a778); + a379=(a379+a388); + a388=(a521*a379); + a344=(a517*a344); + a518=(a521*a344); + a388=(a388+a518); + a518=(a452*a344); + a300=(a529*a300); + a518=(a518+a300); + a300=(a521*a518); + a388=(a388+a300); + a300=(a452*a344); + a358=(a520*a358); + a300=(a300+a358); + a358=(a521*a300); + a388=(a388+a358); + a758=(a520*a758); + a758=(a344+a758); + a358=(a521*a758); + a388=(a388+a358); + a358=(a521*a388); + a314=(a517*a314); + a831=(a521*a314); + a358=(a358+a831); + a831=(a452*a314); + a270=(a529*a270); + a831=(a831+a270); + a270=(a521*a831); + a358=(a358+a270); + a270=(a452*a314); + a328=(a520*a328); + a270=(a270+a328); + a328=(a521*a270); + a358=(a358+a328); + a738=(a520*a738); + a738=(a314+a738); + a328=(a521*a738); + a358=(a358+a328); + a328=(a521*a358); + a284=(a517*a284); + a525=(a521*a284); + a328=(a328+a525); + a525=(a452*a284); + a240=(a529*a240); + a525=(a525+a240); + a240=(a521*a525); + a328=(a328+a240); + a240=(a452*a284); + a298=(a520*a298); + a240=(a240+a298); + a298=(a521*a240); + a328=(a328+a298); + a718=(a520*a718); + a718=(a284+a718); + a298=(a521*a718); + a328=(a328+a298); + a298=(a521*a328); + a254=(a517*a254); + a836=(a521*a254); + a298=(a298+a836); + a836=(a452*a254); + a210=(a529*a210); + a836=(a836+a210); + a210=(a521*a836); + a298=(a298+a210); + a210=(a452*a254); + a268=(a520*a268); + a210=(a210+a268); + a268=(a521*a210); + a298=(a298+a268); + a698=(a520*a698); + a698=(a254+a698); + a268=(a521*a698); + a298=(a298+a268); + a268=(a521*a298); + a224=(a517*a224); + a528=(a521*a224); + a268=(a268+a528); + a528=(a452*a224); + a180=(a529*a180); + a528=(a528+a180); + a180=(a521*a528); + a268=(a268+a180); + a180=(a452*a224); + a238=(a520*a238); + a180=(a180+a238); + a238=(a521*a180); + a268=(a268+a238); + a678=(a520*a678); + a678=(a224+a678); + a238=(a521*a678); + a268=(a268+a238); + a238=(a521*a268); + a194=(a517*a194); + a833=(a521*a194); + a238=(a238+a833); + a833=(a452*a194); + a150=(a529*a150); + a833=(a833+a150); + a150=(a521*a833); + a238=(a238+a150); + a150=(a452*a194); + a208=(a520*a208); + a150=(a150+a208); + a208=(a521*a150); + a238=(a238+a208); + a658=(a520*a658); + a658=(a194+a658); + a208=(a521*a658); + a238=(a238+a208); + a208=(a521*a238); + a164=(a517*a164); + a532=(a521*a164); + a208=(a208+a532); + a532=(a452*a164); + a120=(a529*a120); + a532=(a532+a120); + a120=(a521*a532); + a208=(a208+a120); + a120=(a452*a164); + a178=(a520*a178); + a120=(a120+a178); + a178=(a521*a120); + a208=(a208+a178); + a638=(a520*a638); + a638=(a164+a638); + a178=(a521*a638); + a208=(a208+a178); + a178=(a521*a208); + a134=(a517*a134); + a519=(a521*a134); + a178=(a178+a519); + a519=(a452*a134); + a90=(a529*a90); + a519=(a519+a90); + a90=(a521*a519); + a178=(a178+a90); + a90=(a452*a134); + a148=(a520*a148); + a90=(a90+a148); + a148=(a521*a90); + a178=(a178+a148); + a618=(a520*a618); + a618=(a134+a618); + a148=(a521*a618); + a178=(a178+a148); + a148=(a521*a178); + a104=(a517*a104); + a829=(a521*a104); + a148=(a148+a829); + a829=(a452*a104); + a60=(a529*a60); + a829=(a829+a60); + a60=(a521*a829); + a148=(a148+a60); + a60=(a452*a104); + a118=(a520*a118); + a60=(a60+a118); + a118=(a521*a60); + a148=(a148+a118); + a598=(a520*a598); + a598=(a104+a598); + a118=(a521*a598); + a148=(a148+a118); + a118=(a521*a148); + a74=(a517*a74); + a534=(a521*a74); + a118=(a118+a534); + a534=(a452*a74); + a30=(a529*a30); + a534=(a534+a30); + a30=(a521*a534); + a118=(a118+a30); + a30=(a452*a74); + a88=(a520*a88); + a30=(a30+a88); + a88=(a521*a30); + a118=(a118+a88); + a578=(a520*a578); + a578=(a74+a578); + a88=(a521*a578); + a118=(a118+a88); + a88=(a521*a118); + a44=(a517*a44); + a838=(a521*a44); + a88=(a88+a838); + a838=(a452*a44); + a842=(a529*a842); + a838=(a838+a842); + a842=(a521*a838); + a88=(a88+a842); + a842=(a452*a44); + a58=(a520*a58); + a842=(a842+a58); + a58=(a521*a842); + a88=(a88+a58); + a558=(a520*a558); + a558=(a44+a558); + a58=(a521*a558); + a88=(a88+a58); + a58=(a521*a88); + a14=(a517*a14); + a535=(a521*a14); + a58=(a58+a535); + a535=(a452*a14); + a825=(a529*a825); + a535=(a535+a825); + a825=(a521*a535); + a58=(a58+a825); + a825=(a452*a14); + a28=(a520*a28); + a825=(a825+a28); + a28=(a521*a825); + a58=(a58+a28); + a538=(a520*a538); + a538=(a14+a538); + a28=(a521*a538); + a58=(a58+a28); + a28=(a521*a58); + a837=(a517*a837); + a835=(a521*a837); + a28=(a28+a835); + a835=(a452*a837); + a847=(a529*a847); + a835=(a835+a847); + a847=(a521*a835); + a28=(a28+a847); + a847=(a452*a837); + a849=(a520*a849); + a847=(a847+a849); + a849=(a521*a847); + a28=(a28+a849); + a851=(a520*a851); + a851=(a837+a851); + a849=(a521*a851); + a28=(a28+a849); + a28=(a523*a28); + a821=(a517*a821); + a849=(a523*a821); + a28=(a28+a849); + a849=(a452*a821); + a830=(a529*a830); + a849=(a849+a830); + a849=(a523*a849); + a28=(a28+a849); + a849=(a452*a821); + a832=(a520*a832); + a849=(a849+a832); + a849=(a523*a849); + a28=(a28+a849); + a834=(a520*a834); + a821=(a821+a834); + a821=(a523*a821); + a28=(a28+a821); + a462=(a462*a840); + a28=(a28-a462); + a463=(a463*a840); + a463=(a456*a463); + a28=(a28-a463); + a821=(a458*a840); + a28=(a28+a821); + a821=(a459*a840); + a821=(a456*a821); + a28=(a28+a821); + a12=(a12+a12); + a12=(a12*a840); + a28=(a28+a12); + if (res[2]!=0) res[2][49]=a28; + a556=(a556*a846); + a555=(a555*a852); + a556=(a556+a555); + a554=(a554*a855); + a556=(a556+a554); + a543=(a543*a846); + a556=(a556+a543); + a552=(a552*a848); + a556=(a556+a552); + a549=(a549*a850); + a556=(a556+a549); + a546=(a546*a853); + a556=(a556+a546); + a542=(a542*a848); + a556=(a556+a542); + a461=(a461*a840); + a556=(a556+a461); + a464=(a464*a840); + a464=(a452*a464); + a556=(a556+a464); + a455=(a455-a556); + a455=(a455+a460); + a460=(a36+a36); + a460=(a460*a840); + a455=(a455+a460); + if (res[2]!=0) res[2][50]=a455; + a58=(a523*a58); + a837=(a523*a837); + a58=(a58+a837); + a835=(a523*a835); + a58=(a58+a835); + a847=(a523*a847); + a58=(a58+a847); + a851=(a523*a851); + a58=(a58+a851); + a466=(a466*a840); + a58=(a58-a466); + a467=(a467*a840); + a467=(a456*a467); + a58=(a58-a467); + a58=(a58+a462); + a58=(a58+a463); + a42=(a42+a42); + a42=(a42*a840); + a58=(a58+a42); + if (res[2]!=0) res[2][51]=a58; + a576=(a576*a17); + a575=(a575*a826); + a576=(a576+a575); + a574=(a574*a858); + a576=(a576+a574); + a563=(a563*a17); + a576=(a576+a563); + a572=(a572*a530); + a576=(a576+a572); + a569=(a569*a26); + a576=(a576+a569); + a566=(a566*a856); + a576=(a576+a566); + a562=(a562*a530); + a576=(a576+a562); + a465=(a465*a840); + a576=(a576+a465); + a468=(a468*a840); + a468=(a452*a468); + a576=(a576+a468); + a461=(a461-a576); + a461=(a461+a464); + a464=(a66+a66); + a464=(a464*a840); + a461=(a461+a464); + if (res[2]!=0) res[2][52]=a461; + a88=(a523*a88); + a14=(a523*a14); + a88=(a88+a14); + a535=(a523*a535); + a88=(a88+a535); + a825=(a523*a825); + a88=(a88+a825); + a538=(a523*a538); + a88=(a88+a538); + a470=(a470*a840); + a88=(a88-a470); + a471=(a471*a840); + a471=(a456*a471); + a88=(a88-a471); + a88=(a88+a466); + a88=(a88+a467); + a72=(a72+a72); + a72=(a72*a840); + a88=(a88+a72); + if (res[2]!=0) res[2][53]=a88; + a596=(a596*a47); + a595=(a595*a843); + a596=(a596+a595); + a594=(a594*a861); + a596=(a596+a594); + a583=(a583*a47); + a596=(a596+a583); + a592=(a592*a550); + a596=(a596+a592); + a589=(a589*a56); + a596=(a596+a589); + a586=(a586*a859); + a596=(a596+a586); + a582=(a582*a550); + a596=(a596+a582); + a469=(a469*a840); + a596=(a596+a469); + a472=(a472*a840); + a472=(a452*a472); + a596=(a596+a472); + a465=(a465-a596); + a465=(a465+a468); + a468=(a96+a96); + a468=(a468*a840); + a465=(a465+a468); + if (res[2]!=0) res[2][54]=a465; + a118=(a523*a118); + a44=(a523*a44); + a118=(a118+a44); + a838=(a523*a838); + a118=(a118+a838); + a842=(a523*a842); + a118=(a118+a842); + a558=(a523*a558); + a118=(a118+a558); + a474=(a474*a840); + a118=(a118-a474); + a475=(a475*a840); + a475=(a456*a475); + a118=(a118-a475); + a118=(a118+a470); + a118=(a118+a471); + a102=(a102+a102); + a102=(a102*a840); + a118=(a118+a102); + if (res[2]!=0) res[2][55]=a118; + a616=(a616*a77); + a615=(a615*a23); + a616=(a616+a615); + a614=(a614*a864); + a616=(a616+a614); + a603=(a603*a77); + a616=(a616+a603); + a612=(a612*a570); + a616=(a616+a612); + a609=(a609*a86); + a616=(a616+a609); + a606=(a606*a862); + a616=(a616+a606); + a602=(a602*a570); + a616=(a616+a602); + a473=(a473*a840); + a616=(a616+a473); + a476=(a476*a840); + a476=(a452*a476); + a616=(a616+a476); + a469=(a469-a616); + a469=(a469+a472); + a472=(a126+a126); + a472=(a472*a840); + a469=(a469+a472); + if (res[2]!=0) res[2][56]=a469; + a148=(a523*a148); + a74=(a523*a74); + a148=(a148+a74); + a534=(a523*a534); + a148=(a148+a534); + a30=(a523*a30); + a148=(a148+a30); + a578=(a523*a578); + a148=(a148+a578); + a478=(a478*a840); + a148=(a148-a478); + a479=(a479*a840); + a479=(a456*a479); + a148=(a148-a479); + a148=(a148+a474); + a148=(a148+a475); + a132=(a132+a132); + a132=(a132*a840); + a148=(a148+a132); + if (res[2]!=0) res[2][57]=a148; + a636=(a636*a107); + a635=(a635*a53); + a636=(a636+a635); + a634=(a634*a867); + a636=(a636+a634); + a623=(a623*a107); + a636=(a636+a623); + a632=(a632*a590); + a636=(a636+a632); + a629=(a629*a116); + a636=(a636+a629); + a626=(a626*a865); + a636=(a636+a626); + a622=(a622*a590); + a636=(a636+a622); + a477=(a477*a840); + a636=(a636+a477); + a480=(a480*a840); + a480=(a452*a480); + a636=(a636+a480); + a473=(a473-a636); + a473=(a473+a476); + a476=(a156+a156); + a476=(a476*a840); + a473=(a473+a476); + if (res[2]!=0) res[2][58]=a473; + a178=(a523*a178); + a104=(a523*a104); + a178=(a178+a104); + a829=(a523*a829); + a178=(a178+a829); + a60=(a523*a60); + a178=(a178+a60); + a598=(a523*a598); + a178=(a178+a598); + a482=(a482*a840); + a178=(a178-a482); + a483=(a483*a840); + a483=(a456*a483); + a178=(a178-a483); + a178=(a178+a478); + a178=(a178+a479); + a162=(a162+a162); + a162=(a162*a840); + a178=(a178+a162); + if (res[2]!=0) res[2][59]=a178; + a656=(a656*a137); + a655=(a655*a83); + a656=(a656+a655); + a654=(a654*a870); + a656=(a656+a654); + a643=(a643*a137); + a656=(a656+a643); + a652=(a652*a610); + a656=(a656+a652); + a649=(a649*a146); + a656=(a656+a649); + a646=(a646*a868); + a656=(a656+a646); + a642=(a642*a610); + a656=(a656+a642); + a481=(a481*a840); + a656=(a656+a481); + a484=(a484*a840); + a484=(a452*a484); + a656=(a656+a484); + a477=(a477-a656); + a477=(a477+a480); + a480=(a186+a186); + a480=(a480*a840); + a477=(a477+a480); + if (res[2]!=0) res[2][60]=a477; + a208=(a523*a208); + a134=(a523*a134); + a208=(a208+a134); + a519=(a523*a519); + a208=(a208+a519); + a90=(a523*a90); + a208=(a208+a90); + a618=(a523*a618); + a208=(a208+a618); + a486=(a486*a840); + a208=(a208-a486); + a487=(a487*a840); + a487=(a456*a487); + a208=(a208-a487); + a208=(a208+a482); + a208=(a208+a483); + a192=(a192+a192); + a192=(a192*a840); + a208=(a208+a192); + if (res[2]!=0) res[2][61]=a208; + a676=(a676*a167); + a675=(a675*a113); + a676=(a676+a675); + a674=(a674*a873); + a676=(a676+a674); + a663=(a663*a167); + a676=(a676+a663); + a672=(a672*a630); + a676=(a676+a672); + a669=(a669*a176); + a676=(a676+a669); + a666=(a666*a871); + a676=(a676+a666); + a662=(a662*a630); + a676=(a676+a662); + a485=(a485*a840); + a676=(a676+a485); + a488=(a488*a840); + a488=(a452*a488); + a676=(a676+a488); + a481=(a481-a676); + a481=(a481+a484); + a484=(a216+a216); + a484=(a484*a840); + a481=(a481+a484); + if (res[2]!=0) res[2][62]=a481; + a238=(a523*a238); + a164=(a523*a164); + a238=(a238+a164); + a532=(a523*a532); + a238=(a238+a532); + a120=(a523*a120); + a238=(a238+a120); + a638=(a523*a638); + a238=(a238+a638); + a490=(a490*a840); + a238=(a238-a490); + a491=(a491*a840); + a491=(a456*a491); + a238=(a238-a491); + a238=(a238+a486); + a238=(a238+a487); + a222=(a222+a222); + a222=(a222*a840); + a238=(a238+a222); + if (res[2]!=0) res[2][63]=a238; + a696=(a696*a197); + a695=(a695*a143); + a696=(a696+a695); + a694=(a694*a876); + a696=(a696+a694); + a683=(a683*a197); + a696=(a696+a683); + a692=(a692*a650); + a696=(a696+a692); + a689=(a689*a206); + a696=(a696+a689); + a686=(a686*a874); + a696=(a696+a686); + a682=(a682*a650); + a696=(a696+a682); + a489=(a489*a840); + a696=(a696+a489); + a492=(a492*a840); + a492=(a452*a492); + a696=(a696+a492); + a485=(a485-a696); + a485=(a485+a488); + a488=(a246+a246); + a488=(a488*a840); + a485=(a485+a488); + if (res[2]!=0) res[2][64]=a485; + a268=(a523*a268); + a194=(a523*a194); + a268=(a268+a194); + a833=(a523*a833); + a268=(a268+a833); + a150=(a523*a150); + a268=(a268+a150); + a658=(a523*a658); + a268=(a268+a658); + a494=(a494*a840); + a268=(a268-a494); + a495=(a495*a840); + a495=(a456*a495); + a268=(a268-a495); + a268=(a268+a490); + a268=(a268+a491); + a252=(a252+a252); + a252=(a252*a840); + a268=(a268+a252); + if (res[2]!=0) res[2][65]=a268; + a716=(a716*a227); + a715=(a715*a173); + a716=(a716+a715); + a714=(a714*a879); + a716=(a716+a714); + a703=(a703*a227); + a716=(a716+a703); + a712=(a712*a670); + a716=(a716+a712); + a709=(a709*a236); + a716=(a716+a709); + a706=(a706*a877); + a716=(a716+a706); + a702=(a702*a670); + a716=(a716+a702); + a493=(a493*a840); + a716=(a716+a493); + a496=(a496*a840); + a496=(a452*a496); + a716=(a716+a496); + a489=(a489-a716); + a489=(a489+a492); + a492=(a276+a276); + a492=(a492*a840); + a489=(a489+a492); + if (res[2]!=0) res[2][66]=a489; + a298=(a523*a298); + a224=(a523*a224); + a298=(a298+a224); + a528=(a523*a528); + a298=(a298+a528); + a180=(a523*a180); + a298=(a298+a180); + a678=(a523*a678); + a298=(a298+a678); + a498=(a498*a840); + a298=(a298-a498); + a499=(a499*a840); + a499=(a456*a499); + a298=(a298-a499); + a298=(a298+a494); + a298=(a298+a495); + a282=(a282+a282); + a282=(a282*a840); + a298=(a298+a282); + if (res[2]!=0) res[2][67]=a298; + a736=(a736*a257); + a735=(a735*a203); + a736=(a736+a735); + a734=(a734*a882); + a736=(a736+a734); + a723=(a723*a257); + a736=(a736+a723); + a732=(a732*a690); + a736=(a736+a732); + a729=(a729*a266); + a736=(a736+a729); + a726=(a726*a880); + a736=(a736+a726); + a722=(a722*a690); + a736=(a736+a722); + a497=(a497*a840); + a736=(a736+a497); + a500=(a500*a840); + a500=(a452*a500); + a736=(a736+a500); + a493=(a493-a736); + a493=(a493+a496); + a496=(a306+a306); + a496=(a496*a840); + a493=(a493+a496); + if (res[2]!=0) res[2][68]=a493; + a328=(a523*a328); + a254=(a523*a254); + a328=(a328+a254); + a836=(a523*a836); + a328=(a328+a836); + a210=(a523*a210); + a328=(a328+a210); + a698=(a523*a698); + a328=(a328+a698); + a502=(a502*a840); + a328=(a328-a502); + a503=(a503*a840); + a503=(a456*a503); + a328=(a328-a503); + a328=(a328+a498); + a328=(a328+a499); + a312=(a312+a312); + a312=(a312*a840); + a328=(a328+a312); + if (res[2]!=0) res[2][69]=a328; + a756=(a756*a287); + a755=(a755*a233); + a756=(a756+a755); + a754=(a754*a885); + a756=(a756+a754); + a743=(a743*a287); + a756=(a756+a743); + a752=(a752*a710); + a756=(a756+a752); + a749=(a749*a296); + a756=(a756+a749); + a746=(a746*a883); + a756=(a756+a746); + a742=(a742*a710); + a756=(a756+a742); + a501=(a501*a840); + a756=(a756+a501); + a504=(a504*a840); + a504=(a452*a504); + a756=(a756+a504); + a497=(a497-a756); + a497=(a497+a500); + a500=(a336+a336); + a500=(a500*a840); + a497=(a497+a500); + if (res[2]!=0) res[2][70]=a497; + a358=(a523*a358); + a284=(a523*a284); + a358=(a358+a284); + a525=(a523*a525); + a358=(a358+a525); + a240=(a523*a240); + a358=(a358+a240); + a718=(a523*a718); + a358=(a358+a718); + a506=(a506*a840); + a358=(a358-a506); + a507=(a507*a840); + a507=(a456*a507); + a358=(a358-a507); + a358=(a358+a502); + a358=(a358+a503); + a342=(a342+a342); + a342=(a342*a840); + a358=(a358+a342); + if (res[2]!=0) res[2][71]=a358; + a776=(a776*a317); + a775=(a775*a263); + a776=(a776+a775); + a774=(a774*a888); + a776=(a776+a774); + a763=(a763*a317); + a776=(a776+a763); + a772=(a772*a730); + a776=(a776+a772); + a769=(a769*a326); + a776=(a776+a769); + a766=(a766*a886); + a776=(a776+a766); + a762=(a762*a730); + a776=(a776+a762); + a505=(a505*a840); + a776=(a776+a505); + a508=(a508*a840); + a508=(a452*a508); + a776=(a776+a508); + a501=(a501-a776); + a501=(a501+a504); + a504=(a366+a366); + a504=(a504*a840); + a501=(a501+a504); + if (res[2]!=0) res[2][72]=a501; + a388=(a523*a388); + a314=(a523*a314); + a388=(a388+a314); + a831=(a523*a831); + a388=(a388+a831); + a270=(a523*a270); + a388=(a388+a270); + a738=(a523*a738); + a388=(a388+a738); + a510=(a510*a840); + a388=(a388-a510); + a511=(a511*a840); + a511=(a456*a511); + a388=(a388-a511); + a388=(a388+a506); + a388=(a388+a507); + a372=(a372+a372); + a372=(a372*a840); + a388=(a388+a372); + if (res[2]!=0) res[2][73]=a388; + a796=(a796*a347); + a795=(a795*a293); + a796=(a796+a795); + a794=(a794*a891); + a796=(a796+a794); + a783=(a783*a347); + a796=(a796+a783); + a792=(a792*a750); + a796=(a796+a792); + a789=(a789*a356); + a796=(a796+a789); + a786=(a786*a889); + a796=(a796+a786); + a782=(a782*a750); + a796=(a796+a782); + a509=(a509*a840); + a796=(a796+a509); + a512=(a512*a840); + a512=(a452*a512); + a796=(a796+a512); + a505=(a505-a796); + a505=(a505+a508); + a508=(a396+a396); + a508=(a508*a840); + a505=(a505+a508); + if (res[2]!=0) res[2][74]=a505; + a379=(a523*a379); + a344=(a523*a344); + a379=(a379+a344); + a518=(a523*a518); + a379=(a379+a518); + a300=(a523*a300); + a379=(a379+a300); + a758=(a523*a758); + a379=(a379+a758); + a514=(a514*a840); + a379=(a379-a514); + a515=(a515*a840); + a515=(a456*a515); + a379=(a379-a515); + a379=(a379+a510); + a379=(a379+a511); + a402=(a402+a402); + a402=(a402*a840); + a379=(a379+a402); + if (res[2]!=0) res[2][75]=a379; + a815=(a815*a377); + a814=(a814*a323); + a815=(a815+a814); + a813=(a813*a894); + a815=(a815+a813); + a422=(a422*a377); + a815=(a815+a422); + a812=(a812*a770); + a815=(a815+a812); + a809=(a809*a386); + a815=(a815+a809); + a806=(a806*a892); + a815=(a815+a806); + a802=(a802*a770); + a815=(a815+a802); + a509=(a509-a815); + a509=(a509+a512); + a512=(a426+a426); + a512=(a512*a840); + a509=(a509+a512); + if (res[2]!=0) res[2][76]=a509; + a374=(a523*a374); + a536=(a523*a536); + a374=(a374+a536); + a330=(a523*a330); + a374=(a374+a330); + a523=(a523*a778); + a374=(a374+a523); + a374=(a374+a514); + a374=(a374+a515); + a432=(a432+a432); + a432=(a432*a840); + a374=(a374+a432); + if (res[2]!=0) res[2][77]=a374; + a428=(-a428); + if (res[3]!=0) res[3][0]=a428; + a824=(-a824); + if (res[3]!=0) res[3][1]=a824; + a516=(-a516); + if (res[3]!=0) res[3][2]=a516; + a820=(a820*a423); + a3=(a3*a840); + a820=(a820-a3); + a4=(a4*a840); + a4=(a0*a4); + a820=(a820-a4); + if (res[3]!=0) res[3][3]=a820; + a819=(a819*a423); + a8=(a8*a840); + a819=(a819+a8); + a9=(a9*a840); + a9=(a0*a9); + a819=(a819+a9); + a819=(-a819); + if (res[3]!=0) res[3][4]=a819; + a819=sin(a11); + a9=casadi_sq(a25); + a8=casadi_sq(a29); + a9=(a9+a8); + a25=(a25/a9); + a22=(a22*a840); + a25=(a25*a22); + a8=(a27*a25); + a819=(a819*a8); + a8=cos(a11); + a5=(a5*a423); + a8=(a8*a5); + a5=sin(a11); + a513=(a513*a423); + a5=(a5*a513); + a8=(a8+a5); + a5=cos(a11); + a25=(a24*a25); + a5=(a5*a25); + a8=(a8+a5); + a819=(a819-a8); + a8=cos(a11); + a29=(a29/a9); + a29=(a29*a22); + a27=(a27*a29); + a8=(a8*a27); + a819=(a819-a8); + a8=sin(a11); + a24=(a24*a29); + a8=(a8*a24); + a819=(a819-a8); + a8=cos(a11); + a24=casadi_sq(a16); + a29=casadi_sq(a20); + a24=(a24+a29); + a16=(a16/a24); + a31=(a31*a840); + a16=(a16*a31); + a29=(a15*a16); + a8=(a8*a29); + a819=(a819-a8); + a8=sin(a11); + a16=(a18*a16); + a8=(a8*a16); + a819=(a819+a8); + a8=cos(a11); + a20=(a20/a24); + a20=(a20*a31); + a18=(a18*a20); + a8=(a8*a18); + a819=(a819-a8); + a11=sin(a11); + a15=(a15*a20); + a11=(a11*a15); + a819=(a819-a11); + if (res[3]!=0) res[3][5]=a819; + a2=(a2*a828); + a34=(a34*a840); + a2=(a2-a34); + a35=(a35*a840); + a35=(a0*a35); + a2=(a2-a35); + if (res[3]!=0) res[3][6]=a2; + a7=(a7*a828); + a39=(a39*a840); + a7=(a7+a39); + a40=(a40*a840); + a40=(a0*a40); + a7=(a7+a40); + a7=(-a7); + if (res[3]!=0) res[3][7]=a7; + a7=sin(a41); + a40=casadi_sq(a55); + a39=casadi_sq(a59); + a40=(a40+a39); + a55=(a55/a40); + a52=(a52*a840); + a55=(a55*a52); + a39=(a57*a55); + a7=(a7*a39); + a39=cos(a41); + a1=(a1*a828); + a39=(a39*a1); + a1=sin(a41); + a6=(a6*a828); + a1=(a1*a6); + a39=(a39+a1); + a1=cos(a41); + a55=(a54*a55); + a1=(a1*a55); + a39=(a39+a1); + a7=(a7-a39); + a39=cos(a41); + a59=(a59/a40); + a59=(a59*a52); + a57=(a57*a59); + a39=(a39*a57); + a7=(a7-a39); + a39=sin(a41); + a54=(a54*a59); + a39=(a39*a54); + a7=(a7-a39); + a39=cos(a41); + a54=casadi_sq(a46); + a59=casadi_sq(a50); + a54=(a54+a59); + a46=(a46/a54); + a61=(a61*a840); + a46=(a46*a61); + a59=(a45*a46); + a39=(a39*a59); + a7=(a7-a39); + a39=sin(a41); + a46=(a48*a46); + a39=(a39*a46); + a7=(a7+a39); + a39=cos(a41); + a50=(a50/a54); + a50=(a50*a61); + a48=(a48*a50); + a39=(a39*a48); + a7=(a7-a39); + a41=sin(a41); + a45=(a45*a50); + a41=(a41*a45); + a7=(a7-a41); + if (res[3]!=0) res[3][8]=a7; + a33=(a33*a845); + a64=(a64*a840); + a33=(a33-a64); + a65=(a65*a840); + a65=(a0*a65); + a33=(a33-a65); + if (res[3]!=0) res[3][9]=a33; + a38=(a38*a845); + a69=(a69*a840); + a38=(a38+a69); + a70=(a70*a840); + a70=(a0*a70); + a38=(a38+a70); + a38=(-a38); + if (res[3]!=0) res[3][10]=a38; + a38=sin(a71); + a70=casadi_sq(a85); + a69=casadi_sq(a89); + a70=(a70+a69); + a85=(a85/a70); + a82=(a82*a840); + a85=(a85*a82); + a69=(a87*a85); + a38=(a38*a69); + a69=cos(a71); + a32=(a32*a845); + a69=(a69*a32); + a32=sin(a71); + a37=(a37*a845); + a32=(a32*a37); + a69=(a69+a32); + a32=cos(a71); + a85=(a84*a85); + a32=(a32*a85); + a69=(a69+a32); + a38=(a38-a69); + a69=cos(a71); + a89=(a89/a70); + a89=(a89*a82); + a87=(a87*a89); + a69=(a69*a87); + a38=(a38-a69); + a69=sin(a71); + a84=(a84*a89); + a69=(a69*a84); + a38=(a38-a69); + a69=cos(a71); + a84=casadi_sq(a76); + a89=casadi_sq(a80); + a84=(a84+a89); + a76=(a76/a84); + a91=(a91*a840); + a76=(a76*a91); + a89=(a75*a76); + a69=(a69*a89); + a38=(a38-a69); + a69=sin(a71); + a76=(a78*a76); + a69=(a69*a76); + a38=(a38+a69); + a69=cos(a71); + a80=(a80/a84); + a80=(a80*a91); + a78=(a78*a80); + a69=(a69*a78); + a38=(a38-a69); + a71=sin(a71); + a75=(a75*a80); + a71=(a71*a75); + a38=(a38-a71); + if (res[3]!=0) res[3][11]=a38; + a63=(a63*a19); + a94=(a94*a840); + a63=(a63-a94); + a95=(a95*a840); + a95=(a0*a95); + a63=(a63-a95); + if (res[3]!=0) res[3][12]=a63; + a68=(a68*a19); + a99=(a99*a840); + a68=(a68+a99); + a100=(a100*a840); + a100=(a0*a100); + a68=(a68+a100); + a68=(-a68); + if (res[3]!=0) res[3][13]=a68; + a68=sin(a101); + a100=casadi_sq(a115); + a99=casadi_sq(a119); + a100=(a100+a99); + a115=(a115/a100); + a112=(a112*a840); + a115=(a115*a112); + a99=(a117*a115); + a68=(a68*a99); + a99=cos(a101); + a62=(a62*a19); + a99=(a99*a62); + a62=sin(a101); + a67=(a67*a19); + a62=(a62*a67); + a99=(a99+a62); + a62=cos(a101); + a115=(a114*a115); + a62=(a62*a115); + a99=(a99+a62); + a68=(a68-a99); + a99=cos(a101); + a119=(a119/a100); + a119=(a119*a112); + a117=(a117*a119); + a99=(a99*a117); + a68=(a68-a99); + a99=sin(a101); + a114=(a114*a119); + a99=(a99*a114); + a68=(a68-a99); + a99=cos(a101); + a114=casadi_sq(a106); + a119=casadi_sq(a110); + a114=(a114+a119); + a106=(a106/a114); + a121=(a121*a840); + a106=(a106*a121); + a119=(a105*a106); + a99=(a99*a119); + a68=(a68-a99); + a99=sin(a101); + a106=(a108*a106); + a99=(a99*a106); + a68=(a68+a99); + a99=cos(a101); + a110=(a110/a114); + a110=(a110*a121); + a108=(a108*a110); + a99=(a99*a108); + a68=(a68-a99); + a101=sin(a101); + a105=(a105*a110); + a101=(a101*a105); + a68=(a68-a101); + if (res[3]!=0) res[3][14]=a68; + a93=(a93*a49); + a124=(a124*a840); + a93=(a93-a124); + a125=(a125*a840); + a125=(a0*a125); + a93=(a93-a125); + if (res[3]!=0) res[3][15]=a93; + a98=(a98*a49); + a129=(a129*a840); + a98=(a98+a129); + a130=(a130*a840); + a130=(a0*a130); + a98=(a98+a130); + a98=(-a98); + if (res[3]!=0) res[3][16]=a98; + a98=sin(a131); + a130=casadi_sq(a145); + a129=casadi_sq(a149); + a130=(a130+a129); + a145=(a145/a130); + a142=(a142*a840); + a145=(a145*a142); + a129=(a147*a145); + a98=(a98*a129); + a129=cos(a131); + a92=(a92*a49); + a129=(a129*a92); + a92=sin(a131); + a97=(a97*a49); + a92=(a92*a97); + a129=(a129+a92); + a92=cos(a131); + a145=(a144*a145); + a92=(a92*a145); + a129=(a129+a92); + a98=(a98-a129); + a129=cos(a131); + a149=(a149/a130); + a149=(a149*a142); + a147=(a147*a149); + a129=(a129*a147); + a98=(a98-a129); + a129=sin(a131); + a144=(a144*a149); + a129=(a129*a144); + a98=(a98-a129); + a129=cos(a131); + a144=casadi_sq(a136); + a149=casadi_sq(a140); + a144=(a144+a149); + a136=(a136/a144); + a151=(a151*a840); + a136=(a136*a151); + a149=(a135*a136); + a129=(a129*a149); + a98=(a98-a129); + a129=sin(a131); + a136=(a138*a136); + a129=(a129*a136); + a98=(a98+a129); + a129=cos(a131); + a140=(a140/a144); + a140=(a140*a151); + a138=(a138*a140); + a129=(a129*a138); + a98=(a98-a129); + a131=sin(a131); + a135=(a135*a140); + a131=(a131*a135); + a98=(a98-a131); + if (res[3]!=0) res[3][17]=a98; + a123=(a123*a79); + a154=(a154*a840); + a123=(a123-a154); + a155=(a155*a840); + a155=(a0*a155); + a123=(a123-a155); + if (res[3]!=0) res[3][18]=a123; + a128=(a128*a79); + a159=(a159*a840); + a128=(a128+a159); + a160=(a160*a840); + a160=(a0*a160); + a128=(a128+a160); + a128=(-a128); + if (res[3]!=0) res[3][19]=a128; + a128=sin(a161); + a160=casadi_sq(a175); + a159=casadi_sq(a179); + a160=(a160+a159); + a175=(a175/a160); + a172=(a172*a840); + a175=(a175*a172); + a159=(a177*a175); + a128=(a128*a159); + a159=cos(a161); + a122=(a122*a79); + a159=(a159*a122); + a122=sin(a161); + a127=(a127*a79); + a122=(a122*a127); + a159=(a159+a122); + a122=cos(a161); + a175=(a174*a175); + a122=(a122*a175); + a159=(a159+a122); + a128=(a128-a159); + a159=cos(a161); + a179=(a179/a160); + a179=(a179*a172); + a177=(a177*a179); + a159=(a159*a177); + a128=(a128-a159); + a159=sin(a161); + a174=(a174*a179); + a159=(a159*a174); + a128=(a128-a159); + a159=cos(a161); + a174=casadi_sq(a166); + a179=casadi_sq(a170); + a174=(a174+a179); + a166=(a166/a174); + a181=(a181*a840); + a166=(a166*a181); + a179=(a165*a166); + a159=(a159*a179); + a128=(a128-a159); + a159=sin(a161); + a166=(a168*a166); + a159=(a159*a166); + a128=(a128+a159); + a159=cos(a161); + a170=(a170/a174); + a170=(a170*a181); + a168=(a168*a170); + a159=(a159*a168); + a128=(a128-a159); + a161=sin(a161); + a165=(a165*a170); + a161=(a161*a165); + a128=(a128-a161); + if (res[3]!=0) res[3][20]=a128; + a153=(a153*a109); + a184=(a184*a840); + a153=(a153-a184); + a185=(a185*a840); + a185=(a0*a185); + a153=(a153-a185); + if (res[3]!=0) res[3][21]=a153; + a158=(a158*a109); + a189=(a189*a840); + a158=(a158+a189); + a190=(a190*a840); + a190=(a0*a190); + a158=(a158+a190); + a158=(-a158); + if (res[3]!=0) res[3][22]=a158; + a158=sin(a191); + a190=casadi_sq(a205); + a189=casadi_sq(a209); + a190=(a190+a189); + a205=(a205/a190); + a202=(a202*a840); + a205=(a205*a202); + a189=(a207*a205); + a158=(a158*a189); + a189=cos(a191); + a152=(a152*a109); + a189=(a189*a152); + a152=sin(a191); + a157=(a157*a109); + a152=(a152*a157); + a189=(a189+a152); + a152=cos(a191); + a205=(a204*a205); + a152=(a152*a205); + a189=(a189+a152); + a158=(a158-a189); + a189=cos(a191); + a209=(a209/a190); + a209=(a209*a202); + a207=(a207*a209); + a189=(a189*a207); + a158=(a158-a189); + a189=sin(a191); + a204=(a204*a209); + a189=(a189*a204); + a158=(a158-a189); + a189=cos(a191); + a204=casadi_sq(a196); + a209=casadi_sq(a200); + a204=(a204+a209); + a196=(a196/a204); + a211=(a211*a840); + a196=(a196*a211); + a209=(a195*a196); + a189=(a189*a209); + a158=(a158-a189); + a189=sin(a191); + a196=(a198*a196); + a189=(a189*a196); + a158=(a158+a189); + a189=cos(a191); + a200=(a200/a204); + a200=(a200*a211); + a198=(a198*a200); + a189=(a189*a198); + a158=(a158-a189); + a191=sin(a191); + a195=(a195*a200); + a191=(a191*a195); + a158=(a158-a191); + if (res[3]!=0) res[3][23]=a158; + a183=(a183*a139); + a214=(a214*a840); + a183=(a183-a214); + a215=(a215*a840); + a215=(a0*a215); + a183=(a183-a215); + if (res[3]!=0) res[3][24]=a183; + a188=(a188*a139); + a219=(a219*a840); + a188=(a188+a219); + a220=(a220*a840); + a220=(a0*a220); + a188=(a188+a220); + a188=(-a188); + if (res[3]!=0) res[3][25]=a188; + a188=sin(a221); + a220=casadi_sq(a235); + a219=casadi_sq(a239); + a220=(a220+a219); + a235=(a235/a220); + a232=(a232*a840); + a235=(a235*a232); + a219=(a237*a235); + a188=(a188*a219); + a219=cos(a221); + a182=(a182*a139); + a219=(a219*a182); + a182=sin(a221); + a187=(a187*a139); + a182=(a182*a187); + a219=(a219+a182); + a182=cos(a221); + a235=(a234*a235); + a182=(a182*a235); + a219=(a219+a182); + a188=(a188-a219); + a219=cos(a221); + a239=(a239/a220); + a239=(a239*a232); + a237=(a237*a239); + a219=(a219*a237); + a188=(a188-a219); + a219=sin(a221); + a234=(a234*a239); + a219=(a219*a234); + a188=(a188-a219); + a219=cos(a221); + a234=casadi_sq(a226); + a239=casadi_sq(a230); + a234=(a234+a239); + a226=(a226/a234); + a241=(a241*a840); + a226=(a226*a241); + a239=(a225*a226); + a219=(a219*a239); + a188=(a188-a219); + a219=sin(a221); + a226=(a228*a226); + a219=(a219*a226); + a188=(a188+a219); + a219=cos(a221); + a230=(a230/a234); + a230=(a230*a241); + a228=(a228*a230); + a219=(a219*a228); + a188=(a188-a219); + a221=sin(a221); + a225=(a225*a230); + a221=(a221*a225); + a188=(a188-a221); + if (res[3]!=0) res[3][26]=a188; + a213=(a213*a169); + a244=(a244*a840); + a213=(a213-a244); + a245=(a245*a840); + a245=(a0*a245); + a213=(a213-a245); + if (res[3]!=0) res[3][27]=a213; + a218=(a218*a169); + a249=(a249*a840); + a218=(a218+a249); + a250=(a250*a840); + a250=(a0*a250); + a218=(a218+a250); + a218=(-a218); + if (res[3]!=0) res[3][28]=a218; + a218=sin(a251); + a250=casadi_sq(a265); + a249=casadi_sq(a269); + a250=(a250+a249); + a265=(a265/a250); + a262=(a262*a840); + a265=(a265*a262); + a249=(a267*a265); + a218=(a218*a249); + a249=cos(a251); + a212=(a212*a169); + a249=(a249*a212); + a212=sin(a251); + a217=(a217*a169); + a212=(a212*a217); + a249=(a249+a212); + a212=cos(a251); + a265=(a264*a265); + a212=(a212*a265); + a249=(a249+a212); + a218=(a218-a249); + a249=cos(a251); + a269=(a269/a250); + a269=(a269*a262); + a267=(a267*a269); + a249=(a249*a267); + a218=(a218-a249); + a249=sin(a251); + a264=(a264*a269); + a249=(a249*a264); + a218=(a218-a249); + a249=cos(a251); + a264=casadi_sq(a256); + a269=casadi_sq(a260); + a264=(a264+a269); + a256=(a256/a264); + a271=(a271*a840); + a256=(a256*a271); + a269=(a255*a256); + a249=(a249*a269); + a218=(a218-a249); + a249=sin(a251); + a256=(a258*a256); + a249=(a249*a256); + a218=(a218+a249); + a249=cos(a251); + a260=(a260/a264); + a260=(a260*a271); + a258=(a258*a260); + a249=(a249*a258); + a218=(a218-a249); + a251=sin(a251); + a255=(a255*a260); + a251=(a251*a255); + a218=(a218-a251); + if (res[3]!=0) res[3][29]=a218; + a243=(a243*a199); + a274=(a274*a840); + a243=(a243-a274); + a275=(a275*a840); + a275=(a0*a275); + a243=(a243-a275); + if (res[3]!=0) res[3][30]=a243; + a248=(a248*a199); + a279=(a279*a840); + a248=(a248+a279); + a280=(a280*a840); + a280=(a0*a280); + a248=(a248+a280); + a248=(-a248); + if (res[3]!=0) res[3][31]=a248; + a248=sin(a281); + a280=casadi_sq(a295); + a279=casadi_sq(a299); + a280=(a280+a279); + a295=(a295/a280); + a292=(a292*a840); + a295=(a295*a292); + a279=(a297*a295); + a248=(a248*a279); + a279=cos(a281); + a242=(a242*a199); + a279=(a279*a242); + a242=sin(a281); + a247=(a247*a199); + a242=(a242*a247); + a279=(a279+a242); + a242=cos(a281); + a295=(a294*a295); + a242=(a242*a295); + a279=(a279+a242); + a248=(a248-a279); + a279=cos(a281); + a299=(a299/a280); + a299=(a299*a292); + a297=(a297*a299); + a279=(a279*a297); + a248=(a248-a279); + a279=sin(a281); + a294=(a294*a299); + a279=(a279*a294); + a248=(a248-a279); + a279=cos(a281); + a294=casadi_sq(a286); + a299=casadi_sq(a290); + a294=(a294+a299); + a286=(a286/a294); + a301=(a301*a840); + a286=(a286*a301); + a299=(a285*a286); + a279=(a279*a299); + a248=(a248-a279); + a279=sin(a281); + a286=(a288*a286); + a279=(a279*a286); + a248=(a248+a279); + a279=cos(a281); + a290=(a290/a294); + a290=(a290*a301); + a288=(a288*a290); + a279=(a279*a288); + a248=(a248-a279); + a281=sin(a281); + a285=(a285*a290); + a281=(a281*a285); + a248=(a248-a281); + if (res[3]!=0) res[3][32]=a248; + a273=(a273*a229); + a304=(a304*a840); + a273=(a273-a304); + a305=(a305*a840); + a305=(a0*a305); + a273=(a273-a305); + if (res[3]!=0) res[3][33]=a273; + a278=(a278*a229); + a309=(a309*a840); + a278=(a278+a309); + a310=(a310*a840); + a310=(a0*a310); + a278=(a278+a310); + a278=(-a278); + if (res[3]!=0) res[3][34]=a278; + a278=sin(a311); + a310=casadi_sq(a325); + a309=casadi_sq(a329); + a310=(a310+a309); + a325=(a325/a310); + a322=(a322*a840); + a325=(a325*a322); + a309=(a327*a325); + a278=(a278*a309); + a309=cos(a311); + a272=(a272*a229); + a309=(a309*a272); + a272=sin(a311); + a277=(a277*a229); + a272=(a272*a277); + a309=(a309+a272); + a272=cos(a311); + a325=(a324*a325); + a272=(a272*a325); + a309=(a309+a272); + a278=(a278-a309); + a309=cos(a311); + a329=(a329/a310); + a329=(a329*a322); + a327=(a327*a329); + a309=(a309*a327); + a278=(a278-a309); + a309=sin(a311); + a324=(a324*a329); + a309=(a309*a324); + a278=(a278-a309); + a309=cos(a311); + a324=casadi_sq(a316); + a329=casadi_sq(a320); + a324=(a324+a329); + a316=(a316/a324); + a331=(a331*a840); + a316=(a316*a331); + a329=(a315*a316); + a309=(a309*a329); + a278=(a278-a309); + a309=sin(a311); + a316=(a318*a316); + a309=(a309*a316); + a278=(a278+a309); + a309=cos(a311); + a320=(a320/a324); + a320=(a320*a331); + a318=(a318*a320); + a309=(a309*a318); + a278=(a278-a309); + a311=sin(a311); + a315=(a315*a320); + a311=(a311*a315); + a278=(a278-a311); + if (res[3]!=0) res[3][35]=a278; + a303=(a303*a259); + a334=(a334*a840); + a303=(a303-a334); + a335=(a335*a840); + a335=(a0*a335); + a303=(a303-a335); + if (res[3]!=0) res[3][36]=a303; + a308=(a308*a259); + a339=(a339*a840); + a308=(a308+a339); + a340=(a340*a840); + a340=(a0*a340); + a308=(a308+a340); + a308=(-a308); + if (res[3]!=0) res[3][37]=a308; + a308=sin(a341); + a340=casadi_sq(a355); + a339=casadi_sq(a359); + a340=(a340+a339); + a355=(a355/a340); + a352=(a352*a840); + a355=(a355*a352); + a339=(a357*a355); + a308=(a308*a339); + a339=cos(a341); + a302=(a302*a259); + a339=(a339*a302); + a302=sin(a341); + a307=(a307*a259); + a302=(a302*a307); + a339=(a339+a302); + a302=cos(a341); + a355=(a354*a355); + a302=(a302*a355); + a339=(a339+a302); + a308=(a308-a339); + a339=cos(a341); + a359=(a359/a340); + a359=(a359*a352); + a357=(a357*a359); + a339=(a339*a357); + a308=(a308-a339); + a339=sin(a341); + a354=(a354*a359); + a339=(a339*a354); + a308=(a308-a339); + a339=cos(a341); + a354=casadi_sq(a346); + a359=casadi_sq(a350); + a354=(a354+a359); + a346=(a346/a354); + a361=(a361*a840); + a346=(a346*a361); + a359=(a345*a346); + a339=(a339*a359); + a308=(a308-a339); + a339=sin(a341); + a346=(a348*a346); + a339=(a339*a346); + a308=(a308+a339); + a339=cos(a341); + a350=(a350/a354); + a350=(a350*a361); + a348=(a348*a350); + a339=(a339*a348); + a308=(a308-a339); + a341=sin(a341); + a345=(a345*a350); + a341=(a341*a345); + a308=(a308-a341); + if (res[3]!=0) res[3][38]=a308; + a333=(a333*a289); + a364=(a364*a840); + a333=(a333-a364); + a365=(a365*a840); + a365=(a0*a365); + a333=(a333-a365); + if (res[3]!=0) res[3][39]=a333; + a338=(a338*a289); + a369=(a369*a840); + a338=(a338+a369); + a370=(a370*a840); + a370=(a0*a370); + a338=(a338+a370); + a338=(-a338); + if (res[3]!=0) res[3][40]=a338; + a338=sin(a371); + a370=casadi_sq(a385); + a369=casadi_sq(a389); + a370=(a370+a369); + a385=(a385/a370); + a382=(a382*a840); + a385=(a385*a382); + a369=(a387*a385); + a338=(a338*a369); + a369=cos(a371); + a332=(a332*a289); + a369=(a369*a332); + a332=sin(a371); + a337=(a337*a289); + a332=(a332*a337); + a369=(a369+a332); + a332=cos(a371); + a385=(a384*a385); + a332=(a332*a385); + a369=(a369+a332); + a338=(a338-a369); + a369=cos(a371); + a389=(a389/a370); + a389=(a389*a382); + a387=(a387*a389); + a369=(a369*a387); + a338=(a338-a369); + a369=sin(a371); + a384=(a384*a389); + a369=(a369*a384); + a338=(a338-a369); + a369=cos(a371); + a384=casadi_sq(a376); + a389=casadi_sq(a380); + a384=(a384+a389); + a376=(a376/a384); + a391=(a391*a840); + a376=(a376*a391); + a389=(a375*a376); + a369=(a369*a389); + a338=(a338-a369); + a369=sin(a371); + a376=(a378*a376); + a369=(a369*a376); + a338=(a338+a369); + a369=cos(a371); + a380=(a380/a384); + a380=(a380*a391); + a378=(a378*a380); + a369=(a369*a378); + a338=(a338-a369); + a371=sin(a371); + a375=(a375*a380); + a371=(a371*a375); + a338=(a338-a371); + if (res[3]!=0) res[3][41]=a338; + a363=(a363*a319); + a394=(a394*a840); + a363=(a363-a394); + a395=(a395*a840); + a395=(a0*a395); + a363=(a363-a395); + if (res[3]!=0) res[3][42]=a363; + a368=(a368*a319); + a399=(a399*a840); + a368=(a368+a399); + a400=(a400*a840); + a400=(a0*a400); + a368=(a368+a400); + a368=(-a368); + if (res[3]!=0) res[3][43]=a368; + a368=sin(a401); + a400=casadi_sq(a415); + a399=casadi_sq(a419); + a400=(a400+a399); + a415=(a415/a400); + a412=(a412*a840); + a415=(a415*a412); + a399=(a417*a415); + a368=(a368*a399); + a399=cos(a401); + a362=(a362*a319); + a399=(a399*a362); + a362=sin(a401); + a367=(a367*a319); + a362=(a362*a367); + a399=(a399+a362); + a362=cos(a401); + a415=(a414*a415); + a362=(a362*a415); + a399=(a399+a362); + a368=(a368-a399); + a399=cos(a401); + a419=(a419/a400); + a419=(a419*a412); + a417=(a417*a419); + a399=(a399*a417); + a368=(a368-a399); + a399=sin(a401); + a414=(a414*a419); + a399=(a399*a414); + a368=(a368-a399); + a399=cos(a401); + a414=casadi_sq(a406); + a419=casadi_sq(a410); + a414=(a414+a419); + a406=(a406/a414); + a421=(a421*a840); + a406=(a406*a421); + a419=(a405*a406); + a399=(a399*a419); + a368=(a368-a399); + a399=sin(a401); + a406=(a408*a406); + a399=(a399*a406); + a368=(a368+a399); + a399=cos(a401); + a410=(a410/a414); + a410=(a410*a421); + a408=(a408*a410); + a399=(a399*a408); + a368=(a368-a399); + a401=sin(a401); + a405=(a405*a410); + a401=(a401*a405); + a368=(a368-a401); + if (res[3]!=0) res[3][44]=a368; + a393=(a393*a349); + a424=(a424*a840); + a393=(a393-a424); + a425=(a425*a840); + a425=(a0*a425); + a393=(a393-a425); + if (res[3]!=0) res[3][45]=a393; + a398=(a398*a349); + a429=(a429*a840); + a398=(a398+a429); + a430=(a430*a840); + a0=(a0*a430); + a398=(a398+a0); + a398=(-a398); + if (res[3]!=0) res[3][46]=a398; + a398=sin(a431); + a0=casadi_sq(a445); + a430=casadi_sq(a449); + a0=(a0+a430); + a445=(a445/a0); + a442=(a442*a840); + a445=(a445*a442); + a430=(a447*a445); + a398=(a398*a430); + a430=cos(a431); + a392=(a392*a349); + a430=(a430*a392); + a392=sin(a431); + a397=(a397*a349); + a392=(a392*a397); + a430=(a430+a392); + a392=cos(a431); + a445=(a444*a445); + a392=(a392*a445); + a430=(a430+a392); + a398=(a398-a430); + a430=cos(a431); + a449=(a449/a0); + a449=(a449*a442); + a447=(a447*a449); + a430=(a430*a447); + a398=(a398-a430); + a430=sin(a431); + a444=(a444*a449); + a430=(a430*a444); + a398=(a398-a430); + a430=cos(a431); + a444=casadi_sq(a436); + a449=casadi_sq(a440); + a444=(a444+a449); + a436=(a436/a444); + a451=(a451*a840); + a436=(a436*a451); + a449=(a435*a436); + a430=(a430*a449); + a398=(a398-a430); + a430=sin(a431); + a436=(a438*a436); + a430=(a430*a436); + a398=(a398+a430); + a430=cos(a431); + a440=(a440/a444); + a440=(a440*a451); + a438=(a438*a440); + a430=(a430*a438); + a398=(a398-a430); + a431=sin(a431); + a435=(a435*a440); + a431=(a431*a435); + a398=(a398-a431); + if (res[3]!=0) res[3][47]=a398; + a454=(a454*a840); + a453=(a453*a840); + a453=(a452*a453); + a454=(a454+a453); + a454=(-a454); + if (res[3]!=0) res[3][48]=a454; + a454=sin(a784); + a453=casadi_sq(a810); + a398=casadi_sq(a817); + a453=(a453+a398); + a810=(a810/a453); + a810=(a810*a890); + a816=(a816*a810); + a817=(a817/a453); + a817=(a817*a890); + a807=(a807*a817); + a816=(a816+a807); + a454=(a454*a816); + a784=cos(a784); + a427=(a427*a817); + a818=(a818*a810); + a427=(a427-a818); + a784=(a784*a427); + a454=(a454+a784); + a454=(a517*a454); + a784=(a521*a454); + a427=(a452*a454); + a818=sin(a811); + a373=(a517*a373); + a810=(a426*a373); + a818=(a818*a810); + a811=cos(a811); + a381=(a517*a381); + a810=(a426*a381); + a811=(a811*a810); + a818=(a818-a811); + a818=(a529*a818); + a427=(a427+a818); + a427=(a521*a427); + a784=(a784+a427); + a427=(a452*a454); + a818=sin(a808); + a811=(a452*a373); + a811=(a426*a811); + a818=(a818*a811); + a808=cos(a808); + a811=(a452*a381); + a811=(a426*a811); + a808=(a808*a811); + a818=(a818-a808); + a818=(a520*a818); + a427=(a427+a818); + a427=(a521*a427); + a784=(a784+a427); + a427=sin(a805); + a373=(a452*a373); + a373=(a426*a373); + a427=(a427*a373); + a805=cos(a805); + a381=(a452*a381); + a426=(a426*a381); + a805=(a805*a426); + a427=(a427-a805); + a427=(a520*a427); + a454=(a454+a427); + a454=(a521*a454); + a784=(a784+a454); + a784=(a521*a784); + a454=sin(a764); + a427=casadi_sq(a787); + a805=casadi_sq(a800); + a427=(a427+a805); + a787=(a787/a427); + a787=(a787*a887); + a799=(a799*a787); + a800=(a800/a427); + a800=(a800*a887); + a797=(a797*a800); + a799=(a799+a797); + a454=(a454*a799); + a764=cos(a764); + a793=(a793*a800); + a801=(a801*a787); + a793=(a793-a801); + a764=(a764*a793); + a454=(a454+a764); + a454=(a517*a454); + a764=(a521*a454); + a784=(a784+a764); + a764=(a452*a454); + a793=sin(a791); + a343=(a517*a343); + a801=(a396*a343); + a793=(a793*a801); + a791=cos(a791); + a351=(a517*a351); + a801=(a396*a351); + a791=(a791*a801); + a793=(a793-a791); + a793=(a529*a793); + a764=(a764+a793); + a764=(a521*a764); + a784=(a784+a764); + a764=(a452*a454); + a793=sin(a788); + a791=(a452*a343); + a791=(a396*a791); + a793=(a793*a791); + a788=cos(a788); + a791=(a452*a351); + a791=(a396*a791); + a788=(a788*a791); + a793=(a793-a788); + a793=(a520*a793); + a764=(a764+a793); + a764=(a521*a764); + a784=(a784+a764); + a764=sin(a785); + a343=(a452*a343); + a343=(a396*a343); + a764=(a764*a343); + a785=cos(a785); + a351=(a452*a351); + a396=(a396*a351); + a785=(a785*a396); + a764=(a764-a785); + a764=(a520*a764); + a454=(a454+a764); + a454=(a521*a454); + a784=(a784+a454); + a784=(a521*a784); + a454=sin(a744); + a764=casadi_sq(a767); + a785=casadi_sq(a780); + a764=(a764+a785); + a767=(a767/a764); + a767=(a767*a884); + a779=(a779*a767); + a780=(a780/a764); + a780=(a780*a884); + a777=(a777*a780); + a779=(a779+a777); + a454=(a454*a779); + a744=cos(a744); + a773=(a773*a780); + a781=(a781*a767); + a773=(a773-a781); + a744=(a744*a773); + a454=(a454+a744); + a454=(a517*a454); + a744=(a521*a454); + a784=(a784+a744); + a744=(a452*a454); + a773=sin(a771); + a313=(a517*a313); + a781=(a366*a313); + a773=(a773*a781); + a771=cos(a771); + a321=(a517*a321); + a781=(a366*a321); + a771=(a771*a781); + a773=(a773-a771); + a773=(a529*a773); + a744=(a744+a773); + a744=(a521*a744); + a784=(a784+a744); + a744=(a452*a454); + a773=sin(a768); + a771=(a452*a313); + a771=(a366*a771); + a773=(a773*a771); + a768=cos(a768); + a771=(a452*a321); + a771=(a366*a771); + a768=(a768*a771); + a773=(a773-a768); + a773=(a520*a773); + a744=(a744+a773); + a744=(a521*a744); + a784=(a784+a744); + a744=sin(a765); + a313=(a452*a313); + a313=(a366*a313); + a744=(a744*a313); + a765=cos(a765); + a321=(a452*a321); + a366=(a366*a321); + a765=(a765*a366); + a744=(a744-a765); + a744=(a520*a744); + a454=(a454+a744); + a454=(a521*a454); + a784=(a784+a454); + a784=(a521*a784); + a454=sin(a724); + a744=casadi_sq(a747); + a765=casadi_sq(a760); + a744=(a744+a765); + a747=(a747/a744); + a747=(a747*a881); + a759=(a759*a747); + a760=(a760/a744); + a760=(a760*a881); + a757=(a757*a760); + a759=(a759+a757); + a454=(a454*a759); + a724=cos(a724); + a753=(a753*a760); + a761=(a761*a747); + a753=(a753-a761); + a724=(a724*a753); + a454=(a454+a724); + a454=(a517*a454); + a724=(a521*a454); + a784=(a784+a724); + a724=(a452*a454); + a753=sin(a751); + a283=(a517*a283); + a761=(a336*a283); + a753=(a753*a761); + a751=cos(a751); + a291=(a517*a291); + a761=(a336*a291); + a751=(a751*a761); + a753=(a753-a751); + a753=(a529*a753); + a724=(a724+a753); + a724=(a521*a724); + a784=(a784+a724); + a724=(a452*a454); + a753=sin(a748); + a751=(a452*a283); + a751=(a336*a751); + a753=(a753*a751); + a748=cos(a748); + a751=(a452*a291); + a751=(a336*a751); + a748=(a748*a751); + a753=(a753-a748); + a753=(a520*a753); + a724=(a724+a753); + a724=(a521*a724); + a784=(a784+a724); + a724=sin(a745); + a283=(a452*a283); + a283=(a336*a283); + a724=(a724*a283); + a745=cos(a745); + a291=(a452*a291); + a336=(a336*a291); + a745=(a745*a336); + a724=(a724-a745); + a724=(a520*a724); + a454=(a454+a724); + a454=(a521*a454); + a784=(a784+a454); + a784=(a521*a784); + a454=sin(a704); + a724=casadi_sq(a727); + a745=casadi_sq(a740); + a724=(a724+a745); + a727=(a727/a724); + a727=(a727*a878); + a739=(a739*a727); + a740=(a740/a724); + a740=(a740*a878); + a737=(a737*a740); + a739=(a739+a737); + a454=(a454*a739); + a704=cos(a704); + a733=(a733*a740); + a741=(a741*a727); + a733=(a733-a741); + a704=(a704*a733); + a454=(a454+a704); + a454=(a517*a454); + a704=(a521*a454); + a784=(a784+a704); + a704=(a452*a454); + a733=sin(a731); + a253=(a517*a253); + a741=(a306*a253); + a733=(a733*a741); + a731=cos(a731); + a261=(a517*a261); + a741=(a306*a261); + a731=(a731*a741); + a733=(a733-a731); + a733=(a529*a733); + a704=(a704+a733); + a704=(a521*a704); + a784=(a784+a704); + a704=(a452*a454); + a733=sin(a728); + a731=(a452*a253); + a731=(a306*a731); + a733=(a733*a731); + a728=cos(a728); + a731=(a452*a261); + a731=(a306*a731); + a728=(a728*a731); + a733=(a733-a728); + a733=(a520*a733); + a704=(a704+a733); + a704=(a521*a704); + a784=(a784+a704); + a704=sin(a725); + a253=(a452*a253); + a253=(a306*a253); + a704=(a704*a253); + a725=cos(a725); + a261=(a452*a261); + a306=(a306*a261); + a725=(a725*a306); + a704=(a704-a725); + a704=(a520*a704); + a454=(a454+a704); + a454=(a521*a454); + a784=(a784+a454); + a784=(a521*a784); + a454=sin(a684); + a704=casadi_sq(a707); + a725=casadi_sq(a720); + a704=(a704+a725); + a707=(a707/a704); + a707=(a707*a875); + a719=(a719*a707); + a720=(a720/a704); + a720=(a720*a875); + a717=(a717*a720); + a719=(a719+a717); + a454=(a454*a719); + a684=cos(a684); + a713=(a713*a720); + a721=(a721*a707); + a713=(a713-a721); + a684=(a684*a713); + a454=(a454+a684); + a454=(a517*a454); + a684=(a521*a454); + a784=(a784+a684); + a684=(a452*a454); + a713=sin(a711); + a223=(a517*a223); + a721=(a276*a223); + a713=(a713*a721); + a711=cos(a711); + a231=(a517*a231); + a721=(a276*a231); + a711=(a711*a721); + a713=(a713-a711); + a713=(a529*a713); + a684=(a684+a713); + a684=(a521*a684); + a784=(a784+a684); + a684=(a452*a454); + a713=sin(a708); + a711=(a452*a223); + a711=(a276*a711); + a713=(a713*a711); + a708=cos(a708); + a711=(a452*a231); + a711=(a276*a711); + a708=(a708*a711); + a713=(a713-a708); + a713=(a520*a713); + a684=(a684+a713); + a684=(a521*a684); + a784=(a784+a684); + a684=sin(a705); + a223=(a452*a223); + a223=(a276*a223); + a684=(a684*a223); + a705=cos(a705); + a231=(a452*a231); + a276=(a276*a231); + a705=(a705*a276); + a684=(a684-a705); + a684=(a520*a684); + a454=(a454+a684); + a454=(a521*a454); + a784=(a784+a454); + a784=(a521*a784); + a454=sin(a664); + a684=casadi_sq(a687); + a705=casadi_sq(a700); + a684=(a684+a705); + a687=(a687/a684); + a687=(a687*a872); + a699=(a699*a687); + a700=(a700/a684); + a700=(a700*a872); + a697=(a697*a700); + a699=(a699+a697); + a454=(a454*a699); + a664=cos(a664); + a693=(a693*a700); + a701=(a701*a687); + a693=(a693-a701); + a664=(a664*a693); + a454=(a454+a664); + a454=(a517*a454); + a664=(a521*a454); + a784=(a784+a664); + a664=(a452*a454); + a693=sin(a691); + a193=(a517*a193); + a701=(a246*a193); + a693=(a693*a701); + a691=cos(a691); + a201=(a517*a201); + a701=(a246*a201); + a691=(a691*a701); + a693=(a693-a691); + a693=(a529*a693); + a664=(a664+a693); + a664=(a521*a664); + a784=(a784+a664); + a664=(a452*a454); + a693=sin(a688); + a691=(a452*a193); + a691=(a246*a691); + a693=(a693*a691); + a688=cos(a688); + a691=(a452*a201); + a691=(a246*a691); + a688=(a688*a691); + a693=(a693-a688); + a693=(a520*a693); + a664=(a664+a693); + a664=(a521*a664); + a784=(a784+a664); + a664=sin(a685); + a193=(a452*a193); + a193=(a246*a193); + a664=(a664*a193); + a685=cos(a685); + a201=(a452*a201); + a246=(a246*a201); + a685=(a685*a246); + a664=(a664-a685); + a664=(a520*a664); + a454=(a454+a664); + a454=(a521*a454); + a784=(a784+a454); + a784=(a521*a784); + a454=sin(a644); + a664=casadi_sq(a667); + a685=casadi_sq(a680); + a664=(a664+a685); + a667=(a667/a664); + a667=(a667*a869); + a679=(a679*a667); + a680=(a680/a664); + a680=(a680*a869); + a677=(a677*a680); + a679=(a679+a677); + a454=(a454*a679); + a644=cos(a644); + a673=(a673*a680); + a681=(a681*a667); + a673=(a673-a681); + a644=(a644*a673); + a454=(a454+a644); + a454=(a517*a454); + a644=(a521*a454); + a784=(a784+a644); + a644=(a452*a454); + a673=sin(a671); + a163=(a517*a163); + a681=(a216*a163); + a673=(a673*a681); + a671=cos(a671); + a171=(a517*a171); + a681=(a216*a171); + a671=(a671*a681); + a673=(a673-a671); + a673=(a529*a673); + a644=(a644+a673); + a644=(a521*a644); + a784=(a784+a644); + a644=(a452*a454); + a673=sin(a668); + a671=(a452*a163); + a671=(a216*a671); + a673=(a673*a671); + a668=cos(a668); + a671=(a452*a171); + a671=(a216*a671); + a668=(a668*a671); + a673=(a673-a668); + a673=(a520*a673); + a644=(a644+a673); + a644=(a521*a644); + a784=(a784+a644); + a644=sin(a665); + a163=(a452*a163); + a163=(a216*a163); + a644=(a644*a163); + a665=cos(a665); + a171=(a452*a171); + a216=(a216*a171); + a665=(a665*a216); + a644=(a644-a665); + a644=(a520*a644); + a454=(a454+a644); + a454=(a521*a454); + a784=(a784+a454); + a784=(a521*a784); + a454=sin(a624); + a644=casadi_sq(a647); + a665=casadi_sq(a660); + a644=(a644+a665); + a647=(a647/a644); + a647=(a647*a866); + a659=(a659*a647); + a660=(a660/a644); + a660=(a660*a866); + a657=(a657*a660); + a659=(a659+a657); + a454=(a454*a659); + a624=cos(a624); + a653=(a653*a660); + a661=(a661*a647); + a653=(a653-a661); + a624=(a624*a653); + a454=(a454+a624); + a454=(a517*a454); + a624=(a521*a454); + a784=(a784+a624); + a624=(a452*a454); + a653=sin(a651); + a133=(a517*a133); + a661=(a186*a133); + a653=(a653*a661); + a651=cos(a651); + a141=(a517*a141); + a661=(a186*a141); + a651=(a651*a661); + a653=(a653-a651); + a653=(a529*a653); + a624=(a624+a653); + a624=(a521*a624); + a784=(a784+a624); + a624=(a452*a454); + a653=sin(a648); + a651=(a452*a133); + a651=(a186*a651); + a653=(a653*a651); + a648=cos(a648); + a651=(a452*a141); + a651=(a186*a651); + a648=(a648*a651); + a653=(a653-a648); + a653=(a520*a653); + a624=(a624+a653); + a624=(a521*a624); + a784=(a784+a624); + a624=sin(a645); + a133=(a452*a133); + a133=(a186*a133); + a624=(a624*a133); + a645=cos(a645); + a141=(a452*a141); + a186=(a186*a141); + a645=(a645*a186); + a624=(a624-a645); + a624=(a520*a624); + a454=(a454+a624); + a454=(a521*a454); + a784=(a784+a454); + a784=(a521*a784); + a454=sin(a604); + a624=casadi_sq(a627); + a645=casadi_sq(a640); + a624=(a624+a645); + a627=(a627/a624); + a627=(a627*a863); + a639=(a639*a627); + a640=(a640/a624); + a640=(a640*a863); + a637=(a637*a640); + a639=(a639+a637); + a454=(a454*a639); + a604=cos(a604); + a633=(a633*a640); + a641=(a641*a627); + a633=(a633-a641); + a604=(a604*a633); + a454=(a454+a604); + a454=(a517*a454); + a604=(a521*a454); + a784=(a784+a604); + a604=(a452*a454); + a633=sin(a631); + a103=(a517*a103); + a641=(a156*a103); + a633=(a633*a641); + a631=cos(a631); + a111=(a517*a111); + a641=(a156*a111); + a631=(a631*a641); + a633=(a633-a631); + a633=(a529*a633); + a604=(a604+a633); + a604=(a521*a604); + a784=(a784+a604); + a604=(a452*a454); + a633=sin(a628); + a631=(a452*a103); + a631=(a156*a631); + a633=(a633*a631); + a628=cos(a628); + a631=(a452*a111); + a631=(a156*a631); + a628=(a628*a631); + a633=(a633-a628); + a633=(a520*a633); + a604=(a604+a633); + a604=(a521*a604); + a784=(a784+a604); + a604=sin(a625); + a103=(a452*a103); + a103=(a156*a103); + a604=(a604*a103); + a625=cos(a625); + a111=(a452*a111); + a156=(a156*a111); + a625=(a625*a156); + a604=(a604-a625); + a604=(a520*a604); + a454=(a454+a604); + a454=(a521*a454); + a784=(a784+a454); + a784=(a521*a784); + a454=sin(a584); + a604=casadi_sq(a607); + a625=casadi_sq(a620); + a604=(a604+a625); + a607=(a607/a604); + a607=(a607*a860); + a619=(a619*a607); + a620=(a620/a604); + a620=(a620*a860); + a617=(a617*a620); + a619=(a619+a617); + a454=(a454*a619); + a584=cos(a584); + a613=(a613*a620); + a621=(a621*a607); + a613=(a613-a621); + a584=(a584*a613); + a454=(a454+a584); + a454=(a517*a454); + a584=(a521*a454); + a784=(a784+a584); + a584=(a452*a454); + a613=sin(a611); + a73=(a517*a73); + a621=(a126*a73); + a613=(a613*a621); + a611=cos(a611); + a81=(a517*a81); + a621=(a126*a81); + a611=(a611*a621); + a613=(a613-a611); + a613=(a529*a613); + a584=(a584+a613); + a584=(a521*a584); + a784=(a784+a584); + a584=(a452*a454); + a613=sin(a608); + a611=(a452*a73); + a611=(a126*a611); + a613=(a613*a611); + a608=cos(a608); + a611=(a452*a81); + a611=(a126*a611); + a608=(a608*a611); + a613=(a613-a608); + a613=(a520*a613); + a584=(a584+a613); + a584=(a521*a584); + a784=(a784+a584); + a584=sin(a605); + a73=(a452*a73); + a73=(a126*a73); + a584=(a584*a73); + a605=cos(a605); + a81=(a452*a81); + a126=(a126*a81); + a605=(a605*a126); + a584=(a584-a605); + a584=(a520*a584); + a454=(a454+a584); + a454=(a521*a454); + a784=(a784+a454); + a784=(a521*a784); + a454=sin(a564); + a584=casadi_sq(a587); + a605=casadi_sq(a600); + a584=(a584+a605); + a587=(a587/a584); + a587=(a587*a857); + a599=(a599*a587); + a600=(a600/a584); + a600=(a600*a857); + a597=(a597*a600); + a599=(a599+a597); + a454=(a454*a599); + a564=cos(a564); + a593=(a593*a600); + a601=(a601*a587); + a593=(a593-a601); + a564=(a564*a593); + a454=(a454+a564); + a454=(a517*a454); + a564=(a521*a454); + a784=(a784+a564); + a564=(a452*a454); + a593=sin(a591); + a43=(a517*a43); + a601=(a96*a43); + a593=(a593*a601); + a591=cos(a591); + a51=(a517*a51); + a601=(a96*a51); + a591=(a591*a601); + a593=(a593-a591); + a593=(a529*a593); + a564=(a564+a593); + a564=(a521*a564); + a784=(a784+a564); + a564=(a452*a454); + a593=sin(a588); + a591=(a452*a43); + a591=(a96*a591); + a593=(a593*a591); + a588=cos(a588); + a591=(a452*a51); + a591=(a96*a591); + a588=(a588*a591); + a593=(a593-a588); + a593=(a520*a593); + a564=(a564+a593); + a564=(a521*a564); + a784=(a784+a564); + a564=sin(a585); + a43=(a452*a43); + a43=(a96*a43); + a564=(a564*a43); + a585=cos(a585); + a51=(a452*a51); + a96=(a96*a51); + a585=(a585*a96); + a564=(a564-a585); + a564=(a520*a564); + a454=(a454+a564); + a454=(a521*a454); + a784=(a784+a454); + a784=(a521*a784); + a454=sin(a544); + a564=casadi_sq(a567); + a585=casadi_sq(a580); + a564=(a564+a585); + a567=(a567/a564); + a567=(a567*a854); + a579=(a579*a567); + a580=(a580/a564); + a580=(a580*a854); + a577=(a577*a580); + a579=(a579+a577); + a454=(a454*a579); + a544=cos(a544); + a573=(a573*a580); + a581=(a581*a567); + a573=(a573-a581); + a544=(a544*a573); + a454=(a454+a544); + a454=(a517*a454); + a544=(a521*a454); + a784=(a784+a544); + a544=(a452*a454); + a573=sin(a571); + a13=(a517*a13); + a581=(a66*a13); + a573=(a573*a581); + a571=cos(a571); + a21=(a517*a21); + a581=(a66*a21); + a571=(a571*a581); + a573=(a573-a571); + a573=(a529*a573); + a544=(a544+a573); + a544=(a521*a544); + a784=(a784+a544); + a544=(a452*a454); + a573=sin(a568); + a571=(a452*a13); + a571=(a66*a571); + a573=(a573*a571); + a568=cos(a568); + a571=(a452*a21); + a571=(a66*a571); + a568=(a568*a571); + a573=(a573-a568); + a573=(a520*a573); + a544=(a544+a573); + a544=(a521*a544); + a784=(a784+a544); + a544=sin(a565); + a13=(a452*a13); + a13=(a66*a13); + a544=(a544*a13); + a565=cos(a565); + a21=(a452*a21); + a66=(a66*a21); + a565=(a565*a66); + a544=(a544-a565); + a544=(a520*a544); + a454=(a454+a544); + a454=(a521*a454); + a784=(a784+a454); + a784=(a521*a784); + a454=sin(a457); + a544=casadi_sq(a547); + a565=casadi_sq(a560); + a544=(a544+a565); + a547=(a547/a544); + a547=(a547*a844); + a559=(a559*a547); + a560=(a560/a544); + a560=(a560*a844); + a557=(a557*a560); + a559=(a559+a557); + a454=(a454*a559); + a457=cos(a457); + a553=(a553*a560); + a561=(a561*a547); + a553=(a553-a561); + a457=(a457*a553); + a454=(a454+a457); + a454=(a517*a454); + a457=(a521*a454); + a784=(a784+a457); + a457=(a452*a454); + a553=sin(a551); + a839=(a517*a839); + a561=(a36*a839); + a553=(a553*a561); + a551=cos(a551); + a841=(a517*a841); + a561=(a36*a841); + a551=(a551*a561); + a553=(a553-a551); + a553=(a529*a553); + a457=(a457+a553); + a457=(a521*a457); + a784=(a784+a457); + a457=(a452*a454); + a553=sin(a548); + a551=(a452*a839); + a551=(a36*a551); + a553=(a553*a551); + a548=cos(a548); + a551=(a452*a841); + a551=(a36*a551); + a548=(a548*a551); + a553=(a553-a548); + a553=(a520*a553); + a457=(a457+a553); + a457=(a521*a457); + a784=(a784+a457); + a457=sin(a545); + a839=(a452*a839); + a839=(a36*a839); + a457=(a457*a839); + a545=cos(a545); + a841=(a452*a841); + a36=(a36*a841); + a545=(a545*a36); + a457=(a457-a545); + a457=(a520*a457); + a454=(a454+a457); + a454=(a521*a454); + a784=(a784+a454); + a784=(a521*a784); + a454=sin(a522); + a457=casadi_sq(a526); + a545=casadi_sq(a540); + a457=(a457+a545); + a526=(a526/a457); + a526=(a526*a827); + a539=(a539*a526); + a540=(a540/a457); + a540=(a540*a827); + a537=(a537*a540); + a539=(a539+a537); + a454=(a454*a539); + a522=cos(a522); + a533=(a533*a540); + a541=(a541*a526); + a533=(a533-a541); + a522=(a522*a533); + a454=(a454+a522); + a454=(a517*a454); + a522=(a521*a454); + a784=(a784+a522); + a522=(a452*a454); + a533=sin(a531); + a822=(a517*a822); + a541=(a10*a822); + a533=(a533*a541); + a531=cos(a531); + a517=(a517*a823); + a823=(a10*a517); + a531=(a531*a823); + a533=(a533-a531); + a529=(a529*a533); + a522=(a522+a529); + a522=(a521*a522); + a784=(a784+a522); + a522=(a452*a454); + a529=sin(a527); + a533=(a452*a822); + a533=(a10*a533); + a529=(a529*a533); + a527=cos(a527); + a533=(a452*a517); + a533=(a10*a533); + a527=(a527*a533); + a529=(a529-a527); + a529=(a520*a529); + a522=(a522+a529); + a522=(a521*a522); + a784=(a784+a522); + a522=sin(a524); + a822=(a452*a822); + a822=(a10*a822); + a522=(a522*a822); + a524=cos(a524); + a452=(a452*a517); + a10=(a10*a452); + a524=(a524*a10); + a522=(a522-a524); + a520=(a520*a522); + a454=(a454+a520); + a521=(a521*a454); + a784=(a784+a521); + a458=(a458*a840); + a784=(a784-a458); + a459=(a459*a840); + a456=(a456*a459); + a784=(a784-a456); + if (res[3]!=0) res[3][49]=a784; + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_grad(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f3(arg, res, iw, w, mem); +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_grad_alloc_mem(void) { + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_grad_init_mem(int mem) { + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_grad_free_mem(int mem) { +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_grad_checkout(void) { + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_grad_release(int mem) { +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_grad_incref(void) { +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_grad_decref(void) { +} + +extern "C" CASADI_SYMBOL_EXPORT casadi_int nlp_grad_n_in(void) { return 4;} + +extern "C" CASADI_SYMBOL_EXPORT casadi_int nlp_grad_n_out(void) { return 4;} + +extern "C" CASADI_SYMBOL_EXPORT casadi_real nlp_grad_default_in(casadi_int i) { + switch (i) { + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const char* nlp_grad_name_in(casadi_int i) { + switch (i) { + case 0: return "x"; + case 1: return "p"; + case 2: return "lam_f"; + case 3: return "lam_g"; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const char* nlp_grad_name_out(casadi_int i) { + switch (i) { + case 0: return "f"; + case 1: return "g"; + case 2: return "grad_gamma_x"; + case 3: return "grad_gamma_p"; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const casadi_int* nlp_grad_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s1; + case 2: return casadi_s2; + case 3: return casadi_s3; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const casadi_int* nlp_grad_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s2; + case 1: return casadi_s3; + case 2: return casadi_s0; + case 3: return casadi_s1; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_grad_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 4; + if (sz_res) *sz_res = 4; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 0; + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_grad_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 4*sizeof(const casadi_real*); + if (sz_res) *sz_res = 4*sizeof(casadi_real*); + if (sz_iw) *sz_iw = 0*sizeof(casadi_int); + if (sz_w) *sz_w = 0*sizeof(casadi_real); + return 0; +} + +/* nlp_grad_f:(x[78],p[50])->(f,grad_f_x[78]) */ +static int casadi_f4(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_real a0, a1, a10, a100, a101, a102, a103, a104, a105, a106, a107, a108, a109, a11, a110, a111, a112, a113, a114, a115, a116, a117, a118, a119, a12, a120, a121, a122, a123, a124, a125, a126, a127, a128, a129, a13, a130, a131, a132, a133, a134, a135, a136, a137, a138, a139, a14, a140, a141, a142, a143, a144, a145, a146, a147, a148, a149, a15, a150, a151, a152, a153, a154, a155, a156, a157, a158, a159, a16, a160, a161, a162, a163, a164, a165, a166, a167, a168, a169, a17, a170, a171, a172, a173, a174, a175, a176, a177, a178, a179, a18, a180, a181, a182, a183, a184, a185, a186, a187, a188, a189, a19, a190, a191, a192, a193, a194, a195, a196, a197, a198, a199, a2, a20, a200, a201, a202, a203, a204, a205, a206, a207, a208, a209, a21, a210, a211, a212, a213, a214, a215, a216, a217, a218, a219, a22, a220, a221, a222, a223, a224, a225, a226, a227, a228, a229, a23, a230, a231, a232, a233, a234, a235, a236, a237, a238, a239, a24, a240, a241, a242, a243, a244, a245, a246, a247, a248, a249, a25, a250, a251, a252, a253, a254, a255, a256, a257, a258, a259, a26, a260, a261, a262, a263, a264, a265, a266, a267, a268, a269, a27, a270, a271, a272, a273, a274, a275, a276, a277, a278, a279, a28, a280, a281, a282, a283, a284, a285, a286, a287, a288, a289, a29, a290, a291, a292, a293, a294, a295, a296, a297, a298, a299, a3, a30, a300, a301, a302, a303, a304, a305, a306, a307, a308, a309, a31, a310, a311, a312, a313, a314, a315, a316, a317, a318, a319, a32, a320, a321, a322, a323, a324, a325, a326, a327, a328, a329, a33, a330, a331, a332, a333, a334, a335, a336, a337, a338, a339, a34, a340, a341, a342, a343, a344, a345, a346, a347, a348, a349, a35, a350, a351, a352, a353, a354, a355, a356, a357, a358, a359, a36, a360, a361, a362, a363, a364, a365, a366, a367, a368, a369, a37, a370, a371, a372, a373, a374, a375, a376, a377, a378, a379, a38, a380, a381, a382, a383, a384, a385, a386, a387, a388, a389, a39, a390, a391, a392, a393, a4, a40, a41, a42, a43, a44, a45, a46, a47, a48, a49, a5, a50, a51, a52, a53, a54, a55, a56, a57, a58, a59, a6, a60, a61, a62, a63, a64, a65, a66, a67, a68, a69, a7, a70, a71, a72, a73, a74, a75, a76, a77, a78, a79, a8, a80, a81, a82, a83, a84, a85, a86, a87, a88, a89, a9, a90, a91, a92, a93, a94, a95, a96, a97, a98, a99; + a0=10.; + a1=arg[0]? arg[0][3] : 0; + a2=arg[1]? arg[1][3] : 0; + a3=(a1-a2); + a3=(a0*a3); + a1=(a1-a2); + a2=(a3*a1); + a4=arg[0]? arg[0][4] : 0; + a5=arg[1]? arg[1][4] : 0; + a6=(a4-a5); + a6=(a0*a6); + a4=(a4-a5); + a5=(a6*a4); + a2=(a2+a5); + a5=arg[0]? arg[0][48] : 0; + a7=casadi_sq(a5); + a8=arg[0]? arg[0][49] : 0; + a9=casadi_sq(a8); + a7=(a7+a9); + a2=(a2+a7); + a7=arg[1]? arg[1][5] : 0; + a9=cos(a7); + a10=arg[0]? arg[0][5] : 0; + a11=sin(a10); + a12=(a9*a11); + a13=sin(a7); + a14=cos(a10); + a15=(a13*a14); + a12=(a12-a15); + a15=cos(a7); + a14=(a15*a14); + a16=sin(a7); + a11=(a16*a11); + a14=(a14+a11); + a11=atan2(a12,a14); + a17=cos(a7); + a18=sin(a10); + a19=(a17*a18); + a20=sin(a7); + a21=cos(a10); + a22=(a20*a21); + a19=(a19-a22); + a22=cos(a7); + a21=(a22*a21); + a7=sin(a7); + a18=(a7*a18); + a21=(a21+a18); + a18=atan2(a19,a21); + a23=(a11*a18); + a2=(a2+a23); + a23=arg[0]? arg[0][6] : 0; + a24=arg[1]? arg[1][6] : 0; + a25=(a23-a24); + a25=(a0*a25); + a23=(a23-a24); + a24=(a25*a23); + a26=arg[0]? arg[0][7] : 0; + a27=arg[1]? arg[1][7] : 0; + a28=(a26-a27); + a28=(a0*a28); + a26=(a26-a27); + a27=(a28*a26); + a24=(a24+a27); + a2=(a2+a24); + a24=arg[0]? arg[0][50] : 0; + a27=casadi_sq(a24); + a29=arg[0]? arg[0][51] : 0; + a30=casadi_sq(a29); + a27=(a27+a30); + a2=(a2+a27); + a27=arg[1]? arg[1][8] : 0; + a30=cos(a27); + a31=arg[0]? arg[0][8] : 0; + a32=sin(a31); + a33=(a30*a32); + a34=sin(a27); + a35=cos(a31); + a36=(a34*a35); + a33=(a33-a36); + a36=cos(a27); + a35=(a36*a35); + a37=sin(a27); + a32=(a37*a32); + a35=(a35+a32); + a32=atan2(a33,a35); + a38=cos(a27); + a39=sin(a31); + a40=(a38*a39); + a41=sin(a27); + a42=cos(a31); + a43=(a41*a42); + a40=(a40-a43); + a43=cos(a27); + a42=(a43*a42); + a27=sin(a27); + a39=(a27*a39); + a42=(a42+a39); + a39=atan2(a40,a42); + a44=(a32*a39); + a2=(a2+a44); + a44=arg[0]? arg[0][9] : 0; + a45=arg[1]? arg[1][9] : 0; + a46=(a44-a45); + a46=(a0*a46); + a44=(a44-a45); + a45=(a46*a44); + a47=arg[0]? arg[0][10] : 0; + a48=arg[1]? arg[1][10] : 0; + a49=(a47-a48); + a49=(a0*a49); + a47=(a47-a48); + a48=(a49*a47); + a45=(a45+a48); + a2=(a2+a45); + a45=arg[0]? arg[0][52] : 0; + a48=casadi_sq(a45); + a50=arg[0]? arg[0][53] : 0; + a51=casadi_sq(a50); + a48=(a48+a51); + a2=(a2+a48); + a48=arg[1]? arg[1][11] : 0; + a51=cos(a48); + a52=arg[0]? arg[0][11] : 0; + a53=sin(a52); + a54=(a51*a53); + a55=sin(a48); + a56=cos(a52); + a57=(a55*a56); + a54=(a54-a57); + a57=cos(a48); + a56=(a57*a56); + a58=sin(a48); + a53=(a58*a53); + a56=(a56+a53); + a53=atan2(a54,a56); + a59=cos(a48); + a60=sin(a52); + a61=(a59*a60); + a62=sin(a48); + a63=cos(a52); + a64=(a62*a63); + a61=(a61-a64); + a64=cos(a48); + a63=(a64*a63); + a48=sin(a48); + a60=(a48*a60); + a63=(a63+a60); + a60=atan2(a61,a63); + a65=(a53*a60); + a2=(a2+a65); + a65=arg[0]? arg[0][12] : 0; + a66=arg[1]? arg[1][12] : 0; + a67=(a65-a66); + a67=(a0*a67); + a65=(a65-a66); + a66=(a67*a65); + a68=arg[0]? arg[0][13] : 0; + a69=arg[1]? arg[1][13] : 0; + a70=(a68-a69); + a70=(a0*a70); + a68=(a68-a69); + a69=(a70*a68); + a66=(a66+a69); + a2=(a2+a66); + a66=arg[0]? arg[0][54] : 0; + a69=casadi_sq(a66); + a71=arg[0]? arg[0][55] : 0; + a72=casadi_sq(a71); + a69=(a69+a72); + a2=(a2+a69); + a69=arg[1]? arg[1][14] : 0; + a72=cos(a69); + a73=arg[0]? arg[0][14] : 0; + a74=sin(a73); + a75=(a72*a74); + a76=sin(a69); + a77=cos(a73); + a78=(a76*a77); + a75=(a75-a78); + a78=cos(a69); + a77=(a78*a77); + a79=sin(a69); + a74=(a79*a74); + a77=(a77+a74); + a74=atan2(a75,a77); + a80=cos(a69); + a81=sin(a73); + a82=(a80*a81); + a83=sin(a69); + a84=cos(a73); + a85=(a83*a84); + a82=(a82-a85); + a85=cos(a69); + a84=(a85*a84); + a69=sin(a69); + a81=(a69*a81); + a84=(a84+a81); + a81=atan2(a82,a84); + a86=(a74*a81); + a2=(a2+a86); + a86=arg[0]? arg[0][15] : 0; + a87=arg[1]? arg[1][15] : 0; + a88=(a86-a87); + a88=(a0*a88); + a86=(a86-a87); + a87=(a88*a86); + a89=arg[0]? arg[0][16] : 0; + a90=arg[1]? arg[1][16] : 0; + a91=(a89-a90); + a91=(a0*a91); + a89=(a89-a90); + a90=(a91*a89); + a87=(a87+a90); + a2=(a2+a87); + a87=arg[0]? arg[0][56] : 0; + a90=casadi_sq(a87); + a92=arg[0]? arg[0][57] : 0; + a93=casadi_sq(a92); + a90=(a90+a93); + a2=(a2+a90); + a90=arg[1]? arg[1][17] : 0; + a93=cos(a90); + a94=arg[0]? arg[0][17] : 0; + a95=sin(a94); + a96=(a93*a95); + a97=sin(a90); + a98=cos(a94); + a99=(a97*a98); + a96=(a96-a99); + a99=cos(a90); + a98=(a99*a98); + a100=sin(a90); + a95=(a100*a95); + a98=(a98+a95); + a95=atan2(a96,a98); + a101=cos(a90); + a102=sin(a94); + a103=(a101*a102); + a104=sin(a90); + a105=cos(a94); + a106=(a104*a105); + a103=(a103-a106); + a106=cos(a90); + a105=(a106*a105); + a90=sin(a90); + a102=(a90*a102); + a105=(a105+a102); + a102=atan2(a103,a105); + a107=(a95*a102); + a2=(a2+a107); + a107=arg[0]? arg[0][18] : 0; + a108=arg[1]? arg[1][18] : 0; + a109=(a107-a108); + a109=(a0*a109); + a107=(a107-a108); + a108=(a109*a107); + a110=arg[0]? arg[0][19] : 0; + a111=arg[1]? arg[1][19] : 0; + a112=(a110-a111); + a112=(a0*a112); + a110=(a110-a111); + a111=(a112*a110); + a108=(a108+a111); + a2=(a2+a108); + a108=arg[0]? arg[0][58] : 0; + a111=casadi_sq(a108); + a113=arg[0]? arg[0][59] : 0; + a114=casadi_sq(a113); + a111=(a111+a114); + a2=(a2+a111); + a111=arg[1]? arg[1][20] : 0; + a114=cos(a111); + a115=arg[0]? arg[0][20] : 0; + a116=sin(a115); + a117=(a114*a116); + a118=sin(a111); + a119=cos(a115); + a120=(a118*a119); + a117=(a117-a120); + a120=cos(a111); + a119=(a120*a119); + a121=sin(a111); + a116=(a121*a116); + a119=(a119+a116); + a116=atan2(a117,a119); + a122=cos(a111); + a123=sin(a115); + a124=(a122*a123); + a125=sin(a111); + a126=cos(a115); + a127=(a125*a126); + a124=(a124-a127); + a127=cos(a111); + a126=(a127*a126); + a111=sin(a111); + a123=(a111*a123); + a126=(a126+a123); + a123=atan2(a124,a126); + a128=(a116*a123); + a2=(a2+a128); + a128=arg[0]? arg[0][21] : 0; + a129=arg[1]? arg[1][21] : 0; + a130=(a128-a129); + a130=(a0*a130); + a128=(a128-a129); + a129=(a130*a128); + a131=arg[0]? arg[0][22] : 0; + a132=arg[1]? arg[1][22] : 0; + a133=(a131-a132); + a133=(a0*a133); + a131=(a131-a132); + a132=(a133*a131); + a129=(a129+a132); + a2=(a2+a129); + a129=arg[0]? arg[0][60] : 0; + a132=casadi_sq(a129); + a134=arg[0]? arg[0][61] : 0; + a135=casadi_sq(a134); + a132=(a132+a135); + a2=(a2+a132); + a132=arg[1]? arg[1][23] : 0; + a135=cos(a132); + a136=arg[0]? arg[0][23] : 0; + a137=sin(a136); + a138=(a135*a137); + a139=sin(a132); + a140=cos(a136); + a141=(a139*a140); + a138=(a138-a141); + a141=cos(a132); + a140=(a141*a140); + a142=sin(a132); + a137=(a142*a137); + a140=(a140+a137); + a137=atan2(a138,a140); + a143=cos(a132); + a144=sin(a136); + a145=(a143*a144); + a146=sin(a132); + a147=cos(a136); + a148=(a146*a147); + a145=(a145-a148); + a148=cos(a132); + a147=(a148*a147); + a132=sin(a132); + a144=(a132*a144); + a147=(a147+a144); + a144=atan2(a145,a147); + a149=(a137*a144); + a2=(a2+a149); + a149=arg[0]? arg[0][24] : 0; + a150=arg[1]? arg[1][24] : 0; + a151=(a149-a150); + a151=(a0*a151); + a149=(a149-a150); + a150=(a151*a149); + a152=arg[0]? arg[0][25] : 0; + a153=arg[1]? arg[1][25] : 0; + a154=(a152-a153); + a154=(a0*a154); + a152=(a152-a153); + a153=(a154*a152); + a150=(a150+a153); + a2=(a2+a150); + a150=arg[0]? arg[0][62] : 0; + a153=casadi_sq(a150); + a155=arg[0]? arg[0][63] : 0; + a156=casadi_sq(a155); + a153=(a153+a156); + a2=(a2+a153); + a153=arg[1]? arg[1][26] : 0; + a156=cos(a153); + a157=arg[0]? arg[0][26] : 0; + a158=sin(a157); + a159=(a156*a158); + a160=sin(a153); + a161=cos(a157); + a162=(a160*a161); + a159=(a159-a162); + a162=cos(a153); + a161=(a162*a161); + a163=sin(a153); + a158=(a163*a158); + a161=(a161+a158); + a158=atan2(a159,a161); + a164=cos(a153); + a165=sin(a157); + a166=(a164*a165); + a167=sin(a153); + a168=cos(a157); + a169=(a167*a168); + a166=(a166-a169); + a169=cos(a153); + a168=(a169*a168); + a153=sin(a153); + a165=(a153*a165); + a168=(a168+a165); + a165=atan2(a166,a168); + a170=(a158*a165); + a2=(a2+a170); + a170=arg[0]? arg[0][27] : 0; + a171=arg[1]? arg[1][27] : 0; + a172=(a170-a171); + a172=(a0*a172); + a170=(a170-a171); + a171=(a172*a170); + a173=arg[0]? arg[0][28] : 0; + a174=arg[1]? arg[1][28] : 0; + a175=(a173-a174); + a175=(a0*a175); + a173=(a173-a174); + a174=(a175*a173); + a171=(a171+a174); + a2=(a2+a171); + a171=arg[0]? arg[0][64] : 0; + a174=casadi_sq(a171); + a176=arg[0]? arg[0][65] : 0; + a177=casadi_sq(a176); + a174=(a174+a177); + a2=(a2+a174); + a174=arg[1]? arg[1][29] : 0; + a177=cos(a174); + a178=arg[0]? arg[0][29] : 0; + a179=sin(a178); + a180=(a177*a179); + a181=sin(a174); + a182=cos(a178); + a183=(a181*a182); + a180=(a180-a183); + a183=cos(a174); + a182=(a183*a182); + a184=sin(a174); + a179=(a184*a179); + a182=(a182+a179); + a179=atan2(a180,a182); + a185=cos(a174); + a186=sin(a178); + a187=(a185*a186); + a188=sin(a174); + a189=cos(a178); + a190=(a188*a189); + a187=(a187-a190); + a190=cos(a174); + a189=(a190*a189); + a174=sin(a174); + a186=(a174*a186); + a189=(a189+a186); + a186=atan2(a187,a189); + a191=(a179*a186); + a2=(a2+a191); + a191=arg[0]? arg[0][30] : 0; + a192=arg[1]? arg[1][30] : 0; + a193=(a191-a192); + a193=(a0*a193); + a191=(a191-a192); + a192=(a193*a191); + a194=arg[0]? arg[0][31] : 0; + a195=arg[1]? arg[1][31] : 0; + a196=(a194-a195); + a196=(a0*a196); + a194=(a194-a195); + a195=(a196*a194); + a192=(a192+a195); + a2=(a2+a192); + a192=arg[0]? arg[0][66] : 0; + a195=casadi_sq(a192); + a197=arg[0]? arg[0][67] : 0; + a198=casadi_sq(a197); + a195=(a195+a198); + a2=(a2+a195); + a195=arg[1]? arg[1][32] : 0; + a198=cos(a195); + a199=arg[0]? arg[0][32] : 0; + a200=sin(a199); + a201=(a198*a200); + a202=sin(a195); + a203=cos(a199); + a204=(a202*a203); + a201=(a201-a204); + a204=cos(a195); + a203=(a204*a203); + a205=sin(a195); + a200=(a205*a200); + a203=(a203+a200); + a200=atan2(a201,a203); + a206=cos(a195); + a207=sin(a199); + a208=(a206*a207); + a209=sin(a195); + a210=cos(a199); + a211=(a209*a210); + a208=(a208-a211); + a211=cos(a195); + a210=(a211*a210); + a195=sin(a195); + a207=(a195*a207); + a210=(a210+a207); + a207=atan2(a208,a210); + a212=(a200*a207); + a2=(a2+a212); + a212=arg[0]? arg[0][33] : 0; + a213=arg[1]? arg[1][33] : 0; + a214=(a212-a213); + a214=(a0*a214); + a212=(a212-a213); + a213=(a214*a212); + a215=arg[0]? arg[0][34] : 0; + a216=arg[1]? arg[1][34] : 0; + a217=(a215-a216); + a217=(a0*a217); + a215=(a215-a216); + a216=(a217*a215); + a213=(a213+a216); + a2=(a2+a213); + a213=arg[0]? arg[0][68] : 0; + a216=casadi_sq(a213); + a218=arg[0]? arg[0][69] : 0; + a219=casadi_sq(a218); + a216=(a216+a219); + a2=(a2+a216); + a216=arg[1]? arg[1][35] : 0; + a219=cos(a216); + a220=arg[0]? arg[0][35] : 0; + a221=sin(a220); + a222=(a219*a221); + a223=sin(a216); + a224=cos(a220); + a225=(a223*a224); + a222=(a222-a225); + a225=cos(a216); + a224=(a225*a224); + a226=sin(a216); + a221=(a226*a221); + a224=(a224+a221); + a221=atan2(a222,a224); + a227=cos(a216); + a228=sin(a220); + a229=(a227*a228); + a230=sin(a216); + a231=cos(a220); + a232=(a230*a231); + a229=(a229-a232); + a232=cos(a216); + a231=(a232*a231); + a216=sin(a216); + a228=(a216*a228); + a231=(a231+a228); + a228=atan2(a229,a231); + a233=(a221*a228); + a2=(a2+a233); + a233=arg[0]? arg[0][36] : 0; + a234=arg[1]? arg[1][36] : 0; + a235=(a233-a234); + a235=(a0*a235); + a233=(a233-a234); + a234=(a235*a233); + a236=arg[0]? arg[0][37] : 0; + a237=arg[1]? arg[1][37] : 0; + a238=(a236-a237); + a238=(a0*a238); + a236=(a236-a237); + a237=(a238*a236); + a234=(a234+a237); + a2=(a2+a234); + a234=arg[0]? arg[0][70] : 0; + a237=casadi_sq(a234); + a239=arg[0]? arg[0][71] : 0; + a240=casadi_sq(a239); + a237=(a237+a240); + a2=(a2+a237); + a237=arg[1]? arg[1][38] : 0; + a240=cos(a237); + a241=arg[0]? arg[0][38] : 0; + a242=sin(a241); + a243=(a240*a242); + a244=sin(a237); + a245=cos(a241); + a246=(a244*a245); + a243=(a243-a246); + a246=cos(a237); + a245=(a246*a245); + a247=sin(a237); + a242=(a247*a242); + a245=(a245+a242); + a242=atan2(a243,a245); + a248=cos(a237); + a249=sin(a241); + a250=(a248*a249); + a251=sin(a237); + a252=cos(a241); + a253=(a251*a252); + a250=(a250-a253); + a253=cos(a237); + a252=(a253*a252); + a237=sin(a237); + a249=(a237*a249); + a252=(a252+a249); + a249=atan2(a250,a252); + a254=(a242*a249); + a2=(a2+a254); + a254=arg[0]? arg[0][39] : 0; + a255=arg[1]? arg[1][39] : 0; + a256=(a254-a255); + a256=(a0*a256); + a254=(a254-a255); + a255=(a256*a254); + a257=arg[0]? arg[0][40] : 0; + a258=arg[1]? arg[1][40] : 0; + a259=(a257-a258); + a259=(a0*a259); + a257=(a257-a258); + a258=(a259*a257); + a255=(a255+a258); + a2=(a2+a255); + a255=arg[0]? arg[0][72] : 0; + a258=casadi_sq(a255); + a260=arg[0]? arg[0][73] : 0; + a261=casadi_sq(a260); + a258=(a258+a261); + a2=(a2+a258); + a258=arg[1]? arg[1][41] : 0; + a261=cos(a258); + a262=arg[0]? arg[0][41] : 0; + a263=sin(a262); + a264=(a261*a263); + a265=sin(a258); + a266=cos(a262); + a267=(a265*a266); + a264=(a264-a267); + a267=cos(a258); + a266=(a267*a266); + a268=sin(a258); + a263=(a268*a263); + a266=(a266+a263); + a263=atan2(a264,a266); + a269=cos(a258); + a270=sin(a262); + a271=(a269*a270); + a272=sin(a258); + a273=cos(a262); + a274=(a272*a273); + a271=(a271-a274); + a274=cos(a258); + a273=(a274*a273); + a258=sin(a258); + a270=(a258*a270); + a273=(a273+a270); + a270=atan2(a271,a273); + a275=(a263*a270); + a2=(a2+a275); + a275=arg[0]? arg[0][42] : 0; + a276=arg[1]? arg[1][42] : 0; + a277=(a275-a276); + a277=(a0*a277); + a275=(a275-a276); + a276=(a277*a275); + a278=arg[0]? arg[0][43] : 0; + a279=arg[1]? arg[1][43] : 0; + a280=(a278-a279); + a280=(a0*a280); + a278=(a278-a279); + a279=(a280*a278); + a276=(a276+a279); + a2=(a2+a276); + a276=arg[0]? arg[0][74] : 0; + a279=casadi_sq(a276); + a281=arg[0]? arg[0][75] : 0; + a282=casadi_sq(a281); + a279=(a279+a282); + a2=(a2+a279); + a279=arg[1]? arg[1][44] : 0; + a282=cos(a279); + a283=arg[0]? arg[0][44] : 0; + a284=sin(a283); + a285=(a282*a284); + a286=sin(a279); + a287=cos(a283); + a288=(a286*a287); + a285=(a285-a288); + a288=cos(a279); + a287=(a288*a287); + a289=sin(a279); + a284=(a289*a284); + a287=(a287+a284); + a284=atan2(a285,a287); + a290=cos(a279); + a291=sin(a283); + a292=(a290*a291); + a293=sin(a279); + a294=cos(a283); + a295=(a293*a294); + a292=(a292-a295); + a295=cos(a279); + a294=(a295*a294); + a279=sin(a279); + a291=(a279*a291); + a294=(a294+a291); + a291=atan2(a292,a294); + a296=(a284*a291); + a2=(a2+a296); + a296=arg[0]? arg[0][45] : 0; + a297=arg[1]? arg[1][45] : 0; + a298=(a296-a297); + a298=(a0*a298); + a296=(a296-a297); + a297=(a298*a296); + a299=arg[0]? arg[0][46] : 0; + a300=arg[1]? arg[1][46] : 0; + a301=(a299-a300); + a301=(a0*a301); + a299=(a299-a300); + a300=(a301*a299); + a297=(a297+a300); + a2=(a2+a297); + a297=arg[0]? arg[0][76] : 0; + a300=casadi_sq(a297); + a302=arg[0]? arg[0][77] : 0; + a303=casadi_sq(a302); + a300=(a300+a303); + a2=(a2+a300); + a300=arg[1]? arg[1][47] : 0; + a303=cos(a300); + a304=arg[0]? arg[0][47] : 0; + a305=sin(a304); + a306=(a303*a305); + a307=sin(a300); + a308=cos(a304); + a309=(a307*a308); + a306=(a306-a309); + a309=cos(a300); + a308=(a309*a308); + a310=sin(a300); + a305=(a310*a305); + a308=(a308+a305); + a305=atan2(a306,a308); + a311=cos(a300); + a312=sin(a304); + a313=(a311*a312); + a314=sin(a300); + a315=cos(a304); + a316=(a314*a315); + a313=(a313-a316); + a316=cos(a300); + a315=(a316*a315); + a300=sin(a300); + a312=(a300*a312); + a315=(a315+a312); + a312=atan2(a313,a315); + a317=(a305*a312); + a2=(a2+a317); + a317=2.; + a318=arg[1]? arg[1][48] : 0; + a319=(a5-a318); + a319=(a317*a319); + a318=(a5-a318); + a320=(a319*a318); + a321=5.0000000000000000e-01; + a322=arg[1]? arg[1][49] : 0; + a323=(a8-a322); + a323=(a321*a323); + a322=(a8-a322); + a324=(a323*a322); + a320=(a320+a324); + a2=(a2+a320); + a320=(a24-a5); + a320=(a317*a320); + a324=(a24-a5); + a325=(a320*a324); + a326=(a29-a8); + a327=(a321*a326); + a328=(a29-a8); + a329=(a327*a328); + a325=(a325+a329); + a2=(a2+a325); + a325=(a45-a24); + a325=(a317*a325); + a329=(a45-a24); + a330=(a325*a329); + a331=(a50-a29); + a332=(a321*a331); + a333=(a50-a29); + a334=(a332*a333); + a330=(a330+a334); + a2=(a2+a330); + a330=(a66-a45); + a330=(a317*a330); + a334=(a66-a45); + a335=(a330*a334); + a336=(a71-a50); + a337=(a321*a336); + a338=(a71-a50); + a339=(a337*a338); + a335=(a335+a339); + a2=(a2+a335); + a335=(a87-a66); + a335=(a317*a335); + a339=(a87-a66); + a340=(a335*a339); + a341=(a92-a71); + a342=(a321*a341); + a343=(a92-a71); + a344=(a342*a343); + a340=(a340+a344); + a2=(a2+a340); + a340=(a108-a87); + a340=(a317*a340); + a344=(a108-a87); + a345=(a340*a344); + a346=(a113-a92); + a347=(a321*a346); + a348=(a113-a92); + a349=(a347*a348); + a345=(a345+a349); + a2=(a2+a345); + a345=(a129-a108); + a345=(a317*a345); + a349=(a129-a108); + a350=(a345*a349); + a351=(a134-a113); + a352=(a321*a351); + a353=(a134-a113); + a354=(a352*a353); + a350=(a350+a354); + a2=(a2+a350); + a350=(a150-a129); + a350=(a317*a350); + a354=(a150-a129); + a355=(a350*a354); + a356=(a155-a134); + a357=(a321*a356); + a358=(a155-a134); + a359=(a357*a358); + a355=(a355+a359); + a2=(a2+a355); + a355=(a171-a150); + a355=(a317*a355); + a359=(a171-a150); + a360=(a355*a359); + a361=(a176-a155); + a362=(a321*a361); + a363=(a176-a155); + a364=(a362*a363); + a360=(a360+a364); + a2=(a2+a360); + a360=(a192-a171); + a360=(a317*a360); + a364=(a192-a171); + a365=(a360*a364); + a366=(a197-a176); + a367=(a321*a366); + a368=(a197-a176); + a369=(a367*a368); + a365=(a365+a369); + a2=(a2+a365); + a365=(a213-a192); + a365=(a317*a365); + a369=(a213-a192); + a370=(a365*a369); + a371=(a218-a197); + a372=(a321*a371); + a373=(a218-a197); + a374=(a372*a373); + a370=(a370+a374); + a2=(a2+a370); + a370=(a234-a213); + a370=(a317*a370); + a374=(a234-a213); + a375=(a370*a374); + a376=(a239-a218); + a377=(a321*a376); + a378=(a239-a218); + a379=(a377*a378); + a375=(a375+a379); + a2=(a2+a375); + a375=(a255-a234); + a375=(a317*a375); + a379=(a255-a234); + a380=(a375*a379); + a381=(a260-a239); + a382=(a321*a381); + a383=(a260-a239); + a384=(a382*a383); + a380=(a380+a384); + a2=(a2+a380); + a380=(a276-a255); + a380=(a317*a380); + a384=(a276-a255); + a385=(a380*a384); + a386=(a281-a260); + a387=(a321*a386); + a388=(a281-a260); + a389=(a387*a388); + a385=(a385+a389); + a2=(a2+a385); + a385=(a297-a276); + a385=(a317*a385); + a389=(a297-a276); + a390=(a385*a389); + a391=(a302-a281); + a392=(a321*a391); + a393=(a302-a281); + a392=(a392*a393); + a390=(a390+a392); + a2=(a2+a390); + if (res[0]!=0) res[0][0]=a2; + a2=0.; + if (res[1]!=0) res[1][0]=a2; + if (res[1]!=0) res[1][1]=a2; + if (res[1]!=0) res[1][2]=a2; + a1=(a0*a1); + a3=(a3+a1); + if (res[1]!=0) res[1][3]=a3; + a4=(a0*a4); + a6=(a6+a4); + if (res[1]!=0) res[1][4]=a6; + a6=sin(a10); + a4=casadi_sq(a19); + a3=casadi_sq(a21); + a4=(a4+a3); + a19=(a19/a4); + a19=(a19*a11); + a22=(a22*a19); + a21=(a21/a4); + a21=(a21*a11); + a20=(a20*a21); + a22=(a22+a20); + a6=(a6*a22); + a22=cos(a10); + a17=(a17*a21); + a7=(a7*a19); + a17=(a17-a7); + a22=(a22*a17); + a6=(a6+a22); + a22=sin(a10); + a17=casadi_sq(a12); + a7=casadi_sq(a14); + a17=(a17+a7); + a12=(a12/a17); + a12=(a12*a18); + a15=(a15*a12); + a14=(a14/a17); + a14=(a14*a18); + a13=(a13*a14); + a15=(a15+a13); + a22=(a22*a15); + a6=(a6+a22); + a10=cos(a10); + a9=(a9*a14); + a16=(a16*a12); + a9=(a9-a16); + a10=(a10*a9); + a6=(a6+a10); + if (res[1]!=0) res[1][5]=a6; + a23=(a0*a23); + a25=(a25+a23); + if (res[1]!=0) res[1][6]=a25; + a26=(a0*a26); + a28=(a28+a26); + if (res[1]!=0) res[1][7]=a28; + a28=sin(a31); + a26=casadi_sq(a40); + a25=casadi_sq(a42); + a26=(a26+a25); + a40=(a40/a26); + a40=(a40*a32); + a43=(a43*a40); + a42=(a42/a26); + a42=(a42*a32); + a41=(a41*a42); + a43=(a43+a41); + a28=(a28*a43); + a43=cos(a31); + a38=(a38*a42); + a27=(a27*a40); + a38=(a38-a27); + a43=(a43*a38); + a28=(a28+a43); + a43=sin(a31); + a38=casadi_sq(a33); + a27=casadi_sq(a35); + a38=(a38+a27); + a33=(a33/a38); + a33=(a33*a39); + a36=(a36*a33); + a35=(a35/a38); + a35=(a35*a39); + a34=(a34*a35); + a36=(a36+a34); + a43=(a43*a36); + a28=(a28+a43); + a31=cos(a31); + a30=(a30*a35); + a37=(a37*a33); + a30=(a30-a37); + a31=(a31*a30); + a28=(a28+a31); + if (res[1]!=0) res[1][8]=a28; + a44=(a0*a44); + a46=(a46+a44); + if (res[1]!=0) res[1][9]=a46; + a47=(a0*a47); + a49=(a49+a47); + if (res[1]!=0) res[1][10]=a49; + a49=sin(a52); + a47=casadi_sq(a61); + a46=casadi_sq(a63); + a47=(a47+a46); + a61=(a61/a47); + a61=(a61*a53); + a64=(a64*a61); + a63=(a63/a47); + a63=(a63*a53); + a62=(a62*a63); + a64=(a64+a62); + a49=(a49*a64); + a64=cos(a52); + a59=(a59*a63); + a48=(a48*a61); + a59=(a59-a48); + a64=(a64*a59); + a49=(a49+a64); + a64=sin(a52); + a59=casadi_sq(a54); + a48=casadi_sq(a56); + a59=(a59+a48); + a54=(a54/a59); + a54=(a54*a60); + a57=(a57*a54); + a56=(a56/a59); + a56=(a56*a60); + a55=(a55*a56); + a57=(a57+a55); + a64=(a64*a57); + a49=(a49+a64); + a52=cos(a52); + a51=(a51*a56); + a58=(a58*a54); + a51=(a51-a58); + a52=(a52*a51); + a49=(a49+a52); + if (res[1]!=0) res[1][11]=a49; + a65=(a0*a65); + a67=(a67+a65); + if (res[1]!=0) res[1][12]=a67; + a68=(a0*a68); + a70=(a70+a68); + if (res[1]!=0) res[1][13]=a70; + a70=sin(a73); + a68=casadi_sq(a82); + a67=casadi_sq(a84); + a68=(a68+a67); + a82=(a82/a68); + a82=(a82*a74); + a85=(a85*a82); + a84=(a84/a68); + a84=(a84*a74); + a83=(a83*a84); + a85=(a85+a83); + a70=(a70*a85); + a85=cos(a73); + a80=(a80*a84); + a69=(a69*a82); + a80=(a80-a69); + a85=(a85*a80); + a70=(a70+a85); + a85=sin(a73); + a80=casadi_sq(a75); + a69=casadi_sq(a77); + a80=(a80+a69); + a75=(a75/a80); + a75=(a75*a81); + a78=(a78*a75); + a77=(a77/a80); + a77=(a77*a81); + a76=(a76*a77); + a78=(a78+a76); + a85=(a85*a78); + a70=(a70+a85); + a73=cos(a73); + a72=(a72*a77); + a79=(a79*a75); + a72=(a72-a79); + a73=(a73*a72); + a70=(a70+a73); + if (res[1]!=0) res[1][14]=a70; + a86=(a0*a86); + a88=(a88+a86); + if (res[1]!=0) res[1][15]=a88; + a89=(a0*a89); + a91=(a91+a89); + if (res[1]!=0) res[1][16]=a91; + a91=sin(a94); + a89=casadi_sq(a103); + a88=casadi_sq(a105); + a89=(a89+a88); + a103=(a103/a89); + a103=(a103*a95); + a106=(a106*a103); + a105=(a105/a89); + a105=(a105*a95); + a104=(a104*a105); + a106=(a106+a104); + a91=(a91*a106); + a106=cos(a94); + a101=(a101*a105); + a90=(a90*a103); + a101=(a101-a90); + a106=(a106*a101); + a91=(a91+a106); + a106=sin(a94); + a101=casadi_sq(a96); + a90=casadi_sq(a98); + a101=(a101+a90); + a96=(a96/a101); + a96=(a96*a102); + a99=(a99*a96); + a98=(a98/a101); + a98=(a98*a102); + a97=(a97*a98); + a99=(a99+a97); + a106=(a106*a99); + a91=(a91+a106); + a94=cos(a94); + a93=(a93*a98); + a100=(a100*a96); + a93=(a93-a100); + a94=(a94*a93); + a91=(a91+a94); + if (res[1]!=0) res[1][17]=a91; + a107=(a0*a107); + a109=(a109+a107); + if (res[1]!=0) res[1][18]=a109; + a110=(a0*a110); + a112=(a112+a110); + if (res[1]!=0) res[1][19]=a112; + a112=sin(a115); + a110=casadi_sq(a124); + a109=casadi_sq(a126); + a110=(a110+a109); + a124=(a124/a110); + a124=(a124*a116); + a127=(a127*a124); + a126=(a126/a110); + a126=(a126*a116); + a125=(a125*a126); + a127=(a127+a125); + a112=(a112*a127); + a127=cos(a115); + a122=(a122*a126); + a111=(a111*a124); + a122=(a122-a111); + a127=(a127*a122); + a112=(a112+a127); + a127=sin(a115); + a122=casadi_sq(a117); + a111=casadi_sq(a119); + a122=(a122+a111); + a117=(a117/a122); + a117=(a117*a123); + a120=(a120*a117); + a119=(a119/a122); + a119=(a119*a123); + a118=(a118*a119); + a120=(a120+a118); + a127=(a127*a120); + a112=(a112+a127); + a115=cos(a115); + a114=(a114*a119); + a121=(a121*a117); + a114=(a114-a121); + a115=(a115*a114); + a112=(a112+a115); + if (res[1]!=0) res[1][20]=a112; + a128=(a0*a128); + a130=(a130+a128); + if (res[1]!=0) res[1][21]=a130; + a131=(a0*a131); + a133=(a133+a131); + if (res[1]!=0) res[1][22]=a133; + a133=sin(a136); + a131=casadi_sq(a145); + a130=casadi_sq(a147); + a131=(a131+a130); + a145=(a145/a131); + a145=(a145*a137); + a148=(a148*a145); + a147=(a147/a131); + a147=(a147*a137); + a146=(a146*a147); + a148=(a148+a146); + a133=(a133*a148); + a148=cos(a136); + a143=(a143*a147); + a132=(a132*a145); + a143=(a143-a132); + a148=(a148*a143); + a133=(a133+a148); + a148=sin(a136); + a143=casadi_sq(a138); + a132=casadi_sq(a140); + a143=(a143+a132); + a138=(a138/a143); + a138=(a138*a144); + a141=(a141*a138); + a140=(a140/a143); + a140=(a140*a144); + a139=(a139*a140); + a141=(a141+a139); + a148=(a148*a141); + a133=(a133+a148); + a136=cos(a136); + a135=(a135*a140); + a142=(a142*a138); + a135=(a135-a142); + a136=(a136*a135); + a133=(a133+a136); + if (res[1]!=0) res[1][23]=a133; + a149=(a0*a149); + a151=(a151+a149); + if (res[1]!=0) res[1][24]=a151; + a152=(a0*a152); + a154=(a154+a152); + if (res[1]!=0) res[1][25]=a154; + a154=sin(a157); + a152=casadi_sq(a166); + a151=casadi_sq(a168); + a152=(a152+a151); + a166=(a166/a152); + a166=(a166*a158); + a169=(a169*a166); + a168=(a168/a152); + a168=(a168*a158); + a167=(a167*a168); + a169=(a169+a167); + a154=(a154*a169); + a169=cos(a157); + a164=(a164*a168); + a153=(a153*a166); + a164=(a164-a153); + a169=(a169*a164); + a154=(a154+a169); + a169=sin(a157); + a164=casadi_sq(a159); + a153=casadi_sq(a161); + a164=(a164+a153); + a159=(a159/a164); + a159=(a159*a165); + a162=(a162*a159); + a161=(a161/a164); + a161=(a161*a165); + a160=(a160*a161); + a162=(a162+a160); + a169=(a169*a162); + a154=(a154+a169); + a157=cos(a157); + a156=(a156*a161); + a163=(a163*a159); + a156=(a156-a163); + a157=(a157*a156); + a154=(a154+a157); + if (res[1]!=0) res[1][26]=a154; + a170=(a0*a170); + a172=(a172+a170); + if (res[1]!=0) res[1][27]=a172; + a173=(a0*a173); + a175=(a175+a173); + if (res[1]!=0) res[1][28]=a175; + a175=sin(a178); + a173=casadi_sq(a187); + a172=casadi_sq(a189); + a173=(a173+a172); + a187=(a187/a173); + a187=(a187*a179); + a190=(a190*a187); + a189=(a189/a173); + a189=(a189*a179); + a188=(a188*a189); + a190=(a190+a188); + a175=(a175*a190); + a190=cos(a178); + a185=(a185*a189); + a174=(a174*a187); + a185=(a185-a174); + a190=(a190*a185); + a175=(a175+a190); + a190=sin(a178); + a185=casadi_sq(a180); + a174=casadi_sq(a182); + a185=(a185+a174); + a180=(a180/a185); + a180=(a180*a186); + a183=(a183*a180); + a182=(a182/a185); + a182=(a182*a186); + a181=(a181*a182); + a183=(a183+a181); + a190=(a190*a183); + a175=(a175+a190); + a178=cos(a178); + a177=(a177*a182); + a184=(a184*a180); + a177=(a177-a184); + a178=(a178*a177); + a175=(a175+a178); + if (res[1]!=0) res[1][29]=a175; + a191=(a0*a191); + a193=(a193+a191); + if (res[1]!=0) res[1][30]=a193; + a194=(a0*a194); + a196=(a196+a194); + if (res[1]!=0) res[1][31]=a196; + a196=sin(a199); + a194=casadi_sq(a208); + a193=casadi_sq(a210); + a194=(a194+a193); + a208=(a208/a194); + a208=(a208*a200); + a211=(a211*a208); + a210=(a210/a194); + a210=(a210*a200); + a209=(a209*a210); + a211=(a211+a209); + a196=(a196*a211); + a211=cos(a199); + a206=(a206*a210); + a195=(a195*a208); + a206=(a206-a195); + a211=(a211*a206); + a196=(a196+a211); + a211=sin(a199); + a206=casadi_sq(a201); + a195=casadi_sq(a203); + a206=(a206+a195); + a201=(a201/a206); + a201=(a201*a207); + a204=(a204*a201); + a203=(a203/a206); + a203=(a203*a207); + a202=(a202*a203); + a204=(a204+a202); + a211=(a211*a204); + a196=(a196+a211); + a199=cos(a199); + a198=(a198*a203); + a205=(a205*a201); + a198=(a198-a205); + a199=(a199*a198); + a196=(a196+a199); + if (res[1]!=0) res[1][32]=a196; + a212=(a0*a212); + a214=(a214+a212); + if (res[1]!=0) res[1][33]=a214; + a215=(a0*a215); + a217=(a217+a215); + if (res[1]!=0) res[1][34]=a217; + a217=sin(a220); + a215=casadi_sq(a229); + a214=casadi_sq(a231); + a215=(a215+a214); + a229=(a229/a215); + a229=(a229*a221); + a232=(a232*a229); + a231=(a231/a215); + a231=(a231*a221); + a230=(a230*a231); + a232=(a232+a230); + a217=(a217*a232); + a232=cos(a220); + a227=(a227*a231); + a216=(a216*a229); + a227=(a227-a216); + a232=(a232*a227); + a217=(a217+a232); + a232=sin(a220); + a227=casadi_sq(a222); + a216=casadi_sq(a224); + a227=(a227+a216); + a222=(a222/a227); + a222=(a222*a228); + a225=(a225*a222); + a224=(a224/a227); + a224=(a224*a228); + a223=(a223*a224); + a225=(a225+a223); + a232=(a232*a225); + a217=(a217+a232); + a220=cos(a220); + a219=(a219*a224); + a226=(a226*a222); + a219=(a219-a226); + a220=(a220*a219); + a217=(a217+a220); + if (res[1]!=0) res[1][35]=a217; + a233=(a0*a233); + a235=(a235+a233); + if (res[1]!=0) res[1][36]=a235; + a236=(a0*a236); + a238=(a238+a236); + if (res[1]!=0) res[1][37]=a238; + a238=sin(a241); + a236=casadi_sq(a250); + a235=casadi_sq(a252); + a236=(a236+a235); + a250=(a250/a236); + a250=(a250*a242); + a253=(a253*a250); + a252=(a252/a236); + a252=(a252*a242); + a251=(a251*a252); + a253=(a253+a251); + a238=(a238*a253); + a253=cos(a241); + a248=(a248*a252); + a237=(a237*a250); + a248=(a248-a237); + a253=(a253*a248); + a238=(a238+a253); + a253=sin(a241); + a248=casadi_sq(a243); + a237=casadi_sq(a245); + a248=(a248+a237); + a243=(a243/a248); + a243=(a243*a249); + a246=(a246*a243); + a245=(a245/a248); + a245=(a245*a249); + a244=(a244*a245); + a246=(a246+a244); + a253=(a253*a246); + a238=(a238+a253); + a241=cos(a241); + a240=(a240*a245); + a247=(a247*a243); + a240=(a240-a247); + a241=(a241*a240); + a238=(a238+a241); + if (res[1]!=0) res[1][38]=a238; + a254=(a0*a254); + a256=(a256+a254); + if (res[1]!=0) res[1][39]=a256; + a257=(a0*a257); + a259=(a259+a257); + if (res[1]!=0) res[1][40]=a259; + a259=sin(a262); + a257=casadi_sq(a271); + a256=casadi_sq(a273); + a257=(a257+a256); + a271=(a271/a257); + a271=(a271*a263); + a274=(a274*a271); + a273=(a273/a257); + a273=(a273*a263); + a272=(a272*a273); + a274=(a274+a272); + a259=(a259*a274); + a274=cos(a262); + a269=(a269*a273); + a258=(a258*a271); + a269=(a269-a258); + a274=(a274*a269); + a259=(a259+a274); + a274=sin(a262); + a269=casadi_sq(a264); + a258=casadi_sq(a266); + a269=(a269+a258); + a264=(a264/a269); + a264=(a264*a270); + a267=(a267*a264); + a266=(a266/a269); + a266=(a266*a270); + a265=(a265*a266); + a267=(a267+a265); + a274=(a274*a267); + a259=(a259+a274); + a262=cos(a262); + a261=(a261*a266); + a268=(a268*a264); + a261=(a261-a268); + a262=(a262*a261); + a259=(a259+a262); + if (res[1]!=0) res[1][41]=a259; + a275=(a0*a275); + a277=(a277+a275); + if (res[1]!=0) res[1][42]=a277; + a278=(a0*a278); + a280=(a280+a278); + if (res[1]!=0) res[1][43]=a280; + a280=sin(a283); + a278=casadi_sq(a292); + a277=casadi_sq(a294); + a278=(a278+a277); + a292=(a292/a278); + a292=(a292*a284); + a295=(a295*a292); + a294=(a294/a278); + a294=(a294*a284); + a293=(a293*a294); + a295=(a295+a293); + a280=(a280*a295); + a295=cos(a283); + a290=(a290*a294); + a279=(a279*a292); + a290=(a290-a279); + a295=(a295*a290); + a280=(a280+a295); + a295=sin(a283); + a290=casadi_sq(a285); + a279=casadi_sq(a287); + a290=(a290+a279); + a285=(a285/a290); + a285=(a285*a291); + a288=(a288*a285); + a287=(a287/a290); + a287=(a287*a291); + a286=(a286*a287); + a288=(a288+a286); + a295=(a295*a288); + a280=(a280+a295); + a283=cos(a283); + a282=(a282*a287); + a289=(a289*a285); + a282=(a282-a289); + a283=(a283*a282); + a280=(a280+a283); + if (res[1]!=0) res[1][44]=a280; + a296=(a0*a296); + a298=(a298+a296); + if (res[1]!=0) res[1][45]=a298; + a0=(a0*a299); + a301=(a301+a0); + if (res[1]!=0) res[1][46]=a301; + a301=sin(a304); + a0=casadi_sq(a313); + a299=casadi_sq(a315); + a0=(a0+a299); + a313=(a313/a0); + a313=(a313*a305); + a316=(a316*a313); + a315=(a315/a0); + a315=(a315*a305); + a314=(a314*a315); + a316=(a316+a314); + a301=(a301*a316); + a316=cos(a304); + a311=(a311*a315); + a300=(a300*a313); + a311=(a311-a300); + a316=(a316*a311); + a301=(a301+a316); + a316=sin(a304); + a311=casadi_sq(a306); + a300=casadi_sq(a308); + a311=(a311+a300); + a306=(a306/a311); + a306=(a306*a312); + a309=(a309*a306); + a308=(a308/a311); + a308=(a308*a312); + a307=(a307*a308); + a309=(a309+a307); + a316=(a316*a309); + a301=(a301+a316); + a304=cos(a304); + a303=(a303*a308); + a310=(a310*a306); + a303=(a303-a310); + a304=(a304*a303); + a301=(a301+a304); + if (res[1]!=0) res[1][47]=a301; + a324=(a317*a324); + a301=(a320+a324); + a319=(a319-a301); + a318=(a317*a318); + a319=(a319+a318); + a5=(a5+a5); + a319=(a319+a5); + if (res[1]!=0) res[1][48]=a319; + a323=(a323-a326); + a322=(a321*a322); + a323=(a323+a322); + a8=(a8+a8); + a323=(a323+a8); + if (res[1]!=0) res[1][49]=a323; + a329=(a317*a329); + a323=(a325+a329); + a320=(a320-a323); + a320=(a320+a324); + a24=(a24+a24); + a320=(a320+a24); + if (res[1]!=0) res[1][50]=a320; + a327=(a327-a331); + a328=(a321*a328); + a327=(a327+a328); + a29=(a29+a29); + a327=(a327+a29); + if (res[1]!=0) res[1][51]=a327; + a334=(a317*a334); + a327=(a330+a334); + a325=(a325-a327); + a325=(a325+a329); + a45=(a45+a45); + a325=(a325+a45); + if (res[1]!=0) res[1][52]=a325; + a332=(a332-a336); + a333=(a321*a333); + a332=(a332+a333); + a50=(a50+a50); + a332=(a332+a50); + if (res[1]!=0) res[1][53]=a332; + a339=(a317*a339); + a332=(a335+a339); + a330=(a330-a332); + a330=(a330+a334); + a66=(a66+a66); + a330=(a330+a66); + if (res[1]!=0) res[1][54]=a330; + a337=(a337-a341); + a338=(a321*a338); + a337=(a337+a338); + a71=(a71+a71); + a337=(a337+a71); + if (res[1]!=0) res[1][55]=a337; + a344=(a317*a344); + a337=(a340+a344); + a335=(a335-a337); + a335=(a335+a339); + a87=(a87+a87); + a335=(a335+a87); + if (res[1]!=0) res[1][56]=a335; + a342=(a342-a346); + a343=(a321*a343); + a342=(a342+a343); + a92=(a92+a92); + a342=(a342+a92); + if (res[1]!=0) res[1][57]=a342; + a349=(a317*a349); + a342=(a345+a349); + a340=(a340-a342); + a340=(a340+a344); + a108=(a108+a108); + a340=(a340+a108); + if (res[1]!=0) res[1][58]=a340; + a347=(a347-a351); + a348=(a321*a348); + a347=(a347+a348); + a113=(a113+a113); + a347=(a347+a113); + if (res[1]!=0) res[1][59]=a347; + a354=(a317*a354); + a347=(a350+a354); + a345=(a345-a347); + a345=(a345+a349); + a129=(a129+a129); + a345=(a345+a129); + if (res[1]!=0) res[1][60]=a345; + a352=(a352-a356); + a353=(a321*a353); + a352=(a352+a353); + a134=(a134+a134); + a352=(a352+a134); + if (res[1]!=0) res[1][61]=a352; + a359=(a317*a359); + a352=(a355+a359); + a350=(a350-a352); + a350=(a350+a354); + a150=(a150+a150); + a350=(a350+a150); + if (res[1]!=0) res[1][62]=a350; + a357=(a357-a361); + a358=(a321*a358); + a357=(a357+a358); + a155=(a155+a155); + a357=(a357+a155); + if (res[1]!=0) res[1][63]=a357; + a364=(a317*a364); + a357=(a360+a364); + a355=(a355-a357); + a355=(a355+a359); + a171=(a171+a171); + a355=(a355+a171); + if (res[1]!=0) res[1][64]=a355; + a362=(a362-a366); + a363=(a321*a363); + a362=(a362+a363); + a176=(a176+a176); + a362=(a362+a176); + if (res[1]!=0) res[1][65]=a362; + a369=(a317*a369); + a362=(a365+a369); + a360=(a360-a362); + a360=(a360+a364); + a192=(a192+a192); + a360=(a360+a192); + if (res[1]!=0) res[1][66]=a360; + a367=(a367-a371); + a368=(a321*a368); + a367=(a367+a368); + a197=(a197+a197); + a367=(a367+a197); + if (res[1]!=0) res[1][67]=a367; + a374=(a317*a374); + a367=(a370+a374); + a365=(a365-a367); + a365=(a365+a369); + a213=(a213+a213); + a365=(a365+a213); + if (res[1]!=0) res[1][68]=a365; + a372=(a372-a376); + a373=(a321*a373); + a372=(a372+a373); + a218=(a218+a218); + a372=(a372+a218); + if (res[1]!=0) res[1][69]=a372; + a379=(a317*a379); + a372=(a375+a379); + a370=(a370-a372); + a370=(a370+a374); + a234=(a234+a234); + a370=(a370+a234); + if (res[1]!=0) res[1][70]=a370; + a377=(a377-a381); + a378=(a321*a378); + a377=(a377+a378); + a239=(a239+a239); + a377=(a377+a239); + if (res[1]!=0) res[1][71]=a377; + a384=(a317*a384); + a377=(a380+a384); + a375=(a375-a377); + a375=(a375+a379); + a255=(a255+a255); + a375=(a375+a255); + if (res[1]!=0) res[1][72]=a375; + a382=(a382-a386); + a383=(a321*a383); + a382=(a382+a383); + a260=(a260+a260); + a382=(a382+a260); + if (res[1]!=0) res[1][73]=a382; + a317=(a317*a389); + a389=(a385+a317); + a380=(a380-a389); + a380=(a380+a384); + a276=(a276+a276); + a380=(a380+a276); + if (res[1]!=0) res[1][74]=a380; + a387=(a387-a391); + a321=(a321*a388); + a387=(a387+a321); + a281=(a281+a281); + a387=(a387+a281); + if (res[1]!=0) res[1][75]=a387; + a385=(a385+a317); + a297=(a297+a297); + a385=(a385+a297); + if (res[1]!=0) res[1][76]=a385; + a302=(a302+a302); + a391=(a391+a302); + if (res[1]!=0) res[1][77]=a391; + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_grad_f(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f4(arg, res, iw, w, mem); +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_grad_f_alloc_mem(void) { + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_grad_f_init_mem(int mem) { + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_grad_f_free_mem(int mem) { +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_grad_f_checkout(void) { + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_grad_f_release(int mem) { +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_grad_f_incref(void) { +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_grad_f_decref(void) { +} + +extern "C" CASADI_SYMBOL_EXPORT casadi_int nlp_grad_f_n_in(void) { return 2;} + +extern "C" CASADI_SYMBOL_EXPORT casadi_int nlp_grad_f_n_out(void) { return 2;} + +extern "C" CASADI_SYMBOL_EXPORT casadi_real nlp_grad_f_default_in(casadi_int i) { + switch (i) { + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const char* nlp_grad_f_name_in(casadi_int i) { + switch (i) { + case 0: return "x"; + case 1: return "p"; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const char* nlp_grad_f_name_out(casadi_int i) { + switch (i) { + case 0: return "f"; + case 1: return "grad_f_x"; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const casadi_int* nlp_grad_f_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s1; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const casadi_int* nlp_grad_f_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s2; + case 1: return casadi_s0; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_grad_f_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 2; + if (sz_res) *sz_res = 2; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 0; + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_grad_f_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 2*sizeof(const casadi_real*); + if (sz_res) *sz_res = 2*sizeof(casadi_real*); + if (sz_iw) *sz_iw = 0*sizeof(casadi_int); + if (sz_w) *sz_w = 0*sizeof(casadi_real); + return 0; +} + +/* nlp_hess_l:(x[78],p[50],lam_f,lam_g[63])->(triu_hess_gamma_x_x[78x78,480nz]) */ +static int casadi_f5(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_real a0, a1, a10, a100, a101, a102, a103, a104, a105, a106, a107, a108, a109, a11, a110, a111, a112, a113, a114, a115, a116, a117, a118, a119, a12, a120, a121, a122, a123, a124, a125, a126, a127, a128, a129, a13, a130, a131, a132, a133, a134, a135, a136, a137, a138, a139, a14, a140, a141, a142, a143, a144, a145, a146, a147, a148, a149, a15, a150, a151, a152, a153, a154, a155, a156, a157, a158, a159, a16, a160, a161, a162, a163, a164, a165, a166, a167, a168, a169, a17, a170, a171, a172, a173, a174, a175, a176, a177, a178, a179, a18, a180, a181, a182, a183, a184, a185, a186, a187, a188, a189, a19, a190, a191, a192, a193, a194, a195, a196, a197, a198, a199, a2, a20, a200, a201, a202, a203, a204, a205, a206, a207, a208, a209, a21, a210, a211, a212, a213, a214, a215, a216, a217, a218, a219, a22, a220, a221, a222, a223, a224, a225, a226, a227, a228, a229, a23, a230, a231, a232, a233, a234, a235, a236, a237, a238, a239, a24, a240, a241, a242, a243, a244, a245, a246, a247, a248, a249, a25, a250, a251, a252, a253, a254, a255, a256, a257, a258, a259, a26, a260, a261, a262, a263, a264, a265, a266, a267, a268, a269, a27, a270, a271, a272, a273, a274, a275, a276, a277, a278, a279, a28, a280, a281, a282, a283, a284, a285, a286, a287, a288, a289, a29, a290, a291, a292, a293, a294, a295, a296, a297, a298, a299, a3, a30, a300, a301, a302, a303, a304, a305, a306, a307, a308, a309, a31, a310, a311, a312, a313, a314, a315, a316, a317, a318, a319, a32, a320, a321, a322, a323, a324, a325, a326, a327, a328, a329, a33, a330, a331, a332, a333, a334, a335, a336, a337, a338, a339, a34, a340, a341, a342, a343, a344, a345, a346, a347, a348, a349, a35, a350, a351, a352, a353, a354, a355, a356, a357, a358, a359, a36, a360, a361, a362, a363, a364, a365, a366, a367, a368, a369, a37, a370, a371, a372, a373, a374, a375, a376, a377, a378, a379, a38, a380, a381, a382, a383, a384, a385, a386, a387, a388, a389, a39, a390, a391, a392, a393, a394, a395, a396, a397, a398, a399, a4, a40, a400, a401, a402, a403, a404, a405, a406, a407, a408, a409, a41, a410, a411, a412, a413, a414, a415, a416, a417, a418, a419, a42, a420, a421, a422, a423, a424, a425, a426, a427, a428, a429, a43, a430, a431, a432, a433, a434, a435, a436, a437, a438, a439, a44, a440, a441, a442, a443, a444, a445, a446, a447, a448, a449, a45, a450, a451, a452, a453, a454, a455, a456, a457, a458, a459, a46, a460, a461, a462, a463, a464, a465, a466, a467, a468, a469, a47, a470, a471, a472, a473, a474, a475, a476, a477, a478, a479, a48, a480, a481, a482, a483, a484, a485, a486, a487, a488, a489, a49, a490, a491, a492, a493, a494, a495, a496, a497, a498, a499, a5, a50, a500, a501, a502, a503, a504, a505, a506, a507, a508, a509, a51, a510, a511, a512, a513, a514, a515, a516, a517, a518, a519, a52, a520, a521, a522, a523, a524, a525, a526, a527, a528, a529, a53, a530, a531, a532, a533, a534, a535, a536, a537, a538, a539, a54, a540, a541, a542, a543, a544, a545, a546, a547, a548, a549, a55, a550, a551, a552, a553, a554, a555, a556, a557, a558, a559, a56, a560, a561, a562, a563, a564, a565, a566, a567, a568, a569, a57, a570, a571, a572, a573, a574, a575, a576, a577, a578, a579, a58, a580, a581, a582, a583, a584, a585, a586, a587, a588, a589, a59, a590, a591, a592, a593, a594, a595, a596, a597, a598, a599, a6, a60, a600, a601, a602, a603, a604, a605, a606, a607, a608, a609, a61, a610, a611, a612, a613, a614, a615, a616, a617, a618, a619, a62, a620, a621, a622, a623, a624, a625, a626, a627, a628, a629, a63, a630, a631, a632, a633, a634, a635, a636, a637, a638, a639, a64, a640, a641, a642, a643, a644, a645, a646, a647, a648, a649, a65, a650, a651, a652, a653, a654, a655, a656, a657, a658, a659, a66, a660, a661, a662, a663, a664, a665, a666, a667, a668, a669, a67, a670, a671, a672, a673, a674, a675, a676, a677, a678, a679, a68, a680, a681, a682, a683, a684, a685, a686, a687, a688, a689, a69, a690, a691, a692, a693, a694, a695, a696, a697, a698, a699, a7, a70, a700, a701, a702, a703, a704, a705, a706, a707, a708, a709, a71, a710, a711, a712, a713, a714, a715, a716, a717, a718, a719, a72, a720, a721, a722, a723, a724, a725, a726, a727, a728, a729, a73, a730, a731, a732, a733, a734, a735, a736, a737, a738, a739, a74, a740, a741, a742, a743, a744, a745, a746, a747, a748, a749, a75, a750, a751, a752, a753, a754, a755, a756, a757, a758, a759, a76, a760, a761, a762, a763, a764, a765, a766, a767, a768, a769, a77, a770, a771, a772, a773, a774, a775, a776, a777, a778, a779, a78, a780, a781, a782, a783, a784, a785, a786, a787, a788, a789, a79, a790, a791, a792, a793, a794, a795, a796, a797, a798, a799, a8, a80, a800, a801, a802, a803, a804, a805, a806, a807, a808, a809, a81, a810, a811, a812, a813, a814, a815, a816, a817, a818, a819, a82, a820, a821, a822, a823, a824, a825, a83, a84, a85, a86, a87, a88, a89, a9, a90, a91, a92, a93, a94, a95, a96, a97, a98, a99; + a0=arg[0]? arg[0][5] : 0; + a1=cos(a0); + a2=cos(a0); + a3=arg[0]? arg[0][2] : 0; + a4=4.1666666666666664e-02; + a5=4.0000000000000002e-01; + a6=arg[1]? arg[1][49] : 0; + a7=(a5*a6); + a8=5.9999999999999998e-01; + a9=arg[0]? arg[0][49] : 0; + a10=(a8*a9); + a7=(a7+a10); + a10=2.; + a11=(a5*a6); + a12=(a8*a9); + a11=(a11+a12); + a12=(a10*a11); + a12=(a7+a12); + a13=(a5*a6); + a14=(a8*a9); + a13=(a13+a14); + a14=(a10*a13); + a12=(a12+a14); + a14=(a5*a6); + a15=(a8*a9); + a14=(a14+a15); + a12=(a12+a14); + a12=(a4*a12); + a12=(a3+a12); + a14=sin(a12); + a15=(a2*a14); + a16=sin(a0); + a17=cos(a12); + a18=(a16*a17); + a15=(a15-a18); + a18=casadi_sq(a15); + a19=(a1*a17); + a20=sin(a0); + a21=(a20*a14); + a19=(a19+a21); + a21=casadi_sq(a19); + a18=(a18+a21); + a21=(a15/a18); + a22=arg[3]? arg[3][5] : 0; + a23=(a21*a22); + a24=(a1*a23); + a25=(a19/a18); + a26=(a25*a22); + a27=(a16*a26); + a24=(a24+a27); + a27=cos(a12); + a28=(a24*a27); + a29=sin(a12); + a30=cos(a12); + a31=(a2*a30); + a32=sin(a12); + a33=(a16*a32); + a31=(a31+a33); + a33=(a31/a18); + a21=(a21/a18); + a15=(a15+a15); + a31=(a15*a31); + a19=(a19+a19); + a34=(a20*a30); + a35=(a1*a32); + a34=(a34-a35); + a35=(a19*a34); + a31=(a31+a35); + a35=(a21*a31); + a33=(a33-a35); + a33=(a22*a33); + a35=(a1*a33); + a34=(a34/a18); + a25=(a25/a18); + a31=(a25*a31); + a34=(a34-a31); + a34=(a22*a34); + a31=(a16*a34); + a35=(a35+a31); + a35=(a29*a35); + a28=(a28+a35); + a35=cos(a12); + a34=(a2*a34); + a33=(a20*a33); + a34=(a34-a33); + a34=(a35*a34); + a33=(a2*a26); + a31=(a20*a23); + a33=(a33-a31); + a12=sin(a12); + a31=(a33*a12); + a34=(a34-a31); + a28=(a28+a34); + a34=arg[0]? arg[0][48] : 0; + a31=arg[3]? arg[3][4] : 0; + a31=(a4*a31); + a36=(a34*a31); + a37=sin(a3); + a36=(a36*a37); + a28=(a28+a36); + a36=arg[3]? arg[3][3] : 0; + a36=(a4*a36); + a37=(a34*a36); + a38=2.5000000000000000e-01; + a13=(a38*a13); + a13=(a3+a13); + a39=cos(a13); + a40=(a37*a39); + a41=(a34*a31); + a42=sin(a13); + a43=(a41*a42); + a40=(a40+a43); + a28=(a28+a40); + a40=(a10*a36); + a43=(a34*a40); + a44=1.2500000000000000e-01; + a11=(a44*a11); + a11=(a3+a11); + a45=cos(a11); + a46=(a43*a45); + a47=(a10*a31); + a48=(a34*a47); + a49=sin(a11); + a50=(a48*a49); + a46=(a46+a50); + a28=(a28+a46); + a46=(a10*a36); + a50=(a34*a46); + a7=(a44*a7); + a7=(a3+a7); + a51=cos(a7); + a52=(a50*a51); + a53=(a10*a31); + a54=(a34*a53); + a55=sin(a7); + a56=(a54*a55); + a52=(a52+a56); + a28=(a28+a52); + a34=(a34*a36); + a52=cos(a3); + a34=(a34*a52); + a28=(a28+a34); + if (res[0]!=0) res[0][0]=a28; + a28=10.; + a34=arg[2]? arg[2][0] : 0; + a52=(a28*a34); + a56=(a28*a34); + a52=(a52+a56); + if (res[0]!=0) res[0][1]=a52; + a52=(a28*a34); + a56=(a28*a34); + a52=(a52+a56); + if (res[0]!=0) res[0][2]=a52; + a52=cos(a0); + a56=(a26*a52); + a57=cos(a0); + a58=(a14*a57); + a59=sin(a0); + a60=(a17*a59); + a58=(a58-a60); + a60=(a58/a18); + a58=(a19*a58); + a61=sin(a0); + a62=(a14*a61); + a52=(a17*a52); + a62=(a62+a52); + a52=(a15*a62); + a58=(a58-a52); + a52=(a25*a58); + a60=(a60-a52); + a60=(a22*a60); + a52=(a16*a60); + a56=(a56+a52); + a59=(a23*a59); + a62=(a62/a18); + a58=(a21*a58); + a62=(a62+a58); + a62=(a22*a62); + a58=(a1*a62); + a59=(a59+a58); + a56=(a56-a59); + a56=(a29*a56); + a59=(a2*a60); + a61=(a26*a61); + a59=(a59-a61); + a57=(a23*a57); + a61=(a20*a62); + a57=(a57-a61); + a59=(a59-a57); + a59=(a35*a59); + a56=(a56+a59); + if (res[0]!=0) res[0][3]=a56; + a56=arg[0]? arg[0][8] : 0; + a59=cos(a56); + a57=cos(a56); + a9=(a8*a9); + a6=(a5*a6); + a9=(a9+a6); + a6=(a5*a9); + a61=arg[0]? arg[0][51] : 0; + a58=(a8*a61); + a6=(a6+a58); + a58=(a5*a9); + a52=(a8*a61); + a58=(a58+a52); + a52=(a10*a58); + a52=(a6+a52); + a63=(a5*a9); + a64=(a8*a61); + a63=(a63+a64); + a64=(a10*a63); + a52=(a52+a64); + a64=(a5*a9); + a65=(a8*a61); + a64=(a64+a65); + a52=(a52+a64); + a52=(a4*a52); + a52=(a0+a52); + a64=sin(a52); + a65=(a57*a64); + a66=sin(a56); + a67=cos(a52); + a68=(a66*a67); + a65=(a65-a68); + a68=casadi_sq(a65); + a69=(a59*a67); + a70=sin(a56); + a71=(a70*a64); + a69=(a69+a71); + a71=casadi_sq(a69); + a68=(a68+a71); + a71=(a65/a68); + a72=arg[3]? arg[3][8] : 0; + a73=(a71*a72); + a74=(a59*a73); + a75=(a69/a68); + a76=(a75*a72); + a77=(a66*a76); + a74=(a74+a77); + a77=cos(a52); + a78=(a74*a77); + a79=sin(a52); + a80=cos(a52); + a81=(a57*a80); + a82=sin(a52); + a83=(a66*a82); + a81=(a81+a83); + a83=(a81/a68); + a71=(a71/a68); + a65=(a65+a65); + a81=(a65*a81); + a69=(a69+a69); + a84=(a70*a80); + a85=(a59*a82); + a84=(a84-a85); + a85=(a69*a84); + a81=(a81+a85); + a85=(a71*a81); + a83=(a83-a85); + a83=(a72*a83); + a85=(a59*a83); + a84=(a84/a68); + a75=(a75/a68); + a81=(a75*a81); + a84=(a84-a81); + a84=(a72*a84); + a81=(a66*a84); + a85=(a85+a81); + a85=(a79*a85); + a78=(a78+a85); + a85=cos(a52); + a81=(a57*a84); + a86=(a70*a83); + a81=(a81-a86); + a81=(a85*a81); + a86=(a57*a76); + a87=(a70*a73); + a86=(a86-a87); + a52=sin(a52); + a87=(a86*a52); + a81=(a81-a87); + a78=(a78+a81); + a81=arg[0]? arg[0][50] : 0; + a87=arg[3]? arg[3][7] : 0; + a87=(a4*a87); + a88=(a81*a87); + a89=sin(a0); + a88=(a88*a89); + a78=(a78+a88); + a88=arg[3]? arg[3][6] : 0; + a88=(a4*a88); + a89=(a81*a88); + a63=(a38*a63); + a63=(a0+a63); + a90=cos(a63); + a91=(a89*a90); + a92=(a81*a87); + a93=sin(a63); + a94=(a92*a93); + a91=(a91+a94); + a78=(a78+a91); + a91=(a10*a88); + a94=(a81*a91); + a58=(a44*a58); + a58=(a0+a58); + a95=cos(a58); + a96=(a94*a95); + a97=(a10*a87); + a98=(a81*a97); + a99=sin(a58); + a100=(a98*a99); + a96=(a96+a100); + a78=(a78+a96); + a96=(a10*a88); + a100=(a81*a96); + a6=(a44*a6); + a6=(a0+a6); + a101=cos(a6); + a102=(a100*a101); + a103=(a10*a87); + a104=(a81*a103); + a105=sin(a6); + a106=(a104*a105); + a102=(a102+a106); + a78=(a78+a102); + a81=(a81*a88); + a102=cos(a0); + a81=(a81*a102); + a78=(a78+a81); + a81=(a14*a23); + a102=sin(a0); + a81=(a81*a102); + a102=cos(a0); + a106=(a14*a62); + a106=(a102*a106); + a81=(a81+a106); + a78=(a78+a81); + a81=(a17*a23); + a106=cos(a0); + a81=(a81*a106); + a106=sin(a0); + a62=(a17*a62); + a62=(a106*a62); + a81=(a81-a62); + a78=(a78+a81); + a81=cos(a0); + a62=(a17*a60); + a62=(a81*a62); + a107=(a17*a26); + a108=sin(a0); + a107=(a107*a108); + a62=(a62-a107); + a78=(a78-a62); + a62=(a14*a26); + a107=cos(a0); + a62=(a62*a107); + a107=sin(a0); + a60=(a14*a60); + a60=(a107*a60); + a62=(a62+a60); + a78=(a78-a62); + a62=arg[1]? arg[1][5] : 0; + a60=cos(a62); + a108=cos(a62); + a109=sin(a0); + a110=(a108*a109); + a111=sin(a62); + a112=cos(a0); + a113=(a111*a112); + a110=(a110-a113); + a113=casadi_sq(a110); + a112=(a60*a112); + a114=sin(a62); + a109=(a114*a109); + a112=(a112+a109); + a109=casadi_sq(a112); + a113=(a113+a109); + a109=(a110/a113); + a115=cos(a62); + a116=sin(a0); + a117=(a115*a116); + a118=sin(a62); + a119=cos(a0); + a120=(a118*a119); + a117=(a117-a120); + a120=cos(a62); + a119=(a120*a119); + a62=sin(a62); + a116=(a62*a116); + a119=(a119+a116); + a116=atan2(a117,a119); + a116=(a116*a34); + a121=(a109*a116); + a122=(a60*a121); + a123=(a112/a113); + a124=(a123*a116); + a125=(a111*a124); + a122=(a122+a125); + a125=cos(a0); + a122=(a122*a125); + a125=sin(a0); + a126=cos(a0); + a127=(a108*a126); + a128=sin(a0); + a129=(a111*a128); + a127=(a127+a129); + a129=(a127/a113); + a130=(a109/a113); + a131=(a110+a110); + a131=(a131*a127); + a132=(a112+a112); + a126=(a114*a126); + a128=(a60*a128); + a126=(a126-a128); + a132=(a132*a126); + a131=(a131+a132); + a130=(a130*a131); + a129=(a129-a130); + a129=(a116*a129); + a130=casadi_sq(a117); + a132=casadi_sq(a119); + a130=(a130+a132); + a132=(a119/a130); + a128=cos(a0); + a133=(a115*a128); + a134=sin(a0); + a135=(a118*a134); + a133=(a133+a135); + a132=(a132*a133); + a130=(a117/a130); + a128=(a62*a128); + a134=(a120*a134); + a128=(a128-a134); + a130=(a130*a128); + a132=(a132-a130); + a132=(a34*a132); + a109=(a109*a132); + a129=(a129+a109); + a60=(a60*a129); + a109=(a126/a113); + a113=(a123/a113); + a113=(a113*a131); + a109=(a109-a113); + a116=(a116*a109); + a123=(a123*a132); + a116=(a116+a123); + a111=(a111*a116); + a60=(a60+a111); + a125=(a125*a60); + a122=(a122+a125); + a78=(a78+a122); + a122=cos(a0); + a116=(a108*a116); + a129=(a114*a129); + a116=(a116-a129); + a122=(a122*a116); + a108=(a108*a124); + a114=(a114*a121); + a108=(a108-a114); + a114=sin(a0); + a108=(a108*a114); + a122=(a122-a108); + a78=(a78+a122); + a122=casadi_sq(a117); + a108=casadi_sq(a119); + a122=(a122+a108); + a108=(a117/a122); + a114=atan2(a110,a112); + a114=(a114*a34); + a121=(a108*a114); + a124=(a120*a121); + a116=(a119/a122); + a129=(a116*a114); + a125=(a118*a129); + a124=(a124+a125); + a125=cos(a0); + a124=(a124*a125); + a125=sin(a0); + a60=(a133/a122); + a111=(a108/a122); + a117=(a117+a117); + a117=(a117*a133); + a119=(a119+a119); + a119=(a119*a128); + a117=(a117+a119); + a111=(a111*a117); + a60=(a60-a111); + a60=(a114*a60); + a111=casadi_sq(a110); + a119=casadi_sq(a112); + a111=(a111+a119); + a112=(a112/a111); + a112=(a112*a127); + a110=(a110/a111); + a110=(a110*a126); + a112=(a112-a110); + a112=(a34*a112); + a108=(a108*a112); + a60=(a60+a108); + a120=(a120*a60); + a128=(a128/a122); + a122=(a116/a122); + a122=(a122*a117); + a128=(a128-a122); + a114=(a114*a128); + a116=(a116*a112); + a114=(a114+a116); + a118=(a118*a114); + a120=(a120+a118); + a125=(a125*a120); + a124=(a124+a125); + a78=(a78+a124); + a124=cos(a0); + a114=(a115*a114); + a60=(a62*a60); + a114=(a114-a60); + a124=(a124*a114); + a115=(a115*a129); + a62=(a62*a121); + a115=(a115-a62); + a62=sin(a0); + a115=(a115*a62); + a124=(a124-a115); + a78=(a78+a124); + if (res[0]!=0) res[0][4]=a78; + a78=(a28*a34); + a124=(a28*a34); + a78=(a78+a124); + if (res[0]!=0) res[0][5]=a78; + a78=(a28*a34); + a124=(a28*a34); + a78=(a78+a124); + if (res[0]!=0) res[0][6]=a78; + a78=sin(a56); + a124=(a67*a83); + a115=(a73*a82); + a124=(a124-a115); + a124=(a78*a124); + a115=cos(a56); + a62=(a73*a80); + a83=(a64*a83); + a62=(a62+a83); + a62=(a115*a62); + a124=(a124-a62); + a62=cos(a56); + a83=(a67*a84); + a121=(a76*a82); + a83=(a83-a121); + a83=(a62*a83); + a124=(a124-a83); + a83=sin(a56); + a121=(a76*a80); + a84=(a64*a84); + a121=(a121+a84); + a121=(a83*a121); + a124=(a124-a121); + if (res[0]!=0) res[0][7]=a124; + a124=arg[0]? arg[0][11] : 0; + a121=cos(a124); + a84=cos(a124); + a61=(a8*a61); + a9=(a5*a9); + a61=(a61+a9); + a9=(a5*a61); + a129=arg[0]? arg[0][53] : 0; + a114=(a8*a129); + a9=(a9+a114); + a114=(a5*a61); + a60=(a8*a129); + a114=(a114+a60); + a60=(a10*a114); + a60=(a9+a60); + a125=(a5*a61); + a120=(a8*a129); + a125=(a125+a120); + a120=(a10*a125); + a60=(a60+a120); + a120=(a5*a61); + a118=(a8*a129); + a120=(a120+a118); + a60=(a60+a120); + a60=(a4*a60); + a60=(a56+a60); + a120=sin(a60); + a118=(a84*a120); + a116=sin(a124); + a112=cos(a60); + a128=(a116*a112); + a118=(a118-a128); + a128=casadi_sq(a118); + a122=(a121*a112); + a117=sin(a124); + a108=(a117*a120); + a122=(a122+a108); + a108=casadi_sq(a122); + a128=(a128+a108); + a108=(a118/a128); + a110=arg[3]? arg[3][11] : 0; + a126=(a108*a110); + a111=(a121*a126); + a127=(a122/a128); + a119=(a127*a110); + a133=(a116*a119); + a111=(a111+a133); + a133=cos(a60); + a123=(a111*a133); + a132=sin(a60); + a109=cos(a60); + a113=(a84*a109); + a131=sin(a60); + a130=(a116*a131); + a113=(a113+a130); + a130=(a113/a128); + a108=(a108/a128); + a118=(a118+a118); + a113=(a118*a113); + a122=(a122+a122); + a134=(a117*a109); + a135=(a121*a131); + a134=(a134-a135); + a135=(a122*a134); + a113=(a113+a135); + a135=(a108*a113); + a130=(a130-a135); + a130=(a110*a130); + a135=(a121*a130); + a134=(a134/a128); + a127=(a127/a128); + a113=(a127*a113); + a134=(a134-a113); + a134=(a110*a134); + a113=(a116*a134); + a135=(a135+a113); + a135=(a132*a135); + a123=(a123+a135); + a135=cos(a60); + a134=(a84*a134); + a130=(a117*a130); + a134=(a134-a130); + a134=(a135*a134); + a130=(a84*a119); + a113=(a117*a126); + a130=(a130-a113); + a60=sin(a60); + a113=(a130*a60); + a134=(a134-a113); + a123=(a123+a134); + a134=arg[0]? arg[0][52] : 0; + a113=arg[3]? arg[3][10] : 0; + a113=(a4*a113); + a136=(a134*a113); + a137=sin(a56); + a136=(a136*a137); + a123=(a123+a136); + a136=arg[3]? arg[3][9] : 0; + a136=(a4*a136); + a137=(a134*a136); + a125=(a38*a125); + a125=(a56+a125); + a138=cos(a125); + a139=(a137*a138); + a140=(a134*a113); + a141=sin(a125); + a142=(a140*a141); + a139=(a139+a142); + a123=(a123+a139); + a139=(a10*a136); + a142=(a134*a139); + a114=(a44*a114); + a114=(a56+a114); + a143=cos(a114); + a144=(a142*a143); + a145=(a10*a113); + a146=(a134*a145); + a147=sin(a114); + a148=(a146*a147); + a144=(a144+a148); + a123=(a123+a144); + a144=(a10*a136); + a148=(a134*a144); + a9=(a44*a9); + a9=(a56+a9); + a149=cos(a9); + a150=(a148*a149); + a151=(a10*a113); + a152=(a134*a151); + a153=sin(a9); + a154=(a152*a153); + a150=(a150+a154); + a123=(a123+a150); + a134=(a134*a136); + a150=cos(a56); + a134=(a134*a150); + a123=(a123+a134); + a134=(a64*a73); + a150=sin(a56); + a134=(a134*a150); + a150=sin(a56); + a150=(a64*a150); + a154=cos(a56); + a154=(a67*a154); + a150=(a150+a154); + a154=(a150/a68); + a155=cos(a56); + a155=(a64*a155); + a156=sin(a56); + a156=(a67*a156); + a155=(a155-a156); + a156=(a69*a155); + a150=(a65*a150); + a156=(a156-a150); + a150=(a71*a156); + a154=(a154+a150); + a154=(a72*a154); + a150=(a64*a154); + a150=(a115*a150); + a134=(a134+a150); + a123=(a123+a134); + a134=(a67*a73); + a150=cos(a56); + a134=(a134*a150); + a154=(a67*a154); + a154=(a78*a154); + a134=(a134-a154); + a123=(a123+a134); + a155=(a155/a68); + a156=(a75*a156); + a155=(a155-a156); + a155=(a72*a155); + a156=(a67*a155); + a156=(a62*a156); + a134=(a67*a76); + a154=sin(a56); + a134=(a134*a154); + a156=(a156-a134); + a123=(a123-a156); + a156=(a64*a76); + a134=cos(a56); + a156=(a156*a134); + a155=(a64*a155); + a155=(a83*a155); + a156=(a156+a155); + a123=(a123-a156); + a156=arg[1]? arg[1][8] : 0; + a155=cos(a156); + a134=cos(a156); + a154=sin(a56); + a150=(a134*a154); + a157=sin(a156); + a158=cos(a56); + a159=(a157*a158); + a150=(a150-a159); + a159=casadi_sq(a150); + a158=(a155*a158); + a160=sin(a156); + a154=(a160*a154); + a158=(a158+a154); + a154=casadi_sq(a158); + a159=(a159+a154); + a154=(a150/a159); + a161=cos(a156); + a162=sin(a56); + a163=(a161*a162); + a164=sin(a156); + a165=cos(a56); + a166=(a164*a165); + a163=(a163-a166); + a166=cos(a156); + a165=(a166*a165); + a156=sin(a156); + a162=(a156*a162); + a165=(a165+a162); + a162=atan2(a163,a165); + a162=(a162*a34); + a167=(a154*a162); + a168=(a155*a167); + a169=(a158/a159); + a170=(a169*a162); + a171=(a157*a170); + a168=(a168+a171); + a171=cos(a56); + a168=(a168*a171); + a171=sin(a56); + a172=cos(a56); + a173=(a134*a172); + a174=sin(a56); + a175=(a157*a174); + a173=(a173+a175); + a175=(a173/a159); + a176=(a154/a159); + a177=(a150+a150); + a177=(a177*a173); + a178=(a158+a158); + a172=(a160*a172); + a174=(a155*a174); + a172=(a172-a174); + a178=(a178*a172); + a177=(a177+a178); + a176=(a176*a177); + a175=(a175-a176); + a175=(a162*a175); + a176=casadi_sq(a163); + a178=casadi_sq(a165); + a176=(a176+a178); + a178=(a165/a176); + a174=cos(a56); + a179=(a161*a174); + a180=sin(a56); + a181=(a164*a180); + a179=(a179+a181); + a178=(a178*a179); + a176=(a163/a176); + a174=(a156*a174); + a180=(a166*a180); + a174=(a174-a180); + a176=(a176*a174); + a178=(a178-a176); + a178=(a34*a178); + a154=(a154*a178); + a175=(a175+a154); + a155=(a155*a175); + a154=(a172/a159); + a159=(a169/a159); + a159=(a159*a177); + a154=(a154-a159); + a162=(a162*a154); + a169=(a169*a178); + a162=(a162+a169); + a157=(a157*a162); + a155=(a155+a157); + a171=(a171*a155); + a168=(a168+a171); + a123=(a123+a168); + a168=cos(a56); + a162=(a134*a162); + a175=(a160*a175); + a162=(a162-a175); + a168=(a168*a162); + a134=(a134*a170); + a160=(a160*a167); + a134=(a134-a160); + a160=sin(a56); + a134=(a134*a160); + a168=(a168-a134); + a123=(a123+a168); + a168=casadi_sq(a163); + a134=casadi_sq(a165); + a168=(a168+a134); + a134=(a163/a168); + a160=atan2(a150,a158); + a160=(a160*a34); + a167=(a134*a160); + a170=(a166*a167); + a162=(a165/a168); + a175=(a162*a160); + a171=(a164*a175); + a170=(a170+a171); + a171=cos(a56); + a170=(a170*a171); + a171=sin(a56); + a155=(a179/a168); + a157=(a134/a168); + a163=(a163+a163); + a163=(a163*a179); + a165=(a165+a165); + a165=(a165*a174); + a163=(a163+a165); + a157=(a157*a163); + a155=(a155-a157); + a155=(a160*a155); + a157=casadi_sq(a150); + a165=casadi_sq(a158); + a157=(a157+a165); + a158=(a158/a157); + a158=(a158*a173); + a150=(a150/a157); + a150=(a150*a172); + a158=(a158-a150); + a158=(a34*a158); + a134=(a134*a158); + a155=(a155+a134); + a166=(a166*a155); + a174=(a174/a168); + a168=(a162/a168); + a168=(a168*a163); + a174=(a174-a168); + a160=(a160*a174); + a162=(a162*a158); + a160=(a160+a162); + a164=(a164*a160); + a166=(a166+a164); + a171=(a171*a166); + a170=(a170+a171); + a123=(a123+a170); + a170=cos(a56); + a160=(a161*a160); + a155=(a156*a155); + a160=(a160-a155); + a170=(a170*a160); + a161=(a161*a175); + a156=(a156*a167); + a161=(a161-a156); + a156=sin(a56); + a161=(a161*a156); + a170=(a170-a161); + a123=(a123+a170); + if (res[0]!=0) res[0][8]=a123; + a123=(a28*a34); + a170=(a28*a34); + a123=(a123+a170); + if (res[0]!=0) res[0][9]=a123; + a123=(a28*a34); + a170=(a28*a34); + a123=(a123+a170); + if (res[0]!=0) res[0][10]=a123; + a123=cos(a124); + a170=(a119*a123); + a161=cos(a124); + a156=(a120*a161); + a167=sin(a124); + a175=(a112*a167); + a156=(a156-a175); + a175=(a156/a128); + a156=(a122*a156); + a160=sin(a124); + a155=(a120*a160); + a123=(a112*a123); + a155=(a155+a123); + a123=(a118*a155); + a156=(a156-a123); + a123=(a127*a156); + a175=(a175-a123); + a175=(a110*a175); + a123=(a116*a175); + a170=(a170+a123); + a167=(a126*a167); + a155=(a155/a128); + a156=(a108*a156); + a155=(a155+a156); + a155=(a110*a155); + a156=(a121*a155); + a167=(a167+a156); + a170=(a170-a167); + a170=(a132*a170); + a167=(a84*a175); + a160=(a119*a160); + a167=(a167-a160); + a161=(a126*a161); + a160=(a117*a155); + a161=(a161-a160); + a167=(a167-a161); + a167=(a135*a167); + a170=(a170+a167); + if (res[0]!=0) res[0][11]=a170; + a170=arg[0]? arg[0][14] : 0; + a167=cos(a170); + a161=cos(a170); + a129=(a8*a129); + a61=(a5*a61); + a129=(a129+a61); + a61=(a5*a129); + a160=arg[0]? arg[0][55] : 0; + a156=(a8*a160); + a61=(a61+a156); + a156=(a5*a129); + a123=(a8*a160); + a156=(a156+a123); + a123=(a10*a156); + a123=(a61+a123); + a171=(a5*a129); + a166=(a8*a160); + a171=(a171+a166); + a166=(a10*a171); + a123=(a123+a166); + a166=(a5*a129); + a164=(a8*a160); + a166=(a166+a164); + a123=(a123+a166); + a123=(a4*a123); + a123=(a124+a123); + a166=sin(a123); + a164=(a161*a166); + a162=sin(a170); + a158=cos(a123); + a174=(a162*a158); + a164=(a164-a174); + a174=casadi_sq(a164); + a168=(a167*a158); + a163=sin(a170); + a134=(a163*a166); + a168=(a168+a134); + a134=casadi_sq(a168); + a174=(a174+a134); + a134=(a164/a174); + a150=arg[3]? arg[3][14] : 0; + a172=(a134*a150); + a157=(a167*a172); + a173=(a168/a174); + a165=(a173*a150); + a179=(a162*a165); + a157=(a157+a179); + a179=cos(a123); + a169=(a157*a179); + a178=sin(a123); + a154=cos(a123); + a159=(a161*a154); + a177=sin(a123); + a176=(a162*a177); + a159=(a159+a176); + a176=(a159/a174); + a134=(a134/a174); + a164=(a164+a164); + a159=(a164*a159); + a168=(a168+a168); + a180=(a163*a154); + a181=(a167*a177); + a180=(a180-a181); + a181=(a168*a180); + a159=(a159+a181); + a181=(a134*a159); + a176=(a176-a181); + a176=(a150*a176); + a181=(a167*a176); + a180=(a180/a174); + a173=(a173/a174); + a159=(a173*a159); + a180=(a180-a159); + a180=(a150*a180); + a159=(a162*a180); + a181=(a181+a159); + a181=(a178*a181); + a169=(a169+a181); + a181=cos(a123); + a159=(a161*a180); + a182=(a163*a176); + a159=(a159-a182); + a159=(a181*a159); + a182=(a161*a165); + a183=(a163*a172); + a182=(a182-a183); + a123=sin(a123); + a183=(a182*a123); + a159=(a159-a183); + a169=(a169+a159); + a159=arg[0]? arg[0][54] : 0; + a183=arg[3]? arg[3][13] : 0; + a183=(a4*a183); + a184=(a159*a183); + a185=sin(a124); + a184=(a184*a185); + a169=(a169+a184); + a184=arg[3]? arg[3][12] : 0; + a184=(a4*a184); + a185=(a159*a184); + a171=(a38*a171); + a171=(a124+a171); + a186=cos(a171); + a187=(a185*a186); + a188=(a159*a183); + a189=sin(a171); + a190=(a188*a189); + a187=(a187+a190); + a169=(a169+a187); + a187=(a10*a184); + a190=(a159*a187); + a156=(a44*a156); + a156=(a124+a156); + a191=cos(a156); + a192=(a190*a191); + a193=(a10*a183); + a194=(a159*a193); + a195=sin(a156); + a196=(a194*a195); + a192=(a192+a196); + a169=(a169+a192); + a192=(a10*a184); + a196=(a159*a192); + a61=(a44*a61); + a61=(a124+a61); + a197=cos(a61); + a198=(a196*a197); + a199=(a10*a183); + a200=(a159*a199); + a201=sin(a61); + a202=(a200*a201); + a198=(a198+a202); + a169=(a169+a198); + a159=(a159*a184); + a198=cos(a124); + a159=(a159*a198); + a169=(a169+a159); + a159=(a120*a126); + a198=sin(a124); + a159=(a159*a198); + a198=cos(a124); + a202=(a120*a155); + a202=(a198*a202); + a159=(a159+a202); + a169=(a169+a159); + a159=(a112*a126); + a202=cos(a124); + a159=(a159*a202); + a202=sin(a124); + a155=(a112*a155); + a155=(a202*a155); + a159=(a159-a155); + a169=(a169+a159); + a159=cos(a124); + a155=(a112*a175); + a155=(a159*a155); + a203=(a112*a119); + a204=sin(a124); + a203=(a203*a204); + a155=(a155-a203); + a169=(a169-a155); + a155=(a120*a119); + a203=cos(a124); + a155=(a155*a203); + a203=sin(a124); + a175=(a120*a175); + a175=(a203*a175); + a155=(a155+a175); + a169=(a169-a155); + a155=arg[1]? arg[1][11] : 0; + a175=cos(a155); + a204=cos(a155); + a205=sin(a124); + a206=(a204*a205); + a207=sin(a155); + a208=cos(a124); + a209=(a207*a208); + a206=(a206-a209); + a209=casadi_sq(a206); + a208=(a175*a208); + a210=sin(a155); + a205=(a210*a205); + a208=(a208+a205); + a205=casadi_sq(a208); + a209=(a209+a205); + a205=(a206/a209); + a211=cos(a155); + a212=sin(a124); + a213=(a211*a212); + a214=sin(a155); + a215=cos(a124); + a216=(a214*a215); + a213=(a213-a216); + a216=cos(a155); + a215=(a216*a215); + a155=sin(a155); + a212=(a155*a212); + a215=(a215+a212); + a212=atan2(a213,a215); + a212=(a212*a34); + a217=(a205*a212); + a218=(a175*a217); + a219=(a208/a209); + a220=(a219*a212); + a221=(a207*a220); + a218=(a218+a221); + a221=cos(a124); + a218=(a218*a221); + a221=sin(a124); + a222=cos(a124); + a223=(a204*a222); + a224=sin(a124); + a225=(a207*a224); + a223=(a223+a225); + a225=(a223/a209); + a226=(a205/a209); + a227=(a206+a206); + a227=(a227*a223); + a228=(a208+a208); + a222=(a210*a222); + a224=(a175*a224); + a222=(a222-a224); + a228=(a228*a222); + a227=(a227+a228); + a226=(a226*a227); + a225=(a225-a226); + a225=(a212*a225); + a226=casadi_sq(a213); + a228=casadi_sq(a215); + a226=(a226+a228); + a228=(a215/a226); + a224=cos(a124); + a229=(a211*a224); + a230=sin(a124); + a231=(a214*a230); + a229=(a229+a231); + a228=(a228*a229); + a226=(a213/a226); + a224=(a155*a224); + a230=(a216*a230); + a224=(a224-a230); + a226=(a226*a224); + a228=(a228-a226); + a228=(a34*a228); + a205=(a205*a228); + a225=(a225+a205); + a175=(a175*a225); + a205=(a222/a209); + a209=(a219/a209); + a209=(a209*a227); + a205=(a205-a209); + a212=(a212*a205); + a219=(a219*a228); + a212=(a212+a219); + a207=(a207*a212); + a175=(a175+a207); + a221=(a221*a175); + a218=(a218+a221); + a169=(a169+a218); + a218=cos(a124); + a212=(a204*a212); + a225=(a210*a225); + a212=(a212-a225); + a218=(a218*a212); + a204=(a204*a220); + a210=(a210*a217); + a204=(a204-a210); + a210=sin(a124); + a204=(a204*a210); + a218=(a218-a204); + a169=(a169+a218); + a218=casadi_sq(a213); + a204=casadi_sq(a215); + a218=(a218+a204); + a204=(a213/a218); + a210=atan2(a206,a208); + a210=(a210*a34); + a217=(a204*a210); + a220=(a216*a217); + a212=(a215/a218); + a225=(a212*a210); + a221=(a214*a225); + a220=(a220+a221); + a221=cos(a124); + a220=(a220*a221); + a221=sin(a124); + a175=(a229/a218); + a207=(a204/a218); + a213=(a213+a213); + a213=(a213*a229); + a215=(a215+a215); + a215=(a215*a224); + a213=(a213+a215); + a207=(a207*a213); + a175=(a175-a207); + a175=(a210*a175); + a207=casadi_sq(a206); + a215=casadi_sq(a208); + a207=(a207+a215); + a208=(a208/a207); + a208=(a208*a223); + a206=(a206/a207); + a206=(a206*a222); + a208=(a208-a206); + a208=(a34*a208); + a204=(a204*a208); + a175=(a175+a204); + a216=(a216*a175); + a224=(a224/a218); + a218=(a212/a218); + a218=(a218*a213); + a224=(a224-a218); + a210=(a210*a224); + a212=(a212*a208); + a210=(a210+a212); + a214=(a214*a210); + a216=(a216+a214); + a221=(a221*a216); + a220=(a220+a221); + a169=(a169+a220); + a220=cos(a124); + a210=(a211*a210); + a175=(a155*a175); + a210=(a210-a175); + a220=(a220*a210); + a211=(a211*a225); + a155=(a155*a217); + a211=(a211-a155); + a155=sin(a124); + a211=(a211*a155); + a220=(a220-a211); + a169=(a169+a220); + if (res[0]!=0) res[0][12]=a169; + a169=(a28*a34); + a220=(a28*a34); + a169=(a169+a220); + if (res[0]!=0) res[0][13]=a169; + a169=(a28*a34); + a220=(a28*a34); + a169=(a169+a220); + if (res[0]!=0) res[0][14]=a169; + a169=sin(a170); + a220=(a158*a176); + a211=(a172*a177); + a220=(a220-a211); + a220=(a169*a220); + a211=cos(a170); + a155=(a172*a154); + a176=(a166*a176); + a155=(a155+a176); + a155=(a211*a155); + a220=(a220-a155); + a155=cos(a170); + a176=(a158*a180); + a217=(a165*a177); + a176=(a176-a217); + a176=(a155*a176); + a220=(a220-a176); + a176=sin(a170); + a217=(a165*a154); + a180=(a166*a180); + a217=(a217+a180); + a217=(a176*a217); + a220=(a220-a217); + if (res[0]!=0) res[0][15]=a220; + a220=arg[0]? arg[0][17] : 0; + a217=cos(a220); + a180=cos(a220); + a160=(a8*a160); + a129=(a5*a129); + a160=(a160+a129); + a129=(a5*a160); + a225=arg[0]? arg[0][57] : 0; + a210=(a8*a225); + a129=(a129+a210); + a210=(a5*a160); + a175=(a8*a225); + a210=(a210+a175); + a175=(a10*a210); + a175=(a129+a175); + a221=(a5*a160); + a216=(a8*a225); + a221=(a221+a216); + a216=(a10*a221); + a175=(a175+a216); + a216=(a5*a160); + a214=(a8*a225); + a216=(a216+a214); + a175=(a175+a216); + a175=(a4*a175); + a175=(a170+a175); + a216=sin(a175); + a214=(a180*a216); + a212=sin(a220); + a208=cos(a175); + a224=(a212*a208); + a214=(a214-a224); + a224=casadi_sq(a214); + a218=(a217*a208); + a213=sin(a220); + a204=(a213*a216); + a218=(a218+a204); + a204=casadi_sq(a218); + a224=(a224+a204); + a204=(a214/a224); + a206=arg[3]? arg[3][17] : 0; + a222=(a204*a206); + a207=(a217*a222); + a223=(a218/a224); + a215=(a223*a206); + a229=(a212*a215); + a207=(a207+a229); + a229=cos(a175); + a219=(a207*a229); + a228=sin(a175); + a205=cos(a175); + a209=(a180*a205); + a227=sin(a175); + a226=(a212*a227); + a209=(a209+a226); + a226=(a209/a224); + a204=(a204/a224); + a214=(a214+a214); + a209=(a214*a209); + a218=(a218+a218); + a230=(a213*a205); + a231=(a217*a227); + a230=(a230-a231); + a231=(a218*a230); + a209=(a209+a231); + a231=(a204*a209); + a226=(a226-a231); + a226=(a206*a226); + a231=(a217*a226); + a230=(a230/a224); + a223=(a223/a224); + a209=(a223*a209); + a230=(a230-a209); + a230=(a206*a230); + a209=(a212*a230); + a231=(a231+a209); + a231=(a228*a231); + a219=(a219+a231); + a231=cos(a175); + a230=(a180*a230); + a226=(a213*a226); + a230=(a230-a226); + a230=(a231*a230); + a226=(a180*a215); + a209=(a213*a222); + a226=(a226-a209); + a175=sin(a175); + a209=(a226*a175); + a230=(a230-a209); + a219=(a219+a230); + a230=arg[0]? arg[0][56] : 0; + a209=arg[3]? arg[3][16] : 0; + a209=(a4*a209); + a232=(a230*a209); + a233=sin(a170); + a232=(a232*a233); + a219=(a219+a232); + a232=arg[3]? arg[3][15] : 0; + a232=(a4*a232); + a233=(a230*a232); + a221=(a38*a221); + a221=(a170+a221); + a234=cos(a221); + a235=(a233*a234); + a236=(a230*a209); + a237=sin(a221); + a238=(a236*a237); + a235=(a235+a238); + a219=(a219+a235); + a235=(a10*a232); + a238=(a230*a235); + a210=(a44*a210); + a210=(a170+a210); + a239=cos(a210); + a240=(a238*a239); + a241=(a10*a209); + a242=(a230*a241); + a243=sin(a210); + a244=(a242*a243); + a240=(a240+a244); + a219=(a219+a240); + a240=(a10*a232); + a244=(a230*a240); + a129=(a44*a129); + a129=(a170+a129); + a245=cos(a129); + a246=(a244*a245); + a247=(a10*a209); + a248=(a230*a247); + a249=sin(a129); + a250=(a248*a249); + a246=(a246+a250); + a219=(a219+a246); + a230=(a230*a232); + a246=cos(a170); + a230=(a230*a246); + a219=(a219+a230); + a230=(a166*a172); + a246=sin(a170); + a230=(a230*a246); + a246=sin(a170); + a246=(a166*a246); + a250=cos(a170); + a250=(a158*a250); + a246=(a246+a250); + a250=(a246/a174); + a251=cos(a170); + a251=(a166*a251); + a252=sin(a170); + a252=(a158*a252); + a251=(a251-a252); + a252=(a168*a251); + a246=(a164*a246); + a252=(a252-a246); + a246=(a134*a252); + a250=(a250+a246); + a250=(a150*a250); + a246=(a166*a250); + a246=(a211*a246); + a230=(a230+a246); + a219=(a219+a230); + a230=(a158*a172); + a246=cos(a170); + a230=(a230*a246); + a250=(a158*a250); + a250=(a169*a250); + a230=(a230-a250); + a219=(a219+a230); + a251=(a251/a174); + a252=(a173*a252); + a251=(a251-a252); + a251=(a150*a251); + a252=(a158*a251); + a252=(a155*a252); + a230=(a158*a165); + a250=sin(a170); + a230=(a230*a250); + a252=(a252-a230); + a219=(a219-a252); + a252=(a166*a165); + a230=cos(a170); + a252=(a252*a230); + a251=(a166*a251); + a251=(a176*a251); + a252=(a252+a251); + a219=(a219-a252); + a252=arg[1]? arg[1][14] : 0; + a251=cos(a252); + a230=cos(a252); + a250=sin(a170); + a246=(a230*a250); + a253=sin(a252); + a254=cos(a170); + a255=(a253*a254); + a246=(a246-a255); + a255=casadi_sq(a246); + a254=(a251*a254); + a256=sin(a252); + a250=(a256*a250); + a254=(a254+a250); + a250=casadi_sq(a254); + a255=(a255+a250); + a250=(a246/a255); + a257=cos(a252); + a258=sin(a170); + a259=(a257*a258); + a260=sin(a252); + a261=cos(a170); + a262=(a260*a261); + a259=(a259-a262); + a262=cos(a252); + a261=(a262*a261); + a252=sin(a252); + a258=(a252*a258); + a261=(a261+a258); + a258=atan2(a259,a261); + a258=(a258*a34); + a263=(a250*a258); + a264=(a251*a263); + a265=(a254/a255); + a266=(a265*a258); + a267=(a253*a266); + a264=(a264+a267); + a267=cos(a170); + a264=(a264*a267); + a267=sin(a170); + a268=cos(a170); + a269=(a230*a268); + a270=sin(a170); + a271=(a253*a270); + a269=(a269+a271); + a271=(a269/a255); + a272=(a250/a255); + a273=(a246+a246); + a273=(a273*a269); + a274=(a254+a254); + a268=(a256*a268); + a270=(a251*a270); + a268=(a268-a270); + a274=(a274*a268); + a273=(a273+a274); + a272=(a272*a273); + a271=(a271-a272); + a271=(a258*a271); + a272=casadi_sq(a259); + a274=casadi_sq(a261); + a272=(a272+a274); + a274=(a261/a272); + a270=cos(a170); + a275=(a257*a270); + a276=sin(a170); + a277=(a260*a276); + a275=(a275+a277); + a274=(a274*a275); + a272=(a259/a272); + a270=(a252*a270); + a276=(a262*a276); + a270=(a270-a276); + a272=(a272*a270); + a274=(a274-a272); + a274=(a34*a274); + a250=(a250*a274); + a271=(a271+a250); + a251=(a251*a271); + a250=(a268/a255); + a255=(a265/a255); + a255=(a255*a273); + a250=(a250-a255); + a258=(a258*a250); + a265=(a265*a274); + a258=(a258+a265); + a253=(a253*a258); + a251=(a251+a253); + a267=(a267*a251); + a264=(a264+a267); + a219=(a219+a264); + a264=cos(a170); + a258=(a230*a258); + a271=(a256*a271); + a258=(a258-a271); + a264=(a264*a258); + a230=(a230*a266); + a256=(a256*a263); + a230=(a230-a256); + a256=sin(a170); + a230=(a230*a256); + a264=(a264-a230); + a219=(a219+a264); + a264=casadi_sq(a259); + a230=casadi_sq(a261); + a264=(a264+a230); + a230=(a259/a264); + a256=atan2(a246,a254); + a256=(a256*a34); + a263=(a230*a256); + a266=(a262*a263); + a258=(a261/a264); + a271=(a258*a256); + a267=(a260*a271); + a266=(a266+a267); + a267=cos(a170); + a266=(a266*a267); + a267=sin(a170); + a251=(a275/a264); + a253=(a230/a264); + a259=(a259+a259); + a259=(a259*a275); + a261=(a261+a261); + a261=(a261*a270); + a259=(a259+a261); + a253=(a253*a259); + a251=(a251-a253); + a251=(a256*a251); + a253=casadi_sq(a246); + a261=casadi_sq(a254); + a253=(a253+a261); + a254=(a254/a253); + a254=(a254*a269); + a246=(a246/a253); + a246=(a246*a268); + a254=(a254-a246); + a254=(a34*a254); + a230=(a230*a254); + a251=(a251+a230); + a262=(a262*a251); + a270=(a270/a264); + a264=(a258/a264); + a264=(a264*a259); + a270=(a270-a264); + a256=(a256*a270); + a258=(a258*a254); + a256=(a256+a258); + a260=(a260*a256); + a262=(a262+a260); + a267=(a267*a262); + a266=(a266+a267); + a219=(a219+a266); + a266=cos(a170); + a256=(a257*a256); + a251=(a252*a251); + a256=(a256-a251); + a266=(a266*a256); + a257=(a257*a271); + a252=(a252*a263); + a257=(a257-a252); + a252=sin(a170); + a257=(a257*a252); + a266=(a266-a257); + a219=(a219+a266); + if (res[0]!=0) res[0][16]=a219; + a219=(a28*a34); + a266=(a28*a34); + a219=(a219+a266); + if (res[0]!=0) res[0][17]=a219; + a219=(a28*a34); + a266=(a28*a34); + a219=(a219+a266); + if (res[0]!=0) res[0][18]=a219; + a219=cos(a220); + a266=(a215*a219); + a257=cos(a220); + a252=(a216*a257); + a263=sin(a220); + a271=(a208*a263); + a252=(a252-a271); + a271=(a252/a224); + a252=(a218*a252); + a256=sin(a220); + a251=(a216*a256); + a219=(a208*a219); + a251=(a251+a219); + a219=(a214*a251); + a252=(a252-a219); + a219=(a223*a252); + a271=(a271-a219); + a271=(a206*a271); + a219=(a212*a271); + a266=(a266+a219); + a263=(a222*a263); + a251=(a251/a224); + a252=(a204*a252); + a251=(a251+a252); + a251=(a206*a251); + a252=(a217*a251); + a263=(a263+a252); + a266=(a266-a263); + a266=(a228*a266); + a263=(a180*a271); + a256=(a215*a256); + a263=(a263-a256); + a257=(a222*a257); + a256=(a213*a251); + a257=(a257-a256); + a263=(a263-a257); + a263=(a231*a263); + a266=(a266+a263); + if (res[0]!=0) res[0][19]=a266; + a266=arg[0]? arg[0][20] : 0; + a263=cos(a266); + a257=cos(a266); + a225=(a8*a225); + a160=(a5*a160); + a225=(a225+a160); + a160=(a5*a225); + a256=arg[0]? arg[0][59] : 0; + a252=(a8*a256); + a160=(a160+a252); + a252=(a5*a225); + a219=(a8*a256); + a252=(a252+a219); + a219=(a10*a252); + a219=(a160+a219); + a267=(a5*a225); + a262=(a8*a256); + a267=(a267+a262); + a262=(a10*a267); + a219=(a219+a262); + a262=(a5*a225); + a260=(a8*a256); + a262=(a262+a260); + a219=(a219+a262); + a219=(a4*a219); + a219=(a220+a219); + a262=sin(a219); + a260=(a257*a262); + a258=sin(a266); + a254=cos(a219); + a270=(a258*a254); + a260=(a260-a270); + a270=casadi_sq(a260); + a264=(a263*a254); + a259=sin(a266); + a230=(a259*a262); + a264=(a264+a230); + a230=casadi_sq(a264); + a270=(a270+a230); + a230=(a260/a270); + a246=arg[3]? arg[3][20] : 0; + a268=(a230*a246); + a253=(a263*a268); + a269=(a264/a270); + a261=(a269*a246); + a275=(a258*a261); + a253=(a253+a275); + a275=cos(a219); + a265=(a253*a275); + a274=sin(a219); + a250=cos(a219); + a255=(a257*a250); + a273=sin(a219); + a272=(a258*a273); + a255=(a255+a272); + a272=(a255/a270); + a230=(a230/a270); + a260=(a260+a260); + a255=(a260*a255); + a264=(a264+a264); + a276=(a259*a250); + a277=(a263*a273); + a276=(a276-a277); + a277=(a264*a276); + a255=(a255+a277); + a277=(a230*a255); + a272=(a272-a277); + a272=(a246*a272); + a277=(a263*a272); + a276=(a276/a270); + a269=(a269/a270); + a255=(a269*a255); + a276=(a276-a255); + a276=(a246*a276); + a255=(a258*a276); + a277=(a277+a255); + a277=(a274*a277); + a265=(a265+a277); + a277=cos(a219); + a255=(a257*a276); + a278=(a259*a272); + a255=(a255-a278); + a255=(a277*a255); + a278=(a257*a261); + a279=(a259*a268); + a278=(a278-a279); + a219=sin(a219); + a279=(a278*a219); + a255=(a255-a279); + a265=(a265+a255); + a255=arg[0]? arg[0][58] : 0; + a279=arg[3]? arg[3][19] : 0; + a279=(a4*a279); + a280=(a255*a279); + a281=sin(a220); + a280=(a280*a281); + a265=(a265+a280); + a280=arg[3]? arg[3][18] : 0; + a280=(a4*a280); + a281=(a255*a280); + a267=(a38*a267); + a267=(a220+a267); + a282=cos(a267); + a283=(a281*a282); + a284=(a255*a279); + a285=sin(a267); + a286=(a284*a285); + a283=(a283+a286); + a265=(a265+a283); + a283=(a10*a280); + a286=(a255*a283); + a252=(a44*a252); + a252=(a220+a252); + a287=cos(a252); + a288=(a286*a287); + a289=(a10*a279); + a290=(a255*a289); + a291=sin(a252); + a292=(a290*a291); + a288=(a288+a292); + a265=(a265+a288); + a288=(a10*a280); + a292=(a255*a288); + a160=(a44*a160); + a160=(a220+a160); + a293=cos(a160); + a294=(a292*a293); + a295=(a10*a279); + a296=(a255*a295); + a297=sin(a160); + a298=(a296*a297); + a294=(a294+a298); + a265=(a265+a294); + a255=(a255*a280); + a294=cos(a220); + a255=(a255*a294); + a265=(a265+a255); + a255=(a216*a222); + a294=sin(a220); + a255=(a255*a294); + a294=cos(a220); + a298=(a216*a251); + a298=(a294*a298); + a255=(a255+a298); + a265=(a265+a255); + a255=(a208*a222); + a298=cos(a220); + a255=(a255*a298); + a298=sin(a220); + a251=(a208*a251); + a251=(a298*a251); + a255=(a255-a251); + a265=(a265+a255); + a255=cos(a220); + a251=(a208*a271); + a251=(a255*a251); + a299=(a208*a215); + a300=sin(a220); + a299=(a299*a300); + a251=(a251-a299); + a265=(a265-a251); + a251=(a216*a215); + a299=cos(a220); + a251=(a251*a299); + a299=sin(a220); + a271=(a216*a271); + a271=(a299*a271); + a251=(a251+a271); + a265=(a265-a251); + a251=arg[1]? arg[1][17] : 0; + a271=cos(a251); + a300=cos(a251); + a301=sin(a220); + a302=(a300*a301); + a303=sin(a251); + a304=cos(a220); + a305=(a303*a304); + a302=(a302-a305); + a305=casadi_sq(a302); + a304=(a271*a304); + a306=sin(a251); + a301=(a306*a301); + a304=(a304+a301); + a301=casadi_sq(a304); + a305=(a305+a301); + a301=(a302/a305); + a307=cos(a251); + a308=sin(a220); + a309=(a307*a308); + a310=sin(a251); + a311=cos(a220); + a312=(a310*a311); + a309=(a309-a312); + a312=cos(a251); + a311=(a312*a311); + a251=sin(a251); + a308=(a251*a308); + a311=(a311+a308); + a308=atan2(a309,a311); + a308=(a308*a34); + a313=(a301*a308); + a314=(a271*a313); + a315=(a304/a305); + a316=(a315*a308); + a317=(a303*a316); + a314=(a314+a317); + a317=cos(a220); + a314=(a314*a317); + a317=sin(a220); + a318=cos(a220); + a319=(a300*a318); + a320=sin(a220); + a321=(a303*a320); + a319=(a319+a321); + a321=(a319/a305); + a322=(a301/a305); + a323=(a302+a302); + a323=(a323*a319); + a324=(a304+a304); + a318=(a306*a318); + a320=(a271*a320); + a318=(a318-a320); + a324=(a324*a318); + a323=(a323+a324); + a322=(a322*a323); + a321=(a321-a322); + a321=(a308*a321); + a322=casadi_sq(a309); + a324=casadi_sq(a311); + a322=(a322+a324); + a324=(a311/a322); + a320=cos(a220); + a325=(a307*a320); + a326=sin(a220); + a327=(a310*a326); + a325=(a325+a327); + a324=(a324*a325); + a322=(a309/a322); + a320=(a251*a320); + a326=(a312*a326); + a320=(a320-a326); + a322=(a322*a320); + a324=(a324-a322); + a324=(a34*a324); + a301=(a301*a324); + a321=(a321+a301); + a271=(a271*a321); + a301=(a318/a305); + a305=(a315/a305); + a305=(a305*a323); + a301=(a301-a305); + a308=(a308*a301); + a315=(a315*a324); + a308=(a308+a315); + a303=(a303*a308); + a271=(a271+a303); + a317=(a317*a271); + a314=(a314+a317); + a265=(a265+a314); + a314=cos(a220); + a308=(a300*a308); + a321=(a306*a321); + a308=(a308-a321); + a314=(a314*a308); + a300=(a300*a316); + a306=(a306*a313); + a300=(a300-a306); + a306=sin(a220); + a300=(a300*a306); + a314=(a314-a300); + a265=(a265+a314); + a314=casadi_sq(a309); + a300=casadi_sq(a311); + a314=(a314+a300); + a300=(a309/a314); + a306=atan2(a302,a304); + a306=(a306*a34); + a313=(a300*a306); + a316=(a312*a313); + a308=(a311/a314); + a321=(a308*a306); + a317=(a310*a321); + a316=(a316+a317); + a317=cos(a220); + a316=(a316*a317); + a317=sin(a220); + a271=(a325/a314); + a303=(a300/a314); + a309=(a309+a309); + a309=(a309*a325); + a311=(a311+a311); + a311=(a311*a320); + a309=(a309+a311); + a303=(a303*a309); + a271=(a271-a303); + a271=(a306*a271); + a303=casadi_sq(a302); + a311=casadi_sq(a304); + a303=(a303+a311); + a304=(a304/a303); + a304=(a304*a319); + a302=(a302/a303); + a302=(a302*a318); + a304=(a304-a302); + a304=(a34*a304); + a300=(a300*a304); + a271=(a271+a300); + a312=(a312*a271); + a320=(a320/a314); + a314=(a308/a314); + a314=(a314*a309); + a320=(a320-a314); + a306=(a306*a320); + a308=(a308*a304); + a306=(a306+a308); + a310=(a310*a306); + a312=(a312+a310); + a317=(a317*a312); + a316=(a316+a317); + a265=(a265+a316); + a316=cos(a220); + a306=(a307*a306); + a271=(a251*a271); + a306=(a306-a271); + a316=(a316*a306); + a307=(a307*a321); + a251=(a251*a313); + a307=(a307-a251); + a251=sin(a220); + a307=(a307*a251); + a316=(a316-a307); + a265=(a265+a316); + if (res[0]!=0) res[0][20]=a265; + a265=(a28*a34); + a316=(a28*a34); + a265=(a265+a316); + if (res[0]!=0) res[0][21]=a265; + a265=(a28*a34); + a316=(a28*a34); + a265=(a265+a316); + if (res[0]!=0) res[0][22]=a265; + a265=sin(a266); + a316=(a254*a272); + a307=(a268*a273); + a316=(a316-a307); + a316=(a265*a316); + a307=cos(a266); + a251=(a268*a250); + a272=(a262*a272); + a251=(a251+a272); + a251=(a307*a251); + a316=(a316-a251); + a251=cos(a266); + a272=(a254*a276); + a313=(a261*a273); + a272=(a272-a313); + a272=(a251*a272); + a316=(a316-a272); + a272=sin(a266); + a313=(a261*a250); + a276=(a262*a276); + a313=(a313+a276); + a313=(a272*a313); + a316=(a316-a313); + if (res[0]!=0) res[0][23]=a316; + a316=arg[0]? arg[0][23] : 0; + a313=cos(a316); + a276=cos(a316); + a256=(a8*a256); + a225=(a5*a225); + a256=(a256+a225); + a225=(a5*a256); + a321=arg[0]? arg[0][61] : 0; + a306=(a8*a321); + a225=(a225+a306); + a306=(a5*a256); + a271=(a8*a321); + a306=(a306+a271); + a271=(a10*a306); + a271=(a225+a271); + a317=(a5*a256); + a312=(a8*a321); + a317=(a317+a312); + a312=(a10*a317); + a271=(a271+a312); + a312=(a5*a256); + a310=(a8*a321); + a312=(a312+a310); + a271=(a271+a312); + a271=(a4*a271); + a271=(a266+a271); + a312=sin(a271); + a310=(a276*a312); + a308=sin(a316); + a304=cos(a271); + a320=(a308*a304); + a310=(a310-a320); + a320=casadi_sq(a310); + a314=(a313*a304); + a309=sin(a316); + a300=(a309*a312); + a314=(a314+a300); + a300=casadi_sq(a314); + a320=(a320+a300); + a300=(a310/a320); + a302=arg[3]? arg[3][23] : 0; + a318=(a300*a302); + a303=(a313*a318); + a319=(a314/a320); + a311=(a319*a302); + a325=(a308*a311); + a303=(a303+a325); + a325=cos(a271); + a315=(a303*a325); + a324=sin(a271); + a301=cos(a271); + a305=(a276*a301); + a323=sin(a271); + a322=(a308*a323); + a305=(a305+a322); + a322=(a305/a320); + a300=(a300/a320); + a310=(a310+a310); + a305=(a310*a305); + a314=(a314+a314); + a326=(a309*a301); + a327=(a313*a323); + a326=(a326-a327); + a327=(a314*a326); + a305=(a305+a327); + a327=(a300*a305); + a322=(a322-a327); + a322=(a302*a322); + a327=(a313*a322); + a326=(a326/a320); + a319=(a319/a320); + a305=(a319*a305); + a326=(a326-a305); + a326=(a302*a326); + a305=(a308*a326); + a327=(a327+a305); + a327=(a324*a327); + a315=(a315+a327); + a327=cos(a271); + a326=(a276*a326); + a322=(a309*a322); + a326=(a326-a322); + a326=(a327*a326); + a322=(a276*a311); + a305=(a309*a318); + a322=(a322-a305); + a271=sin(a271); + a305=(a322*a271); + a326=(a326-a305); + a315=(a315+a326); + a326=arg[0]? arg[0][60] : 0; + a305=arg[3]? arg[3][22] : 0; + a305=(a4*a305); + a328=(a326*a305); + a329=sin(a266); + a328=(a328*a329); + a315=(a315+a328); + a328=arg[3]? arg[3][21] : 0; + a328=(a4*a328); + a329=(a326*a328); + a317=(a38*a317); + a317=(a266+a317); + a330=cos(a317); + a331=(a329*a330); + a332=(a326*a305); + a333=sin(a317); + a334=(a332*a333); + a331=(a331+a334); + a315=(a315+a331); + a331=(a10*a328); + a334=(a326*a331); + a306=(a44*a306); + a306=(a266+a306); + a335=cos(a306); + a336=(a334*a335); + a337=(a10*a305); + a338=(a326*a337); + a339=sin(a306); + a340=(a338*a339); + a336=(a336+a340); + a315=(a315+a336); + a336=(a10*a328); + a340=(a326*a336); + a225=(a44*a225); + a225=(a266+a225); + a341=cos(a225); + a342=(a340*a341); + a343=(a10*a305); + a344=(a326*a343); + a345=sin(a225); + a346=(a344*a345); + a342=(a342+a346); + a315=(a315+a342); + a326=(a326*a328); + a342=cos(a266); + a326=(a326*a342); + a315=(a315+a326); + a326=(a262*a268); + a342=sin(a266); + a326=(a326*a342); + a342=sin(a266); + a342=(a262*a342); + a346=cos(a266); + a346=(a254*a346); + a342=(a342+a346); + a346=(a342/a270); + a347=cos(a266); + a347=(a262*a347); + a348=sin(a266); + a348=(a254*a348); + a347=(a347-a348); + a348=(a264*a347); + a342=(a260*a342); + a348=(a348-a342); + a342=(a230*a348); + a346=(a346+a342); + a346=(a246*a346); + a342=(a262*a346); + a342=(a307*a342); + a326=(a326+a342); + a315=(a315+a326); + a326=(a254*a268); + a342=cos(a266); + a326=(a326*a342); + a346=(a254*a346); + a346=(a265*a346); + a326=(a326-a346); + a315=(a315+a326); + a347=(a347/a270); + a348=(a269*a348); + a347=(a347-a348); + a347=(a246*a347); + a348=(a254*a347); + a348=(a251*a348); + a326=(a254*a261); + a346=sin(a266); + a326=(a326*a346); + a348=(a348-a326); + a315=(a315-a348); + a348=(a262*a261); + a326=cos(a266); + a348=(a348*a326); + a347=(a262*a347); + a347=(a272*a347); + a348=(a348+a347); + a315=(a315-a348); + a348=arg[1]? arg[1][20] : 0; + a347=cos(a348); + a326=cos(a348); + a346=sin(a266); + a342=(a326*a346); + a349=sin(a348); + a350=cos(a266); + a351=(a349*a350); + a342=(a342-a351); + a351=casadi_sq(a342); + a350=(a347*a350); + a352=sin(a348); + a346=(a352*a346); + a350=(a350+a346); + a346=casadi_sq(a350); + a351=(a351+a346); + a346=(a342/a351); + a353=cos(a348); + a354=sin(a266); + a355=(a353*a354); + a356=sin(a348); + a357=cos(a266); + a358=(a356*a357); + a355=(a355-a358); + a358=cos(a348); + a357=(a358*a357); + a348=sin(a348); + a354=(a348*a354); + a357=(a357+a354); + a354=atan2(a355,a357); + a354=(a354*a34); + a359=(a346*a354); + a360=(a347*a359); + a361=(a350/a351); + a362=(a361*a354); + a363=(a349*a362); + a360=(a360+a363); + a363=cos(a266); + a360=(a360*a363); + a363=sin(a266); + a364=cos(a266); + a365=(a326*a364); + a366=sin(a266); + a367=(a349*a366); + a365=(a365+a367); + a367=(a365/a351); + a368=(a346/a351); + a369=(a342+a342); + a369=(a369*a365); + a370=(a350+a350); + a364=(a352*a364); + a366=(a347*a366); + a364=(a364-a366); + a370=(a370*a364); + a369=(a369+a370); + a368=(a368*a369); + a367=(a367-a368); + a367=(a354*a367); + a368=casadi_sq(a355); + a370=casadi_sq(a357); + a368=(a368+a370); + a370=(a357/a368); + a366=cos(a266); + a371=(a353*a366); + a372=sin(a266); + a373=(a356*a372); + a371=(a371+a373); + a370=(a370*a371); + a368=(a355/a368); + a366=(a348*a366); + a372=(a358*a372); + a366=(a366-a372); + a368=(a368*a366); + a370=(a370-a368); + a370=(a34*a370); + a346=(a346*a370); + a367=(a367+a346); + a347=(a347*a367); + a346=(a364/a351); + a351=(a361/a351); + a351=(a351*a369); + a346=(a346-a351); + a354=(a354*a346); + a361=(a361*a370); + a354=(a354+a361); + a349=(a349*a354); + a347=(a347+a349); + a363=(a363*a347); + a360=(a360+a363); + a315=(a315+a360); + a360=cos(a266); + a354=(a326*a354); + a367=(a352*a367); + a354=(a354-a367); + a360=(a360*a354); + a326=(a326*a362); + a352=(a352*a359); + a326=(a326-a352); + a352=sin(a266); + a326=(a326*a352); + a360=(a360-a326); + a315=(a315+a360); + a360=casadi_sq(a355); + a326=casadi_sq(a357); + a360=(a360+a326); + a326=(a355/a360); + a352=atan2(a342,a350); + a352=(a352*a34); + a359=(a326*a352); + a362=(a358*a359); + a354=(a357/a360); + a367=(a354*a352); + a363=(a356*a367); + a362=(a362+a363); + a363=cos(a266); + a362=(a362*a363); + a363=sin(a266); + a347=(a371/a360); + a349=(a326/a360); + a355=(a355+a355); + a355=(a355*a371); + a357=(a357+a357); + a357=(a357*a366); + a355=(a355+a357); + a349=(a349*a355); + a347=(a347-a349); + a347=(a352*a347); + a349=casadi_sq(a342); + a357=casadi_sq(a350); + a349=(a349+a357); + a350=(a350/a349); + a350=(a350*a365); + a342=(a342/a349); + a342=(a342*a364); + a350=(a350-a342); + a350=(a34*a350); + a326=(a326*a350); + a347=(a347+a326); + a358=(a358*a347); + a366=(a366/a360); + a360=(a354/a360); + a360=(a360*a355); + a366=(a366-a360); + a352=(a352*a366); + a354=(a354*a350); + a352=(a352+a354); + a356=(a356*a352); + a358=(a358+a356); + a363=(a363*a358); + a362=(a362+a363); + a315=(a315+a362); + a362=cos(a266); + a352=(a353*a352); + a347=(a348*a347); + a352=(a352-a347); + a362=(a362*a352); + a353=(a353*a367); + a348=(a348*a359); + a353=(a353-a348); + a348=sin(a266); + a353=(a353*a348); + a362=(a362-a353); + a315=(a315+a362); + if (res[0]!=0) res[0][24]=a315; + a315=(a28*a34); + a362=(a28*a34); + a315=(a315+a362); + if (res[0]!=0) res[0][25]=a315; + a315=(a28*a34); + a362=(a28*a34); + a315=(a315+a362); + if (res[0]!=0) res[0][26]=a315; + a315=cos(a316); + a362=(a311*a315); + a353=cos(a316); + a348=(a312*a353); + a359=sin(a316); + a367=(a304*a359); + a348=(a348-a367); + a367=(a348/a320); + a348=(a314*a348); + a352=sin(a316); + a347=(a312*a352); + a315=(a304*a315); + a347=(a347+a315); + a315=(a310*a347); + a348=(a348-a315); + a315=(a319*a348); + a367=(a367-a315); + a367=(a302*a367); + a315=(a308*a367); + a362=(a362+a315); + a359=(a318*a359); + a347=(a347/a320); + a348=(a300*a348); + a347=(a347+a348); + a347=(a302*a347); + a348=(a313*a347); + a359=(a359+a348); + a362=(a362-a359); + a362=(a324*a362); + a359=(a276*a367); + a352=(a311*a352); + a359=(a359-a352); + a353=(a318*a353); + a352=(a309*a347); + a353=(a353-a352); + a359=(a359-a353); + a359=(a327*a359); + a362=(a362+a359); + if (res[0]!=0) res[0][27]=a362; + a362=arg[0]? arg[0][26] : 0; + a359=cos(a362); + a353=cos(a362); + a321=(a8*a321); + a256=(a5*a256); + a321=(a321+a256); + a256=(a5*a321); + a352=arg[0]? arg[0][63] : 0; + a348=(a8*a352); + a256=(a256+a348); + a348=(a5*a321); + a315=(a8*a352); + a348=(a348+a315); + a315=(a10*a348); + a315=(a256+a315); + a363=(a5*a321); + a358=(a8*a352); + a363=(a363+a358); + a358=(a10*a363); + a315=(a315+a358); + a358=(a5*a321); + a356=(a8*a352); + a358=(a358+a356); + a315=(a315+a358); + a315=(a4*a315); + a315=(a316+a315); + a358=sin(a315); + a356=(a353*a358); + a354=sin(a362); + a350=cos(a315); + a366=(a354*a350); + a356=(a356-a366); + a366=casadi_sq(a356); + a360=(a359*a350); + a355=sin(a362); + a326=(a355*a358); + a360=(a360+a326); + a326=casadi_sq(a360); + a366=(a366+a326); + a326=(a356/a366); + a342=arg[3]? arg[3][26] : 0; + a364=(a326*a342); + a349=(a359*a364); + a365=(a360/a366); + a357=(a365*a342); + a371=(a354*a357); + a349=(a349+a371); + a371=cos(a315); + a361=(a349*a371); + a370=sin(a315); + a346=cos(a315); + a351=(a353*a346); + a369=sin(a315); + a368=(a354*a369); + a351=(a351+a368); + a368=(a351/a366); + a326=(a326/a366); + a356=(a356+a356); + a351=(a356*a351); + a360=(a360+a360); + a372=(a355*a346); + a373=(a359*a369); + a372=(a372-a373); + a373=(a360*a372); + a351=(a351+a373); + a373=(a326*a351); + a368=(a368-a373); + a368=(a342*a368); + a373=(a359*a368); + a372=(a372/a366); + a365=(a365/a366); + a351=(a365*a351); + a372=(a372-a351); + a372=(a342*a372); + a351=(a354*a372); + a373=(a373+a351); + a373=(a370*a373); + a361=(a361+a373); + a373=cos(a315); + a351=(a353*a372); + a374=(a355*a368); + a351=(a351-a374); + a351=(a373*a351); + a374=(a353*a357); + a375=(a355*a364); + a374=(a374-a375); + a315=sin(a315); + a375=(a374*a315); + a351=(a351-a375); + a361=(a361+a351); + a351=arg[0]? arg[0][62] : 0; + a375=arg[3]? arg[3][25] : 0; + a375=(a4*a375); + a376=(a351*a375); + a377=sin(a316); + a376=(a376*a377); + a361=(a361+a376); + a376=arg[3]? arg[3][24] : 0; + a376=(a4*a376); + a377=(a351*a376); + a363=(a38*a363); + a363=(a316+a363); + a378=cos(a363); + a379=(a377*a378); + a380=(a351*a375); + a381=sin(a363); + a382=(a380*a381); + a379=(a379+a382); + a361=(a361+a379); + a379=(a10*a376); + a382=(a351*a379); + a348=(a44*a348); + a348=(a316+a348); + a383=cos(a348); + a384=(a382*a383); + a385=(a10*a375); + a386=(a351*a385); + a387=sin(a348); + a388=(a386*a387); + a384=(a384+a388); + a361=(a361+a384); + a384=(a10*a376); + a388=(a351*a384); + a256=(a44*a256); + a256=(a316+a256); + a389=cos(a256); + a390=(a388*a389); + a391=(a10*a375); + a392=(a351*a391); + a393=sin(a256); + a394=(a392*a393); + a390=(a390+a394); + a361=(a361+a390); + a351=(a351*a376); + a390=cos(a316); + a351=(a351*a390); + a361=(a361+a351); + a351=(a312*a318); + a390=sin(a316); + a351=(a351*a390); + a390=cos(a316); + a394=(a312*a347); + a394=(a390*a394); + a351=(a351+a394); + a361=(a361+a351); + a351=(a304*a318); + a394=cos(a316); + a351=(a351*a394); + a394=sin(a316); + a347=(a304*a347); + a347=(a394*a347); + a351=(a351-a347); + a361=(a361+a351); + a351=cos(a316); + a347=(a304*a367); + a347=(a351*a347); + a395=(a304*a311); + a396=sin(a316); + a395=(a395*a396); + a347=(a347-a395); + a361=(a361-a347); + a347=(a312*a311); + a395=cos(a316); + a347=(a347*a395); + a395=sin(a316); + a367=(a312*a367); + a367=(a395*a367); + a347=(a347+a367); + a361=(a361-a347); + a347=arg[1]? arg[1][23] : 0; + a367=cos(a347); + a396=cos(a347); + a397=sin(a316); + a398=(a396*a397); + a399=sin(a347); + a400=cos(a316); + a401=(a399*a400); + a398=(a398-a401); + a401=casadi_sq(a398); + a400=(a367*a400); + a402=sin(a347); + a397=(a402*a397); + a400=(a400+a397); + a397=casadi_sq(a400); + a401=(a401+a397); + a397=(a398/a401); + a403=cos(a347); + a404=sin(a316); + a405=(a403*a404); + a406=sin(a347); + a407=cos(a316); + a408=(a406*a407); + a405=(a405-a408); + a408=cos(a347); + a407=(a408*a407); + a347=sin(a347); + a404=(a347*a404); + a407=(a407+a404); + a404=atan2(a405,a407); + a404=(a404*a34); + a409=(a397*a404); + a410=(a367*a409); + a411=(a400/a401); + a412=(a411*a404); + a413=(a399*a412); + a410=(a410+a413); + a413=cos(a316); + a410=(a410*a413); + a413=sin(a316); + a414=cos(a316); + a415=(a396*a414); + a416=sin(a316); + a417=(a399*a416); + a415=(a415+a417); + a417=(a415/a401); + a418=(a397/a401); + a419=(a398+a398); + a419=(a419*a415); + a420=(a400+a400); + a414=(a402*a414); + a416=(a367*a416); + a414=(a414-a416); + a420=(a420*a414); + a419=(a419+a420); + a418=(a418*a419); + a417=(a417-a418); + a417=(a404*a417); + a418=casadi_sq(a405); + a420=casadi_sq(a407); + a418=(a418+a420); + a420=(a407/a418); + a416=cos(a316); + a421=(a403*a416); + a422=sin(a316); + a423=(a406*a422); + a421=(a421+a423); + a420=(a420*a421); + a418=(a405/a418); + a416=(a347*a416); + a422=(a408*a422); + a416=(a416-a422); + a418=(a418*a416); + a420=(a420-a418); + a420=(a34*a420); + a397=(a397*a420); + a417=(a417+a397); + a367=(a367*a417); + a397=(a414/a401); + a401=(a411/a401); + a401=(a401*a419); + a397=(a397-a401); + a404=(a404*a397); + a411=(a411*a420); + a404=(a404+a411); + a399=(a399*a404); + a367=(a367+a399); + a413=(a413*a367); + a410=(a410+a413); + a361=(a361+a410); + a410=cos(a316); + a404=(a396*a404); + a417=(a402*a417); + a404=(a404-a417); + a410=(a410*a404); + a396=(a396*a412); + a402=(a402*a409); + a396=(a396-a402); + a402=sin(a316); + a396=(a396*a402); + a410=(a410-a396); + a361=(a361+a410); + a410=casadi_sq(a405); + a396=casadi_sq(a407); + a410=(a410+a396); + a396=(a405/a410); + a402=atan2(a398,a400); + a402=(a402*a34); + a409=(a396*a402); + a412=(a408*a409); + a404=(a407/a410); + a417=(a404*a402); + a413=(a406*a417); + a412=(a412+a413); + a413=cos(a316); + a412=(a412*a413); + a413=sin(a316); + a367=(a421/a410); + a399=(a396/a410); + a405=(a405+a405); + a405=(a405*a421); + a407=(a407+a407); + a407=(a407*a416); + a405=(a405+a407); + a399=(a399*a405); + a367=(a367-a399); + a367=(a402*a367); + a399=casadi_sq(a398); + a407=casadi_sq(a400); + a399=(a399+a407); + a400=(a400/a399); + a400=(a400*a415); + a398=(a398/a399); + a398=(a398*a414); + a400=(a400-a398); + a400=(a34*a400); + a396=(a396*a400); + a367=(a367+a396); + a408=(a408*a367); + a416=(a416/a410); + a410=(a404/a410); + a410=(a410*a405); + a416=(a416-a410); + a402=(a402*a416); + a404=(a404*a400); + a402=(a402+a404); + a406=(a406*a402); + a408=(a408+a406); + a413=(a413*a408); + a412=(a412+a413); + a361=(a361+a412); + a412=cos(a316); + a402=(a403*a402); + a367=(a347*a367); + a402=(a402-a367); + a412=(a412*a402); + a403=(a403*a417); + a347=(a347*a409); + a403=(a403-a347); + a347=sin(a316); + a403=(a403*a347); + a412=(a412-a403); + a361=(a361+a412); + if (res[0]!=0) res[0][28]=a361; + a361=(a28*a34); + a412=(a28*a34); + a361=(a361+a412); + if (res[0]!=0) res[0][29]=a361; + a361=(a28*a34); + a412=(a28*a34); + a361=(a361+a412); + if (res[0]!=0) res[0][30]=a361; + a361=sin(a362); + a412=(a350*a368); + a403=(a364*a369); + a412=(a412-a403); + a412=(a361*a412); + a403=cos(a362); + a347=(a364*a346); + a368=(a358*a368); + a347=(a347+a368); + a347=(a403*a347); + a412=(a412-a347); + a347=cos(a362); + a368=(a350*a372); + a409=(a357*a369); + a368=(a368-a409); + a368=(a347*a368); + a412=(a412-a368); + a368=sin(a362); + a409=(a357*a346); + a372=(a358*a372); + a409=(a409+a372); + a409=(a368*a409); + a412=(a412-a409); + if (res[0]!=0) res[0][31]=a412; + a412=arg[0]? arg[0][29] : 0; + a409=cos(a412); + a372=cos(a412); + a352=(a8*a352); + a321=(a5*a321); + a352=(a352+a321); + a321=(a5*a352); + a417=arg[0]? arg[0][65] : 0; + a402=(a8*a417); + a321=(a321+a402); + a402=(a5*a352); + a367=(a8*a417); + a402=(a402+a367); + a367=(a10*a402); + a367=(a321+a367); + a413=(a5*a352); + a408=(a8*a417); + a413=(a413+a408); + a408=(a10*a413); + a367=(a367+a408); + a408=(a5*a352); + a406=(a8*a417); + a408=(a408+a406); + a367=(a367+a408); + a367=(a4*a367); + a367=(a362+a367); + a408=sin(a367); + a406=(a372*a408); + a404=sin(a412); + a400=cos(a367); + a416=(a404*a400); + a406=(a406-a416); + a416=casadi_sq(a406); + a410=(a409*a400); + a405=sin(a412); + a396=(a405*a408); + a410=(a410+a396); + a396=casadi_sq(a410); + a416=(a416+a396); + a396=(a406/a416); + a398=arg[3]? arg[3][29] : 0; + a414=(a396*a398); + a399=(a409*a414); + a415=(a410/a416); + a407=(a415*a398); + a421=(a404*a407); + a399=(a399+a421); + a421=cos(a367); + a411=(a399*a421); + a420=sin(a367); + a397=cos(a367); + a401=(a372*a397); + a419=sin(a367); + a418=(a404*a419); + a401=(a401+a418); + a418=(a401/a416); + a396=(a396/a416); + a406=(a406+a406); + a401=(a406*a401); + a410=(a410+a410); + a422=(a405*a397); + a423=(a409*a419); + a422=(a422-a423); + a423=(a410*a422); + a401=(a401+a423); + a423=(a396*a401); + a418=(a418-a423); + a418=(a398*a418); + a423=(a409*a418); + a422=(a422/a416); + a415=(a415/a416); + a401=(a415*a401); + a422=(a422-a401); + a422=(a398*a422); + a401=(a404*a422); + a423=(a423+a401); + a423=(a420*a423); + a411=(a411+a423); + a423=cos(a367); + a422=(a372*a422); + a418=(a405*a418); + a422=(a422-a418); + a422=(a423*a422); + a418=(a372*a407); + a401=(a405*a414); + a418=(a418-a401); + a367=sin(a367); + a401=(a418*a367); + a422=(a422-a401); + a411=(a411+a422); + a422=arg[0]? arg[0][64] : 0; + a401=arg[3]? arg[3][28] : 0; + a401=(a4*a401); + a424=(a422*a401); + a425=sin(a362); + a424=(a424*a425); + a411=(a411+a424); + a424=arg[3]? arg[3][27] : 0; + a424=(a4*a424); + a425=(a422*a424); + a413=(a38*a413); + a413=(a362+a413); + a426=cos(a413); + a427=(a425*a426); + a428=(a422*a401); + a429=sin(a413); + a430=(a428*a429); + a427=(a427+a430); + a411=(a411+a427); + a427=(a10*a424); + a430=(a422*a427); + a402=(a44*a402); + a402=(a362+a402); + a431=cos(a402); + a432=(a430*a431); + a433=(a10*a401); + a434=(a422*a433); + a435=sin(a402); + a436=(a434*a435); + a432=(a432+a436); + a411=(a411+a432); + a432=(a10*a424); + a436=(a422*a432); + a321=(a44*a321); + a321=(a362+a321); + a437=cos(a321); + a438=(a436*a437); + a439=(a10*a401); + a440=(a422*a439); + a441=sin(a321); + a442=(a440*a441); + a438=(a438+a442); + a411=(a411+a438); + a422=(a422*a424); + a438=cos(a362); + a422=(a422*a438); + a411=(a411+a422); + a422=(a358*a364); + a438=sin(a362); + a422=(a422*a438); + a438=sin(a362); + a438=(a358*a438); + a442=cos(a362); + a442=(a350*a442); + a438=(a438+a442); + a442=(a438/a366); + a443=cos(a362); + a443=(a358*a443); + a444=sin(a362); + a444=(a350*a444); + a443=(a443-a444); + a444=(a360*a443); + a438=(a356*a438); + a444=(a444-a438); + a438=(a326*a444); + a442=(a442+a438); + a442=(a342*a442); + a438=(a358*a442); + a438=(a403*a438); + a422=(a422+a438); + a411=(a411+a422); + a422=(a350*a364); + a438=cos(a362); + a422=(a422*a438); + a442=(a350*a442); + a442=(a361*a442); + a422=(a422-a442); + a411=(a411+a422); + a443=(a443/a366); + a444=(a365*a444); + a443=(a443-a444); + a443=(a342*a443); + a444=(a350*a443); + a444=(a347*a444); + a422=(a350*a357); + a442=sin(a362); + a422=(a422*a442); + a444=(a444-a422); + a411=(a411-a444); + a444=(a358*a357); + a422=cos(a362); + a444=(a444*a422); + a443=(a358*a443); + a443=(a368*a443); + a444=(a444+a443); + a411=(a411-a444); + a444=arg[1]? arg[1][26] : 0; + a443=cos(a444); + a422=cos(a444); + a442=sin(a362); + a438=(a422*a442); + a445=sin(a444); + a446=cos(a362); + a447=(a445*a446); + a438=(a438-a447); + a447=casadi_sq(a438); + a446=(a443*a446); + a448=sin(a444); + a442=(a448*a442); + a446=(a446+a442); + a442=casadi_sq(a446); + a447=(a447+a442); + a442=(a438/a447); + a449=cos(a444); + a450=sin(a362); + a451=(a449*a450); + a452=sin(a444); + a453=cos(a362); + a454=(a452*a453); + a451=(a451-a454); + a454=cos(a444); + a453=(a454*a453); + a444=sin(a444); + a450=(a444*a450); + a453=(a453+a450); + a450=atan2(a451,a453); + a450=(a450*a34); + a455=(a442*a450); + a456=(a443*a455); + a457=(a446/a447); + a458=(a457*a450); + a459=(a445*a458); + a456=(a456+a459); + a459=cos(a362); + a456=(a456*a459); + a459=sin(a362); + a460=cos(a362); + a461=(a422*a460); + a462=sin(a362); + a463=(a445*a462); + a461=(a461+a463); + a463=(a461/a447); + a464=(a442/a447); + a465=(a438+a438); + a465=(a465*a461); + a466=(a446+a446); + a460=(a448*a460); + a462=(a443*a462); + a460=(a460-a462); + a466=(a466*a460); + a465=(a465+a466); + a464=(a464*a465); + a463=(a463-a464); + a463=(a450*a463); + a464=casadi_sq(a451); + a466=casadi_sq(a453); + a464=(a464+a466); + a466=(a453/a464); + a462=cos(a362); + a467=(a449*a462); + a468=sin(a362); + a469=(a452*a468); + a467=(a467+a469); + a466=(a466*a467); + a464=(a451/a464); + a462=(a444*a462); + a468=(a454*a468); + a462=(a462-a468); + a464=(a464*a462); + a466=(a466-a464); + a466=(a34*a466); + a442=(a442*a466); + a463=(a463+a442); + a443=(a443*a463); + a442=(a460/a447); + a447=(a457/a447); + a447=(a447*a465); + a442=(a442-a447); + a450=(a450*a442); + a457=(a457*a466); + a450=(a450+a457); + a445=(a445*a450); + a443=(a443+a445); + a459=(a459*a443); + a456=(a456+a459); + a411=(a411+a456); + a456=cos(a362); + a450=(a422*a450); + a463=(a448*a463); + a450=(a450-a463); + a456=(a456*a450); + a422=(a422*a458); + a448=(a448*a455); + a422=(a422-a448); + a448=sin(a362); + a422=(a422*a448); + a456=(a456-a422); + a411=(a411+a456); + a456=casadi_sq(a451); + a422=casadi_sq(a453); + a456=(a456+a422); + a422=(a451/a456); + a448=atan2(a438,a446); + a448=(a448*a34); + a455=(a422*a448); + a458=(a454*a455); + a450=(a453/a456); + a463=(a450*a448); + a459=(a452*a463); + a458=(a458+a459); + a459=cos(a362); + a458=(a458*a459); + a459=sin(a362); + a443=(a467/a456); + a445=(a422/a456); + a451=(a451+a451); + a451=(a451*a467); + a453=(a453+a453); + a453=(a453*a462); + a451=(a451+a453); + a445=(a445*a451); + a443=(a443-a445); + a443=(a448*a443); + a445=casadi_sq(a438); + a453=casadi_sq(a446); + a445=(a445+a453); + a446=(a446/a445); + a446=(a446*a461); + a438=(a438/a445); + a438=(a438*a460); + a446=(a446-a438); + a446=(a34*a446); + a422=(a422*a446); + a443=(a443+a422); + a454=(a454*a443); + a462=(a462/a456); + a456=(a450/a456); + a456=(a456*a451); + a462=(a462-a456); + a448=(a448*a462); + a450=(a450*a446); + a448=(a448+a450); + a452=(a452*a448); + a454=(a454+a452); + a459=(a459*a454); + a458=(a458+a459); + a411=(a411+a458); + a458=cos(a362); + a448=(a449*a448); + a443=(a444*a443); + a448=(a448-a443); + a458=(a458*a448); + a449=(a449*a463); + a444=(a444*a455); + a449=(a449-a444); + a444=sin(a362); + a449=(a449*a444); + a458=(a458-a449); + a411=(a411+a458); + if (res[0]!=0) res[0][32]=a411; + a411=(a28*a34); + a458=(a28*a34); + a411=(a411+a458); + if (res[0]!=0) res[0][33]=a411; + a411=(a28*a34); + a458=(a28*a34); + a411=(a411+a458); + if (res[0]!=0) res[0][34]=a411; + a411=cos(a412); + a458=(a407*a411); + a449=cos(a412); + a444=(a408*a449); + a455=sin(a412); + a463=(a400*a455); + a444=(a444-a463); + a463=(a444/a416); + a444=(a410*a444); + a448=sin(a412); + a443=(a408*a448); + a411=(a400*a411); + a443=(a443+a411); + a411=(a406*a443); + a444=(a444-a411); + a411=(a415*a444); + a463=(a463-a411); + a463=(a398*a463); + a411=(a404*a463); + a458=(a458+a411); + a455=(a414*a455); + a443=(a443/a416); + a444=(a396*a444); + a443=(a443+a444); + a443=(a398*a443); + a444=(a409*a443); + a455=(a455+a444); + a458=(a458-a455); + a458=(a420*a458); + a455=(a372*a463); + a448=(a407*a448); + a455=(a455-a448); + a449=(a414*a449); + a448=(a405*a443); + a449=(a449-a448); + a455=(a455-a449); + a455=(a423*a455); + a458=(a458+a455); + if (res[0]!=0) res[0][35]=a458; + a458=arg[0]? arg[0][32] : 0; + a455=cos(a458); + a449=cos(a458); + a417=(a8*a417); + a352=(a5*a352); + a417=(a417+a352); + a352=(a5*a417); + a448=arg[0]? arg[0][67] : 0; + a444=(a8*a448); + a352=(a352+a444); + a444=(a5*a417); + a411=(a8*a448); + a444=(a444+a411); + a411=(a10*a444); + a411=(a352+a411); + a459=(a5*a417); + a454=(a8*a448); + a459=(a459+a454); + a454=(a10*a459); + a411=(a411+a454); + a454=(a5*a417); + a452=(a8*a448); + a454=(a454+a452); + a411=(a411+a454); + a411=(a4*a411); + a411=(a412+a411); + a454=sin(a411); + a452=(a449*a454); + a450=sin(a458); + a446=cos(a411); + a462=(a450*a446); + a452=(a452-a462); + a462=casadi_sq(a452); + a456=(a455*a446); + a451=sin(a458); + a422=(a451*a454); + a456=(a456+a422); + a422=casadi_sq(a456); + a462=(a462+a422); + a422=(a452/a462); + a438=arg[3]? arg[3][32] : 0; + a460=(a422*a438); + a445=(a455*a460); + a461=(a456/a462); + a453=(a461*a438); + a467=(a450*a453); + a445=(a445+a467); + a467=cos(a411); + a457=(a445*a467); + a466=sin(a411); + a442=cos(a411); + a447=(a449*a442); + a465=sin(a411); + a464=(a450*a465); + a447=(a447+a464); + a464=(a447/a462); + a422=(a422/a462); + a452=(a452+a452); + a447=(a452*a447); + a456=(a456+a456); + a468=(a451*a442); + a469=(a455*a465); + a468=(a468-a469); + a469=(a456*a468); + a447=(a447+a469); + a469=(a422*a447); + a464=(a464-a469); + a464=(a438*a464); + a469=(a455*a464); + a468=(a468/a462); + a461=(a461/a462); + a447=(a461*a447); + a468=(a468-a447); + a468=(a438*a468); + a447=(a450*a468); + a469=(a469+a447); + a469=(a466*a469); + a457=(a457+a469); + a469=cos(a411); + a447=(a449*a468); + a470=(a451*a464); + a447=(a447-a470); + a447=(a469*a447); + a470=(a449*a453); + a471=(a451*a460); + a470=(a470-a471); + a411=sin(a411); + a471=(a470*a411); + a447=(a447-a471); + a457=(a457+a447); + a447=arg[0]? arg[0][66] : 0; + a471=arg[3]? arg[3][31] : 0; + a471=(a4*a471); + a472=(a447*a471); + a473=sin(a412); + a472=(a472*a473); + a457=(a457+a472); + a472=arg[3]? arg[3][30] : 0; + a472=(a4*a472); + a473=(a447*a472); + a459=(a38*a459); + a459=(a412+a459); + a474=cos(a459); + a475=(a473*a474); + a476=(a447*a471); + a477=sin(a459); + a478=(a476*a477); + a475=(a475+a478); + a457=(a457+a475); + a475=(a10*a472); + a478=(a447*a475); + a444=(a44*a444); + a444=(a412+a444); + a479=cos(a444); + a480=(a478*a479); + a481=(a10*a471); + a482=(a447*a481); + a483=sin(a444); + a484=(a482*a483); + a480=(a480+a484); + a457=(a457+a480); + a480=(a10*a472); + a484=(a447*a480); + a352=(a44*a352); + a352=(a412+a352); + a485=cos(a352); + a486=(a484*a485); + a487=(a10*a471); + a488=(a447*a487); + a489=sin(a352); + a490=(a488*a489); + a486=(a486+a490); + a457=(a457+a486); + a447=(a447*a472); + a486=cos(a412); + a447=(a447*a486); + a457=(a457+a447); + a447=(a408*a414); + a486=sin(a412); + a447=(a447*a486); + a486=cos(a412); + a490=(a408*a443); + a490=(a486*a490); + a447=(a447+a490); + a457=(a457+a447); + a447=(a400*a414); + a490=cos(a412); + a447=(a447*a490); + a490=sin(a412); + a443=(a400*a443); + a443=(a490*a443); + a447=(a447-a443); + a457=(a457+a447); + a447=cos(a412); + a443=(a400*a463); + a443=(a447*a443); + a491=(a400*a407); + a492=sin(a412); + a491=(a491*a492); + a443=(a443-a491); + a457=(a457-a443); + a443=(a408*a407); + a491=cos(a412); + a443=(a443*a491); + a491=sin(a412); + a463=(a408*a463); + a463=(a491*a463); + a443=(a443+a463); + a457=(a457-a443); + a443=arg[1]? arg[1][29] : 0; + a463=cos(a443); + a492=cos(a443); + a493=sin(a412); + a494=(a492*a493); + a495=sin(a443); + a496=cos(a412); + a497=(a495*a496); + a494=(a494-a497); + a497=casadi_sq(a494); + a496=(a463*a496); + a498=sin(a443); + a493=(a498*a493); + a496=(a496+a493); + a493=casadi_sq(a496); + a497=(a497+a493); + a493=(a494/a497); + a499=cos(a443); + a500=sin(a412); + a501=(a499*a500); + a502=sin(a443); + a503=cos(a412); + a504=(a502*a503); + a501=(a501-a504); + a504=cos(a443); + a503=(a504*a503); + a443=sin(a443); + a500=(a443*a500); + a503=(a503+a500); + a500=atan2(a501,a503); + a500=(a500*a34); + a505=(a493*a500); + a506=(a463*a505); + a507=(a496/a497); + a508=(a507*a500); + a509=(a495*a508); + a506=(a506+a509); + a509=cos(a412); + a506=(a506*a509); + a509=sin(a412); + a510=cos(a412); + a511=(a492*a510); + a512=sin(a412); + a513=(a495*a512); + a511=(a511+a513); + a513=(a511/a497); + a514=(a493/a497); + a515=(a494+a494); + a515=(a515*a511); + a516=(a496+a496); + a510=(a498*a510); + a512=(a463*a512); + a510=(a510-a512); + a516=(a516*a510); + a515=(a515+a516); + a514=(a514*a515); + a513=(a513-a514); + a513=(a500*a513); + a514=casadi_sq(a501); + a516=casadi_sq(a503); + a514=(a514+a516); + a516=(a503/a514); + a512=cos(a412); + a517=(a499*a512); + a518=sin(a412); + a519=(a502*a518); + a517=(a517+a519); + a516=(a516*a517); + a514=(a501/a514); + a512=(a443*a512); + a518=(a504*a518); + a512=(a512-a518); + a514=(a514*a512); + a516=(a516-a514); + a516=(a34*a516); + a493=(a493*a516); + a513=(a513+a493); + a463=(a463*a513); + a493=(a510/a497); + a497=(a507/a497); + a497=(a497*a515); + a493=(a493-a497); + a500=(a500*a493); + a507=(a507*a516); + a500=(a500+a507); + a495=(a495*a500); + a463=(a463+a495); + a509=(a509*a463); + a506=(a506+a509); + a457=(a457+a506); + a506=cos(a412); + a500=(a492*a500); + a513=(a498*a513); + a500=(a500-a513); + a506=(a506*a500); + a492=(a492*a508); + a498=(a498*a505); + a492=(a492-a498); + a498=sin(a412); + a492=(a492*a498); + a506=(a506-a492); + a457=(a457+a506); + a506=casadi_sq(a501); + a492=casadi_sq(a503); + a506=(a506+a492); + a492=(a501/a506); + a498=atan2(a494,a496); + a498=(a498*a34); + a505=(a492*a498); + a508=(a504*a505); + a500=(a503/a506); + a513=(a500*a498); + a509=(a502*a513); + a508=(a508+a509); + a509=cos(a412); + a508=(a508*a509); + a509=sin(a412); + a463=(a517/a506); + a495=(a492/a506); + a501=(a501+a501); + a501=(a501*a517); + a503=(a503+a503); + a503=(a503*a512); + a501=(a501+a503); + a495=(a495*a501); + a463=(a463-a495); + a463=(a498*a463); + a495=casadi_sq(a494); + a503=casadi_sq(a496); + a495=(a495+a503); + a496=(a496/a495); + a496=(a496*a511); + a494=(a494/a495); + a494=(a494*a510); + a496=(a496-a494); + a496=(a34*a496); + a492=(a492*a496); + a463=(a463+a492); + a504=(a504*a463); + a512=(a512/a506); + a506=(a500/a506); + a506=(a506*a501); + a512=(a512-a506); + a498=(a498*a512); + a500=(a500*a496); + a498=(a498+a500); + a502=(a502*a498); + a504=(a504+a502); + a509=(a509*a504); + a508=(a508+a509); + a457=(a457+a508); + a508=cos(a412); + a498=(a499*a498); + a463=(a443*a463); + a498=(a498-a463); + a508=(a508*a498); + a499=(a499*a513); + a443=(a443*a505); + a499=(a499-a443); + a443=sin(a412); + a499=(a499*a443); + a508=(a508-a499); + a457=(a457+a508); + if (res[0]!=0) res[0][36]=a457; + a457=(a28*a34); + a508=(a28*a34); + a457=(a457+a508); + if (res[0]!=0) res[0][37]=a457; + a457=(a28*a34); + a508=(a28*a34); + a457=(a457+a508); + if (res[0]!=0) res[0][38]=a457; + a457=sin(a458); + a508=(a446*a464); + a499=(a460*a465); + a508=(a508-a499); + a508=(a457*a508); + a499=cos(a458); + a443=(a460*a442); + a464=(a454*a464); + a443=(a443+a464); + a443=(a499*a443); + a508=(a508-a443); + a443=cos(a458); + a464=(a446*a468); + a505=(a453*a465); + a464=(a464-a505); + a464=(a443*a464); + a508=(a508-a464); + a464=sin(a458); + a505=(a453*a442); + a468=(a454*a468); + a505=(a505+a468); + a505=(a464*a505); + a508=(a508-a505); + if (res[0]!=0) res[0][39]=a508; + a508=arg[0]? arg[0][35] : 0; + a505=cos(a508); + a468=cos(a508); + a448=(a8*a448); + a417=(a5*a417); + a448=(a448+a417); + a417=(a5*a448); + a513=arg[0]? arg[0][69] : 0; + a498=(a8*a513); + a417=(a417+a498); + a498=(a5*a448); + a463=(a8*a513); + a498=(a498+a463); + a463=(a10*a498); + a463=(a417+a463); + a509=(a5*a448); + a504=(a8*a513); + a509=(a509+a504); + a504=(a10*a509); + a463=(a463+a504); + a504=(a5*a448); + a502=(a8*a513); + a504=(a504+a502); + a463=(a463+a504); + a463=(a4*a463); + a463=(a458+a463); + a504=sin(a463); + a502=(a468*a504); + a500=sin(a508); + a496=cos(a463); + a512=(a500*a496); + a502=(a502-a512); + a512=casadi_sq(a502); + a506=(a505*a496); + a501=sin(a508); + a492=(a501*a504); + a506=(a506+a492); + a492=casadi_sq(a506); + a512=(a512+a492); + a492=(a502/a512); + a494=arg[3]? arg[3][35] : 0; + a510=(a492*a494); + a495=(a505*a510); + a511=(a506/a512); + a503=(a511*a494); + a517=(a500*a503); + a495=(a495+a517); + a517=cos(a463); + a507=(a495*a517); + a516=sin(a463); + a493=cos(a463); + a497=(a468*a493); + a515=sin(a463); + a514=(a500*a515); + a497=(a497+a514); + a514=(a497/a512); + a492=(a492/a512); + a502=(a502+a502); + a497=(a502*a497); + a506=(a506+a506); + a518=(a501*a493); + a519=(a505*a515); + a518=(a518-a519); + a519=(a506*a518); + a497=(a497+a519); + a519=(a492*a497); + a514=(a514-a519); + a514=(a494*a514); + a519=(a505*a514); + a518=(a518/a512); + a511=(a511/a512); + a497=(a511*a497); + a518=(a518-a497); + a518=(a494*a518); + a497=(a500*a518); + a519=(a519+a497); + a519=(a516*a519); + a507=(a507+a519); + a519=cos(a463); + a518=(a468*a518); + a514=(a501*a514); + a518=(a518-a514); + a518=(a519*a518); + a514=(a468*a503); + a497=(a501*a510); + a514=(a514-a497); + a463=sin(a463); + a497=(a514*a463); + a518=(a518-a497); + a507=(a507+a518); + a518=arg[0]? arg[0][68] : 0; + a497=arg[3]? arg[3][34] : 0; + a497=(a4*a497); + a520=(a518*a497); + a521=sin(a458); + a520=(a520*a521); + a507=(a507+a520); + a520=arg[3]? arg[3][33] : 0; + a520=(a4*a520); + a521=(a518*a520); + a509=(a38*a509); + a509=(a458+a509); + a522=cos(a509); + a523=(a521*a522); + a524=(a518*a497); + a525=sin(a509); + a526=(a524*a525); + a523=(a523+a526); + a507=(a507+a523); + a523=(a10*a520); + a526=(a518*a523); + a498=(a44*a498); + a498=(a458+a498); + a527=cos(a498); + a528=(a526*a527); + a529=(a10*a497); + a530=(a518*a529); + a531=sin(a498); + a532=(a530*a531); + a528=(a528+a532); + a507=(a507+a528); + a528=(a10*a520); + a532=(a518*a528); + a417=(a44*a417); + a417=(a458+a417); + a533=cos(a417); + a534=(a532*a533); + a535=(a10*a497); + a536=(a518*a535); + a537=sin(a417); + a538=(a536*a537); + a534=(a534+a538); + a507=(a507+a534); + a518=(a518*a520); + a534=cos(a458); + a518=(a518*a534); + a507=(a507+a518); + a518=(a454*a460); + a534=sin(a458); + a518=(a518*a534); + a534=sin(a458); + a534=(a454*a534); + a538=cos(a458); + a538=(a446*a538); + a534=(a534+a538); + a538=(a534/a462); + a539=cos(a458); + a539=(a454*a539); + a540=sin(a458); + a540=(a446*a540); + a539=(a539-a540); + a540=(a456*a539); + a534=(a452*a534); + a540=(a540-a534); + a534=(a422*a540); + a538=(a538+a534); + a538=(a438*a538); + a534=(a454*a538); + a534=(a499*a534); + a518=(a518+a534); + a507=(a507+a518); + a518=(a446*a460); + a534=cos(a458); + a518=(a518*a534); + a538=(a446*a538); + a538=(a457*a538); + a518=(a518-a538); + a507=(a507+a518); + a539=(a539/a462); + a540=(a461*a540); + a539=(a539-a540); + a539=(a438*a539); + a540=(a446*a539); + a540=(a443*a540); + a518=(a446*a453); + a538=sin(a458); + a518=(a518*a538); + a540=(a540-a518); + a507=(a507-a540); + a540=(a454*a453); + a518=cos(a458); + a540=(a540*a518); + a539=(a454*a539); + a539=(a464*a539); + a540=(a540+a539); + a507=(a507-a540); + a540=arg[1]? arg[1][32] : 0; + a539=cos(a540); + a518=cos(a540); + a538=sin(a458); + a534=(a518*a538); + a541=sin(a540); + a542=cos(a458); + a543=(a541*a542); + a534=(a534-a543); + a543=casadi_sq(a534); + a542=(a539*a542); + a544=sin(a540); + a538=(a544*a538); + a542=(a542+a538); + a538=casadi_sq(a542); + a543=(a543+a538); + a538=(a534/a543); + a545=cos(a540); + a546=sin(a458); + a547=(a545*a546); + a548=sin(a540); + a549=cos(a458); + a550=(a548*a549); + a547=(a547-a550); + a550=cos(a540); + a549=(a550*a549); + a540=sin(a540); + a546=(a540*a546); + a549=(a549+a546); + a546=atan2(a547,a549); + a546=(a546*a34); + a551=(a538*a546); + a552=(a539*a551); + a553=(a542/a543); + a554=(a553*a546); + a555=(a541*a554); + a552=(a552+a555); + a555=cos(a458); + a552=(a552*a555); + a555=sin(a458); + a556=cos(a458); + a557=(a518*a556); + a558=sin(a458); + a559=(a541*a558); + a557=(a557+a559); + a559=(a557/a543); + a560=(a538/a543); + a561=(a534+a534); + a561=(a561*a557); + a562=(a542+a542); + a556=(a544*a556); + a558=(a539*a558); + a556=(a556-a558); + a562=(a562*a556); + a561=(a561+a562); + a560=(a560*a561); + a559=(a559-a560); + a559=(a546*a559); + a560=casadi_sq(a547); + a562=casadi_sq(a549); + a560=(a560+a562); + a562=(a549/a560); + a558=cos(a458); + a563=(a545*a558); + a564=sin(a458); + a565=(a548*a564); + a563=(a563+a565); + a562=(a562*a563); + a560=(a547/a560); + a558=(a540*a558); + a564=(a550*a564); + a558=(a558-a564); + a560=(a560*a558); + a562=(a562-a560); + a562=(a34*a562); + a538=(a538*a562); + a559=(a559+a538); + a539=(a539*a559); + a538=(a556/a543); + a543=(a553/a543); + a543=(a543*a561); + a538=(a538-a543); + a546=(a546*a538); + a553=(a553*a562); + a546=(a546+a553); + a541=(a541*a546); + a539=(a539+a541); + a555=(a555*a539); + a552=(a552+a555); + a507=(a507+a552); + a552=cos(a458); + a546=(a518*a546); + a559=(a544*a559); + a546=(a546-a559); + a552=(a552*a546); + a518=(a518*a554); + a544=(a544*a551); + a518=(a518-a544); + a544=sin(a458); + a518=(a518*a544); + a552=(a552-a518); + a507=(a507+a552); + a552=casadi_sq(a547); + a518=casadi_sq(a549); + a552=(a552+a518); + a518=(a547/a552); + a544=atan2(a534,a542); + a544=(a544*a34); + a551=(a518*a544); + a554=(a550*a551); + a546=(a549/a552); + a559=(a546*a544); + a555=(a548*a559); + a554=(a554+a555); + a555=cos(a458); + a554=(a554*a555); + a555=sin(a458); + a539=(a563/a552); + a541=(a518/a552); + a547=(a547+a547); + a547=(a547*a563); + a549=(a549+a549); + a549=(a549*a558); + a547=(a547+a549); + a541=(a541*a547); + a539=(a539-a541); + a539=(a544*a539); + a541=casadi_sq(a534); + a549=casadi_sq(a542); + a541=(a541+a549); + a542=(a542/a541); + a542=(a542*a557); + a534=(a534/a541); + a534=(a534*a556); + a542=(a542-a534); + a542=(a34*a542); + a518=(a518*a542); + a539=(a539+a518); + a550=(a550*a539); + a558=(a558/a552); + a552=(a546/a552); + a552=(a552*a547); + a558=(a558-a552); + a544=(a544*a558); + a546=(a546*a542); + a544=(a544+a546); + a548=(a548*a544); + a550=(a550+a548); + a555=(a555*a550); + a554=(a554+a555); + a507=(a507+a554); + a554=cos(a458); + a544=(a545*a544); + a539=(a540*a539); + a544=(a544-a539); + a554=(a554*a544); + a545=(a545*a559); + a540=(a540*a551); + a545=(a545-a540); + a540=sin(a458); + a545=(a545*a540); + a554=(a554-a545); + a507=(a507+a554); + if (res[0]!=0) res[0][40]=a507; + a507=(a28*a34); + a554=(a28*a34); + a507=(a507+a554); + if (res[0]!=0) res[0][41]=a507; + a507=(a28*a34); + a554=(a28*a34); + a507=(a507+a554); + if (res[0]!=0) res[0][42]=a507; + a507=cos(a508); + a554=(a503*a507); + a545=cos(a508); + a540=(a504*a545); + a551=sin(a508); + a559=(a496*a551); + a540=(a540-a559); + a559=(a540/a512); + a540=(a506*a540); + a544=sin(a508); + a539=(a504*a544); + a507=(a496*a507); + a539=(a539+a507); + a507=(a502*a539); + a540=(a540-a507); + a507=(a511*a540); + a559=(a559-a507); + a559=(a494*a559); + a507=(a500*a559); + a554=(a554+a507); + a551=(a510*a551); + a539=(a539/a512); + a540=(a492*a540); + a539=(a539+a540); + a539=(a494*a539); + a540=(a505*a539); + a551=(a551+a540); + a554=(a554-a551); + a554=(a516*a554); + a551=(a468*a559); + a544=(a503*a544); + a551=(a551-a544); + a545=(a510*a545); + a544=(a501*a539); + a545=(a545-a544); + a551=(a551-a545); + a551=(a519*a551); + a554=(a554+a551); + if (res[0]!=0) res[0][43]=a554; + a554=arg[0]? arg[0][38] : 0; + a551=cos(a554); + a545=cos(a554); + a513=(a8*a513); + a448=(a5*a448); + a513=(a513+a448); + a448=(a5*a513); + a544=arg[0]? arg[0][71] : 0; + a540=(a8*a544); + a448=(a448+a540); + a540=(a5*a513); + a507=(a8*a544); + a540=(a540+a507); + a507=(a10*a540); + a507=(a448+a507); + a555=(a5*a513); + a550=(a8*a544); + a555=(a555+a550); + a550=(a10*a555); + a507=(a507+a550); + a550=(a5*a513); + a548=(a8*a544); + a550=(a550+a548); + a507=(a507+a550); + a507=(a4*a507); + a507=(a508+a507); + a550=sin(a507); + a548=(a545*a550); + a546=sin(a554); + a542=cos(a507); + a558=(a546*a542); + a548=(a548-a558); + a558=casadi_sq(a548); + a552=(a551*a542); + a547=sin(a554); + a518=(a547*a550); + a552=(a552+a518); + a518=casadi_sq(a552); + a558=(a558+a518); + a518=(a548/a558); + a534=arg[3]? arg[3][38] : 0; + a556=(a518*a534); + a541=(a551*a556); + a557=(a552/a558); + a549=(a557*a534); + a563=(a546*a549); + a541=(a541+a563); + a563=cos(a507); + a553=(a541*a563); + a562=sin(a507); + a538=cos(a507); + a543=(a545*a538); + a561=sin(a507); + a560=(a546*a561); + a543=(a543+a560); + a560=(a543/a558); + a518=(a518/a558); + a548=(a548+a548); + a543=(a548*a543); + a552=(a552+a552); + a564=(a547*a538); + a565=(a551*a561); + a564=(a564-a565); + a565=(a552*a564); + a543=(a543+a565); + a565=(a518*a543); + a560=(a560-a565); + a560=(a534*a560); + a565=(a551*a560); + a564=(a564/a558); + a557=(a557/a558); + a543=(a557*a543); + a564=(a564-a543); + a564=(a534*a564); + a543=(a546*a564); + a565=(a565+a543); + a565=(a562*a565); + a553=(a553+a565); + a565=cos(a507); + a564=(a545*a564); + a560=(a547*a560); + a564=(a564-a560); + a564=(a565*a564); + a560=(a545*a549); + a543=(a547*a556); + a560=(a560-a543); + a507=sin(a507); + a543=(a560*a507); + a564=(a564-a543); + a553=(a553+a564); + a564=arg[0]? arg[0][70] : 0; + a543=arg[3]? arg[3][37] : 0; + a543=(a4*a543); + a566=(a564*a543); + a567=sin(a508); + a566=(a566*a567); + a553=(a553+a566); + a566=arg[3]? arg[3][36] : 0; + a566=(a4*a566); + a567=(a564*a566); + a555=(a38*a555); + a555=(a508+a555); + a568=cos(a555); + a569=(a567*a568); + a570=(a564*a543); + a571=sin(a555); + a572=(a570*a571); + a569=(a569+a572); + a553=(a553+a569); + a569=(a10*a566); + a572=(a564*a569); + a540=(a44*a540); + a540=(a508+a540); + a573=cos(a540); + a574=(a572*a573); + a575=(a10*a543); + a576=(a564*a575); + a577=sin(a540); + a578=(a576*a577); + a574=(a574+a578); + a553=(a553+a574); + a574=(a10*a566); + a578=(a564*a574); + a448=(a44*a448); + a448=(a508+a448); + a579=cos(a448); + a580=(a578*a579); + a581=(a10*a543); + a582=(a564*a581); + a583=sin(a448); + a584=(a582*a583); + a580=(a580+a584); + a553=(a553+a580); + a564=(a564*a566); + a580=cos(a508); + a564=(a564*a580); + a553=(a553+a564); + a564=(a504*a510); + a580=sin(a508); + a564=(a564*a580); + a580=cos(a508); + a584=(a504*a539); + a584=(a580*a584); + a564=(a564+a584); + a553=(a553+a564); + a564=(a496*a510); + a584=cos(a508); + a564=(a564*a584); + a584=sin(a508); + a539=(a496*a539); + a539=(a584*a539); + a564=(a564-a539); + a553=(a553+a564); + a564=cos(a508); + a539=(a496*a559); + a539=(a564*a539); + a585=(a496*a503); + a586=sin(a508); + a585=(a585*a586); + a539=(a539-a585); + a553=(a553-a539); + a539=(a504*a503); + a585=cos(a508); + a539=(a539*a585); + a585=sin(a508); + a559=(a504*a559); + a559=(a585*a559); + a539=(a539+a559); + a553=(a553-a539); + a539=arg[1]? arg[1][35] : 0; + a559=cos(a539); + a586=cos(a539); + a587=sin(a508); + a588=(a586*a587); + a589=sin(a539); + a590=cos(a508); + a591=(a589*a590); + a588=(a588-a591); + a591=casadi_sq(a588); + a590=(a559*a590); + a592=sin(a539); + a587=(a592*a587); + a590=(a590+a587); + a587=casadi_sq(a590); + a591=(a591+a587); + a587=(a588/a591); + a593=cos(a539); + a594=sin(a508); + a595=(a593*a594); + a596=sin(a539); + a597=cos(a508); + a598=(a596*a597); + a595=(a595-a598); + a598=cos(a539); + a597=(a598*a597); + a539=sin(a539); + a594=(a539*a594); + a597=(a597+a594); + a594=atan2(a595,a597); + a594=(a594*a34); + a599=(a587*a594); + a600=(a559*a599); + a601=(a590/a591); + a602=(a601*a594); + a603=(a589*a602); + a600=(a600+a603); + a603=cos(a508); + a600=(a600*a603); + a603=sin(a508); + a604=cos(a508); + a605=(a586*a604); + a606=sin(a508); + a607=(a589*a606); + a605=(a605+a607); + a607=(a605/a591); + a608=(a587/a591); + a609=(a588+a588); + a609=(a609*a605); + a610=(a590+a590); + a604=(a592*a604); + a606=(a559*a606); + a604=(a604-a606); + a610=(a610*a604); + a609=(a609+a610); + a608=(a608*a609); + a607=(a607-a608); + a607=(a594*a607); + a608=casadi_sq(a595); + a610=casadi_sq(a597); + a608=(a608+a610); + a610=(a597/a608); + a606=cos(a508); + a611=(a593*a606); + a612=sin(a508); + a613=(a596*a612); + a611=(a611+a613); + a610=(a610*a611); + a608=(a595/a608); + a606=(a539*a606); + a612=(a598*a612); + a606=(a606-a612); + a608=(a608*a606); + a610=(a610-a608); + a610=(a34*a610); + a587=(a587*a610); + a607=(a607+a587); + a559=(a559*a607); + a587=(a604/a591); + a591=(a601/a591); + a591=(a591*a609); + a587=(a587-a591); + a594=(a594*a587); + a601=(a601*a610); + a594=(a594+a601); + a589=(a589*a594); + a559=(a559+a589); + a603=(a603*a559); + a600=(a600+a603); + a553=(a553+a600); + a600=cos(a508); + a594=(a586*a594); + a607=(a592*a607); + a594=(a594-a607); + a600=(a600*a594); + a586=(a586*a602); + a592=(a592*a599); + a586=(a586-a592); + a592=sin(a508); + a586=(a586*a592); + a600=(a600-a586); + a553=(a553+a600); + a600=casadi_sq(a595); + a586=casadi_sq(a597); + a600=(a600+a586); + a586=(a595/a600); + a592=atan2(a588,a590); + a592=(a592*a34); + a599=(a586*a592); + a602=(a598*a599); + a594=(a597/a600); + a607=(a594*a592); + a603=(a596*a607); + a602=(a602+a603); + a603=cos(a508); + a602=(a602*a603); + a603=sin(a508); + a559=(a611/a600); + a589=(a586/a600); + a595=(a595+a595); + a595=(a595*a611); + a597=(a597+a597); + a597=(a597*a606); + a595=(a595+a597); + a589=(a589*a595); + a559=(a559-a589); + a559=(a592*a559); + a589=casadi_sq(a588); + a597=casadi_sq(a590); + a589=(a589+a597); + a590=(a590/a589); + a590=(a590*a605); + a588=(a588/a589); + a588=(a588*a604); + a590=(a590-a588); + a590=(a34*a590); + a586=(a586*a590); + a559=(a559+a586); + a598=(a598*a559); + a606=(a606/a600); + a600=(a594/a600); + a600=(a600*a595); + a606=(a606-a600); + a592=(a592*a606); + a594=(a594*a590); + a592=(a592+a594); + a596=(a596*a592); + a598=(a598+a596); + a603=(a603*a598); + a602=(a602+a603); + a553=(a553+a602); + a602=cos(a508); + a592=(a593*a592); + a559=(a539*a559); + a592=(a592-a559); + a602=(a602*a592); + a593=(a593*a607); + a539=(a539*a599); + a593=(a593-a539); + a539=sin(a508); + a593=(a593*a539); + a602=(a602-a593); + a553=(a553+a602); + if (res[0]!=0) res[0][44]=a553; + a553=(a28*a34); + a602=(a28*a34); + a553=(a553+a602); + if (res[0]!=0) res[0][45]=a553; + a553=(a28*a34); + a602=(a28*a34); + a553=(a553+a602); + if (res[0]!=0) res[0][46]=a553; + a553=cos(a554); + a602=(a549*a553); + a593=cos(a554); + a539=(a550*a593); + a599=sin(a554); + a607=(a542*a599); + a539=(a539-a607); + a607=(a539/a558); + a539=(a552*a539); + a592=sin(a554); + a559=(a550*a592); + a553=(a542*a553); + a559=(a559+a553); + a553=(a548*a559); + a539=(a539-a553); + a553=(a557*a539); + a607=(a607-a553); + a607=(a534*a607); + a553=(a546*a607); + a602=(a602+a553); + a599=(a556*a599); + a559=(a559/a558); + a539=(a518*a539); + a559=(a559+a539); + a559=(a534*a559); + a539=(a551*a559); + a599=(a599+a539); + a602=(a602-a599); + a602=(a562*a602); + a599=(a545*a607); + a592=(a549*a592); + a599=(a599-a592); + a593=(a556*a593); + a592=(a547*a559); + a593=(a593-a592); + a599=(a599-a593); + a599=(a565*a599); + a602=(a602+a599); + if (res[0]!=0) res[0][47]=a602; + a602=arg[0]? arg[0][41] : 0; + a599=cos(a602); + a593=cos(a602); + a544=(a8*a544); + a513=(a5*a513); + a544=(a544+a513); + a513=(a5*a544); + a592=arg[0]? arg[0][73] : 0; + a539=(a8*a592); + a513=(a513+a539); + a539=(a5*a544); + a553=(a8*a592); + a539=(a539+a553); + a553=(a10*a539); + a553=(a513+a553); + a603=(a5*a544); + a598=(a8*a592); + a603=(a603+a598); + a598=(a10*a603); + a553=(a553+a598); + a598=(a5*a544); + a596=(a8*a592); + a598=(a598+a596); + a553=(a553+a598); + a553=(a4*a553); + a553=(a554+a553); + a598=sin(a553); + a596=(a593*a598); + a594=sin(a602); + a590=cos(a553); + a606=(a594*a590); + a596=(a596-a606); + a606=casadi_sq(a596); + a600=(a599*a590); + a595=sin(a602); + a586=(a595*a598); + a600=(a600+a586); + a586=casadi_sq(a600); + a606=(a606+a586); + a586=(a596/a606); + a588=arg[3]? arg[3][41] : 0; + a604=(a586*a588); + a589=(a599*a604); + a605=(a600/a606); + a597=(a605*a588); + a611=(a594*a597); + a589=(a589+a611); + a611=cos(a553); + a601=(a589*a611); + a610=sin(a553); + a587=cos(a553); + a591=(a593*a587); + a609=sin(a553); + a608=(a594*a609); + a591=(a591+a608); + a608=(a591/a606); + a586=(a586/a606); + a596=(a596+a596); + a591=(a596*a591); + a600=(a600+a600); + a612=(a595*a587); + a613=(a599*a609); + a612=(a612-a613); + a613=(a600*a612); + a591=(a591+a613); + a613=(a586*a591); + a608=(a608-a613); + a608=(a588*a608); + a613=(a599*a608); + a612=(a612/a606); + a605=(a605/a606); + a591=(a605*a591); + a612=(a612-a591); + a612=(a588*a612); + a591=(a594*a612); + a613=(a613+a591); + a613=(a610*a613); + a601=(a601+a613); + a613=cos(a553); + a591=(a593*a612); + a614=(a595*a608); + a591=(a591-a614); + a591=(a613*a591); + a614=(a593*a597); + a615=(a595*a604); + a614=(a614-a615); + a553=sin(a553); + a615=(a614*a553); + a591=(a591-a615); + a601=(a601+a591); + a591=arg[0]? arg[0][72] : 0; + a615=arg[3]? arg[3][40] : 0; + a615=(a4*a615); + a616=(a591*a615); + a617=sin(a554); + a616=(a616*a617); + a601=(a601+a616); + a616=arg[3]? arg[3][39] : 0; + a616=(a4*a616); + a617=(a591*a616); + a603=(a38*a603); + a603=(a554+a603); + a618=cos(a603); + a619=(a617*a618); + a620=(a591*a615); + a621=sin(a603); + a622=(a620*a621); + a619=(a619+a622); + a601=(a601+a619); + a619=(a10*a616); + a622=(a591*a619); + a539=(a44*a539); + a539=(a554+a539); + a623=cos(a539); + a624=(a622*a623); + a625=(a10*a615); + a626=(a591*a625); + a627=sin(a539); + a628=(a626*a627); + a624=(a624+a628); + a601=(a601+a624); + a624=(a10*a616); + a628=(a591*a624); + a513=(a44*a513); + a513=(a554+a513); + a629=cos(a513); + a630=(a628*a629); + a631=(a10*a615); + a632=(a591*a631); + a633=sin(a513); + a634=(a632*a633); + a630=(a630+a634); + a601=(a601+a630); + a591=(a591*a616); + a630=cos(a554); + a591=(a591*a630); + a601=(a601+a591); + a591=(a550*a556); + a630=sin(a554); + a591=(a591*a630); + a630=cos(a554); + a634=(a550*a559); + a634=(a630*a634); + a591=(a591+a634); + a601=(a601+a591); + a591=(a542*a556); + a634=cos(a554); + a591=(a591*a634); + a634=sin(a554); + a559=(a542*a559); + a559=(a634*a559); + a591=(a591-a559); + a601=(a601+a591); + a591=cos(a554); + a559=(a542*a607); + a559=(a591*a559); + a635=(a542*a549); + a636=sin(a554); + a635=(a635*a636); + a559=(a559-a635); + a601=(a601-a559); + a559=(a550*a549); + a635=cos(a554); + a559=(a559*a635); + a635=sin(a554); + a607=(a550*a607); + a607=(a635*a607); + a559=(a559+a607); + a601=(a601-a559); + a559=arg[1]? arg[1][38] : 0; + a607=cos(a559); + a636=cos(a559); + a637=sin(a554); + a638=(a636*a637); + a639=sin(a559); + a640=cos(a554); + a641=(a639*a640); + a638=(a638-a641); + a641=casadi_sq(a638); + a640=(a607*a640); + a642=sin(a559); + a637=(a642*a637); + a640=(a640+a637); + a637=casadi_sq(a640); + a641=(a641+a637); + a637=(a638/a641); + a643=cos(a559); + a644=sin(a554); + a645=(a643*a644); + a646=sin(a559); + a647=cos(a554); + a648=(a646*a647); + a645=(a645-a648); + a648=cos(a559); + a647=(a648*a647); + a559=sin(a559); + a644=(a559*a644); + a647=(a647+a644); + a644=atan2(a645,a647); + a644=(a644*a34); + a649=(a637*a644); + a650=(a607*a649); + a651=(a640/a641); + a652=(a651*a644); + a653=(a639*a652); + a650=(a650+a653); + a653=cos(a554); + a650=(a650*a653); + a653=sin(a554); + a654=cos(a554); + a655=(a636*a654); + a656=sin(a554); + a657=(a639*a656); + a655=(a655+a657); + a657=(a655/a641); + a658=(a637/a641); + a659=(a638+a638); + a659=(a659*a655); + a660=(a640+a640); + a654=(a642*a654); + a656=(a607*a656); + a654=(a654-a656); + a660=(a660*a654); + a659=(a659+a660); + a658=(a658*a659); + a657=(a657-a658); + a657=(a644*a657); + a658=casadi_sq(a645); + a660=casadi_sq(a647); + a658=(a658+a660); + a660=(a647/a658); + a656=cos(a554); + a661=(a643*a656); + a662=sin(a554); + a663=(a646*a662); + a661=(a661+a663); + a660=(a660*a661); + a658=(a645/a658); + a656=(a559*a656); + a662=(a648*a662); + a656=(a656-a662); + a658=(a658*a656); + a660=(a660-a658); + a660=(a34*a660); + a637=(a637*a660); + a657=(a657+a637); + a607=(a607*a657); + a637=(a654/a641); + a641=(a651/a641); + a641=(a641*a659); + a637=(a637-a641); + a644=(a644*a637); + a651=(a651*a660); + a644=(a644+a651); + a639=(a639*a644); + a607=(a607+a639); + a653=(a653*a607); + a650=(a650+a653); + a601=(a601+a650); + a650=cos(a554); + a644=(a636*a644); + a657=(a642*a657); + a644=(a644-a657); + a650=(a650*a644); + a636=(a636*a652); + a642=(a642*a649); + a636=(a636-a642); + a642=sin(a554); + a636=(a636*a642); + a650=(a650-a636); + a601=(a601+a650); + a650=casadi_sq(a645); + a636=casadi_sq(a647); + a650=(a650+a636); + a636=(a645/a650); + a642=atan2(a638,a640); + a642=(a642*a34); + a649=(a636*a642); + a652=(a648*a649); + a644=(a647/a650); + a657=(a644*a642); + a653=(a646*a657); + a652=(a652+a653); + a653=cos(a554); + a652=(a652*a653); + a653=sin(a554); + a607=(a661/a650); + a639=(a636/a650); + a645=(a645+a645); + a645=(a645*a661); + a647=(a647+a647); + a647=(a647*a656); + a645=(a645+a647); + a639=(a639*a645); + a607=(a607-a639); + a607=(a642*a607); + a639=casadi_sq(a638); + a647=casadi_sq(a640); + a639=(a639+a647); + a640=(a640/a639); + a640=(a640*a655); + a638=(a638/a639); + a638=(a638*a654); + a640=(a640-a638); + a640=(a34*a640); + a636=(a636*a640); + a607=(a607+a636); + a648=(a648*a607); + a656=(a656/a650); + a650=(a644/a650); + a650=(a650*a645); + a656=(a656-a650); + a642=(a642*a656); + a644=(a644*a640); + a642=(a642+a644); + a646=(a646*a642); + a648=(a648+a646); + a653=(a653*a648); + a652=(a652+a653); + a601=(a601+a652); + a652=cos(a554); + a642=(a643*a642); + a607=(a559*a607); + a642=(a642-a607); + a652=(a652*a642); + a643=(a643*a657); + a559=(a559*a649); + a643=(a643-a559); + a559=sin(a554); + a643=(a643*a559); + a652=(a652-a643); + a601=(a601+a652); + if (res[0]!=0) res[0][48]=a601; + a601=(a28*a34); + a652=(a28*a34); + a601=(a601+a652); + if (res[0]!=0) res[0][49]=a601; + a601=(a28*a34); + a652=(a28*a34); + a601=(a601+a652); + if (res[0]!=0) res[0][50]=a601; + a601=sin(a602); + a652=(a590*a608); + a643=(a604*a609); + a652=(a652-a643); + a652=(a601*a652); + a643=cos(a602); + a559=(a604*a587); + a608=(a598*a608); + a559=(a559+a608); + a559=(a643*a559); + a652=(a652-a559); + a559=cos(a602); + a608=(a590*a612); + a649=(a597*a609); + a608=(a608-a649); + a608=(a559*a608); + a652=(a652-a608); + a608=sin(a602); + a649=(a597*a587); + a612=(a598*a612); + a649=(a649+a612); + a649=(a608*a649); + a652=(a652-a649); + if (res[0]!=0) res[0][51]=a652; + a652=arg[0]? arg[0][44] : 0; + a649=cos(a652); + a612=cos(a652); + a592=(a8*a592); + a544=(a5*a544); + a592=(a592+a544); + a544=(a5*a592); + a657=arg[0]? arg[0][75] : 0; + a642=(a8*a657); + a544=(a544+a642); + a642=(a5*a592); + a607=(a8*a657); + a642=(a642+a607); + a607=(a10*a642); + a607=(a544+a607); + a653=(a5*a592); + a648=(a8*a657); + a653=(a653+a648); + a648=(a10*a653); + a607=(a607+a648); + a648=(a5*a592); + a646=(a8*a657); + a648=(a648+a646); + a607=(a607+a648); + a607=(a4*a607); + a607=(a602+a607); + a648=sin(a607); + a646=(a612*a648); + a644=sin(a652); + a640=cos(a607); + a656=(a644*a640); + a646=(a646-a656); + a656=casadi_sq(a646); + a650=(a649*a640); + a645=sin(a652); + a636=(a645*a648); + a650=(a650+a636); + a636=casadi_sq(a650); + a656=(a656+a636); + a636=(a646/a656); + a638=arg[3]? arg[3][44] : 0; + a654=(a636*a638); + a639=(a649*a654); + a655=(a650/a656); + a647=(a655*a638); + a661=(a644*a647); + a639=(a639+a661); + a661=cos(a607); + a651=(a639*a661); + a660=sin(a607); + a637=cos(a607); + a641=(a612*a637); + a659=sin(a607); + a658=(a644*a659); + a641=(a641+a658); + a658=(a641/a656); + a636=(a636/a656); + a646=(a646+a646); + a641=(a646*a641); + a650=(a650+a650); + a662=(a645*a637); + a663=(a649*a659); + a662=(a662-a663); + a663=(a650*a662); + a641=(a641+a663); + a663=(a636*a641); + a658=(a658-a663); + a658=(a638*a658); + a663=(a649*a658); + a662=(a662/a656); + a655=(a655/a656); + a641=(a655*a641); + a662=(a662-a641); + a662=(a638*a662); + a641=(a644*a662); + a663=(a663+a641); + a663=(a660*a663); + a651=(a651+a663); + a663=cos(a607); + a662=(a612*a662); + a658=(a645*a658); + a662=(a662-a658); + a662=(a663*a662); + a658=(a612*a647); + a641=(a645*a654); + a658=(a658-a641); + a607=sin(a607); + a641=(a658*a607); + a662=(a662-a641); + a651=(a651+a662); + a662=arg[0]? arg[0][74] : 0; + a641=arg[3]? arg[3][43] : 0; + a641=(a4*a641); + a664=(a662*a641); + a665=sin(a602); + a664=(a664*a665); + a651=(a651+a664); + a664=arg[3]? arg[3][42] : 0; + a664=(a4*a664); + a665=(a662*a664); + a653=(a38*a653); + a653=(a602+a653); + a666=cos(a653); + a667=(a665*a666); + a668=(a662*a641); + a669=sin(a653); + a670=(a668*a669); + a667=(a667+a670); + a651=(a651+a667); + a667=(a10*a664); + a670=(a662*a667); + a642=(a44*a642); + a642=(a602+a642); + a671=cos(a642); + a672=(a670*a671); + a673=(a10*a641); + a674=(a662*a673); + a675=sin(a642); + a676=(a674*a675); + a672=(a672+a676); + a651=(a651+a672); + a672=(a10*a664); + a676=(a662*a672); + a544=(a44*a544); + a544=(a602+a544); + a677=cos(a544); + a678=(a676*a677); + a679=(a10*a641); + a680=(a662*a679); + a681=sin(a544); + a682=(a680*a681); + a678=(a678+a682); + a651=(a651+a678); + a662=(a662*a664); + a678=cos(a602); + a662=(a662*a678); + a651=(a651+a662); + a662=(a598*a604); + a678=sin(a602); + a662=(a662*a678); + a678=sin(a602); + a678=(a598*a678); + a682=cos(a602); + a682=(a590*a682); + a678=(a678+a682); + a682=(a678/a606); + a683=cos(a602); + a683=(a598*a683); + a684=sin(a602); + a684=(a590*a684); + a683=(a683-a684); + a684=(a600*a683); + a678=(a596*a678); + a684=(a684-a678); + a678=(a586*a684); + a682=(a682+a678); + a682=(a588*a682); + a678=(a598*a682); + a678=(a643*a678); + a662=(a662+a678); + a651=(a651+a662); + a662=(a590*a604); + a678=cos(a602); + a662=(a662*a678); + a682=(a590*a682); + a682=(a601*a682); + a662=(a662-a682); + a651=(a651+a662); + a683=(a683/a606); + a684=(a605*a684); + a683=(a683-a684); + a683=(a588*a683); + a684=(a590*a683); + a684=(a559*a684); + a662=(a590*a597); + a682=sin(a602); + a662=(a662*a682); + a684=(a684-a662); + a651=(a651-a684); + a684=(a598*a597); + a662=cos(a602); + a684=(a684*a662); + a683=(a598*a683); + a683=(a608*a683); + a684=(a684+a683); + a651=(a651-a684); + a684=arg[1]? arg[1][41] : 0; + a683=cos(a684); + a662=cos(a684); + a682=sin(a602); + a678=(a662*a682); + a685=sin(a684); + a686=cos(a602); + a687=(a685*a686); + a678=(a678-a687); + a687=casadi_sq(a678); + a686=(a683*a686); + a688=sin(a684); + a682=(a688*a682); + a686=(a686+a682); + a682=casadi_sq(a686); + a687=(a687+a682); + a682=(a678/a687); + a689=cos(a684); + a690=sin(a602); + a691=(a689*a690); + a692=sin(a684); + a693=cos(a602); + a694=(a692*a693); + a691=(a691-a694); + a694=cos(a684); + a693=(a694*a693); + a684=sin(a684); + a690=(a684*a690); + a693=(a693+a690); + a690=atan2(a691,a693); + a690=(a690*a34); + a695=(a682*a690); + a696=(a683*a695); + a697=(a686/a687); + a698=(a697*a690); + a699=(a685*a698); + a696=(a696+a699); + a699=cos(a602); + a696=(a696*a699); + a699=sin(a602); + a700=cos(a602); + a701=(a662*a700); + a702=sin(a602); + a703=(a685*a702); + a701=(a701+a703); + a703=(a701/a687); + a704=(a682/a687); + a705=(a678+a678); + a705=(a705*a701); + a706=(a686+a686); + a700=(a688*a700); + a702=(a683*a702); + a700=(a700-a702); + a706=(a706*a700); + a705=(a705+a706); + a704=(a704*a705); + a703=(a703-a704); + a703=(a690*a703); + a704=casadi_sq(a691); + a706=casadi_sq(a693); + a704=(a704+a706); + a706=(a693/a704); + a702=cos(a602); + a707=(a689*a702); + a708=sin(a602); + a709=(a692*a708); + a707=(a707+a709); + a706=(a706*a707); + a704=(a691/a704); + a702=(a684*a702); + a708=(a694*a708); + a702=(a702-a708); + a704=(a704*a702); + a706=(a706-a704); + a706=(a34*a706); + a682=(a682*a706); + a703=(a703+a682); + a683=(a683*a703); + a682=(a700/a687); + a687=(a697/a687); + a687=(a687*a705); + a682=(a682-a687); + a690=(a690*a682); + a697=(a697*a706); + a690=(a690+a697); + a685=(a685*a690); + a683=(a683+a685); + a699=(a699*a683); + a696=(a696+a699); + a651=(a651+a696); + a696=cos(a602); + a690=(a662*a690); + a703=(a688*a703); + a690=(a690-a703); + a696=(a696*a690); + a662=(a662*a698); + a688=(a688*a695); + a662=(a662-a688); + a688=sin(a602); + a662=(a662*a688); + a696=(a696-a662); + a651=(a651+a696); + a696=casadi_sq(a691); + a662=casadi_sq(a693); + a696=(a696+a662); + a662=(a691/a696); + a688=atan2(a678,a686); + a688=(a688*a34); + a695=(a662*a688); + a698=(a694*a695); + a690=(a693/a696); + a703=(a690*a688); + a699=(a692*a703); + a698=(a698+a699); + a699=cos(a602); + a698=(a698*a699); + a699=sin(a602); + a683=(a707/a696); + a685=(a662/a696); + a691=(a691+a691); + a691=(a691*a707); + a693=(a693+a693); + a693=(a693*a702); + a691=(a691+a693); + a685=(a685*a691); + a683=(a683-a685); + a683=(a688*a683); + a685=casadi_sq(a678); + a693=casadi_sq(a686); + a685=(a685+a693); + a686=(a686/a685); + a686=(a686*a701); + a678=(a678/a685); + a678=(a678*a700); + a686=(a686-a678); + a686=(a34*a686); + a662=(a662*a686); + a683=(a683+a662); + a694=(a694*a683); + a702=(a702/a696); + a696=(a690/a696); + a696=(a696*a691); + a702=(a702-a696); + a688=(a688*a702); + a690=(a690*a686); + a688=(a688+a690); + a692=(a692*a688); + a694=(a694+a692); + a699=(a699*a694); + a698=(a698+a699); + a651=(a651+a698); + a698=cos(a602); + a688=(a689*a688); + a683=(a684*a683); + a688=(a688-a683); + a698=(a698*a688); + a689=(a689*a703); + a684=(a684*a695); + a689=(a689-a684); + a684=sin(a602); + a689=(a689*a684); + a698=(a698-a689); + a651=(a651+a698); + if (res[0]!=0) res[0][52]=a651; + a651=(a28*a34); + a698=(a28*a34); + a651=(a651+a698); + if (res[0]!=0) res[0][53]=a651; + a651=(a28*a34); + a698=(a28*a34); + a651=(a651+a698); + if (res[0]!=0) res[0][54]=a651; + a651=cos(a652); + a698=(a647*a651); + a689=cos(a652); + a684=(a648*a689); + a695=sin(a652); + a703=(a640*a695); + a684=(a684-a703); + a703=(a684/a656); + a684=(a650*a684); + a688=sin(a652); + a683=(a648*a688); + a651=(a640*a651); + a683=(a683+a651); + a651=(a646*a683); + a684=(a684-a651); + a651=(a655*a684); + a703=(a703-a651); + a703=(a638*a703); + a651=(a644*a703); + a698=(a698+a651); + a695=(a654*a695); + a683=(a683/a656); + a684=(a636*a684); + a683=(a683+a684); + a683=(a638*a683); + a684=(a649*a683); + a695=(a695+a684); + a698=(a698-a695); + a698=(a660*a698); + a695=(a612*a703); + a688=(a647*a688); + a695=(a695-a688); + a689=(a654*a689); + a688=(a645*a683); + a689=(a689-a688); + a695=(a695-a689); + a695=(a663*a695); + a698=(a698+a695); + if (res[0]!=0) res[0][55]=a698; + a695=arg[0]? arg[0][47] : 0; + a689=cos(a695); + a688=cos(a695); + a657=(a8*a657); + a592=(a5*a592); + a657=(a657+a592); + a592=(a5*a657); + a684=arg[0]? arg[0][77] : 0; + a651=(a8*a684); + a592=(a592+a651); + a651=(a5*a657); + a699=(a8*a684); + a651=(a651+a699); + a699=(a10*a651); + a699=(a592+a699); + a694=(a5*a657); + a692=(a8*a684); + a694=(a694+a692); + a692=(a10*a694); + a699=(a699+a692); + a657=(a5*a657); + a684=(a8*a684); + a657=(a657+a684); + a699=(a699+a657); + a699=(a4*a699); + a699=(a652+a699); + a657=sin(a699); + a684=(a688*a657); + a692=sin(a695); + a690=cos(a699); + a686=(a692*a690); + a684=(a684-a686); + a686=casadi_sq(a684); + a702=(a689*a690); + a696=sin(a695); + a691=(a696*a657); + a702=(a702+a691); + a691=casadi_sq(a702); + a686=(a686+a691); + a691=(a684/a686); + a662=arg[3]? arg[3][47] : 0; + a678=(a691*a662); + a700=(a689*a678); + a685=(a702/a686); + a701=(a685*a662); + a693=(a692*a701); + a700=(a700+a693); + a693=cos(a699); + a707=(a700*a693); + a697=sin(a699); + a706=cos(a699); + a682=(a688*a706); + a687=sin(a699); + a705=(a692*a687); + a682=(a682+a705); + a705=(a682/a686); + a691=(a691/a686); + a684=(a684+a684); + a682=(a684*a682); + a702=(a702+a702); + a704=(a696*a706); + a708=(a689*a687); + a704=(a704-a708); + a708=(a702*a704); + a682=(a682+a708); + a708=(a691*a682); + a705=(a705-a708); + a705=(a662*a705); + a708=(a689*a705); + a704=(a704/a686); + a685=(a685/a686); + a682=(a685*a682); + a704=(a704-a682); + a704=(a662*a704); + a682=(a692*a704); + a708=(a708+a682); + a708=(a697*a708); + a707=(a707+a708); + a708=cos(a699); + a682=(a688*a704); + a709=(a696*a705); + a682=(a682-a709); + a682=(a708*a682); + a709=(a688*a701); + a710=(a696*a678); + a709=(a709-a710); + a699=sin(a699); + a710=(a709*a699); + a682=(a682-a710); + a707=(a707+a682); + a682=arg[0]? arg[0][76] : 0; + a710=arg[3]? arg[3][46] : 0; + a710=(a4*a710); + a711=(a682*a710); + a712=sin(a652); + a711=(a711*a712); + a711=(a707+a711); + a712=arg[3]? arg[3][45] : 0; + a712=(a4*a712); + a713=(a682*a712); + a694=(a38*a694); + a694=(a652+a694); + a714=cos(a694); + a715=(a713*a714); + a716=(a682*a710); + a717=sin(a694); + a718=(a716*a717); + a715=(a715+a718); + a711=(a711+a715); + a718=(a10*a712); + a719=(a682*a718); + a651=(a44*a651); + a651=(a652+a651); + a720=cos(a651); + a721=(a719*a720); + a722=(a10*a710); + a723=(a682*a722); + a724=sin(a651); + a725=(a723*a724); + a721=(a721+a725); + a711=(a711+a721); + a725=(a10*a712); + a726=(a682*a725); + a592=(a44*a592); + a592=(a652+a592); + a727=cos(a592); + a728=(a726*a727); + a729=(a10*a710); + a730=(a682*a729); + a731=sin(a592); + a732=(a730*a731); + a728=(a728+a732); + a711=(a711+a728); + a682=(a682*a712); + a732=cos(a652); + a682=(a682*a732); + a711=(a711+a682); + a682=(a648*a654); + a732=sin(a652); + a682=(a682*a732); + a732=cos(a652); + a733=(a648*a683); + a733=(a732*a733); + a682=(a682+a733); + a711=(a711+a682); + a682=(a640*a654); + a733=cos(a652); + a682=(a682*a733); + a733=sin(a652); + a683=(a640*a683); + a683=(a733*a683); + a682=(a682-a683); + a711=(a711+a682); + a682=cos(a652); + a683=(a640*a703); + a683=(a682*a683); + a734=(a640*a647); + a735=sin(a652); + a734=(a734*a735); + a683=(a683-a734); + a711=(a711-a683); + a683=(a648*a647); + a734=cos(a652); + a683=(a683*a734); + a734=sin(a652); + a703=(a648*a703); + a703=(a734*a703); + a683=(a683+a703); + a711=(a711-a683); + a683=arg[1]? arg[1][44] : 0; + a703=cos(a683); + a735=cos(a683); + a736=sin(a652); + a737=(a735*a736); + a738=sin(a683); + a739=cos(a652); + a740=(a738*a739); + a737=(a737-a740); + a740=casadi_sq(a737); + a739=(a703*a739); + a741=sin(a683); + a736=(a741*a736); + a739=(a739+a736); + a736=casadi_sq(a739); + a740=(a740+a736); + a736=(a737/a740); + a742=cos(a683); + a743=sin(a652); + a744=(a742*a743); + a745=sin(a683); + a746=cos(a652); + a747=(a745*a746); + a744=(a744-a747); + a747=cos(a683); + a746=(a747*a746); + a683=sin(a683); + a743=(a683*a743); + a746=(a746+a743); + a743=atan2(a744,a746); + a743=(a743*a34); + a748=(a736*a743); + a749=(a703*a748); + a750=(a739/a740); + a751=(a750*a743); + a752=(a738*a751); + a749=(a749+a752); + a752=cos(a652); + a749=(a749*a752); + a752=sin(a652); + a753=cos(a652); + a754=(a735*a753); + a755=sin(a652); + a756=(a738*a755); + a754=(a754+a756); + a756=(a754/a740); + a757=(a736/a740); + a758=(a737+a737); + a758=(a758*a754); + a759=(a739+a739); + a753=(a741*a753); + a755=(a703*a755); + a753=(a753-a755); + a759=(a759*a753); + a758=(a758+a759); + a757=(a757*a758); + a756=(a756-a757); + a756=(a743*a756); + a757=casadi_sq(a744); + a759=casadi_sq(a746); + a757=(a757+a759); + a759=(a746/a757); + a755=cos(a652); + a760=(a742*a755); + a761=sin(a652); + a762=(a745*a761); + a760=(a760+a762); + a759=(a759*a760); + a757=(a744/a757); + a755=(a683*a755); + a761=(a747*a761); + a755=(a755-a761); + a757=(a757*a755); + a759=(a759-a757); + a759=(a34*a759); + a736=(a736*a759); + a756=(a756+a736); + a703=(a703*a756); + a736=(a753/a740); + a740=(a750/a740); + a740=(a740*a758); + a736=(a736-a740); + a743=(a743*a736); + a750=(a750*a759); + a743=(a743+a750); + a738=(a738*a743); + a703=(a703+a738); + a752=(a752*a703); + a749=(a749+a752); + a711=(a711+a749); + a749=cos(a652); + a743=(a735*a743); + a756=(a741*a756); + a743=(a743-a756); + a749=(a749*a743); + a735=(a735*a751); + a741=(a741*a748); + a735=(a735-a741); + a741=sin(a652); + a735=(a735*a741); + a749=(a749-a735); + a711=(a711+a749); + a749=casadi_sq(a744); + a735=casadi_sq(a746); + a749=(a749+a735); + a735=(a744/a749); + a741=atan2(a737,a739); + a741=(a741*a34); + a748=(a735*a741); + a751=(a747*a748); + a743=(a746/a749); + a756=(a743*a741); + a752=(a745*a756); + a751=(a751+a752); + a752=cos(a652); + a751=(a751*a752); + a752=sin(a652); + a703=(a760/a749); + a738=(a735/a749); + a744=(a744+a744); + a744=(a744*a760); + a746=(a746+a746); + a746=(a746*a755); + a744=(a744+a746); + a738=(a738*a744); + a703=(a703-a738); + a703=(a741*a703); + a738=casadi_sq(a737); + a746=casadi_sq(a739); + a738=(a738+a746); + a739=(a739/a738); + a739=(a739*a754); + a737=(a737/a738); + a737=(a737*a753); + a739=(a739-a737); + a739=(a34*a739); + a735=(a735*a739); + a703=(a703+a735); + a747=(a747*a703); + a755=(a755/a749); + a749=(a743/a749); + a749=(a749*a744); + a755=(a755-a749); + a741=(a741*a755); + a743=(a743*a739); + a741=(a741+a743); + a745=(a745*a741); + a747=(a747+a745); + a752=(a752*a747); + a751=(a751+a752); + a711=(a711+a751); + a751=cos(a652); + a741=(a742*a741); + a703=(a683*a703); + a741=(a741-a703); + a751=(a751*a741); + a742=(a742*a756); + a683=(a683*a748); + a742=(a742-a683); + a683=sin(a652); + a742=(a742*a683); + a751=(a751-a742); + a711=(a711+a751); + if (res[0]!=0) res[0][56]=a711; + a711=(a28*a34); + a751=(a28*a34); + a711=(a711+a751); + if (res[0]!=0) res[0][57]=a711; + a711=(a28*a34); + a28=(a28*a34); + a711=(a711+a28); + if (res[0]!=0) res[0][58]=a711; + a711=sin(a695); + a28=(a690*a705); + a751=(a678*a687); + a28=(a28-a751); + a28=(a711*a28); + a751=cos(a695); + a742=(a678*a706); + a705=(a657*a705); + a742=(a742+a705); + a742=(a751*a742); + a28=(a28-a742); + a742=cos(a695); + a705=(a690*a704); + a683=(a701*a687); + a705=(a705-a683); + a705=(a742*a705); + a28=(a28-a705); + a705=sin(a695); + a683=(a701*a706); + a704=(a657*a704); + a683=(a683+a704); + a683=(a705*a683); + a28=(a28-a683); + if (res[0]!=0) res[0][59]=a28; + a28=(a690*a678); + a683=cos(a695); + a28=(a28*a683); + a683=sin(a695); + a683=(a657*a683); + a704=cos(a695); + a704=(a690*a704); + a683=(a683+a704); + a704=(a683/a686); + a748=cos(a695); + a748=(a657*a748); + a756=sin(a695); + a756=(a690*a756); + a748=(a748-a756); + a756=(a702*a748); + a683=(a684*a683); + a756=(a756-a683); + a683=(a691*a756); + a704=(a704+a683); + a704=(a662*a704); + a683=(a690*a704); + a683=(a711*a683); + a28=(a28-a683); + a683=(a657*a678); + a741=sin(a695); + a683=(a683*a741); + a704=(a657*a704); + a704=(a751*a704); + a683=(a683+a704); + a28=(a28+a683); + a748=(a748/a686); + a756=(a685*a756); + a748=(a748-a756); + a748=(a662*a748); + a756=(a690*a748); + a756=(a742*a756); + a683=(a690*a701); + a704=sin(a695); + a683=(a683*a704); + a756=(a756-a683); + a28=(a28-a756); + a756=(a657*a701); + a683=cos(a695); + a756=(a756*a683); + a748=(a657*a748); + a748=(a705*a748); + a756=(a756+a748); + a28=(a28-a756); + a756=arg[1]? arg[1][47] : 0; + a748=cos(a756); + a683=cos(a756); + a704=sin(a695); + a741=(a683*a704); + a703=sin(a756); + a752=cos(a695); + a747=(a703*a752); + a741=(a741-a747); + a747=casadi_sq(a741); + a752=(a748*a752); + a745=sin(a756); + a704=(a745*a704); + a752=(a752+a704); + a704=casadi_sq(a752); + a747=(a747+a704); + a704=(a741/a747); + a743=cos(a756); + a739=sin(a695); + a755=(a743*a739); + a749=sin(a756); + a744=cos(a695); + a735=(a749*a744); + a755=(a755-a735); + a735=cos(a756); + a744=(a735*a744); + a756=sin(a756); + a739=(a756*a739); + a744=(a744+a739); + a739=atan2(a755,a744); + a739=(a739*a34); + a737=(a704*a739); + a753=(a748*a737); + a738=(a752/a747); + a754=(a738*a739); + a746=(a703*a754); + a753=(a753+a746); + a746=cos(a695); + a753=(a753*a746); + a746=sin(a695); + a760=cos(a695); + a750=(a683*a760); + a759=sin(a695); + a736=(a703*a759); + a750=(a750+a736); + a736=(a750/a747); + a740=(a704/a747); + a758=(a741+a741); + a758=(a758*a750); + a757=(a752+a752); + a760=(a745*a760); + a759=(a748*a759); + a760=(a760-a759); + a757=(a757*a760); + a758=(a758+a757); + a740=(a740*a758); + a736=(a736-a740); + a736=(a739*a736); + a740=casadi_sq(a755); + a757=casadi_sq(a744); + a740=(a740+a757); + a757=(a744/a740); + a759=cos(a695); + a761=(a743*a759); + a762=sin(a695); + a763=(a749*a762); + a761=(a761+a763); + a757=(a757*a761); + a740=(a755/a740); + a759=(a756*a759); + a762=(a735*a762); + a759=(a759-a762); + a740=(a740*a759); + a757=(a757-a740); + a757=(a34*a757); + a704=(a704*a757); + a736=(a736+a704); + a748=(a748*a736); + a704=(a760/a747); + a747=(a738/a747); + a747=(a747*a758); + a704=(a704-a747); + a739=(a739*a704); + a738=(a738*a757); + a739=(a739+a738); + a703=(a703*a739); + a748=(a748+a703); + a746=(a746*a748); + a753=(a753+a746); + a28=(a28+a753); + a753=cos(a695); + a739=(a683*a739); + a736=(a745*a736); + a739=(a739-a736); + a753=(a753*a739); + a683=(a683*a754); + a745=(a745*a737); + a683=(a683-a745); + a745=sin(a695); + a683=(a683*a745); + a753=(a753-a683); + a28=(a28+a753); + a753=casadi_sq(a755); + a683=casadi_sq(a744); + a753=(a753+a683); + a683=(a755/a753); + a745=atan2(a741,a752); + a745=(a745*a34); + a737=(a683*a745); + a754=(a735*a737); + a739=(a744/a753); + a736=(a739*a745); + a746=(a749*a736); + a754=(a754+a746); + a746=cos(a695); + a754=(a754*a746); + a746=sin(a695); + a748=(a761/a753); + a703=(a683/a753); + a755=(a755+a755); + a755=(a755*a761); + a744=(a744+a744); + a744=(a744*a759); + a755=(a755+a744); + a703=(a703*a755); + a748=(a748-a703); + a748=(a745*a748); + a703=casadi_sq(a741); + a744=casadi_sq(a752); + a703=(a703+a744); + a752=(a752/a703); + a752=(a752*a750); + a741=(a741/a703); + a741=(a741*a760); + a752=(a752-a741); + a752=(a34*a752); + a683=(a683*a752); + a748=(a748+a683); + a735=(a735*a748); + a759=(a759/a753); + a753=(a739/a753); + a753=(a753*a755); + a759=(a759-a753); + a745=(a745*a759); + a739=(a739*a752); + a745=(a745+a739); + a749=(a749*a745); + a735=(a735+a749); + a746=(a746*a735); + a754=(a754+a746); + a28=(a28+a754); + a754=cos(a695); + a745=(a743*a745); + a748=(a756*a748); + a745=(a745-a748); + a754=(a754*a745); + a743=(a743*a736); + a756=(a756*a737); + a743=(a743-a756); + a695=sin(a695); + a743=(a743*a695); + a754=(a754-a743); + a28=(a28+a754); + if (res[0]!=0) res[0][60]=a28; + a28=sin(a13); + a28=(a28*a36); + a754=cos(a13); + a754=(a754*a31); + a28=(a28-a754); + a754=cos(a3); + a754=(a754*a31); + a28=(a28-a754); + a754=sin(a11); + a754=(a754*a40); + a743=cos(a11); + a743=(a743*a47); + a754=(a754-a743); + a28=(a28+a754); + a754=sin(a7); + a754=(a754*a46); + a743=cos(a7); + a743=(a743*a53); + a754=(a754-a743); + a28=(a28+a754); + a3=sin(a3); + a3=(a3*a36); + a28=(a28+a3); + if (res[0]!=0) res[0][61]=a28; + a28=(a10*a34); + a3=-2.; + a754=(a3*a34); + a743=(a10*a34); + a695=(a754-a743); + a28=(a28-a695); + a695=(a10*a34); + a28=(a28+a695); + a695=(a10*a34); + a28=(a28+a695); + if (res[0]!=0) res[0][62]=a28; + a28=1.4999999999999999e-01; + a27=(a28*a27); + a24=(a24*a27); + a30=(a28*a30); + a27=(a2*a30); + a32=(a28*a32); + a695=(a16*a32); + a27=(a27+a695); + a695=(a27/a18); + a15=(a15*a27); + a27=(a20*a30); + a756=(a1*a32); + a27=(a27-a756); + a19=(a19*a27); + a15=(a15+a19); + a21=(a21*a15); + a695=(a695-a21); + a695=(a22*a695); + a1=(a1*a695); + a27=(a27/a18); + a25=(a25*a15); + a27=(a27-a25); + a22=(a22*a27); + a16=(a16*a22); + a1=(a1+a16); + a29=(a29*a1); + a24=(a24+a29); + a2=(a2*a22); + a20=(a20*a695); + a2=(a2-a20); + a35=(a35*a2); + a12=(a28*a12); + a33=(a33*a12); + a35=(a35-a33); + a24=(a24+a35); + a39=(a28*a39); + a37=(a37*a39); + a42=(a28*a42); + a41=(a41*a42); + a37=(a37+a41); + a41=(a24+a37); + a42=7.4999999999999997e-02; + a45=(a42*a45); + a43=(a43*a45); + a49=(a42*a49); + a48=(a48*a49); + a43=(a43+a48); + a41=(a41+a43); + a51=(a42*a51); + a50=(a50*a51); + a55=(a42*a55); + a54=(a54*a55); + a50=(a50+a54); + a41=(a41+a50); + if (res[0]!=0) res[0][63]=a41; + a41=5.9999999999999998e-02; + a54=(a41*a77); + a54=(a74*a54); + a55=(a41*a80); + a51=(a57*a55); + a48=(a41*a82); + a49=(a66*a48); + a51=(a51+a49); + a49=(a51/a68); + a51=(a65*a51); + a45=(a70*a55); + a39=(a59*a48); + a45=(a45-a39); + a39=(a69*a45); + a51=(a51+a39); + a39=(a71*a51); + a49=(a49-a39); + a49=(a72*a49); + a39=(a59*a49); + a45=(a45/a68); + a51=(a75*a51); + a45=(a45-a51); + a45=(a72*a45); + a51=(a66*a45); + a39=(a39+a51); + a39=(a79*a39); + a54=(a54+a39); + a39=(a57*a45); + a51=(a70*a49); + a39=(a39-a51); + a39=(a85*a39); + a51=(a41*a52); + a51=(a86*a51); + a39=(a39-a51); + a54=(a54+a39); + a39=(a41*a90); + a39=(a89*a39); + a51=(a41*a93); + a51=(a92*a51); + a39=(a39+a51); + a51=(a54+a39); + a35=2.9999999999999999e-02; + a33=(a35*a95); + a33=(a94*a33); + a12=(a35*a99); + a12=(a98*a12); + a33=(a33+a12); + a51=(a51+a33); + a12=(a35*a101); + a12=(a100*a12); + a2=(a35*a105); + a2=(a104*a2); + a12=(a12+a2); + a51=(a51+a12); + a2=(a23*a30); + a20=(a14*a695); + a2=(a2+a20); + a102=(a102*a2); + a51=(a51-a102); + a695=(a17*a695); + a23=(a23*a32); + a695=(a695-a23); + a106=(a106*a695); + a51=(a51+a106); + a17=(a17*a22); + a32=(a26*a32); + a17=(a17-a32); + a81=(a81*a17); + a51=(a51-a81); + a26=(a26*a30); + a14=(a14*a22); + a26=(a26+a14); + a107=(a107*a26); + a51=(a51-a107); + if (res[0]!=0) res[0][64]=a51; + a51=2.4000000000000000e-02; + a107=(a51*a133); + a107=(a111*a107); + a26=(a51*a109); + a14=(a84*a26); + a22=(a51*a131); + a30=(a116*a22); + a14=(a14+a30); + a30=(a14/a128); + a14=(a118*a14); + a81=(a117*a26); + a17=(a121*a22); + a81=(a81-a17); + a17=(a122*a81); + a14=(a14+a17); + a17=(a108*a14); + a30=(a30-a17); + a30=(a110*a30); + a17=(a121*a30); + a81=(a81/a128); + a14=(a127*a14); + a81=(a81-a14); + a81=(a110*a81); + a14=(a116*a81); + a17=(a17+a14); + a17=(a132*a17); + a107=(a107+a17); + a17=(a84*a81); + a14=(a117*a30); + a17=(a17-a14); + a17=(a135*a17); + a14=(a51*a60); + a14=(a130*a14); + a17=(a17-a14); + a107=(a107+a17); + a17=(a51*a138); + a17=(a137*a17); + a14=(a51*a141); + a14=(a140*a14); + a17=(a17+a14); + a14=(a107+a17); + a32=1.2000000000000000e-02; + a106=(a32*a143); + a106=(a142*a106); + a695=(a32*a147); + a695=(a146*a695); + a106=(a106+a695); + a14=(a14+a106); + a695=(a32*a149); + a695=(a148*a695); + a23=(a32*a153); + a23=(a152*a23); + a695=(a695+a23); + a14=(a14+a695); + a23=(a73*a55); + a102=(a64*a49); + a23=(a23+a102); + a23=(a115*a23); + a14=(a14-a23); + a49=(a67*a49); + a23=(a73*a48); + a49=(a49-a23); + a49=(a78*a49); + a14=(a14+a49); + a49=(a67*a45); + a48=(a76*a48); + a49=(a49-a48); + a49=(a62*a49); + a14=(a14-a49); + a55=(a76*a55); + a45=(a64*a45); + a55=(a55+a45); + a55=(a83*a55); + a14=(a14-a55); + if (res[0]!=0) res[0][65]=a14; + a14=9.5999999999999992e-03; + a55=(a14*a179); + a55=(a157*a55); + a45=(a14*a154); + a49=(a161*a45); + a48=(a14*a177); + a23=(a162*a48); + a49=(a49+a23); + a23=(a49/a174); + a49=(a164*a49); + a102=(a163*a45); + a2=(a167*a48); + a102=(a102-a2); + a2=(a168*a102); + a49=(a49+a2); + a2=(a134*a49); + a23=(a23-a2); + a23=(a150*a23); + a2=(a167*a23); + a102=(a102/a174); + a49=(a173*a49); + a102=(a102-a49); + a102=(a150*a102); + a49=(a162*a102); + a2=(a2+a49); + a2=(a178*a2); + a55=(a55+a2); + a2=(a161*a102); + a49=(a163*a23); + a2=(a2-a49); + a2=(a181*a2); + a49=(a14*a123); + a49=(a182*a49); + a2=(a2-a49); + a55=(a55+a2); + a2=9.6000000000000009e-03; + a49=(a2*a186); + a49=(a185*a49); + a20=(a2*a189); + a20=(a188*a20); + a49=(a49+a20); + a20=(a55+a49); + a29=4.8000000000000004e-03; + a1=(a29*a191); + a1=(a190*a1); + a16=(a29*a195); + a16=(a194*a16); + a1=(a1+a16); + a20=(a20+a1); + a16=(a29*a197); + a16=(a196*a16); + a27=(a29*a201); + a27=(a200*a27); + a16=(a16+a27); + a20=(a20+a16); + a27=(a126*a26); + a25=(a120*a30); + a27=(a27+a25); + a27=(a198*a27); + a20=(a20-a27); + a30=(a112*a30); + a27=(a126*a22); + a30=(a30-a27); + a30=(a202*a30); + a20=(a20+a30); + a30=(a112*a81); + a22=(a119*a22); + a30=(a30-a22); + a30=(a159*a30); + a20=(a20-a30); + a26=(a119*a26); + a81=(a120*a81); + a26=(a26+a81); + a26=(a203*a26); + a20=(a20-a26); + if (res[0]!=0) res[0][66]=a20; + a20=3.8400000000000001e-03; + a26=(a20*a229); + a26=(a207*a26); + a81=(a20*a205); + a30=(a180*a81); + a22=(a20*a227); + a27=(a212*a22); + a30=(a30+a27); + a27=(a30/a224); + a30=(a214*a30); + a25=(a213*a81); + a15=(a217*a22); + a25=(a25-a15); + a15=(a218*a25); + a30=(a30+a15); + a15=(a204*a30); + a27=(a27-a15); + a27=(a206*a27); + a15=(a217*a27); + a25=(a25/a224); + a30=(a223*a30); + a25=(a25-a30); + a25=(a206*a25); + a30=(a212*a25); + a15=(a15+a30); + a15=(a228*a15); + a26=(a26+a15); + a15=(a180*a25); + a30=(a213*a27); + a15=(a15-a30); + a15=(a231*a15); + a30=(a20*a175); + a30=(a226*a30); + a15=(a15-a30); + a26=(a26+a15); + a15=3.8400000000000005e-03; + a30=(a15*a234); + a30=(a233*a30); + a18=(a15*a237); + a18=(a236*a18); + a30=(a30+a18); + a18=(a26+a30); + a21=1.9200000000000003e-03; + a19=(a21*a239); + a19=(a238*a19); + a756=(a21*a243); + a756=(a242*a756); + a19=(a19+a756); + a18=(a18+a19); + a756=(a21*a245); + a756=(a244*a756); + a737=(a21*a249); + a737=(a248*a737); + a756=(a756+a737); + a18=(a18+a756); + a737=(a172*a45); + a736=(a166*a23); + a737=(a737+a736); + a737=(a211*a737); + a18=(a18-a737); + a23=(a158*a23); + a737=(a172*a48); + a23=(a23-a737); + a23=(a169*a23); + a18=(a18+a23); + a23=(a158*a102); + a48=(a165*a48); + a23=(a23-a48); + a23=(a155*a23); + a18=(a18-a23); + a45=(a165*a45); + a102=(a166*a102); + a45=(a45+a102); + a45=(a176*a45); + a18=(a18-a45); + if (res[0]!=0) res[0][67]=a18; + a18=1.5360000000000003e-03; + a45=(a18*a275); + a45=(a253*a45); + a102=(a18*a250); + a23=(a257*a102); + a48=(a18*a273); + a737=(a258*a48); + a23=(a23+a737); + a737=(a23/a270); + a23=(a260*a23); + a736=(a259*a102); + a745=(a263*a48); + a736=(a736-a745); + a745=(a264*a736); + a23=(a23+a745); + a745=(a230*a23); + a737=(a737-a745); + a737=(a246*a737); + a745=(a263*a737); + a736=(a736/a270); + a23=(a269*a23); + a736=(a736-a23); + a736=(a246*a736); + a23=(a258*a736); + a745=(a745+a23); + a745=(a274*a745); + a45=(a45+a745); + a745=(a257*a736); + a23=(a259*a737); + a745=(a745-a23); + a745=(a277*a745); + a23=(a18*a219); + a23=(a278*a23); + a745=(a745-a23); + a45=(a45+a745); + a745=(a18*a282); + a745=(a281*a745); + a23=(a18*a285); + a23=(a284*a23); + a745=(a745+a23); + a23=(a45+a745); + a748=7.6800000000000013e-04; + a746=(a748*a287); + a746=(a286*a746); + a735=(a748*a291); + a735=(a290*a735); + a746=(a746+a735); + a23=(a23+a746); + a735=(a748*a293); + a735=(a292*a735); + a749=(a748*a297); + a749=(a296*a749); + a735=(a735+a749); + a23=(a23+a735); + a749=(a222*a81); + a739=(a216*a27); + a749=(a749+a739); + a749=(a294*a749); + a23=(a23-a749); + a27=(a208*a27); + a749=(a222*a22); + a27=(a27-a749); + a27=(a298*a27); + a23=(a23+a27); + a27=(a208*a25); + a22=(a215*a22); + a27=(a27-a22); + a27=(a255*a27); + a23=(a23-a27); + a81=(a215*a81); + a25=(a216*a25); + a81=(a81+a25); + a81=(a299*a81); + a23=(a23-a81); + if (res[0]!=0) res[0][68]=a23; + a23=6.1440000000000019e-04; + a81=(a23*a325); + a81=(a303*a81); + a25=(a23*a301); + a27=(a276*a25); + a22=(a23*a323); + a749=(a308*a22); + a27=(a27+a749); + a749=(a27/a320); + a27=(a310*a27); + a739=(a309*a25); + a752=(a313*a22); + a739=(a739-a752); + a752=(a314*a739); + a27=(a27+a752); + a752=(a300*a27); + a749=(a749-a752); + a749=(a302*a749); + a752=(a313*a749); + a739=(a739/a320); + a27=(a319*a27); + a739=(a739-a27); + a739=(a302*a739); + a27=(a308*a739); + a752=(a752+a27); + a752=(a324*a752); + a81=(a81+a752); + a752=(a276*a739); + a27=(a309*a749); + a752=(a752-a27); + a752=(a327*a752); + a27=(a23*a271); + a27=(a322*a27); + a752=(a752-a27); + a81=(a81+a752); + a752=(a23*a330); + a752=(a329*a752); + a27=(a23*a333); + a27=(a332*a27); + a752=(a752+a27); + a27=(a81+a752); + a759=3.0720000000000009e-04; + a753=(a759*a335); + a753=(a334*a753); + a755=(a759*a339); + a755=(a338*a755); + a753=(a753+a755); + a27=(a27+a753); + a755=(a759*a341); + a755=(a340*a755); + a683=(a759*a345); + a683=(a344*a683); + a755=(a755+a683); + a27=(a27+a755); + a683=(a268*a102); + a741=(a262*a737); + a683=(a683+a741); + a683=(a307*a683); + a27=(a27-a683); + a737=(a254*a737); + a683=(a268*a48); + a737=(a737-a683); + a737=(a265*a737); + a27=(a27+a737); + a737=(a254*a736); + a48=(a261*a48); + a737=(a737-a48); + a737=(a251*a737); + a27=(a27-a737); + a102=(a261*a102); + a736=(a262*a736); + a102=(a102+a736); + a102=(a272*a102); + a27=(a27-a102); + if (res[0]!=0) res[0][69]=a27; + a27=2.4576000000000003e-04; + a102=(a27*a371); + a102=(a349*a102); + a736=(a27*a346); + a737=(a353*a736); + a48=(a27*a369); + a683=(a354*a48); + a737=(a737+a683); + a683=(a737/a366); + a737=(a356*a737); + a741=(a355*a736); + a760=(a359*a48); + a741=(a741-a760); + a760=(a360*a741); + a737=(a737+a760); + a760=(a326*a737); + a683=(a683-a760); + a683=(a342*a683); + a760=(a359*a683); + a741=(a741/a366); + a737=(a365*a737); + a741=(a741-a737); + a741=(a342*a741); + a737=(a354*a741); + a760=(a760+a737); + a760=(a370*a760); + a102=(a102+a760); + a760=(a353*a741); + a737=(a355*a683); + a760=(a760-a737); + a760=(a373*a760); + a737=(a27*a315); + a737=(a374*a737); + a760=(a760-a737); + a102=(a102+a760); + a760=2.4576000000000009e-04; + a737=(a760*a378); + a737=(a377*a737); + a703=(a760*a381); + a703=(a380*a703); + a737=(a737+a703); + a703=(a102+a737); + a750=1.2288000000000004e-04; + a744=(a750*a383); + a744=(a382*a744); + a761=(a750*a387); + a761=(a386*a761); + a744=(a744+a761); + a703=(a703+a744); + a761=(a750*a389); + a761=(a388*a761); + a738=(a750*a393); + a738=(a392*a738); + a761=(a761+a738); + a703=(a703+a761); + a738=(a318*a25); + a757=(a312*a749); + a738=(a738+a757); + a738=(a390*a738); + a703=(a703-a738); + a749=(a304*a749); + a738=(a318*a22); + a749=(a749-a738); + a749=(a394*a749); + a703=(a703+a749); + a749=(a304*a739); + a22=(a311*a22); + a749=(a749-a22); + a749=(a351*a749); + a703=(a703-a749); + a25=(a311*a25); + a739=(a312*a739); + a25=(a25+a739); + a25=(a395*a25); + a703=(a703-a25); + if (res[0]!=0) res[0][70]=a703; + a703=9.8304000000000040e-05; + a25=(a703*a421); + a25=(a399*a25); + a739=(a703*a397); + a749=(a372*a739); + a22=(a703*a419); + a738=(a404*a22); + a749=(a749+a738); + a738=(a749/a416); + a749=(a406*a749); + a757=(a405*a739); + a704=(a409*a22); + a757=(a757-a704); + a704=(a410*a757); + a749=(a749+a704); + a704=(a396*a749); + a738=(a738-a704); + a738=(a398*a738); + a704=(a409*a738); + a757=(a757/a416); + a749=(a415*a749); + a757=(a757-a749); + a757=(a398*a757); + a749=(a404*a757); + a704=(a704+a749); + a704=(a420*a704); + a25=(a25+a704); + a704=(a372*a757); + a749=(a405*a738); + a704=(a704-a749); + a704=(a423*a704); + a749=(a703*a367); + a749=(a418*a749); + a704=(a704-a749); + a25=(a25+a704); + a704=(a703*a426); + a704=(a425*a704); + a749=(a703*a429); + a749=(a428*a749); + a704=(a704+a749); + a749=(a25+a704); + a747=4.9152000000000020e-05; + a758=(a747*a431); + a758=(a430*a758); + a740=(a747*a435); + a740=(a434*a740); + a758=(a758+a740); + a749=(a749+a758); + a740=(a747*a437); + a740=(a436*a740); + a762=(a747*a441); + a762=(a440*a762); + a740=(a740+a762); + a749=(a749+a740); + a762=(a364*a736); + a763=(a358*a683); + a762=(a762+a763); + a762=(a403*a762); + a749=(a749-a762); + a683=(a350*a683); + a762=(a364*a48); + a683=(a683-a762); + a683=(a361*a683); + a749=(a749+a683); + a683=(a350*a741); + a48=(a357*a48); + a683=(a683-a48); + a683=(a347*a683); + a749=(a749-a683); + a736=(a357*a736); + a741=(a358*a741); + a736=(a736+a741); + a736=(a368*a736); + a749=(a749-a736); + if (res[0]!=0) res[0][71]=a749; + a749=3.9321600000000013e-05; + a736=(a749*a467); + a736=(a445*a736); + a741=(a749*a442); + a683=(a449*a741); + a48=(a749*a465); + a762=(a450*a48); + a683=(a683+a762); + a762=(a683/a462); + a683=(a452*a683); + a763=(a451*a741); + a764=(a455*a48); + a763=(a763-a764); + a764=(a456*a763); + a683=(a683+a764); + a764=(a422*a683); + a762=(a762-a764); + a762=(a438*a762); + a764=(a455*a762); + a763=(a763/a462); + a683=(a461*a683); + a763=(a763-a683); + a763=(a438*a763); + a683=(a450*a763); + a764=(a764+a683); + a764=(a466*a764); + a736=(a736+a764); + a764=(a449*a763); + a683=(a451*a762); + a764=(a764-a683); + a764=(a469*a764); + a683=(a749*a411); + a683=(a470*a683); + a764=(a764-a683); + a736=(a736+a764); + a764=3.9321600000000020e-05; + a683=(a764*a474); + a683=(a473*a683); + a765=(a764*a477); + a765=(a476*a765); + a683=(a683+a765); + a765=(a736+a683); + a766=1.9660800000000010e-05; + a767=(a766*a479); + a767=(a478*a767); + a768=(a766*a483); + a768=(a482*a768); + a767=(a767+a768); + a765=(a765+a767); + a768=(a766*a485); + a768=(a484*a768); + a769=(a766*a489); + a769=(a488*a769); + a768=(a768+a769); + a765=(a765+a768); + a769=(a414*a739); + a770=(a408*a738); + a769=(a769+a770); + a769=(a486*a769); + a765=(a765-a769); + a738=(a400*a738); + a769=(a414*a22); + a738=(a738-a769); + a738=(a490*a738); + a765=(a765+a738); + a738=(a400*a757); + a22=(a407*a22); + a738=(a738-a22); + a738=(a447*a738); + a765=(a765-a738); + a739=(a407*a739); + a757=(a408*a757); + a739=(a739+a757); + a739=(a491*a739); + a765=(a765-a739); + if (res[0]!=0) res[0][72]=a765; + a765=1.5728640000000010e-05; + a739=(a765*a517); + a739=(a495*a739); + a757=(a765*a493); + a738=(a468*a757); + a22=(a765*a515); + a769=(a500*a22); + a738=(a738+a769); + a769=(a738/a512); + a738=(a502*a738); + a770=(a501*a757); + a771=(a505*a22); + a770=(a770-a771); + a771=(a506*a770); + a738=(a738+a771); + a771=(a492*a738); + a769=(a769-a771); + a769=(a494*a769); + a771=(a505*a769); + a770=(a770/a512); + a738=(a511*a738); + a770=(a770-a738); + a770=(a494*a770); + a738=(a500*a770); + a771=(a771+a738); + a771=(a516*a771); + a739=(a739+a771); + a771=(a468*a770); + a738=(a501*a769); + a771=(a771-a738); + a771=(a519*a771); + a738=(a765*a463); + a738=(a514*a738); + a771=(a771-a738); + a739=(a739+a771); + a771=(a765*a522); + a771=(a521*a771); + a738=(a765*a525); + a738=(a524*a738); + a771=(a771+a738); + a738=(a739+a771); + a772=7.8643200000000050e-06; + a773=(a772*a527); + a773=(a526*a773); + a774=(a772*a531); + a774=(a530*a774); + a773=(a773+a774); + a738=(a738+a773); + a774=(a772*a533); + a774=(a532*a774); + a775=(a772*a537); + a775=(a536*a775); + a774=(a774+a775); + a738=(a738+a774); + a775=(a460*a741); + a776=(a454*a762); + a775=(a775+a776); + a775=(a499*a775); + a738=(a738-a775); + a762=(a446*a762); + a775=(a460*a48); + a762=(a762-a775); + a762=(a457*a762); + a738=(a738+a762); + a762=(a446*a763); + a48=(a453*a48); + a762=(a762-a48); + a762=(a443*a762); + a738=(a738-a762); + a741=(a453*a741); + a763=(a454*a763); + a741=(a741+a763); + a741=(a464*a741); + a738=(a738-a741); + if (res[0]!=0) res[0][73]=a738; + a738=6.2914560000000037e-06; + a741=(a738*a563); + a741=(a541*a741); + a763=(a738*a538); + a762=(a545*a763); + a48=(a738*a561); + a775=(a546*a48); + a762=(a762+a775); + a775=(a762/a558); + a762=(a548*a762); + a776=(a547*a763); + a777=(a551*a48); + a776=(a776-a777); + a777=(a552*a776); + a762=(a762+a777); + a777=(a518*a762); + a775=(a775-a777); + a775=(a534*a775); + a777=(a551*a775); + a776=(a776/a558); + a762=(a557*a762); + a776=(a776-a762); + a776=(a534*a776); + a762=(a546*a776); + a777=(a777+a762); + a777=(a562*a777); + a741=(a741+a777); + a777=(a545*a776); + a762=(a547*a775); + a777=(a777-a762); + a777=(a565*a777); + a762=(a738*a507); + a762=(a560*a762); + a777=(a777-a762); + a741=(a741+a777); + a777=6.2914560000000045e-06; + a762=(a777*a568); + a762=(a567*a762); + a778=(a777*a571); + a778=(a570*a778); + a762=(a762+a778); + a778=(a741+a762); + a779=3.1457280000000023e-06; + a780=(a779*a573); + a780=(a572*a780); + a781=(a779*a577); + a781=(a576*a781); + a780=(a780+a781); + a778=(a778+a780); + a781=(a779*a579); + a781=(a578*a781); + a782=(a779*a583); + a782=(a582*a782); + a781=(a781+a782); + a778=(a778+a781); + a782=(a510*a757); + a783=(a504*a769); + a782=(a782+a783); + a782=(a580*a782); + a778=(a778-a782); + a769=(a496*a769); + a782=(a510*a22); + a769=(a769-a782); + a769=(a584*a769); + a778=(a778+a769); + a769=(a496*a770); + a22=(a503*a22); + a769=(a769-a22); + a769=(a564*a769); + a778=(a778-a769); + a757=(a503*a757); + a770=(a504*a770); + a757=(a757+a770); + a757=(a585*a757); + a778=(a778-a757); + if (res[0]!=0) res[0][74]=a778; + a778=2.5165824000000020e-06; + a757=(a778*a611); + a757=(a589*a757); + a770=(a778*a587); + a769=(a593*a770); + a22=(a778*a609); + a782=(a594*a22); + a769=(a769+a782); + a782=(a769/a606); + a769=(a596*a769); + a783=(a595*a770); + a784=(a599*a22); + a783=(a783-a784); + a784=(a600*a783); + a769=(a769+a784); + a784=(a586*a769); + a782=(a782-a784); + a782=(a588*a782); + a784=(a599*a782); + a783=(a783/a606); + a769=(a605*a769); + a783=(a783-a769); + a783=(a588*a783); + a769=(a594*a783); + a784=(a784+a769); + a784=(a610*a784); + a757=(a757+a784); + a784=(a593*a783); + a769=(a595*a782); + a784=(a784-a769); + a784=(a613*a784); + a769=(a778*a553); + a769=(a614*a769); + a784=(a784-a769); + a757=(a757+a784); + a784=(a778*a618); + a784=(a617*a784); + a769=(a778*a621); + a769=(a620*a769); + a784=(a784+a769); + a769=(a757+a784); + a785=1.2582912000000010e-06; + a786=(a785*a623); + a786=(a622*a786); + a787=(a785*a627); + a787=(a626*a787); + a786=(a786+a787); + a769=(a769+a786); + a787=(a785*a629); + a787=(a628*a787); + a788=(a785*a633); + a788=(a632*a788); + a787=(a787+a788); + a769=(a769+a787); + a788=(a556*a763); + a789=(a550*a775); + a788=(a788+a789); + a788=(a630*a788); + a769=(a769-a788); + a775=(a542*a775); + a788=(a556*a48); + a775=(a775-a788); + a775=(a634*a775); + a769=(a769+a775); + a775=(a542*a776); + a48=(a549*a48); + a775=(a775-a48); + a775=(a591*a775); + a769=(a769-a775); + a763=(a549*a763); + a776=(a550*a776); + a763=(a763+a776); + a763=(a635*a763); + a769=(a769-a763); + if (res[0]!=0) res[0][75]=a769; + a769=1.0066329600000008e-06; + a763=(a769*a661); + a763=(a639*a763); + a776=(a769*a637); + a775=(a612*a776); + a48=(a769*a659); + a788=(a644*a48); + a775=(a775+a788); + a788=(a775/a656); + a775=(a646*a775); + a789=(a645*a776); + a790=(a649*a48); + a789=(a789-a790); + a790=(a650*a789); + a775=(a775+a790); + a790=(a636*a775); + a788=(a788-a790); + a788=(a638*a788); + a790=(a649*a788); + a789=(a789/a656); + a775=(a655*a775); + a789=(a789-a775); + a789=(a638*a789); + a775=(a644*a789); + a790=(a790+a775); + a790=(a660*a790); + a763=(a763+a790); + a790=(a612*a789); + a775=(a645*a788); + a790=(a790-a775); + a790=(a663*a790); + a775=(a769*a607); + a775=(a658*a775); + a790=(a790-a775); + a763=(a763+a790); + a790=(a769*a666); + a790=(a665*a790); + a775=(a769*a669); + a775=(a668*a775); + a790=(a790+a775); + a775=(a763+a790); + a791=5.0331648000000040e-07; + a792=(a791*a671); + a792=(a670*a792); + a793=(a791*a675); + a793=(a674*a793); + a792=(a792+a793); + a775=(a775+a792); + a793=(a791*a677); + a793=(a676*a793); + a794=(a791*a681); + a794=(a680*a794); + a793=(a793+a794); + a775=(a775+a793); + a794=(a604*a770); + a795=(a598*a782); + a794=(a794+a795); + a794=(a643*a794); + a775=(a775-a794); + a782=(a590*a782); + a794=(a604*a22); + a782=(a782-a794); + a782=(a601*a782); + a775=(a775+a782); + a782=(a590*a783); + a22=(a597*a22); + a782=(a782-a22); + a782=(a559*a782); + a775=(a775-a782); + a770=(a597*a770); + a783=(a598*a783); + a770=(a770+a783); + a770=(a608*a770); + a775=(a775-a770); + if (res[0]!=0) res[0][76]=a775; + a775=4.0265318400000030e-07; + a770=(a775*a693); + a770=(a700*a770); + a783=(a775*a706); + a782=(a688*a783); + a22=(a775*a687); + a794=(a692*a22); + a782=(a782+a794); + a794=(a782/a686); + a782=(a684*a782); + a795=(a696*a783); + a796=(a689*a22); + a795=(a795-a796); + a796=(a702*a795); + a782=(a782+a796); + a796=(a691*a782); + a794=(a794-a796); + a794=(a662*a794); + a796=(a689*a794); + a795=(a795/a686); + a782=(a685*a782); + a795=(a795-a782); + a795=(a662*a795); + a782=(a692*a795); + a796=(a796+a782); + a796=(a697*a796); + a770=(a770+a796); + a796=(a688*a795); + a782=(a696*a794); + a796=(a796-a782); + a796=(a708*a796); + a775=(a775*a699); + a775=(a709*a775); + a796=(a796-a775); + a770=(a770+a796); + a796=4.0265318400000035e-07; + a775=(a796*a714); + a775=(a713*a775); + a782=(a796*a717); + a782=(a716*a782); + a775=(a775+a782); + a782=(a770+a775); + a797=2.0132659200000017e-07; + a798=(a797*a720); + a798=(a719*a798); + a799=(a797*a724); + a799=(a723*a799); + a798=(a798+a799); + a782=(a782+a798); + a799=(a797*a727); + a799=(a726*a799); + a800=(a797*a731); + a800=(a730*a800); + a799=(a799+a800); + a782=(a782+a799); + a800=(a654*a776); + a801=(a648*a788); + a800=(a800+a801); + a800=(a732*a800); + a782=(a782-a800); + a788=(a640*a788); + a800=(a654*a48); + a788=(a788-a800); + a788=(a733*a788); + a782=(a782+a788); + a788=(a640*a789); + a48=(a647*a48); + a788=(a788-a48); + a788=(a682*a788); + a782=(a782-a788); + a776=(a647*a776); + a789=(a648*a789); + a776=(a776+a789); + a776=(a734*a776); + a782=(a782-a776); + if (res[0]!=0) res[0][77]=a782; + a782=(a690*a794); + a776=(a678*a22); + a782=(a782-a776); + a782=(a711*a782); + a776=(a678*a783); + a794=(a657*a794); + a776=(a776+a794); + a776=(a751*a776); + a782=(a782-a776); + a776=(a690*a795); + a22=(a701*a22); + a776=(a776-a22); + a776=(a742*a776); + a782=(a782-a776); + a783=(a701*a783); + a795=(a657*a795); + a783=(a783+a795); + a783=(a705*a783); + a782=(a782-a783); + if (res[0]!=0) res[0][78]=a782; + a782=cos(a13); + a782=(a28*a782); + a31=(a31*a782); + a782=cos(a11); + a782=(a42*a782); + a47=(a47*a782); + a31=(a31+a47); + a47=cos(a7); + a47=(a42*a47); + a53=(a53*a47); + a31=(a31+a53); + a13=sin(a13); + a13=(a28*a13); + a36=(a36*a13); + a31=(a31-a36); + a11=sin(a11); + a11=(a42*a11); + a40=(a40*a11); + a31=(a31-a40); + a7=sin(a7); + a7=(a42*a7); + a46=(a46*a7); + a31=(a31-a46); + a31=(-a31); + if (res[0]!=0) res[0][79]=a31; + a770=(a4*a770); + a31=(a5*a770); + a46=(a10*a770); + a775=(a38*a775); + a46=(a46+a775); + a775=(a5*a46); + a31=(a31+a775); + a775=(a10*a770); + a798=(a44*a798); + a775=(a775+a798); + a798=(a5*a775); + a31=(a31+a798); + a799=(a44*a799); + a799=(a770+a799); + a798=(a5*a799); + a31=(a31+a798); + a798=(a5*a31); + a763=(a4*a763); + a7=(a5*a763); + a798=(a798+a7); + a7=(a10*a763); + a790=(a38*a790); + a7=(a7+a790); + a790=(a5*a7); + a798=(a798+a790); + a790=(a10*a763); + a792=(a44*a792); + a790=(a790+a792); + a792=(a5*a790); + a798=(a798+a792); + a793=(a44*a793); + a793=(a763+a793); + a792=(a5*a793); + a798=(a798+a792); + a792=(a5*a798); + a757=(a4*a757); + a40=(a5*a757); + a792=(a792+a40); + a40=(a10*a757); + a784=(a38*a784); + a40=(a40+a784); + a784=(a5*a40); + a792=(a792+a784); + a784=(a10*a757); + a786=(a44*a786); + a784=(a784+a786); + a786=(a5*a784); + a792=(a792+a786); + a787=(a44*a787); + a787=(a757+a787); + a786=(a5*a787); + a792=(a792+a786); + a786=(a5*a792); + a741=(a4*a741); + a11=(a5*a741); + a786=(a786+a11); + a11=(a10*a741); + a762=(a38*a762); + a11=(a11+a762); + a762=(a5*a11); + a786=(a786+a762); + a762=(a10*a741); + a780=(a44*a780); + a762=(a762+a780); + a780=(a5*a762); + a786=(a786+a780); + a781=(a44*a781); + a781=(a741+a781); + a780=(a5*a781); + a786=(a786+a780); + a780=(a5*a786); + a739=(a4*a739); + a36=(a5*a739); + a780=(a780+a36); + a36=(a10*a739); + a771=(a38*a771); + a36=(a36+a771); + a771=(a5*a36); + a780=(a780+a771); + a771=(a10*a739); + a773=(a44*a773); + a771=(a771+a773); + a773=(a5*a771); + a780=(a780+a773); + a774=(a44*a774); + a774=(a739+a774); + a773=(a5*a774); + a780=(a780+a773); + a773=(a5*a780); + a736=(a4*a736); + a13=(a5*a736); + a773=(a773+a13); + a13=(a10*a736); + a683=(a38*a683); + a13=(a13+a683); + a683=(a5*a13); + a773=(a773+a683); + a683=(a10*a736); + a767=(a44*a767); + a683=(a683+a767); + a767=(a5*a683); + a773=(a773+a767); + a768=(a44*a768); + a768=(a736+a768); + a767=(a5*a768); + a773=(a773+a767); + a767=(a5*a773); + a25=(a4*a25); + a53=(a5*a25); + a767=(a767+a53); + a53=(a10*a25); + a704=(a38*a704); + a53=(a53+a704); + a704=(a5*a53); + a767=(a767+a704); + a704=(a10*a25); + a758=(a44*a758); + a704=(a704+a758); + a758=(a5*a704); + a767=(a767+a758); + a740=(a44*a740); + a740=(a25+a740); + a758=(a5*a740); + a767=(a767+a758); + a758=(a5*a767); + a102=(a4*a102); + a47=(a5*a102); + a758=(a758+a47); + a47=(a10*a102); + a737=(a38*a737); + a47=(a47+a737); + a737=(a5*a47); + a758=(a758+a737); + a737=(a10*a102); + a744=(a44*a744); + a737=(a737+a744); + a744=(a5*a737); + a758=(a758+a744); + a761=(a44*a761); + a761=(a102+a761); + a744=(a5*a761); + a758=(a758+a744); + a744=(a5*a758); + a81=(a4*a81); + a782=(a5*a81); + a744=(a744+a782); + a782=(a10*a81); + a752=(a38*a752); + a782=(a782+a752); + a752=(a5*a782); + a744=(a744+a752); + a752=(a10*a81); + a753=(a44*a753); + a752=(a752+a753); + a753=(a5*a752); + a744=(a744+a753); + a755=(a44*a755); + a755=(a81+a755); + a753=(a5*a755); + a744=(a744+a753); + a753=(a5*a744); + a45=(a4*a45); + a783=(a5*a45); + a753=(a753+a783); + a783=(a10*a45); + a745=(a38*a745); + a783=(a783+a745); + a745=(a5*a783); + a753=(a753+a745); + a745=(a10*a45); + a746=(a44*a746); + a745=(a745+a746); + a746=(a5*a745); + a753=(a753+a746); + a735=(a44*a735); + a735=(a45+a735); + a746=(a5*a735); + a753=(a753+a746); + a746=(a5*a753); + a26=(a4*a26); + a795=(a5*a26); + a746=(a746+a795); + a795=(a10*a26); + a30=(a38*a30); + a795=(a795+a30); + a30=(a5*a795); + a746=(a746+a30); + a30=(a10*a26); + a19=(a44*a19); + a30=(a30+a19); + a19=(a5*a30); + a746=(a746+a19); + a756=(a44*a756); + a756=(a26+a756); + a19=(a5*a756); + a746=(a746+a19); + a19=(a5*a746); + a55=(a4*a55); + a776=(a5*a55); + a19=(a19+a776); + a776=(a10*a55); + a49=(a38*a49); + a776=(a776+a49); + a49=(a5*a776); + a19=(a19+a49); + a49=(a10*a55); + a1=(a44*a1); + a49=(a49+a1); + a1=(a5*a49); + a19=(a19+a1); + a16=(a44*a16); + a16=(a55+a16); + a1=(a5*a16); + a19=(a19+a1); + a1=(a5*a19); + a107=(a4*a107); + a22=(a5*a107); + a1=(a1+a22); + a22=(a10*a107); + a17=(a38*a17); + a22=(a22+a17); + a17=(a5*a22); + a1=(a1+a17); + a17=(a10*a107); + a106=(a44*a106); + a17=(a17+a106); + a106=(a5*a17); + a1=(a1+a106); + a695=(a44*a695); + a695=(a107+a695); + a106=(a5*a695); + a1=(a1+a106); + a1=(a5*a1); + a54=(a4*a54); + a106=(a5*a54); + a1=(a1+a106); + a106=(a10*a54); + a39=(a38*a39); + a106=(a106+a39); + a106=(a5*a106); + a1=(a1+a106); + a106=(a10*a54); + a33=(a44*a33); + a106=(a106+a33); + a106=(a5*a106); + a1=(a1+a106); + a12=(a44*a12); + a54=(a54+a12); + a54=(a5*a54); + a1=(a1+a54); + a1=(a8*a1); + a24=(a4*a24); + a54=(a8*a24); + a1=(a1+a54); + a54=(a10*a24); + a37=(a38*a37); + a54=(a54+a37); + a54=(a8*a54); + a1=(a1+a54); + a54=(a10*a24); + a43=(a44*a43); + a54=(a54+a43); + a54=(a8*a54); + a1=(a1+a54); + a50=(a44*a50); + a24=(a24+a50); + a24=(a8*a24); + a1=(a1+a24); + a24=-5.0000000000000000e-01; + a50=(a24*a34); + a1=(a1-a50); + a50=5.0000000000000000e-01; + a54=(a50*a34); + a1=(a1+a54); + a54=(a50*a34); + a1=(a1+a54); + a54=(a50*a34); + a1=(a1+a54); + a54=(a10*a34); + a1=(a1+a54); + if (res[0]!=0) res[0][80]=a1; + a1=cos(a63); + a54=(a87*a1); + a43=cos(a58); + a37=(a97*a43); + a54=(a54+a37); + a37=cos(a6); + a12=(a103*a37); + a54=(a54+a12); + a12=cos(a0); + a12=(a87*a12); + a54=(a54+a12); + a63=sin(a63); + a12=(a88*a63); + a54=(a54-a12); + a58=sin(a58); + a12=(a91*a58); + a54=(a54-a12); + a6=sin(a6); + a12=(a96*a6); + a54=(a54-a12); + a0=sin(a0); + a0=(a88*a0); + a54=(a54-a0); + a54=(-a54); + if (res[0]!=0) res[0][81]=a54; + a754=(a754-a743); + if (res[0]!=0) res[0][82]=a754; + a754=(a41*a1); + a754=(a87*a754); + a743=(a35*a43); + a743=(a97*a743); + a754=(a754+a743); + a743=(a35*a37); + a743=(a103*a743); + a754=(a754+a743); + a743=(a41*a63); + a743=(a88*a743); + a754=(a754-a743); + a743=(a35*a58); + a743=(a91*a743); + a754=(a754-a743); + a743=(a35*a6); + a743=(a96*a743); + a754=(a754-a743); + a754=(-a754); + if (res[0]!=0) res[0][83]=a754; + a754=(a10*a34); + a743=(a3*a34); + a54=(a10*a34); + a743=(a743-a54); + a754=(a754-a743); + a743=(a10*a34); + a754=(a754+a743); + a743=(a10*a34); + a754=(a754+a743); + if (res[0]!=0) res[0][84]=a754; + a77=(a28*a77); + a74=(a74*a77); + a80=(a28*a80); + a77=(a57*a80); + a82=(a28*a82); + a754=(a66*a82); + a77=(a77+a754); + a754=(a77/a68); + a65=(a65*a77); + a77=(a70*a80); + a743=(a59*a82); + a77=(a77-a743); + a69=(a69*a77); + a65=(a65+a69); + a71=(a71*a65); + a754=(a754-a71); + a754=(a72*a754); + a59=(a59*a754); + a77=(a77/a68); + a75=(a75*a65); + a77=(a77-a75); + a72=(a72*a77); + a66=(a66*a72); + a59=(a59+a66); + a79=(a79*a59); + a74=(a74+a79); + a57=(a57*a72); + a70=(a70*a754); + a57=(a57-a70); + a85=(a85*a57); + a52=(a28*a52); + a86=(a86*a52); + a85=(a85-a86); + a74=(a74+a85); + a90=(a28*a90); + a89=(a89*a90); + a93=(a28*a93); + a92=(a92*a93); + a89=(a89+a92); + a92=(a74+a89); + a95=(a42*a95); + a94=(a94*a95); + a99=(a42*a99); + a98=(a98*a99); + a94=(a94+a98); + a92=(a92+a94); + a101=(a42*a101); + a100=(a100*a101); + a105=(a42*a105); + a104=(a104*a105); + a100=(a100+a104); + a92=(a92+a100); + if (res[0]!=0) res[0][85]=a92; + a92=(a41*a133); + a92=(a111*a92); + a104=(a41*a109); + a105=(a84*a104); + a101=(a41*a131); + a98=(a116*a101); + a105=(a105+a98); + a98=(a105/a128); + a105=(a118*a105); + a99=(a117*a104); + a95=(a121*a101); + a99=(a99-a95); + a95=(a122*a99); + a105=(a105+a95); + a95=(a108*a105); + a98=(a98-a95); + a98=(a110*a98); + a95=(a121*a98); + a99=(a99/a128); + a105=(a127*a105); + a99=(a99-a105); + a99=(a110*a99); + a105=(a116*a99); + a95=(a95+a105); + a95=(a132*a95); + a92=(a92+a95); + a95=(a84*a99); + a105=(a117*a98); + a95=(a95-a105); + a95=(a135*a95); + a105=(a41*a60); + a105=(a130*a105); + a95=(a95-a105); + a92=(a92+a95); + a95=(a41*a138); + a95=(a137*a95); + a105=(a41*a141); + a105=(a140*a105); + a95=(a95+a105); + a105=(a92+a95); + a93=(a35*a143); + a93=(a142*a93); + a90=(a35*a147); + a90=(a146*a90); + a93=(a93+a90); + a105=(a105+a93); + a90=(a35*a149); + a90=(a148*a90); + a85=(a35*a153); + a85=(a152*a85); + a90=(a90+a85); + a105=(a105+a90); + a85=(a73*a80); + a86=(a64*a754); + a85=(a85+a86); + a115=(a115*a85); + a105=(a105-a115); + a754=(a67*a754); + a73=(a73*a82); + a754=(a754-a73); + a78=(a78*a754); + a105=(a105+a78); + a67=(a67*a72); + a82=(a76*a82); + a67=(a67-a82); + a62=(a62*a67); + a105=(a105-a62); + a76=(a76*a80); + a64=(a64*a72); + a76=(a76+a64); + a83=(a83*a76); + a105=(a105-a83); + if (res[0]!=0) res[0][86]=a105; + a105=(a51*a179); + a105=(a157*a105); + a83=(a51*a154); + a76=(a161*a83); + a64=(a51*a177); + a72=(a162*a64); + a76=(a76+a72); + a72=(a76/a174); + a76=(a164*a76); + a80=(a163*a83); + a62=(a167*a64); + a80=(a80-a62); + a62=(a168*a80); + a76=(a76+a62); + a62=(a134*a76); + a72=(a72-a62); + a72=(a150*a72); + a62=(a167*a72); + a80=(a80/a174); + a76=(a173*a76); + a80=(a80-a76); + a80=(a150*a80); + a76=(a162*a80); + a62=(a62+a76); + a62=(a178*a62); + a105=(a105+a62); + a62=(a161*a80); + a76=(a163*a72); + a62=(a62-a76); + a62=(a181*a62); + a76=(a51*a123); + a76=(a182*a76); + a62=(a62-a76); + a105=(a105+a62); + a62=(a51*a186); + a62=(a185*a62); + a76=(a51*a189); + a76=(a188*a76); + a62=(a62+a76); + a76=(a105+a62); + a67=(a32*a191); + a67=(a190*a67); + a82=(a32*a195); + a82=(a194*a82); + a67=(a67+a82); + a76=(a76+a67); + a82=(a32*a197); + a82=(a196*a82); + a78=(a32*a201); + a78=(a200*a78); + a82=(a82+a78); + a76=(a76+a82); + a78=(a126*a104); + a754=(a120*a98); + a78=(a78+a754); + a78=(a198*a78); + a76=(a76-a78); + a98=(a112*a98); + a78=(a126*a101); + a98=(a98-a78); + a98=(a202*a98); + a76=(a76+a98); + a98=(a112*a99); + a101=(a119*a101); + a98=(a98-a101); + a98=(a159*a98); + a76=(a76-a98); + a104=(a119*a104); + a99=(a120*a99); + a104=(a104+a99); + a104=(a203*a104); + a76=(a76-a104); + if (res[0]!=0) res[0][87]=a76; + a76=(a14*a229); + a76=(a207*a76); + a104=(a14*a205); + a99=(a180*a104); + a98=(a14*a227); + a101=(a212*a98); + a99=(a99+a101); + a101=(a99/a224); + a99=(a214*a99); + a78=(a213*a104); + a754=(a217*a98); + a78=(a78-a754); + a754=(a218*a78); + a99=(a99+a754); + a754=(a204*a99); + a101=(a101-a754); + a101=(a206*a101); + a754=(a217*a101); + a78=(a78/a224); + a99=(a223*a99); + a78=(a78-a99); + a78=(a206*a78); + a99=(a212*a78); + a754=(a754+a99); + a754=(a228*a754); + a76=(a76+a754); + a754=(a180*a78); + a99=(a213*a101); + a754=(a754-a99); + a754=(a231*a754); + a99=(a14*a175); + a99=(a226*a99); + a754=(a754-a99); + a76=(a76+a754); + a754=(a2*a234); + a754=(a233*a754); + a99=(a2*a237); + a99=(a236*a99); + a754=(a754+a99); + a99=(a76+a754); + a73=(a29*a239); + a73=(a238*a73); + a115=(a29*a243); + a115=(a242*a115); + a73=(a73+a115); + a99=(a99+a73); + a115=(a29*a245); + a115=(a244*a115); + a85=(a29*a249); + a85=(a248*a85); + a115=(a115+a85); + a99=(a99+a115); + a85=(a172*a83); + a86=(a166*a72); + a85=(a85+a86); + a85=(a211*a85); + a99=(a99-a85); + a72=(a158*a72); + a85=(a172*a64); + a72=(a72-a85); + a72=(a169*a72); + a99=(a99+a72); + a72=(a158*a80); + a64=(a165*a64); + a72=(a72-a64); + a72=(a155*a72); + a99=(a99-a72); + a83=(a165*a83); + a80=(a166*a80); + a83=(a83+a80); + a83=(a176*a83); + a99=(a99-a83); + if (res[0]!=0) res[0][88]=a99; + a99=(a20*a275); + a99=(a253*a99); + a83=(a20*a250); + a80=(a257*a83); + a72=(a20*a273); + a64=(a258*a72); + a80=(a80+a64); + a64=(a80/a270); + a80=(a260*a80); + a85=(a259*a83); + a86=(a263*a72); + a85=(a85-a86); + a86=(a264*a85); + a80=(a80+a86); + a86=(a230*a80); + a64=(a64-a86); + a64=(a246*a64); + a86=(a263*a64); + a85=(a85/a270); + a80=(a269*a80); + a85=(a85-a80); + a85=(a246*a85); + a80=(a258*a85); + a86=(a86+a80); + a86=(a274*a86); + a99=(a99+a86); + a86=(a257*a85); + a80=(a259*a64); + a86=(a86-a80); + a86=(a277*a86); + a80=(a20*a219); + a80=(a278*a80); + a86=(a86-a80); + a99=(a99+a86); + a86=(a15*a282); + a86=(a281*a86); + a80=(a15*a285); + a80=(a284*a80); + a86=(a86+a80); + a80=(a99+a86); + a52=(a21*a287); + a52=(a286*a52); + a57=(a21*a291); + a57=(a290*a57); + a52=(a52+a57); + a80=(a80+a52); + a57=(a21*a293); + a57=(a292*a57); + a70=(a21*a297); + a70=(a296*a70); + a57=(a57+a70); + a80=(a80+a57); + a70=(a222*a104); + a79=(a216*a101); + a70=(a70+a79); + a70=(a294*a70); + a80=(a80-a70); + a101=(a208*a101); + a70=(a222*a98); + a101=(a101-a70); + a101=(a298*a101); + a80=(a80+a101); + a101=(a208*a78); + a98=(a215*a98); + a101=(a101-a98); + a101=(a255*a101); + a80=(a80-a101); + a104=(a215*a104); + a78=(a216*a78); + a104=(a104+a78); + a104=(a299*a104); + a80=(a80-a104); + if (res[0]!=0) res[0][89]=a80; + a80=(a18*a325); + a80=(a303*a80); + a104=(a18*a301); + a78=(a276*a104); + a101=(a18*a323); + a98=(a308*a101); + a78=(a78+a98); + a98=(a78/a320); + a78=(a310*a78); + a70=(a309*a104); + a79=(a313*a101); + a70=(a70-a79); + a79=(a314*a70); + a78=(a78+a79); + a79=(a300*a78); + a98=(a98-a79); + a98=(a302*a98); + a79=(a313*a98); + a70=(a70/a320); + a78=(a319*a78); + a70=(a70-a78); + a70=(a302*a70); + a78=(a308*a70); + a79=(a79+a78); + a79=(a324*a79); + a80=(a80+a79); + a79=(a276*a70); + a78=(a309*a98); + a79=(a79-a78); + a79=(a327*a79); + a78=(a18*a271); + a78=(a322*a78); + a79=(a79-a78); + a80=(a80+a79); + a79=(a18*a330); + a79=(a329*a79); + a78=(a18*a333); + a78=(a332*a78); + a79=(a79+a78); + a78=(a80+a79); + a59=(a748*a335); + a59=(a334*a59); + a66=(a748*a339); + a66=(a338*a66); + a59=(a59+a66); + a78=(a78+a59); + a66=(a748*a341); + a66=(a340*a66); + a77=(a748*a345); + a77=(a344*a77); + a66=(a66+a77); + a78=(a78+a66); + a77=(a268*a83); + a75=(a262*a64); + a77=(a77+a75); + a77=(a307*a77); + a78=(a78-a77); + a64=(a254*a64); + a77=(a268*a72); + a64=(a64-a77); + a64=(a265*a64); + a78=(a78+a64); + a64=(a254*a85); + a72=(a261*a72); + a64=(a64-a72); + a64=(a251*a64); + a78=(a78-a64); + a83=(a261*a83); + a85=(a262*a85); + a83=(a83+a85); + a83=(a272*a83); + a78=(a78-a83); + if (res[0]!=0) res[0][90]=a78; + a78=(a23*a371); + a78=(a349*a78); + a83=(a23*a346); + a85=(a353*a83); + a64=(a23*a369); + a72=(a354*a64); + a85=(a85+a72); + a72=(a85/a366); + a85=(a356*a85); + a77=(a355*a83); + a75=(a359*a64); + a77=(a77-a75); + a75=(a360*a77); + a85=(a85+a75); + a75=(a326*a85); + a72=(a72-a75); + a72=(a342*a72); + a75=(a359*a72); + a77=(a77/a366); + a85=(a365*a85); + a77=(a77-a85); + a77=(a342*a77); + a85=(a354*a77); + a75=(a75+a85); + a75=(a370*a75); + a78=(a78+a75); + a75=(a353*a77); + a85=(a355*a72); + a75=(a75-a85); + a75=(a373*a75); + a85=(a23*a315); + a85=(a374*a85); + a75=(a75-a85); + a78=(a78+a75); + a75=(a23*a378); + a75=(a377*a75); + a85=(a23*a381); + a85=(a380*a85); + a75=(a75+a85); + a85=(a78+a75); + a65=(a759*a383); + a65=(a382*a65); + a68=(a759*a387); + a68=(a386*a68); + a65=(a65+a68); + a85=(a85+a65); + a68=(a759*a389); + a68=(a388*a68); + a71=(a759*a393); + a71=(a392*a71); + a68=(a68+a71); + a85=(a85+a68); + a71=(a318*a104); + a69=(a312*a98); + a71=(a71+a69); + a71=(a390*a71); + a85=(a85-a71); + a98=(a304*a98); + a71=(a318*a101); + a98=(a98-a71); + a98=(a394*a98); + a85=(a85+a98); + a98=(a304*a70); + a101=(a311*a101); + a98=(a98-a101); + a98=(a351*a98); + a85=(a85-a98); + a104=(a311*a104); + a70=(a312*a70); + a104=(a104+a70); + a104=(a395*a104); + a85=(a85-a104); + if (res[0]!=0) res[0][91]=a85; + a85=(a27*a421); + a85=(a399*a85); + a104=(a27*a397); + a70=(a372*a104); + a98=(a27*a419); + a101=(a404*a98); + a70=(a70+a101); + a101=(a70/a416); + a70=(a406*a70); + a71=(a405*a104); + a69=(a409*a98); + a71=(a71-a69); + a69=(a410*a71); + a70=(a70+a69); + a69=(a396*a70); + a101=(a101-a69); + a101=(a398*a101); + a69=(a409*a101); + a71=(a71/a416); + a70=(a415*a70); + a71=(a71-a70); + a71=(a398*a71); + a70=(a404*a71); + a69=(a69+a70); + a69=(a420*a69); + a85=(a85+a69); + a69=(a372*a71); + a70=(a405*a101); + a69=(a69-a70); + a69=(a423*a69); + a70=(a27*a367); + a70=(a418*a70); + a69=(a69-a70); + a85=(a85+a69); + a69=(a760*a426); + a69=(a425*a69); + a70=(a760*a429); + a70=(a428*a70); + a69=(a69+a70); + a70=(a85+a69); + a743=(a750*a431); + a743=(a430*a743); + a54=(a750*a435); + a54=(a434*a54); + a743=(a743+a54); + a70=(a70+a743); + a54=(a750*a437); + a54=(a436*a54); + a0=(a750*a441); + a0=(a440*a0); + a54=(a54+a0); + a70=(a70+a54); + a0=(a364*a83); + a12=(a358*a72); + a0=(a0+a12); + a0=(a403*a0); + a70=(a70-a0); + a72=(a350*a72); + a0=(a364*a64); + a72=(a72-a0); + a72=(a361*a72); + a70=(a70+a72); + a72=(a350*a77); + a64=(a357*a64); + a72=(a72-a64); + a72=(a347*a72); + a70=(a70-a72); + a83=(a357*a83); + a77=(a358*a77); + a83=(a83+a77); + a83=(a368*a83); + a70=(a70-a83); + if (res[0]!=0) res[0][92]=a70; + a70=(a703*a467); + a70=(a445*a70); + a83=(a703*a442); + a77=(a449*a83); + a72=(a703*a465); + a64=(a450*a72); + a77=(a77+a64); + a64=(a77/a462); + a77=(a452*a77); + a0=(a451*a83); + a12=(a455*a72); + a0=(a0-a12); + a12=(a456*a0); + a77=(a77+a12); + a12=(a422*a77); + a64=(a64-a12); + a64=(a438*a64); + a12=(a455*a64); + a0=(a0/a462); + a77=(a461*a77); + a0=(a0-a77); + a0=(a438*a0); + a77=(a450*a0); + a12=(a12+a77); + a12=(a466*a12); + a70=(a70+a12); + a12=(a449*a0); + a77=(a451*a64); + a12=(a12-a77); + a12=(a469*a12); + a77=(a703*a411); + a77=(a470*a77); + a12=(a12-a77); + a70=(a70+a12); + a12=(a703*a474); + a12=(a473*a12); + a77=(a703*a477); + a77=(a476*a77); + a12=(a12+a77); + a77=(a70+a12); + a106=(a747*a479); + a106=(a478*a106); + a33=(a747*a483); + a33=(a482*a33); + a106=(a106+a33); + a77=(a77+a106); + a33=(a747*a485); + a33=(a484*a33); + a39=(a747*a489); + a39=(a488*a39); + a33=(a33+a39); + a77=(a77+a33); + a39=(a414*a104); + a794=(a408*a101); + a39=(a39+a794); + a39=(a486*a39); + a77=(a77-a39); + a101=(a400*a101); + a39=(a414*a98); + a101=(a101-a39); + a101=(a490*a101); + a77=(a77+a101); + a101=(a400*a71); + a98=(a407*a98); + a101=(a101-a98); + a101=(a447*a101); + a77=(a77-a101); + a104=(a407*a104); + a71=(a408*a71); + a104=(a104+a71); + a104=(a491*a104); + a77=(a77-a104); + if (res[0]!=0) res[0][93]=a77; + a77=(a749*a517); + a77=(a495*a77); + a104=(a749*a493); + a71=(a468*a104); + a101=(a749*a515); + a98=(a500*a101); + a71=(a71+a98); + a98=(a71/a512); + a71=(a502*a71); + a39=(a501*a104); + a794=(a505*a101); + a39=(a39-a794); + a794=(a506*a39); + a71=(a71+a794); + a794=(a492*a71); + a98=(a98-a794); + a98=(a494*a98); + a794=(a505*a98); + a39=(a39/a512); + a71=(a511*a71); + a39=(a39-a71); + a39=(a494*a39); + a71=(a500*a39); + a794=(a794+a71); + a794=(a516*a794); + a77=(a77+a794); + a794=(a468*a39); + a71=(a501*a98); + a794=(a794-a71); + a794=(a519*a794); + a71=(a749*a463); + a71=(a514*a71); + a794=(a794-a71); + a77=(a77+a794); + a794=(a764*a522); + a794=(a521*a794); + a71=(a764*a525); + a71=(a524*a71); + a794=(a794+a71); + a71=(a77+a794); + a789=(a766*a527); + a789=(a526*a789); + a788=(a766*a531); + a788=(a530*a788); + a789=(a789+a788); + a71=(a71+a789); + a788=(a766*a533); + a788=(a532*a788); + a48=(a766*a537); + a48=(a536*a48); + a788=(a788+a48); + a71=(a71+a788); + a48=(a460*a83); + a800=(a454*a64); + a48=(a48+a800); + a48=(a499*a48); + a71=(a71-a48); + a64=(a446*a64); + a48=(a460*a72); + a64=(a64-a48); + a64=(a457*a64); + a71=(a71+a64); + a64=(a446*a0); + a72=(a453*a72); + a64=(a64-a72); + a64=(a443*a64); + a71=(a71-a64); + a83=(a453*a83); + a0=(a454*a0); + a83=(a83+a0); + a83=(a464*a83); + a71=(a71-a83); + if (res[0]!=0) res[0][94]=a71; + a71=(a765*a563); + a71=(a541*a71); + a83=(a765*a538); + a0=(a545*a83); + a64=(a765*a561); + a72=(a546*a64); + a0=(a0+a72); + a72=(a0/a558); + a0=(a548*a0); + a48=(a547*a83); + a800=(a551*a64); + a48=(a48-a800); + a800=(a552*a48); + a0=(a0+a800); + a800=(a518*a0); + a72=(a72-a800); + a72=(a534*a72); + a800=(a551*a72); + a48=(a48/a558); + a0=(a557*a0); + a48=(a48-a0); + a48=(a534*a48); + a0=(a546*a48); + a800=(a800+a0); + a800=(a562*a800); + a71=(a71+a800); + a800=(a545*a48); + a0=(a547*a72); + a800=(a800-a0); + a800=(a565*a800); + a0=(a765*a507); + a0=(a560*a0); + a800=(a800-a0); + a71=(a71+a800); + a800=(a765*a568); + a800=(a567*a800); + a0=(a765*a571); + a0=(a570*a0); + a800=(a800+a0); + a0=(a71+a800); + a801=(a772*a573); + a801=(a572*a801); + a802=(a772*a577); + a802=(a576*a802); + a801=(a801+a802); + a0=(a0+a801); + a802=(a772*a579); + a802=(a578*a802); + a803=(a772*a583); + a803=(a582*a803); + a802=(a802+a803); + a0=(a0+a802); + a803=(a510*a104); + a804=(a504*a98); + a803=(a803+a804); + a803=(a580*a803); + a0=(a0-a803); + a98=(a496*a98); + a803=(a510*a101); + a98=(a98-a803); + a98=(a584*a98); + a0=(a0+a98); + a98=(a496*a39); + a101=(a503*a101); + a98=(a98-a101); + a98=(a564*a98); + a0=(a0-a98); + a104=(a503*a104); + a39=(a504*a39); + a104=(a104+a39); + a104=(a585*a104); + a0=(a0-a104); + if (res[0]!=0) res[0][95]=a0; + a0=(a738*a611); + a0=(a589*a0); + a104=(a738*a587); + a39=(a593*a104); + a98=(a738*a609); + a101=(a594*a98); + a39=(a39+a101); + a101=(a39/a606); + a39=(a596*a39); + a803=(a595*a104); + a804=(a599*a98); + a803=(a803-a804); + a804=(a600*a803); + a39=(a39+a804); + a804=(a586*a39); + a101=(a101-a804); + a101=(a588*a101); + a804=(a599*a101); + a803=(a803/a606); + a39=(a605*a39); + a803=(a803-a39); + a803=(a588*a803); + a39=(a594*a803); + a804=(a804+a39); + a804=(a610*a804); + a0=(a0+a804); + a804=(a593*a803); + a39=(a595*a101); + a804=(a804-a39); + a804=(a613*a804); + a39=(a738*a553); + a39=(a614*a39); + a804=(a804-a39); + a0=(a0+a804); + a804=(a777*a618); + a804=(a617*a804); + a39=(a777*a621); + a39=(a620*a39); + a804=(a804+a39); + a39=(a0+a804); + a805=(a779*a623); + a805=(a622*a805); + a806=(a779*a627); + a806=(a626*a806); + a805=(a805+a806); + a39=(a39+a805); + a806=(a779*a629); + a806=(a628*a806); + a807=(a779*a633); + a807=(a632*a807); + a806=(a806+a807); + a39=(a39+a806); + a807=(a556*a83); + a808=(a550*a72); + a807=(a807+a808); + a807=(a630*a807); + a39=(a39-a807); + a72=(a542*a72); + a807=(a556*a64); + a72=(a72-a807); + a72=(a634*a72); + a39=(a39+a72); + a72=(a542*a48); + a64=(a549*a64); + a72=(a72-a64); + a72=(a591*a72); + a39=(a39-a72); + a83=(a549*a83); + a48=(a550*a48); + a83=(a83+a48); + a83=(a635*a83); + a39=(a39-a83); + if (res[0]!=0) res[0][96]=a39; + a39=(a778*a661); + a39=(a639*a39); + a83=(a778*a637); + a48=(a612*a83); + a72=(a778*a659); + a64=(a644*a72); + a48=(a48+a64); + a64=(a48/a656); + a48=(a646*a48); + a807=(a645*a83); + a808=(a649*a72); + a807=(a807-a808); + a808=(a650*a807); + a48=(a48+a808); + a808=(a636*a48); + a64=(a64-a808); + a64=(a638*a64); + a808=(a649*a64); + a807=(a807/a656); + a48=(a655*a48); + a807=(a807-a48); + a807=(a638*a807); + a48=(a644*a807); + a808=(a808+a48); + a808=(a660*a808); + a39=(a39+a808); + a808=(a612*a807); + a48=(a645*a64); + a808=(a808-a48); + a808=(a663*a808); + a48=(a778*a607); + a48=(a658*a48); + a808=(a808-a48); + a39=(a39+a808); + a808=(a778*a666); + a808=(a665*a808); + a48=(a778*a669); + a48=(a668*a48); + a808=(a808+a48); + a48=(a39+a808); + a809=(a785*a671); + a809=(a670*a809); + a810=(a785*a675); + a810=(a674*a810); + a809=(a809+a810); + a48=(a48+a809); + a810=(a785*a677); + a810=(a676*a810); + a811=(a785*a681); + a811=(a680*a811); + a810=(a810+a811); + a48=(a48+a810); + a811=(a604*a104); + a812=(a598*a101); + a811=(a811+a812); + a811=(a643*a811); + a48=(a48-a811); + a101=(a590*a101); + a811=(a604*a98); + a101=(a101-a811); + a101=(a601*a101); + a48=(a48+a101); + a101=(a590*a803); + a98=(a597*a98); + a101=(a101-a98); + a101=(a559*a101); + a48=(a48-a101); + a104=(a597*a104); + a803=(a598*a803); + a104=(a104+a803); + a104=(a608*a104); + a48=(a48-a104); + if (res[0]!=0) res[0][97]=a48; + a48=(a769*a693); + a48=(a700*a48); + a104=(a769*a706); + a803=(a688*a104); + a101=(a769*a687); + a98=(a692*a101); + a803=(a803+a98); + a98=(a803/a686); + a803=(a684*a803); + a811=(a696*a104); + a812=(a689*a101); + a811=(a811-a812); + a812=(a702*a811); + a803=(a803+a812); + a812=(a691*a803); + a98=(a98-a812); + a98=(a662*a98); + a812=(a689*a98); + a811=(a811/a686); + a803=(a685*a803); + a811=(a811-a803); + a811=(a662*a811); + a803=(a692*a811); + a812=(a812+a803); + a812=(a697*a812); + a48=(a48+a812); + a812=(a688*a811); + a803=(a696*a98); + a812=(a812-a803); + a812=(a708*a812); + a803=(a769*a699); + a803=(a709*a803); + a812=(a812-a803); + a48=(a48+a812); + a812=(a769*a714); + a812=(a713*a812); + a803=(a769*a717); + a803=(a716*a803); + a812=(a812+a803); + a803=(a48+a812); + a813=(a791*a720); + a813=(a719*a813); + a814=(a791*a724); + a814=(a723*a814); + a813=(a813+a814); + a803=(a803+a813); + a814=(a791*a727); + a814=(a726*a814); + a815=(a791*a731); + a815=(a730*a815); + a814=(a814+a815); + a803=(a803+a814); + a815=(a654*a83); + a816=(a648*a64); + a815=(a815+a816); + a815=(a732*a815); + a803=(a803-a815); + a64=(a640*a64); + a815=(a654*a72); + a64=(a64-a815); + a64=(a733*a64); + a803=(a803+a64); + a64=(a640*a807); + a72=(a647*a72); + a64=(a64-a72); + a64=(a682*a64); + a803=(a803-a64); + a83=(a647*a83); + a807=(a648*a807); + a83=(a83+a807); + a83=(a734*a83); + a803=(a803-a83); + if (res[0]!=0) res[0][98]=a803; + a803=(a690*a98); + a83=(a678*a101); + a803=(a803-a83); + a803=(a711*a803); + a83=(a678*a104); + a98=(a657*a98); + a83=(a83+a98); + a83=(a751*a83); + a803=(a803-a83); + a83=(a690*a811); + a101=(a701*a101); + a83=(a83-a101); + a83=(a742*a83); + a803=(a803-a83); + a104=(a701*a104); + a811=(a657*a811); + a104=(a104+a811); + a104=(a705*a104); + a803=(a803-a104); + if (res[0]!=0) res[0][99]=a803; + a48=(a4*a48); + a803=(a5*a48); + a104=(a10*a48); + a812=(a38*a812); + a104=(a104+a812); + a812=(a5*a104); + a803=(a803+a812); + a812=(a10*a48); + a813=(a44*a813); + a812=(a812+a813); + a813=(a5*a812); + a803=(a803+a813); + a814=(a44*a814); + a814=(a48+a814); + a813=(a5*a814); + a803=(a803+a813); + a813=(a5*a803); + a39=(a4*a39); + a811=(a5*a39); + a813=(a813+a811); + a811=(a10*a39); + a808=(a38*a808); + a811=(a811+a808); + a808=(a5*a811); + a813=(a813+a808); + a808=(a10*a39); + a809=(a44*a809); + a808=(a808+a809); + a809=(a5*a808); + a813=(a813+a809); + a810=(a44*a810); + a810=(a39+a810); + a809=(a5*a810); + a813=(a813+a809); + a809=(a5*a813); + a0=(a4*a0); + a83=(a5*a0); + a809=(a809+a83); + a83=(a10*a0); + a804=(a38*a804); + a83=(a83+a804); + a804=(a5*a83); + a809=(a809+a804); + a804=(a10*a0); + a805=(a44*a805); + a804=(a804+a805); + a805=(a5*a804); + a809=(a809+a805); + a806=(a44*a806); + a806=(a0+a806); + a805=(a5*a806); + a809=(a809+a805); + a805=(a5*a809); + a71=(a4*a71); + a101=(a5*a71); + a805=(a805+a101); + a101=(a10*a71); + a800=(a38*a800); + a101=(a101+a800); + a800=(a5*a101); + a805=(a805+a800); + a800=(a10*a71); + a801=(a44*a801); + a800=(a800+a801); + a801=(a5*a800); + a805=(a805+a801); + a802=(a44*a802); + a802=(a71+a802); + a801=(a5*a802); + a805=(a805+a801); + a801=(a5*a805); + a77=(a4*a77); + a98=(a5*a77); + a801=(a801+a98); + a98=(a10*a77); + a794=(a38*a794); + a98=(a98+a794); + a794=(a5*a98); + a801=(a801+a794); + a794=(a10*a77); + a789=(a44*a789); + a794=(a794+a789); + a789=(a5*a794); + a801=(a801+a789); + a788=(a44*a788); + a788=(a77+a788); + a789=(a5*a788); + a801=(a801+a789); + a789=(a5*a801); + a70=(a4*a70); + a807=(a5*a70); + a789=(a789+a807); + a807=(a10*a70); + a12=(a38*a12); + a807=(a807+a12); + a12=(a5*a807); + a789=(a789+a12); + a12=(a10*a70); + a106=(a44*a106); + a12=(a12+a106); + a106=(a5*a12); + a789=(a789+a106); + a33=(a44*a33); + a33=(a70+a33); + a106=(a5*a33); + a789=(a789+a106); + a106=(a5*a789); + a85=(a4*a85); + a64=(a5*a85); + a106=(a106+a64); + a64=(a10*a85); + a69=(a38*a69); + a64=(a64+a69); + a69=(a5*a64); + a106=(a106+a69); + a69=(a10*a85); + a743=(a44*a743); + a69=(a69+a743); + a743=(a5*a69); + a106=(a106+a743); + a54=(a44*a54); + a54=(a85+a54); + a743=(a5*a54); + a106=(a106+a743); + a743=(a5*a106); + a78=(a4*a78); + a72=(a5*a78); + a743=(a743+a72); + a72=(a10*a78); + a75=(a38*a75); + a72=(a72+a75); + a75=(a5*a72); + a743=(a743+a75); + a75=(a10*a78); + a65=(a44*a65); + a75=(a75+a65); + a65=(a5*a75); + a743=(a743+a65); + a68=(a44*a68); + a68=(a78+a68); + a65=(a5*a68); + a743=(a743+a65); + a65=(a5*a743); + a80=(a4*a80); + a815=(a5*a80); + a65=(a65+a815); + a815=(a10*a80); + a79=(a38*a79); + a815=(a815+a79); + a79=(a5*a815); + a65=(a65+a79); + a79=(a10*a80); + a59=(a44*a59); + a79=(a79+a59); + a59=(a5*a79); + a65=(a65+a59); + a66=(a44*a66); + a66=(a80+a66); + a59=(a5*a66); + a65=(a65+a59); + a59=(a5*a65); + a99=(a4*a99); + a816=(a5*a99); + a59=(a59+a816); + a816=(a10*a99); + a86=(a38*a86); + a816=(a816+a86); + a86=(a5*a816); + a59=(a59+a86); + a86=(a10*a99); + a52=(a44*a52); + a86=(a86+a52); + a52=(a5*a86); + a59=(a59+a52); + a57=(a44*a57); + a57=(a99+a57); + a52=(a5*a57); + a59=(a59+a52); + a52=(a5*a59); + a76=(a4*a76); + a817=(a5*a76); + a52=(a52+a817); + a817=(a10*a76); + a754=(a38*a754); + a817=(a817+a754); + a754=(a5*a817); + a52=(a52+a754); + a754=(a10*a76); + a73=(a44*a73); + a754=(a754+a73); + a73=(a5*a754); + a52=(a52+a73); + a115=(a44*a115); + a115=(a76+a115); + a73=(a5*a115); + a52=(a52+a73); + a73=(a5*a52); + a105=(a4*a105); + a818=(a5*a105); + a73=(a73+a818); + a818=(a10*a105); + a62=(a38*a62); + a818=(a818+a62); + a62=(a5*a818); + a73=(a73+a62); + a62=(a10*a105); + a67=(a44*a67); + a62=(a62+a67); + a67=(a5*a62); + a73=(a73+a67); + a82=(a44*a82); + a82=(a105+a82); + a67=(a5*a82); + a73=(a73+a67); + a73=(a5*a73); + a92=(a4*a92); + a67=(a5*a92); + a73=(a73+a67); + a67=(a10*a92); + a95=(a38*a95); + a67=(a67+a95); + a67=(a5*a67); + a73=(a73+a67); + a67=(a10*a92); + a93=(a44*a93); + a67=(a67+a93); + a67=(a5*a67); + a73=(a73+a67); + a90=(a44*a90); + a92=(a92+a90); + a92=(a5*a92); + a73=(a73+a92); + a92=(a5*a73); + a74=(a4*a74); + a90=(a5*a74); + a92=(a92+a90); + a90=(a10*a74); + a89=(a38*a89); + a90=(a90+a89); + a89=(a5*a90); + a92=(a92+a89); + a89=(a10*a74); + a94=(a44*a94); + a89=(a89+a94); + a94=(a5*a89); + a92=(a92+a94); + a100=(a44*a100); + a100=(a74+a100); + a94=(a5*a100); + a92=(a92+a94); + a92=(a8*a92); + a94=(a50*a34); + a92=(a92-a94); + a67=(a50*a34); + a92=(a92-a67); + if (res[0]!=0) res[0][100]=a92; + a1=(a28*a1); + a87=(a87*a1); + a43=(a42*a43); + a97=(a97*a43); + a87=(a87+a97); + a37=(a42*a37); + a103=(a103*a37); + a87=(a87+a103); + a63=(a28*a63); + a88=(a88*a63); + a87=(a87-a88); + a58=(a42*a58); + a91=(a91*a58); + a87=(a87-a91); + a6=(a42*a6); + a96=(a96*a6); + a87=(a87-a96); + a87=(-a87); + if (res[0]!=0) res[0][101]=a87; + a73=(a8*a73); + a74=(a8*a74); + a73=(a73+a74); + a90=(a8*a90); + a73=(a73+a90); + a89=(a8*a89); + a73=(a73+a89); + a100=(a8*a100); + a73=(a73+a100); + a100=(a24*a34); + a73=(a73-a100); + a100=(a50*a34); + a73=(a73+a100); + a73=(a73+a94); + a73=(a73+a67); + a67=(a10*a34); + a73=(a73+a67); + if (res[0]!=0) res[0][102]=a73; + a73=sin(a125); + a73=(a73*a136); + a67=cos(a125); + a67=(a67*a113); + a73=(a73-a67); + a67=cos(a56); + a67=(a67*a113); + a73=(a73-a67); + a67=sin(a114); + a67=(a67*a139); + a94=cos(a114); + a94=(a94*a145); + a67=(a67-a94); + a73=(a73+a67); + a67=sin(a9); + a67=(a67*a144); + a94=cos(a9); + a94=(a94*a151); + a67=(a67-a94); + a73=(a73+a67); + a56=sin(a56); + a56=(a56*a136); + a73=(a73+a56); + if (res[0]!=0) res[0][103]=a73; + a73=cos(a125); + a56=(a51*a73); + a56=(a113*a56); + a67=cos(a114); + a94=(a32*a67); + a94=(a145*a94); + a56=(a56+a94); + a94=cos(a9); + a100=(a32*a94); + a100=(a151*a100); + a56=(a56+a100); + a125=sin(a125); + a100=(a51*a125); + a100=(a136*a100); + a56=(a56-a100); + a114=sin(a114); + a100=(a32*a114); + a100=(a139*a100); + a56=(a56-a100); + a9=sin(a9); + a100=(a32*a9); + a100=(a144*a100); + a56=(a56-a100); + a56=(-a56); + if (res[0]!=0) res[0][104]=a56; + a56=(a10*a34); + a100=(a10*a34); + a89=(a56+a100); + a89=(-a89); + if (res[0]!=0) res[0][105]=a89; + a89=(a41*a73); + a89=(a113*a89); + a90=(a35*a67); + a90=(a145*a90); + a89=(a89+a90); + a90=(a35*a94); + a90=(a151*a90); + a89=(a89+a90); + a90=(a41*a125); + a90=(a136*a90); + a89=(a89-a90); + a90=(a35*a114); + a90=(a139*a90); + a89=(a89-a90); + a90=(a35*a9); + a90=(a144*a90); + a89=(a89-a90); + a89=(-a89); + if (res[0]!=0) res[0][106]=a89; + a89=(a3*a34); + a90=(a10*a34); + a74=(a89-a90); + a56=(a56-a74); + a56=(a56+a100); + a100=(a10*a34); + a56=(a56+a100); + if (res[0]!=0) res[0][107]=a56; + a133=(a28*a133); + a111=(a111*a133); + a109=(a28*a109); + a133=(a84*a109); + a131=(a28*a131); + a56=(a116*a131); + a133=(a133+a56); + a56=(a133/a128); + a118=(a118*a133); + a133=(a117*a109); + a100=(a121*a131); + a133=(a133-a100); + a122=(a122*a133); + a118=(a118+a122); + a108=(a108*a118); + a56=(a56-a108); + a56=(a110*a56); + a121=(a121*a56); + a133=(a133/a128); + a127=(a127*a118); + a133=(a133-a127); + a110=(a110*a133); + a116=(a116*a110); + a121=(a121+a116); + a132=(a132*a121); + a111=(a111+a132); + a84=(a84*a110); + a117=(a117*a56); + a84=(a84-a117); + a135=(a135*a84); + a60=(a28*a60); + a130=(a130*a60); + a135=(a135-a130); + a111=(a111+a135); + a138=(a28*a138); + a137=(a137*a138); + a141=(a28*a141); + a140=(a140*a141); + a137=(a137+a140); + a140=(a111+a137); + a143=(a42*a143); + a142=(a142*a143); + a147=(a42*a147); + a146=(a146*a147); + a142=(a142+a146); + a140=(a140+a142); + a149=(a42*a149); + a148=(a148*a149); + a153=(a42*a153); + a152=(a152*a153); + a148=(a148+a152); + a140=(a140+a148); + if (res[0]!=0) res[0][108]=a140; + a140=(a41*a179); + a140=(a157*a140); + a152=(a41*a154); + a153=(a161*a152); + a149=(a41*a177); + a146=(a162*a149); + a153=(a153+a146); + a146=(a153/a174); + a153=(a164*a153); + a147=(a163*a152); + a143=(a167*a149); + a147=(a147-a143); + a143=(a168*a147); + a153=(a153+a143); + a143=(a134*a153); + a146=(a146-a143); + a146=(a150*a146); + a143=(a167*a146); + a147=(a147/a174); + a153=(a173*a153); + a147=(a147-a153); + a147=(a150*a147); + a153=(a162*a147); + a143=(a143+a153); + a143=(a178*a143); + a140=(a140+a143); + a143=(a161*a147); + a153=(a163*a146); + a143=(a143-a153); + a143=(a181*a143); + a153=(a41*a123); + a153=(a182*a153); + a143=(a143-a153); + a140=(a140+a143); + a143=(a41*a186); + a143=(a185*a143); + a153=(a41*a189); + a153=(a188*a153); + a143=(a143+a153); + a153=(a140+a143); + a141=(a35*a191); + a141=(a190*a141); + a138=(a35*a195); + a138=(a194*a138); + a141=(a141+a138); + a153=(a153+a141); + a138=(a35*a197); + a138=(a196*a138); + a135=(a35*a201); + a135=(a200*a135); + a138=(a138+a135); + a153=(a153+a138); + a135=(a126*a109); + a130=(a120*a56); + a135=(a135+a130); + a198=(a198*a135); + a153=(a153-a198); + a56=(a112*a56); + a126=(a126*a131); + a56=(a56-a126); + a202=(a202*a56); + a153=(a153+a202); + a112=(a112*a110); + a131=(a119*a131); + a112=(a112-a131); + a159=(a159*a112); + a153=(a153-a159); + a119=(a119*a109); + a120=(a120*a110); + a119=(a119+a120); + a203=(a203*a119); + a153=(a153-a203); + if (res[0]!=0) res[0][109]=a153; + a153=(a51*a229); + a153=(a207*a153); + a203=(a51*a205); + a119=(a180*a203); + a120=(a51*a227); + a110=(a212*a120); + a119=(a119+a110); + a110=(a119/a224); + a119=(a214*a119); + a109=(a213*a203); + a159=(a217*a120); + a109=(a109-a159); + a159=(a218*a109); + a119=(a119+a159); + a159=(a204*a119); + a110=(a110-a159); + a110=(a206*a110); + a159=(a217*a110); + a109=(a109/a224); + a119=(a223*a119); + a109=(a109-a119); + a109=(a206*a109); + a119=(a212*a109); + a159=(a159+a119); + a159=(a228*a159); + a153=(a153+a159); + a159=(a180*a109); + a119=(a213*a110); + a159=(a159-a119); + a159=(a231*a159); + a119=(a51*a175); + a119=(a226*a119); + a159=(a159-a119); + a153=(a153+a159); + a159=(a51*a234); + a159=(a233*a159); + a119=(a51*a237); + a119=(a236*a119); + a159=(a159+a119); + a119=(a153+a159); + a112=(a32*a239); + a112=(a238*a112); + a131=(a32*a243); + a131=(a242*a131); + a112=(a112+a131); + a119=(a119+a112); + a131=(a32*a245); + a131=(a244*a131); + a202=(a32*a249); + a202=(a248*a202); + a131=(a131+a202); + a119=(a119+a131); + a202=(a172*a152); + a56=(a166*a146); + a202=(a202+a56); + a202=(a211*a202); + a119=(a119-a202); + a146=(a158*a146); + a202=(a172*a149); + a146=(a146-a202); + a146=(a169*a146); + a119=(a119+a146); + a146=(a158*a147); + a149=(a165*a149); + a146=(a146-a149); + a146=(a155*a146); + a119=(a119-a146); + a152=(a165*a152); + a147=(a166*a147); + a152=(a152+a147); + a152=(a176*a152); + a119=(a119-a152); + if (res[0]!=0) res[0][110]=a119; + a119=(a14*a275); + a119=(a253*a119); + a152=(a14*a250); + a147=(a257*a152); + a146=(a14*a273); + a149=(a258*a146); + a147=(a147+a149); + a149=(a147/a270); + a147=(a260*a147); + a202=(a259*a152); + a56=(a263*a146); + a202=(a202-a56); + a56=(a264*a202); + a147=(a147+a56); + a56=(a230*a147); + a149=(a149-a56); + a149=(a246*a149); + a56=(a263*a149); + a202=(a202/a270); + a147=(a269*a147); + a202=(a202-a147); + a202=(a246*a202); + a147=(a258*a202); + a56=(a56+a147); + a56=(a274*a56); + a119=(a119+a56); + a56=(a257*a202); + a147=(a259*a149); + a56=(a56-a147); + a56=(a277*a56); + a147=(a14*a219); + a147=(a278*a147); + a56=(a56-a147); + a119=(a119+a56); + a56=(a2*a282); + a56=(a281*a56); + a147=(a2*a285); + a147=(a284*a147); + a56=(a56+a147); + a147=(a119+a56); + a126=(a29*a287); + a126=(a286*a126); + a198=(a29*a291); + a198=(a290*a198); + a126=(a126+a198); + a147=(a147+a126); + a198=(a29*a293); + a198=(a292*a198); + a135=(a29*a297); + a135=(a296*a135); + a198=(a198+a135); + a147=(a147+a198); + a135=(a222*a203); + a130=(a216*a110); + a135=(a135+a130); + a135=(a294*a135); + a147=(a147-a135); + a110=(a208*a110); + a135=(a222*a120); + a110=(a110-a135); + a110=(a298*a110); + a147=(a147+a110); + a110=(a208*a109); + a120=(a215*a120); + a110=(a110-a120); + a110=(a255*a110); + a147=(a147-a110); + a203=(a215*a203); + a109=(a216*a109); + a203=(a203+a109); + a203=(a299*a203); + a147=(a147-a203); + if (res[0]!=0) res[0][111]=a147; + a147=(a20*a325); + a147=(a303*a147); + a203=(a20*a301); + a109=(a276*a203); + a110=(a20*a323); + a120=(a308*a110); + a109=(a109+a120); + a120=(a109/a320); + a109=(a310*a109); + a135=(a309*a203); + a130=(a313*a110); + a135=(a135-a130); + a130=(a314*a135); + a109=(a109+a130); + a130=(a300*a109); + a120=(a120-a130); + a120=(a302*a120); + a130=(a313*a120); + a135=(a135/a320); + a109=(a319*a109); + a135=(a135-a109); + a135=(a302*a135); + a109=(a308*a135); + a130=(a130+a109); + a130=(a324*a130); + a147=(a147+a130); + a130=(a276*a135); + a109=(a309*a120); + a130=(a130-a109); + a130=(a327*a130); + a109=(a20*a271); + a109=(a322*a109); + a130=(a130-a109); + a147=(a147+a130); + a130=(a15*a330); + a130=(a329*a130); + a109=(a15*a333); + a109=(a332*a109); + a130=(a130+a109); + a109=(a147+a130); + a60=(a21*a335); + a60=(a334*a60); + a84=(a21*a339); + a84=(a338*a84); + a60=(a60+a84); + a109=(a109+a60); + a84=(a21*a341); + a84=(a340*a84); + a117=(a21*a345); + a117=(a344*a117); + a84=(a84+a117); + a109=(a109+a84); + a117=(a268*a152); + a132=(a262*a149); + a117=(a117+a132); + a117=(a307*a117); + a109=(a109-a117); + a149=(a254*a149); + a117=(a268*a146); + a149=(a149-a117); + a149=(a265*a149); + a109=(a109+a149); + a149=(a254*a202); + a146=(a261*a146); + a149=(a149-a146); + a149=(a251*a149); + a109=(a109-a149); + a152=(a261*a152); + a202=(a262*a202); + a152=(a152+a202); + a152=(a272*a152); + a109=(a109-a152); + if (res[0]!=0) res[0][112]=a109; + a109=(a18*a371); + a109=(a349*a109); + a152=(a18*a346); + a202=(a353*a152); + a149=(a18*a369); + a146=(a354*a149); + a202=(a202+a146); + a146=(a202/a366); + a202=(a356*a202); + a117=(a355*a152); + a132=(a359*a149); + a117=(a117-a132); + a132=(a360*a117); + a202=(a202+a132); + a132=(a326*a202); + a146=(a146-a132); + a146=(a342*a146); + a132=(a359*a146); + a117=(a117/a366); + a202=(a365*a202); + a117=(a117-a202); + a117=(a342*a117); + a202=(a354*a117); + a132=(a132+a202); + a132=(a370*a132); + a109=(a109+a132); + a132=(a353*a117); + a202=(a355*a146); + a132=(a132-a202); + a132=(a373*a132); + a202=(a18*a315); + a202=(a374*a202); + a132=(a132-a202); + a109=(a109+a132); + a132=(a18*a378); + a132=(a377*a132); + a202=(a18*a381); + a202=(a380*a202); + a132=(a132+a202); + a202=(a109+a132); + a121=(a748*a383); + a121=(a382*a121); + a116=(a748*a387); + a116=(a386*a116); + a121=(a121+a116); + a202=(a202+a121); + a116=(a748*a389); + a116=(a388*a116); + a133=(a748*a393); + a133=(a392*a133); + a116=(a116+a133); + a202=(a202+a116); + a133=(a318*a203); + a127=(a312*a120); + a133=(a133+a127); + a133=(a390*a133); + a202=(a202-a133); + a120=(a304*a120); + a133=(a318*a110); + a120=(a120-a133); + a120=(a394*a120); + a202=(a202+a120); + a120=(a304*a135); + a110=(a311*a110); + a120=(a120-a110); + a120=(a351*a120); + a202=(a202-a120); + a203=(a311*a203); + a135=(a312*a135); + a203=(a203+a135); + a203=(a395*a203); + a202=(a202-a203); + if (res[0]!=0) res[0][113]=a202; + a202=(a23*a421); + a202=(a399*a202); + a203=(a23*a397); + a135=(a372*a203); + a120=(a23*a419); + a110=(a404*a120); + a135=(a135+a110); + a110=(a135/a416); + a135=(a406*a135); + a133=(a405*a203); + a127=(a409*a120); + a133=(a133-a127); + a127=(a410*a133); + a135=(a135+a127); + a127=(a396*a135); + a110=(a110-a127); + a110=(a398*a110); + a127=(a409*a110); + a133=(a133/a416); + a135=(a415*a135); + a133=(a133-a135); + a133=(a398*a133); + a135=(a404*a133); + a127=(a127+a135); + a127=(a420*a127); + a202=(a202+a127); + a127=(a372*a133); + a135=(a405*a110); + a127=(a127-a135); + a127=(a423*a127); + a135=(a23*a367); + a135=(a418*a135); + a127=(a127-a135); + a202=(a202+a127); + a127=(a23*a426); + a127=(a425*a127); + a135=(a23*a429); + a135=(a428*a135); + a127=(a127+a135); + a135=(a202+a127); + a118=(a759*a431); + a118=(a430*a118); + a128=(a759*a435); + a128=(a434*a128); + a118=(a118+a128); + a135=(a135+a118); + a128=(a759*a437); + a128=(a436*a128); + a108=(a759*a441); + a108=(a440*a108); + a128=(a128+a108); + a135=(a135+a128); + a108=(a364*a152); + a122=(a358*a146); + a108=(a108+a122); + a108=(a403*a108); + a135=(a135-a108); + a146=(a350*a146); + a108=(a364*a149); + a146=(a146-a108); + a146=(a361*a146); + a135=(a135+a146); + a146=(a350*a117); + a149=(a357*a149); + a146=(a146-a149); + a146=(a347*a146); + a135=(a135-a146); + a152=(a357*a152); + a117=(a358*a117); + a152=(a152+a117); + a152=(a368*a152); + a135=(a135-a152); + if (res[0]!=0) res[0][114]=a135; + a135=(a27*a467); + a135=(a445*a135); + a152=(a27*a442); + a117=(a449*a152); + a146=(a27*a465); + a149=(a450*a146); + a117=(a117+a149); + a149=(a117/a462); + a117=(a452*a117); + a108=(a451*a152); + a122=(a455*a146); + a108=(a108-a122); + a122=(a456*a108); + a117=(a117+a122); + a122=(a422*a117); + a149=(a149-a122); + a149=(a438*a149); + a122=(a455*a149); + a108=(a108/a462); + a117=(a461*a117); + a108=(a108-a117); + a108=(a438*a108); + a117=(a450*a108); + a122=(a122+a117); + a122=(a466*a122); + a135=(a135+a122); + a122=(a449*a108); + a117=(a451*a149); + a122=(a122-a117); + a122=(a469*a122); + a117=(a27*a411); + a117=(a470*a117); + a122=(a122-a117); + a135=(a135+a122); + a122=(a760*a474); + a122=(a473*a122); + a117=(a760*a477); + a117=(a476*a117); + a122=(a122+a117); + a117=(a135+a122); + a100=(a750*a479); + a100=(a478*a100); + a74=(a750*a483); + a74=(a482*a74); + a100=(a100+a74); + a117=(a117+a100); + a74=(a750*a485); + a74=(a484*a74); + a87=(a750*a489); + a87=(a488*a87); + a74=(a74+a87); + a117=(a117+a74); + a87=(a414*a203); + a96=(a408*a110); + a87=(a87+a96); + a87=(a486*a87); + a117=(a117-a87); + a110=(a400*a110); + a87=(a414*a120); + a110=(a110-a87); + a110=(a490*a110); + a117=(a117+a110); + a110=(a400*a133); + a120=(a407*a120); + a110=(a110-a120); + a110=(a447*a110); + a117=(a117-a110); + a203=(a407*a203); + a133=(a408*a133); + a203=(a203+a133); + a203=(a491*a203); + a117=(a117-a203); + if (res[0]!=0) res[0][115]=a117; + a117=(a703*a517); + a117=(a495*a117); + a203=(a703*a493); + a133=(a468*a203); + a110=(a703*a515); + a120=(a500*a110); + a133=(a133+a120); + a120=(a133/a512); + a133=(a502*a133); + a87=(a501*a203); + a96=(a505*a110); + a87=(a87-a96); + a96=(a506*a87); + a133=(a133+a96); + a96=(a492*a133); + a120=(a120-a96); + a120=(a494*a120); + a96=(a505*a120); + a87=(a87/a512); + a133=(a511*a133); + a87=(a87-a133); + a87=(a494*a87); + a133=(a500*a87); + a96=(a96+a133); + a96=(a516*a96); + a117=(a117+a96); + a96=(a468*a87); + a133=(a501*a120); + a96=(a96-a133); + a96=(a519*a96); + a133=(a703*a463); + a133=(a514*a133); + a96=(a96-a133); + a117=(a117+a96); + a96=(a703*a522); + a96=(a521*a96); + a133=(a703*a525); + a133=(a524*a133); + a96=(a96+a133); + a133=(a117+a96); + a6=(a747*a527); + a6=(a526*a6); + a91=(a747*a531); + a91=(a530*a91); + a6=(a6+a91); + a133=(a133+a6); + a91=(a747*a533); + a91=(a532*a91); + a58=(a747*a537); + a58=(a536*a58); + a91=(a91+a58); + a133=(a133+a91); + a58=(a460*a152); + a88=(a454*a149); + a58=(a58+a88); + a58=(a499*a58); + a133=(a133-a58); + a149=(a446*a149); + a58=(a460*a146); + a149=(a149-a58); + a149=(a457*a149); + a133=(a133+a149); + a149=(a446*a108); + a146=(a453*a146); + a149=(a149-a146); + a149=(a443*a149); + a133=(a133-a149); + a152=(a453*a152); + a108=(a454*a108); + a152=(a152+a108); + a152=(a464*a152); + a133=(a133-a152); + if (res[0]!=0) res[0][116]=a133; + a133=(a749*a563); + a133=(a541*a133); + a152=(a749*a538); + a108=(a545*a152); + a149=(a749*a561); + a146=(a546*a149); + a108=(a108+a146); + a146=(a108/a558); + a108=(a548*a108); + a58=(a547*a152); + a88=(a551*a149); + a58=(a58-a88); + a88=(a552*a58); + a108=(a108+a88); + a88=(a518*a108); + a146=(a146-a88); + a146=(a534*a146); + a88=(a551*a146); + a58=(a58/a558); + a108=(a557*a108); + a58=(a58-a108); + a58=(a534*a58); + a108=(a546*a58); + a88=(a88+a108); + a88=(a562*a88); + a133=(a133+a88); + a88=(a545*a58); + a108=(a547*a146); + a88=(a88-a108); + a88=(a565*a88); + a108=(a749*a507); + a108=(a560*a108); + a88=(a88-a108); + a133=(a133+a88); + a88=(a764*a568); + a88=(a567*a88); + a108=(a764*a571); + a108=(a570*a108); + a88=(a88+a108); + a108=(a133+a88); + a63=(a766*a573); + a63=(a572*a63); + a103=(a766*a577); + a103=(a576*a103); + a63=(a63+a103); + a108=(a108+a63); + a103=(a766*a579); + a103=(a578*a103); + a37=(a766*a583); + a37=(a582*a37); + a103=(a103+a37); + a108=(a108+a103); + a37=(a510*a203); + a97=(a504*a120); + a37=(a37+a97); + a37=(a580*a37); + a108=(a108-a37); + a120=(a496*a120); + a37=(a510*a110); + a120=(a120-a37); + a120=(a584*a120); + a108=(a108+a120); + a120=(a496*a87); + a110=(a503*a110); + a120=(a120-a110); + a120=(a564*a120); + a108=(a108-a120); + a203=(a503*a203); + a87=(a504*a87); + a203=(a203+a87); + a203=(a585*a203); + a108=(a108-a203); + if (res[0]!=0) res[0][117]=a108; + a108=(a765*a611); + a108=(a589*a108); + a203=(a765*a587); + a87=(a593*a203); + a120=(a765*a609); + a110=(a594*a120); + a87=(a87+a110); + a110=(a87/a606); + a87=(a596*a87); + a37=(a595*a203); + a97=(a599*a120); + a37=(a37-a97); + a97=(a600*a37); + a87=(a87+a97); + a97=(a586*a87); + a110=(a110-a97); + a110=(a588*a110); + a97=(a599*a110); + a37=(a37/a606); + a87=(a605*a87); + a37=(a37-a87); + a37=(a588*a37); + a87=(a594*a37); + a97=(a97+a87); + a97=(a610*a97); + a108=(a108+a97); + a97=(a593*a37); + a87=(a595*a110); + a97=(a97-a87); + a97=(a613*a97); + a87=(a765*a553); + a87=(a614*a87); + a97=(a97-a87); + a108=(a108+a97); + a97=(a765*a618); + a97=(a617*a97); + a87=(a765*a621); + a87=(a620*a87); + a97=(a97+a87); + a87=(a108+a97); + a43=(a772*a623); + a43=(a622*a43); + a1=(a772*a627); + a1=(a626*a1); + a43=(a43+a1); + a87=(a87+a43); + a1=(a772*a629); + a1=(a628*a1); + a92=(a772*a633); + a92=(a632*a92); + a1=(a1+a92); + a87=(a87+a1); + a92=(a556*a152); + a93=(a550*a146); + a92=(a92+a93); + a92=(a630*a92); + a87=(a87-a92); + a146=(a542*a146); + a92=(a556*a149); + a146=(a146-a92); + a146=(a634*a146); + a87=(a87+a146); + a146=(a542*a58); + a149=(a549*a149); + a146=(a146-a149); + a146=(a591*a146); + a87=(a87-a146); + a152=(a549*a152); + a58=(a550*a58); + a152=(a152+a58); + a152=(a635*a152); + a87=(a87-a152); + if (res[0]!=0) res[0][118]=a87; + a87=(a738*a661); + a87=(a639*a87); + a152=(a738*a637); + a58=(a612*a152); + a146=(a738*a659); + a149=(a644*a146); + a58=(a58+a149); + a149=(a58/a656); + a58=(a646*a58); + a92=(a645*a152); + a93=(a649*a146); + a92=(a92-a93); + a93=(a650*a92); + a58=(a58+a93); + a93=(a636*a58); + a149=(a149-a93); + a149=(a638*a149); + a93=(a649*a149); + a92=(a92/a656); + a58=(a655*a58); + a92=(a92-a58); + a92=(a638*a92); + a58=(a644*a92); + a93=(a93+a58); + a93=(a660*a93); + a87=(a87+a93); + a93=(a612*a92); + a58=(a645*a149); + a93=(a93-a58); + a93=(a663*a93); + a58=(a738*a607); + a58=(a658*a58); + a93=(a93-a58); + a87=(a87+a93); + a93=(a777*a666); + a93=(a665*a93); + a58=(a777*a669); + a58=(a668*a58); + a93=(a93+a58); + a58=(a87+a93); + a95=(a779*a671); + a95=(a670*a95); + a819=(a779*a675); + a819=(a674*a819); + a95=(a95+a819); + a58=(a58+a95); + a819=(a779*a677); + a819=(a676*a819); + a820=(a779*a681); + a820=(a680*a820); + a819=(a819+a820); + a58=(a58+a819); + a820=(a604*a203); + a821=(a598*a110); + a820=(a820+a821); + a820=(a643*a820); + a58=(a58-a820); + a110=(a590*a110); + a820=(a604*a120); + a110=(a110-a820); + a110=(a601*a110); + a58=(a58+a110); + a110=(a590*a37); + a120=(a597*a120); + a110=(a110-a120); + a110=(a559*a110); + a58=(a58-a110); + a203=(a597*a203); + a37=(a598*a37); + a203=(a203+a37); + a203=(a608*a203); + a58=(a58-a203); + if (res[0]!=0) res[0][119]=a58; + a58=(a778*a693); + a58=(a700*a58); + a203=(a778*a706); + a37=(a688*a203); + a110=(a778*a687); + a120=(a692*a110); + a37=(a37+a120); + a120=(a37/a686); + a37=(a684*a37); + a820=(a696*a203); + a821=(a689*a110); + a820=(a820-a821); + a821=(a702*a820); + a37=(a37+a821); + a821=(a691*a37); + a120=(a120-a821); + a120=(a662*a120); + a821=(a689*a120); + a820=(a820/a686); + a37=(a685*a37); + a820=(a820-a37); + a820=(a662*a820); + a37=(a692*a820); + a821=(a821+a37); + a821=(a697*a821); + a58=(a58+a821); + a821=(a688*a820); + a37=(a696*a120); + a821=(a821-a37); + a821=(a708*a821); + a37=(a778*a699); + a37=(a709*a37); + a821=(a821-a37); + a58=(a58+a821); + a821=(a778*a714); + a821=(a713*a821); + a37=(a778*a717); + a37=(a716*a37); + a821=(a821+a37); + a37=(a58+a821); + a822=(a785*a720); + a822=(a719*a822); + a823=(a785*a724); + a823=(a723*a823); + a822=(a822+a823); + a37=(a37+a822); + a823=(a785*a727); + a823=(a726*a823); + a824=(a785*a731); + a824=(a730*a824); + a823=(a823+a824); + a37=(a37+a823); + a824=(a654*a152); + a825=(a648*a149); + a824=(a824+a825); + a824=(a732*a824); + a37=(a37-a824); + a149=(a640*a149); + a824=(a654*a146); + a149=(a149-a824); + a149=(a733*a149); + a37=(a37+a149); + a149=(a640*a92); + a146=(a647*a146); + a149=(a149-a146); + a149=(a682*a149); + a37=(a37-a149); + a152=(a647*a152); + a92=(a648*a92); + a152=(a152+a92); + a152=(a734*a152); + a37=(a37-a152); + if (res[0]!=0) res[0][120]=a37; + a37=(a690*a120); + a152=(a678*a110); + a37=(a37-a152); + a37=(a711*a37); + a152=(a678*a203); + a120=(a657*a120); + a152=(a152+a120); + a152=(a751*a152); + a37=(a37-a152); + a152=(a690*a820); + a110=(a701*a110); + a152=(a152-a110); + a152=(a742*a152); + a37=(a37-a152); + a203=(a701*a203); + a820=(a657*a820); + a203=(a203+a820); + a203=(a705*a203); + a37=(a37-a203); + if (res[0]!=0) res[0][121]=a37; + a19=(a8*a19); + a107=(a8*a107); + a19=(a19+a107); + a22=(a8*a22); + a19=(a19+a22); + a17=(a8*a17); + a19=(a19+a17); + a695=(a8*a695); + a19=(a19+a695); + if (res[0]!=0) res[0][122]=a19; + a58=(a4*a58); + a19=(a5*a58); + a695=(a10*a58); + a821=(a38*a821); + a695=(a695+a821); + a821=(a5*a695); + a19=(a19+a821); + a821=(a10*a58); + a822=(a44*a822); + a821=(a821+a822); + a822=(a5*a821); + a19=(a19+a822); + a823=(a44*a823); + a823=(a58+a823); + a822=(a5*a823); + a19=(a19+a822); + a822=(a5*a19); + a87=(a4*a87); + a17=(a5*a87); + a822=(a822+a17); + a17=(a10*a87); + a93=(a38*a93); + a17=(a17+a93); + a93=(a5*a17); + a822=(a822+a93); + a93=(a10*a87); + a95=(a44*a95); + a93=(a93+a95); + a95=(a5*a93); + a822=(a822+a95); + a819=(a44*a819); + a819=(a87+a819); + a95=(a5*a819); + a822=(a822+a95); + a95=(a5*a822); + a108=(a4*a108); + a22=(a5*a108); + a95=(a95+a22); + a22=(a10*a108); + a97=(a38*a97); + a22=(a22+a97); + a97=(a5*a22); + a95=(a95+a97); + a97=(a10*a108); + a43=(a44*a43); + a97=(a97+a43); + a43=(a5*a97); + a95=(a95+a43); + a1=(a44*a1); + a1=(a108+a1); + a43=(a5*a1); + a95=(a95+a43); + a43=(a5*a95); + a133=(a4*a133); + a107=(a5*a133); + a43=(a43+a107); + a107=(a10*a133); + a88=(a38*a88); + a107=(a107+a88); + a88=(a5*a107); + a43=(a43+a88); + a88=(a10*a133); + a63=(a44*a63); + a88=(a88+a63); + a63=(a5*a88); + a43=(a43+a63); + a103=(a44*a103); + a103=(a133+a103); + a63=(a5*a103); + a43=(a43+a63); + a63=(a5*a43); + a117=(a4*a117); + a37=(a5*a117); + a63=(a63+a37); + a37=(a10*a117); + a96=(a38*a96); + a37=(a37+a96); + a96=(a5*a37); + a63=(a63+a96); + a96=(a10*a117); + a6=(a44*a6); + a96=(a96+a6); + a6=(a5*a96); + a63=(a63+a6); + a91=(a44*a91); + a91=(a117+a91); + a6=(a5*a91); + a63=(a63+a6); + a6=(a5*a63); + a135=(a4*a135); + a203=(a5*a135); + a6=(a6+a203); + a203=(a10*a135); + a122=(a38*a122); + a203=(a203+a122); + a122=(a5*a203); + a6=(a6+a122); + a122=(a10*a135); + a100=(a44*a100); + a122=(a122+a100); + a100=(a5*a122); + a6=(a6+a100); + a74=(a44*a74); + a74=(a135+a74); + a100=(a5*a74); + a6=(a6+a100); + a100=(a5*a6); + a202=(a4*a202); + a820=(a5*a202); + a100=(a100+a820); + a820=(a10*a202); + a127=(a38*a127); + a820=(a820+a127); + a127=(a5*a820); + a100=(a100+a127); + a127=(a10*a202); + a118=(a44*a118); + a127=(a127+a118); + a118=(a5*a127); + a100=(a100+a118); + a128=(a44*a128); + a128=(a202+a128); + a118=(a5*a128); + a100=(a100+a118); + a118=(a5*a100); + a109=(a4*a109); + a152=(a5*a109); + a118=(a118+a152); + a152=(a10*a109); + a132=(a38*a132); + a152=(a152+a132); + a132=(a5*a152); + a118=(a118+a132); + a132=(a10*a109); + a121=(a44*a121); + a132=(a132+a121); + a121=(a5*a132); + a118=(a118+a121); + a116=(a44*a116); + a116=(a109+a116); + a121=(a5*a116); + a118=(a118+a121); + a121=(a5*a118); + a147=(a4*a147); + a110=(a5*a147); + a121=(a121+a110); + a110=(a10*a147); + a130=(a38*a130); + a110=(a110+a130); + a130=(a5*a110); + a121=(a121+a130); + a130=(a10*a147); + a60=(a44*a60); + a130=(a130+a60); + a60=(a5*a130); + a121=(a121+a60); + a84=(a44*a84); + a84=(a147+a84); + a60=(a5*a84); + a121=(a121+a60); + a60=(a5*a121); + a119=(a4*a119); + a120=(a5*a119); + a60=(a60+a120); + a120=(a10*a119); + a56=(a38*a56); + a120=(a120+a56); + a56=(a5*a120); + a60=(a60+a56); + a56=(a10*a119); + a126=(a44*a126); + a56=(a56+a126); + a126=(a5*a56); + a60=(a60+a126); + a198=(a44*a198); + a198=(a119+a198); + a126=(a5*a198); + a60=(a60+a126); + a126=(a5*a60); + a153=(a4*a153); + a92=(a5*a153); + a126=(a126+a92); + a92=(a10*a153); + a159=(a38*a159); + a92=(a92+a159); + a159=(a5*a92); + a126=(a126+a159); + a159=(a10*a153); + a112=(a44*a112); + a159=(a159+a112); + a112=(a5*a159); + a126=(a126+a112); + a131=(a44*a131); + a131=(a153+a131); + a112=(a5*a131); + a126=(a126+a112); + a126=(a5*a126); + a140=(a4*a140); + a112=(a5*a140); + a126=(a126+a112); + a112=(a10*a140); + a143=(a38*a143); + a112=(a112+a143); + a112=(a5*a112); + a126=(a126+a112); + a112=(a10*a140); + a141=(a44*a141); + a112=(a112+a141); + a112=(a5*a112); + a126=(a126+a112); + a138=(a44*a138); + a140=(a140+a138); + a140=(a5*a140); + a126=(a126+a140); + a140=(a5*a126); + a111=(a4*a111); + a138=(a5*a111); + a140=(a140+a138); + a138=(a10*a111); + a137=(a38*a137); + a138=(a138+a137); + a137=(a5*a138); + a140=(a140+a137); + a137=(a10*a111); + a142=(a44*a142); + a137=(a137+a142); + a142=(a5*a137); + a140=(a140+a142); + a148=(a44*a148); + a148=(a111+a148); + a142=(a5*a148); + a140=(a140+a142); + a140=(a8*a140); + a142=(a50*a34); + a140=(a140-a142); + a112=(a50*a34); + a140=(a140-a112); + if (res[0]!=0) res[0][123]=a140; + a73=(a28*a73); + a113=(a113*a73); + a67=(a42*a67); + a145=(a145*a67); + a113=(a113+a145); + a94=(a42*a94); + a151=(a151*a94); + a113=(a113+a151); + a125=(a28*a125); + a136=(a136*a125); + a113=(a113-a136); + a114=(a42*a114); + a139=(a139*a114); + a113=(a113-a139); + a9=(a42*a9); + a144=(a144*a9); + a113=(a113-a144); + a113=(-a113); + if (res[0]!=0) res[0][124]=a113; + a126=(a8*a126); + a111=(a8*a111); + a126=(a126+a111); + a138=(a8*a138); + a126=(a126+a138); + a137=(a8*a137); + a126=(a126+a137); + a148=(a8*a148); + a126=(a126+a148); + a148=(a24*a34); + a126=(a126-a148); + a148=(a50*a34); + a126=(a126+a148); + a126=(a126+a142); + a126=(a126+a112); + a112=(a10*a34); + a126=(a126+a112); + if (res[0]!=0) res[0][125]=a126; + a126=cos(a171); + a112=(a183*a126); + a142=cos(a156); + a148=(a193*a142); + a112=(a112+a148); + a148=cos(a61); + a137=(a199*a148); + a112=(a112+a137); + a137=cos(a124); + a137=(a183*a137); + a112=(a112+a137); + a171=sin(a171); + a137=(a184*a171); + a112=(a112-a137); + a156=sin(a156); + a137=(a187*a156); + a112=(a112-a137); + a61=sin(a61); + a137=(a192*a61); + a112=(a112-a137); + a124=sin(a124); + a124=(a184*a124); + a112=(a112-a124); + a112=(-a112); + if (res[0]!=0) res[0][126]=a112; + a112=(a2*a126); + a112=(a183*a112); + a124=(a29*a142); + a124=(a193*a124); + a112=(a112+a124); + a124=(a29*a148); + a124=(a199*a124); + a112=(a112+a124); + a124=(a2*a171); + a124=(a184*a124); + a112=(a112-a124); + a124=(a29*a156); + a124=(a187*a124); + a112=(a112-a124); + a124=(a29*a61); + a124=(a192*a124); + a112=(a112-a124); + a112=(-a112); + if (res[0]!=0) res[0][127]=a112; + a112=(a51*a126); + a112=(a183*a112); + a124=(a32*a142); + a124=(a193*a124); + a112=(a112+a124); + a124=(a32*a148); + a124=(a199*a124); + a112=(a112+a124); + a124=(a51*a171); + a124=(a184*a124); + a112=(a112-a124); + a124=(a32*a156); + a124=(a187*a124); + a112=(a112-a124); + a124=(a32*a61); + a124=(a192*a124); + a112=(a112-a124); + a112=(-a112); + if (res[0]!=0) res[0][128]=a112; + a89=(a89-a90); + if (res[0]!=0) res[0][129]=a89; + a89=(a41*a126); + a89=(a183*a89); + a90=(a35*a142); + a90=(a193*a90); + a89=(a89+a90); + a90=(a35*a148); + a90=(a199*a90); + a89=(a89+a90); + a90=(a41*a171); + a90=(a184*a90); + a89=(a89-a90); + a90=(a35*a156); + a90=(a187*a90); + a89=(a89-a90); + a90=(a35*a61); + a90=(a192*a90); + a89=(a89-a90); + a89=(-a89); + if (res[0]!=0) res[0][130]=a89; + a89=(a10*a34); + a90=(a3*a34); + a112=(a10*a34); + a90=(a90-a112); + a89=(a89-a90); + a90=(a10*a34); + a89=(a89+a90); + a90=(a10*a34); + a89=(a89+a90); + if (res[0]!=0) res[0][131]=a89; + a179=(a28*a179); + a157=(a157*a179); + a154=(a28*a154); + a179=(a161*a154); + a177=(a28*a177); + a89=(a162*a177); + a179=(a179+a89); + a89=(a179/a174); + a164=(a164*a179); + a179=(a163*a154); + a90=(a167*a177); + a179=(a179-a90); + a168=(a168*a179); + a164=(a164+a168); + a134=(a134*a164); + a89=(a89-a134); + a89=(a150*a89); + a167=(a167*a89); + a179=(a179/a174); + a173=(a173*a164); + a179=(a179-a173); + a150=(a150*a179); + a162=(a162*a150); + a167=(a167+a162); + a178=(a178*a167); + a157=(a157+a178); + a161=(a161*a150); + a163=(a163*a89); + a161=(a161-a163); + a181=(a181*a161); + a123=(a28*a123); + a182=(a182*a123); + a181=(a181-a182); + a157=(a157+a181); + a186=(a28*a186); + a185=(a185*a186); + a189=(a28*a189); + a188=(a188*a189); + a185=(a185+a188); + a188=(a157+a185); + a191=(a42*a191); + a190=(a190*a191); + a195=(a42*a195); + a194=(a194*a195); + a190=(a190+a194); + a188=(a188+a190); + a197=(a42*a197); + a196=(a196*a197); + a201=(a42*a201); + a200=(a200*a201); + a196=(a196+a200); + a188=(a188+a196); + if (res[0]!=0) res[0][132]=a188; + a188=(a41*a229); + a188=(a207*a188); + a200=(a41*a205); + a201=(a180*a200); + a197=(a41*a227); + a194=(a212*a197); + a201=(a201+a194); + a194=(a201/a224); + a201=(a214*a201); + a195=(a213*a200); + a191=(a217*a197); + a195=(a195-a191); + a191=(a218*a195); + a201=(a201+a191); + a191=(a204*a201); + a194=(a194-a191); + a194=(a206*a194); + a191=(a217*a194); + a195=(a195/a224); + a201=(a223*a201); + a195=(a195-a201); + a195=(a206*a195); + a201=(a212*a195); + a191=(a191+a201); + a191=(a228*a191); + a188=(a188+a191); + a191=(a180*a195); + a201=(a213*a194); + a191=(a191-a201); + a191=(a231*a191); + a201=(a41*a175); + a201=(a226*a201); + a191=(a191-a201); + a188=(a188+a191); + a191=(a41*a234); + a191=(a233*a191); + a201=(a41*a237); + a201=(a236*a201); + a191=(a191+a201); + a201=(a188+a191); + a189=(a35*a239); + a189=(a238*a189); + a186=(a35*a243); + a186=(a242*a186); + a189=(a189+a186); + a201=(a201+a189); + a186=(a35*a245); + a186=(a244*a186); + a181=(a35*a249); + a181=(a248*a181); + a186=(a186+a181); + a201=(a201+a186); + a181=(a172*a154); + a182=(a166*a89); + a181=(a181+a182); + a211=(a211*a181); + a201=(a201-a211); + a89=(a158*a89); + a172=(a172*a177); + a89=(a89-a172); + a169=(a169*a89); + a201=(a201+a169); + a158=(a158*a150); + a177=(a165*a177); + a158=(a158-a177); + a155=(a155*a158); + a201=(a201-a155); + a165=(a165*a154); + a166=(a166*a150); + a165=(a165+a166); + a176=(a176*a165); + a201=(a201-a176); + if (res[0]!=0) res[0][133]=a201; + a201=(a51*a275); + a201=(a253*a201); + a176=(a51*a250); + a165=(a257*a176); + a166=(a51*a273); + a150=(a258*a166); + a165=(a165+a150); + a150=(a165/a270); + a165=(a260*a165); + a154=(a259*a176); + a155=(a263*a166); + a154=(a154-a155); + a155=(a264*a154); + a165=(a165+a155); + a155=(a230*a165); + a150=(a150-a155); + a150=(a246*a150); + a155=(a263*a150); + a154=(a154/a270); + a165=(a269*a165); + a154=(a154-a165); + a154=(a246*a154); + a165=(a258*a154); + a155=(a155+a165); + a155=(a274*a155); + a201=(a201+a155); + a155=(a257*a154); + a165=(a259*a150); + a155=(a155-a165); + a155=(a277*a155); + a165=(a51*a219); + a165=(a278*a165); + a155=(a155-a165); + a201=(a201+a155); + a155=(a51*a282); + a155=(a281*a155); + a165=(a51*a285); + a165=(a284*a165); + a155=(a155+a165); + a165=(a201+a155); + a158=(a32*a287); + a158=(a286*a158); + a177=(a32*a291); + a177=(a290*a177); + a158=(a158+a177); + a165=(a165+a158); + a177=(a32*a293); + a177=(a292*a177); + a169=(a32*a297); + a169=(a296*a169); + a177=(a177+a169); + a165=(a165+a177); + a169=(a222*a200); + a89=(a216*a194); + a169=(a169+a89); + a169=(a294*a169); + a165=(a165-a169); + a194=(a208*a194); + a169=(a222*a197); + a194=(a194-a169); + a194=(a298*a194); + a165=(a165+a194); + a194=(a208*a195); + a197=(a215*a197); + a194=(a194-a197); + a194=(a255*a194); + a165=(a165-a194); + a200=(a215*a200); + a195=(a216*a195); + a200=(a200+a195); + a200=(a299*a200); + a165=(a165-a200); + if (res[0]!=0) res[0][134]=a165; + a165=(a14*a325); + a165=(a303*a165); + a200=(a14*a301); + a195=(a276*a200); + a194=(a14*a323); + a197=(a308*a194); + a195=(a195+a197); + a197=(a195/a320); + a195=(a310*a195); + a169=(a309*a200); + a89=(a313*a194); + a169=(a169-a89); + a89=(a314*a169); + a195=(a195+a89); + a89=(a300*a195); + a197=(a197-a89); + a197=(a302*a197); + a89=(a313*a197); + a169=(a169/a320); + a195=(a319*a195); + a169=(a169-a195); + a169=(a302*a169); + a195=(a308*a169); + a89=(a89+a195); + a89=(a324*a89); + a165=(a165+a89); + a89=(a276*a169); + a195=(a309*a197); + a89=(a89-a195); + a89=(a327*a89); + a195=(a14*a271); + a195=(a322*a195); + a89=(a89-a195); + a165=(a165+a89); + a89=(a2*a330); + a89=(a329*a89); + a195=(a2*a333); + a195=(a332*a195); + a89=(a89+a195); + a195=(a165+a89); + a172=(a29*a335); + a172=(a334*a172); + a211=(a29*a339); + a211=(a338*a211); + a172=(a172+a211); + a195=(a195+a172); + a211=(a29*a341); + a211=(a340*a211); + a181=(a29*a345); + a181=(a344*a181); + a211=(a211+a181); + a195=(a195+a211); + a181=(a268*a176); + a182=(a262*a150); + a181=(a181+a182); + a181=(a307*a181); + a195=(a195-a181); + a150=(a254*a150); + a181=(a268*a166); + a150=(a150-a181); + a150=(a265*a150); + a195=(a195+a150); + a150=(a254*a154); + a166=(a261*a166); + a150=(a150-a166); + a150=(a251*a150); + a195=(a195-a150); + a176=(a261*a176); + a154=(a262*a154); + a176=(a176+a154); + a176=(a272*a176); + a195=(a195-a176); + if (res[0]!=0) res[0][135]=a195; + a195=(a20*a371); + a195=(a349*a195); + a176=(a20*a346); + a154=(a353*a176); + a150=(a20*a369); + a166=(a354*a150); + a154=(a154+a166); + a166=(a154/a366); + a154=(a356*a154); + a181=(a355*a176); + a182=(a359*a150); + a181=(a181-a182); + a182=(a360*a181); + a154=(a154+a182); + a182=(a326*a154); + a166=(a166-a182); + a166=(a342*a166); + a182=(a359*a166); + a181=(a181/a366); + a154=(a365*a154); + a181=(a181-a154); + a181=(a342*a181); + a154=(a354*a181); + a182=(a182+a154); + a182=(a370*a182); + a195=(a195+a182); + a182=(a353*a181); + a154=(a355*a166); + a182=(a182-a154); + a182=(a373*a182); + a154=(a20*a315); + a154=(a374*a154); + a182=(a182-a154); + a195=(a195+a182); + a182=(a15*a378); + a182=(a377*a182); + a154=(a15*a381); + a154=(a380*a154); + a182=(a182+a154); + a154=(a195+a182); + a123=(a21*a383); + a123=(a382*a123); + a161=(a21*a387); + a161=(a386*a161); + a123=(a123+a161); + a154=(a154+a123); + a161=(a21*a389); + a161=(a388*a161); + a163=(a21*a393); + a163=(a392*a163); + a161=(a161+a163); + a154=(a154+a161); + a163=(a318*a200); + a178=(a312*a197); + a163=(a163+a178); + a163=(a390*a163); + a154=(a154-a163); + a197=(a304*a197); + a163=(a318*a194); + a197=(a197-a163); + a197=(a394*a197); + a154=(a154+a197); + a197=(a304*a169); + a194=(a311*a194); + a197=(a197-a194); + a197=(a351*a197); + a154=(a154-a197); + a200=(a311*a200); + a169=(a312*a169); + a200=(a200+a169); + a200=(a395*a200); + a154=(a154-a200); + if (res[0]!=0) res[0][136]=a154; + a154=(a18*a421); + a154=(a399*a154); + a200=(a18*a397); + a169=(a372*a200); + a197=(a18*a419); + a194=(a404*a197); + a169=(a169+a194); + a194=(a169/a416); + a169=(a406*a169); + a163=(a405*a200); + a178=(a409*a197); + a163=(a163-a178); + a178=(a410*a163); + a169=(a169+a178); + a178=(a396*a169); + a194=(a194-a178); + a194=(a398*a194); + a178=(a409*a194); + a163=(a163/a416); + a169=(a415*a169); + a163=(a163-a169); + a163=(a398*a163); + a169=(a404*a163); + a178=(a178+a169); + a178=(a420*a178); + a154=(a154+a178); + a178=(a372*a163); + a169=(a405*a194); + a178=(a178-a169); + a178=(a423*a178); + a169=(a18*a367); + a169=(a418*a169); + a178=(a178-a169); + a154=(a154+a178); + a178=(a18*a426); + a178=(a425*a178); + a169=(a18*a429); + a169=(a428*a169); + a178=(a178+a169); + a169=(a154+a178); + a167=(a748*a431); + a167=(a430*a167); + a162=(a748*a435); + a162=(a434*a162); + a167=(a167+a162); + a169=(a169+a167); + a162=(a748*a437); + a162=(a436*a162); + a179=(a748*a441); + a179=(a440*a179); + a162=(a162+a179); + a169=(a169+a162); + a179=(a364*a176); + a173=(a358*a166); + a179=(a179+a173); + a179=(a403*a179); + a169=(a169-a179); + a166=(a350*a166); + a179=(a364*a150); + a166=(a166-a179); + a166=(a361*a166); + a169=(a169+a166); + a166=(a350*a181); + a150=(a357*a150); + a166=(a166-a150); + a166=(a347*a166); + a169=(a169-a166); + a176=(a357*a176); + a181=(a358*a181); + a176=(a176+a181); + a176=(a368*a176); + a169=(a169-a176); + if (res[0]!=0) res[0][137]=a169; + a169=(a23*a467); + a169=(a445*a169); + a176=(a23*a442); + a181=(a449*a176); + a166=(a23*a465); + a150=(a450*a166); + a181=(a181+a150); + a150=(a181/a462); + a181=(a452*a181); + a179=(a451*a176); + a173=(a455*a166); + a179=(a179-a173); + a173=(a456*a179); + a181=(a181+a173); + a173=(a422*a181); + a150=(a150-a173); + a150=(a438*a150); + a173=(a455*a150); + a179=(a179/a462); + a181=(a461*a181); + a179=(a179-a181); + a179=(a438*a179); + a181=(a450*a179); + a173=(a173+a181); + a173=(a466*a173); + a169=(a169+a173); + a173=(a449*a179); + a181=(a451*a150); + a173=(a173-a181); + a173=(a469*a173); + a181=(a23*a411); + a181=(a470*a181); + a173=(a173-a181); + a169=(a169+a173); + a173=(a23*a474); + a173=(a473*a173); + a181=(a23*a477); + a181=(a476*a181); + a173=(a173+a181); + a181=(a169+a173); + a164=(a759*a479); + a164=(a478*a164); + a174=(a759*a483); + a174=(a482*a174); + a164=(a164+a174); + a181=(a181+a164); + a174=(a759*a485); + a174=(a484*a174); + a134=(a759*a489); + a134=(a488*a134); + a174=(a174+a134); + a181=(a181+a174); + a134=(a414*a200); + a168=(a408*a194); + a134=(a134+a168); + a134=(a486*a134); + a181=(a181-a134); + a194=(a400*a194); + a134=(a414*a197); + a194=(a194-a134); + a194=(a490*a194); + a181=(a181+a194); + a194=(a400*a163); + a197=(a407*a197); + a194=(a194-a197); + a194=(a447*a194); + a181=(a181-a194); + a200=(a407*a200); + a163=(a408*a163); + a200=(a200+a163); + a200=(a491*a200); + a181=(a181-a200); + if (res[0]!=0) res[0][138]=a181; + a181=(a27*a517); + a181=(a495*a181); + a200=(a27*a493); + a163=(a468*a200); + a194=(a27*a515); + a197=(a500*a194); + a163=(a163+a197); + a197=(a163/a512); + a163=(a502*a163); + a134=(a501*a200); + a168=(a505*a194); + a134=(a134-a168); + a168=(a506*a134); + a163=(a163+a168); + a168=(a492*a163); + a197=(a197-a168); + a197=(a494*a197); + a168=(a505*a197); + a134=(a134/a512); + a163=(a511*a163); + a134=(a134-a163); + a134=(a494*a134); + a163=(a500*a134); + a168=(a168+a163); + a168=(a516*a168); + a181=(a181+a168); + a168=(a468*a134); + a163=(a501*a197); + a168=(a168-a163); + a168=(a519*a168); + a163=(a27*a463); + a163=(a514*a163); + a168=(a168-a163); + a181=(a181+a168); + a168=(a760*a522); + a168=(a521*a168); + a163=(a760*a525); + a163=(a524*a163); + a168=(a168+a163); + a163=(a181+a168); + a90=(a750*a527); + a90=(a526*a90); + a112=(a750*a531); + a112=(a530*a112); + a90=(a90+a112); + a163=(a163+a90); + a112=(a750*a533); + a112=(a532*a112); + a124=(a750*a537); + a124=(a536*a124); + a112=(a112+a124); + a163=(a163+a112); + a124=(a460*a176); + a137=(a454*a150); + a124=(a124+a137); + a124=(a499*a124); + a163=(a163-a124); + a150=(a446*a150); + a124=(a460*a166); + a150=(a150-a124); + a150=(a457*a150); + a163=(a163+a150); + a150=(a446*a179); + a166=(a453*a166); + a150=(a150-a166); + a150=(a443*a150); + a163=(a163-a150); + a176=(a453*a176); + a179=(a454*a179); + a176=(a176+a179); + a176=(a464*a176); + a163=(a163-a176); + if (res[0]!=0) res[0][139]=a163; + a163=(a703*a563); + a163=(a541*a163); + a176=(a703*a538); + a179=(a545*a176); + a150=(a703*a561); + a166=(a546*a150); + a179=(a179+a166); + a166=(a179/a558); + a179=(a548*a179); + a124=(a547*a176); + a137=(a551*a150); + a124=(a124-a137); + a137=(a552*a124); + a179=(a179+a137); + a137=(a518*a179); + a166=(a166-a137); + a166=(a534*a166); + a137=(a551*a166); + a124=(a124/a558); + a179=(a557*a179); + a124=(a124-a179); + a124=(a534*a124); + a179=(a546*a124); + a137=(a137+a179); + a137=(a562*a137); + a163=(a163+a137); + a137=(a545*a124); + a179=(a547*a166); + a137=(a137-a179); + a137=(a565*a137); + a179=(a703*a507); + a179=(a560*a179); + a137=(a137-a179); + a163=(a163+a137); + a137=(a703*a568); + a137=(a567*a137); + a179=(a703*a571); + a179=(a570*a179); + a137=(a137+a179); + a179=(a163+a137); + a138=(a747*a573); + a138=(a572*a138); + a111=(a747*a577); + a111=(a576*a111); + a138=(a138+a111); + a179=(a179+a138); + a111=(a747*a579); + a111=(a578*a111); + a113=(a747*a583); + a113=(a582*a113); + a111=(a111+a113); + a179=(a179+a111); + a113=(a510*a200); + a144=(a504*a197); + a113=(a113+a144); + a113=(a580*a113); + a179=(a179-a113); + a197=(a496*a197); + a113=(a510*a194); + a197=(a197-a113); + a197=(a584*a197); + a179=(a179+a197); + a197=(a496*a134); + a194=(a503*a194); + a197=(a197-a194); + a197=(a564*a197); + a179=(a179-a197); + a200=(a503*a200); + a134=(a504*a134); + a200=(a200+a134); + a200=(a585*a200); + a179=(a179-a200); + if (res[0]!=0) res[0][140]=a179; + a179=(a749*a611); + a179=(a589*a179); + a200=(a749*a587); + a134=(a593*a200); + a197=(a749*a609); + a194=(a594*a197); + a134=(a134+a194); + a194=(a134/a606); + a134=(a596*a134); + a113=(a595*a200); + a144=(a599*a197); + a113=(a113-a144); + a144=(a600*a113); + a134=(a134+a144); + a144=(a586*a134); + a194=(a194-a144); + a194=(a588*a194); + a144=(a599*a194); + a113=(a113/a606); + a134=(a605*a134); + a113=(a113-a134); + a113=(a588*a113); + a134=(a594*a113); + a144=(a144+a134); + a144=(a610*a144); + a179=(a179+a144); + a144=(a593*a113); + a134=(a595*a194); + a144=(a144-a134); + a144=(a613*a144); + a134=(a749*a553); + a134=(a614*a134); + a144=(a144-a134); + a179=(a179+a144); + a144=(a764*a618); + a144=(a617*a144); + a134=(a764*a621); + a134=(a620*a134); + a144=(a144+a134); + a134=(a179+a144); + a9=(a766*a623); + a9=(a622*a9); + a139=(a766*a627); + a139=(a626*a139); + a9=(a9+a139); + a134=(a134+a9); + a139=(a766*a629); + a139=(a628*a139); + a114=(a766*a633); + a114=(a632*a114); + a139=(a139+a114); + a134=(a134+a139); + a114=(a556*a176); + a136=(a550*a166); + a114=(a114+a136); + a114=(a630*a114); + a134=(a134-a114); + a166=(a542*a166); + a114=(a556*a150); + a166=(a166-a114); + a166=(a634*a166); + a134=(a134+a166); + a166=(a542*a124); + a150=(a549*a150); + a166=(a166-a150); + a166=(a591*a166); + a134=(a134-a166); + a176=(a549*a176); + a124=(a550*a124); + a176=(a176+a124); + a176=(a635*a176); + a134=(a134-a176); + if (res[0]!=0) res[0][141]=a134; + a134=(a765*a661); + a134=(a639*a134); + a176=(a765*a637); + a124=(a612*a176); + a166=(a765*a659); + a150=(a644*a166); + a124=(a124+a150); + a150=(a124/a656); + a124=(a646*a124); + a114=(a645*a176); + a136=(a649*a166); + a114=(a114-a136); + a136=(a650*a114); + a124=(a124+a136); + a136=(a636*a124); + a150=(a150-a136); + a150=(a638*a150); + a136=(a649*a150); + a114=(a114/a656); + a124=(a655*a124); + a114=(a114-a124); + a114=(a638*a114); + a124=(a644*a114); + a136=(a136+a124); + a136=(a660*a136); + a134=(a134+a136); + a136=(a612*a114); + a124=(a645*a150); + a136=(a136-a124); + a136=(a663*a136); + a124=(a765*a607); + a124=(a658*a124); + a136=(a136-a124); + a134=(a134+a136); + a136=(a765*a666); + a136=(a665*a136); + a124=(a765*a669); + a124=(a668*a124); + a136=(a136+a124); + a124=(a134+a136); + a125=(a772*a671); + a125=(a670*a125); + a151=(a772*a675); + a151=(a674*a151); + a125=(a125+a151); + a124=(a124+a125); + a151=(a772*a677); + a151=(a676*a151); + a94=(a772*a681); + a94=(a680*a94); + a151=(a151+a94); + a124=(a124+a151); + a94=(a604*a200); + a145=(a598*a194); + a94=(a94+a145); + a94=(a643*a94); + a124=(a124-a94); + a194=(a590*a194); + a94=(a604*a197); + a194=(a194-a94); + a194=(a601*a194); + a124=(a124+a194); + a194=(a590*a113); + a197=(a597*a197); + a194=(a194-a197); + a194=(a559*a194); + a124=(a124-a194); + a200=(a597*a200); + a113=(a598*a113); + a200=(a200+a113); + a200=(a608*a200); + a124=(a124-a200); + if (res[0]!=0) res[0][142]=a124; + a124=(a738*a693); + a124=(a700*a124); + a200=(a738*a706); + a113=(a688*a200); + a194=(a738*a687); + a197=(a692*a194); + a113=(a113+a197); + a197=(a113/a686); + a113=(a684*a113); + a94=(a696*a200); + a145=(a689*a194); + a94=(a94-a145); + a145=(a702*a94); + a113=(a113+a145); + a145=(a691*a113); + a197=(a197-a145); + a197=(a662*a197); + a145=(a689*a197); + a94=(a94/a686); + a113=(a685*a113); + a94=(a94-a113); + a94=(a662*a94); + a113=(a692*a94); + a145=(a145+a113); + a145=(a697*a145); + a124=(a124+a145); + a145=(a688*a94); + a113=(a696*a197); + a145=(a145-a113); + a145=(a708*a145); + a738=(a738*a699); + a738=(a709*a738); + a145=(a145-a738); + a124=(a124+a145); + a145=(a777*a714); + a145=(a713*a145); + a738=(a777*a717); + a738=(a716*a738); + a145=(a145+a738); + a738=(a124+a145); + a113=(a779*a720); + a113=(a719*a113); + a67=(a779*a724); + a67=(a723*a67); + a113=(a113+a67); + a738=(a738+a113); + a67=(a779*a727); + a67=(a726*a67); + a73=(a779*a731); + a73=(a730*a73); + a67=(a67+a73); + a738=(a738+a67); + a73=(a654*a176); + a140=(a648*a150); + a73=(a73+a140); + a73=(a732*a73); + a738=(a738-a73); + a150=(a640*a150); + a73=(a654*a166); + a150=(a150-a73); + a150=(a733*a150); + a738=(a738+a150); + a150=(a640*a114); + a166=(a647*a166); + a150=(a150-a166); + a150=(a682*a150); + a738=(a738-a150); + a176=(a647*a176); + a114=(a648*a114); + a176=(a176+a114); + a176=(a734*a176); + a738=(a738-a176); + if (res[0]!=0) res[0][143]=a738; + a738=(a690*a197); + a176=(a678*a194); + a738=(a738-a176); + a738=(a711*a738); + a176=(a678*a200); + a197=(a657*a197); + a176=(a176+a197); + a176=(a751*a176); + a738=(a738-a176); + a176=(a690*a94); + a194=(a701*a194); + a176=(a176-a194); + a176=(a742*a176); + a738=(a738-a176); + a200=(a701*a200); + a94=(a657*a94); + a200=(a200+a94); + a200=(a705*a200); + a738=(a738-a200); + if (res[0]!=0) res[0][144]=a738; + a746=(a8*a746); + a55=(a8*a55); + a746=(a746+a55); + a776=(a8*a776); + a746=(a746+a776); + a49=(a8*a49); + a746=(a746+a49); + a16=(a8*a16); + a746=(a746+a16); + if (res[0]!=0) res[0][145]=a746; + a52=(a8*a52); + a105=(a8*a105); + a52=(a52+a105); + a818=(a8*a818); + a52=(a52+a818); + a62=(a8*a62); + a52=(a52+a62); + a82=(a8*a82); + a52=(a52+a82); + if (res[0]!=0) res[0][146]=a52; + a124=(a4*a124); + a52=(a5*a124); + a82=(a10*a124); + a145=(a38*a145); + a82=(a82+a145); + a145=(a5*a82); + a52=(a52+a145); + a145=(a10*a124); + a113=(a44*a113); + a145=(a145+a113); + a113=(a5*a145); + a52=(a52+a113); + a67=(a44*a67); + a67=(a124+a67); + a113=(a5*a67); + a52=(a52+a113); + a113=(a5*a52); + a134=(a4*a134); + a62=(a5*a134); + a113=(a113+a62); + a62=(a10*a134); + a136=(a38*a136); + a62=(a62+a136); + a136=(a5*a62); + a113=(a113+a136); + a136=(a10*a134); + a125=(a44*a125); + a136=(a136+a125); + a125=(a5*a136); + a113=(a113+a125); + a151=(a44*a151); + a151=(a134+a151); + a125=(a5*a151); + a113=(a113+a125); + a125=(a5*a113); + a179=(a4*a179); + a818=(a5*a179); + a125=(a125+a818); + a818=(a10*a179); + a144=(a38*a144); + a818=(a818+a144); + a144=(a5*a818); + a125=(a125+a144); + a144=(a10*a179); + a9=(a44*a9); + a144=(a144+a9); + a9=(a5*a144); + a125=(a125+a9); + a139=(a44*a139); + a139=(a179+a139); + a9=(a5*a139); + a125=(a125+a9); + a9=(a5*a125); + a163=(a4*a163); + a105=(a5*a163); + a9=(a9+a105); + a105=(a10*a163); + a137=(a38*a137); + a105=(a105+a137); + a137=(a5*a105); + a9=(a9+a137); + a137=(a10*a163); + a138=(a44*a138); + a137=(a137+a138); + a138=(a5*a137); + a9=(a9+a138); + a111=(a44*a111); + a111=(a163+a111); + a138=(a5*a111); + a9=(a9+a138); + a138=(a5*a9); + a181=(a4*a181); + a746=(a5*a181); + a138=(a138+a746); + a746=(a10*a181); + a168=(a38*a168); + a746=(a746+a168); + a168=(a5*a746); + a138=(a138+a168); + a168=(a10*a181); + a90=(a44*a90); + a168=(a168+a90); + a90=(a5*a168); + a138=(a138+a90); + a112=(a44*a112); + a112=(a181+a112); + a90=(a5*a112); + a138=(a138+a90); + a90=(a5*a138); + a169=(a4*a169); + a16=(a5*a169); + a90=(a90+a16); + a16=(a10*a169); + a173=(a38*a173); + a16=(a16+a173); + a173=(a5*a16); + a90=(a90+a173); + a173=(a10*a169); + a164=(a44*a164); + a173=(a173+a164); + a164=(a5*a173); + a90=(a90+a164); + a174=(a44*a174); + a174=(a169+a174); + a164=(a5*a174); + a90=(a90+a164); + a164=(a5*a90); + a154=(a4*a154); + a49=(a5*a154); + a164=(a164+a49); + a49=(a10*a154); + a178=(a38*a178); + a49=(a49+a178); + a178=(a5*a49); + a164=(a164+a178); + a178=(a10*a154); + a167=(a44*a167); + a178=(a178+a167); + a167=(a5*a178); + a164=(a164+a167); + a162=(a44*a162); + a162=(a154+a162); + a167=(a5*a162); + a164=(a164+a167); + a167=(a5*a164); + a195=(a4*a195); + a776=(a5*a195); + a167=(a167+a776); + a776=(a10*a195); + a182=(a38*a182); + a776=(a776+a182); + a182=(a5*a776); + a167=(a167+a182); + a182=(a10*a195); + a123=(a44*a123); + a182=(a182+a123); + a123=(a5*a182); + a167=(a167+a123); + a161=(a44*a161); + a161=(a195+a161); + a123=(a5*a161); + a167=(a167+a123); + a123=(a5*a167); + a165=(a4*a165); + a55=(a5*a165); + a123=(a123+a55); + a55=(a10*a165); + a89=(a38*a89); + a55=(a55+a89); + a89=(a5*a55); + a123=(a123+a89); + a89=(a10*a165); + a172=(a44*a172); + a89=(a89+a172); + a172=(a5*a89); + a123=(a123+a172); + a211=(a44*a211); + a211=(a165+a211); + a172=(a5*a211); + a123=(a123+a172); + a172=(a5*a123); + a201=(a4*a201); + a738=(a5*a201); + a172=(a172+a738); + a738=(a10*a201); + a155=(a38*a155); + a738=(a738+a155); + a155=(a5*a738); + a172=(a172+a155); + a155=(a10*a201); + a158=(a44*a158); + a155=(a155+a158); + a158=(a5*a155); + a172=(a172+a158); + a177=(a44*a177); + a177=(a201+a177); + a158=(a5*a177); + a172=(a172+a158); + a172=(a5*a172); + a188=(a4*a188); + a158=(a5*a188); + a172=(a172+a158); + a158=(a10*a188); + a191=(a38*a191); + a158=(a158+a191); + a158=(a5*a158); + a172=(a172+a158); + a158=(a10*a188); + a189=(a44*a189); + a158=(a158+a189); + a158=(a5*a158); + a172=(a172+a158); + a186=(a44*a186); + a188=(a188+a186); + a188=(a5*a188); + a172=(a172+a188); + a188=(a5*a172); + a157=(a4*a157); + a186=(a5*a157); + a188=(a188+a186); + a186=(a10*a157); + a185=(a38*a185); + a186=(a186+a185); + a185=(a5*a186); + a188=(a188+a185); + a185=(a10*a157); + a190=(a44*a190); + a185=(a185+a190); + a190=(a5*a185); + a188=(a188+a190); + a196=(a44*a196); + a196=(a157+a196); + a190=(a5*a196); + a188=(a188+a190); + a188=(a8*a188); + a190=(a50*a34); + a188=(a188-a190); + a158=(a50*a34); + a188=(a188-a158); + if (res[0]!=0) res[0][147]=a188; + a126=(a28*a126); + a183=(a183*a126); + a142=(a42*a142); + a193=(a193*a142); + a183=(a183+a193); + a148=(a42*a148); + a199=(a199*a148); + a183=(a183+a199); + a171=(a28*a171); + a184=(a184*a171); + a183=(a183-a184); + a156=(a42*a156); + a187=(a187*a156); + a183=(a183-a187); + a61=(a42*a61); + a192=(a192*a61); + a183=(a183-a192); + a183=(-a183); + if (res[0]!=0) res[0][148]=a183; + a172=(a8*a172); + a157=(a8*a157); + a172=(a172+a157); + a186=(a8*a186); + a172=(a172+a186); + a185=(a8*a185); + a172=(a172+a185); + a196=(a8*a196); + a172=(a172+a196); + a196=(a24*a34); + a172=(a172-a196); + a196=(a50*a34); + a172=(a172+a196); + a172=(a172+a190); + a172=(a172+a158); + a158=(a10*a34); + a172=(a172+a158); + if (res[0]!=0) res[0][149]=a172; + a172=sin(a221); + a172=(a172*a232); + a158=cos(a221); + a158=(a158*a209); + a172=(a172-a158); + a158=cos(a170); + a158=(a158*a209); + a172=(a172-a158); + a158=sin(a210); + a158=(a158*a235); + a190=cos(a210); + a190=(a190*a241); + a158=(a158-a190); + a172=(a172+a158); + a158=sin(a129); + a158=(a158*a240); + a190=cos(a129); + a190=(a190*a247); + a158=(a158-a190); + a172=(a172+a158); + a170=sin(a170); + a170=(a170*a232); + a172=(a172+a170); + if (res[0]!=0) res[0][150]=a172; + a172=cos(a221); + a170=(a15*a172); + a170=(a209*a170); + a158=cos(a210); + a190=(a21*a158); + a190=(a241*a190); + a170=(a170+a190); + a190=cos(a129); + a196=(a21*a190); + a196=(a247*a196); + a170=(a170+a196); + a221=sin(a221); + a196=(a15*a221); + a196=(a232*a196); + a170=(a170-a196); + a210=sin(a210); + a196=(a21*a210); + a196=(a235*a196); + a170=(a170-a196); + a129=sin(a129); + a196=(a21*a129); + a196=(a240*a196); + a170=(a170-a196); + a170=(-a170); + if (res[0]!=0) res[0][151]=a170; + a170=(a2*a172); + a170=(a209*a170); + a196=(a29*a158); + a196=(a241*a196); + a170=(a170+a196); + a196=(a29*a190); + a196=(a247*a196); + a170=(a170+a196); + a196=(a2*a221); + a196=(a232*a196); + a170=(a170-a196); + a196=(a29*a210); + a196=(a235*a196); + a170=(a170-a196); + a196=(a29*a129); + a196=(a240*a196); + a170=(a170-a196); + a170=(-a170); + if (res[0]!=0) res[0][152]=a170; + a170=(a51*a172); + a170=(a209*a170); + a196=(a32*a158); + a196=(a241*a196); + a170=(a170+a196); + a196=(a32*a190); + a196=(a247*a196); + a170=(a170+a196); + a196=(a51*a221); + a196=(a232*a196); + a170=(a170-a196); + a196=(a32*a210); + a196=(a235*a196); + a170=(a170-a196); + a196=(a32*a129); + a196=(a240*a196); + a170=(a170-a196); + a170=(-a170); + if (res[0]!=0) res[0][153]=a170; + a170=(a10*a34); + a196=(a10*a34); + a185=(a170+a196); + a185=(-a185); + if (res[0]!=0) res[0][154]=a185; + a185=(a41*a172); + a185=(a209*a185); + a186=(a35*a158); + a186=(a241*a186); + a185=(a185+a186); + a186=(a35*a190); + a186=(a247*a186); + a185=(a185+a186); + a186=(a41*a221); + a186=(a232*a186); + a185=(a185-a186); + a186=(a35*a210); + a186=(a235*a186); + a185=(a185-a186); + a186=(a35*a129); + a186=(a240*a186); + a185=(a185-a186); + a185=(-a185); + if (res[0]!=0) res[0][155]=a185; + a185=(a3*a34); + a186=(a10*a34); + a157=(a185-a186); + a170=(a170-a157); + a170=(a170+a196); + a196=(a10*a34); + a170=(a170+a196); + if (res[0]!=0) res[0][156]=a170; + a229=(a28*a229); + a207=(a207*a229); + a205=(a28*a205); + a229=(a180*a205); + a227=(a28*a227); + a170=(a212*a227); + a229=(a229+a170); + a170=(a229/a224); + a214=(a214*a229); + a229=(a213*a205); + a196=(a217*a227); + a229=(a229-a196); + a218=(a218*a229); + a214=(a214+a218); + a204=(a204*a214); + a170=(a170-a204); + a170=(a206*a170); + a217=(a217*a170); + a229=(a229/a224); + a223=(a223*a214); + a229=(a229-a223); + a206=(a206*a229); + a212=(a212*a206); + a217=(a217+a212); + a228=(a228*a217); + a207=(a207+a228); + a180=(a180*a206); + a213=(a213*a170); + a180=(a180-a213); + a231=(a231*a180); + a175=(a28*a175); + a226=(a226*a175); + a231=(a231-a226); + a207=(a207+a231); + a234=(a28*a234); + a233=(a233*a234); + a237=(a28*a237); + a236=(a236*a237); + a233=(a233+a236); + a236=(a207+a233); + a239=(a42*a239); + a238=(a238*a239); + a243=(a42*a243); + a242=(a242*a243); + a238=(a238+a242); + a236=(a236+a238); + a245=(a42*a245); + a244=(a244*a245); + a249=(a42*a249); + a248=(a248*a249); + a244=(a244+a248); + a236=(a236+a244); + if (res[0]!=0) res[0][157]=a236; + a236=(a41*a275); + a236=(a253*a236); + a248=(a41*a250); + a249=(a257*a248); + a245=(a41*a273); + a242=(a258*a245); + a249=(a249+a242); + a242=(a249/a270); + a249=(a260*a249); + a243=(a259*a248); + a239=(a263*a245); + a243=(a243-a239); + a239=(a264*a243); + a249=(a249+a239); + a239=(a230*a249); + a242=(a242-a239); + a242=(a246*a242); + a239=(a263*a242); + a243=(a243/a270); + a249=(a269*a249); + a243=(a243-a249); + a243=(a246*a243); + a249=(a258*a243); + a239=(a239+a249); + a239=(a274*a239); + a236=(a236+a239); + a239=(a257*a243); + a249=(a259*a242); + a239=(a239-a249); + a239=(a277*a239); + a249=(a41*a219); + a249=(a278*a249); + a239=(a239-a249); + a236=(a236+a239); + a239=(a41*a282); + a239=(a281*a239); + a249=(a41*a285); + a249=(a284*a249); + a239=(a239+a249); + a249=(a236+a239); + a237=(a35*a287); + a237=(a286*a237); + a234=(a35*a291); + a234=(a290*a234); + a237=(a237+a234); + a249=(a249+a237); + a234=(a35*a293); + a234=(a292*a234); + a231=(a35*a297); + a231=(a296*a231); + a234=(a234+a231); + a249=(a249+a234); + a231=(a222*a205); + a226=(a216*a170); + a231=(a231+a226); + a294=(a294*a231); + a249=(a249-a294); + a170=(a208*a170); + a222=(a222*a227); + a170=(a170-a222); + a298=(a298*a170); + a249=(a249+a298); + a208=(a208*a206); + a227=(a215*a227); + a208=(a208-a227); + a255=(a255*a208); + a249=(a249-a255); + a215=(a215*a205); + a216=(a216*a206); + a215=(a215+a216); + a299=(a299*a215); + a249=(a249-a299); + if (res[0]!=0) res[0][158]=a249; + a249=(a51*a325); + a249=(a303*a249); + a299=(a51*a301); + a215=(a276*a299); + a216=(a51*a323); + a206=(a308*a216); + a215=(a215+a206); + a206=(a215/a320); + a215=(a310*a215); + a205=(a309*a299); + a255=(a313*a216); + a205=(a205-a255); + a255=(a314*a205); + a215=(a215+a255); + a255=(a300*a215); + a206=(a206-a255); + a206=(a302*a206); + a255=(a313*a206); + a205=(a205/a320); + a215=(a319*a215); + a205=(a205-a215); + a205=(a302*a205); + a215=(a308*a205); + a255=(a255+a215); + a255=(a324*a255); + a249=(a249+a255); + a255=(a276*a205); + a215=(a309*a206); + a255=(a255-a215); + a255=(a327*a255); + a215=(a51*a271); + a215=(a322*a215); + a255=(a255-a215); + a249=(a249+a255); + a255=(a51*a330); + a255=(a329*a255); + a215=(a51*a333); + a215=(a332*a215); + a255=(a255+a215); + a215=(a249+a255); + a208=(a32*a335); + a208=(a334*a208); + a227=(a32*a339); + a227=(a338*a227); + a208=(a208+a227); + a215=(a215+a208); + a227=(a32*a341); + a227=(a340*a227); + a298=(a32*a345); + a298=(a344*a298); + a227=(a227+a298); + a215=(a215+a227); + a298=(a268*a248); + a170=(a262*a242); + a298=(a298+a170); + a298=(a307*a298); + a215=(a215-a298); + a242=(a254*a242); + a298=(a268*a245); + a242=(a242-a298); + a242=(a265*a242); + a215=(a215+a242); + a242=(a254*a243); + a245=(a261*a245); + a242=(a242-a245); + a242=(a251*a242); + a215=(a215-a242); + a248=(a261*a248); + a243=(a262*a243); + a248=(a248+a243); + a248=(a272*a248); + a215=(a215-a248); + if (res[0]!=0) res[0][159]=a215; + a215=(a14*a371); + a215=(a349*a215); + a248=(a14*a346); + a243=(a353*a248); + a242=(a14*a369); + a245=(a354*a242); + a243=(a243+a245); + a245=(a243/a366); + a243=(a356*a243); + a298=(a355*a248); + a170=(a359*a242); + a298=(a298-a170); + a170=(a360*a298); + a243=(a243+a170); + a170=(a326*a243); + a245=(a245-a170); + a245=(a342*a245); + a170=(a359*a245); + a298=(a298/a366); + a243=(a365*a243); + a298=(a298-a243); + a298=(a342*a298); + a243=(a354*a298); + a170=(a170+a243); + a170=(a370*a170); + a215=(a215+a170); + a170=(a353*a298); + a243=(a355*a245); + a170=(a170-a243); + a170=(a373*a170); + a243=(a14*a315); + a243=(a374*a243); + a170=(a170-a243); + a215=(a215+a170); + a170=(a2*a378); + a170=(a377*a170); + a243=(a2*a381); + a243=(a380*a243); + a170=(a170+a243); + a243=(a215+a170); + a222=(a29*a383); + a222=(a382*a222); + a294=(a29*a387); + a294=(a386*a294); + a222=(a222+a294); + a243=(a243+a222); + a294=(a29*a389); + a294=(a388*a294); + a231=(a29*a393); + a231=(a392*a231); + a294=(a294+a231); + a243=(a243+a294); + a231=(a318*a299); + a226=(a312*a206); + a231=(a231+a226); + a231=(a390*a231); + a243=(a243-a231); + a206=(a304*a206); + a231=(a318*a216); + a206=(a206-a231); + a206=(a394*a206); + a243=(a243+a206); + a206=(a304*a205); + a216=(a311*a216); + a206=(a206-a216); + a206=(a351*a206); + a243=(a243-a206); + a299=(a311*a299); + a205=(a312*a205); + a299=(a299+a205); + a299=(a395*a299); + a243=(a243-a299); + if (res[0]!=0) res[0][160]=a243; + a243=(a20*a421); + a243=(a399*a243); + a299=(a20*a397); + a205=(a372*a299); + a206=(a20*a419); + a216=(a404*a206); + a205=(a205+a216); + a216=(a205/a416); + a205=(a406*a205); + a231=(a405*a299); + a226=(a409*a206); + a231=(a231-a226); + a226=(a410*a231); + a205=(a205+a226); + a226=(a396*a205); + a216=(a216-a226); + a216=(a398*a216); + a226=(a409*a216); + a231=(a231/a416); + a205=(a415*a205); + a231=(a231-a205); + a231=(a398*a231); + a205=(a404*a231); + a226=(a226+a205); + a226=(a420*a226); + a243=(a243+a226); + a226=(a372*a231); + a205=(a405*a216); + a226=(a226-a205); + a226=(a423*a226); + a205=(a20*a367); + a205=(a418*a205); + a226=(a226-a205); + a243=(a243+a226); + a226=(a15*a426); + a226=(a425*a226); + a205=(a15*a429); + a205=(a428*a205); + a226=(a226+a205); + a205=(a243+a226); + a175=(a21*a431); + a175=(a430*a175); + a180=(a21*a435); + a180=(a434*a180); + a175=(a175+a180); + a205=(a205+a175); + a180=(a21*a437); + a180=(a436*a180); + a213=(a21*a441); + a213=(a440*a213); + a180=(a180+a213); + a205=(a205+a180); + a213=(a364*a248); + a228=(a358*a245); + a213=(a213+a228); + a213=(a403*a213); + a205=(a205-a213); + a245=(a350*a245); + a213=(a364*a242); + a245=(a245-a213); + a245=(a361*a245); + a205=(a205+a245); + a245=(a350*a298); + a242=(a357*a242); + a245=(a245-a242); + a245=(a347*a245); + a205=(a205-a245); + a248=(a357*a248); + a298=(a358*a298); + a248=(a248+a298); + a248=(a368*a248); + a205=(a205-a248); + if (res[0]!=0) res[0][161]=a205; + a205=(a18*a467); + a205=(a445*a205); + a248=(a18*a442); + a298=(a449*a248); + a245=(a18*a465); + a242=(a450*a245); + a298=(a298+a242); + a242=(a298/a462); + a298=(a452*a298); + a213=(a451*a248); + a228=(a455*a245); + a213=(a213-a228); + a228=(a456*a213); + a298=(a298+a228); + a228=(a422*a298); + a242=(a242-a228); + a242=(a438*a242); + a228=(a455*a242); + a213=(a213/a462); + a298=(a461*a298); + a213=(a213-a298); + a213=(a438*a213); + a298=(a450*a213); + a228=(a228+a298); + a228=(a466*a228); + a205=(a205+a228); + a228=(a449*a213); + a298=(a451*a242); + a228=(a228-a298); + a228=(a469*a228); + a298=(a18*a411); + a298=(a470*a298); + a228=(a228-a298); + a205=(a205+a228); + a228=(a18*a474); + a228=(a473*a228); + a298=(a18*a477); + a298=(a476*a298); + a228=(a228+a298); + a298=(a205+a228); + a217=(a748*a479); + a217=(a478*a217); + a212=(a748*a483); + a212=(a482*a212); + a217=(a217+a212); + a298=(a298+a217); + a212=(a748*a485); + a212=(a484*a212); + a229=(a748*a489); + a229=(a488*a229); + a212=(a212+a229); + a298=(a298+a212); + a229=(a414*a299); + a223=(a408*a216); + a229=(a229+a223); + a229=(a486*a229); + a298=(a298-a229); + a216=(a400*a216); + a229=(a414*a206); + a216=(a216-a229); + a216=(a490*a216); + a298=(a298+a216); + a216=(a400*a231); + a206=(a407*a206); + a216=(a216-a206); + a216=(a447*a216); + a298=(a298-a216); + a299=(a407*a299); + a231=(a408*a231); + a299=(a299+a231); + a299=(a491*a299); + a298=(a298-a299); + if (res[0]!=0) res[0][162]=a298; + a298=(a23*a517); + a298=(a495*a298); + a299=(a23*a493); + a231=(a468*a299); + a216=(a23*a515); + a206=(a500*a216); + a231=(a231+a206); + a206=(a231/a512); + a231=(a502*a231); + a229=(a501*a299); + a223=(a505*a216); + a229=(a229-a223); + a223=(a506*a229); + a231=(a231+a223); + a223=(a492*a231); + a206=(a206-a223); + a206=(a494*a206); + a223=(a505*a206); + a229=(a229/a512); + a231=(a511*a231); + a229=(a229-a231); + a229=(a494*a229); + a231=(a500*a229); + a223=(a223+a231); + a223=(a516*a223); + a298=(a298+a223); + a223=(a468*a229); + a231=(a501*a206); + a223=(a223-a231); + a223=(a519*a223); + a231=(a23*a463); + a231=(a514*a231); + a223=(a223-a231); + a298=(a298+a223); + a223=(a23*a522); + a223=(a521*a223); + a231=(a23*a525); + a231=(a524*a231); + a223=(a223+a231); + a231=(a298+a223); + a214=(a759*a527); + a214=(a526*a214); + a224=(a759*a531); + a224=(a530*a224); + a214=(a214+a224); + a231=(a231+a214); + a224=(a759*a533); + a224=(a532*a224); + a204=(a759*a537); + a204=(a536*a204); + a224=(a224+a204); + a231=(a231+a224); + a204=(a460*a248); + a218=(a454*a242); + a204=(a204+a218); + a204=(a499*a204); + a231=(a231-a204); + a242=(a446*a242); + a204=(a460*a245); + a242=(a242-a204); + a242=(a457*a242); + a231=(a231+a242); + a242=(a446*a213); + a245=(a453*a245); + a242=(a242-a245); + a242=(a443*a242); + a231=(a231-a242); + a248=(a453*a248); + a213=(a454*a213); + a248=(a248+a213); + a248=(a464*a248); + a231=(a231-a248); + if (res[0]!=0) res[0][163]=a231; + a231=(a27*a563); + a231=(a541*a231); + a248=(a27*a538); + a213=(a545*a248); + a242=(a27*a561); + a245=(a546*a242); + a213=(a213+a245); + a245=(a213/a558); + a213=(a548*a213); + a204=(a547*a248); + a218=(a551*a242); + a204=(a204-a218); + a218=(a552*a204); + a213=(a213+a218); + a218=(a518*a213); + a245=(a245-a218); + a245=(a534*a245); + a218=(a551*a245); + a204=(a204/a558); + a213=(a557*a213); + a204=(a204-a213); + a204=(a534*a204); + a213=(a546*a204); + a218=(a218+a213); + a218=(a562*a218); + a231=(a231+a218); + a218=(a545*a204); + a213=(a547*a245); + a218=(a218-a213); + a218=(a565*a218); + a213=(a27*a507); + a213=(a560*a213); + a218=(a218-a213); + a231=(a231+a218); + a218=(a760*a568); + a218=(a567*a218); + a213=(a760*a571); + a213=(a570*a213); + a218=(a218+a213); + a213=(a231+a218); + a196=(a750*a573); + a196=(a572*a196); + a157=(a750*a577); + a157=(a576*a157); + a196=(a196+a157); + a213=(a213+a196); + a157=(a750*a579); + a157=(a578*a157); + a183=(a750*a583); + a183=(a582*a183); + a157=(a157+a183); + a213=(a213+a157); + a183=(a510*a299); + a192=(a504*a206); + a183=(a183+a192); + a183=(a580*a183); + a213=(a213-a183); + a206=(a496*a206); + a183=(a510*a216); + a206=(a206-a183); + a206=(a584*a206); + a213=(a213+a206); + a206=(a496*a229); + a216=(a503*a216); + a206=(a206-a216); + a206=(a564*a206); + a213=(a213-a206); + a299=(a503*a299); + a229=(a504*a229); + a299=(a299+a229); + a299=(a585*a299); + a213=(a213-a299); + if (res[0]!=0) res[0][164]=a213; + a213=(a703*a611); + a213=(a589*a213); + a299=(a703*a587); + a229=(a593*a299); + a206=(a703*a609); + a216=(a594*a206); + a229=(a229+a216); + a216=(a229/a606); + a229=(a596*a229); + a183=(a595*a299); + a192=(a599*a206); + a183=(a183-a192); + a192=(a600*a183); + a229=(a229+a192); + a192=(a586*a229); + a216=(a216-a192); + a216=(a588*a216); + a192=(a599*a216); + a183=(a183/a606); + a229=(a605*a229); + a183=(a183-a229); + a183=(a588*a183); + a229=(a594*a183); + a192=(a192+a229); + a192=(a610*a192); + a213=(a213+a192); + a192=(a593*a183); + a229=(a595*a216); + a192=(a192-a229); + a192=(a613*a192); + a229=(a703*a553); + a229=(a614*a229); + a192=(a192-a229); + a213=(a213+a192); + a192=(a703*a618); + a192=(a617*a192); + a229=(a703*a621); + a229=(a620*a229); + a192=(a192+a229); + a229=(a213+a192); + a61=(a747*a623); + a61=(a622*a61); + a187=(a747*a627); + a187=(a626*a187); + a61=(a61+a187); + a229=(a229+a61); + a187=(a747*a629); + a187=(a628*a187); + a156=(a747*a633); + a156=(a632*a156); + a187=(a187+a156); + a229=(a229+a187); + a156=(a556*a248); + a184=(a550*a245); + a156=(a156+a184); + a156=(a630*a156); + a229=(a229-a156); + a245=(a542*a245); + a156=(a556*a242); + a245=(a245-a156); + a245=(a634*a245); + a229=(a229+a245); + a245=(a542*a204); + a242=(a549*a242); + a245=(a245-a242); + a245=(a591*a245); + a229=(a229-a245); + a248=(a549*a248); + a204=(a550*a204); + a248=(a248+a204); + a248=(a635*a248); + a229=(a229-a248); + if (res[0]!=0) res[0][165]=a229; + a229=(a749*a661); + a229=(a639*a229); + a248=(a749*a637); + a204=(a612*a248); + a245=(a749*a659); + a242=(a644*a245); + a204=(a204+a242); + a242=(a204/a656); + a204=(a646*a204); + a156=(a645*a248); + a184=(a649*a245); + a156=(a156-a184); + a184=(a650*a156); + a204=(a204+a184); + a184=(a636*a204); + a242=(a242-a184); + a242=(a638*a242); + a184=(a649*a242); + a156=(a156/a656); + a204=(a655*a204); + a156=(a156-a204); + a156=(a638*a156); + a204=(a644*a156); + a184=(a184+a204); + a184=(a660*a184); + a229=(a229+a184); + a184=(a612*a156); + a204=(a645*a242); + a184=(a184-a204); + a184=(a663*a184); + a204=(a749*a607); + a204=(a658*a204); + a184=(a184-a204); + a229=(a229+a184); + a184=(a764*a666); + a184=(a665*a184); + a204=(a764*a669); + a204=(a668*a204); + a184=(a184+a204); + a204=(a229+a184); + a171=(a766*a671); + a171=(a670*a171); + a199=(a766*a675); + a199=(a674*a199); + a171=(a171+a199); + a204=(a204+a171); + a199=(a766*a677); + a199=(a676*a199); + a148=(a766*a681); + a148=(a680*a148); + a199=(a199+a148); + a204=(a204+a199); + a148=(a604*a299); + a193=(a598*a216); + a148=(a148+a193); + a148=(a643*a148); + a204=(a204-a148); + a216=(a590*a216); + a148=(a604*a206); + a216=(a216-a148); + a216=(a601*a216); + a204=(a204+a216); + a216=(a590*a183); + a206=(a597*a206); + a216=(a216-a206); + a216=(a559*a216); + a204=(a204-a216); + a299=(a597*a299); + a183=(a598*a183); + a299=(a299+a183); + a299=(a608*a299); + a204=(a204-a299); + if (res[0]!=0) res[0][166]=a204; + a204=(a765*a693); + a204=(a700*a204); + a299=(a765*a706); + a183=(a688*a299); + a216=(a765*a687); + a206=(a692*a216); + a183=(a183+a206); + a206=(a183/a686); + a183=(a684*a183); + a148=(a696*a299); + a193=(a689*a216); + a148=(a148-a193); + a193=(a702*a148); + a183=(a183+a193); + a193=(a691*a183); + a206=(a206-a193); + a206=(a662*a206); + a193=(a689*a206); + a148=(a148/a686); + a183=(a685*a183); + a148=(a148-a183); + a148=(a662*a148); + a183=(a692*a148); + a193=(a193+a183); + a193=(a697*a193); + a204=(a204+a193); + a193=(a688*a148); + a183=(a696*a206); + a193=(a193-a183); + a193=(a708*a193); + a183=(a765*a699); + a183=(a709*a183); + a193=(a193-a183); + a204=(a204+a193); + a193=(a765*a714); + a193=(a713*a193); + a183=(a765*a717); + a183=(a716*a183); + a193=(a193+a183); + a183=(a204+a193); + a142=(a772*a720); + a142=(a719*a142); + a126=(a772*a724); + a126=(a723*a126); + a142=(a142+a126); + a183=(a183+a142); + a126=(a772*a727); + a126=(a726*a126); + a188=(a772*a731); + a188=(a730*a188); + a126=(a126+a188); + a183=(a183+a126); + a188=(a654*a248); + a189=(a648*a242); + a188=(a188+a189); + a188=(a732*a188); + a183=(a183-a188); + a242=(a640*a242); + a188=(a654*a245); + a242=(a242-a188); + a242=(a733*a242); + a183=(a183+a242); + a242=(a640*a156); + a245=(a647*a245); + a242=(a242-a245); + a242=(a682*a242); + a183=(a183-a242); + a248=(a647*a248); + a156=(a648*a156); + a248=(a248+a156); + a248=(a734*a248); + a183=(a183-a248); + if (res[0]!=0) res[0][167]=a183; + a183=(a690*a206); + a248=(a678*a216); + a183=(a183-a248); + a183=(a711*a183); + a248=(a678*a299); + a206=(a657*a206); + a248=(a248+a206); + a248=(a751*a248); + a183=(a183-a248); + a248=(a690*a148); + a216=(a701*a216); + a248=(a248-a216); + a248=(a742*a248); + a183=(a183-a248); + a299=(a701*a299); + a148=(a657*a148); + a299=(a299+a148); + a299=(a705*a299); + a183=(a183-a299); + if (res[0]!=0) res[0][168]=a183; + a753=(a8*a753); + a26=(a8*a26); + a753=(a753+a26); + a795=(a8*a795); + a753=(a753+a795); + a30=(a8*a30); + a753=(a753+a30); + a756=(a8*a756); + a753=(a753+a756); + if (res[0]!=0) res[0][169]=a753; + a59=(a8*a59); + a76=(a8*a76); + a59=(a59+a76); + a817=(a8*a817); + a59=(a59+a817); + a754=(a8*a754); + a59=(a59+a754); + a115=(a8*a115); + a59=(a59+a115); + if (res[0]!=0) res[0][170]=a59; + a60=(a8*a60); + a153=(a8*a153); + a60=(a60+a153); + a92=(a8*a92); + a60=(a60+a92); + a159=(a8*a159); + a60=(a60+a159); + a131=(a8*a131); + a60=(a60+a131); + if (res[0]!=0) res[0][171]=a60; + a204=(a4*a204); + a60=(a5*a204); + a131=(a10*a204); + a193=(a38*a193); + a131=(a131+a193); + a193=(a5*a131); + a60=(a60+a193); + a193=(a10*a204); + a142=(a44*a142); + a193=(a193+a142); + a142=(a5*a193); + a60=(a60+a142); + a126=(a44*a126); + a126=(a204+a126); + a142=(a5*a126); + a60=(a60+a142); + a142=(a5*a60); + a229=(a4*a229); + a159=(a5*a229); + a142=(a142+a159); + a159=(a10*a229); + a184=(a38*a184); + a159=(a159+a184); + a184=(a5*a159); + a142=(a142+a184); + a184=(a10*a229); + a171=(a44*a171); + a184=(a184+a171); + a171=(a5*a184); + a142=(a142+a171); + a199=(a44*a199); + a199=(a229+a199); + a171=(a5*a199); + a142=(a142+a171); + a171=(a5*a142); + a213=(a4*a213); + a92=(a5*a213); + a171=(a171+a92); + a92=(a10*a213); + a192=(a38*a192); + a92=(a92+a192); + a192=(a5*a92); + a171=(a171+a192); + a192=(a10*a213); + a61=(a44*a61); + a192=(a192+a61); + a61=(a5*a192); + a171=(a171+a61); + a187=(a44*a187); + a187=(a213+a187); + a61=(a5*a187); + a171=(a171+a61); + a61=(a5*a171); + a231=(a4*a231); + a153=(a5*a231); + a61=(a61+a153); + a153=(a10*a231); + a218=(a38*a218); + a153=(a153+a218); + a218=(a5*a153); + a61=(a61+a218); + a218=(a10*a231); + a196=(a44*a196); + a218=(a218+a196); + a196=(a5*a218); + a61=(a61+a196); + a157=(a44*a157); + a157=(a231+a157); + a196=(a5*a157); + a61=(a61+a196); + a196=(a5*a61); + a298=(a4*a298); + a59=(a5*a298); + a196=(a196+a59); + a59=(a10*a298); + a223=(a38*a223); + a59=(a59+a223); + a223=(a5*a59); + a196=(a196+a223); + a223=(a10*a298); + a214=(a44*a214); + a223=(a223+a214); + a214=(a5*a223); + a196=(a196+a214); + a224=(a44*a224); + a224=(a298+a224); + a214=(a5*a224); + a196=(a196+a214); + a214=(a5*a196); + a205=(a4*a205); + a115=(a5*a205); + a214=(a214+a115); + a115=(a10*a205); + a228=(a38*a228); + a115=(a115+a228); + a228=(a5*a115); + a214=(a214+a228); + a228=(a10*a205); + a217=(a44*a217); + a228=(a228+a217); + a217=(a5*a228); + a214=(a214+a217); + a212=(a44*a212); + a212=(a205+a212); + a217=(a5*a212); + a214=(a214+a217); + a217=(a5*a214); + a243=(a4*a243); + a754=(a5*a243); + a217=(a217+a754); + a754=(a10*a243); + a226=(a38*a226); + a754=(a754+a226); + a226=(a5*a754); + a217=(a217+a226); + a226=(a10*a243); + a175=(a44*a175); + a226=(a226+a175); + a175=(a5*a226); + a217=(a217+a175); + a180=(a44*a180); + a180=(a243+a180); + a175=(a5*a180); + a217=(a217+a175); + a175=(a5*a217); + a215=(a4*a215); + a817=(a5*a215); + a175=(a175+a817); + a817=(a10*a215); + a170=(a38*a170); + a817=(a817+a170); + a170=(a5*a817); + a175=(a175+a170); + a170=(a10*a215); + a222=(a44*a222); + a170=(a170+a222); + a222=(a5*a170); + a175=(a175+a222); + a294=(a44*a294); + a294=(a215+a294); + a222=(a5*a294); + a175=(a175+a222); + a222=(a5*a175); + a249=(a4*a249); + a76=(a5*a249); + a222=(a222+a76); + a76=(a10*a249); + a255=(a38*a255); + a76=(a76+a255); + a255=(a5*a76); + a222=(a222+a255); + a255=(a10*a249); + a208=(a44*a208); + a255=(a255+a208); + a208=(a5*a255); + a222=(a222+a208); + a227=(a44*a227); + a227=(a249+a227); + a208=(a5*a227); + a222=(a222+a208); + a222=(a5*a222); + a236=(a4*a236); + a208=(a5*a236); + a222=(a222+a208); + a208=(a10*a236); + a239=(a38*a239); + a208=(a208+a239); + a208=(a5*a208); + a222=(a222+a208); + a208=(a10*a236); + a237=(a44*a237); + a208=(a208+a237); + a208=(a5*a208); + a222=(a222+a208); + a234=(a44*a234); + a236=(a236+a234); + a236=(a5*a236); + a222=(a222+a236); + a236=(a5*a222); + a207=(a4*a207); + a234=(a5*a207); + a236=(a236+a234); + a234=(a10*a207); + a233=(a38*a233); + a234=(a234+a233); + a233=(a5*a234); + a236=(a236+a233); + a233=(a10*a207); + a238=(a44*a238); + a233=(a233+a238); + a238=(a5*a233); + a236=(a236+a238); + a244=(a44*a244); + a244=(a207+a244); + a238=(a5*a244); + a236=(a236+a238); + a236=(a8*a236); + a238=(a50*a34); + a236=(a236-a238); + a208=(a50*a34); + a236=(a236-a208); + if (res[0]!=0) res[0][172]=a236; + a172=(a28*a172); + a209=(a209*a172); + a158=(a42*a158); + a241=(a241*a158); + a209=(a209+a241); + a190=(a42*a190); + a247=(a247*a190); + a209=(a209+a247); + a221=(a28*a221); + a232=(a232*a221); + a209=(a209-a232); + a210=(a42*a210); + a235=(a235*a210); + a209=(a209-a235); + a129=(a42*a129); + a240=(a240*a129); + a209=(a209-a240); + a209=(-a209); + if (res[0]!=0) res[0][173]=a209; + a222=(a8*a222); + a207=(a8*a207); + a222=(a222+a207); + a234=(a8*a234); + a222=(a222+a234); + a233=(a8*a233); + a222=(a222+a233); + a244=(a8*a244); + a222=(a222+a244); + a244=(a24*a34); + a222=(a222-a244); + a244=(a50*a34); + a222=(a222+a244); + a222=(a222+a238); + a222=(a222+a208); + a208=(a10*a34); + a222=(a222+a208); + if (res[0]!=0) res[0][174]=a222; + a222=cos(a267); + a208=(a279*a222); + a238=cos(a252); + a244=(a289*a238); + a208=(a208+a244); + a244=cos(a160); + a233=(a295*a244); + a208=(a208+a233); + a233=cos(a220); + a233=(a279*a233); + a208=(a208+a233); + a267=sin(a267); + a233=(a280*a267); + a208=(a208-a233); + a252=sin(a252); + a233=(a283*a252); + a208=(a208-a233); + a160=sin(a160); + a233=(a288*a160); + a208=(a208-a233); + a220=sin(a220); + a220=(a280*a220); + a208=(a208-a220); + a208=(-a208); + if (res[0]!=0) res[0][175]=a208; + a208=(a18*a222); + a208=(a279*a208); + a220=(a748*a238); + a220=(a289*a220); + a208=(a208+a220); + a220=(a748*a244); + a220=(a295*a220); + a208=(a208+a220); + a220=(a18*a267); + a220=(a280*a220); + a208=(a208-a220); + a220=(a748*a252); + a220=(a283*a220); + a208=(a208-a220); + a220=(a748*a160); + a220=(a288*a220); + a208=(a208-a220); + a208=(-a208); + if (res[0]!=0) res[0][176]=a208; + a208=(a15*a222); + a208=(a279*a208); + a220=(a21*a238); + a220=(a289*a220); + a208=(a208+a220); + a220=(a21*a244); + a220=(a295*a220); + a208=(a208+a220); + a220=(a15*a267); + a220=(a280*a220); + a208=(a208-a220); + a220=(a21*a252); + a220=(a283*a220); + a208=(a208-a220); + a220=(a21*a160); + a220=(a288*a220); + a208=(a208-a220); + a208=(-a208); + if (res[0]!=0) res[0][177]=a208; + a208=(a2*a222); + a208=(a279*a208); + a220=(a29*a238); + a220=(a289*a220); + a208=(a208+a220); + a220=(a29*a244); + a220=(a295*a220); + a208=(a208+a220); + a220=(a2*a267); + a220=(a280*a220); + a208=(a208-a220); + a220=(a29*a252); + a220=(a283*a220); + a208=(a208-a220); + a220=(a29*a160); + a220=(a288*a220); + a208=(a208-a220); + a208=(-a208); + if (res[0]!=0) res[0][178]=a208; + a208=(a51*a222); + a208=(a279*a208); + a220=(a32*a238); + a220=(a289*a220); + a208=(a208+a220); + a220=(a32*a244); + a220=(a295*a220); + a208=(a208+a220); + a220=(a51*a267); + a220=(a280*a220); + a208=(a208-a220); + a220=(a32*a252); + a220=(a283*a220); + a208=(a208-a220); + a220=(a32*a160); + a220=(a288*a220); + a208=(a208-a220); + a208=(-a208); + if (res[0]!=0) res[0][179]=a208; + a185=(a185-a186); + if (res[0]!=0) res[0][180]=a185; + a185=(a41*a222); + a185=(a279*a185); + a186=(a35*a238); + a186=(a289*a186); + a185=(a185+a186); + a186=(a35*a244); + a186=(a295*a186); + a185=(a185+a186); + a186=(a41*a267); + a186=(a280*a186); + a185=(a185-a186); + a186=(a35*a252); + a186=(a283*a186); + a185=(a185-a186); + a186=(a35*a160); + a186=(a288*a186); + a185=(a185-a186); + a185=(-a185); + if (res[0]!=0) res[0][181]=a185; + a185=(a10*a34); + a186=(a3*a34); + a208=(a10*a34); + a186=(a186-a208); + a185=(a185-a186); + a186=(a10*a34); + a185=(a185+a186); + a186=(a10*a34); + a185=(a185+a186); + if (res[0]!=0) res[0][182]=a185; + a275=(a28*a275); + a253=(a253*a275); + a250=(a28*a250); + a275=(a257*a250); + a273=(a28*a273); + a185=(a258*a273); + a275=(a275+a185); + a185=(a275/a270); + a260=(a260*a275); + a275=(a259*a250); + a186=(a263*a273); + a275=(a275-a186); + a264=(a264*a275); + a260=(a260+a264); + a230=(a230*a260); + a185=(a185-a230); + a185=(a246*a185); + a263=(a263*a185); + a275=(a275/a270); + a269=(a269*a260); + a275=(a275-a269); + a246=(a246*a275); + a258=(a258*a246); + a263=(a263+a258); + a274=(a274*a263); + a253=(a253+a274); + a257=(a257*a246); + a259=(a259*a185); + a257=(a257-a259); + a277=(a277*a257); + a219=(a28*a219); + a278=(a278*a219); + a277=(a277-a278); + a253=(a253+a277); + a282=(a28*a282); + a281=(a281*a282); + a285=(a28*a285); + a284=(a284*a285); + a281=(a281+a284); + a284=(a253+a281); + a287=(a42*a287); + a286=(a286*a287); + a291=(a42*a291); + a290=(a290*a291); + a286=(a286+a290); + a284=(a284+a286); + a293=(a42*a293); + a292=(a292*a293); + a297=(a42*a297); + a296=(a296*a297); + a292=(a292+a296); + a284=(a284+a292); + if (res[0]!=0) res[0][183]=a284; + a284=(a41*a325); + a284=(a303*a284); + a296=(a41*a301); + a297=(a276*a296); + a293=(a41*a323); + a290=(a308*a293); + a297=(a297+a290); + a290=(a297/a320); + a297=(a310*a297); + a291=(a309*a296); + a287=(a313*a293); + a291=(a291-a287); + a287=(a314*a291); + a297=(a297+a287); + a287=(a300*a297); + a290=(a290-a287); + a290=(a302*a290); + a287=(a313*a290); + a291=(a291/a320); + a297=(a319*a297); + a291=(a291-a297); + a291=(a302*a291); + a297=(a308*a291); + a287=(a287+a297); + a287=(a324*a287); + a284=(a284+a287); + a287=(a276*a291); + a297=(a309*a290); + a287=(a287-a297); + a287=(a327*a287); + a297=(a41*a271); + a297=(a322*a297); + a287=(a287-a297); + a284=(a284+a287); + a287=(a41*a330); + a287=(a329*a287); + a297=(a41*a333); + a297=(a332*a297); + a287=(a287+a297); + a297=(a284+a287); + a285=(a35*a335); + a285=(a334*a285); + a282=(a35*a339); + a282=(a338*a282); + a285=(a285+a282); + a297=(a297+a285); + a282=(a35*a341); + a282=(a340*a282); + a277=(a35*a345); + a277=(a344*a277); + a282=(a282+a277); + a297=(a297+a282); + a277=(a268*a250); + a278=(a262*a185); + a277=(a277+a278); + a307=(a307*a277); + a297=(a297-a307); + a185=(a254*a185); + a268=(a268*a273); + a185=(a185-a268); + a265=(a265*a185); + a297=(a297+a265); + a254=(a254*a246); + a273=(a261*a273); + a254=(a254-a273); + a251=(a251*a254); + a297=(a297-a251); + a261=(a261*a250); + a262=(a262*a246); + a261=(a261+a262); + a272=(a272*a261); + a297=(a297-a272); + if (res[0]!=0) res[0][184]=a297; + a297=(a51*a371); + a297=(a349*a297); + a272=(a51*a346); + a261=(a353*a272); + a262=(a51*a369); + a246=(a354*a262); + a261=(a261+a246); + a246=(a261/a366); + a261=(a356*a261); + a250=(a355*a272); + a251=(a359*a262); + a250=(a250-a251); + a251=(a360*a250); + a261=(a261+a251); + a251=(a326*a261); + a246=(a246-a251); + a246=(a342*a246); + a251=(a359*a246); + a250=(a250/a366); + a261=(a365*a261); + a250=(a250-a261); + a250=(a342*a250); + a261=(a354*a250); + a251=(a251+a261); + a251=(a370*a251); + a297=(a297+a251); + a251=(a353*a250); + a261=(a355*a246); + a251=(a251-a261); + a251=(a373*a251); + a261=(a51*a315); + a261=(a374*a261); + a251=(a251-a261); + a297=(a297+a251); + a251=(a51*a378); + a251=(a377*a251); + a261=(a51*a381); + a261=(a380*a261); + a251=(a251+a261); + a261=(a297+a251); + a254=(a32*a383); + a254=(a382*a254); + a273=(a32*a387); + a273=(a386*a273); + a254=(a254+a273); + a261=(a261+a254); + a273=(a32*a389); + a273=(a388*a273); + a265=(a32*a393); + a265=(a392*a265); + a273=(a273+a265); + a261=(a261+a273); + a265=(a318*a296); + a185=(a312*a290); + a265=(a265+a185); + a265=(a390*a265); + a261=(a261-a265); + a290=(a304*a290); + a265=(a318*a293); + a290=(a290-a265); + a290=(a394*a290); + a261=(a261+a290); + a290=(a304*a291); + a293=(a311*a293); + a290=(a290-a293); + a290=(a351*a290); + a261=(a261-a290); + a296=(a311*a296); + a291=(a312*a291); + a296=(a296+a291); + a296=(a395*a296); + a261=(a261-a296); + if (res[0]!=0) res[0][185]=a261; + a261=(a14*a421); + a261=(a399*a261); + a296=(a14*a397); + a291=(a372*a296); + a290=(a14*a419); + a293=(a404*a290); + a291=(a291+a293); + a293=(a291/a416); + a291=(a406*a291); + a265=(a405*a296); + a185=(a409*a290); + a265=(a265-a185); + a185=(a410*a265); + a291=(a291+a185); + a185=(a396*a291); + a293=(a293-a185); + a293=(a398*a293); + a185=(a409*a293); + a265=(a265/a416); + a291=(a415*a291); + a265=(a265-a291); + a265=(a398*a265); + a291=(a404*a265); + a185=(a185+a291); + a185=(a420*a185); + a261=(a261+a185); + a185=(a372*a265); + a291=(a405*a293); + a185=(a185-a291); + a185=(a423*a185); + a291=(a14*a367); + a291=(a418*a291); + a185=(a185-a291); + a261=(a261+a185); + a185=(a2*a426); + a185=(a425*a185); + a291=(a2*a429); + a291=(a428*a291); + a185=(a185+a291); + a291=(a261+a185); + a268=(a29*a431); + a268=(a430*a268); + a307=(a29*a435); + a307=(a434*a307); + a268=(a268+a307); + a291=(a291+a268); + a307=(a29*a437); + a307=(a436*a307); + a277=(a29*a441); + a277=(a440*a277); + a307=(a307+a277); + a291=(a291+a307); + a277=(a364*a272); + a278=(a358*a246); + a277=(a277+a278); + a277=(a403*a277); + a291=(a291-a277); + a246=(a350*a246); + a277=(a364*a262); + a246=(a246-a277); + a246=(a361*a246); + a291=(a291+a246); + a246=(a350*a250); + a262=(a357*a262); + a246=(a246-a262); + a246=(a347*a246); + a291=(a291-a246); + a272=(a357*a272); + a250=(a358*a250); + a272=(a272+a250); + a272=(a368*a272); + a291=(a291-a272); + if (res[0]!=0) res[0][186]=a291; + a291=(a20*a467); + a291=(a445*a291); + a272=(a20*a442); + a250=(a449*a272); + a246=(a20*a465); + a262=(a450*a246); + a250=(a250+a262); + a262=(a250/a462); + a250=(a452*a250); + a277=(a451*a272); + a278=(a455*a246); + a277=(a277-a278); + a278=(a456*a277); + a250=(a250+a278); + a278=(a422*a250); + a262=(a262-a278); + a262=(a438*a262); + a278=(a455*a262); + a277=(a277/a462); + a250=(a461*a250); + a277=(a277-a250); + a277=(a438*a277); + a250=(a450*a277); + a278=(a278+a250); + a278=(a466*a278); + a291=(a291+a278); + a278=(a449*a277); + a250=(a451*a262); + a278=(a278-a250); + a278=(a469*a278); + a250=(a20*a411); + a250=(a470*a250); + a278=(a278-a250); + a291=(a291+a278); + a278=(a15*a474); + a278=(a473*a278); + a250=(a15*a477); + a250=(a476*a250); + a278=(a278+a250); + a250=(a291+a278); + a219=(a21*a479); + a219=(a478*a219); + a257=(a21*a483); + a257=(a482*a257); + a219=(a219+a257); + a250=(a250+a219); + a257=(a21*a485); + a257=(a484*a257); + a259=(a21*a489); + a259=(a488*a259); + a257=(a257+a259); + a250=(a250+a257); + a259=(a414*a296); + a274=(a408*a293); + a259=(a259+a274); + a259=(a486*a259); + a250=(a250-a259); + a293=(a400*a293); + a259=(a414*a290); + a293=(a293-a259); + a293=(a490*a293); + a250=(a250+a293); + a293=(a400*a265); + a290=(a407*a290); + a293=(a293-a290); + a293=(a447*a293); + a250=(a250-a293); + a296=(a407*a296); + a265=(a408*a265); + a296=(a296+a265); + a296=(a491*a296); + a250=(a250-a296); + if (res[0]!=0) res[0][187]=a250; + a250=(a18*a517); + a250=(a495*a250); + a296=(a18*a493); + a265=(a468*a296); + a293=(a18*a515); + a290=(a500*a293); + a265=(a265+a290); + a290=(a265/a512); + a265=(a502*a265); + a259=(a501*a296); + a274=(a505*a293); + a259=(a259-a274); + a274=(a506*a259); + a265=(a265+a274); + a274=(a492*a265); + a290=(a290-a274); + a290=(a494*a290); + a274=(a505*a290); + a259=(a259/a512); + a265=(a511*a265); + a259=(a259-a265); + a259=(a494*a259); + a265=(a500*a259); + a274=(a274+a265); + a274=(a516*a274); + a250=(a250+a274); + a274=(a468*a259); + a265=(a501*a290); + a274=(a274-a265); + a274=(a519*a274); + a265=(a18*a463); + a265=(a514*a265); + a274=(a274-a265); + a250=(a250+a274); + a274=(a18*a522); + a274=(a521*a274); + a265=(a18*a525); + a265=(a524*a265); + a274=(a274+a265); + a265=(a250+a274); + a263=(a748*a527); + a263=(a526*a263); + a258=(a748*a531); + a258=(a530*a258); + a263=(a263+a258); + a265=(a265+a263); + a258=(a748*a533); + a258=(a532*a258); + a275=(a748*a537); + a275=(a536*a275); + a258=(a258+a275); + a265=(a265+a258); + a275=(a460*a272); + a269=(a454*a262); + a275=(a275+a269); + a275=(a499*a275); + a265=(a265-a275); + a262=(a446*a262); + a275=(a460*a246); + a262=(a262-a275); + a262=(a457*a262); + a265=(a265+a262); + a262=(a446*a277); + a246=(a453*a246); + a262=(a262-a246); + a262=(a443*a262); + a265=(a265-a262); + a272=(a453*a272); + a277=(a454*a277); + a272=(a272+a277); + a272=(a464*a272); + a265=(a265-a272); + if (res[0]!=0) res[0][188]=a265; + a265=(a23*a563); + a265=(a541*a265); + a272=(a23*a538); + a277=(a545*a272); + a262=(a23*a561); + a246=(a546*a262); + a277=(a277+a246); + a246=(a277/a558); + a277=(a548*a277); + a275=(a547*a272); + a269=(a551*a262); + a275=(a275-a269); + a269=(a552*a275); + a277=(a277+a269); + a269=(a518*a277); + a246=(a246-a269); + a246=(a534*a246); + a269=(a551*a246); + a275=(a275/a558); + a277=(a557*a277); + a275=(a275-a277); + a275=(a534*a275); + a277=(a546*a275); + a269=(a269+a277); + a269=(a562*a269); + a265=(a265+a269); + a269=(a545*a275); + a277=(a547*a246); + a269=(a269-a277); + a269=(a565*a269); + a277=(a23*a507); + a277=(a560*a277); + a269=(a269-a277); + a265=(a265+a269); + a269=(a23*a568); + a269=(a567*a269); + a277=(a23*a571); + a277=(a570*a277); + a269=(a269+a277); + a277=(a265+a269); + a260=(a759*a573); + a260=(a572*a260); + a270=(a759*a577); + a270=(a576*a270); + a260=(a260+a270); + a277=(a277+a260); + a270=(a759*a579); + a270=(a578*a270); + a230=(a759*a583); + a230=(a582*a230); + a270=(a270+a230); + a277=(a277+a270); + a230=(a510*a296); + a264=(a504*a290); + a230=(a230+a264); + a230=(a580*a230); + a277=(a277-a230); + a290=(a496*a290); + a230=(a510*a293); + a290=(a290-a230); + a290=(a584*a290); + a277=(a277+a290); + a290=(a496*a259); + a293=(a503*a293); + a290=(a290-a293); + a290=(a564*a290); + a277=(a277-a290); + a296=(a503*a296); + a259=(a504*a259); + a296=(a296+a259); + a296=(a585*a296); + a277=(a277-a296); + if (res[0]!=0) res[0][189]=a277; + a277=(a27*a611); + a277=(a589*a277); + a296=(a27*a587); + a259=(a593*a296); + a290=(a27*a609); + a293=(a594*a290); + a259=(a259+a293); + a293=(a259/a606); + a259=(a596*a259); + a230=(a595*a296); + a264=(a599*a290); + a230=(a230-a264); + a264=(a600*a230); + a259=(a259+a264); + a264=(a586*a259); + a293=(a293-a264); + a293=(a588*a293); + a264=(a599*a293); + a230=(a230/a606); + a259=(a605*a259); + a230=(a230-a259); + a230=(a588*a230); + a259=(a594*a230); + a264=(a264+a259); + a264=(a610*a264); + a277=(a277+a264); + a264=(a593*a230); + a259=(a595*a293); + a264=(a264-a259); + a264=(a613*a264); + a259=(a27*a553); + a259=(a614*a259); + a264=(a264-a259); + a277=(a277+a264); + a264=(a760*a618); + a264=(a617*a264); + a259=(a760*a621); + a259=(a620*a259); + a264=(a264+a259); + a259=(a277+a264); + a186=(a750*a623); + a186=(a622*a186); + a208=(a750*a627); + a208=(a626*a208); + a186=(a186+a208); + a259=(a259+a186); + a208=(a750*a629); + a208=(a628*a208); + a220=(a750*a633); + a220=(a632*a220); + a208=(a208+a220); + a259=(a259+a208); + a220=(a556*a272); + a233=(a550*a246); + a220=(a220+a233); + a220=(a630*a220); + a259=(a259-a220); + a246=(a542*a246); + a220=(a556*a262); + a246=(a246-a220); + a246=(a634*a246); + a259=(a259+a246); + a246=(a542*a275); + a262=(a549*a262); + a246=(a246-a262); + a246=(a591*a246); + a259=(a259-a246); + a272=(a549*a272); + a275=(a550*a275); + a272=(a272+a275); + a272=(a635*a272); + a259=(a259-a272); + if (res[0]!=0) res[0][190]=a259; + a259=(a703*a661); + a259=(a639*a259); + a272=(a703*a637); + a275=(a612*a272); + a246=(a703*a659); + a262=(a644*a246); + a275=(a275+a262); + a262=(a275/a656); + a275=(a646*a275); + a220=(a645*a272); + a233=(a649*a246); + a220=(a220-a233); + a233=(a650*a220); + a275=(a275+a233); + a233=(a636*a275); + a262=(a262-a233); + a262=(a638*a262); + a233=(a649*a262); + a220=(a220/a656); + a275=(a655*a275); + a220=(a220-a275); + a220=(a638*a220); + a275=(a644*a220); + a233=(a233+a275); + a233=(a660*a233); + a259=(a259+a233); + a233=(a612*a220); + a275=(a645*a262); + a233=(a233-a275); + a233=(a663*a233); + a275=(a703*a607); + a275=(a658*a275); + a233=(a233-a275); + a259=(a259+a233); + a233=(a703*a666); + a233=(a665*a233); + a275=(a703*a669); + a275=(a668*a275); + a233=(a233+a275); + a275=(a259+a233); + a234=(a747*a671); + a234=(a670*a234); + a207=(a747*a675); + a207=(a674*a207); + a234=(a234+a207); + a275=(a275+a234); + a207=(a747*a677); + a207=(a676*a207); + a209=(a747*a681); + a209=(a680*a209); + a207=(a207+a209); + a275=(a275+a207); + a209=(a604*a296); + a240=(a598*a293); + a209=(a209+a240); + a209=(a643*a209); + a275=(a275-a209); + a293=(a590*a293); + a209=(a604*a290); + a293=(a293-a209); + a293=(a601*a293); + a275=(a275+a293); + a293=(a590*a230); + a290=(a597*a290); + a293=(a293-a290); + a293=(a559*a293); + a275=(a275-a293); + a296=(a597*a296); + a230=(a598*a230); + a296=(a296+a230); + a296=(a608*a296); + a275=(a275-a296); + if (res[0]!=0) res[0][191]=a275; + a275=(a749*a693); + a275=(a700*a275); + a296=(a749*a706); + a230=(a688*a296); + a293=(a749*a687); + a290=(a692*a293); + a230=(a230+a290); + a290=(a230/a686); + a230=(a684*a230); + a209=(a696*a296); + a240=(a689*a293); + a209=(a209-a240); + a240=(a702*a209); + a230=(a230+a240); + a240=(a691*a230); + a290=(a290-a240); + a290=(a662*a290); + a240=(a689*a290); + a209=(a209/a686); + a230=(a685*a230); + a209=(a209-a230); + a209=(a662*a209); + a230=(a692*a209); + a240=(a240+a230); + a240=(a697*a240); + a275=(a275+a240); + a240=(a688*a209); + a230=(a696*a290); + a240=(a240-a230); + a240=(a708*a240); + a749=(a749*a699); + a749=(a709*a749); + a240=(a240-a749); + a275=(a275+a240); + a240=(a764*a714); + a240=(a713*a240); + a749=(a764*a717); + a749=(a716*a749); + a240=(a240+a749); + a749=(a275+a240); + a230=(a766*a720); + a230=(a719*a230); + a129=(a766*a724); + a129=(a723*a129); + a230=(a230+a129); + a749=(a749+a230); + a129=(a766*a727); + a129=(a726*a129); + a235=(a766*a731); + a235=(a730*a235); + a129=(a129+a235); + a749=(a749+a129); + a235=(a654*a272); + a210=(a648*a262); + a235=(a235+a210); + a235=(a732*a235); + a749=(a749-a235); + a262=(a640*a262); + a235=(a654*a246); + a262=(a262-a235); + a262=(a733*a262); + a749=(a749+a262); + a262=(a640*a220); + a246=(a647*a246); + a262=(a262-a246); + a262=(a682*a262); + a749=(a749-a262); + a272=(a647*a272); + a220=(a648*a220); + a272=(a272+a220); + a272=(a734*a272); + a749=(a749-a272); + if (res[0]!=0) res[0][192]=a749; + a749=(a690*a290); + a272=(a678*a293); + a749=(a749-a272); + a749=(a711*a749); + a272=(a678*a296); + a290=(a657*a290); + a272=(a272+a290); + a272=(a751*a272); + a749=(a749-a272); + a272=(a690*a209); + a293=(a701*a293); + a272=(a272-a293); + a272=(a742*a272); + a749=(a749-a272); + a296=(a701*a296); + a209=(a657*a209); + a296=(a296+a209); + a296=(a705*a296); + a749=(a749-a296); + if (res[0]!=0) res[0][193]=a749; + a744=(a8*a744); + a45=(a8*a45); + a744=(a744+a45); + a783=(a8*a783); + a744=(a744+a783); + a745=(a8*a745); + a744=(a744+a745); + a735=(a8*a735); + a744=(a744+a735); + if (res[0]!=0) res[0][194]=a744; + a65=(a8*a65); + a99=(a8*a99); + a65=(a65+a99); + a816=(a8*a816); + a65=(a65+a816); + a86=(a8*a86); + a65=(a65+a86); + a57=(a8*a57); + a65=(a65+a57); + if (res[0]!=0) res[0][195]=a65; + a121=(a8*a121); + a119=(a8*a119); + a121=(a121+a119); + a120=(a8*a120); + a121=(a121+a120); + a56=(a8*a56); + a121=(a121+a56); + a198=(a8*a198); + a121=(a121+a198); + if (res[0]!=0) res[0][196]=a121; + a123=(a8*a123); + a201=(a8*a201); + a123=(a123+a201); + a738=(a8*a738); + a123=(a123+a738); + a155=(a8*a155); + a123=(a123+a155); + a177=(a8*a177); + a123=(a123+a177); + if (res[0]!=0) res[0][197]=a123; + a275=(a4*a275); + a123=(a5*a275); + a177=(a10*a275); + a240=(a38*a240); + a177=(a177+a240); + a240=(a5*a177); + a123=(a123+a240); + a240=(a10*a275); + a230=(a44*a230); + a240=(a240+a230); + a230=(a5*a240); + a123=(a123+a230); + a129=(a44*a129); + a129=(a275+a129); + a230=(a5*a129); + a123=(a123+a230); + a230=(a5*a123); + a259=(a4*a259); + a155=(a5*a259); + a230=(a230+a155); + a155=(a10*a259); + a233=(a38*a233); + a155=(a155+a233); + a233=(a5*a155); + a230=(a230+a233); + a233=(a10*a259); + a234=(a44*a234); + a233=(a233+a234); + a234=(a5*a233); + a230=(a230+a234); + a207=(a44*a207); + a207=(a259+a207); + a234=(a5*a207); + a230=(a230+a234); + a234=(a5*a230); + a277=(a4*a277); + a738=(a5*a277); + a234=(a234+a738); + a738=(a10*a277); + a264=(a38*a264); + a738=(a738+a264); + a264=(a5*a738); + a234=(a234+a264); + a264=(a10*a277); + a186=(a44*a186); + a264=(a264+a186); + a186=(a5*a264); + a234=(a234+a186); + a208=(a44*a208); + a208=(a277+a208); + a186=(a5*a208); + a234=(a234+a186); + a186=(a5*a234); + a265=(a4*a265); + a201=(a5*a265); + a186=(a186+a201); + a201=(a10*a265); + a269=(a38*a269); + a201=(a201+a269); + a269=(a5*a201); + a186=(a186+a269); + a269=(a10*a265); + a260=(a44*a260); + a269=(a269+a260); + a260=(a5*a269); + a186=(a186+a260); + a270=(a44*a270); + a270=(a265+a270); + a260=(a5*a270); + a186=(a186+a260); + a260=(a5*a186); + a250=(a4*a250); + a121=(a5*a250); + a260=(a260+a121); + a121=(a10*a250); + a274=(a38*a274); + a121=(a121+a274); + a274=(a5*a121); + a260=(a260+a274); + a274=(a10*a250); + a263=(a44*a263); + a274=(a274+a263); + a263=(a5*a274); + a260=(a260+a263); + a258=(a44*a258); + a258=(a250+a258); + a263=(a5*a258); + a260=(a260+a263); + a263=(a5*a260); + a291=(a4*a291); + a198=(a5*a291); + a263=(a263+a198); + a198=(a10*a291); + a278=(a38*a278); + a198=(a198+a278); + a278=(a5*a198); + a263=(a263+a278); + a278=(a10*a291); + a219=(a44*a219); + a278=(a278+a219); + a219=(a5*a278); + a263=(a263+a219); + a257=(a44*a257); + a257=(a291+a257); + a219=(a5*a257); + a263=(a263+a219); + a219=(a5*a263); + a261=(a4*a261); + a56=(a5*a261); + a219=(a219+a56); + a56=(a10*a261); + a185=(a38*a185); + a56=(a56+a185); + a185=(a5*a56); + a219=(a219+a185); + a185=(a10*a261); + a268=(a44*a268); + a185=(a185+a268); + a268=(a5*a185); + a219=(a219+a268); + a307=(a44*a307); + a307=(a261+a307); + a268=(a5*a307); + a219=(a219+a268); + a268=(a5*a219); + a297=(a4*a297); + a120=(a5*a297); + a268=(a268+a120); + a120=(a10*a297); + a251=(a38*a251); + a120=(a120+a251); + a251=(a5*a120); + a268=(a268+a251); + a251=(a10*a297); + a254=(a44*a254); + a251=(a251+a254); + a254=(a5*a251); + a268=(a268+a254); + a273=(a44*a273); + a273=(a297+a273); + a254=(a5*a273); + a268=(a268+a254); + a268=(a5*a268); + a284=(a4*a284); + a254=(a5*a284); + a268=(a268+a254); + a254=(a10*a284); + a287=(a38*a287); + a254=(a254+a287); + a254=(a5*a254); + a268=(a268+a254); + a254=(a10*a284); + a285=(a44*a285); + a254=(a254+a285); + a254=(a5*a254); + a268=(a268+a254); + a282=(a44*a282); + a284=(a284+a282); + a284=(a5*a284); + a268=(a268+a284); + a284=(a5*a268); + a253=(a4*a253); + a282=(a5*a253); + a284=(a284+a282); + a282=(a10*a253); + a281=(a38*a281); + a282=(a282+a281); + a281=(a5*a282); + a284=(a284+a281); + a281=(a10*a253); + a286=(a44*a286); + a281=(a281+a286); + a286=(a5*a281); + a284=(a284+a286); + a292=(a44*a292); + a292=(a253+a292); + a286=(a5*a292); + a284=(a284+a286); + a284=(a8*a284); + a286=(a50*a34); + a284=(a284-a286); + a254=(a50*a34); + a284=(a284-a254); + if (res[0]!=0) res[0][198]=a284; + a222=(a28*a222); + a279=(a279*a222); + a238=(a42*a238); + a289=(a289*a238); + a279=(a279+a289); + a244=(a42*a244); + a295=(a295*a244); + a279=(a279+a295); + a267=(a28*a267); + a280=(a280*a267); + a279=(a279-a280); + a252=(a42*a252); + a283=(a283*a252); + a279=(a279-a283); + a160=(a42*a160); + a288=(a288*a160); + a279=(a279-a288); + a279=(-a279); + if (res[0]!=0) res[0][199]=a279; + a268=(a8*a268); + a253=(a8*a253); + a268=(a268+a253); + a282=(a8*a282); + a268=(a268+a282); + a281=(a8*a281); + a268=(a268+a281); + a292=(a8*a292); + a268=(a268+a292); + a292=(a24*a34); + a268=(a268-a292); + a292=(a50*a34); + a268=(a268+a292); + a268=(a268+a286); + a268=(a268+a254); + a254=(a10*a34); + a268=(a268+a254); + if (res[0]!=0) res[0][200]=a268; + a268=sin(a317); + a268=(a268*a328); + a254=cos(a317); + a254=(a254*a305); + a268=(a268-a254); + a254=cos(a266); + a254=(a254*a305); + a268=(a268-a254); + a254=sin(a306); + a254=(a254*a331); + a286=cos(a306); + a286=(a286*a337); + a254=(a254-a286); + a268=(a268+a254); + a254=sin(a225); + a254=(a254*a336); + a286=cos(a225); + a286=(a286*a343); + a254=(a254-a286); + a268=(a268+a254); + a266=sin(a266); + a266=(a266*a328); + a268=(a268+a266); + if (res[0]!=0) res[0][201]=a268; + a268=cos(a317); + a266=(a23*a268); + a266=(a305*a266); + a254=cos(a306); + a286=(a759*a254); + a286=(a337*a286); + a266=(a266+a286); + a286=cos(a225); + a292=(a759*a286); + a292=(a343*a292); + a266=(a266+a292); + a317=sin(a317); + a292=(a23*a317); + a292=(a328*a292); + a266=(a266-a292); + a306=sin(a306); + a292=(a759*a306); + a292=(a331*a292); + a266=(a266-a292); + a225=sin(a225); + a292=(a759*a225); + a292=(a336*a292); + a266=(a266-a292); + a266=(-a266); + if (res[0]!=0) res[0][202]=a266; + a266=(a18*a268); + a266=(a305*a266); + a292=(a748*a254); + a292=(a337*a292); + a266=(a266+a292); + a292=(a748*a286); + a292=(a343*a292); + a266=(a266+a292); + a292=(a18*a317); + a292=(a328*a292); + a266=(a266-a292); + a292=(a748*a306); + a292=(a331*a292); + a266=(a266-a292); + a292=(a748*a225); + a292=(a336*a292); + a266=(a266-a292); + a266=(-a266); + if (res[0]!=0) res[0][203]=a266; + a266=(a15*a268); + a266=(a305*a266); + a292=(a21*a254); + a292=(a337*a292); + a266=(a266+a292); + a292=(a21*a286); + a292=(a343*a292); + a266=(a266+a292); + a292=(a15*a317); + a292=(a328*a292); + a266=(a266-a292); + a292=(a21*a306); + a292=(a331*a292); + a266=(a266-a292); + a292=(a21*a225); + a292=(a336*a292); + a266=(a266-a292); + a266=(-a266); + if (res[0]!=0) res[0][204]=a266; + a266=(a2*a268); + a266=(a305*a266); + a292=(a29*a254); + a292=(a337*a292); + a266=(a266+a292); + a292=(a29*a286); + a292=(a343*a292); + a266=(a266+a292); + a292=(a2*a317); + a292=(a328*a292); + a266=(a266-a292); + a292=(a29*a306); + a292=(a331*a292); + a266=(a266-a292); + a292=(a29*a225); + a292=(a336*a292); + a266=(a266-a292); + a266=(-a266); + if (res[0]!=0) res[0][205]=a266; + a266=(a51*a268); + a266=(a305*a266); + a292=(a32*a254); + a292=(a337*a292); + a266=(a266+a292); + a292=(a32*a286); + a292=(a343*a292); + a266=(a266+a292); + a292=(a51*a317); + a292=(a328*a292); + a266=(a266-a292); + a292=(a32*a306); + a292=(a331*a292); + a266=(a266-a292); + a292=(a32*a225); + a292=(a336*a292); + a266=(a266-a292); + a266=(-a266); + if (res[0]!=0) res[0][206]=a266; + a266=(a10*a34); + a292=(a10*a34); + a281=(a266+a292); + a281=(-a281); + if (res[0]!=0) res[0][207]=a281; + a281=(a41*a268); + a281=(a305*a281); + a282=(a35*a254); + a282=(a337*a282); + a281=(a281+a282); + a282=(a35*a286); + a282=(a343*a282); + a281=(a281+a282); + a282=(a41*a317); + a282=(a328*a282); + a281=(a281-a282); + a282=(a35*a306); + a282=(a331*a282); + a281=(a281-a282); + a282=(a35*a225); + a282=(a336*a282); + a281=(a281-a282); + a281=(-a281); + if (res[0]!=0) res[0][208]=a281; + a281=(a3*a34); + a282=(a10*a34); + a253=(a281-a282); + a266=(a266-a253); + a266=(a266+a292); + a292=(a10*a34); + a266=(a266+a292); + if (res[0]!=0) res[0][209]=a266; + a325=(a28*a325); + a303=(a303*a325); + a301=(a28*a301); + a325=(a276*a301); + a323=(a28*a323); + a266=(a308*a323); + a325=(a325+a266); + a266=(a325/a320); + a310=(a310*a325); + a325=(a309*a301); + a292=(a313*a323); + a325=(a325-a292); + a314=(a314*a325); + a310=(a310+a314); + a300=(a300*a310); + a266=(a266-a300); + a266=(a302*a266); + a313=(a313*a266); + a325=(a325/a320); + a319=(a319*a310); + a325=(a325-a319); + a302=(a302*a325); + a308=(a308*a302); + a313=(a313+a308); + a324=(a324*a313); + a303=(a303+a324); + a276=(a276*a302); + a309=(a309*a266); + a276=(a276-a309); + a327=(a327*a276); + a271=(a28*a271); + a322=(a322*a271); + a327=(a327-a322); + a303=(a303+a327); + a330=(a28*a330); + a329=(a329*a330); + a333=(a28*a333); + a332=(a332*a333); + a329=(a329+a332); + a332=(a303+a329); + a335=(a42*a335); + a334=(a334*a335); + a339=(a42*a339); + a338=(a338*a339); + a334=(a334+a338); + a332=(a332+a334); + a341=(a42*a341); + a340=(a340*a341); + a345=(a42*a345); + a344=(a344*a345); + a340=(a340+a344); + a332=(a332+a340); + if (res[0]!=0) res[0][210]=a332; + a332=(a41*a371); + a332=(a349*a332); + a344=(a41*a346); + a345=(a353*a344); + a341=(a41*a369); + a338=(a354*a341); + a345=(a345+a338); + a338=(a345/a366); + a345=(a356*a345); + a339=(a355*a344); + a335=(a359*a341); + a339=(a339-a335); + a335=(a360*a339); + a345=(a345+a335); + a335=(a326*a345); + a338=(a338-a335); + a338=(a342*a338); + a335=(a359*a338); + a339=(a339/a366); + a345=(a365*a345); + a339=(a339-a345); + a339=(a342*a339); + a345=(a354*a339); + a335=(a335+a345); + a335=(a370*a335); + a332=(a332+a335); + a335=(a353*a339); + a345=(a355*a338); + a335=(a335-a345); + a335=(a373*a335); + a345=(a41*a315); + a345=(a374*a345); + a335=(a335-a345); + a332=(a332+a335); + a335=(a41*a378); + a335=(a377*a335); + a345=(a41*a381); + a345=(a380*a345); + a335=(a335+a345); + a345=(a332+a335); + a333=(a35*a383); + a333=(a382*a333); + a330=(a35*a387); + a330=(a386*a330); + a333=(a333+a330); + a345=(a345+a333); + a330=(a35*a389); + a330=(a388*a330); + a327=(a35*a393); + a327=(a392*a327); + a330=(a330+a327); + a345=(a345+a330); + a327=(a318*a301); + a322=(a312*a266); + a327=(a327+a322); + a390=(a390*a327); + a345=(a345-a390); + a266=(a304*a266); + a318=(a318*a323); + a266=(a266-a318); + a394=(a394*a266); + a345=(a345+a394); + a304=(a304*a302); + a323=(a311*a323); + a304=(a304-a323); + a351=(a351*a304); + a345=(a345-a351); + a311=(a311*a301); + a312=(a312*a302); + a311=(a311+a312); + a395=(a395*a311); + a345=(a345-a395); + if (res[0]!=0) res[0][211]=a345; + a345=(a51*a421); + a345=(a399*a345); + a395=(a51*a397); + a311=(a372*a395); + a312=(a51*a419); + a302=(a404*a312); + a311=(a311+a302); + a302=(a311/a416); + a311=(a406*a311); + a301=(a405*a395); + a351=(a409*a312); + a301=(a301-a351); + a351=(a410*a301); + a311=(a311+a351); + a351=(a396*a311); + a302=(a302-a351); + a302=(a398*a302); + a351=(a409*a302); + a301=(a301/a416); + a311=(a415*a311); + a301=(a301-a311); + a301=(a398*a301); + a311=(a404*a301); + a351=(a351+a311); + a351=(a420*a351); + a345=(a345+a351); + a351=(a372*a301); + a311=(a405*a302); + a351=(a351-a311); + a351=(a423*a351); + a311=(a51*a367); + a311=(a418*a311); + a351=(a351-a311); + a345=(a345+a351); + a351=(a51*a426); + a351=(a425*a351); + a311=(a51*a429); + a311=(a428*a311); + a351=(a351+a311); + a311=(a345+a351); + a304=(a32*a431); + a304=(a430*a304); + a323=(a32*a435); + a323=(a434*a323); + a304=(a304+a323); + a311=(a311+a304); + a323=(a32*a437); + a323=(a436*a323); + a394=(a32*a441); + a394=(a440*a394); + a323=(a323+a394); + a311=(a311+a323); + a394=(a364*a344); + a266=(a358*a338); + a394=(a394+a266); + a394=(a403*a394); + a311=(a311-a394); + a338=(a350*a338); + a394=(a364*a341); + a338=(a338-a394); + a338=(a361*a338); + a311=(a311+a338); + a338=(a350*a339); + a341=(a357*a341); + a338=(a338-a341); + a338=(a347*a338); + a311=(a311-a338); + a344=(a357*a344); + a339=(a358*a339); + a344=(a344+a339); + a344=(a368*a344); + a311=(a311-a344); + if (res[0]!=0) res[0][212]=a311; + a311=(a14*a467); + a311=(a445*a311); + a344=(a14*a442); + a339=(a449*a344); + a338=(a14*a465); + a341=(a450*a338); + a339=(a339+a341); + a341=(a339/a462); + a339=(a452*a339); + a394=(a451*a344); + a266=(a455*a338); + a394=(a394-a266); + a266=(a456*a394); + a339=(a339+a266); + a266=(a422*a339); + a341=(a341-a266); + a341=(a438*a341); + a266=(a455*a341); + a394=(a394/a462); + a339=(a461*a339); + a394=(a394-a339); + a394=(a438*a394); + a339=(a450*a394); + a266=(a266+a339); + a266=(a466*a266); + a311=(a311+a266); + a266=(a449*a394); + a339=(a451*a341); + a266=(a266-a339); + a266=(a469*a266); + a339=(a14*a411); + a339=(a470*a339); + a266=(a266-a339); + a311=(a311+a266); + a266=(a2*a474); + a266=(a473*a266); + a339=(a2*a477); + a339=(a476*a339); + a266=(a266+a339); + a339=(a311+a266); + a318=(a29*a479); + a318=(a478*a318); + a390=(a29*a483); + a390=(a482*a390); + a318=(a318+a390); + a339=(a339+a318); + a390=(a29*a485); + a390=(a484*a390); + a327=(a29*a489); + a327=(a488*a327); + a390=(a390+a327); + a339=(a339+a390); + a327=(a414*a395); + a322=(a408*a302); + a327=(a327+a322); + a327=(a486*a327); + a339=(a339-a327); + a302=(a400*a302); + a327=(a414*a312); + a302=(a302-a327); + a302=(a490*a302); + a339=(a339+a302); + a302=(a400*a301); + a312=(a407*a312); + a302=(a302-a312); + a302=(a447*a302); + a339=(a339-a302); + a395=(a407*a395); + a301=(a408*a301); + a395=(a395+a301); + a395=(a491*a395); + a339=(a339-a395); + if (res[0]!=0) res[0][213]=a339; + a339=(a20*a517); + a339=(a495*a339); + a395=(a20*a493); + a301=(a468*a395); + a302=(a20*a515); + a312=(a500*a302); + a301=(a301+a312); + a312=(a301/a512); + a301=(a502*a301); + a327=(a501*a395); + a322=(a505*a302); + a327=(a327-a322); + a322=(a506*a327); + a301=(a301+a322); + a322=(a492*a301); + a312=(a312-a322); + a312=(a494*a312); + a322=(a505*a312); + a327=(a327/a512); + a301=(a511*a301); + a327=(a327-a301); + a327=(a494*a327); + a301=(a500*a327); + a322=(a322+a301); + a322=(a516*a322); + a339=(a339+a322); + a322=(a468*a327); + a301=(a501*a312); + a322=(a322-a301); + a322=(a519*a322); + a301=(a20*a463); + a301=(a514*a301); + a322=(a322-a301); + a339=(a339+a322); + a322=(a15*a522); + a322=(a521*a322); + a301=(a15*a525); + a301=(a524*a301); + a322=(a322+a301); + a301=(a339+a322); + a271=(a21*a527); + a271=(a526*a271); + a276=(a21*a531); + a276=(a530*a276); + a271=(a271+a276); + a301=(a301+a271); + a276=(a21*a533); + a276=(a532*a276); + a309=(a21*a537); + a309=(a536*a309); + a276=(a276+a309); + a301=(a301+a276); + a309=(a460*a344); + a324=(a454*a341); + a309=(a309+a324); + a309=(a499*a309); + a301=(a301-a309); + a341=(a446*a341); + a309=(a460*a338); + a341=(a341-a309); + a341=(a457*a341); + a301=(a301+a341); + a341=(a446*a394); + a338=(a453*a338); + a341=(a341-a338); + a341=(a443*a341); + a301=(a301-a341); + a344=(a453*a344); + a394=(a454*a394); + a344=(a344+a394); + a344=(a464*a344); + a301=(a301-a344); + if (res[0]!=0) res[0][214]=a301; + a301=(a18*a563); + a301=(a541*a301); + a344=(a18*a538); + a394=(a545*a344); + a341=(a18*a561); + a338=(a546*a341); + a394=(a394+a338); + a338=(a394/a558); + a394=(a548*a394); + a309=(a547*a344); + a324=(a551*a341); + a309=(a309-a324); + a324=(a552*a309); + a394=(a394+a324); + a324=(a518*a394); + a338=(a338-a324); + a338=(a534*a338); + a324=(a551*a338); + a309=(a309/a558); + a394=(a557*a394); + a309=(a309-a394); + a309=(a534*a309); + a394=(a546*a309); + a324=(a324+a394); + a324=(a562*a324); + a301=(a301+a324); + a324=(a545*a309); + a394=(a547*a338); + a324=(a324-a394); + a324=(a565*a324); + a394=(a18*a507); + a394=(a560*a394); + a324=(a324-a394); + a301=(a301+a324); + a324=(a18*a568); + a324=(a567*a324); + a394=(a18*a571); + a394=(a570*a394); + a324=(a324+a394); + a394=(a301+a324); + a313=(a748*a573); + a313=(a572*a313); + a308=(a748*a577); + a308=(a576*a308); + a313=(a313+a308); + a394=(a394+a313); + a308=(a748*a579); + a308=(a578*a308); + a325=(a748*a583); + a325=(a582*a325); + a308=(a308+a325); + a394=(a394+a308); + a325=(a510*a395); + a319=(a504*a312); + a325=(a325+a319); + a325=(a580*a325); + a394=(a394-a325); + a312=(a496*a312); + a325=(a510*a302); + a312=(a312-a325); + a312=(a584*a312); + a394=(a394+a312); + a312=(a496*a327); + a302=(a503*a302); + a312=(a312-a302); + a312=(a564*a312); + a394=(a394-a312); + a395=(a503*a395); + a327=(a504*a327); + a395=(a395+a327); + a395=(a585*a395); + a394=(a394-a395); + if (res[0]!=0) res[0][215]=a394; + a394=(a23*a611); + a394=(a589*a394); + a395=(a23*a587); + a327=(a593*a395); + a312=(a23*a609); + a302=(a594*a312); + a327=(a327+a302); + a302=(a327/a606); + a327=(a596*a327); + a325=(a595*a395); + a319=(a599*a312); + a325=(a325-a319); + a319=(a600*a325); + a327=(a327+a319); + a319=(a586*a327); + a302=(a302-a319); + a302=(a588*a302); + a319=(a599*a302); + a325=(a325/a606); + a327=(a605*a327); + a325=(a325-a327); + a325=(a588*a325); + a327=(a594*a325); + a319=(a319+a327); + a319=(a610*a319); + a394=(a394+a319); + a319=(a593*a325); + a327=(a595*a302); + a319=(a319-a327); + a319=(a613*a319); + a327=(a23*a553); + a327=(a614*a327); + a319=(a319-a327); + a394=(a394+a319); + a319=(a23*a618); + a319=(a617*a319); + a327=(a23*a621); + a327=(a620*a327); + a319=(a319+a327); + a327=(a394+a319); + a310=(a759*a623); + a310=(a622*a310); + a320=(a759*a627); + a320=(a626*a320); + a310=(a310+a320); + a327=(a327+a310); + a320=(a759*a629); + a320=(a628*a320); + a300=(a759*a633); + a300=(a632*a300); + a320=(a320+a300); + a327=(a327+a320); + a300=(a556*a344); + a314=(a550*a338); + a300=(a300+a314); + a300=(a630*a300); + a327=(a327-a300); + a338=(a542*a338); + a300=(a556*a341); + a338=(a338-a300); + a338=(a634*a338); + a327=(a327+a338); + a338=(a542*a309); + a341=(a549*a341); + a338=(a338-a341); + a338=(a591*a338); + a327=(a327-a338); + a344=(a549*a344); + a309=(a550*a309); + a344=(a344+a309); + a344=(a635*a344); + a327=(a327-a344); + if (res[0]!=0) res[0][216]=a327; + a327=(a27*a661); + a327=(a639*a327); + a344=(a27*a637); + a309=(a612*a344); + a338=(a27*a659); + a341=(a644*a338); + a309=(a309+a341); + a341=(a309/a656); + a309=(a646*a309); + a300=(a645*a344); + a314=(a649*a338); + a300=(a300-a314); + a314=(a650*a300); + a309=(a309+a314); + a314=(a636*a309); + a341=(a341-a314); + a341=(a638*a341); + a314=(a649*a341); + a300=(a300/a656); + a309=(a655*a309); + a300=(a300-a309); + a300=(a638*a300); + a309=(a644*a300); + a314=(a314+a309); + a314=(a660*a314); + a327=(a327+a314); + a314=(a612*a300); + a309=(a645*a341); + a314=(a314-a309); + a314=(a663*a314); + a309=(a27*a607); + a309=(a658*a309); + a314=(a314-a309); + a327=(a327+a314); + a314=(a760*a666); + a314=(a665*a314); + a309=(a760*a669); + a309=(a668*a309); + a314=(a314+a309); + a309=(a327+a314); + a292=(a750*a671); + a292=(a670*a292); + a253=(a750*a675); + a253=(a674*a253); + a292=(a292+a253); + a309=(a309+a292); + a253=(a750*a677); + a253=(a676*a253); + a279=(a750*a681); + a279=(a680*a279); + a253=(a253+a279); + a309=(a309+a253); + a279=(a604*a395); + a288=(a598*a302); + a279=(a279+a288); + a279=(a643*a279); + a309=(a309-a279); + a302=(a590*a302); + a279=(a604*a312); + a302=(a302-a279); + a302=(a601*a302); + a309=(a309+a302); + a302=(a590*a325); + a312=(a597*a312); + a302=(a302-a312); + a302=(a559*a302); + a309=(a309-a302); + a395=(a597*a395); + a325=(a598*a325); + a395=(a395+a325); + a395=(a608*a395); + a309=(a309-a395); + if (res[0]!=0) res[0][217]=a309; + a309=(a703*a693); + a309=(a700*a309); + a395=(a703*a706); + a325=(a688*a395); + a302=(a703*a687); + a312=(a692*a302); + a325=(a325+a312); + a312=(a325/a686); + a325=(a684*a325); + a279=(a696*a395); + a288=(a689*a302); + a279=(a279-a288); + a288=(a702*a279); + a325=(a325+a288); + a288=(a691*a325); + a312=(a312-a288); + a312=(a662*a312); + a288=(a689*a312); + a279=(a279/a686); + a325=(a685*a325); + a279=(a279-a325); + a279=(a662*a279); + a325=(a692*a279); + a288=(a288+a325); + a288=(a697*a288); + a309=(a309+a288); + a288=(a688*a279); + a325=(a696*a312); + a288=(a288-a325); + a288=(a708*a288); + a325=(a703*a699); + a325=(a709*a325); + a288=(a288-a325); + a309=(a309+a288); + a288=(a703*a714); + a288=(a713*a288); + a325=(a703*a717); + a325=(a716*a325); + a288=(a288+a325); + a325=(a309+a288); + a160=(a747*a720); + a160=(a719*a160); + a283=(a747*a724); + a283=(a723*a283); + a160=(a160+a283); + a325=(a325+a160); + a283=(a747*a727); + a283=(a726*a283); + a252=(a747*a731); + a252=(a730*a252); + a283=(a283+a252); + a325=(a325+a283); + a252=(a654*a344); + a280=(a648*a341); + a252=(a252+a280); + a252=(a732*a252); + a325=(a325-a252); + a341=(a640*a341); + a252=(a654*a338); + a341=(a341-a252); + a341=(a733*a341); + a325=(a325+a341); + a341=(a640*a300); + a338=(a647*a338); + a341=(a341-a338); + a341=(a682*a341); + a325=(a325-a341); + a344=(a647*a344); + a300=(a648*a300); + a344=(a344+a300); + a344=(a734*a344); + a325=(a325-a344); + if (res[0]!=0) res[0][218]=a325; + a325=(a690*a312); + a344=(a678*a302); + a325=(a325-a344); + a325=(a711*a325); + a344=(a678*a395); + a312=(a657*a312); + a344=(a344+a312); + a344=(a751*a344); + a325=(a325-a344); + a344=(a690*a279); + a302=(a701*a302); + a344=(a344-a302); + a344=(a742*a344); + a325=(a325-a344); + a395=(a701*a395); + a279=(a657*a279); + a395=(a395+a279); + a395=(a705*a395); + a325=(a325-a395); + if (res[0]!=0) res[0][219]=a325; + a758=(a8*a758); + a81=(a8*a81); + a758=(a758+a81); + a782=(a8*a782); + a758=(a758+a782); + a752=(a8*a752); + a758=(a758+a752); + a755=(a8*a755); + a758=(a758+a755); + if (res[0]!=0) res[0][220]=a758; + a743=(a8*a743); + a80=(a8*a80); + a743=(a743+a80); + a815=(a8*a815); + a743=(a743+a815); + a79=(a8*a79); + a743=(a743+a79); + a66=(a8*a66); + a743=(a743+a66); + if (res[0]!=0) res[0][221]=a743; + a118=(a8*a118); + a147=(a8*a147); + a118=(a118+a147); + a110=(a8*a110); + a118=(a118+a110); + a130=(a8*a130); + a118=(a118+a130); + a84=(a8*a84); + a118=(a118+a84); + if (res[0]!=0) res[0][222]=a118; + a167=(a8*a167); + a165=(a8*a165); + a167=(a167+a165); + a55=(a8*a55); + a167=(a167+a55); + a89=(a8*a89); + a167=(a167+a89); + a211=(a8*a211); + a167=(a167+a211); + if (res[0]!=0) res[0][223]=a167; + a175=(a8*a175); + a249=(a8*a249); + a175=(a175+a249); + a76=(a8*a76); + a175=(a175+a76); + a255=(a8*a255); + a175=(a175+a255); + a227=(a8*a227); + a175=(a175+a227); + if (res[0]!=0) res[0][224]=a175; + a309=(a4*a309); + a175=(a5*a309); + a227=(a10*a309); + a288=(a38*a288); + a227=(a227+a288); + a288=(a5*a227); + a175=(a175+a288); + a288=(a10*a309); + a160=(a44*a160); + a288=(a288+a160); + a160=(a5*a288); + a175=(a175+a160); + a283=(a44*a283); + a283=(a309+a283); + a160=(a5*a283); + a175=(a175+a160); + a160=(a5*a175); + a327=(a4*a327); + a255=(a5*a327); + a160=(a160+a255); + a255=(a10*a327); + a314=(a38*a314); + a255=(a255+a314); + a314=(a5*a255); + a160=(a160+a314); + a314=(a10*a327); + a292=(a44*a292); + a314=(a314+a292); + a292=(a5*a314); + a160=(a160+a292); + a253=(a44*a253); + a253=(a327+a253); + a292=(a5*a253); + a160=(a160+a292); + a292=(a5*a160); + a394=(a4*a394); + a76=(a5*a394); + a292=(a292+a76); + a76=(a10*a394); + a319=(a38*a319); + a76=(a76+a319); + a319=(a5*a76); + a292=(a292+a319); + a319=(a10*a394); + a310=(a44*a310); + a319=(a319+a310); + a310=(a5*a319); + a292=(a292+a310); + a320=(a44*a320); + a320=(a394+a320); + a310=(a5*a320); + a292=(a292+a310); + a310=(a5*a292); + a301=(a4*a301); + a249=(a5*a301); + a310=(a310+a249); + a249=(a10*a301); + a324=(a38*a324); + a249=(a249+a324); + a324=(a5*a249); + a310=(a310+a324); + a324=(a10*a301); + a313=(a44*a313); + a324=(a324+a313); + a313=(a5*a324); + a310=(a310+a313); + a308=(a44*a308); + a308=(a301+a308); + a313=(a5*a308); + a310=(a310+a313); + a313=(a5*a310); + a339=(a4*a339); + a167=(a5*a339); + a313=(a313+a167); + a167=(a10*a339); + a322=(a38*a322); + a167=(a167+a322); + a322=(a5*a167); + a313=(a313+a322); + a322=(a10*a339); + a271=(a44*a271); + a322=(a322+a271); + a271=(a5*a322); + a313=(a313+a271); + a276=(a44*a276); + a276=(a339+a276); + a271=(a5*a276); + a313=(a313+a271); + a271=(a5*a313); + a311=(a4*a311); + a211=(a5*a311); + a271=(a271+a211); + a211=(a10*a311); + a266=(a38*a266); + a211=(a211+a266); + a266=(a5*a211); + a271=(a271+a266); + a266=(a10*a311); + a318=(a44*a318); + a266=(a266+a318); + a318=(a5*a266); + a271=(a271+a318); + a390=(a44*a390); + a390=(a311+a390); + a318=(a5*a390); + a271=(a271+a318); + a318=(a5*a271); + a345=(a4*a345); + a89=(a5*a345); + a318=(a318+a89); + a89=(a10*a345); + a351=(a38*a351); + a89=(a89+a351); + a351=(a5*a89); + a318=(a318+a351); + a351=(a10*a345); + a304=(a44*a304); + a351=(a351+a304); + a304=(a5*a351); + a318=(a318+a304); + a323=(a44*a323); + a323=(a345+a323); + a304=(a5*a323); + a318=(a318+a304); + a318=(a5*a318); + a332=(a4*a332); + a304=(a5*a332); + a318=(a318+a304); + a304=(a10*a332); + a335=(a38*a335); + a304=(a304+a335); + a304=(a5*a304); + a318=(a318+a304); + a304=(a10*a332); + a333=(a44*a333); + a304=(a304+a333); + a304=(a5*a304); + a318=(a318+a304); + a330=(a44*a330); + a332=(a332+a330); + a332=(a5*a332); + a318=(a318+a332); + a332=(a5*a318); + a303=(a4*a303); + a330=(a5*a303); + a332=(a332+a330); + a330=(a10*a303); + a329=(a38*a329); + a330=(a330+a329); + a329=(a5*a330); + a332=(a332+a329); + a329=(a10*a303); + a334=(a44*a334); + a329=(a329+a334); + a334=(a5*a329); + a332=(a332+a334); + a340=(a44*a340); + a340=(a303+a340); + a334=(a5*a340); + a332=(a332+a334); + a332=(a8*a332); + a334=(a50*a34); + a332=(a332-a334); + a304=(a50*a34); + a332=(a332-a304); + if (res[0]!=0) res[0][225]=a332; + a268=(a28*a268); + a305=(a305*a268); + a254=(a42*a254); + a337=(a337*a254); + a305=(a305+a337); + a286=(a42*a286); + a343=(a343*a286); + a305=(a305+a343); + a317=(a28*a317); + a328=(a328*a317); + a305=(a305-a328); + a306=(a42*a306); + a331=(a331*a306); + a305=(a305-a331); + a225=(a42*a225); + a336=(a336*a225); + a305=(a305-a336); + a305=(-a305); + if (res[0]!=0) res[0][226]=a305; + a318=(a8*a318); + a303=(a8*a303); + a318=(a318+a303); + a330=(a8*a330); + a318=(a318+a330); + a329=(a8*a329); + a318=(a318+a329); + a340=(a8*a340); + a318=(a318+a340); + a340=(a24*a34); + a318=(a318-a340); + a340=(a50*a34); + a318=(a318+a340); + a318=(a318+a334); + a318=(a318+a304); + a304=(a10*a34); + a318=(a318+a304); + if (res[0]!=0) res[0][227]=a318; + a318=cos(a363); + a304=(a375*a318); + a334=cos(a348); + a340=(a385*a334); + a304=(a304+a340); + a340=cos(a256); + a329=(a391*a340); + a304=(a304+a329); + a329=cos(a316); + a329=(a375*a329); + a304=(a304+a329); + a363=sin(a363); + a329=(a376*a363); + a304=(a304-a329); + a348=sin(a348); + a329=(a379*a348); + a304=(a304-a329); + a256=sin(a256); + a329=(a384*a256); + a304=(a304-a329); + a316=sin(a316); + a316=(a376*a316); + a304=(a304-a316); + a304=(-a304); + if (res[0]!=0) res[0][228]=a304; + a304=(a760*a318); + a304=(a375*a304); + a316=(a750*a334); + a316=(a385*a316); + a304=(a304+a316); + a316=(a750*a340); + a316=(a391*a316); + a304=(a304+a316); + a316=(a760*a363); + a316=(a376*a316); + a304=(a304-a316); + a316=(a750*a348); + a316=(a379*a316); + a304=(a304-a316); + a316=(a750*a256); + a316=(a384*a316); + a304=(a304-a316); + a304=(-a304); + if (res[0]!=0) res[0][229]=a304; + a304=(a23*a318); + a304=(a375*a304); + a316=(a759*a334); + a316=(a385*a316); + a304=(a304+a316); + a316=(a759*a340); + a316=(a391*a316); + a304=(a304+a316); + a316=(a23*a363); + a316=(a376*a316); + a304=(a304-a316); + a316=(a759*a348); + a316=(a379*a316); + a304=(a304-a316); + a316=(a759*a256); + a316=(a384*a316); + a304=(a304-a316); + a304=(-a304); + if (res[0]!=0) res[0][230]=a304; + a304=(a18*a318); + a304=(a375*a304); + a316=(a748*a334); + a316=(a385*a316); + a304=(a304+a316); + a316=(a748*a340); + a316=(a391*a316); + a304=(a304+a316); + a316=(a18*a363); + a316=(a376*a316); + a304=(a304-a316); + a316=(a748*a348); + a316=(a379*a316); + a304=(a304-a316); + a316=(a748*a256); + a316=(a384*a316); + a304=(a304-a316); + a304=(-a304); + if (res[0]!=0) res[0][231]=a304; + a304=(a15*a318); + a304=(a375*a304); + a316=(a21*a334); + a316=(a385*a316); + a304=(a304+a316); + a316=(a21*a340); + a316=(a391*a316); + a304=(a304+a316); + a316=(a15*a363); + a316=(a376*a316); + a304=(a304-a316); + a316=(a21*a348); + a316=(a379*a316); + a304=(a304-a316); + a316=(a21*a256); + a316=(a384*a316); + a304=(a304-a316); + a304=(-a304); + if (res[0]!=0) res[0][232]=a304; + a304=(a2*a318); + a304=(a375*a304); + a316=(a29*a334); + a316=(a385*a316); + a304=(a304+a316); + a316=(a29*a340); + a316=(a391*a316); + a304=(a304+a316); + a316=(a2*a363); + a316=(a376*a316); + a304=(a304-a316); + a316=(a29*a348); + a316=(a379*a316); + a304=(a304-a316); + a316=(a29*a256); + a316=(a384*a316); + a304=(a304-a316); + a304=(-a304); + if (res[0]!=0) res[0][233]=a304; + a304=(a51*a318); + a304=(a375*a304); + a316=(a32*a334); + a316=(a385*a316); + a304=(a304+a316); + a316=(a32*a340); + a316=(a391*a316); + a304=(a304+a316); + a316=(a51*a363); + a316=(a376*a316); + a304=(a304-a316); + a316=(a32*a348); + a316=(a379*a316); + a304=(a304-a316); + a316=(a32*a256); + a316=(a384*a316); + a304=(a304-a316); + a304=(-a304); + if (res[0]!=0) res[0][234]=a304; + a281=(a281-a282); + if (res[0]!=0) res[0][235]=a281; + a281=(a41*a318); + a281=(a375*a281); + a282=(a35*a334); + a282=(a385*a282); + a281=(a281+a282); + a282=(a35*a340); + a282=(a391*a282); + a281=(a281+a282); + a282=(a41*a363); + a282=(a376*a282); + a281=(a281-a282); + a282=(a35*a348); + a282=(a379*a282); + a281=(a281-a282); + a282=(a35*a256); + a282=(a384*a282); + a281=(a281-a282); + a281=(-a281); + if (res[0]!=0) res[0][236]=a281; + a281=(a10*a34); + a282=(a3*a34); + a304=(a10*a34); + a282=(a282-a304); + a281=(a281-a282); + a282=(a10*a34); + a281=(a281+a282); + a282=(a10*a34); + a281=(a281+a282); + if (res[0]!=0) res[0][237]=a281; + a371=(a28*a371); + a349=(a349*a371); + a346=(a28*a346); + a371=(a353*a346); + a369=(a28*a369); + a281=(a354*a369); + a371=(a371+a281); + a281=(a371/a366); + a356=(a356*a371); + a371=(a355*a346); + a282=(a359*a369); + a371=(a371-a282); + a360=(a360*a371); + a356=(a356+a360); + a326=(a326*a356); + a281=(a281-a326); + a281=(a342*a281); + a359=(a359*a281); + a371=(a371/a366); + a365=(a365*a356); + a371=(a371-a365); + a342=(a342*a371); + a354=(a354*a342); + a359=(a359+a354); + a370=(a370*a359); + a349=(a349+a370); + a353=(a353*a342); + a355=(a355*a281); + a353=(a353-a355); + a373=(a373*a353); + a315=(a28*a315); + a374=(a374*a315); + a373=(a373-a374); + a349=(a349+a373); + a378=(a28*a378); + a377=(a377*a378); + a381=(a28*a381); + a380=(a380*a381); + a377=(a377+a380); + a380=(a349+a377); + a383=(a42*a383); + a382=(a382*a383); + a387=(a42*a387); + a386=(a386*a387); + a382=(a382+a386); + a380=(a380+a382); + a389=(a42*a389); + a388=(a388*a389); + a393=(a42*a393); + a392=(a392*a393); + a388=(a388+a392); + a380=(a380+a388); + if (res[0]!=0) res[0][238]=a380; + a380=(a41*a421); + a380=(a399*a380); + a392=(a41*a397); + a393=(a372*a392); + a389=(a41*a419); + a386=(a404*a389); + a393=(a393+a386); + a386=(a393/a416); + a393=(a406*a393); + a387=(a405*a392); + a383=(a409*a389); + a387=(a387-a383); + a383=(a410*a387); + a393=(a393+a383); + a383=(a396*a393); + a386=(a386-a383); + a386=(a398*a386); + a383=(a409*a386); + a387=(a387/a416); + a393=(a415*a393); + a387=(a387-a393); + a387=(a398*a387); + a393=(a404*a387); + a383=(a383+a393); + a383=(a420*a383); + a380=(a380+a383); + a383=(a372*a387); + a393=(a405*a386); + a383=(a383-a393); + a383=(a423*a383); + a393=(a41*a367); + a393=(a418*a393); + a383=(a383-a393); + a380=(a380+a383); + a383=(a41*a426); + a383=(a425*a383); + a393=(a41*a429); + a393=(a428*a393); + a383=(a383+a393); + a393=(a380+a383); + a381=(a35*a431); + a381=(a430*a381); + a378=(a35*a435); + a378=(a434*a378); + a381=(a381+a378); + a393=(a393+a381); + a378=(a35*a437); + a378=(a436*a378); + a373=(a35*a441); + a373=(a440*a373); + a378=(a378+a373); + a393=(a393+a378); + a373=(a364*a346); + a374=(a358*a281); + a373=(a373+a374); + a403=(a403*a373); + a393=(a393-a403); + a281=(a350*a281); + a364=(a364*a369); + a281=(a281-a364); + a361=(a361*a281); + a393=(a393+a361); + a350=(a350*a342); + a369=(a357*a369); + a350=(a350-a369); + a347=(a347*a350); + a393=(a393-a347); + a357=(a357*a346); + a358=(a358*a342); + a357=(a357+a358); + a368=(a368*a357); + a393=(a393-a368); + if (res[0]!=0) res[0][239]=a393; + a393=(a51*a467); + a393=(a445*a393); + a368=(a51*a442); + a357=(a449*a368); + a358=(a51*a465); + a342=(a450*a358); + a357=(a357+a342); + a342=(a357/a462); + a357=(a452*a357); + a346=(a451*a368); + a347=(a455*a358); + a346=(a346-a347); + a347=(a456*a346); + a357=(a357+a347); + a347=(a422*a357); + a342=(a342-a347); + a342=(a438*a342); + a347=(a455*a342); + a346=(a346/a462); + a357=(a461*a357); + a346=(a346-a357); + a346=(a438*a346); + a357=(a450*a346); + a347=(a347+a357); + a347=(a466*a347); + a393=(a393+a347); + a347=(a449*a346); + a357=(a451*a342); + a347=(a347-a357); + a347=(a469*a347); + a357=(a51*a411); + a357=(a470*a357); + a347=(a347-a357); + a393=(a393+a347); + a347=(a51*a474); + a347=(a473*a347); + a357=(a51*a477); + a357=(a476*a357); + a347=(a347+a357); + a357=(a393+a347); + a350=(a32*a479); + a350=(a478*a350); + a369=(a32*a483); + a369=(a482*a369); + a350=(a350+a369); + a357=(a357+a350); + a369=(a32*a485); + a369=(a484*a369); + a361=(a32*a489); + a361=(a488*a361); + a369=(a369+a361); + a357=(a357+a369); + a361=(a414*a392); + a281=(a408*a386); + a361=(a361+a281); + a361=(a486*a361); + a357=(a357-a361); + a386=(a400*a386); + a361=(a414*a389); + a386=(a386-a361); + a386=(a490*a386); + a357=(a357+a386); + a386=(a400*a387); + a389=(a407*a389); + a386=(a386-a389); + a386=(a447*a386); + a357=(a357-a386); + a392=(a407*a392); + a387=(a408*a387); + a392=(a392+a387); + a392=(a491*a392); + a357=(a357-a392); + if (res[0]!=0) res[0][240]=a357; + a357=(a14*a517); + a357=(a495*a357); + a392=(a14*a493); + a387=(a468*a392); + a386=(a14*a515); + a389=(a500*a386); + a387=(a387+a389); + a389=(a387/a512); + a387=(a502*a387); + a361=(a501*a392); + a281=(a505*a386); + a361=(a361-a281); + a281=(a506*a361); + a387=(a387+a281); + a281=(a492*a387); + a389=(a389-a281); + a389=(a494*a389); + a281=(a505*a389); + a361=(a361/a512); + a387=(a511*a387); + a361=(a361-a387); + a361=(a494*a361); + a387=(a500*a361); + a281=(a281+a387); + a281=(a516*a281); + a357=(a357+a281); + a281=(a468*a361); + a387=(a501*a389); + a281=(a281-a387); + a281=(a519*a281); + a387=(a14*a463); + a387=(a514*a387); + a281=(a281-a387); + a357=(a357+a281); + a281=(a2*a522); + a281=(a521*a281); + a387=(a2*a525); + a387=(a524*a387); + a281=(a281+a387); + a387=(a357+a281); + a364=(a29*a527); + a364=(a526*a364); + a403=(a29*a531); + a403=(a530*a403); + a364=(a364+a403); + a387=(a387+a364); + a403=(a29*a533); + a403=(a532*a403); + a373=(a29*a537); + a373=(a536*a373); + a403=(a403+a373); + a387=(a387+a403); + a373=(a460*a368); + a374=(a454*a342); + a373=(a373+a374); + a373=(a499*a373); + a387=(a387-a373); + a342=(a446*a342); + a373=(a460*a358); + a342=(a342-a373); + a342=(a457*a342); + a387=(a387+a342); + a342=(a446*a346); + a358=(a453*a358); + a342=(a342-a358); + a342=(a443*a342); + a387=(a387-a342); + a368=(a453*a368); + a346=(a454*a346); + a368=(a368+a346); + a368=(a464*a368); + a387=(a387-a368); + if (res[0]!=0) res[0][241]=a387; + a387=(a20*a563); + a387=(a541*a387); + a368=(a20*a538); + a346=(a545*a368); + a342=(a20*a561); + a358=(a546*a342); + a346=(a346+a358); + a358=(a346/a558); + a346=(a548*a346); + a373=(a547*a368); + a374=(a551*a342); + a373=(a373-a374); + a374=(a552*a373); + a346=(a346+a374); + a374=(a518*a346); + a358=(a358-a374); + a358=(a534*a358); + a374=(a551*a358); + a373=(a373/a558); + a346=(a557*a346); + a373=(a373-a346); + a373=(a534*a373); + a346=(a546*a373); + a374=(a374+a346); + a374=(a562*a374); + a387=(a387+a374); + a374=(a545*a373); + a346=(a547*a358); + a374=(a374-a346); + a374=(a565*a374); + a346=(a20*a507); + a346=(a560*a346); + a374=(a374-a346); + a387=(a387+a374); + a374=(a15*a568); + a374=(a567*a374); + a346=(a15*a571); + a346=(a570*a346); + a374=(a374+a346); + a346=(a387+a374); + a315=(a21*a573); + a315=(a572*a315); + a353=(a21*a577); + a353=(a576*a353); + a315=(a315+a353); + a346=(a346+a315); + a353=(a21*a579); + a353=(a578*a353); + a355=(a21*a583); + a355=(a582*a355); + a353=(a353+a355); + a346=(a346+a353); + a355=(a510*a392); + a370=(a504*a389); + a355=(a355+a370); + a355=(a580*a355); + a346=(a346-a355); + a389=(a496*a389); + a355=(a510*a386); + a389=(a389-a355); + a389=(a584*a389); + a346=(a346+a389); + a389=(a496*a361); + a386=(a503*a386); + a389=(a389-a386); + a389=(a564*a389); + a346=(a346-a389); + a392=(a503*a392); + a361=(a504*a361); + a392=(a392+a361); + a392=(a585*a392); + a346=(a346-a392); + if (res[0]!=0) res[0][242]=a346; + a346=(a18*a611); + a346=(a589*a346); + a392=(a18*a587); + a361=(a593*a392); + a389=(a18*a609); + a386=(a594*a389); + a361=(a361+a386); + a386=(a361/a606); + a361=(a596*a361); + a355=(a595*a392); + a370=(a599*a389); + a355=(a355-a370); + a370=(a600*a355); + a361=(a361+a370); + a370=(a586*a361); + a386=(a386-a370); + a386=(a588*a386); + a370=(a599*a386); + a355=(a355/a606); + a361=(a605*a361); + a355=(a355-a361); + a355=(a588*a355); + a361=(a594*a355); + a370=(a370+a361); + a370=(a610*a370); + a346=(a346+a370); + a370=(a593*a355); + a361=(a595*a386); + a370=(a370-a361); + a370=(a613*a370); + a361=(a18*a553); + a361=(a614*a361); + a370=(a370-a361); + a346=(a346+a370); + a370=(a18*a618); + a370=(a617*a370); + a361=(a18*a621); + a361=(a620*a361); + a370=(a370+a361); + a361=(a346+a370); + a359=(a748*a623); + a359=(a622*a359); + a354=(a748*a627); + a354=(a626*a354); + a359=(a359+a354); + a361=(a361+a359); + a354=(a748*a629); + a354=(a628*a354); + a371=(a748*a633); + a371=(a632*a371); + a354=(a354+a371); + a361=(a361+a354); + a371=(a556*a368); + a365=(a550*a358); + a371=(a371+a365); + a371=(a630*a371); + a361=(a361-a371); + a358=(a542*a358); + a371=(a556*a342); + a358=(a358-a371); + a358=(a634*a358); + a361=(a361+a358); + a358=(a542*a373); + a342=(a549*a342); + a358=(a358-a342); + a358=(a591*a358); + a361=(a361-a358); + a368=(a549*a368); + a373=(a550*a373); + a368=(a368+a373); + a368=(a635*a368); + a361=(a361-a368); + if (res[0]!=0) res[0][243]=a361; + a361=(a23*a661); + a361=(a639*a361); + a368=(a23*a637); + a373=(a612*a368); + a358=(a23*a659); + a342=(a644*a358); + a373=(a373+a342); + a342=(a373/a656); + a373=(a646*a373); + a371=(a645*a368); + a365=(a649*a358); + a371=(a371-a365); + a365=(a650*a371); + a373=(a373+a365); + a365=(a636*a373); + a342=(a342-a365); + a342=(a638*a342); + a365=(a649*a342); + a371=(a371/a656); + a373=(a655*a373); + a371=(a371-a373); + a371=(a638*a371); + a373=(a644*a371); + a365=(a365+a373); + a365=(a660*a365); + a361=(a361+a365); + a365=(a612*a371); + a373=(a645*a342); + a365=(a365-a373); + a365=(a663*a365); + a373=(a23*a607); + a373=(a658*a373); + a365=(a365-a373); + a361=(a361+a365); + a365=(a23*a666); + a365=(a665*a365); + a373=(a23*a669); + a373=(a668*a373); + a365=(a365+a373); + a373=(a361+a365); + a356=(a759*a671); + a356=(a670*a356); + a366=(a759*a675); + a366=(a674*a366); + a356=(a356+a366); + a373=(a373+a356); + a366=(a759*a677); + a366=(a676*a366); + a326=(a759*a681); + a326=(a680*a326); + a366=(a366+a326); + a373=(a373+a366); + a326=(a604*a392); + a360=(a598*a386); + a326=(a326+a360); + a326=(a643*a326); + a373=(a373-a326); + a386=(a590*a386); + a326=(a604*a389); + a386=(a386-a326); + a386=(a601*a386); + a373=(a373+a386); + a386=(a590*a355); + a389=(a597*a389); + a386=(a386-a389); + a386=(a559*a386); + a373=(a373-a386); + a392=(a597*a392); + a355=(a598*a355); + a392=(a392+a355); + a392=(a608*a392); + a373=(a373-a392); + if (res[0]!=0) res[0][244]=a373; + a373=(a27*a693); + a373=(a700*a373); + a392=(a27*a706); + a355=(a688*a392); + a386=(a27*a687); + a389=(a692*a386); + a355=(a355+a389); + a389=(a355/a686); + a355=(a684*a355); + a326=(a696*a392); + a360=(a689*a386); + a326=(a326-a360); + a360=(a702*a326); + a355=(a355+a360); + a360=(a691*a355); + a389=(a389-a360); + a389=(a662*a389); + a360=(a689*a389); + a326=(a326/a686); + a355=(a685*a355); + a326=(a326-a355); + a326=(a662*a326); + a355=(a692*a326); + a360=(a360+a355); + a360=(a697*a360); + a373=(a373+a360); + a360=(a688*a326); + a355=(a696*a389); + a360=(a360-a355); + a360=(a708*a360); + a27=(a27*a699); + a27=(a709*a27); + a360=(a360-a27); + a373=(a373+a360); + a360=(a760*a714); + a360=(a713*a360); + a27=(a760*a717); + a27=(a716*a27); + a360=(a360+a27); + a27=(a373+a360); + a355=(a750*a720); + a355=(a719*a355); + a282=(a750*a724); + a282=(a723*a282); + a355=(a355+a282); + a27=(a27+a355); + a282=(a750*a727); + a282=(a726*a282); + a304=(a750*a731); + a304=(a730*a304); + a282=(a282+a304); + a27=(a27+a282); + a304=(a654*a368); + a316=(a648*a342); + a304=(a304+a316); + a304=(a732*a304); + a27=(a27-a304); + a342=(a640*a342); + a304=(a654*a358); + a342=(a342-a304); + a342=(a733*a342); + a27=(a27+a342); + a342=(a640*a371); + a358=(a647*a358); + a342=(a342-a358); + a342=(a682*a342); + a27=(a27-a342); + a368=(a647*a368); + a371=(a648*a371); + a368=(a368+a371); + a368=(a734*a368); + a27=(a27-a368); + if (res[0]!=0) res[0][245]=a27; + a27=(a690*a389); + a368=(a678*a386); + a27=(a27-a368); + a27=(a711*a27); + a368=(a678*a392); + a389=(a657*a389); + a368=(a368+a389); + a368=(a751*a368); + a27=(a27-a368); + a368=(a690*a326); + a386=(a701*a386); + a368=(a368-a386); + a368=(a742*a368); + a27=(a27-a368); + a392=(a701*a392); + a326=(a657*a326); + a392=(a392+a326); + a392=(a705*a392); + a27=(a27-a392); + if (res[0]!=0) res[0][246]=a27; + a767=(a8*a767); + a102=(a8*a102); + a767=(a767+a102); + a47=(a8*a47); + a767=(a767+a47); + a737=(a8*a737); + a767=(a767+a737); + a761=(a8*a761); + a767=(a767+a761); + if (res[0]!=0) res[0][247]=a767; + a106=(a8*a106); + a78=(a8*a78); + a106=(a106+a78); + a72=(a8*a72); + a106=(a106+a72); + a75=(a8*a75); + a106=(a106+a75); + a68=(a8*a68); + a106=(a106+a68); + if (res[0]!=0) res[0][248]=a106; + a100=(a8*a100); + a109=(a8*a109); + a100=(a100+a109); + a152=(a8*a152); + a100=(a100+a152); + a132=(a8*a132); + a100=(a100+a132); + a116=(a8*a116); + a100=(a100+a116); + if (res[0]!=0) res[0][249]=a100; + a164=(a8*a164); + a195=(a8*a195); + a164=(a164+a195); + a776=(a8*a776); + a164=(a164+a776); + a182=(a8*a182); + a164=(a164+a182); + a161=(a8*a161); + a164=(a164+a161); + if (res[0]!=0) res[0][250]=a164; + a217=(a8*a217); + a215=(a8*a215); + a217=(a217+a215); + a817=(a8*a817); + a217=(a217+a817); + a170=(a8*a170); + a217=(a217+a170); + a294=(a8*a294); + a217=(a217+a294); + if (res[0]!=0) res[0][251]=a217; + a219=(a8*a219); + a297=(a8*a297); + a219=(a219+a297); + a120=(a8*a120); + a219=(a219+a120); + a251=(a8*a251); + a219=(a219+a251); + a273=(a8*a273); + a219=(a219+a273); + if (res[0]!=0) res[0][252]=a219; + a373=(a4*a373); + a219=(a5*a373); + a273=(a10*a373); + a360=(a38*a360); + a273=(a273+a360); + a360=(a5*a273); + a219=(a219+a360); + a360=(a10*a373); + a355=(a44*a355); + a360=(a360+a355); + a355=(a5*a360); + a219=(a219+a355); + a282=(a44*a282); + a282=(a373+a282); + a355=(a5*a282); + a219=(a219+a355); + a355=(a5*a219); + a361=(a4*a361); + a251=(a5*a361); + a355=(a355+a251); + a251=(a10*a361); + a365=(a38*a365); + a251=(a251+a365); + a365=(a5*a251); + a355=(a355+a365); + a365=(a10*a361); + a356=(a44*a356); + a365=(a365+a356); + a356=(a5*a365); + a355=(a355+a356); + a366=(a44*a366); + a366=(a361+a366); + a356=(a5*a366); + a355=(a355+a356); + a356=(a5*a355); + a346=(a4*a346); + a120=(a5*a346); + a356=(a356+a120); + a120=(a10*a346); + a370=(a38*a370); + a120=(a120+a370); + a370=(a5*a120); + a356=(a356+a370); + a370=(a10*a346); + a359=(a44*a359); + a370=(a370+a359); + a359=(a5*a370); + a356=(a356+a359); + a354=(a44*a354); + a354=(a346+a354); + a359=(a5*a354); + a356=(a356+a359); + a359=(a5*a356); + a387=(a4*a387); + a297=(a5*a387); + a359=(a359+a297); + a297=(a10*a387); + a374=(a38*a374); + a297=(a297+a374); + a374=(a5*a297); + a359=(a359+a374); + a374=(a10*a387); + a315=(a44*a315); + a374=(a374+a315); + a315=(a5*a374); + a359=(a359+a315); + a353=(a44*a353); + a353=(a387+a353); + a315=(a5*a353); + a359=(a359+a315); + a315=(a5*a359); + a357=(a4*a357); + a217=(a5*a357); + a315=(a315+a217); + a217=(a10*a357); + a281=(a38*a281); + a217=(a217+a281); + a281=(a5*a217); + a315=(a315+a281); + a281=(a10*a357); + a364=(a44*a364); + a281=(a281+a364); + a364=(a5*a281); + a315=(a315+a364); + a403=(a44*a403); + a403=(a357+a403); + a364=(a5*a403); + a315=(a315+a364); + a364=(a5*a315); + a393=(a4*a393); + a294=(a5*a393); + a364=(a364+a294); + a294=(a10*a393); + a347=(a38*a347); + a294=(a294+a347); + a347=(a5*a294); + a364=(a364+a347); + a347=(a10*a393); + a350=(a44*a350); + a347=(a347+a350); + a350=(a5*a347); + a364=(a364+a350); + a369=(a44*a369); + a369=(a393+a369); + a350=(a5*a369); + a364=(a364+a350); + a364=(a5*a364); + a380=(a4*a380); + a350=(a5*a380); + a364=(a364+a350); + a350=(a10*a380); + a383=(a38*a383); + a350=(a350+a383); + a350=(a5*a350); + a364=(a364+a350); + a350=(a10*a380); + a381=(a44*a381); + a350=(a350+a381); + a350=(a5*a350); + a364=(a364+a350); + a378=(a44*a378); + a380=(a380+a378); + a380=(a5*a380); + a364=(a364+a380); + a380=(a5*a364); + a349=(a4*a349); + a378=(a5*a349); + a380=(a380+a378); + a378=(a10*a349); + a377=(a38*a377); + a378=(a378+a377); + a377=(a5*a378); + a380=(a380+a377); + a377=(a10*a349); + a382=(a44*a382); + a377=(a377+a382); + a382=(a5*a377); + a380=(a380+a382); + a388=(a44*a388); + a388=(a349+a388); + a382=(a5*a388); + a380=(a380+a382); + a380=(a8*a380); + a382=(a50*a34); + a380=(a380-a382); + a350=(a50*a34); + a380=(a380-a350); + if (res[0]!=0) res[0][253]=a380; + a318=(a28*a318); + a375=(a375*a318); + a334=(a42*a334); + a385=(a385*a334); + a375=(a375+a385); + a340=(a42*a340); + a391=(a391*a340); + a375=(a375+a391); + a363=(a28*a363); + a376=(a376*a363); + a375=(a375-a376); + a348=(a42*a348); + a379=(a379*a348); + a375=(a375-a379); + a256=(a42*a256); + a384=(a384*a256); + a375=(a375-a384); + a375=(-a375); + if (res[0]!=0) res[0][254]=a375; + a364=(a8*a364); + a349=(a8*a349); + a364=(a364+a349); + a378=(a8*a378); + a364=(a364+a378); + a377=(a8*a377); + a364=(a364+a377); + a388=(a8*a388); + a364=(a364+a388); + a388=(a24*a34); + a364=(a364-a388); + a388=(a50*a34); + a364=(a364+a388); + a364=(a364+a382); + a364=(a364+a350); + a350=(a10*a34); + a364=(a364+a350); + if (res[0]!=0) res[0][255]=a364; + a364=sin(a413); + a364=(a364*a424); + a350=cos(a413); + a350=(a350*a401); + a364=(a364-a350); + a350=cos(a362); + a350=(a350*a401); + a364=(a364-a350); + a350=sin(a402); + a350=(a350*a427); + a382=cos(a402); + a382=(a382*a433); + a350=(a350-a382); + a364=(a364+a350); + a350=sin(a321); + a350=(a350*a432); + a382=cos(a321); + a382=(a382*a439); + a350=(a350-a382); + a364=(a364+a350); + a362=sin(a362); + a362=(a362*a424); + a364=(a364+a362); + if (res[0]!=0) res[0][256]=a364; + a364=cos(a413); + a362=(a703*a364); + a362=(a401*a362); + a350=cos(a402); + a382=(a747*a350); + a382=(a433*a382); + a362=(a362+a382); + a382=cos(a321); + a388=(a747*a382); + a388=(a439*a388); + a362=(a362+a388); + a413=sin(a413); + a388=(a703*a413); + a388=(a424*a388); + a362=(a362-a388); + a402=sin(a402); + a388=(a747*a402); + a388=(a427*a388); + a362=(a362-a388); + a321=sin(a321); + a388=(a747*a321); + a388=(a432*a388); + a362=(a362-a388); + a362=(-a362); + if (res[0]!=0) res[0][257]=a362; + a362=(a760*a364); + a362=(a401*a362); + a388=(a750*a350); + a388=(a433*a388); + a362=(a362+a388); + a388=(a750*a382); + a388=(a439*a388); + a362=(a362+a388); + a388=(a760*a413); + a388=(a424*a388); + a362=(a362-a388); + a388=(a750*a402); + a388=(a427*a388); + a362=(a362-a388); + a388=(a750*a321); + a388=(a432*a388); + a362=(a362-a388); + a362=(-a362); + if (res[0]!=0) res[0][258]=a362; + a362=(a23*a364); + a362=(a401*a362); + a388=(a759*a350); + a388=(a433*a388); + a362=(a362+a388); + a388=(a759*a382); + a388=(a439*a388); + a362=(a362+a388); + a388=(a23*a413); + a388=(a424*a388); + a362=(a362-a388); + a388=(a759*a402); + a388=(a427*a388); + a362=(a362-a388); + a388=(a759*a321); + a388=(a432*a388); + a362=(a362-a388); + a362=(-a362); + if (res[0]!=0) res[0][259]=a362; + a362=(a18*a364); + a362=(a401*a362); + a388=(a748*a350); + a388=(a433*a388); + a362=(a362+a388); + a388=(a748*a382); + a388=(a439*a388); + a362=(a362+a388); + a388=(a18*a413); + a388=(a424*a388); + a362=(a362-a388); + a388=(a748*a402); + a388=(a427*a388); + a362=(a362-a388); + a388=(a748*a321); + a388=(a432*a388); + a362=(a362-a388); + a362=(-a362); + if (res[0]!=0) res[0][260]=a362; + a362=(a15*a364); + a362=(a401*a362); + a388=(a21*a350); + a388=(a433*a388); + a362=(a362+a388); + a388=(a21*a382); + a388=(a439*a388); + a362=(a362+a388); + a388=(a15*a413); + a388=(a424*a388); + a362=(a362-a388); + a388=(a21*a402); + a388=(a427*a388); + a362=(a362-a388); + a388=(a21*a321); + a388=(a432*a388); + a362=(a362-a388); + a362=(-a362); + if (res[0]!=0) res[0][261]=a362; + a362=(a2*a364); + a362=(a401*a362); + a388=(a29*a350); + a388=(a433*a388); + a362=(a362+a388); + a388=(a29*a382); + a388=(a439*a388); + a362=(a362+a388); + a388=(a2*a413); + a388=(a424*a388); + a362=(a362-a388); + a388=(a29*a402); + a388=(a427*a388); + a362=(a362-a388); + a388=(a29*a321); + a388=(a432*a388); + a362=(a362-a388); + a362=(-a362); + if (res[0]!=0) res[0][262]=a362; + a362=(a51*a364); + a362=(a401*a362); + a388=(a32*a350); + a388=(a433*a388); + a362=(a362+a388); + a388=(a32*a382); + a388=(a439*a388); + a362=(a362+a388); + a388=(a51*a413); + a388=(a424*a388); + a362=(a362-a388); + a388=(a32*a402); + a388=(a427*a388); + a362=(a362-a388); + a388=(a32*a321); + a388=(a432*a388); + a362=(a362-a388); + a362=(-a362); + if (res[0]!=0) res[0][263]=a362; + a362=(a10*a34); + a388=(a10*a34); + a377=(a362+a388); + a377=(-a377); + if (res[0]!=0) res[0][264]=a377; + a377=(a41*a364); + a377=(a401*a377); + a378=(a35*a350); + a378=(a433*a378); + a377=(a377+a378); + a378=(a35*a382); + a378=(a439*a378); + a377=(a377+a378); + a378=(a41*a413); + a378=(a424*a378); + a377=(a377-a378); + a378=(a35*a402); + a378=(a427*a378); + a377=(a377-a378); + a378=(a35*a321); + a378=(a432*a378); + a377=(a377-a378); + a377=(-a377); + if (res[0]!=0) res[0][265]=a377; + a377=(a3*a34); + a378=(a10*a34); + a349=(a377-a378); + a362=(a362-a349); + a362=(a362+a388); + a388=(a10*a34); + a362=(a362+a388); + if (res[0]!=0) res[0][266]=a362; + a421=(a28*a421); + a399=(a399*a421); + a397=(a28*a397); + a421=(a372*a397); + a419=(a28*a419); + a362=(a404*a419); + a421=(a421+a362); + a362=(a421/a416); + a406=(a406*a421); + a421=(a405*a397); + a388=(a409*a419); + a421=(a421-a388); + a410=(a410*a421); + a406=(a406+a410); + a396=(a396*a406); + a362=(a362-a396); + a362=(a398*a362); + a409=(a409*a362); + a421=(a421/a416); + a415=(a415*a406); + a421=(a421-a415); + a398=(a398*a421); + a404=(a404*a398); + a409=(a409+a404); + a420=(a420*a409); + a399=(a399+a420); + a372=(a372*a398); + a405=(a405*a362); + a372=(a372-a405); + a423=(a423*a372); + a367=(a28*a367); + a418=(a418*a367); + a423=(a423-a418); + a399=(a399+a423); + a426=(a28*a426); + a425=(a425*a426); + a429=(a28*a429); + a428=(a428*a429); + a425=(a425+a428); + a428=(a399+a425); + a431=(a42*a431); + a430=(a430*a431); + a435=(a42*a435); + a434=(a434*a435); + a430=(a430+a434); + a428=(a428+a430); + a437=(a42*a437); + a436=(a436*a437); + a441=(a42*a441); + a440=(a440*a441); + a436=(a436+a440); + a428=(a428+a436); + if (res[0]!=0) res[0][267]=a428; + a428=(a41*a467); + a428=(a445*a428); + a440=(a41*a442); + a441=(a449*a440); + a437=(a41*a465); + a434=(a450*a437); + a441=(a441+a434); + a434=(a441/a462); + a441=(a452*a441); + a435=(a451*a440); + a431=(a455*a437); + a435=(a435-a431); + a431=(a456*a435); + a441=(a441+a431); + a431=(a422*a441); + a434=(a434-a431); + a434=(a438*a434); + a431=(a455*a434); + a435=(a435/a462); + a441=(a461*a441); + a435=(a435-a441); + a435=(a438*a435); + a441=(a450*a435); + a431=(a431+a441); + a431=(a466*a431); + a428=(a428+a431); + a431=(a449*a435); + a441=(a451*a434); + a431=(a431-a441); + a431=(a469*a431); + a441=(a41*a411); + a441=(a470*a441); + a431=(a431-a441); + a428=(a428+a431); + a431=(a41*a474); + a431=(a473*a431); + a441=(a41*a477); + a441=(a476*a441); + a431=(a431+a441); + a441=(a428+a431); + a429=(a35*a479); + a429=(a478*a429); + a426=(a35*a483); + a426=(a482*a426); + a429=(a429+a426); + a441=(a441+a429); + a426=(a35*a485); + a426=(a484*a426); + a423=(a35*a489); + a423=(a488*a423); + a426=(a426+a423); + a441=(a441+a426); + a423=(a414*a397); + a418=(a408*a362); + a423=(a423+a418); + a486=(a486*a423); + a441=(a441-a486); + a362=(a400*a362); + a414=(a414*a419); + a362=(a362-a414); + a490=(a490*a362); + a441=(a441+a490); + a400=(a400*a398); + a419=(a407*a419); + a400=(a400-a419); + a447=(a447*a400); + a441=(a441-a447); + a407=(a407*a397); + a408=(a408*a398); + a407=(a407+a408); + a491=(a491*a407); + a441=(a441-a491); + if (res[0]!=0) res[0][268]=a441; + a441=(a51*a517); + a441=(a495*a441); + a491=(a51*a493); + a407=(a468*a491); + a408=(a51*a515); + a398=(a500*a408); + a407=(a407+a398); + a398=(a407/a512); + a407=(a502*a407); + a397=(a501*a491); + a447=(a505*a408); + a397=(a397-a447); + a447=(a506*a397); + a407=(a407+a447); + a447=(a492*a407); + a398=(a398-a447); + a398=(a494*a398); + a447=(a505*a398); + a397=(a397/a512); + a407=(a511*a407); + a397=(a397-a407); + a397=(a494*a397); + a407=(a500*a397); + a447=(a447+a407); + a447=(a516*a447); + a441=(a441+a447); + a447=(a468*a397); + a407=(a501*a398); + a447=(a447-a407); + a447=(a519*a447); + a407=(a51*a463); + a407=(a514*a407); + a447=(a447-a407); + a441=(a441+a447); + a447=(a51*a522); + a447=(a521*a447); + a407=(a51*a525); + a407=(a524*a407); + a447=(a447+a407); + a407=(a441+a447); + a400=(a32*a527); + a400=(a526*a400); + a419=(a32*a531); + a419=(a530*a419); + a400=(a400+a419); + a407=(a407+a400); + a419=(a32*a533); + a419=(a532*a419); + a490=(a32*a537); + a490=(a536*a490); + a419=(a419+a490); + a407=(a407+a419); + a490=(a460*a440); + a362=(a454*a434); + a490=(a490+a362); + a490=(a499*a490); + a407=(a407-a490); + a434=(a446*a434); + a490=(a460*a437); + a434=(a434-a490); + a434=(a457*a434); + a407=(a407+a434); + a434=(a446*a435); + a437=(a453*a437); + a434=(a434-a437); + a434=(a443*a434); + a407=(a407-a434); + a440=(a453*a440); + a435=(a454*a435); + a440=(a440+a435); + a440=(a464*a440); + a407=(a407-a440); + if (res[0]!=0) res[0][269]=a407; + a407=(a14*a563); + a407=(a541*a407); + a440=(a14*a538); + a435=(a545*a440); + a434=(a14*a561); + a437=(a546*a434); + a435=(a435+a437); + a437=(a435/a558); + a435=(a548*a435); + a490=(a547*a440); + a362=(a551*a434); + a490=(a490-a362); + a362=(a552*a490); + a435=(a435+a362); + a362=(a518*a435); + a437=(a437-a362); + a437=(a534*a437); + a362=(a551*a437); + a490=(a490/a558); + a435=(a557*a435); + a490=(a490-a435); + a490=(a534*a490); + a435=(a546*a490); + a362=(a362+a435); + a362=(a562*a362); + a407=(a407+a362); + a362=(a545*a490); + a435=(a547*a437); + a362=(a362-a435); + a362=(a565*a362); + a435=(a14*a507); + a435=(a560*a435); + a362=(a362-a435); + a407=(a407+a362); + a362=(a2*a568); + a362=(a567*a362); + a435=(a2*a571); + a435=(a570*a435); + a362=(a362+a435); + a435=(a407+a362); + a414=(a29*a573); + a414=(a572*a414); + a486=(a29*a577); + a486=(a576*a486); + a414=(a414+a486); + a435=(a435+a414); + a486=(a29*a579); + a486=(a578*a486); + a423=(a29*a583); + a423=(a582*a423); + a486=(a486+a423); + a435=(a435+a486); + a423=(a510*a491); + a418=(a504*a398); + a423=(a423+a418); + a423=(a580*a423); + a435=(a435-a423); + a398=(a496*a398); + a423=(a510*a408); + a398=(a398-a423); + a398=(a584*a398); + a435=(a435+a398); + a398=(a496*a397); + a408=(a503*a408); + a398=(a398-a408); + a398=(a564*a398); + a435=(a435-a398); + a491=(a503*a491); + a397=(a504*a397); + a491=(a491+a397); + a491=(a585*a491); + a435=(a435-a491); + if (res[0]!=0) res[0][270]=a435; + a435=(a20*a611); + a435=(a589*a435); + a491=(a20*a587); + a397=(a593*a491); + a398=(a20*a609); + a408=(a594*a398); + a397=(a397+a408); + a408=(a397/a606); + a397=(a596*a397); + a423=(a595*a491); + a418=(a599*a398); + a423=(a423-a418); + a418=(a600*a423); + a397=(a397+a418); + a418=(a586*a397); + a408=(a408-a418); + a408=(a588*a408); + a418=(a599*a408); + a423=(a423/a606); + a397=(a605*a397); + a423=(a423-a397); + a423=(a588*a423); + a397=(a594*a423); + a418=(a418+a397); + a418=(a610*a418); + a435=(a435+a418); + a418=(a593*a423); + a397=(a595*a408); + a418=(a418-a397); + a418=(a613*a418); + a397=(a20*a553); + a397=(a614*a397); + a418=(a418-a397); + a435=(a435+a418); + a418=(a15*a618); + a418=(a617*a418); + a397=(a15*a621); + a397=(a620*a397); + a418=(a418+a397); + a397=(a435+a418); + a367=(a21*a623); + a367=(a622*a367); + a372=(a21*a627); + a372=(a626*a372); + a367=(a367+a372); + a397=(a397+a367); + a372=(a21*a629); + a372=(a628*a372); + a405=(a21*a633); + a405=(a632*a405); + a372=(a372+a405); + a397=(a397+a372); + a405=(a556*a440); + a420=(a550*a437); + a405=(a405+a420); + a405=(a630*a405); + a397=(a397-a405); + a437=(a542*a437); + a405=(a556*a434); + a437=(a437-a405); + a437=(a634*a437); + a397=(a397+a437); + a437=(a542*a490); + a434=(a549*a434); + a437=(a437-a434); + a437=(a591*a437); + a397=(a397-a437); + a440=(a549*a440); + a490=(a550*a490); + a440=(a440+a490); + a440=(a635*a440); + a397=(a397-a440); + if (res[0]!=0) res[0][271]=a397; + a397=(a18*a661); + a397=(a639*a397); + a440=(a18*a637); + a490=(a612*a440); + a437=(a18*a659); + a434=(a644*a437); + a490=(a490+a434); + a434=(a490/a656); + a490=(a646*a490); + a405=(a645*a440); + a420=(a649*a437); + a405=(a405-a420); + a420=(a650*a405); + a490=(a490+a420); + a420=(a636*a490); + a434=(a434-a420); + a434=(a638*a434); + a420=(a649*a434); + a405=(a405/a656); + a490=(a655*a490); + a405=(a405-a490); + a405=(a638*a405); + a490=(a644*a405); + a420=(a420+a490); + a420=(a660*a420); + a397=(a397+a420); + a420=(a612*a405); + a490=(a645*a434); + a420=(a420-a490); + a420=(a663*a420); + a490=(a18*a607); + a490=(a658*a490); + a420=(a420-a490); + a397=(a397+a420); + a420=(a18*a666); + a420=(a665*a420); + a490=(a18*a669); + a490=(a668*a490); + a420=(a420+a490); + a490=(a397+a420); + a409=(a748*a671); + a409=(a670*a409); + a404=(a748*a675); + a404=(a674*a404); + a409=(a409+a404); + a490=(a490+a409); + a404=(a748*a677); + a404=(a676*a404); + a421=(a748*a681); + a421=(a680*a421); + a404=(a404+a421); + a490=(a490+a404); + a421=(a604*a491); + a415=(a598*a408); + a421=(a421+a415); + a421=(a643*a421); + a490=(a490-a421); + a408=(a590*a408); + a421=(a604*a398); + a408=(a408-a421); + a408=(a601*a408); + a490=(a490+a408); + a408=(a590*a423); + a398=(a597*a398); + a408=(a408-a398); + a408=(a559*a408); + a490=(a490-a408); + a491=(a597*a491); + a423=(a598*a423); + a491=(a491+a423); + a491=(a608*a491); + a490=(a490-a491); + if (res[0]!=0) res[0][272]=a490; + a490=(a23*a693); + a490=(a700*a490); + a491=(a23*a706); + a423=(a688*a491); + a408=(a23*a687); + a398=(a692*a408); + a423=(a423+a398); + a398=(a423/a686); + a423=(a684*a423); + a421=(a696*a491); + a415=(a689*a408); + a421=(a421-a415); + a415=(a702*a421); + a423=(a423+a415); + a415=(a691*a423); + a398=(a398-a415); + a398=(a662*a398); + a415=(a689*a398); + a421=(a421/a686); + a423=(a685*a423); + a421=(a421-a423); + a421=(a662*a421); + a423=(a692*a421); + a415=(a415+a423); + a415=(a697*a415); + a490=(a490+a415); + a415=(a688*a421); + a423=(a696*a398); + a415=(a415-a423); + a415=(a708*a415); + a423=(a23*a699); + a423=(a709*a423); + a415=(a415-a423); + a490=(a490+a415); + a415=(a23*a714); + a415=(a713*a415); + a423=(a23*a717); + a423=(a716*a423); + a415=(a415+a423); + a423=(a490+a415); + a406=(a759*a720); + a406=(a719*a406); + a416=(a759*a724); + a416=(a723*a416); + a406=(a406+a416); + a423=(a423+a406); + a416=(a759*a727); + a416=(a726*a416); + a396=(a759*a731); + a396=(a730*a396); + a416=(a416+a396); + a423=(a423+a416); + a396=(a654*a440); + a410=(a648*a434); + a396=(a396+a410); + a396=(a732*a396); + a423=(a423-a396); + a434=(a640*a434); + a396=(a654*a437); + a434=(a434-a396); + a434=(a733*a434); + a423=(a423+a434); + a434=(a640*a405); + a437=(a647*a437); + a434=(a434-a437); + a434=(a682*a434); + a423=(a423-a434); + a440=(a647*a440); + a405=(a648*a405); + a440=(a440+a405); + a440=(a734*a440); + a423=(a423-a440); + if (res[0]!=0) res[0][273]=a423; + a423=(a690*a398); + a440=(a678*a408); + a423=(a423-a440); + a423=(a711*a423); + a440=(a678*a491); + a398=(a657*a398); + a440=(a440+a398); + a440=(a751*a440); + a423=(a423-a440); + a440=(a690*a421); + a408=(a701*a408); + a440=(a440-a408); + a440=(a742*a440); + a423=(a423-a440); + a491=(a701*a491); + a421=(a657*a421); + a491=(a491+a421); + a491=(a705*a491); + a423=(a423-a491); + if (res[0]!=0) res[0][274]=a423; + a773=(a8*a773); + a25=(a8*a25); + a773=(a773+a25); + a53=(a8*a53); + a773=(a773+a53); + a704=(a8*a704); + a773=(a773+a704); + a740=(a8*a740); + a773=(a773+a740); + if (res[0]!=0) res[0][275]=a773; + a789=(a8*a789); + a85=(a8*a85); + a789=(a789+a85); + a64=(a8*a64); + a789=(a789+a64); + a69=(a8*a69); + a789=(a789+a69); + a54=(a8*a54); + a789=(a789+a54); + if (res[0]!=0) res[0][276]=a789; + a6=(a8*a6); + a202=(a8*a202); + a6=(a6+a202); + a820=(a8*a820); + a6=(a6+a820); + a127=(a8*a127); + a6=(a6+a127); + a128=(a8*a128); + a6=(a6+a128); + if (res[0]!=0) res[0][277]=a6; + a90=(a8*a90); + a154=(a8*a154); + a90=(a90+a154); + a49=(a8*a49); + a90=(a90+a49); + a178=(a8*a178); + a90=(a90+a178); + a162=(a8*a162); + a90=(a90+a162); + if (res[0]!=0) res[0][278]=a90; + a214=(a8*a214); + a243=(a8*a243); + a214=(a214+a243); + a754=(a8*a754); + a214=(a214+a754); + a226=(a8*a226); + a214=(a214+a226); + a180=(a8*a180); + a214=(a214+a180); + if (res[0]!=0) res[0][279]=a214; + a263=(a8*a263); + a261=(a8*a261); + a263=(a263+a261); + a56=(a8*a56); + a263=(a263+a56); + a185=(a8*a185); + a263=(a263+a185); + a307=(a8*a307); + a263=(a263+a307); + if (res[0]!=0) res[0][280]=a263; + a271=(a8*a271); + a345=(a8*a345); + a271=(a271+a345); + a89=(a8*a89); + a271=(a271+a89); + a351=(a8*a351); + a271=(a271+a351); + a323=(a8*a323); + a271=(a271+a323); + if (res[0]!=0) res[0][281]=a271; + a490=(a4*a490); + a271=(a5*a490); + a323=(a10*a490); + a415=(a38*a415); + a323=(a323+a415); + a415=(a5*a323); + a271=(a271+a415); + a415=(a10*a490); + a406=(a44*a406); + a415=(a415+a406); + a406=(a5*a415); + a271=(a271+a406); + a416=(a44*a416); + a416=(a490+a416); + a406=(a5*a416); + a271=(a271+a406); + a406=(a5*a271); + a397=(a4*a397); + a351=(a5*a397); + a406=(a406+a351); + a351=(a10*a397); + a420=(a38*a420); + a351=(a351+a420); + a420=(a5*a351); + a406=(a406+a420); + a420=(a10*a397); + a409=(a44*a409); + a420=(a420+a409); + a409=(a5*a420); + a406=(a406+a409); + a404=(a44*a404); + a404=(a397+a404); + a409=(a5*a404); + a406=(a406+a409); + a409=(a5*a406); + a435=(a4*a435); + a89=(a5*a435); + a409=(a409+a89); + a89=(a10*a435); + a418=(a38*a418); + a89=(a89+a418); + a418=(a5*a89); + a409=(a409+a418); + a418=(a10*a435); + a367=(a44*a367); + a418=(a418+a367); + a367=(a5*a418); + a409=(a409+a367); + a372=(a44*a372); + a372=(a435+a372); + a367=(a5*a372); + a409=(a409+a367); + a367=(a5*a409); + a407=(a4*a407); + a345=(a5*a407); + a367=(a367+a345); + a345=(a10*a407); + a362=(a38*a362); + a345=(a345+a362); + a362=(a5*a345); + a367=(a367+a362); + a362=(a10*a407); + a414=(a44*a414); + a362=(a362+a414); + a414=(a5*a362); + a367=(a367+a414); + a486=(a44*a486); + a486=(a407+a486); + a414=(a5*a486); + a367=(a367+a414); + a414=(a5*a367); + a441=(a4*a441); + a263=(a5*a441); + a414=(a414+a263); + a263=(a10*a441); + a447=(a38*a447); + a263=(a263+a447); + a447=(a5*a263); + a414=(a414+a447); + a447=(a10*a441); + a400=(a44*a400); + a447=(a447+a400); + a400=(a5*a447); + a414=(a414+a400); + a419=(a44*a419); + a419=(a441+a419); + a400=(a5*a419); + a414=(a414+a400); + a414=(a5*a414); + a428=(a4*a428); + a400=(a5*a428); + a414=(a414+a400); + a400=(a10*a428); + a431=(a38*a431); + a400=(a400+a431); + a400=(a5*a400); + a414=(a414+a400); + a400=(a10*a428); + a429=(a44*a429); + a400=(a400+a429); + a400=(a5*a400); + a414=(a414+a400); + a426=(a44*a426); + a428=(a428+a426); + a428=(a5*a428); + a414=(a414+a428); + a428=(a5*a414); + a399=(a4*a399); + a426=(a5*a399); + a428=(a428+a426); + a426=(a10*a399); + a425=(a38*a425); + a426=(a426+a425); + a425=(a5*a426); + a428=(a428+a425); + a425=(a10*a399); + a430=(a44*a430); + a425=(a425+a430); + a430=(a5*a425); + a428=(a428+a430); + a436=(a44*a436); + a436=(a399+a436); + a430=(a5*a436); + a428=(a428+a430); + a428=(a8*a428); + a430=(a50*a34); + a428=(a428-a430); + a400=(a50*a34); + a428=(a428-a400); + if (res[0]!=0) res[0][282]=a428; + a364=(a28*a364); + a401=(a401*a364); + a350=(a42*a350); + a433=(a433*a350); + a401=(a401+a433); + a382=(a42*a382); + a439=(a439*a382); + a401=(a401+a439); + a413=(a28*a413); + a424=(a424*a413); + a401=(a401-a424); + a402=(a42*a402); + a427=(a427*a402); + a401=(a401-a427); + a321=(a42*a321); + a432=(a432*a321); + a401=(a401-a432); + a401=(-a401); + if (res[0]!=0) res[0][283]=a401; + a414=(a8*a414); + a399=(a8*a399); + a414=(a414+a399); + a426=(a8*a426); + a414=(a414+a426); + a425=(a8*a425); + a414=(a414+a425); + a436=(a8*a436); + a414=(a414+a436); + a436=(a24*a34); + a414=(a414-a436); + a436=(a50*a34); + a414=(a414+a436); + a414=(a414+a430); + a414=(a414+a400); + a400=(a10*a34); + a414=(a414+a400); + if (res[0]!=0) res[0][284]=a414; + a414=cos(a459); + a400=(a471*a414); + a430=cos(a444); + a436=(a481*a430); + a400=(a400+a436); + a436=cos(a352); + a425=(a487*a436); + a400=(a400+a425); + a425=cos(a412); + a425=(a471*a425); + a400=(a400+a425); + a459=sin(a459); + a425=(a472*a459); + a400=(a400-a425); + a444=sin(a444); + a425=(a475*a444); + a400=(a400-a425); + a352=sin(a352); + a425=(a480*a352); + a400=(a400-a425); + a412=sin(a412); + a412=(a472*a412); + a400=(a400-a412); + a400=(-a400); + if (res[0]!=0) res[0][285]=a400; + a400=(a764*a414); + a400=(a471*a400); + a412=(a766*a430); + a412=(a481*a412); + a400=(a400+a412); + a412=(a766*a436); + a412=(a487*a412); + a400=(a400+a412); + a412=(a764*a459); + a412=(a472*a412); + a400=(a400-a412); + a412=(a766*a444); + a412=(a475*a412); + a400=(a400-a412); + a412=(a766*a352); + a412=(a480*a412); + a400=(a400-a412); + a400=(-a400); + if (res[0]!=0) res[0][286]=a400; + a400=(a703*a414); + a400=(a471*a400); + a412=(a747*a430); + a412=(a481*a412); + a400=(a400+a412); + a412=(a747*a436); + a412=(a487*a412); + a400=(a400+a412); + a412=(a703*a459); + a412=(a472*a412); + a400=(a400-a412); + a412=(a747*a444); + a412=(a475*a412); + a400=(a400-a412); + a412=(a747*a352); + a412=(a480*a412); + a400=(a400-a412); + a400=(-a400); + if (res[0]!=0) res[0][287]=a400; + a400=(a760*a414); + a400=(a471*a400); + a412=(a750*a430); + a412=(a481*a412); + a400=(a400+a412); + a412=(a750*a436); + a412=(a487*a412); + a400=(a400+a412); + a412=(a760*a459); + a412=(a472*a412); + a400=(a400-a412); + a412=(a750*a444); + a412=(a475*a412); + a400=(a400-a412); + a412=(a750*a352); + a412=(a480*a412); + a400=(a400-a412); + a400=(-a400); + if (res[0]!=0) res[0][288]=a400; + a400=(a23*a414); + a400=(a471*a400); + a412=(a759*a430); + a412=(a481*a412); + a400=(a400+a412); + a412=(a759*a436); + a412=(a487*a412); + a400=(a400+a412); + a412=(a23*a459); + a412=(a472*a412); + a400=(a400-a412); + a412=(a759*a444); + a412=(a475*a412); + a400=(a400-a412); + a412=(a759*a352); + a412=(a480*a412); + a400=(a400-a412); + a400=(-a400); + if (res[0]!=0) res[0][289]=a400; + a400=(a18*a414); + a400=(a471*a400); + a412=(a748*a430); + a412=(a481*a412); + a400=(a400+a412); + a412=(a748*a436); + a412=(a487*a412); + a400=(a400+a412); + a412=(a18*a459); + a412=(a472*a412); + a400=(a400-a412); + a412=(a748*a444); + a412=(a475*a412); + a400=(a400-a412); + a412=(a748*a352); + a412=(a480*a412); + a400=(a400-a412); + a400=(-a400); + if (res[0]!=0) res[0][290]=a400; + a400=(a15*a414); + a400=(a471*a400); + a412=(a21*a430); + a412=(a481*a412); + a400=(a400+a412); + a412=(a21*a436); + a412=(a487*a412); + a400=(a400+a412); + a412=(a15*a459); + a412=(a472*a412); + a400=(a400-a412); + a412=(a21*a444); + a412=(a475*a412); + a400=(a400-a412); + a412=(a21*a352); + a412=(a480*a412); + a400=(a400-a412); + a400=(-a400); + if (res[0]!=0) res[0][291]=a400; + a400=(a2*a414); + a400=(a471*a400); + a412=(a29*a430); + a412=(a481*a412); + a400=(a400+a412); + a412=(a29*a436); + a412=(a487*a412); + a400=(a400+a412); + a412=(a2*a459); + a412=(a472*a412); + a400=(a400-a412); + a412=(a29*a444); + a412=(a475*a412); + a400=(a400-a412); + a412=(a29*a352); + a412=(a480*a412); + a400=(a400-a412); + a400=(-a400); + if (res[0]!=0) res[0][292]=a400; + a400=(a51*a414); + a400=(a471*a400); + a412=(a32*a430); + a412=(a481*a412); + a400=(a400+a412); + a412=(a32*a436); + a412=(a487*a412); + a400=(a400+a412); + a412=(a51*a459); + a412=(a472*a412); + a400=(a400-a412); + a412=(a32*a444); + a412=(a475*a412); + a400=(a400-a412); + a412=(a32*a352); + a412=(a480*a412); + a400=(a400-a412); + a400=(-a400); + if (res[0]!=0) res[0][293]=a400; + a377=(a377-a378); + if (res[0]!=0) res[0][294]=a377; + a377=(a41*a414); + a377=(a471*a377); + a378=(a35*a430); + a378=(a481*a378); + a377=(a377+a378); + a378=(a35*a436); + a378=(a487*a378); + a377=(a377+a378); + a378=(a41*a459); + a378=(a472*a378); + a377=(a377-a378); + a378=(a35*a444); + a378=(a475*a378); + a377=(a377-a378); + a378=(a35*a352); + a378=(a480*a378); + a377=(a377-a378); + a377=(-a377); + if (res[0]!=0) res[0][295]=a377; + a377=(a10*a34); + a378=(a3*a34); + a400=(a10*a34); + a378=(a378-a400); + a377=(a377-a378); + a378=(a10*a34); + a377=(a377+a378); + a378=(a10*a34); + a377=(a377+a378); + if (res[0]!=0) res[0][296]=a377; + a467=(a28*a467); + a445=(a445*a467); + a442=(a28*a442); + a467=(a449*a442); + a465=(a28*a465); + a377=(a450*a465); + a467=(a467+a377); + a377=(a467/a462); + a452=(a452*a467); + a467=(a451*a442); + a378=(a455*a465); + a467=(a467-a378); + a456=(a456*a467); + a452=(a452+a456); + a422=(a422*a452); + a377=(a377-a422); + a377=(a438*a377); + a455=(a455*a377); + a467=(a467/a462); + a461=(a461*a452); + a467=(a467-a461); + a438=(a438*a467); + a450=(a450*a438); + a455=(a455+a450); + a466=(a466*a455); + a445=(a445+a466); + a449=(a449*a438); + a451=(a451*a377); + a449=(a449-a451); + a469=(a469*a449); + a411=(a28*a411); + a470=(a470*a411); + a469=(a469-a470); + a445=(a445+a469); + a474=(a28*a474); + a473=(a473*a474); + a477=(a28*a477); + a476=(a476*a477); + a473=(a473+a476); + a476=(a445+a473); + a479=(a42*a479); + a478=(a478*a479); + a483=(a42*a483); + a482=(a482*a483); + a478=(a478+a482); + a476=(a476+a478); + a485=(a42*a485); + a484=(a484*a485); + a489=(a42*a489); + a488=(a488*a489); + a484=(a484+a488); + a476=(a476+a484); + if (res[0]!=0) res[0][297]=a476; + a476=(a41*a517); + a476=(a495*a476); + a488=(a41*a493); + a489=(a468*a488); + a485=(a41*a515); + a482=(a500*a485); + a489=(a489+a482); + a482=(a489/a512); + a489=(a502*a489); + a483=(a501*a488); + a479=(a505*a485); + a483=(a483-a479); + a479=(a506*a483); + a489=(a489+a479); + a479=(a492*a489); + a482=(a482-a479); + a482=(a494*a482); + a479=(a505*a482); + a483=(a483/a512); + a489=(a511*a489); + a483=(a483-a489); + a483=(a494*a483); + a489=(a500*a483); + a479=(a479+a489); + a479=(a516*a479); + a476=(a476+a479); + a479=(a468*a483); + a489=(a501*a482); + a479=(a479-a489); + a479=(a519*a479); + a489=(a41*a463); + a489=(a514*a489); + a479=(a479-a489); + a476=(a476+a479); + a479=(a41*a522); + a479=(a521*a479); + a489=(a41*a525); + a489=(a524*a489); + a479=(a479+a489); + a489=(a476+a479); + a477=(a35*a527); + a477=(a526*a477); + a474=(a35*a531); + a474=(a530*a474); + a477=(a477+a474); + a489=(a489+a477); + a474=(a35*a533); + a474=(a532*a474); + a469=(a35*a537); + a469=(a536*a469); + a474=(a474+a469); + a489=(a489+a474); + a469=(a460*a442); + a470=(a454*a377); + a469=(a469+a470); + a499=(a499*a469); + a489=(a489-a499); + a377=(a446*a377); + a460=(a460*a465); + a377=(a377-a460); + a457=(a457*a377); + a489=(a489+a457); + a446=(a446*a438); + a465=(a453*a465); + a446=(a446-a465); + a443=(a443*a446); + a489=(a489-a443); + a453=(a453*a442); + a454=(a454*a438); + a453=(a453+a454); + a464=(a464*a453); + a489=(a489-a464); + if (res[0]!=0) res[0][298]=a489; + a489=(a51*a563); + a489=(a541*a489); + a464=(a51*a538); + a453=(a545*a464); + a454=(a51*a561); + a438=(a546*a454); + a453=(a453+a438); + a438=(a453/a558); + a453=(a548*a453); + a442=(a547*a464); + a443=(a551*a454); + a442=(a442-a443); + a443=(a552*a442); + a453=(a453+a443); + a443=(a518*a453); + a438=(a438-a443); + a438=(a534*a438); + a443=(a551*a438); + a442=(a442/a558); + a453=(a557*a453); + a442=(a442-a453); + a442=(a534*a442); + a453=(a546*a442); + a443=(a443+a453); + a443=(a562*a443); + a489=(a489+a443); + a443=(a545*a442); + a453=(a547*a438); + a443=(a443-a453); + a443=(a565*a443); + a453=(a51*a507); + a453=(a560*a453); + a443=(a443-a453); + a489=(a489+a443); + a443=(a51*a568); + a443=(a567*a443); + a453=(a51*a571); + a453=(a570*a453); + a443=(a443+a453); + a453=(a489+a443); + a446=(a32*a573); + a446=(a572*a446); + a465=(a32*a577); + a465=(a576*a465); + a446=(a446+a465); + a453=(a453+a446); + a465=(a32*a579); + a465=(a578*a465); + a457=(a32*a583); + a457=(a582*a457); + a465=(a465+a457); + a453=(a453+a465); + a457=(a510*a488); + a377=(a504*a482); + a457=(a457+a377); + a457=(a580*a457); + a453=(a453-a457); + a482=(a496*a482); + a457=(a510*a485); + a482=(a482-a457); + a482=(a584*a482); + a453=(a453+a482); + a482=(a496*a483); + a485=(a503*a485); + a482=(a482-a485); + a482=(a564*a482); + a453=(a453-a482); + a488=(a503*a488); + a483=(a504*a483); + a488=(a488+a483); + a488=(a585*a488); + a453=(a453-a488); + if (res[0]!=0) res[0][299]=a453; + a453=(a14*a611); + a453=(a589*a453); + a488=(a14*a587); + a483=(a593*a488); + a482=(a14*a609); + a485=(a594*a482); + a483=(a483+a485); + a485=(a483/a606); + a483=(a596*a483); + a457=(a595*a488); + a377=(a599*a482); + a457=(a457-a377); + a377=(a600*a457); + a483=(a483+a377); + a377=(a586*a483); + a485=(a485-a377); + a485=(a588*a485); + a377=(a599*a485); + a457=(a457/a606); + a483=(a605*a483); + a457=(a457-a483); + a457=(a588*a457); + a483=(a594*a457); + a377=(a377+a483); + a377=(a610*a377); + a453=(a453+a377); + a377=(a593*a457); + a483=(a595*a485); + a377=(a377-a483); + a377=(a613*a377); + a483=(a14*a553); + a483=(a614*a483); + a377=(a377-a483); + a453=(a453+a377); + a377=(a2*a618); + a377=(a617*a377); + a483=(a2*a621); + a483=(a620*a483); + a377=(a377+a483); + a483=(a453+a377); + a460=(a29*a623); + a460=(a622*a460); + a499=(a29*a627); + a499=(a626*a499); + a460=(a460+a499); + a483=(a483+a460); + a499=(a29*a629); + a499=(a628*a499); + a469=(a29*a633); + a469=(a632*a469); + a499=(a499+a469); + a483=(a483+a499); + a469=(a556*a464); + a470=(a550*a438); + a469=(a469+a470); + a469=(a630*a469); + a483=(a483-a469); + a438=(a542*a438); + a469=(a556*a454); + a438=(a438-a469); + a438=(a634*a438); + a483=(a483+a438); + a438=(a542*a442); + a454=(a549*a454); + a438=(a438-a454); + a438=(a591*a438); + a483=(a483-a438); + a464=(a549*a464); + a442=(a550*a442); + a464=(a464+a442); + a464=(a635*a464); + a483=(a483-a464); + if (res[0]!=0) res[0][300]=a483; + a483=(a20*a661); + a483=(a639*a483); + a464=(a20*a637); + a442=(a612*a464); + a438=(a20*a659); + a454=(a644*a438); + a442=(a442+a454); + a454=(a442/a656); + a442=(a646*a442); + a469=(a645*a464); + a470=(a649*a438); + a469=(a469-a470); + a470=(a650*a469); + a442=(a442+a470); + a470=(a636*a442); + a454=(a454-a470); + a454=(a638*a454); + a470=(a649*a454); + a469=(a469/a656); + a442=(a655*a442); + a469=(a469-a442); + a469=(a638*a469); + a442=(a644*a469); + a470=(a470+a442); + a470=(a660*a470); + a483=(a483+a470); + a470=(a612*a469); + a442=(a645*a454); + a470=(a470-a442); + a470=(a663*a470); + a442=(a20*a607); + a442=(a658*a442); + a470=(a470-a442); + a483=(a483+a470); + a470=(a15*a666); + a470=(a665*a470); + a442=(a15*a669); + a442=(a668*a442); + a470=(a470+a442); + a442=(a483+a470); + a411=(a21*a671); + a411=(a670*a411); + a449=(a21*a675); + a449=(a674*a449); + a411=(a411+a449); + a442=(a442+a411); + a449=(a21*a677); + a449=(a676*a449); + a451=(a21*a681); + a451=(a680*a451); + a449=(a449+a451); + a442=(a442+a449); + a451=(a604*a488); + a466=(a598*a485); + a451=(a451+a466); + a451=(a643*a451); + a442=(a442-a451); + a485=(a590*a485); + a451=(a604*a482); + a485=(a485-a451); + a485=(a601*a485); + a442=(a442+a485); + a485=(a590*a457); + a482=(a597*a482); + a485=(a485-a482); + a485=(a559*a485); + a442=(a442-a485); + a488=(a597*a488); + a457=(a598*a457); + a488=(a488+a457); + a488=(a608*a488); + a442=(a442-a488); + if (res[0]!=0) res[0][301]=a442; + a442=(a18*a693); + a442=(a700*a442); + a488=(a18*a706); + a457=(a688*a488); + a485=(a18*a687); + a482=(a692*a485); + a457=(a457+a482); + a482=(a457/a686); + a457=(a684*a457); + a451=(a696*a488); + a466=(a689*a485); + a451=(a451-a466); + a466=(a702*a451); + a457=(a457+a466); + a466=(a691*a457); + a482=(a482-a466); + a482=(a662*a482); + a466=(a689*a482); + a451=(a451/a686); + a457=(a685*a457); + a451=(a451-a457); + a451=(a662*a451); + a457=(a692*a451); + a466=(a466+a457); + a466=(a697*a466); + a442=(a442+a466); + a466=(a688*a451); + a457=(a696*a482); + a466=(a466-a457); + a466=(a708*a466); + a457=(a18*a699); + a457=(a709*a457); + a466=(a466-a457); + a442=(a442+a466); + a466=(a18*a714); + a466=(a713*a466); + a457=(a18*a717); + a457=(a716*a457); + a466=(a466+a457); + a457=(a442+a466); + a455=(a748*a720); + a455=(a719*a455); + a450=(a748*a724); + a450=(a723*a450); + a455=(a455+a450); + a457=(a457+a455); + a450=(a748*a727); + a450=(a726*a450); + a467=(a748*a731); + a467=(a730*a467); + a450=(a450+a467); + a457=(a457+a450); + a467=(a654*a464); + a461=(a648*a454); + a467=(a467+a461); + a467=(a732*a467); + a457=(a457-a467); + a454=(a640*a454); + a467=(a654*a438); + a454=(a454-a467); + a454=(a733*a454); + a457=(a457+a454); + a454=(a640*a469); + a438=(a647*a438); + a454=(a454-a438); + a454=(a682*a454); + a457=(a457-a454); + a464=(a647*a464); + a469=(a648*a469); + a464=(a464+a469); + a464=(a734*a464); + a457=(a457-a464); + if (res[0]!=0) res[0][302]=a457; + a457=(a690*a482); + a464=(a678*a485); + a457=(a457-a464); + a457=(a711*a457); + a464=(a678*a488); + a482=(a657*a482); + a464=(a464+a482); + a464=(a751*a464); + a457=(a457-a464); + a464=(a690*a451); + a485=(a701*a485); + a464=(a464-a485); + a464=(a742*a464); + a457=(a457-a464); + a488=(a701*a488); + a451=(a657*a451); + a488=(a488+a451); + a488=(a705*a488); + a457=(a457-a488); + if (res[0]!=0) res[0][303]=a457; + a780=(a8*a780); + a736=(a8*a736); + a780=(a780+a736); + a13=(a8*a13); + a780=(a780+a13); + a683=(a8*a683); + a780=(a780+a683); + a768=(a8*a768); + a780=(a780+a768); + if (res[0]!=0) res[0][304]=a780; + a801=(a8*a801); + a70=(a8*a70); + a801=(a801+a70); + a807=(a8*a807); + a801=(a801+a807); + a12=(a8*a12); + a801=(a801+a12); + a33=(a8*a33); + a801=(a801+a33); + if (res[0]!=0) res[0][305]=a801; + a63=(a8*a63); + a135=(a8*a135); + a63=(a63+a135); + a203=(a8*a203); + a63=(a63+a203); + a122=(a8*a122); + a63=(a63+a122); + a74=(a8*a74); + a63=(a63+a74); + if (res[0]!=0) res[0][306]=a63; + a138=(a8*a138); + a169=(a8*a169); + a138=(a138+a169); + a16=(a8*a16); + a138=(a138+a16); + a173=(a8*a173); + a138=(a138+a173); + a174=(a8*a174); + a138=(a138+a174); + if (res[0]!=0) res[0][307]=a138; + a196=(a8*a196); + a205=(a8*a205); + a196=(a196+a205); + a115=(a8*a115); + a196=(a196+a115); + a228=(a8*a228); + a196=(a196+a228); + a212=(a8*a212); + a196=(a196+a212); + if (res[0]!=0) res[0][308]=a196; + a260=(a8*a260); + a291=(a8*a291); + a260=(a260+a291); + a198=(a8*a198); + a260=(a260+a198); + a278=(a8*a278); + a260=(a260+a278); + a257=(a8*a257); + a260=(a260+a257); + if (res[0]!=0) res[0][309]=a260; + a313=(a8*a313); + a311=(a8*a311); + a313=(a313+a311); + a211=(a8*a211); + a313=(a313+a211); + a266=(a8*a266); + a313=(a313+a266); + a390=(a8*a390); + a313=(a313+a390); + if (res[0]!=0) res[0][310]=a313; + a315=(a8*a315); + a393=(a8*a393); + a315=(a315+a393); + a294=(a8*a294); + a315=(a315+a294); + a347=(a8*a347); + a315=(a315+a347); + a369=(a8*a369); + a315=(a315+a369); + if (res[0]!=0) res[0][311]=a315; + a442=(a4*a442); + a315=(a5*a442); + a369=(a10*a442); + a466=(a38*a466); + a369=(a369+a466); + a466=(a5*a369); + a315=(a315+a466); + a466=(a10*a442); + a455=(a44*a455); + a466=(a466+a455); + a455=(a5*a466); + a315=(a315+a455); + a450=(a44*a450); + a450=(a442+a450); + a455=(a5*a450); + a315=(a315+a455); + a455=(a5*a315); + a483=(a4*a483); + a347=(a5*a483); + a455=(a455+a347); + a347=(a10*a483); + a470=(a38*a470); + a347=(a347+a470); + a470=(a5*a347); + a455=(a455+a470); + a470=(a10*a483); + a411=(a44*a411); + a470=(a470+a411); + a411=(a5*a470); + a455=(a455+a411); + a449=(a44*a449); + a449=(a483+a449); + a411=(a5*a449); + a455=(a455+a411); + a411=(a5*a455); + a453=(a4*a453); + a294=(a5*a453); + a411=(a411+a294); + a294=(a10*a453); + a377=(a38*a377); + a294=(a294+a377); + a377=(a5*a294); + a411=(a411+a377); + a377=(a10*a453); + a460=(a44*a460); + a377=(a377+a460); + a460=(a5*a377); + a411=(a411+a460); + a499=(a44*a499); + a499=(a453+a499); + a460=(a5*a499); + a411=(a411+a460); + a460=(a5*a411); + a489=(a4*a489); + a393=(a5*a489); + a460=(a460+a393); + a393=(a10*a489); + a443=(a38*a443); + a393=(a393+a443); + a443=(a5*a393); + a460=(a460+a443); + a443=(a10*a489); + a446=(a44*a446); + a443=(a443+a446); + a446=(a5*a443); + a460=(a460+a446); + a465=(a44*a465); + a465=(a489+a465); + a446=(a5*a465); + a460=(a460+a446); + a460=(a5*a460); + a476=(a4*a476); + a446=(a5*a476); + a460=(a460+a446); + a446=(a10*a476); + a479=(a38*a479); + a446=(a446+a479); + a446=(a5*a446); + a460=(a460+a446); + a446=(a10*a476); + a477=(a44*a477); + a446=(a446+a477); + a446=(a5*a446); + a460=(a460+a446); + a474=(a44*a474); + a476=(a476+a474); + a476=(a5*a476); + a460=(a460+a476); + a476=(a5*a460); + a445=(a4*a445); + a474=(a5*a445); + a476=(a476+a474); + a474=(a10*a445); + a473=(a38*a473); + a474=(a474+a473); + a473=(a5*a474); + a476=(a476+a473); + a473=(a10*a445); + a478=(a44*a478); + a473=(a473+a478); + a478=(a5*a473); + a476=(a476+a478); + a484=(a44*a484); + a484=(a445+a484); + a478=(a5*a484); + a476=(a476+a478); + a476=(a8*a476); + a478=(a50*a34); + a476=(a476-a478); + a446=(a50*a34); + a476=(a476-a446); + if (res[0]!=0) res[0][312]=a476; + a414=(a28*a414); + a471=(a471*a414); + a430=(a42*a430); + a481=(a481*a430); + a471=(a471+a481); + a436=(a42*a436); + a487=(a487*a436); + a471=(a471+a487); + a459=(a28*a459); + a472=(a472*a459); + a471=(a471-a472); + a444=(a42*a444); + a475=(a475*a444); + a471=(a471-a475); + a352=(a42*a352); + a480=(a480*a352); + a471=(a471-a480); + a471=(-a471); + if (res[0]!=0) res[0][313]=a471; + a460=(a8*a460); + a445=(a8*a445); + a460=(a460+a445); + a474=(a8*a474); + a460=(a460+a474); + a473=(a8*a473); + a460=(a460+a473); + a484=(a8*a484); + a460=(a460+a484); + a484=(a24*a34); + a460=(a460-a484); + a484=(a50*a34); + a460=(a460+a484); + a460=(a460+a478); + a460=(a460+a446); + a446=(a10*a34); + a460=(a460+a446); + if (res[0]!=0) res[0][314]=a460; + a460=sin(a509); + a460=(a460*a520); + a446=cos(a509); + a446=(a446*a497); + a460=(a460-a446); + a446=cos(a458); + a446=(a446*a497); + a460=(a460-a446); + a446=sin(a498); + a446=(a446*a523); + a478=cos(a498); + a478=(a478*a529); + a446=(a446-a478); + a460=(a460+a446); + a446=sin(a417); + a446=(a446*a528); + a478=cos(a417); + a478=(a478*a535); + a446=(a446-a478); + a460=(a460+a446); + a458=sin(a458); + a458=(a458*a520); + a460=(a460+a458); + if (res[0]!=0) res[0][315]=a460; + a460=cos(a509); + a458=(a765*a460); + a458=(a497*a458); + a446=cos(a498); + a478=(a772*a446); + a478=(a529*a478); + a458=(a458+a478); + a478=cos(a417); + a484=(a772*a478); + a484=(a535*a484); + a458=(a458+a484); + a509=sin(a509); + a484=(a765*a509); + a484=(a520*a484); + a458=(a458-a484); + a498=sin(a498); + a484=(a772*a498); + a484=(a523*a484); + a458=(a458-a484); + a417=sin(a417); + a484=(a772*a417); + a484=(a528*a484); + a458=(a458-a484); + a458=(-a458); + if (res[0]!=0) res[0][316]=a458; + a458=(a764*a460); + a458=(a497*a458); + a484=(a766*a446); + a484=(a529*a484); + a458=(a458+a484); + a484=(a766*a478); + a484=(a535*a484); + a458=(a458+a484); + a484=(a764*a509); + a484=(a520*a484); + a458=(a458-a484); + a484=(a766*a498); + a484=(a523*a484); + a458=(a458-a484); + a484=(a766*a417); + a484=(a528*a484); + a458=(a458-a484); + a458=(-a458); + if (res[0]!=0) res[0][317]=a458; + a458=(a703*a460); + a458=(a497*a458); + a484=(a747*a446); + a484=(a529*a484); + a458=(a458+a484); + a484=(a747*a478); + a484=(a535*a484); + a458=(a458+a484); + a484=(a703*a509); + a484=(a520*a484); + a458=(a458-a484); + a484=(a747*a498); + a484=(a523*a484); + a458=(a458-a484); + a484=(a747*a417); + a484=(a528*a484); + a458=(a458-a484); + a458=(-a458); + if (res[0]!=0) res[0][318]=a458; + a458=(a760*a460); + a458=(a497*a458); + a484=(a750*a446); + a484=(a529*a484); + a458=(a458+a484); + a484=(a750*a478); + a484=(a535*a484); + a458=(a458+a484); + a484=(a760*a509); + a484=(a520*a484); + a458=(a458-a484); + a484=(a750*a498); + a484=(a523*a484); + a458=(a458-a484); + a484=(a750*a417); + a484=(a528*a484); + a458=(a458-a484); + a458=(-a458); + if (res[0]!=0) res[0][319]=a458; + a458=(a23*a460); + a458=(a497*a458); + a484=(a759*a446); + a484=(a529*a484); + a458=(a458+a484); + a484=(a759*a478); + a484=(a535*a484); + a458=(a458+a484); + a484=(a23*a509); + a484=(a520*a484); + a458=(a458-a484); + a484=(a759*a498); + a484=(a523*a484); + a458=(a458-a484); + a484=(a759*a417); + a484=(a528*a484); + a458=(a458-a484); + a458=(-a458); + if (res[0]!=0) res[0][320]=a458; + a458=(a18*a460); + a458=(a497*a458); + a484=(a748*a446); + a484=(a529*a484); + a458=(a458+a484); + a484=(a748*a478); + a484=(a535*a484); + a458=(a458+a484); + a484=(a18*a509); + a484=(a520*a484); + a458=(a458-a484); + a484=(a748*a498); + a484=(a523*a484); + a458=(a458-a484); + a484=(a748*a417); + a484=(a528*a484); + a458=(a458-a484); + a458=(-a458); + if (res[0]!=0) res[0][321]=a458; + a458=(a15*a460); + a458=(a497*a458); + a484=(a21*a446); + a484=(a529*a484); + a458=(a458+a484); + a484=(a21*a478); + a484=(a535*a484); + a458=(a458+a484); + a484=(a15*a509); + a484=(a520*a484); + a458=(a458-a484); + a484=(a21*a498); + a484=(a523*a484); + a458=(a458-a484); + a484=(a21*a417); + a484=(a528*a484); + a458=(a458-a484); + a458=(-a458); + if (res[0]!=0) res[0][322]=a458; + a458=(a2*a460); + a458=(a497*a458); + a484=(a29*a446); + a484=(a529*a484); + a458=(a458+a484); + a484=(a29*a478); + a484=(a535*a484); + a458=(a458+a484); + a484=(a2*a509); + a484=(a520*a484); + a458=(a458-a484); + a484=(a29*a498); + a484=(a523*a484); + a458=(a458-a484); + a484=(a29*a417); + a484=(a528*a484); + a458=(a458-a484); + a458=(-a458); + if (res[0]!=0) res[0][323]=a458; + a458=(a51*a460); + a458=(a497*a458); + a484=(a32*a446); + a484=(a529*a484); + a458=(a458+a484); + a484=(a32*a478); + a484=(a535*a484); + a458=(a458+a484); + a484=(a51*a509); + a484=(a520*a484); + a458=(a458-a484); + a484=(a32*a498); + a484=(a523*a484); + a458=(a458-a484); + a484=(a32*a417); + a484=(a528*a484); + a458=(a458-a484); + a458=(-a458); + if (res[0]!=0) res[0][324]=a458; + a458=(a10*a34); + a484=(a10*a34); + a473=(a458+a484); + a473=(-a473); + if (res[0]!=0) res[0][325]=a473; + a473=(a41*a460); + a473=(a497*a473); + a474=(a35*a446); + a474=(a529*a474); + a473=(a473+a474); + a474=(a35*a478); + a474=(a535*a474); + a473=(a473+a474); + a474=(a41*a509); + a474=(a520*a474); + a473=(a473-a474); + a474=(a35*a498); + a474=(a523*a474); + a473=(a473-a474); + a474=(a35*a417); + a474=(a528*a474); + a473=(a473-a474); + a473=(-a473); + if (res[0]!=0) res[0][326]=a473; + a473=(a3*a34); + a474=(a10*a34); + a473=(a473-a474); + a458=(a458-a473); + a458=(a458+a484); + a484=(a10*a34); + a458=(a458+a484); + if (res[0]!=0) res[0][327]=a458; + a517=(a28*a517); + a495=(a495*a517); + a493=(a28*a493); + a517=(a468*a493); + a515=(a28*a515); + a458=(a500*a515); + a517=(a517+a458); + a458=(a517/a512); + a502=(a502*a517); + a517=(a501*a493); + a484=(a505*a515); + a517=(a517-a484); + a506=(a506*a517); + a502=(a502+a506); + a492=(a492*a502); + a458=(a458-a492); + a458=(a494*a458); + a505=(a505*a458); + a517=(a517/a512); + a511=(a511*a502); + a517=(a517-a511); + a494=(a494*a517); + a500=(a500*a494); + a505=(a505+a500); + a516=(a516*a505); + a495=(a495+a516); + a468=(a468*a494); + a501=(a501*a458); + a468=(a468-a501); + a519=(a519*a468); + a463=(a28*a463); + a514=(a514*a463); + a519=(a519-a514); + a495=(a495+a519); + a522=(a28*a522); + a521=(a521*a522); + a525=(a28*a525); + a524=(a524*a525); + a521=(a521+a524); + a524=(a495+a521); + a527=(a42*a527); + a526=(a526*a527); + a531=(a42*a531); + a530=(a530*a531); + a526=(a526+a530); + a524=(a524+a526); + a533=(a42*a533); + a532=(a532*a533); + a537=(a42*a537); + a536=(a536*a537); + a532=(a532+a536); + a524=(a524+a532); + if (res[0]!=0) res[0][328]=a524; + a524=(a41*a563); + a524=(a541*a524); + a536=(a41*a538); + a537=(a545*a536); + a533=(a41*a561); + a530=(a546*a533); + a537=(a537+a530); + a530=(a537/a558); + a537=(a548*a537); + a531=(a547*a536); + a527=(a551*a533); + a531=(a531-a527); + a527=(a552*a531); + a537=(a537+a527); + a527=(a518*a537); + a530=(a530-a527); + a530=(a534*a530); + a527=(a551*a530); + a531=(a531/a558); + a537=(a557*a537); + a531=(a531-a537); + a531=(a534*a531); + a537=(a546*a531); + a527=(a527+a537); + a527=(a562*a527); + a524=(a524+a527); + a527=(a545*a531); + a537=(a547*a530); + a527=(a527-a537); + a527=(a565*a527); + a537=(a41*a507); + a537=(a560*a537); + a527=(a527-a537); + a524=(a524+a527); + a527=(a41*a568); + a527=(a567*a527); + a537=(a41*a571); + a537=(a570*a537); + a527=(a527+a537); + a537=(a524+a527); + a525=(a35*a573); + a525=(a572*a525); + a522=(a35*a577); + a522=(a576*a522); + a525=(a525+a522); + a537=(a537+a525); + a522=(a35*a579); + a522=(a578*a522); + a519=(a35*a583); + a519=(a582*a519); + a522=(a522+a519); + a537=(a537+a522); + a519=(a510*a493); + a514=(a504*a458); + a519=(a519+a514); + a580=(a580*a519); + a537=(a537-a580); + a458=(a496*a458); + a510=(a510*a515); + a458=(a458-a510); + a584=(a584*a458); + a537=(a537+a584); + a496=(a496*a494); + a515=(a503*a515); + a496=(a496-a515); + a564=(a564*a496); + a537=(a537-a564); + a503=(a503*a493); + a504=(a504*a494); + a503=(a503+a504); + a585=(a585*a503); + a537=(a537-a585); + if (res[0]!=0) res[0][329]=a537; + a537=(a51*a611); + a537=(a589*a537); + a585=(a51*a587); + a503=(a593*a585); + a504=(a51*a609); + a494=(a594*a504); + a503=(a503+a494); + a494=(a503/a606); + a503=(a596*a503); + a493=(a595*a585); + a564=(a599*a504); + a493=(a493-a564); + a564=(a600*a493); + a503=(a503+a564); + a564=(a586*a503); + a494=(a494-a564); + a494=(a588*a494); + a564=(a599*a494); + a493=(a493/a606); + a503=(a605*a503); + a493=(a493-a503); + a493=(a588*a493); + a503=(a594*a493); + a564=(a564+a503); + a564=(a610*a564); + a537=(a537+a564); + a564=(a593*a493); + a503=(a595*a494); + a564=(a564-a503); + a564=(a613*a564); + a503=(a51*a553); + a503=(a614*a503); + a564=(a564-a503); + a537=(a537+a564); + a564=(a51*a618); + a564=(a617*a564); + a503=(a51*a621); + a503=(a620*a503); + a564=(a564+a503); + a503=(a537+a564); + a496=(a32*a623); + a496=(a622*a496); + a515=(a32*a627); + a515=(a626*a515); + a496=(a496+a515); + a503=(a503+a496); + a515=(a32*a629); + a515=(a628*a515); + a584=(a32*a633); + a584=(a632*a584); + a515=(a515+a584); + a503=(a503+a515); + a584=(a556*a536); + a458=(a550*a530); + a584=(a584+a458); + a584=(a630*a584); + a503=(a503-a584); + a530=(a542*a530); + a584=(a556*a533); + a530=(a530-a584); + a530=(a634*a530); + a503=(a503+a530); + a530=(a542*a531); + a533=(a549*a533); + a530=(a530-a533); + a530=(a591*a530); + a503=(a503-a530); + a536=(a549*a536); + a531=(a550*a531); + a536=(a536+a531); + a536=(a635*a536); + a503=(a503-a536); + if (res[0]!=0) res[0][330]=a503; + a503=(a14*a661); + a503=(a639*a503); + a536=(a14*a637); + a531=(a612*a536); + a530=(a14*a659); + a533=(a644*a530); + a531=(a531+a533); + a533=(a531/a656); + a531=(a646*a531); + a584=(a645*a536); + a458=(a649*a530); + a584=(a584-a458); + a458=(a650*a584); + a531=(a531+a458); + a458=(a636*a531); + a533=(a533-a458); + a533=(a638*a533); + a458=(a649*a533); + a584=(a584/a656); + a531=(a655*a531); + a584=(a584-a531); + a584=(a638*a584); + a531=(a644*a584); + a458=(a458+a531); + a458=(a660*a458); + a503=(a503+a458); + a458=(a612*a584); + a531=(a645*a533); + a458=(a458-a531); + a458=(a663*a458); + a531=(a14*a607); + a531=(a658*a531); + a458=(a458-a531); + a503=(a503+a458); + a458=(a2*a666); + a458=(a665*a458); + a531=(a2*a669); + a531=(a668*a531); + a458=(a458+a531); + a531=(a503+a458); + a510=(a29*a671); + a510=(a670*a510); + a580=(a29*a675); + a580=(a674*a580); + a510=(a510+a580); + a531=(a531+a510); + a580=(a29*a677); + a580=(a676*a580); + a519=(a29*a681); + a519=(a680*a519); + a580=(a580+a519); + a531=(a531+a580); + a519=(a604*a585); + a514=(a598*a494); + a519=(a519+a514); + a519=(a643*a519); + a531=(a531-a519); + a494=(a590*a494); + a519=(a604*a504); + a494=(a494-a519); + a494=(a601*a494); + a531=(a531+a494); + a494=(a590*a493); + a504=(a597*a504); + a494=(a494-a504); + a494=(a559*a494); + a531=(a531-a494); + a585=(a597*a585); + a493=(a598*a493); + a585=(a585+a493); + a585=(a608*a585); + a531=(a531-a585); + if (res[0]!=0) res[0][331]=a531; + a531=(a20*a693); + a531=(a700*a531); + a585=(a20*a706); + a493=(a688*a585); + a494=(a20*a687); + a504=(a692*a494); + a493=(a493+a504); + a504=(a493/a686); + a493=(a684*a493); + a519=(a696*a585); + a514=(a689*a494); + a519=(a519-a514); + a514=(a702*a519); + a493=(a493+a514); + a514=(a691*a493); + a504=(a504-a514); + a504=(a662*a504); + a514=(a689*a504); + a519=(a519/a686); + a493=(a685*a493); + a519=(a519-a493); + a519=(a662*a519); + a493=(a692*a519); + a514=(a514+a493); + a514=(a697*a514); + a531=(a531+a514); + a514=(a688*a519); + a493=(a696*a504); + a514=(a514-a493); + a514=(a708*a514); + a20=(a20*a699); + a20=(a709*a20); + a514=(a514-a20); + a531=(a531+a514); + a514=(a15*a714); + a514=(a713*a514); + a20=(a15*a717); + a20=(a716*a20); + a514=(a514+a20); + a20=(a531+a514); + a493=(a21*a720); + a493=(a719*a493); + a463=(a21*a724); + a463=(a723*a463); + a493=(a493+a463); + a20=(a20+a493); + a463=(a21*a727); + a463=(a726*a463); + a468=(a21*a731); + a468=(a730*a468); + a463=(a463+a468); + a20=(a20+a463); + a468=(a654*a536); + a501=(a648*a533); + a468=(a468+a501); + a468=(a732*a468); + a20=(a20-a468); + a533=(a640*a533); + a468=(a654*a530); + a533=(a533-a468); + a533=(a733*a533); + a20=(a20+a533); + a533=(a640*a584); + a530=(a647*a530); + a533=(a533-a530); + a533=(a682*a533); + a20=(a20-a533); + a536=(a647*a536); + a584=(a648*a584); + a536=(a536+a584); + a536=(a734*a536); + a20=(a20-a536); + if (res[0]!=0) res[0][332]=a20; + a20=(a690*a504); + a536=(a678*a494); + a20=(a20-a536); + a20=(a711*a20); + a536=(a678*a585); + a504=(a657*a504); + a536=(a536+a504); + a536=(a751*a536); + a20=(a20-a536); + a536=(a690*a519); + a494=(a701*a494); + a536=(a536-a494); + a536=(a742*a536); + a20=(a20-a536); + a585=(a701*a585); + a519=(a657*a519); + a585=(a585+a519); + a585=(a705*a585); + a20=(a20-a585); + if (res[0]!=0) res[0][333]=a20; + a786=(a8*a786); + a739=(a8*a739); + a786=(a786+a739); + a36=(a8*a36); + a786=(a786+a36); + a771=(a8*a771); + a786=(a786+a771); + a774=(a8*a774); + a786=(a786+a774); + if (res[0]!=0) res[0][334]=a786; + a805=(a8*a805); + a77=(a8*a77); + a805=(a805+a77); + a98=(a8*a98); + a805=(a805+a98); + a794=(a8*a794); + a805=(a805+a794); + a788=(a8*a788); + a805=(a805+a788); + if (res[0]!=0) res[0][335]=a805; + a43=(a8*a43); + a117=(a8*a117); + a43=(a43+a117); + a37=(a8*a37); + a43=(a43+a37); + a96=(a8*a96); + a43=(a43+a96); + a91=(a8*a91); + a43=(a43+a91); + if (res[0]!=0) res[0][336]=a43; + a9=(a8*a9); + a181=(a8*a181); + a9=(a9+a181); + a746=(a8*a746); + a9=(a9+a746); + a168=(a8*a168); + a9=(a9+a168); + a112=(a8*a112); + a9=(a9+a112); + if (res[0]!=0) res[0][337]=a9; + a61=(a8*a61); + a298=(a8*a298); + a61=(a61+a298); + a59=(a8*a59); + a61=(a61+a59); + a223=(a8*a223); + a61=(a61+a223); + a224=(a8*a224); + a61=(a61+a224); + if (res[0]!=0) res[0][338]=a61; + a186=(a8*a186); + a250=(a8*a250); + a186=(a186+a250); + a121=(a8*a121); + a186=(a186+a121); + a274=(a8*a274); + a186=(a186+a274); + a258=(a8*a258); + a186=(a186+a258); + if (res[0]!=0) res[0][339]=a186; + a310=(a8*a310); + a339=(a8*a339); + a310=(a310+a339); + a167=(a8*a167); + a310=(a310+a167); + a322=(a8*a322); + a310=(a310+a322); + a276=(a8*a276); + a310=(a310+a276); + if (res[0]!=0) res[0][340]=a310; + a359=(a8*a359); + a357=(a8*a357); + a359=(a359+a357); + a217=(a8*a217); + a359=(a359+a217); + a281=(a8*a281); + a359=(a359+a281); + a403=(a8*a403); + a359=(a359+a403); + if (res[0]!=0) res[0][341]=a359; + a367=(a8*a367); + a441=(a8*a441); + a367=(a367+a441); + a263=(a8*a263); + a367=(a367+a263); + a447=(a8*a447); + a367=(a367+a447); + a419=(a8*a419); + a367=(a367+a419); + if (res[0]!=0) res[0][342]=a367; + a531=(a4*a531); + a367=(a5*a531); + a419=(a10*a531); + a514=(a38*a514); + a419=(a419+a514); + a514=(a5*a419); + a367=(a367+a514); + a514=(a10*a531); + a493=(a44*a493); + a514=(a514+a493); + a493=(a5*a514); + a367=(a367+a493); + a463=(a44*a463); + a463=(a531+a463); + a493=(a5*a463); + a367=(a367+a493); + a493=(a5*a367); + a503=(a4*a503); + a447=(a5*a503); + a493=(a493+a447); + a447=(a10*a503); + a458=(a38*a458); + a447=(a447+a458); + a458=(a5*a447); + a493=(a493+a458); + a458=(a10*a503); + a510=(a44*a510); + a458=(a458+a510); + a510=(a5*a458); + a493=(a493+a510); + a580=(a44*a580); + a580=(a503+a580); + a510=(a5*a580); + a493=(a493+a510); + a510=(a5*a493); + a537=(a4*a537); + a263=(a5*a537); + a510=(a510+a263); + a263=(a10*a537); + a564=(a38*a564); + a263=(a263+a564); + a564=(a5*a263); + a510=(a510+a564); + a564=(a10*a537); + a496=(a44*a496); + a564=(a564+a496); + a496=(a5*a564); + a510=(a510+a496); + a515=(a44*a515); + a515=(a537+a515); + a496=(a5*a515); + a510=(a510+a496); + a510=(a5*a510); + a524=(a4*a524); + a496=(a5*a524); + a510=(a510+a496); + a496=(a10*a524); + a527=(a38*a527); + a496=(a496+a527); + a496=(a5*a496); + a510=(a510+a496); + a496=(a10*a524); + a525=(a44*a525); + a496=(a496+a525); + a496=(a5*a496); + a510=(a510+a496); + a522=(a44*a522); + a524=(a524+a522); + a524=(a5*a524); + a510=(a510+a524); + a524=(a5*a510); + a495=(a4*a495); + a522=(a5*a495); + a524=(a524+a522); + a522=(a10*a495); + a521=(a38*a521); + a522=(a522+a521); + a521=(a5*a522); + a524=(a524+a521); + a521=(a10*a495); + a526=(a44*a526); + a521=(a521+a526); + a526=(a5*a521); + a524=(a524+a526); + a532=(a44*a532); + a532=(a495+a532); + a526=(a5*a532); + a524=(a524+a526); + a524=(a8*a524); + a526=(a50*a34); + a524=(a524-a526); + a496=(a50*a34); + a524=(a524-a496); + if (res[0]!=0) res[0][343]=a524; + a460=(a28*a460); + a497=(a497*a460); + a446=(a42*a446); + a529=(a529*a446); + a497=(a497+a529); + a478=(a42*a478); + a535=(a535*a478); + a497=(a497+a535); + a509=(a28*a509); + a520=(a520*a509); + a497=(a497-a520); + a498=(a42*a498); + a523=(a523*a498); + a497=(a497-a523); + a417=(a42*a417); + a528=(a528*a417); + a497=(a497-a528); + a497=(-a497); + if (res[0]!=0) res[0][344]=a497; + a510=(a8*a510); + a495=(a8*a495); + a510=(a510+a495); + a522=(a8*a522); + a510=(a510+a522); + a521=(a8*a521); + a510=(a510+a521); + a532=(a8*a532); + a510=(a510+a532); + a532=(a24*a34); + a510=(a510-a532); + a532=(a50*a34); + a510=(a510+a532); + a510=(a510+a526); + a510=(a510+a496); + a496=(a10*a34); + a510=(a510+a496); + if (res[0]!=0) res[0][345]=a510; + a510=sin(a555); + a510=(a510*a566); + a496=cos(a555); + a496=(a496*a543); + a510=(a510-a496); + a496=cos(a508); + a496=(a496*a543); + a510=(a510-a496); + a496=sin(a540); + a496=(a496*a569); + a526=cos(a540); + a526=(a526*a575); + a496=(a496-a526); + a510=(a510+a496); + a496=sin(a448); + a496=(a496*a574); + a526=cos(a448); + a526=(a526*a581); + a496=(a496-a526); + a510=(a510+a496); + a508=sin(a508); + a508=(a508*a566); + a510=(a510+a508); + if (res[0]!=0) res[0][346]=a510; + a510=cos(a555); + a508=(a777*a510); + a508=(a543*a508); + a496=cos(a540); + a526=(a779*a496); + a526=(a575*a526); + a508=(a508+a526); + a526=cos(a448); + a532=(a779*a526); + a532=(a581*a532); + a508=(a508+a532); + a555=sin(a555); + a532=(a777*a555); + a532=(a566*a532); + a508=(a508-a532); + a540=sin(a540); + a532=(a779*a540); + a532=(a569*a532); + a508=(a508-a532); + a448=sin(a448); + a532=(a779*a448); + a532=(a574*a532); + a508=(a508-a532); + a508=(-a508); + if (res[0]!=0) res[0][347]=a508; + a508=(a765*a510); + a508=(a543*a508); + a532=(a772*a496); + a532=(a575*a532); + a508=(a508+a532); + a532=(a772*a526); + a532=(a581*a532); + a508=(a508+a532); + a532=(a765*a555); + a532=(a566*a532); + a508=(a508-a532); + a532=(a772*a540); + a532=(a569*a532); + a508=(a508-a532); + a532=(a772*a448); + a532=(a574*a532); + a508=(a508-a532); + a508=(-a508); + if (res[0]!=0) res[0][348]=a508; + a508=(a764*a510); + a508=(a543*a508); + a532=(a766*a496); + a532=(a575*a532); + a508=(a508+a532); + a532=(a766*a526); + a532=(a581*a532); + a508=(a508+a532); + a532=(a764*a555); + a532=(a566*a532); + a508=(a508-a532); + a532=(a766*a540); + a532=(a569*a532); + a508=(a508-a532); + a532=(a766*a448); + a532=(a574*a532); + a508=(a508-a532); + a508=(-a508); + if (res[0]!=0) res[0][349]=a508; + a508=(a703*a510); + a508=(a543*a508); + a532=(a747*a496); + a532=(a575*a532); + a508=(a508+a532); + a532=(a747*a526); + a532=(a581*a532); + a508=(a508+a532); + a532=(a703*a555); + a532=(a566*a532); + a508=(a508-a532); + a532=(a747*a540); + a532=(a569*a532); + a508=(a508-a532); + a532=(a747*a448); + a532=(a574*a532); + a508=(a508-a532); + a508=(-a508); + if (res[0]!=0) res[0][350]=a508; + a508=(a760*a510); + a508=(a543*a508); + a532=(a750*a496); + a532=(a575*a532); + a508=(a508+a532); + a532=(a750*a526); + a532=(a581*a532); + a508=(a508+a532); + a532=(a760*a555); + a532=(a566*a532); + a508=(a508-a532); + a532=(a750*a540); + a532=(a569*a532); + a508=(a508-a532); + a532=(a750*a448); + a532=(a574*a532); + a508=(a508-a532); + a508=(-a508); + if (res[0]!=0) res[0][351]=a508; + a508=(a23*a510); + a508=(a543*a508); + a532=(a759*a496); + a532=(a575*a532); + a508=(a508+a532); + a532=(a759*a526); + a532=(a581*a532); + a508=(a508+a532); + a532=(a23*a555); + a532=(a566*a532); + a508=(a508-a532); + a532=(a759*a540); + a532=(a569*a532); + a508=(a508-a532); + a532=(a759*a448); + a532=(a574*a532); + a508=(a508-a532); + a508=(-a508); + if (res[0]!=0) res[0][352]=a508; + a508=(a18*a510); + a508=(a543*a508); + a532=(a748*a496); + a532=(a575*a532); + a508=(a508+a532); + a532=(a748*a526); + a532=(a581*a532); + a508=(a508+a532); + a532=(a18*a555); + a532=(a566*a532); + a508=(a508-a532); + a532=(a748*a540); + a532=(a569*a532); + a508=(a508-a532); + a532=(a748*a448); + a532=(a574*a532); + a508=(a508-a532); + a508=(-a508); + if (res[0]!=0) res[0][353]=a508; + a508=(a15*a510); + a508=(a543*a508); + a532=(a21*a496); + a532=(a575*a532); + a508=(a508+a532); + a532=(a21*a526); + a532=(a581*a532); + a508=(a508+a532); + a532=(a15*a555); + a532=(a566*a532); + a508=(a508-a532); + a532=(a21*a540); + a532=(a569*a532); + a508=(a508-a532); + a532=(a21*a448); + a532=(a574*a532); + a508=(a508-a532); + a508=(-a508); + if (res[0]!=0) res[0][354]=a508; + a508=(a2*a510); + a508=(a543*a508); + a532=(a29*a496); + a532=(a575*a532); + a508=(a508+a532); + a532=(a29*a526); + a532=(a581*a532); + a508=(a508+a532); + a532=(a2*a555); + a532=(a566*a532); + a508=(a508-a532); + a532=(a29*a540); + a532=(a569*a532); + a508=(a508-a532); + a532=(a29*a448); + a532=(a574*a532); + a508=(a508-a532); + a508=(-a508); + if (res[0]!=0) res[0][355]=a508; + a508=(a51*a510); + a508=(a543*a508); + a532=(a32*a496); + a532=(a575*a532); + a508=(a508+a532); + a532=(a32*a526); + a532=(a581*a532); + a508=(a508+a532); + a532=(a51*a555); + a532=(a566*a532); + a508=(a508-a532); + a532=(a32*a540); + a532=(a569*a532); + a508=(a508-a532); + a532=(a32*a448); + a532=(a574*a532); + a508=(a508-a532); + a508=(-a508); + if (res[0]!=0) res[0][356]=a508; + a508=(a10*a34); + a532=(a10*a34); + a521=(a508+a532); + a521=(-a521); + if (res[0]!=0) res[0][357]=a521; + a521=(a41*a510); + a521=(a543*a521); + a522=(a35*a496); + a522=(a575*a522); + a521=(a521+a522); + a522=(a35*a526); + a522=(a581*a522); + a521=(a521+a522); + a522=(a41*a555); + a522=(a566*a522); + a521=(a521-a522); + a522=(a35*a540); + a522=(a569*a522); + a521=(a521-a522); + a522=(a35*a448); + a522=(a574*a522); + a521=(a521-a522); + a521=(-a521); + if (res[0]!=0) res[0][358]=a521; + a521=(a3*a34); + a522=(a10*a34); + a495=(a521-a522); + a508=(a508-a495); + a508=(a508+a532); + a532=(a10*a34); + a508=(a508+a532); + if (res[0]!=0) res[0][359]=a508; + a563=(a28*a563); + a541=(a541*a563); + a538=(a28*a538); + a563=(a545*a538); + a561=(a28*a561); + a508=(a546*a561); + a563=(a563+a508); + a508=(a563/a558); + a548=(a548*a563); + a563=(a547*a538); + a532=(a551*a561); + a563=(a563-a532); + a552=(a552*a563); + a548=(a548+a552); + a518=(a518*a548); + a508=(a508-a518); + a508=(a534*a508); + a551=(a551*a508); + a563=(a563/a558); + a557=(a557*a548); + a563=(a563-a557); + a534=(a534*a563); + a546=(a546*a534); + a551=(a551+a546); + a562=(a562*a551); + a541=(a541+a562); + a545=(a545*a534); + a547=(a547*a508); + a545=(a545-a547); + a565=(a565*a545); + a507=(a28*a507); + a560=(a560*a507); + a565=(a565-a560); + a541=(a541+a565); + a568=(a28*a568); + a567=(a567*a568); + a571=(a28*a571); + a570=(a570*a571); + a567=(a567+a570); + a570=(a541+a567); + a573=(a42*a573); + a572=(a572*a573); + a577=(a42*a577); + a576=(a576*a577); + a572=(a572+a576); + a570=(a570+a572); + a579=(a42*a579); + a578=(a578*a579); + a583=(a42*a583); + a582=(a582*a583); + a578=(a578+a582); + a570=(a570+a578); + if (res[0]!=0) res[0][360]=a570; + a570=(a41*a611); + a570=(a589*a570); + a582=(a41*a587); + a583=(a593*a582); + a579=(a41*a609); + a576=(a594*a579); + a583=(a583+a576); + a576=(a583/a606); + a583=(a596*a583); + a577=(a595*a582); + a573=(a599*a579); + a577=(a577-a573); + a573=(a600*a577); + a583=(a583+a573); + a573=(a586*a583); + a576=(a576-a573); + a576=(a588*a576); + a573=(a599*a576); + a577=(a577/a606); + a583=(a605*a583); + a577=(a577-a583); + a577=(a588*a577); + a583=(a594*a577); + a573=(a573+a583); + a573=(a610*a573); + a570=(a570+a573); + a573=(a593*a577); + a583=(a595*a576); + a573=(a573-a583); + a573=(a613*a573); + a583=(a41*a553); + a583=(a614*a583); + a573=(a573-a583); + a570=(a570+a573); + a573=(a41*a618); + a573=(a617*a573); + a583=(a41*a621); + a583=(a620*a583); + a573=(a573+a583); + a583=(a570+a573); + a571=(a35*a623); + a571=(a622*a571); + a568=(a35*a627); + a568=(a626*a568); + a571=(a571+a568); + a583=(a583+a571); + a568=(a35*a629); + a568=(a628*a568); + a565=(a35*a633); + a565=(a632*a565); + a568=(a568+a565); + a583=(a583+a568); + a565=(a556*a538); + a560=(a550*a508); + a565=(a565+a560); + a630=(a630*a565); + a583=(a583-a630); + a508=(a542*a508); + a556=(a556*a561); + a508=(a508-a556); + a634=(a634*a508); + a583=(a583+a634); + a542=(a542*a534); + a561=(a549*a561); + a542=(a542-a561); + a591=(a591*a542); + a583=(a583-a591); + a549=(a549*a538); + a550=(a550*a534); + a549=(a549+a550); + a635=(a635*a549); + a583=(a583-a635); + if (res[0]!=0) res[0][361]=a583; + a583=(a51*a661); + a583=(a639*a583); + a635=(a51*a637); + a549=(a612*a635); + a550=(a51*a659); + a534=(a644*a550); + a549=(a549+a534); + a534=(a549/a656); + a549=(a646*a549); + a538=(a645*a635); + a591=(a649*a550); + a538=(a538-a591); + a591=(a650*a538); + a549=(a549+a591); + a591=(a636*a549); + a534=(a534-a591); + a534=(a638*a534); + a591=(a649*a534); + a538=(a538/a656); + a549=(a655*a549); + a538=(a538-a549); + a538=(a638*a538); + a549=(a644*a538); + a591=(a591+a549); + a591=(a660*a591); + a583=(a583+a591); + a591=(a612*a538); + a549=(a645*a534); + a591=(a591-a549); + a591=(a663*a591); + a549=(a51*a607); + a549=(a658*a549); + a591=(a591-a549); + a583=(a583+a591); + a591=(a51*a666); + a591=(a665*a591); + a549=(a51*a669); + a549=(a668*a549); + a591=(a591+a549); + a549=(a583+a591); + a542=(a32*a671); + a542=(a670*a542); + a561=(a32*a675); + a561=(a674*a561); + a542=(a542+a561); + a549=(a549+a542); + a561=(a32*a677); + a561=(a676*a561); + a634=(a32*a681); + a634=(a680*a634); + a561=(a561+a634); + a549=(a549+a561); + a634=(a604*a582); + a508=(a598*a576); + a634=(a634+a508); + a634=(a643*a634); + a549=(a549-a634); + a576=(a590*a576); + a634=(a604*a579); + a576=(a576-a634); + a576=(a601*a576); + a549=(a549+a576); + a576=(a590*a577); + a579=(a597*a579); + a576=(a576-a579); + a576=(a559*a576); + a549=(a549-a576); + a582=(a597*a582); + a577=(a598*a577); + a582=(a582+a577); + a582=(a608*a582); + a549=(a549-a582); + if (res[0]!=0) res[0][362]=a549; + a549=(a14*a693); + a549=(a700*a549); + a582=(a14*a706); + a577=(a688*a582); + a576=(a14*a687); + a579=(a692*a576); + a577=(a577+a579); + a579=(a577/a686); + a577=(a684*a577); + a634=(a696*a582); + a508=(a689*a576); + a634=(a634-a508); + a508=(a702*a634); + a577=(a577+a508); + a508=(a691*a577); + a579=(a579-a508); + a579=(a662*a579); + a508=(a689*a579); + a634=(a634/a686); + a577=(a685*a577); + a634=(a634-a577); + a634=(a662*a634); + a577=(a692*a634); + a508=(a508+a577); + a508=(a697*a508); + a549=(a549+a508); + a508=(a688*a634); + a577=(a696*a579); + a508=(a508-a577); + a508=(a708*a508); + a14=(a14*a699); + a14=(a709*a14); + a508=(a508-a14); + a549=(a549+a508); + a508=(a2*a714); + a508=(a713*a508); + a14=(a2*a717); + a14=(a716*a14); + a508=(a508+a14); + a14=(a549+a508); + a577=(a29*a720); + a577=(a719*a577); + a556=(a29*a724); + a556=(a723*a556); + a577=(a577+a556); + a14=(a14+a577); + a556=(a29*a727); + a556=(a726*a556); + a630=(a29*a731); + a630=(a730*a630); + a556=(a556+a630); + a14=(a14+a556); + a630=(a654*a635); + a565=(a648*a534); + a630=(a630+a565); + a732=(a732*a630); + a14=(a14-a732); + a534=(a640*a534); + a654=(a654*a550); + a534=(a534-a654); + a733=(a733*a534); + a14=(a14+a733); + a640=(a640*a538); + a550=(a647*a550); + a640=(a640-a550); + a682=(a682*a640); + a14=(a14-a682); + a647=(a647*a635); + a648=(a648*a538); + a647=(a647+a648); + a734=(a734*a647); + a14=(a14-a734); + if (res[0]!=0) res[0][363]=a14; + a14=(a690*a579); + a734=(a678*a576); + a14=(a14-a734); + a14=(a711*a14); + a734=(a678*a582); + a579=(a657*a579); + a734=(a734+a579); + a734=(a751*a734); + a14=(a14-a734); + a734=(a690*a634); + a576=(a701*a576); + a734=(a734-a576); + a734=(a742*a734); + a14=(a14-a734); + a582=(a701*a582); + a634=(a657*a634); + a582=(a582+a634); + a582=(a705*a582); + a14=(a14-a582); + if (res[0]!=0) res[0][364]=a14; + a792=(a8*a792); + a741=(a8*a741); + a792=(a792+a741); + a11=(a8*a11); + a792=(a792+a11); + a762=(a8*a762); + a792=(a792+a762); + a781=(a8*a781); + a792=(a792+a781); + if (res[0]!=0) res[0][365]=a792; + a809=(a8*a809); + a71=(a8*a71); + a809=(a809+a71); + a101=(a8*a101); + a809=(a809+a101); + a800=(a8*a800); + a809=(a809+a800); + a802=(a8*a802); + a809=(a809+a802); + if (res[0]!=0) res[0][366]=a809; + a95=(a8*a95); + a133=(a8*a133); + a95=(a95+a133); + a107=(a8*a107); + a95=(a95+a107); + a88=(a8*a88); + a95=(a95+a88); + a103=(a8*a103); + a95=(a95+a103); + if (res[0]!=0) res[0][367]=a95; + a125=(a8*a125); + a163=(a8*a163); + a125=(a125+a163); + a105=(a8*a105); + a125=(a125+a105); + a137=(a8*a137); + a125=(a125+a137); + a111=(a8*a111); + a125=(a125+a111); + if (res[0]!=0) res[0][368]=a125; + a171=(a8*a171); + a231=(a8*a231); + a171=(a171+a231); + a153=(a8*a153); + a171=(a171+a153); + a218=(a8*a218); + a171=(a171+a218); + a157=(a8*a157); + a171=(a171+a157); + if (res[0]!=0) res[0][369]=a171; + a234=(a8*a234); + a265=(a8*a265); + a234=(a234+a265); + a201=(a8*a201); + a234=(a234+a201); + a269=(a8*a269); + a234=(a234+a269); + a270=(a8*a270); + a234=(a234+a270); + if (res[0]!=0) res[0][370]=a234; + a292=(a8*a292); + a301=(a8*a301); + a292=(a292+a301); + a249=(a8*a249); + a292=(a292+a249); + a324=(a8*a324); + a292=(a292+a324); + a308=(a8*a308); + a292=(a292+a308); + if (res[0]!=0) res[0][371]=a292; + a356=(a8*a356); + a387=(a8*a387); + a356=(a356+a387); + a297=(a8*a297); + a356=(a356+a297); + a374=(a8*a374); + a356=(a356+a374); + a353=(a8*a353); + a356=(a356+a353); + if (res[0]!=0) res[0][372]=a356; + a409=(a8*a409); + a407=(a8*a407); + a409=(a409+a407); + a345=(a8*a345); + a409=(a409+a345); + a362=(a8*a362); + a409=(a409+a362); + a486=(a8*a486); + a409=(a409+a486); + if (res[0]!=0) res[0][373]=a409; + a411=(a8*a411); + a489=(a8*a489); + a411=(a411+a489); + a393=(a8*a393); + a411=(a411+a393); + a443=(a8*a443); + a411=(a411+a443); + a465=(a8*a465); + a411=(a411+a465); + if (res[0]!=0) res[0][374]=a411; + a549=(a4*a549); + a411=(a5*a549); + a465=(a10*a549); + a508=(a38*a508); + a465=(a465+a508); + a508=(a5*a465); + a411=(a411+a508); + a508=(a10*a549); + a577=(a44*a577); + a508=(a508+a577); + a577=(a5*a508); + a411=(a411+a577); + a556=(a44*a556); + a556=(a549+a556); + a577=(a5*a556); + a411=(a411+a577); + a577=(a5*a411); + a583=(a4*a583); + a443=(a5*a583); + a577=(a577+a443); + a443=(a10*a583); + a591=(a38*a591); + a443=(a443+a591); + a591=(a5*a443); + a577=(a577+a591); + a591=(a10*a583); + a542=(a44*a542); + a591=(a591+a542); + a542=(a5*a591); + a577=(a577+a542); + a561=(a44*a561); + a561=(a583+a561); + a542=(a5*a561); + a577=(a577+a542); + a577=(a5*a577); + a570=(a4*a570); + a542=(a5*a570); + a577=(a577+a542); + a542=(a10*a570); + a573=(a38*a573); + a542=(a542+a573); + a542=(a5*a542); + a577=(a577+a542); + a542=(a10*a570); + a571=(a44*a571); + a542=(a542+a571); + a542=(a5*a542); + a577=(a577+a542); + a568=(a44*a568); + a570=(a570+a568); + a570=(a5*a570); + a577=(a577+a570); + a570=(a5*a577); + a541=(a4*a541); + a568=(a5*a541); + a570=(a570+a568); + a568=(a10*a541); + a567=(a38*a567); + a568=(a568+a567); + a567=(a5*a568); + a570=(a570+a567); + a567=(a10*a541); + a572=(a44*a572); + a567=(a567+a572); + a572=(a5*a567); + a570=(a570+a572); + a578=(a44*a578); + a578=(a541+a578); + a572=(a5*a578); + a570=(a570+a572); + a570=(a8*a570); + a572=(a50*a34); + a570=(a570-a572); + a542=(a50*a34); + a570=(a570-a542); + if (res[0]!=0) res[0][375]=a570; + a510=(a28*a510); + a543=(a543*a510); + a496=(a42*a496); + a575=(a575*a496); + a543=(a543+a575); + a526=(a42*a526); + a581=(a581*a526); + a543=(a543+a581); + a555=(a28*a555); + a566=(a566*a555); + a543=(a543-a566); + a540=(a42*a540); + a569=(a569*a540); + a543=(a543-a569); + a448=(a42*a448); + a574=(a574*a448); + a543=(a543-a574); + a543=(-a543); + if (res[0]!=0) res[0][376]=a543; + a577=(a8*a577); + a541=(a8*a541); + a577=(a577+a541); + a568=(a8*a568); + a577=(a577+a568); + a567=(a8*a567); + a577=(a577+a567); + a578=(a8*a578); + a577=(a577+a578); + a578=(a24*a34); + a577=(a577-a578); + a578=(a50*a34); + a577=(a577+a578); + a577=(a577+a572); + a577=(a577+a542); + a542=(a10*a34); + a577=(a577+a542); + if (res[0]!=0) res[0][377]=a577; + a577=cos(a603); + a542=(a615*a577); + a572=cos(a539); + a578=(a625*a572); + a542=(a542+a578); + a578=cos(a513); + a567=(a631*a578); + a542=(a542+a567); + a567=cos(a554); + a567=(a615*a567); + a542=(a542+a567); + a603=sin(a603); + a567=(a616*a603); + a542=(a542-a567); + a539=sin(a539); + a567=(a619*a539); + a542=(a542-a567); + a513=sin(a513); + a567=(a624*a513); + a542=(a542-a567); + a554=sin(a554); + a554=(a616*a554); + a542=(a542-a554); + a542=(-a542); + if (res[0]!=0) res[0][378]=a542; + a542=(a778*a577); + a542=(a615*a542); + a554=(a785*a572); + a554=(a625*a554); + a542=(a542+a554); + a554=(a785*a578); + a554=(a631*a554); + a542=(a542+a554); + a554=(a778*a603); + a554=(a616*a554); + a542=(a542-a554); + a554=(a785*a539); + a554=(a619*a554); + a542=(a542-a554); + a554=(a785*a513); + a554=(a624*a554); + a542=(a542-a554); + a542=(-a542); + if (res[0]!=0) res[0][379]=a542; + a542=(a777*a577); + a542=(a615*a542); + a554=(a779*a572); + a554=(a625*a554); + a542=(a542+a554); + a554=(a779*a578); + a554=(a631*a554); + a542=(a542+a554); + a554=(a777*a603); + a554=(a616*a554); + a542=(a542-a554); + a554=(a779*a539); + a554=(a619*a554); + a542=(a542-a554); + a554=(a779*a513); + a554=(a624*a554); + a542=(a542-a554); + a542=(-a542); + if (res[0]!=0) res[0][380]=a542; + a542=(a765*a577); + a542=(a615*a542); + a554=(a772*a572); + a554=(a625*a554); + a542=(a542+a554); + a554=(a772*a578); + a554=(a631*a554); + a542=(a542+a554); + a554=(a765*a603); + a554=(a616*a554); + a542=(a542-a554); + a554=(a772*a539); + a554=(a619*a554); + a542=(a542-a554); + a554=(a772*a513); + a554=(a624*a554); + a542=(a542-a554); + a542=(-a542); + if (res[0]!=0) res[0][381]=a542; + a542=(a764*a577); + a542=(a615*a542); + a554=(a766*a572); + a554=(a625*a554); + a542=(a542+a554); + a554=(a766*a578); + a554=(a631*a554); + a542=(a542+a554); + a554=(a764*a603); + a554=(a616*a554); + a542=(a542-a554); + a554=(a766*a539); + a554=(a619*a554); + a542=(a542-a554); + a554=(a766*a513); + a554=(a624*a554); + a542=(a542-a554); + a542=(-a542); + if (res[0]!=0) res[0][382]=a542; + a542=(a703*a577); + a542=(a615*a542); + a554=(a747*a572); + a554=(a625*a554); + a542=(a542+a554); + a554=(a747*a578); + a554=(a631*a554); + a542=(a542+a554); + a554=(a703*a603); + a554=(a616*a554); + a542=(a542-a554); + a554=(a747*a539); + a554=(a619*a554); + a542=(a542-a554); + a554=(a747*a513); + a554=(a624*a554); + a542=(a542-a554); + a542=(-a542); + if (res[0]!=0) res[0][383]=a542; + a542=(a760*a577); + a542=(a615*a542); + a554=(a750*a572); + a554=(a625*a554); + a542=(a542+a554); + a554=(a750*a578); + a554=(a631*a554); + a542=(a542+a554); + a554=(a760*a603); + a554=(a616*a554); + a542=(a542-a554); + a554=(a750*a539); + a554=(a619*a554); + a542=(a542-a554); + a554=(a750*a513); + a554=(a624*a554); + a542=(a542-a554); + a542=(-a542); + if (res[0]!=0) res[0][384]=a542; + a542=(a23*a577); + a542=(a615*a542); + a554=(a759*a572); + a554=(a625*a554); + a542=(a542+a554); + a554=(a759*a578); + a554=(a631*a554); + a542=(a542+a554); + a554=(a23*a603); + a554=(a616*a554); + a542=(a542-a554); + a554=(a759*a539); + a554=(a619*a554); + a542=(a542-a554); + a554=(a759*a513); + a554=(a624*a554); + a542=(a542-a554); + a542=(-a542); + if (res[0]!=0) res[0][385]=a542; + a542=(a18*a577); + a542=(a615*a542); + a554=(a748*a572); + a554=(a625*a554); + a542=(a542+a554); + a554=(a748*a578); + a554=(a631*a554); + a542=(a542+a554); + a554=(a18*a603); + a554=(a616*a554); + a542=(a542-a554); + a554=(a748*a539); + a554=(a619*a554); + a542=(a542-a554); + a554=(a748*a513); + a554=(a624*a554); + a542=(a542-a554); + a542=(-a542); + if (res[0]!=0) res[0][386]=a542; + a542=(a15*a577); + a542=(a615*a542); + a554=(a21*a572); + a554=(a625*a554); + a542=(a542+a554); + a554=(a21*a578); + a554=(a631*a554); + a542=(a542+a554); + a554=(a15*a603); + a554=(a616*a554); + a542=(a542-a554); + a554=(a21*a539); + a554=(a619*a554); + a542=(a542-a554); + a554=(a21*a513); + a554=(a624*a554); + a542=(a542-a554); + a542=(-a542); + if (res[0]!=0) res[0][387]=a542; + a542=(a2*a577); + a542=(a615*a542); + a554=(a29*a572); + a554=(a625*a554); + a542=(a542+a554); + a554=(a29*a578); + a554=(a631*a554); + a542=(a542+a554); + a554=(a2*a603); + a554=(a616*a554); + a542=(a542-a554); + a554=(a29*a539); + a554=(a619*a554); + a542=(a542-a554); + a554=(a29*a513); + a554=(a624*a554); + a542=(a542-a554); + a542=(-a542); + if (res[0]!=0) res[0][388]=a542; + a542=(a51*a577); + a542=(a615*a542); + a554=(a32*a572); + a554=(a625*a554); + a542=(a542+a554); + a554=(a32*a578); + a554=(a631*a554); + a542=(a542+a554); + a554=(a51*a603); + a554=(a616*a554); + a542=(a542-a554); + a554=(a32*a539); + a554=(a619*a554); + a542=(a542-a554); + a554=(a32*a513); + a554=(a624*a554); + a542=(a542-a554); + a542=(-a542); + if (res[0]!=0) res[0][389]=a542; + a521=(a521-a522); + if (res[0]!=0) res[0][390]=a521; + a521=(a41*a577); + a521=(a615*a521); + a522=(a35*a572); + a522=(a625*a522); + a521=(a521+a522); + a522=(a35*a578); + a522=(a631*a522); + a521=(a521+a522); + a522=(a41*a603); + a522=(a616*a522); + a521=(a521-a522); + a522=(a35*a539); + a522=(a619*a522); + a521=(a521-a522); + a522=(a35*a513); + a522=(a624*a522); + a521=(a521-a522); + a521=(-a521); + if (res[0]!=0) res[0][391]=a521; + a521=(a10*a34); + a522=(a3*a34); + a542=(a10*a34); + a522=(a522-a542); + a521=(a521-a522); + a522=(a10*a34); + a521=(a521+a522); + a522=(a10*a34); + a521=(a521+a522); + if (res[0]!=0) res[0][392]=a521; + a611=(a28*a611); + a589=(a589*a611); + a587=(a28*a587); + a611=(a593*a587); + a609=(a28*a609); + a521=(a594*a609); + a611=(a611+a521); + a521=(a611/a606); + a596=(a596*a611); + a611=(a595*a587); + a522=(a599*a609); + a611=(a611-a522); + a600=(a600*a611); + a596=(a596+a600); + a586=(a586*a596); + a521=(a521-a586); + a521=(a588*a521); + a599=(a599*a521); + a611=(a611/a606); + a605=(a605*a596); + a611=(a611-a605); + a588=(a588*a611); + a594=(a594*a588); + a599=(a599+a594); + a610=(a610*a599); + a589=(a589+a610); + a593=(a593*a588); + a595=(a595*a521); + a593=(a593-a595); + a613=(a613*a593); + a553=(a28*a553); + a614=(a614*a553); + a613=(a613-a614); + a589=(a589+a613); + a618=(a28*a618); + a617=(a617*a618); + a621=(a28*a621); + a620=(a620*a621); + a617=(a617+a620); + a620=(a589+a617); + a623=(a42*a623); + a622=(a622*a623); + a627=(a42*a627); + a626=(a626*a627); + a622=(a622+a626); + a620=(a620+a622); + a629=(a42*a629); + a628=(a628*a629); + a633=(a42*a633); + a632=(a632*a633); + a628=(a628+a632); + a620=(a620+a628); + if (res[0]!=0) res[0][393]=a620; + a620=(a41*a661); + a620=(a639*a620); + a632=(a41*a637); + a633=(a612*a632); + a629=(a41*a659); + a626=(a644*a629); + a633=(a633+a626); + a626=(a633/a656); + a633=(a646*a633); + a632=(a645*a632); + a629=(a649*a629); + a632=(a632-a629); + a629=(a650*a632); + a633=(a633+a629); + a629=(a636*a633); + a626=(a626-a629); + a626=(a638*a626); + a629=(a649*a626); + a632=(a632/a656); + a633=(a655*a633); + a632=(a632-a633); + a632=(a638*a632); + a633=(a644*a632); + a629=(a629+a633); + a629=(a660*a629); + a620=(a620+a629); + a632=(a612*a632); + a626=(a645*a626); + a632=(a632-a626); + a632=(a663*a632); + a626=(a41*a607); + a626=(a658*a626); + a632=(a632-a626); + a620=(a620+a632); + a632=(a41*a666); + a632=(a665*a632); + a626=(a41*a669); + a626=(a668*a626); + a632=(a632+a626); + a626=(a620+a632); + a629=(a35*a671); + a629=(a670*a629); + a633=(a35*a675); + a633=(a674*a633); + a629=(a629+a633); + a626=(a626+a629); + a633=(a35*a677); + a633=(a676*a633); + a627=(a35*a681); + a627=(a680*a627); + a633=(a633+a627); + a626=(a626+a633); + a627=(a604*a587); + a623=(a598*a521); + a627=(a627+a623); + a643=(a643*a627); + a626=(a626-a643); + a521=(a590*a521); + a604=(a604*a609); + a521=(a521-a604); + a601=(a601*a521); + a626=(a626+a601); + a590=(a590*a588); + a609=(a597*a609); + a590=(a590-a609); + a559=(a559*a590); + a626=(a626-a559); + a597=(a597*a587); + a598=(a598*a588); + a597=(a597+a598); + a608=(a608*a597); + a626=(a626-a608); + if (res[0]!=0) res[0][394]=a626; + a707=(a4*a707); + a626=(a5*a707); + a608=(a10*a707); + a715=(a38*a715); + a608=(a608+a715); + a608=(a5*a608); + a626=(a626+a608); + a608=(a10*a707); + a721=(a44*a721); + a608=(a608+a721); + a608=(a5*a608); + a626=(a626+a608); + a728=(a44*a728); + a707=(a707+a728); + a707=(a5*a707); + a626=(a626+a707); + a707=(a5*a626); + a698=(a4*a698); + a728=(a5*a698); + a707=(a707+a728); + a728=(a10*a698); + a608=(a5*a728); + a707=(a707+a608); + a608=(a10*a698); + a721=(a5*a608); + a707=(a707+a721); + a721=(a5*a698); + a707=(a707+a721); + a707=(a8*a707); + if (res[0]!=0) res[0][395]=a707; + a707=(a51*a706); + a721=(a688*a707); + a715=(a51*a687); + a597=(a692*a715); + a721=(a721+a597); + a597=(a721/a686); + a721=(a684*a721); + a598=(a696*a707); + a588=(a689*a715); + a598=(a598-a588); + a588=(a702*a598); + a721=(a721+a588); + a588=(a691*a721); + a597=(a597-a588); + a597=(a662*a597); + a588=(a690*a597); + a587=(a678*a715); + a588=(a588-a587); + a588=(a711*a588); + a587=(a678*a707); + a559=(a657*a597); + a587=(a587+a559); + a587=(a751*a587); + a588=(a588-a587); + a598=(a598/a686); + a721=(a685*a721); + a598=(a598-a721); + a598=(a662*a598); + a721=(a690*a598); + a715=(a701*a715); + a721=(a721-a715); + a721=(a742*a721); + a588=(a588-a721); + a707=(a701*a707); + a721=(a657*a598); + a707=(a707+a721); + a707=(a705*a707); + a588=(a588-a707); + if (res[0]!=0) res[0][396]=a588; + a798=(a8*a798); + a757=(a8*a757); + a798=(a798+a757); + a40=(a8*a40); + a798=(a798+a40); + a784=(a8*a784); + a798=(a798+a784); + a787=(a8*a787); + a798=(a798+a787); + if (res[0]!=0) res[0][397]=a798; + a813=(a8*a813); + a0=(a8*a0); + a813=(a813+a0); + a83=(a8*a83); + a813=(a813+a83); + a804=(a8*a804); + a813=(a813+a804); + a806=(a8*a806); + a813=(a813+a806); + if (res[0]!=0) res[0][398]=a813; + a822=(a8*a822); + a108=(a8*a108); + a822=(a822+a108); + a22=(a8*a22); + a822=(a822+a22); + a97=(a8*a97); + a822=(a822+a97); + a1=(a8*a1); + a822=(a822+a1); + if (res[0]!=0) res[0][399]=a822; + a113=(a8*a113); + a179=(a8*a179); + a113=(a113+a179); + a818=(a8*a818); + a113=(a113+a818); + a144=(a8*a144); + a113=(a113+a144); + a139=(a8*a139); + a113=(a113+a139); + if (res[0]!=0) res[0][400]=a113; + a142=(a8*a142); + a213=(a8*a213); + a142=(a142+a213); + a92=(a8*a92); + a142=(a142+a92); + a192=(a8*a192); + a142=(a142+a192); + a187=(a8*a187); + a142=(a142+a187); + if (res[0]!=0) res[0][401]=a142; + a230=(a8*a230); + a277=(a8*a277); + a230=(a230+a277); + a738=(a8*a738); + a230=(a230+a738); + a264=(a8*a264); + a230=(a230+a264); + a208=(a8*a208); + a230=(a230+a208); + if (res[0]!=0) res[0][402]=a230; + a160=(a8*a160); + a394=(a8*a394); + a160=(a160+a394); + a76=(a8*a76); + a160=(a160+a76); + a319=(a8*a319); + a160=(a160+a319); + a320=(a8*a320); + a160=(a160+a320); + if (res[0]!=0) res[0][403]=a160; + a355=(a8*a355); + a346=(a8*a346); + a355=(a355+a346); + a120=(a8*a120); + a355=(a355+a120); + a370=(a8*a370); + a355=(a355+a370); + a354=(a8*a354); + a355=(a355+a354); + if (res[0]!=0) res[0][404]=a355; + a406=(a8*a406); + a435=(a8*a435); + a406=(a406+a435); + a89=(a8*a89); + a406=(a406+a89); + a418=(a8*a418); + a406=(a406+a418); + a372=(a8*a372); + a406=(a406+a372); + if (res[0]!=0) res[0][405]=a406; + a455=(a8*a455); + a453=(a8*a453); + a455=(a455+a453); + a294=(a8*a294); + a455=(a455+a294); + a377=(a8*a377); + a455=(a455+a377); + a499=(a8*a499); + a455=(a455+a499); + if (res[0]!=0) res[0][406]=a455; + a493=(a8*a493); + a537=(a8*a537); + a493=(a493+a537); + a263=(a8*a263); + a493=(a493+a263); + a564=(a8*a564); + a493=(a493+a564); + a515=(a8*a515); + a493=(a493+a515); + if (res[0]!=0) res[0][407]=a493; + a493=(a51*a693); + a493=(a700*a493); + a515=(a689*a597); + a564=(a692*a598); + a515=(a515+a564); + a515=(a697*a515); + a493=(a493+a515); + a598=(a688*a598); + a597=(a696*a597); + a598=(a598-a597); + a598=(a708*a598); + a597=(a51*a699); + a597=(a709*a597); + a598=(a598-a597); + a493=(a493+a598); + a493=(a4*a493); + a598=(a5*a493); + a597=(a10*a493); + a515=(a51*a714); + a515=(a713*a515); + a564=(a51*a717); + a564=(a716*a564); + a515=(a515+a564); + a515=(a38*a515); + a597=(a597+a515); + a515=(a5*a597); + a598=(a598+a515); + a515=(a10*a493); + a564=(a32*a720); + a564=(a719*a564); + a263=(a32*a724); + a263=(a723*a263); + a564=(a564+a263); + a564=(a44*a564); + a515=(a515+a564); + a564=(a5*a515); + a598=(a598+a564); + a564=(a32*a727); + a564=(a726*a564); + a263=(a32*a731); + a263=(a730*a263); + a564=(a564+a263); + a564=(a44*a564); + a564=(a493+a564); + a263=(a5*a564); + a598=(a598+a263); + a263=(a5*a598); + a620=(a4*a620); + a537=(a5*a620); + a263=(a263+a537); + a537=(a10*a620); + a632=(a38*a632); + a537=(a537+a632); + a632=(a5*a537); + a263=(a263+a632); + a632=(a10*a620); + a629=(a44*a629); + a632=(a632+a629); + a629=(a5*a632); + a263=(a263+a629); + a633=(a44*a633); + a633=(a620+a633); + a629=(a5*a633); + a263=(a263+a629); + a629=(a5*a263); + a589=(a4*a589); + a455=(a5*a589); + a629=(a629+a455); + a455=(a10*a589); + a617=(a38*a617); + a455=(a455+a617); + a617=(a5*a455); + a629=(a629+a617); + a617=(a10*a589); + a622=(a44*a622); + a617=(a617+a622); + a622=(a5*a617); + a629=(a629+a622); + a628=(a44*a628); + a628=(a589+a628); + a622=(a5*a628); + a629=(a629+a622); + a629=(a8*a629); + a622=(a50*a34); + a629=(a629-a622); + a499=(a50*a34); + a629=(a629-a499); + if (res[0]!=0) res[0][408]=a629; + a577=(a28*a577); + a615=(a615*a577); + a572=(a42*a572); + a625=(a625*a572); + a615=(a615+a625); + a578=(a42*a578); + a631=(a631*a578); + a615=(a615+a631); + a603=(a28*a603); + a616=(a616*a603); + a615=(a615-a616); + a539=(a42*a539); + a619=(a619*a539); + a615=(a615-a619); + a513=(a42*a513); + a624=(a624*a513); + a615=(a615-a624); + a615=(-a615); + if (res[0]!=0) res[0][409]=a615; + a263=(a8*a263); + a589=(a8*a589); + a263=(a263+a589); + a455=(a8*a455); + a263=(a263+a455); + a617=(a8*a617); + a263=(a263+a617); + a628=(a8*a628); + a263=(a263+a628); + a628=(a24*a34); + a263=(a263-a628); + a617=(a50*a34); + a263=(a263+a617); + a263=(a263+a622); + a263=(a263+a499); + a499=(a10*a34); + a263=(a263+a499); + if (res[0]!=0) res[0][410]=a263; + a263=sin(a653); + a263=(a263*a664); + a653=cos(a653); + a653=(a653*a641); + a263=(a263-a653); + a653=cos(a602); + a653=(a653*a641); + a653=(a263-a653); + a641=sin(a642); + a641=(a641*a667); + a642=cos(a642); + a642=(a642*a673); + a641=(a641-a642); + a653=(a653+a641); + a642=sin(a544); + a642=(a642*a672); + a544=cos(a544); + a544=(a544*a679); + a642=(a642-a544); + a653=(a653+a642); + a602=sin(a602); + a602=(a602*a664); + a653=(a653+a602); + if (res[0]!=0) res[0][411]=a653; + a263=(a38*a263); + a653=(a5*a263); + a641=(a44*a641); + a602=(a5*a641); + a653=(a653+a602); + a642=(a44*a642); + a602=(a5*a642); + a653=(a653+a602); + a602=(a5*a653); + a664=(a5*a602); + a544=(a5*a664); + a679=(a5*a544); + a672=(a5*a679); + a673=(a5*a672); + a667=(a5*a673); + a499=(a5*a667); + a622=(a5*a499); + a455=(a5*a622); + a589=(a5*a455); + a615=(a5*a589); + a615=(a8*a615); + if (res[0]!=0) res[0][412]=a615; + a589=(a8*a589); + if (res[0]!=0) res[0][413]=a589; + a455=(a8*a455); + if (res[0]!=0) res[0][414]=a455; + a622=(a8*a622); + if (res[0]!=0) res[0][415]=a622; + a499=(a8*a499); + if (res[0]!=0) res[0][416]=a499; + a667=(a8*a667); + if (res[0]!=0) res[0][417]=a667; + a673=(a8*a673); + if (res[0]!=0) res[0][418]=a673; + a672=(a8*a672); + if (res[0]!=0) res[0][419]=a672; + a679=(a8*a679); + if (res[0]!=0) res[0][420]=a679; + a544=(a8*a544); + if (res[0]!=0) res[0][421]=a544; + a664=(a8*a664); + if (res[0]!=0) res[0][422]=a664; + a602=(a8*a602); + if (res[0]!=0) res[0][423]=a602; + a602=(a10*a34); + a664=(a10*a34); + a544=(a602+a664); + a544=(-a544); + if (res[0]!=0) res[0][424]=a544; + a653=(a8*a653); + if (res[0]!=0) res[0][425]=a653; + a3=(a3*a34); + a653=(a10*a34); + a544=(a3-a653); + a602=(a602-a544); + a602=(a602+a664); + a664=(a10*a34); + a602=(a602+a664); + if (res[0]!=0) res[0][426]=a602; + a661=(a28*a661); + a639=(a639*a661); + a637=(a28*a637); + a661=(a612*a637); + a659=(a28*a659); + a602=(a644*a659); + a661=(a661+a602); + a602=(a661/a656); + a646=(a646*a661); + a637=(a645*a637); + a659=(a649*a659); + a637=(a637-a659); + a650=(a650*a637); + a646=(a646+a650); + a636=(a636*a646); + a602=(a602-a636); + a602=(a638*a602); + a649=(a649*a602); + a637=(a637/a656); + a655=(a655*a646); + a637=(a637-a655); + a638=(a638*a637); + a644=(a644*a638); + a649=(a649+a644); + a660=(a660*a649); + a639=(a639+a660); + a612=(a612*a638); + a645=(a645*a602); + a612=(a612-a645); + a663=(a663*a612); + a607=(a28*a607); + a658=(a658*a607); + a663=(a663-a658); + a639=(a639+a663); + a666=(a28*a666); + a665=(a665*a666); + a669=(a28*a669); + a668=(a668*a669); + a665=(a665+a668); + a668=(a639+a665); + a671=(a42*a671); + a670=(a670*a671); + a675=(a42*a675); + a674=(a674*a675); + a670=(a670+a674); + a668=(a668+a670); + a677=(a42*a677); + a676=(a676*a677); + a681=(a42*a681); + a680=(a680*a681); + a676=(a676+a680); + a668=(a668+a676); + if (res[0]!=0) res[0][427]=a668; + a626=(a8*a626); + a668=(a8*a698); + a626=(a626+a668); + a728=(a8*a728); + a626=(a626+a728); + a608=(a8*a608); + a626=(a626+a608); + a698=(a8*a698); + a626=(a626+a698); + if (res[0]!=0) res[0][428]=a626; + a626=(a41*a706); + a698=(a688*a626); + a608=(a41*a687); + a728=(a692*a608); + a698=(a698+a728); + a728=(a698/a686); + a698=(a684*a698); + a668=(a696*a626); + a680=(a689*a608); + a668=(a668-a680); + a680=(a702*a668); + a698=(a698+a680); + a680=(a691*a698); + a728=(a728-a680); + a728=(a662*a728); + a680=(a690*a728); + a681=(a678*a608); + a680=(a680-a681); + a680=(a711*a680); + a681=(a678*a626); + a677=(a657*a728); + a681=(a681+a677); + a681=(a751*a681); + a680=(a680-a681); + a668=(a668/a686); + a698=(a685*a698); + a668=(a668-a698); + a668=(a662*a668); + a698=(a690*a668); + a608=(a701*a608); + a698=(a698-a608); + a698=(a742*a698); + a680=(a680-a698); + a626=(a701*a626); + a698=(a657*a668); + a626=(a626+a698); + a626=(a705*a626); + a680=(a680-a626); + if (res[0]!=0) res[0][429]=a680; + a31=(a8*a31); + a763=(a8*a763); + a31=(a31+a763); + a7=(a8*a7); + a31=(a31+a7); + a790=(a8*a790); + a31=(a31+a790); + a793=(a8*a793); + a31=(a31+a793); + if (res[0]!=0) res[0][430]=a31; + a803=(a8*a803); + a39=(a8*a39); + a803=(a803+a39); + a811=(a8*a811); + a803=(a803+a811); + a808=(a8*a808); + a803=(a803+a808); + a810=(a8*a810); + a803=(a803+a810); + if (res[0]!=0) res[0][431]=a803; + a19=(a8*a19); + a87=(a8*a87); + a19=(a19+a87); + a17=(a8*a17); + a19=(a19+a17); + a93=(a8*a93); + a19=(a19+a93); + a819=(a8*a819); + a19=(a19+a819); + if (res[0]!=0) res[0][432]=a19; + a52=(a8*a52); + a134=(a8*a134); + a52=(a52+a134); + a62=(a8*a62); + a52=(a52+a62); + a136=(a8*a136); + a52=(a52+a136); + a151=(a8*a151); + a52=(a52+a151); + if (res[0]!=0) res[0][433]=a52; + a60=(a8*a60); + a229=(a8*a229); + a60=(a60+a229); + a159=(a8*a159); + a60=(a60+a159); + a184=(a8*a184); + a60=(a60+a184); + a199=(a8*a199); + a60=(a60+a199); + if (res[0]!=0) res[0][434]=a60; + a123=(a8*a123); + a259=(a8*a259); + a123=(a123+a259); + a155=(a8*a155); + a123=(a123+a155); + a233=(a8*a233); + a123=(a123+a233); + a207=(a8*a207); + a123=(a123+a207); + if (res[0]!=0) res[0][435]=a123; + a175=(a8*a175); + a327=(a8*a327); + a175=(a175+a327); + a255=(a8*a255); + a175=(a175+a255); + a314=(a8*a314); + a175=(a175+a314); + a253=(a8*a253); + a175=(a175+a253); + if (res[0]!=0) res[0][436]=a175; + a219=(a8*a219); + a361=(a8*a361); + a219=(a219+a361); + a251=(a8*a251); + a219=(a219+a251); + a365=(a8*a365); + a219=(a219+a365); + a366=(a8*a366); + a219=(a219+a366); + if (res[0]!=0) res[0][437]=a219; + a271=(a8*a271); + a397=(a8*a397); + a271=(a271+a397); + a351=(a8*a351); + a271=(a271+a351); + a420=(a8*a420); + a271=(a271+a420); + a404=(a8*a404); + a271=(a271+a404); + if (res[0]!=0) res[0][438]=a271; + a315=(a8*a315); + a483=(a8*a483); + a315=(a315+a483); + a347=(a8*a347); + a315=(a315+a347); + a470=(a8*a470); + a315=(a315+a470); + a449=(a8*a449); + a315=(a315+a449); + if (res[0]!=0) res[0][439]=a315; + a367=(a8*a367); + a503=(a8*a503); + a367=(a367+a503); + a447=(a8*a447); + a367=(a367+a447); + a458=(a8*a458); + a367=(a367+a458); + a580=(a8*a580); + a367=(a367+a580); + if (res[0]!=0) res[0][440]=a367; + a411=(a8*a411); + a583=(a8*a583); + a411=(a411+a583); + a443=(a8*a443); + a411=(a411+a443); + a591=(a8*a591); + a411=(a411+a591); + a561=(a8*a561); + a411=(a411+a561); + if (res[0]!=0) res[0][441]=a411; + a598=(a8*a598); + a620=(a8*a620); + a598=(a598+a620); + a537=(a8*a537); + a598=(a598+a537); + a632=(a8*a632); + a598=(a598+a632); + a633=(a8*a633); + a598=(a598+a633); + a598=(a598+a628); + a598=(a598-a617); + if (res[0]!=0) res[0][442]=a598; + a263=(a8*a263); + a641=(a8*a641); + a263=(a263+a641); + a642=(a8*a642); + a263=(a263+a642); + if (res[0]!=0) res[0][443]=a263; + a263=(a41*a693); + a263=(a700*a263); + a642=(a689*a728); + a641=(a692*a668); + a642=(a642+a641); + a642=(a697*a642); + a263=(a263+a642); + a668=(a688*a668); + a728=(a696*a728); + a668=(a668-a728); + a668=(a708*a668); + a728=(a41*a699); + a728=(a709*a728); + a668=(a668-a728); + a263=(a263+a668); + a263=(a4*a263); + a668=(a5*a263); + a728=(a10*a263); + a642=(a41*a714); + a642=(a713*a642); + a641=(a41*a717); + a641=(a716*a641); + a642=(a642+a641); + a642=(a38*a642); + a728=(a728+a642); + a728=(a5*a728); + a668=(a668+a728); + a728=(a10*a263); + a642=(a35*a720); + a642=(a719*a642); + a641=(a35*a724); + a641=(a723*a641); + a642=(a642+a641); + a642=(a44*a642); + a728=(a728+a642); + a728=(a5*a728); + a668=(a668+a728); + a728=(a35*a727); + a728=(a726*a728); + a642=(a35*a731); + a642=(a730*a642); + a728=(a728+a642); + a728=(a44*a728); + a263=(a263+a728); + a263=(a5*a263); + a668=(a668+a263); + a668=(a8*a668); + a639=(a4*a639); + a263=(a8*a639); + a668=(a668+a263); + a263=(a10*a639); + a665=(a38*a665); + a263=(a263+a665); + a263=(a8*a263); + a668=(a668+a263); + a263=(a10*a639); + a670=(a44*a670); + a263=(a263+a670); + a263=(a8*a263); + a668=(a668+a263); + a676=(a44*a676); + a639=(a639+a676); + a639=(a8*a639); + a668=(a668+a639); + a24=(a24*a34); + a668=(a668-a24); + a24=(a50*a34); + a668=(a668+a24); + a24=(a50*a34); + a668=(a668+a24); + a24=(a50*a34); + a668=(a668+a24); + a24=(a10*a34); + a668=(a668+a24); + if (res[0]!=0) res[0][444]=a668; + a668=cos(a694); + a24=(a710*a668); + a639=cos(a651); + a676=(a722*a639); + a24=(a24+a676); + a676=cos(a592); + a263=(a729*a676); + a24=(a24+a263); + a263=cos(a652); + a263=(a710*a263); + a24=(a24+a263); + a694=sin(a694); + a263=(a712*a694); + a24=(a24-a263); + a651=sin(a651); + a263=(a718*a651); + a24=(a24-a263); + a592=sin(a592); + a263=(a725*a592); + a24=(a24-a263); + a652=sin(a652); + a652=(a712*a652); + a24=(a24-a652); + a24=(-a24); + if (res[0]!=0) res[0][445]=a24; + a24=(a796*a668); + a24=(a710*a24); + a652=(a797*a639); + a652=(a722*a652); + a24=(a24+a652); + a652=(a797*a676); + a652=(a729*a652); + a24=(a24+a652); + a796=(a796*a694); + a796=(a712*a796); + a24=(a24-a796); + a796=(a797*a651); + a796=(a718*a796); + a24=(a24-a796); + a797=(a797*a592); + a797=(a725*a797); + a24=(a24-a797); + a24=(-a24); + if (res[0]!=0) res[0][446]=a24; + a24=(a769*a668); + a24=(a710*a24); + a797=(a791*a639); + a797=(a722*a797); + a24=(a24+a797); + a797=(a791*a676); + a797=(a729*a797); + a24=(a24+a797); + a769=(a769*a694); + a769=(a712*a769); + a24=(a24-a769); + a769=(a791*a651); + a769=(a718*a769); + a24=(a24-a769); + a791=(a791*a592); + a791=(a725*a791); + a24=(a24-a791); + a24=(-a24); + if (res[0]!=0) res[0][447]=a24; + a24=(a778*a668); + a24=(a710*a24); + a791=(a785*a639); + a791=(a722*a791); + a24=(a24+a791); + a791=(a785*a676); + a791=(a729*a791); + a24=(a24+a791); + a778=(a778*a694); + a778=(a712*a778); + a24=(a24-a778); + a778=(a785*a651); + a778=(a718*a778); + a24=(a24-a778); + a785=(a785*a592); + a785=(a725*a785); + a24=(a24-a785); + a24=(-a24); + if (res[0]!=0) res[0][448]=a24; + a24=(a777*a668); + a24=(a710*a24); + a785=(a779*a639); + a785=(a722*a785); + a24=(a24+a785); + a785=(a779*a676); + a785=(a729*a785); + a24=(a24+a785); + a777=(a777*a694); + a777=(a712*a777); + a24=(a24-a777); + a777=(a779*a651); + a777=(a718*a777); + a24=(a24-a777); + a779=(a779*a592); + a779=(a725*a779); + a24=(a24-a779); + a24=(-a24); + if (res[0]!=0) res[0][449]=a24; + a24=(a765*a668); + a24=(a710*a24); + a779=(a772*a639); + a779=(a722*a779); + a24=(a24+a779); + a779=(a772*a676); + a779=(a729*a779); + a24=(a24+a779); + a765=(a765*a694); + a765=(a712*a765); + a24=(a24-a765); + a765=(a772*a651); + a765=(a718*a765); + a24=(a24-a765); + a772=(a772*a592); + a772=(a725*a772); + a24=(a24-a772); + a24=(-a24); + if (res[0]!=0) res[0][450]=a24; + a24=(a764*a668); + a24=(a710*a24); + a772=(a766*a639); + a772=(a722*a772); + a24=(a24+a772); + a772=(a766*a676); + a772=(a729*a772); + a24=(a24+a772); + a764=(a764*a694); + a764=(a712*a764); + a24=(a24-a764); + a764=(a766*a651); + a764=(a718*a764); + a24=(a24-a764); + a766=(a766*a592); + a766=(a725*a766); + a24=(a24-a766); + a24=(-a24); + if (res[0]!=0) res[0][451]=a24; + a24=(a703*a668); + a24=(a710*a24); + a766=(a747*a639); + a766=(a722*a766); + a24=(a24+a766); + a766=(a747*a676); + a766=(a729*a766); + a24=(a24+a766); + a703=(a703*a694); + a703=(a712*a703); + a24=(a24-a703); + a703=(a747*a651); + a703=(a718*a703); + a24=(a24-a703); + a747=(a747*a592); + a747=(a725*a747); + a24=(a24-a747); + a24=(-a24); + if (res[0]!=0) res[0][452]=a24; + a24=(a760*a668); + a24=(a710*a24); + a747=(a750*a639); + a747=(a722*a747); + a24=(a24+a747); + a747=(a750*a676); + a747=(a729*a747); + a24=(a24+a747); + a760=(a760*a694); + a760=(a712*a760); + a24=(a24-a760); + a760=(a750*a651); + a760=(a718*a760); + a24=(a24-a760); + a750=(a750*a592); + a750=(a725*a750); + a24=(a24-a750); + a24=(-a24); + if (res[0]!=0) res[0][453]=a24; + a24=(a23*a668); + a24=(a710*a24); + a750=(a759*a639); + a750=(a722*a750); + a24=(a24+a750); + a750=(a759*a676); + a750=(a729*a750); + a24=(a24+a750); + a23=(a23*a694); + a23=(a712*a23); + a24=(a24-a23); + a23=(a759*a651); + a23=(a718*a23); + a24=(a24-a23); + a759=(a759*a592); + a759=(a725*a759); + a24=(a24-a759); + a24=(-a24); + if (res[0]!=0) res[0][454]=a24; + a24=(a18*a668); + a24=(a710*a24); + a759=(a748*a639); + a759=(a722*a759); + a24=(a24+a759); + a759=(a748*a676); + a759=(a729*a759); + a24=(a24+a759); + a18=(a18*a694); + a18=(a712*a18); + a24=(a24-a18); + a18=(a748*a651); + a18=(a718*a18); + a24=(a24-a18); + a748=(a748*a592); + a748=(a725*a748); + a24=(a24-a748); + a24=(-a24); + if (res[0]!=0) res[0][455]=a24; + a24=(a15*a668); + a24=(a710*a24); + a748=(a21*a639); + a748=(a722*a748); + a24=(a24+a748); + a748=(a21*a676); + a748=(a729*a748); + a24=(a24+a748); + a15=(a15*a694); + a15=(a712*a15); + a24=(a24-a15); + a15=(a21*a651); + a15=(a718*a15); + a24=(a24-a15); + a21=(a21*a592); + a21=(a725*a21); + a24=(a24-a21); + a24=(-a24); + if (res[0]!=0) res[0][456]=a24; + a24=(a2*a668); + a24=(a710*a24); + a21=(a29*a639); + a21=(a722*a21); + a24=(a24+a21); + a21=(a29*a676); + a21=(a729*a21); + a24=(a24+a21); + a2=(a2*a694); + a2=(a712*a2); + a24=(a24-a2); + a2=(a29*a651); + a2=(a718*a2); + a24=(a24-a2); + a29=(a29*a592); + a29=(a725*a29); + a24=(a24-a29); + a24=(-a24); + if (res[0]!=0) res[0][457]=a24; + a24=(a51*a668); + a24=(a710*a24); + a29=(a32*a639); + a29=(a722*a29); + a24=(a24+a29); + a29=(a32*a676); + a29=(a729*a29); + a24=(a24+a29); + a51=(a51*a694); + a51=(a712*a51); + a24=(a24-a51); + a51=(a32*a651); + a51=(a718*a51); + a24=(a24-a51); + a32=(a32*a592); + a32=(a725*a32); + a24=(a24-a32); + a24=(-a24); + if (res[0]!=0) res[0][458]=a24; + a3=(a3-a653); + if (res[0]!=0) res[0][459]=a3; + a3=(a41*a668); + a3=(a710*a3); + a653=(a35*a639); + a653=(a722*a653); + a3=(a3+a653); + a653=(a35*a676); + a653=(a729*a653); + a3=(a3+a653); + a41=(a41*a694); + a41=(a712*a41); + a3=(a3-a41); + a41=(a35*a651); + a41=(a718*a41); + a3=(a3-a41); + a35=(a35*a592); + a35=(a725*a35); + a3=(a3-a35); + a3=(-a3); + if (res[0]!=0) res[0][460]=a3; + a3=(a10*a34); + a35=(a10*a34); + a3=(a3+a35); + a35=(a10*a34); + a3=(a3+a35); + if (res[0]!=0) res[0][461]=a3; + a693=(a28*a693); + a700=(a700*a693); + a706=(a28*a706); + a693=(a688*a706); + a687=(a28*a687); + a3=(a692*a687); + a693=(a693+a3); + a3=(a693/a686); + a684=(a684*a693); + a693=(a696*a706); + a35=(a689*a687); + a693=(a693-a35); + a702=(a702*a693); + a684=(a684+a702); + a691=(a691*a684); + a3=(a3-a691); + a3=(a662*a3); + a689=(a689*a3); + a693=(a693/a686); + a685=(a685*a684); + a693=(a693-a685); + a662=(a662*a693); + a692=(a692*a662); + a689=(a689+a692); + a697=(a697*a689); + a700=(a700+a697); + a688=(a688*a662); + a696=(a696*a3); + a688=(a688-a696); + a708=(a708*a688); + a699=(a28*a699); + a709=(a709*a699); + a708=(a708-a709); + a700=(a700+a708); + a714=(a28*a714); + a713=(a713*a714); + a717=(a28*a717); + a716=(a716*a717); + a713=(a713+a716); + a716=(a700+a713); + a720=(a42*a720); + a719=(a719*a720); + a724=(a42*a724); + a723=(a723*a724); + a719=(a719+a723); + a716=(a716+a719); + a727=(a42*a727); + a726=(a726*a727); + a731=(a42*a731); + a730=(a730*a731); + a726=(a726+a730); + a716=(a716+a726); + if (res[0]!=0) res[0][462]=a716; + a716=(a690*a3); + a730=(a678*a687); + a716=(a716-a730); + a711=(a711*a716); + a678=(a678*a706); + a3=(a657*a3); + a678=(a678+a3); + a751=(a751*a678); + a711=(a711-a751); + a690=(a690*a662); + a687=(a701*a687); + a690=(a690-a687); + a742=(a742*a690); + a711=(a711-a742); + a701=(a701*a706); + a657=(a657*a662); + a701=(a701+a657); + a705=(a705*a701); + a711=(a711-a705); + if (res[0]!=0) res[0][463]=a711; + a770=(a8*a770); + a46=(a8*a46); + a770=(a770+a46); + a775=(a8*a775); + a770=(a770+a775); + a799=(a8*a799); + a770=(a770+a799); + if (res[0]!=0) res[0][464]=a770; + a48=(a8*a48); + a104=(a8*a104); + a48=(a48+a104); + a812=(a8*a812); + a48=(a48+a812); + a814=(a8*a814); + a48=(a48+a814); + if (res[0]!=0) res[0][465]=a48; + a58=(a8*a58); + a695=(a8*a695); + a58=(a58+a695); + a821=(a8*a821); + a58=(a58+a821); + a823=(a8*a823); + a58=(a58+a823); + if (res[0]!=0) res[0][466]=a58; + a124=(a8*a124); + a82=(a8*a82); + a124=(a124+a82); + a145=(a8*a145); + a124=(a124+a145); + a67=(a8*a67); + a124=(a124+a67); + if (res[0]!=0) res[0][467]=a124; + a204=(a8*a204); + a131=(a8*a131); + a204=(a204+a131); + a193=(a8*a193); + a204=(a204+a193); + a126=(a8*a126); + a204=(a204+a126); + if (res[0]!=0) res[0][468]=a204; + a275=(a8*a275); + a177=(a8*a177); + a275=(a275+a177); + a240=(a8*a240); + a275=(a275+a240); + a129=(a8*a129); + a275=(a275+a129); + if (res[0]!=0) res[0][469]=a275; + a309=(a8*a309); + a227=(a8*a227); + a309=(a309+a227); + a288=(a8*a288); + a309=(a309+a288); + a283=(a8*a283); + a309=(a309+a283); + if (res[0]!=0) res[0][470]=a309; + a373=(a8*a373); + a273=(a8*a273); + a373=(a373+a273); + a360=(a8*a360); + a373=(a373+a360); + a282=(a8*a282); + a373=(a373+a282); + if (res[0]!=0) res[0][471]=a373; + a490=(a8*a490); + a323=(a8*a323); + a490=(a490+a323); + a415=(a8*a415); + a490=(a490+a415); + a416=(a8*a416); + a490=(a490+a416); + if (res[0]!=0) res[0][472]=a490; + a442=(a8*a442); + a369=(a8*a369); + a442=(a442+a369); + a466=(a8*a466); + a442=(a442+a466); + a450=(a8*a450); + a442=(a442+a450); + if (res[0]!=0) res[0][473]=a442; + a531=(a8*a531); + a419=(a8*a419); + a531=(a531+a419); + a514=(a8*a514); + a531=(a531+a514); + a463=(a8*a463); + a531=(a531+a463); + if (res[0]!=0) res[0][474]=a531; + a549=(a8*a549); + a465=(a8*a465); + a549=(a549+a465); + a508=(a8*a508); + a549=(a549+a508); + a556=(a8*a556); + a549=(a549+a556); + if (res[0]!=0) res[0][475]=a549; + a493=(a8*a493); + a597=(a8*a597); + a493=(a493+a597); + a515=(a8*a515); + a493=(a493+a515); + a564=(a8*a564); + a493=(a493+a564); + if (res[0]!=0) res[0][476]=a493; + a4=(a4*a700); + a700=(a5*a4); + a493=(a10*a4); + a38=(a38*a713); + a493=(a493+a38); + a38=(a5*a493); + a700=(a700+a38); + a38=(a10*a4); + a719=(a44*a719); + a38=(a38+a719); + a719=(a5*a38); + a700=(a700+a719); + a44=(a44*a726); + a44=(a4+a44); + a5=(a5*a44); + a700=(a700+a5); + a700=(a8*a700); + a5=(a50*a34); + a700=(a700-a5); + a50=(a50*a34); + a700=(a700-a50); + if (res[0]!=0) res[0][477]=a700; + a668=(a28*a668); + a710=(a710*a668); + a639=(a42*a639); + a722=(a722*a639); + a710=(a710+a722); + a676=(a42*a676); + a729=(a729*a676); + a710=(a710+a729); + a28=(a28*a694); + a712=(a712*a28); + a710=(a710-a712); + a651=(a42*a651); + a718=(a718*a651); + a710=(a710-a718); + a42=(a42*a592); + a725=(a725*a42); + a710=(a710-a725); + a710=(-a710); + if (res[0]!=0) res[0][478]=a710; + a4=(a8*a4); + a493=(a8*a493); + a4=(a4+a493); + a38=(a8*a38); + a4=(a4+a38); + a8=(a8*a44); + a4=(a4+a8); + a4=(a4+a5); + a4=(a4+a50); + a10=(a10*a34); + a4=(a4+a10); + if (res[0]!=0) res[0][479]=a4; + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_hess_l(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f5(arg, res, iw, w, mem); +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_hess_l_alloc_mem(void) { + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_hess_l_init_mem(int mem) { + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_hess_l_free_mem(int mem) { +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_hess_l_checkout(void) { + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_hess_l_release(int mem) { +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_hess_l_incref(void) { +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_hess_l_decref(void) { +} + +extern "C" CASADI_SYMBOL_EXPORT casadi_int nlp_hess_l_n_in(void) { return 4;} + +extern "C" CASADI_SYMBOL_EXPORT casadi_int nlp_hess_l_n_out(void) { return 1;} + +extern "C" CASADI_SYMBOL_EXPORT casadi_real nlp_hess_l_default_in(casadi_int i) { + switch (i) { + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const char* nlp_hess_l_name_in(casadi_int i) { + switch (i) { + case 0: return "x"; + case 1: return "p"; + case 2: return "lam_f"; + case 3: return "lam_g"; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const char* nlp_hess_l_name_out(casadi_int i) { + switch (i) { + case 0: return "triu_hess_gamma_x_x"; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const casadi_int* nlp_hess_l_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s1; + case 2: return casadi_s2; + case 3: return casadi_s3; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const casadi_int* nlp_hess_l_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s4; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_hess_l_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 4; + if (sz_res) *sz_res = 1; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 0; + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_hess_l_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 4*sizeof(const casadi_real*); + if (sz_res) *sz_res = 1*sizeof(casadi_real*); + if (sz_iw) *sz_iw = 0*sizeof(casadi_int); + if (sz_w) *sz_w = 0*sizeof(casadi_real); + return 0; +} + +/* nlp_jac_g:(x[78],p[50])->(g[63],jac_g_x[63x78,543nz]) */ +static int casadi_f6(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem) { + casadi_real a0, a1, a10, a100, a101, a102, a103, a104, a105, a106, a107, a108, a109, a11, a110, a111, a112, a113, a114, a115, a116, a117, a118, a119, a12, a120, a121, a122, a123, a124, a125, a126, a127, a128, a129, a13, a130, a131, a132, a133, a134, a135, a136, a137, a138, a139, a14, a140, a141, a142, a143, a144, a145, a146, a147, a148, a149, a15, a150, a151, a152, a153, a154, a155, a156, a157, a158, a159, a16, a160, a161, a162, a163, a164, a165, a166, a167, a168, a169, a17, a170, a171, a172, a173, a174, a175, a176, a177, a178, a179, a18, a180, a181, a182, a183, a184, a185, a186, a187, a188, a189, a19, a190, a191, a192, a193, a194, a195, a196, a197, a198, a199, a2, a20, a200, a201, a202, a203, a204, a205, a206, a207, a208, a209, a21, a210, a211, a212, a213, a214, a215, a216, a217, a218, a219, a22, a220, a221, a222, a223, a224, a225, a226, a227, a228, a229, a23, a230, a231, a232, a233, a234, a235, a236, a237, a238, a239, a24, a240, a241, a242, a243, a244, a245, a246, a247, a248, a249, a25, a250, a251, a252, a253, a254, a255, a256, a257, a258, a259, a26, a260, a261, a262, a263, a264, a265, a266, a267, a268, a269, a27, a270, a271, a272, a273, a274, a275, a276, a277, a278, a279, a28, a280, a281, a282, a283, a284, a285, a286, a287, a288, a289, a29, a290, a291, a292, a293, a294, a295, a296, a297, a298, a299, a3, a30, a300, a301, a302, a303, a304, a305, a306, a307, a308, a309, a31, a310, a311, a312, a313, a314, a315, a316, a317, a318, a319, a32, a320, a321, a322, a323, a324, a325, a326, a327, a328, a329, a33, a330, a331, a332, a333, a334, a335, a336, a337, a338, a339, a34, a340, a341, a342, a343, a344, a345, a346, a347, a348, a349, a35, a350, a351, a352, a353, a354, a355, a356, a357, a358, a359, a36, a360, a361, a362, a363, a364, a365, a366, a367, a368, a37, a38, a39, a4, a40, a41, a42, a43, a44, a45, a46, a47, a48, a49, a5, a50, a51, a52, a53, a54, a55, a56, a57, a58, a59, a6, a60, a61, a62, a63, a64, a65, a66, a67, a68, a69, a7, a70, a71, a72, a73, a74, a75, a76, a77, a78, a79, a8, a80, a81, a82, a83, a84, a85, a86, a87, a88, a89, a9, a90, a91, a92, a93, a94, a95, a96, a97, a98, a99; + a0=arg[0]? arg[0][0] : 0; + a1=arg[1]? arg[1][0] : 0; + a1=(a0-a1); + if (res[0]!=0) res[0][0]=a1; + a1=arg[0]? arg[0][1] : 0; + a2=arg[1]? arg[1][1] : 0; + a2=(a1-a2); + if (res[0]!=0) res[0][1]=a2; + a2=arg[0]? arg[0][2] : 0; + a3=arg[1]? arg[1][2] : 0; + a3=(a2-a3); + if (res[0]!=0) res[0][2]=a3; + a3=arg[0]? arg[0][3] : 0; + a4=4.1666666666666664e-02; + a5=arg[0]? arg[0][48] : 0; + a6=cos(a2); + a7=(a5*a6); + a8=2.; + a9=1.2500000000000000e-01; + a10=4.0000000000000002e-01; + a11=arg[1]? arg[1][49] : 0; + a12=(a10*a11); + a13=5.9999999999999998e-01; + a14=arg[0]? arg[0][49] : 0; + a15=(a13*a14); + a12=(a12+a15); + a15=(a9*a12); + a15=(a2+a15); + a16=cos(a15); + a17=(a5*a16); + a17=(a8*a17); + a7=(a7+a17); + a17=(a10*a11); + a18=(a13*a14); + a17=(a17+a18); + a18=(a9*a17); + a18=(a2+a18); + a19=cos(a18); + a20=(a5*a19); + a20=(a8*a20); + a7=(a7+a20); + a20=2.5000000000000000e-01; + a21=(a10*a11); + a22=(a13*a14); + a21=(a21+a22); + a22=(a20*a21); + a22=(a2+a22); + a23=cos(a22); + a24=(a5*a23); + a7=(a7+a24); + a7=(a4*a7); + a7=(a0+a7); + a7=(a3-a7); + if (res[0]!=0) res[0][3]=a7; + a7=arg[0]? arg[0][4] : 0; + a24=sin(a2); + a25=(a5*a24); + a26=sin(a15); + a27=(a5*a26); + a27=(a8*a27); + a25=(a25+a27); + a27=sin(a18); + a28=(a5*a27); + a28=(a8*a28); + a25=(a25+a28); + a28=sin(a22); + a29=(a5*a28); + a25=(a25+a29); + a25=(a4*a25); + a25=(a1+a25); + a25=(a7-a25); + if (res[0]!=0) res[0][4]=a25; + a25=arg[0]? arg[0][5] : 0; + a29=cos(a25); + a17=(a8*a17); + a12=(a12+a17); + a21=(a8*a21); + a12=(a12+a21); + a21=(a10*a11); + a17=(a13*a14); + a21=(a21+a17); + a12=(a12+a21); + a12=(a4*a12); + a12=(a2+a12); + a21=sin(a12); + a17=(a29*a21); + a30=sin(a25); + a31=cos(a12); + a32=(a30*a31); + a17=(a17-a32); + a32=cos(a25); + a33=(a32*a31); + a34=sin(a25); + a35=(a34*a21); + a33=(a33+a35); + a35=atan2(a17,a33); + if (res[0]!=0) res[0][5]=a35; + a35=arg[0]? arg[0][6] : 0; + a36=arg[0]? arg[0][50] : 0; + a37=cos(a25); + a38=(a36*a37); + a14=(a13*a14); + a11=(a10*a11); + a14=(a14+a11); + a11=(a10*a14); + a39=arg[0]? arg[0][51] : 0; + a40=(a13*a39); + a11=(a11+a40); + a40=(a9*a11); + a40=(a25+a40); + a41=cos(a40); + a42=(a36*a41); + a42=(a8*a42); + a38=(a38+a42); + a42=(a10*a14); + a43=(a13*a39); + a42=(a42+a43); + a43=(a9*a42); + a43=(a25+a43); + a44=cos(a43); + a45=(a36*a44); + a45=(a8*a45); + a38=(a38+a45); + a45=(a10*a14); + a46=(a13*a39); + a45=(a45+a46); + a46=(a20*a45); + a46=(a25+a46); + a47=cos(a46); + a48=(a36*a47); + a38=(a38+a48); + a38=(a4*a38); + a38=(a3+a38); + a38=(a35-a38); + if (res[0]!=0) res[0][6]=a38; + a38=arg[0]? arg[0][7] : 0; + a48=sin(a25); + a49=(a36*a48); + a50=sin(a40); + a51=(a36*a50); + a51=(a8*a51); + a49=(a49+a51); + a51=sin(a43); + a52=(a36*a51); + a52=(a8*a52); + a49=(a49+a52); + a52=sin(a46); + a53=(a36*a52); + a49=(a49+a53); + a49=(a4*a49); + a49=(a7+a49); + a49=(a38-a49); + if (res[0]!=0) res[0][7]=a49; + a49=arg[0]? arg[0][8] : 0; + a53=cos(a49); + a42=(a8*a42); + a11=(a11+a42); + a45=(a8*a45); + a11=(a11+a45); + a45=(a10*a14); + a42=(a13*a39); + a45=(a45+a42); + a11=(a11+a45); + a11=(a4*a11); + a11=(a25+a11); + a45=sin(a11); + a42=(a53*a45); + a54=sin(a49); + a55=cos(a11); + a56=(a54*a55); + a42=(a42-a56); + a56=cos(a49); + a57=(a56*a55); + a58=sin(a49); + a59=(a58*a45); + a57=(a57+a59); + a59=atan2(a42,a57); + if (res[0]!=0) res[0][8]=a59; + a59=arg[0]? arg[0][9] : 0; + a60=arg[0]? arg[0][52] : 0; + a61=cos(a49); + a62=(a60*a61); + a39=(a13*a39); + a14=(a10*a14); + a39=(a39+a14); + a14=(a10*a39); + a63=arg[0]? arg[0][53] : 0; + a64=(a13*a63); + a14=(a14+a64); + a64=(a9*a14); + a64=(a49+a64); + a65=cos(a64); + a66=(a60*a65); + a66=(a8*a66); + a62=(a62+a66); + a66=(a10*a39); + a67=(a13*a63); + a66=(a66+a67); + a67=(a9*a66); + a67=(a49+a67); + a68=cos(a67); + a69=(a60*a68); + a69=(a8*a69); + a62=(a62+a69); + a69=(a10*a39); + a70=(a13*a63); + a69=(a69+a70); + a70=(a20*a69); + a70=(a49+a70); + a71=cos(a70); + a72=(a60*a71); + a62=(a62+a72); + a62=(a4*a62); + a62=(a35+a62); + a62=(a59-a62); + if (res[0]!=0) res[0][9]=a62; + a62=arg[0]? arg[0][10] : 0; + a72=sin(a49); + a73=(a60*a72); + a74=sin(a64); + a75=(a60*a74); + a75=(a8*a75); + a73=(a73+a75); + a75=sin(a67); + a76=(a60*a75); + a76=(a8*a76); + a73=(a73+a76); + a76=sin(a70); + a77=(a60*a76); + a73=(a73+a77); + a73=(a4*a73); + a73=(a38+a73); + a73=(a62-a73); + if (res[0]!=0) res[0][10]=a73; + a73=arg[0]? arg[0][11] : 0; + a77=cos(a73); + a66=(a8*a66); + a14=(a14+a66); + a69=(a8*a69); + a14=(a14+a69); + a69=(a10*a39); + a66=(a13*a63); + a69=(a69+a66); + a14=(a14+a69); + a14=(a4*a14); + a14=(a49+a14); + a69=sin(a14); + a66=(a77*a69); + a78=sin(a73); + a79=cos(a14); + a80=(a78*a79); + a66=(a66-a80); + a80=cos(a73); + a81=(a80*a79); + a82=sin(a73); + a83=(a82*a69); + a81=(a81+a83); + a83=atan2(a66,a81); + if (res[0]!=0) res[0][11]=a83; + a83=arg[0]? arg[0][12] : 0; + a84=arg[0]? arg[0][54] : 0; + a85=cos(a73); + a86=(a84*a85); + a63=(a13*a63); + a39=(a10*a39); + a63=(a63+a39); + a39=(a10*a63); + a87=arg[0]? arg[0][55] : 0; + a88=(a13*a87); + a39=(a39+a88); + a88=(a9*a39); + a88=(a73+a88); + a89=cos(a88); + a90=(a84*a89); + a90=(a8*a90); + a86=(a86+a90); + a90=(a10*a63); + a91=(a13*a87); + a90=(a90+a91); + a91=(a9*a90); + a91=(a73+a91); + a92=cos(a91); + a93=(a84*a92); + a93=(a8*a93); + a86=(a86+a93); + a93=(a10*a63); + a94=(a13*a87); + a93=(a93+a94); + a94=(a20*a93); + a94=(a73+a94); + a95=cos(a94); + a96=(a84*a95); + a86=(a86+a96); + a86=(a4*a86); + a86=(a59+a86); + a86=(a83-a86); + if (res[0]!=0) res[0][12]=a86; + a86=arg[0]? arg[0][13] : 0; + a96=sin(a73); + a97=(a84*a96); + a98=sin(a88); + a99=(a84*a98); + a99=(a8*a99); + a97=(a97+a99); + a99=sin(a91); + a100=(a84*a99); + a100=(a8*a100); + a97=(a97+a100); + a100=sin(a94); + a101=(a84*a100); + a97=(a97+a101); + a97=(a4*a97); + a97=(a62+a97); + a97=(a86-a97); + if (res[0]!=0) res[0][13]=a97; + a97=arg[0]? arg[0][14] : 0; + a101=cos(a97); + a90=(a8*a90); + a39=(a39+a90); + a93=(a8*a93); + a39=(a39+a93); + a93=(a10*a63); + a90=(a13*a87); + a93=(a93+a90); + a39=(a39+a93); + a39=(a4*a39); + a39=(a73+a39); + a93=sin(a39); + a90=(a101*a93); + a102=sin(a97); + a103=cos(a39); + a104=(a102*a103); + a90=(a90-a104); + a104=cos(a97); + a105=(a104*a103); + a106=sin(a97); + a107=(a106*a93); + a105=(a105+a107); + a107=atan2(a90,a105); + if (res[0]!=0) res[0][14]=a107; + a107=arg[0]? arg[0][15] : 0; + a108=arg[0]? arg[0][56] : 0; + a109=cos(a97); + a110=(a108*a109); + a87=(a13*a87); + a63=(a10*a63); + a87=(a87+a63); + a63=(a10*a87); + a111=arg[0]? arg[0][57] : 0; + a112=(a13*a111); + a63=(a63+a112); + a112=(a9*a63); + a112=(a97+a112); + a113=cos(a112); + a114=(a108*a113); + a114=(a8*a114); + a110=(a110+a114); + a114=(a10*a87); + a115=(a13*a111); + a114=(a114+a115); + a115=(a9*a114); + a115=(a97+a115); + a116=cos(a115); + a117=(a108*a116); + a117=(a8*a117); + a110=(a110+a117); + a117=(a10*a87); + a118=(a13*a111); + a117=(a117+a118); + a118=(a20*a117); + a118=(a97+a118); + a119=cos(a118); + a120=(a108*a119); + a110=(a110+a120); + a110=(a4*a110); + a110=(a83+a110); + a110=(a107-a110); + if (res[0]!=0) res[0][15]=a110; + a110=arg[0]? arg[0][16] : 0; + a120=sin(a97); + a121=(a108*a120); + a122=sin(a112); + a123=(a108*a122); + a123=(a8*a123); + a121=(a121+a123); + a123=sin(a115); + a124=(a108*a123); + a124=(a8*a124); + a121=(a121+a124); + a124=sin(a118); + a125=(a108*a124); + a121=(a121+a125); + a121=(a4*a121); + a121=(a86+a121); + a121=(a110-a121); + if (res[0]!=0) res[0][16]=a121; + a121=arg[0]? arg[0][17] : 0; + a125=cos(a121); + a114=(a8*a114); + a63=(a63+a114); + a117=(a8*a117); + a63=(a63+a117); + a117=(a10*a87); + a114=(a13*a111); + a117=(a117+a114); + a63=(a63+a117); + a63=(a4*a63); + a63=(a97+a63); + a117=sin(a63); + a114=(a125*a117); + a126=sin(a121); + a127=cos(a63); + a128=(a126*a127); + a114=(a114-a128); + a128=cos(a121); + a129=(a128*a127); + a130=sin(a121); + a131=(a130*a117); + a129=(a129+a131); + a131=atan2(a114,a129); + if (res[0]!=0) res[0][17]=a131; + a131=arg[0]? arg[0][18] : 0; + a132=arg[0]? arg[0][58] : 0; + a133=cos(a121); + a134=(a132*a133); + a111=(a13*a111); + a87=(a10*a87); + a111=(a111+a87); + a87=(a10*a111); + a135=arg[0]? arg[0][59] : 0; + a136=(a13*a135); + a87=(a87+a136); + a136=(a9*a87); + a136=(a121+a136); + a137=cos(a136); + a138=(a132*a137); + a138=(a8*a138); + a134=(a134+a138); + a138=(a10*a111); + a139=(a13*a135); + a138=(a138+a139); + a139=(a9*a138); + a139=(a121+a139); + a140=cos(a139); + a141=(a132*a140); + a141=(a8*a141); + a134=(a134+a141); + a141=(a10*a111); + a142=(a13*a135); + a141=(a141+a142); + a142=(a20*a141); + a142=(a121+a142); + a143=cos(a142); + a144=(a132*a143); + a134=(a134+a144); + a134=(a4*a134); + a134=(a107+a134); + a134=(a131-a134); + if (res[0]!=0) res[0][18]=a134; + a134=arg[0]? arg[0][19] : 0; + a144=sin(a121); + a145=(a132*a144); + a146=sin(a136); + a147=(a132*a146); + a147=(a8*a147); + a145=(a145+a147); + a147=sin(a139); + a148=(a132*a147); + a148=(a8*a148); + a145=(a145+a148); + a148=sin(a142); + a149=(a132*a148); + a145=(a145+a149); + a145=(a4*a145); + a145=(a110+a145); + a145=(a134-a145); + if (res[0]!=0) res[0][19]=a145; + a145=arg[0]? arg[0][20] : 0; + a149=cos(a145); + a138=(a8*a138); + a87=(a87+a138); + a141=(a8*a141); + a87=(a87+a141); + a141=(a10*a111); + a138=(a13*a135); + a141=(a141+a138); + a87=(a87+a141); + a87=(a4*a87); + a87=(a121+a87); + a141=sin(a87); + a138=(a149*a141); + a150=sin(a145); + a151=cos(a87); + a152=(a150*a151); + a138=(a138-a152); + a152=cos(a145); + a153=(a152*a151); + a154=sin(a145); + a155=(a154*a141); + a153=(a153+a155); + a155=atan2(a138,a153); + if (res[0]!=0) res[0][20]=a155; + a155=arg[0]? arg[0][21] : 0; + a156=arg[0]? arg[0][60] : 0; + a157=cos(a145); + a158=(a156*a157); + a135=(a13*a135); + a111=(a10*a111); + a135=(a135+a111); + a111=(a10*a135); + a159=arg[0]? arg[0][61] : 0; + a160=(a13*a159); + a111=(a111+a160); + a160=(a9*a111); + a160=(a145+a160); + a161=cos(a160); + a162=(a156*a161); + a162=(a8*a162); + a158=(a158+a162); + a162=(a10*a135); + a163=(a13*a159); + a162=(a162+a163); + a163=(a9*a162); + a163=(a145+a163); + a164=cos(a163); + a165=(a156*a164); + a165=(a8*a165); + a158=(a158+a165); + a165=(a10*a135); + a166=(a13*a159); + a165=(a165+a166); + a166=(a20*a165); + a166=(a145+a166); + a167=cos(a166); + a168=(a156*a167); + a158=(a158+a168); + a158=(a4*a158); + a158=(a131+a158); + a158=(a155-a158); + if (res[0]!=0) res[0][21]=a158; + a158=arg[0]? arg[0][22] : 0; + a168=sin(a145); + a169=(a156*a168); + a170=sin(a160); + a171=(a156*a170); + a171=(a8*a171); + a169=(a169+a171); + a171=sin(a163); + a172=(a156*a171); + a172=(a8*a172); + a169=(a169+a172); + a172=sin(a166); + a173=(a156*a172); + a169=(a169+a173); + a169=(a4*a169); + a169=(a134+a169); + a169=(a158-a169); + if (res[0]!=0) res[0][22]=a169; + a169=arg[0]? arg[0][23] : 0; + a173=cos(a169); + a162=(a8*a162); + a111=(a111+a162); + a165=(a8*a165); + a111=(a111+a165); + a165=(a10*a135); + a162=(a13*a159); + a165=(a165+a162); + a111=(a111+a165); + a111=(a4*a111); + a111=(a145+a111); + a165=sin(a111); + a162=(a173*a165); + a174=sin(a169); + a175=cos(a111); + a176=(a174*a175); + a162=(a162-a176); + a176=cos(a169); + a177=(a176*a175); + a178=sin(a169); + a179=(a178*a165); + a177=(a177+a179); + a179=atan2(a162,a177); + if (res[0]!=0) res[0][23]=a179; + a179=arg[0]? arg[0][24] : 0; + a180=arg[0]? arg[0][62] : 0; + a181=cos(a169); + a182=(a180*a181); + a159=(a13*a159); + a135=(a10*a135); + a159=(a159+a135); + a135=(a10*a159); + a183=arg[0]? arg[0][63] : 0; + a184=(a13*a183); + a135=(a135+a184); + a184=(a9*a135); + a184=(a169+a184); + a185=cos(a184); + a186=(a180*a185); + a186=(a8*a186); + a182=(a182+a186); + a186=(a10*a159); + a187=(a13*a183); + a186=(a186+a187); + a187=(a9*a186); + a187=(a169+a187); + a188=cos(a187); + a189=(a180*a188); + a189=(a8*a189); + a182=(a182+a189); + a189=(a10*a159); + a190=(a13*a183); + a189=(a189+a190); + a190=(a20*a189); + a190=(a169+a190); + a191=cos(a190); + a192=(a180*a191); + a182=(a182+a192); + a182=(a4*a182); + a182=(a155+a182); + a182=(a179-a182); + if (res[0]!=0) res[0][24]=a182; + a182=arg[0]? arg[0][25] : 0; + a192=sin(a169); + a193=(a180*a192); + a194=sin(a184); + a195=(a180*a194); + a195=(a8*a195); + a193=(a193+a195); + a195=sin(a187); + a196=(a180*a195); + a196=(a8*a196); + a193=(a193+a196); + a196=sin(a190); + a197=(a180*a196); + a193=(a193+a197); + a193=(a4*a193); + a193=(a158+a193); + a193=(a182-a193); + if (res[0]!=0) res[0][25]=a193; + a193=arg[0]? arg[0][26] : 0; + a197=cos(a193); + a186=(a8*a186); + a135=(a135+a186); + a189=(a8*a189); + a135=(a135+a189); + a189=(a10*a159); + a186=(a13*a183); + a189=(a189+a186); + a135=(a135+a189); + a135=(a4*a135); + a135=(a169+a135); + a189=sin(a135); + a186=(a197*a189); + a198=sin(a193); + a199=cos(a135); + a200=(a198*a199); + a186=(a186-a200); + a200=cos(a193); + a201=(a200*a199); + a202=sin(a193); + a203=(a202*a189); + a201=(a201+a203); + a203=atan2(a186,a201); + if (res[0]!=0) res[0][26]=a203; + a203=arg[0]? arg[0][27] : 0; + a204=arg[0]? arg[0][64] : 0; + a205=cos(a193); + a206=(a204*a205); + a183=(a13*a183); + a159=(a10*a159); + a183=(a183+a159); + a159=(a10*a183); + a207=arg[0]? arg[0][65] : 0; + a208=(a13*a207); + a159=(a159+a208); + a208=(a9*a159); + a208=(a193+a208); + a209=cos(a208); + a210=(a204*a209); + a210=(a8*a210); + a206=(a206+a210); + a210=(a10*a183); + a211=(a13*a207); + a210=(a210+a211); + a211=(a9*a210); + a211=(a193+a211); + a212=cos(a211); + a213=(a204*a212); + a213=(a8*a213); + a206=(a206+a213); + a213=(a10*a183); + a214=(a13*a207); + a213=(a213+a214); + a214=(a20*a213); + a214=(a193+a214); + a215=cos(a214); + a216=(a204*a215); + a206=(a206+a216); + a206=(a4*a206); + a206=(a179+a206); + a206=(a203-a206); + if (res[0]!=0) res[0][27]=a206; + a206=arg[0]? arg[0][28] : 0; + a216=sin(a193); + a217=(a204*a216); + a218=sin(a208); + a219=(a204*a218); + a219=(a8*a219); + a217=(a217+a219); + a219=sin(a211); + a220=(a204*a219); + a220=(a8*a220); + a217=(a217+a220); + a220=sin(a214); + a221=(a204*a220); + a217=(a217+a221); + a217=(a4*a217); + a217=(a182+a217); + a217=(a206-a217); + if (res[0]!=0) res[0][28]=a217; + a217=arg[0]? arg[0][29] : 0; + a221=cos(a217); + a210=(a8*a210); + a159=(a159+a210); + a213=(a8*a213); + a159=(a159+a213); + a213=(a10*a183); + a210=(a13*a207); + a213=(a213+a210); + a159=(a159+a213); + a159=(a4*a159); + a159=(a193+a159); + a213=sin(a159); + a210=(a221*a213); + a222=sin(a217); + a223=cos(a159); + a224=(a222*a223); + a210=(a210-a224); + a224=cos(a217); + a225=(a224*a223); + a226=sin(a217); + a227=(a226*a213); + a225=(a225+a227); + a227=atan2(a210,a225); + if (res[0]!=0) res[0][29]=a227; + a227=arg[0]? arg[0][30] : 0; + a228=arg[0]? arg[0][66] : 0; + a229=cos(a217); + a230=(a228*a229); + a207=(a13*a207); + a183=(a10*a183); + a207=(a207+a183); + a183=(a10*a207); + a231=arg[0]? arg[0][67] : 0; + a232=(a13*a231); + a183=(a183+a232); + a232=(a9*a183); + a232=(a217+a232); + a233=cos(a232); + a234=(a228*a233); + a234=(a8*a234); + a230=(a230+a234); + a234=(a10*a207); + a235=(a13*a231); + a234=(a234+a235); + a235=(a9*a234); + a235=(a217+a235); + a236=cos(a235); + a237=(a228*a236); + a237=(a8*a237); + a230=(a230+a237); + a237=(a10*a207); + a238=(a13*a231); + a237=(a237+a238); + a238=(a20*a237); + a238=(a217+a238); + a239=cos(a238); + a240=(a228*a239); + a230=(a230+a240); + a230=(a4*a230); + a230=(a203+a230); + a230=(a227-a230); + if (res[0]!=0) res[0][30]=a230; + a230=arg[0]? arg[0][31] : 0; + a240=sin(a217); + a241=(a228*a240); + a242=sin(a232); + a243=(a228*a242); + a243=(a8*a243); + a241=(a241+a243); + a243=sin(a235); + a244=(a228*a243); + a244=(a8*a244); + a241=(a241+a244); + a244=sin(a238); + a245=(a228*a244); + a241=(a241+a245); + a241=(a4*a241); + a241=(a206+a241); + a241=(a230-a241); + if (res[0]!=0) res[0][31]=a241; + a241=arg[0]? arg[0][32] : 0; + a245=cos(a241); + a234=(a8*a234); + a183=(a183+a234); + a237=(a8*a237); + a183=(a183+a237); + a237=(a10*a207); + a234=(a13*a231); + a237=(a237+a234); + a183=(a183+a237); + a183=(a4*a183); + a183=(a217+a183); + a237=sin(a183); + a234=(a245*a237); + a246=sin(a241); + a247=cos(a183); + a248=(a246*a247); + a234=(a234-a248); + a248=cos(a241); + a249=(a248*a247); + a250=sin(a241); + a251=(a250*a237); + a249=(a249+a251); + a251=atan2(a234,a249); + if (res[0]!=0) res[0][32]=a251; + a251=arg[0]? arg[0][33] : 0; + a252=arg[0]? arg[0][68] : 0; + a253=cos(a241); + a254=(a252*a253); + a231=(a13*a231); + a207=(a10*a207); + a231=(a231+a207); + a207=(a10*a231); + a255=arg[0]? arg[0][69] : 0; + a256=(a13*a255); + a207=(a207+a256); + a256=(a9*a207); + a256=(a241+a256); + a257=cos(a256); + a258=(a252*a257); + a258=(a8*a258); + a254=(a254+a258); + a258=(a10*a231); + a259=(a13*a255); + a258=(a258+a259); + a259=(a9*a258); + a259=(a241+a259); + a260=cos(a259); + a261=(a252*a260); + a261=(a8*a261); + a254=(a254+a261); + a261=(a10*a231); + a262=(a13*a255); + a261=(a261+a262); + a262=(a20*a261); + a262=(a241+a262); + a263=cos(a262); + a264=(a252*a263); + a254=(a254+a264); + a254=(a4*a254); + a254=(a227+a254); + a254=(a251-a254); + if (res[0]!=0) res[0][33]=a254; + a254=arg[0]? arg[0][34] : 0; + a264=sin(a241); + a265=(a252*a264); + a266=sin(a256); + a267=(a252*a266); + a267=(a8*a267); + a265=(a265+a267); + a267=sin(a259); + a268=(a252*a267); + a268=(a8*a268); + a265=(a265+a268); + a268=sin(a262); + a269=(a252*a268); + a265=(a265+a269); + a265=(a4*a265); + a265=(a230+a265); + a265=(a254-a265); + if (res[0]!=0) res[0][34]=a265; + a265=arg[0]? arg[0][35] : 0; + a269=cos(a265); + a258=(a8*a258); + a207=(a207+a258); + a261=(a8*a261); + a207=(a207+a261); + a261=(a10*a231); + a258=(a13*a255); + a261=(a261+a258); + a207=(a207+a261); + a207=(a4*a207); + a207=(a241+a207); + a261=sin(a207); + a258=(a269*a261); + a270=sin(a265); + a271=cos(a207); + a272=(a270*a271); + a258=(a258-a272); + a272=cos(a265); + a273=(a272*a271); + a274=sin(a265); + a275=(a274*a261); + a273=(a273+a275); + a275=atan2(a258,a273); + if (res[0]!=0) res[0][35]=a275; + a275=arg[0]? arg[0][36] : 0; + a276=arg[0]? arg[0][70] : 0; + a277=cos(a265); + a278=(a276*a277); + a255=(a13*a255); + a231=(a10*a231); + a255=(a255+a231); + a231=(a10*a255); + a279=arg[0]? arg[0][71] : 0; + a280=(a13*a279); + a231=(a231+a280); + a280=(a9*a231); + a280=(a265+a280); + a281=cos(a280); + a282=(a276*a281); + a282=(a8*a282); + a278=(a278+a282); + a282=(a10*a255); + a283=(a13*a279); + a282=(a282+a283); + a283=(a9*a282); + a283=(a265+a283); + a284=cos(a283); + a285=(a276*a284); + a285=(a8*a285); + a278=(a278+a285); + a285=(a10*a255); + a286=(a13*a279); + a285=(a285+a286); + a286=(a20*a285); + a286=(a265+a286); + a287=cos(a286); + a288=(a276*a287); + a278=(a278+a288); + a278=(a4*a278); + a278=(a251+a278); + a278=(a275-a278); + if (res[0]!=0) res[0][36]=a278; + a278=arg[0]? arg[0][37] : 0; + a288=sin(a265); + a289=(a276*a288); + a290=sin(a280); + a291=(a276*a290); + a291=(a8*a291); + a289=(a289+a291); + a291=sin(a283); + a292=(a276*a291); + a292=(a8*a292); + a289=(a289+a292); + a292=sin(a286); + a293=(a276*a292); + a289=(a289+a293); + a289=(a4*a289); + a289=(a254+a289); + a289=(a278-a289); + if (res[0]!=0) res[0][37]=a289; + a289=arg[0]? arg[0][38] : 0; + a293=cos(a289); + a282=(a8*a282); + a231=(a231+a282); + a285=(a8*a285); + a231=(a231+a285); + a285=(a10*a255); + a282=(a13*a279); + a285=(a285+a282); + a231=(a231+a285); + a231=(a4*a231); + a231=(a265+a231); + a285=sin(a231); + a282=(a293*a285); + a294=sin(a289); + a295=cos(a231); + a296=(a294*a295); + a282=(a282-a296); + a296=cos(a289); + a297=(a296*a295); + a298=sin(a289); + a299=(a298*a285); + a297=(a297+a299); + a299=atan2(a282,a297); + if (res[0]!=0) res[0][38]=a299; + a299=arg[0]? arg[0][39] : 0; + a300=arg[0]? arg[0][72] : 0; + a301=cos(a289); + a302=(a300*a301); + a279=(a13*a279); + a255=(a10*a255); + a279=(a279+a255); + a255=(a10*a279); + a303=arg[0]? arg[0][73] : 0; + a304=(a13*a303); + a255=(a255+a304); + a304=(a9*a255); + a304=(a289+a304); + a305=cos(a304); + a306=(a300*a305); + a306=(a8*a306); + a302=(a302+a306); + a306=(a10*a279); + a307=(a13*a303); + a306=(a306+a307); + a307=(a9*a306); + a307=(a289+a307); + a308=cos(a307); + a309=(a300*a308); + a309=(a8*a309); + a302=(a302+a309); + a309=(a10*a279); + a310=(a13*a303); + a309=(a309+a310); + a310=(a20*a309); + a310=(a289+a310); + a311=cos(a310); + a312=(a300*a311); + a302=(a302+a312); + a302=(a4*a302); + a302=(a275+a302); + a302=(a299-a302); + if (res[0]!=0) res[0][39]=a302; + a302=arg[0]? arg[0][40] : 0; + a312=sin(a289); + a313=(a300*a312); + a314=sin(a304); + a315=(a300*a314); + a315=(a8*a315); + a313=(a313+a315); + a315=sin(a307); + a316=(a300*a315); + a316=(a8*a316); + a313=(a313+a316); + a316=sin(a310); + a317=(a300*a316); + a313=(a313+a317); + a313=(a4*a313); + a313=(a278+a313); + a313=(a302-a313); + if (res[0]!=0) res[0][40]=a313; + a313=arg[0]? arg[0][41] : 0; + a317=cos(a313); + a306=(a8*a306); + a255=(a255+a306); + a309=(a8*a309); + a255=(a255+a309); + a309=(a10*a279); + a306=(a13*a303); + a309=(a309+a306); + a255=(a255+a309); + a255=(a4*a255); + a255=(a289+a255); + a309=sin(a255); + a306=(a317*a309); + a318=sin(a313); + a319=cos(a255); + a320=(a318*a319); + a306=(a306-a320); + a320=cos(a313); + a321=(a320*a319); + a322=sin(a313); + a323=(a322*a309); + a321=(a321+a323); + a323=atan2(a306,a321); + if (res[0]!=0) res[0][41]=a323; + a323=arg[0]? arg[0][42] : 0; + a324=arg[0]? arg[0][74] : 0; + a325=cos(a313); + a326=(a324*a325); + a303=(a13*a303); + a279=(a10*a279); + a303=(a303+a279); + a279=(a10*a303); + a327=arg[0]? arg[0][75] : 0; + a328=(a13*a327); + a279=(a279+a328); + a328=(a9*a279); + a328=(a313+a328); + a329=cos(a328); + a330=(a324*a329); + a330=(a8*a330); + a326=(a326+a330); + a330=(a10*a303); + a331=(a13*a327); + a330=(a330+a331); + a331=(a9*a330); + a331=(a313+a331); + a332=cos(a331); + a333=(a324*a332); + a333=(a8*a333); + a326=(a326+a333); + a333=(a10*a303); + a334=(a13*a327); + a333=(a333+a334); + a334=(a20*a333); + a334=(a313+a334); + a335=cos(a334); + a336=(a324*a335); + a326=(a326+a336); + a326=(a4*a326); + a326=(a299+a326); + a326=(a323-a326); + if (res[0]!=0) res[0][42]=a326; + a326=arg[0]? arg[0][43] : 0; + a336=sin(a313); + a337=(a324*a336); + a338=sin(a328); + a339=(a324*a338); + a339=(a8*a339); + a337=(a337+a339); + a339=sin(a331); + a340=(a324*a339); + a340=(a8*a340); + a337=(a337+a340); + a340=sin(a334); + a341=(a324*a340); + a337=(a337+a341); + a337=(a4*a337); + a337=(a302+a337); + a337=(a326-a337); + if (res[0]!=0) res[0][43]=a337; + a337=arg[0]? arg[0][44] : 0; + a341=cos(a337); + a330=(a8*a330); + a279=(a279+a330); + a333=(a8*a333); + a279=(a279+a333); + a333=(a10*a303); + a330=(a13*a327); + a333=(a333+a330); + a279=(a279+a333); + a279=(a4*a279); + a279=(a313+a279); + a333=sin(a279); + a330=(a341*a333); + a342=sin(a337); + a343=cos(a279); + a344=(a342*a343); + a330=(a330-a344); + a344=cos(a337); + a345=(a344*a343); + a346=sin(a337); + a347=(a346*a333); + a345=(a345+a347); + a347=atan2(a330,a345); + if (res[0]!=0) res[0][44]=a347; + a347=arg[0]? arg[0][45] : 0; + a348=arg[0]? arg[0][76] : 0; + a349=cos(a337); + a350=(a348*a349); + a327=(a13*a327); + a303=(a10*a303); + a327=(a327+a303); + a303=(a10*a327); + a351=arg[0]? arg[0][77] : 0; + a352=(a13*a351); + a303=(a303+a352); + a352=(a9*a303); + a352=(a337+a352); + a353=cos(a352); + a354=(a348*a353); + a354=(a8*a354); + a350=(a350+a354); + a354=(a10*a327); + a355=(a13*a351); + a354=(a354+a355); + a9=(a9*a354); + a9=(a337+a9); + a355=cos(a9); + a356=(a348*a355); + a356=(a8*a356); + a350=(a350+a356); + a356=(a10*a327); + a357=(a13*a351); + a356=(a356+a357); + a20=(a20*a356); + a20=(a337+a20); + a357=cos(a20); + a358=(a348*a357); + a350=(a350+a358); + a350=(a4*a350); + a350=(a323+a350); + a347=(a347-a350); + if (res[0]!=0) res[0][45]=a347; + a347=arg[0]? arg[0][46] : 0; + a350=sin(a337); + a358=(a348*a350); + a359=sin(a352); + a360=(a348*a359); + a360=(a8*a360); + a358=(a358+a360); + a360=sin(a9); + a361=(a348*a360); + a361=(a8*a361); + a358=(a358+a361); + a361=sin(a20); + a362=(a348*a361); + a358=(a358+a362); + a358=(a4*a358); + a358=(a326+a358); + a347=(a347-a358); + if (res[0]!=0) res[0][46]=a347; + a347=arg[0]? arg[0][47] : 0; + a358=cos(a347); + a354=(a8*a354); + a303=(a303+a354); + a356=(a8*a356); + a303=(a303+a356); + a10=(a10*a327); + a13=(a13*a351); + a10=(a10+a13); + a303=(a303+a10); + a303=(a4*a303); + a303=(a337+a303); + a10=sin(a303); + a13=(a358*a10); + a351=sin(a347); + a327=cos(a303); + a356=(a351*a327); + a13=(a13-a356); + a356=cos(a347); + a354=(a356*a327); + a362=sin(a347); + a363=(a362*a10); + a354=(a354+a363); + a363=atan2(a13,a354); + if (res[0]!=0) res[0][47]=a363; + a363=arg[1]? arg[1][5] : 0; + a364=cos(a363); + a365=arg[1]? arg[1][4] : 0; + a1=(a1-a365); + a1=(a364*a1); + a363=sin(a363); + a365=arg[1]? arg[1][3] : 0; + a0=(a0-a365); + a0=(a363*a0); + a1=(a1-a0); + if (res[0]!=0) res[0][48]=a1; + a1=arg[1]? arg[1][8] : 0; + a0=cos(a1); + a365=arg[1]? arg[1][7] : 0; + a7=(a7-a365); + a7=(a0*a7); + a1=sin(a1); + a365=arg[1]? arg[1][6] : 0; + a3=(a3-a365); + a3=(a1*a3); + a7=(a7-a3); + if (res[0]!=0) res[0][49]=a7; + a7=arg[1]? arg[1][11] : 0; + a3=cos(a7); + a365=arg[1]? arg[1][10] : 0; + a38=(a38-a365); + a38=(a3*a38); + a7=sin(a7); + a365=arg[1]? arg[1][9] : 0; + a35=(a35-a365); + a35=(a7*a35); + a38=(a38-a35); + if (res[0]!=0) res[0][50]=a38; + a38=arg[1]? arg[1][14] : 0; + a35=cos(a38); + a365=arg[1]? arg[1][13] : 0; + a62=(a62-a365); + a62=(a35*a62); + a38=sin(a38); + a365=arg[1]? arg[1][12] : 0; + a59=(a59-a365); + a59=(a38*a59); + a62=(a62-a59); + if (res[0]!=0) res[0][51]=a62; + a62=arg[1]? arg[1][17] : 0; + a59=cos(a62); + a365=arg[1]? arg[1][16] : 0; + a86=(a86-a365); + a86=(a59*a86); + a62=sin(a62); + a365=arg[1]? arg[1][15] : 0; + a83=(a83-a365); + a83=(a62*a83); + a86=(a86-a83); + if (res[0]!=0) res[0][52]=a86; + a86=arg[1]? arg[1][20] : 0; + a83=cos(a86); + a365=arg[1]? arg[1][19] : 0; + a110=(a110-a365); + a110=(a83*a110); + a86=sin(a86); + a365=arg[1]? arg[1][18] : 0; + a107=(a107-a365); + a107=(a86*a107); + a110=(a110-a107); + if (res[0]!=0) res[0][53]=a110; + a110=arg[1]? arg[1][23] : 0; + a107=cos(a110); + a365=arg[1]? arg[1][22] : 0; + a134=(a134-a365); + a134=(a107*a134); + a110=sin(a110); + a365=arg[1]? arg[1][21] : 0; + a131=(a131-a365); + a131=(a110*a131); + a134=(a134-a131); + if (res[0]!=0) res[0][54]=a134; + a134=arg[1]? arg[1][26] : 0; + a131=cos(a134); + a365=arg[1]? arg[1][25] : 0; + a158=(a158-a365); + a158=(a131*a158); + a134=sin(a134); + a365=arg[1]? arg[1][24] : 0; + a155=(a155-a365); + a155=(a134*a155); + a158=(a158-a155); + if (res[0]!=0) res[0][55]=a158; + a158=arg[1]? arg[1][29] : 0; + a155=cos(a158); + a365=arg[1]? arg[1][28] : 0; + a182=(a182-a365); + a182=(a155*a182); + a158=sin(a158); + a365=arg[1]? arg[1][27] : 0; + a179=(a179-a365); + a179=(a158*a179); + a182=(a182-a179); + if (res[0]!=0) res[0][56]=a182; + a182=arg[1]? arg[1][32] : 0; + a179=cos(a182); + a365=arg[1]? arg[1][31] : 0; + a206=(a206-a365); + a206=(a179*a206); + a182=sin(a182); + a365=arg[1]? arg[1][30] : 0; + a203=(a203-a365); + a203=(a182*a203); + a206=(a206-a203); + if (res[0]!=0) res[0][57]=a206; + a206=arg[1]? arg[1][35] : 0; + a203=cos(a206); + a365=arg[1]? arg[1][34] : 0; + a230=(a230-a365); + a230=(a203*a230); + a206=sin(a206); + a365=arg[1]? arg[1][33] : 0; + a227=(a227-a365); + a227=(a206*a227); + a230=(a230-a227); + if (res[0]!=0) res[0][58]=a230; + a230=arg[1]? arg[1][38] : 0; + a227=cos(a230); + a365=arg[1]? arg[1][37] : 0; + a254=(a254-a365); + a254=(a227*a254); + a230=sin(a230); + a365=arg[1]? arg[1][36] : 0; + a251=(a251-a365); + a251=(a230*a251); + a254=(a254-a251); + if (res[0]!=0) res[0][59]=a254; + a254=arg[1]? arg[1][41] : 0; + a251=cos(a254); + a365=arg[1]? arg[1][40] : 0; + a278=(a278-a365); + a278=(a251*a278); + a254=sin(a254); + a365=arg[1]? arg[1][39] : 0; + a275=(a275-a365); + a275=(a254*a275); + a278=(a278-a275); + if (res[0]!=0) res[0][60]=a278; + a278=arg[1]? arg[1][44] : 0; + a275=cos(a278); + a365=arg[1]? arg[1][43] : 0; + a302=(a302-a365); + a302=(a275*a302); + a278=sin(a278); + a365=arg[1]? arg[1][42] : 0; + a299=(a299-a365); + a299=(a278*a299); + a302=(a302-a299); + if (res[0]!=0) res[0][61]=a302; + a302=arg[1]? arg[1][47] : 0; + a299=cos(a302); + a365=arg[1]? arg[1][46] : 0; + a326=(a326-a365); + a326=(a299*a326); + a302=sin(a302); + a365=arg[1]? arg[1][45] : 0; + a323=(a323-a365); + a323=(a302*a323); + a326=(a326-a323); + if (res[0]!=0) res[0][62]=a326; + a326=1.; + if (res[1]!=0) res[1][0]=a326; + a323=-1.; + if (res[1]!=0) res[1][1]=a323; + a363=(-a363); + if (res[1]!=0) res[1][2]=a363; + if (res[1]!=0) res[1][3]=a326; + if (res[1]!=0) res[1][4]=a323; + if (res[1]!=0) res[1][5]=a364; + if (res[1]!=0) res[1][6]=a326; + a364=sin(a2); + a364=(a5*a364); + a363=sin(a15); + a365=(a5*a363); + a365=(a8*a365); + a364=(a364+a365); + a365=sin(a18); + a366=(a5*a365); + a366=(a8*a366); + a364=(a364+a366); + a366=sin(a22); + a367=(a5*a366); + a364=(a364+a367); + a364=(a4*a364); + if (res[1]!=0) res[1][7]=a364; + a2=cos(a2); + a2=(a5*a2); + a15=cos(a15); + a364=(a5*a15); + a364=(a8*a364); + a2=(a2+a364); + a18=cos(a18); + a364=(a5*a18); + a364=(a8*a364); + a2=(a2+a364); + a22=cos(a22); + a364=(a5*a22); + a2=(a2+a364); + a2=(a4*a2); + a2=(-a2); + if (res[1]!=0) res[1][8]=a2; + a2=casadi_sq(a17); + a364=casadi_sq(a33); + a2=(a2+a364); + a33=(a33/a2); + a364=cos(a12); + a367=(a29*a364); + a12=sin(a12); + a368=(a30*a12); + a367=(a367+a368); + a367=(a33*a367); + a17=(a17/a2); + a2=(a34*a364); + a368=(a32*a12); + a2=(a2-a368); + a2=(a17*a2); + a367=(a367-a2); + if (res[1]!=0) res[1][9]=a367; + if (res[1]!=0) res[1][10]=a326; + if (res[1]!=0) res[1][11]=a323; + a1=(-a1); + if (res[1]!=0) res[1][12]=a1; + if (res[1]!=0) res[1][13]=a326; + if (res[1]!=0) res[1][14]=a323; + if (res[1]!=0) res[1][15]=a0; + a0=sin(a25); + a0=(a21*a0); + a1=cos(a25); + a1=(a31*a1); + a0=(a0+a1); + a0=(a33*a0); + a1=cos(a25); + a21=(a21*a1); + a1=sin(a25); + a31=(a31*a1); + a21=(a21-a31); + a21=(a17*a21); + a0=(a0+a21); + a0=(-a0); + if (res[1]!=0) res[1][16]=a0; + a0=sin(a25); + a0=(a36*a0); + a21=sin(a40); + a31=(a36*a21); + a31=(a8*a31); + a0=(a0+a31); + a31=sin(a43); + a1=(a36*a31); + a1=(a8*a1); + a0=(a0+a1); + a1=sin(a46); + a367=(a36*a1); + a0=(a0+a367); + a0=(a4*a0); + if (res[1]!=0) res[1][17]=a0; + a25=cos(a25); + a25=(a36*a25); + a40=cos(a40); + a0=(a36*a40); + a0=(a8*a0); + a25=(a25+a0); + a43=cos(a43); + a0=(a36*a43); + a0=(a8*a0); + a25=(a25+a0); + a46=cos(a46); + a0=(a36*a46); + a25=(a25+a0); + a25=(a4*a25); + a25=(-a25); + if (res[1]!=0) res[1][18]=a25; + a25=casadi_sq(a42); + a0=casadi_sq(a57); + a25=(a25+a0); + a57=(a57/a25); + a0=cos(a11); + a367=(a53*a0); + a11=sin(a11); + a2=(a54*a11); + a367=(a367+a2); + a367=(a57*a367); + a42=(a42/a25); + a25=(a58*a0); + a2=(a56*a11); + a25=(a25-a2); + a25=(a42*a25); + a367=(a367-a25); + if (res[1]!=0) res[1][19]=a367; + if (res[1]!=0) res[1][20]=a326; + if (res[1]!=0) res[1][21]=a323; + a7=(-a7); + if (res[1]!=0) res[1][22]=a7; + if (res[1]!=0) res[1][23]=a326; + if (res[1]!=0) res[1][24]=a323; + if (res[1]!=0) res[1][25]=a3; + a3=sin(a49); + a3=(a45*a3); + a7=cos(a49); + a7=(a55*a7); + a3=(a3+a7); + a3=(a57*a3); + a7=cos(a49); + a45=(a45*a7); + a7=sin(a49); + a55=(a55*a7); + a45=(a45-a55); + a45=(a42*a45); + a3=(a3+a45); + a3=(-a3); + if (res[1]!=0) res[1][26]=a3; + a3=sin(a49); + a3=(a60*a3); + a45=sin(a64); + a55=(a60*a45); + a55=(a8*a55); + a3=(a3+a55); + a55=sin(a67); + a7=(a60*a55); + a7=(a8*a7); + a3=(a3+a7); + a7=sin(a70); + a367=(a60*a7); + a3=(a3+a367); + a3=(a4*a3); + if (res[1]!=0) res[1][27]=a3; + a49=cos(a49); + a49=(a60*a49); + a64=cos(a64); + a3=(a60*a64); + a3=(a8*a3); + a49=(a49+a3); + a67=cos(a67); + a3=(a60*a67); + a3=(a8*a3); + a49=(a49+a3); + a70=cos(a70); + a3=(a60*a70); + a49=(a49+a3); + a49=(a4*a49); + a49=(-a49); + if (res[1]!=0) res[1][28]=a49; + a49=casadi_sq(a66); + a3=casadi_sq(a81); + a49=(a49+a3); + a81=(a81/a49); + a3=cos(a14); + a367=(a77*a3); + a14=sin(a14); + a25=(a78*a14); + a367=(a367+a25); + a367=(a81*a367); + a66=(a66/a49); + a49=(a82*a3); + a25=(a80*a14); + a49=(a49-a25); + a49=(a66*a49); + a367=(a367-a49); + if (res[1]!=0) res[1][29]=a367; + if (res[1]!=0) res[1][30]=a326; + if (res[1]!=0) res[1][31]=a323; + a38=(-a38); + if (res[1]!=0) res[1][32]=a38; + if (res[1]!=0) res[1][33]=a326; + if (res[1]!=0) res[1][34]=a323; + if (res[1]!=0) res[1][35]=a35; + a35=sin(a73); + a35=(a69*a35); + a38=cos(a73); + a38=(a79*a38); + a35=(a35+a38); + a35=(a81*a35); + a38=cos(a73); + a69=(a69*a38); + a38=sin(a73); + a79=(a79*a38); + a69=(a69-a79); + a69=(a66*a69); + a35=(a35+a69); + a35=(-a35); + if (res[1]!=0) res[1][36]=a35; + a35=sin(a73); + a35=(a84*a35); + a69=sin(a88); + a79=(a84*a69); + a79=(a8*a79); + a35=(a35+a79); + a79=sin(a91); + a38=(a84*a79); + a38=(a8*a38); + a35=(a35+a38); + a38=sin(a94); + a367=(a84*a38); + a35=(a35+a367); + a35=(a4*a35); + if (res[1]!=0) res[1][37]=a35; + a73=cos(a73); + a73=(a84*a73); + a88=cos(a88); + a35=(a84*a88); + a35=(a8*a35); + a73=(a73+a35); + a91=cos(a91); + a35=(a84*a91); + a35=(a8*a35); + a73=(a73+a35); + a94=cos(a94); + a35=(a84*a94); + a73=(a73+a35); + a73=(a4*a73); + a73=(-a73); + if (res[1]!=0) res[1][38]=a73; + a73=casadi_sq(a90); + a35=casadi_sq(a105); + a73=(a73+a35); + a105=(a105/a73); + a35=cos(a39); + a367=(a101*a35); + a39=sin(a39); + a49=(a102*a39); + a367=(a367+a49); + a367=(a105*a367); + a90=(a90/a73); + a73=(a106*a35); + a49=(a104*a39); + a73=(a73-a49); + a73=(a90*a73); + a367=(a367-a73); + if (res[1]!=0) res[1][39]=a367; + if (res[1]!=0) res[1][40]=a326; + if (res[1]!=0) res[1][41]=a323; + a62=(-a62); + if (res[1]!=0) res[1][42]=a62; + if (res[1]!=0) res[1][43]=a326; + if (res[1]!=0) res[1][44]=a323; + if (res[1]!=0) res[1][45]=a59; + a59=sin(a97); + a59=(a93*a59); + a62=cos(a97); + a62=(a103*a62); + a59=(a59+a62); + a59=(a105*a59); + a62=cos(a97); + a93=(a93*a62); + a62=sin(a97); + a103=(a103*a62); + a93=(a93-a103); + a93=(a90*a93); + a59=(a59+a93); + a59=(-a59); + if (res[1]!=0) res[1][46]=a59; + a59=sin(a97); + a59=(a108*a59); + a93=sin(a112); + a103=(a108*a93); + a103=(a8*a103); + a59=(a59+a103); + a103=sin(a115); + a62=(a108*a103); + a62=(a8*a62); + a59=(a59+a62); + a62=sin(a118); + a367=(a108*a62); + a59=(a59+a367); + a59=(a4*a59); + if (res[1]!=0) res[1][47]=a59; + a97=cos(a97); + a97=(a108*a97); + a112=cos(a112); + a59=(a108*a112); + a59=(a8*a59); + a97=(a97+a59); + a115=cos(a115); + a59=(a108*a115); + a59=(a8*a59); + a97=(a97+a59); + a118=cos(a118); + a59=(a108*a118); + a97=(a97+a59); + a97=(a4*a97); + a97=(-a97); + if (res[1]!=0) res[1][48]=a97; + a97=casadi_sq(a114); + a59=casadi_sq(a129); + a97=(a97+a59); + a129=(a129/a97); + a59=cos(a63); + a367=(a125*a59); + a63=sin(a63); + a73=(a126*a63); + a367=(a367+a73); + a367=(a129*a367); + a114=(a114/a97); + a97=(a130*a59); + a73=(a128*a63); + a97=(a97-a73); + a97=(a114*a97); + a367=(a367-a97); + if (res[1]!=0) res[1][49]=a367; + if (res[1]!=0) res[1][50]=a326; + if (res[1]!=0) res[1][51]=a323; + a86=(-a86); + if (res[1]!=0) res[1][52]=a86; + if (res[1]!=0) res[1][53]=a326; + if (res[1]!=0) res[1][54]=a323; + if (res[1]!=0) res[1][55]=a83; + a83=sin(a121); + a83=(a117*a83); + a86=cos(a121); + a86=(a127*a86); + a83=(a83+a86); + a83=(a129*a83); + a86=cos(a121); + a117=(a117*a86); + a86=sin(a121); + a127=(a127*a86); + a117=(a117-a127); + a117=(a114*a117); + a83=(a83+a117); + a83=(-a83); + if (res[1]!=0) res[1][56]=a83; + a83=sin(a121); + a83=(a132*a83); + a117=sin(a136); + a127=(a132*a117); + a127=(a8*a127); + a83=(a83+a127); + a127=sin(a139); + a86=(a132*a127); + a86=(a8*a86); + a83=(a83+a86); + a86=sin(a142); + a367=(a132*a86); + a83=(a83+a367); + a83=(a4*a83); + if (res[1]!=0) res[1][57]=a83; + a121=cos(a121); + a121=(a132*a121); + a136=cos(a136); + a83=(a132*a136); + a83=(a8*a83); + a121=(a121+a83); + a139=cos(a139); + a83=(a132*a139); + a83=(a8*a83); + a121=(a121+a83); + a142=cos(a142); + a83=(a132*a142); + a121=(a121+a83); + a121=(a4*a121); + a121=(-a121); + if (res[1]!=0) res[1][58]=a121; + a121=casadi_sq(a138); + a83=casadi_sq(a153); + a121=(a121+a83); + a153=(a153/a121); + a83=cos(a87); + a367=(a149*a83); + a87=sin(a87); + a97=(a150*a87); + a367=(a367+a97); + a367=(a153*a367); + a138=(a138/a121); + a121=(a154*a83); + a97=(a152*a87); + a121=(a121-a97); + a121=(a138*a121); + a367=(a367-a121); + if (res[1]!=0) res[1][59]=a367; + if (res[1]!=0) res[1][60]=a326; + if (res[1]!=0) res[1][61]=a323; + a110=(-a110); + if (res[1]!=0) res[1][62]=a110; + if (res[1]!=0) res[1][63]=a326; + if (res[1]!=0) res[1][64]=a323; + if (res[1]!=0) res[1][65]=a107; + a107=sin(a145); + a107=(a141*a107); + a110=cos(a145); + a110=(a151*a110); + a107=(a107+a110); + a107=(a153*a107); + a110=cos(a145); + a141=(a141*a110); + a110=sin(a145); + a151=(a151*a110); + a141=(a141-a151); + a141=(a138*a141); + a107=(a107+a141); + a107=(-a107); + if (res[1]!=0) res[1][66]=a107; + a107=sin(a145); + a107=(a156*a107); + a141=sin(a160); + a151=(a156*a141); + a151=(a8*a151); + a107=(a107+a151); + a151=sin(a163); + a110=(a156*a151); + a110=(a8*a110); + a107=(a107+a110); + a110=sin(a166); + a367=(a156*a110); + a107=(a107+a367); + a107=(a4*a107); + if (res[1]!=0) res[1][67]=a107; + a145=cos(a145); + a145=(a156*a145); + a160=cos(a160); + a107=(a156*a160); + a107=(a8*a107); + a145=(a145+a107); + a163=cos(a163); + a107=(a156*a163); + a107=(a8*a107); + a145=(a145+a107); + a166=cos(a166); + a107=(a156*a166); + a145=(a145+a107); + a145=(a4*a145); + a145=(-a145); + if (res[1]!=0) res[1][68]=a145; + a145=casadi_sq(a162); + a107=casadi_sq(a177); + a145=(a145+a107); + a177=(a177/a145); + a107=cos(a111); + a367=(a173*a107); + a111=sin(a111); + a121=(a174*a111); + a367=(a367+a121); + a367=(a177*a367); + a162=(a162/a145); + a145=(a178*a107); + a121=(a176*a111); + a145=(a145-a121); + a145=(a162*a145); + a367=(a367-a145); + if (res[1]!=0) res[1][69]=a367; + if (res[1]!=0) res[1][70]=a326; + if (res[1]!=0) res[1][71]=a323; + a134=(-a134); + if (res[1]!=0) res[1][72]=a134; + if (res[1]!=0) res[1][73]=a326; + if (res[1]!=0) res[1][74]=a323; + if (res[1]!=0) res[1][75]=a131; + a131=sin(a169); + a131=(a165*a131); + a134=cos(a169); + a134=(a175*a134); + a131=(a131+a134); + a131=(a177*a131); + a134=cos(a169); + a165=(a165*a134); + a134=sin(a169); + a175=(a175*a134); + a165=(a165-a175); + a165=(a162*a165); + a131=(a131+a165); + a131=(-a131); + if (res[1]!=0) res[1][76]=a131; + a131=sin(a169); + a131=(a180*a131); + a165=sin(a184); + a175=(a180*a165); + a175=(a8*a175); + a131=(a131+a175); + a175=sin(a187); + a134=(a180*a175); + a134=(a8*a134); + a131=(a131+a134); + a134=sin(a190); + a367=(a180*a134); + a131=(a131+a367); + a131=(a4*a131); + if (res[1]!=0) res[1][77]=a131; + a169=cos(a169); + a169=(a180*a169); + a184=cos(a184); + a131=(a180*a184); + a131=(a8*a131); + a169=(a169+a131); + a187=cos(a187); + a131=(a180*a187); + a131=(a8*a131); + a169=(a169+a131); + a190=cos(a190); + a131=(a180*a190); + a169=(a169+a131); + a169=(a4*a169); + a169=(-a169); + if (res[1]!=0) res[1][78]=a169; + a169=casadi_sq(a186); + a131=casadi_sq(a201); + a169=(a169+a131); + a201=(a201/a169); + a131=cos(a135); + a367=(a197*a131); + a135=sin(a135); + a145=(a198*a135); + a367=(a367+a145); + a367=(a201*a367); + a186=(a186/a169); + a169=(a202*a131); + a145=(a200*a135); + a169=(a169-a145); + a169=(a186*a169); + a367=(a367-a169); + if (res[1]!=0) res[1][79]=a367; + if (res[1]!=0) res[1][80]=a326; + if (res[1]!=0) res[1][81]=a323; + a158=(-a158); + if (res[1]!=0) res[1][82]=a158; + if (res[1]!=0) res[1][83]=a326; + if (res[1]!=0) res[1][84]=a323; + if (res[1]!=0) res[1][85]=a155; + a155=sin(a193); + a155=(a189*a155); + a158=cos(a193); + a158=(a199*a158); + a155=(a155+a158); + a155=(a201*a155); + a158=cos(a193); + a189=(a189*a158); + a158=sin(a193); + a199=(a199*a158); + a189=(a189-a199); + a189=(a186*a189); + a155=(a155+a189); + a155=(-a155); + if (res[1]!=0) res[1][86]=a155; + a155=sin(a193); + a155=(a204*a155); + a189=sin(a208); + a199=(a204*a189); + a199=(a8*a199); + a155=(a155+a199); + a199=sin(a211); + a158=(a204*a199); + a158=(a8*a158); + a155=(a155+a158); + a158=sin(a214); + a367=(a204*a158); + a155=(a155+a367); + a155=(a4*a155); + if (res[1]!=0) res[1][87]=a155; + a193=cos(a193); + a193=(a204*a193); + a208=cos(a208); + a155=(a204*a208); + a155=(a8*a155); + a193=(a193+a155); + a211=cos(a211); + a155=(a204*a211); + a155=(a8*a155); + a193=(a193+a155); + a214=cos(a214); + a155=(a204*a214); + a193=(a193+a155); + a193=(a4*a193); + a193=(-a193); + if (res[1]!=0) res[1][88]=a193; + a193=casadi_sq(a210); + a155=casadi_sq(a225); + a193=(a193+a155); + a225=(a225/a193); + a155=cos(a159); + a367=(a221*a155); + a159=sin(a159); + a169=(a222*a159); + a367=(a367+a169); + a367=(a225*a367); + a210=(a210/a193); + a193=(a226*a155); + a169=(a224*a159); + a193=(a193-a169); + a193=(a210*a193); + a367=(a367-a193); + if (res[1]!=0) res[1][89]=a367; + if (res[1]!=0) res[1][90]=a326; + if (res[1]!=0) res[1][91]=a323; + a182=(-a182); + if (res[1]!=0) res[1][92]=a182; + if (res[1]!=0) res[1][93]=a326; + if (res[1]!=0) res[1][94]=a323; + if (res[1]!=0) res[1][95]=a179; + a179=sin(a217); + a179=(a213*a179); + a182=cos(a217); + a182=(a223*a182); + a179=(a179+a182); + a179=(a225*a179); + a182=cos(a217); + a213=(a213*a182); + a182=sin(a217); + a223=(a223*a182); + a213=(a213-a223); + a213=(a210*a213); + a179=(a179+a213); + a179=(-a179); + if (res[1]!=0) res[1][96]=a179; + a179=sin(a217); + a179=(a228*a179); + a213=sin(a232); + a223=(a228*a213); + a223=(a8*a223); + a179=(a179+a223); + a223=sin(a235); + a182=(a228*a223); + a182=(a8*a182); + a179=(a179+a182); + a182=sin(a238); + a367=(a228*a182); + a179=(a179+a367); + a179=(a4*a179); + if (res[1]!=0) res[1][97]=a179; + a217=cos(a217); + a217=(a228*a217); + a232=cos(a232); + a179=(a228*a232); + a179=(a8*a179); + a217=(a217+a179); + a235=cos(a235); + a179=(a228*a235); + a179=(a8*a179); + a217=(a217+a179); + a238=cos(a238); + a179=(a228*a238); + a217=(a217+a179); + a217=(a4*a217); + a217=(-a217); + if (res[1]!=0) res[1][98]=a217; + a217=casadi_sq(a234); + a179=casadi_sq(a249); + a217=(a217+a179); + a249=(a249/a217); + a179=cos(a183); + a367=(a245*a179); + a183=sin(a183); + a193=(a246*a183); + a367=(a367+a193); + a367=(a249*a367); + a234=(a234/a217); + a217=(a250*a179); + a193=(a248*a183); + a217=(a217-a193); + a217=(a234*a217); + a367=(a367-a217); + if (res[1]!=0) res[1][99]=a367; + if (res[1]!=0) res[1][100]=a326; + if (res[1]!=0) res[1][101]=a323; + a206=(-a206); + if (res[1]!=0) res[1][102]=a206; + if (res[1]!=0) res[1][103]=a326; + if (res[1]!=0) res[1][104]=a323; + if (res[1]!=0) res[1][105]=a203; + a203=sin(a241); + a203=(a237*a203); + a206=cos(a241); + a206=(a247*a206); + a203=(a203+a206); + a203=(a249*a203); + a206=cos(a241); + a237=(a237*a206); + a206=sin(a241); + a247=(a247*a206); + a237=(a237-a247); + a237=(a234*a237); + a203=(a203+a237); + a203=(-a203); + if (res[1]!=0) res[1][106]=a203; + a203=sin(a241); + a203=(a252*a203); + a237=sin(a256); + a247=(a252*a237); + a247=(a8*a247); + a203=(a203+a247); + a247=sin(a259); + a206=(a252*a247); + a206=(a8*a206); + a203=(a203+a206); + a206=sin(a262); + a367=(a252*a206); + a203=(a203+a367); + a203=(a4*a203); + if (res[1]!=0) res[1][107]=a203; + a241=cos(a241); + a241=(a252*a241); + a256=cos(a256); + a203=(a252*a256); + a203=(a8*a203); + a241=(a241+a203); + a259=cos(a259); + a203=(a252*a259); + a203=(a8*a203); + a241=(a241+a203); + a262=cos(a262); + a203=(a252*a262); + a241=(a241+a203); + a241=(a4*a241); + a241=(-a241); + if (res[1]!=0) res[1][108]=a241; + a241=casadi_sq(a258); + a203=casadi_sq(a273); + a241=(a241+a203); + a273=(a273/a241); + a203=cos(a207); + a367=(a269*a203); + a207=sin(a207); + a217=(a270*a207); + a367=(a367+a217); + a367=(a273*a367); + a258=(a258/a241); + a241=(a274*a203); + a217=(a272*a207); + a241=(a241-a217); + a241=(a258*a241); + a367=(a367-a241); + if (res[1]!=0) res[1][109]=a367; + if (res[1]!=0) res[1][110]=a326; + if (res[1]!=0) res[1][111]=a323; + a230=(-a230); + if (res[1]!=0) res[1][112]=a230; + if (res[1]!=0) res[1][113]=a326; + if (res[1]!=0) res[1][114]=a323; + if (res[1]!=0) res[1][115]=a227; + a227=sin(a265); + a227=(a261*a227); + a230=cos(a265); + a230=(a271*a230); + a227=(a227+a230); + a227=(a273*a227); + a230=cos(a265); + a261=(a261*a230); + a230=sin(a265); + a271=(a271*a230); + a261=(a261-a271); + a261=(a258*a261); + a227=(a227+a261); + a227=(-a227); + if (res[1]!=0) res[1][116]=a227; + a227=sin(a265); + a227=(a276*a227); + a261=sin(a280); + a271=(a276*a261); + a271=(a8*a271); + a227=(a227+a271); + a271=sin(a283); + a230=(a276*a271); + a230=(a8*a230); + a227=(a227+a230); + a230=sin(a286); + a367=(a276*a230); + a227=(a227+a367); + a227=(a4*a227); + if (res[1]!=0) res[1][117]=a227; + a265=cos(a265); + a265=(a276*a265); + a280=cos(a280); + a227=(a276*a280); + a227=(a8*a227); + a265=(a265+a227); + a283=cos(a283); + a227=(a276*a283); + a227=(a8*a227); + a265=(a265+a227); + a286=cos(a286); + a227=(a276*a286); + a265=(a265+a227); + a265=(a4*a265); + a265=(-a265); + if (res[1]!=0) res[1][118]=a265; + a265=casadi_sq(a282); + a227=casadi_sq(a297); + a265=(a265+a227); + a297=(a297/a265); + a227=cos(a231); + a367=(a293*a227); + a231=sin(a231); + a241=(a294*a231); + a367=(a367+a241); + a367=(a297*a367); + a282=(a282/a265); + a265=(a298*a227); + a241=(a296*a231); + a265=(a265-a241); + a265=(a282*a265); + a367=(a367-a265); + if (res[1]!=0) res[1][119]=a367; + if (res[1]!=0) res[1][120]=a326; + if (res[1]!=0) res[1][121]=a323; + a254=(-a254); + if (res[1]!=0) res[1][122]=a254; + if (res[1]!=0) res[1][123]=a326; + if (res[1]!=0) res[1][124]=a323; + if (res[1]!=0) res[1][125]=a251; + a251=sin(a289); + a251=(a285*a251); + a254=cos(a289); + a254=(a295*a254); + a251=(a251+a254); + a251=(a297*a251); + a254=cos(a289); + a285=(a285*a254); + a254=sin(a289); + a295=(a295*a254); + a285=(a285-a295); + a285=(a282*a285); + a251=(a251+a285); + a251=(-a251); + if (res[1]!=0) res[1][126]=a251; + a251=sin(a289); + a251=(a300*a251); + a285=sin(a304); + a295=(a300*a285); + a295=(a8*a295); + a251=(a251+a295); + a295=sin(a307); + a254=(a300*a295); + a254=(a8*a254); + a251=(a251+a254); + a254=sin(a310); + a367=(a300*a254); + a251=(a251+a367); + a251=(a4*a251); + if (res[1]!=0) res[1][127]=a251; + a289=cos(a289); + a289=(a300*a289); + a304=cos(a304); + a251=(a300*a304); + a251=(a8*a251); + a289=(a289+a251); + a307=cos(a307); + a251=(a300*a307); + a251=(a8*a251); + a289=(a289+a251); + a310=cos(a310); + a251=(a300*a310); + a289=(a289+a251); + a289=(a4*a289); + a289=(-a289); + if (res[1]!=0) res[1][128]=a289; + a289=casadi_sq(a306); + a251=casadi_sq(a321); + a289=(a289+a251); + a321=(a321/a289); + a251=cos(a255); + a367=(a317*a251); + a255=sin(a255); + a265=(a318*a255); + a367=(a367+a265); + a367=(a321*a367); + a306=(a306/a289); + a289=(a322*a251); + a265=(a320*a255); + a289=(a289-a265); + a289=(a306*a289); + a367=(a367-a289); + if (res[1]!=0) res[1][129]=a367; + if (res[1]!=0) res[1][130]=a326; + if (res[1]!=0) res[1][131]=a323; + a278=(-a278); + if (res[1]!=0) res[1][132]=a278; + if (res[1]!=0) res[1][133]=a326; + if (res[1]!=0) res[1][134]=a323; + if (res[1]!=0) res[1][135]=a275; + a275=sin(a313); + a275=(a309*a275); + a278=cos(a313); + a278=(a319*a278); + a275=(a275+a278); + a275=(a321*a275); + a278=cos(a313); + a309=(a309*a278); + a278=sin(a313); + a319=(a319*a278); + a309=(a309-a319); + a309=(a306*a309); + a275=(a275+a309); + a275=(-a275); + if (res[1]!=0) res[1][136]=a275; + a275=sin(a313); + a275=(a324*a275); + a309=sin(a328); + a319=(a324*a309); + a319=(a8*a319); + a275=(a275+a319); + a319=sin(a331); + a278=(a324*a319); + a278=(a8*a278); + a275=(a275+a278); + a278=sin(a334); + a367=(a324*a278); + a275=(a275+a367); + a275=(a4*a275); + if (res[1]!=0) res[1][137]=a275; + a313=cos(a313); + a313=(a324*a313); + a328=cos(a328); + a275=(a324*a328); + a275=(a8*a275); + a313=(a313+a275); + a331=cos(a331); + a275=(a324*a331); + a275=(a8*a275); + a313=(a313+a275); + a334=cos(a334); + a275=(a324*a334); + a313=(a313+a275); + a313=(a4*a313); + a313=(-a313); + if (res[1]!=0) res[1][138]=a313; + a313=casadi_sq(a330); + a275=casadi_sq(a345); + a313=(a313+a275); + a345=(a345/a313); + a275=cos(a279); + a367=(a341*a275); + a279=sin(a279); + a289=(a342*a279); + a367=(a367+a289); + a367=(a345*a367); + a330=(a330/a313); + a313=(a346*a275); + a289=(a344*a279); + a313=(a313-a289); + a313=(a330*a313); + a367=(a367-a313); + if (res[1]!=0) res[1][139]=a367; + if (res[1]!=0) res[1][140]=a326; + if (res[1]!=0) res[1][141]=a323; + a302=(-a302); + if (res[1]!=0) res[1][142]=a302; + if (res[1]!=0) res[1][143]=a326; + if (res[1]!=0) res[1][144]=a323; + if (res[1]!=0) res[1][145]=a299; + a299=sin(a337); + a299=(a333*a299); + a323=cos(a337); + a323=(a343*a323); + a299=(a299+a323); + a299=(a345*a299); + a323=cos(a337); + a333=(a333*a323); + a323=sin(a337); + a343=(a343*a323); + a333=(a333-a343); + a333=(a330*a333); + a299=(a299+a333); + a299=(-a299); + if (res[1]!=0) res[1][146]=a299; + a299=sin(a337); + a299=(a348*a299); + a333=sin(a352); + a343=(a348*a333); + a343=(a8*a343); + a299=(a299+a343); + a343=sin(a9); + a323=(a348*a343); + a323=(a8*a323); + a299=(a299+a323); + a323=sin(a20); + a302=(a348*a323); + a299=(a299+a302); + a299=(a4*a299); + if (res[1]!=0) res[1][147]=a299; + a337=cos(a337); + a337=(a348*a337); + a352=cos(a352); + a299=(a348*a352); + a299=(a8*a299); + a337=(a337+a299); + a9=cos(a9); + a299=(a348*a9); + a299=(a8*a299); + a337=(a337+a299); + a20=cos(a20); + a299=(a348*a20); + a337=(a337+a299); + a337=(a4*a337); + a337=(-a337); + if (res[1]!=0) res[1][148]=a337; + a337=casadi_sq(a13); + a299=casadi_sq(a354); + a337=(a337+a299); + a354=(a354/a337); + a299=cos(a303); + a302=(a358*a299); + a303=sin(a303); + a367=(a351*a303); + a302=(a302+a367); + a302=(a354*a302); + a13=(a13/a337); + a337=(a362*a299); + a367=(a356*a303); + a337=(a337-a367); + a337=(a13*a337); + a302=(a302-a337); + if (res[1]!=0) res[1][149]=a302; + if (res[1]!=0) res[1][150]=a326; + if (res[1]!=0) res[1][151]=a326; + a326=sin(a347); + a326=(a10*a326); + a302=cos(a347); + a302=(a327*a302); + a326=(a326+a302); + a326=(a354*a326); + a302=cos(a347); + a10=(a10*a302); + a347=sin(a347); + a327=(a327*a347); + a10=(a10-a327); + a10=(a13*a10); + a326=(a326+a10); + a326=(-a326); + if (res[1]!=0) res[1][152]=a326; + a16=(a8*a16); + a6=(a6+a16); + a19=(a8*a19); + a6=(a6+a19); + a6=(a6+a23); + a6=(a4*a6); + a6=(-a6); + if (res[1]!=0) res[1][153]=a6; + a26=(a8*a26); + a24=(a24+a26); + a27=(a8*a27); + a24=(a24+a27); + a24=(a24+a28); + a24=(a4*a24); + a24=(-a24); + if (res[1]!=0) res[1][154]=a24; + a24=7.4999999999999997e-02; + a363=(a24*a363); + a363=(a5*a363); + a363=(a8*a363); + a365=(a24*a365); + a365=(a5*a365); + a365=(a8*a365); + a363=(a363+a365); + a365=1.4999999999999999e-01; + a366=(a365*a366); + a366=(a5*a366); + a363=(a363+a366); + a363=(a4*a363); + if (res[1]!=0) res[1][155]=a363; + a15=(a24*a15); + a15=(a5*a15); + a15=(a8*a15); + a18=(a24*a18); + a18=(a5*a18); + a18=(a8*a18); + a15=(a15+a18); + a22=(a365*a22); + a5=(a5*a22); + a15=(a15+a5); + a15=(a4*a15); + a15=(-a15); + if (res[1]!=0) res[1][156]=a15; + a364=(a365*a364); + a29=(a29*a364); + a12=(a365*a12); + a30=(a30*a12); + a29=(a29+a30); + a33=(a33*a29); + a34=(a34*a364); + a32=(a32*a12); + a34=(a34-a32); + a17=(a17*a34); + a33=(a33-a17); + if (res[1]!=0) res[1][157]=a33; + a33=2.9999999999999999e-02; + a17=(a33*a21); + a17=(a36*a17); + a17=(a8*a17); + a34=(a33*a31); + a34=(a36*a34); + a34=(a8*a34); + a17=(a17+a34); + a34=5.9999999999999998e-02; + a32=(a34*a1); + a32=(a36*a32); + a17=(a17+a32); + a17=(a4*a17); + if (res[1]!=0) res[1][158]=a17; + a17=(a33*a40); + a17=(a36*a17); + a17=(a8*a17); + a32=(a33*a43); + a32=(a36*a32); + a32=(a8*a32); + a17=(a17+a32); + a32=(a34*a46); + a32=(a36*a32); + a17=(a17+a32); + a17=(a4*a17); + a17=(-a17); + if (res[1]!=0) res[1][159]=a17; + a17=(a34*a0); + a32=(a53*a17); + a12=(a34*a11); + a364=(a54*a12); + a32=(a32+a364); + a32=(a57*a32); + a17=(a58*a17); + a12=(a56*a12); + a17=(a17-a12); + a17=(a42*a17); + a32=(a32-a17); + if (res[1]!=0) res[1][160]=a32; + a32=1.2000000000000000e-02; + a17=(a32*a45); + a17=(a60*a17); + a17=(a8*a17); + a12=(a32*a55); + a12=(a60*a12); + a12=(a8*a12); + a17=(a17+a12); + a12=2.4000000000000000e-02; + a364=(a12*a7); + a364=(a60*a364); + a17=(a17+a364); + a17=(a4*a17); + if (res[1]!=0) res[1][161]=a17; + a17=(a32*a64); + a17=(a60*a17); + a17=(a8*a17); + a364=(a32*a67); + a364=(a60*a364); + a364=(a8*a364); + a17=(a17+a364); + a364=(a12*a70); + a364=(a60*a364); + a17=(a17+a364); + a17=(a4*a17); + a17=(-a17); + if (res[1]!=0) res[1][162]=a17; + a17=(a12*a3); + a364=(a77*a17); + a29=(a12*a14); + a30=(a78*a29); + a364=(a364+a30); + a364=(a81*a364); + a17=(a82*a17); + a29=(a80*a29); + a17=(a17-a29); + a17=(a66*a17); + a364=(a364-a17); + if (res[1]!=0) res[1][163]=a364; + a364=4.8000000000000004e-03; + a17=(a364*a69); + a17=(a84*a17); + a17=(a8*a17); + a29=(a364*a79); + a29=(a84*a29); + a29=(a8*a29); + a17=(a17+a29); + a29=9.6000000000000009e-03; + a30=(a29*a38); + a30=(a84*a30); + a17=(a17+a30); + a17=(a4*a17); + if (res[1]!=0) res[1][164]=a17; + a17=(a364*a88); + a17=(a84*a17); + a17=(a8*a17); + a30=(a364*a91); + a30=(a84*a30); + a30=(a8*a30); + a17=(a17+a30); + a30=(a29*a94); + a30=(a84*a30); + a17=(a17+a30); + a17=(a4*a17); + a17=(-a17); + if (res[1]!=0) res[1][165]=a17; + a17=9.5999999999999992e-03; + a30=(a17*a35); + a15=(a101*a30); + a5=(a17*a39); + a22=(a102*a5); + a15=(a15+a22); + a15=(a105*a15); + a30=(a106*a30); + a5=(a104*a5); + a30=(a30-a5); + a30=(a90*a30); + a15=(a15-a30); + if (res[1]!=0) res[1][166]=a15; + a15=1.9200000000000003e-03; + a30=(a15*a93); + a30=(a108*a30); + a30=(a8*a30); + a5=(a15*a103); + a5=(a108*a5); + a5=(a8*a5); + a30=(a30+a5); + a5=3.8400000000000005e-03; + a22=(a5*a62); + a22=(a108*a22); + a30=(a30+a22); + a30=(a4*a30); + if (res[1]!=0) res[1][167]=a30; + a30=(a15*a112); + a30=(a108*a30); + a30=(a8*a30); + a22=(a15*a115); + a22=(a108*a22); + a22=(a8*a22); + a30=(a30+a22); + a22=(a5*a118); + a22=(a108*a22); + a30=(a30+a22); + a30=(a4*a30); + a30=(-a30); + if (res[1]!=0) res[1][168]=a30; + a30=3.8400000000000001e-03; + a22=(a30*a59); + a18=(a125*a22); + a363=(a30*a63); + a366=(a126*a363); + a18=(a18+a366); + a18=(a129*a18); + a22=(a130*a22); + a363=(a128*a363); + a22=(a22-a363); + a22=(a114*a22); + a18=(a18-a22); + if (res[1]!=0) res[1][169]=a18; + a18=7.6800000000000013e-04; + a22=(a18*a117); + a22=(a132*a22); + a22=(a8*a22); + a363=(a18*a127); + a363=(a132*a363); + a363=(a8*a363); + a22=(a22+a363); + a363=1.5360000000000003e-03; + a366=(a363*a86); + a366=(a132*a366); + a22=(a22+a366); + a22=(a4*a22); + if (res[1]!=0) res[1][170]=a22; + a22=(a18*a136); + a22=(a132*a22); + a22=(a8*a22); + a366=(a18*a139); + a366=(a132*a366); + a366=(a8*a366); + a22=(a22+a366); + a366=(a363*a142); + a366=(a132*a366); + a22=(a22+a366); + a22=(a4*a22); + a22=(-a22); + if (res[1]!=0) res[1][171]=a22; + a22=(a363*a83); + a366=(a149*a22); + a28=(a363*a87); + a27=(a150*a28); + a366=(a366+a27); + a366=(a153*a366); + a22=(a154*a22); + a28=(a152*a28); + a22=(a22-a28); + a22=(a138*a22); + a366=(a366-a22); + if (res[1]!=0) res[1][172]=a366; + a366=3.0720000000000009e-04; + a22=(a366*a141); + a22=(a156*a22); + a22=(a8*a22); + a28=(a366*a151); + a28=(a156*a28); + a28=(a8*a28); + a22=(a22+a28); + a28=6.1440000000000019e-04; + a27=(a28*a110); + a27=(a156*a27); + a22=(a22+a27); + a22=(a4*a22); + if (res[1]!=0) res[1][173]=a22; + a22=(a366*a160); + a22=(a156*a22); + a22=(a8*a22); + a27=(a366*a163); + a27=(a156*a27); + a27=(a8*a27); + a22=(a22+a27); + a27=(a28*a166); + a27=(a156*a27); + a22=(a22+a27); + a22=(a4*a22); + a22=(-a22); + if (res[1]!=0) res[1][174]=a22; + a22=(a28*a107); + a27=(a173*a22); + a26=(a28*a111); + a6=(a174*a26); + a27=(a27+a6); + a27=(a177*a27); + a22=(a178*a22); + a26=(a176*a26); + a22=(a22-a26); + a22=(a162*a22); + a27=(a27-a22); + if (res[1]!=0) res[1][175]=a27; + a27=1.2288000000000004e-04; + a22=(a27*a165); + a22=(a180*a22); + a22=(a8*a22); + a26=(a27*a175); + a26=(a180*a26); + a26=(a8*a26); + a22=(a22+a26); + a26=2.4576000000000009e-04; + a6=(a26*a134); + a6=(a180*a6); + a22=(a22+a6); + a22=(a4*a22); + if (res[1]!=0) res[1][176]=a22; + a22=(a27*a184); + a22=(a180*a22); + a22=(a8*a22); + a6=(a27*a187); + a6=(a180*a6); + a6=(a8*a6); + a22=(a22+a6); + a6=(a26*a190); + a6=(a180*a6); + a22=(a22+a6); + a22=(a4*a22); + a22=(-a22); + if (res[1]!=0) res[1][177]=a22; + a22=2.4576000000000003e-04; + a6=(a22*a131); + a23=(a197*a6); + a19=(a22*a135); + a16=(a198*a19); + a23=(a23+a16); + a23=(a201*a23); + a6=(a202*a6); + a19=(a200*a19); + a6=(a6-a19); + a6=(a186*a6); + a23=(a23-a6); + if (res[1]!=0) res[1][178]=a23; + a23=4.9152000000000020e-05; + a6=(a23*a189); + a6=(a204*a6); + a6=(a8*a6); + a19=(a23*a199); + a19=(a204*a19); + a19=(a8*a19); + a6=(a6+a19); + a19=9.8304000000000040e-05; + a16=(a19*a158); + a16=(a204*a16); + a6=(a6+a16); + a6=(a4*a6); + if (res[1]!=0) res[1][179]=a6; + a6=(a23*a208); + a6=(a204*a6); + a6=(a8*a6); + a16=(a23*a211); + a16=(a204*a16); + a16=(a8*a16); + a6=(a6+a16); + a16=(a19*a214); + a16=(a204*a16); + a6=(a6+a16); + a6=(a4*a6); + a6=(-a6); + if (res[1]!=0) res[1][180]=a6; + a6=(a19*a155); + a16=(a221*a6); + a326=(a19*a159); + a10=(a222*a326); + a16=(a16+a10); + a16=(a225*a16); + a6=(a226*a6); + a326=(a224*a326); + a6=(a6-a326); + a6=(a210*a6); + a16=(a16-a6); + if (res[1]!=0) res[1][181]=a16; + a16=1.9660800000000010e-05; + a6=(a16*a213); + a6=(a228*a6); + a6=(a8*a6); + a326=(a16*a223); + a326=(a228*a326); + a326=(a8*a326); + a6=(a6+a326); + a326=3.9321600000000020e-05; + a10=(a326*a182); + a10=(a228*a10); + a6=(a6+a10); + a6=(a4*a6); + if (res[1]!=0) res[1][182]=a6; + a6=(a16*a232); + a6=(a228*a6); + a6=(a8*a6); + a10=(a16*a235); + a10=(a228*a10); + a10=(a8*a10); + a6=(a6+a10); + a10=(a326*a238); + a10=(a228*a10); + a6=(a6+a10); + a6=(a4*a6); + a6=(-a6); + if (res[1]!=0) res[1][183]=a6; + a6=3.9321600000000013e-05; + a10=(a6*a179); + a327=(a245*a10); + a347=(a6*a183); + a302=(a246*a347); + a327=(a327+a302); + a327=(a249*a327); + a10=(a250*a10); + a347=(a248*a347); + a10=(a10-a347); + a10=(a234*a10); + a327=(a327-a10); + if (res[1]!=0) res[1][184]=a327; + a327=7.8643200000000050e-06; + a10=(a327*a237); + a10=(a252*a10); + a10=(a8*a10); + a347=(a327*a247); + a347=(a252*a347); + a347=(a8*a347); + a10=(a10+a347); + a347=1.5728640000000010e-05; + a302=(a347*a206); + a302=(a252*a302); + a10=(a10+a302); + a10=(a4*a10); + if (res[1]!=0) res[1][185]=a10; + a10=(a327*a256); + a10=(a252*a10); + a10=(a8*a10); + a302=(a327*a259); + a302=(a252*a302); + a302=(a8*a302); + a10=(a10+a302); + a302=(a347*a262); + a302=(a252*a302); + a10=(a10+a302); + a10=(a4*a10); + a10=(-a10); + if (res[1]!=0) res[1][186]=a10; + a10=(a347*a203); + a302=(a269*a10); + a337=(a347*a207); + a367=(a270*a337); + a302=(a302+a367); + a302=(a273*a302); + a10=(a274*a10); + a337=(a272*a337); + a10=(a10-a337); + a10=(a258*a10); + a302=(a302-a10); + if (res[1]!=0) res[1][187]=a302; + a302=3.1457280000000023e-06; + a10=(a302*a261); + a10=(a276*a10); + a10=(a8*a10); + a337=(a302*a271); + a337=(a276*a337); + a337=(a8*a337); + a10=(a10+a337); + a337=6.2914560000000045e-06; + a367=(a337*a230); + a367=(a276*a367); + a10=(a10+a367); + a10=(a4*a10); + if (res[1]!=0) res[1][188]=a10; + a10=(a302*a280); + a10=(a276*a10); + a10=(a8*a10); + a367=(a302*a283); + a367=(a276*a367); + a367=(a8*a367); + a10=(a10+a367); + a367=(a337*a286); + a367=(a276*a367); + a10=(a10+a367); + a10=(a4*a10); + a10=(-a10); + if (res[1]!=0) res[1][189]=a10; + a10=6.2914560000000037e-06; + a367=(a10*a227); + a313=(a293*a367); + a289=(a10*a231); + a265=(a294*a289); + a313=(a313+a265); + a313=(a297*a313); + a367=(a298*a367); + a289=(a296*a289); + a367=(a367-a289); + a367=(a282*a367); + a313=(a313-a367); + if (res[1]!=0) res[1][190]=a313; + a313=1.2582912000000010e-06; + a367=(a313*a285); + a367=(a300*a367); + a367=(a8*a367); + a289=(a313*a295); + a289=(a300*a289); + a289=(a8*a289); + a367=(a367+a289); + a289=2.5165824000000020e-06; + a265=(a289*a254); + a265=(a300*a265); + a367=(a367+a265); + a367=(a4*a367); + if (res[1]!=0) res[1][191]=a367; + a367=(a313*a304); + a367=(a300*a367); + a367=(a8*a367); + a265=(a313*a307); + a265=(a300*a265); + a265=(a8*a265); + a367=(a367+a265); + a265=(a289*a310); + a265=(a300*a265); + a367=(a367+a265); + a367=(a4*a367); + a367=(-a367); + if (res[1]!=0) res[1][192]=a367; + a367=(a289*a251); + a265=(a317*a367); + a241=(a289*a255); + a217=(a318*a241); + a265=(a265+a217); + a265=(a321*a265); + a367=(a322*a367); + a241=(a320*a241); + a367=(a367-a241); + a367=(a306*a367); + a265=(a265-a367); + if (res[1]!=0) res[1][193]=a265; + a265=5.0331648000000040e-07; + a367=(a265*a309); + a367=(a324*a367); + a367=(a8*a367); + a241=(a265*a319); + a241=(a324*a241); + a241=(a8*a241); + a367=(a367+a241); + a241=1.0066329600000008e-06; + a217=(a241*a278); + a217=(a324*a217); + a367=(a367+a217); + a367=(a4*a367); + if (res[1]!=0) res[1][194]=a367; + a367=(a265*a328); + a367=(a324*a367); + a367=(a8*a367); + a217=(a265*a331); + a217=(a324*a217); + a217=(a8*a217); + a367=(a367+a217); + a217=(a241*a334); + a217=(a324*a217); + a367=(a367+a217); + a367=(a4*a367); + a367=(-a367); + if (res[1]!=0) res[1][195]=a367; + a367=(a241*a275); + a217=(a341*a367); + a193=(a241*a279); + a169=(a342*a193); + a217=(a217+a169); + a217=(a345*a217); + a367=(a346*a367); + a193=(a344*a193); + a367=(a367-a193); + a367=(a330*a367); + a217=(a217-a367); + if (res[1]!=0) res[1][196]=a217; + a217=2.0132659200000017e-07; + a367=(a217*a333); + a367=(a348*a367); + a367=(a8*a367); + a193=(a217*a343); + a193=(a348*a193); + a193=(a8*a193); + a367=(a367+a193); + a193=4.0265318400000035e-07; + a169=(a193*a323); + a169=(a348*a169); + a367=(a367+a169); + a367=(a4*a367); + if (res[1]!=0) res[1][197]=a367; + a367=(a217*a352); + a367=(a348*a367); + a367=(a8*a367); + a217=(a217*a9); + a217=(a348*a217); + a217=(a8*a217); + a367=(a367+a217); + a193=(a193*a20); + a193=(a348*a193); + a367=(a367+a193); + a367=(a4*a367); + a367=(-a367); + if (res[1]!=0) res[1][198]=a367; + a367=4.0265318400000030e-07; + a193=(a367*a299); + a217=(a358*a193); + a367=(a367*a303); + a169=(a351*a367); + a217=(a217+a169); + a217=(a354*a217); + a193=(a362*a193); + a367=(a356*a367); + a193=(a193-a367); + a193=(a13*a193); + a217=(a217-a193); + if (res[1]!=0) res[1][199]=a217; + a41=(a8*a41); + a37=(a37+a41); + a44=(a8*a44); + a37=(a37+a44); + a37=(a37+a47); + a37=(a4*a37); + a37=(-a37); + if (res[1]!=0) res[1][200]=a37; + a50=(a8*a50); + a48=(a48+a50); + a51=(a8*a51); + a48=(a48+a51); + a48=(a48+a52); + a48=(a4*a48); + a48=(-a48); + if (res[1]!=0) res[1][201]=a48; + a21=(a24*a21); + a21=(a36*a21); + a21=(a8*a21); + a31=(a24*a31); + a31=(a36*a31); + a31=(a8*a31); + a21=(a21+a31); + a1=(a365*a1); + a1=(a36*a1); + a21=(a21+a1); + a21=(a4*a21); + if (res[1]!=0) res[1][202]=a21; + a40=(a24*a40); + a40=(a36*a40); + a40=(a8*a40); + a43=(a24*a43); + a43=(a36*a43); + a43=(a8*a43); + a40=(a40+a43); + a46=(a365*a46); + a36=(a36*a46); + a40=(a40+a36); + a40=(a4*a40); + a40=(-a40); + if (res[1]!=0) res[1][203]=a40; + a0=(a365*a0); + a53=(a53*a0); + a11=(a365*a11); + a54=(a54*a11); + a53=(a53+a54); + a57=(a57*a53); + a58=(a58*a0); + a56=(a56*a11); + a58=(a58-a56); + a42=(a42*a58); + a57=(a57-a42); + if (res[1]!=0) res[1][204]=a57; + a57=(a33*a45); + a57=(a60*a57); + a57=(a8*a57); + a42=(a33*a55); + a42=(a60*a42); + a42=(a8*a42); + a57=(a57+a42); + a42=(a34*a7); + a42=(a60*a42); + a57=(a57+a42); + a57=(a4*a57); + if (res[1]!=0) res[1][205]=a57; + a57=(a33*a64); + a57=(a60*a57); + a57=(a8*a57); + a42=(a33*a67); + a42=(a60*a42); + a42=(a8*a42); + a57=(a57+a42); + a42=(a34*a70); + a42=(a60*a42); + a57=(a57+a42); + a57=(a4*a57); + a57=(-a57); + if (res[1]!=0) res[1][206]=a57; + a57=(a34*a3); + a42=(a77*a57); + a58=(a34*a14); + a56=(a78*a58); + a42=(a42+a56); + a42=(a81*a42); + a57=(a82*a57); + a58=(a80*a58); + a57=(a57-a58); + a57=(a66*a57); + a42=(a42-a57); + if (res[1]!=0) res[1][207]=a42; + a42=(a32*a69); + a42=(a84*a42); + a42=(a8*a42); + a57=(a32*a79); + a57=(a84*a57); + a57=(a8*a57); + a42=(a42+a57); + a57=(a12*a38); + a57=(a84*a57); + a42=(a42+a57); + a42=(a4*a42); + if (res[1]!=0) res[1][208]=a42; + a42=(a32*a88); + a42=(a84*a42); + a42=(a8*a42); + a57=(a32*a91); + a57=(a84*a57); + a57=(a8*a57); + a42=(a42+a57); + a57=(a12*a94); + a57=(a84*a57); + a42=(a42+a57); + a42=(a4*a42); + a42=(-a42); + if (res[1]!=0) res[1][209]=a42; + a42=(a12*a35); + a57=(a101*a42); + a58=(a12*a39); + a56=(a102*a58); + a57=(a57+a56); + a57=(a105*a57); + a42=(a106*a42); + a58=(a104*a58); + a42=(a42-a58); + a42=(a90*a42); + a57=(a57-a42); + if (res[1]!=0) res[1][210]=a57; + a57=(a364*a93); + a57=(a108*a57); + a57=(a8*a57); + a42=(a364*a103); + a42=(a108*a42); + a42=(a8*a42); + a57=(a57+a42); + a42=(a29*a62); + a42=(a108*a42); + a57=(a57+a42); + a57=(a4*a57); + if (res[1]!=0) res[1][211]=a57; + a57=(a364*a112); + a57=(a108*a57); + a57=(a8*a57); + a42=(a364*a115); + a42=(a108*a42); + a42=(a8*a42); + a57=(a57+a42); + a42=(a29*a118); + a42=(a108*a42); + a57=(a57+a42); + a57=(a4*a57); + a57=(-a57); + if (res[1]!=0) res[1][212]=a57; + a57=(a17*a59); + a42=(a125*a57); + a58=(a17*a63); + a56=(a126*a58); + a42=(a42+a56); + a42=(a129*a42); + a57=(a130*a57); + a58=(a128*a58); + a57=(a57-a58); + a57=(a114*a57); + a42=(a42-a57); + if (res[1]!=0) res[1][213]=a42; + a42=(a15*a117); + a42=(a132*a42); + a42=(a8*a42); + a57=(a15*a127); + a57=(a132*a57); + a57=(a8*a57); + a42=(a42+a57); + a57=(a5*a86); + a57=(a132*a57); + a42=(a42+a57); + a42=(a4*a42); + if (res[1]!=0) res[1][214]=a42; + a42=(a15*a136); + a42=(a132*a42); + a42=(a8*a42); + a57=(a15*a139); + a57=(a132*a57); + a57=(a8*a57); + a42=(a42+a57); + a57=(a5*a142); + a57=(a132*a57); + a42=(a42+a57); + a42=(a4*a42); + a42=(-a42); + if (res[1]!=0) res[1][215]=a42; + a42=(a30*a83); + a57=(a149*a42); + a58=(a30*a87); + a56=(a150*a58); + a57=(a57+a56); + a57=(a153*a57); + a42=(a154*a42); + a58=(a152*a58); + a42=(a42-a58); + a42=(a138*a42); + a57=(a57-a42); + if (res[1]!=0) res[1][216]=a57; + a57=(a18*a141); + a57=(a156*a57); + a57=(a8*a57); + a42=(a18*a151); + a42=(a156*a42); + a42=(a8*a42); + a57=(a57+a42); + a42=(a363*a110); + a42=(a156*a42); + a57=(a57+a42); + a57=(a4*a57); + if (res[1]!=0) res[1][217]=a57; + a57=(a18*a160); + a57=(a156*a57); + a57=(a8*a57); + a42=(a18*a163); + a42=(a156*a42); + a42=(a8*a42); + a57=(a57+a42); + a42=(a363*a166); + a42=(a156*a42); + a57=(a57+a42); + a57=(a4*a57); + a57=(-a57); + if (res[1]!=0) res[1][218]=a57; + a57=(a363*a107); + a42=(a173*a57); + a58=(a363*a111); + a56=(a174*a58); + a42=(a42+a56); + a42=(a177*a42); + a57=(a178*a57); + a58=(a176*a58); + a57=(a57-a58); + a57=(a162*a57); + a42=(a42-a57); + if (res[1]!=0) res[1][219]=a42; + a42=(a366*a165); + a42=(a180*a42); + a42=(a8*a42); + a57=(a366*a175); + a57=(a180*a57); + a57=(a8*a57); + a42=(a42+a57); + a57=(a28*a134); + a57=(a180*a57); + a42=(a42+a57); + a42=(a4*a42); + if (res[1]!=0) res[1][220]=a42; + a42=(a366*a184); + a42=(a180*a42); + a42=(a8*a42); + a57=(a366*a187); + a57=(a180*a57); + a57=(a8*a57); + a42=(a42+a57); + a57=(a28*a190); + a57=(a180*a57); + a42=(a42+a57); + a42=(a4*a42); + a42=(-a42); + if (res[1]!=0) res[1][221]=a42; + a42=(a28*a131); + a57=(a197*a42); + a58=(a28*a135); + a56=(a198*a58); + a57=(a57+a56); + a57=(a201*a57); + a42=(a202*a42); + a58=(a200*a58); + a42=(a42-a58); + a42=(a186*a42); + a57=(a57-a42); + if (res[1]!=0) res[1][222]=a57; + a57=(a27*a189); + a57=(a204*a57); + a57=(a8*a57); + a42=(a27*a199); + a42=(a204*a42); + a42=(a8*a42); + a57=(a57+a42); + a42=(a26*a158); + a42=(a204*a42); + a57=(a57+a42); + a57=(a4*a57); + if (res[1]!=0) res[1][223]=a57; + a57=(a27*a208); + a57=(a204*a57); + a57=(a8*a57); + a42=(a27*a211); + a42=(a204*a42); + a42=(a8*a42); + a57=(a57+a42); + a42=(a26*a214); + a42=(a204*a42); + a57=(a57+a42); + a57=(a4*a57); + a57=(-a57); + if (res[1]!=0) res[1][224]=a57; + a57=(a22*a155); + a42=(a221*a57); + a58=(a22*a159); + a56=(a222*a58); + a42=(a42+a56); + a42=(a225*a42); + a57=(a226*a57); + a58=(a224*a58); + a57=(a57-a58); + a57=(a210*a57); + a42=(a42-a57); + if (res[1]!=0) res[1][225]=a42; + a42=(a23*a213); + a42=(a228*a42); + a42=(a8*a42); + a57=(a23*a223); + a57=(a228*a57); + a57=(a8*a57); + a42=(a42+a57); + a57=(a19*a182); + a57=(a228*a57); + a42=(a42+a57); + a42=(a4*a42); + if (res[1]!=0) res[1][226]=a42; + a42=(a23*a232); + a42=(a228*a42); + a42=(a8*a42); + a57=(a23*a235); + a57=(a228*a57); + a57=(a8*a57); + a42=(a42+a57); + a57=(a19*a238); + a57=(a228*a57); + a42=(a42+a57); + a42=(a4*a42); + a42=(-a42); + if (res[1]!=0) res[1][227]=a42; + a42=(a19*a179); + a57=(a245*a42); + a58=(a19*a183); + a56=(a246*a58); + a57=(a57+a56); + a57=(a249*a57); + a42=(a250*a42); + a58=(a248*a58); + a42=(a42-a58); + a42=(a234*a42); + a57=(a57-a42); + if (res[1]!=0) res[1][228]=a57; + a57=(a16*a237); + a57=(a252*a57); + a57=(a8*a57); + a42=(a16*a247); + a42=(a252*a42); + a42=(a8*a42); + a57=(a57+a42); + a42=(a326*a206); + a42=(a252*a42); + a57=(a57+a42); + a57=(a4*a57); + if (res[1]!=0) res[1][229]=a57; + a57=(a16*a256); + a57=(a252*a57); + a57=(a8*a57); + a42=(a16*a259); + a42=(a252*a42); + a42=(a8*a42); + a57=(a57+a42); + a42=(a326*a262); + a42=(a252*a42); + a57=(a57+a42); + a57=(a4*a57); + a57=(-a57); + if (res[1]!=0) res[1][230]=a57; + a57=(a6*a203); + a42=(a269*a57); + a58=(a6*a207); + a56=(a270*a58); + a42=(a42+a56); + a42=(a273*a42); + a57=(a274*a57); + a58=(a272*a58); + a57=(a57-a58); + a57=(a258*a57); + a42=(a42-a57); + if (res[1]!=0) res[1][231]=a42; + a42=(a327*a261); + a42=(a276*a42); + a42=(a8*a42); + a57=(a327*a271); + a57=(a276*a57); + a57=(a8*a57); + a42=(a42+a57); + a57=(a347*a230); + a57=(a276*a57); + a42=(a42+a57); + a42=(a4*a42); + if (res[1]!=0) res[1][232]=a42; + a42=(a327*a280); + a42=(a276*a42); + a42=(a8*a42); + a57=(a327*a283); + a57=(a276*a57); + a57=(a8*a57); + a42=(a42+a57); + a57=(a347*a286); + a57=(a276*a57); + a42=(a42+a57); + a42=(a4*a42); + a42=(-a42); + if (res[1]!=0) res[1][233]=a42; + a42=(a347*a227); + a57=(a293*a42); + a58=(a347*a231); + a56=(a294*a58); + a57=(a57+a56); + a57=(a297*a57); + a42=(a298*a42); + a58=(a296*a58); + a42=(a42-a58); + a42=(a282*a42); + a57=(a57-a42); + if (res[1]!=0) res[1][234]=a57; + a57=(a302*a285); + a57=(a300*a57); + a57=(a8*a57); + a42=(a302*a295); + a42=(a300*a42); + a42=(a8*a42); + a57=(a57+a42); + a42=(a337*a254); + a42=(a300*a42); + a57=(a57+a42); + a57=(a4*a57); + if (res[1]!=0) res[1][235]=a57; + a57=(a302*a304); + a57=(a300*a57); + a57=(a8*a57); + a42=(a302*a307); + a42=(a300*a42); + a42=(a8*a42); + a57=(a57+a42); + a42=(a337*a310); + a42=(a300*a42); + a57=(a57+a42); + a57=(a4*a57); + a57=(-a57); + if (res[1]!=0) res[1][236]=a57; + a57=(a10*a251); + a42=(a317*a57); + a58=(a10*a255); + a56=(a318*a58); + a42=(a42+a56); + a42=(a321*a42); + a57=(a322*a57); + a58=(a320*a58); + a57=(a57-a58); + a57=(a306*a57); + a42=(a42-a57); + if (res[1]!=0) res[1][237]=a42; + a42=(a313*a309); + a42=(a324*a42); + a42=(a8*a42); + a57=(a313*a319); + a57=(a324*a57); + a57=(a8*a57); + a42=(a42+a57); + a57=(a289*a278); + a57=(a324*a57); + a42=(a42+a57); + a42=(a4*a42); + if (res[1]!=0) res[1][238]=a42; + a42=(a313*a328); + a42=(a324*a42); + a42=(a8*a42); + a57=(a313*a331); + a57=(a324*a57); + a57=(a8*a57); + a42=(a42+a57); + a57=(a289*a334); + a57=(a324*a57); + a42=(a42+a57); + a42=(a4*a42); + a42=(-a42); + if (res[1]!=0) res[1][239]=a42; + a42=(a289*a275); + a57=(a341*a42); + a58=(a289*a279); + a56=(a342*a58); + a57=(a57+a56); + a57=(a345*a57); + a42=(a346*a42); + a58=(a344*a58); + a42=(a42-a58); + a42=(a330*a42); + a57=(a57-a42); + if (res[1]!=0) res[1][240]=a57; + a57=(a265*a333); + a57=(a348*a57); + a57=(a8*a57); + a42=(a265*a343); + a42=(a348*a42); + a42=(a8*a42); + a57=(a57+a42); + a42=(a241*a323); + a42=(a348*a42); + a57=(a57+a42); + a57=(a4*a57); + if (res[1]!=0) res[1][241]=a57; + a57=(a265*a352); + a57=(a348*a57); + a57=(a8*a57); + a265=(a265*a9); + a265=(a348*a265); + a265=(a8*a265); + a57=(a57+a265); + a265=(a241*a20); + a265=(a348*a265); + a57=(a57+a265); + a57=(a4*a57); + a57=(-a57); + if (res[1]!=0) res[1][242]=a57; + a57=(a241*a299); + a265=(a358*a57); + a241=(a241*a303); + a42=(a351*a241); + a265=(a265+a42); + a265=(a354*a265); + a57=(a362*a57); + a241=(a356*a241); + a57=(a57-a241); + a57=(a13*a57); + a265=(a265-a57); + if (res[1]!=0) res[1][243]=a265; + a65=(a8*a65); + a61=(a61+a65); + a68=(a8*a68); + a61=(a61+a68); + a61=(a61+a71); + a61=(a4*a61); + a61=(-a61); + if (res[1]!=0) res[1][244]=a61; + a74=(a8*a74); + a72=(a72+a74); + a75=(a8*a75); + a72=(a72+a75); + a72=(a72+a76); + a72=(a4*a72); + a72=(-a72); + if (res[1]!=0) res[1][245]=a72; + a45=(a24*a45); + a45=(a60*a45); + a45=(a8*a45); + a55=(a24*a55); + a55=(a60*a55); + a55=(a8*a55); + a45=(a45+a55); + a7=(a365*a7); + a7=(a60*a7); + a45=(a45+a7); + a45=(a4*a45); + if (res[1]!=0) res[1][246]=a45; + a64=(a24*a64); + a64=(a60*a64); + a64=(a8*a64); + a67=(a24*a67); + a67=(a60*a67); + a67=(a8*a67); + a64=(a64+a67); + a70=(a365*a70); + a60=(a60*a70); + a64=(a64+a60); + a64=(a4*a64); + a64=(-a64); + if (res[1]!=0) res[1][247]=a64; + a3=(a365*a3); + a77=(a77*a3); + a14=(a365*a14); + a78=(a78*a14); + a77=(a77+a78); + a81=(a81*a77); + a82=(a82*a3); + a80=(a80*a14); + a82=(a82-a80); + a66=(a66*a82); + a81=(a81-a66); + if (res[1]!=0) res[1][248]=a81; + a81=(a33*a69); + a81=(a84*a81); + a81=(a8*a81); + a66=(a33*a79); + a66=(a84*a66); + a66=(a8*a66); + a81=(a81+a66); + a66=(a34*a38); + a66=(a84*a66); + a81=(a81+a66); + a81=(a4*a81); + if (res[1]!=0) res[1][249]=a81; + a81=(a33*a88); + a81=(a84*a81); + a81=(a8*a81); + a66=(a33*a91); + a66=(a84*a66); + a66=(a8*a66); + a81=(a81+a66); + a66=(a34*a94); + a66=(a84*a66); + a81=(a81+a66); + a81=(a4*a81); + a81=(-a81); + if (res[1]!=0) res[1][250]=a81; + a81=(a34*a35); + a66=(a101*a81); + a82=(a34*a39); + a80=(a102*a82); + a66=(a66+a80); + a66=(a105*a66); + a81=(a106*a81); + a82=(a104*a82); + a81=(a81-a82); + a81=(a90*a81); + a66=(a66-a81); + if (res[1]!=0) res[1][251]=a66; + a66=(a32*a93); + a66=(a108*a66); + a66=(a8*a66); + a81=(a32*a103); + a81=(a108*a81); + a81=(a8*a81); + a66=(a66+a81); + a81=(a12*a62); + a81=(a108*a81); + a66=(a66+a81); + a66=(a4*a66); + if (res[1]!=0) res[1][252]=a66; + a66=(a32*a112); + a66=(a108*a66); + a66=(a8*a66); + a81=(a32*a115); + a81=(a108*a81); + a81=(a8*a81); + a66=(a66+a81); + a81=(a12*a118); + a81=(a108*a81); + a66=(a66+a81); + a66=(a4*a66); + a66=(-a66); + if (res[1]!=0) res[1][253]=a66; + a66=(a12*a59); + a81=(a125*a66); + a82=(a12*a63); + a80=(a126*a82); + a81=(a81+a80); + a81=(a129*a81); + a66=(a130*a66); + a82=(a128*a82); + a66=(a66-a82); + a66=(a114*a66); + a81=(a81-a66); + if (res[1]!=0) res[1][254]=a81; + a81=(a364*a117); + a81=(a132*a81); + a81=(a8*a81); + a66=(a364*a127); + a66=(a132*a66); + a66=(a8*a66); + a81=(a81+a66); + a66=(a29*a86); + a66=(a132*a66); + a81=(a81+a66); + a81=(a4*a81); + if (res[1]!=0) res[1][255]=a81; + a81=(a364*a136); + a81=(a132*a81); + a81=(a8*a81); + a66=(a364*a139); + a66=(a132*a66); + a66=(a8*a66); + a81=(a81+a66); + a66=(a29*a142); + a66=(a132*a66); + a81=(a81+a66); + a81=(a4*a81); + a81=(-a81); + if (res[1]!=0) res[1][256]=a81; + a81=(a17*a83); + a66=(a149*a81); + a82=(a17*a87); + a80=(a150*a82); + a66=(a66+a80); + a66=(a153*a66); + a81=(a154*a81); + a82=(a152*a82); + a81=(a81-a82); + a81=(a138*a81); + a66=(a66-a81); + if (res[1]!=0) res[1][257]=a66; + a66=(a15*a141); + a66=(a156*a66); + a66=(a8*a66); + a81=(a15*a151); + a81=(a156*a81); + a81=(a8*a81); + a66=(a66+a81); + a81=(a5*a110); + a81=(a156*a81); + a66=(a66+a81); + a66=(a4*a66); + if (res[1]!=0) res[1][258]=a66; + a66=(a15*a160); + a66=(a156*a66); + a66=(a8*a66); + a81=(a15*a163); + a81=(a156*a81); + a81=(a8*a81); + a66=(a66+a81); + a81=(a5*a166); + a81=(a156*a81); + a66=(a66+a81); + a66=(a4*a66); + a66=(-a66); + if (res[1]!=0) res[1][259]=a66; + a66=(a30*a107); + a81=(a173*a66); + a82=(a30*a111); + a80=(a174*a82); + a81=(a81+a80); + a81=(a177*a81); + a66=(a178*a66); + a82=(a176*a82); + a66=(a66-a82); + a66=(a162*a66); + a81=(a81-a66); + if (res[1]!=0) res[1][260]=a81; + a81=(a18*a165); + a81=(a180*a81); + a81=(a8*a81); + a66=(a18*a175); + a66=(a180*a66); + a66=(a8*a66); + a81=(a81+a66); + a66=(a363*a134); + a66=(a180*a66); + a81=(a81+a66); + a81=(a4*a81); + if (res[1]!=0) res[1][261]=a81; + a81=(a18*a184); + a81=(a180*a81); + a81=(a8*a81); + a66=(a18*a187); + a66=(a180*a66); + a66=(a8*a66); + a81=(a81+a66); + a66=(a363*a190); + a66=(a180*a66); + a81=(a81+a66); + a81=(a4*a81); + a81=(-a81); + if (res[1]!=0) res[1][262]=a81; + a81=(a363*a131); + a66=(a197*a81); + a82=(a363*a135); + a80=(a198*a82); + a66=(a66+a80); + a66=(a201*a66); + a81=(a202*a81); + a82=(a200*a82); + a81=(a81-a82); + a81=(a186*a81); + a66=(a66-a81); + if (res[1]!=0) res[1][263]=a66; + a66=(a366*a189); + a66=(a204*a66); + a66=(a8*a66); + a81=(a366*a199); + a81=(a204*a81); + a81=(a8*a81); + a66=(a66+a81); + a81=(a28*a158); + a81=(a204*a81); + a66=(a66+a81); + a66=(a4*a66); + if (res[1]!=0) res[1][264]=a66; + a66=(a366*a208); + a66=(a204*a66); + a66=(a8*a66); + a81=(a366*a211); + a81=(a204*a81); + a81=(a8*a81); + a66=(a66+a81); + a81=(a28*a214); + a81=(a204*a81); + a66=(a66+a81); + a66=(a4*a66); + a66=(-a66); + if (res[1]!=0) res[1][265]=a66; + a66=(a28*a155); + a81=(a221*a66); + a82=(a28*a159); + a80=(a222*a82); + a81=(a81+a80); + a81=(a225*a81); + a66=(a226*a66); + a82=(a224*a82); + a66=(a66-a82); + a66=(a210*a66); + a81=(a81-a66); + if (res[1]!=0) res[1][266]=a81; + a81=(a27*a213); + a81=(a228*a81); + a81=(a8*a81); + a66=(a27*a223); + a66=(a228*a66); + a66=(a8*a66); + a81=(a81+a66); + a66=(a26*a182); + a66=(a228*a66); + a81=(a81+a66); + a81=(a4*a81); + if (res[1]!=0) res[1][267]=a81; + a81=(a27*a232); + a81=(a228*a81); + a81=(a8*a81); + a66=(a27*a235); + a66=(a228*a66); + a66=(a8*a66); + a81=(a81+a66); + a66=(a26*a238); + a66=(a228*a66); + a81=(a81+a66); + a81=(a4*a81); + a81=(-a81); + if (res[1]!=0) res[1][268]=a81; + a81=(a22*a179); + a66=(a245*a81); + a82=(a22*a183); + a80=(a246*a82); + a66=(a66+a80); + a66=(a249*a66); + a81=(a250*a81); + a82=(a248*a82); + a81=(a81-a82); + a81=(a234*a81); + a66=(a66-a81); + if (res[1]!=0) res[1][269]=a66; + a66=(a23*a237); + a66=(a252*a66); + a66=(a8*a66); + a81=(a23*a247); + a81=(a252*a81); + a81=(a8*a81); + a66=(a66+a81); + a81=(a19*a206); + a81=(a252*a81); + a66=(a66+a81); + a66=(a4*a66); + if (res[1]!=0) res[1][270]=a66; + a66=(a23*a256); + a66=(a252*a66); + a66=(a8*a66); + a81=(a23*a259); + a81=(a252*a81); + a81=(a8*a81); + a66=(a66+a81); + a81=(a19*a262); + a81=(a252*a81); + a66=(a66+a81); + a66=(a4*a66); + a66=(-a66); + if (res[1]!=0) res[1][271]=a66; + a66=(a19*a203); + a81=(a269*a66); + a82=(a19*a207); + a80=(a270*a82); + a81=(a81+a80); + a81=(a273*a81); + a66=(a274*a66); + a82=(a272*a82); + a66=(a66-a82); + a66=(a258*a66); + a81=(a81-a66); + if (res[1]!=0) res[1][272]=a81; + a81=(a16*a261); + a81=(a276*a81); + a81=(a8*a81); + a66=(a16*a271); + a66=(a276*a66); + a66=(a8*a66); + a81=(a81+a66); + a66=(a326*a230); + a66=(a276*a66); + a81=(a81+a66); + a81=(a4*a81); + if (res[1]!=0) res[1][273]=a81; + a81=(a16*a280); + a81=(a276*a81); + a81=(a8*a81); + a66=(a16*a283); + a66=(a276*a66); + a66=(a8*a66); + a81=(a81+a66); + a66=(a326*a286); + a66=(a276*a66); + a81=(a81+a66); + a81=(a4*a81); + a81=(-a81); + if (res[1]!=0) res[1][274]=a81; + a81=(a6*a227); + a66=(a293*a81); + a82=(a6*a231); + a80=(a294*a82); + a66=(a66+a80); + a66=(a297*a66); + a81=(a298*a81); + a82=(a296*a82); + a81=(a81-a82); + a81=(a282*a81); + a66=(a66-a81); + if (res[1]!=0) res[1][275]=a66; + a66=(a327*a285); + a66=(a300*a66); + a66=(a8*a66); + a81=(a327*a295); + a81=(a300*a81); + a81=(a8*a81); + a66=(a66+a81); + a81=(a347*a254); + a81=(a300*a81); + a66=(a66+a81); + a66=(a4*a66); + if (res[1]!=0) res[1][276]=a66; + a66=(a327*a304); + a66=(a300*a66); + a66=(a8*a66); + a81=(a327*a307); + a81=(a300*a81); + a81=(a8*a81); + a66=(a66+a81); + a81=(a347*a310); + a81=(a300*a81); + a66=(a66+a81); + a66=(a4*a66); + a66=(-a66); + if (res[1]!=0) res[1][277]=a66; + a66=(a347*a251); + a81=(a317*a66); + a82=(a347*a255); + a80=(a318*a82); + a81=(a81+a80); + a81=(a321*a81); + a66=(a322*a66); + a82=(a320*a82); + a66=(a66-a82); + a66=(a306*a66); + a81=(a81-a66); + if (res[1]!=0) res[1][278]=a81; + a81=(a302*a309); + a81=(a324*a81); + a81=(a8*a81); + a66=(a302*a319); + a66=(a324*a66); + a66=(a8*a66); + a81=(a81+a66); + a66=(a337*a278); + a66=(a324*a66); + a81=(a81+a66); + a81=(a4*a81); + if (res[1]!=0) res[1][279]=a81; + a81=(a302*a328); + a81=(a324*a81); + a81=(a8*a81); + a66=(a302*a331); + a66=(a324*a66); + a66=(a8*a66); + a81=(a81+a66); + a66=(a337*a334); + a66=(a324*a66); + a81=(a81+a66); + a81=(a4*a81); + a81=(-a81); + if (res[1]!=0) res[1][280]=a81; + a81=(a10*a275); + a66=(a341*a81); + a82=(a10*a279); + a80=(a342*a82); + a66=(a66+a80); + a66=(a345*a66); + a81=(a346*a81); + a82=(a344*a82); + a81=(a81-a82); + a81=(a330*a81); + a66=(a66-a81); + if (res[1]!=0) res[1][281]=a66; + a66=(a313*a333); + a66=(a348*a66); + a66=(a8*a66); + a81=(a313*a343); + a81=(a348*a81); + a81=(a8*a81); + a66=(a66+a81); + a81=(a289*a323); + a81=(a348*a81); + a66=(a66+a81); + a66=(a4*a66); + if (res[1]!=0) res[1][282]=a66; + a66=(a313*a352); + a66=(a348*a66); + a66=(a8*a66); + a313=(a313*a9); + a313=(a348*a313); + a313=(a8*a313); + a66=(a66+a313); + a313=(a289*a20); + a313=(a348*a313); + a66=(a66+a313); + a66=(a4*a66); + a66=(-a66); + if (res[1]!=0) res[1][283]=a66; + a66=(a289*a299); + a313=(a358*a66); + a289=(a289*a303); + a81=(a351*a289); + a313=(a313+a81); + a313=(a354*a313); + a66=(a362*a66); + a289=(a356*a289); + a66=(a66-a289); + a66=(a13*a66); + a313=(a313-a66); + if (res[1]!=0) res[1][284]=a313; + a89=(a8*a89); + a85=(a85+a89); + a92=(a8*a92); + a85=(a85+a92); + a85=(a85+a95); + a85=(a4*a85); + a85=(-a85); + if (res[1]!=0) res[1][285]=a85; + a98=(a8*a98); + a96=(a96+a98); + a99=(a8*a99); + a96=(a96+a99); + a96=(a96+a100); + a96=(a4*a96); + a96=(-a96); + if (res[1]!=0) res[1][286]=a96; + a69=(a24*a69); + a69=(a84*a69); + a69=(a8*a69); + a79=(a24*a79); + a79=(a84*a79); + a79=(a8*a79); + a69=(a69+a79); + a38=(a365*a38); + a38=(a84*a38); + a69=(a69+a38); + a69=(a4*a69); + if (res[1]!=0) res[1][287]=a69; + a88=(a24*a88); + a88=(a84*a88); + a88=(a8*a88); + a91=(a24*a91); + a91=(a84*a91); + a91=(a8*a91); + a88=(a88+a91); + a94=(a365*a94); + a84=(a84*a94); + a88=(a88+a84); + a88=(a4*a88); + a88=(-a88); + if (res[1]!=0) res[1][288]=a88; + a35=(a365*a35); + a101=(a101*a35); + a39=(a365*a39); + a102=(a102*a39); + a101=(a101+a102); + a105=(a105*a101); + a106=(a106*a35); + a104=(a104*a39); + a106=(a106-a104); + a90=(a90*a106); + a105=(a105-a90); + if (res[1]!=0) res[1][289]=a105; + a105=(a33*a93); + a105=(a108*a105); + a105=(a8*a105); + a90=(a33*a103); + a90=(a108*a90); + a90=(a8*a90); + a105=(a105+a90); + a90=(a34*a62); + a90=(a108*a90); + a105=(a105+a90); + a105=(a4*a105); + if (res[1]!=0) res[1][290]=a105; + a105=(a33*a112); + a105=(a108*a105); + a105=(a8*a105); + a90=(a33*a115); + a90=(a108*a90); + a90=(a8*a90); + a105=(a105+a90); + a90=(a34*a118); + a90=(a108*a90); + a105=(a105+a90); + a105=(a4*a105); + a105=(-a105); + if (res[1]!=0) res[1][291]=a105; + a105=(a34*a59); + a90=(a125*a105); + a106=(a34*a63); + a104=(a126*a106); + a90=(a90+a104); + a90=(a129*a90); + a105=(a130*a105); + a106=(a128*a106); + a105=(a105-a106); + a105=(a114*a105); + a90=(a90-a105); + if (res[1]!=0) res[1][292]=a90; + a90=(a32*a117); + a90=(a132*a90); + a90=(a8*a90); + a105=(a32*a127); + a105=(a132*a105); + a105=(a8*a105); + a90=(a90+a105); + a105=(a12*a86); + a105=(a132*a105); + a90=(a90+a105); + a90=(a4*a90); + if (res[1]!=0) res[1][293]=a90; + a90=(a32*a136); + a90=(a132*a90); + a90=(a8*a90); + a105=(a32*a139); + a105=(a132*a105); + a105=(a8*a105); + a90=(a90+a105); + a105=(a12*a142); + a105=(a132*a105); + a90=(a90+a105); + a90=(a4*a90); + a90=(-a90); + if (res[1]!=0) res[1][294]=a90; + a90=(a12*a83); + a105=(a149*a90); + a106=(a12*a87); + a104=(a150*a106); + a105=(a105+a104); + a105=(a153*a105); + a90=(a154*a90); + a106=(a152*a106); + a90=(a90-a106); + a90=(a138*a90); + a105=(a105-a90); + if (res[1]!=0) res[1][295]=a105; + a105=(a364*a141); + a105=(a156*a105); + a105=(a8*a105); + a90=(a364*a151); + a90=(a156*a90); + a90=(a8*a90); + a105=(a105+a90); + a90=(a29*a110); + a90=(a156*a90); + a105=(a105+a90); + a105=(a4*a105); + if (res[1]!=0) res[1][296]=a105; + a105=(a364*a160); + a105=(a156*a105); + a105=(a8*a105); + a90=(a364*a163); + a90=(a156*a90); + a90=(a8*a90); + a105=(a105+a90); + a90=(a29*a166); + a90=(a156*a90); + a105=(a105+a90); + a105=(a4*a105); + a105=(-a105); + if (res[1]!=0) res[1][297]=a105; + a105=(a17*a107); + a90=(a173*a105); + a106=(a17*a111); + a104=(a174*a106); + a90=(a90+a104); + a90=(a177*a90); + a105=(a178*a105); + a106=(a176*a106); + a105=(a105-a106); + a105=(a162*a105); + a90=(a90-a105); + if (res[1]!=0) res[1][298]=a90; + a90=(a15*a165); + a90=(a180*a90); + a90=(a8*a90); + a105=(a15*a175); + a105=(a180*a105); + a105=(a8*a105); + a90=(a90+a105); + a105=(a5*a134); + a105=(a180*a105); + a90=(a90+a105); + a90=(a4*a90); + if (res[1]!=0) res[1][299]=a90; + a90=(a15*a184); + a90=(a180*a90); + a90=(a8*a90); + a105=(a15*a187); + a105=(a180*a105); + a105=(a8*a105); + a90=(a90+a105); + a105=(a5*a190); + a105=(a180*a105); + a90=(a90+a105); + a90=(a4*a90); + a90=(-a90); + if (res[1]!=0) res[1][300]=a90; + a90=(a30*a131); + a105=(a197*a90); + a106=(a30*a135); + a104=(a198*a106); + a105=(a105+a104); + a105=(a201*a105); + a90=(a202*a90); + a106=(a200*a106); + a90=(a90-a106); + a90=(a186*a90); + a105=(a105-a90); + if (res[1]!=0) res[1][301]=a105; + a105=(a18*a189); + a105=(a204*a105); + a105=(a8*a105); + a90=(a18*a199); + a90=(a204*a90); + a90=(a8*a90); + a105=(a105+a90); + a90=(a363*a158); + a90=(a204*a90); + a105=(a105+a90); + a105=(a4*a105); + if (res[1]!=0) res[1][302]=a105; + a105=(a18*a208); + a105=(a204*a105); + a105=(a8*a105); + a90=(a18*a211); + a90=(a204*a90); + a90=(a8*a90); + a105=(a105+a90); + a90=(a363*a214); + a90=(a204*a90); + a105=(a105+a90); + a105=(a4*a105); + a105=(-a105); + if (res[1]!=0) res[1][303]=a105; + a105=(a363*a155); + a90=(a221*a105); + a106=(a363*a159); + a104=(a222*a106); + a90=(a90+a104); + a90=(a225*a90); + a105=(a226*a105); + a106=(a224*a106); + a105=(a105-a106); + a105=(a210*a105); + a90=(a90-a105); + if (res[1]!=0) res[1][304]=a90; + a90=(a366*a213); + a90=(a228*a90); + a90=(a8*a90); + a105=(a366*a223); + a105=(a228*a105); + a105=(a8*a105); + a90=(a90+a105); + a105=(a28*a182); + a105=(a228*a105); + a90=(a90+a105); + a90=(a4*a90); + if (res[1]!=0) res[1][305]=a90; + a90=(a366*a232); + a90=(a228*a90); + a90=(a8*a90); + a105=(a366*a235); + a105=(a228*a105); + a105=(a8*a105); + a90=(a90+a105); + a105=(a28*a238); + a105=(a228*a105); + a90=(a90+a105); + a90=(a4*a90); + a90=(-a90); + if (res[1]!=0) res[1][306]=a90; + a90=(a28*a179); + a105=(a245*a90); + a106=(a28*a183); + a104=(a246*a106); + a105=(a105+a104); + a105=(a249*a105); + a90=(a250*a90); + a106=(a248*a106); + a90=(a90-a106); + a90=(a234*a90); + a105=(a105-a90); + if (res[1]!=0) res[1][307]=a105; + a105=(a27*a237); + a105=(a252*a105); + a105=(a8*a105); + a90=(a27*a247); + a90=(a252*a90); + a90=(a8*a90); + a105=(a105+a90); + a90=(a26*a206); + a90=(a252*a90); + a105=(a105+a90); + a105=(a4*a105); + if (res[1]!=0) res[1][308]=a105; + a105=(a27*a256); + a105=(a252*a105); + a105=(a8*a105); + a90=(a27*a259); + a90=(a252*a90); + a90=(a8*a90); + a105=(a105+a90); + a90=(a26*a262); + a90=(a252*a90); + a105=(a105+a90); + a105=(a4*a105); + a105=(-a105); + if (res[1]!=0) res[1][309]=a105; + a105=(a22*a203); + a90=(a269*a105); + a106=(a22*a207); + a104=(a270*a106); + a90=(a90+a104); + a90=(a273*a90); + a105=(a274*a105); + a106=(a272*a106); + a105=(a105-a106); + a105=(a258*a105); + a90=(a90-a105); + if (res[1]!=0) res[1][310]=a90; + a90=(a23*a261); + a90=(a276*a90); + a90=(a8*a90); + a105=(a23*a271); + a105=(a276*a105); + a105=(a8*a105); + a90=(a90+a105); + a105=(a19*a230); + a105=(a276*a105); + a90=(a90+a105); + a90=(a4*a90); + if (res[1]!=0) res[1][311]=a90; + a90=(a23*a280); + a90=(a276*a90); + a90=(a8*a90); + a105=(a23*a283); + a105=(a276*a105); + a105=(a8*a105); + a90=(a90+a105); + a105=(a19*a286); + a105=(a276*a105); + a90=(a90+a105); + a90=(a4*a90); + a90=(-a90); + if (res[1]!=0) res[1][312]=a90; + a90=(a19*a227); + a105=(a293*a90); + a106=(a19*a231); + a104=(a294*a106); + a105=(a105+a104); + a105=(a297*a105); + a90=(a298*a90); + a106=(a296*a106); + a90=(a90-a106); + a90=(a282*a90); + a105=(a105-a90); + if (res[1]!=0) res[1][313]=a105; + a105=(a16*a285); + a105=(a300*a105); + a105=(a8*a105); + a90=(a16*a295); + a90=(a300*a90); + a90=(a8*a90); + a105=(a105+a90); + a90=(a326*a254); + a90=(a300*a90); + a105=(a105+a90); + a105=(a4*a105); + if (res[1]!=0) res[1][314]=a105; + a105=(a16*a304); + a105=(a300*a105); + a105=(a8*a105); + a90=(a16*a307); + a90=(a300*a90); + a90=(a8*a90); + a105=(a105+a90); + a90=(a326*a310); + a90=(a300*a90); + a105=(a105+a90); + a105=(a4*a105); + a105=(-a105); + if (res[1]!=0) res[1][315]=a105; + a105=(a6*a251); + a90=(a317*a105); + a106=(a6*a255); + a104=(a318*a106); + a90=(a90+a104); + a90=(a321*a90); + a105=(a322*a105); + a106=(a320*a106); + a105=(a105-a106); + a105=(a306*a105); + a90=(a90-a105); + if (res[1]!=0) res[1][316]=a90; + a90=(a327*a309); + a90=(a324*a90); + a90=(a8*a90); + a105=(a327*a319); + a105=(a324*a105); + a105=(a8*a105); + a90=(a90+a105); + a105=(a347*a278); + a105=(a324*a105); + a90=(a90+a105); + a90=(a4*a90); + if (res[1]!=0) res[1][317]=a90; + a90=(a327*a328); + a90=(a324*a90); + a90=(a8*a90); + a105=(a327*a331); + a105=(a324*a105); + a105=(a8*a105); + a90=(a90+a105); + a105=(a347*a334); + a105=(a324*a105); + a90=(a90+a105); + a90=(a4*a90); + a90=(-a90); + if (res[1]!=0) res[1][318]=a90; + a90=(a347*a275); + a105=(a341*a90); + a106=(a347*a279); + a104=(a342*a106); + a105=(a105+a104); + a105=(a345*a105); + a90=(a346*a90); + a106=(a344*a106); + a90=(a90-a106); + a90=(a330*a90); + a105=(a105-a90); + if (res[1]!=0) res[1][319]=a105; + a105=(a302*a333); + a105=(a348*a105); + a105=(a8*a105); + a90=(a302*a343); + a90=(a348*a90); + a90=(a8*a90); + a105=(a105+a90); + a90=(a337*a323); + a90=(a348*a90); + a105=(a105+a90); + a105=(a4*a105); + if (res[1]!=0) res[1][320]=a105; + a105=(a302*a352); + a105=(a348*a105); + a105=(a8*a105); + a302=(a302*a9); + a302=(a348*a302); + a302=(a8*a302); + a105=(a105+a302); + a337=(a337*a20); + a337=(a348*a337); + a105=(a105+a337); + a105=(a4*a105); + a105=(-a105); + if (res[1]!=0) res[1][321]=a105; + a105=(a10*a299); + a337=(a358*a105); + a10=(a10*a303); + a302=(a351*a10); + a337=(a337+a302); + a337=(a354*a337); + a105=(a362*a105); + a10=(a356*a10); + a105=(a105-a10); + a105=(a13*a105); + a337=(a337-a105); + if (res[1]!=0) res[1][322]=a337; + a113=(a8*a113); + a109=(a109+a113); + a116=(a8*a116); + a109=(a109+a116); + a109=(a109+a119); + a109=(a4*a109); + a109=(-a109); + if (res[1]!=0) res[1][323]=a109; + a122=(a8*a122); + a120=(a120+a122); + a123=(a8*a123); + a120=(a120+a123); + a120=(a120+a124); + a120=(a4*a120); + a120=(-a120); + if (res[1]!=0) res[1][324]=a120; + a93=(a24*a93); + a93=(a108*a93); + a93=(a8*a93); + a103=(a24*a103); + a103=(a108*a103); + a103=(a8*a103); + a93=(a93+a103); + a62=(a365*a62); + a62=(a108*a62); + a93=(a93+a62); + a93=(a4*a93); + if (res[1]!=0) res[1][325]=a93; + a112=(a24*a112); + a112=(a108*a112); + a112=(a8*a112); + a115=(a24*a115); + a115=(a108*a115); + a115=(a8*a115); + a112=(a112+a115); + a118=(a365*a118); + a108=(a108*a118); + a112=(a112+a108); + a112=(a4*a112); + a112=(-a112); + if (res[1]!=0) res[1][326]=a112; + a59=(a365*a59); + a125=(a125*a59); + a63=(a365*a63); + a126=(a126*a63); + a125=(a125+a126); + a129=(a129*a125); + a130=(a130*a59); + a128=(a128*a63); + a130=(a130-a128); + a114=(a114*a130); + a129=(a129-a114); + if (res[1]!=0) res[1][327]=a129; + a129=(a33*a117); + a129=(a132*a129); + a129=(a8*a129); + a114=(a33*a127); + a114=(a132*a114); + a114=(a8*a114); + a129=(a129+a114); + a114=(a34*a86); + a114=(a132*a114); + a129=(a129+a114); + a129=(a4*a129); + if (res[1]!=0) res[1][328]=a129; + a129=(a33*a136); + a129=(a132*a129); + a129=(a8*a129); + a114=(a33*a139); + a114=(a132*a114); + a114=(a8*a114); + a129=(a129+a114); + a114=(a34*a142); + a114=(a132*a114); + a129=(a129+a114); + a129=(a4*a129); + a129=(-a129); + if (res[1]!=0) res[1][329]=a129; + a129=(a34*a83); + a114=(a149*a129); + a130=(a34*a87); + a128=(a150*a130); + a114=(a114+a128); + a114=(a153*a114); + a129=(a154*a129); + a130=(a152*a130); + a129=(a129-a130); + a129=(a138*a129); + a114=(a114-a129); + if (res[1]!=0) res[1][330]=a114; + a114=(a32*a141); + a114=(a156*a114); + a114=(a8*a114); + a129=(a32*a151); + a129=(a156*a129); + a129=(a8*a129); + a114=(a114+a129); + a129=(a12*a110); + a129=(a156*a129); + a114=(a114+a129); + a114=(a4*a114); + if (res[1]!=0) res[1][331]=a114; + a114=(a32*a160); + a114=(a156*a114); + a114=(a8*a114); + a129=(a32*a163); + a129=(a156*a129); + a129=(a8*a129); + a114=(a114+a129); + a129=(a12*a166); + a129=(a156*a129); + a114=(a114+a129); + a114=(a4*a114); + a114=(-a114); + if (res[1]!=0) res[1][332]=a114; + a114=(a12*a107); + a129=(a173*a114); + a130=(a12*a111); + a128=(a174*a130); + a129=(a129+a128); + a129=(a177*a129); + a114=(a178*a114); + a130=(a176*a130); + a114=(a114-a130); + a114=(a162*a114); + a129=(a129-a114); + if (res[1]!=0) res[1][333]=a129; + a129=(a364*a165); + a129=(a180*a129); + a129=(a8*a129); + a114=(a364*a175); + a114=(a180*a114); + a114=(a8*a114); + a129=(a129+a114); + a114=(a29*a134); + a114=(a180*a114); + a129=(a129+a114); + a129=(a4*a129); + if (res[1]!=0) res[1][334]=a129; + a129=(a364*a184); + a129=(a180*a129); + a129=(a8*a129); + a114=(a364*a187); + a114=(a180*a114); + a114=(a8*a114); + a129=(a129+a114); + a114=(a29*a190); + a114=(a180*a114); + a129=(a129+a114); + a129=(a4*a129); + a129=(-a129); + if (res[1]!=0) res[1][335]=a129; + a129=(a17*a131); + a114=(a197*a129); + a130=(a17*a135); + a128=(a198*a130); + a114=(a114+a128); + a114=(a201*a114); + a129=(a202*a129); + a130=(a200*a130); + a129=(a129-a130); + a129=(a186*a129); + a114=(a114-a129); + if (res[1]!=0) res[1][336]=a114; + a114=(a15*a189); + a114=(a204*a114); + a114=(a8*a114); + a129=(a15*a199); + a129=(a204*a129); + a129=(a8*a129); + a114=(a114+a129); + a129=(a5*a158); + a129=(a204*a129); + a114=(a114+a129); + a114=(a4*a114); + if (res[1]!=0) res[1][337]=a114; + a114=(a15*a208); + a114=(a204*a114); + a114=(a8*a114); + a129=(a15*a211); + a129=(a204*a129); + a129=(a8*a129); + a114=(a114+a129); + a129=(a5*a214); + a129=(a204*a129); + a114=(a114+a129); + a114=(a4*a114); + a114=(-a114); + if (res[1]!=0) res[1][338]=a114; + a114=(a30*a155); + a129=(a221*a114); + a130=(a30*a159); + a128=(a222*a130); + a129=(a129+a128); + a129=(a225*a129); + a114=(a226*a114); + a130=(a224*a130); + a114=(a114-a130); + a114=(a210*a114); + a129=(a129-a114); + if (res[1]!=0) res[1][339]=a129; + a129=(a18*a213); + a129=(a228*a129); + a129=(a8*a129); + a114=(a18*a223); + a114=(a228*a114); + a114=(a8*a114); + a129=(a129+a114); + a114=(a363*a182); + a114=(a228*a114); + a129=(a129+a114); + a129=(a4*a129); + if (res[1]!=0) res[1][340]=a129; + a129=(a18*a232); + a129=(a228*a129); + a129=(a8*a129); + a114=(a18*a235); + a114=(a228*a114); + a114=(a8*a114); + a129=(a129+a114); + a114=(a363*a238); + a114=(a228*a114); + a129=(a129+a114); + a129=(a4*a129); + a129=(-a129); + if (res[1]!=0) res[1][341]=a129; + a129=(a363*a179); + a114=(a245*a129); + a130=(a363*a183); + a128=(a246*a130); + a114=(a114+a128); + a114=(a249*a114); + a129=(a250*a129); + a130=(a248*a130); + a129=(a129-a130); + a129=(a234*a129); + a114=(a114-a129); + if (res[1]!=0) res[1][342]=a114; + a114=(a366*a237); + a114=(a252*a114); + a114=(a8*a114); + a129=(a366*a247); + a129=(a252*a129); + a129=(a8*a129); + a114=(a114+a129); + a129=(a28*a206); + a129=(a252*a129); + a114=(a114+a129); + a114=(a4*a114); + if (res[1]!=0) res[1][343]=a114; + a114=(a366*a256); + a114=(a252*a114); + a114=(a8*a114); + a129=(a366*a259); + a129=(a252*a129); + a129=(a8*a129); + a114=(a114+a129); + a129=(a28*a262); + a129=(a252*a129); + a114=(a114+a129); + a114=(a4*a114); + a114=(-a114); + if (res[1]!=0) res[1][344]=a114; + a114=(a28*a203); + a129=(a269*a114); + a130=(a28*a207); + a128=(a270*a130); + a129=(a129+a128); + a129=(a273*a129); + a114=(a274*a114); + a130=(a272*a130); + a114=(a114-a130); + a114=(a258*a114); + a129=(a129-a114); + if (res[1]!=0) res[1][345]=a129; + a129=(a27*a261); + a129=(a276*a129); + a129=(a8*a129); + a114=(a27*a271); + a114=(a276*a114); + a114=(a8*a114); + a129=(a129+a114); + a114=(a26*a230); + a114=(a276*a114); + a129=(a129+a114); + a129=(a4*a129); + if (res[1]!=0) res[1][346]=a129; + a129=(a27*a280); + a129=(a276*a129); + a129=(a8*a129); + a114=(a27*a283); + a114=(a276*a114); + a114=(a8*a114); + a129=(a129+a114); + a114=(a26*a286); + a114=(a276*a114); + a129=(a129+a114); + a129=(a4*a129); + a129=(-a129); + if (res[1]!=0) res[1][347]=a129; + a129=(a22*a227); + a114=(a293*a129); + a130=(a22*a231); + a128=(a294*a130); + a114=(a114+a128); + a114=(a297*a114); + a129=(a298*a129); + a130=(a296*a130); + a129=(a129-a130); + a129=(a282*a129); + a114=(a114-a129); + if (res[1]!=0) res[1][348]=a114; + a114=(a23*a285); + a114=(a300*a114); + a114=(a8*a114); + a129=(a23*a295); + a129=(a300*a129); + a129=(a8*a129); + a114=(a114+a129); + a129=(a19*a254); + a129=(a300*a129); + a114=(a114+a129); + a114=(a4*a114); + if (res[1]!=0) res[1][349]=a114; + a114=(a23*a304); + a114=(a300*a114); + a114=(a8*a114); + a129=(a23*a307); + a129=(a300*a129); + a129=(a8*a129); + a114=(a114+a129); + a129=(a19*a310); + a129=(a300*a129); + a114=(a114+a129); + a114=(a4*a114); + a114=(-a114); + if (res[1]!=0) res[1][350]=a114; + a114=(a19*a251); + a129=(a317*a114); + a130=(a19*a255); + a128=(a318*a130); + a129=(a129+a128); + a129=(a321*a129); + a114=(a322*a114); + a130=(a320*a130); + a114=(a114-a130); + a114=(a306*a114); + a129=(a129-a114); + if (res[1]!=0) res[1][351]=a129; + a129=(a16*a309); + a129=(a324*a129); + a129=(a8*a129); + a114=(a16*a319); + a114=(a324*a114); + a114=(a8*a114); + a129=(a129+a114); + a114=(a326*a278); + a114=(a324*a114); + a129=(a129+a114); + a129=(a4*a129); + if (res[1]!=0) res[1][352]=a129; + a129=(a16*a328); + a129=(a324*a129); + a129=(a8*a129); + a114=(a16*a331); + a114=(a324*a114); + a114=(a8*a114); + a129=(a129+a114); + a114=(a326*a334); + a114=(a324*a114); + a129=(a129+a114); + a129=(a4*a129); + a129=(-a129); + if (res[1]!=0) res[1][353]=a129; + a129=(a6*a275); + a114=(a341*a129); + a130=(a6*a279); + a128=(a342*a130); + a114=(a114+a128); + a114=(a345*a114); + a129=(a346*a129); + a130=(a344*a130); + a129=(a129-a130); + a129=(a330*a129); + a114=(a114-a129); + if (res[1]!=0) res[1][354]=a114; + a114=(a327*a333); + a114=(a348*a114); + a114=(a8*a114); + a129=(a327*a343); + a129=(a348*a129); + a129=(a8*a129); + a114=(a114+a129); + a129=(a347*a323); + a129=(a348*a129); + a114=(a114+a129); + a114=(a4*a114); + if (res[1]!=0) res[1][355]=a114; + a114=(a327*a352); + a114=(a348*a114); + a114=(a8*a114); + a327=(a327*a9); + a327=(a348*a327); + a327=(a8*a327); + a114=(a114+a327); + a327=(a347*a20); + a327=(a348*a327); + a114=(a114+a327); + a114=(a4*a114); + a114=(-a114); + if (res[1]!=0) res[1][356]=a114; + a114=(a347*a299); + a327=(a358*a114); + a347=(a347*a303); + a129=(a351*a347); + a327=(a327+a129); + a327=(a354*a327); + a114=(a362*a114); + a347=(a356*a347); + a114=(a114-a347); + a114=(a13*a114); + a327=(a327-a114); + if (res[1]!=0) res[1][357]=a327; + a137=(a8*a137); + a133=(a133+a137); + a140=(a8*a140); + a133=(a133+a140); + a133=(a133+a143); + a133=(a4*a133); + a133=(-a133); + if (res[1]!=0) res[1][358]=a133; + a146=(a8*a146); + a144=(a144+a146); + a147=(a8*a147); + a144=(a144+a147); + a144=(a144+a148); + a144=(a4*a144); + a144=(-a144); + if (res[1]!=0) res[1][359]=a144; + a117=(a24*a117); + a117=(a132*a117); + a117=(a8*a117); + a127=(a24*a127); + a127=(a132*a127); + a127=(a8*a127); + a117=(a117+a127); + a86=(a365*a86); + a86=(a132*a86); + a117=(a117+a86); + a117=(a4*a117); + if (res[1]!=0) res[1][360]=a117; + a136=(a24*a136); + a136=(a132*a136); + a136=(a8*a136); + a139=(a24*a139); + a139=(a132*a139); + a139=(a8*a139); + a136=(a136+a139); + a142=(a365*a142); + a132=(a132*a142); + a136=(a136+a132); + a136=(a4*a136); + a136=(-a136); + if (res[1]!=0) res[1][361]=a136; + a83=(a365*a83); + a149=(a149*a83); + a87=(a365*a87); + a150=(a150*a87); + a149=(a149+a150); + a153=(a153*a149); + a154=(a154*a83); + a152=(a152*a87); + a154=(a154-a152); + a138=(a138*a154); + a153=(a153-a138); + if (res[1]!=0) res[1][362]=a153; + a153=(a33*a141); + a153=(a156*a153); + a153=(a8*a153); + a138=(a33*a151); + a138=(a156*a138); + a138=(a8*a138); + a153=(a153+a138); + a138=(a34*a110); + a138=(a156*a138); + a153=(a153+a138); + a153=(a4*a153); + if (res[1]!=0) res[1][363]=a153; + a153=(a33*a160); + a153=(a156*a153); + a153=(a8*a153); + a138=(a33*a163); + a138=(a156*a138); + a138=(a8*a138); + a153=(a153+a138); + a138=(a34*a166); + a138=(a156*a138); + a153=(a153+a138); + a153=(a4*a153); + a153=(-a153); + if (res[1]!=0) res[1][364]=a153; + a153=(a34*a107); + a138=(a173*a153); + a154=(a34*a111); + a152=(a174*a154); + a138=(a138+a152); + a138=(a177*a138); + a153=(a178*a153); + a154=(a176*a154); + a153=(a153-a154); + a153=(a162*a153); + a138=(a138-a153); + if (res[1]!=0) res[1][365]=a138; + a138=(a32*a165); + a138=(a180*a138); + a138=(a8*a138); + a153=(a32*a175); + a153=(a180*a153); + a153=(a8*a153); + a138=(a138+a153); + a153=(a12*a134); + a153=(a180*a153); + a138=(a138+a153); + a138=(a4*a138); + if (res[1]!=0) res[1][366]=a138; + a138=(a32*a184); + a138=(a180*a138); + a138=(a8*a138); + a153=(a32*a187); + a153=(a180*a153); + a153=(a8*a153); + a138=(a138+a153); + a153=(a12*a190); + a153=(a180*a153); + a138=(a138+a153); + a138=(a4*a138); + a138=(-a138); + if (res[1]!=0) res[1][367]=a138; + a138=(a12*a131); + a153=(a197*a138); + a154=(a12*a135); + a152=(a198*a154); + a153=(a153+a152); + a153=(a201*a153); + a138=(a202*a138); + a154=(a200*a154); + a138=(a138-a154); + a138=(a186*a138); + a153=(a153-a138); + if (res[1]!=0) res[1][368]=a153; + a153=(a364*a189); + a153=(a204*a153); + a153=(a8*a153); + a138=(a364*a199); + a138=(a204*a138); + a138=(a8*a138); + a153=(a153+a138); + a138=(a29*a158); + a138=(a204*a138); + a153=(a153+a138); + a153=(a4*a153); + if (res[1]!=0) res[1][369]=a153; + a153=(a364*a208); + a153=(a204*a153); + a153=(a8*a153); + a138=(a364*a211); + a138=(a204*a138); + a138=(a8*a138); + a153=(a153+a138); + a138=(a29*a214); + a138=(a204*a138); + a153=(a153+a138); + a153=(a4*a153); + a153=(-a153); + if (res[1]!=0) res[1][370]=a153; + a153=(a17*a155); + a138=(a221*a153); + a154=(a17*a159); + a152=(a222*a154); + a138=(a138+a152); + a138=(a225*a138); + a153=(a226*a153); + a154=(a224*a154); + a153=(a153-a154); + a153=(a210*a153); + a138=(a138-a153); + if (res[1]!=0) res[1][371]=a138; + a138=(a15*a213); + a138=(a228*a138); + a138=(a8*a138); + a153=(a15*a223); + a153=(a228*a153); + a153=(a8*a153); + a138=(a138+a153); + a153=(a5*a182); + a153=(a228*a153); + a138=(a138+a153); + a138=(a4*a138); + if (res[1]!=0) res[1][372]=a138; + a138=(a15*a232); + a138=(a228*a138); + a138=(a8*a138); + a153=(a15*a235); + a153=(a228*a153); + a153=(a8*a153); + a138=(a138+a153); + a153=(a5*a238); + a153=(a228*a153); + a138=(a138+a153); + a138=(a4*a138); + a138=(-a138); + if (res[1]!=0) res[1][373]=a138; + a138=(a30*a179); + a153=(a245*a138); + a154=(a30*a183); + a152=(a246*a154); + a153=(a153+a152); + a153=(a249*a153); + a138=(a250*a138); + a154=(a248*a154); + a138=(a138-a154); + a138=(a234*a138); + a153=(a153-a138); + if (res[1]!=0) res[1][374]=a153; + a153=(a18*a237); + a153=(a252*a153); + a153=(a8*a153); + a138=(a18*a247); + a138=(a252*a138); + a138=(a8*a138); + a153=(a153+a138); + a138=(a363*a206); + a138=(a252*a138); + a153=(a153+a138); + a153=(a4*a153); + if (res[1]!=0) res[1][375]=a153; + a153=(a18*a256); + a153=(a252*a153); + a153=(a8*a153); + a138=(a18*a259); + a138=(a252*a138); + a138=(a8*a138); + a153=(a153+a138); + a138=(a363*a262); + a138=(a252*a138); + a153=(a153+a138); + a153=(a4*a153); + a153=(-a153); + if (res[1]!=0) res[1][376]=a153; + a153=(a363*a203); + a138=(a269*a153); + a154=(a363*a207); + a152=(a270*a154); + a138=(a138+a152); + a138=(a273*a138); + a153=(a274*a153); + a154=(a272*a154); + a153=(a153-a154); + a153=(a258*a153); + a138=(a138-a153); + if (res[1]!=0) res[1][377]=a138; + a138=(a366*a261); + a138=(a276*a138); + a138=(a8*a138); + a153=(a366*a271); + a153=(a276*a153); + a153=(a8*a153); + a138=(a138+a153); + a153=(a28*a230); + a153=(a276*a153); + a138=(a138+a153); + a138=(a4*a138); + if (res[1]!=0) res[1][378]=a138; + a138=(a366*a280); + a138=(a276*a138); + a138=(a8*a138); + a153=(a366*a283); + a153=(a276*a153); + a153=(a8*a153); + a138=(a138+a153); + a153=(a28*a286); + a153=(a276*a153); + a138=(a138+a153); + a138=(a4*a138); + a138=(-a138); + if (res[1]!=0) res[1][379]=a138; + a138=(a28*a227); + a153=(a293*a138); + a154=(a28*a231); + a152=(a294*a154); + a153=(a153+a152); + a153=(a297*a153); + a138=(a298*a138); + a154=(a296*a154); + a138=(a138-a154); + a138=(a282*a138); + a153=(a153-a138); + if (res[1]!=0) res[1][380]=a153; + a153=(a27*a285); + a153=(a300*a153); + a153=(a8*a153); + a138=(a27*a295); + a138=(a300*a138); + a138=(a8*a138); + a153=(a153+a138); + a138=(a26*a254); + a138=(a300*a138); + a153=(a153+a138); + a153=(a4*a153); + if (res[1]!=0) res[1][381]=a153; + a153=(a27*a304); + a153=(a300*a153); + a153=(a8*a153); + a138=(a27*a307); + a138=(a300*a138); + a138=(a8*a138); + a153=(a153+a138); + a138=(a26*a310); + a138=(a300*a138); + a153=(a153+a138); + a153=(a4*a153); + a153=(-a153); + if (res[1]!=0) res[1][382]=a153; + a153=(a22*a251); + a138=(a317*a153); + a154=(a22*a255); + a152=(a318*a154); + a138=(a138+a152); + a138=(a321*a138); + a153=(a322*a153); + a154=(a320*a154); + a153=(a153-a154); + a153=(a306*a153); + a138=(a138-a153); + if (res[1]!=0) res[1][383]=a138; + a138=(a23*a309); + a138=(a324*a138); + a138=(a8*a138); + a153=(a23*a319); + a153=(a324*a153); + a153=(a8*a153); + a138=(a138+a153); + a153=(a19*a278); + a153=(a324*a153); + a138=(a138+a153); + a138=(a4*a138); + if (res[1]!=0) res[1][384]=a138; + a138=(a23*a328); + a138=(a324*a138); + a138=(a8*a138); + a153=(a23*a331); + a153=(a324*a153); + a153=(a8*a153); + a138=(a138+a153); + a153=(a19*a334); + a153=(a324*a153); + a138=(a138+a153); + a138=(a4*a138); + a138=(-a138); + if (res[1]!=0) res[1][385]=a138; + a138=(a19*a275); + a153=(a341*a138); + a154=(a19*a279); + a152=(a342*a154); + a153=(a153+a152); + a153=(a345*a153); + a138=(a346*a138); + a154=(a344*a154); + a138=(a138-a154); + a138=(a330*a138); + a153=(a153-a138); + if (res[1]!=0) res[1][386]=a153; + a153=(a16*a333); + a153=(a348*a153); + a153=(a8*a153); + a138=(a16*a343); + a138=(a348*a138); + a138=(a8*a138); + a153=(a153+a138); + a138=(a326*a323); + a138=(a348*a138); + a153=(a153+a138); + a153=(a4*a153); + if (res[1]!=0) res[1][387]=a153; + a153=(a16*a352); + a153=(a348*a153); + a153=(a8*a153); + a16=(a16*a9); + a16=(a348*a16); + a16=(a8*a16); + a153=(a153+a16); + a326=(a326*a20); + a326=(a348*a326); + a153=(a153+a326); + a153=(a4*a153); + a153=(-a153); + if (res[1]!=0) res[1][388]=a153; + a153=(a6*a299); + a326=(a358*a153); + a6=(a6*a303); + a16=(a351*a6); + a326=(a326+a16); + a326=(a354*a326); + a153=(a362*a153); + a6=(a356*a6); + a153=(a153-a6); + a153=(a13*a153); + a326=(a326-a153); + if (res[1]!=0) res[1][389]=a326; + a161=(a8*a161); + a157=(a157+a161); + a164=(a8*a164); + a157=(a157+a164); + a157=(a157+a167); + a157=(a4*a157); + a157=(-a157); + if (res[1]!=0) res[1][390]=a157; + a170=(a8*a170); + a168=(a168+a170); + a171=(a8*a171); + a168=(a168+a171); + a168=(a168+a172); + a168=(a4*a168); + a168=(-a168); + if (res[1]!=0) res[1][391]=a168; + a141=(a24*a141); + a141=(a156*a141); + a141=(a8*a141); + a151=(a24*a151); + a151=(a156*a151); + a151=(a8*a151); + a141=(a141+a151); + a110=(a365*a110); + a110=(a156*a110); + a141=(a141+a110); + a141=(a4*a141); + if (res[1]!=0) res[1][392]=a141; + a160=(a24*a160); + a160=(a156*a160); + a160=(a8*a160); + a163=(a24*a163); + a163=(a156*a163); + a163=(a8*a163); + a160=(a160+a163); + a166=(a365*a166); + a156=(a156*a166); + a160=(a160+a156); + a160=(a4*a160); + a160=(-a160); + if (res[1]!=0) res[1][393]=a160; + a107=(a365*a107); + a173=(a173*a107); + a111=(a365*a111); + a174=(a174*a111); + a173=(a173+a174); + a177=(a177*a173); + a178=(a178*a107); + a176=(a176*a111); + a178=(a178-a176); + a162=(a162*a178); + a177=(a177-a162); + if (res[1]!=0) res[1][394]=a177; + a177=(a33*a165); + a177=(a180*a177); + a177=(a8*a177); + a162=(a33*a175); + a162=(a180*a162); + a162=(a8*a162); + a177=(a177+a162); + a162=(a34*a134); + a162=(a180*a162); + a177=(a177+a162); + a177=(a4*a177); + if (res[1]!=0) res[1][395]=a177; + a177=(a33*a184); + a177=(a180*a177); + a177=(a8*a177); + a162=(a33*a187); + a162=(a180*a162); + a162=(a8*a162); + a177=(a177+a162); + a162=(a34*a190); + a162=(a180*a162); + a177=(a177+a162); + a177=(a4*a177); + a177=(-a177); + if (res[1]!=0) res[1][396]=a177; + a177=(a34*a131); + a162=(a197*a177); + a178=(a34*a135); + a176=(a198*a178); + a162=(a162+a176); + a162=(a201*a162); + a177=(a202*a177); + a178=(a200*a178); + a177=(a177-a178); + a177=(a186*a177); + a162=(a162-a177); + if (res[1]!=0) res[1][397]=a162; + a162=(a32*a189); + a162=(a204*a162); + a162=(a8*a162); + a177=(a32*a199); + a177=(a204*a177); + a177=(a8*a177); + a162=(a162+a177); + a177=(a12*a158); + a177=(a204*a177); + a162=(a162+a177); + a162=(a4*a162); + if (res[1]!=0) res[1][398]=a162; + a162=(a32*a208); + a162=(a204*a162); + a162=(a8*a162); + a177=(a32*a211); + a177=(a204*a177); + a177=(a8*a177); + a162=(a162+a177); + a177=(a12*a214); + a177=(a204*a177); + a162=(a162+a177); + a162=(a4*a162); + a162=(-a162); + if (res[1]!=0) res[1][399]=a162; + a162=(a12*a155); + a177=(a221*a162); + a178=(a12*a159); + a176=(a222*a178); + a177=(a177+a176); + a177=(a225*a177); + a162=(a226*a162); + a178=(a224*a178); + a162=(a162-a178); + a162=(a210*a162); + a177=(a177-a162); + if (res[1]!=0) res[1][400]=a177; + a177=(a364*a213); + a177=(a228*a177); + a177=(a8*a177); + a162=(a364*a223); + a162=(a228*a162); + a162=(a8*a162); + a177=(a177+a162); + a162=(a29*a182); + a162=(a228*a162); + a177=(a177+a162); + a177=(a4*a177); + if (res[1]!=0) res[1][401]=a177; + a177=(a364*a232); + a177=(a228*a177); + a177=(a8*a177); + a162=(a364*a235); + a162=(a228*a162); + a162=(a8*a162); + a177=(a177+a162); + a162=(a29*a238); + a162=(a228*a162); + a177=(a177+a162); + a177=(a4*a177); + a177=(-a177); + if (res[1]!=0) res[1][402]=a177; + a177=(a17*a179); + a162=(a245*a177); + a178=(a17*a183); + a176=(a246*a178); + a162=(a162+a176); + a162=(a249*a162); + a177=(a250*a177); + a178=(a248*a178); + a177=(a177-a178); + a177=(a234*a177); + a162=(a162-a177); + if (res[1]!=0) res[1][403]=a162; + a162=(a15*a237); + a162=(a252*a162); + a162=(a8*a162); + a177=(a15*a247); + a177=(a252*a177); + a177=(a8*a177); + a162=(a162+a177); + a177=(a5*a206); + a177=(a252*a177); + a162=(a162+a177); + a162=(a4*a162); + if (res[1]!=0) res[1][404]=a162; + a162=(a15*a256); + a162=(a252*a162); + a162=(a8*a162); + a177=(a15*a259); + a177=(a252*a177); + a177=(a8*a177); + a162=(a162+a177); + a177=(a5*a262); + a177=(a252*a177); + a162=(a162+a177); + a162=(a4*a162); + a162=(-a162); + if (res[1]!=0) res[1][405]=a162; + a162=(a30*a203); + a177=(a269*a162); + a178=(a30*a207); + a176=(a270*a178); + a177=(a177+a176); + a177=(a273*a177); + a162=(a274*a162); + a178=(a272*a178); + a162=(a162-a178); + a162=(a258*a162); + a177=(a177-a162); + if (res[1]!=0) res[1][406]=a177; + a177=(a18*a261); + a177=(a276*a177); + a177=(a8*a177); + a162=(a18*a271); + a162=(a276*a162); + a162=(a8*a162); + a177=(a177+a162); + a162=(a363*a230); + a162=(a276*a162); + a177=(a177+a162); + a177=(a4*a177); + if (res[1]!=0) res[1][407]=a177; + a177=(a18*a280); + a177=(a276*a177); + a177=(a8*a177); + a162=(a18*a283); + a162=(a276*a162); + a162=(a8*a162); + a177=(a177+a162); + a162=(a363*a286); + a162=(a276*a162); + a177=(a177+a162); + a177=(a4*a177); + a177=(-a177); + if (res[1]!=0) res[1][408]=a177; + a177=(a363*a227); + a162=(a293*a177); + a178=(a363*a231); + a176=(a294*a178); + a162=(a162+a176); + a162=(a297*a162); + a177=(a298*a177); + a178=(a296*a178); + a177=(a177-a178); + a177=(a282*a177); + a162=(a162-a177); + if (res[1]!=0) res[1][409]=a162; + a162=(a366*a285); + a162=(a300*a162); + a162=(a8*a162); + a177=(a366*a295); + a177=(a300*a177); + a177=(a8*a177); + a162=(a162+a177); + a177=(a28*a254); + a177=(a300*a177); + a162=(a162+a177); + a162=(a4*a162); + if (res[1]!=0) res[1][410]=a162; + a162=(a366*a304); + a162=(a300*a162); + a162=(a8*a162); + a177=(a366*a307); + a177=(a300*a177); + a177=(a8*a177); + a162=(a162+a177); + a177=(a28*a310); + a177=(a300*a177); + a162=(a162+a177); + a162=(a4*a162); + a162=(-a162); + if (res[1]!=0) res[1][411]=a162; + a162=(a28*a251); + a177=(a317*a162); + a178=(a28*a255); + a176=(a318*a178); + a177=(a177+a176); + a177=(a321*a177); + a162=(a322*a162); + a178=(a320*a178); + a162=(a162-a178); + a162=(a306*a162); + a177=(a177-a162); + if (res[1]!=0) res[1][412]=a177; + a177=(a27*a309); + a177=(a324*a177); + a177=(a8*a177); + a162=(a27*a319); + a162=(a324*a162); + a162=(a8*a162); + a177=(a177+a162); + a162=(a26*a278); + a162=(a324*a162); + a177=(a177+a162); + a177=(a4*a177); + if (res[1]!=0) res[1][413]=a177; + a177=(a27*a328); + a177=(a324*a177); + a177=(a8*a177); + a162=(a27*a331); + a162=(a324*a162); + a162=(a8*a162); + a177=(a177+a162); + a162=(a26*a334); + a162=(a324*a162); + a177=(a177+a162); + a177=(a4*a177); + a177=(-a177); + if (res[1]!=0) res[1][414]=a177; + a177=(a22*a275); + a162=(a341*a177); + a178=(a22*a279); + a176=(a342*a178); + a162=(a162+a176); + a162=(a345*a162); + a177=(a346*a177); + a178=(a344*a178); + a177=(a177-a178); + a177=(a330*a177); + a162=(a162-a177); + if (res[1]!=0) res[1][415]=a162; + a162=(a23*a333); + a162=(a348*a162); + a162=(a8*a162); + a177=(a23*a343); + a177=(a348*a177); + a177=(a8*a177); + a162=(a162+a177); + a177=(a19*a323); + a177=(a348*a177); + a162=(a162+a177); + a162=(a4*a162); + if (res[1]!=0) res[1][416]=a162; + a162=(a23*a352); + a162=(a348*a162); + a162=(a8*a162); + a23=(a23*a9); + a23=(a348*a23); + a23=(a8*a23); + a162=(a162+a23); + a23=(a19*a20); + a23=(a348*a23); + a162=(a162+a23); + a162=(a4*a162); + a162=(-a162); + if (res[1]!=0) res[1][417]=a162; + a162=(a19*a299); + a23=(a358*a162); + a19=(a19*a303); + a177=(a351*a19); + a23=(a23+a177); + a23=(a354*a23); + a162=(a362*a162); + a19=(a356*a19); + a162=(a162-a19); + a162=(a13*a162); + a23=(a23-a162); + if (res[1]!=0) res[1][418]=a23; + a185=(a8*a185); + a181=(a181+a185); + a188=(a8*a188); + a181=(a181+a188); + a181=(a181+a191); + a181=(a4*a181); + a181=(-a181); + if (res[1]!=0) res[1][419]=a181; + a194=(a8*a194); + a192=(a192+a194); + a195=(a8*a195); + a192=(a192+a195); + a192=(a192+a196); + a192=(a4*a192); + a192=(-a192); + if (res[1]!=0) res[1][420]=a192; + a165=(a24*a165); + a165=(a180*a165); + a165=(a8*a165); + a175=(a24*a175); + a175=(a180*a175); + a175=(a8*a175); + a165=(a165+a175); + a134=(a365*a134); + a134=(a180*a134); + a165=(a165+a134); + a165=(a4*a165); + if (res[1]!=0) res[1][421]=a165; + a184=(a24*a184); + a184=(a180*a184); + a184=(a8*a184); + a187=(a24*a187); + a187=(a180*a187); + a187=(a8*a187); + a184=(a184+a187); + a190=(a365*a190); + a180=(a180*a190); + a184=(a184+a180); + a184=(a4*a184); + a184=(-a184); + if (res[1]!=0) res[1][422]=a184; + a131=(a365*a131); + a197=(a197*a131); + a135=(a365*a135); + a198=(a198*a135); + a197=(a197+a198); + a201=(a201*a197); + a202=(a202*a131); + a200=(a200*a135); + a202=(a202-a200); + a186=(a186*a202); + a201=(a201-a186); + if (res[1]!=0) res[1][423]=a201; + a201=(a33*a189); + a201=(a204*a201); + a201=(a8*a201); + a186=(a33*a199); + a186=(a204*a186); + a186=(a8*a186); + a201=(a201+a186); + a186=(a34*a158); + a186=(a204*a186); + a201=(a201+a186); + a201=(a4*a201); + if (res[1]!=0) res[1][424]=a201; + a201=(a33*a208); + a201=(a204*a201); + a201=(a8*a201); + a186=(a33*a211); + a186=(a204*a186); + a186=(a8*a186); + a201=(a201+a186); + a186=(a34*a214); + a186=(a204*a186); + a201=(a201+a186); + a201=(a4*a201); + a201=(-a201); + if (res[1]!=0) res[1][425]=a201; + a201=(a34*a155); + a186=(a221*a201); + a202=(a34*a159); + a200=(a222*a202); + a186=(a186+a200); + a186=(a225*a186); + a201=(a226*a201); + a202=(a224*a202); + a201=(a201-a202); + a201=(a210*a201); + a186=(a186-a201); + if (res[1]!=0) res[1][426]=a186; + a186=(a32*a213); + a186=(a228*a186); + a186=(a8*a186); + a201=(a32*a223); + a201=(a228*a201); + a201=(a8*a201); + a186=(a186+a201); + a201=(a12*a182); + a201=(a228*a201); + a186=(a186+a201); + a186=(a4*a186); + if (res[1]!=0) res[1][427]=a186; + a186=(a32*a232); + a186=(a228*a186); + a186=(a8*a186); + a201=(a32*a235); + a201=(a228*a201); + a201=(a8*a201); + a186=(a186+a201); + a201=(a12*a238); + a201=(a228*a201); + a186=(a186+a201); + a186=(a4*a186); + a186=(-a186); + if (res[1]!=0) res[1][428]=a186; + a186=(a12*a179); + a201=(a245*a186); + a202=(a12*a183); + a200=(a246*a202); + a201=(a201+a200); + a201=(a249*a201); + a186=(a250*a186); + a202=(a248*a202); + a186=(a186-a202); + a186=(a234*a186); + a201=(a201-a186); + if (res[1]!=0) res[1][429]=a201; + a201=(a364*a237); + a201=(a252*a201); + a201=(a8*a201); + a186=(a364*a247); + a186=(a252*a186); + a186=(a8*a186); + a201=(a201+a186); + a186=(a29*a206); + a186=(a252*a186); + a201=(a201+a186); + a201=(a4*a201); + if (res[1]!=0) res[1][430]=a201; + a201=(a364*a256); + a201=(a252*a201); + a201=(a8*a201); + a186=(a364*a259); + a186=(a252*a186); + a186=(a8*a186); + a201=(a201+a186); + a186=(a29*a262); + a186=(a252*a186); + a201=(a201+a186); + a201=(a4*a201); + a201=(-a201); + if (res[1]!=0) res[1][431]=a201; + a201=(a17*a203); + a186=(a269*a201); + a202=(a17*a207); + a200=(a270*a202); + a186=(a186+a200); + a186=(a273*a186); + a201=(a274*a201); + a202=(a272*a202); + a201=(a201-a202); + a201=(a258*a201); + a186=(a186-a201); + if (res[1]!=0) res[1][432]=a186; + a186=(a15*a261); + a186=(a276*a186); + a186=(a8*a186); + a201=(a15*a271); + a201=(a276*a201); + a201=(a8*a201); + a186=(a186+a201); + a201=(a5*a230); + a201=(a276*a201); + a186=(a186+a201); + a186=(a4*a186); + if (res[1]!=0) res[1][433]=a186; + a186=(a15*a280); + a186=(a276*a186); + a186=(a8*a186); + a201=(a15*a283); + a201=(a276*a201); + a201=(a8*a201); + a186=(a186+a201); + a201=(a5*a286); + a201=(a276*a201); + a186=(a186+a201); + a186=(a4*a186); + a186=(-a186); + if (res[1]!=0) res[1][434]=a186; + a186=(a30*a227); + a201=(a293*a186); + a202=(a30*a231); + a200=(a294*a202); + a201=(a201+a200); + a201=(a297*a201); + a186=(a298*a186); + a202=(a296*a202); + a186=(a186-a202); + a186=(a282*a186); + a201=(a201-a186); + if (res[1]!=0) res[1][435]=a201; + a201=(a18*a285); + a201=(a300*a201); + a201=(a8*a201); + a186=(a18*a295); + a186=(a300*a186); + a186=(a8*a186); + a201=(a201+a186); + a186=(a363*a254); + a186=(a300*a186); + a201=(a201+a186); + a201=(a4*a201); + if (res[1]!=0) res[1][436]=a201; + a201=(a18*a304); + a201=(a300*a201); + a201=(a8*a201); + a186=(a18*a307); + a186=(a300*a186); + a186=(a8*a186); + a201=(a201+a186); + a186=(a363*a310); + a186=(a300*a186); + a201=(a201+a186); + a201=(a4*a201); + a201=(-a201); + if (res[1]!=0) res[1][437]=a201; + a201=(a363*a251); + a186=(a317*a201); + a202=(a363*a255); + a200=(a318*a202); + a186=(a186+a200); + a186=(a321*a186); + a201=(a322*a201); + a202=(a320*a202); + a201=(a201-a202); + a201=(a306*a201); + a186=(a186-a201); + if (res[1]!=0) res[1][438]=a186; + a186=(a366*a309); + a186=(a324*a186); + a186=(a8*a186); + a201=(a366*a319); + a201=(a324*a201); + a201=(a8*a201); + a186=(a186+a201); + a201=(a28*a278); + a201=(a324*a201); + a186=(a186+a201); + a186=(a4*a186); + if (res[1]!=0) res[1][439]=a186; + a186=(a366*a328); + a186=(a324*a186); + a186=(a8*a186); + a201=(a366*a331); + a201=(a324*a201); + a201=(a8*a201); + a186=(a186+a201); + a201=(a28*a334); + a201=(a324*a201); + a186=(a186+a201); + a186=(a4*a186); + a186=(-a186); + if (res[1]!=0) res[1][440]=a186; + a186=(a28*a275); + a201=(a341*a186); + a202=(a28*a279); + a200=(a342*a202); + a201=(a201+a200); + a201=(a345*a201); + a186=(a346*a186); + a202=(a344*a202); + a186=(a186-a202); + a186=(a330*a186); + a201=(a201-a186); + if (res[1]!=0) res[1][441]=a201; + a201=(a27*a333); + a201=(a348*a201); + a201=(a8*a201); + a186=(a27*a343); + a186=(a348*a186); + a186=(a8*a186); + a201=(a201+a186); + a186=(a26*a323); + a186=(a348*a186); + a201=(a201+a186); + a201=(a4*a201); + if (res[1]!=0) res[1][442]=a201; + a201=(a27*a352); + a201=(a348*a201); + a201=(a8*a201); + a27=(a27*a9); + a27=(a348*a27); + a27=(a8*a27); + a201=(a201+a27); + a26=(a26*a20); + a26=(a348*a26); + a201=(a201+a26); + a201=(a4*a201); + a201=(-a201); + if (res[1]!=0) res[1][443]=a201; + a201=(a22*a299); + a26=(a358*a201); + a22=(a22*a303); + a27=(a351*a22); + a26=(a26+a27); + a26=(a354*a26); + a201=(a362*a201); + a22=(a356*a22); + a201=(a201-a22); + a201=(a13*a201); + a26=(a26-a201); + if (res[1]!=0) res[1][444]=a26; + a209=(a8*a209); + a205=(a205+a209); + a212=(a8*a212); + a205=(a205+a212); + a205=(a205+a215); + a205=(a4*a205); + a205=(-a205); + if (res[1]!=0) res[1][445]=a205; + a218=(a8*a218); + a216=(a216+a218); + a219=(a8*a219); + a216=(a216+a219); + a216=(a216+a220); + a216=(a4*a216); + a216=(-a216); + if (res[1]!=0) res[1][446]=a216; + a189=(a24*a189); + a189=(a204*a189); + a189=(a8*a189); + a199=(a24*a199); + a199=(a204*a199); + a199=(a8*a199); + a189=(a189+a199); + a158=(a365*a158); + a158=(a204*a158); + a189=(a189+a158); + a189=(a4*a189); + if (res[1]!=0) res[1][447]=a189; + a208=(a24*a208); + a208=(a204*a208); + a208=(a8*a208); + a211=(a24*a211); + a211=(a204*a211); + a211=(a8*a211); + a208=(a208+a211); + a214=(a365*a214); + a204=(a204*a214); + a208=(a208+a204); + a208=(a4*a208); + a208=(-a208); + if (res[1]!=0) res[1][448]=a208; + a155=(a365*a155); + a221=(a221*a155); + a159=(a365*a159); + a222=(a222*a159); + a221=(a221+a222); + a225=(a225*a221); + a226=(a226*a155); + a224=(a224*a159); + a226=(a226-a224); + a210=(a210*a226); + a225=(a225-a210); + if (res[1]!=0) res[1][449]=a225; + a225=(a33*a213); + a225=(a228*a225); + a225=(a8*a225); + a210=(a33*a223); + a210=(a228*a210); + a210=(a8*a210); + a225=(a225+a210); + a210=(a34*a182); + a210=(a228*a210); + a225=(a225+a210); + a225=(a4*a225); + if (res[1]!=0) res[1][450]=a225; + a225=(a33*a232); + a225=(a228*a225); + a225=(a8*a225); + a210=(a33*a235); + a210=(a228*a210); + a210=(a8*a210); + a225=(a225+a210); + a210=(a34*a238); + a210=(a228*a210); + a225=(a225+a210); + a225=(a4*a225); + a225=(-a225); + if (res[1]!=0) res[1][451]=a225; + a225=(a34*a179); + a210=(a245*a225); + a226=(a34*a183); + a224=(a246*a226); + a210=(a210+a224); + a210=(a249*a210); + a225=(a250*a225); + a226=(a248*a226); + a225=(a225-a226); + a225=(a234*a225); + a210=(a210-a225); + if (res[1]!=0) res[1][452]=a210; + a210=(a32*a237); + a210=(a252*a210); + a210=(a8*a210); + a225=(a32*a247); + a225=(a252*a225); + a225=(a8*a225); + a210=(a210+a225); + a225=(a12*a206); + a225=(a252*a225); + a210=(a210+a225); + a210=(a4*a210); + if (res[1]!=0) res[1][453]=a210; + a210=(a32*a256); + a210=(a252*a210); + a210=(a8*a210); + a225=(a32*a259); + a225=(a252*a225); + a225=(a8*a225); + a210=(a210+a225); + a225=(a12*a262); + a225=(a252*a225); + a210=(a210+a225); + a210=(a4*a210); + a210=(-a210); + if (res[1]!=0) res[1][454]=a210; + a210=(a12*a203); + a225=(a269*a210); + a226=(a12*a207); + a224=(a270*a226); + a225=(a225+a224); + a225=(a273*a225); + a210=(a274*a210); + a226=(a272*a226); + a210=(a210-a226); + a210=(a258*a210); + a225=(a225-a210); + if (res[1]!=0) res[1][455]=a225; + a225=(a364*a261); + a225=(a276*a225); + a225=(a8*a225); + a210=(a364*a271); + a210=(a276*a210); + a210=(a8*a210); + a225=(a225+a210); + a210=(a29*a230); + a210=(a276*a210); + a225=(a225+a210); + a225=(a4*a225); + if (res[1]!=0) res[1][456]=a225; + a225=(a364*a280); + a225=(a276*a225); + a225=(a8*a225); + a210=(a364*a283); + a210=(a276*a210); + a210=(a8*a210); + a225=(a225+a210); + a210=(a29*a286); + a210=(a276*a210); + a225=(a225+a210); + a225=(a4*a225); + a225=(-a225); + if (res[1]!=0) res[1][457]=a225; + a225=(a17*a227); + a210=(a293*a225); + a226=(a17*a231); + a224=(a294*a226); + a210=(a210+a224); + a210=(a297*a210); + a225=(a298*a225); + a226=(a296*a226); + a225=(a225-a226); + a225=(a282*a225); + a210=(a210-a225); + if (res[1]!=0) res[1][458]=a210; + a210=(a15*a285); + a210=(a300*a210); + a210=(a8*a210); + a225=(a15*a295); + a225=(a300*a225); + a225=(a8*a225); + a210=(a210+a225); + a225=(a5*a254); + a225=(a300*a225); + a210=(a210+a225); + a210=(a4*a210); + if (res[1]!=0) res[1][459]=a210; + a210=(a15*a304); + a210=(a300*a210); + a210=(a8*a210); + a225=(a15*a307); + a225=(a300*a225); + a225=(a8*a225); + a210=(a210+a225); + a225=(a5*a310); + a225=(a300*a225); + a210=(a210+a225); + a210=(a4*a210); + a210=(-a210); + if (res[1]!=0) res[1][460]=a210; + a210=(a30*a251); + a225=(a317*a210); + a226=(a30*a255); + a224=(a318*a226); + a225=(a225+a224); + a225=(a321*a225); + a210=(a322*a210); + a226=(a320*a226); + a210=(a210-a226); + a210=(a306*a210); + a225=(a225-a210); + if (res[1]!=0) res[1][461]=a225; + a225=(a18*a309); + a225=(a324*a225); + a225=(a8*a225); + a210=(a18*a319); + a210=(a324*a210); + a210=(a8*a210); + a225=(a225+a210); + a210=(a363*a278); + a210=(a324*a210); + a225=(a225+a210); + a225=(a4*a225); + if (res[1]!=0) res[1][462]=a225; + a225=(a18*a328); + a225=(a324*a225); + a225=(a8*a225); + a210=(a18*a331); + a210=(a324*a210); + a210=(a8*a210); + a225=(a225+a210); + a210=(a363*a334); + a210=(a324*a210); + a225=(a225+a210); + a225=(a4*a225); + a225=(-a225); + if (res[1]!=0) res[1][463]=a225; + a225=(a363*a275); + a210=(a341*a225); + a226=(a363*a279); + a224=(a342*a226); + a210=(a210+a224); + a210=(a345*a210); + a225=(a346*a225); + a226=(a344*a226); + a225=(a225-a226); + a225=(a330*a225); + a210=(a210-a225); + if (res[1]!=0) res[1][464]=a210; + a210=(a366*a333); + a210=(a348*a210); + a210=(a8*a210); + a225=(a366*a343); + a225=(a348*a225); + a225=(a8*a225); + a210=(a210+a225); + a225=(a28*a323); + a225=(a348*a225); + a210=(a210+a225); + a210=(a4*a210); + if (res[1]!=0) res[1][465]=a210; + a210=(a366*a352); + a210=(a348*a210); + a210=(a8*a210); + a366=(a366*a9); + a366=(a348*a366); + a366=(a8*a366); + a210=(a210+a366); + a366=(a28*a20); + a366=(a348*a366); + a210=(a210+a366); + a210=(a4*a210); + a210=(-a210); + if (res[1]!=0) res[1][466]=a210; + a210=(a28*a299); + a366=(a358*a210); + a28=(a28*a303); + a225=(a351*a28); + a366=(a366+a225); + a366=(a354*a366); + a210=(a362*a210); + a28=(a356*a28); + a210=(a210-a28); + a210=(a13*a210); + a366=(a366-a210); + if (res[1]!=0) res[1][467]=a366; + a233=(a8*a233); + a229=(a229+a233); + a236=(a8*a236); + a229=(a229+a236); + a229=(a229+a239); + a229=(a4*a229); + a229=(-a229); + if (res[1]!=0) res[1][468]=a229; + a242=(a8*a242); + a240=(a240+a242); + a243=(a8*a243); + a240=(a240+a243); + a240=(a240+a244); + a240=(a4*a240); + a240=(-a240); + if (res[1]!=0) res[1][469]=a240; + a213=(a24*a213); + a213=(a228*a213); + a213=(a8*a213); + a223=(a24*a223); + a223=(a228*a223); + a223=(a8*a223); + a213=(a213+a223); + a182=(a365*a182); + a182=(a228*a182); + a213=(a213+a182); + a213=(a4*a213); + if (res[1]!=0) res[1][470]=a213; + a232=(a24*a232); + a232=(a228*a232); + a232=(a8*a232); + a235=(a24*a235); + a235=(a228*a235); + a235=(a8*a235); + a232=(a232+a235); + a238=(a365*a238); + a228=(a228*a238); + a232=(a232+a228); + a232=(a4*a232); + a232=(-a232); + if (res[1]!=0) res[1][471]=a232; + a179=(a365*a179); + a245=(a245*a179); + a183=(a365*a183); + a246=(a246*a183); + a245=(a245+a246); + a249=(a249*a245); + a250=(a250*a179); + a248=(a248*a183); + a250=(a250-a248); + a234=(a234*a250); + a249=(a249-a234); + if (res[1]!=0) res[1][472]=a249; + a249=(a33*a237); + a249=(a252*a249); + a249=(a8*a249); + a234=(a33*a247); + a234=(a252*a234); + a234=(a8*a234); + a249=(a249+a234); + a234=(a34*a206); + a234=(a252*a234); + a249=(a249+a234); + a249=(a4*a249); + if (res[1]!=0) res[1][473]=a249; + a249=(a33*a256); + a249=(a252*a249); + a249=(a8*a249); + a234=(a33*a259); + a234=(a252*a234); + a234=(a8*a234); + a249=(a249+a234); + a234=(a34*a262); + a234=(a252*a234); + a249=(a249+a234); + a249=(a4*a249); + a249=(-a249); + if (res[1]!=0) res[1][474]=a249; + a249=(a34*a203); + a234=(a269*a249); + a250=(a34*a207); + a248=(a270*a250); + a234=(a234+a248); + a234=(a273*a234); + a249=(a274*a249); + a250=(a272*a250); + a249=(a249-a250); + a249=(a258*a249); + a234=(a234-a249); + if (res[1]!=0) res[1][475]=a234; + a234=(a32*a261); + a234=(a276*a234); + a234=(a8*a234); + a249=(a32*a271); + a249=(a276*a249); + a249=(a8*a249); + a234=(a234+a249); + a249=(a12*a230); + a249=(a276*a249); + a234=(a234+a249); + a234=(a4*a234); + if (res[1]!=0) res[1][476]=a234; + a234=(a32*a280); + a234=(a276*a234); + a234=(a8*a234); + a249=(a32*a283); + a249=(a276*a249); + a249=(a8*a249); + a234=(a234+a249); + a249=(a12*a286); + a249=(a276*a249); + a234=(a234+a249); + a234=(a4*a234); + a234=(-a234); + if (res[1]!=0) res[1][477]=a234; + a234=(a12*a227); + a249=(a293*a234); + a250=(a12*a231); + a248=(a294*a250); + a249=(a249+a248); + a249=(a297*a249); + a234=(a298*a234); + a250=(a296*a250); + a234=(a234-a250); + a234=(a282*a234); + a249=(a249-a234); + if (res[1]!=0) res[1][478]=a249; + a249=(a364*a285); + a249=(a300*a249); + a249=(a8*a249); + a234=(a364*a295); + a234=(a300*a234); + a234=(a8*a234); + a249=(a249+a234); + a234=(a29*a254); + a234=(a300*a234); + a249=(a249+a234); + a249=(a4*a249); + if (res[1]!=0) res[1][479]=a249; + a249=(a364*a304); + a249=(a300*a249); + a249=(a8*a249); + a234=(a364*a307); + a234=(a300*a234); + a234=(a8*a234); + a249=(a249+a234); + a234=(a29*a310); + a234=(a300*a234); + a249=(a249+a234); + a249=(a4*a249); + a249=(-a249); + if (res[1]!=0) res[1][480]=a249; + a249=(a17*a251); + a234=(a317*a249); + a250=(a17*a255); + a248=(a318*a250); + a234=(a234+a248); + a234=(a321*a234); + a249=(a322*a249); + a250=(a320*a250); + a249=(a249-a250); + a249=(a306*a249); + a234=(a234-a249); + if (res[1]!=0) res[1][481]=a234; + a234=(a15*a309); + a234=(a324*a234); + a234=(a8*a234); + a249=(a15*a319); + a249=(a324*a249); + a249=(a8*a249); + a234=(a234+a249); + a249=(a5*a278); + a249=(a324*a249); + a234=(a234+a249); + a234=(a4*a234); + if (res[1]!=0) res[1][482]=a234; + a234=(a15*a328); + a234=(a324*a234); + a234=(a8*a234); + a249=(a15*a331); + a249=(a324*a249); + a249=(a8*a249); + a234=(a234+a249); + a249=(a5*a334); + a249=(a324*a249); + a234=(a234+a249); + a234=(a4*a234); + a234=(-a234); + if (res[1]!=0) res[1][483]=a234; + a234=(a30*a275); + a249=(a341*a234); + a250=(a30*a279); + a248=(a342*a250); + a249=(a249+a248); + a249=(a345*a249); + a234=(a346*a234); + a250=(a344*a250); + a234=(a234-a250); + a234=(a330*a234); + a249=(a249-a234); + if (res[1]!=0) res[1][484]=a249; + a249=(a18*a333); + a249=(a348*a249); + a249=(a8*a249); + a234=(a18*a343); + a234=(a348*a234); + a234=(a8*a234); + a249=(a249+a234); + a234=(a363*a323); + a234=(a348*a234); + a249=(a249+a234); + a249=(a4*a249); + if (res[1]!=0) res[1][485]=a249; + a249=(a18*a352); + a249=(a348*a249); + a249=(a8*a249); + a18=(a18*a9); + a18=(a348*a18); + a18=(a8*a18); + a249=(a249+a18); + a18=(a363*a20); + a18=(a348*a18); + a249=(a249+a18); + a249=(a4*a249); + a249=(-a249); + if (res[1]!=0) res[1][486]=a249; + a249=(a363*a299); + a18=(a358*a249); + a363=(a363*a303); + a234=(a351*a363); + a18=(a18+a234); + a18=(a354*a18); + a249=(a362*a249); + a363=(a356*a363); + a249=(a249-a363); + a249=(a13*a249); + a18=(a18-a249); + if (res[1]!=0) res[1][487]=a18; + a257=(a8*a257); + a253=(a253+a257); + a260=(a8*a260); + a253=(a253+a260); + a253=(a253+a263); + a253=(a4*a253); + a253=(-a253); + if (res[1]!=0) res[1][488]=a253; + a266=(a8*a266); + a264=(a264+a266); + a267=(a8*a267); + a264=(a264+a267); + a264=(a264+a268); + a264=(a4*a264); + a264=(-a264); + if (res[1]!=0) res[1][489]=a264; + a237=(a24*a237); + a237=(a252*a237); + a237=(a8*a237); + a247=(a24*a247); + a247=(a252*a247); + a247=(a8*a247); + a237=(a237+a247); + a206=(a365*a206); + a206=(a252*a206); + a237=(a237+a206); + a237=(a4*a237); + if (res[1]!=0) res[1][490]=a237; + a256=(a24*a256); + a256=(a252*a256); + a256=(a8*a256); + a259=(a24*a259); + a259=(a252*a259); + a259=(a8*a259); + a256=(a256+a259); + a262=(a365*a262); + a252=(a252*a262); + a256=(a256+a252); + a256=(a4*a256); + a256=(-a256); + if (res[1]!=0) res[1][491]=a256; + a203=(a365*a203); + a269=(a269*a203); + a207=(a365*a207); + a270=(a270*a207); + a269=(a269+a270); + a273=(a273*a269); + a274=(a274*a203); + a272=(a272*a207); + a274=(a274-a272); + a258=(a258*a274); + a273=(a273-a258); + if (res[1]!=0) res[1][492]=a273; + a273=(a33*a261); + a273=(a276*a273); + a273=(a8*a273); + a258=(a33*a271); + a258=(a276*a258); + a258=(a8*a258); + a273=(a273+a258); + a258=(a34*a230); + a258=(a276*a258); + a273=(a273+a258); + a273=(a4*a273); + if (res[1]!=0) res[1][493]=a273; + a273=(a33*a280); + a273=(a276*a273); + a273=(a8*a273); + a258=(a33*a283); + a258=(a276*a258); + a258=(a8*a258); + a273=(a273+a258); + a258=(a34*a286); + a258=(a276*a258); + a273=(a273+a258); + a273=(a4*a273); + a273=(-a273); + if (res[1]!=0) res[1][494]=a273; + a273=(a34*a227); + a258=(a293*a273); + a274=(a34*a231); + a272=(a294*a274); + a258=(a258+a272); + a258=(a297*a258); + a273=(a298*a273); + a274=(a296*a274); + a273=(a273-a274); + a273=(a282*a273); + a258=(a258-a273); + if (res[1]!=0) res[1][495]=a258; + a258=(a32*a285); + a258=(a300*a258); + a258=(a8*a258); + a273=(a32*a295); + a273=(a300*a273); + a273=(a8*a273); + a258=(a258+a273); + a273=(a12*a254); + a273=(a300*a273); + a258=(a258+a273); + a258=(a4*a258); + if (res[1]!=0) res[1][496]=a258; + a258=(a32*a304); + a258=(a300*a258); + a258=(a8*a258); + a273=(a32*a307); + a273=(a300*a273); + a273=(a8*a273); + a258=(a258+a273); + a273=(a12*a310); + a273=(a300*a273); + a258=(a258+a273); + a258=(a4*a258); + a258=(-a258); + if (res[1]!=0) res[1][497]=a258; + a258=(a12*a251); + a273=(a317*a258); + a274=(a12*a255); + a272=(a318*a274); + a273=(a273+a272); + a273=(a321*a273); + a258=(a322*a258); + a274=(a320*a274); + a258=(a258-a274); + a258=(a306*a258); + a273=(a273-a258); + if (res[1]!=0) res[1][498]=a273; + a273=(a364*a309); + a273=(a324*a273); + a273=(a8*a273); + a258=(a364*a319); + a258=(a324*a258); + a258=(a8*a258); + a273=(a273+a258); + a258=(a29*a278); + a258=(a324*a258); + a273=(a273+a258); + a273=(a4*a273); + if (res[1]!=0) res[1][499]=a273; + a273=(a364*a328); + a273=(a324*a273); + a273=(a8*a273); + a258=(a364*a331); + a258=(a324*a258); + a258=(a8*a258); + a273=(a273+a258); + a258=(a29*a334); + a258=(a324*a258); + a273=(a273+a258); + a273=(a4*a273); + a273=(-a273); + if (res[1]!=0) res[1][500]=a273; + a273=(a17*a275); + a258=(a341*a273); + a274=(a17*a279); + a272=(a342*a274); + a258=(a258+a272); + a258=(a345*a258); + a273=(a346*a273); + a274=(a344*a274); + a273=(a273-a274); + a273=(a330*a273); + a258=(a258-a273); + if (res[1]!=0) res[1][501]=a258; + a258=(a15*a333); + a258=(a348*a258); + a258=(a8*a258); + a273=(a15*a343); + a273=(a348*a273); + a273=(a8*a273); + a258=(a258+a273); + a273=(a5*a323); + a273=(a348*a273); + a258=(a258+a273); + a258=(a4*a258); + if (res[1]!=0) res[1][502]=a258; + a258=(a15*a352); + a258=(a348*a258); + a258=(a8*a258); + a15=(a15*a9); + a15=(a348*a15); + a15=(a8*a15); + a258=(a258+a15); + a5=(a5*a20); + a5=(a348*a5); + a258=(a258+a5); + a258=(a4*a258); + a258=(-a258); + if (res[1]!=0) res[1][503]=a258; + a258=(a30*a299); + a5=(a358*a258); + a30=(a30*a303); + a15=(a351*a30); + a5=(a5+a15); + a5=(a354*a5); + a258=(a362*a258); + a30=(a356*a30); + a258=(a258-a30); + a258=(a13*a258); + a5=(a5-a258); + if (res[1]!=0) res[1][504]=a5; + a281=(a8*a281); + a277=(a277+a281); + a284=(a8*a284); + a277=(a277+a284); + a277=(a277+a287); + a277=(a4*a277); + a277=(-a277); + if (res[1]!=0) res[1][505]=a277; + a290=(a8*a290); + a288=(a288+a290); + a291=(a8*a291); + a288=(a288+a291); + a288=(a288+a292); + a288=(a4*a288); + a288=(-a288); + if (res[1]!=0) res[1][506]=a288; + a261=(a24*a261); + a261=(a276*a261); + a261=(a8*a261); + a271=(a24*a271); + a271=(a276*a271); + a271=(a8*a271); + a261=(a261+a271); + a230=(a365*a230); + a230=(a276*a230); + a261=(a261+a230); + a261=(a4*a261); + if (res[1]!=0) res[1][507]=a261; + a280=(a24*a280); + a280=(a276*a280); + a280=(a8*a280); + a283=(a24*a283); + a283=(a276*a283); + a283=(a8*a283); + a280=(a280+a283); + a286=(a365*a286); + a276=(a276*a286); + a280=(a280+a276); + a280=(a4*a280); + a280=(-a280); + if (res[1]!=0) res[1][508]=a280; + a227=(a365*a227); + a293=(a293*a227); + a231=(a365*a231); + a294=(a294*a231); + a293=(a293+a294); + a297=(a297*a293); + a298=(a298*a227); + a296=(a296*a231); + a298=(a298-a296); + a282=(a282*a298); + a297=(a297-a282); + if (res[1]!=0) res[1][509]=a297; + a297=(a33*a285); + a297=(a300*a297); + a297=(a8*a297); + a282=(a33*a295); + a282=(a300*a282); + a282=(a8*a282); + a297=(a297+a282); + a282=(a34*a254); + a282=(a300*a282); + a297=(a297+a282); + a297=(a4*a297); + if (res[1]!=0) res[1][510]=a297; + a297=(a33*a304); + a297=(a300*a297); + a297=(a8*a297); + a282=(a33*a307); + a282=(a300*a282); + a282=(a8*a282); + a297=(a297+a282); + a282=(a34*a310); + a282=(a300*a282); + a297=(a297+a282); + a297=(a4*a297); + a297=(-a297); + if (res[1]!=0) res[1][511]=a297; + a297=(a34*a251); + a282=(a317*a297); + a298=(a34*a255); + a296=(a318*a298); + a282=(a282+a296); + a282=(a321*a282); + a297=(a322*a297); + a298=(a320*a298); + a297=(a297-a298); + a297=(a306*a297); + a282=(a282-a297); + if (res[1]!=0) res[1][512]=a282; + a282=(a32*a309); + a282=(a324*a282); + a282=(a8*a282); + a297=(a32*a319); + a297=(a324*a297); + a297=(a8*a297); + a282=(a282+a297); + a297=(a12*a278); + a297=(a324*a297); + a282=(a282+a297); + a282=(a4*a282); + if (res[1]!=0) res[1][513]=a282; + a282=(a32*a328); + a282=(a324*a282); + a282=(a8*a282); + a297=(a32*a331); + a297=(a324*a297); + a297=(a8*a297); + a282=(a282+a297); + a297=(a12*a334); + a297=(a324*a297); + a282=(a282+a297); + a282=(a4*a282); + a282=(-a282); + if (res[1]!=0) res[1][514]=a282; + a282=(a12*a275); + a297=(a341*a282); + a298=(a12*a279); + a296=(a342*a298); + a297=(a297+a296); + a297=(a345*a297); + a282=(a346*a282); + a298=(a344*a298); + a282=(a282-a298); + a282=(a330*a282); + a297=(a297-a282); + if (res[1]!=0) res[1][515]=a297; + a297=(a364*a333); + a297=(a348*a297); + a297=(a8*a297); + a282=(a364*a343); + a282=(a348*a282); + a282=(a8*a282); + a297=(a297+a282); + a282=(a29*a323); + a282=(a348*a282); + a297=(a297+a282); + a297=(a4*a297); + if (res[1]!=0) res[1][516]=a297; + a297=(a364*a352); + a297=(a348*a297); + a297=(a8*a297); + a364=(a364*a9); + a364=(a348*a364); + a364=(a8*a364); + a297=(a297+a364); + a29=(a29*a20); + a29=(a348*a29); + a297=(a297+a29); + a297=(a4*a297); + a297=(-a297); + if (res[1]!=0) res[1][517]=a297; + a297=(a17*a299); + a29=(a358*a297); + a17=(a17*a303); + a364=(a351*a17); + a29=(a29+a364); + a29=(a354*a29); + a297=(a362*a297); + a17=(a356*a17); + a297=(a297-a17); + a297=(a13*a297); + a29=(a29-a297); + if (res[1]!=0) res[1][518]=a29; + a305=(a8*a305); + a301=(a301+a305); + a308=(a8*a308); + a301=(a301+a308); + a301=(a301+a311); + a301=(a4*a301); + a301=(-a301); + if (res[1]!=0) res[1][519]=a301; + a314=(a8*a314); + a312=(a312+a314); + a315=(a8*a315); + a312=(a312+a315); + a312=(a312+a316); + a312=(a4*a312); + a312=(-a312); + if (res[1]!=0) res[1][520]=a312; + a285=(a24*a285); + a285=(a300*a285); + a285=(a8*a285); + a295=(a24*a295); + a295=(a300*a295); + a295=(a8*a295); + a285=(a285+a295); + a254=(a365*a254); + a254=(a300*a254); + a285=(a285+a254); + a285=(a4*a285); + if (res[1]!=0) res[1][521]=a285; + a304=(a24*a304); + a304=(a300*a304); + a304=(a8*a304); + a307=(a24*a307); + a307=(a300*a307); + a307=(a8*a307); + a304=(a304+a307); + a310=(a365*a310); + a300=(a300*a310); + a304=(a304+a300); + a304=(a4*a304); + a304=(-a304); + if (res[1]!=0) res[1][522]=a304; + a251=(a365*a251); + a317=(a317*a251); + a255=(a365*a255); + a318=(a318*a255); + a317=(a317+a318); + a321=(a321*a317); + a322=(a322*a251); + a320=(a320*a255); + a322=(a322-a320); + a306=(a306*a322); + a321=(a321-a306); + if (res[1]!=0) res[1][523]=a321; + a321=(a33*a309); + a321=(a324*a321); + a321=(a8*a321); + a306=(a33*a319); + a306=(a324*a306); + a306=(a8*a306); + a321=(a321+a306); + a306=(a34*a278); + a306=(a324*a306); + a321=(a321+a306); + a321=(a4*a321); + if (res[1]!=0) res[1][524]=a321; + a321=(a33*a328); + a321=(a324*a321); + a321=(a8*a321); + a306=(a33*a331); + a306=(a324*a306); + a306=(a8*a306); + a321=(a321+a306); + a306=(a34*a334); + a306=(a324*a306); + a321=(a321+a306); + a321=(a4*a321); + a321=(-a321); + if (res[1]!=0) res[1][525]=a321; + a321=(a34*a275); + a306=(a341*a321); + a322=(a34*a279); + a320=(a342*a322); + a306=(a306+a320); + a306=(a345*a306); + a321=(a346*a321); + a322=(a344*a322); + a321=(a321-a322); + a321=(a330*a321); + a306=(a306-a321); + if (res[1]!=0) res[1][526]=a306; + a306=(a32*a333); + a306=(a348*a306); + a306=(a8*a306); + a321=(a32*a343); + a321=(a348*a321); + a321=(a8*a321); + a306=(a306+a321); + a321=(a12*a323); + a321=(a348*a321); + a306=(a306+a321); + a306=(a4*a306); + if (res[1]!=0) res[1][527]=a306; + a306=(a32*a352); + a306=(a348*a306); + a306=(a8*a306); + a32=(a32*a9); + a32=(a348*a32); + a32=(a8*a32); + a306=(a306+a32); + a32=(a12*a20); + a32=(a348*a32); + a306=(a306+a32); + a306=(a4*a306); + a306=(-a306); + if (res[1]!=0) res[1][528]=a306; + a306=(a12*a299); + a32=(a358*a306); + a12=(a12*a303); + a321=(a351*a12); + a32=(a32+a321); + a32=(a354*a32); + a306=(a362*a306); + a12=(a356*a12); + a306=(a306-a12); + a306=(a13*a306); + a32=(a32-a306); + if (res[1]!=0) res[1][529]=a32; + a329=(a8*a329); + a325=(a325+a329); + a332=(a8*a332); + a325=(a325+a332); + a325=(a325+a335); + a325=(a4*a325); + a325=(-a325); + if (res[1]!=0) res[1][530]=a325; + a338=(a8*a338); + a336=(a336+a338); + a339=(a8*a339); + a336=(a336+a339); + a336=(a336+a340); + a336=(a4*a336); + a336=(-a336); + if (res[1]!=0) res[1][531]=a336; + a309=(a24*a309); + a309=(a324*a309); + a309=(a8*a309); + a319=(a24*a319); + a319=(a324*a319); + a319=(a8*a319); + a309=(a309+a319); + a278=(a365*a278); + a278=(a324*a278); + a309=(a309+a278); + a309=(a4*a309); + if (res[1]!=0) res[1][532]=a309; + a328=(a24*a328); + a328=(a324*a328); + a328=(a8*a328); + a331=(a24*a331); + a331=(a324*a331); + a331=(a8*a331); + a328=(a328+a331); + a334=(a365*a334); + a324=(a324*a334); + a328=(a328+a324); + a328=(a4*a328); + a328=(-a328); + if (res[1]!=0) res[1][533]=a328; + a275=(a365*a275); + a341=(a341*a275); + a279=(a365*a279); + a342=(a342*a279); + a341=(a341+a342); + a345=(a345*a341); + a346=(a346*a275); + a344=(a344*a279); + a346=(a346-a344); + a330=(a330*a346); + a345=(a345-a330); + if (res[1]!=0) res[1][534]=a345; + a345=(a33*a333); + a345=(a348*a345); + a345=(a8*a345); + a330=(a33*a343); + a330=(a348*a330); + a330=(a8*a330); + a345=(a345+a330); + a330=(a34*a323); + a330=(a348*a330); + a345=(a345+a330); + a345=(a4*a345); + if (res[1]!=0) res[1][535]=a345; + a345=(a33*a352); + a345=(a348*a345); + a345=(a8*a345); + a33=(a33*a9); + a33=(a348*a33); + a33=(a8*a33); + a345=(a345+a33); + a33=(a34*a20); + a33=(a348*a33); + a345=(a345+a33); + a345=(a4*a345); + a345=(-a345); + if (res[1]!=0) res[1][536]=a345; + a345=(a34*a299); + a33=(a358*a345); + a34=(a34*a303); + a330=(a351*a34); + a33=(a33+a330); + a33=(a354*a33); + a345=(a362*a345); + a34=(a356*a34); + a345=(a345-a34); + a345=(a13*a345); + a33=(a33-a345); + if (res[1]!=0) res[1][537]=a33; + a353=(a8*a353); + a349=(a349+a353); + a355=(a8*a355); + a349=(a349+a355); + a349=(a349+a357); + a349=(a4*a349); + a349=(-a349); + if (res[1]!=0) res[1][538]=a349; + a359=(a8*a359); + a350=(a350+a359); + a360=(a8*a360); + a350=(a350+a360); + a350=(a350+a361); + a350=(a4*a350); + a350=(-a350); + if (res[1]!=0) res[1][539]=a350; + a333=(a24*a333); + a333=(a348*a333); + a333=(a8*a333); + a343=(a24*a343); + a343=(a348*a343); + a343=(a8*a343); + a333=(a333+a343); + a323=(a365*a323); + a323=(a348*a323); + a333=(a333+a323); + a333=(a4*a333); + if (res[1]!=0) res[1][540]=a333; + a352=(a24*a352); + a352=(a348*a352); + a352=(a8*a352); + a24=(a24*a9); + a24=(a348*a24); + a8=(a8*a24); + a352=(a352+a8); + a20=(a365*a20); + a348=(a348*a20); + a352=(a352+a348); + a4=(a4*a352); + a4=(-a4); + if (res[1]!=0) res[1][541]=a4; + a299=(a365*a299); + a358=(a358*a299); + a365=(a365*a303); + a351=(a351*a365); + a358=(a358+a351); + a354=(a354*a358); + a362=(a362*a299); + a356=(a356*a365); + a362=(a362-a356); + a13=(a13*a362); + a354=(a354-a13); + if (res[1]!=0) res[1][542]=a354; + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_jac_g(const casadi_real** arg, casadi_real** res, casadi_int* iw, casadi_real* w, int mem){ + return casadi_f6(arg, res, iw, w, mem); +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_jac_g_alloc_mem(void) { + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_jac_g_init_mem(int mem) { + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_jac_g_free_mem(int mem) { +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_jac_g_checkout(void) { + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_jac_g_release(int mem) { +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_jac_g_incref(void) { +} + +extern "C" CASADI_SYMBOL_EXPORT void nlp_jac_g_decref(void) { +} + +extern "C" CASADI_SYMBOL_EXPORT casadi_int nlp_jac_g_n_in(void) { return 2;} + +extern "C" CASADI_SYMBOL_EXPORT casadi_int nlp_jac_g_n_out(void) { return 2;} + +extern "C" CASADI_SYMBOL_EXPORT casadi_real nlp_jac_g_default_in(casadi_int i) { + switch (i) { + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const char* nlp_jac_g_name_in(casadi_int i) { + switch (i) { + case 0: return "x"; + case 1: return "p"; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const char* nlp_jac_g_name_out(casadi_int i) { + switch (i) { + case 0: return "g"; + case 1: return "jac_g_x"; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const casadi_int* nlp_jac_g_sparsity_in(casadi_int i) { + switch (i) { + case 0: return casadi_s0; + case 1: return casadi_s1; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT const casadi_int* nlp_jac_g_sparsity_out(casadi_int i) { + switch (i) { + case 0: return casadi_s3; + case 1: return casadi_s5; + default: return 0; + } +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_jac_g_work(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 2; + if (sz_res) *sz_res = 2; + if (sz_iw) *sz_iw = 0; + if (sz_w) *sz_w = 0; + return 0; +} + +extern "C" CASADI_SYMBOL_EXPORT int nlp_jac_g_work_bytes(casadi_int *sz_arg, casadi_int* sz_res, casadi_int *sz_iw, casadi_int *sz_w) { + if (sz_arg) *sz_arg = 2*sizeof(const casadi_real*); + if (sz_res) *sz_res = 2*sizeof(casadi_real*); + if (sz_iw) *sz_iw = 0*sizeof(casadi_int); + if (sz_w) *sz_w = 0*sizeof(casadi_real); + return 0; +} + + diff --git a/main/src/vtr_path_planning/src/mpc/speed_scheduler.cpp b/main/src/vtr_path_planning/src/mpc/speed_scheduler.cpp index 480787137..631c3af90 100644 --- a/main/src/vtr_path_planning/src/mpc/speed_scheduler.cpp +++ b/main/src/vtr_path_planning/src/mpc/speed_scheduler.cpp @@ -19,65 +19,48 @@ #include "vtr_path_planning/mpc/speed_scheduler.hpp" #include -#include - -double ScheduleSpeed(const std::vector& disc_path_curvature_xy, const std::vector& disc_path_curvature_xz_yz, double VF, unsigned curr_sid, double planar_curv_weight, double profile_curv_weight, double eop_weight, double horizon_step_size, double min_vel) +namespace vtr::path_planning { + double ScheduleSpeed(tactic::LocalizationChain::Ptr chain, const SpeedSchedConfig& params) { + + unsigned curr_sid = chain->trunkSequenceId(); - // Experimental Speed Scheduler: - // Takes in the desired forward_velocity and the pre-processed global path and reduces the set speed based on a range of tunable factors: - // 1. Planar Curvature - // 2. Profile Curvature - // 3. End of Path + CLOG(DEBUG, "mpc.speed_scheduler") << "TRYING TO SCHEDULE SPEED:"; + CLOG(DEBUG, "mpc.speed_scheduler") << "CURRENT SID IS:" << curr_sid; - // Pseudocode: - // - Estimate the current p value of the vehicle (doesnt need to be super precise so here we can imply opt to use the sid value) - // - Average the radius of curvature in the upcoming segments of the path + double VF_EOP = std::max(params.min_vel, params.target_vel * (params.eop_weight * (chain->size()- 1 - curr_sid) / 20.0)); + double VF_SOP = std::max(params.min_vel, params.target_vel * (params.eop_weight * curr_sid / 10)); - // Basic implementation - weights hardcoded for now - CLOG(INFO, "mpc.speed_scheduler") << "TRYING TO SCHEDULE SPEED:"; - CLOG(INFO, "mpc.speed_scheduler") << "CURRENT SID IS:" << curr_sid; - double VF_EOP; - double VF_XY; - double VF_XZ_YZ; - double avg_curvature_xy = 0.0; - double avg_curvature_xz_yz = 0.0; - double end_of_path = 0.0; - unsigned horizon_steps = 5.0 / horizon_step_size; // Set lookahead horizon to 5m (default params tuned for this value) - for (size_t i = curr_sid; i < curr_sid + horizon_steps; i++) - { - // Handle end of path case - if (i == (disc_path_curvature_xy.size()-1)) - { - end_of_path = 1.0; - break; - } - avg_curvature_xy = avg_curvature_xy + disc_path_curvature_xy[i]; - avg_curvature_xz_yz = avg_curvature_xz_yz + disc_path_curvature_xz_yz[i]; + double VF_XY; + double avg_curvature = 0.0; + unsigned window_steps = 0; + for (auto itr = chain->begin(curr_sid); itr != chain->end() && unsigned(itr) < curr_sid + params.horizon_steps; itr++) { + const auto curvature = CurvatureInfo::fromTransform(itr->T()); + avg_curvature += abs(curvature.curvature()); + ++window_steps; } - avg_curvature_xy = avg_curvature_xy / horizon_steps; - avg_curvature_xz_yz = avg_curvature_xz_yz / horizon_steps; - CLOG(INFO, "mpc.speed_scheduler") << "THE AVERAGE PLANAR CURVATURE IS: " << avg_curvature_xy; - CLOG(INFO, "mpc.speed_scheduler") << "THE AVERAGE PROFILE CURVATURE IS: " << avg_curvature_xz_yz; + avg_curvature /= window_steps; + CLOG(DEBUG, "mpc.speed_scheduler") << "THE AVERAGE CURVATURE IS: " << avg_curvature; - // handle forward/referse case and calculate a candidate VF speed for each of our scheduler modules (XY curvature, XZ curvature, End of Path etc) + // handle forward/reverse case and calculate a candidate VF speed for each of our scheduler modules (XY curvature, XZ curvature, End of Path etc) - VF_EOP = std::max(min_vel, VF / (1 + (end_of_path * end_of_path * eop_weight))); - VF_XY = std::max(min_vel, VF / (1 + (avg_curvature_xy * avg_curvature_xy * planar_curv_weight))); - VF_XZ_YZ = std::max(min_vel, VF / (1 + (avg_curvature_xz_yz * avg_curvature_xz_yz * profile_curv_weight))); + VF_XY = std::max(params.min_vel, params.target_vel / (1 + (avg_curvature * avg_curvature * params.planar_curv_weight))); // Take the minimum of all candidate (positive) scheduled speeds (Lowest allowed scheduled velocity is 0.5m/s, should be left this way) - VF = std::min({VF_EOP, VF_XY, VF_XZ_YZ}); - CLOG(INFO, "mpc.speed_scheduler") << "THE VF_EOP SPEED IS: " << VF_EOP; - CLOG(INFO, "mpc.speed_scheduler") << "THE VF_XY SPEED IS: " << VF_XY; - CLOG(INFO, "mpc.speed_scheduler") << "THE VF_XZ SPEED IS: " << VF_XZ_YZ; + double VF = std::min({VF_EOP, VF_SOP, VF_XY}); + CLOG(DEBUG, "mpc.speed_scheduler") << "THE VF_EOP SPEED IS: " << VF_EOP; + CLOG(DEBUG, "mpc.speed_scheduler") << "THE VF_SOP SPEED IS: " << VF_SOP; + CLOG(DEBUG, "mpc.speed_scheduler") << "THE VF_XY SPEED IS: " << VF_XY; // Take the minimum of all candidate scheduled speeds - CLOG(INFO, "mpc.speed_scheduler") << "THE SPEED SCHEDULED SPEED IS: " << VF; + CLOG(DEBUG, "mpc.speed_scheduler") << "THE SPEED SCHEDULED SPEED IS: " << VF; // End of speed scheduler code // Return the scheduled speed return VF; -} \ No newline at end of file + } + + +} // namespace vtr::path_planning \ No newline at end of file diff --git a/main/src/vtr_path_planning/test/closed_loop/unicycle_tests.cpp b/main/src/vtr_path_planning/test/closed_loop/unicycle_tests.cpp new file mode 100644 index 000000000..9bd998b99 --- /dev/null +++ b/main/src/vtr_path_planning/test/closed_loop/unicycle_tests.cpp @@ -0,0 +1,559 @@ +#include +#include + +#include "vtr_logging/logging_init.hpp" + +#include "lgmath.hpp" +#include "vtr_path_planning/mpc/mpc_path_planner.hpp" +#include "vtr_tactic/cache.hpp" + +using namespace ::testing; +using namespace vtr; +using namespace vtr::tactic; +using namespace vtr::pose_graph; +using namespace vtr::path_planning; + +constexpr unsigned NUM_STEPS = 15; + + +void print(const tactic::LocalizationChain& chain) { + CLOG(INFO, "test") << "trunk sid: " << chain.trunkSequenceId() + << ", trunk vid: " << chain.trunkVertexId(); + CLOG(INFO, "test") << "T_branch_trunk: " + << chain.T_branch_trunk().vec().transpose(); + CLOG(INFO, "test") << "branch sid: " << chain.branchSequenceId() + << ", branch vid: " << chain.branchVertexId(); + CLOG(INFO, "test") << "T_twig_branch: " + << chain.T_twig_branch().vec().transpose(); + CLOG(INFO, "test") << "twig vid: " << chain.twigVertexId(); + CLOG(INFO, "test") << "T_petiole_twig: " + << chain.T_petiole_twig().vec().transpose(); + CLOG(INFO, "test") << "petiole vid: " << chain.petioleVertexId(); + CLOG(INFO, "test") << "T_leaf_petiole: " + << chain.T_leaf_petiole().vec().transpose(); +} + + + +using SE2Pose = std::tuple; + +class ChainTest : public TestWithParam { + public: + ChainTest() : graph_(new RCGraph(std::string("temp/") + std::string(UnitTest::GetInstance()->current_test_info()->name()), false)), chain_(new tactic::LocalizationChain(graph_)) { + P_tran << 1, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 1; + } + ~ChainTest() override {} + + void createConstantCurve(const EdgeTransform& T_wr0, const EdgeTransform& T_wrf, const Timestamp& t1, const Timestamp& t2, unsigned nodes=2, bool is_teach=true) { + ASSERT_GE(nodes, 2); + ASSERT_GT(t2, t1); + + EdgeTransform edge = T_wr0.inverse() * T_wrf; + Timestamp delta_t = t2 - t2; + + auto start_v = graph_->addVertex(t1); + uint64_t run_id = start_v->id().majorId(); + uint64_t minor_offset = start_v->id().minorId(); + start_v->SetTerrainType(5); + for(unsigned i = 1; i < nodes; ++i) { + double interp = -1.0 / (nodes - 1); + auto interp_tf = EdgeTransform(interp * edge.vec(), 0); + auto v = graph_->addVertex(t1 + interp * delta_t); + v->SetTerrainType(5); + + interp_tf.setZeroCovariance(); + graph_->addEdge(VertexId(run_id, i - 1 + minor_offset), VertexId(run_id, i + minor_offset), EdgeType::Temporal, + is_teach, interp_tf); + } + + } + + void SetUp() override { + graph_->addRun(); + + createConstantCurve(tf_from_global(0, 0, 0), tf_from_global(20.0, 0.0, 0), 0, 10*1e9, 20); + createConstantCurve(tf_from_global(20.5, 0, 0), tf_from_global(25.5, 5.0, M_PI_2), 10*1e9, 20*1e9, 20); + graph_->addEdge(VertexId(0, 19), VertexId(0, 20), EdgeType::Temporal, + true, tf_from_global(0.5, 0.0, 0).inverse()); + + + using PrivEvaluator = eval::mask::privileged::CachedEval; + auto eval = std::make_shared(*graph_); + auto path = graph_->getSubgraph(0ul, eval); + VertexId::Vector sequence; + for (auto it = path->begin(0ul); it != path->end(); ++it) + sequence.push_back(it->v()->id()); + chain_->setSequence(sequence); + chain_->expand(); + } + + void TearDown() override {} + + protected: + size_t num_vertices_ = 20; + RCGraph::Ptr graph_; + tactic::LocalizationChain::Ptr chain_; + uint64_t t_offset_ = 4*60*1e9; + Eigen::Matrix P_tran; + +}; + + +class RealChainTest : public TestWithParam { + public: + RealChainTest() : graph_(new RCGraph(GetParam(), true)), chain_(new tactic::LocalizationChain(graph_)) { + P_tran << 1, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 0, + 0, 1; + } + ~RealChainTest() override {} + + + void SetUp() override { + CLOG(DEBUG, "test") << "Starting set up"; + + using PrivEvaluator = eval::mask::privileged::CachedEval; + using ForwardEvaluator = eval::mask::direction_from_vertex::Eval; + auto teachEval = std::make_shared(*graph_); + auto fwdEval = std::make_shared(2ul); + auto path = graph_->getSubgraph(0ul, teachEval); + CLOG(DEBUG, "test") << "Got subgraph"; + + VertexId::Vector sequence; + for (auto it = path->begin(2ul, 0, fwdEval); it != path->end(); ++it) + sequence.push_back(it->v()->id()); + chain_->setSequence(sequence); + chain_->expand(); + } + + void TearDown() override {} + + protected: + RCGraph::Ptr graph_; + tactic::LocalizationChain::Ptr chain_; + uint64_t t_offset_ = 4*60*1e9; + Eigen::Matrix P_tran; + +}; + +template +Eigen::Matrix normalVector(double mu, Eigen::Matrix sigmas) { + static std::random_device rd{}; + static std::mt19937 gen{rd()}; + + // values near the mean are the most likely + // standard deviation affects the dispersion of generated values from the mean + Eigen::Matrix out; + for(unsigned i = 0; i < dim; ++i) { + std::normal_distribution d{mu, sigmas(i)}; + out(i) = d(gen); + } + return out; +} + +TEST_P(ChainTest, NoiseFreeMPC) { + //Init localization + uint64_t repeat_run = graph_->addRun(); + + SE2Pose init_pose = GetParam(); + auto x_init = std::get<0>(init_pose); + auto y_init = std::get<1>(init_pose); + auto theta_init = std::get<2>(init_pose); + + EdgeTransform T_init = tf_from_global(x_init, y_init, theta_init); + graph_->addVertex(t_offset_); + graph_->addEdge(VertexId(repeat_run, 0), VertexId(0, 0), EdgeType::Spatial, + false, T_init); + + ASSERT_GT(chain_->p(chain_->size() - 1), 0); + + // initialize the localization chain + chain_->setPetiole(VertexId(repeat_run, 0)); + auto live_id = chain_->petioleVertexId(); + auto map_id = chain_->trunkVertexId(); + auto map_sid = chain_->trunkSequenceId(); + chain_->updateBranchToTwigTransform(live_id, map_id, map_sid, + T_init.inverse(), true, false); + + print(*chain_); + + + CasadiUnicycleMPC solver; + CasadiUnicycleMPC::Config config; + config.VF = 1.0; + config.vel_max = {1.5, 0.5}; + + Eigen::Vector2d last_velo = {0.0, 0.0}; + config.previous_vel = {0.0, 0.0}; + config.T0 = {x_init, y_init, theta_init}; + + double state_p = findRobotP(chain_->T_start_leaf(), chain_); + + for(int i = 1; i < config.N+1; i++){ + config.reference_poses.push_back({i*config.VF*config.DT, 0, 0}); + } + + Eigen::Matrix2d alpha = Eigen::Matrix2d::Zero(); + // alpha.diagonal() << 0, config.alpha; + const Eigen::Matrix2d one_m_alpha = Eigen::Matrix2d::Identity() - alpha; + + auto mpc_res = solver.solve(config); + CLOG(DEBUG, "test") << "Shape: (" << mpc_res["pose"].rows() << ", " << mpc_res["pose"].columns() << ")"; + + std::vector mpc_poses; + for(int i = 0; i < mpc_res["pose"].columns(); i++) { + const auto& pose_i = mpc_res["pose"](casadi::Slice(), i).get_elements(); + mpc_poses.push_back(tf_from_global(pose_i[0], pose_i[1], pose_i[2])); + } + + for(unsigned i = 0; i < 20*NUM_STEPS; ++i) { + auto mpc_res = solver.solve(config); + const auto& mpc_vel = mpc_res["vel"](casadi::Slice(), 0).get_elements(); + + Eigen::Vector2d applied_vel; + applied_vel << mpc_vel[0], mpc_vel[1]; + + EXPECT_TRUE(applied_vel.allFinite()); + auto new_velo = alpha * last_velo + one_m_alpha * applied_vel; + + EdgeTransform delta_TF = EdgeTransform{-config.DT * P_tran * new_velo, 0}; + delta_TF.setZeroCovariance(); + auto new_vertex = graph_->addVertex(t_offset_ + (i + 1) * config.DT * 1e9); + graph_->addEdge(new_vertex->id() - 1, new_vertex->id(), EdgeType::Temporal, + false, delta_TF); + + chain_->updatePetioleToLeafTransform(t_offset_ + (i + 1) * config.DT * 1e9, P_tran * new_velo, + delta_TF, false); + + last_velo = new_velo; + + chain_->setPetiole(new_vertex->id()); + auto live_id = chain_->petioleVertexId(); + auto map_id = chain_->trunkVertexId(); + auto map_sid = chain_->trunkSequenceId(); + chain_->updateBranchToTwigTransform(live_id, map_id, map_sid, + chain_->T_leaf_trunk(), true, false); + + state_p = findRobotP(chain_->T_start_leaf(), chain_); + std::vector p_rollout; + for(int j = 1; j < config.N+1; j++){ + p_rollout.push_back(state_p + j*config.VF*config.DT); + } + + config.reference_poses.clear(); + auto referenceInfo = generateHomotopyReference(p_rollout, chain_); + for(const auto& Tf : referenceInfo.poses) { + config.reference_poses.push_back(tf_to_global(Tf)); + CLOG(DEBUG, "test") << "Target " << tf_to_global(Tf); + } + + config.up_barrier_q = referenceInfo.barrier_q_max; + config.low_barrier_q = referenceInfo.barrier_q_min; + + config.previous_vel = {last_velo(0, 0), last_velo(1, 0)}; + config.T0 = tf_to_global(chain_->T_start_leaf()); + } + + uint64_t mpc_rollout_run = graph_->addRun(); + graph_->addVertex(t_offset_); + graph_->addEdge(VertexId(mpc_rollout_run, 0), VertexId(0, 0), EdgeType::Spatial, + false, T_init); + + for(unsigned i = 0; i < config.N; ++i) { + auto new_vertex = graph_->addVertex(t_offset_ + (i + 1) * config.DT * 1e9); + graph_->addEdge(new_vertex->id() - 1, new_vertex->id(), EdgeType::Temporal, + false, mpc_poses[i+1].inverse()*mpc_poses[i]); + } + +} + + +TEST_P(ChainTest, NoisyMPC) { + + //Init localization + uint64_t repeat_run = graph_->addRun(); + + SE2Pose init_pose = GetParam(); + auto x_init = std::get<0>(init_pose); + auto y_init = std::get<1>(init_pose); + auto theta_init = std::get<2>(init_pose); + + EdgeTransform T_init = tf_from_global(x_init, y_init, theta_init); + graph_->addVertex(t_offset_); + graph_->addEdge(VertexId(repeat_run, 0), VertexId(0, 0), EdgeType::Spatial, + false, T_init); + + ASSERT_GT(chain_->p(chain_->size() - 1), 0); + + // initialize the localization chain + chain_->setPetiole(VertexId(repeat_run, 0)); + auto live_id = chain_->petioleVertexId(); + auto map_id = chain_->trunkVertexId(); + auto map_sid = chain_->trunkSequenceId(); + chain_->updateBranchToTwigTransform(live_id, map_id, map_sid, + T_init.inverse(), true, false); + + print(*chain_); + + + CasadiUnicycleMPC solver; + CasadiUnicycleMPC::Config config; + config.VF = 1.0; + config.vel_max = {1.5, 0.5}; + + Eigen::Vector2d last_velo = {0.0, 0.0}; + config.previous_vel = {0.0, 0.0}; + config.T0 = {x_init, y_init, theta_init}; + + double state_p = findRobotP(chain_->T_start_leaf(), chain_); + + for(int i = 1; i < config.N+1; i++){ + config.reference_poses.push_back({i*config.VF*config.DT, 0, 0}); + } + + Eigen::Matrix2d alpha = Eigen::Matrix2d::Zero(); + // alpha.diagonal() << 0, config.alpha; + const Eigen::Matrix2d one_m_alpha = Eigen::Matrix2d::Identity() - alpha; + + auto mpc_res = solver.solve(config); + CLOG(DEBUG, "test") << "Shape: (" << mpc_res["pose"].rows() << ", " << mpc_res["pose"].columns() << ")"; + + std::vector mpc_poses; + for(int i = 0; i < mpc_res["pose"].columns(); i++) { + const auto& pose_i = mpc_res["pose"](casadi::Slice(), i).get_elements(); + mpc_poses.push_back(tf_from_global(pose_i[0], pose_i[1], pose_i[2])); + } + + for(unsigned i = 0; i < 20*NUM_STEPS; ++i) { + auto mpc_res = solver.solve(config); + const auto& mpc_vel = mpc_res["vel"](casadi::Slice(), 0).get_elements(); + + Eigen::Vector2d applied_vel; + applied_vel << mpc_vel[0], mpc_vel[1]; + + EXPECT_TRUE(applied_vel.allFinite()); + auto new_velo = alpha * last_velo + one_m_alpha * applied_vel; + + EdgeTransform delta_TF = EdgeTransform{-config.DT * P_tran * new_velo + normalVector<6>(0, {0.1, 0.1, 0, 0.001, 0.001, 0.01}), 0}; + delta_TF.setZeroCovariance(); + auto new_vertex = graph_->addVertex(t_offset_ + (i + 1) * config.DT * 1e9); + graph_->addEdge(new_vertex->id() - 1, new_vertex->id(), EdgeType::Temporal, + false, delta_TF); + + chain_->updatePetioleToLeafTransform(t_offset_ + (i + 1) * config.DT * 1e9, P_tran * new_velo, + delta_TF, false); + + last_velo = new_velo + normalVector<2>(0, {0.1, 0.1}); + + chain_->setPetiole(new_vertex->id()); + auto live_id = chain_->petioleVertexId(); + auto map_id = chain_->trunkVertexId(); + auto map_sid = chain_->trunkSequenceId(); + chain_->updateBranchToTwigTransform(live_id, map_id, map_sid, + chain_->T_leaf_trunk(), true, false); + + state_p = findRobotP(chain_->T_start_leaf(), chain_); + std::vector p_rollout; + for(int j = 1; j < config.N+1; j++){ + p_rollout.push_back(state_p + j*config.VF*config.DT); + } + + config.reference_poses.clear(); + auto referenceInfo = generateHomotopyReference(p_rollout, chain_); + for(const auto& Tf : referenceInfo.poses) { + config.reference_poses.push_back(tf_to_global(Tf)); + CLOG(DEBUG, "test") << "Target " << tf_to_global(Tf); + } + + config.up_barrier_q = referenceInfo.barrier_q_max; + config.low_barrier_q = referenceInfo.barrier_q_min; + + config.previous_vel = {last_velo(0, 0), last_velo(1, 0)}; + config.T0 = tf_to_global(chain_->T_start_leaf()); + } + + uint64_t mpc_rollout_run = graph_->addRun(); + graph_->addVertex(t_offset_); + graph_->addEdge(VertexId(mpc_rollout_run, 0), VertexId(0, 0), EdgeType::Spatial, + false, T_init); + + for(unsigned i = 0; i < config.N; ++i) { + auto new_vertex = graph_->addVertex(t_offset_ + (i + 1) * config.DT * 1e9); + graph_->addEdge(new_vertex->id() - 1, new_vertex->id(), EdgeType::Temporal, + false, mpc_poses[i+1].inverse()*mpc_poses[i]); + } + +} + + +// INSTANTIATE_TEST_SUITE_P(MPCSet, ChainTest, Values(std::make_tuple(0.0, 0.0, 0.0), std::make_tuple(0.0, 0.5, 0.0), std::make_tuple(0.0, -0.5, 0.0))); + + + +TEST_P(RealChainTest, NoiseFreeRealMPC) { + //Init localization + uint64_t repeat_run = graph_->addRun(); + + double x_init = 0; + double y_init = -0.2; + double theta_init = 0; + + EdgeTransform T_init = tf_from_global(x_init, y_init, theta_init); + graph_->addVertex(t_offset_); + graph_->addEdge(VertexId(repeat_run, 0), VertexId(0, 2), EdgeType::Spatial, + false, T_init); + + ASSERT_GT(chain_->p(chain_->size() - 1), 0); + + // initialize the localization chain + chain_->setPetiole(VertexId(repeat_run, 0)); + auto live_id = chain_->petioleVertexId(); + auto map_id = chain_->trunkVertexId(); + auto map_sid = chain_->trunkSequenceId(); + chain_->updateBranchToTwigTransform(live_id, map_id, map_sid, + T_init.inverse(), true, false); + + + CasadiUnicycleMPC solver; + CasadiUnicycleMPC::Config config; + config.VF = 1.0; + config.vel_max = {1.5, 0.5}; + + Eigen::Vector2d last_velo = {0.0, 0.0}; + config.previous_vel = {0.0, 0.0}; + config.T0 = {x_init, y_init, theta_init}; + + std::vector target_poses_0; + + double state_p = findRobotP(chain_->T_start_leaf(), chain_); + std::vector p_rollout; + for(int j = 1; j < config.N+1; j++){ + p_rollout.push_back(state_p + j*config.VF*config.DT); + } + + config.reference_poses.clear(); + auto referenceInfo = generateHomotopyReference(p_rollout, chain_); + target_poses_0.push_back(referenceInfo.poses.front()); + for(const auto& Tf : referenceInfo.poses) { + config.reference_poses.push_back(tf_to_global(chain_->T_start_trunk().inverse() * Tf)); + CLOG(DEBUG, "test") << "Target " << tf_to_global(Tf); + } + + config.up_barrier_q = referenceInfo.barrier_q_max; + config.low_barrier_q = referenceInfo.barrier_q_min; + + Eigen::Matrix2d alpha = Eigen::Matrix2d::Zero(); + alpha.diagonal() << 0, config.alpha; + const Eigen::Matrix2d one_m_alpha = Eigen::Matrix2d::Identity() - alpha; + + auto mpc_res = solver.solve(config); + CLOG(DEBUG, "test") << "Shape: (" << mpc_res["pose"].rows() << ", " << mpc_res["pose"].columns() << ")"; + + std::vector mpc_poses; + for(int i = 0; i < mpc_res["pose"].columns(); i++) { + const auto& pose_i = mpc_res["pose"](casadi::Slice(), i).get_elements(); + mpc_poses.push_back(tf_from_global(pose_i[0], pose_i[1], pose_i[2])); + } + + + + for(unsigned i = 0; i < 250 / config.DT; ++i) { + auto mpc_res = solver.solve(config); + const auto& mpc_vel = mpc_res["vel"](casadi::Slice(), 0).get_elements(); + + Eigen::Vector2d applied_vel; + applied_vel << mpc_vel[0], mpc_vel[1]; + + EXPECT_TRUE(applied_vel.allFinite()); + auto new_velo = alpha * last_velo + one_m_alpha * applied_vel; + + EdgeTransform delta_TF = EdgeTransform{-config.DT * P_tran * new_velo, 0}; + delta_TF.setZeroCovariance(); + auto new_vertex = graph_->addVertex(t_offset_ + (i + 1) * config.DT * 1e9); + graph_->addEdge(new_vertex->id() - 1, new_vertex->id(), EdgeType::Temporal, + false, delta_TF); + + chain_->updatePetioleToLeafTransform(t_offset_ + (i + 1) * config.DT * 1e9, P_tran * new_velo, + delta_TF, false); + + last_velo = new_velo; + + chain_->setPetiole(new_vertex->id()); + auto live_id = chain_->petioleVertexId(); + auto map_id = chain_->trunkVertexId(); + auto map_sid = chain_->trunkSequenceId(); + chain_->updateBranchToTwigTransform(live_id, map_id, map_sid, + chain_->T_leaf_trunk(), true, false); + + state_p = findRobotP(chain_->T_start_leaf(), chain_); + std::vector p_rollout; + for(int j = 1; j < config.N+1; j++){ + p_rollout.push_back(state_p + j*config.VF*config.DT); + } + + config.reference_poses.clear(); + auto referenceInfo = generateHomotopyReference(p_rollout, chain_); + target_poses_0.push_back(referenceInfo.poses.front()); + for(const auto& Tf : referenceInfo.poses) { + config.reference_poses.push_back(tf_to_global(chain_->T_start_trunk().inverse() * Tf)); + CLOG(DEBUG, "test") << "Target " << config.reference_poses.back(); + } + + config.up_barrier_q = referenceInfo.barrier_q_max; + config.low_barrier_q = referenceInfo.barrier_q_min; + + config.previous_vel = {last_velo(0, 0), last_velo(1, 0)}; + config.T0 = tf_to_global(chain_->T_leaf_trunk().inverse()); + + if (map_sid == chain_->size() - 1) + break; + } + + uint64_t mpc_rollout_run = graph_->addRun(); + graph_->addVertex(t_offset_); + graph_->addEdge(VertexId(mpc_rollout_run, 0), VertexId(0, 2), EdgeType::Spatial, + false, T_init); + + for(unsigned i = 0; i < config.N; ++i) { + auto new_vertex = graph_->addVertex(t_offset_ + (i + 1) * config.DT * 1e9); + graph_->addEdge(new_vertex->id() - 1, new_vertex->id(), EdgeType::Temporal, + false, mpc_poses[i+1].inverse()*mpc_poses[i]); + } + + + uint64_t target_pose_run = graph_->addRun(); + + auto new_vertex = graph_->addVertex(t_offset_); + auto start_vertex = graph_->addEdge(new_vertex->id(), VertexId(0, 2), EdgeType::Spatial, + false, target_poses_0.front()); + for(size_t i = 1; i < target_poses_0.size(); i++) { + auto new_vertex = graph_->addVertex(t_offset_ + i * config.DT * 1e9); + + graph_->addEdge(new_vertex->id() - 1, new_vertex->id(), EdgeType::Temporal, + false, target_poses_0[i].inverse()*target_poses_0[i-1]); + } + + +} + +INSTANTIATE_TEST_SUITE_P(RealMPC, RealChainTest, Values("temp/dome4eva")); + + +//Variations: +//Motion model (add low pass filter) +//Noise to applied and measured velocity and position +//Sampling frequency + +int main(int argc, char** argv) { + logging::configureLogging("", true); + InitGoogleTest(&argc, argv); + return RUN_ALL_TESTS(); +} diff --git a/main/src/vtr_path_planning_msgs/CMakeLists.txt b/main/src/vtr_path_planning_msgs/CMakeLists.txt new file mode 100644 index 000000000..929344fe0 --- /dev/null +++ b/main/src/vtr_path_planning_msgs/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.16) +project(vtr_path_planning_msgs) + +# Common setup for vtr packages +include("${CMAKE_CURRENT_LIST_DIR}/../vtr_common/vtr_include.cmake") + +# Find dependencies +find_package(ament_cmake REQUIRED) +find_package(rosidl_default_generators REQUIRED) +find_package(builtin_interfaces REQUIRED) +find_package(geometry_msgs REQUIRED) +find_package(vtr_common_msgs REQUIRED) + +# ROS messages +file(GLOB_RECURSE MSG_SRC RELATIVE ${CMAKE_CURRENT_SOURCE_DIR} msg/*.msg) + +rosidl_generate_interfaces(${PROJECT_NAME} + ${MSG_SRC} + DEPENDENCIES + builtin_interfaces + geometry_msgs + vtr_common_msgs +) + +ament_export_dependencies( + rosidl_default_runtime + std_msgs + vtr_common_msgs +) + +ament_package() diff --git a/main/src/vtr_path_planning_msgs/msg/PathInfoForExternalNavigation.msg b/main/src/vtr_path_planning_msgs/msg/PathInfoForExternalNavigation.msg new file mode 100644 index 000000000..fde6cb34a --- /dev/null +++ b/main/src/vtr_path_planning_msgs/msg/PathInfoForExternalNavigation.msg @@ -0,0 +1,23 @@ +std_msgs/Header header + + + + +# Path in robot frame - flattened Nx3 array +float32[] path + +# array of size 5x2 for the MPC v,w solution +float32[] mpc_solution + +# array of size 3x1 for robot x,y,theta pose in path frame +float32[3] robot_pose + +float32[3] closest_interpolated_node_pose + + +uint64 closest_node_idx +float32 p_value +float32 distance_to_closest_interpolated_node +float32 theta_offset_to_closest_interpolated_node +float32 distance_to_goal_node +float32 theta_offset_to_goal_node \ No newline at end of file diff --git a/main/src/vtr_path_planning_msgs/package.xml b/main/src/vtr_path_planning_msgs/package.xml new file mode 100644 index 000000000..dcc21cd8f --- /dev/null +++ b/main/src/vtr_path_planning_msgs/package.xml @@ -0,0 +1,26 @@ + + + vtr_path_planning_msgs + 0.0.0 + VTR path_planning messages. + james + Apache License 2.0 + + ament_cmake + rosidl_default_generators + + builtin_interfaces + geometry_msgs + vtr_common_msgs + + rosidl_default_runtime + + ament_cmake_gtest + ament_lint_auto + + rosidl_interface_packages + + + ament_cmake + + diff --git a/main/src/vtr_pose_graph/include/vtr_pose_graph/path/localization_chain.hpp b/main/src/vtr_pose_graph/include/vtr_pose_graph/path/localization_chain.hpp index a338baaee..5bef8faea 100644 --- a/main/src/vtr_pose_graph/include/vtr_pose_graph/path/localization_chain.hpp +++ b/main/src/vtr_pose_graph/include/vtr_pose_graph/path/localization_chain.hpp @@ -58,6 +58,7 @@ class LocalizationChain : public Path { int search_depth = 20; int search_back_depth = 10; double distance_warning = 3.0; + double alpha = 0.25; }; using Parent = Path; @@ -78,7 +79,7 @@ class LocalizationChain : public Path { PTR_TYPEDEFS(LocalizationChain); LocalizationChain(const Config &config, const typename Graph::Ptr &graph) - : Parent(graph), config_(config) {} + : Parent(graph, config.alpha), config_(config) {} LocalizationChain(const typename Graph::Ptr &graph) : LocalizationChain(Config(), graph) {} @@ -152,19 +153,19 @@ class LocalizationChain : public Path { } /// What is the privileged vehicle pose (relative to the start of the path) - EdgeTransform T_start_twig() { + EdgeTransform T_start_twig() const { LockGuard lock(this->mutex_); return this->pose(trunk_sid_) * T_twig_trunk().inverse(); } - EdgeTransform T_start_petiole() { + EdgeTransform T_start_petiole() const { LockGuard lock(this->mutex_); return this->pose(trunk_sid_) * T_petiole_trunk().inverse(); } - EdgeTransform T_start_leaf() { + EdgeTransform T_start_leaf() const { LockGuard lock(this->mutex_); return this->pose(trunk_sid_) * T_leaf_trunk().inverse(); } - EdgeTransform T_start_trunk() { + EdgeTransform T_start_trunk() const { LockGuard lock(this->mutex_); return this->pose(trunk_sid_); } diff --git a/main/src/vtr_pose_graph/include/vtr_pose_graph/path/path.hpp b/main/src/vtr_pose_graph/include/vtr_pose_graph/path/path.hpp index 7ab757cec..2216f551d 100644 --- a/main/src/vtr_pose_graph/include/vtr_pose_graph/path/path.hpp +++ b/main/src/vtr_pose_graph/include/vtr_pose_graph/path/path.hpp @@ -39,7 +39,7 @@ class Path { using UniqueLock = std::unique_lock; using LockGuard = std::lock_guard; - Path(const typename GraphT::Ptr& graph) : graph_(graph) {} + Path(const typename GraphT::Ptr& graph, double angle_weight=0.75) : graph_(graph), alpha_{angle_weight} {} public: /** \brief Sequence setters */ @@ -68,6 +68,13 @@ class Path { /** \brief Vertex id implicitly converts to unsigned */ double dist(VertexId vtx_id) const = delete; + /** \brief Gets the curvilinear p value along the path at a sequence index */ + double p(unsigned seq_id) const; + /** \brief Gets the cumu. distance along the path at an iterator position */ + double p(const Iterator& it) const { return p(unsigned(it)); } + /** \brief Vertex id implicitly converts to unsigned */ + double p(VertexId vtx_id) const = delete; + /** \brief Returns the current sequence */ Sequence sequence() const; /** \brief Total number of poses in the sequence */ @@ -81,18 +88,23 @@ class Path { /** \brief Gets the cumu. distance along the path at a sequence index */ int query_terrain_type(unsigned seq_id) const; - protected: - virtual void initSequence(); + static double terrian_type_corridor_width(int terrain_type); /** \brief An iterator to a specified id along the path */ Iterator begin(const unsigned& seq_id = 0) const; /** \brief An iterator to the end of the path (beyond the last vertex) */ Iterator end() const; + protected: + virtual void initSequence(); + + typename GraphT::Ptr graph_; Sequence sequence_; mutable std::vector poses_; mutable std::vector distances_; + mutable std::vector p_vals_; + double alpha_; /** \brief for thread safety, use whenever read from/write to the path */ mutable Mutex mutex_; @@ -195,6 +207,34 @@ double Path::dist(unsigned seq_id) const { return distances_[seq_id]; } +template +double Path::p(unsigned seq_id) const { + LockGuard lock(mutex_); + if (seq_id >= sequence_.size()) { + std::string err{"[Path][dist] id out of range."}; + CLOG(ERROR, "pose_graph") << err; + throw std::range_error(err); + } + // We've already done up to this point + if (seq_id < p_vals_.size()) return p_vals_[seq_id]; + + // expand on demand + auto it = begin(p_vals_.size()); + if (p_vals_.empty()) { + p_vals_.emplace_back(0.); + ++it; + } + for (; unsigned(it) <= seq_id; ++it) { + Eigen::Matrix se3_vec = it->T().vec(); + double distance = se3_vec.head<3>().norm() + + alpha_ * se3_vec.tail<3>().norm(); + const_cast*>(this)->p_vals_.push_back( + p_vals_.back() + distance); + } + + return p_vals_[seq_id]; +} + template int Path::query_terrain_type(unsigned seq_id) const { int terrain_type = graph_->at(sequence_.at(seq_id))->GetTerrainType(); @@ -240,6 +280,21 @@ auto Path::end() const -> Iterator { return Iterator(this, sequence_.end()); } + +/** based on mapping in AnnotateSlider.js*/ +template +double Path::terrian_type_corridor_width (int terrain_type) { + switch (terrain_type) { + case 0: return 0.2; + case 1: return 0.5; + case 2: return 1.0; + case 3: return 1.5; + case 4: return 2.0; + case 5: return 2.5; + default: return 10.0; + } +} + using BasicPathBase = Path; } // namespace pose_graph diff --git a/main/src/vtr_tactic/include/vtr_tactic/tactic.hpp b/main/src/vtr_tactic/include/vtr_tactic/tactic.hpp index da38e38ad..9b8ee83f4 100644 --- a/main/src/vtr_tactic/include/vtr_tactic/tactic.hpp +++ b/main/src/vtr_tactic/include/vtr_tactic/tactic.hpp @@ -57,6 +57,7 @@ class Tactic : public PipelineInterface, public TacticInterface { /** \brief */ double route_completion_translation_threshold = 0.5; + double route_completion_angle_threshold = 0.26; /** \brief Configuration for the localization chain */ LocalizationChain::Config chain_config; diff --git a/main/src/vtr_tactic/src/tactic.cpp b/main/src/vtr_tactic/src/tactic.cpp index 877b90f00..50f4dc92f 100644 --- a/main/src/vtr_tactic/src/tactic.cpp +++ b/main/src/vtr_tactic/src/tactic.cpp @@ -37,6 +37,7 @@ auto Tactic::Config::fromROS(const rclcpp::Node::SharedPtr& node, config->task_queue_size = node->declare_parameter(prefix+".task_queue_size", -1); config->route_completion_translation_threshold = node->declare_parameter(prefix+".route_completion_translation_threshold", 0.5); + config->route_completion_angle_threshold = node->declare_parameter(prefix+".route_completion_angle_threshold", 0.26); /// setup localization chain config->chain_config.min_cusp_distance = node->declare_parameter(prefix+".chain.min_cusp_distance", 1.5); @@ -44,6 +45,7 @@ auto Tactic::Config::fromROS(const rclcpp::Node::SharedPtr& node, config->chain_config.search_depth = node->declare_parameter(prefix+".chain.search_depth", 20); config->chain_config.search_back_depth = node->declare_parameter(prefix+".chain.search_back_depth", 10); config->chain_config.distance_warning = node->declare_parameter(prefix+".chain.distance_warning", 3); + config->chain_config.alpha = node->declare_parameter(prefix+".chain.alpha", 0.25); config->save_odometry_result = node->declare_parameter(prefix+".save_odometry_result", false); config->save_odometry_vel_result = node->declare_parameter(prefix+".save_odometry_vel_result", false); @@ -166,7 +168,11 @@ bool Tactic::passedSeqId(const uint64_t& sid) const { bool Tactic::routeCompleted() const { auto lock = chain_->guard(); - const auto translation = (chain_->T_leaf_trunk()*chain_->T_trunk_target(chain_->sequence().size() - 1)).r_ba_ina().norm(); + const auto T_leaf_target = chain_->T_leaf_trunk()*chain_->T_trunk_target(chain_->sequence().size() - 1); + const auto translation = T_leaf_target.r_ba_ina().norm(); // lgmath bring to aang + // angle threshold of 15 degrees hardcoded for now + const auto T_leaf_target_matrix = T_leaf_target.matrix(); + const auto angle = atan2(T_leaf_target_matrix(1, 0), T_leaf_target_matrix(0, 0)); CLOG(DEBUG, "tactic.eop") << "Translation: " << translation; if (chain_->trunkSequenceId() < (chain_->sequence().size() - 2)) { @@ -176,6 +182,10 @@ bool Tactic::routeCompleted() const { if (translation > config_->route_completion_translation_threshold) { return false; } + + if (std::abs(angle) > config_->route_completion_angle_threshold) { + return false; + } return true; } diff --git a/main/src/vtr_tactic/test/tactic/test_query_cache.cpp b/main/src/vtr_tactic/test/tactic/test_query_cache.cpp index 77cfc6626..774d934e5 100644 --- a/main/src/vtr_tactic/test/tactic/test_query_cache.cpp +++ b/main/src/vtr_tactic/test/tactic/test_query_cache.cpp @@ -93,7 +93,7 @@ TEST(QueryCache, query_cache_copy_move) { EXPECT_EQ(qdata->stamp.ptr(), stamp); EXPECT_EQ(qdata2->stamp.ptr(), stamp2); - // values are differenct now + // values are different now (*stamp)--; (*stamp2)++; EXPECT_EQ(*qdata->stamp, *stamp); diff --git a/main/src/vtr_tactic_msgs/msg/EnvInfo.msg b/main/src/vtr_tactic_msgs/msg/EnvInfo.msg index 975e52004..3c0c907e1 100644 --- a/main/src/vtr_tactic_msgs/msg/EnvInfo.msg +++ b/main/src/vtr_tactic_msgs/msg/EnvInfo.msg @@ -1,2 +1,2 @@ # current type of terrain for planning - to be stored in the vertex -int8 terrain_type 0 \ No newline at end of file +int8 terrain_type 1 \ No newline at end of file diff --git a/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp index 1aff3c680..09c159ca4 100644 --- a/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp +++ b/main/src/vtr_torch/include/vtr_torch/modules/torch_module.hpp @@ -18,11 +18,9 @@ */ #pragma once -//#define C10_UTIL_LOGGING_IS_NOT_GOOGLE_GLOG_H_ #include "vtr_tactic/modules/base_module.hpp" #include "vtr_common/utils/filesystem.hpp" #include "vtr_tactic/task_queue.hpp" -#include #include #include "vtr_torch/types.hpp" #include diff --git a/main/src/vtr_torch/include/vtr_torch/types.hpp b/main/src/vtr_torch/include/vtr_torch/types.hpp index 574ae4f48..62c083443 100644 --- a/main/src/vtr_torch/include/vtr_torch/types.hpp +++ b/main/src/vtr_torch/include/vtr_torch/types.hpp @@ -1,5 +1,5 @@ +#include -#include using ModulePtr = std::shared_ptr; using Module = torch::jit::Module; diff --git a/rviz/ouster.rviz b/rviz/ouster.rviz index fecd477e5..1a761f1de 100644 --- a/rviz/ouster.rviz +++ b/rviz/ouster.rviz @@ -5,10 +5,13 @@ Panels: Property Tree Widget: Expanded: - /Global Options1 - - /TF1/Status1 + - /TF1 + - /TF1/Frames1 - /Teach Path1/Topic1 + - /MPC Solution1 + - /MPC Tracking Reference Poses1 Splitter Ratio: 0.4507042169570923 - Tree Height: 871 + Tree Height: 1391 - Class: rviz_common/Selection Name: Selection - Class: rviz_common/Tool Properties @@ -60,10 +63,74 @@ Visualization Manager: Frame Timeout: 99999 Frames: All Enabled: false + base_link: + Value: true + chassis_link: + Value: true + diff_link: + Value: true + front_left_wheel_link: + Value: true + front_right_wheel_link: + Value: true + imu_link: + Value: true + left_diff_unit_headlight_link: + Value: true + left_diff_unit_link: + Value: true + left_diff_unit_taillight_link: + Value: true lidar: Value: false + loc vertex frame: + Value: true + loc vertex frame (offset): + Value: false + navtech_base: + Value: true + nerian_left: + Value: true + nerian_stereo: + Value: true + novatel: + Value: true + odo vertex frame: + Value: false + odom: + Value: true + os_imu: + Value: true + os_lidar: + Value: true + os_sensor: + Value: true + os_sensor_base_link: + Value: true + os_sensor_baseplate: + Value: true + planning frame: + Value: false + rear_left_wheel_link: + Value: true + rear_right_wheel_link: + Value: true + right_diff_unit_headlight_link: + Value: true + right_diff_unit_link: + Value: true + right_diff_unit_taillight_link: + Value: true robot: Value: true + robot planning: + Value: false + robot planning (extrapolated) mpc: + Value: true + sensor_tower: + Value: true + top_chassis_link: + Value: true world: Value: false world (offset): @@ -75,8 +142,20 @@ Visualization Manager: Show Names: true Tree: world: - world (offset): + loc vertex frame: {} + odo vertex frame: + robot: + lidar: + {} + planning frame: + robot planning: + {} + robot planning (extrapolated) mpc: + {} + world (offset): + loc vertex frame (offset): + {} Update Interval: 0 Value: true - Alpha: 1 @@ -389,7 +468,7 @@ Visualization Manager: Y: 0 Z: 0 Pose Color: 255; 85; 255 - Pose Style: None + Pose Style: Axes Radius: 0.029999999329447746 Shaft Diameter: 0.10000000149011612 Shaft Length: 0.10000000149011612 @@ -539,7 +618,7 @@ Visualization Manager: Name: MPC Tracking Reference Poses Shaft Length: 0.10000000149011612 Shaft Radius: 0.10000000149011612 - Shape: Arrow (3D) + Shape: Axes Topic: Depth: 5 Durability Policy: Volatile @@ -614,26 +693,26 @@ Visualization Manager: Value: true Views: Current: - Class: rviz_default_plugins/Orbit - Distance: 41.55506134033203 + Class: rviz_default_plugins/ThirdPersonFollower + Distance: 16.207958221435547 Enable Stereo Rendering: Stereo Eye Separation: 0.05999999865889549 Stereo Focal Distance: 1 Swap Stereo Eyes: false Value: false Focal Point: - X: 12.343294143676758 - Y: -3.8882362842559814 - Z: 0.2389792650938034 - Focal Shape Fixed Size: false + X: -9.903048515319824 + Y: -2.746500015258789 + Z: 0.005913601256906986 + Focal Shape Fixed Size: true Focal Shape Size: 0.05000000074505806 Invert Z Axis: false Name: Current View Near Clip Distance: 0.009999999776482582 - Pitch: 1.5697963237762451 + Pitch: 0.5547972917556763 Target Frame: robot - Value: Orbit (rviz_default_plugins) - Yaw: 3.441279411315918 + Value: ThirdPersonFollower (rviz_default_plugins) + Yaw: 3.401355266571045 Saved: - Class: rviz_default_plugins/Orbit Distance: 111.39698028564453 @@ -838,10 +917,10 @@ Visualization Manager: Window Geometry: Displays: collapsed: false - Height: 1016 + Height: 1536 Hide Left Dock: false - Hide Right Dock: true - QMainWindow State: 000000ff00000000fd000000040000000000000281000003a2fc0200000007fc0000003d000001a20000000000fffffffaffffffff0100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000000ffffffff0000008c00fffffffb0000001200530065006c0065006300740069006f006e0000000000000002530000006c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000003a2000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001c0000003bdfc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003b000003bd000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000006efc0100000002fb0000000800540069006d00650000000000000005000000025300fffffffb0000000800540069006d00650100000000000004500000000000000000000004b1000003a200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 + Hide Right Dock: false + QMainWindow State: 000000ff00000000fd000000040000000000000281000005aafc0200000007fc0000003d000001a20000000000fffffffaffffffff0100000002fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000000ffffffff0000008c00fffffffb0000001200530065006c0065006300740069006f006e0000000000000002530000006c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003b000005aa000000c700fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c0000026100000001000001c0000005aafc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003b000005aa000000a000fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000006efc0100000002fb0000000800540069006d00650000000000000005000000025300fffffffb0000000800540069006d0065010000000000000450000000000000000000000565000005aa00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000 Selection: collapsed: false Time: @@ -849,7 +928,7 @@ Window Geometry: Tool Properties: collapsed: false Views: - collapsed: true - Width: 1848 - X: 72 + collapsed: false + Width: 2482 + X: 78 Y: 27